基于SINS/CNS的月球車自主導航方法

周標準，裴福俊，董國成

(北京工業大學 電子信息與控制工程學院，北京 100124)

月球車是一種在復雜的非結構化月面環境中執行探測任務的機器人，它是目前完成月球近距離探測的最直接有效的工具。在復雜的月面環境中，為了保證月球車探測任務的順利完成，要求其具備高度的自主功能。其中，導航系統是月球車實現自主性的一項重要手段，而高精度的姿態信息是進行自主避障和路徑規劃的前提和基礎［1］。通常，月球車是借助地面站通過無線電測控進行導航控制，但由于地球和月球的自轉和公轉運動以及由此所帶來的地面站和月球車相對地理位置的變化，有時月球車會處于地面站無法測控的區域內，因此，月球車依靠自身攜帶的測量工具完成自主導航控制已成為重要的研究方向。

目前適用于月球車導航定向的方法一般可分為相對定向技術和絕對定向技術。相對定向技術主要包括以里程計和慣性導航系統為主的導航技術［1-2］，具有可靠的短期精度、絕對的實時性和完全的自主性。但由于依賴于測量值的積累，所以其測量精度隨時間增加而降低，不適于長時間、長距離導航定向［3］。絕對定向技術定向誤差不隨時間增加而增大，因此成為月球車導航定位技術的主流。目前可在實際工程中應用的絕對定向方法主要有視覺導航定向［4］，無線電導航定向以及利用太陽敏感器［5］、星敏感器等絕對傳感器進行定向等。

根據以上分析，對月球車導航定位幾種重要算法的性能進行比較，比較參數如表1 所示。

根據月球車導航技術的特點以及月面環境自身的特殊性和復雜性，現有的任何單一的導航定位技術都無法滿足要求，因此，組合導航方法應運而生。現階段月球車的長距離自主定位導航的主要方法是依靠太陽、地球和恒星的觀測信息進行定位定向的天文導航方法與慣性導航等方法相結合的組合導航方法。

表1 月球車導航算法性能比較

為滿足月球車高精度定位的需要，綜合單個導航系統自身的特點，設計了基于改進粒子濾波的捷聯慣導/天文導航的組合導航方案。以太陽敏感器來測定月球車的航向角，從而確定月球車的相對航位。通過考慮月面與太陽間的位置關系，利用太陽敏感器來獲取太陽方位角和高度角，結合測速儀的測量信息來確定月球車的航向，為月球車提供了一套組合導航方案。

最后，對基于改進粒子濾波的天文與速度聯合觀測的月球車導航方法進行了仿真。仿真結果表明，基于粒子濾波的天文與速度聯合觀測的方法具有更好的位置和姿態估計精度，有效減小了線性化誤差。仿真結果證明了該方法的有效性和實用性。

1 月球車天文導航原理

天文導航［5］就是利用對天體的觀測來確定自身航向和位置的自主導航技術。太陽敏感器是航天器姿態控制系統中的重要測量部件，它通過檢測太陽矢量的方位，為航天器提供可靠的姿態測量信息。在月球表面，太陽可為月球車提供穩定而可靠的觀測信息。因此，通過在月球車上裝載的太陽敏感器進行姿態測量，對提高月球車姿態測量精度，實現月球巡視探測器長時間、長距離導航具有非常重要的意義。

月球車自主天文導航的基本原理如圖1 所示，天體的高度(H)或頂距(Z)取決于月球車和天體投影點(GP)之間的距離。天體投影點是天體到月球質心的連線與月球表面的交點。

圖1 天體投影點及位置圓

根據天體的自然運動規律，可以精確得到太陽在某個時刻相對固定坐標系的位置矢量。相對于月球車來說，太陽的高度(太陽矢量方向與當地水平方向的夾角)和方位(太陽矢量在當地地平上的投影與真北方向之間的夾角)取決于月球車所在位置的經度λ 和緯度φ。因此，當已知月球車所在位置的經度和緯度時，可以推導出太陽的高度角和方位角，其方程為:

其中:H 和A 為當地水平坐標系中天體的高度和方位值;λ、φ為月球車在J2000. 0 月固坐標系中的經度和緯度;Dec 和LHA 為天體的赤緯和地方時角，地方時角LHA 為天體的格林時角GHA(0°經線處的地方時角)和月球車所在位置的經度λ 之和，且當經度為東經時，λ 取正，經度為西經時，λ 取負。Dec 和LHA 可以通過查閱JPL 星歷表得到。

2 月球車自主導航系統誤差模型

2.1 捷聯慣導系統非線性誤差模型

捷聯慣導系統［1］采用歐拉角縱搖角誤差φE、橫搖角誤差φN、航向角誤差φU來描述平臺坐標系與導航坐標系之間的不對準角，并假定φE和φN為小角度，忽略高層和高度誤差、天向速度誤差的影響。

