机器人学基础第四章机械臂的速度与静力

首次发布: 2025-05-19

... 次访问

机械臂连杆的速度方程和静力分析

4.1 速度分析目标#

分析机械臂连杆的速度的目标是求机械臂的末端及各个中间连杆的运动速度与关节速率之间的关系

设连杆0的固连框架 $\{0\}$ 为参考坐标系。固连框架 $\{i\}$ 的速度有两种，分别是框架 $\{i\}$ 原点的线速度 $^{0}v_i = \ ^{0}V_{iorg}$ 和连杆 $i$ 整个刚体的角速度 $^{0}\omega_i = \ ^{0}\Omega_i$ 。有时，我们会利用在刚体自身框架下的速度向量表达，以方便进行向量的迭代计算，因此需要涉及到速度向量在不同框架下的转换。具体的方法是使用旋转矩阵进行变换，例如从框架 $\{i\}$ 变到框架 $\{0\}$ 的公式为

\begin{aligned} ^{0}v_i = \ ^{0}_{i}R\ ^{i}v_i\\ ^{0}\omega_i = \ ^{0}_{i}R\ ^{i}\omega_i\\ \end{aligned}

连杆的速度表达	速度传播

4.2 速度传递公式#

连杆间的速度传递公式有两种形式，一种是在绝对框架 $\{0\}$ 表达下的迭代形式，另一种是在自身框架 $\{i\}$ 表达的迭代形式。

4.2.1 旋转关节#

旋转关节 $i + 1$ 连接的连杆 $i$ 和连杆 $i + 1$ 之间的速度传播公式为

自身框架形式

\begin{cases} &^{i+1}\omega_{i+1} = \ ^{i+1}_{i}R\ ^{i}\omega_i + \dot\theta_{i+1} \ ^{i+1} \hat{\mathbf{z}}_{i+1} \\ &^{i+1}v_{i+1} = \ ^{i+1}_{i}R(^{i} v_i + \ ^{i}\omega_i \times \ ^{i}p_{i+1}) \end{cases}

其中 $\hat{\mathbf{z}}_{i+1}$ 是框架 $\{i+1\}$ 的 Z 轴方向的单位向量。

注：

框架 $\{i+1\}$ 的原点其实就是连杆 $i$ 的末端，因此 $\vec{v}_{i+1} = \vec{v}_i + \vec{\omega}_i \times \vec{p}_{i+1}$ ，其中 $\vec{p}_{i+1}$ 是框架 $\{i+1\}$ 原点 $O_{i+1}$ 在框架 $\{i\}$ 中的位矢；
连杆 $i+1$ 的角速度满足 $\vec{\omega}_{i+1} = \vec{\omega}_{i} + \dot\theta_{i+1} \hat{\mathbf{z}}_{i+1}$ ，因为 $\dot\theta_{i+1}$ 是连杆 $i+1$ 相对于连杆 $i$ 的相对角速度大小，其方向沿框架 $\{i+1\}$ 的 Z 轴方向，因此总的角速度用合成公式即可得到。

绝对框架形式

\begin{cases} &^{0}\omega_{i+1} = \ ^{0}\omega_i + \dot\theta_{i+1} \ ^{0}_{i+1}R\ ^{i+1} \hat{\mathbf{z}}_{i+1} \\ &^{0}v_{i+1} = \ ^{0} v_i + \ ^{0}\omega_i \times \ ^{0}_{i}R\ ^{i}p_{i+1} \end{cases}

物理含义同上。

4.2.2 棱柱关节#

棱柱关节 $i + 1$ 连接的连杆 $i$ 和连杆 $i + 1$ 之间的速度传播公式为

自身框架形式

\begin{cases} &^{i+1}\omega_{i+1} = \ ^{i+1}_{i}R\ ^{i}\omega_i \\ &^{i+1}v_{i+1} = \ ^{i+1}_{i}R(^{i} v_i + \ ^{i}\omega_i \times \ ^{i}p_{i+1}) + \dot{d}_{i+1}\ ^{i+1}\hat{\mathbf{z}}_{i+1} \end{cases}

其中 $\hat{\mathbf{z}}_{i+1}$ 是框架 $\{i+1\}$ 的 Z 轴方向的单位向量。

注：

