CN112325892B - 一种基于改进a*算法的类三维路径规划方法 - Google Patents

Abstract

本发明属于智能无人车领域，具体涉及一种基于改进A*算法的类三维路径规划方法。具体包括如下步骤：步骤(1)：预处理原始数字高程模型，即DEM；步骤(2)：提取分析障碍物因素，建立环境地图模型：建立的环境地图模型基于模糊化障碍物概念，所建模型包含了模糊化障碍物的分布；步骤(3)：优化改进A*算法的代价函数，建立总代价函数；步骤(4)：运用改进A*算法于建立更优的环境地图模型，搜索得到最优路径。本发明方法仅用二维的地图上就有效的表示了环境信息，极大地缩小了数据存储量；本发明把可行驶区域和模糊化障碍物融合到一起，扩大了路径规划的区域，为缩短规划长度提供了可行性。

Description

一种基于改进A*算法的类三维路径规划方法

技术领域

本发明属于智能无人车领域，具体涉及一种基于改进A*算法的类三维路径规划方法。

背景技术

自人工智能交通被研究开始，路径规划一直是这一领域的研究热点和关键技术之一。而车辆领域的路径规划研究主要聚焦于以城市结构化道路为主的二维场景。随着无人驾驶技术日渐成熟，对于拓展路径规划技术应用场景的需求也日益迫切。一些有特殊作业任务的车辆，常常需要在远离城市的非结构化越野道路上行驶，此时常规的路径规划方法难以得到很好的适应，主要的难点主要体现在环境建模和算法的移植性方面。

CN101769754B提出一种基于等高线的类三维地图，利用下降方向的概念找出避障方向，搜索起点与终点之间的直连线周围一定范围的障碍区域，若路径穿过了障碍区域，则找到每段位于障碍区域的路径的下降方向，然后将每段位于障碍区域的路径用沿着障碍区域下降方向绕过的障碍区域的新路径代替，基本规划就是对初始规划进行修正，基本规划后的路径段成为修正路径段。其弊端在于环境建模过于理想，路径规划结果无法保证最优，且适用性和可靠性较差。

发明内容

本发明的目的在于提供一种基于改进A*算法的类三维路径规划方法，规划效果好、适用性高。

实现本发明目的的技术解决方案为：一种基于改进A*算法的类三维路径规划方法，具体包括如下步骤：

步骤(1)：预处理原始数字高程模型(Digital Elevation Model，简称DEM)；

步骤(2)：提取分析障碍物因素，建立环境地图模型：建立的环境地图模型基于模糊化障碍物概念，所建模型包含了模糊化障碍物的分布；

步骤(3)：优化改进A*算法的代价函数，建立总代价函数f(n)＝g(n)+h(n)，

其中g(n)表示从初始节点到任意节点n的代价，h(n)表示从任意节点n到目标节点的启发式评估代价，g(n)和h(n)均考量了实际地形的距离转换；

步骤(4)：运用改进A*算法于建立更优的环境地图模型，搜索得到最优路径。

所述步骤(1)预处理原始数字高程模型具体为：

设定原始DEM数据的分辨率为

M_DEM×N_DEM

M、N分别为x、y方向的栅格数量

则预处理之后的分辨率为

(M_DEM·δ)×(N_DEM×δ)

其中，δ为缩放系数，预处理之后坐标系不变，缩放的新栅格称为分析窗口。

进一步的，所述步骤(2)“提取分析障碍物因素，建立环境地图模型”的具体方法如下：

分析窗口内有n个高程点，任意一个高程点为(x_i,y_i,z_i)，i＝1,2,3…n，其中(x_i,y_i)表示该点在地图中的位置，z_i表示其高程数值，假设拟合的平面方程为：

z＝Ax+By+C

由最小二乘法，对A、B、C使得E最小，其中

对A、B、C分别求偏导：

解得A、B、C的值，已知拟合平面的法向量后，可以根据下式得到平面坡度θ

倾斜度即坡度为d_slope＝θ_n；

将坡度等级分为平地、缓坡、中坡以及陡坡，对于θ>45°的栅格，进行阶跃高程差信息分析提取；

对分析区域内的n个高程点进行其高程值的比较做差，n个高程点中的最高和最低高度之间差的绝对值即为该栅格的阶跃高程差，公式表达如下：

d_step＝H_max-H_min＝H_n

H_max是栅格内的最大高程值，H_min是栅格内的最小高程值；

由此原始的DEM模型转化为一个包含障碍物因素分布的环境地图模型，数学上可以看成一个由No.col*No.row个Node节点组成的二维数组：Data[No.col][No.row]。

进一步的，所述步骤(2)中的每个节点是以下几类信息素的集合：

