智能驾驶规划控制理论学习08-自动驾驶控制模块(轨迹跟踪)

目录

一、基于几何的轨迹跟踪方法

1、基本思想

2、纯追踪

3、Stanly Method 

二、PID控制器

三、LQR(Linear Quadratic Regulator)

1、基本思想        

2、LQR解法

3、案例学习

基于LQR的路径跟踪 

 基于LQR的速度跟踪

 4、MPC(Model Predicitive Control)

 1、基本思想

2、优化思路

3、MPC vs LQR

4、案例学习

系统模型

生成参考状态 

数学优化

代价函数设计

约束设计


一、基于几何的轨迹跟踪方法

1、基本思想

        基于几何的方法利用车辆和路径之间的几何关系,得到路径跟踪问题的控制律解。基于几何的方法通常利用前视距离来测量车辆前方的误差。该算法的目标是找出每个时刻的前轮角度。

        基于几何的轨迹跟踪方法主要有纯跟踪和斯坦福提出的Stanley Method。在几何方法中使用的依然是在前面规划章节使用的自行车模型。

        如上图所示,R是在给定转向角度下,后桥沿着的圆周半径。这个模型较好地近似了汽车在低速和中等转向角时的运动。 

2、纯追踪

         纯追踪方法的最终目的是计算前轮转角,可以通过当前车辆后轴位置到生成目标路径的前视距离la可以得到一条车辆的运动圆弧曲线和路径上的目标点(g_{x},g_{y}),然后通过几何关系计算得到前轮转角。

        \alpha角是车身行驶方向与前向距离的夹角,根据数学上的正弦定理,进行如下计算:

        使用简单的自行车模型,转向角可以写成:

\delta = tan^{-1}(kL) 

        将曲率代入,可得到纯追踪控制律如下:

\delta (t)=tan^{-1}(\frac{2Lsin(\alpha (t))}{l_{d}}) 

        为了使得控制律更具有直观性,可以引入一个新的变量cross track error e_{ld}(横跨跟踪误差),该误差描述的是车辆行驶方向与目标点之间的横向距离,那么:

sin(\alpha )=\frac{e_{ld}}{l_{d}}

k=\frac{2}{l_{d}^{2}}e_{l_{d}}

         由上式可知,纯追踪方法是一个方向盘角度的比例控制器,它作用在车里前方一些前视距离的横跨追踪误差上,增益为\frac{2}{l_{d}^{2}}

        在实践中,增益(前视距离)被独立地调整为在几个恒定的速度下保持稳定,从而使l_{d}更好地被指定为车速的函数。

3、Stanly Method 

        纯追踪方法以车辆后轴为参考点,而Stanly Method以前轴中心为参考点,同时考虑相对于路径最近点的行驶方向和位置误差。

         如上图所示,\theta角是前轮偏转角,\theta _{p}是在点(c_{x},c_{y})处前轮与路径的夹角,那么前轮要再偏转的角度\theta _{e}为:

\theta _{e}=\theta - \theta _{p}

         Stanly Method使用非线性反馈函数,从前轴中心到最近路径点(c_{x},c_{y})测量横跨跟踪误差e_{fa},公式如下:

二、PID控制器

        PID控制器试图通过调整控制变量u(t)使误差随时间的误差最小,整体的控制函数如下:

u(t)=K_{p}e(t)+K_{i}\int_{0}^{t}e(\tau)d \tau+K_{d}\frac{de(t)}{dt} 

         比例项P_{out}=K_{p}e(t)产生一个与当前误差值e(t)成正比的输出值,K_{p}被称为比例增益常数。上图可以直观反映比例增益的取值对系统的影响,如果比例增益过高,系统就会变的不稳定,而如果比例增益太低,则当系统发生扰动时,控制作用可能太小。

        积分项I_{out}=K_{i}\int_{0}^{t}e(\tau )d\tau是针对比例控制器工作时存在的稳态误差,稳态误差指的是期望的最终输出与实际输出之间的差值。积分项跟踪累积误差,累计误差乘以积分增益K_{i}再添加到控制器输出中。如果积分增益过高,则会导致参考值超调。 

         为了减小控制的波动性(提高阻尼稳定性),又引入微分项D_{out}=K_{d}\frac{de(t)}{dt},通过确定误差随时间的斜率来计算微分误差,计算该误差的变化率乘以微分增益K_{d},并加上控制器输出。微分项根据当前变化率来估计误差的未来趋势。误差变化越大,阻尼效应越大,微分项能够提高系统的稳定时间和稳定性。

