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

本文主要是介绍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 radiansconst 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 verticallyconst 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 beVector2f 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 verticallyconst 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 frameconst 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 attitudeaccel_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);
}