一种基于多传感器融合的三维姿态估计方法及装置

摘要

本发明提出了一种基于多传感器融合的三维姿态估计方法及装置，该方法包括：获取被测对象的RGB图像，并提取FAST特征点以及进行特征点的跟踪；采集被测对象的IMU测量值，并积分得到当前时刻的被测对象的位置、速度和旋转；获取被测对象的深度图像，对所述深度图像进行处理得到迭代最近点；基于先验信息、迭代最近点、IMU测量值和视觉重投影构建目标函数；使用迭代法对所述目标函数求解最小值得到所述对象最优的位姿。本方法基于多种传感器进行姿态估计，并构建了用于姿态估计的目标函数，该目标函数使用最小二乘法进行求解，该函数的输入值通过滑动窗口进行输入，设置了滑动窗口的具体数目，降低求解时间，提高了位姿的估计精度。