526互联

角速度变化时四元数和旋转矩阵微分方程的证明

发布时间 2023-09-18 19:06:11作者: 找不到服务器zhn

摘要： 本文证明了在角速度向量不是常数时，四元数和旋转矩阵微分方程依然成立，成立的条件和性质等，最后给出仿真验证。

四元数微分方程的证明

  首先列出一些需要用到的四元数公式（基本知识见参考链接[1]）

\[ Q_1Q_2=[s_1,\vec v_1][s_2,\vec v_2] =[s_1s_2-\vec v_1\cdot\vec v_2,s_1\vec v_2+s_2\vec v_1+\vec v_1\times\vec v_2] \\ Q=[s,\vec v],\ Q^{-1}=[s,-\vec v],\ QQ^{-1}=[1,0] \\ \frac{\text dQ}{\text dt}Q^{-1}+Q\frac{\text d(Q^{-1})}{\text dt}=0 \]

四元数微分方程有两个等价形式，本文主要推导更常见的第一种形式

\[\begin{bmatrix} \dot{q}_0 \\ \dot{q}_1 \\ \dot{q}_2 \\ \dot{q}_3 \\ \end{bmatrix} =\frac{1}{2}\begin{bmatrix} 0 & -w_x & -w_y & -w_z \\ w_x & 0 & -w_z & w_y \\ w_y & w_z & 0 & -w_x \\ w_z & -w_y & w_x & 0 \end{bmatrix} \begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \]

当把角速度向量写成四元数形式（即 \(W=[0,\vec w]\)）时还可以写作

\[\dot Q(t)=\frac 12W(t)Q(t) \]

或写成向量形式

\[\begin{bmatrix} \dot s \\ \dot{\vec v} \end{bmatrix} =\frac{1}{2}\begin{bmatrix} 0 & -\vec w^\text T \\ \vec w & \vec w^\times \end{bmatrix} \begin{bmatrix} s \\ \vec v \end{bmatrix} \]

四元数旋转公式

  对任意单位四元数

\[Q=\left[\cos\frac\theta 2,\vec n\sin\frac\theta 2\right] \]

和任意向量 \(\vec v\in\mathbb R^3\)，下面的公式（或算子）

\[L_q(\vec v)=QVQ^{-1} \]

等价于将向量 \(\vec v\) 沿旋转轴 \(\vec n\) 旋转了 \(\theta\) 角度，其中 \(V=[0,\vec v]\) 表示向量 \(\vec v\) 的四元数形式。
证明： （见参考链接[2]）

正式推导

  这个推导来自参考链接[3]，与参考链接[4]相比我认为更简单，没有复杂的坐标系变换，只用了基本的求导，证明过程非常精彩。
  根据四元数旋转公式

\[R(t) = Q(t)R_0Q^{-1}(t) \]

其中 \(R(t)\) 表示向量 \(\vec R\) 的四元数形式，于是

\[\begin{aligned} \dot R(t) =& \dot Q(t)R_0Q^{-1}(t)+Q(t)R_0\frac{\text d}{\text dt}(Q^{-1}(t)) \\ =& \dot QQ^{-1}RQQ^{-1}+QQ^{-1}RQ\frac{\text d}{\text dt}(Q^{-1}) \\ =& \dot QQ^{-1}R+RQ\frac{\text d}{\text dt}(Q^{-1}) \\ =& \dot QQ^{-1}R - R\dot QQ^{-1} \end{aligned}\]

其中第1行到第2行是因为 \(R_0=Q^{-1}(t)R(t)Q(t)\)。因为单位四元数模长为1，等式两边对 \(t\) 求导

\[\begin{aligned} & q_0^2(t)+q_1^2(t)+q_2^2(t)+q_3^2(t)=1 \\ & 2q_0\dot q_0+2q_1\dot q_1+2q_2\dot q_2+2q_3\dot q_3=0 \end{aligned}\]

所以

\[\begin{aligned} \dot QQ^{-1} =& [\dot q_s,\dot{\vec q_v}][q_s,-\vec q_v] \\ =& [\dot q_sq_s+\dot{\vec q_v}\cdot\vec q_v, q_s\dot{\vec q_v}-\dot q_s\vec q_v-\dot{\vec q}_v\times\vec q_v] \\ =& [0, q_s\dot{\vec q_v}-\dot q_s\vec q_v-\dot{\vec q}_v\times\vec q_v] \end{aligned}\]

将 \(\dot QQ^{-1}\) 的向量部分简记作 \(\vec v_1=q_s\dot{\vec q_v}-\dot q_s\vec q_v-\dot{\vec q}_v\times\vec q_v\)（后面也不会再拆开了），继续推导

\[\begin{aligned} \dot R(t) =& [0,\vec v_1][0,\vec r]-[0,\vec r][0,\vec v_1] \\ =& [0,2\vec v_1\times\vec r] \end{aligned}\]

根据角速度公式得到

\[\dot{\vec r}=\vec w\times\vec r=2\vec v_1\times\vec r \]

因为该式对刚体上的任意向量 \(\vec r\) 都成立，所以必然有 \(\vec w=2\vec v_1\)，写成四元数形式为

\[W=2\dot QQ^{-1},\ \dot Q=\frac 12WQ \]

式中的 \(W\) 和 \(Q\) 都是四元数，与矩阵形式等价。

角速度不变的特殊情况

  角速度不变时，角速度向量与四元数的虚部方向相同

\[Q(t)=\begin{bmatrix} \cos\frac{\theta(t)}2 \\ i\sin\frac{\theta(t)}2 \\ j\sin\frac{\theta(t)}2 \\ k\sin\frac{\theta(t)}2 \end{bmatrix} =\begin{bmatrix} \cos\frac{wt}2 \\ \frac{w_x}w\sin\frac{wt}2 \\ \frac{w_y}w\sin\frac{wt}2 \\ \frac{w_z}w\sin\frac{wt}2 \end{bmatrix} =\begin{bmatrix} \cos\frac{wt}2 \\ \left(\frac 1w\sin\frac{wt}2\right)\vec w \end{bmatrix} \]

其中 \(w=\sqrt{w_x^2+w_y^2+w_z^2}\) 是角速度大小。

\[\begin{aligned} WQ(t) =& \begin{bmatrix} 0 & -w_x & -w_y & -w_z \\ w_x & 0 & -w_z & w_y \\ w_y & -w_z & 0 & -w_x \\ w_z & w_y & -w_x & 0 \end{bmatrix} \begin{bmatrix} \cos\frac{wt}2 \\ \left(\frac 1w\sin\frac{wt}2\right)\vec w \end{bmatrix} \\ =& \begin{bmatrix} -\left(\frac 1w\sin\frac{wt}2\right)\vec w^\text T\vec w \\ \vec w\cos\frac{wt}2 +\left(\frac 1w\sin\frac{wt}2\right)\vec w\times\vec w \end{bmatrix} \\ =& \begin{bmatrix} -w\sin\frac{wt}2 \\ \vec w\cos\frac{wt}2 \end{bmatrix} =\dot Q(t) \end{aligned}\]

但是当角速度变化时，也就是 \(\theta(t)\neq wt\) 时，这种关系不成立，角速度向量与四元数的虚部不再同向。

旋转矩阵微分方程的证明

旋转矩阵 \(R(t)\) 满足

\[\dot R(t)=w^\times(t)R(t) \]

角速度不变的推导

  这个推导来自参考文献[5]。书中是在假设角速度不变的前提下推导的，而且也没有说明角速度向量的实际含义。
  旋转矩阵 \(R(t)\) 是正交矩阵，满足

\[RR^\text T=I \]

等式两边同时对时间 \(t\) 求导得到

\[\dot RR^\text T+R\dot R^\text T=\dot RR^\text T+(\dot RR^\text T)^\text T =0 \]

可以看出 \(\dot R(t)R^\text T(t)\) 是个反对称矩阵，用 \(M(t)\) 表示。等式两边同时右乘 \(R(t)\) 得到

\[\dot R(t)R^\text T(t)=M(t) \\ \dot R(t)=M(t)R(t) \\ \]

式中的 \(M(t)\) 实际上就是角速度对应的反对称矩阵 \(w^\times\)，当角速度不变时，\(M(t)\) 为常数，旋转矩阵的解为

\[ \dot R(t)=w^\times R(t) \\ R(t)=\text e^{w^\times t}R_0 \]

例如，绕z轴的旋转矩阵为

\[ R=\begin{bmatrix} \cos t & -\sin t & 0 \\ \sin t & \cos t & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} \]

则

\[w^\times R(t)= \begin{bmatrix} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \begin{bmatrix} \cos t & -\sin t & 0 \\ \sin t & \cos t & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} =\begin{bmatrix} -\sin t & -\cos t & 0 \\ \cos t & -\sin t & 0 \\ 0 & 0 & 1 \\ \end{bmatrix} =\dot R(t) \]

角速度变化的推导

  当角速度变化时,旋转矩阵微分方程仍然成立。我想到的不严谨的证明方法是只需要证明角速度公式

\[\dot{\vec r}(t)=\vec\omega(t)\times\vec r(t) \]

成立即可（证明见参考链接[6]），然后旋转矩阵等价于矩阵中的3个正交坐标轴向量在惯性系下的坐标，因此当3个坐标轴成立时旋转矩阵就成立

\[\begin{bmatrix} \dot{\vec e_x} & \dot{\vec e_y} & \dot{\vec e_z} \end{bmatrix} =\vec\omega(t)\times \begin{bmatrix} \vec e_x & \vec e_y & \vec e_z \end{bmatrix} \]

不知道有没有严谨证明。

参考

  1. Understanding Quaternions -3D Game Engine Programming
  2. Quaternions and Rotations
  3. Quaternion differentiation -euclideanspace
  4. 四元数微分方程的推导和解算实现
  5. 高博. 视觉SLAM十四讲[M]. 电子工业出版社.
  6. How to derive the formula for angular velocity? -stackexchange

仿真验证

  下面的验证程序的思路是，同时对四元数和旋转矩阵两个微分方程

