CN113984046B - 一种基于体域惯性传感器网多特征融合的高精度室内定位方法 - Google Patents

一种基于体域惯性传感器网多特征融合的高精度室内定位方法 Download PDF

Info

Publication number
CN113984046B
CN113984046B CN202111243967.9A CN202111243967A CN113984046B CN 113984046 B CN113984046 B CN 113984046B CN 202111243967 A CN202111243967 A CN 202111243967A CN 113984046 B CN113984046 B CN 113984046B
Authority
CN
China
Prior art keywords
acceleration
foot
point
sensor network
fusion
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111243967.9A
Other languages
English (en)
Other versions
CN113984046A (zh
Inventor
于宁
马晓峰
冯仁剑
吴银锋
陈烜赫
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Beihang University
Original Assignee
Beihang University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Beihang University filed Critical Beihang University
Priority to CN202111243967.9A priority Critical patent/CN113984046B/zh
Publication of CN113984046A publication Critical patent/CN113984046A/zh
Application granted granted Critical
Publication of CN113984046B publication Critical patent/CN113984046B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • GPHYSICS
    • G01MEASURING; TESTING
    • G01CMEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/166Mechanical, construction or arrangement details of inertial navigation systems
    • YGENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02DCLIMATE CHANGE MITIGATION TECHNOLOGIES IN INFORMATION AND COMMUNICATION TECHNOLOGIES [ICT], I.E. INFORMATION AND COMMUNICATION TECHNOLOGIES AIMING AT THE REDUCTION OF THEIR OWN ENERGY USE
    • Y02D30/00Reducing energy consumption in communication networks
    • Y02D30/70Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Navigation (AREA)

Abstract

本文提出基于体域惯性传感网多特征融合的高精度室内定位方法,属于微型惯性传感器定位领域,包括:步骤一:确定惯性传感网最小系统;步骤二:提出了一种基于球铰链模型融合的精确步长估计方法;步骤三:提出了一种基于多节点加权融合的航向角融合方法。本发明同时考虑了步长和偏航角对航位推算的影响,并分别提出了有针对性的优化方案。在长时间、远距离定位中取得了良好的效果。

Description

一种基于体域惯性传感器网多特征融合的高精度室内定位 方法
技术领域
本发明属于微型惯性传感器定位领域,特别是无第三方辅助情况下的长时间高精度定位,具体为一种基于体域惯性传感器网络多特征融合的高精度定位方法。
背景技术
定位系统是物联网应用中一个令人注目的研究领域,例如机器人、车辆和人员的定位,需求涉及保障施工人员安全;协助养老院、疫情隔离场所搭建保障平台;提高应急救援的位置搜救效率;等等。同时,进入物联网大数据时代的室内定位导航,还希望基于精准的人员位置信息,获得商场等大型建筑的人流热力图分析,以及各品牌、商家甚至是商品的精准位置数据,从而建立智能的万物互联和获取极具价值的大数据统计。
目前常见定位技术有UWB测距技术、RFID、雷达、基于WIFI或蓝牙的RSSI测距定位,这些技术容易受到多径传播等环境因素的干扰。基于指纹库的wifi或蓝牙定位也是基于物联网的定位方法,且是目前应用比较广泛的定位系统。除此之外,还有一些其他的定位技术,例如利用磁场指纹定位,其比较容易受到室内磁场环境的干扰。这些无线定位方式,均有其存在的优势和缺点,一般来说,无线定位方式容易受到周围复杂环境干扰,且需要提前布设相关的接入点或基准信标。这时,基于惯性传感器的定位方法称为复杂环境中简单有效的定位方法。
惯性导航定位技术无需提前布设信标等基础设施,仅基于人员自身携带的惯性测量单元(IMU)进行航迹推算,即可获取实时位置信息,具有自主性强、复杂环境适应性好和不易受干扰等优点。针对应急救援等突发灾害性场合,基站和无线通讯都处于瘫痪状态,惯性导航定位技术无需任何额外的基础设施或网络布设,可以实现在复杂环境中人员的实时定位。但是可供人员随身穿戴式携带的商用级惯性系统,随着导航定位时间的推移,其定位精度会因惯性器件固有的漂移等累计误差的影响而大幅降低,且惯性传感器本身的精度和噪声也会对航迹推算产生影响。
基于IMU的航迹推算方法可以分类两大类:捷联惯导(SINS)和Pedestrian DeadReckoning(PDR)。SINS是直接利用IMU所有时刻数据的一种递推式方法,它首先通过陀螺仪数据计算旋转矩阵(DCM),然后利用DCM将加速度转换到导航坐标系,再对加速度进行积分得到每一时刻的位置。PDR是间接利用IMU数据的一种递推式方法,它仅利用一步内某一时刻的航向和一步的步长进行递推推算轨迹,因此相对SINS对mems级别的IMU噪声容忍度更大。这两类航迹推算的定位精度均依赖于航向角和位移两大因素。其中,航向角可以通过磁力计或陀螺仪估计得到,但室内中磁场环境复杂,磁力计往往受到严重干扰而变得不可靠,在缺乏外部参考设备的情况下,基于陀螺仪估计的航向角漂移往往难以被有效的抑制。虽然ZUPT技术可以对SINS的轨迹的位移长度进行较为有效的误差修正,但由于航向角的漂移,导致水平加速度在导航坐标系x轴和y轴的分量存在误差,由此得到的位置坐标远远偏离真实坐标;PDR中的步长估计一般通过步长公式计算,然而步长的估计往往容易出错,且大多依赖于针对个体的前期调参,这给应用带来不便,同时,PDR也面临航向角漂移导致的坐标计算不准确的问题。
发明内容
本发明的目的是提出一种基于体域惯性传感网多特征融合的高精度室内定位方法,通过融合脚部IMU、小腿部IMU、腰部IMU和基于IMU识别的室内特征地标信息,实现高精度的室内定位。研究融合了惯性传感器网络多节点信息以及不同航迹推算方法各自优点的方法,基于球铰链模型融合脚部和小腿部的惯性传感网信息,实现自适应变速运动的准确步长计算;基于多部位惯性传感网节点信息加权融合的方法,实现航向角的准确获取和误差有效抑制;基于地标的全局多特征融合的位置求精,实现位置的阶段性历史误差清除。最终实现高精度的室内定位效果。
本发明的目的是提出一种基于体域惯性传感网多特征融合的高精度室内定位方法,具体包括以下步骤:
步骤一:惯性传感网络部署,阐述了惯性网最小系统的确定过程;
体域惯性传感网部节点由脚部、小腿部和腰部惯性传感器(IMU)构成。其中,IMU安装部位是基于大量运动实验分析得到的可用于安装传感器的身体各部位的运动特点以及各部位提供的可用于航迹推算的信息,而确定出的传感器网络布设位置最小系统。
脚部陀螺仪x轴可以反映完整的步态周期信息;小腿部陀螺仪x轴可反映相对完整的步态周期信息,同时可有效的区分上下楼梯与走跑状态;腰部加速度计z轴反映了清晰的重心变化特点。
步骤二:基于小腿和脚部球铰链模型融合的精确步长估计方法;
1)基于球铰链模型约束的步长计算
利用球铰链模型的加速度约束关系将融合脚部与小腿的信息,
记小腿部IMU安装于D点,其局部载体坐标系记为d系,脚部IMU安装于F点,其局部载体坐标系记为f系,踝关节为O点。根据刚体的运动加速度关系可以得到
Figure GDA0003360607650000021
Figure GDA0003360607650000022
其中,
Figure GDA0003360607650000023
和/>
Figure GDA0003360607650000024
分别为关节中心加速度在d系和f系下的表示,/>
Figure GDA0003360607650000025
和/>
Figure GDA0003360607650000026
分别为D点F点的运动加速度,/>
Figure GDA0003360607650000027
为D点相对O点的加速度,/>
Figure GDA0003360607650000028
为F点相对O点的加速度,刚体上的点相对旋转中心的加速度又满足
Figure GDA0003360607650000029
其中,由于D、F点相对关节中心的运动线速度为0,因此他们相对O点的相对运动加速度
Figure GDA0003360607650000031
以及科氏加速度/>
Figure GDA0003360607650000032
均为0。带入转动向心加速度和切向加速度最终得到
Figure GDA0003360607650000033
/>
Figure GDA0003360607650000034
其中,
Figure GDA0003360607650000035
和/>
Figure GDA0003360607650000036
分别为小腿部和脚部陀螺仪输出角速度在d系和f系下的表示,
Figure GDA0003360607650000037
和/>
Figure GDA0003360607650000038
分别为两个IMU相对于踝关节中心的位置向量,由于IMU被固定于脚部确定位置,因此这两个向量可以通过测量获得,角加速度按照下式进行计算
Figure GDA0003360607650000039
其中
Figure GDA00033606076500000310
为角加速度,ω(t)为t时刻角速度。
又根据加速度计原理,易得
Figure GDA00033606076500000311
Figure GDA00033606076500000312
其中,
Figure GDA00033606076500000313
和/>
Figure GDA00033606076500000314
分别为小腿部和脚部加速度计输出比力在d系和f系下的表示,gd和gf分别为重力加速度在d系下和f系下的表示。
通过小腿局部方向余弦矩阵(DCM)转换到导航坐标系下,得到脚踝关节的运动加速度
Figure GDA00033606076500000315
Figure GDA00033606076500000316
Figure GDA00033606076500000317
脚部局部DCM/>
Figure GDA00033606076500000318
式(5)、式(2)、式(8),得估算脚部DCM的扩展卡尔曼滤波器的量测方程:
Figure GDA00033606076500000319
2)设置扩展卡尔曼滤波器
通过扩展卡尔曼滤波器估算脚部DCM。状态取为脚部的四元数λ P1 P2 P2、脚部IMU陀螺仪零偏εx εy εz、脚部IMU加速度计零偏
Figure GDA00033606076500000320
构成10维状态量/>
Figure GDA00033606076500000321
Figure GDA00033606076500000322
以n系下的重力加速度为测量值,利用式(10),构建状态空间表达式和量测方程。
Figure GDA00033606076500000323
Figure GDA00033606076500000324
/>
其中,
Figure GDA00033606076500000325
为陀螺仪三轴输出,Γk-1为系统噪声传递阵,Wk-1为系统噪声,由陀螺仪噪声引起,/>
Figure GDA0003360607650000041
为加速度计三轴输出,/>
Figure GDA0003360607650000042
为加速度计三轴零偏,gn=[0 0 -g]T,/>
Figure GDA0003360607650000043
为由载体坐标系(b系)到导航坐标系(n系)的旋转矩阵(DCM),其可由四元数计算得到,如式(13)所示。
Figure GDA0003360607650000044
量测更新需要在零速率状态时进行,以避免比力中会包含运动加速度干扰。
由式(14)将加速度投影至导航坐标系,并在每一步的摆动期内进行积分
Figure GDA0003360607650000045
其中,k指第k个单步,积分区间为第k个单步内的零速率区间之外的采样时刻,步伐的划分也可以通过零速修正得到。
步骤三:基于多节点加权融合的精确航向角获取;
分别记脚部、小腿和腰部三个部位的航向角为Ψfoo-和Ψshank、Ψwais-、。然后由式(15),对三部位航向角数据进行加权融合,其中βfoo-、βshank和βwais-为融合权重。
Ψfuse,k=βfoo-Ψfoo-,kshankΨshank,kwaistΨwaist,k (15)
其中,Ψfuse,k为融合后的航向角。系数βfoot、βshank和βwaist的解算方法如下:
基于行人航迹推算(PDR)框架有式(16)
Figure GDA0003360607650000046
其中,posk+1为第k步的终点坐标,第k步的终点坐标为第k+1步的起点坐标。
假设行人到达位置M1,则可得到以位置M0为起点进行迭代计算的坐标表达式,如式(17)所示
Figure GDA0003360607650000047
其中N为地标M0到地标M1的之间的步数。
利用牛顿迭代法求解三个参数βfoot、βshank和βwaist
Figure GDA0003360607650000048
其中
Figure GDA0003360607650000049
为以位置M0为起点进行PDR推算时到达位置Mj时的估计坐标,/>
Figure GDA00033606076500000410
为位置Mj的坐标值。β=[βfootshankwaist]T,为F(β)=0的解。求解得到的β值,是使得PDR推算的位置估计值与真实值最接近的航向角融合系数,经此融合系数融合的航向角也是行进中经各个位置验证的最优航向角。/>
附图说明
图1为本发明提出的一种基于体域惯性传感器网络多特征融合的高精度定位方法流程图;
图2为体域惯性传感网络部署示意图;
图3为PDR推算示意图;
图4为使用三个地标计算融合系数得到的效果图;
图5为三个单部件定位方法与本发明提出的融合方法定位效果图;
具体实施方式
下面将结合附图对本发明作进一步的详细说明。
本发明提出的一种基于体域惯性传感器网络多特征融合的高精度定位方法,如图1所示,具体包括以下步骤:
步骤一:惯性传感网络部署,阐述了惯性网最小系统的确定过程;
体域惯性传感网部节点由脚部、小腿部和腰部惯性传感器(IMU)构成。其中,IMU安装部位是基于大量运动实验分析得到的可用于安装传感器的身体各部位的运动特点以及各部位提供的可用于航迹推算的信息,而确定出的传感器网络布设位置最小系统,如图2所示。
脚部陀螺仪x轴可以反映完整的步态周期信息;小腿部陀螺仪x轴可反映相对完整的步态周期信息,同时可有效的区分上下楼梯与走跑状态;腰部加速度计z轴反映了清晰的重心变化特点。
步骤二:基于小腿和脚部球铰链模型融合的精确步长估计方法;
1)基于球铰链模型约束的步长计算
利用球铰链模型的加速度约束关系将融合脚部与小腿的信息,
记小腿部IMU安装于D点,其局部载体坐标系记为d系,脚部IMU安装于F点,其局部载体坐标系记为f系,踝关节为O点。根据刚体的运动加速度关系可以得到
Figure GDA0003360607650000051
Figure GDA0003360607650000052
其中,
Figure GDA0003360607650000053
和/>
Figure GDA0003360607650000054
分别为关节中心加速度在d系和f系下的表示,/>
Figure GDA0003360607650000055
和/>
Figure GDA0003360607650000056
分别为D点F点的运动加速度,/>
Figure GDA0003360607650000057
为D点相对O点的加速度,/>
Figure GDA0003360607650000058
为F点相对O点的加速度,刚体上的点相对旋转中心的加速度又满足
Figure GDA0003360607650000059
其中,由于D、F点相对关节中心的运动线速度为0,因此他们相对O点的相对运动加速度
Figure GDA00033606076500000510
以及科氏加速度/>
Figure GDA00033606076500000511
均为0。带入转动向心加速度和切向加速度最终得到
Figure GDA00033606076500000512
/>
Figure GDA00033606076500000513
其中,
Figure GDA00033606076500000514
和/>
Figure GDA00033606076500000515
分别为小腿部和脚部陀螺仪输出角速度在d系和f系下的表示,/>
Figure GDA00033606076500000516
和/>
Figure GDA00033606076500000517
分别为两个IMU相对于踝关节中心的位置向量,由于IMU被固定于脚部确定位置,因此这两个向量可以通过测量获得,角加速度按照下式进行计算
Figure GDA0003360607650000061
其中
Figure GDA0003360607650000062
为角加速度,ω(t)为t时刻角速度。
又根据加速度计原理,易得
Figure GDA0003360607650000063
Figure GDA0003360607650000064
其中,
Figure GDA0003360607650000065
和/>
Figure GDA0003360607650000066
分别为小腿部和脚部加速度计输出比力在d系和f系下的表示,gd和gf分别为重力加速度在d系下和f系下的表示。
通过小腿局部方向余弦矩阵(DCM)即方向余弦矩阵转换到导航坐标系下,得到脚踝关节的运动加速度
Figure GDA0003360607650000067
Figure GDA0003360607650000068
Figure GDA0003360607650000069
脚部局部DCM/>
Figure GDA00033606076500000610
式(5)、式(2)、式(8),得估算脚部DCM的扩展卡尔曼滤波器的量测方程:
Figure GDA00033606076500000611
2)设置扩展卡尔曼滤波器
通过扩展卡尔曼滤波器估算脚部DCM。状态取为脚部的四元数λ P1 P2 P2、脚部IMU陀螺仪零偏εx εy εz、脚部IMU加速度计零偏
Figure GDA00033606076500000612
构成10维状态量/>
Figure GDA00033606076500000613
Figure GDA00033606076500000614
以n系下的重力加速度为测量值,利用式(10),构建状态空间表达式和量测方程。
Figure GDA00033606076500000615
Figure GDA00033606076500000616
/>
其中,
Figure GDA00033606076500000617
为陀螺仪三轴输出,Γk-1为系统噪声传递阵,Wk-1为系统噪声,由陀螺仪噪声引起,/>
Figure GDA00033606076500000618
为加速度计三轴输出,/>
Figure GDA00033606076500000619
为加速度计三轴零偏,gn=[0 0 -g]T,/>
Figure GDA00033606076500000620
为由载体坐标系(b系)到导航坐标系(n系)的旋转矩阵(DCM),其可由四元数计算得到,如式(13)所示。
Figure GDA00033606076500000621
量测更新需要在零速率状态时进行,以避免比力中会包含运动加速度干扰。
由式(14)将加速度投影至导航坐标系,并在每一步的摆动期内进行积分
Figure GDA0003360607650000071
其中,k指第k个单步,积分区间为第k个单步内的零速率区间之外的采样时刻,步伐的划分也可以通过零速修正得到。
步骤三:基于多节点加权融合的精确航向角获取;
分别记脚部、小腿和腰部三个部位的航向角为Ψfoot和Ψshank、Ψwaist、。然后由式(15),对三部位航向角数据进行加权融合,其中βfoot、βshank和βwaist为融合权重。
Ψfuse,k=βfootΨfoot,kshankΨshank,kwaistΨwaist,k (15)
其中,Ψfuse,k为融合后的航向角。系数βfoot、βshank和βwaist的解算方法如下:
基于行人航迹推算(PDR)框架有式(16)
Figure GDA0003360607650000072
其中,posk+1为第k步的终点坐标,第k步的终点坐标为第k+1步的起点坐标。
假设行人到达位置M1,则可得到以位置M0为起点进行迭代计算的坐标表达式,如式(17)所示
Figure GDA0003360607650000073
其中N为地标M0到地标M1的之间的步数。
利用牛顿迭代法求解三个参数βfoot、βshank和βwais-
Figure GDA0003360607650000074
其中
Figure GDA0003360607650000075
为以位置M0为起点进行PDR推算时到达位置Mj时的估计坐标,/>
Figure GDA0003360607650000076
为位置Mj的坐标值。β=[βfootshankwaist]T,为F(β)=0的解。求解得到的β值,是使得PDR推算的位置估计值与真实值(如图4中/>
Figure GDA0003360607650000077
Figure GDA0003360607650000078
)最接近的航向角融合系数,经此融合系数融合的航向角也是行进中经各个位置验证的最优航向角。
通过上述分析,本发明针对现有室内定位方法过分依赖外部环境部署,以及迭代算位法不能单独进行长时间定位的问题,提出了一种基于惯性传感器网络的高精度室内定位方法。该方法结合安装在脚、小腿、腰部的IMU以及走廊、楼梯角等地标的数据,可以在不依赖外部设备的情况下实现长时间可靠定位。
所提出的融合方法包括两个内容:
1)基于球铰模型,融合脚和小腿信息,在不考虑个体相关参数的情况下获得适应变速运动的步长;
2)基于偏航角加权融合方法,基于地标观测数据融合足、小腿和腰部数据,获得更准确的偏航角;
在实际实施中,需要实验参与人员按照图2所示,将多个惯性传感器固定在脚部,小腿部,腰部进行进行试验,其中传感器的y轴冲向身体前进方向,z轴竖直向上。实验者在试验区域内进行走动,各个传感器记录加速度、角速度等数据。待测试者完成行走之后,将数据导出到电脑,使用软件进行计算。通过步骤二的方法更精确地得到步长,根据步骤三的方法更精确地得到偏航角等信息。
如图5所示,本发明提出的结果即深蓝色的曲线最接近实际轨迹。最终测试结果表明,所提出的方法能够很好地适应变速运动,偏航角的总体漂移率小于0.005°/s,定位精度的相对误差小于1%。而单部件偏航角的整体漂移率约为0.01°/s,定位精度的相对误差超过1%。该方法同时考虑了步长和偏航角对航位推算的影响,并分别提出了有针对性的优化方案。在长时间、远距离定位中取得了良好的效果。

