CN108088446B - 移动机器人航向角检测方法、装置与计算机可读存储介质 - Google Patents

Abstract

本发明公开了一种移动机器人航向角检测方法、装置与计算机可读存储介质，航向角检测方法包含以下步骤：检测流程初始化、采集传感器数据、航向角先验估计、提取地面视觉航向角检测值集合与航向角后验估计。航向角检测装置由机器人偏航速率检测单元、地面图像采集单元与数据处理单元组成。计算机可读存储介质用于存储航向角检测程序。相比于已有技术，本发明具有成本低、精度高、性能稳定、检测周期短、覆盖区域广、适用场景多、系统可靠性高、隐私入侵度低等优点。由于没有磁力传感器的参与，所以本发明可以应用于铁磁干扰、电磁干扰较为丰富的环境。

Description

移动机器人航向角检测方法、装置与计算机可读存储介质

技术领域

本发明涉及机器人技术领域，特别是涉及一种移动机器人航向角检测方法、装置与计算机可读存储介质。

背景技术

移动机器人在工厂自动化、建筑、农业、服务等领域有着广泛的应用场景，为了实现移动机器人的精准控制，精准航向角的检测是一个重要前提。传统的轮式机器人的航向角的检测方法，一般需要涉及到磁力传感器。由于机器人工作环境周围往往存在铁磁干扰与电磁干扰，所以需要事先校正，如论文(DOI：10.1109/TGRS.2013.2291839、10.1109/TMAG.2013.2259842、10.1088/0957-0233/24/7/075102)所示。对于环境中磁干扰分布不均或者时变的情况，常见的做法是利用磁力传感器和陀螺仪的互补特性进行数据融合，从而消除陀螺仪的温度漂移误差和磁力传感器的磁干扰误差，如论文(DOI：10.1109/TIE.2015.2416692、10.1109/TSMC.2017.2701353、10.3390/S150511222)所示。然而，由于移动机器人的工作环境中的磁干扰是随时空变化且难以预知的，所以对于室内环境，如果存在大量铁磁、电磁干扰，基于磁力传感器的检测手段可能失效。因此在实际中应当尽可能避免使用磁力传感器。

本发明提出一种移动机器人航向角检测方法、装置与计算机可读存储介质，相比于已有技术，本发明具有成本低、精度高、性能稳定、检测周期短、覆盖区域广、适用场景多、系统可靠性高、隐私入侵度低等优点。由于没有磁力传感器的参与，本发明可以应用于铁磁干扰、电磁干扰较为丰富的环境。

发明内容

本发明的目的在于提供一种移动机器人航向角检测方法，在机器人工作区域的地面配置有辅助线，包括以下步骤：

S101：对采样点序号t初始化赋值：t←0；根据实际情况对采样间隔T、航向角后验估计值透视变换矩阵图像采集器安装角度集合Φ、辅助线颜色集合辅助线角度集合辅助线颜色-角度对应法则进行初始化；其中，透视变换矩阵是一个3×3的常值矩阵，图像采集器安装角度集合Φ＝{φ₁,φ₂,…}包含了所有图像采集器的安装角度，安装角度定义为图像采集器相对于移动机器人右侧方向轴的逆时针旋转角度；辅助线颜色集合包含所有辅助线颜色；辅助线角度集合包含所有的辅助线角度，辅助线角度定义为辅助线相对于零度基准线的逆时针方向旋转角度；零度基准线由人工选取；

S102：将采样点序号自增t←t+1，获取N_G个偏航速率检测器的数据，得到第t个采样点的机器人偏航速率检测值集合{r_i,t|i＝1,2,…N_G}，其中r_i,t表示第i个偏航速率检测器采集的偏航速率检测值；获取N_M个图像采集器的数据，得到第t个采样点的地面图像集合1,2,…N_M}，其中表示第i个图像采集器采集的地面图像；

S103：利用步骤S102获取的第t个采样点的机器人偏航速率检测值集合{r_i,t|i＝1,2,…N_G}，并基于第t-1个采样点的航向角后验估计值进行航向角先验估计，以获取第t个采样点的航向角先验估计值

S104：根据步骤S102获取的第t个采样点的地面图像集合并基于透视变换矩阵提取第t个采样点的地面视觉航向角检测值集合Θ_t；

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

S106：重复步骤S102至S105，输出每个采样点的航向角后验估计值，即航向角检测值。

