如何基于离散状态适配PIDController输入端口以切换控制对象？

在Drake框架中，Diagram构建完成后端口连接无法动态修改，所以直接“重连”LeafSystem的输入端口是不可行的。我们可以通过引入选择器/分发器系统，用离散状态信号动态切换数据流向，实现类似“自适应重连”的效果，具体方案如下：

核心思路 #

用两个中间系统实现动态路由：

• Selector：从多个输入中选择一个输出，用于切换PID的反馈输入来源（对应不同受控对象的状态）
• InverseSelector：将单个输入分发到多个输出中的一个，用于切换PID的控制输出目标（对应选中的受控对象）
• 配合一个周期性离散系统，生成随机索引信号，控制上述两个选择器的切换逻辑

步骤实现 #

1. 实现周期随机索引生成器 #

这个DiscreteLeafSystem每1秒输出一个0-9的随机整数（对应10个受控对象的索引）：

#include <drake/systems/framework/leaf_system.h>
#include <random>

class RandomIndexGenerator : public systems::DiscreteLeafSystem<double> {
 public:
  RandomIndexGenerator() {
    // 声明1秒的离散更新周期
    DeclarePeriodicDiscreteUpdate(systems::PeriodicDiscreteUpdatePeriod(1.0));
    // 声明输出端口：1维整数索引
    DeclareVectorOutputPort("index", systems::BasicVector<double>(1),
                            &RandomIndexGenerator::CalcIndex);
    
    // 初始化随机数生成器
    gen_ = std::mt19937(std::random_device{}());
    dist_ = std::uniform_int_distribution<>(0, 9); // 对应10个对象的0-based索引
  }

 private:
  void CalcIndex(const systems::Context<double>&,
                 systems::BasicVector<double>* output) const override {
    const int idx = dist_(gen_);
    output->SetAtIndex(0, static_cast<double>(idx));
  }

  mutable std::mt19937 gen_;
  mutable std::uniform_int_distribution<> dist_;
};

2. 搭建完整系统并连接路由 #

假设你已经有10个受控对象（存在std::vector<systems::System<double>*> plants中），以下是系统构建和连接的核心代码：

#include <drake/systems/framework/diagram_builder.h>
#include <drake/systems/primitives/selector.h>
#include <drake/systems/primitives/inverse_selector.h>
#include <drake/systems/primitives/pid_controller.h>

int main() {
  systems::DiagramBuilder<double> builder;
  const int kNumObjects = 10;

  // 添加PID控制器（根据你的需求调整Kp/Ki/Kd和输入维度）
  auto pid = builder.AddSystem<systems::PidController<double>>(
      systems::Vector<double>::Make({1.0, 0.1, 0.05}), // 增益参数
      systems::Vector<double>::Make({1.0}));           // 输入状态维度

  // 添加反馈输入选择器：1维输入，共10个输入端口
  auto feedback_selector = builder.AddSystem<systems::Selector<double>>(1, kNumObjects);
  // 添加控制输出分发器：1维输入，共10个输出端口
  auto control_demux = builder.AddSystem<systems::InverseSelector<double>>(1, kNumObjects);

  // 添加随机索引生成器
  auto index_gen = builder.AddSystem<RandomIndexGenerator>();

  // 1. 连接所有受控对象的状态输出到Selector的对应输入端口
  for (int i = 0; i < kNumObjects; ++i) {
    builder.Connect(
        plants[i]->get_output_port("state"),
        feedback_selector->get_input_port(i));
  }

  // 2. 连接索引信号到选择器/分发器的控制端口
  builder.Connect(
      index_gen->get_output_port("index"),
      feedback_selector->get_selection_input_port());
  builder.Connect(
      index_gen->get_output_port("index"),
      control_demux->get_selection_input_port());

  // 3. 连接PID的反馈输入与控制输出
  builder.Connect(feedback_selector->get_output_port(), pid->get_input_port_feedback());
  // 导出设定值输入端口（可外部传入目标值）
  builder.ExportInput(pid->get_input_port_desired_state(), "desired_state");
  builder.Connect(pid->get_output_port_control(), control_demux->get_input_port());

  // 4. 连接控制输出到对应受控对象
  for (int i = 0; i < kNumObjects; ++i) {
    builder.Connect(
        control_demux->get_output_port(i),
        plants[i]->get_input_port("control"));
  }

  // 构建并运行Diagram
  auto diagram = builder.Build();
  // 后续可添加仿真逻辑...
  return 0;
}

关键注意事项 #

• 索引匹配：确保随机生成的索引范围与Selector/InverseSelector的端口数量完全匹配（代码中用0-based索引对应10个对象，若你的对象是1-based，需在生成索引时做+1/-1转换）
• 维度一致：Selector的输入维度必须和受控对象的状态输出维度一致，若状态是多维（比如3位姿态），需将Selector构造的第一个参数改为对应维度
• 设定值切换：如果不同对象需要不同的设定值，可以再添加一个Selector来切换PID的desired_state输入端口

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