MPC实现自动驾驶轨迹跟踪_MPC,参考点,泰勒展开

cell

自动驾驶

55 浏览量更新于2023-05-04 评论 14 收藏 97KB PDF 举报

身份认证购VIP最低享 7 折!

领优惠券(最高得80元）

资源详情

资源评论

资源推荐

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);

本内容试读结束，登录后可阅读更多

下载后可阅读完整内容，剩余3页未读，立即下载

weixin_38631225

粉丝: 5
资源: 909

会员权益专享

MPC实现自动驾驶轨迹跟踪

评论0

会员权益专享

最新资源

MPC实现自动驾驶轨迹跟踪

评论0

大名鼎鼎的MPC的C++源代码

离散控制Matlab代码-MPC-for-Autonomous-Vehicle:C++中的自行车模型上的MPC

chap4 Matlab Code_轨迹跟踪_基于mpc的轨迹跟踪控制器_无人驾驶车辆

自动驾驶轨迹跟踪mpc完整python代码

怎么搭建一个PID自动驾驶轨迹跟踪的设计框架

mpc动力学换道跟踪控制

轨迹跟踪控制国内研究现状

自动驾驶决策和控制算法

matlab自动驾驶仿真

carnd-mpc-project-master

mpc模型预测控制 python

python mpc

MPC_XY_CRUISE

python MPC

apollo用了哪些算法

四轮差速小车的mpc运动学模型建立

ros 的ego-planner用的什么算法

MPC_control_robot：基于MPC的移动机器人轨迹跟踪控制

基于自适应MPC的自动驾驶汽车轨迹跟踪控制.pdf

MPCcontrol_路径规划_路径_路径跟随_模型预测控制_汽车_

会员权益专享

最新资源