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

windSeS

已于 2023-07-07 16:54:48 修改

阅读量4.1w

收藏 1k

点赞数 225

分类专栏：无人驾驶技术系统文章标签： Frenet坐标系无人驾驶路径规划 lattic

于 2020-09-23 20:35:13 首次发布

本文为博主原创文章，未经博主允许不得转载。

本文链接：https://blog.csdn.net/u013468614/article/details/108748016

版权

无人驾驶技术系统专栏收录该内容

16 篇文章 554 订阅

订阅专栏

Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。Frenet坐标系使用道路的中心线作为Base frame，使用参考线的切线向量和法线向量建立坐标系。相比笛卡尔坐标系，Frenet坐标系简化了路径规划问题。网上关于Frenet坐标系的博客或资料很多，此处不再赘述。本文先给出简单的转换公式推导过程，然后给出三对Frenet坐标系与Cartesian坐标系互转的python代码与使用示例方便自己查阅使用。详细推导参见博客Apollo项目坐标系研究。note：下图中Frenet的q坐标轴也可以用d或l表示。

文章目录

- 一、 Frenet坐标系与Cartesian坐标系的转换公式简单推导

一、 Frenet坐标系与Cartesian坐标系的转换公式简单推导

1.1 Frenet公式

下图显示了一条3D空间中一条连续可微的曲线K，P为曲线K上的一个点，黄色平面为曲线K在点P处的运动平面。 $\vec{T}$ 为曲线K在点P处的切向量， $\vec{N}$ 为K在P处的法向量（ $\vec{N}$ 与 $\vec{T}$ 在同一运动平面）, $\vec{B}$ 为曲线K在P处的副法向量（ $\vec{B}$ 垂直于运动平面）。
在这里插入图片描述
有Frenet公式：
$\left\{\begin{matrix} \frac{d\vec{T}}{ds}= & k\vec{N} \\ \frac{d\vec{N}}{ds}= & -k\vec{T}+\tau\vec{B} \\ \frac{d\vec{B}}{ds}= & -\tau\vec{N} \end{matrix}\right.$

其中， $\frac{d}{ds}$ 表示某一方向向量对弧长 $s$ 的导数， $k$ 为曲率（ curvature）， $\tau$ 为挠率（ torsion）。

曲率：曲线不能形成一条直线的度量值，曲率越趋于0，则曲线越趋近于直线；
挠率：是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。

在大地表面，局部路面可看作一个平面，对于无人驾驶路径规划应用，挠率可设定为0。从而Frenet公式进一步简化为：
$\left\{\begin{matrix} \frac{d\vec{T}}{ds}= & k\vec{N} \\ \frac{d\vec{N}}{ds}= & -k\vec{T} \end{matrix}\right. \tag{1}$

1.2 简单推导过程

引博客Apollo项目坐标系研究中的图如下：
在这里插入图片描述
Frenet坐标系： $\dot{s}, \ddot{s}, l, \dot{l}, \ddot{l}, l', l'']$ ，Cartesian坐标系： $[\vec{x},v_{x}, a_{a}, \theta_{x},k_{x}]$ ，释义如下：

$s$ ：Frenet纵坐标；
$\dot{s}=\frac{ds}{dt}$ ：Frenet纵坐标对时间的导数，也即沿base frame的速度；
$\ddot{s}=\frac{d\dot{s}}{dt}$ ：沿base frame的加速度；
$l$ ：Frenet横坐标；
$\dot{l}=\frac{dl}{dt}$ ：Frenet横向速度；
$\ddot{l}=\frac{d\dot{l}}{dt}$ ：Frenet横向加速度；
$l'=\frac{dl}{ds}$ ：Frenet横向坐标对纵向坐标的导数；
$l''=\frac{dl'}{ds}$ ：Frenet横向坐标对纵向坐标的二阶导；
$\vec{x}$ ：为对应Cartesian坐标系下的坐标，是一个向量；
$\theta_{x}$ ：Cartesian坐标系下的朝向；
$k_{x}=\frac{d\theta_{x}}{ds}$ ：曲率；
$v_{x}=||\dot{\vec{x}}||_{2}$ ：Cartesian坐标系下的线速度（无方向，无正负）；
$a_{x}=\frac{dv_{x}}{dt}$ ：Cartesian坐标系下的加速度；

已知 $\vec{x}=[x_{x}, y_{x}]$ 推导 $[s, l]$

求 $s$
首先，找到曲线上离 $x_{x},y_{x}]$ 最近的参考点 $\vec{r}=[x_{r}, y_{r}]$ ，该参考点处的 $s$ 即为 $x_{x}, y_{x}]$ 在Frenet坐标系下的 $s$ 。
求 $l$
$\vec{x},\vec{r}$ 分别为参考点与待转换点在Cartesian坐标系下的向量，我们有 $\vec{x}=\vec{r}+l\vec{N_{r}}$ ，变换可得：
$l=(\vec{x}-\vec{r})^{T}\vec{N_{r}}=||\vec{x}-\vec{r}||_{2}\cos(\theta_{x-r}-(\theta_{r}+\frac{\pi}{2}))=||\vec{x}-\vec{r}||_{2}(\sin(\theta_{x-r})\cos(\theta_{r})-\cos(\theta_{x-r})\sin(\theta_{r}))$

备注：上式中， $\theta_{x-r}$ 为向量 $\vec{x}-\vec{r}$ 的方向角度， $\theta_{r}+\frac{\pi}{2}$ 为单位向量 $\vec{N}_{r}$ 的方向角度（对比上一张图可知），则上式可利用向量点乘公式 $\vec{a}\times \vec{b}=|a||b|\cos{\theta}$ 得到。此处， $\theta$ 为向量 $\vec{a}$ 与向量 $\vec{b}$ 之间的夹角。

假定 $\vec{x}=(x_{x}, y_{x}), \vec{r}=(x_{r}, y_{r})$ ，则 $||\vec{x}-\vec{r}||_{2}=\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2}$

