三维空间刚体变换:旋转矩阵

点与坐标系

向量与坐标

空间中最基本的元素是点,点没有长度、体积。将两个点连接得到向量,向量表示由某点指向另一个点的有向线段。

对于一空间基底为 [ i j k ] \begin{bmatrix}i&j&k\end{bmatrix} [ijk]的坐标系,向量 a a a的坐标如下:
a = [ i j k ] ⋅ [ a 1 a 2 a 3 ] = a 1 i + a 2 j + a 3 k a=\begin{bmatrix}i&j&k\end{bmatrix}\cdot\begin{bmatrix}a_1\\a_2\\a_3\end{bmatrix}=a_1i+a_2j+a_3k a=[ijk]a1a2a3=a1i+a2j+a3k
[ a 1 a 2 a 3 ] T \begin{bmatrix}a_1&a_2&a_3\end{bmatrix}^T [a1a2a3]T为向量 a a a在此基底下的坐标,坐标的具体取值同向量本身以及坐标系的选取有关。

坐标系可根据由左手法制或右手法制得到而分左手系和右手系:

在这里插入图片描述

向量的内积用于描述向量间的投影关系:
a ⋅ b = a T b = ∑ i = 1 3 a i b i = ∣ a ∣ ∣ b ∣ cos ⁡ ⟨ a , b ⟩ a\cdot b=a^Tb=\sum^3_{i=1}a_ib_i=\vert a\vert\vert b\vert\cos\langle a,b\rangle ab=aTb=i=13aibi=abcosa,b
式中, ⟨ a , b ⟩ \langle a,b\rangle a,b为向量 a , b a,b a,b间的夹角。

向量的外积为一个向量,其大小等于 ∣ a ∣ ∣ b ∣ sin ⁡ ⟨ a , b ⟩ \vert a\vert\vert b\vert\sin\langle a,b\rangle absina,b,方向垂直于这两个向量:
a × b = ∥ i j k a 1 a 2 a 3 b 1 b 2 b 3 ∥ = [ a 2 b 3 − a 3 b 2 a 3 b 1 − a 1 b 3 a 1 b 2 − a 2 b 1 ] = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] b = a ∧ b a\times b=\begin{Vmatrix}i&j&k\\a_1&a_2&a_3\\b_1&b_2&b_3\end{Vmatrix}=\begin{bmatrix}a_2b_3-a_3b_2\\a_3b_1-a_1b_3\\a_1b_2-a_2b_1\end{bmatrix}=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix}b=a^\wedge b a×b=ia1b1ja2b2ka3b3=a2b3a3b2a3b1a1b3a1b2a2b1=0a3a2a30a1a2a10b=ab
引入反对称矩阵,并用 a ∧ a^\wedge a表示向量 a a a的反对称矩阵。任何向量仅有唯一一个反对称矩阵:
a ∧ = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] a^\wedge=\begin{bmatrix}0&-a_3&a_2\\a_3&0&-a_1\\-a_2&a_1&0\end{bmatrix} a=0a3a2a30a1a2a10

坐标系与欧式变换

在机器人中,定义世界坐标系固定不动,而机器人坐标系跟随着机器人一起移动。同时对每个传感器(IMU、激光雷达、摄像头等)建立传感器坐标系。

设世界坐标系内某点坐标为 W p {}^Wp Wp,上标 W ^W W表示为参考坐标系为世界坐标系 { W } \{W\} {W}
W p = [ p x p y p z ] ^Wp=\begin{bmatrix}p_x\\p_y\\p_z\end{bmatrix} Wp=pxpypz
由一个旋转加上一个平移得到的运动称为刚体运动。机器人相对于世界的移动就是一个刚体运动,两坐标系间相差一个欧式变换,可通过变换矩阵 T T T得到。

在这里插入图片描述

则向量 p p p相对于机器人坐标系的坐标可通过坐标 W p ^Wp Wp与变换矩阵 T T T得到。

旋转矩阵与旋转群

欧式变换由一个旋转和一个平移得到,首先仅考虑旋转运动。

旋转矩阵

