水下機器人姿態融合算法研究

朱明明，辛紹杰，鄧寅喆

（上海電機學院機械學院，上海201306）

隨著人類探索海洋資源速度的加快，勘探海洋資源已成為各國重要的戰略目標［1］。水下遙控機器人（Remotely Operated Vehicle，ROV）操作簡單、響應快，可以輔助人類在水下復雜環境中執行特殊任務。當前，ROV的導航主要有慣性導航、水聲學導航、聲吶導航以及航位推算導航等［2-4］。其中慣性導航系統誤差會隨時間而逐漸累加，如果沒有其他導航傳感器數據信息定期修正導航誤差，會產生極大的偏差［5］。盧峰等［6］提出利用水聲學傳感器進行水下導航，能夠解算出ROV在水下作業時較為準確的位置信息，但該方法水下作業距離有限，限制了ROV自主作業能力以及運行范圍。孫玉山等［7］采用ROV航位推算導航系統，分析了航位推算導航系統存在的推算誤差，并沒有解決導航傳感器自身數據采集時帶來的誤差。高延增等［8］提出了融合短基線定位聲吶實現ROV定位，只是模塊化地設計了ROV定位系統，以定位算法實現定位，但并未處理聲吶信號噪聲，無法有效降低聲吶自噪聲和水體噪聲對ROV定位的影響。馬劍等[9]提出將卡爾曼濾波（Extended Kalman Filter，EKF）算法應用于ROV超短基線定位中，卻忽視了算法存在精度差、數據噪聲大等問題。

針對以上問題，本文提出基于四元數的改進EKF算法，并通過理論設計和實驗仿真對導航精度進行驗證。結果表明，使用改進EKF算法可提高ROV姿態融合精度，改善了目前多傳感器融合算法數據處理的一些不足。

1 導航結構設計

本文采用自主設計的結合水面無人艇協同作業的便攜式水下機器人（Unmanned Surface-Remotely Operated Vehicle，US-ROV）見 圖1。US-ROV通過臍帶纜將水面操縱臺與ROV相連，臍帶纜為ROV提供動力并進行信息交互。USROV是一種新型的ROV，是未來水下工程檢測與特種作業的關鍵設備。

圖1 便攜式US-ROV

導航系統傳感器部分主要由慣性測量單元（Inertial Measurement Unit，IMU）組成，用來采集ROV的實時姿態信息。信息傳輸給控制模塊，用改進EKF算法進行ROV的姿態解算，同時為了防止GPS信號在水下丟失，需要水面無人艇進行GPS信號校正。導航系統結構如圖2所示。

圖2 導航系統結構

2 導航算法設計

2.1 EKF

ROV導航系統利用安裝的姿態傳感器獲取姿態信息數據，通過姿態解算確定其位置信息［10］。經狀態估計算法融合各類傳感器數據，通過坐標轉換解算出ROV姿態信息。較為常用的狀態估計算法為EKF［11］。EKF是通過最大似然估計在系統狀態過程中狀態向量近似估計的優化算法。US-ROV姿態導航采用傳統EKF時的狀態模型方程和狀態更新方程如下：

（1）時間更新

式中：Rk+1為測量噪聲在k+1時刻的協方差矩陣；Kk+1為k+1時刻的卡爾曼增益矩陣；Hk+1為狀態變量到測量的轉換矩陣；而

為在k+1時刻所求得的系統狀態協方差矩陣；

為在k+1時刻所求得的系統狀態估值。

由于水下作業不能設置靜態零速進行慣性導航系統采樣誤差修正［12］，因此需要采用高靈敏度傳感器進行ROV的姿態解算，但即使高靈敏度的傳感器仍然存在測量誤差。

由于傳感器采集到的數據會有很大的噪聲干擾，使輸出結果誤差較大。例如，加速度計傳輸的加速度信息需要經積分運算才能轉換到速度信息。因此，這種發散影響會隨著數學運算進一步增強［13］。雖然利用EKF可以根據當前狀態和上一時刻的狀態作為對比預，測輸出結果，在一定程度上優化了傳感器采樣值。但傳統EKF進行多傳感器信息融合仍然存在數據噪聲大、誤差較為明顯等問題［14-15］。

2.2 改進EKF

針對采用EKF進行多傳感器信息融合存在的誤差大、噪聲擾動大等問題。本文對EKF進行改進，著重探討以四元數為狀態量，進行基于四元數的改進EKF設計。

首先，根據系統需求建立EKF的非線性狀態方程：

