月面环境三维激光SLAM技术

2021-03-27尚天祥王景川董凌峰陈卫东

航空学报 2021年1期

尚天祥，王景川,*，董凌峰，陈卫东

1. 上海交通大学电子信息与电气工程学院，上海 200240 2. 系统控制与信息处理教育部重点实验室，上海 200240

月球探索机器人[1]在无定位系统辅助的未知环境中执行任务，需要精确获取机器人的实时位姿。高可靠的定位性能为未知环境探索、局部导航避障提供了重要的依据。因此，研究在复杂月球表面环境中精确、快速得获取机器人的位姿尤为重要。

在无定位系统辅助的环境中进行定位主要依靠多传感融合，利用同步建图与定位(Simultaneous Localization And Mapping，SLAM)的策略进行实时定位[2]，月球探索机器人一般配备有深度相机，二维、三维激光雷达，里程计和惯性传感器等。地面环境中的激光或视觉同步建图与定位的算法如文献[3-5]都在自动驾驶上得到了很好的应用效果。文献[6]提出的基于多层ICP(Iterative Clostest Point)的匹配算法在室外巡检机器人SLAM中取得了不错的应用效果。

但是针对月球探索机器人来说，无辅助定位系统，表面陨坑[7]的存在影响移动机器人的正常行走，颠簸情况导致机器人的位置和姿态变化剧烈，组合导航系统可能失效。月球表面特征不显著、机器人的机载计算资源有限等问题都对同步建图与定位方法提出了挑战。同时，月球表面环境的光照变化极为剧烈，对视觉同步建图与定位来说是一个巨大的挑战，文献[8]提出了一个针对月面环境的视觉同步建图与定位算法。基于视觉的同步建图与定位很难全天候进行工作，但是激光雷达不受光照条件限制，且探测距离远、精度高[9]。文献[10-11]的研究基于扩展卡尔曼滤波的同步建图与定位，通过三维激光进行自然信标的跟踪，在模拟火星环境验证了有效性，但是月球表面难以捕捉稳定的特征信标，算法在计算实时性和精度上有待提高。文献[12]提出第一个完整的基于激光的同步建图与定位算法，在模拟月球的环境中验证了有效性，但是由于没有融合惯性传感器等其他元件，系统的精度还有待提升。文献[13]利用激光的强度信息和提取的地标提升了月球环境里程计的精度，但是目前仍依赖于人工反复测量的地标，在实际应用中存在一定的困难。

针对月球表面坑洼路面导致的机器人位姿剧烈变化的问题，通过扩展卡尔曼滤波[14](Extend-ed Kalman Filter，EKF)对轮式里程计和惯性传感器进行融合，实现位姿的粗略估计；月面环境地形起伏程度不一，基于三维激光的点云观测会存在部分特征不显著的冗余信息，本文提出一种显著特征点云提取算法，基于深度图和高度灰度图进行灰度熵计算和点云分割，对点云特征进行解析计算，提取特征显著的点云。针对位姿估计不确定情况下的位姿优化问题，提出了基于曲面定位能力估计的增量式优化方法，通过对组合导航位姿优化，实现有限计算资源下的高精度实时定位。最后，在物理仿真月面环境中开展测试，验证算法的有效性。

1 月面三维激光SLAM算法架构

算法整体架构如图1所示。在显著特征点云提取部分，根据点云映射的深度图、高度图进行点云分割，通过主成分分析解析点云块的特征值，基于特征值定义平面度和线性度，根据线性度和平面度筛选提取数据，从而获得显著特征点云。

同时，基于里程计和惯性传感器数据扩展卡尔曼滤波融合，在获得粗略位姿估计基础上，融合激光观测计算位姿估计的不确定性，将位姿估计的不确定性指标作为优化参数之一，利用增量式的优化算法实现高精度定位。

2 显著特征点云提取

月球表面环境特征不显著，仅存在起伏地形、凹陷以及大小不一的石块，缺乏平整地面、树木、建筑等地面环境特征。月表起伏程度不一致的地面和大小不一的石块包含着不同的特征信息量，基于三维激光进行观测，在点云中获取特征显著的数据，有利于进行数据融合实现高精度的定位。

基于月面环境特征均在地面附近的特点，选择三维激光倾斜向下安装。首先对点云数据进行旋转变化，使其平行于机器人本体质心。根据激光雷达的扫描模型，计算激光点的水平角度α和垂直角度ω，对点云数据进行深度矩阵映射。得到一个16×2 000维的矩阵，映射方法如式(1)所示：

(1)

式中：vbottom为最下面一层扫描线的垂直角度；vres为垂直角分辨率；hres为水平角分辨率；r、c分别为映射后的行坐标和列坐标。

得到深度矩阵后，在文献[15]的点云分割算法基础上，设定分割阈值S，得到若干点云块。由于月面地形变化大，平地和坑洼地形处的特征显著程度不一致。为了选择显著特征点云，将每个点云块转化为高度灰度图，通过式(2)计算图像的一元灰度熵H判断点云特征的显著程度。gk表示某个灰度在图像中出现的概率，其中k=0,1,…,255表示灰度值，H越大表明特征越显著。

