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

news2024/11/13 9:24:42

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

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

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

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

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


文章目录

  • 引言
  • 一、误差计算概述
    • 1、连续轨迹的横向误差
    • 2、连续曲线的投影问题
    • 3、离散轨迹点的误差
  • 二、离散轨迹规划误差计算步骤
    • 1、匹配点
    • 2、投影点
    • 3、横向误差
    • 4、纵向误差
    • 5、投影点的航向角
    • 6、四个式子
  • 三、总结


引言

  本篇博客是 自动驾驶控制算法 系列的第七节。内容整理自 B站知名up主 忠厚老实的老王 的视频,作为博主的学习笔记,分享给大家共同学习。

  本节博客讲解离散轨迹的误差计算,是理论部分中的最后一节,如果看完了前七节,可以自己尝试搭建横向控制模型,因为横向控制所需要的知识和理论,以及存在的障碍,都讲明白了,大家可以自己尝试搭建相关理论模型,编写代码。


一、误差计算概述

  在前面六节讲到了最优的横向控制:
u = − k e r r + δ f u=-ke_{rr}+\delta _f u=kerr+δf其中, k {k} k l q r ( A , B , Q , R ) \mathrm{lqr}(A,B,Q,R) lqr(A,B,Q,R) d l q r ( A ˉ , B ˉ , Q , R ) {{\mathrm{dlqr}(\bar{A},\bar{B},Q,R)}} dlqr(Aˉ,Bˉ,Q,R)的解。

关于这些量的计算:

  • A 、 B A、B AB 的计算在第三节和第四节
  • k k k 的计算在第五节
  • δ f \delta_f δf 的计算在第六节

还剩误差 e r r e_{rr} err 的计算。

1、连续轨迹的横向误差

  在第四节讲过相关误差的计算,先把图画一下:
在这里插入图片描述

   在第四节讲到了 e d e_d ed
e d = ( x ⃗ − x ⃗ r ) ⋅ n ⃗ r e ˙ d = ∣ v ⃗ ∣ sin ⁡ ( θ − θ r ) e φ = φ − θ r e ˙ φ = φ ˙ − κ s ˙ \begin{aligned} e_d&=\left( \vec{x}-\vec{x}_r \right) \cdot \vec{n}_r\\ \dot{e}_d&=|\vec{v}|\sin \left( \theta -\theta _r \right)\\ e_{\varphi}&=\varphi -\theta _r\\ \dot{e}_{\varphi}&=\dot{\varphi}-\kappa \dot{s}\\ \end{aligned} ede˙deφe˙φ=(x x r)n r=v sin(θθr)=φθr=φ˙κs˙其中, θ ˙ r = κ s ˙ \dot{\theta}_r=\kappa \dot{s} θ˙r=κs˙ 由曲率定义式推导出来。

计算误差需要 x ⃗ r \vec{x}_r x r n ⃗ r \vec{n}_r n r 这种投影点的相关信息:

  • x ⃗ r = ( x r , y r ) \vec{x}_r=(x_r,y_r) x r=(xr,yr) 为投影点的直角坐标
  • θ r \theta _r θr为投影点的速度 s ˙ \dot s s˙ 与 x 轴的夹角
  • κ \kappa κ 为投影点的曲率
  • s ˙ \dot{s} s˙ 为投影点的速度
    s ˙ = ∣ v ⃗ ∣ cos ⁡ ( θ − θ r ) 1 − κ e d \dot{s}=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d} s˙=1κedv cos(θθr)

而剩下的这几个自变量:

  • x ⃗ \vec{x} x 为车的位置
  • v ⃗ \vec v v 为车的速度
  • φ ˙ \dot \varphi φ˙ 为车的横摆角速度

都可以通过上游的定位模块以及传感器的信号采集到,视为已知。

   所以只要知道 x r 、 y r 、 θ r 、 k 、 s r x_r、y_r、\theta_r 、k、s_r xryrθrksr s r s_r sr 为投影点在自然坐标系下的坐标)
