[飞控] 姿态误差 (五)-PX4 如何计算姿态误差?

之前我们非常详细的介绍了 APM 计算姿态误差的过程,而且还用 MATLAB 验证了它的算法,效果并不是特别好。

好奇心驱使,我想对比一下 PX4 的算法是否会有不一样的结果呢?毕竟是江湖的两大门派那就比一下。

结果一看,PX4 竟然更新了它的姿态算法,跟以前完全不一样的了。

好在给出了参考文献,我看了一下论文 ,还好基本思路没什么大变化。

1.先对齐z轴求得一个「倾斜误差」。

2.「总误差」—「倾斜误差」得到「旋转误差」。

3.对这个真实的旋转误差z做一些限制,得到一个「虚拟旋转误差」。

4.「虚拟旋转误差」加「倾斜误差」得到「虚拟总误差

这个「虚拟总误差」就是直接传递给控制器的误差啦,控制器就通过这个值来进行控制。

其实这个过程跟 APM 是基本一致的,就是最后合成新旋转的方法, APM 因为直接抛弃「倾斜误差」的 z 轴分量,有点没有道理。PX4 的处理方法就更加合理,直接还原成「虚拟总误差」很好理解,整个过程就相当于减去了一部分yaw。

分析到这,我们已经非常熟悉这套思路了,所有知识点,之前的文章都有画过重点,很明显这是道送分题。

是不是在脑海里已经有了算法的过程呢?(没有的话,就该去把我之前的文章都点赞了,哈哈)

唯一一个最困扰的问题是

论文和程序,在这个部分看起来是相反的,这个问题困扰了我很久。论文推了很多遍,代码也推了很多遍,甚至MATLAB仿真也跑了很多遍,只能得出程序没错,论文也没错,就是不知道为什么是反过来的!

后来就在我放弃的时候,我想反正我对姿态控制这么熟悉了,不看他的代码,如果我来做我会怎么做?

问题一下就迎刃而解了,主要的知识点我在[飞控]向量叉乘究竟是个什么样的旋转?中介绍过了。

我再来详细介绍一下整个计算过程:

当前姿态:cur 目标姿态:tar z轴对齐的中间状态:half

1.z 轴向量叉乘,得到对齐 z 轴的旋转,注意转轴还是N系

$$[Q_{half}^{cur}]^N$$

2.转轴转换到 cur 系 (这一步和APM是一模一样的哟)

$$Q_{half}^{cur}=(Q_{cur}^N)^{*} [Q_{half}^{cur}]^N (Q_{cur}^N)$$

3.计算地理系下的倾斜旋转(论文里写的其实是这一步)

$$Q_{half}^N = Q_{cur}^N Q_{half}^{cur}$$

4.计算旋转误差

$$Q_{tar}^{half}=(Q_{half}^N)^*Q_{tar}^N$$

5.使用固定系数yaw_w 限制 旋转误差

$$q_{tar}^{half}=Q_{tar}^{half}*yaw_w$$

可以看到程序里是把第 2 步合第 3 步合起来写了,才有看起来反过来的感觉,很有迷惑。

除了这个困惑,其他的部分我们就非常熟悉了,总结一下:

1.在倾转分离后,PX4 采用了 「衰减」系数的形式来限制「倾斜误差

2.使用四元数乘法还原「虚拟总误差」,一定可以对其 z 轴。

3.不知道为什么新算法采用固定的「衰减」系数,之前是根据当前的 roll 和 pitch 计算变化的系数

至此,能想像到这个算法的结果,当前姿态可以和期望姿态 z 轴重合,偏航会被限制,导致 x 轴 ,y 轴还有一定的误差

ok,今天就讲这么多,下期我会使用 MATLAB 看看这个算法的直观效果,我是zing,一个有趣的算法工程师,我们下期见。

相关阅读:
[飞控]向量叉乘究竟是个什么样的旋转?
[飞控]姿态误差(四)-APM如何计算姿态误差
[飞控]倾转分离(一)-APM的倾转分离竟然没有效果?

ps:为了方便感兴趣的朋友,给出这部分代码的注释,方便对照

MulticopterAttitudeControl::control_attitude()
{
    vehicle_attitude_setpoint_poll();

    _thrust_sp = -_v_att_sp.thrust_body[2];

    //zing-note:modules\mc_att_control\mc_att_control_params.c
    //zing-note:根据增益求偏航权重
    Vector3f attitude_gain = _attitude_p;
    // 6.5    6.5   2.8
    const float roll_pitch_gain = (attitude_gain(0) + attitude_gain(1)) / 2.f;
    const float yaw_w = math::constrain(attitude_gain(2) / roll_pitch_gain, 0.f, 1.f);
    attitude_gain(2) = roll_pitch_gain;
    //zing-note: 目标到初始
    Quatf q(_v_att.q);
    Quatf qd(_v_att_sp.q_d);
    //zing-note: 归一化
    q.normalize();
    qd.normalize();
    //zing-note:z轴转到机体系
    Vector3f e_z = q.dcm_z();
    Vector3f e_z_d = qd.dcm_z();    
    //zing-note:Chalf->cur 
    Quatf qd_red(e_z, e_z_d);
    if (abs(qd_red(1)) > (1.f - 1e-5f) || abs(qd_red(2)) > (1.f - 1e-5f)) {
        //zing-note:没有倾斜误差的情况
        qd_red = qd
    } else {
        //zing-note:qd_red = Qhalf->N  =  Qcur->half * Qcur->n
        qd_red *= q;
    }
    //zing-note:q_mix 旋转误差  Qtar->half = (Qhalf->n)^-1 * Qtar->n
    Quatf q_mix = qd_red.inversed() * qd;
    q_mix *= math::signNoZero(q_mix(0));
    q_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);
    q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);    
    //zing-note:限制旋转误差  组合成新的旋转 qd= Qhalf->N * Qtar->half = Qtar->N
    qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3))));
    //zing-note:计算出[虚拟总误差]
    Quatf qe = q.inversed() * qd;

    //zing-note:后面的可以战胜不管
    Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();
    _rates_sp = eq.emult(attitude_gain);
    _rates_sp += q.inversed().dcm_z() * _v_att_sp.yaw_sp_move_rate;
    /* limit rates */
    for (int i = 0; i < 3; i++) {
        if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
            !_v_control_mode.flag_control_manual_enabled) {
            _rates_sp(i) = math::constrain(_rates_sp(i), -_auto_rate_max(i), _auto_rate_max(i));

        } else {
            _rates_sp(i) = math::constrain(_rates_sp(i), -_mc_rate_max(i), _mc_rate_max(i));
        }
    }
}

关注公众号回复【倾转分离】获得PX4的参考论文。