在Drake中定义含四元数的四旋翼连续时间状态空间的正确方法

问题描述 #

我在C++环境下基于Drake框架搭建四旋翼动力学仿真，参考官方四旋翼示例（使用欧拉角表示姿态），希望改用四元数描述姿态。目前面临的核心问题：

• 是否可以将四元数与位置等常规状态一同纳入系统状态量？
• 如何让积分器保证四元数的归一化特性？

尝试过的方案及问题 #

1. 使用DeclarePerStepUnrestrictedUpdateEvent在每个时间步归一化四元数，但仿真输出的四元数始终未严格归一化（虽偏离单位范数程度不高）。
2. 使用DeclarePerStepPublishEvent结合以下归一化函数，该方法因归一化在积分步骤后执行有一定效果，但代码中使用了const_cast，不符合C++编码规范：

auto QuadcopterPlant::normalize_quaternion(const drake::systems::Context<double>& ctx) const
    -> drake::systems::EventStatus
{
    Quadcopter::State::Vector state_vec = ctx.get_continuous_state_vector().CopyToVector();
    state_vec.segment<4>(3).normalize();
    const_cast<drake::systems::Context<double>&>(ctx).get_mutable_continuous_state_vector().SetFromVector(state_vec);
    return drake::systems::EventStatus::Succeeded();
}

我的系统主函数结构如下（QuadcopterPlant继承自LeafSystem）：

int main()
{
    auto builder = DiagramBuilder<double>{};
    // Create the simple system.
    auto quadcopter = std::make_unique<quadsim::quadcopter::Quadcopter>(quadsim::quadcopter::Quadcopter::Parameters{});

    auto system = builder.AddSystem<quadsim::quadcopter::QuadcopterPlant>(std::move(quadcopter));

    auto logger = LogVectorOutput(system->get_output_port(0), &builder);

    auto diagram = builder.Build();

    // Create the simulator.
    Simulator<double> simulator(*diagram);
    // Set the initial conditions x₀.
    Eigen::VectorXd init_state = Eigen::VectorXd::Zero(quadsim::quadcopter::Quadcopter::State::DIM);
    init_state(3) = 1.0;
    simulator.get_mutable_context().get_mutable_continuous_state().SetFromVector(init_state);

    // Simulate for 10 seconds.
    simulator.AdvanceTo(10.0);

    const auto sim_log = logger->FindLog(simulator.get_context());
    log_to_file(sim_log);

    return 0;
}

目前我未使用MultibodyPlant，觉得它过于冗余，但不确定是否必须使用该模块。

最终可行解决方案 #

1. 配置仿真器积分器 #

更换积分器为RadauIntegrator并设置参数：

simulator.reset_integrator<drake::systems::RadauIntegrator<double>>();
simulator.get_mutable_integrator().set_maximum_step_size(0.1);
simulator.get_mutable_integrator().set_target_accuracy(1e-3);

2. 修改运动学方程 #

在四元数的导数计算中加入修正项，驱动四元数保持归一化状态：

Eigen::Quaterniond qw_w{};
    qw_w.w() = 0.0;
    qw_w.vec() = curr_state.lin_vel_w;
    const auto rho = 10.0;
    Eigen::Vector4d q_dot =
        0.5 * (qw_w * curr_state.quat_wb).coeffs() +
        0.5 * rho * curr_state.quat_wb.coeffs() * (1.0 / curr_state.quat_wb.coeffs().squaredNorm() - 1.0);

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