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

写在前面:
🌟 欢迎光临 清流君 的博客小天地,这里是我分享技术与心得的温馨角落。📝
个人主页:清流君_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.rhkb.cn/news/428870.html

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

相关文章

Centos中关闭swap分区,关闭内存交换

概述&#xff1a; Swap 分区是 Linux 系统中扩展物理内存的一种机制。Swap的主要功能是当全部的RAM被占用并需要更多内存时&#xff0c;用磁盘空间代理RAM内存。Swap对虚拟化技术资源损耗非常大&#xff0c;一般虚拟化是不允许开启交换空间的&#xff0c;如果不关闭Swap&…

LED显示屏迎来革新:GOB封装技术引领行业新风尚

在我们日常生活中&#xff0c;LED显示屏无处不在&#xff0c;从繁华的街头广告牌到家庭娱乐中心的大屏幕电视&#xff0c;它们都以鲜明的色彩和清晰的画质吸引着我们的目光。然而&#xff0c;在LED显示屏技术日新月异的今天&#xff0c;一种名为GOB&#xff08;Glue On Board&a…

ChatCADChatCAD+:Towards a Universal and Reliable Interactive CAD using LLMs

ChatCAD&#xff08;论文链接&#xff1a;[2302.07257] ChatCAD: Interactive Computer-Aided Diagnosis on Medical Image using Large Language Models (arxiv.org)&#xff09; 网络流程图&#xff1a; 辅助阅读&#xff1a; 基于大型语言模型的医学图像交互式计算机辅助诊…

7、论等保的必要性

数据来源&#xff1a;7.论等保的必要性_哔哩哔哩_bilibili 等级保护必要性 降低信息安全风险 等级保护旨在降低信息安全风险&#xff0c;提高信息系统的安全防护能力。 风险发现与整改 开展等级保护的最重要原因是通过测评工作&#xff0c;发现单位系统内外部的安全风险和脆弱…

基于SpringBoot的考研助手系统+LW参考示例

系列文章目录 1.基于SSM的洗衣房管理系统原生微信小程序LW参考示例 2.基于SpringBoot的宠物摄影网站管理系统LW参考示例 3.基于SpringBootVue的企业人事管理系统LW参考示例 4.基于SSM的高校实验室管理系统LW参考示例 5.基于SpringBoot的二手数码回收系统原生微信小程序LW参考示…

c++9月20日

1.思维导图 2.顺序表 头文件 #ifndef RECTANGLE_H #define RECTANGLE_H#include <iostream>using namespace std;using datatype int ;//类型重定义class Seqlist { private://私有权限datatype *ptr; //指向堆区申请空间的起始地址int size;//堆区空间的长度int len …

在python爬虫中xpath方式提取lxml.etree._ElementUnicodeResult转化为字符串str类型

简单提取网页中的数据时发现的 当通过xpath方式提取出需要的数据的text文本后想要转为字符串&#xff0c;但出现lxml.etree._ElementUnicodeResult的数据类型不能序列化&#xff0c;在网上查找到很多说是编码问题Unicode编码然后解码什么的&#xff1b;有些是(导入的xml库而不…

【24华为杯数模研赛赛题思路已出】国赛B题思路丨附参考代码丨免费分享

2024年华为杯研赛B题解题思路 B题 WLAN组网中网络吞吐量建模 问题1 请根据附件WLAN网络实测训练集中所提供的网络拓扑、业务流量、门限、节点间RSSI的测试基本信息&#xff0c;分析其中各参数对AP发送机会的影响&#xff0c;并给出影响性强弱的顺序。通过训练的模型&#xff…

在SpringBoot项目中利用Redission实现布隆过滤器(布隆过滤器的应用场景、布隆过滤器误判的情况、与位图相关的操作)

文章目录 1. 布隆过滤器的应用场景2. 在SpringBoot项目利用Redission实现布隆过滤器3. 布隆过滤器误判的情况4. 与位图相关的操作5. 可能遇到的问题&#xff08;Redission是如何记录布隆过滤器的配置参数的&#xff09;5.1 问题产生的原因5.2 解决方案5.2.1 方案一&#xff1a;…

夏日遛娃绝佳之地:气膜儿童乐园—轻空间

