一.下载源码

首先，我们先下载PX4的源码。本人将要分析的为1.54版本的PX4源码。

1.官方地址传送门：https://github.com/PX4/Firmware/archive/v1.5.4.zip

2.百度网盘：https://pan.baidu.com/s/1HYUuxED1Nv2_uE_sMx1RQQ

打开文件夹Firmware-1.5.4\src\modules\mc_att_control，文件mc_att_control_main.cpp就是PX4多旋翼无人机的姿态控制主文件。

定位到程序void MulticopterAttitudeControl::control_attitude(float dt)，PX4的源码是使用C++写的，C++与C的不同在于类，上述函数的意思是control_attitude是MulticopterAttitudeControl这个类的类成员函数，其实也不需要知道，有兴趣的朋友可以看看《C++ primer plus》这本书。

二.分析源码

下面我们开始分析源码。

	vehicle_attitude_setpoint_poll();

	_thrust_sp = _v_att_sp.thrust;

1.首先获取最新的期望角度，然后获取期望的推力，对于姿态模式来说来说，推力，期望角度是遥控器直接赋值的，对于GPS定点模式来说，期望的推力，速度，是无人机计算得出的。

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

	/* all input data is ready, run controller itself */

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

2.首先构建一个期望姿态的四元数q_sp。然后将四元数转化为期望姿态的旋转矩阵R_sp，学C的可能不怎么懂这一步，C++有一个特性，类在初始化的时候可以直接赋值调用该类的构建函数。此时我们已经有了一个期望姿态的旋转矩阵，该矩阵的第一列（X轴），第二列（Y轴），第三列（Z轴）（PX4的旋转矩阵是每一列代表一个坐标轴，与正常每一行代表一个坐标轴的矩阵是刚好互为转置的）。然后再把实际的姿态也转化为一个四元数q_att,再转化为旋转矩阵R。最后取出这期望姿态的Z轴和实际姿态的Z轴。

为什么这里要把两个Z轴单独拿出来呢？是因为某篇论文上写的，由于无人机的俯仰，横滚的控制比航向的控制要快，所以把航向控制与俯仰、横滚的控制独立出来对于控制效果更好。（下载地址：https://pan.baidu.com/s/1TxPFfoVXaU6Xh9NpEj7kmg）所以PX4的姿态控制，是先把两个Z轴重合，此时得出的过渡姿态的俯仰横滚与期望姿态的俯仰横滚是一致的，除了它们航向不一样。然后再求过渡姿态与期望姿态的航向角误差，对航向控制进行各种抑制，限幅后再叠加到姿态控制上。也就是说，正常PX4的姿态控制是先大幅度的旋转俯仰横滚到与期望姿态的俯仰横滚一致，再进行航向的旋转。当然，这只是一种比较容易的理解方法，实际是转俯仰横滚的同时也会转航向，但是航向的优先级是最低。毕竟对于多旋翼无人机来说，俯仰，横滚不乱就不会炸机，航向只是为了更好的控制位置。对于PX4的姿态控制，输入是期望角度，实际角度，输出是期望角速度（机体坐标系）。

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

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

3.然后进行实际姿态的Z轴R_z向量与期望姿态的Z轴R_sp_z向量的叉乘。我们知道，向量与向量的叉乘可以得到一个与原来两个向量都垂直的向量。这个向量其实就是这个一个向量旋转到另一个向量的旋转轴。

所以e_R实际就是实际姿态旋转到过渡姿态的旋转轴。然后用R的转置矩阵乘旋转轴向量，实际就是把e_R映射到机体坐标系中。如果是实际姿态的矩阵R乘机体坐标系的向量，就是把机体坐标系转换到大地坐标系，而R的转置矩阵，就是把大地坐标系转换为机体坐标系。之所以要转换是因为姿态控制给出的期望角速度是要转换到机体坐标系中方便控制。

然后求出旋转角度e_R_z的sin和cos，得到e_R_z_sin和e_R_z_cos。我们知道假设两个向量的夹角为a，那么a*b=|a||b|cosa(点乘)，axb=|a||b|sina。所以e_R_z_sin=|e_R|（e_R.length就是求e_R的模长，R_z和R_sp_z是单位向量，在四元数转换旋转矩阵的时候就被归一化了，所以它们的模长等于1。R_z和R_sp_z的叉乘e_R的模长就等于sina，还有一点，坐标系的转换是不会改变两个向量的夹角的，所以不管e_R在哪个坐标系，它的模长是固定的。），e_R_z_cos就等于R_z和R_sp_z的点乘。

上图代码最后计算了航向的权重yaw_w。它等于R_sp的第三行第三列的矩阵元素的平方。第三列是期望姿态的Z轴的Z坐标。在期望姿态为水平时，Z轴应为（0,0,1），即Z坐标为1。当期望姿态Z轴与垂直呈45°时，Z坐标应为cos45°=0.707。它的平方为0.5。可以看到，随着期望姿态的Z轴越偏离竖直朝下的Z轴，航向的比重会相应下降。  

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;
	}

4.上面的代码就是求实际姿态Z轴旋转到与期望姿态Z轴重合的过渡姿态R_rp。R_rp与期望姿态的Z轴重合，但是航向存在误差。

那么问题来了，如何求这个过渡姿态对应的旋转矩阵R_rp。首先，我们知道向量R_z和R_sp_z的夹角。上面已经求了该夹角的三角函数值e_R_z_sin和e_R_z_cos。然后我们还知道它们旋转的旋转轴。理论上，可以构造这个旋转过程的四元数，然后在把四元数转化为代表着同一旋转旋转矩阵。然后做一个矩阵的乘法运算就可以得出对应R_rp。PX4也是想求出这个旋转矩阵，但是它的算法是走的另一条路——罗德里格（Rodrigues）旋转方程。

