Part I · 第 5 章 刚体运动与李群 · 5.8

伴随表示

同一个刚体运动,在世界系里看和在机体系里看,twist 的六个数不一样。伴随矩阵 Ad_T 就是两种看法之间的换算表:它把 twist 从一个坐标系搬到另一个坐标系,也把左扰动换成右扰动。机器人身上的传感器和控制器各用各的坐标系,没有它,数据就对不上。

约 25 分钟 前置:5.3 SE(3) · 5.6 指数映射 · 5.7 扰动 4 个交互

    1为什么机器人需要它

    一台移动操作机器人身上有一串坐标系:世界系、底盘系、机械臂基座系、法兰盘系、相机系、工具系。每个传感器和每个控制器各讲各的坐标系:IMU 的陀螺仪测的是 IMU 系里的角速度,轮式里程计给的是底盘系里的速度,视觉伺服说"相机往右挪两厘米",是相机系里的指令,而机械臂控制器要的是基座系里的末端速度,腕部力传感器报告的力和力矩又在传感器系里。

    如果只是位置向量,换系很简单:左乘一个旋转加上平移。可是速度不一样。一个刚体的速度不是一个向量,而是 6 个数 \((\boldsymbol\omega,\mathbf v)\),而且其中的线速度 \(\mathbf v\) 取决于"你问的是哪个点的速度"。把 twist 换到另一个坐标系,不仅要旋转,还要把"参考点"也换掉,中间多出一项由杠杆臂产生的 \(\mathbf t\times\boldsymbol\omega\)。完成这个换算的 6×6 矩阵,就是伴随矩阵 \(\operatorname{Ad}_T\)。

    它还有两个更隐蔽的用处:把 5.7 节的左扰动换成右扰动,以及把一个坐标系里的协方差换到另一个坐标系里(5.9 节)。几乎每个涉及多坐标系的估计器或控制器里,都有一个 \(\operatorname{Ad}\)。

    2直觉:同一个速度场,两种读法

    把一个正在运动的刚体想成一整片"橡皮板":板上每个点都有自己的速度,所有速度合起来是一个速度场。刚体运动的速度场很规整:\(\mathbf u(\mathbf x) = \mathbf u_0 + \boldsymbol\omega\times(\mathbf x-\mathbf x_0)\),只要知道角速度 \(\boldsymbol\omega\) 和某一个参考点 \(\mathbf x_0\) 的速度 \(\mathbf u_0\),整个场就确定了。twist 就是这 6 个数。区别只在于选哪个点做参考点,用哪套坐标轴读数:

    • 机体 twist \(\boldsymbol\xi_b = (\boldsymbol\omega_b,\mathbf v_b)\):参考点是机体原点,数值在机体系的坐标轴上读出。这是机载传感器天然看到的:陀螺仪给 \(\boldsymbol\omega_b\),轮速给 \(\mathbf v_b\)。
    • 空间 twist \(\boldsymbol\xi_s = (\boldsymbol\omega_s,\mathbf v_s)\):参考点是"此刻恰好位于世界原点的那个机体点"(想象橡皮板延伸到世界原点),数值在世界系的坐标轴上读出。这是 5.6 节里 \(\Exp(s\boldsymbol\xi)\) 的 \(\boldsymbol\xi\)。

    下面先在平面上看,twist 只有三个数 \((\omega, v_x, v_y)\)。机器人在世界里有位姿 \(T\),正绕着某个瞬时转动中心 \(C\) 以角速度 \(\omega\) 转动。拖动机器人、它的朝向手柄和中心 \(C\),看两种读法如何变。青色小箭头就是整个速度场。

    FIG 1平面刚体:机体 twist 与空间 twist
    本课的配色:角速度用紫色,线速度用琥珀色;机体量画实线,空间量画虚线。琥珀色实线箭头:机体原点的速度(图中画在世界系里,机体系的读数是它旋转 \(R\T\) 之后的 \(\mathbf v_b\))。琥珀色虚线箭头:位于世界原点的那个机体点的速度,也就是 \(\mathbf v_s\)。青色小箭头是整个速度场;它在 \(C\) 处为零,紫色虚圆是机器人原点绕 \(C\) 走的圆。三个手柄:机器人原点、朝向、转动中心 \(C\)。
    试一试
    • 把转动中心 \(C\) 拖到世界原点。\(\mathbf v_s\) 变成了什么?为什么?
    • 把 \(C\) 拖到机器人自己的原点(原地打转)。\(\mathbf v_b = 0\),但 \(\mathbf v_s\) 还是 0 吗?
    • 保持 \(C\) 和 \(\omega\) 不动,把机器人拖到离出发点 2 个单位以外。\(\boldsymbol\xi_b\) 和 \(\boldsymbol\xi_s\) 哪一个变了?

    你应该看到:\(\boldsymbol\xi_s\) 只取决于"世界里的运动本身"(转动中心和角速度),和机器人此刻在哪儿无关;\(\boldsymbol\xi_b\) 则取决于机器人的位姿,因为同一个运动,机器人站得不同,机体原点的速度就不同。\(\boldsymbol\xi_b\) 和 \(\boldsymbol\xi_s\) 之间的换算,是读数里的那个 3×3 矩阵 \(\operatorname{Ad}_T\)。注意它左下角的那一列 \((t_y,\,-t_x)\):这是杠杆臂,机器人离世界原点越远,两个读数差得越多。

    3动手玩:同一根螺旋轴,换个坐标系

    三维里也一样。在 5.6 节我们看到,每个 twist 都对应一根螺旋轴(轴线、转角、螺距)。机体 twist \(\boldsymbol\xi_b\) 在机体系里描述了一根轴;把机器人放到世界里的位姿 \(T\) 上,这根轴就跟着机器人一起搬到了世界里的某个位置,朝向某个方向。而空间 twist \(\boldsymbol\xi_s = \operatorname{Ad}_T\boldsymbol\xi_b\) 直接在世界系里描述螺旋轴。两者描述的是同一根轴:伴随矩阵只是改了读法。

    FIG 2 验证这一点。宽而半透明的紫色实线带是"把机体系里的轴用 \(T\) 搬进世界"得到的轴(机体量,实线);紫色细虚线是用 \(\operatorname{Ad}_T\boldsymbol\xi_b\) 算出来的轴(空间量,虚线)。如果虚线始终落在实线带里(实线带只在两端露出头),说明 \(\operatorname{Ad}_T\) 做对了。读数还会显示螺距 \(h\) 和转角 \(\theta\):它们在换系时不变。

    FIG 2Ad_T 把螺旋轴从机体系搬到世界系
    拖动画面旋转视角
    机体 twist 预设
    灰色线框是世界系中的起点 \(T\)(带坐标轴的盒子),绿色盒子是 \(T\,\Exp(s\boldsymbol\xi_b)\),它和 \(\Exp(s\boldsymbol\xi_s)\,T\) 是同一个位姿(读数中给出两者的差)。拖动画面旋转视角,可以从侧面检查两条轴线是否重合。配色同 FIG 1:紫色是角速度和螺旋轴,琥珀色是线速度,机体量画实线、空间量画虚线。
    试一试
    • 选"绕机体 z 轴自转"预设,把盒子移到离世界原点 1 个单位以外。机体 twist 的线速度是 0,空间 twist 的 \(\mathbf v_s\) 却不是 0(\(\|\mathbf v_s\| > 0.5\))。\(\mathbf v_s\) 等于什么?
    • 选"纯平移"预设。\(\boldsymbol\omega = 0\) 时,\(\operatorname{Ad}_T\) 对 \(\mathbf v\) 做的只是旋转:\(\mathbf v_s = R\mathbf v_b\)。螺旋轴到哪儿去了?

    4再看数学

    提示:把鼠标悬停在公式中带底色的符号上,FIG 1 的箭头或 FIG 2 的轴线会被高亮。

    共轭:把一个运动"搬到"另一个坐标系

    设 \(T=(R,\mathbf t)\),\(\boldsymbol\xi^\wedge\) 是 twist 的 4×4 矩阵(5.5 节)。直接做矩阵乘法:

    $$ T\,\boldsymbol\xi^\wedge\,T^{-1} = \begin{bmatrix} R\skew{\boldsymbol\omega}R\T & R\mathbf v - R\skew{\boldsymbol\omega}R\T\mathbf t \\ \mathbf 0\T & 0 \end{bmatrix} = \begin{bmatrix} \skew{R\boldsymbol\omega} & R\mathbf v + \mathbf t\times R\boldsymbol\omega \\ \mathbf 0\T & 0 \end{bmatrix}, $$

    用到 \(R\skew{\boldsymbol\omega}R\T = \skew{R\boldsymbol\omega}\)(1.8 节、1.10 节)。右边仍然是一个 twist 的 hat,它的向量是 \((R\boldsymbol\omega,\ R\mathbf v + \skew{\mathbf t}R\boldsymbol\omega)\)。把这个线性映射写成 6×6 矩阵,就是伴随矩阵。对矩阵指数取共轭,指数符号里面的 twist 就换成 \(\operatorname{Ad}_T\boldsymbol\xi\):

    $$ T\,\Exp(\boldsymbol\xi)\,T^{-1} = \Exp\big(\operatorname{Ad}_T\boldsymbol\xi\big), \qquad \operatorname{Ad}_T = \begin{bmatrix} R & 0 \\ \hl{lever}{\skew{\mathbf t}R} & R \end{bmatrix}\ \text{(order }(\boldsymbol\omega,\mathbf v)\text{)}, \qquad \operatorname{Ad}_R = R\ \text{ on }\SO. $$

    对 \(\SO\) 只剩旋转部分:\(R\,\Exp(\boldsymbol\omega)R\T = \Exp(R\boldsymbol\omega)\),也就是 5.7 节用到的那条。直观含义:把"绕 \(\boldsymbol\omega\) 转"这个动作,由 \(R\) 描述的坐标系看过去,转轴变成 \(R\boldsymbol\omega\)。

    空间 twist 与机体 twist

    设机体系 \(\{b\}\) 在世界系 \(\{s\}\) 中的位姿 \(T(t)\) 随时间变化。两种速度分别是 \(\hat{\boldsymbol\xi}_b = T^{-1}\dot T\) 与 \(\hat{\boldsymbol\xi}_s = \dot T T^{-1}\)(取 vee 得到 6 维向量)。展开:

    $$ T^{-1}\dot T = \begin{bmatrix} R\T\dot R & R\T\dot{\mathbf t} \\ 0 & 0\end{bmatrix}\ \Rightarrow\ \hl{xib}{\boldsymbol\xi_b} = (\boldsymbol\omega_b,\ R\T\dot{\mathbf t}), \qquad \dot T T^{-1} = \begin{bmatrix} \dot RR\T & \dot{\mathbf t} - \dot RR\T\mathbf t \\ 0 & 0\end{bmatrix}\ \Rightarrow\ \hl{xis}{\boldsymbol\xi_s} = (\boldsymbol\omega_s,\ \dot{\mathbf t} + \mathbf t\times\boldsymbol\omega_s), $$ $$ \boldsymbol\xi_s = \operatorname{Ad}_T\,\boldsymbol\xi_b, \qquad \boldsymbol\xi_b = \operatorname{Ad}_{T^{-1}}\boldsymbol\xi_s = \operatorname{Ad}_T^{-1}\boldsymbol\xi_s. $$

    读法:\(\mathbf v_b = R\T\dot{\mathbf t}\) 是机体原点的速度,用机体轴读;\(\mathbf v_s = \dot{\mathbf t} + \mathbf t\times\boldsymbol\omega_s\) 是"位于世界原点的那个机体点"的速度(刚体速度场在 \(\mathbf x = \mathbf 0\) 处取值:\(\dot{\mathbf t} + \boldsymbol\omega_s\times(\mathbf 0 - \mathbf t)\)),用世界轴读。两者相差的就是杠杆臂 \(\mathbf t\times\boldsymbol\omega_s\)。

    左扰动与右扰动

    共轭公式移项就是 5.7 节留下的承诺:

    $$ T\,\Exp(\boldsymbol\delta_r) = \Exp(\operatorname{Ad}_T\boldsymbol\delta_r)\,T \quad\Longleftrightarrow\quad \boldsymbol\delta_l = \operatorname{Ad}_T\,\boldsymbol\delta_r, \qquad \Sigma_l = \operatorname{Ad}_T\,\Sigma_r\operatorname{Ad}_T\T. $$

    最后一个式子说的是:如果右扰动的协方差是 \(\Sigma_r\),换成左扰动后协方差是 \(\operatorname{Ad}_T\Sigma_r\operatorname{Ad}_T\T\)(5.9 节会用)。

    性质与 wrench

    性质公式含义
    复合\(\operatorname{Ad}_{T_1T_2} = \operatorname{Ad}_{T_1}\operatorname{Ad}_{T_2}\)沿坐标系链一路换算
    逆\(\operatorname{Ad}_T^{-1} = \operatorname{Ad}_{T^{-1}} = \begin{bmatrix}R\T & 0\\ -R\T\skew{\mathbf t} & R\T\end{bmatrix}\)反方向换算,不用求 6×6 的逆
    不变量\(\|\boldsymbol\omega\|,\ h = \tfrac{\boldsymbol\omega\cdot\mathbf v}{\|\boldsymbol\omega\|^2}\)转角和螺距不随坐标系改变
    wrench\(\mathbf F_b = \operatorname{Ad}_T\T\,\mathbf F_s\)力矩与力 \((\mathbf m,\mathbf f)\) 与 twist 对偶,用转置换系
    无穷小形式\(\operatorname{Ad}_{\Exp(\boldsymbol\xi)} = \exp(\operatorname{ad}_{\boldsymbol\xi}),\ \operatorname{ad}_{\boldsymbol\xi} = \begin{bmatrix}\skew{\boldsymbol\omega} & 0\\ \skew{\mathbf v} & \skew{\boldsymbol\omega}\end{bmatrix}\)李括号 \([\boldsymbol\xi_1,\boldsymbol\xi_2] = \operatorname{ad}_{\boldsymbol\xi_1}\boldsymbol\xi_2\)

    wrench 那一行的来源是功率不变:功率 \(\mathbf F\T\boldsymbol\xi\) 是标量,换系后不能变,\(\mathbf F_b\T\boldsymbol\xi_b = \mathbf F_s\T\operatorname{Ad}_T\boldsymbol\xi_b\),所以 \(\mathbf F_b = \operatorname{Ad}_T\T\mathbf F_s\)。这里 wrench 的顺序是(力矩,力),与 twist(角速度,线速度)对应。

    5机器人中的它:手眼速度与腕力传感器

    场景一:视觉伺服里的"相机系指令"

    手眼相机装在机械臂末端。视觉伺服律说:"目标在相机系里的位置是 \(\mathbf p_c\),所以相机应该以 \(\mathbf v_c = k\,\mathbf p_c\) 朝目标走,同时转向它。"这是相机系里的 twist \(\boldsymbol\xi_c\)。机械臂控制器却要基座系里的末端速度。正确的转换是 \(\boldsymbol\xi_s = \operatorname{Ad}_{T_{bc}}\boldsymbol\xi_c\),其中 \(T_{bc}\) 由当前关节角和手眼标定共同给出。如果把 \(\boldsymbol\xi_c\) 当成基座系里的 twist 直接执行,相机就会朝错误的方向走。

    下面的平面机械臂上,相机用一个固定的手眼变换装在末端。两支琥珀色箭头与 FIG 1 同义:实线是相机(机体)原点的速度 \(\mathbf v_c\),虚线是空间 twist 的 \(\mathbf v_s\)。绿色是"用 \(\operatorname{Ad}\) 换系后执行"(即 \(T_{bc}\Exp(\tau\boldsymbol\xi_c)\))预期到达的相机位姿,红色是"没换系直接当基座系 twist 执行"(\(\Exp(\tau\boldsymbol\xi_c)T_{bc}\))到达的位姿;\(\tau = 0.6\)。

    FIG 3手眼视觉伺服:相机系 twist → 基座系 twist
    拖动标着"目标"的圆点。机械臂基座在原点,两节连杆,滑块调关节角。相机(小楔形)通过固定的手眼变换装在末端,朝向它自己的 \(x\) 轴。读数里的距离是执行之后相机原点到目标的距离。上方读数给出相机系 twist \(\boldsymbol\xi_c\) 和换系后的 \(\boldsymbol\xi_s = \operatorname{Ad}_{T_{bc}}\boldsymbol\xi_c\)(紫色为角速度分量,琥珀色为线速度分量)。机体(相机)量画实线,空间量画虚线。
    试一试
    • 调整关节角和目标位置,使"换系后执行"让相机更接近目标,而"没换系"让相机离目标更远。这说明直接把 \(\boldsymbol\xi_c\) 当作基座系 twist 会造成什么?
    • 把目标拖到相机原点附近(距离 < 0.15)。\(\boldsymbol\xi_c\) 和 \(\boldsymbol\xi_s\) 怎么了?

    场景二:腕力传感器的 wrench

    腕部六维力传感器报告的是传感器系里的 wrench \((\mathbf m_b,\mathbf f_b)\):作用在传感器原点上的合力矩和合力。要判断末端对世界施加了多大的力和力矩,或者做阻抗控制(Part IV),就要把它换到世界系或基座系。换系用的是 \(\operatorname{Ad}\) 的转置。平面里这是一个 3×3 矩阵。关键物理:力不变,只是把"力矩的参考点"从世界原点挪到了传感器原点,改变的是杠杆臂。

    FIG 4力传感器:F_b = Ad_Tᵀ F_s
    琥珀色箭头是接触点 \(P\) 处的力 \(\mathbf f\)(拖动 \(P\) 会让它沿作用线滑动;拖动箭头尖改变力的大小和方向,作用线绕 \(P\) 转)。紫色虚线是世界原点到作用线的杠杆臂 \(d_s\)(空间量,虚线),紫色实线是传感器原点到作用线的杠杆臂 \(d_b\)(机体量,实线)。力矩 \(m = \pm d\,\|\mathbf f\|\),符号取逆时针为正。读数里 "几何直接算" 与 "\(\operatorname{Ad}_T\T\mathbf F_s\)" 两种算法应该相同;公式里力矩分量为紫色,力分量为琥珀色。
    试一试
    • 沿着力的作用线把 \(P\) 滑动 1 个单位以上。力矩 \(m_s\)、\(m_b\) 变了吗?为什么?
    • 拖动传感器(或转动力的方向),让传感器原点落在力的作用线上(杠杆臂 \(d_b < 0.1\),\(\|\mathbf f\| > 0.5\))。传感器测到的是什么?
    真实系统里

    机械臂的空间雅可比的第 \(i\) 列是 \(\operatorname{Ad}_{e^{[S_1]\theta_1}\cdots e^{[S_{i-1}]\theta_{i-1}}}S_i\),机体雅可比与它只差一个 \(\operatorname{Ad}_{T^{-1}}\)(3.4 节、5.10 节)。手眼标定求出的 \(T_{ec}\),每次用到相机速度或位姿误差时,都通过 \(\operatorname{Ad}_{T_{ec}}\) 进入机械臂的控制回路。阻抗/导纳控制和接触估计里,力传感器的 wrench 要用 \(\operatorname{Ad}^{\!\top}\) 换到工具坐标系或世界系。在状态估计器里,协方差 \(\Sigma\) 在左右两种约定之间转换,用的是 \(\operatorname{Ad}\,\Sigma\operatorname{Ad}\T\)。

    6常见误区

    误区 1:只旋转 ω 和 v,忘了杠杆臂

    把 twist 当成两个普通向量,各自乘 \(R\),得到的是 \(\operatorname{diag}(R,R)\boldsymbol\xi\),丢了 \(\skew{\mathbf t}R\boldsymbol\omega\) 一项。只在 \(\mathbf t = 0\)(两个坐标系原点重合)或 \(\boldsymbol\omega = 0\) 时才碰巧正确。症状:直线行驶(\(\boldsymbol\omega = 0\))没问题,一旦转弯,哪怕是原地转,偏离原点安装的传感器的速度就会多出或少掉一个横向的 \(\boldsymbol\omega\times\mathbf t\) 分量。

    误区 2:Ad 的下标和方向写反

    \(\boldsymbol\xi_s = \operatorname{Ad}_{T_{sb}}\boldsymbol\xi_b\):用"机体在世界里的位姿"把机体 twist 换成空间 twist。反过来要用 \(\operatorname{Ad}_{T_{sb}}^{-1} = \operatorname{Ad}_{T_{bs}}\)。写成 \(T_{sb}\) 还是 \(T_{bs}\) 是最常见的符号错误;给每个变换标明"谁在谁里",让下标像链条一样首尾相接,能避免大部分错误。

    误区 3:twist 顺序与 wrench 顺序不配套

    本课 \(\operatorname{Ad}_T = \begin{bmatrix}R&0\\ \skew{\mathbf t}R&R\end{bmatrix}\) 对应 twist \((\boldsymbol\omega,\mathbf v)\)。把平移放在前面的库,\(\operatorname{Ad}\) 是 \(\begin{bmatrix}R&\skew{\mathbf t}R\\ 0&R\end{bmatrix}\),块的位置对调。wrench 的顺序要跟 twist 的顺序保持一致,不然 \(\mathbf F\T\boldsymbol\xi\) 就不是功率了。

    误区 4:把"空间速度"当成"世界系里的末端线速度"

    \(\mathbf v_s\) 是"位于世界原点的那个机体点"的速度,不是机体原点的速度。机体原点在世界系里的线速度是 \(\dot{\mathbf t} = \mathbf v_s + \boldsymbol\omega_s\times\mathbf t\)。想要末端点的速度,要自己减去(或加上)这一项。

    7小练习

    1. \(R\,\Exp(\boldsymbol\omega)\,R\T\) 等于?
    这是 \(\operatorname{Ad}_R = R\):\(R\skew{\boldsymbol\omega}R\T = \skew{R\boldsymbol\omega}\),对矩阵指数取共轭就把转轴也转到新坐标系。
    2. 一辆小车位于世界系 \(\mathbf t = (2,0,0)\),朝向与世界系一致(\(R = I\)),在原地绕自己的 z 轴自转:机体 twist \(\boldsymbol\xi_b = (0,0,1;\ 0,0,0)\)。它的空间 twist 是?
    \(\mathbf v_s = R\mathbf v_b + \mathbf t\times R\boldsymbol\omega = (2,0,0)\times(0,0,1) = (0\cdot1-0\cdot0,\ 0\cdot0-2\cdot1,\ 0) = (0,-2,0)\)。直观验证:世界原点在小车原点的 \(-x\) 方向 2 个单位处,逆时针转动时那里的点向 \(-y\) 方向运动,速度大小 \(\omega\cdot2 = 2\)。
    3. 用 \(\operatorname{Ad}_T\) 把 twist 从机体系换到空间系之后,下面哪个量保持不变?
    \(\boldsymbol\omega\) 被 \(R\) 转了方向,\(\mathbf v\) 被旋转并加了杠杆臂,轴的位置也随 \(T\) 搬走了。只有"这个运动有多快、每转一弧度前进多少"这类与坐标系无关的量保持不变。
    4. 一个小的右扰动 \(T\,\Exp(\boldsymbol\delta_r)\) 等价于左扰动 \(\Exp(\boldsymbol\delta_l)\,T\),两者的关系是?
    \(T\Exp(\boldsymbol\delta_r) = (T\Exp(\boldsymbol\delta_r)T^{-1})T = \Exp(\operatorname{Ad}_T\boldsymbol\delta_r)T\)。选项 D 是反方向(左换右)。
    5. 传感器系 \(\{b\}\) 在世界系 \(\{s\}\) 里的位姿是 \(T\),世界系里的 wrench 是 \(\mathbf F_s\)。传感器应该读到什么?
    功率不变:\(\mathbf F_b\T\boldsymbol\xi_b = \mathbf F_s\T\boldsymbol\xi_s = \mathbf F_s\T\operatorname{Ad}_T\boldsymbol\xi_b\),所以 \(\mathbf F_b = \operatorname{Ad}_T\T\mathbf F_s\)。wrench 与 twist 对偶,换系用转置。选项 C 是 twist 的逆换算。

    8延伸阅读

    • K. Lynch & F. Park, Modern Robotics, Sec. 3.3.2 & 3.4 — 空间 twist 与机体 twist、伴随表示、wrench 的换系;本课的 (ω, v) 顺序与它一致
    • R. Murray, Z. Li, S. Sastry, A Mathematical Introduction to Robotic Manipulation, Ch. 2 — 刚体速度、伴随变换与螺旋理论的经典叙述
    • J. Solà, J. Deray, D. Atchuthan, "A micro Lie theory for state estimation in robotics", arXiv:1812.01537 — 伴随矩阵与左右扰动、协方差换系(平移在前)
    • T. Barfoot, State Estimation for Robotics, Ch. 7 — SE(3) 的伴随以及不确定性的变换