陣列式多MEMS-IMU協同導航系統與算法

沈 凱，左健文，左思琪，李岳倫，劉雅萱，郭文君

（北京理工大學自動化學院，北京 100081）

目前無人車輛上廣泛使用的組合導航系統是以全球衛星導航系統（Global Navigation Satellite System,GNSS）和慣性導航系統（Inertial Navigation System,INS）為基礎的導航定位系統。實踐表明，GNSS/INS組合導航系統在開闊無遮擋的環境中表現良好，可以為無人車輛提供高精度和高可靠性的導航定位信息，但當車輛行駛到反介入/區域拒止環境下,全球衛星導航系統能力被嚴重削弱[1]，此時通過慣性導航系統[2,3]實現無人車輛高精度的自主導航。與全球衛星導航系統相比，慣性導航系統是一種不依賴于外部信息、也不向外部輻射能量的自主式導航系統，其精確度、可靠性等性能直接影響著極端環境下系統部署和導航任務的完成。隨著微機電系統（Micro-Electro-Mechanical System, MEMS）技術的快速發展，成本低、體積小、重量輕的微慣性測量單元（MEMS-IMU）精確度、可靠性不斷提升，逐步在智能車輛、無人機和機器人導航與定位等領域應用[4-6]。

在全球衛星導航系統拒止環境下，MEMS-IMU誤差隨時間不斷累積。為了抑制MEMS-IMU誤差發散，一方面從硬件上（即硬件方式）改良慣性敏感元件的生產工藝或使用新材料，但研制周期長、成本高、難度大；另一方面從系統和算法設計上（即算法方式）來補償誤差，從而提升MEMS-IMU的精度。與硬件方式相比，采用算法方式能夠在很大程度上縮短研制周期，具有成本低、見效快、效果好等優點。

當前，為了實現慣導誤差補償，主要采取以下兩種算法方式：一是旋轉調制技術，通過采用旋轉或者翻轉等物理方式，周期性地改變IMU相對于導航系的方位，從而將IMU的常值誤差調制成周期性變化的物理量，促使其在一個周期內積分為零或一個極小的值，達到抑制IMU誤差發散的目的[7]；二是協同導航技術，通過聯合利用多個IMU的測量信息，采用互補濾波或信息融合算法[8]，精確地估計IMU的各項誤差，并對IMU測量誤差進行補償，從而提高IMU的導航定位性能。

為了實現不依賴衛星的高精度位置、速度和姿態等導航信息解算，本文從微慣性器件自身工作機理出發，分析影響其精度和可靠性的關鍵因素，綜合利用MEMS-IMU、旋轉調制和協同導航技術，分析對比了基于旋轉調制技術的雙MEMS-IMU協同導航方案，并設計了適用于自主無人系統導航的陣列式四MEMS-IMU協同導航系統，從而提升了MEMS-IMU的在線使用精度、可靠性等性能。

1 基于旋轉調制技術的微慣性測量單元

1.1 單軸旋轉調制型MEMS-IMU系統

旋轉調制型慣導是在IMU的外部加裝轉動機構和測角裝置，并采用一定的旋轉方案而構成的系統。旋轉調制型慣導按轉軸數目的多少可以分為單軸系統、雙軸系統和三軸系統等類型[9,10]，而每種類型又可以采用不同的轉動方案，例如連續旋轉、多位置轉停方案等[11,12]。

為了提高MEMS-IMU長時間工作的導航精度，采用旋轉調制技術將慣導誤差調制成周期性變化信號，能夠有效抑制MEMS-IMU誤差發散。圖1展示了單軸旋轉調制型MEMS-IMU，其主要由三部分組成，即MEMS-IMUA、轉臺以及電機。MEMS-IMUA與轉臺固聯，電機內部集成了光電編碼器作為測角機構，并采取繞平臺y軸正反180 °旋轉調制方案。

在圖1所示的單軸旋轉調制型MEMS-IMU中，影響MEMS-IMU測量誤差的主要因素包括：陀螺儀漂移、加速度計零偏誤差、標度因數誤差、安裝誤差以及測量噪聲等。為了建立單軸旋轉調制型MEMS-IMU模型，需要建立載體坐標系、平臺坐標系與傳感坐標系之間的轉換關系。

圖1 單軸旋轉調制型MEMS-IMUFig.1 Single-axis rotary modulation MEMS-IMU

在單軸旋轉調制型MEMS-IMU中，定義載體坐標系b的原點在載體的重心位置，坐標系與載體固聯，Xb軸指向載體的右方，Yb軸指向載體的前方，Zb軸指向載體的上方，構成右手系；定義平臺坐標系p與轉動機構固聯，其旋轉時與載體系在平面上的夾角定義為旋轉機構的旋轉角度；定義傳感器坐標系s與MEMS-IMU 固聯，其三個軸的方向分別與MEMS-IMU傳感器的三個敏感軸方向相重合，但與平臺系之間存在安裝誤差。

單軸旋轉調制型MEMS-IMU中平臺坐標系p到傳感器坐標系s的轉換矩陣為：

