企业官网建设流程全解析

IMU963RA 航向积分精读：零偏、加权滤波、角度环绕与 GPS 角差耦合

本篇只盯code/IMU_1.c/IMU_1.h里几十行imu()，却串起：陀螺零偏、三采样加权滤波、量程换算、周期积分、双重角度环绕、与 Follow_track 的符号关系。
仓库：https://github.com/shuifanyu/TC264-GPS-Vision-Car

目录

1. 这段代码在系统里被谁调用
2. 全局状态机变量
3. imu() 数据流总览
4. 陀螺原始值与零偏 average
5. 三采样加权滤波在算什么
6. 量化截断 test=(int)test/10*10
7. 从 LSB 到 deg：系数 14.3
8. 周期积分：YAW 与 gyro_dt
9. Angle_z 与 angle_light：两套环绕
10. 与 GPS Nomal_Error 的契约
11. 代码问题清单
12. 可运行的改进版骨架
13. 实验与调试
14. 小结

1. 这段代码在系统里被谁调用

isr.c：

// CCU60_CH0, 20msif(IMU_1_Open_flag==1){imu();}

core0_main.c：

imu963ra_init();// 先初始化驱动pit_ms_init(CCU60_CH0,20);

菜单叶子页（如fun_c33）会：

IMU_1_Open_flag=1;ips200_show_float(...,angle_light,...);

契约：

菜单：置 IMU_1_Open_flag CCU60：20ms 调 imu() imu()：更新 angle_light / Angle_z GPS Follow_track：读 angle_light 与 Azimuth 做差

若 flag=0，angle_light停止积分，GPS 角差会冻结在旧值——惯导模式前必须先开 IMU。

2. 全局状态机变量

intIMU_1_Open_flag=0;intI_navigation_flag=0;intG_navigation_flag=0;floataverage;// 陀螺零偏（静止平均）floatYAW;// 本周期角度增量（deg）floatAngle_z;// 0~360 连续航向floatangle_light;// -180~180 航向（GPS 用）floattest;// 滤波后的原始 gyro（调试）

flags 定义在 IMU_1.c，头文件 extern——模式开关与 IMU 数据绑在同一翻译单元，耦合偏紧，但竞赛里好找。

3. imu() 数据流总览

imu963ra_get_gyro() → gyro_raw = imu963ra_gyro_z → gyro = gyro_raw - average // 去零偏 → 三采样加权：0.5, 0.3, 0.2 → test = 量化截断 → YAW = -(test / 14.3) * gyro_dt // 增量角 → Angle_z += YAW → wrap [0,360) → angle_light += YAW → wrap [-180,180)

这是典型捷联式偏航（yaw）速率积分的极简竞赛实现：只用 Z 轴陀螺，没有磁力计融合、没有加速度计水平修正。

4. 陀螺原始值与零偏 average

gyro=((float)imu963ra_gyro_z-average);

4.1 为什么要减 average

MEMS 陀螺即使静止也有输出零偏 bias（温度相关）。
若不减：

Angle += (bias/LSB_scale)*dt → 航向持续单向漂

GPS 角差会慢慢歪掉，表现为“直道越跑越偏”。

4.2 average 从哪来

文件里average初始化为 0（BSS）。

注释掉的imu_up()意图是：静止采 20 次 gyro_z 求平均赋给 average。

//void imu_up() {// ...// for(i=0;i<20;i++){ imu963ra_get_gyro(); data[2]+=imu963ra_gyro_z; }// average = data[2]/20;//}

现状问题：若从未调用标定，average=0，滤波后的 gyro 仍含系统偏置。
上电应：

imu963ra_init();system_delay_ms(10);imu_calib_gyro_bias(200);// 车体静止

5. 三采样加权滤波在算什么

floatgyro=0;floatgyro_less=0;floatgyro_last=0;gyro_last=gyro_less;gyro_less=gyro;gyro=(float)imu963ra_gyro_z-average;gyro=0.5f*gyro+0.3f*gyro_less+0.2f*gyro_last;

5.1 权重

[
g_f[k]=0.5,g[k]+0.3,g[k-1]+0.2,g[k-2]
]

和为 1，直流增益 1，本质是3 抽头 FIR 低通。

5.2 严重实现问题：局部变量

gyro / gyro_less / gyro_last是每次进入 imu() 都新建的局部变量，初值 0。

因此：

gyro_last ← 0 gyro_less ← 0 gyro ← 本次 raw 滤波结果 ≈ 0.5 * raw （0.3*0 + 0.2*0）

滤波器实际上没有跨周期记忆，只相当于把当前值乘了约 0.5（还抬高了有效零偏/缩放关系）。

正确做法：三个变量应为static或放到文件作用域。

staticfloatgyro_f1=0,gyro_f2=0;floatg0=raw-bias;floatgf=0.5f*g0+0.3f*gyro_f1+0.2f*gyro_f2;gyro_f2=gyro_f1;gyro_f1=g0;

5.3 权重设计意图（在实现修好后）

比单极点 IIR 参数更直观，适合比赛手调。

6. 量化截断 test=(int)test/10*10

test=gyro;test=(int)test/10*10;

表达式解析

C 中(int)test / 10 * 10：

1. (int)test向零截断成整型
2. /10整数除法（丢弃余数）
3. *10恢复到十位步进

例：

意图

把陀螺量化到10 LSB 档，抑制小抖动（死区+粗量化）。

副作用

若要做死区，更清晰：

if(fabs(g)<DEADBAND)g=0;elseg=copysign(fabs(g)-DEADBAND,g);// 或只保留原值

7. 从 LSB 到 deg：系数 14.3

