CN109945856A - 基于惯性/雷达的无人机自主定位与建图方法 - Google Patents

Abstract

本发明涉及激光雷达传感器、无人机领域，为解决无人机在无GPS信息下的自主定位与环境感知问题，并同时能够实时建立精确的环境地图。本发明，基于惯性/雷达的无人机自主定位与建图方法，步骤如下：1)特征提取：提取点云数据中的线、面特征；2)雷达里程计：将两次雷达扫描之间的点到线与点到面的距离作为优化指标，采用L‑M方法进行特征匹配估计无人机位姿信息，同时再次对当前雷达扫描数据进行失真处理；3、建立环境地图：将上一步得到的未失真点云数据使用L‑M法与地图层进行精确匹配，得到更为精确的无人机位姿信息，建立精确三维地图；4、回环检测。发明主要应用于无人机设计制造场合。

Description

基于惯性/雷达的无人机自主定位与建图方法

技术领域

本发明涉及激光雷达传感器领域、嵌入式系统领域、无人机领域，尤其涉及一种基于惯性/激光雷达的无人机感知系统，解决了在无GPS信号下基于惯性/雷达的无人机自主定位与环境感知问题。

背景技术

无人机的自主飞行技术是近年来研究的极大热点之一，尤其是旋翼式无人机在民用和军用领域都有着广泛的应用前景。无人机控制技术的越来越成熟，其定位与环境感知问题也得到关注，尤其是随着无人机应用场景的复杂化，无人机通过多传感器的自主定位与环境感知能力显得尤为关键。

无人机自主定位主要解决无人机在未知复杂环境下能够自主定位与环境感知两大问题。其中，自主定位是无人机完成自主飞行的基础和关键，环境感知能力是无人机实现自主规划导航、避障的必要能力。传统的无人机定位方法是采用GPS、北斗等全球卫星导航定位系统，但其定位精度受卫星信号强弱影响，在室内、隧道、楼体间信号不稳定，无法实时提供准确的定位信息。室内我们还可以采用OptiTrack运动捕捉系统进行定位，其定位精度达到毫米级别，但成本过高且仅限于在OptiTrack设备测量范围内。因此，为了解决无GPS信号下无人机自主定位与环境感知问题，我们常采用激光雷达、单双目摄像头、RGB-D摄像头和IMU(Inertial Measurement Unit)惯性测量单元等多传感器设备进行无人机自主定位与建图。

针对无人机自主定位与建图问题，国内外各大高校都进行了深入的研究。目前，美国麻省理工大学、宾夕法尼亚大学的GRASP实验室、卡耐基梅隆的Ji Zhang实验室、瑞士苏黎世联邦理工大学、新加坡国立大学等国家的科研团队都采用视觉或激光雷达设备进行了相关研究，并取得了一定成果；国内的清华大学、北京航空航天大学、哈尔滨工业大学、天津大学等也开展了相关研究。由此可见无人机在复杂环境下的自主定位与建图是目前国际无人机领域的研究热点之一，因此，设计一套精度高、实时性强、可扩展性强的无人机自主定位与建图系统具有十分重要的研究价值和应用意义。

发明内容

为克服现有技术的不足，本发明旨在解决无人机在无GPS信息下的自主定位与环境感知问题，并同时能够实时建立精确的环境地图。本发明提出了一种基于激光雷达和IMU惯性测量单元的无人机自主定位与建图方法，通过设计无人自主定位平台并运行相应定位与建图算法，实现无人机快速自主定位及对周围环境的精确感知。本发明有利于其他无人机自主定位算法在该平台上进行验证，有限提高了无人机自主定位的研究效率，具有极大的应用价值。本发明采取的技术方案是，基于惯性/雷达的无人机自主定位与建图方法，步骤如下：

1、特征提取：在特征提取部分，通过接收激光雷达和IMU测量数据，利用IMU测量的机体姿态信息以及雷达点云数据进行点云预注册，处理由无人机快速运动而产生的点云失真问题，利用点云预注册数据采用基于PCA(principal Component Analysis)主成分分析的特征提取方法，提取点云数据中的线、面特征；

