基于多目视觉的室内导航定位装置及其定位方法与流程

本发明涉及机器人导航定位技术领域，尤其涉及一种基于多目视觉的室内导航定位装置及其定位方法。

背景技术：

定位是确定目标物体的位置，定位技术根据环境不同可以分为室外定位和室内定位。现今室外定位技术已经成熟，如美国gps、俄罗斯的glonass、欧盟的galileo以及中国北斗卫星导航系统，这些定位技术已经足以满足大多数室外环境下的定位。但是由于在室内条件下，障碍物多、环境复杂甚至多维，使得这些室外定位技术一旦应用到室内场景条件下，由于卫星信号衰减，定位精度会出现很大程度的降低，所以这些技术无法直接应用到室内。

目前，如何实现在复杂的室内场景下获得位置信息，已经成为现今的研究热点，并涌现了一批以红外线定位、超声波定位、wifi信号定位、超带宽定位、射频识别定位为代表的专用设备的解决方案及基于地磁定位的解决方案。但是，基于专用设备的解决方案及基于地磁定位的解决方案存在容易被干扰、定位区域有限、铺设成本高昂等问题。

技术实现要素：

鉴于背景技术中存在的问题，本发明的目的在于提供一种基于多目视觉的室内导航定位装置及其定位方法，其定位过程不受外界环境干扰、也不受定位区域限制，从而提高了定位精度、降低了定位成本。

为了实现上述目的，本发明提供了一种基于多目视觉的室内导航定位装置，其包括：机器人、l形线光源、多个单目相机以及控制系统。l形线光源设置在机器人上。所述多个单目相机位于机器人上方，相邻两个单目相机的视场范围有交叉，且所述多个单目相机的总视场范围不小于机器人的行走范围。控制系统包括视觉处理系统和信号传输系统，视觉处理系统通信连接于所述多个单目相机，信号传输系统包括上位机和下位机，上位机通信连接于视觉处理系统，下位机设置在机器人上并通信连接于上位机和机器人。

在根据一些实施例的所述基于多目视觉的室内导航定位装置中，所述基于多目视觉的室内导航定位装置还包括安装支架，所述安装支架固定安装所述多个单目相机。

本发明还提供了一种基于多目视觉的室内导航定位装置的定位方法，其由上述所述的基于多目视觉的室内导航定位装置来实现。其中，所述定位方法包括步骤s1-s7。

s1，对所述多个单目相机进行编号，建立各单目相机的相机坐标系o2a-xayaza、各单目相机对应的像素坐标系o1a-uava以及室内场景中的世界坐标系o-xyz，其中a为相机编号。s2，利用所述多个单目相机采集室内场景的初始图像，通过视觉处理系统获取所有单目相机的初始图像数据，并通过上位机将所有的初始图像数据进行拼接处理以获得二维全景地图。s3，通过上位机在所述二维全景地图上手动规划出机器人的目标运动轨迹，其中所述目标运动轨迹由所述二维全景地图上的一系列规划点构成。s4，计算出所述二维全景地图上的一系列规划点在世界坐标系下的坐标。s5，将机器人置于所述室内场景中，通过上位机将机器人选定为跟踪目标，并在机器人的移动过程中，上位机对机器人进行目标跟踪并获得机器人在像素坐标系下的实时位置，然后计算出机器人在世界坐标系下的实时位置。s6，基于机器人上的l形线光源在单目相机采集到的实时图像中的位置，计算出机器人在步骤s5中的实时位置下的实时姿态，其中，机器人在世界坐标系下的实时位置和实时姿态即为机器人的实时位姿。s7，上位机将移动机器人的实时位姿与目标运动轨迹进行比对、并向下位机输出行走控制信号，下位机将接收到的行走控制信号传递给机器人，机器人基于行走控制信号完成行走指令并最终到达规划的目的地。

在根据一些实施例的基于多目视觉的室内导航定位装置的定位方法中，在步骤s4中，所述二维全景地图上的任意规划点的计算过程包括步骤：s41，读出该规划点在像素坐标系下的坐标(u1,v1)；s42，在采集到该规划点的所有单目相机中选取相邻两个单目相机，并将所述相邻两个单目相机的相机坐标系的原点投影到世界坐标系中，并获得所述相邻两个单目相机的相机坐标系的原点的投影点的坐标p1(x1,y1)和p2(x2,y2)；s43，计算出该规划点在世界坐标系下的坐标(x,y)，计算公式为：

其中，fax为单目相机沿ua轴的归一化焦距，fay为单目相机沿va轴的归一化焦距，cax为单目相机光心的ua轴坐标，cay为单目相机光心的va轴坐标，zc为单目相机距机器人所在平面的铅垂距离。

