視線測量光學導航系統觀測模型選取

李建國 崔祜濤 田陽

（1哈爾濱工業大學深空探測基礎研究中心，哈爾濱150080）（2中國人民解放軍61345部隊，西安710010）

1 引言

隨著光學敏感器的發展，視線測量自主光學導航技術在太空任務特別是深空探測領域中得到了廣泛的關注[1-3]。導航系統對光學設備的依賴使得選取高精度的測量敏感器成為必要。但當存在較高的測量精度時，測量模型非線性與測量噪聲對導航性能的影響是同等重要的，特別在初始狀態估計誤差較大的情況下，忽略測量模型非線性高階項會使濾波算法發散或收斂到錯誤的狀態估計值[4]。因此，對于高精度測量設備，測量模型非線性對導航精度的作用至關重要。

基于局部線性化的擴展卡爾曼濾波（Extended Kalman Filter，EKF）已被成功用于眾多導航系統來處理非線性濾波問題[3]。作為一種近似非線性濾波器，EKF假設系統的非線性模型可以由當前狀態展開的線性模型很好的近似。因此為了使EKF獲得最優的濾波性能，應該盡可能選擇非線性強度弱的測量模型，并對非線性高階項進行補償。盡管導航系統可以考慮更先進的非線性濾波算法，但任務的實時性使得粒子濾波（Particle Filtering，PF）不適合工程要求[5]，無跡卡爾曼濾波（Unscented Kalman Filter，UKF）需要在線調節濾波參數，不一定能在實際應用中取得滿意的效果[6]。所以在導航系統設計時，定量測量非線性估計問題中模型的非線性強度，可以為導航系統設計時測量模型和濾波算法的選擇提供參考。

光學敏感器測量模型主要分為焦平面測量模型和單位矢量測量模型[7]。焦平面測量模型直接與實際的物理觀測量和測量噪聲相關聯，是光學敏感器基本的測量模型。單位矢量測量模型由共線方程非線性轉換得到，并對測量噪聲統計特性進行了簡化[8]，廣泛應用于姿態確定算法中。文獻[9]認為單位矢量測量模型比焦平面測量模型非線性強度低，可以產生更好的估計性能。但其結論建立在單個例子的仿真分析上，缺乏理論分析和通用性。

本文基于微分幾何非線性強度測量理論10，從理論上分析了不同測量模型的非線性強度，并以基于視線矢量觀測的高精度定姿任務為例，比較了兩種測量模型對濾波精度的影響，最后通過仿真驗證了理論分析結果。

2 視線測量光學導航算法

2.1 觀測方程

假設視線矢量測量由導航相機給出，對其測量模型作如下簡化處理：相機坐標系的Z軸沿著視軸方向；飛行器體坐標系與相機坐標系重合；導航相機視為理想的小孔成像相機；測量噪聲建模為獨立的零均值高斯白噪聲。

導航相機通過測量天體視線方向偏離視軸的方位角來確定飛行器位姿，圖1給出了測量原理圖[10]。飛行器位置、姿態與觀測量之間的關系可由式（1）給出。

式中 （ζ1，ζ2）是天體視線方向在焦平面上的投影值；（Xi，Yi，Zi）是已知的天體位置；（x，y，z）是未知的飛行器的位置；Aij（i，j＝1，2，3）為姿態矩陣A的各分量；f為已知的導航相機焦長，一般假設其值為1。如果目標天體位于無窮遠處，式（1）簡化為星敏感器共線測量方程。

在考慮測量噪聲的情況下，k時刻的焦平面測量模型可表示為

圖1 相機測量幾何Fig.1 Measurement geometric sketch of camera

式中yk為目標天體在焦平面上的測量值；h（x）為測量函數，x為飛行器的狀態矢量；ηk為零均值高斯白噪聲，在實際工程應用中廣泛使用的測量噪聲方差矩陣為[7]

式中c為量綱為1的常數；σ2ST為相機主點的測量噪聲方差強度。

經過非線性轉換，共線測量方程可以表示為單位矢量形式：

當焦平面測量模型轉化為單位矢量測量模型時，相應的測量噪聲也要進行轉化，表現為與測量值耦合在一起的乘性噪聲。Shuster在EKF算法框架內，提出了一個簡化的測量噪聲方差模型[8]：

其中測量噪聲方差特性滿足

由于單位矢量的歸一化約束使得測量方差矩陣奇異，在實際工程應用中可使用一個無奇異的測量噪聲方差矩陣代替：

2.2 測量模型非線性

為了分析測量模型非線性對濾波性能的影響，本節比較了標準EKF和二階EKF的測量更新過程。不失一般性，將非線性測量模型表示為如下形式

測量誤差方程和測量殘差方差矩陣可以分別寫為

其中，測量敏感性矩陣定義為

基于上述定義，可以得到先驗和后驗狀態估計誤差方差之間的關系式：

當測量模型非線性強度較大時，考慮其泰勒級數二階展開項：

其中，Hessian矩陣定義為

考慮二階項影響的測量函數預測估計值，變為

后驗誤差方差矩陣的表達式變為

