CN112147899B - 一种基于模糊滑模算法的水下机器人自主避障控制方法 - Google Patents

Abstract

本发明公开一种基于模糊滑模算法的水下机器人自主避障控制方法，属于水下机器人运动规划和轨迹跟踪技术领域，该方法在水下机器人通过加速、匀速和减速完成既定的规划路线基础上增加了洋流干扰、洋流测速以及避障部分。当遇到障碍物时，水下机器人控制系统根据水下机器人与障碍物的距离以及障碍物的大小，通过模糊滑模算法调节水下机器人的速度以及航向角，达到快速避障的效果。本方法可以有效降低水下机器人运动时的洋流干扰，保障水下机器人平稳安全运行。

Description

一种基于模糊滑模算法的水下机器人自主避障控制方法

技术领域

本发明涉及水下机器人运动规划和轨迹跟踪技术领域，尤其涉及一种基于模糊滑模算法的水下机器人自主避障控制方法。

背景技术

海洋总面积约占地球表面积的71％，丰富的海洋资源等待着人们探索和开发，水下机器人应运而生。水下机器人在未知的海洋环境下工作，除了完成既定的勘测任务之外，保障自身的安全也尤为重要。因此水下机器人不仅要具备轨迹跟踪能力，还要具备快速避障的能力。

水下机器人按照控制变量和被控自由度的数量关系分为三大类：全驱动水下机器人、过驱动水下机器人和欠驱动水下机器人。为了降低成本和减轻质量，本发明采用欠驱动水下机器人，欠驱动水下机器人具有能源消耗低和系统推进效率高的优点，但是欠驱动水下机器人系统具有较强的耦合性，模型参数不定，易受海洋环境影响，因此采用合适的控制算法不仅可以提高欠驱动水下机器人系统的鲁棒性，而且对水下机器人实现快速避障至关重要。

发明内容

针对上述现有技术的不足，本发明提供一种基于模糊滑模算法的水下机器人自主避障控制方法。

为解决上述技术问题，本发明所采取的技术方案是：一种基于模糊滑模算法的水下机器人自主避障控制方法，包括如下步骤：

步骤1：建立水下机器人在水平面的推力分布以及运动学模型；给定水下机器人的运动路线，行驶速度以及外界扰动，设置障碍物，障碍物的位置和大小未知；

所述水下机器人在水平面的运动学模型为：

V＝[u v r]

其中，M为包括水动力引起的附加重力的惯性矩阵；V为水下机器人的运动速度，其中u、v为直角坐标系中水下机器人在X、Y轴的速度分量，r为水下机器人在水平面的角速度；

为运动速度关于时间的导数；C(V)为包括水动力引起的附加重力的哥式力和向心力矩阵；D(V)为水阻力与动力矩阵；G为重力和浮力矩阵；T为外部施加的作用力和转矩，包括控制输入和外界扰动；

所述直角坐标系以水下机器人的质心点为坐标系原点，X轴平行于艇体基线指向艇首，Y轴平行于基面指向右舷。

所述水下机器人在水平面的推力分布为：

U＝[T₁ T₂]

其中，F为外部施加的作用力；Q为外部施加的转矩；L为转换矩阵；U为推进器推力向量，T₁为推进器1的推力，T₂为推进器2的推力；W为外界扰动。

步骤2：水下机器人根据给定的运动路线和行驶速度进行路径跟踪，同时由声纳检测其前方是否存在障碍物；

所述水下机器人根据给定的运动路线和行驶速度进行路径跟踪时，水平面轨迹跟踪误差表示为：

其中，Xe为水下机器人在X轴的位移跟踪误差，Ye为水下机器人在Y轴的位移跟踪误差,

为水下机器人的航向角跟踪误差，

为水下机器人在X轴的位移跟踪误差关于时间的导数，

为水下机器人在Y轴的位移跟踪误差关于时间的导数，

为水下机器人的航向角跟踪误差关于时间的导数。

步骤3：若声纳检测到障碍物时，根据声纳探测的水下机器人与障碍物之间的距离，采用模糊滑模算法调整水下机器人的速度以及航向角，完成避障行为；

所述步骤3中采用模糊滑模算法调整水下机器人的速度以及航向角的过程如下：

