基于车辆运动学模型的LQR横向控制算法

最新推荐文章于 2025-06-19 20:44:01 发布

原创最新推荐文章于 2025-06-19 20:44:01 发布 · 6.9k 阅读

110 ·

CC 4.0 BY-SA版权

文章标签：

#车辆运动学模型 #LQR #横向控制

Control 专栏收录该内容

14 篇文章

订阅专栏

本文介绍了车辆运动学模型的建立、线性化、离散化过程，并详细阐述了基于LQR（线性二次调节器）的控制理论，通过泰勒展开和前向欧拉法对模型进行处理，最后给出了一段用于计算控制量的代码示例。

摘要生成于 C知道，由 DeepSeek-R1 满血版支持，前往体验 >

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

如图所示，对于横向控制而言，因变量为 $X,Y,\varphi$ ，自变量为 $\delta$

将速度 $v$ 沿 $X,Y$ 轴分解得车辆的模型为：

$\dot{x} = vcos\varphi$

$\dot{y}=vsin\varphi$

$\dot{\varphi }=\frac{v}{R}= \frac{vtan\delta}{l}$

即：

$\dot{X}= \begin{bmatrix} \dot{x}\\ \dot{x}\\ \dot{\varphi } \end{bmatrix} = \begin{bmatrix} vcos\varphi \\ vsin\varphi \\ v\frac{tan\delta}{l} \end{bmatrix}=\begin{bmatrix} f_{1} \\ f_{2} \\ f_{3} \end{bmatrix} =f(X,u)$

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

$X=\begin{bmatrix} x\\ y\\ \varphi \end{bmatrix}$

$u=\delta$

上述车辆运动学模型为非线性模型： $\dot{X}=f(X,u)$

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

$f(X,u)$ 在参考点 $(X_{r},u_{r})$ 处的泰勒展开公式,忽略高次项为：

$f(X,u)=f(X_{r},u_{r})+\frac{\partial f(X,u)}{\partial X}|_{(X_{r},u_{r})}(X-X_{r})+\frac{\partial f(X,u)}{\partial u}|_{(X_{r},u_{r})}(u-u_{r})$

$\frac{\partial f(X,u)}{\partial X}|_{(X_{r},u_{r})}=\begin{bmatrix} \frac{\partial f_{1}}{\partial x} & \frac{\partial f_{1}}{\partial y} & \frac{\partial f_{1}}{\partial \varphi }\\ \frac{\partial f_{2}}{\partial x} & \frac{\partial f_{2}}{\partial y} & \frac{\partial f_{2}}{\partial \varphi } \\ \frac{\partial f_{3}}{\partial x} & \frac{\partial f_{3}}{\partial y} & \frac{\partial f_{3}}{\partial \varphi } \end{bmatrix}= \begin{bmatrix} 0& 0 &-vsin\varphi_{r} \\ 0& 0& vcos\varphi_{r} \\ 0& 0& 0 \end{bmatrix}$

$\frac{\partial f(X,u)}{\partial u}|_{(X_{r},u_{r})}=\begin{bmatrix} \frac{\partial f_{1}}{\partial \delta }\\ \frac{\partial f_{2}}{\partial \delta } \\ \frac{\partial f_{3}}{\partial \delta } \end{bmatrix}= \begin{bmatrix} 0\\ 0 \\ \frac{v}{l(cos\delta _{r})^{2}} \end{bmatrix}$

$\dot{X}-\dot{X_{r}}= \begin{bmatrix} 0& 0 &-vsin\varphi_{r} \\ 0& 0& vcos\varphi_{r} \\ 0& 0& 0 \end{bmatrix}(X-X_{r})+\begin{bmatrix} 0\\ 0 \\ \frac{v}{l(cos\delta _{r})^{2}} \end{bmatrix}(\delta -\delta _{r})$

其中， $tan\delta _{r}=\frac{l}{R_{r}}=k_{r}l$

令 $\chi =X-X_{r}=\begin{bmatrix} x-x_{r}\\ y-y_{r} \\ \varphi -\varphi _{r} \end{bmatrix}$ , $\mu =(\delta -\delta _{r})$

则上式可表示为：

$\dot{\chi }= \begin{bmatrix} 0& 0 &-vsin\varphi_{r} \\ 0& 0& vcos\varphi_{r} \\ 0& 0& 0 \end{bmatrix}\chi +\begin{bmatrix} 0\\ 0 \\ \frac{v}{l(cos\delta _{r})^{2}} \end{bmatrix}\mu$

$\dot{\chi }= A\chi +B\mu$

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

$\frac{\chi _{k+1} - \chi _{k}}{T}=\dot{\chi _{k}}=A\chi_{k} +B\mu_{k}$

$\chi _{k+1}=(AT+E)\chi_{k} +BT\mu_{k}$

$\chi_{k+1}=A_{d}\chi_{k} +B_{d}\mu_{k}$

其中，

$A_{d}=AT+E$

$B_{d}=BT$

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

LQR的代价函数为： $J= \sum_{i=0}^{n-1}(X_{i}^{T}QX_{i}+u_{i}^{T}Ru_{i})$

假设 $Q=\begin{bmatrix} 1.0 &0 &0 \\ 0&1.0 &0 \\ 0& 0 & 1.0 \end{bmatrix}$ , $R=[1.0]$

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

$K=-(B_{d}^{T}PB+R)^{-1}B_{d}^{T}PA_{d}$

反馈量为：

$\mu =\delta -\delta r =-K\chi$

$\delta = -K\chi +\delta r = -K\chi + actan(k_{r}l)$

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;
}