PX4姿态估计源码分析

写在前面

今天入坑PX4开始学习PX4代码，pixhawk硬件是可以支持PX4、ardupilot两套固件。我用的是1.6.0rc1版本代码。

代码位置：\Firmware1.6.0rc1\src\modules\attitude_estimator_q

整体框架：

1. 先进行数据的获取（a、subscribe订阅的数据 b、从参数文件拷贝的默认参数）
2. 姿态估计
3. 数据发布（a、vehicle_attitude b、control_state）

PX4姿态估计源码分析

先找到功能入口函数：

int attitude_estimator_q_main(int argc, char *argv[])
{
	if (argc < 2) {
		warnx("usage: attitude_estimator_q {start|stop|status}");
		return 1;
	}

	if (!strcmp(argv[1], "start")) {

		if (attitude_estimator_q::instance != nullptr) {
			warnx("already running");
			return 1;
		}

		attitude_estimator_q::instance = new AttitudeEstimatorQ;

		if (attitude_estimator_q::instance == nullptr) {
			warnx("alloc failed");
			return 1;
		}

		if (OK != attitude_estimator_q::instance->start()) {
			delete attitude_estimator_q::instance;
			attitude_estimator_q::instance = nullptr;
			warnx("start failed");
			return 1;
		}

		return 0;
	}

	if (!strcmp(argv[1], "stop")) {
		if (attitude_estimator_q::instance == nullptr) {
			warnx("not running");
			return 1;
		}

		delete attitude_estimator_q::instance;
		attitude_estimator_q::instance = nullptr;
		return 0;
	}

	if (!strcmp(argv[1], "status")) {
		if (attitude_estimator_q::instance) {
			attitude_estimator_q::instance->print();
			warnx("running");
			return 0;

		} else {
			warnx("not running");
			return 1;
		}
	}

	warnx("unrecognized command");
	return 1;
}

此部分代码分为几个部分：

进入后会判断输入的字符个数，如果小于2会打印提示应该输入的字符：
“usage: attitude_estimator_q {start|stop|status}“

后面就是对这个字符的判断:
if 输入的是start
if 此进程已经运行就打印already running
否则 就实例化一个对象 （其中包含了一些私有变量的初始化）
if 创建失败就打印出alloc failed
if 创建成功就start()这个进程（这也是我们最关心的 后面会跟进这个进程）

if输入的是stop
如果没有运行就打印not running，否则删除进程

if输入的是status
如果在运行就打印出打印函数里面所有的东西。并显示runing否则打印 not runing

多说一句：功能函数入口函数基本都是这个结构。

/*************************************************************************************************************************************/
跟进start()

int AttitudeEstimatorQ::start()
{
 //AEESRT是assert函数的define了，作用是参数值为0时终止程序
	ASSERT(_control_task == -1);
	/* start the task */
	_control_task = px4_task_spawn_cmd("attitude_estimator_q",
					   SCHED_DEFAULT,
					   SCHED_PRIORITY_MAX - 5,
					   2000,
					   (px4_main_t)&AttitudeEstimatorQ::task_main_trampoline,
					   nullptr);

	if (_control_task < 0) {
		warn("task start failed");
		return -errno;
	}

	return OK;
}

函数中px4_task_spawn_cmd函数的作用是创建一个新的进程，在nuttx系统中作为一个独立的进程运行，和其他模块之间通过uORB进程间相互通讯，参数变量表示分别为：1、进程的入口函数 2、进程默认调度 3、进程优先级 4、进程栈大小 5、进程入口函数（下一步会跟进这个函数） 6、nullptr。

跟进进程入口函数：

void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[])
{
	attitude_estimator_q::instance->task_main();
}

跟进task_main() 因函数体积较大故将其拆开分析

#ifdef __PX4_POSIX
	perf_counter_t _perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay"));
	perf_counter_t _perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay"));
	perf_counter_t _perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay"));
#endif

	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));

	_vision_sub = orb_subscribe(ORB_ID(vision_position_estimate));
	_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));

	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));

	_params_sub = orb_subscribe(ORB_ID(parameter_update));
	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));

函数开始订阅了一批数据，这些数据用于后面计算使用。

update_parameters(true);

上面函数用于讲attitude_estimator_q_params.c中的参数拷贝到进程中使用，当传入true时是强制拷贝，当传入false时是判断是否有变动，如有变动才会更新拷贝。

hrt_abstime last_time = 0;

	px4_pollfd_struct_t fds[1] = {};
	fds[0].fd = _sensors_sub;
	fds[0].events = POLLIN;

上面定义一个即将要阻塞等待的某个topic。fds[0].fd=阻塞等待的句柄，fds[0].events=阻塞等待方式。

	while (!_task_should_exit) {
		int ret = px4_poll(fds, 1, 1000);
		if (ret < 0) {
			// Poll error, sleep and try again
			usleep(10000);
			PX4_WARN("Q POLL ERROR");
			continue;
		} else if (ret == 0) {
			// Poll timeout, do nothing
			PX4_WARN("Q POLL TIMEOUT");
			continue;
		}
		update_parameters(false);
		// Update sensors
		sensor_combined_s sensors;

开始一个大循环，以1000毫秒阻塞等待句柄为_sensors_sub的数据，前面可以看到其消息ID为sensor_combined，如果返回值<0，说明出现错误，休息一会(usleep(10000);)继续(continue)阻塞等待这个这个数据；如果返回值=0，说明在等待1000毫秒之后依然没有拿到数据，继续(continue)阻塞等待，直到拿到数据为止。

下面的update_parameters(false);传入参数是false是检查更新拷贝参数。

定义一个sensors结构体用于存放消息ID为sensor_combined的数据，其中包含了姿态结算所必要的数据，acc、gyro、mag等。
可以到\Firmware-1.6.0rc1\msg\sensor_combined文件路径看一下sensor_combined里面都有什么数据。

if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
			// Feed validator with recent sensor data
			if (sensors.timestamp > 0) {
				// Filter gyro signal since it is not fildered in the drivers.
				_gyro(0) = _lp_gyro_x.apply(sensors.gyro_rad[0]);
				_gyro(1) = _lp_gyro_y.apply(sensors.gyro_rad[1]);
				_gyro(2) = _lp_gyro_z.apply(sensors.gyro_rad[2]);
			}
			if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
				// Filter accel signal since it is not fildered in the drivers.
				_accel(0) = _lp_accel_x.apply(sensors.accelerometer_m_s2[0]);
				_accel(1) = _lp_accel_y.apply(sensors.accelerometer_m_s2[1]);
				_accel(2) = _lp_accel_z.apply(sensors.accelerometer_m_s2[2]);
				if (_accel.length() < 0.01f) {
					PX4_DEBUG("WARNING: degenerate accel!");
					continue;
				}
			}
			if (sensors.magnetometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
				_mag(0) = sensors.magnetometer_ga[0];
				_mag(1) = sensors.magnetometer_ga[1];
				_mag(2) = sensors.magnetometer_ga[2];
				if (_mag.length() < 0.01f) {
					PX4_DEBUG("WARNING: degenerate mag!");
					continue;
				}
			}
			_data_good = true;
		

将消息ID为sensor_combined中的数据复制到sensors这个结构体中，再将其分别滤波后进行赋值。

后面是检测视觉、mocap、空速计的数据更新，用处不大。先略过。

bool gpos_updated;
		orb_check(_global_pos_sub, &gpos_updated);
		//磁偏角的处理
		if (gpos_updated) {
			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
				/* set magnetic declination automatically */
				update_mag_declination(math::radians(get_mag_declination(_gpos.lat, _gpos.lon)));
			}
		}
		//运动加速度补偿的处理
		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f && _inited) {
			/* position data is actual */
			if (gpos_updated) 
				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
				/* velocity updated */
				if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
					float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
					/* calculate acceleration in body frame */
					_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
				}
				_vel_prev_t = _gpos.timestamp;
				_vel_prev = vel;
			}
		} else {
			/* position data is outdated, reset acceleration */
			_pos_acc.zero();
			_vel_prev.zero();
			_vel_prev_t = 0;
		}

这一部分定义一个GPS更新标志位，检查更新GPS数据

if（是否通过当前数据获取磁偏角(_mag_decl_auto) && GPS精度在允许范围内 && GPS获取时间未超时）
就通过当前GPS数据获取当地磁偏角。
if（是否需要进行运动加速度的测量(_acc_comp)&&GPS数据是否有效&&GPS数据获取未超时&&GPS精度在允许范围内）
用GPS获取的速度计算出运动加速度
转换到机体系下（因为GPS测得的速度是导航系下的速度，用其微分出来的速度也是导航系下的，但是补偿的加速度是机体系下的，所以要对其转换到机体坐标系下使用）

		/* time from previous iteration */
		hrt_abstime now = hrt_absolute_time();
		float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.00001f;
		last_time = now;
		if (dt > _dt_max) {
			dt = _dt_max;
		}
		if (!update(dt)) {
			continue;
		}

通过高精度定时器获取积分时间dt，（update(dt)；）这个函数就是第二部分姿态结算。这里先不跟进，先把大体框架捋顺清楚。

Vector<3> euler = _q.to_euler();
		struct vehicle_attitude_s att = {};
		att.timestamp = sensors.timestamp;
		att.rollspeed = _rates(0);
		att.pitchspeed = _rates(1);
		att.yawspeed = _rates(2);
		memcpy(&att.q[0], _q.data, sizeof(att.q));
				/* the instance count is not used here */
		int att_inst;
		orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH);

