CN105629974B - 一种基于改进型人工势场法的机器人路径规划方法及系统 - Google Patents

Abstract

本发明公开了一种基于改进型人工势场法的机器人路径规划方法及系统，首先在激光雷达的可视范围内找到局部目标点；然后规划出机器人当前位置到达局部目标点的可达路径，最后控制驱动机器人行进，循环检测局部目标点，直至机器人达到最终目标点；该方法采用基于人工势场法来规划机器人路径规划，解决了传统势场法对机器人进行路径规划出现的局部极小点问题，对传统的人工势场法进行了改进，即改进引力势函数，同时将整个任务划分为许多局部目标点，从而达到最优的路径；提高了路径规划的实时性、环境适应性效率。

Description

一种基于改进型人工势场法的机器人路径规划方法及系统

技术领域

本发明涉及机器人及智能车辆的局部导航领域，特别是一种基于改进型人工势场法的机器人路径规划方法。

背景技术

移动机器人实时路径规划和导航是反映机器人自主能力的关键要素之一，也是较难解决的问题之一。机器人路径规划主要分为环境信息已知的规划和环境信息未知的规划。对于前者多采用离线规划，得到的路径较优，后者多采用在线规划，体现了路径规划的实时性。

近年来许多移动机器人路径规划的方法被人们所研究。主要的路径规划的方法可以分为两类：人工智能的方法(AI)和人工势场法(APF)。前者主要运用的方法有遗传算法(GA)、模糊逻辑控制(FLC)和人工神经网络(ANN)，这些方法往往较为复杂运算速度也较为缓慢。而后者由于其简洁性和快速性在机器人路径规划中得到广泛应用，其基本思想是环境中的目标点对其的吸引力以及障碍物的对其的排斥力构成一种势场环境。然而人工势场法在避障应用的过程中，常常会遇到局部最小的问题。

如何避免人工势场法在避障路径规划中出现局部最小的问题，是移动机器人路径规划的关键。通过查阅专利和论文，主要有沿墙跟踪方法、极限环法、虚拟水流法及引入内部主体状态法等算法。

这些方法虽然在一定程度上缓解了局部最小问题，但都有其各自的缺陷。其中沿墙跟踪法和极限环法存在规划速度慢的问题。虚拟水流法在解决环境已经情况下的局部极小点问题有一定的效果，但算法效率不高。引入内部主体状态的方法成功的解决了复杂环境中的局部最小值问题，但是不能解决通常情况下的静态势场问题，通用性不强。

由于上述算法存在实时性不好、效率不高、会引入如环境适应能力差等一些新的问题等缺点。

因此，急需一种既具有高效率，又能保证实时性且通用性强的改进的人工势场法。

发明内容

本发明的目的就是提供一种基于改进型人工势场法的机器人路径规划方法及系统，适用于局部导航中，采用基于势场法的运动规划及规避障碍物。

本发明的目的是通过这样的技术方案实现的：

本发明提供了一种基于改进型人工势场法的机器人路径规划方法，包括以下步骤：

S1：获取机器人的初始化状态参数、环境信息和最终目标点；

S2：获取机器人当前坐标位置和局部目标点；

S3：建立基于时间虚拟驱动力的人工势场法生成机器人当前坐标位置和局部目标点之间的可达路径；

S4：控制机器人沿可达路径行进；

S5：在激光雷达可视范围内检测机器人当前坐标位置是否达到局部目标点，如果没有达到，则返回步骤S4继续控制驱动机器人行进；

S6：如果达到局部目标点，则检测机器人是否达到最终目标点，如果没有达到，则返回步骤S2；

S7：如果达到最终目标点，则结束机器人的行进。

进一步，所述局部目标点的确定，具体包括以下步骤：

S21：检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则设置最终目标点作为局部目标点；

S22：如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍物充分安全距离的任一点最为新坐标点作为局部目标点；

S23：如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试探点寻找斥力场合力零的坐标作为局部目标点。

进一步，所述试探点寻找斥力场合力零的坐标具体按照以下步骤来实现：

S231：按照以下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，对于所有的q′∈C_obs，ρ(q)＝min||q-q′||；

