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

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

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

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

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

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

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

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

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

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

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

• 2）寫一個局部規(guī)劃器，控制電動車跟隨全局軌跡，并實現(xiàn)避障等功能。

So easy!

由于每個停車點相對固定、校園環(huán)境也比較簡單，所以全局規(guī)劃器并不復(fù)雜，將校園地圖做成網(wǎng)格（或拓?fù)鋱D），跑一個A*就可以了。當(dāng)然，畢竟我是機(jī)動學(xué)院的，所以我做的這種車也只能走機(jī)動車道，為了防止它跑出機(jī)動車道，可以加一個costmap。不多說。

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

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

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

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

哎呦，不錯喲！

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

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

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

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

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

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

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

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

cost function + map = costmap

重點是局部規(guī)劃器。

由于，局部規(guī)劃器所需滿足的約束條件比較多，就可以通過設(shè)計一堆由路徑?jīng)Q定的優(yōu)化目標(biāo)函數(shù)，利用優(yōu)化算法對它求解進(jìn)行了。

對于二維路徑的描述，有一個有趣的方法，叫做Elatic Band（橡皮筋）。

簡而言之，就是連接起始、目標(biāo)點，并讓這個路徑可以變形，變形的條件就是將所有約束當(dāng)做橡皮筋的外力。

先定義一下我們的橡皮筋：

起始點、目標(biāo)點狀態(tài)由用戶/全局規(guī)劃器指定，中間插入N個控制橡皮筋形狀的控制點（機(jī)器人姿態(tài)），當(dāng)然，為了顯示軌跡的運動學(xué)信息，我們在點與點之間定義運動時間Time。于是，這個方法就叫做Timed-Elastic-Band。即

time + elastic band = timed elatics band

之后就需要定義「內(nèi)外力」的數(shù)學(xué)表達(dá)形式，即目標(biāo)函數(shù)。（以下均為中學(xué)知識）

1) 要跟蹤全局軌跡+要避開障礙物：

這兩個其實算是一類問題，都是在橡皮筋上找到距離某一點（全局路徑點/障礙物）最近的狀態(tài)，計算兩者之間距離，之后定義一個基于距離的勢場就好了。

當(dāng)然，上面兩種目標(biāo)函數(shù)隨距離變化方向正好相反，一個隨著距離增大而增大（跟蹤），一個隨著距離增大而減?。ㄕ系K物）。

2) 加速度/速度限制

這個其實就是一個不等式約束。

我們的橡皮筋只定義了姿態(tài)（x,y,θ）與兩兩狀態(tài)直接的時間，所以就直接用差分近似計算好了。

3) 運動學(xué)限制

這個比較重要，畢竟我們一般都不希望自己的車漂移起來。

所以，我們的控制量只有車速（油門）與轉(zhuǎn)角（方向盤）。

我們假設(shè)兩個狀態(tài)點之間轉(zhuǎn)角相同，（如果發(fā)生了轉(zhuǎn)向，中間加狀態(tài)點就好了）。

簡單的向量叉乘求夾角即可

當(dāng)然，由于汽車的結(jié)構(gòu)限制，汽車會有一個最小轉(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)過來，這就是一個bundle adjustment問題。

簡而言之，雖然待優(yōu)化的橡皮筋有不少狀態(tài)點與時間段，目標(biāo)函數(shù)也好像很多。但是，每個目標(biāo)函數(shù)只與橡皮筋中的某幾個狀態(tài)有關(guān)，而非整條橡皮筋。認(rèn)識到這是一個稀疏優(yōu)化（Sparse Optimization）問題就比較容易了。

將它描述成圖，然后用圖優(yōu)化。

如圖，這個圖的節(jié)點vertexs是橡皮筋的狀態(tài)（機(jī)器人姿態(tài)+時間）；圖的邊edges是我們自己定義的優(yōu)化目標(biāo)函數(shù)。

求解的框架，自然是可以去使用g2o（A General Framework for Graph Optimization）了。當(dāng)然，節(jié)點和邊的類型需要我們自己利用g2o中的模板定義。

最后是臨時工剪輯的視頻Cost Map Elastic Band，不能只有我一個人被洗腦：

https://v.qq.com/x/page/c0354qk9qne.html

雷峰網(wǎng)特約稿件，未經(jīng)授權(quán)禁止轉(zhuǎn)載。詳情見轉(zhuǎn)載須知。