变换矩阵

坐标系描述

用 A p B o ^Ap_{Bo} ApBo表示在参考坐标系 { A } \{A\} {A}下坐标系 { B } \{B\} {B}的原点坐标，并用旋转矩阵 B A R ^A_BR BAR描述坐标系 { B } \{B\} {B}在参考坐标系 { A } \{A\} {A}下的姿态，则坐标系 { B } \{B\} {B}的姿态可表示为：
{ B } = { A p B o B A R } \{B\}=\{\enspace^Ap_{Bo}\quad{}^A_BR\enspace\} {B}={ApBoBAR}
坐标系的描述概括了刚体的位置和姿态的描述，当表示位置时，旋转矩阵 B A R = I {}^A_BR=I BAR=I；当表示姿态时，位移矢量 A p B o = 0 ^Ap_{Bo}=0 ApBo=0

平移映射

当两坐标系具有相同的方位，当坐标原点不重合时，可用位移矢量 A p B o ^Ap_{Bo} ApBo描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的位置：

称 A p B o ^Ap_{Bo} ApBo为坐标系 { B } \{B\} {B}相对于 { A } \{A\} {A}的平移矢量，若点 p p p在坐标系 { B } \{B\} {B}下的位置为 B p ^Bp Bp，则它相对于坐标系 { A } \{A\} {A}的位置 A p ^Ap Ap可表示为：
A p = B p + A p B o {}^Ap={}^Bp+{}^Ap_{Bo} Ap=Bp+ApBo

旋转映射

当两坐标系原点重合，但方位不同时，可用旋转矩阵 B A R {}^A_BR BAR描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的姿态：

称 B A R {}^A_BR BAR为坐标系 { B } \{B\} {B}相对于 { A } \{A\} {A}的方位，若点 p p p在坐标系 { B } \{B\} {B}下的位置为 B p ^Bp Bp，则它相对于坐标系 { A } \{A\} {A}的位置 A p ^Ap Ap可表示为：
A p = B A R B p {}^Ap={}^A_BR\:{}^Bp Ap=BARBp
同样，用 A B R {}^B_AR ABR表示坐标系 { A } \{A\} {A}相对于 { B } \{B\} {B}的方位，则可由旋转矩阵的正交约束得：
A B R = B A R − 1 = B A R T {}^B_AR={}^A_BR^{-1}={}^A_BR^T ABR=BAR−1=BART

一般刚体变换

在三维空间中，最常见的情况如下：坐标系 { B } \{B\} {B}与 { A } \{A\} {A}不但原点不重合，同时姿态也不相同。此时采用位移矢量 A p B o ^Ap_{Bo} ApBo描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的位置，用旋转矩阵 B A R {}^A_BR BAR描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的姿态：

则若点 p p p在坐标系 { B } \{B\} {B}下的位置为 B p ^Bp Bp，则它相对于坐标系 { A } \{A\} {A}的位置 A p ^Ap Ap可表示为：
A p = B A R B p + A p B o {}^Ap={}^A_BR\:{}^Bp+{}^Ap_{Bo} Ap=BARBp+ApBo
也即，为旋转映射与平移映射的复合。先将坐标系 { B } \{B\} {B}根据旋转矩阵进行旋转，在沿着位移矢量进行平移。

将上式进行齐次变换，得到变换矩阵 B A T {}^A_BT BAT：
[ A p 1 ] = [ B A R A p B o 0 1 ] [ B p 1 ] = B A T B p \begin{bmatrix}{}^Ap\\1\end{bmatrix}=\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}\begin{bmatrix}{}^Bp\\1\end{bmatrix}={}^A_BT\:{}^Bp [Ap1]=[BAR0ApBo1][Bp1]=BATBp
其中齐次变换矩阵 B A T {}^A_BT BAT为 4 × 4 4\times4 4×4的矩阵，它表示了由坐标系 { B } \{B\} {B}到坐标系 { A } \{A\} {A}的变换关系：
B A T = [ B A R A p B o 0 1 ] {}^A_BT=\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix} BAT=[BAR0ApBo1]

齐次坐标