①节点属性标识：Flag；②节点的横纵坐标位置信息：X,Y；③节点的障碍物地形信息，包括坡度和阶跃高程差：θ_n，H_n；④节点的代价值信息：Value_h,Value_g,Value_f；⑤节点的父节点信息：Parent。

进一步的，所述节点属性标识包含以下几种：

①起始属性标识，表示当前节点位于起点/终点：Startpoint,Goalpoint；②通行属性标识，表示当前节点可以通行/不可通行：Passable,Impassable；③列表属性标识，表示节点在开放列表/关闭列表：Inopen,Inclose。

进一步的，所述步骤(3)的“优化改进A*算法的代价函数，建立总代价函数f(n)＝g(n)+h(n)”具体方法包括：

设定车辆动力输出能实现最大爬坡度T_s、悬架系统能承受的最大阶跃高度T_h(m)，引入障碍指数：α和β

改进代价函数g(n)、h(n)

g(n)表示从初始节点到任意节点n的代价，h(n)表示从节点n到目标点的启发式评估代价；

其中，l_n表示两个分析窗口之间的实际水平位移，

总代价函数f(n)＝g(n)+h(n)。

进一步的，所述步骤(4)的“运用改进A*算法于建立更优的环境地图模型，搜索得到最优路径”具体方法包括：

步骤(4-1)：建立两个不同的列表来存储节点的数据，一个命名为“开启列表”Openlist，用来存储等待检索的节点，另一个命名为“关闭列表”Closelist，用来存储已经检索过的节点；

步骤(4-2)：初始化二维数组，设定起始点与终点，分别将其节点Flag置为Startpoint和Goalpoint，并将起始点存入Openlist，作为第一个检索父节点，其父节点为null；

步骤(4-3)：遍历检索父节点的周围节点，若附近8个节点检索到Flag为Goalpoint的节点，则退出路径搜索，转至步骤(4-6)；

步骤(4-4)：检查被检索节点的障碍系数和列表标识，若被检索的节点障碍系数大于等于1，将通行标识置为Impassable，表明该节点无法通过；若被检索的节点障碍系数小于1，将通行标识置为Passable，表明该节点可以通过，同时将其放入Openlist，并将列表标识置为Inopen；若检查到检索的节点列表标识为Inclose，则说明其已经存放于Closelist，跳过子节点，继续遍历下一节点，对新加入Openlist的节点进行f值比较，选择f值最小的节点移入Closelist；

步骤(4-5)：以Step3中移入Closelist的节点为新的当前检索父节点，遍历检索其周围节点，若某新检索的节点已在Openlist中时，计算此节点相对于父节点的g(n)值，如果新计算的g(n)小于之前存储的g(n)值，则将此节点的g(n)更新为新计算的值；否则，放弃选择当前节点为新的检索父节点，重新按Step3检索f值最小的节点

步骤(4-6)：重复步骤(4-3)-(4-5)直至Flag为Destinationpoint的节点被加入Clsoelist；

步骤(4-7)：由终点Node开始根据父节点往前追溯，直到追溯到父节点为null的起始点，得出最优路径。

与现有技术相比，本发明所具有的有益效果为：

1.与传统的三维环境模型相比，本发明方法仅用二维的地图上就有效的表示了环境信息，极大地缩小了数据存储量。

2.与传统的栅格地图相比，本发明方法把可行驶区域和模糊化障碍物融合到一起，扩大了路径规划的区域，为缩短规划长度提供了可行性。

3.本发明将车辆越野性能参数作为障碍物判定的依据之一，兼顾了地形与车辆信息，适用范围广，可移植性好。

附图说明

图1为本发明缩放预处理DEM的效果图。

图2为本发明提取分析障碍物因素后的环境地图模型。

图3为本发明改进A*算法的类三维路径规划方法流程图。

图4为本发明算法应用于一种高性能车辆仿真结果。

图5为本发明算法应用于一种低性能车辆仿真结果。

具体实施方式

下面结合附图对本发明作进一步详细描述。

实施例1

步骤1：

如图1所示，在地理信息空间云下载选取了某野外非结构化道路环境的数字高程模型，原始模型分辨率为5m*5m，高程点数量为40×40共1600个。缩放预处理后的新栅格分辨率为10m*10m，个数为20×20共400个，规划总面积为40000m^2。

步骤2：提取分析障碍物因素，建立环境地图模型。

分析窗口内有4个高程点，任意一个高程点为(x_i,y_i,z_i)，i＝1,2,3,4。其中(x_i,y_i)表示该点在地图中的位置，z_i表示其高程数值。假设拟合的平面方程为：

z＝Ax+By+C#

由最小二乘法，对A、B、C使得E最小。其中

对A、B、C分别求偏导：

解得A、B、C的值，已知拟合平面的法向量后，可以根据下式得到平面坡度θ

倾斜度即坡度为d_slope＝θ_n。

