丁明杰，胡 橋，2，3，蘇文斌，2，3，李 俊，孫帥帥

（1.西安交通大學(xué)機(jī)械工程學(xué)院西安710049；2.西安交通大學(xué)機(jī)械制造系統(tǒng)工程國(guó)家重點(diǎn)實(shí)驗(yàn)室西安710049；3.陜西省智能機(jī)器人重點(diǎn)實(shí)驗(yàn)室西安710049）

1 引 言

21世紀(jì)是人類向海洋進(jìn)軍的世紀(jì)，地球上海洋占地球表面積的71%，具有重大戰(zhàn)略意義。隨著人類對(duì)海洋資源認(rèn)識(shí)的逐步深入，開(kāi)發(fā)和利用海洋資源已經(jīng)成為各國(guó)重要的戰(zhàn)略目標(biāo)［1］。由于海洋世界深水環(huán)境的復(fù)雜性以及不確定性，水下機(jī)器人在水下復(fù)雜作業(yè)任務(wù)中承擔(dān)越來(lái)越重要的角色。矢量推進(jìn)水下機(jī)器人具有航行機(jī)動(dòng)性強(qiáng)、任務(wù)能耗低和環(huán)境適應(yīng)性強(qiáng)等特點(diǎn)［2］。水下機(jī)器人在復(fù)雜的海況下進(jìn)行水底探測(cè)與勘查時(shí)常常會(huì)受到水流和波浪的隨機(jī)擾動(dòng)等一系列因素的影響，通過(guò)慣性測(cè)量單元獲取姿態(tài)角，其測(cè)量的數(shù)據(jù)噪聲較大，穩(wěn)定性和精度低［3］。因此，一般采用多傳感器信息融合的方法來(lái)獲取水下機(jī)器人的姿態(tài)［4］。

