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

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

如图所示，对于横向控制而言，因变量为 $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;
}

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

基于车辆运动学模型的纯跟踪(Pure Pursuit)法
一.定义及概论纯跟踪控制算法(Pure Pursuit)是一种典型的横向控制方法,最早由 R. Wallace 在1985年提出,该方法对外界的鲁棒性较好. 该算法的思想:基于当前车辆后轮中心位置, ...
基于阿克曼转向的车辆运动学模型
基于阿克曼转向的车辆运动学模型在simulink中建立车辆运动学模型,为路径规划奠定基础,能够更好的检验简化的运动学模型反映运动过程的准确性. 包括:1.simulink仿真验证(版本为2018b) ...
自动驾驶控制算法——车辆运动学模型
车辆运动学模型自行车模型(Bicycle Model) 自行车模型(Bicycle Model)的建立基于如下假设: 不考虑车辆在垂直方向(Z轴方向)的运动,即假设车辆的运动是一个二维平面上的运动; ...
基于阿克曼转向的车辆运动学模型在simulink中建立车辆运动学模型
基于阿克曼转向的车辆运动学模型在simulink中建立车辆运动学模型,为路径规划奠定基础,能够更好的检验简化的运动学模型反映运动过程的准确性. 包括:1.simulink仿真验证(版本为2018b) ...
自动驾驶车辆控制（车辆运动学模型）
本文应配合b站up主"忠厚老实的老王"的自动驾驶控制算法系列视频食用. 文章目录 1. 两个车辆运动学模型 1.1 三个坐标系 1.2符号定义 1.3车辆运动学模型 1.4车辆动力 ...
Apollo学习笔记（7）车辆运动学模型
这里又整了一遍车辆运动学,之前在Autoware那会,已经在控制那一部分整过一次了,就当复习了吧. 前言车辆的控制目前用的最广的有三种方式: PID LQR MPC 其中,PID是一种对模型无要求的 ...
Apollo代码学习(二)—车辆运动学模型
Apollo代码学习-车辆运动学模型前言车辆模型单车模型(Bicycle Model) 车辆运动学模型阿克曼转向几何(Ackerman turning geometry) 小结 Apollo( ...
车辆运动学模型到动力学模型推导
车辆运动学模型到动力学模型推导参考 https://www.bilibili.com/video/BV1St411A7nH/?spm_id_from=333.788.recommend_more_v ...
【自动驾驶】车辆运动学模型
文章目录参考资料 1. 以车辆重心为中心的单车运动学模型 1.1 参数说明 1.2 几何关系 1.2.1 偏航角ψ\psiψ的关系 1.2.1 滑移角 β\betaβ 的关系 1.2.2 运动学模型 ...
自动驾驶（五）---------车辆运动学模型
车辆运动学模型我的定义:正常直线行驶,位置变化等于速度*时间,车辆不是直线行驶,已知速度和方向盘转角等,求新的位置. 1.单车模型(Bicycle Model) 其中将左/右前轮合并为一个点,位于A ...

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

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

最新文章

热门文章