PIXHAWK位置控制整体框架及期望推力向量转化成目标姿态旋转矩阵算法的深度解析
万事开头难，这是我的第一篇博客。谨以此将所学所悟记录下来，以防遗失，同时欢迎与大家进行技术交流，共同学习，共同进步，玩的开心！
这里的位置控制主要看PX4的mc_pos_control_main.cpp,其中位置控制主要执行函数为MulticopterPositionControl::task_main()。
首先位置控制的数据来源主要有三种：1、手动位置控制；2、offboard控制模式（由其他板载控制器提供控制指令）；3、auto控制模式（由设置好的航迹自动计算）。其代码可追溯：
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;
} else if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
_mode_auto = false;
} else {
/* AUTO */
control_auto(dt);
}
PX4的手动位置控制模式，手动打杆时采用速度控制模式，摇杆回中时采用位置控制模式。位置环采用P控制，即单纯比例控制，速度环采用PID控制，速度环的输出转换成期望推力。期望推力同位置、速度环一样都处于NED（北东地）坐标系。示例代码：
/* thrust vector in NED frame */
math::Vector<3> thrust_sp = vel_err.emult(_params.vel_p) + vel_err_d.emult(_params.vel_d) + thrust_int;
PX4位置控制的思想是垂直推力优先于水平推力，即先保持高度上的稳定，其次满足水平上的移动。由于飞机倾斜水平飞行时，其垂直推力向量减少，故对垂直推力进行了倾角补偿，从而提高垂向维度控制效果，使飞机在倾转时不至于突然掉高。
可见代码：
倾角补偿： /* thrust compensation for altitude only control mode */
float att_comp;
if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
att_comp = 1.0f / PX4_R(_att.R, 2, 2);
} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
saturation_z = true;
} else {
att_comp = 1.0f;
saturation_z = true;
}
thrust_sp(2) *= att_comp;
推力优先满足于高度维度：
/* preserve thrust Z component and lower XY, keeping altitude is more important than position */
float thrust_xy_max = sqrtf(_params.thr_max * _params.thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
计算出北东地期望推力（即期望加速度）输出，需要转化到机体坐标系（前右下）的期望角度。PX4的角度控制采用方向余弦矩阵控制，故需要将期望推力向量转化成目标余弦矩阵。关于方向余弦矩阵推荐看一下mahony的Direction Cosine Matrix IMU:Theory.(PX4的相关控制及姿态解算便是参考于此)。方向余弦矩阵见下图：
其中方向余弦矩阵与欧拉角的关系如下：
接下来对期望推力向量转化成目标姿态旋转矩阵算法进行一下深度解析。
首先将期望推力向量单位化，转换到zb(由于北东低转换到zb(b 代表body)维度无需进行相应的分量转换),代码：
body_z = -thrust_sp / thrust_abs;
然后将单位化后的期望推力向量根据当前航向角转换到xb维度，代码：
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
body_x = y_C % body_z;
其中%代表叉乘，该处是3*1的矩阵叉乘，矩阵叉乘方法在mahony中的讲解：
可根据此公式将北东地坐标系下的推力向量依据航向角（正北方向为0度）向前右下坐标系转换一下进行验证。转换之后将bx列向量进行单位化，代码：body_x.normalize();
最后将单位化的北东地坐标系下的推力向量依据单位化后的xb向量转换到by维度。其方法是yb=bz%bx,代码：body_y = body_z % body_x;（可将含欧拉角的zb与xb进行叉乘计算，可得到yb,同时可类比得出方向余弦矩阵的相关性质：zb=xb%yb; yb=zb%xb; xb=(-sin(),cos(
),0)%zb）,将xb,yb,zb进行汇总即得机体系的期望R，代码：
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
其中期望推力转化成期望姿态矩阵的执行细节和缜密证明请前往我的知乎专栏，就不直接搬过来了，否则行文太长了。
其中包括对于横滚为90度的特殊处理，很值得细细一看。
至此，PIXHAWK位置控制整体框架及期望推力向量转化成目标姿态旋转矩阵算法的深度解析完，如有误，还望提出指正。下集进行PIXHAWK姿态控制整体框架及控制算法的深度解析。