根据本发明的另一方面，提供了一种移动机器人航向角检测装置，包括：机器人偏航速率检测单元、地面图像采集单元、以及数据处理单元，机器人偏航速率检测单元由若干偏航速率检测器组成，用于检测机器人偏航速率；地面图像采集单元由若干图像采集器组成，用于采集地面图像；图像采集器的镜头朝向地面，允许镜头的光轴不垂直于地面；数据处理单元用于执行航向角检测程序，以获得航向角信息，所述航向角检测程序在执行时实现上述步骤S101至S106。

本发明还提供了一种计算机可读存储介质，存储有上面所描述的航向角检测程序。

本发明还提供了一种具有上述航向角检测装置的移动机器人。

相比于已有技术，本发明具有成本低、精度高、性能稳定、检测周期短、覆盖区域广、适用场景多、系统可靠性高、隐私入侵度低等优点。由于没有磁力传感器的参与，所以本发明可以应用于铁磁干扰、电磁干扰较为丰富的环境。

除了上面所描述的目的、特征和优点之外，本发明还有其它的目的、特征和优点。下面将参照附图，对本发明作进一步详细的说明。

附图说明

构成本申请的一部分的说明书附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为根据本发明的移动机器人航向角检测方法的流程图；

图2为根据本发明的移动机器人航向角检测方法中的地面辅助线的示意图；以及

图3为根据本发明的移动机器人航向角检测方法的真实实验结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图与具体实施例对本发明进行详细说明。

在本发明实施前，需要对机器人工作区域的地面配置辅助线，具体如下：

在机器人工作区域的地面上，配置有N_A个平行线组，每个平行线组中包含若干条间隔不定且颜色相同的平行线；线宽根据环境适当设定；平行线组相对于零度基准线的逆时针方向旋转角度称之为该平行线组的辅助线角度，零度基准线可以任意选择，一般可以选择地理东向坐标轴为零度基准线；所有的平行线组的辅助线角度互不相同，则N_A个平行线组一共有N_A个不同的辅助线角度，这些辅助线角度组成集合称之为辅助线角度集合；平行线组的颜色称为辅助线颜色，不同的平行线组的辅助线颜色可以相同或者不同，即，可能存在多个平行线组具有相同的辅助线颜色，令所有的不同的辅助线颜色组成集合称之为辅助线颜色集合，集合含有N_C个元素，每个元素表示一种辅助线颜色且所有元素互不相同；集合与集合之间的对应关系称之为辅助线颜色-角度对应法则；由于N_A≥N_C，可能存在某个辅助线颜色对应多个辅助线角度。

在一实施例中，我们可以令N_A＝N_C＝2， 中的元素为角度制，中元素为具有两种相差较大色相的颜色，如红色与蓝色。一个配置实例如图2所示，选择X轴为零度基准线，矩形虚线外框为机器人工作区域，框内与零度基准线相差0度角的平行的红色虚线组为在地面绘制的红色辅助线，这里为了区分将红色辅助线标为虚线，实际中应当尽可能采用空隙较小的虚线或者实线；框内与零度基准线相差90度角的平行的蓝色实线组为在地面绘制的蓝色辅助线，这里为了区分将蓝色辅助线标为实线，实际中应当尽可能采用空隙较小的虚线或者实线；每组平行线的间隔距离是相等的。这个实施例优点在于：1)由于和地面的横纵轴线平行，实际配置中操作较为简单；2)每条平行线的投影在横纵轴线的位置是平行线间隔距离的整数倍，易于计算。

如图1所示，本发明的一种移动机器人航向角检测方法包括以下步骤：检测流程初始化、采集传感器数据、航向角先验估计、提取地面视觉航向角检测值集合与航向角后验估计，具体如下：

步骤1、检测流程初始化

对采样点序号t初始化赋值：t←0；根据实际情况对采样间隔T、航向角后验估计值透视变换矩阵图像采集器安装角度集合Φ、辅助线颜色集合辅助线角度集合辅助线颜色-角度对应法则进行初始化；其中，透视变换矩阵是一个3×3的常值矩阵，图像采集器安装角度集合Φ＝{φ₁,φ₂,…}包含了所有图像采集器的安装角度，安装角度定义为图像采集器相对于移动机器人右侧方向轴的逆时针旋转角度；辅助线颜色集合包含所有辅助线颜色；辅助线角度集合包含所有的辅助线角度，辅助线角度定义为辅助线相对于零度基准线的逆时针方向旋转角度；零度基准线由人工选取。

步骤2、采集传感器数据

将采样点序号自增t←t+1，获取N_G个偏航速率检测器的数据，得到第t个采样点的机器人偏航速率检测值集合{r_i,t|i＝1,2,…N_G}，其中r_i,t表示第i个偏航速率检测器采集的偏航速率检测值；获取N_M个图像采集器的数据，得到第t个采样点的地面图像集合其中表示第i个图像采集器采集的地面图像。

