飞控简析-从入门到跑路 第一章PX4的姿态控制(3)

接着上一篇的代码继续进行分析，上一篇的最后我们已经得到了过渡姿态R_rp的旋转矩阵（大地坐标系）。并得到了从实际姿态旋转到过渡姿态时，在机体坐标系的三轴角度差e_R。

	/* R_rp and R_sp has the same Z axis, calculate yaw error */
	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;

	if (e_R_z_cos < 0.0f) {
		/* for large thrust vector rotations use another rotation method:
		 * calculate angle and axis for R -> R_sp rotation directly */
		math::Quaternion q_error;
		q_error.from_dcm(R.transposed() * R_sp);
		math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag()  * 2.0f : -q_error.imag() * 2.0f;

		/* use fusion of Z axis based rotation and direct rotation */
		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
	}

然后，创建期望姿态的X轴向量R_sp_x和过渡姿态的X轴向量R_rp_x。然后求出过渡姿态与期望姿态的航向偏差e_R(2)。e_R(2)是直接由反三角函数求出的。当R_rp_x旋转到R_sp_x为顺时针时，得到一个与期望姿态R_sp的Z轴正方向一致的向量。再点乘R_sp_z，由于其与R_sp_z方向一致且R_sp_z是单位向量，所以可以得到该向量的模（正负看方向）。之所以要点乘R_sp_z，是因为R_rp_x叉乘R_sp_x等到的是一个向量，点乘R_sp_z就可以得到模长，且当该向量与R_sp_z方向相反时（此时夹角为负），得到的模长会有负号。而该向量的模就等于R_sp_x和R_rp_x的夹角的sin值（R_sp_x和R_rp_x为单位向量）。R_sp_x点乘R_rp_x就等于R_sp_x和R_rp_x的夹角的cos值。最后e_R(2)再乘上一个航向的比例yaw_w。此时，我们已经得到了俯仰，横滚，航向的角度差了。

当e_R_z_cos小于0时，期望姿态的Z轴与实际姿态的Z轴的夹角大于90°。此时更换另一种姿态差计算的方法。求两个矩阵旋转的四元数（机体坐标系），然后如果是正旋转（参考的正方向是旋转轴的正旋转），就返回四元数的虚部（即旋转轴的坐标）的两倍，然后计算一个直接旋转的系数direct_w 。e_R_z_cos=-0.01时（即期望姿态的Z轴与实际姿态的Z轴的夹角刚大于90°），direct_w会很小。当e_R_z_cos=-1时（cos180°），direct_w会等于yaw_w。

假设实际姿态与期望姿态只有横滚相差180°，实际姿态为地面坐标系左滚89°。此时，旋转角度为180°，半角为90°，按照标准四元数的方法来构造四元数，可知对应的单位四元数应为（0,1,0,0），虚部为（1,0,0），乘2是（2,0,0）。而它真正的角度差为（3.14,0,0），单位为弧度。可知，进过该段程序调整，可以降低控制用的角度差，变相的减少期望角速度。

	/* calculate angular rates setpoint */
	_rates_sp = _params.att_p.emult(e_R);

	/* limit rates */
	for (int i = 0; i < 3; i++) {
		if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
		    !_v_control_mode.flag_control_manual_enabled) {
			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));

		} else {
			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
		}
	}

得到角度差后，直接乘上姿态控制PID对应的参数P就得到了期望角速度。然后如果是自动控制（自主飞行，任务飞行的时候），使用自动飞行的最大角速度来限制期望角速度，如果是手动控制，使用自动控制的最大角速度来限制期望角速度。

	/* feed forward yaw setpoint rate */
	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;

	/* weather-vane mode, dampen yaw rate */
	if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
	    _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
		// prevent integrator winding up in weathervane mode
		_rates_int(2) = 0.0f;
	}
}

然后航向的期望角速度要加上前馈控制。前馈的作用在于放大输入的期望角速度。对于手动控制来说，遥控器航向输入的是角速度_v_att_sp.yaw_sp_move_rate，为了弥补航向角控制更慢的影响，加大期望航向角速度的输入，可以使航向控制更快。当遥控器输入的期望航向角速度为零时（即航向杆回中），前馈是不起作用的。可以简单的理解前馈起到放大输入的作用。

最后的处理是当飞机是垂直起降固定翼时（VTOL），在飞机切换到多旋翼状态时，风对飞机的影响会非常大。此时就会进入风向标模式，即此时不在对航向角速度进行积分，飞机理论上会停在旋转的扭力与风的扭力平衡的位置。如果不使用这样的策略，飞机的航向控制会非常不稳，机头会摆来摆去。过程如下：飞机被风吹偏航，角速度积分增加，角速度比例控制增加，飞机往回旋转，角速度积分减少，角速度比例控制减少，飞机又被风吹偏航。

至此，PX4的姿态控制（角度控制）就到此结束了。得到期望角速度的我们，直接对角速度使用PID控制即可。

最后，附上一张姿态控制的流程图：