在Frenet坐标系下，每个点到base frame上参考点的向量都与该参考点的法向量 $\vec{N}$ 同向或反向，因此， $\sin(\theta_{x-r})\cos(\theta_{r})-\cos(\theta_{x-r})\sin(\theta_{r})=1 或 -1$ 。 $\theta_{x-r}$ 为向量 $\vec{x}-\vec{r}$ 的角度，因此， $\frac{\sin(\theta_{x-r})}{\cos(\theta_{x-r})}=\frac{y_{x}-y_{r}}{x_{x}-x_{r}}$ 。可以根据 $\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2}$ 来确定 $l$ 的绝对取值， $(y_{x}-y_{r})\cos(\theta_{r})-(x_{x}-x_{r})\sin(\theta_{r})$ 的正负来确定 $l$ 的正负号，公式如下：

$sign\left((y_{x}-y_{r})\cos(\theta_{r})-(x_{x}-x_{r})\sin(\theta_{r})\right)\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2} \tag{2}$

已知 $\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x$ 推导 $l,\dot{s},\dot{l},l']$

求 $\dot{l}$ （根据 $v_x$ 与 $\dot{s}$ 定义，有 $\dot{\vec{r}}=\dot{s}\vec{T_r}$ 和 $\dot{\vec{x}}=v_x \vec{T_x}$ ）
由 $\vec{x}$ 与 $\vec{r}$ 的关系 $\vec{x}=\vec{r}+l\vec{N_{r}}$ ，可得 $l=(\vec{x}-\vec{r})^{T}\vec{N_{r}}$ ，两边对时间 $t$ 求导得如下等式:
$\dot{l}=(\dot{\vec{x}}-\dot{\vec{r}})^{T}\vec{N_{r}}+(\vec{x}-\vec{r})^{T}\dot{\vec{N_{r}}} \\ = v_{x}\vec{T_{x}}^{T}\vec{N_{r}}-\dot{s}\vec{T_{r}}^{T}\vec{N_{r}}+l\vec{N_{r}}^{T}\dot{\vec{N_{r}}}$

由Frenet公式（见上式(1)）有 $\vec{N_{r}}'=\frac{d\vec{N_{r}}}{ds}=-k_{r}\vec{T_{r}}$ ，于是有 $\dot{\vec{N_{r}}}=\frac{ds}{dt}\frac{d\vec{N_{r}}}{ds}=-k_{r}\dot{s}\vec{T_{r}}$ ，代入上式可得：

$\dot{l}=v_{x}\vec{T_{x}}^{T}\vec{N_{r}}-\dot{s}\vec{T_{r}}^{T}\vec{N_{r}}-l\dot{s}k_{r}\vec{N_{r}}^{T}\vec{T_{r}}$

