CN101833333A - 水下清洗作业机器人未知环境路径规划方法 - Google Patents

Abstract

本发明提供了一种水下清洗作业机器人未知环境路径规划方法，该方法为一种未知环境下轨迹规划的自适应学习模糊控制方法。其特征在于模拟人的路径规划思维、避障思维、以及快速规划优先还是避障优先的智能决策思维，对应的提出三种不同的子模糊控制器用于处理三类问题，同时通过对被控机器人运动结果性能的评估，相应的修改模糊控制器的规则库，使之能够应对更复杂的外界扰动和有效地解决U型障碍和对称障碍的问题，使机器人在更复杂环境下也可以完成规划任务到达目的地。本发明能够克服水下复杂环境因素对机器人路径规划的不确定性影响，提高机器人的运动安全性，使机器人能够更好的适应水下复杂的工业环境。

Description

水下清洗作业机器人未知环境路径规划方法

技术领域

本发明属于工业自动化领域，涉及一种水下机器人未知环境路径规划方法，特别适用于水下复杂未知环境下的清洗作业机器人的路径规划。

技术背景

随着科学技术的不断发展，机器人在工业自动化领域中的应用日趋广泛，例如各种工业机器手，工业自动化生产线等。但是当需要围绕较大的目标主体进行作业时，固定式工业机器手和自动化生产线将不能满足要求，此时工业移动机器人以其具有大范围的活动能力的优点将可以完成任务。

然而，移动机器人如何运动以及选取怎样的路径运动以满足现场作业的要求又成为了工业作业任务完成的关键。工业现场的环境复杂，各种设备、管道、线路等就成为了阻碍机器人运动的障碍物，因此机器人很重要的一个任务是在运动过程中要避开各种障碍物并且尽快达到目的地。不仅如此，许多复杂的工业作业任务的环境是在水下，比如清洗火电厂的大型冷凝设备、清洗化工厂的排污设配、清洗自来水厂净化设备等。这些水下作业清洗任务直接关系到企业的节能程度和污水处理能力，然而进行人工清洗不但效率低而且必须将水排尽，这样便产生了大量水资源的浪费，因此水下清洗机器人的需要则更为迫切。

本专利提出的水下清洗作业机器人未知环境路径规划方法是在专利“冷凝器铜管二关节式在线清洗机器人”(公开号CN1945196)和“凝汽器智能移动清洗机器人”(CN101430176A)的基础上，以类似的机器人作为被控对象，提出一种路径规划方法，并推广到工业现场的水下作业领域。在复杂的水下环境中作业，需要更加提高移动机器人运动时的安全防范策略以及运动的精确性。例如水流速度、冲击力、压强等不确定环境因素在机器人路径规划控制中引入了很强的干扰和随机性，导致常规规划方法难以实现最佳的控制和高精度的定位。另外，因为工业现场设备的不规则，使得机器人如何处理复杂障碍例如U型障碍、对称障碍等也将是评价水下清洗机器人路径导航规划性能的关键。因此，如何设计一种快速、高精度、高可靠性、实时性强、具有实际操作性的水下清洗作业机器人未知环境路径规划方法是解决安全高效的水下工业清洗作业的关键难题。

发明内容

本发明所要解决的技术问题是：提供了一种水下清洗作业机器人未知环境路径规划方法。本路径规划方法可以实现在水下对机器人稳定性的控制、高效避障、快速完成规划，使机器人更趋于类人化、灵活化，同时可以处理U型陷阱障碍和复杂对称障碍问题，使机器人不会在U型障碍中无法脱离。

本发明的所采用的具体技术方案是：

一种水下清洗作业机器人未知环境路径规划方法，其特征在于，包含下述步骤：

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，输入输出之间的关系为：

$U_I=<\mathrm{\&lambda;}\min \{{\tilde{U}}_I,{\mathrm{KV}}_F\}>;$

${\mathrm{\&theta;}}_I=<\mathrm{\&lambda;}{\widetilde{\mathrm{\&theta;}}}_I+(1-\mathrm{\&lambda;}){\mathrm{\&omega;}}_{1F}>;$

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

