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

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

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

目錄

環(huán)境需求

怎樣使用

本地化

擴展卡爾曼濾波本地化

無損卡爾曼濾波本地化

粒子濾波本地化

直方圖濾波本地化

映射

高斯網格映射

光線投射網格映射

k均值物體聚類

圓形擬合物體形狀識別

SLAM

迭代最近點匹配

EKF SLAM

FastSLAM 1.0

FastSLAM 2.0

基于圖的SLAM

路徑規(guī)劃

動態(tài)窗口方式

基于網格的搜索

迪杰斯特拉算法

A*算法

勢場算法

模型預測路徑生成

路徑優(yōu)化示例

查找表生成示例

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

均勻極性采樣（Uniform polar sampling）

偏差極性采樣（Biased polar sampling）

路線采樣（Lane sampling）

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

Voronoi路徑圖規(guī)劃

快速搜索隨機樹（RRT）

基本RRT

RRT*

基于Dubins路徑的RRT

基于Dubins路徑的RRT*

基于reeds-shepp路徑的RRT*

Informed RRT*

批量Informed RRT*

三次樣條規(guī)劃

B樣條規(guī)劃

貝濟埃路徑規(guī)劃

五次多項式規(guī)劃

Dubins路徑規(guī)劃

Reeds Shepp路徑規(guī)劃

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

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

路徑跟蹤

純追跡跟蹤

史坦利控制

后輪反饋控制

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

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

項目支持

環(huán)境需求

Python 3.6.x

numpy

scipy

matplotlib

pandas

cvxpy 0.4.x

怎樣使用

安裝必要的庫；

克隆本代碼倉庫；

執(zhí)行每個目錄下的python腳本；

如果你喜歡，則收藏本代碼庫：）

本地化

擴展卡爾曼濾波本地化

該算法利用擴展卡爾曼濾波器（Extended Kalman Filter, EKF）實現(xiàn)傳感器混合本地化。

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

紅色橢圓為EKF估算的協(xié)方差。

相關閱讀：

概率機器人學

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

無損卡爾曼濾波本地化

該算法利用無損卡爾曼濾波器（Unscented Kalman Filter, UKF）實現(xiàn)傳感器混合本地化。

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

相關閱讀：

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

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

粒子濾波本地化

該算法利用粒子濾波器（Particle Filter, PF）實現(xiàn)傳感器混合本地化。

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

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

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

相關閱讀：

概率機器人學

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

直方圖濾波本地化

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

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

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

在該模擬中，x，y是未知數(shù)，yaw已知。

濾波器整合了速度輸入和從RFID獲得距離觀測數(shù)據(jù)進行本地化。

不需要初始位置。

相關閱讀：

概率機器人學

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

路徑規(guī)劃

動態(tài)窗口方式

這是使用動態(tài)窗口方式（Dynamic Window Approach）進行二維導航的示例代碼。

相關閱讀：

用動態(tài)窗口方式避免碰撞

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

基于網格的搜索

迪杰斯特拉算法

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

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

A*算法

下面是使用A星算法進行基于二維網格的最短路徑規(guī)劃。

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

啟發(fā)算法為二維歐幾里得距離。

勢場算法

下面是使用勢場算法進行基于二維網格的路徑規(guī)劃。

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

相關閱讀：

機器人運動規(guī)劃：勢能函數(shù)

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

模型預測路徑生成

下面是模型預測路徑生成的路徑優(yōu)化示例。

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

路徑優(yōu)化示例

查找表生成示例

相關閱讀：

用于帶輪子的機器人的最優(yōu)不平整地形路徑生成

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

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

這個腳本使用了狀態(tài)晶格規(guī)劃（state lattice planning）實現(xiàn)路徑規(guī)劃。

這段代碼通過模型預測路徑生成來解決邊界問題。

相關閱讀：

用于帶輪子的機器人的最優(yōu)不平整地形路徑生成

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

用于復雜環(huán)境下的高性能運動機器人導航的可行運動的狀態(tài)空間采樣

http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf

均勻極性采樣（Uniform polar sampling）

偏差極性采樣（Biased polar sampling）

路線采樣（Lane sampling）

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

這個隨機路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。

動畫中的藍點為采樣點。

青色叉為迪杰斯特拉方法搜索過的點。

紅線為PRM的最終路徑。

相關閱讀：

隨機路徑圖

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

Voronoi路徑圖規(guī)劃

這個Voronoi路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。

動畫中的藍點為Voronoi點。

青色叉為迪杰斯特拉方法搜索過的點。

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

相關閱讀：

機器人運動規(guī)劃

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

快速搜索隨機樹（RRT）

基本RRT

這是個使用快速搜索隨機樹（Rapidly-Exploring Random Trees，RRT）的簡單路徑規(guī)劃代碼。

黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標位置。

RRT*

這是使用RRT*的路徑規(guī)劃代碼。

黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標位置。

相關閱讀：

最優(yōu)運動規(guī)劃的基于增量采樣的算法

https://arxiv.org/abs/1005.0416

最優(yōu)運動規(guī)劃的基于采樣的算法

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

基于Dubins路徑的RRT

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

基于Dubins路徑的RRT*

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

基于reeds-shepp路徑的RRT*

為汽車形機器人提供的使用RRT*和reeds shepp路徑規(guī)劃的路徑規(guī)劃算法。

Informed RRT*

這是使用Informed RRT*的路徑規(guī)劃代碼。

青色橢圓為Informed RRT*的啟發(fā)采樣域。

相關閱讀：

Informed RRT*：通過對可接受的橢球啟發(fā)的直接采樣實現(xiàn)最優(yōu)的基于采樣的路徑規(guī)劃

https://arxiv.org/pdf/1404.2334.pdf

批量Informed RRT*

這是使用批量Informed RRT*的路徑規(guī)劃代碼。

相關閱讀：

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

https://arxiv.org/abs/1405.5848

閉合回路RRT*

使用閉合回路RRT*（Closed loop RRT*）實現(xiàn)的基于車輛模型的路徑規(guī)劃。

這段代碼里，轉向控制用的是純追跡算法（pure-pursuit algorithm）。

速度控制采用了PID。

相關閱讀：

使用閉合回路預測在復雜環(huán)境內實現(xiàn)運動規(guī)劃

http://acl.mit.edu/papers/KuwataGNC08.pdf）

應用于自動城市駕駛的實時運動規(guī)劃

http://acl.mit.edu/papers/KuwataTCST09.pdf

[1601.06326]采用閉合回路預測實現(xiàn)最優(yōu)運動規(guī)劃的基于采樣的算法

https://arxiv.org/abs/1601.06326

LQR-RRT*

這是個使用LQR-RRT*的路徑規(guī)劃模擬。

LQR局部規(guī)劃采用了雙重積分運動模型。

相關閱讀：

LQR-RRT*：使用自動推導擴展啟發(fā)實現(xiàn)最優(yōu)基于采樣的運動規(guī)劃

http://lis.csail.mit.edu/pubs/perez-icra12.pdf

MahanFathi/LQR-RRTstar：LQR-RRT*方法用于單擺相位中的隨機運動規(guī)劃

https://github.com/MahanFathi/LQR-RRTstar

三次樣條規(guī)劃

這是段三次路徑規(guī)劃的示例代碼。

這段代碼根據(jù)x-y的路點，利用三次樣條生成一段曲率連續(xù)的路徑。

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

B樣條規(guī)劃

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

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

第一個和最后一個路點位于最后的路徑上。

相關閱讀：

B樣條

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

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

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

相關閱讀：

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

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

貝濟埃路徑規(guī)劃

貝濟埃路徑規(guī)劃的示例代碼。

根據(jù)四個控制點生成貝濟埃路徑。

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

相關閱讀：

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

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

五次多項式規(guī)劃

利用五次多項式進行路徑規(guī)劃。

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

相關閱讀：

用于Agv In定位的局部路徑規(guī)劃和運動控制

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

Dubins路徑規(guī)劃

Dubins路徑規(guī)劃的示例代碼。

相關閱讀：

Dubins路徑

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

Reeds Shepp路徑規(guī)劃

Reeds Shepp路徑規(guī)劃的示例代碼。

相關閱讀：

15.3.2 Reeds-Shepp曲線

http://planning.cs.uiuc.edu/node822.html

用于能前進和后退的汽車的最優(yōu)路徑

https://pdfs.semanticscholar.org/932e/c495b1d0018fd59dee12a0bf74434fac7af4.pdf

ghliu/pyReedsShepp：實現(xiàn)Reeds Shepp曲線

https://github.com/ghliu/pyReedsShepp

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

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

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

這段代碼在Frenet Frame中生成最優(yōu)路徑。

青色線為目標路徑，黑色叉為障礙物。

紅色線為預測的路徑。

相關閱讀：

Frenet Frame中的動態(tài)接到場景中的最優(yōu)路徑生成

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中的動態(tài)接到場景中的最優(yōu)路徑生成

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

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

相關閱讀：

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

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

相關閱讀：

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

https://arxiv.org/abs/1604.07446

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

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

相關閱讀：

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

https://github.com/ApolloAuto/apollo

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

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

相關閱讀：

完全自動駕駛：系統(tǒng)和算法 - IEEE會議出版物

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

模型預測速度和轉向控制

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

這段代碼使用了cxvxpy作為最優(yōu)建模工具。