四元数的微分形式

最新推荐文章于 2025-06-04 22:57:57 发布

Aromash

最新推荐文章于 2025-06-04 22:57:57 发布

阅读量6.7k

点赞数 15

CC 4.0 BY-SA版权

分类专栏：飞控算法文章标签：四元数微分算法

本文链接：https://blog.youkuaiyun.com/qq_39554681/article/details/88909564

飞控算法专栏收录该内容

11 篇文章

订阅专栏

四元数微分形式的推导

关于四元数表示旋转的一些知识

1.四元数表示向量旋转
定义 $pv=[pxpypz]Tp_v=\left[\begin{array}{ccc}{p_{x}} & {p_{y}} & {p_{z}}\end{array}\right]^T$ 为三维空间中的一点，将其转换成纯四元数形式即： $p=[0pv]Tp=\left[\begin{array}{cc}0 & {p_{v}}\end{array}\right]^T$ ，令 $q=[cos(θ2)vsin(θ2)]Tq=\left[\begin{array}{cc}{cos(\frac{\theta}{2})} & {v sin(\frac{\theta}{2})}\end{array}\right]^T$ 为单位四元数，则有 $p′=q⋅p⋅q−1=[0pv′]Tp^{\prime} =q \cdot p \cdot q^{-1}=\left[\begin{array}{cc}0 & {p^{\prime}_{v}}\end{array}\right]^T$ 其中 $pv′=[px′py′pz′]Tp^{\prime}_{v}=\left[\begin{array}{ccc}{p^{\prime}_{x}} & {p^{\prime}_{y}} & {p^{\prime}_{z}}\end{array}\right]^T$ 表示 $p_v$ 绕旋转轴 $v$ 旋转 $θ\theta$ 角度后得到的新向量在原三维空间中的坐标表示。

2.四元数表示坐标系旋转
定义向量 $v_0$ 在 $o x y z$ 坐标系中的表示为 $v1=[vxvyvz]Tv_1=\left[\begin{array}{ccc}{v_{x}} & {v_{y}} & {v_{z}}\end{array}\right]^T$ ，令坐标系 $o x y z$ 绕单位旋转轴 $v$ 旋转 $θ\theta$ 角度，得到新坐标系 $o′x′y′z′o^{\prime}x^{\prime}y^{\prime}z^{\prime}$ ，此时 $v_0$ 在新坐标系中的坐标表示为 $v1′=[vx′vy′vz′]Tv^{\prime}_1=\left[\begin{array}{ccc}{v^{\prime}_{x}} & {v^{\prime}_{y}} & {v^{\prime}_{z}}\end{array}\right]^T$ 。那么，向量 $v_0$ 在两个坐标系之间的坐标转换关系为：
$[0v1′]T=q−1⋅[0v1]T⋅q\left[\begin{array}{cc}0 & {v^{\prime}_{1}}\end{array}\right]^T=q^{-1} \cdot \left[\begin{array}{cc}0 & {v_{1}}\end{array}\right]^T \cdot q$
其中
$q=[cos(θ2)vsin(θ2)]Tq=\left[\begin{array}{cc}{cos(\frac{\theta}{2})} & {v sin(\frac{\theta}{2})}\end{array}\right]^T$
值得注意的是，上述两种四元数表示旋转的方式得到的四元数旋转关系是不同的，这是因为1.表示向量在同一个坐标系中的旋转，而2.中旋转的是坐标系。

以1.为例，先做 $q_1$ 旋转，再做 $q_2$ 旋转之后向量 $x$ 在新坐标系中的表示为 $x′x^{\prime}$ ，则有：
$xtemp=q1⋅x⋅q1−1x_{temp} = q_1 \cdot x \cdot {q_1}^{-1}$
$x′=q2⋅xtemp⋅q2−1x^{\prime} = q_2 \cdot x_{temp} \cdot {q_2}^{-1}$
$x′=q2q1⋅xtemp⋅q2q1−1x^{\prime} = {q_2 q_1} \cdot x_{temp} \cdot {q_2 q_1}^{-1}$