S232：根据斥力势函数按照以下公式确定机器人所受的排斥力：

式中，用q_c表示障碍物区域C_obs上距离q最近的位置点；

ρ(q)＝||q-q_c||；

是由q_c指向q的单位向量，

S233：选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力势场，

S234：根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。

进一步，所述机器人在达到局部目标点之前还包括以下步骤来实现：

S31：建立机器人与障碍物之间的斥力势函数并计算斥力：

S32：建立机器人与局部目标点之间的引力势函数；

式中：

ε：人工势场法引力势场增益参数；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离；

S33：按照以下公式计算局部目标点对机器人的引力：

S34：通过以下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；

S35：按照以下公式计算时间虚拟驱动力的大小：

式中：

γ：调节F_time的常数。

本发明提供了一种基于改进型人工势场法的机器人路径规划系统，包括机器人参数信息采集单元、机器人位置采集单元、局部目标点生成单元、可达路径生成单元、进行控制单元、局部目标点判断单元和最终目标点判断单元；

所述机器人参数信息采集单元，用于获取机器人的初始化状态参数、环境信息和最终目标点；

所述机器人位置采集单元，用于获取机器人当前坐标位置；

所述局部目标点生成单元，用于确定机器人行进中的局部目标点；

所述可达路径生成单元，用于计算机器人当前位置和局部目标点之间的可达路径；

所述进行控制单元，用于控制、驱动机器人沿可达路径行进；

所述局部目标点判断单元，用于计算并判断机器人当前坐标位置是否达到局部目标点；

所述最终目标点判断单元，用于计算并判断机器人是否达到最终目标点；

所述机器人参数信息采集单元和机器人位置采集单元分别与局部目标点生成单元连接；所述机器人位置采集单元与可达路径生成单元连接；所述可达路径生成单元与进行控制单元连接；所述局部目标点判断单元和最终目标点判断单元分别与行进控制单元连接。

进一步，所述局部目标点生成单元是按照以下步骤来进行的：

S11：检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则设置最终目标点作为局部目标点；

S12：如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍物充分安全距离的任一点最为新坐标点作为局部目标点；

S13：如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试探点寻找斥力场合力零的坐标作为局部目标点；

所述试探点寻找斥力场合力零的坐标具体按照以下步骤来实现：

S14：按照以下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，对于所有的q′∈C_obs，ρ(q)＝min||q-q′||；

S15：根据斥力势函数按照以下公式确定机器人所受的排斥力：

式中，用q_c表示障碍物区域C_obs上距离q最近的位置点；

ρ(q)＝||q-q_c||；

是由q_c指向q的单位向量，

S16：选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力势场，

S17：根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。

进一步，还包括与进行控制单元连接的时间虚拟驱动力单元；所述时间虚拟驱动力单元机是通过以下步骤来实现：

S41：建立机器人与障碍物之间的斥力势函数并计算斥力：

S42：建立机器人与局部目标点之间的引力势函数；

式中：

ε：人工势场法引力势场增益参数；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离；

S43：按照以下公式计算局部目标点对机器人的引力：

S44：通过以下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；

S45：按照以下公式计算时间虚拟驱动力的大小：

式中：

γ：调节F_time的常数。

由于采用了上述技术方案，本发明具有如下的优点：

本发明是基于人工势场法来规划机器人路径规划，解决了传统势场法对机器人进行路径规划出现的局部极小点问题，提高了路径规划的实时性、环境适应性效率。本发明对传统的人工势场法进行了改进，即改进引力势函数，同时将整个任务划分为许多局部目标点，从而达到最优的路径。

引力势函数把机器人到达目标的预计时间考虑进去，并将该时间转化为虚拟的引力，其一，保证目标点为整个势场的全局极小点，从而避免传统人工势场法出现的局部极小点的出现；其二，由于引入局部目标点后将路径规划为折线，路径虽为最优路径，但是不适用于机器人控制，通过时间虚拟驱动力可增加路径平滑度，解决机器人在行进过程中的控制量突变的情况，从而实现机器人的平稳行进。

加入了局部目标点后，可以将机器人的路径规划任务划分为很多子任务，只需控制每个子任务的路径都是最优、且最利于机器人行进控制，那么，整体的路径就可以呈现最优的状态。加入局部目标点后，就可以主动避免上述第二种出现局部极小点的情景，而无需增加额外的判断和计算，因此提高效率。

本发明的其他优点、目标和特征在某种程度上将在随后的说明书中进行阐述，并且在某种程度上，基于对下文的考察研究对本领域技术人员而言将是显而易见的，或者可以从本发明的实践中得到教导。本发明的目标和其他优点可以通过下面的说明书和权利要求书来实现和获得。

附图说明

本发明的附图说明如下。

图1a为本实施例提供的机器人陷入局部最小值的第一种常见情况(目标在障碍物与机器人之间)。

图1b为本实施例提供的机器人陷入局部最小值的第二种常见情况(障碍物在目标与机器人之间)。

图1c为本实施例提供的机器人陷入局部最小值的第三种常见情况(障碍物群与目标构成的合力为零)。

图2为本实施例提供的在激光雷达可视范围内一个障碍物时局部目标点的安全距离示意图。

图3为本实施例提供的在激光雷达可视范围内一个障碍物时局部目标点的安全距离示意图。

图4为本实施例提供的改进型人工势场法总体流程图。

图5为本实施例提供的局部目标点选取流程图。

图6为本实施例提供的改进型人工势场法系统原理框图。

具体实施方式

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

实施例1

本实施例提供的基于改进型人工势场法的机器人路径规划方法，

本实施例提供了一种基于改进型人工势场法的机器人路径规划方法，包括以下步骤：

S1：获取机器人的初始化状态参数、环境信息和最终目标点；

S2：获取机器人当前坐标位置和局部目标点；

S3：建立基于时间虚拟驱动力的人工势场法生成机器人当前坐标位置和局部目标点之间的可达路径；

S4：控制机器人沿可达路径行进；

S5：在激光雷达可视范围内检测机器人当前坐标位置是否达到局部目标点，如果没有达到，则返回步骤S4继续控制驱动机器人行进；

S6：如果达到局部目标点，则检测机器人是否达到最终目标点，如果没有达到，则返回步骤S2；

S7：如果达到最终目标点，则结束机器人的行进。

所述局部目标点的确定，具体包括以下步骤：

S21：检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则设置最终目标点作为局部目标点；

S22：如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍物充分安全距离的任一点最为新坐标点作为局部目标点；

S23：如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试探点寻找斥力场合力零的坐标作为局部目标点。

所述试探点寻找斥力场合力零的坐标具体按照以下步骤来实现：

S231：按照以下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，对于所有的q′∈C_obs，ρ(q)＝min||q-q′||；

S232：根据斥力势函数按照以下公式确定机器人所受的排斥力：

式中，用q_c表示障碍物区域C_obs上距离q最近的位置点；

ρ(q)＝||q-q_c||；

是由q_c指向q的单位向量，

S233：选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力势场，

S234：根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。

所述机器人在达到局部目标点之前还包括以下步骤来实现：

S31：建立机器人与障碍物之间的斥力势函数并计算斥力：

S32：建立机器人与局部目标点之间的引力势函数；

式中：

ε：人工势场法引力势场增益参数；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离；

S33：按照以下公式计算局部目标点对机器人的引力：

S34：通过以下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；

S35：按照以下公式计算时间虚拟驱动力的大小：

式中：

γ：调节F_time的常数。

本实施例还提供了一种基于改进型人工势场法的机器人路径规划系统，包括机器人参数信息采集单元、机器人位置采集单元、局部目标点生成单元、可达路径生成单元、进行控制单元、局部目标点判断单元和最终目标点判断单元；

所述机器人参数信息采集单元，用于获取机器人的初始化状态参数、环境信息和最终目标点；

所述机器人位置采集单元，用于获取机器人当前坐标位置；

所述局部目标点生成单元，用于确定机器人行进中的局部目标点；

所述可达路径生成单元，用于计算机器人当前位置和局部目标点之间的可达路径；

所述进行控制单元，用于控制、驱动机器人沿可达路径行进；

所述局部目标点判断单元，用于计算并判断机器人当前坐标位置是否达到局部目标点；

所述最终目标点判断单元，用于计算并判断机器人是否达到最终目标点；

所述机器人参数信息采集单元和机器人位置采集单元分别与局部目标点生成单元连接；所述机器人位置采集单元与可达路径生成单元连接；所述可达路径生成单元与进行控制单元连接；所述局部目标点判断单元和最终目标点判断单元分别与行进控制单元连接。

所述局部目标点生成单元是按照以下步骤来进行的：

S11：检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则设置最终目标点作为局部目标点；

S12：如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍物充分安全距离的任一点最为新坐标点作为局部目标点；

S13：如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试探点寻找斥力场合力零的坐标作为局部目标点；

所述试探点寻找斥力场合力零的坐标具体按照以下步骤来实现：

S14：按照以下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，对于所有的q′∈C_obs，ρ(q)＝min||q-q′||；

S15：根据斥力势函数按照以下公式确定机器人所受的排斥力：

式中，用q_c表示障碍物区域C_obs上距离q最近的位置点；

ρ(q)＝||q-q_c||；

是由q_c指向q的单位向量，

S16：选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力势场，

S17：根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。

还包括与进行控制单元连接的时间虚拟驱动力单元；所述时间虚拟驱动力单元机是通过以下步骤来实现：

S41：建立机器人与障碍物之间的斥力势函数并计算斥力：

S42：建立机器人与局部目标点之间的引力势函数；

式中：

ε：人工势场法引力势场增益参数；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离；

S43：按照以下公式计算局部目标点对机器人的引力：

S44：通过以下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；

S45：按照以下公式计算时间虚拟驱动力的大小：

式中：

γ：调节F_time的常数。

实施例2

基于人工势场法的机器人路径规划会出现局部最小的情景有三种：图1a-c为本实施例提供的基于人工势场法的机器人路径规划会出现局部最小情景示意图；图1a为本实施例提供的机器人陷入局部最小值的第一种常见情况(目标在障碍物与机器人之间)；图中方框表示障碍物影响范围，方框中的圆点表示目标，方框外部圆圈为机器人；图1b为本实施例提供的机器人陷入局部最小值的第二种常见情况(障碍物在目标与机器人之间)；图中方框表示障碍物影响范围，方框上方的圆点表示目标，方框外部圆圈为机器人；图1c为本实施例提供的机器人陷入局部最小值的第三种常见情况(障碍物群与目标构成的合力为零)；图中大圆圈表示障碍物影响范围，大圆圈上方的圆点表示目标，大圆圈外部下方的圆圈为机器人；

针对如图1所示三种情景，当用传统的势场法对机器人进行路径规划时不能解决该局部极小点问题，且现有方法实时性、环境适应性效率不高的情况下。

本实施例在对传统的人工势场法进行了改进，即改进引力势函数，同时将整个任务划分为许多局部目标点，从而达到最优的路径。

新的引力势函数把机器人到达目标的预计时间考虑进去，并将该时间转化为虚拟的引力，其一，保证目标点为整个势场的全局极小点，从而避免传统人工势场法出现的局部极小点的出现；其二，由于引入局部目标点后将路径规划为折线，路径虽为最优路径，但是不适用于机器人控制，通过时间虚拟驱动力可增加路径平滑度，解决机器人在行进过程中的控制量突变的情况，从而实现机器人的平稳行进。

加入了局部目标点后，可以将机器人的路径规划任务划分为很多子任务，只需控制每个子任务的路径都是最优、且最利于机器人行进控制，那么，整体的路径就可以呈现最优的状态。加入局部目标点后，就可以主动避免上述第二种出现局部极小点的情景，而无需增加额外的判断和计算，因此提高效率。

根据上面的表述，本实施例通过以下技术方案实现的：

步骤1：以起始点的机器人视角为整个路径规划的基准，建立平面直角坐标系(机器人视角的正前方为y轴，正右方为x轴)。

步骤2：利用传感器获取机器人在起始点的初始化状态S₀(包括机器人位置的直角坐标(x₀，y₀)＝(0，0)、当前机器人相对起始位置的姿态角θ₀＝0°及当前机器人速度v₀＝0)。

步骤3：利用单线激光雷达传感器获得环境信息，包括环境中障碍物信息。并在机器人控制系统中设置最终目标点的直角坐标(x_g，y_g)(为了配合后面的局部目标点，将之称为全局目标点)。

步骤4：选取局部目标点。根据步骤3中设置的全局目标点坐标与当前机器人坐标位置，检测两个位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则全局目标点就是最后一个局部目标点，反之，按照下述方法选取障碍物之间最优的通过点作为局部目标点。

选取障碍物之间的局部目标点均在激光雷达可视范围内(一般单线激光雷达的视角范围是-120°～+120°，实际应用中常采用-180°～+180°)。具体步骤如下：

建立障碍物对机器人造成的人工斥力势场模型：

斥力势函数定义为：

(式1)

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，也就是说对于所有的q′∈C_obs，ρ(q)＝min||q-q′||。

按照势场力的定义，势场力是势场函数的梯度函数，因此，斥力即如下式所示：

(式2)

因此，机器人所受的排斥力为：

(式3)

用q_c表示障碍物区域C_obs上距离q最近的位置点，也就是ρ(q)＝||q-q_c||。则是由q_c指向q的单位向量，即：

(式4)

在激光雷达可视范围内所有障碍物个数不少于2个时，选取距离全局目标点距离最近的2个障碍物，并作出这两个障碍物在平面上呈现的斥力势场。选取该斥力势场下，某“试探点(如图2中G点)”所受斥力合力为0的点作为局部目标点(x_g，y_g)。图2和图3中上方的圆点表示目标，下方的方块表示Ladar，如图2所示：

在激光雷达可视范围内障碍物只有1个时，此时的局部目标点即在距障碍物保持机器人的安全距离。如图3所示G点即为该情况下的局部目标点：

步骤5：检测机器人当前坐标与局部目标点距离，当距离小于ρ₀，则认为机器人已经到达目标点，反之则继续往下：

建立斥力势函数：

斥力势函数的建立如同步骤4所示，同样的，斥力即为势场的梯度函数，公式见(式1)。

建立引力势函数

目标点的引力势函数

引力势函数定义为：

(式5)

所有参数见步骤4所示。

按照势场力的定义，势场力是势场函数的梯度函数，因此，目标点对机器人的引力即如下式所示：

(式6)

预计到达时间的虚拟驱动力势函数

本发明将预计到达目标点的时间作为引力来源之一，不同于其他的改进型人工势场法不同，当机器人陷入局部极小点时，改变条件破坏平衡。本发明提出的虚拟作用力从规划开始就一直存在。该引力大小和方向均受到预计到达目标点时间有关。如下所示：

预计到达目标点时间算法：

通过惯性导航系统可以计算机器人当前的位置(x_i，y_i)、速度v_i及姿态角θ_i，结合距离目标点直线距离，在速度v_i条件下，计算出预计到达目标点时间。

机器人当前位置距离局部目标点的距离(欧氏距离)是：

机器人预计到达目标点的时间为：

时间虚拟驱动力的方向：

在机器人在处于障碍物群时，该发明的思想是不仅要避开这些障碍物，同时，更渴望机器人早点到达目标点，然而，在不控制机器人速度的情况下，使其更快的到达目标点，即需要路径最短。因此，来自预计到达时间的虚拟吸引力的方向需要与来自目标点对机器人的虚拟吸引力方向一致。

时间虚拟驱动力的大小：

预计到达时间最理想的情况下就是机器人当前位置到目标点的直线距离。如上式所述。同时，经过分析，机器人陷入上述3种情形的局部最小时，有一个共性：合力为零，或者吸引力与斥力呈180°。因此，当处于这种状态时需要时间虚拟驱动力必须至少可以平衡斥力，从而保留目标点的吸引力。因此，在此提出以下时间虚拟驱动力公式：

式中：

γ：调节F_time的常数，使得当来自目标的吸引力与来自障碍物的排斥力反向时，F_time≥F_rep。

本实施例的引力势函数把机器人到达目标的预计时间考虑进去，并将该时间转化为虚拟的引力，其一，保证目标点为整个势场的全局极小点，从而避免传统人工势场法出现的局部极小点的出现；其二，由于引入局部目标点后将路径规划为折线，路径虽为最优路径，但是不适用于机器人控制，通过时间虚拟驱动力可增加路径平滑度，解决机器人在行进过程中的控制量突变的情况，从而实现机器人的平稳行进。

综合上述的虚拟力建立规则，从而建立基于时间虚拟驱动力的人工势场法模型，模型如下：

通过上述模型即可得出机器人前进的方向。根据上述方向即可得到下一单位时刻机器人的位置及方向，如此往复，直到到达局部目标点。从而形成在单位时刻序列上的离散路径。

步骤6：继续检测全局目标点是否在机器人直线视角内且可达，如果在，该全局目标点即为最后一个目标点，如果不在，继续循环步骤4及步骤5直到全局目标点在机器人视角范围内并可达。

实施例3

流程图见图4，下面结合流程图，对本发明实施例中的技术方案进行完整地描述。

步骤1：以起始点的机器人视角为整个路径规划的基准，建立平面直角坐标系(机器人视角的正前方为y轴，正右方为x轴)。

步骤2：利用惯性导航系统获取机器人在起始点的初始化状态S₀(包括机器人位置的直角坐标(x₀，y₀)＝(0，0)、当前机器人相对起始位置的姿态角θ₀＝0°及当前机器人速度v₀＝0)。

步骤3：利用单线激光雷达传感器获得环境信息，包括环境中障碍物信息。并在机器人控制系统中设置最终目标点的直角坐标(x_g，y_g)(为了配合下文的局部目标点，将之称为全局目标点)。

步骤4：选取局部目标点。根据步骤3中设置的全局目标点坐标与当前机器人坐标位置，检测两个位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则全局目标点就是最后一个局部目标点，反之，按照下述方法选取障碍物之间最优的通过点作为局部目标点。

选取障碍物之间的局部目标点均在激光雷达可视范围内(一般单线激光雷达的视角范围是-120°～+120°，实际应用中常采用-180°～+180°)。具体步骤如下：

建立障碍物对机器人造成的人工斥力势场模型：

斥力势函数定义为：

(式1)

式中：

δ：相应的正比例位置增益系数，一般取0.5；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离一般取0.5；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，也就是说对于所有的q′∈C_obs，ρ(q)＝min||q-q′||。

按照势场力的定义，势场力是势场函数的梯度函数，因此，斥力即如下式所示：

(式2)

因此，机器人所受的排斥力为：

(式3)

用q_c表示障碍物区域C_obs上距离q最近的位置点，也就是ρ(q)＝||q-q_c||。则是由q_c指向q的单位向量，即：

(式4)

在激光雷达可视范围内所有障碍物个数不少于2个时，选取距离全局目标点距离最近的2个障碍物，并做出这两个障碍物在平面上呈现的斥力势场。选取该斥力势场下，某“试探点”所受斥力合力为0的点作为局部目标点(x_g，y_g)。

在激光雷达可视范围内障碍物只有1个时，此时的局部目标点即在距障碍物保持机器人的安全距离。

步骤5：检测机器人当前坐标与局部目标点距离，当距离小于ρ₀，则认为机器人已经到达目标点，反之则继续往下：

建立斥力势函数：

斥力势函数的建立如同步骤4所示，同样的，斥力即为势场的梯度函数，公式见(式1)。

建立引力势函数

目标点的引力势函数

引力势函数定义为：

(式5)

式中：

ε：人工势场法引力势场增益参数，一般取-6；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离。

按照势场力的定义，势场力是势场函数的梯度函数，因此，目标点对机器人的引力即如下式所示：

(式6)

预计到达时间的虚拟驱动力势函数

本发明将预计到达目标点的时间作为引力来源之一，不同于其他的改进型人工势场法不同——当机器人陷入局部极小点时，改变条件破坏平衡。本发明提出的虚拟作用力从规划开始就一直存在。该引力大小和方向均受到预计到达目标点时间有关。条件及关系如下所示：