通过上面的姿态结算（update(dt)；）得到的就是这个四元数（_q），通过四元数转换成欧拉角（_q.to_euler()），把数据进行填充，填充到att这个结构体中。并以
消息主题：vehicle_attitude、
消息句柄：_att_pub、
消息数据：att、
消息实体：att_inst、
消息优先级：ORB_PRIO_HIGH
发布数据。

			struct control_state_s ctrl_state = {};

			ctrl_state.timestamp = sensors.timestamp;

			/* attitude quaternions for control state */
			ctrl_state.q[0] = _q(0);
			ctrl_state.q[1] = _q(1);
			ctrl_state.q[2] = _q(2);
			ctrl_state.q[3] = _q(3);

			ctrl_state.x_acc = _accel(0);
			ctrl_state.y_acc = _accel(1);
			ctrl_state.z_acc = _accel(2);

			/* attitude rates for control state */
			ctrl_state.roll_rate = _rates(0);
			ctrl_state.pitch_rate = _rates(1);
			ctrl_state.yaw_rate = _rates(2);

			/* TODO get bias estimates from estimator */
			ctrl_state.roll_rate_bias = 0.0f;
			ctrl_state.pitch_rate_bias = 0.0f;
			ctrl_state.yaw_rate_bias = 0.0f;

			ctrl_state.airspeed_valid = false;

数据填充到ctrl_state结构体中，没啥好说的。后面就是对_airspeed_mode判断，就不粘过来了。

int ctrl_inst;
			/* publish to control state topic */
			orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH);
}//end task_main()

接着又把刚刚填充到control_state里面的数据进行发布。

好了，姿态估计的大体流程框架就是这样，下面进入最重要的姿态估计具体函数中（update(dt)；）。
/*************************************************************************************************************************************/
同样，将update(dt)拆开逐步分析。

bool AttitudeEstimatorQ::update(float dt)
{
	if (!_inited) {
		if (!_data_good) {
			return false;
		}
		return init();
	}

函数开始会判断是否第一次进入，如果第一次进入需要初始化四元数。跟进init()函数：

bool AttitudeEstimatorQ::init()
{
	// Rotation matrix can be easily constructed from acceleration and mag field vectors
	// 'k' is Earth Z axis (Down) unit vector in body frame
	Vector<3> k = -_accel;
	k.normalize();
	
	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
	Vector<3> i = (_mag - k * (_mag * k));
	i.normalize();
	
	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
	Vector<3> j = k % i;

	// Fill rotation matrix
	Matrix<3, 3> R;
	R.set_row(0, i);
	R.set_row(1, j);
	R.set_row(2, k);

	// Convert to quaternion
	_q.from_dcm(R);
	
	// Compensate for magnetic declination
	Quaternion decl_rotation;
	decl_rotation.from_yaw(_mag_decl);
	_q = decl_rotation * _q;

	_q.normalize();
	
	if (PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
	    PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)) &&
	    _q.length() > 0.95f && _q.length() < 1.05f) {
		_inited = true;
	} else {
		_inited = false;
	}
	return _inited;
}

i、j、k分别对应机体系下x、y、z三轴，仅用加速度计和磁力计生成姿态四元数矩阵 ：

k轴（z轴）：加速度计测量的是目标向量与 b 系三轴的夹角，假设加 速度计输出为 _accel = [𝑎1, 𝑎2, 𝑎3]𝑇，z 轴向上；加速度计能感应重力，重力向量指向地心，由于定义的坐标系为 NED，z 轴向下；k 为加速度计测量到的加速度方向向量，k = −[𝑎1, 𝑎2, 𝑎3]𝑇。再将其单位化。

i轴（x轴）：磁力计可以感应地理的北极。假设磁力计的输出为_mag = [𝑚1, 𝑚2, 𝑚3]𝑇。

为防止磁力计向量与加速度向量 k 不垂直，计算△ k=k·(_mag·k)=𝑚1a1^2 + 𝑚2a2^2 + 𝑚3a3^2为_mag 在 k 轴上 的投影，
通过 i = _mag - △k =[𝑚1 − 𝑚1a1^2, 𝑚2 − 𝑚2a2^2, 𝑚3 − 𝑚3a3^2]T，可以得到一个与 k 轴垂直的向量。并进行正交化后得到指向北的单位向量。i=[ i1, i2, i3]T。

j轴（y轴）：通过i轴和k轴的叉乘找到（j = k % i）。

构造旋转矩阵R，再通过旋转矩阵R变换成四元数（_q）。

