仿人机器人 - 2 运动学

本文为书籍 Introduction to Humanoid Robotics 的汉化, 主要借助翻译器 DeepL.

另外, 原书有管贻生教授的译作版本:《仿人机器人》(清华大学出版社, 2007年). 强烈推荐.

---

2 运动学 (Kinematics)

分析链节的位置和姿态与机构的关节角度之间关系的理论称为运动学. 它是机器人学的基础, 但同样的理论也用于计算机制图. 二者都需要能够在三维空间中清晰表现运动物体的数学和算法.

2.1 坐标变换 (Coordinate Transformations)

图 2.1 展示了在仿人机器人项目 [65] 期间开发的仿人机器人 HRP-2. 它的高度为 154 厘米, 包括电池在内的重量为 58 千克. 仅靠电池, 它就能行走约一小时. HRP-2 共有 30 个可独立控制的关节. 图 2.1(b) 显示了各关节的名称及其局部坐标位置.

2.1.1 世界坐标 (World Coordinates)

我们的首要任务是明确机器人各部分的位置. 为此, 我们在机器人初始位置的正下方定义一个特殊点(实际上, 它是通过腰部关节局部坐标原点的垂直线与地板的交点), 如图 2.2 所示. 以这一点为原点, 我们定义了一个固定坐标系, 其 轴朝前, 轴朝左, 轴朝上.

我们将这些坐标命名为 , 此后将用它来描述我们的机器人及其运动. 像这样的坐标称为世界坐标. 通过使用通用坐标系来定义机器人本身和周围物体, 我们可以检查脚是否安全着地, 手是否成功抓住物体, 是否与环境发生碰撞等.

使用世界坐标定义的位置称为绝对位置. 例如, 我们用以下三维向量来描述图 2.2 中左手尖端的绝对位置

$$\boldsymbol{p}_{h}= \begin{bmatrix} \boldsymbol{p}_{hx}\newline \boldsymbol{p}_{hy}\newline \boldsymbol{p}_{hz} \end{bmatrix}.$$

我们还将使用 "绝对姿态" 和 "绝对速度" 这两个术语来说明它们是在世界坐标中定义的.

2.1.2 局部坐标与齐次变换

让我们来看看手尖位置 是如何随着机器人肩膀的旋转而变化的. 从图 2.3(a) 我们可以看到, 左肩的绝对位置由向量 确定, 而向量 则表示手端相对于肩膀的位置. 从图中我们可以看到

$$\boldsymbol{p}_h=\boldsymbol{p}_a+\boldsymbol{r}.$$

在图 2.3(b) 的张开手臂姿势中, 可以通过引入另一个向量 来表示手的绝对末端位置, 该向量指向从肩部抬起的手.

$$\boldsymbol{p}_h=\boldsymbol{p}_a+\boldsymbol{r}'.\tag{2.1}$$

手通过向量从 到 的旋转抬起, 而肩膀则保持相同的位置 .

如图 2.3 所示, 让我们设想一个连接在左肩上的局部坐标系 . 与固定在地面上的世界坐标系不同, 局部坐标系会随着连接的关节一起 "移动".

局部坐标系 由肩部原点和描述 , , 轴的单位向量定义. 如图 2.3(a) 所示, 在手臂放下的初始姿势下, 三个单位向量 , 和 与 平行. 如图 2.3(b) 所示, 当机器人抬起手臂时, 绕 轴旋转, 角度为 .

肩部旋转量 和 之间的关系可用以下公式描述

$$\boldsymbol{e}_{ax}=\begin{bmatrix}1\\0\\0\end{bmatrix},\enspace\boldsymbol{e}_{ay}=\begin{bmatrix}0\\\cos\phi\\\sin\phi\end{bmatrix},\enspace\boldsymbol{e}_{az}=\begin{bmatrix}1\\-\sin\phi\\\cos\phi\end{bmatrix}.\tag{2.2}$$

由于是绕 轴旋转, 因此只有 和 通过 发生变化. 让我们定义一个 矩阵 如下:

$$\boldsymbol{R}_a\equiv\begin{bmatrix} \boldsymbol{e}_{ax} & \boldsymbol{e}_{ay} & \boldsymbol{e}_{az} \end{bmatrix}\tag{2.3}$$

利用矩阵 , 我们可以将图 2.3 中的 和 之间的关系描述如下:

$$\boldsymbol{r}'=\boldsymbol{R}_a\boldsymbol{r}.\tag{2.4}$$

换句话说, 向量乘以矩阵 即可旋转. 我们将在第 2.2 节中详细介绍这一点.

现在, 让我们定义手端点在局部坐标系 中的位置 . 左上角的 表示它位于由 定义的局部坐标系中. 根据图 2.3(a), 我们可以得到

$$^a\boldsymbol{p}_h=\boldsymbol{r}.\tag{2.5}$$

在图 2.3 所示的示例中, 和左臂作为一个整体移动, 因此向量 保持不变.

为了描述左臂末端的位置, 我们使用了两种不同的符号.

• 从世界坐标 看手末端 的位置.
• 从局部坐标 看手末端 的位置.

两者之间的关系如下. 根据 (2.1), (2.4) 和 (2.5), 我们可以得到

$$\boldsymbol{p}_h=\boldsymbol{p}_a+\boldsymbol{R}_a{^a\boldsymbol{p}}_h.\tag{2.6}$$

公式 (2.6) 也可改写为

$$\begin{bmatrix}\boldsymbol{p}_h\\1\end{bmatrix}= \begin{bmatrix}\boldsymbol{R}_a&\boldsymbol{p}_a\\0\enspace0\enspace0&1\end{bmatrix} \begin{bmatrix}{^a\boldsymbol{p}}_h\\1\end{bmatrix}.\tag{2.7}$$

在这里, 我们在矩阵中添加了 0 和 1 以匹配维数, 使其符合 (2.6). 等式右侧的 矩阵来自位置向量 和旋转矩阵 的组合. 可以写成

$$\boldsymbol{T}_a\equiv\begin{bmatrix}\boldsymbol{R}_a&\boldsymbol{p}_a\\0\enspace0\enspace0&1\end{bmatrix}.$$

这样的矩阵称为齐次变换矩阵.

同样的矩阵在计算机图形学中被称为仿射变换矩阵. 两者之间存在区别, 因为在仿射变换中, 不一定是一个简单的旋转矩阵 ( 第 2.2 节) , 还可能包括缩放和剪切.

齐次变换矩阵可将用手臂局部坐标描述的点转换为世界坐标

$$\begin{bmatrix}\boldsymbol{p}\\1\end{bmatrix}=\boldsymbol{T}_a\begin{bmatrix}{^a\boldsymbol{p}}\\1\end{bmatrix}.$$

需要注意的是, 点 可以是左臂上的任意一点, 因此手臂的形状可以用它的集合来表示. 另一方面, 手臂的参数 (位置和朝向) 则由矩阵 单独表示. 一般来说, 我们可以说齐次变换本身就能描述物体的位置和姿态.

2.1.3 局部坐标系上的局部坐标系

我们可以定义一个以另一个局部坐标系为父坐标系的局部坐标系. 图 2.4(a) 显示了一个以 为父坐标系的局部坐标系 . 与下臂一起运动, 当肘部伸直时, 的坐标轴与 的坐标轴共线. 我们将肘关节的旋转角度定义为 . 我们还可以定义单位向量 , , , 它们分别是 的 , , 轴. 它们的定义如下

$$^a\boldsymbol{e}_{bx}=\begin{bmatrix}\cos\theta\\0\\\sin\theta\end{bmatrix},\enspace^a\boldsymbol{e}_{by}=\begin{bmatrix}0\\1\\0\end{bmatrix},\enspace^a\boldsymbol{e}_{bz}=\begin{bmatrix}-\sin\theta\\0\\\cos\theta\end{bmatrix}.\tag{2.8}$$

由于肘部绕 轴旋转, 只有 和 这两个向量会因 而改变. 让我们定义一个矩阵 为三个单位向量的组合.

$${^a\boldsymbol{R}}_b\equiv\begin{bmatrix} {^a\boldsymbol{e}}_{bx} & {^a\boldsymbol{e}}_{by} & {^a\boldsymbol{e}}_{bz} \end{bmatrix}\tag{2.9}$$

将 空间中的 (图 2.4 (a)) 转换为 空间中的 (图 2.4 (b)) 的过程如下

$$\begin{bmatrix}{^a\boldsymbol{p}}_h\\1\end{bmatrix}={^a\boldsymbol{T}}_b\begin{bmatrix}{^b\boldsymbol{p}}_h\\1\end{bmatrix}.\tag{2.10}$$

在这里, 我们对齐次变换 定义如下

$${^a\boldsymbol{T}}_b \equiv \begin{bmatrix}{^a\boldsymbol{R}}_b & {^a\boldsymbol{p}}_b \\ 0\enspace0\enspace0 & 1 \end{bmatrix}.$$

注意, 是从 看 的原点.

将 (2.10) 的结果与 (2.7) 结合起来, 我们就得到了将 空间中定义的点转换为世界坐标的方程. 下面的示例将转换空间 中的机械手端点

$$\begin{bmatrix}{\boldsymbol{p}}_h\\1\end{bmatrix}={\boldsymbol{T}}_a{^a\boldsymbol{T}}_b\begin{bmatrix}{^b\boldsymbol{p}}_h\\1\end{bmatrix}.\tag{2.11}$$

让我们把等式右边的齐次变换乘积定义为一个矩阵, 并称之为

$${\boldsymbol{T}}_b\equiv{\boldsymbol{T}}_a{^a\boldsymbol{T}}_b.$$

矩阵 是一种齐次变换, 实际上就是用世界坐标描述的 , 它描述了前臂的位置和姿态. 描述的是肩部转动的幅度. 因此我们可以看到, 的描述显示了肩部和肘部旋转的影响.

2.1.4 齐次变换与链式法则

我们上面描述的内容可以加以推广. 假设我们有一个连接局部坐标 到 的机制. 如果我们有一个描述相邻局部坐标 和 的齐次变换, 例如:

$${^i\boldsymbol{T}}_{i+1},$$

重复上述过程, 我们可以得到以下等式

$${\boldsymbol{T}}_{N}={\boldsymbol{T}}_{1}{^1\boldsymbol{T}}_{2}{^2\boldsymbol{T}}_{3} \cdots {^{N-1}\boldsymbol{T}}_{N}.\tag{2.12}$$

这里的 是描述第 个关节位置和姿态的齐次变换. 要在这个关节的末端添加另一个关节, 我们需要将其与齐次变换矩阵做右乘.

这种乘以齐次变换以计算坐标变换矩阵的方法被称为链式法则 [87]. 链式法则使我们能够计算具有多个关节的手臂的运动学, 而不会过于复杂.

2.2 旋转运动的特性

