百度Apollo项目用到了多种坐标系，其中帮助文档提及的坐标系包括：

全球地理坐标系（The Global Geographic coordinate system ）
局部坐标系—东-北-天坐标（The Local Frame – East-North-Up，ENU）
车身坐标系—右-前-天坐标（The Vehicle Frame —Right-Forward-Up，RFU）
车身坐标系—前-左-天坐标（The Vehicle Frame —Front-Left-Up，FLU）
还有一种Frenet坐标系（又称Frenet–Serret公式，Apollo项目文档未提及，但在Apollo项目的规划模块中得到广泛使用。)

本文首先简介Apollo项目提及的三种坐标系，之后重点介绍Frenet坐标系及其与车辆坐标系之间的转换公式，最后对Apollo项目中Frenet坐标系与车辆坐标系的转换代码进行详细解释。

一、Apollo文档提及的坐标系

（一）全球地理坐标系

Apollo项目使用全球地理坐标系表示高精地图（ the high-definition map，HD Map）中诸元素的几何位置，通常包括：纬度（latitude）、经度（longitude）和海拔（elevation）。

全球地理坐标系普遍采用地理信息系统（Geographic Information System，GIS）中用到的WGS-84坐标系（the World Geodetic System dating from 1984），如下图所示，注意海拔定义为椭球体高程（the ellipsoidal height）：

（二）局部坐标系—东-北-天坐标（ENU)

局部坐标系定义为：

X轴：指向东边
Y轴：指向北边
Z轴：指向天顶

如下图所示：

ENU局部坐标系采用三维直角坐标系来描述地球表面，实际应用较为困难，因此一般使用简化后的二维投影坐标系来描述。

在众多二维投影坐标系中，统一横轴墨卡托（The Universal Transverse Mercator ，UTM）坐标系是一种应用较为广泛的一种。UTM 坐标系统使用基于网格的方法表示坐标，它将地球分为 60 个经度区，每个区包含6度的经度范围，每个区内的坐标均基于横轴墨卡托投影，如下图所示：

（三）车身坐标系—右-前-天坐标（RFU）

车身坐标系—右-前-天坐标（RFU）的定义如下：

X轴：面向车辆前方，右手所指方向
Y轴：车辆前进方向
Z轴：与地面垂直，指向车顶方向

注意：车辆参考点为后轴中心。该坐标系一般用于感知模块。

如下图所示：

（四）车身坐标系—前-左-天坐标（FLU）

车身坐标系—前-左-天坐标（FLU）的定义如下：

X轴：车辆前进方向
Y轴：面向车辆前方，左手所指方向
Z轴：与地面垂直，指向车顶方向

注意：车辆参考点为后轴中心。

该坐标系常用于实时相对地图模块。将RFU坐标系向左旋转90度即可得到FLU坐标系。

二、Frenet坐标系

Frenet坐标系又称Frenet–Serret公式，Apollo项目文档未提及，但在规划模块中广泛使用。Frenet–Serret公式用于描述粒子在三维欧氏空间 R3R3内沿一条连续可微曲线的运动学特征，如下图所示：

