【DR_CAN控制理论学习】最优控制中的DP与LQR

Arikota

已于 2024-08-13 17:45:43 修改

阅读量583

点赞数 11

文章标签：学习算法动态规划

于 2024-08-13 16:01:27 首次发布

本文链接：https://blog.csdn.net/weixin_57968446/article/details/141158425

版权

绪

本笔记根据b站【DR_CAN】的最优控制教程整理，主要是梳理公式推导过程（不包含详细的计算），主要是作为后续工程应用的理论参考，特别是一些结论性的东西。
希望能对其他正在学习最优控制的朋友有帮助，如有错误也请帮忙指出，万分感谢。
附bilibili视频链接：【最优控制-合集】（不得不说，can老师真是全天下最好的控制老师！）

1.最优控制的基本概念

1.1 系统描述

任意系统的状态-空间方程：
- 状态变量： $x(t) =[ x_1(t) ,x_2(t) ,...,x_n(t) ]^T$ (即一个n维的列向量）
- 系统输入： $u(t) =[ u_1(t),u_2(t),...,u_m(t) ]^T$ (即一个m维的列向量）
- 例如对于一个七轴机械臂而言:
  - $x$ 可以是包含末端位置与线速度的向量，即 $n = 6$
  - $u$ 则可以为七个关节驱动器的控制量，可以为角度、扭矩等，均有 $m = 7$
- 此时系统状态空间可表示为： $\dot{\vec{x(t)}}=f(\vec{x(t)},\vec{u(t)})$
为了方便求解，可选取一个采样时间，将上述方程离散化，即
- $\large\color{black}\dot{\vec{x}}(k+1)=f_d(\vec{x}(k),\vec{u}(k))$

1.2 控制的性能指标

对任意控制，均存在一系列的期望目标与约束，我们希望的好的控制，应该是在设定的时间内使系统状态趋近于期望目标，同时在期间满足系统约束。
为了描述这种好的控制，我们引入一个性能指标方程，也可称为代价函数，而我们的控制目标就是最小化控制代价。
- 性能指标的一般形式为 $J=\int_0^N \phi(x,xd,u)$
- 最小化J后所得的输入u，即为最优控制策略
  - 表示为： $u^*=[u(0),u(1),u(2),...,u(N-1)]$
  - 也可写为： $u^*=argmin J$
对于不同的控制场景，J的具体形式也会不同，下面举几个例子
- 场景1：在N时刻将小车停在p点，类似这样的定时达点问题,性能指标可写为：
  - $J_1=(x(N)-x_d)^TQ(x(N)-x_d)$
    - Q一般为正对角矩阵，即 $Q=diag(s_1,s_2,...,s_n)$
    - 需要注意s必须提前标准化以排除各状态单位不同的影响

示例场景

场景2：在1的基础上，考虑能耗最小，那么性能可写为：
- $J_2=(x(N)-x_d)^TQ(x(N)-x_d)+\sum_{k=1}^{n-1} u_k^TRu_k$
  - R与Q的特点与作用一致，用来调整可控制量的权重
场景3：在2的基础上，加上轨迹跟踪要求，则有：
- $J_3=\begin{Vmatrix} x_N-x_d \end{Vmatrix}_S^2+\sum_{k=1}^{n-1} \begin{Vmatrix} x_k-x_{d_k} \end{Vmatrix}_Q^2+\begin{Vmatrix} u_k \end{Vmatrix}_R^2$
  - S、Q、R表示三个二次范数的对角权重矩阵
场景4：在3的基础上，加上循迹时的避障要求，则有：
- $J_4=J_3$
- 但同时有 $[x_1(k),x_2(k)]\in [x_1^*,x_2^*]$ ，即轨迹需在容许轨迹范围内。

1.3 最优控制的通用描述

对于系统： $\dot{\vec{x}}(k+1)=f_d(\vec{x}(k),\vec{u}(k),k)$
控制目标：
- 找到最优控制策略 $u^*\in\Omega$ ,使得 $\vec{x}\in X^*$ 从 $\vec{x}(0)$ 转移到 $\vec{x}_d$
- 并满足 $\color{black}min J=h_d(\vec{x}(N),N)+\sum_{k=1}^{N-1}g_d(\vec{x}(k),u(k),k$

2.动态规划的基本概念

2.1 最优化理论

贝尔曼最优化理论，由Richard·Bellman提出，大意为：
- 最优化策略拥有一个特性，即无论初始状态与策略为何，接下来的控制“基于过去决策所带来的结果”所得的最优策略。即最优控制应该是一种面向未来的动态规划。
  
  The best time to plant a tree is 20 years ago.The second-best time is
  now. 即如果过去选择了“不种树策略”导致了现在没有树的结果，那么此时最优的策略就是尽早把这棵树种好。
根据其核心思想，也就是说考虑当下的最小代价时，只需要基于上一时刻到这一时刻的最小即可，至于之前所积累的代价，当下并不考虑；如果每一时刻都是如此，即可保证每一时刻代价最小，进而达成全局最优。
- 那么当我们想要求全局代价，只需要根据这一思想列出递推方程，即可求解，即：
  - J(N)=J(N-1)+minJ(N)

2.2 动态规划（Dynamic Programming）

将动态规划问题离散化之后，就是一个逆向分级去求动态最小cost的问题。
利用二维数组+递归即可解决，即考虑每个状态最小的CostToGo（详细推导见Dr_Can视频）

[dp推导]

3.最优控制之LQR

设系统的代价函数为：
- $J=\frac{1}{2}x(N)^TSx(N)+\frac{1}{2}\sum_{k=1}^{n-1} x(k)^TQx(k)+u(k)^TRu(k)$
- 注意：这是一个regulator，要求 $\color{orange}x_d=0$ ，也可将其视为误差为零（增广变换之后）。
接下来，逆向递推
- k=n: $J^*_N=\frac{1}{2}x_N^TSx_N$
- k=n-1: $J^*_{N-1}=J^*_N+\frac{1}{2}x_{N-1}^TQx_{N-1}+u_{N-1}^TRu_{N-1}$
- 可以发现，此时任意的J均只于当前的输入u有关，因此对其求偏导并使偏导为0，即可求出此时的最优控制策略，即
  $u^*_{N-k}\bigg| {\frac{\partial J_{N-k}^*}{\partial u_{N-k}}=0}$
- 推理可得：
  - $J_{N-k}^*=\frac{1}{2}x_{N-k}^TP_kx_{N-k}$
  - $P_k=[A-BF_{N-k}]^TP_{k-1}(A-BF_{N-k})+F^T_{N-k}RF_{N-k}+Q$
  - $F_{N-k}=[B^TP_{k-1}B+R]^{-1}B^TP_{k-1}A$
  - $u^*_{N-k}=-F_{N-k} x_{N-k}$
控制思路：
- 利用递推关系，离线计算并存储 $u^*=[u^*_N,u^*_N-k,...,u^*_1]$
- 在线控制时按时间执行相应的 $u^*_k,k\in[1,N]$

4.轨迹跟踪控制

轨迹跟踪控制器大致框架（Tracking Controller）

4.1 目标误差控制

在轨迹追踪中，我们需要将调节目标转换为调节系统误差至0，具体方法是将目标状态列入原系统-空间方程，将其改写为增广后的形式。
具体过程如下：
- 原系统： $x_{k+1}=Ax_k+Bu_k$
- 设: $x_{d_{k+1}}=A_Dx_{d_k},A_D=I$
- 定义: $e_k=x_k-x_{d_k}$
- 令 $x_{a_{k+1}}=[x_{k+1},x_{d_{k+1}}]^T$
- 则有 $x_{a_{k+1}}=\begin{bmatrix} A & 0 \\ 0 & A_D \end{bmatrix}\begin{bmatrix} x_k\\ x_{d_k} \end{bmatrix}+\begin{bmatrix} B\\ 0 \end{bmatrix}u_k$
由此，我们可以将原状态空间方程写为：
- $\large x_{a_{k+1}}=A_ax_{a_k}+B_au_k$
此时 $e_k=x_k-x_{d_k}=[I -I]x_{a_k}=C_ax_{a_k}$
据此，我们将代价函数定义为： $\begin{equation}\begin{split} J &=\frac{1}{2}e(N)^TSe(N)+\frac{1}{2}\sum_{k=0}^{N-1} e(k)^TQe(k)+u(k)^TRu(k)\\ &=\frac{1}{2}x_a(N)^TC_a^TQC_ax_a(N)+\frac{1}{2}\sum_{k=0}^{N-1} x_a(k)^TC_a^TQC_ax_a(k)+u(k)^TRu(k)\\ &=\color{red}\frac{1}{2}x_a(N)^TS_ax_a(N)+\frac{1}{2}\sum_{k=0}^{N-1} x_a(k)^TQ_ax_a(k)+u(k)^TRu(k) \end{split}\nonumber\end{equation}$

4.1.0 应用局限性

基于上述代价函数，我们可将其应用至弹簧-阻尼-质量系统，并将目标终点设置为某个x值。但是会出现一个问题

我们会发现无论如何设置R的权重，即输入量的大小约束，目标最终都很难收敛至 $x_d$
这是因为对于这个系统，要想系统最终稳定在非零位，就必须要施加一个恒力，也就是代价无法缩小至零。
在这个优化过程中，如果R过大，系统便出现了“躺平”的现象，我们需要引入其它的方法来改进这一缺陷。

4.2 稳态目标控制

对于一些系统而言，系统存在稳态，即当输入为 $u_d$ 时，输出为 $x_d$ ，即 $x_d[k]=x_d$
此时系统表现为： $x_d=Ax_d+Bu_d$
- 可得： $u_d=B^{-1}(I-A)x_d$ ，需要注意: $B^{-1}$ 不一定存在（B不为方阵时），求解要注意方法
接下来，定义系统稳态输入误差为
- $\delta u_k=u_k-u_d$ 即 $u_k=\delta u_k+u_d$
- 联立系统状态空间方程即稳态时输入与输出的关系，可得
  - $x_{k+1}=Ax_k+B\delta u_k+(I-A)x_d$
参照4.1，我们可将目标输出列入状态矩阵，将状态-空间方程进行改写
- $x_{a_{k+1}}= \begin{bmatrix} x_{k+1}\\ x_d \end{bmatrix}= \begin{bmatrix} A & I-A \\ 0 & I \end{bmatrix} \begin{bmatrix} x_k\\ x_{d} \end{bmatrix} +\begin{bmatrix} B\\ 0 \end{bmatrix} \delta u_k$
- 即 $ $\color{black}\large x_{a_{k+1}}=A_ax_{a_k}+B_a\delta u_k$
由此，便可得到新系统的代价函数：
$J=\frac{1}{2}x_a(N)^TS_ax_a(N)+\frac{1}{2}\sum_{k=0}^{n-1} x_a(k)^TQ_ax_a(k)+\delta u(k)^TR\delta u(k)$
- 最后一项关于输入的代价，反映了当前输入相较于稳态输入的偏离程度

4.3 增量输入控制

在4.1中，我们将目标定义为： $x_{d_{k+1}}=A_Dx_{d_k}$ ，其中 $A_D=I$ 为常数项
如果在控制序列中， $x_d$ 在发生变化，则变成了轨迹跟踪问题，此时 $A_D$ 不为常数
为了处理这样的非稳态控制系统，我们需要对输入进行增量控制。
定义输入的增量为：
- $\Delta u_k=u_k-u_{k-1}$ $
- $u_k=\Delta u_k+u_{k-1}$
由此，系统状态-空间方程可写为： $x_{k+1}=Ax_k+B\Delta u_k+Bu_{k-1}$
设增广向量 $x_{a_k}=[x_{k},x_{d_{k}},u_{k-1}]^T$ ,那么可写出增广形式的状态空间方程如下
$\begin{equation}\begin{split}x_{a_{k+1}}&= \begin{bmatrix} x_{k+1}\\x_{d_{k+1}}\\u_k \end{bmatrix}= \begin{bmatrix} A & 0 &B\\ 0 & A_D & 0\\ 0 & 0 & I \end{bmatrix} \begin{bmatrix} x_k\\x_{d_k}\\u_{k-1} \end{bmatrix}+ \begin{bmatrix} B\\0 \\I \end{bmatrix} \Delta u_k\\\\ & = A_ax_{a_k}+B_a\Delta u_k \end{split}\nonumber\end{equation}$
同理4.1与4.2，可将代价函数定义为：
$J=\frac{1}{2}x_a(N)^TS_ax_a(N)+\frac{1}{2}\sum_{k=0}^{n-1} x_a(k)^TQ_ax_a(k)+\Delta u(k)^TR\Delta u(k)$
- 通过调节R的值，便可让输入按期望的平滑度进行控制

4.3.1 实例与结论

按照上述方法，如果我们想让弹簧-阻尼-质量系统中的滑块匀速运动，则有
- $\dot x_1=x_{2_d}$ ， $x_1$ 表示位移， $x_2$ 表示速度
- $\dot{x}_{2_d}=0$ ，即加速度为0
将系统离散化，即选取一个采样时间间隔 $T_s$ 后，可得
$x_{d_{k+1}}=\begin{bmatrix}1&T_s\\0&1\end{bmatrix}x_{d_k}=A_Dx_{d_k}$
- 之后，设计 $x_{2_d}$ 关于时间的函数 $X_{2_d}(k)$ ，应用LQR控制，即可实现轨迹跟踪控制
其它结论
- 如果轨迹为非线性函数，则需要先对其做线性化处理。
- 轨迹跟踪的本质，就是通过矩阵变换将Tracking问题转为Regulation问题，再应用诸如LQR的调节方法，即可完成轨迹跟踪控制。