一种基于双目相机的四轴飞行器自主避障与导航方法与流程

本发明属于四轴飞行器技术领域，更为具体地讲，涉及一种基于双目相机的四轴飞行器自主避障与导航方法。

背景技术

近年来，四轴飞行器在生活、生产各个领域的应用呈现百花齐放的姿态，比如航拍、搜救、监控、物流等。但随着四轴飞行器应用领域的不断发展以及其飞行活动量的不断增加，不可避免地会对空域环境的其他飞行器以及地面第三方目标造成很大的安全隐患。因此四轴飞行器如何实现高效且准确的自主避障与导航已成为制约四轴飞行器发展的关键挑战之一。

目前，一些研究单位通过给四轴飞行器加载离线地图的方式规划好路径，并在飞行过程中通过gps来进行定位，使四轴飞行器按照规划好的路径飞行以实现自主避障与导航。这种方法弊端很多，首先gps的定位精度达到米级，精度太低，这样会导致四轴飞行器在飞行过程中容易偏离规划好的路径；然后是预先加载离线地图，利用离线地图来进行路径规划，这样会使四轴飞行器只能在静态的场景中实现自主避障与导航，应用场景大大受限。

基于此，本发明采用了视觉检测系统，该系统具有体积小、功耗低、重量轻、精度高等优势，可以实现精确定位，并且能够从复杂混乱的飞行环境中实时获取环境信息，从而使四轴飞行器不仅能在静态场景还能够在动态场景中实现自主避障与导航。

本发明严格遵循自主避障与导航的设计要求，设计了一种基于双目相机的自主避障与导航四轴飞行器平台，具备良好的自主避障与导航能力。

技术实现要素：

本发明的目的在于克服现有技术的不足，提供一种基于双目相机的四轴飞行器自主避障与导航方法，基于实时碰撞检测结果进行路径规划，实现自主避障和导航，最终完成四轴飞行器的位置控制。

为实现上述发明目的，本发明一种基于双目相机的四轴飞行器自主避障与导航方法，其特征在于，包括以下步骤：

(1)、系统初始化

系统上电并初始化，初始化完成后，通过遥控器将四轴飞行器的控制模式切换到offborad模式，然后四轴飞行器从地面初始位置处起飞至竖直方向高度h处；

(2)、生成初始局部地图

四轴飞行器前端的双目相机读入第一帧图像，并均匀化提取图像的orb特征点，如果提取到的orb特征点数量小于n，则重新读取图像，直至特征点数量大于n；然后将提取到的orb特征点作为地图点，生成初始局部地图；

(3)、定位并跟踪局部地图

(3.1)、双目相机以每秒n帧的速度读入图像，并分别提取前后相邻两帧图像的orb特征点；

(3.2)、匹配前后相邻两帧图像的orb特征点,得到这两帧图像的匹配点对；

(3.3)、利用icp算法计算这两帧图像的匹配点对，求解出这两帧图像间的位姿变化，若icp算法求解失败，则进入步骤(3.4)，否则，计算得到四轴飞行器当前位姿的估计值，再进入步骤(3.5)；

(3.4)、从以前的图像中找到与当前帧匹配点对最多的图像，再通过icp算法求解四轴飞行器当前位姿的估计值，实现重定位；

(3.5)、将前后相邻两帧图像中的后一帧图像作为当前帧，然后根据当前位姿的估计值跟踪初始局部地图，将初始局部地图的地图点投影到当前帧，得到地图点与当前帧的orb特征点的匹配点对，根据匹配点对，通过局部ba算法对当前位姿的估计值进行优化；

(4)、构建位姿图和局部地图

(4.1)、将满足插入条件的当前帧作为关键帧，并插入到后端线程中；在后端线程中，将关键帧中优化后的当前位姿估计值作为顶点，相邻两关键帧的帧间位姿变化作为边，从而构建出位姿图；

(4.2)、将关键帧与其相邻的关键帧进行orb特征点匹配，然后再三角化处理这些匹配点对，得到新的地图点；利用ba算法对初始局部地图的地图点以及与地图点关联的关键帧的位姿估计值进行优化，得到更新后的位姿图和局部地图；

(5)、构建八叉树地图

对双目相机拍摄得到的三维点云图进行滤波处理，将处理后的三维点云图与位姿图的顶点相关联，再根据位姿图的边拼接这些三维点云图，得到表征周围环境的稠密地图，最后再将稠密地图转换为八叉树地图；

(6)、四轴飞行器构建aabb包围盒

设置四轴飞行器的避障飞行安全距离d；为四轴飞行器构造一个包围球体，然后在这个包围球体的基础上结合避障飞行安全距离d，采用max-min算法构建一个aabb包围盒；

(7)、对八叉树地图和aabb包围盒进行碰撞检测

(7.1)、设置八叉树地图中每个节点被占据的概率阈值为λ；

(7.2)、从八叉树地图的根节点开始遍历，判断当前节点被占据的概率是否超过阈值λ，若不超过λ，则认为该节点与aabb包围盒不会发生碰撞，直接输出判断结果，然后进入步骤(7.5)；否则，进入步骤(7.3)；

(7.3)、通过分离轴定理判断aabb包围盒与该节点间是否会发生碰撞，若判断结果为发生碰撞，则直接输出判断结果，然后进入步骤(7.5)；否则，进入步骤(7.4)；

(7.4)、判断该节点是否为叶节点，若该节点为叶子节点，则不会发生碰撞，并直接输出判断结果，进入步骤(7.5)；否则继续遍历该节点的八个子节点与aabb包围盒进行碰撞检测，直到出现碰撞检测结果为不发生碰撞或者该节点对应的所有子节点被检测完，则停止检测，并输出碰撞检测结果，进入步骤(7.5)；

(7.5)、根据输出的碰撞检测结果，设置障碍物约束；

(8)、四轴飞行器自主导航

(8.1)、规划随机树扩展区域，并进行随机树的扩展；

(8.1.1)、手动设定本次飞行的终点qgoal；四轴飞行器以竖直方高度h处作为路径规划起点，qgoal作为路径规划的终点，在x、y、z三轴上分别设置(-ax,ax)，(-ay,ay)，(-az,az)作为随机树的扩展区域，ax、ay、az分别为x、y、z三轴上的取值；

(8.1.2)、从随机树扩展区域中随机选取一个目标点qrandom，以度量函数为依据，选择离qrandom最近的状态点qnear，从qnear到qrandom的向量中按照步长为s选定一个状态点qnew；如果从qnear到qnew满足四轴飞行器自身飞行的性能的约束和障碍物约束，那么就把qnew加入到随机树中；

其中，ζ表示一个常量，dobstacle表示障碍物约束，dperformance表示四轴飞行器自身的性能约束，表示偏航角转动限制，(xa,ya,za)和(xb,yb,zb)分别表示两个状态点的坐标；

(8.1.3)、在指定时间内，重复步骤(8.1.2)，直到随机树中出现了与qgoal的距离在s内的qnew，从而得到扩展后的随机树以及相应的路径状态点；

(8.2)、四轴飞行器沿着(8.1)得到的路径状态点开始飞行，接收到第一个状态点，并根据当前位姿估计值控制四轴飞行器向该状态点飞行，当四轴飞行器与第一个状态点的距离小于s时，便开始接收第二个状态点，并依次类推，接收完所有状态点；如果在飞行过程中某一状态点出发生碰撞，则反馈发生碰撞的状态点，并以当前状态点位置为起点，返回步骤(8.1)，重新规划路径；

(8.3)、当四轴飞行器按照实时规划的路径抵达终点位置时，自动切换到定点模式，在目标位置处悬停，在接收到降落指令后自动降落，此完成此次飞行任务。

本发明的发明目的是这样实现的：

本发明基于双目相机的四轴飞行器自主避障与导航方法，通过icp算法处理相邻两帧图像中的匹配点对，使四轴飞行器实现精确定位，同时生成能够准确表征周围环境的稠密地图。再将稠密地图转化为适合用于碰撞检测算法的八叉树地图，通过四轴飞行器与八叉树地图碰撞检测反馈回来的结果，得到改进的rrt*算法所需要的障碍物约束，进而实时地规划出可通行的路径。四轴飞行器沿着规划好的路径飞行，便可以成功避开障碍物，到达预先设定好的终点。

同时，本发明基于双目相机的四轴飞行器自主避障与导航方法还具有以下

有益效果：

(1)、本发明基于视觉完成对四轴飞行器的定位，通过视觉里程计算并优化当前位姿，在环境条件满足情况下四轴飞行器具备良好的定位精度，精度达到厘米级。该定位方法为四轴飞行器的后续飞行提供了精度保障。

(2)、本发明将稠密地图转化成八叉树地图，八叉树地图对稠密地图中的信息进行了针对性的优化，八叉树地图只保留了可否通行的信息，稠密地图中的其他信息全部被过滤掉了，从而大大减少了地图占用的存储空间；另一方面，八叉树地图的结构适合碰撞检测算法的实现，提升了碰撞检测算法的执行速率。

(3)、在路径规划算法方面，本发明采用了改进的rrt*算法。原始rrt*算法的度量函数采用的是较为简单的距离度量方式，改进后的rrt*算法结合四轴飞行器的实际飞行性能约束，重新设计了度量函数，使算法的效率大大提升，能够在复杂环境中得到一个较为理想的规划路径。

(4)、八叉树地图随着摄像头拍摄场景的变换，不断地进行更新，同时路径规划算法也根据更新后的八叉树地图实时地反馈新的有效路径。因而，本发明在动态场景中也能够进行有效且可靠的避障与导航。

附图说明

图1是本发明基于双目相机的四轴飞行器自主避障与导航方法流程图；

图2是定位并跟踪局部地图的流程图；

图3是稠密地图转八叉树地图的流程图；

图4是八叉树地图示意图。

具体实施方式

下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。

实施例

为了方便描述，先对具体实施方式中出现的相关专业术语进行说明：

offboard:外部模式；

orb(oriebtedfastandrotatedbrief)：一种快速特征点提取和描述的算法；

icp(iterativeclosestpoint)：迭代最近点算法；

ba(bundleadjustment)：光束平差法；

boundingbox：包围盒；

aabb(axis-alignedboudingbox)：包围某物体且平行于坐标轴的最小六面体；

max-min：通过右上角和左下角来唯一地定义一个包围体；

rrt*(rapid-explorationrandomtreestar)：快速探索随机树算法；

图1是本发明基于双目相机的四轴飞行器自主避障与导航方法流程图。

基于双目相机的自主避障与导航四轴飞行器能够自主完成从起点到指定目标点的飞行任务。在飞行过程中，搭载了机载计算机的四轴飞行器能够实时更新周围环境的信息，并根据得到的信息更新飞行路径，避开障碍物抵达目标位置。整个飞行过程均由四轴飞行器自主完成，无须手动控制。

本实施例将飞行任务划分为三个子任务，分别是(1)四轴飞行器基于视觉进行定位，并生成飞行环境的稠密地图；(2)利用稠密地图生成的八叉树地图，四轴飞行器与飞行环境进行实时碰撞检测；(3)通过计算得到的碰撞检测结果，四轴飞行器进行路径规划，更新飞行路径，最终完成飞行任务。

下面我们结合图1，对本发明一种基于双目相机的四轴飞行器自主避障与导航方法进行详细说明，具体包括以下步骤：

s1、系统初始化

系统上电并初始化，初始化完成后，通过遥控器将四轴飞行器的控制模式切换到offborad模式，然后四轴飞行器从地面初始位置处起飞至竖直方向高度h＝3米处，并悬停于此；

s2、生成初始局部地图

四轴飞行器开启一个线程，运行视觉里程计；

四轴飞行器前端的双目相机读入第一帧图像，均匀化提取图像的orb特征点，如果提取到的orb特征点数量小于1500个，则重新读取图像，直至特征点数量大于1500个。如果图像的特征点数量过少，可能会导致后续匹配点对数量不足以满足icp算法的条件；

