CN116540723A - 一种基于人工势场的水下机器人滑模轨迹跟踪控制方法 - Google Patents

Abstract

本发明提供了一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，属于水下机器人轨迹跟踪控制技术领域。解决了水下机器人在水下检修控制过程中受水流冲击和动态障碍物而导致的路径跟踪与姿态不稳定的问题。其技术方案为：包括以下步骤：S1、建立机器人运动学模型；S2、确定目标位置，确定到达目标位置的运动轨迹；S3、设计双闭环滑模控制器，设计双闭环控制率；S4、根据公式对进行姿态和位置解算，通过上位机向主控制器模块发送控制指令，控制水下机器人达到期望位置。本发明的有益效果为：在面对干扰和障碍物时，有较好的鲁棒性，维持水下检修机器人的稳定，实现了精准的姿态位置控制，以确保能够在水下顺利作业，完成目标任务。

Description

一种基于人工势场的水下机器人滑模轨迹跟踪控制方法

技术领域

本发明涉及水下机器人控制技术领域，尤其涉及一种基于人工势场的水下机器人滑模轨迹跟踪控制方法。

背景技术

随着我国海洋强国战略的发展，近年来海上风电成为沿海各省大力发展的热点项目，海上风电设备水下桩腿结构件容易遭受海水腐蚀，造成事故损失。根据我国相关要求应对其定期检修，东部沿海海水的低能见度使得人工水下检修作业极为困难，后期的维修保护工程压力大，市场对水下检修机器人的需求日益增加，然而当水下检修机器人进行维修作业时，容易受到复杂的海洋环境影响，水下机器人的跟踪控制与姿态调整成为海上风电桩检修作业的难题。

水下机器人运动为强耦合非线性系统，局部流速、水流方向不稳定，造成动力系统刚度难以满足水下作业要求。针对水下风电桩检修的水下机器人较少，对与风电桩水下部分检修工作环境复杂而使用的控制方法不足。文献《Sliding mode control of anunderwater robot with a low-cost MEMS sensor》(Eduardo Mendes,Carlos Roque,RuiRocha,Nuno Cruz)(Ocean Engineering,Volume 125,15January 2016,Pages 123-134)本文提出了一种基于滑膜控制的方法，用于在水下机器人上使用低成本MEMS传感器进行姿态控制。通过将传感器的测量值作为反馈信号，可以实现机器人的姿态稳定和运动控制。该方法通过一系列数值模拟和实验验证，证明了其在水下机器人控制中的有效性和可行性。然而，此方法只使用了单一的滑模控制，在实际运用的过程中存在着较强的局限性，同时对于水下障碍物，尤其是动态障碍物不能完成自动避障功能，对机器人的危害较大。

综上，现有技术具有稳定性不高、灵活性较差、控制框架完整性欠缺等缺点，如何解决上述问题，是本发明的课题。

发明内容

本发明的目的在于提供一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，通过人工势场算法和双环滑模控制使水下机器人能够在复杂的水下环境有较高的灵活性和稳定性。

为了实现上述发明目的，本发明采用技术方案具体包括如下步骤：

S1、根据传感器系统模块感知水下环境，获取水下环境信息，获取水下机器人的状态并进行检测，获取机体姿态的欧拉角与位置参数，建立机器人运动学模型；

S2、基于人工势场算法，确定目标位置，确定到达目标位置的运动轨迹，采用差分进化算法达到了轨迹优化的目的；

S3、选择滑模变量，设计双闭环滑模控制器，设计双闭环控制率，构建Lyapunov函数进行稳定性分析；

S4、为实现对水下机器人运动状态的精准控制，对推进器所提供的推进器进行解算，根据公式对进行姿态和位置解算，通过上位机向主控制器模块发送控制指令，控制水下机器人达到期望位置。

步骤S1具体为：

在步骤S1中，根据传感器系统模块感知水下环境，获取水下环境信息，获取水下机器人的状态并进行检测，得到水下机器人平动、转动的动力学方程：

