买专利卖专利找龙图腾，真高效！ 查专利查商标用IPTOP,全免费！专利年费监控用IP管家,真方便！

申请/专利权人：辽宁工程技术大学

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

主权项：1.一种基于ARKF的单目VIOUWB室内组合定位方法，其特征在于，包括如下步骤：步骤1、利用ROS时间标签实现组合系统中不同传感器的时间同步，并通过四参数转换模型完成组合系统坐标系统一；步骤2、获取IMU、相机和UWB的量测信息；步骤3、进行单目VIO位置解算；步骤4、构建单目VIOUWB组合系统的状态模型和量测模型；步骤5、构建单目VIOUWB组合系统抗差模型，具体步骤如下：步骤5-1、利用新息向量构造检验信息；具体公式如下： 其中，ek为新息向量，Dk为新息向量所对应的协方差矩阵；步骤5-2、利用检验信息Dek,i和Huber函数构建抗差因子；具体公式如下： 其中，L为抗差阈值，由计算得到；步骤5-3、利用抗差因子更新量测信息协方差矩阵R；具体公式如下： 其中，α＝diag[a1a1...αM-1]；步骤5-4、利用更新后的量测信息协方差矩阵计算增益矩阵K；具体公式如下： 其中，Pk,k-1为预测状态协方差矩阵；步骤5-5、引入Sage-Husa算法对系统噪声协方差矩阵Q进行实时更新修正；具体公式如下： 其中，Pk和Pk-1分别为k时刻和k-1时刻的估计状态协方差矩阵，b为遗忘因子，0b1；dk＝1-b1-bk+1；步骤6、利用ARKF解算组合系统的定位结果，具体公式如下： 其中，和为更新修正后的量测信息协方差矩阵和系统噪声协方差矩阵，为预测状态向量，为k时刻的估计状态向量，I为单位矩阵。

全文数据：

权利要求：

百度查询： 辽宁工程技术大学 一种基于ARKF的单目VIO/UWB室内组合定位方法