步骤3.1：根据水下机器人推力分布和水平面运动学模型，设置变量x₁、x₂和u_c；

步骤3.1.1：当对水下机器人的速度进行控制时，使变量x₁＝e_su,

建立状态方程：

其中，a₁，a₂和b为时变参数，f为外界扰动，e_su为水下机器人的期望速度u_ref与实际速度u的差值，

为速度调控时推进器的推力调整值关于时间的导数；

步骤3.1.2：当对水下机器人的航向角进行控制时，使变量x₁＝e_jiao,

u_c＝T″，建立的状态方程同步骤3.1.1，其中，e_jiao为水下机器人的期望航向角

与实际角度

的差值，T″为航向角控制时推进器的推力调整值；

步骤3.2：设计滑模控制器的切换函数以及选取滑模控制率；

步骤3.2.1.1：设计调节速度的滑模控制器的切换函数为：

s_su＝c_sux₁+x₂ c_su＞0

其中，c_su为常量，s_su为滑模控制器的切换函数；

步骤3.2.1.2：选取滑模控制率为：

u_c-su(t)＝ψ_1-sux₁+ψ_2-sux₂

其中：

其中，ψ_1-su和ψ_2-su为调节速度的滑膜控制器的增益，该增益值由模糊控制器选择，α_1-su、β_1-su、α_2-su、β_2-su均为变量；

步骤3.2.2.1：设计调节航向角的滑模控制器的切换函数为：

s_jiao＝c_jiaox₁+x₂ c_jiao＞0

其中，c_jiao为常量，s_jiao为滑模控制器的切换函数；

步骤3.2.2.2：选取滑模控制率为：

u_c-jiao(t)＝ψ_1-jiaox₁+ψ_2-jiaox₂

其中：

其中，ψ_1-jiao和ψ_2-jiao为调节航向角的滑膜控制器的增益，该增益值由模糊控制器选择，α_1-jiao、β_1-jiao、α_2-jiao、β_2-jiao均为变量；

步骤3.3：水下机器人的实际速度与期望速度的差值经模糊控制器调节输出调节速度的滑模模块的比例切换控制的增益，水下机器人的实际航向角与期望航向角的差值经模糊控制器调节输出调节航向角的滑模模块的比例切换控制的增益；

所述调节速度的滑膜控制器的输入变量为速度偏差e_su和偏差变化率ec，输出变量为α_1-su、α_2-su、β_1-su和β_2-su；设置e_su和ec的论域为[-p,p],α_1-su和α_2-su的论域均为[q,p]，β_1-su和β_2-su的论域均为[-p,-q]；模糊控制器的输出为o个语言值，构成一个输出模糊集合A，实际控制时需要从输出模糊集合中采取加权平均法判决出一个精确控制量，针对论域中的每个元素a_k，其中k＝1,2,…,o，以a_k作为待判决输出模糊集合A的隶属度μ_A(a_k)的加权系数，对输出模糊集合进行加权判断，得到调节速度的滑模控制器的增益结果g表示为：

得到调节航向角的模糊控制器的增益结果的方法与上述调节速度的滑膜控制器的增益相同。

步骤3.4：当水下机器人的期望速度u_ref与实际速度u的差值为x₁，x₂为x₁关于时间的导数时，根据滑模控制率得到u_c-su后，对时间积分即可得到T′；

步骤3.5：当水下机器人的期望航向角

与实际角度

的差值为x₁，x₂为x₁关于时间的导数时，根据滑模控制率得到的u_c-jiao，即为T″；

步骤3.6：推进器的推力T表示为：

T＝T′+T″。

步骤4：水下机器人躲避障碍物后需要根据当前位置与给定的运动路线中目标点的位置重新规划路线，匀速直线行驶向目标点，转置执行步骤2；

步骤5：若没有检测到障碍物，待水下机器人到达给定运动路线的目标点后，完成任务。

采用上述技术方案所产生的有益效果在于：

1、本发明充分考虑了水下机器人运动时外界环境的不确定因素，包括洋流干扰和未知障碍物；

2、本发明根据水下机器人的实际位置与规划路线的偏差，通过模糊滑模算法调节水下机器人的速度以及航向角，实现路径追踪；当检测到前方有障碍物时，根据水下机器人的实际位置与障碍物的距离，依照圆形避障规则重新规划路径，通过模糊滑模算法调节水下机器人的速度以及航向角，实现快速避障；