在上一节中, 我们用一个 矩阵描述了机器人关节的旋转, 但未作任何解释. 这个矩阵称为旋转矩阵. 它描述了关节的姿态, 也记述了旋转运动. 在本节中, 我们将利用旋转矩阵了解三维旋转的特点. 为简化起见, 我们将只讨论绕原点的旋转.

2.2.1 滚动角 (Roll) , 俯仰角 (Pitch) 和偏航角 (Yaw) 记号

旋转的基本原理是绕 , 和 轴的旋转, 我们分别称之为 "滚动" (Roll) , "俯仰" (Pitch) 和 "偏航" (Yaw) . 在图 2.5 中, 我们展示了一个位于 平面上的三角形, 它经历了 "滚动" , "俯仰" 和 "偏航".

下面列出了旋转轴, 名称和常用符号.

Rotation Axis	Name	Notation
axis	Roll
axis	Pitch
axis	Yaw

要使物体滚动, 俯仰和偏航一个给定角度, 我们需要使用以下矩阵

$$\boldsymbol{R}_x(\phi)= \begin{bmatrix}1 & 0 & 0 \\ 0 & \cos\phi &-\sin\phi \\ 0 & \sin\phi & \cos\phi\end{bmatrix}$$ $$\boldsymbol{R}_y(\phi)= \begin{bmatrix}\cos\phi & 0 & \sin\phi \\ 0 & 1 & 0 \\ -\sin\phi & 0 & \cos\phi\end{bmatrix}$$ $$\boldsymbol{R}_x(\phi)= \begin{bmatrix} \cos\phi &-\sin\phi & 0\\ \sin\phi & \cos\phi & 0 \\ 0 & 0 & 1 \end{bmatrix}.$$

如果我们将一个点 绕原点滚动, 俯仰, 然后偏转, 它就会移动到另一个点,

$$\boldsymbol{p}'=\boldsymbol{R}_z(\psi)\boldsymbol{R}_y(\theta)\boldsymbol{R}_x(\phi)\boldsymbol{p}.$$

我们可以将其改写为

$$\boldsymbol{p}'=\boldsymbol{R}_{rpy}(\phi,\theta,\psi)\boldsymbol{p}.$$

在此, 我们引入以下符号

$$\begin{align*}\boldsymbol{R}_{rpy}(\phi,\theta,\psi) & \equiv\boldsymbol{R}_z(\psi)\boldsymbol{R}_y(\theta)\boldsymbol{R}_x(\phi)\\ &=\begin{bmatrix}c_\psi c_\theta & -s_\psi c_\phi+c_\psi s_\theta s_\phi & s_\psi s_\phi+c_\psi s_\theta c_\phi \\ s_\psi c_\theta & c_\psi c_\phi+s_\psi s_\theta s_\phi & -c_\psi s_\phi+s_\psi s_\theta c_\phi \\ -s_\theta & c_\theta s_\phi & c_\theta c_\phi \end{bmatrix},\end{align*}\tag{2.13}$$

其中, , , , 以此类推.

矩阵 也是一个旋转矩阵, 众所周知, 三维空间中的任何给定姿态都可以用它来实现. 因此, 任意姿态都可以用一组三个数字 来表示, 这组数字被称为滚动-俯仰-偏航符号或 Z-Y-X 欧拉角.

由于 "滚动-俯仰-偏航" 符号易于理解, 因此常用于记录船舶, 飞机和机器人的姿态.

2.2.2 旋转矩阵的意义

有两种不同的方法来解释旋转矩阵. 一种是作为旋转向量的算子. 图 2.6(a) 显示了向量 通过旋转矩阵 旋转后得到的新向量

$$\boldsymbol{p}'=\boldsymbol{R}\boldsymbol{p}$$

其中 和 位于同一坐标空间.

解释 的第二种方法是将其视为局部坐标空间的姿态. 图 2.6(b) 显示了局部坐标轴 和在该局部坐标中定义的点 . 旋转矩阵定义为坐标轴的集合, 即

$$\boldsymbol{R}\equiv\begin{bmatrix} \boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z \end{bmatrix}.\tag{2.14}$$

该点在世界坐标中表示为

$$\boldsymbol{p}=\boldsymbol{R}\bar{\boldsymbol{p}}.$$

在这种情况下, 我们只是改变了一个静止点的视角, 而没有任何实际的运动.

我们怎样才能弄清旋转矩阵用于哪种含义, (1) 旋转向量的算子, 还是 (2) 局部坐标系的姿态? 我们可以通过检查计算前后该点是否存在于相同的坐标空间来了解. 不过, 大多数情况下, 这一点从上下文中就能看出来.

2.2.3 计算旋转矩阵的逆矩阵

假设我们有一个显示本地坐标系姿态的旋转矩阵. 我们将把该坐标系的单位向量定义为 , 和 . 由于这些单位向量互成直角, 我们可以说

$$\boldsymbol{e}_i^T\boldsymbol{e}_j=\left\{ \begin{array}{ll}1 & (i=j)\\0 & (i\neq j)\end{array}\right..$$

这里的 是两个向量的内积. 如果我们计算转置 和 的积, 就会得到

$$\begin{align*} \boldsymbol{R}^T\boldsymbol{R} &= \begin{bmatrix}\boldsymbol{e}_x^T\\\boldsymbol{e}_y^T\\\boldsymbol{e}_z^T\end{bmatrix}\begin{bmatrix}\boldsymbol{e}_x & \boldsymbol{e}_y & \boldsymbol{e}_z\end{bmatrix}=\begin{bmatrix}\boldsymbol{e}_x^T\boldsymbol{e}_x & \boldsymbol{e}_x^T\boldsymbol{e}_y& \boldsymbol{e}_x^T\boldsymbol{e}_z\\ \boldsymbol{e}_y^T\boldsymbol{e}_x & \boldsymbol{e}_y^T\boldsymbol{e}_y & \boldsymbol{e}_y^T\boldsymbol{e}_z \\ \boldsymbol{e}_z^T\boldsymbol{e}_x & \boldsymbol{e}_z^T\boldsymbol{e}_y & \boldsymbol{e}_z^T\boldsymbol{e}_z\end{bmatrix} \\ &= \begin{bmatrix}1&0&0\\0&1&0\\0&0&1\end{bmatrix} = \boldsymbol{E}\enspace(3\times3\enspace\text{identity matrix}) .\end{align*}$$

由此我们可以肯定地说, . 如果我们将等式两边右乘 , 得到

$$\boldsymbol{R}^T=\boldsymbol{R}^{-1}.\tag{2.15}$$

因此, 当我们对旋转矩阵进行转置时, 就会得到它的逆矩阵. 具有这种特性的矩阵称为正交矩阵.

2.2.4 角速度向量

接下来, 我们将定义一种在三维空间中记录旋转速度的方法. 最简单的例子如图 2.7 所示. 该图显示一个圆柱体以 1 [rad/s] 的速度绕 Z 轴旋转. 在这种情况下, 该物体的旋转速度 可以用以下公式描述

$$\boldsymbol{\omega}=\begin{bmatrix}0\\0\\1\end{bmatrix}.\tag{2.16}$$

这就是角速度向量. 该向量中的每个元素的单位都是 [rad/s].

角速度向量 () 具有以下特征.

1. 被定义为单位向量 标量值

   设 为旋转轴上的单位向量, 为旋转速度 (标量值) . 那么物体的角速度向量就是它们的乘积

   $$\boldsymbol{\omega}=\boldsymbol{a}\dot{q}.\tag{2.17}$$

2. 描述旋转物体上所有点的速度

   令 为一个向量, 表示物体表面上的给定点 . 我们还可以将该向量的基点定义为旋转轴上的某处. 在这种情况下, 点的速度将由 描述. 算子是两个向量的叉积或外积, 其定义如下.

   

   通过使用 和 这两个向量, 我们可以得到一个新的向量 , 它具有以下特征 (图 2.8)

   $$\begin{align*}&|\boldsymbol{v}|=|\boldsymbol{\omega}||\boldsymbol{p}|\sin\theta \\ &(\boldsymbol{v}\perp\boldsymbol{\omega})\cap(\boldsymbol{v}\perp\boldsymbol{p}).\end{align*}$$

   然而, 有两个向量满足这一条件, 因此我们使用右手螺旋定则, 选择符合右手螺旋转动时移动方向的那个向量. 可以表示为

   $$\boldsymbol{v}=\boldsymbol{\omega}\times\boldsymbol{p}\tag{2.18}$$

   称为 " 与 的叉乘".

   当给定 和 的元素时, 可以计算出如下的叉乘. 作为练习, 我们还可以检验一下下面的等式是否可以从上面的等式中推导出来.

   $$\boldsymbol{\omega}\times\boldsymbol{p}=\begin{bmatrix}\omega_x\\\omega_y\\\omega_z\end{bmatrix}\times\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}\equiv\begin{bmatrix}\omega_yp_z-\omega_zp_y\\\omega_zp_x-\omega_xp_z\\\omega_xp_y-\omega_yp_x\end{bmatrix}\tag{2.19}$$

   我们可以说, 叉乘的定义使其符合角速度的性质.

   从这里开始, 我们将用它来计算力矩和角动量. 在电磁学中, 叉乘本身非常重要. 弗莱明右手定则和左手定则本身就是叉乘的结果.

   下式为角速度向量增加了 "物理意义".

   $$(\text{顶点速度})=\boldsymbol{\omega}\times(\text{顶点位置})$$

   如图 2.9 所示, 我们用上面的方程将以 旋转的橄榄球形物体 (椭圆体) 的表面速度形象化. 在旋转物体上, 速度的大小和方向会因表面上的点的位置而不同. 角速度向量只用三个元素就能表示这一事实.

   如果你想了解严格的数学事实, 角速度向量被称为伪向量 (或轴向向量) , 它与法向量的处理方法有些不同[105]. 不过在本书的范围内, 将其视为法向量是没有问题的. 从这个意义上说, 力矩和角动量也同样使用伪向量表示.

   

3. 也可以旋转

   让我们用某个旋转矩阵 将 (2.17) 的两边相乘

   $$\boldsymbol{R}\boldsymbol{\omega}=\boldsymbol{R}\boldsymbol{a}\dot{q}.\tag{2.20}$$

   如果我们引入以下新的向量,

   $$\boldsymbol{\omega}'=\boldsymbol{R}\boldsymbol{\omega},\quad\boldsymbol{a}'=\boldsymbol{R}\boldsymbol{a}$$

   那么我们可以将 (2.20) 改写为

   $$\boldsymbol{\omega}'=\boldsymbol{a}'\dot{q}.$$

   这与 (2.17) 中的定义相吻合, 因此我们可以看到, 是绕着由 定义的轴的新角速度向量. 因此我们可以说, 角速度向量可以通过旋转矩阵 直接变换.

   

   接下来, 让我们尝试使用相同的旋转矩阵 旋转一个角速度向量, 一个位置向量和相应的速度向量, 如图 2.10 所示, 由此得到

   $$\boldsymbol{\omega}'=\boldsymbol{R}\boldsymbol{\omega},\quad\boldsymbol{p}'=\boldsymbol{R}\boldsymbol{p},\quad\boldsymbol{v}'=\boldsymbol{R}\boldsymbol{v}.$$

   由于这三个向量之间的空间关系不会因旋转而改变, 因此必须保留叉乘的定义, 所以

   $$\boldsymbol{v}'=\boldsymbol{\omega}'\times\boldsymbol{p}'.\tag{2.21}$$

   将旋转后的向量 , 和 分别代入它们的原值 , 和 , 即可得出

   $$\boldsymbol{R}(\boldsymbol{\omega}\times\boldsymbol{p})=(\boldsymbol{R}\boldsymbol{\omega})\times(\boldsymbol{R}\boldsymbol{p}).\tag{2.22}$$

