CN115480580A - 一种基于nmpc的水下机器人路径跟踪与避障的控制方法 - Google Patents

Abstract

本发明涉及一种基于NMPC的水下机器人路径跟踪与避障的控制方法，包括：建立水下机器人的运动学动力学模型；计算出当前水下机器人的速度，估计出外部扰动；对水下机器人未来N个控制周期的运动状态进行预测，预测水下障碍物的短时间内的轨迹；对最优化问题进行求解并获得最优控制序列；声呐扫描周期结束，计算卡尔曼滤波器新的先验状态量与后验状态量，对水下障碍物的不确定性同样进行更新计算，否则不进行更新，保持不变；直至水下机器人进行路径跟踪且能完成避障。本发明采用改进的扩张状态观测器进行扰动观测，大大提升水下机器人抗干扰能力，运动的鲁棒性；使得水下机器人碰撞概率降低，缩短行驶距离且扩大了与障碍物的安全距离。

Description

一种基于NMPC的水下机器人路径跟踪与避障的控制方法

技术领域

本发明涉及水下仿生机器人路径跟踪技术领域，尤其是一种基于NMPC的 水下机器人路径跟踪与避障的控制方法。

背景技术

海洋是地球最大的资源宝藏，由于陆地资源不断被挖掘匮乏，人类对海洋 不断地进行研究与开发，水下机器人就成为了开发海洋资源的重要工具之一， 它可代替人类在海洋中完成高难度高风险的资源探测与采集等一系列工作，其 中水下机器人的相关技术，最核心的就是运动控制，来完成高精度路径跟踪与 自主动态避障。

几十年来，水下机器人或水面无人船(USV)的路径跟踪控制一直是世界各 国学者共同关注的问题，并提出了许多优秀的控制策略：如自适应控制、反步 控制、滑模方法、神经网络控制、模型预测控制等。这些传统控制方法对于模 型参数不确定性和未知扰动情况下的水下机器人或水面舰船的路径跟踪问题 都有一定作用效果，但很少有在控制方法中能考虑到避障问题，以及对障碍物 未来位置的有效预测，然而，在海洋自然环境扰动中，一旦偏离预定轨迹，发 生碰撞的风险将极大增加，水下机器人如何能够安全避免碰撞，并保持对预定 路径的跟踪，这对于仅依靠冲击力和偏航力矩的欠驱动水下机器人来说是一个严峻的挑战。此外，依赖波动鳍推进的仿生水下机器人控制输入不同于传统的 螺旋桨推进水下机器人和水面舰艇，导致模型的不确定性和未知外部干扰的估 计变得更加复杂。

发明内容

本发明的目的在于提供一种大大提升水下机器人抗干扰能力和运动的鲁 棒性，对短周期内的障碍物未来位置进行预测，使得水下机器人碰撞概率降低， 缩短行驶距离且扩大了与障碍物的安全距离的基于NMPC的水下机器人路径跟 踪与避障的控制方法。

为实现上述目的，本发明采用了以下技术方案：一种基于NMPC的水下机 器人路径跟踪与避障的控制方法，该方法包括下列顺序的步骤：

(1)建立水下机器人的运动学动力学模型，建立最优化问题的目标函数， 设定全局路径参考点，利用贝塞尔曲线插值获得全局跟踪路径

，计算得到艏 摇角；

(2)测量水下机器人当前的位置，读取艏摇角，通过扩张状态观测器计 算出当前水下机器人的速度，估计出外部扰动,同时采用声呐对水下障碍物状 态进行扫描，并将探测结果输入卡尔曼滤波器；

(3)利用建立的运动学动力学模型，根据当前水下机器人的速度和外部 扰动，对水下机器人未来N个控制周期的运动状态进行预测，并同时预测水下 障碍物的短时间内的轨迹；

