课程导论与坐标系基础
大家好，我是你们这门课的主讲人。在自动驾驶这个行当摸爬滚打了十来年，踩过的坑比走过的路还多。今天咱们开始第一讲，聊聊规划控制的全貌，再把这个领域最基础、也最容易出问题的坐标系讲透。
说实话，坐标系这个东西，看着简单，但我在项目里见过太多人在这上面栽跟头。有一次我们路测，车突然往路边偏，查了两天，最后发现是某个模块的坐标变换矩阵写反了。嗯，从那以后，我每次做坐标变换都要手算一遍验证。
自动驾驶规划控制：到底在解决什么问题？
规划控制，说白了就是回答三个问题：
- 我在哪？ —— 定位与感知
- 我要去哪？ —— 路径规划
- 我怎么去？ —— 运动控制
你想想看，一个自动驾驶系统，从传感器拿到数据，到最终方向盘转动、油门踩下，中间经历了多少层抽象？我个人习惯把整个流程分成五层：
- 感知层：看懂周围世界（车道线、障碍物、红绿灯）
- 预测层：猜别人要干嘛（旁边那辆车会不会变道？）
- 决策层：定大方向（是超车还是跟车？）
- 规划层：算具体路径（怎么打方向、怎么踩刹车）
- 控制层：执行动作（让车实际动起来）
我们这门课，重点就在第4和第5层。但别急，坐标系是贯穿所有层的基石，必须先讲透。
核心观点：规划控制不是孤立的算法堆砌，而是一个从「环境理解」到「物理执行」的完整闭环。坐标系就是连接这个闭环的「通用语言」。
车辆坐标系与全局坐标系：两个世界的对话
做规划控制，你至少得掌握两套坐标系。我刚开始带新人时，发现很多人搞混这两者，结果路径规划出来，车直接往墙上怼。
1. 车辆坐标系（车身坐标系）
这个坐标系是跟着车走的。一般定义：
- x轴：车头方向为正
- y轴：车身左侧为正（注意，有些标准用右侧，一定要确认清楚）
- z轴：车顶方向为正
- 原点：通常在后轴中心，或者车辆质心
为什么用后轴中心？我在做路径跟踪时发现，用后轴中心作为参考点，计算车辆运动学模型会简单很多。你想想看，前轮可以转向，后轮是固定的，以后轴为原点，车辆的瞬时转向中心就好算了。
2. 全局坐标系（世界坐标系）
这个坐标系是固定不动的。常见的有：
- UTM坐标系：通用横轴墨卡托投影，把地球表面投影到平面上
- 高斯-克吕格投影：国内常用
- 自定义笛卡尔坐标系：小范围场景，比如停车场
全局坐标系的原点通常是固定的，比如某个路口中心，或者高精地图的基准点。
个人经验：做实际项目时，我建议在全局坐标系中统一使用UTM。虽然它在中高纬度地区有变形，但自动驾驶通常只在城市道路跑，几十公里范围内误差可以忽略。别混用多种投影，否则坐标变换会让你怀疑人生。
坐标变换基础：从车身到世界，再回来
坐标变换，本质上就是矩阵乘法。但别被数学吓到，实际用起来就两种操作：
- 平移：把原点挪个位置
- 旋转：把坐标轴转个方向
组合起来就是齐次变换矩阵。我给你们看个最常用的例子：
// 车辆坐标系到全局坐标系的变换
// 假设车辆在全局坐标系中的位置为 (x, y)，航向角为 yaw
// 齐次变换矩阵 T
T = [
[cos(yaw), -sin(yaw), x],
[sin(yaw), cos(yaw), y],
[0, 0, 1]
]
// 车辆坐标系中的点 (px_vehicle, py_vehicle)
// 变换到全局坐标系：
px_global = cos(yaw) * px_vehicle - sin(yaw) * py_vehicle + x
py_global = sin(yaw) * px_vehicle + cos(yaw) * py_vehicle + y
这段代码看着简单吧？但我告诉你，实际项目中90%的坐标变换bug都出在符号上。sin和cos的顺序、正负号，稍微搞错一个，车就往反方向跑了。
避坑指南：我曾经在写路径平滑模块时，把旋转矩阵的转置和逆搞混了。结果路径点全部镜像翻转，车在测试场里画了个完美的对称图形——然后撞上了另一侧的路沿。记住：旋转矩阵是正交矩阵，它的逆等于转置。但平移部分不能这么搞！
知识体系总览
下面这张图，是我自己梳理的规划控制知识体系。你可以把它当作整个课程的地图：
从这张图你能看到，坐标系不是单独存在的，它像一条线，把感知、决策、规划、控制、执行全部串起来。你在规划层算出来的路径点，最终要变成控制层的方向盘转角，中间每一步都在做坐标变换。
实际项目中的坐标系陷阱
讲几个我踩过的坑，你们以后遇到了能少走弯路：
| 陷阱类型 | 具体表现 | 解决方案 |
|---|---|---|
| 航向角定义不一致 | 有的模块用北偏东，有的用东偏北 | 统一用「北偏东为正，逆时针增大」 |
| 坐标系手性混淆 | 左手系和右手系混用 | 全部统一为右手系，z轴向上 |
| 时间戳不同步 | 坐标变换用了不同时刻的位姿 | 加时间戳缓存，做插值对齐 |
| 浮点精度误差 | 多次变换后累积误差导致路径偏移 | 定期用全局坐标重置，避免长链变换 |
我的习惯：每次写完坐标变换代码，我都会用一组已知点做正向和反向验证。比如：车身坐标(1,0)经过变换到全局，再变换回来，看是不是(1,0)。如果误差超过1e-6，说明代码有问题。这个习惯救了我好几次。
好了，这一章的内容就到这。坐标系这个东西，看似基础，但它是整个规划控制系统的地基。地基不稳，上面盖多高的楼都得塌。下一章我们会深入路径规划的具体算法，到时候你会发现，所有算法都离不开今天讲的坐标变换。
可可精品资料库