CN112347840B - 视觉传感器激光雷达融合无人机定位与建图装置和方法 - Google Patents

Abstract

本发明属于视觉传感器、激光雷达、嵌入式、无人机领域，为解决无人机在GPS拒止环境下自主定位与环境地图建立问题，实现无人机在未知环境下自主飞行与避障，本发明，视觉传感器激光雷达融合无人机定位与建图装置和方法，包括双目相机、激光雷达、IMU惯性测量单元、嵌入式机载处理器和无人机平台；双目相机采集周围环境的视觉信息；激光雷达主要用于测量无人机周围环境的3维位置信息；IMU惯性测量单元用于高频测量无人机自身加速度与角速度信息；嵌入式机载处理器用于算法运行，通过处理相机、雷达与IMU采集到的数据，运行自主定位与建图算法，获得无人机自身位姿并同时进行环境地图的建立。本发明主要应用于无人机设计制造场合。

Description

视觉传感器激光雷达融合无人机定位与建图装置和方法

技术领域

本发明属于视觉传感器领域、激光雷达传感器领域、嵌入式系统领域、无人机领域，尤 其涉及一种基于视觉与激光雷达结合的无人机感知系统，解决了在GPS(全球卫星定位系统) 拒止环境下基于视觉雷达的无人机自主定位与环境地图建立的问题。

背景技术

近年来无人机技术是国内外研究的重要热点之一，无人机以无线遥控或嵌入式程序控制 代替人工完成各种任务，凭借其体积小、成本低、易用性好、对飞行环境要求低等优点，在 军事领域和民用领域得到了广泛的应用。军事方面无人机在侦查、攻击、电子对抗等方面发 挥了重要的作用。民用方面无人机在航拍、灾情监控、线路巡检以及农药喷洒等领域的应用 越来越广泛。

传统的无人机定位方法是采用GPS、北斗等全球卫星导航定位系统。但随着无人机技术 的发展尤其是随着无人机应用场景的复杂化，在室内、隧道、楼体间等场景下GPS的定位精 度受卫星信号强弱影响，信号呈现不稳定乃至拒止现象，无法实时为无人机提供准确的定位 信息。基于卫星定位的无人机在类似场景下的应用受到严重限制，因此近年来基于无人机自 身携带的传感器的无人机自主飞行技术是无人机领域研究热点。

实现无人机自主飞行，要解决无人机在GPS拒止且未知复杂环境下自主定位与环境地图 建立两大问题。自主定位指传感器提供精准可靠的无人机飞行状态信息，这是实现无人机自 主飞行的基础与关键。环境地图建立指无人机利用传感器探测的环境信息建立周围环境的地 图，是无人机实现自主导航、路径规划与避障的必要能力。为了解决无人机的自主定位与环 境建图问题，我们常采用激光雷达、单双目摄像头、深度图像相机和IMU(Inertial Measurement Unit)惯性测量单元等多传感器设备进行无人机自主定位与建图。

针对无人机自主定位与建图问题，国内外各大高校都进行了深入的研究。2014年，德国 慕尼黑工业大学提出了一种基于直接法的稠密单目视觉SLAM(simultaneouslocalization and mapping，同步定位与建图)算法来构建大规模的环境地图。2015年，卡耐基梅隆大学的团 队提出一种基于鱼眼相机与2D激光雷达融合的无人机自主飞行系统，2018年，香港科技大 学的团队提出了一种飞行状态估计的单目视觉惯性系统，实现了无人机自主导航飞行。目前 美国麻省理工大学、宾夕法尼亚大学、苏黎世联邦理工大学、新加坡国立大学等国家的科研 团队都采用视觉或激光雷达设备进行了相关研究，并取得了一定成果；国内的清华大学、北 京航空航天大学、哈尔滨工业大学、天津大学等也开展了相关研究。由此可见无人机在复杂 环境下的自主定位与建图是目前国际无人机领域的研究热点之一，因此，设计一套精度高、 实时性强、可扩展性强的无人机自主定位与建图系统具有十分重要的研究价值和应用意义。

发明内容

为克服现有技术的不足，本发明旨在提出解决无人机在GPS拒止环境下无人机自主定位 与环境地图建立问题，实现无人机在未知环境下自主飞行与避障。具体而言，本发明将相机 和雷达感知的信息作为输入，通过计算得到无人机自身状态信息和无人机周围环境地图。此 外本发明有利于其他无人机自主定位算法在该平台上进行验证，有限提高了无人机自主定位 的研究效率，具有极大的应用价值。为此，本发明采取的技术方案是，视觉传感器激光雷达 融合无人机定位与建图方法，步骤如下：

1)视觉里程计：视觉里程计部分采用基于多状态卡尔曼滤波MSCKF(Multi-StateConstraint Kalman Filter)的算法，将惯性测量单元IMU(Inertial Measurement Unit)预积分 结果作为预测值，同时接收双目相机的原始数据，从相机的图像中提取特征点并进行特征匹 配，然后构建观测方程更新预测值，得到高频的无人机位姿、速度信息；

2)视觉雷达松耦合里程计：首先将MSCKF视觉里程计的高频的无人机位姿信息作为先 验，对当前扫描的雷达点云做去畸变处理，然后从去畸变的点云中提出特征点并与点云地图 做特征匹配，构建出非线性优化问题，将MSCKF预测值作为优化初值，求解无人机位姿， 最终将高频的MSCKF估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状 态信息，同时逐步拼接点云地图；

3)建立占据体素地图：基于循环缓冲区的建图策略，将点云地图转化为以无人机位置为 中心的局部占据体素地图，占据体素地图的建立与更新通过循环缓冲区进行维护。

具体步骤如下：

第一步：视觉里程计，采用MSCKF视觉里程计，MSCKF视觉里程计算法分为两部分，第一部分是对IMU与相机的原始信息的数据处理，首先算法从双目相机图像上做有向快速Oriented Fast特征点提取，Oriented Fast特征点是计算机视觉领域的图像特征点，特征提取完 成后基于灰度不变假设，同一个特征点在不同图像帧上是相同的，所以算法搜索匹配当前得 到的特征点与之前图像上已经提取到的特征点，搜索过程使用IMU预积分得到的对应相机帧 的位姿变换加速搜索过程，以上过程称为特征跟踪，这一部分得到匹配成功的特征点的像素 位置作为滤波算法里的观测值输出到第二部分；

