CN116222556B - 一种基于多源传感器融合的室内定位方法及系统 - Google Patents
一种基于多源传感器融合的室内定位方法及系统 Download PDFInfo
- Publication number
- CN116222556B CN116222556B CN202310039739.2A CN202310039739A CN116222556B CN 116222556 B CN116222556 B CN 116222556B CN 202310039739 A CN202310039739 A CN 202310039739A CN 116222556 B CN116222556 B CN 116222556B
- Authority
- CN
- China
- Prior art keywords
- uwb
- residual error
- positioning
- imu
- vio
- 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
- 230000004927 fusion Effects 0.000 title claims abstract description 73
- 238000000034 method Methods 0.000 title claims abstract description 57
- 238000005457 optimization Methods 0.000 claims abstract description 73
- 238000007781 pre-processing Methods 0.000 claims abstract description 25
- 238000005259 measurement Methods 0.000 claims abstract description 24
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 15
- 238000001514 detection method Methods 0.000 claims abstract description 15
- 230000000007 visual effect Effects 0.000 claims description 22
- 230000010354 integration Effects 0.000 claims description 20
- 238000004364 calculation method Methods 0.000 claims description 11
- 238000010276 construction Methods 0.000 claims description 11
- 230000008859 change Effects 0.000 claims description 7
- 238000005070 sampling Methods 0.000 claims description 7
- 238000012545 processing Methods 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000005286 illumination Methods 0.000 abstract description 9
- 230000008569 process Effects 0.000 abstract description 7
- 230000007774 longterm Effects 0.000 abstract description 6
- 230000000295 complement effect Effects 0.000 abstract description 5
- 230000007613 environmental effect Effects 0.000 abstract description 4
- 230000007246 mechanism Effects 0.000 abstract description 3
- 238000010586 diagram Methods 0.000 description 22
- 238000004458 analytical method Methods 0.000 description 12
- 230000009466 transformation Effects 0.000 description 11
- 238000005516 engineering process Methods 0.000 description 6
- 238000004088 simulation Methods 0.000 description 6
- 230000033001 locomotion Effects 0.000 description 5
- 239000011159 matrix material Substances 0.000 description 5
- 238000012800 visualization Methods 0.000 description 5
- 230000002146 bilateral effect Effects 0.000 description 3
- 239000003795 chemical substances by application Substances 0.000 description 3
- 230000000694 effects Effects 0.000 description 3
- 230000001133 acceleration Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- 241001465754 Metazoa Species 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000006073 displacement reaction Methods 0.000 description 1
- 230000008030 elimination Effects 0.000 description 1
- 238000003379 elimination reaction Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000007500 overflow downdraw method Methods 0.000 description 1
- 230000000750 progressive effect Effects 0.000 description 1
- 238000005295 random walk Methods 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
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
- 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
-
- 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
- G01S5/0258—Hybrid positioning by combining or switching between measurements derived from different systems
- G01S5/02585—Hybrid positioning by combining or switching between measurements derived from different systems at least one of the measurements being a non-radio measurement
-
- 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
- G01S5/0263—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems
- G01S5/0264—Hybrid positioning by combining or switching between positions derived from two or more separate positioning systems at least one of the systems being a non-radio wave positioning system
-
- 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
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Image Analysis (AREA)
- Image Processing (AREA)
- Navigation (AREA)
Abstract
本发明涉及一种基于多源传感器融合的室内定位方法及系统。将IMU数据和UWB数据与相机关键帧对齐，降低传感器数据预处理误差；改进VINS‑MONO算法，构建VIO融合残差项，提高VIO里程计的定位精度；引入UWB的闭环检测项，通过闭环检测与优化，纠正测量噪声；将UWB非关键帧数据与IMU数据融合，且UWB定位估计与VIO里程计定位估计相互约束，纠正在定位过程中单一传感器较大的误差，避免当前累计误差对后续定位的影响；本发明将相机、IMU、UWB多源传感器信息融合起来，降低环境条件、光照强度、光照均匀性、本体高机动性、累计误差等因素对VIO里程计定位精度的影响，同时降低高斯白噪声对UWB定位精度的影响，从而形成一种优势互补的机制，可以实现长期、鲁棒、高精度的定位。
Description
技术领域
本发明涉及无人系统定位领域，特别是涉及一种基于多源传感器融合的室内定位方法及系统。
背景介绍
在完全未知的环境下，智能体首先通过定位和感知技术来确定其在环境中的位置，即解决“机器人在哪里”的问题，得到机器人在环境中的位置，同时预测其他智能体的位置和运动。定位是智能体执行任务的第一阶段，因此准确的定位信息是高效完成规划和控制的必要条件。最早期的人类甚至是一些低等动物可以使用地形匹配的方式进行定位和导航。在16世纪，望远镜、测绘仪器等一些观测手段的出现，可以通过测量的方式实现定位。现代可以通过卫星得到地球上任意一处的位置。定位的终极目标是不依赖系统外部的定位设备，实现可靠的全自主的定位导航。在室内或者是大场景复杂环境下，动捕系统和卫星定位都无法工作，那么系统的自主性和灵活性都会受到限制。定位的本质就是利用环境中不变的或已有的信息来确定机器人的位置和方向。
VIO(Visual-Inertial Odometry)视觉惯性里程计是一种基于相机和IMU(Inertial Measurement Unit)惯性测量单元融合的定位方法。可独立工作在无外部定位系统辅助的未知环境、相对于传统的里程计定位方法精度较高。基于视觉的定位技术已经有30年的研究历史，但是由于在实际应用中还有许多感知的不确定性，比如会受到环境条件、光照强度、光照的均匀性、机器人本体的高机动性的影响。例如，在高机动的场景下，相机会产生运动模糊和运动畸变。此外，虽然IMU可以提供准确的两帧之间的相对位移，但是IMU的陀螺仪和加速度计存在漂移，无法实现长期的精确定位。
UWB(Ultra Wide Band)超宽带技术是一种新型的无线通信技术，通过接收和发送具有纳秒级以下的极窄脉冲来传输数据，具有GHz量级的宽带。UWB定位技术具有实时性，定位延时远远小于蓝牙、WIFI等其他无线定位技术，定位精度可以达到10cm左右。UWB的工作原理为通过计算标签与基站之间无线信号飞行时间差，得到标签与基站之间的距离，使用DS-TWR算法解算标签在基站坐标系中的位置。UWB定位技术没有累计误差，可以实现长期的定位，缺陷在于UWB测距带有高斯白噪声，无法人为去除噪声，导致UWB定位轨迹不光滑，定位精度也会受到影响。
基于单一的传感器数据进行定位在精度和应用场景有较大限制，许多学者将多源传感器信息融合起来形成一种优势互补的机制，以达到提升定位性能的目的。多源传感器采集数据的频率不同，例如，相机频率一般为30Hz，UWB频率一般为50Hz，IMU频率一般为100Hz，目前多数研究成果认为多源传感器采集到的数据与相机的关键帧对齐，但是在实际应用中，由于传感器的上电时间不同、采样频率不同、环境变化等因素的影响，传感器采集数据的频率会发生微小的波动，无法精确保证传感器数据对齐；此外，现有方法只利用与相机关键帧对齐的UWB数据，其他UWB数据视为无效数据，导致数据利用率低，无法充分挖掘传感器数据价值，不能保证更精确的定位效果。综上所述，研究多源传感器融合的室内定位方法及系统不仅具有理论意义，更加具有工程实际意义。
发明内容
本发明目的是提供一种基于多源传感器融合的室内定位方法及系统，充分利用多源传感器数据实现长期、鲁棒、精确的室内定位。
为实现上述目的，本发明提供了如下方案：
一种基于多源传感器融合的室内定位方法，包括：
对相机、惯性测量单元IMU、UWB采集到的数据进行预处理，包括对IMU关键帧对齐，并对对齐后的IMU数据预积分，以及对UWB前端测距并对UWB关键帧对齐；
根据处理后的相机数据、IMU数据基于VINS-MONO构建VIO里程计估计残差；并改进VINS-MONO算法引入VIO融合残差项；
根据处理后的UWB数据，引入闭环检测及优化构建UWB定位估计残差；
使用非线性优化融合对VIO里程计估计残差和UWB定位估计残差进行融合，并最终得到融合定位结果；
可选地，IMU数据预处理包括IMU关键帧对齐和预积分处理，具体包括：
所述的IMU关键帧对齐是利用插值法将IMU采集到的数据与相机的关键帧对齐：
利用公式插值计算得到第k个相机关键帧对应的IMU数据，其中，Zk表示与第k个相机关键帧对应的IMU数据，Zs、Zs+1分别表示第s时刻、第s+1时刻IMU的测量值，tk表示第k个相机关键帧的时间戳，ts、ts+1分别表示第s时刻、第s+1时刻IMU测量的时间戳，满足ts＜tk＜ts+1；
所述的预积分处理事根据所述的IMU关键帧对齐得到的IMU数据进行预积分操作，从而根据IMU数据得到两个相邻关键帧之间的位姿变化；
可选地，UWB数据预处理包括UWB前端测距和UWB关键帧对齐，具体包括：
所述的UWB前端测距是通过已知的基站坐标以及标签到基站的距离计算标签在基站坐标系下的绝对位置：
利用公式得到u时刻标签在基站坐标系下的位置Lu；
其中，Lu表示u时刻标签在基站坐标系下的位置，表示第a(a∈{1,2,3,4})个基站在基站坐标系下的坐标，/>表示u时刻标签到第a个基站的距离；
利用公式error(Lu,Lu+1)＝||Lu+1-Lu||2确定相邻两时刻的UWB测量数据差，根据UWB采样频率恒定的工作原理，若error(Lu,Lu+1)＞σ说明该相邻两时刻的UWB数据噪声较大，则认为该相邻两时刻的UWB数据无效，σ为增益系数，表示相邻两时刻的UWB测量数据差的最大容忍值，可根据需要设置；
所述的UWB关键帧对齐是利用插值法将UWB采集到的数据与相机的关键帧对齐：
利用公式插值计算得到第k个相机关键帧对应的UWB数据；其中，Lk表示与第k个相机关键帧对应的UWB数据，Lu、Lu+1分别表示第u时刻、第u+1时刻UWB定位估计结果，tk表示第k个相机关键帧的时间戳，tu、tu+1分别表示第u时刻、第u+1时刻UWB的时间戳，满足tu＜tk＜tu+1；
可选地，所述的构建VIO里程计估计残差是基于VINS-MONO构建视觉重投影残差项、构建IMU约束残差项、构建边缘化先验信息残差项，同时改进VINS-MONO以构建VIO融合残差项；其中VINS-MONO算法具体可参见ARobust and Versatile Monocular Visual-Inertial State Estimator IEEE TRANSACTIONS ON ROBOTICS,2018，Tong Qin，PeiliangLi，Shaojie Shen；
可选地，所述的构建视觉重投影残差项具体包括：
根据相机的关键帧数据，采用VINS-MONO前端检测并跟踪图像中的特征点，利用Sfm算法实现基于图像的运动估计，并采用VINS-MONO后端闭环检测，将当前特征点与历史图像的特征点进行匹配，从而构建视觉重投影残差
其中，为第l个路标点在第j个相机坐标系中的坐标观测值；X为待优化的状态向量，/>为正切平面上的任意两个正交基，/>为第l个路标点在第j个归一化相机坐标系中的估计值，/>为第l个路标点在第j个归一化相机坐标系中的观测值；
可选地，根据IMU数据预处理得到的数据构建IMU约束残差项；
可选地，所述的构建VIO融合残差项具体包括：
采用VINS-MONO融合所述的视觉重投影残差项和IMU约束残差项，得到VIO里程计估计结果，从而构建VIO融合残差
其中，分别为VIO里程计在第k和第k+1帧时估计的定位结果，xk、xk+1分别为在第k和第k+1帧时待估计的状态向量，/>为第k到第k+1帧之间的相对位姿变换；
可选地，所述的构建UWB定位估计残差包括构建UWB后端优化残差项、构建UWB约束残差项、构建UWB非关键帧残差项；
可选地，所述的构建UWB后端优化残差项包含UWB闭环优化，具体包括：
利用公式对构成闭环的第u时刻、第u+m时刻UWB定位估计结果Lu、Lu+m进行优化；
其中，Lu、Lu+m分别表示第u时刻、第u+m时刻UWB定位估计结果，表示u时刻标签到第a个基站的距离，/>表示u+m时刻标签到第a个基站的距离，θa表示u时刻、u+m时刻标签与第a个基站连线的夹角；
若所述的构成闭环的第u时刻或第u+m时刻UWB的采样数据与相机关键帧紧邻，则根据所述的UWB关键帧对齐重新将闭环优化后的UWB数据与相机的关键帧对齐；
根据所述的UWB关键帧对齐与所述的UWB闭环优化，得到UWB定位估计结果，从而构建UWB后端优化残差其中，/>表示待优化状态向量的前三维，即位置状态向量；Lk表示与第k个相机关键帧对应的UWB数据；
可选地，所述的构建UWB约束残差项具体包括：
根据所述的VIO里程计估计结果和所述的UWB定位估计结果，构建UWB约束残差项
其中，分别为VIO里程计估计的定位结果/>的前三维，表示位置状态向量，Lk+1表示与第k+1个相机关键帧对应的UWB数据；
利用公式约束所述的VIO里程计估计结果和所述的UWB定位估计结果；
其中，ε为超参数；
可选地，所述的构建UWB非关键帧残差项具体包括：
根据所述的IMU关键帧对齐插值计算得到第kh个UWB数据对应的IMU数据，并通过所述的预积分处理对第k个相机关键帧对应的IMU数据到第kh个UWB数据对应的IMU数据进行预积分操作，从而构建UWB非关键帧残差项：
其中，为待优化状态向量的前三维，即位置状态向量，/>为IMU定位估计的前三维，即位置状态向量，/>为第kh时刻UWB的定位估计；/>为根据第kh个UWB数据对应的IMU数据得到的IMU的定位估计；
可选地，根据所述的VIO里程计估计残差和UWB定位估计残差进行非线性优化融合，所述的非线性优化融合的优化方程为：
其中，为VIO里程计估计残差，是视觉重投影残差优化项、IMU约束残差优化项、边缘化先验信息残差优化项、VIO融合残差优化项的总和，PV为VIO里程计估计残差的协方差，/>为UWB定位估计残差，是UWB后端优化残差优化项、UWB约束残差优化项、UWB非关键帧残差优化项的总和，PU为UWB定位估计残差的协方差，用于表示权重，权重越大代表该残差项对最终的优化方程影响程度越大；
本发明还提供一种用于实现上述方法的基于多源传感器融合的室内定位系统，包括：
传感器数据预处理模块，对相机、惯性测量单元IMU、UWB采集到的数据进行预处理，具体包括IMU关键帧对齐模块、预积分模块、UWB前端测距模块和UWB关键帧对齐模块；
VIO里程计估计残差模块，用于构建VIO里程计估计残差项，具体包括视觉重投影残差项、IMU约束残差项、边缘化先验信息残差项、VIO融合残差项；
UWB定位估计残差模块，用于构建UWB定位估计残差项，具体包括UWB后端优化残差项、UWB约束残差项、UWB非关键帧残差项；
非线性优化融合模块，用于对所述的VIO里程计估计残差和所述的UWB定位估计残差进行融合，并解算得到融合定位结果。
根据本发明提供的具体实施例，发明公开了以下技术效果：
本发明提供的一种基于多源传感器融合的室内定位方法及系统，分别将IMU数据和UWB数据与相机关键帧对齐，降低传感器数据预处理误差；改进VINS-MONO算法，构建VIO融合残差项，提高VIO里程计的定位精度；验证相邻两时刻的UWB数据的可靠性，防止UWB噪声过大影响定位精度；引入UWB的闭环检测项，若当前时刻的UWB数据与历史上某一时刻的UWB数据形成闭环，则通过闭环检测与优化模块，纠正测量噪声；将UWB非关键帧数据与IMU数据融合，以充分利用采集到的UWB数据，提高数据的利用率；UWB定位估计与VIO里程计定位估计相互约束，纠正在定位过程中单一传感器较大的误差，避免当前累计误差对后续定位的影响；本发明将相机、IMU、UWB多源传感器信息融合起来，降低环境条件、光照强度、光照的均匀性、本体的高机动性、累计误差等因素对VIO里程计定位精度的影响，同时降低高斯白噪声对UWB定位精度的影响，从而形成一种优势互补的机制，可以实现长期、鲁棒、高精度的定位。
附图说明
为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图做简单的介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，再不付出创造性的前提下，还可以根据这些附图获得其他的附图。
图1为本发明所提供的一种基于多源传感器融合的室内定位方法及系统流程示意图；
图2为多源传感器数据采集时间戳示意图；
图3为UWB前端测距示意图，其中，图3(a)表示UWB双边双向测距算法示意图，图3(b)表示确定标签在基站坐标系下的位置示意图；
图4为VIO状态估计误差示意图；
图5为UWB后端优化示意图，其中，图5(a)表示UWB闭环检测与优化示意图，图5(b)表示UWB状态估计误差示意图；
图6为UWB定位估计结果约束VIO里程计估计示意图；
图7为UWB非关键帧定位估计示意图；
图8为本发明所提供的一种基于多源传感器融合的室内定位方法及系统框架示意图；
图9为Machine Hall 01场景定位误差可视化分析，其中，图9(a)为UWB定位估计误差分析示意图，图9(b)为VIO里程计估计误差分析示意图，图9(c)为本发明提出的基于多源传感器融合的定位估计误差分析示意图，图9(d)为UWB定位轨迹，图9(e)为VIO里程计定位轨迹，图9(f)为基于多源传感器融合方法的定位轨迹；
图10为Machine Hall 01场景定位误差对比示意图；
图11为EuRoc数据集仿真场景的轨迹可视化。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明的目的是提供一种基于多源传感器融合的室内定位方法及系统，充分利用多源传感器数据实现长期、鲁棒、精确的室内定位。
为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细说明。
图1为本发明所提供的一种基于多源传感器融合的室内定位方法及系统流程示意图，如图1所示，本发明所提供的一种基于多源传感器融合的室内定位方法及系统，包括：
S101，传感器数据预处理模块。所述的传感器数据预处理模块用于对相机、惯性测量单元IMU(Inertial Measurement Unit)、UWB(Ultra Wide Band)采集到的数据进行预处理。所述的数据预处理模块包括IMU数据预处理模块、UWB数据预处理模块。
IMU数据预处理模块包括IMU关键帧对齐模块和预积分模块，具体包括：
利用公式插值计算得到第k个相机关键帧对应的IMU数据，从而将IMU数据与第k个相机关键帧对齐。其中，Zk表示与第k个相机关键帧对应的IMU数据，Zs、Zs+1分别表示第s时刻、第s+1时刻IMU的测量值，tk表示第k个相机关键帧的时间戳，ts、ts+1分别表示第s时刻、第s+1时刻IMU测量的时间戳，满足ts＜tk＜ts+1。
所述的预积分模块根据所述的IMU关键帧对齐模块得到的IMU数据进行预积分操作，从而根据IMU数据得到两个相邻关键帧之间的位姿变化；即第k到第k+1个相机关键帧之间的IMU预积分可以表示为：
其中，分别表示加速度计和陀螺仪的测量值，/>分别表示第k个相机关键帧相对于第k+1个相机关键帧的位置、速度、方位角，/>表示t时刻位置到第k个相机关键帧处位置的旋转矩阵，/>分别表示加速度计和陀螺仪的偏移。
在实际应用过程中，考虑到IMU采集到的数据是离散的，同时为了降低计算复杂度，采用基于中值法的离散IMU预积分公式：
其中，/>
其中，分别表示加速度计和陀螺仪的测量值，/> 分别表示第s+1个IMU数据位置相对于第k个相机关键帧的位置、速度、方位角，/>分别表示第s个IMU数据位置相对于第k个相机关键帧的位置、速度、方位角，/>表示第s+1个IMU数据位置相对于第s个IMU数据位置的方位角，/>分别表示加速度计和陀螺仪的偏移，δt表示两相邻IMU数据之间时间间隔，/>分别为两相邻IMU数据间的平均加速度、平均角速度，qs表示第s个IMU坐标系到第k个相机关键帧坐标系的旋转变换，qs+1表示第s+1个IMU坐标系到第k个相机关键帧坐标系的旋转变化。
通过中值法离散IMU预积分公式不断迭代，即可得到第k+1个相机关键帧坐标系相对于第k个相机关键帧坐标系的位置速度/>方位角/>
图2所示为多源传感器数据采集时间戳示意图，由于各种传感器采样频率不同，三种传感器在关键帧位置处的数据不能完全对齐，为保证精确定位，需提供对齐的传感器数据。tk表示第k个相机关键帧的时间戳，在第k个相机关键帧两侧取邻近的两个IMU数据，对应的时间戳分别为ts、ts+1，通过所述的IMU关键帧对齐模块，将IMU数据与相机关键帧位置的数据对齐。
UWB数据预处理模块包括UWB前端测距模块和UWB关键帧对齐模块，具体包括：
采用双边双向测距算法得到u时刻标签到基站的距离基站主动发送第一次测距消息，标签响应，当基站收到标签反馈的消息之后再返回数据，最终得到4个时间差Tround1 Treply1 Tround2 Treply2，基于4个时间差数据就可以得到无限信号在基站到标签之间的飞行时间，再乘以光速c，就可以得到基站到标签之间的距离：
其中，c表示光速。
利用公式得到u时刻标签在基站坐标系下的位置Lu。
其中，Lu表示u时刻标签在基站坐标系下的位置，表示第a(a∈{1,2,3,4})个基站在基站坐标系下的坐标，/>表示u时刻标签到第a个基站的距离。
利用公式error(Lu,Lu+1)＝||Lu+1-Lu||2确定相邻两时刻的UWB测量数据差，根据UWB采样频率恒定的工作原理，若error(Lu,Lu+1)＞σ说明该相邻两时刻的UWB数据噪声较大，则认为该相邻两时刻的UWB数据无效。
如图3所示为UWB前端测距示意图。其中，采用双边双向测距算法对基站与标签之间进行测距，图3(a)表示UWB双边双向测距算法示意图。图3(b)表示确定标签在基站坐标系下的位置示意图。在完成基站部署后，以编号为0的基站作为基站坐标系的原点，通过标定可以得到各基站在基站坐标系下的坐标，通过UWB前端测距模块优化即可得到标签在基站坐标系下的坐标。
所述的UWB关键帧对齐模块利用插值法将UWB采集到的数据与相机的关键帧对齐：
利用公式插值计算得到第k个相机关键帧对应的UWB数据，从而将UWB数据与第k个相机关键帧对齐；其中，Lk表示与第k个相机关键帧对应的UWB数据，Lu、Lu+1分别表示第u时刻、第u+1时刻UWB定位估计结果，tk表示第k个相机关键帧的时间戳，tu、tu+1分别表示第u时刻、第u+1时刻UWB的时间戳，满足tu＜tk＜tu+1。
同理，如图2所示，由于各种传感器采样频率不同，三种传感器在关键帧位置处的数据不能完全对齐，为保证精确定位，需提供与相机关键帧对齐的UWB数据。在第k个相机关键帧两侧取邻近的两个UWB数据，对应的时间戳分别为tu、tu+1，通过所述的UWB关键帧对齐模块，UWB数据与相机关键帧位置的数据对齐。
S102，构建VIO里程计估计残差模块，包括构建视觉重投影残差项、构建IMU约束残差项、构建边缘化先验信息残差项、构建VIO融合残差项。
S102具体包括：
构建视觉重投影残差项。根据相机的关键帧数据，采用VINS-MONO前端检测并跟踪图像中的特征点，利用Sfm算法实现基于图像的运动估计，采用VINS-MONO后端闭环检测，将当前特征点与历史图像的特征点进行匹配，从而构建视觉重投影残差：
其中，X为待优化的状态向量， 表示第k个相机关键帧对应的IMU坐标系在世界坐标系下的位置，/>表示第k个相机关键帧对应的IMU坐标系在世界坐标系下的速度，/>表示第k个相机关键帧对应的IMU坐标系到世界坐标系的旋转变换，ba表示当前关键帧下加速度计的偏移，bg表示当前关键帧下陀螺仪的偏移；共n+1个待优化的时刻即待优化的关键帧；/>为相机到IMU的坐标变换，λ为特征点的逆深度，/>为正切平面上的任意两个正交基，/>为第l个路标点在第j个归一化相机坐标系中的坐标估计值，物理意义为对于第l个路标点P，将路标点P从第一次观看到它的第i个相机坐标系转换到当前的第j个相机坐标系下得到的像素坐标，/>为第l个路标点在第j个归一化相机坐标系中的坐标观测值，/>为IMU到相机的旋转变换，/>为相机到IMU的旋转变换，满足/>为世界坐标系到第j个相机坐标系的旋转变化，/>为第i个相机坐标系到世界坐标系的旋转变化，/>为相机到IMU的平移变换。
视觉重投影残差的协方差与标定相机内参时的重投影误差相关，定义为：
其中，f为相机的焦距，认为相机内参的重投影误差偏移1.5个像素；
S102具体还包括：
构建IMU约束残差项，
其中，δba、δbg分别表示第k个到k+1个关键帧间位置、角度、速度、加速度计偏移、陀螺仪偏移的误差，/>表示世界坐标系ω到第k个关键帧坐标系的旋转变换，/>分别表示第k帧和第k+1帧坐标系相对于世界坐标系ω的位置，/>分别表示第k帧和第k+1帧坐标系相对于世界坐标系ω的速度，gw为重力加速度，Δtk为两关键帧之间的时间间隔，/>分别表示第k帧和第k+1帧坐标系相对于世界坐标系ω的旋转变换，/>分别表示第k帧和第k+1帧下加速度计的偏移，/>分别表示第k帧和第k+1帧下陀螺仪的偏移。
针对离散形式IMU预积分误差，满足δzk+1＝FB·δzk+VBQB，其中，表示第k+1个到k+2个关键帧间位置、角度、速度、加速度计偏移、陀螺仪偏移的误差向量，/>表示第k个到k+1个关键帧间位置、角度、速度、加速度计偏移、陀螺仪偏移的误差向量，/> 表示噪声项对角协方差矩阵。
根据IMU预积分误差迭代公式，构建IMU约束残差项的协方差迭代公式为其中，初始值/>
S102具体还包括：
构建边缘化先验信息残差项。在优化过程中，会将滑动窗口内某些较旧或者不满足要求的相机帧剔除，边缘化的目标是不再计算被剔除的相机帧的位姿，但是保留该帧对滑动窗口内其他帧的约束关系，降低计算量的同时不会减少约束信息，避免对定位精度产生较大影响。
根据高斯牛顿法的非线性优化理论，高斯牛顿法的增量方程为Hδx＝g，待优化的变量δx＝[δxa,δxb]T，认为δxa为被剔除的变量，δxb为实际被优化的变量，g＝[ga,gb]T，因此高斯牛顿的增量方程可以写成如下形式：
为了保留变量δxb而不再优化δxa，采用Schur补公式消元：
即有从而通过Schur补公式消元可以保留待优化的变量δxb；
其中，表示高斯牛顿法中的海塞矩阵。
构建边缘化先验信息残差项errormarg＝rp-HpX，其中rp、Hp为边缘化之后形成的先验信息。
S102具体还包括：
构建VIO融合残差项。根据视觉重投影残差项、IMU约束残差项、边缘化先验信息残差项，采用VINS-MONO可以初步得到VIO里程计估计，第k和第k+1帧时VIO里程计估计的定位结果分别为则根据VIO里程计估计的定位结果可得两帧之间的相对位姿变换为
从理论分析上应有其中，xk、xk+1分别为在第k和第k+1帧时待估计的状态向量，即第k帧下的状态向量乘以第k和第k+1帧之间的相对位姿变换应严格等于第k+1帧下的状态向量。但是由于VIO里程计估计存在误差，理论分析得到的结果并不严格成立，从而构建VIO融合残差/>
针对VIO状态估计误差，满足δxk+1＝FF·δxk+VFQF，其中，表示第k+1个关键帧处的状态误差，表示第k个关键帧处的状态误差，/>
根据VIO状态估计误差迭代公式，构建VIO融合残差项的协方差迭代公式为其中，初始值/>
如图4所示为VIO状态估计误差示意图。
S103，构建UWB定位估计残差模块，包括构建UWB后端优化残差项、构建UWB约束残差项、构建UWB非关键帧残差项。
S103具体包括：
构建UWB后端优化残差项。通过UWB闭环检测将当前u+m时刻的UWB定位估计结果Lu+m与历史上第u时刻的UWB定位估计结果Lu构建起闭环约束，可形成UWB闭环约束的条件为：||Lu+m-Lu||2≤η，从而对前后两时刻的UWB定位估计结果进行优化，提升整体的定位性能。
利用公式对构成闭环的第u时刻、第u+m时刻UWB定位估计结果Lu、Lu+m进行优化；
其中，Lu、Lu+m分别表示第u时刻、第u+m时刻UWB定位估计结果，表示u时刻标签到第a个基站的距离，/>表示u+m时刻标签到第a个基站的距离，θa表示u时刻、u+m时刻标签与基站a连线的夹角。
如图5所示为UWB后端优化示意图。其中，图5(a)表示UWB闭环检测与优化示意图。在定位过程中，通过闭环检测发现当前时刻标签在基站坐标系下的坐标与历史上某一时刻标签在基站坐标系下的坐标满足UWB闭环约束的条件，那么这两时刻的UWB数据可进行闭环优化，提高UWB定位估计结果的可靠性，降低UWB测量噪声对定位精确的影响。
若所述的构成闭环的第u时刻或第u+m时刻UWB的采样数据与相机关键帧紧邻，则根据所述的UWB关键帧对齐模块重新将闭环优化后的UWB数据与相机的关键帧对齐，即利用公式重新计算第k个相机关键帧对应的UWB数据。
根据所述的UWB关键帧对齐模块与所述的UWB闭环检测与优化，得到优化后的UWB定位估计结果，从而构建UWB后端优化残差其中，/>表示待优化状态向量的前三维，即位置状态向量。
UWB后端优化残差的协方差与UWB硬件设备测量误差的协方差有关，定义为ΩUWB。
图5(b)表示UWB状态估计误差示意图。UWB定位估计结果理论上应等于待优化的真实状态，但由于系统存在漂移、随机游走等误差，导致UWB定位估计结果并不严格等于待优化的真实状态。因此通过构建UWB后端优化残差rOU来提高多源传感器融合的定位精度。
S103具体还包括：
构建UWB约束残差项。在S102中，采用VINS-MONO根据视觉重投影残差项、IMU约束残差项可以初步得到VIO里程计估计，第k和第k+1帧时VIO里程计估计的定位结果分别为在S103中，根据所述的UWB关键帧对齐模块与所述的UWB闭环检测与优化，可以得到优化后的UWB定位估计结果，第k和第k+1帧时UWB定位估计结果分别为Lk、Lk+1。考虑到VIO在长期定位过程中由于IMU的累计误差会出现严重的漂移，UWB在定位过程中仅会受到随机测量噪声的影响，并没有累计误差，因此为了防止当前过大的VIO里程计累计漂移误差对后续定位结果的影响，采用UWB定位估计结果约束VIO里程计估计。
根据所述的VIO里程计估计结果和所述的UWB定位估计结果，构建UWB约束残差项
其中，分别为VIO里程计估计的定位结果/>的前三维，表示位置状态向量。
利用公式约束所述的VIO里程计估计结果和所述的UWB定位估计结果。
即当时，表示由VIO里程计估计的第k和第k+1帧之间的状态变换与由UWB定位估计的第k和第k+1帧之间的状态变换相差较大，由于UWB不会出现累计误差，认为UWB定位估计结果可信度更高，因此丢弃原有的VIO里程计估计结果/>并将UWB定位估计结果Lk+1赋值给VIO里程计估计结果，即/>与此同时，为了防止UWB约束残差项rCU过大而影响其他残差项的优化，令/>将UWB约束残差项rCU约束在一定范围内。
其中，ε为超参数。
如图6所示为UWB定位估计结果约束VIO里程计估计示意图。当检测到VIO里程计估计结果和UWB定位估计结果相差较大时，即认为此时VIO里程计估计结果产生较大的累计误差，由于UWB定位不会出现累计误差因而可信度更高，即用UWB定位估计结果代替VIO里程计估计结果，VIO里程计从当前代替值开始继续进行定位估计，从而避免VIO产生更大的累计误差。
S103具体还包括：
构建UWB非关键帧残差项。多源传感器采集数据的频率不同，例如，相机频率一般为30Hz，UWB频率一般为50Hz，IMU频率一般为100Hz，在实际应用中，通常从帧率最低的传感器采集的数据中选取关键帧，即从图像数据中选取关键帧。所述的传感器数据预处理模块中，在相机关键帧两侧取邻近的两个UWB数据，通过所述的UWB关键帧对齐模块，将所取的两个UWB数据与相机关键帧位置的数据对齐，然而非相机关键帧两侧邻近的UWB数据将被视为无效数据而丢弃。为了提高UWB采样数据的利用率，构建UWB非关键帧残差项充分挖掘UWB数据价值以提供更精确的定位效果。
根据所述的IMU关键帧对齐模块插值计算得到第kh个UWB数据对应的IMU数据，并通过所述的预积分模块对第k个相机关键帧对应的IMU数据到第kh个UWB数据对应的IMU数据进行预积分操作，从而构建UWB非关键帧残差项：
/>
其中，为待优化状态向量的前三维，即位置状态向量，/>为IMU定位估计的前三维，即位置状态向量，/>为第kh时刻UWB的定位估计。
UWB非关键帧残差项的协方差与UWB硬件设备测量误差的协方差有关，定义为ΩUWB。
如图7所示为UWB非关键帧定位估计示意图。为了将UWB非关键帧数据用于定位优化，提高数据的利用率，利用插值法计算UWB非关键帧数据对应的IMU数据，利用所得的IMU数据通过预积分模块求得当前非位置到上一个关键帧之间的坐标变换，并与UWB数据计算得到的当前位置到上一个关键帧之间的坐标变换作差，用于优化上一个关键帧位置的状态。
S104，非线性优化融合模块。为了利用所述的各项残差求得基于多源传感器融合的定位估计，将所述的VIO里程计估计残差和UWB定位估计残差融合起来，构建非线性优化融合模块。所述的非线性优化融合模块的优化方程为：
其中，X为待优化的状态向量，
为相机到IMU的坐标变换，λ为特征点的逆深度，其中，/>为VIO里程计估计残差，PV为VIO里程计估计残差的协方差，为UWB定位估计残差，PU为UWB定位估计残差的协方差，PV、PU用于表示权重，权重越大代表该残差项对最终的优化方程影响程度越大。
S104具体包括：
VIO里程计估计残差优化项由视觉重投影残差优化项、IMU约束残差优化项、边缘化先验信息残差优化项、VIO融合残差优化项构成，即有：
其中，ΩVIS为视觉重投影残差的协方差，PB为IMU约束残差项的协方差，PF为VIO融合残差项的协方差。
S104具体还包括：
UWB定位估计残差优化项由UWB后端优化残差优化项、UWB约束残差优化项、UWB非关键帧残差优化项构成，即有：
其中，ΩUWB为UWB硬件设备测量误差的协方差。
因此，所述的非线性优化融合模块的优化方程可表达为：
根据高斯牛顿法，若要计算目标函数的最小值，可以理解为当优化变量有一个增量ΔX后，目标函数值最小。以IMU约束残差优化项为例，可等价于如下形式：
其中/>为误差项rB关于xk、xk+1的雅克比矩阵，将上式展开并令关于ΔXk的导数为0，可得增量ΔXk的计算公式：
其中，为IMU约束残差项在第k个关键帧处的协方差，用于表示权重，/>越大，表示权重越大，则对最终的优化方程影响程度越大。
其他六项残差优化项同理，通过求解高斯牛顿法的增量方程，当增量ΔXk足够小时，则认为目标函数已取得最小值，否则，求解雅克比矩阵和协方差矩阵/>继续进行迭代直至ΔXk足够小。
图8所示为本发明所提供的一种基于多源传感器融合的室内定位系统框架示意图。该系统用于实现本发明的定位方法，首先通过传感器数据预处理模块对相机、惯性测量单元IMU、UWB采集到的数据进行预处理，将IMU数据与UWB数据与相机关键帧对齐，并对对齐后的IMU数据进行预积分操作；其次，根据处理后的相机数据和IMU数据构建VIO里程计估计残差模块，包括视觉重投影残差项、IMU测量残差项、边缘化先验信息残差项、VIO融合残差项，根据处理后的UWB数据构建UWB定位估计残差模块，包括UWB后端优化残差项、UWB约束残差项、UWB非关键帧残差项；最后，使用非线性优化融合模块对VIO里程计估计残差和UWB定位估计残差进行融合，并最终得到融合定位结果。
为验证本发明所提出方案的有效性，在EuRoc数据集下进行仿真实验。EuRoc数据集中包含易、中、难三种等级定位任务，覆盖长短距飞行、障碍物密集程度不同、光照条件不同等11个场景，是室内定位领域中常用的数据集之一。EuRoc数据集提供相机、IMU数据、位置真值数据，但是该数据集不包含UWB数据，本发明在EuRoc数据集提供的位置真值基础上施加一个高斯白噪声来模拟UWB数据。
以Machine Hall 01数据为例，Machine Hall 01场景因其飞行距离较短、光照条件良好、特征点充足而被归为简单类的定位任务。如图9所示为Machine Hall 01场景定位误差可视化分析，其中图9(a)为UWB定位估计误差分析示意图，图9(b)为VIO里程计估计误差分析示意图，图9(c)为本发明提出的基于多源传感器融合的定位估计误差分析示意图，其中，rmse为均方根误差，衡量预测值与真实值之间的偏差，std为标准偏差，衡量定位误差偏离误差平均值的程度，mean为误差平均值，median为误差中值，APE表示绝对位置误差，单位为m；图9(d)为UWB定位轨迹，图9(e)为VIO里程计定位轨迹，图9(f)为基于多源传感器融合的定位轨迹，轨迹图可以更直观的表示定位误差，轨迹图中的虚线表示真值。
如图10所示为Machine Hall 01场景定位误差对比示意图，其中，“uwb_vio_fusion”表示本发明所提出的基于多源传感器融合的定位估计方法误差曲线，“uwb_sim_pose”表示UWB定位估计误差曲线，“vio_result_loop”表示有闭环检测与优化的VIO里程计定位估计误差曲线。可以看出在室内环境理想、特征点充足、光照条件良好的情况下，本发明所提出的基于多源传感器融合的定位估计方法及系统相较于单一传感器而言可以提供更高的定位精度。为了充分验证本发明所提出方案的有效性，在EuRoc数据集提供的11种场景进行实验，表1为EuRoc数据集仿真场景的定位误差统计表。
表1：EuRoc数据集仿真场景的定位误差统计表(单位：m)
经过多组数据集测试，从表1中分析可得，本发明所提出的基于多源传感器融合定位估计结果在rmse、std、mean三个性能指标上均优于单一传感器的定位估计结果，在仿真测试下的平均定位误差保持在1cm-3cm之间。以VR2场景下的三个数据集为例，VR2_01被官方认定为简单类定位任务，VR2_02被认定为中等类定位任务，VR2_03被认定为困难类定位任务，根据本发明提出的多源传感器融合定位方法，VR2_01数据集的平均定位误差为0.0126m，VR2_02数据集的平均定位误差为0.0174m，VR2_03数据集的平均定位误差为0.0216m。可以发现，随着数据集定位任务难度的提升，所提出的多源传感器融合的定位方法的定位精度也随之降低，但融合定位精度仍高于单一传感器的定位精度。
为了更直观、清晰的表示不同定位方法对定位精度的影响，对VIO里程计定位、UWB定位、本发明提出的基于多源传感器融合定位方法进行轨迹可视化分析，在EuRoc数据集提供的11种场景进行分析，图11为EuRoc数据集仿真场景的轨迹可视化。
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。对于实施例公开的系统而言，由于其与实施例公开的方法相对应，相关之处参见方法部分说明即可。
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。
Claims (5)
1.一种基于多源传感器融合的室内定位方法，其特征在于，包括：
对相机、惯性测量单元IMU、UWB采集到的数据进行预处理，包括对IMU关键帧对齐，并对对齐后的IMU数据预积分，以及对UWB前端测距并对UWB关键帧对齐；
根据处理后的相机数据、IMU数据基于VINS-MONO算法构建VIO里程计估计残差；其中改进VINS-MONO算法以引入VIO融合残差项；所述的构建VIO里程计估计残差是基于VINS-MONO算法构建视觉重投影残差项、构建IMU约束残差项、构建边缘化先验信息残差项，并改进VINS-MONO算法引入VIO融合残差项；构建VIO融合残差项具体包括：
采用VINS-MONO融合所述的视觉重投影残差项和IMU约束残差项，得到VIO里程计估计结果，从而构建VIO融合残差
其中，分别为VIO里程计在第k和第k+1帧时估计的定位结果，X为待优化的状态向量，xk、xk+1分别为在第k和第k+1帧时待估计的状态向量，/>为第k到第k+1帧之间的相对位姿变换；
根据处理后的UWB数据，引入闭环检测及优化构建UWB定位估计残差；所述的构建UWB定位估计残差包括构建UWB后端优化残差项、构建UWB约束残差项、构建UWB非关键帧残差项；
所述的构建UWB后端优化残差项包含UWB闭环优化，具体包括：
利用公式对构成闭环的第u时刻、第u+m时刻UWB定位估计结果Lu、Lu+m进行优化；
其中，Lu、Lu+m分别表示第u时刻、第u+m时刻UWB定位估计结果，表示u时刻标签到第a个基站的距离，/>表示u+m时刻标签到第a个基站的距离，θa表示u时刻、u+m时刻标签与第a个基站连线的夹角；
若所述的构成闭环的第u时刻或第u+m时刻UWB的采样数据与相机关键帧紧邻，则根据所述的UWB关键帧对齐重新将闭环优化后的UWB数据与相机的关键帧对齐；
根据所述的UWB关键帧对齐与所述的UWB闭环优化，得到UWB定位估计结果，从而构建UWB后端优化残差其中，/>表示待优化状态向量的前三维，即位置状态向量；Lk表示与第k个相机关键帧对应的UWB数据；
所述的构建UWB约束残差项具体包括：
根据所述的VIO里程计估计结果和所述的UWB定位估计结果，构建UWB约束残差项
其中，分别为VIO里程计估计的定位结果/>的前三维，表示位置状态向量，Lk+1表示与第k+1个相机关键帧对应的UWB数据；
利用公式约束所述的VIO里程计估计结果和所述的UWB定位估计结果；
其中，ε为超参数；
所述的构建UWB非关键帧残差项具体包括：
根据所述的IMU关键帧对齐插值计算得到第kh个UWB数据对应的IMU数据，并通过所述的预积分对第k个相机关键帧对应的IMU数据到第kh个UWB数据对应的IMU数据进行预积分操作，从而构建UWB非关键帧残差项：
其中，为待优化状态向量的前三维，即位置状态向量，/>为IMU定位估计的前三维，即位置状态向量，/>为第kh时刻UWB的定位估计；
使用非线性优化融合对VIO里程计估计残差和UWB定位估计残差进行融合，并最终得到融合定位结果。
2.根据权利要求1所述的一种基于多源传感器融合的室内定位方法，其特征在于，对IMU数据的预处理包括IMU关键帧对齐和预积分处理，具体包括：
利用插值法将IMU采集到的数据与相机的关键帧对齐：
利用公式插值计算得到第k个相机关键帧对应的IMU数据，其中，Zk表示与第k个相机关键帧对应的IMU数据，Zs、Zs+1分别表示第s时刻、第s+1时刻IMU的测量值，tk表示第k个相机关键帧的时间戳，ts、ts+1分别表示第s时刻、第s+1时刻IMU测量的时间戳，满足ts＜tk＜ts+1；
对IMU关键帧对齐得到的IMU数据进行预积分操作，从而根据IMU数据得到两个相邻关键帧之间的位姿变化。
3.根据权利要求1所述的一种基于多源传感器融合的室内定位方法，其特征在于，对UWB数据的预处理包括UWB前端测距和UWB关键帧对齐，具体包括：
所述的UWB前端测距是通过已知的基站坐标以及标签到基站的距离计算标签在基站坐标系下的绝对位置：
利用公式得到u时刻标签在基站坐标系下的位置Lu；
其中，Lu表示u时刻标签在基站坐标系下的位置，表示第a个基站在基站坐标系下的坐标，a∈{1,2,3,4}，/>表示u时刻标签到第a个基站的距离；
利用公式error(Lu,Lu+1)＝||Lu+1-Lu||2确定相邻两时刻的UWB测量数据差，根据UWB采样频率恒定的工作原理，若error(Lu,Lu+1)＞σ说明该相邻两时刻的UWB数据噪声较大，则认为该相邻两时刻的UWB数据无效，σ为增益系数；
所述的UWB关键帧对齐是利用插值法将UWB采集到的数据与相机的关键帧对齐：
利用公式插值计算得到第k个相机关键帧对应的UWB数据；其中，Lk表示与第k个相机关键帧对应的UWB数据，Lu、Lu+1分别表示第u时刻、第u+1时刻UWB定位估计结果，tk表示第k个相机关键帧的时间戳，tu、tu+1分别表示第u时刻、第u+1时刻UWB的时间戳，满足tu＜tk＜tu+1。
4.根据权利要求1所述的一种基于多源传感器融合的室内定位方法，其特征在于，将所述的VIO里程计估计残差和UWB定位估计残差进行非线性优化融合，非线性优化融合的优化方程为：
其中，为VIO里程计估计残差，是视觉重投影残差项、IMU约束残差项、边缘化先验信息残差项、VIO融合残差项的总和，其中，/>为第l个路标点在第j个相机坐标系中的坐标观测值，/>分别为VIO里程计在第k和第k+1帧时估计的定位结果，X为待优化的状态向量，PV为VIO里程计估计残差的协方差；/>为UWB定位估计残差，是UWB后端优化残差项、UWB约束残差项、UWB非关键帧残差项的总和，其中，Lk、Lk+1分别表示与第k、k+1个相机关键帧对应的UWB数据，Lu、Lu+m分别表示第u时刻、第u+m时刻UWB定位估计结果，/>表示根据第kh个UWB数据对应的IMU数据得到的IMU定位估计，/>为第kh时刻UWB的定位估计，PU为UWB定位估计残差的协方差。
5.一种基于多源传感器融合的室内定位系统，其特征在于，用于实现如权利要求1-4任一项所述的方法，系统包括：
传感器数据预处理模块，对相机、惯性测量单元IMU、UWB采集到的数据进行预处理，具体包括IMU关键帧对齐模块、预积分模块、UWB前端测距模块和UWB关键帧对齐模块；
VIO里程计估计残差模块，用于构建VIO里程计估计残差项，具体包括视觉重投影残差项、IMU约束残差项、边缘化先验信息残差项、VIO融合残差项；
UWB定位估计残差模块，用于构建UWB定位估计残差项，具体包括UWB后端优化残差项、UWB约束残差项、UWB非关键帧残差项；
非线性优化融合模块，用于对所述的VIO里程计估计残差和所述的UWB定位估计残差进行融合，并解算得到融合定位结果。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310039739.2A CN116222556B (zh) | 2023-01-13 | 2023-01-13 | 一种基于多源传感器融合的室内定位方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310039739.2A CN116222556B (zh) | 2023-01-13 | 2023-01-13 | 一种基于多源传感器融合的室内定位方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116222556A CN116222556A (zh) | 2023-06-06 |
| CN116222556B true CN116222556B (zh) | 2024-03-26 |
Family
ID=86577902
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310039739.2A Active CN116222556B (zh) | 2023-01-13 | 2023-01-13 | 一种基于多源传感器融合的室内定位方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116222556B (zh) |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111739063A (zh) * | 2020-06-23 | 2020-10-02 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
| CN113721189A (zh) * | 2021-09-26 | 2021-11-30 | 中国科学院空天信息创新研究院 | 一种基于uwb和vio融合的多运动节点定位方法及系统 |
| CN113758488A (zh) * | 2021-09-27 | 2021-12-07 | 同济大学 | 一种基于uwb与vio的室内定位方法及设备 |
| WO2022156755A1 (zh) * | 2021-01-21 | 2022-07-28 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
| CN115183767A (zh) * | 2022-07-06 | 2022-10-14 | 辽宁工程技术大学 | 一种基于arkf的单目vio/uwb室内组合定位方法 |
-
2023
- 2023-01-13 CN CN202310039739.2A patent/CN116222556B/zh active Active
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111739063A (zh) * | 2020-06-23 | 2020-10-02 | 郑州大学 | 一种基于多传感器融合的电力巡检机器人定位方法 |
| WO2022156755A1 (zh) * | 2021-01-21 | 2022-07-28 | 深圳市普渡科技有限公司 | 一种室内定位方法、装置、设备和计算机可读存储介质 |
| CN113721189A (zh) * | 2021-09-26 | 2021-11-30 | 中国科学院空天信息创新研究院 | 一种基于uwb和vio融合的多运动节点定位方法及系统 |
| CN113758488A (zh) * | 2021-09-27 | 2021-12-07 | 同济大学 | 一种基于uwb与vio的室内定位方法及设备 |
| CN115183767A (zh) * | 2022-07-06 | 2022-10-14 | 辽宁工程技术大学 | 一种基于arkf的单目vio/uwb室内组合定位方法 |
Non-Patent Citations (2)
| Title |
|---|
| Jia-Rong Zhan 等.Improving Visual Inertial Odometry with UWB Positioning for UAV Indoor Navigation.IEEE：2022 26th International Conference on Pattern Recognition (ICPR).2022,全文. * |
| Sungjae Shin 等.MIR-VIO: Mutual Information Residual-based Visual Inertial Odometry with UWB Fusion for Robust Localization.IEEE：2021 The 21st International Conference on Control, Automation and Systems (ICCAS 2021).2021,全文. * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116222556A (zh) | 2023-06-06 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN112347840B (zh) | 视觉传感器激光雷达融合无人机定位与建图装置和方法 | |
| CN108731670B (zh) | 基于量测模型优化的惯性/视觉里程计组合导航定位方法 | |
| Tardif et al. | A new approach to vision-aided inertial navigation | |
| CN111288989B (zh) | 一种小型无人机视觉定位方法 | |
| CN110726406A (zh) | 一种改进的非线性优化单目惯导slam的方法 | |
| CN105509739A (zh) | 采用固定区间crts平滑的ins/uwb紧组合导航系统及方法 | |
| Cai et al. | Mobile robot localization using gps, imu and visual odometry | |
| CN114526745A (zh) | 一种紧耦合激光雷达和惯性里程计的建图方法及系统 | |
| CN112525197B (zh) | 基于图优化算法超宽带惯性导航融合位姿估计方法 | |
| CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
| CN115371665A (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
| CN115183762A (zh) | 一种机场仓库内外建图方法、系统、电子设备及介质 | |
| CN114964276A (zh) | 一种融合惯导的动态视觉slam方法 | |
| CN115930977A (zh) | 特征退化场景的定位方法、系统、电子设备和可读存介质 | |
| CN115218889A (zh) | 一种基于点线特征融合的多传感器室内定位方法 | |
| CN115574816A (zh) | 仿生视觉多源信息智能感知无人平台 | |
| Xian et al. | Fusing stereo camera and low-cost inertial measurement unit for autonomous navigation in a tightly-coupled approach | |
| CN112945233B (zh) | 一种全局无漂移的自主机器人同时定位与地图构建方法 | |
| CN113358117A (zh) | 一种利用地图的视觉惯性室内定位方法 | |
| CN112731503A (zh) | 一种基于前端紧耦合的位姿估计方法及系统 | |
| CN116222556B (zh) | 一种基于多源传感器融合的室内定位方法及系统 | |
| CN115540854A (zh) | 一种基于uwb辅助的主动定位方法、设备和介质 | |
| Gang et al. | Robust tightly coupled pose estimation based on monocular vision, inertia, and wheel speed | |
| CN112837374B (zh) | 一种空间定位方法及系统 | |
| CN114690230A (zh) | 一种基于视觉惯性slam的自动驾驶车辆导航方法 |
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 |