3、本发明在遇到障碍时，不仅可以实现快速稳定的避障处理，还可以降低水下机器人在避障转弯时左右摇摆的幅度，有效提高水下机器人行驶的安全系数。

附图说明

图1为本发明实施例中一种基于模糊滑模算法的水下机器人自主避障控制方法的流程图；

图2(a)为本发明实施例中欠驱动水下机器人的正视图；

图2(b)为本发明实施例中欠驱动水下机器人的俯视图

图2(c)为本发明实施例中欠驱动水下机器人的底部视图；

图3为本发明实施例中模糊滑模算法调整水下机器人的速度以及航向角的示意图；

图4(a)为本发明实施例中速度偏差e_su和偏差变化率ec模糊子集的隶属度函数；

图4(b)为本发明实施例中α_1-su、α_1-jiao、α_2-su和α_2-jiao模糊子集的隶属度函数；

图4(c)为本发明实施例中β_1-su、β_1-jiao、β_2-su和β_2-jiao模糊子集的隶属度函数；

图5为本发明实施例中欠驱动水下机器人位移运动曲线；

图6为本发明实施例中欠驱动水下机器人速度期望/实际曲线；

图7为本发明实施例中欠驱动水下机器人航向角期望/实际曲线；

图8为本发明实施例中洋流速度检测曲线；

其中，1、2和3均为声纳传感器，4和5为推进器，6为油囊，7为水下机器人艇体。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以下实施例用于说明本发明，但不用来限制本发明的范围。

如图1所示，本实施例的方法如下所述。本实施例中采用的欠驱动水下机器人如图2(a)所示，其俯视图如图2(b)所示，其底部视图如图2(c)所示。艇体前端装有一个声纳传感器，艇体两侧分别装有一个声纳传感器和推进器。本实施例根据声纳传感器反馈的位置信息，通过模糊滑模控制器调节水下机器人的速度和航向角，从而调节推进器的推力，实现水下机器人的路径追踪和避障功能。

步骤1：建立水下机器人在水平面的推力分布以及运动学模型；给定水下机器人的运动路线，行驶速度以及外界扰动，设置障碍物，障碍物的位置和大小未知；

所述水下机器人在水平面的运动学模型为：

V＝[u v r]

其中，M为包括水动力引起的附加重力的惯性矩阵；V为水下机器人的运动速度，其中u、v为直角坐标系中水下机器人在X、Y轴的速度分量，r为水下机器人在水平面的角速度；

为运动速度关于时间的导数；C(V)为包括水动力引起的附加重力的哥式力和向心力矩阵；D(V)为水阻力与动力矩阵；G为重力和浮力矩阵；T为外部施加的作用力和转矩，包括控制输入和外界扰动；

所述直角坐标系以水下机器人的质心点为坐标系原点，X轴平行于艇体基线指向艇首，Y轴平行于基面指向右舷。

所述水下机器人在水平面的推力分布为：

U＝[T₁ T₂]

其中，F为外部施加的作用力；Q为外部施加的转矩；L为转换矩阵；U为推进器推力向量，T₁为推进器1的推力，T₂为推进器2的推力；W为外界扰动。

本实施例中所述的外界扰动以洋流干扰为主。

步骤2：水下机器人根据给定的运动路线和行驶速度进行路径跟踪，同时由声纳检测其前方是否存在障碍物；

所述水下机器人根据给定的运动路线和行驶速度进行路径跟踪时，水平面轨迹跟踪误差表示为：

其中，Xe为水下机器人在X轴的位移跟踪误差，Ye为水下机器人在Y轴的位移跟踪误差,

为水下机器人的航向角跟踪误差，

为水下机器人在X轴的位移跟踪误差关于时间的导数，

为水下机器人在Y轴的位移跟踪误差关于时间的导数，

为水下机器人的航向角跟踪误差关于时间的导数。

本实施例中，水下机器人的规划路线如图5所示：从(0，0)出发行驶到目标点(100，100)，水下机器人在0-10m进行加速运动，在10-32m进行匀速运动，在32-42m进行减速运动。行驶到150s时出现洋流，此时控制系统进行自动调节且检测出洋流速度，如图8所示，检测到洋流速度为1.07m/s，10s后洋流消失；继续向目标点(100，100)驶去。

所述步骤2中当艇体前端的声纳传感器检测到障碍物和水下机器人的距离d₁小于给定的避障距离D时，即为检测到危险，进行避障处理；当d₁大于给定的避障距离D时，即为检测到无障碍，继续路径追踪。

步骤3：若声纳检测到障碍物时，根据声纳探测的水下机器人与障碍物之间的距离，采用模糊滑模算法调整水下机器人的速度以及航向角，完成避障行为；

所述步骤3中采用模糊滑模算法调整水下机器人的速度以及航向角的流程如图3所示，过程如下：

步骤3.1：根据水下机器人推力分布和水平面运动学模型，设置变量x₁、x₂和u_c；

步骤3.1.1：当对水下机器人的速度进行控制时，使变量x₁＝e_su,

建立状态方程：

其中，a₁，a₂和b为时变参数，f为外界扰动，e_su为水下机器人的期望速度u_ref与实际速度u的差值，

为速度调控时推进器的推力调整值关于时间的导数；

步骤3.1.2：当对水下机器人的航向角进行控制时，使变量x₁＝e_jiao,

u_c＝T″，建立的状态方程同步骤3.1.1，其中，e_jiao为水下机器人的期望航向角

与实际角度

的差值，T″为航向角控制时推进器的推力调整值；

步骤3.2：设计滑模控制器的切换函数以及选取滑模控制率；

步骤3.2.1.1：设计调节速度的滑模控制器的切换函数为：

s_su＝c_sux₁+x₂ c_su＞0

其中，c_su为常量，s_su为滑模控制器的切换函数；

步骤3.2.1.2：选取滑模控制率为：

u_c-su(t)＝ψ_1-sux₁+ψ_2-sux₂

其中：

其中，ψ_1-su和ψ_2-su为调节速度的滑膜控制器的增益，该增益值由模糊控制器选择，α_1-su、β_1-su、α_2-su、β_2-su均为变量；

步骤3.2.2.1：设计调节航向角的滑模控制器的切换函数为：

s_jiao＝c_jiaox₁+x₂ c_jiao＞0

其中，c_jiao为常量，s_jiao为滑模控制器的切换函数；

步骤3.2.2.2：选取滑模控制率为：

u_c-jiao(t)＝ψ_1-jiaox₁+ψ_2-jiaox₂

其中：

其中，ψ_1-jiao和ψ_2-jiao为调节航向角的滑膜控制器的增益，该增益值由模糊控制器选择，α_1-jiao、β_1-jiao、α_2-jiao、β_2-jiao均为变量；

步骤3.3：水下机器人的实际速度与期望速度的差值经模糊控制器调节输出调节速度的滑模模块的比例切换控制的增益，水下机器人的实际航向角与期望航向角的差值经模糊控制器调节输出调节航向角的滑模模块的比例切换控制的增益；

所述调节速度的滑膜控制器的输入变量为速度偏差e_su和偏差变化率ec，输出变量为α_1-su、α_2-su、β_1-su和β_2-su；设置e_su和ec的论域为[-p,p],α_1-su和α_2-su的论域均为[q,p]，β_1-su和β_2-su的论域均为[-p,-q]；模糊控制器的输出为o个语言值，构成一个输出模糊集合A，实际控制时需要从输出模糊集合中采取加权平均法判决出一个精确控制量，针对论域中的每个元素a_k，其中k＝1,2,…,o，以a_k作为待判决输出模糊集合A的隶属度μ_A(a_k)的加权系数，对输出模糊集合进行加权判断，得到调节速度的滑模控制器的增益结果g表示为：

得到调节航向角的模糊控制器的增益结果的方法与上述调节速度的滑膜控制器的增益相同。

步骤3.4：当水下机器人的期望速度u_ref与实际速度u的差值为x₁，x₂为x₁关于时间的导数时，根据滑模控制率得到u_c-su后，对时间积分即可得到T′；

步骤3.5：当水下机器人的期望航向角

与实际角度

的差值为x₁，x₂为x₁关于时间的导数时，根据滑模控制率得到的u_c-jiao，即为T″；

