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

技术特征：
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)描述的洋流观测器，计算所述洋流速度估计值：式中，v
f
(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，表示的导数，表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，表示的导数，k
v
和k
η
均表示增益矩阵，j表示惯性坐标系到载体坐标系的坐标转换矩阵。3.如权利要求1或2所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤2中，所述运动学状态空间模型设置为预测时域n
p
和控制时域n
c
下的式(10)：所述优化问题被描述为式(12)：式中：式中：式中：表示k 1时刻在未来np个预测步长内的预测状态量，x(k|k)表示k时刻预测的第一个状态量，u(k)表示k时刻在未来n
c
个预测步长内的预测控制量，u
*
(k)表示u(k)的最优值，表示k时刻状态量的系数矩阵，表示k时刻控制量的系数矩阵，表示权重
矩阵，表示权重矩阵，表示期望状态量，表示k时刻在未来np个预测步长内的预测状态量。4.如权利要求3所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述约束设置为s.t.：u(i|k)≤u
max
‑
u(i|k)≤
‑
u
minmin
式中，u(i|k)表示k时刻在未来第i时刻的预测控制量，u
max
表示u(k)中的最大值，u
min
表示u(k)中的最小值，v(i|k)表示k时刻在未来第i时刻的预测速度，c表示哥氏力离心矩阵，d表示流体阻尼矩阵，f表示静水恢复力，m表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，t为采样时间，τ
max
表示期望控制量中的五自由度力的最大值，τ
min
表示所述五自由度力的最小值。5.如权利要求4所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤2中，通过式(9)描述的控制量u(k)、以及式(13)，计算期望加速度u(k)＝v(k)
‑
v(k
‑
1)
ꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(9)通过式(14)，计算得到期望控制量u
mpc
(k)：u
mpc
(k)＝mu
*
(k)/t
‑
(c d)v(k)
‑
f
ꢀꢀꢀꢀꢀꢀꢀꢀ
(14)式中，u
*
(k)是u
*
(k)的第一个元素。6.如权利要求5所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤4中，所述积分滑模面被描述为s(v)，通过如下式(15)得到所述控制量u
ismc
(t)：其中，v(t)表示t时刻的速度，h(v(τ))是状态反馈项。7.如权利要求6所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤4中，在载体坐标系下沿x轴方向的所述饱和函数x
ismc
(t)被描述为式(14)：其中，ρ
x
为海浪扰动力的上限，s
u
(t)表示载体坐标系相对于惯性坐标系沿x轴方向速度的积分滑模面。8.一种抗洋流扰动的自主水下机器人动力定位系统，其特征在于，包括：测量单元，其用于采集自主水下机器人的测量状态量；
模型预测控制器，其用于根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；洋流观测器，其用于根据所述测量状态量，获取洋流速度估计值；滑膜控制器，其用于根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑膜控制量；计算单元，其用于根据所述滑膜控制量和期望控制量，获取实际控制量，并监控自主水下机器人的位姿调整至期望位姿。9.如权利要求8所述的抗洋流扰动的自主水下机器人动力定位系统，其特征在于，所述测量单元获得的当前时刻k的所述测量状态量信息被描述为x(k)＝[η(k)；v(k
‑
1)]，式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，v(k)表示k时刻的惯性坐标系下的速度测量值；所述洋流观测器获取所述洋流速度估计值的方法具体包括：通过式(7)描述的洋流观测器，计算所述洋流速度估计值：式中，v
f
(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，表示的导数，表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，表示的导数，k
v
和k
η
均表示增益矩阵，j表示惯性坐标系到载体坐标系的坐标转换矩阵。10.如权利要求8或9所述的抗洋流扰动的自主水下机器人动力定位系统，其特征在于，所述运动学状态空间模型设置为预测时域n
p
和控制时域n
c
下的式(9)：所述优化问题被描述为式(11)：式中：式中：式中：表示k时刻在未来np个预测步长内的预测状态量，x(k|k)表示k时刻预测的第一个状态量，u(k)表示k时刻在未来n
c
个预测步长内的控制量，u
*
(k)表示u(k)的最优值，表示k时刻状态量的系数矩阵，表示k时刻控制量的系数矩阵，表示权重矩阵，
表示权重矩阵，表示期望状态量，表示k
‑
1时刻在未来np个预测步长内的预测状态量；所述约束设置为s.t.：u(i|k)≤u
max
‑
u(i|k)≤
‑
u
minmin
式中，u(i|k)表示k时刻在未来i时刻的预测控制量，u
max
表示u(k)中的最大值，u
min
表示u(k)中的最小值，v(i|k)表示k时刻在未来i时刻的预测速度，c表示哥氏力离心矩阵，d表示流体阻尼矩阵，f表示静水恢复力，m表示期望控制量中的力矩控制量在载体坐标系下沿y轴的分量，t为采样时间，τ
max
表示期望控制量中的五自由度力的最大值，τ
min
表示所述五自由度力的最小值。

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


技术研发人员：边有钢 陈昱衡 徐彪 崔庆佳 胡满江 秦兆博 秦洪懋 秦晓辉 谢国涛 王晓伟 丁荣军
受保护的技术使用者：湖南大学
技术研发日：2021.08.26
技术公布日：2021/10/8