LQR算法原理和代码实现-Toy模板网

这篇具有很好参考价值的文章主要介绍了LQR算法原理和代码实现。希望对大家有所帮助。如果存在错误或未考虑完全的地方，请大家不吝赐教，您也可以点击"举报违法"按钮提交疑问。

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

前言

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

一、LQR的原理

1.1 一个小例子

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

同理在LQR计算过程中，我们的目的就是是得到使代价函数最小的控制量和状态量；
LQR算法原理和代码实现
在上面的小例子中，LQR为一维的问题；在二维的LQR问题中，Q和R为矩阵，在Q矩阵中，如果q1比q2大就意味着第一个状态量x1比第二个状态量x2重要，在R矩阵，如果r1比r2大就意味着第一个控制量u1比第二个状态量u2重要。

1.2 黎卡提方程求解LQR问题

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

前面说过线性二次优化器LQR就是通过最小化损失函数来得到最佳控制量u的，现在我们来最小化损失函数J来求得最优控制量u*；
首先我们假设P矩阵为一个n * n的对称矩阵，并且假设xT * P * x的微分为-J，即：
LQR算法原理和代码实现
我们把左边的d/dt（xTPx）展开，同时把右边的式子挪到左边来，对方程进行整理，同时假设K = R的逆矩阵乘以B矩阵的转置乘以P矩阵，最后得到了黎卡提方程，计算过程如下：

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

二、车辆动力学模型

2.1 自行车模型

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

LQR算法原理和代码实现

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

最终得到如下的形式：
LQR算法原理和代码实现

把坐标y和航向角φ也作为状态量，得到如下的状态方程：
LQR算法原理和代码实现

2.2 基于LQR的轨迹追踪

首先选择横向误差Wθ和航向角误差ecg以及这两个状态量的导数作为描述系统的状态，方向盘打的角度作为控制输入；
LQR算法原理和代码实现
根据上面自行车模型推出来的状态方程和这里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 计算横向误差并且更新状态向量x
    UpdateState(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 Problem
    SolveLQRProblem(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);
    } else if (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;
}

// 计算横向误差并且更新状态向量x
void 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(const double x, const double y, const double theta, const double linear_v, const double angular_v, const double 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(const double x, const double 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, const double 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