一、MPC的力学原理

刚体的力与加速度，转矩与角加速度可以通过牛顿方程和欧拉方程求出：

1、牛顿公式：

基本公式：

F=mdvcdt=mv˙cF =m\frac{dv_c}{dt}=m\dot v_cF=mdtdvc=mv˙c

展开形式（ncn_cnc为与地面接触点的数量）：

m[x¨y¨z¨]=∑i=1nc[fxifyifzi]−[00mg]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\\ mg \end{bmatrix}m⎣⎡x¨y¨z¨⎦⎤=i=1∑nc⎣⎡fxifyifzi⎦⎤−⎣⎡00mg⎦⎤

将质量移至右边，可求得加速度：

[x¨y¨z¨]=∑i=1nc[fxi/mfyi/mfzi/m]−[00g]\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 \end{bmatrix}⎣⎡x¨y¨z¨⎦⎤=i=1∑nc⎣⎡fxi/mfyi/mfzi/m⎦⎤−⎣⎡00g⎦⎤

写成更简洁的形式即：

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

2、欧拉公式

基本公式(ω\omegaω为角速度，III为转动惯量)：

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

对上式做近似考虑：

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

因此有展开式：

IΘ¨=∑i=1nc[0−rziryirzi0−rxi−ryirxi0][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} IΘ¨=i=1∑nc⎣⎡0rzi−ryi−rzi0rxiryi−rxi0⎦⎤⎣⎡fxifyifzi⎦⎤

其中：

Θ=[αβγ]\Theta = \begin{bmatrix} \alpha \\ \beta \\ \gamma \end{bmatrix}Θ=⎣⎡αβγ⎦⎤

同样将转动惯量移至右边，得到角加速度Θ¨\ddot \ThetaΘ¨

Θ¨=∑i=1ncI−1[0−rziryirzi0−rxi−ryirxi0][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 Θ¨=i=1∑ncI−1⎣⎡0rzi−ryi−rzi0rxiryi−rxi0⎦⎤⎣⎡fxifyifzi⎦⎤(2)

3、对不同量的参考系作近似变换

为了尽可能让系统保持线性关系，同时由于运动过程中滚转角α\alphaα和俯仰角β\betaβ较小，因此有以下近似变换：

角度变换：

Θ˙G≈Rz(γ)Θ˙B\dot \Theta_G \approx R_z(\gamma)\dot \Theta_BΘ˙G≈Rz(γ)Θ˙B

惯量矩阵变换：

IG≈Rz(γ)IBRz(γ)TI_G \approx R_z(\gamma) I_B R_z(\gamma)^TIG≈Rz(γ)IBRz(γ)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}Rz(γ)=⎣⎡cosγsinγ0−sinγcosγ0001⎦⎤

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}IB=⎣⎡0.070000.260000.242⎦⎤

4、状态迭代方程

综合公式（1）所计算的加速度P¨\ddot PP¨，公式（2）中所计算的角加速度Θ¨\ddot \ThetaΘ¨，规定右上标为时刻kkk，右下标为所在参考系

各状态量之间的迭代方式如下：

旋转角：
Θk+1=Θk+Rzk(−γ)Θ˙BΔt(3)\Theta^{k+1} = \Theta^{k} + R^k_z(-\gamma) \dot \Theta_B\Delta t \tag 3Θk+1=Θk+Rzk(−γ)Θ˙BΔt(3)
位置：Pk+1=Pk+P˙kΔt(4)P^{k+1} = P^{k} +\dot P^{k} \Delta t \tag4Pk+1=Pk+P˙kΔt(4)
角速度：Θ˙k+1=Θ˙k+Θ¨kΔt(5)\dot \Theta^{k+1} = \dot \Theta^{k} + \ddot \Theta^{k} \Delta t \tag 5Θ˙k+1=Θ˙k+Θ¨kΔt(5)
速度：P˙k+1=P˙k+P¨kΔt(6)\dot P^{k+1} = \dot P^{k} + \ddot P^k\Delta t \tag6P˙k+1=P˙k+P¨kΔt(6)

将式（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⎣⎢⎢⎡ΘPΘ˙P˙⎦⎥⎥⎤Gk+1=Ak⎣⎢⎢⎡ΘPΘ˙P˙⎦⎥⎥⎤Gk+Bk⎣⎢⎢⎡f1..fn⎦⎥⎥⎤k+⎣⎢⎢⎡030303g⎦⎥⎥⎤(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}Ak=⎣⎢⎢⎡1303030303130303Rz(γ)Δt0313030313Δt0313⎦⎥⎥⎤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}^ kBk=⎣⎢⎢⎡0303IGr1~Δt13Δt/m............0303IGrn~Δt13Δt/m⎦⎥⎥⎤k

二、构建二次规划问题

1、完整轨迹表示

我们式（7）中的状态迭代方程写成如下形式：

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

其中：

x=[ΘPΘ˙P˙]x = \begin{bmatrix} \Theta \\ P \\ \dot \Theta \\ \dot P\end{bmatrix}x=⎣⎢⎢⎡ΘPΘ˙P˙⎦⎥⎥⎤

此时式（8）是一个线性方程，我们根据其计算未来h步状态：

将上式写成矩阵运算形式：

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

其中：

X=[xk+1xk+2xk+3⋯xk+h]Tf=[fkfk+1fk+2⋯fk+h]TA=[AA2A3⋯Ah]TB=[B00⋯0ABB0⋯0A2BABB⋯0⋮⋮⋮⋮⋮Ah−1BAh−2BAh−3⋯B]g=[gAg+gA2g+Ag+g⋮Ak+h−1g+⋯+g]\begin{aligned} &\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} \end{aligned}X=[xk+1xk+2xk+3⋯xk+h]Tf=[fkfk+1fk+2⋯fk+h]TA=[AA2A3⋯Ah]TB=⎣⎢⎢⎢⎢⎢⎡BABA2B⋮Ah−1B0BAB⋮Ah−2B00B⋮Ah−3⋯⋯⋯⋮⋯000⋮B⎦⎥⎥⎥⎥⎥⎤g=⎣⎢⎢⎢⎢⎢⎡gAg+gA2g+Ag+g⋮Ak+h−1g+⋯+g⎦⎥⎥⎥⎥⎥⎤

现在我们得到了未来h步轨迹的状态矩阵以及其计算表达式（9）

2、二次规划问题

根据式（9），构造代价函数如下，由轨迹误差项和控制项组成：

J=(X−Xref)2⋅L+f2⋅K\mathbf J = ( \mathbf X- \mathbf X^{ref})^2 \cdot L+\mathbf f^2 \cdot K J=(X−Xref)2⋅L+f2⋅K

式中L,KL,KL,K分别为权重矩阵（对角矩阵），由于状态X\mathbf XX是一个矩阵，安装规范写法，表达如下：

J=(X−Xref)T⋅L⋅(X−Xref)+fT⋅K⋅f(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}J=(X−Xref)T⋅L⋅(X−Xref)+fT⋅K⋅f(10)

接下来构造一个二次规划如下（最小化代价函数），该式的物理意义在于，使得计算轨迹尽可能与参考轨迹重合的同时，所使用到的力最小：

minx,fJ(11)\underset{\mathbf x,\mathbf f}{min} \quad \mathbf J \tag{11}x,fminJ(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}∣fx∣≤μfz,∣fy∣≤μfz,fz<fmax

将式（10）展开，去掉与控制量f\mathbf ff无关的项，可化成标准二次型如下：

min⁡f12fTHf+RTfs.t.cmin≤Cf≤cmax(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}fmins.t.21fTHf+RTfcmin≤Cf≤cmax(12)

其中：

H=2(BTLB+K)R=2BTL(Axk+g−Xref)\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}H=2(BTLB+K)R=2BTL(Axk+g−Xref)

利用式（12）求解，便能得到满足约束条件下的最优控制序列f=[fkfk+1⋯fk+h−1]\mathbf f=\begin{bmatrix}f_k& f_{k+1}& \cdots & f_{k+h-1} \end{bmatrix}f=[fkfk+1⋯fk+h−1]

三、二次规划求解

由于式（12）中数据量比较大，且与机器人系统的配置有关，因此我们从以下简单案例进行讲解：

对于二次多项式：

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

求解其在约束条件下的最小值，即为二次规划问题，有以下表达：

min⁡x2x12+x22+x1x2+x1+x2s.t.x1≥0x2≥0x1+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}xmins.t.2x12+x22+x1x2+x1+x2x1≥0x2≥0x1+x2=1

将其化为以下标准形式：

min⁡x12xTPx+qTxs.t.Gx≤hAx=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}xmins.t.21xTPx+qTxGx≤hAx=b

