前言：
????????因為工作需要開始學習車輛橫縱向控制，然后學到了LQR，正好寫一個博客把程序保存下來。為了加強C++代碼能力，本次仿真的所有文件均用C++完成。
?文章來源地址http://www.zghlxwxcb.cn/news/detail-422686.html
代碼結構梳理
????????開始之前非常感謝這位大佬給出的參考：【自動駕駛】LQR實現(xiàn)軌跡跟蹤，這次項目大部分都是將該博客從python翻譯成C++，當然其中也發(fā)現(xiàn)了一些問題，后續(xù)再談。
該項目用多個模塊組成，分別為LQR、LQR_node、tool、trajectory、matplot5個模塊。
1.LQR_node為主函數(shù)節(jié)點，負責調用軌跡生成模塊、LQR控制器模塊和畫圖；
2.LQR為LQR控制器模塊，控制器中構造了模型參數(shù)A、B，計算黎卡提方程等功能；
3.trajectory為軌跡生成模塊，并且計算出坐標點對應的曲率值；
4.tool為工具模塊，定義了項目中需要的數(shù)據(jù)類型和一些角度處理函數(shù)（雖然沒用到）；
5.matplot為畫圖模塊，調用了python的matplot功能進行作圖；
????????該項目用到的庫有Eigen、python、matplotlibcpp，其中最為重要的是Eigen庫，建議提前看一下該庫的基本命令。
?
準備工作
1.項目配置Eigen庫：
安裝和使用C++線性代數(shù)庫eigen（Windows下minGW+VS code, VS2019配置方式）
2.項目配置matplot庫：
VS C++調用python進行畫圖matplotlib
?windows下配置C++版本的matplotlib繪圖工具matplotlibcpp
?
別忘了把解決方案配置換成Release，我在這里卡了好久
?
代碼
1.tool.h
#pragma once
#include <iostream>
using namespace std;
#define pi acos(-1)
//定義路徑點
typedef struct waypoint {
int ID;
double x, y, yaw, K;//x,y,yaw,曲率K
}waypoint;
//定義小車狀態(tài)
typedef struct vehicleState {
double x, y, yaw, v, kesi;//x,y,yaw,前輪偏角kesi
}vehicleState;
//定義控制量
typedef struct U {
double v;
double kesi;//速度v,前輪偏角kesi
}U;
double normalize_angle(double angle);//角度歸一化 [-pi,pi];
double limit_kesi(double kesi);//前輪轉角限幅 [-pi/2,pi/2];
2.tool.cpp
#include<iostream>
#include<tool.h>
double normalize_angle(double angle)//角度歸一化 [-pi,pi];
{
if (angle > pi) {
angle -= 2.0 * pi;
}
if (angle <= -pi) {
angle += 2.0 * pi;
}
return angle;
}
double limit_kesi(double kesi) {
if (kesi > pi / 2) {
kesi = pi / 2;
}
if (kesi < -pi / 2) {
kesi = -pi / 2;
}
return kesi;
}3.LQR.h
#include <iostream>
#include <Eigen/Dense>
#include "tool.h"
using namespace std;
typedef Eigen::Matrix<double, 3, 3> Matrix3x3;
typedef Eigen::Matrix<double, 3, 1> Matrix3x1;
typedef Eigen::Matrix<double, 2, 1> Matrix2x1;
typedef Eigen::Matrix<double, 2, 2> Matrix2x2;
typedef Eigen::Matrix<double, 3, 2> Matrix3x2;
typedef Eigen::Matrix<double, 2, 3> Matrix2x3;
//狀態(tài)方程變量: X = [x_e y_e yaw_e]^T
//狀態(tài)方程控制輸入: U = [v_e kesi_e]^T
class LQR
{
private:
Matrix3x3 A_d;
Matrix3x2 B_d;
Matrix3x3 Q;
Matrix2x2 R;
Matrix3x1 X_e;
Matrix2x1 U_e;
double L;//車輛軸距
double T;//采樣間隔
double x_car, y_car, yaw_car, x_d, y_d, yaw_d;//車輛位姿和目標點位姿
double v_d, kesi_d;//期望速度和前輪偏角
double Q3[3];//Q權重，三項
double R2[2];//R權重，兩項
int temp = 0;
public:
void initial(double L_, double T_, vehicleState car, waypoint waypoint, U U_r, double* Q_, double* R_);//初始化
void param_struct();//構造狀態(tài)方程參數(shù)
Matrix2x3 cal_Riccati();//黎卡提方程求解
U cal_vel();//LQR控制器計算速度
void test();
};
4.LQR.cpp
#include <iostream>
#include <LQR.h>
using namespace std;
void LQR::initial(double L_, double T_, vehicleState car, waypoint waypoint, U U_r, double *Q_, double *R_) {
L = L_;
T = T_;
x_car = car.x; y_car = car.y; yaw_car = car.yaw;
x_d = waypoint.x; y_d = waypoint.y; yaw_d = waypoint.yaw;
v_d = U_r.v;kesi_d = U_r.kesi;
for (int i = 0; i < 3; i++) {
Q3[i] = Q_[i];
}
for (int j = 0; j < 2; j++) {
R2[j] = R_[j];
}
}
void LQR::param_struct() {
Q << Q3[0], 0.0, 0.0,
0.0, Q3[1], 0.0,
0.0, 0.0, Q3[2];
//cout << "Q矩陣為：\n" << Q << endl;
R << R2[0], 0.0,
0.0, R2[1];
//cout << "R矩陣為：\n" << R << endl;
A_d << 1.0, 0.0, -v_d * T * sin(yaw_d),
0.0, 1.0, v_d* T* cos(yaw_d),
0.0, 0.0, 1.0;
//cout << "A_d矩陣為:\n" << A_d << endl;
B_d << T * cos(yaw_d), 0.0,
T* sin(yaw_d), 0.0,
T* tan(kesi_d)/L, v_d* T / (L * cos(kesi_d) * cos(kesi_d));
//cout << "B_d矩陣為:\n" << B_d << endl;
X_e << x_car - x_d, y_car - y_d, yaw_car - yaw_d;
cout << "X_e矩陣為:\n" << X_e << endl;
}
Matrix2x3 LQR::cal_Riccati() {
int N = 150;//迭代終止次數(shù)
double err = 100;//誤差值
double err_tolerance = 0.01;//誤差收斂閾值
Matrix3x3 Qf = Q;
Matrix3x3 P = Qf;//迭代初始值
//cout << "P初始矩陣為\n" << P << endl;
Matrix3x3 Pn;//計算的最新P矩陣
for (int iter_num = 0; iter_num < N; iter_num++) {
Pn = Q + A_d.transpose() * P * A_d - A_d.transpose() * P * B_d * (R + B_d.transpose() * P * B_d).inverse() * B_d.transpose() * P * A_d;//迭代公式
//cout << "收斂誤差為" << (Pn - P).array().abs().maxCoeff() << endl;
//err = (Pn - P).array().abs().maxCoeff();//
err = (Pn - P).lpNorm<Eigen::Infinity>();
if(err < err_tolerance)//
{
P = Pn;
cout << "迭代次數(shù)" << iter_num << endl;
break;
}
P = Pn;
}
//cout << "P矩陣為\n" << P << endl;
//P = Q;
Matrix2x3 K = -(R + B_d.transpose() * P * B_d).inverse() * B_d.transpose() * P * A_d;//反饋率K
return K;
}
U LQR::cal_vel() {
U output;
param_struct();
Matrix2x3 K = cal_Riccati();
Matrix2x1 U = K * X_e;
//cout << "反饋增益K為：\n" << K << endl;
//cout << "控制輸入U為：\n" << U << endl;
output.v = U[0] + v_d;
output.kesi = U[1] + kesi_d;
return output;
}
void LQR::test() //控制器效果測試
{
/*param_struct();
while (temp < 1000) {
Matrix2x3 K = cal_Riccati();
Matrix2x1 U = K * X_e;
//cout <<"state variable is:\n" <<X_e << endl;
//cout <<"control input is:\n"<< U << endl;
Matrix3x1 X_e_ = A_d * X_e + B_d * U;
X_e = X_e_;
temp++;
}*/
Matrix3x3 C,D,F;
C << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 1.0;
F << 1.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 7.0;
D = (C - F);
double BBBB = D.lpNorm<Eigen::Infinity>();
cout << BBBB << endl;
}5.trajectory.h
#include <iostream>
#include <vector>
#include "tool.h"
using namespace std;
class trajectory {
private:
vector<waypoint> waypoints;
public:
//set reference trajectory
void refer_path();
vector<waypoint> get_path();
};
6.trajectory.cpp
#include <iostream>
#include <vector>
#include <trajectory.h>
#include <math.h>
using namespace std;
void trajectory::refer_path() {
waypoint PP;
for (int i = 0; i < 1000; i++)
{
PP.ID = i;
PP.x = 0.1 * i;//x
PP.y = 2.0 * sin(PP.x / 5.0) + 2.0 * cos(PP.x / 2.5);//y
//直線
//PP.y = 5.5;
PP.yaw = PP.K = 0.0;
waypoints.push_back(PP);
}
for (int j = 0; j < waypoints.size(); j++) {
//差分法求一階導和二階導
double dx, dy, ddx, ddy;
if (j == 0) {
dx = waypoints[1].x - waypoints[0].x;
dy = waypoints[1].y - waypoints[0].y;
ddx = waypoints[2].x + waypoints[0].x - 2 * waypoints[1].x;
ddy = waypoints[2].y + waypoints[0].y - 2 * waypoints[1].y;
}
else if (j == (waypoints.size() - 1)) {
dx = waypoints[j].x - waypoints[j - 1].x;
dy = waypoints[j].y - waypoints[j - 1].y;
ddx = waypoints[j].x + waypoints[j - 2].x - 2 * waypoints[j].x;
ddy = waypoints[j].y + waypoints[j - 2].y - 2 * waypoints[j].y;
}
else {
dx = waypoints[j + 1].x - waypoints[j].x;
dy = waypoints[j + 1].y - waypoints[j].y;
ddx = waypoints[j + 1].x + waypoints[j - 1].x - 2 * waypoints[j].x;
ddy = waypoints[j + 1].y + waypoints[j - 1].y - 2 * waypoints[j].y;
}
waypoints[j].yaw = atan2(dy, dx);//yaw
//計算曲率：設曲線r(t) =(x(t),y(t)),則曲率k=(x'y" - x"y')/((x')^2 + (y')^2)^(3/2).
double AAA = sqrt(pow((pow(dx, 2) + pow(dy, 2)), 3));
waypoints[j].K = (ddy * dx - ddx * dy) / (sqrt(pow((pow(dx, 2) + pow(dy, 2)), 3)));
}
}
vector<waypoint> trajectory::get_path() {
return waypoints;
}7.matplotlibcpp.h
這個配置matplot庫的時候會有這個頭文件，代碼里面直接調用就可以畫圖啦。
8.LQR_node.cpp
// _ooOoo_
// o8888888o
// 88" . "88
// (| -_- |)
// O\ = /O
// ____/`---'\____
// . ' \\| |// `.
// / \\||| : |||// \
// / _||||| -:- |||||- \
// | | \\\ - /// | |
// | \_| ''\---/'' | |
// \ .-\__ `-` ___/-. /
// ___`. .' /--.--\ `. . __
// ."" '< `.___\_<|>_/___.' >'"".
// | | : `- \`.;`\ _ /`;.`/ - ` : | |
// \ \ `-. \_ __\ /__ _/ .-` / /
// ======`-.____`-.___\_____/___.-`____.-'======
// `=---='
//
// .............................................
// 佛祖保佑 永無BUG
// 佛曰:
// 寫字樓里寫字間，寫字間里程序員；
// 程序人員寫程序，又拿程序換酒錢。
// 酒醒只在網(wǎng)上坐，酒醉還來網(wǎng)下眠；
// 酒醉酒醒日復日，網(wǎng)上網(wǎng)下年復年。
// 但愿老死電腦間，不愿鞠躬老板前；
// 奔馳寶馬貴者趣，公交自行程序員。
// 別人笑我忒瘋癲，我笑自己命太賤；
// 不見滿街漂亮妹，哪個歸得程序員？
#include <iostream>
#include <LQR.h>
#include <vector>
#include <trajectory.h>
#include <stdlib.h>
#include "matplotlibcpp.h"
using namespace std;
namespace plt = matplotlibcpp;
#define pi acos(-1)
#define T 0.05//采樣時間 很有意思的測試數(shù)據(jù)：T=0.5s，允許誤差范圍為±5.0m；T=0.1s，允許誤差范圍為±10.0m；T=0.05s；允許誤差范圍為±11m
#define L 0.5//車輛軸距
#define V_DESIRED 0.5//期望速度
vehicleState update_state(U control, vehicleState car) {
car.v = control.v;
car.kesi = control.kesi;
car.x += car.v * cos(car.yaw) * T;
car.y += car.v * sin(car.yaw) * T;
car.yaw += car.v / L * tan(car.kesi) * T;
//car.yaw = normalize_angle(car.yaw);
return car;
}
class Path {
private:
vector<waypoint> path;
public:
//添加新的路徑點
void Add_new_point(waypoint& p)
{
path.push_back(p);
}
void Add_new_point(vector<waypoint>& p)
{
path = p;
}
//路徑點個數(shù)
unsigned int Size()
{
return path.size();
}
//獲取路徑點
waypoint Get_waypoint(int index)
{
waypoint p;
p.ID = path[index].ID;
p.x = path[index].x;
p.y = path[index].y;
p.yaw = path[index].yaw;
p.K = path[index].K;
return p;
}
// 搜索路徑點, 將小車到起始點的距離與小車到每一個點的距離對比，找出最近的目標點索引值
int Find_target_index(vehicleState state)
{
double min = abs(sqrt(pow(state.x - path[0].x, 2) + pow(state.y - path[0].y, 2)));
int index = 0;
for (int i = 0; i < path.size(); i++)
{
double d = abs(sqrt(pow(state.x - path[i].x, 2) + pow(state.y - path[i].y, 2)));
if (d < min)
{
min = d;
index = i;
}
}
//索引到終點前，當（機器人與下一個目標點的距離Lf）小于（當前目標點到下一個目標點距離L)時，索引下一個目標點
if ((index + 1) < path.size())
{
double current_x = path[index].x; double current_y = path[index].y;
double next_x = path[index + 1].x; double next_y = path[index + 1].y;
double L_ = abs(sqrt(pow(next_x - current_x, 2) + pow(next_y - current_y, 2)));
double L_1 = abs(sqrt(pow(state.x - next_x, 2) + pow(state.y - next_y, 2)));
//ROS_INFO("L is %f,Lf is %f",L,Lf);
if (L_1 < L_)
{
index += 1;
}
}
return index;
}
};
class LQR_node {
private:
vehicleState car;//小車狀態(tài)
double Q[3];
double R[2];
int lastIndex;//最后一個點索引值
waypoint lastPoint;//最后一個點信息
vector<double> x,y,x_p,y_p,v_a,v_d,kesi_a,kesi_d;
public:
LQR* controller = new LQR();
Path* path = new Path();
trajectory* trajec = new trajectory();
LQR_node()//初始化中添加軌跡、小車初始位姿
{
//ROS:
addpointcallback();
//robot:
car.x = -1.325;
car.y = 2.562;
car.yaw = 0.964;
car.v = 0.0;
car.kesi = 0.1;
}
~LQR_node() {
free(controller);
free(path);
free(trajec);
}
void addpointcallback(){
trajec->refer_path();
vector<waypoint> waypoints = trajec->get_path();
path->Add_new_point(waypoints);
cout << "path size is:" << path->Size() << endl;
lastIndex = path->Size() - 1;
lastPoint = path->Get_waypoint(lastIndex);
}
double slow_judge(double distance) {
if (distance>=5.0&&distance <= 15.0) {
return 0.35;
}
else if (distance>=0.1&&distance < 5.0) {
return 0.15;
}
else if (distance < 0.1) {
printf("reach goal!\n");
plot_();
}
else
{
return V_DESIRED;
}
}
//控制器流程
void LQR_track() {
U U_r;
waypoint Point;
//搜索路徑點
int target_index = path->Find_target_index(car);
printf("target index is : %d\n", target_index);
//獲取路徑點信息，構造期望控制量
Point = path->Get_waypoint(target_index);
//printf("waypoint information is x:%f,y:%f,yaw:%f,K:%f\n", Point.x, Point.y, Point.yaw, Point.K);
//減速判斷
double kesi = atan2(L * Point.K, 1);
double v_distance = abs(sqrt(pow(car.x - lastPoint.x, 2) + pow(car.y - lastPoint.y, 2)));
printf("the distance is %f\n", v_distance);
U_r.v = slow_judge(v_distance);U_r.kesi = kesi;
printf("the desired v is: %f,the desired kesi is: %f\n", U_r.v,U_r.kesi);
//權重矩陣
Q[0] = 1.0; Q[1] = 1.0; Q[2] = 1.0;
R[0] = 4.0; R[1] = 4.0;
//使用LQR控制器
controller->initial(L, T, car, Point, U_r, Q, R);//初始化控制器
U control = controller->cal_vel();//計算輸入[v, kesi]
printf("the speed is: %f,the kesi is: %f\n", control.v, control.kesi);
printf("the car position is x: %f, y: %f\n", car.x, car.y);
//儲存小車位姿用來畫圖
x.push_back(car.x);
y.push_back(car.y);
v_a.push_back(car.v);
v_d.push_back(U_r.v);
kesi_a.push_back(car.kesi);
kesi_d.push_back(U_r.kesi);
//小車位姿狀態(tài)更新
car = update_state(control, car);
}
//控制啟停函數(shù)
void control() {
int i = 0;
while (i < 10000) {
LQR_track();
i++;
}
}
//畫圖程序
void plot_() {
vector<double> time;
for (int i = 0; i < path->Size(); i++) {
x_p.push_back(path->Get_waypoint(i).x);
y_p.push_back(path->Get_waypoint(i).y);
}
for (int j = 0; j < v_a.size(); j++) {
time.push_back(double(j));
}
plt::subplot(3, 1, 1);
plt::title("Car position");
plt::plot(x_p, y_p, "-k", x, y, "-.r");
plt::subplot(3, 1, 2);
plt::title("Car speed");
plt::plot(time, v_d, "-k", time, v_a, "-.r");
plt::subplot(3, 1, 3);
plt::title("Car kesi");
plt::plot(time, kesi_d, "-k", time, kesi_a, "-.r");
plt::show();
exit(0);
}
};
int main(char argc, char* argv) {
LQR_node* node = new LQR_node();
node->control();
return 0;
}
?
仿真測試結果：
?????????三幅圖分別為車輛位置、車輛速度、車輛前輪轉角。黑色實線為期望值，紅色虛線為實際值。速度為0.5，后面有減速處理，可以看到效果還是挺不錯的。
?
?重要的說明??！
1、解黎卡提方程的問題
????????在大佬python版本中發(fā)現(xiàn)了一個問題，經(jīng)過解黎卡提方程后，得到的矩陣K只迭代了一次(如下圖)，這個地方我想了挺久的，最后還是按照迭代法求黎卡提（Riccati）方程的解這篇博客來解的，判定收斂的條件是無窮范數(shù)。
?
?2.初始誤差的選擇(小車與軌跡起點的距離)
在調試過程中發(fā)現(xiàn)了一個很有意思的現(xiàn)象，采樣時間與允許初始誤差范圍有關系，太大的初始誤差可能會導致跟蹤失敗，仿真的期望速度為0.5m/s：
| 采樣時間(s) | 允許初始誤差范圍(m) |
| 0.5 | ±5.0 |
| 0.1 | ±10.0 |
| 0.05 | ±11.0 |
????????測試了三種調試頻率，2Hz，10Hz，20Hz，在實驗平臺上一般使用20Hz的，不過取極限初始誤差意義不是很大，自動駕駛里面不可能在離車輛11m的地方開始規(guī)劃路徑。。。但還是可以測試控制器極限性能。
3.權重的選擇
????????這個問題我調了很久，最后發(fā)現(xiàn)Q權重一定要比R小，不然速度就會提前計算到0，或者前輪轉角值異常。我也不知道是怎么回事，相同的控制率算法寫到python里面沒有問題，C++有問題，就令我很費解。
4.完整代碼
完整代碼請見：LQR_cpp
5.后續(xù)工作
????????過幾天會把這個項目弄到實驗平臺上進行仿真和實驗。?文章來源：http://www.zghlxwxcb.cn/news/detail-422686.html
?
到了這里，關于Visual studio C++：LQR軌跡跟蹤仿真的文章就介紹完了。如果您還想了解更多內容，請在右上角搜索TOY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關文章，希望大家以后多多支持TOY模板網(wǎng)！
