CN108469822B - 一种室内导盲机器人在动态环境下的路径规划方法 - Google Patents

Abstract

一种室内导盲机器人在动态环境下的路径规划方法，通过生成环境的栅格地图、膨胀处理、给定目标点、采集地图信息、规划出一条全局路线、局部路径规划、避障及返回全局路线的方式，完成任意动态环境下的导盲机器人的路线规划及运动；降低了局部避障的随机性，提高了动态环境下避障的可靠性；随机路图法避免局部极小值，考虑规划效率，不会增加计算量；导盲机器人会尽可能的沿着全局路线行走，这样大大的保证了盲人在行进过程中的安全。

Description

一种室内导盲机器人在动态环境下的路径规划方法

(一)技术领域：

本发明属于机器人相关技术领域，是一种导盲机器人路径规划算法的设计，特别是一种室内导盲机器人在动态环境下的路径规划方法。

(二)背景技术：

导盲机器人可以为盲人的出行带来方便，一般用在家庭、商场、医院等场所。在这些场景中，会同时存在静态障碍物和动态障碍物，这对于导盲机器人来说是一个挑战。对于已知的静态障碍物导盲机器人可以很好的躲避，但是当环境中的信息发生了改变，这时候就需要导盲机器人利用局部规划器躲避障碍物了。局部路径是需要实时规划的，这就表示局部规划器对实时信息的依赖性比较大。

人工势场法是根据物理学中力学的原理提出的一种虚拟力法，它的中心思想是让障碍物产生对导盲机器人的斥力场，在目标点周围形成引力场，导盲机器人在这两个场的作用下进行运动。人工势场法具有比较好的实时性，满足局部规划的条件，但是该方法存在局部最优点问题，而且在躲避障碍物的时候存在随机性，这对于导盲机器人来说是需要避免的，减小局部规划中的随机性可以大大提高导盲机器人的安全性。

随机路图法(PRM——Probabilistic Road Maps)在规划空间中随机的选取一些点，然后将这些点进行连接，将穿过障碍物的连线去掉，最终得到一个随机路图供查询路径。随机路图法面对环境中复杂的障碍物时也能很好的解决，它克服了路径规划中易于出现局部极小的缺点。

(三)发明内容：

本发明的目的在于提供一种室内导盲机器人在动态环境下的路径规划方法，针对导盲机器人无法提前检测到的动态区域，采用人工势场法和随机路径图法进行局部规划，使导盲机器人可以安全的避开动态障碍；本发明降低了局部避障的随机性，避免规划的路径陷入局部极小值，尽可能让导盲机器人沿着全局路线行走，是一种简单易行的路径规划方法。

一种室内导盲机器人在动态环境下的路径规划方法，其特征在于它包括以下步骤：

(1)生成导盲机器人工作地点所在环境的栅格地图，并对该地图中的障碍物进行膨胀处理，通过调节inflation_radius障碍物膨胀参数，使最佳路径尽可能远离每侧的障碍物，因此机器人可以在障碍物中间移动；

(2)给定导盲机器人需要到达的目标点，导盲机器人将目标点确定在栅格地图相应的位置上，并根据当前的位置、需要到达的目标点以及提前采集好的栅格地图，利用静态路网中求解最短路径最有效的直接搜索方法，简称A*算法，该算法是一种启发式搜索方法，在搜索中加入了与问题有关的启发性信息，指导搜索朝最有希望的方向进行，用于搜索状态空间的最短路径。通过该算法规划出一条全局路线，使导盲机器人之后的行动始终围绕这条路径进行，此路径是基于事先扫描的静态地图的；

(3)若此时室内摆放的东西位置发生了变化，即障碍物与静态地图上的不同，则导盲机器人要根据障碍物出现的位置进行局部路径规划对障碍物进行躲避；

(4)导盲机器人按照规划路径向目标点移动，当导盲机器人检测到前方有障碍物时，机器人先减速并对障碍物的大小和周围环境进行估计，并向比较容易躲避的方向偏转，然后利用人工势场法将导盲机器人的路径重新规划到躲开障碍物的地方，并可以根据激光雷达反馈的数据得知障碍物后方的具体情况；

(5)当导盲机器人成功的检测到了障碍物后面的实际情况后，根据步骤(1)规划出的全局路径和导盲机器人当前的位置及姿态，在它能够看到障碍物后方的全局路线上寻找一个合适的点作为局部目标点，使导盲机器人能够再次行驶到全局路线上来；同时利用随机路径图法规划导盲机器人当前位置到局部目标点之间的路径。

所述步骤(4)中利用人工势场法将导盲机器人的路径重新规划的方法为：

①当碰到障碍物时，则会根据引力函数和斥力函数求二者的合力，其中，

引力函数为：F_att(X)＝k(X_G-X_R)

斥力函数为：

其合力为：

式中，k表示引力增益，η_X表示斥力增益系数，ρ(X_R,X_i)表示障碍物到导盲机器人的距离，ρ(X_R,X_G)表示导盲机器人到全局目标点的距离，ρ₀表示障碍物可以影响到导盲机器人的最大距离，X_R表示机器人当前的坐标，X_i表示障碍物的当前坐标，X_G代表全局目标点的坐标；

②根据合力结果，进行基于人工势场法的局部路径规划：

将机器人的运动环境抽象成一个人造引力场内运动，假定环境中的目标点与机器人之间生有“吸引力”，障碍物与机器人有“排斥力”，由二力的合力控制机器人运动，以此规划出一条平滑安全的路径。

所述步骤(5)中选择局部目标点的方法为：

①假设步骤(4)中导盲机器人躲开障碍物的地方即为b点，局部目标点记为c点，则由b点做一条与障碍物相切的延长线，取该延长线与全局路径的交点，即为c点；

②如果c点上有障碍物或者c点在膨胀距离范围内，则此时放弃c点，再次利用人工势场法找到另一个可以躲开障碍物的地点d点，并步骤用①同样的方法去选取e点作为新的局部目标点，利用随机路图法进行规划。

所述步骤(5)的步骤②中随机路图法的规划步骤由以下几步构成：

1)随机路图法首先需要构建路线图，即在构型空间中随机的选择点v，如果v不属于障碍物上的点，将v加入到图结点集合V中；

2)将v与集合V中的其他点v′进行连接，如果能够连接，则将这条边(v,v′)加入到对应的路线子图N中，路线子图N为单条路线的集合，否则将v加入到一个新的路线子图；

3)如此重复进行以上两个步骤，当采样点能够连接多个子图时，则这些子图会被合成为一个新的子图，由此最终得到路线图；

4)将起点和终点加入到路线图中，利用路线图搜索出一条路径，作为局部路径。

本发明的优越性：降低了局部避障的随机性，提高了导盲机器人在动态环境下避障的可靠性；引用的随机路图法避免了规划的路径陷入局部极小值，并且将规划效率考虑进来，不会增加计算量；导盲机器人会尽可能的沿着全局路线行走，这样大大的保证了盲人在行进过程中的安全。

(四)附图说明：

图1为本发明所涉一种室内导盲机器人在动态环境下的路径规划方法中当环境中有一个障碍物的情况下导盲机器人进行避障的路线规划过程原理示意图。

图2为本发明所涉一种室内导盲机器人在动态环境下的路径规划方法中当环境中出现多个障碍物的情况下小车进行避障的路线规划过程原理示意图。

图3为本发明所涉一种室内导盲机器人在动态环境下的路径规划方法的工作流程示意图。

(五)具体实施方式：

实施例：一种室内导盲机器人在动态环境下的路径规划方法，如图3，其特征在于它包括以下步骤：

