资讯

历史

科技

环境与自然

成长

游戏

财经

文学与艺术

美食

健康

家居

文化

情感

汽车

三农

军事

旅行

运动

教育

生活

星座命理

加速度计和陀螺仪：姿态位置求解详解

创作时间:

作者:

@小白创作中心

加速度计和陀螺仪：姿态位置求解详解

引用

CSDN

https://blog.csdn.net/m0_49842530/article/details/144094059

加速度计和陀螺仪是现代运动追踪和姿态控制技术中的核心传感器，广泛应用于无人机、机器人、运动追踪等领域。本文将详细介绍这两种传感器的工作原理及其在姿态和位置求解中的应用，并深入探讨欧拉角法和四元数法在坐标系转换中的原理和联系。

1. 案例分析

背景

使用给定的GITHUB代码和数据，编写主函数以满足要求。需要注意的是，左脚数据正常满足要求，而右脚数据无法满足要求。此外，作者在学习四旋翼飞行器时也遇到了相同的数据类型，因此对这一主题进行了深入研究。

数据类型

加速度计数据（ACC）：包含三个轴向的加速度值，即acc_x、acc_y和acc_z。
陀螺仪数据（GYR）：包含三个轴向的角速度值，即gyr_x、gyr_y和gyr_z。

数据来源

加速度计（ACC）：用于测量物体在空间中的加速度，包括重力加速度和运动加速度。通过积分加速度值可以计算出物体的速度和位置。
陀螺仪（GYR）：用于测量物体绕X、Y、Z轴的旋转速度，从而计算出物体的角度和姿态。

坐标系

全局坐标系（Global Coordinate System）：通常表示地球坐标系。
局部坐标系（Body Coordinate System）：表示物体本身的坐标系。

在标准的右手坐标系中：

X轴：从原点向右的方向。
Y轴：从原点向上的方向。
Z轴：从原点指向观察者的方向（即从屏幕向外的方向）。

常见的坐标系转换方法包括：

三参数法：欧拉角法
四参数法：四元数法
九参数法：方向余弦法

角度定义

航向角（Yaw）：飞行器绕Z轴的旋转。
俯仰角（Pitch）：飞行器绕Y轴的旋转。
滚转角（Roll）：飞行器绕X轴的旋转。

数据处理（以四旋翼为例）

姿态求解

为什么同时使用加速度计和陀螺仪？

单独使用加速度计会存在抖动噪声较大，无法直接使用；
单独使用陀螺仪不易受到抖动影响，但测得的数据是角速度，为获取实际姿态需要进行积分，但是长时间的积分行为会导致积分值逐渐偏离实际值。

假设从全局坐标系中转化到局部坐标系的四元数为Q，旋转矩阵为R的形式。这里的Q是使用陀螺仪数据来获取的，也就是说用陀螺仪数据积分获取当前姿态（即局部坐标系）。更新规则为：

其中Δt为采样时间间隔

加速度计获取的数据是局部坐标系下的加速度数据acc_x、acc_y、acc_z，所以需要将该加速度数据转化到全局坐标系下。首先进行加速度数据归一化

将重力加速度从全局坐标系中转化到局部坐标系，以重力单位向量为例

则重力单位向量在局部坐标系中的分量为

但是单独使用陀螺仪数据会导致随着时间变化积分数据会逐渐偏离实际值，为此使用加速度计数据进行补偿更新。使用陀螺仪数据获取的重力单位向量和加速度计的重力单位向量进行叉乘，求取两向量间的偏差值（为什么能进行叉乘，因为g和a都是在局部坐标系中，只是说两个获取来源不一样，存在偏差）

为什么叉乘可以表示误差？

垂直性：叉乘的结果是一个垂直于两个原始向量的向量。这意味着，如果两个向量不平行，它们的叉乘结果将指向它们之间的“偏差”方向。
大小与角度的关系：叉乘的大小等于两个向量的大小乘以它们之间夹角的正弦值。公式为 ∥A×B∥=∥A∥∥B∥sin⁡θ，其中θ是两个向量之间的夹角。当两个向量完全平行时，夹角为0，正弦值为0，叉乘结果为零向量，表示没有偏差；当两个向量不平行时，存在一个非零的角度，叉乘结果将有一个非零的大小，表示两个向量之间的偏差。
方向的判定：叉乘结果的符号（正或负）可以告诉我们一个向量旋转到另一个向量的方向。如果结果为正，则表示一个向量逆时针旋转到另一个向量；如果结果为负，则表示顺时针旋转。这可以用来表示两个向量之间的相对方向偏差。
误差表示：在某些应用中，如无人机算法，叉乘被用来表示两个向量之间的误差。例如，如果两个向量分别代表两个不同的参考系中同一物理量的方向，它们的叉乘可以表示这两个参考系之间的偏差。
物理意义：在姿态估计中，单位向量的叉积在物理意义上可以看作是轴角，轴角本身刻画了旋转，也就是叉乘的结果可以看成是旋转误差

有了偏差之后，我们可以对陀螺仪数据进行PI环节调节，以减小误差

并使用调节后的ω来更新Q值。至此，四元数Q已经获得。也就是说3个欧拉角也可以得到，姿态信息已得到。

位置求解

那么如何获取在全局坐标系中的位置？在全局坐标系中使用加速度计进行积分获得速度，在积分获得具体位置。首先将加速度计的测得的加速度数据a从局部坐标系转化到全局坐标系，旋转矩阵为R的形式

全局坐标系中速度分量

全局坐标系中位置分量

算法原理

欧拉角法

基本思想就是将两个坐标系的变换分解为绕三个不同的坐标轴的三次连续转动组成的序列。欧拉角的旋转规定为连续两次旋转，必须绕着不同的转动轴旋转，常见的旋转顺序有围绕2轴或3轴的各6种顺序，一共有12种旋转顺规，且不同的旋转顺序会产生不同的效果。

三维笛卡尔坐标系中，以环绕z轴的偏航角、环绕y轴的俯仰角、环绕x轴的滚转角来旋转

针对向量考虑绕Z轴旋转：得到新向量

得

同理，考虑绕Y轴旋转：得到新向量

考虑绕X轴旋转：得到新向量

我们以的旋转顺序为例，进行欧拉角法中两坐标系之间的转化

可以得到转移矩阵

（与下述的四元数法中R的右下角3*3矩阵内容相对应）

为什么欧拉角法需要绕3各轴旋转而不是绕两个轴旋转？
答：对于任意的向量V（除X轴上的向量），通过环绕X、Y轴的两次旋转，可以将V变化到三维空间中任意的一个新向量V'。如果V是X轴上的向量，环绕X轴的旋转无法起到效果，会导致一个自由度的丢失。为了规避这种情况需要进行三次旋转，但是仍然也会有旋转轴和向量重合的情况导致自由度的丢失，因此采用欧拉角法的三次旋转角度也有要求，要不等于90°，避免上边的旋转轴和向量重合的情况。