一文洞悉Python必備的50種算法

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

作者說明：

  • AtsushiSakai，日本機器人工程師，從事自動駕駛技術(shù)開發(fā)，精通C++、ROS、MATLAB、Python、Vim和Robotics。

文章說明：

  • 本文是一些機器人算法（特別是自動導(dǎo)航算法）的Python代碼合集。

特點：

  • 選擇了在實踐中廣泛應(yīng)用的算法；
  • 依賴最少；
  • 容易閱讀，容易理解每個算法的基本思想

目錄：

  • 一、環(huán)境需求
  • 二、怎樣使用
  • 三、本地化
    • 3.1 擴展卡爾曼濾波本地化
    • 3.2 無損卡爾曼濾波本地化
    • 3.3 粒子濾波本地化
    • 3.4 直方圖濾波本地化
  • 四、映射
    • 4.1 高斯網(wǎng)格映射
    • 4.2 光線投射網(wǎng)格映射
    • 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
  • 六、路徑規(guī)劃
    • 6.1 動態(tài)窗口方式
    • 6.2 基于網(wǎng)格的搜索
      • 迪杰斯特拉算法
      • A*算法
      • 勢場算法
    • 6.3 模型預(yù)測路徑生成
      • 路徑優(yōu)化示例
      • 查找表生成示例
    • 6.4 狀態(tài)晶格規(guī)劃
      • 均勻極性采樣（Uniform polar sampling）
      • 偏差極性采樣（Biased polar sampling）
      • 路線采樣（Lane sampling）
    • 6.5 隨機路徑圖（PRM）規(guī)劃
    • 6.6 Voronoi路徑圖規(guī)劃
    • 6.7 快速搜索隨機樹（RRT）
      • 基本RRT
      • RRT*
      • 基于Dubins路徑的RRT
      • 基于Dubins路徑的RRT*
      • 基于reeds-shepp路徑的RRT*
      • Informed RRT*
      • 批量Informed RRT*
      • 閉合回路RRT*
      • LQR-RRT*
    • 6.8 三次樣條規(guī)劃
    • 6.9 B樣條規(guī)劃
    • 6.10 Eta^3樣條路徑規(guī)劃
    • 6.11 貝濟埃路徑規(guī)劃
    • 6.12 五次多項式規(guī)劃
    • 6.13 Dubins路徑規(guī)劃
    • 6.14 Reeds Shepp路徑規(guī)劃
    • 6.15 基于LQR的路徑規(guī)劃
    • 6.16 Frenet Frame中的最優(yōu)路徑
  • 七、路徑跟蹤
    • 7.1 姿勢控制跟蹤
    • 7.2 純追跡跟蹤
    • 7.3 史坦利控制
    • 7.4 后輪反饋控制
    • 7.5 線性二次regulator（LQR）轉(zhuǎn)向控制
    • 7.6 線性二次regulator（LQR）轉(zhuǎn)向和速度控制
    • 7.7 模型預(yù)測速度和轉(zhuǎn)向控制
  • 八、項目支持

一、環(huán)境需求：

  • Python 3.8.x
  • numpy
  • scipy
  • matplotlib
  • pandas
  • cvxpy

二、怎樣使用

三、本地化

3.1 擴展卡爾曼濾波本地化
3.2 無損卡爾曼濾波本地化
3.3 粒子濾波本地化

  • 圖解：


    粒子濾波本地化

  • 算法說明：

    • 該算法利用粒子濾波器（Particle Filter, PF）實現(xiàn)傳感器混合本地化。
    • 藍線為真實路徑，黑線為導(dǎo)航推測路徑（dead reckoning trajectory），綠點為位置觀測（如GPS），紅線為PF估算的路徑。
    • 該算法假設(shè)機器人能夠測量與地標（RFID）之間的距離。
    • PF本地化會用到該測量結(jié)果。、

  • 推薦相關(guān)閱讀：概率機器人學：http://www.probabilistic-robotics.org/