四元数 $q_2 q_1$ 表示了连续两次的旋转。

以2.为例，先做 $q_1$ 旋转，再做 $q_2$ 旋转之后向量 $x$ 在新坐标系中的表示为 $x′x^{\prime}$ ，则有：
$xtemp=q1−1⋅x⋅q1x_{temp} = q_1^{-1} \cdot x \cdot {q_1}$
$x′=q2−1⋅xtemp⋅q2x^{\prime} = q_2^{-1} \cdot x_{temp} \cdot {q_2}$
$x′=q1q2−1⋅xtemp⋅q1q2x^{\prime} = {q_1 q_2}^{-1} \cdot x_{temp} \cdot {q_1 q_2}$

四元数 $q_1 q_2$ 表示了连续两次的旋转。

四元数微分形式推导

以无人机的姿态表示为例子，我们定义单位四元数 $q (t)$ 来表示从无人机的地理系E到机体系B的旋转关系。在 $t+Δtt+\Delta t$ 时刻，旋转可表示为 $q(t+Δt)q(t+\Delta t)$ ；即在 $Δt\Delta t$ 过程中，机体坐标系又经过了一个微小旋转，这个微小旋转的瞬时旋转角速度为 $w$ ；接着对瞬时旋转轴做单位化处理 $w^=w/∣∣w∣∣\hat{w}=w/||w||$ 在 $Δt\Delta t$ 时刻转过的角度为 $Δθ=Δt∣∣w∣∣\Delta \theta =\Delta t||w||$ 则这次的微小旋转可由如下形式的单位四元数表示：

$Δq=cos⁡Δθ2+sin⁡Δθ2w^=cos⁡∣∣w∣∣2Δt+sin⁡∣∣w∣∣2Δtw^\Delta q=\cos \frac{\Delta \theta}{2}+\sin \frac{\Delta \theta}{2} \hat{w} = \cos \frac{||w||}{2} \Delta t + \sin \frac{||w||}{2} \Delta t \hat{w}$

假定地理系到机体系的旋转四元数为 $qeb=[q0q1q2q3]Tq^b_e =\left[\begin{array}{cccc}q_0 & q_1 & q_2 & q_3 \end{array}\right]^T$ ；
则根据上述2.有
$[0er]T=(qbe)−1[0br]Tqbe\left[\begin{array}{cc}0 & ^er \end{array}\right]^T=(q^e_b)^{-1} \left[\begin{array}{cc}0 & ^br \end{array}\right]^T q^e_b \\$
即(注意：此处转化为与1.相同的表示形式，但两者的物理意义不同)：
$[0er]T=(qeb)[0br]T(qeb)−1\left[\begin{array}{cc}0 & ^er \end{array}\right]^T=(q^b_e) \left[\begin{array}{cc}0 & ^br \end{array}\right]^T (q^b_e)^{-1}$

那么根据上面的推导，连续两次的旋转可以表示为： $q(t+Δt)=Δq⋅qq(t+\Delta t)=\Delta q \cdot q$ ；则有：

$q(t+Δt)−q(t)=(cos⁡∣∣w∣∣2Δt+sin⁡∣∣w∣∣2Δtw^)q(t)−q(t)=−2sin⁡2∣∣w∣∣2Δtq(t)+w^sin⁡∣∣w∣∣2Δtq(t)q(t+\Delta t)-q(t)=(\cos \frac{||w||}{2} \Delta t + \sin \frac{||w||}{2} \Delta t \hat{w})q(t)-q(t) \\ =-2{\sin ^2 \frac{||w||}{2}} \Delta t q(t) + \hat{w} \sin \frac{||w||}{2} \Delta t q(t)$

略去高阶项可得：
$q(t+Δt)−q(t)=w^sin⁡∣∣w∣∣2Δtq(t)q(t+\Delta t)-q(t)= \hat{w} \sin \frac{||w||}{2} \Delta t q(t)$
$q(t)˙=lim⁡Δt→0q(t+Δt)−q(t)Δt=lim⁡Δt→0w^sin⁡∣∣w∣∣2Δt⋅q(t)=12w^⋅∥w∥⋅q(t)=12⋅w⋅q(t)\dot{q(t)}=\lim _{\Delta t \rightarrow 0} \frac{q(t+\Delta t)-q(t)}{\Delta t} =\lim _{\Delta t \rightarrow 0} \frac{\hat{w} \sin \frac{||w||}{2}}{\Delta t}\cdot q(t) \\ =\frac{1}{2} \hat{w} \cdot\|w\| \cdot q(t) \\ = \frac{1}{2} \cdot w \cdot q(t)$

注意：此处的 $⋅\cdot$ 表示四元数乘法； $w$ 为角速度的纯四元数表示， $w=[0wxwywz]Tw=\left[\begin{array}{cccc}{0} & {w_{x}} & {w_{y}} & {w_{z}}\end{array}\right]^T$ ;

由于实际工程中我们都是通过固连在机体上的陀螺仪等传感器来获知机体角速度 $w^b$ 它与地理坐标系下的角速度表示有如下关系 $w=q(t)w^bq(t)^{-1}$ 带入上式即可得到姿态解算过程中常用的四元数的微分形式 $q˙(t)=12q(t)wb\dot q(t)=\frac {1} {2}q(t)w^b$
可以看出，通过一次四元数乘法运算便可得到四元数的微分。
上式可以写成如下的矩阵形式：

