PX4垂起（Tiltrotor）偏航控制研究

PX4垂起（Tiltrotor）偏航控制研究

固件版本 V1.10.2

源码位置：
src/modules/mc_att_control/mc_att_control_main.cpp
src/modules/mc_pos_control/mc_pos_control_main.cpp
src/modules/fw_att_control/FixedWingAttitudeControl.cpp
src/modules/fw_pos_control/FixedWingPositionControl.cpp
src/modules/vtol_att_control/vtol_att_control_main.cpp
src/modules/vtol_att_control/tiltrotor.cpp
src/lib/FlightTask/task/Transition/FlightTackTransition,cpp
src/lib/ecl/attitude_fw/ecl_yaw_control.h

1. 问题描述

在PX4垂直起降固件中，偏航控制一直是一个真空点，在这里代码处理的比较简单，仅仅只是针对偏航角速率进行控制，就会出现下面这个问题：在过渡过程中，仅仅只有偏航角速率控制而没有偏航角控制，导致飞机会出现方向不稳定的情况。

下面就来讨论这个问题产生的原因。

2. 过渡过程中为什么没有偏航角度控制

我们先看看整个固件倾转旋翼的控制逻辑，如下两幅图所示：

由此可见，过渡阶段角速率的指令值会由旋翼切换到固定翼，这里我们仔细研究这个过程。

首先是最外环的位置控制，这部分是在’mc_pos_control_main.cpp’里面，首先判断是否有飞行任务更新，如果有的话就获得位置指令：

setpoint = _flight_tasks.getPositionSetpoint();
constraints = _flight_tasks.getConstraints();

找到对应task指令的update函数，这里使用的是lib/flighttask/task/transition里面的，找到对应函数如下所示

bool FlightTaskTransition::update()
{
	// level wings during the transition, altitude should be controlled
	_thrust_setpoint(0) = _thrust_setpoint(1) = 0.0f;
	_thrust_setpoint(2) = NAN;
	_position_setpoint *= NAN;
	_velocity_setpoint *= NAN;
	_position_setpoint(2) = _transition_altitude;

	updateAccelerationEstimate();

	_yaw_setpoint = _transition_yaw;
	return true;
}

从这里可以看出偏航角给的是过渡一开始的偏航角，相当于在位置控制要保持航向稳定，高度给的也是过渡开始时候的高度，相当于是定高控制。

在旋翼位置控制主函数最后有这个函数’publish_attitude’，这里相当于从旋翼位置控制发布姿态控制指令值。

问题1：为什么在过渡阶段固定翼位置控制没有起作用？

接下来这个指令值是直接进入固定翼姿态控制主函数里面的，这里又有一个问题：**为什么在过渡阶段固定翼位置控制没有起作用？**下面先讨论这个问题

为什么过渡阶段固定翼位置控制没有起作用？

从图一可以看出，固定翼和旋翼位置控制器计算得到指令值之后都给到垂直起降姿态控制器中，我们就应该查看这部分的代码。里面有一段冠以过渡模式的控制代码：

case mode::TRANSITION_TO_MC:
			// vehicle is doing a transition
			_vtol_vehicle_status.vtol_in_trans_mode = true;
			_vtol_vehicle_status.vtol_in_rw_mode = true; // making mc attitude controller work during transition
			_vtol_vehicle_status.in_transition_to_fw = (_vtol_type->get_mode() == mode::TRANSITION_TO_FW);

			_fw_virtual_att_sp_sub.update(&_fw_virtual_att_sp);

			if (mc_att_sp_updated || fw_att_sp_updated) {

				// reinitialize the setpoint while not armed to make sure no value from the last mode or flight is still kept
				if (!_v_control_mode.flag_armed) {
					Vector3f().copyTo(_mc_virtual_att_sp.thrust_body);
					Vector3f().copyTo(_v_att_sp.thrust_body);
				}

				_vtol_type->update_transition_state();
				_v_att_sp_pub.publish(_v_att_sp);
			}

			break;

如果只看到这里可能会认为这里旋翼和固定翼的att_sp检测了，是不是姿态的控制指令可以从固定翼位置控制得到，这时候专门进到函数**_vtol_type->update_transition_state();**里面最后一句有这么一句话很关键：

// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
    memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));

这里就一目了然了，不论前面怎么处理，这里都只发布旋翼得到的att_sp

问题2：关于virtual_attitude_setpoint的使用