步骤3.6：推进器的推力T表示为：

T＝T′+T″。

本实施例中，在模糊化过程中，考虑到水下机器人的速度变化范围大，为进一步提高系统的调节能力，将速度偏差、偏差变化率的论域划分6个模糊集合，即负大(NB)，负中(NM)，负小(NS)，正小(PS)，正中(PM)，正大(PB)；将输出变量α_1-su、α_1-jiao、α_2-su和α_2-jiao的论域划分3个模糊集合，即正小(PS)，正中(PM)，正大(PB)；将输出变量β_1-su、β_1-jiao、β_2-su和β_2-jiao的论域划分3个模糊集合，即负大(NB)，负中(NM)，负小(NS)。考虑到输入输出量为连续值，因此根据相关知识和经验，并结合水下机器人实际控制对计算量的要求，选取三角形作为模糊控制器控制量的隶属度函数，速度偏差和偏差变化率的模糊子集的隶属度函数如图4(a)所示，输出变量α_1-su、α_1-jiao、α_2-su和α_2-jiao模糊子集的隶属度函数如图4(b)所示，输出变量β_1-su、β_1-jiao、β_2-su和β_2-jiao模糊子集的隶属度函数如图4(c)所示。输出变量α_1-su、α_1-jiao、α_2-su和α_2-jiao的模糊规则如表1所示，输出变量β_1-su、β_1-jiao、β_2-su和β_2-jiao的模糊规则如表2所示。

表1

表2

欠驱动水下机器人的速度期望/实际曲线如图6所示；欠驱动水下机器人的航向角期望/实际曲线如图7所示。

步骤4：水下机器人躲避障碍物后需要根据当前位置与给定的运动路线中目标点的位置重新规划路线，匀速直线行驶向目标点，转置执行步骤2；

步骤5：若没有检测到障碍物，待水下机器人到达给定运动路线的目标点后，完成任务。

Claims (5)

1.一种基于模糊滑模算法的水下机器人自主避障控制方法，其特征在于，包括如下步骤：

步骤1：建立水下机器人在水平面的推力分布以及运动学模型；给定水下机器人的运动路线，行驶速度以及外界扰动，设置障碍物，障碍物的位置和大小未知；

步骤2：水下机器人根据给定的运动路线和行驶速度进行路径跟踪，同时由声纳检测其前方是否存在障碍物；

步骤3：若声纳检测到障碍物时，根据声纳探测的水下机器人与障碍物之间的距离，采用模糊滑模算法调整水下机器人的速度以及航向角，完成避障行为；

所述采用模糊滑模算法调整水下机器人的速度以及航向角的过程如下：

步骤3.1：根据水下机器人推力分布和水平面运动学模型，设置变量x₁、x₂和u_c；

步骤3.1.1：当对水下机器人的速度进行控制时，使变量

建立状态方程：

其中，a₁，a₂和b为时变参数，f为外界扰动，e_su为水下机器人的期望速度u_ref与实际速度u的差值，

为速度调控时推进器的推力调整值关于时间的导数；

步骤3.1.2：当对水下机器人的航向角进行控制时，使变量x₁＝e_jiao,

建立的状态方程同步骤3.1.1，其中，e_jiao为水下机器人的期望航向角

与实际角度

的差值，T″为航向角控制时推进器的推力调整值；

步骤3.2：设计滑模控制器的切换函数以及选取滑模控制率；

步骤3.2.1.1：设计调节速度的滑模控制器的切换函数为：

s_su＝c_sux₁+x₂ c_su＞0

其中，c_su为常量，s_su为滑模控制器的切换函数；

步骤3.2.1.2：选取滑模控制率为：

u_c-su(t)＝ψ_1-sux₁+ψ_2-sux₂

其中：

其中，ψ_1-su和ψ_2-su为调节速度的滑膜控制器的增益，该增益值由模糊控制器选择，α_1-su、β_1-su、α_2-su、β_2-su均为变量；

步骤3.2.2.1：设计调节航向角的滑模控制器的切换函数为：

s_jiao＝c_jiaox₁+x₂ c_jiao＞0

其中，c_jiao为常量，s_jiao为滑模控制器的切换函数；

