ardupilot 遥控器输入量如何转换成目标加速度

摘要

主要根据遥控器的横滚，俯仰通道值转换成对应的欧拉角度，然后根据欧拉角度转换成地理坐标系下的目标加速度的过程。

1.理论依据

在这里插入图片描述

2程序细节分析

根据公式（8）我们可以得到前右下坐标系下对应的加速度：
在这里插入图片描述
到这里我们得到了前右下坐标系下的加速度，此时我们只需要转换到地理坐标系即可。

在这里插入图片描述

3.代码实现

很早之前的代码实现过程

void AC_Loiter::set_sky_droid_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd, float dt)
{

    //在公用接口上转换从cm度到弧度---- Convert from centidegrees on public interface to radians
	//单位缩小了100,之前扩大了100倍,并且这里单位变成弧度
    const float euler_roll_angle = radians(euler_roll_angle_cd*0.01f);
    const float euler_pitch_angle = radians(euler_pitch_angle_cd*0.01f);

    //转换我们期望的姿态到加速度矢量，当我们是悬停时候---- convert our desired attitude to an acceleration vector assuming we are hovering
    //因此这里的假设是悬停操作，下面得到机体坐标系下的目标加速度信息
    const float pilot_cos_pitch_target = constrain_float(cosf(euler_pitch_angle), 0.5f, 1.0f); //限制俯仰角度在-60到60度
    //得到右侧的加速度信息
    const float pilot_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(euler_roll_angle)/pilot_cos_pitch_target;
    //得到前向加速度信息
    const float pilot_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(euler_pitch_angle);

    //旋转加速度矢量到地理坐标系下（经纬度坐标系） rotate acceleration vectors input to lat/lon frame
    _desired_accel.x = (pilot_accel_fwd_cms*_ahrs.cos_yaw() - pilot_accel_rgt_cms*_ahrs.sin_yaw());
    _desired_accel.y = (pilot_accel_fwd_cms*_ahrs.sin_yaw() + pilot_accel_rgt_cms*_ahrs.cos_yaw());

    //计算误差关于我们认为我们应该到和我们想要到的 difference between where we think we should be and where we want to be
    //得到姿态误差值
    Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y));

    // calculate the angular velocity that we would expect given our desired and predicted attitude
    //计算给定期望和预测姿态时的角速度
    _attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt);

    //根据预测角速度更新预测姿态----- update our predicted attitude based on our predicted angular velocity
    _predicted_euler_angle += _predicted_euler_rate * dt;

    // convert our predicted attitude to an acceleration vector assuming we are hovering
    //假设我们在悬停，将预测的姿态转换为加速度矢量
    const float pilot_predicted_cos_pitch_target = cosf(_predicted_euler_angle.y);
    //获取右向的加速度
    const float pilot_predicted_accel_rgt_cms = GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.x)/pilot_predicted_cos_pitch_target;
    //获取向前的加速度
    const float pilot_predicted_accel_fwd_cms = -GRAVITY_MSS*100.0f * tanf(_predicted_euler_angle.y);

    //旋转加速度矢量到地理坐标系---- rotate acceleration vectors input to lat/lon frame
    _predicted_accel.x = (pilot_predicted_accel_fwd_cms*_ahrs.cos_yaw() - pilot_predicted_accel_rgt_cms*_ahrs.sin_yaw());
    _predicted_accel.y = (pilot_predicted_accel_fwd_cms*_ahrs.sin_yaw() + pilot_predicted_accel_rgt_cms*_ahrs.cos_yaw());
}

//最新代码

void AC_Loiter::set_pilot_desired_acceleration(float euler_roll_angle_cd, float euler_pitch_angle_cd)
{
    const float dt = _attitude_control.get_dt();
    // Convert from centidegrees on public interface to radians
    const float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
    const float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);

    // convert our desired attitude to an acceleration vector assuming we are not accelerating vertically
    const Vector3f desired_euler {euler_roll_angle, euler_pitch_angle, _ahrs.yaw};
    const Vector3f desired_accel = _pos_control.lean_angles_to_accel(desired_euler);

    _desired_accel.x = desired_accel.x;
    _desired_accel.y = desired_accel.y;

    // difference between where we think we should be and where we want to be
    Vector2f angle_error(wrap_PI(euler_roll_angle - _predicted_euler_angle.x), wrap_PI(euler_pitch_angle - _predicted_euler_angle.y));

    // calculate the angular velocity that we would expect given our desired and predicted attitude
    _attitude_control.input_shaping_rate_predictor(angle_error, _predicted_euler_rate, dt);

    // update our predicted attitude based on our predicted angular velocity
    _predicted_euler_angle += _predicted_euler_rate * dt;

    // convert our predicted attitude to an acceleration vector assuming we are not accelerating vertically
    const Vector3f predicted_euler {_predicted_euler_angle.x, _predicted_euler_angle.y, _ahrs.yaw};
    const Vector3f predicted_accel = _pos_control.lean_angles_to_accel(predicted_euler);

    _predicted_accel.x = predicted_accel.x;
    _predicted_accel.y = predicted_accel.y;
}

Vector3f AC_PosControl::lean_angles_to_accel(const Vector3f& att_target_euler) const
{
    // rotate our roll, pitch angles into lat/lon frame
    const float sin_roll = sinf(att_target_euler.x);
    const float cos_roll = cosf(att_target_euler.x);
    const float sin_pitch = sinf(att_target_euler.y);
    const float cos_pitch = cosf(att_target_euler.y);
    const float sin_yaw = sinf(att_target_euler.z);
    const float cos_yaw = cosf(att_target_euler.z);

    return Vector3f{
        (GRAVITY_MSS * 100.0f) * (-cos_yaw * sin_pitch * cos_roll - sin_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
        (GRAVITY_MSS * 100.0f) * (-sin_yaw * sin_pitch * cos_roll + cos_yaw * sin_roll) / MAX(cos_roll * cos_pitch, 0.1f),
        (GRAVITY_MSS * 100.0f)
    };
}

可以看出最新的代码对加速度的限制范围相对较小。其实之前代码也有这个倾斜角到加速度的转换只是没有使用

// get_lean_angles_to_accel - convert roll, pitch lean angles to lat/lon frame accelerations in cm/s/s
void AC_PosControl::lean_angles_to_accel(float& accel_x_cmss, float& accel_y_cmss) const
{
    // rotate our roll, pitch angles into lat/lon frame
    // todo: this should probably be based on the desired attitude not the current attitude
    accel_x_cmss = (GRAVITY_MSS * 100) * (-_ahrs.cos_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() - _ahrs.sin_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll() * _ahrs.cos_pitch(), 0.5f);
    accel_y_cmss = (GRAVITY_MSS * 100) * (-_ahrs.sin_yaw() * _ahrs.sin_pitch() * _ahrs.cos_roll() + _ahrs.cos_yaw() * _ahrs.sin_roll()) / MAX(_ahrs.cos_roll() * _ahrs.cos_pitch(), 0.5f);
}