一种为第三方提供空间定位信息的自跟随机器人装置及方法与流程

本发明涉及机器人领域，具体涉及一种为第三方提供空间定位信息的自跟随机器人装置及方法。

背景技术：

随着机器人相关技术不断在生产生活领域的应用，人们对机器人的交互功能需求越来越高，机器人跟随技术是人机交互领域的重要组成部分，机器人跟随技术在机器人家用、商用以及军用多个领域都有重要的作用。现有的发明专利cn105955251a涉及一种机器人的视觉跟随控制方法及机器人，传感器检测单元检测周围环境获取环境的深度数据图，并将获取的深度数据图传送给数据处理控制单元；数据处理控制单元接收到深度数据图后对数据进行处理，获取真实世界坐标系，对环境中的人和障碍物进行识别，并对跟随目标进行骨架和动作识别和跟踪，获取执行命令；运动执行单元根据获取的跟随目标信息控制机器人对跟随目标进行跟踪，根据获取的执行命令对机器人进行控制，执行所述命令。该发明可以快速识别跟随目标，完成对多人环境中跟随目标的有效辨识，在跟随过程中根据跟随目标的位置变化迅速做出反应，适应绝大多数的商用应用场景。但是使用在虚拟现实的空间定位，要求精度高、延时低、稳定性高。往往只能通过在室内固定的位置架设摄像头或激光发射器等，而且定位范围有限、成本高。

本发明的内容

本发明的目的是为解决上述不足，提供一种为第三方提供空间定位信息的自跟随机器人装置及方法。

本发明的目的是通过以下技术方案实现的：

一种为第三方提供空间定位信息的自跟随机器人装置，包括视觉单元、云台、控制装置和行走轮，视觉单元安装在云台上，云台安装在控制装置的上部，4个行走轮安装在控制装置的两侧，控制装置包括运动机构、主板、运动距离测量机构、通信单元、第三方系统和定制光学标记，运动机构、运动距离测量机构和通信单元分别连接主板，第三方系统连接通信单元，定制光学标记连接第三方系统。

运动机构包括伺服电机和传动机构，运动机构在预设的轨道上运行，所述轨道为滑轨、光学标记轨道、或者通过slam记录的无形路径。

云台能在前进和转弯的过程中，保证视觉模块的稳定，从而保证体验者的虚拟视角的稳定和平滑。

第三方系统为移动vr头戴式显示器、背负式主机和主机端头戴式显示器、需要被定位的游戏道具中的一种。

视觉单元采集与体验者的相对位置信息，结合运动距离测量进行计算，将体验者相对于场地的绝对空间位置信息通过通信模块发送给体验者。

一种为第三方提供空间定位信息的方法，具体方法如下：

步骤201：基于双目红外视觉模块的单光点标识定位计算

在标定好的双目视觉系统的相机镜片上贴上红外滤光片，过滤除标识点外的光点信息。通过双目视觉系统拍摄的差异图片，对匹配的光点标识进行三维空间的定位。其计算如下：

设两个镜头中心点的中点作为双目系统的坐标原点，中心点距离为b；光轴方向为z轴正方向，左相机镜头中点到右相机镜头中点的方向为x轴正方向；相机设待测的光点标识三维坐标为(x,y,z)，设左右摄像机拍摄到的点在成像平面的坐标分别为(xl,yl)、(xr,yr)，两个摄像机的焦距为f。根据相似三角形的性质可以知道：

求解的

步骤301：多点标识的目标旋转计算

由步骤一可以测量出多个光点的三维坐标.通过测量人体上四个不共面的光学标识的三维坐标可以确定人体相对于相机的的旋转信息。具体方法如下：

设被测量四个点a、b、c、d的齐次化坐标为：p'd＝(x1,y1,z1,1)，p'c＝(x2,y2,z2,1)，p'b＝(x3,y3,z3,1)，p'a＝(x4,y4,z4,1)。构建pd＝(0,0,0,1)，pc＝(|dc|,0,0,1)，pb＝(0,|db|,0,1)，pa＝(0,0,|da|,1)。设所求的旋转信息有基于机器人坐标系的旋转矩阵r2，偏移矩阵t2：

p＝[r|t]3x4p'

根据的旋转关系，等式右边代入测量点坐标，等式坐标代入对应的构建点的坐标，求解12条线性方程组即可求解旋基于机器人坐标系的转矩阵和偏移矩阵的12个参数[r|t]3x4。

步骤202：设计轨道

根据具体场地，设定有向的连续轨道，轨道的高度是恒定的，以机器人的起始位置为世界坐标系的原点。机器人只能在设定的轨道上作沿着轨道向前或沿着轨道向后运动，其视觉平面始终垂直轨道的切线。

机器人在轨道上的实时运动中，由于轨道是预设的，根据机器人在轨道的三维坐标在世界坐标系下的三维坐标为t1，和机器人和轨道切线的关系，可以知道其在世界坐标系下旋转矩阵r1

步骤302：移动跟随

设定机器人需要保持的相对距离r(t)；机器人实时测量与人的相对距离c(t)；通过pid控制器，计算差值e(t)＝r(t)-c(t)和调整量根据pid控制器的调整量，在预设定的轨迹上选择向前或者向后运动，在实时测量的距离c(t)和设定距离r(t)的差值稳定在可接受范围内停止机器人的运动。

步骤401：坐标系换算

根据步骤301得出的目标在机器人坐标系的旋转矩阵r2，偏移矩阵t2和步骤202得出的机器人在世界坐标下的旋转矩阵r1，偏移矩阵t1，计算目标在世界坐标系的旋转矩阵r和偏移矩阵t，即可确定目标在大型场景中的位置和旋转。

