【自动驾驶】决策规划算法 | 数学基础(三)直角坐标与自然坐标转换Ⅰ

发布于:2024-09-18 ⋅ 阅读:(259) ⋅ 点赞:(0)

写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_CSDN博客,期待与您一同探索 移动机器人 领域的无限可能。

🔍 本文系 清流君 原创之作,荣幸在CSDN首发🐒
若您觉得内容有价值,还请评论告知一声,以便更多人受益。
转载请注明出处,尊重原创,从我做起。

👍 点赞、评论、收藏,三连走一波,让我们一起养成好习惯😜
在这里,您将收获的不只是技术干货,还有思维的火花!

📚 系列专栏:【决策规划】系列,带您深入浅出,领略控制之美。🖊
愿我的分享能为您带来启迪,如有不足,敬请指正,让我们共同学习,交流进步!

🎭 人生如戏,我们并非能选择舞台和剧本,但我们可以选择如何演绎 🌟
感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行~~~


引言

  各位小伙伴们大家好,本篇博客是自动驾驶决策规划算法数学基础的第三节第Ⅰ部分,内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。

  本篇博客讲数学基础部分中 Frenet 坐标系和笛卡坐标系之间的坐标转换,即直角坐标和自然坐标的转换。

  本节内容如果只应用,难度其实还好,但如果想真彻底理解它是怎么来,难度非常高。需要非常熟悉微积分以及向量微积分。


一、龙格现象与多项式拟合

1.1 龙格现象概述

  讲解坐标转换之前,首先讲一下龙格现象,是数值分析里的知识,用高次多项式拟合点,可能出现震荡现象,所以要慎用高次多项式。

在这里插入图片描述
  对于较多点的拟合,尽可能用分段低次多项式拟合,而不用高次多项式拟合。

1.2 高次多项式拟合的弊端

  一般认为如果多项式次数越高,拟合精度越好,这是想当然的想法,在有些问题确实是这样,比如拟合 sin ⁡ x , cos ⁡ x \sin x,\cos x sinx,cosx 这样的三角函数,次数越高拟合越精确。

  但在有些情况并不是这样,比如在拟合 y = 25 x 2 + 1 e y=25x^2+\frac{1}{e} y=25x2+e1​ 时,用高次多项式拟合会出现震荡。

  在拟合时尽可能不用高次多项式,高次多项式很可能出现震荡现象,所以一般对大量点进行拟合时,用分段一次多样式拟合,而不用高次多项式。


二、Frenet 坐标系的作用

2.1 自然坐标系的优势

为什么要把笛卡尔坐标转化为自然坐标?

  因为实际上道路它都是千变万化的,有不同的曲率和长短。如果只在直角坐标下分析问题,就意味着不同的道路形状、不同道路的长短都要分开考虑,非常麻烦。

  如果以道路中心线为曲线坐标系建立自然坐标,只需要关注怎么进行坐标转换可以,只要把直角坐标转换为自然坐标,无论是什么道路、什么路况,都可以用同一套方法解决问题,也就是只要做把直角坐标转化为自然坐标,解决问题后,再把自然坐标转化为直角坐标。极大简化分析问题的难度。不同道路都可以转化成同样坐标系分析,再加坐标转换就可以了。

  关于转换公式,可以参考以下博客:
  Frenet坐标系与Cartesian坐标系互转(一):公式推导

  上面的博客给出了笛卡尔坐标转Frenet坐标和Frenet坐标转笛卡尔坐标,直接会用公式就可以了。

  下面使用向量法推导,可降低推导难度,不用上面博客的推导方法也能得出一样的结果。

2.2 坐标转换涉及的变量及其关系

  首先建立直角坐标系:
在这里插入图片描述

  第一条曲线是车辆轨迹,第二条曲线是道路几何,在车辆轨迹曲线取一点,有速度 v v v,加速度 a a a,位置矢量 r ⃗ \vec r r ,以及在直角坐标系下的曲率 κ \kappa κ。

  无人驾驶车一般用 Host Vehicle 表示,已知车在笛卡尔标系下的 r ⃗ h , v ⃗ h , a ⃗ h , κ h \vec r_h,\vec v_h,\vec a_h,\kappa_h r h​,v h​,a h​,κh​,求车在以道路为坐标轴的Frenet坐标系下的坐标 s , s ˙ , s ¨ , l , l ′ , l ′ ′ , l ˙ , l ¨ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime},\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},\dot{l}=\frac{dl}{ds} s˙=dtds​,l˙=dtdl​,l˙=dsdl​。

共有 8 8 8 个坐标要求,但一般实际上用不到这 8 8 8 个变量。一般只用 6 6 6 个:

  • s , s ˙ , s ¨ , l , l ˙ , l ¨ s,\dot{s},\ddot{s},l,\dot l,\ddot l s,s˙,s¨,l,l˙,l¨
  • s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′

到底用哪 6 6 6 个变量,取决于规划方法是什么:

  • 对于 EM Planner ,求 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′
  • 对于 Lattice Planner,求 s , s ˙ , s ¨ , l , l ˙ , l ¨ s,\dot{s},\ddot{s},l,\dot l,\ddot l s,s˙,s¨,l,l˙,l¨

  虽然变量有 8 8 8 个,但独立变量只有 6 6 6 个,因为 l ′ , l ′ ′ l^{\prime},l^{\prime\prime} l′,l′′ 和 l ˙ , l ¨ \dot l,\ddot l l˙,l¨ 可以互相转化:
