CN113341975B - 基于变目标函数萤火虫优化路径的机器人移动方法 - Google Patents

Abstract

基于变目标函数萤火虫优化路径的机器人移动方法，包括以下步骤：S1、构建路径规划场景；S2、设计两个不同的目标函数；S3、启用搜索终点目标函数作为初始目标函数；S4、判断机器人终点方向有无障碍物，若无障碍物则机器人直接向终点移动，若有障碍物，启动萤火虫算法搜索位置；S5、判断移动后的机器人是否满足启用逃离陷阱目标函数条件，若满足则启用逃离陷阱目标函数，若不满足则保持目标函数不变；S6、判断移动后的机器人是否满足启用搜索终点目标函数条件，若满足则启用搜索终点目标函数，若不满足则保持目标函数不变；S7、判断机器人是否到达终点，若成立则结束，若不成立则返回步骤S4。通过本发明提出的方法，使得萤火虫算法不仅能逃离出局部陷阱还能搜索到终点。

Description

基于变目标函数萤火虫优化路径的机器人移动方法

技术领域

本发明涉及一种机器人移动方法，尤其涉及基于变目标函数萤火虫优化路径的机器人移动方法。

背景技术

路径规划是移动机器人导航中的关键技术之一，现有的路径规划优化算法可分为两类：动态路径规划算法和静态路径规划算法。在动态路径规划中，移动机器人面临着躲避障碍物，逃离局部最优陷阱和到达目标点等多种挑战。为了应对上述复杂任务挑战，机器人动态路径规划算法研究的几个主要环节包括：(1)构建路径规划场景模型；(2)设计特定的最小化目标函数；(3)采用特定优化算法来求解目标函数，得出最优解，从而得到移动机器人下一步移动的位置。其中，路径规划的目标函数和优化算法对路径规划的结果影响较大。现有的动态路径规划目标函数通常设计为：将移动机器人和障碍物之间距离的倒数与移动机器人和目标点之间距离进行线性加权，采用这样一种的目标函数能同时表征机器人向目标点移动和避开障碍物的需求。智能启发式算法在机器人路径规划上是一种重要的优化方法，应对复杂场景有一定优势。常规的遗传算法、粒子群算法在机器人路径规划中得到了不断发展，新型的萤火虫算法由于算法参数少，优化能力强，特别是针对多峰函数的优化具有较好的优势。因此，萤火虫算法在路径规划中也得到了较好地应用。

在动态路径规划中，由于环境信息的未知，容易导致移动机器人陷入局部最优陷阱。尤其是当存在凹形障碍物，并且移动机器人的初始点和目标点的连线穿越障碍物，目标点设置在障碍物的背后时，机器人更容易陷入凹形障碍物中，不能顺利到达目标点。为了解决上述问题，学者们也提出了一些改进方法，并大致可以总结为如下三类：第一种是在机器人移动过程中引入振荡函数，使机器人逃出陷阱；第二种是在机器人移动过的路径上添加虚拟障碍物，并依靠虚拟障碍物将机器人排斥到陷阱外部；第三种是通过设立虚拟子目标点从而引导机器人逃出局部最优陷阱。然而，上述第一种和第二种改进方法具有不可预测的特点从而不能确保机器人可以逃离出陷阱，第三种改进方法需要依靠全局信息才能设立合理的虚拟子目标点。本发明专利提出一种基于变目标函数萤火虫优化路径的机器人移动方法，用于指导机器人逃出局部最优陷阱和躲避动态障碍物。

发明内容

本发明主要解决上述现有技术的不足。结合路径规划问题的特点，提供一种基于变目标函数萤火虫优化路径的机器人移动方法。

基于变目标函数萤火虫优化路径的机器人移动方法包括以下步骤：

