欠驱动水面机器人的水动力系数混合自适应估计方法

1.本发明涉及一种水面机器人测试估计方法，尤其涉及一种欠驱动水面机器人的水动力系数估计方法。


背景技术：

2.随着世界经济的飞速发展和人口的不断增加，人类消耗的自然资源越来越多，陆地上的资源正逐渐减少。海洋不仅仅作为一种交通运输通道存在，更重要的是它能从一定程度上缓解人类的资源危机。海洋里蕴藏着丰富的矿产资源，海洋的探测也具有极强的吸引力、挑战性。随着海洋探测的日益增多，欠驱动水面机器人被越来越广泛应用于海洋的探索、监测及救援中。欠驱动水面机器人水动力参数的获取成为了其实现水下目标跟踪、轨迹跟随控制的基础与关键。
3.然而，欠驱动水面机器人的形状一般不规则，故传统的水动力系数经验公式大多难以适用。而在基于cfd的水动力系数获取方法中，程序的编写和网格的划分很大程度上依赖于经验与技巧，故该方法的精度有待提高。约束船模实验法需要小型水面救援机器人设置有相应的测试安装机构，且对测试仪器有更高的要求，因此需要的经费投入更高。支持向量机、神经网络等方法前期需要大量的实船实验以获取数据进行训练，需要较长的实验时间和计算时间。最小二乘法受参数初始给定值影响较大，需要得到准确的角加速度数值，且不能保证速度估计误差收敛。
4.传统的参数自适应法没有考虑存在外界干扰和测量噪声的情况，现有实验所得到的水动力导数难以达到真实值附近的精度，并不准确。


技术实现要素：

