一种基于光流传感器定位导航的多旋翼无人机及控制方法与流程

本发明属于无人机控制领域，具体涉及一种基于光流传感器定位导航的多旋翼无人机及控制方法。

背景技术：

四轴飞行器在室内飞行，由于无法接收到gps信号，很难做到准确定位。大量飞行的试验也表明，紧紧依靠惯性传感器组成的传感单元设计姿态控制回路，虽然可以实现四旋翼的室内无震荡飞行，但是会存在很严重的漂移现象。

技术实现要素：

针对上述存在的技术问题，本发明提供一种基于光流传感器定位导航的多旋翼无人机，包括：无人机主体、超声波传感器、气压传感器、gps传感器、imu传感器和光流传感器；

所述无人机主体的控制系统包括：主控制器、辅控制器、存储器、数据收发模块、电调电机模块；

所述主控制器分别连接辅控制器和电调电机模块；

所述辅控制器分别连接存储器和数据收发模块；

所述数据收发模块分别与基站和遥控器建立无线连接；

所述超声波传感器、气压传感器、gps传感器、imu传感器和光流传感器分别与主控器连接；

所述光流传感器采用px4flow光流传感器。

所述主控制器与辅控制器之间采用pwm信号进行数据传输。

所述无线连接采用蓝牙通讯方式。

所述主控制器和辅控制器均采用stm32f407单片机。

所述主控制器采用usart串口读取光流传感器的监测数据，并采用mavlink协议进行数据解析。

一种基于光流传感器定位导航的多旋翼无人机的控制方法，采用上述的基于光流传感器定位导航的多旋翼无人机，包括以下步骤：

步骤1，启动无人机和遥控器，建立无人机与遥控器间的无线连接；通过遥控器向无人机发送以下启动指令中的一个，包括：

启动指令1：启动go模式，进入步骤4；

启动指令2：启动goback模式，进入步骤4；

启动指令3：启动手动模式，进入步骤2；

步骤2，通过遥控器向无人机发送动作控制指令；

步骤3，无人机数据收发模块接收到动作控制指令，并通过辅控制器发送至主控制器；主控制器根据动作控制指令，得出悬停控制指令，并发送至电调电机模块，从而控制无人机动作，直至控制结束。

步骤4，启动基站，建立无人机与基站间的无线连接；

步骤5，通过基站向无人机发送目标位置信息x,y,z，其中x,y表示在水平面上的x方向与y方向的目标水平坐标，z表示在垂直于水平面的z方向的目标高度坐标；

步骤6，无人机的数据收发模块接收到目标位置信息x,y,z，并发送至辅控制器；辅控制器将目标位置信息发送至存储器保存，并发送至主控制器；

步骤7，超声波传感器和气压传感器实时将各自的高度监测信息传送至主控制器，主控制器将高度监测信息转换为实际高度坐标z1，并与目标高度坐标z对比得出高度控制需求；

步骤8，gps传感器将其位置监测信息实时传送至主控制器，主控制器将位置监测信息转换为实际水平坐标x1,y1，并与目标水平坐标x,y对比得出位置控制需求；

步骤9，imu传感器将其姿态监测信息实时传送至主控制器，姿态监测信息即无人机的旋转角速度ω，主控制器据此计算得出实际姿态信息，包括滚转角φ、俯仰角θ、偏航角ψ；将位置控制需求转换为姿态需求；将实际姿态信息与姿态需求对比得出姿态控制需求；

步骤10，主控制器通过辅控制器将接受到的实际高度坐标z1、实际水平坐标x1,y1和实际姿态信息实时发送至存储器保存；主控制器根据高度控制需求和姿态控制需求，得出位置控制指令，并发送至电调电机模块，从而控制无人机动作；

步骤11，当实际水平坐标x1,y1与目标水平坐标x,y一致，且实际高度坐标z1与目标高度坐标z一致时，即无人机到达目标位置；

如果此时的实际高度坐标不大于20cm，则主控制器发送降落控制指令送至电调电机模块，从而控制无人机落地；如果步骤1中发送的是启动指令1，则控制结束；如果步骤1中发送的不是启动指令1，则发送的是启动指令2，进入步骤14；

如果此时的实际高度坐标大于20cm，则无人机进入悬停模式，进入步骤12；

步骤12，光流传感器将其图像监测信息实时传送至主控制器，主控制器根据图像监测信息得出偏移速度[vxvy]，并根据姿态监测信息对偏移速度进行修正，得到实际偏移速度[vxvy]；

步骤13，主控制器根据实际偏移速度[vxvy]得出悬停控制需求；主控制器根据悬停控制需求，得出悬停控制指令，并发送至电调电机模块，从而控制无人机动作；

如果步骤1中发送的是启动指令1，则返回步骤12；如果步骤1中发送的不是启动指令1，则发送的是启动指令2，进入步骤14；

步骤14，通过遥控器向无人机发送返航指令；

步骤15，无人机数据收发模块接收到返航指令，并发送至辅控制器；辅控制器从存储器中提取本次控制过程中最初的实际位置坐标，将其作为目标位置信息，进行步骤7-步骤10，然后进入步骤16；

