CN112562077B - 一种融合pdr和先验地图的行人室内定位方法 - Google Patents

Abstract

本发明公开了一种融合PDR和先验地图的行人室内定位方法，首先分别获取室内地图和行人的IMU数据，对所获得的室内地图进行LSD特征提取，将地图信息转化为具有墙壁信息的矩阵；然后对所获得的行人IMU数据进行PDR处理，得到原始的行人室内轨迹，再经过处理后与地图对准；利用地图信息和处理后的轨迹信息判别“穿墙”现象，得到穿墙点及其坐标；最后采用粒子滤波算法得到最优轨迹坐标，进行定位结果更新，得到最优的行人室内轨迹。本发明方法修正了由于PDR积分特性引起的定位误差，提高了定位精度，得到了更好的定位效果。

Description

一种融合PDR和先验地图的行人室内定位方法

技术领域

本发明属于定位技术领域，具体涉及一种行人室内定位方法。

背景技术

位置服务分为室内室外，在室外有全球定位系统辅助进行定位。但在室内环境里，由于墙体结构复杂等原因导致无法接收卫星信号。PDR定位方法其无源特性并且定位不需要外部协助，仅由IMU就可以完成行人室内定位的特点已经成了行人室内定位的常用方法。但是PDR定位会产生定位误差，虽然可以利用惯性元件IMU来采集行人的步态数据，然后进行步态检测，判断出行人静止的时刻，并以该时刻下的速度作为卡尔曼滤波的观测量，进而对定位误差进行修正。然而，由于该方法是基于加速度的双重积分，因此随着时间的累积，定位误差也会随之累积。虽然可以采用ZUPT和ZARU的方法对误差进行修正，但依旧不能让定位结果达到一个最优的状态。同时由于步态检测的方法可能不够准确，所以定位误差依旧会存在。

因此为了修正定位误差，提高定位精度，便加入室内地图这一先验信息，通过PDR和先验地图的融合进一步校正定位误差，从而提高行人室内定位的精度。

发明内容

为了克服现有技术的不足，本发明提供了一种融合PDR和先验地图的行人室内定位方法，首先分别获取室内地图和行人的IMU数据，对所获得的室内地图进行LSD特征提取，将地图信息转化为具有墙壁信息的矩阵；然后对所获得的行人IMU数据进行PDR处理，得到原始的行人室内轨迹，再经过处理后与地图对准；利用地图信息和处理后的轨迹信息判别“穿墙”现象，得到穿墙点及其坐标；最后采用粒子滤波算法得到最优轨迹坐标，进行定位结果更新，得到最优的行人室内轨迹。本发明方法修正了由于PDR积分特性引起的定位误差，提高了定位精度，得到了更好的定位效果。

本发明解决其技术问题所采用的技术方案包括以下步骤：

步骤1：绘制室内地图；

步骤2：对IMU进行标定处理，使用IMU采集行人室内运动数据；

步骤3：使用IMU数据，利用PDR定位方法得到行人室内轨迹；

步骤4：对行人室内轨迹进行平滑处理；

步骤5：对平滑处理后的行人室内轨迹进行旋转和平移，与室内地图对准；

步骤6：对室内地图进行LSD线特征提取，把室内地图转化为具有墙壁特征的地图向量矩阵；

步骤7：将步骤5与室内地图对准后的行人室内轨迹转换成轨迹向量矩阵；使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点；

步骤8：采用粒子群算法，对步骤5与室内地图对准后的行人室内轨迹进行粒子滤波；

步骤9：在粒子滤波过程中，使用LSI算法检测粒子群中是否有穿墙的粒子，并对穿墙的粒子进行权重归零，得到地图信息修正后的粒子群；

步骤10：粒子滤波完成后，计算出粒子群所表示的最优值，得到最优坐标，即最优行人室内轨迹。

进一步地，所述步骤1中绘制室内地图采用ARCGIS进行绘制。

进一步地，所述步骤2中对IMU进行标定处理的方法为：将IMU器件放入二十一面体中，然后把每个平面静止十秒，利用采集到的数据对IMU进行标定处理。

进一步地，所述步骤3中使用IMU数据，利用PDR定位方法得到原始的行人室内轨迹，包括如下步骤：

步骤3-1：MSINS初始对准：确定初始捷联矩阵；

步骤3-2：MSINS捷联解算；

进行四元数更新，即求解下式四元数微分方程：

其中，n表示导航坐标系，矩阵表示载体坐标系相对于n系的姿态四元数，定义载体坐标系为b系，是姿态角速率；

使用等效旋转矢量法求解(1)式，通过旋转矩阵链乘规则将式(1)写成数字递推的形式，如式(2)所示：

其中，是t_k时刻从b系到n系的变换四元数，是t_k-1时刻的姿态四元数；是以地心惯性坐标系为参考系时，n系从t_k-1时刻到t_k时刻的变换四元数，定义地心惯性坐标系为i系；是以i系为参考系时，b系从t_k-1时刻到t_k时刻的变换四元数；

计算n系从t_k-1时刻到t_k时刻的转动等效旋转矢量：

其中，是t时刻在导航坐标系下的角速度，是k-1时刻在导航坐标系下的角速度，是k时刻在导航坐标系下的角速度，τ_k＝t_k-t_k-1是采样间隔；

根据等效旋转与旋转四元数之间的转换关系，得到：

记b系从t_k-1时刻到t_k时刻的转动等效旋转矢量为采用角增量二子样等效旋转算法进行求解：

Δθ_k＝Δθ_k，1+Δθ_k，2 (6)

其中，是陀螺仪测量得到的载体角速度，是一个快变量；Δθ_k是陀螺仪在t_k-1时刻到t_k时刻的角增量；Δθ_k，1和Δθ_k，2分别对应于陀螺仪从t_k-1时刻到时刻和从时刻到t_k时刻的角增量，计算方法为：

其中是t_k-1时刻的载体角速度，是时刻的载体角速度，是t_k时刻的载体角速度；

采用高速率采样，使用单子样算法求解计算方法为：

其中是单子样方法下t_k-1时刻的载体角速度，是单子样方法下t_k时刻的载体角速度；

根据等效旋转矢量与旋转四元数之间的转换关系，得到：

将式(4)和式(10)代入到(1)式中，即完成姿态更新过程；

如果和有关的为[1，0，0，0]^T，则式(1)简化为：

对式(9)进行简化，得到：

步骤3-3：行人步态检测和静止检测；

对于陀螺仪向量序列和加速度计向量序列 构造如下统计量：

式中，是陀螺仪在x轴的M个序列的平方和，是陀螺仪在y轴的M个序列的平方和，是陀螺仪在z轴的M个序列的平方和； 是加速度计在x轴的M个序列的平方和，是加速度计在y轴的M个序列的平方和，是加速度计在z轴的M个序列的平方和； 种分别是陀螺仪和加速度计向量的二范数平方；是陀螺仪序列的均值，是加速度计序列的均值；

由均值不等式，存在以下不等式的成立：

式中，分别是陀螺仪x轴、y轴、z轴采样角速率序列{ω_ix}、{ω_iy}、{ω_iz}的均值； 分别是加速度计x轴、y轴、z轴采样比力序列{f_ix}、{f_iy}、{f_iz}的均值；上面不等式等号成立的条件是各测量轴的采样序列元素全部相等；

式(16)表明，当序列各元素的离差越大，不等式两端的差值越大；

对于陀螺仪，当其处于静态时，三个测量轴输出几乎为零，可以直接利用式(16)两端之差构造反映角运动动态程度的特征量，如式(18)：

对于加速度计，加速度计输出的比力数据含有重力加速度g分量，先从合矢量中把g扣除，按式(19)方法进行处理：

再将式(19)右侧的替换成式(17)的右侧项，构造出反映线运动动态程度的特征量为：

最后，综合陀螺仪和加速度计的信息，构成既反映线运动又反映角运动动态程度的特征量为：

