# 四元数姿态解算 **Repository Path**: liuxcfreddy/quat_imu ## Basic Information - **Project Name**: 四元数姿态解算 - **Description**: 基于四元数的互补滤波算法 - **Primary Language**: C - **License**: MIT - **Default Branch**: master - **Homepage**: None - **GVP Project**: No ## Statistics - **Stars**: 1 - **Forks**: 3 - **Created**: 2024-09-27 - **Last Updated**: 2026-03-09 ## Categories & Tags **Categories**: Uncategorized **Tags**: None ## README
logo # 基于四元数的IMU数据融合算法 作者:Liuxc 日期:2024年9月
## 1. 四元数 四元数（Quaternion）是一种用来表示三维空间旋转的数学工具。它由三个元素组成： $$ q = w + xi + yj + zk $$ 其中，w, x, y, z是四元数的四个元素，分别代表四元数的实部、虚部。 四元数的实部w是一个实数，它表示旋转的角度。虚部x、y、z是三个实数，它们的模长表示旋转轴的方向。 四元数的加法和乘法运算满足四则运算的交换律和结合律。 ## 2. 四元数的应用 四元数的应用主要有以下几种： 1. 物体姿态估计：四元数可以用来估计物体的姿态，包括位置、姿态、方向等。 2. 空间姿态估计：四元数可以用来估计空间中的物体的相对位置，例如，两个相机的相对位置。 3. 空间旋转估计：四元数可以用来估计空间中的物体的旋转姿态，例如，一个物体绕着一个轴旋转的角度。 4. 空间欧拉角变换：四元数可以用来将欧拉角转换为四元数，并用于空间中的物体的姿态估计。 ## 3. 四元数的融合算法 由于使用陀螺仪更新四元数进行姿态时,介于陀螺仪本身的零点漂移和测量误差,将会使解算的姿态误差积累越来越大,因此需要对四元数进行融合算法的优化。 本工程利用加速度计的数值对四元数值进行修正,使用互补滤波的方式进行四元数的融合。 ## 4. 四元数的融合算法原理 陀螺仪具备更好的高频灵敏度,加速度计则在低频上拥有更好的精度 故通过融合两者的数据可以得到较为可用的值 具体步骤非常简单 通过加速度计的矢量方向求得在各个方向上的分量 求得与当前通过陀螺仪测得的四元数的差值,并将差值乘以一个权值 将这个权值加到当前的角速度分量上,得到新的四元数; 步骤非常简单 ## 5. 四元数的融合算法实现 在上方的原理之中 我们需要关注以下的部分: 陀螺仪是如何更新四元数的? 如何求得四元数在当前世界坐标系下的分量? 如何求得当前四元数的姿态分量与加速度计测得矢量的差值? 如何求得权值?并将其叠加到角速度输入上 以达到修正的目的? ## 6. 数学描述 $$ q=\begin{bmatrix}q0 \\q1 \\q2 \\q3 \end{bmatrix} $$ 关于四元数q的微分方程可以定义为: $$ \frac{d}{dt}q=\frac{1}{2} \omega q $$ 即: $$ \dot{q} = \frac{1}{2} \Omega q $$ 其中 $\Omega$ 为角速度矩阵, 单位为 $rad/s$ $$ \Omega=\begin{bmatrix}0 & -\omega_x&-\omega_y&-\omega_z \\ \omega_x &0& \omega_z & -\omega_y\\ \omega_y &-\omega_z & 0 & \omega_x \\ \omega_z &\omega_y & -\omega_x & 0\end{bmatrix} $$ 拥有角速度矩阵 $\Omega$ 后,我们可以通过角速度去更新四元数了 更新过程其实很好理解 我们对四元数求微分后的微分矩阵中的角速度正好可以对应我们姿态传感器的陀螺仪数据;如果将我们陀螺仪的数据按照上述矩阵中的位置填入,那么我们就得到了当前四元数矩阵的微分了,只要我们对这个微分矩阵进行积分,就可以得到我们当前的姿态角了; 但是这个是微分方程组,我们只是简单叠加上去就可以吗? 从原理上来讲 当然可以 但是有个前提就是 我们的计算机系统是一个不连续的离散系统 积分对于计算机来说就是在 新的值=原来的值+(微分值*时间) 这样的过程,但是计算机的运算能力有限,所以我们只能在一定时间内积分,然后将积分结果作为新的四元数的值,这样就导致了四元数的更新存在偏差,导致四元数的融合存在偏差。有没有什么办法呢? 其中一种就是龙格-库塔(Runge—Kutta)方法,它的核心思想是 利用拉格朗日中值定理,将多个值进行线性插值,得到一个更加精确的微分值,然后再进行积分,得到更加精确的结果。 我们不难列出基本的积分公式 $$ y_{n+1} = y_n + f(x_n,y_n)h $$ 其中的$f(x_n,y_n)$就是我们在这个时刻的微分值,即角速度矩阵的积分,而$h$是时间间隔,单位为$s$ 将其替换为我们姿态求解的方程中去可以得到: $$ q_{n+1} = q_{n} + \frac{1}{2} \Delta t \Omega q_{n} $$ 通过上述公式,我们可以得到新的四元数, 但此时四元数的分量并不一定是单位向量, 因此我们还需要对其进行归一化处理 $$ q_{new} = \frac{q_{new}}{\sqrt{\dot{q}_{new}^T \dot{q}_{new}}} $$ 上文提到单一的用斜率去积分buff的过程是不够精确的,所以我们需要用更加精确的积分方法,为了提高精度, 我们可以使用四阶龙格库塔公式,即: $$ q_{n+1} = q_{n} + \frac{1}{24} \Delta t \left( \Omega^T \Omega q_{n} + 2 \Omega^T \omega_{meas} q_{n} + \omega_{meas}^T \Omega q_{n} \right) $$ 其实本质上就是将当前的角速度循环(4阶)叠加到当前的四元数(姿态)上 将四次的位置进行了平均后叠加在原来的四元数上 从而得到新的四元数 也就是新的姿态; 写成方程组的形式为: $$ y_{n+1} = y_n + \frac{1}{6}h(k_1+k_2+k_3+k_4) $$ 其中 $$ k_1 = f(x_n,y_n) \\ k_2 = f(x_n+\frac{1}{2}h,y_n+\frac{1}{2}k_{1}h) \\ k_3 = f(x_n+\frac{1}{2}h,y_n+\frac{1}{2}k_{2}h) \\ k_4 = f(x_n+h,y_n+k_{3}h) $$ 其中的f为矩阵微分过程 需要提醒的是此时龙格库塔方程组里的x y都是四元数矩阵 由于是更新了y 所以此时的x自然代表着当前角速度量的输入 $ y_{n+1}=f(x_n,y_n) $是q的微分方程组, 我们需要求解这个方程组, 得到新的四元数 将角速度矩阵转至$\Omega$的形式 与待更新的四元数相乘就是新的四元数所需的变化量 $ \dot{f}= \frac{1}{2} * \Omega_{x}*q_{y} $ 但是着很明显输出精度不够(毕竟是微分方程组嘛,所以我们继续完成了剩下的三个阶)注意这个$\frac{1}{2}$ 是直接叠加在了我们角速度输入上了 并不是某个环节 既然是是对微分求解 倒推四元数的解 自然少不了对积分的理解 我们此时是对角速度的变化认为是一个线性过程自然定积分形式就是这样了 ## 7.利用加速度计对四元数进行修正 从理论上来讲,如果我们有一个理想的陀螺仪,不存在温漂,测量值绝对的精准,那么仅通过陀螺仪的数据更新四元数也足矣了;现实情况里,我们无法拥有这样一个理想的陀螺仪;所以我们需要通过加速度计来进行修正; # 工程化实现中的问题 ## 1.零偏移问题 由于陀螺仪的零点漂移,导致陀螺仪的测量值与实际值存在偏差,导致四元数的更新存在偏差,导致四元数的融合存在偏差。通俗一点的讲就是,我们在静止之时,传感器传来的数据并不为0,或者说传来的数据的平均值并不为0,这就导致了四元数的更新存在偏差,导致四元数的融合存在偏差。 ![零偏](image/image1.png "图-2") 如上图所示,在静止状态时 yaw轴陀螺仪测量的加速度 并不是均匀散落在0度处,而是在0.004的位置,这样就会导致我们在缓慢的旋转我们的姿态体,虽然这个过程很缓慢,但是我们也要尽量去避免;为了解决这个问题,我们先分析问题产生的原因: 为什么会产生零偏? 传感器的零偏的产生主要有两个原因: 一是传感器的生产过程中不可避免的公差,二是传感器所处环境的温度产生的温漂; 传感器生产过程中的公差是不可避免的,但是也相对比较固定,因为这个值是固定的,我们可以进行基本的标定之后,将这个误差滤除;但是温漂的产生就较为麻烦了; 有一定模电基础的朋友一定知道,对抗温漂常用的手法是利用电阻运放等电路设计补偿电路,这是一种有效的方法,但是还是那个问题,传感器本身存在的公差以及精度要求;陀螺仪的传感灵敏度非常高;16bit的传感精度,依靠温度补偿电路恐怕不能满足需要;其次电路设计的本身也比较麻烦;所以行业内常用的惯用手法就是,让温度不产生变化; 例如:让传感器一直稳定在一个温度下工作,然后进行标定,此时的到的零点漂移修正值就很稳定了; 所以如何让传感器保持在一个温度呢?我们经常使用的一种方式是对传感器进行加热;即在传感器周围设计加热电阻通过PWM控制加热电路;保证传感器一直稳定在一个温度(我们常常选择一个超过环境温度的值 45度是一个不错的选择;毕竟我们很难找到一个气温超过45度的地方) 在通过对温度的控制之后,传感器的零点的飘逸就足够的稳定了,我们可以通过静止状态下的监测 确定零点位置;甚至对于陀螺仪与加速度计本身测量值的离散情况都可以进行测量与统计 从而拟合出传感器足够精确的测量曲线; ## 2.初始状态校准与安装角偏移 姿态传感器启动的时候,如果我们的初始状态就不是水平的,那我们应该怎么办呢? 很简单 我们可以加速度计的值作为基准,将四元数的初始值设为加速度计的初始值,这样四元数的分量就不会偏离加速度计的初始值,从而避免了初始状态校准的问题。或者在启动之后动态的调整加速度计的权重可以达到初始状态的快速归位; 如果是姿态传感器本身的机械安装角偏移,可以使传感器数据在输入时通过旋转矩阵进行矫正; 在单轴旋转的问题中,我们知道 将矩阵绕$x$轴进行旋转的旋转矩阵为: $$ \begin{bmatrix}1 & 0 & 0 \\ 0 &cos\theta & -sin\theta \\ 0 &sin\theta & cos\theta \end{bmatrix} $$ 将矩阵绕$y$轴进行旋转的旋转矩阵为: $$ \begin{bmatrix}cos\theta & 0 & sin\theta \\ 0 &1 & 0 \\ -sin\theta & 0 & cos\theta \end{bmatrix} $$ 将矩阵绕$z$轴进行旋转的旋转矩阵为: $$ \begin{bmatrix}cos\theta & -sin\theta & 0 \\ sin\theta & cos\theta & 0 \\ 0 & 0 & 1 \end{bmatrix} $$ 以上旋转矩阵其实就可以作为罗德里格旋转公式的特解 罗德里格旋转公式: $$ \vec{v_{rot}} = cos\theta \vec{v} + (1-cos\theta)(\vec{k}\cdot\vec{v})\vec{k} + sin\theta{k}\times\vec{v} $$ v_rot为旋转后的向量,v原向量，k是旋转轴的单位向量，则v在右手螺旋定则意义下绕旋转轴k旋转角度θ得到的向量 若 $$ \vec k=\begin{bmatrix}k_1\\ k_2\\ k_3 \end{bmatrix} $$ $$ \vec v=\begin{bmatrix}v_1\\ v_2\\ v_3 \end{bmatrix} $$ 设 $\left.\mathbf{R}=Ecos\theta+(1-\cos\theta)\left(\begin{array}{c}k_x\\k_y\\k_z\end{array}\right.\right)(k_x,k_y,k_z)+\sin\theta\left(\begin{array}{ccc}0&-k_z&k_y\\k_z&0&-k_x\\-k_y&k_x&0\end{array}\right)$ E为单位矩阵 $\left(\begin{array}{ccc}1&0&0\\0&1&0\\0&0&1\end{array}\right)$ 以上 可将公式写为 $$ v_{rot} = \mathbf{R}\vec{v} $$ 展开后的矩阵表达: $$ \begin{bmatrix}\cos\theta+(1-\cos\theta)v_1^2&(1-\cos\theta)v_1v_2-\sin\theta v_3&(1-\cos\theta)v_1v_3+\sin\theta v_2\\(1-\cos\theta)v_1v_2+\sin\theta v_3&\cos\theta+(1-\cos\theta)v_2^2&(1-\cos\theta)v_2v_3-\sin\theta v_1\\(1-\cos\theta)v_1v_3-\sin\theta v_2&(1-\cos\theta)v_2v_3+\sin\theta v_1&\cos\theta+(1-\cos\theta)v_3^2\end{bmatrix} $$ ## 3.YAW轴的偏移问题 由于我们的传感器 大部分时间内是Z轴朝天的,所以x,y轴的分量几乎为0,如此一来,我们就无法依靠加速度计来修正yaw轴旋转;常用的方法是叠加上地磁计,通过地磁计的数值来修正yaw轴的偏移问题。 毕竟地磁计的值在一个区域内我们可以认为是不变的(其实地磁的方向与地球的自旋轴是不重合,所以越到高维度,地磁计的数值就越不准确,不过这个不在我们的讨论范围之内,毕竟我们只讨论传感器的融合问题),有了地磁计的数据一切就好办了 地磁计往往需要进行校准后才能有效的修正姿态,但是由于我们只需要修正yaw轴的偏移问题,所以可以只使用地磁计的偏移量来修正yaw轴的偏移;修正过程与使用加速度计修正类似; 以下是未校准地磁计的情况下 静态姿态的yaw轴偏移情况: 航向修正 ## 4.其余的问题 在以上问题都解决之后就是对算法上限的探寻了;比如传感器探测范围的超限,滤波器的性能,平移加速度造成的加速度计矢量偏移等等;这些问题都需要我们在实际的工程实践中不断的去探索,才能找到最优的解决方案。在上面我们提到了通过地磁计去修正yaw轴的偏移问题,其实针对实际运动中,我们常常增加双目光流,超声定位,UWB等手段进一步融合我们的传感器数据,从而提供更多的运动信息与姿态信息; 现代的传感器融合算法已经越来越复杂,涉及到很多领域,比如在自动驾驶领域十分重要的感知,AI,机器人等等,都涉及到多数据源的融合利用,这一切也在向着更加高端智能的方向发展!