CN110686671A - 基于多传感器信息融合的室内3d实时定位方法及装置 - Google Patents

基于多传感器信息融合的室内3d实时定位方法及装置 Download PDF

Info

Publication number
CN110686671A
CN110686671A CN201910934581.9A CN201910934581A CN110686671A CN 110686671 A CN110686671 A CN 110686671A CN 201910934581 A CN201910934581 A CN 201910934581A CN 110686671 A CN110686671 A CN 110686671A
Authority
CN
China
Prior art keywords
barometer
air pressure
height
information
uwb
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.)
Granted
Application number
CN201910934581.9A
Other languages
English (en)
Other versions
CN110686671B (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.)
Tongji University
Original Assignee
Tongji 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 Tongji University filed Critical Tongji University
Priority to CN201910934581.9A priority Critical patent/CN110686671B/zh
Publication of CN110686671A publication Critical patent/CN110686671A/zh
Application granted granted Critical
Publication of CN110686671B publication Critical patent/CN110686671B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; 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/16—Navigation; 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/165—Navigation; 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 combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C5/00—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels
    • G01C5/06—Measuring height; Measuring distances transverse to line of sight; Levelling between separated points; Surveyors' levels by using barometric means
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0294—Trajectory determination or predictive filtering, e.g. target tracking or Kalman filtering

Abstract

本发明涉及一种基于多传感器信息融合的室内3D实时定位方法及装置，其中方法包括：步骤S1：对UWB的测距信息进行滤波，并解算得到UWB水平位置信息；步骤S2：使用光纤陀螺与加速度计进行姿态解算，分别得到载体在竖直方向解算结果和水平解算结果；步骤S3：基于水平解算结果和UWB水平位置信息得到水平位置信息；步骤S4：基于竖直方向解算结果和气压计阵列数据得到高度位置信息。与现有技术相比，本发明具有可以解决单一定位系统受环境影响较大且鲁棒性不足的缺点等优点。

Description

