手把手教用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=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/