申请/专利权人：西北工业大学

申请日：2020-11-25

公开（公告）日：2024-01-09

公开（公告）号：CN112562077B

主分类号：G06T17/05

分类号：G06T17/05;G06T7/73;G06N3/006;G01C21/20;G01C21/18;G01C21/16;G01C21/14;G01C21/00

优先权：

专利状态码：有效-授权

法律状态：2024.01.09#授权;2021.04.13#实质审查的生效;2021.03.26#公开

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

主权项：1.一种融合PDR和先验地图的行人室内定位方法，其特征在于，包括以下步骤：步骤1：绘制室内地图；步骤2：对IMU进行标定处理，使用IMU采集行人室内运动数据；步骤3：使用IMU数据，利用PDR定位方法得到行人室内轨迹；步骤3-1：MSINS初始对准：确定初始捷联矩阵；步骤3-2：MSINS捷联解算；进行四元数更新，即求解下式四元数微分方程： 其中，n表示导航坐标系，矩阵表示载体坐标系相对于n系的姿态四元数，定义载体坐标系为b系，是姿态角速率；使用等效旋转矢量法求解1式，通过旋转矩阵链乘规则将式1写成数字递推的形式，如式2所示： 其中，是tk时刻从b系到n系的变换四元数，是tk-1时刻的姿态四元数；是以地心惯性坐标系为参考系时，n系从tk-1时刻到tk时刻的变换四元数，定义地心惯性坐标系为i系；是以i系为参考系时，b系从tk-1时刻到tk时刻的变换四元数；计算n系从tk-1时刻到tk时刻的转动等效旋转矢量： 其中，是t时刻在导航坐标系下的角速度，是k-1时刻在导航坐标系下的角速度，是k时刻在导航坐标系下的角速度，τk＝tk-tk-1是采样间隔；根据等效旋转与旋转四元数之间的转换关系，得到： 记b系从tk-1时刻到tk时刻的转动等效旋转矢量为采用角增量二子样等效旋转算法进行求解： Δθk＝Δθk,1+Δθk,26其中，是陀螺仪测量得到的载体角速度，是一个快变量；△θk是陀螺仪在tk-1时刻到tk时刻的角增量；Δθk,1和Δθk,2分别对应于陀螺仪从tk-1时刻到时刻和从时刻到tk时刻的角增量，计算方法为： 其中是tk-1时刻的载体角速度，是时刻的载体角速度，是tk时刻的载体角速度；采用高速率采样，使用单子样算法求解计算方法为： 其中是单子样方法下tk-1时刻的载体角速度，是单子样方法下tk时刻的载体角速度；根据等效旋转矢量与旋转四元数之间的转换关系，得到： 将式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轴采样比力序列{fix}、{fiy}、{fiz}的均值；上面不等式等号成立的条件是各测量轴的采样序列元素全部相等；式16表明，当序列各元素的离差越大，不等式两端的差值越大；对于陀螺仪，当其处于静态时，三个测量轴输出几乎为零，可以直接利用式16两端之差构造反映角运动动态程度的特征量，如式18： 对于加速度计，加速度计输出的比力数据含有重力加速度g分量，先从合矢量中把g扣除，按式19方法进行处理： 再将式19右侧的替换成式17的右侧项，构造出反映线运动动态程度的特征量为： 最后，综合陀螺仪和加速度计的信息，构成既反映线运动又反映角运动动态程度的特征量为： 其中，c是角运动特征影响因子；影响因子c用于调节陀螺和加速度计的数据量级到等同的程度，使用序列方差的比值作为c的值；综上所述，特征量和Δ2都用于检测载体运动的动态程度，通过设置特征量和Δ2阈值来达到对目标运动特性检测的目的；步骤3-4：ZUPT卡尔曼滤波器设计；滤波器的误差状态选择为九维，具体如下： 其中，x是误差状态向量，δpn是位置误差，δvn是速度误差，φn是姿态失准角，三者都是SINS系统误差；将式22改写成矩阵的形式，有： 其中，fn×是fn的反对称阵，是陀螺仪的零偏，是随机误差；由于IMU安装在足部，当佩戴者脚跟落地时，IMU的实际地速为0，构造观测方程为： 其中，Hv＝[03×3I3×303×3]是速度域观测矩阵，υv是速度测量误差，是惯性导航系统输出的地速；离散时间状态空间方程由式23和式24离散化得到，表示为： 其中，xk是k时刻的状态量，xk-1是k-1时刻的状态量，zk是k时刻的观测量，Hk是k时刻的观测矩阵，Φk,k-1≈I+Ftk-1·τ是状态转移矩阵，Ftk-1是tk-1时刻的误差非线性函数，wk-1是离散化的系统驱动噪声，υk是观测噪声，τ是状态更新的时间间隔；为了保持滤波过程中估计始终无偏，初始状态为P0＝Covx0；由于状态向量全部是误差向量，所以状态的初始值为0，即对于初始状态估计误差的均方根矩阵P0，描述如下： 其中，是初始失准角向量误差的方差，和初始对准有关系；是初始速度误差向量的方差，在静止的情况下速度误差为零，所以是初始位置误差向量的方差，给定按照下列公式及步骤进行滤波过程： 滤波过程完成后，得到误差状态的最小方差估计，误差状态校正对应状态变量的公式如下： 其中，是当前历元SINS系统的经过位置，是当前历元SINS系统的速度，是当前历元SINS系统输出的姿态矩阵；步骤4：对行人室内轨迹进行平滑处理；步骤5：对平滑处理后的行人室内轨迹进行旋转和平移，与室内地图对准；步骤6：对室内地图进行LSD线特征提取，把室内地图转化为具有墙壁特征的地图向量矩阵；步骤7：将步骤5与室内地图对准后的行人室内轨迹转换成轨迹向量矩阵；使用LSI算法对地图向量矩阵和轨迹向量矩阵进行判断，得到行人室内轨迹与墙壁的交叉点，即穿墙点；步骤8：采用粒子群算法，对步骤5与室内地图对准后的行人室内轨迹进行粒子滤波；步骤9：在粒子滤波过程中，使用LSI算法检测粒子群中是否有穿墙的粒子，并对穿墙的粒子进行权重归零，得到地图信息修正后的粒子群；步骤10：粒子滤波完成后，计算出粒子群所表示的最优值，得到最优坐标，即最优行人室内轨迹。

全文数据：

权利要求：

百度查询： 西北工业大学 一种融合PDR和先验地图的行人室内定位方法

免责声明
1、本报告根据公开、合法渠道获得相关数据和信息，力求客观、公正，但并不保证数据的最终完整性和准确性。
2、报告中的分析和结论仅反映本公司于发布本报告当日的职业理解，仅供参考使用，不能作为本公司承担任何法律责任的依据或者凭证。

阅读全文 双屏查看 官方信息 专利公告 收藏专利 下载PDF 下载WORD