同样地，这里框架 $\{i+1\}$ 的原点也是连杆 $i$ ，因此 $\vec{v}_{i+1} = \vec{v}_i + \vec{\omega}_i \times \vec{p}_{i+1} + \dot{d}_{i+1}\hat{\mathbf{z}}_{i+1}$ ，其中 $\vec{p}_{i+1}$ 是框架 $\{i+1\}$ 原点 $O_{i+1}$ 在框架 $\{i\}$ 中的位矢，这里比旋转关节多出来的那一项是相对运动的速度；
由于是旋转关节，因此没有相对的角速度部分，故连杆 $i+1$ 的角速度就是连杆 $i$ 的角速度 $\vec{\omega}_{i+1} = \vec{\omega}_{i}$

绝对框架形式

\begin{cases} &^{0}\omega_{i+1} = \ ^{0}\omega_i \\ &^{0}v_{i+1} = \ ^{0} v_i + \ ^{0}\omega_i \times \ ^{0}_{i}R\ ^{i}p_{i+1} + \dot{d}_{i+1}\ ^{0}_{i+1} R \ ^{i+1}\hat{\mathbf{z}}_{i+1} \end{cases}

物理含义同上。

根据上述的迭代公式，可以得到从连杆 1 开始的，直至最后一根连杆 $n$ 的速度结果。

求平面 2R 的末端速度与关节速率的函数关系

解:

前向运动学方程为

\begin{aligned} ^{0}_{1}T &= \begin{bmatrix} c\theta_1 & -s\theta_1 & 0 & 0\\ s\theta_1 & c\theta_1 &0 &0\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ ^{1}_{2}T &= \begin{bmatrix} c\theta_2 & -s\theta_2 & 0 & L_1\\ s\theta_2 & c\theta_2 &0 &0\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ ^{2}_{3}T &= \begin{bmatrix} 1 & 0 & 0 & L_2\\ 0 & 1 &0 &0\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ ^{0}_{3}T(\theta_1, \theta_2) &= \begin{bmatrix} c(\theta_1 + \theta_2) & -s(\theta_1 + \theta_2) & 0 & L_1 c\theta_1 + L_2 c(\theta_1 + \theta_2)\\ s(\theta_1 + \theta_2) & c(\theta_1 + \theta_2) &0 &L_1 s\theta_1 + L_2 s(\theta_1 + \theta_2)\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ \end{aligned}

其中， $c(\cdot)$ 和 $s(\cdot)$ 分别是余弦函数和正弦函数的缩写。

已知 $^0\omega_0 = 0, \ ^0v_0 = 0$ ，利用迭代公式，有

\begin{aligned} \ ^{1}\omega_1 &= \ ^{1}_{0}R \ ^{0}w_0 + \dot\theta_1 \ ^{1}\mathbf{\hat{z}}_1 = \begin{bmatrix}0\\ 0\\ \dot\theta_1\end{bmatrix}\\ \ ^{2}\omega_2 &= \ ^{2}_{1}R \ ^{1}w_1 + \dot\theta_2 \ ^{2}\mathbf{\hat{z}}_2 = \begin{bmatrix} 0 \\ 0\\ \dot\theta_1 + \dot\theta_2\end{bmatrix} \\ \ ^{3}\omega_3 &= \ ^{3}_{2}R\ ^{2}\omega_2 = \ ^{2}\omega_2 \end{aligned}

注意这里计算 $\ ^{i+1}_{i} R$ 使用 $\ ^{i+1}_{i}R = \ ^{i}_{i+1}R^\top$ 最方便。

\begin{aligned} \ ^{1}v_1 &= \ ^{1}_{0}R(\ ^{0}v_0 + \ ^{0}\omega_0 \times \ ^{0} p_1) = 0\\ \ ^{2}v_2 &= \ ^{2}_{1}R(\ ^{1}v_1 + \ ^{1}\omega_1 \times \ ^{1}p_2) = \begin{bmatrix} L_1\dot\theta_1 s\theta_2 \\ L_1\dot\theta_1c\theta_2 \\ 0\end{bmatrix} \\ \ ^{3}v_3 &= \ ^{3}_{2}R(\ ^{2}v_2 + \ ^{2}\omega_2 \times \ ^{2}p_3) = \begin{bmatrix} L_1\dot\theta_1 s\theta_2\\ L_1\dot\theta_1c\theta_2 + L_2(\dot\theta_1 + \dot\theta_2) \\ 0\end{bmatrix} \end{aligned}

注意，这里的 $\ ^{3}v_3$ 依然需要使用旋转关节的传播公式，因为线速度实际是上一个连杆末端的速度。