r＝r1r2

本发明具有如下有益的效果：

本发明使用了红外激光发射器、红外滤光片、红外被动反光标记组合的方式，可以有效排除环境的干扰；降低区分人和环境算法、生成人体骨骼的运算时间，提高了实时性，降低了定位过程中的摄像头使用量和计算量，降低定位技术难度，可在vr移动系统中实现定位产品量化。

附图说明

图1为本发明的机器人整体结构示意图；

图2为本发明的机器人功能框图；

图3为本发明的方法流程图；

图4为本发明的双目单点定位原理图；

图5为本发明的多点标识的目标旋转计算原理图；

图6为本发明的场地的设定和抽象图；

图7为本发明的相对距离pid控制图；

图8为本发明的应用场景图。

具体实施方式

下面结合附图对本发明作进一步的说明：

如图1和图2所示，一种为第三方提供空间定位信息的自跟随机器人装置，包括视觉单元1、云台2、控制装置9和行走轮10，视觉单元1安装在云台2上，云台2安装在控制装置9的上部，4个行走轮10安装在控制装置9的两侧，控制装置9包括运动机构3、主板4、运动距离测量机构5、通信单元6、第三方系统7和定制光学标记8，运动机构3、运动距离测量机构5和通信单元6分别连接主板4，第三方系统7连接通信单元6，定制光学标记8连接第三方系统7。

运动机构3包括伺服电机和传动机构，运动机构在预设的轨道上运行，所述轨道为滑轨、光学标记轨道、或者通过slam记录的无形路径。

云台2能在前进和转弯的过程中，保证视觉模块的稳定，从而保证体验者的虚拟视角的稳定和平滑。

第三方系统7为移动vr头戴式显示器、背负式主机和主机端头戴式显示器、需要被定位的游戏道具中的一种。

视觉单元采集与体验者的相对位置信息，结合运动距离测量进行计算，将体验者相对于场地的绝对空间位置信息通过通信模块发送给体验者。

一种为第三方提供空间定位信息的方法，方法流程如图3所示：

步骤201：基于双目红外视觉模块的单光点标识定位计算

在标定好的双目视觉系统的相机镜片上贴上红外滤光片，过滤除标识点外的光点信息。通过双目视觉系统拍摄的差异图片，对匹配的光点标识进行三维空间的定位。其计算如下：

如图4，设两个镜头中心点的中点作为双目系统的坐标原点，中心点距离为b；光轴方向为z轴正方向，左相机镜头中点到右相机镜头中点的方向为x轴正方向；相机设待测的光点标识三维坐标为(x,y,z)，设左右摄像机拍摄到的点在成像平面的坐标分别为(xl,yl)、(xr,yr)，两个摄像机的焦距为f。根据相似三角形的性质可以知道：

求解的

步骤301：多点标识的目标旋转计算

由步骤一可以测量出多个光点的三维坐标.通过测量人体上四个不共面的光学标识的三维坐标可以确定人体相对于相机的的旋转信息。具体方法如下：

如图5，设被测量四个点a、b、c、d的齐次化坐标为：p'd＝(x1,y1,z1,1)，p'c＝(x2,y2,z2,1)，p'b＝(x3,y3,z3,1)，p'a＝(x4,y4,z4,1)。构建pd＝(0,0,0,1)，pc＝(|dc|,0,0,1)，pb＝(0,|db|,0,1)，pa＝(0,0,|da|,1)。设所求的旋转信息有基于机器人坐标系的旋转矩阵r2，偏移矩阵t2：

p＝[r|t]3x4p'

根据的旋转关系，等式右边代入测量点坐标，等式坐标代入对应的构建点的坐标，求解12条线性方程组即可求解旋基于机器人坐标系的转矩阵和偏移矩阵的12个参数[r|t]3x4。

步骤202：设计轨道

如图6，根据具体场地，设定有向的连续轨道，轨道的高度是恒定的，以机器人的起始位置为世界坐标系的原点。机器人只能在设定的轨道上作沿着轨道向前或沿着轨道向后运动，其视觉平面始终垂直轨道的切线。

机器人在轨道上的实时运动中，由于轨道是预设的，根据机器人在轨道的三维坐标在世界坐标系下的三维坐标为t1，和机器人和轨道切线的关系，可以知道其在世界坐标系下旋转矩阵r1

步骤302：移动跟随

如图7的pid控制器，设定机器人需要保持的相对距离r(t)；机器人实时测量与人的相对距离c(t)；通过pid控制器，计算差值e(t)＝r(t)-c(t)和调整量根据pid控制器的调整量，在预设定的轨迹上选择向前或者向后运动，在实时测量的距离c(t)和设定距离r(t)的差值稳定在可接受范围内停止机器人的运动。

步骤401：坐标系换算

根据步骤301得出的目标在机器人坐标系的旋转矩阵r2，偏移矩阵t2和步骤202得出的机器人在世界坐标下的旋转矩阵r1，偏移矩阵t1，计算目标在世界坐标系的旋转矩阵r和偏移矩阵t，即可确定目标在大型场景中的位置和旋转。

r＝r1r2

如图8所示，体验者在虚拟主题公园的场地中走动，背上背负式主机电脑，戴上虚拟现实头戴式显示器。跟随机器人运行在体验者的前面，并保持一定的距离。跟随机器人通过wifi、蓝牙等无线通信方式，为体验者提供虚拟视角、数据手套、游戏道具的空间位置和姿态信息，并能保证视角平滑地移动。