CN115183767A - 一种基于arkf的单目vio/uwb室内组合定位方法 - Google Patents

一种基于arkf的单目vio/uwb室内组合定位方法 Download PDF

Info

Publication number
CN115183767A
CN115183767A CN202210790610.0A CN202210790610A CN115183767A CN 115183767 A CN115183767 A CN 115183767A CN 202210790610 A CN202210790610 A CN 202210790610A CN 115183767 A CN115183767 A CN 115183767A
Authority
CN
China
Prior art keywords
uwb
time
vio
monocular
positioning
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
CN202210790610.0A
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.)
Liaoning Technical University
Original Assignee
Liaoning Technical 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 Liaoning Technical University filed Critical Liaoning Technical University
Priority to CN202210790610.0A priority Critical patent/CN115183767A/zh
Publication of CN115183767A publication Critical patent/CN115183767A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • 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
    • G01C21/1656—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 with passive imaging devices, e.g. cameras
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Position Fixing By Use Of Radio Waves (AREA)

Abstract

本发明公开了一种基于ARKF的单目VIO/UWB室内组合定位方法，属于导航定位技术领域。所述方法包括以下步骤：第一步，利用ROS时间标签实现组合系统中不同传感器的时间同步，并通过四参数转换模型完成组合系统坐标系统一；第二步，获取IMU、相机和UWB的量测信息；第三步，进行单目VIO位置解算；第四步，构建单目VIO/UWB组合系统的状态模型和量测模型；第五步，构建单目VIO/UWB组合系统抗差模型；第六步，利用ARKF解算组合系统的定位结果。本发明能够在不依赖回环检测的情况下有效解决单目VIO易漂移和定位误差较大的问题，并降低UWB的非视距误差对组合系统的影响，实现室内高精度定位。

Description

