CN108827306A - 一种基于多传感器融合的无人机slam导航方法及系统 - Google Patents

Abstract

本发明公开了一种基于多传感器融合的无人机SLAM导航方法，包括实时采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息；实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息；根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径，根据所述飞行路径控制无人机的自主避障飞行。该方法及系统可以实现复杂环境中无人机的实时定位与地图构建。相对于传统的无人机导航技术实现实时定位和地图构建与自主导航，提高无人机的智能化程度和导航精度。

Description

一种基于多传感器融合的无人机SLAM导航方法及系统

技术领域

本发明涉及导航定位与控制技术领域，特别是指一种基于多传感器融合的无人机SLAM导航方法及系统。

背景技术

无人机(Unmanned Aerial Vehicle，UAV)是一种利用无线电遥控设备和自身的程序控制的无人飞行器。近年来，无人机在诸多领域上的广泛应用引起了人们的关注，比如在军事上能够进行侦查、打击；在民用领域可用于航拍、测绘、农林植保和信息监测，四旋翼无人机作为无人机的一种，具有机动性好、结构简单、控制稳定等特点，适合应用于复杂环境下的飞行作业。对于无人机导航来说，最关键的环节是如何获取无人机的位姿信息。

传统的无人机导航技术，例如GPS、惯性传感器、超声波传感器等技术，基于GPS定位技术，在室内环境或者林冠下环境中，因为存在遮挡物，影响 GPS信号的接收；基于惯性传感器的定位技术，由于采用的电子陀螺仪容易受到噪声的干扰，导致积分误差不断累积；基于超声波传感器的定位技术，其测距精度低且不能和障碍物靠的太近。SLAM(simultaneous localization and mapping，实时定位与地图构建)技术是解决复杂环境下导航的关键技术， SLAM技术已经广泛应用于地面机器人、室内机器人。基于SLAM技术的传感器有激光雷达，单目视觉，双目视觉，深度相机等。SLAM技术使得无人机能够拥有更加精确的定位能力，并且能对周围的障碍物构建深度地图。但是，现有SLAM都存在一定的局限性，现有的单目视觉SLAM，例如LSD-SLAM、 ORB-SLAM，在单一背景环境下，存在特征点匹配易丢失，匹配速度慢等缺点；双目视觉SLAM对算法和机载硬件的要求比较高，并且对小型障碍物的识别能力较低；现有的深度相机只能应用于室内，在室外受到自然光的影响比较大；激光SLAM，例如hectorSLAM、gmapping，存在计算量大、噪声大、难以进行回环检测、匹配易丢失等问题。因此，如何将SLAM技术应用到无人机导航上，并且实现无人机在复杂环境下的自主导航飞行，成为本领域技术人员亟待解决的问题。

发明内容

有鉴于此，本发明的目的在于提出一种基于多传感器融合的无人机SLAM 导航方法及系统，实现无人机在复杂环境下的自主导航飞行。

基于上述目的本发明提供的一种基于多传感器融合的无人机SLAM导航方法，包括：

实时采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息；

实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息；

根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径，根据所述飞行路径控制无人机的自主避障飞行。

进一步，所述采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息，包括：

根据当前帧图像信息以及前一帧图像信息获取无人机的相对位姿；

根据关键帧图像信息以及当前帧图像信息对无人机的所述相对位姿进行优化，获取优化后的特征点位置信息；

根据优化后的所述特征点位置信息优化世界位姿，获取无人机在世界坐标系下的所述位姿信息。

进一步，所述根据当前帧图像信息以及前一帧图像信息获取无人机的相对位姿，包括：

将前一帧图像信息I_k-1的特征点投影到当前帧图像信息I_k上，根据两帧的特征点的灰度差构建无人机的相对位姿的优化函数；

其中，T_k,k-1为无人机的所述相对位姿，δI(T,u)＝I_k(h(T·h^-1(u,du)))-I_k-1(u)，u为 (u,v)^T，du为深度，h()为相机投影模型函数，h^-1()是逆投影函数，函数I_k(u)为在第I_k帧上位置u的灰度值；

通过高斯牛顿迭代法求解，获得无人机的所述相对位姿T_k,k-1。

进一步，所述根据关键帧图像信息以及当前帧图像信息对无人机的所述相对位姿进行优化，获取优化后的特征点位置信息，包括：

