说明：本文章用于记录四足相关论文的公式理解，由于本人能力有限，公式的理解来自对论文内容的研读，网上的相关文章以及个人猜测的结合，不准确之处欢迎各位批评指正，本文也会不断更新。欢迎与我联系：2250017028@qq.com

《MIT Cheetah 3: Design and Control of a Robust, Dynamic Quadruped Robot》

1.支撑腿控制器

（1）基于集中质量模型(lumped model)的平衡控制器(Balance Controller)

i.将机器人看作单刚体模型的机器人运动方程:
[ I 3 … I 3 [ p 1 p c ] × … [ p 4 p c ] × ] ⏟ A F = [ m ( p ¨ c + g ) I G ω b ˙ ] ⏟ b \underbrace{\begin{bmatrix} I_3 &\dots & I_3 \\ [p_1 p_c]\times &\dots &[p_4 p_c]\times \end{bmatrix}}_{A} F=\underbrace{\begin{bmatrix} m(\ddot{p}_c+g )\\ I_G\dot{\omega _b} \end{bmatrix}}_b A [I3[p1pc]×……I3[p4pc]×]F=b [m(p¨c+g)IGωb˙]

使用了基本的牛顿运动定律

合力 = ma
合力矩 = 惯量*角加速度

ii.使用PD控制律计算质心加速度和角加速度
[ p ¨ c , d ω ˙ b , d ] = [ K p , p ( p c , d − p c ) + K d , p ( p ˙ c , d − p ˙ c ) K p , ω log ⁡ ( R d R T ) + K d , ω ( ω b , d − ω ) ] \left[\begin{array}{c} \ddot{\boldsymbol{p}}_{c, d} \\ \dot{\boldsymbol{\omega}}_{b, d} \end{array}\right]=\left[\begin{array}{c} \boldsymbol{K}_{p, p}\left(\boldsymbol{p}_{c, d}-\boldsymbol{p}_{c}\right)+\boldsymbol{K}_{d, p}\left(\dot{\boldsymbol{p}}_{c, d}-\dot{\boldsymbol{p}}_{c}\right) \\ \boldsymbol{K}_{p, \omega} \log \left(\boldsymbol{R}_{d} \boldsymbol{R}^{T}\right)+\boldsymbol{K}_{d, \omega}\left(\boldsymbol{\omega}_{b, d}-\boldsymbol{\omega}\right) \end{array}\right] [p¨c,dω˙b,d]=[Kp,p(pc,d−pc)+Kd,p(p˙c,d−p˙c)Kp,ωlog(RdRT)+Kd,ω(ωb,d−ω)]
目标：得到最优的控制力F，使质心趋于理想的质心动力学状态
b d = [ m ( p ¨ c , d + g ) I G ω ˙ b , d ] \boldsymbol{b_d} = \begin{bmatrix} m(\ddot{\boldsymbol{p}}_{c,d}+\boldsymbol{g})\\ \boldsymbol{I_G}\dot{\boldsymbol{\omega}}_{b,d} \end{bmatrix} bd=[m(p¨c,d+g)IGω˙b,d]
由于机器人运动方程为线性的，可以将该优化问题转为QP问题
F ∗ = min ⁡ F ∈ R 12 ( A F − b d ) T S ( A F − b d ) + α ∣ ∣ F ∣ ∣ 2 + β ∣ ∣ F − F p r e v ∣ ∣ 2 s . t . C F ≤ d \boldsymbol{F^*}=\min_{F\in R^{12}} \quad (AF-b_d)^TS(AF-b_d)+\alpha ||F||^2+\beta || F-F_{prev}||^2\\ s.t. \quad CF\le d F∗=F∈R12min(AF−bd)TS(AF−bd)+α∣∣F∣∣2+β∣∣F−Fprev∣∣2s.t.CF≤d
意义：

