一种捷联式刚体三维姿态实时监测显示系统和方法与流程

本发明涉及姿态监测领域，尤其涉及一种捷联式刚体三维姿态实时监测显示系统和方法。

背景技术：

在工程、探测等领域，姿态信息的获取对提高探测精度具有重要意义，因此，在机械臂控制中需要实时获取机械臂的姿态信息作为反馈；卫星天线中需要对天线平台的三维姿态进行实时检测和控制。

在声呐探测和海洋磁力仪探测中，拖鱼体的姿态偏移是影响其探测精度的主要原因之一，常用的浮式及半潜式三维姿态监测显示系统，由于采用单一的加速度计作为姿态敏感元件，噪声大、积累误差严重、精度低；此外，虽然该系统提供了三维仿真模型显示，但不具备波形显示功能，无法满足观测者对姿态信息定量分析的需求，且不具有体积小巧、轻便的特点，无法与其他被测物体进行同轴捷联、组装。

技术实现要素：

有鉴于此，本发明的实施例提供了一种高精度，显示界面高效、直观，基于多传感器融合的捷联式刚体三维姿态实时监测显示系统和方法。

本发明的实施例提供一种捷联式刚体三维姿态实时监测显示系统，包括上位机和下位机，所述下位机包括：

姿态检测模块,用于获取刚体的九轴姿态数据；

数据处理模块，用于对原始数据滤波预处理、数据融合，得到欧拉角；

无线通信模块，用于将欧拉角数据打包发送至上位机；

所述姿态检测模块获取刚体的九轴姿态数据，并将数据传输给数据处理模块进行处理，经数据处理模块处理后得到欧拉角数据，所述无线通信模块将欧拉角数据传输给上位机；

所述上位机包括：

数据基本收发模块，用于实现基本的串口数据收发功能；

波形显示模块，用于以波形图的形式实时显示刚体三维姿态变化；

三维仿真模型动态显示模块，用于以三维仿真模型的形式实时动态的显示刚体三维姿态；

数据保存模块，用于保存当前的三维姿态数据和测试时间；

所述数据基本收发模块接收下位机传输的数据，并分别通过波形显示模块和三维仿真模型动态显示模块以波形图和三维仿真模型的形式实时显示，所述数据保存模块对三维姿态数据和测试时间进行保存。

进一步，所述姿态检测模块包括三轴加速度计、三轴陀螺仪和三轴磁力计，所述三轴加速度计、三轴陀螺仪和三轴磁力计组成九轴姿态传感器。

进一步，所述三轴加速度计、三轴陀螺仪和三轴磁力计均采用微机电系统传感器。

进一步，所述无线通信模块为蓝牙、zigbee、wifi或2.4g频段通信。

一种捷联式刚体三维姿态实时监测显示方法，包括以下步骤：

s1.保持下位机的轴向与刚体的轴向平行进行捷联，并建立刚体坐标系；

s2.将姿态检测模块初始化，并对三轴加速度计、三轴陀螺仪和三轴磁力计进行异常检测，若异常，则终止，若正常，则获取三轴加速度计、三轴陀螺仪和三轴磁力计的初始值并校准；

s3.数据处理模块获取刚体当前姿态下三轴加速度计、三轴陀螺仪和三轴磁力计的原始数据，并进行滤波预处理、数据融合、欧拉角解算，得到欧拉角；

s4.将步骤s3得到欧拉角数据通过无线通信模块传输给上位机，数据基本收发模块接收欧拉角数据；

s5.波形显示模块和三维仿真模型动态显示模块提取数据基本收发模块接收的数据，分别以波形图和三维仿真模型的形式实时显示刚体的三维姿态和姿态变化，并通过数据保存模块保存当前的三维姿态数据和测试时间。

进一步，所述数据处理模块根据三轴加速度计和三轴陀螺仪输出数据的不同特点采用不同的滤波算法进行预处理；所述三轴加速度计采用直接i型iir算法进行预处理，所述三轴陀螺仪采用一阶数字高通滤波算法进行预处理。

进一步，所述欧拉角包括横滚角、俯仰角以及偏航角，所横滚角和俯仰角是先通过一阶龙格库塔算法解四元数微分方程得到四元数，再进行欧拉角解算得到；所述偏航角是通过三轴陀螺仪z轴积分和三轴磁力计输出互补滤波得到。

与现有技术相比，本发明具有以下有益效果：采用九轴姿态融合的方案对刚体姿态进行检测，将检测精度控制在1°以内；上位机采用波形图、三维仿真模型等形式对姿态信息进行显示，可便捷、直观、高效的进行定量、定性分析；下位机采用微机电系统传感器，体积小巧、轻便，可与任何被测刚体进行同轴捷联、组装，具有广阔的应用前景。

附图说明

图1为本发明一种捷联式刚体三维姿态实时监测显示系统的示意图。

图2为三轴加速度计数据融合前后对比图。

图3为三轴陀螺仪数据融合前后对比图。

图4为本发明实例中三维姿态数据的波形显示图。

图5为本发明实例中三维姿态数据的三维仿真模型动态显示图。

图6为本发明实例中数据基本收发模块收发数据的示意图。

图7为本发明实例中数据保存模块数据保存的示意图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明实施方式作进一步地描述。

请参考图1，本发明的实施例提供了一种捷联式刚体三维姿态实时监测显示系统，包括上位机1和下位机2。

所述下位机2包括：