若一点的 p p p的坐标为 [ x y z ] T \begin{bmatrix}x&y&z\end{bmatrix}^T [xyz]T，则它的齐次坐标表示如下，齐次坐标并不唯一，可将隔行同乘一个常数 ω \omega ω表示的仍为同一坐标：
p = [ x y z 1 ] = [ ω x ω y ω z ω ] p=\begin{bmatrix}x\\y\\z\\1\end{bmatrix}=\begin{bmatrix}\omega x\\\omega y\\\omega z\\\omega\end{bmatrix} p=⎣⎢⎢⎡xyz1⎦⎥⎥⎤=⎣⎢⎢⎡ωxωyωzω⎦⎥⎥⎤
同时，规定坐标 [ 0 0 0 0 ] T \begin{bmatrix}0&0&0&0\end{bmatrix}^T [0000]T无意义，各个轴的无穷远处如下表示：
X 轴 : [ 1 0 0 0 ] Y 轴 : [ 0 1 0 0 ] Z 轴 : [ 0 0 1 0 ] 任意无穷远处 : [ a b c 0 ] X轴:\begin{bmatrix}1\\0\\0\\0\end{bmatrix}\quad Y轴:\begin{bmatrix}0\\1\\0\\0\end{bmatrix}\quad Z轴:\begin{bmatrix}0\\0\\1\\0\end{bmatrix}\quad 任意无穷远处: \begin{bmatrix}a\\b\\c\\0\end{bmatrix} X轴:⎣⎢⎢⎡1000⎦⎥⎥⎤Y轴:⎣⎢⎢⎡0100⎦⎥⎥⎤Z轴:⎣⎢⎢⎡0010⎦⎥⎥⎤任意无穷远处:⎣⎢⎢⎡abc0⎦⎥⎥⎤

变换矩阵运算与刚体群

运动算子

变换矩阵 B A T {}^A_BT BAT可分解为两个矩阵相乘的形式，其中用 T r a n s ( A p B o ) Trans({}^Ap_{Bo}) Trans(ApBo)表示平移变换矩阵，用 R o t ( k , θ ) Rot(k,\theta) Rot(k,θ)旋转变换矩阵：
B A T = [ B A R A p B o 0 1 ] = [ I 3 × 3 A p B o 0 1 ] [ B A R ( k , θ ) 0 0 1 ] = T r a n s ( A p B o ) R o t ( k , θ ) \begin{aligned} {}^A_BT =&\begin{bmatrix}{}^A_BR&{}^Ap_{Bo}\\0&1\end{bmatrix}\\ =&\begin{bmatrix}I_{3\times3}&{}^Ap_{Bo}\\0&1\end{bmatrix}\begin{bmatrix}{}^A_BR(k,\theta)&0\\0&1\end{bmatrix}\\ =&Trans({}^Ap_{Bo})Rot(k,\theta) \end{aligned} BAT===[BAR0ApBo1][I3×30ApBo1][BAR(k,θ)001]Trans(ApBo)Rot(k,θ)
T r a n s ( A p B o ) Trans({}^Ap_{Bo}) Trans(ApBo)表示沿着位移矢量 A p B o {}^Ap_{Bo} ApBo进行平移， R o t ( k , θ ) Rot(k,\theta) Rot(k,θ)则表示绕着过原点的轴 k k k旋转 θ \theta θ角。

平移算子

相对坐标系 { A } \{A\} {A}，位移矢量 A p 1 {}^Ap_1 Ap1沿着 A p {}^Ap Ap移动至位移矢量 A p 2 {}^Ap_2 Ap2可用矢量相加表示：
A p 2 = A p 1 + A p {}^Ap_2={}^Ap_1+{}^Ap Ap2=Ap1+Ap
将其写为算子形式如下：
A p 2 = T r a n s ( A p ) A p 1 {}^Ap_2=Trans({}^Ap)\:{}^Ap_1 Ap2=Trans(Ap)Ap1
平移算子为 T r a n s ( A p ) Trans({}^Ap) Trans(Ap)表示沿着位移矢量 A p {}^Ap Ap的大小和方向进行平移。

旋转算子

相对坐标系 { A } \{A\} {A}，某点由 A p 1 {}^Ap_1 Ap1旋转至 A p 2 {}^Ap_2 Ap2可表示为：
A p 2 = R A p 1 {}^Ap_2=R\:{}^Ap_1 Ap2=RAp1
将其写为算子形式如下：
A p 2 = R o t ( k , θ ) A p 1 {}^Ap_2=Rot(k,\theta)\:{}^Ap_1 Ap2=Rot(k,θ)Ap1
旋转算子 R o t ( k , θ ) Rot(k,\theta) Rot(k,θ)表示沿着过原点的轴 k k k旋转角度 θ \theta θ。

运动算子一般形式

齐次变换矩阵可用作为运动算子，描述某点在坐标系内的运动（包括平移和旋转）。当变换矩阵作为运动算子使用时，不带上、下标。

