基于六轴陀螺仪加速度传感器的姿态解算方法

您所在的位置：网站首页 › 角速度中的角怎么算 › 基于六轴陀螺仪加速度传感器的姿态解算方法

基于六轴陀螺仪加速度传感器的姿态解算方法

2024-06-06 05:53:57| 来源: 网络整理| 查看: 265

本文介绍基于六轴陀螺仪加速度传感器的姿态解算方法。

1.六轴陀螺仪加速度传感器

六轴陀螺仪加速度传感器包含3轴陀螺仪（角速度传感器）和3轴重力加速度传感器。经常被用于无人机/四旋翼的姿态解算传感器。本文所介绍的姿态解算也是基于这种传感器值的。常见的六轴陀螺仪加速度传感器有MPU6050，ICM-20602。

2.姿态解算

姿态解算其实就是根据六轴陀螺仪加速度传感器获取当前物体的姿态角，也就是下面要讲的欧拉角。

3.基本概念 1)地面惯性坐标系

地面惯性坐标系用于研究飞行器相对于地面的运动状态，确定机体的空间位置坐标。通常用Z轴垂直向下于地平面来表示，坐标系按右手系建立。如下图所示（下标为e为地面惯性坐标系，下标为b为机体坐标系）。

2)机体坐标系

原点O位于飞行器质心，OX平行于机身轴线或对称面机翼弦线指向前方（机头方向），OY垂直于对称面指向右翼，OZ在飞机对称面内，垂直于OX指向下方。符合右手系建立规定。如下图所示。

3)欧拉角

机体坐标系与地面惯性坐标系之间的夹角就是飞行器的姿态角，又称为欧拉角。如下图所示。

a)俯仰角 $\theta$ （pitch）。机体轴与地平面（水平面）之间的夹角，机体抬头为正方向。

b)偏航角 $\psi$ （yaw）。机体轴在水平面上投影和地轴xe之间的夹角，以机头右偏为正方向。

c)滚转角 $\phi$ （roll）。机体对称面绕机体轴转过的角度，向右滚为正方向。

注：机体对称面为机体的剖面。

通过这3个值就可以唯一的确定飞行器的姿态。

这里，引用网上的一个动图作直观的解释。如下图。

前面简要介绍了六轴陀螺仪加速度传感器，它包含3轴陀螺仪（角速度传感器）和3轴重力加速度传感器，基于其中任1个3轴传感器都可以获取欧拉角（其中根据重力加速度不可获取偏航角（yaw）），下面对其作相关介绍。

4.由角速度求欧拉角

这里采用四元数法由角速度求解欧拉角，共分2个步骤，先由角速度求四元数，再由四元数求欧拉角，下面分别进行介绍。

1)四元数

引入四元数是为了方便通过角速度获取欧拉角。将角速度转换为四元数是其中的过渡过程。这里，采用一阶龙格库塔法求四元数。推导过程略，这里直接给出结论。

$q(t+T)=q(t)+T\frac{\mathrm{d} Q}{\mathrm{d} t}=q(t)+T\Omega _{b}q(t)$

其中，

$\Omega _{b}=\frac{1}{2}\begin{bmatrix} 0 & -\omega _{x}^{b} & -\omega _{y}^{b}&-\omega _{z}^{b} \\ \omega _{x}^{b} & 0 & \omega _{z}^{b} & -\omega _{y}^{b}\\ \omega _{y}^{b} & -\omega _{z}^{b} & 0 &\omega _{x}^{b} \\ \omega _{z}^{b} & \omega _{y}^{b} & -\omega _{x}^{b} & 0 \end{bmatrix}$

$q(t)=\begin{bmatrix} q_{0}\\ q_{1} \\ q_{2} \\ q_{3} \end{bmatrix}$

代入可得：

