简介:面向C#与.NET平台的GPS路径规划示例工程,适合需要学习定位、导航模块开发的初中级开发者。工程以GPSDemo为主程序,涵盖串口读取、NMEA协议解析、经纬度实时获取以及速度、方向计算等关键环节,并涉及地图显示与路径规划的算法思路,可作为从零接入GPS功能时的参考模板。压缩包仅64KB,共21个文件,其中8个cs源码文件承载核心逻辑,2个resx资源文件与resources文件保存界面配置,另有exe可执行程序、pdb调试符号、txt说明文档及项目配置文件,结构清晰,便于对照阅读。目前已有302人浏览学习。通过运行示例和研读源码,开发者可以快速理解CGPSReader、CFGPS等类如何配合串口获取位置数据,并在此基础上扩展地图显示、路线搜索等功能;资源明确标注禁止商业使用,适合个人学习与教学研究。
1. 为什么串口能读到经纬度,导航却总在半成品阶段
串口能读到经纬度,但画在地图上却歪出几百米;车载终端动起来,轨迹在原地打转;点一下路径规划,界面直接假死几分钟。这三个现象,几乎是 C#/.NET 开发者做 GPS 路径规划导航时必踩的坎。定位这条链路上,串口读数只是最外层,真正决定成败的是坐标系转换、路网建模和线程交互这三块。这套资源就是围绕这三块展开的一套可运行 Demo:从 NMEA 0183 解析、GCJ-02 偏移,到地图轨迹绘制、Dijkstra/A* 路径规划,再到 NMEA 回放压测,源码里带小范围路网和回放数据。适合做车载终端、物流定位、巡检轨迹类项目的 C# 开发者,尤其是处于“硬件能读数、软件画不出来”状态的人。
2. 先过坐标系这一关:NMEA解析、GCJ-02偏移与数据清洗
2.1 GPS模块输出的是句子,不是坐标
GPS 模块上电后,默认按 1Hz 或 5Hz 从串口吐出一堆以$开头的句子,常见的有$GPRMC、$GPGGA、$GPGSA。新手最容易犯的错是把整条句子当字符串存起来,然后对着界面发呆。这串东西不是坐标,是一份带校验的报文,真正要提取的是$GPRMC里的时间、状态、纬度、经度、速度和航向。
private static bool CheckSum(string sentence) { int star = sentence.IndexOf('*'); if (star < 0 || sentence.Length - star < 3) return false; byte xor = 0; // 校验范围:$ 与 * 之间的所有字符,逐字节异或 for (int i = 1; i < star; i++) xor ^= (byte)sentence[i]; string expect = xor.ToString("X2"); return string.Equals(expect, sentence.Substring(star + 1, 2), StringComparison.OrdinalIgnoreCase); }校验和这步不能省。我见过太多“偶尔乱码”的案例,最后定位到就是校验和没做,半条句子被当有效数据塞进了轨迹列表。CheckSum的逻辑很简单:把$和*之间的每个字符按字节异或,得到两位十六进制,和*后面的原文比对。只要不等,整条句子直接丢弃,不进入后续解析。
拿到合法句子后,解析$GPRMC的字段。典型句子是:
$GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A其中纬度和经度是度分格式:4807.038表示 48 度 07.038 分,不是 48.07038 度,更不是 4807.038 度。直接double.Parse出来的数直接错出十万八千里。
private static double NmeaToDecimal(string coord) { // 4807.038 -> 48 + 07.038 / 60 = 48.1173 度 double raw = double.Parse(coord, CultureInfo.InvariantCulture); int degree = (int)(raw / 100.0); double minute = raw - degree * 100.0; return degree + minute / 60.0; }NmeaToDecimal是整套解析的地基。它把度分合并值拆成度和小数分,再把分除以 60 转成十进制度。注意CultureInfo.InvariantCulture必须写,否则在部分中文系统上小数点解析会翻车。南纬S和西经W还要乘-1,这个符号位在$GPRMC的第 4、6 个字段里。
组装后的解析方法长这样:
public GpsReading ParseGprmc(string sentence) { if (!CheckSum(sentence)) return null; string[] f = sentence.Split(','); // $GPRMC,UTC时间,定位状态,纬度,NS,经度,EW,速度,航向,日期,... if (f.Length < 12 || f[2] != "A") return null; // A=有效定位,V=无效 var reading = new GpsReading { UtcTime = DateTime.ParseExact(f[1], "HHmmss.fff", CultureInfo.InvariantCulture), Latitude = NmeaToDecimal(f[3]) * (f[4] == "S" ? -1 : 1), Longitude = NmeaToDecimal(f[5]) * (f[6] == "W" ? -1 : 1), SpeedKnots = double.TryParse(f[7], out double sp) ? sp : 0, Course = double.TryParse(f[8], out double co) ? co : 0 }; return reading; }f[2]的A/V是定位状态,V表示当前帧没有有效定位,直接返回null,不要往下画点。SpeedKnots是节,转公里每小时要乘 1.852,导航界面显示速度时别漏了这步。UTC 时间默认是格林尼治时间,国内显示要加 8 小时,并且要处理跨日期,不能只对小时做加法。
2.2 WGS-84、GCJ-02 和火星坐标:三层坐标系别混用
不管硬件输出什么,工程里绕不开三套坐标:
| 坐标系 | 原始来源 | 典型使用场景 |
|---|---|---|
| WGS-84 | GPS 模块直接输出 | 轨迹原始数据、国际地图 |
| GCJ-02 | 对 WGS-84 做非线性偏移 | 国内在线地图底图 |
| BD-09 | 在 GCJ-02 基础上再偏移 | 部分地图服务的专用坐标 |
GPS 模块原生吐出来的是 WGS-84。直接把 WGS-84 经纬度画到国内在线地图底图上,轨迹会整体偏移几十到几百米。这不是模块坏,是两套坐标系不兼容。资源入口处必须先做一次坐标统一,我一般把规则定死:串口原始数据一律按 WGS-84 处理,进地图图层前统一转成 GCJ-02。
public static GeoPoint Wgs84ToGcj02(double lat, double lon) { const double A = 6378245.0; // 参考椭球长半轴 const double EE = 0.00669342162296594323; // 偏心率平方 // 中国境外区域不做偏移,直接返回原值 if (lon < 72.004 || lon > 137.8347 || lat < 0.8293 || lat > 55.8271) return new GeoPoint(lat, lon); double dLat = TransformLat(lon - 105.0, lat - 35.0); double dLon = TransformLon(lon - 105.0, lat - 35.0); double radLat = lat / 180.0 * Math.PI; double magic = Math.Sin(radLat); magic = 1 - EE * magic * magic; double sqrtMagic = Math.Sqrt(magic); dLat = (dLat * 180.0) / ((A * (1 - EE)) / (magic * sqrtMagic) * Math.PI); dLon = (dLon * 180.0) / (A / sqrtMagic * Math.Cos(radLat) * Math.PI); return new GeoPoint(lat + dLat, lon + dLon); }TransformLat和TransformLon是实现里最像“玄学”的部分,它们是一组公开的拟合多项式,用来模拟 GCJ-02 的非线性偏移。我的建议是直接原样复制,不要去“优化”里面的常数,动一个数,偏移误差可能从 5 米变成 500 米。A是参考椭球长半轴,EE是偏心率平方,这两个值取自公开的椭球参数。境外判断的经纬度边界,是工程上约定俗成的范围,挡住明显不在中国大陆区域的数据,防止把合法坐标偏移成错误坐标。
2.3 别急着画点:定位状态、速度上限与跳点过滤
即使坐标转对了,裸数据直接上地图也会出现“轨迹乱飞”。问题不在 GPS,在数据清洗。我见过最典型的现象:车停在原地,地图上的点却在以 30 公里时速绕圈。原因是多路径效应加上模块冷启动未收敛,输出的一系列坐标在真实位置附近随机抖动。
private GpsPoint RejectOutlier(GpsPoint current, GpsPoint previous, double maxSpeedMps) { if (previous == null) return current; double dist = HaversineMeters(previous, current); double deltaTime = (current.Timestamp - previous.Timestamp).TotalSeconds; double speed = dist / deltaTime; // 超过设定速度上限,按跳点处理:丢弃当前点,沿用上一帧 if (speed > maxSpeedMps) return previous; return current; }maxSpeedMps是米每秒的物理上限。普通车载终端我设35,对应约 126 公里每小时;如果是物流车,设25就够;如果做无人机,可能要放宽到60。这个参数的物理意义是“两台设备不可能是这个相对速度”,超出就判定为跳点。HaversineMeters用球面距离公式算两点间距,不是平面直角坐标的勾股定理,后者在经度跨度大时误差明显。
提示:跳点过滤只解决“明显飞点”,不解决“整体慢漂”。慢漂靠 2.2 节的坐标统一,飞点靠这里的速度上限,两件事别混为一谈。
3. 地图显示与轨迹刷新:线程模型、瓦片缓存和抽稀阈值
3.1 在线瓦片还是离线瓦片:先决定网络依赖
地图底图是导航的视觉骨架。Demo 里做了一个双模式切换,因为真实项目里两种诉求都存在。在线瓦片加载简单、数据最新,但车跑在信号差的路段时整张图白板;离线瓦片需要预下载,占用磁盘空间,换来的是启动快、无网络依赖。
| 对比项 | 在线瓦片 | 离线瓦片 |
|---|---|---|
| 部署成本 | 低,配 URL 模板即可 | 高,需要切片工具预处理 |
| 加载速度 | 受网络波动影响 | 本地读取,稳定 |
| 磁盘占用 | 运行时缓存,可控制 | 需提前规划存储空间 |
| 适用场景 | 城市道路、演示环境 | 野外、隧道、园区 |
瓦片 URL 模板里关键是{z}/{x}/{y}三个占位符,分别对应缩放级别、横轴切片编号、纵轴切片编号。缩放级别从 1 到 18,级别越高,切片数量指数增长。离线包我只预下载当前作业区域的 14 到 18 级,再低级别的瓦片在车辆导航场景里没有实际意义,因为 1 级瓦片覆盖的是整个国家。
缓存策略上,在线模式我限制本地缓存目录最大 500MB,超过后按访问时间淘汰旧瓦片。离线模式则直接走File.ReadAllBytes,不走网络栈。很多卡顿不是绘制问题,是瓦片图层每次刷新都重新请求,网络栈和磁盘 IO 打架,UI 线程自然被拖垮。
3.2 DataReceived 在后台线程:UI 刷新必须排队
SerialPort.DataReceived事件是在后台线程触发的,这是无数 WinForms/WPF 开发者翻车的地方。直接在这个事件里操作地图控件,会抛跨线程异常;如果写了Control.CheckForIllegalCrossThreadCalls = false强行压下去,程序不报错,但 UI 会间歇性假死,因为两个线程在同时改同一个控件句柄。
private void ListenSerialPort(SerialPort port) { port.DataReceived += (s, e) => { try { string line = port.ReadLine(); // NMEA 以 \r\n 结尾 GpsReading reading = Parser.ParseGprmc(line); if (reading == null) return; // 回到 UI 线程再刷新地图,不能直接操作控件 mapControl.BeginInvoke(new Action(() => { currentReading = reading; UpdateMapUi(reading); })); } catch (TimeoutException) { // 串口超时是常态,记录日志即可 } catch (InvalidOperationException) { // 端口被关闭时触发,直接忽略 } }; }BeginInvoke是异步投递,不会阻塞串口读线程;Invoke是同步等待,在数据频率高时会拖慢接收。1Hz 数据用哪个都无所谓,5Hz 以上建议用BeginInvoke。ReadLine依赖SerialPort.NewLine属性,默认就是\r\n,NMEA 句子正好匹配。波特率我统一用 9600,部分模块默认 115200,要先看模块手册,波特率配错读到的是满屏乱码。
UpdateMapUi里不要整幅地图Invalidate。高频刷新时只更新当前定位点和最近一段轨迹,用Invalidate(Rectangle)传局部区域,否则 GPS 频率一上来,GDI+ 的绘制队列会被打满。
3.3 轨迹抽稀:3 米阈值和像素阈值二选一
轨迹画久了,内存里的点列表会膨胀。1Hz 数据跑一小时就是 3600 个点,跑一天就是 86400 个点,全画到 GDI+ 上,叠了一万次也不会有明显视觉变化,但 CPU 占用率会持续走高。抽稀不是丢数据,是把误差控制在一定范围内的采样。
private List<TrackPoint> ThinTrack(List<TrackPoint> pts, double minDistanceMeters) { var result = new List<TrackPoint>(); TrackPoint last = null; foreach (TrackPoint p in pts) { if (last != null && GeoUtil.HaversineMeters(last, p) < minDistanceMeters) continue; result.Add(p); last = p; } return result; }minDistanceMeters是相邻两个保留点之间的最小距离。车载场景我取 3 米,车辆每秒最多移动 30 米,3 米抽样不会丢拐弯特征。如果是步行巡检,阈值要降到 1 米,因为人步行的位移本来就小,3 米阈值会把一个小弯拉成直线。另一种思路是像素阈值:把点投影到屏幕坐标系,前后两点间距小于 2 个像素就丢弃。像素阈值更适配地图缩放,缺点是每次缩放要重算,适合对性能要求高的场景,我没在 Demo 里默认开。
4. 路径规划不玄学:路网模型、A*搜索与偏航重算的实际参数
4.1 路网数据的组织:节点表和邻接表
路径规划的前提是有一张路网图,不是拿着 GPS 点硬连。Demo 内置的训练路网是小型区域,数据来自开放地图数据的 Road 图层,导出成两个 CSV:节点表(ID、纬度、经度)和边表(起点 ID、终点 ID、道路等级)。加载时构建邻接表,这是图搜索最常用的内存结构。
public class RoadNode { public int Id { get; set; } public double Lat { get; set; } public double Lon { get; set; } public List<RoadEdge> Edges { get; set; } = new List<RoadEdge>(); } public class RoadEdge { public int ToNodeId { get; set; } public double WeightMeters { get; set; } // 通行代价,默认取边长 public int SpeedClass { get; set; } // 道路等级,用于动态调权 }路网建模有两个容易踩的坑。第一,节点的Id必须全局唯一,CSV 里重复 ID 会导致邻接表覆盖,搜索时出现鬼路径。第二,边表的权重不一定是几何距离,实际工程里红绿灯、限速、单行道都要折算进WeightMeters。Demo 里默认用HaversineMeters计算边长作为权重,这样跑出来的最短路径是“距离最短”,不是“时间最短”。要改成最快路径,把WeightMeters换成距离 / 该路段平均速度即可。
加载路网后,务必做一次连通性检查:从任意节点出发做 BFS(广度优先),统计能到达的节点数占比。占比低于 95% 说明路网有断裂,路径规划大概率搜不到解。这个检查在 Demo 里是启动时的自检项。
4.2 搜索算法:什么时候用 Dijkstra,什么时候换 A*
路网节点少、道路是静态属性时,Dijkstra 就够用,代码简单、不容易错。节点多且需要频繁重算时,A* 是工程上更合适的选择。两者的差别是 A* 在搜索时用启发函数引导方向,减少探索面积,但代价是启发函数要一致,否则可能得不到最优路径。
public List<int> FindPath(int startId, int goalId) { var gScore = new Dictionary<int, double>(); var fScore = new Dictionary<int, double>(); var cameFrom = new Dictionary<int, int>(); var visited = new HashSet<int>(); // 防止同一节点重复入队 var open = new SortedSet<QueueNode>(Comparer<QueueNode>.Create( (a, b) => a.F != b.F ? a.F.CompareTo(b.F) : a.Id.CompareTo(b.Id))); gScore[startId] = 0; fScore[startId] = Heuristic(startId, goalId); open.Add(new QueueNode(startId, fScore[startId])); while (open.Count > 0) { var cur = open.Min; open.Remove(cur); if (cur.Id == goalId) break; if (!visited.Add(cur.Id)) continue; // 已处理过,跳过 foreach (var edge in nodeMap[cur.Id].Edges) { double tentative = gScore[cur.Id] + edge.WeightMeters; if (tentative < gScore.GetValueOrDefault( edge.ToNodeId, double.PositiveInfinity)) { cameFrom[edge.ToNodeId] = cur.Id; gScore[edge.ToNodeId] = tentative; fScore[edge.ToNodeId] = tentative + Heuristic(edge.ToNodeId, goalId); open.Add(new QueueNode(edge.ToNodeId, fScore[edge.ToNodeId])); } } } return ReconstructPath(cameFrom, startId, goalId); }Heuristic用当前节点到终点的球面距离做预估。SortedSet按F = G + H排序,其中G是起点到当前节点的实际代价,H是当前节点到终点的预估代价。visited防重是关键优化,不加它会在大路网里重复处理同一个节点,搜索时间从毫秒级膨胀到秒级。
工程上的取舍:路网节点在 5 万以下、路况固定,Dijkstra 足够;节点几十万、需要频繁偏航重算,换 A*。Demo 默认用 A*,因为偏航重算会频繁触发,Dijkstra 每次重算全图,UI 线程压力大。另一个细节是搜索超时,我习惯给 2 秒的CancellationTokenSource上限,超过就返回失败提示,而不是让界面转圈等一个未知的结果。
4.3 偏航判定与重算:局部窗口和重试上限
导航过程中,车不是永远乖乖沿着规划路线走。偏航判定不能把当前 GPS 点和整条路线的每个点都比一遍,路线有几千个点,每 1 秒全量遍历一次,UI 就卡了。正确做法是记住上一次匹配到的路线点下标,只在下标前后各 20 个点的小窗口里找最近距离。
private bool IsOffRoute(GeoPoint pos, List<GeoPoint> route, int lastIndex, out int matchedIndex, double thresholdMeters = 30) { int start = Math.Max(0, lastIndex - 20); int end = Math.Min(route.Count - 1, lastIndex + 20); double minDist = double.MaxValue; matchedIndex = lastIndex; for (int i = start; i <= end; i++) { double dist = GeoUtil.HaversineMeters(pos, route[i]); if (dist < minDist) { minDist = dist; matchedIndex = i; } } return minDist > thresholdMeters; // 超过阈值视为偏航 }thresholdMeters取 30 米是经验值。城市环境下 GPS 漂移通常在 10 到 20 米,30 米阈值能容忍正常漂移,又能在真的走错路口时及时触发重算。lastIndex局部窗口假设车辆按路线顺序前进,如果车辆掉头往回走,窗口会跟不上,此时匹配距离会突然变大,偏航判定照样触发,重算逻辑会生成一条掉头后的新路线。
触发偏航后,不要立刻重算。连续两次判定都在 1 秒内,说明 GPS 信号瞬间跳变,不是真实偏航,给一个重试延迟。我习惯重算最多连续触发三次,三次都失败就停止自动重算,等用户手动操作,防止导航进入“无限重算”的死循环。
5. 避坑实录:串口丢帧、坐标漂移与UI假死的五个案例
5.1 现象:串口读到的句子断成两截,解析全部失败
某次联调,日志里全是CheckSum失败,偶尔能解出一条合法数据。看串口监视器,句子在中间被截断,$GPRMC后半段跑到下一条句子里去了。
原因:USB 转串口驱动缓冲不够大,数据到达过快时缓冲区溢出,NMEA 句子被切断;另外波特率配成 115200 时,模块输出频率高,接收端处理不过来。
解决:串口接收改用行缓冲拼接,不直接依赖ReadLine。收到字节先追加到StringBuilder,检测到\n才算一条完整句子,再交给解析器。同时把SerialPort.ReadBufferSize调到 4096,波特率降回 9600。这是资源里 Parser 层的默认实现,血的教训。
5.2 现象:车停在原地,轨迹却绕着小圈转
户外测试,车静止停在路边,地图上的定位点以 20 米半径绕圈。同事第一反应是 GPS 模块坏了,换了好几个模块都一样。
原因:模块处于冷启动状态,星历未收敛;加上基站辅助定位和 GPS 信号多路径叠加,位置解算结果在真实位置附近随机抖动。数据清洗没做,裸坐标直接上绘制层。
解决:先做 HDOP 精度因子过滤,HDOP 大于 3 的帧直接丢弃;再做静止判定,速度低于 0.5 米每秒时把位置锁定在上一帧有效点。Demo 里的RejectOutlier加上这层逻辑后,静止场景轨迹是一个点而不是一个圈。
5.3 现象:拖动地图卡死,松开后恢复
地图拖动时界面完全无响应,鼠标松开后才一帧一帧地刷新。把瓦片加载日志打开,发现拖拽过程中每秒发了几十个瓦片请求。
原因:DataReceived每来一个定位点就触发整图刷新,地图控件在拖拽本身占满 UI 线程的同时,还要处理轨迹重绘和瓦片请求,线程队列被打爆。
解决:地图控件的拖拽事件里挂一个isDragging标志,拖拽期间暂停轨迹刷新,松手后再刷新一次当前帧。瓦片换成离线模式后,拖拽卡顿基本消失。这个开关在资源里是mapControl.DragStarted和DragCompleted两个事件控制的。
5.4 现象:路径规划点下去,界面转圈不出结果
导航按钮按下后,程序卡了十几秒才弹出路线,有时直接无响应。单步调试发现搜索算法陷入一个巨大的循环里。
原因:路网 CSV 加载时边表有重复记录,同一个节点对出现了三条平行边,搜索过程反复更新同一批节点的代价值;加上没有visited去重,节点被反复入队。
解决:加载边表时,同一(FromNodeId, ToNodeId)只保留权重最小的一条;搜索算法加上visited集合;给规划加上 2 秒超时。三条都做了,规划耗时从十几秒降到 200 毫秒以内。
5.5 现象:回放数据正常,真机轨迹却整体偏一大截
用保存的 NMEA 文件回放,轨迹和道路完全贴合。换到真机上车测试,轨迹整体往东偏移约 200 米,形状不像,是平行位移。
原因:回放文件里的坐标是 WGS-84,而地图底图用的国内在线地图图层,底图坐标系是 GCJ-02。真机测试时,没注意图层坐标系和原始坐标系不一致,导致轨迹整体平移。
解决:在串口数据入口统一调用一次Wgs84ToGcj02,让所有进图层的坐标都走同一套转换。这个坑特别隐蔽,因为轨迹形状没变,只有整体偏移,肉眼很难发现。从那以后,我每接到一个新项目,第一件事就是确认三个坐标系:硬件输出什么、地图底图用什么、代码入口统一到哪套。
6. 验证:用NMEA回放压测整套导航,定位三个软肋
Demo 里附带了一组真实采集的 NMEA 回放文件,这是整套资源里最值钱的验证工具。回放不是把文件里的句子按最快速度灌进解析器,而要按采集时的真实时间间隔逐条触发,才能暴露线程交互和刷新策略的问题。
private async Task ReplayNmeaFileAsync(string path, Action<GpsReading> onReading) { string[] lines = await File.ReadAllLinesAsync(path); DateTime? lastUtc = null; foreach (string line in lines) { if (string.IsNullOrWhiteSpace(line) || !line.StartsWith("$GPRMC")) continue; GpsReading reading = Parser.ParseGprmc(line); if (reading == null) continue; if (lastUtc.HasValue && reading.UtcTime > lastUtc.Value) { int gapMs = (int)(reading.UtcTime - lastUtc.Value) .TotalMilliseconds; await Task.Delay(gapMs); // 按采集时的真实间隔回放 } lastUtc = reading.UtcTime; onReading(reading); } }回放脚本跑起来后,我固定压三个软肋。第一是解析正确率:统计有效定位帧占全部 GPRMC 帧的比例,低于 99% 说明解析层有丢帧,回头看校验和与缓冲逻辑。第二是路径规划耗时:随机取 50 组起终点循环规划,记录最大耗时,超过 1 秒就检查路网连通性和visited去重是否生效。第三是长时间运行稳定性:回放 30 分钟,盯任务管理器里的内存和 GDI+ 句柄数,内存持续上涨说明轨迹列表没做抽稀和裁剪,句柄数突破 10000 说明绘制层有泄漏。
这套流程救过我一次大麻烦。那次项目交付前夜,真机测试一切正常,我顺手跑了一遍回放,发现内存以每分钟 15MB 的速度上涨。查了半天,发现轨迹图层在每次刷新时新建Pen和Brush,没走缓存池,GDI+ 对象没有及时释放。回放脚本第一时间把这个隐藏的内存泄漏暴露出来,比在客户现场翻车再返工划算得多。从那以后,每次接定位类项目,我都强制把当天采集的原始 NMEA 文件存一份,先做回放压测再谈上线。数据是黑匣子,回放就是给黑匣子留的后悔药。这套 Demo 的资源包已经整理了完整源码、路网 CSV 和回放数据,下载后直接跑一遍回放,你会对自己要修的部分有直观认识。希望帮到你。
本文还有配套的精品资源,点击获取