[飞控]姿态误差(五)-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$$

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

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

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的参考论文。