浅谈线性定常系统最优跟踪问题——Optimal Tracking of LTI Systems

本文简单介绍下线性定常系统（Linear Time-Invariant System）的最优跟踪问题，主要介绍L-Q问题(LQT)。

本文主要讨论跟踪（Tracking）问题，不详细讨论调节（Regulation）问题，且本文中均假设参考轨迹在任意时刻已知，即不讨论伺服问题和模型跟踪问题。

离散时间系统

姑且不考虑观测器问题，假设系统状态完全可得，考虑如下线性定常离线时间系统： $${\xi }_{t+1}=A{\xi }_t+B{u}_t$$ 或者展开写成更清晰的形式： $$\left[\begin{array}{c}{x}_{t+1}\\ {\dot{x}}_{t+1}\end{array}\right]=\left[\begin{array}{cc}I& \Delta t\cdot I\\ 0& I\end{array}\right]\left[\begin{array}{c}{x}_t\\ {\dot{x}}_t\end{array}\right]+\left[\begin{array}{c}0\\ \Delta t\cdot I\end{array}\right]u_t$$ 其中，$x_t\in {\mathbb{R}}^D,{\xi }_t\in {\mathbb{R}}^{CD},u_t\in {\mathbb{R}}^D$.

批处理方法（Batch Form）

那么，定义LQ指标为：
$$C=({\hat{\xi }}_T-{\xi }_T{)}^{\top }{Q}_T({\hat{\xi }}_T-{\xi }_T)+\sum _{t=1}^{T-1}\left(({\hat{\xi }}_t-{\xi }_t{)}^{\top }{Q}_t({\hat{\xi }}_t-{\xi }_t)+u_t^{\top }{R}_t{u}_t\right)$$ 其中 $Q$为半正定矩阵，$R$为正定矩阵。将上式写成更紧凑形式：$$C=(\hat{\xi }-\xi {)}^{\top }\tilde{Q}(\hat{\xi }-\xi )+U^{\top }\tilde{R}U$$ 其中 $\xi \in {\mathbb{R}}^{TCD},U\in {\mathbb{R}}^{(T-1)D}$分别是状态和输入向量，$\hat{\xi }\in {\mathbb{R}}^{TCD}$为参考状态，可以看出 $$\tilde{Q}=diag(Q_1,Q_2,\dots ,Q_T)\in {\mathbb{R}}^{TCD\times TCD}$$ 且 $$\tilde{R}=diag(R_1,R_2,\dots ,R_T)\in {\mathbb{R}}^{(T-1)CD\times (T-1)CD}$$
对于离散时间系统，我们可以将系统状态序列写成如下形式： $$\begin{array}{rl}{\xi }_2& =A{\xi }_1+B{u}_1\\ {\xi }_3& =A(A{\xi }_1+B{u}_1)+B{u}_2\\ & ⋮\\ {\xi }_T& =A^T{\xi }_1+A^{T-1}B{u}_1+A^{T-2}B{u}_2+\cdots B{u}_T\end{array}$$ 如此，我们可以将 $1\sim T$ 时刻所有的状态及输入写称紧凑形式： $$\left[\begin{array}{c}{\xi }_1\\ {\xi }_2\\ {\xi }_3\\ ⋮\\ {\xi }_T\end{array}\right]=\left[\begin{array}{c}I\\ A\\ A^2\\ ⋮\\ A^T\end{array}\right]{\xi }_1+\left[\begin{array}{cccc}0& 0& \cdots & 0\\ B& 0& \cdots & 0\\ AB& B& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ A^{T-1}B& A^{T-2}B& \cdots & B\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ ⋮\\ u_{T-1}\end{array}\right]$$ 即 $$\xi =S^{\xi }{\xi }_1+S^{u}U$$ 可见 $S^{\xi }\in {\mathbb{R}}^{TCD\times CD},S^u\in {\mathbb{R}}^{TCD\times (T-1)D}$。此时，我们可以将上述公式代入到优化指标函数中： $$C=(\hat{\xi }-S^{\xi }{\xi }_1-S^{u}U{)}^{\top }\tilde{Q}(\hat{\xi }-S^{\xi }{\xi }_1-S^{u}U)+U^{\top }\tilde{R}U$$ 如此，问题转换成了一个无约束凸二次优化问题： $$\underset{U}{\mathrm{argmin}}C$$ 利用梯度下降法，对$U$求梯度并令其为零得： $$U^{\ast }=({S^u}^{\top }\tilde{Q}{S}^u+\tilde{R}{)}^{-1}{S^u}^{\top }\tilde{Q}(\hat{\xi }-S^{\xi }{\xi }_1)$$ 这其实等同于一个基于岭回归的最小二乘参数估计。