3.4 直方圖濾波本地化

  • 圖解：


    直方圖濾波本地化

https://github.com/AtsushiSakai/PythonRoboticsGifs/raw/master/Localization/histogram_filter/animation.gif

  • 算法說明：

    • 該算法是利用直方圖濾波器（Histogram filter）實現(xiàn)二維本地化的例子。
    • 紅十字是實際位置，黑點是RFID的位置。
    • 藍色格子是直方圖濾波器的概率位置。
    • 在該模擬中，x，y是未知數(shù)，yaw已知。
    • 濾波器整合了速度輸入和從RFID獲得距離觀測數(shù)據(jù)進行本地化。
    • 不需要初始位置。

  • 推薦相關(guān)閱讀：概率機器人學：http://www.probabilistic-robotics.org/

四、映射

4.1 高斯網(wǎng)格映射

  • 圖解：


    高斯網(wǎng)格映射
  • 算法說明：本算法是二維高斯網(wǎng)格映射（Gaussian grid mapping）的例子。
4.2 光線投射網(wǎng)格映射

  • 圖解：
    光線投射網(wǎng)格映射
  • 算法說明：本算法是二維光線投射網(wǎng)格映射（Ray casting grid map）的例子。
4.3 k均值物體聚類

  • 圖解：


    k均值物體聚類

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

4.4 圓形擬合物體形狀識別

  • 圖解：
    圓形擬合物體形狀識別

  • 圖形說明：

    • 藍圈是實際的物體形狀。

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

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

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

五、SLAM

5.1 迭代最近點匹配

  • 圖解：
    迭代最近點匹配

  • 算法說明：

    • 本算法是使用單值解構(gòu)進行二維迭代最近點（Iterative Closest Point，ICP）匹配的例子。
    • 它能計算從一些點到另一些點的旋轉(zhuǎn)矩陣和平移矩陣。

  • 推薦相關(guān)閱讀：機器人運動介紹：迭代最近點算法
    https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf

5.2 EKF SLAM

  • 圖解：


    EKF SLAM

  • 算法說明：

    • 這是基于擴展卡爾曼濾波的SLAM示例。
    • 藍線是真實路徑，黑線是導(dǎo)航推測路徑，紅線是EKF SLAM估計的路徑。
    • 綠叉是估計的地標。

  • 推薦相關(guān)閱讀：概率機器人學：http://www.probabilistic-robotics.org/

5.3 FastSLAM 1.0

  • 圖解：


    FastSLAM 1.0

  • 算法說明：

    • 這是用FastSLAM 1.0進行基于特征的SLAM的示例。
    • 藍線是實際路徑，黑線是導(dǎo)航推測，紅線是FastSLAM的推測路徑。
    • 紅點是FastSLAM中的粒子。
    • 黑點是地標，藍叉是FastLSAM估算的地標位置。

  • 推薦相關(guān)閱讀：概率機器人學：http://www.probabilistic-robotics.org/

5.4 FastSLAM 2.0
5.5 基于圖的SLAM

  • 圖解：


    基于圖的SLAM

  • 算法說明：

    • 這是基于圖的SLAM的示例。
    • 藍線是實際路徑。
    • 黑線是導(dǎo)航推測路徑。
    • 紅線是基于圖的SLAM估算的路徑。
    • 黑星是地標，用于生成圖的邊。

  • 推薦相關(guān)閱讀：基于圖的SLAM入門：http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf

六、路徑規(guī)劃

6.1 動態(tài)窗口方式
6.2 基于網(wǎng)格的搜索

  • 迪杰斯特拉算法

    • 圖解：


      迪杰斯特拉算法

    • 算法說明：

    • 這是利用迪杰斯特拉（Dijkstra）算法實現(xiàn)的基于二維網(wǎng)格的最短路徑規(guī)劃。

    • 動畫中青色點為搜索過的節(jié)點。

  • A*算法

    • 圖解：


      A*算法

    • 算法說明：

      • 使用A星算法進行基于二維網(wǎng)格的最短路徑規(guī)劃。
      • 動畫中青色點為搜索過的節(jié)點。
      • 啟發(fā)算法為二維歐幾里得距離。

  • 勢場算法

