机械臂抓取物体时,既要把夹爪送到物体附近,也要让夹爪朝向与物体配合。关节编码器给出角度,相机给出物体姿态,惯性传感器给出角速度;控制器需要把这些数据放到一致的坐标中,才能算出工具在哪里、下一步怎样转动。位置和旋转在这里有不同的运算规则:位置可以相加,旋转要按顺序组合,旋转矩阵的九个元素也受到几何约束。

这些约束是理解李群的起点。几何上,群元素描述物体和参考系的位置、朝向;代数上,群运算给出变换的组合规则;从拓扑看,流形的形状决定了局部坐标、切空间和指数映射怎样联系。先从机构有哪些独立坐标开始,就能逐步看清旋转、刚体运动和局部修正各该怎样表示。

1. 配置与广义坐标

关节角一旦确定,连杆位置和末端姿态便受到机构几何的限制。建立模型时,需要先选出足以确定整套机构的独立坐标。

1.1 连杆与关节

机器人机构由连杆和连接它们的关节组成。这里先把连杆视为刚体,忽略受力后的弹性变形。移动关节允许相邻连杆沿一条直线相对移动,转动关节允许它们绕固定轴相对旋转;这两类常见关节分别记为 P 和 R。

把三个移动关节串联,可以得到笛卡尔机构,它以三个直线位移定位末端。圆柱机构具有一个转动关节和两个移动关节,可用角度、半径和高度描述末端位置。球面机构则具有两个转动关节和一个移动关节。三个转动关节组成的关节式机械臂,具有类似人体手臂的结构:与直线运动机构相比,它在较小的机械结构中可以获得较大的运动范围。

对于由基本关节串联起来的机构,各关节位移通常可以独立改变。用每个移动关节的直线位移、每个转动关节的角位移组成坐标向量

便能确定整个机构。这组完整、独立的位置变量称为广义坐标:“完整”表示它们足以定位整个系统,“独立”表示其中没有重复描述同一运动的变量。

机构的运动自由度决定了需要多少个独立坐标。同一个转动关节即使同时装有编码器和视觉标记,仍然只有原来的运动自由度。角度绕一周会回到同一物理方向,有机械限位的关节则只能在一定区间内活动。因此,用一串实数存储坐标时,还要保留它们的取值范围以及配置空间的整体连接方式。

1.2 闭环约束

闭环机构中的关节角会互相约束。五杆机构包括机架在内有五根连杆、五个转动关节,其中两个主动驱动,另外三个被动随动。为了让连杆首尾相接,各关节角必须满足闭环几何条件。这种结构可以把电机都安装在基座上,减轻运动部分的质量,也能让多个连杆共同承担负载。

Stewart 平台用六根可伸缩支杆连接固定平台与运动平台。两端的球关节允许支杆改变方向,六根支杆的长度共同限制平台的位置和朝向。描述这类并联机构时,需要分别统计关节数量、驱动数量和独立配置变量的数量。

把机器人所有允许的配置合在一起,得到配置空间 $\mathcal C$。坐标 $q$ 是对配置的描述方式,$\mathcal C$ 则是这些配置本身的集合。

闭环方程中,独立约束的数量决定局部自由度。在约束导数保持固定秩的邻域内,每个独立约束减少一个自由度,重复方程不会再减少一次。特殊装配姿态可能使约束失去独立性,因此那里的自由度需要重新检查。支杆长度与平台姿态也可能对应多个装配分支,长度相同的支杆组未必全局唯一确定平台姿态。

关节坐标描述全部连杆的配置,末端位姿只描述选定的刚体。工具处在相同位置时,肘部和中间连杆仍可能处在不同位置。因此,运动规划要在完整配置空间中检查碰撞,才能发现工具点以外的干涉。

2. 流形与李群

机械臂改变朝向、移动机器人积累里程计运动时,计算出的状态都要满足旋转约束。流形给出这些状态的集合,群运算则规定运动怎样组合。

2.1 光滑流形

流形允许我们在每一点附近使用局部坐标。可以把它想成一张没有尖角的光滑曲面:整体虽然弯曲,小区域里的变化仍能用局部线性空间来分析。这样,微积分就能用于带几何约束的状态。