Claims (5)

1.一种基于体域惯性传感网多特征融合的高精度室内定位方法,其特征在于:具体包括以下步骤:
步骤一:
体域惯性传感网部节点由脚部、小腿部和腰部惯性传感器IMU构成;
步骤二:
基于小腿和脚部球铰链模型融合的精确步长估计方法;
1)基于球铰链模型约束的步长计算
利用球铰链模型的加速度约束关系将融合脚部与小腿的信息:
记小腿部IMU安装于D点,其局部载体坐标系记为d系,脚部IMU安装于F点,其局部载体坐标系记为f系,踝关节为O点,根据刚体的运动加速度关系可以得到:
Figure QLYQS_1
Figure QLYQS_2
其中,
Figure QLYQS_3
和/>
Figure QLYQS_4
分别为关节中心加速度在d系和f系下的表示,/>
Figure QLYQS_5
和/>
Figure QLYQS_6
分别为D点F点的运动加速度,/>
Figure QLYQS_7
为D点相对O点的加速度,/>
Figure QLYQS_8
为F点相对O点的加速度,刚体上的点相对旋转中心的加速度又满足:
Figure QLYQS_9
其中,由于D、F点相对关节中心的运动线速度为0,因此他们相对O点的相对运动加速度
Figure QLYQS_10
以及科氏加速度/>
Figure QLYQS_11
均为0,带入转动向心加速度和切向加速度最终得到:
Figure QLYQS_12
Figure QLYQS_13
其中,
Figure QLYQS_14
和/>
Figure QLYQS_15
分别为小腿部和脚部陀螺仪输出角速度在d系和f系下的表示,/>
Figure QLYQS_16
Figure QLYQS_17
分别为两个IMU相对于踝关节中心的位置向量,由于IMU被固定于脚部确定位置,因此这两个向量可以通过测量获得,角加速度按照下式进行计算:
Figure QLYQS_18
其中
Figure QLYQS_19
为角加速度,ω(t)为t时刻角速度;
又根据加速度计原理,易得:
Figure QLYQS_20
Figure QLYQS_21
其中,
Figure QLYQS_22
和/>
Figure QLYQS_23
分别为小腿部和脚部加速度计输出比力在d系和f系下的表示,gd和gf分别为重力加速度在d系下和f系下的表示;/>
通过小腿局部方向余弦矩阵DCM转换到导航坐标系下,得到脚踝关节的运动加速度
Figure QLYQS_24
Figure QLYQS_25
Figure QLYQS_26
脚部局部DCM/>
Figure QLYQS_27
式(5)、式(2)、式(8),得估算脚部DCM的扩展卡尔曼滤波器的量测方程:
Figure QLYQS_28
2)设置扩展卡尔曼滤波器
通过扩展卡尔曼滤波器估算脚部DCM,状态取为脚部的四元数λP1P2P2、脚部IMU陀螺仪零偏εxεyε5、脚部IMU加速度计零偏
Figure QLYQS_29
构成10维状态量/>
Figure QLYQS_30
Figure QLYQS_31
以n系下的重力加速度为测量值,利用式(10),构建状态空间表达式和量测方程:
Figure QLYQS_32
Figure QLYQS_33
其中,
Figure QLYQS_34
为陀螺仪三轴输出,Γk-1为系统噪声传递阵,Wk-1为系统噪声,由陀螺仪噪声引起,/>
Figure QLYQS_35
为加速度计三轴输出,/>
Figure QLYQS_36
为加速度计三轴零偏,gn=[00 -g]T,/>
Figure QLYQS_37
为由载体坐标系(b系)到导航坐标系(n系)的旋转矩阵(DCM),其可由四元数计算得到,如式(13)所示:
Figure QLYQS_38
量测更新需要在零速率状态时进行,以避免比力中会包含运动加速度干扰;
由式(14)将加速度投影至导航坐标系,并在每一步的摆动期内进行积分:
Figure QLYQS_39
其中,k指第k个单步,积分区间为第k个单步内的零速率区间之外的采样时刻,步伐的划分也可以通过零速修正得到;
步骤三:
基于多节点加权融合的精确航向角获取;
分别记脚部、小腿和腰部三个部位的航向角为Ψf88t和Ψ9hank、Ψwa=9t,然后由式(15),对三部位航向角数据进行加权融合,其中βfoot、β9hank和βwa=9t为融合权重:
Ψfu9e,k=βfootΨfoot,k9hankΨ9hank,kwai9tΨwaist,k (15)
其中,Ψfu9e,k为融合后的航向角,系数βf8ot、βshank和βwa=st的解算方法如下:
基于行人航迹推算PDR框架有式(16)
Figure QLYQS_40
其中,posk+1为第k步的终点坐标,第k步的终点坐标为第k+1步的起点坐标;
假设行人到达位置M1,则可得到以位置M0为起点进行迭代计算的坐标表达式,如式(17)所示:
Figure QLYQS_41
其中N为地标M0到地标M1的之间的步数;
利用牛顿迭代法求解三个参数βf88t、βshank和βwa=st
Figure QLYQS_42
其中
Figure QLYQS_43
为以位置M0为起点进行PDR推算时到达位置Mj时的估计坐标,/>
Figure QLYQS_44
为位置Mj的坐标值,β=[βf88tshankwaist]T,为F(β)=0的解,求解得到的β值,是使得PDR推算的位置估计值与真实值最接近的航向角融合系数,经此融合系数融合的航向角也是行进中经各个位置验证的最优航向角。
2.根据权利要求1所述的一种基于体域惯性传感网多特征融合的高精度室内定位方法,其特征在于:所述步骤一中的传感器网络节点分别在脚部、小腿部、腰部可以全面反应步态信息、运动状态和重心变化。
3.根据权利要求1所述的一种基于体域惯性传感网多特征融合的高精度室内定位方法,其特征在于:步骤二1)中球铰链模型两端存在加速度约束,即踝关节和关节两侧的骨骼上的加速度存在耦合。
4.根据权利要求1所述的一种基于体域惯性传感网多特征融合的高精度室内定位方法,其特征在于:步骤三1)、2)中的人体处于同一时间的MS位时,传感器网络三个部位几乎处于同一垂直线且与前进方向重合度最高,且最适合用于获取多部位加权融合的航向角。
5.根据权利要求1所述的一种基于体域惯性传感网多特征融合的高精度室内定位方法,其特征在于:步骤三2)中当经过至少三个地标点时,即可求解得到权重系数βf88t、βshank和βwaist,并由此获得融合后的精确航向角Ψfuse,k;且轨迹每经过一个(或多个)新的地标点,对历史的权重系数βfoot、βshank和βwaist值进行更新,保证当前最佳的轨迹推算精度。
CN202111243967.9A 2021-10-25 2021-10-25 一种基于体域惯性传感器网多特征融合的高精度室内定位方法 Active CN113984046B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111243967.9A CN113984046B (zh) 2021-10-25 2021-10-25 一种基于体域惯性传感器网多特征融合的高精度室内定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111243967.9A CN113984046B (zh) 2021-10-25 2021-10-25 一种基于体域惯性传感器网多特征融合的高精度室内定位方法