根据所述相对位姿T_k,k-1，将所述关键帧图像信息的特征点投影到所述当前帧图像信息，根据所述关键帧图像信息和所述当前帧图像信息的特征点的灰度差优化特征点位置，所使用的优化函数为：

其中，u_i′为优化后的特征点位置，函数I_r(u_i)为在关键帧图像信息I_r上位置u_i的灰度值，u_i为h(T_k,k-1，p_i)，T_k,k-1为上一步解算获得的相对位姿，p_i为所述关键帧图像中特征点的位置；

利用高斯牛顿迭代法求解出u_i′，获得优化后的特征点位置信息u_i′。

进一步，所述根据优化后的所述特征点位置信息优化世界位姿，获取无人机在世界坐标系下的所述位姿信息，包括：

根据优化后的所述特征点位置信息，生成投影残差||δu_i||＝||u_i′-h(T_k,w,p_i)||≠0，构造世界位姿T_k,w的优化函数为：

利用高斯牛顿迭代法，求解得到T_k,w为无人机的所述位姿信息。

进一步，所述实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息，包括：

根据所述位姿信息以及所述深度信息获取无人机的位置信息；

判断所述深度信息是否为关键帧图像中的深度信息；

若是，则将世界坐标系下的地图栅格化生成栅格化地图，将关键帧图像中的所述深度信息影射到所述栅格化地图中构建查找表，根据所述查找表将无人机的所述位置信息影射到所述栅格化地图中生成所述障碍物深度地图；

判断无人机的位移在所述障碍物深度地图中是否形成一个闭环；

若是，则采用图优化对所述障碍物深度地图进行优化，根据优化后的所述障碍物深度地图获得无人机的所述全球位姿信息。

进一步，所述判断无人机的位移在所述障碍物深度地图中是否形成一个闭环，包括：

获取无人机的所述位姿信息作为当前节点；

从所述障碍物深度地图中找到与所述当前节点相邻的所有节点；

依据所述节点的id序号挑选与当前节点在一定距离范围内的节点进行匹配，当匹配的均值和协方差满足预先设定的阈值则无人机的位移在所述障碍物深度地图中形成一个闭环。

进一步，所述采用图优化对所述障碍物深度地图进行优化，根据优化后的所述障碍物深度地图获得无人机的所述全球位姿信息，包括：

无人机的全球位姿信息为c_i＝[t_i,θ]^T＝[x_i,y_i,θ]^T；

无人机从c_i到c_j的偏移量为其中，R_i ^T是一个2×2的关于θ_i的坐标系转换矩阵，0<j≤i；

总误差方程为其中Λ_ij为协方差的逆矩阵；

采用图优化对所述障碍物深度地图进行优化的优化方程为：

求解所述优化方程得到Δx，将Δx代入姿态更新方程t_i＝t_i+Δt_i，θ_i＝θ_i+Δθ_i得到无人机的所述全球位姿信息。

进一步，所述根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径，包括：

获取无人机的起始点q_start和目标点q_goal；

判断所述起始点q_start与所述目标点q_goal是否相同；

若不相同，则生成随机点q_rand，其中q_rand∈C_free，C为所有空间，C_free为无障碍空间，步长为ε，所有空间C中任意两点距离为Dis(x₁,x₂)，T_k为无障碍空间C_free中有k个节点的随机树；

生成与所述随机点q_rand距离最近的新节点q_near；其中所述新节点q_new在所述随机点q_rand和所述新节点q_near的连线上，所述随机点q_rand与所述新节点q_near满足 Dis(q_near,q_rand)≤Dis(q,q_rand)，所述新节点q_new满足q_new∈C_free，且Dis(q_new,q_rand)＝ε；

判断是否存在新节点q_new，如果存在新节点q_new，则在所述随机树T_k上添加新节点生成T_k+1，并转到下一步；

判断所述新节点q_new是否是目标点q_goal，若是，则结束该算法，获得无人机的飞行路径。

本发明还提出一种基于多传感器融合的无人机SLAM导航系统，包括机载SLAM传感器、空中飞行平台、机载计算平台和地面控制平台，所述机载SLAM传感器包括单目图像传感器和激光雷达传感器；

所述单目图像传感器用于实时采集无人机周围环境的图像信息；

所述激光雷达传感器用于实时采集无人机与障碍物的深度信息；

所述机载计算平台用于接收所述单目图像传感器实时采集的所述图像信息，并根据所述图像信息获取无人机的位姿信息；接收所述激光雷达传感器采集的深度信息将所述位姿信息，以及将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息；根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径；

