CN108776492A - 一种基于双目相机的四轴飞行器自主避障与导航方法 - Google Patents

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

Info

Publication number
CN108776492A
CN108776492A CN201810678916.0A CN201810678916A CN108776492A CN 108776492 A CN108776492 A CN 108776492A CN 201810678916 A CN201810678916 A CN 201810678916A CN 108776492 A CN108776492 A CN 108776492A
Authority
CN
China
Prior art keywords
map
point
axle aircraft
pose
octree
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN201810678916.0A
Other languages
English (en)
Other versions
CN108776492B (zh
Inventor
李�瑞
史莹晶
邓质
杨峰
闫晓东
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China
Original Assignee
University of Electronic Science and Technology of China
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China filed Critical University of Electronic Science and Technology of China
Priority to CN201810678916.0A priority Critical patent/CN108776492B/zh
Publication of CN108776492A publication Critical patent/CN108776492A/zh
Application granted granted Critical
Publication of CN108776492B publication Critical patent/CN108776492B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/10—Simultaneous control of position or course in three dimensions
    • G05D1/101—Simultaneous control of position or course in three dimensions specially adapted for aircraft
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations

Abstract

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

Description

一种基于双目相机的四轴飞行器自主避障与导航方法
技术领域
本发明属于四轴飞行器技术领域，更为具体地讲，涉及一种基于双目相机的四轴飞行器自主避障与导航方法。
背景技术
近年来，四轴飞行器在生活、生产各个领域的应用呈现百花齐放的姿态，比如航拍、搜救、监控、物流等。但随着四轴飞行器应用领域的不断发展以及其飞行活动量的不断增加，不可避免地会对空域环境的其他飞行器以及地面第三方目标造成很大的安全隐患。因此四轴飞行器如何实现高效且准确的自主避障与导航已成为制约四轴飞行器发展的关键挑战之一。
目前，一些研究单位通过给四轴飞行器加载离线地图的方式规划好路径，并在飞行过程中通过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(Oriebted Fast and Rotated BRIEF)：一种快速特征点提取和描述的算法；
ICP(Iterative Closest Point)：迭代最近点算法；
BA(Bundle Adjustment)：光束平差法；
Bounding Box：包围盒；
AABB(Axis-aligned bouding box)：包围某物体且平行于坐标轴的最小六面体；
Max-min：通过右上角和左下角来唯一地定义一个包围体；
RRT*(Rapid-exploration Random Tree Star)：快速探索随机树算法；
图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、根据稠密地图中的点云三维数据的大小区间，得到一个Bounding Box，再将得到Bounding Box作为八叉树地图的扩展空间；
S5.2、结合实际飞行任务，根据碰撞检测的精度需求设置八叉树对象的分辨率为0.05m3；
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、当四轴飞行器按照实时规划的路径抵达终点位置时，自动切换到定点模式，在目标位置处悬停，在接收到降落指令后自动降落，完成此次飞行任务。
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (4)

1.一种基于双目相机的四轴飞行器自主避障与导航方法，其特征在于，包括以下步骤：
(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)、当四轴飞行器按照实时规划的路径抵达终点位置时，自动切换到定点模式，在目标位置处悬停，在接收到降落指令后自动降落，此完成此次飞行任务。
2.根据权利要求1所述的基于双目相机的四轴飞行器自主避障与导航方法，其特征在于，所述步骤(4)中，当前帧满足的插入条件为：(a)距离上一次重定位，双目相机已读入超过20帧图像；(b)距离上一关键帧插入，双目相机已读入超过20帧图像；(c)后端线程处于空闲状态：(d)当前帧跟踪局部地图，所匹配到的地图点数量少于50个；(e)当前帧匹配到的地图点数量小于最近一帧关键帧匹配到地图点数量的90％；
若当前帧满足以上任意一个条件，则将当前帧作为关键帧。
3.根据权利要求1所述的基于双目相机的四轴飞行器自主避障与导航方法，其特征在于，所述的稠密地图转换为八叉树地图的方法为：
1)、根据稠密地图中的点云三维数据的大小区间，得到一个Bounding Box，再将得到Bounding Box作为八叉树地图的扩展空间；
2)、结合实际飞行任务，根据碰撞检测的精度需求设置八叉树对象的分辨率为V；
3)、遍历稠密地图中的三维点云，将三维点云携带的信息转化成可否通过的信息，并存入八叉树地图的叶子节点中；
4)、采用递归方式依次递归地对八叉树节点的立方体块进行八等分，直到达到最大递归深度，即最小的立方体块与分辨率大小相同，得到包含环境信息且适合用于碰撞检测的八叉树地图。
4.根据权利要求1所述的基于双目相机的四轴飞行器自主避障与导航方法，其特征在于，所述的AABB盒在x、y、z三个方向上的取值范围分别是(xc-bx,xc+bx),(yc-by,yc+by),(zc-bz,zc+bz)，其中，xc,yc,zc分别是四轴飞行器在三个方向上的位置数据，bx、by、bz为常数，表示x、y、z三个方向上的取值范围。
CN201810678916.0A 2018-06-27 2018-06-27 一种基于双目相机的四轴飞行器自主避障与导航方法 Active CN108776492B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810678916.0A CN108776492B (zh) 2018-06-27 2018-06-27 一种基于双目相机的四轴飞行器自主避障与导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810678916.0A CN108776492B (zh) 2018-06-27 2018-06-27 一种基于双目相机的四轴飞行器自主避障与导航方法

