月球车多传感器SLAM 仿真系统

2021-12-31范亚娴陈卫东董凌峰王景川王鹏基

载人航天 2021年6期

范亚娴，陈卫东，董凌峰，王景川，王鹏基

(1.上海交通大学电子信息与电气工程学院，上海 200240； 2.北京控制工程研究所，北京 100190)

1 引言

月球车在月面环境长时间、大范围工作，月球车的定位是完成其工作的基础，需要对定位性能进行大量测试。由于月球探测任务的高风险和高价值，大规模还原月球环境成本太高，为了验证月球车传感器选型和算法，可行的替代方案是建立一个能够反映真实月球表面地形特征的月面仿真系统。

月球车仿真系统主要包括月面三维环境、月球车以及传感器3 个部分的模拟。国内外研究人员对此开展了一系列的研究。

月面三维环境的模拟是月球车仿真系统的基础。 Allan 等[1]提出了一种基于数字高程模型(Digital Evaluation Model， DEM)构建仿真月面环境的方法，仿照资源勘探者号月球车模拟了月球车和仿真月面之间的摩擦；谢圆等[2]模拟月海数字地形，用于月球车任务规划仿真；Schuster 等[3]搭建了一个类月面的崎岖地形并设计仿真环境；骆训纪等[4]设计了基于组件对象模型(Component Object Model， COM)技术的月球漫游车分布式仿真系统，并提供了环境编辑器，以便利用预先创建的地形模块搭建虚拟月面环境；Win等[5]介绍了一个用于验证月球探测SLAM 系统实用性的系统，通过模拟安宁山坑口的现有环境数据进行了测试；Vassallo 等[6]提出了无基础设施的视觉SLAM 算法，采用的月面仿真模型是利用月球勘测轨道器LRO 的数字高程数据与过滤器共同构建的。但是，上述月面模型均缺乏多样性，并且未模拟星空背景。谢洪乐等[7]针对月面环境特有的恒星无穷远点干扰问题，提出一种高效的恒星点剔除算法；尚天祥等[8]提出了一种针对月面环境的显著特征点云提取方法。但是，此前的仿真月面地形不可重构，月面场景多样性不足，不能模拟光照变化。

对于月球车的研究，Tian 等[9]对六轮摇臂式月球车进行仿真建模，并建立月球车在崎岖月面的动力学模型；Wang 等[10]对轮腿式月球车进行仿真，研究给出月球车运行的建议步幅和倾角；江磊等[11]基于Vortex 物理引擎，建立月球车车体动力学模型。但以上研究未描述车体质量、车轮悬挂、轮地摩擦重配置的方法。

在传感器方面，Bualat 等[12]利用双目相机构建视觉里程计来跟踪机器人(Astrobee)的运动，并使用深度相机构建用于路径规划的地图；赵静[13]利用视景仿真技术，在全局地图已知的情况下执行路径规划任务。但是，上述研究大多采用单一传感器。

由于月面地形崎岖，分布多种陨石坑和月面石块，色彩单一，月面纹理信息缺乏，需要考虑采用同时定位与地图创建(Simultaneous Localization and Mapping，SLAM)处理月球车的定位和地图创建问题。 SLAM 指在未知环境中，根据传感器观测和已建立的部分地图进行定位，在定位的基础上增量式建造地图。

本文基于开源机器人操作系统[14](Robot Operating System， ROS)和Gazebo 仿真器[15]，创建一个月球车仿真系统，该系统加载月面的地形数据，并在该环境中创建月球车和传感器的仿真原型，以验证、评估不同的传感器选型和不同的月球车算法。

2 系统架构

系统框架如图1 所示，主要由月面环境模型、月球车本体模型和传感器模型组成。模块的功能如下:

1) 月面环境模型。主要实现合成地形、光照阴影和星空背景的模拟。

2) 月球车本体模型。包括月球车车体、车轮悬挂和轮地摩擦等的相关配置。

3) 传感器模型。主要模拟惯性测量单元(Inertial Measurement Unit，IMU)、三维激光雷达、双目相机和亮度仪数据。

