介绍：WBC控制器是当今足式机器人最常用的控制器之一。基于多任务不同的优先级，它可以控制关节电机使其按照优先级的顺序满足各个任务的需求。MIT开源的cheetah mini就用到了MPC+WBIC算法来控制落足点。

推导的过程参考了以下的资料
mini cheetah中WBC公式解析
基于零空间方法（NUB）的全身控制（WBC）的简单实现
Robot Dynamics Lecture Notes
由于本人能力有限，对于WBC控制器的理解还不够深，推导过程有误之处欢迎批评指正。
联系方式：2250017028@qq.com

2.公式说明

$$\begin{array}{l}\Delta {\mathbf{q}}_i=\Delta {\mathbf{q}}_{i-1}+{\mathbf{J}}_{i\mid pre}^{†}\left({\mathbf{e}}_i-{\mathbf{J}}_i\Delta {\mathbf{q}}_{i-1}\right)\\ \\ {\dot{\mathbf{q}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}={\dot{\mathbf{q}}}_{i-1}^{\text{cmd }}+{\mathbf{J}}_{i\mid pre}^{†}\left({\dot{\mathbf{x}}}_i^{\mathrm{d}\mathrm{e}\mathrm{s}}-{\mathbf{J}}_i{\dot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}\right)\\ \\ {\ddot{\mathbf{q}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}={\ddot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}+\overline{J_{i\mid pre}^{\mathrm{d}\mathrm{y}\mathrm{n}}}\left({\ddot{\mathbf{x}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}-{\dot{\mathbf{J}}}_i\dot{\mathbf{q}}-{\mathbf{J}}_i{\ddot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}\right)\end{array}$$
where
$$\begin{array}{l}{\mathbf{J}}_{i\mid prc}={\mathbf{J}}_i{\mathbf{N}}_{i-1}\\ \\ {\mathbf{N}}_{i-1}={\mathbf{N}}_0{\mathbf{N}}_{1\mid 0}\cdots {\mathbf{N}}_{i-1\mid i-2}\\ \\ {\mathbf{N}}_0=\mathbf{I}-{\mathbf{J}}_c^{†}{\mathbf{J}}_c\\ \\ {\mathbf{N}}_{i\mid i-1}=\mathbf{I}-{\mathbf{J}}_{i\mid i-1}{}^{†}{\mathbf{J}}_{i\mid i-1}\end{array}$$

• 推导过程涉及到了线性代数的伪逆矩阵和零空间，可以参考维基百科中的描述
  伪逆矩阵
  零空间
• $\mathbf{q}$：机器人的配置矩阵，反应了每个关节的位置。若关节i为旋转关节，则$q_i$表示旋转角度。若关节i为移动关节，则$q_i$表示位移量。

接下来主要推导以下这条公式：
$${\dot{\mathbf{q}}}_i^{\mathrm{c}\mathrm{m}\mathrm{d}}={\dot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}+{\mathbf{J}}_{i\mid pre}^{†}\left({\dot{\mathbf{x}}}_i^{\mathrm{d}\mathrm{e}\mathrm{s}}-{\mathbf{J}}_i{\dot{\mathbf{q}}}_{i-1}^{\mathrm{c}\mathrm{m}\mathrm{d}}\right)$$

（1）首先了解任务的表示方法。
假定有n个任务，即n种目标状态，用$x_i,i=1,...n$表示，则基于正运动学的知识得，
$$\begin{gathered} \dot{x_i}=J_i\dot{q} \\ ⋮ \\ \dot{x_n}=J_n\dot{q} \end{gathered}$$

（2）冗余控制
$$\dot{q}=J^{†}\dot{\mathbf{x}}+\left(I-J^{†}J\right)y$$
这也是WBC控制的核心思想，即其中$y$是满足行数的任意向量。
对于控制任务$\mathbf{x}$来说，$\left(I-J^{†}J\right)$是零向量矩阵，相当于一个“过滤器”，把与控制$\mathbf{x}$无关的机器人自由度过滤掉。
即，无论$y$是什么，都不会对控制$\mathbf{x}$产生任何影响。这样，我们就可以通过$y$来控制更低优先级的控制任务，同时不影响高优先级的任务。
如图所示：子空间A表示第一个任务下的操作空间，B表示第二个任务的操作空间，C表示还剩下的冗余自由度。

（3）推导过程
任务一：
$$\begin{gathered} \dot{{\mathbf{x}}_0}=J_0\dot{q}-----(1) \\ \downarrow \\ \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+\left(I-J_0^{†}{J}_0\right)y-----(2) \end{gathered}$$
任务二：
$$\begin{gathered} \dot{{\mathbf{x}}_1}=J_1\dot{q}-----(3) \\ \downarrow \\ \dot{q}=J_1^{†}\dot{{\mathbf{x}}_1}+\left(I-J_1^{†}{J}_1\right)y-----(4) \end{gathered}$$
将（2）代入（3）中
$$\begin{gathered} \dot{{\mathbf{x}}_1}=J_1{J}_0^{†}\dot{{\mathbf{x}}_0}+J_1\left(I-J_0^{†}{J}_0\right)y \\ \downarrow \\ \dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0}=J_1\left(I-J_0^{†}{J}_0\right)y \end{gathered}$$
根据上述公式中的参数定义：
$$\begin{gathered} N_0=I-J_0^{†}{J}_0 \\ {J}_{1\mid 0}=J_1{N}_0 \\ \downarrow \\ \dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0}=J_{1\mid 0}y \end{gathered}$$
由于$J_{1\mid 0}$也是冗余矩阵，所以处理方法如上：
$$\begin{gathered} y=J_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+(I-J_{1\mid 0}^{†}{J}_{1\mid 0})z \\ \downarrow \\ y=J_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_{1\mid 0}z----(5) \end{gathered}$$
将（5）代回（2）中
$$\begin{gathered} \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+N_0{J}_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_0{N}_{1\mid 0}z \\ \downarrow \\ \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+N_0{J}_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_{1}z \end{gathered}$$
根据零空间投影的性质
$$\begin{gathered} N_0{J}_{1\mid 0}^{†}=J_{1\mid 0}^{†} \\ \downarrow \\ \dot{q}=J_0^{†}\dot{{\mathbf{x}}_0}+J_{1\mid 0}^{†}(\dot{{\mathbf{x}}_1}-J_1{J}_0^{†}\dot{{\mathbf{x}}_0})+N_{1}z \end{gathered}$$
根据前面的讨论
$${\dot{q}}_0^{cmd}=J_0^{†}\dot{{\mathbf{x}}_0}$$
若此时已无自由冗余度，则此时以得到最终的结果
$${\dot{q}}_1^{cmd}={\dot{q}}_0^{cmd}+J_{1\mid 0}^{†}({\dot{\mathbf{x}}}_0^{\mathbf{c}\mathbf{m}\mathbf{d}}-J_1{\dot{q}}_0^{cmd})$$
比较要推导的公式，其实可以发现，形式已经一致了。所以不再作进一步的迭代。