4
| 本文作者： qqfly | 2016-12-23 09:10 |
雷鋒網(wǎng)按：本文作者qqfly，上海交通大學(xué)機(jī)器人所博士生，本科畢業(yè)于清華大學(xué)機(jī)械工程系，主要研究方向機(jī)器視覺(jué)與運(yùn)動(dòng)規(guī)劃，個(gè)人微信公眾號(hào)：Nao（ID:qRobotics），雷鋒網(wǎng)授權(quán)發(fā)布。
最近，天氣變得越來(lái)越冷，實(shí)驗(yàn)室也就變得越來(lái)越遠(yuǎn)。就連像我這樣比較勤奮的博士生都沒(méi)辦法準(zhǔn)時(shí)到實(shí)驗(yàn)室了。
短短的2.2km，成為了我學(xué)術(shù)道路上最大的絆腳石——一想到要在寒風(fēng)中騎15分鐘自行車，就根本不想起床了。
現(xiàn)在，有了「××」分時(shí)租賃電動(dòng)汽車（顯然沒(méi)有廣告費(fèi)），開(kāi)開(kāi)心心就能到實(shí)驗(yàn)室。一口氣開(kāi)兩公里，不會(huì)冷。
但是，用過(guò)幾次分時(shí)租賃之后，問(wèn)題出現(xiàn)了。
沒(méi)錯(cuò)，那就是附近沒(méi)有車！尤其是像我這樣比較勤奮的博士生，經(jīng)常學(xué)習(xí)（刷知乎）到深夜，那樣就只能默默走2.2km回宿舍了。
「要是預(yù)定之后，汽車能自動(dòng)開(kāi)到你附近就好了?！刮也唤@樣想。
這不就是二維路徑規(guī)劃問(wèn)題嘛，對(duì)于一個(gè)知道四十種運(yùn)動(dòng)規(guī)劃算法的博士生，這個(gè)完全是道送分題。
要是做出來(lái)了，那豈不是立馬制霸分時(shí)租賃市場(chǎng)，再隨便忽悠個(gè)風(fēng)投，然后把「××」打車收購(gòu)了，改成「××」自動(dòng)駕駛出租車。想想都激動(dòng)。
利益熏心之下，我立馬投入了工作。由于平時(shí)見(jiàn)多識(shí)廣，我很快就找到了解決這個(gè)問(wèn)題的辦法：
1）寫(xiě)一個(gè)全局規(guī)劃器，讓電動(dòng)車能在任意兩個(gè)停車點(diǎn)之間找到開(kāi)車路線；
2）寫(xiě)一個(gè)局部規(guī)劃器，控制電動(dòng)車跟隨全局軌跡，并實(shí)現(xiàn)避障等功能。
So easy!
由于每個(gè)停車點(diǎn)相對(duì)固定、校園環(huán)境也比較簡(jiǎn)單，所以全局規(guī)劃器并不復(fù)雜，將校園地圖做成網(wǎng)格（或拓?fù)鋱D），跑一個(gè)A*就可以了。當(dāng)然，畢竟我是機(jī)動(dòng)學(xué)院的，所以我做的這種車也只能走機(jī)動(dòng)車道，為了防止它跑出機(jī)動(dòng)車道，可以加一個(gè)costmap。不多說(shuō)。
costmap示意圖，讓距離不可行區(qū)域（如非機(jī)動(dòng)車道）較近的路徑耗費(fèi)更高即可
寫(xiě)完全局規(guī)劃器，就只剩下局部規(guī)劃器了。
這個(gè)局部規(guī)劃器也簡(jiǎn)單，看了文獻(xiàn)，大概有個(gè)叫做Timed-Elastic-Band的多目標(biāo)優(yōu)化方法，只要寫(xiě)出需要滿足的優(yōu)化目標(biāo)函數(shù)即可：
算了，差不多這樣就可以了，出于嚴(yán)謹(jǐn)?shù)目茖W(xué)態(tài)度，我用stage搭了一個(gè)仿真平臺(tái)，測(cè)試這個(gè)算法。
哎呦，不錯(cuò)喲！
那就直接上真車吧。但畢竟「××」分時(shí)租賃不肯贊助我，我沒(méi)辦法用他們的車做實(shí)驗(yàn)，我只能把視線投向了實(shí)驗(yàn)室的移動(dòng)小車。
算了，我還是去當(dāng)電影剪輯師好了。
===============
不好意思，上面都是臨時(shí)工寫(xiě)的。為了不劇透，臨時(shí)工剪輯的視頻也放在最后。
由于之前一直宣揚(yáng)「path planning 跟 motion planning 在數(shù)學(xué)上是一個(gè)問(wèn)題」，于是被拉入坑，讓我去做這個(gè)自動(dòng)駕駛車的東西。
當(dāng)然，一開(kāi)始讓我去做這個(gè)路徑規(guī)劃，其實(shí)我的拒絕的。
但后來(lái)一想，正好能在低維情況下試試那些基于優(yōu)化的路徑規(guī)劃算法，然后順便再發(fā)篇論文，也不錯(cuò)。
全局規(guī)劃器跟上面臨時(shí)工說(shuō)得差不多，就是costmap + graph search。costmap就是根據(jù)地圖的可行區(qū)域，再通過(guò)耗費(fèi)函數(shù)計(jì)算出來(lái)的一個(gè)圖。即，
cost function + map = costmap
重點(diǎn)是局部規(guī)劃器。
由于，局部規(guī)劃器所需滿足的約束條件比較多，就可以通過(guò)設(shè)計(jì)一堆由路徑?jīng)Q定的優(yōu)化目標(biāo)函數(shù)，利用優(yōu)化算法對(duì)它求解進(jìn)行了。
對(duì)于二維路徑的描述，有一個(gè)有趣的方法，叫做Elatic Band（橡皮筋）。
簡(jiǎn)而言之，就是連接起始、目標(biāo)點(diǎn)，并讓這個(gè)路徑可以變形，變形的條件就是將所有約束當(dāng)做橡皮筋的外力。
先定義一下我們的橡皮筋：
起始點(diǎn)、目標(biāo)點(diǎn)狀態(tài)由用戶/全局規(guī)劃器指定，中間插入N個(gè)控制橡皮筋形狀的控制點(diǎn)（機(jī)器人姿態(tài)），當(dāng)然，為了顯示軌跡的運(yùn)動(dòng)學(xué)信息，我們?cè)邳c(diǎn)與點(diǎn)之間定義運(yùn)動(dòng)時(shí)間Time。于是，這個(gè)方法就叫做Timed-Elastic-Band。即
time + elastic band = timed elatics band
之后就需要定義「內(nèi)外力」的數(shù)學(xué)表達(dá)形式，即目標(biāo)函數(shù)。（以下均為中學(xué)知識(shí)）
1) 要跟蹤全局軌跡+要避開(kāi)障礙物：
這兩個(gè)其實(shí)算是一類問(wèn)題，都是在橡皮筋上找到距離某一點(diǎn)（全局路徑點(diǎn)/障礙物）最近的狀態(tài)，計(jì)算兩者之間距離，之后定義一個(gè)基于距離的勢(shì)場(chǎng)就好了。
當(dāng)然，上面兩種目標(biāo)函數(shù)隨距離變化方向正好相反，一個(gè)隨著距離增大而增大（跟蹤），一個(gè)隨著距離增大而減?。ㄕ系K物）。
2) 加速度/速度限制
這個(gè)其實(shí)就是一個(gè)不等式約束。
我們的橡皮筋只定義了姿態(tài)（x,y,θ）與兩兩狀態(tài)直接的時(shí)間，所以就直接用差分近似計(jì)算好了。
3) 運(yùn)動(dòng)學(xué)限制
這個(gè)比較重要，畢竟我們一般都不希望自己的車漂移起來(lái)。
所以，我們的控制量只有車速（油門）與轉(zhuǎn)角（方向盤）。
我們假設(shè)兩個(gè)狀態(tài)點(diǎn)之間轉(zhuǎn)角相同，（如果發(fā)生了轉(zhuǎn)向，中間加狀態(tài)點(diǎn)就好了）。
簡(jiǎn)單的向量叉乘求夾角即可
當(dāng)然，由于汽車的結(jié)構(gòu)限制，汽車會(huì)有一個(gè)最小轉(zhuǎn)彎半徑。（畢竟汽車不能原地轉(zhuǎn)彎）
4) 當(dāng)然，如果有其他約束也可以扔進(jìn)去
目標(biāo)函數(shù)都定好之后，就需要進(jìn)行求解了。
這么復(fù)雜的多目標(biāo)優(yōu)化問(wèn)題，一看就不想做
好吧，這步雖然看似復(fù)雜，但是了解SLAM或者SFM的同學(xué)應(yīng)該能很快反應(yīng)過(guò)來(lái)，這就是一個(gè)bundle adjustment問(wèn)題。
簡(jiǎn)而言之，雖然待優(yōu)化的橡皮筋有不少狀態(tài)點(diǎn)與時(shí)間段，目標(biāo)函數(shù)也好像很多。但是，每個(gè)目標(biāo)函數(shù)只與橡皮筋中的某幾個(gè)狀態(tài)有關(guān)，而非整條橡皮筋。認(rèn)識(shí)到這是一個(gè)稀疏優(yōu)化（Sparse Optimization）問(wèn)題就比較容易了。
將它描述成圖，然后用圖優(yōu)化。
如圖，這個(gè)圖的節(jié)點(diǎn)vertexs是橡皮筋的狀態(tài)（機(jī)器人姿態(tài)+時(shí)間）；圖的邊edges是我們自己定義的優(yōu)化目標(biāo)函數(shù)。
求解的框架，自然是可以去使用g2o（A General Framework for Graph Optimization）了。當(dāng)然，節(jié)點(diǎn)和邊的類型需要我們自己利用g2o中的模板定義。
最后是臨時(shí)工剪輯的視頻Cost Map Elastic Band，不能只有我一個(gè)人被洗腦：
https://v.qq.com/x/page/c0354qk9qne.html
雷峰網(wǎng)特約稿件，未經(jīng)授權(quán)禁止轉(zhuǎn)載。詳情見(jiàn)轉(zhuǎn)載須知。