( A F − b d ) T S ( A F − b d ) (AF-b_d)^TS(AF-b_d) (AF−bd)TS(AF−bd) ----使质心状态符合期望，S矩阵表示控制中旋转和位移的优先级。
∣ ∣ F ∣ ∣ 2 ||F||^2 ∣∣F∣∣2 ----作用力最小化
∣ ∣ F − F p r e v ∣ ∣ 2 ||F-F_{prev}||^2 ∣∣F−Fprev∣∣2 ---- F p r e v F_{prev} Fprev表示迭代中上一次的作用力，目标是限制住两次迭代的解的差值。
α ， β > 0 \alpha，\beta>0 α，β>0 ----规定了解的标准化，以及对解进行了一定的过滤作用。
C F ≤ d CF\le d CF≤d ----具体内容未知，目的是为了保证解在可行域内。

（2）MPC控制器(Model Predictive Control)

J = ∑ i = 0 k ( x i − x i r e f ) T Q i ( x i − x i r e f ) + u i T R i u i J=\sum_{i=0}^{k}(x_i-x_i^{ref})^TQ_i(x_i-x_i^{ref})+u_i^TR_iu_i J=i=0∑k(xi−xiref)TQi(xi−xiref)+uiTRiui
目标：不断更新 u i u_i ui输入到控制系统，使上述代价函数最小化。

更新频率f：论文中给的是优化解以30Hz的频率输入到控制器
求解过程：论文指出，需要将MPC问题转化为QP问题进行求解。
MPC控制器的具体内容可以参考我的学习笔记：MPC学习笔记

2. 摆动腿控制器

（1）落足点计算

p s t e p , i = p h , i + T c ϕ 2 p ˙ c , d ⏟ R a i b e r t H e u r i s t i c + z 0 ∣ ∣ g ∣ ∣ ( p ˙ c − p ˙ c , d ) ⏟ C a p t u r e P o i n t p_{step,i}=p_{h,i}+\underbrace{\frac{T_{c_\phi}}{2}{\dot{p}}_{c,d}}_{Raibert \ Heuristic}+\underbrace{\sqrt{\frac{z_0}{||g||}}({\dot{p}}_c-{\dot{p}}_{c,d})}_{Capture \ Point} pstep,i=ph,i+Raibert Heuristic 2Tcϕp˙c,d+Capture Point ∣∣g∣∣z0 (p˙c−p˙c,d)
T c ϕ 2 p ˙ c , d \frac{T_{c_\phi}}{2}{\dot{p}}_{c,d} 2Tcϕp˙c,d ----根据《Legged robots that balance》中的描述，足端按照该公式确定的落足点，会使运动为对称运动，加速度为0

z 0 ∣ ∣ g ∣ ∣ ( p ˙ c − p ˙ c , d ) \sqrt{\frac{z_0}{||g||}}({\dot{p}}_c-{\dot{p}}_{c,d}) ∣∣g∣∣z0 (p˙c−p˙c,d) ----根据《Capture point: A step toward humanoid push recovery》中的描述，目标就是让落脚点趋近capture point，在该点腿部会获得平衡。

T c φ T_{c_\varphi} Tcφ ----理想状态下的stance态持续时间
z 0 z_0 z0 ----目标位置点的高度
p h , i p_{h,i} ph,i----腿i对应髋关节电机hip的位置

（2）摆动轨迹计算–PD控制器+前馈控制

i.前馈力矩的计算
τ f f , i = J i ⊤ Λ i ( B a i , ref − J ˙ i q ˙ i ) + C i q ˙ i + G i \tau_{\mathrm{ff}, i}=\boldsymbol{J}_{i}^{\top} \boldsymbol{\Lambda}_{i}\left({ }^{\mathfrak{B}} \boldsymbol{a}_{i, \text { ref }}-\dot{\boldsymbol{J}}_{i} \dot{\boldsymbol{q}}_{i}\right)+\boldsymbol{C}_{i} \dot{\boldsymbol{q}}_{i}+\boldsymbol{G}_{i} τff,i=Ji⊤Λi(Bai, ref −J˙iq˙i)+Ciq˙i+Gi
J i J_i Ji ----足部的雅可比矩阵
Λ i \Lambda _i Λi ----操作空间的惯性矩阵
B a i , r e f ^\mathfrak{B}a_{i,ref} Bai,ref ----参考加速度
q i q_i qi ----关节结构的向量
C i C_i Ci ----科里奥利矩阵
G i G_i Gi -----重力矩