所述空中飞行平台用于控制根据所述飞行路径控制无人机的自主避障飞行；

所述地面控制平台与所述空中飞行平台实现无线连接，用于实现对无人机的远程操控。

从上面所述可以看出，本发明提供的基于多传感器融合的无人机SLAM 导航方法及系统，可以实现复杂环境中无人机的实时定位与地图构建。相对于传统的无人机导航技术实现实时定位和地图构建与自主导航，提高无人机的智能化程度和导航精度。

附图说明

图1为本发明实施例一种基于多传感器融合的无人机SLAM导航方法的流程图；

图2为本发明实施例一种基于多传感器融合的无人机SLAM导航装置的结构图；

图3为本发明实施例空中飞行平台电路结构图；

图4为本发明实施例工作原理示意图；

图5为本发明实施例所述构建障碍物深度地图的方法流程图；

图6为本发明实施例所述生成无人机的飞行路径的方法流程图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。

需要说明的是，本发明实施例中所有使用“第一”和“第二”的表述均是为了区分两个相同名称非相同的实体或者非相同的参量，可见“第一”“第二”仅为了表述的方便，不应理解为对本发明实施例的限定，后续实施例对此不再一一说明。

图1为本发明实施例一种基于多传感器融合的无人机SLAM导航方法的流程图。

在本发明的一个实施例中，所述基于多传感器融合的无人机SLAM导航方法包括：

S100，实时采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息。

S200，实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息。具体的，激光传感器采集当前时刻的距离信息作为深度信息。

S300，根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径，根据所述飞行路径控制无人机的自主避障飞行。

在本发明的另一个实施例中，S100所述采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息，包括：

S101，根据当前帧图像信息以及前一帧图像信息获取无人机的相对位姿。

进一步，S101具体包括：将前一帧图像信息I_k-1的特征点投影到当前帧图像信息I_k上，根据两帧的特征点的灰度差构建无人机的相对位姿的优化函数；

其中，T_k,k-1为无人机的所述相对位姿，δI(T,u)＝I_k(h(T·h^-1(u,du)))-I_k-1(u)，u为 (u,v)^T，du为深度，h()为相机投影模型函数，h^-1()是逆投影函数，函数I_k(u)为在第I_k帧上位置u的灰度值；通过高斯牛顿迭代法求解，获得无人机的所述相对位姿T_k,k-1。

具体的，假设相机相邻两帧之间的相对位姿为根据相对位姿将上一帧I_k-1的特征点投影到当前帧I_k，根据两帧特征点的灰度差构建优化函数，优化相机两帧的相对位姿具体的处理过程如下：

预备阶段：假设相邻帧之间的位姿已知，通过之前的多帧特征检测及深度估计，已知I_k-1帧的特征点的位置(u,v)及它的深度d。

投影特征点：将I_k-1帧的特征点投影到三维空间P为[X,Y,Z]，该点是在世界坐标系下的，再通过位姿转换和相机内参投影到I_k帧的平面(u′,v′)。

迭代优化更新位姿，由于假设极短的时间内相邻的两个点它的亮度值没变化，得出的位姿只是一个假设值，导致投影点不准确，所以要不断优化优化函数为最小化残差函数其中δI(T,u)＝I_k(h(T·h-¹(u,du)))-I_k-1(u)，u为(u,v)^T，du是深度，h()是相机投影模型函数，h-¹()是逆投影函数。上述的非线性最小化二乘问题，可以用高斯牛顿迭代法求解无人机的相对位姿T_k,k-1，位姿的迭代增量ξ(李代数)可以通过方程 J^TJξ＝-J^TδI(0)计算。

S102，根据关键帧图像信息以及当前帧图像信息对无人机的所述相对位姿进行优化，获取优化后的特征点位置信息。

优选的，S102进一步包括：

根据所述相对位姿T_k,k-1，将所述关键帧图像信息的特征点投影到所述当前帧图像信息，根据所述关键帧图像信息和所述当前帧图像信息的特征点的灰度差优化特征点位置，所使用的优化函数为：

其中，u_i′为优化后的特征点位置，函数I_r(u_i)为在关键帧图像信息I_r上位置u_i的灰度值，u_i为h(T_k,k-1，p_i)，T_k,k-1为上一步解算获得的相对位姿，p_i为关键帧中特征点的位置；

