[飞控]姿态误差(四)-APM如何计算姿态误差？

hamycui

通过我们之前的文章分析，对姿态的误差已经有了比较清楚的认识。
不妨假设如果你负责控制部分的设计，你会怎么做？
这时候就体现出来做控制的好处了，当前姿态是导航算解好的，控制只需要从导航中取出使用即可。
如果是我的话，我会喜欢最直接最简单最直观的思路：
通过导航 得到 当前姿态 ， 通过遥控得到期望姿态。
将姿态使用旋转矩阵形式描述，直接求出姿态误差


然后将误差通过罗德里格斯公式转换成轴角，直接给控制器，简单直接高效！


没错，你没有看错，计算旋转误差就是这么简单。
但是很多朋友都觉得看了这么多理论还是看不懂代码为什么要这么写，今天就给他整个明明白白。
以APM为例，在基本思路的基础上做了两点改动。
1.不用旋转矩阵用四元数计算误差。
虽然在单次旋转时旋转矩阵计算量小于四元数，但是在多次不断的旋转时四元数计算是可以的减少计算量的，所以使用四元数更新姿态更加迅速。四元数的引用是为了减少计算量和计算时存储占用的空间。
2.使用倾转分离的思路，先计算倾斜误差，再计算旋转误差
简单说一下倾转分离，看起来很高大上，其实主要灵感来源于工程经验。
旋翼飞行器的速度与机头无关，所以机头的方向对飞行没有影响，无论机头指向哪都可以飞到目的地。
旋翼飞行器最重要的是维持桨平面状态，也就是roll 和 pitch ，所以姿态中roll 和 pitch 更为重要
而且，roll 和 pitch 的改变使用的是螺旋桨的生力，而 yaw 的改变 是螺旋桨的差速 ，所以同样的角度误差 ，yaw 需要更多时间才能达到期望。
这就意味着当roll pitch 都达到期望时 yaw可能没达到 ，此时你希望飞机达到下一个期望姿态 ， 导致 yaw 的误差越来越大，导致yaw通道一直有较大的输出 ,反而会影响 roll 和 pitch 的控制。
所以我们希望的是 如果期望误差三个轴都很大，那么先全力把roll，pitch稳定，然后再去处理yaw 。
那岂不是要先启动 roll 和 pitch 控制器，然后再启动yaw控制器？
不不不没这么复杂，我们可以欺骗控制器，最简单的粗暴的方法是直接限制z轴误差。
例如 如果z轴误差大于3 那么z轴误差等于3。
if(z轴误差>3)

    z轴误差=3;

else if(z轴误差<-3)

    z轴误差=-3;
这样的话即使实际的z轴误差很大，控制器也会以为z轴误差很小，可以使z轴姿态控制器的输出比较小，不影响整体的控制。
当然我们希望有更加智能的方法 ，把yaw乘一个系数K(0,1) ，
roll ，pitch 误差越大，系数k越小 ，越接近0
roll ，pitch 误差越小 ，系数k越大，越接近1。（这是PX4的思路）
倾转分离的目的是让姿态更加迅速的稳定。
ok，思路清晰了我们就可以真正的进入实战分析了。
位置：libraries\AC_AttitudeControl\AC_AttitudeControl.cpp
函数名：AC_AttitudeControl::thrust_heading_rotation_angles
源码：
void AC_AttitudeControl::thrust_heading_rotation_angles

(Quaternion& att_to_quat, const Quaternion& att_from_quat,

Vector3f& att_diff_angle, float& thrust_vec_dot)

