基于汽车运动学模型的LQR控制

一、汽车运动学模型与LQR控制算法部分

        模型建立、算法细节、代码框架基于B站up主（小黎的Ally）的路径规划与轨迹跟踪系列算法学习视频

路径规划与轨迹跟踪系列算法学习_第14讲_轨迹跟踪算法勘误、改进及比较_哔哩哔哩_bilibili

        以及白菜丁院士（bushi）的课程笔记

控制算法-线性二次调节器LQR法 - 知乎

        本文主要是C++的代码实现。

二、代码部分

        四部分七个文件构成

        主函数部分

        main.cpp

#include <iostream>
#include "LQR.h"
#include "Pose.h"
#include"graph2d.h"
using namespace std;

void LQRtest(int control_time);

int main()
{
	LQRtest(100);
}

void LQRtest(int control_time)
{
	Pose pos;
	LQRControl ctrl;
	//给定refpos的回调函数和控制时间
	function<double(double)> fct = [](double x) {return 4 / (exp(-0.1 * (x - 50)) + 1); };
	pos.refpos.setfun(fct, control_time);

	//存放参考轨迹与每一时刻的轨迹，供画图使用
	vector<Point> actual_pos(pos.refpos.nums);
	vector<Point> ref_pos(pos.refpos.nums);

	//开始循环
	for (int i = 0; i < control_time; ++i)
	{
        //寻找最近路径编号索引
		pos.refpos.refposition(pos.pos_x, pos.pos_y);
        //生成控制量
		ctrl.actOnce(pos);
        //状态更新
		pos.update(pos.para.dt, ctrl.U);
        //存下路径，便于画图
		actual_pos[i].x = pos.pos_x;
		actual_pos[i].y = pos.pos_y;
		ref_pos[i].x = pos.refpos.ref_x[i];
		ref_pos[i].y = pos.refpos.ref_y[i];
	}

	//画图
	graph2d g2d(700, 590, { 0,-5 }, { 125,5 });
	g2d.xlabel("x轴");
	g2d.ylabel("y轴");
	g2d.title("LQR控制情况");
	g2d.plot(actual_pos, BLUE);
	g2d.plot(ref_pos, RED);
	g2d.waitKey();
}

        汽车状态部分

        Pos.h

#pragma once
#include <iostream>
#include <vector>
#include <functional>
#include "Eigen/Dense"
using namespace std;
using namespace Eigen;

//存放车辆参数信息
class Parameter
{
public:
	Parameter(double len = 2.9, double t = 0.1, double refd = 0, double refs = 40 / 3.6)
	{
		L = len;//轴距
		dt = t;//控制周期
		ref_delta = refd;
		ref_speed = refs;
	}
public:
	double L;
	double dt;
	double ref_delta, ref_speed;
};

//存放期望位姿信息
class refPos
{
public:
	refPos() = default;
	void setfun(function<double(double)>& fun, int x_rg);
	double refposition(double pos_x, double pos_y);   //还未定义
public:
	double idx;
	int nums;
	vector<double> ref_x;
	vector<double> ref_y;
	vector<double> ref_yaw;
	vector<double> ref_pos_d;
	vector<double> ref_pos_dd;
	vector<double> ref_pos_k;
private:
	void refderiCalcu();
};

//存放汽车状态信息
class Pose
{
public:
	Pose(double x = 0,double y = 5,double yaw = 0,double velo = 10.0,double del = 0)
	{
		pos_x = x;
		pos_y = y;
		pos_yaw = yaw;
		v = velo;
		delta = del;
	}
	void update(double dt, double v_inc, double delta_inc);
	void update(double dt, Vector2d& U);

public:
	double pos_x, pos_y, pos_yaw;
	double v;
	double delta;
	Parameter para;
	refPos refpos;
};

Pose.cpp

#include <Eigen/Dense>
#include "Pose.h"
using namespace std;
using namespace Eigen;

void Pose::update(double dt, double v_inc, double delta_inc)
{
	delta = para.ref_delta + delta_inc;
	pos_x += v * cos(pos_yaw) * dt;
	pos_y += v * sin(pos_yaw) * dt;
	pos_yaw += v * tan(delta) * dt / para.L;
	v = para.ref_speed + v_inc;
}

void Pose::update(double dt,Vector2d& U)
{
	double v_inc = U(0);
	double delta_inc = U(1);
	delta = para.ref_delta + delta_inc;
	pos_x += v * cos(pos_yaw) * dt;
	pos_y += v * sin(pos_yaw) * dt;
	pos_yaw += v * tan(delta) * dt / para.L;
	v = para.ref_speed + v_inc;
}

//参考轨迹的生成，使用了传入的回调函数，x_rg是轨迹的长度，即总共有多少个点
void refPos::setfun(function<double(double)>& fun,int x_rg)
{
	nums = x_rg;
	ref_x.resize(x_rg);
	ref_y.resize(x_rg);
	for (int i = 0; i < x_rg; ++i)
	{
		ref_x[i] = i + 1;
		ref_y[i] = fun(i + 1);
	}
	refderiCalcu();
}

//参考轨迹上距离当前距离的轨迹点索引计算
double refPos::refposition(double pos_x, double pos_y)
{
	int min_idx = 0;
	double min_dis = DBL_MAX;
	double this_dis = 0;
	for (int i = 0; i < nums; ++i)
	{
		this_dis = sqrt(pow(ref_x[i] - pos_x, 2) + pow(ref_y[i] - pos_y, 2));
		if (this_dis < min_dis)
		{
			min_idx = i;
			min_dis = this_dis;
		}
	}
	idx = min_idx;
	return this_dis;
}

