机器人学导论 一、空间变换（1）位姿，变换
机器人学导论 一、空间变换（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) BAR=(AX^BAY^BAZ^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} BAR=(AX^BAY^BAZ^B)=⎣⎡X^B⋅X^AX^B⋅Y^AX^B⋅Z^AY^B⋅X^AY^B⋅Y^AY^B⋅Y^AZ^B⋅X^AZ^B⋅Y^AZ^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 BART BAR=⎣⎡AX^BTAY^BTAZ^BT⎦⎤(AX^BAY^BAZ^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} \} {BAR, 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⋅ BPBY^A⋅ BPBZ^A⋅ BP]= BAR 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= BAR 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]=[ BAR0APBORG1][ BP1]AP= BAT BP
复合变换
如果已知 B A T , C B T ^A_B T, \ ^B_CT BAT, CBT，则可以通过 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=BAT CBT CP
逆变换
旋转矩阵的逆：
B A R = A B R T ^A_B R = \ ^B_AR^T BAR= ABRT
变换矩阵的逆：
[ 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} [ BART0−BART APBORG1]
变换矩阵的逆有两种求法，一种是直接根据矩阵求逆；另一种是通过变换性质求逆。介绍一下后一种。
如果已知 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= ABR 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=ABR APBORG+ BPAORG BPAORG=−ABR APBORG=−BART APBORG BP= BART AP− BART APBORG
旋转矩阵，变换矩阵的意义
旋转矩阵的意义
- 描述一个坐标系相对参考坐标系的姿态
- 将点从一个坐标系转换到参考坐标系
- 将向量在一个坐标系中进行旋转
变换矩阵的意义
- 描述一个坐标系相对参考坐标系的位姿
- 将点从一个坐标系转换到参考坐标系
- 将向量在一个坐标系中先旋转，后平移
后记
本篇实际上是SLAM十四讲中位姿变换的更清晰的表述方法，参考系放左上标。