ELF 1搭载了六轴运动跟踪传感器 ICM20607，集成了三轴加速度计和三轴陀螺仪，那么我们就可以简单的进行开发，将角速度和加速度的数值进行数据融合，计算出开发板当前的姿态，也就是俯仰角，翻滚角，偏航角，为应用到机器人姿态控制 以及 定位做准备；
IMU（Inertial Measurement Unit）是一种用于测量载体在三维空间中的加速度、角速度和姿态等信息的传感器。这些信息可以用于计算载体的位置、速度和方向，从而实现惯性导航、姿态控制、动态监测等功能。IMU通常由三个方向的加速度计和三个方向的陀螺仪组成，可以测量载体在三个方向上的加速度和角速度。
而在机器人应用中：
因此在机器人控制领域，对于IMU算法的开发至关重要；
先对集成了ICM20607的开发板进行测试：终端输入：
elf1_cmd_icm20607
运行如下：
已知六轴传感器的数据，利用三轴角速度和三轴加速度，我们需要使用四元数或欧拉角来表示这些方向，为引入概念和控制篇幅，本文提供了一个简易的方式来计算和拟合数据，其中涉及了使用互补滤波的方式在计算。
互补滤波器结合了加速度计的高频噪声抑制和陀螺仪的低频稳定性。其基本思想是：在高频段主要依赖陀螺仪数据，而在低频段主要依赖加速度计数据。
源码：
头文件：
#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
视频中，我们可以看见，调整开发板的姿态，我们计算的偏航角，翻滚角，俯仰角都在进行变化：
更多回帖
