一种适用于水下机器人的在线水动力参数辨识方法

1.本发明涉及水下机器人参数辨识领域，尤其涉及变结构水下机器人在线水动力参数辨识方法。


背景技术：

2.水下机器人是海洋油气勘探、油气管线巡检、水下油气管线铺设等海洋工程任务的主要装备，水下机器人主要包括遥控水下机器人和自主水下机器人，其中遥控水下机器人以脐带电缆为纽带，从母船获得能源供给和遥控指令，承担水下油气管线铺设、水下精细作业等任务，遥控水下机器人搭载作业机械臂，作业能力较强，但是阻力较大、航行速度较慢且作业半径有限；自主水下机器人本身携带能源，不受到脐带电缆约束，依托水声设备进行定位通信，搭载声学、光学、水文多种探测载荷，多承担水下资源勘查、区域巡航、远距离投送等任务，自主水下机器人作业半径较大，但是能源和空间有限，无法搭载破坏流体线型的作业机械臂，一般无作业能力。对比传统水下机器人，变结构水下机器人是兼容自主水下机器人和遥控水下机器人优势的新概念机器人，它自身携带能源、作业机械臂，外形是低阻力流线形，通过变形方式实现稳定作业模式和低阻巡航模式的切换。变结构水下机器人实现了航渡、勘察、小区域精细目标作业等任务的一体化作业，极大地降低了海洋工程的作业时间成本和设备保障成本。与传统水下机器人是恒定水动力学参数模型不同，变形前后机器人的水动力参数模型发生显著变化，增加了机器人控制器设计的难度。变结构水下机器人的主要技术难点是如何在线辨识变形前后机器人水动力参数模型，为机器人控制器设计提供精确模型参数支撑。
3.与传统水下机器人近似恒定流体外形不同，变结构水下机器人通过变形方式切换航行模式和机械臂作业模式，即航行模式机械臂位于艇体内部，作业模式机械臂位于艇体艏部，变结构水下机器人变形前后流体外形变化显著，即变形前后机器人的水动力系数模型发生显著变化。


技术实现要素：

4.针对变结构机器人变形前后水动力参数模型变化显著的问题，本发明提出一种基于动力学模型的在线水动力参数辨识方法，创新地利用导航传感器量测与动力学模型速度先验的误差、水动力参数与速度先验的数据关联反馈校正水动力参数并估计参数辨识的精度，为降低量测数据累积的“数据饱和”风险，在辨识过程中引入人工过程驱动噪声，提升了对水动力模型变化的跟踪能力，实现了变结构水下机器人利用自身导航传感器数据对水动力参数的在线辨识，并以水动力系数为控制量设计基础，实现水下机器人的高精度运动控制。
5.本发明为实现上述目的所采用的技术方案是：
6.一种适用于水下机器人的在线水动力参数辨识方法，包括以下步骤：
7.1)根据重心、转动惯量、水动力附加质量，计算水下机器人在当前时刻的附加惯性
质量矩阵；
8.2)基于水下机器人上一时刻的重力、浮力、转动惯量、水动力附加质量，预测当前时刻的静力学矩阵；
9.3)基于水下机器人上一时刻的粘性水动力系数，预测当前时刻的水动力的阻力/阻力距向量；
10.4)根据当前时刻的静力学矩阵和阻力/阻力距向量，得到当前时刻的粘性水动力系数先验和速度先验；
11.5)计算水下机器人从上一时刻到当前时刻的状态迁移矩阵；
12.6)基于水下机器人上一时刻粘性水动力系数先验精度和速度先验精度，预测当前时刻粘性水动力系数先验精度和速度先验精度；
13.7)基于导航传感器量测值，更新水动力系数和速度的估计及精度；
14.8)迭代步骤1)～步骤7)，完成从初始时刻到当前时刻的在线估计水下机器人的粘性水动力系数，实现水下机器人水动力参数的在线辨识，并依据水动力系数结合目标速度、目标姿态预测机器人阻力和阻力矩，分配水下机器人各推进器控制转速，实现水下机器人的运动控制。
15.所述步骤2)具体为：根据水下机器人在上一时刻t
k-1
的重力、浮力、转动惯量、水动力附加质量，预测在当前时刻tk水下机器人的重力、浮力、向心力的静力学矩阵
[0016][0017]
其中，g(η)表示重力和浮力在载体坐标系的合力和合力矩，c(ν)是向心力和向心力矩的系数矩阵，c
11
,c
12
,c
21
,c
22
是c(ν)的子块矩阵，w和b分别表示水下机器人的重力和浮力，表示姿态角的旋转矩阵，表示水下机器人的浮心，表示水下机器人的重心，η表示水下机器人的位姿向量，ν表示水下机器人的速度向量，ν1、ν2、u、v、w、p、q、r均是ν的子元素，m表示水下机器人的质量，s(
·
)表示计算向量的反对称矩阵算子，分别表示水下机器人的水动力附加质量，ib表示水下机器人的转动惯量。
[0018]
所述步骤3)具体为：根据水下机器人在上一时刻t
k-1
的粘性水动力系数，预测水下机器人在当前时刻tk的水动力的阻力/阻力矩向量
[0019][0020]
其中，d(ν)是粘性水动力系数矩阵，d
11
,d
12
,d
21
,d
22
是d(ν)的子块矩阵，ω是粘性水动力系数集合，x
u|u|
,y
v|v|
,y
uvl
,z
w|w|
,z
uwl
,y
r|r|
|r|,y
url
,z
q|q|
,z
uql
,z
q|q|
,z
uql
,m
w|w|
,m
uwl
,n
v|v|
,n
uvl
,k
p|p|
,m
q|q|
,m
uql
,n
r|r|
,n
url
均是ω的子元素，dlg(
·
)表示对角矩阵算子。
[0021]
所述步骤4)具体为：基于机器人的动力学模型，根据水下机器人在上一时刻t
k-1
的粘性水动力系数ω、速度ν、控制输入量τ，预测水下机器人在当前时刻tk的粘性水动力系数先验ω-和速度先验ν-：
[0022][0023]
其中，x-是ω-和ν-的按行合并矩阵，ν