在这里插入图片描述

   如果知道这些信息,就可以把误差计算出来。

   注意:如果规划曲线为连续曲线,那么可能导致投影点不唯一。

2、连续曲线的投影问题

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

A A A 的投影定义:如果 A A A A ′ A' A 的连线与 A ′ A' A 的切线垂直,则称 A ′ A' A A A A 的投影。

  • 直线
    直线的投影是最简单的, A A A 在直线的投影就是 A ′ A' A

  • 简单曲线
    简单曲线的投影比较简单,只要 A A A A ′ A' A 的连线和 A ′ A' A 的切线方向 τ ⃗ \vec \tau τ 垂直,则 A ′ A' A 就是它的投影。


  • 对于封闭的圆, A A A 正好在圆心处,按照定义, A A A 在圆上的投影有无数个,圆上每点都是它的投影。

  • 椭圆
    在椭圆半长轴上, A A A 的投影有四个。

   如果曲线连续,不仅求投影比较麻烦,而且要处理多值问题,即投影可能不止一个,这是非常麻烦的事情。

   在这里并不是说连续曲线的规划就不能用,各有各种用法,要用连续曲线做规划,在规划层面上就必须要考虑到投影的多值性问题。

3、离散轨迹点的误差

如果轨迹规划是离散的,怎么求投影相对比较容易处理?

   本篇博客讲解离散轨迹点的误差计算,应用比较广泛,且相对较简单。第四节讲过的是连续轨迹曲线的误差计算公式。

   比如,轨迹规划出一系列离散点:
在这里插入图片描述

   要用离散的轨迹规划做相应的控制,就要进行相应的误差计算,离散轨迹点的每一点都包含四个信息:
( x 1 , y 1 , θ 1 , κ 1 ) ( x 2 , y 2 , θ 2 , κ 2 ) . . . ( x n , y n , θ n , κ n ) \begin{gathered} (x_1,y_1,\theta_1,\kappa_1) \\ (x_2,y_2,\theta_2,\kappa_2) \\ ...\\ (x_n,y_n,\theta_n,\kappa_n) \end{gathered} (x1,y1,θ1,κ1)(x2,y2,θ2,κ2)...(xn,yn,θn,κn)   这仅仅是做横向控制,如果要做纵向控制的话,还需要包含离散点的在自然坐标系下的 s 1 、 s 2 、 s 3 s_1、s_2、s_3 s1s2s3。不过本节只讲横向控制,只需要这 4 4 4 个信息就够了。


二、离散轨迹规划误差计算步骤

   下面计算基于离散轨迹规划的误差 e d 、 e ˙ d 、 e φ 、 e ˙ φ e_d、\dot e_d、e_\varphi、\dot e_\varphi ede˙deφe˙φ

1、匹配点

   第一步,找到离散轨迹规划点中与真实位置 ( x , y ) (x,y) (x,y) 最近的点,计算规划点到真实位置之间的距离,比大小找出最短距离,最短距离对应的点就是匹配点。

2、投影点

   第二步,计算投影点。

匹配点和投影点是什么关系呢?

   比如,在直线里有三个规划点:
在这里插入图片描述

   图中蓝色点为匹配点,红色点为真正的投影点。投影点是不在规划轨迹点中的理想点。因此,匹配点并不等于投影点。

匹配点能否近似代替投影点?

   用最短距离的匹配点近似代替投影点,理论上可以,但有个前提,规划点要特别密集,这样匹配点和投影点的位置比较接近,越密就越接近,近似程度就越高。但是理论归理论,现实生活中这种方法依然不可行:

  • 规划点不可能特别密集
  • 规划点取得非常密集会增加计算负担

   在找匹配点时,是计算所有规划点和真实位置 ( x , y ) (x,y) (x,y) 的距离,规划点越密集,计算量就越大。

   在自动驾驶中控制的实时性要求最高,要尽可能不惜代价提升控制代码的运行速度,所以用匹配点近似投影点在实际运用中不可行。

   可以通过匹配点近似计算出投影点。

   假设有一条连续曲线,上面有三个离散规划点:
