CN103900611B - 一种惯导天文高精度复合两位置对准及误差标定方法 - Google Patents

Abstract

一种惯导天文高精度复合两位置对准及误差标定方法，它有五大步骤：步骤一：将着陆器所确定的着陆位置作为火星车的初始位置信息；步骤二：根据火星车的初始位置以及天文导航子系统输出的惯性姿态矩阵确定火星车的初始姿态矩阵；步骤三：建立火星车两位置对准的系统状态模型和量测模型；步骤四：火星车通过自转实现两位置对准；步骤五：通过修正火星车的初始位姿信息实现对高精度初始对准的要求。本发明充分发挥了捷联惯导子系统和天文导航子系统各自的优势，并且还可以应用于其它巡视探测器，具有广阔的应用前景。

Description

一种惯导天文高精度复合两位置对准及误差标定方法

技术领域

本发明涉及一种惯导天文高精度复合两位置对准及误差标定方法，属于惯性导航技术领域。

背景技术

在火星探测中，火星车是在火星表面进行巡视探测的航天器，是火星探测必不可少的工具和手段。通过火星车在火星表面的探测活动，人们可以获取火星表面的环境资料，收集火星表面的岩石和土壤标本，从而完成科学探测任务。火星车通常借助地面站通过无线电测控进行导航控制，但由于地球和火星的自转和公转运动以及所带来的地面站和火星车相对地理位置的变化等，有时火星车会处于地面站无法测控的区域内；同时，火星车与地面的通讯也存在时延和带宽的问题。因此，火星车利用自身携带的测量设备进行自主导航控制已经成为重要的研究方向。

目前，任何一种单一的导航方式都无法同时满足火星车自主导航的要求，突破这一难点的最佳方案就是采用多传感器信息融合的组合导航技术。由于捷联惯导系统(SINS)具有完全自主、快速、动态性能好、短时间内精度高等优点，因此，适用于火星车的组合导航系统中一般均包括捷联惯导系统。但只要有捷联惯导系统参与的组合导航系统，就要首先考虑捷联惯导系统的初始对准问题，因为火星车初始位置和姿态的确定是运动过程中火星车定位的参考基准，其精度直接影响着火星车在整个运动过程中的定位精度。目前在初始定位算法方面的研究甚少，可进行的初始位置的确定方法有无线电跟踪测量和基于路标的自主位姿确定方法。无线电测控定位方法的优点是全局绝对定位，缺点是无法实现实时自主定位。勇气号和机遇号火星车着陆后，用其无线电系统同地球跟踪站直接通信，根据第2个火星日至第4个火星日地球测控站直接得到的双向多普勒信号以及火星车与奥德赛轨道器在两次通信窗口得到的双向多普勒信号，得到了着陆器的位置(即火星车的初始位置)；基于路标的自主位姿确定方法即利用陨石坑作为导航路标进行探测器状态估计，勇气号与机遇号着陆火星的过程中成功匹配了相邻两帧图像中的路标点，对着陆器相对火星表面的水平速度进行了估算，但导航路标位置的精确获取是保证该方法成功应用的重要前提。

为了保证捷联惯导系统的初始对准精度以及导航精度，必须减小惯性器件的测量误差。因此，对捷联惯导系统进行误差标定与补偿是另一个需要考虑的问题。捷联惯导系统误差标定技术根据场所的不同，可分为实验室标定和外场标定。实验室标定是指在实验室内利用惯性测试设备标定系统的参数，外场标定则是将系统安装在载体上后进行的标定。由于通过实验室标定出的各项误差系数并不是固定不变的，包括陀螺仪常值误差和加速度计常值误差。这些参数随着系统的使用或存放时间的推移而变化。因此，在惯性测量组件的使用现场对其进行各项误差系数标定，不仅可以减小惯性器件测量误差对捷联惯导系统初始对准精度的影响，还能提高捷联惯导系统的使用精度。

为了满足火星车高精度自主导航的要求，本专利申请提出了一种惯导天文高精度复合两位置对准及误差标定方法。

发明内容

针对现有技术中存在的问题，本发明提供一种惯导天文高精度复合两位置对准及误差标定方法，它利用着陆器所确定的着陆位置以及天文导航系统提供的高精度惯性姿态信息，得到火星车的初始位姿信息，在此基础上，利用天文导航系统辅助火星车捷联惯导系统进行两位置对准，并对火星车的初始位姿信息进行修正，最终达到高精度初始对准。

本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，包括捷联惯导子系统、天文导航子系统、初始位姿确定单元、惯导姿态量测信息构造单元和组合导航滤波器；它们之间的关系是：天文导航子系统将惯性姿态矩阵提供给初始位姿确定单元和组合导航滤波器，初始位姿确定单元将确定的初始位姿信息分别提供给惯导姿态量测信息构造单元和捷联惯导子系统，捷联惯导子系统将解算出的姿态矩阵和速度误差信息分别提供给惯导姿态量测信息构造单元和组合导航滤波器，惯导姿态量测信息构造单元将确定的惯性姿态矩阵提供给组合导航滤波器，组合导航滤波器为初始位姿确定单元提供初始位姿的估计误差。

所述捷联惯导子系统包括惯性测量组件和导航解算单元；惯性测量组件测得火星车相对于惯性空间的角速度和比力，将得到的角速度和比力信息传送给导航解算单元；导航解算单元根据惯性测量组件传输的信息通过力学编排算法实时计算出火星车的速度误差和姿态矩阵；

所述天文导航子系统包括大视场星敏感器和多矢量定姿单元；大视场星敏感器同一时刻能够观测得到三颗及三颗以上恒星的星光矢量信息，并将得到的观测信息提供给多矢量定姿单元；多矢量定姿单元对接收到的星光矢量信息进行处理，得到火星车相对惯性空间的姿态矩阵；

所述初始位姿确定单元包括初始位置确定模块和初始姿态确定模块；初始位置确定模块将着陆器所确定的着陆位置作为火星车的初始位置信息，并将火星车的初始位置信息提供给初始姿态确定模块；初始姿态确定模块对接收到的火星车的初始位置信息、火星车的惯性姿态矩阵以及当前的导航时间进行处理，得到火星车的初始姿态矩阵；

