CN106843282B - 基于m100开发平台的区域完全搜索与避障***及方法 - Google Patents

Abstract

本发明涉及一种基于M100开发平台的区域完全搜索与避障***，所述***包括：APP模块、巡航模块以及避障模块，其中APP模块负责发送起飞、降落命令，并实时显示图传画面，巡航模块包括GPS校准模块、返回基线模块和转向模块，避障模块包括判断模块和决策模块，其中判断模块负责判断是否该方向有障碍物，决策模块负责得出当前避障进行到哪个步骤。该技术方案能够在特定区域进行完全自主、全面搜索的同时实现躲避障碍物，通过对双目视觉***的数据和超声波数据结合，提高检测障碍物的精度和效率，进而提高无人机自动避障的成功率和区域搜索的效率。

Description

基于M100开发平台的区域完全搜索与避障***及方法

技术领域

本发明涉及一种基于大疆公司M100的全方位自动避障和巡航方法，属于无人机巡航和避障技术领域。

背景技术

随着无人机的应用越来越广泛,无人机应用的环境越来越复杂,对无人机感知环境,躲避障碍物的要求越来越高。目前,无人机的避障方法主要包括TOF技术、视觉避障、超声波避障、红外线避障、雷达避障等，这些方法往往是采取其中一个，或两个方法单独进行。其中，ToF技术容易受光污染以及灰尘烟雾的影响，目标鉴别和排除能力也较弱，雷达避障基于中低频，雷达体积较大，很难在无人机上安装使用；此外，躲避障碍物的***大多没有实现真正的自主飞行，从无人机起飞到降落的整个过程没能实现全自动，仅考虑如何躲避障碍物。目前的无人机避障***中未考虑如何和特定区域的巡航相结合，如果仅避障，则搜索到的区域会有很大的重叠，且很有可能达不到完全的搜索。

M100是大疆公司研制的一款平稳可靠、功能强大、可灵活扩展的飞行平台，可进行二次开发。其中，M100已具备5个方向的双目视觉***和超声波***，可以获取深度图数据和超声波数据。基于该平台的基础以及目前无人机避障存在的问题，申请人研发了一种基于大疆公司M100的全方位自动避障和巡航方法。

发明内容

本发明正是针对现有技术中存在的技术问题，提供一种基于M100开发平台的区域完全搜索与避障***与方法，该技术方案利用手机APP控制其自动起飞降落，实时传回图像数据；结合超声波数据和深度图数据，更好的识别出不同种类的障碍物；在不同情况下调整云台的角度，实现区域完全扫描；达到了真正的自主飞行和区域的完全搜索，将巡航和避障更好地结合。

为了实现上述目的，本发明的技术方案如下，一种基于M100开发平台的区域完全搜索与避障***，其特征在于，所述***包括：APP模块、巡航模块以及避障模块，其中巡航模块作为主线程，APP模块和避障模块作为子线程，在巡航模块和避障模块两个线程中，有以下变量用作信息传递，由巡航模块控制的变量有控制权(当控制权在巡航模式下时，避障模式线程不能运行，反之，巡航模式等待；只有当其中一个模式获取控制权后才可以更改数据)、飞行方向(北东坐标系下)、当前关注的图像传感器方向；由避障模块控制的变量有申请控制权、飞行高度、飞行速度、回到基线时是否安全。其中APP模块负责发送起飞、降落命令，并实时显示图传画面，巡航模块包括GPS校准模块、返回基线模块和转向模块，GPS校准模块负责GPS坐标获取和校准，返回基线模块负责避障结束后安全回到基线上，转向模块负责到达转弯处平稳调整方向并计算下一目标点，避障模块包括判断模块和决策模块，其中判断模块负责判断是否该方向有障碍物，决策模块负责得出当前避障进行到哪个步骤。所述无人机由于带宽的限制，双目视觉***中只能传输两个方向的深度图数据具体包括：在无人机的上方、前方、左方、右方、下方都装有图像传感器和超声波传感器，5个方向的超声波数据都可以获取，但是深度图数据只能传输两个方向，在这里我们采用图像传感器1号始终传输前方的图像数据，然后根据不同情况图像传感器2号传输左方、右方或上方的深度图数据。

其中，所述APP端发送起飞命令，M100进入避障模式一，即APP端的数据通过局域网发送给M100，此时，M100起飞，控制权在避障模式一，调整高度后，进入巡航模式，即控制权在巡航模式具体包括：该***分为两个线程，巡航模块作为主线程，避障模块作为子线程，控制权由巡航模块进行管理，拥有控制权的模块可以对无人机的飞行状态进行改变，无人机起始默认进入避障模式，调整高度后，巡航模式收回控制权。该***能够在特定区域进行完全自主、全面搜索的同时实现躲避障碍物，通过对双目视觉***的数据和超声波数据结合，提高检测障碍物的精度和效率，进而提高无人机自动避障的成功率和区域搜索的效率。

