自动驾驶控制与规划——Project 3: LQR车辆横向控制

睡觉狂魔er2024-12-23 22:41

目录

零、任务介绍

  1. 补全src/ros-bridge/carla_shenlan_projects/carla_shenlan_lqr_pid_controller/src/lqr_controller.cpp中的TODO部分，实现基于LQR的车辆横向控制。

一、系统建模

1.1 连续模型

选取系统状态变量为 x = [ e c g , e ˙ c g , e θ , e ˙ θ ] ⊤ x = [e_{cg}, \dot e_{cg}, e_\theta, \dot e_\theta]^\top x=[ecg,e˙cg,eθ,e˙θ]⊤，控制输入为 u = δ u = \delta u=δ。系统参考轨迹每个点可以给出位置、偏航角、速度、加速度、曲率，这些量可以表示为沿轨迹行驶路程 s s s的函数。车辆的横向误差动力学可以用如下的线性模型描述
x ˙ = A x + B 1 δ + B 2 r d e s \dot x = Ax + B_1\delta + B_2 r_{des} x˙=Ax+B1δ+B2rdes

其中
A = [ 0 1 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 1 0 l r c r − l f c f I z v l r c r − l f c f I z − ( l f 2 c f + l r 2 c r ) I z v ] A = \left[\begin{matrix} 0 & 1 & 0 & 0\\ 0 & -\frac{(c_f + c_r)}{mv} & \frac{c_f + c_r}{m} & \frac{l_r c_r - l_f c_f}{mv}\\ 0 & 0 & 0 & 1\\ 0 & \frac{l_r c_r - l_f c_f}{I_z v} & \frac{l_r c_r - l_f c_f}{I_z} & -\frac{(l_f^2 c_f + l_r^2 c_r)}{I_z v} \end{matrix}\right] A= 00001−mv(cf+cr)0Izvlrcr−lfcf0mcf+cr0Izlrcr−lfcf0mvlrcr−lfcf1−Izv(lf2cf+lr2cr)

B 1 = [ 0 c f / m 0 l f c f / I z ] B_1 = \left[\begin{matrix} 0\\ c_f / m\\ 0\\ l_f c_f /I_z \end{matrix}\right] B1= 0cf/m0lfcf/Iz

B 2 = [ 0 ( l r c r − l f c f ) / ( m v ) − v 0 − ( l f 2 c f + l r 2 c r ) / ( I z v ) ] B_2 = \left[ \begin{matrix} 0\\ (l_r c_r - l_f c_f)/(mv) - v\\ 0\\ -(l_f^2 c_f + l_r^2 c_r)/(I_z v) \end{matrix} \right] B2= 0(lrcr−lfcf)/(mv)−v0−(lf2cf+lr2cr)/(Izv)

这里实际上是假设在一小段时间内车辆纵向速度 v v v保持不变，然后在工作点线性化，参考轨迹的偏航角速度 r d e s r_{des} rdes可以由参考点速度 v d e s v_{des} vdes和参考点曲率 κ d e s \kappa_{des} κdes计算
r d e s = v d e s κ d e s r_{des} = v_{des} \kappa_{des} rdes=vdesκdes

1.2 离散化

实际实现时，需要将系统离散化，离散化可以采取前向欧拉法和零阶保持器精确离散化等方法。采样周期为 T T T，前向欧拉法得到的离散系统矩阵和输入矩阵为
A d = I + T A B d 1 = T B 1 B d 2 = T B 2 \begin{aligned} A_d &= I + TA \\ B_{d1} &= TB_1 \\ B_{d2} &= TB_2 \end{aligned} AdBd1Bd2=I+TA=TB1=TB2

为了达到更好的准确性和稳定性，可以使用零阶保持器进行精确离散化，精确离散化的计算公式如下：
A d = e A T B d 1 = ( ∫ 0 T e A τ d τ ) B 1 B d 2 = ( ∫ 0 T e A τ d τ ) B 2 \begin{aligned} A_d &= e^{AT}\\ B_{d1} &= (\int_0^T e^{A\tau} d\tau)B_1 \\ B_{d2} &= (\int_0^T e^{A\tau} d\tau)B_2 \end{aligned} AdBd1Bd2=eAT=(∫0TeAτdτ)B1=(∫0TeAτdτ)B2