在根据一些实施例的基于多目视觉的室内导航定位装置的定位方法中，在步骤s5中，对于任意时刻的机器人，其在世界坐标系下的实时位置的计算过程包括步骤：s51，读出机器人当前在像素坐标系下的坐标(u1',v1')；s52，在采集到机器人的所有单目相机中选取相邻两个单目相机，并所述相邻两个单目相机的相机坐标系的原点投影到世界坐标系中，并获得所述相邻两个单目相机的相机坐标系的原点的投影点的坐标p1'(x1',y1')和p2'(x2',y2')；s53，计算出机器人当前在世界坐标系下的坐标(x',y')，且计算公式为：

其中，fax为单目相机沿ua轴的归一化焦距，fay为单目相机沿va轴的归一化焦距，cax为单目相机光心的ua轴坐标，cay为单目相机光心的va轴坐标，zc为单目相机距机器人所在平面的铅垂距离。

在根据一些实施例的基于多目视觉的室内导航定位装置的定位方法中，在步骤s6中，机器人在步骤s5中的实时位置下的实时姿态的计算过程包括步骤：s61，选取l形线光源上的线段ab作为目标线段，分别读出线段ab的端点a、b当前在像素坐标系下的坐标；s62，通过判定机器人的旋转方向，其中为线段ab当前在像素坐标系下的方向向量，为线段ab上一时刻在像素坐标系下的方向向量；s63，通过判定机器人的旋转角度θ。

在根据一些实施例的基于多目视觉的室内导航定位装置的定位方法中，l形线光源包括长线段和短线段，线段ab为l形线光源上的长线段或者短线段。

本发明的有益效果如下：

在本申请的基于多目视觉的室内导航定位装置及其定位方法中，由于该定位装置和该定位方法的定位过程不受外界环境干扰、也不受定位区域限制，从而提高了定位精度、降低了定位成本。并且，通过该定位装置和该定位方法实现的定位范围可以随单目相机的灵活部署实现弹性调整。此外，该定位装置和该定位方法适用于自动化程度要求高、机器作业要求高效的场合，且能有效避免人员介入对生产安全和作业效率的影响。

附图说明

图1是本发明的基于多目视觉的室内导航定位装置的结构示意图。

图2是本发明中的多个单目相机的视场范围示意图。

图3是本发明中的l形线光源在两个不同时刻的位置示意图。

图4是本发明中的三类笛卡尔坐标系的关系示意图。

图5是本发明的基于多目视觉的室内导航定位装置的定位方法的原理框图。

其中，附图标记说明如下：

1机器人41视觉处理系统

2l形线光源42信号传输系统

21长线段421上位机

22短线段422下位机

3单目相机5安装支架

4控制系统s行走范围

具体实施方式

为使本申请实施例的目的、技术方案和优点更加清楚，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同；本文中在申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本申请；本申请的说明书和权利要求书及上述附图说明中的术语“包括”和“具有”以及它们的任何变形，意图在于覆盖不排他的包含。

在本文中提及“实施例”意味着，结合实施例描述的特定特征、结构或特性可以包含在本申请的至少一个实施例中。在说明书中的各个位置出现该短语并不一定均是指相同的实施例，也不是与其它实施例互斥的独立的或备选的实施例。本领域技术人员显式地和隐式地理解的是，本文所描述的实施例可以与其它实施例相结合。此外，本申请中出现的“多个”指的是两个以上(包括两个)。

下面参照附图来详细说明根据本申请的基于多目视觉的室内导航定位装置及其定位方法。

参照图1至图4，本申请的基于多目视觉的室内导航定位装置包括机器人1、l形线光源2、多个单目相机3以及控制系统4。

机器人1(内设有控制程序)通信连接于控制系统4，且机器人1在控制系统4的控制作用下完成行走。在一些实施例中，机器人1可为移动小车或者移动机械手。

l形线光源2设置在机器人1上。其中，在基于多目视觉的室内导航定位装置的定位过程中，l形线光源2在单目相机3采集到的图像中明晰地呈现为“l”形折线段。

在一些实施例中，参照图2，l形线光源2包括长线段21和短线段22。其中，长线段21和短线段22均可为直线形线光源，即此时l形线光源2由两个直线形线光源拼接而成，而两个直线形线光源之间的夹角可基于实际情况进行合理设置。