ii.轨迹跟随控制器
τ i = J i ⊤ [ K p ( B p i , ref − B p i ) + K d ( B v i , r e f − B v i ) ] + τ f f , i \boldsymbol{\tau}_{i}=\boldsymbol{J}_{i}^{\top}\left[\boldsymbol{K}_{p}\left({ }^{\mathfrak{B}} \boldsymbol{p}_{i, \text { ref }}-{ }^{\mathfrak{B}} \boldsymbol{p}_{i}\right)+\boldsymbol{K}_{d}\left({ }^{\mathfrak{B}} \boldsymbol{v}_{i, \mathrm{ref}}-{^\mathfrak{B}} \boldsymbol{v}_{i}\right)\right]+\boldsymbol{\tau}_{\mathrm{ff}, i} τi=Ji⊤[Kp(Bpi, ref −Bpi)+Kd(Bvi,ref−Bvi)]+τff,i

控制频率f：4.5kHz

为了使PD控制器保持稳定，需要对增益 K p K_{p} Kp作如下处理
K p , j = ω d e s 2 Λ j j ( q ) K_{p,j}=\omega_{des}^2\Lambda_{jj}(q) Kp,j=ωdes2Λjj(q)

K p , j K_{p,j} Kp,j ---- K p K_p Kp中的第j个对角项。
ω d e s \omega_{des} ωdes ----目标自然频率
Λ j j \Lambda_{jj} Λjj ----第j个对角项在操作空间的惯性矩阵

3.虚拟预测支持多边形（Virtual Predictive Support Polygon）

i. 计算每条腿权重因子

（1）虚拟支持多边形：提供各种步态下的质心位置点
（2）定义每条腿对预测多边形的贡献度（分相位进行）：指出哪条腿该抬起或触地，以及状态切换的时间
K c ϕ = 1 2 [ erf ⁡ ( ϕ σ c 0 2 ) + erf ⁡ ( 1 − ϕ σ c 1 2 ) ] K c ˉ ϕ = 1 2 [ 2 + erf ⁡ ( − ϕ σ c ˉ 0 2 ) + erf ⁡ ( ϕ − 1 σ c ˉ 1 2 ) ] Φ = s ϕ K c ϕ + s ˉ ϕ K c ˉ ϕ \begin{array}{r} K_{c_{\phi}}=\frac{1}{2}\left[\operatorname{erf}\left(\frac{\phi}{\sigma_{c_{0}} \sqrt{2}}\right)+\operatorname{erf}\left(\frac{1-\phi}{\sigma_{c_{1}} \sqrt{2}}\right)\right] \\\\ K_{\bar{c}_{\phi}}=\frac{1}{2}\left[2+\operatorname{erf}\left(\frac{-\phi}{\sigma_{\bar{c}_{0}} \sqrt{2}}\right)+\operatorname{erf}\left(\frac{\phi-1}{\sigma_{\bar{c}_{1}} \sqrt{2}}\right)\right] \\\\ \Phi=s_{\phi} K_{c_{\phi}}+\bar{s}_{\phi} K_{\bar{c}_{\phi}} \end{array} Kcϕ=21[erf(σc02 ϕ)+erf(σc12 1−ϕ)]Kcˉϕ=21[2+erf(σcˉ02 −ϕ)+erf(σcˉ12 ϕ−1)]Φ=sϕKcϕ+sˉϕKcˉϕ
e r f ( x ) erf(x) erf(x) ---- 高斯误差函数
K c ϕ , K c ˉ ϕ K_{c_{\phi}},K_{\bar{c}_{\phi}} Kcϕ,Kcˉϕ ----支撑相和摆动相的权重因子
Φ \Phi Φ ----该腿总的权重因子
含义：支撑态下，某腿离中心越近，越可作为支撑点，摆动态下，某腿离中心越近，越不可用于平衡

ii.计算每条腿的虚拟点