由于 $\vec{N_{r}}$ 与 $\vec{T_{r}}$ 正交 $\Rightarrow \vec{N_{r}}^{T}\vec{T_{r}}=\vec{T_{r}}^{T}\vec{N_{r}}=0$ ， $\vec{T_{x}}^{T}\vec{N_{r}}=\cos(\theta_{x}-\theta_{r}-\frac{\pi}{2})=\sin(\theta_{x}-\theta_{r})$ ,代入上式得：
$\dot{l}=v_{x}\sin(\theta_{x}-\theta_{r}) \tag{3}$

求 $\dot{s}$
由 $\vec{x}$ 与 $\vec{r}$ 的关系 $\vec{x}=\vec{r}+l\vec{N_{r}}$ ，对等式两边求导可得： $v_x\vec{T_x}=\dot{s}\vec{T_r}+\dot{l}\vec{N_r}-\dot{s}k_r l\vec{T_r}$

上式两边左乘 $\vec{T_r}$ 得到 $v_x\cos(\theta_x-\theta_r)=\dot{s}(1-k_r l)$ ，整理可得：
$\dot{s}=\frac{v_x \cos(\theta_x-\theta_r)}{1-k_r l} \tag{4}$

求 $l^{'}$

$l'=\frac{dl}{ds}=\frac{\dot{l}}{\dot{s}}$ ，将 $\dot{l}$ 与 $\dot{s}$ 代入得：

$l'=(1-k_{r}l)\tan(\theta_{x}-\theta_{r}) \tag{5}$

已知 $\vec{x}=[x_{x}, y_{x}],\theta_{x},v_x,a_x,k_{x}$ 推导 $l,\dot{s},l',\ddot{s},l'']$

求 $l^{''}$

由 $l'=(1-k_{r}l)\tan(\theta_{x}-\theta_{r})$ ，可得 $l''=\frac{dl'}{ds}=(1-k_{r}l)'\tan(\theta_{x}-\theta_{r})+(1-k_{r}l)\frac{(\theta_{x}-\theta_{r})'}{\cos^{2}(\theta_{x}-\theta_{r})}$

假定 $s_{x}$ 为为车辆当前轨迹 $\vec{x}$ 的弧长，有：
$\frac{d}{ds}=\frac{ds_{x}}{dt}\frac{dt}{ds}\frac{d}{ds_{x}}=\frac{v_{x}}{\dot{s}}\frac{d}{ds_{x}}=\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}\frac{d}{ds_{x}}$

又有曲率: $k_{r}=\frac{d\theta_{r}}{ds}$ , $k_{x}=\frac{d\theta_{x}}{ds_{x}}$

故： $l''=\frac{dl'}{ds}=-(k_{r}'l+k_{r}l')\tan(\theta_{x}-\theta_{r})+\frac{(1-k_{r}l)}{\cos^2(\theta_{x}-\theta_{r})}(\frac{d\theta_{x}}{ds}-\frac{d\theta_{r}}{ds})$

由于: $\frac{d\theta_{x}}{ds}=\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}\frac{d\theta_{x}}{ds_{x}}=\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}k_{x}$
综上可得：

$l''=-(k_{r}'l+k_{r}l')\tan(\theta_{x}-\theta_{r})+\frac{(1-k_{r}l)}{\cos^2(\theta_{x}-\theta_{r})}(\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}k_{x}-k_{r})\tag{6}$

