基于I2C接口传感器的用户态6轴IMU姿态解算程序

1 概述

1.1 概要描述

基于I2C完成用户态下MPU6050传感器的调用和数据处理，且通过串口在上位机上实现姿态可视化。

2 设备属性

2.1 Atlas 200I DK A2 40PIN连接器定义

2.2 MPU6050 功能引脚

3 操作流程

3.1 Atlas 200I DK A2环境搭建

环境依赖软件和版本如下表：

3.2 设备接线

见下表

3.3 环境准备

步骤1 获取代码 代码的下载地址： https://gitee.com/ascend/ascend_community_projects/tree/310B/A200IDKA2MPU6050PostureVisualization

下载后，上传至开发板，并解压至相应目录，其中motion_display中的pde文件后续在windows电脑中运行。

步骤2 安装依赖

pip install pyserial
conda install -c conda-forge smbus2

3.4 通过串口和Processing在上位机上实现姿态可视化

步骤1 硬件准备：USB转串口芯片（以CH343为例）、杜邦线若干。

接线：CH343的TXD连接Atlas200I DK A2的UART0_RX（10），CH343的RXD连接Atlas200I DK A2的UART0_TX（8），GND接GND（6）。

USB串口驱动安装成功后，右键“此电脑”，选择“管理”，然后选中“设备管理器”，可以在“端口”中找到端口，例如COM3。若显示驱动未安装成功，则需去下载相应驱动。

步骤2 姿态解算

（感兴趣者可阅读此部分原理细节，略过不影响后续功能实现）

1.欧拉角

绕mpu6050的Z轴旋转：航向角yaw
绕mpu6050的Y轴旋转：俯仰角pitch
绕mpu6050的X轴旋转：横滚角row

2.利用加速度计进行姿态解算

加速度计可以测量mpu6050三个方向上的加速度，由于重力加速度，在静止的情况下mpu6050受到一个竖直向下的重力加速度，即三个方向的加速度向量相加等于1g。由此可以推算出：

pitch = arctan($\frac{a_y}{a_x}$)

roll = -arctan($\frac{a_x}{\sqrt{a_y^2 + a_z^2}}$)

yaw无法通过加速度计计算出。

3.利用陀螺仪进行姿态解算

陀螺仪可以测量mpu6050三个轴转动的角速度，从而对每个时刻dt内的角速度进行积分运算，累加得出当前的姿态。记当前时刻角速度为g，上一时刻角速度为$g_{last}$，计算方法如下：

pitch += ($g_y$ + $g_{y last}$) * dt / 2;

yaw += ($g_z$ + $g_{z last}$) * dt / 2;

roll += ($g_x$ + $g_{x last}$) * dt / 2;

利用上一时刻角速度计算dt时刻内梯形的面积累加比单独使用当前角速度g计算的长方形面积g * dt累加会更加准确。

4.数据融合

加速度计和陀螺仪都存在误差，加速度计易受很多噪声（如自身加速度，外在震动）影响，而陀螺仪对角速度的积分运算由于硬件的数据波动而存在累计误差，且无法在仅使用陀螺仪的情况下消除，所以需要对二者进行数据融合，从而减小误差。由于加速度计无法解算出yaw值，所以无法对yaw值进行校准，除非引入磁力计。

引入一个系数K（0<K<1）,令加速度计解算出的俯仰角为$pitch_{accel}$，翻滚角为$roll_{accel}$，陀螺仪解算出的俯仰角为$pitch_{gyro}$，翻滚角为$roll_{gyro}$，则最终的角度为：

pitch = K * $pitch_{accel}$ + (1 - K) * $pitch_{gyro}$

roll = K * $roll_{accel}$ + (1 - K) * $roll_{gyro}$

步骤3 数据传输和姿态可视化

在Atlas200I DK A2中可以向字符设备dev/ttyAMA0写入数据，从而通过UART0_RX和UART0_TX与上位机进行数据传输。依次以字符串的形式传输pitch、yaw、roll的值，并用“,”隔开。

下载Processing：https://processing.org/download ，选择Windows(Intel 64-bit)版本下载。

在Processing文件motion_display.pde中的void setup()中size(800, 480, P3D)创建3D场景，此外创建串口对象myPort = new Serial(this, "COM3", baudrate)，其中COM3为端口名（通过查看设备端口获取），baudrate为波特率，需与开发者套件串口设置的一致，如取115200。定义函数void serialEvent (Serial myPort)接收数据，其中使用readStringUntil('\n')读取每行数据，再用split(data, ',')分开三个角度的值。在函数draw()中box(box_length, box_height, box_width)创建一个长方体，使用rotateX(radians(roll))、rotateY(radians(yaw))、rotateZ(radians(pitch))来显示其当前姿态，radians()函数是将角度值转化为弧度制。

步骤4 运行步骤

①windows上位机上：

双击motion_display.pde文件，开始在Processing中运行

根据查到的端口名修改以下部分代码：

...
void setup(){
  size(800, 480, P3D); // 创建一个高800、宽480的3D场景
  myPort = new Serial(this, "COM3", baudrate);  // 根据电脑上查询到的端口号进行修改，如这里的"COM3"
}
...

②Atlas 200I DK A2上：

进入文件mpu6050_motion.py所在目录，执行cd时若报错: cd: too many arguments，可以将目标路径加上双引号" "

然后执行如下指令：

python mpu6050_motion.py

③查看结果

在一定时间间隔持续打印输出姿态信息，并在processing显示出模拟可视化状态，效果如下：

3.5 适用场景

姿态解算过程中，通过互补滤波运算，使加速度计与陀螺仪数据融合，pitch和roll的角度解算结果比较理想，但由于加速度计无法解算yaw的角度，在剧烈转动下yaw值计算误差较大，且长时间累计误差积累的情况下yaw值会发生偏移，无法仅通过MPU6050消除误差。

4 参考链接

mpu6050姿态解算： https://blog.csdn.net/hbsyaaa/article/details/108186892

mpu6050姿态解算： https://www.bilibili.com/video/BV1sL411F7fu?spm_id_from=333.788.top_right_bar_window_default_collection.content.click&vd_source=ef29df473c7f5bcf6dd109f1aceda1b2

Processing编程基础： https://www.bilibili.com/video/BV19y4y1Y7EC?p=13&vd_source=ef29df473c7f5bcf6dd109f1aceda1b2