第二部分是多状态卡尔曼滤波模型：首先定义待估计的状态，MSCKF取连续20帧的无 人机状态，将其中无人机状态量的误差作为待估计的状态量，误差的定义是无人机状态的真 值和估计值之差,定义为

其中B_i,i＝1,2,…20代表第i帧相机坐标系，

为IMU误差状态，表达式

其中

分别代表IMU 估计的无人机位置、速度误差状态，

代表四元数误差的三维近似，

与

分别代表IMU 加速度计与陀螺仪的误差状态，相机每帧位姿的误差

其中

代表相 机姿态的误差状态，

代表相机位置的误差状态；

多状态卡尔曼滤波模型分为预测、观测更新两步：预测步骤使用IMU预积分计算无人机 各项状态的预测值，具体步骤如下：首先不考虑噪声时IMU状态的动力学微分方程如下：

其中

分别代表对应IMU位置、速度、姿态、加速度计与陀螺仪 偏差状态，

和

a_m是IMU测量的加速度，w_m是IMU测量的角速度，

代表四元数乘法，

代表四元数转旋转矩阵，当考虑噪声与偏差的随机游走模型 时，得IMU误差状态的动力学方程如下：

其中F是误差状态的状态转移矩 阵，G是IMU噪声的输入矩阵，n_I是IMU噪声矩阵，

分别是加速度 计噪声方差，陀螺仪噪声方差，加速度计偏差噪声方差，陀螺仪偏差噪声方差，设当前帧为 k帧，当k+1帧到来时，使用中值积分离散化IMU动力学方程来更新的IMU状态与IMU误 差状态：

Θ_k是IMU误差状态转移矩阵。IMU误差状态的协方差矩阵

以如下方式传播：

其中

是k帧时刻IMU误差状态协方差矩阵，Q是IMU噪声的协方差矩阵，Q_k代表预测模型的噪声协方差矩阵，然后将

增广到全状态P_k+1|k，P_k+1|k如下：

其中

均为

与

间的协方差矩阵，

为

的协方差矩阵，再来看相机 的位值预测值

和姿态预测值

因为IMU与相机之间有姿态外参^Bq_I与位置外参 ^Bp_I，所以

和

有如下表示：

到此基于IMU预积分的预测步骤完成；

接下来开始观测更新步骤：首先根据特征跟踪的结果，观测到的特征点f_j在第i帧图像 上的归一化坐标

为图像测量的噪声，

为f_j在 第i帧相机坐标下的位置，设

为根据预测环节计算出的f_j的坐标，由此得到观测方程的残 差

的表达式：

其中

是f_j在世界系下的坐标，

代表相机的位置，

代表世界系到相机系的姿 态。将残差对相机帧的位姿的误差及特征点的位置误差取一阶线性化展开得

其中

与

分别为残差相对相机位姿与特征点位置的雅克比矩 阵，同时计算多帧的残差，累计起来的残差

其中

与n^j分别 是累积形式的雅克比矩阵与噪声矩阵，

分别是累计的无人机状态误差与特征点位置 误差，该残差形式不符合卡尔曼滤波的表达形式，所以定义残差在

的左零空间的映射

得到了符合卡尔曼滤波模型的残差形式：

其中

是包含所有观测残差的雅克比矩阵，n是重新映射后的噪声矩阵，为了恢复左 零映射导致的残差维度变化，对

做正交三角分解，得

[M₁ M₂] 是正交矩阵，T_H是上三角矩阵，得最终的观测方程

最后根据传统的卡尔曼滤波模型计算卡尔曼增益以及更新预测值和协方差矩阵：

其中R_n是噪声

的协方差矩阵，K是计算得到的卡尔曼增益，至此，视觉里程计完 成了对无人机状态的估计，因为有IMU的使用，视觉里程计得到高频无人机位姿信息；

第二步：视觉雷达松耦合里程计，在这一部分本发明首先接受雷达点云信息，并使用视 觉里程计的无人机位姿修正点云畸变，平均有10个MSCKF输出的信息，设一段时间内MSCKF位姿变换到雷达坐标下的变换矩阵

其对应的欧拉角与位移的表示方式为

二者可以相互转换。假设当前帧点云扫描结束时刻t_k位于5和6之间。设t_k时刻的位姿变换矩阵为

做线性插值得

对于当前点云帧里的每一个点

时间戳为t_i，采取相同的线性插值的方法获取t_i时刻 的位姿

再计算

与

间的姿态变化

得到

后，将

变换到扫描结 束t_k时刻，得到了

这样当前帧点云坐标便全局统一到当前帧扫描结束时刻的雷达坐标下， 去除了点云畸变；

其次是从点云中提取特征，具体采用LOAM(Lidar Odometry And Mapping)雷达里程计的 方法，从点云中提取两种特征点，位于平坦区域的平面点和位于边缘或尖锐区域的边缘线点， 为了提取这两种点，对点云中每个点

定义平滑度c：

其中E代表

的数个邻域点。根据c的定义，如果点分布在平面上，c的值接近0；若点分布在边缘上，c的值很大，先计算每个点云的平滑度值，排序后取最大和最小的数十个点，即为特征点，在特征提取时为了避免特征点扎堆，规定一个特征点周围10个邻近点不能作为特征点，最终特征提取环节获得了边缘线点集

与平面点集

最后是特征匹配优化位姿，首先将

和

利用

变换到世界坐标系下，在当前位置 200m范围的点云地图P_G上搜索与

和

匹配的特征线与面并计算当前特征点到各自匹配 的特征线、面的距离。定义当前线、面特征

和

里的任一点

到P_G中的匹配特征距离：

其中，d_L、d_Π为当前特征点到匹配的线、面的距离，下面统称为d。点p_(k-1,j)，p_(k-1,l)，p_(k-1,m)为在P_G中距离点

