雷鋒網(wǎng)按：本文作者qqfly，上海交通大學(xué)機(jī)器人所博士生，本科畢業(yè)于清華大學(xué)機(jī)械工程系，主要研究方向機(jī)器視覺與運(yùn)動(dòng)規(guī)劃，個(gè)人微信公眾號(hào)：Nao（ID:qRobotics），雷鋒網(wǎng)授權(quán)發(fā)布。

最近，天氣變得越來越冷，實(shí)驗(yàn)室也就變得越來越遠(yuǎn)。就連像我這樣比較勤奮的博士生都沒辦法準(zhǔn)時(shí)到實(shí)驗(yàn)室了。

短短的2.2km，成為了我學(xué)術(shù)道路上最大的絆腳石——一想到要在寒風(fēng)中騎15分鐘自行車，就根本不想起床了。

現(xiàn)在，有了「××」分時(shí)租賃電動(dòng)汽車（顯然沒有廣告費(fèi)），開開心心就能到實(shí)驗(yàn)室。一口氣開兩公里，不會(huì)冷。

但是，用過幾次分時(shí)租賃之后，問題出現(xiàn)了。

沒錯(cuò)，那就是附近沒有車！尤其是像我這樣比較勤奮的博士生，經(jīng)常學(xué)習(xí)（刷知乎）到深夜，那樣就只能默默走2.2km回宿舍了。

「要是預(yù)定之后，汽車能自動(dòng)開到你附近就好了?！刮也唤@樣想。

這不就是二維路徑規(guī)劃問題嘛，對(duì)于一個(gè)知道四十種運(yùn)動(dòng)規(guī)劃算法的博士生，這個(gè)完全是道送分題。

要是做出來了，那豈不是立馬制霸分時(shí)租賃市場(chǎng)，再隨便忽悠個(gè)風(fēng)投，然后把「××」打車收購了，改成「××」自動(dòng)駕駛出租車。想想都激動(dòng)。

利益熏心之下，我立馬投入了工作。由于平時(shí)見多識(shí)廣，我很快就找到了解決這個(gè)問題的辦法：

• 1）寫一個(gè)全局規(guī)劃器，讓電動(dòng)車能在任意兩個(gè)停車點(diǎn)之間找到開車路線；

• 2）寫一個(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。不多說。

costmap示意圖，讓距離不可行區(qū)域（如非機(jī)動(dòng)車道）較近的路徑耗費(fèi)更高即可

寫完全局規(guī)劃器，就只剩下局部規(guī)劃器了。

這個(gè)局部規(guī)劃器也簡(jiǎn)單，看了文獻(xiàn)，大概有個(gè)叫做Timed-Elastic-Band的多目標(biāo)優(yōu)化方法，只要寫出需要滿足的優(yōu)化目標(biāo)函數(shù)即可：

算了，差不多這樣就可以了，出于嚴(yán)謹(jǐn)?shù)目茖W(xué)態(tài)度，我用stage搭了一個(gè)仿真平臺(tái)，測(cè)試這個(gè)算法。

哎呦，不錯(cuò)喲！

那就直接上真車吧。但畢竟「××」分時(shí)租賃不肯贊助我，我沒辦法用他們的車做實(shí)驗(yàn)，我只能把視線投向了實(shí)驗(yàn)室的移動(dòng)小車。

算了，我還是去當(dāng)電影剪輯師好了。

===============

不好意思，上面都是臨時(shí)工寫的。為了不劇透，臨時(shí)工剪輯的視頻也放在最后。

由于之前一直宣揚(yáng)「path planning 跟 motion planning 在數(shù)學(xué)上是一個(gè)問題」，于是被拉入坑，讓我去做這個(gè)自動(dòng)駕駛車的東西。

當(dāng)然，一開始讓我去做這個(gè)路徑規(guī)劃，其實(shí)我的拒絕的。

但后來一想，正好能在低維情況下試試那些基于優(yōu)化的路徑規(guī)劃算法，然后順便再發(fā)篇論文，也不錯(cuò)。

全局規(guī)劃器跟上面臨時(shí)工說得差不多，就是costmap + graph search。costmap就是根據(jù)地圖的可行區(qū)域，再通過耗費(fèi)函數(shù)計(jì)算出來的一個(gè)圖。即，

cost function + map = costmap

重點(diǎn)是局部規(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) 要跟蹤全局軌跡+要避開障礙物：

這兩個(gè)其實(shí)算是一類問題，都是在橡皮筋上找到距離某一點(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è)比較重要，畢竟我們一般都不希望自己的車漂移起來。

所以，我們的控制量只有車速（油門）與轉(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)化問題，一看就不想做

好吧，這步雖然看似復(fù)雜，但是了解SLAM或者SFM的同學(xué)應(yīng)該能很快反應(yīng)過來，這就是一個(gè)bundle adjustment問題。

簡(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）問題就比較容易了。

將它描述成圖，然后用圖優(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)載。詳情見轉(zhuǎn)載須知。