还有一个关于virtual_attitude_setpoint的问题，这个在位置控制中，根据飞机类型决定发布真实的attitude_setpoint还是virtual_attitude_setpoint，这个问题可以以旋翼位置控制为例解释：

在’mc_pos_control_main’里面有发布姿态指令值的函数：

void
MulticopterPositionControl::publish_attitude()
{
	_att_sp.timestamp = hrt_absolute_time();

	if (_att_sp_pub != nullptr) {
		orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);

	} else if (_attitude_setpoint_id) {
		_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
	}
}

这里有一个参数’_attitude_setpoint_id’，查找这个使用可以看到下面这一段

		if (!_attitude_setpoint_id) {
			if (_vehicle_status.is_vtol) {
				_attitude_setpoint_id = ORB_ID(mc_virtual_attitude_setpoint);

			} else {
				_attitude_setpoint_id = ORB_ID(vehicle_attitude_setpoint);
			}
		}

很明显根据飞机类型给定

解决上面两个问题之后，接着我们查看固定翼的姿态控制主函数fw_att_control/FixedwingAttitudeControl.cpp里面，里面有飞行状态的判断：

		/* if we are in rotary wing mode, do nothing */
		if (_vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING && !_vehicle_status.is_vtol) {
			perf_end(_loop_perf);
			return;
		}

如果旋翼直接结束函数。

如果不是旋翼的话，就进入固定翼的姿态控制FixedWingAttitudeControl.cpp，这里我们关注旋翼的偏航控制，

_rates_sp.yaw = _yaw_ctrl.get_desired_bodyrate();

这里的’_yaw_ctrl’是ecl控制里面，因此打开ecl控制代码src/lib/ecl/ecl_yaw_controller.cpp查看对应代码

	if (!inverted) {
        /* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
        _rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / (ctl_data.airspeed <
                                                                                            ctl_data.airspeed_min ? ctl_data.airspeed_min : ctl_data.airspeed);
        if (ctl_data.transition_p1){
            /* Calculate the error */
            float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);

            /*  Apply P controller: rate setpoint from current error and time constant */
            _rate_setpoint =  yaw_error / _tc;
        }
    }

如果没有反着飞的话使用协调转弯，而姿态指令值roll_sp=0，对应的yaw_rate_sp = 0 ，因此终于找到只控偏航角速度而不控制偏航角度的原因了

过渡状态位置控制是旋翼位置控制，得到姿态指令值后进入固定翼姿态控制，这时候偏航角速度指令值是根据协调转弯计算得到的，而滚转角指令值又为0，因此偏航角速率不是根据偏航角误差得到，而是始终是0，就会有看起来不控偏航角的错觉。

问题3 ：为什么过渡状态姿态角给定值是固定的姿态控制给出的？

既然固定翼给出了姿态角速率控制值，和问题1类似，为什么旋翼姿态控制内环却使用固定翼姿态控制外环，这就要深入查看旋翼姿态控制函数。

进入mc_att_control_main里面的run函数，里面有一个关键的逻辑判断值’run_att_ctrl’

const bool is_hovering = _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
					 && !_vehicle_status.in_transition_mode;

		// vehicle is a tailsitter in transition mode
		const bool is_tailsitter_transition = _vehicle_status.in_transition_mode && _is_tailsitter;

		bool run_att_ctrl = _v_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);

对于tiltrotor而言，is_tailsistter_transition肯定是false，因此run_att_ctrl要为true只能是启动了姿态控制并且在巡航模式，而要是在巡航模式必须是飞机是倾转旋翼并且不再过渡模式，总之，倾转旋翼在过渡模式下 run_att_ctrl这个值是false，is_hovering也是false。直接跳到最下面的控制环节

else {
				/* attitude controller disabled, poll rates setpoint topic */
				if (_v_rates_sp_sub.update(&_v_rates_sp)) {
					_rates_sp(0) = _v_rates_sp.roll;
					_rates_sp(1) = _v_rates_sp.pitch;
					_rates_sp(2) = _v_rates_sp.yaw;
					_thrust_sp = -_v_rates_sp.thrust_body[2];
				}

姿态控制关闭，直接给出角速率控制值，这个值只能是固定翼发布的，因为旋翼的已经关闭了。

3. 如何修改固件使倾转旋翼在过渡状态也控制角度

这部分正在开发，完全验证好了再补充。