最近的点，这些点张成与

匹配的线与面，这样得到两个以距离作为残差，以位姿作为状态变量的非线性函数，将其转化为非线性最小二乘问题，以MSCKF 估计的位姿为先验值，使优化目标函数d近0,采用高斯牛顿法进行位姿优化求解。求解过程 如下，先将特征距离函数写为

设状态的增量为

做一阶泰勒展开得

其中J是函数f()相对于位姿的雅克比矩阵，根据高斯 牛顿方法可得要求解的

有以下方程

经过这样数次迭代，最终优化求 解出了更精确的位姿T_k。这步优化以5Hz低频进行，最后一步是位姿融合，每次得到T_k后计 算

接下来MSCKF输出位姿时，都左乘该△T_k，直到下一次雷达点云匹配优化 更新△T_k；

第三步：建立占据体素地图，将点云地图转化为一种基于循环数组的占据体素地图，建 立占据体素地图有分为两个部分，第一步将3D空间体素化并建立循环数组，首先按照分辨 率r将三维空间划为若干大小一致的立方体称为体素，并对每个体素建立索引index，index为 三维向量index＝[xⁱⁿ,yⁱⁿ,zⁱⁿ]^T，对于空间中任一点p_o，其对应的index表达式为

其中floor()代表向下取整，这样建立起了空间点与体素索引间的映射，维护一个以当前 无人机位置为中心，边长为N个体素长度的局部立方体空间，具体方法为维护一个包含长度 为N³的数组以及局部地图的起始索引O的循环数组，当无人机移动时，只需要改变O和更新 数组中更新变化的体素，判断体素是否在局部空间及计算体素索引τ在循环数组中位置的方 法inVolume(τ_i)定义如下：

inVolume(τ_i)＝0≤τ_i-O_i≤N,i∈{x,y,z} (13)

其中τ_i是τ的任意一维。而τ在数组内的位置l_τ用以下公式计算：

其中address(τ)_i代表体素τ在维度i上的索引，mod代表取余。运用以上公式，当无人机 移动时，更新的体素索引会对数组循环赋值，以上建立了体素索引与循环数组索引的映射， 就可以通过更新循环缓冲区维护局部地图，到此建图的第一步完成；

第二部分是对初始化完成的体素地图进行插值，插值指对每个体素计算占据概率并赋予 状态，具体的插值过程如下：

(1)遍历所有位于局部空间的点云，计算对应体素的占据概率，大于0.8时体素为占据 状态，代表无人机无法通过

(2)对局部空间以外的点，在局部空间内找到距离该点最近的体素索引，计算对应占据 概率，小于0.2即设为空闲状态，代表无人机可以通过；

(3)遍历已有的占据体素，向无人机所在位置投射射线，射线经过的体素都设为空闲；

在插值时，每个体素的占据概率是综合当前和历史所有观测结果得到的，占据概率 P(n|z_1:t)计算公式如下：

其中n代表当前观测的体素，T＝{1,...t}时刻，观测的数据为{z₁,...z_t}，该公式表明当前 体素的占据概率与当前观测结果z_t，先验概率P(n)和之前估计的概率P(n|z_1:t-1)有关；

使用对数logit变换将P(n|z_1:t)变换到全体实数空间L(n|z_1:t)，logit变换公式为

其中log()代表对数运算。那么占据概率的计算公式的logit表达为 L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)。可见占据概率的计算完全变成了简单的加法，最后利用逆logit 变换得到真实的占据概率

其中exp()代表指数运算；到此插值 部分完成，以无人机位置为中心的占据体素地图建立完成。

视觉传感器激光雷达融合无人机定位与建图装置，包括双目相机、3D激光雷达、IMU 惯性测量单元、嵌入式机载处理器和无人机平台；双目相机采集周围环境的视觉信息；3D激 光雷达主要用于测量无人机周围环境的3维位置信息；IMU惯性测量单元用于高频测量无人 机自身加速度与角速度信息；嵌入式机载处理器用于算法运行，通过处理相机、雷达与IMU 采集到的数据，运行自主定位与建图算法，获得无人机自身位姿并同时进行环境地图的建立； 无人机平台用于搭载以上设备进行飞行实验验证。

所述的算法运行步骤如下：

1)视觉里程计：视觉里程计部分采用基于多状态卡尔曼滤波MSCKF(Multi-StateConstraint Kalman Filter，)的算法，将惯性测量单元IMU(Inertial Measurement Unit)预积分 结果作为预测值，同时接收双目相机的原始数据，从相机的图像中提取特征点并进行特征匹 配，然后构建观测方程更新预测值，得到高频的无人机位姿、速度信息；

2)视觉雷达松耦合里程计：首先将MSCKF视觉里程计的高频的无人机位姿信息作为先 验，对当前扫描的雷达点云做去畸变处理，然后从去畸变的点云中提出特征点并与点云地图 做特征匹配，构建出非线性优化问题，将MSCKF预测值作为优化初值，求解无人机位姿， 最终将高频的MSCKF估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状 态信息，同时逐步拼接点云地图；

3)建立占据体素地图：基于循环缓冲区的建图策略，将点云地图转化为以无人机位置为 中心的局部占据体素地图，占据体素地图的建立与更新通过循环缓冲区进行维护。

本发明的特点及有益效果是：

本发明对无人机自主定位与建图领域的算法研究具有十分重要的意义。本发明稳定可靠， 可直接作为无人机自主感知实验平台，进而解决无人机在无GPS信号下自主飞行，路径规划 以及避障问题，具有很高的理论与实用价值。本发明主要具有以下特点和优点：

(1)本发明提出的基于视觉雷达融合的无人机自主定位方法结合了基于视觉与基于雷达 的两类自主定位方法的优点，定位精度高且运算速度快，实时性高，占用计算机资源少。传 统的基于视觉的定位方法受光线影响强烈，鲁棒性差。传统的激光雷达定位方法采用ICP (Iterative Closest Point)就近点迭代算法进行扫描匹配，ICP算法不基于特征，对所有点云 进行最近点匹配，算法计算量过大，且在先验信息不准确的情况下算法不收敛。本发明提出 的视觉里程计与基于特征的雷达里程计松耦合的策略，结合了二者的优点，大大提高了自主 定位与建图算法的精度与鲁棒性。