$\left\{\begin{matrix} q_{0}=q_{0}+\frac{(-w_{x}^{b}\cdot q_{1}-w_{y}^{b}\cdot q_{2}-w_{z}^{b}\cdot q_{3})\cdot T}{2} \\q_{1}=q_{1}+\frac{(w_{x}^{b}\cdot q_{0}-w_{y}^{b}\cdot q_{3}+w_{z}^{b}\cdot q_{2})\cdot T}{2} \\q_{2}=q_{2}+\frac{(w_{x}^{b}\cdot q_{3}+w_{y}^{b}\cdot q_{0}-w_{z}^{b}\cdot q_{1})\cdot T}{2} \\q_{3}=q_{3}+\frac{(-w_{x}^{b}\cdot q_{2}+w_{y}^{b}\cdot q_{1}+w_{z}^{b}\cdot q_{0})\cdot T}{2} \end{matrix}\right.$

其中，

a)各q初始值， $q_{0}=1,q_{1}=0,q_{2}=0,q_{3}=0$

b) $w_{x}^{b},w_{y}^{b},w_{z}^{b}$ 分别为机体坐标系下X轴，Y轴，Z轴角速度

c)T为采样周期

2)由四元数求欧拉角

前面我们由角速度求得四元数，根据四元数我们可以求得欧拉角。推导过程略，直接给出结论。

$\left\{\begin{matrix} r=arctan(\frac{2 (q_{0}q_{1}+q_{2}q_{3})}{1-2(q_{1}^{2}+q_{2}^{2})}) \\ p=arcsin(2(q_{0}q_{2}-q_{1}q_{3})) \\ y=arctan(\frac{2(q_{0}q_{3}+q_{1}q_{2})}{1-2(q_{2}^{2}+q_{3}^{2})})=arctan(\frac{2(q_{0}q_{3}+q_{1}q_{2})}{q_{0}^{2}+q_{1}^{2}-q_{2}^{2}-q_{3}^{2}}) \end{matrix}\right.$

其中，

a) $q_{0},q_{1},q_{2},q_{3}$ 为前面由角速度算得的四元数

b)r(roll),p(pitch),y(yaw)分别为滚转角，俯仰角，偏航角

至此，我们通过角速度求得欧拉角。

5.由加速度求欧拉角

由加速度求欧拉角，原理上是根据各轴加速度（重力加速度在各轴的投影）来求解各个角度，这里可以通过绕各个轴旋转（按ZYX方式）求出各个轴加速度，再进一步求取各个角度，公式推导如下：

$\begin{bmatrix} a_{x} \\ a_{y} \\ a_{z} \end{bmatrix}=M_{x}\cdot M_{y}\cdot M_{z}\cdot \begin{bmatrix} 0\\ 0 \\ g \end{bmatrix}= \begin{bmatrix} 1 & 0 & 0\\ 0 & cos(r) & sin(r)\\ 0 & -sin(r)& cons(r) \end{bmatrix}\cdot \begin{bmatrix} cos(p) &0 &-sin(p) \\ 0 & 1& 0\\ sin(p) &0 &cos(p) \end{bmatrix}\cdot \begin{bmatrix} cos(y) &sin(y) &0 \\ -sin(y) &cos(y) &0 \\ 0&0 &1 \end{bmatrix}\cdot \begin{bmatrix} 0\\0 \\ g \end{bmatrix}= \begin{bmatrix} cos(p)\cdot cos(y) &cos(p)\cdot sin(y) &-sin(p) \\ cos(y) \cdot sin(p)\cdot sin(r)-cos(r)\cdot sin(y)& cos(r)\cdot cos(y)+sin(p)\cdot sin(r)\cdot sin(y)& cos(p)\cdot sin(r)\\ sin(r)\cdot sin(y)+cos(r)\cdot cos(y) \cdot sin(p)&cos(r)\cdot sin(p)\cdot sin(y) -cos(y)\cdot sin(r) & cos(p)\cdot cos(r)) \end{bmatrix}\cdot \begin{bmatrix} 0\\0 \\ g \end{bmatrix}= \begin{bmatrix} -sin(p)\\ cos(p)\cdot sin(r) \\ cos(p)\cdot cos(r) \end{bmatrix}\cdot g$

由于绕Z轴旋转时，感受到的加速度（重力加速度分量）是不变的，因此，由加速度无法计算偏航角（yaw）。通过对上式求解可得：

