Li_阴宅

这个屌丝很懒，什么也没留下！

热门标签

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

作者：Li_阴宅 | 2024-08-01 00:33:38

踩

六轴姿态解算

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

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 temp0, temp1, temp2, temp3; 
 
    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
    temp0 = q0;
    temp1 = q1;
    temp2 = q2;
    temp3 = q3;
    q0 += (-temp1 * gx - temp2 * gy - temp3 * gz) * halfT;
    q1 += (temp0 * gx + temp2 * gz - temp3 * gy) * halfT;
    q2 += (temp0 * gy - temp1 * gz + temp3 * gx) * halfT;
    q3 += (temp0 * gz + temp1 * gy - temp2 * 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
}

注意：

由于加速度计对水平方向的旋转无能为力，故程序得到的偏航角（yaw）数据会一直漂移，无法得到校准，通常的解决方法是增加一个磁场传感器，来获得一个准确的水平方向角来校准陀螺仪的漂移。

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

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

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

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/Li_阴宅/article/detail/911477