S1、构建路径规划场景；S2、设计两个不同的目标函数：搜索终点目标函数f₁、逃离陷阱目标函数f₂；S3、启用搜索终点目标函数作为萤火虫算法的初始目标函数，并计算搜索终点目标函数值记为f₁(0)；S4、判断机器人当前位置与终点连线在一个移动步长内有无障碍物，若没有障碍物则机器人直接向终点方向移动一个步长，若有障碍物，按照所选目标函数启动萤火虫算法搜索下一步位置，机器人并移动到该计算位置，计算搜索终点目标函数值记为f₁(s)；S5、判断移动后的机器人是否满足启用逃离陷阱目标函数的条件，若满足则将萤火虫算法的目标函数设置为逃离陷阱目标函数，并计算机器人此时位置与终点实时位置距离记为d_trap，若不满足则保持目标函数不变；S6、判断移动后的机器人是否满足启用搜索终点目标函数的条件，若满足则将萤火虫算法的目标函数设置为搜索终点目标函数，若不满足则保持目标函数不变；S7、判断机器人是否到达终点，若成立结束路径规划，若不成立则返回步骤S4往复执行其余步骤直至机器人到达终点；

进一步的，步骤S2所述的搜索终点目标函数f₁为：

f₁＝k₁d_rt+k₂(1/d_ro)+k₃θ (1)

其中，k₁，k₂和k₃为加权系数，d_rt为机器人当前位置到终点实时位置的距离，d_ro为机器人与附近障碍物的最小距离，θ为机器人当前移动步的运动方向与前一步的运动方向之间的夹角；

进一步的，步骤S2所述的逃离陷阱目标函数f₂为：

f₂＝k₄|d_ro-d_safe|+k₅d_f+k₆θ (2)

其中，k₄，k₅和k₆为加权系数，d_ro为机器人运动过程中与附近障碍物的最小距离，d_safe为机器人与障碍物之间的安全距离，取常数，θ为机器人当前移动步的运动方向与前一步的运动方向之间的夹角，d_f为机器人在移动方向上与障碍物的距离；

进一步的，步骤S4所述的计算搜索终点目标函数值：当机器人通过两种方式完成一步移动后都需要计算搜索终点目标函数值并记为f₁(s)，其中s为机器人的移动步数，两种移动方式包括：机器人直接向终点方向移动一个步长和机器人通过萤火虫算法搜索到下一步位置并移动到该位置；

进一步的，步骤S5所述的启用逃离陷阱目标函数的条件为：当前萤火虫算法的目标函数为搜索终点目标函数，并且搜索终点目标函数值满足条件：

f₁(s)＜f₁(s-1) (3)

其中，f₁(s)为步骤S4中机器人移动后的搜索终点目标函数值，f₁(s-1)为步骤S4中机器人移动前的搜索终点目标函数值；

进一步的，步骤S6所述的启用搜索终点目标函数的条件为：当前萤火虫算法的目标函数为逃离陷阱目标函数，并且满足如下条件：

d_rt≤(d_trap-d_gap) (4)

其中，d_trap为步骤S5中切换为逃离陷阱目标函数时机器人位置与终点实时位置之间的距离，d_rt为机器人实时位置与终点实时位置之间的距离，d_gap为常数。

与现有技术相比，本发明的优点在于：本发明涉及萤火虫算法在动态路径规划中设计了两个不同的目标函数，分别用于搜索终点和逃离陷阱。在动态路径规划中通过监测目标函数值的变化规律来判断机器人是否进入局部最优陷阱，通过监测机器人与终点的距离是否变近来判断机器人是否已经逃离出局部陷阱，机器人在局部最优陷阱内时，启用逃离陷阱目标函数作为其目标函数，机器人在其余时刻均启用搜索终点目标函数作为其目标函数。通过在动态路径规划的不同状态选择相应的目标函数，使得机器人不仅可以逃离出凹形局部最优陷阱、躲避动态障碍物还可以搜索到动态终点。

附图说明

图1为本发明实施例中该方法路径规划技术流程图。

图2为本发明实施例中使用该发明方法的动态路径规划效果图。

图3为本发明实施例中机器人偏转角度θ的计算方法图。

图4为本发明实施例中机器人陷入局部最优陷阱处目标函数值变化曲线图。

具体实施方式

