自动驾驶算法（九）：多项式轨迹与Minimun Snap原理与Matab代码详解

目录

1 为什么需要轨迹优化

2 代码解析

3 完整代码

---

1 为什么需要轨迹优化

        我们利用前八篇所学的博客可以利用RRT、A*、遗传算法等设计出一条折线轨迹，轨迹优化就是在路径优化的基础上将折线优化成曲线，这样更加有利于无人机的飞行。

        那么什么是多项式轨迹呢？

        每段轨迹的路程以t为变量的函数表示出来就是多项式轨迹。

        我们用三段轨迹表示这段路程（以时间t作为自变量）：

        那么我们的任务就是求解参数向量p确定轨迹，与路程成正比的因此我们可以提前算出。

        那么我们轨迹的要求是什么呢？

        优化目标为：

        这个优化的目的是使加速度尽可能的小：那么速度也是尽可能地小，对无人机动力要求就不那么大了。

        具体推导如下：

        其实我们就要算出来个Q矩阵，里面的r是rows的意思，c是cols的意思。

        其实就是一个二次规划问题？

        也就是求在Ap=b约束下最小的。

2 代码解析

        我们先来运行一下：

        所在位置为*的就是我们航行的路径点，不同颜色的就是几段。

        我们因此要先设置起点的速度和加速度、终点的速度和加速度、还有总时间、我们根据总时间还有路径的长度去分配时间：

function demo1_minimum_snap_simple()
    clear,clc;

    % condition
    % 航路点 起点、终点、中间的这些点
    waypts = [0,0;
              1,2;
              2,0;
              4,5;
              5,2]';
    % 起点、终点速度加速度      
    v0 = [0,0];
    a0 = [0,0];
    v1 = [0,0];
    a1 = [0,0];
    % 总时间
    T = 5; 
    % 计算每一段轨迹的时间
    % 输入参数为航路点 + 总时间
    ts = arrangeT(waypts,T);
    % 设置多项式的阶数
    n_order = 5;

        我们看一下怎么给各段路程分配时间的：

function ts = arrangeT(waypts,T)
    % 这一行代码计算了相邻路径点之间的差值。假设 waypts 是一个矩阵，每一列代表一个路径点的坐标，
    % 那么 waypts(:,2:end) 选择了除了第一个路径点之外的所有路径点，waypts(:,1:end-1) 选择了除了最后一个路径点之外的所有路径点，
    % 然后两者相减，得到了相邻路径点之间的位移。
    % waypts：
    % 0 1 2 4 5
    % 0 2 0 5 2
    % waypts(:,2:end)
    % 冒号表示取到了所有的行，取到了2到最后一列
    % 1 2 4 5
    % 2 0 5 2
    % 0 1 2 4  = 1  1  2  1
    % 0 2 0 5  = 2 -2  5 -3  其实就是相邻的点进行相减
    x = waypts(:,2:end) - waypts(:,1:end-1);
    % 计算了相邻路径点之间的欧几里得距离。x.^2 将 x 中的每个元素平方，然后 sum(x.^2,1) 对每列进行求和，
    % 最后 .^0.5 对每个和取平方根，得到相邻路径点之间的欧几里得距离。
    dist = sum(x.^2,1).^0.5;
    % 计算了一个缩放因子 k，它是总时间 T 与所有相邻路径点之间的距离之和的比值。
    k = T/sum(dist);
    % 构建了时间戳 ts。cumsum(dist*k) 计算了距离的累积和，然后在前面加了一个零，以确保起始时间戳为零。
    ts = [0 cumsum(dist*k)];
end

        我们指定了总时间T=5秒，这里做的就是根据路程进行线性分段。

    %% XY轴分开计算
    % 传入参数 第一行的所有列就是所有的x 每一段的时间 多项式阶 起点终点的加速度
    polys_x = minimum_snap_single_axis_simple(waypts(1,:),ts,n_order,v0(1),a0(1),v1(1),a1(1));
    polys_y = minimum_snap_single_axis_simple(waypts(2,:),ts,n_order,v0(2),a0(2),v1(2),a1(2));

        这里就是重头戏了，进行路径规划，包括求Q矩阵与约束的构建。