参照图1和图2，所述多个单目相机3位于机器人1上方，相邻两个单目相机3的视场范围有交叉，从而保证了机器人1在任意时刻和任意位置均能被至少两个单目相机3拍摄到。其中，针对不同范围的室内场景，单目相机3的数量和相对位置可进行选择性设置。在基于多目视觉的室内导航定位装置的定位过程中，为了保证所述多个单目相机3能够始终拍摄到机器人1的行走，则所述多个单目相机3的总视场范围不小于机器人1的行走范围s。需要说明的是，为了便于定位计算和减少定位误差，所述多个单目相机3的内部参数均一致。

在一些实施例中，参照图1，控制系统4包括视觉处理系统41和信号传输系统42。其中，视觉处理系统41通信连接于所述多个单目相机3。信号传输系统42包括上位机421和下位机422，上位机421通信连接于视觉处理系统41，下位机422设置在机器人1上并通信连接于上位机421和机器人1。

在一些实施例中，参照图1，所述基于多目视觉的室内导航定位装置还包括安装支架5，安装支架5固定安装所述多个单目相机3并保证所述多个单目相机3安装在同一高度，即所有单目相机3距离机器人1所在平面的铅垂距离(即下文所述的zc)均相等。

在本申请的基于多目视觉的室内导航定位装置中，所述多个单目相机3用于采集机器人1的实时位置；视觉处理系统41基于与所述多个单目相机3之间的通信连接获取所述多个单目相机3采集到的图像数据；信号传输系统42的上位机421基于与视觉处理系统41之间的通信连接对图像数据进行拼接以获得室内场景的二维全景地图，并对机器人1进行运动轨迹规划、目标跟踪、位姿坐标换算以及向下位机422输出行走控制信号；下位机422将接收到的行走控制信号传递给机器人1，机器人1基于行走控制信号完成行走指令并最终到达规划的目的地，由此实现室内导航定位。这种基于多目视觉的室内导航定位装置的结构简单、操作方便，且其在定位过程中不会受外界环境干扰，也不受定位区域限制(即适用范围广)，从而提高了定位精度、降低了定位成本。并且，基于单目相机3的数量及位置的可灵活部署性，本申请的基于多目视觉的室内导航定位装置具有高度可移植性、移植成本低、定位范围可以随相机的灵活部署实现弹性调整。此外，本申请定位装置适用于自动化程度要求高、机器作业要求高效的场合(尤其适用于机器人在室内长时往复运动的场合)，且能有效避免人员介入对生产安全和作业效率的影响。

本申请的基于多目视觉的室内导航定位装置的定位方法采用上述基于多目视觉的室内导航定位装置来实现，参照图1至5，本申请的基于多目视觉的室内导航定位装置的定位方法包括步骤s1-s7。

s1，对所述多个单目相机3进行编号，建立各单目相机3的相机坐标系o2a-xayaza、各单目相机3对应的像素坐标系o1a-uava以及室内场景中的世界坐标系o-xyz，其中a为相机编号。其中，参照图4，各单目相机3的相机坐标系的xa轴、各单目相机3对应的像素坐标系的ua轴以及世界坐标系的y轴互相平行；各单目相机3的相机坐标系的ya轴、各单目相机3对应的像素坐标系的va轴以及世界坐标系的x轴互相平行；各单目相机3的相机坐标系的za轴、世界坐标系的z轴互相平行。

s2，利用所述多个单目相机3采集室内场景的初始图像，通过视觉处理系统41获取所有单目相机3的初始图像数据，并通过上位机421将所有的初始图像数据进行拼接处理以获得二维全景地图。

s3，通过上位机421在所述二维全景地图上手动规划出机器人1的目标运动轨迹，其中所述目标运动轨迹由所述二维全景地图上的一系列规划点构成。

s4，计算出所述二维全景地图上的一系列规划点在世界坐标系下的坐标(即规划点在像素坐标系与世界坐标系之间的坐标换算)。

s5，将机器人1置于所述室内场景中，通过上位机421将机器人1选定为跟踪目标，并在机器人1的移动过程中，通过上位机421对机器人1进行目标跟踪并获得机器人1在像素坐标系下的实时位置，然后计算出机器人1在世界坐标系下的实时位置(即机器人1的实时位置在像素坐标系与世界坐标系之间的坐标换算)。

s6，基于机器人1上的l形线光源2在单目相机3采集到的实时图像中的位置，计算出机器人1在步骤s5中的实时位置下的实时姿态，其中，机器人1在世界坐标系下的实时位置和实时姿态即为机器人1的实时位姿。

s7，上位机421将移动机器人1的实时位姿与目标运动轨迹进行比对、并向下位机422输出行走控制信号，下位机422将接收到的行走控制信号传递给机器人1，机器人1基于行走控制信号完成行走指令并最终到达规划的目的地。

