PX4固定翼姿态控制器详细介绍(一)

代码版本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-固定翼的姿态控制

固定翼姿态控制流程