利用高斯牛顿迭代法求解出u_i′，获得优化后的特征点位置信息u_i′。

具体的，S101得到当前的相对位姿，是基于上一帧计算的结果，如果把它当作真实位姿估计，将有较大的累积误差，因此需要进一步和地图的特征点比对，来对当前位姿进一步优化。

对每个当前帧能观察到的地图点p，找到观察p角度最小的关键帧r对应的点u_i，优化得到p在当前帧上的投影u_i′，也就是优化像素位置。优化目标函数为该问题和S101都是非线性最小二乘化问题，迭代计算方程和上一步是一样的。通过这一步得到优化后的特征点预测位置，它比初始预测的位置更加准确。

S103，根据优化后的所述特征点位置信息优化世界位姿，获取无人机在世界坐标系下的所述位姿信息。

优选的，S103进一步包括：

根据优化后的所述特征点位置信息，生成投影残差||δu_i||＝||u_i′-h(T_k,w,p_i)||≠0，构造世界位姿T_k,w的优化函数为：

利用高斯牛顿迭代法，求解得到T_k,w为无人机的所述位姿信息。

具体的，利用上一步建立起的(p_i,u_i)的对应关系，优化世界坐标系下的位姿T_k,w，该位姿为世界坐标下无人机总的位姿变化，S102中由于优化后的特征位置和之前预测的特征位置存在差异，这个能用来构造T_k,w的优化目标函数为

通过上述三个步骤实现的基于单目视觉里程计的位姿估计，通过相机的特征点匹配进行位姿估计，可以获得相机的位姿信息，为后续的无人机位姿控制和航迹规划提供位姿信息。

在本发明的另一个实施例中，S200所述实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息，参照图5所示，包括：

S201，根据所述位姿信息以及所述深度信息获取无人机的位置信息。优选的，读取基于单目视觉里程计定位算法的估计的位姿，获得机器人位置。

S202，判断所述深度信息是否为关键帧图像中的深度信息。

S203，若是，则将世界坐标系下的地图栅格化生成栅格化地图，将关键帧图像中的所述深度信息影射到所述栅格化地图中构建查找表，根据所述查找表将无人机的所述位置信息影射到所述栅格化地图中生成所述障碍物深度地图。

具体的，假设无人机从两个不同的位姿x_i-1、x_i，用激光雷达扫描观测周围环境，将获得两帧与之相关的深度信息z_i-1、z_i，假设无人机从位姿x_i-1移动到x_i，u为无人机的运动量，世界坐标系下观测点的坐标为m，激光雷达距离观测点的距离即深度信息为z。为了得到相对靠后的机器人的位置其中p(z|x_i,m)观测模型，p(x_i|x_i-1,u)是无人机的运动模型。假设每个激光雷达的回波z_j都是独立的，观测方程就可写成p(z|x_i,m)的概率可以通过构建一个二维的查找表格加速计算，这个查找表格包含每个位置(x,y)上的激光雷达观测的对数的概率。

在世界坐标下栅格化地图，无人机的位姿可以通过视觉里程计的数据影射到栅格地图中，同时也可以机体坐标系下的激光数据影射到地图构建查找表。查找表匹配具体处理过程如下：

粗匹配，将激光数据点投影到该区域，获得匹配的参考模型，并对出现激光数据的栅格点附近进行高斯模糊。因为高斯模糊的作用，区域上每个栅格点反映的障碍由一个值表示。由于机器人的位姿存在移动，生成的地图的每个点的最终响应值等于该点的累加值除以可到达的最高值。

精匹配，粗匹配采用较低分辨率搜索得到障碍位置均值，精匹配的方法和粗匹配一样，对障碍位置进一步采取更高的分辨率进行划分，得到更加精确的求解。

计算平均值和协方差，每次匹配过程中，选取几个拥有最优响应的位姿状态，取平均值作为匹配结果。协方差分为位置协方差和角度协方差，位置协方差为x、y的协方差，角度协方差为θ与其自身的协方差。

S204，判断无人机的位移在所述障碍物深度地图中是否形成一个闭环。

具体的，回环检测是检测当前帧是否与之前的帧是否匹配，如果匹配可以认为机器人的位移形成一个闭环，在地图上就需要将形成回环的地图首尾相连接。回环检测的具体实施步骤如下：

从所述障碍物深度地图中找到与所述当前节点相邻的所有节点；

