三维路径规划指在已知三维地图中,规划出一条从出发点到目标点满足某项指标最优,并且避开了所有三维障碍物的三维最优路径。现有的路径规划算法中,大部分算法是在二维规划平面或准二维规划平面中进行路径规划。与传统的二维(2D)路径规划相比,三维(3D)路径规划更接近实际应用。但是,一般的三维路径规划算法具有计算过程复杂、信息存储量大、难以直接进行全局规划等问题。已有的三维路径规划算法主要包括$A^*$算法、遗传算法、粒子群算法等,但是$A^*$算法的计算量会随着维数的增加而急剧增加,遗传算法和粒子群算法只是准三维规划算法。

蚁群算法具有分布计算、群体智能等优势,在路径规划上具有很大潜力,在成功用于二维路径规划的同时也可用于三维路径规划。

三维建模

三维空间的建模不仅需要考虑平面情况同时也要考虑到立体情况,而且更复杂的地图结构也就意味着计算机需要处理更多的数据,不可避免的也要考虑到时间成本的增加。因此在进行三维环境建模的时候,要充分将算法的搜索效率和运行成本结合起来。由于栅格法在建模中具有简单高效的特点,因此在三维环境中,本文依然采用栅格法进行三维建模

三维网格的核心思想是将三维空间沿着一个方向划分成一个接一个的平面然后对这些平面使用栅格法进行划分。如下图所示,创建三维空间 $OABC-DEFG$,首先沿 $Y$ 轴划分为相等的 $n$ 部分,这将生成 $n+1$ 个平面 $\Pi _j\left( j=1,2,···,n \right) $,然后分别沿着 $X$ 轴以及 $Z$ 轴分别划分为相等的 $m,l$ 份,最终得到 $m\times n\times l$ 个立方体。

image-20220627202523496

为了便于后续计算,将规划空间 $OABC-DEFG$ 离散为一系列坐标点,并将这些点的集合设置为 $Q^*$。显然,每个点都有两个与之对应的坐标。即,$Q=\left( x_i,y_j,z_k \right) \left( \{i=1,2,···m\},\{j=1,2,···n\},\{l=1,2,···k\} \right)$ 表示坐标系统 $O-XYZ$ 中该点的相应坐标,和 $Q=\left( i,j,k \right) $ 表示该点处相应的序列号。


通过使用三维栅格法进行空间建模后,可以看出空间中的任意两点都可以通过多个平面 $\Pi_j$,进行跳转。例如从起点 $St$ 到终点 $En$,物体需要先经过平面 $\Pi_1$ 然后是平面 $\Pi_2$ ,依此类推,最终经过平面 $\Pi_n$ 到达终点 $En$。

image-20220627201726305

我们对每个平面都进行了栅格化处理。因此当物体到达平面 $\Pi_1$ 时,也会到达平面 $\Pi_1$ 上个具体的点 $Q_1=(i_1,j_1,k_1)$;当到达平面 $\Pi_2$ 时,也会到达平面 $\Pi_2$ 上一个具体的点,即$Q_2=(i_2,j_2,k_2)$ 。

依此类推,最终通过平面 $\Pi_n$ 上具体的点到达终点 $En$。

蚁群算法的使用

基于蚁群算法的三维路径规划的算法流程图如下:

image-20220628083342089

(1)信息素更新

局部更新:局部更新的目的是增加蚂蚁搜索未经过点的概率,达到全局搜索的目的。

$$ \tau _{ijk}=\left( 1-\zeta \right) \tau _{ijk}\tag{1} $$

其中,$\tau_{ijk}$ 为点 $(i,j,k)$ 上所带的信息素值;$\zeta$ 为信息素的衰减系数。

全局更新:全局更新是指当蚂蚁完成一条路径的搜索时,以该路径的长度作为评价值,从路径集合中选择出最短路径,增加最短路径各节点的信息素值。

$$ \begin{align*} \tau _{ijk}&=\left( 1-\rho \right) \tau _{ijk}+\rho \Delta \tau _{ijk}\\ \Delta \tau _{ijk}&=\frac{K}{\min \left( length\left( m \right) \right)}\\ \end{align*}\tag{2} $$

