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