文章目录

参考资料
2. Frenet坐标系与全局笛卡尔坐标系转换
- 2.1 Cartesian转 Frenet
- - 1. 求 sss
  - 2. 求 ddd
  - 3. 求 d˙\dot dd˙
  - 4. 求 s˙\dot ss˙
  - 5. 求 d′d'd′
  - 6. 求 d′′d''d′′
  - 7. 求 s¨\ddot{s}s¨
  - 8. 求d¨\ddot{d}d¨
- 2.2 Frenet转Cartesian
- - 10. 求x⃗\vec{x}x
  - 11. 求vxv_xvx
  - 12. 求θx\theta_xθx
  - 13. 求axa_xax
  - 14. 求κx\kappa_xκx

参考资料

Frenet坐标系与Cartesian坐标系互转
Trajectory planning and optimization algorithm for automated driving based on Frenet coordinate system
轨迹规划1：Frenet坐标转化公式推导

2. Frenet坐标系与全局笛卡尔坐标系转换

【自动驾驶】Frenet坐标系与Cartesian坐标系（一）

阅读本篇博客之前，请先阅读之前的博客。（由于正文字数的限制，拆成了几篇博客来写。）

在Frenet坐标系下，经规划得到横向、纵向运动轨迹后，需要将其重新映射到全局笛卡尔坐标系，以供控制模块调用。

如图所示, 受车辆运动学、动力学特性及道路环境 (避障) 限制, 车辆的实际行驶轨迹与参考线难以重合。

图中主要参数说明

首先, t⃗,n⃗\mathrm{\vec{t}}, \mathrm{\vec{n}}t,n 都是单位向量且有:
{t⃗r=[cos⁡θr,sin⁡θr]Tn⃗r=[−sin⁡θr,cos⁡θr]Tt⃗x=[cos⁡θx,sin⁡θx]Tn⃗x=[−sin⁡θx,cos⁡θx]T(1.4)\tag{1.4} \left\{ \begin{aligned} \vec{t}_{r} &=\left[\cos \theta_{r}, \sin \theta_{r}\right]^T \\ \vec{n}_{r} &=\left[-\sin \theta_{r}, \cos \theta_{r}\right]^T \\ \vec{t}_{x} &=\left[\cos \theta_{x}, \sin \theta_{x}\right]^T \\ \vec{n}_{x} &=\left[-\sin \theta_{x}, \cos \theta_{x}\right]^T \end{aligned} \right. ⎩⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎧trnrtxnx=[cosθr,sinθr]T=[−sinθr,cosθr]T=[cosθx,sinθx]T=[−sinθx,cosθx]T(1.4)

θr:\theta_{r}:θr: 表示参考线 r⃗\vec{r}r 的方向角
n⃗r\vec{n}_{r}nr : 表示r⃗\vec{r}r的单位法向量
t⃗r\vec{t}_{r}tr : 表示r⃗\vec{r}r的单位切向量
θx\theta_{x}θx : 表示 x⃗\vec{x}x的方向角
n⃗x\vec{n}_{x}nx : 表示 x⃗\vec{x}x的单位法向量
t⃗x\vec{t}_{x}tx : 表示 x⃗\vec{x}x 的单位切向量
Δθ=θx−θr\Delta\theta=\theta_x-\theta_rΔθ=θx−θr：为向量 (x⃗−r⃗\vec{x}-\vec{r}x−r) 的方向角度

全局坐标系下

在全局坐标系下, 任意时刻 ttt 的车辆运动状态可以描述为 [x⃗,θx,κx,vx,ax]\left[\vec{x}, \theta_{x}, \kappa_{x}, v_{x}, a_{x}\right][x,θx,κx,vx,ax]。

其中:

x⃗\vec{x}x 为车辆当前位置 QQQ, 表示车辆在全局坐标系下的位置信息 (x,y)(x, y)(x,y), 是一个向量;
θx\theta_{x}θx 为方位角，即全局坐标系下的朝向；
κx\kappa_{x}κx 为曲率
vx=∣∣x˙⃗∣∣v_{x}=||\vec{\dot x}||vx=∣∣x˙∣∣ 为Cartesian坐标系下的线速度;
ax=dvxdta_{x}=\frac{dv_x}{dt}ax=dtdvx 为加速度.
图中的r⃗(t)\vec{r}(t)r(t) 为位置 QQQ 投影到参考线上的投影点 PPP 在全局坐标系下的位置向量。

Frenet坐标系下

在 Frenet坐标系下, 车辆的运动状态可以描述为 [s,s˙,s¨,d,d˙,d¨,d′,d′′]\left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d} , d^{\prime}, d^{\prime \prime}\right][s,s˙,s¨,d,d˙,d¨,d′,d′′].

其中:

sss 为纵向位移，即Frenet纵坐标；
s˙=dsdt\dot{s}=\frac{ds}{dt}s˙=dtds 为Frenet纵向速度；
s¨=s˙dt\ddot{s}=\frac{\dot{s}}{dt}s¨=dts˙ 为Frenet纵向加速度,；
ddd 为横向位移, 即Frenet横坐标；
d˙\dot{d}d˙ 为Frenet横向速度；
d¨\ddot{d}d¨ 为Frenet横向加速度；
d′=dds(d)d^{\prime}=\frac{\mathrm{d}}{\mathrm{d}s}(d)d′=dsd(d) 为横向位移对纵向坐标的一阶导数,
d′′=dds(d′)d^{\prime \prime}=\frac{\mathrm{d}}{\mathrm{d}s}(d^{\prime})d′′=dsd(d′) 为横向位移对纵向坐标的二阶导数.