依据所述节点的id序号挑选与当前节点在一定距离范围内的节点进行匹配，。

获取无人机的所述位姿信息作为当前节点；

在一定距离范围内从障碍物深度地图中找到与当前节点相邻的所有节点；

依据节点的id序号挑选与当前节点在一定距离范围内的节点进行匹配，当匹配的均值和协方差满足预先设定的阈值则无人机的位移在所述障碍物深度地图中形成一个闭环；

若不形成闭环，则修正地图，在地图形成闭环的位置添加边，使地图形成闭环。

S205，若是，则采用图优化对所述障碍物深度地图进行优化，根据优化后的所述障碍物深度地图获得无人机的所述全球位姿信息。

具体的，全局优化当地图形成闭环之后，需要对地图做全局的优化，优化方式为图优化。已知机器人的全球位姿为c_i＝[t_i,θ]＝[x_i,y_i,θ_i]^T，对于从c_i到c_j的任何实际位姿，它们的偏移量可以计算为其中θ_i为偏航角， R_i ^T是一个2×2的关于θ_i的坐标系转换矩阵，将世界坐标系转换为机体坐标系， 0<j≤i。总误差方程为其中Λ_ij为协方差的逆矩阵。通过最小化总误差x²，来找到位姿c的最佳位姿，利用 Levenberg-Marquardt(LM)法围绕c的当前值迭代求解。定义 H≡J^TΛJ，则LM系统方程为(H+λdiagH)Δx＝J^TΛe，其中，Λ_ij为第i帧激光数据与第j数据的协方差；J为雅可比矩阵

而求解该方程可以得到Δx，将Δx代入姿态更新方程t_i＝t_i+Δt_i，θ_i＝θ_i+Δθ_i。

在本发明的一些实施例中，参照图6所示，S300根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径包括：

S301，获取无人机的起始点q_start和目标点q_goal。

S302，判断所述起始点q_start与所述目标点q_goal是否相同，若相同则结束该算法，若不相同则转到S303.

S303，若不相同，则生成随机点q_rand，其中q_rand∈C_free，C为所有空间，C_free为无障碍空间，步长为ε，所有空间C中任意两点距离为Dis(x₁,x₂)，T_k为无障碍空间C_free中有k个节点的随机树。

S304，生成与所述随机点q_rand距离最近的新节点q_near；其中所述新节点q_new在所述随机点q_rand和所述新节点q_near的连线上，所述随机点q_rand与所述新节点 q_near满足Dis(q_near,q_rand)≤Dis(q,q_rand)，所述新节点q_new满足q_new∈C _fre，_e且 Dis(q_new,q_rand)＝ε。

S305，判断是否存在新节点q_new，如果存在新节点q_new，则在所述随机树T_k上添加新节点生成T_k+1，并转到S306；否则结束算法。

S306，判断所述新节点q_new是否是目标点q_goal，若是，则结束该算法，获得无人机的飞行路径。

本发明还提出一种基于多传感器融合的无人机SLAM导航系统，包括机载SLAM传感器、空中飞行平台、机载计算平台和地面控制平台，所述机载 SLAM传感器包括单目图像传感器和激光雷达传感器，参照附图2、图3所示。

单目图像传感器用于实时采集无人机周围环境的图像信息，然后利用 Semi-Direct的方法对稀疏的特征块进行匹配获得相机的预测位姿，再利用牛顿迭代法对相机的预测位姿进行优化，从而获得更加精确的相机位姿，再通过相机和无人机之间的坐标系变换计算出无人机的位姿信息，利用这个方法构建视觉里程计，实现无人机的位姿控制，解决了无人机在GPS信号丢失的情况下的定位问题。

激光雷达传感器用于实时采集无人机与障碍物的深度信息，对激光信息进行匹配，形成初始的障碍物地图，然后通过关键帧进行回环检测，再采用图优化的方法优化全局的障碍物地图，相比传统的滤波方法优化内存消耗与计算量更小，得到的障碍物地图更加精确。

