Apollo项目坐标系研究

声明：本文系作者davidhopper原创，未经允许，不得转载！
百度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公式用于描述粒子在三维欧氏空间 ${\mathbb{R}}^3$内沿一条连续可微曲线的运动学特征，如下图所示：

其中，$\vec{\mathbf{T}}$称为切向量（tangent），表示沿曲线运动方向的单位向量；$\vec{\mathbf{N}}$称为法向量（normal），表示当前曲线运动平面内垂直于$\vec{\mathbf{T}}$的单位向量；$\vec{\mathbf{B}}$称为副法向量（binormal），表示同时垂直于$\vec{\mathbf{T}}$和$\vec{\mathbf{N}}$的单位向量。
令$\vec{\mathbf{r}}(t)$为欧氏空间内随$t$改变的一条非退化曲线。所谓非退化曲线就是一条不会退化为直线的曲线，亦即曲率不为0的曲线。令$s(t)$是$t$时刻时曲线的累计弧长，其定义如下：
$$s(t)={\int }_0^t||r^{\prime }(\sigma )||\mathrm{d}\sigma$$
假定$r\prime ̸=0$，则意味着$s(t)$是严格单调递增函数。因此可将$t$表示为$s$的函数，从而有：$\vec{\mathbf{r}}(s)=\vec{\mathbf{r}}(t(s))$，这样我们就把曲线表示为弧长$s$的函数。
对于采用弧长参数$s$表示的非退化曲线$\vec{\mathbf{r}}(s)$，我们定义其切向量$\vec{\mathbf{T}}$、法向量$\vec{\mathbf{N}}$、副法向量$\vec{\mathbf{B}}$如下：
$$\vec{\mathbf{T}}=\frac{\frac{\mathrm{d}\vec{\mathbf{r}}}{\mathrm{d}s}}{||\frac{\mathrm{d}\vec{\mathbf{r}}}{\mathrm{d}s}||}$$
$$\vec{\mathbf{N}}=\frac{\frac{\mathrm{d}\vec{\mathbf{T}}}{\mathrm{d}s}}{||\frac{\mathrm{d}\vec{\mathbf{T}}}{\mathrm{d}s}||}$$
$$\vec{\mathbf{B}}=\vec{\mathbf{T}}\times \vec{\mathbf{N}}$$
基于上述定义的Frenet–Serret公式表示为：
$$\frac{\mathrm{d}\vec{\mathbf{T}}}{\mathrm{d}s}=\kappa \vec{\mathbf{N}}$$
$$\frac{\mathrm{d}\vec{\mathbf{N}}}{\mathrm{d}s}=-\kappa \vec{\mathbf{T}}+\tau \vec{\mathbf{B}}$$
$$\frac{\mathrm{d}\vec{\mathbf{B}}}{\mathrm{d}s}=-\tau \vec{\mathbf{N}}$$
其中$\kappa$、$\tau$分别表示曲线$\vec{\mathbf{r}}(s)$的曲率（ curvature）和挠率（ torsion）。直观地讲，曲率是曲线不能形成一条直线的度量值，曲率越趋于0，则曲线越趋近于直线；挠率是曲线不能形成在同一平面内运动曲线的度量值，挠率越趋于0，则曲线越趋近于在同一平面内运动。
令${\vec{\mathbf{T}}}^{\prime }=\frac{\mathrm{d}\vec{\mathbf{T}}}{\mathrm{d}s}$，${\vec{\mathbf{N}}}^{\prime }=\frac{\mathrm{d}\vec{\mathbf{N}}}{\mathrm{d}s}$，${\vec{\mathbf{B}}}^{\prime }=\frac{\mathrm{d}\vec{\mathbf{B}}}{\mathrm{d}s}$，则Frenet–Serret公式的矩阵表示形式为：
$$\left[\begin{array}{c}{\vec{\mathbf{T}}}^{\prime }\\ {\vec{\mathbf{N}}}^{\prime }\\ {\vec{\mathbf{B}}}^{\prime }\end{array}\right]=\left[\begin{array}{ccc}0& \kappa & 0\\ -\kappa & 0& \tau \\ 0& -\tau & 0\end{array}\right]\left[\begin{array}{c}\vec{\mathbf{T}}\\ \vec{\mathbf{N}}\\ \vec{\mathbf{B}}\end{array}\right](1)$$
易见参数矩阵是反对称（ skew-symmetric）的。
对于无人驾驶车辆而言，一般对高度信息不感兴趣，因此可以将车辆运动曲线投影到同一平面内，亦即$\tau =0$，这样Frenet–Serret公式就可以简化为：
$$\left[\begin{array}{c}{\vec{\mathbf{T}}}^{\prime }\\ {\vec{\mathbf{N}}}^{\prime }\end{array}\right]=\left[\begin{array}{cc}0& \kappa \\ -\kappa & 0\end{array}\right]\left[\begin{array}{c}\vec{\mathbf{T}}\\ \vec{\mathbf{N}}\end{array}\right](2)$$

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