矩阵指数可以用如下的拉普拉斯逆变换得到
e A t = L − 1 [ ( s I − A ) − 1 ] e^{At} = \mathcal L^{-1}[(sI - A)^{-1}] eAt=L−1[(sI−A)−1]

本次project中的参数如下

参数名称 符号 数值
前轮侧偏刚度（左右轮之和） c f c_f cf 155494.663
后轮侧偏刚度（左右轮之和） c r c_r cr 155494.663
前轮到质心的距离 l f l_f lf 1.426
后轮到质心的距离 l r l_r lr 1.426
整车质量 m m m 1845.0
Z轴转动惯量 I z I_z Iz 3751.76
控制周期 t s t_s ts 0.01

使用mathematica计算矩阵拉普拉斯反变换

mathematica 复制代码 
ts = 0.01;
cf = 155494.663;
cr = 155494.663;
mfl = 1845.0/4;
mfr = 1845.0/4;
mrl = 1845.0/4;
mrr = 1845.0/4;
mf = mfl + mfr;
mr = mrl + mrr;
m = mf + mr
wheelbase = 2.852;
lf = wheelbase * (1.0 - mf/m)
lr = wheelbase * (1.0 - mr /m)

iz = lf*lf*mf + lr*lr*mr

A = {
  {0, 1, 0, 0},
  {0, -(cf + cr)/(m*v), (cf + cr)/m, (lr*cr - lf*cf)/(m*v)},
  {0, 0, 0, 1},
  {0, (lr*cr - lf*cf)/(iz * v), (lr*cr - lf*cf)/
    iz, -(lf^2*cf + lr^2*cr)/(iz * v)}
  }
  InverseLaplaceTransform[Inverse[s*IdentityMatrix[4] - A], s, 0.01]

计算得到的矩阵 A d A_d Ad如下
A d = ( 1.0 v ( 0.00593268 − 0.00593268 e − 1.68558 / v ) v ( ( 0.00593268 e − 1.68558 / v − 0.00593268 ) v + 0.01 ) 168.558 ( e − 1.68558 / v ( 4.176213162156246 × 1 0 − 7 v + 3.519668608485906 × 1 0 − 7 ) − 4.176213162156246 × 1 0 − 7 v + 3.519668608485956 × 1 0 − 7 ) v 2 0. 1. e − 1.68558 / v v ( 1. − 1. e − 1.68558 / v ) 168.558 ( e − 1.68558 / v ( − 0.0000351967 v − 0.0000593268 ) + 0.0000351967 v ) v 0. 0. 1. e − 1.68558 / v ( − 0.00593268 v − 3.3723378595376876 × 1 0 − 18 ) + 0.00593268 v 0. 0. 0. e − 1.68558 / v ( 1. v + 2.842170943040401 × 1 0 − 16 ) v ) A_d = \left( \begin{array}{cccc} 1.0 & v \left(0.00593268 - 0.00593268 e^{-1.68558/v}\right) & v \left(\left(0.00593268 e^{-1.68558/v}-0.00593268\right) v + 0.01\right) & 168.558 \left(e^{-1.68558/v} \left(4.176213162156246 \times 10^{-7} v + 3.519668608485906 \times 10^{-7}\right) - 4.176213162156246 \times 10^{-7} v + 3.519668608485956 \times 10^{-7}\right) v^2 \\ 0. & 1. e^{-1.68558/v} & v \left(1. - 1. e^{-1.68558/v}\right) & 168.558 \left(e^{-1.68558/v} \left(-0.0000351967 v - 0.0000593268\right) + 0.0000351967 v\right) v \\ 0. & 0. & 1. & e^{-1.68558/v} \left(-0.00593268 v - 3.3723378595376876 \times 10^{-18}\right) + 0.00593268 v \\ 0. & 0. & 0. & \frac{e^{-1.68558/v} \left(1. v + 2.842170943040401 \times 10^{-16}\right)}{v} \end{array} \right) Ad= 1.00.0.0.v(0.00593268−0.00593268e−1.68558/v)1.e−1.68558/v0.0.v((0.00593268e−1.68558/v−0.00593268)v+0.01)v(1.−1.e−1.68558/v)1.0.168.558(e−1.68558/v(4.176213162156246×10−7v+3.519668608485906×10−7)−4.176213162156246×10−7v+3.519668608485956×10−7)v2168.558(e−1.68558/v(−0.0000351967v−0.0000593268)+0.0000351967v)ve−1.68558/v(−0.00593268v−3.3723378595376876×10−18)+0.00593268vve−1.68558/v(1.v+2.842170943040401×10−16)

