【四足機器人那些事2】MiniCheetah中的MPC控制

一、MPC的力學原理

剛體的力與加速度，轉矩與角加速度可以通過牛頓方程和歐拉方程求出：

1、牛頓公式：

基本公式：

$F =m\frac{dv_c}{dt}=m\dot v_c$

展開形式（$n_c$爲與地面接觸點的數量）：

$m\begin{bmatrix} \ddot x\\ \ddot y\\ \ddot z \end{bmatrix} = \sum_{i=1}^{n_c}\begin{bmatrix} f_{x_i} \\ f_{y_i} \\ f_{z_i} \end{bmatrix}- \begin{bmatrix} 0\\ 0\\ g \end{bmatrix}$

將質量移至右邊，可求得加速度：

$\begin{bmatrix} \ddot x\\ \ddot y\\ \ddot z \end{bmatrix} = \sum_{i=1}^{n_c}\begin{bmatrix} f_{x_i}/m \\ f_{y_i}/m \\ f_{z_i}/m \end{bmatrix}- \begin{bmatrix} 0\\ 0\\g/m \end{bmatrix}$

寫成更簡潔的形式即：

$\ddot P = \sum_{i=1}^{n_c} \frac{f_i}{m} - \frac{g}{m} \tag 1$

2、歐拉公式

基本公式($\omega$爲角速度，$I$爲轉動慣量)：

$N = \frac{dI\omega}{dt} = \ ^CI \dot \omega + \omega \times \ ^CI\omega$

對上式做近似考慮：

$\frac{dI\omega}{dt} = \sum_{i=1}^{nc}\mathrm{r_i} \times \mathrm{f_i} \approx I \ddot \Theta$

因此有展開式：

$I\ddot \Theta = \sum_{i=1}^{nc} \begin{bmatrix} 0 & -r_{z_i} & r_{y_i} \\ r_{z_i} & 0& -r_{x_i} \\ -r_{y_i} &r_{x_i} & 0 \end{bmatrix}\begin{bmatrix} f_{x_i} \\ f_{y_i} \\ f_{z_i}\end{bmatrix}$

其中：

$\Theta = \begin{bmatrix} \alpha \\ \beta \\ \gamma \end{bmatrix}$

同樣將轉動慣量移至右邊，得到角加速度$\ddot \Theta$

$\ddot \Theta = \sum_{i=1}^{nc} I^{-1}\begin{bmatrix} 0 & -r_{z_i} & r_{y_i} \\ r_{z_i} & 0& -r_{x_i} \\ -r_{y_i} &r_{x_i} & 0 \end{bmatrix}\begin{bmatrix} f_{x_i} \\ f_{y_i} \\ f_{z_i}\end{bmatrix} \tag 2$

3、對不同量的參考系作近似變換

爲了儘可能讓系統保持線性關係，同時由於運動過程中滾轉角$\alpha$和俯仰角$\beta$較小，因此有以下近似變換：

角度變換：

$\dot \Theta_G \approx R_z(\gamma)\dot \Theta_B$

慣量矩陣變換：

$I_G \approx R_z(\gamma) I_B R_z(\gamma)^T$

繞Z軸的變換的旋轉矩陣：
$R_z(\gamma) = \begin{bmatrix} \cos \gamma & -\sin \gamma &0 \\ \sin \gamma & \cos \gamma &0 \\ 0& 0&1\end{bmatrix}$

Mit實驗室給出的cheetah的轉動慣量：
$I_B = \begin{bmatrix} 0.07 & 0& 0 \\ 0 & 0.26 & 0 \\ 0&0&0.242\end{bmatrix}$

4、狀態迭代方程

綜合公式（1）所計算的加速度$\ddot P$，公式（2）中所計算的角加速度$\ddot \Theta$，規定右上標爲時刻$k$，右下標爲所在參考系

各狀態量之間的迭代方式如下：

• 旋轉角：
  $\Theta^{k+1} = \Theta^{k} + R^k_z(-\gamma) \dot \Theta_B\Delta t \tag 3$

• 位置：$P^{k+1} = P^{k} +\dot P^{k} \Delta t \tag4$

• 角速度：$\dot \Theta^{k+1} = \dot \Theta^{k} + \ddot \Theta^{k} \Delta t \tag 5$

• 速度：$\dot P^{k+1} = \dot P^{k} + \ddot P^k\Delta t \tag6$

將式（3）~（6）整理寫成矩陣形式如下：

$\begin{bmatrix} \Theta \\ P\\ \dot \Theta\\ \dot{P} \end{bmatrix}^{k+1}_G = A_k\begin{bmatrix} \Theta \\ P \\ \dot \Theta \\ \dot{P} \end{bmatrix}_G^{k} + B_k\begin{bmatrix} f_1\\ .\\ .\\ f_n\end{bmatrix}^{k} + \begin{bmatrix} 0_3\\ 0_3\\ 0_3\\ g \end{bmatrix} \tag 7$

其中：

$A_k = \begin{bmatrix} 1_{3} & 0_{3} & R_z(\gamma)\Delta t & 0_{3}\\ 0_{3} & 1_{3} & 0_{3} & 1_{3}\Delta t\\ 0_{3} & 0_{3} & 1_{3} & 0_{3}\\ 0_{3} & 0_{3} & 0_{3} & 1_{3} \end{bmatrix}^{k}$

$B_k = \begin{bmatrix} 0_{3} & ... & 0_{3}\\ 0_{3} & ... & 0_{3}\\ I_G \tilde{r_1}\Delta t & ... &I_G \tilde{r_n}\Delta t \\ 1_{3}\Delta t/m & ... & 1_{3}\Delta t/m \end{bmatrix}^ k$

二、構建二次規劃問題

1、完整軌跡表示

我們式（7）中的狀態迭代方程寫成如下形式 ：

$x(k+1) = A_kx(k)+B_kf(k)+g \tag 8$

其中：

$x = \begin{bmatrix} \Theta \\ P \\ \dot \Theta \\ \dot P\end{bmatrix}$

此時式（8）是一個線性方程，我們根據其計算未來h步狀態：

將上式寫成矩陣運算形式：

$\mathbf{X} = \mathbf{A}x_k + \mathbf{B}\mathbf{f} + \mathbf g \tag 9$

其中：

$\mathbf{X} = \begin{bmatrix}x_{k+1} & x_{k+2} &x_{k+3}& \cdots& x_{k+h} \end{bmatrix}^T$

$\mathbf{f} = \begin{bmatrix} f_k & f_{k+1} & f_{k+2} & \cdots & f_{k+h} \end{bmatrix}^T$

$\mathbf{A} = \begin{bmatrix} A & A^2 & A^3 & \cdots & A^h \end{bmatrix}^T$

$\mathbf{B} = \begin{bmatrix} B & 0 & 0 & \cdots & 0\\ AB & B & 0 & \cdots & 0\\ A^2B & AB & B & \cdots & 0\\ \vdots & \vdots & \vdots& \vdots & \vdots\\ A^{h-1}B & A^{h-2}B & A^{h-3} & \cdots & B \end{bmatrix}$

$\mathbf{g} = \begin{bmatrix} g\\ Ag+g\\ A^2g+ Ag + g\\ \vdots\\ A^{k+h-1}g+ \cdots +g \end{bmatrix}$

現在我們得到了未來h步軌跡的狀態矩陣以及其計算表達式（9）

2、二次規劃問題

根據式（9），構造代價函數如下，由軌跡誤差項和控制項組成：

$\mathbf J = ( \mathbf X- \mathbf X^{ref})^2 \cdot L+\mathbf f^2 \cdot K$

式中$L,K$分別爲權重矩陣（對角矩陣），由於狀態$\mathbf X$是一個矩陣，安裝規範寫法，表達如下：

$\mathbf J = ( \mathbf X- \mathbf X^{ref})^T \cdot L \cdot ( \mathbf X- \mathbf X^{ref})+\mathbf f^T \cdot K \cdot \mathbf f\tag {10}$

