CN109084763B - 基于姿态角度测量的穿戴式三维室内定位装置及方法 - Google Patents

Abstract

本发明公开了一种基于姿态角度测量的穿戴式三维室内定位装置及方法，装置包括惯性传感器、气压传感器、嵌入式处理器、通信模块、电源模块及三维室内建筑模型显示模块。惯性传感器测量左右脚腕处的姿态角和加速度信号，气压传感器采集人员所处位置的气压信号。在嵌入式处理器中，对惯性传感器和气压传感器获得的数据进行信息融合处理，并将处理后的数据通过通信模块的蓝牙模块发送至自组网终端，由自组网将终端接收到的数据包发送到自组网基站，再通过串口将数据包传输给显示模块，显示模块显示人员实时位置等信息。电源模块为嵌入式处理器供能。本发明提高了三维室内定位精度和实时性。

Description

基于姿态角度测量的穿戴式三维室内定位装置及方法

技术领域

本发明涉及室内定位领域，尤其涉及到一种基于姿态角度测量的穿戴式三维室内定位装置及方法。

背景技术

现有技术中提出了多种室内定位技术解决方案，如无线传感器定位技术、ZigBee技术、红外线技术、射频标签识别技术、超宽带技术、Wi-Fi定位技术、光跟踪定位技术，以及图像分析、信标定位、计算机视觉定位技术等。但这些技术都不具有普遍适用性和实时性，均需要外接设备辅助定位。Wi-Fi定位技术需要通过在室内架设Wi-Fi网络进行对接收器位置的估算。图像分析法定位要求对室内图像进行采集再处理，在室内光线昏暗时该方法受到限制并且数据处理时间很长，定位实时性不好。在实际运用中，这些方案很难实现对未知环境内人员的高精度、高实时性和普遍适用性的定位。

因此，在室内定位中，急需一种不需要外接设备辅助定位的高精度实时室内定位装置。

发明内容

针对上述技术问题，本发明的目的在于提供种精确度高、实时性好、普适性强的基于姿态角度测量的穿戴式室内三维定位装置及方法。

为实现上述目的，本发明是根据以下技术方案实现的：

根据本发明的一方面，本发明提出了一种基于姿态角度测量的穿戴式三维室内定位装置，其特征在于，包括：惯性传感器、气压传感器、嵌入式处理器、通信模块、电源模块以及三维室内建筑模型显示模块，所述惯性传感器测量被测人员左右脚腕处的姿态角和加速度信号，所述气压传感器测量人员所处位置的气压信号，所述通信模块包括MESH自组网终端和蓝牙模块，所述嵌入式处理器对姿态角、加速度及气压的信号进行信息融合处理，并将处理后的数据通过所述蓝牙模块发送至所述MESH自组网终端上，所述MESH自组网终端接收到数据包之后通过组网发送到MESH自组网基站，所述MESH自组网基站通过串口将数据包传输给所述三维室内建筑模型显示模块，所述三维室内建筑模型显示模块显示建筑物三维模型和人员的实时位置，所述电源模块为各传感器和所述嵌入式处理器提供电能。

上述技术方案中，所述惯性传感器和所述气压传感器集成在穿戴设备中，同时进行信号测量。

上述技术方案中，所述嵌入式处理器采用STM32F103处理器。

上述技术方案中，所述惯性传感器采用HI219九轴惯性传感器，其内部包含三轴加速度传感器、三轴陀螺仪以及三轴磁力计。

上述技术方案中，所述气压传感器采用BMP280数字传感器。

根据本发明的另一方面，本发明提出了一种基于上述装置的穿戴式三维室内定位方法，其特征在于，包括如下步骤：

步骤S1：惯性传感器测量左、右脚腕处的三轴加速度信号、三轴磁力计信号和三轴角速度信号，气压传感器测量人员所处位置的气压信号，所述惯性传感器和所述气压传感器获得的相关信号数据传输至嵌入式处理器中；

