朱紅坤，郭蘊華，牟軍敏，胡甫才，任文峰

（1.武漢理工大學 船舶動力工程技術(shù)交通行業(yè)重點實驗室，武漢430063；2.武漢理工大學 航運學院，武漢430063）

基于多傳感器遞推總體最小二乘融合的水下機器人動力學模型參數(shù)辨識

朱紅坤1，郭蘊華1，牟軍敏2，胡甫才1，任文峰1

（1.武漢理工大學 船舶動力工程技術(shù)交通行業(yè)重點實驗室，武漢430063；2.武漢理工大學 航運學院，武漢430063）

對于水下機器人動力學模型辨識問題，如果其觀測方程的系數(shù)矩陣包含隨機擾動，則其最小二乘估計一般是有偏的。為此，該文提出一種基于多傳感器遞推總體最小二乘融合的水下機器人動力學模型辨識算法（RTLS_F）。首先，給出了集中式總體最小二乘融合的算法；然后，在總體最小二乘框架下，推導出多傳感器遞推融合估計算法。通過仿真實驗對RTLS_F與其它水下機器人動力學參數(shù)辨識算法進行了比較。實驗結(jié)果表明，在系數(shù)矩陣和觀測向量都含有誤差的情況下，最小二乘融合是有偏估計且難以提高估計精度，而RTLS_F算法可以有效改善參數(shù)辨識性能。

多傳感器融合；遞推總體最小二乘；水下機器人；參數(shù)辨識

0 引 言

水下機器人動力學模型是影響整個水下機器人運動精度的重要因素。因此，對于水下機器人動力學模型進行參數(shù)辨識尤為重要。現(xiàn)有的此類動力學模型的辨識方法通常都由預(yù)測誤差最小化、神經(jīng)網(wǎng)絡(luò)算法、和小波變換等方法發(fā)展而來[1-4]，其中在線辨識算法受到較多關(guān)注和研究[5-6]。Martin等[7]通過建模和實驗提出六自由度耦合非線性二階系統(tǒng)辨識的方法；Ridao等[8]提出一種能夠用于非線性多變量模型且數(shù)值性能較好的積分算法；Sabet等[9]提出擴展無跡卡爾曼濾波器用于提高AUV的參數(shù)辨識性能；Avila等[10]提出開架式水下機器人艏搖動力學參數(shù)的建模和辨識算法；Van等[11]提出水下機器人的基于神經(jīng)網(wǎng)絡(luò)算法的增廣辨識算法；徐[12]提出使用神經(jīng)網(wǎng)絡(luò)算法和廣義預(yù)測控制技術(shù)對水下機器人動力學模型進行辨識，實驗結(jié)果表明廣義預(yù)測控制算法具有較強的魯棒性；袁和劉等[13]提出使用遺傳算法進行水下機器人的動力學參數(shù)辨識。

現(xiàn)有的動力學參數(shù)辨識算法通常假設(shè)僅有推進器產(chǎn)生的力（力矩）向量和水下機器人受到外部干擾（洋流等）產(chǎn)生的力（力矩）向量存在觀測誤差，但是在實際操作中由于受到使用環(huán)境和測量儀器等條件的限制，使得位置、線速度、線加速度、角度、角速度、角加速度等測量值不可避免地存在觀測誤差，從而降低了動力學模型參數(shù)辨識精度。為了解決這個問題，本文考慮在總體最小二乘的框架下提出新的算法。Feng等[14]針對此類問題提出了基于瑞利商求解的快速遞推總體最小二乘算法（FRTLS）；Choi等[15]則提出計算量更小的遞推總體最小二乘算法（RTLS）。這兩種算法均能在一定程度上改善辨識性能，但是仍有可能難以滿足實際工程的精度要求。為此，本文通過對多傳感器進行數(shù)據(jù)融合提出精度更高的RTLS_F算法。

1 水下機器人動力學模型

