MIT開源項目Cheetah3-MPC控制部分原理解析

MPC控制

我們的 MPC 的目標是找到反作用力，使質點遵循給定的軌跡。在 MPC 的優化過程中，步態調度器和步長規劃器預先定義了接觸序列。 這讓公式保持凸性，使得優化問題求解速度快，並且可以始終求解到唯一的全局最小值。 而在非線性優化中並不總是可以得到的。

1、線性模型

由於力臂的交叉積項和轉向動力學，即使是簡單的質點模型也不是完全線性的。爲了實現線性MPC公式，我們應用了三個簡化。

a、假設滾動和俯仰角很小。

基於這個假設，我們可以簡化座標變換如下：

$\dot{\Theta} \approx R_z(\psi)\omega \tag{4}$

$_gI \approx R_z(\psi)_BIR_z(\psi)^T \tag{5}$

其中

• $\dot{\Theta} = [\dot{\phi } \quad \dot{\theta} \quad \dot{\psi}] ^T$：爲機體的角速度（分別對應roll，pitch，yaw）
• $R_z(\psi)$：旋轉矩陣，將角速度映射到世界座標系
• $\omega$：機身座標系下的角速度
• $_gI$：世界座標系下的慣性張量
• $_BI$：機身座標系下的慣性張量

b、狀態與指定軌跡接近

我們利用旋轉矩陣中的指令$\psi$，創建了時變線性動力學方程，並且利用命令的軌跡和當前步位置所計算出的預定軌跡設置等式(2)中的力臂。

c、俯仰和滾轉速度都很小， 慣性張量的非對角線項也很小。

基於上兩個假設，等式2近似於：

$\frac{d}{d_t}(I\omega) = I\dot{\omega} + \omega \times(I\omega) \approx I\dot{\omega} \tag{6}$

2、構建優化問題

將1中的簡化結果整理成以下三條公式：
$\Theta = R_z^T(\psi)\omega \tag{a}$

$\sum_{i=1}^{n}r_i \times f_i = I\dot{\omega} \tag{b}$

$\ddot{p} = \frac{\sum_{i=1}^{n}f_i}{m} - g \tag{c}$

將式（a）~（b）組合寫成矩陣計算形式如下：

$\begin{bmatrix} \Theta \\ \mathbf{p}\\ \omega\\ \dot{p} \end{bmatrix} = A\begin{bmatrix} \Theta \\ \mathbf{p}\\ \omega\\ \dot{p} \end{bmatrix} + B\begin{bmatrix} f_1\\ .\\ .\\ f_n\end{bmatrix} + \begin{bmatrix} 0_3\\ 0_3\\ 0_3\\ g \end{bmatrix}$

其中：

$A = \begin{bmatrix} 1_{3\times3} & 0_{3\times3} & R_z(\psi_k)\Delta t & 0_{3\times3}\\ 0_{3\times3} & 1_{3\times3} & 0_{3\times3} & 1_{3\times3}\Delta t\\ 0_{3\times3} & 0_{3\times3} & 1_{3\times3} & 0_{3\times3}\\ 0_{3\times3} & 0_{3\times3} & 0_{3\times3} & 1_{3\times3} \end{bmatrix}$

$B = \begin{bmatrix} 0_{3\times3} & ... & 0_{3\times3}\\ 0_{3\times3} & ... & 0_{3\times3}\\ _gI_{[r1]\times\Delta t} & ... & _gI_{[rn]\times\Delta t}\\ 1_{3\times3}\Delta t/m & ... & 1_{3\times3}\Delta t/m \end{bmatrix}$

$_{[r_n]}$爲反對稱矩陣，形式如下：
$\begin{bmatrix} 0 & -r_z & r_y\\ r_z & 0 & -r_x\\ -r_y & r_x & 0 \end{bmatrix}$

簡化表達爲一下形式：

$x(k+1)= A_kx(k) + B_kf(k) + g$

爲了減小問題的大小，我們消除了對應於不接觸地面的腳的地面 反作用力的變量。 這減少了成本和約束矩陣的大小，在實踐 中，我們的速度超過 10 倍。

3、最優化問題求解

假設參考軌跡如下(h爲軌跡長度)：

$\bar{\chi} = \begin{bmatrix} \bar{x_{k+1}} & \bar{x_{k+2}}& \bar{x_{k+3}}& ...& \bar{x_{k+h}} \end{bmatrix}^T$

將2中的線性模型表達成以下形式：
$X = A_{qp}X_0 +B_{qp}U$

$X \in \mathbb{R}^{13k}$ 是表示狀態的軌跡的向量（多個預測狀態的集合），$U \in \mathbb{R}^{3nk}$是控制量（地面反作用力）。則MPC的一個簡單的目標代價函數如下：

$\begin{matrix} \underset{U}{\min} & \frac{1}{2}U^THU +U^Tg\\ \\ s.t. & c_{min} \leq CU \leq c_{max} \end{matrix}$

其中$C$是一個約束矩陣，且：

$\begin{matrix} H = 2(B^T_{qp}LB_{qp} + K)\\ \\ g = 2B^T_{qp}L(A_{qp}x_0 - y) \end{matrix}$

$L \in \mathbb{R}^{13k\times13k}$爲狀態偏離權重，是一個對角矩陣，$K\in \mathbb{R}^{3nk\times3nk}$爲力大小的一個對角權重矩陣。

構建好最優化問題後，可有二次規劃的方法求解，得到滿足目標代價函數的最優控制序列：

$U = \begin{bmatrix} u_k & u_{k+1} & u_{k+2}& u_{k+3}& ... & u_{k+h-1} \end{bmatrix}$

取第一組數組作爲輸出值作爲期望地面反作用力，即$u_k$