笛卡尔坐标 → 自然坐标（Frenet） 逐步推导全解 -5

用到的工具：商法则 · 记号

s˙=vxcos⁡Δθ1−krd=uw,u:=vxcos⁡Δθ,w:=1−krd \dot s=\frac{v_x\cos\Delta\theta}{1-k_r d}=\frac{u}{w},\qquad u:=v_x\cos\Delta\theta,\quad w:=1-k_r d s˙=1−krdvxcosΔθ=wu,u:=vxcosΔθ,w:=1−krd

s¨=ddt(uw)=u˙ w−u w˙w2=u˙w−uw⋅w˙w \ddot s=\frac{d}{dt}\left(\frac{u}{w}\right)=\frac{\dot u\,w-u\,\dot w}{w^2}=\frac{\dot u}{w}-\frac{u}{w}\cdot\frac{\dot w}{w} s¨=dtd(wu)=w2u˙w−uw˙=wu˙−wu⋅ww˙

STEP 10.2 求 u̇：乘积法则 + 链式法则

用到的工具：乘积法则 · a_x=v̇_x

u˙=ddt(vxcos⁡Δθ)=v˙xcos⁡Δθ+vxddtcos⁡Δθ \dot u=\frac{d}{dt}\big(v_x\cos\Delta\theta\big) =\dot v_x\cos\Delta\theta+v_x\frac{d}{dt}\cos\Delta\theta u˙=dtd(vxcosΔθ)=v˙xcosΔθ+vxdtdcosΔθ

ddtcos⁡Δθ=−sin⁡Δθ⋅Δθ˙ \frac{d}{dt}\cos\Delta\theta=-\sin\Delta\theta\cdot\dot{\Delta\theta} dtdcosΔθ=−sinΔθ⋅Δθ˙

结果

u˙=axcos⁡Δθ−vxsin⁡Δθ Δθ˙ \dot u=a_x\cos\Delta\theta-v_x\sin\Delta\theta\,\dot{\Delta\theta} u˙=axcosΔθ−vxsinΔθΔθ˙

这里 ax=v˙xa_x=\dot v_xax=v˙x 是已知的切向加速度；Δθ˙\dot{\Delta\theta}Δθ˙ 由 STEP 08.4 给出。

STEP 10.3 求 ẇ：k_r 是 s 的函数，所以 k̇_r=k_r′ṡ

用到的工具：乘积法则 · 链式法则

w˙=ddt(1−krd)=−ddt(krd)=−(k˙r d+krd˙) \dot w=\frac{d}{dt}(1-k_r d)=-\frac{d}{dt}(k_r d)=-\big(\dot k_r\,d+k_r\dot d\big) w˙=dtd(1−krd)=−dtd(krd)=−(k˙rd+krd˙)

其中 k˙r=dkrdss˙=kr′s˙\dot k_r=\dfrac{dk_r}{ds}\dot s=k_r'\dot sk˙r=dsdkrs˙=kr′s˙，d˙=s˙d′\dot d=\dot s d'd˙=s˙d′。代入：

w˙=−(kr′s˙ d+krs˙ d′)=−s˙(kr′d+krd′) \dot w=-\big(k_r'\dot s\,d+k_r\dot s\,d'\big)=-\dot s\big(k_r'd+k_r d'\big) w˙=−(kr′s˙d+krs˙d′)=−s˙(kr′d+krd′)

结果

w˙=−s˙ (kr′d+krd′) \dot w=-\dot s\,(k_r'd+k_r d') w˙=−s˙(kr′d+krd′)

注意这里同时用到了两个「撇」：kr′k_r'kr′ 是曲率的变化率，d′d'd′ 是横向斜率。它们是不同的量，不能合并。

STEP 10.4 先写出「不化简」的完整式

用到的工具：代入