机载计算平台用于接收所述单目图像传感器实时采集的所述图像信息，并根据所述图像信息获取无人机的位姿信息；接收所述激光雷达传感器采集的深度信息将所述位姿信息，以及将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息；根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径。机载计算平台采用高性能的ordoidxu4作为运算处理器，运行ubuntu16.04操作系统，将接收到的相机和激光信息进行处理从而获得无人机自身的位姿信息和障碍物地图信息，利用地面控制站设定飞行目标，无人机的机载计算平台根据无人机自身的位姿信息和障碍物地图信息采用基于RRT 的在线动态规划路径规划算法生成飞行路径，再将规划出的路径引入无人机的位置控制，实现无人机的自主避障飞行。同时运行SLAM算法和导航规划算法，并把控制指令通过串口发送给飞行平台的控制单元。

所述空中飞行平台包括动力驱动单元、系统控制单元，动力驱动单元包括电源与无刷电调与无刷电机，电源为3S锂电池，为整个系统供电，无刷电调与无刷电机用于为无人机提供飞行动力。空中飞行平台用于控制根据所述飞行路径控制无人机的自主避障飞行。

所述地面控制平台与所述空中飞行平台实现无线连接，用于实现对无人机的远程操控。包括PC控制端与移动控制端，通过无线模块与无人机相连接提供程序烧录、传感器校正、姿态角仪表盘显示等一系列功能；可以同时预先设定无人机飞行线速度、角速度、高度、经纬度、飞行航线及飞行任务；同时还可以实现多旋翼无人机平台的远程操控。

具体的，为实现无人机空中的飞行控制和设备的正常工作，本发明设计了以STM32为核心控制芯片的无人机飞行作业平台飞行控制板，可以实现无人机的飞行控制和通信，它与外设连接结构示意图如图3所示。控制板上电启动后，首先进入待机状态，等待遥控器解锁信号和机载计算平台的SLAM系统准备信号。当接收到遥控器解锁信号和机载计算平台的SLAM系统准备信号后，飞行控制板的惯性传感器才进入工作状态，这两个信号缺一不可。机载计算平台的运行SLAM和路径规划算法，最终生成无人机可以安全飞行的路径，并把飞行的路径通过串口发送到飞行控制板，飞行控制板同时接收惯性传感器的姿态信息，结合机载计算平台运算的路径信息对无人机进行位姿校正。

飞行器的飞行控制板通过无线模块连接地面控制站，在地面控制站上的地图标定的区域内找到林区地图并选取飞行路线，在地图上选取航点并写入起始位置。

所述地面控制站上有无人机的俯仰横滚表、航向表、姿态三维表，支持地图设置飞行器的飞行路线并支持离线功能，且提供程序烧录、传感器校正、姿态角仪表盘显示等一系列功能；可以同时预先设定无人机飞行线速度、角速度、高度、经纬度、飞行航线及飞行任务。

无人机搭载提供飞行器飞行行动力的4500mah/3s锂电池一块和提过飞行器航空电子设备电源的BEC电源转换模块，选取相对空旷的起飞环境，接收机接收地面遥控器与地面控制站的飞行控制信号，用遥控器控制飞行器进入自动起飞模式，待飞行器上升至一定高度，切换飞行器的飞行模式，此时飞行器进入巡航模式。

待无人机进入巡航模式，无人机地面控制站操作手将目标航点和路径通过地面控制站发送给无人机，同时SLAM传感器单元进入工作状态，将周围图像信息和激光雷达信息发送给机载计算平台进行计算，生成无人机定位信息和障碍物地图，根据已知的定位信息和障碍物地图，加上地面站设定的目标航点和路径，无人机重新对路径进行规划，生成相对安全的飞行路径，作为无人机位置控制器的控制目标。因为SLAM传感器的探测距离一般远小于起始点到目标航点距离，所以定位和地图构建，路径规划是实时的，也就是飞行器起飞到到达目标航点，无人机需要一边进行定位和地图构建，一边进行路径规划。

除了巡航模式，还可以选择手动模式进行飞行，该模式主要是为了以防巡航模式发生故障设置的保险，当无人机在巡航模式发生故障时，无人机遥控操作手可以迅速地将飞行模式切换到手动模式，手动操作飞行。

