本期核心路径点不是 GPS 打的而是遥控 纯惯导推算打的。轮速里程计 IMU 航向积分出 XY 坐标人工按键逐点记录再经骨架化与 Catmull-Rom 插值变成可跟踪路径。1. 先想清楚精度决定定位方案路径记录的核心是车在哪。候选方案有两个GPS绝对坐标但智能车赛的赛道是厘米级精度要求GPS 米级漂移根本没法用惯导航位推算用轮速积分算位移、用 IMU 航向算方向相对坐标、精度高但会随时间漂移。比赛的结论是精度 长时间漂移——赛道几十米、跑几圈就结束惯导的漂移还没积累多少就完成了而 GPS 的米级误差从头到尾都是致命的。所以最终方案遥控打点 纯惯导定位。GPS 只在代码里留了个可调的占比实际权重设为 0。经纬度(lat,lon) ──(仅调试/备用)──▶ UTM平面 轮速 IMU航向 ──▶ 惯导积分 ──▶ (x, y) ← 真正用的坐标2. 互补滤波GPS 与惯导的可调占比代码里有一套类 EKF 的互补滤波GPS.c的Filter_Update。它的结构是惯导推算 GPS 修正两者占比由一个权重系数决定floatGPS_FIX_WEIGHT0.0f;/* GPS 修正权重0.10f ≈ 10%这里为 0 → 纯惯导 */voidFilter_Update(floatv,floatw,floatgps_x,floatgps_y,uint8 gps_updated,floatdt){/* ---- 1. 惯导推算 (Predict)轮速积分 IMU 航向 ---- */ekf_theta(imu660rc_yawGPS_yaw_bias)*DEG_TO_RAD;/* 航向 IMU yaw */ekf_xv*cosf(ekf_theta)*dt;/* 位移 速度 × cos(航向) */ekf_yv*sinf(ekf_theta)*dt;/* ---- 2. GPS 修正 (Update)权重可调 ---- */if(gps_updated){floatcurrent_weightGPS_FIX_WEIGHT/(1.0ffabsf(v)*0.005f);ekf_xcurrent_weight*(gps_x-ekf_x);/* 向 GPS 拉一点 */ekf_ycurrent_weight*(gps_y-ekf_y);}}这套结构的价值一个权重切换两种定位GPS_FIX_WEIGHT 0→ 纯惯导调大 → 引入 GPS 修正。占比可调是给不同场景留的旋钮速度自适应降权weight / (1 |v|*0.005)—— 车跑得越快、惯导越准GPS 修正占比就越小GPS 延迟补偿预留gps_latency补偿量把过去的 GPS 点推算到现在实际因不用 GPS 而置 0但思路保留着。这个滤波器的架构意义在于惯导定趋势、GPS 纠绝对位置中间一个权重旋钮调节信任谁。比赛把旋钮拧到全信惯导但框架是完整的。3. 惯导数据从哪来ISR 里的实时推算纯惯导坐标current_x/current_y是在1ms 定时中断里实时推算出来的cm7_0_isr.cif(run_flag1||run_flag2){/* 速度左右轮速平均 → 换算成 m/s */floatv_ekfCar_Speed/60.0f*Wheel_Cir;/* 角速度IMU 的 z 轴角速度 */floatw_ekfimu660rc_gyro_transition(imu660rc_gyro_z)*DEG_TO_RAD;Filter_Update(v_ekf,w_ekf,GPS_current_x,GPS_current_y,gps_flag,0.001f);current_xekf_x;/* 惯导坐标 (cm) */current_yekf_y;}为什么放 1ms 中断航位推算是对时间的积分必须严格等时采样。1ms 中断一拍积一次dt 0.001s恒定积分才准。放主循环会被图像处理等慢任务卡住积分就漂。注意遥控模式run_flag2下强制gps_flag 0即遥控打点时 GPS 修正完全关闭——纯惯导定位确认无疑。4. 打点方式遥控 菜单选类型按键逐点记录拿到惯导坐标后怎么打点看my_menu.c的func_gps_record_point——人工按键、逐点记录voidfunc_gps_record_point(void){uint8 flag0;/* 1. 菜单光标决定这个点的路段类型 */if(cursor_index0){flag0;type_nameNormal;}elseif(cursor_index1){flag1;type_nameJump;}elseif(cursor_index2){flag2;type_nameUnilateral;}elseif(cursor_index3){flag3;type_nameTransverse;}elseif(cursor_index4){flag4;type_nameHigh Speed;}elseif(cursor_index5){flag5;type_nameSpin;}elseif(cursor_index6){flag7;type_nameSlope;}if(record_mode1){/* 2. 直接抓取底层纯惯导的实时坐标 */floatfinal_xcurrent_x;floatfinal_ycurrent_y;GPS_track[GPS_track_index][0]final_x;GPS_track[GPS_track_index][1]final_y;GPS_path_flag[GPS_track_index]flag;/* 记录路段类型 */GPS_track_index;}/* record_mode 0 时才走 GPS 求平均打点实际没用上 */}这个设计的关键点点带类型每个点不只是坐标还带flag跳跃/单边桥/坡道/自旋……——这就是路段感知的数据来源后面路径跟踪靠它识别路段人工按键触发遥控车走到关键位置比如坡道起点、跳跃点按一下记一个点。点是人打的不是自动采样的——因为关键路段的位置需要人工判断惯导坐标 cm 级current_x/current_y是 cm 单位的惯导坐标打出来就能直接用。这套人工打点 类型标注的流程本质上是把人的赛道知识编码进路径。车复现时按着这些带类型标签的点走就知道哪里该跳、哪里该减速。5. 骨架化与 Catmull-Rom 插值打点路径是稀疏的、带类型的离散点直接连线段会有折角、密度不匀。所以和上一版思路一致分两步处理5.1 骨架化Skeleton降噪整理skeleton_track[skeleton_count].xGPS_track[i][0];skeleton_track[skeleton_count].yGPS_track[i][1];skeleton_track[skeleton_count].flag...;/* 继承路段类型 */普通点直接进骨架锥桶区整段取出做最小二乘直线拟合 法向偏移生成避障路径。骨架层只保留几何关键点 类型标签。5.2 Catmull-Rom 插值加密for(inti0;isk_cnt-1;i){/* 取四控制点 P0..P3Catmull-Rom 参数化 */intsteps(int)(d1/step_dist);/* 按 2cm 步长加密 */for(intj1;jsteps;j){doubleA1lerp(P0,P1,t);doubleA2lerp(P1,P2,t);doubleA3lerp(P2,P3,t);doubleB1lerp(A1,A2,t);doubleB2lerp(A2,A3,t);doubleClerp(B1,B2,t);/* 曲线上的点 */GPS_track_filted[count]C;/* 加密后的路径点 */...沿途继承路段 flag...count;}}过点平滑Catmull-Rom 穿过所有打点转弯自然步长加密点间距均匀约 2cm后续跟踪的最近点搜索才均匀标志沿途继承插值点继承路段类型车走到哪都知道前方是什么路段。6. Flash 持久化录好的路径存 Flash断电不丢。结构与思路/* 写参数头GPS偏置/曝光/阈值/路段时长→ 每个打点坐标类型锥桶幅度方向 */flash_union_buffer[bi].float_typeGPS_track[i][0];flash_union_buffer[bi].float_typeGPS_track[i][1];.../* 读带回合法性校验防脏数据 */if(GPS_track_indexGPS_TRACK_MAX)GPS_track_indexGPS_TRACK_MAX;if(rd10.0frd1100.0f)s-jump_distance_1rd1;/* 范围检查 */写死板、读校验——嵌入式存储的通用铁律。7. 完整流水线回顾[遥控推车] [菜单选类型 按键] │ │ ▼ ▼ 轮速IMU ──1ms积分──▶ 惯导坐标(x,y) ──人工打点──▶ 带类型打点序列 │ 骨架化锥桶拟合/降噪 ◀────┘ ▼ Catmull-Rom 插值加密 │ ▼ 连续路径(带路段flag) → Flash核心结论定位靠纯惯导轮速IMU 积分打点靠人工按键类型靠菜单标注。GPS 作为可调权重的备用修正源存在但比赛中权重为 0。8. 本期小结定位方案纯惯导航位推算轮速积分 IMU 航向GPS 权重可调但实际为 0互补滤波惯导推算 GPS 修正GPS_FIX_WEIGHT旋钮控制占比人工打点遥控 菜单选路段类型 按键逐点记录点带 flag骨架化 Catmull-Rom降噪、过点平滑、沿途继承类型Flash 分页存储 读回校验。9. 下期预告路径有了接下来是最难的一环纯追踪Pure Pursuit路径跟踪。我们会讲清楚车怎么判断我在路径上吗怎么用前瞻点转弯急弯、特殊路段跳跃/坡道/单边桥又是怎么分阶段处理的欢迎关注持续连载中。