[发明专利]一种月面巡视探测器双模式惯性/视觉/天文导航方法

权利要求书

1.一种月面巡视探测器双模式惯性/视觉/天文导航方法，其特征在于，包括如下步骤：

步骤1：获取星敏感器测量的星光矢量、天体高度，惯性导航系统和视觉导航系统测量的相对位置和姿态误差，以及惯性导航系统测量的水平速度误差；

步骤2：当月面巡视探测器运动时，首先根据月球固连坐标系下捷联惯导的力学编排，以惯性导航误差方程作为状态模型；再分别以惯性导航系统和视觉导航系统获得的相对位置和姿态误差及星敏感器测量的星光矢量作为量测量，之后根据这些量测量分别建立惯性导航系统和视觉导航系统获得的相对位置和姿态误差的量测模型及星光矢量的量测模型；并使用无迹卡尔曼滤波UKF滤波估计月面巡视探测器的位置、速度和姿态，对月面巡视探测器的姿态误差和惯性器件误差进行修正；

步骤3：当月面巡视探测器静止时，速度为零，首先建立简化的惯导误差方程作为状态模型；再分别以惯性导航系统获得的水平速度误差及星敏感器测量的星光矢量和星光高度作为量测量，之后根据这些量测量分别建立水平速度误差的量测模型及星光矢量、星光高度的量测模型；并使用UKF滤波估计月面巡视探测器的位置、速度和姿态，对月面巡视探测器的位置和姿态误差以及惯性器件误差进行修正。

2.根据权利要求1所述的一种月面巡视探测器双模式惯性/视觉/天文导航方法，其特征在于：所述步骤2中，当月面巡视探测器运动时的具体导航步骤如下：

步骤2.1建立月面巡视探测器运动时惯性/视觉/天文组合导航系统状态模型

以月球固连坐标系下捷联惯导的误差方程作为系统状态模型：

其中，sec表示正割函数，cos的倒数，Rm的含义是月球半径，值为1737.5km，含义是东北天坐标系下沿E轴方向的速度，C、D是系数；

式中，φ＝[φ_E φ_N φ_U]^T是姿态误差角，φ_E φ_N φ_U是地理坐标系下每个轴向的姿态误差角，f_n是n系下的比力矢量，是速度误差，是地理坐标系下各轴向的速度误差，δr_n＝[δL δλ]^T是位置误差，L为经度，λ是纬度，δL是经度误差，δλ是纬度误差，是的误差，是巡视器在n系下的月球角速率，是的误差，是n系相对m系的旋转角速率在n系中的表示，是的误差，为n系相对惯性空间的转动角速度矢量在n系下的投影，ε＝(ε_x ε_y ε_z)^T是陀螺仪的常值漂移，ε_xε_yε_z是陀螺仪在本体坐标系下三个轴的常值漂移，▽＝(▽_x ▽_y ▽_z)^T是加速度计的常值偏置，▽_x ▽_y ▽_z是加计在本体坐标系下三个轴的常值偏差，变量上方的圆点表示导数；

状态量：X＝[φ^T δv^T δr^T ε^T ▽^T]^T；

步骤2.2建立惯性导航系统和视觉导航系统获得的相对位置和姿态误差及星光矢量的量测模型

首先，建立惯性导航系统和视觉导航系统获得的相对位置和姿态误差的量测模型的过程如下：

假设在t_k时刻巡视器的真实姿态矩阵，R_err是的估计误差，定义为r_k是巡视器在t_k时刻月球固连坐标系下的真实位置；δr(k)＝r_INS(k)-r(k)是r_INS(k)的估计误差；姿态矩阵和位置r_INS(k)表示为：

假设校正后t_k-1时刻的估计误差很小，满足：

相对旋转矩阵R_INS和位置矢量T_INS可以通过把方程式(3)代入方程式(4)获得：

相对旋转矩阵R_VNS位置矢量T_VNS通过视觉导航系统VNS获得表示为：.

式中，ΔR_VNS和ΔT_VNS表示R_VNS和T_VNS的误差

从等式(5)和等式(6)得到：

当姿态误差角φ小于预定常数，将φ表示为R_err的函数：

φ＝g(R_err) (8)

当g(R_err)和g(ΔR_VNS)都小于预定常数，写为：

