INS/GNSS组合导航（十二）四元数的几点说明

scott198512

已于 2024-05-08 14:24:43 修改

阅读量417

点赞数 3

分类专栏： INS/GNSS组合导航文章标签：坐标系变换四元数哈密顿

于 2024-04-29 10:27:10 首次发布

本文链接：https://blog.csdn.net/scott198510/article/details/138304074

版权

INS/GNSS组合导航专栏收录该内容

18 篇文章 113 订阅

订阅专栏

1.概述

四元数的相关内容已经非常详尽，本文不再详细说明，重点是介绍四元数与旋转矩阵的转换关系。

简单来说，Quaternion（四元数）是一种常用的三维空间旋转的表示法。四元数由一个实部和三个虚部构成，写作如下形式：

$\mathbf{q}=q_1 i + q_2 j + q_3 k+q_0$

也有写成如下：

$\mathbf{q}=q_1 i + q_2 j + q_3 k +q_4$

或者按照旋转矩阵定义，

$\mathbf{q}=q_w + q_x i + q_y j + q_z k$

其中 $i, j, k$ 为三个基向量，满足一定的计算规则，类似复数中虚部。

具体来说，最早是有Hamilton于1843提出并定义了相关的计算法则。

计算法则符合右手系，定义如下：

$i^2=j^2=k^2=ijk=-1 \\ i\times j =k \\ j \times k=i \\ k \times i=j$

但是除此之外，还有按照左手系定义的JPL四元数，可参考Indirect Kalman Filter for 3D Attitude Estimation

两种定义形式的对比如下,具体信息可重点参考一篇对比四元数的经典文献：

Quaternion kinematics for the error-state KF

详细内容可参考以下几篇博客：

四元数旋转表达（Hamilton notation & JPL notation）

右手系转左手系、旋转矩阵转四元数、四元数的两种表达（Hamilton/JPL)

从四元数到旋转矩阵_四元数转换为旋转矩阵-CSDN博客

两种定义形式导致很容易混乱，本人推荐车载领域常用的右手系定义方式。具体可参考这篇文档：

四元数的两种 notation：Hamilton 和 JPL

PS：Hamilton四元数通常表达的是局部系local frame 比如载体系b系，到全局系global frame比如参考坐标系 n系的旋转，JPL四元数表达的是global 到local的旋转。两种情况下的旋转矩阵互为转置矩阵。

特别注意：① 对于不同文献的引用与转载，经常出现一篇文献中Hamilton 与JPL两种

形式混用，这是一种误读，说明使用者没有注意到两者的区别和来历.

②对于四元数的书写形式，通常有如下三种写法：

$\mathbf q=\begin{bmatrix} q_0 \\ q_1 \\ q_2 \\ q_3 \end{bmatrix} \quad \quad \quad \quad \quad \quad \quad \quad\quad\quad\quad(1)$

$\mathbf q=\begin{bmatrix} q_1 \\ q_2 \\ q_3 \\ q_4 \end{bmatrix}\quad \quad \quad \quad \quad \quad \quad \quad\quad\quad\quad(2)$

$\mathbf q=\begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix}\quad \quad \quad \quad \quad \quad \quad \quad\quad\quad\quad(3)$

前两种写法，并不能直观看出哪一部分表示了实部，尽管多数情况下，最后一项表示的实部，这也很容易导致混乱和引用错误，为了规范起见，本文采样对比文献中推荐的第三种形式，因为第三种书写时，默认 $q_w$ 表示的实部。

关于四元数的直观理解：对于一个向量 $\mathbf x$ 在2D平面内旋转一定的角度 $\theta$ ，可以表示为该向量与单位向量 $\mathbf z=e^{i\theta}$ 的点积，即 $\mathbf z'=\mathbf z \cdot \mathbf x$ ，相应地，对于3D空间向量的旋转：

$\mathbf x'=\mathbf q \otimes \mathbf x \mathbf \otimes \mathbf q^{*}$ ，其中 $\mathbf q=e^{(u_xi+u_yj+u_zk)\theta/2}$ , $\mathbf q^{*}$ 是 $\mathbf q^{}$ 的共轭四元数（实部相同，虚部取负）。

2.坐标系

2.1 坐标系定义

在定义相关的旋转关系时，必须先定义坐标系与绕x,y,z旋转的次序。在车载领域，通常使用的是右手系。这里定义车辆坐标系为右手系，前左上，坐标原点为车辆后轴中心，与泊车时的坐标系相同，具体如下

