【自动驾驶】控制算法（八）横向控制Ⅲ | 代码与模型

    count=50;
    [x1,y1,theta1,kr1]=straight([0,0],[20,0],0,count);
    [x2,y2,theta2,kr2]=arc([20,0],[30,10],0,pi/2,count);
    [x3,y3,theta3,kr3]=arc([30,10],[40,20],pi/2,0,count);
    [x4,y4,theta4,kr4]=arc([40,20],[40,40],0,pi,count);
    [x5,y5,theta5,kr5]=arc([40,40],[35,35],pi,3*pi/2,count);
    [x6,y6,theta6,kr6]=arc([35,35],[25,35],3*pi/2,pi/2,count);
    [x7,y7,theta7,kr7]=arc([25,35],[15,35],pi/2,3*pi/2,count);
    [x8,y8,theta8,kr8]=arc([15,35],[5,35],3*pi/2,pi/2,count);
    [x9,y9,theta9,kr9]=arc([5,35],[-15,35],pi/2,3*pi/2,count);
    [x10,y10,theta10,kr10]=straight([-15,35],[-15,15],3*pi/2,count);
    [x11,y11,theta11,kr11]=arc([-15,15],[0,0],3*pi/2,2*pi,count);
    xr=[x1,x2,x3,x4,x5,x6,x7,x8,x9,x10,x11];
    yr=[y1,y2,y3,y4,y5,y6,y7,y8,y9,y10,y11];
    thetar=[theta1,theta2,theta3,theta4,theta5,theta6,theta7,theta8,theta9,theta10,theta11];
    kappar=[kr1,kr2,kr3,kr4,kr5,kr6,kr7,kr8,kr9,kr10,kr11];
    
    scatter(xr,yr);
    
    function[xr,yr,thetar,kr]=straight(init_coord,end_coord,init_angle,count)
    delta_x=(end_coord(1)-init_coord(1))/(count-1);
    delta_y=(end_coord(2)-init_coord(2))/(count-1);
    for i=1:count
    xr(i)=init_coord(1)+delta_x*i;
    yr(i)=init_coord(2)+delta_y*i;
    thetar(i)=init_angle;
    kr(i)=0;
    end      
    end
    
    function[xr,yr,thetar,kr]=arc(init_coord,end_coord,init_angle,end_angle,count)
    L=sqrt((init_coord(1)-end_coord(1))^2+(init_coord(2)-end_coord(2))^2);
    R=L/sqrt(2*(1-cos(end_angle-init_angle)));
    delta_angle=(end_angle-init_angle)/(count-1) ;
      
       for i=1:count
           if delta_angle>0
               xr(i)=init_coord(1)-R*sin(init_angle)+R*sin(init_angle+delta_angle*(i-1));
               yr(i)=init_coord(2)+R*cos(init_angle)-R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=1/R;
           else
               xr(i)=init_coord(1)+R*sin(init_angle)-R*sin(init_angle+delta_angle*(i-1));
    
               yr(i)=init_coord(2)-R*cos(init_angle)+R*cos(init_angle+delta_angle*(i-1));
               thetar(i)=init_angle+delta_angle*i;
               kr(i)=-1/R;
           end               
       end  
    end
    
    plain
![](https://ad.itadn.com/c/weblog/blog-img/images/2025-08-16/BPM2OTEpZXUjVsh3qHtduJ67Gwon.png)

    [xr,yr,~,~] = straight([0,0],[50,50],pi/4,10);
    
    plain

    [xr,yr,~,~] = arc([0,0],[50,50],0,pi/2,100);
    
    plain

    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
    
    plain

    function [kr,err] = fcn(x,y,phi,vx,vy,phi_dot,xr,yr,thetar,kappar)
    n=length(xr);%先找到规划轨迹的长度，共有多少个规划点
    d_min=(x-xr(1))^2+(y-yr(1))^2;%把最短距离设为真实位置与第1个点。可以不用开平方根，减少计算量，因为并不关心距离是多少，只想找到最短点的序号
    min=1;%先把最短点的序号设为1
    for i=1:n%遍历所有的点
        d=(x-xr(i))^2+(y-yr(i))^2;%计算它与真实的位置的之间的距离
        if d<d_min%如果能找到比所设置的最短距离还要短
            d_min=d;%将值赋为最短距离
            min=i;%把序点的序号赋给min
        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);%因为角度具有多值性，使用sin来消除因为多2pi或者少2pi而导致的算法出错
    %%%%%%%%%
    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];
    end
    
    plain
![](https://ad.itadn.com/c/weblog/blog-img/images/2025-08-16/BQIvVnbxtRN1DWjfr8HkiFTGcCZ0.png)

    cf=-110000;%侧偏刚度根据曲线估算
    cr=cf;%严格来说 CR 和 CF 应该不一样，但差别不大，所以近似认为一样
    m=1412;%根据Carsim把整车参数输进去
    Iz=1536.7;
    a=1.015;
    b=2.910-1.015;
    k=zeros(5000,4);%申请矩阵k，用来存放LQR的k
    for i=1:5000
    vx=0.01*i;%只有vx是变量，所以每隔0.01米每秒，算一下对应的k。
    %这样的话就可涵盖v从 0.01 米每秒到 v 等于 50 米每秒这么大范围内所有的 k 
    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=1*eye(4);
    R=10;
    k(i,:)=lqr(A,B,Q,R);
    end
    k1=k(:,1)';%建立四个变量，K1、K2、K3、K4，把表记录下来
    k2=k(:,2)';%因为Simlink不能传矩阵，只能传行向量，需要做个转置
    k3=k(:,3)';
    k4=k(:,4)';
    
    plain
![](https://ad.itadn.com/c/weblog/blog-img/images/2025-08-16/mNYI4d2iAwkc7fB08bWxagtrnHGh.png)

    function k  = fcn(k1,k2,k3,k4,vx)
    if abs(vx)<0.01%特殊处理如果 v 的绝对值小于0.01，
        k=[0,0,0,0];%直接令 k 就等于0，它是航向量
    else
        index=round(vx/0.01);%否则的话就用 v x 除001，再用round四舍五入，值就是所要的匹配的序号
        k=[k1(index),k2(index),k3(index),k4(index)];
    end
    end
    
    plain

    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
    
    plain

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