其中，c是角运动特征影响因子；影响因子c用于调节陀螺和加速度计的数据量级到等同的程度，使用序列方差的比值作为c的值；

综上所述，特征量和Δ²都用于检测载体运动的动态程度，通过设置特征量和Δ²阈值来达到对目标运动特性检测的目的；

步骤3-4：ZUPT卡尔曼滤波器设计；

滤波器的误差状态选择为九维，具体如下：

x＝[(δpⁿ)^T，(δvⁿ)^T，(φⁿ)^T]^T (22)

其中，x是误差状态向量，δpⁿ是位置误差，δvⁿ是速度误差，φⁿ是姿态失准角，三者都是SINS系统误差；

将式(22)改写成矩阵的形式，有：

其中，fⁿ×是fⁿ的反对称阵，是陀螺仪的零偏，是随机误差；

由于IMU安装在足部，当佩戴者脚跟落地时，IMU的实际地速为0，构造观测方程为：

其中，H_v＝[0_3×3 I_3×3 0_3×3]是速度域观测矩阵，υ_v是速度测量误差，是惯性导航系统输出的地速；

离散时间状态空间方程由式(23)和式(24)离散化得到，表示为：

其中，x_k是k时刻的状态量，x_k-1是k-1时刻的状态量，z_k是k时刻的观测量，H_k是k时刻的观测矩阵，Φ_k，k-1≈I+F(t_k-1)·τ是状态转移矩阵，F(t_k-1)是t_k-1时刻的误差非线性函数，w_k-1是离散化的系统驱动噪声，υ_k是观测噪声，τ是状态更新的时间间隔；

为了保持滤波过程中估计始终无偏，初始状态为P₀＝Cov(x₀)；

由于状态向量全部是误差向量，所以状态的初始值为0，即对于初始状态估计误差的均方根矩阵P₀，描述如下：

其中，是初始失准角向量误差的方差，和初始对准有关系；是初始速度误差向量的方差，在静止的情况下速度误差为零，所以 是初始位置误差向量的方差，给定

按照下列公式及步骤进行滤波过程：

滤波过程完成后，得到误差状态的最小方差估计，误差状态校正对应状态变量的公式如下：

其中，是当前历元SINS系统的经过位置，是当前历元SINS系统的速度，是当前历元SINS系统输出的姿态矩阵。

进一步地，所述步骤7中使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点的方法如下：

假定行人室内轨迹首末两个轨迹点坐标分别为(x₁，y₁)和(x₂，y₂)，墙壁线段端点坐标为(x₃，y₃)和(x₄，y₄)，则通过几何方法计算出墙壁线段与行人室内轨迹的交点(p_x，p_y)：

其中，当和的取值范围都在0和1之间时，则判定行人室内轨迹和墙壁线段相交。

进一步地，所述步骤8中采用粒子群算法，对步骤5与实际室内地图对准后的行人室内轨迹进行粒子滤波的方法如下：

步骤8-1：确定粒子群的数目N，建立粒子群，初始化粒子群的中心位置、起始点以及每个粒子的权重；粒子的初始化起始点位置为行人室内轨迹原始轨迹的起点位置，每个粒子的初始化权重为

步骤8-2：对1到N个粒子群进行初始化，即以行人室内轨迹原始轨迹的起点位置为中心产生高斯白噪声，然后进行粒子群的更新；在粒子群更新的过程中，量测为行人室内轨迹原始轨迹的步长和航向。

有益效果：

本发明针对由于单一的PDR定位方法造成定位精度不高的问题，提出了基于粒子滤波的地图匹配行人室内定位设计方法，修正了由于PDR积分特性引起的定位误差，提高了定位精度，得到了更好的定位效果。

附图说明

图1为本发明方法的流程框架图。

图2为本发明PDR定位系统算法执行流程框架图。

图3为本发明实施例室内地图。

图4为本发明实施例经LSD线特征提取后的地图。

图5为本发明实施例穿墙点检测图。

图6为本发明实施例用两种定位方法的仿真图。

