CN103363991B - 一种适应月面崎岖地形的imu与测距敏感器融合方法 - Google Patents

Abstract

一种适应月面崎岖地形的IMU与测距敏感器融合方法，首先利用惯性导航和测距敏感器测量信息分别解算着陆器的高度；然后比较一段时间内惯性导航和测距敏感器分别获得的着陆器高度变化，通过事先设计的随高度变化的检测门限来判断测距敏感器的有效性，如果一致则确认测距敏感器测量有效，可引入修正；修正时，以测距敏感器计算高度和惯性导航解算高度之差作为新息，使用随高度变化的最优滤波系数矩阵来修正惯性导航位置和速度。本发明能够大大提高测距信号引入修正的准确性，降低未知不确定地形对导航系统的影响，保证导航融合的最优性，确保着陆导航的精确性和安全性。

Description

一种适应月面崎岖地形的IMU与测距敏感器融合方法

技术领域

本发明涉及一种适应月面崎岖地形的IMU与测距敏感器融合方法，属于月球探测自主导航、制导和控制领域。

背景技术

月球软着陆过程时间短、动态大，需要采用自主的制导、导航和控制方法。月球着陆的自主导航以惯性导航为基础，但是惯性导航是一种外推式的导航方式，其误差会逐步增大；另外惯性导航只能提供绝对的位置、速度计算结果，而月面存在起伏，因此惯性导航不能提供相对月面的高度、速度，不能满足着陆任务的需要。因此，一般月球软着陆的导航都要引入测距修正。测距敏感器能够提供着陆器沿波束方向到月面的斜距测量，是一种相对测量，能够抑制惯性导航的发散，并提供相对月面的高度。

测距修正的基本原理仍然是滤波，但工程应用中主要有两大问题，一是测距信号在修正前的有效性检测判断方法，二是滤波过程中系数矩阵的选取。对于第一个问题，美国的阿波罗飞船采用的是将测距敏感器折算的高度测量与惯性导航给出的高度测量直接比较的方法。由于两个方法获得的高度基准不同（惯性导航是相对参考月面的，测距是相对真实月面的），因此直接比较会存在地形偏差的问题，导致检测门限偏大，漏检率较高。对于第二个问题，阿波罗飞船采用变系数滤波的方法，滤波系数建模为随高度线性变化的函数。根据现代估计理论，最优滤波系数的选择与系统和测量噪声相关，对于着陆过程来说，它取决于加速度计、测距敏感器的测量噪声和地形起伏的不确定性，与高度的变化关系是非线性的，因此直接用线性拟合的作法会丧失最优性。

发明内容

本发明所解决的技术问题：针对月球软着陆过程，提出一种适应崎岖地形的IMU与测距敏感器融合方法，该方法采用不同于以往的测距信号有效性判别方法，来降低该方法受到地形偏差和不确定性的影响；同时采用的变系数滤波系数矩阵是根据最优滤波计算并拟合而来，在降低计算量的同时保证了导航滤波的最优性。

本发明所采用的技术方案为：使用惯性导航外推获得着陆器高度，利用测距敏感器斜距信息进行惯性导航高度修正。在测距敏感器数据引入修正前，将惯性导航和测距敏感器在相同时间间隔内高度变化量的解算结果进行比较，从而进行测距有效性判断。在修正过程中采用的可变的滤波系数矩阵。该矩阵是事先根据不同高度下的敏感器测量噪声和地形随机起伏使用卡尔曼滤波计算而来，并拟合为随高度变化的非线性函数。实现步骤如下：

（1）惯性导航

利用运动学方程，使用加速度计和陀螺测量的数据进行着陆器相对月心惯性系（原点在月心，三个坐标轴指向惯性空间特定方向）的位置、速度和姿态的外推。

（2）惯性导航解算高度

根据装订的参考月面半径数值，利用步骤（1）中惯性导航给出的相对月心的位置信息计算出相对参考月面的高度。

（3）测距敏感器解算高度

将着陆器上安装的测距敏感器获得的波束方向相对月面的斜距信息，通过步骤（1）中由惯性导航提供的着陆器姿态，解算出相对实际月面高度的测量值。

（4）有效性判断

根据相邻两个周期惯性和测距敏感器的高度输出，分别计算着陆器的高度变化。将步骤（2）和步骤（3）中分别由惯性导航和测距敏感器获得的高度变化数值进行比较，若小于预先设定的检测门限则判断测距信息有效，引入修正，转入步骤（5）。

（5）滤波修正