單軸旋轉調制型MEMS-IMU中載體坐標系b到平臺坐標系p的轉換矩陣為：

式中：φA為MEMS-IMUA的旋轉角。

此時，傳感器坐標系s下的角速率和加速度的量測模型為：

考慮MEMS-IMU的誤差通常都是較小的值，我們可以忽略安裝誤差與標度因數的乘積，作式(4)所示的近似處理，陀螺漂移誤差和測量誤差同理。

因此，平臺坐標系p下的角速率和加速度的量測模型為：

由于慣性導航系統的導航信息需要在導航坐標系下進行解算，為了方便討論，本文假設導航坐標系與載體坐標系重合。因此，式(5)所示的量測模型可以通過坐標變換矩陣轉化到導航坐標系下：

單軸旋轉調制型MEMS-IMU是最簡單、最易實現的系統之一，然而單軸旋轉調制型系統具有一個最大的缺點：旋轉軸方向上的誤差無法被調制，導致其誤差仍然隨時間累積而發散。

2.2 正交式旋轉調制型雙MEMS-IMU協同系統

為了解決單軸旋轉調制型MEMS-IMU系統旋轉軸方向上的誤差累積問題，采用繞不同軸旋轉的雙MEMS-IMU協同導航方法[12]，以提高導航系統誤差狀態的可觀測度，從而在線標定和補償各個MEMS-IMU的誤差。如圖2所示，MEMS-IMUA旋轉調制而MEMS-IMUB靜止（記為型系統），構成型正交式旋轉調制雙MEMS-IMU系統。

圖2 型正交式旋轉調制雙MEMS-IMU系統Fig.2 Dual MEMS-IMU cooperative navigation system based onorthogonal rotary modulation

為構建正交式旋轉調制雙MEMS-IMU誤差模型，將系統中旋轉調制 MEMS-IMUA 和靜止MEMS-IMUB的量測模型相減，得到：

為了進一步提高雙MEMS-IMU姿態誤差的可觀測度，可以采用MEMS-IMUA和B同時旋轉調制的正交式旋轉調制雙MEMS-IMU（記為型系統）方案。如圖3所示，MEMS-IMUA的旋轉調制方式為：繞其平臺坐標系y軸進行正反180 °旋轉，旋轉的角度為Aφ；MEMS-IMUB的旋轉調制方式為：繞其平臺坐標系的x軸進行正反180 °旋轉，旋轉的角度為Bφ。

圖3 型正交式旋轉調制雙MEMS-IMU系統Fig.3 Dual MEMS-IMU cooperative navigation system based onorthogonal rotary modulation

2 陣列式四MEMS-IMU協同導航系統及算法

2.1 陣列式四MEMS-IMU協同導航系統

針對無人車等自主無人系統的行駛特點，在利用雙MEMS-IMU獲取精準姿態信息的基礎上，為了進一步解算出更高精度的定位定向信息，本文提出了如圖4所示的陣列式四MEMS-IMU協同導航系統方案。

圖4 陣列式四MEMS-IMU協同導航系統Fig.4 Array four MEMS-IMU cooperative navigation system

為了充分發揮旋轉調制技術與協同導航系統技術的綜合優勢，對定向和定位誤差進行補償，最大限度地提升導航系統的精度和可靠性，把陣列式四MEMS-IMU協同導航系統劃分為3個子系統，每個子系統包含兩個MEMS-IMU，且子系統內部可以獨立工作，其中子系統劃分如表1所示。

表1 陣列式四MEMS-IMU協同導航系統的子系統劃分Tab.1 Subsystem division of array four MEMS-IMU cooperative navigation system

2.2 聯邦卡爾曼濾波算法

為了高效準確地處理陣列式四MEMS-IMU協同導航系統中各子系統的導航信息，本文采用聯邦卡爾曼濾波算法（Federated Kalman Filter, FKF）融合各子系統信息，如圖5所示。

圖5 聯邦卡爾曼濾波算法框架圖Fig.5 Framework of federated Kalman filter algorithm

以東-北-天地理坐標系作為導航坐標系，在本課題中，導航坐標系與載體坐標系重合，在卡爾曼濾波算法框架下，各子濾波器的狀態空間模型為：

式中：xi(t)為子系統各項誤差組成的狀態變量，fi(t)為狀態轉移矩陣，ui(t)為控制量，gi(t)為輸入矩陣，wi(t)為過程噪聲；zi(t)為測量輸出，hi(t)為測量矩陣，vi(t)為測量噪聲；i表示第i個子系統。

同理，系統的量測方程為：

進而，得到各個子濾波器的離散時間狀態方程和量測方程：

此時，聯邦卡爾曼濾波算法迭代過程如下。

聯邦卡爾曼濾波算法步驟一：子濾波器i初始化0 0 ? [ ]i i X =EX T 0 0 0 0 0 ? ?[()()]i i i i i P EX X X X= - -步驟二：子濾波器i時間更新| 1 | 1 1 ? ? i i i kk kk k X X-- -=Φ T T| 1 | 1 1 | 1 1 1 1 ?i i i i i i i kk kk k kk k k k P P-- - - - - -=Φ Φ+GQG步驟三：子濾波器i量測更新T T| 1 | 1[]i i i i i i i k kk k k kk k k K P H HP H R-1--=+| 1 | 1 ? ? ?()i i i i i i k kk k k k kk-X X KZ HX-= + -| 1 ? ?(I)i i i i k k k kk P KH P-= -步驟四：主濾波器信息融合1 1 1 ? ?[ ()]N g i k k i=P P --=∑ ； 1 ? ? ? ?[ () ]N g g i i k k k k i=1 X P P-X= ∑步驟五：信息共享與全局狀態估計? ? i g k k X=X；N i i=1∑β=1 1? i g k i k P=β-P； 1 i g k i k Q=β-Q

3 MEMS-IMU實驗平臺搭建及仿真試驗分析

3.1 旋轉調制型MEMS-IMU實驗平臺

為了研究旋轉調制型MEMS-IMU的工作性能，基于ADIS16405型MEMS-IMU，搭建了如圖6所示的實驗平臺。

圖6 旋轉調制型MEMS-IMU實驗平臺圖Fig.6 Experimental platform of rotary modulation MEMS-IMU

該系統主要由MEMS-IMU旋轉調制平臺、電源穩壓模塊、控制器模塊和數據采集與處理模塊等構成。MEMS-IMU旋轉調制平臺測量旋轉角速率和加速度；電源穩壓模塊為ADIS16405、控制器等提供工作電壓；控制器模塊提供所需的旋轉調制方案；數據采集與處理模塊用于讀取角速率、加速度、旋轉角等信息，并進行導航解算。

通過將搭建好的旋轉調制型MEMS-IMU實驗平臺分別設置為靜止狀態和旋轉調制狀態，然后讀取加速度、旋轉角速率的位姿測量信息。最后，利用高通濾波器對所得的系統量測數據濾波，從而獲得微慣性測量單元ADIS16405的實際噪聲。

3.2 無人車輛運動仿真設置

本實驗模擬的無人車輛運動情景如圖7所示。

圖7 無人車輛運動模擬情景示意圖Fig.7 Simulation of unmanned vehicle driving environment

無人車輛從GNSS信號良好路段駛入城市區域，由于城市高樓建筑遮擋GNSS信號，因此無人駕駛車輛主要依靠純MEMS-IMU進行慣性導航，以GNSS導航路段與MEMS-IMU導航路段交接處為起點，設置無人車輛的姿態角運動狀態為：

其中ψ為無人車輛的航向角，θ為俯仰角，γ為翻滾角，并設置無人車輛的東向和北向的運動狀態為：

在上述設置的無人車輛運動狀態條件下，慣性導航采用本文設計的陣列式多MEMS-IMU協同導航算法，并注入慣性器件ADIS16405采集的實際噪聲數據，然后仿真測試各導航方案的定向定位性能。

3.3 姿態解算實驗及對比分析

圖8 定向性能及對比分析圖Fig.8 Orientation performance and comparative analysis

為了更直觀地對比分析不同導航系統所能達成的定向性能，在表2中列出了均方根（Root Mean Square,RMS）指標下各方案姿態角解算誤差。

表2 導航系統姿態角解算誤差對比表Tab.2 Comparison of attitude angle calculation error of navigation system

3.4 位置解算實驗及對比分析

圖9 定位性能及對比分析圖Fig.9 Positioning performance and comparative analysis

表3展示了各個方案的位置解算誤差。

表3 導航系統位置解算誤差對比表Tab.3 Comparison of positioning calculation error of navigation system

3.5 定向定位綜合性能分析

表4 各方案導航誤差的RMS指標改善百分比Tab.4 Improvement percentage of navigation error of each system

4 結 論

在全球衛星導航系統失效后，慣性導航是實現自主無人系統高精度自主導航與定位的重要途徑。MEMS-IMU作為慣性導航的一種低成本途徑，其長時間工作的精確度和可靠性較差，為了解決這一問題，本文綜合利用MEMS-IMU、旋轉調制和協同導航技術，設計了陣列式四MEMS-IMU協同導航方案，并利用慣性器件ADIS16405采集的實際噪聲數據進行了仿真實驗。實驗結果表明：本文設計的陣列式四MEMS-IMU協同導航方案能夠有效抑制低成本MEMS-IMU的各項導航誤差；在自主無人系統運行120 s內，姿態角解算誤差不大于0.25 °；自主行駛1000 m時，東向和北向位置誤差不大于3.07%d和1.04%d。因此，陣列式四MEMS-IMU協同導航方案具有一定的實用性，能夠顯著提升低成本MEMS-IMU的導航性能，實現不依賴衛星的高精度位置、速度和姿態等解算。未來研究工作將進一步優化多MEMS-IMU協同導航系統相關算法，開展實驗驗證，致力于系統的具體實現。