具体实施方式

下面结合附图和实施例对本发明进一步说明。

如图1所示，一种融合PDR和先验地图的行人室内定位方法，包括如下步骤：

步骤1：绘制室内地图；

步骤2：对IMU进行标定处理，使用IMU采集行人室内运动数据；

步骤3：使用IMU数据，利用PDR定位方法得到行人室内轨迹；

步骤4：对行人室内轨迹进行平滑处理；

步骤5：对平滑处理后的行人室内轨迹进行旋转和平移，与室内地图对准；

步骤6：对室内地图进行LSD线特征提取，把室内地图转化为具有墙壁特征的地图向量矩阵；

步骤7：将步骤5与室内地图对准后的行人室内轨迹转换成轨迹向量矩阵；使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点；

步骤8：采用粒子群算法，对步骤5与室内地图对准后的行人室内轨迹进行粒子滤波；

步骤9：在粒子滤波过程中，使用LSI算法检测粒子群中是否有穿墙的粒子，并对穿墙的粒子进行权重归零，得到地图信息修正后的粒子群；

步骤10：粒子滤波完成后，计算出粒子群所表示的最优值，得到最优坐标，即最优行人室内轨迹。

进一步地，所述步骤1中绘制室内地图采用ARCGIS进行绘制。

进一步地，所述步骤2中对IMU进行标定处理的方法为：将IMU器件放入二十一面体中，然后把每个平面静止十秒，利用采集到的数据对IMU进行标定处理。

进一步地，所述步骤3中使用IMU数据，利用PDR定位方法得到原始的行人室内轨迹，包括如下步骤：

步骤3-1：MSINS初始对准：确定初始捷联矩阵；

步骤3-2：MSINS捷联解算；

进行四元数更新，即求解下式四元数微分方程：

其中，n表示导航坐标系，矩阵表示载体坐标系相对于n系的姿态四元数，定义载体坐标系为b系，是姿态角速率；

使用等效旋转矢量法求解(1)式，通过旋转矩阵链乘规则将式(1)写成数字递推的形式，如式(2)所示：

其中，是t_k时刻从b系到n系的变换四元数，是t_k-1时刻的姿态四元数；是以地心惯性坐标系为参考系时，n系从t_k-1时刻到t_k时刻的变换四元数，定义地心惯性坐标系为i系；是以i系为参考系时，b系从t_k-1时刻到t_k时刻的变换四元数；

计算n系从t_k-1时刻到t_k时刻的转动等效旋转矢量：

其中，是t时刻在导航坐标系下的角速度，是k-1时刻在导航坐标系下的角速度，是k时刻在导航坐标系下的角速度，τ_k＝t_k-t_k-1是采样间隔；

根据等效旋转与旋转四元数之间的转换关系，得到：

记b系从t_k-1时刻到t_k时刻的转动等效旋转矢量为采用角增量二子样等效旋转算法进行求解：

Δθ_k＝Δθ_k，1+Δθ_k，2 (6)

其中，是陀螺仪测量得到的载体角速度，是一个快变量；Δθ_k是陀螺仪在t_k-1时刻到t_k时刻的角增量；Δθ_k，1和Δθ_k，2分别对应于陀螺仪从t_k-1时刻到时刻和从时刻到t_k时刻的角增量，计算方法为：

其中是t_k-1时刻的载体角速度，是时刻的载体角速度，是t_k时刻的载体角速度；

采用高速率采样，使用单子样算法求解计算方法为：

其中是单子样方法下t_k-1时刻的载体角速度，是单子样方法下t_k时刻的载体角速度；

根据等效旋转矢量与旋转四元数之间的转换关系，得到：

将式(4)和式(10)代入到(1)式中，即完成姿态更新过程；

如果和有关的为[1，0，0，0]^T，则式(1)简化为：

对式(9)进行简化，得到：

步骤3-3：行人步态检测和静止检测；

对于陀螺仪向量序列和加速度计向量序列 构造如下统计量：