2.2 旋转次序

基于x轴、y轴和z轴的滚转、俯仰和偏航欧拉角 (roll, pitch, yaw)，如下图所示

3.转换关系

3.1 旋转矩阵 ->欧拉角

定义旋转次序分别为绕z,y,x旋转，对应的三个欧拉角 $yaw,pitch,roll$ 分别为 $\psi,\theta,\phi$ ，若以 $R_{n}^b$ 表示参考系（或者称导航系）到载体系（旋转前n-frame，到旋转后b-frame）的转换矩阵， $z(\psi)\rightarrow y(\theta)\rightarrow x(\phi)$ ，则旋转矩阵表示如下:

$R_{n}^b=R_{x}(\phi)R_{y}(\theta)R_{z}(\psi)$

也就是：

$R_{n}^b=\begin{bmatrix}1 &0&0\\0&cos(\phi)&sin(\phi)\\0&-sin(\phi)&cos(\phi)\end{bmatrix}\begin{bmatrix}cos(\theta) &0&-sin(\theta)\\0&1&0\\sin(\theta)&0&cos(\theta)\end{bmatrix}\begin{bmatrix}cos(\psi) &sin(\psi)&0\\-sin(\psi)&cos(\psi)&0\\0&0&1\end{bmatrix}$

为了与Hamilton四元数的表示一致，由 b ->n 得到：

$R_{b}^n=R_{z}(-\psi)R_{y}(-\theta)R_{x}(-\phi)={R_{n}^b}^T$

$R_{b}^n = \begin{bmatrix}c\theta c\psi & s\phi s\theta c\psi-c\phi s\psi & c\phi s\theta c\psi+s\phi s\psi\\ c\theta s\psi &s\phi s\theta s\psi +c\phi c \psi & c\phi s\theta s\psi-s\phi c\psi \\ -s\theta &s\phi c\theta & c\phi c \theta \end{bmatrix} =\begin{bmatrix} r_{11} & r_{12} & r_{13}\\ \\ r_{21} & r_{22} & r_{23} \\ \\r_{31} & r_{32} & r_{33}\end{bmatrix}$

解出得到：

$\begin{cases} \phi=arctan2(r_{32},r_{33})\\\\ \theta=-arcsin(r_{31})\\ \\\psi=arctan2(r_{21},r_{11}) \end{cases}$

3.2 欧拉角-> 旋转矩阵

根据旋转次序 $x(-\phi) \rightarrow y(-\theta)\rightarrow z(-\psi)$ ，可得到下面的旋转矩阵：

$R_{b}^n=R_{-z}(\psi)R_{-y}(\theta)R_{-x}(\phi)=\begin{bmatrix} cos\theta cos\psi & sin\phi sin\theta cos\psi-cos\phi sin\psi & cos\phi sin\theta cos\psi+sin\phi sin\psi\\ \\cos\theta sin\psi & sin\phi sin\theta sin\psi+cos\phi cos\psi & cos\phi sin\theta sin\psi - sin\phi cos\psi\\ \\ -sin\theta & sin\phi cos\theta & cos\phi cos\theta \end{bmatrix}$

3.3 四元数-> 旋转矩阵

根据四元数：

$\mathbf q=\begin{bmatrix} q_w \\ q_x \\ q_y \\ q_z \end{bmatrix}$

可根据Hamilton定义的四元数（参考系 -> 载体系），可得到旋转矩阵（具体可参考https://arxiv.org/pdf/1801.07478）：

按照之前规定的书写形式，可表示为：

$_{L}^G R_{Hamilton}=\begin{bmatrix} 1-2q_y^2-2q_z^2 & 2q_xq_y-2q_wq_z & 2q_xq_z+2q_wq_y\\\\ 2q_xq_y+2q_wq_z & 1-2q_x^2-2q_z^2 & 2q_yq_z-2q_wq_x \\\\ 2q_xq_z-2q_wq_y & 2q_yq_z+2q_wq_x & 1-2q_x^2-2q_y^2\end{bmatrix}$

若采用JPL定义的四元数，则

$_{G}^L R_{JPL}=\begin{bmatrix} 1-2q_y^2-2q_z^2 & 2q_xq_y+2q_wq_z & 2q_xq_z-2q_wq_y\\ \\2q_xq_y-2q_wq_z & 1-2q_x^2-2q_z^2 & 2q_yq_z+2q_wq_x \\ \\2q_xq_z+2wq_y & 2q_yq_z-2q_wq_x & 1-2q_x^2-2q_y^2\end{bmatrix}$