当从后续 Jacobian 矩阵的角度看这个问题的时候，会发现 $v$ 关于 x, y 分量的项数并不对称，其实这是因为这里的速度表达是在刚体自身框架下的表达，而不是在绝对框架 $\{0\}$ 下的表达形式

实际上，将上述结果变换到框架 $\{0\}$ 中，得到

\begin{aligned} \ ^{0}v_3 &= \ ^{0}_{3}R \ ^{3}v_3 = \begin{bmatrix} c(\theta_1 + \theta_2) &-s(\theta_1 + \theta_2) &0 \\ s(\theta_1 + \theta_2) &c(\theta_1 + \theta_2) &0 \\ 0 &0 &1 \end{bmatrix} \begin{bmatrix} L_1\dot\theta_1 s\theta_2\\ L_1\dot\theta_1c\theta_2 + L_2(\dot\theta_1 + \dot\theta_2) \\ 0\end{bmatrix}\\ &= \begin{bmatrix} L_1 s\theta_2 c(\theta_1 + \theta_2)\dot\theta_1 - s(\theta_1 + \theta_2)(L_1\dot\theta_1c\theta_2 + L_2(\dot\theta_1 + \dot\theta_2)) \\ L_1 s\theta_2 s(\theta_1 + \theta_2)\dot\theta_1 + c(\theta_1 + \theta_2)(L_1\dot\theta_1c\theta_2 + L_2(\dot\theta_1 + \dot\theta_2)) \\ 0\end{bmatrix}\\ &= \begin{bmatrix} -L_1s\theta_1\dot\theta_1 - L_2 s(\theta_1 + \theta_2)\dot\theta_1 -L_2s(\theta_1 + \theta_2)\dot\theta_2 \\ L_1c\theta_1\dot\theta_1 + L_2 c(\theta_1 + \theta_2)\dot\theta_1 + L_2 c(\theta_1 + \theta_2)\dot\theta_2 \\ 0\end{bmatrix} \end{aligned}

4.3 Jacobian 矩阵#

4.3.1 数学定义#

Jacobian 是向量函数 $f(x) \in \mathbb{R}^m$ 对向量 $x \in \mathbb{R}^n$ 的一阶导数，其形式为一个矩阵

J(x) = \frac{\partial f(x)}{\partial x} \in \mathbb{R}^{m \times n}

对应微分形式为

\mathrm{d}f(x) = J(x)\mathrm{d}x

4.3.2 Jacobian 矩阵求速度关系#

设想我们得到了末端位置 $\mathcal{X} = \begin{bmatrix} x \\ y\\ z\end{bmatrix}$ 对关节变量 $q = \begin{bmatrix} q_1 \\ \vdots \\ q_n\end{bmatrix}$ 的正向运动学关系为

\mathcal{X} = T(q)

那么，通过求 Jacobian 矩阵 $J(q)$ ，并且让微分形式同时对时间 $t$ 求导，可以得到

\dot{\mathcal{X}} = J(q) \dot q

这个公式即是速度关系，实现了从构形空间到笛卡尔空间的速度映射。注意，通常这里所取的向量表达框架就是绝对框架 $\{0\}$ 。

平面2R速度关系的 Jacobian 解法

解：

直接得到正向运动学结果

\begin{aligned} ^{0}_{3}T(\theta_1, \theta_2) &= \begin{bmatrix} c(\theta_1 + \theta_2) & -s(\theta_1 + \theta_2) & 0 & L_1 c\theta_1 + L_2 c(\theta_1 + \theta_2)\\ s(\theta_1 + \theta_2) & c(\theta_1 + \theta_2) &0 &L_1 s\theta_1 + L_2 s(\theta_1 + \theta_2)\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ \end{aligned}

提取末端位置坐标，有

\mathcal{X} = \begin{bmatrix} L_1 c\theta_1 + L_2 c(\theta_1 + \theta_2)\\ L_1 s\theta_1 + L_2 s(\theta_1 + \theta_2)\end{bmatrix}

对时间 $t$ 求导，有

\dot{\mathcal{X}} = \begin{bmatrix} -L_1s\theta_1 - L_2 s(\theta_1 + \theta_2) &-L_2 s(\theta_1 + \theta_2)\\ L_1c\theta_1 + L_2 c(\theta_1 + \theta_2) &L_2 c(\theta_1 + \theta_2)\end{bmatrix}\begin{bmatrix} \dot\theta_1 \\ \dot\theta_2\end{bmatrix}

