一文洞悉 Python 必備 50 種演演算法（附解析）

本文經授權轉自公眾號CSDN（ID：CSDNnews）

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

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

譯者：彎月，責編：郭芮

 

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

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

前排友情提示，文章較長，建議收藏後再看，專案地址在文末，記得點個“好看”哦。

目錄

一、環境需求

二、怎樣使用

三、本地化

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

    3.2 無損卡爾曼濾波本地化

    3.3 粒子濾波本地化

    3.4 直方圖濾波本地化

四、對映

    4.1 高斯網格對映

    4.2 光線投射網格對映

    4.3 k均值物體聚類

    4.4 圓形擬合物體形狀識別

五、SLAM

    5.1 迭代最近點匹配

    5.2 EKF 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

二、怎樣使用

1. 安裝必要的庫；

2. 克隆本程式碼倉庫；

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

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

三、本地化

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轉向控制和PID速度控制的路徑跟蹤模擬。

相關閱讀：

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

https://github.com/ApolloAuto/apollo

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

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

相關閱讀：

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

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

7.7 模型預測速度和轉向控制

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

這段程式碼使用了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://github.com/AtsushiSakai/PythonRobotics