CN109813312A - 一种卡片式测量物体姿态角的测量装置及方法 - Google Patents

Abstract

本发明公开了一种卡片式测量物体姿态角的测量装置及方法，该装置包括陀螺仪传感器模块、MCU微控制器模块、无线传输模块、数据显示模块、电源模块。在被测物体上放置该卡片式测量装置，其中的陀螺仪传感器能够测量物体的姿态角信息，MCU微控制器对陀螺仪传感器测量的数据进行卡尔曼滤波，并把滤波后得到的姿态角信息显示在OLED屏幕上，以及通过无线传输模块发送给运行在智能设备上的上位机进行记录、存储和后续分析。本发明显著的降低了测量物体姿态角时所需的硬件成本，降低了测量的复杂性，提高了测量的灵活性，而且能够通过无线的方式实时准确地测量到待测物体的姿态角，并且通过卡尔曼滤波提高了测量精度。

Description

一种卡片式测量物体姿态角的测量装置及方法

技术领域

本发明属于姿态角测量技术领域，特别涉及一种卡片式测量物体姿态角的测量装置及方法。

背景技术

物体的姿态角信息是无人机、机器人、平衡车和智能自动化农业机械设备等必不可少的反馈信息，此外，在机械加工、安装等过程中也需要知道物体的姿态角信息，以达到最优安装位置。

随着科技技术的发展和进步，无人机、平衡车等设备逐渐走入寻常百姓家中，然而这些设备能够完成正常工作，精确定位的基础，就依赖于能够实时反馈其自身姿态角信息。此外，在物体安装过程中，往往需要物体精准的姿态角信息，以确保被安装物体处于一个最优的位置。随着这类姿态角测量系统应用范围的不断扩展，对测量系统也提出了较高的要求，即希望测量精度高，适用范围广，又希望测量过程简单高效。目前物体的姿态角测量主要分为以下几种方式：

使用倾角传感器测量物体的姿态角，其原理为依靠惯性力一般为重力的分量大小测量水平陀螺仪，从此类传感器的设计结构上可分为以下三种“固体摆传感器”、“液体摆传感器”以及“气体摆传感器”，此类传感器结构简单，但不能实现全姿态工作，由于其工作原理主要依靠惯性力，所以只适合在静态工作环境下。

使用陀螺仪传感器测量物体的姿态角，其通过测量物体运动过程中各轴的转动角速度大小，并对其积分得到物体的姿态角，此类传感器主要分为传统机械陀螺仪、激光陀螺仪，光纤陀螺仪以及微机电(MEMS)惯性测量仪。

申请号为201710119502.X，名称为“一种测量物体姿态角的方法和装置”的中国专利中，其通过测量待测物体上的磁场特征量，以及待测物体所在地理坐标系下的磁场特征量作为基准值，根据两者计算待测物体的姿态角。然而该方法的适用范围受到很大的局限性，比如不能测量含有磁性物体的姿态角以及不能测量在有磁性附近物体的姿态角。除此之外，该方法需要得到不同地理坐标下的磁场特征量的基准值，这需要极大的工作量，由于该方法并没有考虑测量误差的影响，所以测量精度有限。

申请号为201621304083.4，名称为“一种天线姿态角的测量装置”的中国专利中，通过微控制器、陀螺仪传感器、支撑组件、安装架去测量天线姿态角，在测量时需要把该测量装置安装在天线的抱杆上，利用行程换算法测量天线方位角，结合陀螺仪传感器测量天线倾斜角，从而测到天线的姿态角，此方法虽然不受磁场干扰，但是结构复杂，操作繁琐，测量效率低，应用范围受限，而且改方法也没有考虑陀螺仪传感器测量误差的影响，所以测量精度有限。

综上，目前测量物体姿态角的现有方案存在操作复杂、精度差、成本高等问题。

发明内容

本发明所解决的技术问题在于提供一种成本低、体积小、测试便捷、精度高的测量物体姿态角的测量装置及方法。

