CN113074732A - 一种室内外无缝定位系统及其定位方法 - Google Patents

一种室内外无缝定位系统及其定位方法 Download PDF

Info

Publication number
CN113074732A
CN113074732A CN202110303718.8A CN202110303718A CN113074732A CN 113074732 A CN113074732 A CN 113074732A CN 202110303718 A CN202110303718 A CN 202110303718A CN 113074732 A CN113074732 A CN 113074732A
Authority
CN
China
Prior art keywords
positioning
indoor
module
outdoor
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.)
Pending
Application number
CN202110303718.8A
Other languages
English (en)
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.)
Southeast University
Original Assignee
Southeast 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 Southeast University filed Critical Southeast University
Priority to CN202110303718.8A priority Critical patent/CN113074732A/zh
Publication of CN113074732A publication Critical patent/CN113074732A/zh
Pending legal-status Critical Current

Links

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/20—Instruments for performing navigational calculations
    • G01C21/206—Instruments for performing navigational calculations specially adapted for indoor navigation

Abstract

本发明公开了一种室内外无缝定位系统，包括室外定位模块、室内定位模块、室内外定位融合切换模块以及定位结果显示模块；室外定位模块的信号输出端、室内定位模块的信号输出端与室内外定位融合切换模块的信号输入端连接，室内外定位融合切换模块的信号输出端与定位结果显示模块的信号输入端连接。本发明有效将室内室外两种定位方式结合了起来。

Description