对于坡度聚类，可选的：θ<15°时统归为缓坡，15°≤θ<25°时为中坡，25°≤θ≤45°为陡坡，θ>45°时为阶跃高程差。对于θ>45°的栅格，进行阶跃高程差信息分析提取。

对分析区域内的n个高程点进行其高程值的比较做差，n个高程点中的最高和最低高度之间差的绝对值即为该栅格的阶跃高程差，公式表达如下：

d_step＝H_max-H_min＝H_n#

H_max是栅格内的最大高程值，H_min是栅格内的最小高程值。可选的，以野外作业车辆的一般车身高度约2m为界限，将阶跃高程差分为两个等级。

处理后的DEM模型转化为一个包含障碍物因素分布的环境地图模型，数学上可以看成一个由No.col*No.row个Node节点组成的二维数组：Data[No.col][No.row]

每个节点是以下几类信息素的集合：

①节点属性标识：Flag；②节点的横纵坐标位置信息：X,Y；③节点的障碍物地形信息(包括坡度和阶跃高程差)：θ_n，H_n；④节点的代价值信息：Value_h,Value_g,Value_f；⑤节点的父节点信息：Parent

其中，节点属性标识又包含以下几种：

①起始属性标识，表示当前节点位于起点/终点：Startpoint,Goalpoint；②通行属性标识，表示当前节点可以通行/不可通行：Passable,Impassable；③列表属性标识，表示节点在开放列表/关闭列表：Inopen,Inclose。

提取分析障碍物因素后的环境地图模型如图2所示

步骤3：设计改进A*算法的代价函数，建立总代价函数f(n)＝g(n)+h(n)

设定车辆动力输出能实现最大爬坡度T_s1＝75％，T_s2＝27％悬架系统能承受的最大阶跃高度T_h1＝5(m)，T_h2＝1(m)。

引入障碍指数：α和β

在进行节点检索时，当节点的α≥1或β≥1时，将节点的Flag置为Impassable；反之，则置为Passable。

改进代价函数g(n)、h(n)

总代价函数f(n)＝g(n)+h(n)

步骤4：运用改进A*算法于建立好的环境地图模型，搜索得到最优路径，算法步骤如图3所示。

①建立两个不同的列表来存储节点的数据，一个命名为“开启列表”(Openlist)，用来存储等待检索的节点，另一个命名为“关闭列表”(Closelist)，用来存储已经检索过的节点。

②初始化二维数组，设定起始点与终点，直线距离约为47.16m。分别将其节点Flag置为Startpoint和Goalpoint，并将起始点存入Openlist，作为第一个检索父节点，其父节点为null。

③遍历检索父节点的周围节点，若附近8个节点检索到Flag为Goalpoint的节点，则退出路径搜索，转至Step6

④检查被检索节点的障碍系数和列表标识，若被检索的节点障碍系数大于等于1，将通行标识置为Impassable，表明该节点无法通过；若被检索的节点障碍系数小于1，将通行标识置为Passable，表明该节点可以通过，同时将其放入Openlist，并将列表标识置为Inopen。若检查到检索的节点列表标识为Inclose，则说明其已经存放于Closelist，跳过子节点，继续遍历下一节点。对新加入Openlist的节点进行f值比较，选择f值最小的节点移入Closelist。

⑤以Step3中移入Closelist的节点为新的当前检索父节点，遍历检索其周围节点，若某新检索的节点已在Openlist中时，计算此节点相对于父节点的g(n)值，如果新计算的g(n)小于之前存储的g(n)值，则将此节点的g(n)更新为新计算的值。否则，放弃选择当前节点为新的检索父节点，重新按Step3检索f值最小的节点。

⑥重复步骤③-⑤直至Flag为Destinationpoint的节点被加入Clsoelist。

⑦由终点Node开始根据父节点往前追溯，直到追溯到父节点为null的起始点，得出最优路径，路径规划结果如图4、5所示。

Claims (5)

1.一种基于改进A*算法的类三维路径规划方法，其特征在于，具体包括如下步骤：

步骤(1)：预处理原始数字高程模型，即DEM；

步骤(2)：提取分析障碍物因素，建立环境地图模型：建立的环境地图模型基于模糊化障碍物概念，所建模型包含了模糊化障碍物的分布；

步骤(3)：优化改进A*算法的代价函数，建立总代价函数f(n)＝g(n)+h(n)，

其中g(n)表示从初始节点到任意节点n的代价，h(n)表示从任意节点n到目标节点的启发式评估代价，g(n)和h(n)均考量了实际地形的距离转换；

步骤(4)：运用改进A*算法于建立更优的环境地图模型，搜索得到最优路径；

所述步骤(1)预处理原始数字高程模型具体为：

设定原始DEM数据的分辨率为M_DEM×N_DEM