2、雷达里程计：在雷达里程计部分，利用较高频率的特征匹配来估计无人机的位姿，根据上一步提取的线、面特征，将两次雷达扫描之间的点到线与点到面的距离作为优化指标，构建非线性最小二乘问题，采用L--M(Levenberg-Marquardt)方法进行特征匹配估计无人机位姿信息，同时再次对当前雷达扫描数据进行失真处理；

3、建立环境地图：在建图部分，以较低的频率基于地图层的精确匹配和点云注册地图，将上一步得到的未失真点云数据使用L-M法与地图层进行精确匹配，得到更为精确的无人机位姿信息，建立精确三维地图；

4、回环检测：在该部分通过提取关键帧做pose--graph位姿图优化，当无人机回到曾经飞过的历史点附近时，采用位姿图优化结合特征匹配策略进行回环检测，修正累积偏差。

第一步：特征提取具体采用IMU估计的姿态信息作为先验值预先进行点云注册，再将点云预注册信息转化为深度图像，即生成16行、1800列的深度像素图，计算深度图每个点p_i的法向量n_i，法向量n_i通过对深度图像进行主成分分析PCA得到，通过计算点p_i特定邻域的高斯分布N_i(μ_i,Σ_i)的协方差矩阵得到的，对应着该协方差矩阵的最小特征值的特征向量，即计算每一个点p_i的均值μ_i与方差Σ_i：

ν_i为点p_i邻域中所有的点，其中，采用基于深度-边界限制的自适应选择邻域大小，同时计算协方差矩阵时采用积分图像，得到每一个点的高斯分布N_i(μ_i,Σ_i)时，计算其协方差矩阵的特征值与特征向量：

至此，得到每个点的距离和法向量n_i，采用基于区域增长的聚类算法进行分割，具体是增量式的检测种子点的邻域，基于距离和法向量信息判断是否将该邻域点加入当前类，即邻域点p_i相对于种子点p_s需满足：

||p_s-p_i||<ε_d,

n_s·n_i＞ε_n (3)

一旦所有的点集聚类完成，通过再次计算每一个聚类c_i的协方差矩阵得到其升序排列特征值和相应特征向量并根据每个聚类的特征值和特征向量进行线、面检测，通过PCA分析可知，线类L_i的特征值和要明显小于且在该拟合线L_i上点到线的距离之和e应该小于阈值ε_dl，具体约束如下：

平面类Π_i的特征值要明显小于和且在该拟合线Π_i上点到面的距离之和e应该小于阈值ε_dp，具体约束如下：

至此，得到的雷达点云数据中的线、面特征。

第二步：雷达里程计通过L-M法解算其位姿变换信息，得到无人机的位姿信息，将上一扫描的线、面特征信息记为和当前扫描提取的线、面特征记为和在做特征匹配之前还需做一个匀速运动计算，具体是雷达在当前扫描时间0.1s内是匀速运动的，定义t当前时间戳，t_k为当前时间戳，t_i为当前点p_i的时间戳，T_k为无人机在[t_k，t]内的位姿变换信息，根据匀速运动假设得到[t_k，t_i]的位姿变换：

这样求取t_i时刻特征点的信息定义当前扫线、面特征上的点到上一扫描的线面的距离：

其中，d_L、d_Π为点到线、点到面的距离，点jlm为点在上一扫描点云数据中的最近点。这样得到两个距离信息的非线性函数，将其转化为非线性最小二乘问题，优化目标函数d趋近于0,采用L-M法进行位姿求解；

第三步：建图部分。我们将当前扫描结束后精确的线面特征与之前建立好的地图Q^k-1进行一次更为精确的匹配，利用更精确的匹配信息进行点云注册，建立精确的环境地图；

