0 引言

随着移动机器人在各个领域的广泛应用, 非平坦地形(如农田、油田等环境)逐渐成为移动机器人的主要应用场景^[1]. 非平坦地形环境复杂, 这对移动机器人的路径规划方法提出了更多的挑战. 安全可靠的路径规划方法是非平坦地形下移动机器人安全导航的重要保障.

针对3D环境下的路径规划方法, 文献[2]列举了近几年中主要的3D环境中移动机器人的路径规划算法, 其中基于数学模型的算法通过微分方程来描述所有约束条件, 可以提高算法的安全性和可靠性, 但这种方法给计算机带来了沉重的计算负担. 文献[3-4]通过模仿生物的行为对算法进行设计, 这种生物启发式方法可以很好地处理复杂和动态的非结构化环境. 但这种算法通过突变(迭代优化)来优化路径, 使得算法处理时间较长, 效率相对较低. 文献[5-6]是基于采样的算法, 该方法具有使用初始猜测的优点, 随机的初始猜测可确保逃避局部最小值, 并且这种算法不太依赖环境的表示方式. 除此之外, 针对非平坦地形的路径规划常用的是基于3D栅格地图和高程地图的路径规划方法. 文献[7]提出了一种变维度状态空间的建图方法, 该方法在机器人周围的局部区域利用3D栅格地图进行建图, 其他区域使用2D栅格地图进行建图, 这种2D与3D相结合的建图方法, 即可以提高移动机器人局部路径的精度, 又可以保证全局路径的高效性. 文献[8]通过相机对室内的非平坦环境进行3D重建, 以解决带有楼梯与斜坡的3D环境中的移动机器人导航问题. 文献[9]针对非平坦地形, 利用激光雷达和车轮里程计的信息获得地面的高程等数据, 对非平坦地形进行3D栅格建图, 与2D栅格建图相比, 该方法可以更准确地感知周围的环境信息, 但3D栅格地图由于状态空间维数的增加必然导致路径规划时间和计算机内存需求的增加, 尤其是当环境尺寸增加时, 这个问题尤其突出. 为了节省地图存储所占用的空间, 一些学者提出用基于2D栅格地图的高程图法^[10-12], 利用2D栅格地图以及每一个栅格的高程信息组成2.5D栅格地图对环境进行建模. 虽然利用栅格存储单位面积内的高程信息会导致部分环境信息的缺失, 但可以显著降低数据存储空间并提高运算速度.

文献[13]提出了一种基于改进A*的启发式搜索方法, 利用高程地图在非平坦环境上寻找有效路径, 但其设计的启发式函数无法利用高程信息对路径的风险作出判断. 文献[14]通过计算移动机器人在高程地图上的能耗模型, 根据能耗模型提出了一种基于Dijkstra算法的路径规划方法, 该方法通过缩短路径长度以减少移动机器人的能源消耗, 容易产生局部最优解. 文献[15]实现了一种对环境高程信息进行分级的3D栅格地图上的路径规划算法, 对环境中的高程信息进行等级划分, 移动机器人每次移动只能在相同或者相邻等级的栅格间运动, 保证了移动机器人路径的相对平缓, 但是当环境高程差相差过大, 或分级不均匀时, 会直接导致移动机器人无法完成路径规划. 文献[16]提出了一种基于拓扑高程模型的室外三维环境建模和路径规划方法, 虽然该方案可以有效地构建环境并将相邻栅格间的高程差作为路径规划的约束条件之一, 但是由于栅格的高分辨率, 使得相邻栅格之间的距离在环境中只能表示几厘米, 不能根据移动机器人的运动能力进行调节, 因此不适用于其他尺寸的移动机器人.

综上所述, 非平坦地形下的移动机器人路径规划已具有一定的研究成果, 但目前的这些方法没有根据移动机器人的运动能力和环境信息对路径进行优化, 以保证移动机器人在非平坦地形下的安全. 对此, 本文针对非平坦地形环境, 提出一种双分辨率2.5D分层栅格地图的建图方法, 利用双分辨率栅格对环境中的高程信息与障碍物信息进行分层存储, 可以节约移动机器人的存储空间, 提高运算效率. 另外, 在双分辨率2.5D栅格地图的基础上提出一种基于A*算法^[17-19]的路径规划方法, 该方法根据移动机器人的运动能力^[20]对其路径进行优化, 可以规划出一条保证移动机器人自身安全的路径.

