最优控制-动态规划-drcan-2

视频教程–drcan老师

image-20231204190525728

image-20231204190531823

image-20231204190538308

image-20231204190555308

image-20231204190600957

采用逆向分级的方法编写代码，进行求解

Dynamic_program_1.m

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
clc;clear all;close all;
% 初始状态
h_init = 0;
v_init = 0;

% 目标状态
h_final = 10;
v_final = 0;

% 约束条件 速度--加速度
h_min = 0;
h_max = 10;
N_h = 5; % 按照2米一个间隔，将10m分为5个间隔
v_min = 0;
v_max = 3;
N_v = 3; % 将速度分为 0-1-2-3，三个间隔

% 创建状态数组
Hd = h_min:(h_max - h_min)/N_h:h_max;  % 高度 0-2-4-6-8-10
Vd = v_min:(v_max-v_min)/N_v:v_max;    % 速度 0-1-2-3

% 定义输入的约束(加速度)
u_min = -3;u_max = 2;

% 建立代价矩阵
J_costtogo = zeros(N_h+1,N_v+1); 
% 创建加速度输入矩阵
Input_acc = zeros(N_h+1,N_v+1); 

% 从10m到8m
v_avg = 0.5 * (v_final + Vd); % 平均速度
T_delta = (h_max - h_min)./(N_h*v_avg); % 时间
acc = (0-Vd)./T_delta; % 加速度
J_temp = T_delta;  % 临时代价，还需要考虑系统的约束
[acc_x,acc_y] = find(acc < u_min | acc > u_max); % 找到加速度超过约束的
Ind_lin_acc  = sub2ind(size(acc),acc_x,acc_y); % 找到矩阵acc对应位置的索引号
J_temp(Ind_lin_acc) = inf; % 加速度超过约束的，代价变为无穷
J_costtogo(2,:) = J_temp; % 将代价进行存储
Input_acc(2,:) = acc;     % 将加速度进行存储

% 从8m到2m
for k = 3:1:N_h
[Vd_x,Vd_y] = meshgrid(Vd,Vd);
v_avg = 0.5 * (Vd_x + Vd_y); % 从8m至6m有16中选择组合，需要计算16种选择各自的平均速度
T_delta = (h_max - h_min)./(N_h * v_avg);
acc = (Vd_y - Vd_x) ./ T_delta;
J_temp = T_delta;
[acc_x,acc_y] = find(acc < u_min | acc > u_max); % 找到加速度超过约束的
Ind_lin_acc  = sub2ind(size(acc),acc_x,acc_y); % 找到矩阵acc对应位置的索引号
J_temp(Ind_lin_acc) = inf;  % 加速度超过约束的，代价变为无穷
% 重要的一步
J_temp = J_temp + meshgrid(J_costtogo(k-1,:))'; % 将上一步的代价添加上
[J_costtogo(k,:),l] = min(J_temp);  % 得到最小的J_temp存储到下一行代价矩阵
Ind_lin_acc = sub2ind(size(J_temp),l,1:length(l));
Input_acc(k,:) = acc(Ind_lin_acc);
end

% 从2m至0m
v_avg = 0.5 * (Vd+v_init); % 平均速度
T_delta = (h_max - h_min)./(N_h*v_avg); % 时间
acc = (Vd - v_init)./T_delta; % 加速度
J_temp = T_delta;  % 临时代价，还需要考虑系统的约束
[acc_x,acc_y] = find(acc < u_min | acc > u_max); % 找到加速度超过约束的
Ind_lin_acc  = sub2ind(size(acc),acc_x,acc_y); % 找到矩阵acc对应位置的索引号
J_temp(Ind_lin_acc) = inf; % 加速度超过约束的，代价变为无穷

% 重要的一步
J_temp = J_temp + J_costtogo(N_h,:);
[J_costtogo(N_h+1,1),l] = min(J_temp)
Ind_lin_acc = sub2ind(size(J_temp),l);
Input_acc(N_h+1,l) = acc(Ind_lin_acc)

运行结果：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
J_costtogo =

         0         0         0         0
       Inf    4.0000    2.0000    1.3333
    4.0000    2.3333    2.1333    2.0000
    4.1333    3.0000    2.8000    2.6667
    4.8000    3.6667    3.4667    3.3333
    5.4667         0         0         0


l =

     3


Input_acc =

         0         0         0         0
         0   -0.2500   -1.0000   -2.2500
    1.0000    2.0000    1.2500         0
    1.0000    2.0000    1.2500         0
    1.0000    2.0000    1.2500         0
         0         0    1.0000         0