带水弹性接触模型的摆臂机器人轨迹稳定：Finite Horizon LQR应用咨询

错误原因 #

你遇到的以下错误：

SystemExit: Failure at systems/controllers/finite_horizon_linear_quadratic_regulator.cc:62 in RiccatiSystem(): condition 'context_->has_only_continuous_state()' failed.

本质是Drake的FiniteHorizonLinearQuadraticRegulator仅支持纯连续状态系统，而你的系统混合了连续输入和离散状态组（8个广义位置/速度的离散状态），不符合该控制器的前置条件。

如何让Finite Horizon LQR适配你的系统 #

有两种可行路径：

• 重构为纯连续状态模型：检查水弹性接触模型是否真的需要离散状态。如果离散状态是人为采样或建模简化导致的，可尝试将其重构为连续状态的近似模型（比如用连续弹性接触动力学替代离散状态更新），满足has_only_continuous_state()要求后，即可正常使用FiniteHorizonLinearQuadraticRegulator。
• 切换为离散时间版Finite Horizon LQR：如果离散状态是水弹性接触模型的固有部分（比如接触事件的离散触发逻辑），可先将系统连续部分离散化，再使用离散时间的有限时域LQR控制器：
  1. 用Drake的Discretize()函数将连续输入与动力学部分转化为离散时间模型；
  2. 基于离散化后的系统构建控制器，即可兼容离散状态组。

替代方案 #

若上述方法不可行，推荐以下适配混合状态系统的控制器：

• 迭代LQR（iLQR）：Drake的IterativeLinearQuadraticRegulator对混合系统兼容性远优于传统TVLQR，无需纯连续状态。它能基于当前状态实时迭代优化反馈增益，抗干扰性强，调参仅需调整状态权重矩阵Q和输入权重矩阵R，比PID简便。
• 模型预测控制（MPC）：使用Drake的ModelPredictiveController，基于参考轨迹和系统模型实时滚动优化控制输入。MPC天然支持约束处理与混合状态系统，抗干扰性极佳，计算量虽略高于TVLQR，但完全适配机器人仿真场景。
• 增益调度PID：若希望保留PID框架，可基于参考轨迹的不同阶段动态调整PID参数（比如根据关节位置/速度区间预设增益），减少手动调参工作量，但抗干扰性仍弱于LQR类方法。

内容的提问来源于stack exchange，提问作者Grama Shourie