Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析
- 源码解析
- 这个函数做了什么
- 部分细节
- euler_accel_limit()
- input_shaping_angle()
- 姿态变化率与机体角速度之间的关系
本文将以input_euler_angle_roll_pitch_yaw()函数为例，讲解一下APM中的前馈及平滑控制函数，当然其余的类似input_euler_angle_roll_pitch_euler_rate_yaw()等等的函数也是类似的，当学习完这个函数之后，其他的上手应该也会很快的。
源码解析
函数名：input_euler_angle_roll_pitch_yaw()
函数位置：ardupilot/libraries/AC_AttitudeControl/AC_AttitudeControl
官方注释：Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing
注：以下内容会尽可能详细注释，力求让大家都看懂，一些细节部分会着重放在后面讲解。其中以n系简称NED坐标系，b系简称机体坐标系，tb表示期望姿态，cb表示当前姿态。
// 通过角速度前馈和平滑控制欧拉角，俯仰角和偏航角
// 前馈控制器控制量计算在此函数中完成
// 前三个输入参数：本次输入的期望姿态的roll、pitch、yaw角
void AC_AttitudeControl::input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw)
{
// 角度制转换为弧度制（乘以0.01是因为在输入这个函数之前乘以了100）
float euler_roll_angle = radians(euler_roll_angle_cd * 0.01f);
float euler_pitch_angle = radians(euler_pitch_angle_cd * 0.01f);
float euler_yaw_angle = radians(euler_yaw_angle_cd * 0.01f);
// _attitude_target_quat表示当前（未更新本次输入前）的期望姿态的四元数
// 可以理解为操作员通过遥控器摇杆输入的期望姿态是在实时变更的
// 这个总函数(input_euler_xxx)的输入是本次输入的期望，而_attitude_target_quat中保存的是还未收到本次输入进行更新前，当下的期望姿态
// 此处将其转换为欧拉角形式_attitude_target_euler_angle
_attitude_target_quat.to_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// 添加侧倾调整以补偿直升机尾部螺旋桨的推力（在多旋翼飞机上将返回零）
euler_roll_angle += get_roll_trim_rad();
if (_rate_bf_ff_enabled) {
// 如果开启了机体速率前馈，注意下方将期望角度转换为期望角速率的操作表示的是FF前馈控制器的作用
// FF前馈控制器根据期望姿态误差计算出期望姿态的期望角速率，其是作为前馈控制量叠加到P控制器输出的当前姿态的期望角速率上的
// 将侧倾、俯仰和偏航的加速度限制转换到欧拉轴上
// euler_accel_limit()接收当前期望姿态欧拉角和roll、pitch、yaw上最大加速度
// 以此计算出最小加速度限制，使增加加速度时不会超过任何一个轴的最大加速度
Vector3f euler_accel = euler_accel_limit(_attitude_target_euler_angle, Vector3f(get_accel_roll_max_radss(), get_accel_pitch_max_radss(), get_accel_yaw_max_radss()));
// 根据本次输入的期望姿态和未更新输入的当前期望姿态的姿态角度误差计算期望欧拉角速率校正值_attitude_target_euler_rate
// 以roll角为例
// wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x)：计算本次输入期望roll角和当前期望姿态roll角之间的误差，并将其限制在[-pi pi]之间
// input_tc：姿态控制输入时间常数，数字越小，响应越尖锐；数字越大，响应越柔和，默认设定为0.15
// euler_accel.x经限制后的欧拉角加速度
// _attitude_target_euler_rate.x：当前期望姿态欧拉角速率
// _dt：采样周期
_attitude_target_euler_rate.x = input_shaping_angle(wrap_PI(euler_roll_angle - _attitude_target_euler_angle.x), _input_tc, euler_accel.x, _attitude_target_euler_rate.x, _dt);
_attitude_target_euler_rate.y = input_shaping_angle(wrap_PI(euler_pitch_angle - _attitude_target_euler_angle.y), _input_tc, euler_accel.y, _attitude_target_euler_rate.y, _dt);
_attitude_target_euler_rate.z = input_shaping_angle(wrap_PI(euler_yaw_angle - _attitude_target_euler_angle.z), _input_tc, euler_accel.z, _attitude_target_euler_rate.z, _dt);
if (slew_yaw) { // 如果开启了摆率限制，那么还要对期望欧拉角速率进行限制
_attitude_target_euler_rate.z = constrain_float(_attitude_target_euler_rate.z, -get_slew_yaw_rads(), get_slew_yaw_rads());
}
// 将期望姿态的欧拉角速率_attitude_target_euler_rate转换为前馈的期望机体角速度矢量_attitude_target_ang_vel
// 姿态变化率（n系）转换为期望机体角速度（cb系）
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _attitude_target_ang_vel);
// 角速度_attitude_target_ang_vel限幅
ang_vel_limit(_attitude_target_ang_vel, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// 然后将限幅后的期望机体角速度重新转换为所需姿态的欧拉角速率
ang_vel_to_euler_rate(_attitude_target_euler_angle, _attitude_target_ang_vel, _attitude_target_euler_rate);
// 最后得到的期望角速率_attitude_target_ang_vel和期望欧拉角速率_attitude_target_euler_rate是前馈速率，即前馈控制量
// 注意以上运算过程是在本次输入的期望姿态与当前期望姿态之间实现的，最后得到的期望角速率是针对于期望机体坐标系的
// 因此该期望角速率在使用前需要先转换到当前姿态下
} else {
// 如果未启用前馈功能，则将目标欧拉角输入到目标中并将前馈速率归零
// 将本次输入的期望roll和yaw保存进当下的_attitude_target_euler_angle
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
if (slew_yaw) {
// 如果开启了摆率限制
// 计算出本次输入期望yaw角和当前期望yaw角之间的误差，并限幅
float angle_error = constrain_float(wrap_PI(euler_yaw_angle - _attitude_target_euler_angle.z), -get_slew_yaw_rads() * _dt, get_slew_yaw_rads() * _dt);
// 根据误差更新yaw角期望姿态
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
} else {
// 如果没有开启摆率限制
// 直接将本次输入保存为当前期望姿态yaw角
_attitude_target_euler_angle.z = euler_yaw_angle;
}
// 将期望姿态从欧拉角转换为四元数形式（n系下）
_attitude_target_quat.from_euler(_attitude_target_euler_angle.x, _attitude_target_euler_angle.y, _attitude_target_euler_angle.z);
// 将速率前馈设置为零，即没有前馈量
_attitude_target_euler_rate = Vector3f(0.0f, 0.0f, 0.0f);
_attitude_target_ang_vel = Vector3f(0.0f, 0.0f, 0.0f);
}
// 调用四元数姿态控制器（真正的P控制器在其中运行）
// 根据姿态角误差计算出期望角速率
attitude_controller_run_quat();
}
这个函数做了什么
关于什么是前馈，怎么叠加的，以及APM中的PID控制流程，详见我之前的博文：Ardupilot姿态控制器 PID控制流程
这里还是先放一张APM官方的老图：
本函数中主要做了以下几件事：
- 获取到本次输入的期望姿态euler_roll\pitch\yaw_angle（无论是来自遥控器还是地面站MAVLINK消息），以及未更新前（未处理本次输入前）的当下期望姿态_attitude_target_euler_angle（全局变量）；
- 根据是否开启前馈控制分别进行处理：
- 如果开启了前馈控制： 1. 根据本次输入的期望和当前期望计算出欧拉角误差，然后计算得出期望欧拉角速率_attitude_target_euler_rate； 2. 期望欧拉角速率_attitude_target_euler_rate转换为b系下的期望机体角速度_attitude_target_ang_vel并且进行角速度限幅； 3. 最后重新获取到前馈速率：_attitude_target_euler_rate和_attitude_target_ang_vel； 4. _attitude_target_quat的更新将在attitude_controller_run_quat()中进行；
- 如果没有开启前馈控制： 1. 直接将本次输入的期望姿态保存到全局变量 _attitude_target_euler_angle，并获取四元数形式的期望姿态 _attitude_target_quat； 2. 然后将前馈速率 _attitude_target_euler_rate和_attitude_target_ang_vel置零。
- 最后运行四元数姿态控制器，真正的P控制器将在其内部运行。
总结：input_euler_angle_roll_pitch_yaw()这个函数主要在其内部实现了前馈控制量计算及平滑的操作，最后调用了四元数姿态控制器（废话 =.=）。
注意：如果开启了前馈，计算出来的前馈速率 _attitude_target_ang_vel是基于期望姿态 的，因此在使用前，需要先转换到当前姿态坐标系下。
原因：因为input_euler_xxx()里面期望角速率是根据本次输入期望姿态和当下期望姿态计算得到（基于tb系），而attitude_controller_run_quat()中的P控制器则是通过当下期望姿态_attitude_target_quat与当前姿态的误差计算得到期望角速率（基于cb系）
部分细节
euler_accel_limit()
// translates body frame acceleration limits to the euler axis
Vector3f AC_AttitudeControl::euler_accel_limit(const Vector3f &euler_rad, const Vector3f &euler_accel)
{
float sin_phi = constrain_float(fabsf(sinf(euler_rad.x)), 0.1f, 1.0f);
float cos_phi = constrain_float(fabsf(cosf(euler_rad.x)), 0.1f, 1.0f);
float sin_theta = constrain_float(fabsf(sinf(euler_rad.y)), 0.1f, 1.0f);
Vector3f rot_accel;
if (is_zero(euler_accel.x) || is_zero(euler_accel.y) || is_zero(euler_accel.z) || is_negative(euler_accel.x) || is_negative(euler_accel.y) || is_negative(euler_accel.z)) {
rot_accel.x = euler_accel.x;
rot_accel.y = euler_accel.y;
rot_accel.z = euler_accel.z;
} else {
rot_accel.x = euler_accel.x;
rot_accel.y = MIN(euler_accel.y / cos_phi, euler_accel.z / sin_phi);
rot_accel.z = MIN(MIN(euler_accel.x / sin_theta, euler_accel.y / sin_phi), euler_accel.z / cos_phi);
}
return rot_accel;
}原谅我截止到写博文的时间还没有推出这个原型公式到底是什么（然而我还是厚脸皮地放上来了，有大佬懂麻烦留一下言）。然而我可以说一下这个函数的目的到底是啥。
参考资料：Copter: Is there any problem in function euler_accel_limit()?
So the problem here is we need to calculate the acceleration limits that will not let us increase past any one individual axis acceleration limit. I think you are calculating the maximum acceleration limits possible, not the minimum that we need. So for example, if we are banked over by 45 degrees in roll. If we ask apply full yaw then the acceleration will be shared by both the body pitch and body yaw. However, if we apply full yaw and push the pitch stick forward then that euler yaw acceleration is applied only to the body frame yaw and is increased in magnitude because the aircraft is banked by 45 degrees. So we need to limit the yaw acceleration based on this limit rather than the vector addition of both body pitch and body yaw.
翻译过来的大致意思就是：
这个函数计算加速度极限，该极限让我们增加加速度时不会超过欧拉角上任何一个轴的加速度极限。因此该函数求的是各轴上的最小加速度限制。
举例来说，当我们沿俯仰角倾斜了45°，也就是说机头向上仰起，如果要求完全偏航，此时加速度将会由俯仰和偏航共同承担，但是如果我们施加了完全偏航并且向前推动俯仰杆，欧拉偏航加速度仅应用于机体框架，使得在当前倾斜情况下欧拉角的旋转幅度大于机体的旋转幅度，因此我们需要这个函数来限制偏航加速度。
input_shaping_angle()
这个函数实际上内部调用了sqrt_controller（一个修改过的P控制器）来计算得到前馈控制量_attitude_target_euler_rate。其他的不再多做解释。
关于sqrt_controller的解释，看这篇：详解APM的开方控制器sqrt_controller
// 根据角度误差计算速度校正。 角速度具有加速和减速限制，包括使用_input_tc进行的基本加速度限制
float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt)
{
// sqrt_controller作为调整后的P控制器，根据接收到的角度误差计算出期望角速率
// Kp = 1/max(input_tc,0.01)
float desired_ang_vel = sqrt_controller(error_angle, 1.0f / MAX(input_tc, 0.01f), accel_max, dt);
// 直接限制加速度以平滑曲线的起点。
return input_shaping_ang_vel(target_ang_vel, desired_ang_vel, accel_max, dt);
}
...
// 限制请求速度的加减速
float AC_AttitudeControl::input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt)
{
// 直接限制加速度以平滑曲线的起点。
if (is_positive(accel_max)) {
float delta_ang_vel = accel_max * dt;
return constrain_float(desired_ang_vel, target_ang_vel - delta_ang_vel, target_ang_vel + delta_ang_vel);
} else {
return desired_ang_vel;
}
}姿态变化率与机体角速度之间的关系
主要是下面两个函数：
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{
float sin_theta = sinf(euler_rad.y);
float cos_theta = cosf(euler_rad.y);
float sin_phi = sinf(euler_rad.x);
float cos_phi = cosf(euler_rad.x);
ang_vel_rads.x = euler_rate_rads.x - sin_theta * euler_rate_rads.z;
ang_vel_rads.y = cos_phi * euler_rate_rads.y + sin_phi * cos_theta * euler_rate_rads.z;
ang_vel_rads.z = -sin_phi * euler_rate_rads.y + cos_theta * cos_phi * euler_rate_rads.z;
}// Convert an angular velocity vector to a 321-intrinsic euler angle derivative
// Returns false if the vehicle is pitched 90 degrees up or down
bool AC_AttitudeControl::ang_vel_to_euler_rate(const Vector3f& euler_rad, const Vector3f& ang_vel_rads, Vector3f& euler_rate_rads)
{
float sin_theta = sinf(euler_rad.y);
float cos_theta = cosf(euler_rad.y);
float sin_phi = sinf(euler_rad.x);
float cos_phi = cosf(euler_rad.x);
// When the vehicle pitches all the way up or all the way down, the euler angles become discontinuous. In this case, we just return false.
if (is_zero(cos_theta)) {
return false;
}
euler_rate_rads.x = ang_vel_rads.x + sin_phi * (sin_theta / cos_theta) * ang_vel_rads.y + cos_phi * (sin_theta / cos_theta) * ang_vel_rads.z;
euler_rate_rads.y = cos_phi * ang_vel_rads.y - sin_phi * ang_vel_rads.z;
euler_rate_rads.z = (sin_phi / cos_theta) * ang_vel_rads.y + (cos_phi / cos_theta) * ang_vel_rads.z;
return true;
}实际上一些基础理论我都汇总在了这篇博文里面：APM姿态旋转理论基础
这边还是放一下数学原型，摘自全权老师的《多旋翼飞行器设计与控制》：