单位复数位于圆周上,单位四元数位于四维空间中的单位球面上。它们的分量都可以存成实数,但分量之间受到单位长度约束。直接给所有分量加上任意扰动,结果一般会离开原来的集合。

李群同时具有光滑流形和群的结构。流形让我们分析状态附近的微小变化,群运算让我们组合两个有限变换。

局部坐标描述附近有哪些状态,切向量描述从当前状态出发的瞬时变化。例如,圆周上的小邻域可以用一个角度编号,而变化方向沿着当前点的切线。沿切线走有限距离会离开圆周,所以求出向量增量后,还需要将它转换为合法状态。后面的指数映射正承担这一步。

2.2 群公理

设群中的元素为 $X,Y,Z$,组合运算为 $\circ$,单位元素为 $E$。群需要满足:

  1. 封闭性:$X\circ Y$ 仍属于这个群。
  2. 单位元素:$E\circ X=X\circ E=X$。
  3. 逆元素:每个 $X$ 都有 $X^{-1}$,使 $X^{-1}\circ X=X\circ X^{-1}=E$。
  4. 结合律:$(X\circ Y)\circ Z=X\circ(Y\circ Z)$。

空间旋转通常不满足交换律:先绕一根轴旋转,再绕另一根轴旋转,交换顺序往往会改变结果。群中的乘法表示变换组合,只有符合相应公理的运算规则才能用于它。

结合律让机器人坐标链可以分段计算:先合并相邻变换,再作用到点上,结果保持一致。逆元素则让坐标转换可以反向使用,例如在相机坐标与工具坐标之间往返转换。不同轴的旋转仍须保留执行顺序。

2.3 平移群

实向量空间 $\mathbb R^n$ 配合加法,也是一个李群。组合运算是向量相加,单位元素是零向量,逆元素是相反数。它的流形是平的,切空间与原空间具有相同的向量形式,指数与对数映射都是恒等映射。

平移群中的状态更新就是当前位置加上增量。对旋转等弯曲的群,更新改为把当前状态与增量对应的变换组合起来。局部计算仍可使用向量微积分,增量回到状态空间的方式则由群的几何决定。

3. 平面旋转

移动机器人在平面上转向时,一个角度就能描述车体朝向。这个角度也决定怎样把车体中的向量旋转到固定坐标中,因而适合用来先理解旋转表示。

单位复数构成群 $S^1$。任意元素可以写为

其中 $\mathrm i^2=-1$,$\theta$ 为旋转角。它的单位元素是 $1$,逆元素是共轭 $z^*=\cos\theta-\mathrm i\sin\theta$。两个单位复数相乘仍是单位复数,因此组合结果仍然位于圆周上。

把平面向量表示为复数 $x=x_1+\mathrm i x_2$,旋转后的向量为 $zx$。展开乘积,便得到实数矩阵形式

这些矩阵构成 $SO(2)$。它们满足 $R^{\mathsf T}R=I$、$\det R=1$;前一条件保持向量长度与夹角,后一条件保证变换是旋转。

存储分量的数量可以多于对象的自由度。单位复数有两个实数分量,单位长度约束使圆周上的状态只有一个自由参数;矩阵形式有四个元素,描述的平面旋转依然只有一个自由度。

角度、单位复数和矩阵都能表示同一平面旋转,各有不同的计算要求。角度紧凑,但跨过分支边界时,很小的姿态变化可能对应很大的数值差;复数和矩阵便于组合,却需要维护单位长度或正交条件。选择时要一起考虑存储方式、组合运算和误差计算。

4. 空间旋转

抓取任务除了把工具送到目标位置,还要使工具朝向与物体配合。三维朝向的表示必须支持组合、求逆和速度计算。

4.1 SO(3) 与角速度

三维旋转矩阵属于

虽然它用九个矩阵元素表示,但旋转只有三个自由度。为了理解这三个自由度如何出现在微小变化中,对正交条件求时间导数:

因此 $R^{\mathsf T}\dot R$ 是反对称矩阵,可以用三个数表示:

这里 $\omega=(\omega_x,\omega_y,\omega_z)^{\mathsf T}$ 是以当前旋转体坐标表达的角速度,$[\omega]_\times v=\omega\times v$。于是

