1. 引言

随着科技的发展，人类往太空探索步伐加快。月面建图和定位是月面探测的基础技术，对于提高月球探测器的导航精度、实现月球表面的自主导航[1]、支持月球科学研究和资源勘探等具有重要意义。月球是人类探索太空的重要目标之一，也是未来建立太空基地和开发太空资源的理想场所。为了实现对月球的科学探测和资源利用，需要对月球表面进行精确的建图和定位，以提供月面地形、地貌、地质、资源等信息，以及月球车、月球站等设施的位置和姿态信息[2]。

月面定位是指利用各种传感器(如甚长基线干涉测量(VLBI)、惯性导航系统、视觉传感器等)获取月球探测器或着陆器/巡视器的位置和姿态信息[3]。月面建图是指利用各种传感器(如相机、激光雷达、雷达等)获取月球表面的几何、外观和语义信息，并构建一个描述月面环境的模型。月面定位和建图是月球探测的基础性和关键性技术，对于保障月球探测任务的安全、有效和高效执行具有重要意义，包括自主感知环境和避障等[4]。

定位和建图涉及多个学科领域，需要综合运用多种传感器和数据源，以及多种技术和方法，来实现对环境的感知和表达[5]。为了解决导航数据融合时普遍存在精度不高的问题，余志鹏等在航位推算技术基础之上，引入一种基于秩卡尔曼滤波(Rank-k Kalman Filter, RKF)的行人航位推算导航方法(Pedestrian Dead Reckoning, PDR)[6]。刘春等针对传统加权最小二乘法定位精度有限，以及卡尔曼滤波算法在定位过程中对初始位置高度敏感、噪声协方差常假设固定不变的问题，本研究提出了一种融合加权最小二乘法和改进卡尔曼滤波技术的伪距单点定位求解方法[7]。崔展博等面对SINS/GPS/DVS全源组合导航系统在非理想状态下展现出的较差容错性能以及在实现多源最优组合导航时遇到的问题，设计了一种基于卡尔曼滤波器的局部导航子系统滤波器和基于联邦滤波器的全源导航系统全局滤波器，进而构建了一种全源容错优化组合导航系统[8]。然而在太空探测中，月面定位和建图的研究也面临着许多挑战和难题[9]，不仅仅需要精度，也需要速度，太大的计算量并不适合月面建图与定位。文章通过建立月球探测车模型，分析探测车的运动学特征和信息采集。在此基础上，通过分析EKF原理的不足，提出采用改进后的IEKF对月球探测车搭载的传感器采集信息进行融合，实现探测车的月面实时定位及环境构图，从而使得探测车在月面精确进行更加安全。

2. 探测车月面定位构图坐标系建立

2.1. 月球探测车模型

当前我国采用的月球车如图1(a)所示。为了模拟真实的探测车，需要对探测车的结构和运动学进行建模。在探测过程中的定位和构图阶段，不涉及探测车的动力学特性。因此，可以简化我国采用的月球车模型，如图1(b)所示。探测车在月面上自主移动(在平面坐标系中移动)，其前轮可以进行转向操作，后轮为固定的驱动轮。探测车搭载多种传感器，其中搭载在车体上的传感器固联坐标系与自主移动车辆探测车的本体系一致，而搭载在车外的传感器可以通过坐标变换将其转换为车体坐标系。

(a) 月球车 (b) 月球车简单模型

Figure 1.Simple model of lunar exploration vehicle

图1. 月球探测车简单模型

2.2. 月面定位构图坐标系

