以無人駕駛小車車為例子深刻理解MPC

作者：由樂林株發表于舞蹈時間：2022-03-16

前言：

這個倉庫建立的目的主要是為了記錄學習筆記，記錄的目的是加深我對MPC和軌跡最佳化理解。我本人主要專注足試機器人的控制，玩自動駕駛這個主要是為了學習。主要原因相對起來低速情況下的車模型控制相對簡單一些（三個變數，三維）。注意該倉庫主要參考來自［DhruvaKumar MPC］（

https：//

github。com/DhruvaKumar/

model-predictive-control

）。的倉庫。當然我會包括一些原倉庫沒有做的內容，畢竟是筆記也會更詳細一些，歡迎大家和我交流和並支援我。

結果如下：使用MPC進行自動駕駛控制

程式碼已上傳倉庫：

GitHub - Musyue/mpc_self_car_practice

整個倉庫包括的內主要是：

1。如何使用cppad 解微分方程。

2。如何使用CPPAD 中的ipopt來解非線性問題的。

3。無人車的線性和非線性模型推導。

4。基於線性模型的凸最佳化QP求解的MPC（暫時還沒做完）

5。基於非線性模型IPOPT求解的MPC。

6。最後使用來自參考倉庫使用的模擬平臺進行實時模擬［self-driving-car-sim］（

https：//

github。com/udacity/self

-driving-car-sim

）

這裡使用的車模型，腳踏車模型，主要針對低速情況

。

$\begin{array}{c} tan(\delta)=L/R\\ \dot{x}=v \cos (\theta) \\ \dot{y}=v \sin (\theta) \\ \dot{\theta}=v \frac{\tan (\delta)}{L}\\ v 是無人車速度，v_x和v_y在世界座標系下的速度分量\\ \theta 是yaw 方向偏航角,\dot{\theta} 是角速度，記作\omega \end{array}$

腳踏車模型簡化了無人車前輪轉向角和曲率之間的幾何關係，方便建立模型，但注意該模型沒有考慮高速情況下的側滑等情況。因此如果想要在無人車上高速情況下進行更高精度的控制，這個需要建立新的動力學模型。

線性模型如下：

無人車使用雅克比把非線性系統轉化為線性系統，當然這個線性系統只能在很小的範圍內生效，這也是線性化的通病，無人車的速度主要是靠油門控制，反向主要靠方向盤，也就是可以理解為兩個輸入。把動力學模型寫成如下公式：

$\dot{X}=\left[\begin{array}{c} \dot{x} \\ \dot{y} \\ \dot{\theta} \end{array}\right]=\left[\begin{array}{c} v \cos \theta \\ v \sin \theta \\ \frac{v \tan \delta}{L} \end{array}\right]=\left[\begin{array}{l} f_{1} \\ f_{2} \\ f_{3} \end{array}\right]=f(X, u)\\ 系統狀態：X=\left[\begin{array}{l} x\\ y\\ \theta \end{array}\right],系統輸入：u=\left[\begin{array}{l} v\\ \delta \end{array}\right]$

假設在運動過程中的某個s點進行雅克比（泰勒級數，省去高階項）線性化處理得：

$\Delta\dot{X}=\dot{X}-\dot{X}_{s}=\left[\begin{array}{c} \dot{x}-\dot{x}_{s} \\ \dot{y}-\dot{y}_{s} \\ \dot{\theta}-\dot{\theta}_{s} \end{array}\right] \approx\left(\begin{array}{lll} \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial \theta} \\ \frac{\partial f_{2}}{\partial x} & \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial \theta} \\ \frac{\partial f_{3}}{\partial x} & \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial \theta} \end{array}\right)\left[\begin{array}{l} x-x_{s} \\ y-y_{s} \\ \theta-\theta_{s} \end{array}\right]+\left(\begin{array}{ll} \frac{\partial f_{1}}{\partial v} & \frac{\partial f_{1}}{\partial \delta} \\ \frac{\partial f_{2}}{\partial v} & \frac{\partial f_{2}}{\partial \delta} \\ \frac{\partial f_{3}}{\partial v} & \frac{\partial f_{3}}{\partial \delta} \end{array}\right)\left[\begin{array}{l} v-v_{s} \\ \delta-\delta_{s} \end{array}\right]$

整理可以得到如下系統方程：

$\Delta \dot{X}=A_{m} \Delta X+B_{m} \Delta u$

$系統狀態矩陣: A_{m}=\left[\begin{array}{ccc} 0 & 0 & -v \sin \theta \\ 0 & 0 & v \cos \theta \\ 0 & 0 & 0 \end{array}\right]\\ 系統輸入矩陣: B_{m}=\left[\begin{array}{cc} \cos \theta & 0 \\ \sin \theta & 0 \\ \frac{\tan \delta}{L} & \frac{v}{L \cos ^{2} \delta} \end{array}\right]$

假設控制週期為

$\Delta T$

，那麼可以近似得到：