η′＝J(η)v

其中，M为惯性矩阵，m＝6，v∈R^m表示水下机器人的速度向量，v′∈R^m表示水下机器人的加速度向量，C(v)为科氏力和向心力矩阵，D(v)为水动力矩阵，g(η)为恢复力和力矩向量，表示水下机器人的位置和姿态向量，η′对水下机器人的位置和姿态向量求导，T表示6个推进器的控制分配矩阵，τ_j∈R^N，N＝6表示第J个推进器的控制输出推力，J(η)为转换矩阵，N为执行机构个数。

步骤S2具体为：

根据当前机器人的位置和速度，结合障碍物的位置和速度以及所期望的目标位置，确定达到期望目标位置的运动路径，基于人工势场算法，机器人同时受到目标点的引力和障碍物的斥力，其所受合力表达式为：

F(η)＝F_y(η)+∑F_r(η)

其中，向量F_y(η)为当前位置机器人所受目标的引力，方向由水下机器人质心指向目标点；向量F_r(η)为受到障碍物的斥力，方向由障碍物质心指向水下机器人的质心；F(η)为机器人所受到的合力，示水下机器人的位置和姿态向量；

设当前第i采样时刻的矢量坐标和步长为η(i)和σ，则机器人下一点坐标为

按此搜索，得水下机器人到达目标点的参考轨迹，采用差分进化算法，对参考轨迹进行优化，由于此差分进化算法为离散型算法，故对人工势场算法中所求出的运动轨迹式进行间隔采样，采样间隔时间为/>得到离散型参考轨迹为：

其中，表示在时刻/>对于η_τ的采样值，i＝1，2，...，n-1；

其中，表示修正角度，k表示差分进化第k次迭代，/>与参考轨迹的偏差；n为大于0的正整数，/>表示在时刻/>对于η_τ的采样值，i＝1，2，...，n-1。

步骤S3具体为：

对位置状态构建外环积分控制器，定义外环滑模函数为s_v，以位置状态作为虚拟控制输入量，设计其期望值η_d，使得当η＝η_d时对期望位置状态量进行无静差跟踪控制，设计含有跟踪误差积分的滑模面：

其中：k₁为增益矩阵，值为η_e为水下机器人的位置状态量跟踪误差，η_e＝η-η_d，考虑内环控制律的设计与分析，当v_e→0，存在接近于0实数ε＞0；

设计双闭环控制器的外环控制器的虚拟控制率为：

v_d＝η_d′+k₁η_e+ρ₁sgn(s_v)

其中，η_d为期望值η_d的导数。

定义Lyapunov函数为

其中，为S_v的转置矩阵；

求导得

其中，外环控制参ρ₁>||ε||，满所设计的双闭环控制器的外环控制器满足稳定性条件；

内环为速度环，将外环控制率的输出作为内环的速度指令，设计其期望值为外环控制器的虚拟控制率v_d，定义内环滑模面为：

式中k₂为增益矩阵，其值v_e为水下机器人的速度量跟踪误差，其中v_e＝v_d-v；

设计双闭环控制器的控制率τ为：

τ＝M(v_d′+k₂v_e)+C(v)v′+D(v)v′+g(η)+μS_n+ρ₂sgn(s_n)

其中μ＞0，内环控制率ρ₂＞||g(η)||，v_d′为外环控制器的虚拟控制率v_d的导数；

定义Lyapunov函数为：

其中，为S_v的转置矩阵；

求导得

所设计的双闭环控制器的内环控制器满足稳定性条件。

步骤S4具体为：

推进器的推力分配向量通过推力公式结合推进器的安装位置和安装角度计算得出；

将6台推进器的推力分配向量组合得到的推力分配矩阵T为

其中水平布置推进器至水下机器人重心长度L₁＝12.51mm，垂直布置推进器至水下机器人重心长度L₂＝7.31mm，推进器至水下机器人重心垂直距离Z_a＝0.12mm

