李秦牧，朱大奇，鄧志剛

（上海海事大學(xué)信息工程學(xué)院，上海 201306）

0 引言

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

1 慣導(dǎo)的基本介紹

慣性導(dǎo)航是當代技術(shù)科學(xué)中的一門新興的研究領(lǐng)域，將慣性導(dǎo)航技術(shù)運用到水下機器人中可以滿足現(xiàn)代化水下搜索工作。

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

而在所有的導(dǎo)航體系中，慣性導(dǎo)航系統(tǒng)是唯一在真正意義上能夠?qū)崿F(xiàn)自主導(dǎo)航的系統(tǒng)，慣性導(dǎo)航系統(tǒng)能夠根據(jù)運載體測試得到的加速度、角速度、航向角、橫向角等信息，僅僅依靠運載體自身實現(xiàn)自主導(dǎo)航的任務(wù)。

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

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

圖1 平臺式慣性導(dǎo)航系統(tǒng)原理

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

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

通過上面兩個公式就可以求得數(shù)學(xué)平臺的姿態(tài)矩陣已經(jīng)比力變換參考，以代替平臺式慣導(dǎo)中模擬的地面水平面以及加速度計直接讀出的三軸導(dǎo)航方向的加速度值。

圖2 捷聯(lián)慣性導(dǎo)航系統(tǒng)原理

考慮到平臺慣導(dǎo)在實際應(yīng)用受到的條件約束，捷聯(lián)慣導(dǎo)的優(yōu)勢可以總結(jié)為4條：

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

（2）捷聯(lián)慣導(dǎo)的慣性元件可以直接的讀出運載體的加速度、角速度信息，這些都是進行姿態(tài)變換的計算必要信息；

（3）大大提高系統(tǒng)的性能以及可靠性；

（4）捷聯(lián)式慣導(dǎo)系統(tǒng)的初始對準快捷方便。

因此結(jié)合以上優(yōu)點，“海鯊”ROV水下機器人選用捷聯(lián)慣導(dǎo)作為本次實驗的導(dǎo)航系統(tǒng)。

2 捷聯(lián)慣導(dǎo)的數(shù)學(xué)算法

水下機器人的導(dǎo)航在地球上進行的，定位和定向都是相對于地球球面而言的。經(jīng)度、緯度和高度是導(dǎo)航的重要計量參數(shù)，重力加速度是導(dǎo)航的基本自然參考量。為實現(xiàn)水下機器人捷聯(lián)慣導(dǎo)系統(tǒng)，一方面需要完成坐標系之間的轉(zhuǎn)換，實現(xiàn)姿態(tài)轉(zhuǎn)換，另一方面需要設(shè)計得到捷聯(lián)慣導(dǎo)算法[4]。

2.1 坐標軸的定義以及變換

（1）坐標系定義

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

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

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

（2）坐標系之間的變換關(guān)系

①載體坐標系和校正坐標系之間的關(guān)系

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

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

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

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

圖3 安裝偏差角

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

（2）載體坐標系到導(dǎo)航坐標系之間的變換關(guān)系

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

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

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

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

所以，如公式（7）。

僅考慮航向角轉(zhuǎn)動的情況下可以得到：

假設(shè)：

其中：

所以由姿態(tài)矩陣提取姿態(tài)角如下：

2.2 接連慣導(dǎo)數(shù)字迭代算法

（1）姿態(tài)更新算法

選用的是東-北-天（E-N-U）坐標系作為接連慣導(dǎo)系統(tǒng)的導(dǎo)航參考坐標系，則姿態(tài)微分方程為：

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

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

上式即為接連慣導(dǎo)遞推姿態(tài)更新算法。

（2）速度更新算法

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

展開可得：

所以將上式可以改寫為：

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

（3）置更新算法

捷聯(lián)慣導(dǎo)系統(tǒng)位置微分方程為：

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

其中記：

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

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

3 仿真與測量結(jié)果

進一步簡化可以得到：

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

捷聯(lián)慣導(dǎo)位置推算路徑仿真，設(shè)計出水下機器人ROV導(dǎo)航系統(tǒng)框圖如圖4所示。

圖4 導(dǎo)航系統(tǒng)框圖

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

根據(jù)慣性元件采集到的數(shù)據(jù)，進過捷聯(lián)慣導(dǎo)系統(tǒng)推算后得到的仿真導(dǎo)航路徑如圖5所示。

圖5 捷聯(lián)慣導(dǎo)推算路徑

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

圖中，紅色實線為捷聯(lián)慣導(dǎo)推算軌跡，藍色實線為GPS實際軌跡?？梢钥闯鼋萋?lián)慣導(dǎo)推算得到的路徑與GPS實際軌跡能夠保持較高的重合度，并未出現(xiàn)重大誤差的情況。而絕對誤差如圖7所示。

圖6 捷聯(lián)慣導(dǎo)推算路徑和GPS實際軌跡

圖7 捷聯(lián)慣導(dǎo)推算路徑與GPS實際軌跡的絕對誤差

圖7中，x軸為測試時間，單位為秒,y軸為捷聯(lián)慣導(dǎo)推算路徑與GPS實際軌跡的絕對誤,單位為米?？梢钥吹皆谶\行的3分鐘內(nèi)，絕對誤差并沒出現(xiàn)超過3.5米的情況，驗證了算法的準確性。