CN108693879B - 基于改进人工势场法的移动机器人路径规划方法 - Google Patents

Abstract

本发明涉及一种基于改进人工势场法的移动机器人路径规划方法，首先设定移动机器人起点和目标点，根据激光雷达实时反馈周围障碍物的距离信息建立势场模型，引入引力角、斥力角和合力角；并以目标点为中心设定半圆阈值检测。当在阈值范围内未检测到机器人时，通过引入的合力角迭代运动，实时控制机器人的方向；最后在阈值范围检测到移动机器人时，可将机器人在目标点周围的障碍物到移动机器人位置与目标点之间的连线距离作为人为设定的安全距离，当在阈值范围内，机器人满足人为设定的安全距离要求时，可将合力角转化为引力角控制机器人直接导向目标点。不同于传统势场法通过虚拟力的控制方式，并且改进的方法可解决传统势场方法所存在的目标点不可达问题。

Description

基于改进人工势场法的移动机器人路径规划方法

技术领域

本发明涉及一种移动机器人的路径规划方法，尤其是一种改进传统人工势场的路径规划方法。

背景技术

移动机器人的路径规划技术可包括环境信息完全已知的全局路径规划和环境信息部分已知的局部路径规划。人工势场法是较为传统的局部规划方法。根据传感器实时感知周围的障碍物得到与障碍物之间的位置信息，通过人工势场模型得到虚拟力。通过目标点之间的引力和障碍物的斥力共同作用，从而完成规划。因其良好的避障能力和简单的数学表达，则经常被用于移动机器人路径规划中。然而传统势场也会存在一些固有的缺点。比如当目标点周围存在着障碍物时，此时移动机器人在目标点周围存在斥力大于引力，则使得移动机器人无法到达目标点。方法

通过对现有文献查阅，大部分通过对目标点不可达问题改进的方法为在斥力函数中引入一个距离因子。通过在传统斥力场中加入距离因子，改变原有的势场模型。引入距离因子，从本质上说就是改变了传统势场。解决了传统势场法目标点周围存在障碍物时，目标点不是势场最低点的问题。当到达目标点附近时，引入的距离因子即额外引入一个与目标点之间的引力和与障碍物之间的斥力。移动机器人所在目标点周围附近时，引入的距离因子会引入一个不断增强的引力，引入的斥力又会不断减小。通过引力和斥力的变化，最终会使得移动机器人到达目标点。此种方法，可解决传统势场法无法到达目标点的问题。但由于额外引入的距离因子，会使得整个传统势场发生变化。额外引入的引力和斥力会使得规划的路径丢失平滑性。虽然可成功到达目标点，解决传统势场目标点不可达问题，但整个路径规划效果不如传统势场法。使得改进的算法不太具有实用性。

发明内容

本发明是基于传统势场法的不足和固有的缺陷，提出一种改进的人工势场规划方法，该方法基于传统势场函数和势场力对目标点和障碍物进行力的模型分析，将原有对机器人虚拟力的控制转化为角度方向控制；对引力、斥力和合力不同受力模型分析，分别引入和计算出引力角、斥力角、和合力角；在目标点周围设定阈值检测和将目标点周围的障碍物到移动机器人实时位置与目标点连线的距离作为安全距离考量；如若在检测范围内，并且满足安全距离要求。可通过变化合力控制为引力角完成规划，可解决传统目标点不可达的问题。

本发明的技术方案为：一种基于改进人工势场法的移动机器人路径规划方法，包括如下步骤：

首先，通过设定移动机器人起点和目标点，根据激光雷达实时反馈周围障碍物的距离信息建立势场模型，再根据势场模型下的引力、斥力和合力的四种方向状态转化为角度信息，并引入引力角、斥力角和合力角。

然后，在目标点周围设定半圆阈值范围检测，当在阈值范围内未检测到移动机器人时，通过引入的合力角迭代运动，实时控制移动机器人的方向；

最后，在阈值范围检测到移动机器人时，此时移动机器人已经在目标点周围，可将目标点周围的障碍物到移动机器人位置与目标点之间的连线距离作为人为设定的安全距离，当在阈值范围内，移动机器人满足人为设定的安全距离要求时，可将合力角转化为引力角控制移动机器人直接导向目标点。

所述引入的引力角θ_att＝sign(Δy)ar cos(Δx/r_att)表示引力的几种方向状态，斥力角θ_rep＝pi+sign(Δx_ob)ar cos(Δx_ob/r_rep)表示斥力的几种方向状态，最后根据引力角和引力角的分解，可以再引入合力角，此时传统虚拟力的控制，转化为实时的角度方向控制，其中：Δx为机器人当前位置与目标点之间横坐标变化，Δy为机器人当前位置与目标点之间纵坐标变化，

Δx_ob为机器人当前位置与障碍物坐标的横坐标变化，Δy_ob为机器人当前位置与障碍物坐标的纵坐标变化，pi＝(x_i，y_i)，x_i，y_i分别为激光雷达扫描中在第i个方向上测得的距离在直角坐标系上的横坐标和纵坐标值。

在阈值范围内，实时检测移动机器人是否实时进入；如果未检测到移动机器人在阈值内，可通过模型转化的合力角实时迭代控制移动机器人的方向运动；当在阈值范围内检测到移动机器人时，此时可将目标点周围的障碍物到移动机器人的实时位置到目标点连线之间的距离作为安全距离考量；人为设定安全距离作为可行通过域，障碍物带连线的距离随着移动机器人在阈值范围内的移动会不断变化；当检测到距离未满足人为设定的安全距离要求时，可继续通过合力角去迭代控制方向运动，直到某个时刻，周围的障碍物到当前位置与目标点之间连线的距离满足人为设定的全距离时，则转化合力角为引力角直接使得移动机器人导向目标点，此时移动机器人运动不会触碰障碍物，会到达目标点。

本发明的有益效果在于：通过将传统模型虚拟力的控制转化为角度方向控制。使得控制方式更为简单。只需时刻反馈方向角度，并将方向角度信息转化为移动机器人的双轮差速信息，就可实时迭代运动。最后通过在目标点设置阈值范围检测，结合人为所设定的安全距离可行域，特定情况下变换合力角为引力角控制。可解决传统方法目标点不可达的问题。

附图说明

图1为移动机器人引力的四种方向状态；

图2为移动机器人斥力的四种方向状态；

图3为移动机器人合力的四种方向状态；

图4为阈值范围检测图；

图5为本发明的基于改进人工势场法的移动机器人路径规划方法流程图。

具体实施方式

下面结合附图与实施例对本发明作进一步说明，

如图5所示，一种基于改进人工势场法的移动机器人路径规划方法，其步骤为：首先，对整个移动机器人运动控制和目标点进行位置设定。设定时，要求整个运动空间在同一坐标系下描述。移动机器人采用双轮差速驱动。通过激光雷达实时反馈检测到的障碍物距离信息。

激光雷达的测量值信息用集合A＝{i|1..n}表示，其中i表示激光雷达在此次扫描中在第i个方向上测得的距离，n为一组扫描中扫描数据的个数。通过对原始数据进行预处理，将在极坐标系下表示的扫描点变换到直角坐标系中，激光雷达扫描数据就可以表示为集合B＝{pi|i＝1，，n}，式中pi等于x_i，y_i。最后通过坐标转化为在直角坐标系下的位置。激光雷达安装在移动机器人的最前端整正中心位置，可实时反馈周围障碍物的距离通过移动机器人的位置和信息和激光雷达获得的距离信息可通过传统势场法将整个运动空间势场模型求出。下面根据移动机器人的引力方向进行分析。设置机器人运动起点X₀＝[0，0]为坐标系原点。任意位置为X＝[x，y]，目标点设为X_G＝[x_g，y_g]。统一规定横纵坐标方向为力的正方向。规定当前位置与目标点之间横坐标变化为Δx＝x_g-x，纵坐标变化为Δy＝y_g-y_g、，距离为

机器人当前位置与目标点位置横纵坐标变化关系图1表示。当Δx＞0，Δy＞0时，此时引力分解使得分力在横纵坐标正方向，即F_att sinθ1＞0，F_attcosθ1＞0。此时可表示为θ1＝ar cos(Δx/r_att)。当Δx＞0.Δy＜0时，纵向分力为负值，横向分立为正值。即F_att sinθ2＜0，F_att cosθ2＞0。此时可表示为θ2＝-arcos(Δx/r_att)。当Δx＜0，Δy＜0时，横向纵向分力均为负值。即F_att sinθ3＜0，F_att cosθ3＜0。此时可表示为θ3＝-arcos(Δx/r_att)。当Δx＜0，Δy＞0时，横向分力为负值，纵向分力为正值。即F_att sinθ4＞0，F_att cosθ4＜0。此时θ4＝arcos(Δx/r_att)。此时引入θ_att＝sign(Δy)arcos(Δx/r_att)作为引力角可代表以上引力在四种方向状态下的角度。

再对机器人与障碍物之间的斥力进行受力分析。设定障碍物坐标为X_ob＝[x_ob，y_ob]。规定机器人当前位置与障碍物坐标的横坐标变化为Δxob＝xob-x，纵坐标变化为Δyob＝yob-y，距离为

则机器人实时所在位置与障碍物的斥力关系如图2所示。当Δxob＞0，Δyob＞0时，此时斥力分解再横坐标方向为负值，纵坐标方向为负值。即F_rep cosθ11＜0，F_rep sinθ11＜0。此时θ11＝pi+ar cos(Δxob/r_rep)。当ΔΔxob＞0，Δyob＜0时，此时斥力分解横坐标方向为负值，纵坐标方向为正值。即F_rep cosθ9＞0，F_repsinθ9＜0。此时θ9＝pi-ar cos(Δxob/_rep)。当ΔΔxob＜0，Δyob＜0时，此时斥力分解横坐标方向为正值，纵坐标分量为正值。即F_rep cosθ10＞0，F_rep sinθ10＞0。此时θ10＝pi-arcos(Δxob/r_rep)。当Δxob＜0，Δyob＞0时，F_rep cosθ12＞0，F_rep sinθ12＜0，此时θ12＝pi+ar cos(Δxob/r_rep)。下面引入θ_rep＝pi+sign(Δxob)arcos(Δxob/r_rep)作为斥力角来表示斥力在以上四种方向状态下的角度。对于规划空间中存在n个障碍物，坐标分别为X_ob1，X_ob2......X_obn。那么对应的斥力角对应为θ_rep[1]，θ_rep[2].......θ_rep[n]。那么此时斥力在横纵方向的的分量分别为：