下列公式推力系数表示推进器输入与推进器推力之间的关系；

式中τ_j,j＝1,2,…,N＝6为螺旋桨推力，m为螺旋桨转速，ρ为水的密度，D为螺旋桨直径，推进器系数H需要通过实测推力数据得出：

τ_j＝sgn(m)HρD⁴m²

根据推力进行姿态和位置解算，将运算结果发送给主控制器模块，由电源控制模块提供稳压电源；主控制器模块通过网络接收上位机的路径和姿态控制指令，控制机器人前进并将螺旋桨推力传回给上位机；机器人跟踪路径到达指定的水下风电桩，并开始对其进行检修作业。

与现有技术相比，本发明的有益效果为：

1、本发明针对现有的水下机器人的滑膜控制提供了一种水下检修机器人位置姿态控制方法，能够实现水下机器人能够完成既定路径的跟踪任务，提高水下机器人在工作时的安全性能。

2、本发明针对传统的滑模控制方法在处理水下机器人的控制问题时存在一些缺陷，如难以处理未知模型和干扰等问题，双环滑模控制能更好的提高水下机器人在运行中的稳定性，从而为水下探测、海底建设等领域的应用提供更好的支持。

3、本发明针对传统的人工势场中轨迹规划并不是每一步朝着目标点的方向，得到的路径并非最优，采用差分进化法，对参考轨迹式进行间隔采样，得到最优轨迹设计，有效避开水下障碍物。

4、本发明构建了上位机和传感器相配合的完整系统，框架完整可靠，有利于快速应用到水下机器人系统上快速应用，有广泛的应用前景。

附图说明

附图用来提供对本发明的进一步理解，并且构成说明书的一部分，与本发明的实施例一起用于解释本发明，并不构成对本发明的限制。

图1为本发明水下机器人示意图。

图2为本发明的整体流程图。

图3为本发明水下机器人上位机和传感器工作示意图。

图4为本发明水下机器人工作期望轨迹和实际运动轨迹示意图。

图5为本发明水下机器人到达目标位置各坐标响应示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。当然，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

实施例1

本实施例提供了一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，以此建立运动学模型，通过上位机和传感器系统结合，获取机体姿态的欧拉角与位置参数，建立人工势场模型，确定运动轨迹，采用差分进化算法达到了轨迹优化的目的。同时采用双闭环滑模控制，确保水下机器人在复杂水中坏境中能够稳定、安全、灵活地作业。

请参阅图1，图1是水下机器人示意图，此机器人具有6个推进器，4个推进器水平分布，2个推进器垂直分布，能够完成6个自由度的运动。

如图2所示，图2是本发明实施例1提供的水下机器人一种基于人工势场的水下机器人滑模轨迹跟踪控制方法的流程示意图：

所述水下机器人控制方法具体包括如下步骤：

S1、根据传感器系统模块感知水下环境，获取水下环境信息，获取水下机器人的状态并进行检测，获取机体姿态的欧拉角与位置参数，建立机器人运动学模型；

S2、基于人工势场算法，确定目标位置，确定到达目标位置的运动轨迹，采用差分进化算法达到了轨迹优化的目的；

S3、选择滑模变量，设计双闭环滑模控制器，设计双闭环控制率，构建Lyapunov函数进行稳定性分析；

S4、为实现对水下机器人运动状态的精准控制，对推进器所提供的推进器进行解算，根据公式对进行姿态和位置解算，通过上位机向主控制器模块发送控制指令，控制水下机器人达到期望位置。

步骤S1具体为：

在步骤S1中，根据传感器系统模块感知水下环境，获取水下环境信息，获取水下机器人的状态并进行检测，得到水下机器人平动、转动的动力学方程：

得到水下机器人平动、转动的动力学方程：

η′＝J(η)v