1 双分辨率2.5D栅格地图建模

结合2D栅格地图与3D栅格地图的优缺点, 本文提出一种2.5D的建图方案, 称为Multilevel Map, 其是由多层栅格地图组成的一种复合型地图. 如图 1所示, map $ A $与map $ B $为两层栅格地图, map $ A $存储环境中的障碍物信息, map $ B $存储环境中的高程信息. 每一个栅格对应一个节点, 所有的节点都在同一个坐标系下, 即投影到平面下的坐标一致. 例如: 通过函数get.map$ [A] $, map$ (1, 0) $可以获取到map $ A $下(1, 0)节点的障碍物信息; 通过函数get.map$ [B] $, map$ (1, 0) $可以获取到map $ B $下(1, 0)节点的高程信息, 通过这种办法可以得到地图中(1, 0)的环境信息. 移动机器人在路径规划的过程中可以对地图中的每一层信息进行选择性读取.

1.1 障碍物信息层

障碍物信息层利用占据栅格地图存储环境中的障碍物信息, 如图 2所示. 白色栅格表示free状态, 机器人可以在白色栅格中自由移动; 灰色栅格表示occupied状态, 即有障碍物存在, 机器人无法到达该区域.

1.2 高度信息层

高度信息层中的每个节点存储地图的高程信息, 每一个节点中的高程值代表着一个范围(根据分辨率设定)内的高程平均值. 图 3所示是一张尺寸为$ 20\times20 $栅格的高度信息层地图, 每个节点中存储单位面积的高程信息. 比如, (3, 4)节点中的高程值为30, 表示在单位面积中偏离水平面的平均距离为30 cm.

1.3 双分辨率分层

对于大尺寸环境, 利用高分辨建图可能使移动机器人在路径规划时超出其运算能力, 无法快速地获得有效路径^[21]. 因此, 构建高分辨率的大地图是不可取的. 同时, 分辨率过大会导致移动机器人路径规划的精度降低, 增加移动机器人碰撞障碍物的风险, 因此适当的分辨率对大尺寸环境的路径规划有着重要的影响. 使用双分辨率地图是一种很好的解决方法. 障碍物信息层保证移动机器人能准确地识别障碍物的边界, 以实现移动机器人的高效避障, 分辨率可以设置的相对较小. 为避免高度信息层分辨率过低导致大量高程信息丢失, 影响移动机器人路径规划, 本文中高度信息层的分辨率根据移动机器人的尺寸进行设定, 通常选取移动机器人自身长度的二分之一作为高度信息层的分辨率大小, 这样即节省了存储地图所需的空间, 又提高了建图的效率. 如图 4所示, 上层地图为障碍物信息地图, 下层地图为高程信息地图, 其分辨率比例为1:3. 为方便理解, 以下讨论中将使用分辨率为1:3的Multilevel Map地图进行说明.

2 Secure A* (SA*)算法

在双分辨率2.5D栅格地图的基础上, 本文提出一种基于A*算法的安全路径规划算法, 称其为Secure A* (SA*)算法. 传统A*算法的代价函数为

$ \begin{align*} f(n) = g(n)+h(n). \end{align*} $

其中: $ g(n) $表示从起始节点到当前节点的真实消耗; $ h(n) $表示从当前节点到目标节点的估算消耗, 也称为启发值.

SA*算法与传统算法A*不同的是: SA*算法将传统A*算法代价函数中的$ g(n) $与$ h(n) $进行了重新定义, 分别称为$ G_{\rm cost}(n) $和$ H_{\rm cost}(n) $. 新的定义使移动机器人在路径规划时充分考虑环境中高程信息对路径的影响, 从而实现一条符合机器人运动能力以及保证机器人自身安全的平缓路径. SA*算法的代价函数为