所述惯导姿态量测信息构造单元为捷联惯导子系统确定从火星赤道惯性坐标系转换到载体坐标系的惯性姿态矩阵；

所述组合导航滤波器以SINS误差方程为状态方程，以惯导姿态量测信息构造单元与天文导航子系统分别提供的惯性姿态矩阵之差以及捷联惯导子系统提供的速度误差作为观测量，得到火星车初始位姿误差的估计值以及惯性测量组件零偏的估计值；

本发明一种惯导天文高精度复合两位置对准及误差标定方法，具体包括以下步骤：

步骤一：将着陆器所确定的着陆位置作为火星车的初始位置信息

火星车的初始位置也即着陆器的着陆位置，着陆器着陆位置的确定主要依赖于着陆段导航的精度，目前着陆段自主导航的发展趋势为：1)精确着陆导航，其着陆误差直径范围小于10米；2)中等精度着陆导航，其着陆误差直径范围在10米与1千米之间；3)低精度着陆导航，其着陆误差直径范围大于1千米；

以CCD相机作为传感器的视觉导航系统，具有体积小、重量轻、低能耗、视场宽、易于搭载等显著优势，更为重要的是，经过多年的发展，视觉导航算法已经能够通过提取图像中的信息较为精确地确定相机的位置、速度和姿态信息；因此，在火星着陆器着陆过程中，通过融合惯导系统的测量值以及CCD相机提供的位置、速度和姿态信息，就可以实现火星着陆器的精确着陆导航；

在火星着陆器的着陆过程中，当着陆相机开始工作以后，首先通过提取CCD相机所拍摄的火星表面图像上的地标点，并与火星在轨卫星所形成的数字高程地图进行地标点匹配，可以估计出着陆器的绝对位置及姿态信息；由于火星在轨卫星所拍摄的图像的分辨率有限，当火星着陆器接近火星表面时，地标点无法被准确地提取，可以通过跟踪图像序列中特征点的变化，估计出着陆器的速度信息，进而获得着陆器位置和姿态的相对变化信息；在整个着陆过程中，通过利用组合导航滤波器融合惯导系统与视觉导航系统分别提供的位置、速度和姿态信息，能够有效地修正惯导系统的累积误差以及陀螺仪和加速度计的常值漂移，从而极大地提高导航精度、满足火星车精确着陆任务的需要；

约翰逊等通过发射试验火箭模拟探测器的着陆过程对算法进行验证分析；试验结果表明，基于视觉匹配和惯性测量相结合的导航方法的速度误差为0.16m/s，位置误差为6.4m，能够满足未来火星精确着陆探测任务的需要；

通过以上的分析可以看出，将着陆器的着陆位置作为火星车的初始位置时，可以认为火星车初始经纬度的确定误差均为1角秒，即δλ＝1″，δL＝1″，其中，λ代表火星车的经度，L代表火星车的纬度；

步骤二：根据火星车的初始位置以及天文导航子系统输出的惯性姿态矩阵确定火星车的初始姿态矩阵；

新型大视场星敏感器的出现，使得同时观测三颗及三颗以上的导航星成为可能，在不需要任何外部基准信息的前提下，可直接精确地(1″～7″)提供运载体相对于惯性空间的姿态矩阵，且精度在全程保持稳定；因此，根据大视场星敏感器的输出便可获得高精度的火星车载体坐标系(b系)相对于火星赤道惯性坐标系(i系)的姿态矩阵根据当前的导航时间t能够获得从火星赤道惯性坐标系(i系)转换到火星星固坐标系(m系)的方向余弦矩阵由于：

${\tilde{C}}_i^b=C_n^b{C}_m^n{C}_i^m---\left(1\right)$

其中：

$C_m^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}\sin L& -\sin \mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& \sin \mathrm{\λ}\cos L& \sin L\end{array}\right]$

所以，

火星车的初始姿态实际上就是导航坐标系(n系)和火星车载体坐标系(b系)之间的姿态转换矩阵

$C_n^b=\left[\begin{array}{ccc}c\mathrm{\γ}c\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\γ}s\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& -s\mathrm{\γ}c\mathrm{\θ}\\ c\mathrm{\θ}s\mathrm{\ψ}& c\mathrm{\θ}c\mathrm{\ψ}& s\mathrm{\θ}\\ s\mathrm{\γ}c\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -s\mathrm{\γ}s\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& c\mathrm{\γ}c\mathrm{\θ}\end{array}\right]---\left(2\right)$

其中，θ，γ，ψ分别代表火星车的俯仰角、横滚角和偏航角，cθ表示的是cos(θ)，sθ表示的是sin(θ)，其它的以此类推；

根据式(2)可得：

θ＝arcsin(C₂₃) (3)

γ_主＝-arctan(C₁₃/C₃₃) (4)

ψ_主＝arctan(C₂₁/C₂₂) (5)

由以上三式得出的是反三角函数的主值；在实际应用中，俯仰角θ定义在(-π/2,π/2)区间，横滚角γ定义在(-π,π)区间，偏航角ψ定义在(0,2π)区间；这样，θ的主值即为真值，而γ和ψ的真值还需通过如下附加的判式来决定其在哪个象限：

步骤三：建立火星车两位置对准的系统状态模型和量测模型；

a.构建系统状态模型；

以地理坐标系(t系)作为惯性导航解算的基本坐标系，则惯导系统误差模型如下所示：

其中，为惯导的平台失准角，δVⁿ为速度误差，ε^b为陀螺仪常值漂移，为加速度计零偏；

因此，系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

其中，状态矢量系统的噪声矢量W(t)＝[w_gx w_gy w_gz w_ax w_ay w_az]^T，ω_gi(i＝x,y,z)、ω_ai(i＝x,y,z)分别为陀螺仪、加速度计随机白噪声；

b.构建系统量测模型；

根据当前的导航时间t和火星车的初始位置，并结合SINS的姿态矩阵有：

${\hat{C}}_i^b={\hat{C}}_n^b{\hat{C}}_m^n{C}_i^m---\left(10\right)$

