第三节 坐标系与运动学：全局坐标系、局部坐标系、车辆运动学模型及坐标变换

在规控算法面试中，坐标系与运动学是贯穿感知、预测、规划、控制四大模块的基础理论。无论是L2级辅助驾驶的ACC（自适应巡航），还是L4级Robotaxi的复杂路口博弈，所有决策与控制都建立在“物体在哪里、车辆朝向哪里、如何描述运动”这一根本问题上。本节将系统梳理全局坐标系与局部坐标系的定义与转换逻辑，深入拆解自行车模型与阿克曼模型的数学本质，并结合实际工程案例说明坐标变换在真值系统、传感器融合与控制反馈中的具体应用。

上图展示了本节的核心知识框架：全局坐标系提供位置基准，局部坐标系描述相对车辆或传感器的空间关系，而运动学模型则基于车辆自身状态（速度、前轮转角）进行运动预测。三者通过坐标变换矩阵紧密耦合，构成规控算法的空间与运动基础。下面逐一展开。

3.1 全局坐标系与局部坐标系的定义与关系

核心原理

全局坐标系（Global Frame）通常采用UTM（通用横墨卡托）投影或WGS-84经纬度坐标，其原点固定在地球某一点，X轴指向东、Y轴指向北（ENU约定），Z轴垂直向上。局部坐标系（Local Frame）则依附于车辆或传感器，常见的有车体坐标系（Vehicle Frame）和传感器坐标系（Sensor Frame）。车体坐标系原点位于车辆后轴中心（或质心），X轴指向车辆前进方向，Y轴指向左侧，Z轴向上，满足右手定则。传感器坐标系则根据安装位置定义，例如前视相机坐标系原点在光心，Z轴指向车辆前方。

两者之间的转换是刚体变换，包含旋转和平移。设车辆在全局坐标系中的位置为 (x_g, y_g)，航向角为 ψ（车辆纵轴与全局X轴的夹角），则局部坐标点 P_l = (x_l, y_l) 到全局坐标 P_g = (x_g, y_g) 的变换为：

P_g.x = x_g + x_l * cos(ψ) - y_l * sin(ψ)
P_g.y = y_g + x_l * sin(ψ) + y_l * cos(ψ)

反之，全局坐标到局部坐标的变换为：

P_l.x = (P_g.x - x_g) * cos(ψ) + (P_g.y - y_g) * sin(ψ)
P_l.y = -(P_g.x - x_g) * sin(ψ) + (P_g.y - y_g) * cos(ψ)

这一变换在感知融合中至关重要。例如，毫米波雷达检测到的目标位于雷达坐标系，必须通过外参矩阵转换到车体坐标系，再结合车辆位姿转换到全局坐标系，才能与高精地图上的车道线、交通标志进行关联匹配。

判断方法或操作流程

在实际工程中，坐标变换的常见操作流程如下：

1. 确认源坐标系与目标坐标系：明确数据来自哪个传感器（如LiDAR、Camera、Radar），其坐标系定义（原点、轴向、单位）。
2. 获取外参标定结果：传感器到车体的外参（旋转矩阵R_s2v、平移向量T_s2v）通常通过离线标定获得，标定精度直接影响融合效果。
3. 获取车辆位姿：从组合导航系统（RTK+IMU）读取车辆在全局坐标系下的位置 (x_g, y_g) 和航向角 ψ。
4. 执行变换：先传感器→车体，再车体→全局。每一步都使用对应的旋转和平移参数。
5. 验证与调试：将变换后的目标投影到全局地图上，检查是否与真实道路设施对齐，若偏差过大需检查标定参数或时间同步问题。

案例拆解：多传感器目标融合中的坐标对齐

