[发明专利]水下清洗作业机器人未知环境路径规划方法

权利要求书

1.一种水下清洗作业机器人未知环境路径规划方法，其特征在于，包含下述步骤：1)通过加速度传感器测量值a(t)和陀螺仪角加速度传感器的测量值b(t)计算得到机器人当前运动速度V、机器人当前位置与目标点的距离D、机器人姿态角ω₁以及机器人姿态方位与目标方位的夹角Δω；同时通过水下激光测距传感器直接测量得到机器人距离障碍物的距离S和机器人前进方向与障碍物所构成的方位夹角Φ；

2)将D、ω₁、Δω、V进行模糊化处理后分别形成模糊变量D_F、ω_1F、ω_F、V_F并作为第一模糊控制器的输入变量；将S、Φ进行模糊化处理后分别形成模糊变量S_F、Φ_F作为第二模糊控制器的输入变量；

ω₁和Δω的论域均为[-180，180]°，Φ的论域为[-90，90]°，其中符号为负表示在逆时针旋转产生的夹角，符号为正表示顺时针旋转产生的夹角；对应的模糊量ω_1F、ω_F、Φ_F的语言值为[NL(负大)，NM(负中)，NS(负小)，ZO(零)，PS(负小)，PM(负中)，PL(负大)，]；D、S的论域为[0，15]，单位为米，对应的模糊量D_F、S_F的语言值为[VS(非常小)，S(小)，M(中)，L(大)，VL(非常大)]；V的论域为[0，2.8]m/s，对应的模糊量V_F的语言值为[S(慢)，M(中)，F(快)]；

第一模糊控制器采用带比例参数调整的自校正结构，输入量为D_F、ω_1F、ω_F、V_F，输出量为U_I和θ_I；

第一模糊控制器为两级机构，包括第一级模糊控制器和第二级比例模糊控制器；第一级模糊控制器输入变量为D_F和ω_F；输出变量为

第一级模糊控制器的规则表为：

第二级比例模糊控制器的输入量为ω_1F、V_F，输出量为U_I和θ_I，输入输出之间的关系为

UI=<λmin{U~I,KVF}>;]]>

θI=<λθ~I+(1-λ)ω1F>;]]>

其中<·>表示取整运算；K为常量，取值为2；

第二糊控制器用于避障控制，输入量为S_F、Φ_F，输出量为模糊控制量θ_II，θ_II的模糊语言值为[NL(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PS(正中)，PS(正大)]；

第二模糊控制器的规则表为：

3)第一模糊控制器和第二模糊控制器的输出量输入到第三模糊控制器，第三模糊控制器的模糊输出经过清晰化接口得到前进驱动力和旋转角的数值；

所述的第三模糊控制器的输入量为U_I、θ_I和θ_II；输出量为最终的模糊驱动力U_F和模糊旋转角度θ_F；U_F的模糊语言值为[VS(非常小)，S(小)，M(中)，L(大)，VL(非常大)]，θ_F的模糊语言值为[NL(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PS(正中)，PS(正大)]；其中正表示向左旋转，负表示向右旋转；

第三模糊控制的模糊规则表为：

所述的水下清洗作业机器人未知环境路径规划方法还包括一自适应学习评估步骤：该步骤通过一个自适应学习评估器对模糊输入变量的性能进行评估，并通过性能评估规则库进行推理从而动态改变第一模糊控制器和第二模糊控制器的控制规则，具体操作如下：

①当ω₂和D在T1时间内产生N次振荡，说明机器人陷入在U型障碍的区域内重复路径运动，则：在振荡曲线的拐点处，将第二模糊控制器的规则库对应规则由N改为P，由P改为N，即由向左转改为向右转，向右转改为向左转；

②当ω₁、ω₂和D在T2秒内变化率均小于p，说明机器人存在机械故障或者在前进方向上与障碍物发生接触而强迫停止，其中T2选取3-8中的一个值，p为5％，则：修改第三模糊控制器中的规则库中关于驱动力U_F的控制规则，使机器人后退1-3米，再修改关于θ_F的控制，使机器人旋转90°或者180°。

2.根据权利要求1所述的水下清洗作业机器人未知环境路径规划方法步骤1)中关于计算机器人运动参数的方法，其特征在于，ω₁初值为0°，初始位置坐标(x₀，y₀)＝(0，0)，t时刻的机器人姿态角ω₁为：

ω1=∫0T∫0Tb(t)dtdt;]]>

t时刻机器人的前进速度为：

V=(∫0Ta(t)sin(∫0T∫0Tb(t)dtdt)dt)2+(∫0Ta(t)cos(∫0T∫0Tb(t)dtdt)dt)2;]]>

t时刻机器人的位置坐标的横坐标x和纵坐标y为：

x=∫0T∫0Ta(t)sin(∫0T∫0Tb(t)dtdt)dtdt;]]>

y=∫0T∫0Ta(t)cos(∫0T∫0Tb(t)dtdt)dtdt;]]>

另外，路径目标点的坐标为(Dx，Dy)，为给定值，则可以进一步计算得到：

t时刻的机器人当前位置与目标点的距离D的表达式为：

D=(Dx-x)2+(Dy-y)2;]]>

t时刻的目标方位角为：

ω2=arccotDy-yDx-x;]]>

t时刻机器人姿态方位与目标方方位的夹角为：

Δω=ω1-ω2=ω1-arccotDy-yDx-x.]]>