s¨=axcos⁡Δθ−vxsin⁡Δθ Δθ˙(1−krd)−vxcos⁡Δθ⋅−s˙(kr′d+krd′)(1−krd)2 \ddot s=\frac{\biga_x\\cos\\Delta\\theta-v_x\\sin\\Delta\\theta\\,\\dot{\\Delta\\theta}\\big(1-k_r d)-v_x\cos\Delta\theta\cdot\big-\\dot s(k_r'd+k_r d')\\big}{(1-k_r d)^2} s¨=(1−krd)2axcosΔθ−vxsinΔθΔθ˙(1−krd)−vxcosΔθ⋅−s˙(kr′d+krd′)

把负号翻上来：

s¨=axcos⁡Δθ−vxsin⁡Δθ Δθ˙w+vxcos⁡Δθ s˙ (kr′d+krd′)w2 \ddot s=\frac{\biga_x\\cos\\Delta\\theta-v_x\\sin\\Delta\\theta\\,\\dot{\\Delta\\theta}\\bigw+v_x\cos\Delta\theta\,\dot s\,(k_r'd+k_r d')}{w^2} s¨=w2axcosΔθ−vxsinΔθΔθ˙w+vxcosΔθs˙(kr′d+krd′)

这个式子已经可以直接写进代码（只是多算几个乘法）。要化成标准形式，继续做三步消元。

STEP 10.5 消元三件套

用到的工具：一阶关系 · 代数替换

① 把 vxsin⁡Δθv_x\sin\Delta\thetavxsinΔθ 换成 s˙d′\dot s d's˙d′（来自 STEP 07.3 的切向方程）：

vxsin⁡Δθ=s˙ d′ v_x\sin\Delta\theta=\dot s\,d' vxsinΔθ=s˙d′

② 把 vxcos⁡Δθv_x\cos\Delta\thetavxcosΔθ 换成 s˙w\dot s ws˙w （来自 s˙=vxcos⁡Δθ/w\dot s=v_x\cos\Delta\theta/ws˙=vxcosΔθ/w）：

vxcos⁡Δθ=s˙ w v_x\cos\Delta\theta=\dot s\,w vxcosΔθ=s˙w

③ 把 Δθ˙\dot{\Delta\theta}Δθ˙ 换成 Qs˙Q\dot sQs˙（STEP 08.4）：

Δθ˙=Q s˙,Q=kxwcos⁡Δθ−kr \dot{\Delta\theta}=Q\,\dot s,\qquad Q=k_x\frac{w}{\cos\Delta\theta}-k_r Δθ˙=Qs˙,Q=kxcosΔθw−kr

先处理分子中的「vxsin⁡Δθ Δθ˙v_x\sin\Delta\theta\,\dot{\Delta\theta}vxsinΔθΔθ˙」这一项：