两种定义下，正好满足：

$R_{Hamilton}=R_{JPL}^T$

3.4 旋转矩阵-> 四元数

以Hamilton四元数为例：

$R_{Hamilton}=\begin{bmatrix} 1-2q_y^2-2q_z^2 & 2q_xq_y-2q_wq_z & 2q_xq_z+2q_wq_y\\\\ 2q_xq_y+2q_wq_z & 1-2q_x^2-2q_z^2 & 2q_yq_z-2q_wq_x \\\\ 2q_xq_z-2q_wq_y & 2q_yq_z+2q_wq_x & 1-2q_x^2-2q_y^2\end{bmatrix}$

也写成如下形式：

$R_{Hamilton}= \begin{bmatrix} r_{11} & r_{12} & r_{13}\\ \\ r_{21} & r_{22} & r_{23} \\ \\r_{31} & r_{32} & r_{33}\end{bmatrix}$

可以解出四元数：

$\begin{cases} w=\frac{\sqrt {tr(R_{Hamilton})+1}}2 \\ x=\frac {r_{32}-r_{23}} {4w} \\ y=\frac {r_{12}-r_{31}} {4w} \\ z=\frac {r_{21}-r_{12}} {4w} \end{cases}$

3.3 欧拉角-> 四元数

利用欧拉角也可以实现一个物体在空间的旋转，它按照既定的顺序，在全局导航系如依次绕Z,Y,X分别旋转一个固定角度，使用yaw，pitch，roll，分别表示车体绕 Z,Y,X的旋转角度，记为 $\psi, \theta,\phi$ ，若按照局部载体系依次则旋转 $-\phi, -\theta,-\psi$ ，可以利用三个四元数依次表示这三次旋转，即：

$Q_x = cos(\phi/2)+sin(-\phi/2)k \\\\ Q_y = cos(\theta/2)+sin(-\theta/2)j \\\\ Q_z = cos(\psi/2)+sin(-\psi/2)i \\\\$

由此，局部坐标系下逐次绕轴顺序X-Y-Z，就构造了一个单位四元数

$\mathbf q=Q_z * Q_y * Q_x =\begin{bmatrix} cos(\psi/2) \\ 0 \\ 0 \\ sin(\psi/2) \end{bmatrix}\otimes \begin{bmatrix} cos(\theta/2) \\ 0 \\ sin(\theta/2) \\ 0\end{bmatrix}\otimes \begin{bmatrix} cos(\phi/2) \\ sin(\phi/2) \\ 0 \\ 0 \end{bmatrix}$

计算法则参考如下(其中， $\mathbf p=[p_w,p_x,p_y,p_z]^T,\mathbf q=[q_w,q_x,q_y,q_z]^T$ )：

展开可以得到：

$\begin{cases} q_w=cos(\frac{\phi}{2})cos(\frac{\theta}{2})cos(\frac{\psi}{2})-sin(\frac{\phi}{2})sin(\frac{\theta}{2})sin(\frac{\psi}{2}) \\\\\\ q_x=sin(\frac{\phi}{2})cos(\frac{\theta}{2})cos(\frac{\psi}{2})+cos(\frac{\phi}{2})sin(\frac{\theta}{2})sin(\frac{\psi}{2}) \\\\\\ q_y=cos(\frac{\phi}{2})sin(\frac{\theta}{2})cos(\frac{\psi}{2})-sin(\frac{\phi}{2})cos(\frac{\theta}{2})sin(\frac{\psi}{2}) \\\\\\ q_z=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{cases}$

由此得到 $\mathbf q=[q_w,q_x,q_y,q_z]^T$ ,这里定义恒定的满足 $q_w\geqslant 0$ 。

对于旋转次序按照全局坐标系下逐次绕轴顺序X-Y-Z，得到

$\mathbf q= Q_z * Q_y * Q_x = \begin{bmatrix} cos(\phi/2) \\ sin(\phi/2) \\ 0 \\ 0 \end{bmatrix}\otimes \begin{bmatrix} cos(\theta/2) \\ 0 \\ sin(\theta/2) \\ 0\end{bmatrix}\otimes \begin{bmatrix} cos(\psi/2) \\ 0 \\ 0 \\ sin(\psi/2) \end{bmatrix}$

展开得到（第二项前面的符号与上面全部加 - 号）：