\[\begin{aligned} & \dot R(t)=w^\times(t)R(t) \\ & \dot Q(t)=\frac 12W(t)Q(t) \\ \end{aligned}\]

输入变化的角速度（可以自己另外设置）

\[ \vec w(t)= \begin{bmatrix} 1 \\ \ln(t+1) \\ \cos(t) \end{bmatrix} \]

在 \(t=1\) 到 \(10\) 的10个时刻时，将输出的四元数 \(Q(t)\) 转换成旋转矩阵 \(R_q(t)\) 与输出的旋转矩阵 \(R(t)\) 比较，可以验证两者相等。
  同时可以进一步验证，当角速度不是常数时，四元数的虚部与角速度向量之间我没发现有任何关系。
simucpp 代码如下

/**************************************************************
// 验证四元数和旋转矩阵微分方程
simucpp版本：2.1.14
**************************************************************/
#include <iostream>
#include <cmath>
#include "simucpp.hpp"
using namespace simucpp;
using namespace zhnmat;
using namespace std;

int main() {
    Simulator sim1;
    auto intR = new MStateSpace(&sim1, BusSize(3, 3), true, "intR");  // 旋转矩阵R
    auto intQ = new MStateSpace(&sim1, BusSize(4, 1), true, "intQ");  // 四元数Q
    auto misoR = new MFcnMISO(&sim1, BusSize(3, 3), "misoR");  // 旋转矩阵导数R'
    auto misoQ = new MFcnMISO(&sim1, BusSize(4, 1), "misoQ");  // 四元数导数Q'
    auto muxw = new Mux(&sim1, BusSize(3, 1), "muxw");  // 角速度向量w
    UInput **inws = new UInput*[3];  // 三轴角速度
    for (uint32_t i = 0; i < 3; i++) {
        inws[i] = new UInput(&sim1, "inw"+to_string(i));
        sim1.connectU(inws[i], muxw, BusSize(i, 0));
    }
    sim1.connectM(muxw, misoR);
    sim1.connectM(intR, misoR);
    sim1.connectM(misoR, intR);
    sim1.connectM(muxw, misoQ);
    sim1.connectM(intQ, misoQ);
    sim1.connectM(misoQ, intQ);
    inws[0]->Set_Function([](double t){ return 1; });  // 设置三轴角速度函数
    inws[1]->Set_Function([](double t){ return log(t+1); });
    inws[2]->Set_Function([](double t){ return cos(t); });
    intR->Set_InitialValue(eye(3));
    intQ->Set_InitialValue(Mat(vecdble{1, 0, 0, 0}));
    misoR->Set_Function([](Mat *u){  // u[0]为角速度向量，u[1]为旋转矩阵
        Mat W(3, 3, vecdble{  // 角速度向量对应的反对称矩阵
            0, -u[0].at(2, 0), u[0].at(1, 0),
            u[0].at(2, 0), 0, -u[0].at(0, 0),
            -u[0].at(1, 0), u[0].at(0, 0), 0,
        });
        return W*u[1];
    });
    misoQ->Set_Function([](Mat *u){  // u[0]为角速度向量，u[1]为四元数
        Mat W(4, 4, vecdble{  // 角速度向量对应的四元数乘法左矩阵
            0, -u[0].at(0, 0), -u[0].at(1, 0), -u[0].at(2, 0),
            u[0].at(0, 0), 0, -u[0].at(2, 0), u[0].at(1, 0),
            u[0].at(1, 0), u[0].at(2, 0), 0, -u[0].at(0, 0),
            u[0].at(2, 0), -u[0].at(1, 0), u[0].at(0, 0), 0,
        });
        return 0.5*W*u[1];
    });
    sim1.Set_SimStep(0.01);  // 步长0.01
    sim1.Initialize();
    double q0, q1, q2, q3;
    Mat matr, matq, matqr, materr;
    double sumerr;
    for (uint32_t i = 0; i < 10; i++) {
        for (uint32_t j = 0; j < 100; j++)  // 仿真100步为1秒
            sim1.Simulate_OneStep();
        matr = intR->Get_OutValue();  // 旋转矩阵
        matq = intQ->Get_OutValue();  // 四元数
        q0 = matq.at(0, 0);
        q1 = matq.at(1, 0);
        q2 = matq.at(2, 0);
        q3 = matq.at(3, 0);
        matqr = Mat(3, 3, vecdble{  // 四元数转旋转矩阵
            q0*q0+q1*q1-q2*q2-q3*q3, 2*q1*q2-2*q0*q3, 2*q1*q3+2*q0*q2,
            2*q1*q2+2*q0*q3, q0*q0-q1*q1+q2*q2-q3*q3, 2*q2*q3-2*q0*q1,
            2*q1*q3-2*q0*q2, 2*q2*q3+2*q0*q1, q0*q0-q1*q1-q2*q2+q3*q3,
        });
        materr = matr - matqr;  // 两个旋转矩阵的误差矩阵
        sumerr = 0;  // 误差平方和
        for (uint32_t i = 0; i < 3; i++)
            for (uint32_t j = 0; j < 3; j++)
                sumerr += materr.at(i, j);
        cout << sumerr << endl;
    }
    return 0;
}