CN113156964A - 一种基于改进动态窗口法的无人地面车辆路径规划方法 - Google Patents

一种基于改进动态窗口法的无人地面车辆路径规划方法 Download PDF

Info

Publication number
CN113156964A
CN113156964A CN202110478482.1A CN202110478482A CN113156964A CN 113156964 A CN113156964 A CN 113156964A CN 202110478482 A CN202110478482 A CN 202110478482A CN 113156964 A CN113156964 A CN 113156964A
Authority
CN
China
Prior art keywords
ground vehicle
unmanned ground
path
evaluation
latitude
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.)
Pending
Application number
CN202110478482.1A
Other languages
English (en)
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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202110478482.1A priority Critical patent/CN113156964A/zh
Publication of CN113156964A publication Critical patent/CN113156964A/zh
Pending legal-status Critical Current

Links

Images

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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
    • 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

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)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开一种基于改进动态窗口法的无人地面车辆路径规划方法，包括步骤：根据无人地面车辆所处的位置和环境构建一个栅格地图，并在其中设置全局路径的起点和终点；根据已构建的栅格地图与全局路径的起点经纬度和终点经纬度，使用A‑Star算法进行全局路径规划，获得一条全局路径以及该全局路径中的前后共n个转折点，n≥1；依次将全局路径的起点、前后共n个转折点和全局路径的终点共n+2个位置中的每两个相邻位置作为一条局部路径的起点和终点，形成n+1条局部路径；依次对所述n+1条局部路径重复地使用改进的动态窗口法进行局部路径规划，以完成无人地面车辆的路径规划。本技术方案能为无人地面车辆行驶规划出短的、平滑的全局路径，且路径规划时间短，应用前景广阔。

Description

