一，车辆运动学模型的建立

 如图所示，对于横向控制而言，因变量为，自变量为

将速度沿轴分解得车辆的模型为：

即：

其中，因为是横向控制，故假设是常数

上述车辆运动学模型为非线性模型：

二，车辆运动学模型的线性化：泰勒展开

在参考点处的泰勒展开公式,忽略高次项为：

其中，

令,

则上式可表示为：

三，车辆运动学模型的离散化：前向欧拉

 

其中，

四，基于LQR模型，求解控制量

LQR的代价函数为：

假设,

迭代法求黎卡提方程的解，设置迭代次数和迭代精度，如果在迭代次数范围内满足迭代精度的要求，我们认为该方程收敛，从而求得，则反馈增益为：

反馈量为：

LQR控制理论已经非常成熟，核心是建立模型，对模型进行线性化，并离散化，最终带入公式求解LQR的控制量。

五，实例代码

double lqrComputeCommand(double vx, double x, double y, double yaw, Traj_Point match_point,
                                 double vel, double l, double dt)
{
    double steer = 0.0;
    MatrixXd Q = MatrixXd::Zero(3,3);  
    MatrixXd R = MatrixXd::Zero(1,1);
    Q << 1      , 0    , 0,
         0      , 1    , 0,
         0      , 0    , 1;
    R << 1;

    double curvature = match_point.path_point.kappa;
    if(vel < 0) curvature = -curvature;
    double feed_forword = atan2(l * curvature, 1);

    MatrixXd A = MatrixXd::Zero(3, 3);
    A(0, 0) = 1.0;
    A(0, 2) = -vel*sin(match_point.path_point.yaw)*dt;
    A(1, 1) = 1;
    A(1, 2) = vel*cos(match_point.path_point.yaw)*dt;
    A(2, 2) = 1.0;

    MatrixXd B = MatrixXd::Zero(3,1);
    B(2, 0) = vel*dt/l/pow(cos(feed_forword),2);

    double delta_x = x - match_point.path_point.x;
    double delta_y = y - match_point.path_point.y;
    double delta_yaw = NormalizeAngle(yaw - match_point.path_point.yaw);
    VectorXd dx(3); dx << delta_x, delta_y, delta_yaw;

    double eps = 0.01;
    double diff = std::numeric_limits<double>::max();

    MatrixXd P = Q;
    MatrixXd AT = A.transpose();
    MatrixXd BT = B.transpose();
    int num_iter = 0;
    while(num_iter++ < param_.maxiter && diff > eps)
    {
        MatrixXd Pn = AT * P * A - AT * P * B * (R + BT * P * B).inverse() * BT * P * A + Q;
        diff = ((Pn - P).array().abs()).maxCoeff();
        P = Pn;
    }

    MatrixXd feed_back = -((R + BT * P * B).inverse() * BT * P * A) * dx;
    steer = NormalizeAngle(feed_back(0,0) + feed_forword);
    return steer;
}