CN109798909A - 一种全局路径规划的方法 - Google Patents

Abstract

本发明公开了一种全局路径规划的方法，属于机器人技术领域。针对现有技术中对机器人的路径规划中存在转折点较多，不易于操控机器人的问题，本发明提供一种全局路径规划的方法，它包括在原有的A*算法代价函数中加入角度约束函数a(n)，通过改进的A*算法代价函数完成全局路径规划第一阶段搜索，解得对机器人运动角度约束的初步全局路径，n表示机器人状态的当前节点；提取第一阶段搜索的结果进行第二阶段搜索，剔除路径中的冗余折点，得到一条符合机器人运动学原理的最终全局路径。本发明通过加入角度约束函数，且采用两阶段搜索规划出一条符合机器人运动学原理的全局路径，路径中的折点大幅度减少，在保证实时性的条件下平滑的且路程较短的全局路径。

Description

一种全局路径规划的方法

技术领域

本发明属于机器人技术领域，更具体地说，涉及一种全局路径规划的方法。

背景技术

机器人是现代人工智能和自动化技术高度发展和融合的体现，凝结了控制工程技术，计算机技术，传感器技术，信号处理，机械制造等众多学科技术发展的成果。随着相关技术的进步和发展，机器人技术也日益成熟，机器人已渗透到各个应用领域。作为高智能的体现，人们也要求机器人应有更高的自主性，具备感知、处理、决策、执行等能力，比如：在已知或未知动态空间环境下的自主路径规划。

全路径规划是指机器人在具有障碍物的环境内，按照一种或多种性能指标，寻找一条从起始点到目标点的最优无碰撞路径。针对目前已有的路径规划算法，根据机器人所处的具体环境选择合适的算法，使得机器人在环境中能够稳定运行变得至关重要。针对已知地图的全局路径规划，其基本问题是环境建模和搜索运动路径。传统全局路径规划方法如Dijkstra算法、A*算法、D*算法等，A*算法广泛应用于移动机器人自主路径规划，相比于Dijkstra算法，A*算法采用启发式的搜索方式，大幅度降低了搜索节点的数量，从而极大地提高了搜索效率，但是该算法规划的路径易出现转折点，考虑到实际机器人的运动学模型，过多的转折点并不利于机器人的控制。

针对上述问题也进行了相应的改进，如中国专利申请号CN201710253824.3，公开日为2018年11月2日，该专利公开了一种机器人全局路径规划方法，该方法包括环境建模、路径规划，该方法通过二维空间进行环境建模，且通过基于切比雪夫距离的算法实现路径规划。本发明通过二维建模和相关算法，能够有效且准确地规划机器人的路径，运算量小，运算速度快，能够满足机器人实时进行路径规划的需要。该专利的不足之处在于：虽然有效提高了运算效率，但是规划的路径仍然易出现转折点，不利于机器人的操控。

又如中国专利申请号CN201810715045.5，公开日为2019年1月8日，该专利公开了一种仿人机器人路径规划方法及装置，所述方法包括：对进行路径规划的待检索列表中目标栅格地图各结点的评价函数值进行排序，将最小的所述评价函数值对应的结点作为当前结点；根据各所述相邻结点和所述预设终点结点的坐标，获取各所述相邻结点与所述预设终点结点之间的连线；若所述连线中没有所述预设障碍结点，则将所述连线作为从各所述相邻结点到所述预设终点结点之间的第一分段路径，将所述第一分段路径和预先基于A*算法获取的第二分段路径作为从所述预设起始结点到所述预设终点结点的总路径。本发明通过对目标栅格地图的整体判断，直接将确定的连线作为从相邻结点到预设终点结点的路径，从而大大提高路径规划的效率。该专利的不足之处在于：该专利通过约束机器人的偏航时间来间接约束偏转角度，这就关系到机器人本身的一系列特性如机器人的角速度，角加速度等，对路口的规划存在一定的影响。

发明内容

1、要解决的问题

针对现有技术中对机器人的路径规划中存在转折点较多，不易于操控机器人的问题，本发明提供一种全局路径规划的方法。通过加入角度约束函数，且采用两阶段搜索规划出一条符合机器人运动学原理的全局路径，路径中的折点大幅度减少，在保证实时性的条件下平滑的且路程较短的全局路径，整体运算效率较高，机器人容易操控。

2、技术方案

为解决上述问题，本发明采用如下的技术方案。

一种全局规划路径的方法，包括以下步骤：

(1)在原有的A*算法代价函数中加入角度约束函数a(n)，通过改进的A*算法代价函数完成全局路径规划第一阶段搜索，解得对机器人运动角度约束的初步全局路径，n表示机器人状态的当前节点；

