基于辅助线的位姿检测方法、装置和计算机可读存储介质制造方法及图纸

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

Position and pose detection method, device and computer readable storage medium based on auxiliary line

The present invention discloses a position detection method, device and computer readable storage medium based on auxiliary line. The pose detection method includes the following steps: detection process initialization, acquisition of sensor data, priori estimation of position and posture, extraction of direction angle detection value set, direction angle posterior estimation, and extraction position detection value set. And position posterior estimation, and finally output pose information. The invention can detect the pose of robot in real time by using the combination and estimation algorithm of sensors such as inertial measurement unit and camera. Compared with the existing technology, the invention has the characteristics of low cost, high precision and high reliability, and provides a high performance price detection method for the acquisition of the position and posture information of many kinds of robots.

【技术实现步骤摘要】
基于辅助线的位姿检测方法、装置和计算机可读存储介质
本专利技术涉及机器人感知
，特别是涉及一种基于辅助线的位姿检测方法、装置和计算机可读存储介质。
技术介绍
机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，不仅在工业、农业、国防、医疗、服务等行业中得到广泛的应用，而且在排雷、搜捕、救援、辐射和空间领域等有害与危险场合得到很好的应用。因此，机器人技术的研究已经得到世界各国的普遍关注。机器人的一个基本功能是移动，或者称之为位姿控制，这也是机器人实现其他功能的基础。而精准的位姿控制是依赖于精确的位姿检测的，一个普遍的观点认为“控制的精度是无法超过反馈(检测)的精度的”，所以精确的位姿检测系统就显得尤为重要。对于工作在二维平面上的机器人来说，位姿检测包括对航向角与二维位置坐标的检测。传统的方法存在如下问题：1)利用磁力计实现的航向角检测大多只能用在室外空旷的环境，对于室内环境，如果存在大量铁磁、电磁干扰，该检测手段可能失效；2)利用陀螺仪实现的航向角检测会随着时间的推移变得越本文档来自技高网...

【技术保护点】
一种基于辅助线的位姿检测方法，其特征在于，在定位区域的天花板或地面配置有辅助线，所述检测方法包括以下步骤：S101：对采样点序号t、采样间隔T、第一颜色辅助线间隔距离EX、第二颜色辅助线间隔距离EY、第t个采样点的前向移动速度VF，t和侧向移动速度VL，t，第t个采样点的位姿后验估计向量

【技术特征摘要】
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，输出每个采样点的位姿后验估计向量，即位姿检测值。2.根据权利要求1所述的基于辅助线的位姿检测方法，其特征在于，所述辅助线的配置方法如下：定位区域的天花板或地面配置有一系列具有第一颜色CX的平行于X轴的平行线，相邻的平行线具有相等的间隔EX；同时，定位区域的天花板或地面还配置有一系列具有第二颜色CY的平行于Y轴的平行线，相邻的平行线具有相等的间隔EY。3.根据权利要求2所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S103中的第t个采样点的位姿先验估计向量的计算过程如下：VF，t＝VF，t-1+TaF，t；VL，t＝VL，t-1+TaL，t；以及4.根据权利要求3所述的基于辅助线的位姿检测方法，其特征在于，所述步骤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，具体如下：Θt＝Ue∈∈X，t{-e，-e+180}。5.根据权利要求4所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S105中第t个采样点的航向角后验估计值的计算过程如下：如果Θt不是空集，则计算第t个采样点的航向角后验估计集合如下：其中abs(·)为求绝对值符号，为Θt的第i个元素，δθ＞0为航向角筛选阈值，NΘ为Θt的元素个数；如果不是空集，计算航向角后验估计值具体如下其中，为中第i个元素，Nθ为的元素个数；如果Θt是空集或者是空集，计算第t个采样点的航向角后验估计值具体如下：6.根据权利要求5所述的基于辅助线的位姿检测方法，其特征在于，所述步骤S107包括以下步骤：S701：计算X轴坐标后验估计值包括以下步骤：如果不是空集，则计算第t个采样点的X轴坐标后验估计集合如下：其中abs(·)为求绝对值符号，为的第i个元素，δx＞0为X轴坐标筛选阈值，NX为的元素个数；如果不是空集，计算X轴坐标后...

【专利技术属性】
技术研发人员：吕文君，皮明，杜晓冬，李泽瑞，
申请(专利权)人：安徽优思天成智能科技有限公司，
类型：发明
国别省市：安徽,34