[发明专利]基于辅助线的位姿检测方法、装置和计算机可读存储介质

摘要

本发明公开了一种基于辅助线的位姿检测方法、装置和计算机可读存储介质，该位姿检测方法包括以下步骤：检测流程初始化、采集传感器数据、位姿先验估计、提取航向角检测值集合、航向角后验估计、提取位置检测值集合与位置后验估计，最终输出位姿信息。本发明利用惯性测量单元、摄像头等传感器的组合与估计算法，可以实时的检测机器人的位姿。相比于已有技术，本发明具有低成本、高精度与高可靠性的特点，为多种机器人的位姿信息的获取，提供了一种高性价比的检测手段。

主权项

1.一种基于辅助线的位姿检测方法，其特征在于，在定位区域的天花板或地面配置有辅助线，所述检测方法包括以下步骤：S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离EX、第二颜色辅助线间隔距离EY、第t个采样点的前向移动速度VF，t和侧向移动速度VL，t，第t个采样点的位姿后验估计向量进行初始化赋值，其中与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值，航向角定义为机器人前进方向相对于X轴的逆时针旋转角度；S102：将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t；采集图像传感器得到第t个采样点的天花板或地面图像S103：利用步骤S102得到的第t个采样点的偏航速率rt、前进加速度aF，t与侧向加速度aL，t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量S104：根据步骤S102获取的第t个采样点的天花板或地面图像提取第t个采样点的天花板或地面视觉航向角检测值集合Θt和Hough距离集合ρX，t、ρY，t；S105：根据步骤S104获取的第t个采样点的天花板或地面视觉航向角检测值集合Θt，与步骤3获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值S1106：根据步骤S104获取第t个采样点的Hough距离集合ρX，t、ρY，t，与步骤S105获取的航向角后验估计值提取第t个采样点的天花板或地面视觉X轴坐标检测值集合与第t个采样点的天花板或地面视觉Y轴坐标检测值集合以及S107：根据步骤S104获取的第t个采样点的天花板或地面视觉X轴坐标检测值集合与天花板或地面视觉Y轴坐标检测值集合与步骤S103获取的第t个采样点的X轴坐标先验估计与Y轴坐标先验估计进行位置坐标后验估计，以获取第t个采样点的X轴坐标后验估计与Y轴坐标后验估计以及S108：重复步骤S102至S107，输出每个采样点的位姿后验估计向量，即位姿检测值。