一种室内外无缝定位系统及其定位方法
技术领域
本发明属于定位技术领域，具体涉及一种室内外无缝定位系统及其定位方法。
背景技术
随着通信和导航定位技术的迅速发展，人们对基于位置信息的服务LBS(LocationBased Services)的需求也不断提高，在未来的LBS中必定要求定位服务不会随着位置的变化而中断，这就要求对用户的定位服务不会因该室内外的位置变化而出现定位失败的问题，需要实现两种定位环境的无缝切换。室外环境下，全球定位系统、北斗卫星导航系统等全球导航卫星系统能够为终端用户提供高精度的位置信息，但是在室内环境下，由于受墙体的阻挡影像，卫星信号会被严重衰减而无法用于室内的定位和导航。
针对室内环境的定位问题，当前具有代表性的室内定位技术主要包括：射频识别定位、蓝牙定位、Wi-Fi定位、磁场定位、超宽带定位、激光定位、LED可见光定位、音频定位和视觉定位等，但是仍然有许多问题需要解决，比如：系统定位复杂度高、实现成本高、精度差等。同时，由于室外环境的定位是基于卫星导航系统的绝对位置信息的定位方式，而室内环境的定位是基于定位系统构建的坐标系下的相对位置信息的定位方式，因此如何有效的将两种结合起来仍是目前工程应用的难点。
发明内容
为解决上述问题，本发明公开了一种室内外无缝定位系统及其定位方法，有效将室内室外两种定位方式结合了起来。
具体方案如下：
一种室内外无缝定位系统，其特征在于，包括室外定位模块、室内定位模块、室内外定位融合切换模块以及定位结果显示模块；室外定位模块的信号输出端、室内定位模块的信号输出端与室内外定位融合切换模块的信号输入端连接，室内外定位融合切换模块的信号输出端与定位结果显示模块的信号输入端连接；
所述室外定位模块包括GNSS定位模块、INS定位模块和室外定位融合模块，所述GNSS定位模块用于接收卫星定位观测数据，并将GNSS信号定位数据输出给室外定位融合模块处理；所述INS定位模块用于采集惯性测量数据，并将惯性测量数据输出给室外定位融合模块处理；所述室外定位融合模块用于通过采用基于卡尔曼滤波器的方式，建立基于GNSS信号定位数据与惯性测量数据的融合模型，将得到的室外定位数据输出给室内外定位融合切换模块处理；
所述室内定位模块包括激光雷达定位模块、UWB定位模块和室内定位融合模块，所述激光雷达定位模块用于绘制室内点云数据，并将估算的激光雷达定位数据输出给室内定位融合模块处理；所述UWB定位模块通过测距信息估计定位数据，并将估算的UWB定位数据输出给室内定位融合模块处理；所述室内定位融合模块用于通过采用基于卡尔曼滤波器的方式，建立基于激光雷达定位数据与UWB定位数据的融合模型，将得测量到的室内定位数据输出给室内外定位融合切换模块处理；
所述室内外定位融合切换模块通过对GNSS定位模块接收到的卫星数量进行判断，判断出待定位目标所在的位置区域；
所述定位结果显示模块通过加载的室外电子地图和构建的室内三维地图实现待定位目标的定位信息的呈现。
作为本发明的进一步改进，所述的一种室内外无缝定位系统的定位方法，其特征在于，包括以下步骤：
步骤1：所述室内外定位融合切换模块通过对GNSS定位模块接收到的卫星数量进行判断，判断出待定位目标所在的位置区域，对所述位置区域进行区域识别，得到区域识别结果，当GNSS定位模块接收到的卫星数量大于4颗时，所述区域识别结果为室外区域，所述待定位目标的定位模式为室外定位模式，并转入步骤2；当GNSS定位模块接收到的卫星数量不大于4颗时，所述区域识别结果为室内区域，所述待定位目标的定位模式为室内定位模式，并转入步骤3；
步骤2：所述GNSS定位模块接收卫星定位观测数据，输出GNSS信号定位数据给室外定位融合模块处理；所述INS定位模块采集惯性测量数据，输出惯性测量数据给室外定位融合模块处理；所述室外定位融合模块通过采用基于卡尔曼滤波器的方式，建立基于GNSS信号定位数据与惯性测量数据的融合模型，输出室外定位数据，并转入步骤4；
步骤3：所述激光雷达定位模块绘制室内点云数据，并将估算的激光雷达定位数据输出给室内定位融合模块处理；所述UWB定位模块通过测量UWB基站到待定位目标的距离解算出待定位目标的定位数据，将估算的UWB定位数据输出给室内定位融合模块处理；所述室内定位融合模块用于通过采用基于粒子滤波器的方式，建立基于激光雷达定位数据与UWB定位数据的融合模型，输出室内定位数据，并转入步骤4；
步骤4：所述室内外定位融合切换模块通过室内外绝对位置解算算法实现室外和室内定位方式的融合、切换和集成，完成室内外的平滑过渡；
步骤5：所述定位结果显示模块根据所述区域识别结果显示待定位目标的定位信息，当所述区域识别结果为室外区域时，通过加载的室外电子地图实时显示室内外定位融合切换模块输出的待定位目标的定位信息；当所述区域识别结果为室内区域时，通过构建的室内三维地图实时显示室内外定位融合切换模块输出的待定位目标的定位信息；返回步骤1。
作为本发明的进一步改进，所述的一种室内外无缝定位方法，其特征在于，步骤2中所述的基于GNSS信号定位数据与惯性测量数据的融合模型，为GNSS/INS紧组合导航定位算法，所述融合模型的状态方程由GNSS定位模块的时钟误差和INS定位模块系统误差共同组成，可以表示为：
Figure BDA0002987287380000031
所述INS定位模块的状态向量：
Figure BDA0002987287380000032
过程噪声向量：
WSINS(t)＝[wgx，wgy，wgz，wax，way，waz]T
式中，
Figure BDA0002987287380000033
为三个平台误差角，δVE、δVN、δVU分别为东、北、天三个方向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差，∈gx、εgy、εgz分别为沿b系三个轴向的陀螺常值零偏，
Figure BDA0002987287380000034
分别为沿b系三个轴向的加速度计常值零偏，wgi、wai(i＝x，y，z)分别为载体体系三个轴向的陀螺零均值白噪声和加速度计零均值白噪声；
所述GNSS定位模块的系统矩阵：
Figure BDA0002987287380000041
过程噪声矩阵：
Figure BDA0002987287380000042
式中，
Figure BDA0002987287380000043
Figure BDA0002987287380000044
Figure BDA0002987287380000045
Figure BDA0002987287380000046
Figure BDA0002987287380000047
Figure BDA0002987287380000048
Figure BDA0002987287380000051
式中，ωie为地球自转角速度，L为纬度，λ为经度，h为高度，RM和RN别为地球子午圈曲率半径和卯酉圈曲率半径，VE、VN、VU分别为东、北、天三个方向的速度，fi、
Figure BDA0002987287380000052
分别表示比力和加速度计零偏在地理坐标系中东北天向的分量，θ、γ、
Figure BDA0002987287380000053
分别为载体的俯仰角、横滚角和航向角；
所述GNSS定位模块的状态向量：
XGNSS(t)＝[δtu，δtru]T
过程噪声向量：
WGNSS(t)＝[wtu，wtru]T
式中，δtu为钟差等效距离误差，δtru为钟漂等效距离变化率误差，wtu、wtru分别为钟差等效距离和钟漂等效距离变化率对应的白噪声；
所述GNSS定位模块的系统矩阵：
Figure BDA0002987287380000054
过程噪声矩阵：
Figure BDA0002987287380000055
式中，βtru为一阶马尔可夫过程的反相关时间；
所述融合模型的卡尔曼滤波器的观测采用基于伪距、伪距率紧耦合组合的量测方程：
Figure BDA0002987287380000056
式中：伪距量测矩阵Hp(t)＝[0n×6，Hp1，0n×6，Hp2]n×17，伪距率量测矩阵
Figure BDA0002987287380000057
Vp(t)、
Figure BDA0002987287380000058
分别为伪距和伪距率测量噪声；
其中：
Figure BDA0002987287380000061
Figure BDA0002987287380000062
式中：
Figure BDA0002987287380000063
为GNSS的某颗卫星j在地心地固坐标系上的位置，
Figure BDA0002987287380000064
为卫星j到GNSS接收机的真实距离，e为地球椭球第一偏心率。
作为本发明的进一步改进，所述的一种室内外无缝定位方法，其特征在于，步骤3中所述的基于激光雷达定位数据与UWB定位数据的融合模型，为基于粒子滤波的UWB和激光雷达融合定位误差模型；
状态方程：
Xt＝f(Xt-1，Wt-1)
量测方程：
Zt＝g(Xt，Vt)
式中：Xt＝[xt，yt，θt]T为状态信息向量，xt和yt分别为所述室内定位模块在t时刻的横纵坐标，
Figure BDA0002987287380000065
为t时刻的航向角增量，γt-1为t-1时刻的航向角，
Figure BDA0002987287380000066
为量测信息输入向量，
Figure BDA0002987287380000067
为所述UWB定位模块在t时刻横纵坐标的量测值，
Figure BDA0002987287380000068
为所述激光雷达定位模块在t时刻横纵坐标和航向角增量的量测值。
作为本发明的进一步改进，所述的一种室内外无缝定位方法，其特征在于，步骤3中所述的基于激光雷达定位数据与UWB定位数据的融合模型，包括以下步骤：
步骤31：根据所述UWB横纵坐标的量测值输入估算位置：
Figure BDA0002987287380000071
Figure BDA0002987287380000072
Figure BDA0002987287380000073
式中，
Figure BDA0002987287380000074
为所述UWB定位模块在t时刻横纵坐标的量测值，ε为所述UWB定位模块最大测量误差，
Figure BDA0002987287380000075
为生成第i个粒子的横纵坐标，N1为随机粒子样本数；
步骤32：增加粒子的控制输入(速度、角速度等)，预测所有粒子的下一步位置：
Figure BDA0002987287380000076
Figure BDA0002987287380000077
式中，xt、yt为当前粒子状态预测的横纵坐标，xt-1、yt-1为上一时刻的横纵坐标，ΔT为系统运行时间增量，vt为当前时刻所述待定位目标速度，γt-1为上一时刻所述待定位目标航向角，θt为当前时刻所述待定位目标航向角增量，
Figure BDA0002987287380000078
为系统状态噪声，由所述待定位目标横纵向标准偏差确定；
步骤33：计算由UWB观测生成的每个粒子的位置和激光雷达量测到的所述待定位目标位置之间的几何距离，更新所有粒子的权值：
Figure BDA0002987287380000079
Figure BDA00029872873800000710
式中，
Figure BDA00029872873800000711
为第i个粒子的横纵坐标集合，
Figure BDA00029872873800000712
为t+1时刻的所述激光雷达定位模块横纵坐标量测集合，R为所述室内定位模块测量误差，
Figure BDA00029872873800000713
为归一化后的权值；
步骤34：根据粒子的权重，对所有粒子进行重采样：
Figure BDA0002987287380000081
式中，
Figure BDA0002987287380000082
表示i粒子在t时刻的状态函数；
步骤35：回到步骤32，进入下一次循环，在此过程中，遇到所述定位目标状态突变，以当前所述UWB定位模块的定位结果作为初始位置回到步骤31，重新搜索所述定位目标位置。
作为本发明的进一步改进，所述的一种室内外无缝定位方法，其特征在于，步骤4中所述的室内外绝对位置解算算法分为由室外到室内的绝对位置信息实时解算算法和室内到室外的绝对位置信息后解算算法；
所述室外到室内的绝对位置信息实时解算算法：所述室内外定位融合切换模块将定位模式由室外定位模式切换为室内定位模式时，所述室内外定位融合切换模块将切换前所述室外定位模块输出的绝对位置坐标和绝对航向信息传递给所述室内定位模块，作为所述室内定位模块的初始值；
所述室内到室外的绝对位置信息后解算算法：所述室内外定位融合切换模块检测到定位模式由室内定位模式切换到室外定位模式时，将所述室内定位模块延时关闭，测量过渡区域在所述室内定位模块下输出的室内定位数据和所述室外定位模块下输出的室外定位数据，解算出位置变化的旋转和平移矩阵，从而推算出室内绝对航向和绝对位置。
作为本发明的进一步改进，所述的一种室内外无缝定位方法，其特征在于所述UWB定位数据为：
Figure BDA0002987287380000083
A＝[xi+1-x1，yi+1-y1，zi+1-z1]，i＝1：N
Figure BDA0002987287380000084
式中，
Figure BDA0002987287380000085
为所述UWB定位模块在t时刻横纵坐标和高程的量测值，xu＝[xi，yi，zi]为第i个UWB基站的坐标，
Figure BDA0002987287380000086
本发明的有益效果是：
与现有技术相比，本发明有效的提高用户在不同环境下的定位结果。通过GNSS定位模块接收到的卫星数量的方法来判断用户处于的环境，可以更加精准地控制模式之间的切换。建立基于激光雷达定位数据与UWB定位数据的融合模型来进行室内定位可以有效提高室内定位的精度。
具体实施方式
下面结合具体实施方式，进一步阐明本发明，应理解下述具体实施方式仅用于说明本发明而不用于限制本发明的范围。
本发明一种室内外无缝定位系统，包括室外定位模块、室内定位模块、室内外定位融合切换模块以及定位结果显示模块；室外定位模块的信号输出端、室内定位模块的信号输出端与室内外定位融合切换模块的信号输入端连接，室内外定位融合切换模块的信号输出端与定位结果显示模块的信号输入端连接；
所述室外定位模块包括GNSS定位模块、INS定位模块和室外定位融合模块，所述GNSS定位模块用于接收卫星定位观测数据，并将GNSS信号定位数据输出给室外定位融合模块处理；所述INS定位模块用于采集惯性测量数据，并将惯性测量数据输出给室外定位融合模块处理；所述室外定位融合模块用于通过采用基于卡尔曼滤波器的方式，建立基于GNSS信号定位数据与惯性测量数据的融合模型，将得到的室外定位数据输出给室内外定位融合切换模块处理；
所述室内定位模块包括激光雷达定位模块、UWB定位模块和室内定位融合模块，所述激光雷达定位模块用于绘制室内点云数据，并将估算的激光雷达定位数据输出给室内定位融合模块处理；所述UWB定位模块通过测距信息估计定位数据，并将估算的UWB定位数据输出给室内定位融合模块处理；所述室内定位融合模块用于通过采用基于卡尔曼滤波器的方式，建立基于激光雷达定位数据与UWB定位数据的融合模型，将得测量到的室内定位数据输出给室内外定位融合切换模块处理；
所述室内外定位融合切换模块通过对GNSS定位模块接收到的卫星数量进行判断，判断出待定位目标所在的位置区域；
所述定位结果显示模块通过加载的室外电子地图和构建的室内三维地图实现待定位目标的定位信息的呈现。
在本实施例中，所述的一种室内外无缝定位系统的定位方法，其特征在于，包括以下步骤：
步骤1：所述室内外定位融合切换模块通过对GNSS定位模块接收到的卫星数量进行判断，判断出待定位目标所在的位置区域，对所述位置区域进行区域识别，得到区域识别结果，当GNSS定位模块接收到的卫星数量大于4颗时，所述区域识别结果为室外区域，所述待定位目标的定位模式为室外定位模式，并转入步骤2；当GNSS定位模块接收到的卫星数量不大于4颗时，所述区域识别结果为室内区域，所述待定位目标的定位模式为室内定位模式，并转入步骤3；
步骤2：所述GNSS定位模块接收卫星定位观测数据，输出GNSS信号定位数据给室外定位融合模块处理；所述INS定位模块采集惯性测量数据，输出惯性测量数据给室外定位融合模块处理；所述室外定位融合模块通过采用基于卡尔曼滤波器的方式，建立基于GNSS信号定位数据与惯性测量数据的融合模型，输出室外定位数据，并转入步骤4；
步骤3：所述激光雷达定位模块绘制室内点云数据，并将估算的激光雷达定位数据输出给室内定位融合模块处理；所述UWB定位模块通过测量UWB基站到待定位目标的距离解算出待定位目标的定位数据，将估算的UWB定位数据输出给室内定位融合模块处理；所述室内定位融合模块用于通过采用基于粒子滤波器的方式，建立基于激光雷达定位数据与UWB定位数据的融合模型，输出室内定位数据，并转入步骤4；
步骤4：所述室内外定位融合切换模块通过室内外绝对位置解算算法实现室外和室内定位方式的融合、切换和集成，完成室内外的平滑过渡；
步骤5：所述定位结果显示模块根据所述区域识别结果显示待定位目标的定位信息，当所述区域识别结果为室外区域时，通过加载的室外电子地图实时显示室内外定位融合切换模块输出的待定位目标的定位信息；当所述区域识别结果为室内区域时，通过构建的室内三维地图实时显示室内外定位融合切换模块输出的待定位目标的定位信息；返回步骤1。
在本实施例中，所述的一种室内外无缝定位方法，其特征在于，步骤2中所述的基于GNSS信号定位数据与惯性测量数据的融合模型，为GNSS/INS紧组合导航定位算法，所述融合模型的状态方程由GNSS定位模块的时钟误差和INS定位模块系统误差共同组成，可以表示为：
Figure BDA0002987287380000111
所述INS定位模块的状态向量：
Figure BDA0002987287380000112
过程噪声向量：
WsINs(t)＝[wgx，wgy，wgz，wax，way，waz]T
式中，
Figure BDA0002987287380000113
为三个平台误差角，δVE、δVN、δVU分别为东、北、天三个方向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差，∈gx、εgy、εgz分别为沿b系三个轴向的陀螺常值零偏，
Figure BDA0002987287380000114
分别为沿b系三个轴向的加速度计常值零偏，wgi、wai(i＝x，y，z)分别为载体体系三个轴向的陀螺零均值白噪声和加速度计零均值白噪声；
所述GNSS定位模块的系统矩阵：
Figure BDA0002987287380000115
过程噪声矩阵：
Figure BDA0002987287380000116
式中，
Figure BDA0002987287380000117
Figure BDA0002987287380000121
Figure BDA0002987287380000122
Figure BDA0002987287380000123
Figure BDA0002987287380000124
Figure BDA0002987287380000125
Figure BDA0002987287380000126
式中，ωie为地球自转角速度，L为纬度，λ为经度，h为高度，RM和RN别为地球子午圈曲率半径和卯酉圈曲率半径，VE、VN、VU分别为东、北、天三个方向的速度，fi、
Figure BDA0002987287380000127
分别表示比力和加速度计零偏在地理坐标系中东北天向的分量，θ、γ、
Figure BDA0002987287380000128
分别为载体的俯仰角、横滚角和航向角；
所述GNSS定位模块的状态向量：
XGNSS(t)＝[δtu，δtru]T
过程噪声向量：
WGNSS(t)＝[wtu，wtru]T
式中，δtu为钟差等效距离误差，δtru为钟漂等效距离变化率误差，wtu、wtru分别为钟差等效距离和钟漂等效距离变化率对应的白噪声；
所述GNSS定位模块的系统矩阵：
Figure BDA0002987287380000131
过程噪声矩阵：
Figure BDA0002987287380000132
式中，βtru为一阶马尔可夫过程的反相关时间；
所述融合模型的卡尔曼滤波器的观测采用基于伪距、伪距率紧耦合组合的量测方程：
Figure BDA0002987287380000133
式中：伪距量测矩阵Hp(t)＝[0n×6，Hp1，0n×6，Hp2]n×17，伪距率量测矩阵
Figure BDA0002987287380000134
Vp(t)、
Figure BDA0002987287380000135
分别为伪距和伪距率测量噪声；
其中：
Figure BDA0002987287380000136
Figure BDA0002987287380000137
式中：
Figure BDA0002987287380000138
为GNSS的某颗卫星j在地心地固坐标系上的位置，
Figure BDA0002987287380000139
为卫星j到GNSS接收机的真实距离，e为地球椭球第一偏心率。
在本实施例中，所述的一种室内外无缝定位方法，其特征在于，步骤3中所述的基于激光雷达定位数据与UWB定位数据的融合模型，为基于粒子滤波的UWB和激光雷达融合定位误差模型；
状态方程：
Xt＝f(Xt-1，Wt-1)
量测方程：
Zt＝g(Xt，Vt)
式中：Xt＝[xt，yt，θt]T为状态信息向量，xt和yt分别为所述室内定位模块在t时刻的横纵坐标，
Figure BDA0002987287380000141
为t时刻的航向角增量，γt-1为t-1时刻的航向角，
Figure BDA0002987287380000142
为量测信息输入向量，
Figure BDA0002987287380000143
为所述UWB定位模块在t时刻横纵坐标的量测值，
Figure BDA0002987287380000144
为所述激光雷达定位模块在t时刻横纵坐标和航向角增量的量测值。
在本实施例中，所述的一种室内外无缝定位方法，其特征在于，步骤3中所述的基于激光雷达定位数据与UWB定位数据的融合模型，包括以下步骤：
步骤31：根据所述UWB横纵坐标的量测值输入估算位置：
Figure BDA0002987287380000145
Figure BDA0002987287380000146
Figure BDA0002987287380000147
式中，
Figure BDA0002987287380000148
为所述UWB定位模块在t时刻横纵坐标的量测值，ε为所述UWB定位模块最大测量误差，
Figure BDA0002987287380000149
为生成第i个粒子的横纵坐标，N1为随机粒子样本数；
步骤32：增加粒子的控制输入(速度、角速度等)，预测所有粒子的下一步位置：
Figure BDA00029872873800001410
Figure BDA00029872873800001411
式中，xt、yt为当前粒子状态预测的横纵坐标，xt-1、yt-1为上一时刻的横纵坐标，ΔT为系统运行时间增量，vt为当前时刻所述待定位目标速度，γt-1为上一时刻所述待定位目标航向角，θt为当前时刻所述待定位目标航向角增量，
Figure BDA0002987287380000151
为系统状态噪声，由所述待定位目标横纵向标准偏差确定；
步骤33：计算由UWB观测生成的每个粒子的位置和激光雷达量测到的所述待定位目标位置之间的几何距离，更新所有粒子的权值：
Figure BDA0002987287380000152
Figure BDA0002987287380000153
式中，
Figure BDA0002987287380000154
为第i个粒子的横纵坐标集合，
Figure BDA0002987287380000155
为t+1时刻的所述激光雷达定位模块横纵坐标量测集合，R为所述室内定位模块测量误差，
Figure BDA0002987287380000156
为归一化后的权值；
步骤34：根据粒子的权重，对所有粒子进行重采样：
Figure BDA0002987287380000157
式中，
Figure BDA0002987287380000158
表示i粒子在t时刻的状态函数；
步骤35：回到步骤32，进入下一次循环，在此过程中，遇到所述定位目标状态突变，以当前所述UWB定位模块的定位结果作为初始位置回到步骤31，重新搜索所述定位目标位置。
在本实施例中，所述的一种室内外无缝定位方法，其特征在于，步骤4中所述的室内外绝对位置解算算法分为由室外到室内的绝对位置信息实时解算算法和室内到室外的绝对位置信息后解算算法；
所述室外到室内的绝对位置信息实时解算算法：所述室内外定位融合切换模块将定位模式由室外定位模式切换为室内定位模式时，所述室内外定位融合切换模块将切换前所述室外定位模块输出的绝对位置坐标和绝对航向信息传递给所述室内定位模块，作为所述室内定位模块的初始值；
所述室内到室外的绝对位置信息后解算算法：所述室内外定位融合切换模块检测到定位模式由室内定位模式切换到室外定位模式时，将所述室内定位模块延时关闭，测量过渡区域在所述室内定位模块下输出的室内定位数据和所述室外定位模块下输出的室外定位数据，解算出位置变化的旋转和平移矩阵，从而推算出室内绝对航向和绝对位置。
在本实施例中，所述的一种室内外无缝定位方法，其特征在于所述UWB定位数据为：
Figure BDA0002987287380000161
A＝[xi+1-x1，yi+1-y1，zi+1-z1]，i＝1：N
Figure BDA0002987287380000162
式中，
Figure BDA0002987287380000163
为所述UWB定位模块在t时刻横纵坐标和高程的量测值，xu＝[xi，yi，zi]为第i个UWB基站的坐标，
Figure BDA0002987287380000164

