说实话备赛第二十一届全国大学生智能车竞赛轮腿穿越组的时候GPSIMU这套导航绕桩方案让我从“以为很简单”一路踩坑踩到了省赛前一天。场地里一共摆了十二个桩桶车要从起点出发按S形路线绕过每一个桩不能碰桩、不能出界最后停在终点圈里。听起来就是“定位跟线”的老活儿可真正把GPS融合IMU用起来之后才发现误差、漂移、时延、坐标系、轮腿打滑这些问题叠在一起能把一个看似简单的绕桩硬生生变成玄学。这篇文章我就结合自己备赛和现场调试的经验把轮腿穿越组的GPSIMU导航绕桩从方案选型、硬件安装、数据融合到路径跟踪控制完整拆一遍重点放避坑。适合正在备赛智能车竞赛的同学也适合刚接触室外导航定位的机器人爱好者参考。我尽量说人话把那些文档里不会写、问了学长才懂的细节都交代清楚。1. 轮腿穿越组到底在跑什么规则拆解与导航方案选型1.1 竞赛任务与场地特点轮腿穿越组核心载体是“轮腿式”底盘。和传统四轮小车不一样轮腿结构能适应一定的高度变化车体可以抬升、俯仰甚至能过一些小的坡坎。但代价是运动模型更复杂轮胎会打滑轮腿关节会有轻微晃动底盘重心高急转弯的时候姿态变化明显。这些特点直接决定了定位方案不能纯靠轮式里程计。比赛场地一般设在室外范围大概是半个足球场大小GPS信号可用。桩桶固定摆放在场地内提前会给一张“大致”的布置图但不会给你精确坐标。所以整个任务拆开看就是三件事第一确定自己在哪里第二知道桩点在哪里第三规划一条能绕过所有桩并且不碰桩的路径去跑。这里的核心能力就是“定位路径跟踪”。GPS负责把车拉回绝对坐标IMU负责在两帧GPS之间撑住短时姿态和位移两者融合后才能应付绕桩这种对精度有要求的工况。这一小节我想先给后来者提个醒轮腿穿越组最大的坑不是算法本身而是很多人一开始把期望放在“GPS精度足够高、直接用经纬度导航”上。实际跑下来你会发现单点GPS在开阔场地静态误差能到1到3米动态时受多路径和天线安装影响可能更大。绕桩桩间隙往往就两米上下纯GPS根本没法用必须有融合和滤波。1.2 定位方案对比为什么不是“纯编码器视觉”选方案的时候学长推荐过几条路线我们也都简单做过验证。这里直接把我踩过的结论放出来方案优点缺点我们的结论纯编码器里程计短时精度高、实现简单轮腿打滑后误差快速累积两三圈下来位置能偏出好几米只能做短时辅助视觉SLAM不依赖卫星信号光照变化、地面纹理弱、动态阴影都会影响特征提取单片机算力吃紧适合室内组室外绕桩不划算纯GPS定位绝对坐标无漂移更新率低、噪声大、多路径有跳变过桩精度不达标不能单用GPSIMU融合绝对位置短时精度互补需要做标定、时间同步、坐标系转换最终选定方案补充一点很多组会加入磁力计辅助航向但轮腿底盘电机磁场干扰很大磁力计校准起来非常麻烦而且场地如果铺设有铁质围栏或者地面带钢筋航向会跳得怀疑人生。我们测试后干脆把磁力计剔了只靠GPS位置/速度约束IMU的航向漂移。2. 硬件选型与安装GPS陶瓷天线和IMU布置的细节2.1 GPS模块选型与陶瓷天线安装要求先说明我们用过几款模块包括u-blox的NEO-M8N、NEO-M9N也试过国产的AT6558方案。从比赛性价角度选一款支持GPS北斗双模、能输出到10Hz、最好带UBX协议的模块就够了。NMEA协议虽然通用但解析麻烦、数据量大UBX协议可以直接在模块内部配置好输出频率和内容后面处理起来舒服很多。这里最容易被低估的是天线。陶瓷天线不是随便贴在板上就行它有几个硬性要求天线必须要有一个足够大的“地平面”。陶瓷天线底部需要完整的覆铜地通常是天线厂家推荐的尺寸比如18mm×18mm对应约30mm×30mm地平面直接悬空或者贴在一小块板上等效辐射效率会大幅降低。馈电点要远离高频噪声源。电机、舵机、降压模块都是干扰源尤其无刷电机和PWM驱动EMI会沿电源线和地线传导到天线馈点导致卫星信号载噪比下降。天线尽量放在车体最高点正上方不要有碳纤维板、金属车壳、大块电池遮挡。碳纤维本身就导电靠近天线会改变天线谐振特性我们实测把天线从主控板上方移到车顶支架后搜星数从6颗涨到12颗HDOP从3.0降到1.2效果立竿见影。天线到模块之间的同轴馈线要短、屏蔽层要好长导线会引入损耗和噪声。还有一个小细节模块供电要干净。GPS模块对电源纹波敏感如果直接用电池电压经过AMS1117之类的LDO给模块供电电机启动瞬间电压跌落会造成GPS数据偶尔丢帧。我们后面在模块电源入口加了LC滤波问题基本消失。2.2 IMU安装位置与静动态标定流程IMU我们用过MPU6050、ICM20602和ICM42688。对轮腿底盘来说量程选择比精度更重要陀螺仪量程建议选±500°/s甚至±1000°/s因为轮腿急转时瞬时角速度可能很高量程不够会削顶导致积分出来的角度明显偏小。安装位置最重要的一条尽量靠近整车质心而且要固定死。IMU离质心越远车体倾斜和振动时引入的线加速度误差越大固定不牢则会产生额外的高频抖动滤波后依然能看到毛刺。我们最初用双面胶粘在主控板上跑起来后数据曲线像心电图后来改成用尼龙柱加螺丝锁紧数据立刻干净了一大截。标定方面即使不用转台也必须做三件事静止零偏采集。上电后让车水平静止30到120秒取陀螺三个轴的均值作为零偏加速度计同理。注意要等系统跑稳定再采集刚上电前几秒数据往往有扰动。加速度计六面法标定。把IMU分别朝上、朝下、左右、前后静止放置拟合出三轴比例因子和零偏虽然精度不如转台但能把加速度误差从5%压到1%以内。Allan方差分析。长停车时记录陀螺数据画出Allan方差曲线可以估计出角度随机游走和零偏不稳定性。这个参数对后面卡尔曼滤波的Q矩阵设置很有用不要求精确量级对就行。另外强烈建议跑车前先“预热”几分钟。ICM系列传感器温度漂移不小冷启动时零偏和热稳定后能差好几个°/s如果不预热就直接导航等车热起来航向已经悄悄偏了。3. 数据融合与位姿解算让GPS和IMU取长补短3.1 从经纬度到平面坐标场地坐标系建立GPS输出的是WGS84经纬度属于球面坐标不能直接拿来做平面导航。好在比赛场地只有几十米范围可以用“局部切平面”近似也就是在场地附近选一个基准点把经纬度差换算成东北向的平面米数。公式很简单北向位移y R * (lat - lat0)东向位移x R * cos(lat0) * (lon - lon0)其中R取地球平均半径6371000米lat0和lon0是基准点经纬度。在几百米范围内这个近似误差很小足够绕桩使用。如果场地大或者你想更正规一点可以调用pyproj或utm库把经纬度转成UTM坐标然后用UTM下的东、北值减去原点坐标得到平面坐标。这里有个很多人忽略的问题GPS天线和IMU的物理位置不重合。GPS天线在车顶IMU在底盘质心附近两者之间有一个固定杆臂。车辆转向时GPS天线的位置变化和底盘质心变化不完全一致不做杆臂补偿就会出现“定位点绕质心画圈”的假象。融合前用几何关系把GPS位置折算到IMU位置或者反过来统一到车体参考点不然卡尔曼滤波的状态更新会有几厘米到十几厘米的周期性偏差。3.2 卡尔曼滤波融合实战状态量与代码骨架轮腿穿越组的融合我推荐用松耦合EKF把IMU当预测输入把GPS位置和速度当观测更新。状态量取二维位置x、y二维速度vx、vy航向角yaw陀螺零偏bg一共6维就够。预测部分使用IMU的角速度和加速度或者用“航向速度”的运动模型。加速度计在轮腿上噪声很大直接用加速度积分位置不现实所以比较稳的做法是用GPS速度或码盘速度作为前向速度估计用IMU的yaw作为航向做“速度航向模型”预测位置import numpy as np # 状态: [x, y, vx, vy, yaw, bg] # 控制输入: gyro_z(去零偏), speed(前向速度) def predict(x, P, gyro_z, speed, dt, Q): yaw x[4] x_new x.copy() x_new[0] speed * np.cos(yaw) * dt x_new[1] speed * np.sin(yaw) * dt x_new[4] (gyro_z - x[5]) * dt # 线性化雅可比矩阵 F简化版 F np.eye(6) F[0, 4] -speed * np.sin(yaw) * dt F[1, 4] speed * np.cos(yaw) * dt F[4, 5] -dt P_new F P F.T Q return x_new, P_new def update_gps(x, P, z, R): # z [gps_x, gps_y] 已经在平面坐标系下 H np.zeros((2, 6)) H[0, 0] 1.0 H[1, 1] 1.0 y z - H x S H P H.T R K P H.T np.linalg.inv(S) x_new x K y P_new (np.eye(6) - K H) P return x_new, P_new注意几点工程经验GPS数据有延迟和IMU不是同一时刻。简单做法是记录GPS的接收时刻在滤波器里把GPS观测“滞后”到对应的历史状态上再用当前状态外推回来更粗暴但好用的做法是固定补偿一个延迟量比如100ms把GPS观测对齐后再进滤波器。我们实测不处理时延过桩切弯会有明显“外摆”处理之后路径贴合度好很多。GPS速度也是个好观测。很多模块输出的地速和航向比位置噪声小可以把速度分解成vx、vy观测进滤波器进一步约束IMU的航向漂移。异常值剔除不能少。GPS偶尔会跳出去三五米直接进滤波器会拉歪整个轨迹。用新息检验也就是“预测位置和GPS观测位置之间的差值”超过3倍标准差时直接丢弃这一帧不更新。4. 绕桩路径生成与跟踪控制实战4.1 桩点坐标采集与参考线平滑桩点坐标不能靠比赛主办方给的手画地图估算必须到现场实地采。最笨但最有效的方法是把车推到每个桩点位置静止停放GPS模块连续记录一两分钟取经纬度中值并转换到平面坐标。注意不是说推过去就完事车头方向不同、GPS多路径不同同一位置静态数据能分散出半米。所以每个点至少要采三次不同时段、不同车头朝向最后取聚类中心。采完桩点之后还要规划绕桩顺序。比赛一般是S形或8字形绕桩你需要根据桩点坐标确定一条不碰桩的参考线。参考线不是直接把这些点连起来就完事因为折线在转向点处不连续控制会有冲击。先加密中间点再用三次样条或贝塞尔曲线平滑最后得到一串间距5到10厘米的离散参考点每个点可以额外存一个建议速度。路径平滑的原则很简单让曲率连续。过桩时曲率最大直道时曲率趋近于零。如果不做平滑纯跟踪控制器会在每个桩点来回震荡车看起来像在“画龙”。4.2 纯跟踪控制与前馈转向结合路径跟踪我用的是经典的纯跟踪Pure Pursuit加PID横向修正的混合方案。纯跟踪的思想就是在参考线上找一个前方预瞄距离Ld处的点根据当前位置到预瞄点的夹角计算期望转向角车会像追一个不断前移的目标点一样沿着路径走。预瞄距离很关键速度高时Ld要加大速度低时Ld要减小否则要么切弯太晚要么切弯太急。我的经验是 Ld 0.3 0.5 * v(m/s) 左右起步再根据实际弯道响应微调。单纯纯跟踪在直道上有时候会有小幅振荡所以我们叠加了一个PID对横向偏差做修正。横向偏差就是当前位置到参考线最近点的垂距前面纯跟踪给出前馈转向角PID根据横向偏差做补偿。组合起来直道不抖弯道不冲。速度规划也不能忽略。轮腿底盘重心高、轮胎抓地力有限绕桩时速度过高很容易侧滑。我的做法是在每个桩点前后设置一个“减速区”进入桩前1.5米开始从高速降到低速比如从2.2m/s降到1.0m/s过桩点时保持低速出桩后再加速。加减速用梯形曲线平滑避免阶跃跳变。这一块看似简单但直接影响车绕桩的最小稳定转弯半径。5. 现场调试与避坑指南这些坑我替你踩过了5.1 GPS相关高频坑先说搜不到星。GNSS模块都需要冷启动搜星时间如果你在场地上电立刻跑前几十秒可能根本没有定位数据。所以到场地后第一件事就是把车开机放在固定位置等各指示灯显示“定位成功”再开始调车。再说定位跳动。比赛中我们发现最典型的干扰源是场地周围的金属围栏、旗杆、LED大屏和电子计分设备。金属物体反射GPS信号会造成多路径位置会在真实点位附近来回弹。跑直线时还不明显绕桩转弯时一跳就是一道弯。对付这种跳变除了在算法里做新息检验还可以看模块输出的定位质量指标比如HDOPHDOP大于一定阈值比如2.5时直接把GPS观测权重调小让滤波器更多相信自己。另外GPS模块在车辆运动时位置更新有“拖尾”感因为模块内部对位置做了平滑滤波。你急转弯时GPS输出点位会明显滞后于真实位置。这个没法完全消除只能靠IMU在短时间内顶着所以融合质量很大程度取决于IMU的短时精度。5.2 IMU相关高频坑yaw慢漂是常态IMU最头疼的问题就是“基于IMU的位姿解算yaw仍会慢漂”。陀螺仪即使标定完零偏也会随温度、时间缓慢变化纯积分下来也许一分钟就漂好几度。轮腿车本身又没法用里程计航向修正因为打滑太严重。所以我们在EKF里把陀螺零偏作为状态量估计让GPS每秒钟来修正一次实测能把航向漂移控制在可接受范围。有同学问要不要加磁力计。我的建议是除非时间充裕否则慎加。轮腿底盘的电机、电源线、金属车架都会产生磁场畸变磁力计校准没有专业工具很难校好一旦残留误差航向反而比纯GPSIMU融合更差。我们用过一周磁力计最终选择放弃把这个时间留给路径跟踪调参更值。还有一个容易忽略的坑IMU启动阶段数据不稳定。上电瞬间寄存器从默认值切换、内部自检、振动都会让前几秒数据离谱。所以代码里要设计一个“初始化阶段”上电后至少静止3秒钟期间不发布位姿等零偏稳定后再开始导航。5.3 轮腿底盘机械与控制相关坑轮腿底盘和普通四轮还有一个大区别转弯时内外侧轮载荷变化大轮胎打滑明显而且急转时整车容易侧倾。一开始我们按四轮车调纯跟踪参数预瞄距离设得比较小结果车过桩时后腿直接甩尾桩没碰到轨迹先歪了半米。后来解决办法是降低过桩速度同时把预瞄距离加大让转向更缓和。还有一点左右轮的轮胎磨损、胎压不一致会导致直行偏航。这个不靠GPS看不出来因为GPS在室外会把“直行偏”和“航向偏”混在一起。解决办法是每次调车之前在平地上做一次“直线校准”记录方向盘偏置量再在控制里补掉。机械公差也会坑人左右轮腿关节安装高度不一致或者悬挂刚度不同会导致转弯时重心偏移方向固定。这类问题反映在导航数据上就是“同一个弯左右转的误差不一样大”如果你发现左转成功率高、右转总偏外大概率不是算法问题而是机械左侧和右侧不对称。5.4 现场调试流程与数据复盘方法最后分享一下我们的现场调试流程照着做能少走很多弯路。首先是模块拆分。在跑闭环之前先做两件事一是遥控车在场地上跑一圈同时用SD卡或者无线串口记录GPS原始坐标、IMU原始角速度和桩点坐标二是用离线Python脚本打点把采集的数据画出来看看轨迹是否平滑、桩点是否贴合实际位置。这里强烈建议记录原始数据和融合结果不要只记最终位姿否则出了问题根本没法回头查。然后是参数分段验证。先用很保守的速度比如0.8m/s闭环跑圈只看位置误差确认路径跟踪收敛后再逐步提速。比赛的坑在于你不知道当前问题出在定位、路径还是控制上如果一上来就全速跑车一偏三个环节都可能是嫌疑犯。离线数据复盘能帮你快速定位是哪一环歪了。现象可能原因检查方法解决方向过桩时轨迹外摆GPS延迟未补偿打点看急弯处偏差方向增加GPS延迟补偿直道左右摆动预瞄距离太小调大Ld增大预瞄距离绕桩后越来越偏IMU航向漂移看长跑时航向误差曲线增大GPS航向/速度观测权重同一弯左右不对称机械不对称左右各跑三次看误差统计检查轮腿、胎压、悬挂GPS偶尔飞点多路径或卫星几何差看HDOP和载噪比新息检验、降低观测权重过桩时侧倾速度过快看侧向加速度估计降低过桩速度梯形加减速6. 一些实在的备赛建议如果你正准备下一届智能车竞赛轮腿穿越组我说三条掏心窝的建议。第一别急着写工程代码先把场地坐标系和日志系统做好。坐标系方向、单位、杆臂补偿这些如果混乱后面所有算法都是垃圾进垃圾出日志如果能回放现场出问题一小时能排查完否则一个下午都可能耗在猜哪一环错了。第二GPS天线位置和IMU固定方式值得花两周机械时间。这不是“差不多能用”的事天线低一点、地平面小一点、线材长一点性能都会有肉眼可见的差别。赛场上很多组算法水平差不多最后拼的就是这些细节的稳定性。第三比赛前一天不要改任何核心参数。到现场把环境因素适应好后准备好三套备份参数保守、均衡、激进按场地情况现场微调一小部分但不要动融合框架。我们吃过这个亏——决赛前一天觉得“把Q矩阵整体调小一点会不会更稳”结果第二天整个位置估计都迟钝了状态全部乱掉最后靠备份参数才救回来。轮腿穿越组的绕桩本质上不是某一个传感器的独秀而是GPS、IMU、底盘、路径规划、控制器整个链条协同工作的结果。希望这篇踩坑总结能帮你把那些隐形的坑提前填平把精力留给真正值得研究的控制问题。