本文根据B站up：忠厚老实的老王 的视频《自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换》整理，连接：https://www.bilibili.com/video/BV1tQ4y1r7fh?spm_id_from=333.1007.top_right_bar_window_history.content.click
Planning-Frenet坐标和Cartesian坐标转换上
Planning-Frenet坐标和Cartesian坐标转换中
2.4 F r e n e t Frenet Frenet坐标系转换到 C a r t e s i a n Cartesian Cartesian坐标系
已知 r r ⃗ , v r ⃗ , κ r , s , s ˙ , s ¨ , l , l ′ , l ′ ′ \vec{r_r}, \vec{v_r}, \kappa_r,s, \dot{s}, \ddot{s}, l, l^{\prime}, l^{\prime \prime} rr,vr,κr,s,s˙,s¨,l,l′,l′′。
2.4.1 计算 ( x h , y h ) (x_h, y_h) (xh,yh)
{ x h = x r − l s i n ( θ r ) y h = y r + l c o s ( θ r ) (2-25) \begin{cases} x_h = x_r - lsin(\theta_r) \\ y_h = y_r + lcos(\theta_r) \end{cases} \tag{2-25} { xh=xr−lsin(θr)yh=yr+lcos(θr)(2-25)
2.4.2 计算 θ h \theta_h θh
由公式 ( 2 − 14 ) (2-14) (2−14)得：
θ h = a r c t a n ( l ′ 1 − κ r l ) + θ r (2-26) \theta_h = arctan(\frac{l^{\prime}}{1-\kappa_r l}) + \theta_r \tag{2-26} θh=arctan(1−κrll′)+θr(2-26)
2.4.3 计算 ∣ v h ∣ |v_h| ∣vh∣
由 ( 2 − 11 ) (2-11) (2−11)得
∣ v h ⃗ ∣ c o s ( θ h − θ r ) = s r ˙ ( 1 − κ r l ) (2-27) |\vec{v_h}| cos(\theta_h - \theta_r) = \dot{s_r} (1-\kappa_r l) \tag{2-27} ∣vh∣cos(θh−θr)=sr˙(1−κrl)(2-27)
由 ( 2 − 11 ) , ( 2 − 14 ) (2-11),(2-14) (2−11),(2−14)等号两边同时相乘得
s r ˙ l ′ = ∣ v h ⃗ ∣ s i n ( θ h − θ r ) (2-28) \dot{s_r} l^{\prime} = |\vec{v_h}| sin(\theta_h - \theta_r) \tag{2-28} sr˙l′=∣vh∣sin(θh−θr)(2-28)
所以
( ∣ v h ⃗ ∣ ) 2 ( c o s 2 ( θ h − θ r ) + s i n 2 ( θ h − θ r ) ) = ( s r ˙ ( 1 − κ r l ) ) 2 + ( s r ˙ l ′ ) 2 (2-29) (|\vec{v_h}|)^2 (cos^2(\theta_h - \theta_r) + sin^2(\theta_h - \theta_r)) = (\dot{s_r} (1-\kappa_r l))^2 + (\dot{s_r} l^{\prime})^2 \tag{2-29} (∣vh∣)2(cos2(θh−θr)+sin2(θh−θr))=(sr˙(1−κrl))2+(sr˙l′)2(2-29)
∣ v h ⃗ ∣ = ( s r ˙ ( 1 − κ r l ) ) 2 + ( s r ˙ l ′ ) 2 (2-30) |\vec{v_h}|= \sqrt{(\dot{s_r} (1-\kappa_r l))^2 + (\dot{s_r} l^{\prime})^2} \tag{2-30} ∣vh∣=(sr˙(1−κrl))2+(sr˙l′)2(2-30)
2.4.4 计算 κ h \kappa_h κh
由 ( 2 − 24 ) (2-24) (2−24)得：
κ h = ( ( l ′ ′ + ( κ r ′ l + κ r l ′ ) t a n ( θ h − θ r ) ) c o s 2 ( θ h − θ r ) 1 − κ r l + κ r ) c o s ( θ h − θ r ) 1 − κ r l (2-31) \kappa_h = ((l^{\prime \prime} + ({\kappa_r}^{\prime}l + \kappa_r l^{\prime}) tan(\theta_h - \theta_r)) \frac{cos^2(\theta_h - \theta_r)}{1-\kappa_r l} + \kappa_r) \frac{cos(\theta_h - \theta_r)}{1-\kappa_r l} \tag{2-31} κh=((l′′+(κr′l+κrl′)tan(θh−θr))1−κrlcos2(θh−θr)+κr)1−κrlcos(θh−θr)(2-31)
2.4.5 计算 ∣ a h ∣ |a_h| ∣ah∣
有 ( 2 − 18 ) , ( 2 − 31 ) (2-18),(2-31) (2−18),(2−31)得：
∣ a h ∣ = s r ¨ c o s ( θ h − θ r ) 1 − κ r l + s r ˙ 2 c o s ( θ h − θ r ) [ l ′ ( 1 − κ r l c o s ( θ h − θ r ) κ h − κ r ) − ( κ r ′ l + κ r l ′ ) ] (2-32) |a_h| = \ddot{s_r} \frac{cos(\theta_h - \theta_r)}{1-\kappa_r l} + \frac{
{\dot{s_r}}^2}{cos(\theta_h - \theta_r)} [l ^{\prime}(\frac{1-\kappa_r l}{cos(\theta_h - \theta_r)} \kappa_h - \kappa_r) - ({\kappa_r}^{\prime}l + \kappa_r l^{\prime})] \tag{2-32} ∣ah∣=sr¨1−κrlcos(θh−θr)+cos(θh−θr)sr˙2[l′(cos(θh−θr)1−κrlκh−κr)−(κr′l+κrl′)](2-32)
2.5 总结
2.5.1 C a r t e s i a n Cartesian Cartesian坐标系转换到 F r e n e t Frenet Frenet坐标系
2.5.1.1 向量形式
{ l = ( r h ⃗ − r r ⃗ ) ⋅ n r ⃗ s r ˙ = v h ⃗ ⋅ τ r ⃗ 1 − κ r l l ˙ = v h ⃗ ⋅ n r ⃗ l ′ = ( 1 − κ r l ) v h ⃗ ⋅ n r ⃗ v h ⃗ ⋅ τ r ⃗ s r ¨ = a ⃗ ⋅ τ r ⃗ 1 − κ r l + κ r s r ˙ 2 l ′ 1 − κ r l + s r ˙ 2 1 − κ r l ( κ r ′ l + κ r l ′ ) l ¨ = a ⃗ ⋅ n r ⃗ − κ r ( 1 − κ r l ) ( s r ˙ ) 2 l ′ ′ = l ¨ − l ′ s r ¨ s r ˙ 2 (3-33) \begin{cases} l = (\vec{r_h} - \vec{r_r}) \cdot \vec{n_r} \\ \dot{s_r} = \frac{\vec{v_h} \cdot \vec{\tau_r}}{1-\kappa_r l} \\ \dot{l} = \vec{v_h} \cdot \vec{n_r} \\ l^{\prime} = (1-\kappa_r l) \frac{\vec{v_h} \cdot \vec{n_r}}{\vec{v_h} \cdot \vec{\tau_r}} \\ \ddot{s_r} = \frac{\vec{a} \cdot \vec{\tau_r}}{1-\kappa_r l} + \frac{\kappa_r \dot{s_r}^2 l^{\prime}}{1-\kappa_r l} + \frac{\dot{s_r}^2}{1-\kappa_r l}({\kappa_r}^{\prime}l + \kappa_r l^{\prime}) \\ \ddot{l} = \vec{a} \cdot \vec{n_r} - \kappa_r (1-\kappa_r l) (\dot{s_r})^2 \\ l^{\prime \prime} = \frac{\ddot{l} - l^{\prime} \ddot{s_r}}{ {\dot{s_r}}^2} \end{cases} \tag{3-33} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧l=(rh−rr)⋅nrsr˙=1−κrlvh⋅τrl˙=vh⋅nrl′=(1−κrl)vh⋅τrvh⋅nrsr¨=1−κrla⋅τr+1−κrlκrsr˙2l′+1−κrlsr˙2(κr′l+κrl′)l¨=a⋅nr−κr(1−κrl)(sr˙)2l′′=sr˙2l¨−l′sr¨(3-33)
2.5.1.2 三角函数形式
{ l = s i g n ( ( y h − y r ) c o s ( θ r ) − ( x h − x r ) s i n ( θ r ) ) ( y h − y r ) 2 − ( x h − x r ) 2 s r ˙ = ∣ v h ⃗ ∣ c o s ( θ h − θ r ) 1 − κ r l l ˙ = ∣ v h ⃗ ∣ s i n ( θ h − θ r ) l ′ = ( 1 − κ r l ) t a n ( θ h − θ r ) s r ¨ = ∣ a h ⃗ ∣ c o s ( θ h − θ r ) − s r ˙ 2 ( l ′ ( κ h 1 − κ r l c o s ( θ h − θ r ) − κ r ) − ( κ r ′ l + κ r l ′ ) ) 1 − κ r l l ¨ = ∣ a ⃗ ∣ s i n ( θ h − θ r ) − κ r ( 1 − κ r l ) ( s r ˙ ) 2 l ′ ′ = − ( κ r ′ l + κ r l ′ ) t a n ( θ h − θ r ) + 1 − κ r l c o s 2 ( θ h − θ r ) ( 1 − κ r l c o s ( θ h − θ r ) κ h − κ r ) (3-34) \begin{cases} l = sign((y_h - y_r) cos(\theta_r) - (x_h - x_r)sin(\theta_r)) \sqrt{(y_h - y_r)^2 - (x_h - x_r)^2} \\ \dot{s_r} = \frac{|\vec{v_h}| cos(\theta_h - \theta_r)}{1-\kappa_r l} \\ \dot{l} = |\vec{v_h}| sin(\theta_h - \theta_r) \\ l^{\prime} = (1-\kappa_r l) tan(\theta_h - \theta_r) \\ \ddot{s_r} = \frac{|\vec{a_h}| cos(\theta_h - \theta_r) - {\dot{s_r}}^2(l^{\prime} (\kappa_h \frac{1-\kappa_r l}{cos(\theta_h - \theta_r)} - \kappa_r) - ({\kappa_r}^{\prime}l + \kappa_r l^{\prime}))} {1-\kappa_r l} \\ \ddot{l} = |\vec{a}| sin(\theta_h - \theta_r) - \kappa_r (1-\kappa_r l) (\dot{s_r})^2 \\ l^{\prime \prime} = -({\kappa_r}^{\prime}l + \kappa_r l^{\prime}) tan(\theta_h - \theta_r) + \frac{1-\kappa_r l}{cos^2(\theta_h - \theta_r)} (\frac{1-\kappa_r l}{cos(\theta_h - \theta_r)} \kappa_h - \kappa_r) \end{cases} \tag{3-34} ⎩⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎪⎧l=sign((yh−yr)cos(θr)−(xh−xr)sin(θr))(yh−yr)2−(xh−xr)2sr˙=1−κrl∣vh∣cos(θh−θr)l˙=∣vh∣sin(θh−θr)l′=(1−κrl)tan(θh−θr)sr¨=1−κrl∣ah∣cos(θh−θr)−sr˙2(l′(κhcos(θh−θr)1−κrl−κr)−(κr′l+κrl′))l¨=∣a∣sin(θh−θr)−κr(1−κrl)(sr˙)2l′′=−(κr′l+κrl′)tan(θh−θr)+cos2(θh−θr)1−κrl(cos(θh−θr)1−κrlκh−κr)(3-34)
2.5.2 F r e n e t Frenet Frenet坐标系转换到 C a r t e s i a n Cartesian Cartesian坐标系
{ x h = x r − l s i n ( θ r ) y h = y r + l c o s ( θ r ) ∣ v h ⃗ ∣ = ( s r ˙ ( 1 − κ r l ) ) 2 + ( s r ˙ l ′ ) 2 κ h = ( ( l ′ ′ + ( κ r ′ l + κ r l ′ ) t a n ( θ h − θ r ) ) c o s 2 ( θ h − θ r ) 1 − κ r l + κ r ) c o s ( θ h − θ r ) 1 − κ r l ∣ a h ∣ = s r ¨ c o s ( θ h − θ r ) 1 − κ r l + s r ˙ 2 c o s ( θ h − θ r ) [ l ′ ( 1 − κ r l c o s ( θ h − θ r ) κ h − κ r ) − ( κ r ′ l + κ r l ′ ) ] (3-35) \begin{cases} x_h = x_r - lsin(\theta_r) \\ y_h = y_r + lcos(\theta_r) \\ |\vec{v_h}|= \sqrt{(\dot{s_r} (1-\kappa_r l))^2 + (\dot{s_r} l^{\prime})^2} \\ \kappa_h = ((l^{\prime \prime} + ({\kappa_r}^{\prime}l + \kappa_r l^{\prime}) tan(\theta_h - \theta_r)) \frac{cos^2(\theta_h - \theta_r)}{1-\kappa_r l} + \kappa_r) \frac{cos(\theta_h - \theta_r)}{1-\kappa_r l} \\ |a_h| = \ddot{s_r} \frac{cos(\theta_h - \theta_r)}{1-\kappa_r l} + \frac{ {\dot{s_r}}^2}{cos(\theta_h - \theta_r)} [l ^{\prime}(\frac{1-\kappa_r l}{cos(\theta_h - \theta_r)} \kappa_h - \kappa_r) - ({\kappa_r}^{\prime}l + \kappa_r l^{\prime})] \end{cases} \tag{3-35} ⎩⎪⎪⎪⎪⎪⎪⎨⎪⎪⎪⎪⎪⎪⎧xh=xr−lsin(θr)yh=yr+lcos(θr)∣vh∣=(sr˙(1−κrl))2+(sr˙l′)2κh=((l′′+(κr′l+κrl′)tan(θh−θr))1−κrlcos2(θh−θr)+κr)1−κrlcos(θh−θr)∣ah∣=sr¨1−κrlcos(θh−θr)+cos(θh−θr)sr˙2[l′(cos(θh−θr)1−κrlκh−κr)−(κr′l+κrl′)](3-35)