$a_{x}=\frac{dv_{x}}{dt}=\ddot{s}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}+\frac{\dot{s}^{2}}{\cos(\theta_{x}-\theta_{r})}\left[l'(k_{x}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}-k_{r})-(k_{r}'l+k_{r}l')\right]$

$\ddot{s}=\frac{a_{x}\cos(\theta_{x}-\theta_{r})-\dot{s}^{2}\left[l'(k_{x}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}-k_{r})-(k_{r}'l+k_{r}l')\right]}{1-k_{r}l}\tag{7}$

综合以上可得转换公式

Cartesian转 Frenet
$\left\{\begin{matrix}s=& s_{r} \\ \dot{s}=& \frac{v_{x}\cos(\theta_{x}-\theta_{r})}{1-k_{r}l} \\ \ddot{s}=& \frac{a_{x}\cos(\theta_{x}-\theta_{r})-\dot{s}^{2}\left[l'(k_{x}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}-k_{r})-(k_{r}'l+k_{r}l')\right]}{1-k_{r}l} \\ l=& sign\left((y_{x}-y_{r})\cos(\theta_{r})-(x_{x}-x_{r})\sin(\theta_{r})\right)\sqrt{(x_{x}-x_{r})^2+(y_{x}-y_{r})^2} \\ l'=& (1-k_{r}l)\tan(\theta_{x}-\theta_{r}) \\ l''= &-(k_{r}'l+k_{r}l')\tan(\theta_{x}-\theta_{r})+\frac{(1-k_{r}l)}{\cos^2(\theta_{x}-\theta_{r})}(\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}k_{x}-k_{r}) \end{matrix}\right. \tag{8}$
Frenet转Cartesian
$\left\{\begin{matrix} x_{x}= & x_{r}-l\sin(\theta_{r}) \\ y_{x}=& y_{r}+l\cos(\theta_{r}) \\ \theta_{x}=& \arctan(\frac{l'}{1-k_{r}l})+\theta_{r} \in [-\pi, \pi] \\ v_{x}= &\sqrt{[\dot{s}(1-k_{r}l)]^{2}+(\dot{s}l')^{2}} \\ a_{x}= &\ddot{s}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}+\frac{\dot{s}^{2}}{\cos(\theta_{x}-\theta_{r})}\left[l'(k_{x}\frac{1-k_{r}l}{\cos(\theta_{x}-\theta_{r})}-k_{r})-(k_{r}'l+k_{r}l')\right] \\ k_{x}= & ((l''+(k_{r}'l+k_{r}l')\tan(\theta_{x}-\theta_{r}))\frac{\cos^{2}(\theta_{x}-\theta_{r})}{1-k_{r}l}+k_{r})\frac{\cos(\theta_{x}-\theta_{r})}{1-k_{r}l} \end{matrix}\right. \tag{9}$

cartesian转Frenet一般计算步骤

问题：已知 $(x_x, y_x, \theta_x, v_x, a_x, k_x), (s_r, x_r, y_r, \theta_r, k_r, k_r'),$ 求 $\dot{s}, \ddot{s},l,l', l''$

step 1：计算 $d_x=x_x-x_r,d_y=y_x-y_r,d_{\theta}=\theta_x-\theta_r$ ;
step 2：计算 $C_{drdx}=d_y\cos(\theta_r)-d_x\sin(\theta_r)$ ;
step 3：计算 $l=sign(C_drdx)\sqrt{d_x^2+d_y^2}$ ;
step 4：计算 $C_{1-k_r l}=1-k_r l$ ;
step 5：计算 $l'=C_{1-k_r l}\tan(d_{\theta})$ ;
step 6：计算 $C_{k_rl'}=k'_rl+k_rl'$ ;
step 7：计算 $l''=-C_{k_r l'}\tan(d_{\theta})+\frac{C_{1-k_r l}}{\cos(d_{\theta})}(\frac{k_xC_{1-k_r l}}{\cos(d_{\theta})}-k_r)$ ;
step 8：计算 $s=s_r$ ;
step 9：计算 $\dot{s}=\frac{v_x \cos(d_{\theta})}{C_{1-k_r l}}$ ;
step 10：计算 $C_{d_{\theta}'}=\frac{C_{1-k_r l}}{k_x\cos(d_{\theta})}-k_r$ ;
step 11：计算 $\ddot{s}=\frac{a_x\cos(d_{\theta})-\dot{s}^2(l'C_{d_{\theta}'}-C_{k_r l'})}{C_{1-k_r l}}$

Frenet坐标系与Cartesian坐标系互转（二）：Python代码函数实现

Frenet坐标系与Cartesian坐标系互转（三）：应用示例

END
by windSeS
first update 2020.9.23
modified at 2022.5.19

同时也希望大家多多支持本人第一个付费专栏《自动驾驶规划入门（已完结）》，满足我一点点的虚荣心=v=。

windSeS

关注

225
点赞
踩
1048

收藏

觉得还不错? 一键收藏
打赏
54
评论
Frenet坐标系与Cartesian坐标系互转（一）：公式推导

Frenet坐标系在无人驾驶领域被普遍使用，特别是在城市、高速等道路交通环境下无人驾驶的路径规划系统中。Frenet坐标系使用道路的中心线作为Base frame，使用参考线的切线向量和法线向量建立坐标系。相比笛卡尔坐标系，Frenet坐标系简化了路径规划问题。网上关于Frenet坐标系的博客或资料很多，此处不再赘述。本文先给出简单的转换公式推导过程，然后给出三对Frenet坐标系与Cartesian坐标系互转的python代码与使用示例方便自己查阅使用。详细推导参见博客Apollo项目坐标系研究。not.
复制链接

扫一扫