$\left\{\begin{matrix} r=arctan(\frac{a_{y}}{^a{z}}) \\ p=-arctan(\frac{a_{x}}{\sqrt{a_{y}^{2}+a_{z}^{2}}}) \end{matrix}\right.$

其中，

1) $a_{x},a_{y},a_{z}$ 分别为X轴，Y轴，Z轴加速度（重力加速度分量）

2)r,p分别为滚转角，俯仰角

至此，我们通过加速度求得欧拉角（除偏航角（yaw））。

6.数据融合

加速度传感器的静态稳定性更好，但在运动时其数据相对不可靠，而陀螺仪的动态稳定性更好，但在静止时数据相对不可靠。所以，可以通过加速度传感器的输出来修正陀螺仪的漂移误差。这里采用Mahony互补滤波算法来实现。其原理框图如下图。

计算过程如下：

1)求 $\hat{g}^{b}$

$\hat{g}^{b}$ 其实就是加速度传感器的值（ $f^{b}$ ）经过修正后的值（这里没有空速传感器可以认为修正量为0），即 $\hat{g}^{b}=f^{b}$ 。

2)归一化

$\bar{v}=\frac{\hat{g}^{b}}{\left \| \hat{g} \right \|}$

$\left \| \hat{g} \right \|=\sqrt{g_{x}^{2}+g_{y}^{2}+g_{z}^{2}}$

$\bar{v}=\begin{bmatrix} \bar{v_{x}} \\ \bar{v_{y}} \\ \bar{v_{z}} \end{bmatrix}$

3)由四元数求重力加速度

公式推导略，这里直接给出结论。

$\hat{v}=\begin{bmatrix} 2(q_{1}q_{3}-q_{0}q_{2}) \\ 2(q_{2}q_{3}+q_{0}q_{1}) \\ q_{0}^{2}-q_{1}^{2}-q_{2}^{2}+q_{3}^{2} \end{bmatrix}=\begin{bmatrix} \hat{v}_{x}\\\hat{v}_{y} \\\hat{v}_{z} \end{bmatrix}$

这里的 $q_{0},q_{1},q_{2},q_{3}$ 为归一化后值。

4)求偏差

$e= \hat{v} \times \bar{v}= \begin{bmatrix} \vec{i} &\vec{j} &\vec{k} \\ \hat{v}_{x}& \hat{v}_{y}& \hat{v}_{z} \\ \bar{v_{x}}& \bar{v_{y}}& \bar{v_{z}} \end{bmatrix}=\begin{bmatrix} \hat{v}_{y}\bar{v_{z}}-\hat{v}_{z}\bar{v_{y}} \\ -(\hat{v}_{x}\bar{v_{z}}-\hat{v}_{z}\bar{v_{x}}) \\ \hat{v}_{x}\bar{v_{y}}-\hat{v}_{y}\bar{v_{x}} \end{bmatrix}$

注意这里是叉乘。

5)PI控制器消偏差后补偿陀螺仪

采用PI控制器消偏差，消偏差之后和陀螺仪数据（ $\bar{\Omega }^{b}$ ）相加求的 $\hat{\Omega}$ （补偿后的陀螺仪数据）。

6)由角速度求四元数

按前面所述的方法由 $\hat{\Omega}$ 求取四元数，注意这里计算完后，要对四元数进行归一化处理，便于下一次计算。

注：

1)以上计算，角速度数据单位为rad/s

2)以上计算，加速度数据单位为g

知道四元数就可很容易得到欧拉角，进而完成姿态解算。

7.参考代码

代码直接套用上述公式即可。

