CN107702711A - 一种基于低成本传感器和地图约束的行人航向推算系统 - Google Patents

Abstract

本发明属于行人定位与导航技术领域，具体的是一种基于低成本传感器和地图约束的行人航向推算系统。该系统包括依次连接的惯性及磁场传感器数据获取模块100、行人航向推算模块200、粒子滤波修正行人位置模块300和行人位置输出模块400。该系统定位精度高，在地图的约束下，能够达到比原有的定位方法更高的定位精度。

Description

一种基于低成本传感器和地图约束的行人航向推算系统

技术领域

本发明属于行人定位与导航技术领域，具体的是一种基于低成本传感器和地图约束的行人航向推算系统。

背景技术

当前全球卫星导航系统已发展成熟，我国的北斗卫星导航系统、美国的GPS导航系统、俄罗斯的GLONASS导航系统等基于卫星的导航技术已经广泛应用于室外的定位与导航服务。然而，在室内环境、城市峡谷、密集森林和隧道立交桥环境等卫星信号会严重衰减甚至丢失，此时的卫星定位系统将无法提供服务。当前，智能终端技术发展迅速，大多数的智能终端都包含惯性传感器和磁场传感器，靠智能终端中的低成本传感器可以实现行人的自主定位。

为了解决这些环境中的定位和导航难题，当前已经有技术人员在室内环境中通过布设伪卫星、蓝牙、WiFi等设备的方式协助行人在无法使用卫星定位服务的场所提供定位和导航服务。但是这些信号设备在提供定位导航服务的过程中存在许多的不足：

(1)用于辅助行人定位的外部设备需要接入外部电源才能够正常工作，因此对安装位置具有限制。

(2)在紧急情况下，如果失去外部的电力供应，将会出现无法定位的情况。

(3)当前的智能终端接收并处理卫星信号和其他定位信号时消耗的电量较多，不利于智能终端长时间为用户提供定位服务。

发明内容

针对现有技术存在的不足之处，本发明提出了一种基于智能终端的低成本传感器和地图约束的定位方法和系统，以解决当前无法接收卫星定位信号的环境中，定位依赖其他外部信号发射设备的问题，并且能够达到当前定位技术所不能达到的定位精度。

本发明采用如下技术方案：

一种基于低成本传感器和地图约束的行人航向推算系统，该系统包括依次连接的惯性及磁场传感器数据获取模块100、行人航向推算模块200、粒子滤波修正行人位置模块300和行人位置输出模块400；

所述惯性及磁场传感器获取模块100采集行人的加速度信息、角加速度信息、磁场信息和时间戳，并将这些数据传输给行人航向推算模块200；

所述行人航向推算模块200对加速度信息、角加速度信息、磁场信息和时间戳处理后，得到行人的步态信息、步长信息和航向信息，并将这些信息数据传输给粒子滤波修正行人位置模块300；

所述粒子滤波修正行人位置模块300对行人的步态、步长和航向进行修正，将修改后的步态、步长和航向反馈给行人航向推算模块200，还传输给行人位置输出模块400；

行人位置输出模块400输出行人当前的准确位置。

本技术方案进一步的优化，所述惯性及磁场传感器获取模块100包括三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103、标定模块104和标定的惯性信号测量值和磁场测量值输出模块105，所述三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103分别与标定模块104连接，标定模块104与标定的惯性信号测量值和磁场测量值输出模块105连接；

所述三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103分别用以测量行人在运动过程中的加速度信息、角加速度信息和磁场信息，标定模块104对加速度信息、角加速度信息和磁场信息进行标定后，标定的惯性信号测量值和磁场测量值输出模块105输出标定后的加速度信息、角加速度信息和磁场信息。

本技术方案更进一步的优化，所述三轴加速度传感器101、三轴角加速度传感器102和三轴磁力传感器103的采样频率为50Hz。

本技术方案更进一步的优化，所述三轴加速度传感器101、三轴角加速度传感器102和三轴磁力传感器103设置在智能终端内，所述智能终端为智能手机、智能手表或智能眼镜。

本技术方案进一步的优化，所述行人航向推算模块200包括航向信息输出模块204和与其连接的步态检测模块201、步长估计模块202、航向测算模块203，所述步态检测模块201、步长估计模块202和航向信息模块203均与惯性及磁场传感器获取模块100相连，所述步态检测模块201还与步长估计模块202相连；

