03Frenet与Cardesian坐标系(Frenet转Cardesian公式推导)
Frenet转Cardesian
1 明确目标
已知车辆质点在Frenet坐标系下的状态：
- Frenet 坐标系下的纵向坐标： s s s
- 纵向速度： s ˙ \dot{s} s˙
- 纵向加速度： s ¨ \ddot{s} s¨
- 横向坐标： l l l
- 横向速度： l ˙ \dot{l} l˙
- 横向加速度： l ¨ \ddot{l} l¨
- 横向坐标变化率： l ′ l' l′
- 横向坐标的二阶变化率： l ′ ′ l'' l′′
已知参考线上任意点状态（下述状态在参考线生成后都可以通过计算得到）：
- 参考线上的任意点： s r s_r sr和其对应的 r r ⃗ \vec{r_r} rr
- 参考线上任意点的单位切向量： τ r ⃗ \vec{\tau_r} τr
- 参考线上任意点的单位法向量： n r ⃗ \vec{n_r} nr
- 参考线上任意点的曲率： κ r \kappa_r κr
- 参考线上任意点的曲率变化率： κ r ′ \kappa_r' κr′
求解车辆质点在Cardesian坐标系下的状态：
- 位置向量： r h ⃗ \vec{r_h} rh
- 速度向量： v h ⃗ \vec{v_h} vh
- 切线方向与 x x x轴夹角： θ h \theta_h θh
- 加速度向量： a h ⃗ \vec{a_h} ah
- 曲率： κ h \kappa_h κh
2 公式推导
2.1 基础公式汇总
为了方便查阅，现将前文《01》推导的Frenet基础公式汇总如下：
- 核心关系
r h ⃗ = r r ⃗ + l n r ⃗ (2-1) \vec{r_h} = \vec{r_r} + l\vec{n_r}\tag{2-1} rh=rr+lnr(2-1)
- Frenet最小状态集
l ˙ = l ′ s ˙ (2-2) \dot{l} = l'\dot{s}\tag{2-2} l˙=l′s˙(2-2)
l ¨ = l ′ ′ s ˙ 2 + l ′ s ¨ (2-3) \ddot{l} = l''\dot{s}^2+l'\ddot{s}\tag{2-3} l¨=l′′s˙2+l′s¨(2-3)
-
车辆在Cardesian下的向量导数
r h ⃗ ˙ = v h ⃗ = ∣ v h ⃗ ∣ τ h ⃗ = v h τ h ⃗ (2-4) \dot{\vec{r_h}} = \vec{v_h}= |\vec{v_h}|\vec{\tau_h}=v_h\vec{\tau_h}\tag{2-4} rh˙=vh=∣vh∣τh=vhτh(2-4)τ h ⃗ ˙ = κ h v h n h ⃗ (2-5) \dot{\vec{\tau_h}} = \kappa_hv_h\vec{n_h}\tag{2-5} τh˙=κhvhnh(2-5)
n h ⃗ ˙ = − κ h v h τ h ⃗ (2-6) \dot{\vec{n_h}} =-\kappa_hv_h\vec{\tau_h}\tag{2-6} nh˙=−κhvhτh(2-6)
v h ⃗ ˙ = a h ⃗ = a τ τ h ⃗ + κ h v h 2 n h ⃗ (2-7) \dot{\vec{v_h}} = \vec{a_h} = a_{\tau}\vec{\tau_h} +\kappa_h v_h^2 \vec{n_h}\tag{2-7} vh˙=ah=aττh+κhvh2nh(2-7)
-
匹配点在Cardesian下的向量导数
r r ⃗ ˙ = v r ⃗ = ∣ v r ⃗ ∣ τ r ⃗ = s ˙ τ r ⃗ (2-8) \dot{\vec{r_r}} = \vec{v_r}= |\vec{v_r}|\vec{\tau_r}=\dot{s}\vec{\tau_r}\tag{2-8} rr˙=vr=∣vr∣τr=s˙τr(2-8)τ r ⃗ ˙ = κ r s ˙ n r ⃗ (2-9) \dot{\vec{\tau_r}} = \kappa_r \dot{s} \vec{n_r}\tag{2-9} τr˙=κrs˙nr(2-9)
n r ⃗ ˙ = − κ r s ˙ τ r ⃗ (2-10) \dot{\vec{n_r}} =-\kappa_r \dot{s}\vec{\tau_r}\tag{2-10} nr˙=−κrs˙τr(2-10)
2.2 求解 r h ⃗ \vec{r_h} rh
根据（2-1）有：
r h ⃗ = r r ⃗ + l n r ⃗ (2-11) \vec{r_h} = \vec{r_r} + l\vec{n_r}\tag{2-11} rh=rr+lnr(2-11)
2.3 求解 v h ⃗ \vec{v_h} vh
v h ⃗ = r h ⃗ ˙ = r r ⃗ ˙ + l ˙ n r ⃗ + l n r ⃗ ˙ = s τ r ⃗ + l ˙ n r ⃗ − κ r s ˙ τ r ⃗ (2-12) \begin{align} \vec{v_h} &= \dot{\vec{r_h}}\\ &= \dot{\vec{r_r}} + \dot{l}\vec{n_r} + l\dot{\vec{n_r}}\\ &= s\vec{\tau_r} + \dot{l}\vec{n_r} -\kappa_r\dot{s}\vec{\tau_r} \end{align}\tag{2-12} vh=rh˙=rr˙+l˙nr+lnr˙=sτr+l˙nr−κrs˙τr(2-12)
可以进一步求得
v h = ∣ v h ⃗ ∣ θ h = a r g ( v h ⃗ ) (2-13) \begin{align} v_h &= |\vec{v_h}|\\ \theta_h&=arg(\vec{v_h}) \end{align}\tag{2-13} vhθh=∣vh∣=arg(vh)(2-13)
其中，arg代表求向量的相位角。
求出了 v h ⃗ \vec{v_h} vh，可以进一步求得
τ h ⃗ = v h ⃗ v h (2-14) \vec{\tau_h} = \frac{\vec{v_h}}{v_h} \tag{2-14} τh=vhvh(2-14)
对于二维平面，
n h ⃗ = A τ h ⃗ = [ 0 − 1 1 0 ] τ h ⃗ (2-15) \begin{align} \vec{n_h} &= A\vec{\tau_h} \\ &= \left[ \begin{array}{cc} 0 & -1 \\ 1 & 0 \\ \end{array} \right]\vec{\tau_h} \end{align}\tag{2-15} nh=Aτh=[01−10]τh(2-15)
其中， A A A 是一个二维变换矩阵
Tips：如果是编码的话，（2-15）的乘法也可以直接通过调换坐标进行等价替换，可以进一步提升计算效率。例如， τ h ⃗ = ( x x 2 + y 2 , y x 2 + y 2 ) \vec{\tau_h}=(\frac{x}{\sqrt{x^2+y^2}},\frac{y}{\sqrt{x^2+y^2}}) τh=(x2+y2x,x2+y2y)，则 n h ⃗ = ( − y x 2 + y 2 , x x 2 + y 2 ) \vec{n_h}=(\frac{-y}{\sqrt{x^2+y^2}},\frac{x}{\sqrt{x^2+y^2}}) nh=(x2+y2−y,x2+y2x)。
2.4 求解 a h ⃗ \vec{a_h} ah
a h ⃗ = v h ⃗ ˙ = s ˙ τ r ⃗ + s τ r ⃗ ˙ + l ¨ n r ⃗ + l ˙ n r ⃗ ˙ − κ r ( s ¨ τ r ⃗ + s ˙ τ r ⃗ ˙ ) = s ˙ τ r ⃗ + s κ r s ˙ n r ⃗ + l ¨ n r ⃗ + （ − κ r s ˙ τ r ⃗ l ˙ ) + （ − κ r s ¨ τ r ⃗ ) + ( − κ r s ˙ κ r s ˙ n r ⃗ ) = [ ( 1 − κ r l ˙ ) s ˙ − κ r s ¨ ] τ r ⃗ + ( κ r s s ˙ + l ¨ − κ r 2 s ˙ 2 ) n r ⃗ (2-16) \begin{align} \vec{a_h} &= \dot{\vec{v_h}}\\ &= \dot{s}\vec{\tau_r}+s\dot{\vec{\tau_r}} + \ddot{l}\vec{n_r} + \dot{l}\dot{\vec{n_r}} - \kappa_r(\ddot{s}\vec{\tau_r} + \dot{s}\dot{\vec{\tau_r}})\\ &= \dot{s}\vec{\tau_r} + s\kappa_r \dot{s} \vec{n_r} + \ddot{l}\vec{n_r} +（-\kappa_r \dot{s}\vec{\tau_r}\dot{l})+（-\kappa_r\ddot{s}\vec{\tau_r}) + (-\kappa_r\dot{s}\kappa_r \dot{s} \vec{n_r})\\ &=[(1-\kappa_r\dot{l})\dot{s}-\kappa_r\ddot{s}]\vec{\tau_r} + (\kappa_rs\dot{s}+\ddot{l}-\kappa_r^2\dot{s}^2)\vec{n_r} \end{align}\tag{2-16} ah=vh˙=s˙τr+sτr˙+l¨nr+l˙nr˙−κr(s¨τr+s˙τr˙)=s˙τr+sκrs˙nr+l¨nr+（−κrs˙τrl˙)+（−κrs¨τr)+(−κrs˙κrs˙nr)=[(1−κrl˙)s˙−κrs¨]τr+(κrss˙+l¨−κr2s˙2)nr(2-16)
可以进一步求得
a τ = a h ⃗ τ h ⃗ a n = a h ⃗ n h ⃗ (2-17) \begin{align} a_\tau &= \vec{a_h}\vec{\tau_h} \\ a_n &=\vec{a_h}\vec{n_h} \end{align}\tag{2-17} aτan=ahτh=ahnh(2-17)
2.5 求解 κ h \kappa_h κh
由（2-7）可得
κ h = a n v h 2 (2-18) \kappa_h = \frac{a_n}{v_h^2} \tag{2-18} κh=vh2an(2-18)
3 总结
- 还是坚持使用向量推导的一贯作风，不引入三角函数，会使得表达式更简洁；
- 编码时使用向量实现是否比三角函数更快还需要进一步实验（理论上会更快）。