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姿态旋转理论基础

这边还是放一下数学原型，摘自全权老师的《多旋翼飞行器设计与控制》：

如有错误请及时告知

Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析相关推荐

语言中拟合函数计算aic_Go语言函数深度解析(中)
上回函数深度解析给大家聊了一些函数的基本知识,不知道还有没有人记得,不记得赶紧回去复习! 他们是 go语言中函数的基本原理单/多个同/不同类型参数单/多个同/不同类型返回值值传递,引用传递函数 ...
c语言的point函数,C语言中friend友元函数详细解析
C语言中friend友元函数详细解析友元函数是可以直接访问类的私有成员的非成员函数.它是定义在类外的普通函数,它不属于任何类,但需要在类的定义中加以声明,声明时只需在友元的名称前加上关键字frien ...
php 立即执行函数,关于javascrip的立即执行函数的解析
这篇文章主要介绍了关于javascrip的t立即执行函数的解析,有着一定的参考价值,现在分享给大家,有需要的朋友可以参考一下概念: 立即执行函数顾名思义就是函数定义好之后立即执行. 函数表达式方式: ...
C++函数重载解析细节
Copyright(C)<C++从入门到精通-经典完整版> 函数重载解析细节函数重载解析过程有三个步骤这些步骤可以总结如下: 1 确定为该调用而考虑的候选函数以及函数调用中的实参表属性 ...
Python中sort和sorted函数代码解析
Python中sort和sorted函数代码解析本文研究的主要是Python中sort和sorted函数的相关内容,具体如下. 一.sort函数 sort函数是序列的内部函数函数原型: L.sor ...
Linux进程调用execve,linux内核系统调用函数do_execve()解析实例源码
linux内核系统调用函数do_execve()解析,彻底解析内核调用用户空间代码入口函数do_execve() sys_execve() –> do_execve() /usr/src/lin ...
Python文本变量与函数的解析执行，增强自动化测试数据驱动
关注我,每天分享软件测试技术干货.面试经验,想要领取测试资料.进入软件测试学习交流群的可以直接私信我哦~~ 我们在使用Python进行自动化测试或者测试脚本开发时,通常会在代码中融入数据驱动设计,以便 ...
main主函数参数解析
默认的main函数参数 int main(int argc, char *argv[]) {// 主函数的代码逻辑return 0; } 其中,int 是主函数的返回值类型,主函数执行完后会返回一个整 ...
【C语言】自定义函数例题解析
C语言函数例题解析 C语言函数解析 C语言函数例题解析值传递自定义函数求解一元二次方程自定义函数计算给定日期是该年的第几天地址传递自定义函数对N个数排序自定义函数对字符串的处理值传递自 ...

Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析

Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析

源码解析

这个函数做了什么

部分细节

euler_accel_limit()

input_shaping_angle()

姿态变化率与机体角速度之间的关系

Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析相关推荐

最新文章

热门文章