所述步态检测模块201对数据处理后得到行人的步态，步长估计模块202对数据处理后得到行人的步长，航向测算模块203对数据处理后得到行人的航向，航向信息输出模块204输出行人的步态、步长和航向。

本技术方案更进一步的优化，所述步态检测模块201包括以下检测步骤，

S2011，从惯性及磁场传感器获取模块100获取的加速度信息和角加速度信息，计算得到运动过程中的俯仰角，对俯仰角信息进行低通滤波处理，获取俯仰角变化的低频信息；

S2012，判断俯仰角幅值是否大于预设阈值，如果大于预设阈值，则转到步骤S2013，否则，返回到惯性及磁场传感器获取模块100；

S2013，判断俯仰角变化过程的时间间隔是否符合行人运动规律，如果符合，那么判断为行人在此时间内走了一步，转到步骤S2014，否则，返回到惯性及磁场传感器获取模块100；

S2014，输出此次运动过程的开始与结束的时间戳。

本技术方案更进一步的优化，所述航向测算模块203具体步骤，

S2031，从惯性及磁场传感器获取模块100获取标定后加速度信息、角加速度信息、磁场信息和时间戳；

S2032，判断加速度信息、角加速度信息、磁场信息的时间戳是否一致，如果是，则转到步骤S2033，否则，转到惯性及磁场传感器获取模块100重新获取相关数据；

S2034，判断磁场值偏差是否在阈值内，如果不在阈值内，那么选择六轴姿态航向参考系统，则转到步骤S2036，如果偏差在阈值内，那么选择九轴姿态航向参考系统，转到步骤S2035；

S2035，磁场信息数据传入九轴传感器四元数计算，得到根据姿态航向参考系统处理后的姿态四元数组，转到步骤S2037；

S2036，磁场信息数据传入六轴传感器四元数计算，得到根据姿态航向参考系统处理后的姿态四元数组，转到步骤S2037；

S2037，将最新的姿态四元数保存下来，并解算行人姿态的欧拉角形式，得到欧拉角；

S2038，将欧拉角处理后，得到航向信息输出。

本技术方案进一步的优化，所述粒子滤波修正行人位置模块300包括依次连接的地图信息与初始位置加载模块301、粒子初始化模块302、粒子传播模块303、粒子状态更新模块304、粒子重采样模块305和粒子状态输出模块306，所述粒子状态更新模块304、粒子重采样模块305均反馈到粒子传播模块303；

地图信息与初始位置加载模块301加载线性约束地图和行人在地图内的初始位置，粒子初始化模块302以行人初始位置为原点，生成初始化粒子群；粒子传播模块303从行人航向推算模块200获取行人的步态、步长和航向信息，对所有粒子赋予更新后的步长和航向信息，粒子状态更新模块304对粒子进行位置更新和权重更新，所述粒子重采样模块305对粒子进行参数更新和数量更新，传输给粒子状态输出模块306，粒子状态输出模块306输出粒子的权值和位置信息。

本技术方案进一步的优化，行人位置输出模块400包括粒子权重获取模块401、粒子位置获取模块402、粒子位置融合模块403和融合位置输出模块404，所述粒子滤波修正行人位置模块300分别与粒子权重获取模块401、粒子位置获取模块402连接，所述粒子权重获取模块401、粒子位置获取模块402与粒子位置融合模块403连接，粒子位置融合模块403与融合位置输出模块404连接；

粒子权重获取模块401、粒子位置获取模块402分别获取所有粒子的权重和位置，粒子位置融合模块403根据粒子权重和位置计算粒子群的中心位置，作为所有粒子融合的位置点，将所有粒子融合的位置点输出到融合位置输出模块404，融合位置输出模块404处理后输出最终的位置。

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

1.定位过程完全依靠智能终端设备，不需要额外的信号设备。

2.定位精度高，在地图的约束下，能够达到比原有的定位方法更高的定位

精度。

3.智能终端内的惯性传感器和磁力传感器的功耗相对于信号接收模块更

低，能够有效保障智能终端续航时间。

附图说明

图1是基于智能终端的低成本传感器和地图约束的行人航向推算系统结构原理图；

