姿态解算

方向余弦矩阵
- 基本概念
- 已知欧拉角求方向余弦
- 已知方向余弦求欧拉角
四元数
- 基本概念
- 四元数表征姿态
- - <1四元数的共轭
  - <2四元数的逆
- 四元数与方向余弦
姿态表示的三种方法
- 欧拉角
- 方向余弦矩阵
- 四元数
- $$spark-1$$

)

方向余弦矩阵

基本概念

方向余弦：
在坐标系R中设v为一个空间向量，在R坐标系下的投影为（vx,vy,vz）
与其x y z 轴分别成αβγ\alpha \beta \gammaαβγ角度，则cosα,cosβ,cosγcos\alpha ,cos\beta,cos\gammacosα,cosβ,cosγ分别为v在三轴的方向余弦，大小分别等于|vx|,|vy|,|vz|。

方向余弦矩阵（direction cosine matrix）
在一个平面内对向量进行旋转相当于对坐标进行旋转，初始状态下令载体坐标系和参考坐标系完全重合。
方向余弦矩阵便是存放着一系列方向余弦，设r(x,y,z)为参考坐标系，O(X1，Y1，Z1)为载体坐标系，载体转动时，载体坐标系会相对于参考坐标系转动，把载体坐标系的三个轴当做三个单位向量（vx,vy,vz）,每个载体坐标轴都可以在参考坐标系上找到三个对应的方向余弦。最终得到九个方向余弦，把九个方向余弦写成矩阵形式就是方向余弦矩阵。
而方向余弦矩阵的写法有两种，一种是载体坐标系相对于参考坐标系，一种是参考坐标系相对于载体坐标系。
C0r=[c11c12c13c21c22c23c31c32c33]或Cr0=[c11c21c31c12c22c32c13c23c33]C_0^r=\begin{bmatrix}\\ c_{11} c_{12}c_{13} \\ c_{21} c_{22} c_{23} \\ c_{31}c_{32}c_{33} \end{bmatrix}或C_r^0=\begin{bmatrix}\\ c_{11} c_{21}c_{31} \\ c_{12} c_{22} c_{32} \\ c_{13}c_{23}c_{33} \end{bmatrix}C0r=⎣⎡c11c12c13c21c22c23c31c32c33⎦⎤或Cr0=⎣⎡c11c21c31c12c22c32c13c23c33⎦⎤
C_0^r表称O系对于r系的方向余弦矩阵,这两个矩阵互为转置矩阵。
C12=cos(xr.x0)C_{12}=cos(x_r.x_0)C12=cos(xr.x0)载体坐标系上的$x_0相对于参考坐标系上的x_r的方向余弦，
C12=cos(xr,y0)同理C_{12}=cos(x_r,y_0)同理C12=cos(xr,y0)同理 y0相对于参考坐标系上xr的方向余弦y_0相对于参考坐标系上x_r的方向余弦y0相对于参考坐标系上xr的方向余弦

已知欧拉角求方向余弦

ψθγ\psi \theta \gammaψθγ
[cosγcosψ+sinγsinψsinθ−cosγsinψsinθ−sinγcosθsinψcosθcosψcosθsinθsinγcosψ−cosγsinψsinθ−sinγsinψ−cosγcosψsinθcosγcosθ]\begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix}⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤

已知方向余弦求欧拉角

方向余弦矩阵：[T11T21T31T12T22T32T13T23T33]\begin{bmatrix} T_{11}&T_{21}&T_{31}\\T_{12}&T_{22}&T_{32} \\T_{13}&T_{23}&T_{33}\end{bmatrix}⎣⎡T11T12T13T21T22T23T31T32T33⎦⎤
推出：
横滚角：θ=arcsinT32\theta=arcsinT_{32}θ=arcsinT32
俯仰角：
γ=arctan(−T32T33)\gamma=arctan(-\frac{T_{32}}{T_{33}}) γ=arctan(−T33T32)
偏航角：
ψ=arctan(T12T22)\psi =arctan(\frac {T_{12}}{T_{22}})ψ=arctan(T22T12)

四元数

基本概念

四元数是由实数和虚数组成的一组超复数，假设Q是一个四元数。

                      Q =  w  +  xi +  yj +  zj

其中w表示四元数的实数大小，x,y,z表示虚数大小,实部单位为1，虚部单为为i,j,k。

四元数一般表示：
q⃗=[wv⃗]=[w(xyz)]\vec{q}=[w \vec{v}]=[w (xyz)]q=[wv]=[w(xyz)]

四元数表征姿态

其中w为实数部分，v⃗\vec{v}v为矢量部分，首先讨论如何把一个空间向量转换到四元数。
假设在三维空间中有一个点（x,y,z),拓展到四元数空间有
p⃗=[0(xyz)]\vec{p}=[0(x y z)]p=[0(xyz)]
p⃗\vec{p}p就是三维空间中一点四元数的表示，而旋转是通过旋转四元数来实现的。现在假设空间中有一个向量n⃗\vec{n}n,(nx,ny,nz)(n_x,n_y,n_z)(nx,ny,nz),若用四元数来表示点p(x,y,z)绕向量n⃗\vec{n}n旋转θ\thetaθ角度后p,p^,p,
定义旋转四元数为p⃗\vec{p}p则对于本次旋转有：

p⃗=[cos\vec{p}=[cosp=[cos(θ2)(\frac{\theta}{2})(2θ) sin(θ2)(\frac{\theta}{2})(2θ)n⃗\vec{n}n]=[cos(θ2)(\frac{\theta}{2})(2θ) ... sin(θ2)(\frac{\theta}{2})(2θ) nx⃗\vec{n_x}nx ... sin(θ2)(\frac{\theta}{2})(2θ)ny⃗\vec{n_y}ny ... sin(θ2)(\frac{\theta}{2})(2θ)nz⃗\vec{n_z}nz ]
(其中.为分隔用)

其中

nx⃗\vec{n_x}nx	ny⃗\vec{n_y}ny	nz⃗\vec{n_z}nz
nx⃗\vec{n_x}nx=nxn_xnx*i	ny⃗\vec{n_y}ny=nyn_yny*j	nz⃗\vec{n_z}nz=nzn_znz*k

若想得到旋转后的矩阵只需要执行以下式子

p,⃗=q⃗p⃗q⃗−1\vec{p^,}=\vec{q}\vec{p} \vec{q}^-1p,=qpq−1

(q⃗−1为q⃗的逆矩阵)(\vec{q}^-1为\vec{q}的逆矩阵)(q−1为q的逆矩阵)

想要推导下面的公式需要了解两个概念想要推导下面的公式需要了解两个概念想要推导下面的公式需要了解两个概念

<1四元数的共轭

四元数的共轭就是让四元数的向量部分取负，记作
p⃗\vec{p}p=[w v⃗\vec{v}v]=[w -v⃗\vec{v}v]=[w (-x -y -z)]
四元数与他的共轭代表反向的角位移，因为相当于旋转轴反向

<2四元数的逆

四元数的逆就是他的共轭除以他的模
q−1⃗=(q⃗∗)(∣q⃗∣)\vec{q^-1} = \frac{(\vec{q}*)}{(|\vec{q}|)} q−1=(∣q∣)(q∗)
一般使用单位四元数,所以他的逆和共轭是相等的
因为公式中涉及到四元数的乘法，在表示旋转时用叉乘，计算公式如下
[w1(x1y1z1)][w2(x2y2z2)]=[w1w2−x1x2−y1y2−z1z2(w1x2+x1w2+z1y2−y1z2w1y2+y1w2+x1z2−z1x2w1z2+z1w2+y1x2−x1y2)][w_1 (x_1 y_1 z_1)][w_2 ( x_2 y_2 z_2)]=\begin{bmatrix} w_1w_2 -x_1x_2 -y_1y_2-z_1z_2\\ \begin{pmatrix}w_1x_2 + x_1w_2 +z_1y_2-y_1z_2 \\w_1y_2+y_1w_2+x_1z_2-z_1x_2 \\w_1z_2+z_1w_2+y_1x_2-x_1y_2 \end{pmatrix} \end{bmatrix}[w1(x1y1z1)][w2(x2y2z2)]=⎣⎢⎢⎡w1w2−x1x2−y1y2−z1z2⎝⎛w1x2+x1w2+z1y2−y1z2w1y2+y1w2+x1z2−z1x2w1z2+z1w2+y1x2−x1y2⎠⎞⎦⎥⎥⎤
example：

假设空间中有一个点p(0,1,0)饶z轴逆时针旋转90度，求旋转后的p,p^,p,点。理论上p,p^,p,点为（-1,0,0）。
首先把p(0,1,0)拓展成一个四元数p⃗\vec{p}p=(0,0,j,0),然后定义旋转四元数，旋转轴为z轴化为单位向量（0,0,1）,因为旋转角度为90度所以------

q⃗=(cos45。,0,0,1∗sin45。k)=(22,0,0,22k)\vec{q}=(cos45^。,0,0,1*sin45^。k)=(\frac{\sqrt 2}{2},0,0,\frac{\sqrt 2}{2}k)q=(cos45。,0,0,1∗sin45。k)=(22,0,0,22k)
单位四元数的逆的共轭是相等的

q⃗−1=(22,0,0,−22k)\vec{q}^-1=(\frac{\sqrt 2}{2},0,0,-\frac{\sqrt 2}{2}k)q−1=(22,0,0,−22k)

由四元数叉乘计算公式 p,⃗=q⃗p⃗q⃗−1\vec{p^,}=\vec{q}\vec{p} \vec{q}^-1p,=qpq−1 用上述乘法公式计算
q⃗p⃗=(0,22kj,22j,0)\vec{q}\vec{p}=(0,\frac{\sqrt 2}{2}kj,\frac{\sqrt 2}{2}j,0)qp=(0,22kj,22j,0)
k*j=-i再乘以q⃗−1得到p⃗,=[0,−i,0,0]\vec{q}^{-1}得到\vec{p}^{,}=[0,-i,0,0]q−1得到p,=[0,−i,0,0],带回去得到点[-1,0,0]即为旋转之后的坐标。

四元数与方向余弦

由前面的介绍知道了旋转四元数q=cosθ2+x∗sinθ2∗i+y∗sinθ2∗j+z∗sinθ2∗kq=cos\frac {\theta}{2}+x*sin\frac{\theta}{2}*i+y*sin\frac {\theta}{2}*j+z*sin\frac{\theta}{2}*kq=cos2θ+x∗sin2θ∗i+y∗sin2θ∗j+z∗sin2θ∗k
这里令q0=cosθ2,q1=sinθ2,q2=sinθ2,q3=sinθ2,q_0=cos\frac{\theta}{2},q_1=sin\frac{\theta} {2},q_2=sin\frac{\theta}{2},q_3=sin\frac{\theta}{2},q0=cos2θ,q1=sin2θ,q2=sin2θ,q3=sin2θ,
q=q0+q1i+q2j+q3kq_0+q_1i+q_2j+q_3kq0+q1i+q2j+q3k四元数旋转的方向余弦矩阵公式如下：
[xbybzb]=[q02+q12−q22−q322(q1q2+q0q3)2(q1q3−q0q2)2（q1q2−q0q3）q02−q12+q22−q322(q0q1+q2q3)2(q1q2−q0q3)2(q2q3−q0q1)q02−q12−q22+q32]⋅[xnynzn]\begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2& 2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\2（q_1q_2-q_0q_3）&q_0^2-q_1^2+q_2^2-q_3^2&2(q_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix}⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322（q1q2−q0q3）2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤
四元数表示的旋转矩阵公式四元数表示的旋转矩阵公式四元数表示的旋转矩阵公式

姿态表示的三种方法

欧拉角

直观的欧拉角横滚角roll 俯仰角pitch 偏航角 yaw

方向余弦矩阵

[cosγcosψ+sinγsinψsinθ−cosγsinψsinθ−sinγcosθsinψcosθcosψcosθsinθsinγcosψ−cosγsinψsinθ−sinγsinψ−cosγcosψsinθcosγcosθ]\begin{bmatrix} cos \gamma cos\psi +sin\gamma sin\psi sin \theta& -cos\gamma sin\psi sin\theta & -sin\gamma cos\theta \\ sin\psi cos\theta&cos\psi cos\theta &sin\theta \\sin\gamma cos\psi -cos\gamma sin\psi sin\theta &-sin\gamma sin\psi -cos\gamma cos\psi sin\theta&cos\gamma cos\theta \end{bmatrix}⎣⎡cosγcosψ+sinγsinψsinθsinψcosθsinγcosψ−cosγsinψsinθ−cosγsinψsinθcosψcosθ−sinγsinψ−cosγcosψsinθ−sinγcosθsinθcosγcosθ⎦⎤

四元数

[xbybzb]=[q02+q12−q22−q322(q1q2+q0q3)2(q1q3−q0q2)2（q1q2−q0q3）q02−q12+q22−q322(q0q1+q2q3)2(q1q2−q0q3)2(q2q3−q0q1)q02−q12−q22+q32]⋅[xnynzn]\begin{bmatrix} x_b\\y_b\\z_b\end{bmatrix}=\begin{bmatrix}q_0^2+q_1^2-q_2^2-q_3^2& 2(q_1q_2+q_0q_3)&2(q_1q_3-q_0q_2) \\2（q_1q_2-q_0q_3）&q_0^2-q_1^2+q_2^2-q_3^2&2(q_0q_1+q_2q_3) \\2(q_1q_2-q_0q_3)&2(q_2q_3-q_0q_1)&q_0^2-q_1^2-q_2^2+q_3^2 \\ \end{bmatrix}\cdot\begin{bmatrix}x_n \\y_n \\z_n\end{bmatrix}⎣⎡xbybzb⎦⎤=⎣⎡q02+q12−q22−q322（q1q2−q0q3）2(q1q2−q0q3)2(q1q2+q0q3)q02−q12+q22−q322(q2q3−q0q1)2(q1q3−q0q2)2(q0q1+q2q3)q02−q12−q22+q32⎦⎤⋅⎣⎡xnynzn⎦⎤

spark−1spark-1spark−1

方向余弦矩阵与四元数相关推荐

无人机姿态表示方法及相互转换（欧拉角、方向余弦矩阵、四元数）
常用的姿态表示方法有欧拉角.方向余弦矩阵.四元数这几种欧拉角表示方法采用来表示飞行器的姿态,其中为滚转角,为俯仰角和为航向角,表示飞行器首先航向偏转角度,再俯仰角度,然后机体滚转角度得到的姿态方向 ...
PSINS中欧拉角、方向余弦矩阵与姿态四元数的转换公式与代码
文章目录欧拉角转四元数公式代码欧拉角转方向余弦矩阵公式代码四元数转方向余弦矩阵公式代码方向余弦矩阵转欧拉角公式代码方向余弦矩阵转四元数公式代码提示:代码只给出PSINS ...
C语言实现等效旋转矢量转方向余弦矩阵和姿态四元数
等效旋转矢量转方向余弦矩阵和姿态四元数旋转矢量方向余弦矩阵姿态四元数源码旋转矢量力学中刚体的有限次转动是不可交换的.转动的不可交换性决定了转动不是矢量,即两次以上的不同轴转动不能相加.对一 ...
matlab和Eigen库中的一些旋转矩阵（方向余弦矩阵）、四元数和欧拉角之间的转换和绘图的注意事项
最近用matlab和Eigen库中的一些旋转矩阵(方向余弦矩阵).四元数和欧拉角之间的转换和绘图,弄得我有些头疼,把遇到的问题记录一下,以防以后又脑阔疼....有不同的理解可以再评论区批评指正- 主要 ...
C语言实现方向余弦矩阵转欧拉角和姿态四元数
方向余弦矩阵转欧拉角和姿态四元数方向余弦矩阵欧拉角姿态四元数源码方向余弦矩阵方向余弦矩阵(Direction Cosine Matrix,DCM)又被称为"坐标转换矩阵" ...
向量的方向余弦公式_定位中方向余弦矩阵(DCM)简介
1 前言这篇文章是翻译Starlino_DCM_Tutorial.pdf而来, 链接为:http://www.starlino.com/dcm_tutorial.html 这篇文章主要是介绍无人 ...
方向余弦矩阵DCM刚体的矢量—矩阵描述
方向余弦矩阵DCM&刚体的矢量-矩阵描述简介方向余弦矩阵刚体的矢量-矩阵描述简介我本意是为了学习理论力学,但是我对方向余弦矩阵的理解不够,所以不得不补习一下基础知识.这个过程里,我发 ...
方向余弦矩阵(DCM)简介（转）
1 前言这篇文章是翻译Starlino_DCM_Tutorial.pdf而来, 链接为:http://www.starlino.com/dcm_tutorial.html,各位看官可以对照着原文 ...
C语言实现欧拉角转方向余弦矩阵
欧拉角转方向余弦矩阵欧拉角方向余弦矩阵源码欧拉角欧拉证明任意两个正交坐标系之间的相对朝向关系可以通过一组不少于3的角度来描述.这三次旋转的转角称为一组欧拉角,因最早由欧拉(Leonhard ...
C语言实现方向余弦矩阵转旋转矢量
方向余弦矩阵转旋转矢量方向余弦矩阵旋转矢量源码方向余弦矩阵方向余弦矩阵(Direction Cosine Matrix,DCM)又被称为"坐标转换矩阵",常用于将矢量的投 ...

方向余弦矩阵与四元数

姿态解算

方向余弦矩阵

基本概念

已知欧拉角求方向余弦

已知方向余弦求欧拉角

四元数

基本概念

四元数表征姿态

<1四元数的共轭

<2四元数的逆

四元数与方向余弦

姿态表示的三种方法

欧拉角

方向余弦矩阵

四元数

spark−1spark-1spark−1

方向余弦矩阵与四元数相关推荐

最新文章

热门文章