实现本发明目的的技术解决方案为：一种卡片式测量物体姿态角的测量装置，该测量装置为卡片式结构，包括位于该测量装置中心的陀螺仪传感器模块，位于陀螺仪传感器模块所在水平面且处于同一直线上的MCU微控制器模块和无线传输模块，分别位于陀螺仪传感器模块上方和下方的数据显示模块和电源模块；

所述陀螺仪传感器模块，用于采集待测物体的姿态角数据，并将该数据传输至MCU微控制器模块；

所述MCU微控制器模块，用于对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块、数据显示模块；

所述无线传输模块，用于将接收到的姿态角数据传输至外部上位机，由上位机对接收到的姿态角数据进行记录、存储与后续分析；

所述数据显示模块，用于显示接收到的姿态角数据信息；

所述电源模块，用于为陀螺仪传感器模块、MCU微控制器模块、无线传输模块、数据显示模块供电，且该电源模块可充电。

基于上述卡片式测量物体姿态角的测量装置的测量方法，包括以下步骤：

步骤1、陀螺仪传感器模块采集待测物体的姿态角数据，并将该数据发送至MCU微控制器模块；

步骤2、MCU微控制器模块对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块、数据显示模块；

步骤3、无线传输模块将接收到的姿态角数据传输至外部上位机，由上位机对姿态角数据进行记录、存储与后续分析。

本发明与现有技术相比，其显著优点为：1)本发明中通过MEMS传感器测量物体姿态角，相比于现有方案，能够实现测量磁性物体的姿态角，该测量装置应用范围广；2)本发明中通过MCU微控制器对测量的物体的姿态角数据进行卡尔曼滤波，提高了测量精度；3)本发明的测量装置，内部设有可充电的供电电源，无需其它辅助装置，可独立连续工作；4)本发明显著的降低了测量物体姿态角时所需的硬件成本，降低了测量的复杂性；5)本发明的测量装置为卡片式结构，具有体积小，重量轻等优点。

下面结合附图对本发明作进一步详细的描述。

附图说明

图1为本发明卡片式测量物体姿态角的测量装置的结构示意图；其中，1-陀螺仪传感器、2-MCU微控制器、3-无线传输模块、4-数据显示模块、5-电源模块、6-PCB电路板、7-充电接口。

图2为本发明卡片式测量物体姿态角的测量装置模块流程框图。

图3为本发明MCU对陀螺仪数据进行卡尔曼滤波仿真轨迹比较图。

图4为本发明MCU对陀螺仪数据进行卡尔曼滤波仿真误差比较图。

具体实施方式

结合图1，本发明卡片式测量物体姿态角的测量装置为卡片式结构，包括位于该测量装置中心的陀螺仪传感器模块1，位于陀螺仪传感器模块1所在水平面且处于同一直线上的MCU微控制器模块2和无线传输模块3，分别位于陀螺仪传感器模块1上方和下方的数据显示模块4和电源模块5；

陀螺仪传感器模块1，用于采集待测物体的姿态角数据，并将该数据传输至MCU微控制器模块2；

MCU微控制器模块2，用于对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块3、数据显示模块4；

无线传输模块3，用于将接收到的姿态角数据传输至外部上位机，由上位机对接收到的姿态角数据进行记录、存储与后续分析；

数据显示模块4，用于显示接收到的姿态角数据信息；

电源模块5，用于为陀螺仪传感器模块1、MCU微控制器模块2、无线传输模块3、数据显示模块4供电，且该电源模块5可充电。

进一步优选地，陀螺仪传感器模块1包括：

用于测量物体3轴角速度的3轴MEMS陀螺仪；

用于测量物体3轴角加速度的3轴MEMS加速度计；

用于对角速度和角加速度进行运算以减小MCU微控制器模块运算量的可扩展的数字运动处理器DMP。

示例性优选地，无线传输模块3为蓝牙模块或2.4G Wi-Fi模块或5G Wi-Fi模块。

示例性优选地，数据显示模块4为OLED显示屏。