图2是惯性及磁场传感器数据获取模块结构原理图；

图3是行人航向推算模块结构原理图；

图4是步态检测模块结构原理图；

图5是航向测算模块结构原理图；

图6是粒子滤波修正行人位置模块结构原理图；

图7是行人位置输出模块结构原理图。

具体实施方式

为进一步说明各实施例，本发明提供有附图。这些附图为本发明揭露内容的一部分，其主要用以说明实施例，并可配合说明书的相关描述来解释实施例的运作原理。配合参考这些内容，本领域普通技术人员应能理解其他可能的实施方式以及本发明的优点。图中的组件并未按比例绘制，而类似的组件符号通常用来表示类似的组件。

现结合附图和具体实施方式对本发明进一步说明。

请参阅图1所示，本发明优选一实施例一种基于智能终端的低成本传感器和地图约束的行人航向推算系统结构原理图。该系统包括以下几个依次连接的部分：惯性及磁场传感器数据获取模块100、行人航向推算模块200、粒子滤波修正行人位置模块300和行人位置输出模块400。惯性及磁场传感器数据获取模块100将行人的加速度信息、角加速度信息、磁场信息和时间戳传输给行人航向推算模块200，行人航向推算模块200对这些数据处理后，得到行人的步态信息、步长信息和航向信息，并将这些信息数据传输给粒子滤波修正行人位置模块300，粒子滤波修正行人位置模块300对行人的步态、步长和航向进行修正，将修改后的步态、步长和航向一边反馈给行人航向推算模块200，另一边传输给行人位置输出模块400，行人位置输出模块400输出行人当前的准确位置。

参阅图2所示，为惯性及磁场传感器数据获取模块结构原理图。该实施例的惯性及磁场传感器获取模块100包括智能终端自包含的三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103、标定模块104和标定的惯性信号测量值和磁场测量值输出模块105，所述三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103分别与标定模块104连接，标定模块104与标定的惯性信号测量值和磁场测量值输出模块105连接。该实施例所述智能终端设备不局限于智能手机、智能手表和智能眼镜等具有操作系统的智能终端产品，应包括所有包含三轴加速度传感器101、三轴角加速度传感器102和三轴磁力传感器103的终端设备。

该实施例的三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103分别用以测量行人在运动过程中的加速度信息、角加速度信息和磁场信息。标定模块104使用的是智能终端中的软件标定模块，对加速度信息、角加速度信息和磁场信息进行标定，然后将标定过的加速度信息、角加速度信息和磁场信息传输给标定的惯性信号测量值和磁场测量值输出模块105，标定的惯性信号测量值和磁场测量值输出模块105输出标定后的加速度信息、角加速度信息和磁场信息给行人航向推算模块200。需要说明的是，该实施例使用的三轴加速度传感器101、三轴角加速度传感器102和三轴磁力传感器103的采样频率均为50Hz。

标定模块104即将三个传感器采集的数据进行时间戳统一标定。如果三轴加速度传感器101、三轴角加速度传感器102、三轴磁力传感器103在同一时间内分别采集了30、40、50个数据，则采集的时间戳不一样，标定模块104则将这些数据进行同一标定，使得三个传感器采集时间戳一样，采集数据一样。

参阅图3所示，为行人航向推算模块结构原理图。行人航向推算模块200包括航向信息输出模块204和与其连接的步态检测模块201、步长估计模块202、航向测算模块203，所述步态检测模块201与步长估计模块202相连。其中，所述步态检测模块201、步长估计模块202和航向信息模块203均与标定的惯性信号测量值和磁场测量值输出模块105相连，根据标定后的加速度信息、角加速度信息和磁场强度信息获取行人的步态、步长和航向信息，在对行人的步长和航行方向进行估计时，根据地图信息对行人位置约束的结果通过粒子滤波修正行人位置模块300向步长估计模块202和航行测算模块203提供步长和航向的修正信息，将步态信息和修正后步长、航向信息传递给航向信息输出模块204，航向信息输出模块204输出行人的步态、步长和航向。

参阅图4所示，为步态检测模块结构原理图。所述步态检测模块201包括以下检测步骤：

