买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：山西精宏睿电力科技有限公司;太原理工大学

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

主权项：1.一种基于改进A星融合DWA优化算法的巡检机器人路径规划方法，其特征在于：该方法是采用如下步骤实现的：S1：对参数及栅格地图进行初始化；S2：设置路径规划的起点、终点，将起点加入Openlist作为父节点，利用改进A星算法扩展子节点，形成初步全局路径；S3：通过贪心优化方法对初步全局路径进行冗余节点剔除、关键节点保留、平滑处理，得到优化后的全局路径；S4：提取优化后的全局路径的局部关键节点；S5：以提取出的局部关键节点作为DWA优化算法的局部目标点，引导局部路径规划和实时避障，基于运动学模型、速度约束及局部目标点进行速度采样和轨迹预测，并通过DWA优化算法中的改进评价函数选择最优预测轨迹和速度空间，实现改进A星算法与DWA优化算法的融合；S6：到达终点后，回溯所有节点，提取并输出最优路径，改进A星融合DWA优化算法结束，完成基于该算法的路径规划和动态避障；步骤S2是采用如下步骤实现的：S2.1：设定Openlist，以存储没有访问过的节点信息；设定Closelist，以存储已经访问过的节点信息；计算过程中Openlist、Closelist均采用最小堆融合二维数组的复合数据结构来存储、处理改进A星算法的二维搜索节点信息；S2.2：首先设置路径规划的起点、终点，将路径规划的起点加入Openlist，作为父节点，此时Closelist为空；S2.3：而后开始节点搜索，当访问过起点后将其从Openlist中删除并加入到Closelist中，并利用改进A星算法的评价函数选取搜索周期中的最优节点，作为下一父节点，继续搜索更新；每个搜索周期将搜索当前节点的多个相邻节点，利用改进A星算法的评价函数选取其中的最优节点，继续搜索寻找目标点；其中，改进A星算法的评价函数为： 式中，Gn为搜索起点到当前点的真实代价，Ssc为起点到当前节点的距离，Sst为起点到终点的距离，Hn为启发函数，为自适应权重系数，当环境地图的长为C、宽为K时，S2.4：在到达目标点之前，继续搜索起点的相邻节点，判断搜索到的新节点是否在Openlist中：如果新节点不在Openlist中，则将该节点加入Openlist中，采用改进A星算法的评价函数计算该节点的代价值F，并将该节点设置为当前搜索点的父节点，加入Closelist中；如果新节点在Openlist中，采用改进A星算法的评价函数计算该节点的代价值F，并与前一节点的代价值F进行对比，选择代价值F最小的最优节点作为下一次搜索点的父节点，并将代价值F最小的节点加入Closelist中，然后继续朝目标方向搜索节点，将搜索到的节点加入到Openlist中，更新节点信息；S2.5：循环执行步骤S2.3-S2.4以扩展子节点，直至达到目标位置；当搜索到目标点时，将目标点添加到Closelist中，循环结束；S2.6：回溯提取Closelist中的最优节点，形成初步全局路径；步骤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的路径区域内，遍历所有障碍物节点，设正在筛选的障碍物节点坐标为xobc,yobc，取值范围为0xobc、判断不等式6是否成立；[xobc≥minx0,x2∧xobc≤maxx0,x2∧yobc≥miny0,y2∧yobc≤maxy0,y2]6S3.6：设置障碍物节点位置和当前节点与孙子节点连线S0S2之间的安全距离D，D的范围为根据直线方程yline＝kxobc+b计算出障碍物节点位置和当前节点与孙子节点连线S0S2的垂直距离d：d＝∥yobc-ylinecosarctank∥7；S3.7：遍历所有障碍物节点，当夹角θ1不为0、不等式6成立、垂直距离dD时，可判断障碍物节点在当前节点与孙子节点连线S0S2的路径区域内，需要保留子节点，并以孙子节点为当前节点重复执行步骤S3.2-S3.6；当夹角θ1不为0、不等式6不成立时或当夹角θ1不为0、不等式6成立且垂直距离d≥D时，证明障碍物节点不在当前节点与孙子节点连线S0S2的路径区域内，删除子节点S1，以当前节点与孙子节点连线S0S2代替原来的折线路径，然后当前节点不变，修改子节点为S2，重复步骤S3.2-S3.6，得到第一阶段优化路径；S3.8：提取第一阶段优化路径并重新排列路径节点，以第一阶段优化路径的起点S0x0,y0为当前点，得到当前点与优化路径子节点G1x4,y4的连线段S0G1上所有节点集合{g1i|i＝0,1…n}；按照步骤S3.5-S3.7的方法判断障碍物节点是否在优化路径子节点G1x4,y4的连线段S0G1的路径区域内；S3.9：当S0G1上所有节点g1i与起点S0的连线区域内均无障碍物节点，则删除S0G1上所有节点g1i，保留关键节点G1；保留关键节点G1后，再以G1x4,y4为当前节点，G2x5,y5为子节点，线段G1G2上的所有节点集合为{g2i|i＝0,1…n}，重复步骤S3.8-S3.9，直至子节点为目标节点，完成二次优化，得到优化后的全局路径；当S0G1上所有节点g1i与起点S0的连线区域内存在障碍物节点则路径优化失败，则重新优化第一阶段路径；步骤S5是采用如下步骤实现的：S5.1：在栅格地图上设置未知静态、未知动态障碍物；S5.2：基于巡检机器人运动学模型、速度约束，巡检机器人在速度空间集合Vav,ω中进行速度采样，模拟出一定时间间隔内的无穷多条可行运动轨迹；S5.3：以步骤S4中保留的局部关键节点为DWA优化算法的局部目标点，提供局部方向指引，利用DWA优化算法的评价函数评估轨迹，从步骤S5.2中生成的无穷多条可行运动轨迹中选择最优轨迹；其中，DWA优化算法的评价函数如式8所示：Pv,w＝σα1·AGheadingv,w+α2·Gdistv,w+β1·S_Kdistv,w+β2·S_Udistv,w+βS·D_Udistv,w+γ·velocityv,w8式中，Gheadingv,w表示采样轨迹末端方向与局部目标方向的角度差，Gdistv,w表示采样轨迹末端点与局部目标点的距离，共同作用使预测轨迹靠近全局路径；为了进一步提高算法的安全性和避障灵敏性，将原始DWA算法中采样轨迹末端点与最近障碍物之间的最小距离细分为三部分：S_Kdistv,w、S_Udistv,w和D_Udistv,w，分别表示采样轨迹末端点与已知静态障碍物、未知静态障碍物和未知动态障碍物的最小距离；velocityv,w表示当前速度；α1、α2、β1、β2、βS、γ分别为评价函数各子函数对应的权重系数；σ代表归一化过程；根据已知静态障碍物、未知静态障碍物和未知动态障碍物对于巡检机器人的危险程度，采样轨迹末端点与三种障碍物之间的最小距离对应各项的权重系数满足β1β2βS；提出自适应权重系数βs，设置巡检机器人与障碍物的冲突半径Ds，Ds大于机器人转弯半径R，那么规定冲突半径的取值为RDs2R，当机器人与动态障碍物的距离小于或等于机器人转弯半径R时，极易发生碰撞，为了保持安全性，βs必须为无穷大，那么βs取值表示为： S5.4：将最优轨迹对应的速度空间保留下来，并进行速度平滑；S5.5：下放速度指令至巡检机器人底盘控制器，巡检机器人沿着步骤S5.3中得到的最优轨迹移动；S5.6：判断巡检机器人是否移动到终点，如果移动到终点则结束，如果不是终点，则重复执行步骤S5.2至S5.4。

全文数据：

权利要求：

百度查询： 山西精宏睿电力科技有限公司 太原理工大学 基于改进A星融合DWA优化算法的巡检机器人路径规划方法