PID控制器轨迹跟随 实现

参考博客：
【自动驾驶】PID实现轨迹跟踪 | python实现 | C++实现
【C++ matplotlib 画图 Linux】
【无人车系统（一）：运动学模型及其线性化】

1 运动学模型及其线性化

无人车运动学模型：
$v$：无人车的速度
$\dot{x}$：无人车在世界坐标系中X轴方向上的分速度
$\dot{y}$：无人车在世界坐标系中Y轴方向上的分速度
$\theta$：无人车在世界坐标中的航向角
$\dot{\theta }$：无人车的角速度

两个主要控制对象：$v$、$w$

运动学模型线性化:

测量具有离散的本质，现有的物理系统只能被离散的观测与控制。时间维度上的状态都被离散化为一个个状态序列$x_0,x_1,...,x_t,...$。物理系统本质上是连续的，那些状态更新间隔，因为仅仅能够离散的控制、影响这些系统。
模型的离散化是对物理系统在时间维度的近似。离散化后的差分模型与系统真实的连续模型是有误差的。

有误差，还要将就采用离散的差分形式？

不能立马是指控制指令并不能立马从当前值变成期望的控制输入值，就算用连续模型加上阶梯形式控制输入函数计算状态的响应输出也是不准确的。与其不准，不如用形式相对简洁的差分模型。另外，并不是所有的系统模型都是可导的，这使得绝大多数情况是被迫采用差分模型。
差不多就可以 由于控制是按一定的周期观测系统内部真实的状态，也即系统的状态初值是不断更新。如果我们的更新周期不是太长，在短时间内，差分函数的状态预测误差是在可接受的范围。

无人车运动学模型在低速时可以较好的预测（估计）系统未来的状态，但是随着车速的提高，汽车侧向动力学特性的影响越来越明显。因此，在高速运行时，一个更加准确的无人车模型必须加入汽车的动力学特性。

2 运动模型参考代码

参考了【自动驾驶】PID实现轨迹跟踪 | python实现 | C++实现
KinematicModel.h

#pragma once
#include 
#include 
#include 
#include 
using namespace std;
using namespace Eigen;

class KinematicModel {
public:
    KinematicModel();
    KinematicModel(double x, double y, double psi, double v, double L, double dt);

    vector<double> getState();
    void updateState(double a, double delta);
    vector<MatrixXd> stateSpace(double ref_delta, double ref_yaw);
public:
    double x, y, v, L, dt, psi;
};

KinematicModel.cpp

#include "KinematicModel.h"

KinematicModel::KinematicModel(double x, double y, double psi, double v, double L, double dt) : x(x), y(y), psi(psi),
                                                                                                v(v), L(L), dt(dt) {}
void KinematicModel::updateState(double a, double delta)
{
    x = x + v * cos(psi) * dt;
    y = y + v * sin(psi) * dt;
    psi = psi + v / L * tan(delta) * dt;
    v = v + a * dt;
}

vector<double> KinematicModel::getState()
{
    return {x, y, psi, v};
}
// 将模型离散化后的状态空间表达
vector<MatrixXd> KinematicModel::stateSpace(double ref_delta, double ref_yaw)
{
    MatrixXd A(3, 3), B(3, 2);
    A << 1.0, 0.0, -v * sin(ref_yaw) * dt,
         0.0, 1.0, v * cos(ref_yaw) * dt,
         0.0, 0.0, 1.0;
    B << cos(ref_yaw) * dt, 0,
         sin(ref_yaw) * dt, 0,
         tan(ref_delta) * dt / L, v * dt / (L * cos(ref_delta) * cos(ref_delta));
    return {A, B};
}

3 PID控制器

任何闭环控制系统的首要任务是要稳、准、快的响应命令。PID的主要工作就是如何实现这一任务。
PID控制器的比例单元 ( P) 、积分单元(I)和微分单元(D)分别对应目前误差、过去累计误差及未来误差。若是不知道受控系统的特性，一般认为PID控制器是最适用的控制器。

P:增大比例加快系统的响应，它的作用于输出值较快，但不能很好稳定在一个理想的数值。Kp过大，会产生超调，并产生振荡。
I:在P的基础上消除余差，对稳定后有累积误差的系统进行误差修整，减小稳态误差。
D:可以使系统超调量减小，减小振荡，增加稳定性。

位置式PID：当前系统的实际位置，与你想要达到的预期位置的偏差，进行PID控制

当采样时间足够小时，能够获得最够精确的结果，离散控制过程与连续过程非常接近。

位置式PID在积分项达到饱和时,误差仍然会在积分作用下继续累积，一旦误差开始反向变化，系统需要一定时间从饱和区退出，所以在u(k)达到最大和最小时，要停止积分作用，并且要有积分限幅和输出限幅。

抗积分饱和：如果上一次的输出控制量超过了饱和值，饱和值为正，则这一次只积分负的偏差，饱和值为负，则这一次只积分正的偏差，从而避免系统长期留在饱和区！

pid_controller.h

#pragma once
#include 

using namespace std;

class PID_controller {
private:
    double Kp, Ki, Kd, target, upper, lower;
    double error = 0.0, pre_error = 0.0, sum_error = 0.0;
public:
    PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower);

    void setTarget(double target);

    void setK(double Kp, double Ki, double Kd);

    void setBound(double upper, double lower);

    double calOutput(double state);

    void reset();

    void setSumError(double sum_error);
};

pid_controller.cpp

#include "pid_controller.h"

PID_controller::PID_controller(double Kp, double Ki, double Kd, double target, double upper, double lower) : Kp(Kp),
                                                                                                             Ki(Ki),
                                                                                                             Kd(Kd),
                                                                                                             target(target),
                                                                                                             upper(upper),
                                                                                                             lower(lower) {}
void PID_controller::setTarget(double target) {
    PID_controller::target = target;
}

void PID_controller::setK(double Kp, double Ki, double Kd) {
    this->Kp = Kp;
    this->Ki = Ki;
    this->Kd = Kd;
}

void PID_controller::setBound(double upper, double lower) {
    this->upper = upper;
    this->lower = lower;
}

double PID_controller::calOutput(double state) {
    this->error = this->target - state;
    double u = this->error * this->Kp + this->sum_error * this->Ki + (this->error - this->pre_error) * this->Kd;
    if (u < this->lower) u = this->lower;
    else if (u > this->upper) u = this->upper;
    this->pre_error = this->error;
    this->sum_error = this->sum_error + this->error;
    return u;
}

void PID_controller::reset() {
    error = 0.0;
    pre_error = 0.0;
    sum_error = 0.0;
}

void PID_controller::setSumError(double sum_error) {
    this->sum_error = sum_error;
}

4 车辆横向跟踪误差

横向跟踪误差(cross track error, 简称CTE)为车辆中心点$(r_x,r_y)$到最近路径点$(p_x,p_y)$的距离
$e_y=l_{d}sin{\theta }_e$
main.cpp

#include "KinematicModel.h"
#include "pid_controller.h"
#include 
#include "matplotlibcpp.h"
namespace plt = matplotlibcpp;

#define PI 3.1415926
// 得到距离参考轨迹最近点的下标
// robot_state 机器人状态（x,y）
// ref_path 参考路径
// return 距离参考轨迹最近点的下标
double calTargetIndex(vector<double> robot_state, vector<vector<double>> ref_path) {
    vector<double> dists;
    for (vector<double> xy : ref_path) {
        double dist = sqrt(pow(xy[0] - robot_state[0], 2) + pow(xy[1] - robot_state[1], 2));
        dists.push_back(dist);
    }
    return min_element(dists.begin(), dists.end()) - dists.begin();
}

int main()
{
    vector<vector<double>> ref_path(1000, vector<double>(2));
    vector<double> ref_x, ref_y;//保存参考数据用于画图
    // 生成参考轨迹
    for (int i = 0; i < 1000; i++) {
        ref_path[i][0] = 0.1 * i;
        ref_path[i][1] = 2 * sin(ref_path[i][0] / 3.0);
        ref_x.push_back(ref_path[i][0]);
        ref_y.push_back(ref_path[i][1]);
    }
    // 运动学模型
    KinematicModel model(0, -1, 0.5, 2, 2, 0.1);
	  // PID控制器
    PID_controller PID(2, 0.01, 100, 0, PI / 6, -PI / 6);
    // 保存机器人（小车）运动过程中的轨迹
    vector<double> x_, y_;
    // 机器人状态
    vector<double> robot_state(2);
    for (int i = 0; i < 500; i++) {
        plt::clf();
        robot_state[0] = model.x;
        robot_state[1] = model.y;
        double min_ind = calTargetIndex(robot_state, ref_path);
        double alpha = atan2(ref_path[min_ind][1] - robot_state[1], ref_path[min_ind][0] - robot_state[0]);
        double ld = sqrt(pow(ref_path[min_ind][0]-robot_state[0],2)+pow(ref_path[min_ind][1]-robot_state[1],2));
        double theta_error = alpha - model.psi;
        double e_y = -ld * sin(theta_error);
        double delta = PID.calOutput(e_y);

        model.updateState(0, delta);
        x_.push_back(model.x);
        y_.push_back(model.y);
        plt::plot(ref_x, ref_y, "r--");
        plt::plot(x_, y_, "b");
        plt::grid(true);
        plt::ylim(-3.0, 3.0);
        plt::pause(0.01);
    }
    const char* filename = "./pid.png";
    plt::save(filename);
    plt::show();
    return 0;
}

详细步骤：

mkdir -p catkin_ws/src
cd catkin_ws/src
catkin_init_workspace
catkin_create_pkg PID_Controller roscpp std_msgs

在include文件夹下面放入 KinematicModel.h，matplotlibcpp.h，pid_controller.h
在src下放入KinematicModel.cpp,main.cpp,pid_controller.cpp

CMakeList.txt

cmake_minimum_required(VERSION 3.0.2)
project(PID_Controller)

set(CMAKE_CXX_STANDARD 11)

file(GLOB_RECURSE PYTHON2.7_LIB "/usr/lib/python2.7/config-x86_64-linux-gnu/*.so")
set(PYTHON2.7_INLCUDE_DIRS "/usr/include/python2.7")

find_package(catkin REQUIRED COMPONENTS
  roscpp
  std_msgs
)

catkin_package(
#  INCLUDE_DIRS include
#  LIBRARIES huatu
#  CATKIN_DEPENDS roscpp std_msgs
#  DEPENDS system_lib
)

include_directories(include
        ${PYTHON2.7_INLCUDE_DIRS}
)

add_executable(pid_controller src/pid_controller.cpp
                              src/KinematicModel.cpp
                              src/main.cpp)
target_link_libraries(pid_controller ${PYTHON2.7_LIB})