基于Matlab使用互补滤波器和IMU数据估计方向（附源码）

此示例演示如何从 Arduino 流式传输 IMU 数据并使用互补滤波器估计方向。

一、链接硬件

将 MPU-9250 传感器的 SDA、SCL、GND 和 VCC 引脚连接到 Arduino® 硬件的相应引脚。此示例使用具有以下连接的 Arduino® Uno 板：

• SDA - A4

• 标准及校正实验所 - A5

• 可变电压调节 - +3.3V

• 接地 - 接地

确保与传感器的连接完好无损。建议将传感器安装/连接到原型屏蔽，以避免传感器运动时连接松动。请参阅传感器故障排除页面以调试与传感器相关的问题。

二、创建传感器对象

指定传感器采样率和运行循环的时间量。（可选）启用该标志以检查是否有任何样本溢出。通过禁用 theflag，还可以使用保存在 MAT 文件中的传感器数据运行该示例。当 IMU 通常朝向正南时记录数据，然后旋转：

• 围绕 z 轴 +90 度

• 围绕 z 轴 -180 度

• 围绕 z 轴 +90 度

• 围绕 y 轴 +90 度

• 围绕 y 轴 -180 度

• 围绕 y 轴 +90 度

• 围绕 x 轴 +90 度

• 围绕 x 轴 -270 度

• 围绕 x 轴 +180 度

请注意，围绕 x 轴的最后两次旋转是额外的 90 度。这样做是为了将设备倒置。IMU 的最终方向与初始方向相同，正南。

Fs = 100;
samplesPerRead = 10;
runTime = 20;
isVerbose = false;
useHW = true;

if useHW
    a = arduino;
    imu = mpu9250(a, 'SampleRate', Fs, 'OutputFormat', 'matrix', ...
        'SamplesPerRead', samplesPerRead);
else
    load('loggedMPU9250Data.mat', 'allAccel', 'allGyro', 'allMag', ...
        'allT', 'allOverrun', ...
        'numSamplesAccelGyro', 'numSamplesAccelGyroMag')
end

三、将 MPU-9250 传感器的轴与 NED 坐标对齐

MPU-9250 中的加速度计、陀螺仪和磁力计的轴彼此不对齐。指定每个传感器的索引和符号 x、y 和 z 轴，以便传感器在静止时与东北向下 （NED） 坐标系对齐。在此示例中，磁力计轴更改，而加速度计和陀螺仪轴保持固定。对于自己的应用程序，请根据需要更改以下参数。

% Accelerometer axes parameters.
accelXAxisIndex = 1;
accelXAxisSign = 1;
accelYAxisIndex = 2;
accelYAxisSign = 1;
accelZAxisIndex = 3;
accelZAxisSign = 1;

% Gyroscope axes parameters.
gyroXAxisIndex = 1;
gyroXAxisSign = 1;
gyroYAxisIndex = 2;
gyroYAxisSign = 1;
gyroZAxisIndex = 3;
gyroZAxisSign = 1;

% Magnetometer axes parameters.
magXAxisIndex = 2;
magXAxisSign = 1;
magYAxisIndex = 1;
magYAxisSign = 1;
magZAxisIndex = 3;
magZAxisSign = -1;

% Helper functions used to align sensor data axes.

alignAccelAxes = @(in) [accelXAxisSign, accelYAxisSign, accelZAxisSign] ...
    .* in(:, [accelXAxisIndex, accelYAxisIndex, accelZAxisIndex]);

alignGyroAxes = @(in) [gyroXAxisSign, gyroYAxisSign, gyroZAxisSign] ...
    .* in(:, [gyroXAxisIndex, gyroYAxisIndex, gyroZAxisIndex]);

alignMagAxes = @(in) [magXAxisSign, magYAxisSign, magZAxisSign] ...
    .* in(:, [magXAxisIndex, magYAxisIndex, magZAxisIndex]);

执行额外的传感器校准：如有必要，可以校准磁力计以补偿磁失真。

指定补充筛选器参数有两个可调参数。该参数确定加速度计测量相对于陀螺仪测量的信任程度。该参数确定磁力计测量相对于陀螺仪测量的信任程度。

compFilt = complementaryFilter('SampleRate', Fs)

四、使用加速度计和陀螺仪估计方向

将属性设置为 以禁用磁力计测量输入。在这种模式下，滤波器仅将加速度计和陀螺仪测量值作为输入。此外，筛选器假定 IMU 的初始方向与父导航框架对齐。如果 IMU 最初未与导航框架对齐，则方向估计中将存在恒定的偏移。

，滤波器仅将加速度计和陀螺仪测量值作为输入。此外，筛选器假定 IMU 的初始方向与父导航框架对齐。如果 IMU 最初未与导航框架对齐，则方向估计中将存在恒定的偏移。HasMagnetometerfalse

compFilt = complementaryFilter('HasMagnetometer', false);

tuner = HelperOrientationFilterTuner(compFilt);

if useHW
    tic
else
    idx = 1:samplesPerRead;
    overrunIdx = 1;
end
while true
    if useHW
        [accel, gyro, mag, t, overrun] = imu();
        accel = alignAccelAxes(accel);
        gyro = alignGyroAxes(gyro);
    else
        accel = allAccel(idx,:);
        gyro = allGyro(idx,:);
        mag = allMag(idx,:);
        t = allT(idx,:);
        overrun = allOverrun(overrunIdx,:);

        idx = idx + samplesPerRead;
        overrunIdx = overrunIdx + 1;
        pause(samplesPerRead/Fs)
    end

    if (isVerbose && overrun > 0)
        fprintf('%d samples overrun ...\n', overrun);
    end

    q = compFilt(accel, gyro);
    update(tuner, q);

    if useHW
        if toc >= runTime
            break;
        end
    else
        if idx(end) > numSamplesAccelGyro
            break;
        end
    end
end

 五、使用加速度计、陀螺仪和磁力计估计方向

使用默认值 and，滤波器在短期内更信任陀螺仪测量，但在长期内更信任加速度计和磁力计测量。这使得滤波器对快速的方向变化更具反应性，并防止方向估计在更长的时间内漂移。对于特定的 IMU 传感器和应用目的，可能需要调整滤波器的参数以提高方向估计精度。

compFilt = complementaryFilter('SampleRate', Fs);

tuner = HelperOrientationFilterTuner(compFilt);

if useHW
    tic
end
while true
    if useHW
        [accel, gyro, mag, t, overrun] = imu();
        accel = alignAccelAxes(accel);
        gyro = alignGyroAxes(gyro);
        mag = alignMagAxes(mag);
    else
        accel = allAccel(idx,:);
        gyro = allGyro(idx,:);
        mag = allMag(idx,:);
        t = allT(idx,:);
        overrun = allOverrun(overrunIdx,:);

        idx = idx + samplesPerRead;
        overrunIdx = overrunIdx + 1;
        pause(samplesPerRead/Fs)
    end

    if (isVerbose && overrun > 0)
        fprintf('%d samples overrun ...\n', overrun);
    end

    q = compFilt(accel, gyro, mag);
    update(tuner, q);

    if useHW
        if toc >= runTime
            break;
        end
    else
        if idx(end) > numSamplesAccelGyroMag
            break;
        end
    end
end

 六、总结

此示例展示了如何使用来自 Arduino 和互补滤波器的数据估计 IMU 的方向。此示例还展示了如何配置 IMU，并讨论了调整互补滤波器参数的效果。

七、程序

使用Matlab R2022b版本，点击打开。

 打开下面的“EstimateOrientation...Example.m”文件，点击运行，就可以看到上述效果。 

关注下面公众号，后台回复关键词：互补滤波器和IMU数据估计方向，发送源码链接。