(2)

(3)

对式(3)协方差矩阵C进行特征值分解，得到特征向量e0、e1、e2对应的3个特征值为λ0、λ1、λ2，特别地，λ0≥λ1≥λ2且被归一化。由主成分分析[16](Principal Component Analysis，PCA)可知，对于场景中物体的3种特征类型具有不同的特征值表示，若有λ0≈λ1≈λ2则表示散乱特征；若有λ0≫λ1≈λ2则表示线性特征；若有λ0≈λ1≫λ2则表示平面特征。定义线性度Lλ与平面度Plλ，其中Xv、Yv、Zv分别为X、Y、Z方向单位向量：

(4)

对每个点云块计算如下3个值：

(5)

式中:f1表示平面特征且决定对(tx,θ)的贡献;f2表示平面特征且决定对(ty,θ)的贡献;f3表示线性特征且决定对(tx,ty)的贡献。选取f1、f2、f3大的点云块数据，提取了有利于环境中有利于定位(tz,ψ,φ)方向与(tx,ty,θ) 方向的点云数据。

由于点云数据是输入到匹配算法中用于求解连续两帧数据间的变换矩阵，针对月球车的三维姿态估计问题，机器人六自由度位姿为p=[x,y,z,φ,ψ,θ]，变换矩阵为[tx,ty,tz,tφ,tψ,tθ]，其中φ,ψ,θ分别为机器人的俯仰角、滚转角和偏航角。

图2所示的月面环境场景观测中，初始三维点云观测数据如图2(b)所示，一些较小的石块与凹坑，由于其三维点云观测特征不明显被滤除，从而得到显著特征提取后的点云数据，如图2(c)所示。

图2 实际场景与特征提取前后点云观测Fig.2 Actual scene and pointcloud before and after feature extraction

3 基于曲面定位能力估计的增量式优化

基于SLAM方法对月面移动机器人进行位姿估计时，由于某些位置的显著特征点云观测少，易导致不准确的点云数据关联甚至错误匹配。通过计算当前位姿估计的不确定性，自适应地调整点云匹配的优化参数，能够有效减少位姿估计的误差和漂移，提升位姿估计的准确性。

在本文中，将Fisher信息矩阵的逆矩阵作为协方差。Fisher信息矩阵定义为预期测量值和激光传感器扫描的表面坡度的函数[17]。Liu[17]和Wang[18]等给出了基于二维概率网格的Fisher信息矩阵的推导。定义一帧激光观测数据包含一组离散的激光雷达点信息。假设激光点i的预期长度为riE，其中i=1,2,…,M，M为雷达扫描线总数，则离散机器人可定位性矩阵可以定义为

(6)

(7)

最终可以给出月面环境下的位姿不确定性估计如式(8)所示：

(8)

根据概率模型，将SLAM问题转换为基于最大后验概率估计的最小二乘问题，最终基于Kaess等[19]提出的增量优化算法求解该最小二乘问题。

针对优化过程中的点云数据关联问题，很多算法被提出来解决此类问题，例如迭代最近点匹配[20]、正态分布变换[21]、栅格相关性[22]。在本文中，将数据关联分为两类：时间相邻和空间相邻。对于时间相邻的观测，本文利用正态分布变换(Normal Distributions Transform，NDT)算法将时间相邻帧观测数据关联起来，从而可以获得当前机器人姿态的粗略估计。针对月球探索机器人来说，由于观测显著特征的匮乏会导致匹配陷入局部最优，帧间匹配无法获得准确的姿态估计，使地图缺乏一致性，从而导致机器人定位的精度显著降低。因此选择ICP算法将当前帧与全局地图匹配，以进一步消除误差。

(9)

4 实验环境与结果分析

4.1 仿真实验环境

为验证本文算法，在Gazebo中搭建仿真月球车，并搭建了A、B两个仿真环境模拟月面环境中存在的陨坑、石块和山脉等地形，如图3所示。

仿真月球车(如图4所示)配备有三维激光(Velodyne VLP-16)、里程计、惯性传感器(ADIS16495)，三维激光帧发布频率为10 Hz，惯性传感器帧发布频率200 Hz、偏置稳定度0.8 (°)/h。三维激光最远探测距离100 m，分辨率0.001 m，高斯噪声方差为0.01。

程序运行环境为Ubuntu16.04+ROS(Kinetic版本)，CPU为i3-3110M，运行内存为4 G。

4.2 实验数据与分析

为了验证算法的性能，以及在不同环境下的有效性，选择分别在场景A、B进行实验。实验中算法参数如表1所示。

为了体现算法的性能，选择LOAM[23](Lidar Odometry And Mapping)算法进行对比实验。对机器人的平移旋转误差进行分析，场景A实验的定位轨迹与对比如图5所示，位置误差如图6所示，角度误差如图7所示。本文算法与LOAM算法误差分析如表2所示。在图5(c)圆圈所示区域，机器人处于陨坑向上边缘，姿态倾斜，可观测信息量下降，导致定位误差在此区域增大。