在单位旋转 $R=I$ 处,导数直接等于 $[\omega]_\times$。这说明旋转的局部变化可以用三维向量描述,但有限旋转本身仍然属于受约束的矩阵集合。

角速度给出了旋转矩阵满足约束的变化率。正交条件把九个导数联系起来,反对称矩阵只留下三个独立分量。左乘当前旋转的转置,把变化率表达在随物体转动的坐标中;换成固定坐标表达时,矩阵乘法的位置也会改变。后面的左右增量,以及空间、本体 Twist,都沿用这个参考系选择。

4.2 单位四元数

单位四元数可写成

其中 $u$ 是表示单位旋转轴的纯虚四元数,$\theta$ 是物理旋转角。把三维向量 $x$ 写成纯虚四元数后,旋转作用为

四元数在向量两侧各乘一次,每侧包含半角,两次作用共同产生完整的物理旋转。因而,四元数指数中的角度与物理旋转角之间有二倍关系。

单位四元数组成三维流形 $S^3$,嵌入四维实数空间。其逆元素为共轭 $q^{-1}=q^*$。四元数 $q$ 与 $-q$ 对任意向量产生相同旋转,因为两侧的负号互相抵消;单位四元数因此对三维旋转形成双重覆盖。比较或插值四元数时,不能把符号不同立即解释为姿态不同。

数值更新需要维护四元数的单位长度。直接更新四个实数分量后,可以归一化;也可以先在三个旋转增量上求解,再作群更新。连续记录姿态时,常选择与上一帧内积非负的等价符号,避免符号翻转造成数值上的跳变。这些处理保持表示的一致性,传感器偏差仍需另外估计或校准。

4.3 旋转组合

把旋转顺序落实到一个向量上,差别会更清楚。取 $p=(1,1,0)^{\mathsf T}$,分别绕固定坐标系的 $x$ 轴和 $y$ 轴旋转 $90^\circ$。对应矩阵为

本例使用列向量与主动旋转,且两次旋转都绕固定轴,后执行的旋转在乘积左侧。先绕 $x$ 轴、后绕 $y$ 轴,得到

交换顺序则得到

因此 $R_yR_xp\ne R_xR_yp$,但每个中间向量及最终向量的长度都仍然是 $\sqrt2$。手腕依次改变两个方向时,同样需要保留旋转轴约定和组合顺序。

固定轴旋转顺序:相同初始向量经两种顺序得到不同终点

图1. 固定轴旋转顺序:相同初始向量经两种顺序得到不同终点

图中蓝线为第一段旋转,红线为第二段旋转;空间轨迹用相同的斜投影显示,坐标标注给出真实三维分量。轨迹数据保存了两次旋转中每个采样点的三维坐标。

本例的第二次旋转仍绕固定轴。若改为绕随物体移动的轴,矩阵组合就要反映轴的变化。实现时先写清矩阵将哪个坐标系的分量转换到哪个坐标系,再把它作用于一个具体点,通常就能发现顺序错误。

5. 李代数与指数映射

优化器求出的三个旋转修正分量,需要经指数映射转换为合法的有限旋转,才能更新九个元素组成的旋转矩阵。

5.1 切空间

流形上每一点都有描述局部变化的切空间。李群在单位元素处的切空间称为李代数。旋转群的李代数是反对称矩阵集合,刚体变换的李代数则具有相应的分块矩阵结构。

为避免每次都保存完整的结构矩阵,使用“帽”与“反帽”运算:$\widehat{\tau}$ 把一个最小维数的向量映射为李代数元素,$\vee$ 将它还原成向量。例如 $SO(3)$ 中 $\widehat\theta=[\theta]_\times$。

李代数元素可以是反对称矩阵、虚数或纯虚四元数,都能写成一组基元素 $E_i$ 的线性组合:

这里的 $m$ 等于群自身的自由度。帽与反帽是两套表示之间互为逆的线性映射;实际计算可以保存 $\tau\in\mathbb R^m$,需要时再恢复具有特定结构的李代数元素。与直接对状态矩阵所有元素进行运算相比,这些向量维数较小,也更便于构造大型优化问题中的状态增量。