式中，Δφ是φ的误差；

INS表示惯性导航系统，VNS表示视觉导航系统，INS和VNS之间的位置矢量误差表示为：

在公式(10)中，是r_n,INS(k)＝[L_INS(k),λ_INS(k)]^T的函数，位置误差δr_n(k)＝[ΔL(k),Δλ(k)]^T，表示为：

λ(k)＝λ_INS(k)+Δλ(k)

L(k)＝L_INS(k)+ΔL(k) (12)

式中，λ_INS(k)和L_INS(k)是惯性导航系统INS在t_k时刻获得的经度和纬度；

从等式(9)和等式(10)中获得的相对运动误差的量测模型表示为：

式中V₁＝[Δφ,ΔT_VNS]是量测噪声；

建立基于星光矢量的量测模型，S_i和S_s之间的关系如下：

式中，s_s是星敏感器坐标系下的恒星星光方向矢量，s_i是惯性坐标系下所观测恒星星光方向矢量，是从n系到m系的变换矩阵，是从b系到n系的变换矩阵，也是姿态矩阵，R_err是的估计误差，是从m系到i系的变换矩阵，是星敏感器的安装矩阵，星敏感器坐标系相对于惯性坐标系的转换矩阵；

根据公式(14)，星光矢量的量测模型简写为：

Z₂＝[s_s]＝h₂(X)+V₂ (15)

式中V₂是星敏感器的量测噪声；

步骤2.3量测模型(13)、(15)都是非线性的，使用非线性UKF滤波方法，估计月面巡视探测器的位置、速度和姿态，并修正月面巡视探测器的姿态误差和惯性器件误差。

3.根据权利要求1所述的一种月面巡视探测器双模式惯性/视觉/天文导航方法，其特征在于：

所述步骤3中，当月面巡视探测器静止时的具体导航步骤如下：

步骤3.1月面巡视探测器静止时惯性/天文导航系统状态模型

当月面巡视探测器静止时，其真实速度为零，简化的INS误差模型从公式(1)得到，如下所示：

状态量：X₁＝[φ^T δv^T δr^T ε^T ▽^T]^T

步骤3.2量测模型

静止时的惯性/天文组合导航模式下的量测量包括惯导系统获得的水平速度误差、星光矢量和星光高度；

首先，惯性导航系统获得的水平速度误差的量测模型如下：

月面巡视探测器静止时惯导解算得到的水平速度即为速度误差δv，相应的量测方程为：

天文导航是以已知准确空间位置的自然天体为基准，通过天体测量仪器测得的天体高度和星光方位矢量信息进行导航；

其次，基于星光方位矢量的量测模型如下：

静止时的惯性/天文组合导航模式下星光矢量的量测方程如下所示：

是m系到n系的转换矩阵，是i系到m系的转换矩阵，t_SHA是春分点的格林时角，通过观测时间查找天文历表获得；V_s是量测噪声；

方程(18)是X₁的非线性方程；使用Z₁＝[s_s]，量测方程表示为：

Z₁＝h(X₁)+V₁ (19)

式中V₁是量测噪声；

最后，星光高度的量测模型如下：

星光高度是天体投影点与月面巡视探测器位置(L,λ)的函数，它们之间的关系为：

H＝arcsin(sinLsinΔ+cosLcosΔcos(λ+t_SHA-R_A)) (20)

式中，Δ、R_A分别为天体的赤纬和赤经；

天体高度利用加速度计提供的水平基准和星敏感器提供的星光方位矢量获取；当加速度计不存在测量误差时：

式中，f_br为不存在测量误差时的加速度计输出，s_b为本体坐标系下的星光方向矢量；当加速度存在测量误差时：

将式(22)代入式(21)得到：

式中，g_n＝[0 0 -g_m]^T，g_m＝1.618m/s²是月球重力加速度，通过公式(17)、(18)、(23)，以Z₂＝[δv s_s sinH_m]作为量测量，得量测方程为：

Z₂＝h(X₂)+V₂ (24)

式中，V₂为量测噪声；

步骤3.3由于量测模型(19)、(24)都是非线性的，因此使用非线性UKF滤波方法，估计月面巡视探测器的位置、速度和姿态，并修正月面巡视探测器的姿态误差和惯性器件误差。