l ˙ = d l d t = d l d s ⋅ d s d t = l ′ s ˙ \dot{l}=\frac{dl}{dt}=\frac{dl}{ds}\cdot \frac{ds}{dt}=l'\dot{s} l˙=dtdl​=dsdl​⋅dtds​=l′s˙ l ¨ = d l ˙ d t = d ( l ′ s ˙ ) d t = d l ′ d t s ˙ + l ′ ⋅ d s ˙ d t = d l ′ d s d s d t s ˙ + l ′ d s ˙ d t = l ′ ′ s ˙ 2 + l ′ s ¨ \begin{aligned} \ddot{l}&=\frac{\text{d}\dot{l}}{\text{d}t}=\frac{\text{d}\left( l'\dot{s} \right)}{\text{d}t}=\frac{\text{d}l'}{\text{d}t}\dot{s}+l'\cdot \frac{\text{d}\dot{s}}{\text{d}t}\\ &=\frac{\text{d}l'}{\text{d}s}\frac{\text{d}s}{\text{d}t}\dot{s}+l'\frac{\text{d}\dot{s}}{\text{d}t}=l''\dot{s}^2+l'\ddot{s}\\ \end{aligned} l¨​=dtdl˙​=dtd(l′s˙)​=dtdl′​s˙+l′⋅dtds˙​=dsdl′​dtds​s˙+l′dtds˙​=l′′s˙2+l′s¨​   EM planner 采用的是 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′。

三、曲线坐标系的特点

3.1 曲线坐标系与直角坐标系的主要区别

曲线坐标系与直角坐标系有两点不同:

  • 曲线坐标系的基向量一般不是常向量
  • 点的曲线坐标变化与点的实际位移一般不一致

在直角坐标下:
在这里插入图片描述

  基向量是 i i i 和 j j j,显然是常向量 d i d x = 0 ⃗ \frac{di}{dx}=\vec 0 dxdi​=0

  但在曲线坐标系:
在这里插入图片描述

  基向量为 τ ⃗ \vec \tau τ , τ ⃗ \vec \tau τ 的大小它不变,但 τ ⃗ \vec \tau τ 的方向会随 s s s 变化而变化,所以 d τ d s ≠ 0 \frac{d\tau}{ds}\ne 0 dsdτ​=0

3.2 基向量非常数对向量求导的影响

基向量对坐标的导数不为 0 0 0 有什么影响?

  最大的影响就在于对向量求导,比如在直角坐标下,如果向量 v = v x i + v y j v=v_xi+v_yj v=vx​i+vy​j 对向量 v v v 求导,只要把坐标去掉就可以,变成
v ˙ = v ˙ x i + v ˙ y j \dot{v}=\dot{v}_xi+\dot{v}_yj v˙=v˙x​i+v˙y​j  但如果 在曲线坐标系下, v ˙ = v ˙ x τ ⃗ + v ˙ y n ⃗ \dot{v}=\dot{v}_x\vec\tau+\dot{v}_y\vec n v˙=v˙x​τ +v˙y​n ,对向导求导不仅要对 v x , v y v_x,v_y vx​,vy​ 求导,还要对 τ ⃗ , n ⃗ \vec \tau,\vec n τ ,n 求导,因为 τ ⃗ , n ⃗ \vec \tau,\vec n τ ,n 不是常向量,即向量导数不为 0 0 0,这是和直角坐标系相比最大的区别。

  在曲线坐标系下,对向量求导一般比较复杂,不像在直角坐标下,只要对 v x , v y v_x,v_y vx​,vy​ 求导就可以了。

3.3 直角坐标系与自然坐标系中位移描述的差异

  另外,曲线坐标系描述起来比较复杂,举个例子,比如直角坐标系:
在这里插入图片描述

  直角坐标系下有点 ( x , y ) (x,y) (x,y),让 y y y 不动, x x x 移动 Δ x \Delta x Δx。在 y y y 不动的情况下,点的实际位移和坐标的位移一样,点移动了 Δ x \Delta x Δx 的距离,坐标也从 x x x 移到了 x + Δ x x+\Delta x x+Δx 也是 Δ x \Delta x Δx 的距离,在直角坐标系下,如果 y y y 不动, x x x 坐标的移动距离和点实际移动的距离一样,都是 Δ x \Delta x Δx。

  但在自然坐标系:
在这里插入图片描述

  同样点 ( s , l ) (s,l) (s,l),让 l l l 不动, s s s 坐标移动 Δ s \Delta s Δs,点也会移动弧长 Δ s ′ \Delta s' Δs′。 Δ s \Delta s Δs 与 Δ s ′ \Delta s' Δs′ 一般不相等。

  这一结论最反直觉,也是对初学者来说是最别扭的结论。自然坐标系和直角坐标系项最大的不同就是在直角坐标系下只有 d y d x \frac{dy}{dx} dxdy​,而在曲线坐标系下有 d d s \frac{d}{ds} dsd​,还有 d d s x \frac{d}{ds_x} dsx​d​。

  在直角坐标系下,如果对函数求导,只有 d x d x dx, d x d x dx 不区分 d x d x dx 到底是实际点的位移,还是在 x x x 坐标方向上分量的位移。

但在自然坐标系下,因为 Δ s \Delta s Δs 与 Δ s ′ \Delta s' Δs′ 一般不相等,所以它们的微分 d s ds ds 和 d s ′ ds' ds′ 一般也不一样,所以在自然坐标系下,如果想求变量在 d s ds ds 方向上的导数时,必须要指出弧微分 d s ds ds 到底是哪个曲线的弧长。

  总结:在直角坐标系下只有 d x dx dx,在自然坐标系下可能有多个 d s ds ds。

3.4 实例分析:车辆轨迹与道路几何中的坐标位移差异

  举个例子说明:
在这里插入图片描述

  比如短弧线是车辆轨迹,长弧线是道路几何,假设车沿道路平行移动,即车的 l l l 不动。

  车原来在红色点的位置,经过 d y dy dy 的时间,车跑到蓝色点的位置,同样它在坐标轴上的投影也跑到蓝色点处,就有两个 d s ds ds:

  • 车速 d s x d t = ∣ v ⃗ ∣ \frac{ds_x}{dt}=|\vec v| dtdsx​​=∣v ∣。
  • 投影速度 d s d t = s ˙ \frac{ds}{dt}=\dot{s} dtds​=s˙。

  上面算出来的是 v v v 的大小,不带方向,一般情况下 ∣ v ⃗ ∣ |\vec v| ∣v ∣ 不等于 s ˙ \dot s s˙,因为 d s ds ds 和 d s x ds_x dsx​ 不一样,所以它们的速度不一样。


四、预备知识1:质点速度的向量表达式与道路几何投影导数推导

4.1 质点速度的向量表达式

  比如在直角坐标系下质点的轨迹是这样:
在这里插入图片描述

  证明 r ˙ ⃗ = ∣ v ⃗ ∣ τ ⃗ \vec{\dot{r}}=|\vec{v}|\vec{\tau} r˙ =∣v ∣τ , τ ⃗ \vec{\tau} τ 是质点在切线方向上的单位向量,记 ∣ v ⃗ ∣ τ ⃗ |\vec{v}|\vec{\tau} ∣v ∣τ 为 v ⃗ \vec v v 。

  假设经过 d t dt dt 时间, r ⃗ \vec r r 位置变成了 r + d r r+dr r+dr ,它划过的弧长为 d s ds ds。根据向量加减法,紫红色向量为 d r dr dr,位矢对时间的导数 r ˙ ⃗ = d r ⃗ d t = d r ⃗ d s ⋅ d s d t \vec{\dot r}=\frac{d\vec{r}}{dt}=\frac{d\vec{r}}{ds}\cdot\frac{ds}{dt} r˙ =dtdr ​=dsdr ​⋅dtds​  当 d t → 0 dt\rightarrow 0 dt→0 时, ∣ d r ⃗ ∣ d s → 1 \frac{|d\vec{r}|}{ds}\rightarrow 1 ds∣dr ∣​→1,方向趋于轨迹在 r r r 的切线方向 τ ⃗ \vec \tau τ ,所以:
r ˙ ⃗ = 1 ⋅ τ ⃗ ⋅ d s d t = ∣ v ⃗ ∣ τ ⃗ = v ⃗ \vec{\dot{r}}=1\cdot \vec{\tau}\cdot \frac{ds}{dt}=|\vec{v}|\vec{\tau}=\vec{v} r˙ =1⋅τ ⋅dtds​=∣v ∣τ =v

4.2 拓展:质点在道路几何上的投影位矢导数推导

  比如在直角坐标系下有两条曲线:质点轨迹和道路几何
在这里插入图片描述

  比如在质点轨迹曲线上有质点,位置为 r ⃗ h \vec r_h r h​,质点在道路几何上的投影位矢记为 r ⃗ r \vec r_r r r​,根据上面的推导 r ˙ ⃗ h = ∣ v ⃗ ∣ τ ⃗ = v ⃗ \vec{\dot{r}}_h=|\vec{v}|\vec{\tau}=\vec{v} r˙ h​=∣v ∣τ =v , τ ⃗ h \vec \tau_h τ h​ 是质点在轨迹上的切线方向, τ ⃗ r \vec \tau_r τ r​ 是投影在道路几何上的切线方向,投影位矢的导数为:

r ˙ ⃗ r = d r ⃗ r d t = d r ⃗ r d s r ⋅ d s r d t = s ˙ τ ⃗ r \vec{\dot{r}}_r=\frac{\text{d}\vec{r}_r}{\text{d}t}=\frac{\text{d}\vec{r}_r}{\text{d}s_r}\cdot \frac{\text{d}s_r}{\text{d}t}=\dot{s}\vec{\tau}_r r˙ r​=dtdr r​​=dsr​dr r​​⋅dtdsr​​=s˙τ r​  如果道路是 Frenet 坐标系的坐标轴,则 d s r d t = s ˙ \frac{\text{d}s_r}{\text{d}t}=\dot{s} dtdsr​​=s˙。在自然坐标系下有不同的 d s ds ds,但在直角坐标系下只有 d x dx dx。

4.3 质点投影点的定义

  如果点的切线方向和点与质点的连线方向垂直:
在这里插入图片描述

  称为质点的投影点


五、预备知识2:Frenet公式

5.1 曲线坐标系基向量特性

  在曲线坐标系下, d τ ⃗ d s \frac{d\vec \tau}{ds} dsdτ ​ 一般不为零向量。

问题来了, d τ ⃗ d s \frac{d\vec \tau}{ds} dsdτ ​ 到底等于什么呢?包括它的法向量 d n ⃗ d s \frac{d\vec n}{ds} dsdn ​等于什么呢?

5.2 Frenet公式及其证明

  Frenet公式给出了结果:
d t ⃗ d s = κ n ⃗ d n ⃗ d s = − κ τ ⃗ \frac{d\vec{t}}{ds}=\kappa \vec{n}\quad \frac{d\vec{n}}{ds}=-\kappa \vec{\tau} dsdt ​=κn dsdn ​=−κτ   其中, d s ds ds 为所画曲线的弧微分。

  证明:假设 τ \tau τ 经过 d s ds ds 变成了 τ + d τ \tau +d\tau τ+dτ,因为 τ \tau τ 和 τ + d τ \tau +d\tau τ+dτ 是单位向量,所以模都是 1 1 1。
在这里插入图片描述
  几何关系如下:
在这里插入图片描述
  夹角为 d θ d\theta dθ,三角形是等腰三角形,根据几何关系, d τ ⃗ d\vec{\tau} dτ 的长度为:
∣ d τ ⃗ ∣ = 2 ⋅ 1 ⋅ sin ⁡ ( d θ 2 ) |d\vec{\tau}|=2\cdot 1\cdot \sin \left( \frac{d\theta}{2} \right) ∣dτ ∣=2⋅1⋅sin(2dθ​)  当 d s ds ds 趋于 0 0 0 时, d τ ⃗ d\vec \tau dτ 的方向趋于 τ ⃗ \vec \tau τ 的垂直方向 n ⃗ \vec n n ,大小为
∣ d τ ⇀ ∣ d s = 2 sin ⁡ d θ 2 d s = d θ d s = κ \frac{|d\overrightharpoon{\tau }|}{ds}=\frac{2\sin \frac{d\theta}{2}}{ds}=\frac{d\theta}{ds}=\kappa ds∣dτ ∣​=ds2sin2dθ​​=dsdθ​=κ  其中, d θ d s \frac{d\theta}{ds} dsdθ​ 就是曲率的定义,所以
d τ ⃗ d s = κ n ⃗ \frac{d\vec{\tau}}{ds}=\kappa \vec{n} dsdτ ​=κn   同理
d n ⃗ d s = − κ τ ⃗ \frac{d\vec{n}}{ds}=-\kappa \vec{\tau} dsdn ​=−κτ   证明方式和上面一样。

5.3 拓展1:质点轨迹与道路几何的方向导数

在这里插入图片描述
  假设上面弧曲线是质点轨迹,下面是道路几何,有质点的位矢以及质点的投影位矢。记质点的切线方向是 τ ⃗ h \vec \tau_h τ h​,它的投影切线方向是 τ ⃗ r \vec \tau_r τ r​, τ ⃗ h \vec \tau_h τ h​ 对时间的导数:
τ ˙ ⃗ h = d τ ⃗ h d t = d τ ⃗ h d s h ⋅ d s h d t = κ h n ⃗ h ∣ v ⃗ ∣ \vec{\dot{\tau}}_h=\frac{d\vec{\tau}_h}{dt}=\frac{d\vec{\tau}_h}{ds_h}\cdot \frac{ds_h}{dt}=\kappa_h \vec{n}_h|\vec{v}| τ˙ h​=dtdτ h​​=dsh​dτ h​​⋅dtdsh​​=κh​n h​∣v ∣   其中, κ h \kappa_h κh​ 为质点所在轨迹的曲率。

  同理,质点法向量的导数为:
n ˙ ⃗ h = − κ τ ⃗ h ⋅ ∣ v ⃗ ∣ = − κ h ∣ v ⃗ ∣ τ ⃗ h \vec{\dot{n}}_h=-\kappa \vec{\tau}_h\cdot |\vec{v}|=-\kappa_h |\vec{v}|\vec{\tau}_h n˙ h​=−κτ h​⋅∣v ∣=−κh​∣v ∣τ h​  同理,投影点的切向量和法向量对时间的导数分别为:
τ ˙ ⃗ r = κ s ˙ n ⃗ r n ˙ ⃗ r = − κ s ˙ τ ⃗ r \begin{aligned} \vec{\dot{\tau}}_r&=\kappa \dot{s}\vec{n}_r\\ \vec{\dot{n}}_r&=-\kappa \dot{s}\vec{\tau}_r\\ \end{aligned} τ˙ r​n˙ r​​=κs˙n r​=−κs˙τ r​​  这四个公式是坐标转换的核心公式。

  坐标转换有 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 坐标系下坐标轴曲线的弧微分。

  注意:上述四个核心公式中的曲率,无论是 κ h \kappa_h κh​ 还是 κ r \kappa_r κr​,都是指在直角坐标系下的曲率。因为自然坐标系下也有曲线及其曲率,自然坐标系下的曲率和直角坐标系下的曲率不一样。

5.4 拓展2:切向加速度与法向加速度的分解

  同样在直角坐标系下有轨迹和质点:
在这里插入图片描述

  已知 r ⃗ , τ ⃗ , n ⃗ , κ \vec{r},\vec{\tau},\vec{n},\kappa r ,τ ,n ,κ,求 v ⃗ \vec v v :
v ⃗ = r ˙ ⃗ = ∣ v ⃗ ∣ τ ⃗ \vec{v}=\vec{\dot{r}}=|\vec{v}|\vec{\tau} v =r˙ =∣v ∣τ   求 a ⃗ \vec a a ,利用预备知识的拓展1: τ ˙ ⃗ = κ ∣ v ⃗ ∣ n ⃗ \vec{\dot{\tau}}=\kappa |\vec{v}|\vec{n} τ˙ =κ∣v ∣n

a ⃗ = d v ⃗ d t = d ∣ v ⃗ ∣ τ ⃗ d t = d ∣ v ⃗ ∣ d t τ ⃗ + ∣ v ⃗ ∣ d τ ⃗ d t = ∣ v ˙ ⃗ ∣ τ ⃗ + ∣ v ⃗ ∣ τ ˙ ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ + ∣ v ⃗ ∣ 2 κ n ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ + ∣ v ⃗ ∣ 2 ρ n ⃗ \begin{aligned} \vec{a}&=\frac{d\vec{v}}{dt}=\frac{d|\vec{v}|\vec{\tau}}{dt}=\frac{d|\vec{v}|}{dt}\vec{\tau}+|\vec{v}|\frac{d\vec{\tau}}{dt}\\ &=|\vec{\dot{v}}|\vec{\tau}+|\vec{v}|\vec{\dot{\tau}}\\ &=|\vec{\dot{v}}|\vec{\tau}+|\vec{v}|^2\kappa \vec{n}\\ &=|\vec{\dot{v}}|\vec{\tau}+\frac{|\vec{v}|^2}{\rho}\vec{n}\\ \end{aligned} a ​=dtdv ​=dtd∣v ∣τ ​=dtd∣v ∣​τ +∣v ∣dtdτ ​=∣v˙ ∣τ +∣v ∣τ˙ =∣v˙ ∣τ +∣v ∣2κn =∣v˙ ∣τ +ρ∣v ∣2​n ​  其中,曲率半径 ρ = 1 κ \rho =\frac{1}{\kappa} ρ=κ1​,在本例中为负值, ∣ v ˙ ⃗ ∣ τ ⃗ |\vec{\dot{v}}|\vec \tau ∣v˙ ∣τ 为切向加速度, ∣ v ⃗ ∣ 2 ρ n ⃗ \frac{|\vec{v}|^2}{\rho}\vec{n} ρ∣v ∣2​n 为法向加速度,即向心加速度。


六、预备知识总结

  这些公式是以后推导坐标转换时非常有用的辅助公式。
在这里插入图片描述

6.1 变量含义

  以下变量都以笛卡尔坐标为基准

变量 含义 变量 含义
r ⃗ h \vec r_h r h​ 车辆位矢 r ⃗ r \vec r_r r r​ 投影位矢
v ⃗ \vec v v 车辆速度 s ˙ \dot s s˙ 投影速率
a ⃗ \vec a a 车辆加速度 κ r \kappa_r κr​ 投影位矢在道路几何上的曲率
κ h \kappa_h κh​ 车辆位矢在车辆轨迹上的曲率 τ ⃗ r \vec \tau_r τ r​ 投影位矢在道路几何上的切线方向单位向量
τ ⃗ h \vec \tau_h τ h​ 车辆位矢在车辆轨迹上的切线方向单位向量 n ⃗ r \vec n_r n r​ 投影位矢在道路几何上的法线方向单位向量
n ⃗ h \vec n_h n h​ 车辆位矢在车辆轨迹上的法线方向单位向量

6.2 辅助公式

   7 7 7 个辅助公式是求自然坐标和直角坐标之间转化的关键:

r ˙ ⃗ h = ∣ v ⃗ ∣ τ ⃗ h r ˙ ⃗ r = s ˙ τ ⃗ r τ ˙ ⃗ h = κ h ∣ v ⃗ ∣ n ⃗ h n ˙ ⃗ h = − κ h ∣ v ⃗ ∣ τ ⃗ h τ ˙ ⃗ r = κ r s ˙ n ⃗ r n ˙ ⃗ r = − κ r s ˙ τ ⃗ r a ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ h + ∣ v ⃗ ∣ 2 κ h n ⃗ h \begin{align} \vec{\dot{r}}_h&=|\vec{v}|\vec{\tau}_h\\ \vec{\dot{r}}_r&=\dot{s}\vec{\tau}_r\\ \vec{\dot{\tau}}_h&=\kappa _h|\vec{v}|\vec{n}_h\\ \vec{\dot{n}}_h&=-\kappa _h|\vec{v}|\vec{\tau}_h\\ \vec{\dot{\tau}}_r&=\kappa _r\dot{s}\vec{n}_r\\ \vec{\dot{n}}_r&=-\kappa _r\dot{s}\vec{\tau}_r\\ \vec{a}&=|\vec{\dot{v}}|\vec{\tau}_h+|\vec{v}|^2\kappa _h\vec{n}_h\\ \end{align} r˙ h​r˙ r​τ˙ h​n˙ h​τ˙ r​n˙ r​a ​=∣v ∣τ h​=s˙τ r​=κh​∣v ∣n h​=−κh​∣v ∣τ h​=κr​s˙n r​=−κr​s˙τ r​=∣v˙ ∣τ h​+∣v ∣2κh​n h​​​  有了这 7 7 7 个辅助公式,后面一切推导都水到渠成,就是微积分的计算。


七、坐标转换算法

7.1 问题描述

  问题非常明晰了:
在这里插入图片描述

  已知笛卡尔坐标下的 r ⃗ h , v ⃗ h , a ⃗ h , κ h , τ ⃗ h , n ⃗ h \vec{r}_h,\vec{v}_h,\vec{a}_h,\kappa _h,\vec{\tau}_h,\vec{n}_h r h​,v h​,a h​,κh​,τ h​,n h​

  已知 Frenet 坐标系下的起点 ( x 0 , y 0 ) (x_0,y_0) (x0​,y0​)

  求 Frenet 坐标系下的 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′。其中, s ˙ = d s d t \dot s=\frac{ds}{dt} s˙=dtds​, l ′ = d l d s l'=\frac{dl}{ds} l′=dsdl​, d s ds ds 为 Frenet 坐标轴的弧长。

7.2 坐标转换算法步骤概述

  算法分三步:

  第一步: 7 7 7 个辅助公式

  第二步:找到车在Frenet坐标系下的投影点在笛卡尔坐标系下的坐标,记为 x r , y r , θ r , k r {x_r,y_r,\theta_r,k_r} xr​,yr​,θr​,kr​,

  计算
r ⃗ r = ( x r , y r ) τ ⃗ r = ( cos ⁡ θ r , sin ⁡ θ r ) n ⃗ r = ( − sin ⁡ θ r , cos ⁡ θ r ) \vec{r}_r=\left( x_r,y_r \right) \quad \vec{\tau}_r=\left( \cos \theta _r,\sin \theta _r \right) \quad \vec{n}_r=\left( -\sin \theta _r,\cos \theta _r \right) r r​=(xr​,yr​)τ r​=(cosθr​,sinθr​)n r​=(−sinθr​,cosθr​)  其中, θ r \theta_r θr​ 代表投影点的切线方向 τ ⃗ r \vec \tau_r τ r​ 与 x x x 轴的夹角,如下图所示:
在这里插入图片描述

  第三步:利用向量三角形以及微积分求出 s , s ˙ , s ¨ , l , l ′ , l ′ ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime} s,s˙,s¨,l,l′,l′′

7.3 核心公式及其应用

  由向量三角形关系得到坐标转换的核心公式:
r r ⃗ + l n ⃗ r = r ⃗ h \vec{r_r}+l\vec{n}_r=\vec{r}_h rr​ ​+ln r​=r h​  但公式目前还不能直接用,因为 r r ⃗ \vec{r_r} rr​ ​ 和 n ⃗ r \vec{n}_r n r​ 都不知道,知道的只有 r ⃗ h , v ⃗ h , a ⃗ h , κ h , τ ⃗ h , n ⃗ h \vec{r}_h,\vec{v}_h,\vec{a}_h,\kappa _h,\vec{\tau}_h,\vec{n}_h r h​,v h​,a h​,κh​,τ h​,n h​。

  首先要找到投影,即 ( x h , y h ) (x_h,y_h) (xh​,yh​)在 Frenet 坐标系下的投影 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​),但在这里先不讲,假设已经找到了 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr​,yr​,θr​,κr​)。找投影的过程下一节再讲,因为找投影比较绕。

  如果找到投影点的信息,自然可以得到 r ⃗ r , τ ⃗ r , n ⃗ r , κ r \vec{r}_r,\vec{\tau}_r,\vec{n}_r,\kappa _r r r​,τ r​,n r​,κr​。

  有了这些自然就可以用核心公式 r r ⃗ + l n ⃗ r = r ⃗ h \vec{r_r}+l\vec{n}_r=\vec{r}_h rr​ ​+ln r​=r h​。

(1) 计算弧长

  第一步:计算 l l l,根据核心公式 l n ⃗ r = r ⃗ h − r r ⃗ l\vec{n}_r=\vec{r}_h-\vec{r_r} ln r​=r h​−rr​ ​,两边点乘 n ⃗ r \vec{n}_r n r​:
l = ( r ⃗ h − r ⃗ r ) ⋅ n ⃗ r l=\left( \vec{r}_h-\vec{r}_r \right) \cdot \vec{n}_r l=(r h​−r r​)⋅n r​

(2) 计算弧速度

  第二步:计算 s ˙ \dot s s˙,核心公式 r r ⃗ + l n ⃗ r = r ⃗ h \vec{r_r}+l\vec{n}_r=\vec{r}_h rr​ ​+ln r​=r h​ 两边对时间求导:
r ˙ ⃗ r + l n ˙ ⃗ r + l ˙ n ⃗ r = r ˙ ⃗ h \vec{\dot{r}}_r+l\vec{\dot{n}}_r+\dot{l}\vec{n}_r=\vec{\dot{r}}_h r˙ r​+ln˙ r​+l˙n r​=r˙ h​  利用辅助公式 ( 1 ) ( 2 ) ( 5 ) ( 6 ) (1)(2)(5)(6) (1)(2)(5)(6),代入得到:
s ˙ τ ⃗ r + l ( − κ r s ˙ τ ⃗ r ) + l ˙ n ⃗ r = v ⃗ h \dot{s}\vec{\tau}_r+l\left( -\kappa _r\dot{s}\vec{\tau}_r \right) +\dot{l}\vec{n}_r=\vec{v}_h s˙τ r​+l(−κr​s˙τ r​)+l˙n r​=v h​  两边同时点乘 τ ⃗ r \vec{\tau}_r τ r​,得到
s ˙ + l ( − κ r s ˙ ) = v ⃗ h ⋅ τ ⃗ r \dot{s}+l\left( -\kappa _r\dot{s} \right) =\vec{v}_h\cdot \vec{\tau}_r s˙+l(−κr​s˙)=v h​⋅τ r​  这样得到
s ˙ = v ⃗ h ⋅ τ ⃗ r 1 − k r l \dot{s}=\frac{\vec{v}_h\cdot \vec{\tau}_r}{1-k_rl} s˙=1−kr​lv h​⋅τ r​​  又因为
v ⃗ h ⋅ τ ⃗ r = ∣ v ⃗ h ∣ τ ⃗ h ⋅ τ ⃗ r = ∣ v ⃗ h ∣ ∣ τ ⃗ h ∣ ∣ τ ⃗ r ∣ cos ⁡ < τ ⃗ h , τ ⃗ r > = ∣ v ⃗ h ∣ cos ⁡ ( θ h − θ r ) \vec{v}_h\cdot \vec{\tau}_r=|\vec{v}_h|\vec{\tau}_h\cdot \vec{\tau}_r=|\vec{v}_h||\vec{\tau}_h||\vec{\tau}_r|\cos <\vec{\tau}_h,\vec{\tau}_r>=|\vec{v}_h|\cos \left( \theta _h-\theta _r \right) v h​⋅τ r​=∣v h​∣τ h​⋅τ r​=∣v h​∣∣τ h​∣∣τ r​∣cos<τ h​,τ r​>=∣v h​∣cos(θh​−θr​)  得到另一种形式:
s ˙ = ∣ v ⃗ h ∣ cos ⁡ ( θ h − θ r ) 1 − κ r l \dot{s}=\frac{|\vec{v}_h|\cos \left( \theta _h-\theta _r \right)}{1-\kappa _rl} s˙=1−κr​l∣v h​∣cos(θh​−θr​)​  推荐使用上面的向量法表示,更简洁。

