基于多传感器的室内三维定位算法研究*

陈友荣,陈 浩,钟陈健,刘半藤,任条娟,周骏华

(1.浙江树人大学信息科技学院,杭州 310015;2.常州大学信息科学与工程学院,江苏 常州 213164; 3.中移(杭州)信息技术有限公司,杭州 311100)

近年来,城市的建筑结构日益复杂,这给消防员的侦察、灭火和搜救等工作带来极大的挑战。在复杂的建筑结构下开展救援行动,需要获知消防员的实时位置信息,从而保障消防员的自身安全和协调整体救援行动。但是目前在日常救援过程中,消防员和指挥员只采用对讲机进行人工对话,简单收集消防员的位置和所在位置的火情信息,没有考虑消防员的精准定位问题,指挥员和消防员难以准确获知各自的位置坐标[1]。因此需要一种现场救援环境下的消防员实时定位技术。

目前实时定位主要分为室内定位和室外定位。全球卫星定位系统已经广泛应用到室外目标的定位和跟踪。但是由于卫星信号在室内严重衰减,卫星定位系统不能应用到室内环境,而且较多购物中心、大型超市、地下车库等建筑物内部结构复杂,室内人员容易迷路,因此对室内精准定位的需求较大,且受到学术界和产业界的高度关注。目前较多学者侧重于利用WIFI信号、摄像头信号、无线信号、惯性导航等信号,研究室内二维定位算法[2-3],如文献[4]提出了基于零速度更新的脚踏式惯性导航算法FINS(Foot-mounted INS algorithm),实现自身的二维定位。文献[5]提出基于鞋上惯性传感器的室内步行导航算法IPN_S(Indoor Pedestrian Navigation algorithm with shoe-mounted inertial sensors),采用零速度更新和9维扩展卡尔曼滤波器估计自身二维定位。一部分学者侧重于利用单个或多个信号研究室内三维定位算法,如利用现场部署摄像头的图像数据[6],激光信号[7],声音信号[8],LED灯光信号[9]和超宽带信号[10]等单一信号实现室内三维定位。文献[11]利用地磁信号、转向角传感器和气压传感器等传感器融合计算三维定位。文献[12]提出一种三维室内定位算法IPA_3D(3D Indoor Positioning Algorithm),即利用加速度计、陀螺仪和气压计等低成本传感器,采用15维扩展卡尔曼滤波器融合计算室内人员的相对三维位置。

目前关于消防员的室内定位算法的研究结果非常少,主要集中在RFID、惯性导航方面的定位算法研究。如文献[13-14]采集所有消防员的信息,采用集中式方法计算消防员的位置。集中式方法需要收集所有传感节点信息。文献[15-16]利用RFID[15]、声音[16]等信号对室内人员进行定位。文献[17]考虑野外火灾救援场景,考虑部署在货车上的基站设备和消防员身上的beacon节点,根据基站设备信号计算自身位置坐标。

由表1可知,上述算法存在一定缺点,不能有效解决缺电、出现大量浓烟、烈火等火灾现场下的室内定位问题。因此在上述文献的基础上,结合三轴加速度计、三轴陀螺仪、三轴磁力计、气压计等多个传感器,提出一种基于多传感器的室内三维定位算法ITPA(Indoor Three-dimensional Positioning Algorithm based on multi-sensor)。即当消防员进入室内进行救援行动时,通过气压计输出数据和Kalman滤波,计算消防员所在的高度和楼层,判断平地行走、上下楼梯、上下电梯等行为。通过三轴加速度计、三轴陀螺仪和三轴磁力计的数据,采用过零检测识别消防员静止和运动情况,并通过捷联惯导处理和航向角计算移动偏移的坐标。最终选择第一层的楼梯位置作为原点,以楼梯为参照点,采用Kalman融合算法计算消防员的三维位置坐标。总之,当消防员处于复杂室内环境下运动时,本算法不仅能够实时判别其行为情况,还能计算3维位置,可降低定位误差,同时解决火灾复杂环境下室内消防员的定位问题,为其他算法提供位置信息。

表1 室内定位算法分析

1 定位算法原理