通过观察可以得到，矩阵 A A A不可逆，因此计算 B d B_d Bd时采用幂级数展开求 e A τ e^{A\tau} eAτ的积分，即
B d 1 = ∫ 0 T e A τ d τ B 1 = ∫ 0 T ( I + A τ + ( A τ ) 2 2 ! + ( A τ ) 3 3 ! + ⋯   ) d τ B 1 = ( T I + T 2 2 A + T 3 6 A 2 + ⋯   ) B 1 ≈ T B 1 \begin{aligned} B_{d1} &= \int_0^T e^{A\tau} d\tau B_1 \\ &= \int_0^T(I + A\tau + \frac{(A\tau)^2}{2!} + \frac{(A\tau)^3}{3!} + \cdots)d\tau B_1 \\ & = (TI + \frac{T^2}{2}A + \frac{T^3}{6}A^2 + \cdots) B_1 \\ &\approx TB_1 \end{aligned} Bd1=∫0TeAτdτB1=∫0T(I+Aτ+2!(Aτ)2+3!(Aτ)3+⋯)dτB1=(TI+2T2A+6T3A2+⋯)B1≈TB1

类似地有 B d 2 = T B 2 B_{d2} = TB_2 Bd2=TB2。

二、算法

2.1 离散时间LQR

离散时间LQR的最优控制律为
u ∗ ( k ) = − K x ( k ) u^*(k) = - K x(k) u∗(k)=−Kx(k)

其中反馈增益 K K K为
K = ( R + B d ⊤ P B d ) − 1 B d ⊤ P A d K = (R + B_d^\top P B_d)^{-1} B_d^\top P A_d K=(R+Bd⊤PBd)−1Bd⊤PAd

其中正定矩阵P可以使用动态规划算法得出，迭代形式如下
P t − 1 = A d ⊤ P t A d − A d ⊤ P t B d ( R + B d ⊤ P t B d ) − 1 B d ⊤ P t A d + Q P_{t-1} = A_d^\top P_{t} A_d - A_d^\top P_{t} B_d (R + B_d^\top P_{t} B_d)^{-1}B_d^\top P_{t} A_d + Q Pt−1=Ad⊤PtAd−Ad⊤PtBd(R+Bd⊤PtBd)−1Bd⊤PtAd+Q

此处的推导细节参考基础算法 - LQR - 离散时间有限边界

2.2 前馈控制

采用上述状态反馈控制律后，系统的模型可以改写成
x ˙ = ( A − B 1 K ) x + B 2 r d e s \dot x = (A - B_1 K) x + B_2 r_{des} x˙=(A−B1K)x+B2rdes

闭环系统 A − B 1 K A - B_1K A−B1K是渐进稳定的，但是由于 B 2 r d e s B_2 r_{des} B2rdes的存在，无法保证系统状态量收敛至0。因此需要在控制律中加入前馈项
δ = − K x + δ f f \delta = -Kx + \delta_{ff} δ=−Kx+δff

前馈项的设计参考自动驾驶控制算法 ------ 横向LQR控制+前馈控制
δ f f = L κ + K v a y − k 3 κ ( l r − l f m v x 2 2 c α r L ) \begin{aligned} \delta_{ff} = L \kappa + K_v a_y - k_3 \kappa (l_r - \frac{l_f mv_x^2}{2c_{\alpha r} L}) \end{aligned} δff=Lκ+Kvay−k3κ(lr−2cαrLlfmvx2)