一种基于改进动态窗口法的无人地面车辆路径规划方法
技术领域
本发明属于无人地面车辆路径规划技术领域，涉及全局路径规划与局部路径规划相结合的无人地面车辆路径规划方法，尤其是一种基于改进动态窗口法的无人地面车辆路径规划方法。
背景技术
无人地面车辆(unmanned ground vehicle)是一种可自主行驶或者遥控操作的地面移动机械化设备。路径规划(path planning)是无人地面车辆实现自主行驶的关键技术之一，它能够为无人地面车辆规划出一条从起点到终点的优化的行驶路线。
随着社会的进步和科技的发展，无人地面车辆已经开始应用于工业、军事、消防、交通运输等领域，以协助人们完成危险的、常规的、重复性的任务。路径规划方法的效果和性能对无人地面车辆的行驶效率、安全性和稳定性产生很大影响。
路径规划方法分为全局路径规划方法和局部路径规划方法。
典型的全局路径规划方法是A-Star算法(参见：Junwei Yu，Jing Hou，GuangChen.Improved Safety-First A-Star Algorithm for Autonomous Vehicles.5thInternational Conference on Advanced Robotics and Mechatronics，ICARM 2020，December 18-21，2020.IEEE 2020，ISBN 978-1-7281-6479-3，pp.706-710.https：//doi.org/10.1109/ICARM49381.2020.9195318)。A-Star算法的优点是搜索速度快、路径规划时间短、全局优化性能好，不足是路径转折点多、路径不够平滑。
典型的局部路径规划方法是动态窗口法(简称DWA，参见：Fuhai Zhang，Ning Li，Tiegang Xue，Ying Zhu，Rupeng Yuan，Yili Fu：An Improved Dynamic Window ApproachIntegrated Global Path Planning.2019IEEE International Conference on Roboticsand Biomimetics，ROBIO 2019，December 6-8，2019.IEEE 2019，ISBN 978-1-7281-6321-5，pp.2873-2878.https：//doi.org/10.1109/ROBIO49542.2019.8961684)。动态窗口法的优点是计算复杂度较低、规划的路径较平滑，不足是规划出的路径长度较长，路径规划的时间较长。
为了克服所属技术领域中现有技术的不足，即：规划出的路径长度较长、路径不够平滑、路径规划时间长，本发明对动态窗口法(DWA)进行改进后与A-Star算法进行有机结合，形成一种基于改进动态窗口法的无人地面车辆路径规划方法(简称A-Star-DWA方法)。本发明提供的A-Star-DWA方法不仅能够规划出较短且较平滑的路径，而且路径规划时间较短。
本发明的技术方案中利用了所属技术领域中的栅格地图构建技术(Lu Chang，Liang Shan，Jun Li，Yuewei Dai.The Path Planning of Mobile Robots Based on anImproved A-Star Algorithm.16th IEEE International Conference on Networking，Sensing and Control，ICNSC 2019，May 9-11，2019，pp.257-262.https：//doi.org/10.1109/ICNSC.2019.8743249)。
发明内容
本发明所要解决的技术问题是提供一种能够较快地规划出较短且较平滑路径的无人地面车辆路径规划方法。
为解决上述技术问题，本发明是通过以下技术方案实现的：
本发明提供一种基于改进动态窗口法的无人地面车辆路径规划方法，包括下列步骤：
步骤S1：根据无人地面车辆所处的位置和环境构建一个栅格地图，并在其中设置全局路径的起点和终点；
步骤S2：根据已构建的栅格地图与全局路径的起点经纬度和终点经纬度，使用A-Star算法进行全局路径规划，获得一条全局路径以及该全局路径中的前后共n个转折点，n≥1；
步骤S3：依次将全局路径的起点、前后共n个转折点和全局路径的终点共n+2个位置中的每两个相邻位置作为一条局部路径的起点和终点，形成n+1条局部路径；
步骤S4：依次对所述n+1条局部路径重复地使用改进的动态窗口法进行局部路径规划，以完成无人地面车辆的路径规划。
在该方法中，所述步骤S4中使用改进的动态窗口法进行局部路径规划，包括如下步骤S41至S43：
步骤S41：根据无人地面车辆的最大最小速度约束、动力学约束与安全约束得到无人地面车辆的速度空间，其中限定了无人地面车辆行驶的线速度和角速度的取值范围；
步骤S42：根据t时刻的经纬度和方位角使用无人地面车辆的平面运动学方程计算出t+1时刻的经纬度和方位角，所述无人地面车辆的速度空间中线速度和角速度的不同取值形成了多条不同的无人地面车辆的模拟行驶轨迹；
在该方法中，所述步骤S42中的平面运动学方程，进一步包括：
给定无人地面车辆行驶的线速度v和角速度ω，令xt、yt分别表示无人地面车辆在t时刻的经度与纬度，θt表示无人地面车辆在t时刻的方位角，xt+1、yt+1分别表示无人地面车辆在t+1时刻的经度与纬度，θt+1表示无人地面车辆在t+1时刻的方位角，Δt为t+1时刻与t时刻之间的时间间隔，所述平面运动学方程由下列公式定义：
Figure BSA0000240933780000021
步骤S43：使用评价函数对每条无人地面车辆的模拟行驶轨迹进行评价计算，从中选出评价函数值最大的模拟行驶轨迹作为无人地面车辆的最优行驶轨迹；
在该方法中，所述步骤S43中的评价函数，进一步包括：
设v和ω分别为无人地面车辆行驶的线速度和角速度，所述评价函数用G(v，ω)表示，它的值等于下列五个部分的值的总和：方位角评价子函数head(v，ω)、障碍物距离评价子函数dist(v，ω)、速度评价子函数vel(v，ω)、变化角评价子函数change(v，ω)、目标距离评价子函数gdist(v，ω)分别经归一化处理后再各自乘以一个权重系数，更具体地讲，设n_head(v，ω)、n_dist(v，ω)、n_vel(v，ω)、n_change(v，ω)、n_gdist(v，ω)依次分别表示上述五个评价子函数的值经归一化处理后的值，超参数α、β、γ、η、μ依次分别为上述五个评价子函数的权重系数，则评价函数G(v，ω)的值由下列公式计算得到：
G(v，ω)＝α·n_head(v，ω)+β·n_dist(v，ω)+γ·n_vel(v，ω)+η·n_change(v，ω)+μ·n_gdist(v，ω)，式中，head(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的终点的方向之间的角度偏差，按本技术领域中惯用方法计算；dist(v，ω)用于度量无人地面车辆的模拟行驶轨迹上的位置与障碍物之间的最近距离，按本技术领域中惯用方法计算；vel(v，ω)用于度量无人地面车辆的模拟行驶轨迹所对应的线速度，按本技术领域中惯用方法计算；change(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的起点的方向之间的角度偏差，gdist(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端与局部路径的终点之间的距离；
进一步地，设θt为无人地面车辆的模拟行驶轨迹末端的方位角，θo为局部路径的起点的方位角，则change(v，ω)＝180-|θt-θo|；
进一步地，设xi、yi分别为无人地面车辆的模拟行驶轨迹末端的经度与纬度，xj、yi分别为局部路径的终点的经度与纬度，L为局部路径的起点到局部路径的终点的距离，则gdist(v，ω)由下列公式定义：
Figure BSA0000240933780000031
本发明的有益技术效果主要包括两个方面：(1)本发明技术方案为无人地面车辆行驶规划出的全局路径比A-Star算法和动态窗口法规划出的全局路径都更短、更平滑；(2)本发明技术方案大大缩短了动态窗口法的路径规划时间。
下面结合附图对本发明的具体实施方式作进一步的描述。本发明附加的方面和优点将在下面的描述中部分给出，这些将从下面的描述中变得明显，或通过本发明的实践了解到。
附图说明
图1是根据本发明技术方案的一种基于改进动态窗口法的无人地面车辆路径规划方法的流程示意图；
图2是根据本发明技术方案的一种基于改进动态窗口法的无人地面车辆路径规划方法中路径规划过程的示意图；
图3是本发明的一个优选实施例中本发明的A-Star-DWA方法的路径规划过程与结果的示意图；
图4是本发明的一个优选实施例中A-Star算法的路径规划结果的示意图；
图5是本发明的一个优选实施例中DWA方法的路径规划结果的示意图。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的概念、对象、要素等或具有相同或类似功能的概念、对象、要素等。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能解释为对本发明的限制。
本技术领域技术人员可以理解，除非另外定义，这里使用的所有术语(包括技术术语和科学术语)具有与本发明所属领域及相关领域中的普通技术人员的一般理解相同的意义。还应该理解的是，诸如通用字典中定义的那些术语应该被理解为具有与现有技术的上下文中的意义一致的意义，并且除非像这里一样定义，不会用理想化或过于正式的含义来解释。
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。
如图1和图2所示，本发明提供了一种基于改进动态窗口法的无人地面车辆路径规划方法，包括下列步骤S1至S4：
步骤S1：根据无人地面车辆所处的位置和环境构建一个栅格地图，并在其中设置全局路径的起点和终点；
步骤S2：根据已构建的栅格地图与全局路径的起点经纬度和终点经纬度，使用A-Star算法进行全局路径规划，获得一条全局路径以及该全局路径中的前后共n个转折点，n≥1；
步骤S3：依次将全局路径的起点、前后共n个转折点和全局路径的终点共n+2个位置中的每两个相邻位置作为一条局部路径的起点和终点，形成n+1条局部路径；
步骤S4：依次对所述n+1条局部路径重复地使用改进的动态窗口法进行局部路径规划，以完成无人地面车辆的路径规划。
在该方法中，所述步骤S4中使用改进的动态窗口法进行局部路径规划，包括如下步骤S41至S43：
步骤S41：根据无人地面车辆的最大最小速度约束、动力学约束与安全约束得到无人地面车辆的速度空间，其中限定了无人地面车辆行驶的线速度和角速度的取值范围；
步骤S42：根据t时刻的经纬度和方位角使用无人地面车辆的平面运动学方程计算出t+1时刻的经纬度和方位角，所述无人地面车辆的速度空间中线速度和角速度的不同取值形成了多条不同的无人地面车辆的模拟行驶轨迹；
在该方法中，所述步骤S42中的平面运动学方程，进一步包括：
给定无人地面车辆行驶的线速度v和角速度ω，令xt、yt分别表示无人地面车辆在t时刻的经度与纬度，θt表示无人地面车辆在t时刻的方位角，xt+1、yt+1分别表示无人地面车辆在t+1时刻的经度与纬度，θt+1表示无人地面车辆在t+1时刻的方位角，Δt为t+1时刻与t时刻之间的时间间隔，所述平面运动学方程由下列公式定义：
Figure BSA0000240933780000051
步骤S43：使用评价函数对每条无人地面车辆的模拟行驶轨迹进行评价计算，从中选出评价函数值最大的模拟行驶轨迹作为无人地面车辆的最优行驶轨迹；
在该方法中，所述步骤S43中的评价函数，进一步包括：
设v和ω分别为无人地面车辆行驶的线速度和角速度，所述评价函数用G(v，ω)表示，它的值等于下列五个部分的值的总和：方位角评价子函数head(v，ω)、障碍物距离评价子函数dist(v，ω)、速度评价子函数vel(v，ω)、变化角评价子函数change(v，ω)、目标距离评价子函数gdist(v，ω)分别经归一化处理后再各自乘以一个权重系数，更具体地讲，设n_head(v，ω)、n_dist(v，ω)、n_vel(v，ω)、n_change(v，ω)、n_gdist(v，ω)依次分别表示上述五个评价子函数的值经归一化处理后的值，超参数α、β、γ、η、μ依次分别为上述五个评价子函数的权重系数，则评价函数G(v，ω)的值由下列公式计算得到：
G(v，ω)＝α·nhead(v，ω)+β·n_dist(v，ω)+γ·n_vel(v，ω)+η·n_change(v，ω)+μ·n_gdist(v，ω)，式中，head(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的终点的方向之间的角度偏差，按本技术领域中惯用方法计算；dist(v，ω)用于度量无人地面车辆的模拟行驶轨迹上的位置与障碍物之间的最近距离，按本技术领域中惯用方法计算；vel(v，ω)用于度量无人地面车辆的模拟行驶轨迹所对应的线速度，按本技术领域中惯用方法计算；change(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的起点的方向之间的角度偏差，gdist(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端与局部路径的终点之间的距离；
进一步地，设θt为无人地面车辆的模拟行驶轨迹末端的方位角，θo为局部路径的起点的方位角，则change(v，ω)＝180-|θt-θo|；
进一步地，设xi、yi分别为无人地面车辆的模拟行驶轨迹末端的经度与纬度，xj、yj分别为局部路径的终点的经度与纬度，L为局部路径的起点到局部路径的终点的距离，则gdist(v，ω)由下列公式定义：
Figure BSA0000240933780000052
下面以一个具体实施例来进一步说明本发明技术方案的具体实施方式，并给出本发明方案与A-Star算法和动态窗口法(DWA)的性能比较实验结果，其中，性能评价的定量指标使用路径长度、路径规划运行时间，规划出的路径平滑度通过定性来评价。
所述具体实施例中，无人地面车辆所处的位置和环境的地图为沈阳某地区地图，如图3所示。针对所述具体实施例，本发明技术方案的具体实施方式如下：
如图1和图2所示，一种基于改进动态窗口法的无人地面车辆路径规划方法，包括下列步骤S1至S4：
步骤S1：根据无人地面车辆所处的位置和环境构建一个栅格地图，并在其中设置全局路径的起点和终点；在本实施例中：全局路径的起点的经纬度为(123.4123，41.5288)，全局路径的终点的经纬度为(123.4553，41.5847)。
步骤S2：根据已构建的栅格地图与全局路径的起点经纬度和终点经纬度，使用A-Star算法进行全局路径规划，获得一条全局路径以及该全局路径中的前后共n个转折点，n≥1；在本实施例中：如图3所示，所获得的全局路径中前后共n＝10个转折点，它们的经纬度分别为(123.415，41.53175)，(123.42375，41.54226512)，(123.4390431，41.5575)，(123.43925，41.5575)，(123.44625，41.56475)，(123.446375，41.5655)，(123.446625，41.56575)，(123.451625，41.57075)，(123.451625，41.57126698)，(123.45525，41.584625)，图3的地图中标注了全局路径的起点与终点以及转折点1、2、10作为示意。
步骤S3：依次将全局路径的起点、前后共n个转折点和全局路径的终点共n+2个位置中的每两个相邻位置作为一条局部路径的起点和终点，形成n+1条局部路径；在本实施例中：如图3所示，全局路径的起点、10个转折点和全局路径的终点共形成了11条局部路径。
步骤S4：依次对所述n+1条局部路径重复地使用改进的动态窗口法进行局部路径规划，以完成无人地面车辆的路径规划；在本实施例中：依次对11条局部路径重复地使用改进的动态窗口法进行局部路径规划，以完成无人地面车辆的路径规划；
在该方法中，所述步骤S4中使用改进的动态窗口法进行局部路径规划，包括如下步骤S41至S43：
步骤S41：根据无人地面车辆的最大最小速度约束、动力学约束与安全约束得到无人地面车辆的速度空间，其中限定了无人地面车辆行驶的线速度和角速度的取值范围；
步骤S42：根据t时刻的经纬度和方位角使用无人地面车辆的平面运动学方程计算出t+1时刻的经纬度和方位角，所述无人地面车辆的速度空间中线速度和角速度的不同取值形成了多条不同的无人地面车辆的模拟行驶轨迹；
在该方法中，所述步骤S42中的平面运动学方程，进一步包括：
给定无人地面车辆行驶的线速度v和角速度ω，令xt、yt分别表示无人地面车辆在t时刻的经度与纬度，θt表示无人地面车辆在t时刻的方位角，xt+1、yt+1分别表示无人地面车辆在t+1时刻的经度与纬度，θt+1表示无人地面车辆在t+1时刻的方位角，Δt为t+1时刻与t时刻之间的时间间隔，所述平面运动学方程由下列公式定义：
Figure BSA0000240933780000061
步骤S43：使用评价函数对每条无人地面车辆的模拟行驶轨迹进行评价计算，从中选出评价函数值最大的模拟行驶轨迹作为无人地面车辆的最优行驶轨迹；
在该方法中，所述步骤S43中的评价函数，进一步包括：
设v和ω分别为无人地面车辆行驶的线速度和角速度，所述评价函数用G(v，ω)表示，它的值等于下列五个部分的值的总和：方位角评价子函数head(v，ω)、障碍物距离评价子函数dist(v，ω)、速度评价子函数vel(v，ω)、变化角评价子函数change(v，ω)、目标距离评价子函数gdist(v，ω)分别经归一化处理后再各自乘以一个权重系数，更具体地讲，设n_head(v，ω)、n_dist(v，ω)、n_vel(v，ω)、n_change(v，ω)、n_gdist(v，ω)依次分别表示上述五个评价子函数的值经归一化处理后的值，超参数α、β、γ、η、μ依次分别为上述五个评价子函数的权重系数，则评价函数G(v，ω)的值由下列公式计算得到：
G(v，ω)＝α·n_head(v，ω)+β·n_dist(v，ω)+γ·n_vel(v，ω)+η·n_change(v，ω)+μ·n_gdist(v，ω)，式中，head(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的终点的方向之间的角度偏差，按本技术领域中惯用方法计算；dist(v，ω)用于度量无人地面车辆的模拟行驶轨迹上的位置与障碍物之间的最近距离，按本技术领域中惯用方法计算；vel(v，ω)用于度量无人地面车辆的模拟行驶轨迹所对应的线速度，按本技术领域中惯用方法计算；change(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的起点的方向之间的角度偏差，gdist(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端与局部路径的终点之间的距离；
进一步地，设θt为无人地面车辆的模拟行驶轨迹末端的方位角，θo为局部路径的起点的方位角，则change(v，ω)＝180-|θt-θo|；
进一步地，设xi、yi分别为无人地面车辆的模拟行驶轨迹末端的经度与纬度，xj、yi分别为局部路径的终点的经度与纬度，L为局部路径的起点到局部路径的终点的距离，则gdist(v，ω)由下列公式定义：
Figure BSA0000240933780000071
在本实施例：五个评价子函数的权重系数分别设置为α＝0.15、β＝0.2、γ＝0.1、η＝0.05、μ＝0.1；11次重复地使用改进的动态窗口法规划出的11条无人地面车辆的最优行驶轨迹依次组成了一条全局路径，即路径规划的结果，如图3所示。
为了使用上述沈阳某地区地图通过实验将本发明的技术方案(简称A-Star-DWA方法)与A-Star算法、动态窗口法(DWA)进行无人地面车辆路径规划的性能比较，本发明人首先编程实现了这三个方法，所使用的软件开发平台为Qt 5.9，编程语言为C++。下面先给出相关实现技术：
A-Star算法的实现技术：定义get_neighbors函数，实现获取相邻节点的功能；定义adjust_heap函数，实现调整最小堆结构的功能；定义insert_to_opentable函数，实现将节点加入Open表的功能；定义searchRoad函数，实现获得一条全局路径的功能。
动态窗口法(DWA)的实现技术：定义CalcDynamicWindow函数，实现得到无人地面车辆速度空间的功能；定义CarState函数，实现通过无人地面车辆的平面运动学方程推算出模拟行驶轨迹的功能；定义CalcHeadingEval函数，实现计算方位角评价子函数head(v，ω)值的功能；定义CalcDistEval函数，实现计算障碍物距离评价子函数dist(v，ω)值的功能；定义CalcVelEval函数，实现计算速度评价子函数vel(v，ω)值的功能；定义NormalizeEval函数，实现将各个评价子函数的值进行归一化处理的功能；定义DynamicWindowApproach函数，实现选出最优轨迹的功能。
A-Star-DWA方法的实现技术：定义calChangeValue函数，实现计算变化角评价子函数change(v，ω)值的功能；定义calGdistValue函数，实现计算目标距离评价子函数gdist(v，ω)值的功能；定义NormalizeEval函数，实现将各个评价子函数的值进行归一化处理的功能；定义DynamicWindowApproach函数，实现选出最优行驶轨迹的功能；定义HybridPlanning函数，实现完成无人地面车辆路径规划的功能。
实验的软硬件运行环境：(1)操作系统：银河麒麟；(2)CPU：四核phytium FT1500a64bit；(3)内核：Linux-4.4.58-20170818.kylin.5.desktop-generic-aarch64-with-kylin-4.0.2-junipor；(4)内存：8GB；(5)软件平台：Qt 5.9。
实验过程如下：
A-Star-DWA方法的实验过程：(1)调用HybridPlanning函数获得规划出的一条全局路径和转折点；(2)调用DrawRoadPlanResult函数将路径规划的结果以图的形式显示在运行界面上；(3)统计规划出的路径总长度和路径规划时间。
A-Star算法的实验过程：(1)调用searchRoad函数获得规划出的一条全局路径；(2)调用DrawRoadPlanResult函数将路径规划的结果以图的形式显示在运行界面上；(3)统计规划出的路径总长度和路径规划时间。
动态窗口法(DWA)的实验过程：(1)调用DynamicWindowApproach函数获得规划出的一条全局路径；(2)调用DrawRoadPlanResult函数将路径规划的结果以图的形式显示在运行界面上；(3)统计规划出的路径总长度和路径规划时间。
实验结果如下：
沈阳某地区地图是本实验中无人地面车辆所处的位置和环境的地图，上述三个路径规划方法规划出的路径将以软件运行界面截图的形式给出。A-Star-DWA方法规划出的路径如图3所示，A-Star算法规划出的路径如图4所示，动态窗口法(DWA)规划出的路径如图5所示。
上述三个路径规划方法规划出的全局路径长度(单位：公里)以及路径规划时间(单位：秒)如表1所示。表1中实验结果数据表明：与A-Star算法相比，本发明技术方案的A-Star-DWA方法能够规划出更短的路径；与动态窗口法(DWA)相比，本发明技术方案的A-Star-DWA方法能够规划出更短的路径，并且所花的规划时间更短。
表1.路径规划方法的实验结果比较
路径规划方法 规划出的全局路径长度(公里) 路径规划时间(秒)
A-Star-DWA方法 8.052 6.59
A-Star算法 8.236 2.36
DWA方法 8.268 178.05
对比图3、图4和图5中的三条全局路径后可以看出，本发明技术方案的A-Star-DWA方法规划出的全局路径具有更好的平滑度。
总体来说，由本发明上述技术方案及其具体实施方式可以理解的是，本发明的有益技术效果主要包括两个方面：(1)本发明技术方案为无人地面车辆行驶规划出的全局路径比A-Star算法和动态窗口法规划出的全局路径都更短、更平滑；(2)本发明技术方案大大缩短了动态窗口法的路径规划时间。本发明的技术方案具有广阔的应用前景，在实际应用中能够产生很大的社会经济效益。
以上所述仅是本发明的部分实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (4)

1.一种基于改进动态窗口法的无人地面车辆路径规划方法，包括下列步骤：
步骤S1：根据无人地面车辆所处的位置和环境构建一个栅格地图，并在其中设置全局路径的起点和终点；
步骤S2：根据已构建的栅格地图与全局路径的起点经纬度和终点经纬度，使用A-Star算法进行全局路径规划，获得一条全局路径以及该全局路径中的前后共n个转折点，n≥1；
步骤S3：依次将全局路径的起点、前后共n个转折点和全局路径的终点共n+2个位置中的每两个相邻位置作为一条局部路径的起点和终点，形成n+1条局部路径；
步骤S4：依次对所述n+1条局部路径重复地使用改进的动态窗口法进行局部路径规划，以完成无人地面车辆的路径规划。
2.根据权利要求1所述的方法，其特征在于，所述步骤S4中使用改进的动态窗口法进行局部路径规划，包括如下步骤S41至S43：
步骤S41：根据无人地面车辆的最大最小速度约束、动力学约束与安全约束得到无人地面车辆的速度空间，其中限定了无人地面车辆行驶的线速度和角速度的取值范围；
步骤S42：根据t时刻的经纬度和方位角使用无人地面车辆的平面运动学方程计算出t+1时刻的经纬度和方位角，所述无人地面车辆的速度空间中线速度和角速度的不同取值形成了多条不同的无人地面车辆的模拟行驶轨迹；
步骤S43：使用评价函数对每条无人地面车辆的模拟行驶轨迹进行评价计算，从中选出评价函数值最大的模拟行驶轨迹作为无人地面车辆的最优行驶轨迹。
3.根据权利要求1或2所述的方法，其特征在于，所述步骤S42中的平面运动学方程，进一步包括：
给定无人地面车辆行驶的线速度v和角速度ω，令xt、yt分别表示无人地面车辆在t时刻的经度与纬度，θt表示无人地面车辆在t时刻的方位角，xt+1、yt+1分别表示无人地面车辆在t+1时刻的经度与纬度，θt+1表示无人地面车辆在t+1时刻的方位角，Δt为t+1时刻与t时刻之间的时间间隔，所述平面运动学方程由下列公式定义：
Figure FSA0000240933770000011
4.根据权利要求1或2所述的方法，其特征在于，所述步骤S43中的评价函数，进一步包括：
设v和ω分别为无人地面车辆行驶的线速度和角速度，所述评价函数用G(v，ω)表示，它的值等于下列五个部分的值的总和：方位角评价子函数head(v，ω)、障碍物距离评价子函数dist(v，ω)、速度评价子函数vel(v，ω)、变化角评价子函数change(v，ω)、目标距离评价子函数gdist(v，ω)分别经归一化处理后再各自乘以一个权重系数，更具体地讲，设n_head(v，ω)、n_dist(v，ω)、n_vel(v，ω)、n_change(v，ω)、n_gdist(v，ω)依次分别表示上述五个评价子函数的值经归一化处理后的值，超参数α、β、γ、η、μ依次分别为上述五个评价子函数的权重系数，则评价函数G(v，ω)的值由下列公式计算得到：
G(v，ω)＝α·n_head(v，ω)+β·n_dist(v，ω)+γ·n_vel(v，ω)+η·n_change(v，ω)+μ·n_gdist(v，ω)，式中，head(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的终点的方向之间的角度偏差，按本技术领域中惯用方法计算；dist(v，ω)用于度量无人地面车辆的模拟行驶轨迹上的位置与障碍物之间的最近距离，按本技术领域中惯用方法计算；vel(v，ω)用于度量无人地面车辆的模拟行驶轨迹所对应的线速度，按本技术领域中惯用方法计算；change(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端的朝向与该末端到局部路径的起点的方向之间的角度偏差，gdist(v，ω)用于度量无人地面车辆的模拟行驶轨迹末端与局部路径的终点之间的距离；
进一步地，设θt为无人地面车辆的模拟行驶轨迹末端的方位角，θo为局部路径的起点的方位角，则change(v，ω)＝180-|θt-θo|；
进一步地，设xi、yi分别为无人地面车辆的模拟行驶轨迹末端的经度与纬度，xj、yj分别为局部路径的终点的经度与纬度，L为局部路径的起点到局部路径的终点的距离，则gdist(v，ω)由下列公式定义：
Figure FSA0000240933770000021
CN202110478482.1A 2021-04-30 2021-04-30 一种基于改进动态窗口法的无人地面车辆路径规划方法 Pending CN113156964A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110478482.1A CN113156964A (zh) 2021-04-30 2021-04-30 一种基于改进动态窗口法的无人地面车辆路径规划方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110478482.1A CN113156964A (zh) 2021-04-30 2021-04-30 一种基于改进动态窗口法的无人地面车辆路径规划方法

Publications (1)

Publication Number Publication Date
CN113156964A true CN113156964A (zh) 2021-07-23

Family

ID=76872861

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110478482.1A Pending CN113156964A (zh) 2021-04-30 2021-04-30 一种基于改进动态窗口法的无人地面车辆路径规划方法

Country Status (1)

Country Link
CN (1) CN113156964A (zh)

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114326744A (zh) * 2021-12-31 2022-04-12 安徽海博智能科技有限责任公司 一种基于全局地图更新的矿山卡车路径规划方法
CN114428499A (zh) * 2021-12-16 2022-05-03 哈尔滨理工大学 一种融合Astar与DWA算法的移动小车路径规划方法
CN117990119A (zh) * 2024-01-29 2024-05-07 中山大学·深圳 一种基于深度强化学习的分层越野路径规划方法

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105955273A (zh) * 2016-05-25 2016-09-21 速感科技（北京）有限公司 室内机器人导航系统及方法
CN112378408A (zh) * 2020-11-26 2021-02-19 重庆大学 一种实现轮式移动机器人实时避障的路径规划方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105955273A (zh) * 2016-05-25 2016-09-21 速感科技（北京）有限公司 室内机器人导航系统及方法
CN112378408A (zh) * 2020-11-26 2021-02-19 重庆大学 一种实现轮式移动机器人实时避障的路径规划方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
武义: "基于改进A*算法和动态窗口法的机器人路径规划研究", 《工业控制计算进》 *
王洪斌: "基于改进的A*算法与动态窗口法的移动机器人路径规划", 《机器人》 *
陈娇: "改进A*和动态窗口法的移动机器人路径规划", 《计算机集成制造系统》 *

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114428499A (zh) * 2021-12-16 2022-05-03 哈尔滨理工大学 一种融合Astar与DWA算法的移动小车路径规划方法
CN114326744A (zh) * 2021-12-31 2022-04-12 安徽海博智能科技有限责任公司 一种基于全局地图更新的矿山卡车路径规划方法
CN117990119A (zh) * 2024-01-29 2024-05-07 中山大学·深圳 一种基于深度强化学习的分层越野路径规划方法
CN117990119B (zh) * 2024-01-29 2024-10-15 中山大学·深圳 一种基于深度强化学习的分层越野路径规划方法

Similar Documents

Publication Publication Date Title
CN113156964A (zh) 一种基于改进动态窗口法的无人地面车辆路径规划方法
CN109726866A (zh) 基于q学习神经网络的无人船路径规划方法
CN112650242A (zh) 一种基于混合算法的移动机器人路径规划方法
CN113093724A (zh) 一种基于改进蚁群算法的agv路径规划方法
Lacaze et al. Path planning for autonomous vehicles driving over rough terrain
CN106371445A (zh) 一种基于拓扑地图的无人车规划控制方法
CN112556686B (zh) 可预测动态时空环境的最短时间路径规划方法
CN114199248B (zh) 一种基于混合元启发算法优化anfis的auv协同定位方法
US11911902B2 (en) Method for obstacle avoidance in degraded environments of robots based on intrinsic plasticity of SNN
CN116804879A (zh) 一种改进蜣螂算法融合dwa算法的机器人路径规划框架方法
Chen et al. Tracking with UAV using tangent-plus-Lyapunov vector field guidance
CN115562357A (zh) 一种面向无人机集群的智能路径规划方法
Meng et al. Trajectory prediction for automated vehicles on roads with lanes partially covered by ice or snow
Fischer et al. Sampling-based inverse reinforcement learning algorithms with safety constraints
Zheng et al. A hierarchical approach for mobile robot exploration in pedestrian crowd
CN114879660A (zh) 一种基于目标驱动的机器人环境感知方法
Jiang et al. Fuzzy neural network based dynamic path planning
Liu et al. Automatic driving path planning based on A-Star algorithm
CN117590845A (zh) 一种无人机路径规划方法、设备及存储介质
Mi et al. Integration of motion planning and control for high-performance automated vehicles using tube-based nonlinear MPC
Wang et al. A Mapless Navigation Method Based on Deep Reinforcement Learning and Path Planning
Zhang et al. 2D map building and path planning based on LiDAR
Yang et al. The research on real-time map-matching algorithm
Yasuda et al. VERIFICATION OF GRID BASED FASTSLAM WITH MULTIPLE CANDIDATES OF PARTICLES
Skačkauskas et al. Analysis of the hybrid global path planning algorithm for different environments

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
WD01 Invention patent application deemed withdrawn after publication
WD01 Invention patent application deemed withdrawn after publication

Application publication date: 20210723