机器人陷入局部极小点的条件：

机器人在处于局部极小点时，即如图1所示的三种常见场景。机器人受到来自目标点的引力和障碍物的斥力刚好处于平衡状态或二者合力处于震荡状态。无论是哪一种状态，都必须先经历合力为零的状态，因此，将合力为零作为局部极小点出现的条件。即：

预计到达目标点时间算法：

通过惯性导航系统可以计算机器人当前的位置(x_i，y_i)、速度v_i及姿态角θ_i，结合距离目标点直线距离，在速度v_i条件下，计算出预计到达目标点时间。

机器人当前位置距离局部目标点的距离(欧氏距离)是：

机器人预计到达目标点的时间为：

时间虚拟驱动力的方向：

在机器人在处于障碍物群时，该发明的思想是不仅要避开这些障碍物，同时，更渴望机器人早点到达目标点，然而，在不控制机器人速度的情况下，使其更快的到达目标点，即需要路径最短。因此，来自预计到达时间的虚拟吸引力的方向需要与来自目标点对机器人的虚拟吸引力方向一致。

时间虚拟驱动力的大小：

预计到达时间最理想的情况下就是机器人当前位置到目标点的直线距离。如上式所述。同时，经过分析，机器人陷入上述3种情形的局部最小时，有一个共性：合力为零，或者吸引力与斥力呈180°。因此，当处于这种状态时需要时间虚拟驱动力必须至少可以平衡斥力，从而保留目标点的吸引力。因此，在此提出以下时间虚拟驱动力公式：

式中：

γ：调节F_time的常数，使得当来自目标的吸引力与来自障碍物的排斥力反向时F_time≥F_rep，一般取值为：0.1

综合上述的虚拟力建立规则，从而建立基于时间虚拟驱动力的人工势场法模型，模型如下：

通过上述模型即可得出机器人前进的方向。根据上述方向即可得到下一单位时刻机器人的位置及方向，如此往复，直到到达局部目标点。从而形成在单位时刻序列上的离散路径。

步骤6：继续检测全局目标点是否在机器人直线视角内且可达，如果在，该全局目标点即为最后一个目标点，如果不在，继续循环步骤4及步骤5直到全局目标点在机器人视角范围内并可达。

最后说明的是，以上实施例仅用以说明本发明的技术方案而非限制，尽管参照较佳实施例对本发明进行了详细说明，本领域的普通技术人员应当理解，可以对本发明的技术方案进行修改或者等同替换，而不脱离本技术方案的宗旨和范围，其均应涵盖在本发明的权利要求范围当中。

Claims (3)

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

S1：获取机器人的初始化状态参数、环境信息和最终目标点；

S2：获取机器人当前坐标位置和局部目标点；

S3：建立基于时间虚拟驱动力的人工势场法生成机器人当前坐标位置和局部目标点之间的可达路径；

S4：控制机器人沿可达路径行进；

S5：在激光雷达可视范围内检测机器人当前坐标位置是否达到局部目标点，如果没有达到，则返回步骤S4继续控制驱动机器人行进；

S6：如果达到局部目标点，则检测机器人是否达到最终目标点，如果没有达到，则返回步骤S2；

S7：如果达到最终目标点，则结束机器人的行进；

所述局部目标点的确定，具体包括以下步骤：

S21：检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则设置最终目标点作为局部目标点；

S22：如果有障碍物，则判断障碍物的个数是否小于两个，如果是，则设置离此障碍物充分安全距离的任一点作为新坐标点作为局部目标点；

S23：如果否，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试探点寻找斥力场合力零的坐标作为局部目标点；

所述试探点寻找斥力场合力零的坐标具体按照以下步骤来实现：

S231：按照以下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，对于所有的q′∈C_obs，ρ(q)＝min||q-q′||；

S232：根据斥力势函数按照以下公式确定机器人所受的排斥力：

式中，用q_c表示障碍物区域C_obs上距离q最近的位置点；

ρ(q)＝||q-q_c||；

是由q_c指向q的单位向量，

S233：选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力势场，

S234：根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点；