M、N分别为x、y方向的栅格数量

则预处理之后的分辨率为(M_DEM·δ)×(N_DEM×δ)

其中，δ为缩放系数，预处理之后坐标系不变，缩放的新栅格称为分析窗口；

所述步骤(2)“提取分析障碍物因素，建立环境地图模型”的具体方法如下：

分析窗口内有n个高程点，任意一个高程点为(x_i,y_i,z_i)，i＝1,2,3…n，其中(x_i,y_i)表示该点在地图中的位置，z_i表示其高程数值，假设拟合的平面方程为：

z＝Ax+By+C

由最小二乘法，对A、B、C使得E最小，其中

对A、B、C分别求偏导：

解得A、B、C的值，已知拟合平面的法向量(A,B,-1)后，可以根据下式得到平面坡度θ

倾斜度即坡度为d_slope＝θ_n；

将坡度等级分为平地、缓坡、中坡以及陡坡，对于θ>45°的栅格，进行阶跃高程差信息分析提取；

对分析区域内的n个高程点进行其高程值的比较做差，n个高程点中的最高和最低高度之间差的绝对值即为该栅格的阶跃高程差，公式表达如下：

d_step＝H_max-H_min＝H_n

H_max是栅格内的最大高程值，H_min是栅格内的最小高程值；

由此原始的DEM模型转化为一个包含障碍物因素分布的环境地图模型，数学上可以看成一个由No.col*No.row个Node节点组成的二维数组：Data[No.col][No.row]。

2.根据权利要求1所述的方法，其特征在于，所述步骤(2)中的每个节点是以下几类信息素的集合：

①节点属性标识：Flag；②节点的横纵坐标位置信息：X,Y；③节点的障碍物地形信息，包括坡度和阶跃高程差：θ_n，H_n；④节点的代价值信息：Value_h,Value_g,Value_f；⑤节点的父节点信息：Parent。

3.根据权利要求2所述的方法，其特征在于，所述节点属性标识包含以下几种：

①起始属性标识，表示当前节点位于起点/终点：Startpoint,Goalpoint；②通行属性标识，表示当前节点可以通行/不可通行：Passable,Impassable；③列表属性标识，表示节点在开放列表/关闭列表：Inopen,Inclose。

4.根据权利要求3所述的方法，其特征在于，所述步骤(3)的“优化改进A*算法的代价函数，建立总代价函数f(n)＝g(n)+h(n)”具体方法包括：

设定车辆动力输出能实现最大爬坡度T_s、悬架系统能承受的最大阶跃高度T_h(m)，引入障碍指数：α和β

改进代价函数g(n)、h(n)

g(n)表示从初始节点到任意节点n的代价，h(n)表示从节点n到目标点的启发式评估代价；

其中，l_n表示两个分析窗口之间的实际水平位移，

总代价函数f(n)＝g(n)+h(n)。

5.根据权利要求4所述的方法，其特征在于，所述步骤(4)的“运用改进A*算法于建立更优的环境地图模型，搜索得到最优路径”具体方法包括：

步骤(4-1)：建立两个不同的列表来存储节点的数据，一个命名为“开启列表”Openlist，用来存储等待检索的节点，另一个命名为“关闭列表”Closelist，用来存储已经检索过的节点；

步骤(4-2)：初始化二维数组，设定起始点与终点，分别将其节点Flag置为Startpoint和Goalpoint，并将起始点存入Openlist，作为第一个检索父节点，其父节点为null；

步骤(4-3)：遍历检索父节点的周围节点，若附近8个节点检索到Flag为Goalpoint的节点，则退出路径搜索，转至步骤(4-6)；

步骤(4-4)：检查被检索节点的障碍系数和列表标识，若被检索的节点障碍系数大于等于1，将通行标识置为Impassable，表明该节点无法通过；若被检索的节点障碍系数小于1，将通行标识置为Passable，表明该节点可以通过，同时将其放入Openlist，并将列表标识置为Inopen；若检查到检索的节点列表标识为Inclose，则说明其已经存放于Closelist，跳过子节点，继续遍历下一节点，对新加入Openlist的节点进行f值比较，选择f值最小的节点移入Closelist；

步骤(4-5)：以Step3中移入Closelist的节点为新的当前检索父节点，遍历检索其周围节点，若某新检索的节点已在Openlist中时，计算此节点相对于父节点的g(n)值，如果新计算的g(n)小于之前存储的g(n)值，则将此节点的g(n)更新为新计算的值；否则，放弃选择当前节点为新的检索父节点，重新按Step3检索f值最小的节点

步骤(4-6)：重复步骤(4-3)-(4-5)直至Flag为Destinationpoint的节点被加入Clsoelist；

步骤(4-7)：由终点Node开始根据父节点往前追溯，直到追溯到父节点为null的起始点，得出最优路径。