四轴飞行器开启再开启一个新线程，开始局部建图；将第一帧图像的特征点作为地图点，然后将提取到的orb特征点作为地图点，生成初始局部地图；

s3、定位并跟踪局部地图

s3.1、按照图2所示，双目相机以每秒30帧的速度读入图像，并分别提取前后相邻两帧图像的orb特征点；

s3.2、匹配前后相邻两帧图像的orb特征点,得到这两帧图像的匹配点对；

s3.3、利用icp算法计算这两帧图像的匹配点对，求解出这两帧图像间的位姿变化，若icp算法求解失败，则进入步骤s3.4，否则，计算得到四轴飞行器当前位姿的估计值，再进入步骤s3.5；

s3.4、从以前的图像中找到与当前帧匹配点对最多的图像，再通过icp算法求解四轴飞行器当前位姿的估计值，实现重定位；

s3.5、将前后相邻两帧图像中的后一帧图像作为当前帧，然后根据当前位姿的估计值跟踪初始局部地图，将初始局部地图的地图点投影到当前帧，得到地图点与当前帧的orb特征点的匹配点对，根据匹配点对，通过局部ba算法对当前位姿的估计值进行优化，地图点不作优化，因为如果这里对地图点的位置也进行优化，精度提升有限，但运算量较大，导致运算时间增加，定位的实时性受到影响；

s4、构建位姿图和局部地图

s4.1、找出满足插入条件的当前帧，将满足插入条件的当前帧作为关键帧，并插入到后端线程中；

其中，若当前帧满足以下任意一个条件，则将当前帧作为关键帧；

(a)距离上一次重定位，双目相机已读入超过20帧图像；(b)距离上一关键帧插入，双目相机已读入超过20帧图像；(c)后端线程处于空闲状态：(d)当前帧跟踪局部地图，所匹配到的地图点数量少于50；(e)当前帧匹配到的地图点数量小于最近一帧关键帧匹配到地图点数量的90％；

在后端线程中，将关键帧中优化后的当前位姿估计值作为顶点，相邻两关键帧的帧间位姿变化作为边，从而构建出位姿图；

s4.2、将关键帧与其相邻的关键帧进行orb特征点匹配，然后再三角化处理这些匹配点对，得到新的地图点；利用ba算法对局部地图的地图点以及与地图点关联的关键帧的位姿估计值进行优化，得到更新后的位姿图和局部地图。这里对地图点的位置进行了优化，是为了提升局部地图中地图点的精度，从而使s3.5中对当前帧位姿的优化精度更高；

s5、构建八叉树地图

对双目相机拍摄得到的三维点云图进行滤波处理，将处理后的三维点云图与位姿图的顶点相关联，再根据位姿图的边拼接这些三维点云图，得到表征周围环境的稠密地图，最后再将稠密地图转换为八叉树地图；

其中，如图3所示，稠密地图转换为八叉树地图的方法为：

s5.1、根据稠密地图中的点云三维数据的大小区间，得到一个boundingbox，再将得到boundingbox作为八叉树地图的扩展空间；

s5.2、结合实际飞行任务，根据碰撞检测的精度需求设置八叉树对象的分辨率为0.05m³；

s5.3、遍历稠密地图中的三维点云，将三维点云携带的信息转化成可否通过的信息，并存入八叉树地图的叶子节点中；

s5.4、采用递归方式依次递归地对八叉树节点的立方体块进行八等分，直到达到最大递归深度，即最小的立方体块与分辨率大小相同，得到包含环境信息且适合用于碰撞检测的八叉树地图；

s6、四轴飞行器构建aabb包围盒

设置四轴飞行器的避障飞行安全距离d；为四轴飞行器构造一个包围球体，然后在这个包围球体的基础上结合避障飞行安全距离d，采用max-min算法构建一个aabb包围盒，其中，aabb盒在x、y、z三个方向上的取值范围分别是(xc-0.5,xc+0.5),(yc-0.5,yc+0.5),(zc-0.7,zc+0.7),其中xc,yc,zc分别是四轴飞行器在三个方向上的位置数据；

s7、对八叉树地图和aabb包围盒进行碰撞检测

s7.1、设置八叉树地图中每个节点被占据的概率阈值为λ＝0.8；

s7.2、从八叉树地图的根节点开始遍历，判断当前节点被占据的概率是否超过阈值λ，若不超过λ，则认为该节点与aabb包围盒不会发生碰撞，直接输出判断结果，然后进入步骤s7.5；否则，进入步骤s7.3；

s7.3、通过分离轴定理判断aabb包围盒与该节点间是否会发生碰撞，若判断结果为发生碰撞，则直接输出判断结果，然后进入步骤s7.5；否则，进入步骤s7.4；

s7.4、判断该节点是否为叶节点，若该节点为叶子节点，则不会发生碰撞，并直接输出判断结果，进入步骤s7.5；否则继续遍历该节点的八个子节点与aabb包围盒进行碰撞检测，直到出现碰撞检测结果为不发生碰撞或者该节点对应的所有子节点被检测完，则停止检测，并输出碰撞检测结果，进入步骤s7.5；

在本实施例中，如图4所示，从根节点到叶节点一共有三层，其所在层数依次为f0,f1,f2。根节点为f01，f01有8个子节点，从左到右依次为f11,f12,...,f18。每个子节点又有8个叶节点，f11的叶节点从左至右依次为f21,f22,...,f28，其他子节点类似。从f11开始，先对f11与aabb包围盒进行碰撞检测，若检测结果为不发生碰撞，则继续对f11的叶节点进行检测。从f21到f28依次检测，若出现不发生碰撞的检测结果便直接输出，否则退回到f1层，并对f12进行类似f11的检测方式，若f12及其叶节点也没有出现不发生碰撞的检测结果，便开始对f13进行检测，依次进行下去，直到出现碰撞检测结果为不发生碰撞或者f01的所有子节点被检测完，才停止检测并输出碰撞检测结果。

s7.5、根据输出的碰撞检测结果，设置障碍物约束；

s8、完成路径规划，并实现四轴飞行器的自主导航

s8.1、通过改进后的rrt*算法规划随机树扩展区域，并进行随机树的扩展；

s8.1.1、手动设定本次飞行的终点qgoal；四轴飞行器以竖直方高度h处作为路径规划起点，qgoal作为路径规划的终点，x、y、z三轴上范围分别设置为(-10m,10m)，(-10m,10m),(-1.5m,1.5m)，作为随机树的扩展区域；

s8.1.2、从随机树扩展区域中随机选取一个目标点qrandom，以度量函数为依据，选择离qrandom最近的状态点qnear，从rqaen到qrandom的向量中按照步长为0.3m选定一个状态点qnew；如果从qnear到qnew满足四轴飞行器自身飞行的性能的约束和障碍物约束，那么就把qnew加入到随机树中；

其中，dobstacle表示障碍物约束，dperformance表示四轴飞行器自身的性能约束，表示偏航角转动限制，(xa,ya,za)和(xb,yb,zb)分别表示两个状态点的坐标。在满足约束条件的情况下，通过这两个状态点的欧式距离选择最邻近点；

s8.1.3、在指定时间内，重复步骤s8.1.2，直到随机树中出现了与qgoal的距离在0.3m内的qnew，从而得到扩展后的随机树以及相应的路径状态点；

s8.2、四轴飞行器沿着s8.1得到的路径状态点开始飞行，接收到第一个状态点，并根据当前位姿估计值控制四轴飞行器向该状态点飞行，当四轴飞行器与第一个状态点的距离小于0.3m时，便开始接收第二个状态点，并依次类推，接收完所有状态点；如果在飞行过程中某一状态点出发生碰撞，则反馈发生碰撞的状态点，并以当前状态点位置为起点，返回步骤s8.1，重新规划路径；

s8.3、当四轴飞行器按照实时规划的路径抵达终点位置时，自动切换到定点模式，在目标位置处悬停，在接收到降落指令后自动降落，完成此次飞行任务。

尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。