目录

 

一.四元数

 二.姿态解算

算法思路

详解

1.单位化加速度向量

 2. 计算误差

3. 积分误差

4. 应用误差到角速度

5. 四元数更新

7. 欧拉角计算

三.归一化  

四.yaw轴特殊处理（一般不适用yaw，噪声大，零漂重。对于一般陀螺仪）

五.矩阵 

六.完整代码 

七.结论 

 

一.四元数

  • 首先介绍一下四元数，四元数是一种数学工具，用来表示三维空间中的旋转。
  • 四元数通常表示为： [ q = a + bi + cj + dk ] 其中，( a ) 是实部，( b, c, d ) 是虚部的三个分量，( i, j, k ) 是单位虚数，满足 ( i^2 = j^2 = k^2 = ijk = -1 )。在四元数表示中，单位虚数 ( i, j, k ) 分别对应三维空间中的 ( x, y, z ) 轴。四元数的实部 ( a ) 表示旋转的余弦值，而虚部 ( b, c, d ) 则表示旋转的方向和角度的正弦值。四元数的乘法用于表示两个旋转的组合，四元数的共轭用于表示逆旋转，而单位化后的四元数表示一个旋转。

 二.姿态解算

  • 本文采用的是基于四元数的Mahony滤波器姿态估计算法。姿态估计在无人机、机器人和虚拟现实等领域中是至关重要的。其中一种常用的方法是基于四元数的滤波器算法。该算法使用陀螺仪和加速度计的测量数据，结合四元数来实时估计物体的姿态。

  • 算法思路

  1. 单位化加速度向量：通过加速度计测量值，得到重力方向的单位向量，用于后续的计算。

  2. 计算误差：将加速度计测量的重力向量与预估的重力向量进行比较，得到姿态误差。

  3. 积分误差：对误差进行积分，用于补偿陀螺仪的漂移。

  4. 应用误差到角速度：将姿态误差应用到陀螺仪测量的角速度中，以校正姿态。

  5. 四元数更新：使用一阶龙格库塔方法更新四元数，表示物体的姿态。

  6. 四元数归一化：确保四元数保持单位化，以保持姿态的准确性。

  7. 欧拉角计算：将更新后的四元数转换为欧拉角，得到物体的姿态角。

  • 详解

1.单位化加速度向量
gx = gx * (PI / 180.0f);
gy = gy * (PI / 180.0f);
gz = gz * (PI / 180.0f);

norm = invSqrt(ax * ax + ay * ay + az * az);
ax = ax * norm;
ay = ay * norm;
az = az * norm;
 2. 计算误差
vx = Mat.DCM_T[0][2];
vy = Mat.DCM_T[1][2];
vz = Mat.DCM_T[2][2];

ex = (ay * vz) - (az * vy);
ey = (az * vx) - (ax * vz);
ez = (ax * vy) - (ay * vx);
  //机体坐标系下向量叉乘得到误差向量，误差e就是测量得到的vˉ和预测得到的 v^之间的相对旋转。这里的vˉ就是[ax,ay,az]’,v^就是[vx,vy,vz]’
  //利用这个误差来修正DCM方向余弦矩阵(修正DCM矩阵中的四元素)，这个矩阵的作用就是将b系和n正确的转化直到重合。
  //实际上这种修正方法只把b系和n系的XOY平面重合起来，对于z轴旋转的偏航，加速度计无可奈何，
  //但是，由于加速度计无法感知z轴上的旋转运动，所以还需要用地磁计来进一步补偿。
  //两个向量的叉积得到的结果是两个向量的模与他们之间夹角正弦的乘积a×v=|a||v|sinθ,
  //加速度计测量得到的重力向量和预测得到的机体重力向量已经经过单位化，因而他们的模是1，
  //也就是说它们向量的叉积结果仅与sinθ有关，当角度很小时，叉积结果可以近似于角度成正比。
3. 积分误差
exInt = exInt + ex * ki;
eyInt = eyInt + ey * ki;
ezInt = ezInt + ez * ki;
4. 应用误差到角速度
gx = gx + kp * ex + exInt;
gy = gy + kp * ey + eyInt;
gz = gz + kp * ez + ezInt;
5. 四元数更新
q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * MahonyPERIOD * 0.5f;
q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * MahonyPERIOD * 0.5f;
q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * MahonyPERIOD * 0.5f;
q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * MahonyPERIOD * 0.5f;

6. 四元数归一化

norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
q0 = q0 * norm;
q1 = q1 * norm;
q2 = q2 * norm;
q3 = q3 * norm;
7. 欧拉角计算
imu.pitch = asin(-2.0f * (q0 * q2 - q1 * q3)) * (180.0f / PI);
imu.roll = atan2(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * (180.0f / PI);
imuyaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * (180.0f / PI);

三.归一化  

  • 在数学和物理中，计算向量模长的倒数是为了进行归一化。归一化是将向量缩放为单位向量的过程，即确保向量的模长为1。(快速反平方根算法)
//快速反平方根算法
float invSqrt(float x)
{
    float halfx = 0.5f * x; // 计算x的一半，用于后续的牛顿迭代法中
    float y = x;            // 将输入值x初始化为y，开始时假设y是x的平方根的倒数
    long i = *(long*)&y;    // 通过类型转换，把y的浮点表示当作长整型数字来读取其位模式

    // 0x5f3759df，与i的半逆转相减产生y的初估计值
    // 使用位运算来加速运算，并通过位移操作实现除以2的效果
    i = 0x5f3759df - (i >> 1);

    // 将长整型i的内容当作浮点数来读取，得到倒数平方根的初估计值
    y = *(float*)&i;

    // 进行一次牛顿迭代，以改进y的值，提高计算的准确度
    // 牛顿迭代公式：y = y * (1.5f - (halfx * y * y))，它基于牛顿迭代法求解f(y)=1/y^2 - x = 0
    y = y * (1.5f - (halfx * y * y));

    // 返回y值，即x的平方根的倒数
    return y;
}

四.yaw轴特殊处理（一般不适用yaw，噪声大，零漂重。对于一般陀螺仪）

    // 计算偏航角的增量变化
    iyaw = imuyaw - imuyawlast;
    // 当穿越360-0度边界时，纠正偏航的突然跳变
    if (iyaw < -300) {
        // 如果偏航角变化的增量小于 -300 度，说明发生了向负方向超过 300 度的突变
        imux += 360;
    } else if (iyaw > 300) {
        // 如果偏航角变化的增量大于 300 度，说明发生了向正方向超过 300 度的突变
        imux -= 360;
    }
    // 更新上一次的偏航测量值
    imuyawlast = imuyaw;
    // 将修正值加到偏航上
    imuyaw += imux;
    // 用新的偏航值更新imu结构
    imu.yaw = imuyaw;
    // 将偏航角度规范化到0到360度范围内
    if (imuyaw > 360) {
        // 如果偏航角大于 360 度，则减去一个完整的圈数，使其处于 0 到 360 度之间
        imuyaw -= 360;
        imu.yaw = imuyaw;
    } else if (imuyaw < 0) {
        // 如果偏航角小于 0 度，则加上一个完整的圈数，使其处于 0 到 360 度之间
        imuyaw += 360;
        imu.yaw = imuyaw;
    }
    // 计算连续测量之间的偏航角差异
    float i = imuyaw - imuyawlast;
    // 标识特定方向，初始化为0
    int imuflag = 0;
    // 若偏航角超过-170度，则可能设置一个标志位以进行特殊处理
    if (imu.yaw < -170) {
        imuflag = 1;
    }
    // 确保偏航角变化`i`在-180到180度范围内，以适应环绕连续性
    if (i > 180) {
        i -= 360;
    } else if (i < -180) {
        i += 360;
    }
    // 累积偏航角变化并更新偏航角
    imuyaw += i;
    // 将更新后的偏航角重新存储到imu结构中
    imu.yaw = imuyaw;

五.矩阵 

  • 旋转矩阵，将四元数表示的姿态转换为旋转矩阵，用于描述物体在三维空间中的旋转姿态；旋转矩阵的转置矩阵，将地理坐标系下的旋转矩阵转换为机体坐标系下的旋转矩阵。
// 将四元数转换为方向余弦矩阵 (DCM)，用于描述三维空间中的旋转
Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;  // 计算矩阵第1行第1列元素，表示旋转后x轴相对于原始x轴的余弦值
Mat.DCM_T[0][1] = 2.0f * (q1*q2 + q0*q3);              // 计算矩阵第1行第2列元素，表示旋转后x轴相对于原始y轴的余弦值
Mat.DCM_T[0][2] = 2.0f * (q1*q3 - q0*q2);              // 计算矩阵第1行第3列元素，表示旋转后x轴相对于原始z轴的余弦值

Mat.DCM_T[1][0] = 2.0f * (q1*q2 - q0*q3);              // 计算矩阵第2行第1列元素，表示旋转后y轴相对于原始x轴的余弦值
Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;  // 计算矩阵第2行第2列元素，表示旋转后y轴相对于原始y轴的余弦值
Mat.DCM_T[1][2] = 2.0f * (q2*q3 + q0*q1);              // 计算矩阵第2行第3列元素，表示旋转后y轴相对于原始z轴的余弦值

Mat.DCM_T[2][0] = 2.0f * (q1*q3 + q0*q2);              // 计算矩阵第3行第1列元素，表示旋转后z轴相对于原始x轴的余弦值
Mat.DCM_T[2][1] = 2.0f * (q2*q3 - q0*q1);              // 计算矩阵第3行第2列元素，表示旋转后z轴相对于原始y轴的余弦值
Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;  // 计算矩阵第3行第3列元素，表示旋转后z轴相对于原始z轴的余弦值


//转置矩阵
 Mat.DCM_T[0][0] = 1.0f - 2.0f * q2*q2 - 2.0f * q3*q3;
 Mat.DCM_T[0][1] = 2.0f * (q1*q2 +q0*q3);
 Mat.DCM_T[0][2] = 2.0f * (q1*q3 -q0*q2);

 Mat.DCM_T[1][0] = 2.0f * (q1*q2 -q0*q3);
 Mat.DCM_T[1][1] = 1.0f - 2.0f * q1*q1 - 2.0f * q3*q3;
 Mat.DCM_T[1][2] = 2.0f * (q2*q3 +q0*q1);

 Mat.DCM_T[2][0] = 2.0f * (q1*q3 +q0*q2);
 Mat.DCM_T[2][1] = 2.0f * (q2*q3 -q0*q1);
 Mat.DCM_T[2][2] = 1.0f - 2.0f * q1*q1 - 2.0f * q2*q2;

六.完整代码 

_Matrix Mat = {0};
_Attitude att = {0};
#define MahonyPERIOD        5.0f            //姿态解算周期（ms）
#define kp      0.5f                        //比例增益控制加速度计/磁力计的收敛速率
#define ki      0.0001f                     //积分增益控制陀螺仪偏置的收敛速率

float q0 = 1, q1 = 0, q2 = 0, q3 = 0;       //表示估计方向的四元数元素
float exInt = 0, eyInt = 0, ezInt = 0;      //缩放积分误差
float imuyaw,imuyawlast,iyaw,imux;


int16 gyro_x = 0, gyro_y = 0, gyro_z = 0;                           // 三轴陀螺仪滤波数据
int16 acc_x = 0, acc_y = 0, acc_z = 0;                              // 三轴加速度计滤波数据

float invSqrt(float x)
{
    float halfx = 0.5f * x; // 计算x的一半，用于后续的牛顿迭代法中
    float y = x;            // 将输入值x初始化为y，开始时假设y是x的平方根的倒数
    long i = *(long*)&y;    // 通过类型转换，把y的浮点表示当作长整型数字来读取其位模式

    // 0x5f3759df，与i的半逆转相减产生y的初估计值
    // 这里使用位运算来加速运算，并通过位移操作实现除以2的效果
    i = 0x5f3759df - (i >> 1);

    // 将长整型i的内容当作浮点数来读取，得到倒数平方根的初估计值
    y = *(float*)&i;

    // 进行一次牛顿迭代，以改进y的值，提高计算的准确度
    // 牛顿迭代公式：y = y * (1.5f - (halfx * y * y))，它基于牛顿迭代法求解f(y)=1/y^2 - x = 0
    y = y * (1.5f - (halfx * y * y));

    // 返回y值，即x的平方根的倒数
    return y;
}

void mahony_update(float gx, float gy, float gz, float ax, float ay, float az)
{
    // gx, gy, gz 是陀螺仪测量到的角速度值, 单位为度每秒
    // ax, ay, az 是加速度计测量到的加速度值
    float norm;
    float vx, vy, vz;
    float ex, ey, ez;
    // 如果加速度计测量值为零，则退出函数
    if(ax * ay * az == 0)
        return;

    // 将陀螺仪的角速度值从度转换为弧度
    gx = gx * (PI / 180.0f);
    gy = gy * (PI / 180.0f);
    gz = gz * (PI / 180.0f);

    // 对加速度计的测量值进行单位化（归一化），使其成为单位向量（代表重力方向）
    norm = invSqrt(ax * ax + ay * ay + az * az);
    ax = ax * norm;
    ay = ay * norm;
    az = az * norm;

    // vx,vy,vz 是通过估计的姿态得到的世界坐标系中重力向量在机体坐标系中的投影
    // 从四元数方向余弦矩阵转换得到
    vx = Mat.DCM_T[0][2];
    vy = Mat.DCM_T[1][2];
    vz = Mat.DCM_T[2][2];

    // 计算加速度计测量的重力向量（ax,ay,az）和预估的重力向量（vx,vy,vz）之间的误差
    ex = (ay * vz) - (az * vy); // 叉积的x分量
    ey = (az * vx) - (ax * vz); // 叉积的y分量
    ez = (ax * vy) - (ay * vx); // 叉积的z分量

    // 对这个误差进行积分, ki 是积分增益
    exInt = exInt + ex * ki;
    eyInt = eyInt + ey * ki;
    ezInt = ezInt + ez * ki;

    // 将姿态误差（比例和积分）应用于角速度，kp 是比例增益
    gx = gx + kp * ex + exInt;
    gy = gy + kp * ey + eyInt;
    gz = gz + kp * ez + ezInt;

    // 更新四元数，MahonyPERIOD 是更新周期
    q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * MahonyPERIOD * 0.5f;
    q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * MahonyPERIOD * 0.5f;
    q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * MahonyPERIOD * 0.5f;
    q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * MahonyPERIOD * 0.5f;

    // 对新的四元数进行归一化处理，确保它们是单位四元数
    norm = invSqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3);
    q0 = q0 * norm;
    q1 = q1 * norm;
    q2 = q2 * norm;
    q3 = q3 * norm;

    // 将四元数转换为欧拉角，更新imu的姿态角
    imu.pitch = asin(-2.0f * (q0 * q2 - q1 * q3)) * (180.0f / PI);
    imu.roll = atan2(2.0f * (q0 * q1 + q2 * q3), q0 * q0 - q1 * q1 - q2 * q2 + q3 * q3) * (180.0f / PI);
    imuyaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * (180.0f / PI);
    // 计算偏航角的增量变化
    iyaw = imuyaw - imuyawlast;
    // 当穿越360-0度边界时，纠正偏航的突然跳变
    if (iyaw < -300) {
        // 如果偏航角变化的增量小于 -300 度，说明发生了向负方向超过 300 度的突变
        imux += 360;
    } else if (iyaw > 300) {
        // 如果偏航角变化的增量大于 300 度，说明发生了向正方向超过 300 度的突变
        imux -= 360;
    }
    // 更新上一次的偏航测量值
    imuyawlast = imuyaw;
    // 将修正值加到偏航上
    imuyaw += imux;
    // 用新的偏航值更新imu结构
    imu.yaw = imuyaw;
    // 将偏航角度规范化到0到360度范围内
    if (imuyaw > 360) {
        // 如果偏航角大于 360 度，则减去一个完整的圈数，使其处于 0 到 360 度之间
        imuyaw -= 360;
        imu.yaw = imuyaw;
    } else if (imuyaw < 0) {
        // 如果偏航角小于 0 度，则加上一个完整的圈数，使其处于 0 到 360 度之间
        imuyaw += 360;
        imu.yaw = imuyaw;
    }
    // 计算连续测量之间的偏航角差异
    float i = imuyaw - imuyawlast;
    // 标识特定方向，初始化为0
    int imuflag = 0;
    // 若偏航角超过-170度，则可能设置一个标志位以进行特殊处理
    if (imu.yaw < -170) {
        imuflag = 1;
    }
    // 确保偏航角变化`i`在-180到180度范围内，以适应环绕连续性
    if (i > 180) {
        i -= 360;
    } else if (i < -180) {
        i += 360;
    }
    // 累积偏航角变化并更新偏航角
    imuyaw += i;
    // 将更新后的偏航角重新存储到imu结构中
    imu.yaw = imuyaw;
}

七.结论 

Mahony滤波器算法是一种强大的姿态估计工具，通过结合陀螺仪和加速度计数据，以及使用四元数表示姿态，实现了对物体姿态的实时估计。这种算法的优势在于对陀螺仪积分漂移的抑制，同时保持了良好的计算效率。在无人机、机器人等领域的导航和控制中，Mahony滤波器算法展现了其强大的姿态估计性能。

 

# 线性代数 # c# # 矩阵
Logo
GitCode 开源社区

旨在为数千万中国开发者提供一个无缝且高效的云端环境，以支持学习、使用和贡献开源项目。

更多推荐

cover

GitTalk | 使用面向业务的狮偶编程语言提升开发效率

avatar GitCode 开源社区
cover

GitTalk | DevUI Suits 场景解决方案

avatar GitCode 开源社区
cover

GitTalk | DevUI Admin 前端项目构建

avatar GitCode 开源社区