考虑到对准误差及陀螺漂移等因素的影响，SINS数学平台系(n′系)与导航坐标系(n系)之间存在数学平台失准角向量有：

由于天文导航子系统测量的姿态精度较高，且其测量误差可以考虑为零均值的白噪声，所以天文导航子系统输出的惯性姿态矩阵可以认为是真实的姿态矩阵与星敏感器的量测白噪声阵V_s之和，即：

${\tilde{C}}_i^b=C_i^b+V_s---\left(12\right)$

将惯导姿态量测信息构造单元与天文导航子系统分别确定的方向余弦阵的差值记作姿态量测量Z_s，则由式(10)、式(11)以及式(12)可得：

将Z_s(3×3)展开成列向量Z_1(9×1)，并结合系统的状态向量X，可列写出量测方程：

Z₁＝H₁X+V₁ (14)其中，H₁＝[H₁₁ 0_9×3 0_9×3 0_9×3]为姿态量测矩阵，V₁为星敏感器的量测白噪声矩阵；

令则根据式(13)可得：

$H_{11}=\left[\begin{array}{ccc}{T}_{13}{C}_{21}-T_{12}{C}_{31}& T_{11}{C}_{31}-T_{13}{C}_{11}& T_{12}{C}_{11}-T_{11}{C}_{21}\\ T_{13}{C}_{22}-T_{12}{C}_{32}& T_{11}{C}_{32}-T_{13}{C}_{12}& T_{12}{C}_{12}-T_{11}{C}_{22}\\ T_{13}{C}_{23}-T_{12}{C}_{33}& T_{11}{C}_{33}-T_{13}{C}_{13}& T_{12}{C}_{13}-T_{11}{C}_{23}\\ T_{23}{C}_{21}-T_{22}{C}_{31}& T_{21}{C}_{31}-T_{23}{C}_{11}& T_{22}{C}_{11}-T_{21}{C}_{21}\\ T_{23}{C}_{22}-T_{22}{C}_{32}& T_{21}{C}_{32}-T_{23}{C}_{12}& T_{22}{C}_{12}-T_{21}{C}_{22}\\ T_{23}{C}_{23}-T_{22}{C}_{33}& T_{21}{C}_{33}-T_{23}{C}_{13}& T_{22}{C}_{13}-T_{21}{C}_{23}\\ T_{33}{C}_{21}-T_{32}{C}_{31}& T_{31}{C}_{31}-T_{33}{C}_{11}& T_{32}{C}_{11}-T_{31}{C}_{21}\\ T_{33}{C}_{22}-T_{32}{C}_{32}& T_{31}{C}_{32}-T_{33}{C}_{12}& T_{32}{C}_{12}-T_{31}{C}_{22}\\ T_{33}{C}_{23}-T_{32}{C}_{33}& T_{31}{C}_{33}-T_{33}{C}_{13}& T_{32}{C}_{13}-T_{31}{C}_{23}\end{array}\right]$

将捷联惯导子系统输出的速度误差作为观测量，并结合系统的状态向量X，可列写出量测方程：