示例性优选地，电源模块5为可充电锂电池。

示例性优选地，该测量装置为长方体结构或圆柱体结构。

结合图2，本发明卡片式测量物体姿态角的测量装置的测量方法，包括以下步骤：

步骤1、陀螺仪传感器模块采集待测物体的姿态角数据，并将该数据发送至MCU微控制器模块；

步骤2、MCU微控制器模块对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块、数据显示模块；

步骤3、无线传输模块将接收到的姿态角数据传输至外部上位机，由上位机对姿态角数据进行记录、存储与后续分析。

进一步地，步骤2中对物体的姿态角数据进行卡尔曼滤波，具体为：

步骤2-1、建立测量物体姿态角的测量系统的状态空间模型；具体为：

(1)建立观测模型为：

y(k)＝x(k)+v(k)

式中，x(k)表示物体在第k时刻的真实姿态角信息，y(k)表示第k时刻陀螺仪传感器输出的测量值，v(k)表示陀螺仪传感器测量的误差，假设其是零均值、方差为的白噪声；

其中，

x(k)＝[θ_xk,ω_xk,β_xk,θ_yk,ω_yk,β_yk,θ_zk,ω_zk,β_zk]^T

式中，假设在第kT₀时刻时，物体的三轴姿态角分别为θ_xk,θ_yk,θ_zk，三轴姿态角速度分别为ω_xk,ω_yk,ω_zk，三轴姿态角加速度分别为β_xk,β_yk,β_zk；

(2)根据现有的运动学方程：

式中，θ为物体角度，ω为物体角速度，β为物体的角加速度，α为物体角急动度，θ₀为初始角度，ω₀为初始角速度，β₀为初始角加速度；

获得测量系统的状态空间模型：

式中，u(k)为物体自身的急动度，w(k)为随机急动度，A为状态转移矩阵，B为将输入转换为状态的矩阵，H是状态变量到测量变量的转换矩阵，Γ为将预测噪声转换为状态的矩阵；其中，

式中，T为测量系统的测量时间间隔；

步骤2-2、根据卡尔曼滤波原理建立时间更新方程为：

式中，Q是系统过程的协方差矩阵，为第k时刻的后验状态估计值，为第k时刻的先验状态估计值，P_k表示第k时刻的后验估计协方差，为第k时刻的先验估计协方差；

步骤2-3、根据卡尔曼滤波原理建立测量更新方程为：

式中，K_k为第k时刻的卡尔曼增益系数，z_k为第k时刻的测量值即滤波的输入，R为测量噪声协方差，I为单位矩阵；

步骤2-4、结合上述过程中建立的状态空间模型、时间更新方程、测量更新方程对物体的姿态角数据进行卡尔曼滤波。

下面结合实施例对本发明作进一步详细的描述。

实施例

结合图1，本发明实施例提供了一种卡片式测量物体姿态角的测量装置，该装置为卡片式长方体结构，包括位于该测量装置中心的陀螺仪传感器模块1，位于陀螺仪传感器模块1所在水平面且处于同一直线上的MCU微控制器模块2和无线传输模块3，分别位于陀螺仪传感器模块1上方和下方的数据显示模块4和电源模块5。

陀螺仪传感器模块1，用于采集待测物体的姿态角数据，并将该数据传输至MCU微控制器模块2；本实施例中采用的陀螺仪传感器型号为MPU-6050。

MCU微控制器模块2，用于对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块3、数据显示模块4。

无线传输模块3，用于将接收到的姿态角数据传输至外部上位机，由上位机对接收到的姿态角数据进行记录、存储与后续分析；本实施例中该模块具体采用2.4G Wi-Fi模块，型号为ESP8266，且该模块中直接包含了MCU微控制器。

数据显示模块4，用于显示接收到的姿态角数据信息；本实施例中该模块具体采用OLED显示屏。

电源模块5，用于为陀螺仪传感器模块1、MCU微控制器模块2、无线传输模块3、数据显示模块4供电，且该电源模块5可充电。本实施例中该模块具体采用可充电锂电池。

结合图2，可知该卡片式测量物体姿态角的测量装置的测量方法为：MCU微控制器获取陀螺仪传感器的数据，并对其进行卡尔曼滤波，最后显示在OLED屏幕上，以及通过无线传输的方式发送给运行在智能设备上的上位机上，可以对数据进行记录、分析。

其中，对物体的姿态角数据进行卡尔曼滤波，具体为：

(1)建立测量物体姿态角的测量系统的状态空间模型；具体为：

1)建立观测模型为：

y(k)＝x(k)+v(k)

式中，x(k)表示物体在第k时刻的真实姿态角信息，y(k)表示第k时刻陀螺仪传感器输出的测量值，v(k)表示陀螺仪传感器测量的误差，假设其是零均值、方差为的白噪声；

其中，

x(k)＝[θ_xk,ω_xk,β_xk,θ_yk,ω_yk,β_yk,θ_zk,ω_zk,β_zk]^T

式中，假设在第kT₀时刻时，物体的三轴姿态角分别为θ_xk,θ_yk,θ_zk，三轴姿态角速度分别为ω_xk,ω_yk,ω_zk，三轴姿态角加速度分别为β_xk,β_yk,β_zk；

2)根据现有的运动学方程：

式中，θ为物体角度，ω为物体角速度，β为物体的角加速度，α为物体角急动度，θ₀为初始角度，ω₀为初始角速度，β₀为初始角加速度；

获得测量系统的状态空间模型：

式中，u(k)为物体自身的急动度，w(k)为随机急动度，A为状态转移矩阵，B为将输入转换为状态的矩阵，H是状态变量到测量变量的转换矩阵，Γ为将预测噪声转换为状态的矩阵；其中，

式中，T为测量系统的测量时间间隔；

(2)根据卡尔曼滤波原理建立时间更新方程为：

式中，Q是系统过程的协方差矩阵，为第k时刻的后验状态估计值，为第k时刻的先验状态估计值，P_k表示第k时刻的后验估计协方差，为第k时刻的先验估计协方差；

(3)根据卡尔曼滤波原理建立测量更新方程为：

式中，K_k为第k时刻的卡尔曼增益系数，z_k为第k时刻的测量值即滤波的输入，R为测量噪声协方差，I为单位矩阵；

(4)结合上述过程中建立的状态空间模型、时间更新方程、测量更新方程对物体的姿态角数据进行卡尔曼滤波。

结合图3，可以比较直观的看出，相比于测量数据，卡尔曼滤波后的数据更加贴近于真实数据，结合图4，则更加清楚的得出，滤波后的数据误差更加小于滤波前数据误差，由此可知，对数据做卡尔曼滤波后，能够提高测量精度。

综上，本发明卡片式测量物体姿态角的测量装置及方法，显著的降低了测量物体姿态角时所需的硬件成本，降低了测量的复杂性，而且能够通过无线的方式实时准确地测量到待测物体的姿态角，并且通过卡尔曼滤波提高了测量精度。

Claims (8)

1.一种卡片式测量物体姿态角的测量装置，其特征在于，该测量装置为卡片式结构，包括位于该测量装置中心的陀螺仪传感器模块(1)，位于陀螺仪传感器模块(1)所在水平面且处于同一直线上的MCU微控制器模块(2)和无线传输模块(3)，分别位于陀螺仪传感器模块(1)上方和下方的数据显示模块(4)和电源模块(5)；

所述陀螺仪传感器模块(1)，用于采集待测物体的姿态角数据，并将该数据传输至MCU微控制器模块(2)；

所述MCU微控制器模块(2)，用于对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块(3)、数据显示模块(4)；

所述无线传输模块(3)，用于将接收到的姿态角数据传输至外部上位机，由上位机对接收到的姿态角数据进行记录、存储与后续分析；

所述数据显示模块(4)，用于显示接收到的姿态角数据信息；

所述电源模块(5)，用于为陀螺仪传感器模块(1)、MCU微控制器模块(2)、无线传输模块(3)、数据显示模块(4)供电，且该电源模块(5)可充电。

2.根据权利要求1所述的卡片式测量物体姿态角的测量装置，其特征在于，所述陀螺仪传感器模块(1)包括：

用于测量物体3轴角速度的3轴MEMS陀螺仪；

用于测量物体3轴角加速度的3轴MEMS加速度计；

用于对角速度和角加速度进行运算以减小MCU微控制器模块运算量的可扩展的数字运动处理器DMP。

3.根据权利要求2所述的卡片式测量物体姿态角的测量装置，其特征在于，所述无线传输模块(3)为蓝牙模块或2.4G Wi-Fi模块或5G Wi-Fi模块。

4.根据权利要求3所述的卡片式测量物体姿态角的测量装置，其特征在于，所述数据显示模块(4)为OLED显示屏。

5.根据权利要求4所述的卡片式测量物体姿态角的测量装置，其特征在于，所述电源模块(5)为可充电锂电池。

6.根据权利要求1所述的卡片式测量物体姿态角的测量装置，其特征在于，该测量装置为长方体结构或圆柱体结构。

7.基于权利要求1所述的卡片式测量物体姿态角的测量装置的测量方法，其特征在于，包括以下步骤：

步骤1、陀螺仪传感器模块采集待测物体的姿态角数据，并将该数据发送至MCU微控制器模块；

步骤2、MCU微控制器模块对接收到的物体的姿态角数据进行卡尔曼滤波，并将滤波后的姿态角数据传输至无线传输模块、数据显示模块；

步骤3、无线传输模块将接收到的姿态角数据传输至外部上位机，由上位机对姿态角数据进行记录、存储与后续分析。

8.根据权利要求7所述的卡片式测量物体姿态角的测量方法，其特征在于，步骤2所述对物体的姿态角数据进行卡尔曼滤波，具体为：

步骤2-1、建立测量物体姿态角的测量装置的状态空间模型；具体为：

(1)建立观测模型为：

y(k)＝x(k)+v(k)

式中，x(k)表示物体在第k时刻的真实姿态角信息，y(k)表示第k时刻陀螺仪传感器输出的测量值，v(k)表示陀螺仪传感器测量的误差，假设其是零均值、方差为σ_v ²的白噪声；

其中，

x(k)＝[θ_xk,ω_xk,β_xk,θ_yk,ω_yk,β_yk,θ_zk,ω_zk,β_zk]^T

式中，假设在第kT₀时刻时，物体的三轴姿态角分别为θ_xk,θ_yk,θ_zk，三轴姿态角速度分别为ω_xk,ω_yk,ω_zk，三轴姿态角加速度分别为β_xk,β_yk,β_zk；

(2)根据现有的运动学方程：

式中，θ为物体角度，ω为物体角速度，β为物体的角加速度，α为物体角急动度，θ₀为初始角度，ω₀为初始角速度，β₀为初始角加速度；

获得测量装置的状态空间模型：

式中，u(k)为物体自身的急动度，w(k)为随机急动度，A为状态转移矩阵，B为将输入转换为状态的矩阵，Γ为将预测噪声转换为状态的矩阵，H为状态变量到测量变量的转换矩阵；其中，

式中，T为测量系统的测量时间间隔；

步骤2-2、根据卡尔曼滤波原理建立时间更新方程为：

式中，Q为系统过程的协方差矩阵，为第k时刻的后验状态估计值，为第k时刻的先验状态估计值，P_k表示第k时刻的后验估计协方差，为第k时刻的先验估计协方差；

步骤2-3、根据卡尔曼滤波原理建立测量更新方程为：

式中，K_k为第k时刻的卡尔曼增益系数，z_k为第k时刻的测量值即滤波的输入，R为测量噪声协方差，I为单位矩阵；

步骤2-4、结合上述过程中建立的状态空间模型、时间更新方程、测量更新方程对物体的姿态角数据进行卡尔曼滤波。