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

MPC控制

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

1、線性模型

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

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

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

Θ˙Rz(ψ)ω(4)\dot{\Theta} \approx R_z(\psi)\omega \tag{4}

gIRz(ψ)BIRz(ψ)T(5)_gI \approx R_z(\psi)_BIR_z(\psi)^T \tag{5}

其中

  • Θ˙=[ϕ˙θ˙ψ˙]T\dot{\Theta} = [\dot{\phi } \quad \dot{\theta} \quad \dot{\psi}] ^T:爲機體的角速度(分別對應roll,pitch,yaw)
  • Rz(ψ)R_z(\psi):旋轉矩陣,將角速度映射到世界座標系
  • ω\omega:機身座標系下的角速度
  • gI_gI:世界座標系下的慣性張量
  • BI_BI:機身座標系下的慣性張量

b、狀態與指定軌跡接近

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

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

基於上兩個假設,等式2近似於:

ddt(Iω)=Iω˙+ω×(Iω)Iω˙(6)\frac{d}{d_t}(I\omega) = I\dot{\omega} + \omega \times(I\omega) \approx I\dot{\omega} \tag{6}

2、構建優化問題

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

i=1nri×fi=Iω˙(b)\sum_{i=1}^{n}r_i \times f_i = I\dot{\omega} \tag{b}

p¨=i=1nfimg(c)\ddot{p} = \frac{\sum_{i=1}^{n}f_i}{m} - g \tag{c}

將式(a)~(b)組合寫成矩陣計算形式如下:

[Θpωp˙]=A[Θpωp˙]+B[f1..fn]+[030303g]\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=[13×303×3Rz(ψk)Δt03×303×313×303×313×3Δt03×303×313×303×303×303×303×313×3]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=[03×3...03×303×3...03×3gI[r1]×Δt...gI[rn]×Δt13×3Δt/m...13×3Δt/m]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}

[rn]_{[r_n]}爲反對稱矩陣,形式如下:
[0rzryrz0rxryrx0]\begin{bmatrix} 0 & -r_z & r_y\\ r_z & 0 & -r_x\\ -r_y & r_x & 0 \end{bmatrix}

簡化表達爲一下形式:

x(k+1)=Akx(k)+Bkf(k)+gx(k+1)= A_kx(k) + B_kf(k) + g

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

3、最優化問題求解

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

χˉ=[xk+1ˉxk+2ˉxk+3ˉ...xk+hˉ]T\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=AqpX0+BqpUX = A_{qp}X_0 +B_{qp}U

XR13kX \in \mathbb{R}^{13k} 是表示狀態的軌跡的向量(多個預測狀態的集合),UR3nkU \in \mathbb{R}^{3nk}是控制量(地面反作用力)。則MPC的一個簡單的目標代價函數如下:

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

其中CC是一個約束矩陣,且:

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

LR13k×13kL \in \mathbb{R}^{13k\times13k}爲狀態偏離權重,是一個對角矩陣,KR3nk×3nkK\in \mathbb{R}^{3nk\times3nk}爲力大小的一個對角權重矩陣。

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

U=[ukuk+1uk+2uk+3...uk+h1]U = \begin{bmatrix} u_k & u_{k+1} & u_{k+2}& u_{k+3}& ... & u_{k+h-1} \end{bmatrix}

取第一組數組作爲輸出值作爲期望地面反作用力,即uku_k

發表評論
所有評論
還沒有人評論,想成為第一個評論的人麼? 請在上方評論欄輸入並且點擊發布.
相關文章