机器人冗余自由度优化过程中的零空间概念

---

数学概念

  零空间是在线性映射（即矩阵）的背景下出现的，指：像为零的原像空间，即 {x| Ax=0}。 在数学中，一个算子A 的零空间是方程 Av = 0 的所有解 v 的集合。它也叫做A 的核空间。 如果算子是在向量空间上的线性算子 ，零空间就是线性子空间 （来自百度百科）。总而言之，就是找到一个向量使之与矩阵A相乘，不管矩阵A中的变量如何变化，乘积都为零。如果A是满秩的，那么这个向量（零空间）也就位移确定了，如果A是不满秩矩阵，那么这个向量就有无穷多个。

与机器人控制的关系

  那这个与机器人的关节冗余控制有什么关系呢？我们知道对于有冗余关节的机器人在执行任务时，例如7自由度的机械臂，控制末端到达某一位姿（或者6自由度机械臂只对目标位置进行约束），此时各关节的角度是可以改变的，都能使末端满足要求。对于这类问题，我们要做的就是在不改变目标位置（最高优先级）的前提下，控制各关节执行最优命令。此时联想零空间的概念（无论A取怎样的值，在零空间下其结果都为零），也就不会对外产生影响了。

机器人零空间投影

  通过上面的叙述我们可以明白：零空间与原空间之间为相互正切关系，零空间向量不会对原矩阵空间产生影响，因此我们可以将低优先级的任务放置在高优先级任务的零空间下进行计算，再将所有任务的控制量相加即可得到具有不同优先级的控制量，使得控制量在能够不违反高优先级任务的同时完成低优先级任务。

  在这里，为了简化讨论，用一个有两个子任务的情况制定了问题。这个公式可以很容易地扩展到具有两个以上子任务的情况。具有第一优先级的子任务将使用第一个操作变量$r_1\in R^{m_1}$指定，而具有第二优先级的子任务将使用第二个操作变量$r_2\in R^{m_2}$指定。关节角度$\theta \in R^n$和操作变量之间的运动学关系表示如下：
$$r_i=f_i(\theta )(i=1,2)$$
他们的导数关系为等式1：
$$\dot{r_i}=J_i(\theta )\dot{\theta }(i=1,2)$$
其中$J_i(\theta )$就是雅克比矩阵。
  当i=1时，等式的通解使用伪逆可以表示为等式2：
$$\dot{\theta }=J_1^{\#}(\theta ){\dot{r}}_1+[I-J_1^{\#}(\theta )J_1(\theta )]y$$
其中$J_1^{\#}(\theta )$是$J_1(\theta )$的伪逆，$y$是任意向量。如果确切的解不存在，上式的结果表示为最小二乘的形式，即：$minimizing||{\dot{r}}_1-J_1(\theta )\dot{\theta }||$
  当i=2时，将等式2代入等式1，得到：
$${\mathbf{J}}_2\left(\mathbf{I}-{\mathbf{J}}_1^{\#}{\mathbf{J}}_1\right)\mathbf{y}={\dot{\mathbf{r}}}_2-{\mathbf{J}}_2{\mathbf{J}}_1^{\#}{\dot{\mathbf{r}}}_1$$

如果y是确定的，那么第二个操作变量r2就能够被确定。但往往y是不确定的，这时我们可以通过$minimizing||{\dot{r}}_2-J_2(\theta )\dot{\theta }||$来得到，与等式2相类似：
$$\mathbf{y}={\tilde{\mathbf{J}}}_2^{\#}\left({\dot{\mathbf{r}}}_2-{\mathbf{J}}_2{\mathbf{J}}_1^{\#{\tilde{r}}_1}\right)+\left(\mathbf{I}-{\tilde{\mathbf{J}}}_2^{\#}{\tilde{\mathbf{J}}}_2\right)\mathbf{z}$$

其中${\tilde{J}}_2\triangleq J_2(I-J_1^{\#}{J}_1)$，z是任意变量
  因此$\dot{\theta }的解可以表示为等式6：$

$$\begin{array}{rl}\dot{\mathbf{\theta }}=& {\mathbf{J}}_1^{\#}\ast {\dot{r}}_1+\left(\mathbf{I}-{\mathbf{J}}_1^{\#}{\mathbf{J}}_1\right){\tilde{J}}_2^{\#}\left({\tilde{r}}_2-{\mathbf{J}}_2{\mathbf{J}}_1^{\#}{\dot{r}}_1\right)\\ & +\left(\mathbf{I}-{\mathbf{J}}_1{\mathbf{J}}_1^{\#}\right)\left(\mathbf{I}-{\tilde{\mathbf{J}}}_2^{\#}{\tilde{J}}_2\right)\mathbf{z}.\end{array}$$

当r2=θ时，即$J_2=I$，上述等式可以简化为如下形式：
$$\begin{array}{rl}\dot{\mathbf{\theta }}=& {\mathbf{J}}_1^{\#}\ast {\dot{r}}_1+\left(\mathbf{I}-{\mathbf{J}}_1^{\#}{\mathbf{J}}_1\right){\dot{r}}_2\end{array}$$
对应于等式（6）中的第三项等于零，这意味着第三个操作变量不存在任何自由度，因为第二个操作变量r2=θ需要所有剩余的自由度。

总结

  基于零空间投影的方法通过将低优先级任务投影到高优先级任务零空间的方法实现任务层级。零空间投影的一个缺点是无法结合不等式约束。在动作高度动态时，机器人会打破约束，使这个问题突显，基于零空间的方法假设接触约束均为等式，这在物理层面是错误的。

参考文献

[1] 机械臂的零空间（nullspace）及零空间阻抗等相关控制的实现概论
[2] 基于零空间方法（NUB）的全身控制（WBC）的简单实现
[3]MIT Cheetah 2019 WBC 学习
[4] Nakamura Y , Hanafusa H , Yoshikawa T . Task-Priority Based Redundancy Control of Robot Manipulators[J]. The International Journal of Robotics Research, 1987, 6(2).