% 传入参数 第一行的所有列就是所有的x 每一段的时间 多项式阶 起点终点的加速度
function polys = minimum_snap_single_axis_simple(waypts,ts,n_order,v0,a0,ve,ae)
% 起点终点的位置
p0 = waypts(1);
pe = waypts(end);

% 多项式的段数:航路点-1
n_poly = length(waypts)-1;
% 参数个数 = 阶数+1
n_coef = n_order+1;

% 构建优化目标 优化目标的数量为多项式的段数
Q_all = [];
for i=1:n_poly
    % 构建对角矩阵（有几个多项式就有几个Q）
    % 在每次循环中，它调用了一个名为 computeQ 的函数
    % 传递了四个参数：n_order（多项式的阶数）、3（对jack 4阶导数进行优化）、ts(i)这一段的起始时间 和 ts(i+1)这一段的结束时间。
    % computeQ 函数的返回值是一个对角矩阵，这个对角矩阵将会被加入到 Q_all 中，通过 blkdiag 函数实现对 Q_all 的更新。
    Q_all = blkdiag(Q_all,computeQ(n_order,3,ts(i),ts(i+1)));

        这里我们传进来了起点和终点的x坐标。n_poly表示多项式的段数，也就是我们求的路程有几段，为航路点个数-1 = 4段。n_coef就是p矩阵的阶数，我们采用5阶（n_order）项+一个常数项的表示方法，即：

% 传入参数 个体的路径 地图 一行有多少个元素x
% 它告诉 MATLAB 这是一个名为 generate_continuous_path 的函数，它接受三个输入参数 single_pop，G 和 x，并且它会返回一个名为 single_new_pop 的变量。
function [single_new_pop] = generate_continuous_path(single_pop, G, x)
i = 1;
single_new_pop = single_pop;
% 这行代码调用了 size 函数来获取变量 single_new_pop 的大小，并将结果存储在了 single_path_num 变量中。
% 在这里，使用了波浪线 ~ 来表示忽略了 size 函数返回的第一个值（也就是行数），只保留了列数。
% single_new_pop 是一个 1 * 20的向量
% single_path_num 存储了路径经过的点的数量
[~, single_path_num] = size(single_new_pop);

% 遍历每一列 1-20
while i ~= single_path_num
    % 点i所在列 (从左到右编号)
    x_now = mod(single_new_pop(1, i), x) + 1; 
    % 点i所在行 (从上到下编号)
    y_now = fix(single_new_pop(1, i) / x) + 1;
    % 点i+1所在行 (从上到下编号)
    x_next = mod(single_new_pop(1, i + 1), x) + 1;
    y_next = fix(single_new_pop(1, i + 1) / x) + 1;
    
    % 初始化最大迭代次数
    max_iteration = 0;
    
    % 如果他们不相连的话
    while max(abs(x_next - x_now), abs(y_next - y_now)) > 1
        x_insert = floor((x_next + x_now) / 2);
        y_insert = floor((y_next + y_now) / 2);
        
        % 取得两者中点值
        if G(y_insert, x_insert) == 0  
            % 栅格值
            num_insert = (x_insert - 1) + (y_insert - 1) * x;
            % single_new_pop 是一个数组，这段代码的目的是将 num_insert 插入到 single_new_pop 的第 i 个位置之后。
            % single_new_pop(1, 1:i) 表示取 single_new_pop 中第1行，从第1列到第i列的元素。
            % single_new_pop(1, i+1:end) 表示取 single_new_pop 中第1行，从第i+1列到最后一列的元素。
            single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
            
        % 如果是障碍物的话
        else   
            % 它检查该点左面是否有路径 
            % (x_insert - 2) + (y_insert - 1) * x 是否不等于 single_new_pop(1, i)。
            if G(y_insert, x_insert - 1) == 0 && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i)) && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert - 1;
                % 索引号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                               
            elseif G(y_insert, x_insert + 1) == 0 && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i)) && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert + 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            elseif G(y_insert + 1, x_insert) == 0 && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i)) && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i+1))
                y_insert = y_insert + 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];

            elseif  G(y_insert - 1, x_insert) == 0 && ((x_insert - 1) + (y_insert - 2) * x ~= single_new_pop(1, i)) && ((x_insert - 1) + (y_insert-2) * x ~= single_new_pop(1, i+1))
                y_insert = y_insert - 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            % 左右上下都是占据的则删去路径
            else
                %break_pop = single_new_pop
                single_new_pop = [];
                break
            end    
        end
        
        x_next = x_insert;
        y_next = y_insert;
        max_iteration = max_iteration + 1;
        if max_iteration > 20000
            single_new_pop = [];
            break
        end
        
    end
    
    if isempty(single_new_pop)
        break
    end
    
    [~, single_path_num] = size(single_new_pop);
    i = i + 1;
