月球基地车辆自校准导航定位方法

崔 轶， 傅惠民

（北京航空航天大学 小样本技术研究中心， 北京 100191）

0 引言

构建月球基地是月球探测的核心目标之一， 也是人类实现对月球资源的深度开发和利用的重要手段[1，2]。 在月球基地中， 可以通过机器人和智能车辆来控制和照料基地。 因此，为机器人和智能车辆提供导航服务就非常重要。 这种月球基地的导航与一般的月面导航不完全一样，前者是小范围的，后者是大范围的，前者对导航精度的要求远高于后者， 这导致月球基地机器人和车辆的导航面临更大的挑战。 为此，文献[3]提出一种新的步长估计模型和行人航位自校准推算方法， 并建立一种室内自校准导航定位方法，不但解决了室内行人导航定位问题，而且能够为月球基地机器人提供高精度的导航定位。 在此基础上， 本文进一步提出一种月球基地车辆自校准导航定位方法，可以实现月球基地车辆高精度导航定位。

1 惯性导航自校准方程

由于惯性导航系统每次使用之前需要较长的初始对准时间，加上受环境的影响和设备老化等原因，其陀螺仪和加速度计往往存在较大漂移。因此，下面根据惯性导航方程， 给出能够对陀螺仪和加速度计漂移进行有效修正的自校准方程。

1.1 自校准状态方程

由于惯性传感器的采样频率非常高， 相邻两次采样之间加速度变化不大，因此其状态方程可以表示为：

式中：ax，k，ay，k，az，k为第k 次采样时导航坐标系下车辆质心的三轴加速度，bax，k-1，bay，k-1，baz，k-1为未知输入 （模型误差、环境干扰等），wax，k-1，way，k-1，waz，k-1为噪声。

由运动学方程可知， 车辆行进过程中的位置和速度由下面四式给出：

式中：（xk，yk） 为第k 次采样时导航坐标系下车辆质心的位 置 坐 标，vx，k，vy，k为 第k 次 采 样 时 导 航 坐 标 系 下 车 辆 质心的速度， △tk-1为相邻两次采样之间的时间间隔，bx，k-1，by，k-1，bvx，k-1，bvy，k-1为未知输入（模型误差、环境干扰等），wx，k-1，wy，k-1，wvx，k-1，wvy，k-1为噪声。

同理， 相邻两次采样之间车辆绕质心转动的角速度也变化不大，因此有：

式中：ωx，k，ωy，k，ωz，k为第k 次采样时载体坐标 系下车辆绕质 心 转 动 的 三 轴 角 速 度，bωx，k-1，bωy，k-1，bωz，k-1为 未 知 输 入（模型误差、环境干扰等），wωx，k-1，wωy，k-1，wωz，k-1为噪声。

根据欧拉角微分方程[4]，车辆行进过程中的航向角、俯仰角和滚转角可分别由下面三式计算：

式中：ψk是航向角，即车辆纵轴在水平面的投影与地理北向之间的夹角，偏东为正，偏西为负；θk是俯仰角，即车辆纵轴与纵向水平轴之间的夹角，向上为正，向下为负；γk是滚转角，即车辆纵向对称面与铅垂面之间的夹角，右倾为正，左倾为负。 bψ，k-1，bθ，k-1，bγ，k-1为未知输入（模型误差、环境干扰等），wψ，k-1，wθ，k-1，wγ，k-1为噪声。

1.2 自校准量测方程

根据导航坐标系和载体坐标系之间的变换关系[4]，加速度量测方程为：

式中：acx，k，acy，k，acz，k为第k 次采样时载体坐标系下的加速度计量测，dax，k，day，k，daz，k为未知输入（加速度计漂移、环境干扰等），ξax，k，ξay，k，ξaz，k为噪声。 ωgx，k，ωgy，k，ωgz，k为第k 次采样时载体坐标系下的陀螺仪量测，dωx，k，dωy，k，dωz，k为未知输入（陀螺仪漂移、环境干扰等），ξωx，k，ξωy，k，ξωz，k为噪声。Cij，k为坐标变换矩阵Ck第i 行第j 列的元素，由下面九式计算[4]：

2 基于TOA 的车辆自校准导航方法

下面以超宽带（UWB）信号为例对基于到达时间（TOA）的车辆自校准导航方法进行讨论，其他信号也可同样处理。

设第k 次采样时，在位置（xk，yk）处能够接收到n 个UWB 信号，则基于TOA 的UWB 距离量测方程为：

采用文献[5]中的非线性系统双未知输入自校准滤波方法，对状态方程式（1）～式（13），量测方程式（14）～式（19）和式（29）进行滤波，即为基于TOA 的车辆自校准导航方法。滤波时，可选取视距环境中最小的两个距离量测值，令其方程中的未知输入dri，k=0。 也可以采用文献[5]中的两步自校准滤波方法来进一步提高滤波的鲁棒性和精度。

3 基于TDOA 的车辆自校准导航方法

基于TOA 的车辆自校准导航方法要求基站和标签的时间完全同步，这在工程实际中有时难以实现。 为此，下面进一步给出只需各个基站之间时间同步的基于到达时间差 （TDOA） 的方法。 仍然以UWB 信号为例对基于TDOA 的车辆自校准导航方法进行讨论， 其他信号也可同样处理。