步骤16，当实际位置坐标与目标位置坐标一致时，即无人机到达目标位置，返航完成；

如果此时的实际高度坐标不大于20cm，则主控制器发送降落控制指令送至电调电机模块，从而控制无人机落地，控制结束；

如果此时的实际高度坐标大于20cm，则无人机进入悬停模式，进入步骤17；

步骤17，光流传感器将其图像监测信息实时传送至主控制器，主控制器根据图像监测信息得出偏移速度[vxvy]，并根据姿态监测信息对偏移速度[vxvy]进行修正，得到实际偏移速度[vxvy]；

步骤18，悬停控制器根据实际偏移速度[vxvy]得出悬停控制需求并发送至主控制器；主控制器根据悬停控制需求，得出悬停控制指令，并发送至电调电机模块，从而控制无人机动作；返回步骤17。

所述步骤4-步骤18中，如果通过遥控器向无人机发送动作控制指令，则进入步骤2。

所述步骤12和步骤17中，主控制器根据图像监测信息得出偏移速度[vxvy]，并根据姿态监测信息对偏移速度[vxvy]进行修正，得到实际偏移速度[vxvy]，包括以下步骤：

步骤a，光流传感器实时送来的图像监测信息中，将如图2(a)所示的上一帧图像的特征点与如图2(b)所示的下一帧图像的特征点进行对比，即如图2(c)所示，得出水平像素偏移量x'和垂直像素偏移量y'，进而计算得出偏移速度[vxvy]；

步骤b，在无人机姿态的变化，光流传感器也随无人机发生旋转，所以通过以下公式对偏移速度[vxvy]进行修正：

其中，ω＝[ωxωyωz]为无人机的旋转角速度，t＝[txtytz]为平移变换矩阵，z1为实际高度坐标，f为光流传感器的透镜焦距；

步骤c，计算实际偏移速度[vxvy]：

本发明的有益效果：

本发明提出一种基于光流传感器定位导航的多旋翼无人机及控制方法，通过光流传感器反馈速度信息，且精度较高；主控制器利用usart串口读取光流传感器模块的数据，利用mavlink协议解析出速度信息，扩大了其应用范围；通过光流传感器的速度信息引入到悬停控制中，通过悬停控制和姿态控制协调作用，实现了四旋翼的室内悬停保持；

本发明将遥控器和基站的pwm信号发射机替换为蓝牙通讯设备，无人机的pwm信号接收机替换为辅控制器，可以预先设定飞行指令，一键开启后，在无人操纵情况下，四旋翼可以按照预定指令自动飞行。

本发明设计合理，易于实现，具有很好的实用价值。

附图说明

图1为本发明具体实施方式中所述基于光流传感器定位导航的多旋翼无人机的结构示意图；

图2(a)为本发明具体实施方式中所述光流传感器的上一帧图像的特征点示意图；

图2(b)为本发明具体实施方式中所述光流传感器的下一帧图像的特征点示意图；

图2(c)为图2(a)和图2(b)中所述特征点的对比示意图；

图3为本发明具体实施方式中所述基于光流传感器定位导航的多旋翼无人机的控制方法流程图。

图中：1、超声波传感器；2、气压传感器；3、gps传感器；4、imu传感器；5、光流传感器；6、主控制器；7、辅控制器；8、存储器；9、数据收发模块；10、电调电机模块。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施实例，对本发明做出进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

本发明提出一种基于光流传感器定位导航的多旋翼无人机，如图1所示，包括：无人机主体、超声波传感器1、气压传感器2、gps传感器3、imu传感器4和光流传感器5；

所述无人机主体的控制系统包括：主控制器6、辅控制器7、存储器8、数据收发模块9、电调电机模块10；

所述主控制器6分别连接辅控制器7和电调电机模块10；

所述主控制器6与辅控制器7之间采用pwm信号进行数据传输；

所述辅控制器7分别连接存储器8和数据收发模块9；

所述主控制器6和辅控制器7均采用stm32f407单片机；

所述数据收发模块9分别与基站和遥控器建立无线连接；

所述无线连接采用蓝牙通讯方式；

所述超声波传感器1、气压传感器2、gps传感器3、imu传感器4和光流传感器5分别与主控器连接；

所述光流传感器5采用px4flow光流传感器；所述主控制器6采用usart串口读取光流传感器5的监测数据，并采用mavlink协议进行数据解析，扩大了其应用范围。

本发明提出一种基于光流传感器定位导航的多旋翼无人机的控制方法，如图3所示，采用上述基于光流传感器定位导航的多旋翼无人机，包括以下步骤：

步骤1，启动无人机和遥控器，建立无人机与遥控器间的无线连接；通过遥控器向无人机发送以下启动指令中的一个，包括：

启动指令1：启动go模式，进入步骤4；

启动指令2：启动goback模式，进入步骤4；

启动指令3：启动手动模式，进入步骤2；

步骤2，通过遥控器向无人机发送动作控制指令；

