簡化INS與GPS組合系統在三維路面上的導航

這一章節討論一種適用於任何輪式平臺（含GPS）的航跡推算（DR）方法，該方法可消除使用傳統IMU尤其是低精度IMU時產生的一些誤差。
1.首先分析討論一個完整的INS系統，再概述用於處理誤差源的理論方法。
2.介紹了一種簡化的慣性傳感器系統，並與傳統IMU進行比較。推導簡化INS的機械化方程。
3.接着，描述基於KF 的鬆耦合和緊耦合的簡化慣性傳感器系統與GPS的集成，包括線性化的系統模型和各種組合方案的測量模型。

1 MEMS級IMU的三維定位的性能分析

在GPS拒止的情況下，以低精度MEMS級IMU作爲慣性傳感器進行三維導航，三種未經補償的bais如下：三個加速度計中未經補償的bias；水平對齊的陀螺儀未經補償的bias；豎直對齊的陀螺儀未經補償的bias。
正如第7章討論的，LKF和EKF都試圖通過傳感器的誤差模型對這些偏差進行補償。在GPS可用時，誤差模型間接得到更新。在GPS中斷時KF濾波以預測模式運行，並從機械化解中減去INS誤差的預測值進行修正。但這種補償是不完善的，仍然存在一些未補償的剩餘誤差。
下面將討論這些未補償偏差的影響。

第一個誤差源，（第四章提到）當通過加速度計算位置與速度時，未經補償的加速度偏差 bf 將會產生以下影響：
1 速度誤差與時間 t成正比 （GPS拒止時，KF工作在預測模式下）

2 位置誤差與 t 的平方成正比

第二個誤差源，兩個水平方向的陀螺儀之間存在一個未經補償的偏差 bw ：
1 俯仰角或橫搖角的誤差與時間成正比

這一小角度誤差會造成INS無法正確對齊。當加速度從 b 系投影到 L系時，由於這一偏差的存在，會導致加速度矢量的投影產生誤差。

2 加速度在水平方向上的誤差（L系）

在GPS拒止期間，deta（theta）爲一個很小的角，通過近似 sin（deta theta）== data theta

3 偏差 bw將導致速度誤差以及位置誤差

由上式，陀螺儀誤差在位置上產生了三階誤差。當INS單獨工作時，這些陀螺儀誤差將是位置和速度誤差中最具影響力的部分。

誤差的第三個來源：是由於垂直方向上未補償的陀螺儀誤差bw，會引起方位角的誤差。

同樣，該方位角誤差與時間成正比。

2 解決MEMS級IMU在路面導航中存在的問題

land-based vehicle :陸地車輛場景
這一小節主要概述適用於(帶有里程計或輪式編碼器)陸地車輛的導航方法。通過一些方法來消除低精度IMU帶來的未經補償的誤差，使得慣性傳感器的成本最小化。
爲了解決上一節中提到的第一個誤差（由加速度計偏差引起），可以將里程計的速度用到導航解算中。
爲了克服第二個誤差（水平陀螺儀偏差），俯仰角與橫滾角將綜合加速度計與里程計的測量值得到。
以下給出pitch角與roll角的簡要推導方程：
當車輛靜止時，若俯仰和滾轉軸所在平面與水平面之間存在傾斜，則加速度計測量可以在這兩個軸上測量到重力分量。加速度計的測量值爲：

因爲上式只使用了沿着 X 和 Y 方向的兩個加速度計使，則，俯仰角和橫搖角可以表示爲：

當車輛移動時，前向加速度計( y )(已修正傳感器誤差)測量車輛前向加速度以及重力分量。爲了計算俯仰角，前向加速度計測量值必須減去里程計測量得的車輛加速度：

其中，g爲重力加速度，fy爲前向加速度計測量值，aod爲里程計測量的車輛加速度。

與之類似，橫向加速度( x )包含了車輛加速度的垂直分量以及重力分量。爲了計算roll角，必須對加速度的垂直分量進行補償：

其中，fx爲橫向加速度測量值，vod爲里程計測得的速度，wz爲z軸上的陀螺儀測得的角速率。vod 與wz相乘即爲車輛加速的垂直分量。

對於低精度的MEMS IMU來說，通過兩個加速度計來計算pitch和roll角通常比使用兩個陀螺儀在計算具有更大優勢。
原因1： 使用兩個水平陀螺儀計算俯仰和橫搖，涉及到積分操作，而使用加速度計算則不需要。積分的缺點在於，未經補償的傳感器誤差通過積分累積，造成定位精度的不斷下降。
原因2： 陀螺儀與加速度計相比，加速計更便宜
原因3： 垂直方向的陀螺儀的偏差需要用到組合導航的濾波器進行建模。

3 三維簡化的慣性傳感器系統

