水下機器人慣性導航系統

李秦牧，朱大奇，鄧志剛

（上海海事大學信息工程學院，上海 201306）

0 引言

海洋面積十分廣闊，占據著整個地球十分之七的面積。廣袤的海洋蘊藏的種種未知刺激著人們無盡的求知欲望，人類逐漸認識到對于海洋資源的開發與利用的重要性。水下機器人因為其能有效的幫助人類完成水下持續探索，深海工作等水下工作優勢，成為人類科學研究的重點。為了應付水下的復雜環境，路徑導航成為關鍵。本次以“海鯊”ROV水下機器人為模型作為仿真對象，選用捷聯慣導系統完成水下運動的路徑導航，通過仿真以驗證算法的可行性，同時確保3分鐘航行的絕對誤差在2.5米以內。

1 慣導的基本介紹

慣性導航是當代技術科學中的一門新興的研究領域，將慣性導航技術運用到水下機器人中可以滿足現代化水下搜索工作。

導航就是指運動目標從起點出發，沿著事先規劃的航線在沿著規定軌跡到達目的地。自古以來，人們學會利用自然環境進行導航，例如天上星宿中的北極星來確認方向。隨著時代的前進，現代科學的飛速發展，導航逐漸發展成為一門具體研究導航理論方法與導航技術器件的學科[1]。

而在所有的導航體系中，慣性導航系統是唯一在真正意義上能夠實現自主導航的系統，慣性導航系統能夠根據運載體測試得到的加速度、角速度、航向角、橫向角等信息，僅僅依靠運載體自身實現自主導航的任務。

慣性導航的基本原理是在牛頓經典力學的基礎上，依靠掛載在運載體上的慣性元件測量得出加速度、角速度、航向角等信息，通過數學運算解析的方式，計算得出運載體的位置定位信息。雖然慣性導航的理論基礎較為簡單，但是在實際應用中就會面臨諸多困難，例如高精度的慣性器械元件，高計算速度的計算機，可靠高效的慣導系統。隨著科學技術的進步，這些問題才逐漸解決，慣性導航系統才出現實際運用的可能性[2]。

慣性導航系統可以分為兩類，一類稱為平臺慣性導航系統，而另一類稱為捷聯慣性導航系統。在平臺式慣性導航系統中，將陀螺儀和加速度計等慣性元件安置在一個物理平臺上，利用陀螺儀和加速度計等慣性元件，模擬得到一個導航坐標系，為加速度計提供一個標準的坐標模板，也保持了三軸加速度計始終沿著既定方向進行測量，同時也使慣性測量元件和運載體的相對運動保持穩定。

圖1 平臺式慣性導航系統原理

但是平臺式慣導需要高精度的相對水平平臺，這種水平平臺不僅占地面積較大，而且受到工藝的復雜性和精密度制約，所以成本十分昂貴。限于外界條件的限制，平臺式慣導的實際應用受到很大的制約。

而捷聯慣系導航系統不在需要實體平臺，將陀螺儀和加速度計這類慣性元件直接安置于運載載體，慣性元件的檢測軸安裝在與載體坐標系的三軸相平行的三個方向上。在運載體的實際航行過程中，慣性元件測量得到運載體相對于慣性坐標系的角速度，再通過矩陣變換，得出載體坐標系至導航坐標系的坐標變換矩陣，即姿態矩陣。然后再運用姿態陣將加速度計測得的加速度信息進行矩陣運算，并融合進入導航坐標系中進行導航計算，也就是說姿態矩陣代替了實體平臺，充當著“數學平臺”的作用，得到的載體姿態與速度為[3]：

通過上面兩個公式就可以求得數學平臺的姿態矩陣已經比力變換參考，以代替平臺式慣導中模擬的地面水平面以及加速度計直接讀出的三軸導航方向的加速度值。

圖2 捷聯慣性導航系統原理

考慮到平臺慣導在實際應用受到的條件約束，捷聯慣導的優勢可以總結為4條：

（1）節約了絕對水平的慣導平臺，大小、面積、重量以及成本大大降低；

（2）捷聯慣導的慣性元件可以直接的讀出運載體的加速度、角速度信息，這些都是進行姿態變換的計算必要信息；

（3）大大提高系統的性能以及可靠性；

（4）捷聯式慣導系統的初始對準快捷方便。

因此結合以上優點，“海鯊”ROV水下機器人選用捷聯慣導作為本次實驗的導航系統。

2 捷聯慣導的數學算法

水下機器人的導航在地球上進行的，定位和定向都是相對于地球球面而言的。經度、緯度和高度是導航的重要計量參數，重力加速度是導航的基本自然參考量。為實現水下機器人捷聯慣導系統，一方面需要完成坐標系之間的轉換，實現姿態轉換，另一方面需要設計得到捷聯慣導算法[4]。

2.1 坐標軸的定義以及變換

（1）坐標系定義

①載體坐標系b系：用oxbybzb表示，o點位于船體的重心，xb軸沿著船體橫軸向右，yb沿著船體縱軸向前，zb沿著船體豎軸向上。載體坐標系和矯正坐標系的方位關系可以用姿態矩陣或者姿態角、方位角來表示。

②矯正坐標系t系：用oxtytzt表示，陀螺儀和加速度計的3軸應該與船體的3軸對齊，但實際情況中總有不對準角的存在，對導航的精度有著不良的影響，為了提高精度需要校準精度角。o點位于船體的重心，xt軸沿著校準后船體橫軸向右，yt沿著校準后船體縱軸向前，zt沿著船體立軸向上。矯正坐標系和導航坐標系的方向關系可以用姿態矩陣護著姿態角、方位角來表示。

