自动驾驶控制算法——老王Carsim_Simulink环境搭建步骤

老王复盘

Carsim有两个文件路径：Data和Prob

设置Matlab当前路径为Data，设置路径里添加子文件路径包括Data和Prob

修改Matlab当前路径方法：

cd('')

1. 在Carsim的添加在Simulink创建的模型文件，在simulink创建的空文件(.slx),保存到Carsimdata中
2. 左下角import和Export设置I/O Channels：Import，link to new dataset，title随便写一个
3. 点进去，添加路径文件，Quick Start,Baseline
4.  输入：扭矩IMP_MENGINE和制动压力IMP_PCON_BK，输出：车速Vx,加速度Ax,转速Av_eng
5. Procedure，修改仿真时间
6. send to Simulink，填写simfile.sim
7. 搭建需要的所有S-function模块

• predict module模块

function [pre_x,pre_y,pre_phi,pre_vx,pre_vy,pre_phi_dot] = fcn(x,y,phi,vx,vy,phi_dot,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+phi_dot*ts;
    pre_vx = vx;
    pre_vy = vy;
    pre_phi_dot = phi_dot;
end

• err/kappa calculate module模块

function [kr,err] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)%kappar是曲率
    n = length(xr);%查看一共有多少规划点
    d_min = (x-xr(1))^2+(y-yr(1))^2;%找到距离最近的规划点
    min = 1;
    for i=1:n%遍历找到最近的点
        d = (x-xr(i))^2+(y-yr(i))^2;
        if d < d_min
            d_min = d;
            min =i;
        end
    end
    dmin = min;
    tor = [cos(thetar(dmin));sin(thetar(dmin))];
    nor = [-sin(thetar(dmin));cos(thetar(dmin))];
    d_err = [x-xr(dmin);y-yr(dmin)];
    ed = nor'*d_err;
    es = tor'*d_err;
    projection_point_thetar = thetar(dmin);%apollo
    %projection_point_thetar = thetar(dmin) + kappar(dmin)*es;%老王的改进
    ed_dot = vy*cos(phi - projection_point_thetar) + vx*sin(phi - projection_point_thetar);
    %%%%%%
    ephi = sin(phi - projection_point_thetar);%消除pi的影响
    %%%%%%
    s_dot = vx*cos(phi - projection_point_thetar)-vy*sin(phi - projection_point_thetar);
    s_dot = s_dot/(1-kappar(dmin)*ed);
    ephi_dot = phi_dot - kappar(dmin)*s_dot;
    kr = kappar(dmin);
    err = [ed;ed_dot;ephi;ephi_dot];
    

• lqr_offline

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

• forword_angle

function forword_angle = fcn(vx,a,b,m,cf,cr,k,kr)
    forword_angle = kr*(a+b-b*k(3)-(m*vx*vx/(a+b)*(b/cf)+(a/cr)*k(3)-(a/cr)));
end

• 最后一个模块得到angle输出

function angle = fcn(k,err,forword_angle)
    angle = -k*err + forword_angle;
end