一种基于M100开发平台的区域完全搜索与避障***的方法，所述方法如下：1)APP端发送起飞命令，M100进入避障模式一，即控制权在避障模式，调整高度后，进入巡航模式，即控制权在巡航模式；巡航模式中，M100按照巡航规划出的轨迹飞行，云台按照一定角度摆动，并将拍摄到的画面实时传送到APP端；2)巡航的速度随计算的结果变化；图像传感器1号始终开启，方向为前方；结合深度图数据和超声波数据，判断前方是否有障碍物；若是，避障模式申请控制权，巡航模式在非转弯的情况下会把控制权交给避障模式，进入避障模式一；3)图像传感器2号右边开启，判断右方是否有障碍物；若是，右方不安全，M100左转直到前方无障碍物，进入“左转后”状态；若否，右方安全，M100右转直到前方无障碍物；4)进入“右转后”状态；“左(右)转后”状态中，云台在图像传感器方向和地面之间90°摆动，图像传感器2号右(左)方开启，优先判断前方是否有障碍物；若是，M100左(右)转直到前方无障碍物；若否，判断右(左)方是否安全；若否，M100继续前进；若是，M100右(左)转，判断方向是否为巡航的方向；若否，M100继续右(左)转直到方向为巡航方向；若是，M100此时为“前进”状态且偏左(右)，判断右(左)方是否有障碍物；若是，M100前进直到右(左)方无障碍物，然后向右(左)平移；若否，向右(左)平移直到回到基线上，控制权交还给巡航模式；5)当电量小于一定阈值或用户在APP上点击返航时，M100进入避障模式二；图像传感器2号上方开启，判断上方是否有障碍物；若是，M100按照避障模式一飞行，直到上方无障碍物；若否，M100上升到一定高度，直接飞回起点，任务结束。

作为本发明的一种改进，所述步骤1)中，所述巡航模式中，M100按照预先规定的轨迹飞行，云台按照一定角度摆动，并将拍摄到的画面实时传送到APP端的步骤包括：确定飞行轨迹之前，首先要先获取到巡航区域四个顶点的GPS坐标，飞行轨迹采用蛇形迂回的方式，无人机在每走一段折线时，才会计算该线的终点和要转弯的地方，这样可以大大减少计算量；在巡航模式中，摆动的角度和无人机飞行的高度和巡航轨迹的步长密切相关，在这里固定云台左右摆动45°，飞行高度2m，这样可计算得步长，用尽量少的时间和电量达到区域的完全搜索，提高了效率。

作为本发明的一种改进，所述步骤2)中，巡航的速度随计算的结果变化具体如下，巡航速度有三级：快速、慢速一、慢速二(实际测试中，分别采用3.5m/s、2m/s、0.7m/s)，超声波数据返回的距离有两个阈值：进入预警状态的阈值、进入避障模式的阈值(实际测试中，分别采用5m、2m，进入预警状态的阈值大于进入避障模式的阈值)。若在前进方向上没有检测到障碍物且超声波数据返回的距离大于进入预警状态的阈值，以快速前进；若在前进方向上没有检测到障碍物且超声波数据小于进入预警状态的阈值，同时大于进入避障模式的阈值时，无人机以慢速一前进，进入预警状态，即前方可能出现障碍物；若前方检测到障碍物或超声波数据小于进入避障模式的阈值，则无人机以慢速二进入避障模式；避障结束后，再通过新一轮的判断决定巡航速度；

所述步骤2)中，判断前方是否有障碍物的步骤如下：首先获取最原始的深度图数据和超声波数据，超声波数据为无人机到最近障碍物的距离，深度图数据为未经处理的单通道图像；然后对获取到的深度图进行腐蚀、膨胀、二值化，其中，二值化的过程包括：将没有障碍物的黑色和白色部分转变为黑色，有障碍物的灰色部分转变为白色，得到一个二值化后的图像，且障碍物此时在图像中显示为白色，然后计算所有连续白色区域的面积，当至少一个连续白色的面积大于一定的阈值时，认为该方向上有障碍物。此外在实际测试中，发现前方图像的下方某几行像素为地面，会由于光照的影响被判定为障碍物，所以只处理图像上方的深度图数据来判断是否有障碍物，从而减少了计算量，提高了效率；超声波数据的获取频率较快，且会随着无人机的飞行状态变化较快，结果不稳定，比如前方有一个较细的障碍物，两个相邻的超声波数据可能会截然不同，因此采用分段采样方法，每10个数据为一组，在这一组中只取一个值，当新的数据小于当前最小距离时，才更新当前最小距离，这样就可以保证不会由于障碍物形状的问题出现判断错误，提高了安全性；若当前最小距离大于一定阈值时，也认为该方向上有障碍物。

作为本发明的一种改进，所述步骤3)中，进入“右转后”状态的步骤如下，M100观察前方是否有障碍物，当发现前方危险时，图像传感器2号默认右边开启，这时观察右边是否安全，安全则右转，不安全则左转。在这里有一种特殊情况即无人机飞进一个较狭窄的空间，左右都有障碍物的情况下，无人机会进行左转，但由于无人机的右转和左转状态都属于原地动作，即使在左转后也会先观察前方是否有障碍物，若有障碍物，则会继续左转，此时无人机就会从该较狭窄的空间退出。

作为本发明的一种改进，所述步骤4)中，优先判断前方是否有障碍物的步骤如下：在“左(右)转后”状态中，由于左边已经有障碍物，所以为了使实时传输的图像更有意义，云台将有关障碍物的图像传输给APP端，云台摆动方向如图3所示；在“左(右)转后”状态中，每一次判断前方是否有障碍物，都是优先判断运动的方向是否有障碍物，然后再判断另一个图像传感器的数据。

作为本发明的一种改进，所述步骤4)中，所述若是，M100此时为“前进”状态且偏左(右)，判断右(左)方是否有障碍物；若是，M100前进直到右(左)方无障碍物，然后向右(左)平移；若否，向右(左)平移直到回到基线上，控制权交还给巡航模式的步骤如下：M100在方向调整为巡航方向后，接下来要做的就是平移回巡航的线上，在平移的过程中要注意观察平移的方向是否有障碍物，若有，则前进直到跨过障碍物，然后再继续平移。

作为本发明的一种改进，所述步骤5)中，M100进入避障模式二的步骤如下，该发明基于M100开发平台的巡航与避障方法的避障模式共有两种，避障模式一和避障模式二，两者的不同之处在于避障模式二为无人机返航时采用的避障模式，避障模式二状态时，图像传感器2号开启上方，当上方无障碍物时，无人机就会上升到一定高度，然后直接返航，否则会采取避障模式一飞行。

相对于现有技术，本发明具有如下优点，1)该技术方案实现了真正的自主飞行和区域的完全搜索，将巡航和避障更好地结合，该用户点击APP端的“起飞”按钮后，无人机会按照预先设定的巡航轨迹飞行，在途中若遇到障碍物，无人机会进行避障，且在避障结束后回到巡航的基线上并保持原来巡航的方向，继续按照巡航轨迹飞行，当电量低于一定阈值或用户点击APP端的“返航”按钮后，无人机在上方无障碍物的情况下上升至一定高度，返回起飞点，若上方有障碍物，则会先飞至上方无障碍物的区域，然后上升。云台会随着模式的不同调整摆动的方向，将拍摄到的画面实时传送到APP端，整个过程只有用户点击APP上的按钮操作，之后无人机可以进行完全自主的区域搜索和避障；2)在躲避障碍物方面，将超声波数据和图像传感器数据有意义的结合，可以适应更复杂的环境，提高安全性和可靠性；3)在区域搜索方面，根据不同情况调整云台的角度，获取到更全面的信息，通过路径规划，能够达到区域的全面搜索；4)在自主飞行方面，只需用户点击APP端的按钮，即可实现整个飞行过程，并安全返回，通过对双目视觉***的数据和超声波数据结合，提高检测障碍物的精度和效率，进而提高无人机自动避障的成功率和区域搜索的效率。

附图说明

图1是本发明基于M100开发平台的区域完全搜索与避障***与方法的巡航轨迹示意图；

图2是本发明基于M100开发平台的区域完全搜索与避障***与方法的摆动方向的示意图；

图3是本发明基于M100开发平台的区域完全搜索与避障***与方法的摆动方向的示意图；

图4是本发明基于M100开发平台的区域完全搜索与避障***与方法的避障模式一流程图；

图5是本发明基于M100开发平台的区域完全搜索与避障***与方法的避障模式二流程图；

图6是本发明基于M100开发平台的区域完全搜索与避障***与方法的***示意图；

图7是本发明基于M100开发平台的区域完全搜索与避障***与方法的举例示意图。

图8是本发明基于M100开发平台的区域完全搜索与避障***与方法的巡航的矩形区域示意图。

图9是本发明基于M100开发平台的区域完全搜索与避障***与方法的已知两个GPS值计算偏移量的示意图。

图10是本发明基于M100开发平台的区域完全搜索与避障***与方法的修正表示矩形区域的四个GPS值示意图。