tips：x˙=dxdt\dot{x}=\frac{dx}{dt}x˙=dtdx，x′=dxdsx'=\frac{dx}{ds}x′=dsdx

2.1 Cartesian转 Frenet

已知 (x,y,θx,vx,ax,kx),(sr,xr,yr,θr,kr,kr′)(x, y, \theta_x, v_x, a_x, k_x), (s_r, x_r, y_r, \theta_r, k_r, k_r')(x,y,θx,vx,ax,kx),(sr,xr,yr,θr,kr,kr′),求 [s,s˙,s¨,d,d˙,d¨,d′,d′′]\left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d} , d^{\prime}, d^{\prime \prime}\right][s,s˙,s¨,d,d˙,d¨,d′,d′′].

1. 求 sss

首先，找到曲线上离位置(x,y)(x,y)(x,y)最近的参考点r⃗=(xr,yr)\vec{r}=(x_{r}, y_{r})r=(xr,yr)（即将位置QQQ投影到参考线上的投影点PPP），该参考点处的 srs_rsr即为 (x,y)(x, y)(x,y)在Frenet坐标系下的 sss。

2. 求 ddd

x⃗\vec{x}x和r⃗\vec{r}r分别为位置QQQ与投影点PPP在Cartesian坐标系下的向量，他们满足
x⃗=r⃗+dnr⃗(2.1)\tag{2.1} \vec{x}=\vec{r}+d \vec{n_{r}} x=r+dnr(2.1)
变换可得
d=(x⃗−r⃗)T⋅nr⃗=∣∣x⃗−r⃗∣∣2cos⁡(Δθ−(θr+π2))=∣∣x⃗−r⃗∣∣2sin⁡(Δθ−θr)=∣∣x⃗−r⃗∣∣2(sin⁡Δθcos⁡θr−cos⁡Δθsin⁡θr)(2.2)\tag{2.2} \begin{aligned} d&=(\vec{x}-\vec{r})^{T}\cdot\vec{n_{r}}\\ &=||\vec{x}-\vec{r}||_{2}\cos(\Delta \theta-(\theta_{r}+\frac{\pi}{2}))\\ &=||\vec{x}-\vec{r}||_{2}\sin(\Delta \theta-\theta_{r})\\ &=||\vec{x}-\vec{r}||_{2}(\sin{\Delta \theta \cos{\theta_{r}}}-\cos{\Delta \theta}\sin{\theta_{r}}) \end{aligned} d=(x−r)T⋅nr=∣∣x−r∣∣2cos(Δθ−(θr+2π))=∣∣x−r∣∣2sin(Δθ−θr)=∣∣x−r∣∣2(sinΔθcosθr−cosΔθsinθr)(2.2)
上式中， Δθ=θx−θr\Delta \theta=\theta_x-\theta_rΔθ=θx−θr为向量 x⃗−r⃗\vec{x}-\vec{r}x−r 的方向角度（即上图中的Δθ\Delta{\theta}Δθ），n⃗r\vec{n}_{r}nr为参考点PPP的单位法向量，θr+π2\theta_{r}+\frac{\pi}{2}θr+2π为单位向量 n⃗r\vec{n}_{r}nr的方向角度。

设∣∣d∣∣||d||∣∣d∣∣为车辆当前位置 QQQ 与投影点 PPP 之间的距离
∣∣d∣∣=(x−xr)2+(y−yr)2.(2.3)\tag{2.3} ||d||= \sqrt{\left(x-x_{r}\right)^{2}+\left(y-y_{r}\right)^{2}} . ∣∣d∣∣=(x−xr)2+(y−yr)2.(2.3)

在Frenet坐标系下，每个点到参考线上参考点的向量都与该参考点的法向量 nr⃗\vec{n_r}nr 同向或反向（即ddd与nr⃗\vec{n_{r}}nr在同一条线上），因此，
sin⁡(Δθ)cos⁡(θr)−cos⁡(Δθ)sin⁡(θr)=±1(2.4)\tag{2.4} \sin \left(\Delta \theta\right) \cos \left(\theta_{r}\right)-\cos \left(\Delta \theta\right) \sin \left(\theta_{r}\right)=\pm1 sin(Δθ)cos(θr)−cos(Δθ)sin(θr)=±1(2.4)

因为Δθ\Delta \thetaΔθ 为向量 x⃗−r⃗\vec{x}-\vec{r}x−r 的角度，因此，
sin⁡(Δθ)cos⁡(Δθ)=y−yrx−xr\frac{\sin \left(\Delta \theta\right)}{\cos \left(\Delta \theta\right)}=\frac{y-y_{r}}{x-x_{r}} cos(Δθ)sin(Δθ)=x−xry−yr
可以根据 ∣∣d∣∣||d||∣∣d∣∣ 来确定 ddd 的大小， (y−\left(y-\right.(y− yr)cos⁡(θr)−(x−xr)sin⁡(θr)\left.y_{r}\right) \cos \left(\theta_{r}\right)-\left(x-x_{r}\right) \sin \left(\theta_{r}\right)yr)cos(θr)−(x−xr)sin(θr) 的正负来确定 ddd 的正负号，(y−\left(y-\right.(y− yr)cos⁡(θr)−(x−xr)sin⁡(θr)>0\left.y_{r}\right) \cos \left(\theta_{r}\right)-\left(x-x_{r}\right) \sin \left(\theta_{r}\right)>0yr)cos(θr)−(x−xr)sin(θr)>0，ddd 取正号，否则ddd取负号。公式如下:

d=sign⁡((y−yr)cos⁡(θr)−(x−xr)sin⁡(θr))(x−xr)2+(y−yr)2(2.5)\tag{2.5} d=\operatorname{sign}\left(\left(y-y_{r}\right) \cos \left(\theta_{r}\right)-\left(x-x_{r}\right) \sin \left(\theta_{r}\right)\right) \sqrt{\left(x-x_{r}\right)^{2}+\left(y-y_{r}\right)^{2}} d=sign((y−yr)cos(θr)−(x−xr)sin(θr))(x−xr)2+(y−yr)2(2.5)

至于为什么是(y−\left(y-\right.(y− yr)cos⁡(θr)−(x−xr)sin⁡(θr)\left.y_{r}\right) \cos \left(\theta_{r}\right)-\left(x-x_{r}\right) \sin \left(\theta_{r}\right)yr)cos(θr)−(x−xr)sin(θr) 这一项来判断符号。很简单，因为d=(x−xr,y−yr)d=(x-x_r,y-y_r)d=(x−xr,y−yr)，n⃗r=[−sin⁡θr,cos⁡θr]T\vec{n}_{r} =\left[-\sin \theta_{r}, \cos \theta_{r}\right]^Tnr=[−sinθr,cosθr]T，两者的夹角公式为cos⁡θdn=(y−yr)cos⁡(θr)−(x−xr)sin⁡(θr)(x−xr)2+(y−yr)2\cos{\theta_{dn}}=\frac{\left(y-y_{r}\right) \cos \left(\theta_{r}\right)-\left(x-x_{r}\right) \sin \left(\theta_{r}\right)}{\sqrt{(x-x_r)^2+(y-y_r)^2}}cosθdn=(x−xr)2+(y−yr)2(y−yr)cos(θr)−(x−xr)sin(θr)，显然由分子这一项判断。

3. 求 d˙\dot dd˙

令 t⃗x、n⃗x\vec{t}_{x} 、 \vec{n}_{x}tx、nx 为点 QQQ 处的正交单位向量; t⃗r、n⃗r\vec{t}_{r} 、 \vec{n}_{r}tr、nr 为参考线在投影点 PPP 处的正交单位向量。

根据 vxv_xvx与 s˙\dot{s}s˙定义，因为x⃗=∣∣x⃗∣∣t⃗x\vec{x}=||\vec{x}||\vec{t}_{x}x=∣∣x∣∣tx，所以
x⃗˙=dx⃗dt=d∣∣x⃗∣∣dtt⃗x=vxtx⃗\dot{\vec{x}}=\frac{d\vec{x}}{dt}=\frac{d||\vec{x}||}{dt}\vec{t}_{x}=v_x \vec{t_x} x˙=dtdx=dtd∣∣x∣∣tx=vxtx

同理可以推导出r⃗˙=s˙tr⃗\dot{\vec{r}}=\dot{s}\vec{t_r}r˙=s˙tr.

综上，有
{r⃗˙=s˙tr⃗x⃗˙=vxtx⃗(3.1)\tag{3.1} \left\{ \begin{aligned} \dot{\vec{r}}=\dot{s}\vec{t_r}\\ \dot{\vec{x}}=v_x \vec{t_x} \end{aligned} \right. {r˙=s˙trx˙=vxtx(3.1)

根据公式(2.2)，横向速度 d˙\dot{d}d˙ 为
d˙=(x⃗˙−r⃗˙)T⋅n⃗r+(x⃗−r⃗)T⋅n⃗˙r=vxtx⃗Tn⃗r−s˙tr⃗Tn⃗r+dnr⃗Tn⃗˙r(3.2)\tag{3.2} \begin{aligned} \dot{d}&=(\dot{\vec{x}}-\dot{\vec{r}})^{\mathrm{T}} \cdot \vec{n}_{r}+(\vec{x}-\vec{r})^{\mathrm{T}} \cdot \dot{\vec{n}}_{r}\\ &= v_{x} \vec{t_{x}}^{T} \vec{n}_{r}-\dot s \vec{t_{r}}^{T}\vec{n}_{r}+d\vec{n_{r}}^{T}\dot{\vec{n}}_{r}\\ \end{aligned} d˙=(x˙−r˙)T⋅nr+(x−r)T⋅n˙r=vxtxTnr−s˙trTnr+dnrTn˙r(3.2)

由Frenet公式(1.3)，有
nr⃗′=dnr⃗ds=−κrtr⃗(3.3)\tag{3.3} \vec{n_{r}}'=\frac{d\vec{n_{r}}}{ds}=-\kappa_{r}\vec{t_{r}}nr′=dsdnr=−κrtr(3.3)
于是有
nr⃗˙=dsdtdnr⃗ds=−κrs˙tr⃗(3.4)\tag{3.4} \dot{\vec{n_{r}}}=\frac{ds}{dt}\frac{d\vec{n_{r}}}{ds}=-\kappa_{r}\dot{s}\vec{t_{r}}nr˙=dtdsdsdnr=−κrs˙tr(3.4)
代入公式(3.2)可得：
d˙=vxtx⃗Tn⃗r−s˙tr⃗Tn⃗r+dnr⃗Tn⃗˙r=vxtx⃗Tn⃗r−s˙tr⃗Tn⃗r−ds˙κrnr⃗Tt⃗r(3.5)\tag{3.5} \begin{aligned} \dot{d} &= v_{x} \vec{t_{x}}^{T} \vec{n}_{r}-\dot s \vec{t_{r}}^{T}\vec{n}_{r}+d\vec{n_{r}}^{T}\dot{\vec{n}}_{r}\\ &=v_{x} \vec{t_{x}}^{T} \vec{n}_{r}-\dot s \vec{t_{r}}^{T}\vec{n}_{r}-d\dot s \kappa_r\vec{n_{r}}^{T}\vec{t}_{r}\\ \end{aligned} d˙=vxtxTnr−s˙trTnr+dnrTn˙r=vxtxTnr−s˙trTnr−ds˙κrnrTtr(3.5)

由于 nr⃗\vec{n_{r}}nr与 tr⃗\vec{t_{r}}tr正交, 可得
nr⃗Ttr⃗=tr⃗Tnr⃗=0(3.6)\tag{3.6} \vec{n_{r}}^{T}\vec{t_{r}}=\vec{t_{r}}^{T}\vec{n_{r}}=0nrTtr=trTnr=0(3.6)

又因为
tx⃗Tnr⃗=∣∣tx⃗T∣∣⋅∣∣nr⃗∣∣cos⁡(θx−(θr+π2))=cos⁡(θx−θr−π2)=sin⁡(θx−θr)=sin⁡Δθ(3.7)\tag{3.7} \vec{t_{x}}^{T}\vec{n_{r}}=||\vec{t_{x}}^{T}||\cdot||\vec{n_{r}}||\cos(\theta_x-(\theta_r+\frac{\pi}{2}))=\cos(\theta_{x}-\theta_{r}-\frac{\pi}{2})=\sin(\theta_{x}-\theta_{r})=\sin{\Delta \theta}txTnr=∣∣txT∣∣⋅∣∣nr∣∣cos(θx−(θr+2π))=cos(θx−θr−2π)=sin(θx−θr)=sinΔθ(3.7)

故公式(3.5)化简后，d˙\dot{d}d˙最终为

d˙=vxsin⁡Δθ(3.8)\tag{3.8} \dot d = v_x\sin{\Delta \theta} d˙=vxsinΔθ(3.8)

4. 求 s˙\dot ss˙

对公式(2.1)的x⃗\vec{x}x两边对时间ttt求导，结合公式(3.1)和(3.4)可得
x⃗˙=ddt(r⃗+d⋅n⃗r)⇒vxt⃗x=r⃗˙+d˙⋅n⃗r+d⋅n⃗r˙=s˙(1−κrd)⋅t⃗r+d˙⋅n⃗r(4.1)\tag{4.1} \begin{aligned} \dot{\vec{x}}&=\frac{\mathrm{d}}{\mathrm{d} t}\left(\vec{r}+d \cdot \vec{n}_{r}\right)\\ \Rightarrow v_x\vec{t}_x&=\dot {\vec{r}}+\dot{d}\cdot \vec{n}_{r}+d\cdot \dot{\vec{n}_{r}}\\ &=\dot{s}\left(1-\kappa_{r} d\right) \cdot \vec{t}_r+\dot{d} \cdot \vec{n}_{r} \end{aligned} x˙⇒vxtx=dtd(r+d⋅nr)=r˙+d˙⋅nr+d⋅nr˙=s˙(1−κrd)⋅tr+d˙⋅nr(4.1)

公式(4.1)右乘t⃗r\vec{t}_rtr，且由正交性质得到
vxt⃗x⋅t⃗r=s˙(1−κrd)⋅t⃗r⋅t⃗r+d˙⋅n⃗r⋅t⃗r⇓vxcos⁡(θx−θr)=s˙(1−κrd)⇓vxcos⁡(Δθ)=s˙(1−κrd)(4.2)\tag{4.2} \begin{aligned} v_x\vec{t}_x\cdot\vec{t}_r&=\dot{s}\left(1-\kappa_{r} d\right) \cdot \vec{t}_r\cdot\vec{t}_r+\dot{d} \cdot \vec{n}_{r} \cdot\vec{t}_r\\ &\Downarrow\\ v_x\cos(\theta_x-\theta_r)&=\dot{s}\left(1-\kappa_{r} d\right) \\ &\Downarrow\\ v_x\cos(\Delta \theta)&=\dot{s}\left(1-\kappa_{r} d\right) \end{aligned} vxtx⋅trvxcos(θx−θr)vxcos(Δθ)=s˙(1−κrd)⋅tr⋅tr+d˙⋅nr⋅tr⇓=s˙(1−κrd)⇓=s˙(1−κrd)(4.2)

故s˙\dot{s}s˙为

s˙=vxcos⁡Δθ(1−κrd)(4.3)\tag{4.3} \dot{s} =\frac{v_x\cos{\Delta \theta}}{\left(1-\kappa_{r} d\right) } s˙=(1−κrd)vxcosΔθ(4.3)

5. 求 d′d'd′

因为
d′=dds(d)=dtdsddt(d)=d˙s˙(5.1)\tag{5.1} d^{\prime}=\frac{\mathrm{d}}{\mathrm{d} s}(d)=\frac{\mathrm{d} t}{\mathrm{~d} s} \frac{\mathrm{d}}{\mathrm{d} t}(d)=\frac{\dot{d}}{\dot{s}} d′=dsd(d)= dsdtdtd(d)=s˙d˙(5.1)

将公式(3.8)和(4.3)分别求得的d˙和s˙\dot{d}和\dot{s}d˙和s˙代入，化简后可得

d′=(1−κrd)tan⁡Δθ(5.2)\tag{5.2} d^{\prime}=\left(1-\kappa_{r} d\right) \tan \Delta \theta d′=(1−κrd)tanΔθ(5.2)

6. 求 d′′d''d′′

由式(5.2) 可得
d′′=dds(d′)=(1−κrd)′tan⁡Δθ+(1−κrd)(tan⁡Δθ)′=(1−κrd)′tan⁡Δθ+(1−κrd)(Δθ)′cos⁡2(Δθ)(6.1)\tag{6.1} \begin{aligned} d^{\prime \prime}&=\frac{\mathrm{d}}{\mathrm{d} s}\left(d^{\prime}\right)\\ &=\left(1-\kappa_{r} d\right)^{'} \tan \Delta \theta+\left(1-\kappa_{r} d\right) (\tan \Delta \theta)^{'} \\ &=\left(1-\kappa_{r} d\right)^{'} \tan \Delta \theta+\left(1-\kappa_{r} d\right)\frac{(\Delta \theta)'}{\cos^2(\Delta \theta)} \\ \end{aligned} d′′=dsd(d′)=(1−κrd)′tanΔθ+(1−κrd)(tanΔθ)′=(1−κrd)′tanΔθ+(1−κrd)cos2(Δθ)(Δθ)′(6.1)

设 sxs_{x}sx为车辆当前轨迹 x⃗\vec{x}x的弧长，结合公式(4.3)，有：
d(⋅)ds=dsxdtdtdsd(⋅)dsx=vxs˙d(⋅)dsx=1−κrdcos⁡Δθd(⋅)dsx(6.2)\tag{6.2} \frac{\mathrm{d}(\cdot)}{\mathrm{d}s}=\frac{\mathrm{d}s_{x}}{\mathrm{d}t}\frac{\mathrm{d}t}{\mathrm{d}s}\frac{\mathrm{d}(\cdot)}{\mathrm{d}s_{x}}=\frac{v_{x}}{\dot{s}}\frac{\mathrm{d}(\cdot)}{\mathrm{d}s_{x}}=\frac{1-\kappa_{r}d}{\cos{\Delta\theta}}\frac{\mathrm{d}(\cdot)}{\mathrm{d}s_{x}}dsd(⋅)=dtdsxdsdtdsxd(⋅)=s˙vxdsxd(⋅)=cosΔθ1−κrddsxd(⋅)(6.2)

又根据曲率的定义:
{κr=θr′=dθrdsκx=θx′=dθxdsx(6.3)\tag{6.3} \left\{ \begin{aligned} \kappa_{r}=\theta_r^{'}=\frac{\mathrm{d}\theta_{r}}{\mathrm{d}s} \\ \kappa_{x}=\theta_{x}^{'}=\frac{\mathrm{d}\theta_{x}}{\mathrm{d}s_{x}} \end{aligned} \right. ⎩⎪⎪⎨⎪⎪⎧κr=θr′=dsdθrκx=θx′=dsxdθx(6.3)

注意到 Δθ=θx−θr\Delta \theta=\theta_x-\theta_rΔθ=θx−θr，故结合公式(6.2)和(6.3)有
(Δθ)′=dds(θx−θr)=ddsθx−ddsθr=ddsθx−κr=1−κrdcos⁡Δθdθxdsx−κr=κx1−κrdcos⁡Δθ−κr(6.4)\tag{6.4} \begin{aligned} (\Delta\theta)'&=\frac{\mathrm{d}}{\mathrm{d}s}(\theta_x-\theta_r)\\ &=\frac{\mathrm{d}}{\mathrm{d}s}\theta_x-\frac{\mathrm{d}}{\mathrm{d}s}\theta_r\\ &=\frac{\mathrm{d}}{\mathrm{d}s}\theta_x-\kappa_{r}\\ &=\frac{1-\kappa_{r}d}{\cos{\Delta\theta}}\frac{\mathrm{d}\theta_x}{\mathrm{d}s_{x}}-\kappa_{r}\\ &=\kappa_x\frac{1-\kappa_{r}d}{\cos{\Delta\theta}}-\kappa_{r}\\ \end{aligned} (Δθ)′=dsd(θx−θr)=dsdθx−dsdθr=dsdθx−κr=cosΔθ1−κrddsxdθx−κr=κxcosΔθ1−κrd−κr(6.4)

对于(1−κrd)′\left(1-\kappa_{r} d\right)^{'}(1−κrd)′，有
(1−κrd)′=dds(−κrd)=−(κr′d+κrd′)(6.5)\tag{6.5} \begin{aligned} \left(1-\kappa_{r} d\right)^{'} &=\frac{\mathrm{d}}{\mathrm{d}s}(-\kappa_{r} d)\\ &=-(\kappa_{r}^{\prime} d+\kappa_{r} d^{\prime}) \end{aligned} (1−κrd)′=dsd(−κrd)=−(κr′d+κrd′)(6.5)
故公式(6.1)最终化简得

d′′=−(κr′d+κrd′)tan⁡Δθ+1−κrdcos⁡2Δθ(κx1−κrdcos⁡Δθ−κr)(6.6)\tag{6.6} \begin{aligned} d^{\prime \prime} &=-(\kappa_{r}^{\prime} d+\kappa_{r} d^{\prime}) \tan \Delta \theta+ \frac{1-\kappa_{r} d}{\cos ^{2} \Delta \theta}\left(\kappa_{x} \frac{1-\kappa_{r} d}{\cos \Delta \theta}-\kappa_{r}\right) \end{aligned} d′′=−(κr′d+κrd′)tanΔθ+cos2Δθ1−κrd(κxcosΔθ1−κrd−κr)(6.6)

7. 求 s¨\ddot{s}s¨

因为ax=vx˙=dvxdta_x=\dot{v_x}=\frac{{\rm d}v_x}{{\rm d}t}ax=vx˙=dtdvx，故根据公式(4.3)以及公式(6.2)、(6.3)有

ax=dvxdt=ddt(s˙(1−krd)cos⁡Δθ)=s¨1−krdcos⁡Δθ+s˙ddt(1−krdcos⁡Δθ)=s¨1−krdcos⁡Δθ+s˙dsdtdds(1−krdcos⁡Δθ)=s¨1−krdcos⁡Δθ+s˙2dds(1−krdcos⁡Δθ)=s¨1−krdcos⁡Δθ+s˙2((1−krd)′cos⁡Δθ−(1−krd)(cos⁡Δθ)′)(cos⁡Δθ)2)=s¨1−krdcos⁡Δθ+s˙2(−(kr′d+krd′)cos⁡Δθ+(1−krd)sin⁡(Δθ)(Δθ)′(cos⁡Δθ)2)=s¨1−krdcos⁡Δθ+s˙2cos⁡Δθ((1−krd)tan⁡Δθ(Δθ)′−(kr′d+krd′))(7.1)\tag{7.1} \begin{aligned} a_{x} &=\frac{\mathrm{d} v_{x}}{\mathrm{d} t}\\ &=\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{\dot{s}(1-k_{r} d)}{\cos \Delta \theta}\right)\\ &=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\dot{s}\frac{\mathrm{d}}{\mathrm{d}t}\left(\frac{1-k_{r} d}{\cos \Delta \theta}\right)\\ &=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\dot{s}\frac{\mathrm{d}s}{\mathrm{d}t}\frac{\mathrm{d}}{\mathrm{d}s}\left(\frac{1-k_{r} d}{\cos \Delta \theta}\right)\\ &=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\dot{s}^{2}\frac{\mathrm{d}}{\mathrm{d}s}\left(\frac{1-k_{r} d}{\cos \Delta \theta}\right)\\ &=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\dot{s}^{2}\left(\frac{(1-k_{r} d)^{'}\cos{\Delta\theta}-(1-k_{r} d)(\cos{\Delta\theta})^{'})}{(\cos{\Delta\theta)}^2}\right)\\ &=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\dot{s}^{2}\left(\frac{-(k_{r}^{'}d+k_rd^{'})}{\cos{\Delta\theta}}+\frac{(1-k_rd)\sin(\Delta\theta)(\Delta\theta)^{'}}{(\cos{\Delta\theta})^2}\right)\\ &=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\frac{\dot{s}^{2}}{\cos \Delta \theta}\left((1-k_r d)\tan{\Delta\theta}(\Delta\theta)^{'}-\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right)\right) \\ \end{aligned} ax=dtdvx=dtd(cosΔθs˙(1−krd))=s¨cosΔθ1−krd+s˙dtd(cosΔθ1−krd)=s¨cosΔθ1−krd+s˙dtdsdsd(cosΔθ1−krd)=s¨cosΔθ1−krd+s˙2dsd(cosΔθ1−krd)=s¨cosΔθ1−krd+s˙2((cosΔθ)2(1−krd)′cosΔθ−(1−krd)(cosΔθ)′))=s¨cosΔθ1−krd+s˙2(cosΔθ−(kr′d+krd′)+(cosΔθ)2(1−krd)sin(Δθ)(Δθ)′)=s¨cosΔθ1−krd+cosΔθs˙2((1−krd)tanΔθ(Δθ)′−(kr′d+krd′))(7.1)
最后，将公式(5.2)和(6.4)代入，得

ax=s¨1−krdcos⁡Δθ+s˙2cos⁡Δθ[d′(kx1−krdcos⁡Δθ−kr)−(kr′d+krd′)](7.2)\tag{7.2} a_{x}=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\frac{\dot{s}^{2}}{\cos \Delta \theta}\left[d^{\prime}\left(k_{x} \frac{1-k_{r} d}{\cos \Delta \theta}-k_{r}\right)-\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right)\right] ax=s¨cosΔθ1−krd+cosΔθs˙2[d′(kxcosΔθ1−krd−kr)−(kr′d+krd′)](7.2)

故s¨为\ddot{s}为s¨为

s¨=axcos⁡Δθ−s˙2[d′(kx1−krdcos⁡Δθ−kr)−(kr′d+krd′)]1−krd(7.3)\tag{7.3} \ddot{s}=\frac{a_{x} \cos \Delta \theta-\dot{s}^{2}\left[d^{\prime}\left(k_{x} \frac{1-k_{r} d}{\cos \Delta \theta}-k_{r}\right)-\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right)\right]}{1-k_{r} d} s¨=1−krdaxcosΔθ−s˙2[d′(kxcosΔθ1−krd−kr)−(kr′d+krd′)](7.3)

8. 求d¨\ddot{d}d¨

根据公式(3.8)，易得d¨\ddot{d}d¨为

d¨=axsin⁡Δθ(8.1)\tag{8.1} \ddot d = a_x\sin{\Delta \theta} d¨=axsinΔθ(8.1)

再将公式(7.2)得到的axa_xax代入即可。

2.2 Frenet转Cartesian

首先，作为参考系，投影点P点在笛卡尔坐标系下的坐标,角度,切向量角度以及曲率都已知，即(xr,yr),θr,kr(x_r, y_r), θ_r,k_r(xr,yr),θr,kr。Frenet坐标系下, 车辆的运动状态 [s,s˙,s¨,d,d˙,d¨,d′,d′′]\left[s, \dot{s}, \ddot{s}, d, \dot{d}, \ddot{d} , d^{\prime}, d^{\prime \prime}\right][s,s˙,s¨,d,d˙,d¨,d′,d′′]也已知。

10. 求x⃗\vec{x}x

先推x⃗=(x,y)\vec{x}=(x,y)x=(x,y)。

显然，结合公式(1.4)有
x⃗=r⃗+dnr⃗⇓(x,y)=(xr,yr)+(−dsin⁡θr,dcos⁡θr)(10.1)\tag{10.1} \begin{aligned} \vec{x}&=\vec{r}+d \vec{n_{r}}\\ &\Downarrow\\ (x,y)&=(x_r,y_r)+(-d\sin{\theta_r},d\cos{\theta_r}) \end{aligned} x(x,y)=r+dnr⇓=(xr,yr)+(−dsinθr,dcosθr)(10.1)

故

{x=xr−dsin⁡(θr)y=yr+dcos⁡(θr)(10.2)\tag{10.2} \left\{ \begin{aligned} x&= x_{r}-d\sin \left(\theta_{r}\right) \\ y&= y_{r}+d \cos \left(\theta_{r}\right) \\ \end{aligned} \right. {xy=xr−dsin(θr)=yr+dcos(θr)(10.2)

11. 求vxv_xvx

vx=d∣∣x⃗∣∣dt=∣∣dx⃗dt∣∣=∣∣x⃗˙∣∣=x⃗˙Tx⃗˙(11.1)\tag{11.1} v_x=\frac{d||\vec{x}||}{dt}=||\frac{d\vec{x}}{dt}||=||\dot{\vec{x}}||=\sqrt{\dot{\vec{x}}^T\dot{\vec{x}}} vx=dtd∣∣x∣∣=∣∣dtdx∣∣=∣∣x˙∣∣=x˙Tx˙(11.1)

因为

x⃗˙=d(r⃗+dnr⃗)dt=r⃗˙+d(d)dtnr⃗+ddnr⃗dt=r⃗˙+d(d)dtnr⃗+dn⃗˙r(11.2)\tag{11.2} \dot{\vec{x}}=\frac{\mathrm{d}(\vec{r}+d \vec{n_{r}})}{\mathrm{d}t}=\dot{\vec{r}}+\frac{\mathrm{d}(d)}{\mathrm{d}t}\vec{n_{r}}+d\frac{\mathrm{d}\vec{n_{r}}}{\mathrm{d}t}=\dot{\vec{r}}+\frac{\mathrm{d}(d)}{\mathrm{d}t}\vec{n_{r}}+d\dot{\vec{n}}_r x˙=dtd(r+dnr)=r˙+dtd(d)nr+ddtdnr=r˙+dtd(d)nr+dn˙r(11.2)
结合公式(3.1)和公式(3.4)，有
x⃗˙=s˙t⃗r+d(d)dtnr⃗−κrs˙t⃗r=s˙(1−κrd)t⃗r+d˙nr⃗(11.3)\tag{11.3} \dot{\vec{x}}=\dot{s}\vec{t}_r+\frac{\mathrm{d}(d)}{\mathrm{d}t}\vec{n_{r}}-\kappa_r\dot{s}\vec{t}_r=\dot{s}(1-\kappa_r d)\vec{t}_r+\dot{d}\vec{n_{r}} x˙=s˙tr+dtd(d)nr−κrs˙tr=s˙(1−κrd)tr+d˙nr(11.3)

将公式(11.3)代入公式(11.1)，并且注意t⃗r\vec{t}_rtr和n⃗r\vec{n}_rnr是单位正交的，有:
vx=x⃗˙Tx⃗˙={s˙(1−κrd)t⃗r+d˙nr⃗}T{s˙(1−κrd)t⃗r+d˙nr⃗}={s˙(1−κrd)t⃗rT+d˙nr⃗T}{s˙(1−κrd)t⃗r+d˙nr⃗}=s˙2(1−κr)2+d˙2(11.4)\tag{11.4} \begin{aligned} v_x&=\sqrt{\dot{\vec{x}}^T\dot{\vec{x}}}\\ &=\sqrt{\{\dot{s}(1-\kappa_r d)\vec{t}_r+\dot{d}\vec{n_{r}}\}^T\{\dot{s}(1-\kappa_r d)\vec{t}_r+\dot{d}\vec{n_{r}}\}}\\ &=\sqrt{\{\dot{s}(1-\kappa_r d){\vec{t}_r}^T+\dot{d}{\vec{n_{r}}^T}\}\{\dot{s}(1-\kappa_r d)\vec{t}_r+\dot{d}\vec{n_{r}}\}}\\ &=\sqrt{\dot{s}^2(1-\kappa_r)^2+\dot{d}^2}\\ \end{aligned} vx=x˙Tx˙={s˙(1−κrd)tr+d˙nr}T{s˙(1−κrd)tr+d˙nr}={s˙(1−κrd)trT+d˙nrT}{s˙(1−κrd)tr+d˙nr}=s˙2(1−κr)2+d˙2(11.4)

故最终

vx=s˙2(1−κr)2+d˙2=[s˙(1−krd)]2+(s˙d′)2(11.5)\tag{11.5} \begin{aligned} v_x&=\sqrt{\dot{s}^2(1-\kappa_r)^2+\dot{d}^2}\\ &=\sqrt{\left[\dot{s}\left(1-k_{r} d\right)\right]^{2}+\left(\dot{s} d^{\prime}\right)^{2}} \end{aligned} vx=s˙2(1−κr)2+d˙2=[s˙(1−krd)]2+(s˙d′)2(11.5)

12. 求θx\theta_xθx

θx\theta_xθx由公式(5.2)即得

θx=arctan⁡(d′1−krd)+θr(12.1)\tag{12.1} \theta_{x}= \arctan(\frac{d'}{1-k_r d})+\theta_r θx=arctan(1−krdd′)+θr(12.1)

13. 求axa_xax

axa_xax的求解过程已在前面的过程给出，即公式(7.2)

ax=s¨1−krdcos⁡Δθ+s˙2cos⁡Δθ[d′(kx1−krdcos⁡Δθ−kr)−(kr′d+krd′)](13.1)\tag{13.1} a_{x}=\ddot{s} \frac{1-k_{r} d}{\cos \Delta \theta}+\frac{\dot{s}^{2}}{\cos \Delta \theta}\left[d^{\prime}\left(k_{x} \frac{1-k_{r} d}{\cos \Delta \theta}-k_{r}\right)-\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right)\right] ax=s¨cosΔθ1−krd+cosΔθs˙2[d′(kxcosΔθ1−krd−kr)−(kr′d+krd′)](13.1)

14. 求κx\kappa_xκx

曲率的计算根据公式(6.6)，可得

kx=((d′′+(kr′d+krd′)tan⁡Δθ)cos⁡2Δθ1−krd+kr)cos⁡Δθ1−krd(14.1)\tag{14.1} k_{x}= \left(\left(d^{\prime \prime}+\left(k_{r}^{\prime} d+k_{r} d^{\prime}\right) \tan \Delta \theta\right) \frac{\cos ^{2}\Delta \theta}{1-k_{r}d}+k_{r}\right) \frac{\cos \Delta \theta}{1-k_{r} d} kx=((d′′+(kr′d+krd′)tanΔθ)1−krdcos2Δθ+kr)1−krdcosΔθ(14.1)

所有转换公式的整合以及python代码实现见上一篇博客。

【自动驾驶】Frenet坐标系与Cartesian坐标系（二）相关推荐

【自动驾驶】20.自动驾驶中的各种时空坐标系
简介介绍自动驾驶技术中几种常用的坐标系统,以及他们之间如何完成关联和转换,最终构建出统一的环境模型. 所谓时空坐标系,包括三维空间坐标系和一维时间坐标系.在此基础上,用解析的形式(坐标)把物体在空间 ...
【cartographer】园区场景自动驾驶-利用GPS实现ENU坐标系下的建图与定位
园区场景自动驾驶-利用GPS实现当地地理坐标系下的建图与定位概述主要实现如下的功能: 1.利用gps信息,修正题图为当地地理ENU坐标系: 2.在纯定位模式下,利用gps与imu(rtk)信息,实 ...
【自动驾驶】33.【图像坐标系】到【像素坐标系】的度量单位变换、【英寸】、【感光芯片】
我们理论分析的时候,使用小孔成像模型来分析. 其中,(u,v)(u,v)(u,v)为像素坐标,(cx,cy)(c_x, c_y)(cx,cy)表示相机主点(principal point),即相机 ...
自动驾驶轨迹预测论文阅读（二）TPNet: Trajectory Proposal Network for Motion Prediction
论文链接:https://openaccess.thecvf.com/content_CVPR_2020/papers/Fang_TPNet_Trajectory_Proposal_Network_f ...
自动驾驶感知-车道线系列（二）——Canny边缘检测
Canny边缘检测前言一.Canny是什么? 二.算法详细步骤 1. 平滑处理 2. 梯度检测 3. 非极大值抑制 4. 滞后阈值处理三.函数原型四.应用实例五.总结前言边缘检测是图像处 ...
自动驾驶系类文档（二）-- 定位模块（Location）
二.高精度定位模块(Location) 目录二.高精度定位模块(Location) 2.1.定位技术 2.1.1.GNSS 2.1.2.INS
Tesla FSD 自动驾驶软件升级版本说明书解析二（Beta v10.12 Release Notes）
Tesla FSD软件的版本说明书写得十分技术流,里面会详细列举新增了什么feature.修复了什么bug.采用了什么方法(算法还是数据等)提升了多少指标之类的信息,有助于从业人员了解Tesla的底层 ...
matlab自动加坐标,matlab自动驾驶系统-坐标系
自动驾驶工具箱中的坐标系自动驾驶工具箱中的坐标系自动驾驶工具箱™使用以下坐标系: 世界:固定的通用坐标系,所有车辆及其传感器都放置在其中. 车辆:停泊在自我车辆上.通常,车辆坐标系放置在后轴中点正 ...
【自动驾驶】2.车载以太网 - SOME/IP简介
车载以太网 - SOME/IP简介 SOME/IP (Scalable service-Oriented MiddlewarE over IP) 是车载以太网通信引入的一个概念,位于OSI 7层模型的 ...

【自动驾驶】Frenet坐标系与Cartesian坐标系（二）

文章目录

参考资料

2. Frenet坐标系与全局笛卡尔坐标系转换

2.1 Cartesian转 Frenet

1. 求 sss

2. 求 ddd

3. 求 d˙\dot dd˙

4. 求 s˙\dot ss˙

5. 求 d′d'd′

6. 求 d′′d''d′′

7. 求 s¨\ddot{s}s¨

8. 求d¨\ddot{d}d¨

2.2 Frenet转Cartesian

10. 求x⃗\vec{x}x

11. 求vxv_xvx

12. 求θx\theta_xθx

13. 求axa_xax

14. 求κx\kappa_xκx

【自动驾驶】Frenet坐标系与Cartesian坐标系（二）相关推荐

最新文章

热门文章

【自动驾驶】Frenet坐标系与Cartesian坐标系（二）

文章目录

参考资料

2. Frenet坐标系与全局笛卡尔坐标系转换

2.1 Cartesian转 Frenet

1. 求 sss

2. 求 ddd

3. 求 d˙\dot dd˙

4. 求 s˙\dot ss˙

5. 求 d′d'd′

6. 求 d′′d''d′′

7. 求 s¨\ddot{s}s¨

8. 求d¨\ddot{d}d¨

2.2 Frenet转Cartesian

10. 求x⃗\vec{x}x

11. 求vxv_xvx​

12. 求θx\theta_xθx​

13. 求axa_xax​

14. 求κx\kappa_xκx​

【自动驾驶】Frenet坐标系与Cartesian坐标系（二）相关推荐

最新文章

热门文章

11. 求vxv_xvx

12. 求θx\theta_xθx

13. 求axa_xax

14. 求κx\kappa_xκx