Publications (2)

Publication Number Publication Date
CN108776492A true CN108776492A (zh) 2018-11-09
CN108776492B CN108776492B (zh) 2021-01-26

Family

ID=64030365

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810678916.0A Active CN108776492B (zh) 2018-06-27 2018-06-27 一种基于双目相机的四轴飞行器自主避障与导航方法

Country Status (1)

Country Link
CN (1) CN108776492B (zh)

Cited By (20)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109341697A (zh) * 2018-11-29 2019-02-15 北京宇航系统工程研究所 一种无人机航路规划安全性评估方法
CN109741372A (zh) * 2019-01-10 2019-05-10 哈尔滨工业大学 一种基于双目视觉的里程计运动估计方法
CN109940614A (zh) * 2019-03-11 2019-06-28 东北大学 一种融合记忆机制的机械臂多场景快速运动规划方法
CN110146085A (zh) * 2019-05-30 2019-08-20 中国人民解放军国防科技大学 基于建图和快速探索随机树的无人机实时规避重规划方法
CN110209202A (zh) * 2019-06-26 2019-09-06 深圳市道通智能航空技术有限公司 一种可行空间生成方法、装置、飞行器及飞行器系统
CN110361005A (zh) * 2019-06-26 2019-10-22 深圳前海达闼云端智能科技有限公司 定位方法、定位装置、可读存储介质及电子设备
CN110363789A (zh) * 2019-06-25 2019-10-22 电子科技大学 一种面向实际工程应用的长期视觉跟踪方法
CN111325780A (zh) * 2020-02-17 2020-06-23 天目爱视（北京）科技有限公司 一种基于图像筛选的3d模型快速构建方法
CN111457923A (zh) * 2019-01-22 2020-07-28 北京京东尚科信息技术有限公司 路径规划方法、装置及存储介质
CN111638727A (zh) * 2020-05-29 2020-09-08 西北工业大学 一种基于深度图像的多旋翼飞行器安全导航控制方法
CN111880573A (zh) * 2020-07-31 2020-11-03 电子科技大学 一种基于视觉惯导融合的四旋翼自主导航方法
CN112824836A (zh) * 2019-11-21 2021-05-21 北京图森智途科技有限公司 移动工具碰撞检测方法及相关设备
CN112904901A (zh) * 2021-01-14 2021-06-04 吉林大学 一种基于双目视觉slam与融合算法的路径规划方法
CN113203420A (zh) * 2021-05-06 2021-08-03 浙江大学 一种基于变密度搜索空间的工业机器人动态路径规划方法
CN113296498A (zh) * 2021-04-12 2021-08-24 山东科技大学 一种基于自适应分辨率八叉树地图的改进rrt路径规划方法
CN113362387A (zh) * 2021-04-12 2021-09-07 深圳大学 一种动态环境移动机器人的环境地图构建方法
CN113485356A (zh) * 2021-07-27 2021-10-08 西北工业大学 一种机器人快速运动规划方法
CN113741490A (zh) * 2020-05-29 2021-12-03 广州极飞科技股份有限公司 一种巡检方法、装置、飞行器及存储介质
CN113781846A (zh) * 2021-09-28 2021-12-10 中国人民解放军空军工程大学 一种水平维度空域冲突检测方法
CN115793715A (zh) * 2023-01-05 2023-03-14 雄安雄创数字技术有限公司 一种无人机辅助飞行方法、系统、装置及存储介质

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100066587A1 (en) * 2006-07-14 2010-03-18 Brian Masao Yamauchi Method and System for Controlling a Remote Vehicle
CN104597910A (zh) * 2014-11-27 2015-05-06 中国人民解放军国防科学技术大学 一种基于瞬时碰撞点的无人机非协作式实时避障方法
CN104881518A (zh) * 2015-04-30 2015-09-02 电子科技大学 一种飞行器的三维视景仿真系统
CN105242679A (zh) * 2015-10-22 2016-01-13 电子科技大学 一种四旋翼飞行器的控制系统设计方法
CN105892489A (zh) * 2016-05-24 2016-08-24 国网山东省电力公司电力科学研究院 一种基于多传感器融合的自主避障无人机系统及控制方法
CN107085437A (zh) * 2017-03-20 2017-08-22 浙江工业大学 一种基于eb‑rrt的无人机航迹规划方法
CN107278282A (zh) * 2016-09-29 2017-10-20 深圳市大疆创新科技有限公司 路径规划的方法、装置、飞行控制系统、全方位避障系统与无人机
US20180033318A1 (en) * 2016-07-29 2018-02-01 Ge Aviation Systems Llc Sense and avoid maneuvering
CN107703951A (zh) * 2017-07-27 2018-02-16 上海拓攻机器人有限公司 一种基于双目视觉的无人机避障方法及系统
CN107817811A (zh) * 2017-10-26 2018-03-20 哈尔滨市舍科技有限公司 基于视觉的无人机防碰撞装置及方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20100066587A1 (en) * 2006-07-14 2010-03-18 Brian Masao Yamauchi Method and System for Controlling a Remote Vehicle
CN104597910A (zh) * 2014-11-27 2015-05-06 中国人民解放军国防科学技术大学 一种基于瞬时碰撞点的无人机非协作式实时避障方法
CN104881518A (zh) * 2015-04-30 2015-09-02 电子科技大学 一种飞行器的三维视景仿真系统
CN105242679A (zh) * 2015-10-22 2016-01-13 电子科技大学 一种四旋翼飞行器的控制系统设计方法
CN105892489A (zh) * 2016-05-24 2016-08-24 国网山东省电力公司电力科学研究院 一种基于多传感器融合的自主避障无人机系统及控制方法
US20180033318A1 (en) * 2016-07-29 2018-02-01 Ge Aviation Systems Llc Sense and avoid maneuvering
CN107278282A (zh) * 2016-09-29 2017-10-20 深圳市大疆创新科技有限公司 路径规划的方法、装置、飞行控制系统、全方位避障系统与无人机
CN107085437A (zh) * 2017-03-20 2017-08-22 浙江工业大学 一种基于eb‑rrt的无人机航迹规划方法
CN107703951A (zh) * 2017-07-27 2018-02-16 上海拓攻机器人有限公司 一种基于双目视觉的无人机避障方法及系统
CN107817811A (zh) * 2017-10-26 2018-03-20 哈尔滨市舍科技有限公司 基于视觉的无人机防碰撞装置及方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
YAN XIAODONG: "Obstacle avoidance for outdoor flight of a quadrotor based on computer vision", 《2017 29TH CHINESE CONTROL AND DECISION CONFERENCE (CCDC)》 *
余小欢: "基于双目视觉的微型无人机室内3维地图构建", 《信息与控制》 *