为了便于理解本发明，下文将结合说明书附图和较佳的实施例对本发明作更全面、细致地描述，但本发明的保护范围并不限于以下具体的实施例。

实施例：

本实施例的基于变目标函数萤火虫优化路径的机器人移动方法，总体技术图，如图1所示，包括以下步骤：

S1、构建路径规划场景，本实施例的路径规划场景模型，如图2所示。其中，路径规划计算域为500*500的像素点正方形区域，相邻两个像素点的距离代表1cm物理距离，因此环境地图的区域为500cm*500cm。以场景的左下角为原点，以水平方向为x轴，以垂直方向为y轴，以单位长度为1cm建立坐标系。其中起点位置坐标为(10,10)，终点位置初始坐标为(400,480)，圆形障碍物中心点的初始坐标为(100,200)，半径为50cm，凹形陷阱由三个矩形障碍物构成，三个矩形障碍物中心点的初始坐标分别为：(290,360)、(290,360)和(290,360)，三个矩形障碍物的长度与宽度分别为：(20cm,40cm)、(20cm,40cm)和(170cm,20cm)。场景中的所有障碍物均为动态障碍物，并且终点也处于运动状态，并且其运动方向在图2中用带箭头的虚线进行了描述，圆形障碍物的移动速度为0.04m/s，终点和陷阱障碍物的移动速度为0.02m/s。

S2、设计两个不同的目标函数：搜索终点目标函数f₁、逃离陷阱目标函数f₂，其中，搜索终点目标函数f₁的计算方法如下：

f₁＝k₁d_rt+k₂(1/d_ro)+k₃θ (1)

其中，k₁＝1，k₂＝0.01和k₃＝0.0001为选取的较优加权系数，d_rt为机器人到终点的距离，计算方法为

(x_r,y_r)为机器人位置坐标，(x_t,y_t)为终点实时位置坐标，终点实时位置坐标可以通过S1步骤的信息计算得到，例如：终点位置初始坐标为(400,480)，运动速度为0.02m/s，运动方向沿着x轴负方向，当时间t＝10s时，终点实时位置坐标为(380，480)，d_ro为机器人与附近障碍物的最小距离，d_ro依靠激光雷达循环扫描测量，激光雷达以1°为间隔循环扫描360°范围，因此，激光雷达的扫描数据有360个并对应360个角度，通过比较得出360个数据中的最小值就是d_ro的数值，θ为机器人当前移动步的运动方向与前一步的运动方向之间的夹角，θ的计算方法如图3所示。

进一步的，逃离陷阱目标函数f₂的计算方法如下：

f₂＝k₄|d_ro-d_safe|+k₅d_f+k₆θ (2)

其中，k₄＝1，k₅＝-20和k₆＝0.1为选取的较优加权系数，d_ro为机器人与附近障碍物的最小距离，d_ro依靠激光雷达循环扫描测量，激光雷达以1°为间隔循环扫描360°范围，因此，激光雷达的扫描数据有360个并对应360个角度，通过比较算出360个数据中的最小值就是d_ro的数值，d_safe为机器人与障碍物之间的安全距离常数，本案例中设置为10cm，θ为机器人当前移动步的运动方向与前一步的运动方向之间的夹角，θ的计算方法如图3所示，d_f为机器人在移动方向上与障碍物的距离，d_f依靠激光雷达单个方向的扫描测量结果来获取。

S3、启用搜索终点目标函数作为萤火虫算法的初始目标函数，并计算搜索终点目标函数值记为f₁(0)。

S4、判断机器人当前位置与终点连线在一个移动步长内有无障碍物，若没有障碍物则机器人直接向终点方向移动一个步长，案例中机器人的移动步长为10cm；若有障碍物，按照所选目标函数启动萤火虫算法搜索下一步位置，机器人并移动到该计算位置，萤火虫算法搜索下一步位置过程如下：

S41、按照所选的目标函数作为萤火虫算法的目标函数；

S42、利用当前所选的目标函数计算每只萤火虫的亮度，在萤火虫算法中，每只萤火虫代表一个候选解，萤火虫的亮度为候选解的目标函数值，萤火虫数量N＝50；

