一、前言
上一节介绍了PX4姿态估计调用函数的流程，这一节分享一下我对PX4姿态解算的解读.首先，要理解PX4姿态解算的程序，要先从传感器的特性入手，这里主要介绍的传感器有加速度计，磁力计，陀螺仪.
二、传感器特性
1.加速度计
pixhawk上使用的为三轴加速度计，主要用于测量x,y,z三轴的加速度值，常用的传感器例如mpu6000与mpu9250，在进行PX4二次开发时，我们并不需要编写加速度计相关驱动程序，其代码已经在PX4_Firmware/src/drivers/imu下进行了实现，感兴趣可以自己查阅.这里我们只需了解加速度计的原理与特性.
如图所示，该图为加速度计简易模型，由弹簧与质量块构成，当外界有加速度时，质量块位置会发生变化，从而使得电容两端距离改变，流经电容的电流也会发生变化，通过测量电流大小，就可以得到质量块移动距离，从而得到加速度大小。理想状态下，当没有外界作用时，质量块会处于零点位置，但由于工艺，使用时间长短等各种因素，质量块可能会处于非零点位置，即所谓零偏，因此，在飞无人机之前往往需要进行校准。
此外，加速度计校准还涉及到另一个参数，这个参数是标度因数，在这里可以理解为一个比例系数，测量值乘以这个比例系数后得到实际值。
从加速度原理可以看出，在测量加速度时质量块需要不断移动，移动需要时间，因此，在高频情况下加速度计值不一定准确，低频率特性比较好.
2.陀螺仪
pixhawk上另一个比较重要的传感器就是陀螺仪，陀螺仪用于测量x,y,z三轴角速度，其基本的原理是动量守恒。当外部系统发生旋转时，内部转动装置会保持恒定的速度与方向旋转，测量这两个系统的差就可以得到当前系统角速度。为了测量x,y,z三轴角速度，通常采用万向节构成转动装置
以其中一个方向为例，当陀螺仪测量装置随着转动轴转动时，在半径方向上会存在力，使得质量块在半径方向进行周期往复运动，从而得到旋转角速度.但由于存在零点漂移，陀螺仪在低频特性较差，高频特性较好.
3.磁力计
磁力计主要用于测量当前磁场强度。为了能够测量地磁方向，通常将地磁分为水平与垂直方向，水平方向可以近似表示地磁方向。但地球的磁轴与地轴有着夹角，一般称为磁偏角，磁偏角在不同地理位置不同，因此在无人机航向计算时，需要gps获得当前经纬度，然后查表得到当前位置磁偏角，对航向进行修正（后续代码中会看到）
三、姿态解算算法
1.为什么采用四元数计算
在介绍姿态解算算法前，先来谈谈姿态的表示方式，常用的姿态表示方法有：四元数，欧拉角，方向余弦这几种方式，并且这几种方式是可以 互相转换，欧拉角虽然计算简单，但是存在退化现象；方向余弦有9个参数，导致其计算量过大，实时性不好；因此，PX4源码中采用四元数来表示姿态。
2.姿态解算的坐标系
在无人机的坐标变换过程中，一般会涉及到以下四个坐标系
1.传感器坐标系
传感器本身具有自己的测量坐标系，在安装过程中会存在安装误差，而传感器读数是在传感器坐标系下测得，为了能让无人机使用，需要将其转换到机体坐标系。但加速度计，磁力计，陀螺仪这些传感器安装时一般与无人机机体中心位置与方向都重合，所以可以粗略认为传感器坐标系与无人机坐标系重合
2.机体坐标系
一般以无人机几何中心为中心，以右手定则建立的三维直角坐标系，x轴位于无人机机体平面，以无人机前进方向为正方向；y轴在机体平面且垂直x轴，z轴垂直机体平面，以向下为正.
3.本地坐标系（local）
为了确定无人机相对于地面的速度与位置，需要引入本地坐标系（仿真中常用的地面坐标系）。一般情况下，本地坐标系是以无人机起点为坐标中心，在水平面上正北方向为x轴，正东为y轴方向，z轴垂直地面向下，这也是所谓的北东地（NED）坐标系
4.全局坐标系（global）
前面提到，在不同经纬度下，地轴与磁轴之间的磁偏角是不一样的，为了修正无人机航向，还需要引入GPS测得的地球经纬度.
3.Mahony滤波算法
下面来讲解算法，如图为mahony滤波的流程图，取自文献[1]
先来解释一下上图：
1.
a
,
m
a,m
a,m与
ω
\omega
ω分别是加速度计测得的加速度，磁力计测得的磁场强度以及陀螺仪测得的角速度。其实这里就引出了一个问题，为什么需要加速度计与磁力计，光靠陀螺仪不就可以得到无人机姿态吗？
确实，光靠陀螺仪是可以得到无人机姿态的，通过对得到的角速度积分就得到了姿态，但靠积分得到的姿态会存在积分误差，这个光靠陀螺仪是无法解决的，因此引入了加速度计与磁力计来解决陀螺仪积分误差。
2.四元数微分方程为
Q
˙
=
1
2
⊗
ω
n
b
b
\dot{Q} =\frac{1}{2} \otimes \omega _{nb}^{b}
Q˙=21⊗ωnbb,式中
Q
Q
Q为姿态四元数，
ω
n
b
b
\omega _{nb}^{b}
ωnbb为无人机机体坐标系b相对于北东地（NED）坐标系n的角速度，这个式子是姿态解算的核心
3.由加速度计与磁力计得来的姿态同样存在误差，因此需要PI控制器来对误差进行修正，PI控制器的输入是由加速度计与磁力计算出的姿态与最终通过四元数微分方程计算出的实际姿态的差值，输出角速度修正值，补偿到陀螺仪，抵消陀螺仪误差
4.这个过程在无人机运行过程中循环计算，不断进行姿态解算
四、姿态解算算法代码讲解
在PX4代码解析(5)中我已经介绍了代码运行流程，本节只针对姿态解算算法重点部分进行讲解，我将这部分代码分为以下几个部分：
1.AttitudeEstimatorQ对象建立以及相关数据获取
2.四元数q的初始化
3.姿态解算
先来看看.AttitudeEstimatorQ对象的构造函数
//在对象的构造函数中，主要是对对象成员变量清0
AttitudeEstimatorQ::AttitudeEstimatorQ() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::att_pos_ctrl)
{
_vel_prev.zero();
_pos_acc.zero();
//gyro是陀螺仪数据，是一个1*3或者3*1向量
_gyro.zero();
//accel表示的是加速度计数据
_accel.zero();
//mag是磁力计数据
_mag.zero();
//vision是视觉数据，mocap是动作捕捉的数据，暂时用不到
_vision_hdg.zero();
_mocap_hdg.zero();
//四元数q清0
_q.zero();
_rates.zero();
_gyro_bias.zero();
//将参数文件中参数拷贝到当前进程使用，这些参数就是你在地面站里可以修改的那些，比如加速度计的偏移等数据
update_parameters(true);
}
在执行完构造函数后，该进程会执行run函数
//该函数主要是读取各传感器数据，并分配给相应变量，为后续姿态解算做前置工作
void
AttitudeEstimatorQ::Run()
{
//第一个if主要用于判断传感器那边数据准备好没有
if (should_exit()) {
_sensors_sub.unregisterCallback();
exit_and_cleanup();
return;
}
//定义了一个sensor_combine的结构体，用于接收数据
sensor_combined_s sensors;
//将数据拷贝到sensors
if (_sensors_sub.update(&sensors)) {
update_parameters();//默认参数为force
//检查数据是否为最新陀螺仪
// Feed validator with recent sensor data
if (sensors.timestamp > 0) {
_gyro(0) = sensors.gyro_rad[0];
_gyro(1) = sensors.gyro_rad[1];
_gyro(2) = sensors.gyro_rad[2];
}
//更新加速度计数据
if (sensors.accelerometer_timestamp_relative != sensor_combined_s::RELATIVE_TIMESTAMP_INVALID) {
_accel(0) = sensors.accelerometer_m_s2[0];
_accel(1) = sensors.accelerometer_m_s2[1];
_accel(2) = sensors.accelerometer_m_s2[2];
if (_accel.length() < 0.01f) {
PX4_ERR("degenerate accel!");
return;
}
}
// U更新磁力计数据
if (_magnetometer_sub.updated()) {
vehicle_magnetometer_s magnetometer;
if (_magnetometer_sub.copy(&magnetometer)) {
_mag(0) = magnetometer.magnetometer_ga[0];
_mag(1) = magnetometer.magnetometer_ga[1];
_mag(2) = magnetometer.magnetometer_ga[2];
//如果磁力计数据太小，则返回报错
if (_mag.length() < 0.01f) {
PX4_ERR("degenerate mag!");
return;
}
}
}
//数据更新完成标志位
_data_good = true;
// Update vision and motion capture heading
//是否有视觉或者动作捕捉，false不使用
_ext_hdg_good = false;
if (_vision_odom_sub.updated()) {
vehicle_odometry_s vision;
if (_vision_odom_sub.copy(&vision)) {
// validation check for vision attitude data
bool vision_att_valid = PX4_ISFINITE(vision.q[0])
&& (PX4_ISFINITE(vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
vision.pose_covariance[vision.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(vision.pose_covariance[vision.COVARIANCE_MATRIX_PITCH_VARIANCE],
vision.pose_covariance[vision.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (vision_att_valid) {
Dcmf Rvis = Quatf(vision.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Hence Rvis must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_vision_hdg = Rvis.transpose() * v;
// vision external heading usage (ATT_EXT_HDG_M 1)
if (_param_att_ext_hdg_m.get() == 1) {
// Check for timeouts on data
_ext_hdg_good = vision.timestamp > 0 && (hrt_elapsed_time(&vision.timestamp) < 500000);
}
}
}
}
//动捕
if (_mocap_odom_sub.updated()) {
vehicle_odometry_s mocap;
if (_mocap_odom_sub.copy(&mocap)) {
// validation check for mocap attitude data
bool mocap_att_valid = PX4_ISFINITE(mocap.q[0])
&& (PX4_ISFINITE(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE]) ? sqrtf(fmaxf(
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_ROLL_VARIANCE],
fmaxf(mocap.pose_covariance[mocap.COVARIANCE_MATRIX_PITCH_VARIANCE],
mocap.pose_covariance[mocap.COVARIANCE_MATRIX_YAW_VARIANCE]))) <= _eo_max_std_dev : true);
if (mocap_att_valid) {
Dcmf Rmoc = Quatf(mocap.q);
Vector3f v(1.0f, 0.0f, 0.4f);
// Rmoc is Rwr (robot respect to world) while v is respect to world.
// Hence Rmoc must be transposed having (Rwr)' * Vw
// Rrw * Vw = vn. This way we have consistency
_mocap_hdg = Rmoc.transpose() * v;
// Motion Capture external heading usage (ATT_EXT_HDG_M 2)
if (_param_att_ext_hdg_m.get() == 2) {
// Check for timeouts on data
_ext_hdg_good = mocap.timestamp > 0 && (hrt_elapsed_time(&mocap.timestamp) < 500000);
}
}
}
}
//如果gps数据更新
if (_gps_sub.updated()) {
vehicle_gps_position_s gps;//定义结构体，该结构体在msg文件夹中
if (_gps_sub.copy(&gps)) {
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
//通过gps得到的经纬度查表，得到磁偏角，去修正磁力计数据
update_mag_declination(math::radians(get_mag_declination(gps.lat, gps.lon)));
}
}
}
//定义在惯性系坐标系下的位置？
if (_local_position_sub.updated()) {
vehicle_local_position_s lpos;
if (_local_position_sub.copy(&lpos)) {
//是否进行运动加速度补偿，加速度计数据比较好
if (_param_att_acc_comp.get() && (hrt_elapsed_time(&lpos.timestamp) < 20_ms)
&& lpos.v_xy_valid && lpos.v_z_valid && (lpos.eph < 5.0f) && _inited) {
/* position data is actual */
const Vector3f vel(lpos.vx, lpos.vy, lpos.vz);
//更新速度
/* velocity updated */
if (_vel_prev_t != 0 && lpos.timestamp != _vel_prev_t) {
float vel_dt = (lpos.timestamp - _vel_prev_t) / 1e6f;
/* calculate acceleration in body frame *///补偿加速度计
_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
}
_vel_prev_t = lpos.timestamp;
_vel_prev = vel;
} else {//数据过期，重置加速度
/* position data is outdated, reset acceleration */
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
}
}
//以上是获取数据，对数据简单处理
//上一次迭代时间
/* time from previous iteration */
hrt_abstime now = hrt_absolute_time();//高精度定时器，得到当前时间
const float dt = math::constrain((now - _last_time) / 1e6f, _dt_min, _dt_max);
_last_time = now;
if (update(dt)) {//姿态解算
vehicle_attitude_s att = {};//
att.timestamp = sensors.timestamp;
_q.copyTo(att.q);
/* the instance count is not used here */
_att_pub.publish(att);//姿态发布
#if defined(ENABLE_LOCKSTEP_SCHEDULER)
if (_param_est_group.get() == 3) {
// In this case the estimator_q is running without LPE and needs
// to publish ekf2_timestamps because SITL lockstep requires it.
ekf2_timestamps_s ekf2_timestamps;
ekf2_timestamps.timestamp = now;
ekf2_timestamps.airspeed_timestamp_rel = 0;
ekf2_timestamps.distance_sensor_timestamp_rel = 0;
ekf2_timestamps.optical_flow_timestamp_rel = 0;
ekf2_timestamps.vehicle_air_data_timestamp_rel = 0;
ekf2_timestamps.vehicle_magnetometer_timestamp_rel = 0;
ekf2_timestamps.visual_odometry_timestamp_rel = 0;
_ekf2_timestamps_pub.publish(ekf2_timestamps);
}
#endif
}
}
}
代码中，我对其进行了注释，下面有几个需要强调的点
五、参考文献及博客
[1]储开斌, 赵爽, 冯成涛. 基于Mahony-EKF的无人机姿态解算算法[J]. 电子测量与仪器学报, 2020, 32(12):7.
[2]Valenti, Roberto G , Dryanovski, et al. Sensors, Vol. 15, Pages 19302-19330: Keeping a Good Attitude: A Quaternion-Based Orientation Filter for IMUs and MARGs. 2015.
[3]米刚, 田增山, 金悦,等. 基于MIMU和磁力计的姿态更新算法研究[J]. 传感技术学报, 2015, 28(1):6.
[4]姿态估计（互补滤波）
