点与坐标系

向量与坐标

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

对于一空间基底为 [ 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 a⋅b=aTb=i=1∑3aibi=∣a∣∣b∣cos⟨a,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 ∣a∣∣b∣sin⟨a,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∥∥∥∥∥∥=⎣⎡a2b3−a3b2a3b1−a1b3a1b2−a2b1⎦⎤=⎣⎡0a3−a2−a30a1a2−a10⎦⎤b=a∧b
引入反对称矩阵，并用 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∧=⎣⎡0a3−a2−a30a1a2−a10⎦⎤

坐标系与欧式变换

在机器人中，定义世界坐标系固定不动，而机器人坐标系跟随着机器人一起移动。同时对每个传感器（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} AxB⋅AyB=WyB⋅AzB=AzB⋅AxB=0AxB⋅AxB=WyB⋅AyB=AzB⋅AzB=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} {BAR−1=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θ0−sinθcosθ⎦⎤⎣⎡cosθ0−sinθ010sinθ0cosθ⎦⎤⎣⎡cosθsinθ0−sinθcosθ0001⎦⎤

旋转群

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

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

将满足上述条件的旋转矩阵 R ∈ ℜ 3 × 3 R\in\Re^{3\times3} R∈ℜ3×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)={R∈ℜ3×3∣∣RRT=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)={R∈ℜn×n∣∣RRT=I,det(R)=1}
其维数为 n ( n − 1 ) 2 \frac{n(n-1)}{2} 2n(n−1)的流形，当 n = 2 n=2 n=2时用于表示平面运动；当 n = 3 n=3 n=3时用于表示空间转动。旋转群是李群，满足李群条件。

三维空间刚体变换：旋转矩阵相关推荐

三维空间刚体变换：变换矩阵
变换矩阵坐标系描述用 A p B o ^Ap_{Bo} ApBo表示在参考坐标系 { A } \{A\} {A}下坐标系 { B } \{B\} {B}的原点坐标,并用旋转矩阵 B A R ^A ...
三维空间坐标系变换-旋转矩阵
空间中三维坐标变换一般由三种方式实现,第一种是旋转矩阵和旋转向量:第二种是欧拉角:第三种是四元数.这里先介绍旋转矩阵(旋转向量)与欧拉角实现三维空间坐标变换的方法以及两者之间的关系. 这里以常见的世界 ...
视觉SLAM——二维三维几何、三维空间刚体变换
前言本博客为主要学习<视觉SLAM十四讲>第3讲.<机器人学的状态估计>第6章三位几何学基础.<计算机视觉-算法和应用>第2章2.1几何基元变换等SLAM内容的总 ...
SLAM学习：三维空间刚体变换（1）
1. 欧式变换相机运动是一个刚体运动,它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化.这种变换称为欧氏变换. 一个欧式变换由一个旋转和一个平移两部分组成:其中,旋转矩阵表征绕各个坐标轴所 ...
高博14讲--第三讲三维空间刚体运动
高博14讲--第三讲三维空间刚体运动旋转矩阵点和向量.坐标系坐标系间的欧式变换变换矩阵与齐次坐标旋转向量和欧拉角旋转向量欧拉角四元数四元数的定义四元数的运算用四元数表示旋转四 ...
三维空间刚体运动1：旋转矩阵与变换矩阵（详解加代码示例）
三维空间刚体运动1:旋转矩阵与变换矩阵(详解加代码示例) 1. 点.向量和坐标系 2.坐标系间的欧式变换 2.1 旋转 2.2 平移 3.齐次坐标和变换矩阵 4. 相似.仿射和射影变换 4.1 相似变 ...
三维空间变换：刚体变换、仿射变换、线性变换、旋转变换
文章目录 1 刚体变换.仿射变换.线性变换的概念 2 旋转变换 2.1 坐标系旋转(逆时针) 2.2 复合旋转 2.3 向量旋转 1 刚体变换.仿射变换.线性变换的概念刚体变换是指在三维空间中,把一 ...
从零开始一起学习SLAM | 三维空间刚体的旋转
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达刚体,顾名思义,是指本身不会在运动过程中产生形变的物体,如相机的运 ...
三维空间刚体运动4-5：四元数多点离散数值解插值方法：Sping
三维空间刚体运动4-5:四元数多点离散数值解插值方法:Sping 1. 正切曲率κ(γ,t)\kappa(\gamma, t)κ(γ,t)在H1H_{1}H1上的离散数值解--Sping 1.1 离 ...

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