机器人运动姿态估计方法

摘要

本发明提供一种机器人运动姿态估计方法，包括：双目摄像头采集双目视觉图像，以及惯性测量单元采集惯性数据；根据双目视觉图像更新预先获取的关键帧库和路标点库，并将路标点库中的每个路标点构建投影误差项；将惯性数据代入预先构建的惯性测量单元的运动误差模型中，获取惯性测量单元的运动模型估计误差项；将投影误差项和运动模型估计误差项代入高斯牛顿迭代模型，并对高斯牛顿迭代模型进行迭代计算，并将能够使得误差代价函数最小的解作为最优解，用于估计机器人在下一时刻的位置和姿态。本发明提供的机器人运动姿态估计方法，充分利用视觉数据和惯性数据，提高了对机器人运动姿态估计的精确度。

主权项

一种机器人运动姿态估计方法，其特征在于，包括：双目摄像头采集双目视觉图像，以及惯性测量单元采集惯性数据；根据所述双目视觉图像更新预先获取的关键帧库和路标点库，并将所述路标点库中的每个路标点构建投影误差项；将所述惯性数据代入预先构建的惯性测量单元的运动误差模型中，获取惯性测量单元的运动模型估计误差项；将所述投影误差项和所述运动模型估计误差项代入高斯牛顿迭代模型，并对所述高斯牛顿迭代模型进行迭代计算，并将能够使得误差代价函数最小的解作为最优解，用于估计机器人在下一时刻的位置和姿态。