其中， $J(\theta_1, \theta_2) = \begin{bmatrix} -L_1s\theta_1 - L_2 s(\theta_1 + \theta_2) &-L_2 s(\theta_1 + \theta_2)\\ L_1c\theta_1 + L_2 c(\theta_1 + \theta_2) &L_2 c(\theta_1 + \theta_2)\end{bmatrix}$

这里的运算结果和前面的使用迭代方式的运算结果是一致的。

4.3.3 关节变量的 Jacobian 贡献#

相较于框架 $\{0\}$ （默认各个变量的左上标均为 $\ ^0$ ），考察各个关节变量对末端速度的贡献。设

\begin{aligned} \dot{\mathcal{X}} &= J_P(q)\dot{q} = \sum_{i=1}^n J_{P_i}(q)\dot{q}_i \\ \Omega &= J_O(q) \dot{q} = \sum_{i=1}^n J_{O_i}(q)\dot q_i \end{aligned}

其中， $\mathcal{X}$ 表示末端线速度， $\Omega$ 表示末端旋转角速度，线速度 Jacobian 矩阵 $J_P(q) = \begin{bmatrix} J_{P_1}(q) & J_{P_2}(q) & \dots & J_{P_n}(q)\end{bmatrix}$ ，角速度 Jacobian 矩阵 $J_O(q) = \begin{bmatrix} J_{O_1}(q) & J_{O_2}(q) &\dots &J_{O_n}(q)\end{bmatrix}$ 。

旋转关节#

有

\begin{aligned} J_{P_i}(q)\dot\theta_i &= \dot\theta_i\mathbf{\hat{z}}_i \times (\mathcal{X} - p_i) \\ J_{O_i}(q)\dot\theta_i &= \mathbf{\hat{z}}_i\dot\theta_i \end{aligned}

于是

\begin{aligned} J_{P_i}(q) &= \mathbf{\hat{z}}_i \times (\mathcal{X} - p_i) \\ J_{O_i}(q) &= \mathbf{\hat{z}}_i \end{aligned}

其中, $p_i$ 是关节 $i$ 的位置向量，也就是框架 $\{i\}$ 的原点 $p_{iorg}$

棱柱关节#

有

\begin{aligned} J_{P_i}(q)\dot d_i &= \mathbf{\hat{z}}_i \dot d_i\\ J_{O_i}(q)\dot d_i &= 0 \end{aligned}

于是

\begin{aligned} J_{P_i}(q) &= \mathbf{\hat{z}}_i \\ J_{O_i}(q) &= 0 \end{aligned}

4.3.4 运动旋量的 Jacobian#

前面所使用的 Jacobian 主要是关节变量和末端线速度之间或和末端角速度之间的 Jacobian 矩阵。我们可以将线速度和角速度的 Jacobian 矩阵沿行方向拼接起来，从而得到末端运动旋量 $\xi = \begin{bmatrix} \dot{\mathcal{X}} \\ \Omega\end{bmatrix}$ 关于各个关节变量的速度关系。

\xi = \begin{bmatrix} \dot{\mathcal{X}} \\ \Omega\end{bmatrix} = J(q) \dot q = \begin{bmatrix} J_P(q) \\ J_O(q)\end{bmatrix} \dot q = \begin{bmatrix} J_{P_1}(q) & \dots &J_{P_n}(q) \\ J_{O_1}(q) & \dots &J_{O_n}(q)\end{bmatrix} \begin{bmatrix} \dot q_1 \\ \vdots \\ \dot q_n\end{bmatrix}

对于旋转关节，有

J(q) = \begin{bmatrix} \mathbf{\hat{z}}_i \times (\mathcal{X} - p_i) \\ \mathbf{\hat{z}}_i\end{bmatrix}

对于棱柱关节，有

J(q) = \begin{bmatrix} \mathbf{\hat{z}}_i \\ 0\end{bmatrix}

平面2R速度关系的另一种 Jacobian 解法

解：

前向运动学结果为

\begin{aligned} ^{0}_{3}T(\theta_1, \theta_2) &= \begin{bmatrix} c(\theta_1 + \theta_2) & -s(\theta_1 + \theta_2) & 0 & L_1 c\theta_1 + L_2 c(\theta_1 + \theta_2)\\ s(\theta_1 + \theta_2) & c(\theta_1 + \theta_2) &0 &L_1 s\theta_1 + L_2 s(\theta_1 + \theta_2)\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ \end{aligned}

于是，有