Publications (2)

Publication Number Publication Date
CN113984046A CN113984046A (zh) 2022-01-28
CN113984046B true CN113984046B (zh) 2023-05-30

Family

ID=79741224

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111243967.9A Active CN113984046B (zh) 2021-10-25 2021-10-25 一种基于体域惯性传感器网多特征融合的高精度室内定位方法

Country Status (1)

Country Link
CN (1) CN113984046B (zh)

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015146047A1 (ja) * 2014-03-25 2015-10-01 セイコーエプソン株式会社 参照値生成方法、運動解析方法、参照値生成装置及びプログラム
CN107255474A (zh) * 2017-05-11 2017-10-17 杭州电子科技大学 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN108106613A (zh) * 2017-11-06 2018-06-01 上海交通大学 基于视觉辅助的定位方法与系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11733023B2 (en) * 2018-03-20 2023-08-22 Muvr Labs, Inc. System and method for angle calculations for a plurality of inertial measurement units

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2015146047A1 (ja) * 2014-03-25 2015-10-01 セイコーエプソン株式会社 参照値生成方法、運動解析方法、参照値生成装置及びプログラム
CN107255474A (zh) * 2017-05-11 2017-10-17 杭州电子科技大学 一种融合电子罗盘和陀螺仪的pdr航向角确定方法
CN108106613A (zh) * 2017-11-06 2018-06-01 上海交通大学 基于视觉辅助的定位方法与系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
多传感器行人航位推算方法和UKF融合算法;漆钰晖;郭杭;邓林坤;;测绘通报(第03期);全文 *