(1)生成导盲机器人工作地点所在环境的栅格地图，并对该地图中的障碍物进行膨胀处理，通过调节inflation_radius障碍物膨胀参数，使最佳路径尽可能远离每侧的障碍物，因此机器人可以在障碍物中间移动；

(2)给定导盲机器人需要到达的目标点，导盲机器人将目标点确定在栅格地图相应的位置上，并根据当前的位置、需要到达的目标点以及提前采集好的栅格地图，利用静态路网中求解最短路径最有效的直接搜索方法，简称A*算法，该算法一种启发式搜索方法，在搜索中加入了与问题有关的启发性信息，指导搜索朝最有希望的方向进行，用于搜索状态空间的最短路径。通过该算法规划出一条全局路线，使导盲机器人之后的行动始终围绕这条路径进行，此路径是基于事先扫描的静态地图的；

(3)若此时室内摆放的东西位置发生了变化，即障碍物与静态地图上的不同，则导盲机器人要根据障碍物出现的位置进行局部路径规划对障碍物进行躲避，如图1所示；

(4)导盲机器人按照规划路径向目标点移动，当导盲机器人检测到前方有障碍物时，机器人先减速并对障碍物的大小和周围环境进行估计，并向比较容易躲避的方向偏转，然后利用人工势场法将导盲机器人的路径重新规划到躲开障碍物的地方，并可以根据激光雷达反馈的数据得知障碍物后方的具体情况；如图1所示；

①当碰到障碍物时，则会根据引力函数和斥力函数求二者的合力，其中，

引力函数为：F_att(X)＝k(X_G-X_R)

斥力函数为：

其合力为：

式中，k表示引力增益，η_X表示斥力增益系数，ρ(X_R,X_i)表示障碍物到导盲机器人的距离，ρ(X_R,X_G)表示导盲机器人到全局目标点的距离，ρ₀表示障碍物可以影响到导盲机器人的最大距离，X_R表示机器人当前的坐标，X_i表示障碍物的当前坐标，X_G代表全局目标点的坐标；

②根据合力结果，进行基于人工势场法的局部路径规划：

将机器人的运动环境抽象成一个人造引力场内运动，假定环境中的目标点与机器人之间生有“吸引力”，障碍物与机器人有“排斥力”，由二力的合力控制机器人运动，以此规划出一条平滑安全的路径。

(5)当导盲机器人成功的检测到了障碍物后面的实际情况后，根据步骤(1)规划出的全局路径和导盲机器人当前的位置及姿态，在它能够看到障碍物后方的全局路线上寻找一个合适的点作为局部目标点，使导盲机器人能够再次行驶到全局路线上来；同时利用随机路径图法规划导盲机器人当前位置到局部目标点之间的路径，如图2所示：

①假设步骤(4)中导盲机器人躲开障碍物的地方即为b点，局部目标点记为c点，则由b点做一条与障碍物相切的延长线，取该延长线与全局路径的交点，即为c点；

②如果c点上有障碍物或者c点在膨胀距离范围内，如图2所示，则此时放弃c点，再次利用人工势场法找到另一个可以躲开障碍物的地点d点，并步骤用①同样的方法去选取e点作为新的局部目标点，利用随机路图法进行规划。

随机路图法的规划步骤由以下几步构成：

1)随机路图法首先需要构建路线图，即在构型空间中随机的选择点v，如果v不属于障碍物上的点，将v加入到图结点集合V中；

2)将v与集合V中的其他点v′进行连接，如果能够连接，则将这条边(v,v′)加入到对应的路线子图N(路线子图为单条路线的集合)中，否则将v加入到一个新的路线子图；

3)如此重复进行以上两个步骤，当采样点能够连接多个子图时，则这些子图会被合成为一个新的子图，由此最终得到路线图；

4)将起点和终点加入到路线图中，利用路线图搜索出一条路径，作为局部路径。

