PX4多旋翼期望姿态矩阵生成算法

• 1. PX4多旋翼期望姿态生成算法
  • 1.1. 求期望体轴X轴向量
  • 1.2. 求期望体轴Y轴向量
  • 1.3. 求期望姿态矩阵
  • 1.4. 求期望姿态角

1. PX4多旋翼期望姿态生成算法

PX4多旋翼期望姿态生成采用旋转矩阵方法，基本思路为根据外环解算获得期望推力方向（即期望体轴Z轴向量，因为多旋翼推力方向与体轴Z轴重合），再求期望体轴X轴向量，最后根据X轴、Z轴求得Y轴，再根据三个轴的向量求得期望姿态的旋转矩阵，最后根据期望姿态旋转矩阵求得期望姿态角（欧拉角主要用于记录和调试）。

当前PX4固件该段算法代码位于src\modules\mc_pos_control\PositionControl\ControlMath.cpp中的bodyzToAttitude()函数中实现。

void bodyzToAttitude(Vector3f body_z, const float yaw_sp, vehicle_attitude_setpoint_s &att_sp)
{
	// zero vector, no direction, set safe level value
	if (body_z.norm_squared() < FLT_EPSILON) {
		body_z(2) = 1.f;
	}

	body_z.normalize();

	// vector of desired yaw direction in XY plane, rotated by PI/2
	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);

	// desired body_x axis, orthogonal to body_z
	Vector3f body_x = y_C % body_z;

	// keep nose to front while inverted upside down
	if (body_z(2) < 0.0f) {
		body_x = -body_x;
	}

	if (fabsf(body_z(2)) < 0.000001f) {
		// desired thrust is in XY plane, set X downside to construct correct matrix,
		// but yaw component will not be used actually
		body_x.zero();
		body_x(2) = 1.0f;
	}

	body_x.normalize();

	// desired body_y axis
	Vector3f body_y = body_z % body_x;

	Dcmf R_sp;

	// fill rotation matrix
	for (int i = 0; i < 3; i++) {
		R_sp(i, 0) = body_x(i);
		R_sp(i, 1) = body_y(i);
		R_sp(i, 2) = body_z(i);
	}

	// copy quaternion setpoint to attitude setpoint topic
	Quatf q_sp = R_sp;
	q_sp.copyTo(att_sp.q_d);

	// calculate euler angles, for logging only, must not be used for control
	Eulerf euler = R_sp;
	att_sp.roll_body = euler(0);
	att_sp.pitch_body = euler(1);
	att_sp.yaw_body = euler(2);
}

1.1. 求期望体轴X轴向量

  // vector of desired yaw direction in XY plane, rotated by PI/2
	Vector3f y_C(-sinf(yaw_sp), cosf(yaw_sp), 0.0f);

	// desired body_x axis, orthogonal to body_z
	Vector3f body_x = y_C % body_z;

求期望体轴X轴向量主要由上面两行程序执行，其中yaw_sp根据外环解算已经获得，据此可获得期望体轴X轴单位向量在NED坐标系XY平面的投影(cosf(yaw_sp),sinf(yaw_sp),0.0f)，y_C为将投影向量向左旋转90°后的向量（交换x、y坐标，并新向量的x坐标添加负号，表示左转，这样使用y_C叉乘body_z后即为body_x），根据空间几何特性可知，y_C向量也垂直于期望体轴X轴（平面a内的直线x垂直于直线y在平面a里的投影y`，则直线x垂直于直线y —— 因为：直线x垂直于投影y`，直线y到平面a的垂线垂直于直线x，所以直线x垂直于投影y`与垂线组成的平面，而直线y位于投影y`与垂线组成的平面内，所以直线x垂直于直线y）。

期望体轴Z轴向量body_z已知，根据期望体轴X轴垂直于y_C，垂直于body_z，可通过y_C与body_z的向量积求得期望体轴的X轴向量body_x。

1.2. 求期望体轴Y轴向量

	// desired body_y axis
	Vector3f body_y = body_z % body_x;

期望体轴Y轴向量body_y通过期望体轴Z轴向量body_z与期望体轴X轴向量body_x求向量积可求得（注意向量积顺序）。

1.3. 求期望姿态矩阵

	Dcmf R_sp;

	// fill rotation matrix
	for (int i = 0; i < 3; i++) {
		R_sp(i, 0) = body_x(i);
		R_sp(i, 1) = body_y(i);
		R_sp(i, 2) = body_z(i);
	}

期望姿态矩阵根据求得的期望体轴向量借助旋转矩阵作用原理求得。

$$R_e^b\cdot \overrightarrow{v_e}=\overrightarrow{v_b}$$

其中$R_e^b$为NED到体轴的旋转矩阵，$\overrightarrow{v_e}$为NED坐标系下的向量，$\overrightarrow{v_b}$为经过旋转后的体轴坐标系下的向量。$R_e^b$向量如下:

$$R_e^b=\left[\begin{array}{ccc}{a}_{00}& a_{01}& a_{02}\\ a_{10}& a_{11}& a_{12}\\ a_{20}& a_{21}& a_{22}\end{array}\right]$$

取$\overrightarrow{v_e}$为NED坐标系下$x$轴的单位向量$\overrightarrow{x_{ei}}={\left[\begin{array}{ccc}1& 0& 0\end{array}\right]}^{\prime }$，经过旋转后成为体轴坐标系下的向量$\overrightarrow{x_{bi}}$，即body_x，即:
$$R_e^b\cdot \overrightarrow{x_{ei}}=\left[\begin{array}{ccc}{a}_{00}& a_{01}& a_{02}\\ a_{10}& a_{11}& a_{12}\\ a_{20}& a_{21}& a_{22}\end{array}\right]\cdot \left[\begin{array}{c}1\\ 0\\ 0\end{array}\right]=\left[\begin{array}{c}{a}_{00}\\ a_{10}\\ a_{20}\end{array}\right]=\left[\begin{array}{c}body\_x(0)\\ body\_x(1)\\ body\_x(2)\end{array}\right]$$
据此可根据body_x求得$R_e^b$中对应位置变量，同理可采用同样方法通过body_y和body_z求得$R_e^b$其他位置变量，据此可得出期望姿态旋转矩阵$R_e^b$。

1.4. 求期望姿态角

	// calculate euler angles, for logging only, must not be used for control
	Eulerf euler = R_sp;
	att_sp.roll_body = euler(0);
	att_sp.pitch_body = euler(1);
	att_sp.yaw_body = euler(2);

根据旋转矩阵求欧拉角，px4中使用的旋转顺序为ZYX。