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表示当前姿态。
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)
{
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.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) {
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.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());
}
euler_rate_to_ang_vel(_attitude_target_euler_angle, _attitude_target_euler_rate, _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);
} else {
_attitude_target_euler_angle.x = euler_roll_angle;
_attitude_target_euler_angle.y = euler_pitch_angle;
if (slew_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);
_attitude_target_euler_angle.z = wrap_PI(angle_error + _attitude_target_euler_angle.z);
} else {
_attitude_target_euler_angle.z = euler_yaw_angle;
}
_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);
}
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()
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
float AC_AttitudeControl::input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt)
{
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;
}
}
姿态变化率与机体角速度之间的关系
主要是下面两个函数:
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;
}
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);
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姿态旋转理论基础
这边还是放一下数学原型,摘自全权老师的《多旋翼飞行器设计与控制》:
如有错误请及时告知
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)