Cited By (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109341697A (zh) * 2018-11-29 2019-02-15 北京宇航系统工程研究所 一种无人机航路规划安全性评估方法
CN109341697B (zh) * 2018-11-29 2021-02-09 北京宇航系统工程研究所 一种无人机航路规划安全性评估方法
CN109741372A (zh) * 2019-01-10 2019-05-10 哈尔滨工业大学 一种基于双目视觉的里程计运动估计方法
CN111457923A (zh) * 2019-01-22 2020-07-28 北京京东尚科信息技术有限公司 路径规划方法、装置及存储介质
CN111457923B (zh) * 2019-01-22 2022-08-12 北京京东乾石科技有限公司 路径规划方法、装置及存储介质
CN109940614A (zh) * 2019-03-11 2019-06-28 东北大学 一种融合记忆机制的机械臂多场景快速运动规划方法
CN109940614B (zh) * 2019-03-11 2021-01-22 东北大学 一种融合记忆机制的机械臂多场景快速运动规划方法
CN110146085B (zh) * 2019-05-30 2022-04-19 中国人民解放军国防科技大学 基于建图和快速探索随机树的无人机实时规避重规划方法
CN110146085A (zh) * 2019-05-30 2019-08-20 中国人民解放军国防科技大学 基于建图和快速探索随机树的无人机实时规避重规划方法
CN110363789A (zh) * 2019-06-25 2019-10-22 电子科技大学 一种面向实际工程应用的长期视觉跟踪方法
CN110363789B (zh) * 2019-06-25 2022-03-25 电子科技大学 一种面向实际工程应用的长期视觉跟踪方法
CN110209202A (zh) * 2019-06-26 2019-09-06 深圳市道通智能航空技术有限公司 一种可行空间生成方法、装置、飞行器及飞行器系统
CN110361005A (zh) * 2019-06-26 2019-10-22 深圳前海达闼云端智能科技有限公司 定位方法、定位装置、可读存储介质及电子设备
CN112824836A (zh) * 2019-11-21 2021-05-21 北京图森智途科技有限公司 移动工具碰撞检测方法及相关设备
CN111325780A (zh) * 2020-02-17 2020-06-23 天目爱视（北京）科技有限公司 一种基于图像筛选的3d模型快速构建方法
CN111325780B (zh) * 2020-02-17 2021-07-27 天目爱视（北京）科技有限公司 一种基于图像筛选的3d模型快速构建方法
CN113741490A (zh) * 2020-05-29 2021-12-03 广州极飞科技股份有限公司 一种巡检方法、装置、飞行器及存储介质
CN111638727B (zh) * 2020-05-29 2022-09-23 西北工业大学 一种基于深度图像的多旋翼飞行器安全导航控制方法
CN111638727A (zh) * 2020-05-29 2020-09-08 西北工业大学 一种基于深度图像的多旋翼飞行器安全导航控制方法
CN111880573A (zh) * 2020-07-31 2020-11-03 电子科技大学 一种基于视觉惯导融合的四旋翼自主导航方法
CN111880573B (zh) * 2020-07-31 2022-09-06 电子科技大学 一种基于视觉惯导融合的四旋翼自主导航方法
CN112904901A (zh) * 2021-01-14 2021-06-04 吉林大学 一种基于双目视觉slam与融合算法的路径规划方法
CN112904901B (zh) * 2021-01-14 2022-01-21 吉林大学 一种基于双目视觉slam与融合算法的路径规划方法
CN113296498A (zh) * 2021-04-12 2021-08-24 山东科技大学 一种基于自适应分辨率八叉树地图的改进rrt路径规划方法
CN113362387A (zh) * 2021-04-12 2021-09-07 深圳大学 一种动态环境移动机器人的环境地图构建方法
CN113296498B (zh) * 2021-04-12 2022-09-27 山东科技大学 一种基于自适应分辨率八叉树地图的改进rrt路径规划方法
CN113203420A (zh) * 2021-05-06 2021-08-03 浙江大学 一种基于变密度搜索空间的工业机器人动态路径规划方法
CN113485356B (zh) * 2021-07-27 2022-06-21 西北工业大学 一种机器人快速运动规划方法
CN113485356A (zh) * 2021-07-27 2021-10-08 西北工业大学 一种机器人快速运动规划方法
CN113781846A (zh) * 2021-09-28 2021-12-10 中国人民解放军空军工程大学 一种水平维度空域冲突检测方法
CN115793715A (zh) * 2023-01-05 2023-03-14 雄安雄创数字技术有限公司 一种无人机辅助飞行方法、系统、装置及存储介质
CN115793715B (zh) * 2023-01-05 2023-04-28 雄安雄创数字技术有限公司 一种无人机辅助飞行方法、系统、装置及存储介质

Also Published As

Publication number Publication date
CN108776492B (zh) 2021-01-26

Similar Documents

Publication Publication Date Title
CN108776492A (zh) 一种基于双目相机的四轴飞行器自主避障与导航方法
Jung et al. A direct visual servoing‐based framework for the 2016 IROS Autonomous Drone Racing Challenge
Polvara et al. Toward end-to-end control for UAV autonomous landing via deep reinforcement learning
Padhy et al. Deep neural network for autonomous uav navigation in indoor corridor environments
Tijmons et al. Obstacle avoidance strategy using onboard stereo vision on a flapping wing MAV
Kim et al. Deep neural network for real-time autonomous indoor navigation
CN105492985B (zh) 一种用于在环境内控制可移动物体的系统及方法
Gao et al. Online quadrotor trajectory generation and autonomous navigation on point clouds
CN107850902A (zh) 可移动物体上的相机配置
CN109407705A (zh) 一种无人机躲避障碍物的方法、装置、设备和存储介质
CN105786016B (zh) 无人机以及rgbd图像的处理方法
CN105847684A (zh) 无人机
US20200301015A1 (en) Systems and methods for localization
CN110362098A (zh) 无人机视觉伺服控制方法、装置以及无人机
CN107729808A (zh) 一种用于输电线路无人机巡检的图像智能采集系统及方法
CN108700890A (zh) 无人机返航控制方法、无人机和机器可读存储介质
CN205453893U (zh) 无人机
CN107637064A (zh) 用于图像处理的方法和设备
CN109683629A (zh) 基于组合导航和计算机视觉的无人机电力架线系统
CN109238288A (zh) 一种无人机室内自主导航方法
CN111831010A (zh) 一种基于数字空间切片的无人机避障飞行方法
CN111433701A (zh) 无人机的喷洒作业方法和装置
CN116242364A (zh) 一种基于深度强化学习的多无人机智能导航方法
Zhou et al. Toward autonomy of micro aerial vehicles in unknown and global positioning system denied environments
Doukhi et al. Deep reinforcement learning for autonomous map-less navigation of a flying robot

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant