其他分享
首页 > 其他分享> > 三维空间刚体变换:变换矩阵

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

作者:互联网

变换矩阵

坐标系描述

用 A p B o ^Ap_{Bo} ApBo​表示在参考坐标系 { A } \{A\} {A}下坐标系 { B } \{B\} {B}的原点坐标,并用旋转矩阵 B A R ^A_BR BA​R描述坐标系 { 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}={ApBo​BA​R}
坐标系的描述概括了刚体的位置和姿态的描述,当表示位置时,旋转矩阵 B A R = I {}^A_BR=I BA​R=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 BA​R描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的姿态:

在这里插入图片描述

称 B A R {}^A_BR BA​R为坐标系 { 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=BA​RBp
同样,用 A B R {}^B_AR AB​R表示坐标系 { 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 AB​R=BA​R−1=BA​RT

一般刚体变换

在三维空间中,最常见的情况如下:坐标系 { B } \{B\} {B}与 { A } \{A\} {A}不但原点不重合,同时姿态也不相同。此时采用位移矢量 A p B o ^Ap_{Bo} ApBo​描述坐标系 { B } \{B\} {B}相对于坐标系 { A } \{A\} {A}的位置,用旋转矩阵 B A R {}^A_BR BA​R描述坐标系 { 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=BA​RBp+ApBo​
也即,为旋转映射与平移映射的复合。先将坐标系 { B } \{B\} {B}根据旋转矩阵进行旋转,在沿着位移矢量进行平移。

将上式进行齐次变换,得到变换矩阵 B A T {}^A_BT BA​T:
[ 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​]=[BA​R0​ApBo​1​][Bp1​]=BA​TBp
其中齐次变换矩阵 B A T {}^A_BT BA​T为 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} BA​T=[BA​R0​ApBo​1​]

齐次坐标

若一点的 p p p的坐标为 [ x y z ] T \begin{bmatrix}x&y&z\end{bmatrix}^T [x​y​z​]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 [0​0​0​0​]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 BA​T可分解为两个矩阵相乘的形式,其中用 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} BA​T===​[BA​R0​ApBo​1​][I3×3​0​ApBo​1​][BA​R(k,θ)0​01​]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 BA​T、CB​T,则可以得到变换矩阵 C A T {}^A_CT CA​T:
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} CA​T=BA​TCB​T=[BA​RCB​R0​BA​RBpCo​+ApBo​1​]
也即,坐标系 { 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) BA​T=Trans(1​−3​4​)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​−3​4​]T即可。

在这里插入图片描述

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

在这里插入图片描述

变换矩阵求逆

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

此处利用齐次变换矩阵特点,简化计算:已知 B A T {}^A_BT BA​T求解 A B T {}^B_AT AB​T,只需要由 B A R {}^A_BR BA​R和 A p B o {}^Ap_{Bo} ApBo​求解 A B R {}^B_AR AB​R和 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 AB​R=BA​R−1=BA​RT
随后,根据平移映射关系,可知在坐标系 { 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​)=AB​RApBo​+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​=−AB​RApBo​=−BA​RTApBo​
由此得到变换矩阵的逆矩阵:
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} AB​T=[BA​RT0​−BA​RTApBo​1​]
易得: A B T = B A T − 1 {}^B_AT={}^A_BT^{-1} AB​T=BA​T−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=[R0T​t1​]∈ℜ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=[R0T​t1​]∈ℜ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)表示刚体

标签:BA,变换,Bo,矩阵,Ap,三维空间,bmatrix,坐标系,刚体
来源: https://blog.csdn.net/weixin_45929038/article/details/122796956