首先要建立车辆质心相对于车道中心线的误差模型，示意图如下所示：

在 C a r t e s i a n Cartesian Cartesian坐标系下，车辆运动状态可以描述为 ( x , y , θ , v , a , k ) (x, y, \theta, v, a, k) (x,y,θ,v,a,k), 在 F r e n e t Frenet Frenet坐标系下, 车辆的运动状态可以描述为 ( s r , x r , y r , θ r , k r , k r ′ ) (s_r, x_r, y_r,\theta_r, k_r, k_r') (sr​,xr​,yr​,θr​,kr​,kr′​)，求车辆在Frenet坐标系下的运动状态表达： [ s , s ˙ , s ¨ , d , d ˙ , d ¨ , d ′ , d ′ ′ ] \left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d}, d', d''\right] [s,s˙,s¨,d,d˙,d¨,d′,d′′]

公式总结如下： s = s r s=s_r s=sr​

s ˙ = v ∗ cos ⁡ ( θ − θ r ) 1 − k r ∗ d \dot{s} =\frac{v*\cos(\theta -\theta_r)}{1-k_r*d} s˙=1−kr​∗dv∗cos(θ−θr​)​

s ¨ = a ∗ cos ⁡ ( θ − θ r ) − s ˙ 2 ∗ [ d ′ ( k ∗ 1 − k r cos ⁡ ( θ − θ r ) − k r ) − ( k r ′ ∗ d + k r ∗ d ′ ) ] 1 − k r ∗ d \ddot{s}=\frac{a*\cos(\theta -\theta_r)-\dot{s}^2*[d'(k*\frac{1-k_r}{\cos(\theta -\theta_r)}-k_r)-(k_r'*d+k_r*d')]}{1-k_r*d} s¨=1−kr​∗da∗cos(θ−θr​)−s˙2∗[d′(k∗cos(θ−θr​)1−kr​​−kr​)−(kr′​∗d+kr​∗d′)]​

d = − sin ⁡ θ r ∗ ( x − x r ) + cos ⁡ θ r ∗ ( y − y r ) d=-\sin \theta_r*(x-x_r)+\cos \theta_r*(y-y_r) d=−sinθr​∗(x−xr​)+cosθr​∗(y−yr​)

d ˙ = v ∗ sin ⁡ ( θ − θ r ) \dot{d}=v*\sin(\theta-\theta_r) d˙=v∗sin(θ−θr​)

d ¨ = a ∗ sin ⁡ ( θ − θ r ) \ddot{d}=a*\sin(\theta-\theta_r) d¨=a∗sin(θ−θr​)

d ′ = ( 1 − k r ∗ d ) tan ⁡ ( θ − θ r ) d'=(1-k_r*d) \tan(\theta-\theta_r) d′=(1−kr​∗d)tan(θ−θr​)

d ′ ′ = − ( k r ′ + k r ∗ d ′ ) tan ⁡ ( θ − θ r ) + 1 − k r ∗ d cos ⁡ 2 ( θ − θ r ) ∗ ( k ∗ 1 − k r ∗ d cos ⁡ ( θ − θ r ) − k r ) d''=-(k_r'+k_r*d') \tan(\theta-\theta_r) +\frac{1-k_r*d}{\cos^2(\theta-\theta_r)}*(k* \frac{1-k_r*d}{\cos(\theta-\theta_r)} - k_r) d′′=−(kr′​+kr​∗d′)tan(θ−θr​)+cos2(θ−θr​)1−kr​∗d​∗(k∗cos(θ−θr​)1−kr​∗d​−kr​)

其中:

C a r t e s i a n Cartesian Cartesian坐标系下的量：

F r e n e t Frenet Frenet坐标系下的量：

在使用Frenet坐标系时，首先根据车辆位置找到参考路径上距离车辆最近的点（匹配点match_point），以匹配点为参考路径点并建立s-d坐标系，如上图所示。 首先计算车辆相对位置： d x = x − x m a t c h d y = y − y m a t c h dx=x-x_{match} \\ dy=y-y_{match} dx=x−xmatch​dy=y−ymatch​ 通过二维坐标系旋转公式可以得到车辆在frenet坐标系中相对于道路中心线的相对坐标为： [ s d ] = [ cos ⁡ ψ m a t c h sin ⁡ ψ m a t c h − sin ⁡ ψ m a t c h cos ⁡ ψ m a t c h ] [ d x d y ] \begin{bmatrix} s \\d \end{bmatrix} =\begin{bmatrix} \cos \psi_{match} & \sin \psi_{match} \\ -\sin \psi_{match} & \cos \psi_{match} \end{bmatrix} \begin{bmatrix} dx \\ dy \end{bmatrix} [sd​]=[cosψmatch​−sinψmatch​​sinψmatch​cosψmatch​​][dxdy​] 定义： e 1 e_1 e1​为横向误差， e 2 e_2 e2​为航向角误差、 e 1 ˙ \dot{e_1} e1​˙​为横向误差变化速率、 e 2 ˙ \dot{e_2} e2​˙​为航向角误差变化率,则有： e 1 = − sin ⁡ ψ m a t c h ∗ d x + cos ⁡ ψ m a t c h ∗ d y e 2 = ψ − ψ m a t c h \begin{aligned} e_1 & =-\sin \psi_{match}*dx+\cos \psi_{match}*dy \\ e_2 & =\psi-\psi_{match} \end{aligned} e1​e2​​=−sinψmatch​∗dx+cosψmatch​∗dy=ψ−ψmatch​​ 其中: ψ \psi ψ车辆当前位置航向角， ψ m a t c h \psi_{match} ψmatch​为路径参考点的航向角，进一步有： e 1 ˙ = v ∗ sin ⁡ ( ψ − ψ m a t c h ) e 2 ˙ = ψ ˙ − ψ m a t c h ˙ = ψ ˙ − v ∗ k m a t c h \begin{aligned} \dot{e_1} & =v*\sin(\psi-\psi_{match}) \\ \dot{e_2} & =\dot{\psi}-\dot{\psi_{match}}=\dot{\psi}-v*k_{match} \end{aligned} e1​˙​e2​˙​​=v∗sin(ψ−ψmatch​)=ψ˙​−ψmatch​˙​=ψ˙​−v∗kmatch​​ 其中： v v v为车辆当前纵向速度， ψ ˙ \dot{\psi} ψ˙​为车辆当前横摆角速度（或航向角变化率）， k m a t c h k_{match} kmatch​为匹配点曲率。

在纵向控制中，涉及到起始点、匹配点、参考点、预瞄点，示意图如下：

s s s：沿参考轨迹纵向累积距离

s ˙ \dot{s} s˙：沿参考轨迹纵向速度

d d d：参考轨迹横向距离

d ˙ \dot{d} d˙：参考轨迹横向速度

Apollo规划模块计算的参考轨迹具有一个全局时间戳，表示参考轨迹起始点的时间戳。参考轨迹上的其他点采用相对时间戳表示，即相对于起始点的时间戳。

（1）起始点：参考轨迹的起始点，相对时间戳为0s;

（2）匹配点 ( x m a t c h , y m a t c h ) (x_{match},y_{match}) (xmatch​,ymatch​)：根据车辆位置找到参考路径最近点，并以此点建立Frenet坐标系。由位置寻找，与相对时间戳无关;

以匹配点作为参考点，建立Frenet坐标系，车辆质心在Frenet坐标系下与道路中心线相对坐标可表示为： [ s d ] = [ cos ⁡ ψ m a t c h sin ⁡ ψ m a t c h − sin ⁡ ψ m a t c h cos ⁡ ψ m a t c h ] [ d x d y ] \begin{bmatrix} s \\ d \end{bmatrix} =\begin{bmatrix} \cos \psi_{match} & \sin \psi_{match} \\ -\sin \psi_{match} & \cos \psi_{match} \end{bmatrix} \begin{bmatrix} dx \\ dy \end{bmatrix} [sd​]=[cosψmatch​−sinψmatch​​sinψmatch​cosψmatch​​][dxdy​]

车辆的全局坐标需考虑匹配点的纵向里程距离 s m a t c h s_{match} smatch​,故： s = cos ⁡ ψ m a t c h ∗ d x + sin ⁡ ψ m a t c h ∗ d y + s m a t c h d = − sin ⁡ ψ m a t c h ∗ d x + cos ⁡ ψ m a t c h ∗ d y \begin{aligned} s & =\cos \psi_{match}*dx +\sin \psi_{match} * dy + s_{match} \\ d & =-\sin \psi_{match}*dx+\cos \psi_{match}*dy \end{aligned} sd​=cosψmatch​∗dx+sinψmatch​∗dy+smatch​=−sinψmatch​∗dx+cosψmatch​∗dy​

在Frenet坐标系下车辆的纵向速度和横向速度如下： s ˙ = v ∗ cos ⁡ ( ψ − ψ m a t c h ) 1 − k m a t c h ∗ d d ˙ = v ∗ sin ⁡ ( ψ − ψ m a t c h ) \begin{aligned} \dot{s} & =\frac{v*\cos(\psi -\psi_{match})}{1-k_{match}*d} \\ \dot{d} & = v*\sin(\psi-\psi_{match}) \end{aligned} s˙d˙​=1−kmatch​∗dv∗cos(ψ−ψmatch​)​=v∗sin(ψ−ψmatch​)​

3）参考点 ( x r e f , y r e f ) (x_{ref},y_{ref}) (xref​,yref​)：根据当前绝对时间戳确定参考点位置;

（4）预瞄点 ( x p r e , y p r e ) (x_{pre},y_{pre}) (xpre​,ypre​)：根据预瞄绝对时间戳确定预瞄点位置，预瞄绝对时间戳=参考绝对时间戳+预瞄时间。

因此：

根据参考点计算出来的位置误差、速度误差以及加速度误差表达式为： r e f S t a t i o n e r r o r = s r e f − s r e f S p e e d e r r o r = v r e f − s ˙ r e f A c c e l e r a t i o n e r r o r = a r e f − s ¨ refStation_{error}=s_{ref}-s \\ refSpeed_{error}=v_{ref}-\dot{s} \\ refAcceleration_{error}=a_{ref}-\ddot{s} refStationerror​=sref​−srefSpeederror​=vref​−s˙refAccelerationerror​=aref​−s¨

根据预瞄点计算出来的位置误差、速度误差以及加速度误差表达式为： p r e S t a t i o n e r r o r = s r e f − s p r e S p e e d e r r o r = v r e f − s ˙ p r e A c c e l e r a t i o n e r r o r = a p r e − s ¨ preStation_{error}=s_{ref}-s \\ preSpeed_{error}=v_{ref}-\dot{s} \\ preAcceleration_{error}=a_{pre}-\ddot{s} preStationerror​=sref​−spreSpeederror​=vref​−s˙preAccelerationerror​=apre​−s¨