Hybrid Astar 算法剖析和实现（五）

在学习资料满天飞的大环境下，知识变得非常零散，体系化的知识并不多，这就导致很多人每天都努力学习到感动自己，最终却收效甚微，甚至放弃学习。我的使命就是过滤掉大量的无效信息，将知识体系化，以短平快的方式直达问题本质，把大家从大海捞针的痛苦中解脱出来。

文章目录

0 前言
1 什么是节点拓展
2 基于自行车模型拓展
- 2.1 连续拓展
- 2.2 离散拓展
- 2.3 中间节点
3 总结

0 前言

上两篇对Hybrid Astar算法的主体搜索流程做了剖析和实现，本节详细介绍节点拓展。

1 什么是节点拓展

节点拓展就是基于当前节点状态空间向量推导下一个节点状态空间向量。

关于状态空间的介绍可以参看这篇文章：Hybrid Astar 算法剖析和实现（二）_穿越临界点的博客-CSDN博客

2 基于自行车模型拓展

2.1 连续拓展

设当前节点 NodecurrentNode_{current}Nodecurrent 中记录着车辆当前的位姿 [x0,y0,θ0][x_0,y_0,\theta_0][x0,y0,θ0] （状态量）。求在控制量[v,ϕ][v,\phi][v,ϕ] 的控制下，ttt时刻后车辆的位姿。

简易自行车模型如下式，其中 LwL_wLw 代表wheel base，ϕ\phiϕ 为前轮转角。

ddt[x(t)y(t)θ(t)]=[v(t)⋅cosθ(t)v(t)⋅sinθ(t)v(t)⋅tanϕ(t)/Lw],∀t.(1)\frac{d}{dt} \begin{bmatrix} x(t) \\ y(t) \\ \theta(t) \end{bmatrix} = \begin{bmatrix} v(t)\cdot cos\theta(t) \\ v(t) \cdot sin\theta(t) \\ v(t) \cdot tan\phi(t)/L_w \end{bmatrix} ,\forall t. \tag1 dtd⎣⎡x(t)y(t)θ(t)⎦⎤=⎣⎡v(t)⋅cosθ(t)v(t)⋅sinθ(t)v(t)⋅tanϕ(t)/Lw⎦⎤,∀t.(1)

设当前时刻 t=0t=0t=0，则有，
{x(0)=x0y(0)=y0θ(0)=θ0(2)\begin{cases} x(0)=x_0\\ y(0) = y_0 \\ \theta(0) = \theta_0 \end{cases} \tag2 ⎩⎪⎨⎪⎧x(0)=x0y(0)=y0θ(0)=θ0(2)
ttt 时刻后有，
{x(t)=x(0)+∫τ=0tv(τ)⋅cosθ(τ)⋅dτ=x0+∫τ=0tv(τ)⋅cosθ(τ)⋅dτy(t)=y(0)+∫τ=0tv(τ)⋅sinθ(τ)⋅dτ=y0+∫τ=0tv(τ)⋅sinθ(τ)⋅dτθ(t)=θ(0)+∫τ=0tv(τ)⋅tanϕ(τ)/Lw⋅dτ=θ0+∫τ=0tv(τ)⋅tanϕ(τ)/Lw⋅dτ(3)\begin{cases} x(t)&=x(0)+\int_{\tau=0}^t v(\tau)\cdot cos\theta(\tau) \cdot d\tau \\[2ex] &=x_0+\int_{\tau=0}^t v(\tau)\cdot cos\theta(\tau) \cdot d\tau\\[3ex] y(t)&=y(0)+\int_{\tau=0}^t v(\tau)\cdot sin\theta(\tau)\cdot d\tau \\[2ex] &=y_0+\int_{\tau=0}^t v(\tau)\cdot sin\theta(\tau)\cdot d\tau \\[3ex] \theta(t) &= \theta(0) +\int_{\tau=0}^t v(\tau) \cdot tan\phi(\tau)/L_w \cdot d\tau \\[2ex] &= \theta_0 +\int_{\tau=0}^t v(\tau) \cdot tan\phi(\tau)/L_w \cdot d\tau \end{cases}\tag3 ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧x(t)y(t)θ(t)=x(0)+∫τ=0tv(τ)⋅cosθ(τ)⋅dτ=x0+∫τ=0tv(τ)⋅cosθ(τ)⋅dτ=y(0)+∫τ=0tv(τ)⋅sinθ(τ)⋅dτ=y0+∫τ=0tv(τ)⋅sinθ(τ)⋅dτ=θ(0)+∫τ=0tv(τ)⋅tanϕ(τ)/Lw⋅dτ=θ0+∫τ=0tv(τ)⋅tanϕ(τ)/Lw⋅dτ(3)

上式中的 [xt,yt,θt][x_t,y_t,\theta_t][xt,yt,θt] 即为t时刻后车辆的位姿，也就是新的状态节点向量。

2.2 离散拓展

实际使用时不可能进行连续的状态节点拓展。需要离散化以便减少计算量。

首先，对前轮转角ϕ\phiϕ进行离散化
ϕi∈{Φ0,Φ1,…,Φm},i∈[0,m−1],i∈Z(4)\phi_i\in\{\Phi_0,\Phi_1,\ldots,\Phi_m\},\quad i\in[0,m-1],i\in Z \tag4 ϕi∈{Φ0,Φ1,…,Φm},i∈[0,m−1],i∈Z(4)
然后，对速度vvv进行离散化
vj∈{−Vmax,Vmax}(5)v_j\in\{-V_{max},V_{max}\}\tag5 vj∈{−Vmax,Vmax}(5)

最后，对时间ttt进行离散化
tk∈{T0,T1,…,Tn},k∈[0,n−1],k∈Z(6)t_k\in\{T_0,T_1,\ldots,T_n\},\quad k\in[0,n-1],k\in Z \tag6 tk∈{T0,T1,…,Tn},k∈[0,n−1],k∈Z(6)

根据式(4)-(6)进行离散化之后，每次拓展的节点状态个数为 (m+1)×2(m+1)\times2(m+1)×2。

有同学肯定会提出疑问：拓展的节点个数不应该是 (m+1)×2×(k+1)(m+1)\times2\times(k+1)(m+1)×2×(k+1)吗？这就引出了下个话题——中间节点。

2.3 中间节点

我们之前所说的节点状态指的都是最终节点状态。为什么会有最终节点这个名称出现呢？因为引入了中间节点。

那是怎么引入的中间节点呢？是通过对单次拓展时间ttt进行离散化引入的。所以，每个最终节点包含了k+1k+1k+1 个中间节点。

有人肯定会有新的疑问：不使用中间节点到底行不行？答案是不行，原因有两个。

如果将每个中间节点的身份都进行“提升”，提升为最终节点，那么产生的问题就是状态栅格分辨率（参看Hybrid Astar 算法剖析和实现（二）_穿越临界点的博客-CSDN博客）要相应的提升，这样会增长搜索时间，并且增加内存使用量。
每次拓展抛弃中间节点，直接取 t=Tnt=T_nt=Tn 可不可以呢？也是不可以的。因为，如果每次拓展距离过长，只对最终的节点进行碰撞检测是不可靠的——小体积的障碍物会被直接跳过去。

因此，为了平衡状态栅格的精度和碰撞检测的可靠性，才不得不引入中间节点的概念。

Tips：中间节点引入的原因对于初学者来说是很难理解的。我认为这也是Hybrid Astar算法与Astar算法最大的不同。那这种思想或技巧来自哪里呢？

她实际上来自于RRT相关算法。这是搜索树中经常遇到的问题。

说到这里，想必大家对Hybrid的理解也更深刻了吧——就是Astar和RRT的混血。

3 总结

本篇主要介绍了节点拓展的核心思想，并给大家揭开了Hybrid这个字的神秘面纱。下一篇，我们对其进行代码实现，敬请期待。

恭喜你又坚持看完了一篇博客，又进步了一点点！如果感觉还不错就点个赞再走吧，你的点赞和关注将是我持续输出的哒哒哒动力~~