[ ξ i − ξ i + ] = [ p i p i − p i p i + ] [ Φ i 1 − Φ i ] \left[\begin{array}{c} \boldsymbol{\xi}_{i}^{-} \\ \boldsymbol{\xi}_{i}^{+} \end{array}\right]=\left[\begin{array}{ll} \boldsymbol{p}_{i} & \boldsymbol{p}_{i^{-}} \\ \boldsymbol{p}_{i} & \boldsymbol{p}_{i^{+}} \end{array}\right]\left[\begin{array}{c} \Phi_{i} \\ 1-\Phi_{i} \end{array}\right] [ξi−ξi+]=[pipipi−pi+][Φi1−Φi]

上标为正表示逆时针，上标为负表示顺时针

iii.计算每条腿的预测多边形顶点

ξ i = Φ i p i + Φ i ξ i − + Φ i + ξ i + Φ i + Φ i − + Φ i + \boldsymbol{\xi}_{i}=\frac{\Phi_{i} \boldsymbol{p}_{i}+\Phi_{i} \boldsymbol{\xi}_{i}^{-}+\Phi_{i+} \boldsymbol{\xi}_{i}^{+}}{\Phi_{i}+\Phi_{i^{-}}+\Phi_{i^{+}}} ξi=Φi+Φi−+Φi+Φipi+Φiξi−+Φi+ξi+

iv.计算期望质心位置

p ^ C o M , d = 1 N ∑ i = 1 N ξ i \hat{\boldsymbol{p}}_{C o M, d}=\frac{1}{N} \sum_{i=1}^{N} \boldsymbol{\xi}_{i} p^CoM,d=N1i=1∑Nξi

4.倾斜地形下的姿态调整

含义：表示一个平面，即四足机器人当前位于的平面
z ( x , y ) = a 0 + a 1 x + a 2 y z(x, y)=a_{0}+a_{1} x+a_{2} y z(x,y)=a0+a1x+a2y
含义：根据当前机器人的状态去得到一个平面，即a={ a 0 , a 1 , a 2 a_0,a_1,a_2 a0,a1,a2}
a = ( W T W ) † W T p z W = [ 1 p x p y ] 4 × 3 \begin{aligned} \boldsymbol{a} &=\left(\boldsymbol{W}^{T} \boldsymbol{W}\right)^{\dagger} \boldsymbol{W}^{T} \boldsymbol{p}^{z} \\ \boldsymbol{W} &=\left[\begin{array}{lll} \mathbf{1} & \boldsymbol{p}^{x} & \boldsymbol{p}^{y} \end{array}\right]_{4 \times 3} \end{aligned} aW=(WTW)†WTpz=[1pxpy]4×3

p x , p y , p z \boldsymbol{p^x},\boldsymbol{p^y},\boldsymbol{p^z} px,py,pz ----包括了机器人每条腿的状态，比如 p x = ( p 1 x , p 2 x , p 3 x , p 4 x ) \boldsymbol{p^x} = (p_1^x,p_2^x,p_3^x,p_4^x) px=(p1x,p2x,p3x,p4x)

5.状态估计

使用的是卡尔曼滤波和拓展卡尔曼滤波，这里不作赘述。

《Highly Dynamic Quadruped Locomotion via Whole-Body Impulse Control and Model Predictive Control》

1.混合控制器

目标：将位置控制拆分成两个简单的控制器来进行控制

（1）MPC控制器

m p ¨ = ∑ i = 1 n c f i − c g , d d t ( I ω ) = ∑ i = 1 n c r i × f i \begin{array}{l} m \ddot{\mathbf{p}}=\sum_{i=1}^{n_{c}} \mathbf{f}_{i}-\mathbf{c}_{g}, \\\\ \frac{d}{d t}(\boldsymbol{I} \boldsymbol{\omega})=\sum_{i=1}^{n_{c}} \mathbf{r}_{i} \times \mathbf{f}_{i} \end{array} mp¨=∑i=1ncfi−cg,dtd(Iω)=∑i=1ncri×fi

与上面平衡控制器类似，对象都是集中质量模型，不过这里是用MPC来寻找优化的控制力。

