基于雙慣性器件的角速度差分相對姿態測量方法

張 天,靳舒馨,王 強,劉 剛,劉鐵成

（1.中國兵器工業計算機應用技術研究所,北京 100089;2.北京衛星導航中心,北京 100094; 3.清華大學電子工程系,北京 100084）

0 引言

相對位姿測量技術利用傳感器的特征信息解算出被測物相對于載體的位置與姿態,廣泛應用于航天器空間對接、機器人控制、頭盔瞄準器中[1]。在諸多相對位姿測量手段中,慣性測量無需外部信號、計算消耗低并且積分穩定,能夠在惡劣環境中維持穩定輸出,通常作為位姿測量濾波器的核心狀態量[2-3]。由于捷聯慣導的姿態解算會隨陀螺漂移產生累積誤差,在慣性系中通常由GNSS、重力與磁強計、視覺等提供姿態觀測信息,實時修正積分結果[4]。

想要通過慣性技術獲得載體坐標系下被測物的相對姿態,一種思路使用被測物和載體上各自IMU,解算出被測物相對慣性系和載體相對慣性系的實時姿態,將這兩個姿態對齊到載體坐標系中獲取到相對姿態[5]。但在該方法中,由于被測物處于載體內部,GNSS、磁強計等信息無法用于結果修正,相對視覺測姿又難以引入到兩個獨立的捷聯慣導解算中,這限制了該方法的應用范圍[6]。

相對姿態解算的另一種思路是引入當前時刻的相對旋轉矩陣Rrel,將兩個IMU 所測角速度值變換為當前時刻被測物與載體的相對角速度,再積分得到被測物與載體的相對姿態[5]。在該方法中,視覺等相對位姿信息可直接用于慣性位姿測量結果修正[7],但當外部修正失效時（視覺測姿失效時）,每次解算時使用的相對旋轉矩陣Rrel都由上一時刻的姿態解算得到,Rrel中的誤差會隨積分過程迅速累積,導致測量誤差迅速增大[8]。目前,大部分研究集中于使用視覺與慣性姿態測量融合方法[9-10],而缺少相關研究驗證純慣性相對姿態測量的有效性與精度。

鑒于上述原因,為給出合理的慣性相對位姿測量方法,本文首先結合數學關系推導了相對姿態測量的遞推公式,證明了角速度差分相對姿態測量方法是傳統獨立積分方法的等價變換。此后,根據推導過程給出了具體的簡化算法,并對算法適用范圍進行了分析。最后,為驗證角速度差分相對姿態測量方法的正確性,搭建了實驗平臺,對算法進行了工程適用性驗證。

1 差分姿態算法推導

1.1 捷聯慣導姿態解算

首先給出捷聯慣導姿態解算公式便于后續推導[7,11]。使用四元數來表示旋轉,對于表征慣性系i系至捷聯坐標系b系的旋轉四元數Q,有變換四元數與坐標系旋轉角速度微分方程

式（1）中,的獲取滿足

式（2）中,ωbib為捷聯陀螺的輸出,Cbn由姿態更新的最新值確定,ωnen和ωnie分別為位置速率和地球自轉角速率。

其中,?為四元數乘法,有

對于相同采樣時間間隔的角增量,采用Picard求解法求解四元數微分方程,在時間段tk至tk+1滿足定軸轉動條件時,有

其中,

式（5）中,旋轉角Δθb（tk）的物理意義為tk-1時刻b系到tk時刻b系的旋轉角度,Δθx、Δθy、Δθz為陀螺在采樣時間間隔內補償后的角增量。

1.2 相對旋轉性質推導

為獲得兩個已知相對旋轉關系旋轉的矩陣指數運算性質,這里進行假設,兩個初始時刻對齊于慣性坐標系i的兩個坐標系——坐標系a與坐標系b,在一段微小的時間τ內,相對坐標系i存在微小旋轉,記為θa與θb,如圖1所示。根據式（4）,可得到這兩個微小旋轉

