网站页脚模板,wordpress 用户组可见,网站建设 cn,买机票便宜的网站建设参考博客#xff1a; #xff08;1#xff09;LQR的理解与运用 第一期——理解篇 #xff08;2#xff09;线性二次型调节器(LQR)原理详解 #xff08;3#xff09;LQR控制基本原理#xff08;包括Riccati方程具体推导过程#xff09; #xff08;4#xff09;【基础…参考博客 1LQR的理解与运用 第一期——理解篇 2线性二次型调节器(LQR)原理详解 3LQR控制基本原理包括Riccati方程具体推导过程 4【基础】自动驾驶控制算法第五讲 连续方程的离散化与离散LQR原理
0 前言
LQR线性二次调节器设计状态反馈控制器的方法
1 LQR算法原理
系统 x ˙ A x B u \dot xAxBu x˙AxBu 线性反馈控制器 u − K x u-Kx u−Kx 让系统稳定的条件是矩阵 A c l A_{cl} Acl的特征值实部均为负数。因此我们可以手动选择几个满足上述条件的特征值然后反解出K从而得到控制器。
代价函数 J J J 在系统稳定的前提下通过设计合适的K让代价函数J最小。 Q大希望状态变量x更快收敛 R大希望输入量u收敛更快以更小的代价实现系统稳定
1.1 连续时间LQR推导
具体推导参见博客线性二次型调节器(LQR)原理详解 求解连续时间LQR反馈控制器参数K的过程 1设计参数矩阵Q、R 2求解Riccati方程 A T P P A − P B R − 1 B T P Q 0 A^TPPA-PBR^{-1}B^TPQ0 ATPPA−PBR−1BTPQ0得到P 3计算 K R − 1 B T P KR^{-1}B^TP KR−1BTP得到反馈控制量 u − k x u-kx u−kx
1.2 离散时间LQR推导
离散情况下的LQR推导有最小二乘法和动态规划算法 详细推导见博客连续时间LQR和离散时间LQR笔记 离散系统 x ( K 1 ) A x ( k ) B u ( k ) x(K1)Ax(k)Bu(k) x(K1)Ax(k)Bu(k) 代价函数 设计步骤 ① 确定迭代范围N ② 设置迭代初始值 P N Q P_NQ PNQ ③ t N , . . . , 1 tN,...,1 tN,...,1从后向前循环迭代求解离散时间的代数Riccati方程 P t − 1 Q A T P t A − A T P t B ( R B T P t 1 B ) − 1 B T P t A P_{t-1}QA^TP_tA-A^TP_tB(RB^TP_{t1}B)^{-1}B^TP_tA Pt−1QATPtA−ATPtB(RBTPt1B)−1BTPtA ④ t 0 , . . . , N t0,...,N t0,...,N循环计算反馈系数 K t ( R B T P t 1 B ) − 1 B T P t 1 A K_t(RB^TP_{t1}B)^{-1}B^TP_{t1}A Kt(RBTPt1B)−1BTPt1A 得到控制量 u t − K t x t u_t-K_tx_t ut−Ktxt
2 LQR代码
主要步骤 1确定迭代范围N预设精度EPS 2设置迭代初始值P QfQf Q 3循环迭代 t 1 , . . . , N t1,...,N t1,...,N P n e w Q A T P A − A T P B ( R B T P B ) − 1 B T P A P _{new} QA ^TPA−A ^TPB(RB ^T PB) ^{−1}B ^TPA PnewQATPA−ATPB(RBTPB)−1BTPA 若 ∣ ∣ P n e w − P ∣ ∣ E P S ||P_{new}-P||EPS ∣∣Pnew−P∣∣EPS跳出循环否则 P P n e w PP_{new} PPnew 4计算反馈系数 K ( R B T P n e w B ) − 1 B T P n e w A K(R B^TP_{new}B)^{-1}B^TP_{new}A K(RBTPnewB)−1BTPnewA 5最终的优化控制量 u ∗ − K x u^*-Kx u∗−Kx
Reference_path.h
#pragma once#include iostream
#include vector
#include cmath
#include algorithm
#include Eigen/Denseusing namespace std;
using namespace Eigen;#define PI 3.1415926struct refTraj
{MatrixXd xref, dref;int ind;
};struct parameters
{int L;int NX, NU, T;double dt;
};class ReferencePath
{
public:ReferencePath();vectordouble calcTrackError(vectordouble robot_state);double normalizeAngle(double angle);// 计算参考轨迹点统一化变量数组便于后面MPC优化使用.refTraj calc_ref_trajectory(vectordouble robot_state, parameters param, double dl 1.0);public:vectorvectordouble ref_path; // x, y, 切线方向, kvectordouble ref_x, ref_y;
};Reference_path.cpp
#include Reference_path.hReferencePath::ReferencePath()
{ref_path vectorvectordouble(1000, vectordouble(4));// 生成参考轨迹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]);}double dx, dy, ddx, ddy;for (int i 0; i ref_path.size(); i){if (i 0) {dx ref_path[i 1][0] - ref_path[i][0];dy ref_path[i 1][1] - ref_path[i][1];ddx ref_path[2][0] ref_path[0][0] - 2 * ref_path[1][0];ddy ref_path[2][1] ref_path[0][1] - 2 * ref_path[1][1];} else if (i ref_path.size() - 1) {dx ref_path[i][0] - ref_path[i- 1][0];dy ref_path[i][1] - ref_path[i- 1][1];ddx ref_path[i][0] ref_path[i- 2][0] - 2 * ref_path[i - 1][0];ddy ref_path[i][1] ref_path[i - 2][1] - 2 * ref_path[i - 1][1];} else {dx ref_path[i 1][0] - ref_path[i][0];dy ref_path[i 1][1] - ref_path[i][1];ddx ref_path[i 1][0] ref_path[i - 1][0] - 2 * ref_path[i][0];ddy ref_path[i 1][1] ref_path[i - 1][1] - 2 * ref_path[i][1];}ref_path[i][2] atan2(dy, dx);//计算曲率:设曲线r(t) (x(t),y(t)),则曲率k(xy - xy)/((x)^2 (y)^2)^(3/2).ref_path[i][3] (ddy * dx - ddx * dy) / pow((dx * dx dy * dy), 3.0 / 2); // k计算}
}
// 计算跟踪误差
vectordouble ReferencePath::calcTrackError(vectordouble robot_state)
{double x robot_state[0], y robot_state[1];vectordouble d_x(ref_path.size()), d_y(ref_path.size()), d(ref_path.size());for (int i 0; i ref_path.size(); i){d_x[i]ref_path[i][0]-x;d_y[i]ref_path[i][1]-y;d[i] sqrt(d_x[i]*d_x[i]d_y[i]*d_y[i]);}double min_index min_element(d.begin(), d.end()) - d.begin();double yaw ref_path[min_index][2];double k ref_path[min_index][3];double angle normalizeAngle(yaw - atan2(d_y[min_index], d_x[min_index]));double error d[min_index];if (angle 0) error * -1;return {error, k, yaw, min_index};
}double ReferencePath::normalizeAngle(double angle)
{while (angle PI){angle - 2 * PI;}while (angle -PI){angle 2 * PI;}return angle;
}// 计算参考轨迹点统一化变量数组只针对MPC优化使用
// robot_state 车辆的状态(x,y,yaw,v)
refTraj ReferencePath::calc_ref_trajectory(vectordouble robot_state, parameters param, double dl)
{vectordouble track_error calcTrackError(robot_state);double e track_error[0], k track_error[1], ref_yaw track_error[2], ind track_error[3];refTraj ref_traj;ref_traj.xref MatrixXd(param.NX, param.T 1);ref_traj.dref MatrixXd (param.NU,param.T);int ncourse ref_path.size();ref_traj.xref(0,0)ref_path[ind][0];ref_traj.xref(1,0)ref_path[ind][1];ref_traj.xref(2,0)ref_path[ind][2];//参考控制量[v,delta]double ref_delta atan2(k * param.L, 1);for(int i0;iparam.T;i){ref_traj.dref(0,i)robot_state[3];ref_traj.dref(1,i)ref_delta;}double travel 0.0;for(int i 0; i param.T 1; i){travel abs(robot_state[3]) * param.dt;double dind (int)round(travel / dl);if(ind dind ncourse){ref_traj.xref(0,i)ref_path[ind dind][0];ref_traj.xref(1,i)ref_path[ind dind][1];ref_traj.xref(2,i)ref_path[ind dind][2];}else{ref_traj.xref(0,i)ref_path[ncourse-1][0];ref_traj.xref(1,i)ref_path[ncourse-1][1];ref_traj.xref(2,i)ref_path[ncourse-1][2];}}return ref_traj;
}LQR.h
#pragma once#define EPS 1.0e-4
#include Eigen/Dense
#include vector
#include iostream
using namespace std;
using namespace Eigen;class LQR {
private:int N;
public:LQR(int n);MatrixXd calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R);double LQRControl(vectordouble robot_state, vectorvectordouble ref_path, double s0, MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R);
};LQR.cpp
#include LQR.hLQR::LQR(int n) : N(n) {}
// 解代数里卡提方程
MatrixXd LQR::calRicatti(MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R)
{MatrixXd Qf Q;MatrixXd P_old Qf;MatrixXd P_new;// P _{new} QA ^TPA−A ^TPB(RB ^T PB) ^{−1}B ^TPAfor (int i 0; i N; i){P_new Q A.transpose() * P_old * A - A.transpose() * P_old * B * (R B.transpose() * P_old * B).inverse() * B.transpose() * P_old * A;if ((P_new - P_old).maxCoeff() EPS (P_old - P_new).maxCoeff() EPS) break;P_old P_new;}return P_new;
}double LQR::LQRControl(vectordouble robot_state, vectorvectordouble ref_path, double s0, MatrixXd A, MatrixXd B, MatrixXd Q, MatrixXd R)
{MatrixXd X(3, 1);X robot_state[0] - ref_path[s0][0],robot_state[1] - ref_path[s0][1],robot_state[2] - ref_path[s0][2];MatrixXd P calRicatti(A, B, Q, R);// K(R B^TP_{new}B)^{-1}B^TP_{new}AMatrixXd K (R B.transpose() * P * B).inverse() * B.transpose() * P * A;MatrixXd u -K * X; // [v - ref_v, delta - ref_delta]return u(1, 0);
}main.cpp
#include LQR.h
#include KinematicModel.h
#include matplotlibcpp.h
#include Reference_path.h
#include pid_controller.hnamespace plt matplotlibcpp;int main()
{int N 500; // 迭代范围double Target_speed 7.2 / 3.6;MatrixXd Q(3,3);Q 3,0,0,0,3,0,0,0,3;MatrixXd R(2,2);R 2.0,0.0,0.0,2.0;//保存机器人小车运动过程中的轨迹vectordouble x_, y_;ReferencePath referencePath;KinematicModel model(0, 1.0, 0, 0, 2.2, 0.1);PID_controller PID(3, 0.001, 30, Target_speed, 15.0 / 3.6, 0.0);LQR lqr(N);vectordouble robot_state;for (int i 0; i 700; i){plt::clf();robot_state model.getState();vectordouble one_trial referencePath.calcTrackError(robot_state);double k one_trial[1], ref_yaw one_trial[2], s0 one_trial[3];double ref_delta atan2(k * model.L, 1); // L 2.2vectorMatrixXd state_space model.stateSpace(ref_delta, ref_yaw);double delta lqr.LQRControl(robot_state, referencePath.ref_path, s0, state_space[0], state_space[1], Q, R);delta delta ref_delta;double a PID.calOutput(model.v);model.updateState(a, delta);cout Speed: model.v m/s endl;x_.push_back(model.x);y_.push_back(model.y);//画参考轨迹plt::plot(referencePath.ref_x, referencePath.ref_y, b--);plt::grid(true);plt::ylim(-5, 5);plt::plot(x_, y_, r);plt::pause(0.01);}const char* filename ./LQR.png;plt::save(filename);plt::show();return 0;
}CMakeList.txt
cmake_minimum_required(VERSION 3.0.2)
project(LQR)## Compile as C11, supported in ROS Kinetic and newer
# add_compile_options(-stdc11)## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTSroscppstd_msgs
)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)catkin_package(
# INCLUDE_DIRS include
# LIBRARIES huatu
# CATKIN_DEPENDS roscpp std_msgs
# DEPENDS system_lib
)include_directories(include${PYTHON2.7_INLCUDE_DIRS}
)add_executable(lqr_controller src/LQR.cppsrc/KinematicModel.cppsrc/main.cppsrc/pid_controller.cppsrc/Reference_path.cpp)
target_link_libraries(lqr_controller ${PYTHON2.7_LIB})3 PID vs Pure pursuit vs Stanley vs LQR
横向控制算法 1PID鲁棒性较差对路径无要求转弯不会内切速度增加会有一定超调速度增加稳态误差变大适用场景路径曲率较小及低速的跟踪场景
2Pure pursuit鲁棒性较好对路径无要求转弯内切速度增加变得严重速度增加会有一定超调速度增加稳态误差变大适用场景路径连续或不连续或者低速的跟踪场景
3Stanley鲁棒性好对路径要求曲率连续转弯不会内切速度增加会有一定超调速度增加稳态误差变大适用场景路径平滑的中低速跟踪场景
4LQR鲁棒性较差对路径要求曲率连续不会转弯内切曲率快速变化时超调严重稳态误差小除非速度特别大适用场景路径平滑的中高速城市驾驶跟踪场景