步骤S2：所述嵌入式处理器首先对左、右脚腕处三轴加速度信号、三轴磁力计信号和三轴角速度信号进行四元素及卡尔曼滤波预处理；左脚腕穿戴设备内部的嵌入式处理器通过蓝牙模块将预处理后得到的三轴加速度数据和三轴角度数据发送至右脚腕穿戴设备，然后与右脚腕的数据进行同步处理；

步骤S3：对人行走时的信号进行特征提取，首先记录硬件复位时两脚腕处的腿倾角初值以及两脚腕处运动方向角初值，然后根据行走时的角度数据得出当脚落地瞬间的两脚腕处的运动方向角、腿部倾角的角度变化量以及行走时脚腕处的加速度信号，同时根据气压值获得人员所处位置的海拔高度；

步骤S4：对加速度信号进行以窗口大小为5的中值滤波处理，使得加速度信号波形更平滑；

步骤S5：得到左脚着地时两腿的角度变化量和右脚着地时两腿的角度变化量，根据已知的人的髋关节到脚底的腿长用三角函数计算获得被测人员的当前时刻运动位移大小，根据位移和方向角以及海拔高度运用航位推算解算得到人的运动坐标；

步骤S6：运动坐标数据通过蓝牙模块传输到MESH自组网终端，再由MESH自组网终端通过组网将数据传送至MESH自组网基站，所述MESH自主网基站通过串口将数据包传输给三维室内建筑模型显示模块，所述三维室内建筑模型显示模块显示建筑物三维模型和人员的实时位置。

上述技术方案中，步骤S5中的三角函数具体公式如下：

其中，L为人的髋关节到脚底的长度，θ₁和θ₂分别为人在行走过程中右脚着地时左腿倾角变化量和右腿倾角变化量，θ₃和θ₄分别为人在行走过程中左脚着地时左腿倾角变化量和右腿倾角变化量，l₁和l₂分别为当前一步完成时刻左右脚位移，l₁和l₂的和为一步的总长度；利用方向角做航位推算获取平面坐标，具体公式如下：

其中，α为当前一步完成时刻的方向角，x_(t+1)和y_(t+1)是当前一步完成时刻的平面坐标，x_(t)和y_(t)为上一步完成时刻的平面坐标。

与现有技术相比，本发明具有以下有益技术效果：

1、非辅助式高精度实时室内定位。非外接辅助定位的情况下，数据稳定无依赖，对室内人员进行高精度实时定位，在多种不可知的环境内均可对人员位置进行准确定位。通过位移测量与三维建筑模型的结合，能够在模型中实时的显示人员的位置信息，实际位置和模型中位置一一对应，能够在模型中方便直观的了解到人员信息。

2、采用多传感器进行数据采集，利用嵌入式处理器进行多传感器信息融合处理，并且提出了一种根据腿部倾角计算步长的新方法，提高了定位精度，避免了传统惯性导航定位的精度低的问题。

3、脚腕可穿戴设备集检测、分析、通信为一体，使用非常方便，不会对使用者的行动产生影响，拥有很好的普适性。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其它附图。

图1是本发明的定位装置的结构框图；

图2是本发明的左右脚腕可穿戴设备硬件总体结构图；

图3是本发明的右脚腕可穿戴设备程序流程图；

图4是本发明多传感器信息融合处理流程图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。

如图1所示，本发明装置主要由四部分组成，分别是数据采集部分、嵌入式处理器进行多传感器信息融合处理部分、数据传输部分以及上位机显示部分。装置总体结构图如1所示，本发明的一种基于姿态角度测量的穿戴式三维室内定位装置，包括：惯性传感器、气压传感器、嵌入式处理器、通信模块、电源模块以及三维室内建筑模型显示模块，所述惯性传感器测量被测人员左右脚腕处的姿态角和加速度信号，所述气压传感器测量人员所处位置的气压信号，所述通信模块包括MESH自组网终端和蓝牙模块，所述嵌入式处理器对姿态角、加速度及气压的信号进行信息融合处理，并将处理后的数据通过所述蓝牙模块发送至所述MESH自组网终端上，所述MESH自组网终端接收到数据包之后通过组网发送到MESH自组网基站，所述MESH自组网基站通过串口将数据包传输给所述三维室内建筑模型显示模块，所述三维室内建筑模型显示模块显示建筑物三维模型和人员的实时位置，所述电源模块为各传感器和所述嵌入式处理器提供电能。