(2)本发明提出了用高频视觉里程计信息修正雷达点云畸变再从点云中提取特征的策略， 解决雷达点云在无人机高速运动时的畸变问题，有效提高了定位算法在无人机等快速移动物 体上的精度。

(3)本发明提出了基于一种循环缓冲区的占据体素地图建立方法。能够快速高效地将雷 达扫描点云转化为以无人机为中心的占据体素地图，在无人机导航与避障能够发挥巨大作用。

(4)本发明搭建的无人机自主定位平台可扩展性强，除了相机、激光雷达和IMU传感 器设备之外，还可根据开发者需要自行添加传感器设备，可以进行二次开发。

附图说明：

附图1视觉雷达松耦合里程计姿态估计对比图。

附图2自主定位与建图算法结构框图。

附图3MSCKF视觉里程计结构框图。

附图4视觉雷达松耦合里程计位置估计对比图。

附图5走廊环境自主定位与建图实验效果图。

附图6室外环境自主定位与建图实验效果图。

附图7室外占据体素地图效果图。

具体实施方式

本发明提出的基于视觉雷达融合的无人机自主定位与建图系统，主要由硬件部分与算法 部分组成。

硬件部分包括双目相机、3D激光雷达、IMU惯性测量单元、嵌入式机载处理器和无人 机平台。双目相机采集周围环境的视觉信息，测量频率高，信息丰富。3D激光雷达主要用于 测量无人机周围环境的3维位置信息，测量精度高，覆盖范围大，能够将环境信息以3D点云数据形式发布出来；IMU惯性测量单元用于高频测量无人机自身加速度与角速度信息；嵌入式机载处理器主要用于算法运行，通过处理相机、雷达与IMU采集到的数据，运行自主定位与建图算法，获得无人机自身位姿并同时进行环境地图的建立；无人机平台用于搭载以上 设备进行飞行实验验证。

算法部分本发明利用多个传感器的信息计算无人机自身状态信息以及构建环境地图。本 发明提出的基于视觉雷达融合的无人机自主定位与建图算法由以下三个部分组成：

4)视觉里程计。视觉里程计部分采用基于MSCKF(Multi-State ConstraintKalman Filter， 多状态卡尔曼滤波)的算法，该算法将IMU预积分结果作为预测值，同时接收双目相机的原 始数据，从相机的图像中提取特征点并进行特征匹配，然后构建观测方程更新预测值，得到 高频的无人机位姿、速度等信息。

5)视觉雷达松耦合里程计。这一部分首先将MSCKF视觉里程计的高频位姿信息作为先 验，对当前扫描的雷达点云做去畸变处理。然后从去畸变的点云中提出特征点并与点云地图 做特征匹配，构建出非线性优化问题，将MSCKF估计值作为优化初值，求解无人机位姿， 最终将高频的MSCKF估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状 态信息，同时逐步拼接点云地图。

6)建立占据体素地图。因为点云地图无法直接用于无人机导航与避障，所以这一部分将 提出一种基于循环缓冲区的建图策略，将点云地图转化为以无人机位置为中心的局部占据体 素地图，占据体素地图的建立与更新通过循环缓冲区维护。

下面结合附图对本发明的基于视觉与雷达融合的无人机自主定位与建图给出详细说明。

无人机自主定位与建图系统总体结构图如附图1所示。针对无人机在无GPS信号下自主 定位与环境感知问题，本发明设计了一种基于视觉雷达融合的无人机自主定位与建图系统。 首先对系统的硬件构成进行详细的介绍，该系统的硬件组成包括四旋翼无人机、双目相机、 激光雷达、IMU惯性测量单元和嵌入式机载处理器。

考虑到该系统的可靠性与定位精度，需要具有高精度的传感器设备。对双目相机，本发 明采用

相机，该相机具有帧率高、曝光时间短，可外部硬件同步，重量轻、体积小等 诸多优点，该相机搭载到无人机上，使用USB3.0接口将视觉信息传给机载处理器进行处理。

对激光雷达设备，本发明采用

公司的VLP-16型号三维激光雷达。VLP-16测 量精度高，距离远。测量距离100米，垂直方向测量角度30°，分辨率为2°，水平方向测量角度范围为360°，测量频率10Hz，解析度为0.2°。雷达每次扫描获得16线数据，每条线包 含1800个测量点，每个测量点为三维位置信息。激光雷达将采集到的点云数据通过网口发给机载处理器进行处理。

对IMU，IMU(Inertial Measurement Unit，惯性测量单元)是一种直接测量所在机体加速度 与角速度的传感器。本发明采用

公司的MTi-300型号IMU。这是一款基于微型惯性 传感技术的工业级IMU，该传感器以1000Hz频率发布原生的角速率与线加速度信息。采用 USB3.0接口直接将传感器信息传给机载处理器进行处理。

对于嵌入式机载处理器，考虑到需要处理大量数据且能够搭载于无人机上。需选择性能 优异、轻量化的嵌入式开发平台，本发明采用英特尔第七代NUC(Next Unit ofComputing， 微型计算单元)作为机载处理器。NUC是英特尔推出的最小体积电脑，它搭载第七代智能

酷睿^TM i7处理器，32G内存，这些使它成为适合嵌入式机载处理器的优良系统平台。

对四旋翼无人机平台，本发明采用碳纤维机架、Pixhawk飞行器控制器和大疆E300电机 电调组装成的四旋翼无人机。该无人机搭载性能强，续航时间长，可搭载多种类型的传感器 以进行相关研究。且Pixhawk作为一款开源飞控，提供了相当丰富的软件接口，研究者可根 据自身需求进行相应开发，可移植性强，极大算短了无人机飞行控制的研发周期。