Also Published As

Publication number Publication date
CN113984046A (zh) 2022-01-28

Similar Documents

Publication Publication Date Title
Bai et al. A high-precision and low-cost IMU-based indoor pedestrian positioning technique
US10352959B2 (en) Method and system for estimating a path of a mobile element or body
EP3819592B1 (en) Positioning device, positioning method, and program
Shi et al. A robust pedestrian dead reckoning system using low-cost magnetic and inertial sensors
JP7023234B2 (ja) 歩行者の動きを推定する方法
CN109827577A (zh) 基于运动状态检测的高精度惯性导航定位算法
CN102445200A (zh) 微小型个人组合导航系统及其导航定位方法
EP2386828B1 (en) Method and system for detection of a zero velocity state of an object
CN108957510B (zh) 基于惯性/零速/gps的行人无缝组合导航定位方法
Woyano et al. Evaluation and comparison of performance analysis of indoor inertial navigation system based on foot mounted IMU
KR101642286B1 (ko) 보행특성을 이용한 실내에서의 보행자 이동방향 추정 방법
CN105806343A (zh) 基于惯性传感器的室内3d定位系统及方法
Wu et al. Indoor positioning system based on inertial MEMS sensors: Design and realization
CN112066980B (zh) 一种基于人体四节点运动约束的行人导航定位方法
CN111024126A (zh) 一种行人导航定位中的自适应零速修正方法
Zhao et al. Adaptive 3D position estimation of pedestrians by wearing one ankle sensor
CN110260860A (zh) 基于足部惯性传感器的室内移动测量定位定姿方法及系统
Xia et al. Autonomous pedestrian altitude estimation inside a multi-story building assisted by motion recognition
CN113984046B (zh) 一种基于体域惯性传感器网多特征融合的高精度室内定位方法
Li et al. A pedestrian 3-D position estimation method based on equality constraint and moving platforms correction model
Asano et al. A robust pedestrian dead-reckoning positioning based on pedestrian behavior and sensor validity
Liang et al. Accurate estimation of gait altitude using one wearable IMU sensor
Gui et al. Heading constraint algorithm for foot-mounted PNS using low-cost IMU
CN114562995A (zh) 一种基于Mems惯性传感器的行人轨迹推算方法
Ray Pedestrian navigation using particle filtering and a priori building maps

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant