具体实施方式

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

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

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

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

参照图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所在平面的铅垂距离(即下文所述的Z_c)均相等。

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

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

S1，对所述多个单目相机3进行编号，建立各单目相机3的相机坐标系O_2a-X_aY_aZ_a、各单目相机3对应的像素坐标系O_1a-U_aV_a以及室内场景中的世界坐标系O-XYZ，其中a为相机编号。其中，参照图4，各单目相机3的相机坐标系的X_a轴、各单目相机3对应的像素坐标系的U_a轴以及世界坐标系的Y轴互相平行；各单目相机3的相机坐标系的Y_a轴、各单目相机3对应的像素坐标系的V_a轴以及世界坐标系的X轴互相平行；各单目相机3的相机坐标系的Z_a轴、世界坐标系的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，读出该规划点在像素坐标系下的坐标(u₁,v₁)。需要说明的是，至少两个单目相机3采集到的初始图像中含有该规划点，而该规划点在所述至少两个单目相机3对应的像素坐标系下的坐标均一致。

S42，在采集到该规划点的所有单目相机3中选取相邻两个单目相机3，并将采集到该规划点的所述相邻两个单目相机3的相机坐标系的原点分别投影到世界坐标系中，并获得所述相邻两个单目相机3的相机坐标系的原点的投影点的坐标P₁(x₁,y₁)和P₂(x₂,y₂)。

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

其中，f_ax为单目相机3沿U_a轴的归一化焦距，f_ay为单目相机3沿V_a轴的归一化焦距，c_ax为单目相机3光心的U_a轴坐标，c_ay为单目相机3光心的V_a轴坐标，Z_c为单目相机3距机器人1所在平面的铅垂距离。

为了便于定位计算和减少定位误差，所述多个单目相机3的内部参数均一致，而对于任意类型的单目相机，其内部参数参数是固定的，则可另f_1x＝f_2x＝…＝f_x，f_1y＝f_2y＝…f_y，c_1x＝c_2x＝…＝c_x，c_1y＝c_2y＝…＝c_y，则上述计算公式为：

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

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

S52，在采集到机器人1当前位置的所有单目相机3中选取相邻两个单目相机3，并将采集到机器人1当前位置的所述相邻两个单目相机3的相机坐标系的原点投影到世界坐标系中，并获得所述相邻两个单目相机3的相机坐标系的原点的投影点的坐标P₁'(x₁',y₁')和P₂'(x₂',y₂')。

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

其中，f_ax为单目相机3沿U_a轴的归一化焦距，f_ay为单目相机3沿V_a轴的归一化焦距，c_ax为单目相机3光心的U_a轴坐标，c_ay为单目相机3光心的V_a轴坐标，Z_c为单目相机3距机器人1所在平面的铅垂距离。

同理，为了便于定位计算和减少定位误差，所述多个单目相机3的内部参数均一致，而对于任意类型的单目相机，其内部参数参数是固定的，则可另f_1x＝f_2x＝…＝f_x，f_1y＝f_2y＝…f_y，c_1x＝c_2x＝…＝c_x，c_1y＝c_2y＝…＝c_y，则上述计算公式为：

在一实施例中，在步骤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的旋转角度θ的准确性。