CN114779772B - 一种融合全局及局部算法的路径规划方法及装置 - Google Patents

一种融合全局及局部算法的路径规划方法及装置 Download PDF

Info

Publication number
CN114779772B
CN114779772B CN202210384849.8A CN202210384849A CN114779772B CN 114779772 B CN114779772 B CN 114779772B CN 202210384849 A CN202210384849 A CN 202210384849A CN 114779772 B CN114779772 B CN 114779772B
Authority
CN
China
Prior art keywords
node
path
points
nodes
obstacle
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202210384849.8A
Other languages
English (en)
Other versions
CN114779772A (zh
Inventor
郭杰龙
俞辉
陈霞
魏宪
张剑锋
邵东恒
李�杰
兰海
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Quanzhou Institute of Equipment Manufacturing
Original Assignee
Quanzhou Institute of Equipment Manufacturing
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Quanzhou Institute of Equipment Manufacturing filed Critical Quanzhou Institute of Equipment Manufacturing
Priority to CN202210384849.8A priority Critical patent/CN114779772B/zh
Publication of CN114779772A publication Critical patent/CN114779772A/zh
Application granted granted Critical
Publication of CN114779772B publication Critical patent/CN114779772B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Numerical Control (AREA)
  • Manipulator (AREA)

Abstract

本发明提供一种融合全局及局部算法的路径规划方法及装置，方法包括如下步骤：步骤S1：设置障碍物位置、起始节点S和目标节点G；步骤S2：进行全局路径规划，得到路径点集合closelist；步骤S3：判断路径点集合closelist中相邻的三点是否共线，最终形成关键点集合keylist；步骤S4：判断关键点集合keylist中，Ki与Ki+2的连线是否与障碍物相交，最终形成优化路径集合lastlist；步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，进行轨迹预测得到多个运动轨迹，利用评估函数选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度。本发明的路径平滑且无冗余节点，提升效率，且在完成动态避障的同时还有方向性地快速逼近终点，兼具速度与安全性。

Description

一种融合全局及局部算法的路径规划方法及装置
技术领域
本发明涉及一种融合全局及局部算法的路径规划方法及装置。
背景技术
对于机器人运动轨迹的规划，在全局路径规划技术方面已经取得了不错成果，但是全局规划仅适用于活动区域信息己知的情况，当环境比较复杂时，如存在各种不规则障碍物时，全局路径规划可能无法适用，而且现有技术中的全局路径规划还存在冗余节点过多、拐点过多、路径过长的缺陷。而局部路径规划虽然可以在环境未知的情况下使用，但其在复杂环境中计算量过大，消耗时间过长。现有技术中也有将全局路径规划和局部路径规划融合的方案，但是仍然存在拐点过多、路径不平滑、冗余节点过多、路径过长、最终路径与全局路径相差较大、速度与安全性无法同时满足的缺陷。
发明内容
本发明提出一种融合全局及局部算法的路径规划方法及装置，路径平滑且无冗余节点，提升效率，且在完成动态避障的同时还有方向性地快速逼近终点，兼具速度与安全性。
本发明通过以下技术方案实现：
一种融合全局及局部算法的路径规划方法，包括如下步骤：
步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；
步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,…Cn]，其中，C1＝S，Cn＝G；
步骤S3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K1,K2,…,Km]，其中，K1＝S，Km＝G；
步骤S4：判断关键点集合keylist中，Ki与Ki+2的连线是否与障碍物相交，若是，则提取Ki+1作为优化路径点，否则，将Ki+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,…,Lz]，其中，L1＝S，Lz＝G，1≤i≤m-2；
步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和/>分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。
进一步的，所述步骤S5中，将优化路径集合lastlist中的路径点作为局部路径规划的子目标，具体为：
依次将优化路径集合lastlist中的路径点作为子目标Gmin，但当机器人与子目标Gmin＝Li的距离小于第一阈值时，设置Gmin＝Li+1，当子目标为Gmin＝Li且Li+j出现在最优的动作轨迹中时，设置Gmin＝Li+j，其中，第一阈值与机器人底盘半径相关，j＞0。
进一步的，所述步骤S5中，其中，γmax为最短时间通过障碍物密集区域的对应值，γmin为通过障碍物密集区域最安全的对应值，D为机器人到最近障碍物的距离，/>vmax为预设的最大线速度，va为预设的线加速度。
进一步的，所述步骤S2具体包括如下步骤：
步骤S21：创建空集openlist和closelist，将起始节点S存储到closelist中；
步骤S22：寻找起始节点S周围16个方向上可以到达的24个节点，将它们放入openlist中，并设置它们的父节点为S，其中，以节点S为原点，24个节点的坐标为：[[1，0]，[0，1]，[-1，0]，[0，-1]，[-1，-1]，[-1，1]，[1，-1]，[1，1]，[2，0]，[0，2]，[-2，0]，[0，-2]，[-2，-2]，[-2，2]，[2，-2]，[2，2]，[1，3]，[3，1]，[3，-1]，[1，-3]，[-1，3]，[-3，1]，[-3，-1]，[-1，-3]]；
步骤S23：将节点S从从openlist放入closelist中，再分别计算步骤S22中各节点的F值，并将F值最小的节点A从openlist放入closelist中，再将节点A周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为A，并以节点A为当前节点，分别计算以这些节点为目标节点的F值，其中，F＝G+H，F值表示从起始节点到目标节点的代价消耗，G值表示从起始节点到当前节点的实际消耗，H值表示从当前节点到目标节点的估计代价消耗；
步骤S24：按照步骤S23继续从openlist中找出F值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点G，逆序遍历父节点即可得到路径点集合closelist＝[C1,C2,…Cn]。
进一步的，所述步骤S23中，若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变。
进一步的，所述步骤S3中，设Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)为路径点集合closelist中相邻的三点，若(xi+1-xi)(yi+2-yi+1)＝(yi+1-yi)(xi+2-xi+1)成立，判定Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)共线，否则，判定Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)不共线，其中，1≤i≤n-2。
进一步的，所述步骤S5具体包括如下步骤：
步骤S51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径以及DWA算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间；
步骤S52：按照采样周期在由步骤S51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用DWA算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹；
步骤S53：利用评估函数G(v,w)给各运动轨迹进行打分，选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度；
步骤S54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤S52，重新进行采样。
根据权利要求1或2或3所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5中，在利用评估函数G(v,w)进行打分前，分别对heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)进行归一化。
进一步的，所述H值即为当前节点到目标节点的欧式距离。
本发明还通过以下技术方案实现：
一种融合全局及局部算法的路径规划装置，包括：
初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；
全局规划模块：用于通过基于5×5搜索邻域的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,…Cn]，其中，C1＝S，Cn＝G；
关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K1,K2,…,Km]，其中，K1＝S，Km＝G；
冗余点删除模块：用于判断关键点集合keylist中，Ki与Ki+2的连线是否与障碍物相交，若是，则提取Ki+1作为优化路径点，否则，将Ki+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,…,Lz]，其中，L1＝S，Lz＝G，1≤i≤m-2；
轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和/>分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。
本发明具有如下有益效果：
1、本发明先基于16邻域搜索的A*算法进行全局路径规划以得到路径点集合closelist，然后根据closelist中相邻三点是否共线进行关键点提取以得到关键点集合keylist，再将关键点集合keylist中的冗余点删除，得到优化后的全局路径集合lastlist，最后将优化后的全局路径集合lastlist作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，并利用评估函数选择最优的运动轨迹控制机器人运动，如此将全局路径规划和局部路径规划融合，得到在完成动态避障的同时还有方向性地快速逼近终点的最优路径，兼具速度与安全性，当出现未知障碍物时，机器人在行驶过程中不会与故障物发生碰撞，且始终沿着全局最优路径的目标方向行驶，提升了路径规划的效率和实时性；在全局路径规划部分，增大搜索范围，避免出现拐点过多的现象，且提取关键点，删除冗余点，精简路径且使路径更为平滑；评价函数中的加权系数γd与机器人到最近障碍物的距离相关，保证了路径与动态障碍物之间的距离，且使机器人能够根据环境的变化自动调整该加权系数，从而得到机器人的最佳运动速度，以获得更安全合理的路径。
附图说明
下面结合附图对本发明做进一步详细说明。
图1为本发明流程图。
图2-1为现有技术中8邻域搜索方式示意图。
图2-2为本发明中16邻域搜索方式示意图。
图3为本发明中全局路径规划的路径仿真图。
图4为本发明中全局与局部融合路径规划的路径仿真图。
其中，1、已知障碍物；2、未知障碍物；3、动态障碍物；31、动态障碍物运动轨迹；4、优化路径集合lastlist中的路径点。
具体实施方式
如图1所示，融合全局及局部算法的路径规划方法，包括如下步骤：
步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；
步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,…Cn]，其中，C1＝S，Cn＝G；
具体包括如下步骤：
步骤S21：创建空集openlist和closelist，openlist是存放等待检查的节点的列表，closelist是存放不再需要检查的节点的列表，将起始节点S存储到closelist中；
步骤S22：寻找起始节点S周围16个方向上可以到达的24个节点，将它们放入openlist中，并设置它们的父节点为S，其中，16邻域搜索方式如图2-2所示，以节点S为原点，24个节点的坐标为：[[1，0]，[0，1]，[-1，0]，[0，-1]，[-1，-1]，[-1，1]，[1，-1]，[1，1]，[2，0]，[0，2]，[-2，0]，[0，-2]，[-2，-2]，[-2，2]，[2，-2]，[2，2]，[1，3]，[3，1]，[3，-1]，[1，-3]，[-1，3]，[-3，1]，[-3，-1]，[-1，-3]]；
与如图2-1所示的8邻域搜索方式不同，16邻域搜索方式能够去除同方向的多余冗余子节点，提高搜索效率；
步骤S23：将节点S从从openlist放入closelist中，再分别计算步骤S22中各节点的F值，并将F值最小的节点A从openlist放入closelist中，再将节点A周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为A，并以节点A为当前节点，分别计算以这些节点为目标节点的F值，其中，F＝G+H，F值表示从起始节点到目标节点的代价消耗，G值表示从起始节点到当前节点的实际消耗，H值表示从当前节点到目标节点的估计代价消耗，选取当前节点与目标节点之间的欧式距离作为H值；
若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变；
步骤S24：按照步骤S23继续从openlist中找出F值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点G，逆序遍历父节点即可得到路径点集合closelist＝[C1,C2,…Cn]，若openlist中没有了数据，则说明没有合适路径，将重新设置起始点和目标点；
步骤S3：创建空集keylist作为关键点集合，将C1添加至keylist中，判断路径点集合closelist中相邻的三点Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)是否共线，若是不共线，即(xi+1-xi)(yi+2-yi+1)＝(yi+1-yi)(xi+2-xi+1)成立，则将相邻三点的中间点Ci+1(xi+1,yi+1)作为关键点进行提取，若是共线，即(xi+1-xi)(yi+2-yi+1)＝(yi+1-yi)(xi+2-xi+1)不成立，则将相邻三点的中间点Ci+1(xi+1,yi+1)作为冗余点删除，再令i＝i+1，再次进行共线判断，最终形成关键点集合keylist＝[K1,K2,…,Km]，其中，K1＝S，Km＝G，1≤i≤n-2；
步骤S4：创建空集lastlist作为优化路径集合，将K1添加至lastlist中，判断关键点集合keylist中，Ki与Ki+2的连线是否与障碍物相交，若是，则提取Ki+1作为优化路径点添加至lastlist内，否则，将Ki+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,…,Lz]，规划出的路径如图3所示，其中，L1＝S，Lz＝G，1≤i≤m-2；
步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，如图4所示即为规划出的路径，该采样速度包括线速度和角速度；α、β、γd和/>分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度；
具体包括如下步骤：
步骤S51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径R以及DWA算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间以及评估函数的加权系数α、β和
步骤S52：按照采样周期在由步骤S51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用DWA算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹，这个过程为现有技术；
步骤S53：分别通过各运动轨迹，计算机器人与最近障碍物的距离(xt,yt)为机器人当前坐标，(x0,y0)为障碍物坐标，计算动态范围阈值/>vmax为预设的最大线速度，va为预设的最大线加速度，并根据公式计算加权系数γd，γmax为最短时间通过障碍物密集区域的对应值(“最短时间”的具体时长由实际情况中障碍物密集程度而定)，γmin为通过障碍物密集区域最安全的对应值；
则评估函数的各个加权系数均已确定，可利用评估函数进行评价打分，以选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度，其中，
其中，子函数用于衡量机器人对子目标的方向性，θg为机器人指向目标节点与水平线的夹角，θt为机器人行进方向与水平线的夹角，两者差值越小，说明与目标节点的方位差越小；
子函数表示运动轨迹中距离障碍物的最小距离,D表示计算机器人与最近障碍物的距离，为保证机器人移动安全性，在机器人底盘半径R的基础上增加0.2R的安全距离，当机器人到障碍物的距离大于机器人底盘半径R时一般不会发生碰撞，此时子函数值为0，即不发生碰撞时不考虑障碍物距离评价；当机器人到障碍物的距离小于或等于R时，机器人发生碰撞的可能性较大，此时值随着距离的减小而增大，即距离越近发生碰撞的影响越大；
子函数用于评价机器人当前速度v大小，速度v越大，则评价得分越高；
子函数用于衡量向前预估时间内所预测的点(xm,ym)与子目标(xa,ya)的距离；
但在评价之前，要对上述4个子函数进行归一化处理：
其中，i表示所预测的第i条预测轨迹，n表示共有n条预测轨迹；
步骤S54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤S52，重新进行采样。
如图3所示为全局路径规划的路径仿真图，图4为全局与局部融合的路径仿真图，当仅有已知障碍物1，而未出现未知障碍物2时，机器人根据优化路径集合lastlist中的路径点4所形成的路径行驶，但可看出，如此无法避开未知障碍物2，且会和动态障碍物的运动轨迹31相交，而图4中，当出现未知障碍物2和动态障碍物3时，机器人能够自动避开障碍物，不会与障碍物发生碰撞，且还是会沿着优化路径集合lastlist中的路径点所形成的路径方向行驶。
对应的，一种融合全局及局部算法的路径规划装置，包括：
初始化模块：用于初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；
全局规划模块：用于通过基于5×5搜索邻域的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,…Cn]，其中，C1＝S，Cn＝G；
关键点提取模块：用于判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K1,K2,…,Km]，其中，K1＝S，Km＝G；
冗余点删除模块：用于判断关键点集合keylist中，Ki与Ki+2的连线是否与障碍物相交，若是，则提取Ki+1作为优化路径点，否则，将Ki+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,…,Lz]，其中，L1＝S，Lz＝G，1≤i≤m-2；
轨迹确定模块：用于将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和/>分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度。
以上所述，仅为本发明的较佳实施例而已，故不能以此限定本发明实施的范围，即依本发明申请专利范围及说明书内容所作的等效变化与修饰，皆应仍属本发明专利涵盖的范围内。

Claims (3)

1.一种融合全局及局部算法的路径规划方法，其特征在于：包括如下步骤：
步骤S1：初始化栅格地图，设置障碍物位置、起始节点S和目标节点G；
步骤S2：通过基于16邻域搜索的A*算法进行全局路径规划，得到路径点集合closelist＝[C1,C2,…Cn]，其中，C1＝S，Cn＝G；
步骤S3：判断路径点集合closelist中相邻的三点是否共线，若是不共线，则将相邻三点的中间点作为关键点进行提取，若是共线，则将相邻三点的中间点作为冗余点删除，最终形成关键点集合keylist＝[K1,K2,…,Km]，其中，K1＝S，Km＝G；
步骤S4：判断关键点集合keylist中，Ki与Ki+2的连线是否与障碍物相交，若是，则提取Ki+1作为优化路径点，否则，将Ki+1从关键点集合keylist中删除，最终形成优化路径集合lastlist＝[L1,L2,…,Lz]，其中，L1＝S，Lz＝G，1≤i≤m-2；
步骤S5：将优化路径集合lastlist中的路径点作为局部路径规划的子目标，并在预设的速度空间进行采样，针对每个速度采样点均利用DWA算法进行轨迹预测，得到多个运动轨迹，并利用评估函数选择最优的运动轨迹控制机器人运动，heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)分别用于衡量对子目标的方向性、表示运动轨迹中距离障碍物的最小距离、评价当前速度大小和用于衡量与子目标的距离，α、β、γd和/>分别为加权系数，γd与机器人到最近障碍物的距离相关，v、w分别为线速度和角速度；
所述步骤S5中，将优化路径集合lastlist中的路径点作为局部路径规划的子目标，具体为：
依次将优化路径集合lastlist中的路径点作为子目标Gmin，但当机器人与子目标Gmin＝Li的距离小于第一阈值时，设置Gmin＝Li+1，当子目标为Gmin＝Li且Li+j出现在最优的动作轨迹中时，设置Gmin＝Li+j，其中，第一阈值与机器人底盘半径相关，j＞0；
所述步骤S5中，其中，γmax为最短时间通过障碍物密集区域的对应值，γmin为通过障碍物密集区域最安全的对应值，D为机器人到最近障碍物的距离，/>vmax为预设的最大线速度，va为预设的线加速度；
所述步骤S2具体包括如下步骤：
步骤S21：创建空集openlist和closelist，将起始节点S存储到closelist中；
步骤S22：寻找起始节点S周围16个方向上可以到达的24个节点，将它们放入openlist中，并设置它们的父节点为S，其中，以节点S为原点，24个节点的坐标为：[[1，0]，[0，1]，[-1，0]，[0，-1]，[-1，-1]，[-1，1]，[1，-1]，[1，1]，[2，0]，[0，2]，[-2，0]，[0，-2]，[-2，-2]，[-2，2]，[2，-2]，[2，2]，[1，3]，[3，1]，[3，-1]，[1，-3]，[-1，3]，[-3，1]，[-3，-1]，[-1，-3]]；
步骤S23：将节点S从从openlist放入closelist中，再分别计算步骤S22中各节点的F值，并将F值最小的节点A从openlist放入closelist中，再将节点A周围16个方向上除了障碍物和已在closelist中的节点，其他可以到达的节点均放入openlist中，设置这些节点的父节点为A，并以节点A为当前节点，分别计算以这些节点为目标节点的F值，其中，F＝G+H，F值表示从起始节点到目标节点的代价消耗，G值表示从起始节点到当前节点的实际消耗，H值表示从当前节点到目标节点的估计代价消耗；
步骤S24：按照步骤S23继续从openlist中找出F值最小的节点，并将其从openlist放入closelist中，再将该节点设置为父节点，直至openlist中出现目标节点G，逆序遍历父节点即可得到路径点集合closelist＝[C1,C2,…Cn]；
所述步骤S23中，若节点A可以到达的节点B本就已经在openlist中，则计算从起始节点S经节点A到达节点B的G值，将该G值与从起始节点S直接到达节点B的G值进行比较，若前者更低，则将节点B的父节点修改为A，并重新计算节点B的F值，否则，则不作改变；
所述步骤S3中，设Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)为路径点集合closelist中相邻的三点，若(xi+1-xi)(yi+2-yi+1)＝(yi+1-yi)(xi+2-xi+1)成立，判定Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)共线，否则，判定Ci(xi,yi)、Ci+1(xi+1,yi+1)和Ci+2(xi+2,yi+2)不共线，其中，1≤i≤n-2；
所述步骤S5具体包括如下步骤：
步骤S51：将优化路径集合lastlist中的路径点添加到所述栅格地图中，并设置机器人底盘半径以及DWA算法所需要的参数，该参数包括最大线速度、最小线速度、最大角速度、最小角速度、最大线加速度、最大角加速度、线速度分辨率、角速度分辨率、采样周期、向前预测时间；
步骤S52：按照采样周期在由步骤S51所述的参数所形成的速度空间进行速度采样，针对每个速度采样点，均利用DWA算法生成向前预测时间内的运动轨迹，从而得到多个运动轨迹；
步骤S53：利用评估函数G(v,w)给各运动轨迹进行打分，选择最优的运动轨迹，并将该运动轨迹所对应的采样速度作为机器人下一时刻的速度；
步骤S54：判断最优运动轨迹是否达到目标点，若是，输出该运动轨迹，否则，再次进入步骤S52，重新进行采样。
2.根据权利要求1所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述步骤S5中，在利用评估函数G(v,w)进行打分前，分别对heading(v,w)、dist(v,w)、vel(v,w)和path(v,w)进行归一化。
3.根据权利要求1所述的一种融合全局及局部算法的路径规划方法，其特征在于：所述H值即为当前节点到目标节点的欧式距离。
CN202210384849.8A 2022-04-13 2022-04-13 一种融合全局及局部算法的路径规划方法及装置 Active CN114779772B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210384849.8A CN114779772B (zh) 2022-04-13 2022-04-13 一种融合全局及局部算法的路径规划方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210384849.8A CN114779772B (zh) 2022-04-13 2022-04-13 一种融合全局及局部算法的路径规划方法及装置