表示水下机器人的速度估计。
[0024]
所述步骤5)具体为：根据水下机器人在上一时刻t
k-1
的粘性水动力系数矩阵、向心力和向心力矩的系数矩阵，计算水下机器人从上一时刻t
k-1
到当前时刻tk的状态迁移矩阵f：
[0025][0026]
其中，f
11
和f
12
是f的子块矩阵，m、c(ν)、d(ν)分别表示水下机器人的附加惯性质量矩阵、向心力/向心力矩系数矩阵、粘性水动力系数矩阵，δt表示相邻时刻的时间间隔。
[0027]
所述步骤6)具体为：根据水下机器人在上一时刻t
k-1
的粘性水动力系数矩阵、向心力和向心力矩的系数矩阵速度精度、控制输入量精度，同时引入人工过程驱动噪声，计算粘性水动力系数和速度的复合先验精度，得到水下机器人在当前时刻tk的粘性水动力系数先验精度p(ω-)、速度先验精度p(ν-)、粘性水动力系数和速度的复合先验精度p-：
[0028][0029]
其中，是p-的子块矩阵，p(ν

)是上一时刻t
k-1
速度后验估计的精度，p(ω

)是上一时刻t
k-1
粘性水动力系数后验估计的精度，r(τ)表示控制力/控制力矩的误差方差，r(ν)表示机器人的速度的误差方差矩阵，q(ω)表示粘性水动力系数估计的过程驱动噪声矩阵，和分别是多普勒和光纤罗经的设备参数。
[0030]
所述步骤7)具体为：根据导航传感器量测与动力学模型速度先验的误差反馈校正水下机器人的水动力参数估计和速度估计，并计算水动力参数估计精度，即根据水下机器人在当前时刻tk的粘性水动力系数的先验、速度的先验及二者复合先验精度，计算水下机器人在当前时刻tk的粘性水动力系数估计ω

及其精度p(ω

)、速度估计v

及其精度p(ν

)：
[0031][0032]
其中，p

表示水下机器人在时刻tk的粘性水动力系数和速度的复合精度估计，是p

的子块矩阵，ω-和ν-分别表示水下机器人在当前时刻tk的粘性水动力系数先验和速度先验，k表示在时刻tk的校正增益，k
11
,k
21
是k的子块矩阵。
[0033]
本发明具有以下有益效果及优点：
[0034]
1.相对于传统水下机器人离线参数辨识方法，本发明具有在线估计变结构水下机器人变形前后水动力参数变化的能力，具有更好的算法鲁棒性和环境适应性，实现了在线
辨识变结构水下机器人水动力模型参数。以水动力系数为基础，提前预测机器人的阻力和阻力矩，实现水下机器人实现高精度运动控制。
[0035]
2.应用范围广。本发明不但可以应用于变结构水下机器人变形前后、巡航、水下作业工况的水动力参数辨识，还可以用于遥控水下机器人精细作业和自主水下机器人巡航应用场景的水动力参数辨识。
[0036]
3.经济成本较低。本发明不需额外增加传感器而只依托机器人本身传感器即可实现水下机器人的水动力参数辨识，降低了水下机器人的设计成本。
附图说明
[0037]
图1a是本发明的变结构水下机器人结构图——俯视图；
[0038]
图1b是本发明的变结构水下机器人结构图——侧视图；
[0039]
图2是本发明的变结构在线水动力参数辨识方法的流程图。
具体实施方式
[0040]
下面结合附图及实施例对本发明做进一步的详细说明。
[0041]
整个系统组成包括变结构水下机器人、变结构水下机器人搭载的多普勒、光纤罗经、变结构装置、机械臂、推进器等设备。其中变结构水下机器人是搭载多普勒、光纤罗经、机械臂、变结构装置的运载具，多普勒、光纤罗经是变结构水下机器人的导航传感器，多普勒测量水下机器人相对海底的航行速度，它包括前向速度、右向速度、垂向速度等；光纤罗经的作用是测量机器人的姿态角、角速度等信息，它包括横滚角、纵倾角、航向角、横滚角速度、纵倾角速度和转艏角速度；机械臂承担水下样品目标抓取、管线维修等水下作业任务；变结构装置的作用是支撑机器人水下变形和稳心高的动态改变，实现敏捷航行模式和高效作业模式的快速切换；推进器为变结构提供在前向、右向、垂向的控制力和力矩。
[0042]
系统组成如图1a和图1b所示。
[0043]
如图2所示，整个系统按照如下流程工作：
[0044]
首先，计算当前时刻水下机器人的附加惯性质量矩阵、重力/浮力的静力学矩阵、与水动力参数数据相关的水动力力矩矩阵；然后，基于水动力模型预测水下机器人的速度和水动力系数，并在参数估计过程中引入人工过程驱动噪声；计算当前时刻的状态迁移矩阵和预测水动力参数系数的先验精度；最后，根据导航传感器量测与动力学模型速度先验的误差反馈校正水下机器人的水动力参数估计和速度估计，并计算水动力参数估计精度。
[0045]
为了阐述方便，将本文的符号变量进行如下定义：定义t表示时间变量，tk表示第k个时刻，k表示时间索引，且t
k-1
≤tk。定义u、v、w、p、q、r分别表示水下机器人的前向速度、右向速度、垂向速度、横滚角速度、纵倾角速度、转艏角速度；定义ν表示水下机器人的速度向量，ν＝[u,v,w,p,q,r]
t
；定义φ、θ、ψ分别表示水下机器人的横滚角、纵倾角、航向角；定义x、y、z分别表示水下机器人的北向位置、东向位置、垂向位置；定义η1表示水下机器人的位置向量，η1＝[x,y,z]
t
；定义η2表示水下机器人的姿态角向量，η2＝[φ,θ,ψ]
t
；定义η表示水下机器人的位姿向量，定义ν1表示水下机器人的线速度向量，ν1＝[u,v,w]
t
；定义ν2表示水下机器人的角速度向量，ν2＝[p,q,r]
t
；g(η)表示重力和浮力在载体坐标系的
合力和合力矩，它是位姿、重力、浮力的复合函数；
[0046]
定义分别表示水下机器人的水动力附加质量，是已知量；定义x
u|u|
,y
v|v|
,y
uvl
,z
w|w|
,z
uwl
,y
r|r|
|r|,y
url
,z
q|q|
,z
uql
,z
q|q|
,z
uql
,m
w|w|
,m
uwl
,n
v|v|
,n
uvl
,k
p|p|
,m
q|q|
,m
uql
,n
r|r|
,n
url
分别表示水下机器人的粘性水动力系数，是待求解变量；定义ω表示水下机器人待求解的粘性水动力系数集合，它是待求解变量的集合,它是一个列向量。
[0047]
定义ib表示水下机器人的转动惯量，其中ib＝dlg(i
x
,iy,iz)，i
x
、iy、iz分别表示水下机器人相对于x轴、y轴、z轴的转动惯量，它们是已知量；定义m表示水下机器人的质量，是已知量；定义w和b分别表示水下机器人的重力和浮力，是已知量；定义表示水下机器人的重心，其中xg、yg、zg分别表示水下机器人重心在x轴、y轴、z轴的位置，是已知量；定义表示水下机器人的浮心，其中xb、yb、zb分别表示水下机器人浮心在x轴、y轴、z轴的位置，是已知量；定义w和b分别表示水下机器人的重力和浮力，是已知量；定义m表示水下机器人的质量，它是已知量；定义表示姿态角的旋转矩阵，是已知量；定义τ表示推进器输出的控制力和力矩，r(τ)表示控制力和控制力矩的误差方差，它们是已知量。
[0048]
为了方便描述矩阵和向量的运算，定义t表示矩阵或向量的转置算子，定义dlg(
·
)表示对角矩阵算子，定义p(
·
)表示求解括号内变量的误差协方差，定义c(
·
)表示计算余弦函数算子，定义s(
·
)表示计算正弦函数算子，定义s(
·
)表示计算向量的反对称矩阵算子；定义[a；b]表示任意矩阵a和任意矩阵b的按行合并矩阵；定义r(
·
)表示计算括号内变量的均方误差；定义表示计算任意变量u导数的函数；定义表示计算括号内变量的微分；定义im表示m*m的单位矩阵，它是已知常数矩阵；定义0
m*n
表示m*n的零矩阵，它是已知常数矩阵。
[0049]
设定多普勒实时测量变结构机器人的前向速度、右向速度、垂向速度，它们是已知测量数据，且测量误差服从相同的正态分布，即多普勒测量速度的误差均方差是设定光纤罗经实时测量变结构机器人的横滚角、纵倾角、航向角、横滚角速度、纵倾角速度、转艏角速度，它们是已知测量数据，且横滚角、纵倾角、航向角测量误差服从相同的正态分布，横滚角速度、纵倾角速度、转艏角速度服从相同的正态分布，即光纤罗经测量姿态角的误差均方差是光纤罗经测量角速度的误差均方差是
[0050]
第一步，计算水下机器人在当前时刻的附加惯性质量矩阵
[0051]
根据水下机器人在时刻tk的重心、转动惯量、水动力附加质量，计算水下机器人的附加惯性质量矩阵，定义m表示水下机器人的附加惯性质量矩阵，它的计算方法如下：
[0052][0053]
其中m是待求解变量，m
11
,m
12
,m
21
,m
22
是m的子块矩阵，是中间变量；ν和ω分别表示速度和粘性水动力系数集合，它们是中间变量；ν

表示水下机器人的速度估计，它是上一个时刻t
k-1
的计算结果，它是本步骤的已知量；ω

表示水下机器人的粘性水动力系数集合估计，它是上一个时刻t
k-1
的计算结果，它是本步骤的已知量；p

表示水下机器人速度和粘性水动力系数的估计精度，它是上一个时刻t
k-1
的计算结果，它是本步骤的已知量；是p

的子块矩阵，是已知量。
[0054]
第二步，计算水下机器人重力、浮力、向心力在当前时刻的静力学矩阵
[0055]
根据水下机器人在时刻t
k-1
的重力、浮力、转动惯量、水动力附加质量，预测在时刻tk水下机器人的重力、浮力、向心力的静力学矩阵它的计算方法如下：
[0056][0057]
其中是待求解变量，g(η)表示重力和浮力在载体坐标系的合力和合力矩,它是中间变量；c(ν)是向心力和向心力矩的系数矩阵，它是中间变量；c
11
,c
12
,c
21
,c
22
是c(ν)的子块矩阵，是中间变量；η是水下机器人的位置和状态，它是光纤罗经和多普勒测量值的复合计算结果，是已知量；ν是上一步计算结果，是已知量；ν1、ν2、u、v、w、p、q、r是ν的子元素，已知量。
[0058]
第三步，预测水下机器人在当前时刻的水动力的阻力/阻力距向量
[0059]
根据水下机器人在时刻t
k-1
的粘性水动力系数，预测水下机器人在时刻tk的水动力的阻力/阻力矩向量它的计算方法如下：
[0060][0061]
其中是待求解变量；d(ν)是粘性水动力系数矩阵，它是中间变量；d
11
,d
12
,d
21
,d
22
是d(ν)的子块矩阵，是中间变量；ν是上一步计算结果，是已知量；ν1、ν2、u、v、w、p、q、r是ν的子元素，它们是已知量；ω是粘性水动力系数集合，它是上一步的计算结果，是已知中间变量；x
u|u|
,y
v|v|
,y
uvl
,z
w|w|
,z
uwl
,y
r|r|
|r|,y
url
,z
q|q|
,z
uql
,z
q|q|
,z
uql
,m
w|w|
,m
uwl
,n
v|v|
,n
uvl
,k
p|p|
,m
q|q|
,m
uql
,n
r|r|
,n
url
是ω的子元素，它是上一时刻t
k-1
的计算结果，是本步骤的已知量；ν是上一步计算结果，是已知量。
[0062]
第四步，预测水下机器人当前时刻的粘性水动力系数先验和速度先验
[0063]
基于机器人的动力学模型，根据水下机器人在时刻t
k-1
的粘性水动力系数ω、速度ν、控制输入量τ，预测水下机器人在时刻tk的粘性水动力系数先验ω-和速度先验ν-，它们的计算方法如下：
[0064][0065]
其中ω-和ν-是待求解变量；x-是ω-和ν-的按行合并矩阵，它是中间变量。
[0066]
第五步，计算水下机器人从上一时刻到当前时刻的状态迁移矩阵
[0067]
根据水下机器人在时刻t
k-1
的粘性水动力系数矩阵、向心力和向心力矩的系数矩阵，计算水下机器人从时刻t
k-1
到时刻tk的状态迁移矩阵f,计算方法如下：
[0068]
[0069]
其中f表示水下机器人从时刻t
k-1
到时刻tk的状态迁移矩阵，是待求解变量；f
11
和f
12
是f的子块矩阵，是中间变量；m、c(ν)、d(ν)分别表示水下机器人的附加惯性质量矩阵、向心力/向心力矩系数矩阵、粘性水动力系数矩阵，它们是上一步的计算结果，是已知量；δt表示相邻时刻的时间间隔，是已知常量。
[0070]
第六步，预测水下机器人当前时刻粘性水动力系数和速度的先验精度
[0071]
根据水下机器人在时刻t
k-1
的粘性水动力系数矩阵、向心力和向心力矩的系数矩阵速度精度、控制输入量精度，同时引入人工过程驱动噪声，设计粘性水动力系数和速度的复合先验精度描述速度误差和参数误差的数据关联，计算水下机器人在时刻tk的粘性水动力系数先验精度p(ω-)、速度先验精度p(ν-)、粘性水动力系数和速度的复合先验精度p-，它们的计算方法如下：
[0072][0073]
其中p-、p(ω-)和p(ν-)是待求解变量，是p-的子块矩阵，是待求解变量；p(ν

)是上一时刻t
k-1
速度后验估计的精度，它是上一时刻的计算结果，是本步骤已知量；p(ω

)是上一时刻t
k-1
粘性水动力系数后验估计的精度，它是上一时刻的计算结果，是已知量；m表示水下机器人的附加惯性质量矩阵，它是上一步的计算结果，是已知量；c(ν)和d(ν)分别表示水下机器人的向心力/向心力矩的系数矩阵和粘性水动力系数矩阵，它们是上一步的计算结果，是已知量；r(τ)表示控制力/控制力矩的误差方差，是已知量；r(ν)表示机器人的速度(含角速度)的误差方差矩阵，是中间变量；q(ω)表示粘性水动力系数估计的过程驱动噪声矩阵，是用户设定变量，是已知量；和分别是多普勒和光纤罗经的设备参数，是已知量。
[0074]
第七步，基于导航传感器量测，更新水动力系数和速度的估计及精度
[0075]
根据导航传感器量测与动力学模型速度先验的误差反馈校正水下机器人的水动力参数估计和速度估计，并计算水动力参数估计精度。即根据水下机器人在时刻tk的粘性水动力系数的先验、速度的先验及二者复合先验精度，计算水下机器人在时刻tk的粘性水
动力系数估计ω