2.2.5 旋转矩阵和角速度向量的导数

现在, 让我们研究一下角速度向量 和旋转矩阵 之间的关系. 在第 2.2.2 节中, 我们看到旋转矩阵 给出了顶点的局部坐标和世界坐标之间的关系

$$\boldsymbol{p}=\boldsymbol{R}\bar{\boldsymbol{p}}.\tag{2.23}$$

如果我们将该方程关于时间求导, 就能得到该点在世界坐标空间中的速度. 在局部坐标空间中, 点 的位置不会移动,

$$\dot{\boldsymbol{p}}=\dot{\boldsymbol{R}}\bar{\boldsymbol{p}}.\tag{2.24}$$

在这里, 我们利用 的关系将 代入, 得到

$$\dot{\boldsymbol{p}}=\dot{\boldsymbol{R}}\boldsymbol{R}^T\boldsymbol{p}.\tag{2.25}$$

我们可以用这个等式来计算物体上一个顶点的速度.

从上一节中我们知道 , 因此我们可以确定以下关系

$$\boldsymbol{\omega}\times\boldsymbol{p}=\dot{\boldsymbol{R}}\boldsymbol{R}^T\boldsymbol{p}.\tag{2.26}$$

如果我们还记得如下的叉乘计算公式 (2.19),

$$\boldsymbol{\omega}\times\boldsymbol{p}=\begin{bmatrix}\omega_x\\\omega_y\\\omega_z\end{bmatrix}\times\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}\equiv\begin{bmatrix}\omega_yp_z-\omega_zp_y\\\omega_zp_x-\omega_xp_z\\\omega_xp_y-\omega_yp_x\end{bmatrix}.\tag{2.27}$$

我们可以将其改写为 矩阵的乘积, 从而得到

$$\boldsymbol{\omega}\times\boldsymbol{p}= \begin{bmatrix}0&-\omega_z&\omega_y \\ \omega_z&0&-\omega_x \\ -\omega_y&\omega_x&0\end{bmatrix} \times\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix}\equiv\boldsymbol{S}\boldsymbol{p}.\tag{2.28}$$

我们刚刚定义的矩阵有一个有趣的特征. 如果我们对这个矩阵进行转置, 就会得到只反转了符号的相同矩阵. 这样的矩阵被称为反对称矩阵, 并满足

$$\boldsymbol{S}^T=-\boldsymbol{S}.\tag{2.29}$$

通过比较 (2.26) 和 (2.28) 可以看出, 实际上是一个反对称矩阵. 具体证明如下. 首先, 旋转矩阵的转置也是它的逆矩阵. 那么

$$\boldsymbol{R}\boldsymbol{R}^T=\boldsymbol{E}.\tag{2.30}$$

此外, 如果我们对这个方程关于时间求导, 就会得到以下结果.

矩阵的导数就是矩阵所有分量的导数. 必须注意不要改变乘法的顺序.

$$\begin{align*}\dot{\boldsymbol{R}}\boldsymbol{R}^T+\boldsymbol{R}\dot{\boldsymbol{R}}^T&=\boldsymbol{0} \\ (\dot{\boldsymbol{R}}\boldsymbol{R}^T)^T&=-\dot{\boldsymbol{R}}\boldsymbol{R}^T.\end{align*}$$

这就证明, 实际上是一个反对称矩阵.

在本书中, 我们将从反对称矩阵中提取的三维向量称为 "楔 (wedge)" 运算. 从三维向量做出反对称矩阵将被称为取 "帽 (hat)" 运算, 如下所示

$$\begin{bmatrix}0&-\omega_z&\omega_y \\ \omega_z&0&-\omega_x \\ -\omega_y&\omega_x&0\end{bmatrix}^\lor = \begin{bmatrix}\omega_x \\ \omega_y \\ \omega_z\end{bmatrix},\\ \begin{bmatrix}\omega_x \\ \omega_y \\ \omega_z\end{bmatrix}^\land = \begin{bmatrix}0&-\omega_z&\omega_y \\ \omega_z&0&-\omega_x \\ -\omega_y&\omega_x&0\end{bmatrix}.$$

因此, 一个叉乘可以写成下面这样.

$$\boldsymbol{\omega}\times\boldsymbol{p}=(\boldsymbol{\omega}^\land)\times\boldsymbol{p}.$$

为了让这个等式更容易看懂, 我们可以把 放在最上面.

很多教科书和论文都用 代替 , 以强调这是叉乘.

$$\boldsymbol{\omega}\times\boldsymbol{p}=\hat{\boldsymbol{\omega}}\times\boldsymbol{p}.$$

利用这些描述方法, 我们可以在 (2.26) 中用以下公式描述旋转矩阵与其角速度之间的关系

$$\hat{\boldsymbol{\omega}}=\dot{\boldsymbol{R}}\boldsymbol{R}^T\tag{2.31}$$

或

$$\boldsymbol{\omega}=(\dot{\boldsymbol{R}}\boldsymbol{R}^T)^\lor\tag{2.32}$$

2.2.6 角速度向量与矩阵指数的积分

让我们来看看如何积分角速度向量并得到旋转矩阵. 将 (2.31) 的两边右乘 , 我们可以得到

$$\dot{\boldsymbol{R}}=\hat{\boldsymbol{\omega}}\boldsymbol{R}\tag{2.33}$$

这个方程非常重要, 因为它给出了角速度向量和旋转矩阵之间的关系. 我们称这个方程为旋转基本方程. 该方程是矩阵 的微分方程, 因此如果对其进行积分, 就能得到旋转矩阵. 如果初始条件为 , 且角速度 为常数, 则解为

$$\boldsymbol{R}(t)=\boldsymbol{E}+\hat{\boldsymbol{\omega}}t+\frac{(\hat{\boldsymbol{\omega}}t)^2}{2!}+\frac{(\hat{\boldsymbol{\omega}}t)^3}{3!}+\cdots.\tag{2.34}$$

将上述公式代入 (2.33) 即可得出正确结论. 利用与指数函数的类比, 我们将用 来描述解, 并将其称为矩阵指数. 因此, 我们定义

$$e^{\hat{\boldsymbol{\omega}}t}\equiv\boldsymbol{E}+\hat{\boldsymbol{\omega}}t+\frac{(\hat{\boldsymbol{\omega}}t)^2}{2!}+\frac{(\hat{\boldsymbol{\omega}}t)^3}{3!}+\cdots.\tag{2.35}$$

这个无穷级数可以简化 [101]. 首先, 我们将 分解成一个单位向量 和一个标量

$$\boldsymbol{\omega}=\boldsymbol{a}\omega,\quad\omega\equiv\|\boldsymbol{\omega}\|,\|\boldsymbol{a}\|=1.$$

由于 的特性, 所有高幂次项中的 都可以用 代替. 此外, 利用 , 的泰勒级数, 我们可以得到以下方程

$$e^{\hat{\boldsymbol{\omega}}t} = \boldsymbol{E}+\hat{\boldsymbol{a}}\sin(\omega t)+\hat{\boldsymbol{a}}^2(1-\cos(\omega t)).\tag{2.36}$$

公式 (2.36) 被称为罗德里格斯公式 (Rodrigues' formula) , 它可以直接从恒定角速度向量得到旋转矩阵. 这个方程和旋转基本方程是本书中最重要的方程.

方程 (2.36) 可以看作是给出了绕单位向量 定义的轴的旋转角度 [rad]. 用 代替旋转角度, 我们得到

$$e^{\hat{\boldsymbol{a}}\theta} = \boldsymbol{E}+\hat{\boldsymbol{a}}\sin\theta+\hat{\boldsymbol{a}}^2(1-\cos\theta).\tag{2.37}$$

该方程大量用于运动学计算.

请注意它与欧拉公式 的相似性, 理查德-费曼 (Richard Feynman) 称其为 "our jewel")[105].

2.2.7 矩阵对数

在定义了矩阵指数之后, 让我们来定义它的逆, 矩阵对数

$$\ln\boldsymbol{R}=\ln e^{\hat{\boldsymbol{\omega}}}\equiv\hat{\boldsymbol{\omega}}.\tag{2.38}$$

它用于从给定的旋转矩阵中获取角速度向量. 在一秒钟内积分的角速度向量将等同于旋转矩阵所描述的旋转

$$\boldsymbol{\omega}=(\ln\boldsymbol{R})^\lor.$$

实际计算结果为

