手把手教用matlab做无人驾驶（十四）--项目实践(MPCC)

首先导入数据使用matlab程序画图可见;

clear
close all;
clc;

load Tracks/track2.mat %导入数据

figure(1);
plot(track2.outer(1,:),track2.outer(2,:),'k')
hold on
plot(track2.inner(1,:),track2.inner(2,:),'k')

接下来我要在这个地图中去做关于车避障。一般做避障我们首先要把轨道范围缩小。这个给后面MPCC控制限制范围:

matlab程序：

clear
close all;
clc;

ModelParams.W=0.06;
safetyScaling = 1.5;

load Tracks/track2.mat %导入数据
widthTrack = norm([track2.inner(1,1)-track2.outer(1,1),track2.inner(2,1)-track2.outer(2,1)]); %道路宽度
WidthCar = 0.5*ModelParams.W*safetyScaling; %车宽

for i = 1:length(track2.outer)
x1 = track2.outer(1,i);
y1 = track2.outer(2,i);
x2 = track2.inner(1,i);
y2 = track2.inner(2,i);
% vector connecting right and left boundary
numer=(x2-x1);
denom=(y1-y2);

% shrinking ratio
c = WidthCar/widthTrack;
d = -WidthCar/widthTrack;

% shrink track
track.outer(1,i) = x1 + c*numer;
track.inner(1,i) = x2 - c*numer;
track.outer(2,i) = y1 + d*denom;
track.inner(2,i) = y2 - d*denom;
end

figure(1);
plot(track.outer(1,:),track.outer(2,:),'r')
hold on
plot(track.inner(1,:),track.inner(2,:),'r')
plot(track2.outer(1,:),track2.outer(2,:),'k')
plot(track2.inner(1,:),track2.inner(2,:),'k')

画出的图形如下：

通过增加这段程序即可画出障碍物

%% Ohter cars
Y = ObstacelsState(track,traj,trackWidth,n_cars);

if size(Y,2) ~= n_cars-1
error('n_cars and the number of obstacles in "Y" does not match')
end

%画图障碍物的
figure(1)
if ~isempty(Y)
for i=1:size(Y,2)
carBox(Y(:,i),ModelParams.W/2,ModelParams.L/2)
end
end

现在我们地图已经有了障碍物，怎样去避障呢，我们后面用MPCC控制方法，所以就要新的边界，因为如果按照原来的边界肯定会撞上障碍物，那怎样形成新的边界呢？

这里我们模型预测圆弧控制的N=40，我们把每一个坐标分为12*40，得到每一个网格的坐标系：

function [grid_x grid_y new_b_default] = getGridCoordinates(traj,borders,track_center,X,DesignParameters,ModelParams)
% assumes x,y are the 1 and 2 states

nx=ModelParams.nx; %状态值为7
N=DesignParameters.N;%预测40步
grid_width=DesignParameters.grid_width; %把车道宽度分为12等分

tracklength = traj.ppx.breaks(end);%给出轨迹的x长度值，初始值可以看到17.8447

% find physical thetas for each future position in X
theta_phys=zeros(N,1);
for i=1:N
xy_posn=X(i*nx+1:i*nx+2);% assume x,y are the 1 and 2 states
theta_virt = X(nx*i+nx);

%approximate theta_phys as theta_virt: should hold if lag error weighting is high enough
theta_virt=mod(theta_virt,tracklength);
theta_phys(i)=theta_virt;

end

TrackLeftx=ppval(borders.pplx,theta_phys);
TrackLefty=ppval(borders.pply,theta_phys);
TrackRightx=ppval(borders.pprx,theta_phys);
TrackRighty=ppval(borders.ppry,theta_phys);

Dx = TrackRightx - TrackLeftx;
Dy = TrackRighty - TrackLefty;

% border if no car is near
new_b_default = [TrackLeftx TrackLefty TrackRightx TrackRighty];

%画边界
%plot(TrackLeftx,TrackLefty,TrackRightx,TrackRighty,'-b','linewidth',5)

grid_x = zeros(N,grid_width);
grid_y = zeros(N,grid_width);
for i=1:N
for j=1:grid_width
% equally spaced grid
grid_x(i,j) = TrackLeftx(i) + j/(grid_width+1)*Dx(i);
grid_y(i,j) = TrackLefty(i) + j/(grid_width+1)*Dy(i);
%画网格坐标
figure(1)
hold on
plot(grid_x,grid_y,'.c')
end
end

end

实现效果如下：

然后通过比较是否网格有障碍物，如果没有障碍物就把原理的边界（即红线作为边界），否则，就要形成新的边界：