下面我们一起来推导一下这种用于推导任意向量绕某一旋转轴旋转固定角度后得到的新向量如何求出。

现在有一个向量V，它绕单位向量k旋转的角度为θ，旋转后的向量为Vrot。现在我们已知向量V和向量k的坐标，问如何得到向量Vrot。

首先将有一个平面，垂直旋转轴k，向量V在该平面上的投影为V⊥。而V在向量K上的投影为V∥，可得

V=V⊥+V∥         (1-1)

然后设kxV=w。可知w也在k的垂直平面上，且与 V⊥垂直。由此可以推导出

V⊥=-kxw（注意叉乘的方向）=-kx（kxV）         (1-2)

然后，由V=V⊥+V∥可得

V∥=V-V⊥（向量的减法）=V-[-kx（kxV）]=V+kx（kxV）        (1-3)

然后，设Vrot在向量k的垂直平面上的投影为Vrot⊥。同理可得

Vrot=Vrot⊥+V∥         (1-4)

把向量Vrot⊥分解到向量V⊥和向量w上。可得

Vrot⊥=V⊥cosθ+wsinθ=-kx（kxV）cosθ+（kxV）sinθ        (1-5)

证明如下

|w|=|V|sina(假设k与V的夹角为a)

|V⊥|=|Vrot⊥|=|V|cos（π/2-a）=|V|sina=|w|，即V⊥，Vrot⊥，三者模长是相等的。

Vrot⊥的水平分量=|Vrot⊥|*cos（π-θ） *-V⊥/|V⊥|=V⊥cosθ （任何向量都等于模长乘与其方向一致的单位向量）

Vrot⊥的垂直分量=|Vrot⊥|*sin（π-θ） *w/|w=wsinθ

把公式1-5和公式1-3代入公式1-4可得

Vrot=Vrot⊥+V∥   

      =-kx（kxV）cosθ+（kxV）sinθ +V+kx（kxV）

      =V+（1-cosθ）kx（kxV）+sinθ（kxV）          （1-6）

由此，我们已经得到如何求出Vrot的公式了，下面我们把它推广到矩阵。

首先我们看kXV。

所以可以构造一个叉乘矩阵K，使得等式kxv=Kv（矩阵的乘法）成立。

           （1-7）                  

再看公式1-6中的kx（kxV）。设向量temp=kxV=Kv。

所以kx（kxV）=kxtemp=Ktemp=K*K*v。

由于矩阵的乘法满足乘法结合律。所以K*K*V=(K*K)*V=K²V。

又有单位矩阵乘任何向量都等于它本身。设i为3*3的单位矩阵。V=IV。

把上述叉乘矩阵代入公式1-6可得：

Vrot=iV+（1-cosθ）K²V+KVsinθ         （1-8）

由于矩阵的乘法满足左分配律（即（A+B）C=AC+BC,需注意（A+B）C≠CA+CB）

故可得：

Vrot=[i+（1-cosθ）K²+Ksinθ]V        （1-9）

故，我们可得，有矩阵Rrot=[i+（1-cosθ）K²+Ksinθ]。可使Vrot=RrotV成立。即Rrot为向量V旋转到Vrot的旋转矩阵。PX4使用的就是这种算法来计算实际姿态到过渡姿态的矩阵。得到旋转矩阵R后。我们就可以求出过渡姿态对应的旋转矩阵了。（旋转矩阵既可以表示一个旋转，也可以表示坐标系旋转后的坐标）。

根据公式，首先判断e_R_z_sin。当实际姿态的Z轴与过渡姿态的Z轴重合时，它们的角度差为0。故sin也会是0（e_R_z_sin是用模长求出的，没有负数）。此时可直接令过渡姿态的矩阵R_rp=R。当实际姿态的Z轴不与过渡姿态的Z轴重合时（e_R_z_sin>0）。求两个Z轴的夹角e_R_z_angle，直接用反三角函数求的。然后求旋转轴的单位向量e_R_z_axis（罗德里格旋转方程要求旋转轴为单位向量）。然后用夹角乘e_R_z_axis得到该旋转对应的旋转向量e_R。（在物理学中，旋转是用向量表示的。和速度一样，有大小，有方向。大小就是角度，方向就是指向旋转轴。）由于e_R是在机体坐标系的，所以直接可以得到过渡姿态与实际姿态在机体坐标系上的角度差。

然后开始构造叉乘矩阵e_R_cp，直接按照公式（1-7）构造。

然后按公式（1-9）求出过渡姿态的矩阵R_rp。

注意一个细节。PX4源码使用的是R*e_R_cp而不是e_R_cp*R。首先可以肯定是，矩阵的位置是不能改变的，那为什么PX4把这两个矩阵互换位置了呢？是因为e_R_cp其实是在机体坐标系中构造出来的，也就是说e_R_cp不能在大地坐标系中使用。把单位矩阵i加入公式会更清晰，R_rp=R*（i*e_R_cp）。意思是，实际姿态的矩阵位于机体坐标系就是单位矩阵i。所以乘上e_R_cp后得到的就是过渡姿态的矩阵（机体坐标系），然后用R乘上过渡姿态的矩阵就可以得到过渡姿态在大地坐标系的矩阵了。（R*V代表把向量V从机体转换到大地坐标系）。

至此，过渡姿态的矩阵R_rp已经得出（位于大地坐标系）。