步骤3、航向角先验估计

利用步骤2获取的第t个采样点的机器人偏航速率检测值集合{r_i,t|i＝1,2,…N_G}，并基于第t-1个采样点的航向角后验估计值进行航向角先验估计，以获取第t个采样点的航向角先验估计值具体如下：其中W_i∈[0,1]表示偏航速率检测器的加权系数，满足

步骤4、提取地面视觉航向角检测值集合

根据步骤2获取的第t个采样点的地面图像集合并基于透视变换矩阵提取第t个采样点的地面视觉航向角检测值集合Θ_t，具体如下：

步骤401、对第t个采样点的地面图像集合分别进行基于辅助线颜色集合的图像阈值分割，得到N_M×N_C个二值图像集合其中表示第t个采样点的第i个图像采集器的基于颜色C_k进行阈值分割得到的二值图像，中白色部分为颜色C_k的辅助线，其余部分为黑色；C_k表示辅助线颜色集合中的第k个元素；

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

步骤403、基于透视变换矩阵对步骤402获取的二值图像集合 进行透视变换，得到透视变换后的二值图像集合

步骤404、对步骤403获取的二值图像集合进行Hough变换，以获取每个二值图像对应的Hough角度集合

步骤405、基于步骤404获取的Hough角度集合计算地面视觉航向角检测值集合Θ_t。具体如下：

步骤5、航向角后验估计

根据步骤3获取的第t个采样点的航向角先验估计值与步骤4获取的第t个采样点的地面视觉航向角检测值集合Θ_t，进行航向角后验估计，以获取第t个采样点的航向角后验估计值具体如下：

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

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

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

其中，为中的第i个元素，N_θ为的元素个数；

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

重复步骤2至步骤5，输出每个采样点的航向角后验估计值，即航向角检测值。

本发明的上述检测方法以移动机器人的数据处理单元上运行航向角检测程序的方式实施，为此本发明还给出了一种存储有该航向角检测程序的计算机可读存储介质。

本发明还给出了用于实现上述移动机器人航向角检测方法的装置，即，一种移动机器人航向角检测装置，包括：机器人偏航速率检测单元、地面图像采集单元、以及数据处理单元，机器人偏航速率检测单元由若干偏航速率检测器组成，用于检测机器人偏航速率；地面图像采集单元由若干图像采集器组成，用于采集地面图像；数据处理单元用于执行数据处理程序，以获得航向角，所述数据处理程序在执行时实现以下步骤：检测装置初始化、采集传感器数据、航向角先验估计、提取地面视觉航向角检测值集合、以及航向角后验估计。

在实际应用中，还要为该装置增加供电模块、电源管理模块、输入/输出模块、显示模块、通信模块、存储模块等辅助配件。

偏航速率定义为航向角相对于时间的导数。

偏航速率检测单元的安装要求为：保证偏航速率检测器获取移动机器人纯粹的偏航速率；图像采集单元的安装要求为：图像采集器的镜头朝向地面，不要求垂直于地面，保证图像采集器采集到地面图像。

在一实施例中，我们可以采用陀螺仪作为偏航速率检测器，采用使用了标准镜头的摄像头作为图像采集器，以防止图像畸变。

在另一实施例中，我们采用里程计作为偏航速率检测器，里程计定义为一种通过检测车轮转速和车轮相对于车体角度，然后利用运动学模型计算出偏航速率，并且输出该偏航速率的装置。

在又一实施例中，我们同时采用陀螺仪与里程计作为偏航速率检测器，二者得到的数据用于相互验证、相互融合，以确保检测数据的可靠性与精确度。

使用真实的移动机器人对本发明实施例进行测试，结果如图3所示，黑色直线为真实值，红色点线为本发明实施后的航向角检测值，可见二者曲线几乎重合，均方根误差为1.5846度，验证了本发明的有效性。如果在实施中采用性能更好的传感器，如帧率更高、分辨率更高的图像采集器，则能够输出更精确的航向角检测值。

以上所述仅为本发明的优选实施例而已，并不用于限制本发明，对于本领域的技术人员来说，本发明可以有各种更改和变化。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (5)

1.一种移动机器人航向角检测方法，其特征在于，在机器人工作区域的地面配置有辅助线，辅助线的配置方法如下：