其中，M为惯性矩阵，m＝6，v∈R^m表示水下机器人的速度向量，v′∈R^m表示水下机器人的加速度向量，C(v)为科氏力和向心力矩阵，D(v)为水动力矩阵，g(η)为恢复力和力矩向量，表示水下机器人的位置和姿态向量，η′对水下机器人的位置和姿态向量求导，T表示6个推进器的控制分配矩阵，τ_j∈R^N，N＝6表示第j个推进器的控制输出推力，J(η)为转换矩阵，N为执行机构个数。

步骤S2具体为：

根据当前机器人的位置和速度，结合障碍物的位置和速度以及所期望的目标位置，确定达到期望目标位置的运动路径，基于人工势场算法，机器人同时受到目标点的引力和障碍物的斥力，其所受合力表达式为：

F(η)＝F_y(η)+∑F_r(η)

其中，向量F_y(η)为当前位置机器人所受目标的引力，方向由水下机器人质心指向目标点；向量F_r(η)为受到障碍物的斥力，方向由障碍物质心指向水下机器人的质心；F(η)为机器人所受到的合力，示水下机器人的位置和姿态向量；

设当前第i采样时刻的矢量坐标和步长为η(i)和σ，则机器人下一点坐标为

按此搜索，得水下机器人到达目标点的参考轨迹，采用差分进化算法，对参考轨迹进行优化，由于此差分进化算法为离散型算法，故对人工势场算法中所求出的运动轨迹式进行间隔采样，采样间隔时间为/>得到离散型参考轨迹为：

其中，表示在时刻/>对于η_τ的采样值，i＝1，2，...，n-1；

其中，表示修正角度，k表示差分进化第k次迭代，/>与参考轨迹的偏差；n为大于0的正整数，/>表示在时刻/>对于η_τ的采样值，i＝1，2，...，n-1。

步骤S3具体为：

对位置状态构建外环积分控制器，定义外环滑模函数为s_v，以位置状态作为虚拟控制输入量，设计其期望值η_d，使得当η＝η_d时对期望位置状态量进行无静差跟踪控制，设计含有跟踪误差积分的滑模面：

其中：k₁为增益矩阵，值为η_e为水下机器人的位置状态量跟踪误差，η_e＝η-η_d，考虑内环控制律的设计与分析，当v_e→0，存在接近于0实数ε＞0；

设计双闭环控制器的外环控制器的虚拟控制率为：

v_d＝η_d′+k₁η_e+ρ₁sgn(s_v)

其中，η_d为期望值η_d的导数。

定义Lyapunov函数为

其中，为S_v的转置矩阵；

求导得

其中，外环控制参ρ₁>||ε||，满所设计的双闭环控制器的外环控制器满足稳定性条件；

内环为速度环，将外环控制率的输出作为内环的速度指令，设计其期望值为外环控制器的虚拟控制率v_d，定义内环滑模面为：

式中k₂为增益矩阵，其值v_e为水下机器人的速度量跟踪误差，其中v_e＝v_d-v；

设计双闭环控制器的控制率τ为：

τ＝M(v_d′+k₂v_e)+C(v)v′+D(v)v′+g(η)+μS_n+ρ₂sgn(s_n)

其中μ＞0，内环控制率ρ₂＞||g(η)||，v_d′为外环控制器的虚拟控制率v_d的导数；

定义Lyapunov函数为：

其中，为S_v的转置矩阵；

求导得

所设计的双闭环控制器的内环控制器满足稳定性条件。

步骤S4具体为：

推进器的推力分配向量通过推力公式结合推进器的安装位置和安装角度计算得出；

将6台推进器的推力分配向量组合得到的推力分配矩阵T为

其中水平布置推进器至水下机器人重心长度L₁＝12.51mm，垂直布置推进器至水下机器人重心长度L₂＝7.31mm，推进器至水下机器人重心垂直距离Z_a＝0.12mm

下列公式推力系数表示推进器输入与推进器推力之间的关系；

式中τ_j,j＝1,2,…,N＝6为螺旋桨推力，m为螺旋桨转速，ρ为水的密度，D为螺旋桨直径，推进器系数H需要通过实测推力数据得出：

