Ardupilot倾转分离函数thrust_heading_rotation_angles

本文主要写一下自己对于APM倾转分离（轴角分离）函数的一些学习笔记及思考。

ZING已经很好地分析了APM轴角分离函数，以下是我参考的他的一些博文。

[飞控]姿态误差(四)-APM如何计算姿态误差
[飞控]倾转分离(一)-APM的倾转分离竟然没有效果？
[飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵

后续分析过程会主要引用来自上面三篇，侵删。

什么是轴角分离

引文来源：https://zinghd.gitee.io/tilt_torsion_3/
1.起源
姿态中，roll 和 pitch 的改变来自于靠桨直接的力矩调整，调整很快，十几个毫秒就能到位。
而yaw的改变是靠桨速度差产生的旋转力矩来调整的调整比较慢，要快一百个毫秒才能到位。

2.问题
通常旋翼飞机的80%的能量用于油门控制抵抗重力，20%的能量用于控制姿态。
当 roll pitch yaw 都有较大误差时，20%的能量由三个控制器共同使用，但是由于 yaw 响应较慢，会导致 yaw 的误差一直都比较大，占用大部分姿态控制的能量，反而影响了整个姿态控制。

3.思路
因为旋翼稳定飞行的第一要素是保证桨平面( roll pitch )的精确控制，即保证桨平面没有误差 ,
yaw是不是没有误差并不重要。那么我们计算出真实姿态误差时，把误差分成两个部分tilt（pitch和roll）和 torsion（yaw），
但是呢，不直接把 torsion 给控制器
做一点限制，比如，「限幅」或者「衰减」
然后重新合成一个处理后的误差。
这样控制器就好认为，yaw 的误差并不大 ，大部分能量可以留给 tilt（pitch和roll）。

4.算法步骤
① 通过对齐 当前机体坐标系 z 轴 和 期望机体坐标系 z 轴 得到 tilt 误差
② 把 tilt 误差 转到 地理系 或者 转到 机体系
③ 总误差 - tilt 误差 = torsion 误差
④ 限制 torsion 误差 可以使用「限幅」方法 或者「衰减」方法
⑤ 把限制后的 torsion 和 tilt 组合成新的总误差

源码分析

以下内容备注来自我自己的一些考虑

// 将NED坐标系简称n系，机体坐标系简称b系
// 期望姿态表示为tb，当前姿态表示为cb（注意cb即为当前机体坐标系b系）
// 注意四元数表示的是三维空间中的旋转而不是点

程序开始//
// thrust_heading_rotation_angles() - 计算两个有序旋转，以将att_from_quat四元数移动到att_to_quat四元数。
// 第一个旋转校正推力矢量，第二个旋转校正航向矢量。
///上面的是官方注释
// att_to_quat：传入参数，期望姿态四元数。方向：tb->n
// att_from_quat：传入参数，当前姿态四元数。方向：cb->n。同时也表示从机体b系向n系的旋转。
// att_diff_angle：传入传出参数，姿态误差
// thrust_vec_dot：传入传出参数，按轴角法进行Z轴对其时的旋转角
void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot)
{
	///获取姿态的期望Z轴和当前Z轴/

	// 从四元数att_to_quat计算出旋转矩阵att_to_rot_matrix
	// 旋转矩阵att_to_rot_matrix用于将期望姿态旋转到n系
	// 并把期望姿态的Z轴转换到n系下的期望Z轴att_to_thrust_vec
	// 旋转方向：tb->n
    Matrix3f att_to_rot_matrix; 
    att_to_quat.rotation_matrix(att_to_rot_matrix); 
    Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);	// 期望姿态的标准z轴与旋转矩阵相乘得到n系下的期望z轴表示

	// 从四元数att_from_quat计算出旋转矩阵att_from_rot_matrix
	// 旋转矩阵att_from_rot_matrix用于将当前机体坐标系cb转换到n系
	// 并将当前姿态的Z轴转换到NED坐标系下的期望Z轴att_from_thrust_vec
	// 旋转方向：cb->n
    Matrix3f att_from_rot_matrix;
    att_from_quat.rotation_matrix(att_from_rot_matrix);
    Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);

	///下面通过轴角法和四元数配合，以Z轴误差计算tilt倾角误差/
	// 构建轴角（n系下）

	// 叉乘获取旋转向量（转轴）thrust_vec_cross（叉乘后不一定是单位向量）
	// 如c=a×b，表示绕着向量c将a旋转到b，|c|=|a|*|b|，向量c正交于向量a和b
	// thrust_vec_cross即为转轴
	// 旋转方向：att_from_thrust_vec -> att_to_thrust_vec
    Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;

	// 点乘获取旋转角thrust_vec_dot
	// 表示向量att_from_thrust_vec转向向量att_to_thrust_vec的夹角
	// 由于均为单位向量因此直接求arccos，之后限定在-1~1之间即可
    thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));

    // 单位化旋转向量thrust_vec_cross（转换为单位向量）
    float thrust_vector_length = thrust_vec_cross.length();
    if (is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)) {
        thrust_vec_cross = Vector3f(0, 0, 1);
        thrust_vec_dot = 0.0f;
    } else {
        thrust_vec_cross /= thrust_vector_length;
}

	// 轴角构造完毕，转轴为单位向量thrust_vec_cross，转角为thrust_vec_dot
	// 旋转方向：n系下向量att_from_thrust_vec（当前姿态z轴）-> 向量att_to_thrust_vec（期望姿态z轴）

	// 下面开始将Z轴对准，即通过Z轴误差获取倾角误差（不考虑Yaw转角）

	// 轴角转换到四元数
	// 四元数thrust_vec_correction_quat表示在n系下以thrust_vec_cross为转轴，thrust_vec_dot为转角的旋转过程
	// 此处似乎是对轴角和四元数的旋转方向有争议
    Quaternion thrust_vec_correction_quat;	
    thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);

	// 将倾角误差由n系转向机体坐标b系
	// 四元数att_from_quat表示有机体b系向n系旋转的过程，n = q×b×inv(q)
	// b = inv(q) ×n×q
	// 此处q为单位四元数，因此其逆等于共轭
	// 注意此处并非标准的四元数旋转描述，因为不是0标量四元数，详见下方
	thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;

	// 机体b系下的倾角误差获取完毕

	// 计算b系下的yaw转角误差
	// 注意：旋转矩阵B的逆 * 旋转矩阵A 等价于 旋转A与旋转B作差，由A减去B（但不是减法的意思，只是表明旋转姿态的差异）
	// 依次左乘：att_from_quat.inverse() * att_to_quat = 期望姿态 – 当前姿态 = 姿态总误差（total error）
	// thrust_vec_correction_quat.inverse() * total error = 姿态总误差 – 倾角误差（tilt error） = yaw转角误差（torsion error）
    Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;

	// 倾角误差四元数转换回轴角
	// 并通过att_diff_angle获取到roll和pitch上的误差角
    Vector3f rotation;
    thrust_vec_correction_quat.to_axis_angle(rotation);
    att_diff_angle.x = rotation.x;
    att_diff_angle.y = rotation.y;

	// 转角误差四元数转换回轴角形式
	// 通过att_diff_angle获取到yaw上的误差角（roll和pitch上的应该为0）
    yaw_vec_correction_quat.to_axis_angle(rotation);
	att_diff_angle.z = rotation.z;

	// 到此处获得了倾转的所有姿态误差，然而并没有对YAW角误差进行限制

    // Todo: Limit roll an pitch error based on output saturation and maximum error.

    // 基于最大加速度限制偏航误差-更新以包括输出饱和度和最大误差
    // 当前，该限制基于使用SQRT控制器的线性部分的最大加速度
    // 应该将其更新为基于角度限制，饱和度或基于用户定义的参数
    if (!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {
        att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());
        yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, att_diff_angle.z));
        att_to_quat = att_from_quat * thrust_vec_correction_quat * yaw_vec_correction_quat;
    }
}

