PIXHAWK姿态控制整体框架及控制算法的深度解析

    上文写了PIXHAWK的位置控制算法，该文对其姿态控制进行深入解析。其中姿态控制主要看PX4的mc_att_control_main.cpp,其中姿态控制主要执行函数为MulticopterAttitudeControl::control_attitude(float dt)。姿态控制整体框架：角度环采用P控制,角速度采用PID控制（但含有很多先进PID的控制方法，文章会提到）。

        姿态控制首先要获得位置控制计算而得的期望旋转矩阵R_sp和当前的姿态旋转矩阵R。

代码：

R_sp.set(_v_att_sp.R_body);

R.set(_v_att.R);

其次便要获取两个旋转矩阵之间的误差角度，根据方向余弦矩阵求取角度误差，PX4中思想是先对齐Z轴（由于Z轴航向的响应相对横滚和俯仰较慢，故先对其Z轴），然后求取横滚和俯仰维度的角度误差，最后再对航向误差进行相对补偿，这样飞机的行驶路径相对最小，否则飞机在横滚、俯仰补偿的同时转动航向飞机转动行驶路线是个曲线，路径相对较长（个人对矩阵旋转的运动学理解）。英文解释：

/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */，

/* calculate rotation matrix after roll/pitch only rotation */。

        算法实现：首先对齐Z轴，求取横滚和俯仰的角度偏差：math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);

其中R_z % R_sp_z（向量叉乘）产生一个新的机体平面对应的法向量，R.transposed()为R的转置（旋转矩阵R见下图），航向z轴与新产生的法向量垂直，故此时求得的e_R（2）为0，只能求出对应的横滚、俯仰的角度偏差向量（不是实际的偏差值）R的转置与法向量相乘，是当前姿态矩阵在法向量上的投影，求取俩个姿态矩阵的误差。

接下来对实际的横滚，俯仰偏差值进行求解。根据叉乘、点乘公式，如下：