$\Delta\dot{\boldsymbol{x}}=\frac{\Delta\boldsymbol{x}(k+1)-\Delta\boldsymbol{x}(k)}{\Delta T}=A_m \Delta\boldsymbol{x}(k)+B_m \Delta\boldsymbol{u}(k) \\ \Delta\boldsymbol{x}(k+1)=(\mathbb{I}+\Delta T \cdot A) \Delta\boldsymbol{x}(k)+\Delta T \cdot B \Delta\boldsymbol{u}(k)=A_{k} \Delta\boldsymbol{x}(k)+B_{k} \Delta\boldsymbol{u}(k)$

其中：

$系統狀態矩陣: A_{k}=\left[\begin{array}{ccc} 1 & 0 & -v \Delta T\sin \theta \\ 0 & 1 & v \Delta T\cos \theta \\ 0 & 0 & 1 \end{array}\right]\\ 系統輸入矩陣: B_{k}=\left[\begin{array}{cc} \Delta T\cos \theta & 0 \\ \Delta T\sin \theta & 0 \\ \frac{ \Delta T\tan \delta}{L} & \frac{ \Delta Tv}{L \cos ^{2} \delta} \end{array}\right]$

使用MPC來做預測未來h步的線性模型如下：

$\begin{aligned} \Delta\boldsymbol{x}(1) &=A_{0} \Delta\boldsymbol{x}(0)+B_{0} \Delta\boldsymbol{u}(0) \\ \Delta\boldsymbol{x}(2) &=A_{1} A_{0} \Delta\boldsymbol{x}(0)+A_{1} B_{0} \Delta\boldsymbol{u}(0)+B_{1} \Delta\boldsymbol{u}(1) \\ \Delta\boldsymbol{x}(3) &=A_{2} A_{1} A_{0} \Delta\boldsymbol{x}(0)+A_{2} A_{1} B_{0}\Delta \boldsymbol{u}(0)+A_{2} B_{1} \Delta\boldsymbol{u}(1)+B_{2} \Delta\boldsymbol{u}(2) \\ \Delta\boldsymbol{x}(4) &=A_{3} A_{2} A_{1} A_{0} \Delta\boldsymbol{x}(0)+A_{3} A_{2} A_{1} B_{0} \Delta\boldsymbol{u}(0)+A_{3} A_{2} B_{1} \Delta\boldsymbol{u}(1)+A_{3} B_{2} \Delta\boldsymbol{u}(2)+B_{3} \Delta\boldsymbol{u}(3) \\ & \vdots \\ \Delta\boldsymbol{x}(h) &=\left(\prod_{i=h-1}^{0} A_{i}\right) \Delta\boldsymbol{x}(0)+\sum_{i=0}^{h-2}\left[\left(\prod_{j=h-1}^{i+1} A_{j}\right) B_{i} \Delta\boldsymbol{u}(i)\right]+B_{h-1} \Delta\boldsymbol{u}(h-1) \end{aligned}$

寫成緊湊型如下：

$\Delta X=A_{qp}\boldsymbol{x}(0)+B_{qp}\boldsymbol{U}$

其中引數矩陣如下：

$\Delta\mathrm{X}=\left[\begin{array}{c} \Delta\boldsymbol{x}(1) \\ \Delta\boldsymbol{x}(2) \\ \vdots \\ \Delta\boldsymbol{x}(h) \end{array}\right], A_{q p}=\left[\begin{array}{c} A_{0} \\ A_{1} A_{0} \\ A_{2} A_{1} A_{0} \\ \vdots \\ \prod_{i=h-1}^{0} A_{i} \end{array}\right], B_{q p}=\left[\begin{array}{cccc} B_{0} & \mathbf{0} & \cdots & \mathbf{0} \\ A_{1} B_{0} & B_{1} & \cdots & \mathbf{0} \\ \vdots & \vdots & \ddots & \mathbf{0} \\ \left(\prod_{j=h-1}^{1} A_{j}\right) B_{0} & \left(\prod_{j=h-1}^{2} A_{j}\right) B_{1} & \cdots & B_{h-1} \end{array}\right], \mathrm{U}=\left[\begin{array}{c} \Delta\boldsymbol{u}(0) \\ \Delta\boldsymbol{u}(1)\\ \vdots \\ \Delta\boldsymbol{u}(h) \end{array}\right]$

非線性模型的MPC

$\begin{array}{c} \dot{x}=v \cos (\theta) \\ \dot{y}=v \sin (\theta) \\ \dot{\theta}=v \frac{\tan (\delta)}{L}\\ \dot{v}=a\\ v 是無人車速度，v_x和v_y在世界座標系下的速度分量\\ \theta 是yaw 方向偏航角,\dot{\theta} 是角速度，記作\omega \end{array}$

其離散差分模型，其中差分時間間隔或者控制週期為dt如下：

$\begin{array}{c} 世界座標系下: x_{k+1}=x_{k}+v_{k} \cos \left(\theta_{k}\right) d_{t} \\ 世界座標系下：y_{k+1}=y_{k}+v_{k} \sin \left(\theta_{k}\right) d_{t} \\ 偏航角: \theta_{k+1}=\theta_{k}+v_{k} \frac{\tan \left(\delta_{k}\right)}{L} d_{t} \\ 世界座標系下速度: v_{k+1}=v_{k}+a_{k} d_{t} \\ 橫向誤差: \epsilon_{k+1}=\epsilon_{k}+v_{k} \sin \left(\theta_{k}\right) d_{t} \\ 偏航角誤差：\sigma_{k+1}=\sigma_{k}+v_{k} \frac{\tan \left(\delta_{k}\right)}{L} d_{t} \end{array}$

