01Frenet与Cardesian坐标系(基础知识)

1 简介

Frenet坐标系是一种在几何学和物理学中常用的坐标系，特别是在轨迹规划和机器人控制中。它由法国数学家Jean Frenet于1847年为了解决在求解某些几何问题时遇到的环形坐标系的问题而提出的。它依据曲线的切线和法线来定义坐标轴，主要用于局部地描述曲线或曲面的几何特性。在自动驾驶技术领域，这种坐标系统提供了一种相对于传统的Cardesian坐标系统更为直观的方式来表示车辆在道路上的位置。

1.1 概念介绍和理解

如图1所示，它采用道路中心线作为基准线，并基于该基准线的切线向量 $\vec{\tau }$ 与法线向量 $\vec{n}$ 构建坐标框架，坐标轴互相垂直，分别定义为 $s$ 方向（沿基准线方向，也称为纵向）和 $l$ 方向（当前基准线的法向，也称为横向）。其中，$s$ 值表示车辆沿道路的行进距离，即纵向位置，而 $l$ 值则表示车辆在道路上的水平位置，即横向位置。

Tips：光看定义没什么感觉。我们来理解一下从Cardesian转到Frenet到底做了一件什么事情。

其实，只做了一件事——把Cardesian空间中的曲线“拉直”。首先，我们要理解Cardesian中为什么会有曲线：如图所示，曲线上的点的位姿$(x,y,\theta )$都是相对于$(x_{轴}，O_{原点}，y_{轴})$来进行讨论的，当坐标轴不变，位姿变化时，就形成了一条曲线。如果我们深入理解了"运动是相对的"和“参考系”的概念，那自然可以想到沿着我们曲线切线和法线方向建立参考系，不就可以“化曲为直”了。代价是，我们的参考系一直在变（好比是坐标轴在沿着曲线滑动），用代码实现时需要耗费一些计算量。

如果大家了解过电机的定子和转子就更容易理解Cardesian和Frenet的关系了——Cardesian就好比是内转子电机；Frenet就好比是外转子电机。

1.2 符号表示

Cardesian坐标系表示为：$$[\vec{r},\vec{v},\vec{a},\theta ,\kappa ]$$。其中，各个变量的含义如下：

• $\vec{r}$：在笛卡尔坐标系统中的位置向量（位失）；

• $\vec{v}$：在笛卡尔坐标系统中的线速度向量，可以分解为$v_x,v_y$两个标量（大家可以理解一下什么是标量——标量就是在单个坐标轴方向上投影后的那个系数；有人可能有疑问，那向量的模也是标量呢？这可以理解为投影到实数轴上的系数）；模为$v$，方向为切线方向$\tau$;

• $\vec{w}$：在笛卡尔坐标系中的角速度向量，模为$w$，方向为法线方向或者负法线方向，取决于圆心与切点连线向量与切线方向叉乘的结果：结果为正则为法线方向，为负则为法线负方向（可以用右手法则，圆心在切点左侧为正，右侧为负）。

• $\vec{a}$：在笛卡尔坐标系中的加速度向量，可以被分解为切线方向的加速度$\overrightarrow{a_{\tau }}$和法线方向的加速度$\overrightarrow{a_n}$；

• $\theta$：在笛卡尔坐标系统中，切线与X轴正方向之间的夹角，一般用来表示汽车的朝向角heading；

• $\kappa =\frac{d\theta }{ds}$：道路的曲率，注意需要和角速度（$w=\frac{d\theta}{dt} $）进行区分，前者与时间无关是一个静态量，后者是一个动态量（规控中引入$\kappa$主要是用来对$w$进行约束，不至于翻车，约束的关系参见车辆运动学和动力学分析）。

Frenet坐标系表示为：$$[s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l^{\prime },l^{\prime \prime }]$$。其中，各个变量的含义如下（注意s和l都是标量）：

• $s$：Frenet坐标系中的纵向坐标；
• $\dot{s}=\frac{ds}{dt}$：纵向坐标随时间变化的速率，即纵向速度；
• $\ddot{s}=\frac{d\dot{s}}{dt}$：纵向速度的变化率，即纵向加速度；
• $l$：Frenet坐标系中的横向坐标，大小为横向偏离中心线的距离，方向为法线方向或负法线方向，取决于Frenet的原点到切点连线向量与切线向量的叉乘结果，结果为正则为法线方向，为负则为负法线方向（由于法线方向定义为切线方向逆时针旋转90度，因此有“左正右负”的口诀）；
• $\dot{l}=\frac{dl}{dt}$：横向坐标随时间变化的速率，即横向速度；
• $\ddot{l}=\frac{d\dot{l}}{dt}$：横向速度的变化率，即横向加速度；
• $l^{\prime }=\frac{dl}{ds}$：横向坐标相对于纵向坐标的变化率；
• $l^{\prime \prime }=\frac{d{l}^{\prime }}{ds}$：横向坐标相对于纵向坐标的二阶变化率。

