自动驾驶之直角坐标与自然坐标转换
上一讲:b站老王 自动驾驶决策规划学习记录(三)
接着上一讲学习记录b站老王对自动驾驶规划系列的讲解
参考视频:
0 前言
龙格现象:高次多项式拟合可能会出现震荡,慎用高次多项式。
尽可能用分段低次多项式去拟合,而不是高次多项式。
在数学中,笛卡尔坐标系 (Cartesian coordinate system) 是一种正交坐标系,亦称为直角坐标系。
用向量法推导,降低推导难度
车:host_vehicle
问题:
已知车在Cartesian坐标系下的 r h ⃗ , v h ⃗ , a h ⃗ , k h \vec{r_h} ,\vec{v_h} ,\vec{a_h} ,k_h rh,vh,ah,kh,求车在以道路为坐标轴的frenet坐标下的坐标 s , s ˙ , s ¨ , l , l ′ , l ′ ′ , l ˙ , l ¨ s,\dot s,\ddot s,l,l{}' ,l{}'' ,\dot l,\ddot l s,s˙,s¨,l,l′,l′′,l˙,l¨
其中, s ˙ = d S d t , l ˙ = d l d t , l ′ = d l d S \dot s=\frac{dS}{dt},\dot l=\frac{dl}{dt},l{}'=\frac{dl}{dS} s˙=dtdS,l˙=dtdl,l′=dSdl
在推导中,难度最大的就是曲线坐标系(别扭)。
曲线坐标系与直角坐标系的两点不同:
-
曲线坐标系的基向量一般不是常向量
-
点的曲线坐标变化与点的实际位移一般不一致
1 预备知识
1.1 预备知识1
拓展:
1.2 预备知识2
frenet公式
扩展1:
l
,
l
′
,
l
′
′
l,l{}' ,l{}''
l,l′,l′′中的
l
′
=
d
l
d
S
l{}'=\frac{dl}{dS}
l′=dSdl,
d
S
dS
dS描述的是frenet坐标系下坐标轴曲线的弧微分。
曲率 k h , k r k_h,k_r kh,kr都只是在直角坐标系下的曲率
扩展2:
1.3 总结
以下变量都以Cartesian坐标为准
r
n
⃗
\vec{r_n}
rn:车的位矢
v
⃗
\vec{v}
v:车的速度
a
⃗
\vec{a}
a:车的加速度
k
h
k_h
kh:车的位矢在车的轨迹上的曲率
τ
h
⃗
\vec{\tau _h}
τh:车的位矢在车的轨迹上的切线方向单位向量
n
h
⃗
\vec{ n_h}
nh:车的位矢在车的轨迹上的法线方向单位向量
r
r
⃗
\vec{r_r}
rr:投影位矢
S
˙
\dot S
S˙:投影速率
k
r
k_r
kr:投影位矢在道路几何上的曲率
τ
r
⃗
\vec{\tau _r}
τr:投影位矢在道路几何上的切线方向单位向量
n
r
⃗
\vec{n _r}
nr:投影位矢在道路几何上的法线方向单位向量
求自然坐标系和直角坐标系之间转化的关键
七个辅助公式:
问题转化为:
分三步:
- 7个辅助公式
- 找到车在
f
r
e
n
e
t
frenet
frenet坐标系下的投影点在Cartesion的坐标,记为
x
r
,
y
r
,
θ
r
,
k
r
x_r,y_r,\theta_r,k_r
xr,yr,θr,kr
- 利用向量三角形,以及微积分求出
S
,
S
˙
,
S
¨
,
l
,
l
′
,
l
′
′
S,\dot S,\ddot S,l,l_{}',l_{}''
S,S˙,S¨,l,l′,l′′
核心公式
: r ⃗ r + l n ⃗ r = r ⃗ h \vec r_r+l\vec n_r=\vec r_h rr+lnr=rh
如何找到
(
x
h
,
y
h
)
(x_h,y_h)
(xh,yh)在frenet坐标下的投影
x
r
,
y
r
,
θ
r
,
k
r
x_r,y_r,\theta_r,k_r
xr,yr,θr,kr暂时先不管,假设已找到了
x
r
,
y
r
,
θ
r
,
k
r
x_r,y_r,\theta_r,k_r
xr,yr,θr,kr,自然可得到
r
⃗
r
,
τ
⃗
r
,
n
⃗
r
,
k
r
\vec r_r,\vec \tau_r,\vec n_r,k_r
rr,τr,nr,kr
博客的公式:
2 推导过程
1.如何找到 x r , y r , θ r , k r x_r,y_r,\theta_r,k_r xr,yr,θr,kr
2.如何计算S
3.如何从 S , S ˙ , S ¨ , l , l ˙ , l ¨ , l ′ , l ′ ′ S,\dot S,\ddot S,l,\dot l,\ddot l,l_{}',l_{}'' S,S˙,S¨,l,l˙,l¨,l′,l′′到 r ⃗ h , v ⃗ h , a ⃗ h , k h \vec r_h,\vec v_h,\vec a_h,k_h rh,vh,ah,kh
2.1 如何找到 x r , y r , θ r , k r x_r,y_r,\theta_r,k_r xr,yr,θr,kr
一般reference_line都是离散点,只涉及离散点的
x
r
,
y
r
,
θ
r
,
k
r
x_r,y_r,\theta_r,k_r
xr,yr,θr,kr
2.2 如何计算S
有其他更精确的算法,但是不稳定。以分段直线近似曲线虽然比较糙,但是对参数变化不敏感(robust)
2.3 如何从 S , S ˙ , S ¨ , l , l ˙ , l ¨ , l ′ , l ′ ′ S,\dot S,\ddot S,l,\dot l,\ddot l,l_{}',l_{}'' S,S˙,S¨,l,l˙,l¨,l′,l′′到 r ⃗ h , v ⃗ h , a ⃗ h , k h \vec r_h,\vec v_h,\vec a_h,k_h rh,vh,ah,kh