這可能是史上最全的Python演算法集！

來源 | CSDN（ID：CSDNnews ）

本文是一些機器人演算法（特別是自動導航演算法）的Python程式碼合集。

其主要特點有以下三點：選擇了在實踐中廣泛應用的演算法；依賴最少；容易閱讀，容易理解每個演算法的基本思想。希望閱讀本文後能對你有所幫助。

前排友情提示，文章較長，建議收藏後再看。

目錄

環境需求

怎樣使用

本地化

• 擴充套件卡爾曼濾波本地化

• 無損卡爾曼濾波本地化

• 粒子濾波本地化

• 直方圖濾波本地化

對映

• 高斯網格對映

• 光線投射網格對映

• k均值物體聚類

• 圓形擬合物體形狀識別

SLAM

• 迭代最近點匹配

• EKF SLAM

• FastSLAM 1.0

• FastSLAM 2.0

• 基於圖的SLAM

路徑規劃

• 動態視窗方式

• 基於網格的搜尋

• 迪傑斯特拉演算法

• A*演算法

• 勢場演算法

• 模型預測路徑生成

• 路徑優化示例

• 查詢表生成示例

• 狀態晶格規劃

• 均勻極性取樣（Uniform polar sampling）

• 偏差極性取樣（Biased polar sampling）

• 路線取樣（Lane sampling）

• 隨機路徑圖（PRM）規劃

• Voronoi路徑圖規劃

• 快速搜尋隨機樹（RRT）

• 基本RRT

• RRT*

• 基於Dubins路徑的RRT

• 基於Dubins路徑的RRT*

• 基於reeds-shepp路徑的RRT*

• Informed RRT*

• 批量Informed RRT*

• 三次樣條規劃

• B樣條規劃

• 貝濟埃路徑規劃

• 五次多項式規劃

• Dubins路徑規劃

• Reeds Shepp路徑規劃

• 基於LQR的路徑規劃

• Frenet Frame中的最優路徑

路徑跟蹤

• 純追跡跟蹤

• 史坦利控制

• 後輪反饋控制

• 線性二次regulator（LQR）轉向控制

• 線性二次regulator（LQR）轉向和速度控制

專案支援

環境需求

• Python 3.6.x

• numpy

• scipy

• matplotlib

• pandas

• cvxpy 0.4.x

怎樣使用

1. 安裝必要的庫；

2. 克隆本程式碼倉庫；

3. 執行每個目錄下的python指令碼；

4. 如果你喜歡，則收藏本程式碼庫：）

本地化

擴充套件卡爾曼濾波本地化

該演算法利用擴充套件卡爾曼濾波器（Extended Kalman Filter, EKF）實現感測器混合本地化。

藍線為真實路徑，黑線為導航推測路徑（dead reckoning trajectory），綠點為位置觀測（如GPS），紅線為EKF估算的路徑。

紅色橢圓為EKF估算的協方差。

相關閱讀：

概率機器人學

http://www.probabilistic-robotics.org/

無損卡爾曼濾波本地化

該演算法利用無損卡爾曼濾波器（Unscented Kalman Filter, UKF）實現感測器混合本地化。

線和點的含義與EKF模擬的例子相同。

相關閱讀：

利用無差別訓練過的無損卡爾曼濾波進行機器人移動本地化

https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization

粒子濾波本地化

該演算法利用粒子濾波器（Particle Filter, PF）實現感測器混合本地化。

藍線為真實路徑，黑線為導航推測路徑（dead reckoning trajectory），綠點為位置觀測（如GPS），紅線為PF估算的路徑。

該演算法假設機器人能夠測量與地標（RFID）之間的距離。

PF本地化會用到該測量結果。

相關閱讀：

概率機器人學

http://www.probabilistic-robotics.org/

直方圖濾波本地化

該演算法是利用直方圖濾波器（Histogram filter）實現二維本地化的例子。

紅十字是實際位置，黑點是RFID的位置。

藍色格子是直方圖濾波器的概率位置。

在該模擬中，x，y是未知數，yaw已知。

濾波器整合了速度輸入和從RFID獲得距離觀測資料進行本地化。

不需要初始位置。

相關閱讀：

概率機器人學

http://www.probabilistic-robotics.org/

對映

高斯網格對映

本演算法是二維高斯網格對映（Gaussian grid mapping）的例子。