在这里插入图片描述

   规划都是离散点,没有连续曲线,匹配点距离真实点最近,附带相关信息 ( x m , y m , θ m , κ m ) (x_{m},y_{m},\theta_{m},\kappa_{m}) (xm,ym,θm,κm),真实点也有信息 ( x , y , θ , κ ) (x,y,\theta,\kappa) (x,y,θ,κ),通过真实点和匹配点的信息,把投影点信息近似计算出来。

   假设从匹配点到投影点曲线的曲率不变,即近似认为匹配点的曲率等于投影点的曲率。

   黑色曲线是规划轨迹,这是真实的连续轨迹,根据连续轨迹得到离散的规划点。

   要计算的是真实点到投影点的横向距离 e d e_d ed,已知图中红色向量:
x ⃗ − x ⃗ m = ( x − x m , y − y m ) \vec{x}-\vec{x}_m=(x-x_m,y-y_m) x x m=(xxm,yym)   由匹配点的航向角 θ m \theta_m θm 可知切向向量 τ ⃗ m \vec \tau_m τ m和法向向量 n ⃗ m \vec n_m n m,即:
τ ⃗ m = ( cos ⁡ θ m , sin ⁡ θ m ) n ⃗ m = ( − sin ⁡ θ m , cos ⁡ θ m ) \vec{\tau}_m=\left( \cos \theta _m,\sin \theta _m \right) \quad \vec{n}_m=\left( -\sin \theta _m,\cos \theta _m \right) τ m=(cosθm,sinθm)n m=(sinθm,cosθm)

3、横向误差

   第三步,横向误差 e d e_d ed 近似等于红色向量 在 n ⃗ m \vec n_m n m 方向上的投影:
e d ≈ ( x ⃗ − x ⃗ m ) ⋅ n ⃗ m e_d\approx \left( \vec{x}-\vec{x}_m \right) \cdot \vec{n}_m ed(x x m)n m
   注意 e d e_d ed 有正负,为正意味着真实位置在规划位置的左边,为负意味着在右边。

   若规划轨迹是直线,即曲率 κ = 0 \kappa=0 κ=0 ,这种近似完全没有任何误差, e d = ( x ⃗ − x ⃗ m ) ⋅ n ⃗ m e_d= \left( \vec{x}-\vec{x}_m \right) \cdot \vec{n}_m ed=(x x m)n m,这是非常好的性质,有了此性质后:

  • 在曲线段,横向误差 e d e_d ed 和曲率、弧长有关
  • 在直线段,横向误差 e d e_d ed 和曲率、弧长都没有关系,因为上式严格相等

   意味着在直线段用非常少的轨迹规划点就可以完整地进行轨迹跟踪。

   所以只需在曲线曲率比较大的位置,规划的离散轨迹密一点;在直线段或者曲率特别小的位置完全可以用几个点一笔带过,节省计算资源。

4、纵向误差

   第四步,匹配点与投影点之间的弧长 e s e_s es 等于红色向量在 τ ⃗ m \vec{\tau}_m τ m 上的投影:
e s ≈ ( x ⃗ − x ⃗ m ) ⋅ τ ⃗ m e_s\approx \left( \vec{x}-\vec{x}_m \right) \cdot \vec{\tau}_m es(x x m)τ m
   注意:弧长有正负。

   比如,有一条曲线:
在这里插入图片描述

   这种情况下, τ ⃗ m \vec{\tau}_m τ m ( x ⃗ − x ⃗ m ) (\vec x-\vec x_m) (x x m) 夹角小于 90 ° 90° 90° ,点乘为正,意味着投影点在匹配点前面。

   如果是这种情况:
在这里插入图片描述

   其中, τ ⃗ m \vec{\tau}_m τ m ( x ⃗ − x ⃗ m ) (\vec x-\vec x_m) (x x m)夹角大于 90 ° 90° 90° ,点乘为负,意味着投影点在匹配点后面。

