MPC实现自动驾驶轨迹跟踪
65 浏览量
2021-01-06
22:19:37
上传
评论 14
收藏 97KB PDF 举报
MPC实现自动驾驶轨迹跟踪实现自动驾驶轨迹跟踪
本文继MPC运动学方法实现轨迹跟踪推导进行matlab代码实现，虽然你们找到的参考书都是simulink carsim联仿，我却坚持使用纯代码仿真，因为牛逼。
代码模板沿用了LQR轨迹跟踪算法Python/Matlab算法实现，直接复制下来就能用，拿去爽。
clc
clear all
Kp = 1.0 ;
dt = 0.1 ;% [s] Length = 2.9 ;% [m] wheel base of vehicle
Nx=3;%状态量的个数
Nu =2;%控制量的个数
Np =60;%预测步长
Nc=30;%控制步长
Row=10;%松弛因子
Q=100*eye(Nx*Np,Nx*Np);
R=1*eye(Nc*Nu);
max_steer =60 * pi/180; % in rad
target_v =30.0 / 3.6;
cx = 0:0.1:200; % sampling interception from 0 to 100, with step 0.1
for i = 1:500% here we create a original reference line, which the vehicle should always follow when there is no obstacles;
cy(i) = -sin(cx(i)/10)*cx(i)/8;
end
for i = 501: length(cx)
cy(i) = -sin(cx(i)/10)*cx(i)/8; %cy(500);
end
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% here we provide another reference line for testing, now you dont need to
% use it
% %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
p = [cx', cy'];
%计算一阶导数
for i = 1:length(cx)-1
pd(i) = (p(i+1,2)-p(i,2))/(p(i+1,1)-p(i,1));
end
pd(end+1) = pd(end);
%计算二阶导数
for i =2: length(cx)-1
pdd(i) = (p(i+1,2)-2*p(i,2) + p(i-1,2))/(0.5*(-p(i-1,1)+p(i+1,1)))^2;
end
pdd(1) = pdd(2);
pdd(length(cx)) = pdd(length(cx)-1);
%计算曲率
for i = 1:length(cx)-1
k(i) = (pdd(i))/(1+pd(i)^2)^(1.5);
end
cx= cx';
cy =cy';
cyaw = atan(pd');
ck = k';
%%%%%%%%%%%%%%%%%%%% above things are preprocessing %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
i = 1;
T = 80;
lastIndex = length(cx);
x = 0.1; y = -0.1; yaw = 0.1; v = 0.1;
U = [0.01;0.01];
vd1_p = 0;
vd2_p = 0;
vd_p = [vd1_p; vd2_p];
ind =0;
figure
plot(cx,cy,'r-')
hold on
while ind 3% we do not allow the vehicle deviation larger than 4
fprintf('diviation too big!')
break;
end
delta
[x,y,yaw,v] = update(x,y,yaw,v, delta, dt,Length, max_steer); %update the vehicle state for next iteration
posx(i) = x;
posy(i) =y;
i = i+1;
plot(posx(i-1),posy(i-1),'bo')
pause(0.01);
hold on
end
%% supplimented functions are as follows:
% function"Update" updates vehicle states
function [x, y, yaw, v] = update(x, y, yaw, v, delta,dt,Length,max_steer)% update x, y, yaw, and velocity
delta = max(min(max_steer, delta), -max_steer);
x = x + v * cos(yaw) * dt + randn(1)* 0;
y = y + v * sin(yaw) * dt + randn(1)* 0;
yaw = yaw + v / Length * tan(delta) * dt ;
v = v ;
end
function [a] = PIDcontrol(target_v, current_v, Kp) %we originally control v separately
a = Kp * (target_v - current_v);
资源评论
