这篇是坐标转换推导的第二篇。上一篇我们把Frenet坐标系的定义、弧长参数化、参考线切向量与法向量这些基础概念梳理了一遍顺手解释了为什么路径规划里非要用Frenet坐标——因为在结构化道路上纵向和横向分离之后约束的施加、代价的加权、障碍物的处理都会直观很多。这一篇直接进入正题把Cartesian状态和Frenet状态之间的转换公式一步步推出来并给出我在工程实现里踩过的坑。先说清楚这篇要解决什么问题。规划模块里Lattice Planner、EM Planner这类算法往往在Frenet坐标系下撒点、生成轨迹因为横纵向解耦后道路边界约束变成了简单的d通道上下界但最终轨迹必须转回Cartesian坐标给控制模块跟踪。反过来控制模块收到的是车辆在Cartesian下的位置、速度、加速度要计算与参考线的横向偏差、纵向偏差就必须把Cartesian状态映射回Frenet坐标。两个方向都要用而且公式里的每个符号都牵一发动全身。这篇推导的目标读者是正在写轨迹规划、轨迹跟踪或者在做路径重规划的同学。看之前建议先把上一篇的基础概念过一遍特别是参考线曲率的符号约定、切向量和法向量的方向定义这些会直接决定后面所有公式的正负号。1. 坐标系定义与符号约定搞错一个正负号整车方向就反了1.1 两组坐标各管哪一块Cartesian坐标系没什么好说的就是全局直角坐标一个点用(x, y)表示车辆的完整运动状态通常写成(x, y, v, θ, κ, a)位置p (x, y)速度大小v航向角θ_x速度方向与x轴夹角路径曲率κ车辆实际行驶轨迹的曲率加速度a沿速度方向的切向加速度Frenet坐标系则完全依附于参考线。参考线通常是一段平滑曲线用弧长参数s描述r(s)表示参考线上弧长为s的点。一个点在该坐标系下的表达是(s, d)s点在参考线上的投影点的弧长d点到参考线的带符号距离也就是横向偏移车辆运动状态在Frenet下就是(s, d)及其对时间的导数s_dot、d_dot、s_ddot、d_ddot。这里建议直接把“d为左侧偏距为正”作为默认约定后续推导全部基于这个假设。如果你用的库或者论文定义恰好相反那所有涉及d的公式都要整体取反。1.2 切向量、法向量与有符号曲率整套公式的地基参考线上每个点都有两个随弧长移动的单位向量。先说切线方向t_r(s) r(s) dr(s) / ds这是弧长参数化的标准结果单位切向量。法向量n_r(s)我统一取为切向量逆时针旋转90度n_r(s) (-t_y(s), t_x(s))在道路场景里这个约定意味着“车辆位于参考线左侧时d 0”。有的资料里法向量取顺时针旋转这会导致d的符号整体翻转。真正容易出问题的在曲率这里。平面曲线的曲率如果只看绝对值那参考线是顺时针还是逆时针完全没有区分但Frenet公式里需要用有符号曲率κ_r(s)并且要满足法向量的导数关系n_r(s) -κ_r(s) · t_r(s)这里κ_r(s)取正还是取负取决于曲线在局部是朝哪个方向弯。具体验证方法对逆时针圆周κ_r 1/R 0左法向量指向圆心n随s增加会逐渐偏向切线反方向用上面的导数关系一验就能对上。所以整个推导的符号约定可以压缩成一句话d左正、n左法、κ带符号。为了后面查起来方便我把核心符号整理成一张表符号含义说明r(s)参考线上弧长s处的点需要用连续参数化参考线t_r(s)参考线的单位切向量dr/ds方向与s增大方向一致n_r(s)参考线的单位法向量切向量逆时针旋转90度κ_r(s)参考线的有符号曲率逆时针弯曲为正θ_r(s)参考线的切向角与x轴的夹角d横向偏移左侧为正即沿n_r方向为正Δθ方位角差Δθ θ_x - θ_r(s)最后强调一次这组约定必须在正向转换、反向转换、控制偏差计算三处同时保持一致。我见过有人推导时用左正约定写代码时却用了某个现成库里右正的d结果规划出来的轨迹在弯道里完全镜像排查了一个下午才找到原因。2. 正向推导Cartesian坐标到Frenet坐标2.1 第一步是求投影点先找准“对应”到参考线的哪个位置从Cartesian状态求Frenet状态第一件事不是套速度公式而是先把参考线上与车辆对应的点找出来。这个点称为投影点它满足的条件是向量p - r(s)与参考线在r(s)处的切线垂直(p - r(s)) · t_r(s) 0直观理解就是车辆位置到参考线的垂足。找到满足条件的弧长s后横向偏移d就是d (p - r(s)) · n_r(s)如果投影不唯一比如车辆在弯道内侧且距离参考线较远这时会有多个s候选点。工程上的处理方式通常是加一个s连续性的约束以上一帧的s作为邻域约束选最近的候选点。这个问题在后面的“避坑”部分还会重点展开因为在匝道分岔路段投影点直接跳到另一条分支是常见灾难。投影点的求取依赖参考线参数化方式。如果参考线是用三次样条维护的可以直接在样条上做一维搜索迭代如果是稀疏离散点则需要先用样条插值或分段多项式拟合再求解投影。2.2 速度分解最核心的一阶公式位置关系是基础p r(s) d · n_r(s)现在对时间求导。注意s和d都是时间的函数n_r本身又依赖s所以必须用链式法则会把三项都展开p_dot s_dot · t_r(s) d_dot · n_r(s) d · n_r(s) · s_dot把n_r(s) -κ_r(s) · t_r(s)代进去合并t_r方向的系数p_dot (1 - κ_r · d) · s_dot · t_r d_dot · n_r另一方面车辆速度向量本身可以在参考线切向和法向上做投影。设车辆航向角θ_x与参考线切向角θ_r的差为Δθ那么速度的大小和方向可以表示为v_vec v · cos(Δθ) · t_r v · sin(Δθ) · n_r两个式子一对比就得到一阶转换公式d_dot v · sin(Δθ)s_dot v · cos(Δθ) / (1 - κ_r · d)第一个式子说明横向速度来自速度方向与参考线方向不一致的分量。车辆完全沿着参考线方向行驶时d_dot恒为零一旦车辆航向偏离参考线横向偏移率就出现了。第二个式子的分母值得细看。1 - κ_r · d这个因子说明当参考线弯弯曲曲的时候车辆沿参考线方向的“推进速度”不等于车辆自身速度在切向的投影还要考虑横向偏移导致的行车轨迹与参考线长度变化不一致。拿圆弧参考线举例车贴着半径为R的参考线内侧偏移d行驶时行车轨迹半径是R - d同等车速度下圆周角速度增大反映在s_dot上就是被分母放大。2.3 加速度维度二阶项的完整推导一阶公式只解决位置和速度映射轨迹规划往往还需要加速度。先铺垫两个预备关系车辆航向角变化率与车辆轨迹曲率的关系θ_x_dot v · κ。这里的κ是车辆实际行驶轨迹的有符号曲率不是参考线曲率。参考线切向角变化率θ_r_dot κ_r · s_dot。所以Δθ的导数就是Δθ_dot v · κ - κ_r · s_dot对d_dot v · sin(Δθ)求导d_ddot a · sin(Δθ) v · cos(Δθ) · (v · κ - κ_r · s_dot)这个式子物理含义很明确横向加速度由三部分构成——切向加速度的横向分量、车辆轨迹曲率贡献的横向分量离心效应、参考线自身弯曲带来的修正。s_ddot的推导稍微长一点因为分母带着(1 - κ_r · d)这个变量。令A 1 - κ_r · d则A_dot -κ_r · s_dot · d - κ_r · d_dot对s_dot v · cos(Δθ) / A求导整理后得到s_ddot [a · cos(Δθ) - v · sin(Δθ) · (v · κ - κ_r · s_dot)] / (1 - κ_r · d) s_dot · (κ_r · s_dot · d κ_r · d_dot) / (1 - κ_r · d)看着复杂结构上其实就是“分子项”加上“分母变化率修正项”。实际编码时我建议先算几个中间变量Δθ、A、Δθ_dot、A_dot再组合别把整个长公式一次性塞进代码否则出符号错误很难定位。2.4 正向转换的数值算法流程把上面推导汇总成一条可直接落地的流程输入Cartesian状态x, y, v, θ_x, κ, a通过投影算法求参考线上的匹配点得到s并读取参考线在该点的θ_r、κ_r、κ_r计算Δθ θ_x - θ_r(s)注意用角度归一化把差值压到[-π, π]计算A 1 - κ_r · d按公式依次计算d_dot、s_dot、d_ddot、s_ddot输出Frenet状态伪代码示意def cartesian_to_frenet(x, y, v, theta_x, kappa_cart, a_cart, ref_spline): # 1. 求投影点 s solve_projection(ref_spline, x, y) theta_r ref_spline.theta(s) kappa_r ref_spline.kappa(s) kappa_r_prime ref_spline.kappa_prime(s) x_r, y_r ref_spline.position(s) n_x, n_y ref_spline.normal(s) # 2. 横向偏移 d (x - x_r) * n_x (y - y_r) * n_y # 3. 角度差归一化 d_theta normalize_angle(theta_x - theta_r) # 4. 一阶量 d_dot v * sin(d_theta) s_dot v * cos(d_theta) / (1.0 - kappa_r * d) # 5. 二阶量 d_theta_dot v * kappa_cart - kappa_r * s_dot d_ddot a_cart * sin(d_theta) v * cos(d_theta) * d_theta_dot A 1.0 - kappa_r * d A_dot -kappa_r_prime * s_dot * d - kappa_r * d_dot s_ddot (a_cart * cos(d_theta) - v * sin(d_theta) * d_theta_dot) / A \ - v * cos(d_theta) * A_dot / (A * A) return s, d, s_dot, d_dot, s_ddot, d_ddot这里的v * cos(d_theta) * A_dot / (A * A)就是我前面合并整理后的第二种写法和长公式等价。哪种顺手用哪种关键是A_dot别算错。3. 反向推导Frenet坐标回到Cartesian坐标3.1 位置合成与速度/航向反解反方向的核心其实是把正向推导的公式“倒着用”。已知Frenet状态(s, d, s_dot, d_dot)要求Cartesian的位置、速度大小和航向角。位置直接合成就好p r(s) d · n_r(s)速度矢量在参考线切向和法向的分量从正向推导的公式里已经知道了u (1 - κ_r · d) · s_dot w d_dotu可以理解为速度沿参考线切向的等效分量w是横向分量。于是速度大小和航向角分别是v sqrt(u² w²) Δθ atan2(w, u) θ_x θ_r(s) Δθ这里最容易犯的错是用s_dot直接当速度大小、用θ_r当航向角。只有在车辆完全平行于参考线且d_dot 0时才能这样近似。实际跟踪工况下车辆航向与参考线总有偏差d_dot不为零直接忽略会让控制模块对横向偏差的反应滞后一个相位。3.2 二阶状态的反解方法Frenet反向转换能不能还原出加速度和车辆轨迹曲率答案是可以但需要联立两个二阶方程。把正向推导的两个二阶公式重新摆出来d_ddot a · sin(Δθ) v · cos(Δθ) · (v · κ - κ_r · s_dot)s_ddot · (1 - κ_r · d) a · cos(Δθ) - v · sin(Δθ) · (v · κ - κ_r · s_dot) s_dot · (κ_r · s_dot · d κ_r · d_dot)把a和v·κ看成两个未知数两个方程线性方程组可以直接解。令φ v · κ - κ_r · s_dot再令extra s_dot · (κ_r · s_dot · d κ_r · d_dot)则有a d_ddot · sin(Δθ) [s_ddot · (1 - κ_r · d) - extra] · cos(Δθ)φ [d_ddot - a · sin(Δθ)] / (v · cos(Δθ))理论上这样就把加速度还原出来了。但实际工程中除非下游模块硬性需要二阶量否则不建议在这里绕一圈。原因有两点一是Frenet轨迹本身通常就是规划模块生成的Cartesian下的加速度可以直接在生成时同时算出二是这个反解对数值误差很敏感尤其是Δθ接近正负90度时cos(Δθ)出现在分母上一个小噪声会被放大很多倍。3.3 用圆弧工况验证整套公式推导完公式强烈建议先做一轮解析验证再上真实路数据。最优验证场景就是圆弧参考线因为所有量都能手算。假设参考线是逆时针圆周半径R 10m车辆沿参考线内侧距离d 1m的平行圆弧行驶车速v 10m/s航向与参考线完全一致即Δθ 0。车辆实际轨迹半径是R - d 9m车辆圆周角速度是v / (R - d) 10 / 9 1.111 rad/s参考线上的s变化率s_dot R · 角速度 10 · 1.111 11.11 m/s用公式验证κ_r 1/R 0.11 - κ_r · d 1 - 0.1 · 1 0.9s_dot v / 0.9 11.11完全一致。d_dot v · sin(0) 0也正确。这个验证方法非常便宜却能一击命中所有符号层面的错误。我每次在新的算法框架里重新实现坐标转换都会先用这个工况跑一遍相当于给整套公式上个保险。4. 实现中的避坑事项从离散参考线到投影点稳定性4.1 参考线的参数化与曲率计算实际工程里参考线往往是离散的路径点不是现成的连续曲线。直接在这些点上算切向量和曲率如果点间距不均匀或者存在小幅抖动数值噪声会被放大得非常明显。我现在的标准做法是对离散点做三次样条插值让s方向单调递增然后基于样条函数解析求一阶导、曲率和曲率导数。三次样条的连续性能够保证切向量连续曲率虽然可能还是带噪声但至少不会出现锯齿状的导数跳变。曲率的计算统一走解析方式不要用相邻点求三点外接圆那种近似。三点法在点距均匀时勉强能用一旦遇到弯道或急刹造成的点距不均曲率就会像心电图一样乱跳。用样条参数方程有现成的曲率公式κ_r(s) |x(s) · y(s) - y(s) · x(s)| / (x(s)² y(s)²)^(3/2)但注意这个公式给的是绝对值换算成有符号曲率时还需要乘以一个方向标志。4.2 投影点计算的稳定性投影点求解本质上是优化问题min_s || p - r(s) ||²求解方法看参考线形式样条参考线高斯牛顿迭代初值用上一帧的s或者最近离散点索引单纯离散点先暴力找最近点再在最近点附近做抛物线插值或样条细化最容易出问题的是道路分岔和急弯场景。车在匝道出口附近时最近离散点可能落在主路分支等车辆真正驶入匝道后s突然跳一大段d同时跳变控制模块会因为这个跳变猛打方向盘。我的经验是加一个s连续性约束以上一帧s为初值在邻域窗口内搜索投影点仅在当前投影点的距离平方显著小于上一帧投影结果时才允许跳变。这个“显著小于”的阈值不需要很复杂试几次就能调到一个在直路、弯道、分岔三种场景下都能稳住的值。4.3 边界情况与数值病态处理有几个场景需要防御性编程第一分母1 - κ_r · d接近零的情况。这表示车辆在参考线弯道内侧极近的位置Frenet映射本身开始退化。现实中车道内d有限很少真碰到但在泊车或狭窄空间规划时可能出现。建议设一个下限保护比如当A的绝对值小于0.1时强制取0.1并打日志告警。第二Δθ接近正负90度。此时cos(Δθ)接近零s_dot会变得极小甚至为零而d_dot达到极值。公式本身没有奇异但二阶反解在附近会不稳定。正向转换没问题反向还原加速度时要特别小心。第三闭环参考线的s回绕。如果参考线是赛道或园区环线s到达终点后会跳回起点。所有状态量都要做模长修正s_dot、d_dot这类量不需要修但s本身如果不加周期处理投影算法会在终点附近给出一个虚假的远距离投影点。5. 常见问题速查与我的排查经验放一个我自己总结的排查表都是实际调试中出现过的场景现象可能原因排查思路d_dot符号反了法向量方向约定不一致在参考线某点打印t_r和n_r坐标手算验证是否左法s_dot在直道上忽大忽小参考线点数太稀κ_r噪声大换样条插值检查曲率曲线是否平滑弯道里规划轨迹震荡投影点s抖动d阶跃加s连续性约束检查投影算法收敛精度Frenet转Cartesian后航向角不对忘记加θ_r(s)或Δθ象限处理错误先平行行驶工况验证确认Δθ 0时输出正确正反转换来回几次状态漂移参考线曲率半符号不一致统一用有符号曲率不要在某一步偷偷取绝对值最后分享一个算半个技巧的经验我刚做轨迹重规划模块时为了快速验证转换代码没有直接上仿真平台而是先在本地生成一段带噪声的参考线然后让一个虚构车辆沿参考线做正弦横向摆动把Cartesian轨迹存下来再转成Frenet、转回Cartesian对比轨迹还原误差。把全部换算误差控制在毫米级之后才把这个工具函数合并进规划模块。这个验证流程几乎零成本却能提前暴露绝大多数字号、投影、插值层面的低级问题。各位如果在实现中遇到诡异现象建议先跑一遍这个闭环检查比对着屏幕看控制震荡曲线有效得多。