无人驾驶汽车系统入门(四)——反馈控制入门,PID控制
来源:互联网 发布:江东是哪里 知乎 编辑:程序博客网 时间:2024/06/05 22:52
前面几篇博客介绍了卡尔曼滤波的一些基本算法,其实目标追踪,定位,传感器融合还有很多问题要处理,这些我们在以后的系列博客中在进一步细讲,现在我想给大家介绍一下无人驾驶汽车系统开发中需要的控制相关的理论和技术,还是和第一篇说的那样,我想到哪就写到哪,追踪和定位等更高级的算法我在后面会继续写。所以感兴趣的同学可以关注我的博客,无人驾驶汽车系统入门系列博客会一直更新下去。
这一篇主要讲控制的入门,为什么需要控制理论,以及最经典的PID控制,更高级的应用在实际系统中的控制算法我会在后面的文章中详述。编写不易,转载请注明出处:http://blog.csdn.net/adamshan/article/details/78458325
为什么需要控制理论
试想有如下场景,当你驾驶一辆汽车通过这个弯道的时候,假设你已经知道你要开的路线,那么你会怎么去操作控制你的车呢?
显然,如果你不是专业的选手的话,你无法做到一步到位的控制,你需要一边观察车辆相对于你想要开的路线的相对偏差,一边调整你的方向盘的角度和油门踏板的力度,这种基于环境反馈的控制我们称为 反馈控制 。反馈控制是现代控制理论的基础,这是反馈控制的一般思路:
我们希望我们控制的对象(无人车)能够按照我们希望(规划好)的路径行驶,我们会将环境当前给我们的反馈(我们当前的位置)和参考线进行比较,得到我们当前偏离参考线的距离(误差),基于这个误差,我们设计一定的算法来产生输出信号,使得这个误差不断的变小,这样的过程就是反馈控制的一般过程。那么我们如何基于这个误差来产生控制指令呢?我们最直观的感觉就是要让误差在我们的控制下逐渐变小直到为0:
0误差就意味着车一直在你想让它开的路径上开。如何减少误差就是我们这几篇博客要向大家介绍的内容。
为了了解反馈控制,我先向大家介绍 PID控制,PID控制是目前利用最为广泛的控制理论,我们以它为出发点讨论控制理论。
比例,积分和导数
PID就是指 比例(proportion)、积分(integral)、导数(derivative),这三项表示我们如何使用我们的误差来产生控制指令,整个流程如下:
首先是根据反馈和参考值求出误差,这里的误差根据具体的情况可以是各种度量,比如说控制车辆按照指定的路径形式,那么就是车辆当前位置和参考线的距离,控制车辆的速度在设定的值,那么就是当前速度和设定速度的差值,求出误差以后,再根据误差求比例,积分和微分三项,其中
P控制
考虑一个简单的情况,假设我们希望无人车按照图中绿线行驶,但是我们的车在如图所示的位置:
那么我们要转多少度角呢?如果都按照固定的角度转(如下图),那么车的轨迹将如图中所示:
那么显然坐这样的车是不舒服的。一个直观的解决方法就是使用比例控制。如图所示,当偏差大的时候,我们偏转更多的角度,当偏差小的时候,则偏转小一点。
那么这就是P control(比例控制)这里我们使用 CTE(Cross Track Error) 作为偏差度量 ,CTE就是我们到参考线的距离。那么这个时候转角就变成了:
其中的
所以说,如果
此时车辆虽然在参考线上,但是并不是我们希望的状态(它在下一刻就会偏离),但是对于P控制而言,这是理想状态,此时控制转角为0,因此,P控制会一次又一次的超过参考线(overshot),为了矫正这种overshot,我们需要考虑一个额外的误差项——CTE变化率。
PD控制
CTE的变化率描述了我们的无人车向着参考线方向移动的有多快,如果我们的无人车一直都完美的在参考线上运动的话,那么我们的CTE变化率就为0。那么这一项(描述误差的变化率)就可以用导数来表示,那么,现在我们的控制输出就变成了比例项和导数项求和的形式:
其中的
PD控制似乎已经能够胜任良好的反馈控制了,但其实还不够,PD控制器可以保证正常的控制的需求,但是当环境存在扰动的时候,比如说下面这种情况:
车在受力发生轻微偏移以后,由于PD控制中下
PID控制
我们将积分项也就如到我们的控制输出函数中,这个时候,无人车的转角就可以表示为:
其中
同样的,这里的积分项系数的大小也会影响我们整个控制系统的稳定性,过大的
PID控制就是由这三项共同决定的,还有其他应用于无人驾驶汽车的高级控制算法,但是他们都和我们介绍的PID控制的原理相似。
我们发现其实PID实现确实不难,但是三个系数的选择却很难,那么如何选择PID系数呢?我们可以在我们的控制循环中通过一定的算法不断尝试,下面我提供给大家一种寻找参数的算法:
具体的算法见我的C++代码实例。
PID C++代码
pid.cpp
#include <limits>#include <iostream>#include "PID.h"//using namespace std;PID::PID() {}PID::~PID() {}void PID::Init(double Kp, double Ki, double Kd) { parameter.push_back(Kp); parameter.push_back(Ki); parameter.push_back(Kd); this->p_error = 99999999.; this->d_error = 0.0; this->i_error = 0.0; //twiddle parameters need_twiddle = false; step = 1; // let the car run at first 100 steps, then in the next 3000 steps add the cte^2 to the total_error val_step = 100; test_step = 2000; for (int i = 0; i < 3; ++i) { // init the change rate with the value of 0.1*parameter changes.push_back(0.1 * parameter[i]); } index_param = 0; best_error = std::numeric_limits<double>::max(); total_error = 0; // fail to make the total_error better times fail_counter = 0;}void PID::UpdateError(double cte) { if(step == 1){ p_error = cte; } d_error = cte - p_error; p_error = cte; i_error += cte; if(need_twiddle){ if(step % (val_step + test_step) > val_step){ total_error += (cte * cte); } if(step % (val_step + test_step) == 0){ std::cout<<"============== step "<<step<<" =============="<<std::endl; std::cout << "P: "<< parameter[0]<<" I: "<<parameter[1]<<" D: "<<parameter[2]<<std::endl; if (step == (val_step + test_step)){ if(total_error < best_error){ best_error = total_error; } parameter[index_param] += changes[index_param]; } else{ if(total_error < best_error){ best_error = total_error; changes[index_param] *= 1.1; IndexMove(); parameter[index_param] += changes[index_param]; fail_counter = 0; } else if(fail_counter == 0){ parameter[index_param] -= (2*changes[index_param]); fail_counter++; } else{ parameter[index_param] += changes[index_param]; changes[index_param] *= 0.9; IndexMove(); parameter[index_param] += changes[index_param]; fail_counter = 0; } } std::cout << "best_error: "<< best_error<<" total_error: "<<total_error<<std::endl; std::cout << "change_index: "<<index_param<<" new_parameter: "<<parameter[index_param]<<std::endl; std::cout << std::endl; total_error = 0; } } step++;}double PID::TotalError() { return -parameter[0] * p_error - parameter[1] * i_error - parameter[2] * d_error;}void PID::IndexMove() { index_param++; if(index_param >=3){ index_param = 0; }}
pid.h
#ifndef PID_H#define PID_H#include <cmath>#include <vector>class PID {private: int step; std::vector<double> changes; double best_error; double total_error; int index_param; int val_step; int test_step; int fail_counter; void IndexMove(); bool need_twiddle;public: /* * Errors */ double p_error; double i_error; double d_error; /* * Coefficients, the order is P, I, D */ std::vector<double> parameter; /* * Constructor */ PID(); /* * Destructor. */ virtual ~PID(); /* * Initialize PID. */ void Init(double Kp, double Ki, double Kd); /* * Update the PID error variables given cross track error. */ void UpdateError(double cte); /* * Calculate the total PID error. */ double TotalError();};#endif /* PID_H */
用法
在你的实际控制循环中,调用:
PID pid;pid.Init(0.3345, 0.0011011, 2.662); //your init parametersfor (in your control loop) { pid.UpdateError(cte); steer_value = pid.TotalError();}
- 无人驾驶汽车系统入门(四)——反馈控制入门,PID控制
- 无人驾驶汽车系统入门(一)——卡尔曼滤波与目标追踪
- 无人驾驶汽车系统入门(二)——高级运动模型和扩展卡尔曼滤波
- 无人驾驶汽车系统入门(三)——无损卡尔曼滤波,目标追踪,C++
- 无人驾驶汽车系统入门(五)——运动学自行车模型和动力学自行车模型
- 无人驾驶汽车系统入门(六)——基于传统计算机视觉的车道线检测(1)
- 无人驾驶汽车系统入门(七)——基于传统计算机视觉的车道线检测(2)
- Siebel 基础入门(四)权限控制
- Swift快速入门(四)流程控制
- Shell入门(四)流程控制
- Unity3D入门(四):摄像机控制
- Unity3D入门(四):摄像机控制
- 版本控制——入门
- Linux入门学习七——系统服务的控制
- 四轴PID控制算法
- Go入门(四)-流程控制与struct
- 激光雷达—无人驾驶汽车的眼睛
- JQuery入门——控制DOM对象
- EventBus 源码分析
- LockSupport
- Verilog学习笔记--时延
- HTML5权威指南笔记:35-使用canvas元素(1)
- 记录学习过程中碰到的json对象数组字符串转成list方法
- 无人驾驶汽车系统入门(四)——反馈控制入门,PID控制
- 使用expdp和impdp远程导入导出库
- 程序员必备的600个英语词汇合集(2)
- C++设计模式——一个基于C++11的万用单例模板类
- 一个简单的基于注解的controller
- 文章标题
- 模拟+乱搞——我有特殊的懵题技巧
- Oracle的安装及可视化工具PL/SQL配置
- python 属性描述符