光線投射網格對映

本演算法是二維光線投射網格對映（Ray casting grid map）的例子。

k均值物體聚類

本演算法是使用k均值演算法進行二維物體聚類的例子。

圓形擬合物體形狀識別

本演算法是使用圓形擬合進行物體形狀識別的例子。

藍圈是實際的物體形狀。

紅叉是通過距離感測器觀測到的點。

紅圈是使用圓形擬合估計的物體形狀。

SLAM

同時本地化和對映（Simultaneous Localization and Mapping，SLAM）的例子。

迭代最近點匹配

本演算法是使用單值解構進行二維迭代最近點（Iterative Closest Point，ICP）匹配的例子。

它能計算從一些點到另一些點的旋轉矩陣和平移矩陣。

相關閱讀：

機器人運動介紹：迭代最近點演算法

https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf

EKF SLAM

這是基於擴充套件卡爾曼濾波的SLAM示例。

藍線是真實路徑，黑線是導航推測路徑，紅線是EKF SLAM估計的路徑。

綠叉是估計的地標。

相關閱讀：

概率機器人學

http://www.probabilistic-robotics.org/

FastSLAM 1.0

這是用FastSLAM 1.0進行基於特徵的SLAM的示例。

藍線是實際路徑，黑線是導航推測，紅線是FastSLAM的推測路徑。

紅點是FastSLAM中的粒子。

黑點是地標，藍叉是FastLSAM估算的地標位置。

相關閱讀：

概率機器人學

http://www.probabilistic-robotics.org/

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

基於圖的SLAM

這是基於圖的SLAM的示例。

藍線是實際路徑。

黑線是導航推測路徑。

紅線是基於圖的SLAM估算的路徑。

黑星是地標，用於生成圖的邊。

相關閱讀：

基於圖的SLAM入門

http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf

路徑規劃

動態視窗方式

這是使用動態視窗方式（Dynamic Window Approach）進行二維導航的示例程式碼。

相關閱讀：

用動態視窗方式避免碰撞

https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf

基於網格的搜尋

迪傑斯特拉演算法

這是利用迪傑斯特拉（Dijkstra）演算法實現的基於二維網格的最短路徑規劃。

動畫中青色點為搜尋過的節點。

A*演算法

下面是使用A星演算法進行基於二維網格的最短路徑規劃。

動畫中青色點為搜尋過的節點。

啟發演算法為二維歐幾里得距離。

勢場演算法

下面是使用勢場演算法進行基於二維網格的路徑規劃。

動畫中藍色的熱區圖顯示了每個格子的勢能。

相關閱讀：

機器人運動規劃：勢能函式

https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf

模型預測路徑生成

下面是模型預測路徑生成的路徑優化示例。

演算法用於狀態晶格規劃（state lattice planning）。

路徑優化示例

查詢表生成示例

相關閱讀：

用於帶輪子的機器人的最優不平整地形路徑生成

http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328

狀態晶格規劃

這個指令碼使用了狀態晶格規劃（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）

隨機路徑圖（PRM）規劃

這個隨機路徑圖（Probabilistic Road-Map，PRM）規劃演算法在圖搜尋上採用了迪傑斯特拉方法。

動畫中的藍點為取樣點。

青色叉為迪傑斯特拉方法搜尋過的點。

紅線為PRM的最終路徑。

相關閱讀：

隨機路徑圖

https://en.wikipedia.org/wiki/Probabilistic_roadmap

Voronoi路徑圖規劃

這個Voronoi路徑圖（Probabilistic Road-Map，PRM）規劃演算法在圖搜尋上採用了迪傑斯特拉方法。

動畫中的藍點為Voronoi點。

青色叉為迪傑斯特拉方法搜尋過的點。

紅線為Voronoi路徑圖的最終路徑。

相關閱讀：

機器人運動規劃

https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf

快速搜尋隨機樹（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

三次樣條規劃

這是段三次路徑規劃的示例程式碼。

這段程式碼根據x-y的路點，利用三次樣條生成一段曲率連續的路徑。

每個點的指向角度也可以用解析的方式計算。

B樣條規劃

這是段使用B樣條曲線進行規劃的例子。

輸入路點，它會利用B樣條生成光滑的路徑。

第一個和最後一個路點位於最後的路徑上。

相關閱讀：

B樣條

https://en.wikipedia.org/wiki/B-spline

Eta^3樣條路徑規劃

這是使用Eta ^ 3樣條曲線的路徑規劃。

相關閱讀：

\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots

https://ieeexplore.ieee.org/document/4339545/

貝濟埃路徑規劃

貝濟埃路徑規劃的示例程式碼。

根據四個控制點生成貝濟埃路徑。

改變起點和終點的偏移距離，可以生成不同的貝濟埃路徑：

相關閱讀：

根據貝濟埃曲線為自動駕駛汽車生成曲率連續的路徑

http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf

五次多項式規劃

利用五次多項式進行路徑規劃。

它能根據五次多項式計算二維路徑、速度和加速度。

相關閱讀：

用於Agv In定位的區域性路徑規劃和運動控制

http://ieeexplore.ieee.org/document/637936/

Dubins路徑規劃

Dubins路徑規劃的示例程式碼。

相關閱讀：

Dubins路徑

https://en.wikipedia.org/wiki/Dubins_path

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

基於LQR的路徑規劃

為雙重積分模型使用基於LQR的路徑規劃的示例程式碼。

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

路徑跟蹤

姿勢控制跟蹤

這是姿勢控制跟蹤的模擬。

相關閱讀：

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

純追跡跟蹤

使用純追跡（pure pursuit）轉向控制和PID速度控制的路徑跟蹤模擬。

紅線為目標路線，綠叉為純追跡控制的目標點，藍線為跟蹤路線。

相關閱讀：

城市中的自動駕駛汽車的運動規劃和控制技術的調查

https://arxiv.org/abs/1604.07446

史坦利控制

使用史坦利（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

後輪反饋控制

利用後輪反饋轉向控制和PID速度控制的路徑跟蹤模擬。

相關閱讀：

城市中的自動駕駛汽車的運動規劃和控制技術的調查

https://arxiv.org/abs/1604.07446

線性二次regulator（LQR）轉向控制

使用LQR轉向控制和PID速度控制的路徑跟蹤模擬。

相關閱讀：

ApolloAuto/apollo：開源自動駕駛平臺

https://github.com/ApolloAuto/apollo

線性二次regulator（LQR）轉向和速度控制

使用LQR轉向和速度控制的路徑跟蹤模擬。

相關閱讀：

完全自動駕駛：系統和演算法 - IEEE會議出版物

http://ieeexplore.ieee.org/document/5940562/

模型預測速度和轉向控制

使用迭代線性模型預測轉向和速度控制的路徑跟蹤模擬。

這段程式碼使用了cxvxpy作為最優建模工具。

相關閱讀：

車輛動態和控制 | Rajesh Rajamani | Springer 

http://www.springer.com/us/book/9781461414322

MPC課程資料 - MPC Lab @ UC-Berkeley 

http://www.mpc.berkeley.edu/mpc-course-material

專案支援

可以通過Patreon（https://www.patreon.com/myenigma）對該專案進行經濟支援。

如果你在Patreon上支援該專案，則可以得到關於本專案程式碼的郵件技術支援。

本文作者包括有Atsushi Sakai (@Atsushi_twi)，Daniel Ingram，Joe Dinius，Karan Chawla，Antonin RAFFIN，Alexis Paques。

原文：

https://atsushisakai.github.io/PythonRobotics/#what-is-this

作者：AtsushiSakai，日本機器人工程師，從事自動駕駛技術開發，精通C++、ROS、MATLAB、Python、Vim和Robotics。

譯者：彎月，責編：郭芮

（本文為AI科技大本營轉載文章，轉載請聯絡原作者）

精彩推薦

推薦閱讀：

• “安利”一款debug神器：在AI面前，bug都不是事兒

• 一鍵免費自動AI摳圖，效果連PS大哥也點贊！

• 這可能是史上最全的Python演算法集！

• Python之父重回決策層，未來如何發展？

• 華為立 Flag：一年超越三星做全球智慧手機老大！

• 那些簡歷造假拿 Offer 的程式設計師，後來都怎麼樣了？

• 被V神點贊, 我是如何用五子棋打敗以太坊排名最高的應用的？ |人物誌

• 50個最有價值的資料視覺化圖表（推薦收藏）

• 2月報告：Python逆襲成功？踢館Java，碾壓C++！

                         

點選“閱讀原文”，檢視歷史精彩文章。