0. 简介

现在的机器人领域在普遍使用IMU（惯性导航单元）。该系统有三个加速度传感器与三个角速度传感器（陀螺）组成，加速度计用来感受飞机相对于地垂线的加速度分量，陀螺仪用来感知飞机的角速率变化；通过算法融合来计算出飞行器姿态，也用来进行航位推算。而每次我们拿到一个惯性导航的时候会发现参数手册中有很多信息，而这些信息的好坏要怎么评判是比较关键的。下面我们来一一讲解。

1. 最大零点偏移

1.1 含义

IMU（惯性测量单元）的最大零点偏移是指在没有外部力或加速度作用下，IMU输出的测量值与真实值之间的差异。它是由于传感器的制造误差、温度变化、机械振动等因素引起的。

IMU的最大零点偏移是一个统计值，表示在一定的置信水平下，IMU输出值与真实值之间的最大差异。

1.2 指标

对于低成本的消费级IMU，其最大零点偏移范围可能在几个百分比以内，例如加速度计的零点偏移范围可能在±2%以内（对应就是±20mg），陀螺仪的零点偏移范围可能在±5°/s以内。

高精度的工业级或航空航天级IMU，其最大零点偏移范围会更小，通常在几个千分比以内。例如，加速度计的零点偏移范围可能在±0.1%（对应就是±1mg）以内，陀螺仪的零点偏移范围可能在±0.1°/s以内

2. 温度偏移

2.1 含义

IMU的温度偏移是指在不同温度下，IMU输出的测量值与真实值之间的差异。温度偏移是由于传感器的温度敏感性引起的，温度变化会影响传感器的性能和精度。

温度偏移通常以每度或每十度为单位，例如每度的温度偏移为X，表示每增加或减少1度时，IMU输出值可能会相应地增加或减少X。

2.2 指标

消费级IMU的温度偏移范围可能在几个百分比以内，例如加速度计的温度偏移范围可能在±0.2%(对应就是±2mg)以内，陀螺仪的温度偏移范围可能在±0.5°/s以内。

而高精度的工业级或航空航天级IMU的温度偏移范围会更小，通常在几个千分比以内。例如，加速度计的温度偏移范围可能在±0.05%(对应就是±0.5mg)以内，陀螺仪的温度偏移范围可能在±0.1°/s以内。

3. 零偏稳定性

3.1 含义

在零输入状态下的长时间稳态输出是一个平稳的随机过程，即稳态输出将围绕均值（零偏）起伏和波动，习惯上用均方差来表示，这种均方差被定义为零偏稳定性。

零偏漂移是指IMU测量值与真实值之间的偏差随时间的变化。通常，零偏漂移以每小时、每分钟或每秒钟的角度或速度单位来表示。

3.2 指标

加速度计：一般来说，高精度的加速度计的零偏稳定性可以达到几微米每秒平方（µg）级别。典型的高性能加速度计的零偏漂移可能在1 µg或更低。

陀螺仪：高精度的陀螺仪的零偏稳定性一般在几度每小时（°/hr）或更低。典型的高性能陀螺仪的零偏漂移可能在0.01°/hr或更低。

4. 灵敏度

4.1 含义

IMU的灵敏度是指传感器对于输入信号的变化的响应能力。对于加速度计来说，灵敏度表示单位输入加速度变化对应的传感器输出变化；对于陀螺仪来说，灵敏度表示单位输入角速度变化对应的传感器输出变化。

4.2 指标

加速度计：一般的加速度计灵敏度范围可达到几百到几千mV/g（毫伏每重力单位）。高精度的加速度计可能具有更低的灵敏度，可达到几十到几百mV/g。

陀螺仪：一般的陀螺仪灵敏度范围可达到几十到几百mV/°/s（毫伏每度每秒）。高精度的陀螺仪可能具有更低的灵敏度，可达到几到几十mV/°/s。

5. 比例因子误差

5.1 含义

比例因子误差是指IMU中加速度计和陀螺仪输出的测量值与实际值之间的比例差异。它表示传感器的精度和准确性。

5.2 指标

一般来说，加速度计的比例因子误差通常在0.1%到1%之间。这意味着，如果实际加速度为10 m/s²，测量值可能在9.9 m/s²到10.1 m/s²之间。

对于陀螺仪，一般的比例因子误差范围也在0.1%到1%之间。例如，如果实际角速度为100°/s，测量值可能在99°/s到101°/s之间。

5.3 三大基本误差源归纳

