LQR算法原理和代码实现

本文讲解线性二次优化器LQR的原理和C++的代码实现，同时在CARLA-ROS联合仿真环境验证算法效果。

文章目录

前言

本文讲解线性二次优化器LQR的原理和C++的代码实现，同时在CARLA-ROS联合仿真环境验证算法效果

一、LQR的原理

1.1 一个小例子

举一个通俗易懂的关于LQR原理的小例子，假设从家里要去公司，现在有如下几种交通方式以及花费的时间和交通成本如下表所示：
那么哪种方式是最佳的方式去公司呢？我们定义代价Cost J = Q * time + R * money；
如果你认为时间比较重要，时间成本低相对来说总的成本就低的话，假设时间的成本权重为30，这里的30就是Q值，你可以认为是对时间的惩罚系数，或者是权重，目的就是让time小，而交通花费成本为1，意思就是Q的值为1，相对于时间来说，金钱成本惩罚低；通过计算，我们可以得到四种出行方式的成本：
我们可以看到Q和R就是time和money的权重，而我们的目的就是优化代价函数COST，使其最优；

同理在LQR计算过程中，我们的目的就是是得到使代价函数最小的控制量和状态量；

在上面的小例子中，LQR为一维的问题；在二维的LQR问题中，Q和R为矩阵，在Q矩阵中，如果q1比q2大就意味着第一个状态量x1比第二个状态量x2重要，在R矩阵，如果r1比r2大就意味着第一个控制量u1比第二个状态量u2重要。

1.2 黎卡提方程求解LQR问题

在LQR问题中，我们引入状态反馈u = - K * x，并把它带入到状态方程和损失函数中，得到如下的形式：



前面说过线性二次优化器LQR就是通过最小化损失函数来得到最佳控制量u的，现在我们来最小化损失函数J来求得最优控制量u；
首先我们假设P矩阵为一个n * n的对称矩阵，并且假设xT * P * x的微分为-J，即：

我们把左边的d/dt（xTP*x）展开，同时把右边的式子挪到左边来，对方程进行整理，同时假设K = R的逆矩阵乘以B矩阵的转置乘以P矩阵，最后得到了黎卡提方程，计算过程如下：

示例：pandas 是基于NumPy 的一种工具，该工具是为了解决数据分析任务而创建的。

二、车辆动力学模型

2.1 自行车模型

解释一下三个方程的由来：第一个方程式y轴方向的加速度等于y轴方向的加速度加上车辆的向心加速度，这里的vx * φ’，φ’就等于角速度，将角速度记为r，向心加速度等于(vx^2) / R，R为半径，而vx / R等于角速度也就是 φ’，所以向心加速度就等于vx * φ’；第二个方程是y轴方向的牛顿第二定律；第三个方程大家可能比较疑惑，这个其实是扭矩的计算公式，扭矩=力 * 力矩 = 转动惯量 * 角加速度；方程的左边是lf * Fyf - lr * Fyr，因为两个力是同方向的，所以扭矩是相反的，你可以这么认为，Fyr使车辆顺时针运动，Fyf使车辆逆时针运动，因此使lf * Fyf - lr * Fyr；而方程的右边是转动惯量Iz * 角加速度φ’‘，这样就看懂了第三个方程。

来解释一下自行车模型中要用到的参数，αf和αr为质心侧偏角，Fyf和Fyr为前轮和后轮受到的侧向力，θvf 和 θvr分别是vf和vr与Vfx以及Vrx的夹角，对上面三个方程中的后两个方程，将Fyf‘’和Fyr’带入方程，同时把侧偏角和侧向力的计算带入方程中，得到如下的方程：
得到方程后，对一些量做近似，简化计算：

最终得到如下的形式：

把坐标y和航向角φ也作为状态量，得到如下的状态方程：

2.2 基于LQR的轨迹追踪

首先选择横向误差Wθ和航向角误差ecg以及这两个状态量的导数作为描述系统的状态，方向盘打的角度作为控制输入；

根据上面自行车模型推出来的状态方程和这里Vy和r的导数的方程推导出来如下的状态方程：

三、LQR代码