\begin{aligned} \begin{bmatrix} J_{P_1} \\ J_{O_1}\end{bmatrix} &= \begin{bmatrix} \ ^{0}\mathbf{\hat{z}}_i \times (\ ^{0}p_3 - \ ^{0}p_1) \\ \ ^{0}\mathbf{\hat{z}}_1\end{bmatrix} = \begin{bmatrix} \begin{bmatrix} 0\\0\\1\end{bmatrix} \times \left(\begin{bmatrix} L_1 c\theta_1 + L_2c(\theta_1 + \theta_2) \\ L_1 s\theta_1 + L_2s(\theta_1 + \theta_2) \\0 \end{bmatrix} - \begin{bmatrix} 0\\0\\0\end{bmatrix}\right) \\ \begin{bmatrix} 0\\0\\1\end{bmatrix}\end{bmatrix} = \begin{bmatrix} -L_1 s\theta_1 - L_2 s(\theta_1 + \theta_2)\\ L_1 c\theta_1 + L_2 c(\theta_1 + \theta_2) \\ 0 \\ 0\\ 0\\ 1\end{bmatrix} \\ \begin{bmatrix} J_{P_2} \\ J_{O_2}\end{bmatrix} &= \begin{bmatrix} \ ^{0}\mathbf{\hat{z}}_i \times (\ ^{0}p_3 - \ ^{0}p_2) \\ \ ^{0}\mathbf{\hat{z}}_2\end{bmatrix} = \begin{bmatrix} \begin{bmatrix} 0\\0\\1\end{bmatrix} \times \left(\begin{bmatrix} L_1 c\theta_1 + L_2c(\theta_1 + \theta_2) \\ L_1 s\theta_1 + L_2s(\theta_1 + \theta_2) \\0 \end{bmatrix} - \begin{bmatrix} L_1c\theta_1\\L_1s\theta_1\\0\end{bmatrix}\right) \\ \begin{bmatrix} 0\\0\\1\end{bmatrix}\end{bmatrix} = \begin{bmatrix} - L_2 s(\theta_1 + \theta_2)\\ L_2 c(\theta_1 + \theta_2) \\ 0 \\ 0\\ 0\\ 1\end{bmatrix} \\ \end{aligned}

从而得到

\xi = \begin{bmatrix} \dot{\mathcal{X}} \\ \Omega\end{bmatrix} = \begin{bmatrix} J_{P_1} & J_{P_2} \\ J_{O_1} & J_{O_2} \end{bmatrix} \begin{bmatrix} \dot\theta_1 \\ \dot\theta_2\end{bmatrix}

J = \begin{bmatrix} J_{P_1} & J_{P_2} \\ J_{O_1} & J_{O_2} \end{bmatrix} = \begin{bmatrix} -L_1s\theta_1 - L_2 s(\theta_1 + \theta_2) &-L_2 s(\theta_1 + \theta_2)\\ L_1c\theta_1 + L_2 c(\theta_1 + \theta_2) &L_2 c(\theta_1 + \theta_2) \\ 0 &0 \\ 0 &0 \\ 0 &0 \\ 1 & 1\end{bmatrix}

化简后可以得到和之前一样的结果。

4.3.5 Jacobian 在不同框架下的切换#

前面所使用的 Jacobian 矩阵都是在框架 $\{0\}$ 中的向量表示。同样地，Jacobian 矩阵也可以在其他框架下表达。

设在框架 $\{A\}$ 中表达的 Jacobian 矩阵为 $^{A}J(q)$ ，那么其在框架 $\{B\}$ 中的对应表达 $^{B}J(q)$ 满足

^{A}J(q) = \begin{bmatrix} \ ^{A}_{B}R & 0 \\ 0 & \ ^{A}_{B}R\end{bmatrix} \ ^{B}J(q)

这是因为

\begin{aligned} \begin{bmatrix} ^A v \\ ^A\omega\end{bmatrix} &= \begin{bmatrix} \ ^{A}_{B}R & 0 \\ 0 & \ ^{A}_{B}R\end{bmatrix} \begin{bmatrix} ^B v \\ ^B\omega\end{bmatrix} \\ ^{A}\dot{\mathcal{X}} = \begin{bmatrix} ^A v \\ ^A\omega\end{bmatrix} &= \ ^{A}J(q)\dot q \\ ^{B}\dot{\mathcal{X}} = \begin{bmatrix} ^B v \\ ^B\omega\end{bmatrix} &= \ ^{B}J(q)\dot q \end{aligned}

结合在一起即可推导出上述结论。