步骤3，无人机数据收发模块9接收到动作控制指令，并通过辅控制器7发送至主控制器6；主控制器6根据动作控制指令，得出悬停控制指令，并发送至电调电机模块10，从而控制无人机动作。

步骤4，启动基站，建立无人机与基站间的无线连接；

步骤5，通过基站向无人机发送目标位置信息x,y,z，其中x,y表示在水平面上的x方向与y方向的目标水平坐标，z表示在垂直于水平面的z方向的目标高度坐标；

步骤6，无人机的数据收发模块9接收到目标位置信息x,y,z，并发送至辅控制器7；辅控制器7将目标位置信息发送至存储器8保存，并发送至主控制器6；

步骤7，超声波传感器1和气压传感器2实时将各自的高度监测信息传送至主控制器6，主控制器6将高度监测信息转换为实际高度坐标z1，并与目标高度坐标z对比得出高度控制需求；

步骤8，gps传感器3将其位置监测信息实时传送至主控制器6，主控制器6将位置监测信息转换为实际水平坐标x1,y1，并与目标水平坐标x,y对比得出位置控制需求；

步骤9，imu传感器4将其姿态监测信息实时传送至主控制器6，姿态监测信息即无人机的旋转角速度ω，主控制器6据此计算得出实际姿态信息，包括滚转角φ、俯仰角θ、偏航角ψ；将位置控制需求转换为姿态需求；将实际姿态信息与姿态需求对比得出姿态控制需求；

步骤10，主控制器6通过辅控制器7将接受到的实际高度坐标z1、实际水平坐标x1,y1和实际姿态信息实时发送至存储器8保存；主控制器6根据高度控制需求和姿态控制需求，得出位置控制指令，并发送至电调电机模块10，从而控制无人机动作；

步骤11，当实际水平坐标x1,y1与目标水平坐标x,y一致，且实际高度坐标z1与目标高度坐标z一致时，即无人机到达目标位置；

如果此时的实际高度坐标不大于20cm，则主控制器6发送降落控制指令送至电调电机模块10，从而控制无人机落地；如果步骤1中发送的是启动指令1，则控制结束；如果步骤1中发送的不是启动指令1，则发送的是启动指令2，进入步骤14；

如果此时的实际高度坐标大于20cm，则无人机进入悬停模式，进入步骤12；

步骤12，光流传感器5将其图像监测信息实时传送至主控制器6，主控制器6根据图像监测信息得出偏移速度[vxvy]，并根据姿态监测信息对偏移速度进行修正，得到实际偏移速度[vxvy]；

步骤13，主控制器6根据实际偏移速度[vxvy]得出悬停控制需求；主控制器6根据悬停控制需求，得出悬停控制指令，并发送至电调电机模块10，从而控制无人机动作；

如果步骤2中发送的是启动指令1，则返回步骤12；如果步骤2中发送的不是启动指令1，则发送的是启动指令2，进入步骤14；

步骤14，通过遥控器向无人机发送返航指令；

步骤15，无人机数据收发模块9接收到返航指令，并发送至辅控制器7；辅控制器7从存储器8中提取本次控制过程中最初的实际位置坐标，将其作为目标位置信息，进行步骤7-步骤10，然后进入步骤16；

步骤16，当实际位置坐标与目标位置坐标一致时，即无人机到达目标位置，返航完成；

如果此时的实际高度坐标不大于20cm，则主控制器6发送降落控制指令送至电调电机模块10，从而控制无人机落地，控制结束；

如果此时的实际高度坐标大于20cm，则无人机进入悬停模式，进入步骤17；

步骤17，光流传感器5将其图像监测信息实时传送至主控制器6，主控制器6根据图像监测信息得出偏移速度[vxvy]，并根据姿态监测信息对偏移速度[vxvy]进行修正，得到实际偏移速度[vxvy]；

步骤18，悬停控制器根据实际偏移速度[vxvy]得出悬停控制需求并发送至主控制器6；主控制器6根据悬停控制需求，得出悬停控制指令，并发送至电调电机模块10，从而控制无人机动作；返回步骤17；

所述步骤4-步骤18中，如果通过遥控器向无人机发送动作控制指令，则进入步骤3。

所述步骤12和步骤17中，主控制器6根据图像监测信息得出偏移速度[vxvy]，并根据姿态监测信息对偏移速度[vxvy]进行修正，得到实际偏移速度[vxvy]，包括以下步骤：

步骤a，光流传感器5实时送来的图像监测信息中，将如图2(a)所示的上一帧图像的特征点与如图2(b)所示的下一帧图像的特征点进行对比，即如图2(c)所示，得出水平像素偏移量x'和垂直像素偏移量y'，进而计算得出偏移速度[vxvy]；

步骤b，在无人机姿态的变化，光流传感器5也随无人机发生旋转，所以通过以下公式对偏移速度[vxvy]进行修正：

其中，ω＝[ωxωyωz]为无人机的旋转角速度，t＝[txtytz]为平移变换矩阵，z1为实际高度坐标，f为光流传感器5的透镜焦距；

步骤c，计算实际偏移速度[vxvy]：