基于多传感器信息融合的室内3D实时定位方法及装置
技术领域
本发明涉及一种室内定位技术，尤其是涉及一种基于多传感器信息融合的室内3D实时定位方法及装置。
背景技术
人平均80％的时间都在室内，大部分的生产生活活动都在室内进行，故室内定位有着十分广泛的运用前景。因GNSS卫星信号不能穿透建筑物，所以在室内环境中GNSS卫星信号较弱，无法在室内使用卫星定位。在日常生活中，室内定位能够帮助人们找到车辆在地下停车场的位置、在大型超市或购物广场中知道希望购买物品和自己的位置；在机场、火车站等人流量较大的地方，室内定位可以精确引导用户办理手续、指示卫生间位置，甚至可以防止儿童走失；当代制造企业其产品多呈现出小批量多品种的特征，为此在工业4.0环境下的生产单元需要实现物流的智能化以支持弹性管控。物流对象的位置信息在物流优化中是个关键，基于3D实时位置信息的智能工厂物流优化技术和碰撞规避技术是实现高效物流的一种新型解决方法。目前在实际工厂环境中，AGV(自动导引运输车)及其他移动平台的有序控制一直是学术界及工业界的一个挑战和难题。智能移动平台必须拥有实时自身空间定位，环境感知理解及动态优化控制等高级智能化功能，因此室内定位方案的研究具有重要意义。
2010年至今，随着低功耗、短距通信技术的发展，室内定位技术的发展明显加快。当前主流的室内定位技术有红外线定位技术、超声波定位技术、蓝牙定位技术、Wi-Fi定位技术、射频识别定位技术(RFID)和超宽带定位技术(UWB)。
红外线定位技术较为成熟，技术由待定物体上的标识接收发射端的红外线信号，根据接收到的角度或者时间来计算待定点和发射端的距离。但是红外线容易受环境中光、烟雾、悬浮固体和液体颗粒物的影响，定位结果产生较大误差，同时软硬件成本也不低。
超声波室内定位技术以三边交会为计算基础，通过计算超声波从已知点到待定点之间的时间来计算二者之间的距离。但是超声波在大气中衰减系数较大且容易受到障碍物遮挡、反射、折射等影响，故只能在小范围内使用。
由于办公或者日常需要，大部分家庭都布设有Wi-Fi路由器，而且许多公共场所，如候车室、大型商场、图书馆、医院等场所也布设了Wi-Fi，使得应用Wi-Fi定位的成本大大降低，而Wi-Fi定位目前的主要难点在于Wi-Fi信号不稳定，易受环境干扰，定位精度低，在3-5m。
上述定位方法的定位精度受环境影响较大且容易丢失位置，在简单无遮挡环境下定位精度可以达到较好水平，但对于一般复杂环境鲁棒性不足。生产生活中室内环境往往远比实验室的理想环境复杂，工厂中的AGV、机器人及其他运动平台必须拥有实时自身空间定位，并且对定位精度、稳定性要求较高。
发明内容
本发明的目的就是为了克服上述现有技术存在的缺陷而提供一种可以解决单一定位系统受环境影响较大且鲁棒性不足的缺点的基于多传感器信息融合的室内3D实时定位方法及装置。
本发明的目的可以通过以下技术方案来实现：
一种基于多传感器信息融合的室内3D实时定位方法，包括：
步骤S1：对UWB的测距信息进行滤波，并得到UWB水平位置信息；
步骤S2：使用光纤陀螺与加速度计进行姿态解算，分别得到载体在竖直方向解算结果和水平解算结果；
步骤S3：基于水平解算结果和UWB水平位置信息得到水平位置信息；
步骤S4：基于竖直方向解算结果和气压计阵列数据得到高度位置信息。
所述步骤S1具体包括：
步骤S11：使用卡尔曼滤波器对UWB的测距信息进行滤波得到滤波后的距离信息；
步骤S12：基于滤波后的距离信息解算得到UWB测量位置信息；
步骤S13：将UWB测量位置信息输入位置回归模型得到标定后的UWB测量位置信息。
所述卡尔曼滤波器为反遗忘量测噪声自适应卡尔曼滤波器，所述步骤S11具体包括：
步骤S111：基于UWB的测距信息判断当前时刻的距离与上一时刻滤波后的距离的差值是否大于设定的差值阈值，若为是，则执行步骤S112，反之，则执行步骤S113；
步骤S112：将当前时刻的测距信息通过数字一阶滤波器后再通过卡尔曼滤波器得到滤波后的距离信息；
步骤S113：将当前时刻的测距信息通过卡尔曼滤波器得到滤波后的距离信息。
所述步骤S3具体为：使用水平位置导航算法融合标定且滤波后的UWB测量位置信息与UWB坐标下运动载体惯性导航信息，最终融合得到水平位置信息。
所述步骤S4具体包括；
步骤S41：记录在第一固定高度的气压计阵列的各气压计在初始时刻测得的气压，以及在第二固定高度的气压计阵列的各气压计在初始时刻测得的气压；
步骤S42：记录运动载体搭载的气压计在初始时刻的气压值和所在的初始高度；
步骤S43：根据当前时刻的水平位置信息，在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计；
步骤S44：基于该8个气压计测得气压值得到气压值之和，并获取初始时刻的气压值以得到初始时刻的气压值之和；
步骤S45：基于步骤S44的结果得到环境气压变化量：
Figure BDA0002221183660000031
其中：εe(t)为环境气压变化量，P(t)为当前时刻的气压值之和，P(t0)为初始时刻的气压值之和；
步骤S46：基于8个气压计测得气压值，根据各气压计所在气压计阵列的高度以及位置，得到四个气压差值，并进一步得到比例系数：
Figure BDA0002221183660000032
其中：kt为比例系数，ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，hA为第一固定高度，hB为第二固定高度；
步骤S47：根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度，以及环境气压变化量得到高度位置信息：
Figure BDA0002221183660000041
其中：htag(t)为t时刻的高度位置信息，Ptag(t)为运动载体t时刻气压值，Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值，htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。
一种基于多传感器信息融合的室内3D实时定位装置，包括存储器、处理器，以及存储于存储器中并由所述处理器执行的程序，所述处理器执行所述程序时实现以下步骤：
步骤S1：对UWB的测距信息进行滤波，并解算得到UWB水平位置信息；
步骤S2：使用光纤陀螺与加速度计进行姿态解算，分别得到载体在竖直方向解算结果和水平解算结果；
步骤S3：基于水平解算结果和UWB水平位置信息得到水平位置信息；
步骤S4：基于竖直方向解算结果和气压计阵列数据得到高度位置信息。
与现有技术相比，本发明具有以下有益效果：
1)基于UWB、SINS和气压计等多传感器信息融合，可以解决单一定位系统受环境影响较大且鲁棒性不足的缺点。
2)使用光纤陀螺组成惯性导航系统与UWB系统融合的方案，可以解决传统UWB和MEMS融合定位方案在室内容易受到磁干扰影响、无法准确定位的问题，本方案具有较高的精度且受外界环境干扰较小，适用于在较为复杂的环境下的高精度定位。
3)提出了使用GBDT模型和BP神经网络对UWB定位系统进行校准的方案，能够适用于不同的复杂环境。
4)提出了针对UWB测距NLOS问题的滤波方案，及UWB系统与惯性导航系统信息融合改进后的量测噪声自适应卡尔曼滤波器，使定位精度进一步提升并且提高了系统鲁棒性。
5)提出了基于气压计阵列的气压计和加速度计组合定高方案，对环境的影响有良好的适应能力，提高了高度测量的动态特性。
附图说明
图1为姿态角示意图；
图2为SINS姿态解算算法原理图；
图3为光纤陀螺姿态解算算法原理图；
图4为MEMS姿态解算原理图；
图5为GBDT模型训练流程图；
图6为BP神经网络结构示意图；
图7为UWB测距滤波原理图；
图8为UWB安装结构图；
图9为水平载体坐标系与UWB坐标系关系示意图；
图10为水平组合导航系统原理图；
图11为气压计阵列示意图；
图12为多传感器信息融合定高原理图；
图13为本发明方法的主要步骤流程示意图；
图14为方案总体框图。
具体实施方式
下面结合附图和具体实施例对本发明进行详细说明。本实施例以本发明技术方案为前提进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于下述的实施例。
超宽带定位技术(UWB)是一种全新的技术，超宽带是指所用频率是工作频率的125％或者带宽超过1.5GHz的信号。它利用事先布置好的已知位置的锚节点和桥节点，与新加入的盲节点进行通信，并利用三角定位或者“指纹”方式来确定位置。相对于其他信号，有着无法比拟的优势:穿透力强、传输距离长、信号稳定性好、高传输速度等，故而得到了广泛的应用。在室内定位中一般通过三角定位或者信号强度进行匹配定位。超宽带在室内定位中的前景是非常广阔的，具有良好的多径分辨能力，可以提供更高的定位精度，但硬件成本较大，此外超宽带信号只能视距传播，遇到障碍物或者墙壁时无法进行定位。若能在设备及技术上有较大突破，使得设备成本和功耗降低，则超宽带有望成为推广应用的室内定位技术。
捷联惯性导航系统(strapdown inertial navigation system,SINS)是一种自主式导航定位系统，并且有较高的数据更新率，其主要缺点是随着时间的积累，本身的误差将会变大，即只能够在短时间内保持高精度。超宽带测距信号在受到完全遮挡或非视距传播(non-line of sight,NLOS)时定位精度受到影响，若UWB使用环境较为复杂则可能无法满足超高精度的稳定性需求。针对以上两个问题，本专利采用将惯性导航技术和超宽带室内定位技术相结合的组合定位的方法，降低了惯性导航系统(SINS)和超宽带系统的自有误差，提高系统的定位精度和鲁棒性。
本申请方法所使用器件如下：
光纤陀螺是以光导纤维线圈为基础的敏感元件，由激光二极管发射出的光线朝两个方向沿光导纤维传播，基本原理都是利用Sagnac效应。光纤陀螺具有自主纠偏、灵敏度高、寿命长、耐低温耐高温、不受电磁干扰等诸多优点，是迄今综合性能最好的惯性传感器。针对存在异常磁场或磁罗盘容易受到干扰的场景，如在工厂、矿场对实时定位精度要求较高的AGV，移动机器人等运动物体，方法拟采用三轴闭环光纤陀螺与三轴加速度计的组合成为惯性导航系统(SINS)。
微机电系统(MEMS,Micro-Electro-Mechanical System)，也叫做微电子机械系统、微系统、微机械等，是集微传感器、微执行器、微机械机构、信号处理和控制电路、高性能电子集成器件、接口、通信和电源等于一体的微型器件或系统。具有小体积、低成本、集成化等特点。在磁罗盘不容易受到干扰且定位精度要求一般的场景，如大型商场、机场和火车站等地点方法采用低成本MEMS组成惯性导航系统(SINS)。
气压计数据连续且分辨率高，但是容易受大气压力影响。加速度计可测量高度方向加速度，通过积分得到高度方向速度和位移，但是随着时间的增长，高度变化量逐渐累积。
一种基于多传感器信息融合的室内3D实时定位方法，该方法通过计算机系统实现，如图13和图14所示，包括：
步骤S1：对UWB的测距信息进行滤波，并解算得到UWB水平位置信息，具体包括：
步骤S11：使用卡尔曼滤波器对UWB的测距信息进行滤波得到滤波后的距离信息，卡尔曼滤波器为反遗忘量测噪声自适应卡尔曼滤波器，步骤S11具体包括：
步骤S111：基于UWB的测距信息判断当前时刻的距离与上一时刻滤波后的距离的差值是否大于设定的差值阈值，若为是，则执行步骤S112，反之，则执行步骤S113；
步骤S112：将当前时刻的测距信息通过数字一阶滤波器后再通过卡尔曼滤波器得到滤波后的距离信息；
步骤S113：将当前时刻的测距信息通过卡尔曼滤波器得到滤波后的距离信息。
步骤S12：基于滤波后的距离信息解算得到UWB测量位置信息；
步骤S13：将UWB测量位置信息输入位置回归模型得到标定后的UWB测量位置信息。
其中，在水平位置标定方面，可以采用梯度下降树(GBDT)标定方法或者BP神经网络映射标定方法
1)对于梯度下降树(GBDT)标定方法，GBDT(Gradient Boosting Decision Tree)被广泛运用于分类、回归等问题，属于Boosting算法族，集成学习通过构建和结合多个学习器来完成任务。GBDT每一轮迭代拟合残差学习一个CART树(classification andregression tree)作为弱学习器，通过弱学习器组成的基函数的线性组合，不断减小训练中出现的残差。算法流程图见附图5。
UWB系统受到所处环境经纬度、海拔高度、室内环境、墙面、地面和附近障碍物、定位算法等外界条件和自身误差的影响，在测距和定位过程中会累计产生一定的系统定位误差。故对已知位置点实际位置和UWB定位结果利用机器学习方法进行误差校正。方法如下，设已知位置点A0(x0,y0)，A1(x1,y1),…,An(xn,yn)。UWB标签位于上述点时测得位置为
Figure BDA0002221183660000071
设UWB测得位置(xu,yu)T为GBDT模型输入向量，真实位置点x轴坐标位置与y轴坐标位置作为真值训练得到两个由UWB位置向量到x轴与y轴位置映射GBDT模型，用于对UWB测得位置进行纠偏。
2)对于BP神经网络映射标定方法，BP神经网络具有较强的非线性映射能力、较好的泛化和容错能力，比较适合当前因为不同环境影响的定位不准确纠偏问题。
设BP神经网络输入向量为二维向量X＝(xo,yo)，代表原始UWB定位位置，输出向量同为二维向量Y＝(xu,yu)，代表映射后位置。在定位水平面中对区域分布的M个点进行真实位置测量和UWB定位位置测量，放入BP神经网络利用随机梯度下降法进行训练。训练后得到BP神经网络用于对原始UWB定位位置进行纠偏和校正。神经网络结构见附图6。
步骤S2：使用光纤陀螺与加速度计进行姿态解算，分别得到载体在竖直方向解算结果和水平解算结果；
本发明专利使用带三轴加速度计的三轴闭环光纤陀螺(FOG)来进行姿态测量和解算。如附图1所示，物体航向角、俯仰角和横滚角分别用ψ，Θ，Φ表示。设置载体坐标系(b坐标系)x轴指向物体的正前方，y轴指向物体正左方，z轴指向物体正上方。地理坐标系(n坐标系)x轴指向地理的正北方，y轴指向地理正西方，z轴指向地理竖直向上方向。
从姿态角构建方向余弦矩阵
Figure BDA0002221183660000081
如下式所示：
Figure BDA0002221183660000082
由方向余弦矩阵得到姿态角如下式：
考虑到欧拉角表示载体姿态在大角度时可能出现死锁或无解问题，故使用四元数进行实时姿态更新解算。四元数转化为方向余弦矩阵公式如下：
Figure BDA0002221183660000084
结合四元数微分方程更新载体坐标系(b坐标系)，四元数微分方程如下：
Figure BDA0002221183660000085
设在载体坐标系(b坐标系)下捷联三轴光纤陀螺的角速度为：
Figure BDA0002221183660000086
光纤陀螺(FOG)原始角量输出为
Figure BDA0002221183660000087
由姿态更新的最新值确定的地理坐标系(n坐标系)到载体坐标系(b坐标系)的转移矩阵为地球自转角速率为
Figure BDA0002221183660000089
位置速率为
Figure BDA00022211836600000810
Figure BDA00022211836600000811
可由下式计算获得：
Figure BDA00022211836600000812
设RM、RN分别为当地沿子午圈和卯酉圈的曲率半径，VN，VE为载体运动北向、东向速度，L为地理纬度。
当坐标系取得地理坐标系(n坐标系)时，有：
Figure BDA0002221183660000091
采用四元数微分方程的单子样定时采样角增量的皮卡求解法计算。单子样一阶求解近似算法为：
其中，I为单位阵；
Figure BDA0002221183660000093
通过对四元数的实时更新，从而获取姿态角方向余弦矩阵，最终获取实时姿态角。
姿态解算流程见附图2：
光纤陀螺与加速度计实时姿态解算算法流程图见附图3：
在受地磁干扰较小的场景中，低成本MEMS组成的惯性导航系统(SINS)可以取代光纤陀螺作为惯性导航单元，姿态解算流程类似于光纤陀螺和加速度计组成的惯性导航系统(SINS)，使用磁力计对载体航向角进行整定，具体算法流程图见附图4。
步骤S3：基于水平解算结果和UWB水平位置信息得到水平位置信息，具体为：使用水平位置导航算法融合标定且滤波后的UWB测量位置信息与UWB坐标下运动载体惯性导航信息，最终融合得到水平位置信息。
具体的，水平融合算法主要包括以下内容：
1)测距信息卡尔曼滤波水平位置融合算法和策略
在UWB测距后，使用卡尔曼滤波的方法对原始测距信息进行平滑处理减少UWB的非视距(non line of sight,NLOS)误差的影响。设计的滤波器结构见附图7。
设定测量距离差值阈值D，若当前时刻测量距离与上一时刻滤波器输出距离差值大于阈值D，则将距离信息通过数字一阶滤波器后再通过卡尔曼滤波器，以增强系统稳定性；若差值小于阈值D，则直接通过卡尔曼滤波器并输出进行位置解算。
2)运动坐标系建立
室内定位系统基站和标签布置方式如附图8(基站数量不限只为四个，标签数量不限只为一个)：
根据前面建立的载体坐标系和机体坐标系，为了便于定位，特建立基于UWB的平面运动坐标系(u坐标系)如附图。将SINS加速度由b系转换到n系方程为(其中ab为加速度计测得的原始加速度)：
Figure BDA0002221183660000101
由于只考虑水平运动，故将地理坐标系的水平加速度转换到UWB坐标系下：
Figure BDA0002221183660000102
坐标转换关系见附图9。
3)UWB/INS组合算法
惯性导航模型为：
Figure BDA0002221183660000103
上式：ΔT为SINS的采用时间；
Figure BDA0002221183660000104
分别为k时刻载体在u系下的位置；
Figure BDA0002221183660000105
Figure BDA0002221183660000106
为第k时刻载体在u系下的速度；
Figure BDA0002221183660000107
为第k时刻载体在u系下的水平加速度。
本发明使用松组合的方法，根据UWB系统解算的物体位置、速度信息辅助修正SINS导航系统。
以载体的位置和速度作为组合导航系统的状态向量，得组合导航状态方程：
Xk+1＝AXk+BUk+ωk
式中：
ωk为惯性导航系统过程激励噪声，p(ω)～N(0,Q)，Q为过程激励噪声协方差矩阵。
以UWB解算的位置和速度信息作为量测信息、以SINS解算的位置和速度作为状态变量估计值。UWB/SINS组合的量测方程如下式：
Zk＝HXk+vk
其中
Figure BDA0002221183660000111
vk为量测噪声，即为UWB位置和速度解算噪声矩阵，p(v)～N(0,R)，R为过程激励噪声协方差矩阵。
卡尔曼滤波虽然能消除干扰、滤除异常信号、组合惯性导航系统和UWB系统；但是标准卡尔曼滤波依赖先验量测噪声及系统噪声，当UWB系统处于较为复杂的环境中，量测噪声和系统噪声是不断变化的，故传统卡尔曼滤波并不适用。本发明使用改进型自适应卡尔曼滤波适应量测噪声。当UWB解算位置因受NLOS影响大幅度波动时，设置阈值dmax，当UWB解算位置与前一时刻组合导航位置距离Δd大于阈值dmax时：
Figure BDA0002221183660000112
引入反遗忘因子s，减小了状态预测的不确定性，增强了以往的估计，淡化了当前UWB位置测量值对系统的影响，从而提高了解算精度。
反遗忘量测噪声自适应卡尔曼滤波器步骤如下：
(1)离散滤波器时间更新方程
Figure BDA0002221183660000113
其中
Figure BDA0002221183660000114
为滤波器先验姿态估计，
Figure BDA0002221183660000115
为下一个时刻滤波器后验估计。
Figure BDA0002221183660000116
其中，ξk为UWB量测信号与先验估计偏差。
Figure BDA0002221183660000117
其中，
Figure BDA0002221183660000118
为先验估计误差协方差，s≤1，为反遗忘因子，当UWB量测信号突变时用于减弱量测值得权重，利用捷联惯性导航短期精度较高的特点，增大惯性导航姿态预测
Figure BDA0002221183660000119
的权重，防止“遗忘”。
(2)离散滤波器状态更新方程
Figure BDA00022211836600001110
其中，为测量噪声估计值，
Figure BDA00022211836600001112
其中b称为遗忘因子，用于限制自适应滤波器的记忆长度，通常情况下取0.96≤b≤0.99。
Figure BDA00022211836600001113
其中，Kk为卡尔曼增益
其中，
Figure BDA0002221183660000122
为当前时刻滤波器后验估计。
Figure BDA0002221183660000123
其中，Pk为估计得到的状态后验协方差
室内水平定位流程图见附图10。
步骤S4：基于竖直方向解算结果和气压计阵列数据得到高度位置信息，
大量数据表明，气压计数据分辨率较高，市面上较为廉价的气压计都能识别短时间5cm的高度变化，但受大气压影响，测量误差随着高度降低而增大，同时，同一高度气压随着温度、天气不断变化也引入了不小的高度漂移误差；加速度计可以得到高度方向的运动信息，在短时间内具有较高的精度，但是随着时间的增长，误差逐渐积累，最后高度测量信息发散。由此，单一传感器很难满足较高精度的高度测量需求。
当高度在11km以下时，气压高度公式为：
Figure BDA0002221183660000124
上式中：Tb是高度为Hb时的温度(K)；β是温度垂直变化率；Ph和Pb是高度为H和Hb时对应的气压(Pa)；R是空气专用气体常数g是重力加速度常数(m/s2)。
室内定位的高度测量属于小范围内对高度的测量，设空气密度为ρ、重力加速度为g(忽略短时间内重力加速度和空气密度在室内不同高度的变化)。设高度h1处气压为P1，高度h2处气压为P2。在静力平衡条件下，P1和P2气压差等于等于这两个高度面面积为1cm2大气柱重量，即：
P1-P2＝-ρgΔh
故发明为简便计算，假设测量高度差与气压差成线性关系。
单个气压计静止不动时气压测量模型模型如下：
P(t)＝P(to)+εe(t)+εi
运动载体气压计气压测量模型如下：
P(t)＝P(to)+εe(t)+εi-ρgΔh
上式中P(t0)为在t0时刻气压计测量所得气压值，εe(t)为随着时间因为环境变化(温度、天气等)而产生的气压变化，εi(t)为气压计本身特性所产生误差。
为消除环境气压变化εe(t)和室内空气对流对气压测量的影响，假设处在室内空间中各个点εe(t)相近，如附图11所示，在固定高度hA与hB设立气压计阵列，当标签位于气压计阵列中，则使用距标签最近的不同高度各四个气压计解算他们之间的气压差(为保证一致性，发明采用同一品牌同一型号气压计)。
具体包括：
步骤S41：系统开机t0时刻，记录在固定高度hA的气压计阵列的n个气压计在初始时刻测得气压
Figure BDA0002221183660000131
记录在固定高度hB气压计阵列n个气压计在初始时刻测得气压
Figure BDA0002221183660000132
步骤S42：记录运动载体搭载的气压计在初始时刻的气压值Ptag(t0)和所在的初始高度htag(t0)；
步骤S43：根据当前时刻的水平位置信息，在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计，并测得气压值如下：
Figure BDA0002221183660000133
步骤S44：求得气压值之和为P(t)，基于该8个气压计测得气压值得到气压值之和，并获取初始时刻的气压值以得到初始时刻的气压值之和；
步骤S45：基于步骤S44的结果得到环境气压变化量：
Figure BDA0002221183660000134
其中：εe(t)为环境气压变化量，P(t)为当前时刻的气压值之和，P(t0)为初始时刻的气压值之和；
步骤S46：基于8个气压计测得气压值，根据各气压计所在气压计阵列的高度以及位置，得到四个气压差值，并进一步得到比例系数：
其中：kt为比例系数，ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，hA为第一固定高度，hB为第二固定高度；
步骤S47：根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度，以及环境气压变化量得到高度位置信息：
Figure BDA0002221183660000141
其中：htag(t)为t时刻的高度位置信息，Ptag(t)为运动载体t时刻气压值，Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值，htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。

