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

朱紅坤，郭蘊(yùn)華，牟軍敏，胡甫才，任文峰

（1.武漢理工大學(xué) 船舶動(dòng)力工程技術(shù)交通行業(yè)重點(diǎn)實(shí)驗(yàn)室，武漢430063；2.武漢理工大學(xué) 航運(yùn)學(xué)院，武漢430063）

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

朱紅坤1，郭蘊(yùn)華1，牟軍敏2，胡甫才1，任文峰1

（1.武漢理工大學(xué) 船舶動(dòng)力工程技術(shù)交通行業(yè)重點(diǎn)實(shí)驗(yàn)室，武漢430063；2.武漢理工大學(xué) 航運(yùn)學(xué)院，武漢430063）

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

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

0 引 言

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

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

1 水下機(jī)器人動(dòng)力學(xué)模型

在運(yùn)動(dòng)坐標(biāo)系中6自由度水下機(jī)器人的動(dòng)力學(xué)模型可表示為[12]：

其中

本文以某水下機(jī)器人[12]為例，運(yùn)動(dòng)坐標(biāo)系中其重心坐標(biāo)為rG=［0,0,0 ］T，rB=［0, 0,-0.1 ］T重力和浮力分別為 W 和-B，θ=0°，ψ=0°，且有：

則該水下機(jī)器人的動(dòng)力學(xué)模型可以簡(jiǎn)化為：

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

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

2.1 水下機(jī)器人動(dòng)力學(xué)模型的總體最小二乘辨識(shí)算法

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

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

那么

由矩陣求逆引理知

則（16）式可以寫(xiě)成

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

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

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

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

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

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

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

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

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

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

現(xiàn)定義

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

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

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

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

3 水下機(jī)器人水動(dòng)力學(xué)模型辨識(shí)仿真

3.1 仿真實(shí)驗(yàn)

表1 最小二乘融合參數(shù)辨識(shí)的理論偏差與實(shí)驗(yàn)偏差（噪聲方差為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ù)辨識(shí)的理論偏差與實(shí)驗(yàn)偏差（噪聲方差為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時(shí)辨識(shí)誤差（艏向）Fig.1 Identification error when the noise variance is 0.1(yaw)

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

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

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

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

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

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

3.2 實(shí)驗(yàn)結(jié)果分析

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

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

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

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

4 結(jié) 論

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

[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]徐建安.水下機(jī)器人動(dòng)力學(xué)模型辨識(shí)與廣義預(yù)測(cè)控制技術(shù)研究[D].哈爾濱:哈爾濱工程大學(xué),2006.

[13]袁偉杰,劉貴杰,朱紹鋒.基于遺傳算法的自治水下機(jī)器人水動(dòng)力參數(shù)辨識(shí)方法[J].機(jī)械工程學(xué)報(bào),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]吳文祥,朱世強(qiáng),靳興來(lái).基于改進(jìn)傅里葉級(jí)數(shù)的機(jī)器人動(dòng)力學(xué)參數(shù)辨識(shí)[J].浙江大學(xué)學(xué)報(bào)工學(xué)版,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

國(guó)家自然科學(xué)基金（51579201）

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

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