CN113296523A - 一种移动机器人避障路径规划方法 - Google Patents

Abstract

本发明属于机器人及智能车辆的局部导航领域，具体技术方案为：一种移动机器人避障路径规划方法，具体步骤如下：一、基于移动机器人车载外部传感器激光雷达获取的工作环境信息构建栅格环境地图；二、通过改进引力和斥力场函数计算机器人受到的引力、引力与水平方向的夹角、斥力、斥力与水平方向的夹角；三、计算机器人所受到的合力、合力与水平方向的夹角；四、计算并保存机器人的下一步位置；五、机器人由当前位置运动至下一位置，机器人每运行一步增加相应步长；六、判断机器人是否达到指定目标点，若达到目标点，结束；若未达到目标地，返回步骤二操作；本发明通过对原有势场函数进行修正，以保证机器人到达指定目标位置，实现机器人路径规划。

Description

一种移动机器人避障路径规划方法

技术领域

本发明属于机器人及智能车辆的局部导航领域，具体涉及一种基于改进人工势场法和动态窗口法相结合的移动机器人避障路径规划方法。

背景技术

随着科学技术的发展，移动机器人的应用涉及到各行各业。移动机器人涉及多传感器信息融合、计算机和通信技术以及自动控制等多种理论，综合了机械、电子、计算机、自动化和人工智能的学科。自主导航是移动机器人实现智能化的关键技术，而路径规划则是自主导航中的基本部分，路径规划即机器人在工作环境中从当前位置出发，能够在到达指定目标点的过程中避开障碍物规划出一条安全可靠、无碰撞的运动路径。移动机器人的路径规划技术包括环境信息完全已知的全局路径规划和环境信息部分已知或完全未知的局部路径规划。人工势场法是较为传统的规划方法，根据机器人搭载各类型传感器实时感知工作周围的障碍物得到与障碍物之间的位置信息，将工作环境中物体和机器人自身等构建为一个虚拟势场，目标点与物体分别对机器人产生引力和斥力，通过它们的合力控制机器人的运动方向，从而完成运动规划。人工势场法的实时性一般高于其他算法且结构比较简单，能够完成对执行系统的底端控制。其计算量小、结构简单、反应速度快、对未知环境适应性强等优点使其仍是最合适的移动机器人实现避障的基础算法。但同时也存在一些缺陷，如存在局部极小值或目标不可达等问题。

路径规划是当前智能机器人技术研究热点之一，其中避障策略是其重中之重，目前，研究者多借用人工势场法、珊格法等方法研究移动机器人避障算法。其中，人工势场法因算法简单且易于实时控制而广泛应用，但容易陷入局部极小值或无法到达指定目标点使移动机器人无法到达目标位置完成相关任务需求；而珊格法存在着环境分辨率与环境信息存储量之间的矛盾，且计算量大，使用范围受限。

发明内容

为解决现有技术存在的人工势场法会使智能机器人陷入局部极小值或者无法到达指定目标点的问题，本发明公开了一种基于改进传统人工势场法和动态窗口法相结合的避障路径规划方法，并因实际路径规划中局部情况的复杂性，引入动态窗口法进行局部路径规划，保障智能机器人能够做出及时、准确的避障动作。

本发明基于传统势场函数和势场力对机器人进行力的模型分析，通过对原有引力场函数进行修正，增加一个阈值限定目标点与机器人之间的距离，有效解决机器人与目标点距离较远时引力较大导致可能与障碍物碰撞问题；并通过改进传统斥力场函数，以保证机器人能够按照预设规则到达指定目标位置；针对传统人工势场法易使机器人陷入局部极小值问题，通过添加随机扰动点实现机器人避开局部极小点并顺利到达目标点，实现机器人路径规划。在实际工作环境中可能存在动态障碍物，其局部环境信息可能相对复杂，故结合动态窗口法来解决路径规划过程中的局部路径问题。

为实现上述目的，本发明所采用的技术方案为：一种移动机器人避障路径规划方法，具体步骤如下：

一、基于移动机器人车载外部传感器激光雷达获取的工作环境信息构建栅格环境地图，每一个栅格根据环境信息被标记为有障碍物和自由空间，初始化设定引力尺度因子、斥力尺度因子η、障碍物的影响距离ρ₀、步长l、机器人起始点和指定到达目标点。

