如何将推力器力项加入运动方程线性化以构建TVLQR控制器

直接结论 #

完全可以把J^T F_thruster项整合进你的线性化流程，不管是沿用之前的EvalTimeDerivatives+ExtractGradient，还是你考虑的手动AutoDiff+Calc系列函数方案，都能行。

方案一：扩展现有EvalTimeDerivatives流程 #

你之前用的plant.EvalTimeDerivatives是计算系统连续状态导数$\dot{x} = f(x, u)$，其中$x = [q, \dot{q}]^T$，$u = [\tau_1, \tau_2]^T$。要加推力器项，直接修改状态导数的计算逻辑就行：

• 如果$F_{thruster}$是轨迹优化输出的时变预定义值，把J^T F_thruster当成时变外力项，改写运动方程：
  $\dot{x} = \begin{bmatrix} \dot{q} \ M(q)^{-1}(B u + J^T F_{thruster}(t) - C(q, \dot{q})) \end{bmatrix}$
• 把这个修改后的状态导数传入EvalTimeDerivatives，再用ExtractGradient提取A、B矩阵。这里要注意：原关节力矩的贡献会留在B矩阵里，而J^T F_thruster因为依赖状态$q$（随时间t变化），会被线性化进时变A矩阵的对应部分。

方案二：手动AutoDiff+Calc系列函数（你的思路） #

这个方案完全可行，而且能更精准控制线性化细节，步骤如下：

1. 用AutoDiff变量包裹所有状态（浮基+关节的位姿、速度）和输入（关节力矩）。
2. 调用CalcMassMatrix计算$M(q)$，CalcBiasTerm计算$C(q, \dot{q})$，CalcJacobianSpatialVelocity计算末端空间雅可比$J(q)$（这个函数会自动处理浮基的贡献，不用额外调整）。
3. 计算逆质量矩阵$M(q)^{-1}$，代入运动方程得到$\ddot{q} = M(q)^{-1}(B u + J^T F_{thruster} - C(q, \dot{q}))$，再构造完整状态导数$\dot{x} = [\dot{q}, \ddot{q}]^T$。
4. 用AutoDiff对$x$和$u$求导，再用ExtractGradient提取雅可比矩阵——对$x$的导数就是A矩阵，对$u$的导数就是B矩阵。

必踩坑提醒 #

• 时变处理：每个时间步t，必须用当前轨迹上的$q(t), \dot{q}(t)$计算$J(q(t))$和$J^T F_{thruster}(t)$，不能直接用固定值。
• 雅可比类型：一定要用空间雅可比，CalcJacobianSpatialVelocity已经包含浮基的运动贡献，别误用只算关节的雅可比。
• 结果验证：用数值微分（给状态/输入加小扰动算导数）和AutoDiff结果对比，确保线性化没出错。

内容的提问来源于stack exchange，提问作者Shubham Vyas