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

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

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

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

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

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


文章目录

  • 引言
  • 一、坐标转换与投影计算
    • 1.1 自然坐标系与直角坐标系转换
    • 1.2 参考轨迹的离散化
    • 1.3 计算投影点近似解的几何原理
    • 1.4 自然坐标系坐标轴为直线时投影点的精确解
    • 1.5 自然坐标系坐标轴为弧线时的投影点计算
  • 二、自然坐标系下的弧长计算
    • 2.1 路径长度累加算法
    • 2.2 直线近似弧长的合理性及其误差分析
    • 2.3 算法的稳定性与鲁棒性
    • 2.4 精确算法举例及其局限性
    • 2.5 自动驾驶算法的选择与场景适应性
  • 三、自然坐标转换为直角坐标详细步骤
    • 3.1 计算映射表
    • 3.2 查找投影点
    • 3.3 计算投影点的坐标和参数
    • 3.4 计算车辆位矢
    • 3.5 计算车辆速度向量
    • 3.6 计算车辆加速度向量
    • 3.7 计算曲率
  • 四、总结
  • 参考资料


引言

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


一、坐标转换与投影计算

1.1 自然坐标系与直角坐标系转换

  在第一章上已经讲了从直角坐标系到自然坐标系之间的坐标转换,也就是上面所写的这七个公式以及下图:
在这里插入图片描述
  七个辅助转换公式如下:
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˙ll¨s¨l′′=(r hr r)n r=v n r=1κrlv τ r=s˙l˙=a n rκr(1κrl)s˙2=1κrla τ r+1κrls˙2κrl+1κrls˙2(κrl+κrl)=s˙2l¨ls¨本篇博客接着第三讲上篇遗留的几个问题:

  • 如何找到 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr)
  • 如何计算 s s s
  • 如何把自然坐标系下的坐标 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¨ 转回到直角坐标系 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

这就是本篇博客所解决的三个问题。

1.2 参考轨迹的离散化

  如何找到 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr),实际上以前已经讲过,在自动驾驶控制算法第七节:

  【自动驾驶】控制算法(七)离散规划轨迹的误差计算

  虽然以前讲过,但为了考虑系列整个知识的连贯性,所以在此再讲一遍。

  在自动驾驶中,自然坐标系下的参考轨迹一般都是以离散点的形式给出的,所以只讲离散点寻找投影点的方法。

  比如有一条参考轨迹曲线:
在这里插入图片描述

  曲线上有一系列离散点,假设已知 ( x i , y i , θ i , κ i ) (x_i,y_i,\theta_i,\kappa_i) (xi,yi,θi,κi),其中 κ i \kappa_i κi 是曲率, θ i \theta_i θi 是点的切线方向与 x x x 轴的夹角,这里的 κ i \kappa_i κi θ i \theta_i θi 都是指在直角坐标系下的 κ i \kappa_i κi θ i \theta_i θi

  如果有参考轨迹的详细表达式的话,自然可以得到理想投影,因为有蓝色线的表达式,不难找投影点。但在一般情况下没有蓝色线,只有这一堆离散点,以及它们的 ( x i , y i , θ i , κ i ) (x_i,y_i,\theta_i,\kappa_i) (xi,yi,θi,κi) ,所以求车在离散曲线下的投影,必然是近似解,不可能得到理想投影。因为离散点相比连续曲线,天生就缺少信息,连续曲线的信息比较多,在缺失信息下不可能完美解出投影点来,只能得到近似解,但是只要近似程度满足自动驾驶的要求就没问题。

如何计算投影的近似解呢?

  首先要遍历离散曲线,计算出离散点与车辆距离最短的点,称为匹配点。找到匹配点后,就可以计算两个向量,起点为匹配点,终点为车辆当前位置的向量 d ⃗ \vec d d ,匹配点的单位切向量 τ ⃗ \vec \tau τ

  因为已知匹配点的 ( x m , y m , θ m , κ m ) (x_m,y_m,\theta_m,\kappa_m) (xm,ym,θm,κm),设车的坐标是 ( x h , y h ) (x_h,y_h) (xh,yh)
d ⃗ = ( x h − x m , y h − y m ) τ ⃗ = ( cos ⁡ θ m , sin ⁡ θ m ) \begin{aligned} \vec{d}&=\left( x_h-x_m,y_h-y_m \right)\\ \vec{\tau}&=\left( \cos \theta _m,\sin \theta _m \right)\\ \end{aligned} d τ =(xhxm,yhym)=(cosθm,sinθm)  则投影点的近似位矢为
r ⃗ r = ( x r y r ) = r ⃗ m + ( d ⃗ ⋅ τ ⃗ ) τ ⃗ = ( x m y m ) + ( d ⃗ ⋅ τ ⃗ ) ( cos ⁡ θ m sin ⁡ θ m ) \begin{aligned} \vec{r}_r&=\left( \begin{array}{c} x_r\\ y_r\\ \end{array} \right) =\vec{r}_m+\left( \vec{d}\cdot \vec{\tau} \right) \vec{\tau}\\ &=\left( \begin{array}{c} x_m\\ y_m\\ \end{array} \right) +\left( \vec{d} \cdot \vec{\tau} \right) \left( \begin{array}{c} \cos \theta _m\\ \sin \theta _m\\ \end{array} \right)\\ \end{aligned} r r=(xryr)=r m+(d τ )τ =(xmym)+(d τ )(cosθmsinθm)

1.3 计算投影点近似解的几何原理

  公式的原理要从几何角度看,画一下相关示意图:
在这里插入图片描述

  在直角坐标系中,匹配点位矢为 r ⃗ m \vec r_m r m,则 d ⃗ \vec d d 向量在 τ ⃗ \vec \tau τ 向量上的分量为 ( d ⃗ ⋅ τ ⃗ ) ⋅ τ ⃗ (\vec d \cdot \vec \tau) \cdot \vec \tau (d τ )τ ,因为 d ⃗ ⋅ τ ⃗ \vec d \cdot \vec \tau d τ 是向量的大小。 τ ⃗ \vec \tau τ 代表分量的方向, ( d ⃗ ⋅ τ ⃗ ) ⋅ τ ⃗ (\vec d \cdot \vec \tau) \cdot \vec \tau (d τ )τ 就是 d ⃗ \vec d d 在方向 τ ⃗ \vec \tau τ 方向上分量的向量,也就是上图所画出的绿色向量。

   r ⃗ m \vec r_m r m 加绿色小向量等于绿色大向量,把绿色大向量近似认为是 r ⃗ r \vec r_r r r,也就是用 r ⃗ m + d ⃗ \vec r_m+\vec d r m+d τ ⃗ \vec \tau τ 上的分量向量的和近似认为是 r ⃗ r \vec r_r r r,即
r ⃗ m + ( d ⃗ ⋅ τ ⃗ ) τ ⃗ = r ⃗ r \vec{r}_m+\left( \vec{d}\cdot \vec{\tau} \right) \vec{\tau}=\vec{r}_r r m+(d τ )τ =r r

1.4 自然坐标系坐标轴为直线时投影点的精确解

  原理显而易见,如果自然坐标系得坐标轴是直线
在这里插入图片描述

   d ⃗ \vec d d 向量,在 τ ⃗ \vec \tau τ 方向上的投影 ( d ⃗ ⋅ τ ⃗ ) ⋅ τ ⃗ (\vec d \cdot \vec \tau) \cdot \vec \tau (d τ )τ 是上图绿色向量, r ⃗ m \vec r_m r m 加绿色向量等于 r ⃗ r \vec r_r r r。可以发现在直线的情况下, r ⃗ r \vec r_r r r 正好就是投影位矢的精确解。

为什么是精确解呢?

  因为上图自然坐标系的坐标是直线,直线上的任何切向量都相等,且任何一点的法向量也都相等,所以匹配点的 n ⃗ \vec n n r ⃗ r \vec r_r r r 所指的 n ⃗ \vec n n 的法向量一样,是同方向,而且和切向量垂直,正好这就是投影的定义,所以是精确解。

1.5 自然坐标系坐标轴为弧线时的投影点计算

  如果自然坐标系的坐标轴是弧线,但如果弧线的曲率不太大,而且离散点比较密时,精度也比较高,比如这样的曲线:
在这里插入图片描述

  如果曲率越小,而且划分越密,每段就越像直线,精度也就越高,这样的话 r ⃗ r \vec r_r r r 就算出来了,但是算出 r ⃗ r \vec r_r r r 还不够,还得算 θ r \theta_r θr κ r \kappa_r κr,下面讲解如何计算 θ r \theta_r θr κ r \kappa_r κr,把图画一下:
在这里插入图片描述

  可近似认为 κ r = κ m \kappa_r=\kappa_m κr=κm

  由曲率的定义式, Δ s \Delta s Δs 为图中粉色线, d ⃗ ⋅ τ ⃗ \vec{d}\cdot \vec{\tau} d τ 为图中深蓝色线,可近似认为 Δ s = d ⃗ ⋅ τ ⃗ \Delta s=\vec{d}\cdot \vec{\tau} Δs=d τ ,则:
k m = d θ d s ≈ Δ θ Δ s ≈ Δ θ ( d ⃗ ⋅ τ ⃗ ) ⇒ Δ θ = k m ⋅ ( d ⃗ ⋅ τ ⃗ ) k_m=\frac{d\theta}{ds}\approx \frac{\Delta \theta}{\Delta s}\approx \frac{\Delta \theta}{\left( \vec{d}\cdot \vec{\tau} \right)}\Rightarrow \Delta \theta =k_m\cdot \left( \vec{d}\cdot \vec{\tau} \right) km=dsdθΔsΔθ(d τ )ΔθΔθ=km(d τ )  所以
θ r = θ m + Δ θ = θ m + κ m ⋅ ( d ⃗ ⋅ τ ⃗ ) \theta _r=\theta _m+\Delta \theta =\theta _m+\kappa _m\cdot \left( \vec{d}\cdot \vec{\tau} \right) θr=θm+Δθ=θm+κm(d τ )  即 θ r = θ m + κ m ⋅ ( d ⃗ ⋅ τ ⃗ ) \theta _r=\theta _m+\kappa _m\cdot \left( \vec{d}\cdot \vec{\tau} \right) θr=θm+κm(d τ )


二、自然坐标系下的弧长计算

2.1 路径长度累加算法

  从起点开始把各个点的长度累加起来,一直加到投影点位置,如图所示:
在这里插入图片描述

