EEPW论坛

IMU开发

ELF 1搭载了六轴运动跟踪传感器 ICM20607，集成了三轴加速度计和三轴陀螺仪，那么我们就可以简单的进行开发，将角速度和加速度的数值进行数据融合，计算出开发板当前的姿态，也就是俯仰角，翻滚角，偏航角，为应用到机器人姿态控制 以及 定位做准备；

IMU介绍

IMU（Inertial Measurement Unit）是一种用于测量载体在三维空间中的加速度、角速度和姿态等信息的传感器。这些信息可以用于计算载体的位置、速度和方向，从而实现惯性导航、姿态控制、动态监测等功能。IMU通常由三个方向的加速度计和三个方向的陀螺仪组成，可以测量载体在三个方向上的加速度和角速度。

而在机器人应用中：

因此在机器人控制领域，对于IMU算法的开发至关重要；

ICM20607测试

先对集成了ICM20607的开发板进行测试：终端输入：

elf1_cmd_icm20607

运行如下：

IMU算法开发

已知六轴传感器的数据，利用三轴角速度和三轴加速度，我们需要使用四元数或欧拉角来表示这些方向，为引入概念和控制篇幅，本文提供了一个简易的方式来计算和拟合数据，其中涉及了使用互补滤波的方式在计算。

互补滤波器结合了加速度计的高频噪声抑制和陀螺仪的低频稳定性。其基本思想是：在高频段主要依赖陀螺仪数据，而在低频段主要依赖加速度计数据。

源码：

头文件：

#include "stdio.h"
#include "unistd.h"
#include "sys/types.h"
#include "sys/stat.h"
#include "sys/ioctl.h"
#include "fcntl.h"
#include "stdlib.h"
#include "string.h"
#include <poll.h>
#include <sys/select.h>

宏定义：

#define ICM20607_DEV "/dev/icm20607"
#define TIME_STEP 1
#define ALPHA 0.98 //互补滤波器的权重/*
#define Pai 3.1415926

主函数：

int main(int argc, char *argv[])
{
    int fd;
    signed int databuf[7];
    unsigned char data[14];
    signed int gyro_x_adc, gyro_y_adc, gyro_z_adc;
    signed int accel_x_adc, accel_y_adc, accel_z_adc;
    signed int temp_adc;
 
    float gyro_x_act, gyro_y_act, gyro_z_act;
    float accel_x_act, accel_y_act, accel_z_act;
    float temp_act;
 
    int ret = 0;
 
    float roll,pitch,yaw;


    fd = open(ICM20607_DEV, O_RDWR);
    if(fd < 0) {
        printf("can't open file %s\\r\\n", ICM20607_DEV);
        return -1;
    }
    else
    {
        printf("Successfully opened file %s\\r\\n", ICM20607_DEV);
    }
    while (1) {
        ret = read(fd, databuf, sizeof(databuf));
        if(ret == 0) {             
            gyro_x_adc = databuf[0];
            gyro_y_adc = databuf[1];
            gyro_z_adc = databuf[2];
            accel_x_adc = databuf[3];
            accel_y_adc = databuf[4];
            accel_z_adc = databuf[5];
            temp_adc = databuf[6];
 
            
            gyro_x_act = (float)(gyro_x_adc)  / 16.4;
            gyro_y_act = (float)(gyro_y_adc)  / 16.4;
            gyro_z_act = (float)(gyro_z_adc)  / 16.4;
            accel_x_act = (float)(accel_x_adc) / 2048;
            accel_y_act = (float)(accel_y_adc) / 2048;
            accel_z_act = (float)(accel_z_adc) / 2048;
            temp_act = ((float)(temp_adc) - 25 ) / 326.8 + 25;
 
 /********************************************/
             float rollAcc = atan2(accel_y_act,accel_z_act)*180.0f/Pai;
             float pitchAcc = atan2(-accel_x_act,sqrt(accel_y_act*accel_y_act+accel_z_act*accel_z_act))*180.0f/Pai;

             roll = ALPHA*(roll + gyro_x_act*TIME_STEP) + (1-ALPHA)*rollAcc;
             pitch = ALPHA*(pitch + gyro_y_act*TIME_STEP) + (1-ALPHA)*pitchAcc;
             yaw += gyro_z_act*TIME_STEP;

/********************************************/

            printf("\\r\\n");
        
            printf("act gx = %.2f度/S, act gy = %.2f度/S, act gz = %.2f度/S\\r\\n", gyro_x_act, gyro_y_act, gyro_z_act);
            printf("act ax = %.2fg, act ay = %.2fg, act az = %.2fg\\r\\n", accel_x_act, accel_y_act, accel_z_act);
            printf("act temp = %.2f摄氏度\\r\\n", temp_act);

            printf("Roll = %.2f,Yaw = %.2f,Pitch = %.2f\\r\\n",roll,yaw,pitch);
        }
        sleep(TIME_STEP);
    }
    close(fd);    
    return 0;
}

代码编译

Ubuntu中执行配置环境命令：

. /opt/fsl-imx-x11/4.1.15-2.0.0/environment-setup-cortexa7hf-neon-poky-linux-gnueabi

交叉环境编译：

注意源码中包含了<math.h>，需要额外链接

$CC elf1_icm20607.c -o elf1_icm20607 -lm

编码拷贝

Ubuntu中执行：

scp  ielf1_icm20607 root\\@<自己开发板IP地址>：/home/root

开发板终端中执行：

./elf1_icm20607

执行结果：

root@ELF1:~# ./elf1_icm20607
Successfully opened file /dev/icm20607

act gx = -0.49度/S, act gy = 0.12度/S, act gz = -0.24度/S
act ax = -0.03g, act ay = 0.01g, act az = 1.02g
act temp = 32.51摄氏度
Roll = -0.47,Yaw = -0.24,Pitch = 0.15

act gx = -0.49度/S, act gy = 0.12度/S, act gz = -0.24度/S
act ax = -0.02g, act ay = 0.01g, act az = 1.02g
act temp = 32.52摄氏度
Roll = -0.93,Yaw = -0.49,Pitch = 0.29

act gx = -0.49度/S, act gy = 0.12度/S, act gz = -0.24度/S
act ax = -0.03g, act ay = 0.01g, act az = 1.02g
act temp = 32.51摄氏度
Roll = -1.38,Yaw = -0.73,Pitch = 0.44

act gx = -0.49度/S, act gy = 0.12度/S, act gz = -0.24度/S
act ax = -0.03g, act ay = 0.01g, act az = 1.02g
act temp = 32.54摄氏度
Roll = -1.82,Yaw = -0.98,Pitch = 0.58