当探测车在月面运动时需对于空间探测车建立相应的坐标系：探测车坐标系、月面坐标系及搭载传感器自身坐标系。对探测车的定位和对环境的构图也是在特定的坐标系下进行的，文章拟以携带的相机为主要定位导航传感器，探测车坐标系、视觉导航系统坐标系(相机坐标系和图像坐标系)与月面坐标系的关系如图2所示。探测车坐标系 $O_{e_{-}}{X}_e{Y}_e{Z}_e$，并且随着探测车的运动进行相应的移动。月球坐标系( $O_{w-}{X}_w{Y}_w{Z}_w$)按照国标GBT30112-2013建立，月球坐标系的原点位于月心位置，并且该坐标系与月球固连。月球坐标系内的值可以表示探测车在月球表面的任一位置。并且在该坐标系下常用经度λ_l、纬度L_l和高度h_l来表示载体在导航定位系统中的位置。视觉导航系统坐标系包括相机坐标系和图像坐标系，假设相机坐标系 $O_{c_{-}}{X}_c{Y}_c{Z}_c$的原点与探测车坐标系 $O_{t-}{X}_t{Y}_t{Z}_t$原点重合(相同)。根据图像坐标系与相机坐标系(探测车坐标系)的关系，探测车坐标系与月球坐标车的关系将获取的月面图像信息(像素坐标系)在月球坐标系中的表示如式(1)，坐标变换转如图2。 $O_{w-}{X}_w{Y}_w{Z}_w$为月面坐标系， $O_{c_{-}}{X}_c{Y}_c{Z}_c$为相机坐标系，光心为原点； $o_{-}xy$为图像坐标系，原点为成像平面中点， $o_{-}uv$为像素坐标系，原点为图像左上角；P为月球坐标系中的一点，即为月面中真实的一点；p点为P在图像中的成像点，在图像坐标系中的坐标为 $\left(x,y\right)$，在像素坐标系中的坐标为 $\left(u,v\right)$；f为相机焦距；R表示从月面坐标系变相机坐标系即探测车坐标系的旋转矩阵，T表示从月面坐标系变相机坐标系即探测车坐标系的平移矩阵。

$z_c\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{dx}& 0& u_0\\ 0& \frac{1}{dy}& v_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}f& 0& 0& 0\\ 0& f& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}R& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\\ 1\end{array}\right]$(1)

(a) 月面坐标系与相机坐标系关系 (b) 相机坐标系与图像坐标系关系

(c) 图像坐标系与像素坐标系关系

Figure2.Coordinate system of the lunar surface positioning configuration of the rover

图2. 探测车月面定位构图坐标系

2.3. 探测车的运动学模型

探测车在月面的运动状态根据运动学模型可表示为 $X={\left[x,y,\phi \right]}^{\text{T}}$，其中 $x,y$表示平面内一点的二维坐标，探测车的控制输入量为舵角γ和速度V。根据定位导航需求及各坐标系之间的关系可以将图1(b)中的车体模型转换为一长为L的连杆，前端两侧车轮的速度归于中心点，如下图3所示，给出舵角为负的简化模型[10]。

图3以舵角为负时为例，将速度V向探测车坐标系 $O_{e_{-}}{X}_e{Y}_e{Z}_e$的 $X_e{Y}_e$方向分解，再将速度向月球坐标系 $O_{w-}{X}_w{Y}_w{Z}_w$的 $X_w{Y}_w$方向分解，可得式(2)。