仿真系统可实现人机交互，允许用户对月面环境进行重构，使用鼠标控制用户观察视角，通过键盘发送相关控制指令，控制月球车运动；为SLAM 算法模块提供多种传感器数据，并提供算法接口。

3 月面环境模型

月球车在执行任务的过程中受到多种月面环境因素的影响。月球车需要避开陨石坑、月面石块、过于陡峭的月面凸起等。地形是影响月球车运动的首要因素。同时，由于月球车会搭载视觉传感器，因此，月面纹理和光照也会影响月球车的任务。

本文的月面环境仿真针对上述问题，分别对月面地形、月面纹理、光照和阴影、星空背景4 部分进行模拟，生成一个可重构的月面环境。基于基础地形生成合成地形，经过纹理映射、光照和星空背景渲染，生成月面环境模型。其中，星空背景的模拟调用Gazebo 中的星空模型。

3.1 合成地形

合成地形是在月面基础地形上添加陨石坑和月面石块，形成一个包含多种月面场景的地形。其中，陨石坑和月面石块均采用随机分布。对陨石坑和月面石块的仿真不仅是月面仿真的关键，也对月球车相关算法如SLAM、路径规划等起到着至关重要的作用[16]。

3.1.1 基础地形

地形原始数据来自月球侦察轨道卫星LRO，该地点位于阿波罗15 号着陆点以南3 km 处，包含不同坡度的起伏和深度超过20 m 深的环形山[17]。在仿真月面中，月面范围为1000 m×1000 m，基础地形的俯视图如图2 所示。

基础地形模拟场景较为单一，为了提高月面地形模拟的多样性，使地形模拟更为真实，在基础地形上进一步添加不同种类的陨石坑和月面石块。将原本固定的单一月面场景改进为可重构的多种月面场景。

3.1.2 月面陨石坑

通过高度图变换添加月面陨石坑。高度图的每一个像素对应三维空间的一个顶点，一般0(黑色)表示最低点，255(白色)表示最高点。高度图由二维数组v构成，对于月球中的采样点(x，y，z) 。高度图的二维坐标值v[x][y] 对应三维顶点(x，y) 值，亮度值对应顶点的z值的关系如式(1)所示:

式中，hMAX是月球表面的最大高度。v[x][y] 对应的亮度值为l。

Pike[18]对月球陨石坑进行了测量和计算，陨石坑的深度与直径满足经验公式(2):

式中，H为陨石坑深度，D为陨石坑直径。

月面高度图转换后的月面三维场景如图3(a)所示。将该高度图作为地形模型加载到Gazebo 的.world 文件中，对月球环境进行渲染，生成月球车仿真环境，如图3(b)所示。

图3 月面仿真场景Fig.3 Lunar surface simulation scene

3.1.3 月面石块

月面石块形状包括圆形、矩形、凹坑形等[19]。采用SketchUp 设计石块的基础模型，生成41 种不同的石块模型，在建立石块模型后，转换为Gazebo 可识别的统一格式(.dae 格式)，并将其导入到Gazebo 中，形成石块模型数据库。

由于石块模型库中的模型个数有限，实际月面分布的石块远远超出模型库中的石块模型数。因此，为了避免月面环境中石块过于单调，在石块融合到月面过程中，通过随机变换石块在基础地形上的角度和大小，从而达到不同的仿真结果。在月面环境中可以在线调整石块的形状、大小、数量和空间分布。

3.1.4 合成地形实例

合成地形选取3.1.1 节所述具有代表性月面地形特征的区域作为模拟场景。合成地形的局部图如图4 所示，其中图4(a)为合成地形俯视图，红色框选部分为新增陨石坑；图4(b)上图为月面凸起和月面石块局部放大图，下图为陨石坑局部放大图，该陨坑直径约为60 m，深度为约为11 m。

图4 合成地形Fig.4 Synthetic terrain