式中，是陀螺仪在x轴的M个序列的平方和，是陀螺仪在y轴的M个序列的平方和，是陀螺仪在z轴的M个序列的平方和； 是加速度计在x轴的M个序列的平方和，是加速度计在y轴的M个序列的平方和，是加速度计在z轴的M个序列的平方和； 和分别是陀螺仪和加速度计向量的二范数平方；是陀螺仪序列的均值，是加速度计序列的均值；

由均值不等式，存在以下不等式的成立：

式中，分别是陀螺仪x轴、y轴、z轴采样角速率序列{ω_ix}、{ω_iy}、{ω_iz}的均值； 分别是加速度计x轴、y轴、z轴采样比力序列{f_ix}、{f_iy}、{f_iz}的均值；上面不等式等号成立的条件是各测量轴的采样序列元素全部相等；

式(16)表明，当序列各元素的离差越大，不等式两端的差值越大；

对于陀螺仪，当其处于静态时，三个测量轴输出几乎为零，可以直接利用式(16)两端之差构造反映角运动动态程度的特征量，如式(18)：

对于加速度计，加速度计输出的比力数据含有重力加速度g分量，先从合矢量中把g扣除，按式(19)方法进行处理：

再将式(19)右侧的替换成式(17)的右侧项，构造出反映线运动动态程度的特征量为：

最后，综合陀螺仪和加速度计的信息，构成既反映线运动又反映角运动动态程度的特征量为：

其中，c是角运动特征影响因子；影响因子c用于调节陀螺和加速度计的数据量级到等同的程度，使用序列方差的比值作为c的值；

综上所述，特征量和Δ²都用于检测载体运动的动态程度，通过设置特征量和Δ²阈值来达到对目标运动特性检测的目的；

步骤3-4：ZUPT卡尔曼滤波器设计；

滤波器的误差状态选择为九维，具体如下：

x＝[(δpⁿ)^T，(δvⁿ)^T，(φⁿ)^T]^T (22)

其中，x是误差状态向量，δpⁿ是位置误差，δvⁿ是速度误差，φⁿ是姿态失准角，三者都是SINS系统误差；

将式(22)改写成矩阵的形式，有：

其中，fⁿ×是fⁿ的反对称阵，是陀螺仪的零偏，是随机误差；

由于IMU安装在足部，当佩戴者脚跟落地时，IMU的实际地速为0，构造观测方程为：

其中，H_v＝[0_3×3 I_3×3 0_3×3]是速度域观测矩阵，υ_v是速度测量误差，是惯性导航系统输出的地速；

离散时间状态空间方程由式(23)和式(24)离散化得到，表示为：

其中，x_k是k时刻的状态量，x_k-1是k-1时刻的状态量，z_k是k时刻的观测量，H_k是k时刻的观测矩阵，Φ_k，k-1≈I+F(t_k-1)·τ是状态转移矩阵，F(t_k-1)是t_k-1时刻的误差非线性函数，w_k-1是离散化的系统驱动噪声，υ_k是观测噪声，τ是状态更新的时间间隔；

为了保持滤波过程中估计始终无偏，初始状态为P₀＝Cov(x₀)；

由于状态向量全部是误差向量，所以状态的初始值为0，即对于初始状态估计误差的均方根矩阵P₀，描述如下：

其中，是初始失准角向量误差的方差，和初始对准有关系；是初始速度误差向量的方差，在静止的情况下速度误差为零，所以 是初始位置误差向量的方差，给定

按照下列公式及步骤进行滤波过程：

滤波过程完成后，得到误差状态的最小方差估计，误差状态校正对应状态变量的公式如下：

其中，是当前历元SINS系统的经过位置，是当前历元SINS系统的速度，是当前历元SINS系统输出的姿态矩阵。

进一步地，所述步骤7中使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点的方法如下：

假定行人室内轨迹首末两个轨迹点坐标分别为(x₁，y₁)和(x₂，y₂)，墙壁线段端点坐标为(x₃，y₃)和(x₄，y₄)，则通过几何方法计算出墙壁线段与行人室内轨迹的交点(p_x，p_y)：