接下來構造一個二次規劃如下（最小化代價函數），該式的物理意義在於，使得計算軌跡儘可能與參考軌跡重合的同時，所使用到的力最小：

$\underset{\mathbf x,\mathbf f}{min} \quad \mathbf J \tag{11}$

約束如下：

$|f_x| \leq \mu f_z, \quad |f_y| \leq \mu f_z, \quad f_z < f_{max}$

將式（10）展開，去掉與控制量$\mathbf f$無關的項，可化成標準二次型如下：

$\begin{matrix} \underset{\mathbf f}{\min} & \frac{1}{2}\mathbf f^T \mathbf H \mathbf f + \mathbf R^T \mathbf f\\ \\ s.t. & c_{min} \leq C \mathbf f \leq c_{max} \end{matrix} \tag {12}$

其中：

$\begin{matrix} \mathbf H = 2(\mathbf B^TL \mathbf B + K)\\ \\ \mathbf R = 2\mathbf B^TL(\mathbf A x_k + \mathbf g- \mathbf X^{ref}) \end{matrix}$

利用式（12）求解，便能得到滿足約束條件下的最優控制序列$\mathbf f=\begin{bmatrix}f_k& f_{k+1}& \cdots & f_{k+h-1} \end{bmatrix}$

三、二次規劃求解

由於式（12）中數據量比較大，且與機器人系統的配置有關，因此我們從以下簡單案例進行講解：

對於二次多項式：

$f(x) = 2x_1^2 + x_2^2 + x_1x_2 + x_1 + x_2$

求解其在約束條件下的最小值，即爲二次規劃問題，有以下表達：

$\begin{matrix} \underset{x}{\min} &2x_1^2 + x_2^2 + x_1x_2 + x_1 + x_2\\ \\ s.t. & x_1 \geq 0 \\ \\ & x_2 \geq 0 \\ \\& x_1+x_2 =1\end{matrix}$

將其化爲以下標準形式：

$\begin{matrix} \underset{x}{\min} & \frac{1}{2} x^T \mathbf P x + \mathbf q^T x\\ \\ s.t. & Gx \leq h \\ \\ & Ax = b \end{matrix}$

即：
$\begin{matrix} \underset{x}{\min} &0.5 \begin{bmatrix} x_1& x_2 \end{bmatrix}\begin{bmatrix} 4 & 1\\ 1 & 2 \end{bmatrix}\begin{bmatrix} x_1\\ x_2 \end{bmatrix}+ \begin{bmatrix} 1 & 1 \end{bmatrix}\begin{bmatrix} x_1\\ x_2 \end{bmatrix}\\ \\ s.t. &- x_1 \leq 0 \\ \\ & -x_2 \leq 0 \\ \\& x_1+x_2 =1\end{matrix}$

根據約束條件可以得出：

$G = \begin{bmatrix} -1&0 \\ 0&-1 \end{bmatrix},h = \begin{bmatrix} 0 \\ 0 \end{bmatrix},A = \begin{bmatrix} 1 & 1 \end{bmatrix},b = 1$

利用python的cvxopt庫可求解該問題：

from cvxopt import matrix, solvers


P = matrix([[4.0, 1.0], [1.0, 2.0]])
q = matrix([1.0, 1.0])
G = matrix([[-1.0, 0.0], [0.0, -1.0]])
h = matrix([0.0, 0.0])
A = matrix([1.0, 1.0], (1, 2))
b = matrix([1.0])
result = solvers.qp(P, q, G, h, A, b)

print('x:\n', result['x'])

結果如下：

     pcost       dcost       gap    pres   dres
 0:  1.8889e+00  7.7778e-01  1e+00  3e-16  2e+00
 1:  1.8769e+00  1.8320e+00  4e-02  2e-16  6e-02
 2:  1.8750e+00  1.8739e+00  1e-03  2e-16  5e-04
 3:  1.8750e+00  1.8750e+00  1e-05  6e-17  5e-06
 4:  1.8750e+00  1.8750e+00  1e-07  2e-16  5e-08
Optimal solution found.
x: 
[ 2.50e-01]
[ 7.50e-01]