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

权利要求书

1.一种适用于机器人的视觉和惯性融合的位姿估计方法，其特征在于，包括：

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

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

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

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

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

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

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

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

2.如权利要求1所述的适用于机器人的视觉和惯性融合的位姿估计方法，其特征在于，所述惯性传感器包括陀螺仪和加速度计，所述陀螺仪用于检测所述视觉传感器的角速度，所述加速度计用于检测所述视觉传感器的线加速度；

所述通过所述惯性传感器检测到的所述视觉传感器的位姿数据，估计相邻帧图像的姿态和位置的变化，包括：

通过对所述陀螺仪和加速度计检测到的角速度和线加速度进行预积分，得到所述视觉传感器的姿态和位置变化，以估计相邻帧图像的姿态和位置的变化。

3.如权利要求1所述的适用于机器人的视觉和惯性融合的位姿估计方法，其特征在于，所述基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，采用局部搜索法在下一帧图像中检索灭点、地脚点和铅垂线，包括：

基于对下一帧图像中的灭点、地脚点和铅垂线起止点在图像中的坐标位置的预测结果，在下一帧图像中设计搜索框；并在所述搜索框内采用预设的点线特征搜索方法对下一帧图像中的灭点、地脚点和铅垂线进行局部搜索。

4.如权利要求1所述的适用于机器人的视觉和惯性融合的位姿估计方法，其特征在于，所述利用惯性传感器检测所述视觉传感器的位姿数据，并根据所述惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型，包括：

定周期地利用所述视觉位姿模型计算所述视觉传感器的姿态和位置变化，并利用计算结果，对所述惯性传感器进行偏差标定；

利用已标定的惯性传感器检测所述视觉传感器的位姿数据，并根据惯性传感器检测的位姿数据建立用于表示所述视觉传感器的位姿变化的惯性位姿模型。