S2011，从标定的惯性信号测量值和磁场测量值输出模块105获取的加速度信息和角加速度信息，计算得到智能终端在运动过程中的俯仰角，对俯仰角信息进行低通滤波处理，获取俯仰角变化的低频信息；

S2012，判断俯仰角幅值是否大于预设阈值，即判断俯仰角变化是否符合行人运动的规律，如果大于预设阈值，则转到步骤S2013，否则，返回到标定的惯性信号测量值和磁场测量值输出模块105；

S2013，判断俯仰角变化过程的时间间隔是否符合行人运动规律，如果符合，那么判断为行人在此时间内走了一步，转到步骤S2014，否则，返回到标定的惯性信号测量值和磁场测量值输出模块105；

S2014，输出此次运动过程的开始与结束的时间戳。

步长估计模块202是根据行人的行走的步频与步频的方差，利用提前训练好的步频、步频方差和步长二元函数模型对步长进行估计。二元线性函数形式如下所示：

StepLengthL＝α·fre+β·var+γ

其中，fre表示当前步伐的步频，var表示当前步伐的加速度方差；α和β表示步频和加速度方差的权重因子，γ为常量。

提前训练的过程是根据行人行走一定的距离所用的时间和步数，利用二元曲线拟合得到的，根据拟合后得到的二元曲线的参数，在行人的运动过程中，根据行人的步频就能够估计行人的步长。该实施例中，选取了50米的直线路径，通过由慢到快的10次训练，记录每次训练的时间长度和计步数量，计算得到步频的均值和方差，利用拟合工具进行拟合，得到α、β和γ。

参阅图5所示，为航向测算模块结构原理图。在进行航向测算时，具体包括以下步骤：

S2031，从标定的惯性信号测量值和磁场测量值输出模块105获取标定后加速度信息、角加速度信息、磁场信息和时间戳。

S2032，判断加速度信息、角加速度信息、磁场信息的时间戳是否一致，即标定后加速度传感器数据、角速度传感器数据和磁场传感器数据是否在同一时间点采集到数据，如果是，则转到步骤S2033，否则，转到标定的惯性信号测量值和磁场测量值输出模块105重新获取相关数据。

S2034，判断磁场值偏差是否在阈值内，即是否存在地磁干扰，根据国际参考地磁场中的地球磁场模型计算得到当地标准的磁场强度，将所测量到的磁场强度与标准磁场强度比较，如果测量值与标准值之间的偏差不在预设阈值内，那么选择六轴姿态航向参考系统，则转到步骤S2036，如果偏差在阈值内，那么选择九轴姿态航向参考系统，转到步骤S2035。

S2035，磁场信息数据传入九轴传感器四元数计算，得到根据姿态航向参考系统处理后的姿态四元数组，转到步骤S2037。

S2036，磁场信息数据传入六轴传感器四元数计算，得到根据姿态航向参考系统处理后的姿态四元数组，转到步骤S2037。

S2037，将最新的姿态四元数保存下来，并解算智能终端姿态的欧拉角形式，得到智能终端的欧拉角。

S2038，将智能终端的欧拉角处理后，得到智能终端航向信息输出。

行人航向推荐模块200输出行人的步态、步长和航向给粒子滤波修正行人位置模块300。参阅图6所示，为粒子滤波修正行人位置模块结构原理图。粒子滤波修正行人位置模块300包括依次连接的地图信息与初始位置加载模块301、粒子初始化模块302、粒子传播模块303、粒子状态更新模块304、粒子重采样模块305和粒子状态输出模块306，所述粒子状态更新模块304、粒子重采样模块305均反馈到粒子传播模块303。

在定位过程的初始阶段，地图信息与初始位置加载模块301加载线性约束地图和行人在地图内的初始位置，粒子初始化模块302以行人初始位置为原点，按照高斯分布算法，生成初始化粒子群。

粒子传播模块303与航向信息输出模块204相连，根据航向信息更新的结果，对所有粒子赋予更新后的步长和航向信息，粒子状态更新模块304根据粒子获取的更新后的步长和航向信息对粒子群的位置进行更新，在粒子传播过程中，在加入航向信息输出模块204提供的航向信息的基础上，加入高斯随机噪声，保证粒子运动的多样性。

