【无人车】用于无人地面车辆的路径跟踪算法（Matlab代码实现）

 

💥💥💞💞欢迎来到本博客❤️❤️💥💥

🏆博主优势：🌞🌞🌞博客内容尽量做到思维缜密，逻辑清晰，为了方便读者。

⛳️座右铭：行百里者，半于九十。

📋📋📋本文目录如下：🎁🎁🎁

目录

💥1 概述

📚2 运行结果

🎉3 参考文献

🌈4 Matlab代码实现

---

💥1 概述

无人驾驶技术是当前社会的热门技术之一,无人驾驶车辆的应用可以很好地解决环境污染和交通拥堵两大主要社会问题。而在无人驾驶车辆的所有技术中,车辆的底层控制技术和路径跟踪技术是无人车的基础技术。本文用于无人地面车辆的路径跟踪算法，详情可见运行结果图。 

📚2 运行结果

运行视频

部分代码：

clear;
clc;
close all;
addpath('Params','TargetCourse');

%% Choose Vehicle Algrithm Course 
Vehicle = 'C-Class-Hatchback'; 
% B-Class-Hatchback C-Class-Hatchback
path_tracking_alg = 'Kinematics MPC V W'; 
% Pure Pursuit,Stanley,Kinematics MPC V Delta,Dynamics MPC,Kinematics MPC V W
roadmap_name = 'eight';
% eight road double

%% Get Params
Reference = getTargetCourseParams(roadmap_name);
Reference = splinfy(Reference);
VehicleParams = getVehicleParams(Vehicle);
AlgParams = getAlgParams(path_tracking_alg,VehicleParams);
Reference.type = roadmap_name;
VehicleParams.type = Vehicle;
AlgParams.type = path_tracking_alg;
time_step = AlgParams.ts;

%% Initialize State
x0 = Reference.cx(1000);y0 = Reference.cy(1000);yaw0 = Reference.cyaw(1000);s0 = Reference.s(1000);
delta0 = 0;v0 = 20;w0 = 0;vy0=0;
desired_velocity = 20;
desired_angular_v = 0;
desired_delta = 0;

i = 0;simulation_time = 0;
Vehicle_State = [x0,y0,yaw0,s0,v0,w0,vy0];
Control_State = delta0;

%% Log
log.i=i;log.time=simulation_time;
log.X=x0;log.Y=y0;log.Yaw=yaw0;log.Odometry=s0;
log.Vx=v0;log.Angular_V=w0;
log.delta=delta0;
log.error=0;log.solvertime=0;

[path_figure,result_figure,delta_line,error_line,solve_time_line]= Visualization_Init(AlgParams, Reference,... 
    VehicleParams, Vehicle_State, Control_State,simulation_time);

isGoal = norm(Vehicle_State(1:2)-[Reference.cx(end),Reference.cy(end)])<1 && (Reference.s(end)-Vehicle_State(4))<1;
disp([path_tracking_alg,' ',roadmap_name,' simulation start!']);

%% path tracking algrithm
while ~isGoal
    tic;
    i = i + 1;
    simulation_time = simulation_time + time_step;
    tic;
    switch AlgParams.type
        case "Pure Pursuit"
            [steer_cmd,error,preview_point] = UGV_PP(Reference,VehicleParams,AlgParams,Vehicle_State,Control_State);
        case "Stanley"
            [steer_cmd,error,preview_point] = UGV_Stanley(Reference,VehicleParams,AlgParams,Vehicle_State,Control_State);
        case "Kinematics MPC V W"
            Control_ref=[desired_velocity,desired_angular_v];
            [control_cmd,error,MPCprediction] = UGV_Kinematics_MPC_V_W(Reference,VehicleParams,AlgParams,Vehicle_State,Control_ref);
        case "Kinematics MPC V Delta"
            Control_ref=[desired_velocity,desired_delta];
            [control_cmd,error,MPCprediction] = UGV_Kinematics_MPC_V_Delta(Reference,VehicleParams,AlgParams,Vehicle_State,Control_ref);
        case "Dynamics MPC"
            Control_State=[delta0,desired_velocity];
            [steer_cmd,error,MPCprediction,update_state] = UGV_Dynamics_MPC(Reference,VehicleParams,AlgParams,Vehicle_State,Control_State);
    end
    toc;
    
%% update vehicle state
    if AlgParams.type == "Pure Pursuit" || AlgParams.type == "Stanley" || AlgParams.type == "Dynamics MPC" || AlgParams.type == "Kinematics MPC V Delta"
        wheel_base = VehicleParams.wheel_base;t=time_step;
        if AlgParams.type ~= "Kinematics MPC V Delta"
            delta=steer_cmd;v1=v0;
        else
            delta=control_cmd(2);v1=control_cmd(1);
        end
        x0=Vehicle_State(1);y0=Vehicle_State(2);yaw0=Vehicle_State(3);s0=Vehicle_State(4);v0=Vehicle_State(5);
        x1=x0+v0*cos(yaw0)*t;y1=y0+v0*sin(yaw0)*t;yaw1=yaw0+v0/wheel_base*tan(delta)*t;s1=s0+v0*t;w1=(yaw1-yaw0)/t;
        Vehicle_State=[x1,y1,yaw1,s1,v1,w1];
        Vehicle_State(3)=wrapTo2Pi(Vehicle_State(3));
        if AlgParams.type == "Dynamics MPC"
            Vehicle_State(7)=update_state(2);
        end
       
    elseif AlgParams.type == "Kinematics MPC V W"
        wheel_base = VehicleParams.wheel_base;t=time_step;
        x0=Vehicle_State(1);y0=Vehicle_State(2);yaw0=Vehicle_State(3);s0=Vehicle_State(4);
        v1=control_cmd(1);w1=control_cmd(2);
        x1=x0+v1*cos(yaw0)*t;y1=y0+v1*sin(yaw0)*t;yaw1=yaw0+w1*t;s1=s0+v1*t;
        Vehicle_State=[x1,y1,yaw1,s1,v1,w1];
        Vehicle_State(3)=wrapTo2Pi(Vehicle_State(3));
        delta = atan(w1*wheel_base/v1);
    end
    
    log.i(end+1)=i;log.time(end+1)=simulation_time;
    log.X(end+1)=x1;log.Y(end+1)=y1;log.Yaw(end+1)=yaw1;log.Odometry(end+1)=s1;
    log.Vx(end+1)=v1;log.Angular_V(end+1)=w1;log.delta(end+1)=delta;
    log.error(end+1)=error;log.solvertime(end+1)=toc;
    