(3) 计算弧长的时间导数

  第三步:计算 l ˙ \dot l l˙,由第二步推导的 s ˙ τ ⃗ r + l ( − κ r s ˙ τ ⃗ r ) + l ˙ n ⃗ r = v ⃗ h \dot{s}\vec{\tau}_r+l\left( -\kappa _r\dot{s}\vec{\tau}_r \right) +\dot{l}\vec{n}_r=\vec{v}_h s˙τ r​+l(−κr​s˙τ r​)+l˙n r​=v h​,两边同时点乘 n ⃗ r \vec n_r n r​ 得:
l ˙ = v ⃗ h ⋅ n ⃗ r \dot l=\vec v_h \cdot \vec n_r l˙=v h​⋅n r​  若将向量形式展开,得到 l ˙ = ∣ v ⃗ h ∣ sin ⁡ ( θ h − θ r ) \dot l=|\vec{v}_h|\sin( \theta _h-\theta _r) l˙=∣v h​∣sin(θh​−θr​)。

(4) 计算弧长的弧坐标导数

  第四步:计算 l ′ l' l′:
l ′ = d l d s = d l d t d s d t = l ˙ s ˙ = v ⃗ ⋅ n ⃗ r v ⃗ ⋅ τ ⃗ r 1 − κ r l = ( 1 − κ r l ) v ⃗ ⋅ n ⃗ r v ⃗ ⋅ τ ⃗ r l'=\frac{dl}{ds}=\frac{\frac{dl}{dt}}{\frac{ds}{dt}}=\frac{\dot{l}}{\dot{s}}=\frac{\vec{v}\cdot \vec{n}_r}{\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl}}=\left( 1-\kappa _rl \right) \frac{\vec{v}\cdot \vec{n}_r}{\vec{v}\cdot \vec{\tau}_r} l′=dsdl​=dtds​dtdl​​=s˙l˙​=1−κr​lv ⋅τ r​​v ⋅n r​​=(1−κr​l)v ⋅τ r​v ⋅n r​​其中, d s ds ds 为 Frenet 坐标轴的弧坐标导数,因此 d s d t = s ˙ \frac{ds}{dt}=\dot s dtds​=s˙。

(5) 计算弧加速度

  第五步:计算 s ¨ \ddot s s¨,由第二步计算的 s ˙ = v ⃗ h ⋅ τ ⃗ r 1 − k r l \dot{s}=\frac{\vec{v}_h\cdot \vec{\tau}_r}{1-k_rl} s˙=1−kr​lv h​⋅τ r​​,利用复合求导,得到:

s ¨ = d s ˙ d t = 1 ( 1 − κ r l ) 2 ( d ( v ⃗ ⋅ τ ⃗ r ) d t ⋅ ( 1 − κ r l ) − ( v ⃗ ⋅ τ ⃗ r ) ⋅ ( − κ ˙ r l − κ r l ˙ ) ) = 1 1 − κ r l ( d v ⃗ d t ⋅ τ ⃗ r + v ⃗ ⋅ d τ ⃗ r d t ) + 1 1 − κ r l v ⃗ ⋅ τ ⃗ r 1 − κ r l ( κ ˙ r l + κ r l ˙ ) = 1 1 − κ r l ( a ⃗ ⋅ τ ⃗ r + v ⃗ ⋅ ( κ r ⋅ s ˙ ⋅ n ⃗ r ) + 1 1 − κ r l s ˙ ( d κ r d s d s d t l + κ r ⋅ d l d s ⋅ d s d t ) = a ⃗ ⋅ τ ⃗ r 1 − κ r l + ( κ r s ˙ ) ( v ⃗ ⋅ n ⃗ r ) 1 − κ r l + s ˙ 2 1 − κ r l ( κ r ′ l + κ r l ′ ) \begin{aligned} \ddot{s}&=\frac{\text{d}\dot{s}}{\text{d}t}=\frac{1}{\left( 1-\kappa _rl \right) ^2}\left( \frac{\text{d}\left( \vec{v}\cdot \vec{\tau}_r \right)}{\text{d}t}\cdot \left( 1-\kappa _rl \right) -\left( \vec{v}\cdot \vec{\tau}_r \right) \cdot \left( -\dot{\kappa}_rl-\kappa _r\dot{l} \right) \right)\\ &=\frac{1}{1-\kappa _rl}\left( \frac{d\vec{v}}{dt}\cdot \vec{\tau}_r+\vec{v}\cdot \frac{d\vec{\tau}_r}{dt} \right) +\frac{1}{1-\kappa _rl}\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl}\left( \dot{\kappa}_rl+\kappa _r\dot{l} \right)\\ &=\frac{1}{1-\kappa _rl}\text{(}\vec{a}\cdot \vec{\tau}_r+\vec{v}\cdot \left( \kappa _r\cdot \dot{s}\cdot \vec{n}_r \right) +\frac{1}{1-\kappa _rl}\dot{s}\left( \frac{d\kappa _r}{ds}\frac{ds}{dt}l+\kappa _r\cdot \frac{dl}{ds}\cdot \frac{ds}{dt} \right)\\ &=\frac{\vec{a}\cdot \vec{\tau}_r}{1-\kappa _rl}+\frac{\left( \kappa _r\dot{s} \right) \left( \vec{v}\cdot \vec{n}_r \right)}{1-\kappa _rl}+\frac{\dot{s}^2}{1-\kappa _rl}\left( \kappa _{r}'l+\kappa _rl' \right)\\ \end{aligned} s¨​=dtds˙​=(1−κr​l)21​(dtd(v ⋅τ r​)​⋅(1−κr​l)−(v ⋅τ r​)⋅(−κ˙r​l−κr​l˙))=1−κr​l1​(dtdv ​⋅τ r​+v ⋅dtdτ r​​)+1−κr​l1​1−κr​lv ⋅τ r​​(κ˙r​l+κr​l˙)=1−κr​l1​(a ⋅τ r​+v ⋅(κr​⋅s˙⋅n r​)+1−κr​l1​s˙(dsdκr​​dtds​l+κr​⋅dsdl​⋅dtds​)=1−κr​la ⋅τ r​​+1−κr​l(κr​s˙)(v ⋅n r​)​+1−κr​ls˙2​(κr′​l+κr​l′)​  又因为 v ⃗ ⋅ n ⃗ r = l ˙ , l ˙ = l ′ ⋅ s ˙ \vec{v}\cdot \vec{n}_r=\dot{l}, \dot{l}=l'\cdot \dot{s} v ⋅n r​=l˙,l˙=l′⋅s˙,代入最终得到
s ¨ = a ⃗ ⋅ τ ⃗ r 1 − κ r l + κ r s ˙ 2 l ′ 1 − κ r l + s ˙ 2 1 − κ r l ( κ r ′ l + κ r l ′ ) \ddot{s}=\frac{\vec{a}\cdot \vec{\tau}_r}{1-\kappa _rl}+\frac{\kappa _r\dot{s}^2l'}{1-\kappa _rl}+\frac{\dot{s}^2}{1-\kappa _rl}\left( \kappa _{r}'l+\kappa _rl' \right) s¨=1−κr​la ⋅τ r​​+1−κr​lκr​s˙2l′​+1−κr​ls˙2​(κr′​l+κr​l′)  其中, a ⃗ ⋅ τ ⃗ r \vec{a}\cdot \vec{\tau}_r a ⋅τ r​ 的计算如下,利用辅助公式 ( 7 ) (7) (7): a ⃗ = ∣ v ˙ ⃗ ∣ τ ⃗ h + ∣ v ⃗ ∣ 2 κ h n ⃗ h \vec{a}=|\vec{\dot{v}}|\vec{\tau}_h+|\vec{v}|^2\kappa _h\vec{n}_h a =∣v˙ ∣τ h​+∣v ∣2κh​n h​,可得
a ⃗ ⋅ τ ⃗ r = ∣ v ˙ ⃗ ∣ cos ⁡ < τ ⃗ h , τ ⃗ r > + ∣ v ⃗ ∣ 2 κ h n ⃗ h ⋅ τ ⃗ r = ∣ v ˙ ⃗ ∣ cos ⁡ ( θ h − θ r ) + κ h ⋅ ( 1 − κ r l ) 2 cos ⁡ 2 ( θ h − θ r ) s ˙ 2 ⋅ ( − sin ⁡ ( θ h − θ r ) ) = ∣ v ˙ ⃗ ∣ cos ⁡ ( θ h − θ r ) + κ h s ˙ 2 1 − κ r l cos ⁡ ( θ n − θ r ) ⋅ ( − l ′ ) \begin{aligned} \vec{a}\cdot \vec{\tau}_r&=|\vec{\dot{v}}|\cos <\vec{\tau}_h,\vec{\tau}_r>+|\vec{v}|^2\kappa _h\vec{n}_h\cdot \vec{\tau}_r\\ &=|\vec{\dot{v}}|\cos \left( \theta _h-\theta _r \right) +\kappa _h\cdot \frac{\left( 1-\kappa _rl \right) ^2}{\cos ^2\left( \theta _h-\theta _r \right)}\dot{s}^2\cdot \left( -\sin \left( \theta _h-\theta _r \right) \right)\\ &=|\vec{\dot{v}}|\cos \left( \theta _h-\theta _r \right) +\kappa_h\dot{s}^2\frac{1-\kappa _rl}{\cos \left( \theta _n-\theta _r \right)}\cdot \left( -l' \right)\\ \end{aligned} a ⋅τ r​​=∣v˙ ∣cos<τ h​,τ r​>+∣v ∣2κh​n h​⋅τ r​=∣v˙ ∣cos(θh​−θr​)+κh​⋅cos2(θh​−θr​)(1−κr​l)2​s˙2⋅(−sin(θh​−θr​))=∣v˙ ∣cos(θh​−θr​)+κh​s˙2cos(θn​−θr​)1−κr​l​⋅(−l′)​

(6) 计算弧长的二阶时间导数

  第六步:计算 l ¨ \ddot l l¨,由辅助公式 ( 3 ) (3) (3): l ˙ = v ⃗ ⋅ n ⃗ r \dot l=\vec v \cdot \vec n_r l˙=v ⋅n r​ 可得:
l ¨ = d v ⃗ d t ⋅ n r ⃗ + v ⃗ ⋅ d n ⃗ r d t \ddot{l}=\frac{d\vec{v}}{dt}\cdot \vec{n_r}+\vec{v}\cdot \frac{d\vec{n}_r}{dt} l¨=dtdv ​⋅nr​ ​+v ⋅dtdn r​​  由辅助公式 ( 6 ) (6) (6): n ⃗ r = − κ r s ˙ τ ⃗ r \vec{n}_r=-\kappa _r\dot{s}\vec{\tau}_r n r​=−κr​s˙τ r​ 可得:
l ¨ = a ⃗ ⋅ n ⃗ r + v ⃗ ⋅ ( − κ r s ˙ τ ⃗ r ) = a ⃗ ⋅ n ⃗ r − κ r s ˙ ( v ⃗ ⋅ τ ⃗ r ) = a ⃗ ⋅ n ⃗ r − κ r ( 1 − κ r l ) s ˙ 2 \begin{aligned} \ddot{l}&=\vec{a}\cdot \vec{n}_r+\vec{v}\cdot \left( -\kappa _r\dot{s}\vec{\tau}_r \right)\\ &=\vec{a}\cdot \vec{n}_r-\kappa _r\dot{s}\left( \vec{v}\cdot \vec{\tau}_r \right)\\ &=\vec{a}\cdot \vec{n}_r-\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2\\ \end{aligned} l¨​=a ⋅n r​+v ⋅(−κr​s˙τ r​)=a ⋅n r​−κr​s˙(v ⋅τ r​)=a ⋅n r​−κr​(1−κr​l)s˙2​

(7) 计算弧长的二阶弧坐标导数

  第七步:计算 l ′ ′ l'' l′′,用 l ¨ \ddot l l¨ 计算:
l ¨ = d l ˙ d t = d ( l ′ s ˙ ) d t = d l ′ d t s ˙ + l ′ ⋅ s ¨ = d l ′ d s ⋅ d s d t ⋅ s ˙ + l ′ s ¨ = l ′ ‘ s ˙ 2 + l ′ s ¨ \begin{aligned} \ddot{l}&=\frac{d\dot{l}}{dt}=\frac{d\left( l'\dot{s} \right)}{dt}=\frac{dl'}{dt}\dot{s}+l'\cdot \ddot{s}\\ &=\frac{dl'}{ds}\cdot \frac{ds}{dt}\cdot \dot{s}+l'\ddot{s}\\ &=l'‘\dot{s}^2+l'\ddot{s}\\ \end{aligned} l¨​=dtdl˙​=dtd(l′s˙)​=dtdl′​s˙+l′⋅s¨=dsdl′​⋅dtds​⋅s˙+l′s¨=l′‘s˙2+l′s¨​  所以:
l ′ ′ = l ¨ − l ′ s ¨ s ˙ 2 l''=\frac{\ddot{l}-l'\ddot{s}}{\dot{s}^2} l′′=s˙2l¨−l′s¨​  这样基本上所有直角坐标转自然坐标都已讲完。


八、总结

  共有 8 8 8 个坐标需要转化,即 s , s ˙ , s ¨ , l , l ′ , l ′ ′ , l ˙ , l ¨ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime},\dot l,\ddot l s,s˙,s¨,l,l′,l′′,l˙,l¨ ,总结公式如下:
l = ( r ⃗ h − r ⃗ r ) ⋅ n ⃗ r l ˙ = v ⃗ ⋅ n ⃗ r s ˙ = v ⃗ ⋅ τ ⃗ r 1 − κ r l l ′ = l ˙ s ˙ l ¨ = a ⃗ ⋅ n ⃗ r − κ r ( 1 − κ r l ) s ˙ 2 s ¨ = a ⃗ ⋅ τ ⃗ r 1 − κ r l + s ˙ 2 κ r l ′ 1 − κ r l + s ˙ 2 ( κ r ′ l + κ r l ′ ) 1 − κ r l l ′ ′ = l ¨ − l ′ s ¨ s ˙ 2 \begin{aligned} l&=\left( \vec{r}_h-\vec{r}_r \right) \cdot \vec{n}_r\\ \dot{l}&=\vec{v}\cdot \vec{n}_r\\ \dot{s}&=\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl}\\ l'&=\frac{\dot{l}}{\dot{s}}\\ \ddot{l}&=\vec{a}\cdot \vec{n}_r-\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2\\ \ddot{s}&=\frac{\vec{a}\cdot \vec{\tau}_r}{1-\kappa _rl}+\frac{\dot{s}^2\kappa _rl'}{1-\kappa _rl}+\frac{\dot{s}^2\left( \kappa _{r}'l+\kappa _rl' \right)}{1-\kappa _rl}\\ l''&=\frac{\ddot{l}-l'\ddot{s}}{\dot{s}^2}\\ \end{aligned} ll˙s˙l′l¨s¨l′′​=(r h​−r r​)⋅n r​=v ⋅n r​=1−κr​lv ⋅τ r​​=s˙l˙​=a ⋅n r​−κr​(1−κr​l)s˙2=1−κr​la ⋅τ r​​+1−κr​ls˙2κr​l′​+1−κr​ls˙2(κr′​l+κr​l′)​=s˙2l¨−l′s¨​​  现在算出了 7 7 7 个,还差 s s s 没算, s s s 和投影点相关。

  本篇博客讲解了从直角坐标转换到自然坐标,下一篇博客讲解 s s s 如何计算,以及投影点怎么找,如何从自然坐标转换到直角坐标。

  本篇博客到此结束,欢迎关注后续内容!


参考资料

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


后记:

🌟 感谢您耐心阅读这篇关于 直角坐标与自然坐标转换Ⅰ 的技术博客。 📚

🎯 如果您觉得这篇博客对您有所帮助,请不要吝啬您的点赞和评论 📢

🌟您的支持是我继续创作的动力。同时,别忘了收藏本篇博客,以便日后随时查阅。🚀

🚗 让我们一起期待更多的技术分享,共同探索移动机器人的无限可能!💡

🎭感谢您的支持与关注,让我们一起在知识的海洋中砥砺前行 🚀


微信公众号

今日签到

点亮在社区的每一天
去签到