姿态检测模块21,用于获取刚体的九轴姿态数据；所述姿态检测模块21包括三轴加速度计211、三轴陀螺仪212和三轴磁力计213，所述三轴加速度计211、三轴陀螺仪212和三轴磁力计213组成九轴姿态传感器，三轴加速度计211、三轴陀螺仪212和三轴磁力计213均采用微机电系统传感器，保证整个下位机2具有体积小巧、轻便的特点，便于与被测物体进行捷联、装配。

数据处理模块22，用于对原始数据滤波预处理、数据融合，得到欧拉角；欧拉角包括横滚角roll、俯仰角pitch以及偏航角yaw。

无线通信模块23，用于将欧拉角数据打包发送至上位机1；在一实施例中，所述无线通信模块23为蓝牙、zigbee、wifi或2.4g频段通信。

所述姿态检测模块21获取刚体的九轴姿态数据，并将数据传输给数据处理模块22进行处理，经数据处理模块22处理后得到欧拉角数据，所述无线通信模块23将欧拉角数据传输给上位机1；

所述上位机1包括：

数据基本收发模块11，用于实现基本的串口数据收发功能；

波形显示模块12，用于以波形图的形式实时显示刚体三维姿态变化；

三维仿真模型动态显示模块13，用于以三维仿真模型的形式实时动态的显示刚体三维姿态；

数据保存模块14，用于保存当前的三维姿态数据和测试时间；

所述数据基本收发模块11接收下位机2传输的数据，并分别通过波形显示模块12和三维仿真模型动态显示模块13以波形图和三维仿真模型的形式实时显示，所述数据保存模块14对三维姿态数据和测试时间进行保存。

一种捷联式刚体三维姿态实时监测显示方法，包括以下步骤：

s1.保持下位机2的轴向与刚体的轴向平行进行捷联，并建立刚体坐标系；

s2.将姿态检测模块21初始化，并对三轴加速度计211、三轴陀螺仪212和三轴磁力计213进行异常检测，若异常，则终止，若正常，则获取三轴加速度计211、三轴陀螺仪212和三轴磁力计213的初始值并校准；

s3.数据处理模块22获取刚体当前姿态下三轴加速度计211、三轴陀螺仪212和三轴磁力计213的原始数据，并进行滤波预处理、数据融合、欧拉角解算，得到欧拉角；所述欧拉角的解算基于步骤s1建立的刚体坐标系；数据处理模块14根据三轴加速度计211和三轴陀螺仪212输出数据的不同特点采用不同的滤波算法进行预处理；所述三轴加速度计211采用直接i型iir算法进行预处理，所述三轴陀螺仪212采用一阶数字高通滤波算法进行预处理。

欧拉角包括横滚角、俯仰角以及偏航角，所横滚角和俯仰角是先通过一阶龙格库塔算法解四元数微分方程得到四元数，再进行欧拉角解算得到；所述偏航角是通过三轴陀螺仪212z轴积分和三轴磁力计213输出互补滤波得到。

三轴加速度计211数据融合前后对比图如图2所示，由图可知，经数据融合后，三轴加速度计211的高频噪声得到了有效的抑制，而且不丧失三轴加速度计211的灵敏性。

三轴陀螺仪212数据融合前后对比图如图3所示，由图可知，经数据融合后，三轴陀螺仪212的积累误差得到有效的抑制，而且不丧失三轴陀螺仪212的稳定性。

s4.将步骤s3得到欧拉角数据通过无线通信模块23传输给上位机1，数据基本收发模块11接收欧拉角数据；

s5.波形显示模块12和三维仿真模型动态显示模块13提取数据基本收发模块接收11的数据，分别以波形图和三维仿真模型的形式实时显示刚体的三维姿态和姿态变化，并通过数据保存模块14保存当前的三维姿态数据和测试时间。

刚体以拖鱼体为例，当驱使拖鱼体在水中做抖动时，三维姿态数据经预处理、融合、解算，无线发送，在上位机1的波形图显示如图4所示，通过波形图显示界面，不仅可以直观、便捷的获知姿态变化的整个过程，还可定量分析姿态数据。

在某瞬间拖鱼的三维姿态在仿真模型中的反映如图5所示，通过对仿真模型动态图的观察，可以形象、直观、实时的辨别出拖鱼体当前姿态。

此外，本发明实施例的上位机还具有数据基本收发功能和数据保存功能，其运行结果如图6所示，当按下位机2和上位机1界面上“文件保存”键时，将保存当前的三维姿态数据和测试时间(如图7所示)，所示三列数据依次为pitch、roll、yaw，为了便于在上位机1上显示，便于观察微小的变化，将真值扩大100倍。

本发明采用九轴姿态融合的方案对刚体姿态进行检测，将检测精度控制在1°以内；上位机采用波形图、三维仿真模型等形式对姿态信息进行显示，可便捷、直观、高效的进行定量、定性分析；下位机采用微机电系统传感器，体积小巧、轻便，可与任何被测刚体进行同轴捷联、组装，具有广阔的应用前景。

在本文中，所涉及的前、后、上、下等方位词是以附图中零部件位于图中以及零部件相互之间的位置来定义的，只是为了表达技术方案的清楚及方便。应当理解，所述方位词的使用不应限制本申请请求保护的范围。

在不冲突的情况下，本文中上述实施例及实施例中的特征可以相互结合。

以上所述仅为本发明的较佳实施例，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。