其中
K v = l r m 2 c α f ( l f + l r ) − l f m 2 c α r ( l f + l r ) K_v = \frac{l_r m}{2 c_{\alpha f} (l_f + l_r)} - \frac{l_f m}{2 c_{\alpha r} (l_f + l_r)} Kv=2cαf(lf+lr)lrm−2cαr(lf+lr)lfm

三、代码实现

计算控制量

cpp 复制代码 
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
    double v = max(localization.velocity, minimum_speed_protection_);    // 避免除0
    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;

    /*
    b = [0.0, c_f / m, 0.0, l_f * c_f / i_z]^T
    */
    // TODO 02 动力矩阵B
    matrix_bd_ = matrix_b_ * ts_;
    // 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;
    cout << "matrix_k * matrix_state: " << (matrix_k_ * matrix_state_) << endl;
    double steer_angle_feedback = -(matrix_k_ * matrix_state_)(0, 0);

    // TODO 07 计算前馈控制，计算横向转角的反馈量
    double steer_angle_feedforward = 0.0;
    steer_angle_feedforward = ComputeFeedForward(localization, ref_curv_);
    double steer_angle = steer_angle_feedback - 0.9 * steer_angle_feedforward;
    cout << "steer_angle_feedforward: " << steer_angle_feedforward << 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;

    return true;
}

计算横向误差

cpp 复制代码 
// 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 target_point = QueryNearestPointByPosition(x, y);

    double dx = target_point.x - x;
    double dy = target_point.y - y;
    double theta_p = target_point.heading;

    double e_cg = cos(theta_p) * dy - sin(theta_p) * dx;
    double e_theta = NormalizeAngle(theta_p - theta);
    double diff_e_cg = linear_v * sin(e_theta);
    double diff_e_theta = angular_v - target_point.kappa * target_point.v;

    lat_con_err->heading_error = e_theta;
    lat_con_err->lateral_error = e_cg;
    lat_con_err->heading_error_rate = diff_e_theta;
    lat_con_err->lateral_error_rate = diff_e_cg;
}

更新状态矩阵并离散化

cpp 复制代码 
// TODO 04 更新状态矩阵A并将状态矩阵A离散化
void LqrController::UpdateMatrix(const VehicleState &vehicle_state) {
    double v = max(vehicle_state.velocity, minimum_speed_protection_);
    // 更新A矩阵
    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;
    // 更新A_d矩阵，前向欧拉法
    // matrix_ad_ = Matrix::Identity(basic_state_size_, basic_state_size_) + matrix_a_ * ts_;
    // 更新A_d矩阵，ZOH精确离散化
    // 计算指数项
    double exp_term = exp(-1.68558 / v);

    // 使用ZOH精确离散化
    matrix_ad_(0, 0) = 1.0;
    matrix_ad_(0, 1) = 0.00593268 * (1.0 - exp_term);
    matrix_ad_(0, 2) = 0.010000000000000142 + (-0.00593268 + 0.00593268 * exp_term) / v;
    matrix_ad_(0, 3) = 168.55790027100272 * (3.519668608485956e-7 + exp_term * (3.519668608485906e-7 + 4.176213162156246e-7 * v)) - 4.176213162156246e-7 * v;

    matrix_ad_(1, 0) = 0.0;
    matrix_ad_(1, 1) = 1.0 * exp_term;
    matrix_ad_(1, 2) = 168.55790027100272 * (exp_term * (-0.000059326795029614625 - 0.000035196686084859064 * v) + 0.000035196686084859064 * v);
    matrix_ad_(1, 3) = 0.0;

    matrix_ad_(2, 0) = 0.0;
    matrix_ad_(2, 1) = 0.0;
    matrix_ad_(2, 2) = 1.0;
    matrix_ad_(2, 3) = exp_term * (-0.00593268 * v - 3.3723378595376876e-18) + 0.00593268 * v;

    matrix_ad_(3, 0) = 0.0;
    matrix_ad_(3, 1) = 0.0;
    matrix_ad_(3, 2) = 0.0;
    matrix_ad_(3, 3) = exp_term * (1.0 * v + 2.842170943040401e-16) / v;
}

求解LQR问题

cpp 复制代码 
// 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 = Q;
    Matrix P_new = Q;
    Matrix At = A.transpose();
    Matrix Bt = B.transpose();

    for (int i = 0; i < max_num_iteration; i++) {
        P_new = At * P * A - At * P * B * (R + Bt * P * B).inverse() * Bt * P * A + Q;
        double diff = fabs((P_new - P).maxCoeff());
        P = P_new;
        if (diff < tolerance) {
            cout << "diff: " << diff << endl;
            *ptr_K = (R + Bt * P * B).inverse() * Bt * P * A;
            return;
        }
    }
    cout << "exceed max iteration" << endl;
}

计算前馈控制量

cpp 复制代码 
// TODO 07 前馈控制，计算横向转角的反馈量
double LqrController::ComputeFeedForward(const VehicleState &localization, double ref_curvature) {
    if (isnan(ref_curvature)) {
        ref_curvature = 0;
    }
    const double K_v = lr_ * mass_ / (2 * cf_ * (lf_ + lr_));
    const double v = localization.velocity;
    double steer_angle_feedforward = ref_curvature * wheelbase_ 
    	+ K_v * v * v * ref_curvature 
    	- matrix_k_(0, 2) * ref_curvature * (lr_ - lf_ * mass_ * v * v / (2 * cr_ * wheelbase_));
    return steer_angle_feedforward;
}

四、效果展示

ZOH离散化（无前馈）

ZOH离散化（前馈）

前向欧拉法离散化（前馈）

通过观察可以发现

  1. 使用ZOH离散化后，转弯过后的横向振荡明显减少。
  2. 当轨迹的偏航角速度不为0时（转弯），未加入前馈时出现较大的横向误差，加入前馈后横向误差显著减小。

自动驾驶控制与规划------Project 3:LQR车辆横向控制

相关推荐
ZHOU_WUYI12 分钟前
4.metagpt中的软件公司智能体 (ProjectManager 角色)
人工智能·metagpt
靴子学长1 小时前
基于字节大模型的论文翻译（含免费源码）
人工智能·深度学习·nlp
AI_NEW_COME2 小时前
知识库管理系统可扩展性深度测评
人工智能
海棠AI实验室2 小时前
AI的进阶之路：从机器学习到深度学习的演变（一）
人工智能·深度学习·机器学习
hunteritself2 小时前
AI Weekly『12月16-22日』：OpenAI公布o3，谷歌发布首个推理模型，GitHub Copilot免费版上线！
人工智能·gpt·chatgpt·github·openai·copilot
IT古董3 小时前
【机器学习】机器学习的基本分类-强化学习-策略梯度（Policy Gradient，PG）
人工智能·机器学习·分类
centurysee3 小时前
【最佳实践】Anthropic：Agentic系统实践案例
人工智能
mahuifa3 小时前
混合开发环境---使用编程AI辅助开发Qt
人工智能·vscode·qt·qtcreator·编程ai
四口鲸鱼爱吃盐3 小时前
Pytorch | 从零构建GoogleNet对CIFAR10进行分类
人工智能·pytorch·分类
蓝天星空3 小时前
Python调用open ai接口
人工智能·python
热门推荐
01〔AI 绘画〕Stable Diffusion 之 解决绘制多人或面部很小的人物时面部崩坏问题 篇02YOLOv8训练好模型后，追加轮数继续训练、或者提前终止训练，缩减训练轮数03玄机平台应急响应—webshell查杀04DisplayPort AltMode On Type-c协议_Ver2.0(学习笔记)05HCIA-datacom数通题库和录播视频资料06Docker 夺命连环 15 问076个主流的工业3D管道设计软件08校验 GPT-4 真实性的三个经典问题：快速区分 GPT-3.5 与 GPT-4，并提供免费测试网站09(欧拉)openEuler系统添加网卡文件配置流程、(欧拉)openEuler系统手动配置ipv6地址流程、(欧拉)openEuler系统网络管理说明10树莓派 5 AI 套件（Hailo-8L）使用教程