若测距信息有效，则对步骤（3）和步骤（4）中由惯性导航和测距敏感器获得的高度做差，并利用预先设定的最优滤波系数对惯性导航位置、速度进行滤波修正。最优滤波系数矩阵可设计为高度的函数，它根据不同高度下的测量噪声和地形随机起伏使用卡尔曼滤波求取，并通过非线性曲线拟合后装订在星载计算机上。

所述步骤（4）中通过比较一段时间内由惯性导航和测距敏感器分别获得的着陆器高度变化来进行测距信号有效性检测。

所述步骤（4）中的检测门限设计为着陆器高度的函数，并预先装订在星载计算机上。

本发明与现有技术相比的优点在于：本发明通过比较测距和惯性导航分别获得的同一时间段内的着陆器高度变化进行测距有效性检测，从而剔除月面基准误差对检测门限的影响，降低漏检率；滤波系数矩阵根据不同高度下的敏感器测量噪声和地形随机起伏大小使用卡尔曼滤波计算，并通过非线性曲线进行拟合，这样在减小计算量的同时还能够保证滤波的最优性。

附图说明

图1是IMU和测距敏感器融合方法的流程图；

图2是着陆过程测距敏感器高度解算和检测的原理图；

图3是着陆过程崎岖月表地形建模示意图；

图4是测距有效性检测的门限随高度变化曲线；

图5是滤波系数矩阵随高度变化曲线；

图6是仿真中地形随机起伏随时间的变化曲线；

图7是融合之后导航高度误差的变化曲线；

图8是融合之后导航垂直速度误差的变化曲线。

具体实施方式

着陆器处于月球软着陆动力下降过程。着陆器上装备有陀螺和加速度计，下降和减速由制动发动机完成，姿态采用喷气控制。

如图1所示，本发明具体计算过程如下：

（1）惯性导航

惯性导航是一种根据陀螺和加速度计测量值，利用运动学方程进行外推的导航方式。月心惯性系（用符号i表示，其原点在月心，X、Y、Z三个轴指向惯性空间特定方向）下着陆器的运动学方程为

