简介十八届智能车单车越野组沁恒方案分享聚焦CH32V307主控下的GPS基础循迹适合蓝桥杯智能车竞赛单车越野组参赛者和嵌入式开发学习者。文档详细列出K车模、LQ-CH32V307母板、WCH-LINK-RV下载器、无刷电机及驱动、光电传感器、双频GNSS定位模块等硬件选型并给出编码器、电机驱动、陀螺仪等模块的连接方式与注意事项。控制部分重点讲解动量轮平衡调试包括并级PID与串级PID的实现与参数换算以及基于GPS与陀螺仪的循迹路径跟踪思路附关键代码示例。资源为1个PDF文件共837KB篇幅紧凑、可直接对照实践。已有915人学习下载对于准备单车越野组比赛或研究直立平衡算法的读者是一份高性价比的参考方案。1. 方案背景与整体设计思路第十八届全国大学生智能车竞赛的单车越野组算是这几年规则变化里比较有意思的一个方向。单车本身是两轮自平衡结构天然就带着不稳定性再加上越野赛道里有坡道、碎石、草地这类非平路元素控制难度比传统四轮组高了不少。而这套沁恒方案最核心的思路是把GPS作为基础定位手段参与循迹而不是像室内组那样完全依赖电磁或摄像头。注意这里的用词是“基础循迹”不是“纯GPS导航”。GPS在室外开阔环境下的精度确实够用但智能车竞赛跑的是固定赛道赛车需要的是“知道自己大概在哪条路上”而不是“从A点导航到B点”。所以这套方案的实际做法是用GPS拿到全局坐标再配合车身上的陀螺仪、编码器做局部姿态修正最后通过预先采集的赛道点序列计算出横向偏差送给转向和速度环去执行。选择沁恒方案的一个重要原因是芯片本身的定位。CH32V307VCT6是沁恒基于RISC-V内核设计的MCU主频能跑到144MHz带FPU内部集成2路CAN、多路USART还有丰富的高级定时器。放在单车越野这个场景里它的算力足够跑通GPS解析、坐标转换、PID控制和自平衡姿态解算而且外围接口齐全——GPS模块走串口编码器走定时器正交解码陀螺仪走I2C或SPI全部都能直接接不需要额外加转接芯片这在国内竞赛方案里算是非常省心的。如果说这套方案的选型逻辑我觉得可以用一句话概括用最容易买到的通用模块做一套能稳定完赛的最小系统。CH32V307官方有完整的库函数和例程GPS模块用ublox或中科微的方案都很常见编码器和陀螺仪也是智能车玩家手里最不缺的传感器。相比某些方案用树莓派或者高性能DSP这套体系的学习门槛低出问题也好排查特别适合第一次参加单车组、或者从传统四轮组转过来的队伍。适合参考这份方案的读者大概是两类一类是准备参加下一届智能车竞赛、尤其是单车越野或室外循迹方向的在校学生另一类是想把手头CH32V307开发板利用起来、做一套室外GPS定位小车的爱好者。不管哪类人只要把基础循迹这套框架跑通后面加摄像头、加激光雷达甚至加RTK都只是挂在同一个框架上的扩展模块而已。2. GPS基础循迹的核心实现细节2.1 经纬度坐标与平面坐标的转换处理GPS模块输出的原始数据是WGS84坐标系下的经纬度比如北纬34度、东经108度这样。但循迹算法没法直接在经纬度上算横向偏差因为纬度方向和经度方向每度对应的物理距离不一样直接拿经纬度差值算PID会得到完全错误的结果。所以第一步必须做坐标投影把经纬度转到平面直角坐标系。推荐的做法是高斯-克吕格投影或者更简单的UTM投影。UTM把地球表面分成60个投影带每带6度经度在带内用米为单位的平面坐标。具体到竞赛场景一个赛道的范围通常只有几百米完全落在同一个投影带内所以误差完全可以接受。若嫌UTM在边缘区域略有畸变也可以用本地切平面近似——找一个赛道中心点作为原点用等距圆柱投影公式把经纬度差换算成米。// 以赛道中心点为原点将WGS84经纬度转换为本地平面坐标单位米 #define LAT_REF 34.1234567 #define LON_REF 108.9876543 void gps_to_local(double lat, double lon, float *x, float *y) { double dlat lat - LAT_REF; double dlon lon - LON_REF; *y (float)(dlat * 111320.0); // 纬度方向每度约111.32km *x (float)(dlon * 111320.0 * cos(LAT_REF * M_PI / 180.0)); // 经度方向需乘以cos(纬度) }这是最简化的本地切平面换算实测在数百米范围内精度能达到厘米到分米级别对智能车循迹完全够用。这里有个细节经度换算时必须乘以cos(纬度)很多新手会漏掉结果在赛道纵向上的比例是对的横向全错车跑起来就像喝醉了一样。如果你用的GPS模块能直接输出UTM坐标那就省事很多。但要注意UTM坐标是分带的不同带的坐标值会有跳变建议固定使用同一个投影带参数不要自动切换。2.2 赛道路径的采集与匹配算法基础循迹的前提是手里有一条“参考路径”。这条路径怎么来一般有两种方式第一种是拿着GPS模块沿赛道走一圈每隔0.5米或1米记录一个坐标点生成一个点序列文件第二种是赛事组委会直接公布赛道GPS坐标文件选手下载后解析使用。两种方式到手后都可以转成本地平面前提下的(x, y)点集。得到点集之后算法要做的是“找最近点”。每一帧拿到当前GPS坐标遍历参考路径上的所有点找到距离最近的那个这个点对应的索引就是车在路径上的“投影位置”。有了投影位置就可以计算横向偏差——当前坐标到最近路径点的方向向量结合车的航向角算出车偏移了中心线多少。实际工程里不会真的每帧全量遍历几百上千个点那样浪费算力。常见做法是先用一个粗略的“当前索引窗口”缩小范围比如设定上下各50个点的搜索区间只有当车跑出这个区间时才重新全局搜索。因为赛车的运动是连续的上一帧在哪个位置下一帧大概率还在附近用滑动窗口可以把最近点搜索优化到微秒级。int find_nearest_point(float cur_x, float cur_y, int last_idx, int search_range) { int start last_idx - search_range; int end last_idx search_range; float min_dist 1e9f; int min_idx last_idx; for (int i start; i end; i) { int idx (i path_len) % path_len; // 环形路径取模 float dx cur_x - path_x[idx]; float dy cur_y - path_y[idx]; float d dx * dx dy * dy; if (d min_dist) { min_dist d; min_idx idx; } } return min_idx; }参考路径本身也要做预处理。一是平滑处理因为手工采集的点会有抖动可以用滑动平均或三次样条拟合避免路径上出现尖锐的拐角二是等间距重采样保证每个相邻点之间的距离大致相等这样在计算航向角、曲率的时候不会因为点距不均而产生跳变。2.3 GPS定位三边测量算法的补充解析很多做GPS定位的同学会问GPS模块自己就输出坐标了为什么还要学习三边测量算法这里有一个概念需要澄清GPS接收机内部确实是靠“三边测量”或“伪距定位”来解算自身位置的但模块输出的位置是已经解算好的结果。我们学习三边测量更多是为了理解定位误差是怎么来的以及在某些基站定位比如伪GPS、室内定位模拟器场景下如何自己写代码解算位置。三边测量的核心是已知三个以上基站的坐标和到目标的距离联立圆方程组解出目标位置。GPS里的“基站”就是卫星“距离”就是伪距。对于智能车竞赛来说如果你只是用现成GPS模块不需要自己写三边测量但如果你在做的是“基站定位循迹”比如在场地四周布置几个UWB基站那就需要用到这个算法了。// 经典的三边测量最小二乘解法示意 // 已知(mx[i], my[i])为基站坐标d[i]为测量距离 // 构造 Ax b用最小二乘求解目标位置 (x, y) void trilateration(float mx[], float my[], float d[], int n, float *out_x, float *out_y) { // 取第一个基站作为参考构造n-1个方程 // A [2*(mx[i]-mx[0]), 2*(my[i]-my[0])] // b d[0]^2 - d[i]^2 mx[i]^2 - mx[0]^2 my[i]^2 - my[0]^2 // 解线性方程组即可 }理解了原理之后你会发现GPS模块输出的坐标其实已经是对多颗卫星信号做了融合解算的结果模块内部做了大量滤波和校正。所以在做循迹的时候不需要重复造轮子只需关注模块输出的坐标质量即可。但如果你的应用场景是“模拟GPS信号”之类的测试环境三边测量算法就是必需品了。3. 实操过程与核心环节实现3.1 沁恒CH32V307环境搭建与GPS模块连接先说一下硬件连接。CH32V307VCT6开发板上串口资源丰富我们选用USART1接GPS模块波特率9600或115200具体看模块默认配置。GPS模块的TXD接MCU的RXDRXD接TXD共地用3.3V或5V供电取决于模块手册。CH32V307的串口接收用中断或DMA都行推荐DMA加空闲中断这样在处理长串NMEA语句的时候不容易丢字节。软件环境方面沁恒提供了MounRiver Studio开箱即用不需要额外配置工程模板。新建工程后在main函数里初始化串口和GPIO然后进入主循环不断解析串口接收到的数据。// NMEA数据解析提取GNGGA语句中的经纬度 void parse_nmea_line(char *line) { if (strncmp(line, $GNGGA, 6) 0) { // 按逗号分割 char *p line; int field 0; char *lat_str NULL, *lon_str NULL; while ((p strchr(p, ,)) ! NULL) { p; field; if (field 2) lat_str p; // 纬度 if (field 4) lon_str p; // 经度 } // 转换为十进制经纬度并调用 gps_to_local 转平面坐标 } }注意GNGGA语句中纬度格式是“ddmm.mmmm”要转成十进制度数需要把“度分”格式拆开前两位是度后面是分分除以60加到度上。经度同理。另外推荐使用GNGGA或GNRMC语句这两种包含定位质量指示可以根据定位状态决定是否更新坐标避免在未定位时用无效数据污染路径匹配。3.2 五路循迹传感器的引入与GPS互补纯GPS循迹在开阔场地确实能跑但一遇到桥洞、树荫、高架旁边这类遮挡严重的区域GPS信号会明显变差坐标漂移达到几米甚至几十米。这时候如果只靠GPS车的横向偏差会瞬间跳变随之而来的就是转向剧烈抖动。为了避免这种问题沁恒方案里通常会加一组五路灰度循迹传感器作为近场修正。所谓五路循迹就是一字排开五个红外灰度传感器分别检测赛道黑线或白线的位置。正常行驶时车在赛道中间中间三个传感器里靠近中心的有信号车偏左时左侧传感器先触线。通过五个传感器的ON/OFF组合可以粗略判断车相对赛道的横向偏移量。这个偏移量不需要很精确只要能区分“偏左、居中、偏右、大偏左、大偏右”这几种状态即可。GPS负责全局粗定位五路灰度负责局部细纠偏两者融合策略简单有效当GPS的横向偏差绝对值小于阈值比如0.5米时相信灰度传感器用它给出的偏移量参与转向当灰度传感器全部离地或找不到线时切回GPS数据。这样一来GPS的长时间稳定性和灰度传感器的短时间高精度就结合起来了。我个人在调试时发现这种融合策略的关键不是算法多复杂而是阈值切换要平滑。直接硬切换会让转向输出产生跳变车会抖一下。建议在切换点附近做一个线性过渡区比如GPS偏差从0.4米到0.6米之间灰度权重从1线性降到0GPS权重从0升到1这样过渡就柔顺很多。3.3 单车自平衡与转向控制的联动单车越野组和四轮组的本质区别在于单车必须先解决“站得住”的问题才有资格谈“跑得快”。沁恒方案里通常用MPU6050或ICM20602陀螺仪做姿态解算加上编码器反馈车轮转速构成一个典型的自平衡闭环。自平衡环的运行频率要比循迹环高得多。一般建议姿态环跑200Hz到500Hz而GPS循迹环只有10Hz到20HzGPS模块输出频率也不支持太高。这两个环之间怎么协同答案是级联外环是GPS循迹输出期望转向角和期望速度内环是自平衡和转向控制跟踪外环指令。外环频率低没关系内环在两次外环更新之间保持上一次的指令即可。实际工程中要注意的是转向控制不能直接叠加到直立平衡环的输出上。单车的转向靠的是车身倾斜和车轮转速差如果直接把转向PID输出加到电机PWM上会破坏自平衡的稳定性。正确做法是转向环输出的是“目标倾斜角”把目标倾斜角作为姿态环的输入让车身通过倾斜来实现转向。这个逻辑类似于骑自行车你拐弯时是先倾斜车身而不是直接拧车把。// 外环GPS循迹计算横向偏差 - 目标倾斜角 float lateral_error calc_lateral_error(cur_pos, nearest_path_point); float target_lean_angle lean_pid.compute(lateral_error); // 内环姿态环跟踪目标倾斜角 float current_lean_angle imu.get_angle_x(); float balance_output balance_pid.compute(target_lean_angle - current_lean_angle);4. 常见问题与排查技巧实录4.1 GPS数据跳变与静态漂移GPS模块在静止状态下坐标也会在小范围内随机漂移通常半径为1到3米。这个漂移在循迹时会造成横向偏差的抖动严重时车会像“画龙”一样左右摆。解决办法首先是硬件层面——把GPS天线放在车的最高点远离电机、电池等金属和电磁干扰源其次是软件层面——对GPS坐标做低通滤波或卡尔曼滤波。我实测下来最简单的滑动平均滤波取最近5到10个点的平均值就能明显改善漂移。但注意滑动窗口不宜过大否则车真正拐弯时坐标响应会变慢形成转弯迟滞。更进阶的做法是用卡尔曼滤波融合GPS和陀螺仪积分GPS负责修正长期漂移陀螺仪积分负责短期平滑两者互相修正效果非常好。如果你的场地周围有高楼或金属围栏GPS反射会造成多径效应坐标会突然跳出几十米。遇到这种情况一个非常实用的防护手段是设定单帧位移最大阈值。如果当前坐标与上一帧坐标的距离超过合理范围比如5米就判定这次GPS数据无效暂时用上一帧数据等待下一帧更新。这种简单的野值剔除策略能挡住绝大多数多径跳变。4.2 启动阶段无法定位或定位缓慢GPS冷启动时搜星可能需要几十秒甚至几分钟这对比赛来说是不可接受的。建议的解决办法是在赛道调试阶段就把GPS模块放在比赛场地持续上电至少10分钟让模块下载星历数据比赛前不要完全断电如果必须断电尽量缩短断电时间这样模块进入热启动状态重新定位时间能压缩到几秒。另外GPS模块的NMEA语句里有定位质量指示字段GGA语句中的第6个字段为0表示未定位为1表示单点定位为2表示差分定位。循迹代码里一定要判断这个字段未定位时不要使用坐标。否则在启动阶段车还没定位成功就已经开始跑了第一次算出的横向偏差可能指向几十米外车会直接冲出去。4.3 经纬度坐标在本地地图绘制时的大幅偏移问题热词里提到了一个很常见的现象原生GPS坐标在天地图这类国内地图上绘制时会偏移几百米。这个偏移是因为国内地图使用的坐标加偏了不是GPS本身错了。智能车比赛中我们用的是原始GPS坐标不需要做加偏纠偏所以这个问题在竞赛里不存在。但如果你把采集到的赛道坐标导入地图软件查看就会看到明显的偏移这是正常的。只需要在代码里统一使用WGS84坐标处理即可不要在中间混入火星坐标。4.4 常见问题速查表现象可能原因排查与解决办法坐标跳变几十米多径效应或野值增加单帧位移阈值过滤天线远离金属物车跑起来左右画龙GPS静态漂移未滤波加滑动平均或卡尔曼滤波减小转向P转弯明显滞后滑动窗口过大或外环频率低缩小滤波窗口提高GPS更新率或做预测补偿启动即冲出去未判断定位质量增加定位质量字段检查未定位时禁止循迹树荫/桥洞下失控GPS信号遮挡引入五路灰度传感器做近场修正自平衡和转向相互影响操控叠加方式错误转向应输出目标倾斜角而非直接加PWM5. 个人调试心得与扩展建议调试这套方案的过程中有一个体会特别深GPS循迹和室内循迹最大的区别在于“信任度”。室内电磁或摄像头数据每秒更新几百次你几乎可以把传感器数据当作真值来用。但GPS的数据是慢的、有噪声的、偶尔还撒谎的你必须建立一套数据可信度评估机制——什么时候信它什么时候不信什么时候降权使用。这个思维转变比任何算法本身都重要。另外一个小技巧分享给大家调试时不要一上来就跑全赛道先在赛道上选一段100米左右的直线段把车放在中心线上观察输出的横向偏差是否在零附近。再用遥控器或手动推车把车故意偏移0.5米观察横向偏差是否稳定输出0.5米左右的数值。这个“静态标定”能帮你快速验证坐标转换和路径匹配是否正确能省下大量的赛道试跑时间。至于后续扩展这套框架的可玩性其实很高。比如把GPS模块换成RTK高精度定位定位精度能从米级提升到厘米级很多控制策略都能简化再比如用ESP32或树莓派做视觉识别辅助识别路肩、障碍物甚至交叉路口就能从“基础循迹”升级到“高级感知”。甚至你可以在参考路径上标注不同的速度区间下坡前提前减速出弯后提前加速这些都是基于现有GPS框架的小改动。本文还有配套的精品资源点击获取 基于CH32V307的智能车GPS循迹方案解析
简介十八届智能车单车越野组沁恒方案分享聚焦CH32V307主控下的GPS基础循迹适合蓝桥杯智能车竞赛单车越野组参赛者和嵌入式开发学习者。文档详细列出K车模、LQ-CH32V307母板、WCH-LINK-RV下载器、无刷电机及驱动、光电传感器、双频GNSS定位模块等硬件选型并给出编码器、电机驱动、陀螺仪等模块的连接方式与注意事项。控制部分重点讲解动量轮平衡调试包括并级PID与串级PID的实现与参数换算以及基于GPS与陀螺仪的循迹路径跟踪思路附关键代码示例。资源为1个PDF文件共837KB篇幅紧凑、可直接对照实践。已有915人学习下载对于准备单车越野组比赛或研究直立平衡算法的读者是一份高性价比的参考方案。1. 方案背景与整体设计思路第十八届全国大学生智能车竞赛的单车越野组算是这几年规则变化里比较有意思的一个方向。单车本身是两轮自平衡结构天然就带着不稳定性再加上越野赛道里有坡道、碎石、草地这类非平路元素控制难度比传统四轮组高了不少。而这套沁恒方案最核心的思路是把GPS作为基础定位手段参与循迹而不是像室内组那样完全依赖电磁或摄像头。注意这里的用词是“基础循迹”不是“纯GPS导航”。GPS在室外开阔环境下的精度确实够用但智能车竞赛跑的是固定赛道赛车需要的是“知道自己大概在哪条路上”而不是“从A点导航到B点”。所以这套方案的实际做法是用GPS拿到全局坐标再配合车身上的陀螺仪、编码器做局部姿态修正最后通过预先采集的赛道点序列计算出横向偏差送给转向和速度环去执行。选择沁恒方案的一个重要原因是芯片本身的定位。CH32V307VCT6是沁恒基于RISC-V内核设计的MCU主频能跑到144MHz带FPU内部集成2路CAN、多路USART还有丰富的高级定时器。放在单车越野这个场景里它的算力足够跑通GPS解析、坐标转换、PID控制和自平衡姿态解算而且外围接口齐全——GPS模块走串口编码器走定时器正交解码陀螺仪走I2C或SPI全部都能直接接不需要额外加转接芯片这在国内竞赛方案里算是非常省心的。如果说这套方案的选型逻辑我觉得可以用一句话概括用最容易买到的通用模块做一套能稳定完赛的最小系统。CH32V307官方有完整的库函数和例程GPS模块用ublox或中科微的方案都很常见编码器和陀螺仪也是智能车玩家手里最不缺的传感器。相比某些方案用树莓派或者高性能DSP这套体系的学习门槛低出问题也好排查特别适合第一次参加单车组、或者从传统四轮组转过来的队伍。适合参考这份方案的读者大概是两类一类是准备参加下一届智能车竞赛、尤其是单车越野或室外循迹方向的在校学生另一类是想把手头CH32V307开发板利用起来、做一套室外GPS定位小车的爱好者。不管哪类人只要把基础循迹这套框架跑通后面加摄像头、加激光雷达甚至加RTK都只是挂在同一个框架上的扩展模块而已。2. GPS基础循迹的核心实现细节2.1 经纬度坐标与平面坐标的转换处理GPS模块输出的原始数据是WGS84坐标系下的经纬度比如北纬34度、东经108度这样。但循迹算法没法直接在经纬度上算横向偏差因为纬度方向和经度方向每度对应的物理距离不一样直接拿经纬度差值算PID会得到完全错误的结果。所以第一步必须做坐标投影把经纬度转到平面直角坐标系。推荐的做法是高斯-克吕格投影或者更简单的UTM投影。UTM把地球表面分成60个投影带每带6度经度在带内用米为单位的平面坐标。具体到竞赛场景一个赛道的范围通常只有几百米完全落在同一个投影带内所以误差完全可以接受。若嫌UTM在边缘区域略有畸变也可以用本地切平面近似——找一个赛道中心点作为原点用等距圆柱投影公式把经纬度差换算成米。// 以赛道中心点为原点将WGS84经纬度转换为本地平面坐标单位米 #define LAT_REF 34.1234567 #define LON_REF 108.9876543 void gps_to_local(double lat, double lon, float *x, float *y) { double dlat lat - LAT_REF; double dlon lon - LON_REF; *y (float)(dlat * 111320.0); // 纬度方向每度约111.32km *x (float)(dlon * 111320.0 * cos(LAT_REF * M_PI / 180.0)); // 经度方向需乘以cos(纬度) }这是最简化的本地切平面换算实测在数百米范围内精度能达到厘米到分米级别对智能车循迹完全够用。这里有个细节经度换算时必须乘以cos(纬度)很多新手会漏掉结果在赛道纵向上的比例是对的横向全错车跑起来就像喝醉了一样。如果你用的GPS模块能直接输出UTM坐标那就省事很多。但要注意UTM坐标是分带的不同带的坐标值会有跳变建议固定使用同一个投影带参数不要自动切换。2.2 赛道路径的采集与匹配算法基础循迹的前提是手里有一条“参考路径”。这条路径怎么来一般有两种方式第一种是拿着GPS模块沿赛道走一圈每隔0.5米或1米记录一个坐标点生成一个点序列文件第二种是赛事组委会直接公布赛道GPS坐标文件选手下载后解析使用。两种方式到手后都可以转成本地平面前提下的(x, y)点集。得到点集之后算法要做的是“找最近点”。每一帧拿到当前GPS坐标遍历参考路径上的所有点找到距离最近的那个这个点对应的索引就是车在路径上的“投影位置”。有了投影位置就可以计算横向偏差——当前坐标到最近路径点的方向向量结合车的航向角算出车偏移了中心线多少。实际工程里不会真的每帧全量遍历几百上千个点那样浪费算力。常见做法是先用一个粗略的“当前索引窗口”缩小范围比如设定上下各50个点的搜索区间只有当车跑出这个区间时才重新全局搜索。因为赛车的运动是连续的上一帧在哪个位置下一帧大概率还在附近用滑动窗口可以把最近点搜索优化到微秒级。int find_nearest_point(float cur_x, float cur_y, int last_idx, int search_range) { int start last_idx - search_range; int end last_idx search_range; float min_dist 1e9f; int min_idx last_idx; for (int i start; i end; i) { int idx (i path_len) % path_len; // 环形路径取模 float dx cur_x - path_x[idx]; float dy cur_y - path_y[idx]; float d dx * dx dy * dy; if (d min_dist) { min_dist d; min_idx idx; } } return min_idx; }参考路径本身也要做预处理。一是平滑处理因为手工采集的点会有抖动可以用滑动平均或三次样条拟合避免路径上出现尖锐的拐角二是等间距重采样保证每个相邻点之间的距离大致相等这样在计算航向角、曲率的时候不会因为点距不均而产生跳变。2.3 GPS定位三边测量算法的补充解析很多做GPS定位的同学会问GPS模块自己就输出坐标了为什么还要学习三边测量算法这里有一个概念需要澄清GPS接收机内部确实是靠“三边测量”或“伪距定位”来解算自身位置的但模块输出的位置是已经解算好的结果。我们学习三边测量更多是为了理解定位误差是怎么来的以及在某些基站定位比如伪GPS、室内定位模拟器场景下如何自己写代码解算位置。三边测量的核心是已知三个以上基站的坐标和到目标的距离联立圆方程组解出目标位置。GPS里的“基站”就是卫星“距离”就是伪距。对于智能车竞赛来说如果你只是用现成GPS模块不需要自己写三边测量但如果你在做的是“基站定位循迹”比如在场地四周布置几个UWB基站那就需要用到这个算法了。// 经典的三边测量最小二乘解法示意 // 已知(mx[i], my[i])为基站坐标d[i]为测量距离 // 构造 Ax b用最小二乘求解目标位置 (x, y) void trilateration(float mx[], float my[], float d[], int n, float *out_x, float *out_y) { // 取第一个基站作为参考构造n-1个方程 // A [2*(mx[i]-mx[0]), 2*(my[i]-my[0])] // b d[0]^2 - d[i]^2 mx[i]^2 - mx[0]^2 my[i]^2 - my[0]^2 // 解线性方程组即可 }理解了原理之后你会发现GPS模块输出的坐标其实已经是对多颗卫星信号做了融合解算的结果模块内部做了大量滤波和校正。所以在做循迹的时候不需要重复造轮子只需关注模块输出的坐标质量即可。但如果你的应用场景是“模拟GPS信号”之类的测试环境三边测量算法就是必需品了。3. 实操过程与核心环节实现3.1 沁恒CH32V307环境搭建与GPS模块连接先说一下硬件连接。CH32V307VCT6开发板上串口资源丰富我们选用USART1接GPS模块波特率9600或115200具体看模块默认配置。GPS模块的TXD接MCU的RXDRXD接TXD共地用3.3V或5V供电取决于模块手册。CH32V307的串口接收用中断或DMA都行推荐DMA加空闲中断这样在处理长串NMEA语句的时候不容易丢字节。软件环境方面沁恒提供了MounRiver Studio开箱即用不需要额外配置工程模板。新建工程后在main函数里初始化串口和GPIO然后进入主循环不断解析串口接收到的数据。// NMEA数据解析提取GNGGA语句中的经纬度 void parse_nmea_line(char *line) { if (strncmp(line, $GNGGA, 6) 0) { // 按逗号分割 char *p line; int field 0; char *lat_str NULL, *lon_str NULL; while ((p strchr(p, ,)) ! NULL) { p; field; if (field 2) lat_str p; // 纬度 if (field 4) lon_str p; // 经度 } // 转换为十进制经纬度并调用 gps_to_local 转平面坐标 } }注意GNGGA语句中纬度格式是“ddmm.mmmm”要转成十进制度数需要把“度分”格式拆开前两位是度后面是分分除以60加到度上。经度同理。另外推荐使用GNGGA或GNRMC语句这两种包含定位质量指示可以根据定位状态决定是否更新坐标避免在未定位时用无效数据污染路径匹配。3.2 五路循迹传感器的引入与GPS互补纯GPS循迹在开阔场地确实能跑但一遇到桥洞、树荫、高架旁边这类遮挡严重的区域GPS信号会明显变差坐标漂移达到几米甚至几十米。这时候如果只靠GPS车的横向偏差会瞬间跳变随之而来的就是转向剧烈抖动。为了避免这种问题沁恒方案里通常会加一组五路灰度循迹传感器作为近场修正。所谓五路循迹就是一字排开五个红外灰度传感器分别检测赛道黑线或白线的位置。正常行驶时车在赛道中间中间三个传感器里靠近中心的有信号车偏左时左侧传感器先触线。通过五个传感器的ON/OFF组合可以粗略判断车相对赛道的横向偏移量。这个偏移量不需要很精确只要能区分“偏左、居中、偏右、大偏左、大偏右”这几种状态即可。GPS负责全局粗定位五路灰度负责局部细纠偏两者融合策略简单有效当GPS的横向偏差绝对值小于阈值比如0.5米时相信灰度传感器用它给出的偏移量参与转向当灰度传感器全部离地或找不到线时切回GPS数据。这样一来GPS的长时间稳定性和灰度传感器的短时间高精度就结合起来了。我个人在调试时发现这种融合策略的关键不是算法多复杂而是阈值切换要平滑。直接硬切换会让转向输出产生跳变车会抖一下。建议在切换点附近做一个线性过渡区比如GPS偏差从0.4米到0.6米之间灰度权重从1线性降到0GPS权重从0升到1这样过渡就柔顺很多。3.3 单车自平衡与转向控制的联动单车越野组和四轮组的本质区别在于单车必须先解决“站得住”的问题才有资格谈“跑得快”。沁恒方案里通常用MPU6050或ICM20602陀螺仪做姿态解算加上编码器反馈车轮转速构成一个典型的自平衡闭环。自平衡环的运行频率要比循迹环高得多。一般建议姿态环跑200Hz到500Hz而GPS循迹环只有10Hz到20HzGPS模块输出频率也不支持太高。这两个环之间怎么协同答案是级联外环是GPS循迹输出期望转向角和期望速度内环是自平衡和转向控制跟踪外环指令。外环频率低没关系内环在两次外环更新之间保持上一次的指令即可。实际工程中要注意的是转向控制不能直接叠加到直立平衡环的输出上。单车的转向靠的是车身倾斜和车轮转速差如果直接把转向PID输出加到电机PWM上会破坏自平衡的稳定性。正确做法是转向环输出的是“目标倾斜角”把目标倾斜角作为姿态环的输入让车身通过倾斜来实现转向。这个逻辑类似于骑自行车你拐弯时是先倾斜车身而不是直接拧车把。// 外环GPS循迹计算横向偏差 - 目标倾斜角 float lateral_error calc_lateral_error(cur_pos, nearest_path_point); float target_lean_angle lean_pid.compute(lateral_error); // 内环姿态环跟踪目标倾斜角 float current_lean_angle imu.get_angle_x(); float balance_output balance_pid.compute(target_lean_angle - current_lean_angle);4. 常见问题与排查技巧实录4.1 GPS数据跳变与静态漂移GPS模块在静止状态下坐标也会在小范围内随机漂移通常半径为1到3米。这个漂移在循迹时会造成横向偏差的抖动严重时车会像“画龙”一样左右摆。解决办法首先是硬件层面——把GPS天线放在车的最高点远离电机、电池等金属和电磁干扰源其次是软件层面——对GPS坐标做低通滤波或卡尔曼滤波。我实测下来最简单的滑动平均滤波取最近5到10个点的平均值就能明显改善漂移。但注意滑动窗口不宜过大否则车真正拐弯时坐标响应会变慢形成转弯迟滞。更进阶的做法是用卡尔曼滤波融合GPS和陀螺仪积分GPS负责修正长期漂移陀螺仪积分负责短期平滑两者互相修正效果非常好。如果你的场地周围有高楼或金属围栏GPS反射会造成多径效应坐标会突然跳出几十米。遇到这种情况一个非常实用的防护手段是设定单帧位移最大阈值。如果当前坐标与上一帧坐标的距离超过合理范围比如5米就判定这次GPS数据无效暂时用上一帧数据等待下一帧更新。这种简单的野值剔除策略能挡住绝大多数多径跳变。4.2 启动阶段无法定位或定位缓慢GPS冷启动时搜星可能需要几十秒甚至几分钟这对比赛来说是不可接受的。建议的解决办法是在赛道调试阶段就把GPS模块放在比赛场地持续上电至少10分钟让模块下载星历数据比赛前不要完全断电如果必须断电尽量缩短断电时间这样模块进入热启动状态重新定位时间能压缩到几秒。另外GPS模块的NMEA语句里有定位质量指示字段GGA语句中的第6个字段为0表示未定位为1表示单点定位为2表示差分定位。循迹代码里一定要判断这个字段未定位时不要使用坐标。否则在启动阶段车还没定位成功就已经开始跑了第一次算出的横向偏差可能指向几十米外车会直接冲出去。4.3 经纬度坐标在本地地图绘制时的大幅偏移问题热词里提到了一个很常见的现象原生GPS坐标在天地图这类国内地图上绘制时会偏移几百米。这个偏移是因为国内地图使用的坐标加偏了不是GPS本身错了。智能车比赛中我们用的是原始GPS坐标不需要做加偏纠偏所以这个问题在竞赛里不存在。但如果你把采集到的赛道坐标导入地图软件查看就会看到明显的偏移这是正常的。只需要在代码里统一使用WGS84坐标处理即可不要在中间混入火星坐标。4.4 常见问题速查表现象可能原因排查与解决办法坐标跳变几十米多径效应或野值增加单帧位移阈值过滤天线远离金属物车跑起来左右画龙GPS静态漂移未滤波加滑动平均或卡尔曼滤波减小转向P转弯明显滞后滑动窗口过大或外环频率低缩小滤波窗口提高GPS更新率或做预测补偿启动即冲出去未判断定位质量增加定位质量字段检查未定位时禁止循迹树荫/桥洞下失控GPS信号遮挡引入五路灰度传感器做近场修正自平衡和转向相互影响操控叠加方式错误转向应输出目标倾斜角而非直接加PWM5. 个人调试心得与扩展建议调试这套方案的过程中有一个体会特别深GPS循迹和室内循迹最大的区别在于“信任度”。室内电磁或摄像头数据每秒更新几百次你几乎可以把传感器数据当作真值来用。但GPS的数据是慢的、有噪声的、偶尔还撒谎的你必须建立一套数据可信度评估机制——什么时候信它什么时候不信什么时候降权使用。这个思维转变比任何算法本身都重要。另外一个小技巧分享给大家调试时不要一上来就跑全赛道先在赛道上选一段100米左右的直线段把车放在中心线上观察输出的横向偏差是否在零附近。再用遥控器或手动推车把车故意偏移0.5米观察横向偏差是否稳定输出0.5米左右的数值。这个“静态标定”能帮你快速验证坐标转换和路径匹配是否正确能省下大量的赛道试跑时间。至于后续扩展这套框架的可玩性其实很高。比如把GPS模块换成RTK高精度定位定位精度能从米级提升到厘米级很多控制策略都能简化再比如用ESP32或树莓派做视觉识别辅助识别路肩、障碍物甚至交叉路口就能从“基础循迹”升级到“高级感知”。甚至你可以在参考路径上标注不同的速度区间下坡前提前减速出弯后提前加速这些都是基于现有GPS框架的小改动。本文还有配套的精品资源点击获取