本文讲解线性二次优化器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，并把它带入到状态方程和损失函数中，得到如下的形式：
示例：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 计算横向误差并且更新状态向量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
总结
以上就是今天要讲的内容，本文仅仅简单LQR原理和代码，谢谢观看。
文章出处登录后可见！
已经登录？立即刷新