Claims (10)

1.一种基于多传感器信息融合的室内3D实时定位方法，其特征在于，包括：
步骤S1：对UWB的测距信息进行滤波，并解算得到UWB水平位置信息；
步骤S2：使用光纤陀螺与加速度计进行姿态解算，分别得到载体在竖直方向解算结果和水平解算结果；
步骤S3：基于水平解算结果和UWB水平位置信息得到水平位置信息；
步骤S4：基于竖直方向解算结果和气压计阵列数据得到高度位置信息。
2.根据权利要求1所述的一种基于多传感器信息融合的室内3D实时定位方法，其特征在于，所述步骤S1具体包括：
步骤S11：使用卡尔曼滤波器对UWB的测距信息进行滤波得到滤波后的距离信息；
步骤S12：基于滤波后的距离信息解算得到UWB测量位置信息；
步骤S13：将UWB测量位置信息输入位置回归模型得到标定后的UWB测量位置信息。
3.根据权利要求2所述的一种基于多传感器信息融合的室内3D实时定位方法，其特征在于，所述卡尔曼滤波器为反遗忘量测噪声自适应卡尔曼滤波器，所述步骤S11具体包括：
步骤S111：基于UWB的测距信息判断当前时刻的距离与上一时刻滤波后的距离的差值是否大于设定的差值阈值，若为是，则执行步骤S112，反之，则执行步骤S113；
步骤S112：将当前时刻的测距信息通过数字一阶滤波器后再通过卡尔曼滤波器得到滤波后的距离信息；
步骤S113：将当前时刻的测距信息通过卡尔曼滤波器得到滤波后的距离信息。
4.根据权利要求2所述的一种基于多传感器信息融合的室内3D实时定位方法，其特征在于，所述步骤S3具体为：使用水平位置导航算法融合标定且滤波后的UWB测量位置信息与UWB坐标下运动载体惯性导航信息，最终融合得到水平位置信息。
5.根据权利要求1所述的一种基于多传感器信息融合的室内3D实时定位方法，其特征在于，所述步骤S4具体包括：
步骤S41：记录在第一固定高度的气压计阵列的各气压计在初始时刻测得的气压，以及在第二固定高度的气压计阵列的各气压计在初始时刻测得的气压；
步骤S42：记录运动载体搭载的气压计在初始时刻的气压值和所在的初始高度；
步骤S43：根据当前时刻的水平位置信息，在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计；
步骤S44：基于该8个气压计测得气压值得到气压值之和，并获取初始时刻的气压值以得到初始时刻的气压值之和；
步骤S45：基于步骤S44的结果得到环境气压变化量：
Figure FDA0002221183650000021
其中：εe(t)为环境气压变化量，P(t)为当前时刻的气压值之和，P(t0)为初始时刻的气压值之和；
步骤S46：基于8个气压计测得气压值，根据各气压计所在气压计阵列的高度以及位置，得到四个气压差值，并进一步得到比例系数：
其中：kt为比例系数，ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，hA为第一固定高度，hB为第二固定高度；
步骤S47：根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度，以及环境气压变化量得到高度位置信息：
Figure FDA0002221183650000023
其中：htag(t)为t时刻的高度位置信息，Ptag(t)为运动载体t时刻气压值，Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值，htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。
6.一种基于多传感器信息融合的室内3D实时定位装置，其特征在于，包括存储器、处理器，以及存储于存储器中并由所述处理器执行的程序，所述处理器执行所述程序时实现以下步骤：
步骤S1：对UWB的测距信息进行滤波，并解算得到UWB水平位置信息；
步骤S2：使用光纤陀螺与加速度计进行姿态解算，分别得到载体在竖直方向解算结果和水平解算结果；
步骤S3：基于水平解算结果和UWB水平位置信息得到水平位置信息；
步骤S4：基于竖直方向解算结果和气压计阵列数据得到高度位置信息。
7.根据权利要求6所述的一种基于多传感器信息融合的室内3D实时定位装置，其特征在于，所述步骤S1具体包括：
步骤S11：使用卡尔曼滤波器对UWB的测距信息进行滤波得到滤波后的距离信息；
步骤S12：基于滤波后的距离信息解算得到UWB测量位置信息；
步骤S13：将UWB测量位置信息输入位置回归模型得到标定后的UWB测量位置信息。
8.根据权利要求7所述的一种基于多传感器信息融合的室内3D实时定位装置，其特征在于，所述卡尔曼滤波器为反遗忘量测噪声自适应卡尔曼滤波器，所述步骤S11具体包括：
步骤S111：基于UWB的测距信息判断当前时刻的距离与上一时刻滤波后的距离的差值是否大于设定的差值阈值，若为是，则执行步骤S112，反之，则执行步骤S113；
步骤S112：将当前时刻的测距信息通过数字一阶滤波器后再通过卡尔曼滤波器得到滤波后的距离信息；
步骤S113：将当前时刻的测距信息通过卡尔曼滤波器得到滤波后的距离信息。
9.根据权利要求7所述的一种基于多传感器信息融合的室内3D实时定位装置，其特征在于，所述步骤S3具体为：使用水平位置导航算法融合标定且滤波后的UWB测量位置信息与UWB坐标下运动载体惯性导航信息，最终融合得到水平位置信息。
10.根据权利要求6所述的一种基于多传感器信息融合的室内3D实时定位装置，其特征在于，所述步骤S4具体包括：
步骤S41：记录在第一固定高度的气压计阵列的各气压计在初始时刻测得的气压，以及在第二固定高度的气压计阵列的各气压计在初始时刻测得的气压；
步骤S42：记录运动载体搭载的气压计在初始时刻的气压值和所在的初始高度；
步骤S43：根据当前时刻的水平位置信息，在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计；
步骤S44：基于该8个气压计测得气压值得到气压值之和，并获取初始时刻的气压值：
Figure FDA0002221183650000041
以得到初始时刻的气压值之和P(t0)；
步骤S45：基于步骤S44的结果得到环境气压变化量：
Figure FDA0002221183650000042
其中：εe(t)为环境气压变化量，P(t)为当前时刻的气压值之和，P(t0)为初始时刻的气压值之和；
步骤S46：基于8个气压计测得气压值，根据各气压计所在气压计阵列的高度以及位置，得到四个气压差值，并进一步得到比例系数：
Figure FDA0002221183650000043
其中：kt为比例系数，ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值，hA为第一固定高度，hB为第二固定高度；
步骤S47：根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度，以及环境气压变化量得到高度位置信息：
Figure FDA0002221183650000044
其中：htag(t)为t时刻的高度位置信息，Ptag(t)为运动载体t时刻气压值，Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值，htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。
为提高定高系统的容错性和鲁棒性，发明使用互补滤波的方法融合加速度计信息和气压值阵列高度测量信息。由姿态矩阵得到载体在地理坐标系下竖直加速度imu_acc_z，由气压计信息得到气压计高度Bar_hight及气压计速度bar_speed_z，最后使用二级串联互补滤波进行融合。系统结构图见附图12。
CN201910934581.9A 2019-09-29 2019-09-29 基于多传感器信息融合的室内3d实时定位方法及装置 Active CN110686671B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910934581.9A CN110686671B (zh) 2019-09-29 2019-09-29 基于多传感器信息融合的室内3d实时定位方法及装置

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910934581.9A CN110686671B (zh) 2019-09-29 2019-09-29 基于多传感器信息融合的室内3d实时定位方法及装置

