本文详细介绍了自动驾驶中的Frenet坐标与Cartesian坐标之间的转换，包括直角坐标到自然坐标的转换公式，涉及向量形式和三角函数形式的计算方法，以及Frenet坐标到直角坐标的转换。内容涵盖了速度、曲率、加速度等关键参数的计算，是理解自动驾驶路径规划的重要概念。
本文根据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)
扫一扫
421
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