其中，当和的取值范围都在0和1之间时，则判定行人室内轨迹和墙壁线段相交。

进一步地，所述步骤8中采用粒子群算法，对步骤5与实际室内地图对准后的行人室内轨迹进行粒子滤波的方法如下：

步骤8-1：确定粒子群的数目N，建立粒子群，初始化粒子群的中心位置、起始点以及每个粒子的权重；粒子的初始化起始点位置为行人室内轨迹原始轨迹的起点位置，每个粒子的初始化权重为

步骤8-2：对1到N个粒子群进行初始化，即以行人室内轨迹原始轨迹的起点位置为中心产生高斯白噪声，然后进行粒子群的更新；在粒子群更新的过程中，量测为行人室内轨迹原始轨迹的步长和航向。

具体实施例：

位置服务分为室内室外，在室外有全球定位系统辅助进行定位。但在室内环境里，由于墙体结构复杂等原因导致无法接收卫星信号。所以PDR由于其无源特性成为了室内定位一种好的方法。但是定位误差会随着时间而积累，虽然定位误差可以利用惯性元件IMU来采集行人的步态数据，然后进行步态检测，判断出行人静止的时刻，并以该时刻下的速度作为卡尔曼滤波的观测量，进而对定位误差进行修正。但是由于步态检测的方法可能不够准确，所以定位误差依旧会存在。因此为了修正定位误差，提高定位精度，便加入室内地图这一先验信息，通过PDR和先验地图的融合进一步校正定位误差，从而提高行人室内定位的精度。

本实施例针对由于单一的PDR定位方法造成定位精度不高的问题，提出了基于粒子滤波的地图匹配行人室内定位设计方法，修正了由于PDR积分特性引起的定位误差，提高了定位精度，得到了更好的定位效果。

1、把IMU调试好安装在脚后跟的位置，然后在室内行走采集行走数据。

2、使用ARCGIS绘制行人的室内地图，将ARCGIS图层中加入线元素，然后利用线元素将室内布局转化为室内地图产生地图信息并输出为图片。绘制好的地图如图3所示。

3、加载IMU采集到的行人数据，然后进行PDR算法的实现与仿真，得到初始的行人室内轨迹。

4、由室内地图得到地图信息矩阵，如图4所示。然后进行先验地图和PDR产生的原始行人轨迹的融合，得到修正后的行人室内轨迹。

5、比较融合先验地图前如图5所示和融合先验地图后如图6所示的定位效果。从图5中可以看到：在没有融合先验地图之前，仅由PDR算法产生的行人轨迹不仅偏离了真实轨迹而且还存在10个穿墙点，明显不符合真实情况，并且造成了严重的定位误差，走了一圈后没有回到起始点。但是，在融合了先验地图之后，从图6可以看到：修正后的轨迹不仅没有了穿墙点，更加符合实际情况；而且也更加趋向真实轨迹并且在走了一圈后也回到了起始点。

因此，经过分析，可以表明：融合先验地图和PDR是一种行之有效的行人室内定位方法。

Claims (5)

1.一种融合PDR和先验地图的行人室内定位方法，其特征在于，包括以下步骤：

步骤1：绘制室内地图；

步骤2：对IMU进行标定处理，使用IMU采集行人室内运动数据；

步骤3：使用IMU数据，利用PDR定位方法得到行人室内轨迹；

步骤3-1：MSINS初始对准：确定初始捷联矩阵；

步骤3-2：MSINS捷联解算；

进行四元数更新，即求解下式四元数微分方程：

其中，n表示导航坐标系，矩阵表示载体坐标系相对于n系的姿态四元数，定义载体坐标系为b系，是姿态角速率；

使用等效旋转矢量法求解(1)式，通过旋转矩阵链乘规则将式(1)写成数字递推的形式，如式(2)所示：