$ \begin{align} F_{\rm cost}(n) = xG_{\rm cost}(n)+(1-x)H_{\rm cost}(n), \end{align} $

(1)

其中$ x $是自适应调节因子, 将在2.4节中介绍.

2.1 符号说明

为了表达方便, 本文引入一些符号作为标记, 这些符号的表示及其含义如表 1所示.

注1   $ \lambda = \{s, f, n, t\} $. 其中: $ s $为起始节点, $ f $为父节点, $ n $为当前节点, $ t $为目标节点.

2.2 $ {G}_{ cost}(n) $的定义 2.2.1 双分辨率地图的映射关系

$ G_{\rm cost}(n) $是利用当前节点周围的高程信息进行辨别下一步的移动方向, 因为障碍物信息层地图和高度信息层地图使用的是双分辨率, 所以在计算当前节点$ P_n $的$ G_{\rm cost}(n) $时需要先找到在高度信息层地图上相对应的映射节点$ P_{hn} $, 获取到其中的高程信息$ H_{hn} $后再进行计算. 其映射关系通过算法1实现.

算法1 GetHeightMapPoint.

input: $ P_f(x_f, y_f), P_n(x_n, y_n) $;

output: $ N_k $.

1)  function GetHeightMapPoint $ (P_f(x_f, y_f), P_n(x_n, y_n)) $

2) if $ P_f(x_f, y_f) = P_{df}(x_{df}, y_{df})\; P_n(x_n, y_n) = $ $ P_{df}(x_{df}, y_{df}) $ then

3)   $ N_k\leftarrow{\rm GetSurroundPoint}(P_f(x_f, y_f)) $

4)   $ M_k\leftarrow{\rm GetSurroundPoint}(P_{df}(x_{df}, y_{df})) $

5)   return MappingRelation ($ N_k, M_k $)

6) end if

7) if $ P_f(x_f, y_f) = P_{df}(x_{df}, y_{df}) P_n(x_n, y_n) \ne P_{df}(x_{df}, y_{df}) $ then

8)   $ N_k\leftarrow {\rm GetSurroundPoint}{(P_f(x_f, y_f))} $

9)   $ M_k\leftarrow{\rm GetSurroundPoint}(P_{dn}(x_{dn}, y_{dn})) $

10)   return $ {\rm MappingRelation}{(N_k, M_k)} $

11)  end if

12) end function

13) function GetSurroundPoint$ (P(x, y)) $

14)  for $ i = x-1 \to x+1 $ do

15)   for $ j = y-1 \to y+1 $ do

16)    if IsCanreach$ (i, j) $ then

17)     return $ N_k $

18)    end if

19)   end for

20)  end for

21) end function

22) function MappingRelation $ (N_k, M_k) $

23)   for $ i = 1 \to i = k $ do

24)     $ N_k[i] = M_k[i] $

25)    return $ N_k $

26)   end for

27) end function

将父节点$ P_f $与当前节点$ {P}_n $引入算法1中, 首先GetHeightMapPoint函数会判断父节点$ P_f $与当前节点$ P_n $的高度信息层投影节点$ P_{df}(x_{df}, y_{df}) $与$ P_{dn}(x_{dn}, y_{dn}) $是否为同一个节点. 如图 5所示, 为了方便观察, 障碍物信息层只截取了一次迭代所要遍历的节点. 左图表示父节点$ P_f $与当前节点$ P_n $的高度信息层投影节点$ P_{df}(x_{df}, y_{df}) $与$ P_{dn}(x_{dn}, y_{dn}) $不是同一个节点的情况, 右图表示两者的投影节点是同一个节点的情况. 对于前者情况, GetSurroundPoint函数以节点$ P_f $和节点$ P_{dn}(x_{dn}, y_{dn}) $为中心, 以相同顺序去寻找周围节点, 并分别存入集合$ {N}_k $和集合$ M_k $中, 其中$ k $值表示集合中节点的个数. 对于后者情况同理, GetSurroundPoint函数以节点$ P_f $和节点$ P_{df}(x_{df}, y_{df}) $为中心寻找周围节点, 并分别存入集合$ N_k $和集合$ M_k $中. 最后利用MappingRelation函数将集合$ N_k $和集合$ M_k $中的节点相对应.

