CN116182873B - 室内定位方法、系统及计算机可读介质 - Google Patents
室内定位方法、系统及计算机可读介质 Download PDFInfo
- Publication number
- CN116182873B CN116182873B CN202310483907.7A CN202310483907A CN116182873B CN 116182873 B CN116182873 B CN 116182873B CN 202310483907 A CN202310483907 A CN 202310483907A CN 116182873 B CN116182873 B CN 116182873B
- Authority
- CN
- China
- Prior art keywords
- positioning
- positioning target
- determining
- target
- information
- 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 66
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 59
- 230000004927 fusion Effects 0.000 claims abstract description 23
- 238000001914 filtration Methods 0.000 claims abstract description 21
- 230000001133 acceleration Effects 0.000 claims abstract description 19
- 238000005259 measurement Methods 0.000 claims description 57
- 230000007704 transition Effects 0.000 claims description 15
- 238000004590 computer program Methods 0.000 claims description 12
- 238000006073 displacement reaction Methods 0.000 claims description 6
- 230000008054 signal transmission Effects 0.000 claims description 5
- 239000011159 matrix material Substances 0.000 description 22
- 230000008569 process Effects 0.000 description 20
- 238000005516 engineering process Methods 0.000 description 12
- 239000013598 vector Substances 0.000 description 12
- 230000006870 function Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 6
- 230000003287 optical effect Effects 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000005540 biological transmission Effects 0.000 description 3
- 238000004364 calculation method Methods 0.000 description 3
- 230000010354 integration Effects 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000005856 abnormality Effects 0.000 description 2
- 230000003044 adaptive effect Effects 0.000 description 2
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000012545 processing Methods 0.000 description 2
- 230000000644 propagated effect Effects 0.000 description 2
- 230000002829 reductive effect Effects 0.000 description 2
- 238000012546 transfer Methods 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 230000002411 adverse Effects 0.000 description 1
- 238000013459 approach Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000012937 correction Methods 0.000 description 1
- 230000003111 delayed effect Effects 0.000 description 1
- 238000012938 design process Methods 0.000 description 1
- 238000011156 evaluation Methods 0.000 description 1
- 239000000835 fiber Substances 0.000 description 1
- 230000000670 limiting effect Effects 0.000 description 1
- 239000013307 optical fiber Substances 0.000 description 1
- 238000004806 packaging method and process Methods 0.000 description 1
- 230000002441 reversible effect Effects 0.000 description 1
- 239000004065 semiconductor Substances 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 230000009897 systematic effect Effects 0.000 description 1
- 230000007306 turnover 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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching
-
- 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
-
- H—ELECTRICITY
- H04—ELECTRIC COMMUNICATION TECHNIQUE
- H04W—WIRELESS COMMUNICATION NETWORKS
- H04W64/00—Locating users or terminals or network equipment for network management purposes, e.g. mobility management
- H04W64/006—Locating users or terminals or network equipment for network management purposes, e.g. mobility management with additional information processing, e.g. for direction or speed determination
-
- 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)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Signal Processing (AREA)
- Navigation (AREA)
Abstract
本申请提供了一种室内定位方法、系统及计算机可读介质，其中，一种室内定位方法，所述方法包括：根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置；根据IMU系统和预设参量对所述定位目标进行定位，确定所述定位目标的第二位置；其中，所述预设参量至少包括所述定位目标的陀螺仪误差信息和加速度误差信息；通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置。可以至少用以解决相关技术中只可以对处于简单运动状态下的目标进行定位，而无法对处于复杂运动状态的目标进行定位的技术问题。
Description
技术领域
本申请涉及信息技术领域，尤其涉及一种室内定位方法、系统及计算机可读介质。
背景技术
目前对于室外定位大部分采用全球定位系统（英文全称Global PositioningSystem，简称GPS）定位技术，不过大家应该普遍体会过GPS信号弱导致位置迟迟无法更新，开车导航错过下高速路口的窘境等等。既然室外定位都这么不稳定，那么在室内由于房屋的遮掩、GPS定位的信号和精度就更无法满足实际需求，所以诞生了一些室内定位方案，比如基于超宽带技术进行室内定位。
其中，基于UWB技术的室内定位方案因其高精度的定位性能、低复杂度的系统实现以及高安全性等特征而受到越来越多的关注，而基于UWB技术的室内定位方案的测量质量受信道环境、物理遮挡等场景因素影响明显，若直接应用于存在复杂多径和密集非视线传输（英文全称Not Line of Sight，简称NLOS）条件的环境下，则其定位的精度将受到影响。而基于惯性导航技术的定位方案，无需事先部署节点，具备高刷新率、低成本、全天候工作等优势，因此在相关技术中，可以结合UWB技术与IMU技术进行室内定位。
比如，公告号为CN110375730B的专利提出了一种基于IMU和UWB融合的室内定位导航系统，其采用融合定位算法，将IMU和UWB相互结合，IMU得到的数据作为卡尔曼滤波的先验信息，UWB得到的数据作为卡尔曼滤波的观测信息，利用各自表现出的优点，有效地提高系统的定位导航精度，利用少量的定位基站即可实现目标的高精度室内定位导航。
然而，发明人至少发现：相关技术中的方案一般只可以对处于简单运动状态下的目标进行定位，比如行人的匀速运动或者匀加速运动状态；无法对处于复杂运动状态的目标进行定位，比如变速运动或变向运动等等；此外，UWB室内测量环境往往比较复杂，如何规避NLOS测量对定位精度及系统稳定性的影响也是相关技术中未涉及的盲点。
发明内容
本申请的一个目的是提供一种室内定位方法、系统及计算机可读介质，至少用以解决相关技术中只可以对处于简单运动状态下的目标进行定位，而无法对处于复杂运动状态的目标进行定位的技术问题，同时消除NLOS测量对系统稳定性的不良影响并进一步提升定位精度。
为实现上述目的，本申请的一些实施例提供了一种室内定位方法，所述方法包括：根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置；根据IMU系统和预设参量对所述定位目标进行定位，确定所述定位目标的第二位置；其中，所述预设参量至少包括所述定位目标的陀螺仪误差信息和加速度误差信息；通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置。
本申请的一些实施例还提供了一种室内定位系统，所述系统包括：UWB系统、IMU系统以及计算装置；所述计算装置包括一个或多个处理器；以及存储有计算机程序指令的存储器，所述计算机程序指令在被执行时使所述处理器执行如上所述的方法。
本申请的一些实施例还提供了一种计算机可读介质，其上存储有计算机程序指令，所述计算机程序指令可被处理器执行以实现如上所述的方法。
相较于现有技术，本申请实施例提供的方案中，根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置，并且根据IMU系统和预设参量对所述定位目标进行定位，确定所述定位目标的第二位置；然后通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置。由于所述预设参量至少包括所述定位目标的陀螺仪误差信息和加速度误差信息，因此可以对处于复杂运动状态的目标进行定位，比如无人机的旋转状态、翻转状态等；进一步地，于本申请实施例提供的方案，还在根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置的过程中，先根据预设因素对定位结果进行评估，以确定所述定位结果是否为有效数据，当且仅当该定位结果为有效数据时，才将所述定位结果作为所述定位目标的第一位置，这样可以提前过滤到不准确的数据，从而提升定位的准确性；此外，由于还根据所述定位目标的马氏距离，与预设阈值比较确定所述定位目标的轨迹偏差程度。通过巧妙引入马氏距离可以消除不同坐标维度上信息的相关性，以更准确地检测出数据离群点，从而有利于进一步提升定位的准确性。
附图说明
图1为本申请实施例提供的一种室内定位方法的示例性流程图；
图2为本申请实施例提供的另一种的结构示意图；
图3为本申请实施例提供的一种室内定位方法的示例性示意图。
具体实施方式
为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整的描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在本文中使用以下术语。
超宽带，英文全称Ultra Wide Band，简称UWB，是一种无线载波通信技术，利用纳秒级的非正弦波窄脉冲传输数据，尤其适用于室内等密集多径场所的高速无线接入。
惯性导航，英文全称Inertial Measurement Unit，简称 IMU，是测量物体三轴姿态角（或角速率）以及加速度的装置。
到达时间算法，英文全称Time of Arrival，简称TOA，通过信号在空中的传播速度乘飞行的时间来测量目标定位点与接收端的距离来最终定位的方法。
到达时间差算法，英文全称Time Difference of Arrival，简称TDOA。
实施例一
本申请实施例提供了一种室内定位方法，参见图1所示，所述方法可以包括如下步骤：
步骤S101，根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置；
步骤S102，根据IMU系统和预设参量对所述定位目标进行定位，确定所述定位目标的第二位置；其中，所述预设参量至少包括所述定位目标的陀螺仪误差信息和加速度误差信息；
步骤S103，通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置。
为便于理解，以下分别对上述步骤S101至步骤S103进行说明。
针对步骤S101：
在一些例子中，所述UWB系统包括UWB基站，可以参见图2所示。
在一些例子中，所述预设算法可以为LS算法/WLS算法，其中，所述LS算法为基于最小方差的算法，所述WLS为基于带权重的最小方差的算法。
在一些例子中，所述UWB系统可以使用基于到达时间（英文全称Time of Arrival，简称TOA）、基于到达时间差（英文全称Time Difference of Arrival，简称TDOA）算法或者基于信号到达相位差测距算法（Phase Difference of Arrival，简称PDOA）来实现室内定位，此处不做具体限定。
针对步骤S102：
在一些例子中，所述IMU系统包括IMU传感器，可以参见图2所示。
在一些其他例子中，所述预设参量还可以包括所述定位目标的位置误差信息、速度误差信息、方向误差信息
针对步骤S103：
本申请实施例中将所述容积卡尔曼滤波算法简称为CKF算法，其英文全称为Cubature Kalman Filter。
具体地说，根据室内的定位目标的维度的不同，在相关技术中需要在室内设置不同数量的定位基站：基于TOA算法和基于TDOA算法的UWB室内定位方案中，若定位二维目标，则最小定位单元分别为需要至少设置三或四个定位基站；若定位三维目标，则最小定位单元分别为需要至少设置四个或五个定位基站；若目标活动范围较大或者室内布局复杂，则需要按最小定位单元复数扩展为定位网络并相互通联，才能实现实时有效地定位与跟踪，比如一个大房间包括若干小房间，则需要在每个小房间均设置相应数量的定位基站，这无疑将引入一定的基站部署成本以及相应的基站控制成本。而本申请实施例中，由于通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置，可以节省一定数量的定位基站，从而有利于降低定位成本。
与相关技术相比，本申请实施例提供的室内定位方法，根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置，并且根据IMU系统和预设参量对所述定位目标进行定位，确定所述定位目标的第二位置；然后通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置。由于所述预设参量至少包括所述定位目标的陀螺仪误差信息和加速度误差信息，因此可以对处于复杂运动状态的目标进行定位，比如变速运动或变向运动等等。
实施例二
在本申请一些实施例中，所述根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置可以包括：
获取所述定位目标与所述UWB系统中各基站的信号传输时间；
根据所述信号传输时间确定所述定位目标和所述各基站的距离；
根据所述定位目标和所述各基站的距离，确定所述定位目标的第一位置。
本申请实施例以UWB系统基于TOA的定位方案为例，描述结合LS/WLS算法对定位目标进行定位，确定所述定位目标的第一位置的过程。
具体地说，考虑到存在噪声、障碍及NLOS测量的情况，可以通过如下公式计算所述定位目标和所述各基站的距离：
其中，
为所述定位目标和基站的真实距离，/>
为基站，/>
表示基站的数量，/>
为定位目标/>
与所述UWB系统中各基站/>
的信号传输时间，/>
为电磁波在自由空间的传播速度，可近似为光速，/>
为真实距离/>
的测量值，/>
为测量噪声，/>
为NLOS误差。
进一步地，
，UWB系统满足如下测量方程：
其中，公式①用于定位三维目标的情况，公式②用于定位二维目标的情况。
进一步地，以定位三维的定位目标为例，在理想的LOS条件下，均服从高斯分布的随机噪声且均值为0，根据LS估计理论，可以根据如下公式得到定位目标的第一位置的唯一最小二乘解：
式中，系数矩阵
待求矩阵/>
，其中，/>
。
在一些例子中，考虑到定位目标和所述各基站的距离存在不同程度的误差，本申请实施例中可以根据误差大小赋予相应的权重，以减小误差对UWB系统的定位结果的影响。具体地，根据TS-WLS估计理论，可以得到定位目标的第一位置的加权最小二乘解：
其中，
分别为两次WLS估计的计算结果。
实施例三
在本申请一些实施例中，所述根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置可以包括：
在所述根据UWB系统和预设算法对定位目标进行定位后，根据预设因素对定位结果进行评估，以确定所述定位结果是否为有效数据；
若是，则将所述定位结果作为所述定位目标的第一位置；
其中，所述预设因素包括以下至少之一：
各所述基站上报的测量信息、所述定位目标的瞬时位移信息、所述定位目标的瞬时速度信息、所述定位目标的轨迹偏差信息，可以参见图3所示。图3中，计算LS/WLS粗估计即可以理解为据预设因素对定位结果进行评估，统计测量附属信息即为各所述基站上报的测量信息。评估器用于确定所述定位结果是否为有效数据。
具体地说，在根据UWB系统和预设算法对定位目标进行定位后，可以将得到的定位结果作为粗估计的结果，然后对该粗估计的结果进行评估，以确定所述定位结果是否为有效数据，只有在所述定位结果为有效数据时，才将所述定位结果作为所述定位目标的第一位置。
进一步地，这里所说的对该粗估计的结果进行评估，可以是根据预设因素对定位结果进行评估。所述预设因素可以包括但不限于以下任意之一或其任意组合：各所述基站上报的测量信息、所述定位目标的瞬时位移信息、所述定位目标的瞬时速度信息、所述定位目标的轨迹偏差信息。
在一些例子中，预设因素可以包括各所述基站上报的测量信息。其中，各所述基站上报的测量信息可以包括但不限于信道环境优劣、接收信号强度（英文全称ReceivedSignal Strength，简称RSS)、品质因数（英文全称Figure of Merit，简称FoM）等。比如，如果信道环境优劣、接收信号强度与品质因数均低于预设阈值，则将所述定位结果作为无效数据；当信道环境优劣、接收信号强度与品质因数均大于或者等于预设阈值时，所述定位结果为有效数据，将所述定位结果作为所述定位目标的第一位置。
在一些例子中，预设因素可以包括所述定位目标的瞬时位移信息。比如说，可以结合当前时刻的定位结果和前一时刻的定位结果进行融合估计，比如CKF融合估计，从而可以得到两时刻间估计位置的欧氏距离（Euclidean Distance) ，以此距离作为定位目标的瞬时位移信息，判断距离是否大于或者等于预设阈值，当所述距离大于或者等于预设阈值时，所述定位结果为有效数据，将所述定位结果作为所述定位目标的第一位置。
在一些例子中，预设因素可以包括所述定位目标的瞬时速度信息。比如说，可以结合当前时刻的定位结果和前一时刻的定位结果进行融合估计，比如CKF融合估计，然后再结合CKF融合估计的结果和UWB系统的测量周期，得到定位目标的瞬时速度信息，判断此速度是否大于或者等于预设阈值，当所述定位目标的瞬时速度信息大于或者等于预设阈值时，所述定位结果为有效数据，将所述定位结果作为所述定位目标的第一位置。
在一些例子中，预设因素可以包括所述定位目标的轨迹偏差信息。比如说，可以判断定位目标的轨迹偏差信息是否大于或者等于预设阈值，当所述定位目标的轨迹偏差信息大于或者等于预设阈值时，所述定位结果为有效数据，将所述定位结果作为所述定位目标的第一位置。
不难发现，本申请实施例中，在根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置的过程中，先根据预设因素对定位结果进行评估，以确定所述定位结果是否为有效数据，当且仅当该定位结果为有效数据时，才将所述定位结果作为所述定位目标的第一位置，这样可以提前过滤到不准确的数据，从而提升定位的准确性。
进一步地，在本申请一些实施例中，所述预设因素包括所述定位目标的轨迹偏差信息；所述定位目标的轨迹偏差信息的获取方法可以包括：
根据所述定位目标的当前位置和所述定位目标的前一时刻的位置信息，确定所述定位目标的马氏距离；
根据所述定位目标的马氏距离，与预设阈值比较确定所述定位目标的轨迹偏差程度。引入马氏距离可以消除不同坐标维度上信息的相关性，以更准确地检测出数据离群点，从而有利于进一步提升定位的准确性。
实施例四
在本申请一些实施例中，所述通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合可以包括：
根据所述预设参量中的各参量，确定所述定位目标的状态转移信息；
根据容积卡尔曼滤波算法和所述定位目标的状态转移信息，确定所述定位目标的IMU测量结果；
基于所述定位目标的IMU测量结果，对所述第一位置和所述第二位置进行融合。其中，所述定位目标的IMU测量结果是对所述第二位置校准后的结果。在对所述第一位置和所述第二位置进行融合的过程中，对所述所述第一位置和所述定位目标的IMU测量结果融合即可。
在本申请一些实施例中，所述预设参量除了包括所述定位目标的陀螺仪误差信息和加速度误差信息，还可以包括所述定位目标的位置误差信息、速度误差信息、方向误差信息。
在一些例子中，假设定位目标的状态向量为
，其中，/>
、
、/>
分别为导航坐标系N(Navigation Coordinate System) 中定位目标的位置误差信息、速度误差信息、方向误差信息；/>
为目标坐标系B（Body Coordinate System）中定位目标的陀螺仪误差信息和加速度误差信息。
在一些例子中，由于状态向量中包括两个坐标系中的测量信息，因此需要通过坐标变换过程统一坐标系。参考捷联惯性导航系统 （英文全称Strapdown InertialNavigation System，简称SINS）的相关知识，从坐标系B至N的坐标变换矩阵可根据欧拉角法表达如下：
其中，
分别为定位目标的方向角信息、俯仰角信息与翻滚角信息，其具体可以通过对三轴陀螺仪所测角速度进行积分获取。
类似地，对三轴加速度计所测加速度分别进行一次、两次积分可以获取定位目标的速度与位移的信息，即：
其中，
分别为/>
的一阶微分量，/>
为坐标系B中的加速度矢量，/>
为坐标系N中的重力加速度矢量，/>
为坐标系B中角速度信息，角速度矩阵/>
为一斜对称矩阵，定义如下：
引入误差量后，可以分别得到状态向量中的各参量偏差方程与状态转移过程。
陀螺仪与加速度计偏差方程：
其中，
与/>
分别为角速度真实值/>
、加速度真实值/>
的测量结果；
分别为陀螺仪与加速度计的测量噪声，且分别服从于协方差为/>
、/>
的高斯分布；/>
则分别为角速度与加速度的漂移偏差，其状态转移过程均为与时间相关的一阶马尔科夫过程，即：
其中，
分别为陀螺仪与加速度计的测量周期；/>
为过程噪声，且分别服从于协方差为/>
的高斯分布。
方向误差方程：
方向误差由于陀螺仪偏差产生，并满足：
其中，
表示陀螺仪测量偏差从坐标系B至坐标系N的转换，由陀螺仪漂移偏差与其测量噪声决定。
位置误差与速度误差方程：
速度误差由加速度计误差产生，但由于加速度矢量在方向上也受到影响，进一步造成了衍生的速度与位置误差：
其中，
表示方向误差对加速度/>
的衍生误差，/>
由加速度计测量偏差，由加速度计漂移偏差与其测量噪声决定。
综上，状态转移矩阵为：
噪声转移矩阵
与过程噪声/>
为：
其中，
与/>
分别表示3*3的单位矩阵与全零矩阵，/>
为UWB系统刷新周期。
噪声
的协方差矩阵为：
考虑CKF融合定位系统的测量方程：
LOS条件下，观测噪声
可近似为均值为0的高斯白噪声；在NLOS条件下，/>
不再是零均值高斯白噪声；/>
的协方差矩阵为/>
。
记k时刻IMU系统估计定位目标位置
，CKF状态转移过程可获得IMU位置估计的先验误差/>
，则以TOA定位方案为例，测量值即为径向测距结果，测量向量/>
，其中/>
为时刻第/>
个UWB基站与定位目标之间的TOA观测值，/>
为UWB定位系统中的基站个数；
，/>
为时刻第/>
个参考基站与待定位目标之间的真实距离。
根据贝叶斯滤波原理，基于高斯假设的贝叶斯滤波算法可解决高斯域上计算积分的问题，并以非线性函数高斯密度的形式存在。考虑容积卡尔曼滤波方法，根据球面径向容积准则，可通过
个等权值的容积点/>
来实现非线性近似（数值积分需要/>
个积分点），在保持积分近似有效的情况下明显减小了计算量。式中，
分别为径向积分点与球面积分点，/>
和/>
分别表述容积积分点及其对应的积分权值，即：
式中，
表示容积点集合中的第/>
个元素，容积点满足：容积点集合/>
为相互正交、完全对称；若/>
中存在/>
列，则
容积卡尔曼滤波分为状态预测和测量更新两个部分。
状态预测过程包括：
(1)通过如下公式计算容积点：
(2)通过如下公式预测容积点：
(3)通过如下公式计算状态均值与协方差矩阵：
测量更新过程包括：
(1)通过如下公式更新容积点：
(2)通过如下公式预测观测值：
(3)通过如下公式计算观测均值与协方差矩阵：
(4)通过如下公式计算卡尔曼增益：
(5)通过如下公式更新状态值与状态协方差矩阵：
由算法流程可见，CKF依靠确定的样本点来求得概率密度函数，明确非线性函数的具体形式后易实现系统封装与模块化处理。在实际使用中，CKF算法且相比EKF和UKF算法具有更高的定位精度和更快的收敛速度，同时单次迭代CKF计算量较小，时间复杂度介于EKF和UKF之间，具备严谨的理论基础，相对更能满足室内定位对定位精度、实时性与可靠性的需求。
实施例五
在本申请一些实施例中，所述根据容积卡尔曼滤波算法和所述定位目标的状态转移信息，确定所述定位目标的IMU测量结果可以包括：根据容积卡尔曼滤波算法和所述定位目标的状态转移信息，确定所述定位目标的IMU第一测量结果；对所述第一测量结果进行修正，得到所述定位目标的IMU测量结果。
在本申请一些实施例中，所述对所述第一测量结果进行修正，得到所述定位目标的IMU测量结果可以进一步包括：通过抗差因子对所述第一测量结果进行修正，得到所述定位目标的IMU测量结果。
具体地说，对于陀螺仪与加速度计在任意时刻的测量值，其与真实信息的偏差值已通过融合CKF状态向量中得到，因此将IMU测量修正为：
基于坐标系B中的角速度与加速度信息，目标在导航系N中的加速度、速度、位置信息可根据下式计算：
其中，
为融合定位系统的测量周期，/>
分别为目标在k时刻的位置与速度估计误差，可通过CKF状态向量获得。
坐标转移矩阵的更新如下：
具体地说，虽然CKF的使用避免了估计运动模型的非线性误差，降低了系统误差的影响，但与EKF/UKF同样不具备鲁棒性，即在测量误差过大时，滤波器性能将严重退化。为此，在滤波器设计过程中加入自适应估计，并引入抗差因子，则能有效增强滤波算法抵抗异常误差的能力，从而降低NLOS测量影响，提高系统稳健性。
观察到在标准CKF计算过程中，异常值只影响测量更新过程，所以自适应CKF算法相对于标准CKF算法仅在测量更新过程进行调整与修正，即仅对自相关协方差矩阵进行如下式的修正：
式中，
，其中/>
为抗差因子。
在一些例子中，以基于Huber法为例，利用单个预测残差值构建统计量，并且为了保证等价权阵对角线元素不为0，建立等价权矩阵的表达式。
基于预测残差构建的统计量如下所示：
式中，
为预测残差向量对应的分量；/>
为修正前自协方差矩阵的对角线元素。若定位过程中，UWB观测值无异常，则/>
；否则，
，此为常数阈值，一般可通过在LOS场景下取一段时间平均值确定，因此有抗差因子构成的等式为：
令
当出现UWB观测值异常时，对自协方差矩阵/>
进行修正以调整滤波增益矩阵，进而得出测量更新和状态更新。
CKF融合定位系统状态向量应满足如下式所述的状态转移过程：
其中，
与/>
分别表示状态转移矩阵与噪声转移矩阵，/>
为/>
时刻过程噪声向量。
可以理解，IMU系统随工作时间变长，累计误差增加；同时，UWB系统易受NLOS影响，本申请实施例中，通过卡尔曼滤波算法融合处理UWB基站与IMU传感器的测量数据，可以进一步提升定位精度。
综上，本申请实施例提供的方案中，结合了IMU系统高刷新率、不依赖外部基站及其低成本、全天候的特征和UWB系统复杂度低、抗多径性好等优点，基于UWB系统实时测量结果并通过引入马氏距离等从多个维度评估UWB的定位结果，可以有效消除IMU系统的累计误差；基于UWB系统对定位目标的位置进行评估，根据自适应滤波原理确定抗差因子代入CKF，以对抗UWB系统的异常测量并修正IMU传感器上报瞬时加速度及姿态信息等，基于稳健的容积卡尔曼滤波算法进一步完成对定位目标运动状态的修正预测。该融合方案可用于更复杂的运动模型，同时在未引入非线性误差的同时提升了对抗NLOS测量的能力，改进了系统稳定性，优化了定位精度，拓宽了应用范围，具有较高的市场价值。
实施例六
本申请实施例还提供了一种室内定位系统，所述系统包括：UWB系统、IMU系统以及计算装置；所述计算装置包括一个或多个处理器；以及存储有计算机程序指令的存储器，所述计算机程序指令在被执行时使所述处理器执行如上所述的方法。
本申请实施例中的方法和/或实施例可以被实现为计算机软件程序。例如，本公开的实施例包括一种计算机程序产品，其包括承载在计算机可读介质上的计算机程序，该计算机程序包含用于执行流程图所示的方法的程序代码。在该计算机程序被处理单元执行时，执行本申请的方法中限定的上述功能。
需要说明的是，本申请所述的计算机可读介质可以是计算机可读信号介质或者计算机可读存储介质或者是上述两者的任意组合。计算机可读介质例如可以是——但不限于——电、磁、光、电磁、红外线、或半导体的系统、装置或器件，或者任意以上的组合。计算机可读存储介质的更具体的例子可以包括但不限于：具有一个或多个导线的电连接、便携式计算机磁盘、硬盘、随机访问存储器（RAM）、只读存储器（ROM）、可擦式可编程只读存储器（EPROM或闪存）、光纤、便携式紧凑磁盘只读存储器（CD-ROM）、光存储器件、磁存储器件、或者上述的任意合适的组合。在本申请中，计算机可读介质可以是任何包含或存储程序的有形介质，该程序可以被指令执行系统、装置或者器件使用或者与其结合使用。
而在本申请中，计算机可读的信号介质可以包括在基带中或者作为载波一部分传播的数据信号，其中承载了计算机可读的程序代码。这种传播的数据信号可以采用多种形式，包括但不限于电磁信号、光信号或上述的任意合适的组合。计算机可读的信号介质还可以是计算机可读存储介质以外的任何计算机可读介质，该计算机可读介质可以发送、传播或者传输用于由指令执行系统、装置或者器件使用或者与其结合使用的程序。计算机可读介质上包含的程序代码可以用任何适当的介质传输，包括但不限于：无线、电线、光缆、RF等等，或者上述的任意合适的组合。
可以以一种或多种程序设计语言或其组合来编写用于执行本申请的操作的计算机程序代码，所述程序设计语言包括面向对象的程序设计语言—诸如Java、Smalltalk、C++，还包括常规的过程式程序设计语言—诸如“C”语言或类似的程序设计语言。程序代码可以完全地在用户计算机上执行、部分地在用户计算机上执行、作为一个独立的软件包执行、部分在用户计算机上部分在远程计算机上执行、或者完全在远程计算机或服务器上执行。在涉及远程计算机的情形中，远程计算机可以通过任意种类的网络——包括局域网（LAN)或广域网（WAN)—连接到用户计算机，或者，可以连接到外部计算机（例如利用因特网服务提供商来通过因特网连接）。
附图中的流程图或框图示出了按照本申请各种实施例的设备、方法和计算机程序产品的可能实现的体系架构、功能和操作。在这点上，流程图或框图中的每个方框可以代表一个模块、程序段、或代码的一部分，该模块、程序段、或代码的一部分包含一个或多个用于实现规定的逻辑功能的可执行指令。也应当注意，在有些作为替换的实现中，方框中所标注的功能也可以以不同于附图中所标注的顺序发生。例如，两个接连地表示的方框实际上可以基本并行地执行，它们有时也可以按相反的顺序执行，这依所涉及的功能而定。也要注意的是，框图和/或流程图中的每个方框、以及框图和/或流程图中的方框的组合，可以用执行规定的功能或操作的专用的针对硬件的系统来实现，或者可以用专用硬件与计算机指令的组合来实现。
实施例七
本申请实施例还提供了一种计算机可读介质，该计算机可读介质可以是上述实施例中描述的设备中所包含的；也可以是单独存在，而未装配入该设备中。上述计算机可读介质承载有一个或者多个计算机可读指令，所述计算机可读指令可被处理器执行以实现前述本申请的多个实施例的方法和/或技术方案的步骤。
在本申请一个典型的配置中，终端、服务网络的设备均包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器 (RAM) 和/或非易失性内存等形式，如只读存储器 (ROM) 或闪存（flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体，可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存 (PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器 (DRAM)、其他类型的随机存取存储器 (RAM)、只读存储器(ROM)、电可擦除可编程只读存储器 (EEPROM)、快闪记忆体或其他内存技术、只读光盘（CD-ROM)、数字多功能光盘 (DVD) 或其他光学存储、磁盒式磁带，磁带磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。
此外，本申请实施例还提供了一种计算机程序，所述计算机程序存储于计算机设备，使得计算机设备执行所述控制代码执行的方法。
需要注意的是，本申请可在软件和/或软件与硬件的组合体中被实施，例如，可采用专用集成电路（ASIC）、通用目的计算机或任何其他类似硬件设备来实现。在一些实施例中，本申请的软件程序可以通过处理器执行以实现上文步骤或功能。同样地，本申请的软件程序（包括相关的数据结构）可以被存储到计算机可读记录介质中，例如，RAM存储器，磁或光驱动器或软磁盘及类似设备。另外，本申请的一些步骤或功能可采用硬件来实现，例如，作为与处理器配合从而执行各个步骤或功能的电路。
对于本领域技术人员而言，显然本申请不限于上述示范性实施例的细节，而且在不背离本申请的精神或基本特征的情况下，能够以其他的具体形式实现本申请。因此，无论从哪一点来看，均应将实施例看作是示范性的，而且是非限制性的，本申请的范围由所附权利要求而不是上述说明限定，因此旨在将落在权利要求的等同要件的含义和范围内的所有变化涵括在本申请内。不应将权利要求中的任何附图标记视为限制所涉及的权利要求。此外，显然“包括”一词不排除其他单元或步骤，单数不排除复数。装置权利要求中陈述的多个单元或装置也可以由一个单元或装置通过软件或者硬件来实现。第一，第二等词语用来表示名称，而并不表示任何特定的顺序。
Claims (7)
1.一种室内定位方法，其特征在于，所述方法包括：
根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置；
根据IMU系统和预设参量对所述定位目标进行定位，确定所述定位目标的第二位置；其中，所述预设参量至少包括所述定位目标的陀螺仪误差信息和加速度误差信息；
通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合，得到融合结果，并根据所述融合结果确定所述定位目标的当前位置；
其中，所述根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置包括：在所述根据UWB系统和预设算法对定位目标进行定位后，根据预设因素对定位结果进行评估，以确定所述定位结果是否为有效数据；若是，则将所述定位结果作为所述定位目标的第一位置；
其中，所述预设因素包括定位目标的轨迹偏差信息；所述定位目标的轨迹偏差信息的获取方法包括：
根据所述定位目标的当前位置和所述定位目标的前一时刻的位置信息，确定所述定位目标的马氏距离；
根据所述定位目标的马氏距离，确定所述定位目标的轨迹偏差信息；
其中，所述预设参量还包括所述定位目标的位置误差信息、速度误差信息、方向误差信息；
所述通过容积卡尔曼滤波算法，对所述第一位置和所述第二位置进行融合包括：根据所述预设参量中的各参量，确定所述定位目标的状态转移信息；根据容积卡尔曼滤波算法和所述定位目标的状态转移信息，确定所述定位目标的IMU测量结果；基于所述定位目标的IMU测量结果，对所述第一位置和所述第二位置进行融合。
2.根据权利要求1所述的方法，其特征在于，所述根据UWB系统和预设算法对定位目标进行定位，确定所述定位目标的第一位置包括：
获取所述定位目标与所述UWB系统中各基站的信号传输时间；
根据所述信号传输时间确定所述定位目标和所述各基站的距离；
根据所述定位目标和所述各基站的距离，确定所述定位目标的第一位置。
3.根据权利要求1所述的方法，其特征在于，所述预设因素还包括以下至少之一：
各基站上报的测量信息、所述定位目标的瞬时位移信息、所述定位目标的瞬时速度信息。
4.根据权利要求1所述的方法，其特征在于，所述根据容积卡尔曼滤波算法和所述定位目标的状态转移信息，确定所述定位目标的IMU测量结果包括：
根据容积卡尔曼滤波算法和所述定位目标的状态转移信息，确定所述定位目标的IMU第一测量结果；
对所述第一测量结果进行修正，得到所述定位目标的IMU测量结果。
5.根据权利要求4所述的方法，其特征在于，所述对所述第一测量结果进行修正，得到所述定位目标的IMU测量结果包括：
通过抗差因子对所述第一测量结果进行修正，得到所述定位目标的IMU测量结果。
6.一种室内定位系统，其特征在于，所述系统包括：UWB系统、IMU系统以及计算装置；所述计算装置包括
一个或多个处理器；以及
存储有计算机程序指令的存储器，所述计算机程序指令在被执行时使所述处理器执行如权利要求1至5中任意一项所述的方法。
7.一种计算机可读介质，其上存储有计算机程序指令，所述计算机程序指令可被处理器执行以实现如权利要求1至5中任意一项所述的方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310483907.7A CN116182873B (zh) | 2023-05-04 | 2023-05-04 | 室内定位方法、系统及计算机可读介质 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310483907.7A CN116182873B (zh) | 2023-05-04 | 2023-05-04 | 室内定位方法、系统及计算机可读介质 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116182873A CN116182873A (zh) | 2023-05-30 |
| CN116182873B true CN116182873B (zh) | 2023-07-11 |
Family
ID=86446652
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310483907.7A Active CN116182873B (zh) | 2023-05-04 | 2023-05-04 | 室内定位方法、系统及计算机可读介质 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116182873B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117289207B (zh) * | 2023-11-22 | 2024-01-26 | 成都宜泊信息科技有限公司 | 一种适用于室内nlos环境的定位方法 |
| CN118016035A (zh) * | 2024-03-28 | 2024-05-10 | 深圳市戴乐体感科技有限公司 | 一种鼓槌定位系统、方法、设备及介质 |
Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN113074739A (zh) * | 2021-04-09 | 2021-07-06 | 重庆邮电大学 | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 |
| CN114383605A (zh) * | 2021-12-03 | 2022-04-22 | 理大产学研基地(深圳)有限公司 | 基于mems传感器和稀疏地标点的室内定位及优化方法 |
Family Cites Families (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| EP2657647A1 (en) * | 2012-04-23 | 2013-10-30 | Deutsches Zentrum für Luft- und Raumfahrt e. V. | Method for estimating the position and orientation using an inertial measurement unit fixed to a moving pedestrian |
| CN105509739B (zh) * | 2016-02-04 | 2018-04-06 | 济南大学 | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 |
| CN109188352B (zh) * | 2018-09-07 | 2022-09-30 | 东南大学 | 一种组合导航相对定位方法 |
| CN110631576B (zh) * | 2019-08-28 | 2023-01-13 | 南京理工大学 | 一种基于uwb和imu的抗nlos的室内定位系统及其定位方法 |
| CN114396943B (zh) * | 2022-01-12 | 2024-07-23 | 国家电网有限公司 | 一种融合定位方法与终端 |
| CN114739400A (zh) * | 2022-04-11 | 2022-07-12 | 中国科学院合肥物质科学研究院 | 一种基于uwb和imu信息融合的室内定位方法 |
| CN115494450B (zh) * | 2022-11-17 | 2023-03-21 | 长沙驰芯半导体科技有限公司 | 一种高精度的超宽带室内定位跟踪与控制方法及装置 |
-
2023
- 2023-05-04 CN CN202310483907.7A patent/CN116182873B/zh active Active
Patent Citations (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
| CN113074739A (zh) * | 2021-04-09 | 2021-07-06 | 重庆邮电大学 | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 |
| CN114383605A (zh) * | 2021-12-03 | 2022-04-22 | 理大产学研基地(深圳)有限公司 | 基于mems传感器和稀疏地标点的室内定位及优化方法 |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116182873A (zh) | 2023-05-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN116182873B (zh) | 室内定位方法、系统及计算机可读介质 | |
| EP3430419B1 (en) | Estimating locations of mobile devices in a wireless tracking system | |
| CN113108791B (zh) | 一种导航定位方法及导航定位设备 | |
| CN113074739B (zh) | 基于动态鲁棒容积卡尔曼的uwb/ins融合定位方法 | |
| Tian et al. | A resetting approach for INS and UWB sensor fusion using particle filter for pedestrian tracking | |
| US10605925B2 (en) | Processing GPS drifting | |
| US9606848B2 (en) | Iterative Kalman filtering | |
| CN112923919B (zh) | 基于图优化的行人定位方法及系统 | |
| CN113933818A (zh) | 激光雷达外参的标定的方法、设备、存储介质及程序产品 | |
| CN111982102B (zh) | 一种复杂环境下基于bp-ekf的uwb-imu定位方法 | |
| CN113347709B (zh) | 一种基于uwb的室内定位方法及系统 | |
| CN108134640A (zh) | 一种基于节点运动状态约束的协作定位系统和方法 | |
| Li et al. | Research on the UWB/IMU fusion positioning of mobile vehicle based on motion constraints | |
| KR101141984B1 (ko) | Ｄｒ／ｇｐｓ 데이터 융합 방법 | |
| CN114861725A (zh) | 一种目标感知跟踪的后处理方法、装置、设备及介质 | |
| CN112540345B (zh) | 一种基于高斯分布检测uwb质量的双模型定位方法及系统 | |
| Wang et al. | A robust backtracking CKF based on Krein space theory for in-motion alignment process | |
| Chu et al. | Performance comparison of tight and loose INS-Camera integration | |
| CN116718153B (zh) | 一种基于gnss和ins的形变监测方法及系统 | |
| Choi et al. | An adaptive tracking estimator for robust vehicular localization in shadowing areas | |
| JP4804371B2 (ja) | センサのバイアス誤差推定装置 | |
| CN117320148A (zh) | 多源数据融合定位方法、系统、电子设备及存储介质 | |
| Zhang et al. | Adaptive cubature Kalman filter based on the variance-covariance components estimation | |
| CN116222541A (zh) | 利用因子图的智能多源组合导航方法及装置 | |
| CN115790603A (zh) | 一种用于信息拒止环境下的无人机动态目标估计方法 |
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 |