为什么要将笛卡尔坐标系转换为Frenet坐标系？因为可以这样可以将车辆的二维运动问题解耦合为两个一维运动问题。显然，一维问题比二维问题容易求解，这就是笛卡尔坐标系转换为Frenet坐标系的必要性。
前已述及，在不考虑高度信息的前提下，Frenet坐标系可简化为由曲线切向量$\vec{\mathbf{T}}$与法向量$\vec{\mathbf{N}}$组成的二维直角坐标系。如下图所示，假定$\vec{r}(s)$是参考线（reference line）在弧长$s$处的位置，$\vec{x}$是当前车辆轨迹（trajectory）点，该向量一般采用笛卡尔坐标系（常用ENU坐标）表示（$\vec{x}=[x,y,z{]}^T$，$z$坐标一般忽略），但这里我们采用弧长$s$和横向偏移$l$（即沿当前参考线位置$\vec{r}(s)$法线方向${\vec{N}}_r$的偏移量）对其描述，即$\vec{x}=\vec{x}(s,l)$。

令${\theta }_r$、${\vec{\mathbf{T}}}_r$、${\vec{\mathbf{N}}}_r$分别为当前参考线$\vec{r}(s)$的方位角、单位切向量和单位法向量，${\theta }_x$、${\vec{\mathbf{T}}}_x$、${\vec{\mathbf{N}}}_x$分别为当前轨迹点$\vec{x}(s,l)$的方位角、单位切向量和单位法向量，根据正交基的定义有：
$${\vec{\mathbf{T}}}_r={\left[\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_r\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_r\right]}^T$$
$${\vec{\mathbf{N}}}_r={\left[-\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_r\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_r\right]}^T$$
$${\vec{\mathbf{T}}}_x={\left[\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_x\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_x\right]}^T$$
$${\vec{\mathbf{N}}}_x={\left[-\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_x\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_x\right]}^T$$
根据平面几何知识易知：
$$\vec{x}(s,l)=\vec{r}(s)+l(s){\vec{\mathbf{N}}}_r(s)(3)$$
从而有（为简洁起见，下面的推导过程均省略参数）：
$$l{\vec{\mathbf{N}}}_r^T{\vec{\mathbf{N}}}_r={\vec{\mathbf{N}}}_r^T[\vec{r}-\vec{x}]$$
$$l={\vec{\mathbf{N}}}_r^T[\vec{x}-\vec{r}]=[\vec{x}-\vec{r}{]}^T{\vec{\mathbf{N}}}_r(4)$$
(4)式在数学上美观，但不好编程实现，因此换一个表示方法。设$\vec{x}$、$\vec{r}$的笛卡尔坐标分别为：$(x,y)$、$(x_r,y_r)$，根据两点间距离公式及${\vec{\mathbf{N}}}_r$的定义，可得：
$$\begin{gathered} l=\pm \sqrt{(x-x_r{)}^2+(y-y_r{)}^2}, \\ if[(y-y_r)\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_r-(x-x_r)\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_r]>0,positive;otherwisenegetive(4.a) \end{gathered}$$
设a为任意一个变量（或向量），记$\dot{a}=\frac{\mathrm{d}(a)}{\mathrm{d}t}$，$\ddot{a}=\frac{\mathrm{d}(\dot{a})}{\mathrm{d}t}$，$a^{\prime }=\frac{\mathrm{d}(a)}{\mathrm{d}s}$，$a^{\prime \prime }=\frac{\mathrm{d}(a^{\prime })}{\mathrm{d}s}$，根据该约定，有：
$$\dot{l}=[\dot{\vec{x}}-\dot{\vec{r}}{]}^T{\vec{\mathbf{N}}}_r+[\vec{x}-\vec{r}{]}^T{\dot{\vec{\mathbf{N}}}}_r$$
根据单位切向量和单位法向量的定义（参见第二部分内容），有：
$$\dot{\vec{x}}=\frac{\mathrm{d}||\vec{x}||}{\mathrm{d}t}{\vec{\mathbf{T}}}_x=v_x{\vec{\mathbf{T}}}_x(5)$$
$$\dot{\vec{r}}=\frac{\mathrm{d}||\vec{r}||}{\mathrm{d}t}{\vec{\mathbf{T}}}_r=\dot{s}{\vec{\mathbf{T}}}_r(6)$$
根据平面几何知识可知：
$$\vec{x}-\vec{r}=l{\vec{\mathbf{N}}}_r(7)$$
根据链式求导法则，${\dot{\vec{\mathbf{N}}}}_r=\frac{\mathrm{d}{\vec{\mathbf{N}}}_r}{\mathrm{d}s}\frac{\mathrm{d}s}{\mathrm{d}t}$，又由二维Frenet–Serret公式可知：${\vec{\mathbf{N}}}_r^{\prime }=-{\kappa }_r{\vec{\mathbf{T}}}_r$，于是有：
$${\dot{\vec{\mathbf{N}}}}_r=-{\kappa }_r\dot{s}{\vec{\mathbf{T}}}_r(8)$$
将(5)-(8)式代入，得：
$$\dot{l}=[v_x{\vec{\mathbf{T}}}_x-\dot{s}{\vec{\mathbf{T}}}_r{]}^T{\vec{\mathbf{N}}}_r+l{\vec{\mathbf{N}}}_r^T(-{\kappa }_r\dot{s}\vec{\mathbf{T}}{)}_r$$
因为单位切向量和法向量正交，于是有：${\vec{\mathbf{T}}}_r^T{\vec{\mathbf{N}}}_r=0$，${\vec{\mathbf{N}}}_r^T{\vec{\mathbf{T}}}_r=0$，故：
$$\dot{l}=v_x{\vec{\mathbf{T}}}_x^T{\vec{\mathbf{N}}}_r$$
将${\vec{\mathbf{T}}}_x={\left[\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_x\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_x)\right]}^T$和 ${\vec{\mathbf{N}}}_r={\left[-\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_r\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_r\right]}^T$代入，得到：
$$\dot{l}=v_x[-\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_x\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_r+\mathsf{s}\mathsf{i}\mathsf{n}{\theta }_x\mathsf{c}\mathsf{o}\mathsf{s}{\theta }_r]=v_x\mathsf{s}\mathsf{i}\mathsf{n}\Delta \theta (9)$$
上式中:
$$\Delta \theta ={\theta }_x-{\theta }_r(10)$$
现在来计算$v_x$，根据定义：$v_x=\frac{\mathrm{d}||\vec{x}||}{\mathrm{d}t}=||\frac{\mathrm{d}\vec{x}}{\mathrm{d}t}||=||\dot{\vec{x}}||$，下面先计算$\dot{\vec{x}}$。由（3）式，有：
$$\dot{\vec{x}}=\frac{\mathrm{d}(\vec{r}+l{\vec{\mathbf{N}}}_r)}{\mathrm{d}t}=\dot{\vec{r}}+\dot{l}{\vec{\mathbf{N}}}_r+l{\dot{\vec{\mathbf{N}}}}_r$$
将(6)式和(8)式代入，得到：
$$\dot{\vec{x}}=\dot{s}(1-{\kappa }_r)l{\vec{\mathbf{T}}}_r+\dot{l}{\vec{\mathbf{N}}}_r(11)$$
于是有：
$$v_x=||\dot{\vec{x}}||=\sqrt{{\dot{\vec{x}}}^T\dot{\vec{x}}}=\sqrt{[\dot{s}(1-{\kappa }_r)l{]}^2+{\dot{l}}^2}(12)$$
下面计算$l^{\prime }$：
$$l^{\prime }=\frac{\mathrm{d}}{\mathrm{d}s}=\frac{\mathrm{d}}{\mathrm{d}t}\frac{\mathrm{d}t}{\mathrm{d}s}=\frac{\dot{l}}{\dot{s}}$$
将(10)式代入，得到：
$$l^{\prime }=\frac{v_x}{\dot{s}}\mathsf{s}\mathsf{i}\mathsf{n}\Delta \theta (13)$$
再将(12)式代入，得到：
$$l^{\prime }=\sqrt{(1-{\kappa }_{r}l{)}^2+{l^{\prime }}^2}\mathsf{s}\mathsf{i}\mathsf{n}\Delta \theta$$
$${l^{\prime }}^2=[(1-{\kappa }_{r}l{)}^2+{l^{\prime }}^2]{\mathsf{s}\mathsf{i}\mathsf{n}}^2\Delta \theta$$
$${l^{\prime }}^2(1-{\mathsf{s}\mathsf{i}\mathsf{n}}^2\Delta \theta )=(1-{\kappa }_{r}l{)}^2{\mathsf{s}\mathsf{i}\mathsf{n}}^2\Delta \theta$$
假定车辆实际轨迹一直沿参考线附近运动（即不做与参考线反向的运动），使得：
$$|\Delta \theta |<\pi /2$$
$$1-{\kappa }_{r}l>0$$
则求解上述关于$l^{\prime }$的方程得到：
$$l^{\prime }=(1-{\kappa }_{r}l)\mathsf{t}\mathsf{a}\mathsf{n}\Delta \theta (14)$$
将(14)式代入(13)式，可得到速度$v_x$的表达式：
$$(1-{\kappa }_{r}l)\mathsf{t}\mathsf{a}\mathsf{n}\Delta \theta =\frac{v_x}{\dot{s}}\mathsf{s}\mathsf{i}\mathsf{n}\Delta \theta$$
$$v_x=\dot{s}\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }(15)$$
令$s_x$为车辆当前轨迹$\vec{x}$的弧长，则有：
$$\frac{\mathrm{d}}{\mathrm{d}s}=\frac{\mathrm{d}{s}_x}{\mathrm{d}s}\frac{\mathrm{d}}{\mathrm{d}{s}_x}=\frac{\mathrm{d}{s}_x}{\mathrm{d}t}\frac{\mathrm{d}t}{\mathrm{d}s}\frac{\mathrm{d}}{\mathrm{d}{s}_x}=\frac{v_x}{\dot{s}}\frac{\mathrm{d}}{\mathrm{d}{s}_x}$$
将(15)式代入，得到：
$$\frac{\mathrm{d}}{\mathrm{d}s}=\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }\frac{\mathrm{d}}{\mathrm{d}{s}_x}(16)$$
对(14)式求关于参考线弧长$s$的偏导：
$$l^{\prime \prime }=(1-{\kappa }_{r}l{)}^{\prime }\mathsf{t}\mathsf{a}\mathsf{n}\Delta \theta +\frac{(1-{\kappa }_{r}l)(\Delta \theta {)}^{\prime }}{{\mathsf{c}\mathsf{o}\mathsf{s}}^2\Delta \theta }(17)$$
注意到：$\Delta \theta ={\theta }_x-{\theta }_r$，于是有：
$$(\Delta \theta {)}^{\prime }=\frac{\mathrm{d}}{\mathrm{d}s}{\theta }_x-{\theta }_r^{\prime }$$
根据曲率的定义：${\kappa }_r={\theta }_r^{\prime }=\frac{\mathrm{d}{\theta }_r}{\mathrm{d}s}$，${\kappa }_x=\frac{\mathrm{d}{\theta }_x}{\mathrm{d}{s}_x}$，并将(16)式代入，得到：
$$(\Delta \theta {)}^{\prime }=\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }\frac{\mathrm{d}}{\mathrm{d}{s}_x}{\theta }_x-{\theta }_r^{\prime }$$
$$(\Delta \theta {)}^{\prime }={\kappa }_x\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }-{\kappa }_r(18)$$
将(18)式代入(17)式，得：
$$l^{\prime \prime }=-({\kappa }_r^{\prime }l+{\kappa }_r{l}^{\prime })\mathsf{t}\mathsf{a}\mathsf{n}\Delta \theta +\frac{(1-{\kappa }_{r}l)}{{\mathsf{c}\mathsf{o}\mathsf{s}}^2\Delta \theta }[{\kappa }_x\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }-{\kappa }_r](19)$$
最后求解$a_x=\dot{v_x}=\frac{\mathrm{d}{v}_x}{\mathrm{d}t}$。对(15)式求关于时间$t$的导数，得：
$$a_x=\ddot{s}\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }+\dot{s}\frac{\mathrm{d}}{\mathrm{d}s}\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }\dot{s}$$
$$a_x=\ddot{s}\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }+\frac{{\dot{s}}^2}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }[(1-{\kappa }_{r}l)\mathsf{t}\mathsf{a}\mathsf{n}\Delta \theta (\Delta \theta {)}^{\prime }-(k_r^{\prime }l+k_r{l}^{\prime })](20)$$
将(18)式代入(20)式，得：
$$a_x=\ddot{s}\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }+\frac{{\dot{s}}^2}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }[(1-{\kappa }_{r}l)\mathsf{t}\mathsf{a}\mathsf{n}\Delta \theta [{\kappa }_x\frac{1-{\kappa }_{r}l}{\mathsf{c}\mathsf{o}\mathsf{s}\Delta \theta }-{\kappa }_r]-(k_r^{\prime }l+k_r{l}^{\prime })](21)$$
(4.a)、(14)、(15)、(19)、(21)式便是我们要求的坐标转换公式。

四、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, kappa
  static 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 derivative
  static 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;
  // 求解d
  ptr_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 / ds
  ptr_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' / ds
  ptr_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);

  // 求解s
  ptr_s_condition->at(0) = rs;
  // 求解ds / dt
  ptr_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) / dt
  ptr_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;
}

参考资料：

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