加速度计姿态解算_IMU机体方向矫正

加速度计姿态解算_IMU机体方向矫正

所有的操作都是在加速度计 六面校准已完成 的条件下进行

加速度计姿态解算

重力计算欧拉角：
假设 ax ay az 为三轴重力分量，那么可以认为是向量[0, 0 ,g]经过三轴旋转而得，因此满足：
742f0808209d4e85f151c6ef346c10a4

解算结果：

0ca8175031bf0a47094d220f635dd6e4

就得到对应的roll和pitch角度，通过roll和pitch角度构建俯仰和横滚旋转矩阵（Matlab）

% 构造俯仰和横滚偏差的旋转矩阵
R_pitch_bias = [cos(theta_pitch_bias_rad), 0, -sin(theta_pitch_bias_rad);
                                        0, 1, 0;
                sin(theta_pitch_bias_rad), 0, cos(theta_pitch_bias_rad)];

R_roll_bias = [1, 0, 0;
                0, cos(phi_roll_bias_rad), sin(phi_roll_bias_rad);
                0, -sin(phi_roll_bias_rad), cos(phi_roll_bias_rad)];

将两个旋转矩阵进行相乘

注意矩阵乘法顺序，欧拉角默认的旋转顺序是Z-Y-X。Z轴，Y轴和X轴分别对应Yaw，Pitch和Roll。

所有两个矩阵乘法的次序应该是：R_roll_bias * R_pitch_bias （X*Z）

ZYX顺序的旋转矩阵 $\ R_{ZYX} $：
如果你有一个绕Z轴的旋转矩阵 $\ R_Z $，那么按照ZYX顺序的总旋转矩阵将是：

\[R_{ZYX} = R_X \cdot R_Y \cdot R_Z \]

这里，$\ R_X $ 是绕x轴的旋转矩阵 roll，$\ R_Y $ 是绕y轴的旋转矩阵（pitch），而 $\ R_Z $​ 是绕z轴的旋转矩阵。

只有Pitch和Roll时的总旋转矩阵 $\ R_{total} $：
如果只考虑Pitch和Roll，总旋转矩阵将是：

\[R_{total} = R_{roll}(\phi) \cdot R_{pitch}(\theta) \]

R_attitude = R_roll_bias * R_pitch_bias;

得到的 R_attitude 就是IMU与机体的姿态矩阵

如果要将改姿态矩阵用于纠正IMU与机体的偏移，就需要将姿态矩阵进行逆运算，得到的就是反向的偏移，将该偏移矩阵用于加速度计的纠偏，就可将原始加速度计数据映射到机体坐标上

R_attitude_inv = inv(R_attitude);

对原始加速度计数据应用偏移只需要将偏移矩阵乘到加速度计数据上

acc_Cail = R_attitude_inv * normalized_acceleration;

对应的C语言代码（使用arm_math库）

该程序描述如何使用水平机体水平状态下的加速度数据，计算出偏移矩阵

	//============== 机体矫正

    //加速度原始数据
    float matx_AccData[3] = {
        -0.185169,
        -0.192738,
        0.968380,
    };
    //构造加速度矩阵
    arm_matrix_instance_f32 matx_Acc = {0};
    arm_mat_init_f32(&matx_Acc, 3, 1, &matx_AccData[0]);
    float normAcc = 1.0 / sqrt(NorSQR(matx_AccData[0]) + NorSQR(matx_AccData[1]) + NorSQR(matx_AccData[2]));

    //归一化加速度矩阵
    float matx_AccNormData[3];
    arm_matrix_instance_f32 matx_AccNorm;
    arm_mat_init_f32(&matx_AccNorm, 3, 1, &matx_AccNormData[0]);
    arm_mat_scale_f32(&matx_Acc, normAcc, &matx_AccNorm);

    //构造姿态选择矩阵
    float roll_bias = atan2(matx_AccNorm.pData[1], matx_AccNorm.pData[2]);
    float pitch_bias = -atan2(matx_AccNorm.pData[0], sqrt(NorSQR(matx_AccNorm.pData[2]) + NorSQR(matx_AccNorm.pData[1])));

    float rtMat_Pitch_D[3 * 3];
    float rtMat_Roll_D[3 * 3];

    float sin_pitch_bias = sin(pitch_bias);
    float cos_pitch_bias = cos(pitch_bias);

    float sin_roll_bias = sin(roll_bias);
    float cos_roll_bias = cos(roll_bias);

    rtMat_Pitch_D[0] = cos_pitch_bias;
    rtMat_Pitch_D[1] = 0;
    rtMat_Pitch_D[2] = -sin_pitch_bias;
    rtMat_Pitch_D[3] = 0;
    rtMat_Pitch_D[4] = 1.0f;
    rtMat_Pitch_D[5] = 0;
    rtMat_Pitch_D[6] = sin_pitch_bias;
    rtMat_Pitch_D[7] = 0;
    rtMat_Pitch_D[8] = cos_pitch_bias;

    rtMat_Roll_D[0] = 1.0f;
    rtMat_Roll_D[1] = 0;
    rtMat_Roll_D[2] = 0;
    rtMat_Roll_D[3] = 0;
    rtMat_Roll_D[4] = cos_roll_bias;
    rtMat_Roll_D[5] = sin_roll_bias;
    rtMat_Roll_D[6] = 0;
    rtMat_Roll_D[7] = -sin_roll_bias;
    rtMat_Roll_D[8] = cos_roll_bias;

    arm_matrix_instance_f32 rtMat_Roll;
    arm_matrix_instance_f32 rtMat_Pitch;
    arm_mat_init_f32(&rtMat_Pitch,3,3,rtMat_Pitch_D);
    arm_mat_init_f32(&rtMat_Roll,3,3,rtMat_Roll_D);

    //矩阵乘法
    float rtMat_Corr_D[3 * 3];
    arm_matrix_instance_f32 rtMat_Corr;
    arm_mat_init_f32(&rtMat_Corr,3,3,rtMat_Corr_D);

    arm_mat_mult_f32(&rtMat_Roll,&rtMat_Pitch,&rtMat_Corr);

    //内存空间复用 - 俯仰姿态矩阵
    arm_matrix_instance_f32 rtMat_CorrInv;
    arm_mat_init_f32(&rtMat_CorrInv,3,3,rtMat_Pitch_D);
    
    //求解姿态矩阵逆矩阵
    arm_mat_inverse_f32(&rtMat_Corr,&rtMat_CorrInv);

    //矫正加速度计数据
    float matx_AccCorData[3] ;
    arm_matrix_instance_f32 matx_AccCor;
    arm_mat_init_f32(&matx_AccCor,3,1,matx_AccCorData);

    arm_mat_mult_f32(&rtMat_CorrInv,&matx_AccNorm,&matx_AccCor);
posted @ 2025-05-07 14:19  c17VV  阅读(290)  评论(0)    收藏  举报