TDOA 需将基站两两分组，第1 个和第2 个基站为一组，第3 个和第4 个基站为一组，以此类推。 下面给出一种以基站距离远近进行分组的方案：布置n 个UWB 基站（n 为偶数），计算它们两两之间的距离；选取距离最小的两个基站，令其为第1 个和第2 个基站；再在剩下的n-2个基站中选取距离最小的两个基站， 令其为第3 个和第4 个基站；按照这个规则选取下去，直至最后剩下两个基站，令其为第n-1 个和第n 个基站。 此时，基于TDOA 的UWB 距离差量测方程为：

采用文献[5]中的非线性系统双未知输入自校准滤波方法，对状态方程式（1）～式（13），量测方程式（14）～式（19）和式（30）进行滤波，即为基于TDOA 的车辆自校准导航方法。 滤波时，可选取式（30）所示的n/2 个距离差中绝对值最小的两个，直接令其未知输入dri（i+1），k=0。 也可以采用文献[5]中的两步自校准滤波方法来进一步提高滤波的鲁棒性和精度。

4 基于RSSI 的车辆自校准导航方法

基于接收信号强度指示（RSSI）的车辆自校准导航方法中采用的惯性导航自校准方程仍然为式 （1）～式（19），下面进一步给出RSSI 量测方程。

设第k 次（加速度计和陀螺仪）采样时，在位置（xk，yk）处能够接收到n 个热点（AP）发出的信号，则基于RSSI 的量测方程为[3]：

式中：Si，k为第k 次（加速度计和陀螺仪）采样时来自第i个AP 的RSSI，（xk，yk） 为此时导航坐标系下车辆质心的位置坐标，（Xi，Yi）为第i 个AP 的位置坐标，r0i为参考距离，S0i为参考距离处的RSSI，ηi为信号强度的路径损耗系数，dsi，k为未知输入（量测误差、非视距误差等）。 当第i 个AP发出的信号在视距环境中传播时，直接令dsi，k=0，且这样的AP 不少于2 个。ξsi，k为零均值服从正态分布的噪声，即ξsi，k～N（0，σs2i，k）。 考虑到采样频率的不同，设加速度计和陀螺仪的采样频率是RSSI 信号的采样频率的m 倍， 则当k是m 的正整数倍时，滤波的状态方程为式（1）～式（13），量测方程为式（14）～式（19）和式（35）；当k 不是m 的正整数倍时，滤波的量测方程中将不包含RSSI 量测方程式（35）。

同样，采用文献[5]中的非线性系统双未知输入自校准滤波方法进行滤波，也可以采用文献[5]中的两步自校准滤波方法来进一步提高滤波的鲁棒性和精度。

5 月球基地自校准定位方法

文献[3]给出了基于RSSI 的室内自校准定位方法，该方法也可用于基于TDOA 的自校准定位方法。 下面仍以UWB 为例进行讨论。

设在位置（x，y）处能够接收到n 个UWB 信号，则其自校准距离量测方程为：

6 实例

现在北京航空航天大学新主楼20m×30m 的一个长方形走廊内进行导航实验。 在走廊的四个角和四条边中点布置了8 个WiFi AP。 操控小车沿长方形走廊行驶一圈，实时记录陀螺仪、加速度计数据和接收到的每个WiFi AP 的三个信号强度平均值。

采用本文方法（自校准秩滤波、自校准无迹滤波）和传统方法（扩展卡尔曼滤波、无迹卡尔曼滤波）进行计算。计算150 个定位点的导航精度并绘于图1，且计算其平均值列于表1。

图1 导航精度比较

表1 平均导航精度比较

从图1 可以看到， 本文方法的导航精度比传统方法的导航精度高且稳定， 而传统方法的导航精度随时间的增加而迅速下降。从表1 可以看到，自校准秩滤波的平均导航精度比扩展卡尔曼滤波的平均导航精度提高了5.8倍，比无迹卡尔曼滤波的平均导航精度提高了4.2 倍，这说明本文方法能够比传统方法显著提高导航精度。 自校准无迹滤波的平均导航精度比无迹卡尔曼滤波的平均导航精度提高了3.5 倍， 这说明自校准滤波能显著提高导航精度。而因为秩采样属于最佳采样，所以自校准秩滤波的导航精度比自校准无迹滤波的导航精度要略高。

7 结论

（1） 提出了车辆自校准导航方法， 包括基于TOA 和TDOA 的车辆自校准导航方法以及基于RSSI 的车辆自校准导航方法，可以实现月球基地车辆的高精度导航。

（2）给出了惯性导航的自校准方程，包括位置坐标、速度、加速度、角速度、角度等的自校准状态方程和自校准量测方程，能够有效地对模型误差、陀螺仪和加速度计的漂移等进行实时修正。

（3）建立了UWB 的距离差量测方程，给出了基站与标签之间距离差的设计方案及其协方差矩阵， 能够对量测误差和非视距误差等进行实时自校准。

（4） 建立了月球基地自校准定位方法， 包括基于TDOA 的自校准定位方法和能够将TDOA 与RSSI 相融合的自校准定位方法，可进行高精度定位。

（5） 本文的月球基地车辆自校准导航定位方法也可用于室内（如地下车库等）的车辆导航定位。