姿态的三种描述方式——欧拉角、轴角、四元数
欧拉角
旋转矩阵对于坐标系的描述是冗余的。旋转矩阵用了9个元素来描述姿态,而事实上,由正交性条件带来6个约束,这9个元素之间不是独立的,而是相关的。这就意味着,只要三个参数就能描述一个刚体在空间中的姿态。
(1)X-Y-Z固定角坐标系
先将{B}绕{A}的X轴旋转γ\gammaγ,然后绕{A}的Y轴旋转β\betaβ,最后绕{A}的Z轴旋转α\alphaα,每个旋转都是绕着固定参考坐标系{A}的轴。
BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαsβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ]{^A_B}R_{XYZ}(\gamma,\beta,\alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin{bmatrix} c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma\\ s\alpha s\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma\\ -s\beta& c\beta s\gamma & c\beta c\gamma \end{bmatrix}BARXYZ(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαsβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤
当已知一个旋转矩阵,可以通过以下计算其X-Y-Z固定角坐标系
BARXYZ(γ,β,α)=[r11r12r13r21r22r23r31r32r33]{^A_B}R_{XYZ}(\gamma,\beta,\alpha)=\begin{bmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix}BARXYZ(γ,β,α)=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
β=Atan2(−r31,r112+r212)α=Atan2(r21/cβ,r11/cβ)γ=Atan2(r32/cβ,r33/cβ)\begin{aligned} \beta=Atan2(-r_{31},\sqrt{r^2_{11}+r^2_{21}}) \\ \alpha=Atan2(r_{21}/c\beta,r_{11}/c\beta)\\ \gamma=Atan2(r_{32}/c\beta,r_{33}/c\beta) \end{aligned}β=Atan2(−r31,r112+r212)α=Atan2(r21/cβ,r11/cβ)γ=Atan2(r32/cβ,r33/cβ)
如果β=±90∘\beta=\pm90^\circβ=±90∘,在这种情况下,仅能求出α\alphaα和γ\gammaγ的和或差。在这种情况下一般取α\alphaα=0.0,结果如下:当β=90∘\beta=90^\circβ=90∘,解得:
β=90∘α=0γ=Atan2(r12,r2)\begin{aligned} \beta=90^\circ \\ \alpha=0\\ \gamma=Atan2(r_{12},r_{2}) \end{aligned}β=90∘α=0γ=Atan2(r12,r2)
当β=−90∘\beta=-90^\circβ=−90∘,解得:
β=−90∘α=0γ=−Atan2(r12,r2)\begin{aligned}
\beta=-90^\circ \\
\alpha=0\\
\gamma=-Atan2(r_{12},r_{2})
\end{aligned}β=−90∘α=0γ=−Atan2(r12,r2)
(2)Z-Y-X欧拉角
假设开始两个坐标系重合,先将{B}绕自身的Z轴旋转α\alphaα,然后绕Y轴旋转β\betaβ,最后绕X轴旋转γ\gammaγ,就能旋转到当前姿态。称其为Z-Y-X欧拉角,由于是绕自身坐标轴进行旋转,则旋转矩阵为:
BARZ′Y′X′(γ,β,α)=RZ(α)RY(β)RX(γ)=[cαcβcαsβsγ−sαcγcαsβcγ+sαsγsαsβsαsβsγ+cαcγsαsβcγ−cαsγ−sβcβsγcβcγ]{^A_B}R_{Z\prime Y\prime X\prime}(\gamma,\beta,\alpha)=R_Z(\alpha)R_Y(\beta)R_X(\gamma)=\begin{bmatrix} c\alpha c\beta & c\alpha s\beta s\gamma -s\alpha c\gamma & c\alpha s\beta c\gamma +s\alpha s\gamma\\ s\alpha s\beta & s\alpha s\beta s\gamma +c\alpha c\gamma & s\alpha s\beta c\gamma -c\alpha s\gamma\\ -s\beta& c\beta s\gamma & c\beta c\gamma \end{bmatrix}BARZ′Y′X′(γ,β,α)=RZ(α)RY(β)RX(γ)=⎣⎡cαcβsαsβ−sβcαsβsγ−sαcγsαsβsγ+cαcγcβsγcαsβcγ+sαsγsαsβcγ−cαsγcβcγ⎦⎤
由旋转矩阵得出Z-Y-X欧拉角的方法与上述X-Y-Z固定角坐标系计算过程相同,在此不再推导。
轴角
绕空间中某一轴旋转指定角度,可以采用4个参数进行表示。令r为关于参考坐标系O-xyz的旋转轴的单位向量。为了表示绕轴r旋转角度θ\thetaθ的旋转矩阵R(θ,r)R(\theta,r)R(θ,r),方便的做法是对绕参考坐标系的坐标轴的基本旋转进行合成。
于是发现相应于给定角度和轴的旋转矩阵为:
R(θ,r)=[rx2(1−cθ)+cθrxry(1−cθ)−rzsθrxrz(1−cθ)+rysθrxry(1−cθ)+rzsθry2(1−cθ)+cθryrz(1−cθ)−rxsθrxrz(1−cθ)−rysθryrz(1−cθ)+rxsθrz2(1−cθ)+cθ]R(\theta,r)=\begin{bmatrix} r{^2_x}(1-c\theta)+c\theta & r_x r_y(1-c\theta)-r_zs\theta &r_xr_z(1-c\theta)+r_ys\theta \\ r_x r_y(1-c\theta)+r_zs\theta & r{^2_y}(1-c\theta)+c\theta & r_y r_z(1-c\theta)-r_xs\theta \\ r_xr_z(1-c\theta)-r_ys\theta& r_y r_z(1-c\theta)+r_xs\theta & r{^2_z}(1-c\theta)+c\theta \end{bmatrix}R(θ,r)=⎣⎡rx2(1−cθ)+cθrxry(1−cθ)+rzsθrxrz(1−cθ)−rysθrxry(1−cθ)−rzsθry2(1−cθ)+cθryrz(1−cθ)+rxsθrxrz(1−cθ)+rysθryrz(1−cθ)−rxsθrz2(1−cθ)+cθ⎦⎤
如果需要计算逆解以计算相应于如下给定旋转矩阵的轴和角度:
R=[r11r12r13r21r22r23r31r32r33]R=\begin{bmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix}R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
θ=arccos(r11+r22+r33−12)r=12sinθ[r32−r23r13−r31r21−r12]\begin{aligned}
\theta=arccos(\frac{r_{11}+r_{22}+r_{33}-1}{2})\\
r=\frac{1}{2sin\theta}\begin{bmatrix} r_{32}-r_{23}\\ r_{13}-r_{31}\\ r_{21}-r_{12}\end{bmatrix}
\end{aligned}θ=arccos(2r11+r22+r33−1)r=2sinθ1⎣⎡r32−r23r13−r31r21−r12⎦⎤
注意:当θ=0或pi\theta=0或piθ=0或pi单位向量任意是奇异的。
四元数
轴角的表达式的不足可以通过一个不同的四参数表达式加以克服,即单位四元数,定义为:Q={η\etaη,ε\varepsilonε},其中:
ε=cosθ2ε=sinθ2r\begin{aligned}\varepsilon=cos \frac{\theta}{2}\\ \varepsilon=sin \frac{\theta}{2}r\end{aligned}ε=cos2θε=sin2θr
η2+εx2+εy2+εz2=1\begin{aligned}\eta^2+\varepsilon{^2_x}+\varepsilon{^2_y}+\varepsilon{^2_z}=1\end{aligned}η2+εx2+εy2+εz2=1
相应于给定四元数的旋转矩阵有以下形式:
R(θ,r)=[2(η2+εx2)−12(εxεy−ηεz)2(εxεz+ηεy)2(εxεy+ηεz)2(η2+εy2)−12(εyεz−ηεx)2(εxεz−ηεy)2(εyεx)2(η2+εz2)−1]R(\theta,r)=\begin{bmatrix} 2(\eta^2+\varepsilon{^2_x}) -1& 2(\varepsilon_x\varepsilon_y-\eta\varepsilon_z)& 2(\varepsilon_x\varepsilon_z+\eta\varepsilon_y)\\ 2(\varepsilon_x\varepsilon_y+\eta\varepsilon_z) & 2(\eta^2+\varepsilon{^2_y}) -1& 2(\varepsilon_y\varepsilon_z-\eta\varepsilon_x) \\ 2(\varepsilon_x\varepsilon_z-\eta\varepsilon_y)& 2(\varepsilon_y\varepsilon_x) &2(\eta^2+\varepsilon{^2_z}) -1\end{bmatrix}R(θ,r)=⎣⎡2(η2+εx2)−12(εxεy+ηεz)2(εxεz−ηεy)2(εxεy−ηεz)2(η2+εy2)−12(εyεx)2(εxεz+ηεy)2(εyεz−ηεx)2(η2+εz2)−1⎦⎤
如果需要计算逆解以计算相应于如下给定旋转矩阵的四元数:
R=[r11r12r13r21r22r23r31r32r33]R=\begin{bmatrix} r_{11}&r_{12}&r_{13}\\ r_{21}&r_{22}&r_{23}\\r_{31}&r_{32}&r_{33}\end{bmatrix}R=⎣⎡r11r21r31r12r22r32r13r23r33⎦⎤
ε=12r11+r22+r33+1\begin{aligned}\varepsilon= \frac{1}{2}\sqrt{r_{11}+r_{22}+r_{33}+1}\end{aligned}ε=21r11+r22+r33+1
ε=[sgn(r32−r23)r11−r22−r33+1sgn(r13−r31)r22−r33−r11+1sgn(r21−r12)r33−r11−r22+1]\varepsilon=\begin{bmatrix} sgn(r_{32}-r_{23}) \sqrt{r_{11}-r_{22}-r_{33}+1}\\ sgn(r_{13}-r_{31}) \sqrt{r_{22}-r_{33}-r_{11}+1}\\sgn(r_{21}-r_{12}) \sqrt{r_{33}-r_{11}-r_{22}+1}\end{bmatrix}ε=⎣⎡sgn(r32−r23)r11−r22−r33+1sgn(r13−r31)r22−r33−r11+1sgn(r21−r12)r33−r11−r22+1⎦⎤
其中,当x≥0x\geq0x≥0时sgn(x)=1;当x<0时,sgn(x)=-1;与轴角逆解相比,没有奇点现象。
更多推荐


所有评论(0)