$$(\ln\boldsymbol{R})^\lor=\left\{ \begin{align*} &\begin{bmatrix}0&0&0\end{bmatrix}^T &(\text{if }\boldsymbol{R}=\boldsymbol{E}) \\ &\frac{\pi}{2}\begin{bmatrix}r_{11}+1\\r_{22}+1\\r_{33}+1\end{bmatrix} &(\text{else if }\boldsymbol{R}\text{ is diagonal}) \\ &\theta\frac{\boldsymbol{l}}{\|\boldsymbol{l}\|} &(\text{otherwise}) \end{align*} \right.\tag{2.39}$$

给定

$$\begin{align*} \boldsymbol{R}&= \begin{bmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\ r_{31}&r_{32}&r_{33} \end{bmatrix},\\ \boldsymbol{l}&= \begin{bmatrix} r_{32}-r_{23}\\r_{13}-r_{31}\\r_{21}-r_{12} \end{bmatrix},\\ \theta&=\text{atan2}(\|\boldsymbol{l}\|,r_{11}+r_{22}+r_{33}-1). \end{align*}$$

其中 是给出向量 与 轴夹角的函数. 请参阅教科书 [101] 和相关论文 [124] 了解其精确推导.

利用矩阵指数和矩阵对数, 我们可以在两个给定的旋转矩阵 和 之间进行如下插值运算.

1. 获取连接两个矩阵的旋转矩阵.
2. 从旋转矩阵中获取等效角速度向量.
3. 世界坐标中的角速度向量为:
4. 插值为 ​

2.3 三维空间中的速度

在本节中, 我们将平移运动添加到旋转运动中, 并考虑物体在三维空间中自由运动的速度.

2.3.1 单一物体的线速度和角速度

如图 2.11 所示, 物体在三维空间中的运动状态可以用物体中参考点 的位置和物体姿态的旋转矩阵 来描述. 换句话说, 一对 代表了物体上的局部坐标系. 对于在局部坐标中位于 的物体上的一个点, 其在世界坐标中的位置由以下公式给出

$$\boldsymbol{p}_k=\boldsymbol{p}+\boldsymbol{R}\bar{\boldsymbol{p}}_k.\tag{2.40}$$

假设这个物体在空间中翻滚, 同时做平移和旋转运动. 点 的速度可以通过对前述方程关于时间求导得出

$$\begin{align*} \dot{\boldsymbol{p}}_k &=\dot{\boldsymbol{p}}+\dot{\boldsymbol{R}}\bar{\boldsymbol{p}}_k\\ &=\boldsymbol{v}+\hat{\boldsymbol{\omega}}\boldsymbol{R}\bar{\boldsymbol{p}}_k\\ &=\boldsymbol{v}+\boldsymbol{\omega}\times(\boldsymbol{R}\bar{\boldsymbol{p}}_k), \end{align*} \tag{2.41}$$

其中, , 的定义如下

$$\boldsymbol{v}\equiv\dot{\boldsymbol{p}}\tag{2.42}$$ $$\boldsymbol{\omega}\equiv(\dot{\boldsymbol{R}}\boldsymbol{R}^T)^\lor.\tag{2.43}$$

利用 (2.40) 对 (2.41) 进行代换, 我们得到

$$\dot{\boldsymbol{p}}_k =\boldsymbol{v}+\boldsymbol{\omega}\times(\boldsymbol{p}_k-\boldsymbol{p}). \tag{2.44}$$

物体中任何一点的速度都可以用这个等式计算出来. 因此, 我们可以得出以下结论

• 物体在三维空间中的运动可以用一个 6 维向量 来表示, 它是物体线速度 和角速度 的组合.

2.3.2 两个物体的线速度和角速度

接下来, 我们将考虑两个在三维空间中运动的物体. 假设我们已经得到了这两个物体的局部坐标, 如下所示

$$\boldsymbol{T}_1= \begin{bmatrix} \boldsymbol{R}_1 & \boldsymbol{p}_1 \\ 0\enspace0\enspace0 & 1 \end{bmatrix},\tag{2.45}$$ $${^1\boldsymbol{T}_2}= \begin{bmatrix} \boldsymbol{R}_d & \boldsymbol{p}_d \\ 0\enspace0\enspace0 & 1 \end{bmatrix}.\tag{2.46}$$

第二个物体相对于第一个物体的位置和姿态为 . 那么

$$\begin{align*} \boldsymbol{T}_2 &=\boldsymbol{T}_1{^1\boldsymbol{T}_2}\\ &= \begin{bmatrix} \boldsymbol{R}_1 & \boldsymbol{p}_1 \\ 0\enspace0\enspace0 & 1 \end{bmatrix} \begin{bmatrix} \boldsymbol{R}_d & \boldsymbol{p}_d \\ 0\enspace0\enspace0 & 1 \end{bmatrix}\\ &=\begin{bmatrix} (\boldsymbol{R}_1\boldsymbol{R}_d) & (\boldsymbol{p}_1+\boldsymbol{R}_1\boldsymbol{p}_d) \\ 0\enspace0\enspace0 & 1 \end{bmatrix}. \end{align*}\tag{2.47}$$

因此, 第二个物体在世界坐标中的位置和姿态为

$$\boldsymbol{p}_2=\boldsymbol{p}_1+\boldsymbol{R}_1\boldsymbol{p}_d\tag{2.48}$$ $$\boldsymbol{R}_2=\boldsymbol{R}_1\boldsymbol{R}_d.\tag{2.49}$$

第二个物体的线速度可以通过对 (2.48) 关于时间求导得到.

$$\begin{align*} \boldsymbol{v}_2 &=\frac{d}{dt}(\boldsymbol{p}_1+\boldsymbol{R}_1\boldsymbol{p}_d)\\ &=\dot{\boldsymbol{p}}_1+\dot{\boldsymbol{R}}_1\boldsymbol{p}_d+\boldsymbol{R}_1\dot{\boldsymbol{p}}_d\\ &=\boldsymbol{v}_1+\hat{\boldsymbol{\omega}}_1\boldsymbol{R}_1\boldsymbol{p}_d+\boldsymbol{R}_1\boldsymbol{v}_d\\ &=\boldsymbol{v}_1+\boldsymbol{\omega}_1\times(\boldsymbol{R}_1\boldsymbol{p}_d)+\boldsymbol{R}_1\boldsymbol{v}_d. \end{align*}$$

其中, , .

这里, 利用 (2.48) 并重新排列其元素, 我们得到

$$\boldsymbol{v}_2=\boldsymbol{v}_1+\boldsymbol{R}_1\boldsymbol{v}_d+\boldsymbol{\omega}_1\times(\boldsymbol{p}_2-\boldsymbol{p}_1).\tag{2.50}$$

• 图 2.12 坐标变换和角速度矢量

  ---

  让我们思考一下第 2.2.4 节中的 (2.22)

  $$\boldsymbol{R}(\boldsymbol{\omega}\times\boldsymbol{p})=(\boldsymbol{R}\boldsymbol{\omega})\times(\boldsymbol{R}\boldsymbol{p})\tag{a}$$

  这个方程的左边可以改写为

  $$\begin{align*} \boldsymbol{R}(\boldsymbol{\omega}\times\boldsymbol{p}) &=\boldsymbol{R}\hat{\boldsymbol{\omega}}\boldsymbol{p}\\ &=\boldsymbol{R}\hat{\boldsymbol{\omega}}\boldsymbol{R}^T\boldsymbol{R}\boldsymbol{p}\\ &=(\boldsymbol{R}\hat{\boldsymbol{\omega}}\boldsymbol{R}^T)(\boldsymbol{R}\boldsymbol{p}). \end{align*}\tag{b}$$

  比较方程 (a) 和方程 (b) 可以看出

  $$(\boldsymbol{R}\boldsymbol{\omega})^\land=\boldsymbol{R}\hat{\boldsymbol{\omega}}\boldsymbol{R}^T.$$

  ---

第二个物体的角速度可根据 (2.49) 计算如下

$$\begin{align*} \hat{\boldsymbol{\omega}}_2 &=\dot{\boldsymbol{R}}_2\boldsymbol{R}_2^T\\ &=\frac{d}{dt}(\boldsymbol{R}_1\boldsymbol{R}_d)\boldsymbol{R}_2^T\\ &=(\dot{\boldsymbol{R}}_1\boldsymbol{R}_d+\boldsymbol{R}_1\dot{\boldsymbol{R}}_d)\boldsymbol{R}_2^T\\ &=(\hat{\boldsymbol{\omega}}_1\boldsymbol{R}_1\boldsymbol{R}_d+\boldsymbol{R}_1\hat{\boldsymbol{\omega}}_d\boldsymbol{R}_d)\boldsymbol{R}_2^T\\ &=\hat{\boldsymbol{\omega}}_1+\boldsymbol{R}_1\hat{\boldsymbol{\omega}}_d\boldsymbol{R}_d\boldsymbol{R}_d^T\boldsymbol{R}_1^T\\ &=\hat{\boldsymbol{\omega}}_1+\boldsymbol{R}_1\hat{\boldsymbol{\omega}}_d\boldsymbol{R}_1^T \end{align*}$$

其中 , .

从图 2.12 可以看出, , 因此我们得到

$$\hat{\boldsymbol{\omega}}_2=\hat{\boldsymbol{\omega}}_1+(\boldsymbol{R}_1\boldsymbol{\omega}_d)^\land.$$

将 应用于等式两边, 我们得到

$$\boldsymbol{\omega}_2=\boldsymbol{\omega}_1+\boldsymbol{R}_1\boldsymbol{\omega}_d.\tag{2.51}$$

总之, 我们首先将物体 1 和物体 2 的位置和姿态定义为 , . 如果物体 1 的速度为 , 物体 2 相对于物体 1 的速度为 , 则物体 2 的速度为

物体的线速度和角速度被简单地合并在一起, 称为 "速度".

$$\boldsymbol{v}_2=\boldsymbol{v}_1+\boldsymbol{R}_1\boldsymbol{v}_d+\boldsymbol{\omega}_1\times(\boldsymbol{p}_2-\boldsymbol{p}_1)\tag{2.52}$$ $$\boldsymbol{\omega}_2=\boldsymbol{\omega}_1+\boldsymbol{R}_1\boldsymbol{\omega}_d.\tag{2.53}$$

这里的 和 描述了物体在世界坐标中的相对速度. 如果我们用 , 取而代之, 则得到

$$\boldsymbol{v}_2=\boldsymbol{v}_1+{^W\boldsymbol{v}}_d+\boldsymbol{\omega}_1\times(\boldsymbol{p}_2-\boldsymbol{p}_1)\tag{2.54}$$ $$\boldsymbol{\omega}_2=\boldsymbol{\omega}_1+{^W\boldsymbol{\omega}}_d.\tag{2.55}$$

因此, 除了 (2.54) 中第三个元素对旋转速度的影响之外, 物体在世界坐标中的速度只是一个总和.

在图 2.13 中, 我们试图将 (2.55) 的含义形象化.

2.4 机器人数据结构和编程

2.4.1 数据结构

仿人机器人是由多个关节连接而成的机械装置. 在开始分析时, 我们希望将其划分为更小的单元. 在众多可能的分离方式中, 我们在图 2.14 中介绍了两种典型的方式.

在图 2.14(a) 中, 每个关节都包含在离主干较远的链节中. 而在 (b) 中, 每个关节都被定义为主干或离主干较近的链节的一部分. 就计算机编程而言, 前一种方式更为方便. 原因是所有链节都只包含一个关节, 因此可以使用相同的算法来处理所有关节. 此外, 它还能让添加新关节变得更容易. 另一方面, 方法 (b) 将需要具有不同数量关节的链节. 这反过来又使计算机编程变得更加复杂. 请注意, 在这里我们讨论的是机器人的概念分隔. 并不是绝对要求将机器人分割得如图 2.14 所示.

将这些链节以正确的组合方式连接起来, 我们就得到了构成仿人机器人的链节组合. 连接规则由图 2.15 所示的树状图来表示.

树状图是一种结构图, 它有一个基部, 从基部分支出更小的分支, 但不会形成循环.

换句话说, 我们或许可以说, 这种结构显示了以基链节为共同祖先, 以末端链节为最年轻成员的链节家族树.

在给本教科书中的链节命名时, 我们使用 R 或 L 作为前缀来表示链节在哪一边. 因此, 右臂上的链节将使用 RARM, 左臂上的链节将使用 LARM.

然而, 在图 2.15 中, 根据链节的不同, 存在着不同数量的分支 (连接) , 因此正如我们在讨论机器人结构划分时所讨论的那样, 这种表示方法给编程带来了困难.

或者, 我们也可以如图 2.16 所示描述链节连接. 在该图中, 每个链节都有两条连接线, 左下线连接其子链节, 右下线连接其姊妹链节. 例如, 如果您想知道 RARM 的姊妹链节, 您可以沿着右下方的连接线找到 "LARM", "RLEG" 和 "LLEG" 链节. 这些链节的父链节是右上方的 "BODY" 链节, 子链节是左下方的 "RHAND" 链节. 在没有子链节或姊妹链节的末端, 我们使用了 "0". 总之, 虽然这个图表看起来不同, 但它所代表的信息与图 2.15 中的图表相同.

2.4.2 使用递归编程

接下来, 让我们看看如何实际实现一个使用这些数据结构的程序. 在准备过程中, 我们将把图 2.16 中的信息转换成列表. 2.16 中的信息转换成一个列表. 从 ID 编号 1 开始, 我们给每个链节一个 ID 编号, 并在下面的列表中使用.

ID	name	sister	child
1	BODY	0	2
2	RARM	4	3
3	RHAND	0	0
4	LARM	6	5
5	LHAND	0	0
6	RLEG	8	7
7	RFOOT	0	0
8	LLEG	0	9
9	LFOOT	0	0

要在 Matlab 中输入这些信息, 我们应键入以下内容.

在本教科书中, 我们将使用 Matlab, 它是 MathWorks 公司的产品, 可以让用户交互式地进行矩阵计算. 算法本身并不依赖于你所使用的语言, 因此这个程序也可以很容易地用其他语言 (如 C++ 或 Java) 来实现.

>> global uLINK
>> uLINK(1).name = 'BODY';
>> uLINK(1).sister = 0;
>> uLINK(1).child = 2;

上面的 ">>" 是 Matlab 命令提示符. uLINK 是本教科书中使用的一个结构的名称, 它保存了计算中所需的大部分值. 这里我们只看 "name", "sister" 和 "child" 这几个字段. 在第一行中, 我们声明要使用全局变量 uLINK. 在接下来的行中, uLINK(1) 指向 ID 号为 1 的第一个 uLINK. 句号后的名称指定了数据字段名称 (name, sister, child) .

同样, 您也可以通过 uLINK(2) 访问 ID 号为 2 的链节部分中的字段.

一旦程序开始变长, 你就可以将这些命令存储到一个后缀名为 "m" 的文件中, 然后通过命令行执行.

>> uLINK(1).name % print the name of the link with ID=1
ans =
BODY
>> uLINK(uLINK(1).child).name % print first child of BODY
ans =
RARM
>> uLINK(uLINK(uLINK(1).child).child).name % first grandchild of BODY
ans =
RHAND

如果在命令行中省略变量名后的分号 ";", Matlab 将打印出该变量的所有信息. 这样, 您就可以随时查看存储在该树中的信息. 换句话说, 您现在拥有了一个机器人链节信息数据库.

图 2.17 显示了一个简短的脚本 (PrintLinkName.m) , 它可以打印数据库中存储的所有链节名称.

• 图 2.17 PrintLinkName.m: 显示所有链节的名称

  function PrintLinkName(j)
  global uLINK  % Refer global variable uLINK
  if j ~= 0
    fprintf('j=%d : %s\n',j,uLINK(j).name);  % Show my name
    PrintLinkName(uLINK(j).child);           % Show my child's name
    PrintLinkName(uLINK(j).sister);          % Show my sister's name
  end

该程序将 ID 号作为输入, 并首先对其进行检查. 如果 ID 不为 0, 程序将打印相应的链节名称, 并使用姊妹 ID 编号调用 PrintLinkName, 然后使用子 ID 编号调用 PrintLinkName. 如果 ID 为 0, 程序将什么也不做. 将此脚本以 PrintLinkName.m 保存后, 我们就可以在 Matlab 命令行中执行 "PrintLinkName(1)", 并获得 "BODY" 链节下的树中子链节的名称. 从函数内部调用函数被称为递归调用. 每次调用该函数时, 都会在树中向下移动, 直到到达一个既没有姐妹节点也没有子节点的节点. 这为访问树中的每个节点并应用所需的计算提供了一种简单的方法.

利用递归调用技术, 我们可以轻松编写一个函数来求和所有链节的质量 (图 2.18) . 这里我们假设添加了一个新的数据字段 "m" 来保存链节的质量, 并用每个链节的实际质量对其进行初始化.

• 图 2.18 TotalMass.m: 每个链节的质量总和

  function m = TotalMass(j)
  global uLINK
  if j == 0
    m = 0;
  else
    m = uLINK(j).m + TotalMass(uLINK(j).sister) + TotalMass(uLINK(j).child);
  end

如果 ID 号为 0, 程序将返回 0 千克, 因为没有链节需要求和 (第 4 行) . 如果 ID 号不是 0, 那么程序将返回相应链节的质量, 姊妹链节的后代链节的总质量以及子链节的后代链节的总质量之和 (第 6 行) . 如果在命令行中输入 TotalMass(1), 该函数将访问树中的所有节点, 并返回 "BODY" 链节下所有链节的总质量. 与事先检查现有链节并求和的通常方法相比, 我觉得这个方法要聪明得多. 您觉得如何?

2.5 仿人机器人的运动学

2.5.1 创建模型

为了解释仿人机器人的运动学, 我们将使用图 2.19 所示的 12 DoF 模型, 该模型由两条腿组成. 链节名称及其 ID 编号如图 2.19(a) 所示. 按照图 2.14(a) 的方式分割该模型, 除了 "BODY" 链节外, 每个链节都只有一个关节驱动. 因此, 我们可以通过关节名称或其 ID 编号来识别链节. 例如 ID 编号 5 指的是 RLEG_J3 关节以及右小腿的链节.

首先, 我们必须定义每个链节的局部坐标. 每个局部坐标的原点可以设置在其关节轴上的任何位置. 不过, 对于每个髋关节来说, 将三个坐标系的原点指定在三个关节轴相交的同一点是合理的. 同样, 对于每个踝关节, 我们将两个踝关节框架的原点指定在两个关节轴相交的同一点上.

然后, 将所有描述链节姿态的旋转矩阵设置为与机器人初始状态 (膝盖完全伸直直立) 时的世界坐标一致. 因此, 我们将 13 个矩阵设置为

$$\boldsymbol{R}_1=\boldsymbol{R}_2=\dots=\boldsymbol{R}_{13}=\boldsymbol{E}.$$

图 2.19(b) 显示了在此阶段定义的局部坐标.

接下来我们将定义关节轴向量 和相对位置向量 , 如图 2.20 所示. 关节轴向量是一个单位向量, 它定义了父链节局部坐标中的关节旋转轴. 正 (+) 关节旋转被定义为拧紧与关节轴向量同方向的右手螺丝的方式. 以膝关节为例, 关节轴向量为, . 当我们将该链节向 + 方向旋转时, 伸直的膝关节将与人的膝关节向同一方向弯曲. 相对位置向量 是表示局部坐标的原点在父链节局部坐标中的位置的向量. 当它们与踝关节滚动接头位于同一位置时, .

有一种众所周知的方法可以用来描述机器人的链节结构, 叫做德纳维特-哈腾伯格 (Denavit-Hartenberg, DH) 法[18]. 我们最初也使用了这种方法. 不过, 这种方法有一个限制, 即需要改变每个链节坐标的朝向. 我们发现这种限制条件下的实现方法容易出错, 因此我们采用了上述方法.

在下面的章节中, 我们将利用上面的描述来计算正向运动学, 雅可比矩阵和逆向运动学. 为此, 我们需要更多的信息, 如形状, 关节角度, 关节速度等. 链节参数的完整列表如表 2.1 所示.

• 表 2.1 链节参数

Link Parameter	Symbol for Equation	uLINK data field
Self ID		-
Sister ID	None	sister
Child ID	None	child
Parent ID		mother
Position in World Coordinates		p
Attitude in World Coordinates		R
Linear Velocity in World Coordinates		v
Angular Velocity in World Coordinates		w
Joint Angle		q
Joint Velocity		dq
Joint Acceleration		ddq
Joint Axis Vector(Relative to Parent)		a
Joint Relative Position(Relative to Parent)		b
Shape(Vertex Information, Link Local)		vertex
Shape(Vertice Information (Point Connections)	None	face
Mass		m
Center of Mass(Link Local)		c
Moment of Inertia(Link Local)		I

2.5.2 正向运动学: 根据链节角度计算链节位置

正向运动学 (Forward Kinematics) 是一种通过给定的机器人结构及其关节角度来获取某一链节的位置和姿态的计算方法. 当您想计算整个机器人的质心时, 当您只想显示机器人的当前状态时, 或者当您想检测机器人与环境的碰撞时, 都需要用到正向运动学. 因此, 正向运动学构成了机器人仿真的基础.

正向运动学可以通过使用齐次变换的链式法则来计算. 首先我们先计算单链节的齐次变换, 如图 2.21 所示. 我们需要设置一个局部坐标系 , 其原点位于关节轴上. 从父坐标看到的关节轴向量为 , 的原点为 . 关节角为 , 关节角为 0 时链节的姿态为 .

相对于父链节的齐次变换为:

$${^i\boldsymbol{T}}_j= \begin{bmatrix} e^{\hat{\boldsymbol{a}}_jq_j} & \boldsymbol{b}_j \\ 0\enspace0\enspace0 & 1 \end{bmatrix}\tag{2.56}$$

接下来让我们假设有两条链节, 如图 2.22 所示. 我们假设父链节 , 的绝对位置和姿态是已知的. 因此, 对 的齐次变换变为

$${\boldsymbol{T}}_i= \begin{bmatrix} \boldsymbol{R}_i & \boldsymbol{p}_i \\ 0\enspace0\enspace0 & 1 \end{bmatrix}\tag{2.57}$$

由齐次变换的链式法则可得 为

$$\boldsymbol{T}_j=\boldsymbol{T}_i{^i\boldsymbol{T}}_j.\tag{2.58}$$

根据 (2.56), (2.57) 和 (2.58) 可以计算出 的绝对位置 () 和姿态 (),

$$\boldsymbol{p}_j=\boldsymbol{p}_i+\boldsymbol{R}_i\boldsymbol{b}_i,\tag{2.59}$$ $$\boldsymbol{R}_j=\boldsymbol{R}_ie^{\hat{\boldsymbol{a}}_jq_j}.\tag{2.60}$$

利用这种关系和递归算法, 正向运动学可以通过图 2.23 所示的一个极其简单的脚本来完成. 要使用这个程序, 我们首先要设置基础链节 (BODY) 的绝对位置和姿态以及所有关节角度. 然后通过执行 ForwardKinematics(1), 我们可以更新机器人所有链节的位置和姿态.

• 图 2.23 ForwardKinematics.m 计算所有关节的正向运动学参数

  function ForwardKinematics(j)
  global uLINK
  if j == 0 return; end
  if j ~= 1
    i = uLINK(j).mother;
    uLINK(j).p = uLINK(i).R * uLINK(j).b + uLINK(i).p;
    uLINK(j).R = uLINK(i).R * Rodrigues(uLINK(j).a, uLINK(j).q);
  end
  ForwardKinematics(uLINK(j).sister);
  ForwardKinematics(uLINK(j).child);

图 2.24 显示了 12 自由度双足机器人在给定所有 12 个关节角度的随机值后的样子. 这应该有助于您想象一个简单的机构是如何体现大量复杂性的.

2.5.3 反向运动学: 根据链节的位置和姿态计算关节角度

接下来, 我们将讨论如何在获得身体和脚的位置和姿态的情况下计算关节角度. 在这种情况下, 我们需要做的是反向运动学 (Inverse Kinematics) . 例如, 假设我们的机器人位于楼梯前方, 我们希望将其中一只脚放在第一级台阶上, 而台阶的高度和深度是已知的. 我们当然需要确定髋关节, 膝关节和踝关节的旋转量. 在这种情况下, 反向运动学是必要的.

求解反向运动学有分析方法和数值方法. 首先, 我们将解释如何分析求解. 让我们把注意力集中在图 2.19 所示模型的右腿上. 身体和右腿的位置和姿态分别为 (, ) 和 (, ). 为了简化方程, 我们将定义 , 即身体原点与髋关节之间的距离. 如图 2.25(a) 所示, 上肢长度为 , 下肢长度为 . 因此, 髋关节的位置为

$$\boldsymbol{p}_2=\boldsymbol{p}_1+\boldsymbol{R}_1\begin{bmatrix}0\\D\\0\end{bmatrix}.$$

接下来, 我们计算从脚踝坐标空间看胯部的位置

$$\boldsymbol{r}=\boldsymbol{R}^T_7(\boldsymbol{p}_2-\boldsymbol{p}_7)\equiv\begin{bmatrix}r_x&r_y&r_z\end{bmatrix}^T.\tag{2.61}$$

由此我们可以计算出脚踝和髋关节之间的距离, 我们将其定义为

$$C=\sqrt{r_x^2+r_y^2+r_z^2}.$$

如图 2.25(b) 所示, 如果我们考虑三角形 , 就会得到膝角 . 根据余弦定则, 我们可以得到

$$C^2=A^2+B^2-2AB\cos(\pi-q_5).$$

因此, 膝盖的角度将是

$$q_5=-\cos^{-1}\left(\frac{A^2+B^2+C^2}{2AB}\right)+\pi.$$

如果我们将三角形下端的角度定义为 , 根据正弦定则可得

$$\frac{C}{\sin(\pi-q_5)}=\frac{A}{\sin\alpha}.$$

因此

$$\alpha=\sin^{-1}\left(\frac{A\sin(\pi-q_5)}{C}\right).$$

接下来我们将重点讨论脚踝局部坐标. 如图 2.25(c) 所示, 从向量 可以计算出脚踝的滚动角和俯仰角. 那么

$$\begin{align*} &q_7=\text{atan2}(r_y,r_z)\\ &q_6=-\text{atan2}\left(r_x,\text{sign}(r_z)\sqrt{r_y^2+r_z^2}\right)-\alpha. \end{align*}$$

函数 计算向量 与 轴之间的夹角, 这在第 2.2.7 节中已经出现过. 该函数内置在 Matlab 中, 在大多数编程语言中也可以作为内置函数使用. 此外, 函数在 为正值时返回 +1, 在 为负值时返回 -1.

剩下的就是腿底部的偏航角, 滚动角和俯仰角. 根据定义每个关节的方程

$$\boldsymbol{R}_7=\boldsymbol{R}_1\boldsymbol{R}_z(q_2)\boldsymbol{R}_x(q_3)\boldsymbol{R}_y(q_4)\boldsymbol{R}_y(q_5+q_6)\boldsymbol{R}_x(q_7),$$

可得

$$\boldsymbol{R}_z(q_2)\boldsymbol{R}_x(q_3)\boldsymbol{R}_y(q_4)=\boldsymbol{R}_1^T\boldsymbol{R}_7\boldsymbol{R}_x(-q_7)\boldsymbol{R}_y(-q_5-q_6).$$

将方程左边的内容展开, 计算右边的内容, 可以得到以下结果

$$\begin{bmatrix} c_2c_4-s_2s_3s_4 & -s_2c_3 & c_2s_4+s_2s_3c_4 \\ s_2c_4+c_2s_3s_4 & c_2c_3 & s_2s_4-c_2s_3c_4 \\ -c_3s_4 & s_3 & c_3c_4 \end{bmatrix}= \begin{bmatrix} R_{11} & R_{12} & R_{13}\\ R_{21} & R_{22} & R_{23}\\ R_{31} & R_{32} & R_{33} \end{bmatrix}$$

其中 , 且 .

仔细观察这个等式的左侧, 我们可以得到

$$q_2=\text{atan2}(-R_{12},R_{22}),\tag{2.62}$$ $$q_3=\text{atan2}(R_{32},-R_{12}s_2+R_{22}c_2),\tag{2.63}$$ $$q_4=\text{atan2}(-R_{31},R_{33}),\tag{2.64}$$

图 2.26 展示了上述程序的实现过程. 对于左腿, 我们可以反转 的符号并应用相同的程序.

在实际应用中, 我们的程序会覆盖超过腿长和关节角度限制的目标位置.

• 图 2.26 IK_leg.m 反向运动学分析解法的实施示例. 注意！在实际机器人上使用该程序时, 需要不断检查关节角度是否超出极限. 在最坏的情况下, 您可能会毁坏机器人或造成重大人员伤亡.

  function q = IK_leg(Body,D,A,B,Foot)
  r = Foot.R' * (Body.p + Body.R * [0 D 0]'- Foot.p); % crotch from ankle
  C = norm(r);
  c5 = (C^2-A^2-B^2)/(2.0*A*B);
  if c5 >= 1
    q5 = 0.0;
  elseif c5 <= -1
    q5 = pi;
  else
    q5 = acos(c5);            % knee pitch
  end
  q6a = asin((A/C)*sin(pi-q5)); % ankle pitch sub
  q7 = atan2(r(2),r(3));        % ankle roll -pi/2 < q(6) < pi/2
  if q7 > pi/2, q7 = q7-pi; elseif q7 < -pi/2, q7 = q7+pi; end
  q6 = -atan2(r(1),sign(r(3))*sqrt(r(2)^2+r(3)^2)) -q6a; % ankle pitch
  R = Body.R' * Foot.R * Rroll(-q7) * Rpitch(-q6-q5);    % hipZ*hipX*hipY
  q2 = atan2(-R(1,2),R(2,2));                            % hip yaw
  cz = cos(q2); sz = sin(q2);
  q3 = atan2(R(3,2),-R(1,2)*sz + R(2,2)*cz);             % hip roll
  q4 = atan2(-R(3,1), R(3,3));                           % hip pitch
  q = [q2 q3 q4 q5 q6 q7]';

这种方法只能用于与图 2.19 中布局相同的机器人. 如果机器人的三个关节轴不相交于一点, 我们就需要一种完全不同的算法. 机器人教科书 (例如 [96, 127]) 中介绍了许多不同的算法, 但一般来说, 这些算法需要大量繁重的计算, 因此更常见的是使用数值解法, 我们将在下一节中详细介绍.

2.5.4 反向运动学的数值解法

与分析求解反向运动学相比, 正向运动学计算简单 (第 2.5.2 节) . 因此, 如图 2.27 所示, 使用正向运动学的试错法求解反向运动学并不遥远. 示例算法如下

• 步骤 1. 准备基链节的位置和姿态 ；
• 步骤 2. 准备目标链节的位置和姿态 ；
• 步骤 3. 定义向量 , 其中包含从基链节到目标链节的关节角度；
• 步骤 4. 使用正向运动学计算目标链节的位置和姿态 ；
• 步骤 5. 计算位置和姿态差 ；
• 步骤 6. 如果 足够小, 则停止计算；
• 步骤 7. 如果 不够小, 则计算可减少误差的 ；
• 步骤 8. 通过 更新关节角度, 返回步骤 4.

要真正实现这一点, 首先需要克服接下来的两个障碍.

1. 我们所说的位置和姿态误差 足够小到底是什么意思? (步骤 6)
2. 我们究竟该如何计算 , 以减小误差? (步骤 7)

第一个问题相对容易解决. 零位置误差和零姿态误差可用以下公式描述

$$\Delta\boldsymbol{p}=0,\\ \Delta\boldsymbol{R}=\boldsymbol{E}.$$

下面是一个根据误差大小返回正标量的函数示例

$$\text{err}(\Delta\boldsymbol{p},\Delta\boldsymbol{R})=\|\Delta\boldsymbol{p}\|^2+\|\Delta\boldsymbol{\theta}\|^2,\tag{2.65}$$ $$\Delta\boldsymbol{\theta}\equiv(\ln\Delta\boldsymbol{R})^\lor.\tag{2.66}$$

更一般的函数是 . 这里的 和 是某个正数, 需要更高精度的方向越大.

只有当位置和姿态误差都为零时, 该函数才会变为零. 当 小于预定值 (例如 ) 时, 可以说位置和姿态误差足够小.

那么第二个问题呢? 我们真正需要做的是找出一组关节角度 , 从而降低 . 一种方法是每次使用随机数. 如果我们能将 降低哪怕一点点, 我们就将其用于关节角度, 然后重新开始. 机器人本身会利用试错来寻找关节角度, 因此我们会产生智能的错觉.

这个想法是任何人都会想到的, 但出于某种原因, 很多人往往认为只有他们才会想到. 事实上, 作者恰好就是其中之一 :-).

虽然这是一个诱人的想法, 但如今没有一个机器人会使用这种方法. 因为有一种方法更快, 更精确. 在这种被称为牛顿-拉夫逊法 (Newton-Raphson method) 的方法中, 我们首先要考虑的是, 在使用微小的 值改变关节角度时, 位置和姿态 会发生什么变化.

$$\delta\boldsymbol{p}=\boldsymbol{X}_p(\boldsymbol{q},\delta\boldsymbol{q}),\tag{2.67}$$ $$\delta\boldsymbol{\theta}=\boldsymbol{X}_\theta(\boldsymbol{q},\delta\boldsymbol{q}).\tag{2.68}$$

在这里, 和 都是未知数, 但当 较小时, 我们可以说能用加法和乘法来简单描述. 如果使用矩阵, 我们可以得到

$$\begin{bmatrix} \delta\boldsymbol{p}\\ \delta\boldsymbol{\theta} \end{bmatrix}= \begin{bmatrix} J_{11}&J_{12}&J_{13}&J_{14}&J_{15}&J_{16}\\ J_{21}&J_{22}&J_{23}&J_{24}&J_{25}&J_{26}\\ J_{31}&J_{32}&J_{33}&J_{34}&J_{35}&J_{36}\\ J_{41}&J_{42}&J_{43}&J_{44}&J_{45}&J_{46}\\ J_{51}&J_{52}&J_{53}&J_{54}&J_{55}&J_{56}\\ J_{61}&J_{62}&J_{63}&J_{64}&J_{65}&J_{66} \end{bmatrix}\delta\boldsymbol{q}. \tag{2.69}$$

这里的 是常数, 由机器人链节的当前位置和姿态定义. 有 6 个是因为机器人腿上的链节数量. 由于每次都要写出所有的组成部分, 工作量太大, 所以我们简化为

$$\begin{bmatrix} \delta\boldsymbol{p}\\ \delta\boldsymbol{\theta} \end{bmatrix}= \boldsymbol{J}\delta\boldsymbol{q}. \tag{2.70}$$

矩阵 称为雅可比矩阵.

源自德国数学家 Carl Gustav Jacobi (1804-1851). 数学家提到 Jacobian 时, 指的是这个矩阵的行列式, 但机器人专家谈到 Jacobian 时, 通常指的是矩阵本身. 有些人认为这是一个错误, 但这并不是日本的专利, 全世界都在这样做.

有了 (2.70), 我们只需取该矩阵的逆, 就能计算出所需的调整量

$$\delta\boldsymbol{q}= \lambda\boldsymbol{J}^{-1} \begin{bmatrix} \delta\boldsymbol{p}\\ \delta\boldsymbol{\theta} \end{bmatrix}. \tag{2.71}$$

这是根据位置和姿态误差计算关节角度调整的方程. 值 是用于稳定数值计算的系数. 图 2.28 显示了用 Matlab 编写的反向运动学算法实现示例. 在第 7 行, 您将看到用于计算雅可比矩阵的函数 CalcJacobian. 我们将在下一节详细介绍. 第 10 行是 (2.71) 的实际执行. 运算符 \ (反斜杠) 在不进行显式矩阵求逆的情况下有效地求解了线性方程.

• 图 2.28 InvserseKinematics.m 反向运动学数值解法

  function InverseKinematics(to, Target)
  global uLINK
  lambda = 0.5;
  ForwardKinematics(1);
  idx = FindRoute(to);
  for n = 1:10
    J= CalcJacobian(idx);
    err = CalcVWerr(Target, uLINK(to));
    if norm(err) < 1E-6 return, end;
    dq = lambda * (J \ err);
    for nn=1:length(idx)
      j = idx(nn);
      uLINK(j).q = uLINK(j).q + dq(nn);
    end
    ForwardKinematics(1);
  end

函数 FindRoute 返回从基础链节到达目标链节所需经过的链节. CalcVWerr 是一个计算位置和姿态差的函数. 您可以在本章末尾的附录中找到这些函数的实现版本.

图 2.29 展示了将反向运动学作为 Matlab 命令输入的示例. 在这里, 我们使用 SetupBipedRobot 设置机器人数据, 使用 GoHalfSitting 获得非奇异姿态, 使用 DrawAllJoints() 显示双足机器人. 函数 rpy2rot() 是 (2.13) 的实现. 您可以从下载材料中获取这些信息.

• 图 2.29 使用逆运动学的样本和计算结果

  >> SetupBipedRobot;  % Set robot parameters
  >> GoHalfSitting;    % Set knee bending posture
  
  >> Rfoot.p = [-0.3, -0.1, 0]';
  >> Rfoot.R = rpy2rot(0, ToRad*20.0,0);
  >> InverseKinematics(RLEG_J5, Rfoot);
  
  >> Lfoot.p = [ 0.3, 0.1, 0]';
  >> Lfoot.R = rpy2rot(0, -ToRad*30.0,0);
  >> InverseKinematics(LLEG_J5, Lfoot);
  
  >> DrawAllJoints(1); % Show the robot

  

2.5.5 雅可比 (Jacobian)

上一节我们介绍了雅可比矩阵, 它为您提供了小关节运动和空间运动之间的关系. 通过雅可比矩阵, 我们还可以计算出关节所需的力矩, 以便通过手脚产生外力. 由于雅可比矩阵被广泛应用于机器人控制领域, 因此没有雅可比矩阵的机器人学论文实在是凤毛麟角.

我们可以通过阅读 Yoshikawa 博士的教科书 [144], 了解雅可比矩阵在机器人学中的应用.

下面我们将介绍计算雅可比矩阵的实际过程. 图 2.30 显示了一条在空间中漂浮着 个链节的链. 我们假定这些链节从底端 (第 1 个链节) 到末端 (第 个链节) 依次编号. 我们假设末端效应器 (机器人的手或脚) 连接在第 个链节上. 此外, 我们还将假设正向运动学已经计算完毕, 每个链节的位置和姿态都已存储 (第 个链节有 ) .

在这个链节中, 假设除了第 2 个关节外, 所有关节都保持固定, 我们将其转动一个小角度 . 末端效应器 (链节 ) 的位置变化量 和末端效应器的姿态变化量 可以通过以下公式计算得出

$$\left\{\begin{align*} \delta\boldsymbol{p}_N^{(2)}&={^W\boldsymbol{a}}_2\times(\boldsymbol{p}_N-\boldsymbol{p}_2)\delta q_2\\ \delta\boldsymbol{\theta}_N^{(2)}&={^W\boldsymbol{a}}_2\delta q_2 \end{align*}\right.$$

其中, 是第二关节轴相对于世界坐标系的单位向量

$${^W\boldsymbol{a}}_2=\boldsymbol{R}_1\boldsymbol{a}_2.$$

如果我们对从第 1 节到第 节的所有链节采用同样的步骤, 并计算它们的总和, 就可以得出所有关节旋转少量时的变化情况

$$\left\{\begin{align*} \delta\boldsymbol{p}_N&=\sum^N_{j=1}\delta\boldsymbol{p}_N^{(j)},\\ \delta\boldsymbol{\theta}_N&=\sum^N_{j=1}\delta\boldsymbol{\theta}_N^{(j)}. \end{align*}\right.\tag{2.72}$$

我们可以将上式改写成矩阵, 得到

$$\begin{bmatrix}\delta\boldsymbol{p}_N\\\delta\boldsymbol{\theta}_N\end{bmatrix}= \begin{bmatrix} {^W\boldsymbol{a}}_1\times(\boldsymbol{p}_N-\boldsymbol{p}_1)&\dots&{^W\boldsymbol{a}}_{N-1}\times(\boldsymbol{p}_N-\boldsymbol{p}_{N-1})&0\\ {^W\boldsymbol{a}}_1&\dots&{^W\boldsymbol{a}}_{N-1}&{^W\boldsymbol{a}}_N \end{bmatrix}\begin{bmatrix} \delta q_1\\ \delta q_2\\ \vdots\\ \delta q_N \end{bmatrix}.\tag{2.73}$$

换句话说, 雅可比矩阵 可描述为

$$\boldsymbol{J}\equiv \begin{bmatrix} {^W\boldsymbol{a}}_1\times(\boldsymbol{p}_N-\boldsymbol{p}_1)&{^W\boldsymbol{a}}_2\times(\boldsymbol{p}_N-\boldsymbol{p}_2)&\dots&{^W\boldsymbol{a}}_{N-1}\times(\boldsymbol{p}_N-\boldsymbol{p}_{N-1})&0\\ {^W\boldsymbol{a}}_1&{^W\boldsymbol{a}}_2&\dots&{^W\boldsymbol{a}}_{N-1}&{^W\boldsymbol{a}}_N \end{bmatrix}.\tag{2.74}$$

以 Matlab 程序实现的这一过程如图 2.31 所示.

• 图 2.31 CalcJacobian.m: 计算雅可比矩阵

  function J = CalcJacobian(idx)
  global uLINK
  jsize = length(idx);
  target = uLINK(idx(end)).p;        % absolute target position
  J = zeros(6,jsize);
  for n = 1:jsize
    j = idx(n);
    mom = uLINK(j).mother;
    a = uLINK(mom).R * uLINK(j).a; % joint axis in world frame
    J(:,n) = [cross(a, target - uLINK(j).p) ; a ];
  end

2.5.6 雅可比与关节速度

将 (2.70) 除以一小段时间 即可得出关节速度与末端效应器速度之间的关系.

$$\frac{1}{\delta t}\begin{bmatrix} \delta\boldsymbol{p}\\ \delta\boldsymbol{\theta} \end{bmatrix}= \boldsymbol{J}\frac{\delta\boldsymbol{q}}{\delta t}.$$

因此, 关节速度 和末端效应器速度 的关系如下

$$\begin{bmatrix} \boldsymbol{v}\\ \boldsymbol{\omega} \end{bmatrix}= \boldsymbol{J}\dot{\boldsymbol{q}}.\tag{2.75}$$

请注意, 这是身体链节在空间中固定时的情况. 如果身体链节有自己的速度为 , 我们必须使用 (见第 2.3 节)

$$\begin{bmatrix} \boldsymbol{v}\\ \boldsymbol{\omega} \end{bmatrix}= \boldsymbol{J}\dot{\boldsymbol{q}}+ \begin{bmatrix} \boldsymbol{v}_B+\boldsymbol{\omega}_B\times(\boldsymbol{p}-\boldsymbol{p}_B)\\ \boldsymbol{\omega}_B \end{bmatrix}.\tag{2.76}$$

但在下面的讨论中, 为了简单起见, 我们假设主体链节在空间中是固定的. 我们可以通过 (2.75) 的变换计算出实现末端速度 的关节速度

$$\dot{\boldsymbol{q}}= \boldsymbol{J}^{-1}\begin{bmatrix} \boldsymbol{v}\\ \boldsymbol{\omega} \end{bmatrix}.\tag{2.77}$$

让我们来计算图 2.32(a) 中两种姿势的关节速度. 首先, 我们在 Matlab 命令行中设置机器人的所有几何信息, 然后使用 FindRoute() 查找从身体到右脚的路径. 然后使用 SetJointAngles() 设置关节角度, 使右侧髋关节俯仰角, 膝关节俯仰角和角俯仰角为 (-30, 60, -30) 度, 其他关节为 0 度. 我们可以通过以下输入计算雅可比矩阵.

>> SetupBipedRobot;
>> idx = FindRoute(RLEG_J5);
>> SetJointAngles(idx,[0 0 -pi/6 pi/3 -pi/6 0])
>> J = CalcJacobian(idx);

假设我们要让脚以 0.1m/s 的速度竖直抬起, 则关节速度可由 (2.77) 求得.

>> dq = J \ [0 0 0.1 0 0 0]'
dq =
   0
   0
  -0.3333
   0.6667
  -0.3333
   0

从这一结果中我们可以看出, 髋关节俯仰, 膝关节俯仰和踝关节俯仰的关节速度分别为 (-0.3333, 0.6667, -0.3333) rad/s.

让我们尝试对图 2.32(b) 进行同样的计算, 图 2.32(b) 中的膝关节完全拉伸. 为此, 我们将右腿的所有关节角都置零, 计算雅可比矩阵, 并得出关节速度.

>> SetJointAngles(idx,[0 0 0 0 0 0])
>> J = CalcJacobian(idx);
>> dq = J \ [0 0 0.1 0 0 0]'
Warning: Matrix is singular to working precision.
dq =
   NaN
   NaN
   NaN
  -Inf
   Inf
   0

在这种情况下, 我们得到的警告信息是 "矩阵工作精度是奇异的", 并得到了一个由 NaN (非数字) 和 Inf (无穷大) 组成的关节速度向量. 因此, 计算在图 2.32(b) 的奇异姿势处崩溃, 此时膝关节完全拉伸.

机器人关节可能会因 NaN 和 Inf 等非法指令而失控. 因此, 我们必须提出警告和错误, 同时也要注意避免使用此类非法命令执行控制指令.

出现这种情况的原因是, 在这种配置下, 机器人无法通过施加任何关节速度来垂直移动其脚. 这种情况下的机器人姿态称为奇异姿态. 让我们观察一下在这种奇异姿态下雅可比矩阵的逆计算结果.

>> J^(-1)
Warning: Matrix is singular to working precision.
ans =
  Inf Inf Inf Inf Inf Inf
  Inf Inf Inf Inf Inf Inf
  Inf Inf Inf Inf Inf Inf
  Inf Inf Inf Inf Inf Inf
  Inf Inf Inf Inf Inf Inf
  Inf Inf Inf Inf Inf Inf

2.5.7 奇异姿态

奇异姿势的例子如图 2.33 所示. 它们分别是: (a) 上一小节已经解释过的膝盖完全伸展的姿势；(b) 髋关节偏航轴和踝关节滚动轴对齐的姿势；以及 (c) 髋关节滚动轴和踝关节滚动轴对齐的姿势.

当机器人处于奇异姿态时, 雅可比矩阵会变成奇异矩阵, 我们无法计算其逆矩阵. 让我们检查一下图 2.33(b) 的情况.

>> SetupBipedRobot;
>> idx = FindRoute(RLEG_J5);
>> SetJointAngles(idx,[0 0 -pi/6 pi/3 pi/3 0])
>> J = CalcJacobian(idx);
>> J^(-1)
Warning: Matrix is close to singular or badly scaled.
		Results may be inaccurate. RCOND =3.760455e-17B
ans =
  1.0e+15 *
     0      -8.7498  0      4.5465 0       0.0000
     0       0.0000  0      0.0000 0       0.0000
    -0.0000  0      -0.0000 0      0.0000  0
     0.0000  0       0.0000 0      0.0000  0
     0.0000  0      -0.0000 0      0.0000  0
     0      -8.7498  0      4.5465 0      -0.0000
>> det(J)
ans =
  8.9079e-18
>> rank(J)
ans =
  5

在进行逆计算时, 我们得到了一条警告信息和一个毫无意义的结果, 由于数值误差, 其分量大得不切实际 () . 由于同样的原因, 行列式的值非常小 () , 而奇异矩阵的行列式应该为零. 最后, 雅可比矩阵的秩为 5, 这表明矩阵是奇异的.

2.5.8 具有奇异鲁棒性的反向运动学

由于数值不稳定性, 第 2.5.4 节中的 Newton-Raphson 方法在奇异姿态附近无法正常工作. 让我们来观察一下.

在图 2.34 中, 使用 Newton-Raphson 方法计算了让右脚从非弧形姿势向前移动时的关节角度. 右图显示了给定目标脚位置时的髋关节俯仰角, 膝关节俯仰角和踝关节俯仰角. 为便于比较, 用虚线表示了分析解. 脚的位置在垂直链线处达到奇点, 然后数值结果开始振动并偏离图表 (例如, 膝关节角度达到 8000 度以上) .

现在, 让我们再次考虑关节速度和末端执行器速度之间的关系, 其计算公式为

$$\dot{\boldsymbol{x}}=\boldsymbol{J}\dot{\boldsymbol{q}}.$$

这里, 是终端执行器目标速度的六维向量. 我们引入一个误差向量 , 因为在奇异点处将不再满足这一等式.

$$\boldsymbol{e}:=\dot{\boldsymbol{x}}-\boldsymbol{J}\dot{\boldsymbol{q}}.\tag{2.78}$$

我们总能使误差 越小越好, 但一般情况下误差 不能为零. 为此, 我们定义了一个成本函数

$$E(\dot{\boldsymbol{q}})=\frac{1}{2}\boldsymbol{e}^T\boldsymbol{e}.\tag{2.79}$$

如果 使 最小化, 我们就会有

$$\frac{\partial E(\dot{\boldsymbol{q}})}{\partial\dot{\boldsymbol{q}}}=0.\tag{2.80}$$

将 (2.78) 和 (2.79) 代入, 可得

$$\frac{\partial E(\dot{\boldsymbol{q}})}{\partial\dot{\boldsymbol{q}}}=-\boldsymbol{J}^T\dot{\boldsymbol{x}}+\boldsymbol{J}^T\boldsymbol{J}\dot{\boldsymbol{q}}=0.\tag{2.81}$$

因此, 使 最小化的 可通过以下方式求得

$$\dot{\boldsymbol{q}}=(\boldsymbol{J}^T\boldsymbol{J})^{-1}\boldsymbol{J}^T\dot{\boldsymbol{x}}.\tag{2.82}$$

遗憾的是, 我们无法在奇异点处使用它, 因为

$$\det(\boldsymbol{J}^T\boldsymbol{J})=\det(\boldsymbol{J}^T)\det(\boldsymbol{J})=0.$$

这意味着 (2.82) 右侧的逆式无法求解.

现在, 我们对成本函数稍作修改.

$$E(\dot{\boldsymbol{q}})=\frac{1}{2}\boldsymbol{e}^T\boldsymbol{e}+\frac{\lambda}{2}\dot{\boldsymbol{q}}^T\dot{\boldsymbol{q}}.\tag{2.83}$$

这将评估关节速度大小以及末端执行器速度误差. 关节速度通过增加正标量 来计算. 同样, 代入 (2.78) 和 (2.79), 我们可以得到

$$\frac{\partial E(\dot{\boldsymbol{q}})}{\partial\dot{\boldsymbol{q}}}=-\boldsymbol{J}^T\dot{\boldsymbol{x}}+(\boldsymbol{J}^T\boldsymbol{J}+\lambda\boldsymbol{E})\dot{\boldsymbol{q}}=0,\tag{2.84}$$

其中, 是与 规格相同的单位矩阵. 成本 的最小值为

$$\dot{\boldsymbol{q}}=(\boldsymbol{J}^T\boldsymbol{J}+\lambda\boldsymbol{E})^{-1}\boldsymbol{J}^T\dot{\boldsymbol{x}}.\tag{2.85}$$

即使在 的奇异点, 我们也总能用适当的 ​ 解出上述方程. 让我们定义一个新矩阵.

$$\begin{align*} \dot{\boldsymbol{q}}&=\boldsymbol{J}^{\#\lambda}\dot{\boldsymbol{x}}\\ \boldsymbol{J}^{\#\lambda}&:=(\boldsymbol{J}^T\boldsymbol{J}+\lambda\boldsymbol{E})^{-1}\boldsymbol{J}^T. \end{align*}\tag{2.86}$$

矩阵 被称为 SR 逆矩阵.

SR 代表 Singularity-Robust [138]. 它也被称为阻尼最小二乘逆 (Damped Least-Square (DLS) Inverse) [135].

通过使用 SR 逆矩阵, 我们可以保证在奇点处稳定计算逆运动学. 图 2.35 显示了覆盖奇点的鲁棒稳定的反运动学计算, 图 2.36 显示了其算法. 为了实现快速, 稳健的计算, 应根据收敛程度修改参数 . 在此算法中, 我们采用了 Sugihara [124] 提出的方法.

• 图 2.36 InvserseKinematics LM.m 用于处理奇异点的逆运动学算法

  function err_norm = InverseKinematics_LM(to, Target)
  global uLINK
  idx = FindRoute(to);
  wn_pos = 1/0.3;wn_ang = 1/(2*pi);
  We = diag([wn_pos wn_pos wn_pos wn_ang wn_ang wn_ang]);
  Wn = eye(length(idx));
  ForwardKinematics(1);
  err = CalcVWerr(Target, uLINK(to));
  Ek = err'*We*err;
  for n = 1:10
    J = CalcJacobian(idx);
    lambda = Ek + 0.002;
    Jh = J'*We*J + Wn*lambda;      % Hk + wn
    gerr = J'*We*err;              % gk
    dq = Jh \ gerr;                % new
    MoveJoints(idx, dq);
    err = CalcVWerr(Target, uLINK(to));
    Ek2 = err'*We*err;
    if Ek2 < 1E-12
      break;
    elseif Ek2 < Ek
      Ek = Ek2;
    else
      MoveJoints(idx, -dq);        % revert
      break,
    end
  end

类似的优化问题经常出现在计算机视觉和机器学习中. 使用 SR 逆矩阵的迭代算法一般被认为是 Levenberg-Marquardt 方法的一个版本 [135].

2.5.9 附录: 补充函数

第 2.5.4 节及其后各小节中需要使用下面的函数来计算反运动学和雅可比矩阵的数值解.

• 图 2.37 FindRoute.m 查找从身体到目标链节的路径

  function idx = FindRoute(to)
  global uLINK
  i = uLINK(to).mother;
  if i == 1
    idx = [to];
  else
    idx = [FindRoute(i) to];
  end

• 图 2.38 SetJointAngles.m 沿指定索引设置关节角度

  function SetJointAngles(idx, q)
  global uLINK
  for n = 1:length(idx)
    j = idx(n);
    uLINK(j).q = q(n);
  end
  ForwardKinematics(1);

• 图 2.39 CalcVWerr.m 计算位置和姿态误差的函数

  function err = CalcVWerr(Cref, Cnow)
  perr = Cref.p - Cnow.p;
  Rerr = Cnow.R^-1 * Cref.R;
  werr = Cnow.R * rot2omega(Rerr);
  err = [perr; werr];

• 图 2.40 rot2omega.m 将旋转矩阵转换为相应的角速度向量 (2.39)

  function w = rot2omega(R)
  el = [R(3,2)-R(2,3); R(1,3)-R(3,1); R(2,1)-R(1,2)];
  norm_el = norm(el);
  if norm_el > eps
    w = atan2(norm_el, trace(R)-1)/norm_el * el;
  elseif R(1,1)>0 && R(2,2)>0 && R(3,3)>0
    w = [0 0 0]';
  else
    w = pi/2*[R(1,1)+1; R(2,2)+1; R(3,3)+1];
  end