三轴加速度计解算姿态(四元数)

这篇具有很好参考价值的文章主要介绍了三轴加速度计解算姿态(四元数)。希望对大家有所帮助。如果存在错误或未考虑完全的地方,请大家不吝赐教,您也可以点击"举报违法"按钮提交疑问。

原理

当传感器载体静止时,加速度计只会输出重力加速度,可以凭此来计算载体的俯仰角和滚转角。

方法

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

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

先将四元数转为旋转矩阵
C = [ 1 − 2 y 2 − 2 z 2 2 x y − 2 w z 2 w y + 2 x z 2 x y + 2 w z 1 − 2 x 2 − 2 z 2 2 y z − 2 w x 2 x z − 2 w y 2 w x + 2 y z 1 − 2 x 2 − 2 y 2 ] C=\begin{bmatrix} 1-2y^{2}-2z^{2} & 2xy-2wz & 2wy+2xz \\ 2xy+2wz & 1-2x^{2}-2z^{2} & 2yz-2wx \\ 2xz-2wy & 2wx+2yz & 1-2x^{2}-2y^{2} \end{bmatrix} C= 12y22z22xy+2wz2xz2wy2xy2wz12x22z22wx+2yz2wy+2xz2yz2wx12x22y2
可以使用此旋转矩阵对载体在导航坐标系下进行旋转,旋转后的载体坐标系和导航坐标系,对于一相同向量有如下关系,推导参考3维旋转矩阵推导与助记。

