本文僅供學習使用，並非商業用途，全文是針對哈爾濱工業大學劉文之的論文《移動機器人的路徑規劃與定位技術研究》進行提煉與學習。論文來源中國知網，引用格式如下：
[1]劉文之. 基於激光雷達的SLAM和路徑規劃算法研究與實現[D].哈爾濱工業大學,2018.
相關坐標系轉換原理已經在前壹篇文章寫完了，直接上轉換方程。
這裏他的運動模型選擇的是基於裏程計的運動模型，還有壹種基於速度的運動模型，其實都差不多，整體思想都壹樣。裏程計是通過計算壹定時間內光電編碼器輸出脈沖數來估計機器人運動位移的裝置，主要是使用光電碼盤。根據光電碼盤計算出此時輪子的速度，然後通過已知的輪子半徑來獲得單位時間 每個輪子 的位移增量。
高等數學可知單位時間位移增量就是速度，對速度在壹定時間上進行積分就得到這壹段時間所走過的路程。
根據上圖，我們可以求出來機器人航向角角速度、圓弧運動半徑和機器人角度變化量，由此可以解的機器人在當前時刻的位姿。
實際上也是有誤差，所以單獨依靠裏程計會與實際結果產生較大誤差，所以必須引入其他的外部傳感器對外部環境的觀測來修正這些誤差，從而提高定位精度。
首先肯定需要將激光雷達所測得的端點坐標從極坐標、機器人坐標中轉換到世界坐標中。
這張略過，暫時不需要看這個
路徑規劃算法介紹：
因為該算法會產生大量的無用臨時途徑，簡單說就是很慢，所以有了其他算法。
了解兩種代價之後，對於每壹個方塊我們采用預估代價與當前路徑代價相加的方法，這樣可以表示每壹個路徑點距離終點的距離。在BFS搜索過程的基礎上，優先挑選總代價最低的那個路徑進行搜索，就可以少走不少彎路。（算法講解 /video/BV1bv411y79P?from=search&seid=3623681329596549549 ）
在局部路徑規劃算法之中，我們選用DWA算法（dynamic window approach），又叫動態窗口法。動態窗口法主要是在速度(v, w)空間中采樣多組速度，並模擬機器人在這些速度下壹定時間內的軌跡。在得到多組軌跡後，對這些軌跡進行評價，選取最優的軌跡所對應的速度來驅動機器人運動。
state sampling就是按照之前給出的全局路徑規劃，無論是Dijkstra還是A* 都可以方便的得到state sampling，DWA算法所需要提前建立的action sampling有兩種：
但是無論是什麽情況，上述所做的工作就是把機器人的位移轉化到世界坐標中來，而不是機器人坐標系。速度采樣結束之後，只需要對小車的軌跡進行評判，就可以得到最優解了。下面介紹速度采樣的辦法。
對速度進行采樣壹般有以下三個限制：
當確定了速度範圍之後，就需要根據速度分辨率來對小車速度離散化，在每壹時刻將小車在不同直線速度角速度組合下所即將要行駛的距離都可視化出來。
其中每壹條軌跡都是很多小直線連接起來的。
需要用評價函數來對上述軌跡進行選擇，選擇最適合的軌跡
最後為了讓三個參數在評價函數裏所發揮的作用均等，我們使用歸壹化處理來計算權重。
算法流程整體如下：