如果有障碍物要分为三个部分去形成新的边界：

（1）第一步：首先根据40步的状态值和网格值得到最优路径：

function priorPath = Lock2Grid(X,grid_x,grid_y,DesignParameters,ModelParams)
% priorPath - [N]by[1] (dimensionless)

nx = ModelParams.nx;
N = DesignParameters.N;
grid_width = DesignParameters.grid_width;
stateindex_x=ModelParams.stateindex_x;
stateindex_y=ModelParams.stateindex_y;

priorPath = zeros(N,1);
% for c, type cast this to integers
priorPath = cast(priorPath, 'int32');

for i=1:N
min=100; % choose some big number that distance^2 can never reach
ind=1;
for j=1:grid_width
dx = grid_x(i,j) - X(i*nx+stateindex_x);
dy = grid_y(i,j) - X(i*nx+stateindex_y);
d2=dx*dx+dy*dy;
if d2<min
min=d2;
ind=j;
end
end
priorPath(i) = ind;
figure(1)
hold on
plot(grid_x(i,priorPath(i)),grid_y(i,priorPath(i)),'*r')
end

图如下：

（2）第二步：

可以看出上面的最优路径是会碰撞到障碍物，所以我们要优化上面的最优路径，以至于不要碰撞到障碍物，这里我们使用动态规划，找到避开障碍物的

% Optm_Path_Mtx is [N]by[grid_width]by[grid_width]
% OptmPath is [N]by[1]

N=DesignParameters.N;
DEAD_END_PATH = Constants.DEAD_END_PATH;

OptmPath = zeros(N,1); % initialize.
% OptmPath = cast(OptmPath, 'int32'); % cast to int

OptmPath(1) = Optm_Path_Mtx(1,1,1);
OptmPath(2) = Optm_Path_Mtx(2,OptmPath(1),1);

for i1=3:N
% i1 represents i+1

OptmPath(i1) = Optm_Path_Mtx(i1,OptmPath(i1-1),OptmPath(i1-2));
figure(1)
hold on
plot(grid_x(i1,OptmPath(i1)),grid_y(i1,OptmPath(i1)),'*r')

if OptmPath(i1)==DEAD_END_PATH
% exit function. Find_Nearest_Borders() will take care of the rest.
disp('BLAAAAAAAAAAAAAAAAAAAAAAAAAAAAH');
return;
end

end

最后根据这个优化的轨迹得到新的边界如下图，然后这个新的边界输入到MPCC或MPC控制器中:

现在可能有的人会问这个新边界是怎么得到的呢？这个新边界是根据把车长宽外扩，最后求这车的四边与两边的交点即可。

grid_width=DesignParameters.grid_width;
nx=ModelParams.nx;
N=DesignParameters.N;
carlength=DesignParameters.carlength;
carwidth=DesignParameters.carwidth;
UNOCCUPIED=Constants.UNOCCUPIED;
DEAD_END_PATH = Constants.DEAD_END_PATH;
LAST_BORDER_XMAX=Constants.LAST_BORDER_XMAX;

new_b_L = zeros(N,2); %左边边界
new_b_R = zeros(N,2); %右边边界

% for each row
for i=1:N
j_optm = OptmPath(i);

if j_optm==DEAD_END_PATH

% (i,j_optm) is invalid for any choice of j_optm
% note that (i-1,j_prev) is still a valid position
if i==1
% cannot use previous borders, what to do?
error('no valid first point');
end

j_prev=OptmPath(i-1);

% if not first point, use borders from previous point for rest of
% horizon
for ii=i:N
new_b_L(ii,:)=new_b_L(i-1,:);
new_b_R(ii,:)=new_b_R(i-1,:);
end
new_b = [new_b_L new_b_R];

% set the last border
P1=[grid_x(i-1,j_prev) grid_y(i-1,j_prev)];
P2=[grid_x( i,j_prev) grid_y( i,j_prev)];
last_border=calculate_last_border(P1,P2);

% figure(5);hold on;
% plot(grid_x,grid_y,'go');
% plot([P1(1) P2(1)],[P1(2) P2(2)],'r*');
% xx=[-2,2];
% yy=(last_border(3)-last_border(1)*xx)/last_border(2);
% plot(xx,yy,'b');
% axis([-2 2 -2 2]);

return; % exit from function
end

% x,y coordinates of left and right track borders
Lx=new_b_default(i,1);
Ly=new_b_default(i,2);
Rx=new_b_default(i,3);
Ry=new_b_default(i,4);