圖1 相對慣性系的微小旋轉示意圖Fig.1 Schematic diagram of small rotation for relative inertial system

Qa與Qb為旋轉發生后的兩坐標系對應的旋轉四元數。此時,坐標系b到坐標系a的相對旋轉為

采用Picard 求解法求解四元數微分方程,在滿足定軸轉動條件時,微分方程的解為

該微分方程的解可以寫為

式（10）中,的定義如下

根據定義可知: 式（11）中,物理含義為將b系相對i系的旋轉運算M′（θb）通過M′（θa）的旋轉操作,變換為a系中觀察b系相對i系的旋轉運算,即M′（θb）為從b系觀察b系相對i系的旋轉運算,而為從a系觀察b系相對i系的旋轉運算。

式（10）和式（11）表明了兩個初始時刻對齊于慣性坐標系i的兩個坐標系——坐標系a與坐標系b,在一段微小的時間τ內,相對坐標系i存在微小旋轉,它們的相對旋轉等同于將坐標系a與坐標系b的旋轉都轉換到坐標系a中,從a系觀察兩個坐標系相對運動的一次旋轉。

簡言之: 兩個已知相對旋轉關系的旋轉矩陣,其指數函數的積等同于統一坐標系后差分旋轉矩陣的指數函數。該相對旋轉性質是差分測姿迭代算法推導的基礎。

1.3 差分迭代算法推導

在被測物與載體的純慣性差分測姿過程中,約定載體IMU 坐標系為p,被測物IMU 坐標系為h,空間坐標系為i。載體IMU 與載體固連,可在慣性系自由運動,被測物IMU 固連在被測物上與載體相對運動,具體如圖2所示。

圖2 運動載體中姿態測量的坐標系示意圖Fig.2 Schematic diagram of coordinate system for attitude measurement in dynamic vehicles

根據上述坐標系定義分析相對運動條件下四元數微分方程的解。如果此時被測物坐標系到空間坐標系的旋轉四元數Qh（tk）、載體坐標系到慣性坐標系的旋轉四元數Qp（tk）已知,按照式（4）展開公式對Qh（tk）、Qp（tk）展開可得

定義k時刻被測物坐標系到載體坐標系的相對旋轉為Qrel（tk）,則有

將式（12）、式（13）帶入到式（14）中,有

式（16）表明了相對旋轉四元數存在遞推關系。依據1.2 小節推導的差分測姿運算性質公式（式（10））,使用表示載體坐標系下觀察tk-1時刻至tk時刻被測物IMU 的旋轉角度,帶入式（16）中,將指數函數的積變換為統一坐標系后差分旋轉矩陣的指數函數,有

根據相對旋轉的積分關系,相對旋轉可以寫為角速度的積分形式

式（18） 中,Rrel（tk-1）為tk-1時刻的相對旋轉矩陣,可以與相對旋轉四元數Qrel（tk-1）相互轉換,帶入整理得到Qrel（tk）與角速度相關的遞推公式

式（15）與式（19）表明,可以通過慣性系中載體角速度ωp（t）與慣性系中被測物角速度ωh（t）使用遞推方法得到被測物相對載體的旋轉四元數,其各自角速度解算后得到的姿態結果的差分等同于它們差分角速度的姿態解算結果。

2 兩種算法比較

根據1.3 小節的討論,分別利用兩個慣性器件的角速度輸出進行獨立姿態解算,差分得到相對姿態結果的傳統方法稱為“獨立積分法”;本文提出的使用兩個角速度輸出與當前時刻的相對旋轉矩陣獲取相對角速度,利用相對角速度積分得到相對姿態的方法稱為“角速度差分法”。

在“獨立積分法” 中,需要分別通過慣性積分計算被測物與慣性系、載體與慣性系的姿態信息,并在最后使用差分運算,獲得被測物與載體的相對姿態。根據式（12）～式（15）,整理成算法如下:

1）根據被測物與載體的捷聯慣導獲取角速度信息ωh（t）與ωp（t）,計算當前時刻角增量Δθh（tk）與Δθp（tk）

2）使用當前時刻角增量Δθh（tk）與Δθp（tk）計算四元數獨立積分Qh（tk）與Qp（tk）

3）使用獨立積分Qh（tk）與Qp（tk）計算相對旋轉四元數Qrel（tk）

在“角速度差分法” 中,在每一步使用上一次相對姿態的迭代結果,獲取被測物與載體的相對角速度,再通過慣性積分直接獲得被測物相對載體的姿態信息。根據式（17）～式（19）,寫成算法如下:

1）根據被測物與載體的捷聯慣導獲取角速度信息ωh（t）與ωp（t）,計算當前時刻相對角增量Δθrel（tk）

2）使用當前時刻相對角增量Δθrel（tk）計算相對旋轉四元數Qrel（tk）

對比“獨立積分法” 與“角速度差分法”,從算法精度上看,二者均使用ωh（t）與ωp（t）作為捷聯慣導解算輸入量,均使用了Δt內的Picard 求解法求解四元數微分方程,具有相同的定軸轉動邊界條件,因此不考慮計算舍入誤差的影響時,二者的理論精度基本相同。可以近似認為獨立積分方法與直接差分方法具有基本相同的解算效果,這兩種相對姿態測量的主要誤差均源自捷聯慣導中陀螺儀采樣誤差。

從算法的便捷性上看,直接差分方法相比獨立積分方法獲取Qrel（tk）的單次解算計算量更小,并且計算過程中引入了實時迭代的Rrel（tk-1）,便于濾波器外部觀測接入時修正慣性測量引起的相對誤差,是一種在工程上更具實用價值的慣性相對測姿算法。

3 實驗設計

3.1 雙IMU 數據同步采集

為驗證差分測姿算法的有效性,選用兩個IMU分別安裝在載體上和被測物上,通過MCU 進行同步數據采集。考慮到載體的機動性,選用體積小、質量小、具有同步觸發功能的IMU 進行數據采集實驗[12-13]。在本實驗中,選型ADI 公司的六軸微機電慣性測量單元ADIS16475,該傳感器常溫漂移量小于5（°） /h、量程大于300（°） /s,接近戰術級精度,滿足短時測姿精度需求[14]。該IMU 對外數據接口為SPI 總線,最高采樣頻率為2kHz,具備同步觸發功能,可實現慣性數據的高頻實時采集。

為實現兩個IMU 同步觸發,設計被測物上與載體上的兩個IMU 的硬件電路原理如圖3所示。兩個MCU（微控制單元） 電路采用外部12V 供電,使用SPI 接口采集各自IMU 數據,采集頻率為500Hz,并每隔1s 同步一次系統時鐘。采集后的IMU 數據匯總到載體MCU 后傳輸到測試計算機中標記時間戳,用于后續數據處理。按照上述原理設計的被測物上MCU 電路與IMU 實物如圖4所示。

圖3 搭載雙IMU 的最小系統Fig.3 Diagram of minimum system equipped with dual IMU

圖4 被測物上MCU 電路與IMU 實物Fig.4 Diagram of MCU circuit and IMU on the tested object

3.2 差分測姿環境搭建

為模擬載體運動,使用六個機械電缸驅動的六自由度并聯運動臺,通過控制器的運動學解算產生X軸、Y軸、Z軸的三軸平動以及圍繞橫滾軸、俯仰軸、航向軸的三個旋轉運動[15]。本實驗采用的六自由度運動臺平動精度優于1mm,運動范圍優于±0.3m,角運動精度優于0.1°,運動范圍優于±20°,響應延遲優于10ms,可模擬載體常規運動。按照使用習慣定義該六自由度運動臺模擬載體運動軸向,如圖5所示。

圖5 載體運動軸向的定義Fig.5 Definition of the carrier motion axis

為模擬雙IMU 差分工作環境,在六自由度運動臺上的安裝基準面上設置載體IMU 以及搭載被測物IMU 的雙軸轉臺,原理如圖6所示。其中,高精度雙軸旋轉臺安裝在六自由度運動臺的安裝基準面上,模擬被測物與載體的俯仰角與航向角相對姿態運動。該雙軸轉臺可在航向角與俯仰角上連續運動,角度反饋精度優于0.01°。被測物IMU 設置在雙軸旋轉臺的俯仰軸平臺上,通過安裝調整將IMU 敏感軸與轉臺運動軸對齊。載體IMU 設置在六自由度運動臺的安裝基準面上,通過安裝調整將IMU 敏感軸與六自由度運動臺的運動軸重合。

圖6 相對姿態測量實驗原理圖Fig.6 Schematic diagram of relative attitude measurement experiment

按照上述原理搭建的測試環境如圖7所示。完成實驗環境布置后進行設備標定,將被測物IMU、載體IMU 坐標系對齊,開始數據采集。

圖7 相對姿態測量實驗環境Fig.7 Diagram of relative attitude measurement experimental environment

3.3 測姿數據采集

在雙IMU 差分實驗中,六自由度運動臺的各軸以不同頻率做正弦運動模擬載體自由運動。實驗中的運動幅值、頻率與相位選取考慮到載體的較大運動范圍以及避免運動軸周期重合,設定X軸平動幅值50mm、頻率0.25Hz、相位20°,Y軸平動幅值50mm、頻率0.3Hz、相位50°,Z軸平動幅值50mm、頻率0.2Hz、相位70°,俯仰軸幅值10°、頻率0.1Hz、相位90°,橫滾軸幅值10°、頻率0.12Hz、相位0°,航向軸幅值180°、頻率0.05Hz、相位40°。各運動軸幅值、頻率、相位如表1所示。

表1 載體六軸正弦運動Table 1 Six-axis sinusoidal movement of the carrier

雙軸轉臺進行一組覆蓋航向角與俯仰角運動范圍的定點運動,其運動按照如下的方式生成:將雙軸轉臺航向角范圍（-180°～180°）和俯仰角范圍（-75°～75°）均勻劃分為20 個區域,并在每個航向角區域、俯仰角區域內隨機選取一個角度,獲取20 個航向角角度、20 個俯仰角角度,再從20個航向角與20 個俯仰角中不放回抽取,依次組合成為20 個姿態角。在本實驗中,隨機生成的20 組雙軸轉臺運動的姿態角如表2所示。

表2 雙軸轉臺運動的姿態角Table 2 Attitude angles of two-axis turntable movement

完成上述運動數據準備后,按照如下順序進行雙IMU 差分測試數據采集實驗:

1）開啟六自由度運動臺,使運動臺六軸按表1做不同頻的正弦運動以模擬載體自由運動情況;

2）開啟測試計算機,開始雙IMU 差分數據采集,在收到IMU 數據時標記時間戳并記錄;

3）控制雙軸轉臺按照表2從零位出發,依次運動到20 個姿態角,每個角度運行到位置后采集5s數據,20 個姿態角運行完成后,雙軸轉臺回到零位;

4）結束數據記錄并停止六自由度運動臺,結束數據采集實驗。

3.4 數據分析

使用“獨立積分法” 與“角速度差分法” 對數據進行處理,選取俯仰軸數據與轉臺回傳的真實數據進行比較,如圖8所示。可以看出,在450s 內,兩種測姿方法在20 個選取姿態角位置點的解算數據幾乎與轉臺真實數據相同,三條曲線幾乎重合。為了更清晰地觀察測姿結果的準確性,選取7 號位置點局部放大,如圖9所示。可以看出,獨立積分法與角速度差分法的測姿輸出曲線幾乎重合,與轉臺真值相比存在一定測姿誤差。

圖8 俯仰角測姿數據與轉臺實際數據對比Fig.8 Comparison of pitch angle attitude measurement data with actual turntable data

