mpu6050惯性导航学习记录

文章目录

mpu6050惯性导航学习记录
- 一、学习目的
- 二、原理
- - 1、mpu6050简介：
  - 2、mpu6050原理分析
  - 3、数字运动处理器（ DMP ）
- 三、惯性导航初步了解
- - 1、坐标系
  - 2、旋转矩阵
- 四、初步设想
- 五、更新补充
- - 姿态更新

一、学习目的

了解加速度传感器和角速度传感器原理。
初步了解二维惯性导航的原理。
mpu6050 的驱动移植及原始数据获取
初步方案设想

二、原理

1、mpu6050简介：

MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（ DMP: Digital Motion Processor ）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。有了 DMP，我们可以使用 InvenSense 公司提供的运动处理资料库，非常方便的实现姿态解算，降低运动处理运算对操作系统的负荷，同时大大降低开发难度。

2、mpu6050原理分析

加速度传感器是一种能够测量加速度的传感器。对于加速度传感器，我们可以把加速度传感器想象为一个装有小球的盒子模型，假设小球在真空环境下，一切东西都处于无重力状态（失重），这时，小球也不例外，将悬浮在盒子的中间，如果每一面盒壁受到冲击都会返回压力信号，那么通过转换就能测得该方向上的加速度，注意加速度传感器检测到力的方向与它自身运动加速度的方向是相反的，这种力通常被称为惯性力。

陀螺仪传感器的作用是检测三个轴（x、y、z）的旋转，其原理就是，一个旋转物体的旋转轴所指的方向在不受外力影响时，是不会改变的。人们根据这个道理，用它来保持方向。然后用多种方法读取轴所指示的方向，并自动将数据信号传给控制系统。

mpu6050 物理模型可以想象成下图：

mpu6050 对陀螺仪和加速度计分别用了三个 16 位的 ADC，将其测量的模拟量转化为可输出的数字量。

3、数字运动处理器（ DMP ）

要通过6050得到四元数和欧拉角，这个过程有两种办法，一种是用原始数据（三轴加速度、三轴角速度），通过卡尔曼滤波、积分运算、减少误差零点漂移等姿态融合运算转化等一系列复杂运算得出。第二种是直接用MPU6050 内部的自带的数字运动处理器（即 DMP ）。
DMP 就是MPU6050 内部的运动引擎，直接输出四元数，可以减轻外围微处理器的工作负担且避免了繁琐的滤波和数据融合。Invensense 公司有提供了一个 MPU6050 的嵌入式运动驱动库，这个库是基于MSP430的，不过只需要简单修改就可以移植到大部分芯片上。
输出结果：
I0、I1、I2 为处理后的欧拉角；
I3、I4、I5 为加速度计原始值；

传送门：

官方原始库：http://drivers.softpedia.com/get/Other-DRIVERS-TOOLS/Others/InvenSense-Embedded-Motion-Tracker-Driver-51.shtml

移植教程参考：https://blog.csdn.net/haha1fan/article/details/72440574

rtt下移植教程:https://blog.csdn.net/qq_16519885/article/details/121574865

三、惯性导航初步了解

1、坐标系

坐标系可大致分为两种：固定坐标系和载体坐标系。
固定坐标系指固定的坐标系，不管怎么运动，该坐标系都固定不变，如：惯性系（ i 系）、世界系（ w 系）、导航系（ n 系）等；
载体坐标系指随着载体的运动而运动的坐标系，也就是固联在在载体上的坐标系，如：载体系（ b 系）、传感器坐标系（ s 系）、机器坐标系（ r 系）等；
我们从传感器芯片上读取的加速度，角速度等数据，都是载体系上的数据。而真正导航说需要的数据，一般都是固定坐标系上的，所以惯性导航的任务中很大一部分，就是将载体系的数据转换到固定坐标系上，这一点需要旋转矩阵的相关知识来实现。

2、旋转矩阵

二维旋转矩阵可具象化为：原坐标系有一点 P（ $X_a$ , $Y_b$ ），现在将坐标系由逆时针方向旋转一个角度 $θ\theta$ ，P 点不动，求现在P点的坐标。
推导过程如下图：

如果我们把转之前的坐标系定义为导航系（n），把转之后的坐标系定义为载体系（b）, 那么矩阵 M ( $θ\theta$ )正好是载体系（b）到导航系（n）的坐标变换矩阵。
那么有载体系到导航系的坐标变换矩阵:
$Mbn=[cosθ−sinθsinθcosθ]M^n_b= \left[ \begin{matrix} cos \theta & -sin\theta \\ sin\theta & cos\theta \\ \end{matrix} \right]$

matlab仿真结果：

附矩阵计算公式：

四、初步设想

将GPS获取的定位信息看作固定不变的坐标系，即导航系（ n 系），将固联在小车质心处的mpu6050看作一个载体系（ b 系），之后将定点数据通过惯性导航公式在两个坐标系间互相转换，再通过算法可计算角度进行导航。

五、更新补充

姿态更新

航向角（ yaw ）:
yaw = yaw + $∫\int$ yaw_rad $d_t$ ；
( yaw_rad 为 z 轴角速度，注：航向角也可通过 mpu6050 直接获取)

旋转矩阵：
$Mbn=[cos(yaw)−sin(yaw)sin(yaw)cos(yaw)]M^n_b= \left[ \begin{matrix} cos ( yaw ) & -sin (yaw ) \\ sin ( yaw) & cos ( yaw ) \\ \end{matrix} \right]$
将加速度旋转到导航系：
acc_n = acc_b * $M_b^n$ ；
( n为导航系，b为载体系)

速度更新：
V = V + $∫\int$ acc_n $d_t$ ；
(原速度加上加速度的积分)

互补融合(与编码器速度进行互补滤波融合)：
v_mini * cos(yaw) * (trust) + vx * (1 - trust) = Vx；
v_mini * sin(yaw) * (trust) + vy * (1 - trust) = Vy；
( trust 代表信任程度，小于等于1，大于等于0；v_mini 为编码器测得速度)

位置更新：
P = P + $∫\int$ V * $d_t$ ；
(原位置加上对速度积分)