PID控制器轨迹跟随 实现

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

1 运动学模型及其线性化

：
v v v：无人车的速度
x ˙ \dot x x˙：无人车在世界坐标系中X轴方向上的分速度
y ˙ \dot y y˙​：无人车在世界坐标系中Y轴方向上的分速度
θ \theta θ：无人车在世界坐标中的航向角
θ ˙ \dot \theta θ˙：无人车的角速度

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

:

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

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

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

2 运动模型参考代码

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

#pragma once
#include <iostream>
#include <vector>
#include <cmath>
#include <Eigen/Dense>
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控制器是最适用的控制器。

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

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

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

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

pid_controller.h

#pragma once
#include <iostream>

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 ) (r_x,r_y) (rx​,ry​)到最近路径点 ( p x , p y ) (p_x,p_y) (px​,py​)的距离
e y = l d s i n θ e e_y=l_dsin\theta_e ey​=ld​sinθe​
main.cpp

#include "KinematicModel.h"
#include "pid_controller.h"
#include <algorithm>
#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})