坐标变换(6)—齐次变换矩阵

前面的文章主要介绍了旋转矩阵,对于刚体的运动,除了旋转外还有平移。在机器人及自动驾驶中,经常用齐次变换矩阵将旋转和平移进行统一。 前面的文章也介绍过齐次变换矩阵,本文算是一个总结。

1. SE(3)

将旋转矩阵和平移向量写在同一个矩阵中,形成的4×44\times4矩阵,称为special Euclidean group,即SE(3)SE(3)
T=[Rp01]=[r11r12r13p1r21r22r23p2r31r32r33p30001](1) T=\left[\begin{array}{ll} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{llll} r_{11} & r_{12} & r_{13} & p_{1} \\ r_{21} & r_{22} & r_{23} & p_{2} \\ r_{31} & r_{32} & r_{33} & p_{3} \\ 0 & 0 & 0 & 1 \end{array}\right] \tag{1}
很容易验证,齐次变换矩阵满足群所具有的性质,即封闭性,结合律,幺元,逆,所以称其为group是合理的。

T1=[Rp01]1=[RTRTp01](2) T^{-1}=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]^{-1}=\left[\begin{array}{cc} R^{\mathrm{T}} & -R^{\mathrm{T}} p \\ 0 & 1 \end{array}\right] \tag{2}
(T1T2)T3=T1(T2T3)(3) (T_1T_2)T_3=T_1(T_2T_3) \tag{3}
此外齐次变换矩阵还能保持变换前后的距离和角度不变,假定TSE(3)T\in SE(3),同时x,y,zR3x,y,z \in \mathbb{R}^3,则有,

TxTy=xy(4)\|T x-T y\|=\|x-y\| \tag{4}
TxTz,TyTz=xz,yz(5)\langle T x-T z, T y-T z\rangle=\langle x-z, y-z\rangle \tag{5}

2. 齐次变换矩阵的用法

2.1 描述坐标系

如上图所示,vb=(0,0,1.5)v_b=(0,0,1.5),假设fixed frame为{S}\{S\}{a}\{a\}{S}\{S\}重合,则{a}\{a\},{b}\{b\},{c}\{c\}可以描述为:
Tsa=(Rsa,psa)Tsb=(Rsb,psb)Tsc=(Rsc,psc)(4) \begin{aligned} T_{sa}&=(R_{sa},p_{sa}) \\ T_{sb}&=(R_{sb},p_{sb}) \\ T_{sc}&=(R_{sc},p_{sc}) \end{aligned} \tag{4}
其中,
Rsa=[100010001],Rsb=[001010100],Rsc=[100001010]R_{s a}=\left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right], \quad R_{s b}=\left[\begin{array}{rrr} 0 & 0 & 1 \\ 0 & -1 & 0 \\ 1 & 0 & 0 \end{array}\right], \quad R_{s c}=\left[\begin{array}{rrr} -1 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 1 & 0 \end{array}\right]

psa=[000],psb=[020],psc=[110]p_{s a}=\left[\begin{array}{c} 0 \\ 0 \\ 0 \end{array}\right], \quad p_{s b}=\left[\begin{array}{c} 0 \\ -2 \\ 0 \end{array}\right], \quad p_{s c}=\left[\begin{array}{c} -1 \\ 1 \\ 0 \end{array}\right]

2.2 向量(坐标系)在不同坐标系下的描述

对于任意三个坐标系,vv{b}\{b\}下为vbv_b,

TabTbc=TaTc=TacTabvb=Tabvb=va(5)\begin{aligned} T_{a b} T_{b c} &=T_{a \not {b}} T_{\not {b} c}=T_{a c} \\ T_{a b} v_{b} &=T_{a b} v_{b}=v_{a} \end{aligned} \tag{5}
vv{a}\{a\}下为vav_a

2.3 对向量(坐标系)进行平移和旋转

可以将齐次变换矩阵写为公式(6),这里将Rot(ω^,θ)\operatorname{Rot}(\hat{\omega}, \theta)改写为齐次形式,为了便于标记,
T=Trans(p)Rot(ω^,θ)=[100px010py001pz0001][R001](6) \begin{aligned} T&=\operatorname{Trans}(p)\operatorname{Rot}(\hat{\omega}, \theta)\\ &=\left[\begin{array}{llll} 1 & 0 & 0 & p_{x} \\ 0 & 1 & 0 & p_{y} \\ 0 & 0 & 1 & p_{z} \\ 0 & 0 & 0 & 1 \end{array}\right]\left[\begin{array}{ll} R & 0 \\ 0 & 1 \end{array}\right] \end{aligned} \tag{6}

假设body frame{b}\{b\}相对于fixed frame{S}\{S\}的描述为TsbT_{sb},则下面讨论左乘和右乘TT的区别,

Tsb=TTsb=Trans(p)Rot(ω^,θ)Tsb (fixed frame) =[Rp01][Rsbpsb01]=[RRsbRpsb+p01](7)\begin{aligned} &T_{s b^{\prime}}=T T_{s b}=\operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) T_{s b} \quad \text { (fixed frame) }\\ &=\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R R_{s b} & R p_{s b}+p \\ 0 & 1 \end{array}\right] \end{aligned} \tag{7}
Tsb=TsbT=TsbTrans(p)Rot(ω^,θ) (body frame) =[Rsbpsb01][Rp01]=[RsbRRsbp+psb01](8)\begin{aligned} &T_{s b^{\prime \prime}}=T_{s b} T=T_{s b} \operatorname{Trans}(p) \operatorname{Rot}(\hat{\omega}, \theta) \quad \text { (body frame) }\\ &=\left[\begin{array}{cc} R_{s b} & p_{s b} \\ 0 & 1 \end{array}\right]\left[\begin{array}{cc} R & p \\ 0 & 1 \end{array}\right]=\left[\begin{array}{cc} R_{s b} R & R_{s b} p+p_{s b} \\ 0 & 1 \end{array}\right] \end{aligned} \tag{8}

以下图为例,假设ω^=(0,0,1)\hat{\omega}=(0,0,1)θ=90ο\theta=90^{\operatorname {\omicron}},p=(0,2,0)p=(0,2,0),分析左乘和右乘的区别,

首先是左乘,如上图左边所示,根据公式(7):
首先分析Rot(ω^,θ)Tsb\operatorname{Rot}(\hat{\omega},\theta)T_{s b}TsbT_{s b}首先以{S}\{S\}为基准进行旋转,RRsbR R_{s b}为绕着z^s\hat z_s旋转RRRpsbRp_{s b}psbp_{s b}按照图中①的方式进行了旋转;
然后再左乘Trans(p)\operatorname{Trans}(p),将①的结果沿着ys^\hat{y_s}移动2个单位,即得到图中②最终的结果。

其次是右乘,如上图右边所示,根据公式(8):
首先分析TsbTrans(p)T_{s b} \operatorname{Trans}(p)TsbT_{s b}首先以{b}\{b\}为基准进行平移,即沿着yb^\hat{y_b}平移2个单位,Rsbp+psbR_{s b} p+p_{s b},即得到图中的①;
然后再右乘Rot(ω^,θ)\operatorname{Rot}(\hat{\omega}, \theta),以{b}\{b\}为基准进行旋转,即沿着zb^\hat{z_b}旋转θ=90ο\theta=90^{\operatorname {\omicron}},得到图中②的结果。
综上所述,
左乘齐次矩阵,首先以{S}\{S\}为基准进行旋转,然后以{S}\{S\}为基准进行平移;
右乘齐次矩阵,首先以{b}\{b\}为基准进行平移,然后以{b}\{b\}为基准进行旋转;

发布了127 篇原创文章 · 获赞 164 · 访问量 38万+
展开阅读全文

没有更多推荐了,返回首页

©️2019 CSDN 皮肤主题: 创作都市 设计师: CSDN官方博客

分享到微信朋友圈

×

扫一扫,手机浏览