下面结合具体实施例，进行比较详细的阐述。应理解，这些实施例仅用于说明本发明而不用于限制本发明的范围。在阅读了本发明讲述的具体步骤和相关内容之后，相关技术人员可以对本发明作各种改动或应用，这些等价形式同样属于本申请所附权利要求书所限定的范围。

将室内地图提前扫描出来，然后上传到导盲系统中并根据障碍信息转换为栅格地图，在栅格地图中将障碍物进行膨胀处理。盲人通过语音模块给定想要到达的地点给导盲机器人，导盲机器人将目标点确定在栅格地图相应的位置上，利用A*算法规划出到目标点的一条全局路径。

规划出来的全局路径是基于事先扫描的静态地图，如果此时室内摆放的东西位置发生了变化则不在全局规划器的考虑范围，此时极有可能会有障碍物出现在全局路线上。出现上述这些情况时就需要使用局部规划器躲避障碍了，如图1所示，在行进的过程中，当导盲机器人检测到前方有障碍物时，机器人先减速并对障碍物的大小和周围环境进行估计，并向比较容易躲避的方向偏转，然后利用人工势场法将导盲机器人规划到躲开障碍物的地方b点，到达b点后导盲机器人通过激光雷达反馈的数据就能知道障碍物后方的具体情况。

导盲机器人需要尽可能的在已经规划出来的全局路线上，所以在它能够看到障碍物后方的情况后会在障碍物后方的全局路线上寻找一个合适的点c作为局部目标点，使导盲机器人行驶到全局路线上来。选择c点的方法是b点与障碍物相切的延长线在全局路线上的交点，如果c点上没有障碍物且不在膨胀范围内，则点b到点c之间利用随机路图法规划一条路径；如果c点上有障碍物或者c点在膨胀距离范围内，如图2所示，则放弃c点，再次利用人工势场法到达d点，然后用同样的方法去选取e点，并利用随机路图法进行规划。

随机路图法的规划步骤如下：

1)在空间中随机的选择点v，如果v不属于障碍物上的点，将v加入到集合V中。

2)将v与集合V中的其他点进行连接，如果能够连接，将这条边(v,v′)加入到对应的子图N中，否则将v加入到一个新的子图。

3)如此重复进行以上两个步骤，当采样点能够连接多个子图时，则这些子图会被合成为一个新的子图，由此最终得到路线图。

4)将起点和终点加入到路线图中，利用路线图搜索出一条路径。

为了避免大的计算量和浪费现象，随机路图法的计算范围会根据导盲机器人当前的位置和已经选取好的局部目标点，以这两点为矩形的对角线生成一个矩形并将这两个顶点加入到计算范围，在此矩形内利用随机路图法进行规划从而得到一条路径。

在导盲机器人行走的过程中，对于检测到的桌子或者椅子腿机器人能够做出判断。当激光雷达反馈回来的是两个或者四个点时，导盲机器人的局部路径就不会穿过它们，而是在它们的边缘规划，如果有其他路径，其他路径优先，这样可以防止盲人撞到桌子或者椅子，不会出现导盲机器人成功走过去了，而盲人却被障碍物阻止了前进道路的现象。

导盲机器人根据盲人给定的目标点规划一条全局路线，如果室内的环境一直未发生变化，那么导盲机器人就是完全沿着全局路线到达目标点。如果室内的环境发生了改变，如行人、购物车、行李箱等临时障碍物出现在了已经规划出来的全局路线上，导盲机器人则首先通过人工势场法躲开被障碍物挡住的视线，然后在障碍物后方的全局路线上选取一个局部目标点，通过随机路图法规划一条局部路线，使导盲机器人成功躲避障碍物之后继续沿着全局路线前进。

Claims (3)

1.一种室内导盲机器人在动态环境下的路径规划方法，其特征在于它包括以下步骤：

(1)生成导盲机器人工作地点所在环境的栅格地图，并对该地图中的障碍物进行膨胀处理，通过调节inflation_radius障碍物膨胀参数，使最佳路径尽可能远离每侧的障碍物，因此机器人可以在障碍物中间移动；