其中，
T⃗\vec{T}T称为切向量（tangent），表示沿曲线运动方向的单位向量；
N⃗\vec{N}N称为法向量（normal），表示当前曲线运动平面内垂直于T⃗\vec{T}T的单位向量；
B⃗\vec{B}B称为副法向量（binormal），表示同时垂直于T⃗\vec{T}T和N⃗\vec{N}N的单位向量。
令R⃗(t)\vec{R}(t)R(t)为欧氏空间内随 t 改变的一条非退化曲线。
所谓非退化曲线就是一条不会退化为直线的曲线，亦即曲率不为0的曲线。
令 s(t)s(t)s(t) 是 ttt 时刻时曲线的累计弧长，其定义如下：
s(t)=∫0t∥r′(σ)∥dσ(1)s(t)=\int^{t}_{0} \Vert r^{'}(\sigma) \Vert d\sigma \tag{1} s(t)=∫0t∥r′(σ)∥dσ(1)
假定 r′≠0r^{'} \neq 0r′=0，则意味着s(t)s(t)s(t)是严格单调递增函数。因此可将ttt表示为sss的函数，从而有：r⃗(s)=r⃗(t(s))\vec{r}(s) = \vec{r}(t(s))r(s)=r(t(s))，这样我们就把曲线表示为弧长 sss 的函数。

对于采用弧长参数sss表示的非退化曲线r⃗(s)\vec{r}(s)r(s)，我们定义其切向量T⃗\vec{T}T、法向量N⃗\vec{N}N、副法向量B⃗\vec{B}B如下：

T⃗=dr⃗ds∥dr⃗ds∥N⃗=dT⃗ds∥dT⃗ds∥B⃗=T⃗×N⃗\vec{T} = \frac{\frac{d\vec{r}}{ds}}{\Vert\frac{d\vec{r}}{ds}\Vert} \\ \space \\ \vec{N} = \frac{\frac{d\vec{T}}{ds}}{\Vert\frac{d\vec{T}}{ds}\Vert} \\ \space \\ \vec{B}=\vec{T} \times \vec{N} T=∥dsdr∥dsdr N=∥dsdT∥dsdT B=T×N

基于上述定义的Frenet–Serret公式表示为：

dT⃗ds=kN⃗dN⃗ds=−kT⃗+τN⃗dB⃗ds=−τN⃗\frac{d\vec{T}}{ds} = k\vec{N} \\ \space \\ \frac{d\vec{N}}{ds} = -k\vec{T} + \tau\vec{N} \space \\ \frac{d\vec{B}}{ds} = -\tau\vec{N} \space \\ dsdT=kN dsdN=−kT+τN dsdB=−τN

其中κκκ、τττ分别表示曲线r⃗(s)\vec r(s)r(s)的曲率（ curvature）和挠率（ torsion）。直观地讲，曲率是曲线不能形成一条直线的度量值，曲率越趋于0，则曲线越趋近于直线；挠率是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。

令T′⃗=dT⃗ds\vec{T^{'}}=\frac{d\vec{T}}{ds}T′=dsdT，N′⃗=dN⃗ds\vec{N^{'}}=\frac{d\vec{N}}{ds}N′=dsdN，B′⃗=dB⃗ds\vec{B^{'}}=\frac{d\vec{B}}{ds}B′=dsdB，则Frenet–Serret公式的矩阵表示形式为：

[T′⃗N′⃗B′⃗]=[0k0−k0τ0τ0][T⃗N⃗B⃗](1)\begin{bmatrix} \vec{T^{'}} \\ \vec{N^{'}} \\ \vec{B^{'}} \\ \end{bmatrix}= \begin{bmatrix} 0 & k & 0\\ -k & 0 & τ\\ 0 & τ & 0\\ \end{bmatrix} \begin{bmatrix} \vec{T} \\ \vec{N} \\ \vec{B} \\ \end{bmatrix} \tag{1} ⎣⎢⎡T′N′B′⎦⎥⎤=⎣⎡0−k0k0τ0τ0⎦⎤⎣⎡TNB⎦⎤(1)

易见参数矩阵是反对称（ skew-symmetric）的。

对于无人驾驶车辆而言，一般对高度信息不感兴趣，因此可以将车辆运动曲线投影到同一平面内，亦即τ=0τ=0τ=0，这样Frenet–Serret公式就可以简化为：

[T′⃗N′⃗]=[0k−k0][T⃗N⃗](2)\begin{bmatrix} \vec{T^{'}} \\ \vec{N^{'}} \\ \end{bmatrix}= \begin{bmatrix} 0 & k\\ -k & 0\\ \end{bmatrix} \begin{bmatrix} \vec{T} \\ \vec{N} \\ \end{bmatrix} \tag{2} [T′N′]=[0−kk0][TN](2)

三、Frenet坐标系与笛卡尔坐标系的转换公式

为什么要将笛卡尔坐标系转换为Frenet坐标系？因为可以这样可以将车辆的二维运动问题解耦为两个一维运动问题。显然，一维问题比二维问题容易求解，这就是笛卡尔坐标系转换为Frenet坐标系的必要性。

前已述及，在不考虑高度信息的前提下，Frenet坐标系可简化为由曲线切向量T⃗\vec{T}T与法向量N⃗\vec{N}N组成的二维直角坐标系。如下图所示，假定r⃗(s)\vec{r}(s)r(s)是参考线（reference line）在弧长sss处的位置，x⃗\vec{x}x是当前车辆轨迹（trajectory）点，该向量一般采用笛卡尔坐标系(常用ENU坐标)表示 x⃗=[x,y,z]T\vec{x}=[x,y,z]^{T}x=[x,y,z]T,zzz坐标一般忽略，但这里我们采用弧长sss和横向偏移lll（即沿当前参考线位置r⃗(s)\vec{r}(s)r(s)法线方向N⃗r\vec{N}_rNr的偏移量）对其描述，即x⃗=x⃗(s,l)\vec{x}=\vec{x}(s,l)x=x(s,l)。

令θr\theta_rθr、T⃗r\vec{T}_rTr、N⃗r\vec{N}_rNr分别为当前参考线r⃗(s)\vec{r}(s)r(s)的方位角、单位切向量和单位法向量，θx\theta_xθx、T⃗x\vec{T}_xTx、N⃗x\vec{N}_xNx分别为当前轨迹点x⃗(s,l)\vec{x}(s,l)x(s,l)的方位角、单位切向量和单位法向量，根据正交基的定义有：
T⃗r=[cosθrsinθr]TN⃗r=[−sinθrcosθr]TT⃗x=[cosθxsinθx]TN⃗x=[−sinθxcosθx]T\vec{T}_r=[cos \theta_r \space \space sin \theta_r]^T \\ \vec{N}_r=[-sin \theta_r \space \space cos \theta_r]^T \\ \vec{T}_x=[cos \theta_x \space \space sin \theta_x]^T \\ \vec{N}_x=[-sin \theta_x \space \space cos \theta_x]^T \\ Tr=[cosθr sinθr]TNr=[−sinθr cosθr]TTx=[cosθx sinθx]TNx=[−sinθx cosθx]T

根据平面几何知识易知：

x⃗(s,l)=r⃗(s)+l(s)N⃗r(s)(3)\vec{x}(s,l)=\vec{r}(s)+l(s)\vec{N}_r(s) \tag{3}x(s,l)=r(s)+l(s)Nr(s)(3)

从而有（为简洁起见，下面的推导过程均省略参数）：

lN⃗rTN⃗r=N⃗rT[r⃗−x⃗]l=N⃗rT[x⃗−r⃗]=[r⃗−x⃗]N⃗r(4)l\vec{N}^{T}_{r}\vec{N}_{r}=\vec{N}^{T}_{r}[\vec{r}-\vec{x}] \\ \space \\ l=\vec{N}^{T}_{r}[\vec{x}-\vec{r}]=[\vec{r}-\vec{x}]\vec{N}_{r} \tag{4} lNrTNr=NrT[r−x] l=NrT[x−r]=[r−x]Nr(4)

(4)式在数学上美观，但不好编程实现，因此换一个表示方法。
设x⃗,r⃗\vec{x},\vec{r}x,r的笛卡尔坐标分别为：(x,y)(x,y)(x,y)、(xr,yr)(x_r,y_r)(xr,yr)，根据两点间距离公式及N⃗r\vec{N}_rNr的定义，可得：

l=±(x−xr)2+(y−yr)2,[(y−yr)cosθr−(x−xr)sinθr]>0?positive:negtive(4.a)l=\pm \sqrt{(x-x_r)^2+(y-y_r)^2}, \\ [(y-y_r)cos\theta_r-(x-x_r)sin\theta_r]>0 \space ? \space positive:negtive \tag{4.a} l=±(x−xr)2+(y−yr)2,[(y−yr)cosθr−(x−xr)sinθr]>0 ? positive:negtive(4.a)

设aaa为任意一个变量（或向量），记
a˙=d(a)dta¨=d(a˙)dta′=d(a)dsa′′=d(a′)ds\dot{a}=\frac{d(a)}{dt} \\ \space \\ \ddot{a}=\frac{d(\dot{a})}{dt} \\ \space \\ a^{'}=\frac{d(a)}{ds} \\ \space \\ a^{''}=\frac{d(a^{'})}{ds} a˙=dtd(a) a¨=dtd(a˙) a′=dsd(a) a′′=dsd(a′)
根据上式可知，约定a˙\dot{a}a˙表示aaa对时间ttt的一阶求导，a¨\ddot{a}a¨表示aaa对时间ttt的二阶求导，a′a^{'}a′表示aaa对弧长sss的一阶求导，a′′a^{''}a′′表示aaa对弧长sss的二阶求导，有：

l˙=[x⃗˙−r⃗˙]TN⃗r+[x⃗−r⃗]TN⃗r˙\dot{l}=[\dot{\vec{x}}-\dot{\vec{r}}]^{T}\vec{N}_r+[{\vec{x}}-{\vec{r}}]^{T}\dot{\vec{N}_r} l˙=[x˙−r˙]TNr+[x−r]TNr˙

根据单位切向量和单位法向量的定义（参见第二部分内容），有：

x⃗˙=d∥x⃗∥dtT⃗x=vxT⃗x(5)\dot{\vec{x}}=\frac{d\Vert{\vec{x}}\Vert}{dt}\vec{T}_{x}=v_{x}\vec{T}_{x} \tag{5} x˙=dtd∥x∥Tx=vxTx(5)
r⃗˙=d∥r⃗∥dtT⃗r=s˙T⃗r(6)\dot{\vec{r}}=\frac{d\Vert{\vec{r}}\Vert}{dt}\vec{T}_{r}=\dot{s}\vec{T}_{r} \tag{6} r˙=dtd∥r∥Tr=s˙Tr(6)

根据平面几何知识可知：

x⃗−r⃗=lN⃗r(7)\vec{x}-\vec{r} = l\vec{N}_r \tag{7} x−r=lNr(7)

根据链式求导法则，N⃗˙r=dN⃗rdsdsdt\dot{\vec{N}}_{r}=\frac{d\vec{N}_r}{ds} \frac{ds}{dt}N˙r=dsdNrdtds，又由二维Frenet–Serret公式可知：N⃗r′=−krT⃗r\vec{N}^{'}_{r}=-k_r\vec{T}_rNr′=−krTr，于是有：

N⃗˙r=−krs˙T⃗r(8)\dot{\vec{N}}_{r}=-k_r\dot{s}\vec{T}_{r} \tag{8} N˙r=−krs˙Tr(8)

将(5)-(8)式代入，得：

l˙=[vxT⃗x−s˙T⃗r]TN⃗r+lN⃗rT(−krs˙T⃗r)\dot{l}=[v_x \vec{T}_{x}-\dot{s}\vec{T}_{r}]^{T}\vec{N}_r + l\vec{N}_{r}^{T}(-k_r\dot{s}\vec{T}_r) l˙=[vxTx−s˙Tr]TNr+lNrT(−krs˙Tr)

因为单位切向量和法向量正交，于是有：T⃗rTN⃗r=0,N⃗rTT⃗r=0\vec{T}^{T}_{r}\vec{N}_{r}=0,\vec{N}^{T}_{r}\vec{T}_{r}=0TrTNr=0,NrTTr=0，故：

l˙=vxT⃗xTN⃗r\dot{l}=v_{x}\vec{T}^{T}_{x}\vec{N}_{r} l˙=vxTxTNr

将T⃗x=[cosθxsinθx]\vec{T}_x=[cos\theta_x \space\space sin\theta_x]Tx=[cosθx sinθx]和 N⃗r=[−sinθrcosθr]\vec{N}_r=[-sin\theta_r \space\space cos\theta_r]Nr=[−sinθr cosθr]代入，得到：

l˙=vx[−cosθxsinθr+sinθxcosθr]=vxsinΔθ(9)\dot{l}=v_x[-cos\theta_xsin\theta_r+sin\theta_xcos\theta_r]=v_xsin\Delta\theta \tag{9} l˙=vx[−cosθxsinθr+sinθxcosθr]=vxsinΔθ(9)

上式中:

Δθ=θx−θr(10)\Delta\theta=\theta_x-\theta_r \tag{10} Δθ=θx−θr(10)

现在来计算vxv_xvx，根据定义：vx=d∥x⃗∥dt=∥dx⃗dt∥=∥x⃗˙∥v_x=\frac{d\Vert\vec{x}\Vert}{dt}=\Vert\frac{d\vec{x}}{dt}\Vert=\Vert\dot{\vec{x}}\Vertvx=dtd∥x∥=∥dtdx∥=∥x˙∥，下面先计算x˙\dot{x}x˙。由（3）式，有：

x⃗˙=d(r⃗+lN⃗r)dt=r⃗˙+l˙N⃗r+lN⃗r˙\dot{\vec{x}}=\frac{d(\vec{r}+l\vec{N}_r)}{dt}=\dot{\vec{r}}+\dot{l}\vec{N}_r+l\dot{\vec{N}_r} x˙=dtd(r+lNr)=r˙+l˙Nr+lNr˙

将(6)式和(8)式代入，得到：

x⃗˙=s˙(1−kr)lT⃗r+l˙N⃗r(11)\dot{\vec{x}}=\dot{s}(1-k_r)l{\vec{T}_r}+\dot{l}\vec{N}_r \tag{11} x˙=s˙(1−kr)lTr+l˙Nr(11)

于是有：

vx=∥x⃗˙∥=x⃗˙Tx⃗˙=[s˙(1−kr)l]2+l˙2(12)v_x=\Vert \dot{\vec{x}} \Vert=\sqrt{\dot{\vec{x}}^{T}\dot{\vec{x}}}=\sqrt{[\dot{s}(1-k_r)l]^{2}+\dot{l}^2} \tag{12} vx=∥x˙∥=x˙Tx˙=[s˙(1−kr)l]2+l˙2(12)

下面计算l′l^{'}l′：

l′=dlds=dldt/dsdt=l˙s˙l^{'}=\frac{dl}{ds}=\frac{dl}{dt}/\frac{ds}{dt}=\frac{\dot{l}}{\dot{s}} l′=dsdl=dtdl/dtds=s˙l˙

将(10)式代入，得到：

l′=vxs˙sin⁡Δθ(13)l^{'}=\frac{v_x}{\dot{s}}\sin\Delta\theta \tag{13} l′=s˙vxsinΔθ(13)

再将(12)式代入，得到：

l′=(1−krl)2+l′2sin⁡Δθl′2=[(1−krl)2+l′2]sin⁡2Δθl′2(1−sin⁡2Δθ)=(1−krl)2sin⁡2Δθl^{'}=\sqrt{(1-k_rl)^2+l^{'2}}\sin\Delta\theta \\ \space \\ l^{'2}=[(1-k_rl)^2+l^{'2}]\sin^2\Delta\theta \\ \space \\ l^{'2}(1-\sin^2\Delta\theta)=(1-k_rl)^2\sin^2\Delta\theta l′=(1−krl)2+l′2sinΔθ l′2=[(1−krl)2+l′2]sin2Δθ l′2(1−sin2Δθ)=(1−krl)2sin2Δθ

假定车辆实际轨迹一直沿参考线附近运动（即不做与参考线反向的运动），使得：

∣Δθ∣<π/21−krl>0\vert \Delta\theta\vert<\pi/2 \\ \space \\ 1-k_rl>0 \\ ∣Δθ∣<π/2 1−krl>0

则求解上述关于l′l^{'}l′的方程得到：

l′=(1−krl)tan⁡Δθ(14)l^{'}=(1-k_rl)\tan\Delta\theta \tag{14} l′=(1−krl)tanΔθ(14)

将(14)式代入(13)式，可得到速度vxv_xvx的表达式：

(1−krl)tanΔθ=vxs˙sin⁡Δθvx=s˙1−krlcos⁡Δθ(15)(1-k_rl)tan\Delta\theta=\frac{v_x}{\dot{s}}\sin\Delta\theta \\ \space \\ v_x=\dot{s}\frac{1-k_rl}{\cos\Delta\theta} \tag{15} (1−krl)tanΔθ=s˙vxsinΔθ vx=s˙cosΔθ1−krl(15)

令sxs_xsx为车辆当前轨迹x⃗\vec{x}x的弧长，则有：

dds=ddsxdsxds=ddsxdsxdtdtds=vxs˙ddsx\frac{d}{ds}=\frac{d}{ds_x}\frac{ds_x}{ds}=\frac{d}{ds_x}\frac{ds_x}{dt}\frac{dt}{ds}=\frac{v_x}{\dot{s}}\frac{d}{ds_x} dsd=dsxddsdsx=dsxddtdsxdsdt=s˙vxdsxd

将(15)式代入，得到：

dds=1−krlcos⁡Δθddsx(16)\frac{d}{ds}=\frac{1-k_rl}{\cos\Delta\theta}\frac{d}{ds_x} \tag{16} dsd=cosΔθ1−krldsxd(16)

对(14)式求关于参考线弧长sss的偏导：

l′′=(1−krl)′tan⁡Δθ+(1−krl)(Δθ)′cos⁡2Δθ(17)l^{''}=(1-k_rl)^{'}\tan\Delta\theta+\frac{(1-k_rl)(\Delta\theta)^{'}}{\cos^2\Delta\theta} \tag{17} l′′=(1−krl)′tanΔθ+cos2Δθ(1−krl)(Δθ)′(17)

注意到：Δθ=θx−θr\Delta\theta = \theta_x-\theta_rΔθ=θx−θr，于是有：
(Δθ)′=ddsθx−θr′(\Delta\theta)^{'}=\frac{d}{ds}\theta_x-\theta^{'}_{r} (Δθ)′=dsdθx−θr′
根据曲率的定义：kr=θr′=dθrdsk_r=\theta^{'}_{r}=\frac{d\theta_r}{ds}kr=θr′=dsdθr，kx=dθxdsxk_x=\frac{d\theta_x}{ds_x}kx=dsxdθx并将(16)式代入，得到：

(Δθ)′=1−krlcos⁡Δθddsθx−θr′(Δθ)′=kx1−krlcos⁡Δθ−kr(18)(\Delta\theta)^{'}=\frac{1-k_rl}{\cos\Delta\theta}\frac{d}{ds}\theta_x-\theta^{'}_{r} \\ \space \\ (\Delta\theta)^{'}=k_x\frac{1-k_rl}{\cos\Delta\theta}-k_r \tag{18} (Δθ)′=cosΔθ1−krldsdθx−θr′ (Δθ)′=kxcosΔθ1−krl−kr(18)

将(18)式代入(17)式，得：

l′′=−(kr′l+krl′)tan⁡Δθ+(1−krl)cos⁡2Δθ[kx1−krlcos⁡Δθ−kr](19)l^{''}=-(k^{'}_{r}l+k_{r}l^{'})\tan\Delta\theta+\frac{(1-k_rl)}{\cos^2\Delta\theta}[k_x\frac{1-k_rl}{\cos\Delta\theta}-k_r] \tag{19} l′′=−(kr′l+krl′)tanΔθ+cos2Δθ(1−krl)[kxcosΔθ1−krl−kr](19)

最后求解ax=vx˙=vxdta_x=\dot{v_x}=\frac{v_x}{dt}ax=vx˙=dtvx。对(15)式求关于时间ttt的导数，得：

ax=s¨1−krlcos⁡Δθ+s˙dds1−krlcos⁡Δθs˙ax=s¨1−krlcos⁡Δθ+s˙2cos⁡Δθ[(1−krl)tan⁡Δθ(Δθ)′−(kr′l+krl′)](20)a_x=\ddot{s}\frac{1-k_rl}{\cos\Delta\theta}+\dot{s}\frac{d}{ds}\frac{1-k_rl}{\cos\Delta\theta}\dot{s} \\ \space \\ a_x=\ddot{s}\frac{1-k_rl}{\cos\Delta\theta}+\frac{\dot{s}^{2}}{\cos\Delta\theta}[(1-k_rl)\tan\Delta\theta(\Delta\theta)^{'}-(k^{'}_{r}l+k_{r}l^{'})] \tag{20} ax=s¨cosΔθ1−krl+s˙dsdcosΔθ1−krls˙ ax=s¨cosΔθ1−krl+cosΔθs˙2[(1−krl)tanΔθ(Δθ)′−(kr′l+krl′)](20)

将(18)式代入(20)式，得：

ax=s¨1−krlcos⁡Δθ+s˙2cos⁡Δθ[(1−krl)tan⁡Δθ(kx1−krlcos⁡Δθ−kr)−(kr′l+krl′)](21)a_x=\ddot{s}\frac{1-k_rl}{\cos\Delta\theta}+\frac{\dot{s}^{2}}{\cos\Delta\theta}[(1-k_rl)\tan\Delta\theta(k_x\frac{1-k_rl}{\cos\Delta\theta}-k_r)-(k^{'}_{r}l+k_{r}l^{'})] \tag{21} ax=s¨cosΔθ1−krl+cosΔθs˙2[(1−krl)tanΔθ(kxcosΔθ1−krl−kr)−(kr′l+krl′)](21)

(4.a)、(14)、(15)、(19)、(21)式便是我们要求的坐标转换公式，经过一些变换后有，

从笛卡尔坐标系到Frenet坐标系：
l=±(x−xr)2+(y−yr)2[(y−yr)cosθr−(x−xr)sinθr]>0?positive:negtivel=\pm \sqrt{(x-x_r)^2+(y-y_r)^2} \\ \space \\ [(y-y_r)cos\theta_r-(x-x_r)sin\theta_r] > 0 \space ? \space positive \space : \space negtive l=±(x−xr)2+(y−yr)2 [(y−yr)cosθr−(x−xr)sinθr]>0 ? positive : negtive
l′=(1−krl)tan⁡Δθl^{'}=(1-k_rl)\tan\Delta\theta l′=(1−krl)tanΔθ

l′′=−(kr′l+krl′)tan⁡Δθ+(1−krl)cos⁡2Δθ(kx1−krlcos⁡Δθ−kr)l^{''}=-(k^{'}_{r}l+k_{r}l^{'})\tan\Delta\theta+\frac{(1-k_rl)}{\cos^2\Delta\theta}(k_x\frac{1-k_rl}{\cos\Delta\theta}-k_r) l′′=−(kr′l+krl′)tanΔθ+cos2Δθ(1−krl)(kxcosΔθ1−krl−kr)

s˙=vxcos⁡Δθ1−krl\dot{s}=\frac{v_{x}\cos\Delta\theta}{1-k_rl} s˙=1−krlvxcosΔθ

s¨=[axcos⁡Δθ−s˙2((1−krl)tan⁡Δθ(kx1−krlcos⁡Δθ−kr)−(kr′l+krl′))]/(1−krl)\ddot{s} = [a_{x}\cos\Delta\theta-\dot{s}^{2}((1-k_rl)\tan\Delta\theta(k_x\frac{1-k_rl}{\cos\Delta\theta}-k_r)-(k^{'}_{r}l+k_{r}l^{'}))]/(1-k_{r}l) s¨=[axcosΔθ−s˙2((1−krl)tanΔθ(kxcosΔθ1−krl−kr)−(kr′l+krl′))]/(1−krl)

从Frenet到笛卡尔坐标系：
首先，参考线上sss点的对应笛卡尔坐标系的各个参数为xr,yr,θr,kr,dkrx_r,y_r,\theta_r,k_r,dk_rxr,yr,θr,kr,dkr，由于车辆参考点与参考轨迹上的点的连线垂直于切线方向，所以有，
x=xr−lsin⁡θry=yr+lcos⁡θrx=x_r-l\sin{\theta_r} \\ y=y_r+l\cos{\theta_r} x=xr−lsinθry=yr+lcosθr
由式（14）可知，有
θx=arctan⁡l′1−krl+θr\theta_x=\arctan \frac{l^{'}}{1-k_{r}l}+\theta_{r} θx=arctan1−krll′+θr
由式（19）可知，有
kx=[l′′+(kr′l+krl′)tan⁡Δθcos⁡2Δθ(1−krl)+kr]cos⁡Δθ1−krlΔθ=θx−θrk_x=[l^{''}+(k^{'}_{r}l+k_{r}l^{'})\tan\Delta\theta\frac{\cos^2\Delta\theta}{(1-k_rl)}+k_{r}]\frac{\cos\Delta\theta}{1-k_rl} \\ \Delta\theta=\theta_x-\theta_r kx=[l′′+(kr′l+krl′)tanΔθ(1−krl)cos2Δθ+kr]1−krlcosΔθΔθ=θx−θr

vx=s˙1−krlcos⁡Δθ=[s˙(1−kr)l]2+l˙2=[s˙(1−kr)l]2+l′2s˙2v_x=\dot{s}\frac{1-k_rl}{\cos\Delta\theta}=\sqrt{[\dot{s}(1-k_r)l]^{2}+\dot{l}^2} = \sqrt{[\dot{s}(1-k_r)l]^{2}+l^{'2}\dot{s}^2} vx=s˙cosΔθ1−krl=[s˙(1−kr)l]2+l˙2=[s˙(1−kr)l]2+l′2s˙2

ax=s¨1−krlcos⁡Δθ+s˙2cos⁡Δθ[(1−krl)tan⁡Δθ(kx1−krlcos⁡Δθ−kr)−(kr′l+krl′)]a_x=\ddot{s}\frac{1-k_rl}{\cos\Delta\theta}+\frac{\dot{s}^{2}}{\cos\Delta\theta}[(1-k_rl)\tan\Delta\theta(k_x\frac{1-k_rl}{\cos\Delta\theta}-k_r)-(k^{'}_{r}l+k_{r}l^{'})] ax=s¨cosΔθ1−krl+cosΔθs˙2[(1−krl)tanΔθ(kxcosΔθ1−krl−kr)−(kr′l+krl′)]
以上就是Frenet坐标系下的s,s˙,s¨,l,l′,l′′s,\dot{s},\ddot{s},l,l^{'},l^{''}s,s˙,s¨,l,l′,l′′转换到笛卡尔坐标系下的x,y,θx,kx,vx,axx,y,\theta_{x},k_{x},v_{x},a_{x}x,y,θx,kx,vx,ax的公式。

四、Apollo项目中Frenet坐标系与笛卡尔坐标系转换代码

函数声明文件planning/math/frame_conversion/cartesian_frenet_conversion.h：

#ifndef MODULES_PLANNING_MATH_FRAME_CONVERSION_CARTESIAN_FRENET_CONVERSION_H_
#define MODULES_PLANNING_MATH_FRAME_CONVERSION_CARTESIAN_FRENET_CONVERSION_H_#include <array>#include "modules/common/math/vec2d.h"namespace apollo {namespace planning {// Notations:
// s_condition = [s, s_dot, s_ddot]
// s: longitudinal coordinate w.r.t reference line.
// s_dot: ds / dt
// s_ddot: d(s_dot) / dt
// d_condition = [d, d_prime, d_pprime]
// d: lateral coordinate w.r.t. reference line
// d_prime: dd / ds
// d_pprime: d(d_prime) / ds
// l: the same as d.
class CartesianFrenetConverter {public:CartesianFrenetConverter() = delete;/*** Convert a vehicle state in Cartesian frame to Frenet frame.* Decouple a 2d movement to two independent 1d movement w.r.t. reference* line.* The lateral movement is a function of longitudinal accumulated distance s* to achieve better satisfaction of nonholonomic constraints.*/static void cartesian_to_frenet(const double rs, const double rx,const double ry, const double rtheta,const double rkappa, const double rdkappa,const double x, const double y,const double v, const double a,const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition);/*** Convert a vehicle state in Frenet frame to Cartesian frame.* Combine two independent 1d movement w.r.t. reference line to a 2d movement.*/static void frenet_to_cartesian(const double rs, const double rx,const double ry, const double rtheta,const double rkappa, const double rdkappa,const std::array<double, 3>& s_condition,const std::array<double, 3>& d_condition,double* const ptr_x, double* const ptr_y,double* const ptr_theta,double* const ptr_kappa, double* const ptr_v,double* const ptr_a);// given sl point extract x, y, theta, kappastatic double CalculateTheta(const double rtheta, const double rkappa,const double l, const double dl);static double CalculateKappa(const double rkappa, const double rdkappa,const double l, const double dl, const double ddl);                               static common::math::Vec2d CalculateCartesianPoint(const double rtheta, const common::math::Vec2d& rpoint, const double l);/*** @brief: given sl, theta, and road's theta, kappa, extract derivative l,*second order derivative l:*/static double CalculateLateralDerivative(const double theta_ref,const double theta, const double l, const double kappa_ref);// given sl, theta, and road's theta, kappa, extract second order derivativestatic double CalculateSecondOrderLateralDerivative(const double theta_ref, const double theta, const double kappa_ref,const double kappa, const double dkappa_ref, const double l);
};}  // namespace planning
}  // namespace apollo

函数实现文件planning/math/frame_conversion/cartesian_frenet_conversion.cc：

#include "modules/planning/math/frame_conversion/cartesian_frenet_conversion.h"#include <cmath>#include "modules/common/log.h"
#include "modules/common/math/math_utils.h"namespace apollo {namespace planning {using apollo::common::math::Vec2d;void CartesianFrenetConverter::cartesian_to_frenet(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa, const double x, const double y,const double v, const double a, const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition) {const double dx = x - rx;const double dy = y - ry;const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;// 求解dptr_d_condition->at(0) =std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);const double delta_theta = theta - rtheta;const double tan_delta_theta = std::tan(delta_theta);const double cos_delta_theta = std::cos(delta_theta);const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);// 求解d' = dd / dsptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;const double kappa_r_d_prime =rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);// 求解d'' = dd' / dsptr_d_condition->at(2) =-kappa_r_d_prime * tan_delta_theta +one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *(kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);// 求解sptr_s_condition->at(0) = rs;// 求解ds / dtptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;// 求解d(ds) / dtptr_s_condition->at(2) =(a * cos_delta_theta -ptr_s_condition->at(1) * ptr_s_condition->at(1) *(ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) / one_minus_kappa_r_d;return;
}void CartesianFrenetConverter::frenet_to_cartesian(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa,const std::array<double, 3>& s_condition,const std::array<double, 3>& d_condition, double* const ptr_x,double* const ptr_y, double* const ptr_theta, double* const ptr_kappa,double* const ptr_v, double* const ptr_a) {CHECK(std::abs(rs - s_condition[0]) < 1.0e-6)<< "The reference point s and s_condition[0] don't match";const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);*ptr_x = rx - sin_theta_r * d_condition[0];*ptr_y = ry + cos_theta_r * d_condition[0];const double one_minus_kappa_r_d = 1 - rkappa * d_condition[0];const double tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;const double delta_theta = std::atan2(d_condition[1], one_minus_kappa_r_d);const double cos_delta_theta = std::cos(delta_theta);*ptr_theta = common::math::NormalizeAngle(delta_theta + rtheta);const double kappa_r_d_prime =rdkappa * d_condition[0] + rkappa * d_condition[1];*ptr_kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *cos_delta_theta * cos_delta_theta) /(one_minus_kappa_r_d) +rkappa) *cos_delta_theta / (one_minus_kappa_r_d);const double d_dot = d_condition[1] * s_condition[1];*ptr_v = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *s_condition[1] * s_condition[1] +d_dot * d_dot);const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * (*ptr_kappa) - rkappa;*ptr_a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +s_condition[1] * s_condition[1] / cos_delta_theta *(d_condition[1] * delta_theta_prime - kappa_r_d_prime);
}double CartesianFrenetConverter::CalculateTheta(const double rtheta,const double rkappa,const double l,const double dl) {return common::math::NormalizeAngle(rtheta + std::atan2(dl, 1 - l * rkappa));
}double CartesianFrenetConverter::CalculateKappa(const double rkappa,const double rdkappa,const double l, const double dl,const double ddl) {double denominator = (dl * dl + (1 - l * rkappa) * (1 - l * rkappa));if (std::fabs(denominator) < 1e-8) {return 0.0;}denominator = std::pow(denominator, 1.5);const double numerator = rkappa + ddl - 2 * l * rkappa * rkappa -l * ddl * rkappa + l * l * rkappa * rkappa * rkappa +l * dl * rdkappa + 2 * dl * dl * rkappa;return numerator / denominator;
}Vec2d CartesianFrenetConverter::CalculateCartesianPoint(const double rtheta,const Vec2d& rpoint,const double l) {const double x = rpoint.x() - l * std::sin(rtheta);const double y = rpoint.y() + l * std::cos(rtheta);return Vec2d(x, y);
}double CartesianFrenetConverter::CalculateLateralDerivative(const double rtheta, const double theta, const double l,const double rkappa) {return (1 - rkappa * l) * std::tan(theta - rtheta);
}double CartesianFrenetConverter::CalculateSecondOrderLateralDerivative(const double rtheta, const double theta, const double rkappa,const double kappa, const double rdkappa, const double l) {const double dl = CalculateLateralDerivative(rtheta, theta, l, rkappa);const double theta_diff = theta - rtheta;const double cos_theta_diff = std::cos(theta_diff);const double res = -(rdkappa * l + rkappa * dl) * std::tan(theta - rtheta) +(1 - rkappa * l) / (cos_theta_diff * cos_theta_diff) *(kappa * (1 - rkappa * l) / cos_theta_diff - rkappa);if (std::isinf(res)) {AWARN << "result is inf when calculate second order lateral ""derivative. input values are rtheta:"<< rtheta << " theta: " << theta << ", rkappa: " << rkappa<< ", kappa: " << kappa << ", rdkappa: " << rdkappa << ", l: " << l<< std::endl;}return res;
}

参考资料：

https://github.com/ApolloAuto/apollo/blob/master/docs/specs/coordination.pdf
https://en.wikipedia.org/wiki/Frenet–Serret_formulas
Optimal trajectory generation for dynamic street scenarios in a Frenét Frame, http://ieeexplore.ieee.org/document/5509799/citations?tabFilter=papers
https://zhuanlan.zhihu.com/p/29836354
https://zhuanlan.zhihu.com/p/304474902

Apollo学习笔记（4）坐标系相关推荐

Apollo学习笔记
Apollo学习笔记 Apollo课程智能驾驶入门课程无人驾驶概览 1.软件层分为三层: 实时操作系统(RTOS):确保在给定时间内完成特定任务,实时时确保系统稳定性.驾驶安全性的重要要求.通过在 ...
Apollo学习笔记进阶课程之三：定位技术②
Apollo学习笔记进阶课程之三:定位技术② 百度的无人驾驶定位方案 1).GNSS定位 GPS误差来源: 上图为单点定位,基于TOA 载波定位技术:(RPK技术,PPP技术) RPK:可以在五秒内 ...
Apollo学习笔记3-定位模块配置
Apollo学习笔记3-定位模块配置环境介绍导航设备参数配置导航设备配置 (1)杆臂配置 (2)GNSS 航向配置 (3)导航模式配置 (4) USB 接口输出设置 (5)网口配置 (6) PP ...
Apollo学习笔记（12）Lattice Planner规划算法
本文主要参考Apollo开发者社区,以及一些大神的博客,在此膜拜,文末会奉上相关链接. Lattice Planner 规划算法简介之前的相关的规划的算法都是放在无人驾驶专栏下的,Lattice P ...
EPSON LS3-401S机器人学习笔记 5 - 坐标系
坐标系摘自 http://www.jcpeixun.com/knowledge/detail.aspx?id=21242 关节坐标系基坐标系/直角坐标系/世界坐标系基坐标系是以机器人安装基座为基 ...
apollo学习笔记二：定位与感知
定位定位是指无人驾驶车准确知道自身确切位置的方法. 对于定位,车辆将其传感器识别的地标,与高精度地图上存在的地标进行对比,为了进行该对比,必须能够在其自身坐标系和地图坐标系之间转换数据,然后系统必须 ...
学习笔记——经纬度坐标系及定位相关API
一.经纬度坐标系 WGS84坐标系:即地球坐标系,国际上通用的坐标系. GCJ02坐标系:即火星坐标系,WGS84坐标系经加密后的坐标系. BD09坐标系:即百度坐标系,GCJ02坐标系经加密后的坐标 ...
Apollo学习笔记1-ESD_CAN调试
目录适配 ESD CAN 1. ESD CAN卡安装 2.ESD CAN驱动安装 3.Apollo ESD CAN 库添加适配 ESD CAN CANBUS模块是Apollo需要根据底盘协议写底盘 ...
【Apollo学习笔记】从零开始Apollo系统安装
文章目录 1. Ubuntu 18.04 安装 1.1 硬件环境 1.2 Ubuntu 18.04安装以及遇到的问题 2. 常用软件的安装 2.1 搜狗输入法 2.2 Vscode 2.3 Docke ...

Apollo学习笔记（4）坐标系