MIT开源项目Cheetah3-MPC控制部分原理解析
MPC控制
我们的 MPC 的目标是找到反作用力，使质点遵循给定的轨迹。在 MPC 的优化过程中，步态调度器和步长规划器预先定义了接触序列。 这让公式保持凸性，使得优化问题求解速度快，并且可以始终求解到唯一的全局最小值。 而在非线性优化中并不总是可以得到的。
1、线性模型
由于力臂的交叉积项和转向动力学，即使是简单的质点模型也不是完全线性的。为了实现线性MPC公式，我们应用了三个简化。
a、假设滚动和俯仰角很小。
基于这个假设，我们可以简化坐标变换如下：
Θ ˙ ≈ R z ( ψ ) ω (4) \dot{\Theta} \approx R_z(\psi)\omega \tag{4} Θ˙≈Rz(ψ)ω(4)
g I ≈ R z ( ψ ) B I R z ( ψ ) T (5) _gI \approx R_z(\psi)_BIR_z(\psi)^T \tag{5} gI≈Rz(ψ)BIRz(ψ)T(5)
其中
- Θ ˙ = [ ϕ ˙ θ ˙ ψ ˙ ] T \dot{\Theta} = [\dot{\phi } \quad \dot{\theta} \quad \dot{\psi}] ^T Θ˙=[ϕ˙θ˙ψ˙]T：为机体的角速度（分别对应roll，pitch，yaw）
- R z ( ψ ) R_z(\psi) Rz(ψ)：旋转矩阵，将角速度映射到世界坐标系
- ω \omega ω：机身坐标系下的角速度
- g I _gI gI：世界坐标系下的惯性张量
- B I _BI BI：机身坐标系下的惯性张量
b、状态与指定轨迹接近
我们利用旋转矩阵中的指令 ψ \psi ψ，创建了时变线性动力学方程，并且利用命令的轨迹和当前步位置所计算出的预定轨迹设置等式(2)中的力臂。
c、俯仰和滚转速度都很小， 惯性张量的非对角线项也很小。
基于上两个假设，等式2近似于：
d d t ( I ω ) = I ω ˙ + ω × ( I ω ) ≈ I ω ˙ (6) \frac{d}{d_t}(I\omega) = I\dot{\omega} + \omega \times(I\omega) \approx I\dot{\omega} \tag{6} dtd(Iω)=Iω˙+ω×(Iω)≈Iω˙(6)
2、构建优化问题
将1中的简化结果整理成以下三条公式：
Θ = R z T ( ψ ) ω (a) \Theta = R_z^T(\psi)\omega \tag{a} Θ=RzT(ψ)ω(a)
∑ i = 1 n r i × f i = I ω ˙ (b) \sum_{i=1}^{n}r_i \times f_i = I\dot{\omega} \tag{b} i=1∑nri×fi=Iω˙(b)
p ¨ = ∑ i = 1 n f i m − g (c) \ddot{p} = \frac{\sum_{i=1}^{n}f_i}{m} - g \tag{c} p¨=m∑i=1nfi−g(c)
将式（a）~（b）组合写成矩阵计算形式如下：
[ Θ p ω p ˙ ] = A [ Θ p ω p ˙ ] + B [ f 1 . . f n ] + [ 0 3 0 3 0 3 g ] \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} ⎣⎢⎢⎡Θpωp˙⎦⎥⎥⎤=A⎣⎢⎢⎡Θpωp˙⎦⎥⎥⎤+B⎣⎢⎢⎡f1..fn⎦⎥⎥⎤+⎣⎢⎢⎡030303g⎦⎥⎥⎤
其中：
A = [ 1 3 × 3 0 3 × 3 R z ( ψ k ) Δ t 0 3 × 3 0 3 × 3 1 3 × 3 0 3 × 3 1 3 × 3 Δ t 0 3 × 3 0 3 × 3 1 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 0 3 × 3 1 3 × 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} A=⎣⎢⎢⎡13×303×303×303×303×313×303×303×3Rz(ψk)Δt03×313×303×303×313×3Δt03×313×3⎦⎥⎥⎤
B = [ 0 3 × 3 . . . 0 3 × 3 0 3 × 3 . . . 0 3 × 3 g I [ r 1 ] × Δ t . . . g I [ r n ] × Δ t 1 3 × 3 Δ t / m . . . 1 3 × 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} B=⎣⎢⎢⎡03×303×3gI[r1]×Δt13×3Δt/m............03×303×3gI[rn]×Δt13×3Δt/m⎦⎥⎥⎤
[ r n ] _{[r_n]} [rn]为反对称矩阵，形式如下：
[ 0 − r z r y r z 0 − r x − r y r x 0 ] \begin{bmatrix} 0 & -r_z & r_y\\ r_z & 0 & -r_x\\ -r_y & r_x & 0 \end{bmatrix} ⎣⎡0rz−ry−rz0rxry−rx0⎦⎤
简化表达为一下形式：
x ( k + 1 ) = A k x ( k ) + B k f ( k ) + g x(k+1)= A_kx(k) + B_kf(k) + g x(k+1)=Akx(k)+Bkf(k)+g
为了减小问题的大小，我们消除了对应于不接触地面的脚的地面 反作用力的变量。 这减少了成本和约束矩阵的大小，在实践 中，我们的速度超过 10 倍。
3、最优化问题求解
假设参考轨迹如下(h为轨迹长度)：
χ ˉ = [ x k + 1 ˉ x k + 2 ˉ x k + 3 ˉ . . . x k + 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 χˉ=[xk+1ˉxk+2ˉxk+3ˉ...xk+hˉ]T
将2中的线性模型表达成以下形式：
X = A q p X 0 + B q p U X = A_{qp}X_0 +B_{qp}U X=AqpX0+BqpU
X ∈ R 13 k X \in \mathbb{R}^{13k} X∈R13k 是表示状态的轨迹的向量（多个预测状态的集合）， U ∈ R 3 n k U \in \mathbb{R}^{3nk} U∈R3nk是控制量（地面反作用力）。则MPC的一个简单的目标代价函数如下：
min U 1 2 U T H U + U T g s . t . c m i n ≤ C U ≤ c m a x \begin{matrix} \underset{U}{\min} & \frac{1}{2}U^THU +U^Tg\\ \\ s.t. & c_{min} \leq CU \leq c_{max} \end{matrix} Umins.t.21UTHU+UTgcmin≤CU≤cmax
其中 C C C是一个约束矩阵，且：
H = 2 ( B q p T L B q p + K ) g = 2 B q p T L ( A q p x 0 − y ) \begin{matrix} H = 2(B^T_{qp}LB_{qp} + K)\\ \\ g = 2B^T_{qp}L(A_{qp}x_0 - y) \end{matrix} H=2(BqpTLBqp+K)g=2BqpTL(Aqpx0−y)
L ∈ R 13 k × 13 k L \in \mathbb{R}^{13k\times13k} L∈R13k×13k为状态偏离权重，是一个对角矩阵， K ∈ R 3 n k × 3 n k K\in \mathbb{R}^{3nk\times3nk} K∈R3nk×3nk为力大小的一个对角权重矩阵。
构建好最优化问题后，可有二次规划的方法求解，得到满足目标代价函数的最优控制序列：
U = [ u k u k + 1 u k + 2 u k + 3 . . . u k + h − 1 ] U = \begin{bmatrix} u_k & u_{k+1} & u_{k+2}& u_{k+3}& ... & u_{k+h-1} \end{bmatrix} U=[ukuk+1uk+2uk+3...uk+h−1]
取第一组数组作为输出值作为期望地面反作用力，即 u k u_k uk