[发明专利]一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法

权利要求书

1.一种适用于月球着陆器的惯性/视觉/天文/激光测距组合导航方法，其特征在于：

第一步，在月球固连坐标系下，建立基于惯性导航误差方程的月球着陆器的状态模型；

第二步，再分别利用惯性/视觉导航系统获得惯性导航系统和视觉导航系统的相对位置和姿态误差，利用天文导航系统获得恒星的星光矢量，利用激光测距仪获得月球着陆器相对月面的距离作为量测量；

第三步，根据所述量测量分别建立惯性/视觉导航系统相对位置和姿态误差量测模型、星光矢量量测模型及激光测距的量测模型；

第四步，基于第一步中的状态模型和第三步中的量测模型，采用UKF滤波估计月球着陆器的位置、速度和姿态，对月球着陆器的姿态误差和惯性器件误差进行修正；

所述第一步，具体包括以下步骤：

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

其中φ＝[φ_E φ_N φ_U]^T是姿态误差角，φ_E、φ_N、φ_U分别表示惯性导航系统地理坐标系，即n系内东、北、天向的姿态误差角；f_n是n系下的比力矢量；是月球着陆器速度误差，分别表示惯性导航系统东、北、天向的速度误差；δr_n＝[δL δλ δh]^T是n 系下的 月球着陆器位置误差，δL、δλ、δh分别表示惯性导航系统纬度误差、经度误差及高度误差；是月球着陆器在n系下的月球角速率，是的误差；是n系相对m系的旋转角速率在n系中的表示，是的误差,R_m为月球参考半径；是n系相对i系的转动角速度，ε＝(εxεyεz)^T是惯性导航系统x、y、z三个方向的陀螺仪常值漂移，是惯性导航系统x、y、z三个方向加速度计的常值偏置；所述m系是月球固连坐标系，i系为惯性坐标系；

上式状态模型(1)写为：

X_k＝F(X_k-1,k-1)+W_k-1 (2)

其中状态量为分别为月球着陆器的姿态误差角、速度误差、位置误差、陀螺仪的常值漂移与加速度计的常值偏置，X_k，X_k-1分别为k时刻和k-1下的状态量，F(X_k-1,k-1)为惯性/视觉/天文/激光测距组合导航系统非线性转移函数，W_k-1为过程噪声；

所述第三步中，建立惯性/视觉导航系统相对位置和姿态误差量测模型为：

通过惯性导航系统获得相对旋转矩阵R_INS，位置矢量T_INS，表示为：

其中，表示惯性导航系统下真实姿态矩阵，表示t_k时刻惯性导航系统下月球着陆器在地理坐标系中的观测姿态矩阵，r_INS(k)表示t_k时刻惯性导航系统下月球着陆器在月球固连坐标系下的观测位置矢量，表示t_k-1时刻月球着陆器在地理坐标系中的姿态矩阵估计值，表示t_k-1时刻月球着陆器的位置矢量估计值，观测姿态矩阵和位置r_INS(k)表示为：

校正后t_k-1时刻的估计误差满足：

则R_INS与T_INS最终表示为：

其中，R_err是的估计误差，定义为为t_k时刻月球着陆器在地理坐标系中的真实姿态矩阵，表示t_k-1时刻月球着陆器在地理坐标系中的真实姿态矩阵，r(k)是t_k时刻月球着陆器在月球固连坐标系下的真实位置，r(k-1)是t_k-1时刻月球着陆器在月球固连坐标系下的真实位置，δr(k)＝r_INS(k)-r(k)是r_INS(k)的估计误差；

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

其中，△R_VNS和△T_VNS表示R_VNS和T_VNS的误差；

则惯性导航系统和视觉导航系统之间的姿态误差表示为g(R_INS·R^T_VNS)：

g(R_INS·R^T_VNS)＝g(R_err·△R_VNS)≈g(R_err)+g(△R_VNS) (7)

当姿态误差角φ非常小时，将φ表示为R_err的函数：

φ＝g(R_err) (8)

当g(R_err)和g(△R_VNS)都非常小，写为：

g(R_INS·R^T_VNS)＝g(R_err·△R_VNS)≈g(R_err)+g(△R_VNS)＝φ+△φ (9)

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

其中，表示t_k时刻惯性导航系统下月球着陆器在月球固连坐标系下的观测姿态矩阵，表示t_k时刻月球着陆器在月球固连坐标系下的真实姿态矩阵，是r_n,INS(k)和δr_n(k)的函数，表示为：

式中，λ_INS(k)、L_INS(k)及h_INS(k)是t_k时刻惯性导航系统下月球着陆器所获得的经度、纬度和高度；△L(k)、△λ(k)及△h(k)是经度误差、纬度误差和高度误差；r_n,INS(k)＝[L_INS(k),λ_INS(k),h_INS(k)]^T是t_k时刻惯性导航系统下月球着陆器在地理坐标系下的位置矢量，位置误差δr_n(k)＝[△L(k),△λ(k),△h(k)]^T；

把惯性/视觉导航系统相对位置和姿态误差作为量测量Z₁，建立相对位置和姿态误差量测模型的表达式：

其中g(·)是将姿态矩阵转换为欧拉角的函数，h₁(·)表示非线性量测函数，V₁＝[△φ,△T_VNS]是量测噪声矩阵；

所述第三步中，建立距离量测模型如下：

利用激光测距仪获得月球着陆器相对月面的斜距l，并以此作为量测量:

其中，k_l为测距敏感器标度因数，R_m为月球参考半径，h_m()为月球数字高程库，b为测距敏感器常值偏差，为本体系到月心惯性系的方向余弦阵，表示t_k时刻惯性导航系统下月球着陆器在地理坐标系中的观测姿态矩阵，R_err是的估计误差，是从n坐标系到b坐标系的变换矩阵，是从m坐标系到n坐标系的变换矩阵，是从i 坐标系到m坐标系的变换矩阵，A₁为激光测距波束在本体系指向，v₃为测距敏感器测量噪声；

则基于距离的量测模型为：

Z₃＝[l]＝h₃(X)+V₃ (17)

其中，h₃(·)表示距离量测量的非线性量测函数，V₃表示量测噪声。