本发明属于计算机视觉和嵌入式系统开发领域。
背景技术：
1、姿态解算是根据惯性测量单元mpu6050所测数据，求解算出物体姿态的过程。姿态解算的结果表示大地坐标系与惯性测量单元局部坐标系之间的角位置关系，可以用欧拉角表示，包括航向角、俯仰角和横滚角。
2、mpu6050是一款六轴的惯性测量单元，包含三轴加速度计和陀螺仪，如图1所示。mpu6050可以通过i2c总线与开发板之间传输数据，测量出的原始数据在通过单位转换和姿态解算之后可以显示被测物体相对于大地坐标系的三个姿态角。嵌入式系统开发具有小巧便捷、低成本、易于实施的特点，将mpu6050作为外设与嵌入式设备连接，进行程序开发，可以有效地对物体的姿态进行测量、解算。
3、非刚性物体指具有与另一个物体碰撞会产生形变的性质的物体。非刚性运动相较于刚性运动更加复杂，非刚性物体的运动分析已成为当今研究中的一个重要应用领域之一。非刚性运动分为两大类：有约束的非刚性运动和无约束的非刚性运动，有约束的非刚性运动包括连接体运动、类刚体运动等，无约束的非刚性运动包括弹性体运动和流体运动。本发明通过mpu6050检测非刚性物体的姿态数据，对非刚性物体姿态进行量化分析和安全评估，对于非刚性物体物理性质的测量和研究具有理论意义和现实意义。
技术实现思路
1、本发明针对单个mpu6050无法对非刚性物体进行姿态解算分析的问题，提出一种将两个mpu6050进行组合，检测非刚性物体的姿态安全的算法。
2、mpu6050的姿态解算过程包括加速度计姿态解算、陀螺仪姿态解算和数据融合。
3、速度计姿态解算：
4、加速度计可以测量mpu6050三个方向上的加速度分量，如图1所示。在静止或匀速直线运动的情况下mpu6050受到一个竖直向下的重力加速度，即三个方向的加速度向量相加等于1g，设加速度计测出的三轴加速度分量分别为ax，ay，az，则假定mpu6050旋转的顺规为z-y-x，航向角为y，俯仰角为p，横滚角为r。则其旋转矩阵为：
5、
6、三轴加速度分量相加等于重力，即向量[0 0 g]通过三轴旋转得到。那么可以推导：
7、
8、从而分别得出由加速度计测量计算出的
9、
10、
11、横滚角的公式中不包含重力加速度g可以排除不同地理位置的重力加速度不同取值对解算结果的影响。航向角位于水平面，mpu6050在水平面上的转动不会导致航向角变化，同时根据以上推导公式，可以得出航向角无法通过加速度计计算出。
12、陀螺仪姿态解算：
13、陀螺仪可以测量mpu6050三个轴转动的瞬时角速度。对角速度进行积分运算可以得到旋转的角度，即angle＝·∫gdt，其中angle为旋转的角度，g为角速度。mpu6050测量出的数据是离散的，根据积分定义，对每个瞬间时刻内的角速度进行累加运算，即可得到旋转的角度。计算每一时刻内所转动的角度，根据该时刻初和结束时的角速度组成的梯形的面积，比只依据单个角速度计算矩形面积更加准确，如图2所示。
14、假设从静止状态开始测量，记陀螺仪三轴测量的当前时刻角速度分别为gx、gy、gz，上一时刻角速度分别为数据读取间隔为δt，则由陀螺仪测量计算得出三个姿态角的变化量分别为
15、
16、
17、
18、数据融合：
19、加速度计和陀螺仪都存在误差。加速度计易受很多噪声，例如自身加速度，外在震动等影响，而陀螺仪对角速度的累加运算由于硬件的数据波动而存在累计误差，且无法在仅使用陀螺仪的情况下消除，所以需要对二者进行数据融合，从而减小误差，增加解算结果的可靠性。此外，由于加速度计无法解算出yaw值，所以无法对yaw值进行校准，除非引入磁力计。但是分析安全姿态对水平面上的航向角精度需求不是很高，存在误差可以接受。
20、采取卡尔曼滤波的方式对mpu6050进行姿态融合，通过先验估计的算法，相较于简单的一阶互补滤波数据融合法，可以避免硬件误差和数据抖动对数据可靠性的影响。步骤如下：
21、步骤1，进行先验估计运算。
22、根据先验估计公式为k时刻先验估计值，为(k-1)时刻的后验估计值，a为状态转移矩阵，b为控制变量矩阵，uk为状态控制向量，wk为控制系统的噪声。设angle和bias分别为当前时刻姿态角(pitch或roll)和陀螺仪误差的最优化估计值，angle-和bias-分别为当前时刻姿态角和陀螺仪误差的先验估计值，angle0和bias0分别为上一时刻姿态角和陀螺仪误差的后验估计值，δt为测量时间间隔，g测量为当前时刻测量出的角速度值，则有：
23、
24、步骤2，预测协方差矩阵。
25、公式其中为的协方差，pk-1为的协方差，at为a的转置矩阵，q为系统噪声wk的协方差。因为角度噪声和角速度漂移噪声相互独立，有设则有：
26、
27、步骤3，建立测量方程：
28、zk＝hxk+vk，
29、其中，zk为k时刻的测量值，转移矩阵h＝[1 0]，xk为k时刻的系统状态，vk为测量产生的噪声。因为陀螺仪输出自带噪声，所以zk＝g测量。
30、步骤4，计算卡尔曼增益。
31、根据卡尔曼增益计算公式其中kk为k时刻的卡尔曼增益值，r为测量噪声vk的协方差，有：
32、
33、其中，k0用作计算最优估计值，k1用作计算最优估计值的偏差，加速度计测量噪声的协方差rangle为常数。
34、步骤5，计算当前最优化估计值。
35、根据最优化估计值公式其中为k时刻的最优化估算值，有：
36、
37、步骤6，更新协方差矩阵。
38、根据公式其中i为单位矩阵，更新后的协方差矩阵为：
39、
40、同时在非刚性物体上配置两个mpu6050，并对mpu6050分别进行姿态解算，将结算结果加以组合，通过对6050的协同分析，利用局部坐标之间的相对关系分析姿态的安全性。
41、以两个mpu6050为例，设置其中一个mpu6050为主体。可以计算得出另一个分mpu6050相对于主体mpu6050的姿态，并由结算出的相对姿态判断部件的安全状态，计算公式如下：
42、
43、设加速度计解算出的主体mpu6050、部分mpu6050的三轴加速度分别为则有相对加速度根据相对加速度arel求相对位移srel，坐标方向与大地坐标系一致，设s0为初始位移，vrel为相对速度，v0为初始速度，则有公式
44、
45、写成矩阵形式表示两个mpu6050相对坐标有
46、
47、本发明使用了华为自主研发的装载了昇腾310处理器的开发板atalas 200dk用作数据处理。atalas200dk基于linux系统开发，计算效率高，可靠性强。atalas200dk拥有拓展的40pin接口可以用作外设连接，包括gpio、i2c、spi、uart等常用接口。本发明将mpu6050的驱动移植到了上atalas 200 dk，并在之上进行了测试，验证了姿态解算算法的可行性。
48、本发明的结果可视化有两种方案，两种方案的硬件设计框图如图3所示：
49、方案一，将数据结果以数字的形式直接通过lcd屏输出显示。本发明采用了st7789v芯片的lcd屏，通过适配移植linux内核中自带的fbtft驱动，配置相关参数，使用spi总线连接atalas 200 dk和lcd屏，成功将数据结果显示到了lcd屏上。
50、方案二，利用ch343连接atalas 200 dk的uart串口与电脑端的usb接口，将200dk解算的数据传输至电脑，通过processing图形设计语言对姿态进行图形可视化。