e 為橫向誤差，車與目標軌跡的距離

$d(t)和速度相關，一般是d(t)=v/k,k 是增益引數,v 是車的速度，\delta_e表示打舵誤差，\theta_e表示車的偏航角$

這裡無人車追蹤的目標軌跡是一個三階曲線：

，這裡需要把軌跡轉到車本體座標系下，這裡的每個連續點，可以看成一個個離散的點，我們目的就是讓車與軌跡中的

儘量靠近重合，方向和切線方向上重合。而第k 次的橫向誤差和偏航角誤差如下：

$\begin{array}{c} 第k個橫向誤差: \epsilon_{k}=f(x_{k})-y_k\\ 第k個偏航角誤差：\sigma_{k}=\arctan \left(f^{\prime}\left(x_{k}\right)\right)-\theta_k \end{array}$

對於一個預測步長為h的MPC控制器求解問題，根據倉庫所述最佳化目標函式如下，主要分成三層，和目標變數的誤差最小，系統輸入越小越好，系統輸入之間不能有太大波動：

$\min \begin{aligned} \mathcal{J}=& \sum_{k=1}^{N}\left(\omega_{\epsilon} \| \epsilon_{t}-0\left\|^{2}+\omega_{\sigma}\right\| \operatorname{\sigma}_{k}-0\left\|^{2}+\omega_{v}\right\| v_{k}-v_{\text {ref }} \|^{2}\right) \\ & \quad+\sum_{k=1}^{N-1}\left(\omega_{\delta}\left\|\delta_{k}\right\|^{2}+\omega_{a}\left\|a_{k}\right\|^{2}\right) \\ +& \sum_{k=1}^{N-2}\left(\omega_{\text {rate }_{\delta}}\left\|\delta_{k+1}-\delta_{k}\right\|^{2}+\omega_{\text {rate }_{a}}\left\|a_{k+1}-a_{k}\right\|^{2}\right) \end{aligned}$

需要滿足動態模型約束為：

$s.t. \quad x_{k+1}=x_{k}+v_{k} \cos \left(\theta_{k}\right) d_{t}, k=1,2, \ldots, N-1\\ y_{k+1}=y_{k}+v_{k} \sin \left(\theta_{k}\right) d_{t}, k=1,2, \ldots, N-1\\ \theta_{k+1}=\theta_{k}+v_{k} \frac{\tan \left(\delta_{k}\right)}{L} d_{t}, k=1,2, \ldots, N-1\\ v_{k+1}=v_{k}+a_{k} d_{t}, k=1,2, \ldots, N-1\\ \operatorname{\epsilon}_{k+1}=f\left(x_{k}\right)-y_{k}+v_{k} \sin \left(\theta_{k}\right) d_{t}\\ \mathrm{\sigma}_{k+1}=\arctan \left(f^{\prime}\left(x_{k}\right)\right)-\theta+v_{k} \frac{\tan \left(\delta_{k}\right)}{L} d_{t}\\ 方向盤角度約束:\delta \in\left[\delta_{\min }, \delta_{\max }\right]\\ 加速度約束: a \in\left[a_{\min }, a_{\max }\right]\\$

結論：

從實驗效果看，mpc表示還是可以的，但是在控制過程中會出現抖動，有可能是我沒有調好引數。Python版本的我也進行了嘗試，很可惜不知道為啥只能收資料，不能發資料到模擬平臺。

參考文獻(Reference)：

1。［self-driving-car-sim］（

https：//

github。com/udacity/self

-driving-car-sim

）

2。［DhruvaKumar MPC］（

https：//

github。com/DhruvaKumar/

model-predictive-control

）

3。［windSes Blog］（

https：//

windses。blog。csdn。net/a

rticle/details/103519721#comments_20178791

）

4。［QSQP］（

https：//

osqp。org/docs/examples/

index。html

）

5。［Jesse_stanly_method］（

https：//

blog。csdn。net/renyushua

i900/article/details/98460758

）

6。［Gnuplot］（

http：//

stahlke。org/dan/gnuplot

-iostream/

）

7。［matplotlib_cpp］（

https：//

github。com/lava/matplot

lib-cpp

）

8。無人駕駛演算法——使用Stanley method實現無人車軌跡追蹤

標簽： MPC 模型 https 如下倉庫

上一篇:誰能打個比方，用來描述美觀和需求（包括易用性可用性）相悖的好例子？

下一篇：虎牙直播的大神推薦怎麼獲得？虎牙直播推薦位多少錢一天？

以無人駕駛小車車為例子深刻理解MPC

猜你喜歡

古代的八珍都有哪些？

邵通旅遊景點一覽

自抗擾控制演算法（一）— Simulink實現

蘋果電腦如何開啟桌面顯示【硬碟】專案？

什麼樂器適合彈電音？