式中：xk為k時刻的系統狀態變量；xk-1為k-1時刻的系統狀態變量；A為狀態轉移矩陣，負責將k和k-1時刻的狀態聯系起來；N(0，Qk)假定為零均值、方差為Q的高斯分布過程噪聲函數。而

為k時刻的系統觀測量；h(xk)為觀測模型，可將狀態量xk轉換到Zk中；N(0，Rk)假定為零均值、方差為R的高斯分布觀測噪聲函數。

根據式（6）和式（7）得出系統的EKF非線性方程，然后分析基于四元數的改進EKF方程。由四元數的定義可知，四元數是由4個元素構成的四維向量，用來表示載體在三維空間中的姿態方向信息，有

式中：q0、q1、q2、q3為實數；q0為實部；q1i+q2j+q3k為虛部。

基于四元數的改進EKF，建立狀態向量為

式中：q0、q1、q2、q3為姿態信息的四元數。

初始化為

初始化即默認載體的橫傾、俯仰、偏航3種姿態角都是從0°開始。

建立非線性狀態方程并完成初始化后，系統預測模型為

式中：qk-1為k-1時刻的四元數；qk為更新后k時刻的四元數。

得出預測模型后，需要知道四元數微分方程，四元數q的微分方程為

式中：ω=[0ωxωyωz]T為數據融合后運動坐標系下的三軸角速度；ωx為x軸角速度；ωy為y軸角速度；ωz為z軸角速度。

根據式（12）和式（6）可以得出系統預測模型為

通過雅可比矩陣線性化得到狀態轉移方程為

式中：F為四元數更新對ROV角速度狀態轉移矩陣。

根據式（14）求出狀態轉移方程后，對觀測量進行分析。為簡化推導過程，僅對姿態信息中的加速度計測量重力在運動坐標系下的三軸分量作為觀測數據進行分析。加速度計觀測數據為

式中：ax、ay、az為加速度計在運動坐標系下的三軸分量；

為觀測模型；g為重力加速度，是大地坐標系下的坐標向量，需要坐標轉化到載體坐標系下；R為旋轉矩陣，從地面坐標系轉化到運動坐標系；h(xk)為觀測模型，與前面的預測模型一樣進行線性化處理。線性化的觀測模型為

由于本文只對姿態信息中的加速度計測量重力在運動坐標系下的三軸分量作為觀測數據進行分析，所以H即為系統得到的總的觀測矩陣。

3 實驗仿真

ROV下水實驗如圖3所示。實驗測得的ROV姿態數據，經改進EKF算法解算可使姿態數據更為精準，達到對ROV精準控制的目的。

圖3 ROV下水實驗

對于實驗測得的數據，經程序仿真觀測改進EKF的數據處理效果。通過預測模型與觀測模型的理論分析，改進EKF程序設計步驟如下：

按上述步驟，將實驗數據輸入程序進行仿真。通過兩組實驗對改進EKF進行對比分析。坐標系根據坐標轉換規則，轉到前右下坐標系中，并使橫傾角速度對應到x軸，俯仰角速度對應到y軸，偏航角速度對應到z軸。在程序中用gx=rollrate、gy=pitchrate、gz=yawrate來實現坐標系的對齊。取兩組實驗數據進行仿真驗證，其中多傳感器信息的算法融合運用到了陀螺儀、加速度計以及磁力計傳感器。由于地磁計數據的加入使得實驗中偏航角并不是從零開始。

第1組實驗將傳統EKF與參考值進行對比分析，分別與橫傾、俯仰、偏航3種姿態角進行對比，分析EKF對于3種姿態角的影響。圖4為EKF與參考值對比。

圖4 EKF與參考值對比

第2組實驗將改進EKF和參考值進行對比分析，驗證改進EKF的優點。圖5為改進EKF算法與參考值對比。

圖5 改進EKF與參考值對比

采用改進EKF和傳統EKF分別與參考值對比，如表1和表2所示。通過對比分析可知，傳統EKF會有一定程度的噪聲干擾，而改進EKF不僅能在動態響應上符合要求，而且能很好地處理數據噪聲，使姿態數據曲線更加光滑。

表1 改進EKF與參考值對比

表2 EKF與參考值對比

4 結 語

針對ROV使用EKF進行姿態融合解算中存在的噪聲誤差問題，在基于四元數姿態解算算法的基礎上和EKF進行結合，改進EKF在精確性和實時性上均優于傳統EKF。仿真實驗表明，改進EKF可以精確處理數據，系統收斂性得到提高，可以滿足水下機器人在復雜海域條件下作業的需求。