三轴磁力计解算姿态（四元数）

原理

根据地磁场向量在水平面上的投影来计算载体的偏航角，类似于加速度计解算姿态，不同在于磁场易受干扰，且只能得到偏航角。

方法

假设导航坐标系为东北天，载体坐标系为右前上。

初始载体坐标系和导航坐标系重合，对应的四元数为q=[1,0,0,0]，使用此四元数表示载体在导航坐标系下的旋转。

总体步骤参考三轴加速度计解算姿态（四元数），主要根据磁力计测得的磁场向量在水平方向上的投影与磁北向量的夹角来旋转载体。

当载体水平放置时，磁力计磁场向量在水平方向上的投影很好计算，直接取x,y分量即可。但是当载体不处于水平时，需要将测量的向量先转换到导航坐标系，然后再取其在水平面上的投影，这样磁力计解算可以不受载体姿态影响。

以下是Eigen3的示例代码，先根据加速度计的数据得到载体坐标系到导航坐标系的旋转矩阵，然后再对磁力计的数据进行旋转，将其旋转至导航坐标系，最后取水平面上的投影来计算偏航角。

#include "Eigen/Core"
#include "Eigen/Geometry"
#include <cmath>// 初始四元数
Eigen::Quaternionf quaternion = Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f);while(1) {// 读取加速度计数据，单位mgfloat acc[3];read_acc(acc);// 加速度计测量的加速度向量，单位转为gEigen::Vector3f acceleration = Eigen::Vector3f(acc[0] / 1000.0f, acc[1] / 1000.0f, acc[2] / 1000.0f).normalized();// 导航坐标系下的标准重力向量Eigen::Vector3f gravity = Eigen::Vector3f(0.0f, 0.0f, 1.0f);// 测量的加速度从载体系转换到导航系acceleration = (quaternion.toRotationMatrix() * acceleration).normalized();// 计算旋转轴Eigen::Vector3f rotate_vector = acceleration.cross(gravity);// 计算旋转角度float theta = acos(acceleration.dot(gravity));// 得到旋转对应的四元数Eigen::Vector3f v = rotate_vector.normalized() * std::sin(theta / 2.0f);Eigen::Quaternionf q = Eigen::Quaternionf(std::cos(theta / 2.0f), v.x(), v.y(), v.z()).normalized();// 应用旋转quaternion = (q * quaternion).normalized();// 磁力计数据，任意单位float mag[3];read_mag(mag);// 磁北方向，导航坐标系（东北天）Eigen::Vector3f north = Eigen::Vector3f(0, 1, 0);// 磁力计测量的磁场向量，载体坐标系（右前上）Eigen::Vector3f magnetometer = Eigen::Vector3f(mag[0], mag[1], mag[2]);// 转换到导航坐标系magnetometer = quaternion.toRotationMatrix() * magnetometer;// xy平面投影magnetometer.z() = 0;magnetometer.normalize();// 计算旋转轴rotate_vector = magnetometer.cross(north);// 计算旋转角度theta = acos(magnetometer.dot(north));// 获取旋转四元数v = rotate_vector.normalized() * std::sin(theta / 2.0f);q = Eigen::Quaternionf(std::cos(theta / 2.0f), v.x(), v.y(), v.z()).normalized();// 应用旋转quaternion = (q * quaternion).normalized();
}

三轴磁力计解算姿态（四元数）相关推荐

三轴加速度计解算姿态（四元数）
原理当传感器载体静止时,加速度计只会输出重力加速度,可以凭此来计算载体的俯仰角和滚转角. 方法假设导航坐标系为东北天,载体坐标系为右前上. 初始载体坐标系和导航坐标系重合,对应的四元数为q=[1, ...
无人机姿态解算：四元数及其与欧拉角的转换
无人机姿态解算:四元数及其与欧拉角的转换引言:获得无人机飞行时的飞行姿态对于无人机稳定控制来说至关重要.无人机主要通过传感器数据融合来进行状态估计,常用于无人机的传感器包括:MPU(包含了三轴加速度 ...
四元数解算姿态角解析
本文来自:链接一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解算法,下面我们先来看看几种求解姿态角的算法: 1. 欧拉角法: 欧拉 ...
关于无人机四元数解算姿态角解析你知道吗？
原文链接http://www.elecfans.com/d/705815.html 一.概述无人机求解姿态角有多种算法,但由于各种算法的自身限制及计算机计算速度的限制,所以我们需要选择一个较佳的求解 ...
三轴加速度计、三轴陀螺仪、三轴磁力计
1.今天要做的事情列出:可能影响因素,对影响因素做实验.如何设置对比实验? 如何设置对比实验? 三轴加速度计 1.加速度计的原理三轴加速度:输出的速度是去掉重力后的整体加速度,因此,三轴加速度计可 ...
无人机中的IMU单元（MEMS 三轴加速计、三轴陀螺仪、三轴磁力计）
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达作者丨autotian@CSDN 编辑丨古月居三轴加速度计 (1)测量比力三轴加速度计是一种惯性传 ...
全球与中国三轴磁力计市场深度研究分析报告
[报告篇幅]:88 [报告图表数]:130 [报告出版时间]:2022年2月报告摘要 2021年全球三轴磁力计市场销售额达到了亿美元,预计2028年将达到亿美元,年复合增长率(CAGR)为 %( ...
对四元数解算姿态的理解
问题:为什么不用欧拉角来表示旋转而要引入四元数呢??? 前面介绍了什么是欧拉角,而且欧拉角微分方程解算姿态关系简单明了, 概念直观容易理解,那么我们为什么不用欧拉角来表示旋转而要引入四元数呢? 一方面 ...
树莓派pico mpu6050 一阶互补滤波四元数法解算姿态角
micro-python:一阶互补滤波&四元数法代码 2.系统方案 2.1.组成本系统由供电部分, 主控部分, 姿态传感器与通信部份组成 2.2.供电部分电池为一节14500锂电池, 容 ...

三轴磁力计解算姿态（四元数）

原理

方法

三轴磁力计解算姿态（四元数）相关推荐

最新文章

热门文章