基于多传感器的室内三维定位算法的原理如图1所示。首先将加速度计等传感器绑在脚上获取消防员行走的各个数据。其次通过气压计的值,采用Kalman滤波算法对当前的高度数据进行滤波,并通过行为阈值判断方法判断平地行走、上下楼梯、上下电梯等行为。由于三轴加速度计、三轴陀螺仪和三轴磁力计的输出数据存在漂移误差,即当消防员在静止的状态下仍输出明显波动的数据,因此采用过零检测判断消防员是否处于静止状态,如果处于静止状态,则将该三个传感器的输出值归零。如果不为零,则把加速度计,陀螺仪和磁力计数据经过捷联惯导计算处理,同时结合航向角数据,计算平面二维位置信息。最后采用Kalman融合估计消防员的三维定位坐标,从而提高定位的准确度。但是ITPA算法仍需要解决以下3个问题:①如何通过气压计判断当前的高度、移动行为和当前楼层;②如何根据三轴加速度计、三轴陀螺仪和三轴磁力计,计算当前平面的二维坐标;③如何采用Kalman融合算法融合估计当前信息,获取消防员的室内三维坐标,这三个问题的具体解决如下。

图1 室内三维定位算法原理

1.1 传感器数据输出

1.2 过零检测

在消防员行走过程中,每个跨步中脚都要经历平稳、迈步、再次平稳的周期性变化过程。但是当消防员的脚处于平稳状态时,传感器采集到的数据仍会出现一定的漂移,会影响后续的定位效果,提高了定位误差。因此考虑根据加速度计和陀螺仪输出的信息,考虑加速度的幅值均方根、加速度的幅值方差和角速度的幅值均方根的情况,实现过零监测,判断消防员是否静止在地面上,从而控制和消除数据漂移误差。具体条件如下:

t时刻的合加速度值为

(1)

其中,at表示t时刻的三个方向的合加速度大小。

(2)

(3)

其中,C1表示加速度幅值均方根条件的判断指示符,thmin和thmax表示根据实际情况设置的幅值均方根条件最小和最大阈值。

令平均加速度为

(4)

(5)

加速度的幅值方差条件为

(6)

其中,C2表示加速度幅值方差条件的判断指示符,thσ-min表示根据实际情况设置的幅值方差阈值。

令角速度的幅值均方根为

(7)

(8)

其中,C3表示角速度的幅值均方根条件的判断指示符号,thw-max表示根据实际情况设置的角速度阈值。

上述3个条件中,C1表示加速度是在一定的范围内,接近重力值。C2表示加速度的波动不能太大,保持稳定。C3表明角速度小于阈值,没有发生转动。只有当C1,C2和C3同时为1时,才判定消防员的脚落在地面上,没有移动,此时脚处于平稳态。

1.3 高度和移动行为判断

高度和移动行为判断包括高度获取,卡尔曼滤波和异常数据处理、移动行为判断和修正3个方面。

1.3.1 高度获取

采用气压计获取消防员位置的高度信息,并结合该高度信息判断移动行为和所在楼层。即根据大气压力,计算海拔高度。

(9)

其中,ρt表示t时刻消防员所处位置的大气压力值,ρ0表示标准大气压力值1.013 25×105Pa,Ht表示t时刻消防员所处的海拔高度,单位m。

由于三维定位所需的是相对海拔高度值,因此采用以下公式计算Δht。

Δht=Ht-h0

(10)

其中,Δht表示t时刻消防员所处的相对海拔高度,h0表示起始位置的海拔高度。

1.3.2 卡尔曼滤波和异常数据处理

由于相对海拔高度值Δht中具有一定噪声,因此需要对Δht进行卡尔曼滤波处理,其所对应的空间模型为:

(11)

建立观测方程为:

(12)

(13)

结合时间更新方程和测量更新方程进行迭代计算,获得滤波后的相对海拔高度值。

通过过零检测可将每一脚步划分为迈步期与平稳期两种状态。由于气压计存在一定的不稳定性,且考虑脚步迈步期的高度变化较大,因此根据过零检测的结果,分析当前时刻是否处于平稳期。如果处于平稳期且该步平稳期内Kalman滤波器输出的相对海拔高度数据个数大于或等于100个,选择100个相对海拔高度值,否则如果该步平稳期较短且开始进入迈步期,即相对海拔高度数据个数小于100个时,选择该步平稳期内的所有相对海拔高度数据。采用箱型图方法对所选择的相对海拔高度数据进行异常点检测。令H表示所选择的相对海拔高度数据集合。对集合H内所有元素进行排序,选择排序第25%的相对海拔高度值作为上四分位值U,选择排序第75%的相对海拔高度值作为下四分位值L,即集合H内只有1/4个数大于U,只有1/4个数小于L。则定义上界高度和下界高度为

hupper=U+1.5(U-L)

