太阳敏感器辅助的分布式EKF-SLAM火星车自主导航方法

裴福俊，严 鸿，朱明君

（1. 北京工业大学 信息学部，北京 100124；2. 计算智能与智能系统北京市重点实验室，北京 100124）

引 言

火星车是代替人类在火星表面行驶，利用自身携带的科学仪器进行考察探测、样品收集等工作，最后将各种探测数据传送回地球的自主式巡视机器人。自主导航是火星车进行路径规划、自主避障、遥操作等任务的前提和基础，是其顺利实现火星表面巡视探测的一项关键的核心技术。

惯性导航和天文导航方法一直以来都是深空探测机器人（火星车、月球车等）的必备导航系统[1-3]，即使是中国2013年发射的月球车也配备了光纤捷联惯性导航系统与星敏感器作为导航传感器。但是，受到传感器测量原理和火星环境的限制，惯性/天文组合导航方法虽然具有较高的姿态精度，却存在定位精度低和环境信息描述缺乏的问题，无法满足现代行星探测机器人对高精度位姿和环境信息的需求[4]。1986年，Randall C. Smith和Peter Cheeseman用扩展卡尔曼滤波器（Extended Kalman Filter，EKF）对机器人和环境路标的位置进行估计[5]，由此开启了SLAM（Simultaneous Localization and Mapping）算法研究的序幕。经过20多年的发展，SLAM算法以其定位精度高和能够获取未知环境信息的特点，逐渐成为解决机器人自主导航问题的关键技术，并被应用于解决行星机器人自主导航问题[6-7]。

现有的SLAM算法主要包括EKF-SLAM[8-9]、SEIFSLAM[10]、PF-SLAM[11]以及分布式PF-SLAM[12-13]。其中，前面几种算法为集中式结构，由于其状态量将随着路标点的变化而不断扩大，造成了无限高维的状态量，在大地图场景和复杂环境下，算法的实时性和稳定性无法保证。分布式PF-SLAM具有结构上的明显优势，但是其采用的粒子滤波算法不可避免地存在粒子贫化和计算量大的问题，并不适合于火星车的实际应用。

针对已有SLAM算法的问题，本文综合现有SLAM算法的特点，提出了采用分布式结构实现EKFSLAM算法，实现了一种利用太阳敏感器辅助的分布式EKF-SLAM算法。该方法采用双轴模拟式太阳敏感器[14]解算火星车的航向，并将航向信息引入到分布式SLAM的各子系统中，建立航向辅助的观测方程，从而提高整体的观测性、一致性和收敛性。文中详细给出了太阳敏感器辅助分布式SLAM的系统模型和实现原理，并通过实验验证了算法的可行性和有效性。

1 太阳敏感器确定机器人航向

1.1 太阳相对方位角确定

装载在机器人上的太阳敏感器敏感入射光的强度，由测得的太阳高度角hs和方位角 δs计算得到敏感器坐标系下的太阳矢量，如图1所示。经过车体坐标系转换到导航坐标系的矢量可确定太阳相对方位角，即太阳入射光投影和机器人前进方向的夹角。

图1 太阳敏感器坐标系下的太阳单位矢量Fig. 1 Solar unit vector in coordinate system of sun sensor

假设O2O=1，在图1所示的三维坐标系中，可得O2O1=sinhs，O1O=coshs。则太阳敏感器坐标系下太阳单位矢量为

由于太阳敏感器是固定在车体上的，根据太阳敏感器安装的位置确定其坐标系SS和车体坐标系Sb之间的约束关系满足Sb=SS。

为了得到相对于导航坐标系下的太阳单位向量Sn，必须将车体坐标系下的矢量Sb转换到导航坐标系下。根据惯性导航设备测得车体相对当地导航坐标系的X轴的翻滚角为 θ，相对于Y轴的俯仰角为ϕ 。故通过欧拉角坐标旋转矩阵进行两次旋转（不考虑航向）得到车体坐标系和导航坐标系的关系式为

