IMU开发

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

IMU介绍

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

而在机器人应用中：

1. 姿态估计和控制：IMU可以测量机器人的姿态角，包括横滚角、俯仰角和航向角，从而实现对机器人姿态的估计和控制。这对于机器人的平衡控制、路径规划、目标跟踪等任务非常重要。
2. 导航和定位：IMU可以与GPS等其他传感器融合，实现机器人的惯性导航和定位。这种方法可以在GPS信号不佳或无法使用的环境中，如室内、隧道等，提供连续的导航和定位信息。
3. 动态监测：IMU可以实时监测机器人的运动状态，包括速度、加速度、角速度等，从而实现对机器人运动状态的监测和分析。这对于机器人的故障诊断、性能评估等具有重要意义。
4. 碰撞检测和安全控制：IMU可以检测机器人的碰撞和冲击，从而触发安全控制机制，保护机器人和周围环境的安全。例如，当机器人发生碰撞时，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

视频中，我们可以看见，调整开发板的姿态，我们计算的偏航角，翻滚角，俯仰角都在进行变化：