λ为比例因子，表达式为 $\mathrm{\&lambda;}=F\left(D_F\right)=\left\{\begin{array}{cc}1& 4\&le;D_F\&le;16\\ 0.75& 3\&le;D_F<4\\ 0.5& 2\&le;D_F<3\\ 0.25& 1\&le;D_F<2\\ 0& 0\&le;D_F<1\end{array}\right.;$

第二模糊控制器用于避障控制，输入量为S_F、Φ_F，输出量为模糊控制量θ_H，θ_H定义的模糊语言值为[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°。

步骤1)中关于计算机器人运动参数的具体公式如下

ω₁初值为0°，初始位置坐标(x₀，y₀)＝(0，0)，t时刻的机器人姿态角ω₁为：

${\mathrm{\&omega;}}_1={\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt};$

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

$V=\sqrt{{\left({\&Integral;}_0^{T}a\left(t\right)\sin \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dt}\right)}^2+{\left({\&Integral;}_0^{T}a\left(t\right)\cos \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dt}\right)}^2};$

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

$x={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\sin \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dtdt};$

$y={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\cos \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dtdt};$

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

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

$D=\sqrt{{(\mathrm{Dx}-x)}^2+{(\mathrm{Dy}-y)}^2};$

t时刻的目标方位角为：

${\mathrm{\&omega;}}_2=\mathrm{arccot}\frac{\mathrm{Dy}-y}{\mathrm{Dx}-x};$

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

$\mathrm{\&Delta;\&omega;}={\mathrm{\&omega;}}_1-{\mathrm{\&omega;}}_2={\mathrm{\&omega;}}_1-\mathrm{arccot}\frac{\mathrm{Dy}-y}{\mathrm{Dx}-x}.$

有益效果：

1.将带有自适应学习特性的模糊控制器用于水下作业清洗智能机器人的路径规划问题上，使机器人规划的路径轨迹更趋向于人性化，满足人类期望的路径要求，同时具有更高的稳定性；

2.针对水下恶劣的作业环境，对机器人的运动速度和运动姿态进行可靠的控制，增加了机器人在水下的安全系数；

3.自适应学习功能可以使模糊控制器不断的改进控制规则，使机器人在学习中不断改善对U型障碍和对称障碍的处理方法，从而逃出障碍陷阱，使水下清洗作业机器人可以应对更加复杂多变的水下障碍环境。

该方法为一种未知环境下轨迹规划的自适应学习模糊控制方法。其特征在于模拟人的路径规划思维、避障思维、以及快速规划优先还是避障优先的智能决策思维，对应的提出三种不同的子模糊控制器用于处理三类问题，同时通过对被控机器人运动结果性能的评估，相应的修改模糊控制器的规则库，使之能够应对更复杂的外界扰动和有效地解决U型障碍和对称障碍的问题，使机器人在更复杂环境下也可以完成规划任务到达目的地。本发明能够克服水下复杂环境因素对机器人路径规划的不确定性影响，提高机器人的运动安全性，既具有模糊控制鲁棒性强的优点，同时又通过自适应学习功能避免了模糊控制规则单一、对环境适应度差的缺点，使机器人能够更好的适应水下复杂的工业环境。

附图说明

图1是本发明方法的系统结构图；

图2是机器人传感器信息变量示意图；

图3是模糊化隶属度函数图；【a图是ω₁(机器人姿态角)模糊化三角隶属度函数，b图是Δω(机器人姿态方位与目标方方位的夹角)模糊化三角隶属度函数，c图是Φ(机器人前进方向与障碍物所构成的方位夹角)模糊化三角隶属度函数，d图是V(机器人的速度)模糊化梯形隶属度函数，e图是D(机器人距目标点的距离)模糊化梯形隶属度函数，f图是S(机器人距障碍物的距离)模糊化梯形隶属度函数】

图4是第一模糊控制器(目标导航控制器)的结构图；

图5是第二模糊控制器(避障控制器)的结构图；

图6是第三模糊控制器(最优决策控制器)的结构图；

图7是第三模糊控制器的决策分析结果示意图；【a图表示障碍物在机器人右侧并且距离很近时，机器人决策分析后选择从左侧绕过，虽然路径长但是相对安全，b图表示障碍物在机器人右侧但是距离较远时，机器人决策分析后选择大角度右转从右侧绕过】

