王科宇，王俊雄

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

0 引 言

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

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

1 AUV 建模

1.1 坐標(biāo)系定義

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

圖1 坐標(biāo)系定義Fig.1 The definition of coordinate

1.2 運動學(xué)與動力學(xué)建模

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

其中：J1，J2為轉(zhuǎn)換矩陣，將正弦函數(shù)記作s，余弦函數(shù)記作c，正切函數(shù)記作t，則

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

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

2 慣性導(dǎo)航算法設(shè)計

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

圖2 慣性導(dǎo)航系統(tǒng)結(jié)構(gòu)框圖Fig.2 The diagram of inertial navigation system

2.1 位姿解算算法設(shè)計

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

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

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

為直觀表示濾波效果，將四元數(shù)姿態(tài)表示轉(zhuǎn)換成歐拉角表示。四元數(shù)q 與歐拉角[φ,θ,ψ]T有以下轉(zhuǎn)換關(guān)系：

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

2.2 平方根無跡卡爾曼濾波算法設(shè)計

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

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

設(shè)計平方根無跡卡爾曼濾波算法：

1）參數(shù)設(shè)置及初始化

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

2）計算sigma 點

采用對稱選點的方式，選取sigma 點近似非線性系統(tǒng)的統(tǒng)計特性。根據(jù)噪聲的分布特性確定sigma 的點數(shù)。

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

3）計算時間更新方程

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

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

5）結(jié)果更新

3 仿真、實驗與結(jié)果分析

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

圖3 靜態(tài)姿態(tài)解算結(jié)果Fig.3 The angle estimation with still state

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

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

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

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

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

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

圖5 姿態(tài)解算結(jié)果Fig.5 Angle estimation

圖6 速度解算結(jié)果Fig.6 Velocity estimation

圖7 位置解算結(jié)果Fig.7 Position estimation

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

圖8 姿態(tài)解算誤差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 工作平穩(wěn)時解算結(jié)果與INS+EKF 基本一致，當(dāng)AUV 突然改變工況時，仍能較好解算AUV 位置。速度、姿態(tài)誤差的增加主要出現(xiàn)在AUV 工作狀態(tài)突然改變時。定位誤差隨時間增長，但增長緩慢。其最大定位誤差小于2 m。

4 結(jié) 語

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