无人机姿态估计（互补滤波）
前言：多旋翼的姿态估计算法通常采用自适应显示
互补滤波算法、扩展卡尔曼滤波算法、梯度下降算法等，以上三种方法均是对姿态解算的实现，采用的基本思路都是利用陀螺仪的动态稳定性来估计实时姿态，同时由于陀螺仪随时间积分累计漂移误差的固有缺陷，需找一个不随时间变化影响的传感器来估计姿态并进行修正标定。三种方法各有优缺点，互补与梯度算法更适用于处理性能受限的飞行器，例如微型四轴等采用低频Cortex-M0或M1的处理器，而在条件允许性能足够的情况下，建议考虑采用扩展卡尔曼滤波的算法
PX4源码中采用的是改进的互补滤波算法，在原有加速度计校准陀螺仪的基础上，增加磁力计和GPS数据进行更进一步的四元数校准补偿，它的优点是运算简单，因此成为PX4中默认的姿态解算算法
参考文章：【算法基础学习 4】互补滤波算法——PX4姿态估计
PX4源码分析3：attitude_estimator_q
施密特正交化
固件版本：V1.11.3
名词解释
陀螺仪
陀螺仪，测量角速度，具有高动态特性，它是一个间接测量角度的器件。它测量的是角度的导数，即角速度，要将角速度对时间积分才能得到角度。由于噪声等误差影响，在积分作用下不断积累，最终导致陀螺仪的低频干扰和漂移。
加速度计
输出当前加速度（包含重力加速度 g）的方向【也叫重力感应器】，在悬停时，输出为 g。由其测量原理导致的高频信号敏感，使得加速度计在振动环境中高频干扰较大。
磁力计（又叫磁罗盘）
输出为当前机体与地磁场的夹角。测量原理与指南针相似。低频特性较好，易受周围磁场干扰。
磁力计的工作原理参考：http://blog.sina.com.cn/s/blog_402c071e0102v8ig.html
坐标系
- 导航坐标系：在多旋翼中，又叫地球坐标系、地理坐标系。通常，采用北东地（NED）构成坐标系的 X,Y,Z 轴。
- 机体坐标系 ：固联在多旋翼飞行器上，即，坐标系原点定位于飞行器中心点（假设中心点与重心点重合）。
关于航空导航用到的坐标系，请参考 AIAA 系列丛书。在多旋翼中，因为只在低空飞行，且时间较短，只需要以上两个。
姿态表示
- 欧拉角 ：较直观，描述刚体在三维欧式空间中的姿态。此时，坐标系为机体坐标系，随着刚体的旋转而旋转。缺点：万向节锁。
- 四元数：由一组四维向量，表示刚体的三维旋转。适合用于计算机计算。
- 方向余弦矩阵DCM：用
欧拉角余弦值或四元数，表示的坐标系的旋转。
滤波原理
互补滤波要求两个信号的干扰噪声处在不同的频率，通过设置两个滤波器的截止频率，确保融合后的信号能够覆盖需求频率。
在 IMU 的姿态估计中，互补滤波器对陀螺仪（低频噪声）使用高通滤波；对加速度/磁力计（高频噪声）使用低频滤波。
互补滤波中，两个滤波器的截止频率一致，此时就需要根据有用频率的值对其进行取舍。
机体水平时，加速度计无法测量绕 Z 轴的旋转量，即偏航角。磁力计也有同样问题，无法测得要磁轴的旋转量。故，需要加速度计和磁力计同时对陀螺仪进行校正。
无人机姿态估计部分
头文件部分代码
//调用无人机姿态以及参数部分的头文件
#include //高精度的定时器。 在控制过程中多数环节都是使用经典的PID控制算法为
//了获取较为实时的响应最重要的时间变量，这就涉及如何获取高精度的时间问题。pixhawk中就有高精度的RTC（实时时钟），这个
//头文件就做了介绍
#include using关键字
using matrix::Dcmf;
using matrix::Eulerf;
using matrix::Quatf;
using matrix::Vector3f;
using matrix::wrap_pi;
**using关键字：**表示引入名称到 using 说明出现的声明区域
类的定义
class AttitudeEstimatorQ : public ModuleBase<AttitudeEstimatorQ>, public ModuleParams, public px4::WorkItem
{
public:
AttitudeEstimatorQ();
~AttitudeEstimatorQ() override;
/** @see ModuleBase */
static int task_spawn(int argc, char *argv[]);
/** @see ModuleBase */
static int custom_command(int argc, char *argv[]);
/** @see ModuleBase */
static int print_usage(const char *reason = nullptr);
bool init();//初始化函数
private:
void Run() override;
void update_parameters(bool force = false);
bool init_attq();
bool update(float dt); //dt是更新周期，这个函数是整个程序的核心
// Update magnetic declination (in rads) immediately changing yaw rotation
//更新磁偏角( in rads )，立即改变偏航旋转
//偏航角改变立刻更新磁方位角
void update_mag_declination(float new_declination);
const float _eo_max_std_dev = 100.0f; /**< Maximum permissible standard deviation for estimated orientation */
//估计方位的最大允许标准差
const float _dt_min = 0.00001f;//dt是指更新数据的时间间隔，也就是离散pid公式中的T
const float _dt_max = 0.02f;
uORB::SubscriptionCallbackWorkItem _sensors_sub{
this, ORB_ID(sensor_combined)};
uORB::Subscription _parameter_update_sub{
ORB_ID(parameter_update)};
uORB::Subscription _gps_sub{
ORB_ID(vehicle_gps_position)};
uORB::Subscription _local_position_sub{
ORB_ID(vehicle_local_position)};
uORB::Subscription _vision_odom_sub{
ORB_ID(vehicle_visual_odometry)};
//在topicvehicle_odometry里面
uORB::Subscription _mocap_odom_sub{
ORB_ID(vehicle_mocap_odometry)};
uORB::Subscription _magnetometer_sub{
ORB_ID(vehicle_magnetometer)};
//无人机姿态数据发布
uORB::Publication<vehicle_attitude_s> _att_pub{
ORB_ID(vehicle_attitude)};
int _lockstep_component{
-1};
float _mag_decl{
0.0f};/**磁方位角**/
float _bias_max{
0.0f};/**最大偏差**/
Vector3f _gyro; //通过陀螺仪获取的三轴的角速度
Vector3f _accel; //通过加速度计获取的三轴的加速度
Vector3f _mag; //通过磁力计获取的磁航向
Vector3f _vision_hdg;//视觉位置估计;
Vector3f _mocap_hdg; //vicon姿态位置估计;
Quatf _q; //四元数
Vector3f _rates; //姿态角速度 与上面_gyro不同的是这个用于存放修正后的绕三轴角速度
Vector3f _gyro_bias; //陀螺仪偏差
Vector3f _vel_prev; //前一刻加速度
//绝对时间
hrt_abstime _vel_prev_t{
0};//前一时刻计算速度时的绝对时间 用于后面根据速度差除以时间差求加速度
Vector3f _pos_acc;//运动加速度，注意：这个加速度不包括垂直方向的
hrt_abstime _last_time{
0};
bool _inited{
false};//初始化标识;
bool _data_good{
false}; //数据可用;
bool _ext_hdg_good{
false};//外部航向可用;
//参数定义
/**前面带w都是权重的意思**/
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc, //加速度计权重 0.2f
(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag, //磁力计权重 0.1f
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg, //外部航向权重 0.1f
(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias, //陀螺仪偏差权重 0.1f
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl, //磁偏角（°） 0.0f
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a, //启用基于GPS的自动磁偏角校正 1
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m, //外部航向模式 0
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp, // 启用基于GPS速度的加速度补偿 1
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas, //陀螺仪偏差上限 0.05f
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag //振动水平报警阈值 0.2f
)
};
参数定义
//参数定义
/**前面带w都是权重的意思**/
DEFINE_PARAMETERS(
(ParamFloat<px4::params::ATT_W_ACC>) _param_att_w_acc, //加速度计权重 0.2f
(ParamFloat<px4::params::ATT_W_MAG>) _param_att_w_mag, //磁力计权重 0.1f
(ParamFloat<px4::params::ATT_W_EXT_HDG>) _param_att_w_ext_hdg, //外部航向权重 0.1f
(ParamFloat<px4::params::ATT_W_GYRO_BIAS>) _param_att_w_gyro_bias, //陀螺仪偏差权重 0.1f
(ParamFloat<px4::params::ATT_MAG_DECL>) _param_att_mag_decl, //磁偏角（°） 0.0f
(ParamInt<px4::params::ATT_MAG_DECL_A>) _param_att_mag_decl_a, //启用基于GPS的自动磁偏角校正 1
(ParamInt<px4::params::ATT_EXT_HDG_M>) _param_att_ext_hdg_m, //外部航向模式 0
(ParamInt<px4::params::ATT_ACC_COMP>) _param_att_acc_comp, // 启用基于GPS速度的加速度补偿 1
(ParamFloat<px4::params::ATT_BIAS_MAX>) _param_att_bias_mas, //陀螺仪偏差上限 0.05f
(ParamInt<px4::params::SYS_HAS_MAG>) _param_sys_has_mag //振动水平报警阈值 0.2f
)
初始化赋值
//数据的初始化
AttitudeEstimatorQ::AttitudeEstimatorQ() :
ModuleParams(nullptr),
WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers)
{
_vel_prev.zero();/**清零**/
_pos_acc.zero();
_gyro.zero();
_accel.zero();
_mag.zero();
_vision_hdg.zero();/**通过视觉得到的航向清零**/
_mocap_hdg.zero();/**通过mocap得到的航向清零**/
_q.zero();
_rates.zero();
_gyro_bias.zero();
update_parameters(true);
_lockstep_component = px4_lockstep_register_component();
}
析构函数
//析构函数，清除所有任务
AttitudeEstimatorQ::~AttitudeEstimatorQ()
{
px4_lockstep_unregister_component(_lockstep_component);
}
初始化
//初始化函数
bool
AttitudeEstimatorQ::init()
{
if (!_sensors_sub.registerCallback()) {
PX4_ERR("sensor combined callback registration failed!");
return false;
}
return true;
}
Run函数
void
AttitudeEstimatorQ::Run()
{
if (should_exit()) {
_sensors_sub.unregisterCallback();
exit_and_cleanup();
return;
}
sensor_combined_s sensors;
//如果参数更新，进入下面的函数
if (_sensors_sub.update(&sensors)) {
//参数更新
//根据此偏角更新四元数
update_parameters();
// 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];
//length函数为平方和开方
if (_accel.length() < 0.01f) {
PX4_ERR("degenerate accel!");
return;
}
}
// Update magnetometer 磁力计
//如果数据更新，就将数据进行赋值
//topic 里面定义变量只有magnetometer_ga[3]
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
//更新视觉和运动捕捉标题
_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
//视觉姿态数据的验证检查
// vision_att_valid 是bool类型的 主要进行判断，但是里面的数据表示什么意思？
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) {
//将基于视觉得到的四元数赋值给新建的math::q
//得到转换矩阵，将视觉的到的四元数转换为转换矩阵
Dcmf Rvis = Quatf(vision.q);
//这是一个指北向量 至于0.4f是啥？
Vector3f v(1.0f, 0.0f, 0.4f);
// Rvis is Rwr (robot respect to world) while v is respect to world.
// Rvis是Rwr (机器人对世界的关系)，v是对世界的关系。
//v是基于世界坐标 dcm是由b --> e需要进行转置
// Hence Rvis must be transposed having (Rwr)' * Vw
// 因此Rvis必须转置有( Rwr ) ' * Vw
// Rrw * Vw = vn. This way we have consistency
// Rrw * Vw = vn。这样我们就有了一致性
//transpose() 转置的意思
_vision_hdg = Rvis.transpose() * v;/**转置后与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_sample > 0 && (hrt_elapsed_time(&vision.timestamp_sample) < 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_sample > 0 && (hrt_elapsed_time(&mocap.timestamp_sample) < 500000);
}
}
}
}
if (_gps_sub.updated()) {
vehicle_gps_position_s gps;
if (_gps_sub.copy(&gps)) {
//磁力计
//首先检测是否配置了自动磁方位角获取，即用下面的if
if (_param_att_mag_decl_a.get() && (gps.eph < 20.0f)) {
/* set magnetic declination automatically */
// 自动设置磁偏角
//自动获取磁方位角，如果配置了则用当前的经纬度（longitude and latitude）
// 通过get_mag_declination(_gpos.lat,_gpos.lon)函数获取当前位置的磁偏角
update_mag_declination(get_mag_declination_radians(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 */
// 计算机体的加速度
// 计算e系下的运动加速度，这里的pos_acc很重要，要用于后面加速度计的校正
_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 */
// 没有更新就将数据重置，_vel_prev_t这个变量初始值为0，更新就将数据替换为lpos.timestamp，不更新就赋值为0
_pos_acc.zero();
_vel_prev.zero();
_vel_prev_t = 0;
}
}
}
/* time from previous iteration */
// 从上次迭代开始的时间
hrt_abstime now = hrt_absolute_time();
//条件表达式 自右向左
//下面的constrain函数用于限定作用，限定规则：return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);
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);
//最后发布给了vehicle_attitude,这也对应了最开始我们说的我们不能直接把陀螺仪的数据当成姿态信息，而应该
//经过我们处理后才能使用。即vehicle_attitude的信息流为：订阅的是sensor combined，发布的是vehicle attitude
}
px4_lockstep_progress(_lockstep_component);
}
}
参数更新
//查看参数是否更新，false查看参数是否更新，true获取参数，并由磁偏角更新四元数
void
AttitudeEstimatorQ::update_parameters(bool force)
{
// check for parameter updates
if (_parameter_update_sub.updated() || force) {
// clear update
/**建立建构体pupdate，里面有更新的时间、是否更新、填充信息**/
parameter_update_s pupdate;
_parameter_update_sub.copy(&pupdate);
// update parameters from storage
// 从存储中更新参数
updateParams();
// disable mag fusion if the system does not have a mag
// 如果系统没有磁块，则禁用磁块融合
if (_param_sys_has_mag.get() == 0) {
_param_att_w_mag.set(0.0f);
}
// if the weight is zero (=mag disabled), make sure the estimator initializes
// 如果权重为零( = mag disabled )，则确保估计器初始化
if (_param_att_w_mag.get() < FLT_EPSILON) {
_mag(0) = 1.f;
_mag(1) = 0.f;
_mag(2) = 0.f;
}
// math::radians 将角度转换为弧度
update_mag_declination(math::radians(_param_att_mag_decl.get()));
}
}
向量计算小知识
名称 标积/内积/数量积/点积 矢积/外积/向量积/叉积 运算式 a·b=|a||b|·cosθ a×b=c，其中|c|=|a||b|·sinθ，c的方向遵守右手定则 几何意义 向量a在向量b方向上的投影与向量b的模的乘积 c是垂直a、b所在平面，且以|b|·sinθ为高、|a|为底的平行四边形的面积 运算结果的区别 标量（常用于物理）/数量（常用于数学） 矢量（常用于物理）/向量（常用于数学）
**向量归一化：**一种是把数变为（0，1）之间的小数，一种是把有量纲表达式变为无量纲表达式。主要是为了数据处理方便提出来的，把数据映射到0～1范围之内处理，更加便捷快速。
eg：{2.5 3.5 0.5 1.5}归一化后变成了{0.3125 0.4375 0.0625 0.1875}
解：2.5+3.5+0.5+1.5=8，
2.5/8=0.3125，
3.5/8=0.4375，
0.5/8=0.0625，
1.5/8=0.1875.
这个归一化就是将括号里面的总和变成1，然后写出每个数的比例。
初始化姿态估计
/***********init_attq() 函数作用：由加速度计和磁力计初始化旋转矩阵和四元数**************/
bool
AttitudeEstimatorQ::init_attq()
{
// Rotation matrix can be easily constructed from acceleration and mag field vectors
// 旋转矩阵可以很容易地由加速度和磁场矢量构造
// 'k' is Earth Z axis (Down) unit vector in body frame
// ‘k’为体框架中的地球Z轴( Down )单位向量
//加速度传感器得到的三轴加速度 k是加速度的方向向量，在无人机平稳即无yaw时
Vector3f k = -_accel;
//_accel已经在Run()中订阅，是经过赋值后的
//k为加速度传感器测量到加速度方向向量，由于第一次测量数据时无人机一般为平稳状态无运动状态，
//所以可以直接将测到的加速度视为重力加速度g，以此作为dcm旋转矩阵的第三行k
//k是朝下的向量
k.normalize();//向量归一化，也就是向量除以他的长度
// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
// ‘i’为体框架中地球X轴(北)单位向量，与‘k’正交。
Vector3f i = (_mag - k * (_mag * k));
//施密特正交化解释如前言部分介绍
//施密特正交化,强制使i与k垂直;因向量k已归一化，故分母等于1;
//这里的_mag是指北的，所以下面求得的R是默认飞机机头也是指向北
//i是朝北地向量
i.normalize(); //向量归一化，也就是向量除以他的长度
// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
// ' j '是e坐标中地球Y轴(东方)单位向量，与' k '和' i '正交。
//j是朝东的向量
Vector3f j = k % i;
// Fill rotation matrix
Dcmf R; //3 X 3 的旋转矩阵
R.row(0) = i;//北 将i向量填入矩阵第一行
R.row(1) = j;//东 将j向量填入矩阵第二行
R.row(2) = k;//下 将k向量填入矩阵第三行，注意：从这里可知该旋转矩阵为b系到n系
// Convert to quaternion
// 将R矩阵转换为四元数
_q = R;
// Compensate for magnetic declination 补偿飞机的磁方位角，因为前面求得q是默认飞机指向北，但起飞时飞机不一定指向北
Quatf decl_rotation = Eulerf(0.0f, 0.0f, _mag_decl); /**将旋转矩阵仅仅通过yaw偏航的四元数表示**/
_q = _q * decl_rotation;
_q.normalize(); /**归一化**/ /**此时的q才是真正的初始的q**/
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;
}
更新、将四元数进行修正更新
//初始化仅进行一次
bool
AttitudeEstimatorQ::update(float dt)
{
if (!_inited) {
if (!_data_good) {
/**在前面Run()函数中如果传感器数据订阅正常，data_good就为true**/
return false;
}
return init_attq();
/**没有初始化（前提：传感器数据订阅正常）就初始化**/
//并且注意：init在一次飞行trampoline中只执行一次，
// 因为以后的四元数和转换矩阵由输出的角速度经过龙格库塔法求，而飞机在初始飞行时就需要通过init_attq求转换矩阵
}
/**前面的init_attq等都完成就继续往下执行**/
Quatf q_last = _q; /**注意：此时四元数_q已经有内容了，此处的q_last的作用为给q弄一个备份，可从最后一个if看出**/
// Angular rate of correction 修正角速率,下面的是重点
Vector3f corr;
//_gyro是上面Run()经过赋值得到的
float spinRate = _gyro.length(); /**定义一个变量：旋转速率，length函数为平方和开方**/
//首先判断是使用什么mode做修正的，比如vision、mocap、acc和mag，这里我们着重分析用acc和mag做修正，另外两个同理
if (_param_att_ext_hdg_m.get() > 0 && _ext_hdg_good) {
if (_param_att_ext_hdg_m.get() == 1) {
// Vision heading correction 对基于视觉的航向模式进行修正
// Project heading to global frame and extract XY component
Vector3f vision_hdg_earth = _q.conjugate(_vision_hdg); /**将b系转为e系**/
float vision_hdg_err = wrap_pi(atan2f(vision_hdg_earth(1), vision_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -vision_hdg_err)) * _param_att_w_ext_hdg.get();
}
if (_param_att_ext_hdg_m.get() == 2) {
// Mocap heading correction
// Project heading to global frame and extract XY component
Vector3f mocap_hdg_earth = _q.conjugate(_mocap_hdg);
float mocap_hdg_err = wrap_pi(atan2f(mocap_hdg_earth(1), mocap_hdg_earth(0)));
// Project correction to body frame
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mocap_hdg_err)) * _param_att_w_ext_hdg.get();
}
}
//磁力计校准 外部导航失效或不使用
//将磁力计读数从机体坐标系转换到导航坐标系
if (_param_att_ext_hdg_m.get() == 0 || !_ext_hdg_good) {
// Magnetometer correction
// Project mag field vector to global frame and extract XY component
// 将mag字段向量投影到全局框架并提取XY组件
Vector3f mag_earth = _q.conjugate(_mag); /**将b系转为e系**/ //公式如图-1
//只考虑Vector<3> mag_earth中的前两维的数据mag_earth(1)和mag_earth(0)（即x、y，忽略z轴上的偏移），通
//过arctan（mag_earth(1),mag_earth(0)）得到的角度和前面根据经纬度获取的磁方位角做差值得到误差角度mag_err
//wrap_pi函数是用于限定结果-pi到pi的函数，大于pi则与2pi做差取补，小于-pi则与2pi做和取补
//注意1：这里在修正磁力计时用到的是有gps校准时的修正办法，即下面的减去自动获取的磁偏角。没有gps的校准方法见ppt(我找找)
//注意2：这里校正的方法是用磁力计计算的磁偏角与gps得出来的做差，然后转换到b系。
float mag_err = wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
//atan2f: 2 表示两个输入参数; 支持四象限内角度换算; 输出弧度值;
//将磁力计的读数由向量换算到角度; 该角度减去磁偏，得到磁场偏差;
//_mag_decl 由GPS得到;
float gainMult = 1.0f;
const float fifty_dps = 0.873f;
//float spinRate = _gyro.length(); /**定义一个变量：旋转速率，length函数为平方和开方**/
if (spinRate > fifty_dps) {
gainMult = math::min(spinRate / fifty_dps, 10.0f);
}
// Project magnetometer correction to body frame
//下面*Vector<3>(0.0f, 0.0f, -mag_err))是因为raw本质上应该由磁力计产生的水平
// 磁向量与gps产生的水平磁向量叉乘得到，而叉乘得到的正好向下
//将磁场偏差转换到机体坐标系，并乘以其对应权重;
//图2
corr += _q.conjugate_inversed(Vector3f(0.0f, 0.0f, -mag_err)) * _param_att_w_mag.get() * gainMult;
}
_q.normalize();//这里的归一化用于校正update_mag_declination后的偏差。
// Accelerometer correction 加速度计修正
// Project 'k' unit vector of earth frame to body frame 将k从地坐标系投影到机体坐标系
// Vector3f k = _q.conjugate_inversed(Vector3f(0.0f, 0.0f, 1.0f)); //从n系到b系
// Optimized version with dropped zeros
//下面的k是n系中重力的单位向量转换到b系中，即左乘旋转矩阵得到的
Vector3f 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))
);
//总的受到合力的方向（_accel）减去机体加速度方向（_pos_acc）得到g的方向，即总加速度（加速度获取）减去机体运动加速度获取重力加速度
//重力加速度的方向是导航坐标系下的z轴，加上运动加速度之后，总加速度的方向就不是与导航坐标系的天或地平行了，所以要消除这个误差，即“_accel-_pos_acc”
//这里与k差乘得到的是与重力方向的夹角sinx，约等于x，即roll和pitch
// If we are not using acceleration compensation based on GPS velocity, 如果我们不使用基于GPS速度的加速度补偿，
// fuse accel data only if its norm is close to 1 g (reduces drift). 只有当其范数接近1 g (减小漂移)时，才对acel数据进行融合。
const float accel_norm_sq = _accel.norm_squared();
const float upper_accel_limit = CONSTANTS_ONE_G * 1.1f;
const float lower_accel_limit = CONSTANTS_ONE_G * 0.9f;
if (_param_att_acc_comp.get() || ((accel_norm_sq > lower_accel_limit * lower_accel_limit) &&
(accel_norm_sq < upper_accel_limit * upper_accel_limit))) {
// %是叉乘的意思
//向量 k 是重力加速度向量(0,0,1)转换到机体坐标系;
//_accel 是加速度计的读数;
//_pos_acc 是由位置估计(GPS) 微分得到的加速度;
//_accel - _pos_acc 表示飞行器加速度向量减去其水平分量;
//k 与 (_accel - _pos_acc)叉乘，得到偏差;
//公式看图-3
//下面是另一种理解
//总的受到合力的方向（_accel）减去机体加速度方向（_pos_acc）得到g的方向，即总加速度（加速度获取）减去机体运动加速度获取重力加速度，然后姿态矩阵的不是行就是列来与纯重力加速度来做叉积，算出误差。因为运动加速度是有害的干扰，必须减掉。算法的理论基础是[0,0,1]与姿态矩阵相乘。
//该差值获取的重力加速度的方向是导航坐标系下的z轴，加上运动加速度之后，总加速度的方向就不是与导航坐标系的天或地平行了，所以要消除这个误差，即“_accel-_pos_acc”。然后叉乘z轴向量得到误差，进行校准 。
corr += (k % (_accel - _pos_acc).normalized()) * _param_att_w_acc.get();
}
// Gyro bias estimation 陀螺偏差
//spinRate 旋转速率
if (spinRate < 0.175f) {
//得到陀螺仪修正值
//此处修正值为 mahony 滤波的 pi 控制器的 积分部分;
//因为 _gyro_bias 不归零，故不断累积;
//公式见图-4
_gyro_bias += corr * (_param_att_w_gyro_bias.get() * dt); //对应积分控制
//对_gyro_bias做约束处理。
for (int i = 0; i < 3; i++) {
_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
}
}
_rates = _gyro + _gyro_bias; //角速度 = 陀螺仪的测量值 + 误差校准 图5
// Feed forward gyro
// pi控制器的体现，对应比例部分
corr += _rates; //corr为update函数中定义的变量，所以每次进入update()函数中时会刷新corr变量的数据
//这里的 corr = (Ea+Em) + (Ea+Em)*dt + gyro;
//个人认为这里的 corr 才是更新后的角速度;
//参看公式如图6
// Apply correction to state
//最后就是使用修正的数据更新四元数，并把_rates和_gyro_bias置零便于下次调用时使用。
_q += _q.derivative1(corr) * dt;//用龙格库塔法更新四元数
//更新四元数，derivative为求导函数，使用一阶龙格库塔求导。 公式参看图-6
// Normalize quaternion
_q.normalize();
//四元数不合适就将其重置
if (!(PX4_ISFINITE(_q(0)) && PX4_ISFINITE(_q(1)) &&
PX4_ISFINITE(_q(2)) && PX4_ISFINITE(_q(3)))) {
// 如果数据不合适就恢复备份的q，即q_last
// Reset quaternion to last good state
// 将四元数重置为最后一个好状态
_q = q_last;
_rates.zero();
_gyro_bias.zero();
return false;
}
return true;
}
//更新完四元数后，我们跳到update函数被调用的地方，即422行，发现后面将更新后的姿态信息发布出去了，到此整个过程结束
// 调用函数为
// if (update(dt)) {
//}
以上部分公式如下：
更新偏航角
/**偏航角改变立刻更新磁方位角**/
void
AttitudeEstimatorQ::update_mag_declination(float new_declination)
{
// Apply initial declination or trivial rotations without changing estimation
// 忽略微小旋转（比如机体的微小震动），继续沿用之前的磁方位角
if (!_inited || fabsf(new_declination - _mag_decl) < 0.0001f) {
_mag_decl = new_declination;
} else {
//转动较大时，修正姿态（q）和更新磁方位角
// Immediately rotate current estimation to avoid gyro bias growth
Quatf decl_rotation = Eulerf(0.0f, 0.0f, new_declination - _mag_decl);
// 偏航角生成的四元数，生成方法是令另外2个角度为0,使用欧拉角转四元数公式
_q = _q * decl_rotation;
//此处两个四元数相乘表示角度相加，因为在数学上q1*q2表示q1q2这个合成动作，所以此处修正了四元数
_mag_decl = new_declination;
//使用新的磁偏角
}
}
一些配置函数
int
AttitudeEstimatorQ::custom_command(int argc, char *argv[])
{
return print_usage("unknown command");
}
int
AttitudeEstimatorQ::task_spawn(int argc, char *argv[])
{
AttitudeEstimatorQ *instance = new AttitudeEstimatorQ();
if (instance) {
_object.store(instance);
_task_id = task_id_is_work_queue;
if (instance->init()) {
return PX4_OK;
}
} else {
PX4_ERR("alloc failed");
}
delete instance;
_object.store(nullptr);
_task_id = -1;
return PX4_ERROR;
}
int
AttitudeEstimatorQ::print_usage(const char *reason)
{
if (reason) {
PX4_WARN("%s\n", reason);
}
PRINT_MODULE_DESCRIPTION(
R"DESCR_STR(
### Description
Attitude estimator q.
)DESCR_STR");
PRINT_MODULE_USAGE_NAME("AttitudeEstimatorQ", "estimator");
PRINT_MODULE_USAGE_COMMAND("start");
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
return 0;
}
extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[])
{
return AttitudeEstimatorQ::main(argc, argv);
}
后续继续补充…