指数映射从李代数到群;对数映射在选定分支上把群元素带回李代数。采用方便的记号

这样,$\operatorname{Exp}$ 的输入是小维数向量,输出是合法变换;$\operatorname{Log}$ 的输出是小维数向量。

优化时,可以先在三个旋转修正分量上求解线性子问题,再用指数映射把修正组合到当前旋转上。下一轮围绕新状态继续计算,旋转约束由群更新保持。修正较大时,局部线性模型的误差也可能增大,需要重新检查或缩短更新步长。

5.2 Rodrigues 公式

若角速度在体坐标中保持常量,则微分方程 $\dot R=R[\omega]_\times$ 的解为

令旋转向量 $\theta=\omega t=\vartheta u$,其中 $u$ 为单位轴,$\vartheta=\|\theta\|$。利用

把矩阵指数的幂级数分组,得到

这个式子称为 Rodrigues 公式。角度为零时取连续极限,结果为单位矩阵。它将无约束的局部旋转向量转成满足正交条件的旋转矩阵。

一个常用的矩阵对数表达是

这个直接表达在 $0<\vartheta<\pi$ 时可以使用。当角度为零或半周时,$\sin\vartheta=0$,而矩阵又满足 $R_{ij}=R_{ji}$,所以不能再用上面的反对称部分恢复旋转向量。角度仍然可以从矩阵迹确定,旋转轴则需要分别处理。

零角对应单位旋转,旋转向量取零,轴方向没有唯一意义。半周时,可以从对角元素得到单位轴各分量的绝对值:$|u_i|=\sqrt{(1+R_{ii})/2}$,各分量符号再由非对角关系确定。轴的整体符号仍然存在歧义,因为绕 $u$ 旋转半周与绕 $-u$ 旋转半周相同。因而,对数映射需要选择分支,不能把它当作全局无歧义的旋转坐标。

数值实现要分别处理小角度和接近半周的情况。角度很小时,三角函数差值与分母同时变小,直接相除可能损失精度,可以使用幂级数极限。接近半周时则要留意旋转轴和对数分支:相邻测量可能得到方向相反的旋转向量。用对数计算误差,需要让分支选择与当前状态、目标的连续变化相容。

5.3 指数映射性质

对于同一个李代数元素 $\widehat\tau$,指数具有

第一条关系适用于沿同一生成方向的连续运动。不同方向的有限变换一般需要有序组合。对矩阵群还有

这可以通过展开指数的幂级数得到:每个高次幂中相邻的 $X^{-1}X$ 都约掉。它让坐标变换与指数映射之间建立了明确关系,也是下面伴随推导的基础。

指数与对数在切空间和流形之间建立联系

图2. 指数与对数在切空间和流形之间建立联系

同轴的两段旋转可以把角度相加,因为生成矩阵可交换。不同轴的组合则包含与执行顺序有关的影响,直接相加会遗漏这些项。足够小的增量可以近似相加,有限旋转仍应通过指数映射并按顺序组合。

5.4 有限旋转与线性近似

切空间适合描述小增量,不能把它的一阶近似当成任意角度的旋转。取旋转向量 $\theta=(0,0,\pi/2)^{\mathsf T}$。Rodrigues 公式给出

若将一阶表达 $I+[\theta]_\times$ 直接用于这个有限角度,则得到

对单位向量 $p=(1,0,0)^{\mathsf T}$,两者分别给出

$R$ 保持长度为一,$L$ 却改变了长度;直接检查还得到 $L^{\mathsf T}L\ne I$、$\det L=1+\pi^2/4\approx3.467$。下图展示同一平面单位圆在两种运算下的结果。选择 $90^\circ$ 是为了清楚展示有限角度下的差异:$\operatorname{Exp}(\theta)\approx I+[\theta]_\times$ 只是一阶小增量近似,而指数映射给出的有限旋转满足几何约束。

指数映射与一阶矩阵近似:前者保持单位圆,后者改变长度

图3. 指数映射与一阶矩阵近似:前者保持单位圆,后者改变长度

两张计算图的绘图与核算脚本、单位圆数据及数值结果可以下载复算。角度、初始向量和采样间隔在脚本中明确给出。

6. SE(3) 与齐次变换

