這可能是史上最全的Python算法集！
來源 | CSDN（ID：CSDNnews ）
本文是一些機(jī)器人算法（特別是自動導(dǎo)航算法）的Python代碼合集。
其主要特點有以下三點：選擇了在實踐中廣泛應(yīng)用的算法；依賴最少；容易閱讀，容易理解每個算法的基本思想。希望閱讀本文后能對你有所幫助。
前排友情提示，文章較長，建議收藏后再看。
環(huán)境需求
Python 3.6.x
numpy
scipy
matplotlib
pandas
cvxpy 0.4.x
怎樣使用
安裝必要的庫；
克隆本代碼倉庫；
執(zhí)行每個目錄下的python腳本；
如果你喜歡，則收藏本代碼庫；
本地化
擴(kuò)展卡爾曼濾波本地化
該算法利用擴(kuò)展卡爾曼濾波器（Extended Kalman Filter, EKF）實現(xiàn)傳感器混合本地化。
藍(lán)線為真實路徑，黑線為導(dǎo)航推測路徑（dead reckoning trajectory），綠點為位置觀測（如GPS），紅線為EKF估算的路徑。
紅色橢圓為EKF估算的協(xié)方差。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
無損卡爾曼濾波本地化
該算法利用無損卡爾曼濾波器（Unscented Kalman Filter, UKF）實現(xiàn)傳感器混合本地化。
線和點的含義與EKF模擬的例子相同。
相關(guān)閱讀：
利用無差別訓(xùn)練過的無損卡爾曼濾波進(jìn)行機(jī)器人移動本地化
https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization
粒子濾波本地化
該算法利用粒子濾波器（Particle Filter, PF）實現(xiàn)傳感器混合本地化。
藍(lán)線為真實路徑，黑線為導(dǎo)航推測路徑（dead reckoning trajectory），綠點為位置觀測（如GPS），紅線為PF估算的路徑。
該算法假設(shè)機(jī)器人能夠測量與地標(biāo)（RFID）之間的距離。
PF本地化會用到該測量結(jié)果。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
直方圖濾波本地化
該算法是利用直方圖濾波器（Histogram filter）實現(xiàn)二維本地化的例子。
紅十字是實際位置，黑點是RFID的位置。
藍(lán)色格子是直方圖濾波器的概率位置。
在該模擬中，x，y是未知數(shù)，yaw已知。
濾波器整合了速度輸入和從RFID獲得距離觀測數(shù)據(jù)進(jìn)行本地化。
不需要初始位置。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
映射
高斯網(wǎng)格映射
本算法是二維高斯網(wǎng)格映射（Gaussian grid mapping）的例子。
光線投射網(wǎng)格映射
本算法是二維光線投射網(wǎng)格映射（Ray casting grid map）的例子。
k均值物體聚類
本算法是使用k均值算法進(jìn)行二維物體聚類的例子。
圓形擬合物體形狀識別
本算法是使用圓形擬合進(jìn)行物體形狀識別的例子。
藍(lán)圈是實際的物體形狀。
紅叉是通過距離傳感器觀測到的點。
紅圈是使用圓形擬合估計的物體形狀。
SLAM
同時本地化和映射（Simultaneous Localization and Mapping，SLAM）的例子。
迭代最近點匹配
本算法是使用單值解構(gòu)進(jìn)行二維迭代最近點（Iterative Closest Point，ICP）匹配的例子。
它能計算從一些點到另一些點的旋轉(zhuǎn)矩陣和平移矩陣。
相關(guān)閱讀：
機(jī)器人運動介紹：迭代最近點算法
https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf
EKF SLAM
這是基于擴(kuò)展卡爾曼濾波的SLAM示例。
藍(lán)線是真實路徑，黑線是導(dǎo)航推測路徑，紅線是EKF SLAM估計的路徑。
綠叉是估計的地標(biāo)。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
FastSLAM 1.0
這是用FastSLAM 1.0進(jìn)行基于特征的SLAM的示例。
藍(lán)線是實際路徑，黑線是導(dǎo)航推測，紅線是FastSLAM的推測路徑。
紅點是FastSLAM中的粒子。
黑點是地標(biāo)，藍(lán)叉是FastLSAM估算的地標(biāo)位置。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
FastSLAM 2.0
這是用FastSLAM 2.0進(jìn)行基于特征的SLAM的示例。
動畫的含義與FastSLAM 1.0的情況相同。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
Tim Bailey的SLAM模擬
http://www-personal.acfr.usyd.edu.au/tbailey/software/slam_simulations.htm
基于圖的SLAM
這是基于圖的SLAM的示例。
藍(lán)線是實際路徑。
黑線是導(dǎo)航推測路徑。
紅線是基于圖的SLAM估算的路徑。
黑星是地標(biāo)，用于生成圖的邊。
相關(guān)閱讀：
基于圖的SLAM入門
http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
路徑規(guī)劃
動態(tài)窗口方式
這是使用動態(tài)窗口方式（Dynamic Window Approach）進(jìn)行二維導(dǎo)航的示例代碼。
相關(guān)閱讀：
用動態(tài)窗口方式避免碰撞
https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf
基于網(wǎng)格的搜索
迪杰斯特拉算法
這是利用迪杰斯特拉（Dijkstra）算法實現(xiàn)的基于二維網(wǎng)格的最短路徑規(guī)劃。
動畫中青色點為搜索過的節(jié)點。
A*算法
下面是使用A星算法進(jìn)行基于二維網(wǎng)格的最短路徑規(guī)劃。
動畫中青色點為搜索過的節(jié)點。
啟發(fā)算法為二維歐幾里得距離。
勢場算法
下面是使用勢場算法進(jìn)行基于二維網(wǎng)格的路徑規(guī)劃。
動畫中藍(lán)色的熱區(qū)圖顯示了每個格子的勢能。
相關(guān)閱讀：
機(jī)器人運動規(guī)劃：勢能函數(shù)
https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
模型預(yù)測路徑生成
下面是模型預(yù)測路徑生成的路徑優(yōu)化示例。
算法用于狀態(tài)晶格規(guī)劃（state lattice planning）。
路徑優(yōu)化示例
查找表生成示例
相關(guān)閱讀：
用于帶輪子的機(jī)器人的最優(yōu)不平整地形路徑生成
http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328
狀態(tài)晶格規(guī)劃
這個腳本使用了狀態(tài)晶格規(guī)劃（state lattice planning）實現(xiàn)路徑規(guī)劃。
這段代碼通過模型預(yù)測路徑生成來解決邊界問題。
相關(guān)閱讀：
用于帶輪子的機(jī)器人的最優(yōu)不平整地形路徑生成
http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328
用于復(fù)雜環(huán)境下的高性能運動機(jī)器人導(dǎo)航的可行運動的狀態(tài)空間采樣
http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf
均勻極性采樣（Uniform polar sampling）
偏差極性采樣（Biased polar sampling）
路線采樣（Lane sampling）
隨機(jī)路徑圖（PRM）規(guī)劃
這個隨機(jī)路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。
動畫中的藍(lán)點為采樣點。
青色叉為迪杰斯特拉方法搜索過的點。
紅線為PRM的最終路徑。
相關(guān)閱讀：
隨機(jī)路徑圖
https://en.wikipedia.org/wiki/Probabilistic_roadmap
Voronoi路徑圖規(guī)劃
這個Voronoi路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。
動畫中的藍(lán)點為Voronoi點。
青色叉為迪杰斯特拉方法搜索過的點。
紅線為Voronoi路徑圖的最終路徑。
相關(guān)閱讀：
機(jī)器人運動規(guī)劃
https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf
快速搜索隨機(jī)樹（RRT）
基本RRT
這是個使用快速搜索隨機(jī)樹（Rapidly-Exploring Random Trees，RRT）的簡單路徑規(guī)劃代碼。
黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標(biāo)位置。
RRT*
這是使用RRT*的路徑規(guī)劃代碼。
黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標(biāo)位置。
相關(guān)閱讀：
最優(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
為汽車形機(jī)器人提供的使用RRT和dubins路徑規(guī)劃的路徑規(guī)劃算法。
基于Dubins路徑的RRT*
為汽車形機(jī)器人提供的使用RRT*和dubins路徑規(guī)劃的路徑規(guī)劃算法。
基于reeds-shepp路徑的RRT*
為汽車形機(jī)器人提供的使用RRT*和reeds shepp路徑規(guī)劃的路徑規(guī)劃算法。
Informed RRT*
這是使用Informed RRT*的路徑規(guī)劃代碼。
青色橢圓為Informed RRT*的啟發(fā)采樣域。
相關(guān)閱讀：
Informed RRT*：通過對可接受的橢球啟發(fā)的直接采樣實現(xiàn)最優(yōu)的基于采樣的路徑規(guī)劃
https://arxiv.org/pdf/1404.2334.pdf
批量Informed RRT*
這是使用批量Informed RRT*的路徑規(guī)劃代碼。
相關(guān)閱讀：
批量Informed樹（BIT*）：通過對隱含隨機(jī)幾何圖形進(jìn)行啟發(fā)式搜索實現(xiàn)基于采樣的最優(yōu)規(guī)劃
https://arxiv.org/abs/1405.5848
閉合回路RRT*
使用閉合回路RRT*（Closed loop RRT*）實現(xiàn)的基于車輛模型的路徑規(guī)劃。
這段代碼里，轉(zhuǎn)向控制用的是純追跡算法（pure-pursuit algorithm）。
速度控制采用了PID。
相關(guān)閱讀：
使用閉合回路預(yù)測在復(fù)雜環(huán)境內(nèi)實現(xiàn)運動規(guī)劃
http://acl.mit.edu/papers/KuwataGNC08.pdf）
應(yīng)用于自動城市駕駛的實時運動規(guī)劃
http://acl.mit.edu/papers/KuwataTCST09.pdf
[1601.06326]采用閉合回路預(yù)測實現(xiàn)最優(yōu)運動規(guī)劃的基于采樣的算法
https://arxiv.org/abs/1601.06326
LQR-RRT*
這是個使用LQR-RRT*的路徑規(guī)劃模擬。
LQR局部規(guī)劃采用了雙重積分運動模型。
相關(guān)閱讀：
LQR-RRT*：使用自動推導(dǎo)擴(kuò)展啟發(fā)實現(xiàn)最優(yōu)基于采樣的運動規(guī)劃
http://lis.csail.mit.edu/pubs/perez-icra12.pdf
MahanFathi/LQR-RRTstar：LQR-RRT*方法用于單擺相位中的隨機(jī)運動規(guī)劃
https://github.com/MahanFathi/LQR-RRTstar
三次樣條規(guī)劃
這是段三次路徑規(guī)劃的示例代碼。
這段代碼根據(jù)x-y的路點，利用三次樣條生成一段曲率連續(xù)的路徑。
每個點的指向角度也可以用解析的方式計算。
B樣條規(guī)劃
這是段使用B樣條曲線進(jìn)行規(guī)劃的例子。
輸入路點，它會利用B樣條生成光滑的路徑。
第一個和最后一個路點位于最后的路徑上。
相關(guān)閱讀：
B樣條
https://en.wikipedia.org/wiki/B-spline
Eta^3樣條路徑規(guī)劃
這是使用Eta ^ 3樣條曲線的路徑規(guī)劃。
相關(guān)閱讀：
\eta^3-Splines for the Smooth Path Generation of Wheeled Mobile Robots
https://ieeexplore.ieee.org/document/4339545/
貝濟(jì)埃路徑規(guī)劃
貝濟(jì)埃路徑規(guī)劃的示例代碼。
根據(jù)四個控制點生成貝濟(jì)埃路徑。
改變起點和終點的偏移距離，可以生成不同的貝濟(jì)埃路徑：
相關(guān)閱讀：
根據(jù)貝濟(jì)埃曲線為自動駕駛汽車生成曲率連續(xù)的路徑
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf
五次多項式規(guī)劃
利用五次多項式進(jìn)行路徑規(guī)劃。
它能根據(jù)五次多項式計算二維路徑、速度和加速度。
相關(guān)閱讀：
用于Agv In定位的局部路徑規(guī)劃和運動控制
http://ieeexplore.ieee.org/document/637936/
Dubins路徑規(guī)劃
Dubins路徑規(guī)劃的示例代碼。
相關(guān)閱讀：
Dubins路徑
https://en.wikipedia.org/wiki/Dubins_path
Reeds Shepp路徑規(guī)劃
Reeds Shepp路徑規(guī)劃的示例代碼。
相關(guān)閱讀：
15.3.2 Reeds-Shepp曲線
http://planning.cs.uiuc.edu/node822.html
用于能前進(jìn)和后退的汽車的最優(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)路徑。
青色線為目標(biāo)路徑，黑色叉為障礙物。
紅色線為預(yù)測的路徑。
相關(guān)閱讀：
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
路徑跟蹤
姿勢控制跟蹤
這是姿勢控制跟蹤的模擬。
相關(guān)閱讀：
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）轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
紅線為目標(biāo)路線，綠叉為純追跡控制的目標(biāo)點，藍(lán)線為跟蹤路線。
相關(guān)閱讀：
城市中的自動駕駛汽車的運動規(guī)劃和控制技術(shù)的調(diào)查
https://arxiv.org/abs/1604.07446
史坦利控制
使用史坦利（Stanley）轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
史坦利：贏得DARPA大獎賽的機(jī)器人
http://robots.stanford.edu/papers/thrun.stanley05.pdf
用于自動駕駛機(jī)動車路徑跟蹤的自動轉(zhuǎn)向方法
https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf
后輪反饋控制
利用后輪反饋轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
城市中的自動駕駛汽車的運動規(guī)劃和控制技術(shù)的調(diào)查
https://arxiv.org/abs/1604.07446
線性二次regulator（LQR）轉(zhuǎn)向控制
使用LQR轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
ApolloAuto/apollo：開源自動駕駛平臺
https://github.com/ApolloAuto/apollo
線性二次regulator（LQR）轉(zhuǎn)向和速度控制
使用LQR轉(zhuǎn)向和速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
完全自動駕駛：系統(tǒng)和算法 - IEEE會議出版物
http://ieeexplore.ieee.org/document/5940562/
模型預(yù)測速度和轉(zhuǎn)向控制
使用迭代線性模型預(yù)測轉(zhuǎn)向和速度控制的路徑跟蹤模擬。
這段代碼使用了cxvxpy作為最優(yōu)建模工具。
相關(guān)閱讀：
車輛動態(tài)和控制 | 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）對該項目進(jìn)行經(jīng)濟(jì)支持。
如果你在Patreon上支持該項目，則可以得到關(guān)于本項目代碼的郵件技術(shù)支持。
本文作者包括有Atsushi Sakai (@Atsushi_twi)，Daniel Ingram，Joe Dinius，Karan Chawla，Antonin RAFFIN，Alexis Paques。
原文：
https://atsushisakai.github.io/PythonRobotics/#what-is-this
作者：AtsushiSakai，日本機(jī)器人工程師，從事自動駕駛技術(shù)開發(fā)，精通C++、ROS、MATLAB、Python、Vim和Robotics。
譯者：彎月，責(zé)編：郭芮
- EOF -
回復(fù)關(guān)鍵字“簡明python ”，立即獲取入門必備書籍《簡明python教程》電子版
回復(fù)關(guān)鍵字“爬蟲”，立即獲取爬蟲學(xué)習(xí)資料
python入門與進(jìn)階 每天與你一起成長 推薦閱讀