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

原创 已于 2024-12-10 20:43:59 修改 · 731 阅读 · 6 ·
CC 4.0 BY-SA版权
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
文章标签：

#坐标系变换 #四元数 #哈密顿

于 2024-04-29 10:27:10 首次发布
本文详细介绍了四元数在三维旋转表示中的应用，包括其与旋转矩阵、欧拉角的转换关系，特别区分了右手系和左手系的定义，并提供了C++和Python代码示例。

摘要生成于 C知道 ，由 DeepSeek-R1 满血版支持， 前往体验 >

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)

从四元数到旋转矩阵_四元数转换为旋转矩阵-优快云博客

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

四元数的两种 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)

   &nbs

最低0.47元/天 解锁文章

200万优质内容无限畅学
北斗导航 ｜ 载体滚转角从0度很快转到30度，然后保持30度滚转角飞行。在这个过程中，通过松组合得到姿态角，俯仰角误差为什么会增大后保持不变，或不断增大呢
尘世冰封的专栏
06-30 166
这是一个很经典的问题，涉及到惯性导航和组合导航的动态特性。
imu单次测量离散积分代码
闻涛观
03-09 719
这里给出imu积分代码， 所算的tmp_Q， tmp_P， tmp_V在 /world 坐标系下 Eigen::Vector3d tmp_P; Eigen::Quaterniond tmp_Q; Eigen::Vector3d tmp_V; Eigen::Vector3d tmp_Ba;//bias可以优化计算，也可以用工程化方法得出 Eigen::Vector3d tmp_Bg; Eige...
惯性组合导航原理—[2] 用四元数表征旋转
zytjasper的博客
09-15 1939
本文重点：四元数与反对称阵的定义，利用四元数表征旋转关系。 1.1 为什么要引入四元数？ 1.2 什么是四元数？ 1.3 反对称阵的作用及定义 1.4 四元数表征旋转关系 注意：公式（1），公式（4），公式（8）为重点。 1.1 为什么要引入四元数？ 在表征机体的运动时，使用欧拉角按照一定的旋转顺序可以让机体旋转至任意目标角度，但是会造成万向节死锁（Gimbal Lock） ...
四元数矩阵与 so(3) 左右雅可比
m0_37870385的博客
09-29 2566
我们知道，单位四元数 q 和 so(3) 向量 （即 rotation vector）的对应关系为： , in which 当 很小时，可以近似表达为： 四元数小量和李代数小量有很简单的对应关系，所以在使用四元数的优化问题中，往往也取 小量作为更新量，例如 OKVIS [1]。具体到优化过程中，使用李群李代数（如预积分论文 [2]）和使用四元数之间还是有区别的。使用李群李代数，在 residual 推导时会用到 so3 的左雅可比或右雅可比；使用四元数，会用到四元数矩阵（或共轭四元数矩阵）。不
四元数运动学在误差状态卡尔曼滤波中的应用：提升导航精度的关键技术
最新发布
gitblog_06748的博客
05-28 344
四元数运动学在误差状态卡尔曼滤波中的应用：提升导航精度的关键技术 去发现同类优质开源项目:https://gitcode.com/ 项目介绍 在当今高科技时代，导航系统的精度和稳定性对于各类应用至关重要。本文档以《Quaternion kinematics for the error-state Kalman filter》为基础，深入解析四元数运动学在误差状态卡尔曼滤波中的运用。四元数作为一种高...
惯性导航之认识四元数（四）
热门推荐
幻世
10-11 1万+
之前说到，使用欧拉角积分和方向余弦积分求得角度并不合适，而比较合适的是四元数。 在讨论「四元数」之前，我们来想想对三维直角坐标而言，在物体旋转会有何影响，可以扩充三维直角坐标系统的旋转为三角度系统（Three-angle system），在Game Programming Gems中有提供这么一段： Quaternions do not suffer from gimbal lo
【组合导航】四元数的运算及四元数微分理解
RTK学习者
12-26 4065
前言 在导航中一般都具有两套坐标系，载体坐标系与世界坐标系，联系两套坐标系的是用旋转向量，但是旋转向量存在着奇异性，大多使用四元数，记录下四元数的运算及四元数微分方程。 四元数运算 四元数的表达式为： q=q0+q1i+q2i+q3iq=q_0+q_1i+q_2i+q_3iq=q0​+q1​i+q2​i+q3​i 其中ijk为三个虚部，三个虚部满足以下关系式： {i2=j2=k2=−1ij=k,j...
组合导航——欧拉角、姿态阵、四元数的转换关系
liiiuzy的博客
01-05 1590
公式来源于西北工业大学严恭敏老师的《捷联惯导算法与组合导航原理》代码来自西北工业大学严恭敏老师的PSINS开源代码。
INS/GNSS组合导航（二）-INS常用的坐标系统及转换
weixin_45030158的博客
05-06 4530
一、常用坐标系 由于INS是一种完全自主的导航系统，不与外界发生联系。但是我们又想知道物体的的位置，尤其是其与GNSS组合时需要知道其与GNSS在同一坐标系下的位置信息，这就牵扯到了INS的几个导航系统之间的转换。笼统的讲，INS输出的是载体坐标系下的比力和旋转角速度，我们需要将这种输出信息转换到我们常用的如惯性系或者地固系。因此本节主要介绍INS相关的几个常用坐标系及相互之间的转换关系。 1. 惯性系 惯性坐标系（用 i 表示），主要是指在空间固定或者常速运动的坐标下，它提供惯性传感器输出的参考基准。在G
INS/GNSS组合导航（三）-SINS的机械编排
weixin_45030158的博客
06-22 3836
上节介绍了GNSS/INS组合时常用的几种坐标系及其之间的转换，这节就介绍一下SINS的机械编排。SINS即为捷联惯导系统，所谓的机械编排，即为从INS的加速度计和陀螺仪的真实输出值计算推导载体所在位置、速度和姿态的过程。 前言 在进行正式解算之前，我们需要明确，INS的三轴加速度计和三轴陀螺仪到底输出的什么信息。三轴加速度计输出的信息叫比力。何为比力？即假如载体自由落体时，真正的加速度是载体z方向上的重力加速度，但是实际上的加速度计并不能测量中立加速度。所以将这种不包含重力加速度的加速度测量值就称为比力。
惯导与组合导航笔记
qq_27377339的博客
04-27 4524
惯导与组合导航笔记 主要学习行人航位推算相关算法，但相关工作有点难找，遂重新复习学校开设的组合导航算法以及实现相关算法，同时参考了严老师的matlab工具箱以及讲义，现在记录相关笔记，为个人解读，若有问题，请批评改正。 一、 惯性导航相关 1.1 惯性仪器介绍 惯性仪器主要包括加速度计以及陀螺仪。 加速度计目前原理仅有一种，即通过测量由压力导致的各类物理值的变化来获取载体的比力值。其中物理值的测量主要为测量电压值。 陀螺仪主要有两类原理，一类为机械转子陀螺，即通过测量单位时间内机械转子的弧度值来获取载
三轴陀螺仪解算姿态（四元数）
Hugh_W的博客
03-30 5302
三轴陀螺仪解算姿态（四元数形式）
（11）四元数与欧拉角的转换
weixin_45729043的博客
09-10 3210
Table of Contents 0、简介 一、四元数的定义 二、欧拉角到四元数的转换 2.1 公式： 2.2 code: 三、四元数到欧拉角的转换 3.1 公式 3.2 code: 3.3四元素到旋转矩阵转换 四.奇点 五. 矢量旋转 证明： 六 . 其他参考 七 python 四元素欧拉角互相转换 8 Eigen transform 欧拉角到四元素 四元素得到yaw 四元素到旋转向量 旋转轴向量到四元素 0、简介 四元数与欧拉角之间的转换 百度百科四元素..
惯性导航算法(四)-四元数+姿态四元数
十八与她的博客
05-18 8556
文章目录四元数四元数的引出四元数的定义四元数的表示方式四元数的运算法则四元数的三角函数表示法四元数的旋转变换/算子欧拉旋转定理– 坐标系的等效转动姿态四元数姿态四元数的微分方程姿态四元数微分方程的求解四元数和DCM姿态更新的问题解决该问题的方案 四元数 四元数的引出 1.上述运算相当于将z 表示的平面向量旋转了一个角度𝜙 。 类似地，特定形式的复数（四元数）也可用来表示三维空 间中向量的旋转。(乘的时候，把长度自由度消掉，也就是模定义成1) 2.复数由两个自由度，它可以看作是二元数；扩展到三维空间时，由二
四元数运动学笔记（3）四元数和旋转相关的约定表述
weixin_30542079的博客
02-10 668
1.四元数的约定表述1.1 四元数表述的差异1.2 Hamilton vs JPL1.2.1元素的顺序1.2.2 左手系和右手系1.2.3 旋转操作的对象1.2.4 旋转操作的方向1.3 文章采用的表述1.4扰动和时间导数1.4.1右扰动和左扰动1.4.2 Hamilton表示下的（L-G）的四元数时间导数1.4.3 其他有用的表述方式1.5 barfoot书中表述2.旋转角速率表示旋转积分2....
四元数快速入门【Quaternion】
新缸中之脑
05-06 8982
四元数（Quaternion）是用于旋转和拉伸向量的数学运算符。本文提供了一个概述，以帮助理解在空间导航等应用程序中对四元数的需求。推荐：用快速搭建3D场景。可以通过多种方式在空间中准确定位、移动和旋转物体。更熟悉和更容易可视化的滚动（Roll）、俯仰（Pitch）和偏航（Yaw）表示是有局限性的，在某些情况下应该用更强大的四元数代替。随着对象的位置和方向发生变化，称为四元数的数学工具可用于旋转和缩放原始矢量。
VINS fusion软件架构分析（3）--- 输入IMU和相机信息 inputIMU + inputImage
weixin_40084095的博客
05-16 1249
1. 如何使用位姿估计器 从上一节，我们知道主函数rosNodeTest.cpp从IMU和相机结点获取了IMU和相机的信息，并传入到位姿估计器 estimator中，使用的函数分别是 inputIMU inputImage inputFeature 1.1 inputIMU 把时间戳 + 线加速度合并成pair放入buffer里 把时间戳 + 角加速度合并成pair放入buffer里 solver_flag == NON_LINEAR表示初始化完成了 认为初始化完成后， 调用fastPredic
右手系转左手系、旋转矩阵转四元数、四元数的两种表达（Hamilton/JPL)
Barry_123的博客
05-02 9068
右手系转左手系、旋转矩阵转四元数、四元数的两种表达：Hamilton/JPL右手系转左手系旋转矩阵转四元数四元数的两种表达：Hamilton/JPL两种转换代码 最近一个项目需要使用unity做可视化，由于unity使用左手系而我的代码中都是使用右手系，在转化的过程中踩了很多坑，简单记录一下。 右手系转左手系 此处参考这篇博文：从左手坐标系到右手坐标系的变换 左手系转右手系，简单来说就是反转其中一个轴即可。对于坐标点直接取反，以 z 轴取反为例： 式中S_z为： 而对于旋转的处理稍有不同：假设左手系中旋
INS 点云转 ECEF
03-17
### 将 INS 点云数据转换为 ECEF 坐标系的方法 要将点云数据从 INS 坐标系转换到地球中心地球固定 (ECEF) 坐标系，需要考虑以下几个关键因素： #### 1. 外参数校正 在外参标定过程中，描述扫描仪坐标系与惯导坐标系之间的位置关系非常重要。这包括三个安置角和三个安置距离分量[^3]。这些外参用于补偿安装误差，特别是在安置角上的偏差会对最终点云精度产生显著影响。 因此，在进行坐标变换之前，必须先对外部参数进行精确标定并应用修正矩阵 \( T_{\text{scanner-to-INS}} \)，该矩阵可以表示为： ```python import numpy as np def get_transformation_matrix(angles, distances): """ 构建从扫描仪到INS坐标的转换矩阵。 参数: angles: 安置角度 [roll, pitch, yaw] distances: 安置距离 [dx, dy, dz] 返回: transformation_matrix: 4x4 变换矩阵 """ roll, pitch, yaw = angles dx, dy, dz = distances R_x = np.array([[1, 0, 0], [0, np.cos(roll), -np.sin(roll)], [0, np.sin(roll), np.cos(roll)]]) R_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]]) R_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]]) rotation_matrix = R_z @ R_y @ R_x translation_vector = np.array([dx, dy, dz]).reshape((3, 1)) transformation_matrix = np.eye(4) transformation_matrix[:3, :3] = rotation_matrix transformation_matrix[:3, 3:] = translation_vector return transformation_matrix ``` 通过上述方法构建的外部参数矩阵 \( T_{\text{scanner-to-INS}} \) 应用于原始点云数据以消除安装误差。 --- #### 2. INS 数据的时间同步与姿态解算 在 GNSS/INS 集成系统中，通常会利用扩展卡尔曼滤波器 (EKF) 来估计载体的姿态、速度和位置信息[^1]。具体来说，INS 提供的姿态信息（四元数或旋转矩阵形式）以及位置信息是后续坐标转换的基础。 假设已知某时刻 \( t_k \) 的 INS 输出： - **位置**: \( P_{\text{INS},k}^{ECEF} = [X,Y,Z]^T \) 表示当前车辆在 ECEF 下的位置； - **姿态**: \( Q_{\text{INS},k} \) 或者对应的旋转矩阵 \( R_{\text{INS},k} \) 描述了 INS 当前相对于 ECEF 的方向； 则可以通过以下方式完成点云数据的转换。 --- #### 3. 点云数据的转换过程 给定点云中的某个点 \( p_i^{\text{INS}} \in \mathbb{R}^3 \) 在 INS 局部坐标系下的表达式，其对应于 ECEF 坐标系下点 \( p_i^{\text{ECEF}} \) 的计算如下所示： \[ p_i^{\text{ECEF}} = P_{\text{INS},k}^{ECEF} + R_{\text{INS},k} \cdot T_{\text{scanner-to-INS}} \cdot p_i^{\text{INS}} \] 其中， - \( P_{\text{INS},k}^{ECEF} \): INS 记录的当前位置向量； - \( R_{\text{INS},k} \): INS 态度旋转矩阵； - \( T_{\text{scanner-to-INS}} \): 扫描仪至 INS 的外部参数矩阵。 以下是 Python 实现的一个简单例子： ```python def transform_point_cloud_to_ecef(point_cloud_ins, ins_position_ecef, ins_rotation_matrix, scanner_to_ins_transform): """ 将点云从 INS 坐标系转换到 ECEF 坐标系。 参数: point_cloud_ins: N x 3 数组，表示点云在 INS 坐标系下的坐标 ins_position_ecef: 3 x 1 向量，表示 INS 设备在 ECEF 中的位置 ins_rotation_matrix: 3 x 3 旋转矩阵，表示 INS 到 ECEF 的旋转 scanner_to_ins_transform: 4 x 4 转换矩阵，表示扫描仪到 INS 的变换 返回: point_cloud_ecef: N x 3 数组，表示点云在 ECEF 坐标系下的坐标 """ homogenous_points = np.hstack((point_cloud_ins, np.ones((len(point_cloud_ins), 1)))) transformed_points = homogenous_points @ scanner_to_ins_transform.T[:, :3].T rotated_points = transformed_points @ ins_rotation_matrix.T point_cloud_ecef = rotated_points + ins_position_ecef.reshape(1, 3) return point_cloud_ecef ``` 此函数接受输入点云及其相关参数，并返回经过转换后的 ECEF 坐标系下的点云。 --- #### 工具支持 如果希望简化开发流程，可借助一些成熟的工具包来进行处理。例如 PSINS 工具箱提供了完整的捷联惯导系统数据处理功能，涵盖了惯性传感器数据分析、初始对准、AVP 更新解算等内容[^4]。此外，还可以结合 ROS 和 OpenCV 这样的框架进一步优化实现效率。 ---
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

scott198512

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值