其中，本发明的惯性传感器和所述气压传感器集成在穿戴设备中，同时进行信号采集。通过左、右脚腕可穿戴设备内的惯性传感器和气压传感器，以及外围电路，以此获得待处理的运动数据，左、右脚腕可穿戴设备硬件总体结构图如图2所示。

嵌入式处理器采用STM32F103处理器。惯性传感器示意性地采用HI219九轴惯性传感器，包含三轴加速度传感器、三轴陀螺仪以及三轴磁力计。气压传感器采用BMP280数字传感器。蓝牙采用CC2450型号。

所述三维室内建筑模型显示模块运用串口通信接收自MESH自组网终端组网传来的数据并显示人员的实时位置，其中，显示模块接收串口端的数据包，在QT中进行数据包头匹配，提取出有用数据并在上位机的三维室内建筑模型上显示出人员具体位置。

左脚腕可穿戴设备中惯性传感器和气压传感器与嵌入式处理器的信号输入端连接，惯性传感器持续采集人员左腿的运动信息，获取人员运动时的腿部角度数据、加速度信号以及运动方向角度数据以及气压传感器将实时的气压数据输入嵌入式处理器；嵌入式处理器的信号输出端与蓝牙相连，通过蓝牙将采集的数据发送到右脚腕可穿戴设备。右脚腕可穿戴设备中惯性传感器和气压传感器与嵌入式处理器的信号输入端连接，惯性传感器持续采集人员右腿的运动信息，获取人员运动时的腿部角度变化、加速度信号以及运动方向角度变化以及气压传感器实时采集气压数据并输入嵌入式处理器，并且右脚腕穿戴设备内嵌入式处理器还接收左脚腕可穿戴设备传输来的数据。

本发明的一种基于上述装置的穿戴式三维室内定位方法，包括如下步骤：

步骤S1：惯性传感器采集左、右脚腕处的姿态角、加速度信号，气压传感器采集气压信号，所述惯性传感器和所述气压传感器将采集到的数据传输至嵌入式处理器；

步骤S2：所述嵌入式处理器首先对左、右脚腕处测量的三轴加速度信号、三轴磁力计信号和三轴角速度信号进行四元素及卡尔曼滤波预处理；左脚腕穿戴设备将预处理后的数据发送至右脚腕穿戴设备进行同步处理；

首先左、右脚腕穿戴设备将各自采集到的运动数据进行四元素法确立了捷联矩阵，利用三轴陀螺仪所得到的角速度信息建立过程方程，利用三轴加速度传感器和磁力计信息建立测量方程，进而设计了一种卡尔曼滤波器来滤除传感器存在的随机噪声，并解算出了人员运动时的姿态角；然后左脚腕数据传输至右脚腕可穿戴设备上，右脚腕穿戴设备内处理器对数据进行软件同步算法，让左右腿角度在时间上一一对应,其次右脚腕穿戴设备内处理器对两脚腕处气压数据进行高度解算，得出室内所处位置高度信息；最后既是对特征信号的提取与运动模式识别以及根据腿部角度信息和高度信息解算位置坐标。右脚腕可穿戴设备程序流程图如图3所示。

步骤S3：所述嵌入式处理器通过特征提取法对人行走时的信号进行特征提取，得出人行走时两只腿的运动方向角、两只腿角度在垂直于路面方向上的变化量以及运动时脚腕处的加速度信号，并且提取海拔高度的数据；

人员行走时采集到的数据，需要对其进行处理后才能够有效的进行特征提取及坐标解算。数据处理的过程主要是完成以下几项任务：滤波去噪、特征信号提取、坐标解算等，如图4所示。

本发明的步骤S4：对加速度信号进行以窗口大小为5的中值滤波处理，使得加速度信号波形更平滑；

其中，为获得人在行走时的姿态角，因此引入了四元素法表示的捷联矩阵求解姿态角，但是根据捷联惯导系统四元素的相关理论求解微分方程得到的姿态角会随时间产生误差，需要进行滤波处理，因此引入了典型的离散型卡尔曼滤波器。卡尔曼滤波器是一种优化的递归数据处理算法，典型的卡尔曼滤波器描述如下：

