陀螺仪姿态角之磁力计与加速度的融合说明
1. 概述
航向角，姿态角可以通过磁力计与加速度传感器进行解算，这里使用9轴陀螺仪ICM-20948.
姿态角，用于描述物体在三维空间中相对于参考坐标系旋转状态的一组角度参数；包括俯仰角（Pitch），偏航角（Yaw），翻滚角（Roll），总的来说姿态角是用来描述飞机、航天器等飞行器在空中的姿态和方向的一组重要参数。它们对于飞行器的控制和导航至关重要，能够帮助飞行员或自动驾驶系统准确地了解飞行器当前的空间位置和姿态，从而进行有效的操控和调整。
本文使用9轴传感器目的是经过数据融合将地理方位的东南西北与偏航角相对应，这里直接使用解算完成的俯仰角与横滚角，对于俯仰角与横滚角的解算原理请看：
链接 - 数据读取
链接 - 解算滤波
一些思路：
- 若没有俯仰角与横滚角，我们通过单磁力计只能得到以高斯为单位的X、Y、Z三轴数据值（未经校准不可使用）；
- 磁力计的校准；
- 磁力计校准后与俯仰角横滚角融合解算出正确的姿态角（航向角）。
2. MG轴与AC轴
各轴方向，可将加速度，陀螺仪，磁力计的x,y,z看成同一个轴
角度关联
在改变陀螺仪的状态时：
- 加速度Y轴的转动可看做是俯仰角的改变；
- 加速度X轴的转动可看做是横滚角的改变；
- 将磁力线（面）看做水平，当俯仰角和横滚角产生后（≠0），Z轴不再平行于磁力线（或面），将X、Z轴，Y，Z轴分开来看，Z轴在俯仰角和横滚角≠0的状态下都会受到与X、Y轴相反的力（如3中下图）；
- 我们想要的角度可看做是X轴（或者Y轴），与磁力线（或面）的夹角，这样可以通过X，Y轴的投影，再由atan2(Y,X)公式来求解；
2. MG轴的三维空间
空间坐标
X轴视角
X轴的角度随Pitch（y）轴角度改变
Y轴视角
Y轴的角度随Roll（x）轴角度改变
Z轴视角
Z轴分量来自于 Pitch 和 Roll 2次角度变换后的结果
注：
- 初始近似水平状态，Z轴可认为垂直于磁力面；
- Pitch角改变后，Z轴此时延X轴垂直投影的反方向变化（X轴、Z轴垂直）；
- Roll角改变后，Z轴此时延Y轴垂直投影的反方向变化（Y轴、Z轴垂直）；
- Pitch角、Roll角改变不分先后；
- 假设Pitch先改变，则Z轴此时的垂直投影 MgZp = MgZ*sin(pitch);
- Roll改变，则Roll对Z轴改变产生的线段长度为MgZr = MgZ*cos(Roll);
- 将X轴、Y轴看做相互独立的两个轴，则有：
X轴垂直分量： MX垂分 = MX * cos(pitch) - MZ * sin(pitch);
Y轴垂直分量： MY垂分 = MY * cos(roll) - MZ * sin(roll);
Pitch 与Roll对三轴的影响：
Pitch:
Roll:
垂直投影：
这里获得垂直分量之后，经过计算可以得到X轴（或者Y轴）的角度参数。
计算角度 atan2(Y,X)：
atan2(Y,X) 的取值范围是[-π，π]，经过换算后可完全覆盖0~360°；
这里得到 atan2(Y,X) 的结果K后，由K * 180/π 便可得到一个[-180， +180] 的角度数据；
4. 磁力计校正
需要校正的原因：
- 磁力计的误差：可能来自多个方面，如硬件设计、传感器特性以及环境干扰等。通过校准，可以消除或修正这些误差，使磁力计能够准确测量磁场强度；
- 使用环境的变化：不同的应用场景对磁力计的精度和稳定性有不同的要求。通过校准，可以根据具体应用需求调整磁力计的性能参数，以满足特定场景下的测量要求。
关于校正的一些思考：
X轴（或其他轴）平行于磁力线为最大（或最小），垂直于磁力线为0，如果X轴（或其他轴）平行于磁力线获得的最大值为300，最小值为-200，说明器件有误差需要校准，误差近似(300+(-200))/2。
消除误差（校正的）时刻，可将该轴与磁力线水平采样。
5. C代码
这里只做了俯仰角与横滚角的加速度轴简单转换，未使用任何滤波，未使用角速度，比较粗略，想要获取稳定的角度参数需要参考 “1.概述” 部分的链接进行融合解算。
以下代码将加速度与磁力计融合，得到固定的偏航角数据。
//9轴参数变量
int16_t get_s16Accel[3] = {0};
int16_t get_s16Gyro[3] = {0};
int16_t get_s16Magn[3] = {0};
//加速度转存变量
int16_t accel_x; //X
int16_t accel_y; //Y
int16_t accel_z; //Z
//三轴磁力计的取平均数组
int16_t get_s16Magn_x_buf[10];
int16_t get_s16Magn_y_buf[10];
int16_t get_s16Magn_z_buf[10];
//磁力轴保存最大值
int get_s16Magn_x_MAX_AVE;
int get_s16Magn_y_MAX_AVE;
int get_s16Magn_z_MAX_AVE;
//磁力轴保存最小值
int get_s16Magn_x_MIN[3];
int get_s16Magn_y_MIN[3];
int get_s16Magn_z_MIN[3];
//三轴磁力数据
float MXH = 0;
floatMYH = 0;
floatMZH = 0;
//Z轴相对于X和Y轴的反向垂直分量
floatMZXH = 0;
floatMZYH = 0;
//获取九轴参数
icm20948AccelRead(&get_s16Accel[0], &get_s16Accel[1], &get_s16Accel[2]);
icm20948GyroRead(&get_s16Gyro[0], &get_s16Gyro[1], &get_s16Gyro[2]);
icm20948MagRead(&get_s16Magn[0], &get_s16Magn[1], &get_s16Magn[2]);
accel_x = get_s16Accel[0];
accel_y = get_s16Accel[1];
accel_z = get_s16Accel[2];
//俯仰角 横滚角 解算
if(accel_x<32764)
{
get_accel_x=accel_x/(16384.0); //0-1g
}
else
{
get_accel_x=1-(accel_x-49152)/16384.0;
}
if(accel_y<32764)
{
get_accel_y=accel_y/16384.0;
}
else
{
get_accel_y=1-(accel_y-49152)/16384.0;
}
if(accel_z<32764)
{
get_accel_z=accel_z/16384.0;
}
else
{
get_accel_z=(accel_z-49152)/16384.0;
}
//获取俯仰角 横滚角
roll_raw=(atan(get_accel_y/get_accel_z))*180/3.14;
pitch_raw = (atan(get_accel_x/get_accel_z))*180/3.14;
//取平均稳定参数
get_s16Magn_x_buf[9] = get_s16Magn_x_buf[8];
get_s16Magn_x_buf[8] = get_s16Magn_x_buf[7];
get_s16Magn_x_buf[7] = get_s16Magn_x_buf[6];
get_s16Magn_x_buf[6] = get_s16Magn_x_buf[5];
get_s16Magn_x_buf[5] = get_s16Magn_x_buf[4];
get_s16Magn_x_buf[4] = get_s16Magn_x_buf[3];
get_s16Magn_x_buf[3] = get_s16Magn_x_buf[2];
get_s16Magn_x_buf[2] = get_s16Magn_x_buf[1];
get_s16Magn_x_buf[1] = get_s16Magn_x_buf[0];
get_s16Magn_x_buf[0] = get_s16Magn[0];
get_s16Magn[0] = (get_s16Magn_x_buf[0]+get_s16Magn_x_buf[1]+get_s16Magn_x_buf[2]+get_s16Magn_x_buf[3]+get_s16Magn_x_buf[4]+get_s16Magn_x_buf[5]+get_s16Magn_x_buf[6]+get_s16Magn_x_buf[7]+get_s16Magn_x_buf[8]+get_s16Magn_x_buf[9])/10;
get_s16Magn_y_buf[9] = get_s16Magn_y_buf[8];
get_s16Magn_y_buf[8] = get_s16Magn_y_buf[7];
get_s16Magn_y_buf[7] = get_s16Magn_y_buf[6];
get_s16Magn_y_buf[6] = get_s16Magn_y_buf[5];
get_s16Magn_y_buf[5] = get_s16Magn_y_buf[4];
get_s16Magn_y_buf[4] = get_s16Magn_y_buf[3];
get_s16Magn_y_buf[3] = get_s16Magn_y_buf[2];
get_s16Magn_y_buf[2] = get_s16Magn_y_buf[1];
get_s16Magn_y_buf[1] = get_s16Magn_y_buf[0];
get_s16Magn_y_buf[0] = get_s16Magn[1];
get_s16Magn[1] = (get_s16Magn_y_buf[0]+get_s16Magn_y_buf[1]+get_s16Magn_y_buf[2]+get_s16Magn_y_buf[3]+get_s16Magn_y_buf[4]+get_s16Magn_y_buf[5]+get_s16Magn_y_buf[6]+get_s16Magn_y_buf[7]+get_s16Magn_y_buf[8]+get_s16Magn_y_buf[9])/10;
get_s16Magn_z_buf[9] = get_s16Magn_z_buf[8];
get_s16Magn_z_buf[8] = get_s16Magn_z_buf[7];
get_s16Magn_z_buf[7] = get_s16Magn_z_buf[6];
get_s16Magn_z_buf[6] = get_s16Magn_z_buf[5];
get_s16Magn_z_buf[5] = get_s16Magn_z_buf[4];
get_s16Magn_z_buf[4] = get_s16Magn_z_buf[3];
get_s16Magn_z_buf[3] = get_s16Magn_z_buf[2];
get_s16Magn_z_buf[2] = get_s16Magn_z_buf[1];
get_s16Magn_z_buf[1] = get_s16Magn_z_buf[0];
get_s16Magn_z_buf[0] = get_s16Magn[2];
get_s16Magn[2] = (get_s16Magn_z_buf[0]+get_s16Magn_z_buf[1]+get_s16Magn_z_buf[2]+get_s16Magn_z_buf[3]+get_s16Magn_z_buf[4]+get_s16Magn_z_buf[5]+get_s16Magn_z_buf[6]+get_s16Magn_z_buf[7]+get_s16Magn_z_buf[8]+get_s16Magn_z_buf[9])/10;
//这里面获取更新实时参数，使用时可设置采样开关及采样时间
/*¸üÐÂMX MAX*/
if(get_s16Magn[0] > get_s16Magn_x_MAX_AVE)
{
get_s16Magn_x_MAX_AVE = get_s16Magn[0];
}
/*¸üÐÂMY MAX*/
if(get_s16Magn[1] > get_s16Magn_y_MAX_AVE)
{
get_s16Magn_y_MAX_AVE = get_s16Magn[1];
}
/*¸üÐÂMZ MAX*/
if(get_s16Magn[2] > get_s16Magn_z_MAX_AVE)
{
get_s16Magn_z_MAX_AVE = get_s16Magn[2];
}
/*¸üÐÂMX MIN*/
if(get_s16Magn[0] < get_s16Magn_x_MIN_AVE)
{
get_s16Magn_x_MIN_AVE = get_s16Magn[0];
}
/*¸üÐÂMY MIN*/
if(get_s16Magn[1] < get_s16Magn_y_MIN_AVE)
{
get_s16Magn_y_MIN_AVE = get_s16Magn[1];
}
/*¸üÐÂMZ MIN*/
if(get_s16Magn[2] < get_s16Magn_z_MIN_AVE)
{
get_s16Magn_z_MIN_AVE = get_s16Magn[2];
}
//磁力计校正
MXH = get_s16Magn[0]-(get_s16Magn_x_MAX_AVE+get_s16Magn_x_MIN_AVE)/2;
MYH = 1*(get_s16Magn[1]-(get_s16Magn_y_MAX_AVE+get_s16Magn_y_MIN_AVE)/2);
MZH = 1*(get_s16Magn[2]-(get_s16Magn_z_MAX_AVE+get_s16Magn_z_MIN_AVE)/2);
//更新磁力轴数据
MXH = MXH*cos(pitch_raw*3.14/180); //注意弧度与角度的转换
MYH = MYH*cos(roll_raw*3.14/180);
MZXH = MZH*sin(pitch_raw*3.14/180);
MZYH = MZH*sin(roll_raw*3.14/180);
printf("%.1f,\r\n", atan2((MYH-MZYH),(MXH-MZXH))*180/3.14159f); //OK 注意弧度与角度的转换
原文地址：https://blog.csdn.net/uyoev123abc/article/details/143804234
免责声明：本站文章内容转载自网络资源，如本站内容侵犯了原著者的合法权益，可联系本站删除。更多内容请关注自学内容网（zxcms.com）！