Pixhawk学习8——多旋翼姿态控制

姿态控制学习部分主要以多旋翼姿态控制为例。
1.整体流程

姿态控制主要分为几个部分：
姿态误差角计算=>目标角速度计算=>角速度PID
而这部分的重点在姿态误差角计算。

2.姿态误差角计算

姿态角误差的计算目的是要知道无人机从该状态转动多少角度才能到目标状态。从字面意思理解，姿态误差角不就是当前姿态和目标姿态的差值吗（如下公式）？

并且很多飞控就是这么做的（匿名，烈火等等这些玩具级飞控。）

那么这么做有什么问题吗？

首先可以参考上一节无人机的姿态是怎么来的？是通过姿态旋转矩阵的反三角函数来的，而姿态矩阵是通过旋转来的，旋转的前提是坐标系定义。例如欧美坐标系定义和苏俄坐标系定义是不同的，其XYZ轴对应的姿态不同，其旋转顺序也不同，相同的三个姿态角，计算出来的姿态矩阵也不同。

好像扯得有点远了。其实我想说的是：姿态是基于旋转定义的，因此姿态误差也应该是基于旋转定义的。虽然一些飞控中的姿态误差定义满足上式，那是基于小角度的假设得来的，不是真正意义上的姿态误差。所以，Pixhawk中的姿态误差计算方法是基于姿态误差角的旋转矩阵计算的。

言归正转。
Pixhawk中姿态误差角计算涉及到三个坐标系。机体坐标系b、地理坐标系n和航向游移坐标系r。首先，根据当前姿态和期望姿态，由两个状态的四元数给出两个状态的姿态旋转矩阵：

	/* construct attitude setpoint rotation matrix */
	math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);
	math::Matrix<3, 3> R_sp = q_sp.to_dcm();

	/* get current rotation matrix from control state quaternions */
	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
	math::Matrix<3, 3> R = q_att.to_dcm();

其中，R_sp为目标姿态矩阵，是由速度位置回路给出的；R为当前姿态矩阵。Pixhawk中采用先对其Z轴（即计算水平两个速度），再补偿航向角的方法。因此，首先取出代表Z轴矢量的两个向量：

	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));

然后叉乘求得这两个向量要共轴所需要旋转的向量（导到机体系）：

	/* axis and sin(angle) of desired rotation */
	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);

再计算需要绕该向量旋转的角度和单位化该向量，再将该角度分解到两个水平角度上：

/* calculate angle error */
	float e_R_z_sin = e_R.length();
	float e_R_z_cos = R_z * R_sp_z;
	

	float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;

		e_R = e_R_z_axis * e_R_z_angle;

上述e_R中的第一项和第二项就是两个水平姿态角误差。

接下来计算航向姿态误差角。定义游移坐标系r（航向没有对齐）的姿态矩阵为R_rp。该姿态矩阵是由R矩阵，通过上述e_R向量旋转得到的。目前已知该向量的矢量和绕该向量转动的角度，可根据罗德里戈旋转公式得到R_rp。具体推到详见：
根据旋转前后的向量值求旋转矩阵

因此，可以由R_rp和R_sp求得航向角误差：

math::Matrix<3, 3> e_R_cp;
		e_R_cp.zero();
		e_R_cp(0, 1) = -e_R_z_axis(2);
		e_R_cp(0, 2) = e_R_z_axis(1);
		e_R_cp(1, 0) = e_R_z_axis(2);
		e_R_cp(1, 2) = -e_R_z_axis(0);
		e_R_cp(2, 0) = -e_R_z_axis(1);
		e_R_cp(2, 1) = e_R_z_axis(0);

		/* rotation matrix for roll/pitch only rotation */
		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));

其中yaw_w为一个衰减系数，其目的是先进行横滚、俯仰角的旋转。

/* calculate weight for yaw control */
	float yaw_w = R_sp(2, 2) * R_sp(2, 2);

至此三个姿态误差角就已经得到了。

Pixhawk代码中还有一段利用四元数计算姿态角误差的算法，解释为当两个Z轴为90度时，由于存在cos奇点的问题，所以直接采用四元数求解。（有个疑问，为什么不能直接采用四元数求解呢？？？）

/* 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;

3.目标角速度计算

姿态控制中角度控制回路到角速度控制回路的计算比较简单，直接将姿态角误差进行比例计算，就得到了目标角速度：

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

4.角速度回路控制

角速度回路控制比较中规中矩，就是PID控制。

	_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
		       _params.rate_ff.emult(_rates_sp);

特别的，对于PID积分项，该处有几种抗积分饱和的操作。
飞机需要离地才开始积分；
 计算得到的电机指令值在限幅内；
 角速度换计算的值小于总体推力设置的值。

	/* update integral only if not saturated on low limit and if motor commands are not saturated */
	if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit) {
		for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
			if (fabsf(_att_control(i)) < _thrust_sp) {
				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;

				if (PX4_ISFINITE(rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
				    _att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT &&
				    /* if the axis is the yaw axis, do not update the integral if the limit is hit */
				    !((i == AXIS_INDEX_YAW) && _motor_limits.yaw)) {
					_rates_int(i) = rate_i;
				}
			}
		}
	}