基于SRUKF的模型輔助AUV慣性導航系統

王科宇，王俊雄

(上海交通大學 船舶海洋工程國家重點實驗室，上海 200240)

0 引 言

水下自主航行器（AUV）可用于海底資源勘探、目標偵察等，是開發海洋的重要工具。小型AUV 具有成本低、作業方便等優點，隨著海洋開發與商業化的深入，逐漸成為AUV 的重要發展方向[1]。小型AUV 執行任務多樣，作業水況復雜，對于自主定位導航能力要求較高。低功耗、高可靠的導航定位方案成為小型AUV 開發亟待解決的研究熱點。

大量的AUV 導航系統，依賴多普勒測速儀、深度計、磁力計等傳感器測得數據進行導航，而小型AUV 限于成本、功耗與體積，往往不能裝備足夠多的傳感器。同時由于小型AUV 作業工況復雜，多普勒測速儀等在某些情況下不能正常工作，導航系統的可靠性低?；谖C電系統（MEMS）的慣性導航系統（INS），成本低、體積小、不依賴外界信號、反應迅速，是AUV 重要的導航方案。本文基于平方根無跡卡爾曼濾波算法，設計了非線性濾波器，并使用模型輔助提高算法精度。同時經過半實物與仿真實驗，驗證了算法的有效性。

1 AUV 建模

1.1 坐標系定義

在導航中使用2 種坐標系。一種是慣性坐標系On-XnYnZn，固結在地球上。另一種是載體坐標系Ob-XbYbZb，固結在AUV 重心上。為直觀起見，采用歐拉角表示載體坐標系相對慣性坐標系的旋轉。定義載體坐標系相對慣性系按X-Y-Z軸依次旋轉，其歐拉角為[φ,θ,ψ]T，則 φ，θ，ψ分別為AUV 的橫滾角、縱傾角、首向角，如圖1 所示。

圖1 坐標系定義Fig.1 The definition of coordinate

1.2 運動學與動力學建模

設AUV 在慣性坐標系下的坐標為η1=[x,y,z]T，令η2=[φ,θ,ψ]T，AUV在載體坐標系下的速度為ν1= [u,v,w]T，角速度為ν2=[p,q,r]T，令η=[η1,η2]T，ν=[ν1,ν2]T，則可建立AUV 的運動學模型如下：

其中：J1，J2為轉換矩陣，將正弦函數記作s，余弦函數記作c，正切函數記作t，則

水動力外形對于AUV 的影響十分顯著，本文基于Kambara ROV 水動力外形參數建立AUV 六自由度動力學模型[2]為：

其中：M=MA+MRB為質量矩陣，C=CA+CRB為科氏矩陣，D為水動力阻尼矩陣，g(?)為回復力，τ為AUV 所受合外力，則

2 慣性導航算法設計

傳統的模型輔助AUV 導航定位[3]，在AUV 速度計算中引入模型輔助作為參考，忽略了AUV 模型在姿態解算中的潛在應用[4]。為了綜合利用AUV 傳感器測得信息和模型解算得到的加速度與角速度信息，本文設計了基于平方根無跡卡爾曼濾波的慣性導航算法。本文采用的慣性導航系統結構框圖如圖2 所示。

圖2 慣性導航系統結構框圖Fig.2 The diagram of inertial navigation system

2.1 位姿解算算法設計

在慣性導航算法中，為降低運算量，并避免奇異角等問題，使用單位四元數q實時解算AUV 姿態。載體坐標系與慣性坐標系重合時，四元數q=[1,0,0,0]T。

蘇州吉恒納米科技有限公司是上市控股公司控股，主要從事表面PVD涂層，公司擁有一批十幾年涂層經驗的專業團隊，引進歐洲先進的PVD涂層設備，致力于成為國內高端涂層加工服務廠商，豐富的生產經驗，完善的質量管控，嚴謹工藝開發，齊全的檢測設備。在確保質量穩定的前提下,提供了客戶最快的涂層交期與完善及時的售后服務。保證了在同行業中擁有強大的競爭力。

設k時刻AUV 角度變化量為Δθ=[θx,θy,θz]T，加速度值為ak=[ax,ay,az]T。采用角度變化量形式的畢卡算法求解四元數[5]。本文采用4 階近似形式以保證精度，則

為直觀表示濾波效果，將四元數姿態表示轉換成歐拉角表示。四元數q 與歐拉角[φ,θ,ψ]T有以下轉換關系：

小型AUV 由于體積、功率限制，其加速度變化率有限，因此采用tk時刻與tk?1時刻的加速度平均值作為tk?tk?1時間段的加速度值近似平均值，并計算得到tk時刻速度值。同理，采用慣性坐標系下tk時刻與tk?1時刻的速度平均值作為tk?tk?1時間段的速度值，并與AUV 運動學方程聯立，計算得到tk時刻AUV 坐標。

2.2 平方根無跡卡爾曼濾波算法設計

