基于汽车运动学模型的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等等，这篇文章中的绘图方法相对较为简单，遂采用

基于汽车运动学模型的LQR控制相关推荐

路径跟踪—基于车辆运动学方程的离散时间LQR控制仿真
文章目录前言一.运动学模型二.建模仿真总结更新前言路径跟踪,轨迹跟踪的算法有基于运动学和基于动力学的,其中LQR算法基于车辆动力学网上有很多,毕竟百度开源了apollo,该算法通过简单改 ...
基于车辆运动学模型的纯跟踪(Pure Pursuit)法
一.定义及概论纯跟踪控制算法(Pure Pursuit)是一种典型的横向控制方法,最早由 R. Wallace 在1985年提出,该方法对外界的鲁棒性较好. 该算法的思想:基于当前车辆后轮中心位置, ...
无人车系统（一）：运动学模型及其线性化
相对无人机与机械臂来说,无人车系统的运动学模型非常简洁.尽管简洁,无人车的运动学模型也是非线性的.应用于具体控制算法时,有必要对原始运动学模型进行变形或线性化.本篇主要介绍无人车的运动学模型,并对原始 ...
无人驾驶汽车系统入门（十）——基于运动学模型的模型预测控制
无人驾驶汽车系统入门(十)--基于运动学模型的模型预测控制在前面的第五篇博客中,我们介绍了两种常见的车辆模型--运动学自行车模型和动力学自行车模型,本节我们基于运动学车辆模型引入一种新的控制理论-- ...
基于运动学模型的轨迹跟踪控制
章四基于运动学模型的轨迹跟踪控制 MPC(4)基于运动学模型的轨迹跟踪控制器设计无人驾驶车辆模型预测控制(龚建伟)第四章基于运动学模型的轨迹跟踪控制(仿真部分) 无人车辆在惯性坐标系中,车辆必须 ...
基于层次分析法的轴间预瞄和轴距预瞄俯仰半车LQR控制仿真分析（重型汽车）
目录前言 1. 半车悬架模型 2. LQR最优控制原理简述 3. 基于LQR的半车悬架轴间预瞄和轴距预瞄仿真分析 3.1仿真模型 3.2仿真结果 4.总结前言上篇文章简谈了轴距预瞄和轴间预瞄的原 ...
面向6G网络自动化的知识驱动可解释人工智能（XAI）；基于汽车事件数据的脉冲神经网络目标检测；对称分类方案下波导阵列中的BIC；PreMovNet：基于运动前脑电的抓取和提举手运动学估计
可解释的机器学习中文标题:面向6G网络自动化的知识驱动可解释人工智能(XAI) 英文标题:Knowledge-powered Explainable Artificial Intelligence ...
自动驾驶车辆控制（车辆运动学模型）
本文应配合b站up主"忠厚老实的老王"的自动驾驶控制算法系列视频食用. 文章目录 1. 两个车辆运动学模型 1.1 三个坐标系 1.2符号定义 1.3车辆运动学模型 1.4车辆动力 ...
基于蚁群算法的六轴机械臂路径规划（运动学模型建立）
机器人运动学模型的建立 1 D-H参数法建立坐标系 2 机器人运动学分析 2.1 运动学正解 2.2 运动学逆解 3 机器人的轨迹仿真 1 D-H参数法建立坐标系代码: clear; clc; %建 ...
基于角色-功能-资源的权限控制模型的设计与实现-引子
摘要本文在RBAC基本思想的基础上,增加资源权限的概念,设计了在企业应用系统中用户权限控制的一种具体的简单实现方法. 关键字用户权限控制名词解释资源权限:资源指的是纳入企业应用的一切需要管理的 ...

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

基于汽车运动学模型的LQR控制相关推荐

最新文章

热门文章