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

说明书

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

技术领域

本发明涉及机器人感知技术领域，特别是涉及一种基于辅助线的位姿检测方法、装置和计算机可读存储介质。

背景技术

机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，不仅在工业、农业、国防、医疗、服务等行业中得到广泛的应用，而且在排雷、搜捕、救援、辐射和空间领域等有害与危险场合得到很好的应用。因此，机器人技术的研究已经得到世界各国的普遍关注。

机器人的一个基本功能是移动，或者称之为位姿控制，这也是机器人实现其他功能的基础。而精准的位姿控制是依赖于精确的位姿检测的，一个普遍的观点认为“控制的精度是无法超过反馈(检测)的精度的”，所以精确的位姿检测系统就显得尤为重要。

对于工作在二维平面上的机器人来说，位姿检测包括对航向角与二维位置坐标的检测。传统的方法存在如下问题：

1)利用磁力计实现的航向角检测大多只能用在室外空旷的环境，对于室内环境，如果存在大量铁磁、电磁干扰，该检测手段可能失效；

2)利用陀螺仪实现的航向角检测会随着时间的推移变得越来越不可靠，而漂移小的陀螺仪成本过高，在很多低成本应用场景中无法使用；

3)多卫星定位手段大多只能用在室外空旷环境下，不适用于在树林、峡谷、室内、地下、水下、隧道等卫星信号无法到达的环境中；

4)基于WiFi、UWB的无线电定位手段存在着多路径干扰等问题，而且定位范围有限，如果要扩大定位范围将势必增加设备的成本；

5)基于视觉的定位手段受到光照的影响，对于光线过强、过暗基于剧烈变换的场景，该手段的精度较低，除此之外还有特征错误匹配、视距遮挡等问题；

6)基于视觉、WiFi或者磁场指纹的定位手段，由于事先需要做大量的实验以获取机器人位置的指纹特征，所以该手段不够实用，而且对指纹会发生变化的场景(特别是磁场指纹)，该定位手段是不可靠的。

发明内容

本发明的目的在于提供一种基于辅助线的位姿检测方法，用于室内环境。

本发明的目的在于提供一种基于辅助线的位姿检测装置，用于室内环境。

为此，本发明一方面提供了一种基于辅助线的位姿检测方法，在定位区域的天花板或地面配置有辅助线，包括以下步骤：

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

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