设置41 种不同类型的石块，如图5 所示，不同种类石块在月面环境中的数目如图6 所示，其中，每100 m×100 m 的范围内，随机分布的石块总数为1056 块，石块直径大于20 cm 的数目为23块，石块直径在10～20 cm 之间的数目为160 块，石块直径在5～10 cm 之间的数目为753 块，石块直径在2～5 cm 之间的数目为120 块。

图5 月面石块Fig.5 Lunar surface rocks

图6 不同种类石块的数目Fig.6 Number of different types of rocks

3.2 月面纹理

根据月面不同区域的纹理特征，选取3 种不同的纹理[20]对月面地形进行纹理映射。本文在处理纹理映射时，可以简单归纳为以下4 个步骤:

1)加载纹理图像。

2)将纹理的坐标映射到三维空间坐标。

3)将纹理颜色与月面基础地形、颜色和材质颜色融合。

4)采用最近邻差值[21]进行纹理过滤，优化纹理映射的效果。

不同的纹理效果如图7 所示。其中，纹理1 为基础地形纹理图片，纹理2 中纹理特征较纹理1有所减少，纹理3 中纹理接近于灰色，特征最少。

图7 不同月面纹理Fig.7 Different Lunar surface textures

3.3 光照与阴影

在进行月面光照和阴影的模拟时，主要考虑月面是介于漫反射体和镜面反射体之间的物体，在设计光照时需要考虑反射强度与光源之间的关系[16]。因此，采用无限光源，即选用从无限远处照射的光源，主要模拟太阳光照射效果，光线是互相平行的。

通过调整光源的方向角来调节光照角度，通过调节光源中心点与原面的距离调节月面亮度。同时，通过调节光照强度与角度可模拟月面不同时刻的光照。本文仅考虑太阳光照本影的影响。光照显示效果图如图8 所示。

图8 光照效果图Fig.8 Lighting effects

4 月球车本体模型

采用SolidWorks 设计月球车的三维模型，对月球车的车身、车轮、连杆等各部分进行设计。

4.1 月球车车体

4.1.1 车体模型

月球车的3D 建模主要利用SolidWorks 软件生成Gazebo 可执行的URDF 文件，使用XACRO对URDF 文件模型进行优化。其中，URDF 是ROS 中使用XML(可扩展标记语言)格式描述机器人(含月球车)模型的文件，包括外观形状、尺寸、颜色、物理属性和关节类型等基本属性。XACRO 是对URDF 文件进行优化后的模型文件。月球车模型文件导入的流程图如图9 所示。

图9 月球车模型导入流程Fig.9 Import process of lunar rover model

月球车的车体质量为M，车身尺寸为(L，W，H)，车轮直径为D，车轮间距为B，用户可配置上述参数。月球车仿真模型、本体坐标系与传感器坐标系如图10 所示，月球车本体坐标系原点Ob位于月球车的质心，定义月球车前进方向为x轴方向。

图10 月球车车体与传感器配置Fig.10 Configuration of the lunar rover body and sensors

4.1.2 车轮悬挂

通过直接在Gazebo 中配置每一次仿真循环中的修正误差ERP和物理引擎的全局混合约束力CFM模拟弹簧-阻尼约束[22]，实现车轮悬挂模型。ERP表示在一次仿真循环中，系统所允许最大关节误差，取值范围为0～0.8；CFM反映了物体表面的柔软程度，取值范围为0～1，计算公式如式(3)、(4)所示:

式中，Kp为弹性系数，Kd减震系数，模拟步长h由Gazebo 仿真器生成。

设置CFM＝1，ERP＝0.2，添加悬挂系统前后的IMU 的z轴加速度曲线如图11 所示。

图11 IMU z 轴加速度曲线Fig.11 z-axis acceleration curve of IMU

4.2 运动控制器

利用libgazebo_ros_diff_drive.so 插件，将月球车简化为双轮差速模型。通过式(5)将月球车的车体运行速度解算为各个车轮的转速。

式中，vL和vR为左、右轮的线速度，v为车体线速度，w为车体角速度，d为前后轮间距。

4.3 轮地摩擦