三、LQR(Linear Quadratic Regulator)

1、基本思想        

        LQR是一种设计状态变量反馈的方法,它通过最小化一个二次型代价函数来调节线性系统的控制。

        每个时间戳的最优控制都可以用下面的公式实现(状态反馈过程):

u=-Kx

        LQR的整体思路就是给定一个状态量x,通过状态反馈可以得到控制量u,将控制律给到系统作用,得到下一点的状态量然后再重新作为输入量再进行状态反馈,循环迭代直到状态收敛。

         状态反馈中K的求解可以通过公式K=R^{-1}B^{T}P计算得到,其中R和B都是目标函数和线性系统中已知的,而P要通过ARE方程获得:

A^{T}P+PA+Q-PBR^{-1}B^{T}P=0

2、LQR解法

         寻找LQR反馈K的设计过程为:

  • 选择设计目标函数中的参数矩阵Q和R以及线性系统中的参数矩阵A和B;
  • 解关于P的代数方程,由K=R^{-1}B^{T}P取得K的值;
  • 根据反馈u=-Kx取得每个状态量对应的控制量。

3、案例学习

基于LQR的路径跟踪 

        对于传统的自行车模型,可以得到下方的表达式:

  • (x,y,\theta ):后桥处的参考点姿态(状态量);
  • \delta:道路上车轮偏转角(状态量);
  • v:纵向速度(状态量);
  • \gamma:转向角变化速率(控制量);
  • a:纵向加速度(控制量);
  • L:车轴距离(参数)。

         LQR方法要是有标准的线性系统\dot{x}=Ax+Bu,但自行车模型并不是一个线性系统方程,因此需要对自行车模型做一些转化。

         对于横向控制,将状态向量定义为:

x=\begin{bmatrix} x_{1}\\ x_{2}\\ x_{3} \end{bmatrix} = \begin{bmatrix} d\\ \theta ^{*}-\theta \\ \delta \end{bmatrix}

        横向偏移量为:

         对x的每个维度求导,然后代入自行车模型:

         为了获得线性系统方程,需要对上述求导后的结果设置假设:

  • 假设一:\theta ^{*}-\theta要足够小,即车辆航向角与轨迹参考线上的角相差不大;
  • 假设二:偏转角\delta比较小。

        那么根据线性系统方程\dot{x}=Ax+Bu可以写出如下方程:

 基于LQR的速度跟踪

 4、MPC(Model Predicitive Control)

 1、基本思想

        与LQR算法相似,MPC也采用优化的思想进行控制,这种控制技术计算出的控制动作使约束动力系统在有限的、后退的水平的代价函数最小化。

  • Model:给定的动力学系统方程;
  • Prediction:预测未来的状态; 
  • 控制:生成最优策略

2、优化思路

  •  最小化的目标有:相对参考线的误差,消耗的能量……
  • 预测系统状态的系统模型:可以是线性也可以是非线性;
  • 需要满足的约束:控制、状态、线性、非线性……

        在每一个时间步长(采样间隔),通过求解预测视界的开环优化问题来计算控制量。虽然在预测时域内获取了很多时间戳上的最优控制策略,但是在当前时间点只应用计算控制序列的第一个值。然后在下一个时间步长,得到新的系统状态并重新计算。

        虽然我们在当前时间点只应用第一个时间戳的优化策略,但是仍然要计算一个较大预测时域内的控制策略,是采用了类似前面基于几何方法控制中的“前视距离”的思想,能够让系统考虑得更远,以更加平缓的方式接近参考线,即未雨绸缪,决策当下。 

3、MPC vs LQR

  •  MPC和LQR都是最优控制的表达式,但它们的优化代价设置方案不同;
  • LQR优化整个预测时域内,而MPC则更像是优化一个滑动窗口,每次都优化后一个时间窗口;
  • LQR具有较好的全局稳定性,因为它考虑的是整个时间域内的轨迹,而MPC通常具有较好的局部最优性;
  • MPC对线性没有任何假设,它可以处理线性或非线性约束,状态转移方程也可以是非线性的,而这都是LQR的缺点。

4、案例学习

系统模型

 

        为了简化问题,这里我们直接使用自行车模型,和前面类似,不再叙述。

生成参考状态 

        给定规划的轨迹和车辆的当前状态,生成参考状态预测范围内的状态。相比较规划模块较长的预测时域,控制模块对实时性的要求更高,频率更高,相对的预测时域就要小很多。

数学优化

        在优化模块设置目标函数和设置各种约束条件。

代价函数设计

        一般来讲,代价函数由两部分组成,一部分是跟踪轨迹的误差代价(对应左上Tracking Error Cost),另一部分是车辆运动时考虑平顺性的代价(对应右上Smoothness Cost)。

约束设计

 自行车模型和动态约束:

 初始约束:

状态/控制边界约束:

 

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

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

相关文章

day59 线程

创建线程的第二种方式 实现接口Runnable 重写run方法 创建线程的第三种方式 java.util.concurrent下的Callable重写call()方法 java.util.concurrent.FutureTask 创建线程类对象 获取返回值 线程的四种生命周期 线程的优先级1-10 default为5,优先级越高&#xff0c…

基于梯度统计学的渐变型亮缝识别算法

作者:翟天保Steven 版权声明:著作权归作者所有,商业转载请联系作者获得授权,非商业转载请注明出处 一、场景痛点 在图像处理相关的实际工程中,会出现各式各样的现实复杂问题,有的是因为机械设计导致&#x…

【洛谷 P8668】[蓝桥杯 2018 省 B] 螺旋折线 题解(数学+平面几何)

[蓝桥杯 2018 省 B] 螺旋折线 题目描述 如图所示的螺旋折线经过平面上所有整点恰好一次。 对于整点 ( X , Y ) (X, Y) (X,Y),我们定义它到原点的距离 dis ( X , Y ) \text{dis}(X, Y) dis(X,Y) 是从原点到 ( X , Y ) (X, Y) (X,Y) 的螺旋折线段的长度。 例如 …

蓝桥杯练习系统(算法训练)ALGO-981 过河马

资源限制 内存限制:256.0MB C/C时间限制:1.0s Java时间限制:3.0s Python时间限制:5.0s 问题描述 在那个过河卒逃过了马的控制以超级超级多的走法走到了终点之后,这匹马表示它不开心了……   于是&#xff0c…

Java教程:RabbitMq讲解与SpringBoot项目如何对接RabbitMq实现生产者与消费者

在往期文章中,我们讲了如何在Windows与Linux环境下安装RabbitMq服务,并访问Web管理端。 有很多同学其实并不知道RabbitMq是用来干嘛的,它起到一个什么作用,并且如何在常见的SpringBoot项目中集成mq并实现消息收发,本章…

Nginx实现高并发

注:文章是4年前在自己网站上写的,迁移过来了。现在看我之前写的这篇文章,描述得不是特别详细,但描述了Nginx的整体架构思想。如果对Nginx玩得透得或者想了解深入的,可以在网上找找其他的文章。 ......................…

day17_订单(结算,提交订单,支付页,立即购买,我的订单)

文章目录 订单模块1 结算1.1 需求说明1.2 获取用户地址1.2.1 UserAddress1.2.2 UserAddressController1.2.3 UserAddressService1.2.4 UserAddressMapper1.2.5 UserAddressMapper.xml 1.3 获取购物项数据1.3.1 CartController1.3.2 CartService1.3.3 openFeign接口定义 1.4 环境…

NIFI从Oracle11G同步数据到Mysql_亲测可用_解决数据重复_数据跟源表不一致的问题---大数据之Nifi工作笔记0065

首先来看一下整体的流程: 可以看到了用到了上面的这些处理器,然后我们主要看看,这里之前 同步的时候,总是出现重复的数据,奇怪. 比如源表中只有166条数据,但是同步过去以后变成了11万条数据了. ${db.table.name:equals(table1):or(${db.table.name:equals(table2)})} 可以看…

【精选好刊】JCR2区SCI仅17天上线见刊,最后10篇版面!