根据算法1可以得到: 图 5中左图障碍物信息层节点$ \{a, c, g, i\} $对应的高度信息层的映射节点为$ \{A, $ $ C, G, I\} $; 图 5中右图障碍物信息层中节点$ \{a, c, d, e, $ $ f, g, h\} $对应的高度信息层映射节点为$ \{A, C, E, F, $ $ G, H\} $, 此时$ N_k = \{A, C, E, F, G, H\}, k = 8 $.

2.2.2 $ G_{ cost}(n) $的计算

图 6是图 5中右图的立体图, 计算当前节点$ P_n $的坡度信息如下式所示:

$ \begin{align} \theta_n = {\rm arctan}\Big(\frac{|H_{hn}-H_f|}{\sqrt{(x_{hn}-x_f)^2+(y_{hn}-y_f)^2}}\Big). \end{align} $

(2)

根据文献[14]提出的轮式机器人运动能力的性能指标, 本文以最大可爬坡度与最小可爬坡度作为移动机器人运动能力的性能指标. 设移动机器人最大可爬坡度为$ \theta_{\max} $, 即大于$ \theta_{\max} $的坡(坑)机器人无法通过; 最小可爬坡度为$ \theta_{\min} $, 即小于$ \theta_{\min} $的坡(坑)并不影响移动机器人的正常移动, 可以忽略不计. 考虑到移动机器人的运动能力, 对$ {\theta}_n $进行处理, 有

$ \begin{align} \theta_n = \begin{cases} \theta_{n_1}, \; |\theta_n\leqslant\theta_{\min}|;\\[-3pt] \theta_{n_2}, \; |\theta_{\min} <\theta_n\leqslant\theta_{\max}|;\\[-3pt] \theta_{n_3}, \; |\theta_n > \theta_{\max}|. \end{cases} \end{align} $

(3)

其中: $ \theta_{n_1} $、$ \theta_{n_2} $、$ {\theta}_{n_3} $在数据上等于$ \theta_n $; 在优先级上, $ n_1 >n_2, n_3 $不予考虑.

定义1   $ n_a $是周围正方向节点集合, $ {n}_{b} $是斜方向节点集合, 且优先级上, $ n_a>n_b $, 则有$ N_k = n_a\bigcup n_b $. 将算法1中输出的$ N_k $集合, 采用算法2对其进行优先级排序.

算法2 SortSurroundPoint.

input: $ N_k $;

output: $ N_k $.

1) function SortSurroundPoint$ N_k $

2)   JudgmentPriority $ (N_k) $

3)   for $ i = 1 \to i = k $ do

4)     if $ N_k[i] \in n_a\; \; N_k[i]. \theta_i\in \theta_{n_1} $ then

5)       $ n_c\leftarrow N_k[i] $

6)       Delete$ (N_k[i]) $

7)       if $ N_k[i]. \theta_i \in \theta_{n_2} $ then

8)         $ n_b\leftarrow(N_k[i]. \theta_i-\theta_{\min}) $

9)       end if

10)     end if

11)   end for

12)   $ {\rm Sort}(n_c) $

13)   Sort $ (n_b) $

14)   $ N_k\leftarrow{\rm Splice}{(n_c, n_b)} $

15) end function

16) function JudgmentPriority$ N_k $

17)   for $ i = 1 \to i = k $ do

18)     if $ N_k[i]. \theta_i\leqslant\theta_{\min} $ then

19)       $ N_k[i]. \theta_i\in \theta_{n_1} $

20)     end if

21)     if $ N_k[i]. \theta_i\geqslant\theta_{\max} $ then

22)       $ N_k[i]. \theta_i\in \theta_{n_3} $

23)       Delete$ (N_k[i]) $

24)     else

25)       $ N_k[i]. \theta_i\in \theta_{n_2} $

