一种基于ARKF的单目VIO/UWB室内组合定位方法

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所示，具体公式如下：
其中，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、采用光束平差法进行后端非线性优化，输出解算位姿；
具体公式如下：
其中，Ω为滑动窗口内所有状态量，n为帧数，m为滑动窗口内特征点总数，λ为特征点逆深度；Xk为第k帧图像对应的IMU状态，包括位置 速度 旋转 加速度偏置
ba，陀螺仪偏置bg； 为相机外参，包括平移向量 旋转向量
4.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤4中构建单目VIO/UWB组合系统的状态方程和量测方程；
具体步骤如下：
步骤4‑1、建立单目VIO/UWB组合系统的更新模型；
具体公式如下：
其中，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)
T
其中， 为状态转移矩阵，Xk＝[δx δy δvx δvy] 为状态向量，
为系统噪声；
步骤4‑3、建立单目VIO/UWB组合系统的观测模型；
具体公式如下：
其中， 为UWB在k时刻的坐标， 为k时刻第i个基准站的坐标， 为k
时刻UWB所得载体位置与第i个基准站的真实距离， 为量测值， 为单目VIO相邻时刻位移变化的量测值， 和 为单目VIO在k时刻与k‑1时刻坐标，
为观测噪声序列；
步骤4‑4、对式(5)和式(6)进行线性化，获得单目VIO/UWB组合系统的量测方程；
具体公式如下：
Yk＝HXk+Vk                        (7)
其中，观测向量 线性化噪声 量测系数
矩阵
5.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤5中构建单目VIO/UWB组合系统抗差模型；
具体步骤如下：
步骤5‑1、利用新息向量构造检验信息；
具体公式如下：
其中，εk为新息向量，Dk为新息向量所对应的协方差矩阵；
步骤5‑2、利用检验信息Δεk,i和Huber函数构建抗差因子；
具体公式如下：
其中，L为抗差阈值，由 计算得到；
步骤5‑3、利用抗差因子更新量测信息协方差矩阵R；
具体公式如下：
其中，α＝diag[α1α1...αM‑1]；
步骤5‑4、利用更新后的量测信息协方差矩阵计算增益矩阵K；
具体公式如下：
其中，Pk,k‑1为预测状态协方差矩阵；
步骤5‑5、引入Sage‑Husa算法对系统噪声协方差矩阵Q进行实时更新修正；
具体公式如下：
其中，Pk和Pk‑1分别为k时刻和k‑1时刻的估计状态协方差矩阵，b为遗忘因子，0＜b＜1；
k+1
dk＝(1‑b)/(1‑b )。
6.根据权利要求1所述的一种基于ARKF的单目VIO/UWB室内组合定位方法，其特征在于，所述步骤6中利用ARKF解算组合定位系统的定位结果；
具体公式如下：
其中， 和 为更新修正后的量测信息协方差矩阵和系统噪声协方差矩阵， 为预测状态向量， 为k时刻的估计状态向量，I为单位矩阵。

暂无