導航坐標系的選擇:月球天球坐標系統為慣性坐標系，定義為i 系，月固坐標系為m 系，地理坐標系統(東北天)為n 系，月球車體坐標系為b 系。月球環境下的導航需考慮月球公轉的影響。定義月球繞地球公轉的角速度為ω0，則月球公轉的角速度在月固坐標系下的可投影表示為為慣性坐標系到月固坐標系的轉換矩陣，根據天文導航的基本原理可知其中:Ωl為月球相對地球升交點的赤經;ul為該升交點至月球起始子午線間的角度;il為月球赤道相對地球赤道的傾角。這3 個角可以通過查星歷表或計算得到。

定義月球自轉角速度為ω，則月球相對于慣性空間的轉動角速度ωim=ω+Cmi ω0。如果已知月球車所在位置的經度和緯度分別為λ、L，則可以得到月固坐標系到地理坐標系的轉換矩陣

可將ωin轉換到地理坐標系下月固坐標系相對地理坐標系的轉動角速度可以表示為

當θ 和γ 都很小，同時φ 較大時，矩陣可以簡化為

2.2 SINS/CNS 組合導航非線性濾波模型

2.2.1 系統狀態方程

月球車慣導系統的導航信息誤差為7 維:兩維的位置誤差、三維的姿態誤差和兩維的速度誤差，分別表示為δL、δλ、φE、φN、φU、δVE、δVN。另外，陀螺漂移由隨機常值εb和白噪聲ηg組成，加速度計偏置由常值零偏▽b和白噪聲ηa組成。捷聯慣導的誤差模型狀態向量表示如下:

由式(9)及慣導系統誤差方程，可建立月球車自主導航系統的狀態方程:

2.2.2 量測方程

月球車車體坐標系下的高度角Hb和方位角Ab即為車載的太陽敏感器測量的觀測量，以模擬地理坐標系的慣導平臺為基準，可以把車體坐標系下的太陽的高度和方位角轉化為平臺坐標系下的太陽的高度和方位角，記為Hp和Ap，則太陽中心與平臺坐標系原點的連線在平臺坐標系三軸上的分量分別是LcosHpsinAp、LcosHpcosAp、LsinAp，其中L 為地理坐標系原點到太陽的距離。利用慣性導航系統計算出月球車的經緯度，再根據天文導航原理計算太陽的高度角與方位角(Hc和Ac)，則太陽中心與計算坐標系原點的連線在計算坐標系三軸上的分量為LcosHcsinAc、LcosHccosAc、LsinAc。由于平臺坐標系與計算坐標系之間的誤差角 ψn=因此，根據ψn角可得平臺坐標系與計算坐標系之間的方向余弦陣，利用該轉換矩陣可以將太陽的高度和方位從計算坐標系轉換到平臺坐標系，具體的轉換關系為

這里使用天體高度作為觀測量，則由式(1)和(3)可得到系統的量測方程。

基于以上分析，可以建立基于太陽、速度聯合觀測的濾波方程。以測速儀測量的速度信息和太陽敏感器測得的太陽高度角和方位角為觀測量，則可得到系統的量測方程［6］為:

3 改進粒子濾波算法

3.1 標準粒子濾波算法

采樣重要性重采樣粒子濾波(sampling importance resampling particle filter)即標準粒子濾波器。將重要性分布函數取為狀態的轉移先驗分布使粒子的抽樣實現非常方便，并且通過引入了重采樣法解決權值的退化問題。SIR 粒子濾波算法遞推過程由3 部分組成:根據重要性分布函數來更新(預測)粒子;由似然函數更新粒子的權值;進行重采樣。

當獲得新的觀測量時，根據對應粒子的似然概率值更新預測粒子的權值。在重采樣過程中，權值較大的粒子將被多次采樣，采樣后對各個粒子分配相等權值，然后根據系統模型獨立進行動態更新。這樣理論上粒子群的分布會自動逼近狀態的真實后驗概率分布。

文獻［2］中詳細描述了標準粒子濾波算法的基本原理及其特點。

3.2 分層重采樣

為了解決SIS 算法固有的粒子退化問題，本算法引入重采樣算法，目的在于減少權值較小的粒子數目，從而增加權值較大的粒子個數。基本思想是:通過對后驗概率密度函數再采樣N 次，從而產生新的支撐點集使得重采樣是獨立同分布的，所以重采樣后粒子的權值被重新設置為1/N。

重采樣算法［2］一般包括多項式重采樣算法、殘差重采樣算法、分層重采樣算法、系統重采樣算法等。本文采用分層重采樣算法，它是由Carpenter 等人在1999 年提出的。