本发明所述基于多传感器融合的无人机SLAM导航方法及系统，视觉里程计采用Semi-Direct的方法对稀疏的特征块进行匹配，相比传统的对稠密的特征匹配视觉SLAM，减少了视觉SLAM对环境场景特征的依赖，能够处理更多的特征点，场景鲁棒性更好，并且计算耗时更小，定位效率和精度更高；激光SLAM采用查找表法进行前端的激光数据匹配，将世界坐标系下的地图栅格化，构建查找表将无人机的位姿信息和激光数据影射到栅格地图中，并选择几个拥有最优响应的位姿，计算其平均值和协方差。利用图优化的原理优化构建的地图，相比传统的高斯牛顿法或者滤波器法优化方法，计算量更小，效率更高，并且前端使用精确的视觉里程计作为定位数据，为导航提供更加精确的导航数据；路径规划采用基于RRT算法，适用无人机动态环境下的路径规划，在无人机的整体路径规划过程中将总的目标分成几个小目标，提高了避障的效率，缩短了动态避障的时间，较以往的方法更加适应无人机在林区复杂环境的路径规划。

所属领域的普通技术人员应当理解：以上任何实施例的讨论仅为示例性的，并非旨在暗示本公开的范围(包括权利要求)被限于这些例子；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明它们没有在细节中提供。

另外，为简化说明和讨论，并且为了不会使本发明难以理解，在所提供的附图中可以示出或可以不示出与集成电路(IC)芯片和其它部件的公知的电源 /接地连接。此外，可以以框图的形式示出装置，以便避免使本发明难以理解，并且这也考虑了以下事实，即关于这些框图装置的实施方式的细节是高度取决于将要实施本发明的平台的(即，这些细节应当完全处于本领域技术人员的理解范围内)。在阐述了具体细节(例如，电路)以描述本发明的示例性实施例的情况下，对本领域技术人员来说显而易见的是，可以在没有这些具体细节的情况下或者这些具体细节有变化的情况下实施本发明。因此，这些描述应被认为是说明性的而不是限制性的。

尽管已经结合了本发明的具体实施例对本发明进行了描述，但是根据前面的描述，这些实施例的很多替换、修改和变型对本领域普通技术人员来说将是显而易见的。例如，其它存储器架构(例如，动态RAM(DRAM))可以使用所讨论的实施例。

本发明的实施例旨在涵盖落入所附权利要求的宽泛范围之内的所有这样的替换、修改和变型。因此，凡在本发明的精神和原则之内，所做的任何省略、修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种基于多传感器融合的无人机SLAM导航方法，其特征在于，包括：

实时采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息；

实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息；

根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径，根据所述飞行路径控制无人机的自主避障飞行。

2.根据权利要求1所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述采集无人机周围环境的图像信息，并根据所述图像信息获取无人机的位姿信息，包括：

根据当前帧图像信息以及前一帧图像信息获取无人机的相对位姿；

根据关键帧图像信息以及当前帧图像信息对无人机的所述相对位姿进行优化，获取优化后的特征点位置信息；

根据优化后的所述特征点位置信息优化世界位姿，获取无人机在世界坐标系下的所述位姿信息。

3.根据权利要求2所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述根据当前帧图像信息以及前一帧图像信息获取无人机的相对位姿，包括：

将前一帧图像信息I_k-1的特征点投影到当前帧图像信息I_k上，根据两帧的特征点的灰度差构建无人机的相对位姿的优化函数；

其中，T_k,k-1为无人机的所述相对位姿，δI(T,u)＝I_k(h(T·h^-1(u,du)))-I_k-1(u)，u为(u,v)^T，du为深度，h()为相机投影模型函数，h^-1()是逆投影函数，函数I_k(u)为在第I_k帧上位置u的灰度值；

通过高斯牛顿迭代法求解，获得无人机的所述相对位姿T_k,k-1。

4.根据权利要求3所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述根据关键帧图像信息以及当前帧图像信息对无人机的所述相对位姿进行优化，获取优化后的特征点位置信息，包括：

根据所述相对位姿T_k,k-1，将所述关键帧图像信息的特征点投影到所述当前帧图像信息，根据所述关键帧图像信息和所述当前帧图像信息的特征点的灰度差优化特征点位置，所使用的优化函数为：

其中，u_i′为优化后的特征点位置，函数I_r(u)为在关键帧图像信息I_r上位置u的灰度值，u_i为h(T_k,k-1，p_i)，T_k,k-1为上一步解算获得的相对位姿，p_i为所述关键帧图像中特征点的位置；

利用高斯牛顿迭代法求解出u_i′，获得优化后的特征点位置信息u_i′。

5.根据权利要求4所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述根据优化后的所述特征点位置信息优化世界位姿，获取无人机在世界坐标系下的所述位姿信息，包括：

根据优化后的所述特征点位置信息，生成投影残差||δu_i||＝||u_i′-h(T_k,w,p_i)||≠0，构造世界位姿T_k,w的优化函数为：

