[發明專利]一種適用于月球著陸器的慣性/視覺/天文/激光測距組合導航方法

有效

• 申請號: 202010810080.2
• 申請日: 2020-08-13
• 公開（公告）號: CN111947652B
• 公開（公告）日: 2022-09-20
• 發明（設計）人: 吳偉仁;寧曉琳;黃玉琳
• 申請（專利權）人: 北京航空航天大學
• 主分類號: G01C21/16
• 分類號: G01C21/16;G01C21/02;G01S17/08
• 代理公司: 北京科迪生專利代理有限責任公司 11251
• 代理人: 安麗;賈玉忠

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₃表示量測噪聲。