使用Boost ODEint求解飞行器状态微分方程遇运行时错误求助

我采用时间推进法求解飞行器状态量导数Xdot，基于Boost ODEint库实现，但始终出现运行时错误，尝试多种方法仍无法解决，恳请提供技术帮助。

相关代码 #

void Flight::aircraftDynamics(const state_type& x, state_type& dx, double t, const Aircraft& aircraft) {
        VectorXd X = Eigen::Map<VectorXd>(const_cast<double*>(x.data()), x.size());
        VectorXd Xdt = Xdot(aircraft, X);
        for (size_t i = 0; i < Xdt.size(); i++) {
            dx[i] = Xdt[i]; // Direct assignment to dx
        }
}

void Flight::TimeMarcher(const Aircraft& aircraft) {
    state_type x(18);  // Initialize and set all elements to 1.0
    double t = 0.0;
    double dt = 0.1;
    runge_kutta_dopri5<state_type> stepper;

    for (size_t i = 0; i <= 100; ++i) {
        stepper.do_step([this, &aircraft](const state_type& x, state_type& dx, double t) {
            aircraftDynamics(x, dx, t, aircraft);
            }, x, t, dt);
        t += dt;

        std::cout << "Time: " << t << " State: U=" << x[0] << ", Phi=" << x[6]
            << ", Theta=" << x[7] << ", Psi=" << x[8] << std::endl;
    }
}

