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

文章目錄

• • 0 前言
  • 1 什么是狀態(tài)空間柵格
  • 2 狀態(tài)空間柵格的作用
  • 3 與地圖柵格的區(qū)別和聯(lián)系
  • 4 代碼實現
  • • 4.1 連續(xù)狀態(tài)轉離散狀態(tài)
    • 4.2 離散狀態(tài)轉連續(xù)狀態(tài)
  • 5 狀態(tài)空間柵格的精度該如何選取
  • 6 總結

0 前言

本篇承接上篇，主要對狀態(tài)空間柵格作一個介紹，并討論如何將理論落地為代碼。

1 什么是狀態(tài)空間柵格

狀態(tài)空間柵格是對狀態(tài)空間的離散化。

下面對狀態(tài)空間和離散化分別作一個簡要說明。

狀態(tài)空間

狀態(tài)空間就是以單位狀態(tài)向量為基底的所有向量構成的抽象空間，可以類比笛卡爾坐標系。那什么又是狀態(tài)向量呢？

如果將車輛看成一個質點，那么車輛的笛卡爾坐標(x,y)就可以看作是車輛的狀態(tài)向量。此時，使用Astar算法就綽綽有余了，因為當車輛被看作一個質點時，不存在運動學約束（可以簡單理解為轉彎半徑的約束）。

但真實的車輛一般被抽象為一個剛體，剛體有大小，也就有朝向，因此需要新增一個維度 $\theta$ 來對該狀態(tài)進行描述。那么車輛此時的狀態(tài)向量就是 $(x,y,\theta )$ 了。

離散化

離散化就是針對狀態(tài)向量的每一個維度值進行切分，切分的間隔就是離散的分辨率 $resolution$ ，離散之后相當于將連續(xù)的狀態(tài)向量 $(x,y,\theta )$ 映射到了離散空間當中，對應離散空間中的離散狀態(tài)向量$(x_{index},y_{index},{\theta }_{index})$ 。

連續(xù)狀態(tài)向量到離散狀態(tài)向量的轉換關系如下式：
$$?\begin{array}{l}{x}_{index}=?\frac{x?x_{min}}{resolutio{n}_x}?\\ y_{index}=?\frac{y?y_{min}}{resolutio{n}_y}?\\ {\theta }_{index}=?\frac{\theta ?{\theta }_{min}}{resolutio{n}_{\theta }}?\end{array}\text{\hspace{1em}(1)}$$
離散狀態(tài)向量到連續(xù)狀態(tài)向量的轉換關系如下式：
$$?\begin{array}{l}x=x_{min}+x_{index}\times resolutio{n}_x\\ y=y_{min}+y_{index}\times resolutio{n}_y\\ \theta ={\theta }_{min}+{\theta }_{index}\times resolutio{n}_{\theta }\end{array}\text{\hspace{1em}(2)}$$

2 狀態(tài)空間柵格的作用

Astar算法使用狀態(tài)空間柵格對狀態(tài)空間進行離散化，而后依據狀態(tài)空間柵格進行 探索 。這里的探索就是子節(jié)點的拓展，所以，Astar算法使用狀態(tài)空間柵格的本質就是為了離散迭代——一步步從局部最優(yōu)解逼近全局最優(yōu)解。

Hybrid Astar也是借助狀態(tài)空間柵格進行子節(jié)點的拓展的，但僅僅是借助。

Hybrid Astar子節(jié)點拓展最根本的驅動力來自于對車輛控制的采樣或者說是對輸入的采樣。將對輸入的采樣進行前向模擬（基于車輛運動學方程，后面的文章會詳細介紹），此時將車輛的狀態(tài)映射到狀態(tài)空間柵格中，并把落入的柵格狀態(tài)作為拓展之后的節(jié)點狀態(tài)。再根據該狀態(tài)進行后續(xù)的擴展。

有人可能會有一個疑問：為什么要將前向模擬出來的狀態(tài)映射到狀態(tài)空間柵格后才作為拓展后的子節(jié)點狀態(tài)呢？直接用前向模擬之后的連續(xù)狀態(tài)作為子節(jié)點狀態(tài)不可以么？個人認為是可以的。

所以，Hybrid Astar算法使用狀態(tài)空間柵格的作用僅僅是限制了連續(xù)狀態(tài)向量在狀態(tài)空間的落點；目前，我個人認為在Hybrid Astar算法中可以不使用狀態(tài)空間柵格。

3 與地圖柵格的區(qū)別和聯(lián)系

占據柵格地圖中一般使用地圖柵格來表示障礙物。和狀態(tài)空間柵格沒有直接關系。

地圖柵格在碰撞檢測中才會使用，這里暫時不做介紹。

4 代碼實現

代碼實現參考公式（1）和公式（2）實現。

4.1 連續(xù)狀態(tài)轉離散狀態(tài)

#include <Eigen/Core>Eigen::Vector3i HybridAStar::State2Index(const Eigen::Vector3d &state) const {Eigen::Vector3i index;index[0] = (state[0] - x_min_) / STATE_GRID_RESOLUTION;index[1] = (state[1] - y_min_) / STATE_GRID_RESOLUTION;index[2] = ((state[2] - (-M_PI)) / ANGULAR_RESOLUTION;return index; }

4.2 離散狀態(tài)轉連續(xù)狀態(tài)

#include <Eigen/Core>Eigen::Vector3i HybridAStar::Index2State(const Eigen::Vector3i &index) const {Eigen::Vector3d state;state[0] = index[0] * STATE_GRID_RESOLUTION + x_min_;state[1] = index[1] * STATE_GRID_RESOLUTION + y_min_;state[2] = index[2] * ANGULAR_RESOLUTION + (-M_PI);state[2] = NormalizeAngle(state[2]); //對角度做“歸一化”return index; }

Tips：代碼中NormalizeAngle的實現請參考角度歸一化實現_穿越臨界點的博客-CSDN博客

5 狀態(tài)空間柵格的精度該如何選取

有一個很重要的點就是如何對狀態(tài)空間柵格的精度進行選取。

該精度的選取需要參照下述原則：

一個狀態(tài)柵格中保證只有一個狀態(tài)節(jié)點落入。

狀態(tài)柵格分辨率一般要高于對控制采樣的精度。

如果使用狀態(tài)柵格做碰撞檢測則狀態(tài)柵格的分辨率要不低于地圖柵格分辨率（強烈建議不要混用這兩種柵格，也就避免了考慮本條原則）。

6 總結

本篇介紹了狀態(tài)空間柵格，以及它的作用。為后續(xù)Hybrid Astar算法的剖析和實現打下了空間基礎，不然后面遇到各種坐標和空間變換很容易迷失方向。

下一篇我們一起探索Hybrid Astar的核心邏輯——迭代搜索。

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

總結

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

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