爲了降低陸地車輛導航方案的成本，提出了一種簡化的慣性傳感器導航（reduced inertial sensor system，簡稱RISS）方法用於二維環境導航，其中採用單軸陀螺儀與里程作爲傳感器並通過kalman濾波與GPS集成。這種二維 RISS/GPS方法，需要假設車輛在水平面上移動。之所以被稱爲RISS，是因爲它還包含一對用於計算俯仰角和橫滾角的加速度計。
一個單一的陀螺儀，其敏感軸與車輛的天向軸對齊。假定車輛在水平面上行駛，它的速度通過里程計的測量值計算得到，並與陀螺儀獲得的航向信息一起使用，以確定東、北兩個方向的速度，繼而可以計算經度與緯度。這種簡化的多傳感器系統爲2D路面導航提供了一個有效地方案。

如前文中提到的，若要確定俯仰角和滾轉角，可以使用兩個額外的加速度計。這兩個加速度計一個指向車輛的正前方，一個指向橫向，再根據地球重力矢量以及速度測量模型，就可以計算出橫滾與俯仰的角度。這些角度的計算是爲了確定載體平面與水平面的關係，並不包含在KF的動態建模中。

3.1 3D RISS概述

此處主要討論三維RISS解決方案，由兩個用於計算俯仰和橫搖的加速度計提供測量值被合併到一起以估計偏離水平面的運動，這種方法可以作爲三維環境下的導航方案。

事實證明，與完整的慣性傳感器相比（完整的IMU包含了三個加速度計和三個陀螺儀），在陸地車輛上使用一個前向加速度計和三個陀螺儀的傳感器配置，同樣也能夠進行導航參數的求解，計算出三維的位置、速度以及姿態。
這也意味着三個陀螺儀加上從車輛里程計中獲得的前向速度就足以實現三維導航。

這一節介紹使用車輛的里程計、一個單軸陀螺儀和兩個加速度計來實現三維導航方案。
陀螺儀與車輛的垂直軸對齊，原本用於計算俯仰與橫搖的兩個陀螺儀在這裏使用兩個加速度計進行替代。三維RISS集成了垂直方向的陀螺儀和兩個水平加速度計的測量數據，速度讀數由里程錶或車輪編碼器提供，三維RISS系統如圖所示：

利用三維RISS系統可獲得三維位置、三維速度和三維姿態的導航解。

3.2 （輪式車輛）採用3D RISS系統的優點

三維RISS相對於二維航位推算的優點在於，使用兩個加速度計的測量值計算偏離水平面的運動。
優點1：可以計算出正確的方位角。由於陀螺儀(與車身框架垂直對齊)在不完全水平狀態下是傾斜的，因此無法測量水平E-N平面的角速度。由於方位角在E-N平面上，因此相比於2D車輛航跡推算時忽略水平面因素的做法，3D RISS方法通過檢測和校正陀螺儀傾斜可以計算出一個更爲精確的方位角。
優點2：可以得到比2D RISS更精確的2D水平定位。 原因有二: (1)結合俯仰角和里程錶中的速度可以計算出兩個更爲精確的水平速度，因此能夠提供更好的位置估計; (2)方位角計算越精確，東向和北向的速度估計就越準確，繼而得到更精確的位置。
優點3：3D RISS 相比2D車輛航跡推算的優點在於可以計算出向上的速度以及海拔高度。

與傳統IMU相比，3D RISS的優點是通過加速計來計算俯仰角和橫滾角而非陀螺儀，在計算速度上使用了里程計而非加速度計。
那麼，爲什麼使用加速度計來計算角度比使用陀螺儀更好呢？優越性體現在哪裏？
解釋：未經補償的水平陀螺儀bias，會引起俯仰與橫滾的角度誤差，這一誤差由於進行積分計算與時間成正比，會導致加速度矢量從boay繫到 L系統的投影過程產生誤差，引入一個加速度誤差之後，後續計算的速度與 t2成正比，位置與 t3成正比。
若是通過加速度計來計算俯仰和橫滾角，可免去一次積分計算，並且角度誤差不與時間t成比例。此外，角度誤差引起的速度和位置誤差也可以下降一個階次。

除了上述優點，RISS在速度計算上進一步優化。使用里程計來計算速度與使用加速度計計算速度相比，可以獲得更好性能。
解釋：使用加速度計計算速度時，任何未經補償的加速度誤差都會引入一個與t成正比的速度誤差以及與t2成正比的位置誤差。在長期無法獲得GPS信號的情況下，使用加速度計計算速度造成的誤差隨中斷時間不斷增大，這對導航計算來說是十分不利的。
而使用里程計的測量值來計算速度可以避免一次積分計算，這意味只要進行一次積分就能夠得到位置。