第四步：回环检测每隔一定的距离设立一个关键帧，将关键帧的位姿信息加入pose-graph位姿图优化作为一个新的位姿节点，进行位姿图优化，使得求解位姿信息更加平滑，并将该关键帧的特征线、面信息保存下来，同时，加入基于路径点的回环检测约束，即将无人机回到历史点一定范围内就当作回环检测的约束，然后将当前位置的线、面特征与相应历史点匹配，得到偏移位姿量，将该约束加入到pose-graph中，达到回环检测的效果。

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

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

(1)本发明提出的基于惯性/雷达的无人机自主定位方法实时性高，占用计算机资源少，运算速度快，定位精度高。传统的激光雷达定位方法采用ICP(Iterative ClosestPoint)就近点迭代算法进行扫描匹配，ICP算法不基于特征，仅基于就近点进行迭代匹配，算法不稳定、计算量大且可能会无法获取正确解。而本发明采用基于环境中线、面特征的匹配，有效改善了上述问题，大大减少了计算量并提高了算法精度。

(2)本发明提出了基于pose-graph的路径点的回环检测，解决了雷达自主定位与建图技术中难以加入回环检测的问题，大大减少了无人机自主定位在大型场景下的偏移量，有效提高了基于惯性/雷达的无人机自主定位与建图在大型场景下的应用。

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

(4)本发明中自主定位算法部分可根据使用者需要进行更换，还可以融入更多的传感器信息，如单双目相机、深度相机等，有利于自主定位与建图技术研究者对算法进行改进及验证。

附图说明：

附图1基于惯性/雷达的无人机自主定位与建图系统总体结构图。

附图2整体系统结构框图。

附图3特征提取模块结构框图。

附图4特征提取效果图。

附图5室内自主定位与建图实验效果图。

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

具体实施方式

本发明提出的基于惯性/雷达的无人机自主定位与建图系统，主要包括四个部分：3D激光雷达、IMU惯性测量单元、嵌入式机载处理器和无人机平台。3D激光雷达主要用于测量无人机周围环境的3维位置信息，测量精度高，覆盖范围大，能够以较高频率将环境信息以3D点云数据形式发布出来；IMU惯性测量单元用于高频测量无人机的姿态信息，由于受传感器本身特性限制，无人机长时间运动会导致IMU测量数据产生一定的偏移，故IMU测量值仅当作无人机姿态估计的先验值；嵌入式机载处理器主要用于算法运行，通过处理激光雷达点云数据和IMU测量值，运行自主定位算法，获得无人机自身位姿并同时进行环境地图的建立；无人机平台用于搭载以上设备进行飞行实验验证。

本发明采用Feature--Based LOAM(Feature-Based Lidar Odometry andMapping)基于特征的雷达里程计与建图技术来进行无人机实时位姿估计，提出了一种基于Feature--Based LOAM的无人机自主定位方法。Feature--Based LOAM是指无人机在移动过程中利用激光雷达和IMU数据提取周围环境的特征信息并进行匹配，得到准确的位姿估计和精确的环境地图，从而实现无人机的自主定位与地图构建。本发明提出的基于惯性/雷达的无人机自主定位与建图方法主要由以下四个步骤完成：

1、特征提取。在特征提取部分，通过接收激光雷达和IMU测量数据，利用IMU测量的机体姿态信息以及雷达点云数据进行点云预注册，处理由无人机快速运动而产生的点云失真问题。利用点云预注册数据设计了一种基于PCA(principal Component Analysis)主成分分析的特征提取方法，提取点云数据中的线、面特征。

2、雷达里程计。在雷达里程计部分，利用较高频率的特征匹配来估计无人机的位姿。根据上一步提取的线、面特征，将两次雷达扫描之间的点到线与点到面的距离作为优化指标，构建非线性最小二乘问题，采用L--M(Levenberg-Marquardt)方法进行特征匹配估计无人机位姿信息，同时再次对当前雷达扫描数据进行失真处理。

