路径规划与轨迹跟踪 3 —— 全局路径规划_动态规划算法

动态规划算法

算法思想

• 适用于多阶段决策，即每一个阶段决定什么状态，把每个阶段的状态确定了之后，再连起来，就是多阶段决策的最优解。划分为多个阶段，求每个阶段的最优值。

• 第 1 阶段

• 第 2 阶段
  
• 第 3 阶段
  
• 第 4 阶段
  

算法讲解

% 动态规划DP算法

clc
clear
close all

%% 阶段 - 状态定义(定义图的连接方式)
stages = 5; % A、B、C、D、E 一共5个阶段
nodes_dist = cell(stages, 3); % 定义节点和距离的 4 * 3 元胞数组
% 视频里 nodes_dist = cell(stages,3) 5 * 3的元胞数组 

% 第 1 阶段
% 第 1 阶段到第 2 阶段有 3 条路可选
nodes_dist{1,1} = 1;% 第 1 个阶段有 1 个状态值，按行定义
nodes_dist{1,2} = [1,2,3];% 第 2 个阶段有 3 个状态值，按列定义 1,2,3，一共有 1 * 3个值
nodes_dist{1,3} = [2,5,1];% 3个状态值对应的距离值分别是 2,5,1

% 第 2 阶段
% 第 2 阶段到第 3 阶段有 9 条路可选
nodes_dist{2,1} = [1;2;3];% 第 2 阶段有 3 个状态值，按行定义
nodes_dist{2,2} = [1,2,3];% 第 3 阶段有 3 个状态值，按列定义，一共有 3 * 3 = 9 个值
nodes_dist{2,3} = [12, 14, 10; 6, 10, 4; 13, 12, 11];% 9 个状态值对应的距离值

% 第 3 阶段
% 第 3 阶段到第 4 阶段有 6 条路可选
nodes_dist{3,1} = [1;2;3];% 第 3 阶段有 3 个状态值，按行定义
nodes_dist{3,2} = [1,2];% 第 4 阶段有 2 个状态值，按行定义，一共有 3 * 2 = 6 个值 
nodes_dist{3,3} = [3, 9; 6, 5; 8, 10];% 6 个状态值对应的距离值

% 第 4 阶段
% 第 4 阶段到第 5 阶段有 2 条路可选
nodes_dist{4,1} = [1;2];% 第 4 阶段有 2 个状态值，按行定义
nodes_dist{4,2} = 1;% 第 5 阶段有 1 个状态值，按行定义，一共有 2 * 1 = 2 个值
nodes_dist{4,3} = [5; 2];

% 第 5 阶段
nodes_dist{5,1} = 1;
nodes_dist{5,2} = 1;
nodes_dist{5,3} = 0;

% 最优路径及其距离值定义
path = cell(stages, 1);% 5*1的元胞数组
dist = cell(stages, 1);
for i = 1:stages-1 % nodes_dist 是 cell(4,3)
    dist{i, 1} = nodes_dist{i,1};
    % length(a)，求的是二维数组最大维度的元素个数，length * 1的inf矩阵
    dist{i, 2} = inf(length(dist{i, 1}), 1);% 初始化均设置为inf
    path{i, 1} = nodes_dist{i,1};% 路径
end
% 初始化第 5 阶段
dist{stages, 1} = 1; % (5,1)的元胞数组 设为1
dist{stages, 2} = 0;  
path{stages, 1} = 1;
path{stages, 2} = 1;

% 根据最后一个阶段，直接初始化

%% 逆向寻优
% num_states_f 表示前面一个状态数目，num_states_r 表示后面一个状态数目
% 当 i = 4时，num_states_f = 2，num_states_r = 1，第4阶段有2个状态，第5阶段有1个状态

% 第一层循环：逆向遍历每一个阶段( 这里阶段理解为箭头,有4个阶段)
for i = stages-1:-1:1 % 4,3,2,1
    num_states_f = length(nodes_dist{i, 1}); % i=4, f=2(第4阶段状态数目); i=3, f=3, f表示前面   

    % 第二层循环：遍历第i阶段的每一个状态
    for j = 1:num_states_f 
        num_states_r = length(nodes_dist{i+1, 1}); % f=2,j=1:2,r=1(第5阶段状态数目)       
        
        % 第三层循环：遍历第i阶段的第j个状态到第i+1阶段的每一条路径
        for k = 1:num_states_r % 初始时,i=4(第i阶段),j=1(第i阶段状态),k=1(第i+1阶段状态)
% 表示 第i+1阶段第k个状态的值 + 第i阶段的第j个状态的值到第i+1阶段第k个状态的值 < 第i阶段当前的第j个状态的值
            if  nodes_dist{i,3}(j,k) + dist{i+1,2}(k,1) < dist{i,2}(j,1)
                dist{i,2}(j,1) = nodes_dist{i,3}(j,k) + dist{i+1,2}(k,1);
                path{i, 2}(j,:) = [j, path{i+1, 2}(k,:)];
            end
        end
    end
end
            
%% 正向求解
path_opt =  path(1,:);    
dist_opt =  dist{1,2};