YAW=-(float)((test)/14.3f)*gyro_dt;// 注释里还出现过 16.4f

7.1 灵敏度

常见 IMU：若 FS=±2000 dps，LSB 灵敏度约16.4 LSB/(°/s)。
本码用14.3，可能是：

• 另一量程标定结果
• 人工“凑方向/凑幅度”
• 与滤波后 0.5 增益一起补偿

工程做法：用速率转台或“转 360° 看 Angle_z”标定 scale，使ΔAngle≈真实角。

7.2 负号

YAW = - (gyro/scale) * dt

负号定义航向增加与右手系/陀螺 z 正方向相反。
必须与Follow_track里：

Nomal_Error=Azimuth-angle_light;

以及舵机“正 error → 向哪打”一起标定。三处符号不一致会导致正反馈狂转。

7.3 gyro_dt=0.04

floatgyro_dt=0.04;// 40ms

但中断是pit_ms_init(CCU60_CH0, 20)→ 20ms。

除非实际周期是 40ms，否则这是标定/配置不一致的高优先级问题。
应写死共享宏：

#defineIMU_SAMPLE_DT0.02f

并在isr与imu()共用。

8. 周期积分：YAW 与 gyro_dt

[
\theta[k]=\theta[k-1]+\omega[k]\cdot\Delta t
]

YAW=-(test/14.3f)*gyro_dt;Angle_z+=YAW;angle_light+=YAW;

离散积分误差来源

在短时比赛科目（几十秒）上，积分航向常仍可用；长时间必须磁/GPS 校正。

9. Angle_z 与 angle_light：两套环绕

// Angle_z: 保持在 [0, 360)if(Angle_z>360)Angle_z-=360;elseif(Angle_z<0)Angle_z+=360;// angle_light: 保持在 (-180, 180]if(angle_light>180)angle_light-=360;elseif(angle_light<-180)angle_light+=360;

9.1 为何两套

9.2 边界条件瑕疵

稳健 wrap：

staticfloatwrap180(floata){while(a>180.f)a-=360.f;while(a<-180.f)a+=360.f;returna;}

10. 与 GPS Nomal_Error 的契约

GPS.cFollow_track：

Azimuth=get_two_points_azimuth(...);// 目标方位角if(Azimuth>=180)Azimuth-=360;// 与 angle_light 做环绕差if(Azimuth-angle_light>180)Nomal_Error=Azimuth-angle_light-360;elseif(Azimuth-angle_light<-180)Nomal_Error=Azimuth-angle_light+360;elseNomal_Error=Azimuth-angle_light;

10.1 隐含约定

Azimuth 使用 ±180 域（经 >=180 调整后） angle_light 使用 ±180 域 Nomal_Error = 目标方位 - 车体航向（已 wrap）

10.2 基准方向

Follow_track注释：

// Azimuth+=90 正东发车// 对正北发车：Azimuth>=180 → Azimuth-=360

说明GPS 方位角基准与车头朝向的 angle_light=0必须对齐。
现场流程：

车头指向赛道正北（或既定基准） 上电静止标定 average 将 angle_light 归零（或保证积分从 0 起） 再开 G_navigation

10.3 两套传感器时间基准

GPS 解析：主循环，低频（1~10Hz） IMU 积分：CCU60，20ms（或代码里的 40ms） 导航误差：CCU61，5ms

angle_light在 5ms 导航里是保持的阶梯值（最多 20ms 更新一次）。
对航向控制通常可接受；若要更细，可把 IMU 积分改到 5~10ms。

11. 代码问题清单

12. 可运行的改进版骨架

#defineIMU_DT0.02f#defineGYRO_SCALE16.4f/* 按标定修改 */#defineDEADBAND3.0fstaticfloatg_f1,g_f2;floatangle_light=0.0f;voidimu_calib(intn){inti;floatsum=0;for(i=0;i<n;i++){imu963ra_get_gyro();sum+=(float)imu963ra_gyro_z;system_delay_ms(2);}average=sum/n;g_f1=g_f2=0;angle_light=0;}voidimu(void){floatraw,g0,gf,dyaw;imu963ra_get_gyro();raw=(float)imu963ra_gyro_z;g0=raw-average;if(g0>-DEADBAND&&g0<DEADBAND)g0=0;gf=0.5f*g0+0.3f*g_f1+0.2f*g_f2;g_f2=g_f1;g_f1=g0;dyaw=-(gf/GYRO_SCALE)*IMU_DT;angle_light=wrap180(angle_light+dyaw);Angle_z=wrap360(Angle_z+dyaw);}

与菜单配合：进入 IMU 页先imu_calib(200)，再IMU_1_Open_flag=1。

13. 实验与调试

日志建议：

raw, bias, gf, dyaw, angle_light, Azimuth, Nomal_Error

用 20ms 节拍打点，或菜单显示。

标定 scale 的快速法

缓慢转 5 圈，记 ΔAngle_z 累计 真实 1800° scale_new = scale_old * (Δangle_code / 1800) 同时核对 dt

14. 小结

imu()很短，却覆盖嵌入式感知核心链：

传感器读数 → 去零偏 → 滤波 → 标度 → 按周期积分 → 角度域约束 → 作为 GPS 角差的车体航向

本篇同时指出竞赛代码里滤波状态丢失、dt 与 PIT 不一致、零偏未标定等实锤问题——精读的价值正在于：能把“能跑”和“原理正确”区分开。

作者：shuifanyu
标签：IMU963RA陀螺仪航向积分智能车嵌入式代码精读

源码：code/IMU_1.c、user/isr.c、code/GPS.cFollow_track
仓库：https://github.com/shuifanyu/TC264-GPS-Vision-Car