其中，是t_k时刻从b系到n系的变换四元数，是t_k-1时刻的姿态四元数；是以地心惯性坐标系为参考系时，n系从t_k-1时刻到t_k时刻的变换四元数，定义地心惯性坐标系为i系；是以i系为参考系时，b系从t_k-1时刻到t_k时刻的变换四元数；

计算n系从t_k-1时刻到t_k时刻的转动等效旋转矢量：

其中，是t时刻在导航坐标系下的角速度，是k-1时刻在导航坐标系下的角速度，是k时刻在导航坐标系下的角速度，τ_k＝t_k-t_k-1是采样间隔；

根据等效旋转与旋转四元数之间的转换关系，得到：

记b系从t_k-1时刻到t_k时刻的转动等效旋转矢量为采用角增量二子样等效旋转算法进行求解：

Δθ_k＝Δθ_k,1+Δθ_k,2 (6)

其中，是陀螺仪测量得到的载体角速度，是一个快变量；△θ_k是陀螺仪在t_k-1时刻到t_k时刻的角增量；Δθ_k,1和Δθ_k,2分别对应于陀螺仪从t_k-1时刻到时刻和从时刻到t_k时刻的角增量，计算方法为：

其中是t_k-1时刻的载体角速度，是时刻的载体角速度，是t_k时刻的载体角速度；

采用高速率采样，使用单子样算法求解计算方法为：

其中是单子样方法下t_k-1时刻的载体角速度，是单子样方法下t_k时刻的载体角速度；

根据等效旋转矢量与旋转四元数之间的转换关系，得到：

将式(4)和式(10)代入到(1)式中，即完成姿态更新过程；

如果和有关的为[1,0,0,0]^T，则式(1)简化为：

对式(9)进行简化，得到：

步骤3-3：行人步态检测和静止检测；

对于陀螺仪向量序列和加速度计向量序列 构造如下统计量：

式中，是陀螺仪在x轴的M个序列的平方和，是陀螺仪在y轴的M个序列的平方和，是陀螺仪在z轴的M个序列的平方和； 是加速度计在x轴的M个序列的平方和，是加速度计在y轴的M个序列的平方和，是加速度计在z轴的M个序列的平方和； 和分别是陀螺仪和加速度计向量的二范数平方；是陀螺仪序列的均值，是加速度计序列的均值；

由均值不等式，存在以下不等式的成立：

式中，分别是陀螺仪x轴、y轴、z轴采样角速率序列{ω_ix}、{ω_iy}、{ω_iz}的均值； 分别是加速度计x轴、y轴、z轴采样比力序列{f_ix}、{f_iy}、{f_iz}的均值；上面不等式等号成立的条件是各测量轴的采样序列元素全部相等；

式(16)表明，当序列各元素的离差越大，不等式两端的差值越大；

对于陀螺仪，当其处于静态时，三个测量轴输出几乎为零，可以直接利用式(16)两端之差构造反映角运动动态程度的特征量，如式(18)：

对于加速度计，加速度计输出的比力数据含有重力加速度g分量，先从合矢量中把g扣除，按式(19)方法进行处理：

再将式(19)右侧的替换成式(17)的右侧项，构造出反映线运动动态程度的特征量为：

最后，综合陀螺仪和加速度计的信息，构成既反映线运动又反映角运动动态程度的特征量为：

其中，c是角运动特征影响因子；影响因子c用于调节陀螺和加速度计的数据量级到等同的程度，使用序列方差的比值作为c的值；

综上所述，特征量和Δ²都用于检测载体运动的动态程度，通过设置特征量和Δ²阈值来达到对目标运动特性检测的目的；

步骤3-4：ZUPT卡尔曼滤波器设计；

滤波器的误差状态选择为九维，具体如下：

其中，x是误差状态向量，δpⁿ是位置误差，δvⁿ是速度误差，φⁿ是姿态失准角，三者都是SINS系统误差；

将式(22)改写成矩阵的形式，有：

其中，fⁿ×是fⁿ的反对称阵，是陀螺仪的零偏，是随机误差；

由于IMU安装在足部，当佩戴者脚跟落地时，IMU的实际地速为0，构造观测方程为：

其中，H_v＝[0_3×3 I_3×3 0_3×3]是速度域观测矩阵，υ_v是速度测量误差，是惯性导航系统输出的地速；

离散时间状态空间方程由式(23)和式(24)离散化得到，表示为：

其中，x_k是k时刻的状态量，x_k-1是k-1时刻的状态量，z_k是k时刻的观测量，H_k是k时刻的观测矩阵，Φ_k,k-1≈I+F(t_k-1)·τ是状态转移矩阵，F(t_k-1)是t_k-1时刻的误差非线性函数，w_k-1是离散化的系统驱动噪声，υ_k是观测噪声，τ是状态更新的时间间隔；

为了保持滤波过程中估计始终无偏，初始状态为P₀＝Cov(x₀)；

由于状态向量全部是误差向量，所以状态的初始值为0，即对于初始状态估计误差的均方根矩阵P₀，描述如下：

其中，是初始失准角向量误差的方差，和初始对准有关系；是初始速度误差向量的方差，在静止的情况下速度误差为零，所以 是初始位置误差向量的方差，给定

按照下列公式及步骤进行滤波过程：

滤波过程完成后，得到误差状态的最小方差估计，误差状态校正对应状态变量的公式如下：

其中，是当前历元SINS系统的经过位置，是当前历元SINS系统的速度，是当前历元SINS系统输出的姿态矩阵；

步骤4：对行人室内轨迹进行平滑处理；

步骤5：对平滑处理后的行人室内轨迹进行旋转和平移，与室内地图对准；

步骤6：对室内地图进行LSD线特征提取，把室内地图转化为具有墙壁特征的地图向量矩阵；

步骤7：将步骤5与室内地图对准后的行人室内轨迹转换成轨迹向量矩阵；使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点；

步骤8：采用粒子群算法，对步骤5与室内地图对准后的行人室内轨迹进行粒子滤波；

步骤9：在粒子滤波过程中，使用LSI算法检测粒子群中是否有穿墙的粒子，并对穿墙的粒子进行权重归零，得到地图信息修正后的粒子群；

步骤10：粒子滤波完成后，计算出粒子群所表示的最优值，得到最优坐标，即最优行人室内轨迹。

2.根据权利要求1所述的一种融合PDR和先验地图的行人室内定位方法，其特征在于，所述步骤1中绘制室内地图采用ARCGIS进行绘制。

3.根据权利要求1所述的一种融合PDR和先验地图的行人室内定位方法，其特征在于，所述步骤2中对IMU进行标定处理的方法为：将IMU器件放入二十一面体中，然后把每个平面静止十秒，利用采集到的数据对IMU进行标定处理。

4.根据权利要求1所述的一种融合PDR和先验地图的行人室内定位方法，其特征在于，所述步骤7中使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点的方法如下：

假定行人室内轨迹首末两个轨迹点坐标分别为(X₁,y₁)和(x₂,y₂)，墙壁线段端点坐标为(x₃,y₃)和(x₄,y₄)，则通过几何方法计算出墙壁线段与行人室内轨迹的交点(p_x,p_y)：

其中，当和的取值范围都在0和1之间时，则判定行人室内轨迹和墙壁线段相交。

5.根据权利要求1所述的一种融合PDR和先验地图的行人室内定位方法，其特征在于，所述步骤8中采用粒子群算法，对步骤5与实际室内地图对准后的行人室内轨迹进行粒子滤波的方法如下：

步骤8-1：确定粒子群的数目N，建立粒子群，初始化粒子群的中心位置、起始点以及每个粒子的权重；粒子的初始化起始点位置为行人室内轨迹原始轨迹的起点位置，每个粒子的初始化权重为

步骤8-2：对1到N个粒子群进行初始化，即以行人室内轨迹原始轨迹的起点位置为中心产生高斯白噪声，然后进行粒子群的更新；在粒子群更新的过程中，量测为行人室内轨迹原始轨迹的步长和航向。