四足控制中的公式理解

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

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

1.支撑腿控制器

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

i.将机器人看作单刚体模型的机器人运动方程:
$$\underset{A}{\underbrace{\left[\begin{array}{ccc}{I}_3& \dots & I_3\\ [p_1{p}_c]\times & \dots & [p_4{p}_c]\times \end{array}\right]}}F=\underset{b}{\underbrace{\left[\begin{array}{c}m({\ddot{p}}_c+g)\\ I_G\dot{{\omega }_b}\end{array}\right]}}$$

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

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

ii.使用PD控制律计算质心加速度和角加速度
$$\left[\begin{array}{c}{\ddot{\mathbf{p}}}_{c,d}\\ {\dot{\mathbf{\omega }}}_{b,d}\end{array}\right]=\left[\begin{array}{c}{\mathbf{K}}_{p,p}\left({\mathbf{p}}_{c,d}-{\mathbf{p}}_c\right)+{\mathbf{K}}_{d,p}\left({\dot{\mathbf{p}}}_{c,d}-{\dot{\mathbf{p}}}_c\right)\\ {\mathbf{K}}_{p,\omega }\log \left({\mathbf{R}}_d{\mathbf{R}}^T\right)+{\mathbf{K}}_{d,\omega }\left({\mathbf{\omega }}_{b,d}-\mathbf{\omega }\right)\end{array}\right]$$
目标：得到最优的控制力F，使质心趋于理想的质心动力学状态
$${\mathbf{b}}_{\mathbf{d}}=\left[\begin{array}{c}m({\ddot{\mathbf{p}}}_{c,d}+\mathbf{g})\\ {\mathbf{I}}_{\mathbf{G}}{\dot{\mathbf{\omega }}}_{b,d}\end{array}\right]$$
由于机器人运动方程为线性的，可以将该优化问题转为QP问题
$$\begin{gathered} {\mathbf{F}}^{\ast }=\underset{F\in R^{12}}{\min }(AF-b_d{)}^{T}S(AF-b_d)+\alpha ||F|{|}^2+\beta ||F-F_{prev}|{|}^2 \\ s.t.CF\le d \end{gathered}$$
意义：

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

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

$$J=\sum _{i=0}^k(x_i-x_i^{ref}{)}^T{Q}_i(x_i-x_i^{ref})+u_i^T{R}_i{u}_i$$
目标：不断更新$u_i$输入到控制系统，使上述代价函数最小化。

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

2. 摆动腿控制器

（1）落足点计算

$$p_{step,i}=p_{h,i}+\underset{RaibertHeuristic}{\underbrace{\frac{T_{c_{\varphi }}}{2}{\dot{p}}_{c,d}}}+\underset{CapturePoint}{\underbrace{\sqrt{\frac{z_0}{||g||}}({\dot{p}}_c-{\dot{p}}_{c,d})}}$$
$\frac{T_{c_{\varphi }}}{2}{\dot{p}}_{c,d}$ ----根据《Legged robots that balance》中的描述，足端按照该公式确定的落足点，会使运动为对称运动，加速度为0

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

$T_{c_{\phi }}$ ----理想状态下的stance态持续时间
$z_0$ ----目标位置点的高度
$p_{h,i}$----腿i对应髋关节电机hip的位置

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

i.前馈力矩的计算
$${\tau }_{\mathrm{f}\mathrm{f},i}={\mathbf{J}}_i^{\top }{\mathbf{\Lambda }}_i\left({}^{\mathfrak{B}}{\mathbf{a}}_{i,\text{ ref }}-{\dot{\mathbf{J}}}_i{\dot{\mathbf{q}}}_i\right)+{\mathbf{C}}_i{\dot{\mathbf{q}}}_i+{\mathbf{G}}_i$$
$J_i$ ----足部的雅可比矩阵
${\Lambda }_i$ ----操作空间的惯性矩阵
${}^{\mathfrak{B}}{a}_{i,ref}$ ----参考加速度
$q_i$ ----关节结构的向量
$C_i$ ----科里奥利矩阵
$G_i$ -----重力矩

