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

一、MPC的力學原理

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

1、牛頓公式:

基本公式:

F=mdvcdt=mv˙cF =m\frac{dv_c}{dt}=m\dot v_c

展開形式(ncn_c爲與地面接觸點的數量):

m[x¨y¨z¨]=i=1nc[fxifyifzi][00g]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}

將質量移至右邊,可求得加速度:

[x¨y¨z¨]=i=1nc[fxi/mfyi/mfzi/m][00g/m]\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}

寫成更簡潔的形式即:

P¨=i=1ncfimgm(1)\ddot P = \sum_{i=1}^{n_c} \frac{f_i}{m} - \frac{g}{m} \tag 1

2、歐拉公式

基本公式(ω\omega爲角速度,II爲轉動慣量):

N=dIωdt= CIω˙+ω× CIωN = \frac{dI\omega}{dt} = \ ^CI \dot \omega + \omega \times \ ^CI\omega

對上式做近似考慮:

dIωdt=i=1ncri×fiIΘ¨\frac{dI\omega}{dt} = \sum_{i=1}^{nc}\mathrm{r_i} \times \mathrm{f_i} \approx I \ddot \Theta

因此有展開式:

IȨ=i=1nc[0rziryirzi0rxiryirxi0][fxifyifzi]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

Θ¨=i=1ncI1[0rziryirzi0rxiryirxi0][fxifyifzi](2)\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較小,因此有以下近似變換:

角度變換:

Θ˙GRz(γ)Θ˙B \dot \Theta_G \approx R_z(\gamma)\dot \Theta_B

慣量矩陣變換:

IGRz(γ)IBRz(γ)TI_G \approx R_z(\gamma) I_B R_z(\gamma)^T

繞Z軸的變換的旋轉矩陣:
Rz(γ)=[cosγsinγ0sinγcosγ0001]R_z(\gamma) = \begin{bmatrix} \cos \gamma & -\sin \gamma &0 \\ \sin \gamma & \cos \gamma &0 \\ 0& 0&1\end{bmatrix}

Mit實驗室給出的cheetah的轉動慣量:
IB=[0.070000.260000.242]I_B = \begin{bmatrix} 0.07 & 0& 0 \\ 0 & 0.26 & 0 \\ 0&0&0.242\end{bmatrix}

4、狀態迭代方程

綜合公式(1)所計算的加速度P¨\ddot P,公式(2)中所計算的角加速度Θ¨\ddot \Theta,規定右上標爲時刻kk,右下標爲所在參考系

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

  • 旋轉角:
    Θk+1=Θk+Rzk(γ)Θ˙BΔt(3)\Theta^{k+1} = \Theta^{k} + R^k_z(-\gamma) \dot \Theta_B\Delta t \tag 3

  • 位置:Pk+1=Pk+P˙kΔt(4)P^{k+1} = P^{k} +\dot P^{k} \Delta t \tag4

  • 角速度:Θ˙k+1=Θ˙k+Θ¨kΔt(5)\dot \Theta^{k+1} = \dot \Theta^{k} + \ddot \Theta^{k} \Delta t \tag 5

  • 速度:P˙k+1=P˙k+P¨kΔt(6)\dot P^{k+1} = \dot P^{k} + \ddot P^k\Delta t \tag6

將式(3)~(6)整理寫成矩陣形式如下:

[ΘPΘ˙P˙]Gk+1=Ak[ΘPΘ˙P˙]Gk+Bk[f1..fn]k+[030303g](7)\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

其中:

Ak=[1303Rz(γ)Δt0303130313Δt0303130303030313]kA_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}

Bk=[03...0303...03IGr1~Δt...IGrn~Δt13Δt/m...13Δt/m]kB_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)=Akx(k)+Bkf(k)+g(8)x(k+1) = A_kx(k)+B_kf(k)+g \tag 8

其中:

x=[ΘPΘ˙P˙]x = \begin{bmatrix} \Theta \\ P \\ \dot \Theta \\ \dot P\end{bmatrix}

此時式(8)是一個線性方程,我們根據其計算未來h步狀態:

在這裏插入圖片描述

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

X=Axk+Bf+g(9)\mathbf{X} = \mathbf{A}x_k + \mathbf{B}\mathbf{f} + \mathbf g \tag 9

其中:

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

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

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

B=[B000ABB00A2BABB0Ah1BAh2BAh3B]\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}

g=[gAg+gA2g+Ag+gAk+h1g++g]\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),構造代價函數如下,由軌跡誤差項和控制項組成:

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

式中L,KL,K分別爲權重矩陣(對角矩陣),由於狀態X\mathbf X是一個矩陣,安裝規範寫法,表達如下:

J=(XXref)TL(XXref)+fTKf(10)\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}

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

minx,fJ(11)\underset{\mathbf x,\mathbf f}{min} \quad \mathbf J \tag{11}

約束如下:

fxμfz,fyμfz,fz<fmax|f_x| \leq \mu f_z, \quad |f_y| \leq \mu f_z, \quad f_z < f_{max}

將式(10)展開,去掉與控制量f\mathbf f無關的項,可化成標準二次型如下:

minf12fTHf+RTfs.t.cminCfcmax(12)\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}

其中:

H=2(BTLB+K)R=2BTL(Axk+gXref)\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)求解,便能得到滿足約束條件下的最優控制序列f=[fkfk+1fk+h1]\mathbf f=\begin{bmatrix}f_k& f_{k+1}& \cdots & f_{k+h-1} \end{bmatrix}

三、二次規劃求解

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

對於二次多項式:

f(x)=2x12+x22+x1x2+x1+x2f(x) = 2x_1^2 + x_2^2 + x_1x_2 + x_1 + x_2

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

minx2x12+x22+x1x2+x1+x2s.t.x10x20x1+x2=1\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}

將其化爲以下標準形式:

minx12xTPx+qTxs.t.GxhAx=b\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}

即:
minx0.5[x1x2][4112][x1x2]+[11][x1x2]s.t.x10x20x1+x2=1\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=[1001],h=[00],A=[11],b=1G = \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]
發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章