vxsin⁡Δθ Δθ˙=(s˙d′)(Qs˙)=s˙2d′Q v_x\sin\Delta\theta\,\dot{\Delta\theta}=(\dot s d')(Q\dot s)=\dot s^2d'Q vxsinΔθΔθ˙=(s˙d′)(Qs˙)=s˙2d′Q

再处理「vxcos⁡Δθ s˙(kr′d+krd′)v_x\cos\Delta\theta\,\dot s(k_r'd+k_rd')vxcosΔθs˙(kr′d+krd′)」：

vxcos⁡Δθ s˙(kr′d+krd′)=(s˙w)s˙(kr′d+krd′)=s˙2w(kr′d+krd′) v_x\cos\Delta\theta\,\dot s(k_r'd+k_rd')=(\dot s w)\dot s(k_r'd+k_rd')=\dot s^2w(k_r'd+k_rd') vxcosΔθs˙(kr′d+krd′)=(s˙w)s˙(kr′d+krd′)=s˙2w(kr′d+krd′)

把这两块代回 STEP 10.4 的分子：

分子=axcos⁡Δθ−vxsin⁡ΔθΔθ˙w+s˙2w(kr′d+krd′) \text{分子}=\biga_x\\cos\\Delta\\theta-v_x\\sin\\Delta\\theta\\dot{\\Delta\\theta}\\bigw+\dot s^2w(k_r'd+k_rd') 分子=axcosΔθ−vxsinΔθΔθ˙w+s˙2w(kr′d+krd′)

=axwcos⁡Δθ−s˙2d′Q w+s˙2w(kr′d+krd′) =a_xw\cos\Delta\theta-\dot s^2d'Q\,w+\dot s^2w(k_r'd+k_rd') =axwcosΔθ−s˙2d′Qw+s˙2w(kr′d+krd′)

=axwcos⁡Δθ−s˙2wd′Q−(kr′d+krd′) =a_xw\cos\Delta\theta-\dot s^2w\Bigd'Q-(k_r'd+k_rd')\\Big =axwcosΔθ−s˙2wd′Q−(kr′d+krd′)

STEP 10.6 除以 w²，得到最终式

用到的工具：约分

s¨=axwcos⁡Δθ−s˙2wd′Q−(kr′d+krd′)w2=axcos⁡Δθ−s˙2d′Q−(kr′d+krd′)w \ddot s=\frac{a_xw\cos\Delta\theta-\dot s^2w\bigd'Q-(k_r'd+k_rd')\\big}{w^2} =\frac{a_x\cos\Delta\theta-\dot s^2\bigd'Q-(k_r'd+k_rd')\\big}{w} s¨=w2axwcosΔθ−s˙2wd′Q−(kr′d+krd′)=waxcosΔθ−s˙2d′Q−(kr′d+krd′)

结论｜s̈（标准形式，与 PDF 第 57 页一致）

  s¨=axcos⁡Δθ−s˙2d′(kx1−krdcos⁡Δθ−kr)−(kr′d+krd′)1−krd   \boxed{\;\ddot s=\frac{a_x\cos\Delta\theta-\dot s^2\Bigd'\\Big(k_x\\dfrac{1-k_r d}{\\cos\\Delta\\theta}-k_r\\Big)-(k_r'd+k_r d')\\Big}{1-k_r d}\;} s¨=1−krdaxcosΔθ−s˙2d′(kxcosΔθ1−krd−kr)−(kr′d+krd′)

把 Q 展开后的「全展开版」：

s¨=axcos⁡Δθ−s˙2kxd′(1−krd)cos⁡Δθ−krd′−kr′d−krd′1−krd \ddot s=\frac{a_x\cos\Delta\theta-\dot s^2\left\\dfrac{k_x d'(1-k_r d)}{\\cos\\Delta\\theta}-k_r d'-k_r'd-k_r d'\\right}{1-k_r d} s¨=1−krdaxcosΔθ−s˙2cosΔθkxd′(1−krd)−krd′−kr′d−krd′

=axcos⁡Δθ−s˙2kxd′(1−krd)cos⁡Δθ−kr′d−2krd′1−krd =\frac{a_x\cos\Delta\theta-\dot s^2\left\\dfrac{k_x d'(1-k_r d)}{\\cos\\Delta\\theta}-k_r'd-2k_r d'\\right}{1-k_r d} =1−krdaxcosΔθ−s˙2cosΔθkxd′(1−krd)−kr′d−2krd′

最后这个「合并同类项」的形式更能看出结构：第一项是切向加速度投影，第二项是三个曲率修正（车辆曲率、道路曲率变化、道路曲率与斜率的耦合）。

STEP 10.7 特例校验

用到的工具：极限 · 量纲

① 直线参考线 + 车头对正 + 贴线： kr=kr′=0, Δθ=0, d=0⇒w=1, d′=0, Q=kxk_r=k_r'=0,\ \Delta\theta=0,\ d=0\Rightarrow w=1,\ d'=0,\ Q=k_xkr=kr′=0, Δθ=0, d=0⇒w=1, d′=0, Q=kx：

s¨=ax⋅1−s˙20−(0+0)1=ax ✓ \ddot s=\frac{a_x\cdot1-\dot s^20-(0+0)}{1}=a_x\ \checkmark s¨=1ax⋅1−s˙20−(0+0)=ax ✓

② 匀速圆周、车顺着道路（Δθ=0, kx=kr, d′=0\Delta\theta=0,\ k_x=k_r,\ d'=0Δθ=0, kx=kr, d′=0）： s˙=vx, Q=0\dot s=v_x,\ Q=0s˙=vx, Q=0，得 s¨=ax\ddot s=a_xs¨=ax。合理：纵向速度不变时纵向加速度就是切向加速度。

③ 量纲： 分子两项都是 m/s2\mathrm{m/s^2}m/s2，分母 www 无量纲，结果单位 m/s2 ✓\mathrm{m/s^2}\ \checkmarkm/s2 ✓。括号内：d′Qd'Qd′Q 的量纲是 1/m1/\mathrm m1/m，kr′dk_r'dkr′d 也是 1/m1/\mathrm m1/m，可以相减 ✓\checkmark✓。

STEP 10.8 数值例子

用到的工具：代入

沿用 kr=0.02, kr′=0, d=1.2, vx=12, ax=0.5, Δθ=0.15, kx=0.01k_r=0.02,\ k_r'=0,\ d=1.2,\ v_x=12,\ a_x=0.5,\ \Delta\theta=0.15,\ k_x=0.01kr=0.02, kr′=0, d=1.2, vx=12, ax=0.5, Δθ=0.15, kx=0.01：

s˙=12.1570,w=0.976,d′=0.147508,Q=0.01×0.9760.988771−0.02=−0.010129 \dot s=12.1570,\quad w=0.976,\quad d'=0.147508,\quad Q=0.01\times\frac{0.976}{0.988771}-0.02=-0.010129 s˙=12.1570,w=0.976,d′=0.147508,Q=0.01×0.9887710.976−0.02=−0.010129

d′Q=0.147508×(−0.010129)=−0.0014942,kr′d+krd′=0+0.02×0.147508=0.0029502 d'Q=0.147508\times(-0.010129)=-0.0014942,\qquad k_r'd+k_rd'=0+0.02\times0.147508=0.0029502 d′Q=0.147508×(−0.010129)=−0.0014942,kr′d+krd′=0+0.02×0.147508=0.0029502

s¨=0.5×0.988771−147.793×(−0.0014942−0.0029502)0.976=0.49439+0.656830.976=1.1795 m/s2 \ddot s=\frac{0.5\times0.988771-147.793\times(-0.0014942-0.0029502)}{0.976} =\frac{0.49439+0.65683}{0.976}=1.1795\ \mathrm{m/s^2} s¨=0.9760.5×0.988771−147.793×(−0.0014942−0.0029502)=0.9760.49439+0.65683=1.1795 m/s2

可以看到 s¨=1.18\ddot s=1.18s¨=1.18 明显大于 ax=0.5a_x=0.5ax=0.5：车在弯道内侧加速时，纵向坐标的加速度会被几何放大。这是正常的坐标效应，不是数值错误。

11 二阶：d̈

从 d˙=vxsin⁡Δθ\dot d=v_x\sin\Delta\thetad˙=vxsinΔθ 出发求一次导。这一步最容易「漏项」，因为乘积法则会生出两项。

STEP 11.1 乘积法则展开

用到的工具：乘积法则 · 链式法则

d¨=ddt(vxsin⁡Δθ)=v˙xsin⁡Δθ+vxddtsin⁡Δθ \ddot d=\frac{d}{dt}\big(v_x\sin\Delta\theta\big) =\dot v_x\sin\Delta\theta+v_x\frac{d}{dt}\sin\Delta\theta d¨=dtd(vxsinΔθ)=v˙xsinΔθ+vxdtdsinΔθ

ddtsin⁡Δθ=cos⁡Δθ⋅Δθ˙ \frac{d}{dt}\sin\Delta\theta=\cos\Delta\theta\cdot\dot{\Delta\theta} dtdsinΔθ=cosΔθ⋅Δθ˙

中间结果

d¨=axsin⁡Δθ+vxcos⁡Δθ Δθ˙ \ddot d=a_x\sin\Delta\theta+v_x\cos\Delta\theta\,\dot{\Delta\theta} d¨=axsinΔθ+vxcosΔθΔθ˙

STEP 11.2 代入航向差角速度

用到的工具：STEP 08.4

Δθ˙=kxvx−krs˙ \dot{\Delta\theta}=k_xv_x-k_r\dot s Δθ˙=kxvx−krs˙

结论 1｜精确式

d¨=axsin⁡Δθ+vxcos⁡Δθ (kxvx−krs˙) \boxed{\ddot d=a_x\sin\Delta\theta+v_x\cos\Delta\theta\,\big(k_xv_x-k_r\dot s\big)} d¨=axsinΔθ+vxcosΔθ(kxvx−krs˙)

后一项的物理意义是向心加速度 ：vx2kxv_x^2k_xvx2kx 是车辆绕自身曲率中心转的向心项，vxkrs˙v_xk_r\dot svxkrs˙ 是参考线弯曲带来的「坐标旋转」项，两者相减。

STEP 11.3 化成只含 Frenet 量的形式

用到的工具：消元 · Q 记号

用 vxcos⁡Δθ=s˙ wv_x\cos\Delta\theta=\dot s\,wvxcosΔθ=s˙w 与 Δθ˙=Qs˙\dot{\Delta\theta}=Q\dot sΔθ˙=Qs˙：

d¨=axsin⁡Δθ+s˙ w⋅Qs˙=axsin⁡Δθ+s˙2 w Q \ddot d=a_x\sin\Delta\theta+\dot s\,w\cdot Q\dot s =a_x\sin\Delta\theta+\dot s^2\,w\,Q d¨=axsinΔθ+s˙w⋅Qs˙=axsinΔθ+s˙2wQ

结论 2｜紧凑式

d¨=axsin⁡Δθ+s˙2(1−krd)(kx1−krdcos⁡Δθ−kr) \boxed{\ddot d=a_x\sin\Delta\theta+\dot s^2(1-k_r d)\left(k_x\frac{1-k_r d}{\cos\Delta\theta}-k_r\right)} d¨=axsinΔθ+s˙2(1−krd)(kxcosΔθ1−krd−kr)

把 QQQ 展开：

d¨=axsin⁡Δθ+s˙2(kx(1−krd)2cos⁡Δθ−kr(1−krd)) \ddot d=a_x\sin\Delta\theta+\dot s^2\left(k_x\frac{(1-k_r d)^2}{\cos\Delta\theta}-k_r(1-k_r d)\right) d¨=axsinΔθ+s˙2(kxcosΔθ(1−krd)2−kr(1−krd))

量纲检查：s˙2(1−krd)⋅Q→(m/s)2⋅1⋅(1/m)=m/s2 ✓\dot s^2(1-k_rd)\cdot Q\to(\mathrm{m/s})^2\cdot1\cdot(1/\mathrm m)=\mathrm{m/s^2}\ \checkmarks˙2(1−krd)⋅Q→(m/s)2⋅1⋅(1/m)=m/s2 ✓。第一项 axsin⁡Δθa_x\sin\Delta\thetaaxsinΔθ 也是 m/s2 ✓\mathrm{m/s^2}\ \checkmarkm/s2 ✓。

STEP 11.4 与 d″ 的关系（互相换算）

用到的工具：代数反解

由 STEP 09.4：d′′=−(kr′d+krd′)tan⁡Δθ+wcos⁡2ΔθQd''=-(k_r'd+k_rd')\tan\Delta\theta+\dfrac{w}{\cos^2\Delta\theta}Qd′′=−(kr′d+krd′)tanΔθ+cos2ΔθwQ，解出 wQwQwQ：

wQ=cos⁡2Δθd′′+(kr′d+krd′)tan⁡Δθ wQ=\cos^2\Delta\theta\Bigd''+(k_r'd+k_rd')\\tan\\Delta\\theta\\Big wQ=cos2Δθd′′+(kr′d+krd′)tanΔθ

代入 d¨=axsin⁡Δθ+s˙2wQ\ddot d=a_x\sin\Delta\theta+\dot s^2wQd¨=axsinΔθ+s˙2wQ：

结论 3

d¨=axsin⁡Δθ+s˙2cos⁡2Δθd′′+(kr′d+krd′)tan⁡Δθ \ddot d=a_x\sin\Delta\theta+\dot s^2\cos^2\Delta\theta\Bigd''+(k_r'd+k_rd')\\tan\\Delta\\theta\\Big d¨=axsinΔθ+s˙2cos2Δθd′′+(kr′d+krd′)tanΔθ

反过来（已知加速度求 d′′d''d′′）：

d′′=d¨−axsin⁡Δθs˙2cos⁡2Δθ−(kr′d+krd′)tan⁡Δθ d''=\frac{\ddot d-a_x\sin\Delta\theta}{\dot s^2\cos^2\Delta\theta}-(k_r'd+k_rd')\tan\Delta\theta d′′=s˙2cos2Δθd¨−axsinΔθ−(kr′d+krd′)tanΔθ

STEP 11.5 为什么很多资料只写 d̈ = a_x·sin Δθ

用到的工具：误差估计 · 适用条件

被略去的是 vxcos⁡Δθ Δθ˙v_x\cos\Delta\theta\,\dot{\Delta\theta}vxcosΔθΔθ˙。它的量级：

∣vxcos⁡Δθ Δθ˙∣≤vx∣kxvx−krs˙∣≈vx2∣kx−kr∣ \big|v_x\cos\Delta\theta\,\dot{\Delta\theta}\big|\le v_x\big|k_xv_x-k_r\dot s\big|\approx v_x^2|k_x-k_r| vxcosΔθΔθ˙ ≤vx kxvx−krs˙ ≈vx2∣kx−kr∣

以 vx=20 m/sv_x=20\ \mathrm{m/s}vx=20 m/s、∣kx−kr∣=0.01 1/m|k_x-k_r|=0.01\ \mathrm{1/m}∣kx−kr∣=0.01 1/m 计，被略去的项可达 4 m/s24\ \mathrm{m/s^2}4 m/s2------远超舒适性阈值（通常 ±1.5 m/s2\pm1.5\ \mathrm{m/s^2}±1.5 m/s2）。所以：

• 只做低速几何检查 （如停车、蠕行）时，可以近似 d¨≈axsin⁡Δθ\ddot d\approx a_x\sin\Delta\thetad¨≈axsinΔθ；
• 做动力学约束、舒适性评价、MPC 预测时，必须用完整式。

常见错误： 错误示范： 把 d¨=axsin⁡Δθ\ddot d=a_x\sin\Delta\thetad¨=axsinΔθ 当成「Frenet 横向加速度」的严格定义，然后拿它去约束横向 jerk。这样算出来的值在弯道会系统性偏小，导致规划出的轨迹实际横向加速度超限。

STEP 11.6 数值例子

用到的工具：代入

d¨=0.5×0.149438+12.1572×0.976×(−0.010129) \ddot d=0.5\times0.149438+12.157^2\times0.976\times(-0.010129) d¨=0.5×0.149438+12.1572×0.976×(−0.010129)

=0.074719−1.46100=−1.3863 m/s2 =0.074719-1.46100=-1.3863\ \mathrm{m/s^2} =0.074719−1.46100=−1.3863 m/s2

校验另一条路：axsin⁡Δθ+vxcos⁡Δθ(kxvx−krs˙)a_x\sin\Delta\theta+v_x\cos\Delta\theta(k_xv_x-k_r\dot s)axsinΔθ+vxcosΔθ(kxvx−krs˙)

=0.074719+12×0.988771×(0.12−0.24314)=0.074719−1.46100=−1.3863 ✓ =0.074719+12\times0.988771\times(0.12-0.24314)=0.074719-1.46100=-1.3863\ \checkmark =0.074719+12×0.988771×(0.12−0.24314)=0.074719−1.46100=−1.3863 ✓

注意这里 d¨\ddot dd¨ 为负：车虽然在参考线左侧（d>0d>0d>0），但航向正在向右修正（Δθ\Delta\thetaΔθ 减小），横向加速度指向右侧。