接下来对本发明的软件依赖及算法结构进行介绍。本发明定位与建图算法都是在Linux 操作系统下基于ROS(Robot Operating System，机器人操作系统)进行开发。嵌入式机载处 理器内嵌Linux操作系统，在ROS系统环境下接受相机、雷达和IMU传感器采集的数据， 通过本发明设计的自主定位与建图算法进行处理，以实现无人机实时自主定位与环境感知。 其中，关键算法均采用C++语言编写。

自主定位与建图算法流程结构框图如附图2所示。本发明设计的基于视觉雷达融合的无 人机自主定位与建图算法主要包含3个步骤：

第一步：视觉里程计，采用MSCKF视觉里程计。MSCKF(Multi-State ConstraintKalman Filter，多状态约束卡尔曼滤波器)是一种改进的卡尔曼滤波器，其最大的特点是利用特征观测 对多个状态的约束更新状态变量。MSCKF视觉里程计算法分为两部分，第一部分是对IMU 与相机的原始信息的数据处理。首先算法从双目相机图像上做OrientedFast(有向快速)特 征点提取，Oriented Fast特征点计算机视觉领域经典的图像特征，最大的优点是提取速度很 快，适合在无人机等实时性要求较高的平台使用，而且具有尺度不变性、旋转不变性等优点。 特征提取完成后基于灰度不变假设，同一个特征点在不同图像帧上是相同的，所以算法搜索 匹配当前得到的特征点与之前图像上已经提取到的特征点，搜索过程使用IMU预积分得到的 对应相机帧的位姿变换加速搜索过程，以上过程称为特征跟踪，这一部分得到匹配成功的特 征点的像素位置作为滤波算法里的观测值输出到第二部分；

第二部分是多状态卡尔曼滤波模型。首先定义待估计的状态，MSCKF取连续20帧的无 人机状态，将其中无人机状态量的误差作为待估计的状态量，误差的定义是无人机状态的真 值和估计值之差,定义为

其中B_i,i＝1,2,…20代表第i帧相机坐标系，

为IMU误差状态，表达式

其中

分别代表IMU 估计的无人机位置、速度，

代表四元数误差的三维近似，

与

分别代表IMU加速度 计与陀螺仪的误差状态。相机每帧位姿的误差

其中

代表相机姿态 的误差状态，

代表相机位置的误差状态；

接下来介绍多状态卡尔曼滤波模型。多状态卡尔曼滤波模型分为预测、观测更新两步。 预测步骤使用IMU预积分计算无人机各项状态的预测值，具体步骤如下。首先不考虑噪声时 IMU状态的动力学微分方程如下：

其中

分别代表对应IMU位置、速度、姿态、加速度计与陀螺仪 偏差状态。

和

a_m是IMU测量的加速度，w_m是IMU测量的角速度。

代表四元数乘法，

代表四元数转旋转矩阵。当考虑噪声与偏差的随机游走模型 时，得IMU误差状态的动力学方程如下：

其中F是误差状态的状态转移矩 阵，G是IMU噪声的输入矩阵，n_I是IMU噪声矩阵，

分别是加速度 计噪声方差，陀螺仪噪声方差，加速度计偏差噪声方差，陀螺仪偏差噪声方差。设当前帧为 k帧，当k+1帧到来时，使用中值积分离散化IMU动力学方程来更新的IMU状态与IMU误 差状态：

Θ_k是IMU误差状态转移矩阵。IMU误差状态的协方差矩阵

以如下方式传播：

其中

是k帧IMU误差状态协方差矩阵，Q是IMU噪声的协方差矩阵，Q_k代表预测模型的噪声协方差矩阵。然后将

增广到全状态协方差矩阵P_k+1|k，P_k+1|k如下：

其中

均为

与

间的协方差矩阵，

为

的协方差矩阵。再来看相机 的位置预测值

和姿态预测值

因为IMU与相机之间有姿态外参^Bq_I与位置外参 ^Bp_I，所以

和

有如下表示：

到此基于IMU预积分的预测步骤完成。

接下来开始观测更新步骤。首先根据特征跟踪的结果，观测到的特征点f_j在第i帧图像 上的归一化坐标

为图像测量的噪声，

为f_j在 第i帧相机坐标下的位置。设

为根据预测环节计算出的f_j的坐标，由此得到观测方程的残 差

的表达式：

其中

是f_j在世界系下的坐标，其中

代表相机的位置，

代表世界系到相机系 的姿态。将残差对相机帧的位姿的误差及特征点的位置误差取一阶线性化展开得

其中

与

分别为残差相对相机位姿与特征点位置的雅克比矩 阵。累计起来的残差

其中

与n^j分别是累积形式的雅克比矩 阵与噪声矩阵，

分别是累计的无人机状态误差与特征点位置误差。该残差形式不符 合卡尔曼滤波的表达形式，所以我们定义残差在

的左零空间的映射

得到了符合卡尔 曼滤波模型的残差形式：

其中

是包含所有观测残差的雅克比矩阵，n是重新映射后的噪声矩阵。为了恢复左 零映射导致的残差维度变化，对

做正交三角分解，得

[M₁ M₂] 是正交矩阵，T_H是上三角矩阵，得最终的观测方程

最后根据传统的卡尔曼滤波模型计算卡尔曼增益以及更新预测值和协方差矩阵：

其中R_n是噪声

的协方差矩阵，K是计算得到的卡尔曼增益。至此，视觉里程计完 成了对无人机状态的估计，因为有IMU的使用，视觉里程计得到了100hz的高频无人机位姿 信息。

第二步：视觉雷达松耦合里程计，在这一部分本发明首先接受雷达点云信息，并使用视 觉里程计的100Hz的无人机位姿修正点云畸变，点云扫描周期为0.1s，平均有10个MSCKF 输出的信息，设一段时间内MSCKF位姿变换到雷达坐标下的变换矩阵

其对应的欧拉角与位移的表示方式为

二者可以相互转换，假设当前帧点 云扫描结束时刻t_k位于5和6之间。设t_k时刻的位姿变换矩阵为

做线性插值得

对于当前点云帧里的每一个点p_i ^B，时间戳为t_i，采取相同的线性插值的方法获取t_i时刻 的位姿

再计算

与

间的姿态变化

得到

后，将p_i ^B变换到扫描结 束t_k时刻，这样当前帧点云坐标便全局统一到当前帧扫描结束时刻的雷达坐标下，去除了点 云畸变。

其次是从点云中提取特征，具体采用LOAM(Lidar Odometry And Mapping)雷达里程计的 方法，从点云中提取两种特征点，位于平坦区域的平面点和位于边缘或尖锐区域的边缘线点， 为了提取这两种点，对点云中每个点

定义平滑度c：

其中E代表

的数个邻域点。根据c的定义，如果点分布在平面上，c的值接近0；若点分布在边缘上，c的值很大。先计算每个点云的平滑度值，排序后取最大和最小的数十个点，即为特征点，在特征提取时为了避免特征点扎堆，规定一个特征点周围10个邻近点不能作为特征点，最终特征提取环节获得了边缘线点集

与平面点集

最后是特征匹配优化位姿，首先将

和

利用

变换到世界坐标系下，在当前位置 200m范围的点云地图P_G上搜索与

和

匹配的特征线与面，并计算当前特征点到各自匹配 的特征线、面的距离。定义当前线、面特征

和

里的任一点

到P_G中的匹配特征距离：

其中，d_L、d_Π为当前特征点到匹配的线、面的距离，下面统称为d。点p_(k-1,j)，p_(k-1,l)，p_(k-1,m)为在P_G内距离点

最近的点，这些点张成与

匹配的线与面，这样得到两个以距离作为残差，以位姿作为状态变量的非线性函数。将其转化为非线性最小二乘问题，以MSCKF估计的位姿为先验值，使优化目标函数d趋近0,采用高斯牛顿法进行位姿优化求解。求解过程如下，先将特征距离函数写为

设状态的增量为

做一阶泰勒展开得

其中J是函数f()相对于位姿的雅克比矩阵，根据 高斯牛顿方法可得要求解的

有以下方程

经过这样数次迭代，最终优 化求解出了更精确的位姿T_k。这步优化以5Hz低频进行，最后一步是位姿融合，每次得到T_k后计算

接下来MSCKF输出位姿时，都左乘该△T_k，直到下一次雷达点云匹配 优化更新△T_k；

第三步：建立占据体素地图，将点云地图转化为一种基于循环数组的占据体素地图，建 立占据体素地图有分为两个部分。第一步将3D空间体素化并建立循环数组。首先按照分辨 率r将三维空间划为若干大小一致的立方体称为体素，并对每个体素建立索引index，index为 三维向量index＝[xⁱⁿ,yⁱⁿ,zⁱⁿ]^T，对于空间中任一点p_o，其对应的index表达式为

其中floor()代表向下取整，这样建立起了空间点与体素索引间的映射，维护一个以当前 无人机位置为中心，边长为N个体素长度的局部立方体空间，具体方法为维护一个包含长度 为N³的数组以及局部地图的起始索引O的循环数组，当无人机移动时，只需要改变O和更新 数组中更新变化的体素，判断体素是否在局部空间及计算体素索引τ在循环数组中位置的方 法inVolume(τ_i)定义如下：

inVolume(τ_i)＝0≤τ_i-O_i≤N,i∈{x,y,z} (13)

其中τ_i是τ的任意一维。而τ在数组内的位置l_τ用以下公式计算：

其中address(τ)_i代表体素τ在维度i上的索引，mod代表取余。运用以上公式，当无人机 移动时，更新的体素索引会对数组循环赋值。以上建立了体素索引与循环数组索引的映射， 就可以通过更新循环缓冲区维护局部地图，到此建图的第一步完成；

第二部分是对初始化完成的体素地图进行插值，插值指对每个体素计算占据概率并赋予 状态，具体的插值过程如下：

(1)遍历所有位于局部空间的点云，计算对应体素的占据概率，大于0.8时体素为占据 状态，代表无人机无法通过

(2)对局部空间以外的点，在局部空间内找到距离该点最近的体素索引，计算对应占据 概率，小于0.2即设为空闲状态，代表无人机可以通过；

(3)遍历已有的占据体素，向无人机所在位置投射射线，射线经过的体素都设为空闲；

在插值时，每个体素的占据概率是综合当前和历史所有观测结果得到的，占据概率 P(n|z_1:t)计算公式如下：

其中n代表当前观测的体素，T＝{1,...t}时刻，观测的数据为{z₁,...z_t}，该公式表明当前 体素的占据概率与当前观测结果z_t，先验概率P(n)和之前估计的概率P(n|z_1:t-1)有关；

使用logit(对数)变换将P(n|z_1:t)变换到全体实数空间L(n|z_1:t)，logit变换公式为

log()代表对数运算。那么占据概率的计算公式的logit表达为 L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)。可见占据概率的计算完全变成了简单的加法，最后利用逆logit 变换得到真实的占据概率

exp()代表指数运算；到此插值部分 完成，以无人机位置为中心的占据体素地图建立完成。

Claims (3)

1.一种视觉传感器激光雷达融合无人机定位与建图方法，其特征是，步骤如下：

1)视觉里程计：视觉里程计部分采用基于多状态卡尔曼滤波MSCKF(Multi-StateConstraint Kalman Filter)的算法，将惯性测量单元IMU(Inertial Measurement Unit)预积分结果作为预测值，同时接收双目相机的原始数据，从相机的图像中提取特征点并进行特征匹配，然后构建观测方程更新预测值，得到高频的无人机位姿、速度信息；

2)视觉雷达松耦合里程计：首先将MSCKF视觉里程计的高频的无人机位姿信息作为先验，对当前扫描的雷达点云做去畸变处理，然后从去畸变的点云中提出特征点并与点云地图做特征匹配，构建出非线性优化问题，将MSCKF预测值作为优化初值，求解无人机位姿，最终将高频的MSCKF估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状态信息，同时逐步拼接点云地图；

3)建立占据体素地图：基于循环缓冲区的建图策略，将点云地图转化为以无人机位置为中心的局部占据体素地图，占据体素地图的建立与更新通过循环缓冲区进行维护。

2.如权利要求1所述的视觉传感器激光雷达融合无人机定位与建图方法，其特征是，具体步骤如下：

第一步：视觉里程计，采用MSCKF视觉里程计，MSCKF视觉里程计算法分为两部分，第一部分是对IMU与相机的原始信息的数据处理，首先算法从双目相机图像上做有向快速Oriented Fast特征点提取，Oriented Fast特征点是计算机视觉领域的图像特征点，特征提取完成后基于灰度不变假设，同一个特征点在不同图像帧上是相同的，所以算法搜索匹配当前得到的特征点与之前图像上已经提取到的特征点，搜索过程使用IMU预积分得到的对应相机帧的位姿变换加速搜索过程，以上过程称为特征跟踪，这一部分得到匹配成功的特征点的像素位置作为滤波算法里的观测值输出到第二部分；

第二部分是多状态卡尔曼滤波模型：首先定义待估计的状态，MSCKF取连续20帧的无人机状态，将其中无人机状态量的误差作为待估计的状态量，误差的定义是无人机状态的真值和估计值之差,定义为

其中B_i,i＝1,2,…20代表第i帧相机坐标系，

为IMU误差状态，表达式

其中

分别代表IMU估计的无人机位置、速度误差状态，

代表四元数误差的三维近似，

与

分别代表IMU加速度计与陀螺仪的误差状态，相机每帧位姿的误差

其中

代表相机姿态的误差状态，

代表相机位置的误差状态；

多状态卡尔曼滤波模型分为预测、观测更新两步：预测步骤使用IMU预积分计算无人机各项状态的预测值，具体步骤如下：首先不考虑噪声时IMU状态的动力学微分方程如下：

其中

分别代表对应IMU位置、速度、姿态、加速度计与陀螺仪偏差状态，

和

a_m是IMU测量的加速度，w_m是IMU测量的角速度，

代表四元数乘法，

代表四元数转旋转矩阵，当考虑噪声与偏差的随机游走模型时，得IMU误差状态的动力学方程如下：

其中F是误差状态的状态转移矩阵，G是IMU噪声的输入矩阵，n_I是IMU噪声矩阵，

分别是加速度计噪声方差，陀螺仪噪声方差，加速度计偏差噪声方差，陀螺仪偏差噪声方差，设当前帧为k帧，当k+1帧到来时，使用中值积分离散化IMU动力学方程来更新的IMU状态与IMU误差状态：

Θ_k是IMU误差状态转移矩阵，IMU误差状态的协方差矩阵

以如下方式传播：

其中

是k帧时刻IMU误差状态协方差矩阵，Q是IMU噪声的协方差矩阵，Q_k代表预测模型的噪声协方差矩阵，然后将

增广到全状态P_k+1|k，P_k+1|k如下：

其中

均为

与

间的协方差矩阵，

为

的协方差矩阵，再来看相机的位置预测值

和姿态预测值

因为IMU与相机之间有姿态外参^Bq_I与位置外参^Bp_I，所以

和

有如下表示：

到此基于IMU预积分的预测步骤完成；

接下来开始观测更新步骤：首先根据特征跟踪的结果，观测到的特征点f_j在第i帧图像上的归一化坐标

为图像测量的噪声，

为f_j在第i帧相机坐标下的位置，设

为根据预测环节计算出的f_j的坐标，由此得到观测方程的残差

的表达式：

其中

是f_j在世界系下的坐标，

代表相机的位置，

代表世界系到相机系的姿态，将残差对相机帧的位姿的误差及特征点的位置误差取一阶线性化展开得

其中

与

分别为残差相对相机位姿与特征点位置的雅克比矩阵，同时计算多帧的残差，累计起来的残差

其中

与n^j分别是累积形式的雅克比矩阵与噪声矩阵，

分别是累计的无人机状态误差与特征点位置误差，该残差形式不符合卡尔曼滤波的表达形式，所以定义残差在

的左零空间的映射

得到了符合卡尔曼滤波模型的残差形式：

其中

是包含所有观测残差的雅克比矩阵，n是重新映射后的噪声矩阵，为了恢复左零映射导致的残差维度变化，对

做正交三角分解，得

[M₁ M₂]是正交矩阵，T_H是上三角矩阵，得最终的观测方程

最后根据传统的卡尔曼滤波模型计算卡尔曼增益以及更新预测值和协方差矩阵：

P_k+1|k+1＝(I-KT_H)P_k+1|k(I-KT_H)^T+KR_nK^T (8)

其中R_n是噪声

的协方差矩阵，K是计算得到的卡尔曼增益，至此，视觉里程计完成了对无人机状态的估计，因为有IMU的使用，视觉里程计得到高频无人机位姿信息；

第二步：视觉雷达松耦合里程计，在这一部分首先接受雷达点云信息，并使用视觉里程计的无人机位姿修正点云畸变，平均有10个MSCKF输出的信息，设一段时间内MSCKF位姿变换到雷达坐标下的变换矩阵

其对应的欧拉角与位移的表示方式为

二者可以相互转换，假设当前帧点云扫描结束时刻t_k位于5和6之间，设t_k时刻的位姿变换矩阵为

做线性插值得

对于当前点云帧里的每一个点

时间戳为t_i，采取相同的线性插值的方法获取t_i时刻的位姿

再计算

与

间的姿态变化

得到

后，将

变换到扫描结束t_k时刻，得到了

这样当前帧点云坐标便全局统一到当前帧扫描结束时刻的雷达坐标下，去除了点云畸变；

其次是从点云中提取特征，具体采用LOAM(Lidar Odometry And Mapping)雷达里程计的方法，从点云中提取两种特征点，位于平坦区域的平面点和位于边缘或尖锐区域的边缘线点，为了提取这两种点，对点云中每个点

定义平滑度c：

其中Ε代表

的数个邻域点，根据c的定义，如果点分布在平面上，c的值接近0；若点分布在边缘上，c的值很大，先计算每个点云的平滑度值，排序后取最大和最小的数十个点，即为特征点，在特征提取时为了避免特征点扎堆，规定一个特征点周围10个邻近点不能作为特征点，最终特征提取环节获得了边缘线点集

