笛卡尔坐标系转frenet坐标系

引言

在自动驾驶领域，笛卡尔坐标系和frenet坐标系是两种非常常用的坐标系，他们两种方式应用在自动驾驶领域各有千秋，互为补充。今天我们就来浅析一下这两种坐标系。

笛卡尔坐标系

笛卡尔坐标系，是一个我们从小学开始就有学到的坐标系，法国数学家、哲学家勒内·笛卡尔于1637年在《几何》（_La Géométrie_）中发表了关于笛卡尔坐标系的研究[1]。

笛卡尔坐标系包括二维、三维坐标系统等，在笛卡尔坐标系中，两点之间满足经典的平移，旋转，仿射变换等运动规律。一般我们说的笛卡尔坐标系都是右手系。

讨论笛卡尔坐标系，我们还需要知道一个坐标系：UTM坐标系。

UTM坐标系将地球分成60个纬度带，每个纬度带都有自己的投影坐标系。在自动驾驶领域，通常需要首先获取自己所在纬度带的原点坐标，然后将其作为当前的笛卡尔坐标系的原点，进而得到每个位置点在当前笛卡尔坐标系下的UTM坐标。至于为什么需要先转为UTM坐标系，我的理解是为了和GPS等数据进行对应。

frenet坐标系

frenet坐标系由Frenet–Serret 公式所定义，描述了描述了曲线的切向，法向，副法方向之间的关系，是法国数学家于1847提出。其中的单位切向量 T，单位法向量 N，单位副法向量 B，被称作 frenet标架[2]。

本期内容的核心就是讨论如何把点从笛卡尔坐标系转到frenet坐标系，以及这样转换的几何意义。

frenet标架

由于frenet坐标系主要应用于车辆的路径规划阶段，在路径规划阶段，一般不需要根据当前位置的z轴信息进行特别处理，因此这里仅关注平面的单位切向量和单位法法向量的微分公式。

dT/ds

对于单位切向量对于路径s的微分，他的结果等于当前位置曲线的曲率*单位法向量，下图展示了这个公式的几何理解。

曲率：描述几何体弯曲程度的量；直观地说，曲率是曲线偏离直线的量)（程度）[3]。wiki中原话是这样描述的：

柯西这样定义密切圆和曲率：对曲线C一点P，在其附近再找C上的两个点P1,P2，分别过P1,P2作出曲线C的法线，两条法线会有一个交点。当P1,P2无限接近于点P时，相应的交点有一个极限，以这个极限点为圆心，过点P作圆，就是曲线C在点P处的密切圆，密切圆的半径的倒数就是曲率。

因此这样我们就比较清楚了，切向量表示的就是当前点的前进方向，当这个方向发生变化时，曲线必然发生弯曲，而曲线发生弯曲的程度就可以用曲率表示，而发生弯曲的方向就是法线方向。

然后我们需要注意的是这里的曲率是有正负的[4]。

dN/ds

和前面一样，这里我们只讨论二维的情况，因此

\frac{dN}{ds}=-kT

这个再结合前面图中的曲率方向，就可以得到单位法向量对s的导数。

笛卡尔转frenet坐标系

以上介绍了frenet标架，然后我们就可以根据frenet标架，得到frenet下各个量的表示，这里仅推导s，n以及他们对时间的导数

n

设置待转换点为P

n表示的是横向偏移，具体来讲，这个横向偏移指的是其与参考线上match点的横向偏移，当然，在参考线上的n都为0

1、在实际应用时，我们首先要计算P在ref上的投影点，这个投影点可以根据P与ref中离散点的模大小得到，即得到closedmin点

2、通过这个点处的heading \theta_r，得到矢量n（其实第一步计算模的时候已经得到了距离dis，但是那个是标量），从公式来看，它的值为：

-(P_x - R_x)_sin(\theta_r)+(P_y - R_y)_cos(\theta_r)

使用向量表示就是

n = (x-r)^T*Nr

s

frenet是移动坐标系，其由每个路径点的单位切向量和单位法向量组成基底，进而组成那个点处的frenet坐标系，我们可以把这里的s认为是frenet坐标系的原点坐标。

因此在计算的时候，只需要将各个离散点之间的距离累加，就可以得到每个点处的s值。

$\dot n$

由n的公式我们已经知道了n = (x-r)^T*Nr,接下来根据链式求导法则：

\dot n = (\dot x-\dot r)^T Nr + (x-r)^T \dot Nr

=v_xT_x^TN_r - \dot sT_r^TN_r+nN_r^T\dot N_r

根据\frac{dN_r}{ds}=-kT_r,得

\frac{dN_r}{dt}=\frac{dN_r}{ds}\frac{ds}{dt}

=-kT_r\dot s

=v_xT_x^TN_r - \dot sT_r^TN_r-n\dot sN_r^TkT_r

根据基向量正交，得到

\dot n = v_xT_x^TN_r

其中

x表示P点广义二维坐标；

vx为P点合速度；

Tx为P点处纵向速度，也是heading方向的速度；

Nr为P点在参考线match点的法线方向。

Tx=[\begin{matrix}cos(\theta_{ego}) & sin(\theta_{ego}) \end{matrix}]

Nr=[\begin{matrix}-sin(\theta_{ref}) & cos(\theta_{ref}) \end{matrix}]

因此

\dot n = v_x*sin(\theta_{ego}-\theta_{ref})

=v_x*sin(\alpha)

dot s

首先根据n = (x-r)^T*Nr，得到

x=r+nN_r^T

两边求导得到

v_xT_x=\dot sT_r + n\dot N_r^T + \dot nN_r

由\frac{dN_r}{dt}=\frac{dN_r}{ds}\frac{ds}{dt}

=-kT_r\dot s

得

v_xT_x=\dot sT_r-n\dot skT_r+\dot n N_r

两边右乘T_r^T,得

v_xcos(\alpha)=\dot s - \dot snk

移项，得：

\dot s = \frac{v_xcos(a)}{1-nk}

结语

本届花了不少的篇幅描述笛卡尔坐标系转frenet坐标系的细节，后面讲继续分享frenet坐标系相关知识。

参考文献

[1]https://zh.wikipedia.org/zhhans/%E7%AC%9B%E5%8D%A1%E5%B0%94%E5%9D%90%E6%A0%87%E7%B3%BB
[2]https://zh.wikipedia.org/wiki/%E5%BC%97%E8%8E%B1%E7%BA%B3%E5%85%AC%E5%BC%8F
[3]https://zh.wikipedia.org/wiki/%E6%9B%B2%E7%8E%87
[4]https://blog.csdn.net/weixin_40959779/article/details/102509491

May The force be with you!