（2）WBIC控制器（Whole-Body Impulse Control）

A ( q ¨ f q ¨ j ) + b + g = ( 0 6 τ ) + J c ⊤ f r \boldsymbol{A}\left(\begin{array}{c} \ddot{\mathbf{q}}_{f} \\ \ddot{\mathbf{q}}_{j} \end{array}\right)+\mathbf{b}+\mathbf{g}=\left(\begin{array}{c} \mathbf{0}_{6} \\ \boldsymbol{\tau} \end{array}\right)+\boldsymbol{J}_{c}^{\top} \mathbf{f}_{r} A(q¨fq¨j)+b+g=(06τ)+Jc⊤fr
A A A ----质量(惯性)矩阵
b b b ----科里奥利力
g g g ----重力
τ \tau τ ----关节扭矩
f r f_r fr ----增广的作用力
J c J_c Jc ----触地点的雅可比矩阵

迭代公式的理解

参考手推WBC公式

持续更新…

四足控制中的公式理解相关推荐

距离保护整定计算中分支系数公式理解
在一般的距离保护II段和III段的整定计算中,分支系数的计算是必须要的,本文选自电力系统继电保护(第二版)(张保会老师主编)P94-P97页的例题,分析其中分支系数的计算公式. 我们在前面的章节中知道 ...
四足机器人|机器狗|仿生机器人|多足机器人|Adams仿真|Simulink仿真|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货！需要资料及指导的可以联系我！
四足机器人|机器狗|仿生机器人|多足机器人|基于CPG的四足机器人Simulink与Adams虚拟样机|源码可直接执行|绝对干货!需要资料及指导的可以联系我!QQ:1096474659 基于CPG的四 ...
四足动物模型控制中的模型自适应神经网络
四足动物模型控制中的模型自适应神经网络 (Mode-Adaptive Neural Networks for Quadruped Motion Control) 四足动物模型拥有不同的步态,比如行走, ...
MIT四足机器人Cheetah 3控制方案理解笔记（2）——Convex Mpc身体姿态控制
在"MIT四足机器人Cheetah 3控制方案理解笔记(1)"中,主要简单的总结了下一般情况下腿足机器人的摆动腿控制方法以及Cheetah 3的基于集中质量模型的平衡控制器.此外, ...
MIT四足机器人Cheetah 3控制方案理解笔记（1）——摆动腿控制、单刚体模型平衡控制器
之前一段时间在阅读MIT四足机器人Cheetah 3以及Mini Cheetah控制方案的相关论文,在此处做一些笔记.Cheetah 3与Mini Cheetah的控制方案大同小异,此处先以Cheet ...
【转载】MIT四足机器人Cheetah 3控制方案笔记
转载:知乎 Wenboxing https://zhuanlan.zhihu.com/p/190028074 这里写自定义目录标题 MIT四足机器人Cheetah 3控制方案理解笔记(1)--摆动腿控 ...
【机器人学、机器人控视觉与控制】四足机器人MATLAB仿真
文章目录 [机器人学.机器人控视觉与控制]四足机器人MATLAB仿真 1 创建一条机器人腿 2 单腿运动 3 四腿运动 [机器人学.机器人控视觉与控制]四足机器人MATLAB仿真我们的目标是创建一个 ...
动作捕捉技术在四足仿生机器人研究中的应用
轮式/履带式移动机器人可以胜任很多场景的探索.运输的任务,但是随着应用空间的拓展,需要机器人在山地.峭壁.丛林.雪地等崎岖复杂的地形的任务也逐渐增多,轮式/履带式机器人难以在这类地形中移动.自然界中动 ...
【灯哥开源四足机器人】推荐一个开源四足机器狗项目，8自有度，两个舵机控制一个腿，apache开源协议的，已经迭代了好多个版本了，设计的非常好。有官方淘宝店，没有3D打印机的可以购买散装零件自己组装
目录前言 1,关于[灯哥开源四足机器人] 2,使用py-apple 3,总结前言本文的原文连接是: https://blog.csdn.net/freewebsys/article/detail ...

四足控制中的公式理解