全国大学生智能车竞赛的极速越野组，可能是近几届里最“拧巴”的一个组别：赛道没有路肩、没有边界线，光照多变、视野开阔，摄像头能看到的特征少得可怜，电磁更是无从谈起，摆明了就是要逼你用卫星定位。但真把GPS模块焊上板子去试车，你会发现它根本不是想象中那样“指哪打哪”——更新率低、精度波动、信号跳变，稍微跑快点就画龙。这也是为什么越野组的导航方案绕不开惯导融合：GPS负责绝对位置，惯性测量单元负责短时高频率的姿态和位移推算，两者一快一慢、一绝对一相对，配合好了才能稳跑全场。
这篇文章不扯虚的，直接按我实际调通一套“GPS+惯导融合定位导航系统”的经验来拆解，覆盖从硬件选型、数据对齐、滤波融合到转向控制、现场排查的完整链路。适合正在备赛的队员、指导老师，以及所有想了解低成本组合导航到底怎么落地的人。
1. 越野组导航方案的底层逻辑：为什么必须走GPS加惯导
极速越野组的赛道画在室外空地，通常是几百米一圈的行车路径，宽度在3到5米不等。摄像头方案在室内电磁组玩得转，是因为有路肩、有边界、有固定参照，但到了空旷越野场地上，太阳角度一变、地面颜色一换，图像特征就崩了。所以从第19届前后开始，越野组逐渐转向以GPS为绝对导航基准，再用惯性传感器补位，这套技术路线基本成了各强队的共识。
1.1 单靠GPS根本没法直接当控制信号用
这里要先说清楚GPS的天生短板。消费级GPS模块的定位精度标称在2.5米左右（CEP，即50%概率落在半径2.5米的圆内），实际开阔场地动态跑起来，误差在1到3米波动都很正常。越野组赛道宽度只有3到5米，你要是把GPS点直接丢给转向控制器，车头方向会跟着误差来回抽搐，车速一上来直接冲出跑道。
另外一个致命问题就是更新率。普通GPS模块的NMEA数据输出通常只有5到10赫兹，赛事规则又明确不允许用RTK差分设备，所以你把GPS当作唯一位置源，控制器对车体运动的感知就存在100到200毫秒的盲区。车以5米每秒的速度前进，200毫秒已经跑出去1米了，这中间路径曲率的变化完全靠猜。
GPS还有一个“飘移”特性容易被新手忽略：它在静止时输出的坐标也不是固定点，而是会在真实位置附近随机游走；动态时受卫星几何分布、多径效应影响，偶尔还会出现几米的跳变。跳变的本质是GPS解算过程中的整周模糊度变化、可见星数量切换等因素，不是你在软件里做个低通滤波就能随便滤掉的，因为低通滤波会把真实急转弯一起滤平。
1.2 惯导的作用是“填空”而不是“替代”
惯性测量单元（IMU）包括三轴加速度计和三轴陀螺仪，以几百甚至上千赫兹的频率输出原始数据。通过对角速度积分可以得到姿态变化，通过把加速度经过姿态矩阵投影到导航坐标系再二次积分，可以得到位置增量。这个过程中不依赖任何外部信号，所以输出频率高、短期稳定性极好。
但惯导的长期误差是灾难性的：陀螺仪的零偏会让姿态估计缓慢漂移，姿态一偏，重力分量投影到水平轴就会产生虚假加速度，位置误差以时间的二次方甚至三次方增长。所以惯导只能在几百毫秒到几秒的时间尺度内“撑住场面”，时间一长必须用GPS来校正。
GPS和惯导的配合方式，简而言之就是：惯导负责把两个GPS采样点之间的位置变化过程补出来，GPS负责抑制惯导积分误差的积累。二者是典型的优势互补。所谓“松组合”融合，就是各自先独立解算，再把GPS输出的位置速度当作观测值去校准惯导的推算结果；“紧组合”则是更深一层地把卫星原始观测量纳入滤波，竞赛场景基本用不上，也超出绝大多数队伍的算力承受范围。
1.3 竞赛规则对传感器方案的隐性约束
越野组规则明面上只限制最高车速、电池电压和车模尺寸，但仔细读会发现它默认你不用差分设备，也没有激光雷达、视觉导航这类重传感器。很多队伍其实想过用UWB室内定位、用视觉标签辅助，但实操下来都会回到GPS加惯导这条主线，原因很简单：赛场环境不可控，UWB基站没法临时布设，视觉标签会被裁判系统的摆件挡住。倒不如把组合导航算法吃透，用最低成本的传感器做出稳定的系统。
2. 硬件搭建要点：GPS模块、陶瓷天线与IMU的选型和安装
融合算法再漂亮，硬件底子差也白搭。越野组对硬件的要求集中在几个方面：GPS接收机能不能稳定锁星、天线能不能不被车身干扰、IMU是不是真的和车体刚性固定。这三个问题不解决，后面全是玄学调参。
2.1 GPS模块怎么选：看芯片方案也要看输出协议
当前主流的消费级GPS芯片方案是u-blox的M8N、M9N，以及中科微的AT6558，后者的优势是便宜且支持北斗、GPS、GLONASS多系统联合定位，很多国产模块用的就是它。选模块时重点看以下几点：
- 支持系统数量和频段：双频（L1+L5）理论上抗多径更好，但体积和功耗更大；越野组用单频L1基本上是够的。
- 输出频率：很多模块默认1Hz，越野车必须调到5Hz或10Hz，所以买模块前先确认它支持配置输出频率，最好拿到手就连电脑用上位机改一遍。
- NMEA协议完整度：至少能稳定输出GGA、RMC语句，其中GGA带经纬度、定位质量和卫星数，RMC带速度和对地航向角。
- 是否有PPS脉冲脚：这个很多人忽略。PPS（每秒一个脉冲）可以用来做GPS和IMU的时间同步，做融合时非常有用，哪怕只是粗略同步也能显著改善观测值时序对齐的效果。
选模块时还有一个容易被低价产品坑到的点：部分模块的“陶瓷天线”和“接收板”是分开的，主机上电初始化速度慢，冷启动可能要几十秒，赛场上根本没时间等。所以备赛时一定要提前规划上电时序，最好在车模待命状态就保持GPS持续供电，避免冷启动。
2.2 GPS陶瓷天线的封装设计是很多人踩坑的地方
“GPS陶瓷片设计注意事项”成为热词不是没道理的。陶瓷贴片天线（patch antenna）是GPS接收链路里最容易被干扰也最被低估的一环，它的等效辐射中心只有硬币大，对周围的金属、走线、外壳材料极其敏感。
如果用的是带天线的一体化模块，注意模块尽量放在车模顶层，正上方不能有碳纤维板、金属支架、摄像头云台等遮挡物。碳纤维材料对GPS信号的衰减非常严重，我曾经见过一辆越野车因为给GPS模块上面压了一块碳纤维板，导致搜星数量从12颗掉到5颗，定位精度直接崩了。
如果项目选择了外置陶瓷天线和GPS接收板分离的方案，那设计要点更多：
- 陶瓷片的馈电点要和接收板的射频输入引脚方向一致，天线到接收板的馈线越短越好，长馈线会引入插入损耗，GPS信号本身就很微弱。
- 陶瓷天线的接地面要足够大，通常至少是陶瓷片面积的2倍以上。接地面的作用是为天线提供镜像电流，太小了谐振频率整体偏移，天线效率会明显下降，表现为锁星慢、信号弱。
- 天线四周尽量不要平行走长条金属或地线，这会改变天线的极化特性和方向图，导致某一朝向的信号接收特别差。
- 有源天线比无源天线多了一级LNA放大，抗馈线损耗能力强，但需要额外给供电（常见3.3V或5V），而且放大电路靠近天线，布局不合理容易自激，表现为收到大量伪信号、定位误判。
陶瓷天线的器件方向也需要注意：绝大多数陶瓷贴片天线的接收方向是“天顶方向”，也就是陶瓷片水平放置时正上方信号最好，倾斜或倒置安装都会让增益方向图变形。越野车高速转弯时车身会侧倾，这个影响不大，但如果天线装在底盘下面或者侧边，那信号衰减就非常明显了。
2.3 IMU选型与安装：刚性固定比器件型号更重要
越野组的IMU选择范围很宽，MPU6050、ICM20602、ICM42688等消费级六轴芯片都够用，英飞凌的TC系列板卡和逐飞/乾勤等商家做的成品模块也很常见。相比型号之争，这里想重点强调的是安装方式。
IMU必须绝对刚性固定在车架上，不能有减震海绵垫着，不能用双面胶软绵绵地粘着，更不能不固定直接放在底盘上。因为惯性测量本质是测量“芯片封装”的运动，芯片和车体之间任何柔性连接都会引入额外的高频抖动和相位滞后，导致姿态解算产生不可预测的误差。我见过有队伍因为IMU贴在电池上，结果电池的共振频率正好和车模颠簸频率耦合，姿态角输出周期性摆动，全场查了两天才发现是这种机械问题。
安装位置尽量靠近车辆中心，越接近质心，角速度和加速度测量越能反映整车运动。安装在车头也没有大问题，但你要在运动学建模时意识到传感器不在后轴中心，转向时存在杆臂效应，这在做严格组合导航时需要做杆臂补偿。竞赛工程下可以不那么讲究，但在传感器和车体坐标系标定时，三个轴的方向一定要和车体坐标系定义一致，建议IMU的x轴对准车头前进方向，y轴朝左，z轴垂直向上，然后和GPS输出的航向角统一对准。
2.4 供电与地线的细节
GPS模块对电源纹波比较敏感，电机大电流瞬间可能把电源拉低两三百毫伏，导致GPS模块复位重启或定位跳变。所以在系统供电上要避免GPS和电机驱动共用一根电源线，最好从电池经过独立的LDO或DC-DC稳压后供给GPS。IMU也一样，数字地要单点接地，避免电机驱动的大电流在地线上产生地弹噪声，否则陀螺仪数据会出现毛刺。这个问题在纯软件上很难补救，只能在硬件设计时提前规避。
3. 数据链路全打通：GPS解算、惯性姿态解算与坐标系对齐
融合导航的第一步不是直接上卡尔曼滤波器，而是先把两路传感器输出的原始数据变成“在同一坐标系、同一时间基准下的位置和姿态”。很多队伍一上来就调滤波参数，结果发现融合结果比单用GPS还差，原因就是坐标系没对齐、时间戳对不上。
3.1 GPS经纬度怎么变成平面坐标
GPS输出的经纬度是球面坐标，做位置融合必须投影到平面。越野赛场范围通常在一公里以内，不需要严谨的UTM投影，用本地切平面近似即可。
核心思路是这样：在场地附近选一个基准点，记它的经纬度为(lat0, lon0)。对任意一个GPS经纬度(lat, lon)，按下式转换为以基准点为原点的东北天坐标(EAST, NORTH)：
dLat = lat - lat0 dLon = lon - lon0 earthR = 6371000.0 north = dLat * pi / 180.0 * earthR east = dLon * pi / 180.0 * earthR * cos(lat0 * pi / 180.0)其中north是朝北方向的距离，east是朝东方向的距离。这个公式在几十公里范围内精度已经足够，对于越野赛道完全不需要考虑曲率修正。转换之后，赛道上的各个路径点也会从经纬度变成(x, y)坐标，接下来所有控制、融合都用这个平面坐标完成。
这里面要特别注意的是基准点lat0的单位。很多人直接把GPS返回的度数当成弧度丢进公式里，结果算出来几十万米的坐标；还有人是先把度的分秒转成度，结果忘记乘pi/180转弧度。这类低级计算错误，在代码审查里最容易漏掉，但造成的后果极其明显——位置会飞到场外。
3.2 惯性姿态解算：从原始角速度到姿态矩阵
捷联惯导的核心是姿态解算。IMU的陀螺仪输出角速度，加速度计输出比力。越野车在平地上运动时，加速度计可以测到重力方向，静态下可以用它来初始化俯仰角和横滚角；而航向角（偏航角）不能从重力中得到，只能靠GPS航迹方向或者在赛前外设一个初始航向。这个初始航向误差在随后的组合导航里会作为误差状态逐渐被GPS修正，但如果初始误差太大，滤波器收敛过程会非常痛苦。
姿态更新的常用方法有欧拉角、方向余弦矩阵和四元数。越野组强烈建议用四元数，原因很简单：没有万向锁问题，计算量也不大。四元数更新公式可以写成：
q = [qw, qx, qy, qz] 角速度向量 w = [gx, gy, gz]（弧度每秒） 四元数微分方程： dq/dt = 0.5 * q ⊗ w其中⊗表示四元数乘法。实际代码里一般用一阶或二阶龙格库塔积分，每隔固定周期更新一次四元数，再转换为欧拉角供控制系统使用。很多现成库里也有现成的函数，但至少你要能看懂原理。
姿态更新的频率取决于IMU的输出频率，通常使用200Hz到1000Hz。每更新100到200次，还需要对四元数做归一化，否则积分误差累积会导致四元数模长漂移，姿态角输出失真。
3.3 时间同步与坐标基准对齐
数据融合时，GPS是5到10Hz，IMU是200到1000Hz，两路数据天然不在同一个时刻到达。如果你的代码只是“新GPS数据到了就把滤波器里的位置观测值替换掉”，那观测值的到达时刻和惯导推算时刻之间存在随机抖动，融合结果必然有大量毛刺。解决办法有两种：
- 软同步：在每帧IMU数据都附带主控芯片读取时刻的时间戳，收到GPS帧时记录接收时刻。融合预测按IMU时刻推进，当GPS观测的接收时刻落在两个IMU时刻之间时，把GPS观测的时间戳校准到当前滤波器状态估计时刻的下一拍。
- 硬同步：利用GPS模块的PPS脉冲触发主控的外部中断，在PPS上升沿锁存IMU最新数据，从而把GPS观测和IMU数据对齐到边沿。PPS的上升沿对应整秒时刻，误差在微秒级，效果远好于软同步，但实现复杂度高一些。
竞赛环境下PPS可能因为GPS信号短暂丢失而出现丢脉冲，所以工程上更常见的是软同步加防抖，比如给GPS数据打上主控时间戳，然后在滤波更新阶段，GPS观测值先存入一个等待队列，当IMU推算时间戳越过观测时间戳时，再执行观测更新。这样哪怕GPS帧晚到几十毫秒，也不会把错位的数据灌进滤波器。
坐标系对齐指的是GPS的东向和北向与IMU的x轴y轴方向一致。如果你的IMU是歪着装的，那一定要在初始化时测量安装角度偏差，并在姿态解算或融合中补偿。否则融合后的位置轨迹会相对于真实赛道整体旋转一个角度，控制起来怎么调都不对。
4. 融合导航算法实现：误差状态卡尔曼滤波器的工程落地
在松组合框架下，组合导航滤波器可以设计成“直接状态”或“误差状态”。竞赛工程里推荐用误差状态卡尔曼滤波器（ESKF），因为它的状态量都是误差，数值在小范围内变化，线性化误差小，而且可以把姿态误差、陀螺零偏、加速度计零偏都放进状态估计里一起估计。
4.1 状态向量怎么设计
一个常用的15维误差状态向量如下：
δX = [δp_n, δv_n, δθ, δba, δbg]其中δp_n是导航系下的位置误差（3维），δv_n是速度误差（3维），δθ是姿态误差（3维），δba是加速度计零偏误差（3维），δbg是陀螺仪零偏误差（3维）。维度听起来多，但实际滤波器在单片机上跑完全没问题，关键点在于状态转移矩阵的推导和稀疏运算的优化。
状态转移矩阵F描述了误差状态如何随IMU数据的运动更新。简单说，姿态误差会被陀螺仪运动耦合更新，速度误差会受姿态误差和加速度计零偏影响，位置误差是速度误差的积分。这里面最核心的耦合项是“姿态误差通过重力矢量注入速度误差”，也就是当陀螺仪零偏导致的姿态误差变大时，重力在水平方向上的分量会被误认为是水平加速度，从而让位置误差快速增长。这个耦合关系是组合导航误差模型的核心，理解它就能理解为什么光靠GPS位置观测就能间接估计出陀螺零偏。
4.2 滤波器预测与更新流程
预测阶段，利用IMU的角速度和加速度计比力积分得到位置速度姿态的名义值，同时把误差状态协方差矩阵P按状态转移矩阵传播。这一步相当于“让滤波器知道：我不确定系统的运动解算有多准，所以误差协方差随着时间增加而扩大”。
更新阶段，GPS输出位置（可能还有速度）作为观测值。观测方程是：
z = H * δX + v其中z是GPS观测位置与惯导推算位置之差，H矩阵把误差状态映射到位置误差，v是GPS测量噪声。卡尔曼增益K由P、H、观测噪声R计算，然后用K对误差状态进行修正：
δX = K * z P = (I - K*H) * P修正后把误差状态反馈到名义状态上，然后把误差状态清零，再重新开始下一轮预测。这个“预测-修正-重置”的循环就是ESKF的标准流程。
在实际工程中，要格外注意GPS跳变带来的异常观测污染。可以在更新前加一个卡方检测：当GPS量测残差超过预设阈值（比如15米）时，认为这次观测是野值，直接跳过不更新，而不是硬把位置拉过去。这样能防止GPS模块偶尔出现的瞬时跳变把整个轨迹带歪。
4.3 噪声矩阵调参的经验值
Q矩阵和R矩阵的取值直接决定融合结果是偏向GPS还是偏向IMU。经验上，GPS位置噪声R_pos可以取0.5到2平方米的方差，速度噪声R_vel取0.1到0.5米每秒的方差；IMU的加速度零偏随机游走和陀螺零偏随机游走则按芯片手册的Allan方差曲线来定。这些值不需要精确到小数后三位，关键是要让滤波器知道“GPS位置比IMU位置在长时间尺度上更可信，IMU姿态在短时间尺度上更平滑”。
如果发现融合轨迹跟着GPS跳，说明R太大或Q太小，滤波器过度信任GPS观测；如果发现轨迹长时间偏离真实路径，比如转弯转一半就被“拉直”了，说明Q太大或R太小，GPS观测没有被充分利用。调参过程本质是在平滑性和响应速度之间做权衡，没有一个万能值，必须在真实赛场上配合实际跑动来微调。
5. 从定位到控制：路径跟踪算法与转向伺服控制
组合导航的输出是一个平滑的高频位置和航向，但车要跑得快跑得稳，还需要把位置信息变成油门和转向量。越野组最常用的路径跟踪方法有纯追踪（pure pursuit）和Stanley方法，二者各有特点。
5.1 纯追踪算法为什么适合越野组
纯追踪的基本思路是在预瞄距离Ld处找一个期望路径点，然后根据当前位置到该点的横向偏差，按圆弧运动学关系计算出前轮转角。它实现简单，对路径的连续性要求低，很适合GPS路径点这种离散点序列。
转角计算公式为：
δ = atan2(2 * L * sin(α) / Ld, 1)其中α是当前航向与车到预瞄点方向的夹角，L是轴距，Ld是预瞄距离。预瞄距离和车速有强相关性，通常取Ld = k * v + Ld0，车速越快预瞄越远，否则转向会过于激进。
纯追踪的核心是“不要盯着脚底下，要看着前方”，所以它对GPS+惯导融合后的绝对位置误差容忍度较高。即使融合轨迹有几厘米到几十厘米的误差，只要预瞄距离足够远，控制目标点稳定在前方某处，车就不会左右疯狂扭摆。越野组速度快、场地大、边界宽，纯追踪是非常务实的选择。
5.2 速度规划与限速策略
路径跟踪只解决“往哪走”，速度规划解决“走多快”。越野场地的弯道不像室内赛道有非常明显的急弯，但不同曲率的路段仍然需要差异化限速。典型的做法是从融合轨迹中提取路径点序列的曲率，然后设置一个曲率-速度映射表：
- 曲率小于0.02时，可以放开油门冲刺；
- 曲率在0.02到0.05之间，适当收油到中速；
- 曲率大于0.05，降到低速过弯。
别小看这个映射表，实际调车过程中，限速激进一点可能只在弯道慢0.2秒，但稳定性提升非常明显。赛场上很多队伍都是在同一个弯道冲出跑道，不是因为路径跟踪失效，而是速度规划没有提前减速。
5.3 转向执行机构的伺服控制
越野车转向通常用舵机或者差速转向。舵机响应快、控制线性度好，但转向能力有限；差速转向可以实现原地旋转，但高速下容易甩尾。无论哪种，转向控制环路频率都要远高于GPS输出频率。所以转向控制必须用惯导姿态和融合位置作为反馈，闭环频率至少50Hz以上，不能直接拿5Hz的GPS点去给舵机打脉冲。
这里还要注意转向死区。舵机在接近中位的一段行程内，由于齿轮间隙和机械阻尼，转向响应存在死区。如果不做死区补偿，车会长时间处于微偏状态，表现为明明走的路径对，但总是慢慢偏向一侧。解决办法是在控制量中加入一个小的偏置补偿值，停车时标定好死区范围，然后在控制输出中对死区做反向补偿。
6. 实地调车最容易翻车的几个问题与完整排查链路
我在调越野组的时候，一半时间都在处理传感器和机械的“隐性故障”。这些问题不太会直接导致程序崩溃，但会在赛场上以“莫名跑偏”、“偶发冲出赛道”、“时好时坏”的形式暴露。下面几个坑都是实打实踩过的。
6.1 GPS丢星或锁星数骤降
现象：融合轨迹在开阔场地跑得好好的，经过场地边缘或靠近铁丝围栏、裁判帐篷时突然跳一下，或者航向角乱转几秒。
排查链路：
- 先用串口把GPS原始NMEA数据输出到电脑，观察GGA语句中的定位质量指示（Fix Quality）和卫星数量。如果卫星数低于6颗，基本可以断定是信号遮挡。
- 检查车模上方有没有新增的碳纤维板、金属天线、摄像头支架。碳纤维对GPS信号的屏蔽是灾难性的，比金属还厉害。
- 检查陶瓷天线的接地面积和走线，如果天线紧挨着电源线，高频干扰会直接被耦合到射频前端。
- 检查GPS接收板和主控之间的串口通信是否被电机噪声干扰。如果通信偶发乱码，位置自然会跳变，可以在硬件上串电阻、加磁珠，或者软件上只校验通过校验和的NMEA帧。
我在现场还见过一个奇怪现象：GPS轨迹在场地某一角每次都会往东偏两米，换了三个GPS模块都一样。最后发现是场地下方有埋地的金属管线，导致该区域电磁反射异常。这种场地环境因素没法在代码里根治，只能把那一赛段的路径点稍微内移一点，给控制留出容错空间。
6.2 惯导航向漂移导致转弯不跟手
现象：直道跑得很直，但每次进入同一弯道，车的入弯姿态都不一样，有时早转、有时晚转，看起来像“随机犯病”。
排查链路：
- 把姿态解算的航向角和GPS航迹方向做对比，录一段数据看两者的偏差是否随时间线性增大。如果是，说明陀螺零偏没有被充分估计，问题在融合滤波器的陀螺零偏可观性不足。
- 检查IMU安装固定是否松动。如果固定座有微小旷量，每次震动方向不同，引入的噪声是随机的，这种很难从算法上消除。
- 检查初始化时IMU是否处于静止状态，如果车被人推着或者电机还在抖，初始姿态基准就歪了。
- 确认陀螺仪量程。小跑和高速过弯的角速度差异巨大，如果用的小量程陀螺仪，转弯时角速度触顶削波，解算姿态就会滞后，融合结果直接失真。
这个问题的本质是“观测值进入滤波器的时序和质量”。GPS的航向变化只在车速较高时可靠，低速时GPS航向角噪声很大，所以滤波器的航向修正量要在时间上做适当门控：车低速时减少对GPS航向的信任，高速时增加信任。
6.3 滤波参数调得过于“平滑”
现象：车跑起来路径跟踪很顺，但切弯总慢半拍，摄像机回放发现车的实际轨迹比期望路径晚了一个车身位。
这个问题十有八九是滤波调太“软”导致的。位置误差协方差被压得太小，滤波器过度信任IMU的短期推算，但对GPS的修正反应太慢，相当于整条定位轨迹存在几十毫秒的滞后。这种滞后在直接看位置时不容易察觉，但路径跟踪一算，预瞄点和车实际位置错位，切弯自然晚。
解决办法是把GPS观测噪声R适当调小，或者增大预测阶段的Q中加速度随机游走分量，让滤波器更快追随GPS位置变化。注意一次只调一个参数，每次修改后跑同一段赛道对比圈速，不要两个参数一起动，否则发生问题时你根本不知道是哪个改了导致的。
6.4 赛道坐标系的校准
极速越野组路径点的提取方式不同赛道差异很大，有的是从现场GPS走一圈录的轨迹，有的用地图工具手工描点。无论哪种方式，都会遇到“地图坐标系”和“车实际坐标系”之间的偏差问题。这里推荐一个校准方法：
- 在赛道起点放一个明显标记，让车静止在标记上方，记录当前GPS坐标作为基准点。
- 沿赛道外沿缓慢走完整圈，记录GPS轨迹，作为理想路径参考。
- 路径点生成后，先用静态GPS坐标做一次刚体变换对齐，把整体偏差消掉。如果还有局部偏差，把偏差大的路径点做样条插值修正。
注意赛道坐标系的x轴朝东还是朝北，和转化公式输出的east/north对应要一致。很多队伍在仿真里路线的方向是反的，不是算法错了，就是坐标轴定义混了。
7. 一些值得长期保留的工程习惯
这套系统跑通之后，最大的体会是：融合导航项目拼的不是某个高深算法，而是把每一个环节做到稳定。GPS模块、天线、IMU、滤波、控制、供电、机械，任何一个环节出了小毛病，最终表现都在赛道上。
建议备赛时保留好每次调车的原始数据日志，包括GPS原始NMEA帧、IMU原始数据、融合输出、控制量、圈速时间。把这些数据通过无线透传模块发到电脑上回放，很多偶发性问题都能通过数据回放找到规律。哪怕比赛时用不上，这也是对自己工程能力的最好锻炼。
还有一个小经验：上电启动顺序一定要固化。先给GPS模块上电，等待锁星，再做IMU静态初始化，最后再解除电机的待机状态。这个顺序如果搞反了，轻则初始航向错误，重则起步瞬间车直接打横冲出去。把这些流程写进一个启动状态机里，而不是靠人工按键操作，能少背很多锅。
越野组的魅力在于它把卫星导航、惯性导航、控制理论这些听起来很高大上的东西，全部压缩在一辆巴掌大的小车上，逼着你去动手、去查数据、去现场调试。这套融合定位导航的框架，不仅在智能车竞赛里实用，放到未来的无人配送小车、农业机器人、巡检平台上，核心思路完全相通。把越野组吃透了，后面很多路都会好走很多。
