本发明为一种基于惯性测量元件的三维定位系统，该系统包括数据采集终端与服务器端；所述数据采集终端包括传感器模块、第一微控制器、第一无线模块，所述第一微控制器的输入端连接传感器模块，第一微控制器的输出端连接第一无线模块，传感器模块、第一微控制器、第一无线模块集成在一起，数据采集终端固定在行人腰部；所述服务器端包括第二无线模块、第二微控制器、PC机，所述第二微控制器的输入端通过第二无线模块与数据采集终端的第一无线模块连接，第二微控制器的输出端连接PC机；所述传感器模块包括9轴惯性传感器模块和气压传感器模块。该系统能实时检测位于建筑物内消防员的运动状态，实现对消防员的三维定位。
1.一种基于惯性测量元件的三维定位系统，其特征在于该系统包括数据采集终端与服务器端；所述数据采集终端包括传感器模块、第一微控制器、第一无线模块，所述第一微控制器的输入端连接传感器模块，第一微控制器的输出端连接第一无线模块，传感器模块、第一微控制器、第一无线模块集成在一起，数据采集终端固定在行人腰部；所述服务器端包括第二无线模块、第二微控制器、PC机，所述第二微控制器的输入端通过第二无线模块与数据采集终端的第一无线模块连接，第二微控制器的输出端连接PC机；所述传感器模块包括9轴惯性传感器模块和带温度补偿的气压传感器模块，所述第一微控制器内加载有行人航位推算方法；
1)上电后，完成系统初始化、步数清零、获取初始欧拉角，开启第一微控制器中的定时器，定时25ms，定时时间到达之后对行人行走过程中的线性加速度进行采样，获取行人的线性加速度；
2)根据线性加速度的值判断是否达到计步阈值，若线性加速度的值不小于计步阈值则返回继续判断是否到达定时时间；若线性加速度的值小于计步阈值，则使用滑动窗口的方法求谷值，再判断当前采样点是否为谷值，若不为谷值则返回继续判断是否到达定时时间；
若为谷值，则达到计步条件，步数加一，再利用线性加速度采样点的谷值结合行人的步频对步长进行估计；
3)延时250ms，判断延时时间是否到达，若到达延时时间则进入步骤4)；
4)读取9轴惯性传感器模块中当前欧拉角的值，结合初始欧拉角对当前的欧拉角进行校正，然后通过第一无线模块将校正后的欧拉角、步骤2)得到的估计后步长、从气压传感器模块中读取的温度及高度信息发送到服务器端。
2.根据权利要求1所述的基于惯性测量元件的三维定位系统，其特征在于所述行人航位推算方法包括步伐检测、步长推算、方向推算三个部分，通过行人身上携带的9轴惯性传感器模块中的三轴加速度计的输出数据与计步阈值比较，判断行人是否跨出一步，若成功检测到行人跨出一步，则根据行人走动时9轴惯性传感器模块的航向角输出 和估计的步长l来计算行人跨步完成后的位置坐标；若已知行人第k-1步的位置坐标为(Xk-1,Yk-1)，则其第k步的位置坐标(Xk，Yk)为：
3.根据权利要求1所述的基于惯性测量元件的三维定位系统，其特征在于9轴惯性传感器模块的型号为LPMS-ME1，气压传感器模块的型号为BMP180；所述第一微控制器的核心芯片为STM32芯片；所述第二微控制器的核心芯片选用STM8L101芯片；所述第一无线模块和第二无线模块为CC1101无线数据传输模块。
4.根据权利要求3所述的基于惯性测量元件的三维定位系统，其特征在于所述第一微控制器的核心芯片为STM32F103ZET6芯片。
5.根据权利要求1所述的基于惯性测量元件的三维定位系统，其特征在于，计步阈值设定为-0.23。
6.根据权利要求1所述的基于惯性测量元件的三维定位系统，其特征在于服务器端的工作方式为：
1)系统初始化，导入Python中的串口、matplotlib模块，定义串口子线程，以七个字节为单位接收串口数据；
2)判断在3s内是否接收到7个字节的串口数据，如果没有接收到，则程序结束，如果接收到，则根据接收到的串口数据中第二个字节的值，继续判断第二个字节是否为0x00～
3)若第二个字节为0x00～0x02，则调用程序中的cal_position方法，再判断第二个字节是否为0x00，若第二个字节为0x00，表示步数清零，再返回判断在3s内是否接收到7个字节的串口数据；若第二个字节不为0x00，则判断第二个字节是否为0x01，若第二个字节为
0x01，表示步数加一并获取航向角，再返回判断在3s内是否接收到7个字节的串口数据；
若第二个字节不为0x01，则获取步长计算行人二维坐标，实时更新二维定位图，程序结束；
若第二个字节不为0x00～0x02，则判断第二个字节是否为0x10，若第二个字节不为
0x10，则判断第二个字节是否为0x20，若第二个字节为0x20，则调用程序中的altitude方法，获取高度信息并将高度信息打印到命令行窗口，再返回判断在3s内是否接收到7个字节的串口数据；若第二个字节不为0x20则返回判断在3s内是否接收到7个字节的串口数据；
若第二个字节为0x10时，调用程序中的temperature方法，获取温度信息并将温度信息打印到命令行窗口，再返回判断在3s内是否接收到7个字节的串口数据。
一种基于惯性测量元件的三维定位系统
技术领域
[0001] 本发明所属三维定位技术领域，具体涉及一种基于惯性测量元件的三维定位系统。
背景技术
[0002] 随着经济建设高速发展，城市规模不断扩大，社会财富大量积累，城市火灾、突发灾害事故等发生几率逐年增高，各类灾害对人类社会所造成的危害愈发触目惊心。
[0003] 现代城市火灾与突发灾害事故的特点表现在：一是：连锁性增强，一旦发生灾害，很容易出现连锁灾害或衍生灾害，形成大型、恶性火灾或重大灾害事故；二是：易燃易爆材料较多，火灾蔓延速度极快，过火面积迅速扩大，灾害事故殃及面多且广，指挥决策失误或者扑救不及时，在很短时间就可能酿成无法估量的生命和财产损失，还可能造成严重的环境污染和人们的负面心理效应，直接影响社会安定和经济发展，造成重大经济损失和较大政治影响。三是：灾害突发地点建筑布局复杂，楼宇结构不清楚，对消防部队的现场指挥和消防人员定位等方面都是新的巨大的考验，现场应急指挥通信保障能力及对灾害地点建筑格局和楼宇结构的了解能力等在这样的情况下暴露出明显的不足。
[0004] 因而，针对城市火灾与突发灾害事故的灭火救援力量的调度、指挥、部署、增援、协同作战等都提出了新的更高的要求。根据城市火灾与突发灾害事故的特点，研制新型的消防员室内定位系统，是消防救援力量调度指挥的基础，也是保障消防人员安全的硬件基础，具有重要的经济价值和社会价值。
[0005] 目前已有的三维定位系统大致可以分为以下几类：
[0006] (1)基于无线信号发射设备的WiFi，RFID定位方式。这一类定位方式，主要通过接收WiFi，RFID发射的无线信号进行定位。但是由于火场环境的不确定性很高，无法提前布置无线信号发射设备，所以这一类定位方式无法应用于未知环境。
[0007] (2)基于惯性导航(MEMS惯性传感器)的定位方式。惯性传感器无法提供行人的高度信息，所以单纯的使用惯性传感器无法实现行人的三维定位，比如2015年苏菲(苏菲,金志刚,王柄鉴.基于惯性传感器的便携式消防员搜救系统[J].计算机应用研究,2015,32(12):3677-3681.)提出的基于惯性传感器的便携式消防员搜救系统，选用微陀螺仪、微加速度计集成传感器和单轴/双轴磁阻传感器对消防员的运动数据进行采集，用STM32F103RBT6微控制器处理数据。
发明内容
[0008] 本发明的目的是提供一种基于惯性测量元件的三维定位系统，其可以实时检测位于建筑物内消防员的运动状态，采集消防员的运动数据，结合行人航位推算方法和从传感器模块中采集的高度信息对消防员进行三维定位。
[0009] 本发明的技术方案是：
[0010] 一种基于惯性测量元件的三维定位系统，其特征在于该系统包括数据采集终端与服务器端；所述数据采集终端包括传感器模块、第一微控制器、第一无线模块，所述第一微控制器的输入端连接传感器模块，第一微控制器的输出端连接第一无线模块，传感器模块、第一微控制器、第一无线模块集成在一起，数据采集终端固定在行人腰部；所述服务器端包括第二无线模块、第二微控制器、PC机，所述第二微控制器的输入端通过第二无线模块与数据采集终端的第一无线模块连接，第二微控制器的输出端连接PC机；所述传感器模块包括9轴惯性传感器模块和带温度补偿的气压传感器模块，所述第一微控制器内加载有行人航位推算方法。
[0011] 与现有技术相比，本发明的有益效果为：
[0012] 1)本申请采用惯性传感器与气压传感器相结合的方式，并且将数据采集终端固定在腰上，使用单轴加速度的周期性对行人的运动状态进行检测，可以在一定程度上使定位数据解算简单化，同时使用的9轴惯性传感器模块精度更高，输出数据的稳定性更好。
[0013] 2)本发明提供一种基于惯性测量元件的三维定位系统，采用模块化设计，将系统分为了数据采集终端与服务器端两个部分，定位信息的获取与初步解算在数据采集终端进行，定位信息的综合计算与显示在服务器端进行，降低了对数据采集终端的性能要求，使系统简易化。
[0014] 3)本发明利用传感器模块实时采集消防员行走时的惯性参量、所处高度等信息，结合行人航位推算算法，可以实时计算出消防员的航向、相对位置信息；无线模块将数据采集终端采集到的定位数据实时传输到服务器端，经过Python的串口模块接收之后进行再处理，可以直观的将消防员的定位结果显示在服务器端。经过实际测试结果得知，行人正常行走100m，定位误差在1.5m以内，实现了本系统的功能。
[0015] 3)本申请系统可以用于正常情况下室内外的三维定位、发生火灾的情况下建筑物内消防员的三维定位等，能应用于未知环境。
附图说明
[0016] 图1为本发明提供的一种基于惯性测量元件的三维定位系统的硬件框图；
[0017] 图2为本发明提供的一种基于惯性测量元件的三维定位系统数据采集终端的软件流程图；
[0018] 图3为本发明提供的一种基于惯性测量元件的三维定位系统服务器端的软件流程图；
[0019] 图4为本发明提供的一种基于惯性测量元件的三维定位系统仿真实验结果；
[0020] 图中，1数据采集终端、2服务器端、11传感器模块、12第一微控制器、13第一无线模块、21第二无线模块、22第二微控制器、23PC机。
具体实施方式
[0021] 为了便于理解本发明的技术方案，将在以下描述作进一步说明。但本发明的保护范围不限于下述的实施例。
[0022] 参照图1所示：一种基于惯性测量元件的三维定位系统，包括数据采集终端1与服务器端2；所述数据采集终端包括传感器模块11、第一微控制器12、第一无线模块13，所述第一微控制器的输入端连接传感器模块，第一微控制器的输出端连接第一无线模块，传感器模块11、第一微控制器12、第一无线模块13集成在一起，数据采集终端固定在行人腰部；所述服务器端包括第二无线模块21、第二微控制器22、PC机23，所述第二微控制器的输入端通过第二无线模块与数据采集终端的第一无线模块连接，第二微控制器的输出端连接PC机；
[0023] 所述第一微控制器内加载有行人航位推算方法，第一微控制器用来采集传感器模块的数据，并对这些数据进行初步的处理，计算出消防员的步长、校正欧拉角等；控制第一无线模块将步长、温度、高度信息以及校正后的欧拉角发送到服务器端。
[0024] 所述第二微控制器用于控制第二无线模块接收数据采集终端发送来的数据，然后将这些数据发送到PC机的串口。
[0025] 所述惯性测量元件为9轴惯性传感器模块。
[0026] 所述行人航位推算方法是一个根据行人步长和航向计算其距离室内已知初始位置的相对行走距离的算法，包括步伐检测、步长推算、方向推算三个部分。通过行人身上携带的9轴惯性传感器模块中的三轴加速度计的输出数据与计步阈值比较，判断行人是否跨出一步，若成功检测到行人跨出一步，则根据行人走动时惯性传感器的航向角输出 和估计的步长l来计算行人跨步完成后的位置坐标。若已知行人第k-1步的位置坐标为(Xk-1,Yk-1)，则其第k步的位置坐标(Xk，Yk)为：
[0027]
[0028] 所述步伐检测采用过阈值检测法，并利用一段时间的延时来排除因人体抖动造成的误计步，所述计步阈值的具体数值大小通过实验获得。
[0029] 其中，所述传感器模块包括9轴惯性传感器模块和带温度补偿的气压传感器模块，9轴惯性传感器模块的型号为LPMS-ME1，气压传感器的型号为BMP180。所述第一微控制器12的核心芯片为STM32芯片，主频速率为70M以上，优选STM32F103ZET6芯片；所述第二微控制器22的核心芯片选用STM8L101芯片。所述第一无线模块和第二无线模块为支持空闲信道评估(CCA)功能的、工作在433MHz的无线数据传输模块CC1101。
[0030] 使用时，将数据采集终端固定在行人腰部，使用第一微控制器从低成本的9轴惯性传感器模块(三轴加速度计，三轴陀螺仪和三轴磁力计)获取行人行走时的惯性参量、从气压传感器模块获取高度信息，经预处理后，通过第一无线模块、第二无线模块将预处理后的结果传输到第二微控制器，经服务器端的第二微控制器再处理之后直观的将消防员的定位结果显示在PC机上。
[0031] 参照图2所示：本发明基于惯性测量元件的三维定位系统的数据采集终端的工作方式为：
[0032] 1)上电后，完成系统初始化、步数清零、初始欧拉角获取(初始欧拉角即为欧拉角的校正值)，开启第一微控制器中的定时器，定时25ms，定时时间到达之后对消防员行走过程中的线性加速度进行采样，获取行人的线性加速度；
[0033] 2)根据线性加速度的值判断是否达到计步阈值，本实施例中三轴加速度计佩戴在腰部时，其x轴向下，计步阈值设定为-0.23，即判断x轴线性加速度的值是否小于-0.23，若x轴线性加速度的值不小于-0.23则返回继续判断是否到达定时时间；若x轴线性加速度的值是小于-0.23，则使用滑动窗口的方法(窗口长度为2，窗口每向右滑动一次便检测一次该窗口中连续两个采样点值中的较小值，直至检测到局部的极小值)求谷值，再判断当前采样点是否为谷值，若不为谷值则返回继续判断是否到达定时时间；若为谷值，则达到计步条件，步数加一，再利用线性加速度采样点的谷值结合消防员的步频对步长进行估计；
[0034] 3)延时250ms，排除因为人体抖动等原因造成的计步误差，判断延时时间是否到达，若到达延时时间则进入步骤4)；
[0035] 4)读取9轴惯性传感器模块中当前欧拉角的值，结合初始欧拉角对当前的欧拉角进行校正，然后通过第一无线模块将校正后的欧拉角、步骤2)得到的估计后步长、从气压传感器中读取的温度及高度信息发送到服务器端。
[0036] 参照图3所示：本发明基于惯性测量元件的三维定位系统的服务器端的工作方式为：
[0037] 1)系统初始化，导入Python中的串口、matplotlib等模块，定义串口子线程，以七个字节为单位接收串口数据。
[0038] 2)判断在3s内是否接收到7个字节的串口数据，如果没有接收到，则程序结束，如果接收到，则根据接收到的串口数据中第二个字节的值，调用不同的方法来处理接收到的数据，即继续判断第二个字节是否为0x00～0x02；
[0039] 3)若第二个字节为0x00～0x02，则调用程序中的cal_position方法，再判断第二个字节是否为0x00，若第二个字节为0x00，表示步数清零，再返回判断在3s内是否接收到7个字节的串口数据；若第二个字节不为0x00，则判断第二个字节是否为0x01，若第二个字节为0x01，表示步数加一并获取航向角，再返回判断在3s内是否接收到7个字节的串口数据；
[0040] 若第二个字节不为0x01，则获取步长计算行人二维坐标，实时更新二维定位图，程序结束；
[0041] 若第二个字节不为0x00～0x02，则判断第二个字节是否为0x10，若第二个字节不为0x10，则判断第二个字节是否为0x20，若第二个字节为0x20，则调用程序中的altitude方法，获取高度信息并将高度信息打印到命令行窗口，再返回判断在3s内是否接收到7个字节的串口数据；若第二个字节不为0x20则返回判断在3s内是否接收到7个字节的串口数据；
[0042] 若第二个字节为0x10时，调用程序中的temperature方法，获取温度信息并将温度信息打印到命令行窗口，再返回判断在3s内是否接收到7个字节的串口数据。
[0043] 图4所示为基于惯性测量元件的三维定位系统的仿真实验结果，表明本发明系统已可以有效工作。实验条件为：将数据采集终端固定于行人腰部，行人在三楼初始化本系统之后，走到一楼，在服务器端将行人三维定位的结果进行显示(单位：m)。经过实际测试结果得知，行人正常行走100m，定位误差在1.5m以内，实现了本系统的三维定位功能。
[0044] 本发明系统基于Python语言，能实时接收消防员的运动数据，实时监测消防员的运动状态，能更加直观的对消防员定位结果进行显示。本发明使用传感器模块实时采集消防员行走时的惯性参量、所处高度等信息，结合行人航位推算算法，可以实时计算出消防员的航向、相对位置信息，实现对消防人员的精确定位；本系统中对常用的步伐检测方法做了改进，使用的是“过阈值检测法”，并利用一段时间的延时来排除因人体抖动造成的误计步，结合无线通信技术实现对数据采集终端的数据智能传输、综合处理。
[0045] 以上所述仅为本专利的较佳实施作了详细说明，并不用于限制发明，凡在本发明创造的精神和原则内，所作的任何修改、等同替换、改进等，均应包含在本发明创造的保护范围之内。
[0046] 本发明未述及之处适用于现有技术。