所述机器人在达到局部目标点之前还包括以下步骤来实现：

S31：建立机器人与障碍物之间的斥力势函数并计算斥力：

S32：建立机器人与局部目标点之间的引力势函数；

式中：

ε：人工势场法引力势场增益参数；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离；

S33：按照以下公式计算局部目标点对机器人的引力：

S34：通过以下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；

S35：按照以下公式计算时间虚拟驱动力的大小：

式中：

γ：调节F_time的常数，(x_g，y_g)表示最终目标点的直角坐标，当前的位置(x_i，y_i)表示机器人的当前的位置、v_i为速度。

2.一种基于改进型人工势场法的机器人路径规划系统，其特征在于：包括机器人参数信息采集单元、机器人位置采集单元、局部目标点生成单元、可达路径生成单元、进行控制单元、局部目标点判断单元和最终目标点判断单元；

所述机器人参数信息采集单元，用于获取机器人的初始化状态参数、环境信息和最终目标点；

所述机器人位置采集单元，用于获取机器人当前坐标位置；

所述局部目标点生成单元，用于确定机器人行进中的局部目标点；

所述可达路径生成单元，用于计算机器人当前位置和局部目标点之间的可达路径；

所述进行控制单元，用于控制、驱动机器人沿可达路径行进；

所述局部目标点判断单元，用于计算并判断机器人当前坐标位置是否达到局部目标点；

所述最终目标点判断单元，用于计算并判断机器人是否达到最终目标点；

所述机器人参数信息采集单元和机器人位置采集单元分别与局部目标点生成单元连接；所述机器人位置采集单元与可达路径生成单元连接；所述可达路径生成单元与进行控制单元连接；所述局部目标点判断单元和最终目标点判断单元分别与行进控制单元连接；

所述局部目标点生成单元是按照以下步骤来进行的：

S11：检测最终目标点和当前坐标位置之间可通过的直线路径下有无障碍物，如果没有障碍物，则设置最终目标点作为局部目标点；

S12：如果有障碍物，则判断障碍物的个数是否小于两个，如果否，则设置离此障碍物充分安全距离的任一点最为新坐标点作为局部目标点；

S13：如果是，则建立离最终目标点最近的两个障碍物呈现的斥力势场，并构建试探点寻找斥力场合力零的坐标作为局部目标点；

所述试探点寻找斥力场合力零的坐标具体按照以下步骤来实现：

S14：按照以下斥力势函数建立障碍物对机器人造成的人工斥力势场模型；

式中：

δ：相应的正比例位置增益系数；

ρ₀：正常数，表示障碍物区域可对机器人的运动产生影响的最大距离；

ρ(q)：某一障碍物区域C_obs到位置q的最小距离，对于所有的q′∈C_obs，ρ(q)＝min||q-q′||；

S15：根据斥力势函数按照以下公式确定机器人所受的排斥力：

式中，用q_c表示障碍物区域C_obs上距离q最近的位置点；

ρ(q)＝||q-q_c||；

是由q_c指向q的单位向量，

S16：选取距离全局目标点距离最近的障碍物，并获取障碍物在平面上呈现的斥力势场，

S17：根据斥力势场计算排斥力，选取排斥力合力为零的点作为局部目标点。

3.如权利要求2所述的基于改进型人工势场法的机器人路径规划系统，其特征在于：还包括与进行控制单元连接的时间虚拟驱动力单元；所述时间虚拟驱动力单元机是通过以下步骤来实现：

S41：建立机器人与障碍物之间的斥力势函数并计算斥力：

S42：建立机器人与局部目标点之间的引力势函数；

式中：

ε：人工势场法引力势场增益参数；

ρ_g(q)：机器人当前位置距离局部目标点的欧氏距离；

S43：按照以下公式计算局部目标点对机器人的引力：

S44：通过以下公式来计算机器人的引力和斥力的合力为零时确定局部极小点；

S45：按照以下公式计算时间虚拟驱动力的大小：

式中：

γ：调节F_time的常数，(x_g，y_g)表示最终目标点的直角坐标，当前的位置(x_i，y_i)表示机器人的当前的位置、v_i为速度。