一个工具点从手腕坐标系转换到基座坐标系时,会同时经历旋转和平移。齐次变换让这两部分以统一规则参与坐标链计算。

三维刚体变换把旋转 $R$ 和平移 $t\in\mathbb R^3$ 放在同一个齐次矩阵中:

其对点的作用是 $x'=Rx+t$。组合两个变换时

刚体变换的局部向量有六个分量。本篇采用先平移、后旋转的顺序:

其指数为

组合式中,第二段平移先经过前一段旋转,再加到已有位置上,因为它原本在前一坐标系中表达。手腕向自身前方移动时,这个方向在基座中的朝向取决于手腕姿态。只有两段平移已经采用同一参考系时,才可以直接相加。

指数中的线性参数与转动共同决定最终位置。绕轴转动并平移时,线速度方向随着转动改变,积分结果由上面的矩阵计算。令旋转部分为零,可以恢复纯平移的普通向量加法,也可以用这个极限检查一般表达。

6.1 Twist 与螺旋轴

工具的瞬时运动可以用六维速度 Twist 统一表示,其中包含角速度与平移相关速度。与 Modern Robotics 代码接口一致,本小节采用先角速度、后线速度的顺序:

若 $T=(R,p)$ 表示工具坐标系相对固定坐标系的位姿,空间与本体表达分别满足 $[V_s]=\dot T T^{-1}$、$[V_b]=T^{-1}\dot T$。空间表达中的 $v_s=\dot p-\omega_s\times p$,因此不能把它直接当作工具原点的坐标速度 $\dot p$;本体表达中的 $v_b=R^{\mathsf T}\dot p$。同一瞬时刚体运动可以这样使用两套坐标,但各分量的含义必须随参考系一起说明。

前面局部增量采用 $[\rho;\theta]$,代码中的六维位移则采用 $[\theta;\rho]$,六维速度采用 $[\omega;v]$。$\rho$ 是指数坐标的线性部分,与实际平移 $p$ 一般不同;使用时还需按单位区分位移和速度。若将先线性、后角度的排列记为 $V_{v\omega}$,交换前三与后三个分量便得到 $V_{\omega v}$。第 7 节的伴随矩阵也要同步交换行列,Adjoint(T) 在代码中的形式为

单个关节的运动可以用螺旋轴 $S$ 编码。设轴过点 $q_a$,单位方向为 $s$,螺距为 $h$,则

转动关节取 $h=0$;沿单位方向 $s$ 的移动关节取 $S=[0;s]$,此时 $\theta$ 是位移。有限螺距的 $h$ 表示每单位旋转角对应的轴向位移。这里归一化的是旋转部分,或纯平移时的线性部分,而不是把整个六维向量当成普通单位向量。

螺旋轴的位置、方向、转动与轴向移动

图4. 螺旋轴的位置、方向、转动与轴向移动

六维量的排列和参考系要分别确定。本章理论增量先线性、后角度,Modern Robotics 接口先角度、后线性;空间量在固定系中表达,本体量在运动物体系中表达。交换前三与后三项只改变排列,参考系转换还需要伴随矩阵。遇到同一个字母兼指增量、角度或速度时,要回到它的定义和单位判断。

螺旋轴的线性部分记录轴的位置与螺距。即使关节只有转动,只要转轴不穿过所选坐标原点,这部分也可能非零。它记录的是轴相对参考原点的位置,关节的自由度仍由机构结构决定。

6.2 螺旋位移

以单位位姿为起点,沿固定轴运动 $\theta$,便得到 $T=\exp([S]\theta)$。当 $S=[s;v]$、$\|s\|=1$ 时,平移部分不是简单的 $v\theta$,而是

这与前面的 $V(\theta)\rho$ 表达描述同一个 SE(3) 指数,只是这里把旋转轴归一化,并把轴向行程单独提出。取 $q_a=(3,0,0)$、$s=(0,0,1)$、$h=2$,得到 $S=(0,0,1,0,-3,2)^{\mathsf T}$;本例长度采用米,螺距为每弧度两米。走过 $\theta=\pi/2$ 后,旋转为绕 $z$ 轴的四分之一周,平移为 $(3,-3,\pi)^{\mathsf T}$ 米;轴上的点 $(3,0,0)$ 则到达 $(3,0,\pi)$,轴向前进量正好是 $h\theta$。

