自己動手寫全套無人駕駛演算法系列（三）機械人控制

目錄一、概述

1.1 系列整體概述

1.2 控制演算法實際運用問題

二、演算法介紹

2.1 pid_diff

2.2 pid_acm

2.3 mpc_acm

一、概述

1.1 系列整體概述

1.2 控制演算法實際運用問題

實際運用控制演算法不像**那麼簡單，時間上還需要如下步驟：

1.時間同步：設定乙個可信時間區間，對早到的時間，要按運動學補償，遲到超過一定時間的資料，直接按運動學推算，見我的github。

2.倒車邏輯：如航向角為負的trick，steer問題

3.換檔邏輯：如何停下來，如何判斷停穩再換檔，換檔如何判斷換檔成功

4.底盤邏輯：如usb不允許方向盤一次轉超過25度，加速度不能變化超過5m/s2等，要在mpc的凸優化裡開盒函式，輸出的時候也要限制。

5.軌跡處理邏輯：讀取軌跡的時間同步，軌跡如果不符合三次曲線怎麼辦，終點前最後一段軌跡點不足怎麼辦，軌跡雜訊太大怎麼辦。

6.臨時停車邏輯：如何臨時停車而不完全停下

7.緊急制動邏輯：突發情況處理

8.其他忘記說的邏輯。

二、演算法介紹

主要運用於如掃地機械人的差分驅動模型,實際上很多人說要雙環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++
)    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;
pida = 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一樣。

主要運用於如無人車等阿克曼轉向模型,我曾經有一段時間就是專門做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分析。

自己動手寫全套無人駕駛演算法系列（三）機械人控制

自己動手寫bootloader

自己動手寫CMS

自己動手寫SpringMVC 四

自己動手寫全套無人駕駛演算法系列（三）機械人控制

自己動手寫bootloader

自己動手寫CMS

自己動手寫SpringMVC 四

相關推薦