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

文章目錄

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

0 前言

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

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

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

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

狀態(tài)空間

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

如果將車輛看成一個(gè)質(zhì)點(diǎn)，那么車輛的笛卡爾坐標(biāo)(x,y)就可以看作是車輛的狀態(tài)向量。此時(shí)，使用Astar算法就綽綽有余了，因?yàn)楫?dāng)車輛被看作一個(gè)質(zhì)點(diǎn)時(shí)，不存在運(yùn)動(dòng)學(xué)約束（可以簡單理解為轉(zhuǎn)彎半徑的約束）。

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

離散化

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

連續(xù)狀態(tài)向量到離散狀態(tài)向量的轉(zhuǎn)換關(guān)系如下式：
$$?\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)向量的轉(zhuǎn)換關(guān)系如下式：
$$?\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)空間柵格對(duì)狀態(tài)空間進(jìn)行離散化，而后依據(jù)狀態(tài)空間柵格進(jìn)行 探索 。這里的探索就是子節(jié)點(diǎn)的拓展，所以，Astar算法使用狀態(tài)空間柵格的本質(zhì)就是為了離散迭代——一步步從局部最優(yōu)解逼近全局最優(yōu)解。

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

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

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

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

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

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

地圖柵格在碰撞檢測(cè)中才會(huì)使用，這里暫時(shí)不做介紹。

4 代碼實(shí)現(xiàn)

代碼實(shí)現(xiàn)參考公式（1）和公式（2）實(shí)現(xiàn)。

4.1 連續(xù)狀態(tài)轉(zhuǎn)離散狀態(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)轉(zhuǎn)連續(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]); //對(duì)角度做“歸一化”return index; }

Tips：代碼中NormalizeAngle的實(shí)現(xiàn)請(qǐng)參考角度歸一化實(shí)現(xiàn)_穿越臨界點(diǎn)的博客-CSDN博客

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

有一個(gè)很重要的點(diǎn)就是如何對(duì)狀態(tài)空間柵格的精度進(jìn)行選取。

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

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

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

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

6 總結(jié)

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

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

恭喜你又堅(jiān)持看完了一篇博客，又進(jìn)步了一點(diǎn)點(diǎn)！如果感覺還不錯(cuò)就點(diǎn)個(gè)贊再走吧，你的點(diǎn)贊和關(guān)注將是我持續(xù)輸出的噠噠噠動(dòng)力~~

總結(jié)

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

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。