τ_j＝sgn(m)HρD⁴m²

根据推力进行姿态和位置解算，将运算结果发送给主控制器模块，由电源控制模块提供稳压电源；主控制器模块通过网络接收上位机的路径和姿态控制指令，控制机器人前进并将螺旋桨推力传回给上位机；机器人跟踪路径到达指定的水下风电桩，并开始对其进行检修作业。

如图3所示，图3是本发明实施例1提供的水下机器人上位机和传感器工作示意图；

所述传感器部分由距离传感器、姿态传感器、深度传感器构成，用于感知环境参数和位置姿态状态，同时照明系统与工业相机相配合，完成图像同传功能，将数据反馈给上位机，上位机根据反馈数据发送指令，控制机器人快速稳定地达到指定位置。

实施例2

如图4所示，为验证机器人姿态跟踪效果，对本发明提出的人工势场的水下机器人滑模轨迹跟踪控制在MATLAB进行仿真。设机器人初始姿态为[0，0，0]^T，期望姿态曲线设置为正弦曲线为了验证滑模的抗干扰能力和鲁棒性，加入均值为0，方差为4的高斯噪声，最后仿真如图4所示，图4为机器人姿态角度θ、φ、/>跟踪示意图，从图中可见水下机器人能够对期待的姿态角度θ_d、φ_d、/>进行跟踪，误差快速收敛到零，跟踪效果较好。

实施例3

如图5所示，为了验证水下机器人对位置状态的跟踪效果，以机器人重心为坐标原点建立坐标系，建系方向如图1所示，采用实施例1人工势场的水下机器人滑模轨迹跟踪控制方法进行验证。设定机器人初始位置[x，y，z]^T＝[0，1，3]^T，期望位置为[x_d，y_d，z_d]^T＝[1，1.7，2.5]^T，最后仿真如图5所示，图5为水下机器人到达目标位置各坐标响应示意图，从图中可见响应迅速稳定，位置跟踪效果较好。

综上，本发明采用的双环滑模控制的姿态和位置控制方法，在面对干扰时，有较好的鲁棒性，维持水下检修机器人的稳定，实现了精准的姿态位置控制，以确保能够在水下顺利开展水下风电桩检修作业，完成检修目标任务。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，其特征在于，包括以下步骤：

S1、根据传感器系统模块感知水下环境，获取水下环境信息，获取水下机器人的状态并进行检测，获取机体姿态的欧拉角与位置参数，建立机器人运动学模型；

S2、基于人工势场算法，确定目标位置，确定到达目标位置的运动轨迹，采用差分进化算法达到了轨迹优化的目的；

S3、选择滑模变量，设计双闭环滑模控制器，设计双闭环控制率，构建Lyapunov函数进行稳定性分析；

S4、对推进器所提供的推进器进行解算，根据公式对进行姿态和位置解算，通过上位机向主控制器模块发送控制指令，控制水下机器人达到期望位置。

2.根据权利要求1所述的一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，其特征在于，步骤S1具体为：

得到水下机器人平动、转动的动力学方程：

η′＝J(η)v

其中，M为惯性矩阵，m＝6，v∈R^m表示水下机器人的速度向量，v′∈R^m表示水下机器人的加速度向量，C(v)为科氏力和向心力矩阵，D(v)为水动力矩阵，g(η)为恢复力和力矩向量，表示水下机器人的位置和姿态向量，η′对水下机器人的位置和姿态向量求导，T表示6个推进器的控制分配矩阵，τ_j∈R^N，N＝6表示第j个推进器的控制输出推力，H(η)为转换矩阵，N为执行机构个数。

3.根据权利要求2所述的一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，其特征在于，步骤S2具体为：

根据当前机器人的位置和速度，结合障碍物的位置和速度以及所期望的目标位置，确定达到期望目标位置的运动路径，基于人工势场算法，机器人同时受到目标点的引力和障碍物的斥力，其所受合力表达式为：