ii.轨迹跟随控制器
$${\mathbf{\tau }}_i={\mathbf{J}}_i^{\top }\left[{\mathbf{K}}_p\left({}^{\mathfrak{B}}{\mathbf{p}}_{i,\text{ ref }}-{}^{\mathfrak{B}}{\mathbf{p}}_i\right)+{\mathbf{K}}_d\left({}^{\mathfrak{B}}{\mathbf{v}}_{i,\mathrm{r}\mathrm{e}\mathrm{f}}-{}^{\mathfrak{B}}{\mathbf{v}}_i\right)\right]+{\mathbf{\tau }}_{\mathrm{f}\mathrm{f},i}$$

• 控制频率f：4.5kHz

为了使PD控制器保持稳定，需要对增益$K_p$作如下处理
$$K_{p,j}={\omega }_{des}^2{\Lambda }_{jj}(q)$$

• $K_{p,j}$ ---- $K_p$中的第j个对角项。
• ${\omega }_{des}$ ----目标自然频率
• ${\Lambda }_{jj}$ ----第j个对角项在操作空间的惯性矩阵

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

i. 计算每条腿权重因子

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

ii.计算每条腿的虚拟点

$$\left[\begin{array}{c}{\mathbf{\xi }}_i^{-}\\ {\mathbf{\xi }}_i^{+}\end{array}\right]=\left[\begin{array}{ll}{\mathbf{p}}_i& {\mathbf{p}}_{i^{-}}\\ {\mathbf{p}}_i& {\mathbf{p}}_{i^{+}}\end{array}\right]\left[\begin{array}{c}{\Phi }_i\\ 1-{\Phi }_i\end{array}\right]$$

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

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

$${\mathbf{\xi }}_i=\frac{{\Phi }_i{\mathbf{p}}_i+{\Phi }_i{\mathbf{\xi }}_i^{-}+{\Phi }_{i+}{\mathbf{\xi }}_i^{+}}{{\Phi }_i+{\Phi }_{i^{-}}+{\Phi }_{i^{+}}}$$

iv.计算期望质心位置

$${\hat{\mathbf{p}}}_{CoM,d}=\frac{1}{N}\sum _{i=1}^N{\mathbf{\xi }}_i$$

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

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

• ${\mathbf{p}}^{\mathbf{x}},{\mathbf{p}}^{\mathbf{y}},{\mathbf{p}}^{\mathbf{z}}$ ----包括了机器人每条腿的状态，比如${\mathbf{p}}^{\mathbf{x}}=(p_1^x,p_2^x,p_3^x,p_4^x)$

5.状态估计

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

---

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

1.混合控制器

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

（1）MPC控制器

$$\begin{array}{l}m\ddot{\mathbf{p}}={\sum }_{i=1}^{n_c}{\mathbf{f}}_i-{\mathbf{c}}_g,\\ \\ \frac{d}{dt}(\mathbf{I}\mathbf{\omega })={\sum }_{i=1}^{n_c}{\mathbf{r}}_i\times {\mathbf{f}}_i\end{array}$$

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

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

$$\mathbf{A}\left(\begin{array}{c}{\ddot{\mathbf{q}}}_f\\ {\ddot{\mathbf{q}}}_j\end{array}\right)+\mathbf{b}+\mathbf{g}=\left(\begin{array}{c}{0}_6\\ \mathbf{\tau }\end{array}\right)+{\mathbf{J}}_c^{\top }{\mathbf{f}}_r$$
$A$ ----质量(惯性)矩阵
$b$ ----科里奥利力
$g$ ----重力
$\tau$ ----关节扭矩
$f_r$ ----增广的作用力
$J_c$ ----触地点的雅可比矩阵

迭代公式的理解

参考手推WBC公式

持续更新…