end

        了解这些后，我们通过ComputeQ计算Q矩阵，并通过blkdiag合并一个个Q矩阵，最终组成如下形式：

        其中k为多项式的段数，也就是说有几段路程，就有几个Q矩阵。

        我们看一下Q矩阵的计算方法：

% n:polynormial order
% r:derivertive order, 1:minimum vel 2:minimum acc 3:minimum jerk 4:minimum snap
% t1:start timestamp for polynormial
% t2:end timestap for polynormial
% n=5 五次多项式拟合路径 r=3 用jerk模拟 t1 t2 
function Q = computeQ(n,r,t1,t2)
% (n-r)*2+1 = (5-3)*2+1 = 5
T = zeros((n-r)*2+1,1);
for i = 1:(n-r)*2+1
    T(i) = t2^i-t1^i;
end
% 理论说过Q是n+1 * n+1的维度  也就是7*7的维度  r是3 表示用jerk进行优化
% 它的矩阵就右下角有值 
Q = zeros(n+1);
for i = r+1:n+1     % 行 从r+1开始，因为前r行都是0，在PPT中r=4。在我们的实例中r=3
    for j = i:n+1   % 列 从r+1开始
        k1 = i-r-1; %对应r-3  行-r-1
        k2 = j-r-1; %对应c-3  列-r-1
        k = k1+k2+1;% r-3+c-3+1
        % prod是连乘的意思  prod(k1+1:k1+r)= r-3+1 *...* r-3+3 =
        % (r-3)(r-2)(r-1)r  (c-3)(c-2)(c-1)c k就是参数 
        Q(i,j) = prod(k1+1:k1+r)*prod(k2+1:k2+r)/k*T(k);
        Q(j,i) = Q(i,j);
    end
end
%Q

        这里我们传来的n=5，即用五次项拟合曲线，r=4用三阶导数去求极值。

        T矩阵就是后面的那块：

        这里是对应于4阶snap来说的，我们用三阶作为优化目标那么我们的T矩阵就应当是。前三排前三列是0的，因此有值的就是第4行到第n+1=6行。因为是对角矩阵，我们只需算出其中的一半也就是6个值。如下Q矩阵：

      我们初始化矩阵为的方阵，从r+1行也就是3+1=4行开始赋值，比如第四行，从第四列一直到第n+1列也就是第6列，再比如第五行，从第五列一直到第n+1列也就是第6列。

        首先计算r-3，也就是行-3，i表示遍历哪一行，j表示遍历哪一列，因此我们不妨想想当i与j都为4的时候，行-3对应4-3-1！=0，我们补足-1就是0了。k对应r-3+c-3+1。就是T(k)。

        这里我们就理解T矩阵的赋值了，因为它所包含的最高项的次数为r-3+c-3+1的max r和c，r和cmax也就是n+1，因此它的最高项次数也就是 （n-3）*2 + 1。到此，我们的Q矩阵就构造完毕啦。

        我们打印一下Q矩阵：

        我们来建立约束方程：

