電子發(fā)燒友App
本文是一些機(jī)器人算法（特別是自動(dòng)導(dǎo)航算法）的Python代碼合集。
一、環(huán)境需求
Python 3.6.x
numpy
scipy
matplotlib
pandas
cvxpy 0.4.x
二、怎樣使用
安裝必要的庫；
克隆本代碼倉庫；
執(zhí)行每個(gè)目錄下的python腳本；
如果你喜歡，則收藏本代碼庫：）
三、本地化
3.1 擴(kuò)展卡爾曼濾波本地化
該算法利用擴(kuò)展卡爾曼濾波器（Extended Kalman Filter, EKF）實(shí)現(xiàn)傳感器混合本地化。
藍(lán)線為真實(shí)路徑，黑線為導(dǎo)航推測(cè)路徑（dead reckoning trajectory），綠點(diǎn)為位置觀測(cè)（如GPS），紅線為EKF估算的路徑。
紅色橢圓為EKF估算的協(xié)方差。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
3.2 無損卡爾曼濾波本地化
該算法利用無損卡爾曼濾波器（Unscented Kalman Filter, UKF）實(shí)現(xiàn)傳感器混合本地化。
線和點(diǎn)的含義與EKF模擬的例子相同。
相關(guān)閱讀：
利用無差別訓(xùn)練過的無損卡爾曼濾波進(jìn)行機(jī)器人移動(dòng)本地化
https://www.researchgate.net/publication/267963417_Discriminatively_Trained_Unscented_Kalman_Filter_for_Mobile_Robot_Localization
3.3 粒子濾波本地化
該算法利用粒子濾波器（Particle Filter, PF）實(shí)現(xiàn)傳感器混合本地化。
藍(lán)線為真實(shí)路徑，黑線為導(dǎo)航推測(cè)路徑（dead reckoning trajectory），綠點(diǎn)為位置觀測(cè)（如GPS），紅線為PF估算的路徑。
該算法假設(shè)機(jī)器人能夠測(cè)量與地標(biāo)（RFID）之間的距離。
PF本地化會(huì)用到該測(cè)量結(jié)果。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
3.4 直方圖濾波本地化
該算法是利用直方圖濾波器（Histogram filter）實(shí)現(xiàn)二維本地化的例子。
紅十字是實(shí)際位置，黑點(diǎn)是RFID的位置。
藍(lán)色格子是直方圖濾波器的概率位置。
在該模擬中，x，y是未知數(shù)，yaw已知。
濾波器整合了速度輸入和從RFID獲得距離觀測(cè)數(shù)據(jù)進(jìn)行本地化。
不需要初始位置。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
四、映射
4.1 高斯網(wǎng)格映射
本算法是二維高斯網(wǎng)格映射（Gaussian grid mapping）的例子。
4.2 光線投射網(wǎng)格映射
本算法是二維光線投射網(wǎng)格映射（Ray casting grid map）的例子。
4.3 k均值物體聚類
本算法是使用k均值算法進(jìn)行二維物體聚類的例子。
4.4 圓形擬合物體形狀識(shí)別
本算法是使用圓形擬合進(jìn)行物體形狀識(shí)別的例子。
藍(lán)圈是實(shí)際的物體形狀。
紅叉是通過距離傳感器觀測(cè)到的點(diǎn)。
紅圈是使用圓形擬合估計(jì)的物體形狀。
五、SLAM
同時(shí)本地化和映射（Simultaneous Localization and Mapping，SLAM）的例子。
5.1 迭代最近點(diǎn)匹配
本算法是使用單值解構(gòu)進(jìn)行二維迭代最近點(diǎn)（Iterative Closest Point，ICP）匹配的例子。
它能計(jì)算從一些點(diǎn)到另一些點(diǎn)的旋轉(zhuǎn)矩陣和平移矩陣。
相關(guān)閱讀：
機(jī)器人運(yùn)動(dòng)介紹：迭代最近點(diǎn)算法
https://cs.gmu.edu/~kosecka/cs685/cs685-icp.pdf
5.2 EKF SLAM
這是基于擴(kuò)展卡爾曼濾波的SLAM示例。
藍(lán)線是真實(shí)路徑，黑線是導(dǎo)航推測(cè)路徑，紅線是EKF SLAM估計(jì)的路徑。
綠叉是估計(jì)的地標(biāo)。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
5.3 FastSLAM 1.0
這是用FastSLAM 1.0進(jìn)行基于特征的SLAM的示例。
藍(lán)線是實(shí)際路徑，黑線是導(dǎo)航推測(cè)，紅線是FastSLAM的推測(cè)路徑。
紅點(diǎn)是FastSLAM中的粒子。
黑點(diǎn)是地標(biāo)，藍(lán)叉是FastLSAM估算的地標(biāo)位置。
相關(guān)閱讀：
概率機(jī)器人學(xué)
http://www.probabilistic-robotics.org/
5.4 FastSLAM 2.0
這是用FastSLAM 2.0進(jìn)行基于特征的SLAM的示例。
動(dòng)畫的含義與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
5.5 基于圖的SLAM
這是基于圖的SLAM的示例。
藍(lán)線是實(shí)際路徑。
黑線是導(dǎo)航推測(cè)路徑。
紅線是基于圖的SLAM估算的路徑。
黑星是地標(biāo)，用于生成圖的邊。
相關(guān)閱讀：
基于圖的SLAM入門
http://www2.informatik.uni-freiburg.de/~stachnis/pdf/grisetti10titsmag.pdf
六、路徑規(guī)劃
6.1 動(dòng)態(tài)窗口方式
這是使用動(dòng)態(tài)窗口方式（Dynamic Window Approach）進(jìn)行二維導(dǎo)航的示例代碼。
相關(guān)閱讀：
用動(dòng)態(tài)窗口方式避免碰撞
https://www.ri.cmu.edu/pub_files/pub1/fox_dieter_1997_1/fox_dieter_1997_1.pdf
6.2 基于網(wǎng)格的搜索
迪杰斯特拉算法
這是利用迪杰斯特拉（Dijkstra）算法實(shí)現(xiàn)的基于二維網(wǎng)格的最短路徑規(guī)劃。
動(dòng)畫中青色點(diǎn)為搜索過的節(jié)點(diǎn)。
A*算法
下面是使用A星算法進(jìn)行基于二維網(wǎng)格的最短路徑規(guī)劃。
動(dòng)畫中青色點(diǎn)為搜索過的節(jié)點(diǎn)。
啟發(fā)算法為二維歐幾里得距離。
勢(shì)場(chǎng)算法
下面是使用勢(shì)場(chǎng)算法進(jìn)行基于二維網(wǎng)格的路徑規(guī)劃。
動(dòng)畫中藍(lán)色的熱區(qū)圖顯示了每個(gè)格子的勢(shì)能。
相關(guān)閱讀：
機(jī)器人運(yùn)動(dòng)規(guī)劃：勢(shì)能函數(shù)
https://www.cs.cmu.edu/~motionplanning/lecture/Chap4-Potential-Field_howie.pdf
6.3 模型預(yù)測(cè)路徑生成
下面是模型預(yù)測(cè)路徑生成的路徑優(yōu)化示例。
算法用于狀態(tài)晶格規(guī)劃（state lattice planning）。
路徑優(yōu)化示例
查找表生成示例
相關(guān)閱讀：
用于帶輪子的機(jī)器人的最優(yōu)不平整地形路徑生成
http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328
6.4 狀態(tài)晶格規(guī)劃
這個(gè)腳本使用了狀態(tài)晶格規(guī)劃（state lattice planning）實(shí)現(xiàn)路徑規(guī)劃。
這段代碼通過模型預(yù)測(cè)路徑生成來解決邊界問題。
相關(guān)閱讀：
用于帶輪子的機(jī)器人的最優(yōu)不平整地形路徑生成
http://journals.sagepub.com/doi/pdf/10.1177/0278364906075328
用于復(fù)雜環(huán)境下的高性能運(yùn)動(dòng)機(jī)器人導(dǎo)航的可行運(yùn)動(dòng)的狀態(tài)空間采樣
http://www.frc.ri.cmu.edu/~alonzo/pubs/papers/JFR_08_SS_Sampling.pdf
均勻極性采樣（Uniform polar sampling）
偏差極性采樣（Biased polar sampling）
路線采樣（Lane sampling）
6.5 隨機(jī)路徑圖（PRM）規(guī)劃
這個(gè)隨機(jī)路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。
動(dòng)畫中的藍(lán)點(diǎn)為采樣點(diǎn)。
青色叉為迪杰斯特拉方法搜索過的點(diǎn)。
紅線為PRM的最終路徑。
相關(guān)閱讀：
隨機(jī)路徑圖
https://en.wikipedia.org/wiki/Probabilistic_roadmap
6.6 Voronoi路徑圖規(guī)劃
這個(gè)Voronoi路徑圖（Probabilistic Road-Map，PRM）規(guī)劃算法在圖搜索上采用了迪杰斯特拉方法。
動(dòng)畫中的藍(lán)點(diǎn)為Voronoi點(diǎn)。
青色叉為迪杰斯特拉方法搜索過的點(diǎn)。
紅線為Voronoi路徑圖的最終路徑。
相關(guān)閱讀：
機(jī)器人運(yùn)動(dòng)規(guī)劃
https://www.cs.cmu.edu/~motionplanning/lecture/Chap5-RoadMap-Methods_howie.pdf
6.7 快速搜索隨機(jī)樹（RRT）
基本RRT
這是個(gè)使用快速搜索隨機(jī)樹（Rapidly-Exploring Random Trees，RRT）的簡單路徑規(guī)劃代碼。
黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標(biāo)位置。
RRT*
這是使用RRT*的路徑規(guī)劃代碼。
黑色圓為障礙物，綠線為搜索樹，紅叉為開始位置和目標(biāo)位置。
相關(guān)閱讀：
最優(yōu)運(yùn)動(dòng)規(guī)劃的基于增量采樣的算法
https://arxiv.org/abs/1005.0416
最優(yōu)運(yùn)動(dòng)規(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*：通過對(duì)可接受的橢球啟發(fā)的直接采樣實(shí)現(xiàn)最優(yōu)的基于采樣的路徑規(guī)劃
https://arxiv.org/pdf/1404.2334.pdf
批量Informed RRT*
這是使用批量Informed RRT*的路徑規(guī)劃代碼。
相關(guān)閱讀：
批量Informed樹（BIT*）：通過對(duì)隱含隨機(jī)幾何圖形進(jìn)行啟發(fā)式搜索實(shí)現(xiàn)基于采樣的最優(yōu)規(guī)劃
https://arxiv.org/abs/1405.5848
閉合回路RRT*
使用閉合回路RRT*（Closed loop RRT*）實(shí)現(xiàn)的基于車輛模型的路徑規(guī)劃。
這段代碼里，轉(zhuǎn)向控制用的是純追跡算法（pure-pursuit algorithm）。
速度控制采用了PID。
相關(guān)閱讀：
使用閉合回路預(yù)測(cè)在復(fù)雜環(huán)境內(nèi)實(shí)現(xiàn)運(yùn)動(dòng)規(guī)劃
http://acl.mit.edu/papers/KuwataGNC08.pdf）
應(yīng)用于自動(dòng)城市駕駛的實(shí)時(shí)運(yùn)動(dòng)規(guī)劃
http://acl.mit.edu/papers/KuwataTCST09.pdf
[1601.06326]采用閉合回路預(yù)測(cè)實(shí)現(xiàn)最優(yōu)運(yùn)動(dòng)規(guī)劃的基于采樣的算法
https://arxiv.org/abs/1601.06326
LQR-RRT*
這是個(gè)使用LQR-RRT*的路徑規(guī)劃模擬。
LQR局部規(guī)劃采用了雙重積分運(yùn)動(dòng)模型。
相關(guān)閱讀：
LQR-RRT*：使用自動(dòng)推導(dǎo)擴(kuò)展啟發(fā)實(shí)現(xiàn)最優(yōu)基于采樣的運(yùn)動(dòng)規(guī)劃
http://lis.csail.mit.edu/pubs/perez-icra12.pdf
MahanFathi/LQR-RRTstar：LQR-RRT*方法用于單擺相位中的隨機(jī)運(yùn)動(dòng)規(guī)劃
https://github.com/MahanFathi/LQR-RRTstar
6.8 三次樣條規(guī)劃
這是段三次路徑規(guī)劃的示例代碼。
這段代碼根據(jù)x-y的路點(diǎn)，利用三次樣條生成一段曲率連續(xù)的路徑。
每個(gè)點(diǎn)的指向角度也可以用解析的方式計(jì)算。
6.9 B樣條規(guī)劃
這是段使用B樣條曲線進(jìn)行規(guī)劃的例子。
輸入路點(diǎn)，它會(huì)利用B樣條生成光滑的路徑。
第一個(gè)和最后一個(gè)路點(diǎn)位于最后的路徑上。
相關(guān)閱讀：
B樣條
https://en.wikipedia.org/wiki/B-spline
6.10 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/
6.11 貝濟(jì)埃路徑規(guī)劃
貝濟(jì)埃路徑規(guī)劃的示例代碼。
根據(jù)四個(gè)控制點(diǎn)生成貝濟(jì)埃路徑。
改變起點(diǎn)和終點(diǎn)的偏移距離，可以生成不同的貝濟(jì)埃路徑：
相關(guān)閱讀：
根據(jù)貝濟(jì)埃曲線為自動(dòng)駕駛汽車生成曲率連續(xù)的路徑
http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.294.6438&rep=rep1&type=pdf
6.12 五次多項(xiàng)式規(guī)劃
利用五次多項(xiàng)式進(jìn)行路徑規(guī)劃。
它能根據(jù)五次多項(xiàng)式計(jì)算二維路徑、速度和加速度。
相關(guān)閱讀：
用于Agv In定位的局部路徑規(guī)劃和運(yùn)動(dòng)控制
http://ieeexplore.ieee.org/document/637936/
6.13 Dubins路徑規(guī)劃
Dubins路徑規(guī)劃的示例代碼。
相關(guān)閱讀：
Dubins路徑
https://en.wikipedia.org/wiki/Dubins_path
6.14 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：實(shí)現(xiàn)Reeds Shepp曲線
https://github.com/ghliu/pyReedsShepp
6.15 基于LQR的路徑規(guī)劃
為雙重積分模型使用基于LQR的路徑規(guī)劃的示例代碼。
6.16 Frenet Frame中的最優(yōu)路徑
這段代碼在Frenet Frame中生成最優(yōu)路徑。
青色線為目標(biāo)路徑，黑色叉為障礙物。
紅色線為預(yù)測(cè)的路徑。
相關(guān)閱讀：
Frenet Frame中的動(dòng)態(tài)接到場(chǎng)景中的最優(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中的動(dòng)態(tài)接到場(chǎng)景中的最優(yōu)路徑生成
https://www.youtube.com/watch?v=Cj6tAQe7UCY
七、路徑跟蹤
7.1 姿勢(shì)控制跟蹤
這是姿勢(shì)控制跟蹤的模擬。
相關(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
7.2 純追跡跟蹤
使用純追跡（pure pursuit）轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
紅線為目標(biāo)路線，綠叉為純追跡控制的目標(biāo)點(diǎn)，藍(lán)線為跟蹤路線。
相關(guān)閱讀：
城市中的自動(dòng)駕駛汽車的運(yùn)動(dòng)規(guī)劃和控制技術(shù)的調(diào)查
https://arxiv.org/abs/1604.07446
7.3 史坦利控制
使用史坦利（Stanley）轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
史坦利：贏得DARPA大獎(jiǎng)賽的機(jī)器人
http://robots.stanford.edu/papers/thrun.stanley05.pdf
用于自動(dòng)駕駛機(jī)動(dòng)車路徑跟蹤的自動(dòng)轉(zhuǎn)向方法
https://www.ri.cmu.edu/pub_files/2009/2/Automatic_Steering_Methods_for_Autonomous_Automobile_Path_Tracking.pdf
7.4 后輪反饋控制
利用后輪反饋轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
城市中的自動(dòng)駕駛汽車的運(yùn)動(dòng)規(guī)劃和控制技術(shù)的調(diào)查
https://arxiv.org/abs/1604.07446
7.5 線性二次regulator（LQR）轉(zhuǎn)向控制
使用LQR轉(zhuǎn)向控制和PID速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
ApolloAuto/apollo：開源自動(dòng)駕駛平臺(tái)
https://github.com/ApolloAuto/apollo
7.6 線性二次regulator（LQR）轉(zhuǎn)向和速度控制
使用LQR轉(zhuǎn)向和速度控制的路徑跟蹤模擬。
相關(guān)閱讀：
完全自動(dòng)駕駛：系統(tǒng)和算法 - IEEE會(huì)議出版物
http://ieeexplore.ieee.org/document/5940562/
7.7 模型預(yù)測(cè)速度和轉(zhuǎn)向控制
使用迭代線性模型預(yù)測(cè)轉(zhuǎn)向和速度控制的路徑跟蹤模擬。
這段代碼使用了cxvxpy作為最優(yōu)建模工具。
車輛動(dòng)態(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
八、項(xiàng)目支持
可以通過Patreon（
https://www.patreon.com/myenigma）對(duì)該項(xiàng)目進(jìn)行經(jīng)濟(jì)支持。
如果你在Patreon上支持該項(xiàng)目，則可以得到關(guān)于本項(xiàng)目代碼的郵件技術(shù)支持。
本文作者包括有Atsushi Sakai (@Atsushi_twi)，Daniel Ingram，Joe Dinius，Karan Chawla，Antonin RAFFIN，Alexis Paques。
編輯：黃飛
?
工商網(wǎng)監(jiān)
評(píng)論