6.3 模型預(yù)測路徑生成
  • 路徑優(yōu)化示例

    • 圖解：


      路徑優(yōu)化示例

  • 算法說明：算法用于狀態(tài)晶格規(guī)劃（state lattice planning）

  • 查找表生成示例

6.4 狀態(tài)晶格規(guī)劃

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

    • 圖解：


      均勻極性采樣

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

    • 圖解：


      偏差極性采樣

  • 路線采樣（Lane sampling）

    • 圖解：


      路線采樣
6.5 隨機路徑圖（PRM）規(guī)劃

  • 圖解：


    隨機路徑圖（PRM）規(guī)劃

  • 算法說明：

    • 這個隨機路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。
    • 動畫中的藍點為采樣點。
    • 青色叉為迪杰斯特拉方法搜索過的點。
    • 紅線為PRM的最終路徑。

  • 推薦相關(guān)閱讀：隨機路徑圖：https://en.wikipedia.org/wiki/Probabilistic_roadmap

6.6 Voronoi路徑圖規(guī)劃

  • 圖解：


    Voronoi路徑圖規(guī)劃

  • 算法說明：

    • 這個Voronoi路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。
    • 動畫中的藍點為Voronoi點。
    • 青色叉為迪杰斯特拉方法搜索過的點。
    • 紅線為Voronoi路徑圖的最終路徑。

  • 推薦相關(guān)閱讀：機器人運動規(guī)劃：https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf

6.7 快速搜索隨機樹（RRT）

  • 基本RRT

    • 圖解：


      基本RRT

    • 算法說明：

      • 這是個使用快速搜索隨機樹（Rapidly-Exploring Random Trees，RRT）的簡單路徑規(guī)劃代碼。
      • 黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標位置。

  • RRT*

  • 基于Dubins路徑的RRT

    • 圖解：


      基于Dubins路徑的RRT

    • 算法說明：為汽車形機器人提供的使用RRT和dubins路徑規(guī)劃的路徑規(guī)劃算法。

  • 基于Dubins路徑的RRT*

    • 圖解：


      基于Dubins路徑的RRT*

    • 算法說明：為汽車形機器人提供的使用RRT*和dubins路徑規(guī)劃的路徑規(guī)劃算法。

  • 基于reeds-shepp路徑的RRT*

  • Informed RRT*

    • 圖解：


      Informed RRT*

    • 算法說明：

      • 這是使用Informed RRT*的路徑規(guī)劃代碼。
      • 青色橢圓為Informed RRT*的啟發(fā)采樣域。

    • 推薦相關(guān)閱讀：

  • 批量Informed RRT*

    • 圖解：


      批量Informed RRT*

    • 算法說明：這是使用批量Informed RRT*的路徑規(guī)劃代碼。

    • 推薦相關(guān)閱讀：

      • 批量Informed樹（BIT*）：通過對隱含隨機幾何圖形進行啟發(fā)式搜索實現(xiàn)基于采樣的最優(yōu)規(guī)劃
        https://arxiv.org/abs/1405.5848

  • 閉合回路RRT*

    • 圖解：


      閉合回路RRT*

    • 算法說明：

      • 這段代碼里，轉(zhuǎn)向控制用的是純追跡算法（pure-pursuit algorithm）。
      • 速度控制采用了PID。

    • 推薦相關(guān)閱讀：

  • LQR-RRT*

    • 圖解：


      LQR-RRT*

    • 算法說明：

      • 這是個使用LQR-RRT*的路徑規(guī)劃模擬。
      • LQR局部規(guī)劃采用了雙重積分運動模型。

    • 推薦相關(guān)閱讀：