$[q˙0q˙1q˙2q˙3]=12[q0−q1−q2−q3q1q0−q3q2q2q3q0−q1q3−q2q1q0][0ωxbωybωzb]\left[\begin{array}{c}{\dot{q}_{0}} \\ {\dot{q}_{1}} \\ {\dot{q}_{2}} \\ {\dot{q}_{3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q_{0}} & {-q_{1}} & {-q_{2}} & {-q_{3}} \\ {q_{1}} & {q_{0}} & {-q_{3}} & {q_{2}} \\ {q_{2}} & {q_{3}} & {q_{0}} & {-q_{1}} \\ {q_{3}} & {-q_{2}} & {q_{1}} & {q_{0}}\end{array}\right]\left[\begin{array}{c}{0} \\ {\omega_{x}^{b}} \\ {\omega_{y}^{b}} \\ {\omega_{z}^{b}}\end{array}\right]$

或者：

$[q˙0q˙1q˙2q˙3]=12[0−ωxb−ωyb−ωzbωxb0ωzb−ωybωyb−ωzb0ωxbωzbωyb−ωxb0][q0q1q2q3]\left[\begin{array}{l}{\dot{q}_{0}} \\ {\dot{q}_{1}} \\ {\dot{q}_{2}} \\ {\dot{q}_{3}}\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{0} & {-\omega_{x}^{b}} & {-\omega_{y}^{b}} & {-\omega_{z}^{b}} \\ {\omega_{x}^{b}} & {0} & {\omega_{z}^{b}} & {-\omega_{y}^{b}} \\ {\omega_{y}^{b}} & {-\omega_{z}^{b}} & {0} & {\omega_{x}^{b}} \\ {\omega_{z}^{b}} & {\omega_{y}^{b}} & {-\omega_{x}^{b}} & {0}\end{array}\right]\left[\begin{array}{l}{q_{0}} \\ {q_{1}} \\ {q_{2}} \\ {q_{3}}\end{array}\right]$

四元数与旋转矩阵的关系

我们知道，从地理坐标系到机体坐标系的旋转矩阵可以有如下形式表示：（一般均为zyx顺序旋转）

$R^b_e=R_{X} \cdot R_{Y} \cdot R_{z}=\left[ \begin{array}{cc}{\cos \theta \cos \psi} & {\cos \theta \sin \psi} & {-\sin \theta} \\ {\sin \phi \sin \theta \cos \psi-\cos \phi \sin \psi} & {\sin \phi \sin \theta \sin \psi+\cos \phi \cos \psi} & {\sin \phi \cos \theta} \\ {\cos \phi \sin \theta \cos \psi+\sin \phi \sin \psi} & {\cos \phi \sin \theta \sin \psi-\sin \phi \cos \psi} & {\cos \phi \cos \theta}\end{array}\right]$

从机体系到地理系的由四元数表示的旋转矩阵为

$Rbe=[1−2q22−2q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22]R^e_b=\left[ \begin{array}{lll}{1-2 q_{2}^{2}-2 q_{3}^{2}} & {2 q_{1} q_{2}-2 q_{0} q_{3}} & {2 q_{1} q_{3}+2 q_{0} q_{2}} \\ {2 q_{1} q_{2}+2 q_{0} q_{3}} & {1-2 q_{1}^{2}-2 q_{3}^{2}} & {2 q_{2} q_{3}-2 q_{0} q_{1}} \\ {2 q_{1} q_{3}-2 q_{0} q_{2}} & {2 q_{2} q_{3}+2 q_{0} q_{1}} & {1-2 q_{1}^{2}-2 q_{2}^{2}}\end{array}\right]$
则有 $Rbe=RebTR^e_b={R^b_e} ^T$ 再根据对应项相等，即可求出由四元数表示的欧拉角

$\phi=\arctan 2\left(2\left(q_{2} q_{3}+q_{0} q_{1}\right),\left(1-2\left(q_{1}^{2}+q_{2}^{2}\right)\right)\right)$

$\theta=-\arcsin \left(2\left(q_{1} q_{3}-q_{0} q_{2}\right)\right)$

$\psi=\arctan 2\left(2\left(q_{1} q_{2}+q_{0} q_{3}\right),\left(1-2\left(q_{2}^{2}+q_{3}^{2}\right)\right)\right)$

同样的，由欧拉角表示的四元数如下：

$q==\left[\begin{array}{c}{\cos \frac{\phi }{2} \cos \frac{\theta }{2} \cos \frac{\psi}{2}+\sin \frac{\phi }{2} \sin \frac{\theta }{2} \sin \frac{\psi}{2}} \\ {\sin \frac{\psi}{2} \cos \frac{\psi}{2}-\cos \frac{\phi }{2} \sin \frac{\theta }{2} \sin \frac{\psi}{2}} \\ {\cos \frac{\phi }{2} \sin \frac{\theta }{2} \cos \frac{\psi}{2}+\sin \frac{\phi }{2} \cos \frac{\theta }{2} \sin \frac{\psi}{2}} \\ {\cos \frac{\phi }{2} \cos \frac{\theta }{2} \sin \frac{\psi}{2}-\sin \frac{\phi }{2} \sin \frac{\theta }{2} \cos \frac{\psi}{2}}\end{array}\right]$

上式中 $ϕ,θ,ψ\phi ,\theta , \psi$ 分别表示绕机体三轴转动的欧拉角;

欧拉角表示法的弊端

欧拉角速率与机体角速度之间的关系如下式所示：

$\left[\begin{array}{c}{\dot{\phi}} \\ {\dot{\theta}} \\ {\dot{\psi}}\end{array}\right]=\left[\begin{array}{ccc}{1} & {\sin \phi \tan \theta} & {\cos \phi \tan \theta} \\ {0} & {\cos \phi} & {-\sin \phi} \\ {0} & {\sin \phi / \cos \theta} & {\cos \phi / \cos \theta}\end{array}\right]\left[\begin{array}{l}{\boldsymbol{p}} \\ {\boldsymbol{q}} \\ {\boldsymbol{r}}\end{array}\right]$