导航坐标系到车体坐标系的转换矩阵为

太阳单位矢量在导航坐标系下可表示为

因此，太阳在导航坐标系下的方位角为

1.2 天文星历解算太阳绝对方位角

绝对方位角即太阳入射光投影和机器人所在地平坐标系正北方向的夹角。基于天文星历数据的绝对方位角解算是在已知时间和机器人的经度和纬度的前提下，利用星历数据以及坐标系间的换算实现的，如图2所示。

地平坐标系的基圈是地平圈，它的纬线圈是与地平圈平行的小圆，经线是天球上通过天顶和天底且垂直于地平圈的圆。时角坐标系的基圈是天赤道，它的纬线是与天赤道平行的小圆，经线是天球上通过北天极和南天极的圈。

图2 地平坐标和时角坐标变换图示Fig. 2 Diagram of coordinate transformation of horizon and hour angle coordinate

若已知太阳的时角t和赤纬 δ，以及观测点的地理纬度 λ。求解天体的方位A和天顶距Z，利用球面三角公式即可得

由式（5）和式（6）可以解算绝对方位角

最终可得机器人航向角为

2 分布式SLAM系统模型

本文使用激光传感器获取环境中的特征点相对于机器人的距离和方位角。对于航迹的推算是借助安装在车轮上的里程计测得机器人位置变化量，确定机器人的位置。在SLAM系统中，机器人模型如图3所示。

图3 全局坐标系下机器人模型Fig. 3 Robot model in global coordinate system

分布式SLAM结构即根据有效特征点构建多个相互平行的子滤波器，每个子滤波器具有各自的状态方程和观测方程，将各个子滤波器独立滤波估计的结果传送到主滤波器进行融合，融合结果即为最终的状态估计。

分布式SLAM子系统的运动方程描述为

系统的运动模型一般描述了机器人在控制信号作用下的状态变化情况。其中机器人在 (t−1,t)的时间间隔内控制信息为ut=(∆Dt,∆φt)T，∆Dt是机器人移动距离的变化量，∆φt是机器人航向角的变化量。wt是过程噪声，用来表征运动误差的不确定性。

对于常用的两轮差分里程计，可利用编码器信息测得控制量如下

其中： ∆dL和∆dR分别是编码器测得的左右轮的前进距离；l为两轮间的距离。

分布式SLAM系统的观测方程为

系统的观测模型是机器人通过激光传感器获取的t时刻的环境信息。其中：是机器人与第i个有效特征点之间的距离；是机器人相对于特征点的偏转角；vi(t)表示系统的测量噪声。

3 航向辅助的分布式SLAM

3.1 航向辅助观测

在解决SLAM问题时，与其他滤波方法相比，扩展卡尔曼滤波因其良好的稳定性和一致性，仍然是首选滤波算法，EKF-SLAM的计算复杂度和实时性能够有效满足实际应用的要求。

文献[15-16]已经证明，机器人航向的不确定性对EKF-SLAM算法的收敛性和一致性有显著的影响。而且，机器人航向的不确定性也会导致特征点位置的不确定性。也就是说即使知道机器人与特征点在之间的距离，因为航向的不确定性，也无法得到特征点的准确位置，特征点的位置又与机器位姿相互作用，影响估计精度。

为了解决上述问题，在EKF-SLAM系统中引入太阳敏感器作为辅助观测的传感器。通过太阳敏感器获得的航向信息作为加入观测方程量中新的测量量。改进后的系统观测方程如下

其中，Zφ表示由太阳敏感器解算的机器人航向角，其计算方法同式（8）。

假设t时刻观测到n个有效的特征点，则分布式系统模型如下

3.2 滤波融合过程

由于实际应用中SLAM问题是非线性的，在使用EKF解决问题时，首先需要利用泰勒级数展开将运动模型和观测模型线性化。离散化的系统模型为

