一、核心思想：转化为误差调节问题
解决轨迹追踪问题的关键，是将其重新定义为一个针对追踪误差的调节问题。
- 定义追踪误差 我们定义误差向量 为系统实际状态与目标状态之差：
- 新的控制目标 原问题"追踪目标轨迹"就等价于新问题"将误差向量调节至零"。这本质上是一个LQR调节器问题。
二、求解方法：状态向量增广
为了在一个统一的框架内处理系统状态 和目标状态 ，我们将它们组合成一个增广状态向量 。
1. 增广状态空间方程
- 定义增广状态向量
- 推导增广系统动态 结合原系统方程 和目标动态方程 ，我们可以写出增广状态的动态方程：
这可以被写成一个标准的线性状态空间形式：
其中，增广矩阵 和 分别为：
2. 增广代价函数
轨迹追踪的代价函数通常是基于误差 和控制量 来定义的：
为了使用增广状态 求解，我们需要将此代价函数用 来表示。
- 建立误差与增广状态的联系 误差向量可以表示为增广状态向量的线性变换：
- 改写代价函数 将 代入代价函数中：
我们定义新的增广权重矩阵 和 ：
这样，代价函数就完全转换成了关于增广状态 的二次型：
三、结论
通过状态增广，最初的轨迹追踪问题被成功转化为了一个针对增广系统 和增广代价函数 的标准LQR调节器问题。
我们可以直接套用标准的LQR求解算法（如逆向求解Riccati方程）来解决这个增广问题，得到的最优控制律将是增广状态的线性反馈：
这个控制器会同时利用当前状态 和目标状态 的信息来计算最优的控制输入，从而实现对目标轨迹的追踪。
四、代码与实验结果
LQR_tracking_const_1.mlx
实验结果
注意到，会有稳态误差存在
这是因为对于弹簧阻尼系统，我们希望系统停在某个非零的位置，就必须在稳态下给系统施加一个恒力
这就意味着只要不为零，这个代价函数可以翻译为自然语言：
我希望误差为零；我也希望输入为零
不能又要马儿跑，又不给马儿吃草。这是做不到的。