图11是本发明基于M100开发平台的区域完全搜索与避障***与方法的计算各个单程终止点方法的示意图。

具体实施方式：

为了加深对本发明的理解，下面结合附图对本实施例做详细的说明。

实施例1：参见图6，一种基于M100开发平台的区域完全搜索与避障***，所述***包括：APP模块、巡航模块以及避障模块，其中巡航模块作为主线程，APP模块和避障模块作为子线程，在巡航模块和避障模块两个线程中，有以下变量用作信息传递，由巡航模块控制的变量有控制权(当控制权在巡航模式下时，避障模式线程不能运行，反之，巡航模式等待；只有当其中一个模式获取控制权后才可以更改数据)、飞行方向(北东坐标系下)、当前关注的图像传感器方向；由避障模块控制的变量有申请控制权、飞行高度、飞行速度、回到基线时是否安全。其中APP模块负责发送起飞、降落命令，并实时显示图传画面，巡航模块包括GPS校准模块、返回基线模块和转向模块，GPS校准模块负责GPS坐标获取和校准，返回基线模块负责避障结束后安全回到基线上，转向模块负责到达转弯处平稳调整方向并计算下一目标点，避障模块包括判断模块和决策模块，其中判断模块负责判断是否该方向有障碍物，决策模块负责得出当前避障进行到哪个步骤。所述无人机由于带宽的限制，双目视觉***中只能传输两个方向的深度图数据具体包括：在无人机的上方、前方、左方、右方、下方都装有图像传感器和超声波传感器，5个方向的超声波数据都可以获取，但是深度图数据只能传输两个方向，在这里我们采用图像传感器1号始终传输前方的图像数据，然后根据不同情况图像传感器2号传输左方、右方或上方的深度图数据。其中，所述APP端发送起飞命令，M100进入避障模式一，即APP端的数据通过局域网发送给M100，此时，M100起飞，控制权在避障模式一，调整高度后，进入巡航模式，即控制权在巡航模式具体包括：该***分为两个线程，巡航模块作为主线程，避障模块作为子线程，控制权由巡航模块进行管理，拥有控制权的模块可以对无人机的飞行状态进行改变，无人机起始默认进入避障模式，调整高度后，巡航模式收回控制权。该***能够在特定区域进行完全自主、全面搜索的同时实现躲避障碍物，通过对双目视觉***的数据和超声波数据结合，提高检测障碍物的精度和效率，进而提高无人机自动避障的成功率和区域搜索的效率。

实施例2：一种基于M100开发平台的区域完全搜索与避障***方法，所述方法如下：1)APP端发送起飞命令，M100进入避障模式一，即控制权在避障模式，调整高度后，进入巡航模式，即控制权在巡航模式；巡航模式中，M100按照巡航规划出的轨迹飞行，云台按照一定角度摆动，并将拍摄到的画面实时传送到APP端；2)巡航的速度随计算的结果变化；图像传感器1号始终开启，方向为前方；结合深度图数据和超声波数据，判断前方是否有障碍物；若是，避障模式申请控制权，巡航模式在非转弯的情况下会把控制权交给避障模式，进入避障模式一；3)图像传感器2号右边开启，判断右方是否有障碍物；若是，右方不安全，M100左转直到前方无障碍物，进入“左转后”状态；若否，右方安全，M100右转直到前方无障碍物；4)进入“右转后”状态；“左(右)转后”状态中，云台在图像传感器方向和地面之间90°摆动，图像传感器2号右(左)方开启，优先判断前方是否有障碍物；若是，M100左(右)转直到前方无障碍物；若否，判断右(左)方是否安全；若否，M100继续前进；若是，M100右(左)转，判断方向是否为巡航的方向；若否，M100继续右(左)转直到方向为巡航方向；若是，M100此时为“前进”状态且偏左(右)，判断右(左)方是否有障碍物；若是，M100前进直到右(左)方无障碍物，然后向右(左)平移；若否，向右(左)平移直到回到基线上，控制权交还给巡航模式；5)当电量小于一定阈值或用户在APP上点击返航时，M100进入避障模式二；图像传感器2号上方开启，判断上方是否有障碍物；若是，M100按照避障模式一飞行，直到上方无障碍物；若否，M100上升到一定高度，直接飞回起点，任务结束。

所述步骤1)中，所述巡航模式中，M100按照预先规定的轨迹飞行，云台按照一定角度摆动，并将拍摄到的画面实时传送到APP端的步骤包括：确定飞行轨迹之前，首先要先获取到巡航区域四个顶点的GPS坐标，飞行轨迹采用蛇形迂回的方式，见图1c，无人机在每走一段折线时，才会计算该线的终点和要转弯的地方，这样可以大大减少计算量；在巡航模式中，云台的摆动角度如图2所示，摆动的角度和无人机飞行的高度和巡航轨迹的步长密切相关，在这里我们固定云台左右摆动45°，飞行高度2m，这样可计算得步长，用尽量少的时间和电量达到区域的完全搜索，提高了效率。

所述步骤2)中，巡航的速度随计算的结果变化具体如下，巡航速度有三级：快速、慢速一、慢速二(实际测试中，分别采用3.5m/s、2m/s、0.7m/s)，超声波数据返回的距离有两个阈值：进入预警状态的阈值、进入避障模式的阈值(实际测试中，分别采用5m、2m，进入预警状态的阈值大于进入避障模式的阈值)。若在前进方向上没有检测到障碍物且超声波数据返回的距离大于进入预警状态的阈值，以快速前进；若在前进方向上没有检测到障碍物且超声波数据小于进入预警状态的阈值，同时大于进入避障模式的阈值时，无人机以慢速一前进，进入预警状态，即前方可能出现障碍物；若前方检测到障碍物或超声波数据小于进入避障模式的阈值，则无人机以慢速二进入避障模式；避障结束后，再通过新一轮的判断决定巡航速度；

所述步骤2)中，判断前方是否有障碍物的步骤如下：首先获取最原始的深度图数据和超声波数据，超声波数据为无人机到最近障碍物的距离，深度图数据为未经处理的单通道图像；然后对获取到的深度图进行腐蚀、膨胀、二值化，其中，二值化的过程包括：将没有障碍物的黑色和白色部分转变为黑色，有障碍物的灰色部分转变为白色，得到一个二值化后的图像，且障碍物此时在图像中显示为白色，然后计算所有连续白色区域的面积，当至少一个连续白色的面积大于一定的阈值时，认为该方向上有障碍物。此外在实际测试中，发现前方图像的下方某几行像素为地面，会由于光照的影响被判定为障碍物，所以只处理图像上方的深度图数据来判断是否有障碍物，从而减少了计算量，提高了效率；超声波数据的获取频率较快，且会随着无人机的飞行状态变化较快，结果不稳定，比如前方有一个较细的障碍物，两个相邻的超声波数据可能会截然不同，因此采用分段采样方法，每10个数据为一组，在这一组中只取一个值，当新的数据小于当前最小距离时，才更新当前最小距离，这样就可以保证不会由于障碍物形状的问题出现判断错误，提高了安全性；若当前最小距离大于一定阈值时，也认为该方向上有障碍物。

所述步骤3)中，进入“右转后”状态的步骤如下，M100观察前方是否有障碍物，当发现前方危险时，图像传感器2号默认右边开启，这时观察右边是否安全，安全则右转，不安全则左转。在这里有一种特殊情况即无人机飞进一个较狭窄的空间，左右都有障碍物的情况下，无人机会进行左转，但由于无人机的右转和左转状态都属于原地动作，即使在左转后也会先观察前方是否有障碍物，若有障碍物，则会继续左转，此时无人机就会从该较狭窄的空间退出。

所述步骤4)中，优先判断前方是否有障碍物的步骤如下：在“左(右)转后”状态中，由于左边已经有障碍物，所以为了使实时传输的图像更有意义，云台将有关障碍物的图像传输给APP端，云台摆动方向如图3所示；在“左(右)转后”状态中，每一次判断前方是否有障碍物，都是优先判断运动的方向是否有障碍物，然后再判断另一个图像传感器的数据。所述步骤4)中，所述若是，M100此时为“前进”状态且偏左(右)，判断右(左)方是否有障碍物；若是，M100前进直到右(左)方无障碍物，然后向右(左)平移；若否，向右(左)平移直到回到基线上，控制权交还给巡航模式的步骤如下：M100在方向调整为巡航方向后，接下来要做的就是平移回巡航的线上，在平移的过程中要注意观察平移的方向是否有障碍物，若有，则前进直到跨过障碍物，然后再继续平移。

所述步骤5)中，M100进入避障模式二的步骤如下，该发明基于M100开发平台的巡航与避障方法的避障模式共有两种，避障模式一和避障模式二，两者的不同之处在于避障模式二为无人机返航时采用的避障模式，避障模式二状态时，图像传感器2号开启上方，当上方无障碍物时，无人机就会上升到一定高度，然后直接返航，否则会采取避障模式一飞行。模拟应用实施例：参见图1-图11，一种基于M100开发平台的区域完全搜索与避障***方法，所述方法如下，首先确定需要进行巡航的区域。通过现成的地图软件、采用仪器或飞机本身进行实际测量、在APP的地图选取区中选择等方法，我们可以获得需要进行巡航的矩形区域四个边角的GPS坐标值(如图8)，因为通过测量获取的GPS值存在一定的误差，程序在巡航开始时会对取得的GPS值做轻微的调整，使得巡航区域为矩形，该过程在巡航模块中给出。或者可以通过上述方法获取矩形巡航区域四个边角中的一个GPS值，通过指定其余三个点各点在北东坐标系下与该点在北和东两个方向上的偏移值，程序会自动运用如下方法计算出其余三点的GPS坐标值。

