一、汽車運動學模型與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控制的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。