$Z_2=\left[\begin{array}{c}{\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ {\mathrm{\δv}}_U\end{array}\right]=H_{2}X+V_2---\left(15\right)$

式中，H₂＝[0_3×3 I_3×3 0_3×3 0_3×3]为速度误差量测矩阵，V₂为速度误差的量测白噪声矩阵；

步骤四：火星车通过自转实现两位置对准；

火星车两位置对准的过程为：首先，火星车在起始位置处进行初始对准及误差标定，然后，让火星车在原地进行自转，通过积分陀螺仪的输出获得火星车姿态角的变化，当火星车的偏航角改变了180°以后，使火星车停止自转运动，然后在新的位置上继续进行对准过程和误差标定过程；

步骤五：通过修正火星车的初始位姿信息实现对高精度初始对准的要求；

由于火星车的初始位置存在误差δL,δλ，因此，根据火星车初始位置所确定的位置矩阵与真实的位置矩阵之间存在如下的关系：

$C_m^n=(I+\[\mathrm{\δ}P\×\]){\hat{C}}_m^n---\left(16\right)$

其中，δP＝[-δL δλ·cos L δλ·sin L]^T为位置误差向量。

根据式(12)和式(16)可知，天文导航子系统所确定的高精度惯性姿态矩阵可以表示为：

${\tilde{C}}_i^b=C_n^b{C}_m^n{C}_i^m+V_s=C_n^b(I+\[\mathrm{\δ}P\×\]){\hat{C}}_m^n{C}_i^m+V_s---\left(17\right)$

因此，火星车经过初始对准以后所确定的姿态矩阵与真实的姿态矩阵之间存在如下的关系：

$C_p^b=C_n^b(I+\[\mathrm{\δ}P\×\])---\left(18\right)$

火星车进行初始对准时，由于存在位置误差的影响，使得平台失准角的估计值与真实值之间存在一定的偏差：

$\begin{array}{c}{\mathrm{\Δ\φ}}_E={\widehat{\mathrm{\φ}}}_E-{\mathrm{\φ}}_E\\ {\mathrm{\Δ\φ}}_N={\widehat{\mathrm{\φ}}}_N-{\mathrm{\φ}}_N\\ {\mathrm{\Δ\φ}}_U={\widehat{\mathrm{\φ}}}_U-{\mathrm{\φ}}_U\end{array}---\left(19\right)$

其中，Δφ_E,Δφ_N,Δφ_U分别表示平台失准角的估计误差，分别表示平台失准角的估计值，φ_E,φ_N,φ_U分别表示平台失准角的真实值；

由于

因此

根据公式(18)和公式(21)可得：

因此

Δφ_U＝Δφ_N×tan L (23)

而水平方向上平台失准角的估计误差与加速度计零偏的估计误差之间存在如下的关系：

$\begin{array}{c}{\mathrm{\Δ\φ}}_E=\mathrm{\Δ}{\▿}_N/g\\ {\mathrm{\Δ\φ}}_N=-\mathrm{\Δ}{\▿}_E/g\end{array}---\left(24\right)$

其中，分别表示导航坐标系下等效的东向和北向上加速度计零偏的估计误差；

根据式(23)和式(24)可得：

${\mathrm{\Δ\φ}}_U=-\mathrm{\Δ}{\▿}_E\×\tan L/g---\left(25\right)$

由于两位置对准时加速度计零偏的估计精度得到了提高，则Δ▽_E,可以认为是两位置对准时等效的东向和北向上加速度计零偏的估计值与固定位置对准时等效的东向与北向上加速度计零偏的估计值之差；即：

$\begin{array}{c}\mathrm{\Δ}{\▿}_E={\▿}_E^2-{\▿}_E^1\\ \mathrm{\Δ}{\▿}_N={\▿}_N^2-{\▿}_N^1\end{array}---\left(26\right)$

其中，分别表示的是两位置对准时东向和北向上加速度计零偏的估计值，分别表示的是固定位置对准时东向和北向上加速度计零偏的估计值；

将等效的东向和北向上加速度计零偏的估计误差代入式(24)就可以求出Δφ_E,Δφ_N，利用式(25)就可以求出Δφ_U，然后根据式(21)去修正捷联惯导的姿态矩阵就可以提高初始对准的精度；

根据式(18)和式(22)对火星车的初始位置进行修正就可以较大地提高火星车初始位置的精度。

本发明的优点在于：

(1)本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，通过利用着陆器所确定的着陆位置以及天文导航子系统输出的惯性姿态信息，可以快速地确定火星车的初始位姿信息；

(2)本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，利用卡尔曼滤波融合速度误差信息和姿态信息，进而对平台失准角以及惯性测量组件的零偏进行最优地估计，提高了初始对准的精度；

(3)本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，通过火星车自转，改善了捷联惯导系统误差的可观测性，使得加速度计零偏的估计精度得到了较大的提高，利用求得的平台失准角的估计误差对火星车的初始位姿信息进行修正以后，能够满足火星车对高精度初始对准的要求；

(4)本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，充分发挥了捷联惯导子系统和天文导航子系统各自的优势，并且还可以应用于其它巡视探测器，具有广阔的应用前景。

附图说明

图1为本发明提出的一种惯导天文高精度复合两位置对准以及误差标定方法结构示意图；

图2为本发明提出的一种惯导天文高精度复合两位置对准以及误差标定实现方法流程图；

图3为本发明中着陆器组合导航原理图；

图1中：

1—捷联惯导子系统 2—天文导航子系统 3—初始位姿确定单元

4—惯导姿态量测信息构造单元 5—组合导航滤波器

101—惯性测量组件 102—导航解算单元

201—大视场星敏感器 202—多矢量定姿单元

301—初始位置确定模块 302—初始姿态确定模块

—天文导航子系统所确定的惯性姿态矩阵 —姿态矩阵

—捷联惯导子系统所确定的惯性姿态矩阵 λ,L—经度和纬度

δVⁿ—速度误差信息 δP—位置误差向量 —平台失准角的估计误差向量

图3中：

—比力信息 —角速度信息

具体实施方式

下面将结合附图对本发明作进一步的详细说明。

本发明提出一种惯导天文高精度复合两位置对准及误差标定方法。通过将着陆器所确定的着陆位置作为火星车的初始位置信息(λ,L)，然后根据导航时间t和天文导航子系统2的输出确定火星车的初始姿态矩阵在此基础上，利用天文导航子系统2提供的高精度惯性姿态矩阵以及捷联惯导系统输出的速度误差信息(δVⁿ)，并结合捷联惯导系统的误差模型，利用组合导航滤波器5估计、校正系统误差，实现星高精度初始对准和误差标定。

见图1，本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，包括捷联惯导子系统1、天文导航子系统2、初始位姿确定单元3、惯导姿态量测信息构造单元4和组合导航滤波器5；

捷联惯导子系统1包括惯性测量组件101和导航解算单元102；惯性测量组件101测得火星车相对于惯性空间的角速度和比力，将得到的角速度和比力信息传送给导航解算单元102；导航解算单元102根据惯性测量组件101传输的信息通过力学编排算法实时计算出火星车的速度误差(δVⁿ)和姿态矩阵

天文导航子系统2包括大视场星敏感器201和多矢量定姿单元202；大视场星敏感器201同一时刻能够观测得到三颗及三颗以上恒星的星光矢量信息，并将得到的观测信息提供给多矢量定姿单元202；多矢量定姿单元202对接收到的星光矢量信息进行处理，得到火星车相对惯性空间的姿态矩阵

初始位姿确定单元3包括初始位置确定模块301和初始姿态确定模块302；初始位置确定模块301将着陆器所确定的着陆位置作为火星车的初始位置信息(λ,L)，并将火星车的初始位置信息(λ,L)提供给初始姿态确定模块302；初始姿态确定模块302对接收到的火星车的初始位置信息(λ,L)、火星车的惯性姿态矩阵以及当前的导航时间t进行处理，得到火星车的初始姿态矩阵

惯导姿态量测信息构造单元4为捷联惯导子系统1确定从火星赤道惯性坐标系转换到载体坐标系的惯性姿态矩阵

组合导航滤波器5以SINS误差方程为状态方程，以惯导姿态量测信息构造单元4与天文导航子系统2分别提供的惯性姿态信息之差以及捷联惯导子系统1提供的速度误差(δVⁿ)作为观测量，得到火星车初始位姿误差的估计值(δP和)以及惯性测量组件零偏的估计值；

见图2，本发明提出一种惯导天文高精度复合两位置对准及误差标定方法，具体包括以下步骤：

步骤一：将着陆器所确定的着陆位置作为火星车的初始位置信息(λ,L)

火星车的初始位置(λ,L)也即着陆器的着陆位置，着陆器着陆位置的确定主要依赖于着陆段导航的精度，目前着陆段自主导航的发展趋势为：1)精确着陆导航，其着陆误差直径范围小于10米；2)中等精度着陆导航，其着陆误差直径范围在10米与1千米之间；3)低精度着陆导航，其着陆误差直径范围大于1千米；