某L4级自动驾驶测试车搭载了前向毫米波雷达和128线激光雷达。雷达输出的目标位置为 (r, θ) 极坐标，转换为雷达直角坐标 (x_r, y_r) 后，需经过两步变换：首先通过外参矩阵将雷达坐标转换到车体坐标（考虑雷达安装在车头保险杠中心，外参为 T = [1.2m, 0, 0.5m]，旋转角为0），然后利用车辆当前位姿（x_g=500123.45m, y_g=4432112.78m, ψ=1.2345rad）转换到全局坐标。若忽略雷达安装位置（即认为雷达在车体原点），在车辆以72km/h（20m/s）行驶时，1.2m的安装偏移将导致目标在全局坐标系中产生约1.2m的横向误差，足以使车辆错误地并入相邻车道。因此，精确的外参标定和坐标变换是融合感知的基石。

常见误区与总结复盘

此外，时间同步问题常被忽视。传感器数据与车辆位姿数据必须时间对齐，否则在高速运动下，几十毫秒的延迟会导致数十厘米的位置误差。工程上通常采用插值或缓存机制解决。

3.2 车辆运动学模型：自行车模型与阿克曼模型

核心原理

车辆运动学模型忽略轮胎受力与侧偏，仅从几何关系描述车辆运动，适用于低速（通常<10m/s）或规划控制中的参考轨迹生成。最常用的是自行车模型（Bicycle Model），它将车辆的前后轮分别合并为一个虚拟车轮，位于车辆纵轴线上。模型输入为车速 v 和前轮转角 δ，状态为车辆位置 (x, y) 和航向角 ψ。其连续时间运动学方程为：

dx/dt = v * cos(ψ + β)
dy/dt = v * sin(ψ + β)
dψ/dt = v * cos(β) * tan(δ) / L
β = arctan( (l_r / L) * tan(δ) )

其中 L 为轴距，l_r 为质心到后轴的距离，β 为质心侧偏角。若以后轴中心为参考点，则 β=0，方程简化为：

dx/dt = v * cos(ψ)
dy/dt = v * sin(ψ)
dψ/dt = v * tan(δ) / L

阿克曼模型（Ackermann Model）则更精确地描述了四轮车辆转向时内外轮转角不同的几何关系。在低速转弯时，为了确保所有车轮绕同一瞬时转向中心转动，前轮内轮转角 δ_i 与外轮转角 δ_o 满足：

cot(δ_o) - cot(δ_i) = B / L

其中 B 为轮距。实际车辆通过转向梯形机构近似实现阿克曼转向。在规控算法中，若使用自行车模型，通常将 δ 视为等效前轮转角（即内外轮转角的平均值），而阿克曼模型则用于精确计算每个车轮的转向角度，或用于校验车辆转向执行机构是否满足几何约束。

判断方法或操作流程

选择哪种模型取决于应用场景：

操作流程上，使用自行车模型进行状态预测时，先根据当前车速 v 和前轮转角 δ，利用上述微分方程进行离散化（如欧拉法或龙格-库塔法），得到下一时刻的车辆状态。离散化步长通常取 10-50ms，取决于控制周期。

案例拆解：自动泊车路径跟踪中的模型应用

某自动泊车系统采用自行车模型进行路径跟踪控制。车辆轴距 L=2.8m，最大前轮转角 δ_max=35°（约0.61rad），泊车车速 v=1.5m/s。控制器根据参考路径上的预瞄点，计算所需的前轮转角 δ。由于泊车速度极低，轮胎侧偏角可忽略，自行车模型精度足够。但在实际测试中发现，当车辆接近车位边缘时，跟踪误差增大至15cm。分析原因：模型未考虑车辆宽度，导致车辆实际外轮廓与参考路径偏差。解决方案是在路径规划阶段对参考路径进行膨胀处理，将车辆宽度的一半作为安全距离。此外，由于阿克曼转向机构的存在，实际内外轮转角不同，但等效前轮转角 δ 与自行车模型中的 δ 一致，因此控制指令可直接发送给转向执行器，由转向梯形机构自动实现内外轮转角分配。

常见误区与总结复盘