1
2
S = mr.ScrewToAxis([3, 0, 0], [0, 0, 1], 2)
T = mr.MatrixExp6(mr.VecTose3(S * (np.pi / 2)))

可下载完整核算脚本与数值结果。运行脚本需要 NumPy 及放在同一目录的固定版本代码模块,脚本也核验了两种六维顺序下伴随矩阵的转换。

变换矩阵的平移块描述原点变换后的位置。轴上的点只沿轴前进,轴外的点还会绕轴转动,因此各点的位移与这个平移块一般不同。把变换作用在不同位置的点上,可以进一步看清实际点运动、平移块和指数坐标线性部分之间的关系。

7. 局部增量与伴随

同一次运动既可以在机器人自身坐标中表达,也可以在固定坐标中表达。先说明增量在哪个坐标系里,才能一致地使用运动与误差数据。

对一个状态 $X$ 加上局部增量,有两种自然方式。右侧增量定义为

这里的增量位于与当前状态相连的局部坐标中。左侧增量则为

左侧增量在单位元素对应的全局坐标中表达。选用左增量还是右增量,会决定后续误差和导数的参考系,推导时需要保持一致。

同一个变化从局部坐标转到全局坐标,通过伴随矩阵完成:

这一关系来自同一个结果状态的两种写法。令

右乘 $X^{-1}$,并使用指数映射的共轭性质,便有

共轭运算作用于李代数是线性的,所以能够用一个矩阵作用在小维数向量上。伴随保留组合关系:$\operatorname{Ad}_{XY}=\operatorname{Ad}_X\operatorname{Ad}_Y$;逆变换对应 $\operatorname{Ad}_{X^{-1}}=(\operatorname{Ad}_X)^{-1}$。这些性质允许一串坐标变化按顺序组合。

对 $SE(3)$,在本篇先平移、后旋转的向量顺序下,

伴随的非对角块来自参考原点的变化。刚体绕自身原点转动时,本体表示的线性部分可以为零;从另一原点表达同一运动,六维量中便会出现与原点偏移有关的线性分量。这个分量把同一刚体速度场换到新坐标中。

7.1 相同数字与相同运动

取物体当前朝向为绕固定 z 轴旋转 90°,原点位于世界坐标的 (1, 2, 0) 米。先只给一个沿 x 轴的 0.1 米平移增量。右乘把这个 x 解释为物体自身的 x 方向,结果原点为 (1, 2.1, 0);左乘把它解释为世界 x 方向,结果为 (1.1, 2, 0)。数字相同,表达的方向不同,所以结果也不同。

再让物体绕自身 z 轴旋转 30°,保持自身原点不动。本体增量的线性部分为零;使用本节先线性后角度的顺序,转换到空间表达后为

原点偏移与转角的叉乘产生了空间增量的线性部分。用这个完整增量左乘当前位姿,与本体增量右乘得到相同结果,原点仍为 (1, 2, 0)。若左侧只保留相同转角、线性部分取零,转动中心便变成世界原点,物体原点会移到约 (−0.133975, 2.232051, 0) 米。修正这个差异需要转换参考系与转动中心。

这一计算可用标准库核算脚本的 --chapter 02 选项复算,JSON 结果保存了各项输出。脚本分别计算左右组合、伴随转换和完整指数;在该算例中,两种等价表达的矩阵逐元素差异为浮点精度内的零。

8. 群上微分与积分

视觉控制需要计算姿态变化怎样影响点的位置,角速度积分则需要更新姿态。两者都要在合法几何状态与局部向量之间转换。

8.1 群上 Jacobian

普通向量函数 $f:\mathbb R^m\to\mathbb R^n$ 的 Jacobian 由各偏导数组成。第 $i$ 列可以通过沿自然基向量 $e_i$ 扰动输入得到:

流形上的扰动和差值需要采用相应的几何运算。定义右侧加法 $X\oplus\tau=X\operatorname{Exp}(\tau)$、右侧差值 $Y\ominus X=\operatorname{Log}(X^{-1}Y)$,便可以沿用偏导的思路写出