二、通过改进引力和斥力场函数计算机器人受到的引力F_att、引力F_att与水平方向的夹角、斥力F_rep、斥力F_rep与水平方向的夹角。

三、按式F(q)＝F_att(q)+F_rep(q)计算机器人所受到的合力、合力与水平方向的夹角θ。

四、计算并保存机器人的下一步位置，根据式x_k+1＝x_k+l·cosθ计算得到机器人下一位置的横坐标x_k+1，根据式y_k+1＝y_k+l·sinθ计算得到机器人下一位置的纵坐标y_k+1。

其中，x_k为机器人当前位置的横向坐标，y_k为机器人当前位置的竖向坐标。

五、机器人由当前位置(x_k,y_k)运动至下一位置(x_k+1,y_k+1)，机器人每运行一步增加相应步长l。

六、判断机器人是否达到指定目标点，若通过判断机器人所在位置(x_k+1,y_k+1)与目标点位置的距离为0或者小于等于δ时，则判定机器人到达目标点；若通过判断机器人所在位置(x_k+1,y_k+1)与目标点位置的距离不为0或者大于δ时，则返回步骤二操作。

根据机器人车载激光雷达完成工作环境栅格地图构建并获得障碍物位置，根据指定目标进行全局路径规划，然而在机器人实际应用场景中，由于局部工作环境复杂如人员走动等情况的存在，需要车载激光雷达根据障碍物变化信息对障碍物重新进行获取并产生障碍物信息，并将所述障碍物信息传输给局部规划单元，局部规划单元对全局目标路径进行修正。

经过修正后引力场函数具体表达如下：

其中，d_goal为限定目标点与机器人之间距离的阈值，q为机器人所在位置，q_goal为目标点位置，q-q_goal为机器人所在位置与目标点位置之间的距离；

修正后的引力如下式所示：

经过修正后斥力场函数具体表达如下：

其中，q_obs为障碍物位置，q-q_obs为机器人与障碍物之间的距离；

则改进后斥力如下式所示：

其中，F_rep1方向由障碍物指向机器人；F_rep2方向由机器人指向目标位置。

本发明与现有技术相比，具体有益效果体现在：本发明的技术方案可解决现有技术中需要事先对环境进行标记或铺设，不具有环境适应性且无法实现灵活避障路径规划的技术问题，实施本发明的技术方案，可实现不需要事先对环境进行标记或改造，可实现灵活规划避障路径。

本发明针对基于传统人工势场法的不足和缺陷，通过对原有势场函数进行修正，以保证机器人能够按照预设规则到达指定目标位置；针对传统人工势场法易使机器人陷入局部极小值问题，通过添加随机扰动点实现机器人避开局部极小点并顺利到达目标点，实现机器人路径规划。

本发明的技术方案考虑在实际工作环境中可能存在动态障碍物等情况，其局部环境信息相对复杂，故结合动态窗口法来解决路径规划过程中的局部路径问题，有效解决机器人在实际应用场景的环境适应性。

附图说明

图1为机器人受力分析图。

图2为目标位置、障碍物与机器人的布局分析图一。

图3为目标位置、障碍物与机器人的布局分析图二。

图4为本发明的算法流程图。

图5为机器人避障路径规划仿真结果对比图一。

图6为机器人避障路径规划仿真结果对比图二。

图7为动态窗口法采样轨迹图。

图8为动态窗口法仿真结果图一。

图9为动态窗口法仿真结果图二。

图10为路径规划系统框图。

具体实施方式

为了使本发明所要解决的技术问题、技术方案及有益效果更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

一种移动机器人避障路径规划方法，具体步骤如下：

一、基于移动机器人车载外部传感器激光雷达获取的工作环境信息构建栅格环境地图，每一个栅格根据环境信息被标记为有障碍物和自由空间，并进行初始化设定引力尺度因子ε、斥力尺度因子η、障碍物的影响距离ρ₀、步长l和机器人起始点和到达目标点。

二、通过改进引力和斥力场函数分别计算机器人受到的引力F_att、引力F_att与水平方向的夹角、斥力F_rep及斥力F_rep与水平方向的夹角。

三、按式F(q)＝F_att(q)+F_rep(q)计算机器人所受到的合力以及合力与水平方向的夹角θ。

四、计算并保存机器人的下一步位置，根据式x_k+1＝x_k+l·cosθ计算得到机器人下一位置的横坐标x_k+1，根据式y_k+1＝y_k+l·sinθ计算得到机器人下一位置的纵坐标y_k+1。

其中，x_k为机器人当前位置的横向坐标，y_k为机器人当前位置的竖向坐标。

五、机器人由当前位置(x_k,y_k)运动至下一位置(x_k+1,y_k+1)，机器人每运行一步增加相应步长，即k＝k+l。

六、判断机器人是否达到指定目标点，通过判断机器人所在位置(x_k+1,y_k+1)与目标点位置的距离为0或者小于等于δ(一个很小的正数)时，则认为机器人到达目标点，否则返回步骤二继续。

上面所述路径规划根据机器人车载激光雷达完成工作环境栅格地图构建并获得障碍物位置，根据指定目标进行全局路径规划，然而在机器人实际应用场景中，由于局部工作环境复杂如人员走动等情况的存在，需要车载激光雷达根据障碍物变化信息对障碍物重新获取并产生障碍物信息，并将障碍物信息传输给局部规划单元，局部规划单元对全局目标路径进行修正。

如图1所示，机器人在人工势场法作用下，预设目标对机器人产生的作用力为引力，而障碍物对机器人产生斥力，机器人在两者的合力作用下进行运动。但当机器人与预设目标之间距离较远时，此时引力过大可能会导致与障碍物相撞。

针对此问题，对引力场函数进行修正，改进后的引力场函数如式所示：

其中，d_goal为限定目标点与机器人之间距离的阈值，q为机器人所在位置，q_goal为目标点位置，q-q_goal为q、q_goal两者之间的距离；

则改进后引力如下式所示：

由人工势场法原理可知，障碍物对机器人的斥力与距离之间成反比关系，即随着距离减小而斥力会增加，当机器人接近最终目标点时，机器人相较空间物体受到的斥力大于目标位置对于机器人的引力，最终可能导致机器人远离目标位置无法到达。为了解决这一问题，对传统势场函数改进，以保证机器人能够按照预设规则到达指定目标位置，改进后的斥力场函数如式所示：

其中，q_obs为障碍物位置，q-q_obs为机器人与障碍物之间的距离；

则改进后斥力如下式所示：

其中，F_rep1方向由障碍物指向机器人；F_rep2方向由机器人指向目标位置。

通过改进势场函数可以有效解决目标位置与环境物体位置相近时导致自动引导车目标不可达问题，但传统人工势场法还存在局部极小点问题，如图2所示，目标位置、障碍物与机器人三点共线，如图3所示，目标位置、障碍物与机器人虽不共线，但机器人受到目标位置的引力与受到障碍物的斥力互为相反数，则会导致机器人运动过程中过早收敛而无法到达目标点。

针对传统人工势场法存在的缺陷，当其陷入局部极小值时，通过添加随机扰动点，在当前点x处随意挑选一个点x₁，然后根据上述两个公式分别计算当前点x和随机点x₁的势场U(x)和U(x₁)。若U(x₁)≤U(x)，则选择x₁为下一个点；与之相反，若U(x₁)>U(x)，则计算概率重新产生随机点，算法流程如图4所示。

为验证其合理性，在相同初始条件下分别对传统人工势场法和改进势场法在局部极小值问题和目标不可达问题进行仿真实验，如图5-6所示，改进势场法通过判断机器人陷入局部最优，解决了传统势场存在的极小值问题和目标不可达的问题。

如图7所示，在动态窗口法中，要计算模拟机器人运动轨迹，首先需要考虑机器人运动模型，然后可以根据速度推导机器人一定时间内的运行轨迹，根据采样的多组线速度和角速度，可以推断出机器人的运动轨迹运行曲线，并通过指定评价标准得到能满足要求的轨迹指导机器人运动。

在空间中进行速度采样时可以得到无穷多组速度，但考虑到周围环境信息和机器人自身的限制，速度采样中主要有以下三个限制因素：

1、机器人受自身最小和最大速度的限制，故采样速度应符合式：

V_m＝{v∈[v_min,v_max],w∈[w_min,w_max]}

其中，v为机器人的线速度，w为机器人的角速度，v_min为机器人的最小线速度，v_max为机器人的最大线速度，w_min为机器人的最小角速度，w_max为机器人的最大角速度。

2、机器人受自身电机性能的影响，在机器人的运动轨迹模拟时间里，其速度存在一个动态窗口，表示机器人实际能够达到的速度如式：

其中，V_d为机器人的可行速度，v_c为机器人的样本线速度，w_c为机器人的样本角速度，

为机器人的最大加速度，

为机器人的最大减速度，Δt为机器人的速度空间运动时间。

3、基于机器人自身安全考虑，为了避免机器人碰撞到障碍物，故设定一个速度判定条件如式：

其中，dist(v,w)为速度空间对应运动曲线上离障碍物的最近距离。

在实际的机器人采样速度中，会出现多条可用轨迹，但不是所有的运动轨迹可用，需要对每条运动轨迹进行评价，故采用的评价函数如式：

G(v,w)＝s(α·P_head+β·G_dist+γ·D_vel)

其中，P_head表示机器人在当前速度空间下，达到运动曲线末端朝向与目标位置之间角度差距；G_dist表示机器人在当前轨迹上与最近障碍物之间的距离；D_vel表示机器人在当前轨迹上的速度大小；α,β,γ表示权重比。

如图8-9所示，在相同初始条件下，分别设置不同冲突半径的仿真环境，也就是膨胀区间R的取值，从仿真实验中，可以看出动态窗口法可以在膨胀区间不同的情况下，通过采样的速度合理推算出相应的一组运动轨迹，并利用评价函数对每一条轨迹进行评分，最终选择一条最佳的路径进行局部路径规划，成功避开障碍物达到设定的目标点。

如图10所示，通过机器人实际工作环境中的路径规划需求，将路径规划过程由全局路径规划和局部路径相结合进行自动引导车的路径规划，最终完成机器人避障路径规划任务。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包在本发明范围内。

Claims (4)

1.一种移动机器人避障路径规划方法，其特征在于，具体步骤如下：

一、基于移动机器人车载外部传感器激光雷达获取的工作环境信息构建栅格环境地图，每一个栅格根据环境信息被标记为有障碍物和自由空间，初始化设定引力尺度因子ε、斥力尺度因子η、障碍物的影响距离ρ₀、步长l、机器人起始点和指定目标点；

二、通过改进引力和斥力场函数计算机器人受到的引力F_att、引力F_att与水平方向的夹角、斥力F_rep、斥力F_rep与水平方向的夹角；

三、按式F(q)＝F_att(q)+F_rep(q)计算机器人所受到的合力、合力与水平方向的夹角θ；

四、计算并保存机器人的下一步位置，根据式x_k+1＝x_k+l·cosθ计算得到机器人下一位置的横坐标x_k+1，根据式y_k+1＝y_k+l·sinθ计算得到机器人下一位置的纵坐标y_k+1；

其中，x_k为机器人当前位置的横向坐标，y_k为机器人当前位置的竖向坐标；

五、机器人由当前位置(x_k,y_k)运动至下一位置(x_k+1,y_k+1)，机器人每运行一步增加相应步长l；

六、判断机器人是否达到指定目标点，若通过判断机器人所在位置(x_k+1,y_k+1)与目标点位置的距离为0或者小于等于δ时，则判定机器人到达指定目标点；若通过判断机器人所在位置(x_k+1,y_k+1)与目标点位置的距离不为0或者大于δ时，则返回步骤二操作。

2.根据权利要求1所述的一种移动机器人避障路径规划方法，其特征在于，根据机器人车载激光雷达完成工作环境栅格地图构建并获得障碍物位置，根据指定目标进行全局路径规划，并将障碍物信息传输给局部规划单元，局部规划单元对全局目标路径进行修正。

3.根据权利要求1所述的一种移动机器人避障路径规划方法，其特征在于，经过修正后引力场函数具体表达如下：

其中，d_goal为限定目标点与机器人之间距离的阈值，q为机器人所在位置，q_goal为目标点位置，q-q_goal为q、q_goal两者之间的距离；

修正后的引力如下式所示：

4.根据权利要求1所述的一种移动机器人避障路径规划方法，其特征在于，经过修正后斥力场函数具体表达如下：

其中，q_obs为障碍物位置，q-q_obs为机器人与障碍物之间的距离；

则改进后斥力如下式所示：

其中，F_rep1方向由障碍物指向机器人；F_rep2方向由机器人指向目标位置。

Images