即：
min⁡x0.5[x1x2][4112][x1x2]+[11][x1x2]s.t.−x1≤0−x2≤0x1+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}xmins.t.0.5[x1x2][4112][x1x2]+[11][x1x2]−x1≤0−x2≤0x1+x2=1

根据约束条件可以得出：

G=[−100−1],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 = 1G=[−100−1],h=[00],A=[11],b=1

利用python的cvxopt库可求解该问题：

from cvxopt import matrix, solversP = 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   dres0:  1.8889e+00  7.7778e-01  1e+00  3e-16  2e+001:  1.8769e+00  1.8320e+00  4e-02  2e-16  6e-022:  1.8750e+00  1.8739e+00  1e-03  2e-16  5e-043:  1.8750e+00  1.8750e+00  1e-05  6e-17  5e-064:  1.8750e+00  1.8750e+00  1e-07  2e-16  5e-08
Optimal solution found.
x:
[ 2.50e-01]
[ 7.50e-01]

【四足机器人那些事儿】MiniCheetah中的MPC控制相关推荐

【四足机器人那些事儿2】MiniCheetah中所使用的的足端轨迹方程
本篇将讲解MiniCheetah中所使用的的足端轨迹方程-贝塞尔曲线方程一.贝塞尔曲线贝塞尔曲线就是这样的一条曲线,它是依据四个位置任意的点坐标绘制出的一条光滑曲线. 在历史上,研究贝塞尔曲线的人 ...
AI一分钟 | 浙大研发出“踢不倒”的四足机器人；富士康冲击A股上市，AI为最大卖点
一分钟AI 滴滴公布新战略,成立战略事业群和智慧交通事业部浙江大学熊蓉教授的机器人团队发布"绝影"四足机器人,展现"快稳准"优异性能苹果智能音箱HomePo ...
四足机器人——12自由度舵机狗DIY（二）
目录一.四足机器人步态研究控制的现状 1.1目前的三种控制策略 <1>基于静态稳定的控制方法. <2>基于动力学模型的控制方法. <3>基于生物所具有的神经性调节 ...
四足机器人关节锁死故障的容错问题
好久没写博客了,冒个泡. 四足机器人容错方面的运动控制很少被研究.现在能找到的,国内有上海交大做的特殊腿部结构的四足运动学容错方案,国外有韩国Jung-Min Yang所做的一系列的步态容错方法(很多 ...
四足机器人 2.建模和步态规划
1.建模四足机器人建模: 运动学建模和动力学建模四足机器人在运动过程中,与所处环境进行交互作用,为提高机器人运动的稳定性和适应性,需要整体考虑四足机器人的动力学模型.足-地接触模型和步态生成与变换 ...
【四足机器人】强化学习实现minitaur运动控制（介绍篇）
一.minitaur 简介这是来自宾夕法尼亚大学的一款机器人,叫 Minitaur,看图你就明白了. 四足机器人的运动控制通常需要大量的专业知识,以及突如其来的灵感(调参).在之前的文章中,我们就用 ...
【四足机器人】学习笔记单腿逆运动学和站立姿态控制
[四足机器人]学习笔记单腿逆运动学和站立姿态控制一.四足机器人单腿逆运动学原理二.四足机器人站立姿态控制原理近期,博主在古月居学习关于四足机器人的相关部分知识,从阳炼老师的四足机器人控制与仿真 ...
波士顿动力真的无可企及吗？一步步剖析四足机器人技术（一）
四足机器人运动控制第一章序第二章运动状态姿态控制运动控制第三章步态第四章 CPG控制网络介绍 CPG模型分类基于HOPF振荡器的CPG单元模型 CPG网络控制模型 Tips 参考 ...
赤兔四足机器人的作用_跑得快，打不死！清华大学开发“小强”机器人，壮汉狂踩也挡不住前进步伐...
大数据文摘编辑部出品提到蟑螂,很多同学都深恶痛绝. 这种身型小巧的虫子不仅跑得快.繁殖能力强,而且超级抗打抗压,在所有的环境下都能顽强地生存下去. 12mm高的蟑螂可以躲进4mm的缝隙也难怪周星驰 ...
UC伯克利给四足机器人加Buff：瞬间适应各种真实地形，抹了油的地面也能hold住...
丰色发自凹非寺量子位报道 | 公众号 QbitAI 随着四足机器人的应用越来越成功,它们面对的场景也会越来越多: 今天爬楼梯,明天过草地,后天又去坑坑洼洼的石子地-- 这么复杂多变的地形它们可 ...

【四足机器人那些事儿】MiniCheetah中的MPC控制