CN111880573A - 一种基于视觉惯导融合的四旋翼自主导航方法 - Google Patents

一种基于视觉惯导融合的四旋翼自主导航方法 Download PDF

Info

Publication number
CN111880573A
CN111880573A CN202010757433.7A CN202010757433A CN111880573A CN 111880573 A CN111880573 A CN 111880573A CN 202010757433 A CN202010757433 A CN 202010757433A CN 111880573 A CN111880573 A CN 111880573A
Authority
CN
China
Prior art keywords
rotors
polynomial
track
locus
navigation
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
CN202010757433.7A
Other languages
English (en)
Other versions
CN111880573B (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 CN202010757433.7A priority Critical patent/CN111880573B/zh
Publication of CN111880573A publication Critical patent/CN111880573A/zh
Application granted granted Critical
Publication of CN111880573B publication Critical patent/CN111880573B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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/12—Target-seeking control
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

本发明公开了一种基于视觉惯导融合的四旋翼自主导航方法，先利用视觉惯导融合的定位方式，结合相机图像信息和四旋翼位姿信息对四旋翼实现高精度定位功能，再通过地面站系统的上位机发布导航任务指令，完成环境三维重建和自主导航探测搜索任务；在整个导航过程无需外部设备和人为控制，系统具有良好的自主性，此外平台还具有低成本、高精度和强扩展性等特点。

Description

一种基于视觉惯导融合的四旋翼自主导航方法
技术领域
本发明属于四旋翼技术领域，更为具体地讲，涉及一种基于视觉惯导融合的四旋翼自主导航方法。
背景技术
随着人工智能技术的发展，人工智能面临发展的重大历史机遇，建立新一代人工智能关键共性技术体系。在自主无人系统的智能技术中重点突破自主无人系统计算架构、复杂动态场景感知与理解、实时精准定位、面向复杂环境的适应性智能导航等共性技术，无人机自主控制以及汽车、船舶和轨道交通自动驾驶等智能技术，服务机器人、特种机器人等核心技术，支撑无人系统应用和产业发展。而未知环境下的自主导航作为其实现智能化的关键技术在各领域都发挥着至关重要的作用，如地震等自然灾害发生后进入受损建筑物寻找幸存者，在军事战争中执行先前的侦察任务等。
随着无人机应用场景的多样化，普通机器人的路径规划方案已无法满足无人机在三维未知环境中快速导航需求，针对集成定位、感知和规划的小型无人机平台，运动规划需考虑以下三个关键问题：第一，飞行系统复杂的运动、动力学，规划面临着快速飞行和动力学约束限制等条件；第二，在平台有限的计算能力下，所需具备的在线实时规划能力；第三，规划结果的安全可行性和完整最优性。
发明内容
本发明的目的在于克服现有技术的不足，提供一种基于视觉惯导融合的四旋翼自主导航方法，通过视觉惯导融合的方式完成复杂场景下的四旋翼自主导航定位。
为实现上述发明目的，本发明一种基于视觉惯导融合的四旋翼自主导航方法，其特征在于，包括以下步骤：
(1)、四旋翼上电并初始化；
(2)、对四旋翼进行视觉惯导定位，从而获取四旋翼运行期间不同时刻的位置和姿态；
(3)、构建四旋翼的三维导航地图；
(3.1)、通过四旋翼的双目相机获取每一个时刻的当前帧点云数据；
(3.2)、将每一个时刻的当前帧点云数据以及对应时刻的位置和姿态一起输入至TSDF，TSDF通过积分的方式构建出三维地图；
(3.3)、将三维地图转化成八叉树地图，从而得到四旋翼的三维导航地图；
(4)、无障碍离散轨迹搜索；
首先将四旋翼所在位置处标记为起始点，然后给定一个待探索的目标位置，最后采用基于BIT*路径规划算法在三维导航地图中搜索出一条从起始点到目标位置的无障碍离散轨迹；
(5)、无障碍离散轨迹的优化；
(5.1)、建立四旋翼的运动方程和动力学模型；
四旋翼的运动方程为：
Figure RE-GDA0002684613790000021
Figure RE-GDA0002684613790000022
其中，r表示四旋翼的位置向量，ω表示四旋翼的角速度向量，f、M分别表示四旋翼的净推力和力矩，J、m表示四旋翼的惯性张量和质量，zB表示推力方向的单位向量，zW表示重力方向的单位向量；
四旋翼的动力学模型为：
Figure RE-GDA0002684613790000023
Figure RE-GDA0002684613790000024
其中，er、ev、eR、eω分别表示位置、速度、方向和角速度的误差向量，kr、 kv、kR、kω分别表示位置、速度、姿态和角速度的控制量，R表示四旋翼的姿态，下标d表示期望值，
Figure RE-GDA0002684613790000025
表示角速度的估计值；
根据运动方程和动力学模型确定出四旋翼的全状态空间，记为位置 r＝[x,y,z]、速度
Figure RE-GDA0002684613790000026
姿态R→[φ,θ,ψ]、角速度ω；x,y,z表示位置r在坐标轴下的取值，φ,θ,ψ分别表示四旋翼的滚转角、俯仰角和偏航角；
(5.2)、根据微分平坦算法将全状态空间简化为4维的平坦输出空间 [x,y,z,ψ]；
(5.3)、根据平坦输出空间[x,y,z,ψ]优化无障碍离散轨迹；
(5.3.1)、在实际工程中，设定无人机机头方向始终朝向运行轨迹前行方向，ψ角的取值为运行轨迹的切向方向与x轴方向的夹角；
(5.3.2)、将无障碍离散轨迹按照离散点划分为λ段多项式，然后对每一段多项式分别在X、Y、Z轴上进行解耦，分别得到一组多项式轨迹序列，记为fx(t)、 fy(t)和fz(t)；
其中，fx(t)、fy(t)和fz(t)的形式相同，以X轴为例，多项式轨迹序列表示为：
Figure RE-GDA0002684613790000031
其中，i分段段数，i＝1,2,…,λ；j表示每一段多项式的阶数，j＝0,1,2,…,N， N为设定的多项式轨迹的总阶数，Ti为到达第i个离散点的时间，pi,j为第i段多项式轨迹的第j阶的系数；
(5.3.3)、获取每一段多项式轨迹的时间间隔；
根据工程经验，设fx(t)中每一段多项式轨迹间的运动都是经过匀加速、匀速、匀减速的过程；在任意一段多项式轨迹间，根据两个离散点i与i-1的欧式距离di,i-1，给定的最大加速度amax及最大速度vmax，计算出两个离散点i与i-1的时间间隔ΔT；
Figure RE-GDA0002684613790000032
(5.3.5)、采用fx(t)每一段多项式轨迹的四阶导数的平方在对应时间段上的积分来构成代价函数，同时保证多项式轨迹有解，将代价函数用二次型的形式表示为：
Figure RE-GDA0002684613790000041
其中，pi对应fx(t)在第i段上的系数；Qi为第i段多项式轨迹的代价函数的二次型矩阵；
再将代价函数的二次型形式转换成矩阵形式，表示为：
Figure RE-GDA0002684613790000042
(5.3.6)、设置每一段代价函数的连续性约束条件；
Figure RE-GDA0002684613790000043
其中，A为约束矩阵，D＝[d1,…,di,…,dλ]T，di＝[di,start,di,end]T，di,start为第i段多项式轨迹起始点位置及其各阶导数组成的向量，di,end为第i段多项式轨迹终点位置及其各阶导数组成的向量；
(5.3.7)、先将X轴方向上的最优多项式轨迹的求取描述为一个二次规划 QP问题，再将连续性约束条件带入矩阵形式的代价函数Jtotal，得到：
Figure RE-GDA0002684613790000044
设置变换矩阵C，通过变换矩阵C将D中各变量重新排序，使D分解为固定变量DF和待优化变量Dp，其中，DF表示已知位置，Dp表示已知位置的各阶导数；
根据DF和Dp的维数，将二次规划方程的中间矩阵分为四个部分RFF、RFP、 RPF、RPP，从而将代价函数Jtotal改写为如下形式：
Figure RE-GDA0002684613790000051
其中，RFF为固定优化矩阵块、
Figure RE-GDA0002684613790000052
为混合优化矩阵块、RPP为待优化矩阵块；
进而展开得到：
Figure RE-GDA0002684613790000053
在X轴方向上求取最优多项式轨迹：将代价函数Jtotal对Dp求偏导，并令其为0，得到代价函数的最小值，进而求得待优化变量Dp的最优值
Figure RE-GDA0002684613790000054
Figure RE-GDA0002684613790000055
(5.3.8)、通过
Figure RE-GDA0002684613790000056
逆向推导求解，得到最优多项式轨迹在X轴上的最优参数[p1,p2,…,pλ]T；
(5.3.9)、同理，分别求得在Y、Z轴方向上求取最优多项式轨迹，及对应的最优参数；
最后，根据X、Y、Z轴方向上求取最优多项式轨迹及对应的最优参数，得到三维空间中优化后的理想运行轨迹；
(5.4)、判断理想运行轨迹中间是否存在障碍物，若有障碍物，则在理想运行轨迹的两个端点中间插入一个新离散点，再以这三个点作为轨迹优化的离散路径点，并返回步骤(5.3)，否则，将理想运行轨迹作为四旋翼的运行轨迹，并进入步骤(6)；
(6)、四旋翼的运行轨迹跟随
(6.1)、根据等间隔的原则在运行轨迹上设置轨迹跟随路径点集 {P1,P2,…,PL}；
(6.2)、设四旋翼的当前位置为P0，在轨迹跟随路径点集中选出第一个轨迹跟随路径点作为当前规划路径点，记为Pcur，下一个规划路径点记为Pnext，并以此类推；
(6.3)、将轨迹跟随拆分成径向跟随与切向跟随，那么四旋翼在进行轨迹跟随时，其切向跟随速度方向为PcurPnext方向，大小设为固定值vt；径向跟随速度方向为PcurPnext的垂线方向，大小为vp，
Figure RE-GDA0002684613790000061
Figure RE-GDA0002684613790000064
为增益值，
Figure RE-GDA0002684613790000062
为四旋翼在当前规划路径点与PcurPnext的垂直距离；
将切向跟随速度vt和径向跟随速度vp转化到世界坐标系的X轴和Y轴方向后，发送给飞行控制模块；
(6.4)、飞行控制模块控制四旋翼跟随规划路径点飞行，在跟随过程中，飞行控制模块实时判断四旋翼机头方向与PcurPnext方向的角度大小，以及PcurPnext方向与P0Pnext的方向的夹角大小或P0到Pnext的距离dP0Pnext的大小，如果四旋翼机头方向与PcurPnext方向的角度大于阈值γ时，则调整偏航角ψ，保证四旋翼前进方向始终处于相机视角范围以内；如果PcurPnext方向与P0Pnext方向的夹角大于90度，或者
Figure RE-GDA0002684613790000063
小于设定阈值d1时，则进入步骤(6.5)；
(6.5)、更新三维导航地图，再通过分离轴定律对当前运行轨迹上的剩余路径点与更新后的三维导航地图进行碰撞检测，若发生碰撞，则返回步骤(4)以当前位置为起点重新规划路径；若不发生碰撞，则更新下一规划路径点；
(6.6)、判断四旋翼是否已经到达最后一个规划路径点PL，或者四旋翼当前位置P0与PL的距离小于阈值d2，d2＜d1，则导航任务完成，四旋翼进入定点状态；否则，返回步骤(6.5)；
(7)、导航结束
PC端上位机控制四旋翼自主降落，通过遥控器将四旋翼切到手动模式，上锁断电。。
本发明的发明目的是这样实现的：
本发明一种基于视觉惯导融合的四旋翼自主导航方法，先利用视觉惯导融合的定位方式，结合相机图像信息和四旋翼位姿信息对四旋翼实现高精度定位功能，再通过地面站系统的上位机发布导航任务指令，完成环境三维重建和自主导航探测搜索任务；在整个导航过程无需外部设备和人为控制，系统具有良好的自主性，此外平台还具有低成本、高精度和强扩展性等特点。
同时，本发明一种基于视觉惯导融合的四旋翼自主导航方法还具有以下有益效果：
(1)、聚焦无人机的智能化、小型化发展，本发明以低成本的IMU和市面的四旋翼无人机进行开发，结合一个高性能低功耗的嵌入式板载计算机，设计了各模块中实时高效的算法，完成了高精度的定位和导航；
(2)、本发明建图算法结合实际平台的运算能力和应用需求，为了兼顾精度与实效性，采用了改进的TSDF构建方法来支持动态增长的地图，实现了大范围场景的实时稠密建图，且无需获得地图的先验信息。使得无人机具有绘制三维地图与记忆环境的功能，PC端可通过WIFI与无人机进行数据通信，实时观测无人机周围的环境；
(3)、针对无人机的动力学特点，设计了最小能量消耗轨迹生成方法，同时为了使轨迹更加光滑，每段轨迹采用梯形速度曲线进行时间分配。在生成轨迹时采用将能量损耗作为优化量，采用二次规划的方法，最终生成一条光滑、符合动力学约束、能量消耗小的轨迹；
(4)、本发明自主设计了一种基于矢量速度分解的轨迹跟随方法，解决了控点控制中无人机飞行不连续、速度低、跟随效果差等特点，根据无人机当前状态与生成轨迹的径向切向偏差生成控制量，保证了无人机对轨迹的快速跟随效果；
(5)、系统平台模块化的设计方式使得平台具备较强的扩展能力支持后续功能的扩展，可满足实际导航更高的应用需求，可进行目标跟踪、目标打击等多功能的扩展。
附图说明
图1是本发明一种基于视觉惯导融合的四旋翼自主导航方法流程图；
图2是四旋翼在树林自主导航的示意图；
图3是四旋翼的系统框图；
图4是视觉惯性里程计算法的流程图；
图5是TSDF建图算法的流程图；
图6是无障碍离散轨迹搜索流程图；
图7是四旋翼轨迹跟随控制示意图。
具体实施方式
下面结合附图对本发明的具体实施方式进行描述，以便本领域的技术人员更好地理解本发明。需要特别提醒注意的是，在以下的描述中，当已知功能和设计的详细描述也许会淡化本发明的主要内容时，这些描述在这里将被忽略。
实施例
为了方便描述，先对具体实施方式中出现的相关专业术语进行说明：
GFTT：一种改进的Harris角点提取方法；
KLT：一种特征跟踪的光流法；
SFM：一种基于视觉的运动恢复方法；
里程计：估计相邻图像间相机的运动；
点云：由一组离散的点表示的地图；
TSDF：截断符号距离函数，TSDF构建表示一种表面建图方法；
八叉树地图：一种用于导航的地图形式；
MarchingCubes：一种建图可视化算法。
Rviz：ROS机器人系统提供的3D可视化和调试工具；
Mavros：完成飞行控制模块消息与ROS消息相互通信的机制；
图1是本发明一种基于视觉惯导融合的四旋翼自主导航方法流程图。
在本实施例中，如图2所示，以四旋翼在树林导航飞行为例，对本发明一种基于视觉惯导融合的四旋翼自主导航方法进行详细说明，如图1所示，包括以下步骤：
S1、四旋翼上电并初始化；
在本实施例中，四旋翼的系统架构如图3所示，对四旋翼的初始化的具体过程为：地面站模块PC端远程登录嵌入式板载计算机启动相机节点和Mavros 节点，发布频率为30Hz的图像信息和频率为50Hz的IMU信息；启动导航模块的其余程序节点，导航模块初始化；PC端启动上位机节点并连接飞行控制模块。
S2、对四旋翼进行视觉惯导定位，从而获取四旋翼运行期间不同时刻的位置和姿态；
S2.1、如图4所示，预处理模块读入图像信息和IMU的测量数据，提取图像的GFTT角点，使用KLT光流法进行特征点跟踪，同时进行IMU预积分处理。然后判断该系统是否已完成过初始化，若未完成初始化，则进入步骤S2.2；若已完成初始化，直接进入步骤S2.3；
S2.2、给定标定好的相机与IMU的内参和外参作为初始值，手动初始化视觉惯性里程计；首先利用纯视觉SFM估计相机尺度位姿；之后根据连续图像的相对旋转可计算出相机与IMU间的外参旋转；通过对齐IMU预积分与纯视觉 SFM结果，来粗略恢复尺度、重力、速度及IMU的bias；
S2.3、将视觉特征向量与IMU预积分后的测量值数据加入后端状态向量，优化视觉与IMU的整体误差，得到优化后的精确位姿信息，板载计算机将该位姿信息以10Hz的频率发送给飞行控制模块，飞行控制模块以此作为当前位姿信息完成高精度定位；
S3、构建四旋翼的三维导航地图；
S3.1、如图5所示，通过四旋翼的双目相机获取每一个时刻的当前帧点云数据；
S3.2、将每一个时刻的当前帧点云数据以及对应时刻的位置和姿态一起输入至TSDF，TSDF在获取到当前帧点云数据和姿态信息后，便将该帧点云数据与前一时刻的地图进行融合，生成新的三维地图；
S3.3、将三维地图转化成八叉树地图，从而得到四旋翼的三维导航地图；
S4、无障碍离散轨迹搜索；
首先将四旋翼所在位置处标记为起始点，然后给定一个待探索的目标位置，最后采用基于BIT*路径规划算法在三维导航地图中搜索出一条从起始点到目标位置的无障碍离散轨迹；
在本实施例中，我们利用遥控器将四旋翼切入GUIDED外部自主模式，为防止飞行事故的突发，保证遥控器与飞行控制模块始终处于连接状态，可随时切换飞行模式。在PC端上位机的人机交互界面点击起飞按钮指令，使四旋翼自主起飞一定高度h＝1.5m，高度可自行设定，四旋翼悬停5s且稳定无异常后，从起飞状态进入定点状态，等待导航飞行任务；如图6所示，PC端Rviz图形界面中通过3D NAV GOAL话题手动设定三维目标点下达导航飞行任务，然后根据里程计定位信息和已获取的周围环境信息(八叉树地图，未探测区域视为无障碍区域)，对规划系统初始化配置，在配置的搜索空间范围(0～50m,0～50m,0.3～3m) 中，前端采用基于BIT*路径规划算法生成无障碍离散轨迹；
S5、无障碍离散轨迹的优化；
无障碍离散轨迹是一组离散路径点序列，但是离散路径点没有考虑到无人机的运动学和动力学约束，不适用于无人机系统。因此需要结合无人机的特性对轨迹进行优化，设计一条安全、光滑、可行的路径，本方法将无人机飞行过程中的能量损耗考虑在内，设计出一条满足无人机能量损耗最小的路径。
S5.1、建立四旋翼的运动方程和动力学模型；
四旋翼的运动方程为：
Figure RE-GDA0002684613790000101
Figure RE-GDA0002684613790000102
其中，r表示四旋翼的位置向量，ω表示四旋翼的角速度向量，f、M分别表示四旋翼的净推力和力矩，J、m表示四旋翼的惯性张量和质量，zB表示推力方向的单位向量，zW表示重力方向的单位向量；
四旋翼的动力学模型为：
Figure RE-GDA0002684613790000103
Figure RE-GDA0002684613790000104
其中，er、ev、eR、eω分别表示位置、速度、方向和角速度的误差向量，kr、 kv、kR、kω分别表示位置、速度、姿态和角速度的控制量，R表示四旋翼的姿态，下标d表示期望值，
Figure RE-GDA0002684613790000105
表示角速度的估计值；
根据运动方程和动力学模型确定出四旋翼的全状态空间，记为位置 r＝[x,y,z]、速度
Figure RE-GDA0002684613790000106
姿态R→[φ,θ,ψ]、角速度ω；x,y,z表示位置r在坐标轴下的取值，φ,θ,ψ分别表示四旋翼的滚转角、俯仰角和偏航角；
S5.2、根据微分平坦算法将全状态空间简化为4维的平坦输出空间[x,y,z,ψ]；
S5.3、根据平坦输出空间[x,y,z,ψ]优化无障碍离散轨迹；
S5.3.1、在实际工程中，设定无人机机头方向始终朝向运行轨迹前行方向，ψ角的取值为运行轨迹的切向方向与x轴方向的夹角；
S5.3.2、在本实施例中，我们将无障碍离散轨迹设计为分段多项式的形式，只需要在X、Y、Z的每个维度上关于时间变量t进行参数化。
将无障碍离散轨迹按照离散点划分为λ段多项式，然后对每一段多项式分别在X、Y、Z轴上进行解耦，分别得到一组多项式轨迹序列，记为fx(t)、fy(t)和 fz(t)；
其中，fx(t)、fy(t)和fz(t)的形式相同，以X轴为例，多项式轨迹序列表示为：
Figure RE-GDA0002684613790000111
其中，i分段段数，i＝1,2,…,λ；j表示每一段多项式的阶数，j＝0,1,2,…,N， N为设定的多项式轨迹的总阶数，Ti为到达第i个离散点的时间，pi,j为第i段多项式轨迹的第j阶的系数；
在本实施例中，我们将每一段多项式轨迹表达式设置为7维，即第i段轨迹的表达形式为：
fi(t)＝pi,0+pi,1t+pi,2t2+pi,3t3+pi,4t4+pi,5t5+pi,6t6+pi,7t7,Ti-1≤t≤Ti
待优化轨迹在某一维上的表达式为：
Figure RE-GDA0002684613790000112
S5.3.3、获取每一段多项式轨迹的时间间隔；
根据工程经验，为了保证无人机运动的连续性，设fx(t)中每一段多项式轨迹间的运动都是经过匀加速、匀速、匀减速的过程；在任意一段多项式轨迹间，根据两个离散点i与i-1的欧式距离di,i-1，给定的最大加速度amax＝0.8m/s2及最大速度vmax＝0.5m/s，计算出两个离散点i与i-1的时间间隔ΔT，从而确保生成的轨迹更为平缓且符合无人机的运动规律；
Figure RE-GDA0002684613790000121
S5.3.5、为保证无人机的能量消耗最小，我们采用fx(t)每一段多项式轨迹的四阶导数的平方在对应时间段上的积分来构成代价函数，同时保证多项式轨迹有解，将代价函数用二次型的形式表示为：
Figure RE-GDA0002684613790000122
其中，pi对应fx(t)在第i段上的系数；Qi为第i段多项式轨迹的代价函数的二次型矩阵；
再将代价函数的二次型形式转换成矩阵形式，表示为：
Figure RE-GDA0002684613790000123
S5.3.6、设置每一段代价函数的连续性约束条件；
Figure RE-GDA0002684613790000124
其中，A为约束矩阵，D＝[d1,…,di,…,dλ]T，di＝[di,start,di,end]T，di,start为第i段多项式轨迹起始点位置及其各阶导数组成的向量，di,end为第i段多项式轨迹终点位置及其各阶导数组成的向量；
S5.3.7、先将X轴方向上的最优多项式轨迹的求取描述为一个二次规划QP 问题，再将连续性约束条件带入矩阵形式的代价函数Jtotal，得到：
Figure RE-GDA0002684613790000131
设置变换矩阵C，通过变换矩阵C将D中各变量重新排序，使D分解为固定变量DF和待优化变量Dp，其中，DF表示已知位置，Dp表示已知位置的各阶导数；
根据DF和Dp的维数，将二次规划方程的中间矩阵分为四个部分RFF、RFP、 RPF、RPP，从而将代价函数Jtotal改写为如下形式：
Figure RE-GDA0002684613790000132
其中，RFF为固定优化矩阵块、
Figure RE-GDA0002684613790000133
为混合优化矩阵块、RPP为待优化矩阵块；
进而展开得到：
Figure RE-GDA0002684613790000134
在X轴方向上求取最优多项式轨迹：将代价函数Jtotal对Dp求偏导，并令其为0，得到代价函数的最小值，进而求得待优化变量Dp的最优值
Figure RE-GDA0002684613790000137
Figure RE-GDA0002684613790000135
S5.3.8、通过
Figure RE-GDA0002684613790000136
逆向推导求解，得到最优多项式轨迹在X轴上的最优参数 [p1,p2,…,pλ]T；
S5.3.9、同理，分别求得在Y、Z轴方向上求取最优多项式轨迹，及对应的最优参数；
最后，根据X、Y、Z轴方向上求取最优多项式轨迹及对应的最优参数，得到三维空间中优化后的理想运行轨迹；
S5.4、判断理想运行轨迹中间是否存在障碍物，如图7所示，若有障碍物，则在理想运行轨迹的两个端点中间插入一个新离散点，再以这三个点作为轨迹优化的离散路径点，并返回步骤S5.3，否则，将理想运行轨迹作为四旋翼的运行轨迹，并进入步骤S6；
S6、四旋翼的运行轨迹跟随
S6.1、根据等间隔ΔT＝0.5s的原则在运行轨迹上设置轨迹跟随路径点集 {P1,P2,…,PL}；
S6.2、如图7所示，设四旋翼的当前位置为P0，在轨迹跟随路径点集中选出第一个轨迹跟随路径点作为当前规划路径点，记为Pcur，下一个规划路径点记为 Pnext，并以此类推；
S6.3、将轨迹跟随拆分成径向跟随与切向跟随，那么四旋翼在进行轨迹跟随时，其切向跟随速度方向为PcurPnext方向，大小设为固定值vt＝0.5m/s；径向跟随速度方向为PcurPnext的垂线方向，大小为vp，
Figure RE-GDA0002684613790000141
Figure RE-GDA0002684613790000142
为增益值，取值为1.5，
Figure RE-GDA0002684613790000143
为四旋翼在当前规划路径点与PcurPnext的垂直距离；
将切向跟随速度vt和径向跟随速度vp转化到世界坐标系的X轴和Y轴方向后，发送给飞行控制模块；
S6.4、飞行控制模块控制四旋翼跟随规划路径点飞行，在跟随过程中，飞行控制模块实时判断四旋翼机头方向与PcurPnext方向的角度大小，以及PcurPnext方向与 P0Pnext的方向的夹角大小或P0到Pnext的距离
Figure RE-GDA0002684613790000144
的大小，如果四旋翼机头方向与 PcurPnext方向的角度大于阈值γ＝10°时，则调整偏航角ψ，保证四旋翼前进方向始终处于相机视角范围以内；如果PcurPnext方向与P0Pnext方向的夹角大于90度，或者
Figure RE-GDA0002684613790000145
小于设定阈值d1＝20cm时，则进入步骤S6.5；
S6.5、更新三维导航地图，再通过分离轴定律对当前运行轨迹上的剩余路径点与更新后的三维导航地图进行碰撞检测，若发生碰撞，则返回步骤S4以当前位置为起点重新规划路径；若不发生碰撞，则更新下一规划路径点；
S6.6、判断四旋翼是否已经到达最后一个规划路径点PL，或者四旋翼当前位置P0与PL的距离小于阈值d2＝10cm，则导航任务完成，四旋翼进入定点状态；否则，返回步骤S6.5；
S7、导航结束
PC端上位机控制四旋翼自主降落，通过遥控器将四旋翼切到手动模式，上锁断电。
尽管上面对本发明说明性的具体实施方式进行了描述，以便于本技术领域的技术人员理解本发明，但应该清楚，本发明不限于具体实施方式的范围，对本技术领域的普通技术人员来讲，只要各种变化在所附的权利要求限定和确定的本发明的精神和范围内，这些变化是显而易见的，一切利用本发明构思的发明创造均在保护之列。

Claims (1)

1.一种基于视觉惯导融合的四旋翼自主导航方法，其特征在于，包括以下步骤：
(1)、四旋翼上电并初始化；
(2)、对四旋翼进行视觉惯导定位，从而获取四旋翼运行期间不同时刻的位置和姿态；
(3)、构建四旋翼的三维导航地图；
(3.1)、通过四旋翼的双目相机获取每一个时刻的当前帧点云数据；
(3.2)、将每一个时刻的当前帧点云数据以及对应时刻的位置和姿态一起输入至TSDF，TSDF通过积分的方式构建出三维地图；
(3.3)、将三维地图转化成八叉树地图图，从而得到四旋翼的三维导航地图；
(4)、无障碍离散轨迹搜索；
首先将四旋翼所在位置处标记为起始点，然后给定一个待探索的目标位置，最后采用基于BIT*路径规划算法在三维导航地图中搜索出一条从起始点到目标位置的无障碍离散轨迹；
(5)、无障碍离散轨迹的优化；
(5.1)、建立四旋翼的运动方程和动力学模型；
四旋翼的运动方程为：
Figure FDA0002612032000000011
Figure FDA0002612032000000012
其中，r表示四旋翼的位置向量，ω表示四旋翼的角速度向量，f、M分别表示四旋翼的净推力和力矩，J、m表示四旋翼的惯性张量和质量，zB表示推力方向的单位向量；
四旋翼的动力学模型为：
Figure FDA0002612032000000013
Figure FDA0002612032000000014
其中，er、ev、eR、eω分别表示位置、速度、方向和角速度的误差向量，kr、kv、kR、kω分别表示位置、速度、姿态和角速度的控制量，R表示四旋翼的姿态，下标d表示期望值，
Figure FDA0002612032000000015
表示角速度的估计值；
根据运动方程和动力学模型确定出四旋翼的全状态空间，记为位置r＝[x,y,z]、速度
Figure FDA0002612032000000021
姿态R→[φ,θ,ψ]、角速度ω；x,y,z表示位置r在坐标轴下的取值，φ,θ,ψ分别表示四旋翼的滚转角、俯仰角和偏航角；
(5.2)、根据微分平坦算法将全状态空间简化为4维的平坦输出空间[x,y,z,ψ]；
(5.3)、根据平坦输出空间[x,y,z,ψ]优化无障碍离散轨迹；
(5.3.1)、在实际工程中，设定无人机机头方向始终朝向运行轨迹前行方向，ψ角的取值为运行轨迹的切向方向与x轴方向的夹角；
(5.3.2)、将无障碍离散轨迹按照离散点划分为λ段多项式，然后对每一段多项式分别在X、Y、Z轴上进行解耦，分别得到一组多项式轨迹序列，记为fx(t)、fy(t)和fz(t)；
其中，fx(t)、fy(t)和fz(t)的形式相同，以X轴为例，多项式轨迹序列表示为：
Figure FDA0002612032000000022
其中，i分段段数，i＝1,2,…,λ；j表示每一段多项式的阶数，j＝0,1,2,…,N，N为设定的多项式轨迹的总阶数，Ti为到达第i个离散点的时间，pi,j为第i段多项式轨迹的第j阶的系数；
(5.3.3)、获取每一段多项式轨迹的时间间隔；
根据工程经验，设fx(t)中每一段多项式轨迹间的运动都是经过匀加速、匀速、匀减速的过程；在任意一段多项式轨迹间，根据两个离散点i与i-1的欧式距离di,i-1，给定的最大加速度amax及最大速度vmax，计算出两个离散点i与i-1的时间间隔ΔT；
Figure FDA0002612032000000031
(5.3.5)、采用fx(t)每一段多项式轨迹的四阶导数的平方在对应时间段上的积分来构成代价函数，同时保证多项式轨迹有解，将代价函数用二次型的形式表示为：
Figure FDA0002612032000000032
其中，pi对应fx(t)在第i段上的系数；Qi为第i段多项式轨迹的代价函数的二次型矩阵；
再将代价函数的二次型形式转换成矩阵形式，表示为：
Figure FDA0002612032000000033
(5.3.6)、设置每一段代价函数的连续性约束条件；
Figure FDA0002612032000000034
其中，A为约束矩阵，D＝[d1,…,di,…,dλ]T，di＝[di,start,di,end]T，di,start为第i段多项式轨迹起始点位置及其各阶导数组成的向量，di,end为第i段多项式轨迹终点位置及其各阶导数组成的向量；
(5.3.7)、先将X轴方向上的最优多项式轨迹的求取描述为一个二次规划QP问题，再将连续性约束条件带入矩阵形式的代价函数Jtotal，得到：
Figure FDA0002612032000000041
设置变换矩阵C，通过变换矩阵C将D中各变量重新排序，使D分解为固定变量DF和待优化变量Dp，其中，DF表示已知位置，Dp表示已知位置的各阶导数；
根据DF和Dp的维数，将二次规划方程的中间矩阵分为四个部分RFF、RFP、RPF、RPP，从而将代价函数Jtotal改写为如下形式：
Figure FDA0002612032000000042
其中，RFF为固定优化矩阵块、
Figure FDA0002612032000000043
为混合优化矩阵块、RPP为待优化矩阵块；
进而展开得到：
Figure FDA0002612032000000044
在X轴方向上求取最优多项式轨迹：将代价函数Jtotal对Dp求偏导，并令其为0，得到代价函数的最小值，进而求得待优化变量Dp的最优值
Figure FDA0002612032000000045
Figure FDA0002612032000000046
(5.3.8)、通过
Figure FDA0002612032000000047
逆向推导求解，得到最优多项式轨迹在X轴上的最优参数[p1,p2,…,pλ]T；
(5.3.9)、同理，分别求得在Y、Z轴方向上求取最优多项式轨迹，及对应的最优参数；
最后，根据X、Y、Z轴方向上求取最优多项式轨迹及对应的最优参数，得到三维空间中优化后的理想运行轨迹；
(5.4)、判断理想运行轨迹中间是否存在障碍物，若有障碍物，则在理想运行轨迹的两个端点中间插入一个新离散点，再以这三个点作为轨迹优化的离散路径点，并返回步骤(5.3)，否则，将理想运行轨迹作为四旋翼的运行轨迹，并进入步骤(6)；
(6)、四旋翼的运行轨迹跟随
(6.1)、根据等间隔的原则在运行轨迹上设置轨迹跟随路径点集{P1,P2,…,PL}；
(6.2)、设四旋翼的当前位置为P0，在轨迹跟随路径点集中选出第一个轨迹跟随路径点作为当前规划路径点，记为Pcur，下一个规划路径点记为Pnext，并以此类推；
(6.3)、将轨迹跟随拆分成径向跟随与切向跟随，那么四旋翼在进行轨迹跟随时，其切向跟随速度方向为Pcur Pnext方向，大小设为固定值vt；径向跟随速度方向为Pcur Pnext的垂线方向，大小为vp，
Figure FDA0002612032000000051
Figure FDA0002612032000000052
为增益值，
Figure FDA0002612032000000053
为四旋翼在当前规划路径点与Pcur Pnext的垂直距离；
将切向跟随速度vt和径向跟随速度vp转化到世界坐标系的X轴和Y轴方向后，发送给飞行控制模块；
(6.4)、飞行控制模块控制四旋翼跟随规划路径点飞行，在跟随过程中，飞行控制模块实时判断四旋翼机头方向与Pcur Pnext方向的角度大小，以及Pcur Pnext方向与P0 Pnext的方向的夹角大小或P0到Pnext的距离
Figure FDA0002612032000000054
的大小，如果四旋翼机头方向与Pcur Pnext方向的角度大于阈值γ时，则调整偏航角ψ，保证四旋翼前进方向始终处于相机视角范围以内；如果PcurPnext方向与P0 Pnext方向的夹角大于90度，或者
Figure FDA0002612032000000055
小于设定阈值d1时，则进入步骤(6.5)；
(6.5)、更新三维导航地图，再通过分离轴定律对当前运行轨迹上的剩余路径点与更新后的三维导航地图进行碰撞检测，若发生碰撞，则返回步骤(4)以当前位置为起点重新规划路径；若不发生碰撞，则更新下一规划路径点；
(6.6)、判断四旋翼是否已经到达最后一个规划路径点PL，或者四旋翼当前位置P0与PL的距离小于阈值d2，d2＜d1，则导航任务完成，四旋翼进入定点状态；否则，返回步骤(6.5)；
(7)、导航结束
PC端上位机控制四旋翼自主降落，通过遥控器将四旋翼切到手动模式，上锁断电。
CN202010757433.7A 2020-07-31 2020-07-31 一种基于视觉惯导融合的四旋翼自主导航方法 Active CN111880573B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010757433.7A CN111880573B (zh) 2020-07-31 2020-07-31 一种基于视觉惯导融合的四旋翼自主导航方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010757433.7A CN111880573B (zh) 2020-07-31 2020-07-31 一种基于视觉惯导融合的四旋翼自主导航方法

Publications (2)

Publication Number Publication Date
CN111880573A true CN111880573A (zh) 2020-11-03
CN111880573B CN111880573B (zh) 2022-09-06

Family

ID=73205782

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010757433.7A Active CN111880573B (zh) 2020-07-31 2020-07-31 一种基于视觉惯导融合的四旋翼自主导航方法

Country Status (1)

Country Link
CN (1) CN111880573B (zh)

Cited By (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112419417A (zh) * 2021-01-25 2021-02-26 成都翼比特自动化设备有限公司 一种基于无人机的拍照点定位方法及相关装置
CN112747750A (zh) * 2020-12-30 2021-05-04 电子科技大学 一种基于单目视觉里程计和imu融合的定位方法
CN113465596A (zh) * 2021-06-25 2021-10-01 电子科技大学 一种基于多传感器融合的四旋翼无人机定位方法
CN113885566A (zh) * 2021-10-21 2022-01-04 重庆邮电大学 面向多无人机数据采集时间最小化的v型轨迹规划方法
CN113954081A (zh) * 2021-12-07 2022-01-21 中国煤炭科工集团太原研究院有限公司 一种锚杆支护机器人的工作臂轨迹规划方法及系统
CN114355981A (zh) * 2022-01-06 2022-04-15 中山大学 一种四旋翼无人机自主探索建图的方法和系统
CN116009421A (zh) * 2022-12-29 2023-04-25 中电普信(北京)科技发展有限公司 全自由度固定翼飞机通用仿真方法
CN116352725A (zh) * 2023-05-23 2023-06-30 极限人工智能(北京)有限公司 三次三段式的机械臂轨迹规划方法、系统、设备及介质

Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104062977A (zh) * 2014-06-17 2014-09-24 天津大学 基于视觉slam的四旋翼无人机全自主飞行控制方法
WO2016187757A1 (en) * 2015-05-23 2016-12-01 SZ DJI Technology Co., Ltd. Sensor fusion using inertial and image sensors
CN106708066A (zh) * 2015-12-20 2017-05-24 中国电子科技集团公司第二十研究所 基于视觉/惯导的无人机自主着陆方法
US20180164124A1 (en) * 2016-09-15 2018-06-14 Syracuse University Robust and stable autonomous vision-inertial navigation system for unmanned vehicles
CN108776492A (zh) * 2018-06-27 2018-11-09 电子科技大学 一种基于双目相机的四轴飞行器自主避障与导航方法
CN109540126A (zh) * 2018-12-03 2019-03-29 哈尔滨工业大学 一种基于光流法的惯性视觉组合导航方法
CN111367318A (zh) * 2020-03-31 2020-07-03 华东理工大学 一种基于视觉语义信息的动态障碍环境导航方法和装置

Patent Citations (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104062977A (zh) * 2014-06-17 2014-09-24 天津大学 基于视觉slam的四旋翼无人机全自主飞行控制方法
WO2016187757A1 (en) * 2015-05-23 2016-12-01 SZ DJI Technology Co., Ltd. Sensor fusion using inertial and image sensors
CN106708066A (zh) * 2015-12-20 2017-05-24 中国电子科技集团公司第二十研究所 基于视觉/惯导的无人机自主着陆方法
US20180164124A1 (en) * 2016-09-15 2018-06-14 Syracuse University Robust and stable autonomous vision-inertial navigation system for unmanned vehicles
CN108776492A (zh) * 2018-06-27 2018-11-09 电子科技大学 一种基于双目相机的四轴飞行器自主避障与导航方法
CN109540126A (zh) * 2018-12-03 2019-03-29 哈尔滨工业大学 一种基于光流法的惯性视觉组合导航方法
CN111367318A (zh) * 2020-03-31 2020-07-03 华东理工大学 一种基于视觉语义信息的动态障碍环境导航方法和装置

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
史莹晶: "基于四轴运动控制教学平台的多元实践体系改革", 《实验室研究与探索》 *
陶阳: "双目与IMU融合的无人机定位技术研究及系统设计", 《中国优秀硕士学位论文全文数据库 工程科技II辑》 *
黄敏: "基于微分平坦理论的四旋翼无人机轨迹规划的研究", 《中国优秀硕士学位论文全文数据库 工程科技II辑》 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112747750A (zh) * 2020-12-30 2021-05-04 电子科技大学 一种基于单目视觉里程计和imu融合的定位方法
CN112419417A (zh) * 2021-01-25 2021-02-26 成都翼比特自动化设备有限公司 一种基于无人机的拍照点定位方法及相关装置
CN113465596A (zh) * 2021-06-25 2021-10-01 电子科技大学 一种基于多传感器融合的四旋翼无人机定位方法
CN113885566A (zh) * 2021-10-21 2022-01-04 重庆邮电大学 面向多无人机数据采集时间最小化的v型轨迹规划方法
CN113885566B (zh) * 2021-10-21 2024-01-23 重庆邮电大学 面向多无人机数据采集时间最小化的v型轨迹规划方法
CN113954081A (zh) * 2021-12-07 2022-01-21 中国煤炭科工集团太原研究院有限公司 一种锚杆支护机器人的工作臂轨迹规划方法及系统
CN114355981A (zh) * 2022-01-06 2022-04-15 中山大学 一种四旋翼无人机自主探索建图的方法和系统
CN114355981B (zh) * 2022-01-06 2024-01-12 中山大学 一种四旋翼无人机自主探索建图的方法和系统
CN116009421A (zh) * 2022-12-29 2023-04-25 中电普信(北京)科技发展有限公司 全自由度固定翼飞机通用仿真方法
CN116352725A (zh) * 2023-05-23 2023-06-30 极限人工智能(北京)有限公司 三次三段式的机械臂轨迹规划方法、系统、设备及介质
CN116352725B (zh) * 2023-05-23 2023-10-13 极限人工智能(北京)有限公司 三次三段式的机械臂轨迹规划方法、系统、设备及介质

Also Published As

Publication number Publication date
CN111880573B (zh) 2022-09-06

Similar Documents

Publication Publication Date Title
CN111880573B (zh) 一种基于视觉惯导融合的四旋翼自主导航方法
Zhang et al. Robust nonlinear model predictive control based visual servoing of quadrotor UAVs
US10732647B2 (en) Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (MAV)
Scaramuzza et al. Vision-controlled micro flying robots: from system design to autonomous navigation and mapping in GPS-denied environments
Alexis et al. Robust model predictive flight control of unmanned rotorcrafts
Cichella et al. Geometric 3D path-following control for a fixed-wing UAV on SO (3)
Hérissé et al. A terrain-following control approach for a vtol unmanned aerial vehicle using average optical flow
Carrillo et al. Three-dimensional position and velocity regulation of a quad-rotorcraft using optical flow
Kim et al. Flying robots: modeling, control and decision making
Paneque et al. Perception-aware perching on powerlines with multirotors
Becker et al. In flight collision avoidance for a Mini-UAV robot based on onboard sensors
Vílez et al. Trajectory generation and tracking using the AR. Drone 2.0 quadcopter UAV
Roque et al. Fast model predictive image-based visual servoing for quadrotors
Karras et al. Target tracking with multi-rotor aerial vehicles based on a robust visual servo controller with prescribed performance
Wang et al. Image-based visual servoing of quadrotors to arbitrary flight targets
Joshi et al. Implementation of distributed consensus on an outdoor testbed
Adolf et al. An unmanned helicopter for autonomous flights in urban terrain
CN112161626B (zh) 一种基于航路跟踪映射网络的高可飞性航路规划方法
DeBock Guidance, Navigation, And Control of a Quadrotor Drone with PID Controls
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control
Aguiar et al. Coordinated path following of multiple UAVs for time-critical missions in the presence of time-varying communication topologies
Yuan et al. Distributed model predictive formation control for a group of uavs with newton-euler dynamics
Aspragkathos et al. An event-triggered visual servoing predictive control strategy for the surveillance of contour-based areas using multirotor aerial vehicles
Zou et al. Visual track system applied in quadrotor aerial robot
Cella et al. Fueling Glocal: Optimization-Based Path Planning for Indoor Uavs in AN Autonomous Exploration Framework

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