%% show animation
    set(groot, 'CurrentFigure', path_figure);cla;
    switch (Reference.type)
        case {'eight' 'road'}
            axis([x1-40,x1+40,y1-40,y1+40]);
            plot_car(VehicleParams, Vehicle_State, delta);
        case {'double','Emergency'}
            
    end
    h1=plot(Reference.cx, Reference.cy, '-k.','LineWidth',3, 'markersize',3,'DisplayName','Target Trajectory');
    h2=plot(log.X, log.Y, '-b.','LineWidth', 3,'markersize',3,'DisplayName','Real Trajectory');
    h3=plot(Vehicle_State(1),Vehicle_State(2),'Marker','p','MarkerFaceColor','red','MarkerSize',12.0,'DisplayName','CoG');
    switch (AlgParams.type)
        case {"Pure Pursuit","Stanley"}
            h4=plot(preview_point(1),preview_point(2),'d','MarkerFaceColor','yellow','MarkerSize',12,'DisplayName','Preview Point');
            legend([h1 h2 h3 h4],{'Target Trajectory','Real Trajectory','CoG','Preview Point'});
        case {"Kinematics MPC V W","Kinematics MPC V Delta","Dynamics MPC"}
            h4=plot(MPCprediction(1,:),MPCprediction(2,:), '-y.','LineWidth', 3,'markersize',3,'DisplayName','Prediction Trajectory');
            legend([h1 h2 h3 h4],{'Target Trajectory','Real Trajectory','CoG','MPC Prediction Trajectory'});
    end
    title(['Time[s]:',num2str(round(simulation_time,3),3),'s',' Velocity[m/s]:',num2str(round(v1,2))]);
    
    set(groot, 'CurrentFigure', result_figure);
    set(delta_line,'Xdata',log.time,'Ydata',log.delta/pi*180);
    set(error_line,'Xdata',log.time,'Ydata',log.error);
    set(solve_time_line,'Xdata',log.time,'Ydata',log.solvertime);
    pause(0.0001);
    isGoal = norm(Vehicle_State(1:2)-[Reference.cx(end),Reference.cy(end)])<1^2 && (Reference.s(end)-Vehicle_State(4))<1;
end
disp([path_tracking_alg,' Get Goal ! simulation stop!']);

%         syms x(t) y(t) yaw(t) s(t);
%         eqn1 = diff(x,t) == v0*cos(yaw); eqn2 = diff(y,t) == v0*sin(yaw);
%         eqn3 = diff(yaw,t) == v0*tan(steer_cmd)/wheel_base; eqn4 = diff(s,t) == v0;
%         cond1 = x(0) == x0;cond2 = y(0) == y0;cond3 = yaw(0) == yaw0;cond4 = s(0) == s0;
%         Up_State = dsolve(eqn1,eqn2,eqn3,eqn4,cond1,cond2,cond3,cond4);
%         t=time_step;
%         Vehicle_State = [eval([Up_State.x,Up_State.y,Up_State.yaw,eval(Up_State.s)]),v0,(eval(Up_State.yaw)-yaw0)/t];

%         wheel_base = VehicleParams.wheel_base;
%         x0=Vehicle_State(1);y0=Vehicle_State(2);yaw0=Vehicle_State(3);s0=Vehicle_State(4);
%         v0=control_cmd(1);w0=control_cmd(2);
%         syms x(t) y(t) yaw(t) s(t);
%         eqn1 = diff(x,t) == v0*cos(yaw); eqn2 = diff(y,t) == v0*sin(yaw);
%         eqn3 = diff(yaw,t) == w0; eqn4 = diff(s,t) == v0;
%         cond1 = x(0) == x0;cond2 = y(0) == y0;cond3 = yaw(0) == yaw0;cond4 = s(0) == s0;
%         Up_State = dsolve(eqn1,eqn2,eqn3,eqn4,cond1,cond2,cond3,cond4);
%         t=time_step;
%         Vehicle_State = [eval([Up_State.x,Up_State.y,Up_State.yaw,eval(Up_State.s)]),v0,(eval(Up_State.yaw)-yaw0)/t];

🎉3 参考文献

部分理论来源于网络，如有侵权请联系删除。

[1]樊晓楠. 无人观光车底层控制系统改造及路径跟踪算法研究[D].长安大学,2019.

🌈4 Matlab代码实现