26)     end if

27)   end for

28)   return $ N_k $

29) end function

算法2的实施流程是: 首先判断集合$ N_k $中节点的优先级, 按照式(3)将节点的坡度值分为$ n_1 $、$ n_2 $、$ n_3 $三级, 并将集合$ N_k $中优先级为$ n_3 $的节点删除. 然后将集合$ N_k $中剩余的节点根据位置关系划分为$ n_a $和$ n_b $两个子集. 对于集合$ n_a $中的节点, 如果节点的优先级为$ n_1 $, 则将其保存到一个集合$ n_c $中, 并将该节点在集合$ n_c $中删除. 如果集合中节点的优先级是$ n_2 $, 则将该节点的$ \theta_{n_2} $进行$ \theta_{n_2}-\theta_{\min} $处理后加到集合$ n_b $中, 对集合$ n_c $和$ n_b $中的节点按照值升序排序. 最后将集合$ n_c $和$ n_b $中的节点首尾相接返回到集合$ N_k $中, 完成对集合$ N_k $中节点的排序工作.

图 7是用算法2对父节点周围节点进行优先级排序的一个简单例子. 为了直观, 截取高度信息层中的3 $ \times $ 3个栅格大小, 并且对应的障碍物信息层节点都是free状态(可到达).

在这个例子中, 设定机器人的最小可爬坡度$ \theta_{\min} = 10 ° $, 最大可爬坡度$ \theta_{\max} = 30 ° $. 每一个节点的高程信息通过式(2)计算得到对应的$ \theta_n $, 此时$ N_k = \{A, B, C, D, E, F, G, H\}, k = 8 $. 算法2中SortSurroundPoint函数首先对集合$ N_k $中的节点进行优先级判断可以得出: 节点$ B $的优先级为$ n_1 $, 其他节点的优先级为$ n_2 $, 无优先级为$ n_3 $的节点. 然后对集合$ N_k $中的节点进行分类得到$ n_a = \{B, D, E, G\}, n_b = \{A, C, F, H\} $, 对节点的$ \theta $值进行优先级排序, 可以看到节点$ B $的$ \theta_B = 8 ° $, 在$ n_a $最小并且优先级为$ n_1 $, 所以优先级最高. 将节点$ B $放入$ n_c $集合中并在$ n_a $删除, 因为$ N_c $集合只有$ B $一个节点所以无需排序, 其他$ n_a $中的节点$ \theta $值都大于$ \theta_{\min} $小于等于$ \theta_{\max} $, 所以优先级属于$ n_2 $, SortSurroundPoint函数会将$ N_a $集合中现有的节点的$ \theta $进行$ \theta_{n_2}-\theta_{\min} $处理, 即$ \theta_{n_2}-10 ° $, 然后加入到$ n_b $集合中, 此时参数$ \theta_D = 3 °, \theta_E = 19 °, \theta_G = 20 °, n_b = \{A, C, F, H, D, E, G\} $. 根据$ n_b $中各节点的$ \theta $值再次排序得到$ n_b = \{D, A, F, E, G, C, H\} $. 最后将$ n_c $和$ n_b $集合按顺序加入$ N_k $集合得到$ N_k = \{B, D, A, F, E, G, C, H\} $, 排序完成. 完成排序后, 需要对节点进行cost值的赋值.

定义2   集合$ B = 1, 2, \ldots, m $, 集合$ S $: cost标准库; $ f:\{N_k\}_{k = 1}^{m}\rightarrow S $, 其中$ S\triangleq\bigcup\limits_{B_{0}\subset B} B_{0}, m = 2U_c -1 $.

利用下式对$ N_k $中节点的cost值进行赋值:

$ \begin{align} &f(N_k) = \\ &\begin{cases} \Big\{\Big[\dfrac{m}{2}\Big]+1-\dfrac{k-1}{2}, \ldots, \Big[\dfrac{m}{2}\Big]+\dfrac{k-1}{2}\Big\}, \\ \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; \; k\in2{{\mathit{\boldsymbol{ Z }}}}+1;\\ \Big\{\Big[\dfrac{m}{2}\Big]-\dfrac{k}{2}+1, \ldots, \Big[\dfrac{m}{2}\Big], \Big[\dfrac{m}{2}\Big]+\\[6pt] 2, \ldots, \Big[\dfrac{m}{2}\Big]+\dfrac{k}{2}+1\Big\}, \; k\in2{{\mathit{\boldsymbol{ Z }}}}. \end{cases} \end{align} $

(4)

其中$ [a] $为高斯函数.

$ \begin{align*} G_{\rm near}(n) = f(N_k(n)), \end{align*} $

则由图 6可得, $ k = 8 $. 设$ U_c = 5 $, 则$ m = 9, B = \{1, 2, 3, 4, 5, 6, 7, 8, 9\}, G_{\rm near}(n) = \{1, 2, 3, 4, 6, 7, 8, 9\}. {\rm SA}^\ast $算法中的$ G_{\rm cost}(n) $计算方式如下:

$ \begin{align*} G_{\rm cost}(n) = G_{\rm cost}(n-1)+G_{\rm near}(n). \end{align*} $

2.3 $ H_{ cost}(n) $的定义

传统A*算法常用的距离计算方式有曼哈顿距离(Manhattan distance)、欧氏距离(Euclidean distance)等. 为了使路径整体较为平缓, 本文提出一种新的距离计算方式—坡度距离(slope distance), 简称$ S_d $. 如图 8所示, $ L $表示欧氏距离, 因为高度信息层地图的存在, 可以求出图中的$ S_d $距离, 这个距离在实际地图中比欧氏距离更加接近真实值.

$ S_d $的计算方式为

$ \begin{align} S_d = (|H_t-H_n|)/{\sin\theta_t}, \end{align} $

(5)

其中$ \theta_t = {\rm arctan}\Big(\dfrac{|H_t-H_n|}{ \sqrt{(x_t-x_n)^2+(y_t-y_n)^2}}\Big) $, 则SA*算法中的$ H_{\rm cost}(n) $计算方式为

$ \begin{align*} H_{\rm cost}(n) = S_d\times\; U_{c}. \end{align*} $

2.4 自适应因子

为了提高算法的效率与精确度, 本文在式(1)中加入一个比列调节因子$ x $, 用于移动机器人在规划路径时的自适应调节. 算法可以根据比例调节因子$ x $改变代价函数中$ G_{\rm cost}(n) $与$ H_{\rm cost}(n) $的比重, 减少路径中的冗余点和无用的区间搜索, 以提高算法的效率和准确率. 比列调节因子$ x $的值根据当前节点到目标节点的坡度距离$ S_d $进行变化. 当$ S_d $很大时, 说明移动机器人距离目标节点较远, 此时可以稍微增加$ H_{\rm cos t}(n) $的权重比例, 减少路径搜索的范围, 以实现算法加速的目的. 随着$ S_d $的减小, 可以慢慢减小$ H_{\rm cost}(n) $的权重比例, 增加$ G_{\rm cost}(n) $的权重比例, 提高算法的准确性. $ S_d $与$ x $的对应关系根据地图的尺寸和机器人自身的尺寸进行设定, 如下所示:

$ \begin{align} x = \begin{cases} \alpha, \; |S_d\leqslant uL_r|;\\ \dfrac{(\beta-\alpha)(S_d-L_r)}{(v-u)L_r} + \beta, \; |uL_r < S_d < vL_r|; \\ \beta, \; |S_d\geqslant vL_r|. \end{cases} \end{align} $

(6)

3 实验结果及分析

为了验证所提出的基于双分辨率2.5D分层地图的安全路径规划方法的有效性, 实验中随机生成50幅地图进行仿真实验. 实验中需要设定的参数如表 2所示.

首先, 针对不同尺寸的环境分别进行2D、2.5D和3D栅格建图以比较不同建图方式下的存储空间大小, 如图 9所示. 其中: 2D和3D栅格地图的分辨率都为0.1 m $ \times $ 0.1 m, 2.5D栅格地图的障碍物信息层分辨率为0.1 m $ \times $ 0.1 m, 高度信息层分辨率为0.3 m $ \times $ 0.3 m.