(2)提取第一阶段搜索的结果进行第二阶段搜索，剔除路径中的冗余折点，得到一条符合机器人运动学原理的最终全局路径。

更进一步的，所述角度约束函数a(n)＝k*(θ_n-θ_n-1)，其中k为转角约束系数，θ_n为当前时刻机器人的朝向角，θ_n-1为上一时刻机器人的朝向角。

更进一步的，步骤(1)中改进后的A*算法代价函数为：f(n)＝s(n)+h(n)+a(n)，其中，n表示机器人状态的当前节点，f(n)为机器人全局路径规划的代价函数，s(n)表示机器人从起始点到当前节点n所实际花费的代价，h(n)为启发函数，表示机器人从当前节点n到目标点的启发估计代价，a(n)为角度约束函数。

更进一步的，在进行步骤(1)之前还包括以下步骤：

(4)定位，根据机器人位姿和里程计的控制信息获取机器人的初始位姿；

(5)根据初始位姿及及机器人自身携带的传感器的观测数据创建二维栅格地图。

更进一步的，所述步骤(5)中二维栅格地图中的每个基本单位有三种状态，分别为：占有、空闲和未知，对栅格的占有方式来知晓机器人的导航情况，为步骤(1)做准备。

更进一步的，所述步骤(2)包括以下几个步骤：

(2.1)提取第一阶段搜索生成的规划路径节点集P(N),P(N)集包含起始节点到目标节点以及它们之间的所有节点；

(2.2)从起始节点P(1)向后遍历节点，找到所有折点处的节点集T(M)；

(2.3)遍历折点节点集T(M)中的所有节点，判断当前折点T(n)的前后相邻节点T(n-1)和T(n+1)的连线是否穿过障碍物，若没有则判定当前节点T(n)为冗余折点；

(2.4)删除折点节点集T(M)中所有冗余折点后，在节点集P(N)余下的所有节点顺序连线，即为一条符合机器人运动学原理的最终全局路径。

3、有益效果

相比于现有技术，本发明的有益效果为：

(1)本发明通过在原有的A*算法代价函数中加入角度约束函数a(n)完成全局路径规划的第一阶段搜索，生成在保证实时性的条件下平滑的且路程较短的初步全局路径；再将第一阶段的搜索结果进行二次搜索，剔除路径中的冗余折点，得到了一条符合机器人运动学原理的最终的全局路径，折点大幅度减少，使得机器人的运动操控更加便利与精准；

(2)本发明所述的角度约束函数中当机器人上一时刻与当前时刻的朝向角相同时，a(n)＝0，当机器人的转角越大，则a(n)值越大，机器人会优先选择较小代价值的路径，角度约束函数的加入直接通过相邻节点的方向偏角进行约束，与机器人本身的特性没有关系，有效抑制了折点的产生，规划的路径转折点较少，机器人方便操控；

(3)本发明所述的第二阶段每一个折点T(i),i＝1，2，3…，m，找到当前折点T(i)前后相邻的两个折点T(i-1)和T(i+1)，连接的搜索过程中从起始节点P(1)开始按顺序对P(N)节点集进行遍历，筛选出所有折点处的节点集T(M)，再依次遍历节点集T(m)中的两个节点，判断连接T(i-1)和T(i+1)的直线是否穿过障碍物，若没有，则判定折点T(i)为冗余折点，从而剔除节点集T(M)中所有的冗余折点，整个过程操作简便，并且剔除折点的准确率与效率较高。

附图说明

图1为本发明的流程图；

图2为八领域搜索示意图；

图3为改进前算法评价函数的评价效果图；

图4为改进后算法评价函数的评价效果图；

图5为传统的A*算法仿真效果图；

图6为经过第一阶段搜索后的仿真效果图；

图7为经过第二阶段搜索后的仿真效果图。

具体实施方式

下面结合具体实施例和附图对本发明进一步进行描述。

全路径规划是指机器人在具有障碍物的环境内，按照一种或多种性能指标，寻找一条从起始点到目标点的最优无碰撞路径，现有的路径规划算法-A*算法用启发式的搜索方式，大幅度降低了搜索节点的数量，从而极大地提高了搜索效率，但是该算法规划的路径易出现转折点，考虑到实际机器人的运动学模型，过多的转折点并不利于机器人的控制；A*算法结合了Dijkstra算法和广度优先(BFS)算法两者各自的优点，把Dijkstra靠近初始点的结点和BFS靠近目标点的结点的信息块结合起来，实际路径代价函数为：