特别需要注意的一点是 $\begin{bmatrix} ^A v \\ ^A\omega\end{bmatrix}$ 与 $\begin{bmatrix} ^B v \\ ^B\omega\end{bmatrix}$ 不是同一个运动旋量在不同框架下的向量表达。

4.3.6 运动学冗余#

Jacobian 子矩阵#

Jacobian 矩阵的形状不一定是方阵，在前面的例子中，Jacobian 的形状有 $2\times 2$ 的和 $6 \times 2$ 的。事实上这些矩阵都是空间 Jacobian 矩阵 $J(q) \in \mathbb{R}^{6\times n}$ 的子矩阵（其中 $n$ 为关节个数）。例如，在平面的机械臂问题中，实际上只需要 $\begin{bmatrix} \dot x \\\dot y \\ \omega_z\end{bmatrix}$ 三个速度变量即可完全描述机械臂的速度，而不需要剩下的三个速度变量 $\dot z, \omega_x, \omega_y$ 。因此，可以通过选取适当的 Jacobian 子矩阵以方便问题的求解。

（这一段的讲述目的是阐明后续运动学冗余的定义中为什么 Jacobian 矩阵的形状是 $J(q) \in \mathbb{R}^{m\times n}$ ，其中 $m$ 未必是6）

运动学冗余的定义#

定义: 关节变量的数目 $n$ 大于描述给定任务所必须的变量数目 $m$ 。
例如末端执行器的自由度为 $6$ 或 $3$ ，大于 $6$ 轴的空间机械臂和大于 $3$ 轴的平面臂都是运动学冗余的。
又如若目标任务仅是实现平面定位任务（只关注末端 $x, y$ 坐标, $m = 2$ ），那么三轴平面臂 ( $n = 3$ ) 也是运动学冗余的

运动学冗余并不是不好的，冗余能够为机械臂提供灵活、多样的运动。在实际应用中，关节活动范围往往有限制，此时加入运动学冗余可以弥补活动范围限制对末端运动的影响。

存在运动学冗余时的速度逆解#

求速度逆解的目标是给定末端速度 $\dot{\mathcal{X}}$ 和 Jacobian 矩阵 $J(q)$ ，目标是求取相应的关节速率 $\dot q$ 。

\dot{\mathcal{X}} = J(q)\dot q, \quad \dot{\mathcal{X}} \in \mathbb{R}^m, \quad \dot q \in \mathbb{R}^n

当 $m \lt n$ 时，运动学冗余会导致无穷多解的存在，此时求解方程可以用优化方法求解。例如采用正则化的最小二乘法

\min_{\dot q} \mathcal{L}(\dot q, \lambda) = \frac{1}{2}\dot q^\top W \dot q + \lambda^\top (\dot{\mathcal{X}} - J(q) \dot q)

有最优解

\dot q = W^{-1} J^\top(JW^{-1}J^\top)^{-1}\dot{\mathcal{X}}

4.3.7 奇异性#

如果 Jacobian 矩阵满秩且为方阵，则可以通过求逆的方法直接得到速度逆解。

\dot q = J^{-1}(q)\dot{\mathcal{X}}

使 Jacobian 矩阵不满秩（注意是不满秩而不是非方阵）的构形 $q$ 也是有可能存在的，我们称这些构形为 奇异构形 或 奇异点。

边界奇异点：末端执行器处于或接近工作空间边界时出现的奇异点。
内部奇异点：出现在远离工作空间边界的位置，通常是由两个或多个关节轴共线或某个特定构形所致的奇异点。

如果机械臂的构形位于一个奇异点，则其运动自由度减少，无论如何选择关节速率，都无法实现笛卡尔空间内某个方向或子空间的运动。

求解方阵 Jacobian 矩阵奇异点的方法通常是采用 Jacobian 矩阵的行列式，当行列式值为0时，即对应构形为奇异点。
如果 Jacobian 矩阵非方阵，可以用求非满秩的情况的方法求解奇异点

虽然在奇异点附近也可以用优化方法求解速度逆解，但是 Jacobian 矩阵是否满秩对实际中末端速度实现是有影响的。在临近奇异点的区域，实现给定末端速度所需要的关节速率会激增。

平面2R的线速度奇异点

解：

根据前面的结果

J(\theta_1, \theta_2) = \begin{bmatrix} -L_1s\theta_1 - L_2 s(\theta_1 + \theta_2) &-L_2 s(\theta_1 + \theta_2)\\ L_1c\theta_1 + L_2 c(\theta_1 + \theta_2) &L_2 c(\theta_1 + \theta_2)\end{bmatrix}