5.本发明要解决的技术问题设计一种针对欠驱动水面机器人的水动力导数估计方式，该方法考虑实验过程中外界干扰和测量噪声，且不需要得到角加速度的具体信息，最终得到的水动力导数接近真实值且广义速度的预测误差可以收敛到0，达到准确的精度，避免了对速度求导所造成的加速度信息精度低的问题。
6.本发明采用的技术方案如下：
7.(1)获取水面机器人的物理参数，根据水面机器人的物理参数建立水面机器人的线性动力学模型；
8.(2)针对水面机器人，建立水域流场的网格划分模型，且划分为纯纵横荡结构网格和纯艏摇结构网格；
9.(3)将步骤(2)中划分的纯纵横荡结构网格和纯艏摇结构网格进行纯纵荡、纯横荡和纯艏摇的仿真，仿真时获得横荡力曲线和艏摇力矩曲线，利用横荡力曲线和艏摇力矩曲线输入到步骤(1)的线性动力学模型中拟合得到x
u
、y
v
、y
r
、n
v
、n
r
、的10个水动力系数；
10.由10个水动力系数组建水动力系数向量θ中，水动力系数向量θ中有10个元素，10
个元素依次为θ1、θ2、θ3、θ4、θ5、θ6、θ7、θ8、θ9和θ
10
，其中，，其中，θ6＝x
u
，θ7＝y
v
，θ8＝y
r
，θ9＝n
v
，θ
10
＝n
r
；
11.以水动力系数向量θ作为步骤(5)迭代过程中的水动力系数估计向量的初始值，同时设置水动力系数向量θ中的每个元素的自适应范围作为步骤(5)迭代过程中参数自适应率的迭代约束范围；
12.(4)通过水池试验获得水面机器人在给定推力下的各个时刻的位姿信息；
13.(5)根据步骤(4)中水池试验得到的位姿信息处理获得速度向量的估计值向量，进而建立辅助误差向量和辅助向量模型计算辅助误差向量κ，再建立参数自适应率公式获得水面机器人的水动力系数的估计值向量
14.(6)重复步骤(5)，以上一次迭代获得的水面机器人的水动力系数的估计值向量和水池试验获得下一个时刻的位姿信息代入到步骤(5)的速度向量的估计值向量的公式中进行迭代循环，直至步骤(4)获得水池试验获得各个时刻的位姿信息均迭代循环处理过，完成水动力系数混合自适应估计。
15.所述(1)中的线性动力学模型具体为：
[0016][0017]
式中，v表示水面机器人的速度向量；表示水面机器人的速度向量v的导数；δu表示测量水动力系数过程中附体坐标系下水面机器人受到的外界干扰力；m是质量矩阵，h(v)是柯氏力矩阵，d是阻尼矩阵，u
prop
是推进器提供的推力矩阵；
[0018]
上述v、m、h(v)、d、和u
prop
分别计算为：
[0019]
v＝[u v r]
t
[0020][0021]
h(v)＝[mvr
‑
mur 0]
t
[0022][0023]
u
prop
＝[f
pl
+f
pr 0(f
pl
‑
f
pr
)l]
t
[0024]
δu＝[δx δy δn]
t
[0025]
式中，u表示水面机器人的纵荡速度；v表示水面机器人的横荡速度；r表示水面机器人的艏摇角速度；m代表水面机器人的质量；i
z
代表水面机器人关于z轴的转动惯量；l代表水面机器人中两个推进器之间的距离的一半；f
pl
表示左推进器的推进力；f
pr
代表右推进器的推进力；x
u
表示纵荡方向水动力关于纵荡速度的线性水动力导数；y
v
表示横荡方向水动力关于横荡速度的线性水动力导数；y
r
表示横荡方向水动力关于艏摇角速度的线性水动力导数；n
v
表示水动力转艏力矩关于横荡速度的线性水动力导数；n
r
表示水动力转艏力矩关于艏摇角速度的线性水动力导数；表示纵荡方向水动力关于纵荡加速度的线性水动力导数；表示横荡方向水动力关于横荡加速度的线性水动力导数；表示横荡方向水动力关于
艏摇角加速度的线性水动力导数；表示水动力转艏力矩关于横荡加速度的线性水动力导数；表示水动力转艏力矩关于艏摇角加速度的线性水动力导数；δx表示测量过程中水面机器人受到的纵荡方向干扰力；δy表示测量过程中水面机器人受到的横荡方向干扰力；δn表示测量过程中水面机器人受到的艏摇方向干扰力矩。
[0026]
所述步骤(2)中，先建立水域流场三维结构模型，然后由水域流场三维结构模型进行网格划分后获得网格划分模型，对网格划分模型进行纯纵荡、纯横荡和纯艏摇结构网格的划分获得了纯纵横荡结构网格和纯艏摇结构网格。
[0027]
所述步骤(4)具体为：在水面上布置水面机器人和摄像系统，将给定水面机器人的推力施加到水面机器人的推进器上，利用摄像系统实时采集获得水面机器人单一时刻下的横荡速度测量值u
*
、纵荡速度测量值v
*
和艏摇角速度测量值r
*
，构成了该时刻的位姿信息，v
*
表示机器人的速度测量值向量；其中v
*
＝[u
* v
* r
*
]
t
。
[0028]
所述步骤(5)具体为：
[0029]
(5.1)根据步骤(4)中水池试验得到当前时刻的位姿信息，利用当前的水动力系数估计向量通过以下公式计算得到速度向量的估计值向量的导数
[0030][0031][0032][0033][0034][0035]
式中，表示水面机器人的速度向量v的估计值向量的导数；表示质量矩阵m的估计值；表示阻尼矩阵d的估计值；λ表示设定的正定矩阵；u
s
表示补偿矩阵；表示质量矩阵m的估计值的逆矩阵；u
prop
表示给定水面机器人的推力施加到水面机器人的推进器上而形成的推进器提供的推力矩阵；分别表示水面机器人的纵荡速度、横荡速度、艏摇角速度的估计值的导数；分别为x
u
、y
v
、y
r
、n
v
、n
r
的导数。
[0036]
(5.2)由水面机器人的速度向量v的估计值向量的导数处理获得水面机器人的速度向量v的估计值向量度向量v的估计值向量对速度向量估计值向量和步骤(4)试验得到的测量值向量v
*
进行对比，具体是建立以下公式的辅助误差向量和辅助向量模型计算辅助误差向量κ：
[0037]
[0038][0039]
式中，δu表示纵荡速度误差；δv表示横荡速度误差；δr表示艏摇角速度误差；
[0040][0041][0042][0043]
在获得辅助误差向量后，建立以下参数自适应率公式，根据辅助误差向量κ在步骤(3)设定的参数自适应率的迭代约束范围内对参数自适应率公式进行求解获得水面机器人的水动力系数的估计值向量的导数，进而处理获得水面机器人的水动力系数的估计值向量
[0044][0045]
式中，表示水面机器人的水动力系数的估计值向量的导数，γ为正定矩阵，表示非连续映射函数；
[0046]
非连续映射函数表示为：
[0047][0048][0049]
式中，(γκ)
i
表示γκ矩阵的第i个元素，t表示矩阵转置；表示的第i个元素，θ
imax
θ
imin
分别表示中元素的最大值和最小值。
[0050]
相较于现有其他方法，本发明所构建的水动力系数自适应率，利用非连续映射，可以保证水动力系数的估计值严格保持在最小值和最大值之间，而不会因为测量数据中外界干扰的影响而变为无穷大的数据，有助于提高水动力系数估计系统的对外界干扰的抗干扰(测量噪声、外界干扰力等)能力。
[0051]
除此之外，本发明的自适应率针对每个水动力系数自身进行了参数估计，而不是像传统方案一样针对某几个水动力系数的和或者商进行参数估计，因此计算过程更加明确易行。
[0052]
本发明方法特别地增加了补偿矩阵u
s
，在增加补偿矩阵之后，使所设计的算法，在使用存在外界干扰和测量误差工况下获取的试验数据时，得到的水动力系数可以更好地进行精准的速度向量预测。
[0053]
所述的补偿矩阵u
s
设置为：
[0054]
根据外界干扰力的阈值和传感器噪声的阈值，设定补偿矩阵：
[0055][0056]
式中，ε1、ε2和ε3均代表预先设置的第一、第二、第三富余量参数，ε1,ε2,ε3＞0，m1,
m2,m3是质量矩阵m的三个列向量，|| ||2表示二范数，|| ||
2，max
表示二范数的最大值，sgn()表示符号函数，| |
max
表示行列式的最大值；δx表示测量过程中水面机器人受到的纵荡方向干扰力；δy表示测量过程中水面机器人受到的横荡方向干扰力；δn表示测量过程中水面机器人受到的艏摇方向干扰力矩，δv
m
表示测量误差矩阵。
[0057]
方法采用以下装置，包括摄像系统、水面机器人、陀螺仪、左推进器、右推进器、控制器、sd卡和上位机，陀螺仪、左推进器、右推进器、控制器和sd卡均位于水面机器人内部，陀螺仪、左推进器、右推进器和sd卡均与控制器电连接；摄像系统与上位机位于水面机器人外的附近，摄像系统与上位机通信连接，摄像系统中的双摄像头朝向水面机器人进行拍摄。
[0058]
本发明中，加粗的字母为向量。
[0059]
本发明的有益效果是：
[0060]
1.利用cfd技术初步得到了水动力的范围，迭代结果更逼近真实值；
[0061]
2.在设计参数估计器时充分考虑了测量噪声及外界干扰的影响，更加符合测量实际。
[0062]
3.不需要单独获得角加速度信息，避免了对角速度求导所造成的传感器信息精度的下降。
[0063]
因此，本发明可广泛应用于欠驱动水面机器人的水动力系数估计工作中。
附图说明
[0064]
图1是本发明的结构示意图。
[0065]
图2是混合自适应参数估计的流程图。
[0066]
图3是混合自适应参数估计的控制框图。
[0067]
图中：1
‑
摄像系统，2
‑
水面机器人，3
‑
陀螺仪，4
‑
左推进器，5
‑
右推进器，6
‑
控制器，7
‑
sd卡，8
‑
上位机。
具体实施方式
[0068]
本发明要解决的技术问题设计一种针对欠驱动水面机器人的水动力导数估计方式，该方法考虑实验过程中外界干扰和测量噪声，且不需要得到角加速度的具体信息，最终得到的水动力导数接近真实值且广义速度的预测误差可以收敛到0。
[0069]
方法采用的装置如图1所示，方法采用以下装置，包括摄像系统1、水面机器人2、陀螺仪3、左推进器4、右推进器5、控制器6、sd卡7和上位机8，陀螺仪3、左推进器4、右推进器5、控制器6和sd卡7均位于水面机器人2内部，陀螺仪3、左推进器4、右推进器5和sd卡7均与控制器6电连接；摄像系统1与上位机8位于水面机器人2外的附近，摄像系统1与上位机8通信连接，摄像系统1中的双摄像头朝向水面机器人2进行拍摄，摄像系统1用于对水面机器人2进行定位，通过拍摄的图像进行处理分析获得水面机器人2的定位结果。
[0070]
陀螺仪3用于记录水面机器人2的角速度；
[0071]
左推进器4、右推进器5构成了推进器，推进器用于为水面机器人2提供前进和转向的动力。
[0072]
控制器6用于对数据进行采集、处理和记录。
[0073]
sd卡7用于记录陀螺仪3的数据。
[0074]
上位机8用于记录摄像系统1中的双摄像头的数据，并进行图像分析处理。
[0075]
本发明欠驱动水面机器人是指水面机器人的动力只有前后、旋转，不具有左右驱动力，如图1所示的例子，本发明获得的水动力系数通常用于欠驱动水面机器人的运动控制器的设计。
[0076]
如图2和图3所示，本发明的具体实施情况如下：
[0077]
(1)获取水面机器人的物理参数，物理参数包括水面机器人的质量、转动惯量和两个推进器之间的距离，根据水面机器人的物理参数建立水面机器人的线性动力学模型，线性动力学模型为步骤(6)
‑
步骤(7)进行水动力参数估计提供模型基础；
[0078]
(1)中的线性动力学模型具体为：
[0079][0080]
式中，v表示水面机器人的速度向量；表示水面机器人的速度向量v的导数；δu表示测量水动力系数过程中附体坐标系下水面机器人受到的外界干扰力；m是质量矩阵，h(v)是柯氏力矩阵，d是阻尼矩阵，u
prop
是推进器提供的推力矩阵；
[0081]
附体坐标系是指以水面机器人重心为原点，水面机器人的前后方向为x轴，以水面机器人的左右方向为y轴，以水面机器人的上下方向为z轴。
[0082]
v、m、h(v)、d、和u
prop
分别计算为：
[0083]
v＝[u v r]
t
[0084][0085]
h(v)＝[mvr
ꢀ‑
mur 0]
t
[0086][0087]
u
prop
＝[f
pl
+f
pr 0(f
pl
‑
f
pr
)l]
t
[0088]
δu＝[δx δy δn]
t
[0089]
式中，u表示水面机器人的纵荡速度；v表示水面机器人的横荡速度；r表示水面机器人的艏摇角速度；m代表水面机器人的质量；i
z
代表水面机器人关于z轴的转动惯量；z轴为以水面机器人重心为起点，垂直于水面机器人的自身水平面的一条坐标轴；l代表水面机器人中两个推进器之间的距离的一半；f
pl
表示左推进器的推进力；f
pr
代表右推进器的推进力；x
u
表示纵荡方向水动力关于纵荡速度的线性水动力导数；y
v
表示横荡方向水动力关于横荡速度的线性水动力导数；y
r
表示横荡方向水动力关于艏摇角速度的线性水动力导数；n
v
表示水动力转艏力矩关于横荡速度的线性水动力导数；n
r
表示水动力转艏力矩关于艏摇角速度的线性水动力导数；表示纵荡方向水动力关于纵荡加速度的线性水动力导数；表示横荡方向水动力关于横荡加速度的线性水动力导数；表示横荡方向水动力关于艏摇角加速度的线性水动力导数；表示水动力转艏力矩关于横荡加速度的线性水动力导数；表示水动力转艏力矩关于艏摇角加速度的线性水动力导数；δx表示测量过程中水面机器人受到的纵荡方向干扰力；δy表示测量过程中水面机器人受到的横荡方向干扰力；δn表示
测量过程中水面机器人受到的艏摇方向干扰力矩。
[0090]
(2)针对水面机器人，建立水域流场的网格划分模型，且划分为纯纵横荡结构网格和纯艏摇结构网格，为步骤(3)中进行流体仿真提供网格；
[0091]
步骤(2)中，先建立水域流场三维结构模型，然后由水域流场三维结构模型进行网格划分后获得网格划分模型，在处理软件icem中对网格划分模型进行纯纵荡、纯横荡和纯艏摇结构网格的划分获得了纯纵横荡结构网格和纯艏摇结构网格。
[0092]
水域流场三维结构模型是指水面机器人所在水域流场的三维模型，水域流场三维结构模型包括了纯纵荡、纯横荡和纯艏摇结构网格。水域流场三维结构模型满足以下条件：
[0093]
a.水面机器人的头部与水域流场入口之间的距离等于水面机器人的长度l；
[0094]
b.水面机器人的尾部与水域流场出口之间的距离为2l；
[0095]
c.水域流场的宽度为3l；
[0096]
d.水域流场的深度为8.3h，其中h为水面机器人在水域流场中的吃水深度。
[0097]
在icem中进行纯纵荡、纯横荡和纯艏摇结构网格的划分。
[0098]
(3)将步骤(2)中划分的纯纵横荡结构网格和纯艏摇结构网格在流体计算软件fluent中进行纯纵荡、纯横荡和纯艏摇的仿真，仿真时获得横荡力曲线和艏摇力矩曲线，利用横荡力曲线和艏摇力矩曲线输入到步骤(1)的线性动力学模型中拟合得到x
u
、y
v
、y
r
、n
v
、n
r
、的10个水动力系数；
[0099]
由10个水动力系数组建水动力系数向量θ中，水动力系数向量θ中有10个元素，10个元素依次为θ1、θ2、θ3、θ4、θ5、θ6、θ7、θ8、θ9和θ
10
，其中，，其中，θ6＝x
u
，θ7＝y
v
，θ8＝y
r
，θ9＝n
v
，θ
10
＝n
r
；
[0100]
以水动力系数向量θ作为步骤(5)迭代过程中的水动力系数估计向量的初始值，同时设置水动力系数向量θ中的每个元素的自适应范围作为步骤(5)迭代过程中参数自适应率的迭代约束范围，即自适应范围的上下限θ
min
,θ
max
；
[0101]
(4)通过水池试验获得水面机器人在给定推力下的各个时刻的位姿信息，即为试验数据，为后续进行步骤(5)水动力参数的自适应调整提供试验数据；
[0102]
步骤(4)具体为：在平静的水面上布置水面机器人和摄像系统，摄像系统用以确定水面机器人位置，将给定水面机器人的推力施加到水面机器人的推进器上，利用摄像系统实时采集获得水面机器人单一时刻下的横荡速度测量值u
*
、纵荡速度测量值v
*
和艏摇角速度测量值r
*
，构成了该时刻的位姿信息，v
*
表示机器人的速度测量值向量；其中v
*
＝[u
* v
* r
*
]
t
。
[0103]
具体实施采用正弦推力，正弦推力是指推力按照正弦变化。
[0104]
(5)根据步骤(4)中水池试验得到的位姿信息处理获得速度向量的估计值向量，进而建立辅助误差向量和辅助向量模型计算辅助误差向量k，再建立参数自适应率公式获得水面机器人的水动力系数的估计值向量
[0105]
(5.1)利用步骤(4)得到的试验数据，进行10个水动力参数的估计。根据步骤(4)中水池试验得到当前时刻的位姿信息，即横荡速度测量值u
*
、纵荡速度测量值v
*
和艏摇角速度测量值r
*
，利用当前的水动力系数估计向量通过以下公式计算得到速度向量的估计值向量的导数
[0106][0107][0108][0109][0110][0111]
补偿矩阵u
s
根据水池试验时外界干扰力阈值以及水池试验时池边布置的双摄像头的测量噪声阈值设置为：
[0112]
根据外界干扰力的阈值和传感器噪声的阈值，设定补偿矩阵：
[0113][0114][0115][0116][0117]
(5.2)由水面机器人的速度向量v的估计值向量的导数处理获得水面机器人的速度向量v的估计值向量度向量v的估计值向量对速度向量估计值向量和步骤(4)试验得到的测量值向量v
*
进行对比，具体是建立以下公式的辅助误差向量和辅助向量模型计算辅助误差向量k：
[0118][0119]
κ＝[κ
1 κ
2 κ
3 κ
4 κ
5 κ
6 κ
7 κ
8 κ
9 κ
10
]
t
[0120]
式中式中且则满足：式中，表示参数误差向量，表示速度估计误差，t表示矩阵转置，δm表示质量矩阵估计误差，δd表示阻尼矩阵估计误差。表示代表了的意思。
[0121]
在获得辅助误差向量后，建立以下参数自适应率公式，根据辅助误差向量k在步骤(3)设定的参数自适应率的迭代约束范围内对参数自适应率公式进行求解获得水面机器人的水动力系数的估计值向量的导数，进而处理获得水面机器人的水动力系数的估计值向量利用参数自适应率对步骤(3)给定的每个元素的自适应范围内进行反馈以实现水动力系数的自适应修正：
[0122]
[0123][0124]
式中，表示水面机器人的水动力系数的估计值向量的导数，κ表示辅助误差向量，γ为正定矩阵，表示非连续映射函数；
[0125]
非连续映射函数表示为：
[0126][0127][0128]
则该映射满足：
[0129][0130][0131]
(6)重复步骤(5)，以上一次迭代获得的水面机器人的水动力系数的估计值向量中的各个估计值和水池试验获得下一个时刻的位姿信息代入到步骤(5)的速度向量的估计值向量的公式中进行迭代循环，直至步骤(4)获得水池试验获得各个时刻的位姿信息均迭代循环处理过，则完成全部的参数估计过程，完成水动力系数混合自适应估计。
[0132]
以上的实例并非对本发明作任何形式上的限定，任何熟悉本专业的技术人员可以利用上述揭示的技术内容加以变更或修饰为等同变化的等效实例，但是凡是未脱离本发明技术方案内容，依据本发明的技术实质对以上实例所作的任何的简单修改、等同变化与修饰，均仍属于本发明技术方案的范围内。