% 传入参数 个体的路径 地图 一行有多少个元素x
% 它告诉 MATLAB 这是一个名为 generate_continuous_path 的函数，它接受三个输入参数 single_pop，G 和 x，并且它会返回一个名为 single_new_pop 的变量。
function [single_new_pop] = generate_continuous_path(single_pop, G, x)
i = 1;
single_new_pop = single_pop;
% 这行代码调用了 size 函数来获取变量 single_new_pop 的大小，并将结果存储在了 single_path_num 变量中。
% 在这里，使用了波浪线 ~ 来表示忽略了 size 函数返回的第一个值（也就是行数），只保留了列数。
% single_new_pop 是一个 1 * 20的向量
% single_path_num 存储了路径经过的点的数量
[~, single_path_num] = size(single_new_pop);

% 遍历每一列 1-20
while i ~= single_path_num
    % 点i所在列 (从左到右编号)
    x_now = mod(single_new_pop(1, i), x) + 1; 
    % 点i所在行 (从上到下编号)
    y_now = fix(single_new_pop(1, i) / x) + 1;
    % 点i+1所在行 (从上到下编号)
    x_next = mod(single_new_pop(1, i + 1), x) + 1;
    y_next = fix(single_new_pop(1, i + 1) / x) + 1;
    
    % 初始化最大迭代次数
    max_iteration = 0;
    
    % 如果他们不相连的话
    while max(abs(x_next - x_now), abs(y_next - y_now)) > 1
        x_insert = floor((x_next + x_now) / 2);
        y_insert = floor((y_next + y_now) / 2);
        
        % 取得两者中点值
        if G(y_insert, x_insert) == 0  
            % 栅格值
            num_insert = (x_insert - 1) + (y_insert - 1) * x;
            % single_new_pop 是一个数组，这段代码的目的是将 num_insert 插入到 single_new_pop 的第 i 个位置之后。
            % single_new_pop(1, 1:i) 表示取 single_new_pop 中第1行，从第1列到第i列的元素。
            % single_new_pop(1, i+1:end) 表示取 single_new_pop 中第1行，从第i+1列到最后一列的元素。
            single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
            
        % 如果是障碍物的话
        else   
            % 它检查该点左面是否有路径 
            % (x_insert - 2) + (y_insert - 1) * x 是否不等于 single_new_pop(1, i)。
            if G(y_insert, x_insert - 1) == 0 && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i)) && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert - 1;
                % 索引号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                               
            elseif G(y_insert, x_insert + 1) == 0 && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i)) && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert + 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            elseif G(y_insert + 1, x_insert) == 0 && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i)) && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i+1))
                y_insert = y_insert + 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];

            elseif  G(y_insert - 1, x_insert) == 0 && ((x_insert - 1) + (y_insert - 2) * x ~= single_new_pop(1, i)) && ((x_insert - 1) + (y_insert-2) * x ~= single_new_pop(1, i+1))
                y_insert = y_insert - 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            % 左右上下都是占据的则删去路径
            else
                %break_pop = single_new_pop
                single_new_pop = [];
                break
            end    
        end
        
        x_next = x_insert;
        y_next = y_insert;
        max_iteration = max_iteration + 1;
        if max_iteration > 20000
            single_new_pop = [];
            break
        end
        
    end
    
    if isempty(single_new_pop)
        break
    end
    
    [~, single_path_num] = size(single_new_pop);
    i = i + 1;
end

%求解F（不等式） 构建全是0的向量
b_all = zeros(size(Q_all,1),1);