3、建立环境地图。在建图部分，以较低的频率基于地图层的精确匹配和点云注册地图。将上一步得到的未失真点云数据使用L-M法与地图层进行精确匹配，得到更为精确的无人机位姿信息，建立精确三维地图。

4、回环检测。在该部分通过提取关键帧做pose--graph位姿图优化，当无人机回到曾经飞过的历史点附近时，采用位姿图优化结合特征匹配策略进行回环检测，修正累积偏差。

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

无人机自主定位与建图系统总体结构图如附图1所示。针对无人机在无GPS信号下自主定位与环境感知问题，本发明设计了一种基于惯性/雷达的无人机自主定位与建图系统。首先对系统的硬件构成进行详细的介绍，该系统的硬件组成包括六旋翼无人机、激光雷达、IMU惯性测量单元和嵌入式机载处理器。考虑到该系统的可靠性与定位精度，需要具有高精度的传感器设备，其中激光雷达采用美国velodyne公司生产的高精度、轻量化的16线激光雷达velodyne VLP-16。VLP-16是市面上最小型的3维激光雷达，具有100米的远程测量距离，测量精度达到±3cm级别，垂直方向测量角度范围达到30°，分辨率为2°，水平方向测量角度范围为360°，分辨率为0.1°到0.4°，测量频率可调为5到20Hz，每秒高达30万个测量值。在本发明中，我们将测量频率调节至10Hz，解析度为0.2°，即雷达每次扫描获得16线数据，每条线包含1800个测量点，每个测量点为XYZ三维点信息。激光雷达将采集到垂直视场30°距离范围100米的点云数据通过网口发给嵌入式机载处理器进行处理。

对IMU惯性测量单元，本发明采用荷兰Xsens Technologies公司生产的Xsensmti300惯性测量单元，这是一款基于微型惯性传感技术的工业级IMU，该传感器采用其独特的传感器融合算法能够以400Hz高频解算出运动体的姿态信息，并同时以1000Hz频率发布原生的角速率与线加速度信息。采用USB3.0接口直接将传感器信息传给上位机进行处理。

对于嵌入式机载处理器，考虑到需要处理大量数据且能够搭载于无人机上。需选择性能优异、轻量化的嵌入式开发平台，本发明采用英特尔第七代NUC作为机载处理器。NUC是英特尔推出的最小体积电脑，它搭载第七代智能酷睿^TMi7处理器，32G高内存，这些使它成为适合嵌入式机载处理器的优良系统平台。

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

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

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

第一步：特征提取。本发明采用IMU估计的姿态信息作为先验值预先进行点云注册，再将点云预注册信息转化为深度图像，即生成16行、1800列的深度像素图。此步骤的关键在于计算深度图每个点p_i的法向量n_i，法向量n_i通过对深度图像进行PCA(主成分分析)得到，通过计算点p_i特定邻域的高斯分布N_i(μ_i,Σ_i)的协方差矩阵得到的，它对应着该协方差矩阵的最小特征值的特征向量。即计算每一个点p_i的均值μ_i与方差Σ_i：

ν_i为点p_i邻域中所有的点。其中，关键点在于如何选择合适邻域大小以及如何加速协方差矩阵计算的速度，本发明设计了一种基于深度-边界限制的自适应选择邻域大小，同时计算协方差矩阵时采用积分图像，大大加快了计算速度，避免了重复计算。一旦我们得到每一个点的高斯分布N_i(μ_i,Σ_i)时，我们可以计算其协方差矩阵的特征值与特征向量：

至此，我们得到每个点的距离和法向量n_i，本发明采用一种基于区域增长的聚类算法进行分割。该方法增量式的检测种子点的邻域，基于距离和法向量信息判断是否将该邻域点加入当前类。即邻域点p_i相对于种子点p_s需满足：

||p_s-p_i||<ε_d,

n_s·n_i＞ε_n (3)