下一步进行补偿磁力计偏移，前面用GPS获取的磁偏角（_mag_decl）， 设_mag_decl=[m‘1, m’2, m‘3 ]𝑇. 仅偏航后的四元数变化量：
q𝑑𝑒𝑙0 = cos(_mag_decl/2 ) 、 q𝑑𝑒𝑙1 = 0 、q𝑑𝑒𝑙2 = 0 、q𝑑𝑒𝑙3 = sin(_mag_decl/2 )。
_q = decl_rotation * _q （不懂为什么这么搞）

随后四元数更新并归一化得到初始四元数，回到update()函数.

Quaternion q_last = _q;
	// Angular rate of correction
	Vector<3> corr;
	float spinRate = _gyro.length();

定义一个误差变量，获取一下陀螺仪模长。

下面获取磁力计误差：

if (_ext_hdg_mode == 0  || !_ext_hdg_good) {
		// Magnetometer correction
		// Project mag field vector to global frame and extract XY component
		Vector<3> mag_earth = _q.conjugate(_mag);//b系到n系
		float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
		//wrap_pi函数是用于限定结 果-pi到pi的函数
		float gainMult = 1.0f;
		const float fifty_dps = 0.873f;
		if (spinRate > fifty_dps) {
			gainMult = fmin(spinRate / fifty_dps, 10.0f);
		}
		// Project magnetometer correction to body frame
		corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag * gainMult;
	}
	_q.normalize();

这里只粘贴了磁力计误差的获取，视觉mocap因为不大常用获取和通磁力计相同就没有再粘贴。
首先把磁力计的数据旋转回导航系下，然后计算出磁偏角，这个磁偏角和第一部分GPS获取的磁偏角应该是一样的，直接做差就可以得到误差，并限定到[-π π]之间。最后再将误差旋转到机体系下，乘上权重作为磁力计误差。

下面获取加速度计误差：

Vector<3> k(
		2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
		2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
		(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
	);
	corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;

结合上面代码和图，k就是重力向量在机体系下的表示，即为旋转矩阵乘以[0 0 1]后得到的。找到重力向量，还要找到加速度实际测量的重力值，但是加速度计测得的不只是重力加速度，也会包含运动加速度，所以下面一行的代码用加速度值减去了运动加速度值（_accel - _pos_acc），加速度计测量的重力加速度做归一化后，与重力向量做叉乘得到误差[ex ey ez]T，在乘以权重得到加速度误差。

两点说明：

1. 其中_pos_acc是从第一阶段subscribe订阅GPS数据，通过GPS速度算出来的运动加速度。
2. 因为叉乘是判断两个两项是否平行的，重力测得的重力加速度和旋转矩阵得到的重力加速度应该平行，通过叉乘判断是否平行，如果不平行说明有误差 ，所以用叉乘判断向量是都平行，是否存在误差

上面已经将加速度计误差和磁力计误差累积到一起了，接着往下看：

// Gyro bias estimation
	if (spinRate < 0.175f) {
		_gyro_bias += corr * (_w_gyro_bias * dt);
		for (int i = 0; i < 3; i++) {
			//限制幅度
			_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
		}
	}
	//角速度 = 陀螺仪的测量值 + 误差校准
	_rates = _gyro + _gyro_bias;
	// Feed forward gyro
	corr += _rates;
	// Apply correction to state//一阶龙格库塔法求解四元数
	_q += _q.derivative(corr) * dt;
	// Normalize quaternion
	_q.normalize();
	if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
	      PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
		// Reset quaternion to last good state
		_q = q_last;
		_rates.zero();
		_gyro_bias.zero();
		return false;
	}
	return true;
}//end update()

将上面的误差进行PI修正到陀螺偏移量中，得到准确的角速度值，用校正后的陀螺仪重新对四元数求解（一阶龙格库塔法求解四元数），归一化处理后输出四元数。

退出update()函数就是前面提到的用四元数获取欧拉角等数据，填充数据并发布。

/*************************************************************************************************************************************/
姿态估计到这里就结束了

总结：

1. 订阅需要相关的数据并处理
2. 获取初始四元数（利用加速度计测量值为z轴，磁力计测量校正后作为x轴，x轴z轴做叉乘为y轴）
3. 获取加速度值，磁力计值并作单位化处理
4. 从四元数中获得重力向量和磁场向量
5. 四元数获得的重力向量与加速度计获得的除去运动加速度的值做叉乘到误差a，四元数获得的磁场向量与GPS获得的磁偏角向量做差得到误差b
6. 将上述误差做PI修正陀螺仪中
7. 再将修正后的陀螺仪获得的角速度，用一阶龙格库塔法求解四元数
8. 用求解后的四元数转换成欧拉角等数据进行发布

以上是个人对PX4姿态估计的理解，如有地方不正确感谢提出批评指正，欢迎一起讨论 QQ：1103706199。