6.8 三次樣條規(guī)劃

  • 圖解：


    第一階段
第二階段
第三階段
  • 算法說明：
    • 這是段三次路徑規(guī)劃的示例代碼。
    • 這段代碼根據(jù)x-y的路點，利用三次樣條生成一段曲率連續(xù)的路徑。
    • 每個點的指向角度也可以用解析的方式計算。
  • 推薦相關(guān)閱讀：
6.9 B樣條規(guī)劃

  • 圖解：


    B樣條規(guī)劃

  • 算法說明：

    • 這是段使用B樣條曲線進行規(guī)劃的例子。
    • 輸入路點，它會利用B樣條生成光滑的路徑。
    • 第一個和最后一個路點位于最后的路徑上。

  • 推薦相關(guān)閱讀：B樣條
    https://en.wikipedia.org/wiki/B-spline

6.10 Eta^3樣條路徑規(guī)劃

  • 圖解：


    Eta^3樣條路徑規(guī)劃

  • 算法說明：這是使用Eta ^ 3樣條曲線的路徑規(guī)劃。

  • 推薦相關(guān)閱讀：

6.11 貝濟埃路徑規(guī)劃

  • 圖解：


    第一階段
第二階段
6.12 五次多項式規(guī)劃

  • 圖解：


    五次多項式規(guī)劃

  • 算法說明：它能根據(jù)五次多項式計算二維路徑、速度和加速度。

6.13 Dubins路徑規(guī)劃
6.14 Reeds Shepp路徑規(guī)劃
6.15 基于LQR的路徑規(guī)劃

  • 圖解：


    基于LQR的路徑規(guī)劃

  • 算法說明：為雙重積分模型使用基于LQR的路徑規(guī)劃的示例代碼。

6.16 Frenet Frame中的最優(yōu)路徑

七、路徑跟蹤

7.1 姿勢控制跟蹤

  • 圖解：


    姿勢控制跟蹤

  • 算法說明：這是姿勢控制跟蹤的模擬。

  • 推薦相關(guān)閱讀：

7.2 純追跡跟蹤

  • 圖解：


    純追跡跟蹤
  • 算法說明：
    • 使用純追跡（pure pursuit）轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
    • 紅線為目標路線，綠叉為純追跡控制的目標點，藍線為跟蹤路線。
  • 推薦相關(guān)閱讀：
7.3 史坦利控制
7.4 后輪反饋控制

  • 圖解：


    后輪反饋控制

  • 算法說明：利用后輪反饋轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。

  • 推薦相關(guān)閱讀：

7.5 線性二次regulator（LQR）轉(zhuǎn)向控制

  • 圖解：


    線性二次regulator（LQR）轉(zhuǎn)向控制

  • 算法說明：使用LQR轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。

  • 推薦相關(guān)閱讀：

7.6 線性二次regulator（LQR）轉(zhuǎn)向和速度控制

  • 圖解：


    線性二次regulator（LQR）轉(zhuǎn)向和速度控制

  • 算法說明：使用LQR轉(zhuǎn)向和速度控制的路徑跟蹤模擬。

  • 推薦相關(guān)閱讀：

7.7 模型預(yù)測速度和轉(zhuǎn)向控制

八、項目支持

?著作權(quán)歸作者所有,轉(zhuǎn)載或內(nèi)容合作請聯(lián)系作者
【社區(qū)內(nèi)容提示】社區(qū)部分內(nèi)容疑似由AI輔助生成，瀏覽時請結(jié)合常識與多方信息審慎甄別。
平臺聲明：文章內(nèi)容（如有圖片或視頻亦包括在內(nèi)）由作者上傳并發(fā)布，文章內(nèi)容僅代表作者本人觀點，簡書系信息發(fā)布平臺，僅提供信息存儲服務(wù)。

友情鏈接更多精彩內(nèi)容