假设已知点的GPS的纬度和经度分别为x₀和y₀，待计算点的GPS为x₁和y₁，待计算点在北东坐标系下与已知点在北和东两个方向上的偏移量为x和y(如图9)。则待计算点的GPS值可由该式子算出：

x₁＝x/6378137+x₀

y₁＝y/(6378137*cos(x₁/2+x₀/2))+y₀

在确定巡航区域及开始巡航的第一个点(四个边角中的一个)后，用户需要指定无人机的步进长度如图1c，用以确定无人机巡航时步进的距离。之后，用户点击APP端的起飞按钮，无人机首先飞到巡航起点，然后进行巡航和避障。

巡航开始时，程序会对取得的四个边角点的GPS做分析,如图10。程序会优先选用点0、1、2进行飞行路径的规划，点3作为终止参考点。由于搜索区域为矩形区域，线01应与线23平行且相等，线12应与线03平行且相等，线01应与线12垂直，由此可以由点0、1、2得到理论上的点3，若与给定的点3的GPS值误差不大，则将3点的GPS值修正为理论计算所得值，否则认为所给的点不符合矩形规范。巡航算法会在矩形区域内生成一条S型的飞行路径，该路径将遍历整个巡航区域以实现遍历搜寻，飞行路径是实时生成的，单次路径是一条简单的直线，由单程起始点直线飞往单程终止点，在到达单程终止点时，无人机飞到巡航区的边界或飞过一个步进长度，此时需要调整飞行方向，然后计算下一条直线路径的目标GPS坐标，将该目标设为下一条路径的单程终止点，并将此时的GPS坐标置为下一路径的单程起始点，直到完成整个区域的搜寻(图1b、1c)。每一单程的目标GPS坐标的计算以点0和点1作为参考点，而目标点一定在线12和线03上，所以可由点0、点1和步进长度计算得出整个巡航区域所有的目标GPS坐标。

由点0和点1的坐标，假设点0的GPS为x₀和y₀，点1的为x₁和y₁，以点0为原点建立北东坐标系，则由前面计算偏移的方法可以得出点1与点0的北向和东向的偏移x和y，由此可得矩形巡航区域在北东坐标系下的偏角θ＝arctan(y/x)，则线12和线03与北东坐标系的偏角则为(90-θ)度，则再由步进长度(设为t)，可以通过三角变换得到一个步进长度相对于北东坐标系的偏移：

x_t＝t*cos(90-θ)，y_t＝t*sin(90-θ)，

再由点0和点1的GPS值，在线12与线03上的所有目标GPS都可由前文中所述的GPS偏移计算式计算得出：

目标GPS在线12上：

x_d＝(x/6378137+x₀)+x_t

y_d＝(y/(6378137*cos((x/6378137+x₀)/2+x₀/2))+y₀)+y_t

目标GPS在线03上：

x_d＝i*x_t

y_d＝i*y_t

由此可以生成M100在搜索区域飞行的S形路径，在该算法中，M100将通过GPS进行自身定位和飞行，巡航算法不会一次性生成整个飞行路径，而是在到达需要调整飞行方向时(即需要拐弯时)才计算下一条直线路径的目标GPS坐标单程终止点目标GPS坐标，并将此时的GPS坐标置为下一路径的单程起始点的起始坐标，直到完成整个区域的搜寻。

当正常巡航时，M100的传感器检测到前方出现障碍物时，巡航将暂停，无人机进入避障模式，当避障完成后，巡航接管无人机，但在避障结束后，无人机已经飞离了原本的航线，在继续巡航之前，需要回到原来的航线上。以图1d为例，当避障结束后，无人机向右偏离原本的航线，此时可以通过单程起始点和单程终止点确定飞机原本的航线基线所在的位置。基线方程的建立方法是以单程起始点作为原点建立北东坐标系，单程终止点为航线基线上的一点，解直线方程可以得到基线方程，而通过点与直线间的距离公式可以计算出无人机当前位置与基线间的距离。此时，无人机进入返回基线上的状态，检测无人机左方的传感器，若左方检测到有障碍物，则无人机向前飞行，同时检测前方是否有障碍物，若前方再次出现障碍物，则再次进入避障模式；若左方无障碍物，则保持机头朝向不变向左水平飞行，直到回到原本的航线基线上，若左方出现障碍物，则继续向前飞行。直到无人机矫正回到原本的航线后，继续进行搜寻任务。