一种基于ARKF的单目VIO/UWB室内组合定位方法
技术领域
本发明属于导航定位技术领域，具体是一种基于ARKF的单目 VIO/UWB室内组合定位方法。
背景技术
随着时代的发展，人们对于定位精度的要求越来越高。人们 80％-90％的时间均处于室内，因此更多的定位导航和位置服务需求来自室内。室内环境复杂，采用单一传感器很难满足当下的定位需求，因此多传感器组合方式已经成为高精度定位领域的研究热点。由于相机和惯性测量单元(inertial measurement unit，IMU)传感器之间的优势互补性，基于不同融合方式的视觉惯性里程计(visual inertial odometry，VIO)已经成为解决此问题的主流方法。VIO的融合方式主要分基于滤波和基于优化两种方式：前者融合框架简单，但是线性化误差较大；后者定位精度更加精确，但是对硬件计算力要求较高。尽管VIO在短时间内能够获得较高的定位精度，但由于IMU本身定位易发散以及相机易受光照、纹理等因素的影响而导致跟踪失败，VIO自身误差会随着时间的推移而累积，进而导致定位结果的累积误差逐渐变大，因此需要进一步引入其它辅助传感器来降低VIO的定位误差。
目前常用的室内无线定位技术有UWB、Wi-Fi、超声波、蓝牙等，其中UWB定位技术因具有功率谱密度低、脉冲宽度窄、时间分辨率高的特点，能够获得较高的测距精度，从而被广泛应用于室内高精度定位。但UWB定位技术在脉冲信息传播的过程中，障碍物的存在会遮挡UWB 的脉冲信号，使信号在传播过程中发生反射、折射以及穿透的现象，构成非视距(none line of sight，NLOS)环境，形成NLOS误差，导致UWB 定位系统的精度和稳定性大幅度降低，严重时甚至无法测距定位。
综上分析可知，在室内环境下单独依靠VIO或者UWB进行定位很难获得高时效和高精度的定位结果，因此，近几年已有一些专家学者针对VIO和UWB的数据融合展开研究。2021年，Nguyen等人提出了一种VIO和UWB的紧耦合融合方案，将VIO的测量数据和UWB的量测数据进行融合，从而获得机器人的位姿。(参考：Nguyen T H,Nguyen T M,Xie L.Range-focused fusion of camera-IMU-UWB for accurate and drift-reduced localization[J].IEEE Robotics and Automation Letters,2021, 6(2):1678-1685.)2015年，Nyqvist等人提出了一种利用UWB辅助单目 VIO的方法，通过扩展卡尔曼滤波(extend Kalmanfilter，EKF)进行融合解算，以获得改进的无漂移全局位姿估计。(参考：Nyqvist H E,Skoglund M A,Hendeby G,et al.Pose estimation using monocular vision andinertial sensors aided with ultra wide band[C]//2015international conferenceon indoor positioning and indoor navigation(IPIN).IEEE,2015:1-10.)2022 年，申炳琦等人提出了一种双目VIO和UWB的松耦合方案，分别利用双目VIO输出的位置信息和UWB输出的位置信息，通过EKF进行数据融合得到机器人的最优位置估计，该方案采用的是松耦合的融合方式，组合系统的鲁棒性偏低。(参考：申炳琦,张志明,舒少龙.移动机器人UWB 与VIO组合室内定位算法[J].计算机应用，2022(1):1-8.)虽然上述三种融合方案在良好环境下都能获得较好的定位结果，但都并未充分考虑 UWB NLOS误差对组合系统的影响。为此，本发明提出了一种基于自适应抗差卡尔曼滤波(adaptive rosbust Kalman filter，ARKF)的单目 VIO/UWB室内组合定位方法。
发明内容
针对上述技术问题，本发明提出了一种新的单目VIO/UWB室内组合定位方法，以达到有效解决单目VIO在室内环境下易漂移和定位误差较大的问题，抑制UWB NLOS误差对组合系统的影响，提高组合系统鲁棒性和定位精度的目的。
为实现上述目的，本发明提供了一种基于ARKF的单目VIO/UWB 室内组合定位方法，包括如下步骤：
步骤1、利用ROS时间标签实现组合系统中不同传感器的时间同步，并通过四参数转换模型完成组合系统坐标系统一；
步骤2、获取IMU、相机和UWB的量测信息；
步骤3、进行单目VIO位置解算；
步骤4、构建单目VIO/UWB组合系统的状态模型和量测模型；
步骤5、构建单目VIO/UWB组合系统抗差模型；
步骤6、利用ARKF解算组合系统的定位结果。
所述步骤2中UWB采用基于往返时间(tow way-time of flight， TW-TOF)的测距模型，测距示意图如图2所示，具体公式如下：
Figure RE-GDA0003842566890000031
其中，d为UWB移动站与基准站之间的距离，c为光速，Tround为 UWB脉冲信号在UWB流动站与基准站之间的往返时间差，Trespond为 UWB基准站接收到脉冲信号到发射出响应脉冲信号的时间差，n为测量噪声，T1为UWB流动站发出脉冲信号的时间，T2为UWB基准站接收到脉冲信号的时间，T3为UWB基准站发出响应脉冲信号的时间，T4为UWB 流动站接收到响应脉冲信号的时间。
所述步骤3在步骤2获得的相机和IMU数据的基础上，利用VINS 框架实现单目VIO定位，定位流程如图3所示；
具体步骤如下：
步骤3-1、提取图像Harris角点，采用金字塔光流法跟踪相邻帧；
步骤3-2、利用IMU数据计算当前帧的位姿作为初始值，及相邻帧的预积分增量和预积分误差的雅可比矩阵及协方差；
步骤3-3、进行相机初始化和相机惯导联合初始化，计算滑动窗口内所有帧的位姿和路标点的深度，估计重力、尺度初始值、陀螺仪的零偏，并求解世界坐标系和初始相机坐标系的外参矩阵；
步骤3-4、采用光束法平差进行后端非线性优化，输出解算位姿；
具体公式如下：
Figure RE-GDA0003842566890000041
其中，Ω为滑动窗口内所有状态量，n为帧数，m为滑动窗口内特征点总数，λ为特征点逆深度；Xk为第k帧图像对应的IMU状态，包括位置
Figure RE-GDA0003842566890000042
速度
Figure RE-GDA0003842566890000043
旋转
Figure RE-GDA0003842566890000044
加速度偏置ba，陀螺仪偏置bg；
Figure RE-GDA0003842566890000045
为相机外参，包括平移向量
Figure RE-GDA0003842566890000046
旋转向量
Figure RE-GDA0003842566890000047
所述步骤S4中构建单目VIO/UWB组合系统的状态方程和量测方程；
具体步骤如下：
步骤4-1、建立单目VIO/UWB组合系统的更新模型；
具体公式如下：
Figure RE-GDA0003842566890000048
其中，t为组合系统采样的时间间隔，δ为对应数学量改正数的表示形式，(xk,yk)和(xk-1,yk-1)分别为移动平台k时刻和k-1时刻的坐标，vx和 vy分别为移动平台在x和y方向上的速度，ax和ay分别为移动平台在x和 y方向上的加速度；
步骤4-2、对式(3)进行线性化，获得单目VIO/UWB组合系统的状态方程为；
具体公式如下：
Xk＝FXk-1+Wk-1 (4)
其中，
Figure RE-GDA0003842566890000051
为状态转移矩阵，Xk＝[δx δy δvx δvy]T为状态向量，
Figure RE-GDA0003842566890000052
为系统噪声；
步骤4-3、建立单目VIO/UWB组合系统的观测模型；
具体公式如下：
Figure RE-GDA0003842566890000053
Figure RE-GDA0003842566890000054
其中，
Figure RE-GDA0003842566890000055
为UWB在k时刻的坐标，
Figure RE-GDA0003842566890000056
为k时刻第i个基准站的坐标，
Figure RE-GDA0003842566890000057
为k时刻UWB所得载体位置与第i个基准站的真实距离，
Figure RE-GDA0003842566890000058
为量测值，
Figure RE-GDA0003842566890000059
为单目VIO相邻时刻位移变化的量测值，
Figure RE-GDA00038425668900000510
和
Figure RE-GDA00038425668900000511
为单目VIO在k时刻与k-1时刻坐标，
Figure RE-GDA00038425668900000512
为观测噪声序列；
步骤4-4、对式(5)和式(6)进行线性化，获得单目VIO/UWB组合系统的量测方程；
具体公式如下：
Yk＝HXk+Vk (7)
其中，观测向量
Figure RE-GDA00038425668900000513
线性化噪声
Figure RE-GDA00038425668900000514
量测系数矩阵
Figure RE-GDA0003842566890000061
所述步骤5中构建单目VIO/UWB组合系统抗差模型；
具体步骤如下：
步骤5-1、利用新息向量构造检验信息；
具体公式如下：
Figure RE-GDA0003842566890000062
其中，εk为新息向量，Dk为新息向量所对应的协方差矩阵；
步骤5-2、利用检验信息Δεk,i和Huber函数构建抗差因子；
具体公式如下：
Figure RE-GDA0003842566890000063
其中，L为抗差阈值，由
Figure RE-GDA0003842566890000064
计算得到；
步骤5-3、利用抗差因子更新量测信息协方差矩阵R；
具体公式如下：
Figure RE-GDA0003842566890000065
其中，α＝diag[α1α1...αM-1]；
步骤5-4、利用更新后的量测信息协方差矩阵计算增益矩阵K；
具体公式如下：
Figure RE-GDA0003842566890000066
其中，Pk,k-1为预测状态协方差矩阵；
步骤5-5、引入Sage-Husa算法对系统噪声协方差矩阵Q进行实时更新修正；
具体公式如下：
Figure RE-GDA0003842566890000071
其中，Pk和Pk-1分别为k时刻和k-1时刻的估计状态协方差矩阵，b 为遗忘因子，0＜b＜1；dk＝(1-b)/(1-bk+1)。
所述步骤6中利用ARKF解算组合定位系统的定位结果；
具体公式如下：
Figure RE-GDA0003842566890000072
其中，
Figure RE-GDA0003842566890000073
和
Figure RE-GDA0003842566890000074
为更新修正后的量测信息协方差矩阵和系统噪声协方差矩阵，
Figure RE-GDA0003842566890000075
为预测状态向量，
Figure RE-GDA0003842566890000076
为k时刻的估计状态向量，I为单位矩阵。
本发明的特点及有益效果在于：
本发明提出了一种基于ARKF的单目VIO/UWB室内组合定位方法，能够在不依赖回环检测的情况下，通过融合其他传感器的量测信息有效解决了单目VIO在室内环境下易漂移和定位误差较大的问题，同时利用新息向量构建组合系统抗差模型，有效降低了UWBNLOS误差对组合系统的影响，提高了组合系统的定位精度和鲁棒性，平均定位精度可达到亚分米级，可实现室内高精度定位。
附图说明
图1是本发明的一种实施方式的基于ARKF的单目VIO/UWB室内组合定位方法流程图；
图2是本发明的一种实施方式的UWB基于TW-TOF测距原理示意图；
图3是本发明一种实施方式的单目VIO定位流程图；
图4是本发明一种实施方式的单目VIO定位轨迹与参考轨迹图；
图5是本发明一种实施方式的基于EKF解算的单目VIO/UWB组合定位轨迹与参考轨迹图；
图6是本发明一种实施方式的基于ARKF解算的单目VIO/UWB 组合定位轨迹与参考轨迹图；
图7是本发明一种实施方式的单目VIO定位结果、基于EKF的单目VIO/UWB组合定位结果、基于ARKF的单目VIO/UWB组合定位结果误差统计。
具体实施方式
下面结合图1～图6对本发明所述一种基于ARKF的单目VIO/UWB 室内组合定位方法进行详细说明。
本发明实施例中，基于ARKF的单目VIO/UWB室内组合定位方法，如图1所示，包括以下步骤：
步骤1、利用ROS时间标签实现组合系统中不同传感器的时间同步，并通过四参数转换模型完成组合系统坐标系统一；
本发明实例中，首先将单目相机、IMU、UWB、棱镜固定在组合定位实验平台上，通过四参数转换模型完成空间坐标系的统一，然后利用ROS时间标签将单目相机、IMU、UWB所采集的数据进行时间同步。
步骤2、获取IMU、相机和UWB的量测信息；
本发明实施实例中，相机选用的是Basler公司的Aca2500-20gc型号工业单目相机，搭载FL-BC1218A-VG定焦镜头，分辨率为648×512，图像采集频率设置为5Hz，IMU选用的是SBG-SYSTEMS公司的 Ellipse2-N，其陀螺精度为10deg/h，UWB是Time Domain公司生产的PulsON440测距模块，测距精度为5±1cm，参考值为Leica TS50全站仪所测得的坐标值，测距精度为1.5mm+2×10-6×D，D为全站仪所测距离；
本发明实例中，UWB采用基于往返时间(tow way-time of flight， TW-TOF)的测距模型，测距示意图如图2所示，具体公式如下：
Figure RE-GDA0003842566890000091
其中，d为UWB移动站与基准站之间的距离，c为光速，Tround为 UWB脉冲信号在UWB流动站与基准站之间的往返时间差，Trespond为 UWB基准站接收到脉冲信号到发射出响应脉冲信号的时间差，n为测量噪声，T1为UWB流动站发出脉冲信号的时间，T2为UWB基准站接收到脉冲信号的时间，T3为UWB基准站发出响应脉冲信号的时间，T4为UWB 流动站接收到响应脉冲信号的时间。
步骤3、在步骤2获得相机和IMU数据的基础上，利用VINS框架实现单目VIO定位，定位流程如图3所示；
具体步骤如下：
步骤3-1、提取图像Harris角点，采用金字塔光流法跟踪相邻帧；
步骤3-2、利用IMU数据计算当前帧的位姿作为初始值，及相邻帧的预积分增量和预积分误差的雅可比矩阵及协方差；
步骤3-3、进行相机初始化和相机惯导联合初始化，计算滑动窗口内所有帧的位姿和路标点的深度，估计重力、尺度初始值、陀螺仪的零偏，并求解世界坐标系和初始相机坐标系的外参矩阵；
步骤3-4、采用光束法平差进行后端非线性优化，输出解算位姿；
具体公式如下：
Figure RE-GDA0003842566890000092
其中，Ω为滑动窗口内所有状态量，n为帧数，m为滑动窗口内特征点总数，λ为特征点逆深度；Xk为第k帧图像对应的IMU状态，包括位置
Figure RE-GDA0003842566890000101
速度
Figure RE-GDA0003842566890000102
旋转
Figure RE-GDA0003842566890000103
加速度偏置ba，陀螺仪偏置bg；
Figure RE-GDA0003842566890000104
为相机外参，包括平移向量
Figure RE-GDA0003842566890000105
旋转向量
Figure RE-GDA0003842566890000106
步骤4、构建组合系统的状态方程和量测方程；
具体步骤如下：
步骤4-1、建立单目VIO/UWB组合系统的更新模型；
具体公式如下：
Figure RE-GDA0003842566890000107
其中，t为组合系统采样的时间间隔，δ为对应数学量改正数的表示形式，(xk,yk)和(xk-1,yk-1)分别为移动平台k时刻和k-1时刻的坐标，vx和 vy分别为移动平台在x和y方向上的速度，ax和ay分别为移动平台在x和 y方向上的加速度；
步骤4-2、对式(3)进行线性化，获得单目VIO/UWB组合系统的状态方程；
具体公式如下：
Xk＝FXk-1+Wk-1 (4)
其中，
Figure RE-GDA0003842566890000108
为状态转移矩阵，Xk＝[δx δy δvx δvy]T为状态向量，
Figure RE-GDA0003842566890000111
为系统噪声；
步骤4-3、建立单目VIO/UWB组合系统的观测模型；
具体公式如下：
Figure RE-GDA0003842566890000112
Figure RE-GDA0003842566890000113
其中，
Figure RE-GDA0003842566890000114
为UWB在k时刻的坐标，
Figure RE-GDA0003842566890000115
为k时刻第i个基准站的坐标，
Figure RE-GDA0003842566890000116
为k时刻UWB所得载体位置与第i个基准站的真实距离，
Figure RE-GDA0003842566890000117
为量测值，
Figure RE-GDA0003842566890000118
为单目VIO相邻时刻位移变化的量测值，
Figure RE-GDA0003842566890000119
和
Figure RE-GDA00038425668900001110
为单目VIO在k时刻与k-1时刻坐标，
Figure RE-GDA00038425668900001111
为观测噪声序列；
步骤4-4、对式(5)和式(6)进行线性化，获得单目VIO/UWB组合系统的量测方程；
具体公式如下：
Yk＝HXk+Vk (7)
其中，观测向量
Figure RE-GDA00038425668900001112
线性化噪声
Figure RE-GDA00038425668900001113
量测系数矩阵
Figure RE-GDA00038425668900001114
步骤5、构建组合系统抗差模型，具体步骤如下：
步骤5-1、利用新息向量构造检验信息；
具体公式如下：
Figure RE-GDA0003842566890000121
其中，εk为新息向量，Dk为新息向量所对应的协方差矩阵；
步骤5-2、利用检验信息Δεk,i和Huber函数构建抗差因子；
具体公式如下：
Figure RE-GDA0003842566890000122
其中，L为抗差阈值，由
Figure RE-GDA0003842566890000123
计算得到；
步骤5-3、利用抗差因子更新量测信息协方差矩阵R；
具体公式如下：
Figure RE-GDA0003842566890000124
其中，α＝diag[α1α1...αM-1]；
步骤5-4、利用更新后的量测信息协方差矩阵计算增益矩阵K；
具体公式如下：
Figure RE-GDA0003842566890000125
其中，Pk,k-1为预测状态协方差矩阵；
步骤5-5、引入Sage-Husa算法对系统噪声协方差矩阵Q进行实时更新修正；
具体公式如下：
Figure RE-GDA0003842566890000126
其中，Pk和Pk-1分别为k时刻和k-1时刻的估计状态协方差矩阵，b 为遗忘因子，0＜b＜1；dk＝(1-b)/(1-bk+1)。
步骤6、利用ARKF解算组合定位系统的定位结果；
具体公式如下：
Figure RE-GDA0003842566890000131
其中，
Figure RE-GDA0003842566890000132
和
Figure RE-GDA0003842566890000133
为更新修正后的量测信息协方差矩阵和系统噪声协方差矩阵，
Figure RE-GDA0003842566890000134
为预测状态向量，
Figure RE-GDA0003842566890000135
为k时刻的估计状态向量，I为单位矩阵。
本发明实例中，将本发明所述方法与单目VIO定位结果、基于 EKF的单目VIO/UWB组合定位结果进行比较，基于EKF和ARKF的组合模型中初始Qk值和Rk值相同，单目VIO/UWB-ARKF组合模型的抗差阈值L设为0.01，Sage-Husa算法中遗忘因子b设为0.995。
如图4所示，将组合定位解算结果与全站仪的定位结果进行对比，单目VIO在移动的初期阶段，定位精度较高，但随着时间的推移，定位轨迹逐渐出现漂移，其中B点处的漂移最为严重，最大定位误差为0.384 m。
如图5所示，将单目VIO/UWB-EKF组合定位解算结果与全站仪定位结果进行对比，在实验平台移动的初始阶段，组合系统的定位精度较高，在A、B、C、D四点处的漂移明显减小，整体的移动轨迹更加贴近参考轨迹。但由于实验平台在移动过程中受到障碍物的遮挡，导致组合系统存在NLOS误差的影响，随着时间的推移，误差累积仍然越来越大，其最大定位误差为0.269m。
如图6所示，将单目VIO/UWB-ARKF组合定位解算结果与全站仪定位结果进行对比，利用ARKF解算的单目VIO和UWB组合系统的定位精度较好，不仅能够克服单目VIO在A、B、C、D四点处容易出现漂移的问题，同时有效消除了UWB的NLOS误差对组合系统的影响，使得组合系统的定位轨迹更加贴近参考轨迹，组合系统的最大定位误差为 0.157m。
单目VIO、单目VIO/UWB-EKF组合模型和单目VIO/UWB-ARKF 组合模型定位误差的统计结果如图7所示，单目VIO定位误差随着时间逐渐增大，在引入UWB后，定位精度明显提高，但由于UWB的NLOS 误差的存在，对组合系统的定位精度仍然存在较大影响。而ARKF算法的引入有效抑制了UWB的NLOS误差的影响，提高了组合系统的定位精度和鲁棒性。
以上所述，仅为本发明中最基础的具体实施方式，但本发明的保护范围并不局限于此，任何本技术领域人士在本发明所揭露的技术范围内，可理解到的替换，都应涵盖在本发明的包含范围之内。因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (6)

1.一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，包括如下步骤：
步骤1、利用ROS时间标签实现组合系统中不同传感器的时间同步，并通过四参数转换模型完成组合系统坐标系统一；
步骤2、获取IMU、相机和UWB的量测信息；
步骤3、进行单目VIO位置解算；
步骤4、构建单目VIO/UWB组合系统的状态模型和量测模型；
步骤5、构建单目VIO/UWB组合系统抗差模型；
步骤6、利用ARKF解算组合系统的定位结果。
2.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤2中UWB采用基于往返时间(tow way-time of flight，TW-TOF)的测距模型，测距示意图如图2所示，具体公式如下：
Figure FDA0003733780500000011
其中，d为UWB移动站与基准站之间的距离，c为光速，Tround为UWB脉冲信号在UWB流动站与基准站之间的往返时间差，Trespond为UWB基准站接收到脉冲信号到发射出响应脉冲信号的时间差，n为测量噪声，T1为UWB流动站发出脉冲信号的时间，T2为UWB基准站接收到脉冲信号的时间，T3为UWB基准站发出响应脉冲信号的时间，T4为UWB流动站接收到响应脉冲信号的时间。
3.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤3在步骤2获得的相机和IMU数据的基础上，利用VINS框架实现单目VIO定位，定位流程如图3所示；
具体步骤如下：
步骤3-1、提取图像Harris角点，采用金字塔光流法跟踪相邻帧；
步骤3-2、利用IMU数据计算当前帧的位姿作为初始值，及相邻帧的预积分增量和预积分误差的雅可比矩阵及协方差；
步骤3-3、进行相机初始化和相机惯导联合初始化，计算滑动窗口内所有帧的位姿和路标点的深度，估计重力、尺度初始值、陀螺仪的零偏，并求解世界坐标系和初始相机坐标系的外参矩阵；
步骤3-4、采用光束平差法进行后端非线性优化，输出解算位姿；
具体公式如下：
Figure FDA0003733780500000021
其中，Ω为滑动窗口内所有状态量，n为帧数，m为滑动窗口内特征点总数，λ为特征点逆深度；Xk为第k帧图像对应的IMU状态，包括位置
Figure FDA0003733780500000022
速度
Figure FDA0003733780500000023
旋转
Figure FDA0003733780500000024
加速度偏置ba，陀螺仪偏置bg；
Figure FDA0003733780500000025
为相机外参，包括平移向量
Figure FDA0003733780500000026
旋转向量
Figure FDA0003733780500000027
4.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤4中构建单目VIO/UWB组合系统的状态方程和量测方程；
具体步骤如下：
步骤4-1、建立单目VIO/UWB组合系统的更新模型；
具体公式如下：
Figure FDA0003733780500000028
其中，t为组合系统采样的时间间隔，δ为对应数学量改正数的表示形式，(xk,yk)和(xk-1,yk-1)分别为移动平台k时刻和k-1时刻的坐标，vx和vy分别为移动平台在x和y方向上的速度，ax和ay分别为移动平台在x和y方向上的加速度；
步骤4-2、对式(3)进行线性化，获得单目VIO/UWB组合系统的状态方程；
具体公式如下：
Xk＝FXk-1+Wk-1 (4)
其中，
Figure FDA0003733780500000031
为状态转移矩阵，Xk＝[δx δy δvx δvy]T为状态向量，
Figure FDA0003733780500000032
为系统噪声；
步骤4-3、建立单目VIO/UWB组合系统的观测模型；
具体公式如下：
Figure FDA0003733780500000033
Figure FDA0003733780500000034
其中，
Figure FDA0003733780500000035
为UWB在k时刻的坐标，
Figure FDA0003733780500000036
为k时刻第i个基准站的坐标，
Figure FDA0003733780500000037
为k时刻UWB所得载体位置与第i个基准站的真实距离，
Figure FDA0003733780500000038
为量测值，
Figure FDA0003733780500000039
为单目VIO相邻时刻位移变化的量测值，
Figure FDA00037337805000000310
和
Figure FDA00037337805000000311
为单目VIO在k时刻与k-1时刻坐标，
Figure FDA00037337805000000312
为观测噪声序列；
步骤4-4、对式(5)和式(6)进行线性化，获得单目VIO/UWB组合系统的量测方程；
具体公式如下：
Yk＝HXk+Vk (7)
其中，观测向量
Figure FDA0003733780500000041
线性化噪声
Figure FDA0003733780500000042
量测系数矩阵
Figure FDA0003733780500000043
5.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤5中构建单目VIO/UWB组合系统抗差模型；
具体步骤如下：
步骤5-1、利用新息向量构造检验信息；
具体公式如下：
Figure FDA0003733780500000044
其中，εk为新息向量，Dk为新息向量所对应的协方差矩阵；
步骤5-2、利用检验信息Δεk,i和Huber函数构建抗差因子；
具体公式如下：
Figure FDA0003733780500000045
其中，L为抗差阈值，由
Figure FDA0003733780500000046
计算得到；
步骤5-3、利用抗差因子更新量测信息协方差矩阵R；
具体公式如下：
Figure FDA0003733780500000047
其中，α＝diag[α1α1...αM-1]；
步骤5-4、利用更新后的量测信息协方差矩阵计算增益矩阵K；
具体公式如下：
Figure FDA0003733780500000051
其中，Pk,k-1为预测状态协方差矩阵；
步骤5-5、引入Sage-Husa算法对系统噪声协方差矩阵Q进行实时更新修正；
具体公式如下：
Figure FDA0003733780500000052
其中，Pk和Pk-1分别为k时刻和k-1时刻的估计状态协方差矩阵，b为遗忘因子，0＜b＜1；dk＝(1-b)/(1-bk+1)。
6.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤6中利用ARKF解算组合定位系统的定位结果；
具体公式如下：
Figure FDA0003733780500000053
其中，
Figure FDA0003733780500000054
和
Figure FDA0003733780500000055
为更新修正后的量测信息协方差矩阵和系统噪声协方差矩阵，
Figure FDA0003733780500000056
为预测状态向量，
Figure FDA0003733780500000057
为k时刻的估计状态向量，I为单位矩阵。
CN202210790610.0A 2022-07-06 2022-07-06 一种基于arkf的单目vio/uwb室内组合定位方法 Pending CN115183767A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210790610.0A CN115183767A (zh) 2022-07-06 2022-07-06 一种基于arkf的单目vio/uwb室内组合定位方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210790610.0A CN115183767A (zh) 2022-07-06 2022-07-06 一种基于arkf的单目vio/uwb室内组合定位方法

Publications (1)

Publication Number Publication Date
CN115183767A true CN115183767A (zh) 2022-10-14

Family

ID=83517203

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210790610.0A Pending CN115183767A (zh) 2022-07-06 2022-07-06 一种基于arkf的单目vio/uwb室内组合定位方法

Country Status (1)

Country Link
CN (1) CN115183767A (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222556A (zh) * 2023-01-13 2023-06-06 浙江大学 一种基于多源传感器融合的室内定位方法及系统

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116222556A (zh) * 2023-01-13 2023-06-06 浙江大学 一种基于多源传感器融合的室内定位方法及系统
CN116222556B (zh) * 2023-01-13 2024-03-26 浙江大学 一种基于多源传感器融合的室内定位方法及系统

Similar Documents

Publication Publication Date Title
CN110514225B (zh) 一种矿井下多传感器融合的外部参数标定及精准定位方法
CN113781582B (zh) 基于激光雷达和惯导联合标定的同步定位与地图创建方法
CN110702091B (zh) 一种沿地铁轨道移动机器人的高精度定位方法
Fan et al. Data fusion for indoor mobile robot positioning based on tightly coupled INS/UWB
Tardif et al. A new approach to vision-aided inertial navigation
CN103776453B (zh) 一种多模型水下航行器组合导航滤波方法
Liu et al. A LiDAR-aided indoor navigation system for UGVs
CN109579838A (zh) Agv小车的定位方法及定位系统
Ibrahim et al. Inertial measurement unit based indoor localization for construction applications
CN108981687A (zh) 一种视觉与惯性融合的室内定位方法
CN114166221B (zh) 动态复杂矿井环境中辅助运输机器人定位方法及系统
CN114019552A (zh) 一种基于贝叶斯多传感器误差约束的定位置信度优化方法
Yang et al. A stable SINS/UWB integrated positioning method of shearer based on the multi-model intelligent switching algorithm
Li et al. Multi-GNSS PPP/INS/Vision/LiDAR tightly integrated system for precise navigation in urban environments
Wang et al. Magnetic field-enhanced learning-based inertial odometry for indoor pedestrian
Zou et al. A nonlinear transfer alignment of distributed POS based on adaptive second-order divided difference filter
CN115183767A (zh) 一种基于arkf的单目vio/uwb室内组合定位方法
Boche et al. Visual-inertial slam with tightly-coupled dropout-tolerant gps fusion
CN114690229A (zh) 一种融合gps的移动机器人视觉惯性导航方法
Li et al. An indoor and outdoor seamless positioning system for low-cost UGV using PPP/INS/UWB tightly coupled integration
CN116242372A (zh) 一种gnss拒止环境下的uwb-激光雷达-惯导融合定位方法
CN116380070A (zh) 一种基于时间戳优化的视觉惯性定位方法
CN116719071A (zh) 一种采用前向紧组合的gnss-ins因子图优化方法
Kong et al. Hybrid indoor positioning method of BLE and monocular VINS based smartphone
Bakambu et al. Heading-aided odometry and range-data integration for positioning of autonomous mining vehicles

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