自己動手寫全套無人駕駛算法系列（一）機器人控制
一、概述
1.1 系列整體概述：
本文介紹無人駕駛的主要幾大控制算法，PID, PID+Review, MPC,按理算還應該有stanley control，LQR這些，但是目前類似掃地機器人這些，用單純PID就夠了，而對於無人駕駛汽車，百度主要也是PID和MPC，因爲狹窄路段，倒車入庫等PID就可以，而高速公路，跟車，車道保持等MPC就可以，所以關鍵不在多而在適用，stanley是好算法，有時間會寫，但目前人們大多用MPC而不用LQR，因爲LQR實際效果大都不如MPC。另外本人自己主要就是做MPC，能實現無人汽車下旋轉停車場，倒車入庫，上旋轉坡，車道保持等等，我在github給出的是仿真模式，沒有加入優化和邏輯，這個可以自己加，怎麼加見下文和我的github：https://github.com/wujiazheng2020/WJZ_Controller
歡迎大家對我的github項目提出BUG，或者需要我更新新的算法,或者需要技術合作，都可以聯繫我：
163郵箱：[email protected]
gmail:[email protected]
QQ羣：710805413
1.2 控制算法實際運用問題
實際運用控制算法不像仿真那麼簡單，時間上還需要如下步驟：
1.時間同步：設定一個可信時間區間，對早到的時間，要按運動學補償，遲到超過一定時間的數據，直接按運動學推算，見我的github。
2.倒車邏輯：如航向角爲負的Trick，steer問題
3.換檔邏輯：如何停下來，如何判斷停穩再換檔，換檔如何判斷換檔成功
4.底盤邏輯：如USB不允許方向盤一次轉超過25度，加速度不能變化超過5m/s2等，要在MPC的凸優化裏開盒函數，輸出的時候也要限制。
5.軌跡處理邏輯：讀取軌跡的時間同步，軌跡如果不符合三次曲線怎麼辦，終點前最後一段軌跡點不足怎麼辦，軌跡噪聲太大怎麼辦。
6.臨時停車邏輯：如何臨時停車而不完全停下
7.緊急制動邏輯：突發情況處理
8.其他忘記說的邏輯。
一、算法介紹
注：本程序所用軌跡爲從停車場-4上升到-2曾的軌跡，圖片均來自我的github。
2.1 PID_DIFF
主要運用於如掃地機器人的差分驅動模型,實際上很多人說要雙環PID，但是就實際測試情況來說，單環這種對大部分機器人夠了，而雙環機器人，大部分廠家的生產標準不能到大公司的標準，換了機器人要重調試很麻煩，實際過程中對大型機器人，PID需要一些trick，比如停止時，軌跡處理這些，這些不屬於算法內容，可以自己去探索。
算法核心在於如下代碼：
D[2] = D[1];
D[1] = D[0];
D[0] = dis;
Y[2] = Y[1];
Y[1] = Y[0];
Y[0] = yaw_d;
v = Pv*(D[0]) + Iv*(D[0]+D[1]+D[2]) + Dv*(D[0]-D[2]);
w = Pw*(Y[0]) + Iw*(Y[0]+Y[1]+Y[2]) + Dw*(Y[0]-Y[2]);
選取距離爲當前最近點索引爲k的點作爲target點，以對其的距離，角度差爲誤差進行PID控制
控制V，W，PID是利用比列，積分，微分進行控制的算法，算法本身具體可以見一文讀懂PID控制
2.2 PID_ACM
主要運用於如無人車等阿克曼轉向模型,阿克曼轉向模型見我係列第二篇的介紹，
老實說實際過程中，做成雙環效果似乎不如單環，但大家可以自行設置再加一環，會單環加環也很容易，對無人車，需要一些trick才能實際運行，比如時間不同，停車處理，各種邏輯的處理，這些不屬於算法了，可以自行加上。
本算法利用了預瞄(preview)技巧，預瞄前面N個點的航向角，可以讓無人車控制更穩定：
核心代碼如下：
R8 lat_loss = -v_dis;
I4 end_i = (min_i+param.review_num);
end_i = end_i>=line_size?line_size-1:end_i;
R8 yaw_loss = 0;
R8 dis_loss = 0;
for(I4 i = min_i + 1;i<=end_i;i++){
yaw_loss += yaw_sub(refer_line[i].yaw,now_pose.yaw);
dis_loss += refer_line[i].v*param.dt;
}
R8 angle_loss = param.ag*lat_loss + param.bg*yaw_loss;
D[2] = D[1];
D[1] = D[0];
D[0] = dis_loss;
Y[2] = Y[1];
Y[1] = Y[0];
Y[0] = angle_loss;
//3.update PID
a = P_lon*(D[0]) + I_lon*(D[0]+D[1]+D[2]) + D_lon*(D[0]-D[2]);
steer = P_lat*(Y[0]) + I_lat*(Y[0]+Y[1]+Y[2]) + D_lat*(Y[0]-Y[2]);
採用角度差和預瞄作爲角度差，其他和上個PID一樣。
2.3 MPC_ACM
主要運用於如無人車等阿克曼轉向模型,我曾經有一段時間就是專門做MPC，可以實現下停車場並倒車入庫，上旋轉的坡，車道保持跟隨各種，但是MPC運用時是要考慮時間同步的，時間對MPC特別重要，因爲MPC是等時間間隔預測的，另外需要一些軌跡處理，很多停車，倒車邏輯，處理規劃路線邏輯，這些根據不同車有不同處理方法，不屬於算法本身，可以自己探索。
全稱是模型預測控制，就是給定車前方軌跡N個點，時間間隔爲dt，故總時間是N×dt，那麼我們按運動學方程，以不同的a[N]和steer[N] （表示a和steer的數組，a[6]就是表示t=6×dt時刻的給的加速度）推算出到相應時間的x，y，橫向誤差，縱向誤差，a，航向角等狀態，那麼計算x,y，橫向等與軌跡相應點的差，得到一個cost function，其中a[N]和steer[N]都是待優化量，之後我們便可以按IPOPT進行凸優化迭代即可，當然了，我們要對a的變化率，steer變化率，速度大小，加速度大小等進行約束。
最後我們就得到一個最優的a,steer序列，取a[0],steer[0]作爲當前輸出即可。正常行駛效果是各大控制算法最好。具體見MPC分析。