(14)

hlower=U-1.5(U-L)

(15)

其中,hupper表示上界高度数值,hlower表示下界高度数值。

(16)

(17)

1.3.3 移动行为判断和修正

根据经过滤波和异常点处理后的高度数据,判断消防员的移动行为。为区分平地行动、上下楼梯和上下电梯的行为,引入λh和μh两个正阈值。如果前后两个时刻的平均高度变化的差值不大于λh,则表示消防员在平面上行动。差值大于λh,则表示消防员在走楼梯或乘坐电梯。而μh区分消防员是在走楼梯还是乘坐电梯。具体算法伪代码如下:

1. if |h′t-h′t-1|≤λhthen在平面上行动

2. else if (h′t-h′t-1)≥μhthen乘坐电梯且上行

3. else if (h′t-h′t-1)≤-μhthen乘坐电梯且下行

4. else if (h′t-h′t-1)>λhthen向上走楼梯

5. else向下走楼梯

6. end if

在实际的判断过程中,由于没有考虑以下情况,会出现错误判断,需要加入一定的结果修正,从而保证判断的准确度。

情况1电梯启动是一个慢慢加速过程。在电梯启动和停止部分会出现上下楼梯的错误判断。修正办法:在判断行为为乘坐电梯后,则计算开始乘楼梯行动的判断部分的时间长度。若时间长度大于η1,则判断消防员的行为为乘坐电梯,将在开始,中间和结束阶段中走楼梯的判断结果直接修正为乘坐电梯。如果在结束部分出现在平面上行动的判断结果,则结束修正。

情况2移动行为判断为上下楼梯,但在中间部分可能会出现乘坐电梯的误判断。修正办法:在判断行为为上下楼梯后,则计算上下楼梯行动的判断部分的时间长度。若时间长度大于η2,则判断消防员的行为为上下楼梯,将在开始,中间和结束阶段中乘坐电梯的判断结果直接修正为上下楼梯。如果在结束部分出现在平面上行动的判断结果,则结束修正。

情况3移动行为判断为上下楼梯,中间部分会出现在平面上行动的判断,无法区分是在楼梯上停留还是错误判断。修正办法:计算平面上行动的判断部分的时间长度。若时间长度大于η3,则保留判断结果,否则修正为上下楼梯行为。

情况4移动行为判断为平面上行动,中间部分会出现部分判定为上下楼梯或乘坐电梯的错误判断。修正办法:如果移动行为判断为平面上行动且中间部分出现上下楼梯或乘坐电梯,则计算上下楼梯或乘坐电梯判断部分的时间长度。若时间长度小于η4,则修正为平面上行动。

1.4 二维坐标计算

二维坐标计算包括移动距离计算和位置获取两个方面。

1.4.1 移动距离计算

考虑到定位对象是消防员,结合其日常工作环境分析发现:消防员在救灾现场不可能像普通行人那样走路,他们的动作更复杂,跑步、不平坦的地势和摆动幅度大的行走是消防员不可避免的。因此提出一种新的移动距离计算方法,它除了能计算普通行人的移动距离外,更适用于像消防员这类人的移动距离计算。下面就是这种算法的步骤:

(18)

(19)

其中,q0、q1、q0、q0表示四元数。通过加速度计和陀螺仪两个传感器,采用毕卡逼近法更新四元数,可获得当前时刻的旋转矩阵。

(20)

其中,T是采样周期。

步骤3修正速度。根据过零检测的结果,分析当前时刻是否处于平稳期。如果处于平稳期且该步平稳期内的速度数据个数大于或等于100个,选择100个速度数据,否则如果该步平稳期较短且开始进入迈步期,即速度数据个数小于100个,选择该步平稳期内的所有相对速度数据。令Vpi表示所选择平稳期的速度数据集合,计算Vpi的速度平均值,即

(21)

根据平均速度βt,通过式(22)修正迈步期的速度。

(22)

根据平均速度βt,修正平稳期的速度为:

(23)

步骤4计算位移增量ΔPt:

(24)

步骤5计算平面上移动距离:

(25)

其中,SLt表示第t时刻的移动距离。

循环执行步骤1～步骤5,则可以获得在二维平面上移动的距离。

1.4.2 位置获取

假定载体坐标系的X轴与脚相一致后,根据陀螺仪和磁力计两个传感器输出的值,采用互补滤波修正航向角,并通过移动距离计算自身二维位置。即:

(26)

(27)

(28)