#define Kp 10.0f // proportional gain governs rate of convergence to accelerometer/magnetometer #define Ki 0.008f // integral gain governs rate of convergence of gyroscope biases #define halfT 0.001f // half the sample period static float q0 = 1, q1 = 0, q2 = 0, q3 = 0; // quaternion elements representing the estimated orientation static float exInt = 0, eyInt = 0, ezInt = 0; // scaled integral error static float yaw = 0; static float pitch = 0; static float roll = 0; void IMU_Update(float gx, float gy, float gz, float ax, float ay, float az) { float norm; float vx, vy, vz; float ex, ey, ez; float q0q0 = q0 * q0; float q0q1 = q0 * q1; float q0q2 = q0 * q2; //float q0q3 = q0 * q3; float q1q1 = q1 * q1; //float q1q2 = q1 * q2; float q1q3 = q1 * q3; float q2q2 = q2 * q2; float q2q3 = q2 * q3; float q3q3 = q3 * q3; if (ax * ay * az == 0) { return; } norm = sqrt(ax * ax + ay * ay + az * az); // ax = ax / norm; ay = ay / norm; az = az / norm; // estimated direction of gravity and flux (v and w) vx = 2 * (q1q3 - q0q2); vy = 2 * (q0q1 + q2q3); vz = q0q0 - q1q1 - q2q2 + q3q3 ; // error is sum of cross product between reference direction of fields and direction measured by sensors ex = (ay * vz - az * vy) ; ey = (az * vx - ax * vz) ; ez = (ax * vy - ay * vx) ; exInt = exInt + ex * Ki; eyInt = eyInt + ey * Ki; ezInt = ezInt + ez * Ki; // adjusted gyroscope measurements gx = gx + Kp * ex + exInt; gy = gy + Kp * ey + eyInt; gz = gz + Kp * ez + ezInt; // integrate quaternion rate and normalise q0 = q0 + (-q1 * gx - q2 * gy - q3 * gz) * halfT; q1 = q1 + (q0 * gx + q2 * gz - q3 * gy) * halfT; q2 = q2 + (q0 * gy - q1 * gz + q3 * gx) * halfT; q3 = q3 + (q0 * gz + q1 * gy - q2 * gx) * halfT; // normalise quaternion norm = sqrt(q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3); q0 = q0 / norm; q1 = q1 / norm; q2 = q2 / norm; q3 = q3 / norm; yaw = atan2(2 * q1 * q2 + 2 * q0 * q3, -2 * q2 * q2 - 2 * q3 * q3 + 1) * 57.3; // unit:degree pitch = asin(-2 * q1 * q3 + 2 * q0 * q2) * 57.3; // unit:degree roll = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2 * q2 + 1) * 57.3; // unit:degree }

参考论文（可在我的CSDN资源下载）：

A Complementary Filter for Attitude Estimation of a Fixed-Wing UAV

总结，本文介绍了基于六轴陀螺仪加速度传感器的姿态解算方法。

【本文地址】

公司简介

联系我们

今日新闻

点击排行

实验室常用的仪器、试剂和: 说到实验室常用到的东西，主要就分为仪器、试剂和耗

不用再找了，全球10大实验: 01、赛默飞世尔科技（热电）Thermo Fisher Scientif

三代水柜的量产巅峰T-72坦: 作者：寞寒最近，西边闹腾挺大，本来小寞以为忙完这

通风柜跟实验室通风系统有: 说到通风柜跟实验室通风，不少人都纠结二者到底是不

集消毒杀菌、烘干收纳为一: 厨房是家里细菌较多的地方，潮湿的环境、没有完全密

实验室设备之全钢实验台如: 全钢实验台是实验室家具中较为重要的家具之一，很多

图片新闻

实验室药品柜的特性有哪些: 实验室药品柜是实验室家具的重要组成部分之一，主要

小学科学实验中有哪些教学: 计算机计算器一般打孔器打气筒仪器车显微镜

实验室各种仪器原理动图讲: 1.紫外分光光谱UV分析原理：吸收紫外光能量，引起分

高中化学常见仪器及实验装: 1、可加热仪器：2、计量仪器：（1）仪器A的名称：量

微生物操作主要设备和器具: 今天盘点一下微生物操作主要设备和器具，别嫌我啰嗦

浅谈通风柜使用基本常识: 　众所周知，通风柜功能中最主要的就是排气功能。在

基于六轴陀螺仪加速度传感器的姿态解算方法

基于六轴陀螺仪加速度传感器的姿态解算方法

今日新闻

点击排行

推荐新闻

图片新闻

专题文章