最优控制学习

xiaoli0111

已于 2023-12-04 21:22:38 修改

阅读量137

点赞数

文章标签：学习

于 2023-12-04 11:14:23 首次发布

本文链接：https://blog.csdn.net/xiaoli0111/article/details/134778568

版权

性能指标

一、系统最终状态达到期望状态

二、消耗的能量（控制量）小

第一项是期望的状态和实际状态的差，S是每一项状态量的权重，第二项是能量消耗，R也是每一项控制量的权重。注意：在对每一项进行权重分配时，需要将他们先进行单位化，防止因为单位的不同导致天然的权重差的很多。

三、按照设计的轨迹运动

性能指标J里第一项是最终状态变量与期望状态变量的差，即系统最终是否到达了预设状态；第二项是每一时刻与期望目标轨迹的差（即希望系统按照既定的轨迹运动）；第三项是控制量，即能耗。

四、避开障碍

避障：x*是不会碰到障碍的所有轨迹，即容许轨迹，要求轨迹一直在容许轨迹的范围内，即增加约束条件。

总结最优化的目标

对系统的状态量进行控制与观测，找到一个控制策略（在容许控制的范围内） ，让系统从初始的状态量按照一定的轨迹转移到期望的状态量，并用系统最终状态量和每一时刻状态量（可能还有控制量）作为性能指标，实现控制。

动态规划

不管前面做的好与不好，在以当下时刻为初始状态时，后续的决策都是最优的决策。

就像迷宫游戏一样，从出口开始倒推

理解这段决策：

出口在最顶层
到第二层，从左往右的圈里，每种可能性的代价写出来
到第三层，从左往右，首先先看第一个圈，有多种可能到达第一个圈（最左边的圈），找到代价函数最小的路径，写到第一个圈里。这代表，如果机会真给到了第一个圈，那么圈里的数字就代表了他该怎么做。依次把这一层写满，这一层里的数字代表的含义是，如果机会真的站在了它们身边，它们最好的登顶方法。
写下最好的做法后，就可以将多余的路径舍弃，这就简化了运算。比如说第三层（0，6）处，一旦走到了（0，6），只需要知道必然沿着J=4登顶，其余的路径就可以舍弃。

容易陷入的一个误区

从上往下是一种便于理解的方法，而不是真正的方法。注意这里的第二层和第三层的关系，是第三层的最左侧有4种方法到达第二层。

为什么这么做？

暴力求解要算指数种可能，而动态规划只需要计算线性种可能。

最后怎么选择最优路径？

从入口开选，哪一个圈的剩余代价最小，又满足限制条件，就怎么走。即从出口开始往下写，再从入口开始往上选。就算中途走错了路，也能用这种方法走回最优路径。

程序实例

程序的目的

程序的目的，本质上也就是建立一张可供查找的表格，遍历此表，找到J最小且满足约束条件的项，即可得到最优路径。

变量、代价函数、限制条件的定义

%% 变量定义

%定义无人机初始时刻的状态量（高度、速度）
h_init=0;
v_init=0;
%定义无人机最终时刻的状态量
h_final=10;
v_final=0;
%设置边界条件
h_min=0;
h_max=10;N_h=5; %高度被离散化成5个分段
v_min=0;
v_max=3;N_v=3;%速度被离散化成3段，离散化的步长
%建立状态变量
Hd=h_min:(h_max-h_min)/N_h:h_max ;%h_d是一个离散化的高度向量，它表示从min开始，以一定的步长离散到max
Vd=v_min:(v_max-v_min)/N_v:v_max;
%定义输入量（控制量）的约束
u_min=-3;u_max=2;
%% J和acc

%建立剩余代价函数J_costtogo矩阵，本质是建立起一张表格，用来查询“怎样使J最小且满足约束条件”
J_costtogo=zeros(N_h +1,N_v +1); %1.列数是横轴 2.为什么要+1：5个分段，5+1个具体数值 0 2 4 6 8 10
%建立输入限制矩阵，同样是用来查表
Input_acc=zeros(N_h +1,N_v +1);

10-8m的实现

%% 从10m-8m

%J是以剩余时间组建起来的，delta_t=delta_h/average_v,average_v=(1/2)*(v1+v2),acc=delta_v/delta_t
v_avg=0.5*(v_final+Vd);               
delta_t=((h_max-h_min)/(N_h))./v_avg;    %注意矩阵里面的"./"和"/"
acc=(0-Vd)./delta_t;                     %这个算下来也只是第一列 注意：0-Vd,这是正确顺序，而不是决策顺序
J_temp=delta_t;                          %以时间为指标
%找到加速度超出约束限制的，把代价设为无穷
[acc_x,acc_y]=find(acc<u_min | acc>u_max);   %把不符合的acc的行列找出来，不符合约束的话，J就设为无穷，让它没有进入比较的机会
Ind_lin_acc=sub2ind (size(acc),acc_x,acc_y); %这里返回的是一个按列查找的索引号组成的矩阵
                                             %索引号指向了不满足约束条件
J_temp(Ind_lin_acc)=inf;                     %不符合的地方设为了无穷大
J_costtogo(2,:)=J_temp;
Input_acc(2,:)=acc;

Ind_lin_acc=sub2ind (size(acc),acc_x,acc_y);

这个函数sub2ind用于将加速度矩阵 acc 中不符合约束条件的位置的行列索引 (acc_x 和 acc_y) 转换为一维索引。这样就能够在后续的操作中方便地对这些位置进行处理，比如在 J_costtogo 矩阵中将对应的元素设置为无穷大。

8m-6m的实现

%% 从8m-6m
%平均速度是第一个核心变化
[Vd_x,Vd_y]=meshgrid(Vd,Vd);  
v_avg=0.5*(Vd_x+Vd_y);
delta_t=((h_max-h_min)/(N_h))./v_avg;
acc=(Vd_x-Vd_y)./delta_t; 
J_temp=delta_t;  
[acc_x,acc_y]=find(acc<u_min | acc>u_max);
Ind_lin_acc=sub2ind (size(acc),acc_x,acc_y);
J_temp(Ind_lin_acc)=inf;
%J矩阵是第二个核心变化
J_temp=J_temp+meshgrid(J_costtogo(2,:));
J_costtogo(3, :) = min(J_temp, [], 2);