一些细节补充

关于数学原理部分，我之前已经有一篇博文汇总过了：APM姿态旋转理论基础。因此其中一些数学原理的函数如轴角与四元数、四元数与旋转矩阵之间的相互转换此处不再补充。

另外关于叉乘和点乘的定义，可以参看这篇：向量内积（点乘）和外积（叉乘）概念及几何意义

// 轴角转换到四元数
// 四元数thrust_vec_correction_quat表示在n系下以thrust_vec_cross为转轴，thrust_vec_dot为转角的旋转过程
// 此处似乎是对轴角和四元数的旋转方向有争议
Quaternion thrust_vec_correction_quat;	
thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);

关于上面这段函数，参考的博文：姿态误差(四)-APM如何计算姿态误差，认为轴角转换到四元数后旋转方向改变，轴角旋转：当前->期望，而转换成四元数后：期望->当前。理由是轴角法描述的是向量的旋转，而四元数描述的是坐标系的旋转。

具体可以参照这篇解释博文：[飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵

然而参照轴角转换为四元数以及四元数表示旋转的公式（懒得打了），我个人依旧认为旋转方向是不变的（望打脸）。

不过此处相对来说并不那么重要，先了解姿态误差计算流程即可。

效果显示及进一步修改

MATLAB代码来自：[飞控]倾转分离(一)-APM的倾转分离竟然没有效果？

如上图所示，期望姿态欧拉角表示为(60, 60, 25)，att_diff_angle误差为(0.8194, 1.0325, -0.2072)

绿色表示当前姿态，与黑色NED坐标系重合，红色表示期望姿态，蓝色表示APM中轴角分离之后得到的att_diff_angle误差，然而可以看到其与期望姿态的Z轴并没有对齐，但是APM程序中明明是按照对齐Z轴进行姿态误差计算的，这是为什么呢？

因为APM程序中并没有对倾转分离后的z轴误差做处理，忽略处理了倾斜时产生的z轴误差，抑制效果不好。

正确做法是对torsion误差（YAW）按一定的比例进行缩小，限制旋转，然后用四元数乘法，重新叠加两个旋转，得到新的总旋转。

在att_diff_angle.z = rotation.z; 这段代码后面，可添加如下更新

// 以下内容未经过验证，仅提供思路
att_diff_angle.z = 0.5 * att_diff_angle.z；// torsion误差比例缩小进行衰减
yaw_vec_correction_quat.from_axis_angle(att_diff_angle.z);	// torsion误差转换为四元数形式
Quaternion att_diff_angle_quat = thrust_vec_correction_quat * yaw_vec_correction_quat;	// torsion和tilt旋转叠加
att_diff_angle_quat.to_axis_angle(rotation);	// 转换为轴角
att_diff_angle.x = rotation.x;
att_diff_angle.y = rotation.y;
att_diff_angle.z = rotation.z;

MATLAB实现效果如下，黑色部分为经过torsion误差衰减之后的att_diff_angle。误差表示为(0.7651, 1.0740, -0.0881)

结果对比可发现经调整之后的黑色坐标系相对于蓝色坐标系，Z轴已经完成对准。