5、投影点的航向角

   第五步,也就是最关键的一步, 投影点的航向角 θ r \theta _r θr
θ r = θ m + κ m e s \theta _r=\theta _m+\kappa _me_s θr=θm+κmes   比如,有一段圆弧:
在这里插入图片描述

   圆弧上有两个点,与曲线切线方向的夹角分别是 θ 1 \theta_1 θ1 θ 2 \theta_2 θ2,根据几何关系,绿色角是 θ 1 − θ 2 \theta_1-\theta_2 θ1θ2,那么
θ 1 − θ 2 = s R = κ s \theta_{1}-\theta_{2}=\frac{s}{R}=\kappa s θ1θ2=Rs=κs    又因为 e s e_s es 近似认为是匹配点与投影点间的弧长,所以
θ r = θ m + κ m e s \theta _r=\theta _m+\kappa _me_s θr=θm+κmes

6、四个式子

   第六步,得到四个转换关系式子:
e d = ( x ⃗ − x ⃗ m ) ⋅ n ⃗ m e s = ( x ⃗ − x ⃗ m ) ⋅ τ m θ r = θ m + k m ⋅ e s κ r = κ m \begin{aligned} e_d&=\left( \vec{x}-\vec{x}_m \right) \cdot \vec{n}_m\\ e_s&=\left( \vec{x}-\vec x_m \right) \cdot \tau _m\\ \theta _r&=\theta _m+k_m\cdot e_s\\ \kappa _r&=\kappa _m\\ \end{aligned} edesθrκr=(x x m)n m=(x x m)τm=θm+kmes=κm
s ˙ = ∣ v ⃗ ∣ cos ⁡ ( θ − θ r ) 1 − κ e d e φ = φ − θ r e ˙ φ = φ ˙ − θ ˙ r = φ − κ r s ˙ e ˙ d = ∣ v ⃗ ∣ sin ⁡ ( θ − θ r ) \begin{aligned} \dot{s}&=\frac{|\vec{v}|\cos \left( \theta -\theta _r \right)}{1-\kappa e_d}\\ e_{\varphi}&=\varphi -\theta _r\\ \dot{e}_{\varphi}&=\dot{\varphi}-\dot{\theta}_r=\varphi -\kappa _r\dot{s}\\ \dot{e}_d&=|\vec{v}|\sin \left( \theta -\theta _r \right)\\ \end{aligned} s˙eφe˙φe˙d=1κedv cos(θθr)=φθr=φ˙θ˙r=φκrs˙=v sin(θθr)   可计算出 e d 、 e ˙ d 、 e φ 、 e ˙ φ e_d、\dot{e}_d、e_{\varphi}、\dot{e}_{\varphi} ede˙deφe˙φ


三、总结

   最终,横向控制
u = − k e r r + δ f u=-ke_{rr}+\delta _f u=kerr+δf其中, k = l q r ( A , B , Q , R ) {{k}=\mathrm{lqr}(A,B,Q,R)} k=lqr(A,B,Q,R) d l q r ( A ˉ , B ˉ , Q , R ) {{\mathrm{dlqr}(\bar{A},\bar{B},Q,R)}} dlqr(Aˉ,Bˉ,Q,R)的解。

关于这些量的计算:

  • A B AB AB 的计算在第三节和第四节
  • k k k 的计算在第五节
  • δ f \delta_f δf 的计算在第六节
  • 误差 e r r e_{rr} err 的计算在本节。

至此,横向控制理论部分结束。

   在下一节会详细演示怎么把程序代码编写出来,将理论变成实际。第八节是横向控制的核心,将用到前七节所有知识。

   本篇博客到此结束,欢迎关注!


后记:

🌟 感谢您耐心阅读这篇关于 离散规划轨迹的误差计算 的技术博客。 📚

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

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

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

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

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

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

相关文章