子系统的时间更新方程为

系统的量测更新方程为

其中，Pi,k是与对应的误差协方矩阵。

卡尔曼滤波增益为

在分布式SLAM算法中，根据关联成功的有效特征点建立若干个平行的子滤波器。每个子滤波器独立地进行滤波估计，各估计结果最终会传送到主滤波器。在主滤波器中，按照各子滤波器信息所占权重进行数据融合，融合计算的结果即为机器人状态的最终估计。

由于协方差矩阵能够表征滤波器的估计精度，所以利用协方差矩阵定义子滤器的融合权重ηi，则有

主滤波器融合处理后的机器人状态估计结果为

4 实验验证

本文中的验证实验是在北京工业大学科学楼前的广场上进行的。实验时，在场地的道路两侧放置圆柱形球筒作为人工的特征点，实验环境如图4所示。

图4 实验环境Fig. 4 The experimental environment

实验移动平台如图5所示。激光传感器采用SICK LMS111，有效测量距离是20 m，测量误差保证在±35 mm内，实验中设定扫描范围为 0◦～1 80◦，其用于采集机器人和特征点间的距离和偏角数据。实验中使用的双轴模拟式太阳敏感器可在双轴±64°视场范围内实现姿态测量精度为±0.3°，通过四象限硅光电池敏感太阳光光强，实现太阳矢量两轴方向角的测量。装载在车轮上的里程计编码器在采样时间内采集机器人位移和角度的变化量。里程计的最大缺点就是累积误差，故采用太阳敏感器获取的航向信息修正偏差。GPS系统可以实时获取机器人移动轨迹，用于算法验证。

图5 实验移动平台Fig. 5 The experimental mobile platform

实验过程中，机器人从标记位置出发，在实验场地缓慢绕行一个完整的大圈。机器人在运动过程中通过激光扫描球筒边缘的一系列光束提取特征点，其运行轨迹为椭圆形。分别用集中式EKF-SLAM（CEKFSLAM）、分布式EKF-SLAM（DEKF-SLAM）、航向辅助的分布式EKF-SLAM（HADEKF-SLAM）算法对机器人运动轨迹进行估计，结果如图6所示。

图6中黑色虚线、绿色点划线、红色实线分别表示的是CEKF-SLAM、DEKF-SLAM HADEKF-SLAM算法对机器人轨迹的估计。蓝色虚线是里程计对轨迹的推算结果，星号表示环境中的特征点。很显然，里程计的误差不断累加，并且很快偏离真实路线。3种SLAM算法都能够有效修正里程计产生的偏差，在一定程度上保证算法的精度和收敛性。

图6 机器人轨迹估计结果Fig. 6 The trajectory estimation results of robot

以GPS采集的实际数据为基准，对3种算法的估计结果进行误差分析。利用式（23）计算算法估计结果的均方误差，如表1所示。

表1 实验数据与GPS比较的均方误差Table 1 The mean square error of experimental data compared with GPS m

由表1中数据可知，与CEKF-SLAM相比，分布式EKF-SLAM算法的估计精度更高。同时，HADEKFSLAM无论是在X轴方向还是在Y轴方向算法的估计误差都在0.6 m以内，这说明引入航向辅助观测对算法性能有显著影响。

5 结 论

针对现有SLAM算法在实时性和准确性方面无法满足火星车自主导航实际需求的问题，并考虑火星车航向对EKF-SLAM算法性能的影响，本文提出了一种基于航向辅助的分布式EKF-SLAM火星车的自主导航定位方法。对于传统集中式SLAM算法，单一滤波器的状态量因有效路标点的数量增加而不断增大。然而，分布式EKF-SLAM算法中，多个平行的子滤波器的维数固定不变，可以有效地提高系统的运算效率和容错率。同时实验结果表明，引入航向辅助的分布式EKF-SLAM能够保证算法的收敛性和一致性，从而有效地提高了算法的估计精度。