Publications (2)

Publication Number Publication Date
CN114779772A CN114779772A (zh) 2022-07-22
CN114779772B true CN114779772B (zh) 2023-08-08

Family

ID=82428508

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210384849.8A Active CN114779772B (zh) 2022-04-13 2022-04-13 一种融合全局及局部算法的路径规划方法及装置

Country Status (1)

Country Link
CN (1) CN114779772B (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115167466B (zh) * 2022-08-11 2024-07-19 西安交通大学 一种移动机器人标准控制集局部路径规划方法及系统
CN115562272A (zh) * 2022-10-10 2023-01-03 北京信息科技大学 机器人行进路径的规划方法及装置
CN115826590B (zh) * 2023-02-21 2023-06-02 山东大学 一种自适应参数的移动机器人局部路径规划方法及系统
CN115857515B (zh) * 2023-02-22 2023-05-16 成都瑞华康源科技有限公司 一种agv机器人路线规划方法、系统及存储介质
CN116736866B (zh) * 2023-08-10 2023-12-01 苏州观瑞汽车技术有限公司 一种物业室内机器人路径规划与轨迹控制方法及系统

Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104914862A (zh) * 2015-04-21 2015-09-16 电子科技大学 基于目标方向约束的路径规划算法
CN110703762A (zh) * 2019-11-04 2020-01-17 东南大学 一种复杂环境下水面无人艇混合路径规划方法
CN111174798A (zh) * 2020-01-17 2020-05-19 长安大学 一种足式机器人路径规划方法
CN112731916A (zh) * 2020-10-22 2021-04-30 福建工程学院 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN113608531A (zh) * 2021-07-26 2021-11-05 福州大学 基于安全a*引导点的动态窗口的无人车实时全局路径规划方法
CN114237235A (zh) * 2021-12-02 2022-03-25 之江实验室 一种基于深度强化学习的移动机器人避障方法

Patent Citations (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104914862A (zh) * 2015-04-21 2015-09-16 电子科技大学 基于目标方向约束的路径规划算法
CN110703762A (zh) * 2019-11-04 2020-01-17 东南大学 一种复杂环境下水面无人艇混合路径规划方法
CN111174798A (zh) * 2020-01-17 2020-05-19 长安大学 一种足式机器人路径规划方法
CN112731916A (zh) * 2020-10-22 2021-04-30 福建工程学院 融合跳点搜索法和动态窗口法的全局动态路径规划方法
CN113608531A (zh) * 2021-07-26 2021-11-05 福州大学 基于安全a*引导点的动态窗口的无人车实时全局路径规划方法
CN114237235A (zh) * 2021-12-02 2022-03-25 之江实验室 一种基于深度强化学习的移动机器人避障方法

Also Published As

Publication number Publication date
CN114779772A (zh) 2022-07-22

Similar Documents

Publication Publication Date Title
CN114779772B (zh) 一种融合全局及局部算法的路径规划方法及装置
CN111780777B (zh) 一种基于改进a*算法和深度强化学习的无人车路径规划方法
CN111174798A (zh) 一种足式机器人路径规划方法
CN111610786B (zh) 基于改进rrt算法的移动机器人路径规划方法
US11747826B2 (en) Method for route optimization based on dynamic window and redundant node filtering
CN112378408A (zh) 一种实现轮式移动机器人实时避障的路径规划方法
CN115079705A (zh) 基于改进a星融合dwa优化算法的巡检机器人路径规划方法
EP3770711A1 (en) Method for repositioning robot
CN116804879B (zh) 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法
KR20230041752A (ko) 이동 장치의 장애물 회피 경로를 계획하기 위한 방법 및 장치
CN114675649A (zh) 一种融合改进的a*与dwa算法的室内移动机器人路径规划方法
CN116540731B (zh) 融合堆叠lstm与sac算法的路径规划方法及系统
CN113467476B (zh) 考虑转角约束的无碰撞检测快速随机树全局路径规划方法
CN115309161B (zh) 一种移动机器人路径规划方法、电子设备及存储介质
CN115826586B (zh) 一种融合全局算法和局部算法的路径规划方法及系统
CN114610076B (zh) 结合人工势场法与A-star算法角度约束的航迹规划方法
CN114428499A (zh) 一种融合Astar与DWA算法的移动小车路径规划方法
CN115016458B (zh) 基于rrt算法的地洞勘探机器人路径规划方法
CN113867336A (zh) 一种适用于复杂场景下移动机器人路径导航及规划方法
CN116952239A (zh) 一种基于改进a*与dwa融合的无人艇路径规划方法
CN115328208A (zh) 一种实现全局动态路径规划的无人机路径规划方法
CN110849385A (zh) 基于双层启发搜索共轭梯度下降的轨迹规划方法及系统
CN117075607A (zh) 一种适用于复杂环境的改进jps的无人车路径规划方法
Guan et al. Robot dynamic path planning based on improved A* and DWA algorithms
CN117360546A (zh) 一种车辆控制方法、车辆、电子设备及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant