在學習資料滿天飛的大環境下，知識變得非常零散，體系化的知識并不多，這就導致很多人每天都努力學習到感動自己，最終卻收效甚微，甚至放棄學習。我的使命就是過濾掉大量的無效信息，將知識體系化，以短平快的方式直達問題本質，把大家從大海撈針的痛苦中解脫出來。

文章目錄

• • 0 前言
  • 1 什么是節點拓展
  • 2 基于自行車模型拓展
  • • 2.1 連續拓展
    • 2.2 離散拓展
    • 2.3 中間節點
  • 3 總結

0 前言

上兩篇對Hybrid Astar算法的主體搜索流程做了剖析和實現，本節詳細介紹節點拓展。

1 什么是節點拓展

節點拓展就是基于當前節點狀態空間向量推導下一個節點狀態空間向量。

關于狀態空間的介紹可以參看這篇文章：Hybrid Astar 算法剖析和實現（二）_穿越臨界點的博客-CSDN博客

2 基于自行車模型拓展

2.1 連續拓展

設當前節點 $Nod{e}_{current}$ 中記錄著車輛當前的位姿 $[x_0,y_0,{\theta }_0]$ （狀態量）。求在控制量$[v,?]$ 的控制下，$t$時刻后車輛的位姿。

簡易自行車模型如下式，其中 $L_w$ 代表wheel base，$?$ 為前輪轉角。

$$\frac{d}{dt}?\begin{array}{c}x(t)\\ y(t)\\ \theta (t)\end{array}?=?\begin{array}{c}v(t)?cos\theta (t)\\ v(t)?sin\theta (t)\\ v(t)?tan?(t)/L_w\end{array}?,?t.\text{\hspace{1em}(1)}$$

設當前時刻 $t=0$，則有，
$$?\begin{array}{l}x(0)=x_0\\ y(0)=y_0\\ \theta (0)={\theta }_0\end{array}\text{\hspace{1em}(2)}$$
$t$ 時刻后有，
$$?\begin{array}{ll}x(t)& =x(0)+{\int }_{\tau =0}^{t}v(\tau )?cos\theta (\tau )?d\tau \\ & =x_0+{\int }_{\tau =0}^{t}v(\tau )?cos\theta (\tau )?d\tau \\ y(t)& =y(0)+{\int }_{\tau =0}^{t}v(\tau )?sin\theta (\tau )?d\tau \\ & =y_0+{\int }_{\tau =0}^{t}v(\tau )?sin\theta (\tau )?d\tau \\ \theta (t)& =\theta (0)+{\int }_{\tau =0}^{t}v(\tau )?tan?(\tau )/L_w?d\tau \\ & ={\theta }_0+{\int }_{\tau =0}^{t}v(\tau )?tan?(\tau )/L_w?d\tau \end{array}\text{\hspace{1em}(3)}$$

上式中的 $[x_t,y_t,{\theta }_t]$ 即為t時刻后車輛的位姿，也就是新的狀態節點向量。

2.2 離散拓展

實際使用時不可能進行連續的狀態節點拓展。需要離散化以便減少計算量。

首先，對前輪轉角$?$進行離散化
$${?}_i\in \{{\Phi }_0,{\Phi }_1,\dots ,{\Phi }_m\},i\in [0,m?1],i\in Z\text{\hspace{1em}(4)}$$
然后，對速度$v$進行離散化
$$v_j\in \{?V_{max},V_{max}\}\text{\hspace{1em}(5)}$$

最后，對時間$t$進行離散化
$$t_k\in \{T_0,T_1,\dots ,T_n\},k\in [0,n?1],k\in Z\text{\hspace{1em}(6)}$$

根據式(4)-(6)進行離散化之后，每次拓展的節點狀態個數為 $(m+1)\times 2$。

有同學肯定會提出疑問：拓展的節點個數不應該是 $(m+1)\times 2\times (k+1)$嗎？這就引出了下個話題——中間節點。

2.3 中間節點

我們之前所說的節點狀態指的都是最終節點狀態。為什么會有最終節點這個名稱出現呢？因為引入了中間節點。

那是怎么引入的中間節點呢？是通過對單次拓展時間$t$進行離散化引入的。所以，每個最終節點包含了$k+1$ 個中間節點。

有人肯定會有新的疑問：不使用中間節點到底行不行？答案是不行，原因有兩個。

如果將每個中間節點的身份都進行“提升”，提升為最終節點，那么產生的問題就是狀態柵格分辨率（參看Hybrid Astar 算法剖析和實現（二）_穿越臨界點的博客-CSDN博客）要相應的提升，這樣會增長搜索時間，并且增加內存使用量。

每次拓展拋棄中間節點，直接取 $t=T_n$ 可不可以呢？也是不可以的。因為，如果每次拓展距離過長，只對最終的節點進行碰撞檢測是不可靠的——小體積的障礙物會被直接跳過去。

因此，為了平衡狀態柵格的精度和碰撞檢測的可靠性，才不得不引入中間節點的概念。

Tips：中間節點引入的原因對于初學者來說是很難理解的。我認為這也是Hybrid Astar算法與Astar算法最大的不同。那這種思想或技巧來自哪里呢？

她實際上來自于RRT相關算法。這是搜索樹中經常遇到的問題。

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

---

3 總結

本篇主要介紹了節點拓展的核心思想，并給大家揭開了Hybrid這個字的神秘面紗。下一篇，我們對其進行代碼實現，敬請期待。

恭喜你又堅持看完了一篇博客，又進步了一點點！如果感覺還不錯就點個贊再走吧，你的點贊和關注將是我持續輸出的噠噠噠動力~~

總結

以上是生活随笔為你收集整理的Hybrid Astar 算法剖析和实现（五）的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。