在運動坐標系中6自由度水下機器人的動力學模型可表示為[12]：

其中

本文以某水下機器人[12]為例，運動坐標系中其重心坐標為rG=［0,0,0 ］T，rB=［0, 0,-0.1 ］T重力和浮力分別為 W 和-B，θ=0°，ψ=0°，且有：

則該水下機器人的動力學模型可以簡化為：

則可對其各單自由度分別進行辨識，現(xiàn)考慮其艏向和縱向自由度，（3）、（6）式可以寫成：

2 多傳感器融合的遞推總體最小二乘算法

2.1 水下機器人動力學模型的總體最小二乘辨識算法

但是，實際中測得的數(shù)據(jù)含有誤差，則（9）式最小二乘意義下的解為

則多傳感器最小二乘和總體最小二乘集中式融合可以寫成

那么

由矩陣求逆引理知

則（16）式可以寫成

這說明，在觀測向量和系數(shù)矩陣同時含有誤差的情況下，最小二乘融合只能得到有偏估計，且理論偏差為

2.2 多傳感器遞推總體最小二乘融合算法

上述集中式融合屬于批處理方法，不便于實時應(yīng)用。因此，考慮遞推的多傳感器總體最小二乘融合方法，令

對（21）式使用矩陣求逆引理可得：

ΔD=［ΔH ΔZ ］，D=D*+ΔD，方程組

其中：U和V分別為左奇異矩陣和右奇異矩陣，V=［v1,…,vn+1］，∑=diag（ σ1,…,σn+1），σ1＞σ2＞…＞σn+1，σj為矩陣D的第j個奇異值，則由文獻[14]知

將（12）、（13）和（23）式代入（28）式得：

將（21）式代入（29）式得：

最小特征矢量vn+1（t-1 ）可以表示成正交特征矢量v1（t）,…,vn+1（t）的線性組合：

因為P（t）和P（ t-1 ）高度相關(guān)，則vn+1（t）的系數(shù)cn+1（t）大于其他特征矢量系數(shù)，即：

現(xiàn)定義

通過 D 的奇異值分解和（31）、（33）式可得：

由（32）式和奇異值分解的性質(zhì)可知

最小特征矢量vn+1（）t即可近似計算

那么就可得RTLS_F算法的遞推公式

3 水下機器人水動力學模型辨識仿真

3.1 仿真實驗

表1 最小二乘融合參數(shù)辨識的理論偏差與實驗偏差（噪聲方差為0.1）Tab.1 Theoretical deviation and experimental deviation of parameter identification based on the fusion of recursive Least Squares(the variance of noise is 0.1)

表2 最小二乘融合參數(shù)辨識的理論偏差與實驗偏差（噪聲方差為0.2）Tab.2 Theoretical deviation and experimental deviation of parameter identification based on the fusion of recursive Least Squares(the variance of noise is 0.2)

圖1 噪聲方差為0.1時辨識誤差（艏向）Fig.1 Identification error when the noise variance is 0.1(yaw)

圖2 噪聲方差為0.2時辨識誤差（艏向）Fig.2 Identification error when the noise variance is 0.2(yaw)

圖3 噪聲方差為0.1時辨識誤差（縱向）Fig.3 Identification error when the noise variance is 0.1(pitch)

圖4 噪聲方差為0.2時辨識誤差（縱向）Fig.4 Identification error when the noise variance is 0.2(pitch)

圖5 不同算法對參數(shù)mr的估計結(jié)果Fig.5 Estimation results of parameter mrby different algorithms

圖6 不同算法對參數(shù)dr|r|的估計結(jié)果Fig.6 Estimation results of parameterdr|r|by different algorithms

圖中RLS1和RLS2是對應(yīng)傳感器1和傳感器2的測量數(shù)據(jù)的最小二乘算法，RLS_F表示多傳感器融合最小二乘算法。RTLS1和RTLS2是對應(yīng)傳感器1和傳感器2的總體最小二乘算法，RTLS_F表示本文提出的多傳感器融合遞推總體最小二乘算法，F(xiàn)RTLS表示對應(yīng)任意單傳感器的基于瑞利商解法的遞推總體最小二乘算法[14]。