Publications (2)

Publication Number Publication Date
CN110686671A true CN110686671A (zh) 2020-01-14
CN110686671B CN110686671B (zh) 2021-11-09

Family

ID=69111053

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910934581.9A Active CN110686671B (zh) 2019-09-29 2019-09-29 基于多传感器信息融合的室内3d实时定位方法及装置

Country Status (1)

Country Link
CN (1) CN110686671B (zh)

Cited By (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111550551A (zh) * 2020-04-08 2020-08-18 杭州鸿泉物联网技术股份有限公司 节油辅助驾驶方法、电子设备及存储介质
CN112197684A (zh) * 2020-08-28 2021-01-08 浙江农林大学 一种树木位置测量装置及方法
CN112747747A (zh) * 2021-01-20 2021-05-04 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN115265592A (zh) * 2022-07-18 2022-11-01 北京航空航天大学 一种光纤陀螺磁温交联耦合误差的在线补偿方法

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105509739A (zh) * 2016-02-04 2016-04-20 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN105824003A (zh) * 2014-12-16 2016-08-03 国家电网公司 一种基于轨迹平滑的室内移动目标定位方法
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
CN106908759A (zh) * 2017-01-23 2017-06-30 南京航空航天大学 一种基于uwb技术的室内行人导航方法
KR20180042546A (ko) * 2016-10-18 2018-04-26 조선대학교산학협력단 하이브리드 실내 측위 시스템 및 그 방법
CN108036784A (zh) * 2017-11-10 2018-05-15 云保（佛山）智控科技有限公司 一种室内定位方法、导航方法及系统
CN108919181A (zh) * 2018-09-05 2018-11-30 成都精位科技有限公司 基于惯导的uwb定位方法、装置及定位标签
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105824003A (zh) * 2014-12-16 2016-08-03 国家电网公司 一种基于轨迹平滑的室内移动目标定位方法
CN105509739A (zh) * 2016-02-04 2016-04-20 济南大学 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
KR20180042546A (ko) * 2016-10-18 2018-04-26 조선대학교산학협력단 하이브리드 실내 측위 시스템 및 그 방법
CN106908759A (zh) * 2017-01-23 2017-06-30 南京航空航天大学 一种基于uwb技术的室内行人导航方法
US20190037348A1 (en) * 2017-07-28 2019-01-31 Electronics And Telecommunications Research Institute Method of measuring inter-device relative coordinates and device using the same
CN108036784A (zh) * 2017-11-10 2018-05-15 云保（佛山）智控科技有限公司 一种室内定位方法、导航方法及系统
CN108919181A (zh) * 2018-09-05 2018-11-30 成都精位科技有限公司 基于惯导的uwb定位方法、装置及定位标签

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
段世红等: "传感网中UWB和IMU融合定位的性能评估", 《计算机研究与发展》 *

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111550551A (zh) * 2020-04-08 2020-08-18 杭州鸿泉物联网技术股份有限公司 节油辅助驾驶方法、电子设备及存储介质
CN111550551B (zh) * 2020-04-08 2021-07-27 杭州鸿泉物联网技术股份有限公司 节油辅助驾驶方法、电子设备及存储介质
CN112197684A (zh) * 2020-08-28 2021-01-08 浙江农林大学 一种树木位置测量装置及方法
CN112747747A (zh) * 2021-01-20 2021-05-04 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN112747747B (zh) * 2021-01-20 2022-10-11 重庆邮电大学 一种改进的uwb/imu融合室内行人定位方法
CN115265592A (zh) * 2022-07-18 2022-11-01 北京航空航天大学 一种光纤陀螺磁温交联耦合误差的在线补偿方法
CN115265592B (zh) * 2022-07-18 2024-04-09 北京航空航天大学 一种光纤陀螺磁温交联耦合误差的在线补偿方法

Also Published As

Publication number Publication date
CN110686671B (zh) 2021-11-09

Similar Documents

Publication Publication Date Title
CN110686671B (zh) 基于多传感器信息融合的室内3d实时定位方法及装置
Zhou et al. Activity sequence-based indoor pedestrian localization using smartphones
CN109682375B (zh) 一种基于容错决策树的uwb辅助惯性定位方法
US11598886B2 (en) GNSS/IMU surveying and mapping system and method
US8688375B2 (en) Method and system for locating and monitoring first responders
US8751151B2 (en) System and method for localizing a trackee at a location and mapping the location using inertial sensor information
CN110398245B (zh) 基于脚戴式惯性测量单元的室内行人导航姿态估计方法
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN109813311A (zh) 一种无人机编队协同导航方法
CN108120438B (zh) 一种基于imu和rfid信息融合的室内目标快速跟踪方法
CN113108791B (zh) 一种导航定位方法及导航定位设备
CN113124856B (zh) 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法
CN103196445B (zh) 基于匹配技术的地磁辅助惯性的载体姿态测量方法
CN107014375B (zh) 超低部署的室内定位系统及方法
CN111025366B (zh) 基于ins及gnss的网格slam的导航系统及方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN109164411A (zh) 一种基于多数据融合的人员定位方法
CN107246872B (zh) 基于mems传感器和vlc定位融合的单粒子滤波导航装置和方法
Hasan et al. Smart phone based sensor fusion by using Madgwick filter for 3D indoor navigation
CN117289322A (zh) 一种基于imu、gps与uwb的高精度定位算法
Gao et al. An integrated land vehicle navigation system based on context awareness
CN113295174B (zh) 一种车道级定位的方法、相关装置、设备以及存储介质
CN114111802A (zh) 一种行人航迹推算辅助uwb的定位方法
AU2015201877B2 (en) Method and system for locating and monitoring first responders
Elbes et al. Gyroscope drift correction based on TDoA technology in support of pedestrian dead reckoning

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