CN110686671B - 基于多传感器信息融合的室内3d实时定位方法及装置 - Google Patents
基于多传感器信息融合的室内3d实时定位方法及装置 Download PDFInfo
- Publication number
- CN110686671B CN110686671B CN201910934581.9A CN201910934581A CN110686671B CN 110686671 B CN110686671 B CN 110686671B CN 201910934581 A CN201910934581 A CN 201910934581A CN 110686671 B CN110686671 B CN 110686671B
- Authority
- CN
- China
- Prior art keywords
- barometer
- air pressure
- uwb
- height
- array
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 41
- 230000004927 fusion Effects 0.000 title claims abstract description 22
- 238000004364 calculation method Methods 0.000 claims abstract description 33
- 238000001914 filtration Methods 0.000 claims abstract description 17
- 239000003570 air Substances 0.000 claims description 75
- 238000005259 measurement Methods 0.000 claims description 51
- 238000004422 calculation algorithm Methods 0.000 claims description 15
- 230000008859 change Effects 0.000 claims description 11
- 239000012080 ambient air Substances 0.000 claims description 8
- 230000003044 adaptive effect Effects 0.000 claims description 7
- 230000007613 environmental effect Effects 0.000 claims description 6
- 238000003491 array Methods 0.000 claims description 4
- 230000007547 defect Effects 0.000 abstract description 4
- 238000005516 engineering process Methods 0.000 description 19
- 230000001133 acceleration Effects 0.000 description 9
- 238000013528 artificial neural network Methods 0.000 description 9
- 238000010586 diagram Methods 0.000 description 9
- 239000011159 matrix material Substances 0.000 description 9
- 230000008569 process Effects 0.000 description 7
- 239000000835 fiber Substances 0.000 description 6
- 238000004519 manufacturing process Methods 0.000 description 4
- 238000012549 training Methods 0.000 description 4
- 238000012937 correction Methods 0.000 description 3
- 230000005284 excitation Effects 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000005457 optimization Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000009530 blood pressure measurement Methods 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 230000000295 complement effect Effects 0.000 description 2
- 238000011161 development Methods 0.000 description 2
- 230000000694 effects Effects 0.000 description 2
- 230000006870 function Effects 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000010354 integration Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000000903 blocking effect Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 238000003066 decision tree Methods 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 238000011478 gradient descent method Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 239000007788 liquid Substances 0.000 description 1
- 238000010801 machine learning Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 239000002245 particle Substances 0.000 description 1
- 230000000149 penetrating effect Effects 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
- 230000035945 sensitivity Effects 0.000 description 1
- 230000008054 signal transmission Effects 0.000 description 1
- 239000000779 smoke Substances 0.000 description 1
- 239000007787 solid Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000012546 transfer Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
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
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种基于多传感器信息融合的室内3D实时定位方法及装置,其中方法包括:步骤S1:对UWB的测距信息进行滤波,并解算得到UWB水平位置信息;步骤S2:使用光纤陀螺与加速度计进行姿态解算,分别得到载体在竖直方向解算结果和水平解算结果;步骤S3:基于水平解算结果和UWB水平位置信息得到水平位置信息;步骤S4:基于竖直方向解算结果和气压计阵列数据得到高度位置信息。与现有技术相比,本发明具有可以解决单一定位系统受环境影响较大且鲁棒性不足的缺点等优点。
Description
技术领域
本发明涉及一种室内定位技术,尤其是涉及一种基于多传感器信息融合的室内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的结果得到环境气压变化量:
其中:εe(t)为环境气压变化量,P(t)为当前时刻的气压值之和,P(t0)为初始时刻的气压值之和;
步骤S46:基于8个气压计测得气压值,根据各气压计所在气压计阵列的高度以及位置,得到四个气压差值,并进一步得到比例系数:
其中:kt为比例系数,ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,hA为第一固定高度,hB为第二固定高度;
步骤S47:根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度,以及环境气压变化量得到高度位置信息:
其中: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标签位于上述点时测得位置为设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轴指向地理竖直向上方向。
由方向余弦矩阵得到姿态角如下式:
考虑到欧拉角表示载体姿态在大角度时可能出现死锁或无解问题,故使用四元数进行实时姿态更新解算。四元数转化为方向余弦矩阵公式如下:
结合四元数微分方程更新载体坐标系(b坐标系),四元数微分方程如下:
设在载体坐标系(b坐标系)下捷联三轴光纤陀螺的角速度为:
设RM、RN分别为当地沿子午圈和卯酉圈的曲率半径,VN,VE为载体运动北向、东向速度,L为地理纬度。
当坐标系取得地理坐标系(n坐标系)时,有:
采用四元数微分方程的单子样定时采样角增量的皮卡求解法计算。单子样一阶求解近似算法为:
其中,I为单位阵;
通过对四元数的实时更新,从而获取姿态角方向余弦矩阵,最终获取实时姿态角。
姿态解算流程见附图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为加速度计测得的原始加速度):
由于只考虑水平运动,故将地理坐标系的水平加速度转换到UWB坐标系下:
坐标转换关系见附图9。
3)UWB/INS组合算法
惯性导航模型为:
本发明使用松组合的方法,根据UWB系统解算的物体位置、速度信息辅助修正SINS导航系统。
以载体的位置和速度作为组合导航系统的状态向量,得组合导航状态方程:
Xk+1=AXk+BUk+ωk
ωk为惯性导航系统过程激励噪声,p(ω)~N(0,Q),Q为过程激励噪声协方差矩阵。
以UWB解算的位置和速度信息作为量测信息、以SINS解算的位置和速度作为状态变量估计值。UWB/SINS组合的量测方程如下式:
卡尔曼滤波虽然能消除干扰、滤除异常信号、组合惯性导航系统和UWB系统;但是标准卡尔曼滤波依赖先验量测噪声及系统噪声,当UWB系统处于较为复杂的环境中,量测噪声和系统噪声是不断变化的,故传统卡尔曼滤波并不适用。本发明使用改进型自适应卡尔曼滤波适应量测噪声。当UWB解算位置因受NLOS影响大幅度波动时,设置阈值dmax,当UWB解算位置与前一时刻组合导航位置距离Δd大于阈值dmax时:引入反遗忘因子s,减小了状态预测的不确定性,增强了以往的估计,淡化了当前UWB位置测量值对系统的影响,从而提高了解算精度。
反遗忘量测噪声自适应卡尔曼滤波器步骤如下:
(1)离散滤波器时间更新方程
其中,ξk为UWB量测信号与先验估计偏差。
(2)离散滤波器状态更新方程
其中,Kk为卡尔曼增益
其中,Pk为估计得到的状态后验协方差
室内水平定位流程图见附图10。
步骤S4:基于竖直方向解算结果和气压计阵列数据得到高度位置信息,
大量数据表明,气压计数据分辨率较高,市面上较为廉价的气压计都能识别短时间5cm的高度变化,但受大气压影响,测量误差随着高度降低而增大,同时,同一高度气压随着温度、天气不断变化也引入了不小的高度漂移误差;加速度计可以得到高度方向的运动信息,在短时间内具有较高的精度,但是随着时间的增长,误差逐渐积累,最后高度测量信息发散。由此,单一传感器很难满足较高精度的高度测量需求。
当高度在11km以下时,气压高度公式为:
室内定位的高度测量属于小范围内对高度的测量,设空气密度为ρ、重力加速度为g(忽略短时间内重力加速度和空气密度在室内不同高度的变化)。设高度h1处气压为P1,高度h2处气压为P2。在静力平衡条件下,P1和P2气压差等于等于这两个高度面面积为1cm2大气柱重量,即:
P1-P2=-ρgΔh
故发明为简便计算,假设测量高度差与气压差成线性关系。
单个气压计静止不动时气压测量模型模型如下:
P(t)=P(t0)+εe(t)+εi
运动载体气压计气压测量模型如下:
P(t)=P(t0)+εe(t)+εi-ρgΔh
上式中P(t0)为在t0时刻气压计测量所得气压值,εe(t)为随着时间因为环境变化(温度、天气等)而产生的气压变化,εi(t)为气压计本身特性所产生误差。
为消除环境气压变化εe(t)和室内空气对流对气压测量的影响,假设处在室内空间中各个点εe(t)相近,如附图11所示,在固定高度hA与hB设立气压计阵列,当标签位于气压计阵列中,则使用距标签最近的不同高度各四个气压计解算他们之间的气压差(为保证一致性,发明采用同一品牌同一型号气压计)。
具体包括:
步骤S42:记录运动载体搭载的气压计在初始时刻的气压值Ptag(t0)和所在的初始高度htag(t0);
步骤S43:根据当前时刻的水平位置信息,在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计,并测得气压值如下:
步骤S44:求得气压值之和为P(t),基于该8个气压计测得气压值得到气压值之和,并获取初始时刻的气压值以得到初始时刻的气压值之和;
步骤S45:基于步骤S44的结果得到环境气压变化量:
其中:εe(t)为环境气压变化量,P(t)为当前时刻的气压值之和,P(t0)为初始时刻的气压值之和;
步骤S46:基于8个气压计测得气压值,根据各气压计所在气压计阵列的高度以及位置,得到四个气压差值,并进一步得到比例系数:
其中:kt为比例系数,ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,hA为第一固定高度,hB为第二固定高度;
步骤S47:根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度,以及环境气压变化量得到高度位置信息:
其中:htag(t)为t时刻的高度位置信息,Ptag(t)为运动载体t时刻气压值,Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值,htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。
为提高定高系统的容错性和鲁棒性,发明使用互补滤波的方法融合加速度计信息和气压值阵列高度测量信息。由姿态矩阵得到载体在地理坐标系下竖直加速度imu_acc_z,由气压计信息得到气压计高度Bar_hight及气压计速度bar_speed_z,最后使用二级串联互补滤波进行融合。系统结构图见附图12。
Claims (8)
1.一种基于多传感器信息融合的室内3D实时定位方法,其特征在于,包括:
步骤S1:对UWB的测距信息进行滤波,并解算得到UWB水平位置信息;
步骤S2:使用光纤陀螺与加速度计进行姿态解算,分别得到载体在竖直方向解算结果和水平解算结果;
步骤S3:基于水平解算结果和UWB水平位置信息得到水平位置信息;
步骤S4:基于竖直方向解算结果和气压计阵列数据得到高度位置信息;
所述步骤S4具体包括:
步骤S41:记录在第一固定高度的气压计阵列的各气压计在初始时刻测得的气压,以及在第二固定高度的气压计阵列的各气压计在初始时刻测得的气压;
步骤S42:记录运动载体搭载的气压计在初始时刻的气压值和所在的初始高度;
步骤S43:根据当前时刻的水平位置信息,在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计;
步骤S44:基于8个气压计测得气压值得到气压值之和,并获取初始时刻的气压值以得到初始时刻的气压值之和;
步骤S45:基于步骤S44的结果得到环境气压变化量:
其中:εe(t)为环境气压变化量,P(t)为当前时刻的气压值之和,P(t0)为初始时刻的气压值之和;
步骤S46:基于8个气压计测得气压值,根据各气压计所在气压计阵列的高度以及位置,得到四个气压差值,并进一步得到比例系数:
其中:kt为比例系数,ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,hA为第一固定高度,hB为第二固定高度;
步骤S47:根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度,以及环境气压变化量得到高度位置信息:
其中:htag(t)为t时刻的高度位置信息,Ptag(t)为运动载体t时刻气压值,Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值,htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。
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.一种基于多传感器信息融合的室内3D实时定位装置,其特征在于,包括存储器、处理器,以及存储于存储器中并由所述处理器执行的程序,所述处理器执行所述程序时实现以下步骤:
步骤S1:对UWB的测距信息进行滤波,并解算得到UWB水平位置信息;
步骤S2:使用光纤陀螺与加速度计进行姿态解算,分别得到载体在竖直方向解算结果和水平解算结果;
步骤S3:基于水平解算结果和UWB水平位置信息得到水平位置信息;
步骤S4:基于竖直方向解算结果和气压计阵列数据得到高度位置信息;
所述步骤S4具体包括:
步骤S41:记录在第一固定高度的气压计阵列的各气压计在初始时刻测得的气压,以及在第二固定高度的气压计阵列的各气压计在初始时刻测得的气压;
步骤S42:记录运动载体搭载的气压计在初始时刻的气压值和所在的初始高度;
步骤S43:根据当前时刻的水平位置信息,在两个不同高度的气压计阵列中各选取4个距离运动载体最近的气压计;
步骤S44:基于8个气压计测得气压值得到气压值之和,并获取初始时刻的气压值:
以得到初始时刻的气压值之和P(t0);
步骤S45:基于步骤S44的结果得到环境气压变化量:
其中:εe(t)为环境气压变化量,P(t)为当前时刻的气压值之和,P(t0)为初始时刻的气压值之和;
步骤S46:基于8个气压计测得气压值,根据各气压计所在气压计阵列的高度以及位置,得到四个气压差值,并进一步得到比例系数:
其中:kt为比例系数,ΔPm(t)为在第一固定高度的气压计阵列的第m个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPn(t)为在第一固定高度的气压计阵列的第n个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPp(t)为在第一固定高度的气压计阵列的第p个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,ΔPq(t)为在第一固定高度的气压计阵列的第q个气压计与在第二固定高度的气压计阵列中对应气压计的气压差值,hA为第一固定高度,hB为第二固定高度;
步骤S47:根据比例系数、运动载体搭载的气压计在初始时刻的气压值和所在的初始高度,以及环境气压变化量得到高度位置信息:
其中:htag(t)为t时刻的高度位置信息,Ptag(t)为运动载体t时刻气压值,Ptag(t0)为运动载体搭载的气压计在初始时刻的气压值,htag(t0)为运动载体搭载的气压计在初始时刻所在的初始高度。
6.根据权利要求5所述的一种基于多传感器信息融合的室内3D实时定位装置,其特征在于,所述步骤S1具体包括:
步骤S11:使用卡尔曼滤波器对UWB的测距信息进行滤波得到滤波后的距离信息;
步骤S12:基于滤波后的距离信息解算得到UWB测量位置信息;
步骤S13:将UWB测量位置信息输入位置回归模型得到标定后的UWB测量位置信息。
7.根据权利要求6所述的一种基于多传感器信息融合的室内3D实时定位装置,其特征在于,所述卡尔曼滤波器为反遗忘量测噪声自适应卡尔曼滤波器,所述步骤S11具体包括:
步骤S111:基于UWB的测距信息判断当前时刻的距离与上一时刻滤波后的距离的差值是否大于设定的差值阈值,若为是,则执行步骤S112,反之,则执行步骤S113;
步骤S112:将当前时刻的测距信息通过数字一阶滤波器后再通过卡尔曼滤波器得到滤波后的距离信息;
步骤S113:将当前时刻的测距信息通过卡尔曼滤波器得到滤波后的距离信息。
8.根据权利要求6所述的一种基于多传感器信息融合的室内3D实时定位装置,其特征在于,所述步骤S3具体为:使用水平位置导航算法融合标定且滤波后的UWB测量位置信息与UWB坐标下运动载体惯性导航信息,最终融合得到水平位置信息。
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 CN110686671A (zh) | 2020-01-14 |
CN110686671B true 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) |
Families Citing this family (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN113494910B (zh) * | 2020-04-02 | 2024-06-18 | 广州汽车集团股份有限公司 | 一种基于uwb定位的车辆定位方法、装置及存储介质 |
CN111550551B (zh) * | 2020-04-08 | 2021-07-27 | 杭州鸿泉物联网技术股份有限公司 | 节油辅助驾驶方法、电子设备及存储介质 |
CN112197684A (zh) * | 2020-08-28 | 2021-01-08 | 浙江农林大学 | 一种树木位置测量装置及方法 |
CN112747747B (zh) * | 2021-01-20 | 2022-10-11 | 重庆邮电大学 | 一种改进的uwb/imu融合室内行人定位方法 |
CN114608572B (zh) * | 2022-03-25 | 2024-07-05 | 高斯机器人(深圳)有限公司 | 一种使用uwb与imu结合实现agv室内定位的方法 |
CN115265592B (zh) * | 2022-07-18 | 2024-04-09 | 北京航空航天大学 | 一种光纤陀螺磁温交联耦合误差的在线补偿方法 |
CN115444405B (zh) * | 2022-08-29 | 2024-07-23 | 同济大学 | 基于腰骶部穿戴式设备的摔倒预判方法、装置及存储介质 |
Citations (7)
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定位方法、装置及定位标签 |
Family Cites Families (1)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
US10382894B2 (en) * | 2017-07-28 | 2019-08-13 | Electronics And Telecommunications Research Institute | Method of measuring inter-device relative coordinates and device using the same |
-
2019
- 2019-09-29 CN CN201910934581.9A patent/CN110686671B/zh active Active
Patent Citations (7)
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技术的室内行人导航方法 |
CN108036784A (zh) * | 2017-11-10 | 2018-05-15 | 云保(佛山)智控科技有限公司 | 一种室内定位方法、导航方法及系统 |
CN108919181A (zh) * | 2018-09-05 | 2018-11-30 | 成都精位科技有限公司 | 基于惯导的uwb定位方法、装置及定位标签 |
Also Published As
Publication number | Publication date |
---|---|
CN110686671A (zh) | 2020-01-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110686671B (zh) | 基于多传感器信息融合的室内3d实时定位方法及装置 | |
Zhou et al. | Activity sequence-based indoor pedestrian localization using smartphones | |
CN109682375B (zh) | 一种基于容错决策树的uwb辅助惯性定位方法 | |
CN113108791B (zh) | 一种导航定位方法及导航定位设备 | |
US9146113B1 (en) | System and method for localizing a trackee at a location and mapping the location using transitions | |
CN113124856B (zh) | 基于uwb在线锚点的视觉惯性紧耦合里程计及计量方法 | |
US8688375B2 (en) | Method and system for locating and monitoring first responders | |
CN110398245B (zh) | 基于脚戴式惯性测量单元的室内行人导航姿态估计方法 | |
EP3865914A1 (en) | Gnss/imu surveying and mapping system and method | |
CN108120438B (zh) | 一种基于imu和rfid信息融合的室内目标快速跟踪方法 | |
CN108362288B (zh) | 一种基于无迹卡尔曼滤波的偏振光slam方法 | |
Zahran et al. | A new velocity meter based on Hall effect sensors for UAV indoor navigation | |
CN107014375B (zh) | 超低部署的室内定位系统及方法 | |
CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
CN107246872B (zh) | 基于mems传感器和vlc定位融合的单粒子滤波导航装置和方法 | |
CN117289322B (zh) | 一种基于imu、gps与uwb的高精度定位算法 | |
Hasan et al. | Smart phone based sensor fusion by using Madgwick filter for 3D indoor navigation | |
CN114111802B (zh) | 一种行人航迹推算辅助uwb的定位方法 | |
Gao et al. | An integrated land vehicle navigation system based on context awareness | |
AU2015201877B2 (en) | Method and system for locating and monitoring first responders | |
Liu et al. | An indoor 3-D quadrotor localization algorithm based on WiFi RTT and MEMS sensors | |
CN106500706A (zh) | 一种导航的方法和终端 | |
Elbes et al. | Gyroscope drift correction based on TDoA technology in support of pedestrian dead reckoning | |
CN115639585A (zh) | 一种gps/imu和激光雷达的多传感器融合定位方法 | |
Zhang et al. | A vision/inertia integrated positioning method using position and orientation matching |
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 |