// **TODO **计算控制命令 该函数定频调用
bool LqrController::ComputeControlCommand(const VehicleState &localization,const TrajectoryData &planning_published_trajectory, ControlCmd &cmd){// 规划轨迹
    trajectory_points_ = planning_published_trajectory.trajectory_points;/*
    A matrix (Gear Drive)
    [0.0,                               1.0,                            0.0,                                               0.0;
     0.0,            (-(c_f + c_r) / m) / v,                (c_f + c_r) / m,                   (l_r * c_r - l_f * c_f) / m / v;
     0.0,                               0.0,                            0.0,                                               1.0;
     0.0,   ((lr * cr - lf * cf) / i_z) / v,   (l_f * c_f - l_r * c_r) / i_z,   (-1.0 * (l_f^2 * c_f + l_r^2 * c_r) / i_z) / v;]
    */// TODO 01 配置状态矩阵A/*
    b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
    */// TODO 02 动力矩阵B// cout << "matrix_bd_.row(): " << matrix_bd_.rows() << endl;// cout << "matrix_bd_.col(): " << matrix_bd_.cols() << endl;// Update state = [Lateral Error, Lateral Error Rate, Heading Error, Heading Error Rate]// TODO 03 计算横向误差并且更新状态向量xUpdateState(localization);// TODO 04 更新状态矩阵A并将状态矩阵A离散化UpdateMatrix(localization);// cout << "matrix_bd_.row(): " << matrix_bd_.rows() << endl;// cout << "matrix_bd_.col(): " << matrix_bd_.cols() << endl;// TODO 05 Solve Lqr ProblemSolveLQRProblem(matrix_ad_, matrix_bd_, matrix_q_, matrix_r_, lqr_eps_, lqr_max_iteration_,&matrix_k_);// TODO 06 计算feedback, 根据反馈对计算状态变量（误差状态）的时候的符号的理解：K里面的值实际运算中全部为正值，steer = -K *// state，按照代码中采用的横向误差的计算方式，横向误差为正值的时候（state中的第一项为正），参考点位于车辆左侧，车辆应该向左转以减小误差，而根据试验，仿真器中，给正值的时候，车辆向右转，给负值的时候，车辆向左转。//   feedback = - K * state//   Convert vehicle steer angle from rad to degree and then to steer degrees//   then to 100% ratio
    std::cout <<"matrix_k_: "<< matrix_k_ << std::endl;double steer_angle_feedback =0;

    steer_angle_feedback =-(matrix_k_(0,0)*matrix_state_(0,0)+matrix_k_(0,1)*matrix_state_(1,0)+matrix_k_(0,2)*matrix_state_(2,0)+matrix_k_(0,3)*matrix_state_(3,0));// TODO 07 计算前馈控制，计算横向转角的反馈量double steer_angle_feedforward =0.0;
    steer_angle_feedforward =ComputeFeedForward(localization, ref_curv_);

    std::cout <<"steer_angle_feedforward:\t"<< steer_angle_feedforward << std::endl;double steer_angle = steer_angle_feedback -0.9* steer_angle_feedforward;

    std::cout <<"steer_angle Inital:\t"<< steer_angle << std::endl;// 限制前轮最大转角，这里定义前轮最大转角位于 [-20度～20度]if(steer_angle >=atan2_to_PI(20.0)){
        steer_angle =atan2_to_PI(20.0);}elseif(steer_angle <=-atan2_to_PI(20.0)){
        steer_angle =-atan2_to_PI(20.0);}// Set the steer commands
    cmd.steer_target = steer_angle;
    std::cout <<"steer_angle Normalize:\t"<< steer_angle << std::endl;return true;}// 计算横向误差并且更新状态向量xvoid LqrController::UpdateState(const VehicleState &vehicle_state){// LateralControlError lat_con_err;  // 将其更改为智能指针
    std::shared_ptr<LateralControlError> lat_con_err = std::make_shared<LateralControlError>();// 计算横向误差ComputeLateralErrors(vehicle_state.x, vehicle_state.y, vehicle_state.heading, vehicle_state.velocity, vehicle_state.angular_velocity, vehicle_state.acceleration, lat_con_err);// State matrix update;matrix_state_(0,0)= lat_con_err->lateral_error;matrix_state_(1,0)= lat_con_err->lateral_error_rate;matrix_state_(2,0)= lat_con_err->heading_error;matrix_state_(3,0)= lat_con_err->heading_error_rate;// cout << "lateral_error: " << (lat_con_err->lateral_error) << endl;// cout << "heading_error: " << (lat_con_err->heading_error) << endl;}// TODO 04 更新状态矩阵A并将状态矩阵A离散化void LqrController::UpdateMatrix(const VehicleState &vehicle_state){double v = std::max(vehicle_state.velocity, minimum_speed_protection_);matrix_a_(1,1)=matrix_a_coeff_(1,1)/ v;matrix_a_(1,3)=matrix_a_coeff_(1,3)/ v;matrix_a_(3,1)=matrix_a_coeff_(3,1)/ v;matrix_a_(3,3)=matrix_a_coeff_(3,3)/ v;
    Matrix I = Matrix::Identity(basic_state_size_, basic_state_size_);
    matrix_ad_ =(I + matrix_a_ * ts_);}// TODO 07 前馈控制，计算横向转角的反馈量double LqrController::ComputeFeedForward(const VehicleState &localization,double ref_curvature){if(isnan(ref_curvature)){
        ref_curvature=0;}double kv =(lr_*mass_/(2*cf_*(lf_+lr_)))-(lf_*mass_ /(2*cr_*(lf_+lr_)));double v = localization.velocity;double steer_angle_feedforward = ref_curvature * wheelbase_ + kv * v * v * ref_curvature -matrix_k_(0,2)* ref_curvature *(lr_ - lf_ * mass_ * v * v /(2* cr_ * wheelbase_));return steer_angle_feedforward;}// TODO 03 计算误差void LqrController::ComputeLateralErrors(constdouble x,constdouble y,constdouble theta,constdouble linear_v,constdouble angular_v,constdouble linear_a, LateralControlErrorPtr &lat_con_err){

    TrajectoryPoint nearest_point =QueryNearestPointByPosition(x, y);double ref_theta = nearest_point.heading;double dx = nearest_point.x - x;double dy = nearest_point.y - y;double e_theta =NormalizeAngle(ref_theta - theta);
    
    lat_con_err->lateral_error = dy *cos(ref_theta)- dx *sin(ref_theta);
    lat_con_err->lateral_error_rate = linear_v *sin(e_theta);

    lat_con_err->heading_error =NormalizeAngle(ref_theta - theta);
    lat_con_err->heading_error_rate = nearest_point.v * nearest_point.kappa - angular_v ;

    std::cout <<"e_theta : \t"<< e_theta << std::endl;}// 查询距离当前位置最近的轨迹点