当地图面积为1 000 m $ \times $ 1 000 m时, 3D栅格的存储空间为1 254.7 MB, 2.5D栅格地图存储空间为579.2 MB, 2D栅格地图存储空间为490.4 MB. 相比之下, 2.5D栅格地图的存储空间比3D栅格地图减少了53.8 %, 比2D栅格地图增长了18.1 %. 随着建图面积的不断增大, 3D栅格地图所需要的存储空间明显高于其他两种建图方式, 2.5D栅格地图和2D栅格地图所需存储空间增幅较为平缓, 且2.5D栅格地图相比2D栅格地图所需的存储空间增幅不大.

为方便观察, 实验中随机生成50幅地图尺寸大小为20$ \times $20个栅格的2.5D栅格地图, 其障碍物信息层栅格分辨率为0.1 m$ \times $0.1 m, 高度信息层栅格分辨率为0.3 m$ \times $0.3 m. 为了验证所提出路径规划方法的有效性, 实验中在同一环境下对比SA*算法与2D A*算法和3D A*算法在路径高程变化上的差异, 仿真计算的结果如表 3所示. 从表 3可以看出, 本文所提出的SA*算法在规划时间和扩展点上与2D A*几乎一样, 但整体路径高程标准相比传统A*算法降低了63.9 %, 而所得路径的真实消耗并没有增加太多. 因为传统的A*算法在规划的过程中并不会考虑高程对路径的影响, 所以尽管3D A*增加了扩展点的数量, 但是最终路径和2D A*并无明显差异, 反而效率降低.

传统A*算法与SA*算法的路径高程变化情况如图 10所示. 可以看出: SA*的路径整体高程波动并不大, 波动区间为$ [-20, 40] $, 整体高程较为平稳; 传统A*算法路径高程起伏较大, 波动区间为$ [-40, 80] $.

实验中, 取其中一组实验数据进行分析. 为了方便观察, 将高度信息层中的高程信息与障碍物信息层合并, 图 11是传统A*算法的路线, 图 12是SA*算法的路线. 对比发现, SA*算法的路径与传统A*算法的路径并不相同, 在坐标$ (11, 7) $处, SA*算法与传统A*选择了不同路径. 传统A*所选取的$ (12, 8) $与$ (13, 8) $节点的高度差为31 cm, SA*算法所选择的$ (12, 8) $与$ (12, 9) $节点的高度差为14 cm, 明显低于传统A*算法选择的路径. 此外, 传统A*算法所选择的路径高程变化极大, 例如$ (17, 9) $与$ (16, 9) $节点之间的高度变换为50 cm, 坡度达到59.04 $ ^\circ $, 已经远远超出了移动机器人的最大可爬坡度$ \theta_{\max} $, 即已经超出了移动机器人的运动能力, 无法保证移动机器人的安全行进. 反观SA*算法的路径更加合理, 路径高度更加平缓, 可以保证移动机器人在该环境下的安全.

4 结论

针对非平坦地形, 本文提出了一种基于2.5D栅格地图的安全路径规划方法. 首先, 利用双分辨率栅格对环境中的高程信息与障碍物信息进行分层存储, 使用高分辨率栅格存储障碍物信息, 利用低分辨率的栅格存储高程信息, 相比3D栅格地图可以节约53.8 %的存储空间. 在此基础上, 提出了一种保证移动机器人安全的路径规划方法, 该方法根据移动机器人的运动能力对路径进行优化, 使得路径的高程标准差相比传统A*算法降低了63.9 %, 路面坡度变化更加平缓, 保证了移动机器人在非平坦环境下的安全. 同时, 算法中加入了自适应因子, 可以减少路径中的冗余点和无用的区间搜索, 以提高算法的效率和准确率. 最后, 通过仿真实验证实了所提出的规划方法的有效性, 该方法能够快速准确地实现一条保证机器人自身安全的路径.