一旦所有的点集聚类完成，我们通过再次计算每一个聚类c_i的协方差矩阵得到其特征值(升序排列)和相应特征向量并根据每个聚类的特征值和特征向量进行线、面检测。我们通过PCA分析可知，线类L_i的特征值和要明显小于且在该拟合线L_i上点到线的距离之和e应该小于阈值ε_dl，具体约束如下：

平面类Π_i的特征值要明显小于和且在该拟合线Π_i上点到面的距离之和e应该小于阈值ε_dp，具体约束如下：

至此，我们便得到的雷达点云数据中的线、面特征，特征提取模块的结构框图如附图3所示。提取的线、面特征效果如图4所示。

第二步：雷达里程计。在该部分中，本发明根据上一步提取雷达点云中的线、面特征信息进行帧与帧之间的匹配，并通过L--M法解算其位姿变换信息，得到无人机的位姿信息。将上一扫描的线、面特征信息记为和当前扫描提取的线、面特征记为和在做特征匹配之前我们还需做一个匀速运动假设，假设雷达在当前扫描时间0.1s内是匀速运动的。我们定义t当前时间戳，t_k为当前时间戳，t_i为当前点p_i的时间戳，T_k为无人机在[t_k，t]内的位姿变换信息，我们根据匀速运动假设得到[t_k，t_i]的位姿变换：

这样便可以求取t_i时刻特征点的信息我们定义当前扫线、面特征上的点到上一扫描的线面的距离：

其中，d_L、d_Π为点到线、点到面的距离，点jlm为点在上一扫描点云数据中的最近点。这样我们得到了两个距离信息的非线性函数，将其转化为非线性最小二乘问题，优化目标函数d趋近于0,采用L-M法进行位姿求解。

第三步：建图部分。我们将当前扫描结束后精确的线面特征与之前建立好的地图Q^k-1进行一次更为精确的匹配，利用更精确的匹配信息进行点云注册，建立精确的环境地图。

第四步：回环检测。在该部分，我们每隔一定的距离设立一个关键帧，将关键帧的位姿信息加入pose--graph位姿图优化作为一个新的位姿节点，进行位姿图优化，使得求解位姿信息更加平滑，并将该关键帧的特征线、面信息保存下来。同时，加入基于路径点的回环检测约束，即将无人机回到历史点一定范围内就当作回环检测的约束，然后将当前位置的线、面特征与相应历史点匹配，得到偏移位姿量，将该约束加入到pose--graph中，达到回环检测的效果。

无人机自主定位与建图室内效果如附图5所示。图中为利用本发明提出的基于雷达的无人机自主定位与建图方法进行的室内实验效果图，图中红线为里程计部分计算的实时位姿，橙色点为加入回环检测约束后纠正的路径点，可以看出回环检测对该系统具有很大的提升。飞行一圈大致产生1米左右的偏差。由于楼道内部分区域存在窗户，干扰激光雷达测距，会对实验效果产生一定的影响。

室外大型场景实验效果如附图6所示，图中绕教学楼飞行一圈，飞行距离大致为200米左右，可以看出在加入回环检测约束后偏移量为1.5米左右，验证了在大型场景下本发明设计方法的可靠性与高精度性。

Claims (2)

1.一种基于惯性/雷达的无人机自主定位与建图方法，其特征是，步骤如下：

1)特征提取：在特征提取部分，通过接收激光雷达和IMU测量数据，利用IMU测量的机体姿态信息以及雷达点云数据进行点云预注册，处理由无人机快速运动而产生的点云失真问题，利用点云预注册数据采用基于PCA(principal Component Analysis)主成分分析的特征提取方法，提取点云数据中的线、面特征；

2)雷达里程计：在雷达里程计部分，利用较高频率的特征匹配来估计无人机的位姿，根据上一步提取的线、面特征，将两次雷达扫描之间的点到线与点到面的距离作为优化指标，构建非线性最小二乘问题，采用L-M(Levenberg-Marquardt)方法进行特征匹配估计无人机位姿信息，同时再次对当前雷达扫描数据进行失真处理；

3)建立环境地图：在建图部分，以较低的频率基于地图层的精确匹配和点云注册地图，将上一步得到的未失真点云数据使用L-M法与地图层进行精确匹配，得到更为精确的无人机位姿信息，建立精确三维地图；

4)回环检测：在该部分通过提取关键帧做pose-graph位姿图优化，当无人机回到曾经飞过的历史点附近时，采用位姿图优化结合特征匹配策略进行回环检测，修正累积偏差。

2.如权利要求1所述的基于惯性/雷达的无人机自主定位与建图方法，其特征是，具体地：

第1)步：特征提取具体采用IMU估计的姿态信息作为先验值预先进行点云注册，再将点云预注册信息转化为深度图像，即生成16行、1800列的深度像素图，计算深度图每个点p_i的法向量n_i，法向量n_i通过对深度图像进行主成分分析PCA得到，通过计算点p_i特定邻域的高斯分布N_i(μ_i,Σ_i)的协方差矩阵得到的，对应着该协方差矩阵的最小特征值的特征向量，即计算每一个点p_i的均值μ_i与方差Σ_i：

ν_i为点p_i邻域中所有的点，其中，采用基于深度-边界限制的自适应选择邻域大小，同时计算协方差矩阵时采用积分图像，得到每一个点的高斯分布N_i(μ_i,Σ_i)时，计算其协方差矩阵的特征值与特征向量：

至此，得到每个点的距离和法向量n_i，采用基于区域增长的聚类算法进行分割，具体是增量式的检测种子点的邻域，基于距离和法向量信息判断是否将该邻域点加入当前类，即邻域点p_i相对于种子点p_s需满足：

||p_s-p_i||<ε_d,

n_s·n_i＞ε_n (3)

一旦所有的点集聚类完成，通过再次计算每一个聚类c_i的协方差矩阵得到其升序排列特征值和相应特征向量并根据每个聚类的特征值和特征向量进行线、面检测，通过PCA分析可知，线类L_i的特征值和要明显小于且在该拟合线L_i上点到线的距离之和e应该小于阈值ε_dl，具体约束如下：

平面类Π_i的特征值要明显小于和且在该拟合线Π_i上点到面的距离之和e应该小于阈值ε_dp，具体约束如下：

至此，得到的雷达点云数据中的线、面特征。

第2)步：雷达里程计通过L-M法解算其位姿变换信息，得到无人机的位姿信息，将上一扫描的线、面特征信息记为和当前扫描提取的线、面特征记为和在做特征匹配之前还需做一个匀速运动计算，具体是雷达在当前扫描时间0.1s内是匀速运动的，定义t当前时间戳，t_k为当前时间戳，t_i为当前点p_i的时间戳，T_k为无人机在[t_k，t]内的位姿变换信息，根据匀速运动假设得到[t_k，t_i]的位姿变换：

这样求取t_i时刻特征点的信息定义当前扫线、面特征上的点到上一扫描的线面的距离：

其中，d_L、d_Π为点到线、点到面的距离，点jlm为点在上一扫描点云数据中的最近点。这样得到两个距离信息的非线性函数，将其转化为非线性最小二乘问题，优化目标函数d趋近于0,采用L-M法进行位姿求解；

第3)步：建图部分。我们将当前扫描结束后精确的线面特征与之前建立好的地图Q^k-1进行一次更为精确的匹配，利用更精确的匹配信息进行点云注册，建立精确的环境地图；

第4)步：回环检测每隔一定的距离设立一个关键帧，将关键帧的位姿信息加入pose-graph位姿图优化作为一个新的位姿节点，进行位姿图优化，使得求解位姿信息更加平滑，并将该关键帧的特征线、面信息保存下来，同时，加入基于路径点的回环检测约束，即将无人机回到历史点一定范围内就当作回环检测的约束，然后将当前位置的线、面特征与相应历史点匹配，得到偏移位姿量，将该约束加入到pose-graph中，达到回环检测的效果。