(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障 碍物的短时间内的轨迹，利用求解器对最优化问题进行求解并获得最优控制序 列，并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号；

(5)如果声呐扫描周期结束，通过声呐测量的水下障碍运动状态更新， 则卡尔曼滤波器的输入更新，计算卡尔曼滤波器新的先验状态量与后验状态量， 并且对水下障碍物的不确定性同样进行更新计算，如果声呐扫描周期没有结束 则卡尔曼滤波器与水下障碍物不确定性不进行更新，保持不变；

(6)重复步骤(2)至步骤(5)，直至水下机器人进行路径跟踪且能完 成避障。

所述步骤(1)具体是指：

建立水下机器人三自由度下的运动学动力学模型：

其中，

为状态向量，V＝[u r v]^T表示水下机 器人的速度，x和y代表大地坐标系下水下机器人的位置，

表示水下机器人艏 摇角，u为水下机器人前进速度，v为水下机器人横移速度，r为水下机器人艏摇 速度；

为控制输入量，f_u为控制输入纵倾力，f_v为横向驱 动力，水下机器人为欠驱动系统，f_v＝0；f_r为偏航力矩；τ_d＝[d_u d_v d_r]^T为 水下机器人外部扰动，d_u为水下机器人前进方向扰动力，d_v为水下机器人横移 方向扰动力，d_r为艏摇方向的外部环境扰动力矩，M为水下机器人惯性质量矩 阵，D为水下机器人阻尼力对角矩阵，C(v)为向心力和科里奥利矩阵，其公式 如下：

其中，m₁₁、m₂₂、m₃₃均为水下机器人的附加质量参数，d₁₁、d₂₂、d₃₃均为 水动力系数；

是将体坐标系下的水下机器人横移速度v转换为大地坐标系速度η的旋 转矩阵，具体为：

建立最优化问题的目标函数：

其中，J_x(x(t)，x_ref(t))为期望状态代价，其公式为：

J_u(αu(t))为控制输入量代价，其公式为：

J_c(x(t)为碰撞代价，其公式为：

其中，T为水下机器人采样周期，f(x，u)是描述水下机器人连续非线性方 程，d_i(t)表示为与第i个水下障碍物的欧氏距离，k_i＞0为平滑避障轨迹的参 数；

为期望状态代价项的权重系数矩阵，

为控制输入量代 价的权重系数矩阵，Q_c表示碰撞代价系数；U是控制量力与力矩可输入范围， 如下式所示：

水下机器人是欠驱动系统，则有f_vmin＝f_vmax＝0；f_vmin为水下机器人横 向驱动力最小值，f_vmax为水下机器人横向驱动力最大值，f_umin为水下机器人 纵向驱动力的最小值，f_umax为水下机器人纵向驱动力的最大值，f_rmin为水下 机器人艏摇力矩的最小值，f_rmax为水下机器人艏摇力矩最大值。

所述步骤(2)具体是指：

测量与计算水下机器人状态量x(t₀)，采用扩张状态观测器计算水下机器人 速度、外部扰动状态：

其中，l₀至l₈皆为正整数，b₀，b₁为补偿因子，ζ＝u·t与χ＝v·t分别代表 体坐标系下的纵向与横向位移；

表示水下机器人艏摇角，u为水下机器人前 进速度，v为水下机器人横移速度，r为水下机器人艏摇速度；f_u为控制输入纵 倾力，f_r为偏航力矩；m₁₁、m₂₂、m₃₃均为水下机器人的附加质量参数，d₁₁、 d₂₂、d₃₃均为水动力系数；通过分别求解式(6)、(7)、(8)，得出水下 机器人当前的纵向速度

横向速度

艏摇速度

以及纵向

横向

艏摇

三个方向外部扰动的估计值，“∧”表示计算估计值，“·”表示微分符号。

所述步骤(3)具体是指：利用水下机器人的运动学动力学模型对水下机 器人未来状态进行计算预测得到

并且通过水下障碍物 直线运动模型对水下障碍物状态进行预测：

其中，

为水下障碍物i的预测位置，T_s为采样周期，p_{obs i}(t₀)为水下 障碍物当前时刻的测量位置，v^o-为水下障碍物测量先验速度，N为预测步数， t₀为预测开始当下时刻，往后预测n个T_s采样周期；

(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态 进行预测；采用公式(9)，通过声呐测量出的当前水下障碍物的位置，加上 水下障碍物测量先验速度乘上采样周期T_s，获得未来n个T_s周期内的水下障碍 物直线运动位置。

所述步骤(4)具体是指：利用求解器对最优化问题进行求解并获得最优 控制序列u^*(t+nT_s)，n＝1，...，N-1，并将最优控制序列首位

转换作为水下 机器人波动鳍推进器的控制输入信号，u^*表示通过求解器求解出来的最优控制 序列。

所述步骤(5)具体是指：如果声呐扫描周期结束，即水下障碍物的测量 状态量更新，则对卡尔曼滤波器的水下障碍物先验状态量

与后验状态量P_t更新计算，并且对水下障碍物的不确定性r_th，k(t)进行计算更新，否则卡尔曼滤 波器与水下障碍物不确定性不进行更新；

其中卡尔曼滤波器分为如下两大部分：

预测：

更新：

其中，x_t＝[x_obs，y_obs，v_obs]^T是水下障碍物的状态量，x_obs，y_obs为水下障 碍物的坐标位置，v_obs为水下障碍物的速度，Z_t＝[x^o，y^o，v^o]^T是在t时刻的测 量的水下障碍物状态量，x^o，y^o为水下障碍物测量的坐标位置，v^o为水下障碍 物的测量速度，F是状态转移矩阵，H是观测矩阵，Q_t是水下障碍物运动噪声 协方差矩阵，R_t为观测噪声协方差矩阵，I为单位矩阵；

通过求解计算

最大特征值δ_i(t)，通过公式(12)描述第i 个水下障碍物在t时刻的不确定性：

r_th，i(t)＝r_th+10δ_i(t) (12)

其中，r_th为常量参数。

由上述技术方案可知，本发明的有益效果为：第一，本发明对于仿生水下 机器人由于独特的推进方式而导致模型参数不确定性以及外部环境干扰，采用 改进的扩张状态观测器进行扰动观测，并将其作为非线性模型预测控制扰动项 进行不断计算更新，大大提升水下机器人抗干扰能力，运动的鲁棒性；第二， 本发明针对动态障碍物状态观测不确定性，即观测误差，声呐扫描周期过长， 通过结合卡尔曼滤波先验估计，对短周期内的障碍物未来位置进行预测，使得 水下机器人碰撞概率降低，缩短行驶距离且扩大了与障碍物的安全距离。

附图说明

图1是路径跟踪示意图；

图2是本发明的方法框架示意图；

图3、4、5、6均是仿真1过程的重要时刻截图；

图7是曲线跟踪对比结果示意图；

图8、9、10、11、12、13均是多动态水下障碍物仿真截图。

具体实施方式

如图2所示，一种基于NMPC的水下机器人路径跟踪与避障的控制方法， 该方法包括下列顺序的步骤：

(1)建立水下机器人的运动学动力学模型，建立最优化问题的目标函数， 设定全局路径参考点，利用贝塞尔曲线插值获得全局跟踪路径

计算得到艏 摇角；系统参数初始化，包含系列全局跟踪路径

全局跟踪路径参考点为

如图1所示，其中，e_x表示水下机器当前位置坐标x与期望位置坐 标x_d之间的误差，e_y表示水下机器当前位置坐标y与期望位置坐标y_d之间的误 差，

表示水下机器当前水下机器人艏摇角

与期望艏摇角参考角

之间的误 差；x_B和y_B为水下机器人体坐标系坐标轴，x_I和y_I为大地坐标系坐标轴。

(2)测量水下机器人当前的位置，读取艏摇角，通过扩张状态观测器计 算出当前水下机器人的速度，估计出外部扰动,同时采用声呐对水下障碍物状 态进行扫描，并将探测结果输入卡尔曼滤波器；

(3)利用建立的运动学动力学模型，根据当前水下机器人的速度和外部 扰动，对水下机器人未来N个控制周期的运动状态进行预测，并同时预测水下 障碍物的短时间内的轨迹；

(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障 碍物的短时间内的轨迹，利用求解器对最优化问题进行求解并获得最优控制序 列，并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号；

(5)如果声呐扫描周期结束，通过声呐测量的水下障碍运动状态更新， 则卡尔曼滤波器的输入更新，计算卡尔曼滤波器新的先验状态量与后验状态量， 并且对水下障碍物的不确定性同样进行更新计算，如果声呐扫描周期没有结束 则卡尔曼滤波器与水下障碍物不确定性不进行更新，保持不变；

(6)重复步骤(2)至步骤(5)，直至水下机器人进行路径跟踪且能完 成避障。

在本发明中，路径规划是指：在具有障碍物的环境中，给定机器人的起始 点与目标点，然后要求机器人从某一处出发，按照某种控制规律到达该路径上， 并实现其跟踪运动。

NMPC即非线性模型预测控制：是一种基于非线性模型的闭环优化控制策 略，对于强非线性系统，线性模型难以描述复杂动态过程，线性化模型不太适 用，造成线性预测控制难以运用，故提出的非线性系统模型预测控制。

卡尔曼滤波：一种利用线性的系统状态方程，通过系统输入输出观测数据， 对系统状态进行最优估计的算法。

扩张状态观测器：是通用而实用的扰动观测器。

所述步骤(1)具体是指：

建立水下机器人三自由度下的运动学动力学模型：

其中，

为状态向量，V＝[u r v]^T表示水下机 器人的速度，x和y代表大地坐标系下水下机器人的位置，

表示水下机器人艏 摇角，u为水下机器人前进速度，v为水下机器人横移速度，r为水下机器人艏摇 速度；

为控制输入量，f_u为控制输入纵倾力，f_v为横向驱 动力，水下机器人为欠驱动系统，f_v＝0；f_r为偏航力矩；τ_d＝[d_u d_v d_r]^T为 水下机器人外部扰动，d_u为水下机器人前进方向扰动力，d_v为水下机器人横移 方向扰动力，d_r为艏摇方向的外部环境扰动力矩，M为水下机器人惯性质量矩 阵，D为水下机器人阻尼力对角矩阵，C(v)为向心力和科里奥利矩阵，其公式 如下：

其中，m₁₁、m₂₂、m₃₃均为水下机器人的附加质量参数，d₁₁、d₂₂、d₃₃均为 水动力系数；

是将体坐标系下的水下机器人横移速度v转换为大地坐标系速度η的旋 转矩阵，具体为：

建立最优化问题的目标函数：

其中，J_x(x(t)，x_ref(t))为期望状态代价，其公式为：

J_u(αu(t))为控制输入量代价，其公式为：

J_c(x(t)为碰撞代价，其公式为：

其中，T为水下机器人采样周期，f(x，u)是描述水下机器人连续非线性方 程，d_i(t)表示为与第i个水下障碍物的欧氏距离，k_i＞0为平滑避障轨迹的参 数；

为期望状态代价项的权重系数矩阵，

为控制输入量代 价的权重系数矩阵，Q_c表示碰撞代价系数；U是控制量力与力矩可输入范围， 如下式所示：

水下机器人是欠驱动系统，则有f_vmin＝f_vmax＝0；f_vmin为水下机器人横 向驱动力最小值，f_vmax为水下机器人横向驱动力最大值，f_umin为水下机器人 纵向驱动力的最小值，f_umax为水下机器人纵向驱动力的最大值，f_rmin为水下 机器人艏摇力矩的最小值，f_rmax为水下机器人艏摇力矩最大值。

所述步骤(2)具体是指：

测量与计算水下机器人状态量x(t₀)，采用扩张状态观测器计算水下机器人 速度、外部扰动状态：

其中，l₀至l₈皆为正整数，b₀，b₁为补偿因子，ζ＝u·t与χ＝v·t分别代表 体坐标系下的纵向与横向位移；

表示水下机器人艏摇角，u为水下机器人前 进速度，v为水下机器人横移速度，r为水下机器人艏摇速度；f_u为控制输入纵 倾力，f_r为偏航力矩；m₁₁、m₂₂、m₃₃均为水下机器人的附加质量参数，d₁₁、 d₂₂、d₃₃均为水动力系数；通过分别求解式(6)、(7)、(8)，得出水下 机器人当前的纵向速度

横向速度

艏摇速度

以及纵向

横向

艏摇

三个方向外部扰动的估计值，“∧”表示计算估计值，“·”表示微分符号。

所述步骤(3)具体是指：利用水下机器人的运动学动力学模型对水下机 器人未来状态进行计算预测得到

并且通过水下障碍物 直线运动模型对水下障碍物状态进行预测：

其中，

为水下障碍物i的预测位置，T_s为采样周期，p_{obs i}(t₀)为水下 障碍物当前时刻的测量位置，v^o-为水下障碍物测量先验速度，N为预测步数， t₀为预测开始当下时刻，往后预测n个T_s采样周期；

(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态 进行预测；采用公式(9)，通过声呐测量出的当前水下障碍物的位置，加上 水下障碍物测量先验速度乘上采样周期T_s，获得未来n个T_s周期内的水下障碍 物直线运动位置。

所述步骤(4)具体是指：利用求解器对最优化问题进行求解并获得最优 控制序列u^*(t+nT_s)，n＝1，...，N-1，并将最优控制序列首位

转换作为水下 机器人波动鳍推进器的控制输入信号，u^*表示通过求解器求解出来的最优控制 序列。

所述步骤(5)具体是指：如果声呐扫描周期结束，即水下障碍物的测量 状态量更新，则对卡尔曼滤波器的水下障碍物先验状态量

与后验状态量P_t更新计算，并且对水下障碍物的不确定性r_th，k(t)进行计算更新，否则卡尔曼滤 波器与水下障碍物不确定性不进行更新；

其中卡尔曼滤波器分为如下两大部分：

预测：

更新：

其中，x_t＝[x_obs，y_obs，v_obs]^T是水下障碍物的状态量，x_obs，y_obs为水下障 碍物的坐标位置，v_obs为水下障碍物的速度，Z_t＝[x^o，y^o，v^o]^T是在t时刻的测 量的水下障碍物状态量，x^o，y^o为水下障碍物测量的坐标位置，v^o为水下障碍 物的测量速度，F是状态转移矩阵，H是观测矩阵，Q_t是水下障碍物运动噪声 协方差矩阵，R_t为观测噪声协方差矩阵，I为单位矩阵；

通过求解计算

最大特征值δ_i(t)，通过公式(12)描述第i 个水下障碍物在t时刻的不确定性：

r_th，i(t)＝r_th+10δ_i(t) (12)

其中，r_th为常量参数。

仿真实验，本发明选择了单动态水下障碍物与多动态水下障碍物两种情况 进行仿真实验。参数设置：m₁₁＝57.5，m₂₂＝61.3，m₃₃＝1.15，d₁₁＝53，d₂₂＝ 58，以及d₃₃＝3.1。

系统初始状态量均为0，额外扰动为：τ_d＝[0.6*sin(0.075*t)，0.5* sin(0.075*t)，0.4*sin(0.075*t)]^TN·m，非线性模型预测控制相关参数为： T_s＝0.2s，N_p＝25，N_c＝1，相关参数权重： Q_x＝diag{55.0，55.0，5.0，1.0，800，5.0}以及 Q_u＝diag{10.0，25.0}，碰撞代价Q_c＝20，期望纵移速度为0.2m/s。 扩张状态观测器参数分别为：l₁＝3.6，l₂＝4.32，l₃＝1.728，l₄＝2.4，l₅＝ 1.92，l₆＝1.516，l₇＝2.4，l₈＝1.92，l₉＝0.528。初始障碍物坐标

运动速度为-0.15m/s，并添加高斯分布变化为N(0，0.02)。

图3至图6展示了在添加外部扰动情况下进行路径跟踪与一个动态障碍物避 障时的关键时刻序列图，曲线为期望路径，小半径圆圈为水下机器人，大半径 圆圈为动态水下障碍物，圆圈上方的标记符号表示此时的偏航角，其中，图3为 t＝35.0s，图4为t＝37.2s，图5为t＝46.0s，图6为t＝52.6s，大圆圈紧邻着的表示 基于卡尔曼滤波的后验位置。在图3中可以看到，由于此时水下机器人仿真模型 的右侧前方有障碍物，因此它选择偏离左侧的预定轨迹以避免碰撞；值得注意 的是，在图4中，结合卡尔曼滤波中的先验信息预测水下障碍物轨迹时，选择向 右行驶，碰撞概率较低。图5、图6表示在避免动态水下障碍物之后，水下机器 人很快回归到期望路径。

在图7中，可以看出，没有扩张状态观测器的路径跟踪总是存在明显的误差， 而采用本发明可以实现无误差跟踪。另外，可以看到，当水下机器人能够访问 到水下障碍物预测的未来轨迹时，它会选择与水下障碍物相反的运动方向(正x 轴方向)来避免碰撞。

为了进一步验证所提发明算法的有效性，在相同实验场景下添加多水下障 碍物进行进一步验证。

表1多障碍物仿真实验数据

从图8至图13多水下障碍物仿真截图来看，本发明可以稳定的实现在具备外 部扰动条件下跟踪曲线跟踪的同时稳定的避开多障碍物。其中，图8为t＝7s，图9为t＝10s，图10为t＝17s，图11为t＝23s，图12为t＝28s，图13为t＝37.2s，在水下 机器人预定的期望轨迹附近添加多个动态水下障碍物，对于每个添加的水下障 碍物，连续进行10次模拟实验，场景实验结果如表1表示，在将卡尔曼滤波的先 验结果融合进入非线性模型预测控制的预测模块中，在较短周期内对障碍物的 位置进行预测之后，会降低发生碰撞的概率，以及加大与障碍物的最近距离， 大大提升避障的鲁棒性。在图8至图13中，水下机器人仿真模型的起点在坐标系 的原点(0,0)；图8至图13中的曲线为全局跟踪路径

，各个圆圈表示初始位置不同的动态水下障碍物。

综上所述，本发明对于仿生水下机器人由于独特的推进方式而导致模型参 数不确定性以及外部环境干扰，采用改进的扩张状态观测器进行扰动观测，并 将其作为非线性模型预测控制扰动项进行不断计算更新，大大提升水下机器人 抗干扰能力，运动的鲁棒性；本发明针对动态障碍物状态观测不确定性，即观 测误差，声呐扫描周期过长，通过结合卡尔曼滤波先验估计，对短周期内的障 碍物未来位置进行预测，使得水下机器人碰撞概率降低，缩短行驶距离且扩大 了与障碍物的安全距离。

Claims (6)

1.一种基于NMPC的水下机器人路径跟踪与避障的控制方法，其特征在于：该方法包括下列顺序的步骤：

(1)建立水下机器人的运动学动力学模型，建立最优化问题的目标函数，设定全局路径参考点，利用贝塞尔曲线插值获得全局跟踪路径

计算得到艏摇角；

(2)测量水下机器人当前的位置，读取艏摇角，通过扩张状态观测器计算出当前水下机器人的速度，估计出外部扰动,同时采用声呐对水下障碍物状态进行扫描，并将探测结果输入卡尔曼滤波器；

(3)利用建立的运动学动力学模型，根据当前水下机器人的速度和外部扰动，对水下机器人未来N个控制周期的运动状态进行预测，并同时预测水下障碍物的短时间内的轨迹；

(4)根据预测得到的水下机器人未来N个控制周期的运动状态和水下障碍物的短时间内的轨迹，利用求解器对最优化问题进行求解并获得最优控制序列，并将最优控制序列首位转换作为水下机器人波动鳍推进器的控制输入信号；

(5)如果声呐扫描周期结束，通过声呐测量的水下障碍运动状态更新，则卡尔曼滤波器的输入更新，计算卡尔曼滤波器新的先验状态量与后验状态量，并且对水下障碍物的不确定性同样进行更新计算，如果声呐扫描周期没有结束则卡尔曼滤波器与水下障碍物不确定性不进行更新，保持不变；

(6)重复步骤(2)至步骤(5)，直至水下机器人进行路径跟踪且能完成避障。

2.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法，其特征在于：所述步骤(1)具体是指：

建立水下机器人三自由度下的运动学动力学模型：

其中，

为状态向量，V＝[u r v]^T表示水下机器人的速度，x和y代表大地坐标系下水下机器人的位置,

表示水下机器人艏摇角，u为水下机器人前进速度，v为水下机器人横移速度，r为水下机器人艏摇速度；

为控制输入量，f_u为控制输入纵倾力，f_v为横向驱动力，水下机器人为欠驱动系统，f_v＝0；f_r为偏航力矩；τ_d＝[d_u d_v d_r]^T为水下机器人外部扰动，d_u为水下机器人前进方向扰动力，d_v为水下机器人横移方向扰动力，d_r为艏摇方向的外部环境扰动力矩，M为水下机器人惯性质量矩阵，D为水下机器人阻尼力对角矩阵，C(v)为向心力和科里奥利矩阵，其公式如下：

其中，m₁₁、m₂₂、m₃₃均为水下机器人的附加质量参数，d₁₁、d₂₂、d₃₃均为水动力系数；

是将体坐标系下的水下机器人横移速度v转换为大地坐标系速度

的旋转矩阵，具体为：

建立最优化问题的目标函数：

subject to

u(t)∈U，x(0)＝x(t₀) (4)

其中，J_x(x(t),x_ref(t))为期望状态代价，其公式为：

J_u(Δu(t))为控制输入量代价，其公式为：

J_c(x(t)为碰撞代价，其公式为：

其中，T为水下机器人采样周期，f(x,u)是描述水下机器人连续非线性方程，d_i(t)表示为与第i个水下障碍物的欧氏距离，k_i>0为平滑避障轨迹的参数；

为期望状态代价项的权重系数矩阵，

为控制输入量代价的权重系数矩阵，Q_c表示碰撞代价系数；U是控制量力与力矩可输入范围，如下式所示：

水下机器人是欠驱动系统，则有f_vmin＝f_vmax＝0；f_vmin为水下机器人横向驱动力最小值，f_vmax为水下机器人横向驱动力最大值，f_umin为水下机器人纵向驱动力的最小值，f_umax为水下机器人纵向驱动力的最大值，f_rmin为水下机器人艏摇力矩的最小值，f_rmax为水下机器人艏摇力矩最大值。

3.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法，其特征在于：所述步骤(2)具体是指：

测量与计算水下机器人状态量x(t₀)，采用扩张状态观测器计算水下机器人速度、外部扰动状态：

其中，l₀至l₈皆为正整数，b₀,b₁为补偿因子，

与χ＝v·t分别代表体坐标系下的纵向与横向位移；

表示水下机器人艏摇角，u为水下机器人前进速度，v为水下机器人横移速度，r为水下机器人艏摇速度；f_u为控制输入纵倾力，f_r为偏航力矩；m₁₁、m₂₂、m₃₃均为水下机器人的附加质量参数，d₁₁、d₂₂、d₃₃均为水动力系数；通过分别求解式(6)、(7)、(8)，得出水下机器人当前的纵向速度

横向速度

艏摇速度

以及纵向

横向

艏摇

三个方向外部扰动的估计值，“∧”表示计算估计值，“·”表示微分符号。

4.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法，其特征在于：所述步骤(3)具体是指：利用水下机器人的运动学动力学模型对水下机器人未来状态进行计算预测得到

并且通过水下障碍物直线运动模型对水下障碍物状态进行预测：

其中，

为水下障碍物i的预测位置，T_s为采样周期，p_{obs i}(t₀)为水下障碍物当前时刻的测量位置，v^o-为水下障碍物测量先验速度，N为预测步数，t₀为预测开始当下时刻，往后预测n个T_s采样周期；

(3)通过运动学动力学模型对水下机器人未来N个控制周期的运动状态进行预测；采用公式(9)，通过声呐测量出的当前水下障碍物的位置，加上水下障碍物测量先验速度乘上采样周期T_s，获得未来n个T_s周期内的水下障碍物直线运动位置。

5.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法，其特征在于：所述步骤(4)具体是指：利用求解器对最优化问题进行求解并获得最优控制序列u^*(t+nT_s),n＝1,...,N-1，并将最优控制序列首位

转换作为水下机器人波动鳍推进器的控制输入信号，u^*表示通过求解器求解出来的最优控制序列。

6.根据权利要求1所述的基于NMPC的水下机器人路径跟踪与避障的控制方法，其特征在于：所述步骤(5)具体是指：如果声呐扫描周期结束，即水下障碍物的测量状态量更新，则对卡尔曼滤波器的水下障碍物先验状态量

与后验状态量P_t更新计算，并且对水下障碍物的不确定性r_th,k(t)进行计算更新，否则卡尔曼滤波器与水下障碍物不确定性不进行更新；

其中卡尔曼滤波器分为如下两大部分：

预测：

更新：

其中，x_t＝[x_obs,y_obs,v_obs]^T是水下障碍物的状态量，x_obs，y_obs为水下障碍物的坐标位置，v_obs为水下障碍物的速度，Z_t＝[x^o,y^o,v^o]^T是在t时刻的测量的水下障碍物状态量，x^o,y^o为水下障碍物测量的坐标位置，v^o为水下障碍物的测量速度，F是状态转移矩阵，H是观测矩阵，Q_t是水下障碍物运动噪声协方差矩阵，R_t为观测噪声协方差矩阵，I为单位矩阵；

通过求解计算

最大特征值δ_i(t)，通过公式(12)描述第i个水下障碍物在t时刻的不确定性：

r_th,i(t)＝r_th+10δ_i(t) (12)

其中，r_th为常量参数。

Images