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

2024-09-05 06:12

本文主要是介绍【自动驾驶】控制算法(七)离散规划轨迹的误差计算,希望对大家解决编程问题提供一定的参考价值,需要的开发者们随着小编来一起学习吧!

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

相关文章

动态规划---打家劫舍

题目: 你是一个专业的小偷,计划偷窃沿街的房屋。每间房内都藏有一定的现金,影响你偷窃的唯一制约因素就是相邻的房屋装有相互连通的防盗系统,如果两间相邻的房屋在同一晚上被小偷闯入,系统会自动报警。 给定一个代表每个房屋存放金额的非负整数数组,计算你 不触动警报装置的情况下 ,一夜之内能够偷窃到的最高金额。 思路: 动态规划五部曲: 1.确定dp数组及含义 dp数组是一维数组,dp[i]代表

软考系统规划与管理师考试证书含金量高吗?

2024年软考系统规划与管理师考试报名时间节点: 报名时间:2024年上半年软考将于3月中旬陆续开始报名 考试时间:上半年5月25日到28日,下半年11月9日到12日 分数线:所有科目成绩均须达到45分以上(包括45分)方可通过考试 成绩查询:可在“中国计算机技术职业资格网”上查询软考成绩 出成绩时间:预计在11月左右 证书领取时间:一般在考试成绩公布后3~4个月,各地领取时间有所不同

poj 1113 凸包+简单几何计算

题意: 给N个平面上的点,现在要在离点外L米处建城墙,使得城墙把所有点都包含进去且城墙的长度最短。 解析: 韬哥出的某次训练赛上A出的第一道计算几何,算是大水题吧。 用convexhull算法把凸包求出来,然后加加减减就A了。 计算见下图: 好久没玩画图了啊好开心。 代码: #include <iostream>#include <cstdio>#inclu

uva 1342 欧拉定理(计算几何模板)

题意: 给几个点,把这几个点用直线连起来,求这些直线把平面分成了几个。 解析: 欧拉定理: 顶点数 + 面数 - 边数= 2。 代码: #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#include <cstring>#include <cmath>#inc

uva 11178 计算集合模板题

题意: 求三角形行三个角三等分点射线交出的内三角形坐标。 代码: #include <iostream>#include <cstdio>#include <cstdlib>#include <algorithm>#include <cstring>#include <cmath>#include <stack>#include <vector>#include <

XTU 1237 计算几何

题面: Magic Triangle Problem Description: Huangriq is a respectful acmer in ACM team of XTU because he brought the best place in regional contest in history of XTU. Huangriq works in a big compa

poj 2976 分数规划二分贪心(部分对总体的贡献度) poj 3111

poj 2976: 题意: 在n场考试中,每场考试共有b题,答对的题目有a题。 允许去掉k场考试,求能达到的最高正确率是多少。 解析: 假设已知准确率为x,则每场考试对于准确率的贡献值为: a - b * x,将贡献值大的排序排在前面舍弃掉后k个。 然后二分x就行了。 代码: #include <iostream>#include <cstdio>#incl

代码随想录冲冲冲 Day39 动态规划Part7

198. 打家劫舍 dp数组的意义是在第i位的时候偷的最大钱数是多少 如果nums的size为0 总价值当然就是0 如果nums的size为1 总价值是nums[0] 遍历顺序就是从小到大遍历 之后是递推公式 对于dp[i]的最大价值来说有两种可能 1.偷第i个 那么最大价值就是dp[i-2]+nums[i] 2.不偷第i个 那么价值就是dp[i-1] 之后取这两个的最大值就是d

基于51单片机的自动转向修复系统的设计与实现

文章目录 前言资料获取设计介绍功能介绍设计清单具体实现截图参考文献设计获取 前言 💗博主介绍:✌全网粉丝10W+,CSDN特邀作者、博客专家、CSDN新星计划导师,一名热衷于单片机技术探索与分享的博主、专注于 精通51/STM32/MSP430/AVR等单片机设计 主要对象是咱们电子相关专业的大学生,希望您们都共创辉煌!✌💗 👇🏻 精彩专栏 推荐订阅👇🏻 单片机

Python3 BeautifulSoup爬虫 POJ自动提交

POJ 提交代码采用Base64加密方式 import http.cookiejarimport loggingimport urllib.parseimport urllib.requestimport base64from bs4 import BeautifulSoupfrom submitcode import SubmitCodeclass SubmitPoj():de