本文主要写一下自己对于APM倾转分离(轴角分离)函数的一些学习笔记及思考。
ZING已经很好地分析了APM轴角分离函数,以下是我参考的他的一些博文。
[飞控]姿态误差(四)-APM如何计算姿态误差 [飞控]倾转分离(一)-APM的倾转分离竟然没有效果? [飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵
后续分析过程会主要引用来自上面三篇,侵删。
什么是轴角分离
引文来源:https://zinghd./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轴已经完成对准。
|