【自动驾驶】决策规划算法 | 数学基础（三）直角坐标与自然坐标转换Ⅰ

写在前面：

🌟 欢迎光临 清流君 的博客小天地，这里是我分享技术与心得的温馨角落 。📝
个人主页 ：清流君_CSDN博客，期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作，荣幸在CSDN首发🐒
若您觉得内容有价值，还请评论告知一声，以便更多人受益。
转载请注明出处，尊重原创，从我做起。

👍 点赞、评论、收藏 ，三连走一波，让我们一起养成好习惯😜
在这里，您将收获的不只是技术干货 ，还有思维的火花！

📚 系列专栏：【决策规划】系列，带您深入浅出，领略控制之美。🖊
愿我的分享能为您带来启迪，如有不足，敬请指正，让我们共同学习，交流进步！

🎭 人生如戏，我们并非能选择舞台和剧本，但我们可以选择如何演绎 🌟
感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行~~~

文章目录

引言
一、龙格现象与多项式拟合
- [1.1 龙格现象概述](#1.1 龙格现象概述)
- [1.2 高次多项式拟合的弊端](#1.2 高次多项式拟合的弊端)
[二、Frenet 坐标系的作用](#二、Frenet 坐标系的作用)
- [2.1 自然坐标系的优势](#2.1 自然坐标系的优势)
- [2.2 坐标转换涉及的变量及其关系](#2.2 坐标转换涉及的变量及其关系)
三、曲线坐标系的特点
- [3.1 曲线坐标系与直角坐标系的主要区别](#3.1 曲线坐标系与直角坐标系的主要区别)
- [3.2 基向量非常数对向量求导的影响](#3.2 基向量非常数对向量求导的影响)
- [3.3 直角坐标系与自然坐标系中位移描述的差异](#3.3 直角坐标系与自然坐标系中位移描述的差异)
- [3.4 实例分析：车辆轨迹与道路几何中的坐标位移差异](#3.4 实例分析：车辆轨迹与道路几何中的坐标位移差异)
四、预备知识1：质点速度的向量表达式与道路几何投影导数推导
- [4.1 质点速度的向量表达式](#4.1 质点速度的向量表达式)
- [4.2 拓展：质点在道路几何上的投影位矢导数推导](#4.2 拓展：质点在道路几何上的投影位矢导数推导)
- [4.3 质点投影点的定义](#4.3 质点投影点的定义)
五、预备知识2：Frenet公式
- [5.1 曲线坐标系基向量特性](#5.1 曲线坐标系基向量特性)
- [5.2 Frenet公式及其证明](#5.2 Frenet公式及其证明)
- [5.3 拓展1：质点轨迹与道路几何的方向导数](#5.3 拓展1：质点轨迹与道路几何的方向导数)
- [5.4 拓展2：切向加速度与法向加速度的分解](#5.4 拓展2：切向加速度与法向加速度的分解)
六、预备知识总结
- [6.1 变量含义](#6.1 变量含义)
- [6.2 辅助公式](#6.2 辅助公式)
七、坐标转换算法
- [7.1 问题描述](#7.1 问题描述)
- [7.2 坐标转换算法步骤概述](#7.2 坐标转换算法步骤概述)
- [7.3 核心公式及其应用](#7.3 核心公式及其应用)
- - [(1) 计算弧长](#(1) 计算弧长)
  - [(2) 计算弧速度](#(2) 计算弧速度)
  - [(3) 计算弧长的时间导数](#(3) 计算弧长的时间导数)
  - [(4) 计算弧长的弧坐标导数](#(4) 计算弧长的弧坐标导数)
  - [(5) 计算弧加速度](#(5) 计算弧加速度)
  - [(6) 计算弧长的二阶时间导数](#(6) 计算弧长的二阶时间导数)
  - [(7) 计算弧长的二阶弧坐标导数](#(7) 计算弧长的二阶弧坐标导数)
八、总结
参考资料

引言

各位小伙伴们大家好，本篇博客是自动驾驶决策规划算法数学基础的第三节第Ⅰ部分，内容整理自 B站知名up主 忠厚老实的老王 的视频，作为博主的学习笔记，分享给大家共同学习。

本篇博客讲数学基础部分中 Frenet 坐标系和笛卡坐标系之间的坐标转换，即直角坐标和自然坐标的转换。

本节内容如果只应用，难度其实还好，但如果想真彻底理解它是怎么来，难度非常高。需要非常熟悉微积分以及向量微积分。

一、龙格现象与多项式拟合

1.1 龙格现象概述

讲解坐标转换之前，首先讲一下龙格现象，是数值分析里的知识，用高次多项式拟合点，可能出现震荡现象，所以要慎用高次多项式。

对于较多点的拟合，尽可能用分段低次多项式拟合，而不用高次多项式拟合。

1.2 高次多项式拟合的弊端

一般认为如果多项式次数越高，拟合精度越好，这是想当然的想法，在有些问题确实是这样，比如拟合 sin ⁡ x , cos ⁡ x \sin x,\cos x sinx,cosx 这样的三角函数，次数越高拟合越精确。

但在有些情况并不是这样，比如在拟合 y = 25 x 2 + 1 e y=25x^2+\frac{1}{e} y=25x2+e1 时，用高次多项式拟合会出现震荡。

在拟合时尽可能不用高次多项式，高次多项式很可能出现震荡现象，所以一般对大量点进行拟合时，用分段一次多样式拟合，而不用高次多项式。

二、Frenet 坐标系的作用

2.1 自然坐标系的优势

为什么要把笛卡尔坐标转化为自然坐标？

因为实际上道路它都是千变万化的，有不同的曲率和长短。如果只在直角坐标下分析问题，就意味着不同的道路形状、不同道路的长短都要分开考虑，非常麻烦。

如果以道路中心线为曲线坐标系建立自然坐标，只需要关注怎么进行坐标转换可以，只要把直角坐标转换为自然坐标，无论是什么道路、什么路况，都可以用同一套方法解决问题，也就是只要做把直角坐标转化为自然坐标，解决问题后，再把自然坐标转化为直角坐标。极大简化分析问题的难度。不同道路都可以转化成同样坐标系分析，再加坐标转换就可以了。

关于转换公式，可以参考以下博客：

Frenet坐标系与Cartesian坐标系互转（一）：公式推导

上面的博客给出了笛卡尔坐标转Frenet坐标和Frenet坐标转笛卡尔坐标，直接会用公式就可以了。

下面使用向量法推导，可降低推导难度，不用上面博客的推导方法也能得出一样的结果。

2.2 坐标转换涉及的变量及其关系

首先建立直角坐标系：

第一条曲线是车辆轨迹，第二条曲线是道路几何，在车辆轨迹曲线取一点，有速度 v v v，加速度 a a a，位置矢量 r ⃗ \vec r r ，以及在直角坐标系下的曲率 κ \kappa κ。

无人驾驶车一般用 Host Vehicle 表示，已知车在笛卡尔标系下的 r ⃗ h , v ⃗ h , a ⃗ h , κ h \vec r_h,\vec v_h,\vec a_h,\kappa_h r h,v h,a h,κh，求车在以道路为坐标轴的Frenet坐标系下的坐标 s , s ˙ , s ¨ , l , l ′ , l ′ ′ , l ˙ , l ¨ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime},\dot l,\ddot l s,s˙,s¨,l,l′,l′′,l˙,l¨，其中 s ˙ = d s d t , l ˙ = d l d t , l ˙ = d l d s \dot{s}=\frac{ds}{dt},\dot{l}=\frac{dl}{dt},\dot{l}=\frac{dl}{ds} s˙=dtds,l˙=dtdl,l˙=dsdl。

共有 8 8 8 个坐标要求，但一般实际上用不到这 8 8 8 个变量。一般只用 6 6 6 个：

s , s ˙ , s ¨ , l , l ˙ , l ¨ s,\dot{s},\ddot{s},l,\dot l,\ddot l s,s˙,s¨,l,l˙,l¨
s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′

到底用哪 6 6 6 个变量，取决于规划方法是什么：

对于 EM Planner ，求 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′
对于 Lattice Planner，求 s , s ˙ , s ¨ , l , l ˙ , l ¨ s,\dot{s},\ddot{s},l,\dot l,\ddot l s,s˙,s¨,l,l˙,l¨

虽然变量有 8 8 8 个，但独立变量只有 6 6 6 个，因为 l ′ , l ′ ′ l^{\prime},l^{\prime\prime} l′,l′′ 和 l ˙ , l ¨ \dot l,\ddot l l˙,l¨ 可以互相转化：
l ˙ = d l d t = d l d s ⋅ d s d t = l ′ s ˙ \dot{l}=\frac{dl}{dt}=\frac{dl}{ds}\cdot \frac{ds}{dt}=l'\dot{s} l˙=dtdl=dsdl⋅dtds=l′s˙ l ¨ = d l ˙ d t = d ( l ′ s ˙ ) d t = d l ′ d t s ˙ + l ′ ⋅ d s ˙ d t = d l ′ d s d s d t s ˙ + l ′ d s ˙ d t = l ′ ′ s ˙ 2 + l ′ s ¨ \begin{aligned} \ddot{l}&=\frac{\text{d}\dot{l}}{\text{d}t}=\frac{\text{d}\left( l'\dot{s} \right)}{\text{d}t}=\frac{\text{d}l'}{\text{d}t}\dot{s}+l'\cdot \frac{\text{d}\dot{s}}{\text{d}t}\\ &=\frac{\text{d}l'}{\text{d}s}\frac{\text{d}s}{\text{d}t}\dot{s}+l'\frac{\text{d}\dot{s}}{\text{d}t}=l''\dot{s}^2+l'\ddot{s}\\ \end{aligned} l¨=dtdl˙=dtd(l′s˙)=dtdl′s˙+l′⋅dtds˙=dsdl′dtdss˙+l′dtds˙=l′′s˙2+l′s¨ EM planner 采用的是 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′。

三、曲线坐标系的特点

3.1 曲线坐标系与直角坐标系的主要区别

曲线坐标系与直角坐标系有两点不同：

曲线坐标系的基向量一般不是常向量
点的曲线坐标变化与点的实际位移一般不一致

在直角坐标下：

基向量是 i i i 和 j j j，显然是常向量 d i d x = 0 ⃗ \frac{di}{dx}=\vec 0 dxdi=0

但在曲线坐标系：

基向量为 τ ⃗ \vec \tau τ ， τ ⃗ \vec \tau τ 的大小它不变，但 τ ⃗ \vec \tau τ 的方向会随 s s s 变化而变化，所以 d τ d s ≠ 0 \frac{d\tau}{ds}\ne 0 dsdτ=0

3.2 基向量非常数对向量求导的影响

基向量对坐标的导数不为 0 0 0 有什么影响？

最大的影响就在于对向量求导，比如在直角坐标下，如果向量 v = v x i + v y j v=v_xi+v_yj v=vxi+vyj 对向量 v v v 求导，只要把坐标去掉就可以，变成
v ˙ = v ˙ x i + v ˙ y j \dot{v}=\dot{v}_xi+\dot{v}_yj v˙=v˙xi+v˙yj 但如果在曲线坐标系下， v ˙ = v ˙ x τ ⃗ + v ˙ y n ⃗ \dot{v}=\dot{v}_x\vec\tau+\dot{v}_y\vec n v˙=v˙xτ +v˙yn ，对向导求导不仅要对 v x , v y v_x,v_y vx,vy 求导，还要对 τ ⃗ , n ⃗ \vec \tau,\vec n τ ,n 求导，因为 τ ⃗ , n ⃗ \vec \tau,\vec n τ ,n 不是常向量，即向量导数不为 0 0 0，这是和直角坐标系相比最大的区别。

在曲线坐标系下，对向量求导一般比较复杂，不像在直角坐标下，只要对 v x , v y v_x,v_y vx,vy 求导就可以了。

3.3 直角坐标系与自然坐标系中位移描述的差异

另外，曲线坐标系描述起来比较复杂，举个例子，比如直角坐标系：

直角坐标系下有点 ( x , y ) (x,y) (x,y)，让 y y y 不动， x x x 移动 Δ x \Delta x Δx。在 y y y 不动的情况下，点的实际位移和坐标的位移一样，点移动了 Δ x \Delta x Δx 的距离，坐标也从 x x x 移到了 x + Δ x x+\Delta x x+Δx 也是 Δ x \Delta x Δx 的距离，在直角坐标系下，如果 y y y 不动， x x x 坐标的移动距离和点实际移动的距离一样，都是 Δ x \Delta x Δx。

但在自然坐标系：

同样点 ( s , l ) (s,l) (s,l)，让 l l l 不动， s s s 坐标移动 Δ s \Delta s Δs，点也会移动弧长 Δ s ′ \Delta s' Δs′。 Δ s \Delta s Δs 与 Δ s ′ \Delta s' Δs′ 一般不相等。

这一结论最反直觉，也是对初学者来说是最别扭的结论。自然坐标系和直角坐标系项最大的不同就是在直角坐标系下只有 d y d x \frac{dy}{dx} dxdy，而在曲线坐标系下有 d d s \frac{d}{ds} dsd，还有 d d s x \frac{d}{ds_x} dsxd。

在直角坐标系下，如果对函数求导，只有 d x d x dx， d x d x dx 不区分 d x d x dx 到底是实际点的位移，还是在 x x x 坐标方向上分量的位移。

但在自然坐标系下，因为 Δ s \Delta s Δs 与 Δ s ′ \Delta s' Δs′ 一般不相等，所以它们的微分 d s ds ds 和 d s ′ ds' ds′ 一般也不一样，所以在自然坐标系下，如果想求变量在 d s ds ds 方向上的导数时，必须要指出弧微分 d s ds ds 到底是哪个曲线的弧长。

总结：在直角坐标系下只有 d x dx dx，在自然坐标系下可能有多个 d s ds ds。

3.4 实例分析：车辆轨迹与道路几何中的坐标位移差异

举个例子说明：

比如短弧线是车辆轨迹，长弧线是道路几何，假设车沿道路平行移动，即车的 l l l 不动。

车原来在红色点的位置，经过 d y dy dy 的时间，车跑到蓝色点的位置，同样它在坐标轴上的投影也跑到蓝色点处，就有两个 d s ds ds：

车速 d s x d t = ∣ v ⃗ ∣ \frac{ds_x}{dt}=|\vec v| dtdsx=∣v ∣。
投影速度 d s d t = s ˙ \frac{ds}{dt}=\dot{s} dtds=s˙。

上面算出来的是 v v v 的大小，不带方向，一般情况下 ∣ v ⃗ ∣ |\vec v| ∣v ∣ 不等于 s ˙ \dot s s˙，因为 d s ds ds 和 d s x ds_x dsx 不一样，所以它们的速度不一样。

四、预备知识1：质点速度的向量表达式与道路几何投影导数推导

4.1 质点速度的向量表达式

比如在直角坐标系下质点的轨迹是这样：

证明 r ˙ ⃗ = ∣ v ⃗ ∣ τ ⃗ \vec{\dot{r}}=|\vec{v}|\vec{\tau} r˙ =∣v ∣τ ， τ ⃗ \vec{\tau} τ 是质点在切线方向上的单位向量，记 ∣ v ⃗ ∣ τ ⃗ |\vec{v}|\vec{\tau} ∣v ∣τ 为 v ⃗ \vec v v 。

假设经过 d t dt dt 时间， r ⃗ \vec r r 位置变成了 r + d r r+dr r+dr ，它划过的弧长为 d s ds ds。根据向量加减法，紫红色向量为 d r dr dr，位矢对时间的导数 r ˙ ⃗ = d r ⃗ d t = d r ⃗ d s ⋅ d s d t \vec{\dot r}=\frac{d\vec{r}}{dt}=\frac{d\vec{r}}{ds}\cdot\frac{ds}{dt} r˙ =dtdr =dsdr ⋅dtds 当 d t → 0 dt\rightarrow 0 dt→0 时， ∣ d r ⃗ ∣ d s → 1 \frac{|d\vec{r}|}{ds}\rightarrow 1 ds∣dr ∣→1，方向趋于轨迹在 r r r 的切线方向 τ ⃗ \vec \tau τ ，所以：
r ˙ ⃗ = 1 ⋅ τ ⃗ ⋅ d s d t = ∣ v ⃗ ∣ τ ⃗ = v ⃗ \vec{\dot{r}}=1\cdot \vec{\tau}\cdot \frac{ds}{dt}=|\vec{v}|\vec{\tau}=\vec{v} r˙ =1⋅τ ⋅dtds=∣v ∣τ =v

4.2 拓展：质点在道路几何上的投影位矢导数推导

比如在直角坐标系下有两条曲线：质点轨迹和道路几何

比如在质点轨迹曲线上有质点，位置为 r ⃗ h \vec r_h r h，质点在道路几何上的投影位矢记为 r ⃗ r \vec r_r r r，根据上面的推导 r ˙ ⃗ h = ∣ v ⃗ ∣ τ ⃗ = v ⃗ \vec{\dot{r}}_h=|\vec{v}|\vec{\tau}=\vec{v} r˙ h=∣v ∣τ =v ， τ ⃗ h \vec \tau_h τ h 是质点在轨迹上的切线方向， τ ⃗ r \vec \tau_r τ r 是投影在道路几何上的切线方向，投影位矢的导数为：

r ˙ ⃗ r = d r ⃗ r d t = d r ⃗ r d s r ⋅ d s r d t = s ˙ τ ⃗ r \vec{\dot{r}}_r=\frac{\text{d}\vec{r}_r}{\text{d}t}=\frac{\text{d}\vec{r}_r}{\text{d}s_r}\cdot \frac{\text{d}s_r}{\text{d}t}=\dot{s}\vec{\tau}_r r˙ r=dtdr r=dsrdr r⋅dtdsr=s˙τ r 如果道路是 Frenet 坐标系的坐标轴，则 d s r d t = s ˙ \frac{\text{d}s_r}{\text{d}t}=\dot{s} dtdsr=s˙。在自然坐标系下有不同的 d s ds ds，但在直角坐标系下只有 d x dx dx。

4.3 质点投影点的定义

如果点的切线方向和点与质点的连线方向垂直：

称为质点的投影点

五、预备知识2：Frenet公式

5.1 曲线坐标系基向量特性

在曲线坐标系下， d τ ⃗ d s \frac{d\vec \tau}{ds} dsdτ 一般不为零向量。

问题来了， d τ ⃗ d s \frac{d\vec \tau}{ds} dsdτ 到底等于什么呢？包括它的法向量 d n ⃗ d s \frac{d\vec n}{ds} dsdn 等于什么呢？

5.2 Frenet公式及其证明

Frenet公式给出了结果：
d t ⃗ d s = κ n ⃗ d n ⃗ d s = − κ τ ⃗ \frac{d\vec{t}}{ds}=\kappa \vec{n}\quad \frac{d\vec{n}}{ds}=-\kappa \vec{\tau} dsdt =κn dsdn =−κτ 其中， d s ds ds 为所画曲线的弧微分。

证明：假设 τ \tau τ 经过 d s ds ds 变成了 τ + d τ \tau +d\tau τ+dτ，因为 τ \tau τ 和 τ + d τ \tau +d\tau τ+dτ 是单位向量，所以模都是 1 1 1。

几何关系如下：

夹角为 d θ d\theta dθ，三角形是等腰三角形，根据几何关系， d τ ⃗ d\vec{\tau} dτ 的长度为：
∣ d τ ⃗ ∣ = 2 ⋅ 1 ⋅ sin ⁡ ( d θ 2 ) |d\vec{\tau}|=2\cdot 1\cdot \sin \left( \frac{d\theta}{2} \right) ∣dτ ∣=2⋅1⋅sin(2dθ) 当 d s ds ds 趋于 0 0 0 时， d τ ⃗ d\vec \tau dτ 的方向趋于 τ ⃗ \vec \tau τ 的垂直方向 n ⃗ \vec n n ，大小为
∣ d τ ⇀ ∣ d s = 2 sin ⁡ d θ 2 d s = d θ d s = κ \frac{|d\overrightharpoon{\tau }|}{ds}=\frac{2\sin \frac{d\theta}{2}}{ds}=\frac{d\theta}{ds}=\kappa ds∣dτ ∣=ds2sin2dθ=dsdθ=κ 其中， d θ d s \frac{d\theta}{ds} dsdθ 就是曲率的定义，所以
d τ ⃗ d s = κ n ⃗ \frac{d\vec{\tau}}{ds}=\kappa \vec{n} dsdτ =κn 同理
d n ⃗ d s = − κ τ ⃗ \frac{d\vec{n}}{ds}=-\kappa \vec{\tau} dsdn =−κτ 证明方式和上面一样。

5.3 拓展1：质点轨迹与道路几何的方向导数

假设上面弧曲线是质点轨迹，下面是道路几何，有质点的位矢以及质点的投影位矢。记质点的切线方向是 τ ⃗ h \vec \tau_h τ h，它的投影切线方向是 τ ⃗ r \vec \tau_r τ r， τ ⃗ h \vec \tau_h τ h 对时间的导数：
τ ˙ ⃗ h = d τ ⃗ h d t = d τ ⃗ h d s h ⋅ d s h d t = κ h n ⃗ h ∣ v ⃗ ∣ \vec{\dot{\tau}}_h=\frac{d\vec{\tau}_h}{dt}=\frac{d\vec{\tau}_h}{ds_h}\cdot \frac{ds_h}{dt}=\kappa_h \vec{n}_h|\vec{v}| τ˙ h=dtdτ h=dshdτ h⋅dtdsh=κhn h∣v ∣ 其中， κ h \kappa_h κh 为质点所在轨迹的曲率。

同理，质点法向量的导数为：
n ˙ ⃗ h = − κ τ ⃗ h ⋅ ∣ v ⃗ ∣ = − κ h ∣ v ⃗ ∣ τ ⃗ h \vec{\dot{n}}_h=-\kappa \vec{\tau}_h\cdot |\vec{v}|=-\kappa_h |\vec{v}|\vec{\tau}_h n˙ h=−κτ h⋅∣v ∣=−κh∣v ∣τ h 同理，投影点的切向量和法向量对时间的导数分别为：
τ ˙ ⃗ r = κ s ˙ n ⃗ r n ˙ ⃗ r = − κ s ˙ τ ⃗ r \begin{aligned} \vec{\dot{\tau}}_r&=\kappa \dot{s}\vec{n}_r\\ \vec{\dot{n}}_r&=-\kappa \dot{s}\vec{\tau}_r\\ \end{aligned} τ˙ rn˙ r=κs˙n r=−κs˙τ r 这四个公式是坐标转换的核心公式。

坐标转换有 l , l ′ , l ′ ′ l,l',l'' l,l′,l′′，其中， l ′ = d l d s l'=\frac{dl}{ds} l′=dsdl 。 d s ds ds 指的是 Frenet 坐标系下坐标轴曲线的弧微分。

注意：上述四个核心公式中的曲率，无论是 κ h \kappa_h κh 还是 κ r \kappa_r κr，都是指在直角坐标系下的曲率。因为自然坐标系下也有曲线及其曲率，自然坐标系下的曲率和直角坐标系下的曲率不一样。

5.4 拓展2：切向加速度与法向加速度的分解

同样在直角坐标系下有轨迹和质点：

已知 r ⃗ , τ ⃗ , n ⃗ , κ \vec{r},\vec{\tau},\vec{n},\kappa r ,τ ,n ,κ，求 v ⃗ \vec v v ：
v ⃗ = r ˙ ⃗ = ∣ v ⃗ ∣ τ ⃗ \vec{v}=\vec{\dot{r}}=|\vec{v}|\vec{\tau} v =r˙ =∣v ∣τ 求 a ⃗ \vec a a ，利用预备知识的拓展1： τ ˙ ⃗ = κ ∣ v ⃗ ∣ n ⃗ \vec{\dot{\tau}}=\kappa |\vec{v}|\vec{n} τ˙ =κ∣v ∣n

a ⃗ = d v ⃗ d t = d ∣ v ⃗ ∣ τ ⃗ d t = d ∣ v ⃗ ∣ d t τ ⃗ + ∣ v ⃗ ∣ d τ ⃗ d t = ∣ v ˙ ⃗ ∣ τ ⃗ + ∣ v ⃗ ∣ τ ˙ ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ + ∣ v ⃗ ∣ 2 κ n ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ + ∣ v ⃗ ∣ 2 ρ n ⃗ \begin{aligned} \vec{a}&=\frac{d\vec{v}}{dt}=\frac{d|\vec{v}|\vec{\tau}}{dt}=\frac{d|\vec{v}|}{dt}\vec{\tau}+|\vec{v}|\frac{d\vec{\tau}}{dt}\\ &=|\vec{\dot{v}}|\vec{\tau}+|\vec{v}|\vec{\dot{\tau}}\\ &=|\vec{\dot{v}}|\vec{\tau}+|\vec{v}|^2\kappa \vec{n}\\ &=|\vec{\dot{v}}|\vec{\tau}+\frac{|\vec{v}|^2}{\rho}\vec{n}\\ \end{aligned} a =dtdv =dtd∣v ∣τ =dtd∣v ∣τ +∣v ∣dtdτ =∣v˙ ∣τ +∣v ∣τ˙ =∣v˙ ∣τ +∣v ∣2κn =∣v˙ ∣τ +ρ∣v ∣2n 其中，曲率半径 ρ = 1 κ \rho =\frac{1}{\kappa} ρ=κ1，在本例中为负值， ∣ v ˙ ⃗ ∣ τ ⃗ |\vec{\dot{v}}|\vec \tau ∣v˙ ∣τ 为切向加速度， ∣ v ⃗ ∣ 2 ρ n ⃗ \frac{|\vec{v}|^2}{\rho}\vec{n} ρ∣v ∣2n 为法向加速度，即向心加速度。

六、预备知识总结

这些公式是以后推导坐标转换时非常有用的辅助公式。

6.1 变量含义

以下变量都以笛卡尔坐标为基准

变量	含义	变量	含义
r ⃗ h \vec r_h r h	车辆位矢	r ⃗ r \vec r_r r r	投影位矢
v ⃗ \vec v v	车辆速度	s ˙ \dot s s˙	投影速率
a ⃗ \vec a a	车辆加速度	κ r \kappa_r κr	投影位矢在道路几何上的曲率
κ h \kappa_h κh	车辆位矢在车辆轨迹上的曲率	τ ⃗ r \vec \tau_r τ r	投影位矢在道路几何上的切线方向单位向量
τ ⃗ h \vec \tau_h τ h	车辆位矢在车辆轨迹上的切线方向单位向量	n ⃗ r \vec n_r n r	投影位矢在道路几何上的法线方向单位向量
n ⃗ h \vec n_h n h	车辆位矢在车辆轨迹上的法线方向单位向量

6.2 辅助公式

7 7 7 个辅助公式是求自然坐标和直角坐标之间转化的关键：

r ˙ ⃗ h = ∣ v ⃗ ∣ τ ⃗ h r ˙ ⃗ r = s ˙ τ ⃗ r τ ˙ ⃗ h = κ h ∣ v ⃗ ∣ n ⃗ h n ˙ ⃗ h = − κ h ∣ v ⃗ ∣ τ ⃗ h τ ˙ ⃗ r = κ r s ˙ n ⃗ r n ˙ ⃗ r = − κ r s ˙ τ ⃗ r a ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ h + ∣ v ⃗ ∣ 2 κ h n ⃗ h \begin{align} \vec{\dot{r}}_h&=|\vec{v}|\vec{\tau}_h\\ \vec{\dot{r}}_r&=\dot{s}\vec{\tau}_r\\ \vec{\dot{\tau}}_h&=\kappa _h|\vec{v}|\vec{n}_h\\ \vec{\dot{n}}_h&=-\kappa _h|\vec{v}|\vec{\tau}_h\\ \vec{\dot{\tau}}_r&=\kappa _r\dot{s}\vec{n}_r\\ \vec{\dot{n}}_r&=-\kappa _r\dot{s}\vec{\tau}_r\\ \vec{a}&=|\vec{\dot{v}}|\vec{\tau}_h+|\vec{v}|^2\kappa _h\vec{n}_h\\ \end{align} r˙ hr˙ rτ˙ hn˙ hτ˙ rn˙ ra =∣v ∣τ h=s˙τ r=κh∣v ∣n h=−κh∣v ∣τ h=κrs˙n r=−κrs˙τ r=∣v˙ ∣τ h+∣v ∣2κhn h 有了这 7 7 7 个辅助公式，后面一切推导都水到渠成，就是微积分的计算。

七、坐标转换算法

7.1 问题描述

问题非常明晰了：

已知笛卡尔坐标下的 r ⃗ h , v ⃗ h , a ⃗ h , κ h , τ ⃗ h , n ⃗ h \vec{r}_h,\vec{v}_h,\vec{a}_h,\kappa _h,\vec{\tau}_h,\vec{n}_h r h,v h,a h,κh,τ h,n h

已知 Frenet 坐标系下的起点 ( x 0 , y 0 ) (x_0,y_0) (x0,y0)

求 Frenet 坐标系下的 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′。其中， s ˙ = d s d t \dot s=\frac{ds}{dt} s˙=dtds， l ′ = d l d s l'=\frac{dl}{ds} l′=dsdl， d s ds ds 为 Frenet 坐标轴的弧长。

7.2 坐标转换算法步骤概述

算法分三步：

第一步 ： 7 7 7 个辅助公式

第二步 ：找到车在Frenet坐标系下的投影点在笛卡尔坐标系下的坐标，记为 x r , y r , θ r , k r {x_r,y_r,\theta_r,k_r} xr,yr,θr,kr，

计算
r ⃗ r = ( x r , y r ) τ ⃗ r = ( cos ⁡ θ r , sin ⁡ θ r ) n ⃗ r = ( − sin ⁡ θ r , cos ⁡ θ r ) \vec{r}_r=\left( x_r,y_r \right) \quad \vec{\tau}_r=\left( \cos \theta _r,\sin \theta _r \right) \quad \vec{n}_r=\left( -\sin \theta _r,\cos \theta _r \right) r r=(xr,yr)τ r=(cosθr,sinθr)n r=(−sinθr,cosθr) 其中， θ r \theta_r θr 代表投影点的切线方向 τ ⃗ r \vec \tau_r τ r 与 x x x 轴的夹角，如下图所示：

第三步 ：利用向量三角形以及微积分求出 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′

7.3 核心公式及其应用

由向量三角形关系得到坐标转换的核心公式：
r r ⃗ + l n ⃗ r = r ⃗ h \vec{r_r}+l\vec{n}_r=\vec{r}_h rr +ln r=r h 但公式目前还不能直接用，因为 r r ⃗ \vec{r_r} rr 和 n ⃗ r \vec{n}_r n r 都不知道，知道的只有 r ⃗ h , v ⃗ h , a ⃗ h , κ h , τ ⃗ h , n ⃗ h \vec{r}_h,\vec{v}_h,\vec{a}_h,\kappa _h,\vec{\tau}_h,\vec{n}_h r h,v h,a h,κh,τ h,n h。

首先要找到投影，即 ( x h , y h ) (x_h,y_h) (xh,yh)在 Frenet 坐标系下的投影 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr)，但在这里先不讲，假设已经找到了 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr)。找投影的过程下一节再讲，因为找投影比较绕。

如果找到投影点的信息，自然可以得到 r ⃗ r , τ ⃗ r , n ⃗ r , κ r \vec{r}_r,\vec{\tau}_r,\vec{n}_r,\kappa _r r r,τ r,n r,κr。

有了这些自然就可以用核心公式 r r ⃗ + l n ⃗ r = r ⃗ h \vec{r_r}+l\vec{n}_r=\vec{r}_h rr +ln r=r h。

(1) 计算弧长

第一步 ：计算 l l l，根据核心公式 l n ⃗ r = r ⃗ h − r r ⃗ l\vec{n}_r=\vec{r}_h-\vec{r_r} ln r=r h−rr ，两边点乘 n ⃗ r \vec{n}_r n r：
l = ( r ⃗ h − r ⃗ r ) ⋅ n ⃗ r l=\left( \vec{r}_h-\vec{r}_r \right) \cdot \vec{n}_r l=(r h−r r)⋅n r

(2) 计算弧速度

第二步 ：计算 s ˙ \dot s s˙，核心公式 r r ⃗ + l n ⃗ r = r ⃗ h \vec{r_r}+l\vec{n}_r=\vec{r}_h rr +ln r=r h 两边对时间求导：
r ˙ ⃗ r + l n ˙ ⃗ r + l ˙ n ⃗ r = r ˙ ⃗ h \vec{\dot{r}}_r+l\vec{\dot{n}}_r+\dot{l}\vec{n}_r=\vec{\dot{r}}_h r˙ r+ln˙ r+l˙n r=r˙ h 利用辅助公式 ( 1 ) ( 2 ) ( 5 ) ( 6 ) (1)(2)(5)(6) (1)(2)(5)(6)，代入得到：
s ˙ τ ⃗ r + l ( − κ r s ˙ τ ⃗ r ) + l ˙ n ⃗ r = v ⃗ h \dot{s}\vec{\tau}_r+l\left( -\kappa _r\dot{s}\vec{\tau}_r \right) +\dot{l}\vec{n}_r=\vec{v}_h s˙τ r+l(−κrs˙τ r)+l˙n r=v h 两边同时点乘 τ ⃗ r \vec{\tau}_r τ r，得到
s ˙ + l ( − κ r s ˙ ) = v ⃗ h ⋅ τ ⃗ r \dot{s}+l\left( -\kappa _r\dot{s} \right) =\vec{v}_h\cdot \vec{\tau}_r s˙+l(−κrs˙)=v h⋅τ r 这样得到
s ˙ = v ⃗ h ⋅ τ ⃗ r 1 − k r l \dot{s}=\frac{\vec{v}_h\cdot \vec{\tau}_r}{1-k_rl} s˙=1−krlv h⋅τ r 又因为
v ⃗ h ⋅ τ ⃗ r = ∣ v ⃗ h ∣ τ ⃗ h ⋅ τ ⃗ r = ∣ v ⃗ h ∣ ∣ τ ⃗ h ∣ ∣ τ ⃗ r ∣ cos ⁡ < τ ⃗ h , τ ⃗ r > = ∣ v ⃗ h ∣ cos ⁡ ( θ h − θ r ) \vec{v}_h\cdot \vec{\tau}_r=|\vec{v}_h|\vec{\tau}_h\cdot \vec{\tau}_r=|\vec{v}_h||\vec{\tau}_h||\vec{\tau}_r|\cos <\vec{\tau}_h,\vec{\tau}_r>=|\vec{v}_h|\cos \left( \theta _h-\theta _r \right) v h⋅τ r=∣v h∣τ h⋅τ r=∣v h∣∣τ h∣∣τ r∣cos<τ h,τ r>=∣v h∣cos(θh−θr) 得到另一种形式：
s ˙ = ∣ v ⃗ h ∣ cos ⁡ ( θ h − θ r ) 1 − κ r l \dot{s}=\frac{|\vec{v}_h|\cos \left( \theta _h-\theta _r \right)}{1-\kappa _rl} s˙=1−κrl∣v h∣cos(θh−θr) 推荐使用上面的向量法表示，更简洁。

(3) 计算弧长的时间导数

第三步 ：计算 l ˙ \dot l l˙，由第二步推导的 s ˙ τ ⃗ r + l ( − κ r s ˙ τ ⃗ r ) + l ˙ n ⃗ r = v ⃗ h \dot{s}\vec{\tau}_r+l\left( -\kappa _r\dot{s}\vec{\tau}_r \right) +\dot{l}\vec{n}_r=\vec{v}_h s˙τ r+l(−κrs˙τ r)+l˙n r=v h，两边同时点乘 n ⃗ r \vec n_r n r 得：
l ˙ = v ⃗ h ⋅ n ⃗ r \dot l=\vec v_h \cdot \vec n_r l˙=v h⋅n r 若将向量形式展开，得到 l ˙ = ∣ v ⃗ h ∣ sin ⁡ ( θ h − θ r ) \dot l=|\vec{v}_h|\sin( \theta _h-\theta _r) l˙=∣v h∣sin(θh−θr)。

(4) 计算弧长的弧坐标导数

第四步 ：计算 l ′ l' l′：
l ′ = d l d s = d l d t d s d t = l ˙ s ˙ = v ⃗ ⋅ n ⃗ r v ⃗ ⋅ τ ⃗ r 1 − κ r l = ( 1 − κ r l ) v ⃗ ⋅ n ⃗ r v ⃗ ⋅ τ ⃗ r l'=\frac{dl}{ds}=\frac{\frac{dl}{dt}}{\frac{ds}{dt}}=\frac{\dot{l}}{\dot{s}}=\frac{\vec{v}\cdot \vec{n}_r}{\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl}}=\left( 1-\kappa _rl \right) \frac{\vec{v}\cdot \vec{n}_r}{\vec{v}\cdot \vec{\tau}_r} l′=dsdl=dtdsdtdl=s˙l˙=1−κrlv ⋅τ rv ⋅n r=(1−κrl)v ⋅τ rv ⋅n r其中， d s ds ds 为 Frenet 坐标轴的弧坐标导数，因此 d s d t = s ˙ \frac{ds}{dt}=\dot s dtds=s˙。

(5) 计算弧加速度

第五步 ：计算 s ¨ \ddot s s¨，由第二步计算的 s ˙ = v ⃗ h ⋅ τ ⃗ r 1 − k r l \dot{s}=\frac{\vec{v}_h\cdot \vec{\tau}_r}{1-k_rl} s˙=1−krlv h⋅τ r，利用复合求导，得到：

s ¨ = d s ˙ d t = 1 ( 1 − κ r l ) 2 ( d ( v ⃗ ⋅ τ ⃗ r ) d t ⋅ ( 1 − κ r l ) − ( v ⃗ ⋅ τ ⃗ r ) ⋅ ( − κ ˙ r l − κ r l ˙ ) ) = 1 1 − κ r l ( d v ⃗ d t ⋅ τ ⃗ r + v ⃗ ⋅ d τ ⃗ r d t ) + 1 1 − κ r l v ⃗ ⋅ τ ⃗ r 1 − κ r l ( κ ˙ r l + κ r l ˙ ) = 1 1 − κ r l ( a ⃗ ⋅ τ ⃗ r + v ⃗ ⋅ ( κ r ⋅ s ˙ ⋅ n ⃗ r ) + 1 1 − κ r l s ˙ ( d κ r d s d s d t l + κ r ⋅ d l d s ⋅ d s d t ) = a ⃗ ⋅ τ ⃗ r 1 − κ r l + ( κ r s ˙ ) ( v ⃗ ⋅ n ⃗ r ) 1 − κ r l + s ˙ 2 1 − κ r l ( κ r ′ l + κ r l ′ ) \begin{aligned} \ddot{s}&=\frac{\text{d}\dot{s}}{\text{d}t}=\frac{1}{\left( 1-\kappa _rl \right) ^2}\left( \frac{\text{d}\left( \vec{v}\cdot \vec{\tau}_r \right)}{\text{d}t}\cdot \left( 1-\kappa _rl \right) -\left( \vec{v}\cdot \vec{\tau}_r \right) \cdot \left( -\dot{\kappa}_rl-\kappa _r\dot{l} \right) \right)\\ &=\frac{1}{1-\kappa _rl}\left( \frac{d\vec{v}}{dt}\cdot \vec{\tau}_r+\vec{v}\cdot \frac{d\vec{\tau}_r}{dt} \right) +\frac{1}{1-\kappa _rl}\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl}\left( \dot{\kappa}_rl+\kappa _r\dot{l} \right)\\ &=\frac{1}{1-\kappa _rl}\text{(}\vec{a}\cdot \vec{\tau}_r+\vec{v}\cdot \left( \kappa _r\cdot \dot{s}\cdot \vec{n}_r \right) +\frac{1}{1-\kappa _rl}\dot{s}\left( \frac{d\kappa _r}{ds}\frac{ds}{dt}l+\kappa _r\cdot \frac{dl}{ds}\cdot \frac{ds}{dt} \right)\\ &=\frac{\vec{a}\cdot \vec{\tau}_r}{1-\kappa _rl}+\frac{\left( \kappa _r\dot{s} \right) \left( \vec{v}\cdot \vec{n}_r \right)}{1-\kappa _rl}+\frac{\dot{s}^2}{1-\kappa _rl}\left( \kappa _{r}'l+\kappa _rl' \right)\\ \end{aligned} s¨=dtds˙=(1−κrl)21(dtd(v ⋅τ r)⋅(1−κrl)−(v ⋅τ r)⋅(−κ˙rl−κrl˙))=1−κrl1(dtdv ⋅τ r+v ⋅dtdτ r)+1−κrl11−κrlv ⋅τ r(κ˙rl+κrl˙)=1−κrl1(a ⋅τ r+v ⋅(κr⋅s˙⋅n r)+1−κrl1s˙(dsdκrdtdsl+κr⋅dsdl⋅dtds)=1−κrla ⋅τ r+1−κrl(κrs˙)(v ⋅n r)+1−κrls˙2(κr′l+κrl′) 又因为 v ⃗ ⋅ n ⃗ r = l ˙ ， l ˙ = l ′ ⋅ s ˙ \vec{v}\cdot \vec{n}_r=\dot{l}， \dot{l}=l'\cdot \dot{s} v ⋅n r=l˙，l˙=l′⋅s˙，代入最终得到
s ¨ = a ⃗ ⋅ τ ⃗ r 1 − κ r l + κ r s ˙ 2 l ′ 1 − κ r l + s ˙ 2 1 − κ r l ( κ r ′ l + κ r l ′ ) \ddot{s}=\frac{\vec{a}\cdot \vec{\tau}_r}{1-\kappa _rl}+\frac{\kappa _r\dot{s}^2l'}{1-\kappa _rl}+\frac{\dot{s}^2}{1-\kappa _rl}\left( \kappa _{r}'l+\kappa _rl' \right) s¨=1−κrla ⋅τ r+1−κrlκrs˙2l′+1−κrls˙2(κr′l+κrl′) 其中， a ⃗ ⋅ τ ⃗ r \vec{a}\cdot \vec{\tau}_r a ⋅τ r 的计算如下，利用辅助公式 ( 7 ) (7) (7)： a ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ h + ∣ v ⃗ ∣ 2 κ h n ⃗ h \vec{a}=|\vec{\dot{v}}|\vec{\tau}_h+|\vec{v}|^2\kappa _h\vec{n}_h a =∣v˙ ∣τ h+∣v ∣2κhn h，可得
a ⃗ ⋅ τ ⃗ r = ∣ v ˙ ⃗ ∣ cos ⁡ < τ ⃗ h , τ ⃗ r > + ∣ v ⃗ ∣ 2 κ h n ⃗ h ⋅ τ ⃗ r = ∣ v ˙ ⃗ ∣ cos ⁡ ( θ h − θ r ) + κ h ⋅ ( 1 − κ r l ) 2 cos ⁡ 2 ( θ h − θ r ) s ˙ 2 ⋅ ( − sin ⁡ ( θ h − θ r ) ) = ∣ v ˙ ⃗ ∣ cos ⁡ ( θ h − θ r ) + κ h s ˙ 2 1 − κ r l cos ⁡ ( θ n − θ r ) ⋅ ( − l ′ ) \begin{aligned} \vec{a}\cdot \vec{\tau}_r&=|\vec{\dot{v}}|\cos <\vec{\tau}_h,\vec{\tau}_r>+|\vec{v}|^2\kappa _h\vec{n}_h\cdot \vec{\tau}_r\\ &=|\vec{\dot{v}}|\cos \left( \theta _h-\theta _r \right) +\kappa _h\cdot \frac{\left( 1-\kappa _rl \right) ^2}{\cos ^2\left( \theta _h-\theta _r \right)}\dot{s}^2\cdot \left( -\sin \left( \theta _h-\theta _r \right) \right)\\ &=|\vec{\dot{v}}|\cos \left( \theta _h-\theta _r \right) +\kappa_h\dot{s}^2\frac{1-\kappa _rl}{\cos \left( \theta _n-\theta _r \right)}\cdot \left( -l' \right)\\ \end{aligned} a ⋅τ r=∣v˙ ∣cos<τ h,τ r>+∣v ∣2κhn h⋅τ r=∣v˙ ∣cos(θh−θr)+κh⋅cos2(θh−θr)(1−κrl)2s˙2⋅(−sin(θh−θr))=∣v˙ ∣cos(θh−θr)+κhs˙2cos(θn−θr)1−κrl⋅(−l′)

(6) 计算弧长的二阶时间导数

第六步 ：计算 l ¨ \ddot l l¨，由辅助公式 ( 3 ) (3) (3)： l ˙ = v ⃗ ⋅ n ⃗ r \dot l=\vec v \cdot \vec n_r l˙=v ⋅n r 可得：
l ¨ = d v ⃗ d t ⋅ n r ⃗ + v ⃗ ⋅ d n ⃗ r d t \ddot{l}=\frac{d\vec{v}}{dt}\cdot \vec{n_r}+\vec{v}\cdot \frac{d\vec{n}_r}{dt} l¨=dtdv ⋅nr +v ⋅dtdn r 由辅助公式 ( 6 ) (6) (6)： n ⃗ r = − κ r s ˙ τ ⃗ r \vec{n}_r=-\kappa _r\dot{s}\vec{\tau}_r n r=−κrs˙τ r 可得：
l ¨ = a ⃗ ⋅ n ⃗ r + v ⃗ ⋅ ( − κ r s ˙ τ ⃗ r ) = a ⃗ ⋅ n ⃗ r − κ r s ˙ ( v ⃗ ⋅ τ ⃗ r ) = a ⃗ ⋅ n ⃗ r − κ r ( 1 − κ r l ) s ˙ 2 \begin{aligned} \ddot{l}&=\vec{a}\cdot \vec{n}_r+\vec{v}\cdot \left( -\kappa _r\dot{s}\vec{\tau}_r \right)\\ &=\vec{a}\cdot \vec{n}_r-\kappa _r\dot{s}\left( \vec{v}\cdot \vec{\tau}_r \right)\\ &=\vec{a}\cdot \vec{n}_r-\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2\\ \end{aligned} l¨=a ⋅n r+v ⋅(−κrs˙τ r)=a ⋅n r−κrs˙(v ⋅τ r)=a ⋅n r−κr(1−κrl)s˙2

(7) 计算弧长的二阶弧坐标导数

第七步 ：计算 l ′ ′ l'' l′′，用 l ¨ \ddot l l¨ 计算：
l ¨ = d l ˙ d t = d ( l ′ s ˙ ) d t = d l ′ d t s ˙ + l ′ ⋅ s ¨ = d l ′ d s ⋅ d s d t ⋅ s ˙ + l ′ s ¨ = l ′ ' s ˙ 2 + l ′ s ¨ \begin{aligned} \ddot{l}&=\frac{d\dot{l}}{dt}=\frac{d\left( l'\dot{s} \right)}{dt}=\frac{dl'}{dt}\dot{s}+l'\cdot \ddot{s}\\ &=\frac{dl'}{ds}\cdot \frac{ds}{dt}\cdot \dot{s}+l'\ddot{s}\\ &=l''\dot{s}^2+l'\ddot{s}\\ \end{aligned} l¨=dtdl˙=dtd(l′s˙)=dtdl′s˙+l′⋅s¨=dsdl′⋅dtds⋅s˙+l′s¨=l′'s˙2+l′s¨ 所以：
l ′ ′ = l ¨ − l ′ s ¨ s ˙ 2 l''=\frac{\ddot{l}-l'\ddot{s}}{\dot{s}^2} l′′=s˙2l¨−l′s¨ 这样基本上所有直角坐标转自然坐标都已讲完。

八、总结

共有 8 8 8 个坐标需要转化，即 s , s ˙ , s ¨ , l , l ′ , l ′ ′ , l ˙ , l ¨ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime},\dot l,\ddot l s,s˙,s¨,l,l′,l′′,l˙,l¨ ，总结公式如下：
l = ( r ⃗ h − r ⃗ r ) ⋅ n ⃗ r l ˙ = v ⃗ ⋅ n ⃗ r s ˙ = v ⃗ ⋅ τ ⃗ r 1 − κ r l l ′ = l ˙ s ˙ l ¨ = a ⃗ ⋅ n ⃗ r − κ r ( 1 − κ r l ) s ˙ 2 s ¨ = a ⃗ ⋅ τ ⃗ r 1 − κ r l + s ˙ 2 κ r l ′ 1 − κ r l + s ˙ 2 ( κ r ′ l + κ r l ′ ) 1 − κ r l l ′ ′ = l ¨ − l ′ s ¨ s ˙ 2 \begin{aligned} l&=\left( \vec{r}_h-\vec{r}_r \right) \cdot \vec{n}_r\\ \dot{l}&=\vec{v}\cdot \vec{n}_r\\ \dot{s}&=\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl}\\ l'&=\frac{\dot{l}}{\dot{s}}\\ \ddot{l}&=\vec{a}\cdot \vec{n}_r-\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2\\ \ddot{s}&=\frac{\vec{a}\cdot \vec{\tau}_r}{1-\kappa _rl}+\frac{\dot{s}^2\kappa _rl'}{1-\kappa _rl}+\frac{\dot{s}^2\left( \kappa _{r}'l+\kappa _rl' \right)}{1-\kappa _rl}\\ l''&=\frac{\ddot{l}-l'\ddot{s}}{\dot{s}^2}\\ \end{aligned} ll˙s˙l′l¨s¨l′′=(r h−r r)⋅n r=v ⋅n r=1−κrlv ⋅τ r=s˙l˙=a ⋅n r−κr(1−κrl)s˙2=1−κrla ⋅τ r+1−κrls˙2κrl′+1−κrls˙2(κr′l+κrl′)=s˙2l¨−l′s¨ 现在算出了 7 7 7 个，还差 s s s 没算， s s s 和投影点相关。

本篇博客讲解了从直角坐标转换到自然坐标，下一篇博客讲解 s s s 如何计算，以及投影点怎么找，如何从自然坐标转换到直角坐标。

本篇博客到此结束，欢迎关注后续内容！

参考资料

自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换

后记：

🌟 感谢您耐心阅读这篇关于 直角坐标与自然坐标转换Ⅰ 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助，请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时，别忘了收藏本篇博客 ，以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享，共同探索移动机器人 的无限可能！💡

🎭感谢您的支持与关注，让我们一起在知识的海洋中砥砺前行 🚀