更新的过程中，由于行人不存在穿过墙体等地图上无法穿越的物体的行为，因此，认定穿过墙体的粒子的权值设为0，也就是认定粒子死亡，没有穿过墙体的粒子权值设为1，也就是认为粒子依旧存活。根据粒子状态更新的结果，粒子重采样模块305对粒子的信息进行更新，赋予粒子新的位置和权值，根据粒子当前权值的状态，将权值为0的粒子删除，并记录所删除的粒子数量，删除粒子后，根据当前存活的粒子，随机选取粒子进行复制，将数量补充到设定的粒子数量。更新后的粒子将会传递到粒子传播模块303和粒子状态输出模块306，在粒子传播模块303中接收更新的行人航向信息，在粒子状态输出模块306中，将粒子的权值和位置信息记录下来，为其他的模块提供地图约束模式下的航向推算结果。

参阅图7所示，为行人位置输出模块结构原理图。粒子状态输出模块306将根据地图约束获取的行人位置粒子滤波结果传送到行人位置输出模块400。行人位置输出模块400包括粒子权重获取模块401、粒子位置获取模块402、粒子位置融合模块403和融合位置输出模块404。所述粒子状态输出模块306分别与粒子权重获取模块401、粒子位置获取模块402连接，所述粒子权重获取模块401、粒子位置获取模块402与粒子位置融合模块403连接，粒子位置融合模块403与融合位置输出模块404连接。

粒子权重获取模块401、粒子位置获取模块402分别获取所有粒子的权重和位置，粒子位置融合模块403根据粒子权重和位置计算粒子群的中心位置，作为所有粒子融合的位置点，将所有粒子融合的位置点输出到融合位置输出模块404，融合位置输出模块404处理后输出最终的位置。

尽管结合优选实施方案具体展示和介绍了本发明，但所属领域的技术人员应该明白，在不脱离所附权利要求书所限定的本发明的精神和范围内，在形式上和细节上可以对本发明做出各种变化，均为本发明的保护范围。

Claims (9)

1.一种基于低成本传感器和地图约束的行人航向推算系统，其特征在于：该系统包括依次连接的惯性及磁场传感器数据获取模块(100)、行人航向推算模块(200)、粒子滤波修正行人位置模块(300)和行人位置输出模块(400)；

所述惯性及磁场传感器获取模块(100)采集行人的加速度信息、角加速度信息、磁场信息和时间戳，并将这些数据传输给行人航向推算模块(200)；

所述行人航向推算模块(200)对加速度信息、角加速度信息、磁场信息和时间戳处理后，得到行人的步态信息、步长信息和航向信息，并将这些信息数据传输给粒子滤波修正行人位置模块(300)；

所述粒子滤波修正行人位置模块(300)对行人的步态、步长和航向进行修正，将修改后的步态、步长和航向反馈给行人航向推算模块(200)，还传输给行人位置输出模块(400)；

行人位置输出模块(400)输出行人当前的准确位置。

2.如权利要求1所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述惯性及磁场传感器获取模块(100)包括三轴加速度传感器(101)、三轴角加速度传感器(102)、三轴磁力传感器(103)、标定模块(104)和标定的惯性信号测量值和磁场测量值输出模块(105)，所述三轴加速度传感器(101)、三轴角加速度传感器(102)、三轴磁力传感器(103)分别与标定模块(104)连接，标定模块(104)与标定的惯性信号测量值和磁场测量值输出模块(105)连接；

所述三轴加速度传感器(101)、三轴角加速度传感器(102)、三轴磁力传感器(103)分别用以测量行人在运动过程中的加速度信息、角加速度信息和磁场信息，标定模块(104)对加速度信息、角加速度信息和磁场信息进行标定后，标定的惯性信号测量值和磁场测量值输出模块(105)输出标定后的加速度信息、角加速度信息和磁场信息。

3.如权利要求2所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述三轴加速度传感器(101)、三轴角加速度传感器(102)和三轴磁力传感器(103)的采样频率为(50)Hz。

4.如权利要求2所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述三轴加速度传感器(101)、三轴角加速度传感器(102)和三轴磁力传感器(103)设置在智能终端内，所述智能终端为智能手机、智能手表或智能眼镜。

