本篇文章主要参考了以下的文章，目的是为了加深自己对WBC公式以及多优先级任务运用的理解。
基于零空间方法（NUB）的全身控制（WBC）的简单实现

控制系统输入：滑块在轨道上的位置$y_h$，机械臂各关节位置 $\mathbf{\theta }=[{\theta }_1{\theta }_2{\theta }_3{]}^T$。
控制系统输出：机械臂各个关节的速度$\dot{\mathbf{\theta }}$。
控制任务1：让机械臂末端位置保持在世界坐标系下点 $A=[20{]}^T$。
控制任务2：让机械臂末端朝向x轴。

1.无优先级的控制方案

（1）末端期望坐标
$${\mathbf{x}}_d=\left[\begin{array}{c}2\\ -y_h\\ 0\end{array}\right]$$
（2）正运动学求解（末端相对于原点）
$$\mathbf{x}=\left[\begin{array}{l}x\\ y\\ \theta \end{array}\right]=\left[\begin{array}{c}{l}_1{c}_1+l_2{c}_{12}+l_3{c}_{123}\\ l_1{s}_1+l_2{s}_{12}+l_3{s}_{123}\\ {\theta }_1+{\theta }_2+{\theta }_3\end{array}\right]$$
（3）求逆
将$l_1=l_2=l_3=1m$代入
$$\begin{gathered} \dot{\mathbf{x}}=J\dot{\mathbf{\theta }} \\ \downarrow \\ J=\left[\begin{array}{ccc}-s_1-s_{12}-s_{123}& -s_{12}-s_{123}& -s_{123}\\ c_1+c_{12}+c_{123}& c_{12}+c_{123}& c_{123}\\ 1& 1& 1\end{array}\right] \\ \downarrow \\ \dot{\theta }=J^{†}\dot{\mathbf{x}} \end{gathered}$$
伪逆的性质：若$J$有逆，则$J^{†}$是$J$的逆；若 $J$没有逆，则$J^{†}$满足$\dot{\theta }=J^{†}\dot{\mathbf{x}}$的最小二乘解。

2.带优先级的控制方案

以位置优先级高于方向优先级为例
（1）任务分解
$$\begin{gathered} \{\begin{array}{l}{\mathbf{r}}_1={\left[\begin{array}{ll}x& y\end{array}\right]}^T\\ {\mathbf{r}}_2=\theta \end{array} \\ \downarrow \\ \{\begin{array}{l}{\mathbf{r}}_{1d}={\left[\begin{array}{ll}2& -y_h\end{array}\right]}^T\\ {\mathbf{r}}_{2d}=0\end{array} \end{gathered}$$
（2）雅可比矩阵求解
上面已经得到了矩阵的具体参数值，只需要做分解即可
$$\{\begin{array}{l}{J}_1=\left[\begin{array}{ccc}-s_1-s_{12}-s_{123}& -s_{12}-s_{123}& -s_{123}\\ c_1+c_{12}+c_{123}& c_{12}+c_{123}& c_{123}\end{array}\right]\\ J_2=\left[\begin{array}{lll}1& 1& 1\end{array}\right]\end{array}$$
（3）代入WBC公式
$$\dot{\theta }=J_1^{†}{\dot{\mathbf{r}}}_1+\left(I-J_1^{†}{J}_1\right)J_2^{†}{\dot{\mathbf{r}}}_2$$

实际控制中，将得到的速度值输入到关节的控制器中（如PID控制器）对关节电机进行控制即可