在本申请的基于多目视觉的室内导航定位装置的定位方法中，由于该定位方法的定位过程不受外界环境干扰、也不受定位区域限制，从而提高了定位精度、降低了定位成本。并且，通过该定位方法实现的定位范围可以随单目相机的灵活部署实现弹性调整。此外，本申请的定位方法适用于自动化程度要求高、机器作业要求高效的场合(尤其适用于机器人在室内长时往复运动的场合)，且能有效避免人员介入对生产安全和作业效率的影响。

在一实施例中，参照图1，在步骤s4中，所述二维全景地图上的任意规划点的计算过程包括步骤s41-s43。

s41，读出该规划点在像素坐标系下的坐标(u1,v1)。需要说明的是，至少两个单目相机3采集到的初始图像中含有该规划点，而该规划点在所述至少两个单目相机3对应的像素坐标系下的坐标均一致。

s42，在采集到该规划点的所有单目相机3中选取相邻两个单目相机3，并将采集到该规划点的所述相邻两个单目相机3的相机坐标系的原点分别投影到世界坐标系中，并获得所述相邻两个单目相机3的相机坐标系的原点的投影点的坐标p1(x1,y1)和p2(x2,y2)。

s43，计算出该规划点在世界坐标系下的坐标(x,y)，计算公式为：

其中，fax为单目相机3沿ua轴的归一化焦距，fay为单目相机3沿va轴的归一化焦距，cax为单目相机3光心的ua轴坐标，cay为单目相机3光心的va轴坐标，zc为单目相机3距机器人1所在平面的铅垂距离。

为了便于定位计算和减少定位误差，所述多个单目相机3的内部参数均一致，而对于任意类型的单目相机，其内部参数参数是固定的，则可另f1x＝f2x＝…＝fx，f1y＝f2y＝…fy，c1x＝c2x＝…＝cx，c1y＝c2y＝…＝cy，则上述计算公式为：

在一实施例中，在步骤s5中，对于任意时刻的机器人1，其在世界坐标系下的实时位置的计算过程包括步骤s51-s53。

s51，读出机器人1当前在像素坐标系下的坐标(u1',v1')。需要说明的是，在机器人1的移动过程中，在任意时刻，至少两个单目相机3采集到机器人1的当前位置，而机器人1当前在所述至少两个单目相机3对应的像素坐标系下的坐标均一致。

s52，在采集到机器人1当前位置的所有单目相机3中选取相邻两个单目相机3，并将采集到机器人1当前位置的所述相邻两个单目相机3的相机坐标系的原点投影到世界坐标系中，并获得所述相邻两个单目相机3的相机坐标系的原点的投影点的坐标p1'(x1',y1')和p2'(x2',y2')。

s53，计算出机器人1当前在世界坐标系下的坐标(x',y')，且计算公式为：

其中，fax为单目相机3沿ua轴的归一化焦距，fay为单目相机3沿va轴的归一化焦距，cax为单目相机3光心的ua轴坐标，cay为单目相机3光心的va轴坐标，zc为单目相机3距机器人1所在平面的铅垂距离。

同理，为了便于定位计算和减少定位误差，所述多个单目相机3的内部参数均一致，而对于任意类型的单目相机，其内部参数参数是固定的，则可另f1x＝f2x＝…＝fx，f1y＝f2y＝…fy，c1x＝c2x＝…＝cx，c1y＝c2y＝…＝cy，则上述计算公式为：

在一实施例中，在步骤s6中，机器人1在步骤s5中的实时位置下的实时姿态(即方位)的计算过程包括步骤s61-s63。

s61，选取l形线光源2上的线段ab作为目标线段，分别读出线段ab的端点a、b当前在像素坐标系下的坐标。需要说明的是，采集到机器人1当前位置的单目相机3同时采集到该线段ab，而在采集到该线段ab的单目相机3对应的像素坐标系中即可直接读出线段ab的端点a、b的坐标。其中，线段ab的端点a、b在采集到该线段ab的所有单目相机3对应的像素坐标系下的坐标均一致。

s62，通过判定机器人1的旋转方向(即机器人1是顺时针转动还是逆时针转动)，其中为线段ab当前时刻在像素坐标系下的方向向量，为线段ab上一时刻在像素坐标系下的方向向量。

s63，通过判定机器人1的旋转角度θ。

在一实施例中，在步骤s6中，线段ab可为l形线光源2上的长线段21或者短线段22。在整个定位计算过程中，由于l形线光源2的长线段21和短线段22在长度上有差异，从而能够保证在任意相邻两个时刻中线段ab始终为同一线段，进而提高了判定机器人1的旋转角度θ的准确性。