朱红坤，郭蕴华，牟军敏，胡甫才，任文峰

（1.武汉理工大学 船舶动力工程技术交通行业重点实验室，武汉430063；2.武汉理工大学 航运学院，武汉430063）

基于多传感器递推总体最小二乘融合的水下机器人动力学模型参数辨识

朱红坤1，郭蕴华1，牟军敏2，胡甫才1，任文峰1

（1.武汉理工大学 船舶动力工程技术交通行业重点实验室，武汉430063；2.武汉理工大学 航运学院，武汉430063）

对于水下机器人动力学模型辨识问题，如果其观测方程的系数矩阵包含随机扰动，则其最小二乘估计一般是有偏的。为此，该文提出一种基于多传感器递推总体最小二乘融合的水下机器人动力学模型辨识算法（RTLS_F）。首先，给出了集中式总体最小二乘融合的算法；然后，在总体最小二乘框架下，推导出多传感器递推融合估计算法。通过仿真实验对RTLS_F与其它水下机器人动力学参数辨识算法进行了比较。实验结果表明，在系数矩阵和观测向量都含有误差的情况下，最小二乘融合是有偏估计且难以提高估计精度，而RTLS_F算法可以有效改善参数辨识性能。

多传感器融合；递推总体最小二乘；水下机器人；参数辨识

0 引 言

水下机器人动力学模型是影响整个水下机器人运动精度的重要因素。因此，对于水下机器人动力学模型进行参数辨识尤为重要。现有的此类动力学模型的辨识方法通常都由预测误差最小化、神经网络算法、和小波变换等方法发展而来[1-4]，其中在线辨识算法受到较多关注和研究[5-6]。Martin等[7]通过建模和实验提出六自由度耦合非线性二阶系统辨识的方法；Ridao等[8]提出一种能够用于非线性多变量模型且数值性能较好的积分算法；Sabet等[9]提出扩展无迹卡尔曼滤波器用于提高AUV的参数辨识性能；Avila等[10]提出开架式水下机器人艏摇动力学参数的建模和辨识算法；Van等[11]提出水下机器人的基于神经网络算法的增广辨识算法；徐[12]提出使用神经网络算法和广义预测控制技术对水下机器人动力学模型进行辨识，实验结果表明广义预测控制算法具有较强的鲁棒性；袁和刘等[13]提出使用遗传算法进行水下机器人的动力学参数辨识。

现有的动力学参数辨识算法通常假设仅有推进器产生的力（力矩）向量和水下机器人受到外部干扰（洋流等）产生的力（力矩）向量存在观测误差，但是在实际操作中由于受到使用环境和测量仪器等条件的限制，使得位置、线速度、线加速度、角度、角速度、角加速度等测量值不可避免地存在观测误差，从而降低了动力学模型参数辨识精度。为了解决这个问题，本文考虑在总体最小二乘的框架下提出新的算法。Feng等[14]针对此类问题提出了基于瑞利商求解的快速递推总体最小二乘算法（FRTLS）；Choi等[15]则提出计算量更小的递推总体最小二乘算法（RTLS）。这两种算法均能在一定程度上改善辨识性能，但是仍有可能难以满足实际工程的精度要求。为此，本文通过对多传感器进行数据融合提出精度更高的RTLS_F算法。

1 水下机器人动力学模型

在运动坐标系中6自由度水下机器人的动力学模型可表示为[12]：

其中

本文以某水下机器人[12]为例，运动坐标系中其重心坐标为rG=［0,0,0 ］T，rB=［0, 0,-0.1 ］T重力和浮力分别为 W 和-B，θ=0°，ψ=0°，且有：

则该水下机器人的动力学模型可以简化为：

则可对其各单自由度分别进行辨识，现考虑其艏向和纵向自由度，（3）、（6）式可以写成：

2 多传感器融合的递推总体最小二乘算法

2.1 水下机器人动力学模型的总体最小二乘辨识算法

但是，实际中测得的数据含有误差，则（9）式最小二乘意义下的解为

则多传感器最小二乘和总体最小二乘集中式融合可以写成

那么

由矩阵求逆引理知

则（16）式可以写成

这说明，在观测向量和系数矩阵同时含有误差的情况下，最小二乘融合只能得到有偏估计，且理论偏差为

2.2 多传感器递推总体最小二乘融合算法

上述集中式融合属于批处理方法，不便于实时应用。因此，考虑递推的多传感器总体最小二乘融合方法，令

对（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 ）高度相关，则vn+1（t）的系数cn+1（t）大于其他特征矢量系数，即：

现定义

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

由（32）式和奇异值分解的性质可知

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

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

3 水下机器人水动力学模型辨识仿真

3.1 仿真实验

表1 最小二乘融合参数辨识的理论偏差与实验偏差（噪声方差为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 最小二乘融合参数辨识的理论偏差与实验偏差（噪声方差为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 不同算法对参数mr的估计结果Fig.5 Estimation results of parameter mrby different algorithms

图6 不同算法对参数dr|r|的估计结果Fig.6 Estimation results of parameterdr|r|by different algorithms

图中RLS1和RLS2是对应传感器1和传感器2的测量数据的最小二乘算法，RLS_F表示多传感器融合最小二乘算法。RTLS1和RTLS2是对应传感器1和传感器2的总体最小二乘算法，RTLS_F表示本文提出的多传感器融合递推总体最小二乘算法，FRTLS表示对应任意单传感器的基于瑞利商解法的递推总体最小二乘算法[14]。

3.2 实验结果分析

（1）表1和表2中理论偏差与实验偏差基本一致，说明系数矩阵和观测向量同时含有误差时最小二乘融合估计为有偏估计。正因如此，这种情况下最小二乘融合估计相对于单传感器可能难以得到精度更高的结果，而图1~4中局部放大图也显示出最小二乘融合没有改善估计性能。

（2）在同等噪声方差水平下，最小二乘及其融合算法估计误差最大，估计误差曲线几乎重合。RTLS1、RTLS2、FRTLS估计精度均明显好于最小二乘及其融合算法，这是因为总体最小二乘框架下的几种算法同时考虑了系数矩阵和观测向量的扰动，均为无偏估计算法。其中，FRTLS解法较为复杂，计算量更大，RTLS1和RTLS2估计误差略小于FRTLS。RTLS_F估计误差明显小于其它算法，说明本文提出的RTLS_F算法估计性能优于单传感器算法和最小二乘融合算法。

（3）随着采样数据的增加，各种算法均逐渐趋于收敛，最小二乘类算法收敛最快，RTLS_F慢于最小二乘及其融合算法，但快于其他各种算法。不过，随着数据的增加，最小二乘及其融合算法只能收敛到有偏的结果，难以进一步提高精度。

（4）图5和图6中算法RTLS_F的估计结果最接近真实值，其他算法偏差均较大，即验证了本文提出的算法的有效性。

4 结 论

本文同时考虑了水下机器人动力学模型辨识中系数矩阵噪声和观测向量噪声的影响，通过对多传感器数据进行融合提出基于多传感器递推总体最小二乘融合的水下机器人动力学模型辨识算法。仿真结果表明，RTLS_F算法估计精度明显高于其他算法，收敛速度也略优于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]徐建安.水下机器人动力学模型辨识与广义预测控制技术研究[D].哈尔滨:哈尔滨工程大学,2006.

[13]袁伟杰,刘贵杰,朱绍锋.基于遗传算法的自治水下机器人水动力参数辨识方法[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]吴文祥,朱世强,靳兴来.基于改进傅里叶级数的机器人动力学参数辨识[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-），男，博士，教授。