及其精度p(ω

)、速度估计v

及其精度p(ν

)，它们的计算方法如下：
[0076][0077]
其中ω

、p(ω

)、v

、p(ν

)、p

是待求解变量，其中p

水下机器人在时刻tk的粘性水动力系数和速度的复合精度估计，是p

的子块矩阵，是待求解变量。ω-和ν-分别表示水下机器人在时刻tk的粘性水动力系数先验和速度先验，它是上一步计算结果，是本步骤的已知量；v是水下机器人的线速度和角速度，它是光纤罗经、多普勒提供的测量值，它是已知量测；r(ν)表示机器人的速度(含角速度)的误差方差矩阵，它是多普勒、光纤罗经的设备参数，是已知变量；k表示在时刻tk的校正增益，k
11
,k
21
是k的子块矩阵，是中间变量；p-表示在时刻tk粘性水动力系数和速度的复合先验精度，是上一步计算结果，是本步骤的已知量，是p-的子块矩阵，是本步骤的已知量。
[0078]
本发明是周期循环迭代调用步骤一到步骤七，本周期的输入是上一个周期的计算结果，本周期的输出是下一个周期的输入，获得从时刻t1到时刻tk水下机器人粘性水动力系数的在线估计，然后根据水动力参数、目标速度、目标姿态角预测机器人的阻力和阻力矩，采用通用标准控制方法提前分配推进器控制量，实现水下机器人的运动控制。