以CCD相机作为传感器的视觉导航系统，具有体积小、重量轻、低能耗、视场宽、易于搭载等显著优势，更为重要的是，经过多年的发展，视觉导航算法已经能够通过提取图像中的信息较为精确地确定相机的位置、速度和姿态信息；因此，在火星着陆器着陆过程中，通过融合惯导系统的测量值以及CCD相机提供的位置、速度和姿态信息，就可以实现火星着陆器的精确着陆导航；

如图3所示，在火星着陆器的着陆过程中，当着陆相机开始工作以后，首先通过提取CCD相机所拍摄的火星表面图像上的地标点，并与火星在轨卫星所形成的数字高程地图进行地标点匹配，可以估计出着陆器的绝对位置及姿态信息；由于火星在轨卫星所拍摄的图像的分辨率有限，当火星着陆器接近火星表面时，地标点无法被准确地提取，可以通过跟踪图像序列中特征点的变化，估计出着陆器的速度信息，进而获得着陆器位置和姿态的相对变化信息；在整个着陆过程中，通过利用组合导航滤波器融合惯导系统与视觉导航系统分别提供的位置、速度和姿态信息，能够有效地修正惯导系统的累积误差以及陀螺仪和加速度计的常值漂移，从而极大地提高导航精度、满足火星车精确着陆任务的需要；

约翰逊等通过发射试验火箭模拟探测器的着陆过程对算法进行验证分析；试验结果表明，基于视觉匹配和惯性测量相结合的导航方法的速度误差为0.16m/s，位置误差为6.4m，能够满足未来火星精确着陆探测任务的需要；

通过以上的分析可以看出，将着陆器的着陆位置作为火星车的初始位置时，可以认为火星车初始经纬度的确定误差均为1角秒，即δλ＝1″，δL＝1″，其中，λ代表火星车的经度，L代表火星车的纬度；

步骤二：根据火星车的初始位置(λ,L)以及天文导航子系统2输出的惯性姿态矩阵确定火星车的初始姿态矩阵

新型大视场星敏感器201的出现，使得同时观测三颗及三颗以上的导航星成为可能，在不需要任何外部基准信息的前提下，可直接精确地(1″～7″)提供运载体相对于惯性空间的姿态矩阵且精度在全程保持稳定；因此，根据大视场星敏感器201的输出便可获得高精度的火星车载体坐标系(b系)相对于火星赤道惯性坐标系(i系)的姿态矩阵根据当前的导航时间t能够获得从火星赤道惯性坐标系(i系)转换到火星星固坐标系(m系)的方向余弦矩阵由于：

${\tilde{C}}_i^b=C_n^b{C}_m^n{C}_i^m---\left(1\right)$

其中：

$C_m^n=\left[\begin{array}{ccc}-\sin \mathrm{\λ}& \cos \mathrm{\λ}& 0\\ -\cos \mathrm{\λ}\sin L& -\sin \mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& \sin \mathrm{\λ}\cos L& \sin L\end{array}\right]$

所以，

火星车的初始姿态实际上就是导航坐标系(n系)和火星车载体坐标系(b系)之间的姿态转换矩阵

其中，θ，γ，ψ分别代表火星车的俯仰角、横滚角和偏航角，cθ表示的是cos(θ)，sθ表示的是sin(θ)，其它的以此类推；

根据式(2)可得：

θ＝arcsin(C₂₃) (3)

γ_主＝-arctan(C₁₃/C₃₃) (4)

ψ_主＝arctan(C₂₁/C₂₂) (5)

由以上三式得出的是反三角函数的主值；在实际应用中，俯仰角θ定义在(-π/2,π/2)区间，横滚角γ定义在(-π,π)区间，偏航角ψ定义在(0,2π)区间；这样，θ的主值即为真值，而γ和ψ的真值还需通过如下附加的判式来决定其在哪个象限：

步骤三：建立火星车两位置对准的系统状态模型和量测模型；

a.构建系统状态模型；

以地理坐标系(t系)作为惯性导航解算的基本坐标系，则惯导系统误差模型如下所示：

其中，为惯导的平台失准角，δVⁿ为速度误差，ε^b为陀螺仪常值漂移，为加速度计零偏；

因此，系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

其中，状态矢量系统的噪声矢量W(t)＝[w_gx w_gy w_gz w_ax w_ay w_az]^T，ω_gi(i＝x,y,z)、ω_ai(i＝x,y,z)分别为陀螺仪、加速度计随机白噪声；

b.构建系统量测模型；

根据当前的导航时间t和火星车的初始位置(λ,L)，并结合SINS的姿态矩阵有：

${\hat{C}}_i^b={\hat{C}}_n^b{\hat{C}}_m^n{C}_i^m---\left(10\right)$

考虑到对准误差及陀螺漂移等因素的影响，SINS数学平台系(n′系)与导航坐标系(n系)之间存在数学平台失准角向量有：

由于天文导航子系统2测量的姿态精度较高，且其测量误差可以考虑为零均值的白噪声，所以天文导航子系统2输出的姿态矩阵可以认为是真实的姿态矩阵与星敏感器的量测白噪声阵V_s之和，即：

${\tilde{C}}_i^b=C_i^b+V_s---\left(12\right)$

将惯导姿态量测信息构造单元4与天文导航子系统2分别确定的方向余弦阵的差值记作姿态量测量Z_s，则由式(10)、式(11)以及式(12)可得：

将Z_s(3×3)展开成列向量Z_1(9×1)，并结合系统的状态向量X，可列写出量测方程：

Z₁＝H₁X+V₁ (14)其中，H₁＝[H₁₁ 0_9×3 0_9×3 0_9×3]为姿态量测矩阵，V₁为天文导航子系统2的量测白噪声矩阵；

令则根据式(13)可得：