另一个常见误区是混淆“前轮转角”与“方向盘转角”。方向盘转角是驾驶员输入，经过转向系统传动比（通常为15:1~20:1）后得到前轮转角。在模型中使用的是前轮转角，若直接使用方向盘转角会导致控制量错误。

3.3 坐标变换与运动学模型的联合应用

核心原理

在规控算法中，坐标变换与运动学模型并非孤立使用。例如，在轨迹跟踪控制中，控制器通常将全局坐标系下的参考轨迹点转换到车体坐标系下，然后基于车体坐标系的横向误差和航向误差计算控制量。这一过程称为“误差模型”或“路径跟踪模型”。设参考轨迹点 P_ref 在全局坐标系下为 (x_ref, y_ref, ψ_ref)，车辆当前状态为 (x, y, ψ)，则车体坐标系下的误差为：

e_x = (x_ref - x) * cos(ψ) + (y_ref - y) * sin(ψ)
e_y = -(x_ref - x) * sin(ψ) + (y_ref - y) * cos(ψ)
e_ψ = ψ_ref - ψ

其中 e_y 为横向偏差，e_ψ 为航向偏差。基于这些误差，设计PID或纯跟踪（Pure Pursuit）控制器，输出前轮转角 δ。纯跟踪算法以车辆后轴中心为基准，在参考轨迹上寻找一个预瞄距离 L_d 处的点，计算车辆纵轴与该点的夹角 α，则 δ = arctan(2 * L * sin(α) / L_d)。该算法本质上是自行车模型的几何应用。

操作流程：从全局路径到控制指令

1. 路径生成：规划模块输出全局路径（一系列全局坐标点）。
2. 坐标变换：将当前车辆位置附近的路径点转换到车体坐标系。
3. 误差计算：计算横向偏差 e_y 和航向偏差 e_ψ。
4. 控制计算：根据控制算法（如纯跟踪、Stanley或MPC）计算前轮转角 δ。
5. 执行与反馈：将 δ 发送给转向执行器，同时通过IMU/编码器反馈实际转角，形成闭环。

案例拆解：高速公路弯道跟踪的坐标变换与模型联合调优

某自动驾驶系统在高速弯道（半径 R=250m）进行轨迹跟踪测试，车速 80km/h（22.2m/s），轴距 L=3m。使用纯跟踪算法，预瞄距离 L_d 设置为车速的0.8倍（约17.8m）。初始测试发现车辆在弯道内出现明显的“切弯”现象，即实际轨迹比参考轨迹更靠内侧。分析原因：预瞄距离过短，导致控制器响应过快，且未考虑车辆侧向加速度限制。通过将预瞄距离调整为车速的1.2倍（约26.6m），并增加前馈转角项 δ_ff = arctan(L / R)（基于弯道半径的几何前馈），跟踪误差从0.35m降低至0.08m。此案例说明，运动学模型与坐标变换的联合应用需要结合车辆动力学约束进行参数整定。

常见误区与总结复盘

此外，运动学模型的离散化步长选择需权衡精度与计算量。步长过大（>100ms）会导致预测发散，步长过小（<1ms）则计算负担重。一般控制周期为20ms，预测步长取10ms即可满足精度要求。

3.4 知识结构总结与面试要点

面试中，除了要求写出公式，面试官更关注候选人对模型适用边界的理解。例如，当被问及“在冰雪路面低速行驶时，自行车模型是否适用？”正确答案是：低速下轮胎侧偏角小，运动学模型仍可近似，但需注意路面摩擦系数对最大加速度的限制，若车辆打滑则模型失效。此外，坐标变换中的“时间同步”问题也是高频考点，需强调传感器数据与车辆位姿的时间戳对齐。

总结本节，坐标系与运动学是规控算法的“语言”与“语法”。掌握全局与局部坐标系的变换矩阵，理解自行车模型与阿克曼模型的几何本质，并能在实际工程中灵活应用，是面试通关的关键。建议读者在纸上手动推导一遍坐标变换公式和自行车模型微分方程，并尝试用Python实现一个简单的纯跟踪控制器，以加深理解。