例如，质点 p p p在坐标系 { A } \{A\} {A}中的运动轨迹为时间 t t t的函数 A p ( t ) {}^Ap(t) Ap(t)，其初始位置为 A p ( 0 ) {}^Ap(0) Ap(0)，则用运动算子表示该质点的运动轨迹如下：
A p ( t ) = T ( t ) A p ( 0 ) {}^Ap(t)=T(t)\:{}^Ap(0) Ap(t)=T(t)Ap(0)

变换矩阵的运算

变换矩阵乘法

给定变换矩阵 B A T 、 C B T {}^A_BT、{}^B_CT BAT、CBT，则可以得到变换矩阵 C A T {}^A_CT CAT：
C A T = B A T C B T = [ B A R C B R B A R B p C o + A p B o 0 1 ] {}^A_CT={}^A_BT\:{}^B_CT=\begin{bmatrix}{}^A_BR\:{}^B_CR&{}^A_BR\:{}^Bp_{Co}+{}^Ap_{Bo}\\0&1\end{bmatrix} CAT=BATCBT=[BARCBR0BARBpCo+ApBo1]
也即，坐标系 { C } \{C\} {C}在坐标系 { A } \{A\} {A}中的位姿，可以拆为两步进行：先将坐标系 { C } \{C\} {C}转换至坐标系 { B } \{B\} {B}中的位姿；再将此位姿根据坐标系 { B } \{B\} {B}至 { A } \{A\} {A}的变换进行变换，从而得到最终变换矩阵。

变换矩阵乘法满足如下条件：

运动相对固定坐标系而言：满足左乘规则，即变换顺序从右至左
运动相对运动坐标系而言：满足右乘规则，即变换顺序从左至右
变换的顺序不能调换进行

例如，对于一个变换矩阵 B A T = T r a n s ( 1 − 3 4 ) R o t ( y , 90 ° ) R o t ( z , 90 ° ) {}^A_BT=Trans(\begin{matrix}1&-3&4\end{matrix})\:Rot(y,90\degree)\:Rot(z,90\degree) BAT=Trans(1−34)Rot(y,90°)Rot(z,90°)

若相对固定坐标系（坐标系 { A } \{A\} {A}）而言，采用左乘规则进行：首先绕 z A z_A zA轴旋转 90 ° 90\degree 90°，再绕 y A y_A yA轴旋转 90 ° 90\degree 90°，最后相对 { A } \{A\} {A}平移 [ 1 − 3 4 ] T \begin{bmatrix}1&-3&4\end{bmatrix}^T [1−34]T即可。

若相对于运动坐标系（坐标系 { B } \{B\} {B}）而言，采用右乘规则进行：首先相对坐标系 { A } \{A\} {A}平移 [ 1 − 3 4 ] T \begin{bmatrix}1&-3&4\end{bmatrix}^T [1−34]T，再绕 y B y_B yB轴旋转 90 ° 90\degree 90°，最后绕 z B z_B zB轴旋转 90 ° 90\degree 90°即可。

变换矩阵求逆

若已知变换矩阵 B A T {}^A_BT BAT，此时需要求解坐标系 { A } \{A\} {A}相对于坐标系 { B } \{B\} {B}的变换 A B T {}^B_AT ABT，则需要对变换矩阵进行求逆。

此处利用齐次变换矩阵特点，简化计算：已知 B A T {}^A_BT BAT求解 A B T {}^B_AT ABT，只需要由 B A R {}^A_BR BAR和 A p B o {}^Ap_{Bo} ApBo求解 A B R {}^B_AR ABR和 B p A o {}^Bp_{Ao} BpAo即可。

首先，通过旋转矩阵正交约束知：
A B R = B A R − 1 = B A R T {}^B_AR={}^A_BR^{-1}={}^A_BR^T ABR=BAR−1=BART
随后，根据平移映射关系，可知在坐标系 { A } \{A\} {A}下坐标系 { B } \{B\} {B}的原点 A p B o {}^Ap_{Bo} ApBo，在坐标系 { B } \{B\} {B}的坐标 B ( A p B o ) {}^B({}^Ap_{Bo}) B(ApBo)：
B ( A p B o ) = A B R A p B o + B p A o {}^B({}^Ap_{Bo})={}^B_AR\:{}^Ap_{Bo}+{}^Bp_{Ao} B(ApBo)=ABRApBo+BpAo
已知，在坐标系 { B } \{B\} {B}中，点 B o Bo Bo为原点，即上式右侧为零：
B p A o = − A B R A p B o = − B A R T A p B o {}^Bp_{Ao}=-{}^B_AR\:{}^Ap_{Bo}=-{}^A_BR^T\:{}^Ap_{Bo} BpAo=−ABRApBo=−BARTApBo
由此得到变换矩阵的逆矩阵：
A B T = [ B A R T − B A R T A p B o 0 1 ] {}^B_AT=\begin{bmatrix}{}^A_BR^T&-{}^A_BR^T\:{}^Ap_{Bo}\\0&1\end{bmatrix} ABT=[BART0−BARTApBo1]
易得： A B T = B A T − 1 {}^B_AT={}^A_BT^{-1} ABT=BAT−1

