基于改进A星融合DWA优化算法的巡检机器人路径规划方法技术

本发明专利技术具体是一种基于改进A星融合DWA优化算法的巡检机器人路径规划方法，解决了现有A星算法和DWA算法单独使用时存在平滑性较差、规划时间较长、拐点多、路径曲率不连续且无法实现全局最优的问题。该方法是采用如下步骤实现的，S1：初始化参数及栅格地图；S2：利用改进A星算法扩展子节点形成初步全局路径；S3：通过贪心优化方法得到优化后的全局路径；S4：提取局部关键节点；S5：基于运动学模型、速度约束及局部目标点进行速度采样和轨迹预测，并通过改进评价函数选择最优预测轨迹和速度空间；S6：提取并输出最优路径。本发明专利技术提供了一种兼具全局路径最优和实时避障能力的高效路径规划算法，提高了路径规划效率、避障灵敏性和安全性。避障灵敏性和安全性。避障灵敏性和安全性。

【技术实现步骤摘要】
基于改进A星融合DWA优化算法的巡检机器人路径规划方法


[0001]本专利技术涉及机器人的路径规划方法，具体为一种基于改进A星融合DWA优化算法的巡检机器人路径规划方法。

技术介绍

[0002]各类智能机器人的迅速发展对人类工作和生活有着日趋深刻的影响，其中温室技术变得越来越智能化和高效化，在植株高度密集、高温和高湿度甚至有毒的温室环境下，不利于长时间人工作业，再加上劳动力不足和成本增加等影响，衍生出温室机器人这类智能化装备，并且被采用得越来越多。利用温室移动巡检机器人完成温室环境的自主巡检符合精细化农业的需求。而路径规划是温室巡检机器人核心功能中极其重要的一部分，既是热点也是难点。
[0003]简而言之，路径规划就是在满足规划距离、规划时间、能量消耗和平滑性等优化条件的前提下，通过某种算法和评价方式规划出一条连接起止点的无碰安全路径。
[0004]A星算法是广度优先启发式路径搜索算法，虽然可以规划出全局最优路径，但是传统A星算法存在平滑性较差、规划时间较长、拐点多以及路径曲率不连续等局限。规划的路径上还常出现未知障碍物，而全局路径规划无法“认识”未知障碍物，从而导致安全事故，以致于仅适用于静态已知环境，无法实现未知障碍物的实时避障。
[0005]DWA算法是广泛使用的局部避障算法，但是原始DWA算法虽然能准确“认识”未知障碍物实现局部实时避障，却也存在易受困于局部最优，无法实现全局最优等不足。

技术实现思路

