机器人学的数理基础知识
目录位置和姿态的表示位置描述方位描述旋转矩阵位姿描述坐标变换平移坐标变换旋转坐标变换齐次坐标变换齐次变换平移齐次坐标变换旋转齐次坐标变换物体的变换及逆变换齐次变换的逆变换齐次变换的求逆问题通用旋转变换通用旋转变换公式等效转角与转轴位置和姿态的表示位置描述在指定坐标系下，空间中任意一点可有3×13 \times 13×1位置矢量来确定Ap=[pxpypz]^Ap =\begin{bmatrix}p_
目录
位置和姿态的表示
位置描述
在指定坐标系下，空间中任意一点可有
3
×
1
3 \times 1
3×1位置矢量来确定
A
p
=
[
p
x
p
y
p
z
]
^Ap = \begin{bmatrix} p_x\\ p_y\\ p_z \end{bmatrix}
Ap=⎣
⎡pxpypz⎦
⎤
其中
p
x
,
p
y
,
p
z
p_x, p_y, p_z
px,py,pz是点 P 在坐标系
{
A
}
\{A\}
{A}中的三个坐标分量，上标 A 表示参考坐标系
{
A
}
\{A\}
{A}。
我们称 A p ^Ap Ap为位置向量。
方位描述
旋转矩阵
坐标系 B 相对于坐标系 A 的方位，可以由坐标系 B 的三个主矢量相对于坐标系 A 的三个方向余弦组成的 3 × 3 3\times3 3×3矩阵来表示：
B
A
R
=
[
A
x
B
A
y
B
A
z
B
]
^A_BR= \begin{bmatrix} ^Ax_B & ^Ay_B & ^Az_B \end{bmatrix}
BAR=[AxBAyBAzB]
B
A
R
=
[
r
11
r
12
r
13
r
21
r
22
r
23
r
31
r
32
r
33
]
^A_BR= \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ r_{21} & r_{22} & r_{23}\\ r_{31} & r_{32} & r_{33} \end{bmatrix}
BAR=⎣
⎡r11r21r31r12r22r32r13r23r33⎦
⎤
B A R ^A_BR BAR称为旋转矩阵
对应三个坐标轴
x
,
y
,
z
x,y,z
x,y,z作转角位
θ
\theta
θ的旋转变换，其旋转矩阵分别为
R
(
x
,
θ
)
=
[
1
0
0
0
c
θ
−
s
θ
0
s
θ
c
θ
]
R(x,\theta)= \begin{bmatrix} 1 & 0 & 0\\ 0 & c\theta & -s\theta\\ 0 & s\theta & c\theta \end{bmatrix}
R(x,θ)=⎣
⎡1000cθsθ0−sθcθ⎦
⎤
R ( y , θ ) = [ c θ 0 s θ 0 1 0 − s θ 0 c θ ] R(y,\theta)= \begin{bmatrix} c\theta & 0 & s\theta\\ 0 & 1 & 0\\ -s\theta & 0 & c\theta \end{bmatrix} R(y,θ)=⎣ ⎡cθ0−sθ010sθ0cθ⎦ ⎤
R ( z , θ ) = [ c θ − s θ 0 s θ c θ 0 0 0 1 ] R(z,\theta)= \begin{bmatrix} c\theta & -s\theta & 0\\ s\theta & c\theta & 0\\ 0 & 0 & 1 \end{bmatrix} R(z,θ)=⎣ ⎡cθsθ0−sθcθ0001⎦ ⎤
位姿描述
刚体 B 的位姿可以由坐标系
{
B
}
\{B\}
{B}来描述，即
KaTeX parse error: Got group of unknown type: 'internal'
坐标变换
平移坐标变换
平移方程
A
p
=
B
p
+
A
p
B
0
^Ap= ^Bp + ^Ap_{B0}
Ap=Bp+ApB0
其中
- A p ^Ap Ap表示任意点 p 在坐标系 A 中的位置矢量
- B p ^Bp Bp表示点 p 在坐标系 B 中的位置矢量
- A p B 0 ^Ap_{B0} ApB0表示坐标系 B 的原点在坐标系 A 中的位置矢量
旋转坐标变换
坐标旋转方程
A
p
=
B
A
R
B
p
^Ap=^A_BR ^Bp
Ap=BARBp
其中
- A p ^Ap Ap表示任意点 p 在坐标系 A 中的位置矢量
- B p ^Bp Bp表示点 p 在坐标系 B 中的位置矢量
- B A R ^A_BR BAR表示描述坐标系 B 相对于坐标系 A 方位的旋转矩阵
齐次坐标变换
齐次变换
关于齐次坐标的意义：
这样，对于上述讨论的旋转和平移变换公式就可以进行统一，即 [ A p 1 ] \begin{bmatrix} ^Ap \\ 1 \end{bmatrix} [Ap1] = = = [ B A R A p B 0 0 1 ] \begin{bmatrix} ^A_BR & ^Ap_{B0} \\ 0 & 1 \end{bmatrix} [BAR0ApB01] [ B p 1 ] \begin{bmatrix} ^Bp\\ 1 \end{bmatrix} [Bp1]
其中，
4
×
1
4\times1
4×1的列向量表示三维空间的点，上式的矩阵形式为
A
p
=
B
A
T
B
p
^Ap= ^A_BT^Bp
Ap=BATBp
其中
4
×
4
4\times4
4×4的矩阵
B
A
T
^A_BT
BAT称为齐次变换矩阵，具有如下形式
B
A
T
=
[
B
A
R
A
p
B
0
0
1
]
^A_BT= \begin{bmatrix} ^A_BR & ^Ap_{B0}\\ 0 & 1 \end{bmatrix}
BAT=[BAR0ApB01]
平移齐次坐标变换
平移齐次变换
T
r
a
n
s
(
a
,
b
,
c
)
=
[
1
0
0
a
0
1
0
b
0
0
1
c
0
0
0
1
]
Trans(a, b, c)= \begin{bmatrix} 1 & 0 & 0 & a\\ 0 & 1 & 0 & b\\ 0 & 0 & 1 & c\\ 0 & 0 & 0 & 1 \end{bmatrix}
Trans(a,b,c)=⎣
⎡100001000010abc1⎦
⎤
对已知矢量
u
=
[
x
y
z
w
]
T
\displaystyle u=[x~y~ z ~w]^\mathrm{T}
u=[x y z w]T
进行平移变换得到的矢量为：
v
=
[
1
0
0
a
0
1
0
b
0
0
1
c
0
0
0
1
]
v = \begin{bmatrix} 1 & 0 & 0 & a \\ 0 & 1 & 0 & b \\ 0 & 0 & 1 & c \\ 0 & 0 & 0 & 1 \end{bmatrix}
v=⎣
⎡100001000010abc1⎦
⎤
[
x
y
z
w
]
\begin{bmatrix} x \\ y \\ z \\ w \end{bmatrix}
⎣
⎡xyzw⎦
⎤
=
=
=
[
x
+
a
w
y
+
b
w
z
+
c
w
w
]
\begin{bmatrix} x+aw \\ y+bw \\ z+cw \\ w \end{bmatrix}
⎣
⎡x+awy+bwz+cww⎦
⎤
=
=
=
[
x
/
w
+
a
y
/
w
+
b
z
/
w
+
c
1
]
\begin{bmatrix} x/w+a\\ y/w+b\\ z/w+c\\ 1 \end{bmatrix}
⎣
⎡x/w+ay/w+bz/w+c1⎦
⎤
可以看成是矢量
[
x
/
w
y
/
w
z
/
w
]
T
[x/w~~y/w~~z/w]^\mathrm{T}
[x/w y/w z/w]T与矢量
[
a
b
c
]
T
[a ~b~ c]^\mathrm{T}
[a b c]T之和
旋转齐次坐标变换
R
o
t
(
x
,
θ
)
=
[
1
0
0
0
0
c
θ
−
s
θ
0
0
s
θ
c
θ
0
0
0
0
1
]
Rot(x, \theta)= \begin{bmatrix} 1 & 0 & 0 & 0\\ 0 & c\theta & -s\theta & 0\\ 0 & s\theta & c\theta & 0\\ 0 & 0 & 0 &1 \end{bmatrix}
Rot(x,θ)=⎣
⎡10000cθsθ00−sθcθ00001⎦
⎤
R
o
t
(
y
,
θ
)
=
[
c
θ
0
s
θ
0
0
1
0
0
−
s
θ
0
c
θ
0
0
0
0
1
]
Rot(y, \theta)= \begin{bmatrix} c\theta & 0 & s\theta & 0\\ 0 & 1 & 0 & 0\\ -s\theta & 0 & c\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}
Rot(y,θ)=⎣
⎡cθ0−sθ00100sθ0cθ00001⎦
⎤
R
o
t
(
z
,
θ
)
=
[
c
θ
−
s
θ
0
0
s
θ
c
θ
0
0
0
0
1
0
0
0
0
1
]
Rot(z,\theta)= \begin{bmatrix} c\theta & -s\theta & 0 & 0\\ s\theta & c\theta & 0 & 0\\ 0 & 0 & 1 & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}
Rot(z,θ)=⎣
⎡cθsθ00−sθcθ0000100001⎦
⎤
物体的变换及逆变换
齐次变换的逆变换
复合变换：
C
A
T
=
B
A
T
C
B
T
^A_CT = ^A_BT ^B_CT
CAT=BATCBT
齐次变换的求逆问题
从坐标系
{
B
}
\{B\}
{B}相对于坐标系
{
A
}
\{A\}
{A}的描述
B
A
T
^A_BT
BAT，
求得坐标系
{
A
}
\{A\}
{A}相对于
{
B
}
\{B\}
{B}的描述
A
B
T
^B_AT
ABT 。
- 直接对 4 × 4 4\times4 4×4齐次变换矩阵求逆
此问题可以等价于给定 B A R ^A_BR BAR和 A p B 0 ^Ap_{B0} ApB0，计算 A B R ^B_AR ABR和 B p A 0 ^Bp_{A0} BpA0
坐标系
{
B
}
\{B\}
{B}的原点，可以表示成
A
p
B
0
^Ap_{B0}
ApB0 的形式，该点在坐标系
{
B
}
\{B\}
{B}中的描述是
B
(
A
p
B
0
)
=
(
A
B
R
)
(
A
p
B
0
)
+
B
p
A
0
^B(^Ap_{B0}) = (^B_AR)(^Ap_{B0}) + ^Bp_{A0}
B(ApB0)=(ABR)(ApB0)+BpA0
而上式中的第一项我们知道，坐标系 B 的原点在坐标系 B 中的描述为 0，故
B
p
A
0
=
(
−
A
B
R
)
(
A
p
B
0
)
^Bp_{A0} = (-^B_AR)(^Ap_{B0})
BpA0=(−ABR)(ApB0)
A
B
T
=
[
B
A
R
T
(
−
B
A
R
T
)
(
A
p
B
0
)
0
1
]
^B_AT = \begin{bmatrix} ^A_BR^\mathrm{T} & (-^A_BR^\mathrm{T})(^Ap_{B0}) \\ 0 & 1 \end{bmatrix}
ABT=[BART0(−BART)(ApB0)1]
- 直接对 4 × 4 4 \times 4 4×4齐次变换矩阵的求逆方法
一般情况下，已知变换
T
T
T 的各元，
T
=
[
n
x
o
x
a
x
p
x
n
y
o
y
a
y
p
y
n
z
o
z
a
z
p
z
0
0
0
1
]
T = \begin{bmatrix} n_x & o_x & a_x & p_x\\ n_y & o_y & a_y & p_y\\ n_z & o_z & a_z & p_z\\ 0 & 0 & 0 & 1 \end{bmatrix}
T=⎣
⎡nxnynz0oxoyoz0axayaz0pxpypz1⎦
⎤
其逆变换为
T
−
1
=
[
n
x
n
y
n
z
−
p
⋅
n
o
x
o
y
o
z
−
p
⋅
o
a
x
a
y
a
z
−
p
⋅
a
0
0
0
1
]
T^{-1} = \begin{bmatrix} n_x & n_y & n_z & -\pmb{p} \cdot \pmb{n} \\ o_x & o_y & o_z & -\pmb{p} \cdot \pmb{o} \\ a_x & a_y & a_z & -\pmb{p} \cdot \pmb{a} \\ 0 & 0 & 0 & 1 \end{bmatrix}
T−1=⎣
⎡nxoxax0nyoyay0nzozaz0−pp⋅nn−pp⋅oo−pp⋅aa1⎦
⎤
通用旋转变换
我们在前面已经给出了，绕 x , y , z x, y, z x,y,z轴旋转的变换矩阵，现在讨论更一般的情况，即研究绕着从原点出发的任一矢量 f f f旋转 θ \theta θ 角时的旋转矩阵。
通用旋转变换公式
我们假设任意矢量 f f f是坐标系 { C } \{C\} {C}的 z z z轴上的单位矢量。
那么坐标系C就可以表示为：
C
=
[
n
x
o
x
a
x
0
n
y
o
y
a
y
0
n
z
o
z
a
z
0
0
0
0
1
]
C = \begin{bmatrix} n_x & o_x & a_x & 0\\ n_y & o_y & a_y & 0\\ n_z & o_z & a_z & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}
C=⎣
⎡nxnynz0oxoyoz0axayaz00001⎦
⎤
f
=
a
x
x
+
a
y
y
+
a
z
z
\pmb{f} = a_x \pmb{x} + a_y\pmb{y} + a_z\pmb{z}
ff=axxx+ayyy+azzz
那么，绕矢量
f
f
f旋转，就等价于绕坐标系
{
C
}
\{C\}
{C}的
z
z
z轴旋转。
即
R
o
t
(
f
,
θ
)
=
R
o
t
(
C
z
,
θ
)
Rot(\pmb{f}, \theta) = Rot(C_z, \theta)
Rot(ff,θ)=Rot(Cz,θ)
接下来，如果已知一个以参考坐标系描述的坐标系 { T } \{T\} {T}，这个坐标系在坐标系 { C } \{C\} {C}下的描述为坐标系 { S } \{S\} {S}，那么有如下关系：
T = C S T = CS T=CS
那么：
S
=
C
−
1
T
S=C^{-1}T
S=C−1T
接下来，我们知道，坐标系 { T } \{T\} {T}和任意矢量 f \pmb{f} ff都是在参考坐标系下描述的，而坐标系 { S } \{S\} {S}是坐标系 { T } \{T\} {T}在坐标系 { C } \{C\} {C}中的描述，矢量 C z C_z Cz是矢量 f \pmb{f} ff在坐标系 { C } \{C\} {C}中的描述。
那么就存在等价关系：
T T T绕 f \pmb{f} ff旋转等价于 S S S绕坐标系 { C } \{C\} {C}的 z z z轴旋转。
即
R
o
t
(
f
,
θ
)
T
=
R
o
t
(
C
z
,
θ
)
S
Rot(\pmb{f}, \theta)T = Rot(C_z, \theta)S
Rot(ff,θ)T=Rot(Cz,θ)S
有：
R
o
t
(
f
,
θ
)
T
=
C
R
o
t
(
z
,
θ
)
S
Rot(\pmb{f}, \theta)T = CRot(z, \theta)S
Rot(ff,θ)T=CRot(z,θ)S
进一步：
R
o
t
(
f
,
θ
)
T
=
C
R
o
t
(
z
,
θ
)
C
−
1
T
Rot(\pmb{f}, \theta)T = CRot(z, \theta) C^{-1}T
Rot(ff,θ)T=CRot(z,θ)C−1T
进而：
R
o
t
(
f
,
θ
)
=
C
R
o
t
(
z
,
θ
)
C
−
1
Rot(\pmb{f}, \theta) = CRot(z, \theta) C^{-1}
Rot(ff,θ)=CRot(z,θ)C−1
最终：
R
o
t
(
f
,
θ
)
=
[
f
x
f
x
v
e
r
s
θ
+
c
θ
f
y
f
x
v
e
r
s
θ
−
f
z
s
θ
f
z
f
x
v
e
r
s
θ
+
f
y
s
θ
0
f
x
f
y
v
e
r
s
θ
+
f
z
s
θ
f
y
f
y
v
e
r
s
θ
+
c
θ
f
z
f
y
v
e
r
s
θ
−
f
x
s
θ
0
f
x
f
z
v
e
r
s
θ
−
f
y
s
θ
f
y
f
z
v
e
r
s
θ
+
f
x
s
θ
f
z
f
z
v
e
r
s
θ
+
c
θ
0
0
0
0
1
]
Rot(\pmb{f}, \theta) = \begin{bmatrix} f_x f_xvers\theta + c\theta & f_y f_xvers\theta - f_z s\theta & f_z f_xvers\theta + f_y s\theta & 0\\ f_x f_y vers\theta + f_z s \theta & f_y f_y vers \theta + c \theta & f_z f_y vers\theta -f_xs\theta & 0\\ f_x f_z vers\theta -f_ys\theta & f_y f_z vers\theta + f_x s\theta & f_z f_z vers\theta + c\theta & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}
Rot(ff,θ)=⎣
⎡fxfxversθ+cθfxfyversθ+fzsθfxfzversθ−fysθ0fyfxversθ−fzsθfyfyversθ+cθfyfzversθ+fxsθ0fzfxversθ+fysθfzfyversθ−fxsθfzfzversθ+cθ00001⎦
⎤
等效转角与转轴
由上述我们推得的公式，可以知道给出任意一个旋转变换矩阵，都可以找到一个等效的旋转 θ \theta θ角的转轴。
已知旋转变换：
R
=
[
n
x
o
x
a
x
0
n
y
o
y
a
y
0
n
z
o
z
a
z
0
0
0
0
1
]
R= \begin{bmatrix} n_x & o_x & a_x & 0\\ n_y & o_y & a_y & 0\\ n_z & o_z & a_z & 0\\ 0 & 0 & 0 & 1 \end{bmatrix}
R=⎣
⎡nxnynz0oxoyoz0axayaz00001⎦
⎤
令 R = R o t ( f , θ ) R = Rot(\pmb{f}, \theta) R=Rot(ff,θ)
可以确定：
t
a
n
θ
=
(
o
z
−
a
y
)
2
+
(
a
x
−
n
z
)
2
+
(
n
y
−
o
x
)
2
n
x
+
o
y
+
a
z
−
1
tan\theta = \frac{\sqrt{(o_z-a_y)^2+(a_x-n_z)^2+(n_y-o_x)^2}}{n_x+o_y+a_z-1}
tanθ=nx+oy+az−1(oz−ay)2+(ax−nz)2+(ny−ox)2
f x = ( o z − a y ) / 2 s θ f_x = (o_z - a_y)/2s\theta fx=(oz−ay)/2sθ
f y = ( a x − n z ) / 2 s θ f_y = (a_x - n_z)/2s\theta fy=(ax−nz)/2sθ
f z = ( n y − o x ) / 2 s θ f_z = (n_y - o_x)/2s\theta fz=(ny−ox)/2sθ
CSDN联合极客时间，共同打造面向开发者的精品内容学习社区，助力成长！
更多推荐
1
0- 0
已为社区贡献3条内容
所有评论(0)