与平面点集

最后是特征匹配优化位姿，首先将

和

利用

变换到世界坐标系下，在当前位置200m范围的点云地图P_G上搜索与

和

匹配的特征线与面并计算当前特征点到各自匹配的特征线、面的距离，定义当前线、面特征

和

里的任一点

到P_G中的匹配特征距离：

其中，d_L、d_Π为当前特征点到匹配的线、面的距离，下面统称为d，点p_(k-1,j)，p_(k-1,l)，p_(k-1,m)为在P_G中距离点

最近的点，这些点张成与

匹配的线与面，这样得到两个以距离作为残差，以位姿作为状态变量的非线性函数，将其转化为非线性最小二乘问题，以MSCKF估计的位姿为先验值，使优化目标函数d近0,采用高斯牛顿法进行位姿优化求解，求解过程如下，先将特征距离函数写为

设状态的增量为

做一阶泰勒展开得

其中J是函数f()相对于位姿的雅克比矩阵，根据高斯牛顿方法可得要求解的

有以下方程

经过这样数次迭代，最终优化求解出了更精确的位姿T_k，这步优化以5Hz低频进行，最后一步是位姿融合，每次得到T_k后计算

接下来MSCKF输出位姿时，都左乘该ΔT_k，直到下一次雷达点云匹配优化更新ΔT_k；

第三步：建立占据体素地图，将点云地图转化为一种基于循环数组的占据体素地图，建立占据体素地图有分为两个部分，第一步将3D空间体素化并建立循环数组，首先按照分辨率r将三维空间划为若干大小一致的立方体称为体素，并对每个体素建立索引index，index为三维向量index＝[xⁱⁿ,yⁱⁿ,zⁱⁿ]^T，对于空间中任一点p_o，其对应的index表达式为

其中floor()代表向下取整，这样建立起了空间点与体素索引间的映射，维护一个以当前无人机位置为中心，边长为N个体素长度的局部立方体空间，具体方法为维护一个包含长度为N³的数组以及局部地图的起始索引O的循环数组，当无人机移动时，只需要改变O和更新数组中更新变化的体素，判断体素是否在局部空间及计算体素索引τ在循环数组中位置的方法inVolume(τ_i)定义如下：

inVolume(τ_i)＝0≤τ_i-O_i≤N,i∈{x,y,z} (13)

其中τ_i是τ的任意一维，而τ在数组内的位置l_τ用以下公式计算：

address(τ)_i＝(τ_i-O)mod N,i∈{x,y,z}

l_τ＝N²address(τ)_x+Naddress(τ)_y+address(τ)_z (14)

其中address(τ)_i代表体素τ在维度i上的索引，mod代表取余，运用以上公式，当无人机移动时，更新的体素索引会对数组循环赋值，以上建立了体素索引与循环数组索引的映射，就可以通过更新循环缓冲区维护局部地图，到此建图的第一步完成；

第二部分是对初始化完成的体素地图进行插值，插值指对每个体素计算占据概率并赋予状态，具体的插值过程如下：

(1)遍历所有位于局部空间的点云，计算对应体素的占据概率，大于0.8时体素为占据状态，代表无人机无法通过

(2)对局部空间以外的点，在局部空间内找到距离该点最近的体素索引，计算对应占据概率，小于0.2即设为空闲状态，代表无人机可以通过；

(3)遍历已有的占据体素，向无人机所在位置投射射线，射线经过的体素都设为空闲；

在插值时，每个体素的占据概率是综合当前和历史所有观测结果得到的，占据概率P(n|z_1:t)计算公式如下：

其中n代表当前观测的体素，T＝{1,...t}时刻，观测的数据为{z₁,...z_t}，该公式表明当前体素的占据概率与当前观测结果z_t，先验概率P(n)和之前估计的概率P(n|z_1:t-1)有关；

使用对数logit变换将P(n|z_1:t)变换到全体实数空间L(n|z_1:t)，logit变换公式为

其中log()代表对数运算，那么占据概率的计算公式的logit表达为L(n|z_1:t)＝L(n|z_1:t-1)+L(n|z_t)，可见占据概率的计算完全变成了简单的加法，最后利用逆logit变换得到真实的占据概率

其中exp()代表指数运算；到此插值部分完成，以无人机位置为中心的占据体素地图建立完成。

3.一种视觉传感器激光雷达融合无人机定位与建图装置，其特征是，包括双目相机、3D激光雷达、IMU惯性测量单元、嵌入式机载处理器和无人机平台；双目相机采集周围环境的视觉信息；3D激光雷达用于测量无人机周围环境的3维位置信息；IMU惯性测量单元用于高频测量无人机自身加速度与角速度信息；嵌入式机载处理器用于算法运行，通过处理相机、雷达与IMU采集到的数据，运行自主定位与建图算法，获得无人机自身位姿并同时进行环境地图的建立；无人机平台用于搭载以上设备进行飞行实验验证；所述的算法运行步骤如下：

1)视觉里程计：视觉里程计部分采用基于多状态卡尔曼滤波MSCKF(Multi-StateConstraint Kalman Filter，)的算法，将惯性测量单元IMU(Inertial Measurement Unit)预积分结果作为预测值，同时接收双目相机的原始数据，从相机的图像中提取特征点并进行特征匹配，然后构建观测方程更新预测值，得到高频的无人机位姿、速度信息；

2)视觉雷达松耦合里程计：首先将MSCKF视觉里程计的高频的无人机位姿信息作为先验，对当前扫描的雷达点云做去畸变处理，然后从去畸变的点云中提出特征点并与点云地图做特征匹配，构建出非线性优化问题，将MSCKF预测值作为优化初值，求解无人机位姿，最终将高频的MSCKF估计与低频的松耦合里程计的结果融合，得到高频、准确的无人机状态信息，同时逐步拼接点云地图；

3)建立占据体素地图：基于循环缓冲区的建图策略，将点云地图转化为以无人机位置为中心的局部占据体素地图，占据体素地图的建立与更新通过循环缓冲区进行维护。

Images