令 $\det J(\theta_1, \theta_2) = 0$ ，可以得到

\begin{aligned} \det J &= -L_1L_2s\theta_1c(\theta_1 + \theta_2) - L_2^2s(\theta_1 + \theta_2)c(\theta_1 + \theta_2) + L_1L_2c\theta_1s(\theta_1 + \theta_2) + L_2^2s(\theta_1 + \theta_2)c(\theta_1 + \theta_2) \\ &= L_1 L_2 s\theta_2 \end{aligned}

当 $\theta_2 = 0 \text{ or } \pi$ 时是机械臂位于奇异点，此时，末端只能沿臂的垂直方向运动，在二维平面上损失了一个自由度。

从此例中也可以看出为什么在接近奇异点的区域，需要提供的关节速率会无穷大

\dot \theta = J^{-1}(\theta) \dot{\mathcal{X}}

J^{-1}(\theta) = \frac{1}{L_1L_2s\theta_2}\begin{bmatrix} L_2c(\theta_1 + \theta_2) & L_2s(\theta_1 + \theta_2) \\ -L_1c\theta_1 - L_2 c(\theta_1 + \theta_2) & -L_1s\theta_1 - L_2 s(\theta_1 + \theta_2)\end{bmatrix}

这里分母上的 $s\theta_2$ 就导致了为什么关节速率会趋于无穷大。

4.4 静力学分析目标及力和力矩的传播公式#

静力学分析是在机械臂相对静止或匀速状态下的受力分析。目标是求解，为了保证平衡条件，各个关节应该提供的力或力矩是什么。

约定

$f_i$ 表示连杆 $i-1$ 施加在连杆 $i$ 上的力
$n_i$ 表示连杆 $i-1$ 施加在连杆 $i$ 上的力矩

于是根据连杆 $i$ 的平衡方程（忽略自重），可以得到

\begin{cases} &^{i}f_i - ^{i}f_{i+1} = 0\\ &^{i}n_i - ^{i}n_{i+1} -\ ^{i}p_{i+1} \times\ ^{i}f_{i+1} = 0 \end{cases}

其中 $^{i}p_{i+1} = ^{i}p_{(i+1)org}$ 。

这里需要注意的一个点是如何得到这两个方程。

在这个系统中，连杆 $i$ 只受到 $f_i$ 和 $f_{i+1}$ 两个力的作用，因此，根据刚体的静力学分析，直接把刚体视作一个质点作力部分的分析处理，得到第一个方程即力的平衡。
然而，力矩的加减法是在必须在同一参考点下才可以进行。而根据我们所设定的结果， $n_i$ 与 $n_{i+1}$ 分别是相对于关节 $i$ 和关节 $i+1$ 处的力矩（因为力 $f_i$ 和 $f_{i+1}$ 分别作用于关节 $i$ 和关节 $i+1$ 上），是不可以直接作力矩平衡的，需要把 $n_{i+1}$ 平移到关节 $i$ 处才可以。这种力矩的平移会带来一个额外的力矩项，即 $p_{i+1} \times f_{i+1}$ ，此时可以得到第二个方程。(可以参考朗道物理学 P112(实际是pdf文件的P125) )

于是传播公式为

\begin{cases} &^{i}f_{i+1} = \ ^{i}f_i \\ &^{i}n_{i+1} = \ ^{i}n_i + \ ^{i}p_{i+1} \times\ ^{i}f_{i+1} \end{cases}

注意 $f_i, n_i$ 是连杆 $i$ 提供的力和力矩，关节所能够提供的只能是 Z 轴方向的分量

旋转关节（力矩）: $\tau_i = \ ^{i} n_i^\top \ ^{i}\mathbf{\hat{z}}_i$
棱柱关节（力）: $\tau_i = \ ^{i}f_i^\top \ ^{i}\mathbf{\hat{z}}_i$

下面的一道例题，给出了求解静力学平衡的方法（力学的结果，通常是反向传播求解）

平面2R，求为了保持平衡状态需要在末端施加的作用力 $\mathcal{F}$

解：

前向运动学结果