3.2 實驗結(jié)果分析

（1）表1和表2中理論偏差與實驗偏差基本一致，說明系數(shù)矩陣和觀測向量同時含有誤差時最小二乘融合估計為有偏估計。正因如此，這種情況下最小二乘融合估計相對于單傳感器可能難以得到精度更高的結(jié)果，而圖1~4中局部放大圖也顯示出最小二乘融合沒有改善估計性能。

（2）在同等噪聲方差水平下，最小二乘及其融合算法估計誤差最大，估計誤差曲線幾乎重合。RTLS1、RTLS2、FRTLS估計精度均明顯好于最小二乘及其融合算法，這是因為總體最小二乘框架下的幾種算法同時考慮了系數(shù)矩陣和觀測向量的擾動，均為無偏估計算法。其中，F(xiàn)RTLS解法較為復(fù)雜，計算量更大，RTLS1和RTLS2估計誤差略小于FRTLS。RTLS_F估計誤差明顯小于其它算法，說明本文提出的RTLS_F算法估計性能優(yōu)于單傳感器算法和最小二乘融合算法。

（3）隨著采樣數(shù)據(jù)的增加，各種算法均逐漸趨于收斂，最小二乘類算法收斂最快，RTLS_F慢于最小二乘及其融合算法，但快于其他各種算法。不過，隨著數(shù)據(jù)的增加，最小二乘及其融合算法只能收斂到有偏的結(jié)果，難以進一步提高精度。

（4）圖5和圖6中算法RTLS_F的估計結(jié)果最接近真實值，其他算法偏差均較大，即驗證了本文提出的算法的有效性。

4 結(jié) 論

本文同時考慮了水下機器人動力學模型辨識中系數(shù)矩陣噪聲和觀測向量噪聲的影響，通過對多傳感器數(shù)據(jù)進行融合提出基于多傳感器遞推總體最小二乘融合的水下機器人動力學模型辨識算法。仿真結(jié)果表明，RTLS_F算法估計精度明顯高于其他算法，收斂速度也略優(yōu)于RTLS單傳感器算法和FRTLS算法，能夠顯著提高動力學模型辨識的精度。

[1]Zhabko N A.Parameter identification of autonomous underwater vehicle dynamic model[C]//Stability and Control Processes in Memory of VI Zubov(SCP),2015 International Conference.IEEE,2015:611-613.

[2]Skjetne R,Smogeli Y N,Fossen T I.A nonlinear ship manoeuvering model:Identification and adaptive control with experiments for a model ship[J].Modeling Identification&Control,2004,25(1):3-27.

[3]Muske K R,Ashrafiuon H,Haas G,et al.Identification of a control oriented nonlinear dynamic USV model[C]//American Control Conference.IEEE,2008:562-567.

[4]Wirtensohn S,Reuter J,Blaich M,et al.Modelling and identification of a twin hull-based autonomous surface craft[C]//Methods and Models in Automation and Robotics(MMAR),2013 18th International Conference.IEEE,2013:121-126.

[5]Eng Y H,Teo K M,Chitre M,et al.Online system identification of an autonomous underwater vehicle via in-field experiments[J].IEEE Journal of Oceanic Engineering,2016,41(1):5-17.

[6]Hong E Y,Meng T K,Chitre M.Online system identification of the dynamics of an autonomous underwater vehicle[C]//Underwater Technology Symposium(UT),2013 IEEE International.IEEE,2013:1-10.

[7]Martin S C,Whitcomb L L.Experimental Identification of six-degree-of-freedom coupled dynamic plant models for underwater robot vehicles[J].IEEE Journal of Oceanic Engineering,2014,39(4):662-671.