其中,$length(m)$ 为第 $m$ 只蚂蚁经过的路径长度;$\rho$ 为信息素更新系数;$K$ 是系数。

(2)蚁群搜索策略——启发函数的设置

蚂蚁从当前点移动到下一个点时,根据启发函数来计算可视区域内各点的选择概率,启发函数

$$ H\left( i,j,k \right) =D\left( i,j,k \right) ^{w_1}\cdot S\left( i,j,k \right) ^{w_2}\cdot Q\left( i,j,k \right) ^{w_3}\tag{3} $$

其中,

$$ \begin{align*} \left\{ \begin{array}{l} D\left( i,j,k \right)&=\sqrt{\left( x_a-x_b \right) ^2+\left( y_a-y_b \right) ^2+\left( z_a-z_b \right) ^2}\\ S\left( i,j,k \right)&=\frac{Num-UNum}{Num}\\ Q\left( i,j,k \right)&=\sqrt{\left( x_b-x_d \right) ^2+\left( y_b-y_d \right) ^2+\left( z_b-z_d \right) ^2}\\ \end{array} \right.\\ \end{align*}\tag{4} $$

$D(i,j,k)$ 为当前点 $a$ 到下一点 $b$,两点间路径长度,促使蚂蚁选择距离较近的点;

$S(i,j,k)$ 为安全性因素,当选择点不可达到时,该值为 0,促使蚂蚁选择安全点;

$Q(i,j,k)$ 为下一点 $b$ 到目标点 $d$ 的路径长度,促使蚂蚁选择距离目标更近的点;

$w_1,w_2,w_3$ 为系数,代表上述各因素的重要程度;

$Num$ 表示在点 $(i,j,k)$ 中可视点的数量;

$UNum$ 表示可视点中不可达区域的点的数量;

(3)蚁群搜索的步骤

  • Step_1:根据抽象环境确定平面 $\Pi_{j+1}$ 内的可行点集合。
  • Step_2:根据启发函数公式 $(3)$ 依次计算点 $p_i$ 到平面 $\Pi_{j+1}$ 内的可行点集合的启发信息值

$H_{a+1,u,v}$.

  • Step_3:计算在平面 $\Pi_{j+1}$ 内任一点 $(i+1,u,v)$ 的选择概率 $p(i+1,u,v)$

$$ p\left( i+1,u,v \right) =\left\{ \begin{array}{l} \frac{\tau _{a+1,u,v}H_{a+1,u,v}}{\sum{\tau _{a+1,u,v}H_{a+1,u,v}}},& 可行点\\ 0,& 不可行点\\ \end{array} \right.\tag{5} $$

  • Step_4:根据各点的选择概率采用轮盘赌法选择平面 $\Pi_{j+1}$ 内的点。

image-20220628084552515

三维地图避障路径规划仿真

如图所示,我们三维地图其抽象成 $21km*21km$ 的平面上高度 $0\sim2000m$ 的三维空间。为了计算量考虑,某一点高度多少只是一个 $21*21$ 的矩阵,所以也比较稀疏。地图高度信息下载:下载请戳>>>

image-20220628084817288

仿真程序主要由一下四部分构成:
image-20220628084917818

仿真程序

下载请戳>>>

仿真结果

image-20220628092959190

参考文献

[1]熊超文. 蚁群算法的改进及其在路径规划中的应用研究[D].重庆邮电大学,2020.DOI:10.27675/d.cnki.gcydx.2020.000436.

[2] 史峰,王辉,郁磊,胡斐. 《 MATLAB智能算法30个案例分析》[M]. 出版地:北京航空航天大学出版社, 出版年: 2011.7.

Matlab 蚁群算法应用于三维避障

🧐 本文作者:
😉 本文链接:https://lukeyalvin.site/archives/74.html
😊 版权说明:本博客所有文章除特别声明外,均默认采用 CC BY-NC-SA 4.0 许可协议。
最后修改:2022 年 06 月 28 日
赏杯咖啡