F_rep(x)＝F_rep cos(θ_rep[1])+F_rep cos(θ_rep[2])+......+F_rep cos(θ_rep[n])

F_rep(y)＝F_rep sin(θr_ep[1])+F_rep sin(θ_rep[2])+......+F_rep sin(θ_rep[n])

此时将引力分别分解到横纵方向可得到：

F_att(x)＝F_att cos(θ_att)

F(y)＝F_att(y)+F_rep(y)

对图3中合力横纵分量共存在如下四种情况：

(1)当F(x)＞0，F(y)＞0，θ₅＝arctan(F(y)/F(x))：

(2)当F(x)＞0，F(y)＜0，θ₆＝arctan(F(y)/F(x))：

(3)当F(x)＜0，F(y)＜0，θ₇＝arctan(F(y)/F(x))；

(4)当F(x)＜0，F(y)＞0，θ₆＝arctan(F(y)/F(x))；

对于以上四种运动方向下的角度，可用如下伪代码表示：

if F(x)＞0

自＝arctan(F(y)/F(x))

else

θ＝pi+arctan(F(y)/F(x))

θ作为合力角是整个运动的控制角度方向。对于移动机器人在某一时间内向同一方向行驶的距离作为匀速步长L。设当前任一时刻的机器人的位置为X_k＝[x_k，y_k]，下一时刻的位置为X_k+1＝(x_k+1，y_k+1)。那么下一时刻的位置可有势场合力角来确定位置。x_k+1＝x_k+L*cosθ；y_k+1＝y_k+L*sinθ。整个规划通过角度方向来确定下一时刻的位置。时刻间距可由人为自行定义。时间间隔越短，则所需计算量越大，规划路径越平滑。通过对传统势场对模型的转化可实时求出合力角，则当激光雷达不断反馈距离信息时，通过模型分析角度转化，可向机器人反馈一个方向角度。再将方向角度转化为双轮差速信息，即可实现机器人转向。通过不断迭代进行规划运动。

最后，通过在目标点一定范围内设置阈值检测。当未检测到移动机器人进入阈值区域，则使用合力角控制移动机器人的运动。当检测移动机器人进入阈值区域，则检测障碍物到移动机器人和目标连线的安全距离值。当满足人为所设定的安全距离要求，即可通过转化引力角作为控制方向引导机器人到达目标点。

如图4所示当移动机器人在半圆检测范围之外，通过合力角控制整个方向运动。当到达阈值检测范围内，检测是否满足安全距离要求。当满足人为所设定的安全距离，可表明机器人可无碰撞到达目标点运动。此时通过将控制方向的合力角转化为引力角到达目标点完成规划运动。

Claims (2)

1.一种基于改进人工势场法的移动机器人路径规划方法，其特征在于，包括如下步骤：

首先，通过设定移动机器人起点和目标点，根据激光雷达实时反馈周围障碍物的距离信息建立势场模型，再根据势场模型下的引力、斥力和合力的四种方向状态转化为角度信息，并引入引力角、斥力角和合力角；

然后，在目标点周围设定半圆阈值范围检测，当在阈值范围内未检测到移动机器人时，通过引入的合力角迭代运动，实时控制移动机器人的方向；

最后，在阈值范围检测到移动机器人时，此时移动机器人已经在目标点周围，可将目标点周围的障碍物到移动机器人位置与目标点之间的连线距离作为人为设定的安全距离，当在阈值范围内，移动机器人满足人为设定的安全距离要求时，可将合力角转化为引力角控制移动机器人直接导向目标点。

2.根据权利要求1所述的基于改进人工势场法的移动机器人路径规划方法，其特征在于：在阈值范围内，实时检测移动机器人是否实时进入；如果未检测到移动机器人在阈值内，可通过模型转化的合力角实时迭代控制移动机器人的方向运动；当在阈值范围内检测到移动机器人时，此时可将目标点周围的障碍物到移动机器人的实时位置到目标点连线之间的距离作为安全距离考量；人为设定安全距离作为可行通过域，障碍物到目标连线的距离随着移动机器人在阈值范围内的移动会不断变化；当检测到距离未满足人为设定的安全距离要求时，可继续通过合力角去迭代控制方向运动，直到某个时刻，周围的障碍物到当前位置与目标点之间连线的距离满足人为设定的全距离时，则转化合力角为引力角直接使得移动机器人导向目标点，此时移动机器人运动不会触碰障碍物，会到达目标点。

Images