先把输入切空间的增量送回群,经函数映射得到新输出,再把输出变化转回输出点的切空间。这一过程得到的 Jacobian 是普通实数矩阵,映射的对象则是输入、输出两边的局部切向量。

因此,在小增量范围内,

这里两处 $\oplus$ 分别属于输入与输出空间;输入可能是旋转,输出却可能是普通三维向量,它们的增量操作需要分别选择。

数值核对群上 Jacobian 时,要用合法增量更新输入,并采用与解析式一致的输出误差。如果解析式使用右增量,有限差分也要使用右增量,才能比较同一个导数。差分步长需要兼顾截断误差和舍入误差,过大或过小都可能降低准确性。

8.2 旋转作用的导数

考虑 $f(R)=Rp$,其中 $p$ 是固定的三维点。采用右侧旋转增量,利用 $\operatorname{Exp}(\theta)\approx I+[\theta]_\times$:

所以相对于局部旋转向量的 Jacobian 是

这个导数针对三个独立旋转方向,矩阵中的九个元素仍受正交约束联系。因此,局部参数既保留了几何约束,也让导数计算保持紧凑。

沿用前一算例的 90° 朝向,并取物体系中的点 (0.2, 0.1, 0) 米。对自身 z 轴作一个小转角,解析导数为世界系中的 (−0.2, −0.1, 0) 米每弧度。核算脚本以一百万分之一弧度作右增量,差分得到约 (−0.19999995, −0.10000010, 0),与解析式相符。这个检查同时验证了负号、增量位置和输出参考系,比只看矩阵尺寸更有信息。

8.3 旋转积分

对常量局部速度,指数映射给出连续运动的积分。速度随时间变化时,可以把它分成持续短时间的常量片段。设第 $k$ 段局部速度为 $v_k$,时间长度为 $\Delta t_k$,局部增量为 $\tau_k=v_k\Delta t_k$,递推为

每一步先把局部变化转成群元素,再与已有状态组合。对于角速度样本,旋转矩阵与四元数分别更新为

两式中的指数分别按矩阵群和四元数群定义,四元数更新仍使用半角规则。有了有限变换、局部速度与积分的对应关系,下一章就可以进一步计算关节运动怎样带动末端。

群更新保持姿态的几何约束,积分精度则还取决于分段常量近似、角速度偏差和采样延迟。角速度方向改变时,需要按时间顺序组合小旋转;先把样本全部相加,再作一次有限旋转,会丢失顺序影响。检查姿态漂移时,应分别检查几何合法性和时间积分误差。

参考资料

H. Harry Asada. Introduction to Robotics, MIT 2.12, Chapter 3, §§3.1–3.2; Chapter 4, §4.1. © Massachusetts Institute of Technology;中文节选重排、记号校订与例题补充;译编采用 CC BY-NC-SA 4.0。

Joan Solà, Jeremie Deray, Dinesh Atchuthan. A micro Lie theory for state estimation in robotics, version 9, §§I, II.A–G, II.I, Examples 1–6, Appendices A, B, D and E. 中文节选重排与例题补充;指数映射图裁自 p. 4, Figure 6;原文与译编采用 CC BY-NC-SA 4.0。

José Luis Blanco-Claraco. A tutorial on SE(3) transformation parameterizations and on-manifold optimization, version 2, §9.4.1.2, pp. 47–48 (PDF pp. 49–50). 中文节选,补充零角与半周边界说明;原文与译编采用 CC BY-NC-SA 4.0。

Kevin M. Lynch and Frank C. Park. Modern Robotics: Mechanics, Planning, and Control. Cambridge University Press, 2017; updated first-edition preprint, 2019. §§3.3.2–3.3.3, pp. 95–104; Eqs. (3.74)–(3.76), (3.85)–(3.88). 概念整理与公式核对。 Figure 3.19(p. 101,PDF p. 121);按首页授权裁切复用,© 原作者/权利人。

NxRLab. Modern Robotics Python core.py, commit cbd41fdf0bf75dbd5e986d832ae14226964ff0ac. Functions VecTose3, Adjoint, ScrewToAxis, MatrixExp6. © 2019 NxRLab;API 与代码例译编,MIT License;公开模块附许可文本。