$\left\{\begin{array}{c}{\stackrel{\·}{r}}^i=v^i\\ {\stackrel{\·}{v}}^i=C_b^i\left(q\right)f^b+g^i\\ \stackrel{\·}{q}\frac{1}{2}E\left(q\right){\mathrm{\ω}}^b\end{array}\right.---\left(1\right)$

其中，rⁱ和vⁱ分别表示惯性系下的位置和速度；q是着陆器本体相对惯性系的姿态四元数，有q=[q₁,q₂,q₃,q₄]^T；gⁱ是惯性系下的引力加速度；f^b和ω^b分别是表示在着陆器本体坐标系（用符号b表示，原点在着陆器质心，三个坐标轴分别平行于着陆器的三个惯量主轴）下比力（非引力加速度）和角速度。

是由本体系到惯性系的姿态变换矩阵，它可以通过四元数求解，即

$C_b^i\left(q\right)={\left[\begin{array}{ccc}{q}_1^2-q_2^2-q_3^2+q_4^2& 2(q_1{q}_2+q_3{q}_4)& 2(q_1{q}_3-q_2{q}_4)\\ 2(q_1{q}_2-q_3{q}_4)& -q_2^1+q_2^2{-q}_3^2+q_4^2& 2(q_2{q}_3+q_1{q}_4)\\ 2(q_1{q}_3+q_2{q}_4)& 2(q_2{q}_3-q_1{q}_4)& -q_1^2-q_2^2+q_3^2+q_4^2\end{array}\right]}^T---\left(2\right)$

E(q)表示与q相关的矩阵，有

$E\left(q\right)=\left[\begin{array}{ccc}{q}_4& {-q}_3& q_2\\ q_3& q_4& {-q}_1\\ {-q}_2& q_1& q_4\\ {-q}_1& {-q}_2& {-q}_3\end{array}\right]---\left(3\right)$

惯性导航解算是利用对(1)式积分进行的。假设t_k-1时刻，着陆器的位置、速度和姿态的估计值为和而陀螺获得了从t_k-1到t_k时刻的平均角速度测量加速度计获得了t_k-1到t_k时刻的平均比力测量那么就可以通过积分外推出着陆器在t_k时刻的位置、速度和姿态。积分外推的算法很多，这里给出了最简单的一阶方法，即

$\left\{\begin{array}{c}{\hat{v}}_k^i={\hat{v}}_{k-1}^i+C_b^i\left({\hat{q}}_{k-1}\right)\·({\tilde{f}}_{k-1}^b+{\hat{g}}_{k-1}^i)\·\mathrm{\Δt}\\ {\hat{r}}_k^i={\hat{r}}_{k-1}^i+\frac{1}{2}({\hat{v}}_{k-1}^i+{\hat{v}}_k^i)\·\mathrm{\Δt}\\ {\hat{q}}_k={\hat{q}}_{k-1}+\frac{1}{2}E\left({\hat{q}}_{k-1}\right)\·{\widetilde{\mathrm{\ω}}}_{k-1}^b\·\mathrm{\Δt}\end{array}\right.---\left(4\right)$

对于月球软着陆任务来说，引力场可以选用最简单的二体模型，那么可如下计算

${\hat{g}}_{k-1}^i={-\mathrm{\μ}}_m\frac{{\hat{r}}_{k-1}^i}{{\left|\right|{\hat{r}}_{k-1}^i\left|\right|}^3}---\left(5\right)$

其中，μ_m为月球引力常数。

（2）惯性导航解算高度

星载计算机上存储月球的参考半径R_m，则相对月面的高度可如下计算：

${\hat{h}}_k=\left|\right|{\hat{r}}_k^i\left|\right|-R_m---\left(6\right)$

（3）测距敏感器解算高度

测距敏感器固定安装在着陆器上，具有确定的波束指向。它向该方向发射无线电波或者激光光波，并通过反射回波测量着陆器沿该波束指向方向相对实际月面的距离，称为斜距。通过该测量可以利用惯性导航姿态信息解算高度，方法如下。

设测距敏感器在着陆器本体系的波束指向为p^b，利用惯性导航姿态数据，可以将它转换到惯性系下。

${\hat{p}}^i=C_b^i\left({\hat{q}}_k\right)p^b---\left(7\right)$

再利用惯性导航给出的着陆器位置信息，可以求出波束方向与月心方向的夹角的余弦值为

$\cos {\mathrm{\&gamma;}}_k=<{\hat{p}}^i\&CenterDot;-\frac{{\hat{r}}_k^i}{{\left|\right|\hat{r}}_k^i\left|\right|}>---\left(8\right)$

近似认为从测距敏感器波束方向与月面的交点到着陆器星下点的月表为平面，根据在t_k时刻的斜距测量值可以计算出着陆器的高度为

${\tilde{h}}_k={\tilde{p}}_k\cos {\mathrm{\&gamma;}}_k---\left(9\right)$

（4）有效性判断

利用测距敏感器计算的高度信息对惯性导航进行修正之前，必须对测距敏感器高度信息的有效性进行判断。如果直接用测距敏感器解算的高度与惯性导航解算的高度相比较，则会引入地形和月球参考平面绝对数值的误差，检测门限不能太小，从而使得漏检率大大增加。

本发明采用新的检测方法。分别由测距敏感器和惯性导航获得同一时间区间内的高度变化，然后比较两种方法获得的高度变化数值，小于检测门限时则判断测距信号有效，并引入修正。由于相邻采样时间点内着陆器飞行轨迹下的地形起伏变化有限，因此这种方法可以剔除掉较大的地形和参考平面绝对数值误差，检测门限取值变小，从而降低漏检率。

有效性判断的方法如图2所示。

按照步骤(1)-(3)，设t_k-1时刻和t_k时刻，惯性导航获得的高度数值为和对应时间测距敏感器获得的高度测量为和那么可分别计算出

[t_k-1,t_k]时间区间内的着陆器高度变化

$\left\{\begin{array}{c}\mathrm{\&Delta;}{h}_{\mathrm{ranger}}={\tilde{h}}_k-{\tilde{h}}_{k-1}\\ \mathrm{\&Delta;}{h}_{\mathrm{ins}}={\hat{h}}_k-{\hat{h}}_{k-1}\end{array}\right.---\left(10\right)$

设定检测门限G_h。如果|Δh_ranger-Δh_ins|≤G_h，则测距信号有效，置标志Flag=1；否则测距信号无效，置标志Flag=0。

检测门限G_h的设置应考虑测距敏感器的误差、惯性导航在[t_k-1,t_k]时间段内高度计算的误差变化以及地形在[t_k-1,t_k]时间段内的变化，大于三者之和。一般说来，对于月球软着陆来说，高度越低测距敏感器的精度也越高；而且着陆器随着高度的降低，下降速度也越来越小，惯性导航计算的固定时间间隔内的高度误差变化也会越来越小；另外随着高度的降低，固定时间间隔内着陆器飞过的距离也越来越小，对应地形的变化也越来越小。因此检测门限的取值应与高度相关，且随着高度的降低门限逐渐变小。例如，取检测门限为高度的线性函数，

$G_h\&le;G_{h0}+w_h\left|\hat{h}\right|---\left(11\right)$

其中，G_h0和w_h均是常数，具体数值需根据实际情况设计。

（5）滤波修正

由于测距敏感器有测量误差，同时还存在地形的起伏，因此测距敏感器解算的高度相比实际的着陆器距星下点平均月表的高度也存在偏差。

$\tilde{h}=h+\mathrm{\&delta;}{h}_{\mathrm{ranger}}+{\mathrm{\&delta;h}}_n---\left(12\right)$

h是相对星下点平均月面的真实高度，δh_ranger是测距敏感器引入的高度误差，δh_n是相对星下点平均月面的地形随机起伏。

这样就可以建立高度通道滤波的状态方程和测量方程

$\left[\begin{array}{c}\stackrel{\&CenterDot;}{h}\\ {\stackrel{\&CenterDot;}{v}}_{\mathrm{ver}}\end{array}\right]=\left[\begin{array}{cc}0& 1\\ 0& 0\end{array}\right]\left[\begin{array}{c}h\\ v_{\mathrm{ver}}\end{array}\right]+\left[\begin{array}{c}0\\ 1\end{array}\right]({\tilde{f}}_{\mathrm{ver}}+g)-\left[\begin{array}{c}0\\ \mathrm{\&delta;a}\end{array}\right]$ (13)

$\tilde{h}=\left[\begin{array}{cc}1& 0\end{array}\right]\left[\begin{array}{c}h\\ v_{\mathrm{ver}}\end{array}\right]+({\mathrm{\&delta;h}}_{\mathrm{ranger}}+{\mathrm{\&delta;h}}_n)$

其中，v_ver是垂直速度，是加速度计测量比力在垂直方向的分量，g是引力加速度的大小（近似重力加速度），δa是加速度计的测量误差。

式(13)描述的是一个定常系统，因此可以转换为常系数滤波。假设高度和垂直速度的误差为δh和δv_ver，则误差的估算方法为

$\left[\begin{array}{c}\mathrm{\&delta;h}\\ {\mathrm{\&delta;v}}_{\mathrm{ver}}\end{array}\right]=k_h(h_{\mathrm{ranger}}-h)---\left(14\right)$

其中，K_h是2×1的矩阵，它可以选择为对式(13)进行卡尔曼滤波时的稳态增益矩阵，如有必要，例如为了提高收敛速度，也可在稳态值基础上进行适当的放大。

需要说明的是，在式(13)所描述系统的测量噪声中，测距敏感器引起的高度误差δh_ranger随高度的降低逐渐减小，δh_n随着高度的降低（伴随速度的降低）也会逐渐减小，因此系统测量噪声的方差可以建模为高度的函数。那么滤波增益阵K_h也应为高度的函数。因此应该针对有限个不同的高度h，确定测量噪声的方差大小，再利用卡尔曼滤波，计算对应h下的增益矩阵取值；之后根据所获得的h-K_h数据，用非线性函数进行拟合。

由于只有在测距有效时才引入修正，因此t_k时刻实际的修正方法为

$\left[\begin{array}{c}{\mathrm{\&delta;h}}_k\\ {\mathrm{\&delta;v}}_{\mathrm{ver},k}\end{array}\right]=\mathrm{Flag}\&CenterDot;K_h({\tilde{h}}_k-h_k)---\left(15\right)$

然后重置惯性导航的r_k和v_k

$r_k\&LeftArrow;r_k+{\mathrm{\&delta;h}}_k\frac{r_k}{\left|\right|r_k\left|\right|}---\left(16\right)$ $v_k\&LeftArrow;v_k+{\mathrm{\&delta;v}}_{\mathrm{ver},k}\frac{r_k}{\left|\right|r_k\left|\right|}$

需要说明的是，由于垂直速度误差是通过高度测量误差计算的，在高度修正还未平稳时，速度修正的跳动会比较大，因此式(16)中，速度修正应稍微滞后于高度修正若干周期。

随着时间的递推，重复上述步骤，就完成了融合IMU和测距敏感器的导航计算。

仿真分析。以月球着陆器为例对本发明提出的适应月面崎岖地形的IMU与测距敏感器融合方法进行验证。

加速度计的测量噪声δa的均方差取为5×10^-4f，f是加速度计敏感器的比力大小。

测距敏感器的测量误差模型为

$\mathrm{\&Delta;\&rho;}=\left\{\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&rho;}}_0,\mathrm{\&rho;}<{\mathrm{\&rho;}}_{\mathrm{threhold}}\\ \mathrm{\&Delta;}{\mathrm{\&rho;}}_1+k_{\mathrm{\&rho;}}\mathrm{\&rho;},\mathrm{else}\end{array}\right.---\left(17\right)$

相关参数取值假设为

$\left\{\begin{array}{c}\mathrm{\&Delta;}{\mathrm{\&rho;}}_0=0.4\\ \mathrm{\&Delta;}{\mathrm{\&rho;}}_1=0\\ k_{\mathrm{\&rho;}}=0.0033\\ {\mathrm{\&rho;}}_{\mathrm{threhold}=121}\end{array}\right.$

月表地形可用趋势项叠加马尔科夫过程来建模，如图3所示。令月球相对基准面的高度偏差为h_L；地形坡度用α表示，令s为着陆器星下点与参考着陆点的距离；地形高度噪声假定为h_n，并建模为马尔柯夫过程，相关距离为β，驱动白噪声为w。于是，着陆区地形模型可以表达为

$\left\{\begin{array}{c}{h}_{\mathrm{ter}}=h_L+s\tan \mathrm{\&alpha;}+h_n\\ \frac{{\mathrm{dh}}_n}{\mathrm{ds}}=-\frac{h_n}{\mathrm{\&beta;}}+\frac{1}{\mathrm{\&beta;}}w\end{array}\right.---\left(18\right)$

仿真中相关参数取值为

根据着陆过程的参考轨迹、惯性导航的误差变化以及测距敏感器的测量误差，可以计算出检测门限随高度的变化曲线如图4所示。它可以用线性函数进行拟合。拟合后的检测函数为

G_h≤17.609+0.0565|h_ins|

根据地形和敏感器模型，按照卡尔曼滤波稳态增益计算出的滤波系数矩阵随高度变化的曲线如图5所示。

用非线性函数来代替上述K_h的变化曲线，可取

$w_h^{*}=\left[\begin{array}{c}426.2\\ 6.5\end{array}\right],h^{*}=\left[\begin{array}{c}1901.8\\ 120.7\end{array}\right]$

采用本发明提出的融合方法进行导航计算，其中地形变化中随机起伏随时间的变化如图6所示，而融合前后导航系统给出的高度和垂直速度计算结果与真值的差分别如图7和图8所示，其中高度修正于328s开始，速度修正于360s开始，修正引入之前只使用纯惯性导航。很明显，融合测距信息后，导航系统在垂直通道的位置、速度误差逐步减小，能够适应崎岖地形的影响。

本发明未详细阐述部分属于本领域公知技术。

Claims (3)

1.一种适应月面崎岖地形的IMU与测距敏感器融合方法，其特征在于实现步骤如下：

(1)惯性导航

利用运动学方程，使用加速度计和陀螺测量的数据进行着陆器相对月心惯性系的位置、速度和姿态的外推；

(2)惯性导航解算高度

根据装订的参考月面半径数值，利用步骤(1)中惯性导航给出的月心惯性系的位置信息计算出相对参考月面的高度；

(3)测距敏感器解算高度

将着陆器上安装的测距敏感器获得的波束方向相对月面的斜距信息，通过步骤(1)中由惯性导航提供的着陆器姿态，解算出相对实际月面高度的测量值；

(4)有效性判断

根据相邻两个周期惯性导航和测距敏感器的高度输出，分别计算着陆器的高度变化，将步骤(2)和步骤(3)中分别由惯性导航和测距敏感器获得的高度数值进行比较，若小于预先设定的检测门限则判断测距信息有效，引入修正，转入步骤(5)；

(5)滤波修正

若测距信息有效，则对步骤(2)和步骤(3)中由惯性导航和测距敏感器获得的高度做差，并利用预先设定的最优滤波系数对惯性导航位置、速度进行滤波修正；最优滤波系数设计为着陆器高度的函数，它根据不同高度所对应的测距敏感器噪声和地形起伏大小通过卡尔曼滤波稳态增益求取，并经非线性曲线拟合后装订在星载计算机上。

2.根据权利要求1所述的一种适应月面崎岖地形的IMU与测距敏感器融合方法，其特征在于：所述步骤(4)中通过比较一段时间内由惯性导航和测距敏感器分别获得的着陆器高度变化来进行测距信号有效性检测。

3.根据权利要求1所述的一种适应月面崎岖地形的IMU与测距敏感器融合方法，其特征在于：所述步骤(4)中的检测门限设计为着陆器高度的函数，并预先装订在星载计算机上。