close all;
clear all;
clc;
%% 修改Obstacle Avoidance Using Adaptive Model Predictive Control
%% Vehicle Model
% 自车长5米宽2米 自车模型The model has four states:
% * $\theta$ - Heading angle of the car POSE!!!
V = 20; %自车以20m/s的速度向东行驶
x0 = [10; 10; 0; V]; %x0 = [x; y; theta; V]theta为当前车辆相对于世界坐标系x轴的航向角
u0 = [0; 0]; %u0 = [Throttle; Delta]油门开度和方向盘转角
%% Road and Obstacle Information
% 直路3车道,每车道宽4m
% lanes = 3;
% laneWidth = 4;
%%
% 障碍物为静态,在车道中央,大小设置:怎么改动态障碍物???
obstacle1 = struct;
obstacle1.Length = 8;
obstacle1.Width = 3;
%%
% 设定障碍物的位置
obstacle1.X = 25;
obstacle1.Y = 5;
%%
% 在障碍物周围创建一个虚拟安全区域,以便自车在通过障碍物时不会靠近障碍物。
% 安全区域以障碍物为中心,并具有:
% * Length equal to two car lengths.
% * Width equal to two lane widths.
%obstacle.safeDistanceX = obstacle.Length;
%obstacle.safeDistanceY = 3;
%Obstacle:
% Front left
% obstacle1.flX = obstacle1.X+obstacle1.Length/2;
% obstacle1.flY = obstacle1.Y+obstacle1.Width/2;
% % Front right
% obstacle1.frX = obstacle1.X+obstacle1.Length/2;
% obstacle1.frY = obstacle1.Y-obstacle1.Width/2;
% % Rear left
% obstacle1.rlX = obstacle1.X-obstacle1.Length/2;
% obstacle1.rlY = obstacle1.flY;
% Rear right
obstacle1.rrX = obstacle1.X-obstacle1.Length/2;
obstacle1.rrY = obstacle1.Y-obstacle1.Width/2;
%Safe zone:
% Front left
% obstacle.flSafeX = obstacle.X+obstacle.safeDistanceX;
% obstacle.flSafeY = obstacle.Y+obstacle.safeDistanceY;
% % Front right
% obstacle.frSafeX = obstacle.X+obstacle.safeDistanceX;
% obstacle.frSafeY = obstacle.Y-obstacle.safeDistanceY;
% % Rear left
% obstacle.rlSafeX = obstacle.X-obstacle.safeDistanceX;
% obstacle.rlSafeY = obstacle.flSafeY;
% % Rear right
Ro=sqrt((obstacle1.Length/2)^2+(obstacle1.Width/2)^2)+0.2;
obstacle1.rrSafeX = obstacle1.X-Ro;
obstacle1.rrSafeY = obstacle1.Y-Ro;
%%
% 雷达可检测到的距离为30m
% obstacle.DetectionDistance = 30;
%%
% Plot the following at the nominal condition:
f = figure;
% Plot the Ego vehicle.
carLength = 5;
carWidth = 2;
X0 = x0(1);
Y0 = x0(2);
plot(X0,Y0,'gx'); hold on; grid on;
rectangle('Position',[X0-carLength/2,Y0 - carWidth/2,carLength,carWidth]);
% Plot the static obstacle.
plot(obstacle1.X,obstacle1.Y,'rx');
rectangle('Position',[obstacle1.rrX,obstacle1.rrY,obstacle1.Length,obstacle1.Width]);
%% Plot the safe zone around obstacle.
Ro = sqrt((obstacle1.Length/2)^2+(obstacle1.Width/2)^2)+0.2;
Re = sqrt((carLength/2)^2+(carWidth/2)^2)+0.2;
rectangle('Position',[obstacle1.rrSafeX,obstacle1.rrSafeY,2*Ro,2*Ro], 'curvature',[1,1],'edgecolor','r');
% Plot the safe zone around ego car.
rectangle('Position',[X0-Re,Y0-Re,2*Re,2*Re], 'curvature',[1,1],'edgecolor','r');
% Plot the lanes.
% X = [0;50;100];
% Y = [2;2;2];
% line(X,Y,'LineStyle','--','Color','b' );
% X = [0;50;100];
% Y = [-2;-2;-2];
% line(X,Y,'LineStyle','--','Color','b' );
%% Reset the axis.
%% 膨胀区域避障
result = zeros(3,100);%创建一个数组,来存放每次迭代的自车坐标
result(1,1) = X0;
result(2,1) = Y0;
n = 2;%循环次数
k = 0.01;%迭代步长
d = Y0 - obstacle1.Y;
Ro = sqrt((obstacle1.Length/2)^2+(obstacle1.Width/2)^2)+0.2;
Re = sqrt((carLength/2)^2+(carWidth/2)^2)+0.2;
%%该循环结束后自车的位姿返回为(0,0),有问题!!!
while (d < Ro+Re )
theta = acos(d/(Ro+Re))/2;%计算自车的姿态角
result(1,n) = result(1,n-1) + k* V * cos(theta);%更新X轴的坐标
result(2,n) = result(2,n-1) + k* V * sin(theta);%更新Y轴的坐标
result(3,n) = rad2deg(theta);%更新的姿态角
d = result(2,n)-obstacle1.Y;
n = n + 1;
end
for i = n-1:100
result(1,i) = result(1,i-1) + k*V;
result(2,i) = result(2,i-1);
end
%% plot
for i = 1:100
rectangle('Position',[result(1,i)-Re,result(2,i)-Re,2*Re,2*Re], 'curvature',[1,1],'edgecolor','r');
end
ObstacleAvoidingUsingAdaptiveMPCExample.rar_无人_无人 避障_无人驾驶 避障_避障
版权申诉
86 浏览量
2022-07-15
20:05:32
上传
评论
收藏 2KB RAR 举报
周楷雯
- 粉丝: 78
- 资源: 1万+