其中的高斯噪声W(n)和V(n)是相互独立的，它们的协方差分别是Q和R。R是影响滤波器相信预测值还是当前测量值的关键变量我们根据陀螺仪的数据和姿态角之间关系建立过程方程，再根据加速度和磁力计信息与重力和磁场之间的关系建立测量方程。过程方程和离散化的测量方程如下：

其中，W(n)为陀螺仪的噪音，将其近似为均值为零的独立高斯白噪声，并且协方差矩阵式为非负定常值对角阵；g为重力加速度；

为加速度传感器的测量值；M_x、M_y、M_z为地磁场在该地理坐标上的分量；

为磁力计测量值；V是磁力计测量噪声。左、右脚腕穿戴设备将传感器原始数据采集之后根据四元素法和离散型卡尔曼滤波结合求解出姿态角。该算法对噪声有很好的抑制作用，因此其适用于本发明。

在初步滤波后，为提高特征信号提取的准确性，进行同步算法。左脚腕穿戴设备上的嵌入式处理器对内部的惯性传感器的数据进行读取并运用四元素结合卡尔曼滤波处理，然后用蓝牙传输给右脚腕穿戴设备上的嵌入式处理器。右脚腕穿戴设备上的嵌入式处理器上对内部的惯性传感器的数据进行读取并运用四元素结合卡尔曼滤波处理，并且同时接收左脚腕穿戴设备蓝牙传输来的数据。由于坐标解算需要两腿部同步的数据进行联合解算，为避免出现两者信号采集不同步的现象，本发明设计了同步算法。具体过程是：若硬件保持同步，则10个采样点均为该次中断转换结果，若出现硬件不同步，则10个采样点中将会插入前一次中断的最后一个采样点。两脚腕处采集到的气压数据对其进行高度解算，具体公式如下：

p_r＝p₀(1-H/44330)⁵²⁵⁵ (3)

其中，p₀代表标准大气压(1013.25mbar)，H代表海拔高度(m)，p_r代表测得气压值(mbar)。

对同步处理后，为了提取准确的两脚腕处加速度波形判断运动模式，使用中值滤波对加速度波形进行滤波处理。中值滤波可以让加速度波形更平滑，能有效抑制噪声，因此适合于本次发明中对加速度波形的提取前的处理。

使用过零检测和峰谷值检测的方法进行特征信号提取处理，其中对腿部角度特征信号提取过程如下：行人在行走过程中腿的角度呈现周期性摆动，每一个周期代表一个步伐。需要特征点的角度来计算行走的位移。特征点分别为右脚落地时左腿、右腿的倾角，用该特征点的特征值与站立时的角度初始值的差值的绝对值即为右脚移动一步对应的腿倾角的角度变化量，另外两个特征点分别为左脚落地时左腿、右腿的倾角，用该特征点的特征值与站立时的角度初始值的差值的绝对值即为左脚移动一步对应的腿的角度变化量。对人运动的方向角提取是先记录人站立在初始位置时方向角的值，根据判断到一步结束时方向角的瞬时值，根据两角度的差值表示运动方向。对加速度波形的提取是根据过零检测和峰谷值提取结合进行，提取出人运动时的加速度波形。由于人的正常运动方式分为走路和跑步模式，根据测试得到人的跑步模式与走路模式对应的波形相比较，前者波形变化更快、周期持续时间更短、峰值更大，根据这些特征对人的运动模式进行辨别，为后续的位移计算打下基础。

步骤S5：得到左脚着地时两腿的角度变化量和右脚着地时两腿的角度变化量之后，根据已知的人的髋关节到脚底的腿长，用三角函数进行位移计算，得到当前时刻运动位移大小，根据位移和方向角以及海拔高度运用航位推算解算得到人的运动坐标；

其中，三角函数具体公式如下：