[ x ′ y ′ z ′ ] = C − 1 × [ x y z ] \begin{bmatrix} x^{'} \\ y^{'} \\ z^{'} \end{bmatrix} =C^{-1}\times \begin{bmatrix} x \\ y \\ z \end{bmatrix} xyz =C1× xyz

可以使用此公式将导航坐标系中的向量映射到载体坐标系。 C − 1 C^{-1} C1为旋转矩阵的逆矩阵,同样也是转置矩阵 C T C^{T} CT

加速度计测量的重力向量为

[ a x b a y b a z b ] \begin{bmatrix} a_{xb}\\ a_{yb}\\ a_{zb} \end{bmatrix} axbaybazb

需要将其转换到导航坐标系下,根据前面的旋转坐标系的公式可以得到

[ a x b a y b a z b ] = C − 1 × [ a x n a y n a z n ] \begin{bmatrix} a_{xb} \\ a_{yb} \\ a_{zb} \end{bmatrix} =C^{-1}\times \begin{bmatrix} a_{xn} \\ a_{yn} \\ a_{zn} \end{bmatrix} axbaybazb =C1× axnaynazn

[ a x n a y n a z n ] = C × [ a x b a y b a z b ] \begin{bmatrix} a_{xn} \\ a_{yn} \\ a_{zn} \end{bmatrix} =C\times \begin{bmatrix} a_{xb} \\ a_{yb} \\ a_{zb} \end{bmatrix} axnaynazn =C× axbaybazb

其中 ( a x b , a y b , a z b ) (a_{xb},a_{yb},a_{zb}) (axb,ayb,azb)为加速度计在载体坐标系下测量的加速度向量, ( a x n , a y n , a z n ) (a_{xn},a_{yn},a_{zn}) (axn,ayn,azn)为转换到导航坐标系下的加速度向量。

现在,需要将测量的加速度向量在导航坐标系下的映射旋转到标准重力的方向即

[ 0 0 g ] \begin{bmatrix} 0\\ 0\\ g \end{bmatrix} 00g

将这个旋转应用到载体上即为载体当前的姿态。

v 1 → = [ a x n a y n a z n ] \overrightarrow{v_{1}} =\begin{bmatrix} ax_{n} \\ ay_{n} \\ az_{n} \end{bmatrix} v1 = axnaynazn v 2 → = [ 0 0 g ] \overrightarrow{v_{2}}=\begin{bmatrix} 0\\ 0\\ g \end{bmatrix} v2 = 00g v 1 → \overrightarrow{v_{1}} v1 v 2 → \overrightarrow{v_{2}} v2 之间的夹角为 θ \theta θ

则旋转轴 v → = v 1 → × v 2 → \overrightarrow{v}=\overrightarrow{v_{1}} \times \overrightarrow{v_{2}} v =v1 ×v2 ,然后绕此轴旋转 θ \theta θ

根据 v 1 → ⋅ v 2 → = ∣ v 1 → ∣ ∣ v 2 → ∣ ⋅ cos ⁡ ( θ ) \overrightarrow{v_{1}} \cdot \overrightarrow{v_{2}}=\left | \overrightarrow{v_{1}} \right | \left | \overrightarrow{v_{2}} \right | \cdot \cos(\theta) v1 v2 = v1 v2 cos(θ),将 v 1 → \overrightarrow{v_{1}} v1 v 2 → \overrightarrow{v_{2}} v2 归一化,则 θ = arccos ⁡ ( v 1 → ⋅ v 2 → ) \theta=\arccos(\overrightarrow{v_{1}} \cdot \overrightarrow{v_{2}} ) θ=arccos(v1 v2 )

根据四元数公式
q ′ = [ cos ⁡ ( θ 2 ) v → × sin ⁡ ( θ 2 ) ] q^{'}=\begin{bmatrix} \cos(\frac{\theta}{2} ) & \overrightarrow{v} \times \sin(\frac{\theta}{2}) \end{bmatrix} q=[cos(2θ)v ×sin(2θ)]
可以得到此旋转对应的四元数,其中 v → \overrightarrow{v} v 需要归一化处理。

将初始四元数 q q q左乘 q ′ q^{'} q就能得到载体当前姿态对应的四元数。

以下是使用Eigen3的C++示例

#include "Eigen/Core"
#include "Eigen/Geometry"
#include <cmath>

// 加速度计读取的数据,三个坐标轴,单位mg
float acc[3];

// 初始四元数
Eigen::Quaternionf quaternion = Eigen::Quaternionf(1.0f, 0.0f, 0.0f, 0.0f);

while(1) {
	// 读取加速度计数据
	read_data(&acc);

	// 加速度计测量的加速度向量,单位转为g
	Eigen::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();
}

加速度计解算姿态在载体静止时可以得到较准确的俯仰角和滚转角,但是因为重力在水平方向上没有分量,所以无法解算偏航角。而且当物体做非匀速运动时,额外的加速度会导致姿态不正确。文章来源地址https://www.toymoban.com/news/detail-613344.html

到了这里,关于三轴加速度计解算姿态(四元数)的文章就介绍完了。如果您还想了解更多内容,请在右上角搜索TOY模板网以前的文章或继续浏览下面的相关文章,希望大家以后多多支持TOY模板网!

本文来自互联网用户投稿,该文观点仅代表作者本人,不代表本站立场。本站仅提供信息存储空间服务,不拥有所有权,不承担相关法律责任。如若转载,请注明出处: 如若内容造成侵权/违法违规/事实不符,请点击违法举报进行投诉反馈,一经查实,立即删除!

领支付宝红包 赞助服务器费用

相关文章

  • e2studio开发三轴加速度计LIS2DW12(4)----测量倾斜度

    本文将介绍如何驱动和利用LIS2DW12三轴加速度计的倾斜检测理论和倾斜角测量方法。一般来说,这里描述的程序也可以应用于三轴模拟或数字加速度计,这取决于它们各自的规格。 最近在弄ST和瑞萨RA的课程,需要样片的可以加群申请:615061293 。 https://www.bilibili.com/video/BV16i

    2024年01月17日
    浏览(47)
  • STM32无人机-四轴四元数姿态解算与卡尔曼滤波

    MPU6050是一种非常流行的空间运动传感器芯片,可以获取器件当前的三个加速度分量和三个旋转角速度。 什么是四元数 这部分很难,新手知道四元数的功能是将 6 轴传感器数据转化为三轴姿态角度数据即可。 四元数解算程序店家已经封装成一个函数,输入 MPU6050 数值,解算周

    2024年02月03日
    浏览(41)
  • [Android]将实时获取的加速度计、陀螺仪、磁场数据通过卡尔曼滤波,转换为手机的姿态角

    由于需要实时获取传感器数据,我们可以使用Android系统提供的SensorManager类来获取加速度计、陀螺仪和磁场传感器的数据。然后,我们可以将这些数据传递给一个卡尔曼滤波器对象进行滤波。 以下是一段示例代码: 在这个示例代码中,我们注册了对加速度计、陀螺仪和磁场传

    2024年02月17日
    浏览(45)
  • 【无人机/平衡车/机器人】详解STM32+MPU6050姿态解算—卡尔曼滤波+四元数法+互补滤波(文末附3个算法源码)

     效果: MPU6050姿态解算-卡尔曼滤波+四元数+互补滤波 目录 基础知识详解 欧拉角

    2024年04月14日
    浏览(69)
  • 第一章 MEMS惯性器件-加速度计误差分析

    常用MEMS传感器参数分析和应用介绍系列,分十二个章节讨论相关问题: Outline: 第一章 MEMS惯性器件-加速度计误差分析 第二章 MEMS惯性器件-陀螺仪误差分析 第三章 国内工业和消费类厂商惯性器件IMU常见验证手段 第四章 MEMS磁力计与模块的误差分析 第五章 6-DOF与9-DOF模块工厂

    2024年02月09日
    浏览(38)
  • LIS3DH(3轴加速度计)使用

    LIS3DH 是属于 “nano” 系列的超低功耗高性能 3 轴线性加速度计,具有数字 I2C、 SPI 串行接口标准输出。器件具有超低功耗工作模式,可实现高级节能、智能睡眠唤醒以及恢复睡眠功能。LIS3DH 具有 ±2g/±4g/±8g/±16g 的动态用户可选满量程,并能通过 1 Hz 到 5 kHz 的输出数据速率测

    2024年02月15日
    浏览(88)
  • MPU6050 加速度计和陀螺仪传感器与 Arduino 连接

    MPU6050是一款非常流行的加速度计陀螺仪芯片,具有六轴感应和 16 位测量分辨率。这种意义上的高精度和低廉的成本使其在 DIY 社区中非常受欢迎。甚至许多商业产品都配备了 MPU6050。陀螺仪和加速度计的组合通常被称为惯性测量单元或 IMU。 IMU 传感器用于各种应用,例如手机

    2024年02月02日
    浏览(54)
  • ros2 机器人imu传感器 加速度计 陀螺仪精度和数据填充单位换算

    起因,imu解算出了加速度 角速度,但原始数据是没有单位的,只是在一个精度范围的值,要使用这些数据,就需要把这些没有单位的数据换算成带单位的数据,下面解说一下换算原理。 imu读取数据代码参考上期的博客: ros2 c++实现JY_95T IMU解算三轴 加速度 角速度 欧拉角 磁力

    2024年02月13日
    浏览(55)
  • 【STM32+cubemx】0027 HAL库开发:MPU6050陀螺仪和加速度计数据的获取和校准

    在制作平衡车或者飞行器时,不可避免地需要知道设备本身的姿态,一般我们使用陀螺仪和加速度计来获取这些信息。 陀螺仪用来测量物体的角度。传统的机械式陀螺的原理,和我们小时候玩的陀螺一样,是利用了高速旋转的物体能保持轴线稳定的特性;机械式陀螺需要的加

    2023年04月08日
    浏览(48)
  • 持之以恒(一)位姿转换:姿态 / 四元数 / 旋转矩阵 / 欧拉角 及 位姿矩阵

    姿态的几种表示形式, 姿态角 、 四元数 、 欧拉角 、 旋转矩阵 、 位姿矩阵 。 姿态 说明 表示形式 Eigen 姿态角 指的是机体坐标系与地理坐标系的夹角,即旋转向量 rx,ry,rz Eigen::Vector3f(Degrees) 四元数 四元素不存在万向节死锁问题、利用球面插值可以获得均匀的转速 w,x,y,z

    2024年02月15日
    浏览(51)

觉得文章有用就打赏一下文章作者

支付宝扫一扫打赏

博客赞助

微信扫一扫打赏

请作者喝杯咖啡吧~博客赞助

支付宝扫一扫领取红包,优惠每天领

二维码1

领取红包

二维码2

领红包