③導航坐標系n系：用oxnynzn表示，它是導航系統在求解導航參數時所采用的參考坐標軸。o點位于船體的重心，xn軸指向東（E），yn軸指向北（N），zn軸指向天頂（U），即俗稱為東-北-天（E-N-U）坐標系。

（2）坐標系之間的變換關系

①載體坐標系和校正坐標系之間的關系

慣性傳感器（陀螺儀與加速度計）的安裝偏差角主要包括位置的錯位和未校準角。與飛機不同，ROV在行進過程中沒有高速的角運動，正因為如此，位置的錯位是可以忽略的，只需要考慮未校準角[5]。

因為IMU被安裝于船體的質心，校正坐標系t的原點與載體坐標系b的原點重合。在船體運動過程中偏差角也就是圖所示的Δx,Δy,Δz。

所以從t到b的變換矩陣為公式（3）：

其中Δx，Δy，Δz為偏差角，所以數值較小，因此上式可以簡化為：

圖3 安裝偏差角

Δx，Δy，Δz是在校正坐標系t中測量的，所以從載體坐標系b到校正坐標系t的矩陣變換為，也就是：

（2）載體坐標系到導航坐標系之間的變換關系

俯仰角（pitch）：可以用θ表示，載體坐標系x軸與水平面的夾角。當x軸的正半軸位于過坐標軸原點的水平面之上時，俯仰角為正，否則為負，也是說俯仰角是圍繞x軸旋轉的。

橫向角（roll）：可以用γ表示，載體坐標系zb軸與通過載體xb軸的鉛垂面的夾角，載體向右滾為正，反之為負。也就是說橫向角是圍繞z軸旋轉的。

航向角（yaw）：可以用ψ表示，載體坐標系xb軸在水平面上投影與地面坐標系xg軸（在水平面上，指向目標為正）之間的夾角，由xg軸逆時針轉置載體xb的投影線時航向角為正，即載體右航向為正，反之為負。也就是說航向角是圍繞y軸旋轉的。

導航坐標系oxnynzn繞zn軸負向轉角ψ得ox'y'z'，ox'y'z'繞 x'軸轉角 θ得 oxnynzn，oxnynzn再繞 yn軸轉角γ則得到矯正坐標系oxtytzt（旋轉順序不能改變），因此矯正坐標系與導航坐標系之間的關系用方向余弦陣可以表示為公式（6）。

所以，如公式（7）。

僅考慮航向角轉動的情況下可以得到：

假設：

其中：

所以由姿態矩陣提取姿態角如下：

2.2 接連慣導數字迭代算法

（1）姿態更新算法

選用的是東-北-天（E-N-U）坐標系作為接連慣導系統的導航參考坐標系，則姿態微分方程為：

式中，角標括號中的m表示tm時刻。因為i系是一個絕對慣性參考系，所以它與時間無關，而n系和b系相對于i系是動坐標，均與時間有關，需要標注時間[6]。

其中Arot為陀螺儀得到的角速度，為：

上式即為接連慣導遞推姿態更新算法。

（2）速度更新算法

對上式在[tm-1,tm]時間段內兩邊做積分可得：

展開可得：

所以將上式可以改寫為：

因為ROV在水下航行時，短時間[tm,tm-1]內引起的導航坐標系旋轉和重力矢量變化都十分小，因此一般認為的被積函數可以忽略不計。所以只需要考慮[tm,tm-1]時間段內比力速度增加量。

（3）置更新算法

捷聯慣導系統位置微分方程為：

將經度、緯度、高度改寫為矩陣形式可以得到：p=Mpvvn。

其中記：

對上式在[ ]tm-1,tm時間段內兩邊做積分離散化可得：

移項后就可以得到位置更新算法：

3 仿真與測量結果

進一步簡化可以得到：

前一節已經給出基于水下機器ROV的捷聯慣導算法，這里對于所提出的算法進行仿真分析，驗證算法的有效性，并且與實際GPS路徑進行比較，驗證算法的精度。仿真結果主要分為兩部分：捷聯慣導位置推算路徑仿真與推算路徑與實際GPS軌跡的比較。同時需要滿足航行3分鐘位置絕對偏差控制在3.5米以內。

捷聯慣導位置推算路徑仿真，設計出水下機器人ROV導航系統框圖如圖4所示。

圖4 導航系統框圖

首先慣性元件陀螺儀和加速度計采樣到各時刻運載體的角速度和加速度，從而可以得出姿態矩陣，再通過姿態校準將安裝偏差角的誤差消除，然后經過捷聯慣導算法推算出下一時刻的姿態，再結合GPS讀出的初始航向角，推算出導航的位置，繼而實現慣性導航。

根據慣性元件采集到的數據，進過捷聯慣導系統推算后得到的仿真導航路徑如圖5所示。

圖5 捷聯慣導推算路徑

圖中，x軸為經度，y軸為維度，藍色實線即為捷聯慣導推算得到的路徑。為了直觀的檢驗捷聯慣導算法的準確性，將推算路徑與GPS得到的實際軌跡進行比較，如圖6所示。

圖中，紅色實線為捷聯慣導推算軌跡，藍色實線為GPS實際軌跡。可以看出捷聯慣導推算得到的路徑與GPS實際軌跡能夠保持較高的重合度，并未出現重大誤差的情況。而絕對誤差如圖7所示。

圖6 捷聯慣導推算路徑和GPS實際軌跡

圖7 捷聯慣導推算路徑與GPS實際軌跡的絕對誤差

圖7中，x軸為測試時間，單位為秒,y軸為捷聯慣導推算路徑與GPS實際軌跡的絕對誤,單位為米。可以看到在運行的3分鐘內，絕對誤差并沒出現超過3.5米的情況，驗證了算法的準確性。