目前多傳感器融合算法包括：共軛梯度法、互補(bǔ)濾波、無(wú)跡卡爾曼濾波和EKF［5］。南京航空航天大學(xué)孫金秋［6］應(yīng)用共軛梯度法進(jìn)行姿態(tài)解算，收斂性較好，但是較大的運(yùn)算量很難滿足本文水下機(jī)器人應(yīng)用場(chǎng)景；燕山大學(xué)呂印新［7］采用互補(bǔ)濾波的方法進(jìn)行姿態(tài)解算，但是在建立系統(tǒng)模型的時(shí)候沒(méi)有考慮觀測(cè)噪聲的影響，不能持續(xù)獲取穩(wěn)定的姿態(tài)數(shù)據(jù)。Allotta B［8］提出無(wú)跡卡爾曼濾波，有效避免了系統(tǒng)模型的線性誤差，但在實(shí)際應(yīng)用中，算法計(jì)算量過(guò)大，過(guò)程相對(duì)比較復(fù)雜，不易在微控制器中實(shí)現(xiàn)。Simanek J［9］利用擴(kuò)展卡爾曼法的非線性估計(jì)方法修正姿態(tài)角，通過(guò)多傳感器的性能進(jìn)行信息互容，實(shí)際系統(tǒng)中的擾動(dòng)得到了有效的控制，但是同樣存在發(fā)散和截?cái)嗾`差。

針對(duì)上述算法計(jì)算量、實(shí)時(shí)性和收斂性的不足，本文建立狀態(tài)向量時(shí)，考慮四元數(shù)本身以及角度測(cè)量偏差對(duì)四元數(shù)狀態(tài)更新的影響，提出基于EKF 的姿態(tài)估計(jì)方法，并結(jié)合實(shí)驗(yàn)對(duì)所設(shè)計(jì)的姿態(tài)解算系統(tǒng)進(jìn)行了驗(yàn)證。

2 系統(tǒng)設(shè)計(jì)

通過(guò)研究姿態(tài)解算方法可以進(jìn)一步合理設(shè)計(jì)矢量推進(jìn)水下機(jī)器人控制系統(tǒng)，這對(duì)于水下機(jī)器人運(yùn)動(dòng)與作業(yè)的精準(zhǔn)度、高效性起著關(guān)鍵作用［10］。

2.1 水下機(jī)器人探測(cè)系統(tǒng)組成

如圖1 所示，水下機(jī)器人探測(cè)系統(tǒng)包括上位機(jī)、地面通訊基站和矢量推進(jìn)水下機(jī)器人三部分。上位機(jī)的功能包括水下機(jī)器人運(yùn)動(dòng)指令的下達(dá)、運(yùn)動(dòng)狀態(tài)數(shù)據(jù)記錄以及探測(cè)任務(wù)模塊信息如視頻信息和聲納圖像的顯示。地面通訊基站涉及到圖像的上傳和信號(hào)的雙向傳輸，由于信息量比較龐大，需要盡量縮短延遲以做到實(shí)時(shí)監(jiān)測(cè)和控制。由于無(wú)線通信存在速率較慢、水下信號(hào)衰減嚴(yán)重的缺點(diǎn)，因此選擇線纜通信。矢量推進(jìn)水下機(jī)器人是執(zhí)行任務(wù)的載體平臺(tái)，攜帶側(cè)掃聲納、高清攝像頭、深度傳感器等水下探測(cè)設(shè)備。采用兩個(gè)分布式布置的矢量推進(jìn)器作為動(dòng)力來(lái)源，實(shí)現(xiàn)前進(jìn)后退、左右偏航、定點(diǎn)自旋、定深定航等基本運(yùn)動(dòng)模式。

圖1 水下機(jī)器人探測(cè)系統(tǒng)組成圖Fig.1 Composition chart of underwater robot detection system

2.2 水下機(jī)器人的姿態(tài)解算方法

姿態(tài)解算是水下機(jī)器人控制系統(tǒng)設(shè)計(jì)的核心環(huán)節(jié)［11］。水下機(jī)器人在水下執(zhí)行探測(cè)與勘查等作業(yè)任務(wù)時(shí)常常會(huì)受到水流和波浪的隨機(jī)擾動(dòng)等一系列因素的影響，其測(cè)量的數(shù)據(jù)噪聲較大，穩(wěn)定性和精度較差。需要利用姿態(tài)估計(jì)算法，根據(jù)狀態(tài)方程和測(cè)量方程融合陀螺儀、磁強(qiáng)計(jì)和加速度計(jì)等傳感器的數(shù)據(jù)來(lái)進(jìn)行姿態(tài)角的解算。

2.2.1 四元數(shù)更新算法

四元數(shù)q的微分方程=Ωq，寫成矩陣形式為：

結(jié)合水下機(jī)器人執(zhí)行作業(yè)任務(wù)的應(yīng)用場(chǎng)景，需要姿態(tài)解算方法達(dá)到實(shí)時(shí)性和高精度的要求。先通過(guò)多姿態(tài)傳感器數(shù)據(jù)融合獲取角速度，然后采用一階龍格庫(kù)塔法進(jìn)一步更新四元數(shù)［12］，可得：

2.2.2 基于互補(bǔ)濾波的姿態(tài)估計(jì)

由加速度計(jì)測(cè)定的姿態(tài)角在短時(shí)間內(nèi)精度較差，但是測(cè)量誤差不隨時(shí)間累加；而由陀螺儀得到的姿態(tài)角一段時(shí)間后精度降低，這是因?yàn)槭艿酵勇輧x漂移的影響，但短時(shí)間內(nèi)可以提供高動(dòng)態(tài)的姿態(tài)信息?；パa(bǔ)濾波利用兩者在頻域上的互補(bǔ)特性，通過(guò)加速度計(jì)和磁力計(jì)來(lái)補(bǔ)償陀螺儀漂移所引起的姿態(tài)偏差。采用互補(bǔ)濾波器對(duì)加速度計(jì)和磁力計(jì)進(jìn)行數(shù)據(jù)融合可提高姿態(tài)測(cè)量的精度和動(dòng)態(tài)響應(yīng)的速度。

互補(bǔ)濾波器大多采用比例或者比例-積分（PI）補(bǔ)償?shù)姆椒ǎ?3］。取C(s) =Kp+KI/s，在互補(bǔ)濾波器中，比例增益Kp的大小決定了濾波器的截止頻率，積分增益KI的大小決定了濾波器消除靜態(tài)誤差的時(shí)間?；诨パa(bǔ)濾波器的姿態(tài)求解算法原理如圖2所示。

其中，(mx，my，mz)為磁強(qiáng)計(jì)測(cè)量的地磁場(chǎng)的矢量歸一化后的值，將其轉(zhuǎn)到導(dǎo)航坐標(biāo)系中得到(hx，hy，hz)，由于導(dǎo)航坐標(biāo)系為北東地，在y軸方向地磁場(chǎng)矢量應(yīng)該為0，對(duì)(hx，hy，hz)進(jìn)行一次校準(zhǔn)得導(dǎo)航坐標(biāo)系下的地磁場(chǎng)矢量(bx，by，bz)。

其中，為互補(bǔ)濾波器對(duì)重力加速度的最優(yōu)估計(jì)值為從載體坐標(biāo)系到導(dǎo)航坐標(biāo)系的坐標(biāo)轉(zhuǎn)換矩陣。

其中，e為姿態(tài)角的誤差；eθγ為橫滾角和俯仰角的誤差；eφ為偏航角的誤差；δ為誤差e通過(guò)一個(gè)比例積分模塊后生成的對(duì)陀螺漂移的補(bǔ)償量；ωg為陀螺儀測(cè)量得到的角速度；ωb為融合加速度計(jì)和磁強(qiáng)計(jì)數(shù)據(jù)校正后的角速度為加速度計(jì)測(cè)量得到的重力加速度為磁強(qiáng)計(jì)測(cè)量的地磁場(chǎng)矢量；為互補(bǔ)濾波器估計(jì)的地磁場(chǎng)矢量。

圖2 基于互補(bǔ)濾波器的姿態(tài)求解算法原理圖Fig.2 Schematic diagram of attitude solving algorithm based on Complementary filter

將融合校正后的角速度代入公式就可得到隨時(shí)間傳遞的準(zhǔn)確的四元數(shù)，進(jìn)而求得姿態(tài)矩陣和姿態(tài)角。因?yàn)榛パa(bǔ)濾波器對(duì)陀螺儀的高通作用，所以陀螺儀低頻漂移的現(xiàn)象可以得到抑制，而低通作用則可以抑制加速度計(jì)的噪聲。

2.2.3 基于EKF的姿態(tài)估計(jì)

卡爾曼濾波是一種基于狀態(tài)空間法的濾波算法，它適用于線性、離散和有限維系統(tǒng)。

將目標(biāo)狀態(tài)方程表示為：

量測(cè)方程表示為：

其中，X(t)為t時(shí)刻系統(tǒng)狀態(tài)向量；F(t)為t時(shí)刻系統(tǒng)狀態(tài)轉(zhuǎn)移矩陣；Z(t)為t時(shí)刻系統(tǒng)量測(cè)向量；H(t)為t時(shí)刻系統(tǒng)量測(cè)轉(zhuǎn)移矩陣；w(t)為過(guò)程噪聲；v(t)為觀測(cè)噪聲。定義t時(shí)刻系統(tǒng)過(guò)程噪聲w(t)～(0，Q)，量測(cè)噪聲v(t)～(0，R)，二者為相互獨(dú)立的高斯白噪聲。

卡爾曼濾波適合于解決線性數(shù)據(jù)融合問(wèn)題。為了解決系統(tǒng)存在的非線性問(wèn)題，EKF 利用泰勒級(jí)數(shù)展開(kāi)原理，對(duì)狀態(tài)方程和量測(cè)方程求關(guān)于目標(biāo)狀態(tài)的一階偏導(dǎo)并忽略二階以上的高階項(xiàng)來(lái)實(shí)現(xiàn)對(duì)方程的線性化處理。

離散時(shí)間系統(tǒng)的狀態(tài)方程和觀測(cè)方程可以表示為如下形式的差分方程：

此時(shí)狀態(tài)方程和觀測(cè)方程后驗(yàn)估計(jì)表示為：

將上式（11）和上式（12）線性化，得到：

其中：F為f對(duì)X求偏導(dǎo)的雅可比矩陣；G為f對(duì)W求偏導(dǎo)的雅可比矩陣；H為h對(duì)X求偏導(dǎo)的雅可比矩陣。

本文使用了基于四元數(shù)的EKF 來(lái)進(jìn)行水下機(jī)器人的姿態(tài)解算，考慮四元數(shù)本身以及角度測(cè)量偏差對(duì)四元數(shù)狀態(tài)更新的影響。為此建立了如下的狀態(tài)向量：

其中，q=[q0q1q2q3]T為表示姿態(tài)信息的四元數(shù)；bg=[bgx bgv bgz]T為陀螺儀角速度測(cè)量偏差。

采用磁力計(jì)與加速度計(jì)作為四元數(shù)的融合信息，定義測(cè)量向量為：

其中，mb=[mx my mz]T為磁力計(jì)在載體坐標(biāo)系下測(cè)得的三軸磁力分量；為重力加速度在載體坐標(biāo)系下的分量。

建立狀態(tài)向量X(t)的微分關(guān)系式如下：

進(jìn)而可得狀態(tài)轉(zhuǎn)移矩陣為：

其中，為四元數(shù)更新對(duì)自身狀態(tài)轉(zhuǎn)移的影響矩陣，為四元數(shù)更新對(duì)角速度狀態(tài)轉(zhuǎn)移的影響矩陣。

對(duì)于磁力計(jì)來(lái)說(shuō)，載體坐標(biāo)系下的觀測(cè)變量mb=[mx my mz]T與導(dǎo)航坐標(biāo)系下的三軸磁場(chǎng)分量mn由如下關(guān)系：

其中，ν為測(cè)量噪聲，m為地磁場(chǎng)向量的模，mn=m[cosα0 -sinα]T。α為磁偏角。

以上參數(shù)可由地磁場(chǎng)模型得到，帶入(q)可得h(q)為：

根據(jù)上式可得磁力計(jì)的觀測(cè)雅克比矩陣Hmag為：

其中：

磁力計(jì)只能修正矢量推進(jìn)水下機(jī)器人的偏航角，而修正水下機(jī)器人的俯仰角和翻滾角就需要利用重力加速度對(duì)于載體加速度的影響來(lái)修正了。

觀測(cè)函數(shù)ha(q)為：

綜合磁力計(jì)與加速度計(jì)的觀測(cè)矩陣，我們就可以得到總的觀測(cè)矩陣為：

如圖3 所示為EKF 姿態(tài)解算整體流程圖，通過(guò)融合多個(gè)傳感器測(cè)量數(shù)據(jù)，能夠更好地消除具有明顯誤差的傳感器測(cè)量數(shù)據(jù)，更適用于水下機(jī)器人這類非線性系統(tǒng)；在構(gòu)建狀態(tài)向量時(shí)把角度偏差加進(jìn)了狀態(tài)向量，考慮了角度偏差對(duì)于狀態(tài)更新的影響，來(lái)精確地估計(jì)水下機(jī)器人的姿態(tài)信息。EKF 過(guò)程分為時(shí)間更新和量測(cè)更新，具體遞推過(guò)程如下：

（1）一步預(yù)測(cè)目標(biāo)狀態(tài)更新：

（2）一步預(yù)測(cè)估計(jì)誤差協(xié)方差：

（3）新息協(xié)方差矩陣：

（4）計(jì)算卡爾曼濾波增益：

（5）新息過(guò)程：

（6）由觀測(cè)向量更新?tīng)顟B(tài)估計(jì)：

（7）更新協(xié)方差估計(jì)：

圖3 EKF姿態(tài)解算整體流程圖Fig.3 Overall flowchart of Extended Kalman filtering attitude solution

3 實(shí)驗(yàn)分析及算法驗(yàn)證

本文矢量推進(jìn)水下機(jī)器人所采用的Pixhawk 航行控制器包含的姿態(tài)傳感器有L3GD20 3軸數(shù)字16位陀螺儀，LSM303D 3 軸14 位磁強(qiáng)計(jì)，MPU6000 6軸加速度計(jì)。姿態(tài)解算一共有三組實(shí)驗(yàn)。第一組為靜態(tài)實(shí)驗(yàn)，用于比較互補(bǔ)濾波與EKF 的靜態(tài)性能；第二組為水平滑動(dòng)實(shí)驗(yàn)，用于比較兩種姿態(tài)估計(jì)算法在受非重力加速度因素干擾的情況下姿態(tài)解算的精確性；第三組為動(dòng)態(tài)實(shí)驗(yàn)，實(shí)驗(yàn)平臺(tái)如圖4所示，利用開(kāi)發(fā)板控制舵機(jī)帶動(dòng)Pixhawk 航行控制器繞著三軸分別旋轉(zhuǎn)，用于比較兩種姿態(tài)估計(jì)算法的動(dòng)態(tài)性能。評(píng)價(jià)參數(shù)為平均值、標(biāo)準(zhǔn)差、平均絕對(duì)誤差（MAE）和均方根誤差（RMSE）四種性能指標(biāo)。

圖4 姿態(tài)解算動(dòng)態(tài)實(shí)驗(yàn)平臺(tái)圖Fig.4 Diagram of dynamic experimental platform for attitude solution

3.1 靜態(tài)實(shí)驗(yàn)

在靜態(tài)實(shí)驗(yàn)中，先對(duì)陀螺儀、磁強(qiáng)計(jì)以及加速度計(jì)進(jìn)行校準(zhǔn)，因?yàn)橥勇輧x漂移現(xiàn)象的影響，短時(shí)間內(nèi)可以提供高精度的姿態(tài)信息，但一段時(shí)間后精度降低。圖5 為陀螺儀在不采用濾波算法解算獲得的姿態(tài)數(shù)據(jù)。可以看出在不進(jìn)行算法補(bǔ)償時(shí)，在靜止情況下，單一陀螺儀解算的姿態(tài)角由時(shí)間的變化會(huì)持續(xù)發(fā)散，在600s 的時(shí)長(zhǎng)發(fā)散量已經(jīng)超過(guò)了2°。

圖5 靜態(tài)下陀螺儀不采用濾波算法的漂移現(xiàn)象Fig.5 Drift phenomenon of gyro without filtering algorithm in static state

因此本文研究采用基于多傳感器的信息融合算法解決這一問(wèn)題，實(shí)驗(yàn)總時(shí)長(zhǎng)為600s，通過(guò)截取最后60s 左右的實(shí)驗(yàn)數(shù)據(jù)進(jìn)行分析，比較EKF 和互補(bǔ)濾波算法對(duì)于陀螺儀漂移現(xiàn)象的算法補(bǔ)償效果。

在靜止的條件下，分別測(cè)量互補(bǔ)濾波算法和EKF算法輸出的三軸姿態(tài)信息如圖6～8所示。

圖6 靜態(tài)實(shí)驗(yàn)俯仰角對(duì)比圖Fig.6 Comparison of static experimental pitch angle

圖7 靜態(tài)實(shí)驗(yàn)翻滾角對(duì)比圖Fig.7 Comparison of roll angles in static experiments

圖8 靜態(tài)實(shí)驗(yàn)偏航角對(duì)比圖Fig.8 Comparison of static experimental yaw angle

在靜態(tài)實(shí)驗(yàn)中，理想的偏航角、翻滾角以及俯仰角輸出都應(yīng)該為0°。以俯仰角為例，互補(bǔ)濾波的波動(dòng)范圍為-0.1802°～0.0544°，EKF 的波動(dòng)范圍為-0.0724°～0.0416°，是互 補(bǔ)濾波 波動(dòng) 范 圍的48.59%。通過(guò)表1 中EKF 與互補(bǔ)濾波姿態(tài)解算方法在四種誤差性能指標(biāo)方面的綜合比較，可以得出在靜態(tài)實(shí)驗(yàn)中EKF 算法的姿態(tài)解算相比互補(bǔ)濾波更為精確，波動(dòng)范圍更小，且整體曲線更接近理想值的結(jié)論。

表1 靜態(tài)實(shí)驗(yàn)兩種算法數(shù)據(jù)比較Table 1 Data comparison of two algorithms in static experiments

3.2 水平滑動(dòng)實(shí)驗(yàn)

水平滑動(dòng)條件下姿態(tài)數(shù)據(jù)如圖9～10 所示。由圖可得，當(dāng)受到非重力加速度的作用時(shí)，互補(bǔ)濾波和EKF的姿態(tài)輸出都會(huì)受到一定影響。

圖9 水平滑動(dòng)實(shí)驗(yàn)俯仰角對(duì)比圖Fig.9 Comparison chart of elevation angle in horizontal sliding experiment

圖10 水平滑動(dòng)實(shí)驗(yàn)翻滾角對(duì)比圖Fig.10 Comparison chart of roll angle in horizontal sliding experiment

水平滑動(dòng)實(shí)驗(yàn)是將Pixhawk 航行控制器在水平面沿著任意的方向來(lái)回的滑動(dòng)，是為了研究當(dāng)存在非重力運(yùn)動(dòng)加速度時(shí)，兩種算法的俯仰角和翻滾角受到的影響程度，偏航角不是這一實(shí)驗(yàn)的研究對(duì)象。在水平滑動(dòng)實(shí)驗(yàn)中，以翻滾角為例，互補(bǔ)濾波的波動(dòng)范圍為-0.3488°～0.1977°，EKF 的波動(dòng)范圍為-0.1722°～0.1253°，是互補(bǔ)濾波波動(dòng)范圍的54.44%。通過(guò)表2 可得水平滑動(dòng)實(shí)驗(yàn)中EKF 的姿態(tài)輸出波動(dòng)更小，而且總體的姿態(tài)數(shù)據(jù)更接近理想值。因此，EKF 相對(duì)于互補(bǔ)濾波能夠更有效的抑制非重力加速度對(duì)姿態(tài)解算的影響。

表2 水平滑動(dòng)實(shí)驗(yàn)兩種算法數(shù)據(jù)比較Table 2 Data comparison of two algorithms in horizontal sliding experiment

3.3 動(dòng)態(tài)實(shí)驗(yàn)

在動(dòng)態(tài)實(shí)驗(yàn)中，將Pixhawk 航行控制器繞著三軸分別旋轉(zhuǎn)，使其俯仰角、翻滾角和偏航角在-20°～20°之間變化，檢測(cè)其動(dòng)態(tài)性能。三軸姿態(tài)輸出分布如圖11～13所示。

圖11 動(dòng)態(tài)實(shí)驗(yàn)俯仰角對(duì)比圖Fig.11 Dynamic experiment pitch angle comparison chart

由圖11～13 可得，姿態(tài)劇烈變化的時(shí)候，互補(bǔ)濾波相比EKF有輕微的滯后，且有穩(wěn)態(tài)誤差。結(jié)合表3 的相關(guān)指標(biāo)可得，動(dòng)態(tài)實(shí)驗(yàn)中EKF 姿態(tài)角的平均絕對(duì)誤差和均方根誤差均低于互補(bǔ)濾波。EKF的姿態(tài)輸出在轉(zhuǎn)到兩個(gè)極限角度時(shí)相比互補(bǔ)濾波精度更高，而且數(shù)據(jù)的保持性更好。

圖12 動(dòng)態(tài)實(shí)驗(yàn)翻滾角對(duì)比圖Fig.12 Dynamic experiment roll angle comparison chart

圖13 動(dòng)態(tài)實(shí)驗(yàn)偏航角對(duì)比圖Fig.13 Dynamic experiment yaw angle comparison chart

表3 動(dòng)態(tài)實(shí)驗(yàn)兩種算法誤差數(shù)據(jù)比較Table 3 Comparison of error data of two algorithms in dynamic experiments

4 結(jié) 論

本文提出了基于EKF 的矢量推進(jìn)水下機(jī)器人姿態(tài)解算新方法，通過(guò)與互補(bǔ)濾波姿態(tài)解算方法在四種誤差性能指標(biāo)方面的綜合比較，充分驗(yàn)證了本文提出的基于EKF 姿態(tài)解算新方法的可行性和有效性，實(shí)驗(yàn)結(jié)果表明：

（1）在靜態(tài)實(shí)驗(yàn)中EKF 算法的姿態(tài)解算相比互補(bǔ)濾波更為精確，波動(dòng)范圍更小，且整體曲線更接近理想值。

（2）水平滑動(dòng)實(shí)驗(yàn)中EKF 的姿態(tài)輸出波動(dòng)更小，而且總體的姿態(tài)數(shù)據(jù)更接近理想值。說(shuō)明EKF相對(duì)于互補(bǔ)濾波能夠更有效地抑制非重力加速度對(duì)姿態(tài)解算的影響。

（3）動(dòng)態(tài)實(shí)驗(yàn)中EKF 姿態(tài)角的平均絕對(duì)誤差和均方根誤差均低于互補(bǔ)濾波。EKF 的姿態(tài)輸出在轉(zhuǎn)到兩個(gè)極限角度時(shí)相比互補(bǔ)濾波精度更高，而且數(shù)據(jù)的保持性更好。

綜上，為了解決目前姿態(tài)解算算法計(jì)算量大、實(shí)時(shí)性和收斂性差的問(wèn)題，本文采用EKF建立狀態(tài)向量時(shí)，考慮四元數(shù)本身以及角度測(cè)量偏差對(duì)四元數(shù)狀態(tài)更新的影響，經(jīng)過(guò)實(shí)驗(yàn)證明，EKF 在水下機(jī)器人姿態(tài)解算中不僅精度高于互補(bǔ)濾波，而且輸出實(shí)時(shí)性良好，適用于動(dòng)態(tài)性能要求較高的水下機(jī)器人姿態(tài)信息解算。

本文研究了基于EKF 的矢量推進(jìn)水下機(jī)器人姿態(tài)解算方法，實(shí)際工作中水下機(jī)器人的定點(diǎn)作業(yè)和區(qū)域巡航往往還需要精確的位置和速度信息，后續(xù)可以通過(guò)布放超短基線定位系統(tǒng)等裝置獲得水下機(jī)器人的位置數(shù)據(jù)，然后利用多傳感器融合技術(shù)估計(jì)精準(zhǔn)的位置、速度和姿態(tài)綜合運(yùn)動(dòng)信息，這將是今后進(jìn)一步的研究方向。