$H_{11}=\left[\begin{array}{ccc}{T}_{13}{C}_{21}-T_{12}{C}_{31}& T_{11}{C}_{31}-T_{13}{C}_{11}& T_{12}{C}_{11}-T_{11}{C}_{21}\\ T_{13}{C}_{22}-T_{12}{C}_{32}& T_{11}{C}_{32}-T_{13}{C}_{12}& T_{12}{C}_{12}-T_{11}{C}_{22}\\ T_{13}{C}_{23}-T_{12}{C}_{33}& T_{11}{C}_{33}-T_{13}{C}_{13}& T_{12}{C}_{13}-T_{11}{C}_{23}\\ T_{23}{C}_{21}-T_{22}{C}_{31}& T_{21}{C}_{31}-T_{23}{C}_{11}& T_{22}{C}_{11}-T_{21}{C}_{21}\\ T_{23}{C}_{22}-T_{22}{C}_{32}& T_{21}{C}_{32}-T_{23}{C}_{12}& T_{22}{C}_{12}-T_{21}{C}_{22}\\ T_{23}{C}_{23}-T_{22}{C}_{33}& T_{21}{C}_{33}-T_{23}{C}_{13}& T_{22}{C}_{13}-T_{21}{C}_{23}\\ T_{33}{C}_{21}-T_{32}{C}_{31}& T_{31}{C}_{31}-T_{33}{C}_{11}& T_{32}{C}_{11}-T_{31}{C}_{21}\\ T_{33}{C}_{22}-T_{32}{C}_{32}& T_{31}{C}_{32}-T_{33}{C}_{12}& T_{32}{C}_{12}-T_{31}{C}_{22}\\ T_{33}{C}_{23}-T_{32}{C}_{33}& T_{31}{C}_{33}-T_{33}{C}_{13}& T_{32}{C}_{13}-T_{31}{C}_{23}\end{array}\right]$

将捷联惯导子系统1输出的速度误差(δVⁿ)作为观测量，并结合系统的状态向量X，可列写出量测方程：

$Z_2=\left[\begin{array}{c}{\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ {\mathrm{\δv}}_U\end{array}\right]=H_{2}X+V_2---\left(15\right)$

式中，H₂＝[0_3×3 I_3×3 0_3×3 0_3×3]为速度误差量测矩阵，V₂为速度误差的量测白噪声矩阵；

步骤四：火星车通过自转实现两位置对准；

火星车两位置对准的过程为：首先，火星车在起始位置处进行初始对准及误差标定，然后，让火星车在原地进行自转，通过积分陀螺仪的输出获得火星车姿态角的变化，当火星车的偏航角改变了180°以后，使火星车停止自转运动，然后在新的位置上继续进行对准过程和误差标定过程；

步骤五：通过修正火星车的初始位姿信息实现对高精度初始对准的要求；

由于火星车的初始位置存在误差δL,δλ，因此，根据火星车初始位置(λ,L)所确定的位置矩阵与真实的位置矩阵之间存在如下的关系：

$C_m^n=(I+\[\mathrm{\δ}P\×\]){\hat{C}}_m^n---\left(16\right)$

其中，δP＝[-δL δλ·cos L δλ·sin L]^T为位置误差向量。

根据式(12)和式(16)可知，天文导航子系统2所确定的高精度惯性姿态矩阵可以表示为：

${\tilde{C}}_i^b=C_n^b{C}_m^n{C}_i^m+V_s=C_n^b(I+\[\mathrm{\δ}P\×\]){\hat{C}}_m^n{C}_i^m+V_s---\left(17\right)$

因此，火星车经过初始对准以后所确定的姿态矩阵与真实的姿态矩阵之间存在如下的关系：

$C_p^b=C_n^b(I+\[\mathrm{\δ}P\×\])---\left(18\right)$

火星车进行初始对准时，由于存在位置误差的影响，使得平台失准角的估计值与真实值之间存在一定的偏差：

$\begin{array}{c}{\mathrm{\Δ\φ}}_E={\widehat{\mathrm{\φ}}}_E-{\mathrm{\φ}}_E\\ {\mathrm{\Δ\φ}}_N={\widehat{\mathrm{\φ}}}_N-{\mathrm{\φ}}_N\\ {\mathrm{\Δ\φ}}_U={\widehat{\mathrm{\φ}}}_U-{\mathrm{\φ}}_U\end{array}---\left(19\right)$

其中，Δφ_E,Δφ_N,Δφ_U分别表示平台失准角的估计误差，分别表示平台失准角的估计值，φ_E,φ_N,φ_U分别表示平台失准角的真实值；

由于

因此

根据公式(18)和公式(21)可得：

因此

Δφ_U＝Δφ_N×tan L (23)

而水平方向上平台失准角的估计误差与加速度计零偏的估计误差之间存在如下的关系：

$\begin{array}{c}{\mathrm{\Δ\φ}}_E=\mathrm{\Δ}{\▿}_N/g\\ {\mathrm{\Δ\φ}}_N=-\mathrm{\Δ}{\▿}_E/g\end{array}---\left(24\right)$

其中，分别表示导航坐标系下等效的东向和北向上加速度计零偏的估计误差；

根据式(23)和式(24)可得：

${\mathrm{\Δ\φ}}_U=-\mathrm{\Δ}{\▿}_E\×\tan L/g---\left(25\right)$

由于两位置对准时加速度计零偏的估计精度得到了提高，则Δ▽_E,可以认为是两位置对准时等效的东向和北向上加速度计零偏的估计值与固定位置对准时等效的东向与北向上加速度计零偏的估计值之差；即：

$\begin{array}{c}\mathrm{\Δ}{\▿}_E={\▿}_E^2-{\▿}_E^1\\ \mathrm{\Δ}{\▿}_N={\▿}_N^2-{\▿}_N^1\end{array}---\left(26\right)$

其中，分别表示的是两位置对准时东向和北向上加速度计零偏的估计值，分别表示的是固定位置对准时东向和北向上加速度计零偏的估计值；

将等效的东向和北向上加速度计零偏的估计误差代入式(24)就可以求出Δφ_E,Δφ_N，利用式(25)就可以求出Δφ_U，然后根据式(21)去修正捷联惯导的姿态矩阵就可以提高初始对准的精度；

根据式(18)和式(22)对火星车的初始位置(λ,L)进行修正就可以较大地提高火星车初始位置的精度。

Claims (1)

1.一种惯导天文高精度复合两位置对准及误差标定方法，其特征在于：它包括以下步骤：

步骤一：将着陆器所确定的着陆位置作为火星车的初始位置信息

在火星着陆器的着陆过程中，当着陆相机开始工作以后，首先通过提取CCD相机所拍摄的火星表面图像上的地标点，并与火星在轨卫星所形成的数字高程地图进行地标点匹配，预计出着陆器的绝对位置及姿态信息；由于火星在轨卫星所拍摄的图像的分辨率有限，当火星着陆器接近火星表面时，地标点无法被准确地提取，通过跟踪图像序列中特征点的变化，预计出着陆器的速度信息，进而获得着陆器位置和姿态的相对变化信息；在整个着陆过程中，通过利用组合导航滤波器融合惯导系统与视觉导航系统分别提供的位置、速度和姿态信息，能够有效地修正惯导系统的累积误差以及陀螺仪和加速度计的常值漂移，从而提高导航精度、满足火星车精确着陆任务的需要；

试验结果表明，基于视觉匹配和惯性测量相结合的导航方法的速度误差为0.16m/s，位置误差为6.4m，能够满足未来火星精确着陆探测任务的需要；

通过以上的分析看出，将着陆器的着陆位置作为火星车的初始位置时，认为火星车初始经纬度的确定误差均为1角秒，即δλ＝1″，δL＝1″，其中，λ代表火星车的经度，L代表火星车的纬度；

步骤二：根据火星车的初始位置以及天文导航子系统输出的惯性姿态矩阵确定火星车的初始姿态矩阵；

根据大视场星敏感器的输出便获得高精度的火星车载体坐标系b系相对于火星赤道惯性坐标系i系的姿态矩阵根据当前的导航时间t能够获得从火星赤道惯性坐标系i系转换到火星星固坐标系m系的方向余弦矩阵由于：

${\tilde{C}}_i^b=C_n^b{C}_m^n{C}_i^m---\left(1\right)$

其中：

$C_m^n=\left[\begin{array}{ccc}-sin\mathrm{\λ}& cos\mathrm{\λ}& 0\\ -cos\mathrm{\λ}\sin L& -sin\mathrm{\λ}\sin L& \cos L\\ \cos \mathrm{\λ}\cos L& sin\mathrm{\λ}\cos L& \sin L\end{array}\right]$

所以，

火星车的初始姿态实际上就是导航坐标系n系和火星车载体坐标系b系之间的姿态转换矩阵

$C_n^b=\left[\begin{array}{ccc}c\mathrm{\γ}c\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -c\mathrm{\γ}s\mathrm{\ψ}+s\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& -s\mathrm{\γ}c\mathrm{\θ}\\ c\mathrm{\θ}s\mathrm{\ψ}& c\mathrm{\θ}c\mathrm{\ψ}& s\mathrm{\θ}\\ s\mathrm{\γ}c\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}s\mathrm{\ψ}& -s\mathrm{\γ}s\mathrm{\ψ}-c\mathrm{\γ}s\mathrm{\θ}c\mathrm{\ψ}& c\mathrm{\γ}c\mathrm{\θ}\end{array}\right]---\left(2\right)$

