机器人学导论 一、空间变换（1）位姿，变换
机器人学导论 一、空间变换（1）位姿前言坐标系位姿位置姿态前言由于视觉伺服与机械臂关系紧密，因此还是从基础开始，把机器人运动学记录一下。本篇记录刚体的位姿。实际上，空间变换在SLAM专栏里已经讲过一次了，不过机器人学导论给出了更详细的刚体运动说明。坐标系通常有两个坐标系，一个是用于参考的世界坐标系（笛卡尔坐标系），一个是以刚体质心为原点的刚体坐标系。机器人坐标系遵循右手定则，如下图所示位姿位姿就是
前言
由于视觉伺服与机械臂关系紧密，因此还是从基础开始，把机器人运动学记录一下。
本篇记录刚体的位姿。实际上，空间变换在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十四讲中位姿变换的更清晰的表述方法，参考系放左上标。
CSDN联合极客时间，共同打造面向开发者的精品内容学习社区，助力成长！
更多推荐
6
0- 0
已为社区贡献3条内容
所有评论(0)