在3D RISS中，唯一剩下的主要誤差源就是垂直對齊的陀螺引起的方位角誤差，陀螺儀未經補償的bias會導致方位角誤差並與時間成正比。方位角引起的位置誤差將正比於車速、時間以及方位角誤差。這些誤差必須在積分濾波器內通過模擬陀螺儀的隨機漂移來解決。

爲了證明3D RISS比完整的IMU傳感器在陸地車輛導航上更具優勢，以下分別將兩種導航方法用於路面汽車行駛的導航中，在約96公里的軌道上運行，持續時間約100分鐘，得到兩者的位置輸出，兩個系統的定位結果見圖：

圖9.2爲使用3D RISS系統得到的位置軌跡與高精度參考軌跡的對比。

圖9.3爲使用IMU傳感器估計的位置軌跡與高精度參考軌跡的對比。從圖中可以看出，隨着時間推移，Full IMU軌跡很快出現了漂移，並偏離真實軌跡。

圖9.4爲運動起始位置的局部圖，此處展示了使用full IMU更容易產生漂移。其中，3D RISS與full IMU中使用的MEMS傳感器來自於相同的IMU（IMU300CC-100型），該IMU的陀螺儀bias爲2°/s。

3.3 3D RISS的運動方程推導

本節給出了包含位置、速度和姿態狀的三維RISS的非線性運動模型。
使用通用的參考系，b系的x軸與車體的橫向重合，y軸與車體的前向重合z軸沿着車體垂直向上的方向。局部座標系（L系）是沿東、北、天方向的ENU座標系。在k-1時刻時，旋轉矩陣從車載體座標旋轉到局部座標系爲:

爲了描述運動模型，首先需要控制輸入。由一個陀螺儀、兩個加速計和里程計提供的傳感器測量值組成了控制輸入向量：

定義系統的導航狀態爲：

此處狀態向量只包含了前向速度，在有些方程中採用

將東、北、天的速度都作爲狀態向量。

3.3.1 Pitch和roll角的計算

對於陸地車輛來說，俯仰角是相對於水平地面的角度(即繞橫向軸的旋轉)，橫滾角是繞縱向軸的旋轉，如圖所示：

當車輛運動時，前向加速度計測量車輛的前向加速度以及重力分量。爲了計算俯仰角，需要在前向加速度的測量值中減去里程計測得的車輛加速度：

同樣的，橫向加速度計測量車輛的橫向加速度以及重力分量。爲了計算roll角，必須對橫向加速度進行補償：

3.3.2 航向角計算

陸地車輛的方位角實際爲車輛正前方朝向與大地北向的夾角，如圖所示：

在k-1與k的時間間隔內，繞載體b系的z軸逆時針旋轉，得到：

計算目標：求得方位角在E-N平面上的投影。
在k時刻獲得車輛b系下的前向單位矢量：

假設在已知k-1時刻的車輛狀態，對k時刻的車輛前向單位矢量進行估計：

從k-1時刻的車輛載體座標系到k時刻載體座標系之間的旋轉矩陣是Rz，那麼兩個單位矢量的關係可以看成：

由於Rz是一個正交的旋轉矩陣，因此：

（注：正交矩陣性質：ATA=E 、AT=A’）

由式可得，k時刻沿車輛前進方向的單位矢量，可由k-1時刻的車輛在 L系下的前向單位矢量得到：

因此，由於繞z軸的旋轉產生的新的航向角爲：

其中：

使用上式定義的航向角，是相對於北向的夾角，沿順時針方向爲正。

除了表現爲車輛的旋轉之外，方位角還有兩個額外的分量。這是由於地球自轉以及L系方向變化引起的。
··由於地球自轉引起的分量爲：

We爲地球旋轉速率，這一部分可以直接在新的航向角上進行補償。
··由於L座標系相對於地球座標系的方向變化引起的分量爲（沿逆時針方向）：

由於，車輛的body系與local-level系之間的關係爲：

代入（9.16）式，得到：

在計算方位角時需要加上這一項。

綜上，得到最終的方位角計算模型爲：

3.3.3 三維位置、速度計算

注：【非完整約束】非完整約束與完整約束的區別
至少包含一個不可積微分約束的動力系統稱非完整系統。對非完整系統，要用更復雜的微分方程來描述。帶有滾動輪子的系統(如自行車、汽車、飛機起落架等)大都是非完整系統。位置約束爲完整約束，速度約束爲非完整約束。

由於陸地車輛的非完整約束(non-holonomic constraint)，車身body系中的速度僅由車輛前向（縱向）速度構成，水平橫向以及垂直向的速度分量均爲0。
因此，緯度可以表示爲：

其中，RM爲地球曲率半徑，deta( t)爲採樣時間。
同理，經度可以表示爲:

海拔高度表示爲：

其中，車輛前向速度爲：

3.4 3D RISS 運動模型概述