(2)给定导盲机器人需要到达的目标点，导盲机器人将目标点确定在栅格地图相应的位置上，并根据当前的位置、需要到达的目标点以及提前采集好的栅格地图，利用静态路网中求解最短路径最有效的直接搜索方法，简称A*算法，规划出一条全局路线，使导盲机器人之后的行动始终围绕这条路径进行，此路径是基于事先扫描的静态地图的；

(3)若此时室内摆放的东西位置发生了变化，即障碍物与静态地图上的不同，则导盲机器人要根据障碍物出现的位置进行局部路径规划对障碍物进行躲避；

(4)导盲机器人按照规划路径向目标点移动，当导盲机器人检测到前方有障碍物时，机器人先减速并对障碍物的大小和周围环境进行估计，并向比较容易躲避的方向偏转，然后利用人工势场法将导盲机器人的路径重新规划到躲开障碍物的地方，并可以根据激光雷达反馈的数据得知障碍物后方的具体情况；

(5)当导盲机器人成功的检测到了障碍物后面的实际情况后，根据步骤(1)规划出的全局路径和导盲机器人当前的位置及姿态，在它能够看到障碍物后方的全局路线上寻找一个合适的点作为局部目标点，使导盲机器人能够再次行驶到全局路线上来；同时利用随机路径图法规划导盲机器人当前位置到局部目标点之间的路径；

所述步骤(5)中选择局部目标点的方法为：

①假设步骤(4)中导盲机器人躲开障碍物的地方即为b点，局部目标点记为c点，则由b点做一条与障碍物相切的延长线，取该延长线与全局路径的交点，即为c点；

②如果c点上有障碍物或者c点在膨胀距离范围内，则此时放弃c点，再次利用人工势场法找到另一个可以躲开障碍物的地点d点，并步骤用①同样的方法去选取e点作为新的局部目标点，利用随机路图法进行规划。

2.根据权利要求1所述一种室内导盲机器人在动态环境下的路径规划方法，其特征在于所述步骤(4)中利用人工势场法将导盲机器人的路径重新规划的方法为：

①当碰到障碍物时，则会根据引力函数和斥力函数求二者的合力，其中，

引力函数为：F_att(X)＝k(X_G-X_R)

斥力函数为：

其合力为：

式中，k表示引力增益，η_X表示斥力增益系数，ρ(X_R,X_i)表示障碍物到导盲机器人的距离，ρ(X_R,X_G)表示导盲机器人到全局目标点的距离，ρ₀表示障碍物可以影响到导盲机器人的最大距离，X_R表示机器人当前的坐标，X_i表示障碍物的当前坐标，X_G代表全局目标点的坐标；

②根据合力结果，进行基于人工势场法的局部路径规划：

将机器人的运动环境抽象成一个人造引力场内运动，假定环境中的目标点与机器人之间生有“吸引力”，障碍物与机器人有“排斥力”，由二力的合力控制机器人运动，以此规划出一条平滑安全的路径。

3.根据权利要求1所述一种室内导盲机器人在动态环境下的路径规划方法，其特征在于所述步骤(5)的步骤②中随机路图法的规划步骤由以下几步构成：

1)随机路图法首先需要构建路线图，即在构型空间中随机的选择点v，如果v不属于障碍物上的点，将v加入到图结点集合V中；

2)将v与集合V中的其他点v′进行连接，如果能够连接，则将这条边(v,v′)加入到对应的路线子图N中，路线子图N为单条路线的集合，否则将v加入到一个新的路线子图；

3)如此重复进行以上两个步骤，当采样点能够连接多个子图时，则这些子图会被合成为一个新的子图，由此最终得到路线图；

4)将起点和终点加入到路线图中，利用路线图搜索出一条路径，作为局部路径。

Images