机器人学导论 一、空间变换（1）位姿，变换

前言

由于视觉伺服与机械臂关系紧密，因此还是从基础开始，把机器人运动学记录一下。

本篇记录刚体的位姿。实际上，空间变换在SLAM专栏里已经讲过一次了，不过机器人学导论给出了更详细的刚体运动说明。

坐标系

通常有两个坐标系，一个是用于参考的世界坐标系（笛卡尔坐标系），一个是以刚体质心为原点的刚体坐标系。

机器人坐标系遵循右手定则，如下图所示

位姿

位姿就是位置与姿态，对应位移与旋转。机器人的位姿必须以世界坐标系作为参照。

位姿表示的是参考系之间的关系。

位置

刚体的位置由矢量表示。 上图中有一个点 P P P，以坐标系 { A } \{A\} {A}为参考，就可以将矢量 O P OP OP来表示 A P ^AP AP的位置，其左上标表示参考系。 A P ^AP AP的坐标表示为： A P = [ p x , p y , p z ] T ^AP = [p_x,p_y,p_z]^T AP=[px​,py​,pz​]T

姿态

刚体的姿态由刚体坐标系在参考系中三个主轴的单位矢量表示。

上图中， X ^ B , Y ^ B , Z ^ B \hat X_B,\hat Y_B, \hat Z_B X^B​,Y^B​,Z^B​表示右上角刚体的三个主轴方向的单位矢量，它们在参考系 { A } \{A\} {A}中的表示为 A X ^ B , A Y ^ B , A Z ^ B ^A\hat X_B, ^A\hat Y_B, ^A\hat Z_B AX^B​,AY^B​,AZ^B​。这三个单位矢量就组成了旋转矩阵： B A R = ( A X ^ B A Y ^ B A Z ^ B ) ^A_BR=(^A\hat X_B \quad ^A\hat Y_B \quad ^A\hat Z_B) BA​R=(AX^B​AY^B​AZ^B​)

那么如何计算刚体的主轴单位矢量在参考系中的坐标呢？

以 A X ^ B ^A\hat X_B AX^B​为例，其x坐标的值实际上就是 A X ^ B ^A\hat X_B AX^B​在参考系的X轴上的投影，y, z坐标就是在参考系的Y, Z轴上的投影： B A R = ( A X ^ B A Y ^ B A Z ^ B ) = [ X ^ B ⋅ X ^ A Y ^ B ⋅ X ^ A Z ^ B ⋅ X ^ A X ^ B ⋅ Y ^ A Y ^ B ⋅ Y ^ A Z ^ B ⋅ Y ^ A X ^ B ⋅ Z ^ A Y ^ B ⋅ Y ^ A Z ^ B ⋅ X ^ A ] ^A_BR=(^A\hat X_B \quad ^A\hat Y_B \quad ^A\hat Z_B) = \begin{bmatrix} \hat X_B \cdot \hat X_A & \hat Y_B \cdot \hat X_A & \hat Z_B \cdot \hat X_A\\ \hat X_B \cdot \hat Y_A & \hat Y_B \cdot \hat Y_A & \hat Z_B \cdot \hat Y_A\\ \hat X_B \cdot \hat Z_A & \hat Y_B \cdot \hat Y_A & \hat Z_B \cdot \hat X_A\\ \end{bmatrix} BA​R=(AX^B​AY^B​AZ^B​)=⎣⎡​X^B​⋅X^A​X^B​⋅Y^A​X^B​⋅Z^A​​Y^B​⋅X^A​Y^B​⋅Y^A​Y^B​⋅Y^A​​Z^B​⋅X^A​Z^B​⋅Y^A​Z^B​⋅X^A​​⎦⎤​

旋转矩阵是个正交矩阵，证明也很简单： B A R T   B A R = [ A X ^ B T A Y ^ B T A Z ^ B T ] ( A X ^ B A Y ^ B A Z ^ B ) = I 3 ^A_BR^T\ ^A_B R= \begin{bmatrix} ^A\hat X^T_B \\ ^A\hat Y^T_B \\ ^A\hat Z^T_B \end{bmatrix} (^A\hat X_B \quad ^A\hat Y_B \quad ^A\hat Z_B) = I_3 BA​RT BA​R=⎣⎡​AX^BT​AY^BT​AZ^BT​​⎦⎤​(AX^B​AY^B​AZ^B​)=I3​ 同方向的单位矢量的内积为1，相互垂直的单位矢量内积为0，因此 A X ^ B T   A X ^ B = 1 , A X ^ B T   A Y ^ B = 0 ^A\hat X^T_B \ ^A\hat X_B=1, ^A\hat X^T_B \ ^A\hat Y_B=0 AX^BT​ AX^B​=1,AX^BT​ AY^B​=0，所以能得到上面的单位矩阵。

位姿变换

位姿变换指刚体坐标系的位置和姿态发生变化，即平移和旋转，得到刚体系与参考系之间的相对位姿

仍然以图2-2为例，刚体坐标系相对于参考系的位姿变换可表示为： { B A R ,   A P B O R G } \{^A_B R, \ ^AP_{BORG} \} {BA​R, APBORG​} 也就是SLAM十四讲中的 R , t R,t R,t

映射

映射表示的是同一点在不同坐标系之间的坐标转换。

映射包括平移和旋转。

平移

平移与刚体坐标系原点的位置相关。 上图中，已知 B P ^BP BP表示点P在坐标系 { B } \{B\} {B}中的位置， A P B O R G ^AP_{BORG} APBORG​表示在参考系 { A } \{A\} {A}下 { B } \{B\} {B}原点的位置。则 A P ^AP AP可表示为： A P =   B P +   A P B O R G ^AP= \ ^BP + \ ^AP_{BORG} AP= BP+ APBORG​

只有在两个坐标系的姿态相同时，才能进行上面的平移。

旋转

旋转与刚体坐标系的姿态相关。

上图中，已知 B P ^BP BP表示点P在坐标系 { B } \{B\} {B}中的位置。 A P ^AP AP表示在参考系 { A } \{A\} {A}下的位置，即矢量 A P ^AP AP在参考系主轴方向 X ^ A , Y ^ A , Z ^ A \hat X_A, \hat Y_A, \hat Z_A X^A​,Y^A​,Z^A​的投影（单位矢量内积）。此外，我们知道，向量内积需要在同一坐标系下表示才有意义。则 A P ^AP AP可表示为： A P = [ B X ^ A ⋅   B P B Y ^ A ⋅   B P B Z ^ A ⋅   B P ] =   B A R   B P ^AP= \begin{bmatrix} ^B \hat X_A \cdot\ ^BP & ^B \hat Y_A \cdot\ ^BP & ^B \hat Z_A \cdot\ ^BP \end{bmatrix} = \ ^A_BR \ ^BP AP=[BX^A​⋅ BP​BY^A​⋅ BP​BZ^A​⋅ BP​]= BA​R BP

变换

变换=先旋转，后平移。

上图中，需要使用 B P ^BP BP表示 A P ^AP AP。首先将 B P ^BP BP映射到一个中间坐标系 { B ′ } \{B'\} {B′}，该坐标系姿态与 { A } \{A\} {A}相同，原点与 { B } \{B\} {B}相同。然后再用 B ′ P ^{B'}P B′P表示 A P ^AP AP： A P =   B ′ P +   A P B ′ O R G =   B B ′ R   B P +   A P B ′ O R G =   B A R   B P +   A P B O R G ^AP = \ ^{B'}P + \ ^AP_{B'ORG} = \ ^{B'}_BR \ ^BP + \ ^AP_{B'ORG} = \ ^{A}_BR \ ^BP + \ ^AP_{BORG} AP= B′P+ APB′ORG​= BB′​R BP+ APB′ORG​= BA​R BP+ APBORG​ 也就对应SLAM十四讲中的 P ′ = R P + t P'=RP+t P′=RP+t

引入齐次矩阵，就有了变换矩阵的概念： [ A P 1 ] = [   B A R A P B O R G 0 1 ] [   B P 1 ] A P =   B A T   B P \begin{bmatrix} ^AP \\ 1 \\ \end{bmatrix} = \begin{bmatrix} \ ^A_BR & ^AP_{BORG} \\ {\bf 0} & 1 \\ \end{bmatrix} \begin{bmatrix} \ ^BP \\ 1 \\ \end{bmatrix} \\ \quad \\ \\ \quad \\ ^AP = \ ^A_BT \ ^BP [AP1​]=[ BA​R0​APBORG​1​][ BP1​]AP= BA​T BP

复合变换

如果已知 B A T ,   C B T ^A_B T, \ ^B_CT BA​T, CB​T，则可以通过 C P ^CP CP表示 A P ^AP AP： A P = B A T   C B T   C P ^AP = ^A_BT \ ^B_CT \ ^CP AP=BA​T CB​T CP

逆变换

旋转矩阵的逆： B A R =   A B R T ^A_B R = \ ^B_AR^T BA​R= AB​RT

变换矩阵的逆： [   B A R T − B A R T   A P B O R G 0 1 ] \begin{bmatrix} \ ^A_BR^T & -^A_BR^T\ ^AP_{BORG} \\ {\bf 0} & 1 \\ \end{bmatrix} [ BA​RT0​−BA​RT APBORG​1​] 变换矩阵的逆有两种求法，一种是直接根据矩阵求逆；另一种是通过变换性质求逆。介绍一下后一种。

如果已知 A P ^AP AP求 B P ^BP BP，有： B P =   A B R   A P +   B P A O R G ^BP = \ ^B_AR \ ^AP + \ ^BP_{AORG} BP= AB​R AP+ BPAORG​ 如果 A P ^AP AP是 { B } \{B\} {B}的原点，即： 0 = B P B O R G = A B R   A P B O R G +   B P A O R G   B P A O R G = − A B R   A P B O R G = − B A R T   A P B O R G   B P =   B A R T   A P −   B A R T   A P B O R G {\bf 0} = ^BP_{BORG} = ^B_AR \ ^AP_{BORG} + \ ^BP_{AORG} \\ \quad \\ \ ^BP_{AORG} = - ^B_AR \ ^AP_{BORG} = -^A_BR^T \ ^AP_{BORG} \\ \quad \\ \ ^BP = \ ^A_BR^T \ ^AP - \ ^A_BR^T \ ^AP_{BORG} 0=BPBORG​=AB​R APBORG​+ BPAORG​ BPAORG​=−AB​R APBORG​=−BA​RT APBORG​ BP= BA​RT AP− BA​RT APBORG​

旋转矩阵，变换矩阵的意义 旋转矩阵的意义

1. 描述一个坐标系相对参考坐标系的姿态
2. 将点从一个坐标系转换到参考坐标系
3. 将向量在一个坐标系中进行旋转

变换矩阵的意义

1. 描述一个坐标系相对参考坐标系的位姿
2. 将点从一个坐标系转换到参考坐标系
3. 将向量在一个坐标系中先旋转，后平移

后记

本篇实际上是SLAM十四讲中位姿变换的更清晰的表述方法，参考系放左上标。