基于Drake的多无人机协同吊运控制器拆分及仿真问题问询

一、控制器拆分与独立控制、状态监测实现 #

1. 封装独立LQR控制器单元 #

将单无人机LQR逻辑封装为Drake的LeafSystem，每个控制器实例对应一台无人机，实现控制逻辑解耦：

• 输入端口：接收对应无人机的6DOF状态（位置、速度、姿态、角速度，共12维）
• 输出端口：输出无人机的控制指令（推力+3维力矩，共4维），同时可添加状态监测输出端口
• 核心逻辑：在CalcControlCmd方法中完成LQR状态反馈计算，PassThroughState方法直接输出无人机状态用于监测

示例代码（C++）：

class DroneLQRController : public LeafSystem<double> {
 public:
  DroneLQRController(const Eigen::MatrixXd& K, const Eigen::VectorXd& x_des)
      : K_(K), x_des_(x_des) {
    DeclareInputPort("drone_state", kVectorValued, 12);
    DeclareVectorOutputPort("control_cmd", BasicVector<double>(4),
                            &DroneLQRController::CalcControlCmd);
    DeclareVectorOutputPort("monitored_state", BasicVector<double>(12),
                            &DroneLQRController::PassThroughState);
  }

 private:
  void CalcControlCmd(const Context<double>& context,
                      BasicVector<double>* output) const {
    const Eigen::VectorXd x = GetInputPort("drone_state").Eval(context);
    output->set_value(-K_ * (x - x_des_));
  }

  void PassThroughState(const Context<double>& context,
                        BasicVector<double>* output) const {
    output->set_value(GetInputPort("drone_state").Eval(context));
  }

  Eigen::MatrixXd K_;
  Eigen::VectorXd x_des_;
};

2. 多机独立控制部署 #

在仿真图（DiagramBuilder）中实例化多个控制器，通过Demultiplexer将全局状态拆分为单无人机状态，分别连接到对应控制器：

auto builder = std::make_unique<DiagramBuilder<double>>();

// 实例化三台无人机的控制器
auto ctrl1 = builder->AddSystem(std::make_unique<DroneLQRController>(K1, x_des1));
auto ctrl2 = builder->AddSystem(std::make_unique<DroneLQRController>(K2, x_des2));
auto ctrl3 = builder->AddSystem(std::make_unique<DroneLQRController>(K3, x_des3));

// 拆分全局状态为单无人机状态
auto demux = builder->AddSystem<Demultiplexer<double>>(36, 12); // 3台×12维状态
builder->Connect(plant.get_state_output_port(), demux->get_input_port());
builder->Connect(demux->get_output_port(0), ctrl1->get_input_port("drone_state"));
builder->Connect(demux->get_output_port(1), ctrl2->get_input_port("drone_state"));
builder->Connect(demux->get_output_port(2), ctrl3->get_input_port("drone_state"));

// 连接控制器输出到无人机执行器端口
builder->Connect(ctrl1->get_output_port("control_cmd"), plant.get_actuation_input_port(drone1_act_idx));
builder->Connect(ctrl2->get_output_port("control_cmd"), plant.get_actuation_input_port(drone2_act_idx));
builder->Connect(ctrl3->get_output_port("control_cmd"), plant.get_actuation_input_port(drone3_act_idx));

3. 状态与负载位置监测 #

• 无人机状态：直接从控制器的monitored_state输出端口获取，或通过Logger系统记录数据
• 负载位置：通过MultibodyPlant的get_body_pose_output_port获取负载位姿，连接到Logger或自定义监测系统：

// 记录负载位姿（7维：四元数+位置）
auto load_pose_logger = builder->AddSystem<Logger<double>>(7);
builder->Connect(plant.get_body_pose_output_port(load_body_idx), load_pose_logger->get_input_port());

二、绳索/负载Q值设0的错误与仿真缓慢解决 #

问题根源 #

Q值为0时，LQR控制器完全不对绳索和负载的状态施加惩罚，无人机控制逻辑会忽略负载与绳索的动态约束，导致绳索与无人机/负载发生穿透碰撞，触发Drake的不可穿透约束错误；同时无约束的动态行为会让仿真器频繁调整步长以处理冲突，导致运行缓慢。

解决方法 #

• 设置极小非零Q值：将绳索和负载的Q值设为1e-6级别的极小值，既不会过度干预无人机控制，又能让LQR对绳索/负载状态施加微弱约束，避免极端动态行为
• 排除不必要碰撞对：检查绳索连杆、无人机、负载之间的碰撞设置，用MultibodyPlant.ExcludeCollisionsBetween排除允许接触的部件（如绳索与连接的无人机/负载），减少碰撞检测的误触发：

// 排除绳索与无人机1的碰撞
plant.ExcludeCollisionsBetween(rope_body, drone1_body, true);
// 排除绳索与负载的碰撞
plant.ExcludeCollisionsBetween(rope_body, load_body, true);

• 调整仿真参数：若必须设Q为0，可调整MultibodyPlant的接触参数（增加接触刚度/阻尼），或关闭实时速率限制：

// 关闭实时速率限制，让仿真器全速运行
simulator.set_target_realtime_rate(0.0);

• 添加显式约束：在控制器中加入碰撞避免逻辑，比如限制无人机与负载的最小距离，或用逆运动学约束绳索的拉伸范围，从控制层面避免穿透。

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