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

权利要求书

1.一种基于辅助线的位姿检测方法，其特征在于，在定位区域的天花板或地面配置有辅助线，所述检测方法包括以下步骤：

S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离E_X、第二颜色辅助线间隔距离E_Y、第t个采样点的前向移动速度V_F，t和侧向移动速度V_L，t，第t个采样点的位姿后验估计向量进行初始化赋值，其中与分别表示第t个采样点的X轴坐标、Y轴坐标与航向角的后验估计值，航向角定义为机器人前进方向相对于X轴的逆时针旋转角度，其中，矩阵A′表示矩阵A的转置；

S102：将采样点序号自增t←t+1，采集惯性测量单元的数据，得到第t个采样点的偏航速率r_t、前进加速度a_F，t与侧向加速度a_L，t；采集图像传感器得到第t个采样点的天花板或地面图像

S103：利用步骤S102得到的第t个采样点的偏航速率r_t、前进加速度a_F，t与侧向加速度a_L，t，并基于第t-1个采样点的位姿后验估计向量进行位姿先验估计，以获取第t个采样点的位姿先验估计向量

S104：根据步骤S102获取的第t个采样点的天花板或地面图像提取第t个采样点的天花板或地面视觉航向角检测值集合Θ_t和Hough距离集合ρ_X，t、ρ_Y，t；

S105：根据步骤S104获取的第t个采样点的天花板或地面视觉航向角检测值集合Θ_t，与步骤S103获取的第t个采样点的航向角先验估计进行航向角后验估计，以获取第t个采样点的航向角后验估计值

S106：根据步骤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，输出每个采样点的位姿后验估计向量，即位姿检测值，

其中，所述辅助线的配置方法如下：定位区域的天花板或地面配置有一系列具有第一颜色C_X的平行于X轴的平行线，相邻的平行线具有相等的间隔E_X；同时，定位区域的天花板或地面还配置有一系列具有第二颜色C_Y的平行于Y轴的平行线，相邻的平行线具有相等的间隔E_Y，

其中，所述步骤S103中的第t个采样点的位姿先验估计向量的计算过程如下：V_F，t＝V_F，t-1+Ta_F，t；V_L，t＝V_L，t-1+Ta_L，t；以及

其中，所述步骤S104包括以下步骤：

S401：对步骤S102采集到的第t个采样点的天花板或地面图像分别进行基于第一颜色与第二颜色的图像阈值分割，以分别获取只包含对应颜色辅助线的二值图像与其中，中白色区域为第一颜色辅助线，中白色区域为第二颜色辅助线，其余部分为黑色；

S402：对步骤S401获取的二值图像与依次进行形态学闭运算、骨架提取操作、剪枝操作，以填充图像中内细小空洞、提取辅助线的中心线、消除游离于或依附于辅助线骨架上的小枝，得到第t个采样点的只包含辅助线中心线的二值图像集合与

S403：对步骤S402中获取的二值图像与进行Hough变换，以分别获取第t个采样点的Hough距离集合ρ_X，t、ρ_Y，t与Hough角度集合∈_X，t、∈_Y，t；以及

S404：基于步骤S403获取的Hough角度集合∈_X，t，计算天花板或地面视觉航向角检测值集合Θ_t，具体如下：

其中，所述步骤S105中第t个采样点的航向角后验估计值的计算过程如下：

如果Θ_t不是空集，则计算第t个采样点的航向角后验估计集合如下：

其中abs(·)为求绝对值符号，为Θ_t的第i个元素，δ_θ＞0为航向角筛选阈值，N_Θ为Θ_t的元素个数；

如果不是空集，计算航向角后验估计值具体如下：

其中，为中第i个元素，N_θ为的元素个数；如果Θ_t是空集或者是空集，计算第t个采样点的航向角后验估计值具体如下：