% 构建等式约束 4K+2 多项式的个数*每个项的系数
Aeq = zeros(4*n_poly+2,n_coef*n_poly);
beq = zeros(4*n_poly+2,1);

% 起点/终点的位置速度和加速度  (6 equations)
% 1-3行 第一个参数到n_order+1=6 位置、速度、加速度
Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);
                     calc_tvec(ts(1),n_order,1);
                     calc_tvec(ts(1),n_order,2)];
% 终点约束4-6
Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
                    [calc_tvec(ts(end),n_order,0);
                     calc_tvec(ts(end),n_order,1);
                     calc_tvec(ts(end),n_order,2)];
beq(1:6,1) = [p0,v0,a0,pe,ve,ae]';

% r=0:pos  1:vel  2:acc 3:jerk
% n_order 多项式的阶数
% t       第i段占据的时间
% r 0 1 2 对应 位置 速度 加速度
function tvec = calc_tvec(t,n_order,r)
    tvec = zeros(1,n_order+1);
    for i=r+1:n_order+1
        tvec(i) = prod(i-r:i-1)*t^(i-r-1);
    end
end

        回顾一下，我们n_poly，它的值为多项式的段数也就是航路点-1，本例中n_poly =4

        n_coef为多项式的次数+1，这里我们为6。

        先看一下它的形式吧：我们打印一下约束方程的Aeq

        它的行数为18（4*n_poly+2），列数为24（n_coef *n_poly ）。我们看看它里面存储的是什么吧！

        1-6行我们加入起点和终点的约束（位移、速度、加速度）：

% 起点/终点的位置速度和加速度  (6 equations)
% 1-3行 第一个参数到n_order+1=6 位置、速度、加速度
Aeq(1:3,1:n_coef) = [calc_tvec(ts(1),n_order,0);
                     calc_tvec(ts(1),n_order,1);
                     calc_tvec(ts(1),n_order,2)];
% 终点约束4-6
Aeq(4:6,n_coef*(n_poly-1)+1:n_coef*n_poly) = ...
                    [calc_tvec(ts(end),n_order,0);
                     calc_tvec(ts(end),n_order,1);
                     calc_tvec(ts(end),n_order,2)];

        起点约束放在起点的位置上，因为一个约束是有5阶+1个常数项的，因此一个约束占据6列。一共4段路线，是这么来的24行。

        从第七行开始固定中间的点。从第七行开始到第十行。

neq = 6;
% 从第七行开始
for i=1:n_poly-1
    neq=neq+1;
    Aeq(neq,n_coef*i+1:n_coef*(i+1)) = calc_tvec(ts(i+1),n_order,0);
    beq(neq) = waypts(i+1);
end

        位置为行数，一个占据自己的六个位置，如下：

        下面开始连续性约束：

% 段数-1 continuous constraints  ((n_poly-1)*3 equations)
for i=1:n_poly-1
    tvec_p = calc_tvec(ts(i+1),n_order,0);
    tvec_v = calc_tvec(ts(i+1),n_order,1);
    tvec_a = calc_tvec(ts(i+1),n_order,2);
    neq=neq+1;
    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_p,-tvec_p];
    neq=neq+1;
    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_v,-tvec_v];
    neq=neq+1;
    Aeq(neq,n_coef*(i-1)+1:n_coef*(i+1))=[tvec_a,-tvec_a];
end

        对位移、速度、加速度进行约束。n_coef*(i-1)+1:n_coef*(i+1)代表每一次处理一个点，如下：

        圈起来的就是一对邻接点的位移、速度、加速度。

        我们添加完约束之后，开始求解

Aieq = [];
bieq = [];

p = quadprog(Q_all,b_all,Aieq,bieq,Aeq,beq);

polys = reshape(p,n_coef,n_poly);

        我们这里没有不等式约束，故设置Q_all,b_all为空，我们求解的变量就是p。

        是不是很简单呢？

        看一下运行结果：