s = [ ( x 1 − x 0 ) 2 + ( y 1 − y 0 ) 2 ] 1 2 + [ ( x 2 − x 1 ) 2 + ( y 2 − y 1 ) 2 ] 1 2 + . . . s=\left[ \left( x_1-x_0 \right) ^2+\left( y_1-y_0 \right) ^2 \right] ^{\frac{1}{2}}+\left[ \left( x_2-x_1 \right) ^2+\left( y_2-y_1 \right) ^2 \right] ^{\frac{1}{2}}+... s=[(x1x0)2+(y1y0)2]21+[(x2x1)2+(y2y1)2]21+...

  本质上就是以直线代替弧长,将离散点的弧线以直线的长度近似代替。
在这里插入图片描述

  当路径点足够密时,用直线的长度近似代替弧长,误差可以忽略。

2.2 直线近似弧长的合理性及其误差分析

  有人可能会觉得这真的可以吗?看起来好像比刚才讲的算投影点的算法还要粗糙,还要近似得多,这样真的可以吗?

  算法的精度要看规划的范围,如果是几公里到几百公里不等的规划,这么算的话,误差累积肯定很大。因为每一段都用弧线近似直线,误差会累积。但如果规划的长度只有几十米到几百米不等,这样的近似可以接受,这种算法看起来比较糙,但它也有优点:

  • 简单

  • 稳定

  这是因为任何探测上的数据都会有误差,这是不可避免的。地图有地图的误差,传感器有传感器的误差,探测装置也有硬件误差,就算什么都没问题了,计算机表达的位数也有舍入误差。因为计算机能表达的小数据有限,什么都有误差,那算法的要求就是在参数变化很小范围内,结果不要发生大的变化。

2.3 算法的稳定性与鲁棒性

  用专业术语表达就是 对参数的变化不敏感,如果算法对参数的变化不敏感,就认为是稳定的算法。

其实计算 s s s 有其他更精确的算法,但是都不是特别稳定,但是以分段直线近似曲线这样的算法虽然比较粗糙,但是对参数的变化不敏感,比较稳定。

  描述对于参数敏感的性质叫鲁棒性。实际上就是英语 Robust 单词的音译,根据发音而来,意思就是算法对参数的变化不敏感。像这种比较糙的算法,它的稳定性的,或者说鲁棒性还是比较好的,路径点稍微有点误差的话, s s s 的变化并不大,但有些精确算法的稳定性就很差。

2.4 精确算法举例及其局限性

  下面举个使用精确算法计算的例子。

  比如有一段曲线:
在这里插入图片描述
  若采用圆弧近似的方法,首先计算平均曲率
κ ˉ = k i + k i + 1 2 \bar{\kappa}=\frac{k_i+k_{i+1}}{2} κˉ=2ki+ki+1  曲率半径等于平均曲率的倒数 R = 1 κ ˉ R=\frac{1}{\bar{\kappa}} R=κˉ1。根据三角关系
l = 2 R sin ⁡ θ 2 θ = 2 arcsin ⁡ l 2 R l=2R\sin \frac{\theta}{2}\quad \theta =2\arcsin \frac{l}{2R} l=2Rsin2θθ=2arcsin2Rl  则 s = R θ s=R\theta s=,这样可以算出用圆弧近似曲线的 s s s,这是比用直线近似曲线更精确的算法。

为什么不用这种算法呢?

  因为算法不稳定。曲率 κ ˉ \bar{\kappa} κˉ 一般比较小,而 R = 1 κ ˉ R=\frac{1}{\bar{\kappa}} R=κˉ1 κ ˉ \bar{\kappa} κˉ 的微小的变化会引起 R R R 的剧烈波动,导致算法不稳定。

  大家可以想想初中学过的反比例函数 y = 1 x y=\frac{1}{x} y=x1 函数的图像在 x x x 取接近于 0 0 0 时, y y y 的变化非常剧烈,这就是问题的根源。 κ ˉ \bar{\kappa} κˉ 如果是比较小的数,稍微有一点点测量误差,就会导致 R R R 剧烈波动,所以算法很不稳定,对参数变化非常敏感。

2.5 自动驾驶算法的选择与场景适应性

  当然了除了这种算法还有其他精确算法,实际上其他算法都有各种各样的问题,有的非常依赖参数的精确性。对参数很敏感,也有的对参数不敏感,但是计算非常耗时,所以也不行。

  综合起来还是上面说的看起来比较糙的算法最合适。当然如果有些场景需要精确的 s s s 的话,再单独处理,在这里只是说在大多数情况下都通用的算法。自动驾驶算法没有好与不好,只有合适与不合适。不同算法都有各自的问题,要扬长避短。在大多数情况下,这种比较糙的算法能工作,可以满足当前场景的需求,那算法就没问题。如果某些场景这些算法并不能满足场景的要求,再针对场景做另外更精确的处理。


三、自然坐标转换为直角坐标详细步骤

  已知 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¨ 和自然坐标系坐标轴的离散点信息 ( x i , y i , θ i , κ i ) (x_i,y_i,\theta_i,\kappa_i) (xi,yi,θi,κi),求直角坐标 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

在这里插入图片描述
  坐标转换的关键是找投影点 ( x r , y r , θ r , κ r ) (x_r,y_r,\theta_r,\kappa_r) (xr,yr,θr,κr)

3.1 计算映射表

  首先要计算 x i → s i x_i\rightarrow s_i xisi 之间映射的对应表。即每个Frenet 坐标轴上离散点的 x i x_i xi s i s_i si 之间的对应关系,比如下表:

x x x s s s
x 0 x_0 x0 0 0 0
x 1 x_1 x1 [ ( x 1 − x 0 ) 2 + ( y 1 − y 0 ) 2 ] 1 2 [(x_1-x_0)^2+(y_1-y_0)^2]^{\frac12} [(x1x0)2+(y1y0)2]21
x 2 x_2 x2 ∑ i = 1 2 [ ( x i − x i − 1 ) 2 + ( y i − y i − 1 ) 2 ] 1 2 \sum_{i=1}^{2}[(x_{i}-x_{i-1})^{2}+(y_{i}-y_{i-1})^{2}]^{\frac{1}{2}} i=12[(xixi1)2+(yiyi1)2]21

3.2 查找投影点

  查找 x i → s i x_i\rightarrow s_i xisi 对应表,找到 s n s_n sn,使得 s n < s < s n + 1 s_n<s<s_{n+1} sn<s<sn+1 s n s_n sn s n + 1 s_{n+1} sn+1 对应的 x i x_i xi 记为 x n x_n xn x n + 1 x_{n+1} xn+1,也就是找到 x n , y n , θ n , κ n x_n,y_n,\theta _n,\kappa _n xn,yn,θn,κn x n + 1 , y n + 1 , θ n + 1 , κ n + 1 x_{n+1},y_{n+1},\theta _{n+1},\kappa _{n+1} xn+1,yn+1,θn+1,κn+1

3.3 计算投影点的坐标和参数

在这里插入图片描述

  已知 r ⃗ n = ( x n , y n ) \vec{r}_n=\left( x_n,y_n \right) r n=(xn,yn),图中橙色向量记为 r ⃗ d \vec r_d r d,其大小为 s − s n s-s_n ssn,方向近似为 ( cos ⁡ θ n , sin ⁡ θ n ) (\cos \theta_n,\sin \theta_n) (cosθn,sinθn),所以
