弹道解算

RoboMaster OSS 这位讲解的弹道解算非常的详尽，所以我也只简单讲解，具体的公式推导就看里面

   Eigen::Vector3d Barrel_Solve(Eigen::Vector3d position);             // 解算枪管移动角度
   /** 抬升角度计算 */
   float airResistanceSolve(Eigen::Vector3d Pos);
   float BulletModel(float x, float v, float angle);

枪管解算

/**
 *  函数名: Barrel_Solve
 *  传入: Eigen::Vector3d position
 *  传出: Vector3d rpy
 *  功能: 通过传入的坐标计算枪管移动角度
 */
Eigen::Vector3d AngleSolve::Barrel_Solve(Eigen::Vector3d position) {
    Eigen::Vector3d rpy;
    rpy[2] = -atan2(position[0],position[1]) / CV_PI*180.0;
    rpy[1] = atan2(position[2],position[1]) / CV_PI*180.0;
    rpy[0] = atan2(position[2],position[0]) / CV_PI*180.0;

    // 单独计算抬升角度 | atan2()计算角度在车旋转180度之后会解算出[90-180]
    rpy[1] = airResistanceSolve(position);
    // 添加枪管补偿
    rpy[0] += Barrel_offset_angle[0];
    rpy[1] += Barrel_offset_angle[1];
    rpy[2] += Barrel_offset_angle[2];

    return rpy;
}

这里需要注意的就是atan2函数，这个函数的取值范围是 -180~180

对于yaw数据来说，我们直接使用就行，但是呢，pitch数据不能直接使用，因为子弹会下坠

所以我们需要对Pitch数据单独计算，引入空气阻力模型，根据距离解算下坠距离，从而得到我们实际应该抬升的角度

airResistanceSolve函数

空气阻力计算模型

float AngleSolve::airResistanceSolve(Eigen::Vector3d Pos) {

    // -----------要水平距离的融合，否则计算的距离会少，在视野边缘处误差会大----------
    float x = (float)sqrt(Pos[0]*Pos[0]+ Pos[1]*Pos[1]);
    float y = (float)Pos[2];

    float y_temp, y_actual, dy;
    float Solve_pitch;
    y_temp = y;
    bullet_speed = 25;
    // 迭代法 | 使用弹道模型对落点高度进行计算,直到落点高度达到要求,得到抬升角度
    for (int i = 0; i < 20; i++)
    {
        Solve_pitch = (float)atan2(y_temp, x);                        // 计算当前枪管抬升角度
        y_actual = BulletModel(x, bullet_speed, Solve_pitch);           // 得到落点高度
        dy = y - y_actual;                                               // 计算落点高度和目标高度的误差
        y_temp = y_temp + dy;                                            // 误差补偿

        //! 误差精度达到一定要求
        if (fabsf(dy) < 0.00001) {
            break;
        }
    }
    Solve_pitch = Solve_pitch*180.f/CV_PI;  // 解算角度
    return Solve_pitch;
}

这里的迭代法名为 “牛顿迭代法” 有兴趣可以去了解

我们通过计算落点的高度，不断的调整落点高度，实现误差基本为0，如图

所以我们就需要不断的迭代高度，使得实际命中点和目标点重合

/**
 *  函数名: BulletModel
 *  传入: float x, float v, float angle       (水平距离,弹速,抬升角度)
 *  传出: float y                             (落点高度)
 *  功能: 通过传入的坐标计算枪管移动角度
 *  弹道模型推导:
 *  https://robomaster-oss.github.io/rmoss_tutorials/#/rmoss_core/rmoss_projectile_motion/projectile_motion_iteration
 */
float AngleSolve::BulletModel(float x, float v, float angle) { //x:m,v:m/s,angle:rad
    float t,y;
    t = (float)((exp(SMALL_AIR_K * x) - 1) / (SMALL_AIR_K * v * cos(angle)));
    y = (float)(v * sin(angle) * t - GRAVITY * t * t/* * cos(ab_pitch)*/ / 2);
    return y;
}

下面就是实际的一个推导