[8]Ridao P,Tiano A,El-Fakdi A,et al.On the identification of non-linear models of unmanned underwater vehicles[J].Control Engineering Practice,2004,12(12):1483-1499.

[9]Sabet M T,Sarhadi P,Zarini M.Extended and unscented Kalman filters for parameter estimation of an autonomous underwater vehicle[J].Ocean Engineering,2014,91:329-339.

[10]Avila J P,Adamowski J C,Maruyama N,et al.Modeling and identification of an open-frame underwater vehicle:The yaw motion dynamics[J].Journal of Intelligent&Robotic Systems,2012,66(1):37-56.

[11]Van De Ven P W J,Johansen T A,S?rensen A J,et al.Neural network augmented identification of underwater vehicle models[J].Control Engineering Practice,2007,15(6):715-725.

[12]徐建安.水下機器人動力學模型辨識與廣義預(yù)測控制技術(shù)研究[D].哈爾濱:哈爾濱工程大學,2006.

[13]袁偉杰,劉貴杰,朱紹鋒.基于遺傳算法的自治水下機器人水動力參數(shù)辨識方法[J].機械工程學報,2010,46(11):96-100.Yuan W J,Liu G J,Zhu S F.Identification method of hydrodynamic parameters of autonomous underwater vehicle based on genetic algorithm[J].Journal of Mechanical Engineering,2010,46(11):96-100.

[14]Feng D Z,Zhang X D,Chang D X,et al.A fast recursive total least squares algorithm for adaptive FIR filtering[J].IEEE Transactions on Signal Processing,2004,52(10):2729-2737.

[15]Choi N,Lim J S,Sung K M.An efficient recursive total least squares algorithm for training multilayer feedforward neural networks[C]//International Symposium on Neural Networks.Springer Berlin Heidelberg,2005:558-565.

[16]Huffel S V,Vandewalle J.The total least squares problem:Computational aspects and analysis[M].SIAM,Philadelphia,1991.

[17]吳文祥,朱世強,靳興來.基于改進傅里葉級數(shù)的機器人動力學參數(shù)辨識[J].浙江大學學報工學版,2013,47(2):231-237.Wu W X,Zhu S Q,Jin X L.Dynamic identification for robot manipulators based on modified fourier series[J].Journal of Zhejiang University,2013,47(47):231-237.

Dynamics model identification of underwater vehicles based on the multi-sensor fusion of recursive total least squares

ZHU Hong-kun1,Guo Yun-hua1,MOU Jun-min2,HU Fu-cai1,REN Wen-feng1
(1.Key Laboratory of Marine Power Engineering&Technology,Ministry of Communications,Wuhan University of Technology,Wuhan 430063,China;2.School of Navigation,Wuhan University of Technology,Wuhan 430063,China)

For the dynamics model identification of the underwater vehicles,if the coefficient matrix of the observed equation contains random perturbation,its least squares estimation is generally biased.In this paper,a novel algorithm(RTLS_F)for the dynamic model identification of the underwater vehicle is proposed.The centralized fusion method of total least squares is given.Under the framework of the total least squares,the algorithm of multi-sensor recursive fusion is deduced.Performance comparisons between the proposed and the other algorithms are carried out through the simulation experiments.The experimental results show that the least squares fusion is the biased estimation and it is difficult to improve the estimation accuracy if both the coefficient matrix and the observed vector contain errors,whereas the RTLS_F algorithm can effectively improve the performance of parameter identification in the same situation.

multi-sensor fusion;recursive total least squares;underwater vehicle;parameter identification

U661.33

A

10.3969/j.issn.1007-7294.2017.10.010

1007-7294（2017）10-1263-08

2017-03-23

國家自然科學基金（51579201）

朱紅坤（1992-），男，碩士生，E-mail:wutzhk@163.com； 郭蘊華（1975-），男，博士，教授；

牟軍敏（1974-），男，博士，教授。