两者的区别如下：

• Cardesian中基向量对t不变化（$\frac{di}{dt}=0$），对x也不变化（$\frac{di}{dx}=0$）；但是Frenet会变化（$\frac{d\tau }{dt}\ne 0$, $\frac{d\tau }{ds}\ne 0$）；
• Cardesian中$dx$能够描述车辆沿x轴方向实际行进的距离；但是在Frenet中，$ds$不能描述车辆实际行进的路程，实际行进的路程只能在Cardesian中计算，表示为$d{s}_r$(h为vehicle的缩写），$ds\ne d{s}_h$。

1.3 优缺点和适用范围

1.3.1 优点

使用Frenet有以下几个优势：

• 化曲为直；
• 实现了横纵向解耦。

上述优势带来的好处是将非线性规划问题转换为线性规划问题，不论是施加约束还是随机采样，都变得十分方便。

1.3.2 缺点和注意事项

存在的缺点是：

• 使用的是质点模型，忽视了车辆形变；对于大型车辆使用的时要十分小心，需要评估形变并添加额外约束。
• 非常依赖参考线的质量，参考线必须处处可导，否则不能保证轨迹连续；参考线的拼接处一定要做好。
• Cartesian与Frenet坐标系在$l$ 大小接近$\frac{1}{{\kappa }_r}$时转换变得不稳定（后面会详细说明）；大曲率的车道不要使用Frenet。
• 不能描述车辆运动学特性；涉及车辆运动学时不要使用Frenet。
• 多车协同规划支持不太好；多车协同规划不要用Frenet。

2 Frenet基础公式推导

2.1 问题定义

Tips：有一个重要的点想明白，才能跳出单纯推导公式的怪圈。

这个重要的点就是——坐标变换这件事到底涉及了几方？答案是三方（和毕业签三方似的~），而不是两方。

有人可能会有疑问，明明是两方，怎么会是三方？

我们拿《车体坐标系和全局坐标系转换》举例，涉及到传感器的全局坐标、车辆的全局坐标和传感器相对于车辆的车体坐标——这三者（毕业生是传感器，学校是车辆，社会雇佣单位是大地；毕业生从学校中出来到社会中去，从学生身份转变为员工的身份，就好比是相对于学校的局部坐标转相对于社会的全局坐标，桥梁是学校，因为学校有它自身相对于社会的地位）。

理解这一点很重要。推公式推不明白，符号混用往往都是因为没有很清楚的阐明或者理解这一点。Cardesian和Frenet之间的坐标变换同样需要“签三方”，少一个都不行。所以，我们要先明确这三方都是谁，不然就会出现“哭了半天不知道谁死了”的尴尬。从大范围到小范围分析，其中一方是大地，第二方是车道中心线（参考线），第三方是车辆。

从Cardesian坐标转Frenet坐标是在做一件什么事情呢？就是已知车辆相对于大地的全局Cardesian坐标；也已知车道中心线（严格来说是车辆在参考线上的投影）相对于大地的全局Cardesian坐标；求车辆相对于车道中心线（严格来讲纵向是参考中心线的某个固定起点，横向是参考车辆当前时刻在车道中心线上的投影点）的Frenet坐标。

如果是求相对于参考线（严格来讲是参考线的某个固定起点）的Cardesian，那这个问题就退变成了《全局坐标系和局部坐标系的转换》，使用向量的旋转和平移就可以完成。

当就目前的目标来看，需要借助曲线坐标系，即Frenet坐标系相关的知识来完成。

2.2 基础公式

我们最终的目的是完成Cartesian和Frenet之间的转换：已知车辆后轴中心点在Cardesian下的状态$[\overrightarrow{r_h},\overrightarrow{v_h},\overrightarrow{a_h},{\theta }_h,{\kappa }_h]$，求解车辆相对于车道中心线（参考线）的Frenet下的状态$[s,\dot{s},\ddot{s},l,\dot{l},\ddot{l},l^{\prime },l^{\prime \prime }]$。

如下图2表示，下标带h（vehicle的缩写）的为车辆状态（vehicle state），下标带r（reference的缩写）的为车辆在参考线上的匹配点的状态。

2.2.1 核心关系

车辆后轴中心在Cardesian下的状态和匹配点（车辆后轴中心在参考线上的投影点）在Cardesian下的状态之间的关系可以用如图中的向量三角形表示，具体关系如下：
$$\overrightarrow{r_h}=\overrightarrow{r_r}+l\overrightarrow{n_r}\text{\hspace{1em}(1)}$$
后续所有的转换公式都是以此为桥梁建立的，称之为核心公式一点都不为过。

2.2.2 Frenet最小状态集

Frenet的最小状态集可以为$[s,\dot{s},\ddot{s},l,\dot{l},\ddot{l}]$，因为：
$$l^{\prime }=\frac{dl}{ds}=\frac{\frac{dl}{dt}}{\frac{ds}{dt}}=\frac{\dot{l}}{\dot{s}}\text{\hspace{1em}(2)}$$

$$l^{\prime \prime }=\frac{d{l}^{\prime }}{ds}=\frac{\frac{d(\dot{l}/\dot{s})}{dt}}{\dot{s}}=\frac{\ddot{l}}{{\dot{s}}^2}-\frac{\dot{l}\ddot{s}}{{\dot{s}}^3}=\frac{\ddot{l}-l^{\prime }\ddot{s}}{{\dot{s}}^2}\text{\hspace{1em}(3)}$$
也可以为$[s,\dot{s},\ddot{s},l,l^{\prime },l^{\prime \prime }]$，因为：
$$\dot{l}=\frac{dl}{dt}=\frac{dl}{ds}\frac{ds}{dt}=l^{\prime }\dot{s}\text{\hspace{1em}(2)}$$

$$\ddot{l}=\frac{d\dot{l}}{dt}=\frac{d(l^{\prime }\dot{s})}{dt}=\frac{d{l}^{\prime }}{dt}\dot{s}+l^{\prime }\ddot{s}=\frac{d{l}^{\prime }}{ds}\frac{ds}{dt}\dot{s}+l^{\prime }\ddot{s}=l^{\prime \prime }{\dot{s}}^2+l^{\prime }\ddot{s}\text{\hspace{1em}(3)}$$

Tips：公式标号一致，代表两个公式等价。第一种推导比第二种复杂一些，因为乘法的求导链式法则比除法形式上稍简单一些。大家使用时也尽量使用乘法的链式法则。

2.2.3 车辆在Cardesian下的向量导数

1. 求位矢对时间的导数$\overrightarrow{v_h}$

$$\dot{\overrightarrow{r_h}}=\overrightarrow{v_h}=|\overrightarrow{v_h}|\overrightarrow{{\tau }_h}=v_h\overrightarrow{{\tau }_h}\text{\hspace{1em}(4)}$$

其中,$v_h$为标量，可以从轮速计算获得。

2. 求切线向量对时间的导数

如图3（注意图中没有加下标h，因为这个图同样适用于2.2.4）所示：

设时间$dt$内，车辆行驶了$d{s}_h$的距离，向量$\overrightarrow{{\tau }_h}$改变了$\overrightarrow{d{\tau }_h}$，旋转的角度为$d{\theta }_h$，$|\overrightarrow{d{\tau }_h}|=2sin(d{\theta }_h/2)$。

则当$dt\to 0,d{s}_h\to 0$时，$d{\tau }_h$的方向趋于$\overrightarrow{n_h}$，大小趋于$d{\theta }_h$，有
$$\dot{\overrightarrow{{\tau }_h}}=\underset{△t\to 0}{\lim }\frac{\overrightarrow{({\tau }_h+△{\tau }_h)-\stackrel{⃗}{{\tau }_h}}}{△t}=\frac{d{\theta }_h\overrightarrow{n_h}}{dt}=\frac{d{\theta }_h\overrightarrow{n_h}}{d{s}_h}\frac{d{s}_h}{dt}={\kappa }_h{v}_h\overrightarrow{n_h}\text{\hspace{1em}(5)}$$

3. 求法线向量对时间的导数

如图4所示：

设时间$dt$内，车辆行驶了$d{s}_h$的距离，向量$\overrightarrow{n_h}$改变了$\overrightarrow{d{n}_h}$，旋转的角度为$d{\theta }_h$，$|\overrightarrow{d{n}_h}|=2sin(d{\theta }_h/2)$。

则当$dt\to 0,d{s}_h\to 0$时，$d{n}_h$的方向趋于$-\overrightarrow{{\tau }_h}$，大小趋于$d{\theta }_h$，有
$$\dot{\overrightarrow{n_h}}=\underset{△t\to 0}{\lim }\frac{\overrightarrow{(n_h+△n_h)-\stackrel{⃗}{n_h}}}{△t}=\frac{-d{\theta }_h\overrightarrow{{\tau }_h}}{dt}=\frac{-d{\theta }_h\overrightarrow{{\tau }_h}}{d{s}_h}\frac{d{s}_h}{dt}=-{\kappa }_h{v}_h\overrightarrow{{\tau }_h}\text{\hspace{1em}(6)}$$

4. 求速度矢量对时间的导数$\vec{a}$

$$\dot{\overrightarrow{v_h}}=\overrightarrow{a_h}=\dot{(|\overrightarrow{v_h}|\overrightarrow{{\tau }_h})}=\dot{v_h}\overrightarrow{{\tau }_h}+v_h\dot{\overrightarrow{{\tau }_h}}=a_{\tau }\overrightarrow{{\tau }_h}+{\kappa }_h{v}_h^2\overrightarrow{n_h}\text{\hspace{1em}(7)}$$

其中，$a_{\tau }=\dot{v_h}$ 为线加速度，可以由纵向加速度计得到，也可以由线速度$v_h$对时间求导得到。

从式(7)中我们还能得到
$$a_n={\kappa }_h{v}_h^2\text{\hspace{1em}(8)}$$
其中，$a_n$为向心加速度，这个值我们从角加速度计也可以得到，从而可以估计车辆行驶轨迹的曲率${\kappa }_h$ 。

2.2.4 匹配点在Cardesian下的向量导数

1. 求位矢对时间的导数$\overrightarrow{v_r}$

$$\dot{\overrightarrow{r_r}}=\overrightarrow{v_r}=|\overrightarrow{v_r}|\overrightarrow{{\tau }_r}=v_r\overrightarrow{{\tau }_r}\text{\hspace{1em}(9)}$$

其中,$v_r$为标量，可以从轮速计算获得。

2. 求切线向量对时间的导数

如图3所示：

设时间$dt$内，车辆行驶了$d{s}_r$的距离，向量$\overrightarrow{{\tau }_r}$改变了$\overrightarrow{d{\tau }_r}$，旋转的角度为$d{\theta }_r$，$|\overrightarrow{d{\tau }_r}|=2sin(d{\theta }_r/2)$。

则当$dt\to 0,d{s}_r\to 0$时，$d{\tau }_r$的方向趋于$\overrightarrow{n_r}$，大小趋于$d{\theta }_r$，有
$$\dot{\overrightarrow{{\tau }_r}}=\underset{△t\to 0}{\lim }\frac{\overrightarrow{({\tau }_r+△{\tau }_r)-\stackrel{⃗}{{\tau }_r}}}{△t}=\frac{d{\theta }_r\overrightarrow{n_r}}{dt}=\frac{d{\theta }_r\overrightarrow{n_r}}{d{s}_r}\frac{d{s}_r}{dt}={\kappa }_r{v}_r\overrightarrow{n_r}\text{\hspace{1em}(10)}$$

3. 求法线向量对时间的导数

如图4所示：

设时间$dt$内，车辆行驶了$d{s}_r$的距离，向量$\overrightarrow{n_r}$改变了$\overrightarrow{d{n}_r}$，旋转的角度为$d{\theta }_r$，$|\overrightarrow{d{n}_r}|=2sin(d{\theta }_r/2)$。

则当$dt\to 0,d{s}_r\to 0$时，$d{n}_r$的方向趋于$-\overrightarrow{{\tau }_r}$，大小趋于$d{\theta }_r$，有
$$\dot{\overrightarrow{n_r}}=\underset{△t\to 0}{\lim }\frac{\overrightarrow{(n_r+△n_r)-\stackrel{⃗}{n_r}}}{△t}=\frac{-d{\theta }_r\overrightarrow{{\tau }_r}}{dt}=\frac{-d{\theta }_r\overrightarrow{{\tau }_r}}{d{s}_r}\frac{d{s}_r}{dt}=-{\kappa }_r{v}_r\overrightarrow{{\tau }_r}\text{\hspace{1em}(11)}$$

---