研究週記_20190310(軌跡控制實作withMATLAB)
目標規劃：
目前研究的短期目標是利用軌跡規劃和軌跡控制，來輔助A-star(A*)演算法運用在輪型機器人移動的效率與穩定度。
中期目標是與現階段常被使用的move_base做比較，並試圖改進
長期目標是加入行人方位辨識，搭配強化學習，以更有效率的方式與行人互動
本週進度概要：
將參考書目中關於軌跡追蹤(Trajectory Tracking)的控制器部分以Matlab方式實踐並模擬，現階段已可在類似A*的輸出結果來做路徑規劃並追蹤。
背景：
A*：一種包含避障的全域路徑規劃演算法，以方格方式切割地圖，找出在避開障礙物下最有效率的路徑(輸入是地圖資訊和目標 輸出是最佳路徑的方格座標集合)
問題陳述：
現階段A*被我們實驗室拿來做全域的路徑規劃，而讓機器人依照A*移動的方法是將A*輸出的方格座標集合丟給機器人，以"定位控制"的方式來完成方格間的移動，然而此方法會由於是不斷的定位，會造成機器人在前進至各目標點時出現減速現象，導致移動效率不佳。目前決定導入局部軌跡規劃及軌跡追蹤，試圖改善問題。
本週進度呈現：
上圖中，中間三塊藍線方塊為假想障礙物，X點為目標，O點為利用A*計算出來應經過的方塊座標點，車子起始位置為(-20,-20)，座標上較淺的線是標示出在計算A*時將地圖切隔成的方格，O點之間的藍線為利用三次方多項式規劃出各點之間的軌跡。
理論細節：
目前所使用的三次方多項式路徑規劃，是根據參考書目中所提供的方法，k設定為20
而路徑追蹤控制法則，在參考書目中有提供三種，三種方法我都有在程式中實現，目前模擬是以動態模型的線性控制去完成。
下週待辦事項：
- 模擬中，從幾何座標S到時間座標T僅以是直接轉換，並未經過時間規劃(time law)，因此會發生中間速度減慢的現象，預期修正目標是在軌跡上能夠等速前進
- 加入物理尺度與輸入飽和限制，並比較3種控制法則的響應
- 將整個控制移植至Linux系統，以C++或Python包裝成ROS package並搭配turtlesim去模擬(輸入為三個座標點，輸出為根據規畫時間丟出速度與角速度命令)
參考書目：
Chapter 11, Robotics: Modeling, Planning and Control. B. Sicilano, L. Sciavicco, L. Villani
Memo：一些matlab指令
- 多項式：P = [a b c] >> ax^2 + bx + c
- 多項式代值：P(x) = polyval(P,x)
- 改變刻度：set(gca,'xtick',[-30:10:10])
- 繪製直線(X1,Y1)to(X2,Y2)：line( [X1 X2] , [Y1,Y2] )