CN112540345A - 一种基于高斯分布检测uwb质量的双模型定位方法及系统 - Google Patents

一种基于高斯分布检测uwb质量的双模型定位方法及系统 Download PDF

Info

Publication number
CN112540345A
CN112540345A CN202011299400.9A CN202011299400A CN112540345A CN 112540345 A CN112540345 A CN 112540345A CN 202011299400 A CN202011299400 A CN 202011299400A CN 112540345 A CN112540345 A CN 112540345A
Authority
CN
China
Prior art keywords
uwb
model
dual
moment
laser radar
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
CN202011299400.9A
Other languages
English (en)
Other versions
CN112540345B (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.)
Guangdong University of Technology
Original Assignee
Guangdong University of Technology
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 Guangdong University of Technology filed Critical Guangdong University of Technology
Priority to CN202011299400.9A priority Critical patent/CN112540345B/zh
Publication of CN112540345A publication Critical patent/CN112540345A/zh
Application granted granted Critical
Publication of CN112540345B publication Critical patent/CN112540345B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems
    • 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
    • G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
    • G01S17/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
    • 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/0257—Hybrid positioning
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/02—Services making use of location information
    • H04W4/021—Services related to particular areas, e.g. point of interest [POI] services, venue services or geofences
    • H—ELECTRICITY
    • H04—ELECTRIC COMMUNICATION TECHNIQUE
    • H04W—WIRELESS COMMUNICATION NETWORKS
    • H04W4/00—Services specially adapted for wireless communication networks; Facilities therefor
    • H04W4/30—Services specially adapted for particular environments, situations or purposes
    • H04W4/33—Services specially adapted for particular environments, situations or purposes for indoor environments, e.g. buildings
    • Y—GENERAL 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
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02D—CLIMATE 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/00—Reducing energy consumption in communication networks
    • Y02D30/70—Reducing energy consumption in communication networks in wireless communication networks

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • General Physics & Mathematics (AREA)
  • Computer Networks & Wireless Communication (AREA)
  • Signal Processing (AREA)
  • Electromagnetism (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Optical Radar Systems And Details Thereof (AREA)

Abstract

本发明提供的一种基于高斯分布检测UWB质量的双模型定位方法，包括：计算UWB系统预测模型的估计距离，构建UWB系统的量测方程；根据估计距离及UWB系统的量测方程判断UWB质量；构造激光雷达SLAM的量测方程；根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新。本发明提供的双模型定位方法及系统，通过检测UWB系统测距质量，有效提升了测距的可靠性；通过引入双模型定位模型，通过判断UWB系统质量，当UWB系统质量优良时采用双模型定位，当质量较差时，则舍弃UWB系统数据，直接使用激光雷达SLAM预测方程定位，大大提高了室内定位的精准度。

Description

一种基于高斯分布检测UWB质量的双模型定位方法及系统
技术领域
本发明涉及室内高精度定位技术领域，更具体的，涉及一种基于高斯分布检测UWB质量的双模型定位方法及系统。
背景技术
随着定位技术的不断发展，定位信息变得越来越重要，因为基于位置的服务例如导航、导游、导购、救援等，这些服务都使得定位信息变得重要。室外环境下全球卫星导航系统(GNSS)发展成熟，然而在室内，由于建筑物遮挡加上室内封闭的空间和性能显著下降，GNSS无法在室内发挥出精准的定位效果。GNSS的盲区定位已成为亟待解决的科学问题。利用室内定位技术可以解决盲区定位问题，主流的定位技术有超宽带(UWB)技术和激光雷达定位(SLAM)技术。然而UWB受非视距(NLOS)和多路径效应等影响明显，而现有的激光雷达SLAM在定位时存在累积误差漂移，使得激光雷达定位存在误差。
针对以上问题，陈志键、徐爱功等人提出了一种室内UWB/LiDAR组合定位算法[1]陈志键,徐爱功,隋心,郝雨时,郭哲.室内UWB/LiDAR组合定位算法[J].导航定位学报,2019,7(01):38-42+111，来进行定位。将定位分为UWB测距信息、LiDAR(激光雷达)SLAM的位移增量和角度观测值作为量测值，计算步骤如下：
利用UWB测距模型，计算UWB测距信息。基于往返时间(roundtriptime，RTT)测距，通过测量UWB脉冲信号从流动站发出后到达基准站之后返回流动站的总时间，间接确定二者之间的距离。传输过程由输入信号触发，RTT测距不需要基站之间以及基站流动站保持时间同步。
计算LiDAR SLAM的位移增量和角度观测值。通过k时刻LiDAR获得的数据，解算得到k时刻相对于k-1时刻LiDAR的位移增量与角度观测值，同时采用非线性运动模型推算k+1时刻位姿信息，进而实现LiDAR的状态预估。
利用计算机对UWB和LiDAR添加时间标签并进行时间同步；将LiDAR和UWB2类传感器中心固定在同一铅垂线，达到空间同步。将UWB测距观测值、LiDAR SLAM的位移增量和角度观测值作为量测值，建立了UWB/LiDAR组合定位模型。利用卡尔曼滤波算法进行参数解算。
但该技术的实现过程中，UWB受到非视距与多路径效应等因素的影响，容易造成较大的定位误差，现有技术在融合前未对UWB测距值进行检测，当UWB测距质量较差时，大大降低了其定位精准度。
发明内容
本发明为克服现有的融合定位技术在融合前未对UWB测距值进行检测，当UWB测距质量较差时，存在定位精准度低的技术缺陷，提供一种基于高斯分布检测UWB质量的双模型定位方法及系统。
为解决上述技术问题，本发明的技术方案如下：
一种基于高斯分布检测UWB质量的双模型定位方法，包括以下步骤：
S1：构建UWB系统预测模型，计算估计距离；
S2：建立量测模型，计算实际观测距离，构建UWB系统的量测方程；
S3：根据估计距离及UWB系统的量测方程判断UWB质量；
S4：构造激光雷达SLAM的量测方程；
S5：根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；
S6：采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果。
上述方案中，通过本发明提供的定位方法，通过检测UWB系统测距质量，有效提升了测距的可靠性；通过引入双模型定位模型，通过判断UWB系统质量，当UWB系统质量优良时采用双模型定位，当质量较差时，则舍弃UWB系统数据，直接使用激光雷达SLAM预测方程定位，大大提高了室内定位的精准度。
其中，所述步骤S1具体为：
构建UWB系统预测模型，具体为：
Xk＝AXk-1+wk-1 (1)
式中，Xk＝[σxkσyk]T，σxk和σyk为k时刻UWB系统x和y方向的坐标相对于k-1时刻UWB系统x和y方向的坐标增量；wk-1为过程噪声序列；设A为状态转移矩阵，ΔT为UWB系统的数据采样间隔，则有：
Figure BDA0002786365350000031
在UWB系统预测模型下，定义K时刻对i个基站的估计距离ri为：
Figure BDA0002786365350000032
其中，在所述步骤S2中，UWB系统的量测方程：
Figure BDA0002786365350000033
式中：
Figure BDA0002786365350000034
为k时刻第i个基站和UWB系统间的量测距离；
Figure BDA0002786365350000035
分别为第i个基站的x、y方向坐标；vk,i为观测噪声序列。
其中，在所述步骤S3中，定义UWB系统与第i个基站估计距离于系统实际观测距离差值为Mi，在高斯分布中，对该Mi值进行概率密度估计，将Mi作为检测UWB系统对任意一个基站测距质量的判断依据，具体计算公式为：
Figure BDA0002786365350000036
其中，在所述步骤S4中，激光雷达SLAM的量测方程具体为：
Figure BDA0002786365350000037
式中：vk,σd、vk,θ为观测噪声序列；σrk为k时刻激光雷达相对于k-1时刻位置变换的真实距离，且有：
Figure BDA0002786365350000038
其中，在所述步骤S5中，根据式(3)、式(4)、式(6)和式(7)，得到双模型定位模型的过程具体为：
Figure BDA0002786365350000039
Figure BDA00027863653500000310
式中：
Figure BDA00027863653500000311
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的量测信息；
Figure BDA00027863653500000312
为k时刻UWB/激光雷达SLAM组合系统相对于相对于k-1时刻的旋转角度；
Figure BDA00027863653500000313
为UWB/激光雷达SLAM组合系统k-1时刻的航向角；
Figure BDA00027863653500000314
Figure BDA00027863653500000315
为观测噪声序列；
对式(4)、式(8)、式(9)进行线性化，得到：
Figure BDA0002786365350000041
Figure BDA0002786365350000042
Figure BDA0002786365350000043
式中：
Figure BDA0002786365350000044
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的观测距离；
Figure BDA0002786365350000045
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的概略距离；
Figure BDA0002786365350000046
Figure BDA0002786365350000047
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的观测值；
Figure BDA0002786365350000048
为k-1时刻UWB/激光雷达SLAM组合系统距离变换的观测值；
Figure BDA0002786365350000049
Figure BDA00027863653500000410
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻旋转的观测角度；
Figure BDA00027863653500000411
为k-1时刻UWB/激光雷达SLAM组合系统旋转的观测角度；tk-1＝1+[(yk-1-yk)/(xk-1-xk)]2；εk,i、
Figure BDA00027863653500000412
和
Figure BDA00027863653500000413
为线性化噪声；因此得到双模型定位模型：
Yk＝HkXk+vk (13)
式中：
Figure BDA00027863653500000414
Yk为观测向量，具体为：
Figure BDA00027863653500000415
Hk为量测矩阵，具体为：
Figure BDA00027863653500000416
其中，在所述步骤S6中，采用EKF算法对双模型定位模型进行融合测距，递推公式为：
Figure BDA00027863653500000417
Figure BDA0002786365350000051
Figure BDA0002786365350000052
Figure BDA0002786365350000053
Pk＝[I-KkHh]Pk/k-1 (21)
式中，
Figure BDA0002786365350000054
为预测值；
Figure BDA0002786365350000055
为状态的先验预测值；Pk为误差协方差阵；Pk/k-1为误差协方差阵的先验估计；Qk为系统的噪声方差阵；Kk为增益矩阵；Rk为量测噪声方差阵；f(·)和h(·)分别为系统状态和量测的线性化函数；通过初值
Figure BDA0002786365350000056
和
Figure BDA0002786365350000057
由式(17)、式(18)对UWB/激光雷达SLAM组合系统进行预测更新，由式(19)～式(21)对UWB/激光雷达SLAM组合系统进行状态更新；
此时，根据k时刻的量测消息Yk，即递推得到状态估计
Figure BDA0002786365350000058
然后通过状态估计
Figure BDA0002786365350000059
修正更新激光雷达的位置信息和姿态信息，提高双模型定位模型定位结果。
一种基于高斯分布检测UWB质量的双模型定位系统，包括服务器和处理器；所述服务器用于数据的存储并为所述处理器提供相应的连接接口；所述处理器用于：
构建UWB系统预测模型，计算估计距离；
建立量测模型，计算实际观测距离，构建UWB系统的量测方程；
根据估计距离及UWB系统的量测方程判断UWB质量；
构造激光雷达SLAM的量测方程；
根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；
采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果。
其中，所述处理器包括UWB系统预测模型构建模块、UWB系统的量测方程构建模块、UWB质量判断模块、激光雷达SLAM的量测方程构造模块、双模型定位模型构建模块和双模型定位模型状态更新模块；其中：
所述UWB系统预测模型构建模块用于构建UWB系统预测模型，计算估计距离，具体为：
构建UWB系统预测模型，具体为：
Xk＝AXk-1+wk-1 (1)
式中，Xk＝[σxkσyk]T，σxk和σyk为k时刻UWB系统x和y方向的坐标相对于k-1时刻UWB系统x和y方向的坐标增量；wk-1为过程噪声序列；设A为状态转移矩阵，ΔT为UWB系统的数据采样间隔，则有：
Figure BDA0002786365350000061
在UWB系统预测模型下，定义K时刻对i个基站的估计距离ri为：
Figure BDA0002786365350000062
所述UWB系统的量测方程构建模块用于建立量测模型，计算实际观测距离，构建UWB系统的量测方程，具体为：
Figure BDA0002786365350000063
式中：
Figure BDA0002786365350000064
为k时刻第i个基站和UWB系统间的量测距离；
Figure BDA0002786365350000065
分别为第i个基站的x、y方向坐标；vk,i为观测噪声序列；
所述UWB质量判断模块用于根据估计距离及UWB系统的量测方程判断UWB质量，具体为：
定义UWB系统与第i个基站估计距离于系统实际观测距离差值为Mi，在高斯分布中，对该Mi值进行概率密度估计，将Mi作为检测UWB系统对任意一个基站测距质量的判断依据，具体计算公式为：
Figure BDA0002786365350000066
所述激光雷达SLAM的量测方程构造模块用于构造激光雷达SLAM的量测方程，激光雷达SLAM的量测方程具体为：
Figure BDA0002786365350000067
式中：vk,σd、vk,θ为观测噪声序列；σrk为k时刻激光雷达相对于k-1时刻位置变换的真实距离，且有：
Figure BDA0002786365350000068
所述双模型定位模型构建模块用于根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型，具体为：
根据式(3)、式(4)、式(6)和式(7)，得到双模型定位模型的过程具体为：
Figure BDA0002786365350000071
Figure BDA0002786365350000072
式中：
Figure BDA0002786365350000073
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的量测信息；
Figure BDA0002786365350000074
为k时刻UWB/激光雷达SLAM组合系统相对于相对于k-1时刻的旋转角度；
Figure BDA0002786365350000075
为UWB/激光雷达SLAM组合系统k-1时刻的航向角；
Figure BDA0002786365350000076
Figure BDA0002786365350000077
为观测噪声序列；
对式(4)、式(8)、式(9)进行线性化，得到：
Figure BDA0002786365350000078
Figure BDA0002786365350000079
Figure BDA00027863653500000710
式中：
Figure BDA00027863653500000711
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的观测距离；
Figure BDA00027863653500000712
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的概略距离；
Figure BDA00027863653500000713
Figure BDA00027863653500000714
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的观测值；
Figure BDA00027863653500000715
为k-1时刻UWB/激光雷达SLAM组合系统距离变换的观测值；
Figure BDA00027863653500000716
Figure BDA00027863653500000717
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻旋转的观测角度；
Figure BDA00027863653500000718
为k-1时刻UWB/激光雷达SLAM组合系统旋转的观测角度；tk-1＝1+[(yk-1-yk)/(xk-1-xk)]2；εk,i、
Figure BDA00027863653500000719
和
Figure BDA00027863653500000720
为线性化噪声；因此得到双模型定位模型：
Yk＝HkXk+vk (13)
式中：
Figure BDA00027863653500000721
Yk为观测向量，具体为：
Figure BDA00027863653500000722
Hk为量测矩阵，具体为：
Figure BDA0002786365350000081
所述双模型定位模型状态更新模块用于采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果，具体为：
采用EKF算法对双模型定位模型进行融合测距，递推公式为：
Figure BDA0002786365350000082
Figure BDA0002786365350000083
Figure BDA0002786365350000084
Figure BDA0002786365350000085
Pk＝[I-KkHh]Pk/k-1 (21)
式中，
Figure BDA0002786365350000086
为预测值；
Figure BDA0002786365350000087
为状态的先验预测值；Pk为误差协方差阵；Pk/k-1为误差协方差阵的先验估计；Qk为系统的噪声方差阵；Kk为增益矩阵；Rk为量测噪声方差阵；f(·)和h(·)分别为系统状态和量测的线性化函数；通过初值
Figure BDA0002786365350000088
和
Figure BDA0002786365350000089
由式(17)、式(18)对UWB/激光雷达SLAM组合系统进行预测更新，由式(19)～式(21)对UWB/激光雷达SLAM组合系统进行状态更新；
此时，根据k时刻的量测消息Yk，即递推得到状态估计
Figure BDA00027863653500000810
然后通过状态估计
Figure BDA00027863653500000811
修正更新激光雷达的位置信息和姿态信息，提高双模型定位模型定位结果。
其中，所述服务器的主体为存储器。
与现有技术相比，本发明技术方案的有益效果是：
本发明提供的一种基于高斯分布检测UWB质量的双模型定位方法及系统，通过检测UWB系统测距质量，有效提升了测距的可靠性；通过引入双模型定位模型，通过判断UWB系统质量，当UWB系统质量优良时采用双模型定位，当质量较差时，则舍弃UWB系统数据，直接使用激光雷达SLAM预测方程定位，大大提高了室内定位的精准度。
附图说明
图1为本发明方法流程示意图；
图2为UWB解算轨迹图；
图3为参考节点平面图；
图4为标准高斯分布落在[-2σ,2σ]概率示意图；
图5为UWB质量检测流程；
图6为SLAM定位算法流程图；
图7为UWB/激光雷达组合定位解算轨迹图。
具体实施方式
附图仅用于示例性说明，不能理解为对本专利的限制；
为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；
对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。
下面结合附图和实施例对本发明的技术方案做进一步的说明。
实施例1
如图1所示，一种基于高斯分布检测UWB质量的双模型定位方法，包括以下步骤：
S1：构建UWB系统预测模型，计算估计距离；
S2：建立量测模型，计算实际观测距离，构建UWB系统的量测方程；
S3：根据估计距离及UWB系统的量测方程判断UWB质量；
S4：构造激光雷达SLAM的量测方程；
S5：根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；
S6：采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果。
在具体实施过程中，由于现有UWB系统受到非视距于多路径效应等因素的影响，容易造成较大的定位误差，现有技术在融合前未对UWB测距值进行检测，当UWB测距质量较差时，定位精准度会大大降低。通过本发明提供的定位方法，通过检测UWB系统测距质量，有效提升了测距的可靠性；通过引入双模型定位模型，通过判断UWB系统质量，当UWB系统质量优良时采用双模型定位，当质量较差时，则舍弃UWB系统数据，直接使用激光雷达SLAM预测方程定位，大大提高了室内定位的精准度。
实施例2
更具体的，在实施例1的基础上，从图2可以观测到，利用UWB的原始数据解算出的轨迹结果。如图所示，由于遮挡，RTT测距信息出现异常，引起NLOS误差，定位误差最大达到1.14m，水平精度为0.15m，其他视距环境下，观测环境较好，UWB解算轨迹较为精确。
NLOS误差的出现来源于客观环境的遮挡，而这个遮挡也符合我们设置的预期。为了能准确解算UWB参数，这个误差是需要去除的。对此，提出一个基于高斯分布的UWB质量检测机制的场景，考虑一个由三个参考基站(R1,R2,R3)和一个位于平面区域的目标节点组成的传感器网络。其中，参考基站(节点)是固定的，并且它们的位置已知，如图3所示。假设所有的参考节点彼此同步，目标节点(UWB系统)的情况是静止的或移动的。
通过将3个参考节点视为处于LOS(视距)信道条件来执行位置估计。根据估计距离ri，与UWB系统实际观测距离计算距离估计的均方误差。对一个基站，将估计距离与系统实际观测距离差值记为M，在高斯分布中，对该M值进行概率密度估计。根据研究与实际量测结果，建立一个均值为0的高斯分布，将M作为检测UWB质量的判断依据，确定M≤10cm时，认为该概率密度落在正态分布中[-2σ,2σ]，如图4所示,记UWB系统此次测距质量良好，反之判定质量不好。在给出的场景下，利用UWB系统同时对三个基站进行质量检测，当两个或两个以上的M值≤10cm时，认为UWB系统质量良好，反之认为此UWB系统质量不好。
在UWB系统质量良好的情况下，提出UWB与SLAM双模型定位算法对系统进行进一步优化；在UWB系统质量不好的情况下，则舍弃此UWB系统的测距信息，直接使用激光雷达SLAM量测方程进行定位，如图5所示。
实施例3
更具体的，在实施例1、实施例2的基础上，所述步骤S1具体为：
构建UWB系统预测模型，具体为：
Xk＝AXk-1+wk-1 (1)
式中，Xk＝[σxkσyk]T，σxk和σyk为k时刻UWB系统x和y方向的坐标相对于k-1时刻UWB系统x和y方向的坐标增量；wk-1为过程噪声序列；设A为状态转移矩阵，ΔT为UWB系统的数据采样间隔，则有：
Figure BDA0002786365350000111
在UWB系统预测模型下，定义K时刻对i个基站的估计距离ri为：
Figure BDA0002786365350000112
更具体的，在所述步骤S2中，UWB系统的量测方程：
Figure BDA0002786365350000113
式中：
Figure BDA0002786365350000114
为k时刻第i个基站和UWB系统间的量测距离；
Figure BDA0002786365350000115
分别为第i个基站的x、y方向坐标；vk,i为观测噪声序列。
更具体的，在所述步骤S3中，定义UWB系统与第i个基站估计距离于系统实际观测距离差值为Mi，在高斯分布中，对该Mi值进行概率密度估计，将Mi作为检测UWB系统对任意一个基站测距质量的判断依据，具体计算公式为：
Figure BDA0002786365350000116
在具体实施过程中，在高斯分布中，对该Mi值进行概率密度估计，由研究与实际量测结果，确定Mi≤10cm时，认为该概率密度落在正态分布中[2σ,2σ],此时概率为0.9544，记UWB系统此次测距质量良好，反之判定此次测距质量不好。基于此，将Mi作为检测UWB对任意一个基站测距质量的判断依据。
在具体实施过程中，在给出的3个基站的测试场景下，分别记距离估计距离与系统实际观测距离差值为M1，M2，M3；且当M1，M2，M3均≤10cm或任取M1，M2，M3两项≤10cm时，判定此UWB系统质量良好。在UWB系统质量良好的基础上，提出UWB与SLAM双模型定位算法对系统进行进一步优化。
在具体实施过程中，SLAM算法是目前解决LiDAR室内定位的常用的方法，算法的主要原理是通过k时刻激光雷达获得的数据，解算得到激光雷达的位姿信息(σdk,θk)，其中σdk为k时刻LiDAR相对于k-1时刻的位移增量，θk为k时刻LiDAR相对于k-1时刻的旋转角度。本发明采用的是EKF--SLAM算法，主要包括位姿预测、位姿观测、位姿更新和状态扩充4个阶段，算法流程图如图6所示。
位姿预测是LiDAR在已知k-1时刻的位姿(σdk-1,θk-1)的情况下，采用非线性的运动模型推算k时刻位姿信息
Figure BDA0002786365350000121
进而实现LiDAR的状态预估。但推算的位姿信息存在误差，需要通过位姿观测以及位姿更新削弱误差。位姿观测是当LiDAR观测到k时刻的环境特征点时，获得位姿信息
Figure BDA0002786365350000122
位姿更新是将推算得到的位姿信息
Figure BDA0002786365350000123
与观测得到的位姿信息
Figure BDA0002786365350000124
进行数据关联，从而获得此时刻位姿信息((σdk,θk))。在行进过程中，会不断探测到新的特征点，此时需要扩充状态估计，加入新的特征信息。
更具体的，在所述步骤S4中，激光雷达SLAM的量测方程具体为：
Figure BDA0002786365350000125
式中：vk,σd、vk,θ为观测噪声序列；σrk为k时刻激光雷达相对于k-1时刻位置变换的真实距离，且有：
Figure BDA0002786365350000126
更具体的，在所述步骤S5中，根据式(3)、式(4)、式(6)和式(7)，得到双模型定位模型的过程具体为：
Figure BDA0002786365350000127
Figure BDA0002786365350000128
式中：
Figure BDA0002786365350000129
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的量测信息；
Figure BDA00027863653500001210
为k时刻UWB/激光雷达SLAM组合系统相对于相对于k-1时刻的旋转角度；
Figure BDA00027863653500001211
为UWB/激光雷达SLAM组合系统k-1时刻的航向角；
Figure BDA00027863653500001212
Figure BDA00027863653500001213
为观测噪声序列；
对式(4)、式(8)、式(9)进行线性化，得到：
Figure BDA00027863653500001214
Figure BDA00027863653500001215
Figure BDA00027863653500001216
式中：
Figure BDA0002786365350000131
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的观测距离；
Figure BDA0002786365350000132
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的概略距离；
Figure BDA0002786365350000133
Figure BDA0002786365350000134
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的观测值；
Figure BDA0002786365350000135
为k-1时刻UWB/激光雷达SLAM组合系统距离变换的观测值；
Figure BDA0002786365350000136
Figure BDA0002786365350000137
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻旋转的观测角度；
Figure BDA0002786365350000138
为k-1时刻UWB/激光雷达SLAM组合系统旋转的观测角度；tk-1＝1+[(yk-1-yk)/(xk-1-xk)]2；εk,i、
Figure BDA0002786365350000139
和
Figure BDA00027863653500001310
为线性化噪声；因此得到双模型定位模型：
Yk＝HkXk+vk (13)
式中：
Figure BDA00027863653500001311
Yk为观测向量，具体为：
Figure BDA00027863653500001312
Hk为量测矩阵，具体为：
Figure BDA00027863653500001313
其中，在所述步骤S6中，采用EKF算法对双模型定位模型进行融合测距，递推公式为：
Figure BDA00027863653500001314
Figure BDA00027863653500001315
Figure BDA00027863653500001316
Figure BDA00027863653500001317
Pk＝[I-KkHh]Pk/k-1 (21)
式中，
Figure BDA00027863653500001318
为预测值；
Figure BDA00027863653500001319
为状态的先验预测值；Pk为误差协方差阵；Pk/k-1为误差协方差阵的先验估计；Qk为系统的噪声方差阵；Kk为增益矩阵；Rk为量测噪声方差阵；f(·)和h(·)分别为系统状态和量测的线性化函数；通过初值
Figure BDA0002786365350000141
和
Figure BDA0002786365350000142
由式(17)、式(18)对UWB/激光雷达SLAM组合系统进行预测更新，由式(19)～式(21)对UWB/激光雷达SLAM组合系统进行状态更新；
此时，根据k时刻的量测消息Yk，即递推得到状态估计
Figure BDA0002786365350000143
然后通过状态估计
Figure BDA0002786365350000144
修正更新激光雷达的位置信息和姿态信息，提高双模型定位模型定位结果。
在具体实施过程中，如图7所示，在UWB质量良好的情况下，UWB/激光雷达SLAM双模型系统解算轨迹效果良好。
实施例4
更具体的，本发明在一种基于高斯分布检测UWB质量的双模型定位方法的基础上，提出一种双模型定位系统，包括服务器和处理器；所述服务器用于数据的存储并为所述处理器提供相应的连接接口；所述处理器用于：
构建UWB系统预测模型，计算估计距离；
建立量测模型，计算实际观测距离，构建UWB系统的量测方程；
根据估计距离及UWB系统的量测方程判断UWB质量；
构造激光雷达SLAM的量测方程；
根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；
采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果。
更具体的，所述处理器包括UWB系统预测模型构建模块、UWB系统的量测方程构建模块、UWB质量判断模块、激光雷达SLAM的量测方程构造模块、双模型定位模型构建模块和双模型定位模型状态更新模块；其中：
所述UWB系统预测模型构建模块用于构建UWB系统预测模型，计算估计距离，具体为：
构建UWB系统预测模型，具体为：
Xk＝AXk-1+wk-1 (1)
式中，Xk＝[σxkσyk]T，σxk和σyk为k时刻UWB系统x和y方向的坐标相对于k-1时刻UWB系统x和y方向的坐标增量；wk-1为过程噪声序列；设A为状态转移矩阵，ΔT为UWB系统的数据采样间隔，则有：
Figure BDA0002786365350000151
在UWB系统预测模型下，定义K时刻对i个基站的估计距离ri为：
Figure BDA0002786365350000152
所述UWB系统的量测方程构建模块用于建立量测模型，计算实际观测距离，构建UWB系统的量测方程，具体为：
Figure BDA0002786365350000153
式中：
Figure BDA0002786365350000154
为k时刻第i个基站和UWB系统间的量测距离；
Figure BDA0002786365350000155
分别为第i个基站的x、y方向坐标；vk,i为观测噪声序列；
所述UWB质量判断模块用于根据估计距离及UWB系统的量测方程判断UWB质量，具体为：
定义UWB系统与第i个基站估计距离于系统实际观测距离差值为Mi，在高斯分布中，对该Mi值进行概率密度估计，将Mi作为检测UWB系统对任意一个基站测距质量的判断依据，具体计算公式为：
Figure BDA0002786365350000156
所述激光雷达SLAM的量测方程构造模块用于构造激光雷达SLAM的量测方程，激光雷达SLAM的量测方程具体为：
Figure BDA0002786365350000157
式中：vk,σd、vk,θ为观测噪声序列；σrk为k时刻激光雷达相对于k-1时刻位置变换的真实距离，且有：
Figure BDA0002786365350000158
所述双模型定位模型构建模块用于根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型，具体为：
根据式(3)、式(4)、式(6)和式(7)，得到双模型定位模型的过程具体为：
Figure BDA0002786365350000159
Figure BDA0002786365350000161
式中：
Figure BDA0002786365350000162
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的量测信息；
Figure BDA0002786365350000163
为k时刻UWB/激光雷达SLAM组合系统相对于相对于k-1时刻的旋转角度；
Figure BDA0002786365350000164
为UWB/激光雷达SLAM组合系统k-1时刻的航向角；
Figure BDA0002786365350000165
Figure BDA0002786365350000166
为观测噪声序列；
对式(4)、式(8)、式(9)进行线性化，得到：
Figure BDA0002786365350000167
Figure BDA0002786365350000168
Figure BDA0002786365350000169
式中：
Figure BDA00027863653500001610
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的观测距离；
Figure BDA00027863653500001611
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的概略距离；
Figure BDA00027863653500001612
Figure BDA00027863653500001613
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的观测值；
Figure BDA00027863653500001614
为k-1时刻UWB/激光雷达SLAM组合系统距离变换的观测值；
Figure BDA00027863653500001615
Figure BDA00027863653500001616
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻旋转的观测角度；
Figure BDA00027863653500001617
为k-1时刻UWB/激光雷达SLAM组合系统旋转的观测角度；tk-1＝1+[(yk-1-yk)/(xk-1-xk)]2；εk,i、
Figure BDA00027863653500001618
和
Figure BDA00027863653500001619
为线性化噪声；因此得到双模型定位模型：
Yk＝HkXk+vk (13)
式中：
Figure BDA00027863653500001620
Yk为观测向量，具体为：
Figure BDA00027863653500001621
Hk为量测矩阵，具体为：
Figure BDA0002786365350000171
所述双模型定位模型状态更新模块用于采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果，具体为：
采用EKF算法对双模型定位模型进行融合测距，递推公式为：
Figure BDA0002786365350000172
Figure BDA0002786365350000173
Figure BDA0002786365350000174
Figure BDA0002786365350000175
Pk＝[I-KkHh]Pk/k-1 (21)
式中，
Figure BDA0002786365350000176
为预测值；
Figure BDA0002786365350000177
为状态的先验预测值；Pk为误差协方差阵；Pk/k-1为误差协方差阵的先验估计；Qk为系统的噪声方差阵；Kk为增益矩阵；Rk为量测噪声方差阵；f(·)和h(·)分别为系统状态和量测的线性化函数；通过初值
Figure BDA0002786365350000178
和
Figure BDA0002786365350000179
由式(17)、式(18)对UWB/激光雷达SLAM组合系统进行预测更新，由式(19)～式(21)对UWB/激光雷达SLAM组合系统进行状态更新；
此时，根据k时刻的量测消息Yk，即递推得到状态估计
Figure BDA00027863653500001710
然后通过状态估计
Figure BDA00027863653500001711
修正更新激光雷达的位置信息和姿态信息，提高双模型定位模型定位结果。
在具体实施过程中，本发明有益效果如下：
通过高斯分布检测UWB质量：常用的UWB测距系统受到非视距与多路径效应等因素的影响，现有技术在使用UWB测距技术前未对UWB测距值进行误差检测，当UWB系统测距质量较差时，定位精准度会大大降低。本发明提出的系统可以检测UWB测距质量，有效提升测距的可靠性；
双模型定位机制：本发明提出的这个机制，也与常规融合方式不同，常规方法直接对UWB与激光雷达SLAM进行融合并将结果直接作为定位输出，而本文提出的方法可以判断UWB系统质量，当UWB系统质量优良时采用双模型定位，当质量较差时，则舍弃UWB系统数据，直接使用激光雷达SLAM预测方程定位。
显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。

Claims (10)

1.一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，包括以下步骤：
S1：构建UWB系统预测模型，计算估计距离；
S2：建立量测模型，计算实际观测距离，构建UWB系统的量测方程；
S3：根据估计距离及UWB系统的量测方程判断UWB质量；
S4：构造激光雷达SLAM的量测方程；
S5：根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；
S6：采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果。
2.根据权利要求1所述的一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，所述步骤S1具体为：
构建UWB系统预测模型，具体为：
Xk＝AXk-1+wk-1 (1)
式中，Xk＝[σxk σyk]T，σxk和σyk为k时刻UWB系统x和y方向的坐标相对于k-1时刻UWB系统x和y方向的坐标增量；wk-1为过程噪声序列；设A为状态转移矩阵，ΔT为UWB系统的数据采样间隔，则有：
Figure FDA0002786365340000011
在UWB系统预测模型下，定义K时刻对i个基站的估计距离ri为：
Figure FDA0002786365340000012
3.根据权利要求2所述的一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，在所述步骤S2中，UWB系统的量测方程：
Figure FDA0002786365340000013
式中：
Figure FDA0002786365340000014
为k时刻第i个基站和UWB系统间的量测距离；
Figure FDA0002786365340000015
分别为第i个基站的x、y方向坐标；vk,i为观测噪声序列。
4.根据权利要求3所述的一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，在所述步骤S3中，定义UWB系统与第i个基站估计距离于系统实际观测距离差值为Mi，在高斯分布中，对该Mi值进行概率密度估计，将Mi作为检测UWB系统对任意一个基站测距质量的判断依据，具体计算公式为：
Figure FDA0002786365340000021
5.根据权利要求4所述的一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，在所述步骤S4中，激光雷达SLAM的量测方程具体为：
Figure FDA0002786365340000022
式中：vk,σd、vk,θ为观测噪声序列；σrk为k时刻激光雷达相对于k-1时刻位置变换的真实距离，且有：
Figure FDA0002786365340000023
6.根据权利要求5所述的一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，在所述步骤S5中，根据式(3)、式(4)、式(6)和式(7)，得到双模型定位模型的过程具体为：
Figure FDA0002786365340000024
Figure FDA0002786365340000025
式中：
Figure FDA0002786365340000026
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的量测信息；
Figure FDA0002786365340000027
为k时刻UWB/激光雷达SLAM组合系统相对于相对于k-1时刻的旋转角度；
Figure FDA0002786365340000028
为UWB/激光雷达SLAM组合系统k-1时刻的航向角；
Figure FDA0002786365340000029
Figure FDA00027863653400000210
为观测噪声序列；
对式(4)、式(8)、式(9)进行线性化，得到：
Figure FDA00027863653400000211
Figure FDA00027863653400000212
Figure FDA00027863653400000213
式中：
Figure FDA0002786365340000031
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的观测距离；
Figure FDA0002786365340000032
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的概略距离；
Figure FDA0002786365340000033
Figure FDA0002786365340000034
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的观测值；
Figure FDA0002786365340000035
为k-1时刻UWB/激光雷达SLAM组合系统距离变换的观测值；
Figure FDA0002786365340000036
Figure FDA0002786365340000037
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻旋转的观测角度；
Figure FDA0002786365340000038
为k-1时刻UWB/激光雷达SLAM组合系统旋转的观测角度；tk-1＝1+[(yk-1-yk)/(xk-1-xk)]2；εk,i、
Figure FDA0002786365340000039
和
Figure FDA00027863653400000310
为线性化噪声；因此得到双模型定位模型：
Yk＝HkXk+vk (13)
式中：
Figure FDA00027863653400000311
Yk为观测向量，具体为：
Figure FDA00027863653400000312
Hk为量测矩阵，具体为：
Figure FDA00027863653400000313
7.根据权利要求6所述的一种基于高斯分布检测UWB质量的双模型定位方法，其特征在于，在所述步骤S6中，采用EKF算法对双模型定位模型进行融合测距，递推公式为：
Figure FDA00027863653400000314
Figure FDA00027863653400000315
Figure FDA00027863653400000316
Figure FDA00027863653400000317
Pk＝[I-KkHh]Pk/k-1 (21)
式中，
Figure FDA00027863653400000318
为预测值；
Figure FDA00027863653400000319
为状态的先验预测值；Pk为误差协方差阵；Pk/k-1为误差协方差阵的先验估计；Qk为系统的噪声方差阵；Kk为增益矩阵；Rk为量测噪声方差阵；f(·)和h(·)分别为系统状态和量测的线性化函数；通过初值
Figure FDA0002786365340000041
和
Figure FDA0002786365340000042
由式(17)、式(18)对UWB/激光雷达SLAM组合系统进行预测更新，由式(19)～式(21)对UWB/激光雷达SLAM组合系统进行状态更新；
此时，根据k时刻的量测消息Yk，即递推得到状态估计
Figure FDA0002786365340000043
然后通过状态估计
Figure FDA0002786365340000044
修正更新激光雷达的位置信息和姿态信息，提高双模型定位模型定位结果。
8.一种基于高斯分布检测UWB质量的双模型定位系统，其特征在于，包括服务器和处理器；所述服务器用于数据的存储并为所述处理器提供相应的连接接口；所述处理器用于：
构建UWB系统预测模型，计算估计距离；
建立量测模型，计算实际观测距离，构建UWB系统的量测方程；
根据估计距离及UWB系统的量测方程判断UWB质量；
构造激光雷达SLAM的量测方程；
根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型；
采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果。
9.根据权利要求8所述的一种基于高斯分布检测UWB质量的双模型定位系统，其特征在于，所述处理器包括UWB系统预测模型构建模块、UWB系统的量测方程构建模块、UWB质量判断模块、激光雷达SLAM的量测方程构造模块、双模型定位模型构建模块和双模型定位模型状态更新模块；其中：
所述UWB系统预测模型构建模块用于构建UWB系统预测模型，计算估计距离，具体为：
构建UWB系统预测模型，具体为：
Xk＝AXk-1+wk-1 (1)
式中，Xk＝[σxk σyk]T，σxk和σyk为k时刻UWB系统x和y方向的坐标相对于k-1时刻UWB系统x和y方向的坐标增量；wk-1为过程噪声序列；设A为状态转移矩阵，ΔT为UWB系统的数据采样间隔，则有：
Figure FDA0002786365340000051
在UWB系统预测模型下，定义K时刻对i个基站的估计距离ri为：
Figure FDA0002786365340000052
所述UWB系统的量测方程构建模块用于建立量测模型，计算实际观测距离，构建UWB系统的量测方程，具体为：
Figure FDA0002786365340000053
式中：
Figure FDA0002786365340000054
为k时刻第i个基站和UWB系统间的量测距离；
Figure FDA0002786365340000055
分别为第i个基站的x、y方向坐标；vk,i为观测噪声序列；
所述UWB质量判断模块用于根据估计距离及UWB系统的量测方程判断UWB质量，具体为：
定义UWB系统与第i个基站估计距离于系统实际观测距离差值为Mi，在高斯分布中，对该Mi值进行概率密度估计，将Mi作为检测UWB系统对任意一个基站测距质量的判断依据，具体计算公式为：
Figure FDA0002786365340000056
所述激光雷达SLAM的量测方程构造模块用于构造激光雷达SLAM的量测方程，激光雷达SLAM的量测方程具体为：
Figure FDA0002786365340000057
式中：vk,σd、vk,θ为观测噪声序列；σrk为k时刻激光雷达相对于k-1时刻位置变换的真实距离，且有：
Figure FDA0002786365340000058
所述双模型定位模型构建模块用于根据估计距离、UWB系统的量测方程及激光雷达SLAM的量测方程，构建双模型定位模型，具体为：
根据式(3)、式(4)、式(6)和式(7)，得到双模型定位模型的过程具体为：
Figure FDA0002786365340000059
Figure FDA0002786365340000061
式中：
Figure FDA0002786365340000062
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的量测信息；
Figure FDA0002786365340000063
为k时刻UWB/激光雷达SLAM组合系统相对于相对于k-1时刻的旋转角度；
Figure FDA0002786365340000064
为UWB/激光雷达SLAM组合系统k-1时刻的航向角；
Figure FDA0002786365340000065
Figure FDA0002786365340000066
为观测噪声序列；
对式(4)、式(8)、式(9)进行线性化，得到：
Figure FDA0002786365340000067
Figure FDA0002786365340000068
Figure FDA0002786365340000069
式中：
Figure FDA00027863653400000610
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的观测距离；
Figure FDA00027863653400000611
为k时刻第i个基站和UWB/激光雷达SLAM组合系统间的概略距离；
Figure FDA00027863653400000612
Figure FDA00027863653400000613
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻距离变换的观测值；
Figure FDA00027863653400000614
为k-1时刻UWB/激光雷达SLAM组合系统距离变换的观测值；
Figure FDA00027863653400000615
Figure FDA00027863653400000616
为k时刻UWB/激光雷达SLAM组合系统相对于k-1时刻旋转的观测角度；
Figure FDA00027863653400000617
为k-1时刻UWB/激光雷达SLAM组合系统旋转的观测角度；tk-1＝1+[(yk-1-yk)/(xk-1-xk)]2；εk,i、
Figure FDA00027863653400000618
和
Figure FDA00027863653400000619
为线性化噪声；因此得到双模型定位模型：
Yk＝HkXk+vk (13)
式中：
Figure FDA00027863653400000620
Yk为观测向量，具体为：
Figure FDA00027863653400000621
Hk为量测矩阵，具体为：
Figure FDA0002786365340000071
所述双模型定位模型状态更新模块用于采用EKF算法对双模型定位模型进行融合测距，对双模型定位模型进行状态更新，提高双模型定位模型定位结果，具体为：
采用EKF算法对双模型定位模型进行融合测距，递推公式为：
Figure FDA0002786365340000072
Figure FDA0002786365340000073
Figure FDA0002786365340000074
Figure FDA0002786365340000075
Pk＝[I-KkHh]Pk/k-1 (21)
式中，
Figure FDA0002786365340000076
为预测值；
Figure FDA0002786365340000077
为状态的先验预测值；Pk为误差协方差阵；Pk/k-1为误差协方差阵的先验估计；Qk为系统的噪声方差阵；Kk为增益矩阵；Rk为量测噪声方差阵；f(·)和h(·)分别为系统状态和量测的线性化函数；通过初值
Figure FDA0002786365340000078
和
Figure FDA0002786365340000079
由式(17)、式(18)对UWB/激光雷达SLAM组合系统进行预测更新，由式(19)～式(21)对UWB/激光雷达SLAM组合系统进行状态更新；
此时，根据k时刻的量测消息Yk，即递推得到状态估计
Figure FDA00027863653400000710
然后通过状态估计
Figure FDA00027863653400000711
修正更新激光雷达的位置信息和姿态信息，提高双模型定位模型定位结果。
10.根据权利要求9所述的一种基于高斯分布检测UWB质量的双模型定位系统，其特征在于，所述服务器的主体为存储器。
CN202011299400.9A 2020-11-18 2020-11-18 一种基于高斯分布检测uwb质量的双模型定位方法及系统 Active CN112540345B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011299400.9A CN112540345B (zh) 2020-11-18 2020-11-18 一种基于高斯分布检测uwb质量的双模型定位方法及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011299400.9A CN112540345B (zh) 2020-11-18 2020-11-18 一种基于高斯分布检测uwb质量的双模型定位方法及系统

Publications (2)

Publication Number Publication Date
CN112540345A true CN112540345A (zh) 2021-03-23
CN112540345B CN112540345B (zh) 2023-08-01

Family

ID=75014842

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011299400.9A Active CN112540345B (zh) 2020-11-18 2020-11-18 一种基于高斯分布检测uwb质量的双模型定位方法及系统

Country Status (1)

Country Link
CN (1) CN112540345B (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113538410A (zh) * 2021-08-06 2021-10-22 广东工业大学 一种基于3d激光雷达和uwb的室内slam建图方法
CN115406439A (zh) * 2022-08-16 2022-11-29 中国第一汽车股份有限公司 车辆定位方法、系统、装置及非易失性存储介质

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643720A (zh) * 2016-09-28 2017-05-10 深圳市普渡科技有限公司 一种基于uwb室内定位技术与激光雷达的地图构建方法
CN107765220A (zh) * 2017-09-20 2018-03-06 武汉木神机器人有限责任公司 基于uwb和激光雷达混合定位的行人跟随系统及方法
US20180343397A1 (en) * 2017-05-26 2018-11-29 Zoptic, Llc Systems and methods for position tracking
CN109275093A (zh) * 2018-10-08 2019-01-25 高子庆 基于uwb定位与激光地图匹配的定位方法和移动终端
CN109490825A (zh) * 2018-11-20 2019-03-19 武汉万集信息技术有限公司 定位导航方法、装置、设备、系统及存储介质
CN110514225A (zh) * 2019-08-29 2019-11-29 中国矿业大学 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN110849367A (zh) * 2019-10-08 2020-02-28 杭州电子科技大学 基于融合uwb的视觉slam的室内定位与导航的方法
KR20200082234A (ko) * 2018-12-28 2020-07-08 한서대학교 산학협력단 드론 실내 비행 시스템 및 방법

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106643720A (zh) * 2016-09-28 2017-05-10 深圳市普渡科技有限公司 一种基于uwb室内定位技术与激光雷达的地图构建方法
US20180343397A1 (en) * 2017-05-26 2018-11-29 Zoptic, Llc Systems and methods for position tracking
CN107765220A (zh) * 2017-09-20 2018-03-06 武汉木神机器人有限责任公司 基于uwb和激光雷达混合定位的行人跟随系统及方法
CN109275093A (zh) * 2018-10-08 2019-01-25 高子庆 基于uwb定位与激光地图匹配的定位方法和移动终端
CN109490825A (zh) * 2018-11-20 2019-03-19 武汉万集信息技术有限公司 定位导航方法、装置、设备、系统及存储介质
KR20200082234A (ko) * 2018-12-28 2020-07-08 한서대학교 산학협력단 드론 실내 비행 시스템 및 방법
CN110514225A (zh) * 2019-08-29 2019-11-29 中国矿业大学 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN110849367A (zh) * 2019-10-08 2020-02-28 杭州电子科技大学 基于融合uwb的视觉slam的室内定位与导航的方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
YANG SONG等: "UWB/LiDAR Fusion For Cooperative Range-Only SLAM UWB lidar SLAM kalman", 《2019 INTERNATIONAL CONFERENCE ON ROBOTICS AND AUTOMATION (ICRA)》, pages 6568 - 6574 *
杨东辉等: "超宽带与激光雷达组合的室内定位方法", 《测绘科学》, vol. 44, no. 6, pages 72 - 78 *
陈志键等: "室内UWB/LiDAR组合定位算法", 《导航定位学报》, vol. 7, no. 1, pages 38 - 43 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113538410A (zh) * 2021-08-06 2021-10-22 广东工业大学 一种基于3d激光雷达和uwb的室内slam建图方法
CN115406439A (zh) * 2022-08-16 2022-11-29 中国第一汽车股份有限公司 车辆定位方法、系统、装置及非易失性存储介质

Also Published As

Publication number Publication date
CN112540345B (zh) 2023-08-01

Similar Documents

Publication Publication Date Title
Yang et al. A novel NLOS error compensation method based IMU for UWB indoor positioning system
CN109916410B (zh) 一种基于改进平方根无迹卡尔曼滤波的室内定位方法
EP3430419B1 (en) Estimating locations of mobile devices in a wireless tracking system
Jourdan et al. Monte Carlo localization in dense multipath environments using UWB ranging
AU2010304861B2 (en) Improvements in or relating to radio navigation
CN113074739B (zh) 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法
US10852386B2 (en) Method for calibrating a local positioning system based on time-difference-of-arrival measurements
CN109141427B (zh) 在非视距环境下基于距离和角度概率模型的ekf定位方法
CN108882149B (zh) 距离相关概率的nlos补偿定位方法
CN109059907A (zh) 轨迹数据处理方法、装置、计算机设备和存储介质
CN112540345B (zh) 一种基于高斯分布检测uwb质量的双模型定位方法及系统
CN112923919B (zh) 基于图优化的行人定位方法及系统
Ciurana et al. WLAN indoor positioning based on TOA with two reference points
CN114739400A (zh) 一种基于uwb和imu信息融合的室内定位方法
Liu et al. Tightly coupled integration of GNSS/UWB/VIO for reliable and seamless positioning
CN116182873B (zh) 室内定位方法、系统及计算机可读介质
KR102583899B1 (ko) 보행자 실내 위치 측위 장치 및 방법
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Glanzer Personal and first-responder positioning: State of the art and future trends
Jung et al. Scalable and Modular Ultra-Wideband Aided Inertial Navigation
CN114705223A (zh) 多移动智能体在目标跟踪中的惯导误差补偿方法及系统
CN114222365A (zh) 一种基于单基站的超宽带定位方法、装置、计算单元、系统
Carniglia et al. Investigation of sensor bias and signal quality on target tracking with multiple radars
Hu et al. Tightly Coupled Visual-Inertial-UWB Indoor Localization System with Multiple Position-Unknown Anchors
Chi et al. Robust UWB Localization for Indoor Pedestrian Tracking Using EKF and Adaptive Power-Driven Parallel IMM

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