随着夏季的到来&#xff0c;炎炎烈日让户外活动变得有些艰难。然而&#xff0c;在城市的某个角落&#xff0c;一座气膜儿童乐园却为家长和孩子们提供了一个理想的避暑天堂。这里的恒温控制保持在舒适的27℃&#xff0c;让孩子们在欢乐中享受每一个夏日的阳光&#xff0c;而家长…

由于安全风险,安全领导者考虑禁止人工智能编码

安全团队与开发团队之间的紧张关系 83% 的安全领导者表示&#xff0c;他们的开发人员目前使用人工智能来生成代码&#xff0c;57% 的人表示这已成为一种常见做法。 然而&#xff0c;72% 的人认为他们别无选择&#xff0c;只能允许开发人员使用人工智能来保持竞争力&#xff0…

IDA Pro基本使用

IDA Pro基本使用 1.DllMain的地址是什么? 打开默认在的位置1000D02E就是DllMain地址 按空格键可以看到图形化界面选择options、general勾选对应的选项在图像化也能看到 2.使用Imports 窗口并浏览到 gethostbyname&#xff0c;导入函数定位到什么地址? 这里可以打开Impo…

为人机交互保持预见性丨基于G32A1445的T-BOX应用方案

T-BOX是一种集成了通信、计算和控制功能的车载信息处理终端&#xff0c;通过车辆与云端、移动网络等进行数据交互&#xff0c;用于车、人、外部环境的互联互通&#xff0c;支持车辆定位、车载通信、远程控制、故障诊断、数据传输、紧急呼叫等功能&#xff0c;帮助车辆实现更加智…

OpenCV_最简单的鼠标截取ROI区域

在OpenCV中也存在鼠标的操作&#xff0c;今天我们先介绍一下鼠标中的操作事件 void setMousecallback(const string& winname, MouseCallback onMouse, void* userdata0) setMousecallback参数说明&#xff1a; winname:窗口的名字 onMouse:鼠标响应函数&#xff0c;回调…

计算机毕业设计 社区医疗服务系统的设计与实现 Java实战项目 附源码+文档+视频讲解

博主介绍&#xff1a;✌从事软件开发10年之余&#xff0c;专注于Java技术领域、Python人工智能及数据挖掘、小程序项目开发和Android项目开发等。CSDN、掘金、华为云、InfoQ、阿里云等平台优质作者✌ &#x1f345;文末获取源码联系&#x1f345; &#x1f447;&#x1f3fb; 精…

线性代数书中求解线性方程组的三种方法的实例

目录 一、克拉默法则(P45) 二、逆矩阵(P46) 三、高斯-约旦消元法(P65) 一、克拉默法则(P45) 二、逆矩阵(P46) 三、高斯-约旦消元法(P65)

刷题训练之字符串

> 作者&#xff1a;დ旧言~ > 座右铭&#xff1a;松树千年终是朽&#xff0c;槿花一日自为荣。 > 目标&#xff1a;熟练掌握字符串算法。 > 毒鸡汤&#xff1a;学习&#xff0c;学习&#xff0c;再学习 ! 学&#xff0c;然后知不足。 > 专栏选自&#xff1a;刷题…

【笔记】1.3 塑性变形

一、塑性变形的方式 DDWs&#xff08;Dislocation-Dipole Walls&#xff0c;位错偶极墙&#xff09;&#xff1a;指由两个位错构成的结构&#xff0c;它们以一种特定的方式排列在一起&#xff0c;形成一个稳定的结构单元。 DTs&#xff08;Dislocation Tangles&#xff0c;位错…

allWebPlugin中间件自定义alert、confirm及prompt使用

allWebPlugin简介 allWebPlugin中间件是一款为用户提供安全、可靠、便捷的浏览器插件服务的中间件产品&#xff0c;致力于将浏览器插件重新应用到所有浏览器。它将现有ActiveX控件直接嵌入浏览器&#xff0c;实现插件加载、界面显示、接口调用、事件回调等。支持Chrome、Firefo…

关于 NLP 深度学习的核心流程

基本流程实现的先后顺序&#xff08;每一步都包含很多技术点&#xff09;&#xff1a; 选定语言模型结构 关于语言模型&#xff1a; 作用 判断那一句话相对更合理&#xff0c;相对不合理的会得到较底的分值&#xff1a; 挑选成句概率分值最高的评价指标&#xff1a;PPL&…