简单的理解，传感器的误差可以归为三大类：

• bias(零篇): 既理论上输出应该为0，当实际上输出有一个小的偏置。比如陀螺仪如果在惯性系下绝对静止，那么理论上三轴输出为0,0,0。但实际是不可能的，总会有bias存在。而且这个bias 一般还不是一个参数，他会在一定范围内缓慢随机飘移。
• 噪声(noise): 所有传感器都有噪声，一般可以简单理解为高斯白噪声。
• 比例因子(scale factor): 既刻度误差，比如陀螺仪比例误差为0.02. 那么陀螺以10deg/s转动，当实际输出为 10deg/s *1.02 = 10.2deg/s, 差了一个比例因子误差。
  以上分类只是把传感器的误差简单分为三大类，每一类都大有学问，都够一大堆博士毕业的。下面对每一类进行细致讨论：

6. 工作温度范围

6.1 含义

IMU（惯性测量单元）的工作温度范围指的是传感器正常工作的温度范围。传感器的性能和精度通常在这个温度范围内能够保持稳定和可靠的工作。

6.2 指标

加速度计和陀螺仪的一般工作温度范围通常是-40°C至+85°C。这个范围可以适应大多数工业和消费电子应用的环境温度要求。

7. 测量范围

7.1 含义

IMU（惯性测量单元）的测量范围指的是传感器能够测量的最大物理量范围。传感器的输出值通常以数字或模拟信号的形式表示，这些信号对应于物理量的测量值。

7.2 指标

加速度计的一般测量范围通常以g为单位，表示重力加速度的倍数。常见的加速度计测量范围包括±2g、±4g、±8g和±16g。例如，如果加速度计的测量范围为±2g，则传感器可以测量的加速度范围为-2g至+2g。

陀螺仪的一般测量范围通常以角速度为单位，表示单位时间内物体的旋转速度。常见的陀螺仪测量范围包括±250°/s、±500°/s、±1000°/s和±2000°/s。例如，如果陀螺仪的测量范围为±250°/s，则传感器可以测量的角速度范围为-250°/s至+250°/s。

8. 非线性度

8.1 含义

IMU（惯性测量单元）的非线性度指的是传感器输出与真实物理量之间的非线性误差。它表示传感器输出值与真实物理量之间的偏差程度，不符合线性关系。

8.2 指标

加速度计和陀螺仪的非线性度通常以百分比（%）或以最大测量范围的百分比来表示。非线性度可以分为两种类型：满量程非线性度（Full Scale Range Nonlinearity，FSRNL）和零点非线性度（Zero Offset Nonlinearity，ZONL）。

• 满量程非线性度（FSRNL）表示在传感器的整个测量范围内，输出值与真实物理量之间的非线性误差。它通常以百分比来表示，例如FSRNL为±1%表示在整个测量范围内，传感器的输出值与真实物理量之间的最大偏差为传感器测量范围的1%。

• 零点非线性度（ZONL）表示在传感器的零点（即没有外力或角速度作用时）附近，输出值与真实物理量之间的非线性误差。它通常以百分比来表示，例如ZONL为±0.5%表示在传感器零点附近，传感器的输出值与真实物理量之间的最大偏差为传感器测量范围的0.5%。

加速度计和陀螺仪的非线性度范围通常在±0.1%至±1%之间。

9. 交叉耦合噪声

9.1 含义

IMU（惯性测量单元）的交叉耦合噪声指的是在测量过程中，加速度计和陀螺仪之间相互影响导致的噪声。具体来说，当加速度计测量物体的加速度时，陀螺仪的输出也会受到影响，反之亦然。这种相互影响导致的噪声被称为交叉耦合噪声。

9.2 指标

IMU的交叉耦合噪声的范围可以在几个百分比之内，具体取决于传感器的质量和设计

10. 噪声

10.1 含义

IMU（惯性测量单元）的噪声是指传感器本身的随机测量误差。这种误差可以由多种因素引起，包括电子噪声、温度变化、机械振动等。

加速度计的噪声通常以加速度单位（例如m/s^2）表示，而陀螺仪的噪声通常以角速度单位（例如rad/s）表示。噪声的大小取决于传感器的质量和设计。

10.2 指标

加速度计的噪声通常以加速度单位表示，即m/s²或g（重力加速度）。一般而言，高质量的加速度计的噪声范围在几微重力（μg）到几毫重力（mg）之间