\begin{aligned} ^{0}_{1}T &= \begin{bmatrix} c\theta_1 & -s\theta_1 & 0 & 0\\ s\theta_1 & c\theta_1 &0 &0\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ ^{1}_{2}T &= \begin{bmatrix} c\theta_2 & -s\theta_2 & 0 & L_1\\ s\theta_2 & c\theta_2 &0 &0\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ ^{2}_{3}T &= \begin{bmatrix} 1 & 0 & 0 & L_2\\ 0 & 1 &0 &0\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ ^{0}_{3}T(\theta_1, \theta_2) &= \begin{bmatrix} c(\theta_1 + \theta_2) & -s(\theta_1 + \theta_2) & 0 & L_1 c\theta_1 + L_2 c(\theta_1 + \theta_2)\\ s(\theta_1 + \theta_2) & c(\theta_1 + \theta_2) &0 &L_1 s\theta_1 + L_2 s(\theta_1 + \theta_2)\\ 0 &0 &1 &0 \\ 0 &0 &0 &1\end{bmatrix} \\ \end{aligned}

根据末端无施加的外力矩，但是有施加的外力 $\mathcal{F}$ ，有

\begin{aligned} ^{3}\mathcal{F} = \ ^{3} f_3 = \begin{bmatrix}^{3}f_x\\ ^{3}f_y \\ 0\end{bmatrix}, \quad \ ^{3}n_3 = 0 \end{aligned}

带入迭代公式

\begin{aligned} \ ^{2}f_2 &= \ ^{2}_{3}R \ ^{3}f_3 = \begin{bmatrix}^{3}f_x\\ ^{3}f_y \\ 0 \end{bmatrix}, \quad \ ^{2}n_2 = \ ^{2}_{3}R \ ^{3}n_3 + \ ^{2}p_{3} \times \ ^{2}f_2 = \begin{bmatrix} 0 \\ 0\\ L_2 \ ^{3}f_y\end{bmatrix}\\ \ ^{1}f_1 &= \ ^{1}_{2}R \ ^{2}f_2 = \begin{bmatrix} ^{3}f_xc\theta_2 - \ ^{3}f_ys\theta_2 \\ ^{3}f_x s\theta_2 + \ ^{3}f_yc\theta_2 \\ 0\end{bmatrix}, \quad\ ^{1}n_1 = \ ^{1}_{2}R \ ^{2}n_2 + \ ^{1}p_2 \times \ ^{1}f_1 = \begin{bmatrix} 0 \\ 0 \\ L_1^{3}f_xs\theta_2 + L_1 \ ^{3}f_yc\theta_2 + L_2 \ ^{3}f_y \end{bmatrix} \end{aligned}

注意这里的逆向传播不需要计算到连杆0。

于是取沿 Z 轴方向的分量，得到关节需要提供的力矩为

\begin{bmatrix} \tau_1 \\ \tau_2\end{bmatrix} = \begin{bmatrix}^{1}n_1^\top \ ^{1}\mathbf{\hat{z}}_1 \\ ^{2}n_2^\top \ ^{2}\mathbf{\hat{z}}_2\end{bmatrix} = \begin{bmatrix} L_1^{3}f_xs\theta_2 + L_1 \ ^{3}f_yc\theta_2 + L_2 \ ^{3}f_y \\ L_2 \ ^{3}f_y\end{bmatrix} = \begin{bmatrix} L_1 s\theta_2 &L_1c\theta_2 + L_2 \\ 0 & L_2\end{bmatrix}\begin{bmatrix} ^{3}f_x \\ ^{3}f_y\end{bmatrix}

4.4.1 力的 Jacobian 矩阵#

对比上面平面 2R 力学的结果

\tau = \begin{bmatrix} L_1 s\theta_2 &L_1c\theta_2 + L_2 \\ 0 & L_2\end{bmatrix}\begin{bmatrix} ^{3}f_x \\ ^{3}f_y\end{bmatrix}

和速度的关系

^{3}v_3 = \begin{bmatrix} L_1 s\theta_2 & 0\\ L_1c\theta_2 + L_2 & L_2\end{bmatrix}\begin{bmatrix} \dot \theta_1 \\ \dot \theta_2\end{bmatrix}

发现一条规律，若 $\dot{\mathcal{X}} = J(q)\dot q$ ，则应该有 $\tau = J^\top(q)\mathcal{F}$ 。事实上，这个结论是成立的，使用虚功原理可以证明这个结论。

根据虚功原理 (这个很出名，详细讲解很多，例如知乎某个讲解)，主动力做的虚功之和为0，因此

\tau^\top\delta q = \mathcal{F}^\top \delta \mathcal{X}, \quad \forall \delta q

根据 $\delta \mathcal{X} = J(q)\delta q$ ，带入有

\tau = J^\top(q) \mathcal{F}

这实现了从末端笛卡尔空间中的广义力到关节空间中的广义力的映射。