[发明专利]一种抗洋流扰动的自主水下机器人动力定位方法和系统

权利要求书

1.一种抗洋流扰动的自主水下机器人动力定位方法，其特征在于，包括：

步骤1，采集自主水下机器人的测量状态量；当前时刻k的所述测量状态量被描述为X(k)＝[η(k)；V(k-1)]，式中，η(k)表示k时刻的惯性坐标系下的位姿测量值，V(k-1)表示k-1时刻的惯性坐标系下的速度测量值；

步骤2，根据设定的自主水下机器人的期望状态信息和所述测量状态量，构建自主水下机器人运动学状态空间模型和依据饱和性预设的约束，在二次规划求解模型预测控制的优化问题后，再通过动力学模型输出自主水下机器人的期望控制量；

步骤3，根据所述测量状态量，获取洋流速度估计值；所述步骤3获取所述洋流速度估计值的方法具体包括：

通过式(7)描述的洋流观测器，计算所述洋流速度估计值：

式中，V_f(k)表示k时刻的洋流观测器计算得到的载体坐标系下的洋流速度估计值，表示k时刻的洋流观测器计算得到的惯性坐标系下的洋流速度估计值，表示的导数，表示k时刻的洋流观测器计算得到的惯性坐标系下的位姿估计值，表示的导数，K_v和K_η均表示增益矩阵，j表示惯性坐标系到载体坐标系的坐标转换矩阵；

步骤4，根据所述期望控制量、洋流速度估计值和自主水下机器人的速度，通过积分滑模面，获得用于抵消外部扰动的控制量，并通过饱和函数获得滑模控制量；

步骤5，根据所述滑模控制量和期望控制量，获取实际控制量；

步骤6，返回步骤2，直至自主水下机器人的位姿调整至期望位姿；

所述步骤2中，所述运动学状态空间模型设置为预测时域N_p和控制时域N_c下的式(10)：

所述优化问题被描述为式(12)：

式中：

表示k+1时刻在未来N_p个预测步长内的预测状态量，U(k)表示k时刻在未来N_c个预测步长内的预测控制量，U^*(k)表示U(k)的最优值，表示k时刻状态量的系数矩阵，表示k时刻控制量的系数矩阵，表示权重矩阵，表示权重矩阵，表示期望状态量，表示k时刻在未来N_p个预测步长内的预测状态量；所述约束设置为s.t.：

u(i|k)≤U_max

-u(i|k)≤-U_min

式中，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表示所述五自由度力的最小值；所述步骤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)的第一个元素；所述步骤4中，所述积分滑模面被描述为s(V)，通过如下式(15)得到所述控制量u_ISMC(t)：

其中，V(t)表示t时刻的速度，h(V(τ))是状态反馈项。

2.如权利要求1所述的抗洋流扰动的自主水下机器人动力定位方法，其特征在于，所述步骤4中，在载体坐标系下沿x轴方向的所述饱和函数X_ISMC(t)被描述为下式：

其中，ρ_X为海浪扰动力的上限，s_u(t)表示载体坐标系相对于惯性坐标系沿x轴方向速度的积分滑模面。