r ⃗ d = ( s − s n ) τ ⃗ n \vec{r}_d=\left( s-s_n \right) \vec{\tau}_n r d=(ssn)τ n  若想要更精确些,可以使用到 s n + 1 s_{n+1} sn+1 的信息,则
θ ˉ = θ n + θ n + 1 2 τ ˉ n = ( cos ⁡ θ ˉ , sin ⁡ θ ˉ ) \bar{\theta}=\frac{\theta _n+\theta _{n+1}}{2}\quad \bar{\tau}_n=\left( \cos \bar{\theta},\sin \bar{\theta} \right) θˉ=2θn+θn+1τˉn=(cosθˉ,sinθˉ)  可解出带有 x r , y r x_r,y_r xr,yr 信息的 r ⃗ r \vec r_r r r
r ⃗ r = r ⃗ d + r ⃗ r \vec{r}_r=\vec{r}_d+\vec{r}_r r r=r d+r r  以及 θ r = θ n + κ n ⋅ ( s − s n ) κ r = κ n + κ n + 1 2 κ r ′ = d κ r d s ≈ κ n + 1 − κ n s n + 1 − s n τ ⃗ r = ( cos ⁡ θ r , sin ⁡ θ r ) n ⃗ r = ( − sin ⁡ θ r , cos ⁡ θ r ) \begin{aligned} \theta _r&=\theta _n+\kappa _n\cdot \left( s-s_n \right)\\ \kappa _r&=\frac{\kappa _n+\kappa _{n+1}}{2}\\ \kappa _{r}^{'}&=\frac{d\kappa _r}{ds}\approx \frac{\kappa _{n+1}-\kappa _n}{s_{n+1}-s_n}\\ \vec{\tau}_r&=\left( \cos \theta _r,\sin \theta _r \right) \quad \vec{n}_r=\left( -\sin \theta _r,\cos \theta _r \right)\\ \end{aligned} θrκrκrτ r=θn+κn(ssn)=2κn+κn+1=dsdκrsn+1snκn+1κn=(cosθr,sinθr)n r=(sinθr,cosθr)  最后就可以通过第三讲上半篇所讲的这七个辅助公式:
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{align} 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{align} ll˙s˙ll¨s¨l′′=(r hr r)n r=v n r=1κrlv τ r=s˙l˙=a n rκr(1κrl)s˙2=1κrla τ r+1κrls˙2κrl+1κrls˙2(κrl+κrl)=s˙2l¨ls¨  已知 s , s ˙ , s ¨ , l , l ′ , l ′ ′ , l ˙ , l ¨ , r ⃗ r , τ ⃗ r , n ⃗ r , κ r , κ r ′ s,\dot{s},\ddot{s},l,l^{\prime},l^{\prime\prime},\dot l,\ddot l, \vec{r}_r,\vec{\tau}_r,\vec{n}_r,\kappa _r,\kappa _r' s,s˙,s¨,l,l,l′′,l˙,l¨,r r,τ r,n r,κr,κr,求 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

3.4 计算车辆位矢

  计算 r ⃗ h \vec r_h r h, 有如下三角关系:

在这里插入图片描述

  可直接得到
r ⃗ h = r ⃗ r + l n ⃗ r \vec{r}_h=\vec{r}_r+l\vec{n}_r r h=r r+ln r

3.5 计算车辆速度向量

  计算 v ⃗ h \vec v_h v h,由公式 ( 2 ) ( 3 ) (2)(3) (2)(3) l ˙ = v ⃗ ⋅ n ⃗ r \dot{l}=\vec{v}\cdot \vec{n}_r l˙=v n r s ˙ = v ⃗ ⋅ τ ⃗ r 1 − κ r l \dot{s}=\frac{\vec{v}\cdot \vec{\tau}_r}{1-\kappa _rl} s˙=1κrlv τ r 可推导出:
v ⃗ h ⋅ n ⃗ r = l ˙ ⇒ ∣ v ⃗ h ∣ ∣ n ⃗ r ∣ cos ⁡ < v ⃗ h , n ⃗ r > = l ˙ v ⃗ h ⋅ τ ⃗ r = ( 1 − κ r l ) s ˙ ⇒ ∣ v ⃗ n ∣ ∣ τ ⃗ r ∣ cos ⁡ ( v ⃗ h ⋅ τ ⃗ r ) = ( 1 − κ r l ) s ˙ \begin{aligned} \vec{v}_h\cdot \vec{n}_r=\dot{l}\quad &\Rightarrow \quad|\vec{v}_h||\vec{n}_r|\cos <\vec{v}_h,\vec{n}_r>=\dot{l}\\ \vec{v}_h\cdot \vec{\tau}_r=\left( 1-\kappa _rl \right) \dot{s}\quad &\Rightarrow \quad|\vec{v}_n||\vec{\tau}_r|\cos \left( \vec{v}_h\cdot \vec{\tau}_r \right) =\left( 1-\kappa _rl \right) \dot{s}\\ \end{aligned} v hn r=l˙v hτ r=(1κrl)s˙v h∣∣n rcos<v h,n r>=l˙v n∣∣τ rcos(v hτ r)=(1κrl)s˙  设 v h v_h vh x x x 轴夹角为 θ h \theta_h θh,因为 τ ⃗ r \vec \tau_r τ r x x x 轴的夹角为 θ r \theta_r θr n ⃗ r \vec n_r n r τ ⃗ r \vec \tau_r τ r 垂直,所以 n ⃗ r \vec n_r n r x x x 轴夹角为 θ r + π 2 \theta_r+\frac{\pi}{2} θr+2π,所以:
cos ⁡ < v ⃗ h , n ⃗ r > = cos ⁡ ( θ h − ( θ r + π 2 ) ) = sin ⁡ ( θ h − θ r ) cos ⁡ < v ⃗ h , τ ⃗ r > = cos ⁡ ( θ h − θ r ) \begin{aligned} \cos <\vec{v}_h,\vec{n}_r>&=\cos \left( \theta _h-\left( \theta _r+\frac{\pi}{2} \right) \right) =\sin \left( \theta _h-\theta _r \right)\\ \cos <\vec{v}_h,\vec{\tau}_r>&=\cos \left( \theta _h-\theta _r \right)\\ \end{aligned} cos<v h,n r>cos<v h,τ r>=cos(θh(θr+2π))=sin(θhθr)=cos(θhθr)  代入到上式,得到 v ⃗ \vec v v 的大小:
∣ v ⃗ ∣ sin ⁡ ( θ h − θ r ) = l ˙ ∣ v ⃗ ∣ cos ⁡ ( θ h − θ r ) = ( 1 − κ r l ) s ˙ } ⇒ 平方相加 ∣ v ⃗ ∣ = [ l ˙ 2 + ( 1 − κ r l ) 2 s ˙ 2 ] 1 2 \left. \begin{array}{r} |\vec{v}|\sin \left( \theta _h-\theta _r \right) =\dot{l}\\ |\vec{v}|\cos \left( \theta _h-\theta _r \right) =\left( 1-\kappa _rl \right) \dot{s}\\ \end{array} \right\} \xRightarrow{\text{平方相加}}|\vec{v}|=\left[ \dot{l}^2+\left( 1-\kappa _rl \right) ^2\dot{s}^2 \right] ^{\frac{1}{2}} v sin(θhθr)=l˙v cos(θhθr)=(1κrl)s˙}平方相加 v =[l˙2+(1κrl)2s˙2]21  下面计算 v ⃗ \vec v v 的方向,根据公式 ( 5 ) (5) (5): l ′ = l ˙ s ˙ l'=\frac{\dot{l}}{\dot{s}} l=s˙l˙
∣ v ⃗ ∣ sin ⁡ ( θ h − θ r ) = l ˙ ∣ v ⃗ ∣ cos ⁡ ( θ h − θ r ) = ( 1 − κ r l ) s ˙ } ⇒ 相除 tan ⁡ ( θ h − θ r ) = l ˙ ( 1 − κ r l ) s ˙ = l ′ 1 − κ r l \left. \begin{array}{r} |\vec{v}|\sin \left( \theta _h-\theta _r \right) =\dot{l}\\ |\vec{v}|\cos \left( \theta _h-\theta _r \right) =\left( 1-\kappa _rl \right) \dot{s}\\ \end{array} \right\} \xRightarrow{\text{相除}}\tan \left( \theta _h-\theta _r \right) =\frac{\dot{l}}{\left( 1-\kappa _rl \right) \dot{s}}=\frac{l'}{1-\kappa _rl} v sin(θhθr)=l˙v cos(θhθr)=(1κrl)s˙}相除 tan(θhθr)=(1κrl)s˙l˙=1κrll  可得:
θ h = θ r + arctan ⁡ ( l ′ 1 − κ r l ) \theta _h=\theta _r+\arctan \left( \frac{l'}{1-\kappa _rl} \right) θh=θr+arctan(1κrll)  可得到 v ⃗ \vec v v 的切向方向向量 τ ⃗ h \vec \tau_h τ h 以及 v v v 的法向方向向量 n ⃗ h \vec n_h n h
τ ⃗ h = ( cos ⁡ θ h , sin ⁡ θ h ) n ⃗ h = ( − sin ⁡ θ h , cos ⁡ θ h ) \vec{\tau}_h=\left( \cos \theta _h,\sin \theta _h \right) \quad \vec{n}_h=\left( -\sin \theta _h,\cos \theta _h \right) τ h=(cosθh,sinθh)n h=(sinθh,cosθh)  其中, τ ⃗ h \vec{\tau}_h τ h 代表 v ⃗ \vec v v 的方向。

3.6 计算车辆加速度向量

  计算 a ⃗ h \vec a_h a h,使用这两个公式:
l ¨ = a ⃗ ⋅ n ⃗ r − κ r ( 1 − κ r l ) s ˙ 2 \ddot{l}=\vec{a}\cdot \vec{n}_r-\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2 l¨=a n rκr(1κrl)s˙2 s ¨ = a ⃗ ⋅ τ ⃗ r 1 − κ r l + s ˙ 2 κ r l ′ 1 − κ r l + s ˙ 2 ( κ r ′ l + κ r l ′ ) 1 − κ r l \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} s¨=1κrla τ r+1κrls˙2κrl+1κrls˙2(κrl+κrl)  可计算出
a ⃗ h ⋅ n ⃗ r = l ¨ + κ r ( 1 − κ r l ) s ˙ 2 a ⃗ h ⋅ τ ⃗ r = s ¨ ( 1 − κ r l ) − s ˙ 2 κ r l ′ + s ˙ 2 ( κ r ′ l + κ r l ′ ) \begin{aligned} \vec{a}_h\cdot \vec{n}_r&=\ddot{l}+\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2\\ \vec{a}_h\cdot \vec{\tau}_r&=\ddot{s}\left( 1-\kappa _rl \right) -\dot{s}^2\kappa _rl'+\dot{s}^2\left( \kappa _{r}'l+\kappa _rl' \right)\\ \end{aligned} a hn ra hτ r=l¨+κr(1κrl)s˙2=s¨(1κrl)s˙2κrl+s˙2(κrl+κrl)  同理,上面两式平方相加可得
∣ a ⃗ h ∣ = { [ l ˙ + κ r ( 1 − κ r l s ˙ 2 ] 2 + [ s ¨ ( 1 − κ r l ) − s ˙ 2 κ r l ′ + s ˙ 2 ( κ r ′ l + κ r l ′ ) ] 2 } 1 2 |\vec{a}_h|=\{\left[ \dot{l}+\kappa _r\text{(}1-\kappa _rl\dot{s}^2 \right] ^2+\left[ \ddot{s}\left( 1-\kappa _rl \right) -\dot{s}^2\kappa _rl'+\dot{s}^2\left( \kappa _{r}'l+\kappa _rl' \right) \right] ^2\}^{\frac{1}{2}} a h={[l˙+κr(1κrls˙2]2+[s¨(1κrl)s˙2κrl+s˙2(κrl+κrl)]2}21  设 a ⃗ h \vec a_h a h x x x 轴夹角为 θ a h \theta_{ah} θah,则
θ a h = arctan ⁡ ( l ¨ + κ r ( 1 − κ r l ) s ˙ 2 s ¨ ( 1 − κ r l ) − s ˙ 2 κ r l ′ + s ˙ 2 ( κ r ′ l + κ r l ′ ) ) + θ r \theta _{ah}=\arctan \left( \frac{\ddot{l}+\kappa _r\left( 1-\kappa _rl \right) \dot{s}^2}{\ddot{s}\left( 1-\kappa _rl \right) -\dot{s}^2\kappa _rl'+\dot{s}^2\left( \kappa '_rl+\kappa _rl' \right)} \right) +\theta _r θah=arctan(s¨(1κrl)s˙2κrl+s˙2(κrl+κrl)l¨+κr(1κrl)s˙2)+θr
  注意 ∣ v ˙ ⃗ ∣ ≠ ∣ a ⃗ h ∣ |\vec{\dot v}|\neq|\vec{a}_h| v˙ =a h。因为

a ⃗ h = ∣ v ˙ ⃗ ∣ τ ⃗ r + ∣ v ⃗ ∣ 2 κ h n ⃗ r \vec{a}_h=|\vec{\dot{v}}|\vec{\tau}_r+|\vec{v}|^2\kappa _h\vec{n}_r a h=v˙ τ r+v 2κhn r

  其中, ∣ v ˙ ⃗ ∣ τ ⃗ r |\vec{\dot{v}}|\vec{\tau}_r v˙ τ r 为切向加速度, ∣ v ⃗ ∣ 2 κ h n ⃗ r |\vec{v}|^2\kappa _h\vec{n}_r v 2κhn r 为向心加速度。

  由 θ a h \theta_{ah} θah 可计算出 τ ⃗ a h \vec \tau_{ah} τ ah
τ ⃗ a h = ( cos ⁡ ( θ a h ) , sin ⁡ ( θ a h ) ) \vec{\tau}_{ah}=\left( \cos \left( \theta _{ah} \right) ,\sin \left( \theta _{ah} \right) \right) τ ah=(cos(θah),sin(θah))  由此可得到向量 a ⃗ h \vec a_h a h 的完整形式:
a ⃗ h = ∣ a ⃗ h ∣ τ ⃗ a h \vec{a}_h=|\vec{a}_h|\vec{\tau}_{ah} a h=a hτ ah

3.7 计算曲率

  计算曲率 κ h \kappa_h κh,由 a ⃗ h = ∣ v ˙ ⃗ ∣ τ ⃗ r + ∣ v ⃗ ∣ 2 κ h n ⃗ r \vec{a}_h=|\vec{\dot{v}}|\vec{\tau}_r+|\vec{v}|^2\kappa _h\vec{n}_r a h=v˙ τ r+v 2κhn r 两边点乘 n ⃗ r \vec n_r n r
a ⃗ ⋅ n ⃗ r = ∣ v ⃗ ∣ 2 κ h \vec{a}\cdot \vec{n}_r=|\vec{v}|^2\kappa _h a n r=v 2κh  则 κ h = a ⃗ ⋅ n ⃗ r ∣ v ⃗ ∣ 2 \kappa _h=\frac{\vec{a}\cdot \vec{n}_r}{|\vec{v}|^2} κh=v 2a n r  这样就把所有的 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 都算出来了。

四、总结

  数学基础部分的所有内容基本上全讲完了,这也是数学基础部分的完结篇。从第二章开始,将正式进入决策规划的主要内容,讲相关算法。因为数学基础部分的铺垫就已经将所需要的数学知识基本上都已经铺垫完了。

  在博主看来,第一章中最重要、最难的内容肯定是第三节,但最重要的内容实际上是第二集,要仔细体会凸优化以及非凸优化是怎么转化为凸问题的,在求解这些东西时要理解透彻,对于第一节和第三节,其实只是工具类的说明,只要会用就可以了。所以大家要把第一章的第二节好好看一看。后续的第二章将详细讲解决策规划的相关内容,欢迎关注!


参考资料

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


后记:

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

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

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

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

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

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处:http://www.mzph.cn/bicheng/54260.shtml

如若内容造成侵权/违法违规/事实不符,请联系多彩编程网进行投诉反馈email:809451989@qq.com,一经查实,立即删除!

相关文章

Karpathy认为“LLM”这个名字不准确 马斯克非常赞同

LLM 应该改名吗&#xff1f;你怎么看。在 AI 领域&#xff0c;几乎每个人都在谈论大型语言模型&#xff0c;其英文全称为 Large Language Models&#xff0c;简写为 LLM。因为 LLM 中有“Language”一词&#xff0c;因此&#xff0c;大家默认这种技术和语言密切相关。然而&…

纯小白安装pytorch(快速上手)

1.首先进入你的虚拟环境(不进入也没关系) 在anaconda prompt中打开&#xff0c;输入activate 虚拟环境2.查看自己的conda源 conda config --show channels3.清空自己的conda源 conda config --remove-key channels4.添加源 conda config --add channels https://mirrors.tu…

动态规划:07.路径问题_珠宝的最大价值_C++

题目链接&#xff1a;LCR 166. 珠宝的最高价值 - 力扣&#xff08;LeetCode&#xff09;https://leetcode.cn/problems/li-wu-de-zui-da-jie-zhi-lcof/description/ 一、题目解析 题目&#xff1a; 解析&#xff1a; 有过做前几道题的经验&#xff0c;我们会发现这道题其实就…

SpringBoot2:web开发常用功能实现及原理解析-上传与下载

文章目录 一、上传文件1、前端上传文件给Java接口2、Java接口上传文件给Java接口 二、下载文件1、前端调用Java接口下载文件2、Java接口下载网络文件到本地3、前端调用Java接口下载网络文件 一、上传文件 1、前端上传文件给Java接口 Controller接口 此接口支持上传单个文件和…

如何准备教师资格证科目三“学科知识与教学能力”的考试与面试?(理科导向:数学/物理)

如何准备教师资格证科目三“学科知识与教学能力”的考试与面试&#xff1f;&#xff08;理科导向&#xff1a;数学/物理&#xff09; ​ 目录 收起 1 前言 1.1 自身经历 1.2 教师资格证的作用 2 知识点题型分数的分布与学习建议 2.1 科目三的知识点分数分布&#xff1a; …

求和(2)

题目描述 输入两个正整数 l,r&#xff0c;编程计算 l(l1)(l2)...(r−1)r 的结果并输出。 输入格式 一行两个整数 l 和 r 输出格式 一个整数&#xff0c;根据题意计算后的结果 样例数据 样例输入#1 1 5样例输出#1 15样例输入#2 8 10样例输出#2 27数据范围 对于100%的…

【Ubuntu】虚拟机安装USB摄像头ROS驱动 usb_cam(最新方法)

写在前面&#xff1a; &#x1f31f; 欢迎光临 清流君 的博客小天地&#xff0c;这里是我分享技术与心得的温馨角落。&#x1f4dd; 个人主页&#xff1a;清流君_CSDN博客&#xff0c;期待与您一同探索 移动机器人 领域的无限可能。 &#x1f50d; 本文系 清流君 原创之作&…

Nest.js

Nestjs中文文档链接 TypeORM 中文文档 小满视频 1. 安装Nest.js 安装脚手架 npm i -g nestjs/cli创建nestjs工程 nest new工程目录 app.module.ts 根模块用于处理其他类的引用与共享。app.controller.ts 常见功能是用来处理http请求&#xff08;处理请求的路径&#xff09…

如何升级用 Helm 安装的极狐GitLab Runner?

本分分享如何对 Helm 安装的 Runner 进行升级。整个过程分为三步&#xff1a;1、确定 Runner 最新版本或者想要升级的版本是否存在&#xff1b;2、用 Helm upgrade 命令进行升级&#xff1b;3、升级确认。 极狐GitLab 为 GitLab 的中国发行版&#xff0c;中文版本对中国用户更…

react18基础教程系列-- 框架基础理论知识mvc/jsx/createRoot

react的设计模式 React 是 mvc 体系&#xff0c;vue 是 mvvm 体系 mvc: model(数据)-view(视图)-controller(控制器) 我们需要按照专业的语法去构建 app 页面&#xff0c;react 使用的是 jsx 语法构建数据层&#xff0c;需要动态处理的的数据都要数据层支持控制层: 当我们需要…

1730. 购买贺年卡

代码 #include<bits/stdc.h> using namespace std; struct c {int a,b; }t[1005]; int cmp(c a,c b) {return a.a>b.a; } int main() {int n,m,sum0;cin>>n>>m;for(int i1;i<m;i){cin>>t[i].a>>t[i].b;}sort(t1,t1m,cmp);for(int im;i&g…

如何在Linux下升级R版本和RStudio

一、升级R版本 在Linux上&#xff0c;R的安装通常通过包管理器完成。不同的Linux发行版&#xff08;如Ubuntu、Debian、Fedora等&#xff09;可能略有不同。下面以Ubuntu为例&#xff0c;介绍如何升级R版本。如果你使用其他发行版&#xff0c;步骤可能类似。 二.更新步骤 2.…

【可视化大屏系列】数据列表自动滚动效果

要实现列表的自动滚动效果&#xff0c;这里提供两种解决方案&#xff1a; 1.vue插件 官方文档&#xff1a;链接: vue-seamless-scroll &#xff08;1&#xff09;安装依赖 npm install vue-seamless-scroll --save&#xff08;2&#xff09;全局注册&#xff08;main.js中&a…

【机器学习】--- 自监督学习

1. 引言 机器学习近年来的发展迅猛&#xff0c;许多领域都在不断产生新的突破。在监督学习和无监督学习之外&#xff0c;自监督学习&#xff08;Self-Supervised Learning, SSL&#xff09;作为一种新兴的学习范式&#xff0c;逐渐成为机器学习研究的热门话题之一。自监督学习…

【linux-Day3】linux的基本指令<中>

【linux-Day3】linux的基本指令<中> linux下的基本指令&#x1f4e2;man&#xff1a;访问linux手册页&#x1f4e2;echo&#xff1a;把字符串写入指定文件中&#x1f4e2;cat&#xff1a;查看目标文件的内容&#x1f4e2;cp&#xff1a;复制文件或目录&#x1f4e2;mv&am…

AJAX 入门 day3

目录 1.XMLHttpRequest 1.1 XMLHttpRequest认识 1.2 用ajax发送请求 1.3 案例 1.4 XMLHttpRequest - 查询参数 1.5 XMLHttpRequest - 数据提交 2.Promise 2.1 Promise认识 2.2 Promise - 三种状态 2.3 案例 3.封装简易版 axios 3.1 封装_简易axios_获取省份列表 3…

Fisco Bcos 2.11.0配置console控制台2.10.0及部署调用智能合约

Fisco Bcos 2.11.0配置console控制台2.10.0及部署调用智能合约 文章目录 Fisco Bcos 2.11.0配置console控制台2.10.0及部署调用智能合约前言版本适配一、启动FIsco Bcos区块链网络二、获取控制台文件三、配置控制台3.1 执行download_console.sh脚本3.2 拷贝控制台配置文件3.3 修…

Mac下nvm无法安装node问题

背景 最近换用mac开发&#xff0c;然后使用nvm&#xff08;版本0.40.1&#xff09;进行node安装的时候出现了一些问题 使用 nvm ls-remote发现只有 iojs 版本 原因可能是nodejs升级了某个协议导致的 解决方案 可以使用 NVM_NODEJS_ORG_MIRRORhttp://nodejs.org/dist nvm ls-re…

数据结构(八)——Java实现七大排序

一、插入排序 1.直接插入排序 public static void insertSort(int []arr){for (int i 0; i < arr.length; i) {int j i-1;int tmp arr[i];for (; j >0 ; j--) {if(arr[j] > tmp){arr[j1] arr[j];}else{break;}}arr[j1] tmp;}}直接插入排序特性总结 1. 元素集合越…

TikTok商家如何通过真人测评提高流量和销量?

在当今的社交媒体营销领域&#xff0c;TikTok&#xff08;抖音国际版&#xff09;以其独特的短视频内容和庞大的用户群体&#xff0c;成为了品牌营销和产品推广的热门平台。其中&#xff0c;真人测评作为一种有效的营销策略&#xff0c;正逐渐受到商家的青睐。本文将探讨TikTok…