[0006]本专利技术为了解决现有A星算法和DWA算法单独使用时存在平滑性较差、规划时间较长、拐点多、路径曲率不连续且无法实现全局最优的问题，提供了一种基于改进A星融合DWA优化算法的巡检机器人路径规划方法。
[0007]本专利技术是采用如下技术方案实现的：
[0008]一种基于改进A星融合DWA优化算法的巡检机器人路径规划方法，该方法是采用如下步骤实现的：
[0009]S1：对参数及栅格地图进行初始化；
[0010]S2：设置路径规划的起点、终点，将起点加入Open list作为父节点，利用改进A星算法扩展子节点，形成初步全局路径；
[0011]S3：通过贪心优化方法对初步全局路径进行冗余节点剔除、关键节点保留、平滑处理，得到优化后的全局路径；
[0012]S4：提取优化后的全局路径的局部关键节点；
[0013]S5：以提取出的局部关键节点作为DWA优化算法的局部目标点，引导局部路径规划和实时避障，基于运动学模型、速度约束及局部目标点进行速度采样和轨迹预测，并通过DWA优化算法中的改进评价函数选择最优预测轨迹和速度空间，实现改进A星算法与DWA优
化算法的融合；
[0014]S6：到达终点后，回溯所有节点，提取并输出最优路径，改进A星融合DWA优化算法结束，完成基于该算法的路径规划和动态避障。
[0015]进一步地，改进A星融合DWA优化算法的参数包括巡检机器人运动学参数和改进DWA算法参数；其中，巡检机器人运动学参数包括最高速度(m/s)、最高旋转速度(rad/s)、最高加速度(m/s2)、最高旋转加速度(rad/s2)、速度分辨率(m/s)、转速分辨率(rad/s)、时间分辨率(s)、机器人初始方向角(rad)；改进DWA算法参数包括末端点与局部目标点方位角函数系数、末端点与局部目标点距离函数系数、末端点与已知静态障碍物距离函数系数、末端点与未知静态障碍物距离函数系数、末端点与未知动态障碍物距离函数系数、当前速度函数系数、模拟时间(s)、障碍物冲突半径(m)；每一次模拟时间内的算法运行周期都需要重新初始化算法所用栅格地图。
[0016]进一步地，步骤S2是采用如下步骤实现的：
[0017]S2.1：设定Open list，以存储没有访问过的节点信息；设定Close list，以存储已经访问过的节点信息；计算过程中Open list、Close list均采用最小堆融合二维数组的复合数据结构来存储、处理改进A星算法的二维搜索节点信息，将会有效提高搜索效率；
[0018]S2.2：首先设置路径规划的起点、终点，将路径规划的起点加入Open list，作为父节点，此时Close list为空；
[0019]S2.3：而后开始节点搜索，当访问过起点后将其从Open list中删除并加入到Close list中，并利用改进A星算法的评价函数选取搜索周期中的最优节点，作为下一父节点，继续搜索更新；每个搜索周期将搜索当前节点的八个相邻节点，利用改进A星算法的评价函数选取其中的最优节点，继续搜索寻找目标点；
[0020]其中，改进A星算法的评价函数为：
[0021][0022]式中，S
sc
为起点到当前节点的距离，S
st
为起点到终点的距离，H(n)为启发函数，为自适应权重系数，当环境地图的长为C、宽为K时，算法开始时当前点距离终点较远，启发函数估计值远小于真实值，搜索效率低，通过增大权重系数来增加启发函数值，加快搜索速度，提高搜索效率；当前点靠近终点时，为避免算法无法找到最佳路径，启发函数估计值应接近于真实值，需要降低权重系数来减少启发函数估计值，减缓搜索速度，增加搜索节点，虽然搜索效率会相对降低，但总体效率相对于原算法仍会有所提高，还能寻到最佳路径。
[0023]S2.4：在到达目标点之前，继续搜索起点的相邻节点，判断搜索到的新节点是否在Open list中：如果新节点不在Open list中，则将该节点加入Open list中，采用改进A星算法的评价函数计算该节点的代价值F，并将该节点设置为当前搜索点的父节点，加入Close list中；如果新节点在Open list中，采用改进A星算法的评价函数计算该节点的代价值F，并与前一节点的代价值F进行对比，选择代价值F最小的最优节点作为下一次搜索点的父节点，并将代价值F最小的节点加入Close list中，然后继续朝目标方向搜索节点，将搜索到的节点加入到Open list中，更新节点信息；
[0024]S2.5：循环执行步骤S2.3
‑
S2.4以扩展子节点，直至达到目标位置；当搜索到目标点时，将目标点添加到Close list中，循环结束；如果此时Open list为空，则表示没有路径节点；
[0025]S2.6：回溯提取Close list中的最优节点，形成改进A星算法的初步全局路径。
[0026]原始A星算法中，检测节点信息均被存在节点二维数组里，其中Open list存储着当前搜索领域中将要被检测的备选节点，并查找经评价函数评估后F(n)值最小的最优节点，Close list则存储着被查找出的最优节点。Open list需要频繁进行节点信息的读入、删除、查找和更新，Close list则仅仅只是没有其中删除操作，使得在处理备选节点数据时计算压力较大，比较耗时。
[0027]本专利技术选用最小堆融合二维数组结构来存放和处理A星算法搜索节点信息，其实质就是将A本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种基于改进A星融合DWA优化算法的巡检机器人路径规划方法，其特征在于：该方法是采用如下步骤实现的：S1：对参数及栅格地图进行初始化；S2：设置路径规划的起点、终点，将起点加入Open list作为父节点，利用改进A星算法扩展子节点，形成初步全局路径；S3：通过贪心优化方法对初步全局路径进行冗余节点剔除、关键节点保留、平滑处理，得到优化后的全局路径；S4：提取优化后的全局路径的局部关键节点；S5：以提取出的局部关键节点作为DWA优化算法的局部目标点，引导局部路径规划和实时避障，基于运动学模型、速度约束及局部目标点进行速度采样和轨迹预测，并通过DWA优化算法中的改进评价函数选择最优预测轨迹和速度空间，实现改进A星算法与DWA优化算法的融合；S6：到达终点后，回溯所有节点，提取并输出最优路径，改进A星融合DWA优化算法结束，完成基于该算法的路径规划和动态避障。2.根据权利要求1所述的基于改进A星融合DWA优化算法的巡检机器人路径规划方法，其特征在于：改进A星融合DWA优化算法的参数包括巡检机器人运动学参数和改进DWA算法参数；其中，巡检机器人运动学参数包括最高速度、最高旋转速度、最高加速度、最高旋转加速度、速度分辨率、转速分辨率、时间分辨率、机器人初始方向角；改进DWA算法参数包括末端点与局部目标点方位角函数系数、末端点与局部目标点距离函数系数、末端点与已知静态障碍物距离函数系数、末端点与未知静态障碍物距离函数系数、末端点与未知动态障碍物距离函数系数、当前速度函数系数、模拟时间、障碍物冲突半径；每一次模拟时间内的算法运行周期都需要重新初始化算法所用栅格地图。3.根据权利要求1所述的基于改进A星融合DWA优化算法的巡检机器人路径规划方法，其特征在于：步骤S2是采用如下步骤实现的：S2.1：设定Open list，以存储没有访问过的节点信息；设定Close list，以存储已经访问过的节点信息；计算过程中Open list、Close list均采用最小堆融合二维数组的复合数据结构来存储、处理改进A星算法的二维搜索节点信息；S2.2：首先设置路径规划的起点、终点，将路径规划的起点加入Open list，作为父节点，此时Close list为空；S2.3：而后开始节点搜索，当访问过起点后将其从Open list中删除并加入到Close list中，并利用改进A星算法的评价函数选取搜索周期中的最优节点，作为下一父节点，继续搜索更新；每个搜索周期将搜索当前节点的多个相邻节点，利用改进A星算法的评价函数选取其中的最优节点，继续搜索寻找目标点；其中，改进A星算法的评价函数为：式中，S
sc
为起点到当前节点的距离，S
st
为起点到终点的距离，H(n)为启发函数，为自适应权重系数，当环境地图的长为C、宽为K时，
S2.4：在到达目标点之前，继续搜索起点的相邻节点，判断搜索到的新节点是否在Open list中：如果新节点不在Open list中，则将该节点加入Open list中，采用改进A星算法的评价函数计算该节点的代价值F，并将该节点设置为当前搜索点的父节点，加入Close list中；如果新节点在Open list中，采用改进A星算法的评价函数计算该节点的代价值F，并与前一节点的代价值F进行对比，选择代价值F最小的最优节点作为下一次搜索点的父节点，并将代价值F最小的节点加入Close list中，然后继续朝目标方向搜索节点，将搜索到的节点加入到Open list中，更新节点信息；S2.5：循环执行步骤S2.3
‑
S2.4以扩展子节点，直至达到目标位置；当搜索到目标点时，将目标点添加到Close list中，循环结束；S2.6：回溯提取Close list中的最优节点，形成初步全局路径。4.根据权利要求1所述的基于改进A星融合DWA优化算法的巡检机器人路径规划方法，其特征在于：步骤S3中的是采用如下步骤实现的：S3.1：提取步骤S2得到的初步全局路径；S3.2：以起点为当前点，依次提取出当前点S0、子节点S1和孙子节点S2的坐标(x0,y0)、(x1,y1)和(x2,y2)，其取值范围为：式中，C为环境地图的长；K环境地图的宽；S3.3：计算当前点与子节点、子节点与孙子节点坐标之间的向量并判断两向量的夹角θ1：：S3.4：当夹角θ1为0时，说明当前点、子节点及孙子节点在同一条直线上，，则删除冗余节点S1；当夹角θ1不为0时，则计算出当前节点与孙子节点连线S0S2的直线方程，如式(5)所示：S3.5：判断所有障碍物节点是否有一或多个在当前节点与孙子节点连线S0S2的路径区域内，遍历所有障碍物节点，设正在筛选的障碍物节点坐标为(x
obc
,y
obc
)，取值范围为判断不等式(6)是否成立；[x
obc
≥min(x0，x2)∧x
obc
≤max(x0，x2)∧y
obc
≥min(y0，y2)∧y
obc
≤max(y0，y2)](6)S3.6：设置障碍物节点位置和当前节点与孙子节点连线S0S2之间的安全距离D，D的范围为根据直线方程y
line
＝kx
obc
+b计算出障碍物节点位置和当前节点与孙子节点连线S0S2的垂直距离d：d＝||(y
obc
‑
y
line
)cos(arctan(k))||
ꢀꢀꢀꢀꢀꢀꢀꢀ
(7)；S3.7：遍历所有障碍物节点，当夹角θ1不为0、不等式(6)成立、垂直距离d＜D时，可判断障碍物节点在当前节点与孙子节点连线S0S2的路径区域内，需要保留子节点，并以孙子节点
为当前节点重复执行步骤S3.2
‑
S3.6；当夹角θ1不为0、不等式(6)不成立时或当夹角θ1不为0、不等式(6)成立且垂直距离d≥D时，证明障碍物节点不在当前节点与孙子节点连线S0S2的路径区域内，删除子节点S1，以当前节点与孙子节点连线S0S2代替原来的折线路径，然后当前节点不变，修改子节点为S2，...

【专利技术属性】
技术研发人员：张东光，孙斌，刘治，牛蔺楷，吴亚丽，
申请(专利权)人：太原理工大学，
类型：发明
国别省市：