根据雅可比矩阵的推导可以获取工作空间速度与关节空间速度之间的线性映射关系，也就是雅可比矩阵，该矩阵是关节空间向量的函数，并且可以很容易发现，从关节到工作空间的正向速度映射是很容易实现的，对于一个明确的关节空间速度 q˙\mathbf {\dot q}q˙，必然存在一个明确的工作空间速度 x˙\mathbf{\dot x}x˙，并且其是唯一的，也就是只要确定了正运动学映射关系的情况下，关节有运动的时候，机械臂必然存在位姿的变化，并且这个变化是唯一的
但是反过来却不一定，在欠驱动机械臂中，并不是所有的工作空间速度都可以有其对应的关节空间速度的，也就是其在物理上就无法实现故不存在；而对于冗余机械臂来说，其工作空间维度少于关节空间维度，对于一个工作空间速度，甚至有无数个关节空间速度与之对应。这个时候，我们可以定义 R(J)\mathbf R(J)R(J) 为机器人在雅可比矩阵为 JJJ 时可以达到的工作空间速度的集合，其为 RMR^MRM 的一个子集
冗余 (Redundancy) 意味着机器人的自由度（关节数 NNN）大于它需要完成的任务维度（工作空间维度 MMM），例如，一个7自由度的手臂（N=7N=7N=7）在3D空间中抓取一个物体（M=6M=6M=6，即3D位置+3D姿态），这就是冗余
为描述这种冗余特性，这里使用了代数中的零空间理论来表示，定义 N(J)N(J)N(J) 为雅可比矩阵 J 的零空间投影矩阵，简称零空间矩阵， N(J)N(J)N(J) 与 JJJ 满足如下特性，并且零空间矩阵的秩就是机器人的冗余程度：
J⋅N(J)=0J\cdot N(J)=0J⋅N(J)=0
零空间是一个输入的集合，其包含了哪些被矩阵映射为零向量的输入向量，其本质上是一个输入空间的子空间，其数学定义如下
N(A)={w∈RN∣Aw=0}N(A)=\{w \in \mathbb R^N |Aw=0\}N(A)={w∈RN∣Aw=0}
零空间的作用接在于其描述了矩阵方程的解的结构，也就是如果 Aw=bA \boldsymbol{w} = \boldsymbol{b}Aw=b 有一个解 wp\boldsymbol{w}pwp（称为特解），那么该方程的所有解（通解）都可以表示为：
wgeneral=wp+w0, 其中 w0∈N(A)\boldsymbol{w}{\text{general}} = \boldsymbol{w}_p + \boldsymbol{w}_0 \quad \text{, 其中 } \boldsymbol{w}_0 \in N(A)wgeneral=wp+w0, 其中 w0∈N(A)
- wp\boldsymbol{w}_pwp（特解）：完成任务的那个"特定"输入。
- w0\boldsymbol{w}_0w0（零空间解）：你可以"免费"添加的、不会影响输出结果的"额外"输入。
特解 wp\boldsymbol{w}_pwp 上添加任意一个零空间向量 w0\boldsymbol{w}_0w0，输出结果 b\boldsymbol{b}b 保持不变！如果对应工作空间和关节空间中的向量，那就是说，对于冗余机器人来说，其关节自由度大于工作空间自由度，也就是存在特殊情况：关节转动，但是机械臂末端没有位姿变化或者说保持静止，所以可以得出一个结论：只要机器人是冗余的（且不处于奇异状态），它的雅可比矩阵 JJJ 必然存在一个维度大于0的零空间 N(J)N(J)N(J)，也就是对于一个工作空间中的任务 x˙\mathbf{\dot x}x˙ ，有如下的矩阵方程
x˙=Jq˙\dot{\boldsymbol x}=J\dot{\boldsymbol q}x˙=Jq˙
- 通解： q˙general=q˙p+q˙0\dot{\boldsymbol{q}}_{\text{general}} = \dot{\boldsymbol{q}}_p + \dot{\boldsymbol{q}}_0q˙general=q˙p+q˙0，其中 q˙0∈N(J)\dot{\boldsymbol{q}}_0 \in N(J)q˙0∈N(J)
- q˙p\dot{\boldsymbol{q}}_pq˙p (特解)： 为了让机器人末端（手）以速度 x˙\dot{\boldsymbol{x}}x˙ 移动，关节必须执行的那部分速度。
- q˙0\dot{\boldsymbol{q}}_0q˙0 (零空间解)： 机器人关节可以执行的"内部运动"。物理意义是机器人可以以某种方式驱动关节，而它的末端（手）在工作空间中完全保持静止（x˙=0\dot{\boldsymbol{x}}=\boldsymbol{0}x˙=0）
求解这种非齐次线性方程最简单的方法就是使用逆矩阵计算，但是对于冗余机器人来说，其雅可比矩阵并非方阵，并且是一个欠定方程组矩阵，不存在对应的逆矩阵，且有无穷多解，但是可以定义一个 N×MN\times MN×M 的特殊广义逆矩阵 J+J^+J+，其满足 JJ+=IJJ^+=\mathbb IJJ+=I，称其为雅可比矩阵的右逆矩阵，特解可用这个右逆矩阵 J+J^+J+ 来计算，即 q˙p=J+x˙\dot{\boldsymbol{q}}_p = J^+ \dot{\boldsymbol{x}}q˙p=J+x˙ ，并且该矩阵对 JJJ 而言是最优且唯一的（至于为什么最优以及如何计算，会在后面推导）
在定义了该矩阵之后可以推出如下公式
0=J−IJ=J−JJ+J=J(I−J+J)0=J-\mathbb I J=J-JJ^+ J=J(\mathbb I - J^+ J)0=J−IJ=J−JJ+J=J(I−J+J)
因此零空间矩阵以及右逆矩阵可按如下公式求解：
N(J)=I−J+JJ+=JT(JJT)−1N(J)=\mathbb I - J^+ J \qquad J^+=J^T(JJ^T)^{-1}N(J)=I−J+JJ+=JT(JJT)−1
也就是说，对于一阶微分运动学的方程 x˙=Jq˙\dot{\boldsymbol x}=J\dot{\boldsymbol q}x˙=Jq˙ 可以直接获取其物理上最优的特解 q˙p=J+x˙\dot{\boldsymbol q}_p=J^+\dot{\boldsymbol x}q˙p=J+x˙
对于冗余机器人而言，在非奇异构型条件下，雅可比矩阵一定是行满秩的，JJTJJ^TJJT一定是可逆的，但是在实际工程中，当机器人接近奇异点时，JJTJJ^TJJT虽然理论上仍然可逆，但是其行列式接近0，会变成一个病态矩阵，计算其逆会导致数值溢出，所以一般会加以处理，比如说阻尼最小二乘法：J+=JT(JJT+λ2I)−1J^+=J^T(JJ^T+\lambda^2\mathbb I)^{-1}J+=JT(JJT+λ2I)−1，通过加入一个微小的阻尼项，可以保证矩阵始终可逆，即使在奇异点处也可逆，保证了算法的数值稳定性
接下来就可以使用零空间和广义逆矩阵的方法计算特解和零空间解了，二者的叠加就是通解，很容易得出通解有如下形式：
q˙=J+x˙+Nq˙∀\dot{\boldsymbol{q}} = J^{+}\dot{\boldsymbol{x}} + N\dot{\boldsymbol{q}}_{\forall}q˙=J+x˙+Nq˙∀
其中 q˙∀\dot{\boldsymbol{q}}{\forall}q˙∀ 为任意满足 q˙∀∈RN\dot{\boldsymbol{q}}{\forall} \in R^Nq˙∀∈RN 的关节空间速度向量，对上式等号两边同时左乘以雅可比矩阵 JJJ 可以很容易验证该公式的正确性
至此，在尽可能保证工作空间任务 x˙\dot{\boldsymbol{x}}x˙ 完成的前提下，关节可以按照 q˙∀\dot{\boldsymbol{q}}_{\forall}q˙∀ 的速度任意运动，来实现其他额外任务如关节避障等，像这种充分利用关节冗余特性，同时保证多个控制任务的控制方法称为全身控制（WBC）。
那么这个右逆矩阵是否是唯一的呢？因为冗余机器人的关节空间与工作空间的映射关系实际上是一个欠定方程组，所以肯定存在通解和特解，首先可以构建一个特解，使得方程成立，所以这个特解应该满足：q˙p=Gx˙\dot{\boldsymbol{q}}_p=G\dot{\boldsymbol{x}}q˙p=Gx˙，其中的矩阵 GGG 实际上是一个有逆矩阵意义的 N×MN\times MN×M 矩阵，并且需要满足：JG=IJG=\mathbb IJG=I ，这里将满足此方程的矩阵 GGG 称为矩阵 JJJ 的一个右逆矩阵，并且很容易发现，J 存在无数个右逆矩阵，而 J+J^+J+ 是其中的一个特例，在数学上称为 Moore-Penrose 伪逆
那么巨大数量的右逆矩阵中如何选取一个最优呢？可以通过能量最优的概念进行选取，也就是我们希望选取的右逆矩阵所计算出的关节转速的模长是最小的，这意味着可以通过最小的关节速度（或者说最节能和平滑的）来实现期望的工作空间速度，避免不必要的剧烈运动，因此可以定义代价函数或者优化目标，然后使用标准的拉格朗日乘子法 (Lagrange Multipliers) 来证明，该方法是一种求解"带约束优化问题"的经典方法：
minq˙(12∣∣q˙∣∣2)=minq˙(12q˙Tq˙)\min_{\dot{\boldsymbol{q}}} \left( \frac{1}{2} ||\dot{\boldsymbol{q}}||^2 \right) = \min_{\dot{\boldsymbol{q}}} \left( \frac{1}{2} \dot{\boldsymbol{q}}^T \dot{\boldsymbol{q}} \right)q˙min(21∣∣q˙∣∣2)=q˙min(21q˙Tq˙)
然后确定约束条件，也就是关节速度必须实现期望的工作空间速度
Jq˙=x˙⇒Jq˙−x˙=0J\dot{\boldsymbol{q}} = \dot{\boldsymbol{x}} \quad \Rightarrow \quad J\dot{\boldsymbol{q}} - \dot{\boldsymbol{x}} = \boldsymbol{0}Jq˙=x˙⇒Jq˙−x˙=0
我们引入一个 M×1M \times 1M×1 的拉格朗日乘子向量 λ\boldsymbol{\lambda}λ (因为有 MMM 个等式约束)，构造拉格朗日函数 L\mathcal{L}L
L(q˙,λ)=(优化目标)−λT(约束条件)L(q˙,λ)=12q˙Tq˙−λT(Jq˙−x˙)\mathcal{L}(\dot{\boldsymbol{q}}, \boldsymbol{\lambda}) = (\text{优化目标}) - \boldsymbol{\lambda}^T (\text{约束条件})\\[0.5em] \mathcal{L}(\dot{\boldsymbol{q}}, \boldsymbol{\lambda}) = \frac{1}{2} \dot{\boldsymbol{q}}^T \dot{\boldsymbol{q}} - \boldsymbol{\lambda}^T (J\dot{\boldsymbol{q}} - \dot{\boldsymbol{x}})L(q˙,λ)=(优化目标)−λT(约束条件)L(q˙,λ)=21q˙Tq˙−λT(Jq˙−x˙)
要找到最优解 q˙\dot{\boldsymbol{q}}q˙，我们必须找到 L\mathcal{L}L 的平稳点 (Stationary Point)，即 L\mathcal{L}L 对 q˙\dot{\boldsymbol{q}}q˙ 和 λ\boldsymbol{\lambda}λ 的偏导数（梯度）都为零的点，因此分别求偏导
0=∇q˙L=∂∂q˙(12q˙Tq˙−λTJq˙+λTx˙)=q˙−(JTλ)+0⟶q˙=JTλ0=∇λL=∂∂λ(12q˙Tq˙−λT(Jq˙−x˙))=−(Jq˙−x˙)⟶Jq˙=x˙ \boldsymbol{0}=\nabla_{\dot{\boldsymbol{q}}} \mathcal{L} = \frac{\partial}{\partial \dot{\boldsymbol{q}}} \left( \frac{1}{2} \dot{\boldsymbol{q}}^T \dot{\boldsymbol{q}} - \boldsymbol{\lambda}^T J \dot{\boldsymbol{q}} + \boldsymbol{\lambda}^T \dot{\boldsymbol{x}} \right) = \dot{\boldsymbol{q}} - (J^T \boldsymbol{\lambda}) + \boldsymbol{0} \longrightarrow \dot{\boldsymbol{q}} = J^T \boldsymbol{\lambda}\\[0.5em] \boldsymbol{0} = \nabla_{\boldsymbol{\lambda}} \mathcal{L} = \frac{\partial}{\partial \boldsymbol{\lambda}} \left( \frac{1}{2} \dot{\boldsymbol{q}}^T \dot{\boldsymbol{q}} - \boldsymbol{\lambda}^T (J\dot{\boldsymbol{q}} - \dot{\boldsymbol{x}}) \right)=- (J\dot{\boldsymbol{q}} - \dot{\boldsymbol{x}}) \longrightarrow J\dot{\boldsymbol{q}} = \dot{\boldsymbol{x}}0=∇q˙L=∂q˙∂(21q˙Tq˙−λTJq˙+λTx˙)=q˙−(JTλ)+0⟶q˙=JTλ0=∇λL=∂λ∂(21q˙Tq˙−λT(Jq˙−x˙))=−(Jq˙−x˙)⟶Jq˙=x˙
现在获取了两个约束方程或者说等式，其中包含了两个未知数 q˙\dot{\boldsymbol{q}}q˙ 和 λ\boldsymbol{\lambda}λ
可以将等式一带入等式二中，因此有
J(JTλ)=x˙(JJT)λ=x˙J (J^T \boldsymbol{\lambda}) = \dot{\boldsymbol{x}}\\[0.5em] (J J^T) \boldsymbol{\lambda} = \dot{\boldsymbol{x}}J(JTλ)=x˙(JJT)λ=x˙
现在，我们求解 λ\boldsymbol{\lambda}λ。这里我们假设机器人不处于奇异状态，因此 JJJ 是行满秩的， M×MM \times MM×M 矩阵 (JJT)(J J^T)(JJT) 是可逆的：
λ=(JJT)−1x˙\boldsymbol{\lambda} = (J J^T)^{-1} \dot{\boldsymbol{x}}λ=(JJT)−1x˙
将其代入等式一有如下最佳解向量，或者说可以直接计算出对应的伪逆矩阵然后就可以求出最小范数解：
q˙=JT((JJT)−1x˙)q˙=[JT(JJT)−1]x˙=J+x˙J+=JT(JJT)−1\dot{\boldsymbol{q}} = J^T \left( (J J^T)^{-1} \dot{\boldsymbol{x}} \right)\\[0.5em] \dot{\boldsymbol{q}} = \left[ J^T (J J^T)^{-1} \right] \dot{\boldsymbol{x}}=J^+\dot{\boldsymbol{x}}\\[0.5em] J^+=J^T (J J^T)^{-1}q˙=JT((JJT)−1x˙)q˙=[JT(JJT)−1]x˙=J+x˙J+=JT(JJT)−1
满足 Jq˙=x˙J\dot{\boldsymbol{q}} = \dot{\boldsymbol{x}}Jq˙=x˙ 约束的唯一最小范数解，正是由 Moore-Penrose 伪逆 J+J^{+}J+ 给出的特解 q˙p\dot{\boldsymbol{q}}_pq˙p，并且其还有一个很好的性质，就是 J+x˙J^{+}\dot{\boldsymbol{x}}J+x˙ 位于JJJ 的行空间（与零空间正交），这是更深刻的数学原因，也是后续相关推导（零空间、多任务、全身控制）的真正基石。
在代数理论中有秩-零度定理 (Rank-Nullity Theorem)，也就是输入空间的维度等于值域空间维度加零空间维度，在机器人学中输入空间就是关节空间，值域空间就是工作空间，所以可以很容易得出冗余机器人的零空间的维度，而且在非奇异构型下零空间维度必定大于零，一定存在可以利用的内部运动 q˙0\dot{\boldsymbol{q}}_0q˙0，其不会影响机械臂末端的运动，也就是有
Jq˙0=0J\dot{\boldsymbol{q}}_0 = 0Jq˙0=0
那么如何使用一个具体公式表示这个零向量或者说内部运动，以此来实现规划或者控制等，或者说我们想引入一个次要任务，比如避障，这个次要任务可能希望我们执行一个任意的关节速度 q˙∀\dot{\boldsymbol{q}}{\forall}q˙∀。我们不能直接用 q˙∀\dot{\boldsymbol{q}}{\forall}q˙∀，因为它会干扰主任务，我们需要一个"过滤器"，也就是一个投影矩阵 NNN，它能将 q˙∀\dot{\boldsymbol{q}}_{\forall}q˙∀ "投影"到零空间 N(J)N(J)N(J) 中，得到 q˙0\dot{\boldsymbol{q}}_0q˙0。
q˙0=Nq˙∀\dot{\boldsymbol{q}}0 = N \dot{\boldsymbol{q}}{\forall}q˙0=Nq˙∀
投影后的向量通常无法完成"原始意图"，它完成的是"在不干扰主任务的前提下，最接近原始意图的那个运动"，这也就是"优先级"控制的本质，因为投影之前的关节速度向量中的某些分量可能导致末端执行器的位姿变化，这会干扰并破坏高优先级的主任务，也就是说投影矩阵的作用就是过滤掉次要任务中可能导致影响主任务的部分，只保留安全的部分，也就是要求投影后的向量必须在零空间中
实际上零空间投影矩阵的意义不是完美地执行次要任务，而是在完美地执行主任务的前提下，尽最大努力去近似次要任务，而且其作为一个正交投影矩阵，其可以在零空间 N(J)N(J)N(J) 中找到一个唯一的向量，使其与原始意图之间的欧式距离最小
这个矩阵必须满足，无论要执行的关节速度是多少，投影的结果一定位于零空间 N(J)N(J)N(J) 中，根据零空间的定义，这有下面的公式，并且很容易发现，如果想让投影后位于零空间中向量最接近原始向量（或者说欧式距离最小），那么这实际上就是转化为了一个正交投影的问题，也就是投影后的向量必须是原始向量在零空间的投影，且误差向量 (q˙∀−q˙0)(\dot{\boldsymbol{q}}_{\forall} - \dot{\boldsymbol{q}}_0)(q˙∀−q˙0) 与零空间本身垂直或者正交
Jq˙0=0⇒JNq˙∗=0J \dot{\boldsymbol{q}}0 = \boldsymbol{0} \quad \Rightarrow \quad J N \dot{\boldsymbol{q}}{*} = \boldsymbol{0}Jq˙0=0⇒JNq˙∗=0
因为给定的是任意关节速度，所以投影矩阵本身必须满足：
J⋅N=0J\cdot N =0J⋅N=0
也就是这个投影矩阵必须是一个雅可比矩阵的零空间矩阵，到这一步之后，很容易就可以根据伪逆矩阵的定义计算出零空间矩阵的数学表达式了，也很容易发现，这个零空间矩阵也是唯一的正交投影矩阵
如此就初步完成了WBC的数学原理推导，下面给出本人Github笔记仓库，其中包含了机器人规划决策与控制理论的笔记
https://github.com/Michael-Jetson/Planning_Decision_and_Control_Note