PX4姿态控制流程图
图片来源
Px4的姿态控制分为角度环(外环)和角速度环(内环)，角度环使用P控制，角速度环使用PID控制，由于偏航通道响应较慢(多旋翼飞行器的俯仰和滚转运动由旋翼的升力力矩产生，偏航运动由旋翼的反扭矩来产生，而升力力矩要比反扭矩大得多(可从旋翼的升力系数和反扭矩系数中看出)，这造成了偏航运动能力相比滚转和俯仰运动能力要弱)，故在偏航通道内环加入前馈环节；
控制方法
PX4的姿态控制部分使用的是roll-pitch和yaw分开控制的（是为了解耦控制行为），即tilt和torsion两个环节。如下图：
根据经验所得，控制toll-pitch比控制yaw更容易实现。比如同样是实现10°的变化，roll-pitch需要60ms左右；但是yaw控制器却需要接近150ms。**使用分步控制的优点在于解耦控制行为，即分别执行响应较快的动作和响应较慢的动作；同时，相比传统的RPY三轴解耦姿态控制，三个姿态通道的姿态动作范围更小，能耗更小。**下图则比较形象地表示了这一点：
控制算法分析
math::Matrix<3, 3> R_sp;
...
/* rotation matrix for current state */
math::Matrix<3, 3> R;
R.set(_v_att.R);此处，R_sp表示期望姿态角所对应的期望机体系到地理坐标系的旋转矩阵；R：根据当前的姿态角得到当前机体系到地理坐标系的旋转矩阵；
/* 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));得到当前机体系下的z轴在地理系下的坐标表示（旋转矩阵R左乘[0,0,1]）
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));得到期望机体系下的z轴在地理系下的坐标表示（旋转矩阵R_sp左乘[0,0,1]）
/* axis and sin(angle) of desired rotation */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);R_z和 R_sp_z叉乘求出由R_z到R_sp_z的旋转轴，由右手定则判断该旋转轴的方向；注意：此处叉乘得到的旋转轴为地理系下的坐标表示。R为当前机体系到地理系的旋转矩阵，其转置为地理系到当前坐标系的旋转矩阵，因为旋转矩阵为单位正交矩阵，逆等于其转置；最终这一步得到旋转轴在当前机体系下的表示。
/* calculate angle error */
float e_R_z_sin = e_R.length();
float e_R_z_cos = R_z * R_sp_z;a×b=︱a︱︱b︱sinθ，a•b=︱a︱︱b︱cosθ，叉乘得到旋转角正弦，点乘得到旋转角余弦。
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);yaw的权值(不是很懂)，可能为期望机体系z轴与惯性系z轴之间的重合程度？
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* get axis-angle representation */
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;//注意：此处乘旋转角得到旋转角在机体各个轴上的投影，用于后续的姿态角控制
/* cross product matrix for e_R_axis */
//构造叉乘算子
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));//罗德里格斯公式
} else {
/* zero roll/pitch rotation */
R_rp = R;
}得到经过俯仰滚转运动后的坐标系，该坐标系与期望坐标系只有偏航上的偏差，z轴已经对齐；R_rp为该坐标系到地理坐标系的旋转矩阵。
罗德里格斯公式(得到以轴角形式表示的旋转矩阵)
/* 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;求出z轴对齐后两坐标系的x轴偏差角度，用作计算yaw的期望角速度。
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;
q.from_dcm(R.transposed() * R_sp);//直接求从当前机体系到目标机体系的四元数表示
math::Vector<3> e_R_d = q.imag();//根据四元数的定义可知，虚轴即为旋转轴
e_R_d.normalize();//旋转轴单位化
e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));//求出旋转角
/* 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;//最终得到的形式为直接一步旋转与分步旋转的加权和
}上述代码为大角度变化时的控制策略（大于90°时）；当角度过大时，直接一步旋转的权重较大，使系统快速回到90°的范围以内，再进行分步旋转控制。
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);//外环P控制
/* limit yaw rate */
_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);//限幅
/* feed forward yaw setpoint rate */
_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;//前馈相关知识补充
向量叉乘
a×b=︱a︱︱b︱sinθ
向量点乘
a•b=︱a︱︱b︱cosθ
叉乘算子
将向量叉乘形式表示为叉乘矩阵与向量的乘积