分層重采樣算法是在多項式重采樣算法的基礎上改進而來，它將隨機數區間分成N 個連續但不重合的區間，對每個區間采樣一個隨機數，這樣得到的分布集合自動按升序排列，但又沒有排序的開銷，因此優于傳統的多項式重采樣算法。文獻［2］對各種重采樣算法進行了詳細的描述和推導，本文不再詳述。

4 仿真

本文根據以上分析，對基于天文、速度聯合觀測的組合導航系統進行算法的仿真分析。具體的仿真參數如表2所示。

表2 仿真參數

根據表2 進行導航算法的參數設定，然后進行1 h 的仿真，仿真結果如圖2 ～4 所示。

圖2 位置誤差

圖3 速度誤差

圖4 姿態誤差

圖2～4中分別列出了基于天文和測速儀的速度觀測的組合導航粒子濾波算法的仿真結果，包括經度、緯度、三維的姿態和東向北向的速度誤差。從圖2 ～4 中可以看出，基于天文速度聯合觀察的導航方法，姿態和速度誤差能夠較快收斂，位置誤差收斂性較差，達到收斂的時間較長。仿真結果表明本文提出的基于聯合觀測的月球車組合導航方法有效補償了捷聯慣導的平臺誤差角，所采用的狀態估計方法對捷聯慣導系統進行了有效地校正，大大減小了累積誤差，從而為天文導航提供高精度的平臺基準信息，而天文導航的觀測信息實現了對慣導系統的校正，能夠使慣導/天文組合導航系統獲得高精度的位置和姿態估計信息。

本文的方法實際上利用速度觀測補償了捷聯慣導系統的姿態誤差，為天文導航提供高精度的平臺基準，然后利用天文導航信息補償捷聯慣導系統的位置誤差。

從仿真結果可以看出，由于本文采用非線性系統方程和觀測方程，從而減少了線性化帶來的線性化誤差，并且采用了慣性導航和速度聯合觀測的改進粒子濾波算法，通過引入粒子重采樣方法，有效解決了粒子匱乏問題，保證了天文導航對捷聯慣導的誤差補償作用，使得慣導系統獲得較好的補償，從而大大提高了月球車導航定位的精確性和穩定性。

5 結束語

針對月球車導航的特殊環境條件以及要求，本文設計實現了一種基于改進粒子濾波的天文與速度聯合觀測的月球車自主導航方法。根據慣性導航的基本原理建立了月面環境下慣性導航的誤差狀態方程，以天文和測速儀的速度為觀測信息建立了聯合量測方程，并采用了改進的粒子濾波實現月球車的位置和姿態的最優估計。通過仿真結果可以看出該方法有效解決了捷聯慣導系統平臺失準角誤差帶來的天文/慣性組合導航系統發散問題，可以提供高精度的位置和姿態信息，證明了該方法的有效性和實用性。

但通過對仿真結果分析可知，本文提出的算法雖然各狀態收斂效果較好，但是在姿態穩定的時間和精度上還不是非常理想，需進一步對算法進行改進，以提高穩定時間和系統的收斂性能。

本文提出的月球車自主導航方法的優點是量測方程都是非線性方程，利用粒子濾波實現位置和姿態的最優估計，而無須對量測方程進行線性化，從而減小線性化誤差的影響。通過仿真分析結果可知，該方法具有可靠性強、成本低、易于實現的特點。

［1］秦永元.慣性導航［M］.北京:科學出版社，2005.

［2］朱志宇.粒子濾波算法及其應用［M］.北京:北京科學出版社，2010.

［3］Yang Peng，Xie Li，Liu Jilin.Simultaneous celestial positioning and orientation for the lunar rover［J］.Aerospace Science and Technology，2011(9):1-10.

［4］Yenilmez L，Temeltas H.Autonomous navigation for planetary exploration by a mobile robot［Z］.Recent Advances in Space Technologies，2003 International Conference on RAST，2003:397-402.

［5］Cheng D，Ortega R，Panteley E.on port controlled Hamiltonian systems.Advanced Robust and Adaptive Control-Theory and Applications［M］. Beijing:Tsinghua University Press，2005:3-16.

［6］裴福俊，居鶴華，崔平遠.基于天文與速度聯合觀測的月球車自主導航方法［J］. 宇航學報，2005，30(2):476-491.

［7］寧曉琳，房建成.一種基于天體觀測的月球車位置姿態確定方法［J］.北京航空航天大學學報，2006，32(7):756-759，787.

［8］寧曉琳，房建成.一種基于UPF 的月球車自主天文導航方法［J］.宇航學報，2006，27(4):648-653.

［9］寧曉琳，房建成.一種基于純天文觀測的火星車自主導航方法［J］.空間科學學報，2006，26(2):142-147.

［10］Sebastian Thrun.Particle Filters in Robotics［D］.USA:Carnegie Mellon University，2000.