f(n)＝s(n)+h(n)

其中：n表示机器人状态的当前节点，f(n)为机器人全局路径规划的代价函数，s(n)表示机器人从起始点到当前节点n所实际花费的代价，h(n)为启发函数，表示机器人从当前节点n到目标点的启发估计代价；

A*算法使用曼哈顿距离或者欧氏距离来定义启发函数，即为：

h_M(n)＝|n_x-g_x|+|n_y-g_y|

h_g(n)＝(n_x-g_x)²+(n_y-g_y)²

式中，n表示机器人当前节点，h_M(n)表示当前节点到目标节点的曼哈顿距离，h_g(n)表示当前节点到目标节点的欧氏距离，n_x表示n节点x轴方向坐标，n_y表示n节点y轴方向坐标；g表示机器人目标节点，g_x表示g节点x轴方向坐标，g_y表示g节点y轴方向坐标。

如图2所示是传统的A*算法采取的八邻域搜索方式，通过中心节点向附近8个邻域扩散，然后通过评价函数来确定路径方向，运动方向的角度也被限定为这八个方向。因此，规划出来的路径可能会有很多的转折点，而对于移动机器人来说，太多的转折点明显不符合运动学原理，不易于机器人的操控。

实施例1

如图1所示，为了克服现有技术的不足，本发明提出了一种全局路径规划的方法，包括以下步骤：

定位：根据机器人位姿和里程计的控制信息获取机器人的初始位姿即机器人通过已观测到的环境信息，结合自身已知的状态准确计算出自身的位姿信息；

创建先验地图：通过初始位姿及机器人自身携带的传感器的观测数据创建二维栅格地图，优选的，二维栅格地图中的每个基本单位有三种状态，分别为：占有、空闲和未知，对栅格的占有方式来知晓机器人的导航情况，相应的占有方式如下：栅格占有状态对应地图该点存在障碍物，机器人路径规划时不可触碰；空闲状态对应地图该点不存在障碍物，机器人路径规划时可靠近；未知状态表示机器人对地图相应位置的环境信息的不确定，机器人可前往探索。用栅格的状态来表征机器人对所处外部环境的认知，结果准确并且效率高，为下一步骤做准备；所述传感器为激光雷达，激光雷达可以获得极高的角度、距离和速度分辨率，并且激光直线传播、方向性好，具有较高的抗有源干扰能力，体积小、质量轻方便携带与搬运；

第一阶段搜索：在原有的A*算法代价函数中加入角度约束函数a(n)，通过改进的A*算法代价函数完成全局路径规划第一阶段搜索，解得对机器人运动角度约束的初步全局路径，n表示机器人状态的当前节点；所述的角度约束函数

a(n)＝k*(θ_n-θ_n-1)

式中：k为转角约束系数，n为机器人当前节点，n-1为机器人上一节点，θ_n表示机器人当前节点的朝向角，θ_n-1表示机器人上一节点的朝向角；通过添加角度约束项来解决机器人轨迹折点问题，当机器人上一时刻与当前时刻的朝向角相同时，a(n)＝0；当机器人的转角越大，则a(n)值越大，路径代价f(n)值越大，而机器人会优先选择较小代价值的路径，则要保持a(n)尽量小，那就要保证机器人节点间的偏角尽量小且路径轨迹的折点尽量少，这样设计代价函数f(n)就能尽量减少全局路径规划中的转折点；

因此改进后的A*算法代价函数为：

f(n)＝s(n)+h(n)+a(n)

式中，n表示机器人状态的当前节点，f(n)为机器人全局路径规划的代价函数，s(n)表示机器人从起始点到当前节点n所实际花费的代价，h(n)为启发函数，表示机器人从当前节点n到目标点的启发估计代价，a(n)为角度约束函数。

第二阶段搜索：提取第一阶段搜索的结果进行第二阶段搜索，剔除路径中的冗余折点，达到进一步减少轨迹中的折点和减少运动位移的目的，得到一条符合机器人运动学原理的最终全局路径。第二阶段的搜索包括以下几个步骤：

(2.1)提取第一阶段搜索生成的规划路径节点集P(N),P(N)集包含起始节点到目标节点以及它们之间的所有节点；

(2.2)从起始节点P(1)开始按顺序对P(N)节点集进行遍历，筛选出所有折点处的节点集T(M)，所述折点的判断标准为角度约束函数a(n)≠0的节点；