5.如权利要求1所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述行人航向推算模块(200)包括航向信息输出模块(204)和与其连接的步态检测模块(201)、步长估计模块(202)、航向测算模块(203)，所述步态检测模块(201)、步长估计模块(202)和航向信息模块(203)均与惯性及磁场传感器获取模块(100)相连，所述步态检测模块(201)还与步长估计模块(202)相连；

所述步态检测模块(201)对数据处理后得到行人的步态，步长估计模块(202)对数据处理后得到行人的步长，航向测算模块(203)对数据处理后得到行人的航向，航向信息输出模块(204)输出行人的步态、步长和航向。

6.如权利要求5所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述步态检测模块(201)包括以下检测步骤，

S2011，从惯性及磁场传感器获取模块(100)获取的加速度信息和角加速度信息，计算得到运动过程中的俯仰角，对俯仰角信息进行低通滤波处理，获取俯仰角变化的低频信息；

S2012，判断俯仰角幅值是否大于预设阈值，如果大于预设阈值，则转到步骤S2013，否则，返回到惯性及磁场传感器获取模块(100)；

S2013，判断俯仰角变化过程的时间间隔是否符合行人运动规律，如果符合，那么判断为行人在此时间内走了一步，转到步骤S2014，否则，返回到惯性及磁场传感器获取模块(100)；

S2014，输出此次运动过程的开始与结束的时间戳。

7.如权利要求5所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述航向测算模块(203)具体步骤，

S2031，从惯性及磁场传感器获取模块(100)获取标定后加速度信息、角加速度信息、磁场信息和时间戳；

S2032，判断加速度信息、角加速度信息、磁场信息的时间戳是否一致，如果是，则转到步骤S2033，否则，转到惯性及磁场传感器获取模块(100)重新获取相关数据；

S2034，判断磁场值偏差是否在阈值内，如果不在阈值内，那么选择六轴姿态航向参考系统，则转到步骤S2036，如果偏差在阈值内，那么选择九轴姿态航向参考系统，转到步骤S2035；

S2035，磁场信息数据传入九轴传感器四元数计算，得到根据姿态航向参考系统处理后的姿态四元数组，转到步骤S2037；

S2036，磁场信息数据传入六轴传感器四元数计算，得到根据姿态航向参考系统处理后的姿态四元数组，转到步骤S2037；

S2037，将最新的姿态四元数保存下来，并解算行人姿态的欧拉角形式，得到欧拉角；

S2038，将欧拉角处理后，得到航向信息输出。

8.如权利要求1所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：所述粒子滤波修正行人位置模块(300)包括依次连接的地图信息与初始位置加载模块(301)、粒子初始化模块(302)、粒子传播模块(303)、粒子状态更新模块(304)、粒子重采样模块(305)和粒子状态输出模块(306)，所述粒子状态更新模块(304)、粒子重采样模块(305)均反馈到粒子传播模块(303)；

地图信息与初始位置加载模块(301)加载线性约束地图和行人在地图内的初始位置，粒子初始化模块(302)以行人初始位置为原点，生成初始化粒子群；粒子传播模块(303)从行人航向推算模块(200)获取行人的步态、步长和航向信息，对所有粒子赋予更新后的步长和航向信息，粒子状态更新模块(304)对粒子进行位置更新和权重更新，所述粒子重采样模块(305)对粒子进行参数更新和数量更新，传输给粒子状态输出模块(306)，粒子状态输出模块(306)输出粒子的权值和位置信息。

9.如权利要求1所述的基于低成本传感器和地图约束的行人航向推算系统，其特征在于：行人位置输出模块(400)包括粒子权重获取模块(401)、粒子位置获取模块(402)、粒子位置融合模块(403)和融合位置输出模块(404)，所述粒子滤波修正行人位置模块(300)分别与粒子权重获取模块(401)、粒子位置获取模块(402)连接，所述粒子权重获取模块(401)、粒子位置获取模块(402)与粒子位置融合模块(403)连接，粒子位置融合模块(403)与融合位置输出模块(404)连接；

粒子权重获取模块(401)、粒子位置获取模块(402)分别获取所有粒子的权重和位置，粒子位置融合模块(403)根据粒子权重和位置计算粒子群的中心位置，作为所有粒子融合的位置点，将所有粒子融合的位置点输出到融合位置输出模块(404)，融合位置输出模块(404)处理后输出最终的位置。