% identify which is first point to left that is occupied
for j=j_optm:-1:1
k = grid_isOccupiedBy(i,j); % car index
if k~=UNOCCUPIED
% this is the first occupied point to left of car

% get states of car
xypsi = X_all(i*nx+1:i*nx+3,k);

% find closest intersect point to right track border
new_b_L(i,:) = FindClosestIntersect(Rx,Ry,Lx,Ly,xypsi,carlength,carwidth);

break;
elseif j==1
% has hit track border
new_b_L(i,:) = new_b_default(i,1:2);
end
end

% identify which is first point to right that is occupied
for j=j_optm:grid_width
k = grid_isOccupiedBy(i,j); % car index
if k~=UNOCCUPIED
% this is the first occupied point to right of car

% get states of car
xypsi = X_all(i*nx+1:i*nx+3,k);

% find closest intersect point to left track border
new_b_R(i,:) = FindClosestIntersect(Lx,Ly,Rx,Ry,xypsi,carlength,carwidth);

break;
elseif j==grid_width
% has hit track border
new_b_R(i,:) = new_b_default(i,3:4);
end
end
end

new_b = [new_b_L new_b_R];

http://paulbourke.net/geometry/pointlineplane/

手把手教用matlab做无人驾驶（十四）--项目实践(MPCC)相关推荐

手把手教用matlab做无人驾驶（二十五）--基于控制障碍函数的MPC（MPC-CBF）
这边博客主要完成论文的内容:Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function,论 ...
手把手教用matlab做无人驾驶（八）-无迹Kalman滤波算法
1.KF.EKF.UKF都是高斯滤波,下面的介绍一下他们的优缺点: KF优点:计算简单 KF缺点:高斯线性模型约束 EKF优点:可以近似非线性问题 EKF缺点:高斯噪声约束,线性化引入了误差会可 ...
python 函数修饰器父类_手把手教你学python第十四讲（函数装饰器,super用法和时间处理）...
文中有些字在图中是因为每篇文章最多100张图片,我把有的小图片和文字一起截图了,文中所有的引用都会标出原文网址,除此以外都是作者原创. 有时候会在文章最前或者最后补充一些知识或者把前面说的有问题的地方 ...
足疗师手把手教你家中做足疗
足疗师手把手教你家中做足疗一.含苞未放:把脚擦干,之后涂抹润肤油; 二.金鱼摆尾:双手横向拍打双脚外侧,起到放松小腿肌肉的作用; 三.隔墙有耳:双手握住一只脚,向内稍用力挤压; 四.仙鹤展翅:双手在 ...
手把手教你如何做自媒体赚钱，揭开自媒体赚钱真相！记得收藏
这真是一个好时代,一个人人都是自媒体的时代,一个信息爆炸的时代. 自媒体变现,几乎人人都可以实现,离我们并不远,就在你我身边,普通人都可以通过自媒体赚钱. 手把手教你如何做自媒体赚钱,揭开自媒体赚钱真 ...
华为表哥手把手教你利用Jenkins持续集成iOS项目，教不会我花式拉翔！！！
手把手教你利用Jenkins持续集成iOS项目: 前言众所周知,现在App的竞争已经到了用户体验为王,质量为上的白热化阶段.用户们都是很挑剔的.如果一个公司的推广团队好不容易砸了重金推广了一个APP ...
clion浏览linux代码,手把手教你使用 Clion 开发 Linux C++ 项目
手把手教你使用 Clion 开发 Linux C++ 项目关于CLion CLion是一款专为开发C及C++所设计的跨平台IDE.它是以IntelliJ为基础设计的,包含了许多智能功能来提高开发人员 ...
手把手教你上手Git并上传项目到GitHub官网
手把手教你上手Git并上传项目到GitHub官网 Learning Git Branch: 学习 Git 最好的游戏及教程 https://learngitbranching.js.org 这个就是那 ...
手把手教你使用 Clion 开发 Linux C++ 项目
手把手教你使用 Clion 开发 Linux C++ 项目关于CLion CLion是一款专为开发C及C++所设计的跨平台IDE.它是以IntelliJ为基础设计的,包含了许多智能功能来提高开发人员 ...
手把手教你写web全栈入门项目—React+Koa+MongoDB（3w字教程，真的很详细，有代码）
手把手教你写web全栈入门项目-React+Koa+MongoDB

手把手教用matlab做无人驾驶（十四）--项目实践(MPCC)

手把手教用matlab做无人驾驶（十四）--项目实践(MPCC)相关推荐

最新文章

热门文章