【自动驾驶】PID实现轨迹跟踪
文章目录
- 参考资料
- 1. PID控制原理
- 1.1 基本概念
- 1.2 数字 PID 控制算法
- 1. 位置式PID
- python代码实现
- 2. 增量式PID
- python代码实现
- 2. 车辆横向跟踪误差
- 3. PID实现轨迹跟踪
- 后记
参考资料
- 轨迹跟踪PID控制
- PID控制概述
- PID控制器开发笔记
1. PID控制原理
1.1 基本概念
PID( Proportional Integral Derivative)是工业应用最为广泛 的控制器。学习过控制理论的同学对它一定不陌生(毕竟调参这事可以记一辈子呢~~)。
PID控制器(比例-积分-微分控制器),由比例单元(Proportional)、积分单元(Integral)和微分单元(Derivative)组成。可以透过调整这三个单元的增益KpK_{p}Kp,KiK_{i}Ki和KdK_{d}Kd来调定其特性。
PID算法可以用下式表示:
u(t)=Kpe(t)+Ki∫0te(τ)dτ+Kdddte(t)=Kp[e(t)+1Ti∫0te(τ)dτ+Tdddte(t)](1)\tag{1} \begin{matrix} \mathrm{u}(t)&=K_{p} e(t)+K_{i} \int_{0}^{t} e(\tau) d \tau+K_{d} \frac{d}{d t} e(t)\\ &=K_{p} \left[e(t)+\frac{1}{T_i}\int_{0}^{t} e(\tau) d \tau+T_{d} \frac{d}{d t} e(t)\right] \end{matrix} u(t)=Kpe(t)+Ki∫0te(τ)dτ+Kddtde(t)=Kp[e(t)+Ti1∫0te(τ)dτ+Tddtde(t)](1)
其中
- KpK_{p}Kp : 比例增益,是调适参数
- KiK_{i}Ki : 积分增益,也是调适参数
- KdK_{d}Kd : 微分增益,也是调适参数
- TiT_iTi:积分时间常数
- TdT_dTd:微分时间常数
- eee : 误差=设定值 (SP)−(\mathrm{SP})-(SP)− 当前值( PV)\mathrm{PV})PV)
- ttt : 当前时间
- τ\tauτ : 积分变数,数值从 0 到目前时间 ttt
当被控对象的结构和参数不能完全掌握,或得不到精确的数学模型时,控制理论的其它技术难以采用时,系统控制器的结构和参数必须依靠经验和现场调试来确定,这时应用PID控制技术最为方便。即当我们不完全了解一个系统和被控对象,或不能通过有效的测量手段来获得系统参数时,最适合用PID控制技术。
任何闭环控制系统的首要任务是要稳(稳定)、准(准确)、快(快速)的响应命令。PID的主要工作就是如何实现这一任务。
PID控制器的比例单元 ( P) 、积分单元(I)和微分单元(D)分别对应目前误差、过去累计误差及未来误差。若是不知道受控系统的特性,一般认为PID控制器是最适用的控制器.
增大比例环节将加快系统的响应,它的作用于输出值较快,但不能很好稳定在一个理想的数值,不良的结果是虽较能有效的克服扰动的影响,但有余差出现,过大的比例系数会使系统有比较大的超调,并产生振荡,使稳定性变坏。
积分环节能在比例的基础上消除余差,它能对稳定后有累积误差的系统进行误差修整,减小稳态误差。
微分环节具有超前作用,对于具有容量滞后的控制通道,引入微分参与控制,在微分项设置得当的情况下,对于提高系统的动态性能指标,有着显著效果,它可以使系统超调量减小,减小震荡,稳定性增加,动态误差减小。
在调整的时候,我们要做的任务就是在系统结构允许的情况下,在这三个参数之间权衡调整,达到最佳控制效果,实现稳、准、快的控制特点。
在实际应用中,主要有以下不足:
1. 在实际工业生产过程往往具有非线性、时变不确定,难以建立精确的数学模型,常规的PID控制器不能达到理想的控制效果;
2. 在实际生产现场中,由于受到参数整定方法烦杂的困扰,常规PID控制器参数往往整定不良、效果欠佳,对运行工况的适应能力很差。
1.2 数字 PID 控制算法
在实际计算机实现时,由于计算机控制是一种采样控制, 它只能根据采样时刻的偏差计算控制量,而不能像模拟控制那样连续输出控制量, 进行连续控制,所以实现的是离散形式的PID,即数字PID控制算法
1. 位置式PID
位置式PID是当前系统的实际位置,与你想要达到的预期位置的偏差,进行PID控制
设采样时间为 TTT ,公式(1)中的积分项可以用下式近似
∫0te(τ)dτ=T∑j=0kej\int_{0}^{t} e(\tau) d \tau=T\sum_{j=0}^{k} e_j ∫0te(τ)dτ=Tj=0∑kej
微分项可近似为(后向差分法)
de(t)dt=ek−ek−1T\frac{d e\left(t\right)}{d t}=\frac{e_k-e_{k-1}}{T} dtde(t)=Tek−ek−1
故公式(1)化为
uk=Kp{ek+TTi∑j=0kej+TdT[ek−ek−1]}=Kpek+Ki∑j=0kej+Kd[ek−ek−1](2)\tag{2} \begin{matrix} u_k&=K_{p}\left\{e_k+\frac{T}{T_i} \sum_{j=0}^{k} e_j+\frac{T_{d}}{T}[e_k-e_{k-1}]\right\}\\ &=K_{p} e_k+K_{i} \sum_{j=0}^{k} e_j+K_{d}[e_k-e_{k-1}] \end{matrix} uk=Kp{ek+TiT∑j=0kej+TTd[ek−ek−1]}=Kpek+Ki∑j=0kej+Kd[ek−ek−1](2)
当采样时间足够小时,能够获得最够精确的结果,离散控制过程与连续过程非常接近。
位置式PID在积分项达到饱和时,误差仍然会在积分作用下继续累积,一旦误差开始反向变化,系统需要一定时间从饱和区退出,所以在u(k)u(k)u(k)达到最大和最小时,要停止积分作用,并且要有积分限幅和输出限幅。
一般采用抗积分饱和算法,其思路就是:如果上一次的输出控制量超过了饱和值,饱和值为正,则这一次只积分负的偏差,饱和值为负,则这一次只积分正的偏差,从而避免系统长期留在饱和区!
python代码实现
class PID_posi:"""位置式实现"""def __init__(self, k=[1., 0., 0.], target, upper, lower):self.kp, self.ki, self.kd = kself.e = 0 # errorself.pre_e = 0 # previous errorself.sum_e = 0 # sum of errorself.target = target # targetself.upper_bound = upper # upper bound of outputself.lower_bound = lower # lower bound of outputdef set_target(self, target):self.target = targetdef set_k(self, k):self.kp, self.ki, self.kd = kdef set_bound(self, upper, lower):self.upper_bound = upperself.lower_bound = lowerdef cal_output(self, obs): # calculate outputself.e = self.target - obsu = self.e * self.kp + self.sum_e * \self.ki + (self.e - self.pre_e) * self.kdif u < self.lower_bound:u = self.lower_boundelif u > self.upper_bound:u = self.upper_boundself.pre_e = self.eself.sum_e += self.e# print(self.sum_e)return udef reset(self):# self.kp = 0# self.ki = 0# self.kd = 0self.e = 0self.pre_e = 0self.sum_e = 0# self.target = 0def set_sum_e(self, sum_e):self.sum_e = sum_e
2. 增量式PID
增量式 PID 是指数字控制器的输出只是控制量的增量∆uk∆u_k∆uk 。 当执行机构需要的控制量是增量,而不是位置量的绝对数值时,可以使用增量式 PID 控制算法进行控制。
增量式 PID 控制算法公式为:
Δuk=uk−uk−1=Kp(ek−ek−1)+Kiek+Kd(ek−2ek−1+ek−2)=Kp(ek−ek−1+TTiek+Tdek−2ek−1+ek−2T)=Kp(1+TTi+TdT)ek−Kp(1+2TdT)ek−1+KpTdTek−2)=Aek+Bek−1+Cek−2其中 A=Kp(1+TTi+TdT)B=Kp(1+2TdT)C=KpTdT(3)\tag{3} \begin{aligned} \Delta u_{k}&=u_{k}-u_{k-1}\\ &=K_p\left(e_k-e_{k-1}\right)+K_ie_k+K_d\left(e_k-2e_{k-1}+e_{k-2}\right)\\ &=K_p\left(e_{k}-e_{k-1}+\frac{T}{T_i} e_{k}+T_d \frac{e_{k}-2 e_{k-1}+e_{k-2}}{T}\right) \\ &=\left.K_p\left(1+\frac{T}{T_i}+\frac{T_d}{T}\right) e_{k}-K_p\left(1+\frac{2 T_d}{T}\right) e_{k-1}+K_p \frac{T_d}{T} e_{k-2}\right) \\ &= A e_{k}+B e_{k-1}+C e_{k-2} \\ \\ \text { 其中 } \quad \mathrm{A} &=K_p\left(1+\frac{T}{T_i}+\frac{T_d}{T}\right) \\ \mathrm{B}&=K_p\left(1+\frac{2 T_d}{T}\right) \\ \mathrm{C}&=K_p \frac{T_d}{T} \end{aligned} Δuk 其中 ABC=uk−uk−1=Kp(ek−ek−1)+Kiek+Kd(ek−2ek−1+ek−2)=Kp(ek−ek−1+TiTek+TdTek−2ek−1+ek−2)=Kp(1+TiT+TTd)ek−Kp(1+T2Td)ek−1+KpTTdek−2)=Aek+Bek−1+Cek−2=Kp(1+TiT+TTd)=Kp(1+T2Td)=KpTTd(3)
增量式PID根据公式可以很好地看出,一旦确定了 Kp、Ki、KdK_p、K_i 、K_dKp、Ki、Kd,只要使用前后三次测量值的偏差, 即可由公式求出控制增量;
得出的控制量Δuk\Delta u_kΔuk对应的是近几次位置误差的增量,而不是对应与实际位置的偏差 ,因此没有误差累积。
python代码实现
# 增量式
class PID_inc:"""增量式实现"""def __init__(self, k, target, upper=1., lower=-1.):self.kp, self.ki, self.kd = k self.err = 0self.err_last = 0self.err_ll = 0self.target = targetself.upper = upperself.lower = lowerself.value = 0self.inc = 0def cal_output(self, state):self.err = self.target - stateself.inc = self.kp * (self.err - self.err_last) + self.ki * self.err + self.kd * (self.err - 2 * self.err_last + self.err_ll)self._update()return self.valuedef _update(self):self.err_ll = self.err_lastself.err_last = self.errself.value = self.value + self.incif self.value > self.upper:self.value = self.upperelif self.value < self.lower:self.value = self.lowerdef set_target(self, target):self.target = targetdef set_k(self, k):self.kp, self.ki, self.kd = kdef set_bound(self, upper, lower):self.upper_bound = upperself.lower_bound = lower
2. 车辆横向跟踪误差
对于几何路径跟踪一般简化车辆模型为单车模型,如下图所示:
横向跟踪误差(cross track error, 简称CTE)为车辆中心点(rx,ry)(r_x, r_y)(rx,ry)到最近路径点 (px,py)(p_x, p_y)(px,py)的距离,具体如下图所示。
图中——
- θe=α−ψ\theta_e=\alpha-\psiθe=α−ψ
- ψ\psiψ:航向角;
- eye_yey:横向跟踪误差
- ldl_dld:前视距离——后轴中心离当前路点的距离
根据上图,横向跟踪误差(CTE)计算公式如下:
ey=ldsinθe(4)\tag{4} e_y=l_d\sin{\theta_e} ey=ldsinθe(4)
3. PID实现轨迹跟踪
设车辆运动学模型为以后轴中心为车辆中心的单车运动学模型(具体可参考笔者之前的博客),即
{x˙=Vcos(ψ)y˙=Vsin(ψ)ψ˙=VLtanδfV˙=a\left\{\begin{array}{l} \dot{x}=V \cos (\psi) \\ \dot{y}=V \sin (\psi) \\ \dot{\psi}=\frac{V}{L}\tan{\delta_f}\\ \dot{V}=a \end{array}\right. ⎩⎨⎧x˙=Vcos(ψ)y˙=Vsin(ψ)ψ˙=LVtanδfV˙=a
python实现代码如下。
import math
class KinematicModel_3:"""假设控制量为转向角delta_f和加速度a"""def __init__(self, x, y, psi, v, L, dt):self.x = xself.y = yself.psi = psiself.v = vself.L = L# 实现是离散的模型self.dt = dtdef update_state(self, a, delta_f):self.x = self.x+self.v*math.cos(self.psi)*self.dtself.y = self.y+self.v*math.sin(self.psi)*self.dtself.psi = self.psi+self.v/self.L*math.tan(delta_f)*self.dtself.v = self.v+a*self.dtdef get_state(self):return self.x, self.y, self.psi, self.v
位置式和增量式PID的实现代码前文已给出。
下一步,还需要计算参考轨迹上离车辆当前位置最近的航路点:
def cal_target_index(robot_state,refer_path):"""得到临近的路点Args:robot_state (_type_): 当前车辆位置refer_path (_type_): 参考轨迹(数组)Returns:_type_: 最近的路点的索引"""dists = []for xy in refer_path:dis = np.linalg.norm(robot_state-xy)dists.append(dis)min_index = np.argmin(dists)return min_index
这部分可以使用KDTree来搜索搜索最临近的航路点,KDTree的使用可以参考资料
主函数入口(部分参考自资料1):
from scipy.spatial import KDTree
from celluloid import Camera # 保存动图时用,pip install celluloid# set reference trajectory
refer_path = np.zeros((1000, 2))
refer_path[:,0] = np.linspace(0, 100, 1000) # 直线
refer_path[:,1] = 2*np.sin(refer_path[:,0]/3.0)#+2.5*np.cos(refer_path[:,0]/2.0) # 生成正弦轨迹
refer_tree = KDTree(refer_path) # reference trajectory# 假设初始状态为x=0,y=-1,偏航角=0.5rad,前后轴距离2m,速度为2m/s,时间步为0.1秒
ugv = KinematicModel_3(0,-1,0.5,2,2,0.1)
k=0.1
c=2
x_ = []
y_ = []
fig = plt.figure(1)
# 保存动图用
camera = Camera(fig)for i in range(550):robot_state = np.zeros(2)robot_state[0] = ugv.xrobot_state[1] = ugv.ydistance, ind = refer_tree.query(robot_state) # 在参考轨迹上查询离robot_state最近的点# ind = cal_target_index(robot_state,refer_path) # 使用简单的一个函数实现查询离robot_state最近的点,耗时比较长alpha = math.atan2(refer_path[ind, 1]-robot_state[1], refer_path[ind, 0]-robot_state[0])l_d = np.linalg.norm(refer_path[ind]-robot_state)# l_d = k*ugv.v+c # 前视距离theta_e = alpha-ugv.psie_y = -l_d*math.sin(theta_e) # 与博客中公式相比多了个负号,我目前还不是太理解,暂时先放着# e_y = -l_d*np.sign(math.sin(theta_e)) # 第二种误差表示# e_y = robot_state[1]-refer_path[ind, 1] #第三种误差表示# PID.set_target(0)# print(refer_path[i,1])delta_f = PID.cal_output(e_y)# print(e_y)# print(alpha)ugv.update_state(0,delta_f) # 加速度设为0x_.append(ugv.x)y_.append(ugv.y)# 显示动图plt.cla()plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)plt.plot(x_, y_, "-r", label="trajectory")plt.plot(refer_path[ind, 0], refer_path[ind, 1], "go", label="target")# plt.axis("equal")plt.grid(True)plt.pause(0.001)
# camera.snap()
# animation = camera.animate()
# animation.save('trajectory.gif')plt.figure(2)
plt.plot(refer_path[:, 0], refer_path[:, 1], '-.b', linewidth=1.0)
plt.plot(x_,y_,'r')
plt.show()
使用PID得到的跟踪效果大致如下图所示
完整的Python代码可以参考github仓库。
后记
位置式PID的公式有的地方写为
uk=Kpek+KiT∑j=0kej+KdT[ek−ek−1u_k=K_{p} e_k+K_{i}T \sum_{j=0}^{k} e_j+\frac{K_{d}}{T}[e_k-e_{k-1}uk=Kpek+KiTj=0∑kej+TKd[ek−ek−1
公式2的写法是把采样时间也并入到积分和微分系数里了。
【自动驾驶】PID实现轨迹跟踪相关推荐
- MPCC(Model Predictive Contouring Control)—自动驾驶中的轨迹控制算法
MPCC(Model Predictive Contouring Control)-自动驾驶中的轨迹控制算法 引言 在自动驾驶技术快速发展的背景下,轨迹控制算法成为实现高性能自动驾驶的关键技术之一.而 ...
- 自动驾驶中的轨迹预测数据集汇总!
作者 | 冯偲 编辑 | 汽车人 原文链接:https://zhuanlan.zhihu.com/p/555618753 点击下方卡片,关注"自动驾驶之心"公众号 ADAS巨卷 ...
- 自动驾驶-PID控制器
在自动驾驶中控制车辆是指使用方向盘,刹车,油门将车开到目的地.控制车辆的算法一般称为控制器,PID控制器是最常见,最基础的控制器之一,PID控制器是Proportion Integration Dif ...
- MATLAB自动驾驶工具箱——卡尔曼运动跟踪示例解读
引言 自动驾驶感知中,需要对物体(主要是车辆)进行实时跟踪与预测,是车辆进行决策的前提条件之一.MATLAB自动驾驶工具箱rackingKF类及相关函数提供了线性卡尔曼滤波器可以在位置.速度和加速度上 ...
- 自动驾驶:蛇形曲线跟踪(Stanley Model)
检测对生成出的期望轨迹的跟踪效果,期望轨迹为连续变道的蛇形轨迹: clc clear allload('D:\ccs\DaischTest\CurveCurve.mat') % load('D:\cc ...
- 自动驾驶之行人轨迹预测数据集
一. Real Data ETH: Univ. + Hotel; 750 pedestrians exhibiting complex interactions UCY: Zara01, Zara02 ...
- 清华大学车辆学院李升波|强化学习,让自动驾驶汽车自我进化,越开越好
2022年北京智源大会自动驾驶论坛,清华大学车辆与运载学院李升波教授分享了题为<混合型强化学习及其高级别自动驾驶应用>的主题报告.该报告主要探讨了如何将强化学习应用于自动驾驶汽车的问题,目 ...
- 【自动驾驶轨迹预测】一文熟悉自动驾驶轨迹预测发展现状!
来源 | 自动驾驶之心 来源 | https://zhuanlan.zhihu.com/p/365881810 知圈 | 进"滑板底盘群"请加微yanzhi-6,备注底盘 目录 何 ...
- 无人驾驶车辆路径规划及轨迹跟踪控制学习笔记(2)
目录 汇总 学习笔记 汇总 在关键交通场景中,轨迹规划和轨迹跟踪控制是自动驾驶车辆避免碰撞的两个关键.它不仅需要系统功能,而且需要强大的实时性. 我们集成了自动驾驶汽车的轨迹规划器和跟踪控制器,通过轨 ...
- 自动驾驶汽车的规划与控制
1. 概念与意义 自动驾驶汽车作为一个复杂的软硬件结合系统,其安全,可靠地运行需要车载硬件,传感器集成.感知.预测以及规划控制等多个模块的协同配合工作.感知预测和规划控制的紧密配合非常重要.这里的规划 ...
最新文章
- django如何调用php接口,使用django集成第三方api开发接口注意事项
- LeetCode18.四数之和 JavaScript
- Apache配置虚拟主机三大问题--自己的相关坑
- 使用Angular CLI创建一个Hello World级别的Angular应用
- Android自定义控件学习(一)-----属性
- 【OpenCV教程】第一章 Mac上的安装和初步使用
- 2021年中国商业地产市场现状分析,销售面积下降,依托核心城市构建差异化是关键「图」
- matlab函数表达式里分号_matlab中分号
- 品牌建设—长效且动态的关系管理进程:赫联电子亚太区
- git教程 git笔记 git常用 git使用 git操作 git简明 git版本控制 git仓库
- pos共识机制_共识机制:权益证明机制(POS)
- 关键点检测——无监督
- 云计算是什么?它有哪些形式?
- 【太虚AR_v0.1】使用教程 | 图像识别(多目标)
- 框架、架构和设计模式?!
- 零基础学Java语言---编程题
- 2020年4月西安葡萄城挂经
- 计算机英语的save,save是什么意思_save在线翻译_英语_读音_用法_例句_海词词典
- Permutation test(排列(组合)检验)
- 《深入浅出SQL》问答录
热门文章
- 中国城市群产业建设风险与投资发展决策建议报告2022版
- java 中国地图_Echarts+Java生成百度统计中国地图详细教程-Fun言
- python绘制中国_用Python画中国地图(下)
- 创建第一个Android app项目
- 基于YOLO3的人数统计程序
- 【牛客网刷题】(第二弹)中等难度题型来了.这些题你都会做吗?
- 信息系统项目管理师(高项)考试的论文怎么写,怎么背?经验分享
- pdf文件的预览——几种方式实现——技能提升
- 统一认证 ldap mysql_ZABBIX 对接 LDAP实现用户登陆统一认证
- 用Python爬了微信好友,原来他们是这样的人...