CN111880573B - 一种基于视觉惯导融合的四旋翼自主导航方法 - Google Patents
一种基于视觉惯导融合的四旋翼自主导航方法 Download PDFInfo
- Publication number
- CN111880573B CN111880573B CN202010757433.7A CN202010757433A CN111880573B CN 111880573 B CN111880573 B CN 111880573B CN 202010757433 A CN202010757433 A CN 202010757433A CN 111880573 B CN111880573 B CN 111880573B
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 29
- 230000000007 visual effect Effects 0.000 title claims abstract description 22
- 230000004927 fusion Effects 0.000 title claims abstract description 15
- 230000008569 process Effects 0.000 claims abstract description 9
- 238000001514 detection method Methods 0.000 claims abstract description 5
- 239000011159 matrix material Substances 0.000 claims description 31
- 239000013598 vector Substances 0.000 claims description 24
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 claims description 14
- 238000004422 calculation algorithm Methods 0.000 claims description 11
- 230000036544 posture Effects 0.000 claims description 7
- 230000001133 acceleration Effects 0.000 claims description 6
- 230000009466 transformation Effects 0.000 claims description 6
- 230000005484 gravity Effects 0.000 claims description 4
- 238000005457 optimization Methods 0.000 claims description 4
- 238000009795 derivation Methods 0.000 claims description 3
- 238000005096 rolling process Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 23
- 238000005516 engineering process Methods 0.000 description 7
- 238000013507 mapping Methods 0.000 description 5
- 238000013473 artificial intelligence Methods 0.000 description 3
- 238000013461 design Methods 0.000 description 3
- 238000011161 development Methods 0.000 description 3
- 238000005265 energy consumption Methods 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 238000004891 communication Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 238000005259 measurement Methods 0.000 description 2
- 230000003287 optical effect Effects 0.000 description 2
- 238000012800 visualization Methods 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 230000002159 abnormal effect Effects 0.000 description 1
- 230000003044 adaptive effect Effects 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000004364 calculation method Methods 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005315 distribution function Methods 0.000 description 1
- 239000000284 extract Substances 0.000 description 1
- 238000000605 extraction Methods 0.000 description 1
- 230000024703 flight behavior Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000007781 pre-processing Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 238000011084 recovery Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Navigation (AREA)
Abstract
本发明公开了一种基于视觉惯导融合的四旋翼自主导航方法,先利用视觉惯导融合的定位方式,结合相机图像信息和四旋翼位姿信息对四旋翼实现高精度定位功能,再通过地面站***的上位机发布导航任务指令,完成环境三维重建和自主导航探测搜索任务;在整个导航过程无需外部设备和人为控制,***具有良好的自主性,此外平台还具有低成本、高精度和强扩展性等特点。
Description
技术领域
本发明属于四旋翼技术领域,更为具体地讲,涉及一种基于视觉惯导融合的四旋翼自主导航方法。
背景技术
随着人工智能技术的发展,人工智能面临发展的重大历史机遇,建立新一代人工智能关键共性技术体系。在自主无人***的智能技术中重点突破自主无人***计算架构、复杂动态场景感知与理解、实时精准定位、面向复杂环境的适应性智能导航等共性技术,无人机自主控制以及汽车、船舶和轨道交通自动驾驶等智能技术,服务机器人、特种机器人等核心技术,支撑无人***应用和产业发展。而未知环境下的自主导航作为其实现智能化的关键技术在各领域都发挥着至关重要的作用,如地震等自然灾害发生后进入受损建筑物寻找幸存者,在军事战争中执行先前的侦察任务等。
随着无人机应用场景的多样化,普通机器人的路径规划方案已无法满足无人机在三维未知环境中快速导航需求,针对集成定位、感知和规划的小型无人机平台,运动规划需考虑以下三个关键问题:第一,飞行***复杂的运动、动力学,规划面临着快速飞行和动力学约束限制等条件;第二,在平台有限的计算能力下,所需具备的在线实时规划能力;第三,规划结果的安全可行性和完整最优性。
发明内容
本发明的目的在于克服现有技术的不足,提供一种基于视觉惯导融合的四旋翼自主导航方法,通过视觉惯导融合的方式完成复杂场景下的四旋翼自主导航定位。
为实现上述发明目的,本发明一种基于视觉惯导融合的四旋翼自主导航方法,其特征在于,包括以下步骤:
(1)、四旋翼上电并初始化;
(2)、对四旋翼进行视觉惯导定位,从而获取四旋翼运行期间不同时刻的位置和姿态;
(3)、构建四旋翼的三维导航地图;
(3.1)、通过四旋翼的双目相机获取每一个时刻的当前帧点云数据;
(3.2)、将每一个时刻的当前帧点云数据以及对应时刻的位置和姿态一起输入至TSDF,TSDF通过积分的方式构建出三维地图;
(3.3)、将三维地图转化成八叉树地图,从而得到四旋翼的三维导航地图;
(4)、无障碍离散轨迹搜索;
首先将四旋翼所在位置处标记为起始点,然后给定一个待探索的目标位置,最后采用基于BIT*路径规划算法在三维导航地图中搜索出一条从起始点到目标位置的无障碍离散轨迹;
(5)、无障碍离散轨迹的优化;
(5.1)、建立四旋翼的运动方程和动力学模型;
四旋翼的运动方程为:
其中,r表示四旋翼的位置向量,ω表示四旋翼的角速度向量,f、M分别表示四旋翼的净推力和力矩,J、m表示四旋翼的惯性张量和质量,zB表示推力方向的单位向量,zW表示重力方向的单位向量;
四旋翼的动力学模型为:
根据运动方程和动力学模型确定出四旋翼的全状态空间,记为位置 r=[x,y,z]、速度姿态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轴为例,多项式轨迹序列表示为:
其中,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;
(5.3.5)、采用fx(t)每一段多项式轨迹的四阶导数的平方在对应时间段上的积分来构成代价函数,同时保证多项式轨迹有解,将代价函数用二次型的形式表示为:
其中,pi对应fx(t)在第i段上的系数;Qi为第i段多项式轨迹的代价函数的二次型矩阵;
再将代价函数的二次型形式转换成矩阵形式,表示为:
(5.3.6)、设置每一段代价函数的连续性约束条件;
其中,A为约束矩阵,D=[d1,…,di,…,dλ]T,di=[di,start,di,end]T,di,start为第i段多项式轨迹起始点位置及其各阶导数组成的向量,di,end为第i段多项式轨迹终点位置及其各阶导数组成的向量;
(5.3.7)、先将X轴方向上的最优多项式轨迹的求取描述为一个二次规划 QP问题,再将连续性约束条件带入矩阵形式的代价函数Jtotal,得到:
设置变换矩阵C,通过变换矩阵C将D中各变量重新排序,使D分解为固定变量DF和待优化变量Dp,其中,DF表示已知位置,Dp表示已知位置的各阶导数;
根据DF和Dp的维数,将二次规划方程的中间矩阵分为四个部分RFF、RFP、 RPF、RPP,从而将代价函数Jtotal改写为如下形式:
进而展开得到:
(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, 为增益值,为四旋翼在当前规划路径点与PcurPnext的垂直距离;
将切向跟随速度vt和径向跟随速度vp转化到世界坐标系的X轴和Y轴方向后,发送给飞行控制模块;
(6.4)、飞行控制模块控制四旋翼跟随规划路径点飞行,在跟随过程中,飞行控制模块实时判断四旋翼机头方向与PcurPnext方向的角度大小,以及PcurPnext方向与P0Pnext的方向的夹角大小或P0到Pnext的距离dP0Pnext的大小,如果四旋翼机头方向与PcurPnext方向的角度大于阈值γ时,则调整偏航角ψ,保证四旋翼前进方向始终处于相机视角范围以内;如果PcurPnext方向与P0Pnext方向的夹角大于90度,或者小于设定阈值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、建立四旋翼的运动方程和动力学模型;
四旋翼的运动方程为:
其中,r表示四旋翼的位置向量,ω表示四旋翼的角速度向量,f、M分别表示四旋翼的净推力和力矩,J、m表示四旋翼的惯性张量和质量,zB表示推力方向的单位向量,zW表示重力方向的单位向量;
四旋翼的动力学模型为:
根据运动方程和动力学模型确定出四旋翼的全状态空间,记为位置 r=[x,y,z]、速度姿态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轴为例,多项式轨迹序列表示为:
其中,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
待优化轨迹在某一维上的表达式为:
S5.3.3、获取每一段多项式轨迹的时间间隔;
根据工程经验,为了保证无人机运动的连续性,设fx(t)中每一段多项式轨迹间的运动都是经过匀加速、匀速、匀减速的过程;在任意一段多项式轨迹间,根据两个离散点i与i-1的欧式距离di,i-1,给定的最大加速度amax=0.8m/s2及最大速度vmax=0.5m/s,计算出两个离散点i与i-1的时间间隔ΔT,从而确保生成的轨迹更为平缓且符合无人机的运动规律;
S5.3.5、为保证无人机的能量消耗最小,我们采用fx(t)每一段多项式轨迹的四阶导数的平方在对应时间段上的积分来构成代价函数,同时保证多项式轨迹有解,将代价函数用二次型的形式表示为:
其中,pi对应fx(t)在第i段上的系数;Qi为第i段多项式轨迹的代价函数的二次型矩阵;
再将代价函数的二次型形式转换成矩阵形式,表示为:
S5.3.6、设置每一段代价函数的连续性约束条件;
其中,A为约束矩阵,D=[d1,…,di,…,dλ]T,di=[di,start,di,end]T,di,start为第i段多项式轨迹起始点位置及其各阶导数组成的向量,di,end为第i段多项式轨迹终点位置及其各阶导数组成的向量;
S5.3.7、先将X轴方向上的最优多项式轨迹的求取描述为一个二次规划QP 问题,再将连续性约束条件带入矩阵形式的代价函数Jtotal,得到:
设置变换矩阵C,通过变换矩阵C将D中各变量重新排序,使D分解为固定变量DF和待优化变量Dp,其中,DF表示已知位置,Dp表示已知位置的各阶导数;
根据DF和Dp的维数,将二次规划方程的中间矩阵分为四个部分RFF、RFP、 RPF、RPP,从而将代价函数Jtotal改写为如下形式:
进而展开得到:
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, 为增益值,取值为1.5,为四旋翼在当前规划路径点与PcurPnext的垂直距离;
将切向跟随速度vt和径向跟随速度vp转化到世界坐标系的X轴和Y轴方向后,发送给飞行控制模块;
S6.4、飞行控制模块控制四旋翼跟随规划路径点飞行,在跟随过程中,飞行控制模块实时判断四旋翼机头方向与PcurPnext方向的角度大小,以及PcurPnext方向与 P0Pnext的方向的夹角大小或P0到Pnext的距离的大小,如果四旋翼机头方向与 PcurPnext方向的角度大于阈值γ=10°时,则调整偏航角ψ,保证四旋翼前进方向始终处于相机视角范围以内;如果PcurPnext方向与P0Pnext方向的夹角大于90度,或者小于设定阈值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)、建立四旋翼的运动方程和动力学模型;
四旋翼的运动方程为:
其中,r表示四旋翼的位置向量,ω表示四旋翼的角速度向量,f、M分别表示四旋翼的净推力和力矩,J、m表示四旋翼的惯性张量和质量,zB表示推力方向的单位向量,zW表示重力方向的单位向量;
四旋翼的动力学模型为:
根据运动方程和动力学模型确定出四旋翼的全状态空间,记为位置r=[x,y,z]、速度姿态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轴为例,多项式轨迹序列表示为:
其中,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;
(5.3.5)、采用fx(t)每一段多项式轨迹的四阶导数的平方在对应时间段上的积分来构成代价函数,同时保证多项式轨迹有解,将代价函数用二次型的形式表示为:
其中,pi对应fx(t)在第i段上的系数;Qi为第i段多项式轨迹的代价函数的二次型矩阵;
再将代价函数的二次型形式转换成矩阵形式,表示为:
(5.3.6)、设置每一段代价函数的连续性约束条件;
其中,A为约束矩阵,D=[d1,…,di,…,dλ]T,di=[di,start,di,end]T,di,start为第i段多项式轨迹起始点位置及其各阶导数组成的向量,di,end为第i段多项式轨迹终点位置及其各阶导数组成的向量;
(5.3.7)、先将X轴方向上的最优多项式轨迹的求取描述为一个二次规划QP问题,再将连续性约束条件带入矩阵形式的代价函数Jtotal,得到:
设置变换矩阵C,通过变换矩阵C将D中各变量重新排序,使D分解为固定变量DF和待优化变量Dp,其中,DF表示已知位置,Dp表示已知位置的各阶导数;
根据DF和Dp的维数,将二次规划方程的中间矩阵分为四个部分RFF、RFP、RPF、RPP,从而将代价函数Jtotal改写为如下形式:
进而展开得到:
(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, 为增益值,为四旋翼在当前规划路径点与Pcur Pnext的垂直距离;
将切向跟随速度vt和径向跟随速度vp转化到世界坐标系的X轴和Y轴方向后,发送给飞行控制模块;
(6.4)、飞行控制模块控制四旋翼跟随规划路径点飞行,在跟随过程中,飞行控制模块实时判断四旋翼机头方向与Pcur Pnext方向的角度大小,以及Pcur Pnext方向与P0 Pnext的方向的夹角大小或P0到Pnext的距离的大小,如果四旋翼机头方向与Pcur Pnext方向的角度大于阈值γ时,则调整偏航角ψ,保证四旋翼前进方向始终处于相机视角范围以内;如果PcurPnext方向与P0 Pnext方向的夹角大于90度,或者小于设定阈值d1时,则进入步骤(6.5);
(6.5)、更新三维导航地图,再通过分离轴定律对当前运行轨迹上的剩余路径点与更新后的三维导航地图进行碰撞检测,若发生碰撞,则返回步骤(4)以当前位置为起点重新规划路径;若不发生碰撞,则更新下一规划路径点;
(6.6)、判断四旋翼是否已经到达最后一个规划路径点PL,或者四旋翼当前位置P0与PL的距离小于阈值d2,d2<d1,则导航任务完成,四旋翼进入定点状态;否则,返回步骤(6.5);
(7)、导航结束
PC端上位机控制四旋翼自主降落,通过遥控器将四旋翼切到手动模式,上锁断电。
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 CN111880573A (zh) | 2020-11-03 |
CN111880573B true 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) |
Families Citing this family (9)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN114518767A (zh) * | 2020-11-19 | 2022-05-20 | 复旦大学 | 一种基于倾斜摄影模型的无人机三维路径规划方法 |
CN112747750B (zh) * | 2020-12-30 | 2022-10-14 | 电子科技大学 | 一种基于单目视觉里程计和imu融合的定位方法 |
CN112419417B (zh) * | 2021-01-25 | 2021-05-18 | 成都翼比特自动化设备有限公司 | 一种基于无人机的拍照点定位方法及相关装置 |
CN113465596B (zh) * | 2021-06-25 | 2023-05-30 | 电子科技大学 | 一种基于多传感器融合的四旋翼无人机定位方法 |
CN113885566B (zh) * | 2021-10-21 | 2024-01-23 | 重庆邮电大学 | 面向多无人机数据采集时间最小化的v型轨迹规划方法 |
CN113954081A (zh) * | 2021-12-07 | 2022-01-21 | 中国煤炭科工集团太原研究院有限公司 | 一种锚杆支护机器人的工作臂轨迹规划方法及*** |
CN114355981B (zh) * | 2022-01-06 | 2024-01-12 | 中山大学 | 一种四旋翼无人机自主探索建图的方法和*** |
CN116009421A (zh) * | 2022-12-29 | 2023-04-25 | 中电普信(北京)科技发展有限公司 | 全自由度固定翼飞机通用仿真方法 |
CN116352725B (zh) * | 2023-05-23 | 2023-10-13 | 极限人工智能(北京)有限公司 | 三次三段式的机械臂轨迹规划方法、***、设备及介质 |
Citations (6)
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 | 中国电子科技集团公司第二十研究所 | 基于视觉/惯导的无人机自主着陆方法 |
CN108776492A (zh) * | 2018-06-27 | 2018-11-09 | 电子科技大学 | 一种基于双目相机的四轴飞行器自主避障与导航方法 |
CN109540126A (zh) * | 2018-12-03 | 2019-03-29 | 哈尔滨工业大学 | 一种基于光流法的惯性视觉组合导航方法 |
CN111367318A (zh) * | 2020-03-31 | 2020-07-03 | 华东理工大学 | 一种基于视觉语义信息的动态障碍环境导航方法和装置 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10578457B2 (en) * | 2016-09-15 | 2020-03-03 | Syracuse University | Robust and stable autonomous vision-inertial navigation system for unmanned vehicles |
-
2020
- 2020-07-31 CN CN202010757433.7A patent/CN111880573B/zh active Active
Patent Citations (6)
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 | 中国电子科技集团公司第二十研究所 | 基于视觉/惯导的无人机自主着陆方法 |
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)
Title |
---|
双目与IMU融合的无人机定位技术研究及***设计;陶阳;《中国优秀硕士学位论文全文数据库 工程科技II辑》;20190515(第5期);C031-142 * |
基于四轴运动控制教学平台的多元实践体系改革;史莹晶;《实验室研究与探索》;20200325;第39卷(第3期);146-150 * |
基于微分平坦理论的四旋翼无人机轨迹规划的研究;黄敏;《中国优秀硕士学位论文全文数据库 工程科技II辑》;20160315(第3期);C031-438 * |
Also Published As
Publication number | Publication date |
---|---|
CN111880573A (zh) | 2020-11-03 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111880573B (zh) | 一种基于视觉惯导融合的四旋翼自主导航方法 | |
Kamel et al. | Robust collision avoidance for multiple micro aerial vehicles using nonlinear model predictive control | |
Scaramuzza et al. | Vision-controlled micro flying robots: from system design to autonomous navigation and mapping in GPS-denied environments | |
CN109885080B (zh) | 自主控制***及自主控制方法 | |
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 | |
Koo et al. | Hierarchical hybrid system design on Berkeley UAV | |
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 | |
Andert et al. | Online world modeling and path planning for an unmanned helicopter | |
CN114397909B (zh) | 一种针对大型飞机的小型无人机自动巡检方法 | |
Joshi et al. | Implementation of distributed consensus on an outdoor testbed | |
Adolf et al. | An unmanned helicopter for autonomous flights in urban terrain | |
Kanjanawanishkul | Coordinated path following for mobile robots using a virtual structure strategy with model predictive control | |
CN112161626B (zh) | 一种基于航路跟踪映射网络的高可飞性航路规划方法 | |
Aguiar et al. | Coordinated path following of multiple UAVs for time-critical missions in the presence of time-varying communication topologies | |
Aspragkathos et al. | An event-triggered visual servoing predictive control strategy for the surveillance of contour-based areas using multirotor aerial vehicles | |
Yuan et al. | Distributed model predictive formation control for a group of uavs with newton-euler dynamics | |
Cella et al. | Fueling Glocal: Optimization-Based Path Planning for Indoor Uavs in AN Autonomous Exploration Framework | |
Causey | A lateral vision-based control autopilot for micro air vehicles using a horizon detection approach | |
Issame et al. | Real Time Object Detection With Drone Using Deep Learning Algorithm |
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 |