慣性導(dǎo)航一般集成在GPS設(shè)備中，都是由供應(yīng)商集成，那在這里有什么討論的必要呢，要知道在車輛行駛中，我們可以拿到GPS的yawrate和speed信號，而且車輛本身還有一套傳感器獲取yawrate和speed，又因?yàn)楹桔E推算是自動(dòng)駕駛很重要的一部分，所以理解慣性導(dǎo)航的工作原理，能很好地幫助我們做基于車身的航跡推算。

慣性導(dǎo)航

目前GNSS+IMU構(gòu)成的組合導(dǎo)航系統(tǒng)是主流的定位系統(tǒng)方案，慣性導(dǎo)航系統(tǒng)是唯一可以輸出完備的六自由度數(shù)據(jù)的設(shè)備、數(shù)據(jù)更新頻率高、是定位信息的融合中心。

慣導(dǎo)中使用的核心算法主要包括3種：1. 慣性導(dǎo)航解算算法；2. 組合導(dǎo)航的卡爾曼濾波器的耦合。3. 環(huán)境特征信息與慣性導(dǎo)航融合。

組合導(dǎo)航系統(tǒng)核心算法框架

硬件及原理

慣性導(dǎo)航系統(tǒng)（INS）是利用慣性傳感器（IMU）測量載體的比力及角速度信息，結(jié)合給定的初始條件，與 GNSS等系統(tǒng)的信息融合，從而進(jìn)行實(shí)時(shí)推算速度、位置、姿態(tài)等參數(shù)的自主式導(dǎo)航系統(tǒng)。具體來說慣性導(dǎo)航系統(tǒng)屬于一種推算導(dǎo)航方式。即從一已知點(diǎn)的位置根據(jù)連續(xù)測得的運(yùn)載體航向角和速度推算出其下一點(diǎn)的位置，因而可連續(xù)測出運(yùn)動(dòng)體的當(dāng)前位置。

慣性系統(tǒng)工作原理圖

慣性導(dǎo)航系統(tǒng)采用加速度計(jì)和陀螺儀傳感器來測量載體的運(yùn)動(dòng)參數(shù)。其中三個(gè)垂直布置的陀螺儀用于測量載體繞自身三個(gè)坐標(biāo)軸的轉(zhuǎn)動(dòng)角速度，同時(shí)也敏感地球自轉(zhuǎn)的角速度。

加速度計(jì)基于牛頓第二定律，采用電容式、壓阻式或熱對流原理，通過在加速過程中對質(zhì)量塊對應(yīng)慣性力的測量來獲得加速度值。用來測量運(yùn)動(dòng)體坐標(biāo)系上各軸的加速度。     

慣性系統(tǒng)工作原理圖

慣導(dǎo)通過對陀螺儀測量的角速度進(jìn)行積分運(yùn)算和坐標(biāo)變換，計(jì)算車體的姿態(tài)角（橫滾、俯仰角）和方位角。根據(jù)姿態(tài)角可以計(jì)算出重力加速度在各個(gè)坐標(biāo)軸上的分量，加速度計(jì)測量得的各軸加速度，減去重力加速度分量后積分，得到速度和位置。慣導(dǎo)計(jì)算得到的狀態(tài)，用于預(yù)測車輛當(dāng)前的位置，再和衛(wèi)星定位接收機(jī)得到的位置（或觀測數(shù)據(jù)）進(jìn)行比較。比較的偏差包含了慣導(dǎo)的推算誤差和衛(wèi)星接收機(jī)的定位誤差，通過數(shù)據(jù)融合算法進(jìn)行加權(quán)后，用于修正慣導(dǎo)的預(yù)測，讓慣導(dǎo)的預(yù)測越來越準(zhǔn)確。

慣性導(dǎo)航解算算法

通常分以下幾步：

• 姿態(tài)更新：對陀螺儀輸出的角速度進(jìn)行積分得到姿態(tài)增量，疊加到上次的姿態(tài)上；
• 坐標(biāo)轉(zhuǎn)換：從IMU載體坐標(biāo)系到位置、速度求解坐標(biāo)系（慣性坐標(biāo)系）；
• 速度更新：需要考慮重力加速度的去除，得到慣性系下的加速度，通過積分得到速度；
• 位置更新：通過速度積分得到位置。

慣性導(dǎo)航解算算法原理圖

在慣性導(dǎo)航中，導(dǎo)航方程的每一次迭代都需要利用上一次的導(dǎo)航結(jié)果作為初始值，因此慣導(dǎo)的初始化是比較重要的部分之一。姿態(tài)對準(zhǔn)是指得到IMU的roll, pitch, yaw。roll, pitch的對準(zhǔn)過程一般稱為調(diào)平。使當(dāng)車靜止時(shí)，加速度計(jì)測量的比力僅由重力導(dǎo)致，可以通過f=C*g來求解；對于非常高精度的IMU可通過羅經(jīng)對準(zhǔn)的方式，車靜止時(shí)，通過測量載體系中的地球自轉(zhuǎn)來確定載體的方位(yaw)。

慣性導(dǎo)航初始化原理圖

組合導(dǎo)航的卡爾曼濾波器的耦合

使用Kalman濾波器的耦合，對IMU和GNSS即點(diǎn)云定位結(jié)果進(jìn)行融合。可分為松耦合和緊耦合兩種方法。

松耦合濾波器采用位置、速度量測值和解算的位置速度之差作為組合導(dǎo)航濾波器輸入，也即卡爾曼濾波器的量測量。緊耦合的數(shù)據(jù)包括GNSS的導(dǎo)航參數(shù)、定位中的偽距、距離變化等。 

卡爾曼濾波器的松耦合原理圖

卡爾曼濾波器的緊耦合原理圖   

卡爾曼濾波器松耦合和緊耦合的優(yōu)點(diǎn)和不足對比

以百度阿波羅使用的慣導(dǎo)系統(tǒng)為例，采用了松耦合的方式，并且使用了一個(gè)誤差卡爾曼濾波器。慣性導(dǎo)航解算的結(jié)果用于Kalman濾波器的時(shí)間更新，即預(yù)測；而GNSS、點(diǎn)云定位結(jié)果用于Kalman濾波器的量測更新。Kalman濾波會輸出位置、速度、姿態(tài)的誤差用來修正慣導(dǎo)模塊，IMU期間誤差用來補(bǔ)償IMU原始數(shù)據(jù)。   

百度阿波羅卡爾曼濾波器的松耦合

卡爾曼濾波融合示意

環(huán)境特征信息與慣性導(dǎo)航融合

目前常用的GNSS+IMU組合慣導(dǎo)方案在一些場景的定位精度穩(wěn)定性仍不能完全滿足自動(dòng)駕駛的要求。例如，城市樓宇群、地下車庫等GNSS長時(shí)間信號微弱的場景下，依靠GNSS信號更新精確定位穩(wěn)定性不足，因此必須引入新的精確定位更新數(shù)據(jù)源，在組合慣導(dǎo)中引入并融合激光雷達(dá)/視覺傳感定位等環(huán)境信息進(jìn)行融合定位成為必然趨勢。   

一種組合導(dǎo)航和環(huán)境感知信息融合的架構(gòu)示意圖

以百度阿波羅的多傳感器融合定位系統(tǒng)解決方案為例，慣性導(dǎo)航系統(tǒng)處于定位模塊的中心位置，模塊將IMU、GNSS、Lidar等定位信息進(jìn)行融合，通過慣性導(dǎo)航系統(tǒng)解算修正后最終輸出滿足自動(dòng)駕駛需求的6個(gè)自由度的高精度位置信息。

百度阿波羅的慣性融合定位模塊框架