基于ARM的小型化SINS/GNSS組合導航系統設計*

陳 灃,李小博,盧寶鋒,王新龍,陳文海

(西安現代控制技術研究所,西安 710065)

0 引言

隨著現代無人機小型化、輕量化,對于導航系統的體積、重量要求越來越嚴格。目前應用于無人機組合導航系統主要是由捷聯慣性導航系統(SINS)、衛星導航系統(GNSS)等進行組合導航。由于SINS誤差隨時間發散,長時間導航系統需要于GNSS進行組合。

ARM是有英國ARM公司設計的一系列32位RISC微處理器的總稱,在數據處理能力上已經可以滿足組合導航的運算速度要求,在成本上相對DSP有顯著的優勢。

在慣性導航系統的選擇方面,為了在性能不受影響的情況下達到體積最小、重量最輕,選用分立的MEMS陀螺/加速度計成為了最優方案。

1 SINS/GNSS系統軟硬件設計

組合導航系統的硬件部分由慣性測量組件、衛星定位儀、計算采集部分等組成。其中MEMS陀螺選用3個AD公司的單軸陀螺ADXRS642,零位穩定性約為180°/h;MEMS加速度計選用兩個AD公司的雙軸AD22293,零位重復性誤差約為5 mg。陀螺以及加速度計的空間布置方式保證載體系前上右3個方向均至少與一個器件的敏感軸重合。MEMS陀螺、加速度計輸出的模擬量由AD7689進行采樣,通過SPI總線與處理器芯片通信。處理器芯片選用意法半導體的STM32F401RE,最高主頻84 MHz,具有浮點運算單元(FPU)。GNSS芯片選用瑞士ublox公司的NEO-M8N芯片,輸出頻率最高10 Hz,通過TTL電平與處理器芯片的USART接口通信。整個系統實現體積小于90 mm×50 mm×10 mm,重量小于120 g。

組合導航系統的軟件部分由驅動軟件、ARM架構軟件及算法軟件組成。處理器間隔0.5 ms對AD7689采樣的結果進行一次采集,間隔10 ms進行一次慣性導航解算與卡爾曼濾波時間更新;收到GNSS發出的速度、位置信息,在下一個10 ms解算周期加入卡爾曼濾波量測更新,對姿態角、位置、速度進行反饋矯正。

2 慣性導航算法

由于MEMS器件采用印制電路板焊接方式保持的垂直度,MEMS陀螺和加速度計對應3個敏感軸的安裝誤差較大,需要用標定方法進行補償。在不同溫度下MEMS陀螺和加速度計的刻度系數與零位也不同,在這里近似認為是線性變化。

令載體坐標系為b(前上右),導航坐標系為n(北天東),慣性坐標系為i。載體系下比力為:

fb=KaUaccg

(1)

式中:Ka為相應溫度區間的加速度計刻度系數3×3矩陣,經過預先標定得到[1]；g為重力加速度；Uacc為AD測得的3個加速度計電壓扣除相應溫度區間零位的值。

載體系下角速率為:

(2)

式中:Kω為相應溫度區間的陀螺刻度系數3×3矩陣,經過預先標定得到[2]；Ugy為AD測得的3個陀螺電壓扣除相應溫度區間零位的值。

載體系下比力fb轉到導航系下為fn,扣除重力加速度g值可以得到導航系、載體系下加速度an、ab。

則導航系下速度迭代可以表示為:

Vn(t)=Vn(t-dt)+dt·an

(3)

式中:dt為慣性導航解算周期。

大地系下緯度、經度、高度迭代如下:

(4)

式中:Rm為子午圈曲率半徑;Rn為卯酉圈曲率半徑；Vn、Vu、Ve分別為北天東速度。

導航系相對慣性系的旋轉角速率為:

(5)

式中:ωie地球自轉角速率。

載體系相對導航系旋轉角速率在載體系下投影為:

(6)

令

(7)

(8)

姿態四元數迭代可以表示為[4]:

(9)

將上式泰勒展開即可以得到相應階數的姿態四元數離散迭代方式。通過姿態四元數又可以得到相應時刻的姿態轉換矩陣進而得到姿態角[3-4]。

3 組合導航方案設計

衛星導航芯片間隔約100 ms輸出載體位置、速度。令衛星導航芯片輸出的位置、速度分別為Psat、Vsat。

線性條件下,系統的狀態方程和量測方程的簡化形式為[7]:

(10)

Zk=HkXk+Vk

(11)

式中：Xk為k時刻系統狀態;Zk為k時刻測量值;Φk/k-1為tk-1時刻至tk時刻的一步轉移陣;Γk-1為系統噪聲驅動陣;Hk為量測陣;Vk為量測噪聲序列;Wk-1為系統噪聲序列

(12)

時間更新迭代方程為:

狀態一步預測[5]:

(13)

一步預測均方誤差:

(14)

式中,Q為系統噪聲協方差陣[6]。

量測更新迭代方程為:

(15)

式中:ZK為k時刻量測值,LSINS、VSINS分別為慣性導航解算得到的位置、速度,LSAT、VSAT分別為衛星導航芯片輸出的位置、速度。

濾波增益[5]:

(16)

狀態估計[5]:

(17)

均方誤差[5]:

(18)

式中R為量測噪聲協方差陣[6]。

由于在缺乏水平機動的情況下,航向角觀測性較差[7],故組合導航在平直運動時利用衛星導航相對地面的速度航向糾正航向角。算法采用序貫卡爾曼濾波方式設計。

令航跡角Yyaw與航向角ψyaw誤差為Δψ

Δψ=Yyaw-ψyaw

(19)

Yyaw由GNSS水平速度得到,ψyaw由慣性導航解算得到,Δψ定義域取為[-π,π)。

引入航向量測量[8]:

Zyawk=Δψk

(20)

4 組合導航系統車載實驗分析

為了驗證組合導航總體性能,2017年10月進行了跑車實驗。跑車路線為近似直線路線、中途無停頓。共有兩組有效數據進行評估。車載高精度慣性導航系統的姿態角誤差小于0.02°/h。車載GPS位置精度優于10 m、速度精度優于0.5 m/s。將車載導航系統的輸出與文中所述導航系統輸出進行比較,誤差統計表格如表1所示。

表1 誤差統計表格

姿態角誤差統計結果包括了初始安裝誤差、初始對準誤差。取兩組中較差的RMS統計值,滾轉角、俯仰角誤差約為0.5°,航向角誤差約為0.9°。

得到的誤差曲線如下:

圖1 姿態誤差曲線

圖2 位置誤差曲線

圖3 速度誤差曲線

由位置、速度統計數據及曲線可以看出,組合導航系統位置、速度精度應優于或等于車載GPS精度。

由于所用陀螺零位漂移達到0.05°/s,160 s漂移約為8°。從姿態角誤差曲線來看,誤差基本保持在1°以內,誤差保持收斂狀態。航向誤差由于其觀測性差,當載體近似勻速直線運動時仍然會發散。加入航向量測后,航向角誤差成功收斂到1°以內。

5 結論

利用低成本、小型化MEMS IMU器件進行慣導解算,與微小型GNSS芯片低頻率輸出的位置、速度信息進行卡爾曼濾波松組合,可使姿態角、位置、速度誤差保持在較小范圍內。實驗結果表明,在微小型無人機等對空間、重量要求嚴格的應用中,該設計具有顯著意義。