本文讲解线性二次优化器LQR的原理和C++的代码实现，同时在CARLA-ROS联合仿真环境验证算法效果。
文章目录
- 前言
- 一、LQR的原理
- 1.1 一个小例子
- 1.2 黎卡提方程求解LQR问题
- 二、车辆动力学模型
- 2.1 自行车模型
- 2.2 基于LQR的轨迹追踪
- 三、LQR代码
- 总结
前言
本文讲解线性二次优化器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（xTPx）展开，同时把右边的式子挪到左边来，对方程进行整理，同时假设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 steer_angle = atan2_to_PI(20.0); } else if (steer_angle steer_angle = -atan2_to_PI(20.0); } // Set the steer commands cmd.steer_target = steer_angle; std::cout // LateralControlError lat_con_err; // 将其更改为智能指针 std::shared_ptr 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 double d_min = PointDistanceSquare(trajectory_points_.front(), x, y); size_t index_min = 0; for (size_t i = 1; i