(2.3)依次遍历节点集T(m)中的每一个折点T(i),i＝1,2,3,…,m，找到当前折点T(i)前后相邻的两个折点T(i-1)和T(i+1)，连接两个节点，判断连接T(i-1)和T(i+1)的直线是否穿过障碍物，若没有，则判定折点T(i)为冗余折点；其中，T(M)集中的第一个折点T(1)以路径起始节点作为它的前一折点处理，最后一个折点T(m)以路径目标节点作为它的后一折点处理；

(2.4)删除折点节点集T(M)中所有冗余折点后，在节点集P(N)余下的所有节点顺序连线组成一条路径，即为一条符合机器人运动学原理的最终全局路径。

本发明通过重新定义评价函数，引入了新的角度约束函数a(n)，直接通过相邻节点的方向偏角进行约束，和机器人本身的特性没有关系，有效地抑制了折点的产生，改进的评价函数从的代价评价完全地考虑了方便机器人的运动控制；同时采用两阶段搜索方式，大幅度减少折点个数及转折角度，最终规划出一条符合机器人运动学原理的全局路径，能够生成在保证实时性的条件下平滑的且路程较短的全局路径，整个过程效率与准确率较高，并且也便于机器人的操控。如图3和图4所示，传统算法评价函数与改进算法评价函数的评价效果对比图，改进后的算法评价函数的评价效果相比改进前有了很大的改进，折点大幅度减少；图5为传统A*算法规划路径的仿真示意图，如图所示，折点多达5个，机器人的位移量也较大；图6为添加了角度的第一阶段搜索规划路径的仿真示意图，如图所示，折点明显减少，存在2个折点，机器人的位移量也相对减少；图7为经历了第二阶段搜索的仿真示意图，如图所示，折点进一步减少，存在1个折点，由此可见，规划的路径轨迹符合机器人运动学原理，同时方便机器人运动控制。

本发明所述实例仅仅是对本发明的优选实施方式进行描述，并非对本发明构思和范围进行限定，在不脱离本发明设计思想的前提下，本领域工程技术人员对本发明的技术方案作出的各种变形和改进，均应落入本发明的保护范围。

Claims (6)

1.一种全局规划路径的方法，其特征在于：包括以下步骤：

(1)在原有的A*算法代价函数中加入角度约束函数a(n)，通过改进的A*算法代价函数完成全局路径规划第一阶段搜索，解得对机器人运动角度约束的初步全局路径，n表示机器人状态的当前节点；

(2)提取第一阶段搜索的结果进行第二阶段搜索，剔除路径中的冗余折点，得到一条符合机器人运动学原理的最终全局路径。

2.根据权利要求1所述的一种全局规划路径的方法，其特征在于：所述角度约束函数a(n)＝k*(θ_n-θ_n-1)，其中k为转角约束系数，θ_n为当前时刻机器人的朝向角，θ_n-1为上一时刻机器人的朝向角。

3.根据权利要求2所述的一种全局规划路径的方法，其特征在于：步骤(1)中改进后的A*算法代价函数为：f(n)＝s(n)+h(n)+a(n)，其中，n表示机器人状态的当前节点，f(n)为机器人全局路径规划的代价函数，s(n)表示机器人从起始点到当前节点n所实际花费的代价，h(n)为启发函数，表示机器人从当前节点n到目标点的启发估计代价，a(n)为角度约束函数。

4.根据权利要求1或3所述的一种全局规划路径的方法，其特征在于：在进行步骤(1)之前还包括以下步骤：

(4)定位，根据机器人位姿和里程计的控制信息获取机器人的初始位姿；

(5)根据机器人初始位姿及机器人自身携带的传感器的观测数据创建二维栅格地图。

5.根据权利要求4所述的一种全局规划路径的方法，其特征在于：所述步骤(5)中的二维栅格地图中的每个基本单位有三种状态，分别为：占有、空闲和未知，对栅格的占有方式来知晓机器人的导航情况，为步骤(1)做准备。

6.根据权利要求1所述的一种全局规划路径的方法，其特征在于：所述步骤(2)包括以下几个步骤：

(2.1)提取第一阶段搜索生成的规划路径节点集P(N),P(N)集包含起始节点到目标节点以及它们之间的所有节点；

(2.2)从起始节点P(1)向后遍历节点，找到所有折点处的节点集T(M)；

(2.3)遍历折点节点集T(M)中的所有节点，判断当前折点T(n)的前后相邻节点T(n-1)和T(n+1)的连线是否穿过障碍物，若没有则判定当前节点T(n)为冗余折点；

(2.4)删除折点节点集T(M)中所有冗余折点后，在节点集P(N)余下的所有节点顺序连线，即为一条符合机器人运动学原理的最终全局路径。