S43、对所有萤火虫进行两两的亮度对比，若满足条件I_i＜I_j，i和j的变化范围为1至50。说明萤火虫i的亮度低于萤火虫j，亮度低的萤火虫i将向亮度高的萤火虫j靠近，靠近公式为：

其中，x_i(t)和x_j(t)分别为萤火虫i和萤火虫j的当前位置，x_i(t+1)为萤火虫i移动后的位置，β₀＝0.2为固定系数，γ＝0.5控制算法的收敛速度，α＝0.5为固定系数，rand为范围在[0,1]之间的随机数；

为两只萤火虫之间的距离。

S44、当萤火虫移动后重新计算萤火虫亮度，萤火虫的亮度取决于萤火虫所对应路径的目标函数值。

S45、判断萤火虫算法是否达到迭代次数，萤火虫算法的迭代次数为50次，若未达到迭代次数则返回步骤S43往复执行，若萤火虫算法达到迭代次数后，对萤火虫算法所有的候选解进行排序并输出最优解，此最优解就是机器人下一步的位置，机器人并移动到该计算位置。

S5、判断移动后的机器人是否满足启用逃离陷阱目标函数的条件，启用逃离陷阱目标函数的条件为：当前萤火虫算法的目标函数为搜索终点目标函数，并且搜索终点目标函数值满足条件：

f₁(s)＜f₁(s-1) (3)

其中，f₁(s)为步骤S4中机器人移动后的搜索终点目标函数值，f₁(s-1)为步骤S4中机器人移动前的搜索终点目标函数值；如图4所示，当s＝27时，f₁(s)＝3.73(见图4中D点)，f₁(s-1)＝3.69(见图4中C点)，此时满足公式(3)所述的条件，说明机器人陷入了局部最优陷阱，则将搜索终点目标函数切换为逃离陷阱目标函数，并计算此时移动机器人与目标点之间距离，记作d_trap。若不满足，则保持目标函数不变。

S6、进一步判断移动后的机器人是否满足启用搜索终点目标函数的条件，启用搜索终点目标函数的条件为：当前萤火虫算法的目标函数为逃离陷阱目标函数，并实时计算每一步移动机器人与目标点之间距离，记作d_rt，进一步依据如下公式开展判断：

d_rt≤(d_trap-d_gap) (4)

其中，d_trap为步骤S5中改变目标函数时机器人位置与目标点之间的距离，d_rt为当前机器人位置到终点的距离，d_gap为常数，取值为5。若公式(4)不等式条件成立，则将萤火虫算法的目标函数设置为搜索终点目标函数，若不满足，则保持目标函数不变。

S7、判断机器人是否到达终点，若成立结束路径规划，若不成立则返回步骤S4往复执行其余步骤直至机器人到达终点。

图2为依据本发明提出的动态路径规划算法的机器人运动轨迹结果图，图中一共选取了机器人动态路径规划过程中12个不同时刻的结果，其中，实线路径表示当前目标函数为搜索终点目标函数的机器人运动路径，虚线路径则表示当前目标函数为逃离陷阱目标函数的机器人运动路径，路径末端的实心圆为机器人当前时刻的位置。动态障碍物和终点的实时位置用实线表示，并附带着箭头描述其运动方向，虚线则表示障碍物和终点的初始位置。由图2可知，当移动机器人进入到由凹形障碍物构成的局部最优陷阱中时，目标函数由搜索终点目标函数变化为逃离陷阱目标函数，并且在移动机器人逃出陷阱后，目标函数再次切换回搜索终点目标函数，成功到达终点。实验结果表明，本方法对于动态障碍物和动态局部最优陷阱都表现出优异的躲避和逃离能力，并且还可以成功到达动态的终点。