//参考轨迹的一阶导、二阶导、曲率、期望横摆角的计算
void refPos::refderiCalcu()
{
	ref_pos_d.resize(nums);
	ref_pos_dd.resize(nums);
	ref_yaw.resize(nums);
	ref_pos_k.resize(nums);
	for (int i = 0; i < (nums - 1); ++i)
	{
		ref_pos_d[i] = (ref_y[i + 1] - ref_y[i]) / (ref_x[i + 1] - ref_x[i]);
		if (i != 0)
		{
			ref_pos_dd[i] = (ref_y[i + 1] - 2 * ref_y[i] + ref_y[i - 1]) / (pow((0.5 * (ref_x[i + 1] - ref_x[i - 1])), 2));
		}
		ref_yaw[i] = atan(ref_pos_d[i]);
		ref_pos_k[i] = (ref_pos_d[i]) / (pow(1 + pow(ref_pos_d[i], 2), 1.5));
	}
	ref_pos_dd[0] = ref_pos_dd[1];
	ref_pos_d[nums - 1] = ref_pos_d[nums - 2];
	ref_pos_dd[nums - 1] = ref_pos_dd[nums - 2];
	ref_yaw[nums - 1] = ref_yaw[nums - 2];
	ref_pos_k[nums - 1] = ref_pos_k[nums - 2];
}

        LQR控制部分

        LQR.h

#pragma once
#include <iostream>
#include <Eigen/Dense>
#include "Pose.h"

using namespace Eigen;

class LQRControl
{
public:

	LQRControl():Q(Matrix3d::Identity()),R(Matrix2d::Identity()*2){}
	bool Calcu_K(Matrix<double, 3, 3>& A, Matrix<double, 3, 2>& B);
	void actOnce(Pose& p);

private:
	bool cmpEpsl(const Matrix3d&& P, double epsilon);
	double angle_lmt(double angle_in);

public:
	Matrix<double, 2, 3> K;
	Matrix3d Q;
	Matrix2d R;
	Vector2d U;
private:
	Matrix3d P;

};

        LQR.cpp

#include <Eigen/Dense>
#include <stdexcept>
#define _USE_MATH_DEFINES//是为了使用M_PI
#include <math.h>
#include "Pose.h"
#include "LQR.h"
using namespace Eigen;

//计算反馈增益矩阵K
bool LQRControl::Calcu_K(Matrix<double, 3, 3>& A, Matrix<double, 3, 2>& B)
{
	int iter_max = 500;
	double epsilon = 0.01;

	Matrix3d P_old = Q;
	Matrix3d P_new;
	try
	{
		for (int i = 0; i < iter_max; ++i)
		{
			P_new = A.transpose() * P_old * A - (A.transpose() * P_old * B) * ((R + B.transpose() * P_old * B).inverse()) * (B.transpose() * P_old * A) + Q;
			if (cmpEpsl(P_new - P_old, epsilon)) break;
			else P_old = P_new;
		}

		P = P_new;
		K = (B.transpose() * P * B + R).inverse() * (B.transpose() * P * A);
	}
	catch(std::runtime_error& e)
	{
		std::cout << e.what() << std::endl;
		return false;
	}
	return true;
}


void LQRControl::actOnce(Pose& p)
{
	Vector3d X = Vector3d::Zero();
	X(0) = p.pos_x - p.refpos.ref_x[p.refpos.idx];
	X(1) = p.pos_y - p.refpos.ref_y[p.refpos.idx];
	X(2) = angle_lmt(p.pos_yaw - p.refpos.ref_yaw[p.refpos.idx]);
	Matrix3d A = Matrix3d::Identity();
	A(0, 2) = -p.para.ref_speed * p.para.dt * sin(p.refpos.ref_yaw[p.refpos.idx]);
	A(1,2)= p.para.ref_speed * p.para.dt * cos(p.refpos.ref_yaw[p.refpos.idx]);
	Matrix<double, 3, 2> B = Matrix<double, 3, 2>::Zero();  
	B(0, 0) = p.para.dt * cos(p.refpos.ref_yaw[p.refpos.idx]);
	B(1, 0) = p.para.dt * sin(p.refpos.ref_yaw[p.refpos.idx]);
	B(2, 0) = p.para.dt * tan(p.para.ref_delta) / p.para.L;
	B(2, 1) = p.para.ref_speed * p.para.dt / (p.para.L * pow(cos(p.para.ref_delta), 2));
	if (!Calcu_K(A, B))
	{
		cout << "ERROR OCCURED!" << endl;
	}
	U= -K * X;
	U(1) = angle_lmt(U(1));
}

//比较矩阵是否满足超过阈值退出的条件
bool LQRControl::cmpEpsl(const Matrix3d&& P, double epsilon)
{
	for (int i = 0; i < 3; ++i)
	{
		for (int j = 0; j < 3; ++j)
		{
			if (P(i, j) > epsilon) return false;
		}
	}
	return true;
}

//角度限制
double LQRControl::angle_lmt(double angle_in)
{
	if (angle_in > M_PI) return angle_in - 2 * M_PI;
	else if (angle_in < -M_PI) return angle_in + 2 * M_PI;
	else return angle_in;
}

        画图部分参考了这篇文章，使用了graph2d.h、graph2d.cpp

C++使用graphics.h绘制二维坐标_始原始的博客-CSDN博客_c++ graphics

C++画图需要用到外置库，可以用matplotlib等等，这篇文章中的绘图方法相对较为简单，遂采用