利用高斯牛顿迭代法，求解得到T_k,w为无人机的所述位姿信息。

6.根据权利要求1所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述实时采集无人机与障碍物的深度信息，将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息，包括：

根据所述位姿信息以及所述深度信息获取无人机的位置信息；

判断所述深度信息是否为关键帧图像中的深度信息；

若是，则将世界坐标系下的地图栅格化生成栅格化地图，将关键帧图像中的所述深度信息影射到所述栅格化地图中构建查找表，根据所述查找表将无人机的所述位置信息影射到所述栅格化地图中生成所述障碍物深度地图；

判断无人机的位移在所述障碍物深度地图中是否形成一个闭环；

若是，则采用图优化对所述障碍物深度地图进行优化，根据优化后的所述障碍物深度地图获得无人机的所述全球位姿信息。

7.根据权利要求6所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述判断无人机的位移在所述障碍物深度地图中是否形成一个闭环，包括：

获取无人机的所述位姿信息作为当前节点；

从所述障碍物深度地图中找到与所述当前节点相邻的所有节点；

依据所述节点的id序号挑选与当前节点在一定距离范围内的节点进行匹配，当匹配的均值和协方差满足预先设定的阈值则无人机的位移在所述障碍物深度地图中形成一个闭环。

8.根据权利要求7所述的基于多传感器融合的无人机SLAM导航方法，其特征在于，所述采用图优化对所述障碍物深度地图进行优化，根据优化后的所述障碍物深度地图获得无人机的所述全球位姿信息，包括：

无人机的全球位姿信息为c_i＝[t_i,θ_i]^T＝[x_i,y_i,θ_i]^T；

无人机从c_i到c_j的偏移量为其中，R_i ^T是一个2×2的关于θ_i的坐标系转换矩阵，0<j≤i；

总误差方程为其中Λ_ij为协方差的逆矩阵；

采用图优化对所述障碍物深度地图进行优化的优化方程为：

求解所述优化方程得到Δx，将Δx代入姿态更新方程t_i＝t_i+Δt_i，θ_i＝θ_i+Δθ_i得到无人机的所述全球位姿信息。

9.根据权利要求1所述的基于多传感器融合的无人机SLAM导航方法，其特征在于所述根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径，包括：

获取无人机的起始点q_start和目标点q_goal；

判断所述起始点q_start与所述目标点q_goal是否相同；

若不相同，则生成随机点q_rand，其中q_rand∈C_free，C为所有空间，C_free为无障碍空间，步长为ε，所有空间C中任意两点距离为Dis(x₁,x₂)，T_k为无障碍空间C_free中有k个节点的随机树；

生成与所述随机点q_rand距离最近的新节点q_near；其中所述新节点q_new在所述随机点q_rand和所述新节点q_near的连线上，所述随机点q_rand与所述新节点q_near满足Dis(q_near,q_rand)≤Dis(q,q_rand)，所述新节点q_new满足q_new∈C_free，且Dis(q_new,q_rand)＝ε；

判断是否存在新节点q_new，如果存在新节点q_new，则在所述随机树T_k上添加新节点生成T_k+1，并转到下一步；

判断所述新节点q_new是否是目标点q_goal，若是，则结束该算法，获得无人机的飞行路径。

10.一种基于多传感器融合的无人机SLAM导航系统，其特征在于，包括机载SLAM传感器、空中飞行平台、机载计算平台和地面控制平台，所述机载SLAM传感器包括单目图像传感器和激光雷达传感器；

所述单目图像传感器用于实时采集无人机周围环境的图像信息；

所述激光雷达传感器用于实时采集无人机与障碍物的深度信息；

所述机载计算平台用于接收所述单目图像传感器实时采集的所述图像信息，并根据所述图像信息获取无人机的位姿信息；接收所述激光雷达传感器采集的深度信息将所述位姿信息，以及将所述位姿信息以及所述深度信息融合构建障碍物深度地图，根据所述障碍物深度地图获取无人机的全球位姿信息；根据所述全球位姿信息、所述障碍物深度地图采用在线动态规划路径的规划方法生成无人机的飞行路径；

所述空中飞行平台用于控制根据所述飞行路径控制无人机的自主避障飞行；

所述地面控制平台与所述空中飞行平台实现无线连接，用于实现对无人机的远程操控。