基于连通图和迭代搜索的导航路径规划方法技术

本发明专利技术公开基于连通图和迭代搜索的导航路径规划方法，该导航路径规划方法在机器人预先行走构建出的区域连通图内，按照合理的路径代价搜索出最接近实际起点的路径优化起点和最接近实际终点的路径优化终点，再在迭代搜索的方式的基础上利用启发路径代价和匹配的搜索优先级，完成路径优化起点和路径优化终点之间的导航路径规划。有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。路径在地图上的覆盖面增大。路径在地图上的覆盖面增大。

【技术实现步骤摘要】
基于连通图和迭代搜索的导航路径规划方法


[0001]本专利技术涉及路径规划的
，涉及一种基于连通图和迭代搜索的导航路径规划方法。

技术介绍

[0002]传统的导航路径规划算法(例如Dijkstra、A*、D*)，对栅格地图的精度和面积依赖比较大，随着栅格的读取精确程度增加或所要表征的地图面积增大，搜索成本呈现指数形态增长，且传统的图搜索算法搜索出的路径对机器人通过性考虑较少。

技术实现思路

[0003]为了解决上述技术问题，本专利技术技术方案在机器人预先行走构建出的区域连通图内，按照合理的路径代价搜索出最接近实际起点的路径优化起点和最接近实际终点的路径优化终点，再按照迭代搜索的方式完成路径优化起点和路径优化终点之间的导航路径规划。具体技术方案如下：基于连通图和迭代搜索的导航路径规划方法，包括：在预先构建的区域连通图内，利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点；其中，每个节点都位于所属的一条运动轨迹线段上，运动轨迹线段是机器人预先移动产生的；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；再将当前搜索出的优化导航路径的两端分别连接上所述导航起点和所述导航终点，完成导航路径的规划；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点；其中，在所述区域连通图内的任意节点所耗费的启发路径代价和等于从所述路径优化起点到该节点的实际移动代价与从该节点到所述路径优化终点的预测路径代价的和值。与现有技术相比，本技术方案可以有效减少扩展节点的搜索量，提高区域连通图内的路径节点的搜索速度，提高机器人在区域连通图内的通过性，规划出的路径在地图上的覆盖面增大。也在连通图这一替代性的导航地图上发挥启发式搜索算法的路径规划优势。
[0004]进一步地，所述利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点的方法包括：若存在待判断节点与所述导航起点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航起点的节点；然后对每个直通所述导航起点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；
再将代价值最小的直通所述导航起点的节点设置为所述路径优化起点；若存在待判断节点与所述导航终点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航终点的节点；然后对每个直通所述导航终点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航终点的节点设置为所述路径优化终点；其中，所述待判断节点属于预先构建的区域连通图内的节点；每个所述待判断节点都位于各自所属的一条运动轨迹线段上。
[0005]该技术方案在区域连通图内，利用每个节点在其所属的运动轨迹线段上的居中程度值（表示向运动轨迹线段的中点靠拢的程度、或表示为偏离运动轨迹线段的端点的程度），搜索出直通机器人的导航点且产生代价值最小的节点，作为与导航起点的距离相匹配的路径优化起点、以及与导航终点的距离相匹配的路径优化终点，开启在区域连通图内规划出以路径优化起点为起点、以路径优化终点为终点的导航路径。
[0006]进一步地，还包括：计算数值2与所述待判断节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述待判断节点与所述导航起点的欧式距离，作为导航起点匹配距离；再将偏离程度差值、导航起点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航起点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；计算所述待判断节点与所述导航终点的欧式距离，作为导航终点匹配距离；再将偏离程度差值、导航终点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航终点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。
[0007]本技术方案根据待规划的路径偏离障碍物的远近程度、待判断节点与所述导航起点或所述导航终点的欧式距离、待判断节点在所属的运动轨迹线段上的居中程度值的乘积运算的结果，获取待判断节点对应换算出的代价值，但不同于从所述导航终点或所述导航起点到所述待判断节点的实际移动代价，是用于评估待判断节点与所述导航起点或所述导航终点的接近程度及通行效果。
[0008]进一步地，所述从所述路径优化起点到当前扩展节点的实际移动代价等于从所述路径优化起点到当前扩展节点的父节点的实际移动代价与从当前扩展节点的父节点到当前扩展节点的实际移动代价的和值。表示所述从所述路径优化起点到当前扩展节点的实际移动代价是路径代价的累加结果，控制当前扩展节点产生的实际移动代价等于上一次扩展节点累计产生的实际移动代价与当前扩展节点到其父节点产生的实际移动代价的和值。
[0009]进一步地，从当前扩展节点的父节点到当前扩展节点的实际移动代价的计算方法包括：计算数值2与当前扩展节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述当前扩展节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是所述从当前扩展节点的父节点到当前扩展节点的实际移动代价；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。使得从当前扩展节点的父节点到当前扩展节点的实际移动代价能够表示待规划的路径远离障碍物的程度，提高当前扩展节点的父节点到当前扩展节点之间的可通行路径的搜索成功率。
[0010]进一步地，从所述路径优化起点到当前扩展节点的父节点的实际移动代价的计算
方法包括：步骤a1、将当前扩展节点的父节点设置为回溯迭代节点，并设置移动代价迭代和为0；然后进入步骤a2；步骤a2、判断回溯迭代节点是否为所述路径优化起点，是则进入步骤a3，否则进入步骤a4；步骤a3、将从所述路径优化起点到当前扩展节点的父节点的实际移动代价设置为等于所述移动代价迭代和；步骤a4、计算数值2与回溯迭代节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述回溯迭代节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价，再将从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价加本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.基于连通图和迭代搜索的导航路径规划方法，其特征在于，包括：在预先构建的区域连通图内，利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点；其中，每个节点都位于所属的一条运动轨迹线段上，运动轨迹线段是机器人预先移动产生的；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导航路径；再将当前搜索出的优化导航路径的两端分别连接上所述导航起点和所述导航终点，完成导航路径的规划；在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点；其中，在所述区域连通图内的任意节点所耗费的启发路径代价和等于从所述路径优化起点到该节点的实际移动代价与从该节点到所述路径优化终点的预测路径代价的和值。2.根据权利要求1所述导航路径规划方法，其特征在于，所述利用节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值，搜索出与预先设置的导航起点相匹配的路径优化起点以及与预先设置的导航终点相匹配的路径优化终点的方法包括：若存在待判断节点与所述导航起点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航起点的节点；然后对每个直通所述导航起点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航起点的节点设置为所述路径优化起点；若存在待判断节点与所述导航终点的连线不经过障碍物栅格点，则确定该待判断节点是直通所述导航终点的节点；然后对每个直通所述导航终点的节点在其所属的运动轨迹线段上的居中程度值换算出的对应的代价值进行数值排序；再将代价值最小的直通所述导航终点的节点设置为所述路径优化终点；其中，所述待判断节点属于预先构建的区域连通图内的节点；每个所述待判断节点都位于各自所属的一条运动轨迹线段上。3.根据权利要求2所述导航路径规划方法，其特征在于，还包括：计算数值2与所述待判断节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述待判断节点与所述导航起点的欧式距离，作为导航起点匹配距离；再将偏离程度差值、导航起点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航起点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；计算所述待判断节点与所述导航终点的欧式距离，作为导航终点匹配距离；再将偏离程度差值、导航终点匹配距离与居中程度影响系数相乘，得到的乘积是所述直通所述导航终点的节点在所属的运动轨迹线段上的居中程度值换算出的对应的代价值；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。
4.根据权利要求1所述导航路径规划方法，其特征在于，所述从所述路径优化起点到当前扩展节点的实际移动代价等于从所述路径优化起点到当前扩展节点的父节点的实际移动代价与从当前扩展节点的父节点到当前扩展节点的实际移动代价的和值。5.根据权利要求4所述导航路径规划方法，其特征在于，从当前扩展节点的父节点到当前扩展节点的实际移动代价的计算方法包括：计算数值2与当前扩展节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述当前扩展节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是所述从当前扩展节点的父节点到当前扩展节点的实际移动代价；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于调节待规划的路径偏离障碍物的远近程度。6.根据权利要求4所述导航路径规划方法，其特征在于，从所述路径优化起点到当前扩展节点的父节点的实际移动代价的计算方法包括：步骤a1、将当前扩展节点的父节点设置为回溯迭代节点，并设置移动代价迭代和为0；然后进入步骤a2；步骤a2、判断回溯迭代节点是否为所述路径优化起点，是则进入步骤a3，否则进入步骤a4；步骤a3、将从所述路径优化起点到当前扩展节点的父节点的实际移动代价设置为等于所述移动代价迭代和；步骤a4、计算数值2与回溯迭代节点在所属的运动轨迹线段上的居中程度值的差值，作为偏离程度差值；计算所述回溯迭代节点与其父节点的欧式距离，作为邻域节点匹配距离；再将偏离程度差值、邻域节点匹配距离与居中程度影响系数相乘，得到的乘积是从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价，再将从所述回溯迭代节点的父节点到所述回溯迭代节点的实际移动代价加上所述移动代价迭代和，然后将相加得到的和值更新为所述移动代价迭代和；然后进入步骤a5；步骤a5、将所述回溯迭代节点的父节点设置为下一次计算父子节点之间的移动代价的回溯迭代节点，再返回步骤a2；其中，居中程度值处于0至1之间；居中程度影响系数是预先设置的，用于表示待规划的路径偏离障碍物的远近程度；其中，更新前后的所述回溯迭代节点都属于所述区域连通图内的节点，所述回溯迭代节点及其父节点都属于所述区域连通图内的相邻接的两个节点。7.根据权利要求1至6任一项所述导航路径规划方法，其特征在于，前述相应的节点在所属的运动轨迹线段上的居中程度值是：该节点到运动轨迹线段上相距最近的一个端点的直线距离与该运动轨迹线段的长度的一半的比值。8.根据权利要求1至6任一项所述导航路径规划方法，其特征在于，所述从当前扩展节点到所述路径优化终点的预测路径代价是使用所述当前扩展节点与所述路径优化终点之间的欧式距离、曼哈顿距离、或对角距离来表示。9.根据权利要求1所述导航路径规划方法，其特征在于，所述根据所述区域连通图内的节点对应的父节点信息，逐层地搜索出从所述路径优化起点到所述路径优化终点的优化导
航路径的方法包括：在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点是所述路径优化终点时，则从所述路径优化终点开始，按照所述路径优化终点在所述区域连通图的对应邻域内所记录的父节点位置信息，依次连接父节点以及该父节点的父节点，直到在所述区域连通图内连接到所述路径优化起点。10.根据权利要求9所述导航路径规划方法，其特征在于，所述在所述区域连通图内所有的待扩展节点中，若启发路径代价和最小的待扩展节点不是所述路径优化终点时，则将所述启发路径代价和最小的待扩展节点设置为当前扩展节点，再将当前扩展节点的邻域节点中的未扩展节点设置为新的待扩展节点，直到当前搜索出的启发路径代价和最小的待扩展节点是所述路径优化终点的方法包括：步骤b1、在所述区域连通图内所...

【专利技术属性】
技术研发人员：孙永强，李永勇，肖刚军，
申请(专利权)人：珠海一微半导体股份有限公司，
类型：发明
国别省市：