与现有技术相比，本发明的优点在于：本发明涉及萤火虫算法在动态路径规划中设计了两个不同的目标函数，分别用于搜索终点和逃离陷阱。在动态路径规划中通过监测目标函数值的变化规律来判断机器人是否进入局部最优陷阱，通过监测机器人与终点的距离是否变近来判断机器人是否已经逃离出局部陷阱，机器人在局部最优陷阱内时，启用逃离陷阱目标函数作为其目标函数，机器人在其余时刻均启用搜索终点目标函数作为其目标函数。通过在动态路径规划的不同状态选择相应的目标函数，使得机器人不仅可以逃离出凹形局部最优陷阱、躲避动态障碍物还可以搜索到动态终点。

Claims (3)

1.基于变目标函数萤火虫优化路径的机器人移动方法，其特征在于，包括以下步骤：

S1、构建路径规划场景；S2、设计两个不同的目标函数：搜索终点目标函数f₁、逃离陷阱目标函数f₂；S3、启用搜索终点目标函数作为萤火虫算法的初始目标函数，并计算搜索终点目标函数值记为f₁(0)；S4、判断机器人当前位置与终点连线在一个移动步长内有无障碍物，若没有障碍物则机器人直接向终点方向移动一个步长，若有障碍物，按照所选目标函数启动萤火虫算法搜索下一步位置，计算搜索终点目标函数值记为f₁(s)；S5、判断移动后的机器人是否满足启用逃离陷阱目标函数的条件，若满足则将萤火虫算法的目标函数设置为逃离陷阱目标函数，并计算机器人此时位置与终点实时位置距离记为d_trap，若不满足则保持目标函数不变；S6、判断移动后的机器人是否满足启用搜索终点目标函数的条件，若满足则将萤火虫算法的目标函数设置为搜索终点目标函数，若不满足则保持目标函数不变；S7、判断机器人是否到达终点，若成立结束路径规划，若不成立则返回步骤S4往复执行其余步骤直至机器人到达终点；

进一步的，步骤S2所述的搜索终点目标函数f₁为：

f₁＝k₁d_rt+k₂(1/d_ro)+k₃θ (1)

其中，k₁，k₂和k₃为加权系数，d_rt为机器人当前位置到终点实时位置的距离，d_ro为机器人与附近障碍物的最小距离，θ为机器人当前移动步的运动方向与前一步的运动方向之间的夹角；

进一步的，步骤S2所述的逃离陷阱目标函数f₂为：

f₂＝k₄|d_ro-d_safe|+k₅d_f+k₆θ (2)

其中，k₄，k₅和k₆为加权系数，d_ro为机器人运动过程中与附近障碍物的最小距离，d_safe为机器人与障碍物之间的安全距离，取常数，θ为机器人当前移动步的运动方向与前一步的运动方向之间的夹角，d_f为机器人在移动方向上与障碍物的距离；

进一步的，步骤S4所述的计算搜索终点目标函数值：当机器人通过两种方式完成一步移动后都需要计算搜索终点目标函数值并记为f₁(s)，其中s为机器人的移动步数，两种移动方式包括：机器人直接向终点方向移动一个步长和机器人通过萤火虫算法搜索到下一步位置并移动到该位置。

2.根据权利要求1所述的基于变目标函数萤火虫优化路径的机器人移动方法，其特征在于，步骤S5所述的启用逃离陷阱目标函数的条件为：当前萤火虫算法的目标函数为搜索终点目标函数，并且搜索终点目标函数值满足条件：

f₁(s)＜f₁(s-1) (3)

其中，f₁(s)为步骤S4中机器人移动后的搜索终点目标函数值，f₁(s-1)为步骤S4中机器人移动前的搜索终点目标函数值。

3.根据权利要求1所述的基于变目标函数萤火虫优化路径的机器人移动方法，其特征在于，步骤S6所述的启用搜索终点目标函数的条件为：当前萤火虫算法的目标函数为逃离陷阱目标函数，并且满足如下条件：

d_rt≤(d_trap-d_gap) (4)

其中，d_trap为步骤S5中切换为逃离陷阱目标函数时机器人位置与终点实时位置之间的距离，d_rt为机器人实时位置与终点实时位置之间的距离，d_gap为常数。

Images