// calculate state
Eigen::VectorXd Flight::Xdot(const Aircraft& aircraft, const Eigen::VectorXd& X) {

    std::cout << "X entering XDot" << std::endl;
    std::cout << X << std::endl;

    const double& cmac = aircraft.airGeom.cref;
    const double& b = aircraft.airGeom.bref;
    const double& Sw = aircraft.airGeom.S;
    const double& grav = condition.grav;
    //const double& grav = 32.17;
    // CG loc 
    const auto& CG = aircraft.currentConfig.CGloc;

    // assigning values 
    const double& Ixx = aircraft.currentConfig.Inertia[0];
    const double& Ixy = aircraft.currentConfig.Inertia[1];
    const double& Iyy = aircraft.currentConfig.Inertia[2];
    const double& Ixz = aircraft.currentConfig.Inertia[3];
    const double& Izz = aircraft.currentConfig.Inertia[4];
    const double& Iyz = aircraft.currentConfig.Inertia[5];

    const double& DD = aircraft.currentConfig.DD;
    const double& m = aircraft.currentConfig.mass;

    Eigen::Matrix2d Imat;  // Define a 2x2 matrix
    Imat << Izz, Ixz,
        Ixz, Ixx;

    auto U = X[0], beta = X[1], alfa = X[2], P = X[3];
    auto Q = X[4], R = X[5], phi = X[6], theta = X[7], psi = X[8];
    auto xpos = X[9], ypos = X[10], zpos = X[11];
    auto T = X[12]; std::vector<double> xc(X.begin() + 13, X.end());

    // calculate velocities
    double W = tan(alfa) * U;
    double V = tan(beta) * U;
    double Vinfi = sqrt(U * U + V * V + W * W);
    //double qinfi = 0.5 * condition.Rho * Vinfi * Vinfi;
    double qinfi = 0.5 * 0.002377 * Vinfi * Vinfi;

    auto c2v = cmac / (2 * Vinfi);
    auto b2v = b / (2 * Vinfi);

    const auto& Clalpha = aircraft.currentAerodriv.find("ANGLEA")->second[0].derivatives[2];
    const auto& CMalpha = aircraft.currentAerodriv.find("ANGLEA")->second[0].derivatives[4];

    const auto& Cl0 = aircraft.currentAerodriv.find("Ref")->second[0].derivatives[2];
    const auto& CM0 = aircraft.currentAerodriv.find("Ref")->second[0].derivatives[4];

    const auto& CD0 = aircraft.currentConfig.CD0; // Talk to EB 
    auto CDalpha = 0.4; // Either talk to EB or CFD 

    const auto& ClQ = aircraft.currentAerodriv.find("Q")->second[0].derivatives[2];
    const auto& CMQ = aircraft.currentAerodriv.find("Q")->second[0].derivatives[4];
    const auto& CDQ = aircraft.currentAerodriv.find("Q")->second[0].derivatives[0];

    const auto& ClP = aircraft.currentAerodriv.find("P")->second[0].derivatives[3];
    const auto& CyP = -aircraft.currentAerodriv.find("P")->second[0].derivatives[1];
    const auto& CnP = -aircraft.currentAerodriv.find("P")->second[0].derivatives[5];

    const auto& CyR = -aircraft.currentAerodriv.find("R")->second[0].derivatives[1];
    const auto& ClR = aircraft.currentAerodriv.find("R")->second[0].derivatives[3];
    const auto& CnR = aircraft.currentAerodriv.find("R")->second[0].derivatives[5];

    const auto& CyB = aircraft.currentAerodriv.find("SIDES")->second[0].derivatives[1];
    const auto& ClB = -aircraft.currentAerodriv.find("SIDES")->second[0].derivatives[3];
    const auto& CnB = -aircraft.currentAerodriv.find("SIDES")->second[0].derivatives[5];


    std::vector<double> Flap1 = aircraft.currentCSS.find("Flap1")->second[0].derivatives;
    std::vector<double> Flap2 = aircraft.currentCSS.find("Flap2")->second[0].derivatives;
    std::vector<double> Aileron = aircraft.currentCSS.find("Aileron")->second[0].derivatives;
    std::vector<double> Elev = aircraft.currentCSS.find("Elevator")->second[0].derivatives;
    std::vector<double> Rudder = aircraft.currentCSS.find("Rudder")->second[0].derivatives;


    auto CD = CD0 + CDalpha * alfa + CDQ * c2v * Q +
        (Flap1[0] * xc[0] + Flap2[0] * xc[1] + Aileron[0] * xc[2] + Elev[0] * xc[3] + Rudder[0] * xc[4]);
    auto CY = CyB * beta + CyP * P * b2v + CyR * R * b2v +
        (Flap1[1] * xc[0] + Flap2[1] * xc[1] + Aileron[1] * xc[2] + Elev[1] * xc[3] + Rudder[1] * xc[4]);
    auto CL = Cl0 + Clalpha * alfa + ClQ * c2v * Q +
        (Flap1[2] * xc[0] + Flap2[2] * xc[1] + Aileron[2] * xc[2] + Elev[2] * xc[3] + Rudder[2] * xc[4]);
    auto Clroll = ClB * beta + ClP * P * b2v + ClR * R * b2v +
        (Flap1[3] * xc[0] + Flap2[3] * xc[1] + Aileron[3] * xc[2] + Elev[3] * xc[3] + Rudder[3] * xc[4]);
    auto CM = CM0 + CMalpha * alfa + CMQ * c2v * Q +
        (Flap1[4] * xc[0] + Flap2[4] * xc[1] + Aileron[4] * xc[2] + Elev[4] * xc[3] + Rudder[4] * xc[4]);
    auto CN = CnB * beta + CnP * P * b2v + CnR * R * b2v +
        (Flap1[5] * xc[0] + Flap2[5] * xc[1] + Aileron[5] * xc[2] + Elev[5] * xc[3] + Rudder[5] * xc[4]);

    auto Drag = qinfi * Sw * CD;
    auto Side = qinfi * Sw * CY;
    auto Lift = qinfi * Sw * CL;
    auto RollM = qinfi * Sw * b * Clroll;
    auto PitchM = qinfi * Sw * cmac * CM;
    auto YawN = qinfi * Sw * b * CN;

    auto Fx = -Drag * cos(alfa) * cos(beta) - Side * cos(alfa) * sin(beta) + Lift * sin(alfa) + T;
    auto Fy = -Drag * sin(beta) + Side * cos(beta);
    auto Fz = -Drag * sin(alfa) * cos(beta) - Side * sin(alfa) * sin(beta) - Lift * cos(alfa);


    auto Udot = -Q * W + V * R - grav * sin(theta) * condition.Nz + Fx / m;
    auto Vdot = -R * U + P * W + grav * cos(theta) * sin(phi) * condition.Nz + Fy / m;
    auto Wdot = -P * V + Q * U + grav * cos(theta) * cos(phi) * condition.Nz + Fz / m;


    auto PdotX = Ixz * P * Q + (Iyy - Izz) * R * Q + RollM;
    auto Qdot = ((Izz - Ixx) * P * R + Ixz * (R * R - P * P) + PitchM) / Iyy;
    auto RdotX = -Ixz * Q * R + (Ixx - Iyy) * P * Q + YawN;


    Eigen::Vector2d velocityChanges;
    velocityChanges << PdotX, RdotX;

    // Perform the matrix operation
    Eigen::Vector2d Mat = DD * Imat * velocityChanges;

    // Extract Pdot and Rdot from the result
    double Pdot = Mat(0);
    double Rdot = Mat(1);


    auto  phidot = P + Q * sin(phi) * tan(theta) + R * cos(phi) * tan(theta);
    auto thetadot = Q * cos(phi) - R * sin(phi);
    auto psidot = (Q * sin(phi) + R * cos(phi)) * 1 / cos(theta);


    auto xposdot = U * cos(theta) * cos(psi) + V * (sin(phi) * sin(theta)
        * cos(psi) - cos(phi) * sin(psi)) + W * (cos(phi) * sin(theta) * cos(psi) +
            sin(phi) * sin(psi));
    auto yposdot = U * cos(theta) * sin(psi) + V * (sin(phi) * sin(theta) * sin(psi) +
        cos(phi) * cos(psi)) + W * (cos(phi) * sin(theta) * sin(psi) -
            sin(phi) * cos(psi));
    auto zposdot = U * sin(theta) - V * sin(phi) * cos(theta) - W * cos(phi) * cos(theta);
    auto betadot = atan(Vdot / U);
    auto alfadot = atan(Wdot / U);


    Eigen::VectorXd vec(18);
    vec << Udot, betadot, alfadot, Pdot, Qdot, Rdot, phidot, thetadot, psidot, xposdot, yposdot, zposdot, 0, 0, 0, 0, 0, 0;
    // Initialize the vector

    return vec;
}

