带有单个PID的双摆

如何解决带有单个PID的双摆

我试图了解如何使用Drake通过PID控制器来控制简单的双摆。

按照此link上的示例，我创建了一个类似的程序，但我只想尝试在下部关节上使用1个PID，然后看看系统做了什么。 （我知道以这种方式无法达到稳定状态）

from django.db.models import Max

CustomUser.objects.filter(
    languages__language_id=1
).annotate(
    highest_level=Max('languages__level')
).order_by('highest_level')

这大概是我得到的输出，因为一个关节没有被主动控制

  MultibodyPlant<double>* dp = builder.AddSystem<MultibodyPlant<double>>(max_time_step);
  dp->set_name("plant");
  dp->RegisterAsSourceForSceneGraph(&scene_graph);

  Parser parser(dp);

  parser.AddModelFromFile(kDoublePendulumSdfPath);

  // Weld the base link to world frame with no rotation.
  const auto& root_link = dp->GetBodyByName("base");
  dp->AddJoint<WeldJoint>("weld_base",dp->world_body(),std::nullopt,root_link,RigidTransform<double>::Identity());
  dp->AddJointActuator("a2",dp->GetJointByName("joint2"));

  // Now the plant is complete.
  dp->Finalize();

  // Create PID Controller.
  const Eigen::VectorXd Kp = Eigen::VectorXd::Ones(1) * Kp_;
  const Eigen::VectorXd Ki = Eigen::VectorXd::Ones(1) * Ki_;
  const Eigen::VectorXd Kd = Eigen::VectorXd::Ones(1) * Kd_;
  const auto* const pid = builder.AddSystem<PidController<double>>(Kp,Ki,Kd);
  builder.Connect(dp->get_state_output_port(),pid->get_input_port_estimated_state());
  builder.Connect(pid->get_output_port_control(),dp->get_actuation_input_port());
  //...

雄鸭会支撑不受主动控制的关节，我该如何设置呢？

解决方法

代替构造具有标量Kp，Kd，Ki增益的PID控制器，应使用大小为2的矢量作为增益。由于您只想控制下部关节，因此增益矩阵中只有与下部关节相对应的条目为非零。

我认为您可以将代码更改为

const Eigen::VectorXd Kp = Eigen::Vector2d(0,1) * Kp_;
const Eigen::VectorXd Ki = Eigen::Vector2d(0,1) * Ki_;
const Eigen::VectorXd Kd = Eigen::Vector2d(0,1) * Kd_;
const auto* const pid = builder.AddSystem<PidController<double>>(Kp,Ki,Kd);

我假设您的工厂状态的顺序为（upper_joint_position，lower_joint_position，upper_joint_velocity，lower_joint_velocity）。因此，增益为[0，1]。如果在您的工厂状态下lower_joint_position位于upper_joint_position之前，则增益应为[1，0]