录用案例 JCR2区地质环境类SCI&EI (进展顺) 【期刊简介】IF:3.0-4.0,JCR2区,中科院3/4区; 【检索情况】SCI&EI双检; 【征稿领域】地球观测、环境监测和管理相关或结合研究均可; 【案例分享】重…

前端面试练习24.3.8

防抖和节流 防抖(Debouncing): 防抖是指在短时间内连续触发同一事件时,只执行最后一次触发的事件处理函数。 在实际应用中,常常用于处理用户输入的搜索框或者滚动事件。例如,当用户连续输入搜索关键词时&am…

业务代码中如何使用装饰器模式?

装饰器模式(Decorator Pattern)介绍 装饰器模式(Decorator Pattern)是一种结构型设计模式,我们可以动态地给一个对象添加额外的职责。而不是通过继承增加子类的方式来扩展对象的功能,装饰器模式使用组合的…

[N1CTF 2018]eating_cms 不会编程的崽

题倒是不难,但是实在是恶心到了。 上来就是登录框,页面源代码也没什么特别的。寻思抓包看一下,数据包直接返回了sql查询语句。到以为是sql注入的题目,直到我看到了单引号被转义。。。挺抽象,似乎sql语句过滤很严格。又…

Java基础知识点之思维导图

一、走进Java编程世界 二、变量常量和运算符 三、if选择结构 四、switch选择结构 五、while循环结构 六、for循环结构 七、数组 八、类与对象 九、深入循环结构 十、类的无参方法 十一、类的带参方法 十二、字符串

读已提交隔离级别下竟然有间隙锁

业务背景 广告主痛点的为进行一次全媒体联合投放,若投放10个媒体,需要制作和上传10个创意、50张不同尺寸和出血区要求的图片和视频素材、近100个元素,投放成本极高。这也是制约部分用户使用新产品投放的原因。 因此进行升级。以三个创意为例…

Prometheus添加nginx节点显示不支持stub_status 解决办法

1、我们在使用Prometheus监控添加nginx节点监控的时候,在被监控节点的nginx配置文件中添加下面的模块 server { listen 80; server_name localhost; location /stub_status { stub_status on; access_log off; …

微服务技术栈SpringCloud+RabbitMQ+Docker+Redis+搜索+分布式(四):消息队列MQ

文章目录 一、消息队列MQ二、RabbitMQ2.1 单机部署2.2 消息模型 三、SpringAMAP3.1 简单消息队列3.2 工作消息队列3.3 发布-订阅模型:FanoutExchange 广播交换机3.4 发布-订阅模型:DirectExchange 路由交换机3.5 发布-订阅模型:TopicExchange…

Day29:安全开发-JS应用DOM树加密编码库断点调试逆向分析元素属性操作

目录 JS原生开发-DOM树-用户交互 JS导入库开发-编码加密-逆向调试 思维导图 JS知识点: 功能:登录验证,文件操作,SQL操作,云应用接入,框架开发,打包器使用等 技术:原生开发&#x…

推房子游戏c++

这段代码是一个推箱子游戏的实现。游戏中有一个地图,地图上有墙壁、人、箱子和目标位置。玩家通过键盘输入WASD或方向键来控制人物的移动,目标是将所有的箱子推到相应的目标位置上。 代码中的dt数组表示地图,每个位置上的字符表示对应的元素…

c语言在线聊天室

c语言基于tcp和多线程的在线聊天室(c语言通讯系统)功能需求 1.实现多线程 2.构建socke套接字实现一对一通信 3.实现多个电脑的通信 4.数据传输加密和解密 5.多人实时聊天 6.具备群聊和私聊的功能 实现原理: 服务端公网Ip暴露,客户端端口随机分配,通过服务端公网IP连接,服务端…

【开源】SpringBoot框架开发免税店商城管理系统

目录 一、摘要1.1 项目介绍1.2 项目录屏 二、系统设计2.1 功能模块设计2.2 研究方法 三、系统展示四、核心代码4.1 查询免税种类4.2 查询物品档案4.3 新增顾客4.4 新增消费记录4.5 审核免税 五、免责说明 一、摘要 1.1 项目介绍 基于JAVAVueSpringBootMySQL的免税店商城管理系…