代码版本1.8.2
源码地址 Firmware1.8.2
最近需要做一下固定翼的相关姿态控制,只对控制流程进行简单介绍,特此记录下相关流程,方便自己后续进一步调试,以下对PX4固定翼框架中的姿态控制进行简单介绍,仅供参考,欢迎大家在留言区讨论交流
主姿态控制器在Firmware\src\modules\fw_att_control
滚转通道的控制器文件在Firmware\src\lib\ecl\attitude_fw中的ecl_roll_controller.cpp中
俯仰通道的控制器文件在Firmware\src\lib\ecl\attitude_fw中的ecl_pitch_controller.cpp中
偏航通道的控制器文件在Firmware\src\lib\ecl\attitude_fw中的ecl_yaw_controller.cpp中
关于从主文件中的调用其它几个通道的控制器参考如下代码
/* Run attitude controllers */
if (_vcontrol_mode.flag_control_attitude_enabled) {
if (PX4_ISFINITE(roll_sp) && PX4_ISFINITE(pitch_sp)) {
//_roll_ctrl的定义在FixedwingAttitudeControl.hpp中,详细介绍参考“三、得到期望角速率”
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_wheel_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();//得到期望滚转角速率
control_input.pitch_rate_setpoint = _pitch_ctrl.get_desired_rate();//得到期望俯仰角速率
control_input.yaw_rate_setpoint = _yaw_ctrl.get_desired_rate();//得到期望偏航角速率
/* Run attitude RATE controllers which need the desired attitudes from above, add trim */
//此处已经得到输出的舵偏角了,见“四、得到控制量的详细介绍”
float roll_u = _roll_ctrl.control_euler_rate(control_input);
_actuators.control[actuator_controls_s::INDEX_ROLL] = (PX4_ISFINITE(roll_u)) ? roll_u + trim_roll : trim_roll
以滚转通道为例,主程序调用滚转通道,则进入到ecl_roll_controller.cpp中
本小节按照以下方式进行介绍
1.函数传入期望角度与实际角度,用期望角度减去实际角度得到角度差
2.角度差乘以p得到期望角速率
3.p项的大小定义
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(ISFINITE(ctl_data.roll_setpoint) && ISFINITE(ctl_data.roll))) {
return _rate_setpoint;
}
/* Calculate error */
float roll_error = ctl_data.roll_setpoint - ctl_data.roll;//1.期望角度减去实际角度得到角度差
/* Apply P controller */
_rate_setpoint = roll_error / _tc;//2.经过p控制得到期望角速率,_tc的定义见下方介绍
return _rate_setpoint;
}
3._tc的定义在ecl_controller.cpp中
4._tc的定义是在ecl_controller.h中定义的,一层一层搜索一下就会发现是按照如下方式进行传递,以下这些赋值都在FixedwingAttitudeControl.cpp中进行的
(1)先找到参数
_parameter_handles.p_tc = param_find(“FW_P_TC”);
(2)之后赋值
param_get(_parameter_handles.p_tc, &(_parameters.p_tc));
(3)经过函数set_time_constant赋值给_tc
_tc = time_constant;
(4)用角度差除以_tc得到期望角速率
因此在QGC“参数”中修改FW_P_TC的值就可以改变外环p的大小
至此找到了生成期望角速率的位置,即通过角度差除以_tc得到期望角速率。值得一提的是只有俯仰与滚转通道会生成期望角速率
关于这部分函数在各个通道的控制器中,具体路径参考“二、代码文件地址”
float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
//转换到机体坐标系
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
//期望角速率限幅
set_bodyrate_setpoint(_bodyrate_setpoint);
//控制器设计PI+FF
return control_bodyrate(ctl_data);
}
控制量的输出遵循了两部分
4.1 第一部分是将期望角速度转换到机体坐标系中
4.2 第二部分控制器是采用PI+FF控制器
4.1 将期望角速度转换到机体坐标系中,公式的参考参考《小型无人机理论及应用》或者《飞行控制系统》都可以
公式部分:
以俯仰通道的控制器进行介绍,飞控绕着x轴是roll,绕着y是pitch,绕着z是yaw。从公式中看wyb = cos(roll)*pitch_rate + cos(pitch)*sin(roll)*yaw_rate,对应实际代码如下
4.2 控制器设计
(1)期望角速率减去实际角速率得到角速度差
(2)设计控制器
摘取出来其中的控制器部分
_last_output = _bodyrate_setpoint * _k_ff * ctl_data.scaler +
_rate_error * _k_p * ctl_data.scaler * ctl_data.scaler
+ _integrator; //scaler is proportional to 1/airspeed
_k_ff和_k_p的定义晚点会做进一步参数部分的介绍。ctl_data.scaler的定义在FixedwingAttitudeControl.cpp中,不知道乘以这个配平空速有什么影响,为什么不直接用p项呢,这块目前还没有理解。
以上就是目前的姿态控制器的简单介绍,从主文件中不断调用各自通道的控制器,需要修改哪里就找对应的文件即可。
【参考资料】
PX4-固定翼的姿态控制
固定翼姿态控制流程