为模拟地面摩擦，通过XACRO 文件配置开源动力引擎(Open Dynamic Engine，OpenDE)定义的摩擦系数μ，满足库伦定律[23]:

式中，fμ为车轮摩擦力，fN为车轮所受法向力，μ为表面摩擦系数。μ有效范围是任意非负数，0 表示无摩擦。设置μ为0.1。

5 传感器模型

为方便传感器选型以及验证月球车算法的有效性，月球车模型搭载多种传感器，探测月球表面的障碍物和潜在危险，包括本体传感器IMU 和外部传感器双目相机、三维激光雷达、亮度仪等。其中，亮度仪为仿真中额外增加的虚拟传感器，主要是为了定量测量环境中的亮度变化，其他传感器的相关设置均参考现有月球车设置。

5.1 传感器布局

月球车的IMU、双目相机、三维激光雷达传感器坐标系相对于月球车本体坐标系Ob的位置和姿态关系如表1 所示。车体传感器的布局见图10，根据各传感器相对于车体坐标系的位姿，可获得各传感器之间的转换关系，用户可配置各传感器的位姿。

表1 各传感器坐标系相对于车体坐标系的位置关系Table 1 The positional relationship of each sensor coordinate system relative to the base coordinate system

5.2 IMU

IMU 使用libgazebo_ros_imu_sensor.so 插件模拟Analog Devices ADIS16495 IMU，发布月球车的速度和加速度数据，所使用的IMU 传感器参数设置如表2 所示。

表2 IMU 传感器参数Table 2 Parameters of IMU sensor

5.3 双目相机

双目相机采用libgazebo_ros_multicamera.so插件模拟FLIR BumbleBee2 双目相机，发布图像数据。模拟的双目相机传感器的相关参数设置如表3 所示。

表3 双目相机传感器参数Table 3 Parameters of stereo camera sensor

5.4 三维激光雷达

三维激光雷达使用libgazebo_ros_velodyne_laser.so 插件模拟VLP-16 三维激光雷达，发布点云数据。三维激光雷达的相关参数设置如表4所示。

表4 三维激光雷达传感器参数Table 4 Parameters of 3D LiDAR sensor

5.5 亮度仪

亮度仪为月球车中额外增加的虚拟传感器，主要用于定量测量3.3 节中所描述的光照变化，计算图片的平均亮度。相机的原始数据类型为YUV 格式，其中，Y通道为亮度通道。计算图片的平均亮度即是计算Y通道的平均值，亮度仪的测量范围为0～255 px。

亮度仪扩展了相机传感器模块，使用相机捕获图像，计算图像的亮度，通过ROS 话题发布亮度值。

6 仿真案例

基于实际月面环境和月球车的特点，本文建立并集成了月面环境、月球车和传感器模型的计算机仿真系统。该仿真系统的主要功能是为月球车算法开发提供月面数据、月球车运动数据和传感器数据，从而验证各种月球车算法的逻辑和功能。

因此，本文在仿真系统上验证月球车多种传感器SLAM 算法性能，分别测试视觉和激光惯性SLAM 的2 种典型算法:ORB-SLAM2[24]和LeGOLOAM[25]。其中，月球车在月面环境中的位姿真值由Gazebo 插件libgazebo_ros_p3d.so 生成。

6.1 系统配置

仿真系统采用Linux 操作系统Ubuntu16.04、机器人操作系统(ROS Kinetic) 和仿真平台Gazebo 7.16，硬件配置为Intel(R) Core(TM) i7-4790 CPU + 12Gb RAM。考虑到月球车的尺寸，本文在仿真月面中选取100 m×100 m 的范围作为测试区域。月球车参数配置如表5 所示。月球车传感器参数可以由用户自定义设置。

表5 月球车配置参数Table 5 Parameters of lunar rover

仿真系统提供人机交互接口，可以通过键盘控制月球车的移动和转向，人机界面将仿真结果通过Gazebo 以及Rviz[26]反馈给用户，允许用户对月面环境进行重构，使用鼠标调节用户观察视角。 Gazebo 的主界面包括场景、控制面板、顶部工具栏和底部工具栏4 个部分，分别对应图12(a)中①～④。 Rviz 的主界面包括工具栏、显示列表项和3D 视图区3 个部分，分别对应图12(b)中的①②④。