圖9 俯仰角測姿數據與轉臺實際數據對比局部放大圖Fig.9 Partial enlarged view of comparison between pitch angle attitude measurement data and actual turntable data

將“獨立積分法” 與“角速度差分法” 得到的俯仰軸測姿結果減去轉臺俯仰軸返回真值得到測姿誤差,繪制測姿誤差角度隨時間變化曲線,如圖10所示。可以看出,在450s 內,獨立積分法的RMSE 為0.490°,角速度差分法的RMSE 為0.510°,兩種測姿方式均具有較高的測姿準確性。

圖10 俯仰角測姿誤差Fig.10 Attitude measurement error of pitch angle

將兩種測姿結果作差,繪制測姿差別隨時間變化曲線,如圖11所示。可以看出,兩種測姿方式差別的RMSE 為0.060°,相較于約0.510°測姿誤差RMSE,基本可以認為兩種測姿方法等同。

圖11 俯仰角“獨立積分” 與“角速度差分” 的差別Fig.11 Differences between independent integral and angular velocity difference for pitch angle

同樣選取“獨立積分法” 與“角速度差分法”的航向軸數據與轉臺回傳的真實數據進行比較,如圖12所示。可以看出,在450s 內,三條曲線幾乎重合。為了更清晰地觀察測姿結果的準確性,選取10 號位置點局部放大,如圖13所示。可以看出,兩種測姿方法的輸出曲線幾乎重合,與轉臺真值相比存在一定測姿誤差。

圖12 航向角測姿數據與轉臺實際數據對比Fig.12 Comparison of heading angle attitude measurement data with actual turntable data

圖13 航向角測姿數據與轉臺實際數據對比局部放大圖Fig.13 Partial enlarged view of comparison between heading angle attitude measurement data and actual turntable data

將“獨立積分法” 與“角速度差分法” 得到的航向軸測姿結果減去轉臺航向軸返回真值得到測姿誤差,繪制測姿誤差角度隨時間變化曲線,如圖14所示。可以看出,測姿誤差在±1.5°范圍內,獨立積分法的RMSE 為0.372°,角速度差分法的RMSE 為0.372°,兩種測姿方式均具有較高的測姿準確性。

圖14 航向角測姿誤差Fig.14 Attitude measurement error of heading angle

將兩種測姿結果作差,繪制測姿差別隨時間變化曲線,如圖15所示。可以看出,兩種測姿方式差別的RMSE 為0.015°,相較于0.372°測姿RMSE,基本可以認為兩種測姿方法等同。

圖15 航向角“獨立積分” 與“角速度差分” 的差別Fig.15 Differences between independent integral and angular velocity difference for heading angle

將俯仰軸數據與航向軸數據對比可以看出,航向軸測姿精度略優于俯仰軸,該現象主要由雙軸轉臺未修正的安裝誤差、六自由度運動臺的高頻振動對陀螺角速度輸出的影響造成。

4 結論

本文從捷聯慣導姿態解算出發,通過雙IMU差分測姿公式推導,提出了一種基于雙慣性器件的角速度差分相對姿態測量方法,并設計了雙IMU差分測姿實驗,從數學上與實際應用上證明了本文提出的“角速度差分法” 與傳統“獨立積分法”具有相同的測姿結果。從實驗結果可以看出,“角速度差分法” 與“獨立積分法” 兩種差分測姿解算的偏差為0.060°（1σ,俯仰軸）與0.015°（1σ,航向軸）,均比0.510°（1σ,俯仰軸） 與0.372°（1σ,航向軸）的慣性測姿誤差小1 個數量級,基本可以認為兩種測姿方法等同。相較于傳統的“獨立積分法”,本文提出的“角速度差分法” 更便于引入外部觀測量,構造濾波器,具有更強的工程適用性。后續可以圍繞濾波器構造進一步探究,消除純慣性差分測姿過程中的誤差累積,進一步提升算法實用性。