迭代方法（Iterative Form）

当然，我们亦可以采用迭代方法求解该最优跟踪问题。方便起见，我们定义LQ指标为：$$C=\frac{1}{2}({\hat{\xi }}_T-{\xi }_T{)}^{\top }{Q}_T({\hat{\xi }}_T-{\xi }_T)+\frac{1}{2}\sum _{t=1}^{T-1}\left(({\hat{\xi }}_t-{\xi }_t{)}^{\top }{Q}_t({\hat{\xi }}_t-{\xi }_t)+u_t^{\top }R{u}_t\right)$$ 各个变量定义与前文无异。加入系数单纯为了推导方便。此时，利用动态规划方法，我们可以得到离散线性系统的最优跟踪控制律为：$$u_t=-K_t^{\xi }{\xi }_t-K_t^{\beta }{\beta }_t$$ 此时最优的性能指标为：$$C^{\ast }=\frac{1}{2}{\xi }_0^{\top }{P}_0{\xi }_0+{\xi }_0^{\top }{\beta }_0+\frac{1}{2}{\alpha }_0$$
其中每一项迭代过程如下：

• 矩阵 $P$：$$\begin{array}{rl}{P}_T& =Q_T\\ P_t& =A^{\top }{P}_{t+1}A-A^{\top }{P}_{t+1}B(R_t+B^{\top }{P}_{t+1}B{)}^{-1}{B}^{\top }{P}_{t+1}A+Q_t\end{array}$$
• 矩阵 $K^{\xi }$： $$K_t^{\xi }=(R_t+B^{\top }{P}_{t+1}B{)}^{-1}{B}^{\top }{P}_{t+1}A$$
• 矩阵 $K^{\beta }$： $$K_t^{\beta }=(R_t+B^{\top }{P}_{t+1}B{)}^{-1}{B}^{\top }$$
• 向量 $\beta$： $$\begin{array}{rl}{\beta }_T& =-Q_T{\hat{\xi }}_T\\ {\beta }_t& =(A-B{K}_t^{\xi }{)}^{\top }{\beta }_{t+1}-Q_t{\hat{\xi }}_t\end{array}$$
• 向量 $\alpha$： $$\begin{array}{rl}{\alpha }_T& ={\hat{\xi }}_T^{\top }{Q}_T{\hat{\xi }}_T\\ {\alpha }_t& ={\alpha }_{t+1}+{\hat{\xi }}_t^{\top }{Q}_t{\hat{\xi }}_t-{\beta }_{t+1}^{\top }{K_t^{\beta }}^{\top }(R_t+B^{\top }{P}_{t+1}B)K_t^{\beta }{\beta }_{t+1}\end{array}$$

所有符号系统来源于附录引文1，推导从略。
有些资料中还将其写成：$$u_t=K_t({\hat{\xi }}_t-{\xi }_t)+f_t$$ 即反馈项+前馈项。其中参数定义如下：