$\{\begin{array}{l}\{\begin{array}{c}\stackrel{\dot{}}{x}=V_{xt}=V\cos \left(\phi +\gamma \right)\\ \stackrel{\dot{}}{y}=V_{yt}=V\sin \left(\phi +\gamma \right)\end{array}\to \{\begin{array}{c}{V}_{xw}=V\cos \left(\gamma \right)\\ V_{yw}=V\sin \left(\gamma \right)\end{array}\\ \stackrel{\dot{}}{\phi }=\frac{V\sin \left(\gamma \right)}{L}\end{array}$(2)

将其离散化并转化为迭代形式，并加入噪声如式(3)，

$\left[\begin{array}{l}x\left(k+1\right)\hfill \\ y\left(k+1\right)\hfill \\ \phi \left(k+1\right)\hfill \end{array}\right]=\left[\begin{array}{c}x\left(k\right)\\ y\left(k\right)\\ \phi \left(k\right)\end{array}\right]+\left[\begin{array}{c}\Delta TV\left(k+1\right)\cos \left(\phi \left(k\right)+\gamma \left(k+1\right)\right)\\ \Delta TV\left(k+1\right)\sin \left(\phi \left(k\right)+\gamma \left(k+1\right)\right)\\ \frac{\Delta TV\left(k+1\right)}{L}\sin \left(\gamma \left(k+1\right)\right)\end{array}\right]+\left[\begin{array}{c}{q}_x\left(k+1\right)\\ q_y\left(k+1\right)\\ q_{\phi }\left(k+1\right)\end{array}\right]$(3)

为与实际空间探测车一致，将最大舵角及其转舵速度进行限制。实际月面探测过程中是对月面一些典型特征进行探测，因此，探测车在探测过程中可以利用检测到的月面特征进行定位和构图，假设空间探测车将相机返回给探测车的路标i(检测到的月面特征：月球坑或月面石块)的相对距离 $z_r^i\left(k\right)$和方位角 $z_{\theta }^i\left(k\right)$，根据以上的运动学分析，探测车的观测模型式(4)，其中 $r_i\left(k\right)$为观测的噪声向量。

$z_i\left(k\right)=\left[\begin{array}{c}{z}_r^i\left(k\right)\\ z_{\theta }^i\left(k\right)\end{array}\right]=h_i\left(x\left(k\right)\right)+r_i\left(k\right)=\left[\begin{array}{c}\sqrt{{\left(x_i-x\left(k\right)\right)}^2+{\left(y_i-y\left(k\right)\right)}^2}\\ \arctan \left(\frac{y_i-y\left(k\right)}{x_i-x\left(k\right)}\right)-\phi \left(k\right)\end{array}\right]+\left[\begin{array}{c}{r}_i^i\left(k\right)\\ r_0^i\left(k\right)\end{array}\right]$(4)

Figure3.Lunar exploration rover motion model

图3. 月球探测车运动模型

3. 基于IEKF改进的信息融合

卡尔曼滤波器[11]也是一种最优估计理论，卡尔曼滤波理论被广泛应用于各个领域，其成功应用的一个主要方面是设计组合导航系统[12]。由于实际系统多为非线性，因此当前应用的主流为扩展卡尔曼滤波[13]。但是EKF对于有色噪声的非线性系统滤波效果差，有学者提出了迭代扩展卡尔曼滤波算法[14]IEKF。

设 $t_k$时刻的被估计状态 $X_k$受系统噪声序列 $W_{k-1}$驱动，驱动原理由下述状态方程描述：

$X_k={\Phi }_{k,k-1}{X}_{k-1}+{\Gamma }_{k-1}{W}_{k-1}$(5)

对 $X_k$的量测满足线性关系，量测方程为：

$Z_k=H_k{X}_k+V_k$(6)

${\Phi }_{k,k-1}$为 $t_{k-1}$时刻至 $t_k$时刻的一步转移阵，状态转移矩阵， ${\Gamma }_{k-1}$为系统噪声驱动阵， $H_k$为量测阵，是状态变量到测量(观测)的转换矩阵，它负责将m维的测量值转换到n维，使之符合状态变量的数学形式，是滤波的前提条件之一。 $V_k$为量测噪声(协方差矩阵为 $R_k$的高斯分布)序列； $W_k$为系统激励噪声(协方差矩阵为 $Q_k$的高斯分布)序列。同时， $W_k$和 $V_k$满足：

$\begin{array}{ll}E\left[W_k\right]=0,\hfill & \mathrm{Cov}\left[W_k,W_j\right]=E\left[W_k{W}_j^{\text{T}}\right]=Q_k{\delta }_{kj}\hfill \\ E\left[V_k\right]=0,\hfill & \mathrm{Cov}\left[V_k,V_j\right]=E\left[V_k{V}_j^{\text{T}}\right]=R_k{\delta }_{kj}\hfill \\ \hfill & \mathrm{Cov}\left[W_k,V_j\right]=E\left[W_k{V}_j^{\text{T}}\right]=0\hfill \end{array}\}$(7)

$Q_k$为系统噪声序列的方差阵，是卡尔曼滤波器用于估计离散时间过程的状态变量； $R_k$为量测噪声序列的方差阵，测量噪声协方差R一般可以观测得到，是滤波器的已知条件。

系统噪声方差阵 $Q_k$非负定，量测噪声方差阵 $R_k$正定，k时刻的量测为 $Z_k$，则 $X_k$的估计 ${\hat{X}}_k$按下述方程求解(5个基本方程)：

状态一步预测方程：

${\hat{X}}_{k/k-1}={\Phi }_{k,k-1}{\hat{X}}_{k-1}$(8)

状态估计方程：

${\hat{X}}_k={\hat{X}}_{k/k-1}+K_k\left(Z_k-H_k{\hat{X}}_{k/k-1}\right)$(9)

滤波增益(是滤波的中间计算结果，即权值矩阵，卡尔曼增益，或卡尔曼系数)：

$K_k=P_{k/k-1}{H}_k^{\text{T}}{\left(H_k{P}_{k/k-1}{H}_k^{\text{T}}+R_k\right)}^{-1}$(10)

或

$K_k=P_k{H}_k^{\text{T}}{R}_k^{-1}$(11)

一步预测均方误差(k时刻的先验估计协方差( ${\hat{X}}_{k/k-1}$的协方差)，是滤波的中间计算结果)：

$P_{k/k-1}={\Phi }_{k,k-1}{P}_{k-1}{\Phi }_{k,k-1}^{\text{T}}+{\Gamma }_{k-1}{Q}_{k-1}{\Gamma }_{k-1}^{\text{T}}$(12)

估计均方误差：

$P_k=\left(I-K_k{H}_k\right)P_{k/k-1}{\left(I-K_k{H}_k\right)}^{\text{T}}+K_k{R}_k{K}_k^{\text{T}}$(13)

或

$P_k=\left(I-K_k{H}_k\right)P_{k/k-1}$(14)

或

$P_k^{-1}=P_{k/k-1}^{-1}+H_k^{\text{T}}{R}_k^{-1}{H}_k$(15)

式(8)到(15)即为离散型卡尔曼滤波基本方程。只要给定初值 ${\hat{X}}_0$和 $P_0$，根据k时刻的量测 $Z_k$，就可递推计算得k时刻的状态估计 ${\hat{X}}_k\left(k=1,2,\cdots \right)$。

IEKF会将上一步修正后的输出作为下一次迭代修正误差的输入值，通过不断迭代来修正误差减小误差，改进IEKF可利用蒙克卡罗方法根据一步预测均方差生成大量随机点，将随机点输入非线性变换，再利用输出结果计算变换后的估计均方误差。

改进后的IEKF算法的步骤可表达如下：

$\begin{array}{l}{\text{AlgorithmIterated}}_{\text{E}}{\text{xtended}}_{\text{K}}{\text{alman}}_{\text{f}}\text{ilter})\left({\mu }_{t-1},{\Sigma }_{t-1},u_t,z_t,\text{times}\right)\hfill \\ \text{predict}\hfill \\ {\bar{\mu }}_t=g\left(u_t,{\mu }_{t-1}\right)\hfill \\ {\bar{\Sigma }}_t=G_t{\Sigma }_{t-1}{G}_t^T+R_t\hfill \\ \text{Iterated update}\hfill \\ {\bar{\Sigma }}_t^0={\bar{\Sigma }}_t\hfill \\ {\bar{\mu }}_t^0={\bar{\mu }}_t\hfill \\ \text{for}i=1\text{to times do}\hfill \\ K_t^i={\bar{\Sigma }}_t^{i-1}{H}_t^{i-1}{\left(H_t^{i-1}{\bar{\Sigma }}_t^{i-1}{H}_t^{i-1}+Q_t\right)}^{-1}\hfill \\ {\bar{\mu }}_t^i={\bar{\mu }}_t^{i-1}+K_t\left(z_t-h\left({\bar{\mu }}_t^{i-1}\right)\right)\hfill \\ {\bar{\Sigma }}_t^i=\left(I-K_t^i{H}_t^{i-1}\right){\bar{\Sigma }}_t^{i-1}\hfill \\ \text{endfor}\hfill \\ {\mu }_t={\bar{\mu }}_t^i\hfill \\ {\Sigma }_t={\bar{\Sigma }}_t^i\hfill \\ \mathrm{return}{\mu }_t,{\Sigma }_t\hfill \end{array}$

为了方便，将改进后的IEKF依旧称为IEKF。

IEKF进行探测车定位构图的流程如图4所示。

Figure4.Flowchart of detection vehicle positioning composition

图4. 探测车定位构图流程图

4. 仿真实验

4.1. 仿真参数及月面特征数据

实验采用的是2D的含有显著月面特征的地图，月球探测车有三个状态量x,y坐标值和航角φ，控制量为速度V和舵角γ，月球探测车可以通过相机获取探测车与显著路标点间的 $z_r^i$和相对角度 $z_{\theta }^i$。为方便实验结果的对比，采取先生成仿真数据，再在仿真数据上进行定位构图[15]。仿真数据生成时首先会生成一些路径周围的关键特征点，再利用探测车的运动模型，计算舵角，并规划出一条经过这些关键点的曲线，在此过程中收集仿真需要的数据[16]。

仿真实验中各参数的设置如下表1所示，仿真月面图和月面特征点，如下图5所示。

Table 1.Simulation experiment parameters

表1.仿真实验参数

Figure5.Generated data graph

图5. 仿真月面图和月面特征点

其中，仿真月面中X，Y轴的单位为米。其中蓝色星号代表月面特征点；红色圆点代表路径上的关键月面特征路标点；图中外围有蓝色圆圈的为移动车辆的下一目标点；黑色虚线代表移动车辆的真实轨迹；黑色圆代表移动车辆，其中心的圆点为当前车辆位置，圆点与黑色圆边上点的连线代表机器人目前航向；有与车辆圆点连线的月面特征代表其被探测器相机观测到的特征点；大圆代表探测器携带相机传的观测半径，但传感器仅观测车辆正前方180度范围内的特征。

整个仿真实验的噪声参数如下表2所示：

Table2.Simulation experimental noise parameters

表2.仿真实验噪声参数

4.2. 实验结果及分析

在以上相同条件下采用EKF和IEKF进行探测车定位和构图。IEKF和EKF在白噪声下的性能对比如图6。从图6可知在白噪声下探测车的轨迹图真实轨迹(黑色线)、EKF轨迹(红色线)和IEKF(蓝色线)三者几乎一致，因此本实验进一步分析IEKF和EKF白噪声下二范数误差以及 $x,y,\phi$误差。图7左图显示的 $x,y,\phi$为EKF和IEKF下的误差图，从图中可知在0~1500 M的过程中EKF和IEKF时x误差很明显可以看出红色线覆盖了蓝色线，EKF的x的最大误差为0.5，y的最大误差为0.7， $\phi$的最大误差为0.05度都远远大于IEKF的最大误差；在1500-2500的过程中EKF(蓝线)明显高于IEKF (红线)，采用EKF进行定位和构图时x的最大误差为1.1，y的最大误差为0.7， $\phi$的最大误差为0.05；采用IEKF进行定位和构图时x的最大误差为0.6，y的最大误差为0.4， $\phi$的最大误差为0.05；显然也是IEKF优于EKF；在2500~3000的过程中EKF (蓝线)有出现多次和IEKF (红线)重合，采用EKF进行定位和构图时x的最大误差为0.3，y的最大误差为0.5， $\phi$的最大误差为0.05；采用IEKF进行定位和构图时x的最大误差为0.26，y的最大误差为0.3， $\phi$的最大误差为0.05；显然也是IEKF优于EKF。整体分析 $x,y,\phi$的误差，IEKF对 $x,y$即定位精度有明显改善，进而可以提高构图精度，IEKF对 $\phi$的改善不显著。图7右图显示的整体定位构图精度的二范数误差，从图中可知在0~1500 M的过程中EKF和IEKF时很明显可以看出蓝色线低于红色线，EKF的最大误差为0.8，IEKF的最大误差为0.5；在1500~2500的过程中EKF (蓝线)明显高于IEKF (红线)，采用EKF进行定位和构图时最大误差为1.2，IEKF的最大误差为1；在2500~3000的过程中EKF (蓝线)有出现多次和IEKF (红线)重合，采用EKF进行定位和构图时最大误差为0.4，IEKF的最大误差为0.35；显然也是IEKF优于EKF。整体分析 $x,y,\phi$的误差，IEKF在白噪声情况下定位构图精度有明显改善。

Figure6.Comparison of trajectories under IEKF and EKF Gaussian noise

图6. IEKF和EKF白噪声下轨迹对比

白噪声下将图放大也看不出明显的区别，因此画出他们的误差如下图7所示：

IEKF和EKF在有色噪声下的性能对比如图8所示。从图8左图可知在白噪声下探测车的轨迹图真实轨迹(黑色线)、EKF轨迹(红色线)和IEKF (蓝色线)三者几乎一致，从图8右图(放大后的轨迹图)可知在有色噪声下探测车的轨迹图真实轨迹(黑色线)和IEKF(蓝色线)两者几乎一致；EKF轨迹(红色线)则出入较大。为更精确的分析定位构图效果，同白噪声的分析一样也进一步分析IEKF和EKF有色噪声下二范数误差以及 $x,y,\phi$误差。图9左图显示的 $x,y,\phi$为EKF和IEKF下的误差图，从图中可知在0~1500 M的过程中EKF和IEKF时明显可以看出蓝色线低于红色线，EKF的x的最大误差为1.3，y的最大误差高达2.3， $\phi$的最大误差为0.1度都远远大于IEKF的最大误差；在1500~2500的过程中EKF(蓝线)明显高于IEKF (红线)，采用EKF进行定位和构图时x的最大误差为1.1，IEKF的在此区间最大误差也达到1.1，y的最大误差为1.7M， $\phi$的最大误差为0.07度；采用IEKF进行定位和构图时x的最大误差为1.1，y的最大误差为1， $\phi$的最大误差为0.1度；显然也是IEKF优于EKF；在2500~3000的过程中EKF(蓝线)有仍然高于IEKF (红线)，采用EKF进行定位和构图时x的最大误差为1，y的最大误差为1， $\phi$的最大误差为0.07；采用IEKF进行定位和构图时x的最大误差为0.6，y的最大误差为1， $\phi$的最大误差为0.07；从图中也可知IEKF误差线波动小，而EKF误差波动大。整体分析 $x,y,\phi$的在有色噪声下误差，IEKF对 $x,y$误差有明显改善，进而可以提高构图精度，IEKF对 $\phi$的改善不显著。图9右图显示的整体定位构图精度的二范数误差，从图中可知在整个定位构图过程中很明显可以看出蓝色线低于红色线，红色的线在800附近出现了突然增大，但最大值也仅仅与同时刻的EKF的误差相同，其他都远远低于蓝色的线，很明显的可以看出在有色噪声下IEKF能够显著提高定位和构图效果。

Figure7. $x,y,\phi$error and two-parameter error with IEKF and EKF Gaussia

图7. IEKF和EKF白噪声下 $x,y,\phi$误差以及二范数误差

Figure8.Comparison of IEKF and EKF trajectories under colored noise

图8. IEKF和EKF有色噪声下轨迹对比

Figure9.IEKF and EKF colored noise with $x,y,\phi$error and two-parameter error

图9. IEKF和EKF有色噪声下 $x,y,\phi$误差以及二范数误差

5. 结论

文章通过设计分析空间探测车的结构，建立了月面探测时的月面坐标系、探测车坐标系、相机坐标系和图像坐标系，并分析了这四个坐标系之间的坐标变换关系。在此基础上，分析了探测车在月面探测时的运动学模型，以及在滤波过程中输入量与更新量。随后，利用蒙特卡洛方法对IEKF进行了改进，基于改进后的IEKF，利用相机获取的图像数据进行了定位和构图处理。最后，通过仿真实验对白噪声和有色噪声下的EKF和文章提出的改进后的IEKF进行了定位和构图测试。实验结果表明，在白噪声下，EKF的效果与IEKF的效果差距不大，但在有色噪声下，EKF的效果明显落后于改进后的IEKF，而且改进后的IEKF计算量比粒子滤波算法小得多，更适合与月面定位与建图。文章提出的方法能够有效减小定位和构图误差，并为空间星体表面探测的定位导航提供了一种新思路。

基金项目

国家自然科学基金资助项目(项目编号61803035)。

NOTES

^*通讯作者。

参考文献