b站老王 自动驾驶决策规划学习记录(四)

自动驾驶之直角坐标与自然坐标转换

上一讲:b站老王 自动驾驶决策规划学习记录(三)
接着上一讲学习记录b站老王对自动驾驶规划系列的讲解

参考视频:

  1. 自动驾驶决策规划算法第一章第三节(上) 直角坐标与自然坐标转换
  2. 自动驾驶决策规划算法第一章第三节(下) 直角坐标与自然坐标转换

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. 曲线坐标系的基向量一般不是常向量
    在这里插入图片描述

  2. 点的曲线坐标变化与点的实际位移一般不一致
    在这里插入图片描述

在这里插入图片描述

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 :投影位矢在道路几何上的法线方向单位向量

求自然坐标系和直角坐标系之间转化的关键
七个辅助公式:
在这里插入图片描述

问题转化为:
在这里插入图片描述
分三步:

  1. 7个辅助公式
  2. 找到车在 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
    在这里插入图片描述
  3. 利用向量三角形,以及微积分求出 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 r r+ln r=r h

如何找到 ( 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 r r,τ r,n r,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 r h,v h,a h,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 r h,v h,a h,kh

在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述
在这里插入图片描述

  • 3
    点赞
  • 10
    收藏
    觉得还不错? 一键收藏
  • 1
    评论
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值