完整的運動模型如下：

如果在導航狀態矢量中使用是L系中的速度分量，而非前向速度，那麼運動模型將變爲：

三維RISS模型如圖所示：

4 KF用於鬆耦合的3D RISS/GPS集成

非線性運動模型構成了機械化方程的基礎，由此推導出的線性化誤差狀態模型，可作爲kalman濾波的系統模型。
爲了簡化線性化系統模型，並將其作爲3D RISS的誤差模型，這裏做了兩個近似：第一個近似，將俯仰和橫滾角的誤差作爲狀態量排除在積分濾波器之外。（正如前面所討論的，俯仰和橫滾的誤差不隨時間增長）第二個近似，假設陀螺儀(沿着車身b系的垂直向上方向)與位置L系的天向對齊，簡化了方位角的計算。（由於這個陀螺儀的測量包含了地球自轉的分量以及局地座標系在地球繫上的旋轉，所以在積分之前這些量被從測量中除去。）
基於上述方位角的二維近似，可以將L系中的方位角直接表示爲：

鬆耦合的3D RISS/GPS集成的系統框圖如下所示：

4.1 3D RISS的線性化誤差模型

3D RISS KF的誤差狀態系統模型可表示爲:

kalman濾波器的誤差狀態矢量爲：

其中：

如先秦所述，運動模型時非線性的，因此必須對其進行線性化才能得到可作爲KF系統模型的誤差模型。
通過泰勒級數展開並忽略高階項，將方程線性化。
得到對應的線性化誤差狀態系統模型爲：

其中：

陀螺儀的隨機誤差以及里程計得到的速度，都採用高斯-馬爾科夫模型。

4.2 用於更新3D RISS的測量模型

由於考慮的是鬆耦合集成，所以在KF的更新階段使用了GPS的位置和速度進行更新，濾波器使用的測量模型爲：（測量模型爲RISS提供的3D位置、速度與GPS測量值的差值）

5 KF用於緊耦合的3D RISS/GPS集成

緊耦合的3D RISS/GPS集成系統框圖，如下所示：

5.1 系統模型的擴維

緊耦合的系統模型與鬆耦合類似，在系統模型和狀態矢量都包含了GPS接收機的時鐘偏差以及漂移誤差。因此RISS和GPS可以簡單地結合在一起，得到以下完整的系統模型爲：

kalman濾波系統模型中的GPS方程爲：

GPS接收器的時鐘偏差和它的漂移被包含在狀態中，這些狀態的系統模型是：

因此，擴維後的狀態矢量爲：

5.2 用於更新3D RISS的原始GPS測量模型

給出緊耦合的KF線性化測量模型：

GPS的觀測值是僞距以及僞距速率。因此，對於KF的誤差狀態來說，測量矢量由 RISS 與 GPS的僞距測量值以及僞距速率測量值的差值構成：

在第三章中提到過，GPS的測量值經過了時鐘誤差以及電離層和對流層的校正。對於M個可接受信號的衛星，以上方程可寫爲：

這些非線性僞距誤差來自於僞距測量值以及RISS計算的位置預測出的僞距值。
因此，第M顆衛星與GPS之間的僞距可由以下模型表示：

由RISS導航系統輸出的預測僞距定義爲：

其中，r(RISS,K)爲車輛位置，由ECEF大地座標系轉換到ECEF直角座標系，再由機械化方程計算位置輸出得到。

GPS僞距測量值與車輛位置無關，但對最新估計狀態進行泰勒展開之後，使得RISS估計值與GPS測量值之間的差異可以被建模爲與位置誤差線性相關的式子：

以分量形式表示：

其中：

在獲得M個衛星的測量之後，可以得到：

其中：

由於狀態矢量的位置分量在大地座標系中，因此需要將其轉化到對應的直角座標系上：

上式的線性化位置誤差爲：

將上式帶入到（9.42）式中，得到：

通過定義一個H矩陣，線性化的僞距測量模型爲：

通過測量衛星載頻的多普勒頻移，計算了第m顆衛星的僞距離速率測量值。如第3章所述，僞距離速率可視爲接收器和衛星之間的相對速度測量，測量的方向是從衛星指向接收器方向的單位矢量，測量模型可以用數學形式表示：

在（9.48）中：

因此，可重寫爲：

通過RISS估計僞距速速率爲：

其中 v(RISS,k)爲通過RISS估計的ECEF直角座標系下的車輛速度。
因此，線性化的僞距速率測量方程如下：

其中：

對M個可接收信號的衛星，僞距速率測量可以寫爲：

速度在局部水平座標系(L)與ECEF座標系之間的關係爲：

將座標系轉換代入測量方程，得到僞距速率測量模型：

其中：

綜上，得到包含僞距以及僞距速率的完整的測量模型爲：