• 矩阵 $K$： $$K_t=(B^{\top }{P}_{t}B+R_t{)}^{-1}{B}^{\top }{P}_{t}A$$ 其中 $P$ 的定义与前一种方法相同。
• 前馈向量 $f_t$： $$f_t=-(B^{\top }{P}_{t}B+R_t{)}^{-1}{B}^{\top }(P_t(A{\hat{\xi }}_t-{\hat{\xi }}_t)+d_t)$$
• 向量 $d_t$： $$d_t=(A^{\top }-A^{\top }{P}_{t+1}B(B^{\top }{P}_{t+1}B+R_t{)}^{-1}{B}^{\top })(P_{t+1}(A{\hat{\xi }}_t-{\hat{\xi }}_{t+1})+d_{t+1})$$ 其中 $d_T=0$。

LQT与多维高斯分布

假定要跟着的任务轨迹满足多维高斯分布，即 ${\xi }_t\sim \mathcal{N}({\mu }_t,{\Sigma }_t)$。那么对于上述LQT问题，我们可以取： $${\hat{\xi }}_t={\mu }_t,Q_t={\Sigma }_t^{-1}$$ 其它不变。
此时，对于批处理LQT方法，目标函数变为： $$C={\left(\mu -S^{\xi }{\xi }_1-S^{u}U\right)}^{\top }{\Sigma }^{-1}\left(\mu -S^{\xi }{\xi }_1-S^{u}U\right)+U^{\top }\tilde{R}U$$ 其中 $\mu \in {\mathbb{R}}^{TCD},\Sigma \in {\mathbb{R}}^{(TCD)\times (TCD)}$。此时控制信号为：$$U^{\ast }=({S^u}^{\top }{\Sigma }^{-1}{S}^u+\tilde{R}{)}^{-1}{S^u}^{\top }{\Sigma }^{-1}(\mu -S^{\xi }{\xi }_1)$$

对于高斯轨迹分布，LQT控制器还可以拓展为多坐标系下的任务参数化（Task-Parameterized）LQT控制器。假定系统中存在 $P$ 个坐标系，此时我们可以将目标函数拓展为： $$C=\sum _{j=1}^P{\left({\mu }^{(j)}-\xi \right)}^{\top }{{\Sigma }^{(j)}}^{-1}\left({\mu }^{(j)}-\xi \right)+U^{\top }\tilde{R}U$$ 其中${\mu }^{(j)},{\Sigma }^{(j)}$ 均是定义在第 $j$ 个坐标系下的均值与协方差。求解此问题，等同于首先求解：$$\mu =\underset{\xi }{\mathrm{argmin}}\sum _{j=1}^P{\left({\mu }^{(j)}-\xi \right)}^{\top }{{\Sigma }^{(j)}}^{-1}\left({\mu }^{(j)}-\xi \right)$$ 然后求解： $$U^{\ast }=\underset{U}{\mathrm{argmin}}(\mu -\xi {)}^{\top }{\Sigma }^{-1}(\mu -\xi )+U^{\top }\tilde{R}U$$ 即首先求最优轨迹分布，然后就最优控制信号。对于批处理方式，可求得最优控制信号为： $$U^{\ast }={\left({S^u}^{\top }{\Sigma }^{-1}{S}^u+\tilde{R}\right)}^{-1}{S^u}^{\top }{\Sigma }^{-1}\left(\mu -S^{\xi }{\xi }_1\right)$$ 其中： $${\Sigma }^{-1}=\sum _{j=1}^P{{\Sigma }^{(j)}}^{-1},\mu =\Sigma \sum _{j=1}^P{{\Sigma }^{(j)}}^{-1}{\mu }^{(j)}$$

---

• 感谢 钟宜生——《最优控制》清华大学出版社，2015年。
• Thanks Sylvein Calinon, A Tutorial on Task-Parameterized Movement Learning and Retrieval in International Service Robotis, 2016.
• Thanks Sylvein Calinon, et al, Learning Control. in Humanoid Robotics: a Reference, 2019.