问题排查与修复建议 #

• 修复const_cast的不安全转换：aircraftDynamics中对x.data()的强制转换会破坏const语义，改为const映射：

  const VectorXd X = Eigen::Map<const VectorXd>(x.data(), x.size());
  

• 确保状态维度严格匹配：确认Xdot返回的向量始终是18维，和state_type x(18)一致，避免赋值时越界。
• 处理分母为零的情况：
  • psidot中的cos(theta)，当theta接近±π/2时添加保护：

    const double cosTheta = cos(theta);
    auto psidot = fabs(cosTheta) < 1e-6 ? 0.0 : (Q * sin(phi) + R * cos(phi)) / cosTheta;
    

  • betadot和alfadot中检查U的绝对值，避免除零：

    auto betadot = fabs(U) < 1e-6 ? 0.0 : atan(Vdot / U);
    auto alfadot = fabs(U) < 1e-6 ? 0.0 : atan(Wdot / U);
    

• 避免容器访问空指针/越界：
  • 每次调用std::map::find后，必须检查迭代器是否等于end()，比如：

    auto angleAIter = aircraft.currentAerodriv.find("ANGLEA");
    if (angleAIter == aircraft.currentAerodriv.end()) {
        throw std::runtime_error("ANGLEA not found in currentAerodriv");
    }
    const auto& Clalpha = angleAIter->second[0].derivatives[2];
    

  • 确认xc的长度至少为5，避免xc[0]到xc[4]的越界访问，可在初始化时添加断言：

    assert(xc.size() >= 5 && "xc vector has insufficient elements");
    

• 验证矩阵运算合法性：确认DD是标量，Imat * velocityChanges的运算结果是2x1向量，避免维度不匹配错误。
• 检查外部变量有效性：确保condition对象在Xdot函数调用期间始终有效，未被提前销毁。

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