3 完整代码

% 传入参数 个体的路径 地图 一行有多少个元素x
% 它告诉 MATLAB 这是一个名为 generate_continuous_path 的函数，它接受三个输入参数 single_pop，G 和 x，并且它会返回一个名为 single_new_pop 的变量。
function [single_new_pop] = generate_continuous_path(single_pop, G, x)
i = 1;
single_new_pop = single_pop;
% 这行代码调用了 size 函数来获取变量 single_new_pop 的大小，并将结果存储在了 single_path_num 变量中。
% 在这里，使用了波浪线 ~ 来表示忽略了 size 函数返回的第一个值（也就是行数），只保留了列数。
% single_new_pop 是一个 1 * 20的向量
% single_path_num 存储了路径经过的点的数量
[~, single_path_num] = size(single_new_pop);

% 遍历每一列 1-20
while i ~= single_path_num
    % 点i所在列 (从左到右编号)
    x_now = mod(single_new_pop(1, i), x) + 1; 
    % 点i所在行 (从上到下编号)
    y_now = fix(single_new_pop(1, i) / x) + 1;
    % 点i+1所在行 (从上到下编号)
    x_next = mod(single_new_pop(1, i + 1), x) + 1;
    y_next = fix(single_new_pop(1, i + 1) / x) + 1;
    
    % 初始化最大迭代次数
    max_iteration = 0;
    
    % 如果他们不相连的话
    while max(abs(x_next - x_now), abs(y_next - y_now)) > 1
        x_insert = floor((x_next + x_now) / 2);
        y_insert = floor((y_next + y_now) / 2);
        
        % 取得两者中点值
        if G(y_insert, x_insert) == 0  
            % 栅格值
            num_insert = (x_insert - 1) + (y_insert - 1) * x;
            % single_new_pop 是一个数组，这段代码的目的是将 num_insert 插入到 single_new_pop 的第 i 个位置之后。
            % single_new_pop(1, 1:i) 表示取 single_new_pop 中第1行，从第1列到第i列的元素。
            % single_new_pop(1, i+1:end) 表示取 single_new_pop 中第1行，从第i+1列到最后一列的元素。
            single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
            
        % 如果是障碍物的话
        else   
            % 它检查该点左面是否有路径 
            % (x_insert - 2) + (y_insert - 1) * x 是否不等于 single_new_pop(1, i)。
            if G(y_insert, x_insert - 1) == 0 && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i)) && ((x_insert - 2) + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert - 1;
                % 索引号
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                % 插入
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                               
            elseif G(y_insert, x_insert + 1) == 0 && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i)) && (x_insert + (y_insert - 1) * x ~= single_new_pop(1, i+1))
                x_insert = x_insert + 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            elseif G(y_insert + 1, x_insert) == 0 && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i)) && ((x_insert - 1) + y_insert * x ~= single_new_pop(1, i+1))
                y_insert = y_insert + 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];

            elseif  G(y_insert - 1, x_insert) == 0 && ((x_insert - 1) + (y_insert - 2) * x ~= single_new_pop(1, i)) && ((x_insert - 1) + (y_insert-2) * x ~= single_new_pop(1, i+1))
                y_insert = y_insert - 1;
                num_insert = (x_insert - 1) + (y_insert - 1) * x;
                single_new_pop = [single_new_pop(1, 1:i), num_insert, single_new_pop(1, i+1:end)];
                
            % 左右上下都是占据的则删去路径
            else
                %break_pop = single_new_pop
                single_new_pop = [];
                break
            end    
        end
        
        x_next = x_insert;
        y_next = y_insert;
        max_iteration = max_iteration + 1;
        if max_iteration > 20000
            single_new_pop = [];
            break
        end
        
    end
    
    if isempty(single_new_pop)
        break
    end
    
    [~, single_path_num] = size(single_new_pop);
    i = i + 1;
end