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公式用于描述粒子在三维欧氏空间 R 3 ℝ^3 R3内沿一条连续可微曲线的运动学特征，如下图所示：

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

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

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

令 θ r \theta_r θr​、 T ⃗ r {\bf \vec{T}}_r T r​、 N ⃗ r {\bf \vec{N}}_r N r​分别为当前参考线 r ⃗ ( s ) \vec{r}(s) r (s)的方位角、单位切向量和单位法向量， θ x \theta_x θx​、 T ⃗ x {\bf \vec{T}}_x T x​、 N ⃗ x {\bf \vec{N}}_x N x​分别为当前轨迹点 x ⃗ ( s , l ) \vec{x}(s, l) x (s,l)的方位角、单位切向量和单位法向量，根据正交基的定义有：
T ⃗ r = [ c o s θ r s i n θ r ] T {\bf \vec{T}}_r=\left[{\sf cos} \theta_r \quad{\sf sin} \theta_r \right]^T T r​=[cosθr​sinθr​]T
N ⃗ r = [ − s i n θ r c o s θ r ] T {\bf \vec{N}}_r=\left[{\sf -sin} \theta_r \quad {\sf cos} \theta_r\right]^T N r​=[−sinθr​cosθr​]T
T ⃗ x = [ c o s θ x s i n θ x ] T {\bf \vec{T}}_x=\left[{\sf cos} \theta_x \quad{\sf sin} \theta_x\right]^T T x​=[cosθx​sinθx​]T
N ⃗ x = [ − s i n θ x c o s θ x ] T {\bf \vec{N}}_x=\left[{\sf -sin} \theta_x\quad {\sf cos} \theta_x\right]^T N x​=[−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) {\bf \vec{N }}_r(s)\qquad(3) x (s,l)=r (s)+l(s)N r​(s)(3)
从而有（为简洁起见，下面的推导过程均省略参数）：
l N ⃗ r T N ⃗ r = N ⃗ r T [ r ⃗ − x ⃗ ] l {{\bf \vec{N }}_r^T{\bf \vec{N }}_r}= {\bf \vec{N }}_r^T[\vec{r}-\vec{x}] lN rT​N r​=N rT​[r −x ]
l = N ⃗ r T [ x ⃗ − r ⃗ ] = [ x ⃗ − r ⃗ ] T N ⃗ r ( 4 ) l = {\bf \vec{N}}_r^T [\vec{x} − \vec{r}] = [\vec{x} − \vec{r}]^T{\bf \vec{N}}_r \qquad(4) l=N rT​[x −r ]=[x −r ]TN r​(4)
(4)式在数学上美观，但不好编程实现，因此换一个表示方法。设 x ⃗ \vec{x} x 、 r ⃗ \vec{r} r 的笛卡尔坐标分别为： ( x , y ) (x, y) (x,y)、 ( x r , y r ) (x_r, y_r) (xr​,yr​)，根据两点间距离公式及 N ⃗ r {\bf \vec{N}}_r N r​的定义，可得：
l = ± ( x − x r ) 2 + ( y − y r ) 2 , i f [ ( y − y r ) c o s θ r − ( x − x r ) s i n θ r ] > 0 , p o s i t i v e ; o t h e r w i s e n e g e t i v e ( 4. a ) l = \pm\sqrt{(x-x_r)^2 + (y-y_r)^2} , \\ if \quad [(y-y_r){\sf cos} \theta_r - (x-x_r){\sf sin} \theta_r]>0,positive; \quad otherwise \quad negetive \qquad(4.a) l=±(x−xr​)2+(y−yr​)2 ​,if[(y−yr​)cosθr​−(x−xr​)sinθr​]>0,positive;otherwisenegetive(4.a)
设a为任意一个变量（或向量），记 a ˙ = d ( a ) d t \dot{a}=\frac{{\rm d}{(a)}}{{\rm d}t} a˙=dtd(a)​， a ¨ = d ( a ˙ ) d t \ddot{a}=\frac{{\rm d}{(\dot{a})}}{{\rm d}t} a¨=dtd(a˙)​， a ′ = d ( a ) d s a^\prime=\frac{{\rm d}{(a)}}{{\rm d}s} a′=dsd(a)​， a ′ ′ = d ( a ′ ) d s a^{\prime\prime}=\frac{{\rm d}{(a^\prime)}}{{\rm d}s} a′′=dsd(a′)​，根据该约定，有：
l ˙ = [ x ⃗ ˙ − r ⃗ ˙ ] T N ⃗ r + [ x ⃗ − r ⃗ ] T N ⃗ ˙ r \dot{l}=[\dot{\vec{x}} −\dot{\vec{r}} ] ^T {\bf \vec{N}}_r+[\vec{x} − \vec{r} ] ^T \dot{{\bf \vec{N}}}_r l˙=[x ˙−r ˙]TN r​+[x −r ]TN ˙r​
根据单位切向量和单位法向量的定义（参见第二部分内容），有：
x ⃗ ˙ = d ∣ ∣ x ⃗ ∣ ∣ d t T ⃗ x = v x T ⃗ x ( 5 ) \dot{\vec{x}} =\frac{{\rm d}{||\vec{x}||}}{{\rm d}t}{\bf \vec{T}}_x=v_x{\bf \vec{T}}_x \qquad(5) x ˙=dtd∣∣x ∣∣​T x​=vx​T x​(5)
r ⃗ ˙ = d ∣ ∣ r ⃗ ∣ ∣ d t T ⃗ r = s ˙ T ⃗ r ( 6 ) \dot{\vec{r}} =\frac{{\rm d}{||\vec{r}||}}{{\rm d}t}{\bf \vec{T}}_r=\dot{s}{\bf \vec{T}}_r \qquad(6) r ˙=dtd∣∣r ∣∣​T r​=s˙T r​(6)
根据平面几何知识可知：
x ⃗ − r ⃗ = l N ⃗ r ( 7 ) \vec{x} − \vec{r} =l{\bf \vec{N}}_r \qquad(7) x −r =lN r​(7)
根据链式求导法则， N ⃗ ˙ r = d N ⃗ r d s d s d t \dot{{\bf \vec{N}}}_r=\frac{{\rm d}{\bf \vec{N}}_r}{{\rm d}s}\frac{{\rm d}{s}}{{\rm d}t} N ˙r​=dsdN r​​dtds​，又由二维Frenet–Serret公式可知： N ⃗ r ′ = − κ r T ⃗ r {{\bf \vec{N}}_r^\prime}=-\kappa_r{\bf \vec{T}}_r N r′​=−κr​T r​，于是有：
N ⃗ ˙ r = − κ r s ˙ T ⃗ r ( 8 ) \dot{{\bf \vec{N}}}_r=-\kappa_r\dot{s}{\bf \vec{T}}_r \qquad(8) N ˙r​=−κr​s˙T r​(8)
将(5)-(8)式代入，得：
l ˙ = [ v x T ⃗ x − s ˙ T ⃗ r ] T N ⃗ r + l N ⃗ r T ( − κ r s ˙ T ⃗ ) r \dot{l}=[v_x{\bf \vec{T}}_x −\dot{s}{\bf \vec{T}}_r]^T {\bf \vec{N}}_r +l{\bf \vec{N}}_r^T (-\kappa_r\dot{s}{\bf \vec{T}})_r l˙=[vx​T x​−s˙T r​]TN r​+lN rT​(−κr​s˙T )r​
因为单位切向量和法向量正交，于是有： T ⃗ r T N ⃗ r = 0 {\bf \vec{T}}_r^T {\bf \vec{N}}_r =0 T rT​N r​=0， N ⃗ r T T ⃗ r = 0 {\bf \vec{N}}_r^T {\bf \vec{T}}_r =0 N rT​T r​=0，故：
l ˙ = v x T ⃗ x T N ⃗ r \dot{l}=v_x{\bf \vec{T}}_x^T {\bf \vec{N}}_r l˙=vx​T xT​N r​
将 T ⃗ x = [ c o s θ x s i n θ x ) ] T {\bf \vec{T}}_x=\left[{\sf cos} \theta_x \quad{\sf sin} \theta_x)\right]^T T x​=[cosθx​sinθx​)]T和 N ⃗ r = [ − s i n θ r c o s θ r ] T {\bf \vec{N}}_r=\left[{\sf -sin} \theta_r \quad {\sf cos} \theta_r \right]^T N r​=[−sinθr​cosθr​]T代入，得到：
l ˙ = v x [ − c o s θ x s i n θ r + s i n θ x c o s θ r ] = v x s i n Δ θ ( 9 ) \dot{l}=v_x[-{\sf cos} \theta_x{\sf sin} \theta_r+{\sf sin} \theta_x{\sf cos} \theta_r] =v_x{\sf sin}\Delta \theta \qquad(9) l˙=vx​[−cosθx​sinθr​+sinθx​cosθr​]=vx​sinΔθ(9)
上式中:
Δ θ = θ x − θ r ( 10 ) \Delta \theta=\theta_x-\theta_r \qquad(10) Δθ=θx​−θr​(10)
现在来计算 v x v_x vx​，根据定义： v x = d ∣ ∣ x ⃗ ∣ ∣ d t = ∣ ∣ d x ⃗ d t ∣ ∣ = ∣ ∣ x ⃗ ˙ ∣ ∣ v_x=\frac{{\rm d}{||\vec{x}||}}{{\rm d}t}=||\frac{{\rm d}{\vec{x}}}{{\rm d}t}||=||\dot{\vec{x}}|| vx​=dtd∣∣x ∣∣​=∣∣dtdx ​∣∣=∣∣x ˙∣∣，下面先计算 x ⃗ ˙ \dot{\vec{x}} x ˙。由（3）式，有：
x ⃗ ˙ = d ( r ⃗ + l N ⃗ r ) d t = r ⃗ ˙ + l ˙ N ⃗ r + l N ⃗ ˙ r \dot{\vec{x}}=\frac{{\rm d} (\vec{r} + l{\bf \vec{N }}_r)}{{\rm d}t}=\dot{\vec{r}}+\dot{l}{\bf \vec{N }}_r+l\dot{{\bf \vec{N}}}_r x ˙=dtd(r +lN r​)​=r ˙+l˙N r​+lN ˙r​
将(6)式和(8)式代入，得到：
x ⃗ ˙ = s ˙ ( 1 − κ r ) l T ⃗ r + l ˙ N ⃗ r ( 11 ) \dot{\vec{x}}=\dot{s}(1-\kappa_r)l{\bf \vec{T}}_r +\dot{l}{\bf \vec{N }}_r \qquad(11) x ˙=s˙(1−κr​)lT r​+l˙N r​(11)
于是有：
v x = ∣ ∣ x ⃗ ˙ ∣ ∣ = x ⃗ ˙ T x ⃗ ˙ = [ s ˙ ( 1 − κ r ) l ] 2 + l ˙ 2 ( 12 ) v_x=||\dot{\vec{x}}||=\sqrt{\dot{\vec{x}}^T\dot{\vec{x}}}=\sqrt{[\dot{s}(1-\kappa_r)l]^2+\dot{l}^2} \qquad(12) vx​=∣∣x ˙∣∣=x ˙Tx ˙ ​=[s˙(1−κr​)l]2+l˙2 ​(12)
下面计算 l ′ l^\prime l′：
l ′ = d d s = d d t d t d s = l ˙ s ˙ l^\prime=\frac{{\rm d}}{{\rm d}s}=\frac{{\rm d}}{{\rm d}t}\frac{{\rm d}t}{{\rm d}s}=\frac{\dot{l}}{\dot{s}} l′=dsd​=dtd​dsdt​=s˙l˙​
将(10)式代入，得到：
l ′ = v x s ˙ s i n Δ θ ( 13 ) l^\prime=\frac{v_x}{\dot{s}}{\sf sin}\Delta \theta \qquad(13) l′=s˙vx​​sinΔθ(13)
再将(12)式代入，得到：
l ′ = ( 1 − κ r l ) 2 + l ′ 2 s i n Δ θ l^\prime=\sqrt{(1-\kappa_rl)^2+{l^\prime}^2}{\sf sin}\Delta \theta l′=(1−κr​l)2+l′2 ​sinΔθ
l ′ 2 = [ ( 1 − κ r l ) 2 + l ′ 2 ] s i n 2 Δ θ {l^\prime}^2=[(1-\kappa_rl)^2+{l^\prime}^2]{\sf sin}^2\Delta \theta l′2=[(1−κr​l)2+l′2]sin2Δθ
l ′ 2 ( 1 − s i n 2 Δ θ ) = ( 1 − κ r l ) 2 s i n 2 Δ θ {l^\prime}^2(1-{\sf sin}^2\Delta \theta)=(1-\kappa_rl)^2{\sf sin}^2\Delta \theta l′2(1−sin2Δθ)=(1−κr​l)2sin2Δθ
假定车辆实际轨迹一直沿参考线附近运动（即不做与参考线反向的运动），使得：
∣ Δ θ ∣ &lt; π / 2 |\Delta \theta|&lt;\pi/2 ∣Δθ∣<π/2
1 − κ r l &gt; 0 1-\kappa_rl&gt;0 1−κr​l>0
则求解上述关于 l ′ l^\prime l′的方程得到：
l ′ = ( 1 − κ r l ) t a n Δ θ ( 14 ) l^\prime= (1-\kappa_rl){\sf tan}\Delta \theta \qquad(14) l′=(1−κr​l)tanΔθ(14)
将(14)式代入(13)式，可得到速度 v x v_x vx​的表达式：
( 1 − κ r l ) t a n Δ θ = v x s ˙ s i n Δ θ (1-\kappa_rl){\sf tan}\Delta \theta=\frac{v_x}{\dot{s}}{\sf sin}\Delta \theta (1−κr​l)tanΔθ=s˙vx​​sinΔθ
v x = s ˙ 1 − κ r l c o s Δ θ ( 15 ) v_x=\dot{s}\frac{1-\kappa_rl}{{\sf cos}\Delta \theta} \qquad(15) vx​=s˙cosΔθ1−κr​l​(15)
令 s x s_x sx​为车辆当前轨迹 x ⃗ \vec{x} x 的弧长，则有：
d d s = d s x d s d d s x = d s x d t d t d s d d s x = v x s ˙ d d s x \frac{{\rm d}}{{\rm d}s}=\frac{{\rm d}s_x}{{\rm d}s}\frac{{\rm d}}{{\rm d}s_x}=\frac{{\rm d}s_x}{{\rm d}t}\frac{{\rm d}t}{{\rm d}s}\frac{{\rm d}}{{\rm d}s_x}=\frac{v_x}{\dot{s}}\frac{{\rm d}}{{\rm d}s_x} dsd​=dsdsx​​dsx​d​=dtdsx​​dsdt​dsx​d​=s˙vx​​dsx​d​
将(15)式代入，得到：
d d s = 1 − κ r l c o s Δ θ d d s x ( 16 ) \frac{{\rm d}}{{\rm d}s}=\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}\frac{{\rm d}}{{\rm d}s_x} \qquad(16) dsd​=cosΔθ1−κr​l​dsx​d​(16)
对(14)式求关于参考线弧长 s s s的偏导：
l ′ ′ = ( 1 − κ r l ) ′ t a n Δ θ + ( 1 − κ r l ) ( Δ θ ) ′ c o s 2 Δ θ ( 17 ) l^{\prime\prime}= (1-\kappa_rl)^\prime{\sf tan}\Delta \theta +\frac{(1-\kappa_rl)(\Delta \theta)^\prime}{{\sf cos}^2\Delta \theta} \qquad(17) l′′=(1−κr​l)′tanΔθ+cos2Δθ(1−κr​l)(Δθ)′​(17)
注意到： Δ θ = θ x − θ r \Delta \theta=\theta_x-\theta_r Δθ=θx​−θr​，于是有：
( Δ θ ) ′ = d d s θ x − θ r ′ (\Delta \theta)^\prime=\frac{{\rm d}}{{\rm d}s}\theta_x-\theta_r^\prime (Δθ)′=dsd​θx​−θr′​
根据曲率的定义： κ r = θ r ′ = d θ r d s \kappa_r=\theta_r^\prime=\frac{{\rm d}\theta_r}{{\rm d}s} κr​=θr′​=dsdθr​​， κ x = d θ x d s x \kappa_x=\frac{{\rm d}\theta_x}{{\rm d}s_x} κx​=dsx​dθx​​，并将(16)式代入，得到：
( Δ θ ) ′ = 1 − κ r l c o s Δ θ d d s x θ x − θ r ′ (\Delta \theta)^\prime=\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}\frac{{\rm d}}{{\rm d}s_x} \theta_x-\theta_r^\prime (Δθ)′=cosΔθ1−κr​l​dsx​d​θx​−θr′​
( Δ θ ) ′ = κ x 1 − κ r l c o s Δ θ − κ r ( 18 ) (\Delta \theta)^\prime=\kappa_x\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}-\kappa_r \qquad(18) (Δθ)′=κx​cosΔθ1−κr​l​−κr​(18)
将(18)式代入(17)式，得：
l ′ ′ = − ( κ r ′ l + κ r l ′ ) t a n Δ θ + ( 1 − κ r l ) c o s 2 Δ θ [ κ x 1 − κ r l c o s Δ θ − κ r ] ( 19 ) l^{\prime\prime}= -({\kappa_r^\prime}l+\kappa_rl^\prime){\sf tan}\Delta \theta +\frac{(1-\kappa_rl)}{{\sf cos}^2\Delta \theta}[\kappa_x\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}-\kappa_r] \qquad(19) l′′=−(κr′​l+κr​l′)tanΔθ+cos2Δθ(1−κr​l)​[κx​cosΔθ1−κr​l​−κr​](19)
最后求解 a x = v x ˙ = d v x d t a_x=\dot{v_x}=\frac{{\rm d}v_x}{{\rm d}t} ax​=vx​˙​=dtdvx​​。对(15)式求关于时间 t t t的导数，得：
a x = s ¨ 1 − κ r l c o s Δ θ + s ˙ d d s 1 − κ r l c o s Δ θ s ˙ a_x=\ddot{s}\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}+\dot{s}\frac{{\rm d}}{{\rm d}s}\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}\dot{s} ax​=s¨cosΔθ1−κr​l​+s˙dsd​cosΔθ1−κr​l​s˙
a x = s ¨ 1 − κ r l c o s Δ θ + s ˙ 2 c o s Δ θ [ ( 1 − κ r l ) t a n Δ θ ( Δ θ ) ′ − ( k r ′ l + k r l ′ ) ] ( 20 ) a_x=\ddot{s}\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}+\frac{\dot{s}^2}{{\sf cos}\Delta \theta}[(1-\kappa_rl){\sf tan}\Delta \theta(\Delta \theta)^\prime-(k_r^\prime l+ k_r l^\prime)] \qquad(20) ax​=s¨cosΔθ1−κr​l​+cosΔθs˙2​[(1−κr​l)tanΔθ(Δθ)′−(kr′​l+kr​l′)](20)
将(18)式代入(20)式，得：
a x = s ¨ 1 − κ r l c o s Δ θ + s ˙ 2 c o s Δ θ [ ( 1 − κ r l ) t a n Δ θ [ κ x 1 − κ r l c o s Δ θ − κ r ] − ( k r ′ l + k r l ′ ) ] ( 21 ) a_x=\ddot{s}\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}+\frac{\dot{s}^2}{{\sf cos}\Delta \theta}[(1-\kappa_rl){\sf tan}\Delta \theta[\kappa_x\frac{1-\kappa_rl}{{\sf cos}\Delta \theta}-\kappa_r]-(k_r^\prime l+ k_r l^\prime)] \qquad(21) ax​=s¨cosΔθ1−κr​l​+cosΔθs˙2​[(1−κr​l)tanΔθ[κx​cosΔθ1−κr​l​−κr​]−(kr′​l+kr​l′)](21)
(4.a)、(14)、(15)、(19)、(21)式便是我们要求的坐标转换公式。