姿态解算笔记1-浅谈陀螺仪和加速度计的原理和误差模型加速度计原理和误差模型MEMS加速度计可以测量到他自身受到的加速度,包括重力加速度和线性加速度.他是通过悬挂在内部的质量块,连着一个微弹簧,惯性力会改变电容值,从而测量出加速度他的输出是m/s^2比如ICM-45686包含一个三轴的加速度计,这是他的技术指标:可见,他的最大量程有32个G,也就是最大可以测量32*9.81314m/s^2的加速度,但是实际上我们用不到这么高的量程,如果你配置成这么大的量程,会导致一个LSB代表的加速度太大,分辨率会大大降低,根据我的经验,一般在平衡车和无人机的应用中,选择4G或者2G即可.误差模型对这个MEMS加速度计的误差来源进行归纳总结,可以得到误差模型:a O a − 1 ( S a a m b m ) N a a O_a^{-1} (S_a a_m b_m) N_aaOa−1(Saambm)Na其中,a aa是真实的加速度,为三维列向量,O a O_aOa为非正交误差,S a S_aSa为尺度因子误差,b m b_mbm为偏置,N a N_aNa为噪声由于这个器件的非线性误差很小,才0.01%,可以认为是完全线性的,这里不做讨论.偏置偏置就是说,如果运载体实际没有运动,由于结构和工艺限制,加速度计也会敏感到一个测量值,这个器件大概为0.01*9.810.1m/s^2,其实这个值很大了,如果直接使用加速度计去做二重积分得到位移,那么在静止下,10s内位移就会漂移5m的距离,所以如果你想只用这个加速度计去做位置估计,基本是不可能的,性能很差.偏置是最重要的误差来源,他对整个系统的误差影响可以占80%以上,所以他必须补偿.尺度因子误差由于工艺限制,一个轴测到的加速度是10,实际可能是11,这就是尺度因子误差,就相当于给实际加速度乘以了一个缩放系数,这个器件的尺度因子误差是0.2%这在低成本MEMS中,已经很小了,可以忽略不计.非正交误差其中非正交误差表示三个加速度计在设计时,他们是不是完全的正交的,由于工艺限制,他们之间可能不是精确的90度,此时,一个加速度计的测量的值可能反应到另外一个加速度计上,比如平放在桌面上按道理来说,没有误差的情况下,xy应该测量到0,z轴测量到9.81.但是由于非正交误差的存在,可能z轴测量到是9.76,x是0.02,y是-0.05(这里不太准确,实际应该对重力加速度分解才知道每个轴是多少).此时就是有些加速度的值耦合到其他轴上去了,这就是非正交误差,他可以用一个矩阵给缩放拉伸回来,数学好的朋友可能发现了,所谓的非正交误差,其实就是一个坐标系变换到另一个坐标系去了,所以可以用他的逆矩阵补偿回来,如果没有非正交误差,O a O_aOa应该是这样的:O a [ 1 0 0 0 1 0 0 0 1 ] O_a \begin{bmatrix} 1 0 0 \\ 0 1 0 \\ 0 0 1 \end{bmatrix}Oa100010001这说明,每一个轴的数据都完全出去了,另外的轴不会影响他的输出结果.如果有一些非正交误差,他就可能长这样:O a [ 0.98 0.03 − 0.01 0.04 1.02 − 0.02 − 0.02 0.03 1.01 ] O_a \begin{bmatrix} 0.98 0.03 -0.01 \\ 0.04 1.02 -0.02 \\ -0.02 0.03 1.01 \end{bmatrix}Oa0.980.04−0.020.031.020.03−0.01−0.021.01这说明,有一些数据跑到其他的轴上去了,但是大部分还是在对角线位置的.这就会带来误差,但是我们的这个芯片,非正交误差仅有0.2%,已经非常小了,可以忽略不计.直观展示很直观的展现各个误差项的影响(由于非正交误差涉及三个轴,这里不好展示):可见,如果实际测量是10,有偏置的话测出来是11,有尺度因子误差,测出来是9,要命的是,如果有偏置影响,实际是0,测量不会为0,这个影响是很大的.陀螺仪原理和误差模型陀螺仪是通过内部悬浮有 Proof-Mass 的振荡梁,旋转时出现科里奥利力,然后把电容值做差分,就能知道转动的角速度,他输出是deg/s.陀螺仪作为惯导的核心器件,ICM-45686的陀螺仪的精度相比于MPU6050这十几年前的产品好太多了可见,陀螺仪的性能指标非常好.非线性度只有0.05%,非正交误差只有0.2%,尺度因子误差也只有0.2%,在我使用时,这些都忽略不计了,只补偿零偏.可见,他的零偏是0.3deg/s,这是一个很好的指标,噪声功率谱密度很低,在我实际使用时,只要减掉零偏,他的性能就很好了.误差模型同样的,对这个MEMS陀螺仪的误差来源进行归纳总结,可以得到误差模型:g O g − 1 ( S g g m b m ) N g g O_g^{-1} (S_g g_m b_m) N_ggOg−1(Sggmbm)Ng其中,g gg是真实的角速度,为三维列向量,O g O_gOg为非正交误差,S g S_gSg为尺度因子误差,b m b_mbm为偏置,N g N_gNg为噪声.基本和加速度计的一样的误差模型,这里不多赘述,因为在使用中,对于加速度计和陀螺仪,我们都只会补偿零偏误差这一项.需要指出的是,温度会对零偏产生较大的影响,实测温度差超过3摄氏度时,误差就会比较大,所以建议对IMU进行温度查表补偿,或者恒温控制,也可以用EKF来估计零偏.零偏校准C代码这是我写的AHRS的校准静态偏置的代码,具体完整可以参考我的github:JackTang543/sGCARCv5其实就是一个取平均值的过程,然后在后续读取时减掉int AHRS::calcBias(uint16_t points,IMU_StaticBias imu_sbias){ float acc_x_accu 0; float acc_y_accu 0; float acc_z_accu 0; float gyro_x_accu 0; float gyro_y_accu 0; float gyro_z_accu 0; for(uint16_t i 0; i points; i){ acc_x_accu raw_data.acc_x; acc_y_accu raw_data.acc_y; acc_z_accu raw_data.acc_z; gyro_x_accu raw_data.gyr_x; gyro_y_accu raw_data.gyr_y; gyro_z_accu raw_data.gyr_z; vTaskDelay(5); } imu_sbias.acc_x acc_x_accu / points; imu_sbias.acc_y acc_y_accu / points; imu_sbias.acc_z acc_z_accu / points - M_GRAVITY; //重力加速度 NED坐标系 imu_sbias.gyr_x gyro_x_accu / points; imu_sbias.gyr_y gyro_y_accu / points; imu_sbias.gyr_z gyro_z_accu / points; return 0; }我写了一个MATLAB脚本,STM32端读取60s的陀螺仪和加速度数据,然后做噪声功率谱分析的结果:Estimated Fs 198.60 Hz (std 0.000228 s) Gyro X-axis: PSD platform 1.599e-06 (deg/s)^2/Hz Noise density 1.2645 mdps/√Hz ARW 0.076 deg/√h Q_g (EKF) 3.175e-04 (deg/s)^2 Gyro Y-axis: PSD platform 1.657e-06 (deg/s)^2/Hz Noise density 1.2871 mdps/√Hz ARW 0.077 deg/√h Q_g (EKF) 3.290e-04 (deg/s)^2 Gyro Z-axis: PSD platform 1.718e-06 (deg/s)^2/Hz Noise density 1.3108 mdps/√Hz ARW 0.079 deg/√h Q_g (EKF) 3.412e-04 (deg/s)^2具体代码:%% gyro_noise_spectrum.m% 计算静止状态下陀螺仪三轴的功率谱密度(PSD)检查噪声是否接近白噪声% by SIGHTSEER 2025-05%% 0. 读取/预处理数据gyr_xdat.gyr_x(:);gyr_ydat.gyr_y(:);gyr_zdat.gyr_z(:);t_msdat.ts_ms(:);% 时间戳 [ms]% 采样频率 -------------------------------------------------------------dtdiff(t_ms)/1000;% Δt [s]Fs1/mean(dt);% 采样频率 [Hz]fprintf(Estimated Fs %.2f Hz (std %.3g s)\n,Fs,std(dt));% 去掉直流分量静止时平均值------------------------------------------gyr_xgyr_x-mean(gyr_x);gyr_ygyr_y-mean(gyr_y);gyr_zgyr_z-mean(gyr_z);%% 1. 计算 FFT 并转换成单边功率谱密度axesCell{gyr_x,X;gyr_y,Y;gyr_z,Z};Nlength(gyr_x);% 样本点数假设三轴长度一致fFs*(0:floor(N/2))/N;% 频率轴 (0 ~ Nyquist)figure(Name,Gyro noise PSD);fork1:3xaxesCell{k,1};% 加窗汉宁窗降低频谱泄漏whann(N);Xffft(x.*w);% 功率谱密度 (单位: (deg/s)^2 / Hz)Pxx(abs(Xf)/sum(w)).^2/Fs;% 双边Pxx2*Pxx(1:length(f));% 单边去掉负频率并能量翻倍subplot(3,1,k);loglog(f,Pxx);grid on;title([Gyro ,axesCell{k,2},-axis PSD]);xlabel(Frequency (Hz));ylabel(PSD ((deg/s)^2/Hz));end%% --- 3. 计算噪声密度 / ARW / Q_g ------------------------------------% 使用 Welch 平均更稳健nFFT4096;windowhann(2048);noverlap1024;axesCell{gyr_x,X;gyr_y,Y;gyr_z,Z};fork1:3xaxesCell{k,1};[pxx,fWel]pwelch(x,window,noverlap,nFFT,Fs,onesided,power);% 取 1 Hz – Fs/4 之间的平坦段平均作为 PSD 平台idxfWel1fWelFs/4;Pxx_flatmean(pxx(idx));% (deg/s)^2 / Hzsigma_wsqrt(Pxx_flat);% deg/s / sqrt(Hz)ARW_degsigma_w*sqrt(3600);% deg / sqrt(h)Qgsigma_w^2*Fs;% (deg/s)^2fprintf(\nGyro %s-axis:\n,axesCell{k,2});fprintf( PSD platform %.3e (deg/s)^2/Hz\n,Pxx_flat);fprintf( Noise density %.4f mdps/√Hz\n,sigma_w*1e3);fprintf( ARW %.3f deg/√h\n,ARW_deg);fprintf( Q_g (EKF) %.3e (deg/s)^2\n,Qg);end下一节,我们将尝试使用RK4实时迭代四元数微分方程,利用来自三轴陀螺仪的数据求解当前姿态信息.bySightseer. inHNIP9607.250510