车辆横向控制

CHD_Apple

已于 2023-10-16 20:23:24 修改

阅读量717

点赞数

分类专栏：智能驾驶文章标签：自动驾驶人工智能目标跟踪算法

于 2023-09-21 17:33:04 首次发布

本文链接：https://blog.csdn.net/CHD_Apple/article/details/133137275

版权

智能驾驶专栏收录该内容

19 篇文章 6 订阅

订阅专栏

本文探讨了车辆的运动学模型，包括刚性车轮假设的局限性，进而发展到二自由度动力学模型。通过误差分析引入前馈控制来减少稳态误差，并利用Carsim和Simulink进行仿真，展示了在不同道路条件下的前轮转角变化。

摘要由CSDN通过智能技术生成

1、车辆运动学模型

计算可得如下的运动学方程：

运动学模型下，车轮为刚性的，这对于实际车辆来说，实际行驶时车轮轮胎会发生弹性形变，为了更精确的描述车辆运动状态，建立车辆动力学模型。

2、二自由度车辆动力学模型

根据牛顿第二定律

这里引入轮胎侧偏角，这里第一个公式右侧少乘了m,

最后得到如下的方程：

即 $\dot{x} = Ax+Bu$

3、建立误差方程

令 d = ed， $e\psi = \psi -\theta r$ 这里e $\psi$ 并不是航向误差，航向误差 = $\theta - \theta r$ 。

即可得到上诉误差模型。

4、前馈控制 + 反馈控制

对于LQR控制，系统会最终趋于稳定，即

即对于这个方程，系统稳定后， $\dot{err}$ 等于0，而err 不等于0。即

即，系统最终会有稳态误差，因此这里引入前馈控制，

这里可以取合适的

使得，err和 $\dot{err}$ 同时为0。

计算可得:

最终LQR系统稳定后err =

也就是 $e\psi$ = $\psi - \theta r$ 的稳态误差极为- $\beta$ ，而此公式含义也就是横摆角 - 航向角 = -质心侧偏角。

5、Carsim设置

工况设置

这里验证了U形道路，S形道路，圆形道路，以及复杂道路

6、simulink 搭建

代码如下：

预测模块：

function [pre_x,pre_y,pre_phi,pre_vx,pre_vy,pre_dphi] = fcn(x,y,phi,vx,vy,dphi,ts)
    pre_x = x + vx*ts*cos(phi)-vy*ts*sin(phi);
    pre_y = y +vy*ts*cos(phi) + vx*ts*sin(phi);
    pre_phi = phi + dphi*ts;
    pre_vx = vx;
    pre_vy = vy;
    pre_dphi = dphi;
end

误差、曲率计算模块

function [kr,err] = fcn(x,y,phi,vx,vy,dphi,xr,yr,thetar,kappar)
   n = length(xr);
    d_min=(x-xr(1))^2+(y-yr(1))^2;
    min = 1;
    for i = 1:n
       if   d_min >= (x-xr(i))^2+(y-yr(i))^2 
           d_min = (x-xr(i))^2+(y-yr(i))^2;
           min = i;
       end
    end
    index=min;
    tor=[cos(thetar(index));sin(thetar(index))];
    nor=[-sin(thetar(index));cos(thetar(index))];
    d_err=[x-xr(index);y-yr(index)];
    ed=nor'*d_err;
    es = tor'*d_err;
%     projection_point_thetar=thetar(index);%apollo
    projection_point_thetar=thetar(index)+kappar(index)*es;
    ed_dot=vy*cos(phi-projection_point_thetar)+vx*sin(phi-projection_point_thetar);
    %%%%%%%%
    ephi=sin(phi-projection_point_thetar);
    %%%%%%%%
    s_dot_tmp=vx*cos(phi-projection_point_thetar)-vy*sin(phi-projection_point_thetar);
    s_dot=s_dot_tmp/(1-kappar(index)*ed);
    ephi_dot=dphi-kappar(index)*s_dot;
    kr=kappar(index);
    err=[ed;ed_dot;ephi;ephi_dot];
end

前馈模块

function delta_f = fcn(K,kappa,a,b,cf,cr,m,vx)
    delta_f = kappa*((a+ b) - b * K(3) - ((m*vx*vx)*((b/cf) + (a*K(3)/cr) - (a/cr)))/(a+b));
%       delta_f=kappa*((a+b)-b*k(3)-(m*vx*vx/(a+b))*((b/cf)+(a/cr)*k(3)-(a/cr)));
end

前轮转角计算模块

function delta = fcn(K,delta_f,err)
    delta = -K * err + delta_f;
end

依据速度选取的LQR计算的K值模块

function K = fcn(vx,k1,k2,k3,k4)
    if abs(vx) < 0.01
       K = [0,0,0,0]; 
    else
        index = abs(round(vx/0.01));
        K = [k1(index),k2(index),k3(index),k4(index)];
    end
end

车辆参数以及LQR计算模型

 m = 1270 + 88.6 +54.4 ;
 a = 1.015;
 b = 2.91 - a;
 L = 2.91;
 cf = -110000;
 cr = cf;
 Iz = 1536.7;
 K = zeros(5000,4);
 for i = 1:5000
     vx = i * 0.01;
     A =[0,1,0,0;
         0,(cf + cr)/(m*vx),-(cf + cr)/m,(a*cf - b*cr)/(m*vx);
         0,0,0,1;
         0,(a*cf - b*cr)/(Iz*vx),-(a*cf - b*cr)/Iz,(a*a*cf+b*b*cr)/(Iz*vx)];%
     B=[0;
         -cf/m;
         0;
         -a*cf/Iz];
     Q = eye(4);
     R = 100;
     K(i,:) = lqr(A,B,Q,R);
 end
 k1 = K(:,1)';
 k2 = K(:,2)';
 k3 = K(:,3)';
 k4 = K(:,4)';