陀螺仪的噪声通常以角速度单位表示，即rad/s或度/秒。一般而言，高质量的陀螺仪的噪声范围在几度/小时到几度/秒之间

11. 零偏不稳定性

11.1 含义

IMU（惯性测量单元）的零偏不稳定性是指传感器输出在静止状态下的偏差变化。它表示了传感器的零点漂移或零偏的不稳定性。具体来说，零偏不稳定性描述了传感器在时间上的变化，即传感器的输出在静止状态下会随时间而变化。

11.2 指标

一般来说，加速度计的零偏不稳定性通常在几μg到几十μg之间，而陀螺仪的零偏不稳定性通常在几度每小时到几十度每小时之间。

12 代码解析

#!/usr/bin/python
# -*- coding: utf-8 -*-
import matplotlib.pyplot as plt
import numpy as np
from scipy.fft import fft
import math
from matplotlib.ticker import FuncFormatter

def parse_imu_file(file_path):
    imu_data = []
    timestamps = []
    ga = []
    with open(file_path, 'r') as file:
        for line in file:
            data = line.strip().split(' ')
            timestamp = float(data[0])
            accel_x = float(data[1])
            accel_y = float(data[2])
            accel_z = float(data[3])
            gravity_x = float(data[4])
            gravity_y = float(data[5])
            gravity_z = float(data[6])

            imu_data.append((timestamp, accel_x, accel_y, accel_z, gravity_x, gravity_y, gravity_z))
            timestamps.append(timestamp)
            ga.append([gravity_x, gravity_y, gravity_z,accel_x, accel_y, accel_z])
    return imu_data,timestamps,ga


def allan(data, fs):
        """
        https://blog.csdn.net/m0_37576542/article/details/127902701
        @summary: 计算allan方差、随机游走噪声
        @param data: imu原始数据
        @param fs: imu输出频率
        @return:
        """
        # 开始计算allan方差----------------------------------------------------------------------------------------------
        data = np.asarray(data, dtype=float)
        data[:, 0:3] = data[:, 0:3] * 180 / math.pi
        print(data.shape)
        data_int = np.cumsum(data, axis=0) / fs  # 1/freq=dt, p.78 (4.6)
        N = len(data_int)
        taunum = math.floor(np.log((N - 1) / 3) / np.log(2))  # how many kind of tau
        print(N,taunum)
        allan_a = np.empty((int(taunum) + 1, 6))
        for k in range(0, 6):
            for i in range(int(taunum)  + 1):
                n = 2 ** i  # data number in one box is qrow with 2 square to reduce the counter
                # use matrix [] added and  col.27 mean to replace sumation in p.78 (4.9)
                temp2 = data_int[1 + 2 * n:N - 0 * n:n, k]
                temp1 = data_int[1 + 1 * n:N - 1 * n:n, k]
                temp0 = data_int[1 + 0 * n:N - 2 * n:n, k]
                allan_a[i, k] = np.sqrt(np.mean((temp2 - 2 * temp1 + temp0) ** 2) / (2 * (n / fs) ** 2))  # p78 (4.9)

        t_axis = [2 ** i / fs for i in range(0, int(taunum)  + 1)]
        # 输出陀螺仪Allan方差曲线
        # Allan.fitAllanPic(t_axis, allan_a[:, 0:3] * 3600, "Allan Standard Deviation",
        #                   "cluster time(s)", "Allan standard (deg/hr)", "gryo.png")
        # # 输出加速计Allan方差曲线
        # Allan.fitAllanPic(t_axis, allan_a[:, 3:6] * 3600, "Allan Standard Deviation",
        #                   "cluster time(s)", "Allan standard (m/s/sqt(h))", "acc.png")
        # 开始计算随机游走噪声--------------------------------------------------------------------------------------------
        Ng_Ncku, Kg_Ncku, Bsg_Ncku = gryoRandomWalk(allan_a, t_axis)
        Na_Ncku, Ka_Ncku, Bsa_Ncku = accRandomWalk(allan_a, t_axis)
        print("Angle random walk (deg/sqrt(h)): [x:%s][y:%s][z:%s]" % tuple(Ng_Ncku * 3600))
        print("rate random walk (deg/sqrt(h^3)): [x:%s][y:%s][z:%s]" % tuple(Kg_Ncku * 3600))
        print("Gyro bias instabilities (deg/h): [x:%s][y:%s][z:%s]" % tuple(Bsg_Ncku / 0.664 * 3600))
        print("Velocity random walk (m/s/sqrt(h)): [x:%s][y:%s][z:%s]" % tuple(Na_Ncku * 3600))
        print("Acceleration random walk (m/s/sqrt(h^3)): [x:%s][y:%s][z:%s]" % tuple(Ka_Ncku * 3600))
        print("Acceleration bias instabilities (m/s/h)): [x:%s][y:%s][z:%s]" % tuple(Bsa_Ncku / 0.664 * 3600))
        return t_axis,allan_a

def gryoRandomWalk(allan, tAxis):
        """
        @summary: 计算陀螺仪角度随机游走噪声、速度随机游走噪声、零偏不稳定性
        @param allan: allan方差数据矩阵
        @param tAxis:
        @return:
        """
        # fit line for angle random walk
        T_K = 1 * 3600  # vertical line to find the corresponding coefficient
        p1 = 3
        p2 = 10
        A1 = -0.5
        pmid = int(math.floor((p2 - p1) / 2) + p1 - 1)
        B = np.log10(allan[pmid, 0:3]) - A1 * np.log10(tAxis[pmid])
        y2 = A1 * np.log10(T_K) + B
        Ng_Ncku = 10 ** y2
        y3 = A1 * np.log10(tAxis[0]) + B
        # plt.plot([tAxis[1], tAxis[pmid], T_K], 10**y3*3600)
        # plt.plot([tAxis[1], tAxis[pmid], T_K], allan[pmid, 0:3] * 3600)
        # plt.plot([tAxis[1], tAxis[pmid], T_K], Ng_Ncku * 3600)

        # fit line for gyro rate random walk
        T_K = 3 * 3600
        p1 = 11
        p2 = 12
        A1 = 0.5
        T_start = tAxis[1]
        pmid = int(math.floor((p2 - p1) / 2) + p1 - 1)
        print(pmid)
        B = np.log10(allan[pmid, 0:3]) - A1 * np.log10(tAxis[pmid])
        y2 = A1 * np.log10(T_K) + B
        Kg_Ncku = 10 ** y2
        y3 = A1 * np.log10(T_start) + B
        # plt.plot([tAxis[1], tAxis[pmid], T_K], 10 ** y3 * 3600)
        # plt.plot([tAxis[1], tAxis[pmid], T_K], allan[pmid, 0:3] * 3600)
        # plt.plot([tAxis[1], tAxis[pmid], T_K], Kg_Ncku * 3600)

        # fit line for bias instabilities
        Bsg_Ncku = allan[:, 0:3].min(0)
        return Ng_Ncku, Kg_Ncku, Bsg_Ncku

def  accRandomWalk(allan, tAxis):
        """
        @summary: 计算加速度计速度随机游走噪声、加速度随机游走噪声
        @param allan: allan方差数据矩阵
        @param tAxis:
        @return:
        """
        # fit line for velocity random walk
        T_K = 1 * 3600  # vertical line to find the corresponding coefficient
        p1 = 4
        p2 = 8
        A1 = -0.5
        pmid =  int(math.floor((p2 - p1) / 2) + p1 - 1)
        B = np.log10(allan[pmid, 3:6]) - A1 * np.log10(tAxis[pmid])
        y2 = A1 * np.log10(T_K) + B
        Na_Ncku = 10 ** y2
        y3 = A1 * np.log10(tAxis[0]) + B

        # fit line for acceleration random walk
        T_K = 3 * 3600  # % vertical line to find the corresponding coefficient
        p1 = 11  # % the starting point to fit theorectical slop
        p2 = 12  # % the ending point to fit theorectical slop
        A1 = 0.5
        pmid =  int(math.floor((p2 - p1) / 2) + p1 - 1)
        B = np.log10(allan[pmid, 3:6]) - A1 * np.log10(tAxis[pmid])
        y2 = A1 * np.log10(T_K) + B
        Ka_Ncku = 10 ** y2
        # fit line for bias instabilities
        Bsa_Ncku = allan[:, 3:6].min(0)
        return Na_Ncku, Ka_Ncku, Bsa_Ncku


def power_of_ten_formatter(x, pos):
    # 将坐标值转换为以10的幂次方表示
    exponent = int(x)
    if exponent >= 0:
        return f"$10^{exponent}$"
    else:
        return f"$10^{{-{abs(exponent)}}}$"

def plot_imu_data(imu_data,intervals,ga):
    timestamps = [data[0] for data in imu_data]
    accel_x = [data[1] for data in imu_data]
    accel_y = [data[2] for data in imu_data]
    accel_z = [data[3] for data in imu_data]
    gravity_x = [data[4] for data in imu_data]
    gravity_y = [data[5] for data in imu_data]
    gravity_z = [data[6] for data in imu_data]

    interval = [inter for inter in intervals]
    interval.insert(0,10000)

    sample_rate = np.mean(np.diff(timestamps))/100
    t_axis,allan_a = allan(ga, sample_rate)
    ttt = np.arange(len(timestamps)) # 频率个数 
    plt.figure(figsize=(12, 6))
    plt.subplot(2, 3, 1)
    plt.plot(ttt, accel_x, label='accel_x', color='red')
    plt.plot(ttt, accel_y, label='accel_y', color='green')
    plt.plot(ttt, accel_z, label='accel_z', color='blue')
    plt.legend()
    plt.xlabel('Timestamp')
    plt.ylabel('Accel')

    plt.subplot(2, 3, 2)
    plt.plot(ttt, gravity_x, label='gravity_x', color='red')
    plt.plot(ttt, gravity_y, label='gravity_y', color='green')
    plt.plot(ttt, gravity_z, label='gravity_z', color='blue')
    plt.legend()
    plt.xlabel('Timestamp')
    plt.ylabel('Gravity')


    plt.subplot(2, 3, 3)
    plt.plot(ttt, interval, label='interval', color='red')
    plt.legend()
    plt.xlabel('Timestamp')
    plt.ylabel('interval')

    plt.subplot(2, 3, 4)
    plt.plot(t_axis, allan_a[:, 0]*3600, marker="o", label='gravity_x', color='red')
    plt.plot(t_axis, allan_a[:, 1]*3600, marker="o", label='gravity_y', color='green')
    plt.plot(t_axis, allan_a[:, 2]*3600, marker="o", label='gravity_z', color='blue')
    plt.gca().xaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter))  
    plt.gca().yaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter))   
    plt.legend()
    plt.xlabel('Timestamp')
    plt.ylabel('gravity allan')

    plt.subplot(2, 3, 5)
    plt.plot(t_axis, allan_a[:, 3]*3600, marker="o", label='accel_x', color='red')
    plt.plot(t_axis, allan_a[:, 4]*3600, marker="o", label='accel_y', color='green')
    plt.plot(t_axis, allan_a[:, 5]*3600, marker="o", label='accel_z', color='blue')
    plt.gca().xaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter))  
    plt.gca().yaxis.set_major_formatter(FuncFormatter(power_of_ten_formatter))  
    plt.legend()
    plt.xlabel('Timestamp')
    plt.ylabel('accel allan')

    #https://zhuanlan.zhihu.com/p/552293806
    fft_data_gravity_z = fft(gravity_z)
    psd_gravity_z =  np.abs(fft_data_gravity_z)**2
    normalized_power_spectrum = psd_gravity_z / np.max(psd_gravity_z)
    # 选择高频范围
    high_freq_range = (50, 100)  # 示例范围

    # 计算高频噪声指标
    high_freq_psd = normalized_power_spectrum[high_freq_range[0]:high_freq_range[1]]
    total_noise_power = np.sum(high_freq_psd)
    average_noise_power = np.mean(high_freq_psd)
    peak_noise = np.max(high_freq_psd)


    print("Total Noise Power:", total_noise_power)
    print("Average Noise Power:", average_noise_power)
    print("Peak Noise:", peak_noise)
    plt.subplot(2, 3, 6)
    plt.plot(high_freq_psd, label='high_freq_psdGZ', color='red',marker=".")
    plt.legend()
    plt.xlabel('Timestamp')
    plt.ylabel('high_freq_psd')




    plt.tight_layout()
    plt.show()

def calculate_imu_interval(timestamps):
    intervals = []
    for i in range(len(timestamps)-1):
        interval = timestamps[i+1] - timestamps[i]
        intervals.append(interval)

    return intervals

# 示例用法
file_path = 'imu.txt'
imu_data,timestamps,ga = parse_imu_file(file_path)

# 计算imu时间间隔
intervals = calculate_imu_interval(timestamps)

# 绘制imu数据变化图
plot_imu_data(imu_data,intervals,ga)


# # 打印解析后的数据
# for data in imu_data:
#     print(data)

该代码可以查看加速度以及磁力计，并判断时间戳间隔分布，同时如果处于静止状态可以判别Allan方差以及FFT噪声分布