F(η)＝F_y(η)+∑F_r(η)

其中，向量F_y(η)为当前位置机器人所受目标的引力，方向由水下机器人质心指向目标点；向量F_r(η)为受到障碍物的斥力，方向由障碍物质心指向水下机器人的质心；F(η)为机器人所受到的合力，示水下机器人的位置和姿态向量；

设当前第i采样时刻的矢量坐标和步长为η(i)和σ，则机器人下一点坐标为

按此搜索，得水下机器人到达目标点的参考轨迹，采用差分进化算法，对参考轨迹进行优化，由于此差分进化算法为离散型算法，故对人工势场算法中所求出的运动轨迹式进行间隔采样，采样间隔时间为/>得到离散型参考轨迹为：

其中，表示在时刻/>对于η_τ的采样值，i＝1，2，...，n-1；

其中，表示修正角度，k表示差分进化第k次迭代，/>与参考轨迹的偏差；n为大于0的正整数，/>表示在时刻/>对于η_τ的采样值，i＝1，2，...，n-1。

4.根据权利要求3所述的一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，其特征在于，步骤S3具体为：

对位置状态构建外环积分控制器，定义外环滑模函数为s_v，以位置状态作为虚拟控制输入量，设计其期望值η_d，使得当η＝η_d时对期望位置状态量进行无静差跟踪控制，设计含有跟踪误差积分的滑模面：

其中：k₁为增益矩阵，值为η_e为水下机器人的位置状态量跟踪误差，η_e＝η-η_d，考虑内环控制律的设计与分析，当v_e→0，存在接近于0实数ε＞0；

设计双闭环控制器的外环控制器的虚拟控制率为：

v_d＝η_d′+k₁η_e+ρ₁sgn(s_v)

其中，η_d为期望值η_d的导数；

定义Lyapunov函数为：

其中，为S_v的转置矩阵；

求导得：

其中，外环控制参ρ₁>||ε||，满所设计的双闭环控制器的外环控制器满足稳定性条件；

内环为速度环，将外环控制率的输出作为内环的速度指令，设计其期望值为外环控制器的虚拟控制率v_d，定义内环滑模面为：

式中k₂为增益矩阵，其值v_e为水下机器人的速度量跟踪误差，其中v_e＝v_d-v；

设计双闭环控制器的控制率τ为：

τ＝M(v_d′+k₂v_e)+C(v)v′+D(v)v′+g(η)+μS_n+ρ₂sgn(s_n)

其中μ＞0，内环控制率ρ₂＞||g(η)||，v_d′为外环控制器的虚拟控制率v_d的导数；

定义Lyapunov函数为：

其中，为S_v的转置矩阵；

求导得

所设计的双闭环控制器的内环控制器满足稳定性条件。

5.根据权利要求4所述的一种基于人工势场的水下机器人滑模轨迹跟踪控制方法，其特征在于，步骤S4具体为：

推进器的推力分配向量通过推力公式结合推进器的安装位置和安装角度计算得出；

将6台推进器的推力分配向量组合得到的推力分配矩阵T为

其中，水平布置推进器至水下机器人重心长度K₁＝12.51mm，垂直布置推进器至水下机器人重心长度L₂＝7.31mm，推进器至水下机器人重心垂直距离Z_a＝0.12mm；

下列公式推力系数表示推进器输入与推进器推力之间的关系；

式中τ_j,j＝1,2,…,N＝6为螺旋桨推力，m为螺旋桨转速，ρ为水的密度，D为螺旋桨直径，推进器系数H需要通过实测推力数据得出：

τ_j＝sgn(m)HρD⁴m²

根据推力进行姿态和位置解算，将运算结果发送给主控制器模块，由电源控制模块提供稳压电源；主控制器模块通过网络接收上位机的路径和姿态控制指令，控制机器人前进并将螺旋桨推力传回给上位机；机器人跟踪路径到达指定的水下风电桩，并开始对其进行检修作业。