其中,θs=arctan(|T22/T12|)。

1.5 Kalman融合估计

在获得高度值和二维坐标的信息后,采用Kalman融合算法将信息进行融合处理。令Xt表示t时刻Kalman融合算法的状态向量估计值,由位置、速度、姿态角、加速度和角速度构成,即Xt=[rt,ct,φt,bt,ηt]T,其中rt表示3维位置估计值,ct表示3维速度估计值,φt表示3维姿态角估计值,bt表示3维加速度计零偏误差值,ηt表示3维陀螺仪零偏值。其所对应的空间模型为:

Xt=FtXt-1+Γt-1Wt-1

(29)

其中,Xt表示t时刻融合滤波状态向量的估计值,Ft表示状态转移矩阵,Wt-1表示t-1时刻的过程噪声,Γt-1表示系统噪声增益矩阵。Ft可表示为:

(30)

其中,ξ表示t时刻的旋转矩阵,ψ表示加速度an的反对称矩阵,Δt表示采样间隔,I3×3表示3×3单位矩阵,O3×3表示全为0的3×3矩阵。ψ可表示为:

(31)

系统噪声增益矩阵Γt-1为

(32)

建立观测方程为:

Zt=HtXt+nt

(33)

其中,Zt表示t时刻融合滤波的测量值,nt表示t时刻的测量噪声,Ht表示滤波算法的测量矩阵。Ht可表示为:

Ht=[I3×3ΔtI3×3O3×3O3×3O3×3O3×3]

(34)

2 算法实现

本算法可直接应用到消防员上的嵌入式设备中,实现消防员的实时三维定位。如图2所示,其三维定位算法的具体实现步骤如下:

步骤1初始化各个参数,包括起始位置的海拔高度、横轴、纵轴和竖轴的方向、二维平面坐标和航向角等信息。

步骤2实时采集三轴加速度计、三轴陀螺仪、三轴磁力计的姿态传感器和气压计数值,并存储到存储器中。

步骤3根据加速度计和陀螺仪输出的信息,计算当前时刻的加速度幅值均方根条件C1、加速度幅值方差条件C2和角速度幅值均方根条件C3。如果C1,C2和C3的值都为1时,判断当前时刻处于平稳期,否则处于迈步期。

步骤4根据当前的气压计值,参考起始位置的海拔高度,计算相对海拔高度值。采用卡尔曼滤波算法对高度数据进行滤波,消除噪声。

步骤5根据过零检测的结果,分析当前时刻是否处于平稳期。如果处于平稳期且该平稳期内卡尔曼滤波后输出的相对海拔高度数据个数大于或等于100个,则选择该100个相对海拔高度数据,否则如果处于平稳期且该平稳期内卡尔曼滤波后输出的相对海拔高度数据个数小于100个,则直接选择该平稳期内所有相对海拔高度数据,否则直接跳到步骤2。采用箱型图方法对所选择的相对海拔高度数据进行异常点检测和修正,将这些相对海拔数据的平均值作为当前时刻的消防员的高度值,跳到步骤6;

步骤6根据前后高度值,识别平面行动、上下楼梯和上下电梯等消防员的行为,并对识别结果进行修正,从而保证判断的准确度。

步骤7根据加速度计和陀螺仪的值,计算和修正当前二维的移动距离。根据陀螺仪和磁力计的值,采用互补滤波修正航向角,并通过移动距离计算自身的二维位置坐标。

步骤8采用Kalman融合方法对信息进行融合处理,计算和输出消防员的室内三维坐标。

图2 工作流程图

步骤9如果消防员手动结束室内三维定位功能,则退出,否则跳到步骤2。

循环执行步骤2～步骤9,实现消防员室内实时三维定位。

3 算法实验

3.1 实验参数

在实验过程中,选择MPU9025姿态传感器和MS5611气压计。将上述两个模块固定在实验人员的脚部,并通过蓝牙模块、STM32开发板和上位机软件,实现实验人员的运动数据采集,且每次测试时记录下经过测量点的真实步数,位置和移动距离等信息,方便后续误差分析。MPU9250姿态传感器由3轴加速度、3轴陀螺仪和3轴磁力计组成。其中3轴陀螺仪输出具有三个可选择量程(±250 °/s,±500 °/s,±1 000 °/s)的16 位三轴角速度数据;3轴加速度输出具有三个可选择量程(±2gn,±4gn,±8gn,±16gn)的16位三轴加速度数据;3轴磁力计输出具有14位和16位分辨率的磁力计数据,同时最大测量范围在±4 800 μT。MS5611气压计内置24位模数转换器,支持IIC和SPI通讯协议,测量/工作输出范围:1 000 mPa～1 200 mPa,分辨率10 cm。根据实验人员的实测运动数据,选择以下参数,通过MATLAB软件计算消防员室内三维位置、步数误差、距离误差和漂移误差:thmin为0,thmax为1,thσ-min为1,thw-max为1,λh为0.14,μh为0.6,nt、wt-1、qt和wt-1为随机高斯噪声的协方差,采样周期T为0.01 s。

其中,定义步数误差errorzupt为计算所得的当前步数和真实步数差值的绝对值与真实步数的比值,可表示为:

errorzupt=(|steptrue-stepcal|)/steptrue

(35)

其中,steptrue表示测量过程中的真实步数。stepcal表示算法计算所得的步数。定义移动距离误差errordis为计算所得的移动距离和真实距离差值的绝对值与真实距离的比值,可表示为:

errordis=(|distrue-discal|)/distrue

(36)

其中,distrue表示测量过程中的真实移动距离。discal表示算法计算所得的移动路程。定义漂移误差 errordrift为计算所得的三维坐标和真实三维位置坐标差值的模值与真实距离的比值,可表示为:

(37)

其中,(xtrue,ytrue,ztrue)表示测量过程中的真实坐标。(xcal,ycal,zcal)表示算法计算所得的坐标。

3.2 实验结果分析

选择3.1节中参数,比较FINS[4],IPN_S[5],IPA_3D[12]和ITPA算法的消防员室内三维位置、步数误差、距离误差和漂移误差。其中FINS和IPN_S算法是二维定位算法,在算法比较中直接选择ITPA算法的高度计算方法计算其高度值,从而获得三维位置坐标。

3.2.1 直线行走

如图3所示,起始点矩形经过11个中间测量点(菱形),到达终点测量点圆形。真实轨迹是朝一个方向直线行走30 m。从图中可以发现轨迹在x轴方向上稳定增加,而在y轴方向上几乎没有出现明显的波动,较接近真实路线。

图3 直线行走的定位坐标图

如图4所示,ITPA算法的直线行走步数误差在8%以下,远低于FINS,IPN_S和IPA_3D的直线行走步数误差。这是因为:FINS,IPN_S和IPA_3D算法采用单个条件或结合瞬时时刻的数据分析行人行走的迈步态和平稳态,从而判断其行走步数,而ITPA算法选取加速度的幅值均方根、加速度的幅值方差和角速度的幅值均方根的情况,实现过零监测,降低了步数判断的误差。

图4 直线行走的步数误差

如图5所示,ITPA算法的直线行走距离误差低于FINS,IPN_S和IPA_3D的直线行走距离误差,但是当移动距离较大时,ITPA算法和FINS算法的距离误差相差不大。这是因为:在直线行走时,ITPA算法和FINS算法基本上采用相同的方法,但是由于ITPA算法根据平稳期的平均速度修正迈步期和平稳期的速度,从而获得较接近实际的速度,降低了距离误差,因此其距离误差略低于FINS算法的距离误差。IPN_S 和IPA_3D采用静止时的速度作为观测量修正位置,其距离误差略高。

图5 直线行走的距离误差

如图6所示,ITPA算法的直线行走漂移误差低于FINS,IPN_S,IPA_3D的直线行走漂移误差。这是因为:ITPA算法的步数判断较准确,高度且2维坐标获取的误差较小,同时采用Kalman融合算法对由位置、速度、姿态角、加速度和角速度构成的状态向量进行融合估计,降低了系统噪声,并有效防止误差的累计。

图6 直线行走的漂移误差

3.2.2 楼梯行走

如图7所示,起始点(矩形),先向上经过一段楼梯和中间平台,再换一个方向向上经过另一段楼梯,最终行走到上一层楼,即经过11个中间测量点(菱形),到达终点测量点(圆形)。从图中可以发现轨迹在z轴上稳定增加,而在x轴和y轴上有规律的变化,其行走的路线较接近真实路线。

图7 楼梯行走的定位坐标图

如图8所示,ITPA算法的楼梯行走步数误差远低于FINS,IPN_S和IPA_3D的楼梯行走步数误差。这是因为:ITPA算法选取加速度的幅值均方根、加速度的幅值方差和角速度的幅值均方根作为消防员迈出一步的判断标准,不受高度对步数判断的影响,其步数误差与直线行走的步数误差相差不大。而FINS,IPN_S和IPA_3D只是考虑部分条件,走楼梯的行为对其步数判断影响较大,其步数误差明显高于直线行走的步数误差。

