< >
Home » 无人驾驶汽车系统入门 » 无人驾驶汽车系统入门（四）——反馈控制入门，PID控制

无人驾驶汽车系统入门（四）——反馈控制入门，PID控制

无人驾驶汽车系统入门（四）——反馈控制入门，PID控制

说明：

  • 前面几篇博客介绍了卡尔曼滤波的一些基本算法，其实目标追踪，定位，传感器融合还有很多问题要处理，这些我们在以后的系列博客中在进一步细讲，现在我想给大家介绍一下无人驾驶汽车系统开发中需要的控制相关的理论和技术，还是和第一篇说的那样，我想到哪就写到哪，追踪和定位等更高级的算法我在后面会继续写。所以感兴趣的同学可以关注我的博客，无人驾驶汽车系统入门系列博客会一直更新下去。

为什么需要控制理论

  • 试想有如下场景，当你驾驶一辆汽车通过这个弯道的时候，假设你已经知道你要开的路线，那么你会怎么去操作控制你的车呢？
    请输入图片描述

  • 显然，如果你不是专业的选手的话，你无法做到一步到位的控制，你需要一边观察车辆相对于你想要开的路线的相对偏差，一边调整你的方向盘的角度和油门踏板的力度，这种基于环境反馈的控制我们称为 反馈控制 。

  • 反馈控制是现代控制理论的基础，这是反馈控制的一般思路：

请输入图片描述

  • 我们希望我们控制的对象（无人车）能够按照我们希望（规划好）的路径行驶，
  • 我们会将环境当前给我们的反馈（我们当前的位置）和参考线进行比较，得到我们当前偏离参考线的距离（误差），
  • 基于这个误差，我们设计一定的算法来产生输出信号，使得这个误差不断的变小，这样的过程就是反馈控制的一般过程。
  • 那么我们如何基于这个误差来产生控制指令呢？
  • 我们最直观的感觉就是要让误差在我们的控制下逐渐变小直到为0：
    请输入图片描述
  • 0误差就意味着车一直在你想让它开的路径上开。如何减少误差就是我们这几篇博客要向大家介绍的内容。
  • 为了了解反馈控制，我先向大家介绍 PID控制，PID控制是目前利用最为广泛的控制理论，我们以它为出发点讨论控制理论。

比例，积分和导数

  • PID就是指 比例（proportion）、积分（integral）、导数（derivative），
  • 这三项表示我们如何使用我们的误差来产生控制指令，整个流程如下：

请输入图片描述

请输入图片描述

1.P控制

  • 考虑一个简单的情况，假设我们希望无人车按照图中绿线行驶，但是我们的车在如图所示的位置：

请输入图片描述

  • 那么我们要转多少度角呢？如果都按照固定的角度转（如下图），那么车的轨迹将如图中所示：

请输入图片描述

  • 那么显然坐这样的车是不舒服的。一个直观的解决方法就是使用比例控制。

  • 如图所示，当偏差大的时候，我们偏转更多的角度，当偏差小的时候，则偏转小一点。
    请输入图片描述

  • 那么这就是P control（比例控制）这里我们使用 CTE（Cross Track Error） 作为偏差度量 ，

  • CTE就是我们到参考线的距离。那么这个时候转角就变成了：

请输入图片描述

请输入图片描述

请输入图片描述

请输入图片描述

  • 此时车辆虽然在参考线上，但是并不是我们希望的状态（它在下一刻就会偏离），但是对于P控制而言，这是理想状态，此时控制转角为0,因此，P控制会一次又一次的超过参考线（overshot），为了矫正这种overshot，我们需要考虑一个额外的误差项——CTE变化率。

2.PD控制

  • CTE的变化率描述了我们的无人车向着参考线方向移动的有多快，如果我们的无人车一直都完美的在参考线上运动的话，那么我们的CTE变化率就为0。那么这一项（描述误差的变化率）就可以用导数来表示，那么，现在我们的控制输出就变成了比例项和导数项求和的形式：

请输入图片描述

  • PD控制似乎已经能够胜任良好的反馈控制了，但其实还不够，PD控制器可以保证正常的控制的需求，但是当环境存在扰动的时候，比如说下面这种情况：

请输入图片描述

  • 车在受力发生轻微偏移以后，由于PD控制中下 P 项倾向于向参考线方向运动，而 D 项则尝试抵消这种倾向，造成无人车始终都无法沿着参考线运动，这个问题叫做 steady state error 为了解决这个问题，我们再引入一项—— 积分项。

3.PID控制

  • 我们将积分项也就如到我们的控制输出函数中，这个时候，无人车的转角就可以表示为：

请输入图片描述

  • 我们发现其实PID实现确实不难，但是三个系数的选择却很难，那么如何选择PID系数呢？
  • 我们可以在我们的控制循环中通过一定的算法不断尝试，下面我提供给大家一种寻找参数的算法：

请输入图片描述

  • 具体的算法见我的C++代码实例。
  • PID C++代码

1.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;
    }
}
  1. 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 */

3.用法

  • 在你的实际控制循环中，调用：
PID pid;
pid.Init(0.3345, 0.0011011, 2.662); //your init parameters

for (in your control loop) {
  pid.UpdateError(cte);
  steer_value = pid.TotalError();
}

参考：

  • https://blog.csdn.net/adamshan/article/details/78458325

纠错,疑问,交流: 请进入讨论区或 点击加入Q群

获取最新文章: 扫一扫右上角的二维码加入“创客智造”公众号


标签: 无人驾驶汽车系统入门