DR_CAN之最优控制学习笔记

PS: 本文为DR_CAN之最优控制学习笔记，也转载了其他博主的一些内容。原B站视频链接如下：【最优控制】1_最优控制问题与性能指标_哔哩哔哩_bilibili

一.最优控制问题与性能指标

1.独轮车模型

​

这是一个多输入、多状态变量的非线性系统。选取合适的采样周期,将其离散化后可得：

其中，是离散化后的f()，将系统离散化可以更加方便地使用数字控制器。

​

我们需要求解J，使其最小化 ，输出能耗最小、使X接近Xd，可给定权重。

 

二.动态规划 

最优化理论：

An optimal policy has the property that whatever the initial state and initial decision are, the remaining decisions must constitude an optimal policy with regard to the state resulting from the first decision. 

一个最优策略具有这样的性质: 无论初始状态和初始决策是什么，剩余的决策对于第一个决策产生的状态都必须构成一个最优策略。即优化是动态的、面向未来的。

1.无人机高度控制

 

 

2.代码详解 

通用代码，不管是线性或非线性都能使用。

%%DP通用代码
clc;clear all;close all;
%Define IC
h_init =0;%height
v_init = 0;%velocity
%Final state
h_final = 10;
v_final = 0;
%Boundary condition
h_min = 0;
h_max =10;
N_h = 5;
v_min = 0;
v_max = 3;
N_v = 3;
%Creat state array
Hd = h_min : (h_max-h_min)/N_h : h_max;
Vd = v_min : (v_max-v_min)/N_v : v_max;
%Input constraint,acceleration
u_min = -3;
u_max = 2;
%Define cost to go matrix
J_costtogo = zeros(N_h+1,N_v+1);
%Define input accelearation matrix
input_acc = zeros(N_h+1,N_v+1);
%%%%%%%%%%%%% From 10m to 8m %%%%%%%%%%%%%%
v_avg = 0.5 * (v_final + Vd);%Average speed
T_delta = (h_max - h_min)./(N_h * v_avg);%Travel time
acc = (0 - Vd)./T_delta;%Calculate acceleration
J_temp = T_delta;%Assign delta T to cost to go
[acc_x,acc_y] = find(acc <u_min | acc > u_max);%Find which acc is over the limit
Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y);%Find linear index使用sub2ind函数将指定的坐标(acc_x, acc_y)转换为acc矩阵的线性索引.
J_temp (Ind_lin_acc) = inf; %Let certain elements to infitiy将该线性索引对应的J_temp矩阵的元素设为正无穷。
J_costtogo(2,:) = J_temp;%Save to cost to go matrix
input_acc(2,:) = acc;%Save to acceleration matrix
%%%%%%%%%%%%% From 8m to 2m %%%%%%%%%%%%%%
for k = 3:1:N_h
[Vd_x,Vd_y] = meshgrid(Vd , Vd);%Prepare the matrix
v_avg = 0.5 * (Vd_x + Vd_y);
T_delta = (h_max - h_min)./(N_h * v_avg);%Travel time
acc = (Vd_y - Vd_x)./T_delta;%Calculate acceleration
J_temp = T_delta;%Assign delta T to cost to go
[acc_x,acc_y] = find(acc <u_min | acc > u_max);%Find which acc is over the limit
Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y);%Find linear index使用sub2ind函数将指定的坐标(acc_x, acc_y)转换为acc矩阵的线性索引.
J_temp (Ind_lin_acc) = inf; %Let certain elements to infitiy将该线性索引对应的J_temp矩阵的元素设为正无穷。
%%%%%%%%%%%%%%%%%%%%%Important step%%%%%%%%%%%%%%%%%%%%%%%%%%
J_temp = J_temp +meshgrid(J_costtogo(k-1,:))';%Add last cost to go
[J_costtogo(k,:),l] = min(J_temp)%Save to cost to go matrix
Ind_lin_acc = sub2ind (size(J_temp), l,1:length(l));%Find linear index
input_acc(k,:) = acc(Ind_lin_acc)%Save to acceleration matrix
end

%%%%%%%%%%%%% From 2m to 0m %%%%%%%%%%%%%%
v_avg = 0.5 * (v_init + Vd);%Average speed
T_delta = (h_max - h_min)./(N_h * v_avg);%Travel time
acc = (Vd - v_init)./T_delta;%Calculate acceleration
J_temp = T_delta;%Assign delta T to cost to go
[acc_x,acc_y] = find(acc <u_min | acc > u_max);%Find which acc is over the limit
Ind_lin_acc = sub2ind (size(acc),acc_x,acc_y);%Find linear index使用sub2ind函数将指定的坐标(acc_x, acc_y)转换为acc矩阵的线性索引.
J_temp (Ind_lin_acc) = inf; %Let certain elements to infitiy将该线性索引对应的J_temp矩阵的元素设为正无穷。
J_temp = J_temp +J_costtogo(N_h,:);%Add last cost to go
[J_costtogo(N_h+1,1),l] = min(J_temp)%Save to cost to go matrix
Ind_lin_acc = sub2ind (size(J_temp), l);%Find linear index
input_acc(N_h+1,1) = acc(Ind_lin_acc);%Save to acceleration matrix

%%%%%%%%%%%%%% Plot %%%%%%%%%%%%%%%%%%%%

h_plot_init = 0; % Initial height

v_plot_init = 0; % Initial velocity

acc_plot = zeros(length(Hd),1); % Define acc plot array

v_plot = zeros(length(Hd),1);% Define velocity plot array

h_plot = zeros(length(Hd),1);% Define height plot array

h_plot (1) = h_plot_init; % First value

v_plot (1) = v_plot_init; % First value

for k = 1 : 1 : N_h

[min_h,h_plot_index] = min(abs(h_plot(k) - Hd)); % Table look up

[min_v,v_plot_index] = min(abs(v_plot(k) - Vd)); % Table look up

acc_index = sub2ind(size(input_acc), N_h+2-h_plot_index, v_plot_index); % Find control input /acceleration

acc_plot (k) = input_acc(acc_index); % Save acceleration to the matrix

v_plot (k + 1) = sqrt((2 * (h_max - h_min)/N_h * acc_plot(k))+ v_plot (k)^2); % Calculate speed and height

h_plot (k + 1) = h_plot(k) + (h_max - h_min)/N_h;

end

% Plot

subplot(2,1,1);

plot(v_plot,h_plot,'--^'),grid on;

ylabel('h(m)');

xlabel('a(m/s^2)');

subplot(2,1,2);

plot(acc_plot,h_plot,'^'),grid on;

ylabel('h(m)');

xlabel('v(m/s)');