图12 月球车仿真与传感器信息显示Fig.12 Lunar rover simulation and sensor information display

图12 显示了月球车在月面环境中的仿真结果。在图12(b)中，Rviz 中显示双目相机、三维激光的传感器数据。从双目相机生成的图像，如图12(b)③中可以看到仿真的陨石坑、月面石块和纹理信息，三维激光的点云数据，如图12(b)④也显示了月面石块和月面凸起的轮廓。

6.2 ORB-SLAM2 测试案例

相机采用FLIR BumbleBee2 双目相机模型，主要用于检测月面潜在危险，如火山口和小石块，同时，相机对于月面纹理以及光照变化也十分敏感。

ORB-SLAM2 是经典的视觉SLAM 算法，可用于单、双目相机和RGB-D 相机，包括视觉里程计、跟踪、回环检测，是一种完全基于稀疏特征点的SLAM 系统。本文测试了月球车在不同纹理和不同亮度的ORB-SLAM2 算法效果。其中，月球车运行距离约为200 m。

6.2.1 不同特征稠密程度

图13、14 分别为月面场景中的某一帧图像及该帧图像中检测到的特征点，分别对应稠密特征点和稀疏特征点的仿真场景，在运行过程中，图14 中月面陨石坑也会被检测出特征点。

图13 特征稠密场景特征点检测Fig.13 Feature point detection for feature dense scenes

图14 特征稀疏场景特征点检测Fig.14 Feature point detection for feature sparse scene

6.2.2 不同纹理效果

在3 种不同纹理场景下分别测试了ORBSLAM2，定位误差如表6 所示。其中纹理2 较纹理1 视觉特征较少，在进行ORB 特征检测时误匹配特征点较少，因此，定位误差与纹理1 场景相比稍小。在纹理3 场景中，由于纹理接近于灰色，只能检测较少的特征点，定位极大地依赖石块、陨石坑以及部分凸起，定位效果最差，测试结果符合预期效果。

表6 不同纹理场景的ORB-SLAM2 定位误差Table 6 ORB-SLAM2 localization errors in different texture scenes

6.2.3 不同光照效果

图15 表示月球车在背光场景下对应的特征点提取情况，可以清晰地提取石块特征点；图16表示在星空背景下星星被识别为特征点的情况，在月球车运行过程中，星空一直被错误地识别为静态特征点，影响定位误差。因此，在利用视觉定位时，需要滤除远处的静态星空点，以防止其对定位的影响。

图15 背光场景特征点提取Fig.15 Feature point extraction for backlit scene

图16 星空背景特征点提取Fig.16 Feature point extraction for starry sky background

采用纹理1 场景，分别在如图17 亮度值为125 px、90 px、60 px 情况下进行了ORB-SLAM2测试，定位误差如表7 所示。结果表明，在亮度值为125 px 时，定位效果最佳。亮度越高，特征点提取效果越好。

图17 不同光照场景Fig.17 Different illuminance scenes

表7 不同亮度值的ORB-SLAM2 定位误差Table 7 ORB-SLAM2 localization errors under different illumination

6.3 LeGO-LOAM 测试案例

激光雷达使用VLP-16 三维激光模型，用于探测障碍物，如远处的山脉或大岩石。与相机相比，激光雷达不受光照变化、纹理变化影响，只受月面地形和月面石块的影响。因此，分别在不同月面石块数量的月面仿真环境中测试LeGOLOAM。

LeGO-LOAM 将三维激光雷达数据与IMU 数据融合，定位误差如表8 所示。测试发现:①在特征量稀疏场景下，由于缺乏石块等明显的特征，仅仅依靠月面凸起和陨石坑，三维激光雷达无法获得足够的有效特征点，定位误差较大；②在特征量较多场景下，算法依赖月面石块和月面的凸起，定位误差较①的情况有所减小，符合LeGO-LOAM算法的预期效果。