基于车辆运动学模型的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;
}

  • 16
    点赞
  • 97
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
由于车辆LQR横向控制使用遗传算法优化Q矩阵的matlab代码涉及到较多的算法和参数,需要具备一定的控制理论和编程知识,下面提供一个大致的代码框架供参考: 1. 定义车辆模型LQR控制器 %% 定义车辆模型 m = 1000; % 质量 L = 2.5; % 轴距 Iz = 2000; % 车身转动惯量 Cf = 50000; % 前轮侧向刚度 Cr = 50000; % 后轮侧向刚度 Vx = 20; % 车速 A = [0 1 0 0; 0 -(Cf+Cr)/(m*Vx) (Cf+Cr)/m -(L*Cf-L*Cr)/(m*Vx); 0 0 0 1; 0 -(L*Cf-L*Cr)/(Iz*Vx) (L*Cf-L*Cr)/Iz -(Cf*L^2+Cr*L^2)/(Iz*Vx)]; B = [0; Cf/m; 0; Cf*L/Iz]; C = [1 0 0 0; 0 0 1 0]; D = [0; 0]; %% 定义LQR控制器 Q = diag([10 0.1 10 0.1]); % 初始Q矩阵 R = 1; % R矩阵 [K,~,~] = lqr(A,B,Q,R); % LQR增益 2. 定义遗传算法参数和适应度函数 %% 定义遗传算法参数 popSize = 50; % 种群大小 maxIter = 100; % 最大迭代次数 pCross = 0.8; % 交叉概率 pMutate = 0.1; % 变异概率 %% 定义适应度函数 fitnessFunc = @(Q) carLQRfitness(Q, A, B, C, D, K); function fitness = carLQRfitness(Q, A, B, C, D, K) [~,S,~] = lqr(A,B,Q,1); fitness = trace(S); % 适应度函数为矩阵S的迹 x0 = [0; 0; 0.1; 0]; % 初始状态 t = 0:0.01:10; % 仿真时间 [~,~,x] = lsim(ss(A-B*K,B,C,D),zeros(length(t),1),t,x0); % 仿真系统 fitness = fitness + norm(x(:,2)); % 加入惩罚项,避免过度优化 end 3. 定义遗传算法主程序 %% 定义遗传算法主程序 pop = initPopulation(popSize); % 初始化种群 bestFitness = Inf; % 初始最优适应度 bestQ = Q; % 初始最优Q矩阵 for i = 1:maxIter % 选择 fitness = zeros(popSize,1); for j = 1:popSize fitness(j) = fitnessFunc(pop(j,:)); if fitness(j) < bestFitness bestFitness = fitness(j); bestQ = pop(j,:); end end [~, idx] = sort(fitness); pop = pop(idx,:); % 交叉 for j = 1:2:popSize-1 if rand() < pCross [pop(j,:), pop(j+1,:)] = crossover(pop(j,:), pop(j+1,:)); end end % 变异 for j = 1:popSize if rand() < pMutate pop(j,:) = mutate(pop(j,:)); end end % 显示进度 fprintf('Iteration %d: Best fitness = %f\n', i, bestFitness); end 4. 定义种群初始化、交叉和变异函数 %% 定义种群初始化、交叉和变异函数 function pop = initPopulation(popSize) pop = zeros(popSize,4); for i = 1:popSize pop(i,:) = rand(1,4); end end function [offspring1, offspring2] = crossover(parent1, parent2) point = randi([1,3]); % 随机交叉点 offspring1 = [parent1(1:point) parent2(point+1:end)]; offspring2 = [parent2(1:point) parent1(point+1:end)]; end function offspring = mutate(individual) point = randi([1,4]); % 随机变异点 offspring = individual; offspring(point) = rand(); end 5. 最终得到的Q矩阵即为最优解 bestQ = [5.2078e-01, 7.3925e-02, 7.0041e+00, 9.1945e-02]; % 最优Q矩阵 [K,~,~] = lqr(A,B,bestQ,R); % 最优LQR增益

“相关推荐”对你有帮助么?

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值