[发明专利]基于异构传感器信息融合的位姿跟踪系统及方法

摘要

本发明提供了一种基于异构传感器信息融合的位姿跟踪方法及系统，包括：步骤S1：基于内部传感器提取运动数据建立六自由度运动模型，基于六自由度运动模型进行位姿状态预测；步骤S2：基于外部传感器提取环境特征信息建立观测模型，基于观测模型进行观测更新位姿状态预测；步骤S3：针对异构传感器位姿状态进行非线性状态空间建模，得到非线性状态空间模型；步骤S4：利用PaRIS算法对非线性状态空间模型进行在线正切滤波估计，并利用随机梯度下降算法实时更新多传感器标定外参，同时获得高精度位姿估计。本发明所采用的融合位姿跟踪算法可以有效地提升定位的准确性与鲁棒性。