步骤3.2.2.2：选取滑模控制率为：

u_c-jiao(t)＝ψ_1-jiaox₁+ψ_2-jiaox₂

其中：

其中，ψ_1-jiao和ψ_2-jiao为调节航向角的滑膜控制器的增益，该增益值由模糊控制器选择，α_1-jiao、β_1-jiao、α_2-jiao、β_2-jiao均为变量；

步骤3.3：水下机器人的实际速度与期望速度的差值经模糊控制器调节输出调节速度的滑模模块的比例切换控制的增益，水下机器人的实际航向角与期望航向角的差值经模糊控制器调节输出调节航向角的滑模模块的比例切换控制的增益；

步骤3.4：当水下机器人的期望速度u_ref与实际速度u的差值为x₁，x₂为x₁关于时间的导数时，根据滑模控制率得到u_c-su后，对时间积分即可得到T′；

步骤3.5：当水下机器人的期望航向角

与实际角度

的差值为x₁，x₂为x₁关于时间的导数时，根据滑模控制率得到的u_c-jiao，即为T″；

步骤3.6：推进器的推力T表示为：T＝T′+T″；

步骤4：水下机器人躲避障碍物后需要根据当前位置与给定的运动路线中目标点的位置重新规划路线，匀速直线行驶向目标点，转至执行步骤2；

步骤5：若没有检测到障碍物，待水下机器人到达给定运动路线的目标点后，完成任务。

2.根据权利要求1所述的一种基于模糊滑模算法的水下机器人自主避障控制方法，其特征在于：所述步骤1中水下机器人在水平面的运动学模型为：

V＝[u v r]

其中，M为包括水动力引起的附加重力的惯性矩阵；V为水下机器人的运动速度，其中u、v为直角坐标系中水下机器人在X、Y轴的速度分量，r为水下机器人在水平面的角速度；

为运动速度关于时间的导数；C(V)为包括水动力引起的附加重力的哥式力和向心力矩阵；D(V)为水阻力与动力矩阵；G为重力和浮力矩阵；T为外部施加的作用力和转矩，包括控制输入和外界扰动；

所述步骤1中水下机器人在水平面的推力分布为：

U＝[T₁ T₂]

其中，F为外部施加的作用力；Q为外部施加的转矩；L为转换矩阵；U为推进器推力向量，T₁为推进器1的推力，T₂为推进器2的推力；W为外界扰动。

3.根据权利要求2所述的一种基于模糊滑模算法的水下机器人自主避障控制方法，其特征在于：所述直角坐标系以水下机器人的质心点为坐标系原点，X轴平行于艇体基线指向艇首，Y轴平行于基面指向右舷。

4.根据权利要求1所述的一种基于模糊滑模算法的水下机器人自主避障控制方法，其特征在于：所述水下机器人根据给定的运动路线和行驶速度进行路径跟踪时，水平面轨迹跟踪误差表示为：

其中，Xe为水下机器人在X轴的位移跟踪误差，Ye为水下机器人在Y轴的位移跟踪误差,

为水下机器人的航向角跟踪误差，

为水下机器人在X轴的位移跟踪误差关于时间的导数，

为水下机器人在Y轴的位移跟踪误差关于时间的导数，

为水下机器人的航向角跟踪误差关于时间的导数。

5.根据权利要求1所述的一种基于模糊滑模算法的水下机器人自主避障控制方法，其特征在于：所述调节速度的滑膜控制器的输入变量为速度偏差e_su和偏差变化率ec，输出变量为α_1-su、α_2-su、β_1-su和β_2-su；设置e_su和ec的论域为[-p,p],α_1-su和α_2-su的论域均为[q,p]，β_1-su和β_2-su的论域均为[-p,-q]；模糊控制器的输出为o个语言值，构成一个输出模糊集合A，实际控制时需要从输出模糊集合中采取加权平均法判决出一个精确控制量，针对论域中的每个元素a_k，其中k＝1,2,…,o，以a_k作为待判决输出模糊集合A的隶属度μ_A(a_k)的加权系数，对输出模糊集合进行加权判断，得到调节速度的滑模控制器的增益结果g表示为：

得到调节航向角的模糊控制器的增益结果的方法与上述调节速度的滑膜控制器的增益相同。

Images