其中，θ，γ，ψ分别代表火星车的俯仰角、横滚角和偏航角，cθ表示的是cos(θ)，sθ表示的是sin(θ)，其它的以此类推；

根据式(2)得：

θ＝arcsin(C₂₃) (3)

γ_主＝-arctan(C₁₃/C₃₃) (4)

ψ_主＝arctan(C₂₁/C₂₂) (5)

由以上三式得出的是反三角函数的主值；在实际应用中，俯仰角θ定义在(-π/2,π/2)区间，横滚角γ定义在(-π,π)区间，偏航角ψ定义在(0,2π)区间；这样，θ的主值即为真值，而γ和ψ的真值还需通过如下附加的判式来决定其在哪个象限：

步骤三：建立火星车两位置对准的系统状态模型和量测模型；

a.构建系统状态模型；

以地理坐标系t系作为惯性导航解算的基本坐标系，则惯导系统误差模型如下所示：

其中，为惯导的平台失准角，δVⁿ为速度误差，ε^b为陀螺仪常值漂移，为加速度计零偏；

因此，系统的状态方程为：

$\stackrel{\·}{X}\left(t\right)=F\left(t\right)X\left(t\right)+G\left(t\right)W\left(t\right)---\left(9\right)$

其中，状态矢量系统的噪声矢量W(t)＝[w_gx w_gy w_gz w_ax w_ay w_az]^T，ω_gi(i＝x,y,z)、ω_ai(i＝x,y,z)分别为陀螺仪、加速度计随机白噪声；

b.构建系统量测模型；

根据当前的导航时间t和火星车的初始位置，并结合SINS的姿态矩阵有：

${\hat{C}}_i^b={\hat{C}}_n^b{\hat{C}}_m^n{C}_i^m---\left(10\right)$

考虑到对准误差及陀螺漂移等因素的影响，SINS数学平台系n′系与导航坐标系n系之间存在数学平台失准角向量有：

由于天文导航子系统测量的姿态精度较高，且其测量误差考虑为零均值的白噪声，所以天文导航子系统输出的惯性姿态矩阵认为是真实的姿态矩阵与星敏感器的量测白噪声阵V_s之和，即：

${\tilde{C}}_i^b=C_i^b+V_s---\left(12\right)$

将惯导姿态量测信息构造单元与天文导航子系统分别确定的方向余弦阵的差值记作姿态量测量Z_s，则由式(10)、式(11)以及式(12)得：

将Z_s(3×3)展开成列向量Z_1(9×1)，并结合系统的状态向量X，列写出量测方程：

Z₁＝H₁X+V₁ (14)

其中，H₁＝[H₁₁ 0_9×3 0_9×3 0_9×3]为姿态量测矩阵，V₁为星敏感器的量测白噪声矩阵；

令则根据式(13)得：