{

    //把期望姿态的四元数转换为旋转矩阵 期望旋转att_to_rot_matrix

    //期望旋转att_to_rot_matrix 表示 期望机体坐标系 转到 NED 的旋转

    //记为：Ctb->n 期望姿态机体坐标系 转 NED

    Matrix3f att_to_rot_matrix;     

    att_to_quat.rotation_matrix(att_to_rot_matrix);

    //机体坐标系的z轴向量（0,0,1）乘以期望旋转 既把期望机体坐标系的z轴转到NED系

    //zing-note:期望姿态的旋转矩阵 取出z轴列（z轴单位向量）

    Vector3f  att_to_thrust_vec  = att_to_rot_matrix*Vector3f(0.0f,0.0f,1.0f);

    //获取当前姿态对应的旋转矩阵

    Matrix3f att_from_rot_matrix;

    att_from_quat.rotation_matrix(att_from_rot_matrix);

    //把当前姿态的z轴坐标系转到NED系下

    Vector3f att_from_thrust_vec = att_from_rot_matrix*Vector3f(0.0f,0.0f,1.0f);





    //当前机体坐标系的z轴 与 期望机体坐标系的z轴 叉乘 可以得到旋转向量

    //叉乘： a X b 代表 a 转到 b  a为初始 b为目标

    //绕着这个向量可以将两个z轴 转至重合

    Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;



    //当前机体坐标系的z轴 与 期望机体坐标系的z轴 点乘 可以得到两个向量的夹角的余弦 即 cos theta

    //通过反余弦函数 求得 旋转角度 theta   即 thrust_vec_dot 表示 轴角的角

    thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec,-1.0f,1.0f));



    //旋转向量标幺化 得到单位旋转向量 即 轴角的轴

    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;

    }



    // 通过 求得的 旋转轴 和 旋转角度 可以构造轴角 然后将其转换为四元数

    // 该轴角描述的是 初始->目标 的旋转 转成四元素描述的是 目标->初始 的旋转

    // 这里的目标 仅仅是 z轴重合的状态 不考虑yaw是否对齐.

    //因为这是个中间状态所以把这次旋转叫做 tb1->cb(n系)

    //（当前转轴 thrust_vec_cross 是N系下的）

    Quaternion thrust_vec_correction_quat;

    thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);



    //旋转 tb1->cb(n系) 转到 tb1->cb(cb系)



    //这里用的不是标准的四元数描述旋转，不是0标量四元数，

    //目的是 将 这个误差旋转从N系转到机体系 虽然结果相同但是显的思路很乱

    //清晰的思路是 1.将转轴从N系转到机体坐标系 2.再把新的轴角转到四元数 就得到了机体坐标系下的旋转

    thrust_vec_correction_quat = att_from_quat.inverse()*thrust_vec_correction_quat*att_from_quat;

    //至此 得到的这个旋转可以把z轴对齐 ， 现在需要考虑如何把yaw对齐



    //总误差: att_from_quat.inverse()*att_to_quat

    //当前姿态的逆*期望姿态 就是 期望姿态-当前姿态的意思 得到的是总误差

    //旋转误差: 倾斜误差的逆*总误差 就是 总误差减去倾斜误差的意思 得到的是 剩余的旋转误差                                       

    //向量的旋转需要使用四元数旋转公式，四元数的连乘类似于旋转矩阵 可以约去坐标系

    //Qcb->tb1 * Qn->cb * Qtb->n = Qtb->tb1 (即旋转误差)

    Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse()*att_from_quat.inverse()*att_to_quat;





    //倾斜误差转换成 轴角 取其中的x,y

    Vector3f rotation;

    thrust_vec_correction_quat.to_axis_angle(rotation);

    att_diff_angle.x = rotation.x;

    att_diff_angle.y = rotation.y;



    //旋转误差转换成 轴角 取其中的z

    yaw_vec_correction_quat.to_axis_angle(rotation);

    att_diff_angle.z = rotation.z;



    //至此 得到了 期望姿态 与 当前姿态的全部误差



    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;

    }

}
总结一下：
1.通过z轴来表示倾斜(roll,pitch)的误差


2.总误差-倾斜(roll,pitch)误差=旋转误差


有几个不明朗的点：
1.没有使用0标量四元数来做标准的四元数乘法。
个人认为清晰的思路是，将转轴从N系转到机体坐标系 ，再把新的轴角转到四元数 就得到了机体坐标系下的旋转
2,轴角分离之后没有抑制轴角
虽然轴角分开算了，但是没有对分离后的z轴误差做处理，只是抛弃了倾斜时产生的z轴误差，抑制效果不好。
(之后我会用MATLAB来直观的对比一下这样的抑制效果到底强不强)
关注微信公众号【无人机干货铺】，认识一个有趣的飞控算法工程师。

daza110

如果偏航角连续转圈，采用轴角计算出来的姿态差没法连续，会突变

远洋航空

没处理好吧 处理好没问题 因为大部分偏航旋转控制的是角速度

小禹宇鱼

请教下，为什么轴角向量的x, y对应的是roll pitch的误差呢？z对应的是yaw的误差？这种对应关系是怎么建立起来的？

传播11

好问题，我之前想继续研究，大概方向应该是李群李代数方面有解释，但是我后续也没有继续弄了

青田石头

恩，在APM解锁前姿态一致性检测函数attitudes_consistent()也是这样用的，多核EKF计算姿态差异，用四元数做差后，直接构造轴角向量，然后该向量的x,y,z对应的就是roll pitch yaw的差异了，一直没理解为啥能这样对应