SOMEIP_ETS_086: Eventgroup_EventsAndFieldsAll_2_TCP

测试目的: 验证设备(DUT)是否能够针对事件组0x02发送SubscribeEventgroupAck,并且发送所有初始字段。 描述 本测试用例旨在检查DUT在接收到针对事件组0x02的SubscribeEventgroup消息后,是否能够正确响应SubscribeEv…

覆盖 7 百万问答数据,上海 AI Lab 发布 ChemLLM,专业能力比肩 GPT-4

随着人工智能技术的飞速发展,大语言模型 (LLMs) 凭借其强大的自然语言处理能力,在生命科学、海洋学、材料化学等科学研究中被广泛应用。虽然,LLMs 在分子特性预测、分子生成和实验方案设计等化学相关任务上表现良好,但在处理各种化…

IDEA莫名奇妙自动选择光标所在行 -罪魁祸首居然是钉钉

请看受害者视角 作为开发者,工作时基本都会运行钉钉吧。最近,钉钉更新了AI功能,但不知道是不是开发团队平时不使用IDE,竟然让这个AI功能影响到了其他软件,简直让人无语。不仅仅是IDEA受影响,就连WebStorm也…

<C++> AVLTree

目录 1. AVL概念 2. AVL树节点的定义 3. AVL树的插入 4. AVL树的旋转 5. AVL树的验证 6. AVL树的删除 7. AVL树的性能 暴力搜索、二分搜索、二叉搜索树、二叉平衡搜索树(AVL、红黑树)、多叉平衡搜索树(B树)、哈希表 1. AVL概念 二…

RAG 实践- Ollama+RagFlow 部署本地知识库

前言 本文我们介绍另一种实现方式:利用 OllamaRagFlow 来实现,其中 Ollama 中使用的模型仍然是Qwen2 我们再来回顾一下 RAG 常见的应用架构 RagFlow的安装和部署 前置条件 CPU > 4 核RAM > 16 GBDisk > 50 GBDocker > 24.0.0 & Dock…

迅为RK3568 开发板赋能数字广告,推动智能化使用北京迅为RK3568核心板作为广告机主控

广告机作为一种高效且灵活的宣传工具,正逐步成为商场、车站、机场等各类公共场所的标配。广告机通过数字化的方式展示广告内容和提供实时信息更新,有效地提升了广告和信息的覆盖率。 如何设计一款广告机,根据广告机的需求和工作环境&#x…

2024年住宅代理市场概况:趋势与选择指南

在数字化时代,网络安全和数据保护越来越受到重视。尤其在网络环境中,真实的IP地址成为保护个人信息安全和数据安全的重要抓手。住宅代理作为一种有效的解决方案,在这个领域扮演了关键角色。本文将探讨2024年住宅代理市场的概况,分…

【CTF Web】BUUCTF Upload-Labs-Linux Pass-03 Writeup(文件上传+PHP+特殊可解析扩展名绕过)

Upload-Labs-Linux 1 点击部署靶机。 简介 upload-labs是一个使用php语言编写的,专门收集渗透测试和CTF中遇到的各种上传漏洞的靶场。旨在帮助大家对上传漏洞有一个全面的了解。目前一共20关,每一关都包含着不同上传方式。 注意 1.每一关没有固定的…

数据库:笔记01绪论

基本概念 数据(Data) 描述事物的符号记录称为数据,并且可以数字化存入计算机。 数据的含义称为数据的语义,数据与其语义是不可分的。 数据库(DataBase,DB) 存放数据的仓库 数据库数据具有永久存储、有…

【大模型测评】2024大语言模型综合能力测评报告(免费下载)

前言 2024年大语言模型综合能力测评报告揭示了全球大模型市场的爆发式增长,特别是在算法架构、数据处理和应用场景上的显著进步。 从2017年的诞生阶段到2023年的爆发期,大模型技术经历了从Transformer神经网络架构的奠基到多模态理解与内容生成能力的飞…

笔试,牛客.kotori和n皇后​,牛客.AOE还是单体

目录 牛客.kotori和n皇后​编辑 牛客.AOE还是单体 牛客.kotori和n皇后 想起来,我之前还写过n皇后的题,但是这个我开始只能想到暴力解法 判断是不是斜对角线,联想yxb和y-xb,假如在一条线上,那么他们的x和y会对应成比例&#xff0c…

iscntrl函数讲解 <ctype.h>头文件函数

目录 1.头文件 2.iscntrl函数 1.头文件 以上这些函数都属于头文件<ctype.h>中的函数,其中包括iscntrl函数 #include<ctype.h> 2.iscntrl函数 简述&#xff1a;iscntrl 函数用来判断字符是否为控制字符&#xff0c;控制字符为ASCII值在0x00到0x7f之间 控制字符主…

在 Facebook 上投放广告需要多少钱?

Facebook 拥有 23.2 亿的月活跃用户&#xff0c;用户体量非常庞大&#xff0c;你的目标群体出现在社交媒体平台上的可能性非常高&#xff0c;所以企业会选择在Facebook 上投放广告。很多朋友想入局&#xff0c;但总是在思考Facebook 推广到底要花多少钱才能有效&#xff1f;如果…

Unknown command: “create-react-app“

在创建react项目时出现报错" Unknown command: "create-react-app" " 解决方法&#xff1a; 配置全局变量&#xff0c;" win r " 打开cmd窗口&#xff0c;输入下列命令&#xff0c;回车等待结束即可&#xff1a; npx create-react-app my-pro…

中国电子学会Python3级等级考试202403客观题解析4

二、判断题 26、十六进制 3C 转换成八进制为 74。&#xff08; &#xff09; A 正确 B 错误 答案&#xff1a;A 使用“填空法”&#xff0c;先将3C转换为十进制&#xff0c;之后再使用“填空法”或“除8法”将十进制数转换为八进制。 知识扩展&#xff1a;其它进制&#…

边缘智能网关 P1600:智慧城市的创新引擎

智慧城市&#xff08;Smart City&#xff09;是以发展更科学、管理更高效、生活更美好为目标&#xff0c;以信息技术和通信技术为支撑&#xff0c;通过透明、充分的信息获取&#xff0c;广泛、安全的信息传递和有效、科学的信息处理&#xff0c;提高城市运行效率&#xff0c;改…

Shell脚本快速入门(Linux篇)

1.Shell介绍 Shell 是一个 C 语言编写的脚本语言&#xff0c;它是用户与 Linux 的桥梁&#xff0c;用户输入命令交给 Shell 处理&#xff0c;Shell 将相应的操作传递给内核&#xff08;Kernel&#xff09;&#xff0c;内核把处理的结果输出给用户。 Shell 是指一种应用程序&am…

用亚马逊AI代码开发助手Amazon Q Developer开发小游戏(中篇)

快用人工智能帮程序员写代码、开发游戏&#xff01;今天小李哥就来介绍亚马逊推出的国际前沿人工智能AI代码开发助手Amazon Q Developer。目前该代码助手在Hugging Face代码生成权威测试集SWE-bench中排名第一&#xff0c;可以根据我们的需求生成整个代码项目&#xff0c;并可以…

(待更)数据结构与算法:字符串和KMP算法原理(附有leetcode344、541、151、459题解)

字符串交换相关 leetcode344.反转字符串 注意&#xff1a;这里的s是str数组 class Solution(object):def reverseString(self, s):""":type s: List[str]:rtype: None Do not return anything, modify s in-place instead."""n len(s)i 0j …

【C++】容器list常用接口详解

目录 一.基本介绍 二.list的使用 1.构造函数 2.迭代器 3.遍历方式 4.容量相关操作 5.增删改查 三.list迭代器失效问题 四.算法库函数和list关系 一.基本介绍 list是一个带头双向循环链表由于是链表&#xff0c;物理空间不连续&#xff0c;不支持随机访问数据&#xff0…