设坐标系 { A } \{A\} {A}和坐标系 { B } \{B\} {B},已知坐标系 { B } \{B\} {B}的基底为 [ x C y C z C ] T \begin{bmatrix}x_C&y_C&z_C\end{bmatrix}^T [xCyCzC]T,则可用其相对于 { A } \{A\} {A}的方向余弦组成的矩阵可表示坐标系 { B } \{B\} {B}相对 { A } \{A\} {A}的方位:
B A R = [ A x B A y B A z B ] B A R = [ r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ] \begin{aligned} ^A_BR=&\begin{bmatrix}{}^Ax_B&{}^Ay_B&{}^Az_B\end{bmatrix}\\ ^A_BR=&\begin{bmatrix}r_{11}&r_{12}&r_{13}\\r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\\\end{bmatrix} \end{aligned} BAR=BAR=[AxBAyBAzB]r11r21r31r12r22r32r13r23r33
C W R ^W_CR CWR为坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的旋转矩阵。

旋转矩阵的九个元素中仅有三个为独立元素,这是因为基底的三分列向量相互垂直,且长度为1:
A x B ⋅ A y B = W y B ⋅ A z B = A z B ⋅ A x B = 0 A x B ⋅ A x B = W y B ⋅ A y B = A z B ⋅ A z B = 1 \begin{aligned} ^Ax_B\cdot{}^Ay_B=^Wy_B\cdot{}^Az_B=^Az_B\cdot{}^Ax_B=0\\ ^Ax_B\cdot{}^Ax_B=^Wy_B\cdot{}^Ay_B=^Az_B\cdot{}^Az_B=1 \end{aligned} AxBAyB=WyBAzB=AzBAxB=0AxBAxB=WyBAyB=AzBAzB=1
由此可得旋转矩阵为正交矩阵 ,满足如下条件:
{ B A R − 1 = B A R T d e t ( B A R ) = 1 \begin{cases} {}^A_BR^{-1}={}^A_BR^T\\ det({}^A_BR)=1 \end{cases} {BAR1=BARTdet(BAR)=1

单位旋转

对于一个坐标系绕自身的x轴、y轴、z轴旋转 θ \theta θ角的旋转矩阵如下:
R ( x , θ ) = [ 1 0 0 0 cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ ] R ( y , θ ) = [ cos ⁡ θ 0 sin ⁡ θ 0 1 0 − sin ⁡ θ 0 cos ⁡ θ ] R ( z , θ ) = [ cos ⁡ θ − sin ⁡ θ 0 sin ⁡ θ cos ⁡ θ 0 0 0 1 ] \begin{aligned} R(x,\theta)=&\begin{bmatrix}1&0&0\\0&\cos\theta&-\sin\theta\\0&\sin\theta&\cos\theta\end{bmatrix}\\ R(y,\theta)=&\begin{bmatrix}\cos\theta&0&\sin\theta\\0&1&0\\-\sin\theta&0&\cos\theta\end{bmatrix}\\ R(z,\theta)=&\begin{bmatrix}\cos\theta&-\sin\theta&0\\\sin\theta&\cos\theta&0\\0&0&1\end{bmatrix}\\ \end{aligned} R(x,θ)=R(y,θ)=R(z,θ)=1000cosθsinθ0sinθcosθcosθ0sinθ010sinθ0cosθcosθsinθ0sinθcosθ0001

旋转群

旋转矩阵 R ∈ ℜ 3 × 3 R\in\Re^{3\times3} R3×3满足如下约束条件:

  • 正交条件 :旋转矩阵的逆等于它的转置, R − 1 = R T R^{-1}=R^T R1=RT
  • 特殊条件: 旋转矩阵的行列式为一, d e t ( R ) = 1 det(R)=1 det(R)=1

将满足上述条件的旋转矩阵 R ∈ ℜ 3 × 3 R\in\Re^{3\times3} R3×3的集合定义为旋转群 S O ( 3 ) SO(3) SO(3)
S O ( 3 ) = { R ∈ ℜ 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO(3)=\bigl\{R\in\Re^{3\times3}\big\vert RR^T=I,det(R)=1\bigr\} SO(3)={R3×3RRT=I,det(R)=1}
旋转群又叫特殊正交群(Special Orthogonal Group),可推广至 ℜ n × n \Re^{n\times n} n×n空间中:
S O ( n ) = { R ∈ ℜ n × n ∣ R R T = I , d e t ( R ) = 1 } SO(n)=\bigl\{R\in\Re^{n\times n}\big\vert RR^T=I,det(R)=1\bigr\} SO(n)={Rn×nRRT=I,det(R)=1}
其维数为 n ( n − 1 ) 2 \frac{n(n-1)}{2} 2n(n1)的流形,当 n = 2 n=2 n=2时用于表示平面运动;当 n = 3 n=3 n=3时用于表示空间转动。旋转群是李群,满足李群条件。

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值