如何用LQR控制MultibodyPlant中的部分状态（多旋翼带负载）

核心思路 #

通过限制LQR代价矩阵的作用范围，让控制器仅对前12个状态生成反馈增益，其余状态仅作为观测值采集，不参与控制计算。

具体修改步骤及代码示例 #

1. 调整LQR代价矩阵（最简方案）

直接修改代价矩阵Q，仅给前12个状态设置非零权重，其余状态权重置0，求解得到的增益矩阵K会自动忽略未控制状态：

from pydrake.systems.controllers import LinearQuadraticRegulator

# 初始化plant和context（你的原有代码）
plant = ...
context = plant.CreateDefaultContext()

num_controlled = 12
num_total_states = plant.num_states()

# 构造Q矩阵：仅前12个状态有控制代价，其余为0
Q = np.zeros((num_total_states, num_total_states))
Q[:num_controlled, :num_controlled] = np.eye(num_controlled) * 10  # 可根据需求调整权重

# R矩阵保持四旋翼输入的代价权重（假设对应4个电机）
R = np.eye(plant.num_actuators()) * 0.1

# 生成LQR控制器，此时K矩阵中第13列及以后全为0
lqr_controller = LinearQuadraticRegulator(plant, context, Q, R)

2. 自定义控制器（更灵活的状态拆分）

如果需要手动拆分状态、单独处理观测与控制逻辑，可以自定义LeafSystem：

from pydrake.systems.framework import LeafSystem, BasicVector
from pydrake.systems.controllers import Linearize, LinearQuadraticRegulatorSolver

# 1. 先提取四旋翼可控状态对应的线性化子系统
linear_system = Linearize(plant, context)
num_controlled = 12
A_sub = linear_system.A()[:num_controlled, :num_controlled]
B_sub = linear_system.B()[:, :num_controlled]

# 2. 求解子系统的LQR增益
Q_sub = np.eye(num_controlled) * 10
R_sub = np.eye(plant.num_actuators()) * 0.1
solver = LinearQuadraticRegulatorSolver(A_sub, B_sub, Q_sub, R_sub)
K_sub = solver.get_K()

# 3. 构造完整增益矩阵，未控制状态对应列置0
K_full = np.zeros((plant.num_actuators(), num_total_states))
K_full[:, :num_controlled] = K_sub

# 4. 自定义控制器类
class PartialLQRController(LeafSystem):
    def __init__(self, plant, K, num_controlled):
        super().__init__()
        self._K = K
        self._num_controlled = num_controlled
        # 输入：全状态
        self.DeclareVectorInputPort("full_state", BasicVector(plant.num_states()))
        # 输出：控制输入
        self.DeclareVectorOutputPort("control_cmd", BasicVector(plant.num_actuators()),
                                     self.CalcControl)
        
    def CalcControl(self, context, output):
        x_full = self.get_input_port(0).Eval(context)
        # 仅用前12个状态计算反馈
        x_controlled = x_full[:self._num_controlled]
        x_desired = np.zeros(self._num_controlled)  # 设置你的期望状态
        error = x_controlled - x_desired
        u = -self._K[:, :self._num_controlled] @ error
        output.SetFromVector(u)

# 实例化控制器
controller = PartialLQRController(plant, K_full, num_controlled)

3. 未控制状态的采集

在系统中添加一个输出端口，直接输出未控制状态即可：

# 假设在你的系统构建代码中
plant.DeclareVectorOutputPort("observed_states", BasicVector(num_total_states - num_controlled),
                              lambda context, output: output.SetFromVector(
                                  plant.GetPositionsAndVelocities(context)[num_controlled:]
                              ))

注意事项 #

• 确认前12个状态对应四旋翼的可控自由度（通常是3位置+3姿态+3线速度+3角速度），负载状态属于未控制部分。
• 若Linearize得到的矩阵包含负载状态，通过Q矩阵置0的方式会让LQR自动忽略这些状态的反馈计算。

内容的提问来源于stack exchange，提问作者Howard Li