$H_{11}=\left[\begin{array}{ccc}{T}_{13}{C}_{21}-T_{12}{C}_{31}& T_{11}{C}_{31}-T_{13}{C}_{11}& T_{12}{C}_{11}-T_{11}{C}_{21}\\ T_{13}{C}_{22}-T_{12}{C}_{32}& T_{11}{C}_{32}-T_{13}{C}_{12}& T_{12}{C}_{12}-T_{11}{C}_{22}\\ T_{13}{C}_{23}-T_{12}{C}_{33}& T_{11}{C}_{33}-T_{13}{C}_{13}& T_{12}{C}_{13}-T_{11}{C}_{23}\\ T_{23}{C}_{21}-T_{22}{C}_{31}& T_{21}{C}_{31}-T_{23}{C}_{11}& T_{22}{C}_{11}-T_{21}{C}_{21}\\ T_{23}{C}_{22}-T_{22}{C}_{32}& T_{21}{C}_{32}-T_{23}{C}_{12}& T_{22}{C}_{12}-T_{21}{C}_{22}\\ T_{23}{C}_{23}-T_{22}{C}_{33}& T_{21}{C}_{33}-T_{23}{C}_{13}& T_{22}{C}_{13}-T_{21}{C}_{23}\\ T_{33}{C}_{21}-T_{32}{C}_{31}& T_{31}{C}_{31}-T_{33}{C}_{11}& T_{32}{C}_{11}-T_{31}{C}_{21}\\ T_{33}{C}_{22}-T_{32}{C}_{32}& T_{31}{C}_{32}-T_{33}{C}_{12}& T_{32}{C}_{12}-T_{31}{C}_{22}\\ T_{33}{C}_{23}-T_{32}{C}_{33}& T_{31}{C}_{33}-T_{33}{C}_{13}& T_{32}{C}_{13}-T_{31}{C}_{23}\end{array}\right]$

将捷联惯导子系统输出的速度误差作为观测量，并结合系统的状态向量X，列写出量测方程：

$Z_2=\left[\begin{array}{c}{\mathrm{\δv}}_E\\ {\mathrm{\δv}}_N\\ {\mathrm{\δv}}_U\end{array}\right]=H_{2}X+V_2---\left(15\right)$

式中，H₂＝[0_3×3 I_3×3 0_3×3 0_3×3]为速度误差量测矩阵，V₂为速度误差的量测白噪声矩阵；

步骤四：火星车通过自转实现两位置对准；

火星车两位置对准的过程为：首先，火星车在起始位置处进行初始对准及误差标定，然后，让火星车在原地进行自转，通过积分陀螺仪的输出获得火星车姿态角的变化，当火星车的偏航角改变了180°以后，使火星车停止自转运动，然后在新的位置上继续进行对准过程和误差标定过程；

步骤五：通过修正火星车的初始位姿信息实现对高精度初始对准的要求；

由于火星车的初始位置存在误差δL,δλ，因此，根据火星车初始位置所确定的位置矩阵与真实的位置矩阵之间存在如下的关系：

$C_m^n=(I+\[\mathrm{\δ}P\×\]){\hat{C}}_m^n---\left(16\right)$

其中，δP＝[-δL δλ·cosL δλ·sinL]^T为位置误差向量；

根据式(12)和式(16)可知，天文导航子系统所确定的高精度惯性姿态矩阵表示为：

${\tilde{C}}_i^b=C_n^b{C}_m^n{C}_i^m+V_s=C_n^b(I+\[\mathrm{\δ}P\×\]){\hat{C}}_m^n{C}_i^m+V_s---\left(17\right)$

因此，火星车经过初始对准以后所确定的姿态矩阵与真实的姿态矩阵之间存在如下的关系：

$C_p^b=C_n^b(I+\[\mathrm{\δ}P\×\])---\left(18\right)$

火星车进行初始对准时，由于存在位置误差的影响，使得平台失准角的估计值与真实值之间存在一定的偏差：

${\mathrm{\Δ\φ}}_E={\widehat{\mathrm{\φ}}}_E-{\mathrm{\φ}}_E$

${\mathrm{\Δ\φ}}_N={\widehat{\mathrm{\φ}}}_N-{\mathrm{\φ}}_N---\left(19\right)$

${\mathrm{\Δ\φ}}_U={\widehat{\mathrm{\φ}}}_U-{\mathrm{\φ}}_U$

其中，Δφ_E,Δφ_N,Δφ_U分别表示平台失准角的估计误差，分别表示平台失准角的估计值，φ_E,φ_N,φ_U分别表示平台失准角的真实值；

由于

因此

根据公式(18)和公式(21)得：

因此

Δφ_U＝Δφ_N×tanL (23)

而水平方向上平台失准角的估计误差与加速度计零偏的估计误差之间存在如下的关系：

$\begin{array}{c}{\mathrm{\Δ\φ}}_E=\mathrm{\Δ}{\▿}_N/g\\ {\mathrm{\Δ\φ}}_N=-\mathrm{\Δ}{\▿}_E/g\end{array}---\left(24\right)$

其中，分别表示导航坐标系下等效的东向和北向上加速度计零偏的估计误差；

根据式(23)和式(24)得：

${\mathrm{\Δ\φ}}_U=-\mathrm{\Δ}{\▿}_E\×\tan L/g---\left(25\right)$

由于两位置对准时加速度计零偏的估计精度得到了提高，则认为是两位置对准时等效的东向和北向上加速度计零偏的估计值与固定位置对准时等效的东向与北向上加速度计零偏的估计值之差；即：

$\begin{array}{c}\mathrm{\Δ}{\▿}_E={\▿}_E^2-{\▿}_E^1\\ \mathrm{\Δ}{\▿}_N={\▿}_N^2-{\▿}_N^1\end{array}---\left(26\right)$

其中，分别表示的是两位置对准时东向和北向上加速度计零偏的估计值，分别表示的是固定位置对准时东向和北向上加速度计零偏的估计值；

将等效的东向和北向上加速度计零偏的估计误差代入式(24)就求出Δφ_E,Δφ_N，利用式(25)就求出Δφ_U，然后根据式(21)去修正捷联惯导的姿态矩阵就提高初始对准的精度；

根据式(18)和式(22)对火星车的初始位置进行修正，能提高火星车初始位置的精度。