在机器人工作区域的地面上，配置有N_A个平行线组，每个平行线组中包含若干条间隔不定且颜色相同的平行线；线宽根据环境适当设定；平行线组相对于零度基准线的逆时针方向旋转角度称之为该平行线组的辅助线角度，零度基准线可以人工选择；所有的平行线组的辅助线角度互不相同，则N_A个平行线组一共有N_A个不同的辅助线角度，这些辅助线角度组成集合称之为辅助线角度集合；平行线组的颜色称为辅助线颜色，不同的平行线组的辅助线颜色可以相同或者不同，因此可能存在多个平行线组具有相同的辅助线颜色；令所有的不同的辅助线颜色组成集合称之为辅助线颜色集合，集合含有N_C个元素，每个元素表示一种辅助线颜色且所有元素互不相同；集合与集合之间的对应关系称之为辅助线颜色-角度对应法则；由于N_A≥N_C，可能存在某个辅助线颜色对应多个辅助线角度；

所述航向角检测方法包括以下步骤：

S101：对采样点序号t初始化赋值：t←0；根据实际情况对采样间隔T、航向角后验估计值透视变换矩阵图像采集器安装角度集合Φ、辅助线颜色集合辅助线角度集合辅助线颜色-角度对应法则进行初始化；其中，透视变换矩阵是一个3×3的常值矩阵，图像采集器安装角度集合Φ＝{φ₁,φ₂,…}包含了所有图像采集器的安装角度，安装角度定义为图像采集器相对于移动机器人右侧方向轴的逆时针旋转角度；辅助线颜色集合包含所有辅助线颜色；辅助线角度集合包含所有的辅助线角度，辅助线角度定义为辅助线相对于零度基准线的逆时针方向旋转角度；零度基准线由人工选取；

S102：将采样点序号自增t←t+1，获取N_G个偏航速率检测器的数据，得到第t个采样点的机器人偏航速率检测值集合{r_i,t|i＝1,2,…N_G}，其中r_i,t表示第i个偏航速率检测器采集的偏航速率检测值；获取N_M个图像采集器的数据，得到第t个采样点的地面图像集合 其中表示第i个图像采集器采集的地面图像；

S103：利用步骤S102获取的第t个采样点的机器人偏航速率检测值集合{r_i,t|i＝1,2,…N_G}，并基于第t-1个采样点的航向角后验估计值进行航向角先验估计，以获取第t个采样点的航向角先验估计值具体如下：其中W_i∈[0,1]表示偏航速率检测器的加权系数，满足

S104：根据步骤S102获取的第t个采样点的地面图像集合并基于透视变换矩阵提取第t个采样点的地面视觉航向角检测值集合Θ_t，具体如下：

S1041：对步骤S102获取的第t个采样点的地面图像集合分别进行基于辅助线颜色集合的图像阈值分割，得到N_M×N_C个二值图像集合 其中表示第t个采样点的第i个图像采集器的基于颜色C_k进行阈值分割得到的二值图像，中白色部分为颜色C_k的辅助线，其余部分为黑色；C_k表示辅助线颜色集合中的第k个元素；

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

S1043：基于透视变换矩阵对步骤S1042获取的二值图像集合 进行透视变换，得到透视变换后的二值图像集合

S1044：对步骤S1043获取的二值图像集合进行Hough变换，以获取每个二值图像对应的Hough角度集合

S1045：基于步骤S1044获取的Hough角度集合计算地面视觉航向角检测值集合Θ_t，具体如下：

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

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

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

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

其中，为中的第i个元素，N_θ为的元素个数；

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

以及

S106：重复步骤S102至S105，输出每个采样点的航向角后验估计值，即航向角检测值。

2.一种计算机可读存储介质，存储有航向角检测程序，其特征在于，所述航向角检测程序在处理器上执行时实现权利要求1所述的移动机器人航向角检测方法。

3.一种移动机器人航向角检测装置，其特征在于，包括：机器人偏航速率检测单元、地面图像采集单元、以及数据处理单元，机器人偏航速率检测单元由若干偏航速率检测器组成，用于检测机器人偏航速率；地面图像采集单元由若干图像采集器组成，用于采集地面图像；图像采集器的镜头朝向地面，允许镜头的光轴不垂直于地面；数据处理单元用于执行数据处理程序，以获得航向角信息，所述数据处理程序在执行时实现权利要求1所述的移动机器人航向角检测方法。

4.根据权利要求3所述的一种移动机器人航向角检测装置，其特征在于，所述机器人偏航速率检测单元包括陀螺仪和/或里程计。

5.一种移动机器人，包括航向角检测装置，其特征在于，所述航向角检测装置为根据权利要求4所述的航向角检测装置。