TrajectoryPoint LqrController::QueryNearestPointByPosition(constdouble x,constdouble y){double d_min =PointDistanceSquare(trajectory_points_.front(), x, y);size_t index_min =0;for(size_t i =1; i < trajectory_points_.size();++i){double d_temp =PointDistanceSquare(trajectory_points_[i], x, y);if(d_temp < d_min){
            d_min = d_temp;
            index_min = i;}}
    ref_curv_ = trajectory_points_[index_min].kappa;// 对应的最近的轨迹点上的曲率double front_index = index_min +5;if(front_index >= trajectory_points_.size()){
        ref_curv_front_ = trajectory_points_[index_min].kappa;}else{
        ref_curv_front_ = trajectory_points_[front_index].kappa;}return trajectory_points_[index_min];}// TODO 05:求解LQR方程void LqrController::SolveLQRProblem(const Matrix &A,const Matrix &B,const Matrix &Q,const Matrix &R,constdouble tolerance,const uint max_num_iteration, Matrix *ptr_K){// 防止矩阵的维数出错导致后续的运算失败if(A.rows()!= A.cols()|| B.rows()!= A.rows()|| Q.rows()!= Q.cols()|| Q.rows()!= A.rows()|| R.rows()!= R.cols()|| R.rows()!= B.cols()){
        std::cout <<"LQR solver: one or more matrices have incompatible dimensions."<< std::endl;return;}

    Matrix P = Matrix::Zero(basic_state_size_, basic_state_size_);
    Matrix P_next = Matrix::Zero(basic_state_size_, basic_state_size_);
    Matrix AT = A.transpose();
    Matrix BT = B.transpose();double diff =0;for(uint i =0; i < max_num_iteration;++i){
        P_next =(AT*P*A)-(AT*P*B)*(R+BT*P*B).inverse()*(BT*P*A)+ Q;
        diff =fabs((P_next - P).maxCoeff());
        P = P_next;if(diff < tolerance){
            std::cout <<"diff = "<< diff << std::endl;*ptr_K =(R+BT*P*B).inverse()*(BT*P*A);return;}}
    std::cout <<"failed to solver riccati in max"<<  max_num_iteration << std::endl;}}// namespace control}// namespace shenlan

总结

以上就是今天要讲的内容，本文仅仅简单LQR原理和代码，谢谢观看。