刚体变换群

变换矩阵 T T T由旋转矩阵 R R R和位移矢量 t t t决定，也即任意刚体的位姿由 ( t , R ) : t ∈ ℜ 3 , R ∈ S O ( 3 ) (t,R):t\in\Re^{3},R\in SO(3) (t,R):t∈ℜ3,R∈SO(3)决定。

定义刚体变换群 S E ( 3 ) SE(3) SE(3)为乘积空间 ℜ 3 × S O ( 3 ) \Re^{3}\times SO(3) ℜ3×SO(3)：
S E ( 3 ) = { T = [ R t 0 T 1 ] ∈ ℜ 4 × 4 ∣ R ∈ S O ( 3 ) , t ∈ ℜ 3 } = ℜ 3 × S O ( 3 ) SE(3)=\bigl\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \Re^{4\times4}\big\vert R\in SO(3),t\in\Re^{3}\bigr\}=\Re^{3}\times SO(3) SE(3)={T=[R0Tt1]∈ℜ4×4∣∣R∈SO(3),t∈ℜ3}=ℜ3×SO(3)
S E ( 3 ) SE(3) SE(3)又称为三维空间的特殊欧式群（Special Euclidean Group ），推广到n维空间中可得：
S E ( n ) = { T = [ R t 0 T 1 ] ∈ ℜ n + 1 × n + 1 ∣ R ∈ S O ( n ) , t ∈ ℜ n } = ℜ n × S O ( n ) SE(n)=\bigl\{T=\begin{bmatrix}R&t\\0^T&1\end{bmatrix}\in \Re^{n+1\times n+1}\big\vert R\in SO(n),t\in\Re^{n}\bigr\}=\Re^{n}\times SO(n) SE(n)={T=[R0Tt1]∈ℜn+1×n+1∣∣R∈SO(n),t∈ℜn}=ℜn×SO(n)
S E ( n ) SE(n) SE(n)为李群，当n=2时， S E ( 2 ) SE(2) SE(2)表示刚体的平面运动，其单位元为 I 3 I_3 I3；当n=3时， S E ( 3 ) SE(3) SE(3)表示刚体

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

视觉SLAM——二维三维几何、三维空间刚体变换
前言本博客为主要学习<视觉SLAM十四讲>第3讲.<机器人学的状态估计>第6章三位几何学基础.<计算机视觉-算法和应用>第2章2.1几何基元变换等SLAM内容的总 ...
SLAM学习：三维空间刚体变换（1）
1. 欧式变换相机运动是一个刚体运动,它保证了同一个向量在各个坐标系下的长度和夹角都不会发生变化.这种变换称为欧氏变换. 一个欧式变换由一个旋转和一个平移两部分组成:其中,旋转矩阵表征绕各个坐标轴所 ...
三维空间刚体变换：旋转矩阵
点与坐标系向量与坐标空间中最基本的元素是点,点没有长度.体积.将两个点连接得到向量,向量表示由某点指向另一个点的有向线段. 对于一空间基底为 [ i j k ] \begin{bmatrix}i& ...
三维空间刚体运动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 离 ...
三维空间刚体运动4-4：四元数多点连续解析解插值方法：Spicv
三维空间刚体运动4-4:四元数多点连续解析解插值方法:Spicv 1. 总述:多点旋转插值的数学方法 2. 插值曲线及其连续性 2.1 插值曲线定义 2.2 插值曲线连续性的讨论 3. 最优插值曲线 ...
三维空间刚体运动5：详解SLAM中显示机器人运动轨迹及相机位姿（原理流程）
三维空间刚体运动5:详解SLAM中显示机器人运动轨迹及相机位姿(原理流程) 一.显示运动轨迹原理讲解二.前期准备三.git管理子模块及克隆源代码 1.学习使用Git Submodule 2.克隆源 ...

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