CN113485390A - 一种抗洋流扰动的自主水下机器人动力定位方法和系统 - Google Patents

一种抗洋流扰动的自主水下机器人动力定位方法和系统 Download PDF

Info

Publication number
CN113485390A
CN113485390A CN202110987332.3A CN202110987332A CN113485390A CN 113485390 A CN113485390 A CN 113485390A CN 202110987332 A CN202110987332 A CN 202110987332A CN 113485390 A CN113485390 A CN 113485390A
Authority
CN
China
Prior art keywords
ocean current
time
underwater robot
autonomous underwater
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Granted
Application number
CN202110987332.3A
Other languages
English (en)
Other versions
CN113485390B (zh
Inventor
边有钢
陈昱衡
徐彪
崔庆佳
胡满江
秦兆博
秦洪懋
秦晓辉
谢国涛
王晓伟
丁荣军
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Hunan University
Original Assignee
Hunan University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Hunan University filed Critical Hunan University
Priority to CN202110987332.3A priority Critical patent/CN113485390B/zh
Publication of CN113485390A publication Critical patent/CN113485390A/zh
Application granted granted Critical
Publication of CN113485390B publication Critical patent/CN113485390B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/04—Control of altitude or depth
    • G05D1/06—Rate of change of altitude or depth
    • G05D1/0692—Rate of change of altitude or depth specially adapted for under-water vehicles

Abstract

本发明公开了一种抗洋流扰动的自主水下机器人动力定位方法和系统，该方法包括：步骤1，采集自主水下机器人的测量状态量；步骤2，根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；步骤3，根据所述测量状态量，获取洋流速度估计值；步骤4，根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量；步骤5，根据所述滑膜控制量和期望控制量，获取实际控制量。

Description

一种抗洋流扰动的自主水下机器人动力定位方法和系统
技术领域
本发明涉及自主水下机器人控制技术领域，特别是关于一种抗洋流扰动的自主水下机器人动力定位方法和系统。
背景技术
自主水下机器人(autonomous underwater vehicle，AUV)需要在复杂的海洋环境中通过动力定位控制进行定点作业，以完成海洋资源勘测、开发等方面的任务。然而，恶劣海洋环境下动态洋流扰动、强风、海浪等不确定因素作用下自主式水下机器人的受力会发生变化，其动力定位控制系统也面临着控制模型参数摄动问题，对自主式水下机器人定点悬停作业过程中的稳定性造成了较大的影响。
现有的自主水下机器人动力定位控制方法存在明显的不足：1)在控制算法的设计上没有有效考虑执行器的饱和约束，会影响控制算法的控制性能；2)没有有效消除洋流扰动和动力学被控模型不匹配等因素所带来不利影响的方法，动力定位方法的鲁棒性较差。
发明内容
本发明的目的在于提供一种抗洋流扰动的自主水下机器人动力定位方法和系统来克服或至少减轻现有技术的上述缺陷中的至少一个。
为实现上述目的，本发明提供一种抗洋流扰动的自主水下机器人动力定位方法，该方法包括：
步骤1，采集自主水下机器人的测量状态量；
步骤2，根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；
步骤3，根据所述测量状态量，获取洋流速度估计值；
步骤4，根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量；
步骤5，根据所述滑膜控制量和期望控制量，获取实际控制量；
步骤6，返回步骤2，直至自主水下机器人的位姿调整至期望位姿。
进一步地，所述步骤1中，当前时刻k的所述测量状态量被描述为X(k)＝[η(k)；V(k-1)]，式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，V(k-1)表示k-1时刻的惯性坐标系下的速度测量值；
所述步骤3获取所述洋流速度估计值的方法具体包括：
通过式(7)描述的洋流观测器，计算所述洋流速度估计值：
Figure BDA0003231183650000021
式中，Vf(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，
Figure BDA0003231183650000022
表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，
Figure BDA0003231183650000023
表示
Figure BDA0003231183650000024
的导数，
Figure BDA0003231183650000025
表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，
Figure BDA0003231183650000026
表示
Figure BDA0003231183650000027
的导数，Kv和Kη均表示增益矩阵，J表示惯性坐标系到载体坐标系的坐标转换矩阵。
进一步地，所述步骤2中，所述运动学状态空间模型设置为预测时域Np和控制时域Nc下的式(10)：
Figure BDA0003231183650000028
所述优化问题被描述为式(12)：
Figure BDA0003231183650000029
式中：
Figure BDA00032311836500000210
Figure BDA00032311836500000211
Figure BDA00032311836500000212
表示k+1时刻在未来Np个预测步长内的预测状态量，x(k|k)表示k时刻预测的第一个状态量，U(k)表示k时刻在未来Nc个预测步长内的预测控制量，U*(k)表示U(k)的最优值，
Figure BDA0003231183650000031
表示k时刻状态量的系数矩阵，
Figure BDA0003231183650000032
表示k时刻控制量的系数矩阵，
Figure BDA0003231183650000033
表示权重矩阵，
Figure BDA0003231183650000034
表示权重矩阵，
Figure BDA0003231183650000035
表示期望状态量，
Figure BDA0003231183650000036
表示k时刻在未来Np个预测步长内的预测状态量。
进一步地，所述约束设置为s.t.：
u(i|k)≤Umax
-u(i|k)≤-Umin
Figure BDA0003231183650000037
Figure BDA0003231183650000038
式中，u(i|k)表示k时刻在未来第i时刻的预测控制量，Umax表示U(k)中的最大值，Umin表示U(k)中的最小值，V(i|k)表示k时刻在未来第i时刻的预测速度，C表示哥氏力离心矩阵，D表示流体阻尼矩阵，F表示静水恢复力，M表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，T为采样时间，τmax表示期望控制量中的五自由度力的最大值，τmin表示所述五自由度力的最小值。
进一步地，所述步骤2中，通过式(9)描述的控制量u(k)、以及式(13)，计算期望加速度
Figure BDA0003231183650000039
u(k)＝V(k)-V(k-1) (9)
Figure BDA00032311836500000310
通过式(14)，计算得到期望控制量uMPC(k)：
uMPC(k)＝Mu*(k)/T-(C+D)V(k)-F (14)
式中，u*(k)是U*(k)的第一个元素。
进一步地，所述步骤4中，所述积分滑模面被描述为s(V)，通过如下式(15)得到所述控制量uISMC(t)：
Figure BDA00032311836500000311
其中，V(t)表示t时刻的速度，h(V(τ))是状态反馈项。
进一步地，所述步骤4中，在载体坐标系下沿x轴方向的所述饱和函数XISMC(t)被描述为式(14)：
Figure BDA00032311836500000312
其中，ρX为海浪扰动力的上限，su(t)表示载体坐标系相对于惯性坐标系沿x轴方向速度的积分滑模面。
本发明实施例提供的抗洋流扰动的自主水下机器人动力定位系统，该系统包括：
测量单元，其用于采集自主水下机器人的测量状态量；
模型预测控制器，其用于根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；
洋流观测器，其用于根据所述测量状态量，获取洋流速度估计值；
滑膜控制器，其用于根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量；
计算单元，其用于根据所述滑膜控制量和期望控制量，获取实际控制量，并监控自主水下机器人的位姿调整至期望位姿。
进一步地，所述测量单元获得的当前时刻k的所述测量状态量信息被描述为X(k)＝[η(k)；V(k-1)]，式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，V(k-1)表示k-1时刻的惯性坐标系下的速度测量值；
所述洋流观测器获取所述洋流速度估计值的方法具体包括：
通过式(7)描述的洋流观测器，计算所述洋流速度估计值：
Figure BDA0003231183650000041
式中，Vf(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，
Figure BDA0003231183650000042
表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，
Figure BDA0003231183650000043
表示
Figure BDA0003231183650000044
的导数，
Figure BDA0003231183650000045
表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，
Figure BDA0003231183650000046
表示
Figure BDA0003231183650000047
的导数，Kv和Kη均表示增益矩阵，J表示惯性坐标系到载体坐标系的坐标转换矩阵。
进一步地，所述运动学状态空间模型设置为预测时域Np和控制时域Nc下的式(9)：
Figure BDA0003231183650000051
所述优化问题被描述为式(11)：
Figure BDA0003231183650000052
式中：
Figure BDA0003231183650000053
Figure BDA0003231183650000054
Figure BDA0003231183650000055
表示k+1时刻在未来Np个预测步长内的预测状态量，x(k|k)表示k时刻预测的第一个状态量，U(k)表示k时刻在未来Nc个预测步长内的控制量，U*(k)表示U(k)的最优值，
Figure BDA0003231183650000056
表示k时刻状态量的系数矩阵，
Figure BDA0003231183650000057
表示k时刻控制量的系数矩阵，
Figure BDA0003231183650000058
表示权重矩阵，
Figure BDA0003231183650000059
表示权重矩阵，
Figure BDA00032311836500000510
表示期望状态量，
Figure BDA00032311836500000511
表示k-1时刻在未来Np个预测步长内的预测状态量；
所述约束设置为s.t.：
u(i|k)≤Umax
-u(i|k)≤-Umin
Figure BDA00032311836500000512
Figure BDA00032311836500000513
式中，u(i|k)表示k时刻在未来i时刻的预测控制量，Umax表示U(k)中的最大值，Umin表示U(k)中的最小值，V(i|k)表示k时刻在未来i时刻的预测速度，C表示哥氏力离心矩阵，D表示流体阻尼矩阵，F表示静水恢复力，M表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，T为采样时间，τmax表示期望控制量中的五自由度力的最大值，τmin表示所述五自由度力的最小值。
本发明由于采取以上技术方案，其具有以下优点：
在复杂的海洋环境中，自主式水下机器人会受到不规则的洋流干扰力，为此设计了洋流观测器以通过估计实时的洋流速度，以计算洋流对自主式水下机器人的干扰力。而MPC可以通过设计约束考虑执行器的饱和特性。同时，为解决由于控制参考模型与自主式水下机器人实际动力学模型存在的不匹配问题和洋流估计产生的较小有界误差，本发明设计了ISMC以解决模型不匹配的问题，同时消除洋流估计产生的较小有界误差，以提高控制系统的鲁棒性。
附图说明
图1为本发明中的描述自主式水下机器人运动的两个参考坐标系。
图2是本发明中自主式水下机器人动力定位控制系统结构框图。
具体实施方式
下面结合附图和实施例对本发明进行详细的描述。
本实施例提供的抗洋流扰动的自主水下机器人动力定位方法包括：
步骤1，首先定义两个参考坐标系：惯性坐标系和载体坐标系。其中，惯性坐标系固定于地面。载体坐标系则固定于自主式水下机器人，随自主式水下机器人以任何形式移动。两个参考坐标系如图1所示，图中示意出的E-ξηζ表示固定于地面的惯性坐标系，O-xyz表示固定于自主式水下机器人的载体坐标系，O通常选定在自主式水下机器人的质心，但不限于此。
自主式水下机器人的位姿变量被描述为
Figure BDA0003231183650000061
其中，x，y，z为O在惯性坐标系下分别沿x轴、y轴、z轴的坐标，
Figure BDA0003231183650000062
θ，ψ为O分别绕惯性坐标系下的x轴、y轴、z轴的角度，即：纵倾角、航向角、横滚角，这些参数的具体数值可通过自主式水下机器人的导航系统反馈得到。
自主式水下机器人的速度变量被描述为V＝[u，v，w，p，q，r]T。其中，u，v，w为载体坐标系相对于惯性坐标系分别沿x轴、y轴、z轴的速度，p，q，r为载体坐标系相对于惯性坐标系分别围绕x轴、y轴、z轴的角速度。
惯性坐标系与载体坐标系之间的转换关系可由自主式水下机器人的运动学方程(1)反映。
ηT＝JVT (1)
其中：
Figure BDA0003231183650000063
Figure BDA0003231183650000071
Figure BDA0003231183650000072
j11＝cosψcosθ
j12＝-sinψcosθ+cosψsinθsinψ
j13＝sinψsinφ+cosψcosφsinθ
j21＝sinψcosφ
j22＝sinψsinθsinφ+cosψcosφ
j23＝-cosψsinψ+sinθsinψcosφ
j31＝-sinθ
j32＝cosψsinφ
j33＝cosψcosφ
动力学关系式描述为式(2)：
Figure BDA0003231183650000073
其中，M′表示自主式水下机器人惯性矩阵，被描述为式(3)；C表示自主式水下机器人的哥氏力离心矩阵，被描述为式(4)；D表示自主式水下机器人的流体阻尼矩阵，被描述为式(5)；F表示自主式水下机器人的静水恢复力，被描述为式(6)；τ＝[X；Y；Z；0；M；N]为自主式水下机器人的控制量，X、Y、Z表示控制量中的五自由度力控制量分别沿x轴、y轴和z轴的分量，M、N表示控制量中的力矩控制量分别沿y轴和z轴的分量。
Figure BDA0003231183650000074
式(3)中：
Figure BDA0003231183650000081
Figure BDA0003231183650000082
Figure BDA0003231183650000083
Figure BDA0003231183650000084
m表示自主式水下机器人的质量，
Figure BDA0003231183650000085
和
Figure BDA0003231183650000086
表示附加质量，Ixx表示自主式水下机器人绕x轴的转动惯量，Iyy表示自主式水下机器人绕y轴的转动惯量，Izz表示自主式水下机器人绕z轴的转动惯量，这些动力学特性参数可以通过力学试验获得。
Figure BDA0003231183650000087
式(4)中：
Figure BDA0003231183650000088
Figure BDA0003231183650000089
Figure BDA00032311836500000810
Figure BDA00032311836500000811
Yuv，Xwq，Xqq，Xvr，Xrr，Ywp，Ypq，Yur，Zvp，Zrp，Zup，Muw，Nuv，Mvp，Mrp，Muq，Nwp，Npq和Nur是交叉项附加质量，上述参数的具体数值可以通过力学试验得到。
Figure BDA0003231183650000091
式(5)中：
Figure BDA0003231183650000092
Figure BDA0003231183650000093
Figure BDA0003231183650000094
Figure BDA0003231183650000095
Xuu，Yvv，Zww，Yrr，Zqq，Mww，Nvv，Kpp，Mqq和Nrr表示横流阻力系数，上述参数的具体数值可以通过力学试验得到。
Figure BDA0003231183650000096
W表示自主式水下机器人的重量，B表示自主式水下机器人的浮力，zB表示自主式水下机器人的浮心在载体坐标系下对应z轴的坐标。
步骤2，洋流观测器用于估计当前时刻的洋流速度，以获得更精准的动力学模型，例如：洋流观测器可以描述为式(7)：
Figure BDA0003231183650000097
式中，Vf(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，Vf(k)＝[uf(k)，vf(k)，wf(k)，0，0，0]T，uf(k)，vf(k)，wf(k)分别为Vf(k)在载体坐标系下分别沿x轴、y轴和z轴的分量，
Figure BDA0003231183650000101
表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，
Figure BDA0003231183650000102
表示
Figure BDA0003231183650000103
的导数，
Figure BDA0003231183650000104
表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，
Figure BDA0003231183650000105
表示
Figure BDA0003231183650000106
的导数，Kv和Kη均表示增益矩阵。
本发明实施例通过构建洋流观测器估计洋流的速度以计算洋流扰动力，可实现精准动力学估计。
步骤3，基于自主式水下机器人的运动学模型构造，构建由式(8)描述的离散的状态空间模型，用于二次规划的优化求解：
X(k+1)＝A(k)X(k)+B(k)u(k) (8)
其中：
X(k)＝[η(k)；v(k-1)]
Figure BDA0003231183650000107
B(k)＝[J′(k)T；B′]
Figure BDA0003231183650000108
Figure BDA0003231183650000109
Figure BDA00032311836500001010
u(k)＝V(k)-V(k-1) (9)
X(k+1)表示k+1时刻的测量状态量，A(k)表示状态量的系数矩阵，X(k)表示k时刻的测量状态量，B(k)表示是控制量的系数矩阵，u(k)表示k时刻的控制量，V(k-1)表示k-1时刻的速度信息，T表示采样周期，J(k)表示坐标转换矩阵，J′(k)表示J(k)的逆矩阵，B′表示B的逆矩阵，J1表示线速度坐标转换矩阵，03×2表示3*2的零矩阵，03×3表示是三阶零矩阵，J′2表示角速度坐标转换矩阵。
定义预测时域和控制时域分别为Np和Nc，状态空间模型(8)可转换为式(10)：
Figure BDA0003231183650000111
其中：
Figure BDA0003231183650000112
Figure BDA0003231183650000113
Figure BDA0003231183650000114
Figure BDA0003231183650000115
Figure BDA0003231183650000116
表示k+！时刻在未来Np个预测步长内的预测状态量，
Figure BDA0003231183650000117
表示k时刻状态量的系数矩阵，
Figure BDA0003231183650000118
表示k时刻控制量的系数矩阵，U(k)表示k时刻在未来Nc个预测步长内的控制量，X(k|k)表示k时刻预测的第一个状态量，u(k|k)表示k时刻预测的第一个系统控制量，这些参数的具体数值可以通过二次规划算出来的。
通过求解一个标准的二次规划优化问题，其中的代价函数描述为式(11)所示：
J(k)＝0.5*U(k)H(k)U(k)+fT(k)U(k) (11)
其中：
Figure BDA0003231183650000119
Figure BDA0003231183650000121
Figure BDA0003231183650000122
Figure BDA0003231183650000123
Figure BDA0003231183650000124
Figure BDA0003231183650000125
表示期望状态量，
Figure BDA0003231183650000126
表示权重矩阵，
Figure BDA0003231183650000127
表示权重矩阵，这些参数的具体数值可以通过试验调试获得。
上述优化问题的受到的动力学约束如下：
u(k)≤umax
-u(k)≤-umin
Figure BDA0003231183650000128
Figure BDA0003231183650000129
其中，T为采样时间，umax表示控制量的最大值，umin表示控制量的最小值，τmax表示推力的最大值，τmin表示推力的最小值，这些参数可以通过预先设置获得。
用于二次规划的优化问题为：
Figure BDA00032311836500001210
u(i|k)≤Umax
-u(i|k)≤-Umin
Figure BDA00032311836500001211
Figure BDA00032311836500001212
其中，u*(k)＝u*(k|k)是控制序列U*(k)的第一个元素，u(i|k)表示k时刻预测i时刻的控制量，Umax表示控制序列中的最大值，Umin表示控制序列中的最小值，这些参数的具体数值可以通过预先设置获得。
由于控制量u(k)为当前时刻的速度增量，动力学方程中的加速度
Figure BDA0003231183650000131
可再由下式(11)近似计算：
Figure BDA0003231183650000132
T表示采样周期，通过调试选取最佳值。
故根据动力学方程(2)，可计算得到期望控制量uMPC(k)，其通过式(14)计算获得，其被描述为uMPC＝[XMPC，YMPC，ZMPC，0，MMPC，NMPC]T：
uMPC(k)＝Mu*(k)/T-(C+D)V(k)-F (14)
本发明实施例通过MPC的控制量约束解决执行器的饱和特性问题，通过MPC的滚动优化来提高控制性能。
步骤4，通过零阶保持器将离散的MPC控制量uMPC(k)转变为连续型信号后，通过如下式(15)，得到滑膜控制量uISMC(t)，其被描述为uISMC(t)＝[XISMC(t)，YISMC(t)，ZISMC(t)，0，MISMC(t)，NISMC(t)]T：
Figure BDA0003231183650000133
h(V(i))＝M-1((C+D)V(i)+F) (16)
其中，s(V)＝(su(t)，sv(t)，sw(t)，sp(t)，sq(t)，sr(t))T为积分滑模面，su(t)表示u(t)的积分滑模面，sv(t)表示v(t)的积分滑模面，sw(t)表示w(t)的积分滑模面，sp(t)表示p(t)的积分滑模面，sq(t)表示q(t)的积分滑模面，sr(t)表示r(t)的积分滑模面。文中，t表示连续信号，k表示离散信号。
通过引入饱和函数，来缓冲真实系统中抖振现象的不利影响，以XISMC(t)为例：
Figure BDA0003231183650000134
其中ρX为海浪扰动力的上限。当然，载体坐标系相对于惯性坐标系沿其它方向的速度和角速度的饱和函数与XISMC(t)相同，在此不再展开描述。
本发明实施例中的ISMC输出的控制量可以在不产生较大系统抖振的前提下减小模型不匹配和扰动误差的影响，以进一步改进控制算法的控制效果。
步骤5，根据期望控制量uMPC(k)与滑膜控制量uISMC(t)，通过式(17)，获得τ(k)＝[X(k)，Y(k)，Z(k)，0，M(k)，N(k)]T，其中，X(k)表示k时刻载体坐标系x轴方向期望的力，Y(k)表示k时刻载体坐标系y轴方向期望的力，Z(k)表示k时刻载体坐标系z轴方向期望的力，M(k)表示k时刻载体坐标系期望的绕Y轴的力矩，N(k)表示k时刻载体坐标系期望的绕y轴的力矩。
τ(k)＝uMPC(k)+uISMC(k) (17)
步骤6，返回步骤2，直至自主水下机器人的位姿调整至期望位姿。
本发明通过导航定位系统获得自主式水下机器人的速度和姿态等位姿信息，同时设计了洋流观测器估计洋流速度，为动力定位系统提供信息保障；基于MPC设计了动力定位系统的控制方法，进一步地，结合ISMC消除模型不匹配和洋流扰动估计有界误差对动力定位系统控制精度的影响。该方法和系统提高了海洋环境扰动作用下自主式水下机器人动力定位系统的鲁棒性，可以实现自主式水下机器人精准定位作业，具有客观可行的工程应用价值。
本发明实施例还提供一种抗洋流扰动的自主水下机器人动力定位系统，该系统包括测量单元、模型预测控制器、洋流观测器、滑膜控制器和计算单元：
测量单元用于采集自主水下机器人的测量状态量。
模型预测控制器用于根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量。
洋流观测器用于根据所述测量状态量，获取洋流速度估计值；
滑膜控制器，其用于根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量。
计算单元用于根据所述滑膜控制量和期望控制量，获取实际控制量，并监控自主水下机器人的位姿调整至期望位姿。
所述测量单元获得的当前时刻k的所述测量状态量信息被描述为X(k)，
X(k)＝[η(k)；V(k-1)]
式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，V(k)表示k时刻的惯性坐标系下的速度测量值；
所述洋流观测器获取所述洋流速度估计值的方法具体包括：
通过式(7)描述的洋流观测器，计算所述洋流速度估计值：
Figure BDA0003231183650000151
式中，Vf(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，
Figure BDA0003231183650000152
表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，
Figure BDA0003231183650000153
表示
Figure BDA0003231183650000154
的导数，
Figure BDA0003231183650000155
表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，
Figure BDA0003231183650000156
表示
Figure BDA0003231183650000157
的导数，Kv和Kη均表示增益矩阵，J表示惯性坐标系到载体坐标系的坐标转换矩阵。
所述运动学状态空间模型设置为预测时域Np和控制时域Nc下的式(9)：
Figure BDA0003231183650000158
所述优化问题被描述为式(11)：
Figure BDA0003231183650000159
式中：
Figure BDA00032311836500001510
Figure BDA00032311836500001511
Figure BDA00032311836500001512
表示k+1时刻在未来Np个预测步长内的预测状态量，X(k|k)表示k时刻预测的第一个状态量，U(k)表示k时刻在未来Nc个预测步长内的控制量，U*(k)表示U(k)的最优值，
Figure BDA00032311836500001513
表示k时刻状态量的系数矩阵，
Figure BDA00032311836500001514
表示k时刻控制量的系数矩阵，
Figure BDA00032311836500001515
表示权重矩阵，
Figure BDA00032311836500001516
表示权重矩阵，
Figure BDA00032311836500001517
表示期望状态量，
Figure BDA00032311836500001518
表示k-1时刻在未来Np个预测步长内的预测状态量；
所述约束设置为s.t.：
u(i|k)≤Umax
-u(i|k)≤-Umin
Figure BDA00032311836500001519
Figure BDA00032311836500001520
式中，u(i|k)表示k时刻在未来i时刻的预测控制量，Umax表示U(k)中的最大值，Umin表示U(k)中的最小值，V(i|k)表示k时刻在未来i时刻的预测速度，C表示哥氏力离心矩阵，D表示流体阻尼矩阵，F表示静水恢复力，M表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，T为采样时间，τmax表示期望控制量中的五自由度力的最大值，τmin表示所述五自由度力的最小值。
上述参数的经验数值可以参考如下表：
表1
Figure BDA0003231183650000161
最后需要指出的是：以上实施例仅用以说明本发明的技术方案，而非对其限制。本领域的普通技术人员应当理解：可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的精神和范围。

Claims (10)

1.一种抗洋流扰动的自主水下机器人动力定位方法，其特征在于，包括：
步骤1，采集自主水下机器人的测量状态量；
步骤2，根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；
步骤3，根据所述测量状态量，获取洋流速度估计值；
步骤4，根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量；
步骤5，根据所述滑膜控制量和期望控制量，获取实际控制量；
步骤6，返回步骤2，直至自主水下机器人的位姿调整至期望位姿。
2.如权利要求1所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤1中，当前时刻k的所述测量状态量被描述为X(k)＝[η(k)；V(k-1)]，式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，V(k-1)表示k-1时刻的惯性坐标系下的速度测量值；
所述步骤3获取所述洋流速度估计值的方法具体包括：
通过式(7)描述的洋流观测器，计算所述洋流速度估计值：
Figure FDA0003231183640000011
式中，Vf(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，
Figure FDA0003231183640000012
表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，
Figure FDA0003231183640000013
表示
Figure FDA0003231183640000014
的导数，
Figure FDA0003231183640000015
表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，
Figure FDA0003231183640000016
表示
Figure FDA0003231183640000017
的导数，Kv和Kη均表示增益矩阵，J表示惯性坐标系到载体坐标系的坐标转换矩阵。
3.如权利要求1或2所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤2中，所述运动学状态空间模型设置为预测时域np和控制时域Nc下的式(10)：
Figure FDA0003231183640000021
所述优化问题被描述为式(12)：
Figure FDA0003231183640000022
式中：
Figure FDA0003231183640000023
Figure FDA0003231183640000024
Figure FDA0003231183640000025
表示k+1时刻在未来np个预测步长内的预测状态量，x(k|k)表示k时刻预测的第一个状态量，U(k)表示k时刻在未来Nc个预测步长内的预测控制量，U*(k)表示U(k)的最优值，
Figure FDA0003231183640000026
表示k时刻状态量的系数矩阵，
Figure FDA0003231183640000027
表示k时刻控制量的系数矩阵，
Figure FDA0003231183640000028
表示权重矩阵，
Figure FDA0003231183640000029
表示权重矩阵，
Figure FDA00032311836400000210
表示期望状态量，
Figure FDA00032311836400000211
表示k时刻在未来Np个预测步长内的预测状态量。
4.如权利要求3所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述约束设置为s.t.：
u(i|k)≤Umax
-u(i|k)≤-Umin
Figure FDA00032311836400000212
Figure FDA00032311836400000213
式中，u(i|k)表示k时刻在未来第i时刻的预测控制量，Umax表示U(k)中的最大值，Umin表示U(k)中的最小值，V(i|k)表示k时刻在未来第i时刻的预测速度，C表示哥氏力离心矩阵，D表示流体阻尼矩阵，F表示静水恢复力，M表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，T为采样时间，τmax表示期望控制量中的五自由度力的最大值，τmin表示所述五自由度力的最小值。
5.如权利要求4所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤2中，通过式(9)描述的控制量u(k)、以及式(13)，计算期望加速度
Figure FDA0003231183640000031
u(k)＝V(k)-V(k-1) (9)
Figure FDA0003231183640000032
通过式(14)，计算得到期望控制量uMPC(k)：
uMPC(k)＝Mu*(k)/T-(C+D)V(k)-f (14)
式中，u*(k)是U*(k)的第一个元素。
6.如权利要求5所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤4中，所述积分滑模面被描述为s(V)，通过如下式(15)得到所述控制量uISMC(t)：
Figure FDA0003231183640000034
其中，V(t)表示t时刻的速度，h(V(τ))是状态反馈项。
7.如权利要求6所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤4中，在载体坐标系下沿x轴方向的所述饱和函数XISMC(t)被描述为式(14)：
Figure FDA0003231183640000033
其中，ρX为海浪扰动力的上限，su(t)表示载体坐标系相对于惯性坐标系沿x轴方向速度的积分滑模面。
8.一种抗洋流扰动的自主水下机器人动力定位系统，其特征在于，包括：
测量单元，其用于采集自主水下机器人的测量状态量；
模型预测控制器，其用于根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；
洋流观测器，其用于根据所述测量状态量，获取洋流速度估计值；
滑膜控制器，其用于根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量；
计算单元，其用于根据所述滑膜控制量和期望控制量，获取实际控制量，并监控自主水下机器人的位姿调整至期望位姿。
9.如权利要求8所述的抗洋流扰动的自主水下机器人动力定位系统，其特征在于，所述测量单元获得的当前时刻k的所述测量状态量信息被描述为X(k)＝[η(k)；V(k-1)]，式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，V(k)表示k时刻的惯性坐标系下的速度测量值；
所述洋流观测器获取所述洋流速度估计值的方法具体包括：
通过式(7)描述的洋流观测器，计算所述洋流速度估计值：
Figure FDA0003231183640000041
式中，Vf(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，
Figure FDA0003231183640000042
表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，
Figure FDA0003231183640000043
表示
Figure FDA0003231183640000044
的导数，
Figure FDA0003231183640000045
表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，
Figure FDA0003231183640000046
表示
Figure FDA0003231183640000047
的导数，Kv和Kη均表示增益矩阵，J表示惯性坐标系到载体坐标系的坐标转换矩阵。
10.如权利要求8或9所述的抗洋流扰动的自主水下机器人动力定位系统，其特征在于，所述运动学状态空间模型设置为预测时域Np和控制时域Nc下的式(9)：
Figure FDA0003231183640000048
所述优化问题被描述为式(11)：
Figure FDA0003231183640000051
式中：
Figure FDA0003231183640000052
Figure FDA0003231183640000053
Figure FDA0003231183640000054
表示k时刻在未来Np个预测步长内的预测状态量，x(k|k)表示k时刻预测的第一个状态量，U(k)表示k时刻在未来Nc个预测步长内的控制量，U*(k)表示U(k)的最优值，
Figure FDA0003231183640000055
表示k时刻状态量的系数矩阵，
Figure FDA0003231183640000056
表示k时刻控制量的系数矩阵，
Figure FDA0003231183640000057
表示权重矩阵，
Figure FDA0003231183640000058
表示权重矩阵，
Figure FDA0003231183640000059
表示期望状态量，
Figure FDA00032311836400000510
表示k-1时刻在未来Np个预测步长内的预测状态量；
所述约束设置为s.t.：
u(i|k)≤UmAx
-u(i|k)≤-Umin
Figure FDA00032311836400000511
Figure FDA00032311836400000512
式中，u(i|k)表示k时刻在未来i时刻的预测控制量，Umax表示U(k)中的最大值，Umin表示U(k)中的最小值，V(i|k)表示k时刻在未来i时刻的预测速度，C表示哥氏力离心矩阵，D表示流体阻尼矩阵，F表示静水恢复力，M表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，T为采样时间，τmax表示期望控制量中的五自由度力的最大值，τmin表示所述五自由度力的最小值。
CN202110987332.3A 2021-08-26 2021-08-26 一种抗洋流扰动的自主水下机器人动力定位方法和系统 Active CN113485390B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110987332.3A CN113485390B (zh) 2021-08-26 2021-08-26 一种抗洋流扰动的自主水下机器人动力定位方法和系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110987332.3A CN113485390B (zh) 2021-08-26 2021-08-26 一种抗洋流扰动的自主水下机器人动力定位方法和系统

Publications (2)

Publication Number Publication Date
CN113485390A true CN113485390A (zh) 2021-10-08
CN113485390B CN113485390B (zh) 2022-05-03

Family

ID=77946266

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110987332.3A Active CN113485390B (zh) 2021-08-26 2021-08-26 一种抗洋流扰动的自主水下机器人动力定位方法和系统

Country Status (1)

Country Link
CN (1) CN113485390B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114839875A (zh) * 2022-04-26 2022-08-02 江苏集萃清联智控科技有限公司 履带式深海集矿车内外环控制方法及系统

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013174967A1 (de) * 2012-05-24 2013-11-28 Avl List Gmbh Verfahren und eine vorrichtung zur prüfung des antriebsstranges von zumindest teilweise elektrisch betriebenen fahrzeugen
CN105974930A (zh) * 2016-04-18 2016-09-28 哈尔滨工程大学 一种基于非线性模型预测控制的uuv对运动母船的跟踪方法
CN107450572A (zh) * 2017-07-26 2017-12-08 江苏科技大学 基于滑模控制的水下机器人姿态调节控制系统及处理方法
CN107957727A (zh) * 2016-10-17 2018-04-24 江苏舾普泰克自动化科技有限公司 水下机器人控制系统及动力定位方法
CN109343350A (zh) * 2018-11-20 2019-02-15 清华大学 一种基于模型预测控制的水下机器人路径跟踪控制方法
US20190176945A1 (en) * 2017-12-07 2019-06-13 Her Majesty The Queen In Right Of Canada Acoustic Response Control System
CN109901403A (zh) * 2019-04-08 2019-06-18 哈尔滨工程大学 一种自主水下机器人神经网络s面控制方法
CN110286687A (zh) * 2019-06-03 2019-09-27 哈尔滨工程大学 一种海浪对水下机器人扰动评估装置及方法
US20200055570A1 (en) * 2017-04-28 2020-02-20 Fraunhofer-Gesellschaft Zur Foerderung Der Angewandten Forschung E.V. Navigation system for underwater vehicles
CN111324146A (zh) * 2020-03-09 2020-06-23 河海大学常州校区 一种水下巡检机器人的轨迹跟踪控制方法
CN112527018A (zh) * 2020-12-26 2021-03-19 九江职业技术学院 一种欠驱动自主水下航行器的三维镇定控制方法
CN112859889A (zh) * 2021-01-18 2021-05-28 湖南大学 基于自适应动态规划的自主式水下机器人控制方法和系统

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2013174967A1 (de) * 2012-05-24 2013-11-28 Avl List Gmbh Verfahren und eine vorrichtung zur prüfung des antriebsstranges von zumindest teilweise elektrisch betriebenen fahrzeugen
CN105974930A (zh) * 2016-04-18 2016-09-28 哈尔滨工程大学 一种基于非线性模型预测控制的uuv对运动母船的跟踪方法
CN107957727A (zh) * 2016-10-17 2018-04-24 江苏舾普泰克自动化科技有限公司 水下机器人控制系统及动力定位方法
US20200055570A1 (en) * 2017-04-28 2020-02-20 Fraunhofer-Gesellschaft Zur Foerderung Der Angewandten Forschung E.V. Navigation system for underwater vehicles
CN107450572A (zh) * 2017-07-26 2017-12-08 江苏科技大学 基于滑模控制的水下机器人姿态调节控制系统及处理方法
US20190176945A1 (en) * 2017-12-07 2019-06-13 Her Majesty The Queen In Right Of Canada Acoustic Response Control System
CN109343350A (zh) * 2018-11-20 2019-02-15 清华大学 一种基于模型预测控制的水下机器人路径跟踪控制方法
CN109901403A (zh) * 2019-04-08 2019-06-18 哈尔滨工程大学 一种自主水下机器人神经网络s面控制方法
CN110286687A (zh) * 2019-06-03 2019-09-27 哈尔滨工程大学 一种海浪对水下机器人扰动评估装置及方法
CN111324146A (zh) * 2020-03-09 2020-06-23 河海大学常州校区 一种水下巡检机器人的轨迹跟踪控制方法
CN112527018A (zh) * 2020-12-26 2021-03-19 九江职业技术学院 一种欠驱动自主水下航行器的三维镇定控制方法
CN112859889A (zh) * 2021-01-18 2021-05-28 湖南大学 基于自适应动态规划的自主式水下机器人控制方法和系统

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
RAFAEL MEIRELES SABACK: "Nonlinear Model Predictive Control Applied to an Autonomous Underwater Vehicle", 《 IEEE JOURNAL OF OCEANIC ENGINEERING》 *
XINXIN GUO: "Neural network-based nonlinear sliding-mode control for an AUV without velocity measurements", 《INTERNATIONAL JOURNAL OF CONTROL》 *
张丽珍: "基于MPC的半潜式无人艇导航轨迹跟踪控制研究", 《基于MPC的半潜式无人艇导航轨迹跟踪控制研究》 *
窦刚等: "海流环境自主水下机器人动力定位控制器设计", 《重庆理工大学学报(自然科学)》 *

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114839875A (zh) * 2022-04-26 2022-08-02 江苏集萃清联智控科技有限公司 履带式深海集矿车内外环控制方法及系统

Also Published As

Publication number Publication date
CN113485390B (zh) 2022-05-03

Similar Documents

Publication Publication Date Title
CN104898688B (zh) Uuv四自由度动力定位自适应抗扰滑模控制系统及控制方法
Li et al. Robust adaptive motion control for underwater remotely operated vehicles with velocity constraints
CN105807789B (zh) 基于t-s模糊观测器补偿的uuv控制方法
CN109669345B (zh) 基于eso的水下机器人模糊pid运动控制方法
Sebastián et al. Adaptive fuzzy sliding mode controller for the kinematic variables of an underwater vehicle
CN112068440B (zh) 基于模型预测控制的auv回收对接动力定位控制方法
CN113848887A (zh) 一种基于mlp方法的欠驱动无人艇轨迹跟踪控制方法
Tuan et al. Observer-based nonlinear robust control of floating container cranes subject to output hysteresis
CN113485390B (zh) 一种抗洋流扰动的自主水下机器人动力定位方法和系统
Deng et al. UKF based nonlinear offset-free model predictive control for ship dynamic positioning under stochastic disturbances
US11361131B2 (en) Method for establishing the excitation force applied by the swell incident on a movable means of a wave energy system using a model of the drag force
Chu et al. Incorporation of ship motion prediction into active heave compensation for offshore crane operation
CN114967714A (zh) 一种自主式水下机器人抗扰运动控制方法及系统
KR20170071440A (ko) 파라미터 적응형 백스테핑 제어 auv 시스템
Ghorbani Line of sight waypoint guidance for a container ship based on frequency domain identification of Nomoto model of vessel
CN113467231A (zh) 基于侧滑补偿ilos制导律的无人艇路径跟踪方法
Skulstad et al. A co-operative hybrid model for ship motion prediction
Nguyen et al. Fuzzy controller design for autonomous underwater vehicles path tracking
CN110647161A (zh) 基于状态预测补偿的欠驱动uuv水平面轨迹跟踪控制方法
CN113954077B (zh) 带有能量优化的水下游动机械臂轨迹跟踪控制方法及装置
Loueipour Design of dynamic positioning control system for an ROV with unknown dynamics using modified time delay estimation
Ohhira et al. An underwater quadrotor control with wave-disturbance compensation by a UKF
Huang et al. Neural network adaptive backstepping control of multi-link underwater flexible manipulators
CN117250971B (zh) 一种微小型auv的控制方法
CN114564029B (zh) 基于直接参数化法的全驱动船舶轨迹跟踪控制方法和装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant