參考文獻：
- Efficient constrained path planning via search in state lattices
- Differentially Constrained Mobile Robot Motion Planning in State Lattices
- Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios
1. 整體思想
狀態柵格，顧名思義就在在普通的柵格化地圖的基礎上，加多了機器人運動學模型的約束，確保機器人能沿着相鄰兩點之間生成的路徑運動，並且能夠具有狀態連續性（速度連續等）。
2. 普通柵格(grid)與狀態柵格的異同點
相同之處：它們都是將連續空間離散化，簡化路徑規划。
不同之處：狀態點陣中，每個頂點的連線都是根據機器人的運動學模型生成的，都是切實可行的路徑，而普通柵格(grid)的方法並沒有結合模型，所以兩點之間的路徑，機器人並不一定能通過，舉個例子：
如果現在機器人是一輛四輪小車，圖1是柵格地圖，圖二是根據四輪小車的運動學模型建立的規則點陣（紅色點，沒畫全，意思一下）。假設小車初始角度水平向右，對於小車而言，圖1的藍色路徑，它是沒辦法走的，因為B點處得轉角是90度，而小車轉彎是有一定弧度的，對於圖2而言，假設小車將的轉彎角度離散化成三個方向，每個節點之間的路徑都可通行。
3. 狀態柵格生成
對於普通柵格點，直接給定柵格距離等，直接生成就好，而狀態點陣需要結合機器人的運動空間去生成。
有兩種生成方法，分別為對控制空間的采樣和對狀態空間的采樣。
3.1 控制空間的采樣（前向采樣）
已知機器人的運動學模型，則輸入我們輸入控制量u和積分時間T,向前積分預測后面時刻機器人的狀態，代碼里這里的操作是暴力遍歷，從而得到一系列狀態的集合，成為控制空間的采樣。例如下圖：a圖是四輪小車，b圖是小車當前可運動的六種可能性，c是部分采樣，d是完全采樣。
所以控制空間采樣並沒有預先固定點陣的位置，而是通過控制空間的采樣，形成一定規則的，可行的狀態點陣。
3.2 狀態空間的采樣（逆向采樣）
逆向，也就是指定了相鄰點，結合機器人的模型，逆向算出當前點到相鄰點的可能路徑，放張圖就很好理解前向和逆向采樣的區別。
逆向采樣的計算較為復雜，還涉及到了求解邊界值（BVP）的問題，經典的兩點邊界值問題是多項式方程求解系數的過程，這個如何求，具體這里不做介紹。具體見最下面參考博客鏈接和論文鏈接。
可以留意到，狀態空間采樣是考慮了目標點的反向規划，同時考慮了障礙物環境信息和機器人的運動學模型，所以相比於控制空間，狀態空間采樣更加常用。
4 借助狀態柵格進行運動規划
新建的狀態點陣圖是有向圖，因此在建立了狀態圖的基礎上，可以使用任何圖搜索算法進行路徑規划，比如A*、D*等等，具體的如何設置代價函數等等，不在此討論。
思考：state lattice算法適用於什么對象，有什么局限性或顯著有點
1）運動能力有一定約束的對象，比如四輪機器人（不包括麥克納姆輪），不能原地轉彎，並且轉角也有一定限制，因為這類機器人更適合通過離散化運動空間來做規划，當然比如人形機器人等等，這種運動能力強，能原地轉彎，360度都可行的機器人，用類似采樣的方法，個人覺得就有點失去了機器人本身運動能力的意義了；
2）因為本質上還是采樣的運動空間，所以在這個狀態點陣圖上規划路徑，不是嚴格最優的路徑，做不要嚴格的時間最優或者距離最優，只能是相對的在采樣圖上的距離或者時間最優；
3）State lattice最友好的地方在於可以比較簡單地對路徑進行多維約束，比如一個小車，考慮的規划維數為平面X和Y坐標，以及車頭朝向和路徑曲率（x, y, theta，curvature），這些約束可以很好應用於生成候選狀態點陣，需要結合模型規划就顯得簡單了；
4）理論上這個狀態柵格地圖可以離線建立，然后在線規划，可以實現實時規划
疑惑：有一個點還不是很明白，狀態柵格點圖建立的是局部地圖呢，還是全局地圖呢？Efficient constrained path planning via search in state lattices 一文中，在第4.2節生成Path primitives時，對於采樣代碼是結束條件這樣描述的：This process terminates at a certain radial distance from the origin when all paths at that distance can be composed. 從這句話可以理解到，狀態柵格算法生成的地圖，是基於一定范圍內的地圖，按我的理解，也就是運行過程中需要根據機器人信息，多次建立狀態柵格地圖，與離線地圖又相悖了，歡迎討論指教！
相關代碼：
https://github.com/Alanaab/MotionPlanning
https://github.com/amslabtech/state_lattice_planner
參考文獻：
- Efficient constrained path planning via search in state lattices
- Differentially Constrained Mobile Robot Motion Planning in State Lattices
- Spatiotemporal state lattices for fast trajectory planning in dynamic on-road driving scenarios
參考博客：
https://blog.csdn.net/space_dandy/article/details/114396542
https://zhuanlan.zhihu.com/p/14