[发明专利]适用于机器人的视觉和惯性融合的位姿估计方法及系统

说明书

本发明公开了一种适用于机器人的视觉和惯性融合的位姿估计方法及系统，该方法包括：获取视觉传感器输出的图像，对相邻帧图像的点线特征进行检测；根据相邻帧图像的点线特征的位置和姿态变化，建立视觉位姿模型；利用惯性传感器检测视觉传感器的位姿数据，建立惯性位姿模型；将视觉位姿模型和惯性位姿模型进行数据融合，通过融合位姿模型对机器人的视觉传感器的位姿进行估计。本发明解决了机器人视觉传感器受光线和环境遮挡丢帧无法估计位姿、惯性传感器长时间运动零偏误差累积，以及单一传感器位姿估计精度低的问题，改善了现实环境下位姿估计精度和鲁棒性，可广泛应用于机器人环境感知领域。

技术领域

本发明涉及机器人视觉技术领域，特别涉及一种适用于机器人的视觉和惯性融合的位姿估计方法及系统。

背景技术

现阶段，机器人的视觉和惯性传感器被广泛应用于机器人的各领域，如拣选机器人、焊接机器人、蔬菜嫁接机器人、采摘机器人等，通过视觉和惯性传感器控制机器人到达环境中的指定位姿，进而完成所给定的作业任务。

基于视觉的算法能够很好地处理长时间内的变化，但不能正确地观察到突然的旋转速率且容易受到光照影响。另一方面，陀螺仪在短时间内可精确测量其轴线的角速度，但在较长的周期内漂移很大。因此，视觉和陀螺仪的结合是相辅相成的，并可提供更健壮的姿态估计能力。当连续视频帧之间的像素位移主要由摄像机旋转引起时，可实现陀螺仪零漂移补偿。但是，现有技术实现视觉和惯性融合还存在初始化，非线性、坐标和时间不一致等问题。

发明内容

本发明提供了一种适用于机器人的视觉和惯性融合的位姿估计方法及系统，以解决现有位姿估计方法不够准确的技术问题。

为解决上述技术问题，本发明提供了如下技术方案：

一方面，本发明提供了一种适用于机器人的视觉和惯性融合的位姿估计方法，该适用于机器人的视觉和惯性融合的位姿估计方法包括：

获取机器人的视觉传感器采集的图像，对相邻帧图像的点线特征进行检测；

根据相邻帧图像的点线特征的位置和姿态变化，确定所述视觉传感器的位姿变化，建立用于表示所述视觉传感器的位姿变化的视觉位姿模型；

利用惯性传感器检测所述视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型；其中，所述惯性传感器安装在所述视觉传感器上并与所述视觉传感器同轴系设置；

将所述视觉位姿模型和所述惯性位姿模型进行数据融合，得到融合位姿模型，以通过所述融合位姿模型对机器人的视觉传感器的位姿进行估计。

进一步地，所述对相邻帧图像的点线特征进行检测，包括：

对于前一帧图像，通过预设的点线特征提取方法检索图像中的灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置；其中，所述灭点指在透视投影中，不平行于投影面的平行线投影聚集到的点；所述地脚点指不同平面的地脚线的交点；所述铅垂线指垂直于地面的直线；

通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，并基于前一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置，根据估计出的相邻帧图像的姿态和位置的变化，预测下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置；

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，并记录检索到的灭点、地脚点和铅垂线起止点在图像中的坐标位置。

进一步地，所述惯性传感器包括陀螺仪和加速度计，所述陀螺仪用于检测所述视觉传感器的角速度，所述加速度计用于检测所述视觉传感器的线加速度；