這可能是史上最全的 Python 算法集！
來源丨數據 STUDIO
本文是一些機器人算法（特別是自動導航算法）的 Python 代碼合集。
其主要特點有以下三點：選擇了在實踐中廣泛應用的算法；依賴最少；容易閱讀，容易理解每個算法的基本思想。希望閱讀本文後能對你有所幫助。
目錄
一、環境需求
二、怎樣使用
三、本地化
3.1 擴展卡爾曼濾波本地化
3.2 無損卡爾曼濾波本地化
3.3 粒子濾波本地化
3.4 直方圖濾波本地化四、映射
4.1 高斯網格映射
4.2 光線投射網格映射
4.3 k 均值物體聚類
4.4 圓形擬合物體形狀識別五、SLAM
5.1 迭代最近點匹配 5.2EKF SLAM
5.3 FastSLAM 1.0
5.4 FastSLAM 2.0
5.5 基於圖的 SLAM六、路徑規劃
6.1 動態窗口方式
6.2 基於網格的搜索
迪傑斯特拉算法
A * 算法
勢場算法
6.3 模型預測路徑生成
路徑優化示例
查找表生成示例
6.4 狀態晶格規劃
均勻極性採樣（Uniform polar sampling）
偏差極性採樣（Biased polar sampling）
路線採樣（Lane sampling）
6.5 隨機路徑圖（PRM）規劃
6.6 Voronoi 路徑圖規劃
6.7 快速搜索隨機樹（RRT）
基本 RRT
RRT*
基於 Dubins 路徑的 RRT
基於 Dubins 路徑的 RRT*
基於 reeds-shepp 路徑的 RRT*
Informed RRT*
批量 Informed RRT*
閉合迴路 RRT*
LQR-RRT*
6.8 三次樣條規劃
6.9 B 樣條規劃
6.10 Eta^3 樣條路徑規劃
6.11 貝濟埃路徑規劃
6.12 五次多項式規劃
6.13 Dubins 路徑規劃
6.14 Reeds Shepp 路徑規劃
6.15 基於 LQR 的路徑規劃
6.16 Frenet Frame 中的最優路徑七、路徑跟蹤
7.1 姿勢控制跟蹤
7.2 純追跡跟蹤
7.3 史坦利控制
7.4 後輪反饋控制
7.5 線性二次 regulator（LQR）轉向控制
7.6 線性二次 regulator（LQR）轉向和速度控制
7.7 模型預測速度和轉向控制八、項目支持
一、環境需求
Python 3.6.x
numpy
scipy
matplotlib
pandas
cvxpy 0.4.x
二、怎樣使用
安裝必要的庫；
克隆本代碼倉庫；
執行每個目錄下的 python 腳本；
如果你喜歡，則收藏本代碼庫。
三、本地化
3.1 擴展卡爾曼濾波本地化
該算法利用擴展卡爾曼濾波器（Extended Kalman Filter, EKF）實現傳感器混合本地化。
藍線爲真實路徑，黑線爲導航推測路徑（dead reckoning trajectory），綠點爲位置觀測（如 GPS），紅線爲 EKF 估算的路徑。紅色橢圓爲 EKF 估算的協方差。
相關閱讀：
概率機器人學
http://www.probabilistic-robotics.org/
3.2 無損卡爾曼濾波本地化
該算法利用無損卡爾曼濾波器（Unscented Kalman Filter, UKF）實現傳感器混合本地化。線和點的含義與 EKF 模擬的例子相同。
相關閱讀：
利用無差別訓練過的無損卡爾曼濾波進行機器人移動本地化
https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization
3.3 粒子濾波本地化
該算法利用粒子濾波器（Particle Filter, PF）實現傳感器混合本地化。
藍線爲真實路徑，黑線爲導航推測路徑（dead reckoning trajectory），綠點爲位置觀測（如 GPS），紅線爲 PF 估算的路徑。該算法假設機器人能夠測量與地標（RFID）之間的距離。PF 本地化會用到該測量結果。
相關閱讀：
概率機器人學
http://www.probabilistic-robotics.org/
3.4 直方圖濾波本地化
該算法是利用直方圖濾波器（Histogram filter）實現二維本地化的例子。
紅十字是實際位置，黑點是 RFID 的位置。藍色格子是直方圖濾波器的概率位置。在該模擬中，x，y 是未知數，yaw 已知。濾波器整合了速度輸入和從 RFID 獲得距離觀測數據進行本地化。不需要初始位置。
相關閱讀：
概率機器人學
http://www.probabilistic-robotics.org/
四、映射
4.1 高斯網格映射
本算法是二維高斯網格映射（Gaussian grid mapping）的例子。
4.2 光線投射網格映射
本算法是二維光線投射網格映射（Ray casting grid map）的例子。
4.3 k 均值物體聚類
本算法是使用 k 均值算法進行二維物體聚類的例子。
4.4 圓形擬合物體形狀識別
本算法是使用圓形擬合進行物體形狀識別的例子。
藍圈是實際的物體形狀。紅叉是通過距離傳感器觀測到的點。紅圈是使用圓形擬合估計的物體形狀。
五、SLAM
同時本地化和映射（Simultaneous Localization and Mapping，SLAM）的例子。
5.1 迭代最近點匹配
本算法是使用單值解構進行二維迭代最近點（Iterative Closest Point，ICP）匹配的例子。它能計算從一些點到另一些點的旋轉矩陣和平移矩陣。
相關閱讀：
機器人運動介紹：迭代最近點算法
https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf
5.2 EKF SLAM
這是基於擴展卡爾曼濾波的 SLAM 示例。藍線是真實路徑，黑線是導航推測路徑，紅線是 EKF SLAM 估計的路徑。綠叉是估計的地標。
相關閱讀：
概率機器人學
http://www.probabilistic-robotics.org/
5.3 FastSLAM 1.0
這是用 FastSLAM 1.0 進行基於特徵的 SLAM 的示例。藍線是實際路徑，黑線是導航推測，紅線是 FastSLAM 的推測路徑。紅點是 FastSLAM 中的粒子。黑點是地標，藍叉是 FastLSAM 估算的地標位置。
相關閱讀：
概率機器人學
http://www.probabilistic-robotics.org/
5.4 FastSLAM 2.0
這是用 FastSLAM 2.0 進行基於特徵的 SLAM 的示例。動畫的含義與 FastSLAM 1.0 的情況相同。
相關閱讀：
概率機器人學
http://www.probabilistic-robotics.org/
Tim Bailey 的 SLAM 模擬
http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm
5.5 基於圖的 SLAM
這是基於圖的 SLAM 的示例。
藍線是實際路徑。黑線是導航推測路徑。紅線是基於圖的 SLAM 估算的路徑。黑星是地標，用於生成圖的邊。
相關閱讀：
基於圖的 SLAM 入門
http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
六、路徑規劃
6.1 動態窗口方式
這是使用動態窗口方式（Dynamic Window Approach）進行二維導航的示例代碼。
相關閱讀：
用動態窗口方式避免碰撞
https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf
6.2 基於網格的搜索
迪傑斯特拉算法
這是利用迪傑斯特拉（Dijkstra）算法實現的基於二維網格的最短路徑規劃。
動畫中青色點爲搜索過的節點。
A * 算法
下面是使用 A 星算法進行基於二維網格的最短路徑規劃。
動畫中青色點爲搜索過的節點。啓發算法爲二維歐幾里得距離。
勢場算法
下面是使用勢場算法進行基於二維網格的路徑規劃。
動畫中藍色的熱區圖顯示了每個格子的勢能。
相關閱讀：
機器人運動規劃：勢能函數
https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
6.3 模型預測路徑生成
下面是模型預測路徑生成的路徑優化示例。
算法用於狀態晶格規劃（state lattice planning）。路徑優化示例
查找表生成示例
相關閱讀：
用於帶輪子的機器人的最優不平整地形路徑生成
http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328
6.4 狀態晶格規劃
這個腳本使用了狀態晶格規劃（state lattice planning）實現路徑規劃。
這段代碼通過模型預測路徑生成來解決邊界問題。
相關閱讀：
用於帶輪子的機器人的最優不平整地形路徑生成
http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328
用於複雜環境下的高性能運動機器人導航的可行運動的狀態空間採樣
http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf
均勻極性採樣（Uniform polar sampling）
偏差極性採樣（Biased polar sampling）
路線採樣（Lane sampling）
6.5 隨機路徑圖（PRM）規劃
這個隨機路徑圖（Probabilistic Road-Map，PRM）規劃算法在圖搜索上採用了迪傑斯特拉方法。
動畫中的藍點爲採樣點。青色叉爲迪傑斯特拉方法搜索過的點。紅線爲 PRM 的最終路徑。
相關閱讀：
隨機路徑圖
https://en.wikipedia.org/wiki/Probabilistic_roadmap
6.6 Voronoi 路徑圖規劃
這個 Voronoi 路徑圖（Probabilistic Road-Map，PRM）規劃算法在圖搜索上採用了迪傑斯特拉方法。
動畫中的藍點爲 Voronoi 點。青色叉爲迪傑斯特拉方法搜索過的點。紅線爲 Voronoi 路徑圖的最終路徑。
相關閱讀：
機器人運動規劃
https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf
6.7 快速搜索隨機樹（RRT）
基本 RRT
這是個使用快速搜索隨機樹（Rapidly-Exploring Random Trees，RRT）的簡單路徑規劃代碼。
黑色圓爲障礙物，綠線爲搜索樹，紅叉爲開始位置和目標位置。
RRT*
這是使用 RRT * 的路徑規劃代碼。
黑色圓爲障礙物，綠線爲搜索樹，紅叉爲開始位置和目標位置。
相關閱讀：
最優運動規劃的基於增量採樣的算法
https://arxiv.org/abs/1005.0416
最優運動規劃的基於採樣的算法
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf
基於 Dubins 路徑的 RRT
爲汽車形機器人提供的使用 RRT 和 dubins 路徑規劃的路徑規劃算法。
基於 Dubins 路徑的 RRT*
爲汽車形機器人提供的使用 RRT * 和 dubins 路徑規劃的路徑規劃算法。
基於 reeds-shepp 路徑的 RRT*
爲汽車形機器人提供的使用 RRT * 和 reeds shepp 路徑規劃的路徑規劃算法。
Informed RRT*
這是使用 Informed RRT * 的路徑規劃代碼。青色橢圓爲 Informed RRT * 的啓發採樣域。
相關閱讀：
Informed RRT*：通過對可接受的橢球啓發的直接採樣實現最優的基於採樣的路徑規劃
https://arxiv.org/pdf/1404.2334.pdf
批量 Informed RRT*
這是使用批量 Informed RRT * 的路徑規劃代碼。
相關閱讀：
批量 Informed 樹（BIT*）：通過對隱含隨機幾何圖形進行啓發式搜索實現基於採樣的最優規劃
https://arxiv.org/abs/1405.5848
閉合迴路 RRT*
使用閉合迴路 RRT_（Closed loop RRT_）實現的基於車輛模型的路徑規劃。
這段代碼裏，轉向控制用的是純追跡算法（pure-pursuit algorithm）。
速度控制採用了 PID。
相關閱讀：
使用閉合迴路預測在複雜環境內實現運動規劃
http://acl.mit.edu/papers/KuwataGNC08.pdf）
應用於自動城市駕駛的實時運動規劃
http://acl.mit.edu/papers/KuwataTCST09.pdf
[1601.06326] 採用閉合迴路預測實現最優運動規劃的基於採樣的算法
https://arxiv.org/abs/1601.06326
LQR-RRT*
這是個使用 LQR-RRT * 的路徑規劃模擬。
LQR 局部規劃採用了雙重積分運動模型。
相關閱讀：
LQR-RRT*：使用自動推導擴展啓發實現最優基於採樣的運動規劃
http://lis.csail.mit.edu/pubs/perez-icra12.pdf
MahanFathi/LQR-RRTstar：LQR-RRT * 方法用於單擺相位中的隨機運動規劃
https://github.com/MahanFathi/LQR-RRTstar
6.8 三次樣條規劃
這是段三次路徑規劃的示例代碼。
這段代碼根據 x-y 的路點，利用三次樣條生成一段曲率連續的路徑。
每個點的指向角度也可以用解析的方式計算。
6.9 B 樣條規劃
這是段使用 B 樣條曲線進行規劃的例子。
輸入路點，它會利用 B 樣條生成光滑的路徑。
第一個和最後一個路點位於最後的路徑上。
相關閱讀：
B 樣條
https://en.wikipedia.org/wiki/B-spline
6.10 Eta^3 樣條路徑規劃
這是使用 Eta ^ 3 樣條曲線的路徑規劃。
相關閱讀：
eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots
https://ieeexplore.ieee.org/document/4339545/
6.11 貝濟埃路徑規劃
貝濟埃路徑規劃的示例代碼。
根據四個控制點生成貝濟埃路徑。
改變起點和終點的偏移距離，可以生成不同的貝濟埃路徑：
相關閱讀：
根據貝濟埃曲線爲自動駕駛汽車生成曲率連續的路徑
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf
6.12 五次多項式規劃
利用五次多項式進行路徑規劃。
它能根據五次多項式計算二維路徑、速度和加速度。
相關閱讀：
用於 Agv In 定位的局部路徑規劃和運動控制
http://ieeexplore.ieee.org/document/637936/
6.13 Dubins 路徑規劃
Dubins 路徑規劃的示例代碼。
相關閱讀：
Dubins 路徑
https://en.wikipedia.org/wiki/Dubins_path
6.14 Reeds Shepp 路徑規劃
Reeds Shepp 路徑規劃的示例代碼。
相關閱讀：
15.3.2 Reeds-Shepp 曲線
http://planning.cs.uiuc.edu/node822.html
用於能前進和後退的汽車的最優路徑
https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf
ghliu/pyReedsShepp：實現 Reeds Shepp 曲線
https://github.com/ghliu/pyReedsShepp
6.15 基於 LQR 的路徑規劃
爲雙重積分模型使用基於 LQR 的路徑規劃的示例代碼。
6.16 Frenet Frame 中的最優路徑
這段代碼在 Frenet Frame 中生成最優路徑。
青色線爲目標路徑，黑色叉爲障礙物。
紅色線爲預測的路徑。
相關閱讀：
Frenet Frame 中的動態接到場景中的最優路徑生成
https://www.researchgate.net/profile/Moritz_Werling/publication/224156269_Optimal_Trajectory_Generation_for_Dynamic_Street_Scenarios_in_a_Frenet_Frame/links/54f749df0cf210398e9277af.pdf
Frenet Frame 中的動態接到場景中的最優路徑生成
https://www.youtube.com/watch?v=Cj6tAQe7UCY
七、路徑跟蹤
7.1 姿勢控制跟蹤
這是姿勢控制跟蹤的模擬。
相關閱讀：
Robotics, Vision and Control - Fundamental Algorithms In MATLAB® Second, Completely Revised, Extended And Updated Edition | Peter Corke | Springer
https://www.springer.com/us/book/9783319544120
7.2 純追跡跟蹤
使用純追跡（pure pursuit）轉向控制和 PID 速度控制的路徑跟蹤模擬。
紅線爲目標路線，綠叉爲純追跡控制的目標點，藍線爲跟蹤路線。
相關閱讀：
城市中的自動駕駛汽車的運動規劃和控制技術的調查
https://arxiv.org/abs/1604.07446
7.3 史坦利控制
使用史坦利（Stanley）轉向控制和 PID 速度控制的路徑跟蹤模擬。
相關閱讀：
史坦利：贏得 DARPA 大獎賽的機器人
http://robots.stanford.edu/papers/thrun.stanley05.pdf
用於自動駕駛機動車路徑跟蹤的自動轉向方法
https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf
7.4 後輪反饋控制
利用後輪反饋轉向控制和 PID 速度控制的路徑跟蹤模擬。
相關閱讀：
城市中的自動駕駛汽車的運動規劃和控制技術的調查
https://arxiv.org/abs/1604.07446
7.5 線性二次 regulator（LQR）轉向和速度控制
使用 LQR 轉向和速度控制的路徑跟蹤模擬。
相關閱讀：
完全自動駕駛：系統和算法 - IEEE 會議出版物
http://ieeexplore.ieee.org/document/5940562/
7.6 模型預測速度和轉向控制
使用迭代線性模型預測轉向和速度控制的路徑跟蹤模擬。
這段代碼使用了 cxvxpy 作爲最優建模工具。
車輛動態和控制 | Rajesh Rajamani | Springer
http://www.springer.com/us/book/9781461414322
MPC 課程資料 - MPC Lab @ UC-Berkeley
http://www.mpc.berkeley.edu/mpc-course-material
基於 C-GMRES 的非線性模型預測控制
C-GMRES 的 NMPC 運動規劃與路徑跟蹤仿真
項目地址：
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathTracking/cgmres_nmpc/cgmres_nmpc.ipynb
臂導航
N 關節臂到點控制
N 個關節臂以一點式控制仿真。這是一個交互式模擬。可以在繪圖區域左鍵單擊，設置末端執行器的目標位置。
然而，在這個模擬中，N = 10，您可以更改它。
帶避障的手臂導航
手臂導航與避障模擬。
空中導航
無人機 3d 軌跡跟蹤
這是四旋翼飛行器的三維軌跡模擬。
火箭動力着陸
這是火箭動力着陸的三維軌跡生成模擬。
八、項目支持
可以通過 https://www.patreon.com/myenigma 對該項目進行經濟支持。
如果你在 Patreon 上支持該項目，則可以得到關於本項目代碼的郵件技術支持。
本文作者包括有
Atsushi Sakai (@Atsushi_twi)，Daniel Ingram，Joe Dinius，Karan Chawla，Antonin RAFFIN，Alexis Paques。
項目地址：
https://github.com/AtsushiSakai/PythonRobotics
本文由 Readfog 進行 AMP 轉碼，版權歸原作者所有。
來源：https://mp.weixin.qq.com/s/uutox1Ut9qbzrmROxV6ziQ