图8 楼梯行走的步数误差

如图9所示,ITPA算法通过分析人行走时的状态,及时对不同阶段的速度进行修正,同时根据高度值判断行人所在楼层高度,并及时修正楼层高度,并根据移动行为对三维坐标观测值进行修正,有效降低了累计误差,同时结合楼层高度和移动行为进行数据融合,降低了数据的漂移对算法影响,因此ITPA算法的楼梯行走距离误差低于FINS,IPN_S和IPA_3D的楼梯行走距离误差,且与其直线行走距离误差相差不大。但是FINS,IPN_S和IPA_3D的楼梯行走距离误差比其直线行走距离误差提高较多。

图9 楼梯行走的距离误差

如图10所示,FINS和IPN_S算法是二维定位,其高度计算方法采用ITPA算法的高度计算方法,对气压计的输出值进行Kalman滤波和异常数据处理等,提高了高度精度,因此其漂移误差远低于IPA_3D算法。由于楼梯上行走x和y轴的变化相对不大,主要是在z轴上逐渐增加,因此FINS,IPN_S和ITPA算法的漂移误差相差不大,且与其各自直线行走的漂移误差提高不多。由于ITPA算法采用了Kalman融合估计,并根据移动行为对观测值进行修正,从而降低了算法的漂移误差,因此ITPA算法略低于FINS和IPN_S算法。

图10 楼梯行走的漂移误差

3.3.3 综合行走

如图11所示,起始点(矩形),先沿楼梯行走至第二层,再在该层楼行走一个矩形后,经过楼梯返回至第一层,即经过11个中间测量点(菱形)到达终点测量点(圆形)。从图11可以发现轨迹在楼梯行走的时候z轴上稳定增加,而x轴和y轴上有规律的变化,在平面行走时,z轴基本无变化,且其计算路径较接近真实路线。

图11 综合行走的定位图

图12 综合行走的步数误差

如图12所示,ITPA算法不受行走路径的影响,其综合行走的步数误差低于FINS,IPN_S和IPA_3D的综合行走步数误差,且比楼梯行走和直线行走的步数误差略高。IPN_S和IPA_3D受高度变化的影响较大,其综合行走步数误差远大于楼梯行走和直线行走的步数误差。其原因与楼梯行走的类似,请参考3.2.2节。

如图13所示,ITPA算法不受行走路径的影响,其综合行走距离误差远低于FINS,IPN_S和IPA_3D的综合行走距离误差,且与其楼梯行走和直线行走的距离误差相差不大。FINS,IPN_S和IPA_3D的综合行走距离误差比其楼梯行走和直线行走的距离误差提高较多,其中IPN_S达到200%以上,距离误差较大。其原因与楼梯行走的类似,请参考3.2.2节。

图13 综合行走的距离误差

如图14所示,IPA_3D算法的漂移误差较大,远大于其楼梯行走和直线行走的漂移误差。FINS,IPN_S和ITPA算法选用相同的高度计算方法,其综合行走漂移误差相对较小,其中ITPA算法的综合行走漂移误差最小,且与其楼梯行走和直线行走的漂移误差相差不大。其原因与楼梯行走的类似,请参考3.2.2节。

图14 综合行走的漂移误差

4 总结

利用气压计和包含三轴加速度计、三轴陀螺仪和三轴磁力计的姿态传感器数据,提出一种基于多传感器的室内三维定位算法。首先,提出了传感器数据输出和过零检测方法,其次,根据气压计数据,获得海拔值,并采用卡尔曼滤波和异常数据处理等方法,获得消防员的当前高度,并提出消防员移动行为判断和修正方法。接着,通过移动距离计算和位置获取,计算二维坐标,并采用Kalman融合估计方法,利用移动行为修正观测值,降低定位误差。最后给出算法的仿真参数,比较直行行走、楼梯行走和综合行走的FINS,IPN_S,IPA_3D和ITPA算法的性能。

总之,在直行行走、楼梯行走和综合行走下,ITPA算法都能获得较接近真实路线的移动轨迹和消防员室内三维位置,降低了算法的步数误差、距离误差和漂移误差。但是算法目前根据真实实验数据进行仿真分析和比较,因此下一阶段目标是将在嵌入式设备上实现ITPA算法,实现算法的应用。