由式（4）可知，AUV 的6 個自由度間的運動互相交叉耦合，運動方程高度非線性化。擴展卡爾曼濾波（EKF）在計算時使用一階泰勒展開線性化狀態方程，性能不穩定，結果易發散。本文采用平方根無跡卡爾曼濾波算法對傳感器數據與AUV 模型計算得到數據進行濾波融合處理，通過選取sigma 點進行無跡變換，匹配AUV 運動數據的統計特性，從而保障了估計精度[6–7]。同時，在濾波中使用協方差平方根代替協方差進行運算，保障了協方差的對稱正定性，避免濾波算法因舍入誤差積累而發散，提高了算法穩定性。

選取AUV 運行時的加速度a與角速度ν2作為系統狀態量，則X=[a,ν2]T。選取MEMS 慣性測量單元（IMU）測量得到的加速度與角速度作為系統觀測量，則

設計平方根無跡卡爾曼濾波算法：

1）參數設置及初始化

其中chol(A)為矩陣A的Cholesky 分解的上三角矩陣。

2）計算sigma 點

采用對稱選點的方式，選取sigma 點近似非線性系統的統計特性。根據噪聲的分布特性確定sigma 的點數。

其中n為 系統狀態量的維度，此處n=6。λ=α2(n+κ)?n，α，κ為第一、第三刻度因數，決定sigma 點距離平均值點的離散程度。

3）計算時間更新方程

式中：f(*)為濾波系統的狀態方程，這里為AUV 動力學方程式（4），u為上一步濾波后INS 系統解算得到的AUV 姿態、速度數據以及AUV 受到的推進力。

其中：qr(A)計算矩陣A的QR分解的上三角矩陣；cholupdate(R,x)計算矩陣A+x?x?1的Cholesky 分解的上三角矩陣，其中R=chol(A)；ωm，ωc，ωm0，ωc0按照以下公式計算得到：

5）結果更新

3 仿真、實驗與結果分析

使用手機模擬AUV 進行算法的靜態測試。將手機靜置，基于手機內置MEMS IMU 得到加速度與角速度測量數據，并輸入導航算法中進行測試。濾波、解算得到AUV 靜態時姿態如圖3 所示。由圖可知，基于SRUKF 的慣性導航算法能夠較好補償靜態誤差，角度誤差在0.3°以內，且誤差增長緩慢。

圖3 靜態姿態解算結果Fig.3 The angle estimation with still state

使用Matlab 搭建仿真模型，測試AUV 動態運行時慣性導航算法的性能。仿真程序的仿真流程圖如圖4所示。

圖4 仿真流程圖Fig.4 The diagram of simulation flowchart

考慮MEMS 傳感器的刻度因子誤差、非正交安裝誤差、系統誤差等，建立加速度傳感器的誤差模型：

其中βt是t時刻角速度計的白噪聲。

基于上述仿真模型，進行動態誤差測試。計算AUV 在純慣性導航（INS）、使用EKF 濾波（INS+EKF）、使用SRUKF 濾波（INS+SRUKF）3 種情況下的導航效果。解算得到AUV 姿態、速度、位置數據和誤差如圖5～圖10 所示。

由圖5 和圖6 可知，INS 解算的姿態和速度誤差積累迅速。由圖7 可知，INS 解算位置誤差迅速達到百米量級，x軸誤差已大于1 000 m，使得解算結果完全不可用。

圖5 姿態解算結果Fig.5 Angle estimation

圖6 速度解算結果Fig.6 Velocity estimation

圖7 位置解算結果Fig.7 Position estimation

由圖8 和圖9 可知，INS+EKF 解算出的姿態和速度誤差均大于INS+SRUKF 解算結果，且由圖10 可知，由于位置由速度和姿態積分得到，速度和姿態的誤差經積分運算迅速擴大，使得INS+EKF 導航的結果更加不可靠。分析可知，AUV 受力簡單、工作平穩時工作點的1 階泰勒展開能較好反應AUV 運動狀態，此時INS+EKF 解算效果較好。當AUV 突然改變工況、六自由度運動相互影響加劇時，INS+EKF 出現發散現象。

圖8 姿態解算誤差Fig.8 The error of angle estimation

圖9 速度解算誤差Fig.9 The error of velocity estimation

圖10 位移解算誤差Fig.10 The error of position estimation

使用INS+SRUKF 進行解算，AUV 工作平穩時解算結果與INS+EKF 基本一致，當AUV 突然改變工況時，仍能較好解算AUV 位置。速度、姿態誤差的增加主要出現在AUV 工作狀態突然改變時。定位誤差隨時間增長，但增長緩慢。其最大定位誤差小于2 m。

4 結 語

通過建立小型AUV 的運動學和動力學模型，解算得到AUV 的運動狀態和位置信息，可以為限于功耗、體積而不能裝備足夠傳感器的小型AUV 提供定位導航參考。設計平方根無跡卡爾曼濾波算法，綜合利用AUV模型解算數據和MEMS IMU 測量數據，并能夠克服AUV 運動方程高度非線性的困難，提高慣性導航精度。同時經過半實物實驗和Matlab 仿真實驗，驗證了算法的可行性。結果表明，基于模型輔助的平方根無跡卡爾曼濾波算法，性能優于擴展卡爾曼濾波算法，能夠顯著提高慣性導航的精度，滿足小型AUV 的導航定位需求。