六、PX4(v1.14)多旋翼姿态控制代码(mc_att_control)分析

一、mc_att_control姿态控制代码功能介绍

        在PX4旋翼机的位置控制中会计算出期望的姿态，该姿态是以四元数的方式给出的。四元数表示的不仅仅是pitch和roll,同时包含了yaw轴的期望。

        由于pitch和roll的控制是通过旋翼机的倾斜来实现的，yaw轴的控制是通过不同电机差速运动产生的反扭矩来实现的，因此yaw轴的控制比pitch和roll的控制更难、控制的反馈更慢、消耗电机的转速更多。

        鉴于以上原因，对pitch和roll与yaw的分离可以实现更好的控制。且yaw的控制需要更多的电机转速，对yaw单独进行限幅，可以将电机的资源更多的分配给pitch和roll，利于快速相应。

        姿态控制，通过比例控制P，产生期望的角速率。

二、mc_att_control功能结构图

三、代码分析

姿态控制部分最重要的就是update函数，显示了期望姿态的倾转分离、姿态控制及期望角速率的限幅。

matrix::Vector3f AttitudeControl::update(const Quatf &q) const
{
	//将姿态期望值赋值为qd(q为当前姿态的四元数)
	Quatf qd = _attitude_setpoint_q;

	// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch
	//计算当前姿态四元数对应的机体坐标系Z轴单位向量，在机载NED系下表示
	const Vector3f e_z = q.dcm_z();
	//计算期望姿态四元数对应的机体坐标系Z轴单位向量，在机载NED系下表示
	const Vector3f e_z_d = qd.dcm_z();
	Quatf qd_red(e_z, e_z_d);//计算去除旋转误差后仅代表倾斜误差的四元数，计算公式如下：

	//无旋转运动时，直接赋值为期望四元数
	if (fabsf(qd_red(1)) > (1.f - 1e-5f) || fabsf(qd_red(2)) > (1.f - 1e-5f)) {
		// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,
		// full attitude control anyways generates no yaw input and directly takes the combination of
		// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.
		qd_red = qd;

	} else {
		// transform rotation from current to desired thrust vector into a world frame reduced desired attitude
		//将旋转轴从N系转换到B系，结合当前姿态，我们可以得到仅代表倾斜运动的期望四元数
		qd_red *= q;
	}

	// mix full and reduced desired attitude
	//根据期望四元数和倾斜期望四元数可以得到代表旋转运动的期望四元数
	Quatf q_mix = qd_red.inversed() * qd;
	q_mix.canonicalize();
	// catch numerical problems with the domain of acosf and asinf
	//根据旋转运动的性质，q_mix可以表示为（cos(α_mix/2),0,0,sin(α_mix/2))：
	q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
	q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);
	//限制旋转误差，最后结合倾斜运动和受限制旋转运动组成新的期望四元数：
	qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

	// quaternion attitude control law, qe is rotation from q to qd
	//根据新四元数姿态期望计算四元数误差：
	const Quatf qe = q.inversed() * qd;

	// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
	// also taking care of the antipodal unit quaternion ambiguity
	const Vector3f eq = 2.f * qe.canonical().imag();

	// calculate angular rates setpoint
	//根据姿态误差计算期望角速度
	Vector3f rate_setpoint = eq.emult(_proportional_gain);

	// Feed forward the yaw setpoint rate.
	// yawspeed_setpoint is the feed forward commanded rotation around the world z-axis,
	// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).
	// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)
	// and multiply it by the yaw setpoint rate (yawspeed_setpoint).
	// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame
	// such that it can be added to the rates setpoint.
	if (std::isfinite(_yawspeed_setpoint)) {
		rate_setpoint += q.inversed().dcm_z() * _yawspeed_setpoint;
	}

	// limit rates
	//角速度期望输出限幅
	for (int i = 0; i < 3; i++) {
		rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));
	}

	return rate_setpoint;
}

本文参考了：https://zhuanlan.zhihu.com/p/81847591