將式（3）與式（4）進行比較可知，當存在精確觀測值時，測量誤差變得很小，使得Bk與Rk的大小處于同一數量級，若將測量模型泰勒級數高階展開項忽略，則EKF計算得到的估計誤差方差要比實際的估計誤差小，使得濾波器誤認為已經達到了要求的精度。從而使測量更新值不能最優融合，導致濾波發散或者收斂到錯誤的穩態值。因此，在基于EKF的導航算法中，對于高精度光學敏感器，選擇非線性強度低的觀測方程可以有效降低模型非線性對算法的影響。

3 基于微分幾何曲率測量的非線性強度分析

導航系統非線性依賴于觀測幾何、運動學及動力學模型的數學描述形式，但一直缺乏定量的測量方法。下面首先給出非線性曲率測量的定義，然后將其應用于姿態估計問題中。

3.1 非線性曲率測量

將非線性測量模型在先驗估計值處進行泰勒級數展開：

從幾何概念理解，EKF中的線性化近似等價于使用處的切平面近似非線性曲面。要使濾波算法使用的線性近似有效，測量函數在鄰域內必須相對平坦，即二階項大小與一階項大小相比是可以忽略的。基于這種思想，Bates和Watts提出了非線性強度的定量測量方法[11]。

右岸接頭土壩在麻石水電站擴建工程已建而擴建船閘未建時填筑，上游坡比為1∶2.0，下游坡比1∶1.5；采用黏土心墻防滲的型式，基礎布置帷幕灌漿。

首先將幾何學中的垂直投影概念推廣至n維歐式空間，可得到將Hessian矩陣H′k投影到切空間的正交投影矩陣[12]

式中Hk構成了切空間的正交基。則Hessian矩陣投影到切空間和正交空間的分量分別為

將二階項分解到切空間和正交空間的分量分別與一階項相比較，就可以得到參數效應曲率和固有曲率：

KT不但與數學模型相關，而且與測量模型采用的參數描述形式有關，KN則反映了非線性測量函數曲面的本質特性，與系統所采用的描述模型無關。參數效應曲率越小，對應的非線性模型的特性越接近線性模型，濾波性能越好。對于基于四元數的飛行器姿態估計問題，系統非線性主要由測量模型引入，測量模型非線性在確定狀態估計最優值時起著重要作用。

3.2 測量模型非線性強度分析

式中ω為角速度矢量，一般通過陀螺來測量；wb為高斯白噪聲。

由式（6）和式（7）中的定義可知，為了比較不同測量模型的非線性曲率，首先需要計算測量敏感性矩陣。對觀測方程直接求解Jacobi矩陣和Hessian矩陣涉及較大的計算量，本文采用修正羅德里格參數描述局部乘性姿態誤差，避免了對姿態矩陣求偏導數。由四元數乘積定義可得

式中為單位四元數姿態估計值；a為三維姿態參數，描述與q之間的姿態誤差δqa（）。為了使a與實際的誤差旋轉角φ一致，將其定義為四倍的修正羅德里格參數[14]

由于測量模型不依賴于陀螺偏差矢量，則一階測量敏感性矩陣為

將式（8）和式（9）代入式（4）并取其一階近似可得

綜合式（10）、（11）可得其一階測量敏感性矩陣為

對于單位矢量測量模型，由于h′（x）＝νb，所以對應的一階測量敏感性矩陣為

為了求解Hessian矩陣，首先考慮二階項對測量函數的影響，同時將約束條件擴展到二階，則測量方程可表示為

式中

式中H′i，k表示Hessian矩陣H′k的第i面；Φi代表n個基矢，n是測量函數列矢量的個數。根據姿態誤差定義可知

則測量方程二階項表達式為

由于ak為姿態真實值，無法精確得到，因此使用估計值近似代替，這樣就變為如下形式

為了減少計算量，便于比較，對方差矩陣P做如下假設

則測量函數二次項為

對于焦平面測量模型，由于

所以

對于單位矢量測量模型，由于

所以

從上述理論分析過程可知，式（12）和式（13）分別給出了兩種測量模型對應的一階測量敏感性矩陣，根據式（5）可以進一步獲得投影到切空間的正交投影矩陣，泰勒級數二階展開項則分別由式（14）和式（15）給出，這樣測量模型非線性曲率就可以根據式（6）和式（7）計算得到。

4 仿真分析

為了驗證上述所提到的非線性強度測量理論，以基于視線矢量測量的飛行器姿態確定為背景進行數學仿真分析，比較不同測量模型對姿態估計精度和收斂性能的影響。

由于采用兩個非共線的測量矢量就能夠唯一確定飛行器的姿態，所以為了簡化測量敏感性矩陣的計算，慣性參考單位方向矢量取為 [1 0 0]T和 [0 1 0]T。星敏感器視場角為5°×5°，測量周期為2s，測量精度為10″。陀螺測量白噪聲強度為0.02（°）／h1／2，漂移白噪聲強度為0.1（°）／h3／2，采樣周期為0.1s。采用四階龍格-庫塔法對傳播方程進行離散化。由于星敏感器和陀螺采樣周期的不同，假定每20個陀螺采樣周期狀態估計值更新一次。

為了使兩種模型的非線性曲率測量之間的對比更加明顯，初始姿態估計誤差選取為100°，初始陀螺漂移估計誤差選取為150（°）／h。角速度假設為 [0.1 0.03 0.03]Trad／s。圖2和圖3分別給出了觀測模型的參數效應曲率和固有曲率測量值。它們都能夠定量測量模型的非線性強度，但參數效應曲率與測量模型的具體描述形式有關，而固有曲率反映了測量模型的固有特性，因此圖2中兩個模型的曲率測量值差別很大，而圖3中的差別則很小。從仿真結果可看出，單位矢量測量模型具有更低的非線性強度。由于四元數姿態估計問題中非線性主要由測量模型引入，測量模型非線性強度大小直接影響濾波算法性能。因此，為了進一步驗證曲率測量理論的有效性，在上述仿真條件下，比較不同測量模型的姿態估計精度和收斂特性，圖4和圖5為100次蒙特卡羅仿真的姿態估計誤差平均值。由仿真結果可知，在初始狀態估計誤差較大情況下，使用非線性強度較高的焦平面測量模型的光學導航算法不能夠快速收斂至正確的穩態值，而基于單位矢量測量模型的光學導航算法則具有更高的估計精度和更快的收斂特性，即濾波算法性能與非線性曲率的變化一致，進一步驗證了非線性曲率測量能夠真實反映模型的非線性強度。因此，單位矢量測量模型更適合于光學導航系統，這種優勢在初始估計誤差較大和使用高精度測量設備時更為突出。

圖2 測量模型的參數效應曲率比較值Fig.2 Comparison of parameter-effect curvature between measurement models

圖3 測量模型的固有曲率比較值Fig.3 Comparison of intrinsic curvature between measurement models

圖4 焦平面測量模型姿態估計誤差Fig.4 Attitude estimation errors of focal plane measurement model

圖5 單位矢量測量模型姿態估計誤差Fig.5 Attitude estimation errors of unit vector measurement model

5 結束語

針對高精度光學導航系統測量模型選取問題，分析了測量模型非線性對導航性能的影響，并將微分幾何曲率測量理論應用于姿態估計問題中，研究了焦平面測量模型和單位矢量測量模型的非線性強度。仿真結果表明，單位矢量測量模型在估計精度和收斂特性等方面的性能更為優越，非線性曲率與導航系統性能保持一致，適合作為飛行任務前的數學仿真分析工具。

今后的工作將針對非線性濾波問題中濾波不一致的現象，研究合適的非線性補償方法，使得測量更新階段信息融合達到最優。

[1]BAYARD D S，BRUGAROLAS P B.On-board vision-based spacecraft estimation algorithm for small body exploration [J].IEEE Transactions on Aerospace and Electronic Systems，2008，44（1）：243-260.

[2]LI S，CUI P Y，CUI H T.Vision-aided inertial navigation for pinpoint planetary landing [J].Aerospace Science and Technology，2007，11（6）：449-506.

[3]TRAWNY N，MOURIKIS A I，ROUMELIOTIS S I，et al.Vision-aided inertial navigation for pin-point landing using observations of mapped landmarks[J].Journal of Field Robotics，2007，24（5）：357-378.

[4]PEREA L，HOW J，BREGER L，et al.Nonlinearity in sensor fusion：divergence issues in EKF，modified truncated SOF，and UKF [C].AIAA Guidance，Navigation and Control Conference and Exhibit，Hilton Head，USA，August 20-23，2007.

[5]CRASSIDIS J L，MARKLEY F L，CHENG Y.Survey of nonlinear attitude estimation methods [J].Journal of Guidance，Control and Dynamics，2007，30（1）：12-28.

[6]ZANETTI R，DEMARS K J，BISHOP R H.Underweighting nonlinear measurements [J].Journal of Guidance，Control and Dynamics，2010，33（5）：1670-1675.

[7]SHUSTER M D.Kalman filtering of spacecraft attitude and the QUEST model[J].The Journal of the Astronautical Sciences，1990，38（3）：377-393.

[8]SHUSTER M D，OH S D.Three-axis attitude determination from vector observations [J].Journal of Guidance，Control and Dynamics，1981，4（1）：70-77.

[9]CHENG Y，CRASSIDIS J L.Particle filtering for sequential spacecraft attitude estimation [C].AIAA Guidance，Navigation and Control Conference and Exhibit，Providence，USA，August 16-19，2004.

[10]VATHSAL S.Spacecraft attitude determination using a second-order nonlinear filter [J].Journal of Guidance，Control and Dynamics，1987，10（6）：559-566.

[11]BATES D M，WATTS D G.Nonlinear regression analysis and its application [M].New York：John Wiley，1988.

[12]程云鵬，張凱院，徐仲.矩陣論 [M].西安：西北工業大學出版社，2001：289-293.

[13]章仁為.衛星軌道姿態動力學與控制 [M].北京：北京航空航天大學出版社，1998：139-149.

[14]MARKLEY F L.Attitude error representations for Kalman filtering [J].Journal of Guidance，Control and Dynamics，2003，26（2）：311-317.