图8是机器人运动学模型示意图；【a图表示左右履带运动方向相同时机器人运动分析图，b图表示左右履带运动方向相反时机器人运动分析图】

图9是自适应学习评估器的结构图；

图10是自适应学习评估器处理U型障碍过程的分析示意图；【a图表示在U型障碍中机器人的运动轨迹，b图表示参数ω₂(目标方位角)对应左图轨迹运动时的数值变化，c图表示参数D(机器人距离目标的距离)对应左图轨迹运动时的变化】

图11是自适应学习评估器处理对称型障碍过程的分析示意图；【a图表示在对称障碍中机器人的运动轨迹，b图表示参数ω₁(机器人姿态角)和ω₂(目标方位角)对应左图轨迹运动时的数值变化，c图表示参数D(机器人距离目标的距离)对应左图轨迹运动时的变化。】

图12是本专利所使用的路径规划方法的实验仿真结果图。【a图是在第一种障碍环境下的路径仿真结果图，b图是在第二种障碍环境下的路径仿真结果图，c图是在第三种障碍环境(含复杂的U型陷阱障碍)下的路径仿真结果图】

图中：T-目标点，Q-障碍物，M-机器人。

具体实施方式

以下将结合图和具体实施过程对本发明做进一步详细说明：

实施例1：

本发明技术方案的总体框架图如图1所示，具体步骤为：1)通过计算加速度传感器和陀螺仪角加速度传感器的测量值计算得到机器人当前运动速度和位置坐标，同时通过水下激光测距传感器探测附近障碍物距离和方位角。

因为机器人在与履带轮垂直方向上的滑动很小，近似可以忽略不计，所以机器人的运动可以分解成机器人沿履带方向(前后方向)的直线运动和竖直轴的旋转运动。如图2(俯视图)所示，安装在机器人主体上的加速度传感器和陀螺仪传感器分别直接测量获得机器人前后方向的加速度(向前为正，向后为负)和竖直轴的角加速度。机器人在初始点进行位置调零，当机器人运动时对加速度和角加速度进行一次积分获得前进方向上的运动速度和竖直轴旋转角速度；对加速度二次积分并通过计算则可以获得机器人当前X、Y方向的相对位移坐标(x，y)、机器人姿态角ω₁、目标方位角ω₂以及机器人姿态方位与目标方位的夹角Δω。具体计算公式如下：

设加速度传感器(安装在机器人的中心点上并且与履带方向平行)的测量值为a(t)，陀螺仪角加速度传感器(安装在机器人的中心点上)测量值为b(t)(逆时针方向为负)，机器人姿态调零后的初始状态为平行于Y轴，即初始姿态角ω₁＝0°，初始位置坐标(x₀，y₀)＝(0，0)。以上a(t)，b(t)均是关于时间的函数，表示机器人的运动是一个变速过程，a(t)单位为m/s²，b(t)单位为°/s²。

①T时刻机器人的姿态角为：

${\mathrm{\&omega;}}_1={\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}$

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

$V=\sqrt{{\mathrm{Vx}}^2+{\mathrm{Vy}}^2}=\sqrt{{\left({\&Integral;}_0^{T}a\left(t\right)\sin \left({\mathrm{\&omega;}}_1\right)\mathrm{dt}\right)}^2+{\left({\&Integral;}_0^{T}a\left(t\right)\cos \left({\mathrm{\&omega;}}_1\right)\mathrm{dt}\right)}^2}$

$=\sqrt{{\left({\&Integral;}_0^{T}a\left(t\right)\sin \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dt}\right)}^2+{\left({\&Integral;}_0^{T}a\left(t\right)\cos \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dt}\right)}^2}$

T时刻机器人的位置坐标为：

$={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\sin \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dtdt}$

$y={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\sin (90-{\mathrm{\&omega;}}_1)\mathrm{dtdt}={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\cos \left({\mathrm{\&omega;}}_1\right)\mathrm{dtdt}$

$={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\cos \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dtdt}$

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

③T时刻机器人距离目标点的距离为：

$D=\sqrt{{(\mathrm{Dx}-x)}^2+{(\mathrm{Dy}-y)}^2}$

T时刻目标方位角为：

${\mathrm{\&omega;}}_2=\mathrm{arccot}\frac{\mathrm{Dy}-y}{\mathrm{Dx}-x}$

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

$\mathrm{\&Delta;\&omega;}={\mathrm{\&omega;}}_1-{\mathrm{\&omega;}}_2={\mathrm{\&omega;}}_1-\mathrm{arccot}\frac{\mathrm{Dy}-y}{\mathrm{Dx}-x}$

通过以上的计算得到了机器人的导航信息，另外使用激光测距传感器，以某一方向角度上发出激光信号，测量发出原信号与接收反射信号的时间差计算出当前方向上距离的障碍物距离。激光传感器能测量大角度范围内的障碍物距离，例如SICK(施克)公司的激光测距传感器LMS221、LMS211、LMS291、LMS200、LMS400等。

传感器直接测量输出机器人距离障碍物的距离S和机器人前进方向与障碍物所构成的方位夹角Φ。

最后，将得到的D、ω₁、Δω、V、S、Φ输入到步骤2)进行模糊化处理。其中D、ω₁、Δω、V进行模糊化处理后作为第一模糊控制器的输入变量；S、Φ进行模糊化处理后作为第二模糊控制器的输入变量。

2)将步骤1)的两组精确变量(即机器人位置姿态信息D、ω₁、Δω、V和障碍物信息S、Φ)进行模糊化处理后作为步骤3)、4)的输入；

步骤1)中已经得到的机器人位置姿态信息D、ω₁、Δω、V和障碍物信息S、Φ，现在通过模糊化接口转化为模糊变量D_F、ω_1F、ω_F、V_F和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(快)]。

如图3所示，用三角隶属度函数将Δω、ω₁、Φ转换成ω_F、ω_1F、Φ_F，利用梯形隶属度函数将D、S、V转换为D_F、S_F、V_F。

其中三角隶属度函数如下：梯形隶属度函数如下：

ω₁、Δω是机器人姿态的角度值，根据实际情况设定区间范围为

(-180，180]，如果计算后不在此区间范围则通过下式步骤进行转换：

步骤①：x∈(-180，180]是否成立，是则结束，否则转步骤②；

步骤②：如果x≤-180，则x+180→x，转步骤①，否则转步骤③；

步骤③：x-180→x，转步骤①。

障碍物的方位角Φ是来自激光测距传感器的直接测量输出，输出范围为(-90，90](不同传感器的角度测量范围不同)，按图示进行区间划分。

3)将步骤2)中模糊化处理后的用于导航控制的模糊量输入子第一模糊控制器进行导航推算，输出模糊量为前进轴方向的驱动力和导航旋转角；

第一模糊控制器用于完成轨迹最优规划和运动速度控制。同时，该机器人需要对轨迹末端姿态进行调整，即按设定旋转到相应的姿态。如图4所示，第一模糊控制器采用带比例参数调整的自校正结构，通过D_F调整控制比例因子λ，输入变量为D_F和ω_F；输出变量为

同时也是二级比例模糊控制的输入，

的论域和语言值定义与D_F相同，

的论域和语言值定义与ω_F相同。V_F输入变量用于机器人速度模糊控制、ω_1F则是机器人终态时姿态调整的模糊输入。通过对D_F的进行分析进而动态调整比例因子λ，使控制器在初始以及向目标前进时目标控制作用占主导，在终态进行姿态调整时不断减少目标控制的主导性使姿态调整控制器发挥主要作用。这样设计的好处是可以不仅可以快速的对轨迹进行规划，同时在从快速运动阶段过渡到轨迹末端姿态调整阶段时，使机器人的动作更加具有类人性，平稳性，消除过渡时明显的机械性。

定义规则库一并结合目标推理，制定以模糊变量D_F和ω_F为输入，

为输出的模糊控制矩阵查询表如下

表1

输出U_I和θ_I由下列规则计算得到：

$U_I=<\mathrm{\&lambda;}\min \{{\tilde{U}}_I,{\mathrm{KV}}_F\}>$

${\mathrm{\&theta;}}_I=<\mathrm{\&lambda;}{\widetilde{\mathrm{\&theta;}}}_I+(1-\mathrm{\&lambda;}){\mathrm{\&omega;}}_{1F}>$

比例因子λ以D_F为输入的函数如下：

$\mathrm{\&lambda;}=F\left(D_F\right)=\left\{\begin{array}{cc}1& 4\&le;D_F\&le;16\\ 0.75& 3\&le;D_F<4\\ 0.5& 2\&le;D_F<3\\ 0.25& 1\&le;D_F<2\\ 0& 0\&le;D_F<1\end{array}\right.$

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

由计算公式可以知道U_I取决于和KV_F的最小值，说明在对机器人的速度上实现了安全的控制；同时与比例因子λ相乘，使机器人在规划任务的末期逐步减小导航驱动力，使机器人最终停止。而θ_I是由

和ω_1F的比例输出，在任务初始和中间过程中，λ＝1，即此时以

为完全输出，体现了现在处于导航过程；当在规划任务末期时，λ逐渐减小，使

输出比例逐渐减小，ω_1F的输出比例逐渐增加，体现了机器人由导航状态向姿态调整状态的过度，正因为存在这种λ比例调节的机制，使机器人能够快速在导航过程末期就实现姿态的调整，而不必在导航完成后再在原地进行姿态调整，从而提高了任务的完成速度。4)将步骤2)中模糊化处理后的障碍物距离和方位角这两个模糊量输入第二模糊控制器用于避障控制，输出模糊量为避障旋转角；

如图5所示，第二模糊控制器的输出为模糊控制量θ_II，θ_II的模糊语言值为[NL(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PS(正中)，PS(正大)]，同时θ_H也是第三模糊控制器的输入。以S_F、Φ_F为输入，制定规则库二(避障规则库)，经过避障推理，建立模糊控制矩阵查询表如下：

表2

例如表2中，

体现的规则为：

障碍物距离机器人为S(小)，障碍物方位角为NM(左侧，与机器人前进方向夹角为中等大小)，则输出避障旋转控制量为PM(向右侧旋转中等程度角度)，这种规则是符合人类的思维方式的。另外当Φ_F＝ZO，即认为障碍物出现在机器人前进方向上时，默认规定向左旋转，从左侧绕开障碍物。

5)第一模糊控制器、第二模糊控制器的模糊输出量输入到子第三模糊控制器，第三模糊控制器则根据相应的规则库推算出最终的模糊输出，并且经过清晰化

(即去模糊化)接口得到前进驱动力和旋转角；

如图6所示，第三模糊控制器的输入变量是第一模糊控制器的输出变量U_I、θ_I和第二模糊控制器的输出变量θ_II，经过最优决策的模糊推理输出最终的模糊驱动力U_F和模糊旋转角度θ_F，再通过清晰化接口输出并经过建立的运动学参数转换模型转化为驱动左右履带液压马达的控制力矩M_L、M_R实现机器人的运动。第一模糊控制器是导航控制器，第二模糊控制器是避障控制器，在机器人向目标运动时轨迹计算和避障问题同时存在，因而两个模糊控制器时时刻刻都存在输出，第三模糊控制器的作用是如何最优的将控制器一、二的控制量进行模糊的调配，使其在完成良好避障的情况下，实现快速轨迹运动的目的。因而，第三模糊控制器实际为模糊决策器。

U_I、θ_I同为第一模糊控制器的输出，具有独立性，U_I、θ_I和θ_II的论域在控制器一、二中已经做了定义，模糊输出U_F的模糊语言值为[VS(非常小)，S(小)，M(中)，L(大)，VL(非常大)]，θ_F的模糊语言值为[NL(负大)，NM(负中)，NS(负小)，ZO(零)，PS(正小)，PS(正中)，PS(正大)]。

第三模糊控制器决策的规则可以归纳为表3、表4如下：

表3

表4

规则说明：机器人旋转角模糊控制量θ_F的存在着突变，例如

当θ_I＝PL时，θ_II＝NL，θ_F输出为NL，θ_II＝NM，θ_F输出为PL。这种突变的模糊规则，实际上就是模仿人在图7所示情况下对路径的最优选择，是选择最短路径到达目标还是以最安全的路径到达目标，在图中表示为是从障碍物左边慢慢绕过还是急转从右边快速到达。

在图7(左)中，虽然为了躲避障碍物会使机器人偏离目标点角度增大，但是由于障碍物距离机器人太近太正，为了安全也只能选择从上方绕过障碍物的方式。而在图7(右)中，恰恰可以选择从下方急转躲避障碍物朝目标点运动。以上这些模糊规则在规则表中即呈现出输出突变的状态(

)。当然，实际图示状况可能出现几率较低，因为根据其他规则，在障碍物还没有距离如此近的情况下，控制器已经控制机器人对准目标运动。6)建立机器人运动学控制模型，并通过该模型将步骤5)得到的理论推导下的前进驱动力和旋转角转换为实际履带机构中左右液压马达的驱动力，并对机器人的运动产生直接作用从而完成理论到实际的转化；

机器人运动模型建立过程分析如下：

当左右两轮的运动方向相同如图8(a)，当左右两轮的运动方向相反如图8(b)，根据相似三角形关系有

$V=\frac{V_L+V_R}{2}$ $\frac{V}{R}=\frac{V_R-V_L}{L}$

由上两式计算得

旋转半径 $R=\frac{V_L+V_R}{V_R-V_L}\&CenterDot;\frac{L}{2}$ 旋转角速度 $\mathrm{\&omega;}=\frac{V}{R}=\frac{V_R+V_L}{L}$

于是得到 $V_L=V-\frac{\mathrm{\&omega;L}}{2}$ $V_R=V+\frac{\mathrm{\&omega;L}}{2}$

其中V_L表示机器人左侧履带运动速度、V_R表示机器人右侧履带运动速度、V表示机器人实际运动速度、ω表示机器人实际旋转速度、L表示机器人左右履带的垂直距离、R表示机器人旋转的半径；V_L、V_R、V为正表示前进方向，为负表示后退方向；ω为正表示逆时针旋转，为负表示顺时针旋转。

机器人的马达可以进行速度控制，假设左右两马达的控制转速为n_MI、n_MR，则左右履带轮运动速度V_L、V_R与对应的马达呈比例关系：

V_L＝Kn_ML  V_R＝Kn_MR

其中K近似等于履带驱动轮半径(忽略履带厚度，因为履带厚度远小于履带驱动轮半径)。

于是进一步得到控制左右马达的转速为：

$n_{\mathrm{ML}}=\frac{1}{K}(V-\frac{\mathrm{\&omega;L}}{2})$ $n_{\mathrm{MR}}=\frac{1}{K}(V+\frac{\mathrm{\&omega;L}}{2})$

通过上两式，将模糊控制器的输出V和ω转换成了实际马达的控制转速，使该方法具有直接实现性和可操作性。其中L为机器人两轮的间距，K近似为履带轮半径。

7)与步骤3)、4)、5)并行处理的自适应学习评估器对模糊输入变量的性能进行评估，并通过性能评估规则库进行推理动态改变规则库二、三的控制规则，即用于避障控制和用于决策控制的规则。

如图10、图11左侧所示，实际工业现场往往具有复杂的环境而且障碍物的形状也使机器人难以利用常规的模糊规则避开障碍物或者逃出“陷阱”。自适应学习评估器则是为了解决这类问题，如图9所示为其原理示意图，以D、ω₁、ω₂(即步骤1)中的)、V、S、Φ为输入，评估器先判断当前机器人的运动特性和运动阶段，按照知识库相应的规则更新第二、第三模糊控制器的规则库，使新规则库更适应当前的运动过程的控制。如图10所示，a图表示在U型障碍中机器人的运动轨迹，b图表示参数ω₂(目标方位角)对应左图轨迹运动时的数值变化，c图表示参数D(机器人距离目标的距离)对应左图轨迹运动时的变化，三张图中A、B、C、D、E表示不同时间点。在U型障碍中机器人容易在D处继续重复上次的判断，即与在B点的判断相同，导致机器人沿B(D)→C路径重复在陷阱中(左图所示)，表现为ω₂和D相对于时间轴呈现出振荡的特点(b图和c图所示)。当检测到这种情况，可以更新第二模糊控制器的规则库(表2)，原为

更新后规则为

等，当ω₂和D的振荡现象消除后(达到轨迹中E点处)再更新为原规则。另外一种复杂的障碍如图11所示【图11的说明】，a图表示在对称障碍中机器人的运动轨迹，b图表示参数ω₁(机器人姿态角)和ω₂(目标方位角)对应左图轨迹运动时的数值变化，c图表示参数D(机器人距离目标的距离)对应左图轨迹运动时的变化，三张图中A、B、C、D、E、F、G、H、I表示不同时间点。虽然此时ω₁和ω₂呈现振荡特点(b图所示)，但是D却表现为轻微振荡收敛减小的趋势(c图所示)，说明机器人总体正朝目标接近(a图所示)，规划任务正常进行。

基于以上判断知识，最终为学习评估器添加如下知识库：

①如果：ω₂和D在t时间内产生N次振荡，说明机器人陷入在U型障碍的区域【此处检测的目的是判断机器人是否掉入如图10(a)所示的U型障碍中，对于U型障碍的区域大小并不关心】内重复路径运动，其中t的取值与机器人运动速度有关，取t＝5秒。N的数值则根据实时性要求而选取不同的值，N越小，则对陷阱判断越快，但N最小取1，一般可以取3；

那么：在振荡曲线的拐点处，将规则库二对应规则由N(P)改为P(N)，即由向左(右)转改为向右(左)转。

②如果：ω₁、ω₂和D在t秒内变化率均小于p，说明机器人存在机械故障或者在前进方向上与障碍物发生接触而强迫停止，(其中t一般选取3-8，p一般小于5％)

那么：修改规则库三中关于驱动力U_F的控制规则，使机器人低速【比如开始U_F＝L(表示前进驱动力大)，此时则是U_F＝VS(非常小)，则对应的机器人运动速度会变小；至于后退则只要改变电机旋转方向即可】后退1-3米，再修改关于θ_F的控制，使机器人旋转90°或者180°。

结合实践经验，在该知识库中增加更多规则可以增强机器人对更多的异常障碍情况的处理能力，增加本规划方法的稳定性和成功率。

实验结果与总结

本专利所述的一种水下清洗作业机器人未知环境路径规划方法在VC++.net2005下编程并通过ARIA(ActivMedia Robotics Interface for Applications)接口在模拟仿真平台MobileSim软件下的实验结果如图12所示。图12的a图是较简单障碍下的路径仿真结果；b图的障碍物分布更复杂，但机器人仍可以避开障碍物到达目标点；c图表明路径虽然存在摆动，但是机器人最终仍然有效地逃离了U型障碍。实验结果验证了该方法具有实际可操作性和具体实现性。

Claims (2)

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，输入输出之间的关系为

$U_I=<\mathrm{\&lambda;}\min \{{\tilde{U}}_I,{\mathrm{KV}}_F\}>;$

${\mathrm{\&theta;}}_I=<\mathrm{\&lambda;}{\widetilde{\mathrm{\&theta;}}}_I+(1-\mathrm{\&lambda;}){\mathrm{\&omega;}}_{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时刻的机器人姿态角ω₁为：

${\mathrm{\&omega;}}_1={\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt};$

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

$V=\sqrt{{\left({\&Integral;}_0^{T}a\left(t\right)\sin \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dt}\right)}^2+{\left({\&Integral;}_0^{T}a\left(t\right)\cos \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dt}\right)}^2};$

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

$x={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\sin \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dtdt};$

$y={\&Integral;}_0^T{\&Integral;}_0^{T}a\left(t\right)\cos \left({\&Integral;}_0^T{\&Integral;}_0^{T}b\left(t\right)\mathrm{dtdt}\right)\mathrm{dtdt};$

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

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

$D=\sqrt{{(\mathrm{Dx}-x)}^2+{(\mathrm{Dy}-y)}^2};$

t时刻的目标方位角为：

${\mathrm{\&omega;}}_2=\mathrm{arccot}\frac{\mathrm{Dy}-y}{\mathrm{Dx}-x};$

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

$\mathrm{\&Delta;\&omega;}={\mathrm{\&omega;}}_1-{\mathrm{\&omega;}}_2={\mathrm{\&omega;}}_1-\mathrm{arccot}\frac{\mathrm{Dy}-y}{\mathrm{Dx}-x}.$

Images