$\begin{cases}q_w=cos(\frac{\phi}{2})cos(\frac{\theta}{2})cos(\frac{\psi}{2})+sin(\frac{\phi}{2})sin(\frac{\theta}{2})sin(\frac{\psi}{2}) \\\\\\ q_x=sin(\frac{\phi}{2})cos(\frac{\theta}{2})cos(\frac{\psi}{2})-cos(\frac{\phi}{2})sin(\frac{\theta}{2})sin(\frac{\psi}{2}) \\\\\\ q_y=cos(\frac{\phi}{2})sin(\frac{\theta}{2})cos(\frac{\psi}{2})+sin(\frac{\phi}{2})cos(\frac{\theta}{2})sin(\frac{\psi}{2}) \\\\\\ q_z=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{cases}$

对于其它旋转次序，可参考 Jupyter Notebook Viewer (nbviewer.org)

3.2 四元数 -> 欧拉角

根据上述分析，对比欧拉角的计算与四元数表示的转换矩阵

$\begin{cases} \phi=arctan2(r_{23},r_{33})\\\\ \theta=-arcsin(r_{13})\\ \\\psi=arctan2(r_{12},r_{11}) \end{cases}$

由此可得到：

$\begin{cases} \phi=arctan2(2q_yq_z-2q_wq_x ,1-2q_x^2-2q_y^2 )\\\\ \theta=-arcsin(2q_xq_z+2q_wq_y )\\ \\\psi=arctan2( 2q_xq_y-2q_wq_z,1-2q_y^2-2q_z^2) \end{cases}$

4.代码实现

4.1 C++

static Eigen::Quaterniond rotationMatrix2Quaterniond(Eigen::Matrix3d R)  
{  
    Eigen::Quaterniond q = Eigen::Quaterniond(R);  
    q.normalize();  
    std::cout << “RotationMatrix2Quaterniond result is:” <<std::endl;  
    std::cout << ”x = ” << q.x() <<std::endl;  
    std::cout << ”y = ” << q.y() <<std::endl;  
    std::cout << ”z = ” << q.z() <<std::endl;  
    std::cout << ”w = ” << q.w() <<std::endl;  
    return q;  
} 



static Eigen::Vector3d rotationMatrix2EulerAngle(const Eigen::Matrix3d& R)
    {
        Eigen::Vector3d n = R.col(0);
        Eigen::Vector3d o = R.col(1);
        Eigen::Vector3d a = R.col(2);

        Eigen::Vector3d eulerAngle(3);
        double yaw = atan2(n(1), n(0));
        double pitch = atan2(-n(2), n(0) * cos(y) + n(1) * sin(yaw));
        double roll = atan2(a(0) * sin(yaw) - a(1) * cos(yaw), -o(0) * sin(yaw) + o(1) * cos(y));
        eulerAngle(0) = yaw;
        eulerAngle(1) = pitch;
        eulerAngle(2) = roll;

        return eulerAngle/ M_PI * 180.0;
    }

static Eigen::Matrix<typename Derived::Scalar, 3, 3> eulerAngle2rotationMatrix(const Eigen::MatrixBase<Derived> &angle)
    {
        typedef typename Derived::Scalar Scalar_t;

        Scalar_t yaw   = angle(0) / 180.0 * M_PI;
        Scalar_t pitch = angle(1) / 180.0 * M_PI;
        Scalar_t roll  = angle(2) / 180.0 * M_PI;

        Eigen::Matrix<Scalar_t, 3, 3> Rz;
        Rz << cos(y), -sin(y), 0,
            sin(y), cos(y), 0,
            0, 0, 1;

        Eigen::Matrix<Scalar_t, 3, 3> Ry;
        Ry << cos(p), 0., sin(p),
            0., 1., 0.,
            -sin(p), 0., cos(p);

        Eigen::Matrix<Scalar_t, 3, 3> Rx;
        Rx << 1., 0., 0.,
            0., cos(r), -sin(r),
            0., sin(r), cos(r);

        return Rz * Ry * Rx;
    }

Eigen::Matrix3d Quaternion2RotationMatrix(const double q_x, const double q_y,
                                          const double q_z, const double q_w) {
  Eigen::Quaterniond q;
  q.x() = q_x;
  q.y() = q_y;
  q.z() = q_z;
  q.w() = q_w;

  Eigen::Matrix3d R = q.normalized().toRotationMatrix();
  std::cout << "Quaternion2RotationMatrix result is :" << std::endl;
  std::cout << "R= "  << std::endl << R << std::endl << std::endl;
  return R;
}