避障模块中，在APP发送起飞命令后，M100进入避障模式一。该模式共分为“前进”，“右转”，“右转后”，“左转”，“左转后”5个状态。状态转换见图4。

起始时，默认状态为“前进”、图像传感器1号方向为前方，在这种情形下，M100首先原地调整高度到1.5-2m，控制权起始时在避障模块手中，调整高度后交给巡航模块。然后前进，若前方有障碍物，避障模块申请控制权，巡航模块在非原地旋转的情况下会将控制权交给避障模块。此时默认先观察右方(图像传感器2号方向为右，传输右方的图像)，若右方安全，则进入“右转”状态；若右方不安全，则进入“左转”状态。在“右转”状态和“左转”状态中，M100原地旋转直到传感器认为前方无障碍物(这样也可避免M100在左方也有障碍物时撞上去的情况)，之后进入“右转后”和“左转后”状态。

在“右转后”和“左转后”状态中，此时M100的目标是飞过障碍物后，回到之前巡航的方向上，通过实际测试，我们发现在“右转”和“左转”的基础上，M100还需再多转10°以确保安全。对于“右转后”，先检查图像传感器2号是否为左边开启，然后观察周围障碍物情况，若左方有障碍物，则继续前进，在前进过程中，分为两种情况，情况一，直到发现左方无障碍物时，前方都没有发现障碍物，此时M100左转，若左转后的方向为巡航的方向，状态转变为“前进”，标记为偏右，若左转后的方向不为巡航的方向，M100再次左转,直到方向为巡航的方向，情况二，在前进过程中，发现左方无障碍物之前，前方有障碍物，此时M100再次进入“右转”，直到前方无障碍物，再进入“右转后”，观察周围障碍物情况，重复上述的情况一和情况二，直到M100的方向为巡航的方向(即情况一)。“左转后”和“右转后”相似，这里不再赘述。

此时，M100进入“前进”状态。M100方向正确后，接下来就是要回到巡航的基线上。“右转后”转为“前进”状态，则默认为偏右，“左转后”转为“前进”状态，则默认为偏左，这里GPS也会做一次检查。偏右时，图像传感器2号方向左方开启，M100向左平移，若左方检测到障碍物，则M100停止左移，这时向前移动，直到左方无障碍物，然后继续左移直到回到巡航的基线上，此时控制权由避障交给巡航。偏左和偏右类似，不再赘述。避障模式中，在APP发送返航命令后，M100进入避障模式二，如图5所示。该模式共分为“上升”，“前进”，“左转”，“左转后”，“右转”，“右转后”6个状态。避障模式一和避障模式二不同之处在于，避障模式二为返航时的避障策略，M100需要上升到一定高度，直接飞回起点，所以这时图像传感器2号上方开启。若上方有障碍物，则和避障模式一相同，M100先保持原始高度进行避障，直到上方无障碍物，然后M100直接上升，飞回起点。

在巡航和避障过程中，云台角度设置如下。控制权在巡航模块时，云台向下90°左右摆动，如图2所示；控制权在避障模块时，云台转动方向为图像传感器2号和下方90°摆动，如图3所示。

假设在巡航的过程中遇到如图7的障碍物，此时M100从巡航模式在非转弯状态的前提下切换为避障模式，进入“前进”状态,默认右方传感器开启，检测到右方无障碍物，因此进入“右转”状态，当右转直到前方无障碍物为止，然后进入“右转后”状态，由图7所示，在“右转后”状态中前方又遇到障碍物，因此再次进入“右转”状态，然后“右转后”状态，在该状态中，左方传感器开启，M100前进一段距离后发现左方无障碍物，因此左转，再前进直到左方无障碍物，由于此次左转后并没有回到原来巡航的方向，因此需要再次左转，这时M100方向和巡航方向一致，避障模式结束，转换为巡航模式，M100前进直到左方无障碍物，然后平移到原来的巡航基线上，继续飞行到目标点。

需要说明的是上述实施例，并非用来限定本发明的保护范围，在上述技术方案的基础上所作出的等同变换或替代均落入本发明权利要求所保护的范围。

Claims (5)

1.一种基于M100开发平台的区域完全搜索与避障***的方法，***包括：APP模块、巡航模块以及避障模块，其中APP模块负责发送起飞、降落命令，并实时显示图传画面，巡航模块包括GPS校准模块、返回基线模块和转向模块，GPS校准模块负责GPS坐标获取和校准，返回基线模块负责避障结束后安全回到基线上，转向模块负责到达转弯处平稳调整方向并计算下一目标点，避障模块包括判断模块和决策模块，其中判断模块负责判断是否该方向有障碍物，决策模块负责得出当前避障进行到哪个步骤；其特征在于，所述方法如下：步骤1)APP端发送起飞命令，M100进入避障模式一，即控制权在避障模式，调整高度后，进入巡航模式，即控制权在巡航模式；巡航模式中，M100按照巡航规划出的轨迹飞行，云台按照一定角度摆动，并将拍摄到的画面实时传送到APP端；步骤2)巡航的速度随计算的结果变化；图像传感器1号始终开启，方向为前方；结合深度图数据和超声波数据，判断前方是否有障碍物；若是，避障模式申请控制权，巡航模式在非转弯的情况下会把控制权交给避障模式，进入避障模式一；步骤3)图像传感器2号右边开启，判断右方是否有障碍物；若是，右方不安全，M100左转直到前方无障碍物，进入“左转后”状态；若否，右方安全，M100右转直到前方无障碍物；步骤4)进入“右转后”状态；“左/右转后”状态中，云台在图像传感器方向和地面之间90°摆动，图像传感器2号右/左方开启，优先判断前方是否有障碍物；若是，M100左/右转直到前方无障碍物；若否，判断右/左方是否安全；若否，M100继续前进；若是，M100右/左转，判断方向是否为巡航的方向；若否，M100继续右/左转直到方向为巡航方向；若是，M100此时为“前进”状态且偏左/右，判断右/左方是否有障碍物；若是，M100前进直到右/左方无障碍物，然后向右/左平移；若否，向右/左平移直到回到基线上，控制权交还给巡航模式；步骤5)当电量小于一定阈值或用户在APP上点击返航时，M100进入避障模式二；图像传感器2号上方开启，判断上方是否有障碍物；若是，M100按照避障模式一飞行，直到上方无障碍物；若否，M100上升到一定高度，直接飞回起点，任务结束；

所述步骤1)中，所述巡航模式中，M100按照预先规定的轨迹飞行，云台按照一定角度摆动，并将拍摄到的画面实时传送到APP端的步骤包括：确定飞行轨迹之前，首先要先获取到巡航区域四个顶点的GPS坐标，飞行轨迹采用蛇形迂回的方式，无人机在每走一段折线时，才会计算该线的终点和要转弯的地方；在巡航模式中，云台的摆动角度为左右摆动45°，飞行高度2m；

所述步骤2)中，巡航的速度随计算的结果变化具体如下，若在前进方向上没有检测到障碍物且超声波数据大于一定阈值时，以快速前进；若超声波数据小于该阈值时，无人机以慢速前进，进入预警状态，即前方可能出现障碍物；若前方检测到障碍物，则无人机以一个更慢的速度进入避障模式，在这里，判断是否有障碍物的超声波阈值小于进入预警状态的超声波阈值；避障结束后，再通过新一轮的判断决定巡航速度；

所述步骤2)中，判断前方是否有障碍物的步骤如下：首先获取最原始的深度图数据和超声波数据，超声波数据为无人机到最近障碍物的距离，深度图数据为未经处理的单通道图像；然后对获取到的深度图进行腐蚀、膨胀、二值化，得到一个二值化后的图像，且障碍物此时在图像中显示为白色，然后计算所有连续白色区域的面积，当至少一个连续白色的面积大于一定的阈值时，认为该方向上有障碍物。

2.根据权利要求1所述的基于M100开发平台的区域完全搜索与避障***的方法，其特征在于，所述步骤4)中，进入“右转后”状态的步骤如下，M100观察前方是否有障碍物，当发现前方危险时，图像传感器2号默认右边开启，这时观察右边是否安全，安全则右转，不安全则左转。

3.根据权利要求2所述的基于M100开发平台的区域完全搜索与避障***的方法，其特征在于，所述步骤4)中，优先判断前方是否有障碍物的步骤如下：在“左/右转后”状态中，每一次判断前方是否有障碍物，都是优先判断运动的方向是否有障碍物，然后再判断另一个图像传感器的数据。

4.根据权利要求3所述的基于M100开发平台的区域完全搜索与避障***的方法，其特征在于，所述步骤4)中，控制权交还给巡航模式的步骤如下：M100在方向调整为巡航方向后，接下来要做的就是平移回巡航的线上，在平移的过程中要注意观察平移的方向是否有障碍物，若有，则前进直到跨过障碍物，然后再继续平移。

5.根据权利要求4所述的基于M100开发平台的区域完全搜索与避障***的方法，其特征在于，所述步骤5)中，M100进入避障模式二的步骤如下，避障模式二状态时，图像传感器2号开启上方，当上方无障碍物时，无人机就会上升到一定高度，然后直接返航，否则会采取避障模式一飞行。

Images