Claims (7)

1.一种室内外无缝定位系统，其特征在于，包括室外定位模块、室内定位模块、室内外定位融合切换模块以及定位结果显示模块；所述室外定位模块和室内定位模块的信号输出端均与室内外定位融合切换模块的信号输入端连接，所述室内外定位融合切换模块的信号输出端与定位结果显示模块的信号输入端连接；
所述室外定位模块包括GNSS定位模块、INS定位模块和室外定位融合模块，所述GNSS定位模块用于接收卫星定位观测数据，并将GNSS信号定位数据输出给室外定位融合模块处理；所述INS定位模块用于采集惯性测量数据，并将惯性测量数据输出给室外定位融合模块处理；所述室外定位融合模块用于通过采用基于卡尔曼滤波器的方式，建立基于GNSS信号定位数据与惯性测量数据的融合模型，将得到的室外定位数据输出给室内外定位融合切换模块处理；
所述室内定位模块包括激光雷达定位模块、UWB定位模块和室内定位融合模块，所述激光雷达定位模块用于绘制室内点云数据，并将估算的激光雷达定位数据输出给室内定位融合模块处理；所述UWB定位模块通过测距信息估计定位数据，并将估算的UWB定位数据输出给室内定位融合模块处理；所述室内定位融合模块用于通过采用基于卡尔曼滤波器的方式，建立基于激光雷达定位数据与UWB定位数据的融合模型，将得测量到的室内定位数据输出给室内外定位融合切换模块处理；
所述室内外定位融合切换模块通过对GNSS定位模块接收到的卫星数量进行判断，判断出待定位目标所在的位置区域；
所述定位结果显示模块通过加载的室外电子地图和构建的室内三维地图实现待定位目标的定位信息的呈现。
2.根据权利要求1所述的一种室内外无缝定位系统的定位方法，其特征在于,包括以下步骤：
步骤1：所述室内外定位融合切换模块通过对GNSS定位模块接收到的卫星数量进行判断，判断出待定位目标所在的位置区域，对所述位置区域进行区域识别，得到区域识别结果，当GNSS定位模块接收到的卫星数量大于4颗时，所述区域识别结果为室外区域，所述待定位目标的定位模式为室外定位模式，并转入步骤2；当GNSS定位模块接收到的卫星数量不大于4颗时，所述区域识别结果为室内区域，所述待定位目标的定位模式为室内定位模式，并转入步骤3；
步骤2：所述GNSS定位模块接收卫星定位观测数据，输出GNSS信号定位数据给室外定位融合模块处理；所述INS定位模块采集惯性测量数据，输出惯性测量数据给室外定位融合模块处理；所述室外定位融合模块通过采用基于卡尔曼滤波器的方式，建立基于GNSS信号定位数据与惯性测量数据的融合模型，输出室外定位数据，并转入步骤4；
步骤3：所述激光雷达定位模块绘制室内点云数据，并将估算的激光雷达定位数据输出给室内定位融合模块处理；所述UWB定位模块通过测量UWB基站到待定位目标的距离解算出待定位目标的定位数据，将估算的UWB定位数据输出给室内定位融合模块处理；所述室内定位融合模块用于通过采用基于粒子滤波器的方式，建立基于激光雷达定位数据与UWB定位数据的融合模型，输出室内定位数据，并转入步骤4；
步骤4：所述室内外定位融合切换模块通过室内外绝对位置解算算法实现室外和室内定位方式的融合、切换和集成，完成室内外的平滑过渡；
步骤5：所述定位结果显示模块根据所述区域识别结果显示待定位目标的定位信息，当所述区域识别结果为室外区域时，通过加载的室外电子地图实时显示室内外定位融合切换模块输出的待定位目标的定位信息；当所述区域识别结果为室内区域时，通过构建的室内三维地图实时显示室内外定位融合切换模块输出的待定位目标的定位信息；返回步骤1。
3.根据权利要求2所述的一种室内外无缝定位方法，其特征在于，步骤2中所述的基于GNSS信号定位数据与惯性测量数据的融合模型，为GNSS/INS紧组合导航定位算法，所述融合模型的状态方程由GNSS定位模块的时钟误差和INS定位模块系统误差共同组成，可以表示为：
Figure FDA0002987287370000021
所述INS定位模块的状态向量：
Figure FDA0002987287370000022
过程噪声向量：
WSINS(t)＝[wgx，wgy，wgz，wax，way，waz]T
式中，
Figure FDA0002987287370000031
为三个平台误差角，δVE、δVN、δVU分别为东、北、天三个方向的速度误差，δL、δλ、δh分别为纬度、经度和高度误差，∈gx、εgy、εgz分别为沿b系三个轴向的陀螺常值零偏，
Figure FDA0002987287370000032
分别为沿b系三个轴向的加速度计常值零偏，wgi、wai，其中i＝x，y，z；分别为载体体系三个轴向的陀螺零均值白噪声和加速度计零均值白噪声；
所述GNSS定位模块的系统矩阵：
Figure FDA0002987287370000033
过程噪声矩阵：
Figure FDA0002987287370000034
式中，
Figure FDA0002987287370000035
Figure FDA0002987287370000036
Figure FDA0002987287370000037
Figure FDA0002987287370000038
Figure FDA0002987287370000041
F31＝03×3，
Figure FDA0002987287370000042
Figure FDA0002987287370000043
式中，wie为地球自转角速度，L为纬度，λ为经度，h为高度，RM和RN别为地球子午圈曲率半径和卯酉圈曲率半径，VE、VN、VU分别为东、北、天三个方向的速度，fi、
Figure FDA0002987287370000044
其中，i＝E，N，U；分别表示比力和加速度计零偏在地理坐标系中东北天向的分量，θ、γ、
Figure FDA0002987287370000045
分别为载体的俯仰角、横滚角和航向角；
所述GNSS定位模块的状态向量：
XGNSS(t)＝[δtu，δtru]T
过程噪声向量：
WGNSS(t)＝[wtu，wtru]T
式中，δtu为钟差等效距离误差，δtru为钟漂等效距离变化率误差，wtu、ωtru分别为钟差等效距离和钟漂等效距离变化率对应的白噪声；
所述GNSS定位模块的系统矩阵：
Figure FDA0002987287370000046
过程噪声矩阵：
Figure FDA0002987287370000047
式中，βtru为一阶马尔可夫过程的反相关时间；
所述融合模型的卡尔曼滤波器的观测采用基于伪距、伪距率紧耦合组合的量测方程：
Figure FDA0002987287370000051
式中：伪距量测矩阵Hp(t)＝[0n×6，Hp1，0n×6，Hp2]n×17，伪距率量测矩阵
Figure FDA0002987287370000052
Vp(t)、
Figure FDA0002987287370000053
分别为伪距和伪距率测量噪声；
其中：
Figure FDA0002987287370000054
Hp2＝[1，0]4×2
Figure FDA0002987287370000055
式中：
Figure FDA0002987287370000056
为GNSS的某颗卫星j在地心地固坐标系上的位置，
Figure FDA0002987287370000057
为卫星j到GNSS接收机的真实距离，e为地球椭球第一偏心率。
4.根据权利要求2所述的一种室内外无缝定位方法，其特征在于，步骤3中所述的基于激光雷达定位数据与UWB定位数据的融合模型，为基于粒子滤波的UWB和激光雷达融合定位误差模型；
状态方程：
Xt＝f(Xt-1，Wt-1)
量测方程：
Zt＝g(Xt，Vt)
式中：Xt＝[xt，yt，θt]T为状态信息向量，xt和yt分别为所述室内定位模块在t时刻的横纵坐标，
Figure FDA0002987287370000058
为t时刻的航向角增量，γt-1为t-1时刻的航向角，
Figure FDA0002987287370000059
为量测信息输入向量，
Figure FDA00029872873700000510
为所述UWB定位模块在t时刻横纵坐标的量测值，
Figure FDA0002987287370000061
为所述激光雷达定位模块在t时刻横纵坐标和航向角增量的量测值。
5.根据权利要求2所述的一种室内外无缝定位方法，其特征在于，步骤3中所述的基于激光雷达定位数据与UWB定位数据的融合模型，包括以下步骤：
步骤31：根据所述UWB横纵坐标的量测值输入估算位置：
Figure FDA0002987287370000062
Figure FDA0002987287370000063
Figure FDA0002987287370000064
式中，
Figure FDA0002987287370000065
为所述UWB定位模块在t时刻横纵坐标的量测值，ε为所述UWB定位模块最大测量误差，
Figure FDA0002987287370000066
为生成第i个粒子的横纵坐标，N1为随机粒子样本数；
步骤32：增加粒子的控制输入，包括速度、角速度，预测所有粒子的下一步位置：
Figure FDA0002987287370000067
Figure FDA0002987287370000068
式中，xt、yt为当前粒子状态预测的横纵坐标，xt-1、yt-1为上一时刻的横纵坐标，ΔT为系统运行时间增量，vt为当前时刻所述待定位目标速度，γt-1为上一时刻所述待定位目标航向角，θt为当前时刻所述待定位目标航向角增量，
Figure FDA0002987287370000069
为系统状态噪声，由所述待定位目标横纵向标准偏差确定；
步骤33：计算由UWB观测生成的每个粒子的位置和激光雷达量测到的所述待定位目标位置之间的几何距离，更新所有粒子的权值：
Figure FDA00029872873700000610
Figure FDA00029872873700000611
式中，
Figure FDA0002987287370000071
为第i个粒子的横纵坐标集合，
Figure FDA0002987287370000072
为t+1时刻的所述激光雷达定位模块横纵坐标量测集合，R为所述室内定位模块测量误差，
Figure FDA0002987287370000073
为归一化后的权值；
步骤34：根据粒子的权重，对所有粒子进行重采样：
Figure FDA0002987287370000074
式中，
Figure FDA0002987287370000075
表示i粒子在t时刻的状态函数；
步骤35：回到步骤32，进入下一次循环，在此过程中，遇到所述定位目标状态突变，以当前所述UWB定位模块的定位结果作为初始位置回到步骤31，重新搜索所述定位目标位置。
6.根据权利要求2所述的一种室内外无缝定位方法，其特征在于，步骤4中所述的室内外绝对位置解算算法分为由室外到室内的绝对位置信息实时解算算法和室内到室外的绝对位置信息后解算算法；
所述室外到室内的绝对位置信息实时解算算法：所述室内外定位融合切换模块将定位模式由室外定位模式切换为室内定位模式时，所述室内外定位融合切换模块将切换前所述室外定位模块输出的绝对位置坐标和绝对航向信息传递给所述室内定位模块，作为所述室内定位模块的初始值；
所述室内到室外的绝对位置信息后解算算法：所述室内外定位融合切换模块检测到定位模式由室内定位模式切换到室外定位模式时，将所述室内定位模块延时关闭，测量过渡区域在所述室内定位模块下输出的室内定位数据和所述室外定位模块下输出的室外定位数据，解算出位置变化的旋转和平移矩阵，从而推算出室内绝对航向和绝对位置。
7.根据权利要求5所述的一种室内外无缝定位方法，其特征在于，所述UWB定位数据为：
Figure FDA0002987287370000076
A＝[xi+1-x1，yi+1-y1，zi+1-z1]，i＝l:N
Figure FDA0002987287370000077
式中，
Figure FDA0002987287370000081
为所述UWB定位模块在t时刻横纵坐标和高程的量测值，xu＝[xi，yi，zi]为第i个UWB基站的坐标，
Figure FDA0002987287370000082
CN202110303718.8A 2021-03-22 2021-03-22 一种室内外无缝定位系统及其定位方法 Pending CN113074732A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110303718.8A CN113074732A (zh) 2021-03-22 2021-03-22 一种室内外无缝定位系统及其定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110303718.8A CN113074732A (zh) 2021-03-22 2021-03-22 一种室内外无缝定位系统及其定位方法

Publications (1)

Publication Number Publication Date
CN113074732A true CN113074732A (zh) 2021-07-06

Family

ID=76613520

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110303718.8A Pending CN113074732A (zh) 2021-03-22 2021-03-22 一种室内外无缝定位系统及其定位方法

Country Status (1)

Country Link
CN (1) CN113074732A (zh)

Cited By (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113534226A (zh) * 2021-07-30 2021-10-22 东南大学 一种基于智能手机场景识别的室内外无缝定位算法
CN113865588A (zh) * 2021-08-24 2021-12-31 上海交通大学 一种机器人定位方法和装置
CN113899370A (zh) * 2021-09-26 2022-01-07 东南大学 一种基于gnss_uwb的多传感器组合室内外无缝定位方法
CN114222240A (zh) * 2021-10-29 2022-03-22 中国石油大学(华东) 一种基于粒子滤波的多源融合定位方法
CN114485658A (zh) * 2021-12-08 2022-05-13 上海智能网联汽车技术中心有限公司 一种面向路侧感知系统精度评估的装置及方法
CN114526737A (zh) * 2022-01-17 2022-05-24 太原理工大学 一种基于gnss/uwb/dba融合的室内外无缝切换定位方法
CN116086448A (zh) * 2023-04-12 2023-05-09 成都信息工程大学 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103856989A (zh) * 2012-11-28 2014-06-11 中国电信股份有限公司 室内外定位切换的方法和系统、终端以及定位应用平台
CN104849740A (zh) * 2015-05-26 2015-08-19 福州大学 集成卫星导航与蓝牙技术的室内外无缝定位系统及其方法
CN105022077A (zh) * 2015-01-20 2015-11-04 泰斗微电子科技有限公司 一种室内外混合定位的方法及系统
CN209623718U (zh) * 2019-01-25 2019-11-12 湖南超能机器人技术有限公司 巡逻机器人定位系统
CN110645979A (zh) * 2019-09-27 2020-01-03 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103856989A (zh) * 2012-11-28 2014-06-11 中国电信股份有限公司 室内外定位切换的方法和系统、终端以及定位应用平台
CN105022077A (zh) * 2015-01-20 2015-11-04 泰斗微电子科技有限公司 一种室内外混合定位的方法及系统
CN104849740A (zh) * 2015-05-26 2015-08-19 福州大学 集成卫星导航与蓝牙技术的室内外无缝定位系统及其方法
CN209623718U (zh) * 2019-01-25 2019-11-12 湖南超能机器人技术有限公司 巡逻机器人定位系统
CN110645979A (zh) * 2019-09-27 2020-01-03 北京交通大学 基于gnss/ins/uwb组合的室内外无缝定位方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
李中道等: "室内环境下UWB与LiDAR融合定位算法研究", 《计算机工程与应用》, pages 260 - 266 *

Cited By (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113534226A (zh) * 2021-07-30 2021-10-22 东南大学 一种基于智能手机场景识别的室内外无缝定位算法
CN113534226B (zh) * 2021-07-30 2024-04-02 东南大学 一种基于智能手机场景识别的室内外无缝定位算法
CN113865588A (zh) * 2021-08-24 2021-12-31 上海交通大学 一种机器人定位方法和装置
CN113865588B (zh) * 2021-08-24 2024-03-26 上海交通大学 一种机器人定位方法和装置
CN113899370A (zh) * 2021-09-26 2022-01-07 东南大学 一种基于gnss_uwb的多传感器组合室内外无缝定位方法
CN113899370B (zh) * 2021-09-26 2024-02-23 东南大学 一种基于gnss_uwb的多传感器组合室内外无缝定位方法
CN114222240A (zh) * 2021-10-29 2022-03-22 中国石油大学(华东) 一种基于粒子滤波的多源融合定位方法
CN114485658A (zh) * 2021-12-08 2022-05-13 上海智能网联汽车技术中心有限公司 一种面向路侧感知系统精度评估的装置及方法
CN114526737A (zh) * 2022-01-17 2022-05-24 太原理工大学 一种基于gnss/uwb/dba融合的室内外无缝切换定位方法
CN114526737B (zh) * 2022-01-17 2024-04-12 太原理工大学 一种基于gnss/uwb/dba融合的室内外无缝切换定位方法
CN116086448A (zh) * 2023-04-12 2023-05-09 成都信息工程大学 基于uwb、imu、gnss融合的无人设备多场景无缝定位方法

Similar Documents

Publication Publication Date Title
CN113074732A (zh) 一种室内外无缝定位系统及其定位方法
US11965741B2 (en) Methods of attitude and misalignment estimation for constraint free portable navigation
CN110645979B (zh) 基于gnss/ins/uwb组合的室内外无缝定位方法
CN108344415B (zh) 一种组合导航信息融合方法
CN113311436B (zh) 一种移动平台上激光测风雷达运动姿态测风订正方法
JP2000502802A (ja) Ｇｐｓ速度を利用する改良された車両ナビゲーションシステム及びその方法
US10054442B2 (en) Method and apparatus for handling vertical orientations of devices for constraint free portable navigation
US11875519B2 (en) Method and system for positioning using optical sensor and motion sensors
JP2000506604A (ja) 改良された車両ナビゲーションシステム及びその方法
CN113063425B (zh) 车辆定位方法、装置、电子设备及存储介质
CN108885269B (zh) 导航方法、导航装置和导航系统
CN108151765A (zh) 一种在线实时估计补偿磁强计误差的定位测姿方法
CN113295174B (zh) 一种车道级定位的方法、相关装置、设备以及存储介质
Park et al. Implementation of vehicle navigation system using GNSS, INS, odometer and barometer
CN115542363B (zh) 一种适用于垂直下视航空吊舱的姿态测量方法
JP2783924B2 (ja) 車両位置検出装置
CN113419265B (zh) 一种基于多传感器融合的定位方法、装置与电子设备
Chen et al. Comprehensive Evaluation of Robust and Tight Integration of UWB and Low-cost IMU
JP3251705B2 (ja) 車両位置修正方式
Um et al. Location sensors
Ciecko et al. Analysis of the Accuracy and Usefulness of MEMS Chipsets Embedded in Popular Mobile Phones in Inertial Navigation
US20240039153A1 (en) Antenna phase control method and device
Wang et al. An Attitude Data Measurement System Based on Dual-Band Antenna Direction Finding
CN117804436A (zh) 远距离非协作无线定位与惯性导航的组合定位系统及方法
Liu et al. Positioning method of weighted centroid aided inertial measurement

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