其中，L为人的髋关节到脚底的长度，θ₁和θ₂分别为人在行走过程中右脚着地时左腿倾角变化量和右腿倾角变化量，θ₃和θ₄分别为人在行走过程中左脚着地时左腿倾角变化量和右腿倾角变化量，l₁和l₂分别为当前一步完成时刻左右脚位移，l₁和l₂的和为一步的总长度；

由于此时的位移大小并不能与模型坐标匹配上，因此需要对其进行坐标解算。根据提取到的当前一步完成时刻的方向角α，运用三角函数解算出平面位置坐标(x_(t+1)，y_(t+1))，利用方向角做航位推算获取平面坐标，具体公式如下：

其中，α为当前一步完成时刻的方向角，其中x_(t+1)和y_(t+1)是当前一步完成时刻的横纵坐标，x_(t)和y_(t)为上一步完成时刻的平面坐标。垂直平面的坐标z根据海拔高度然后和楼层高度对应解算得到。为了让计算获得的坐标能与模型坐标匹配，需在实际位置中的某一个特定位置处对可穿戴设备进行硬件的复位操作，进而将人员的位置等信息同步到三维模型上，达到实际位置和检测位置一一对应。

步骤S6：运动坐标数据通过蓝牙模块传输到MESH自组网终端，再由MESH自组网终端通过组网将数据传送至MESH自组网基站，所述MESH自主网基站通过串口将数据包传输给三维室内建筑模型显示模块，所述三维室内建筑模型显示模块显示建筑物三维模型和人员的实时位置。

在本发明中，三维室内建筑模型显示模块运用串口通信接收自MESH自组网终端组网传来的数据并显示人员的实时位置。三维建筑模型包括模型的建立、模型与串口之间的数据通信、以及数据的显示。三维建筑模型的搭建是通过OpenGL的模型库完成，通过搭建平面以及对平面进行渲染来实现楼层地面的绘制，房间与走廊等的构建，是通过在平面插入线段，通过线段进行隔断形成房间。四周棱角均通过线段构建。在构建好的三维建筑模型上将校验正确后解析出来的数据实时显示在模型上。

为了更加通俗的理解上述室内定位装置，下面结合实际实验进行说明：

针对实验所提取的传感器信号，包括：左、右脚腕处方向角，左、右脚腕处腿部倾角，左右脚腕处加速度，利用四元素及卡尔曼滤波算法对角度信息进行滤波，中值滤波对加速度信号滤波，然后通过三角函数解算位置坐标，验证本发明室内定位方法的准确有效性。

为了验证算法，利用不同性别、不同身高的实验人员对系统进行测试。在对室内定位精度测试时，我们选用不同的人进行测试，用激光测距仪对人行走的实际位移进行测量，精确度达到±1mm，满足作为实际距离测量工具的要求。记录每次设备计算的所行走的位移，运用matlab软件进行数据分析。

计算值和实际值相关性分别为0.9996和0.9997，标准差分别为0.3833和0.3519，定位平均误差分别为5.21％和5.17％。多次测试得到平均误差为5.22％，说明本发明具有高精度和高普适性。

虽然，上文中已经用一般性说明及具体实施例对本发明作了详尽的描述，但在本发明基础上，可以对之作一些修改或改进，这对本领域技术人员而言是显而易见的。因此，在不偏离本发明精神的基础上所做的这些修改或改进，均属于本发明要求保护的范围。

Claims (5)

1.一种基于姿态角度测量的穿戴式三维室内定位装置，其特征在于，包括：惯性传感器、气压传感器、嵌入式处理器、通信模块、电源模块以及三维室内建筑模型显示模块，所述惯性传感器和所述气压传感器集成在穿戴设备中，同时进行信号测量，所述惯性传感器测量被测人员左右脚腕处的姿态角和加速度信号，所述惯性传感器采用HI219九轴惯性传感器，其内部包含三轴加速度传感器、三轴陀螺仪以及三轴磁力计；所述气压传感器测量人员所处位置的气压信号，所述通信模块包括MESH自组网终端和蓝牙模块，所述嵌入式处理器对姿态角、加速度及气压的信号进行信息融合处理，并将处理后的数据通过所述蓝牙模块发送至所述MESH自组网终端上，所述嵌入式处理器首先对左、右脚腕处三轴加速度信号、三轴磁力计信号和三轴角速度信号进行四元素及卡尔曼滤波预处理；首先左、右脚腕穿戴设备将各自采集到的运动数据进行四元素法确立了捷联矩阵，利用三轴陀螺仪所得到的角速度信息建立过程方程，利用三轴加速度传感器和磁力计信息建立测量方程，进而设计了一种卡尔曼滤波器来滤除传感器存在的随机噪声，并解算出了人员运动时的姿态角；左脚腕穿戴设备内部的嵌入式处理器通过蓝牙模块将预处理后得到的三轴加速度数据和三轴角度数据发送至右脚腕穿戴设备，然后与右脚腕的数据进行同步处理；所述卡尔曼滤波器描述如下：

其中的陀螺仪的噪声W(n)和磁力计测量噪声V(n)是相互独立的，它们的协方差分别是Q和R；根据陀螺仪的数据和姿态角之间关系建立过程方程，再根据加速度和磁力计信息与重力和磁场之间的关系建立测量方程；过程方程和离散化的测量方程如下：

其中，W(n)为陀螺仪的噪声，将其近似为均值为零的独立高斯白噪声，并且协方差矩阵式为非负定常值对角阵；g为重力加速度；

为加速度传感器的测量值；M_x、M_y、M_z为地磁场在地理坐标上的分量；

为磁力计测量值；V是离散化后的磁力计测量噪声；

所述同步处理的具体过程是：若硬件保持同步，则10个采样点均为本次中断转换结果，若出现硬件不同步，则10个采样点中将会插入前一次中断的最后一个采样点；两脚腕处采集到的气压数据对其进行高度解算，具体公式如下：

p_r＝p₀(1-H/44330)⁵²⁵⁵ (3)

其中，p₀代表标准大气压，值为1013.25mbar，H代表海拔高度，单位为m，p_r代表测得气压值，单位为mbar；

同步处理后,使用中值滤波对加速度波形进行滤波处理；使用过零检测和峰谷值检测的方法进行特征信号提取处理，其中对腿部角度特征信号提取过程如下：特征点分别为右脚落地时左腿、右腿的倾角，用该特征点的特征值与站立时的角度初始值的差值的绝对值即为右脚移动一步对应的腿倾角的角度变化量，另外两个特征点分别为左脚落地时左腿、右腿的倾角，用该特征点的特征值与站立时的角度初始值的差值的绝对值即为左脚移动一步对应的腿的角度变化量；对人运动的方向角提取是先记录人站立在初始位置时方向角的值，根据判断到一步结束时方向角的瞬时值，根据两角度的差值表示运动方向；

所述MESH自组网终端接收到数据包之后通过组网发送到MESH自组网基站，所述MESH自组网基站通过串口将数据包传输给所述三维室内建筑模型显示模块，所述三维室内建筑模型显示模块显示建筑物三维模型和人员的实时位置，所述电源模块为各传感器和所述嵌入式处理器提供电能。

2.根据权利要求1所述的基于姿态角度测量的穿戴式三维室内定位装置，其特征在于，所述嵌入式处理器采用STM32F103处理器。

3.根据权利要求1所述的基于姿态角度测量的穿戴式三维室内定位装置，其特征在于，所述气压传感器采用BMP280数字传感器。

4.一种根据权利要求1-3任一项所述装置的穿戴式三维室内定位方法，其特征在于，包括如下步骤：

步骤S1：惯性传感器测量左、右脚腕处的三轴加速度信号、三轴磁力计信号和三轴角速度信号，气压传感器测量人员所处位置的气压信号，所述惯性传感器和所述气压传感器获得的相关信号数据传输至嵌入式处理器中；

步骤S2：所述嵌入式处理器首先对左、右脚腕处三轴加速度信号、三轴磁力计信号和三轴角速度信号进行四元素及卡尔曼滤波预处理；首先左、右脚腕穿戴设备将各自采集到的运动数据进行四元素法确立了捷联矩阵，利用三轴陀螺仪所得到的角速度信息建立过程方程，利用三轴加速度传感器和磁力计信息建立测量方程，进而设计了一种卡尔曼滤波器来滤除传感器存在的随机噪声，并解算出了人员运动时的姿态角；左脚腕穿戴设备内部的嵌入式处理器通过蓝牙模块将预处理后得到的三轴加速度数据和三轴角度数据发送至右脚腕穿戴设备，然后与右脚腕的数据进行同步处理；所述卡尔曼滤波器描述如下：

其中的陀螺仪的噪声和磁力计测量噪声V(n)是相互独立的，它们的协方差分别是Q和R；根据陀螺仪的数据和姿态角之间关系建立过程方程，再根据加速度和磁力计信息与重力和磁场之间的关系建立测量方程；过程方程和离散化的测量方程如下：

其中，W(n)为陀螺仪的噪声，将其近似为均值为零的独立高斯白噪声，并且协方差矩阵式为非负定常值对角阵；g为重力加速度；

为加速度传感器的测量值；M_x、M_y、M_z为地磁场在地理坐标上的分量；

为磁力计测量值；V是离散化后的磁力计测量噪声；

所述同步处理的具体过程是：若硬件保持同步，则10个采样点均为本次中断转换结果，若出现硬件不同步，则10个采样点中将会插入前一次中断的最后一个采样点；两脚腕处采集到的气压数据对其进行高度解算，具体公式如下：

p_r＝p₀(1-H/44330)⁵²⁵⁵ (3)

其中，p₀代表标准大气压，值为1013.25mbar，H代表海拔高度，单位为m，p_r代表测得气压值，单位为mbar；

步骤S3：对人行走时的信号进行特征提取，首先记录硬件复位时两脚腕处的腿倾角初值以及两脚腕处运动方向角初值，然后根据行走时的角度数据得出当脚落地瞬间的两脚腕处的运动方向角、腿部倾角的角度变化量以及行走时脚腕处的加速度信号，同时根据气压值获得人员所处位置的海拔高度；

步骤S4：对加速度信号进行以窗口大小为5的中值滤波处理，使得加速度信号波形更平滑；使用中值滤波对加速度波形进行滤波处理；使用过零检测和峰谷值检测的方法进行特征信号提取处理，其中对腿部角度特征信号提取过程如下：特征点分别为右脚落地时左腿、右腿的倾角，用该特征点的特征值与站立时的角度初始值的差值的绝对值即为右脚移动一步对应的腿倾角的角度变化量，另外两个特征点分别为左脚落地时左腿、右腿的倾角，用该特征点的特征值与站立时的角度初始值的差值的绝对值即为左脚移动一步对应的腿的角度变化量；对人运动的方向角提取是先记录人站立在初始位置时方向角的值，根据判断到一步结束时方向角的瞬时值，根据两角度的差值表示运动方向；

步骤S5：得到左脚着地时两腿的角度变化量和右脚着地时两腿的角度变化量，根据已知的人的髋关节到脚底的腿长用三角函数计算获得被测人员的当前时刻运动位移大小，根据位移和方向角以及海拔高度运用航位推算解算得到人的运动坐标；

步骤S6：运动坐标数据通过蓝牙模块传输到MESH自组网终端，再由MESH自组网终端通过组网将数据传送至MESH自组网基站，所述MESH自组网基站通过串口将数据包传输给三维室内建筑模型显示模块，所述三维室内建筑模型显示模块显示建筑物三维模型和人员的实时位置。

5.根据权利要求4所述的穿戴式三维室内定位方法，其特征在于，步骤S5中的三角函数具体公式如下：

其中，L为人的髋关节到脚底的长度，θ₁和θ₂分别为人在行走过程中右脚着地时左腿倾角变化量和右腿倾角变化量，θ₃和θ₄分别为人在行走过程中左脚着地时左腿倾角变化量和右腿倾角变化量，l₁和l₂分别为当前一步完成时刻左右脚位移，l₁和l₂的和为一步的总长度；利用方向角做航位推算获取平面坐标，具体公式如下：

其中，α为当前一步完成时刻的方向角，x_(t+1)和y_(t+1)是当前一步完成时刻的平面坐标，x_(t)和y_(t)为上一步完成时刻的平面坐标。

Images