一种机器人定位导航系统的制作方法

本发明涉及机器人技术领域，特别是指一种机器人定位导航系统。

背景技术

移动机器人目前已经遍布军事、工业、民用等各大领域，并还在不断的发展中，目前移动机器人技术已获得了可喜的进展，研究成果令人鼓舞。对于移动机器人来说，定位导航技术是机器人的关键技术之一，也是其执行其他任务的前提和基础。

现有技术一、gps定位系统通过机器人本身携带的gps定位模块进行定位，一般采用伪距差分动态定位法，用基准接收机和动态接收机同时观测4个gps微型，按照一定的算法即可求出机器人的三维坐标。但在使用gps定位过程中，解算装置容易受到时钟误差、接收机噪声等多种因素影响，因此，单纯利用gps导航存在定位精度较低、可靠性不高的问题。

现有技术二、在机器人的导航技术中，里程计法是使用最广泛的定位方法。在移动机器人的车轮上装有光电编码器，通过对车轮转动的记录来实现对机器人的位姿跟踪。这种方法的缺点是在测量过程中会有累计误差，影响定位的精度。

现有技术三、在惯性导航法中，采用陀螺仪和加速度计实现定位。通过陀螺仪测得旋转速度的值，通过加速度计测得加速度的值。机器人的位置信息可以通过将测量值进行一次积分和二次积分得到。但是在积分操作中，会引入惯性导航信息的时间漂移，需要及时进行误差修正。

技术实现要素：

本发明要解决的技术问题是提供一种机器人定位导航系统，以解决现有技术所存在的仅利用gps导航或其他导航方式进行定位存在定位精度低、可靠性低的问题。

为解决上述技术问题，本发明实施例提供一种机器人定位导航系统，包括：机器人、1个或多个光定位解算装置、设于所述机器人顶部的基于dlp的定位信息投影装置；其中，

所述定位信息投影装置，用于投影包含坐标信息和机器人id的图像序列，其中，所述坐标信息是光定位解算装置在投影图像坐标系中的坐标信息；

位于所述定位信息投影装置投影区域内的一个或多个光定位解算装置，用于根据接收到的所述定位信息投影装置投影的图像序列，确定自身与机器人之间的相对坐标和机器人id，并根据确定的机器人id，将自身与机器人之间的相对坐标发送给对应的机器人，以便于对应的机器人通过坐标变换实现实时定位和导航。

进一步地，所述定位信息投影装置包括：投影机、固定所述投影机的固定支架和为所述定位信息投影装置供电的电源；

其中，所述投影机与支架固定后安装在机器人顶部；

所述定位信息投影装置与机器人之间的相对坐标和姿态已知。

进一步地，所述定位信息投影装置，用于按照固定频率在水平方向循环投影包含坐标信息和机器人id的图像序列；

位于所述定位信息投影装置投影区域内的一个或多个光定位解算装置，用于按照与所述定位信息投影装置投影相同的频率接收图像序列并解码，得到自身与机器人的相对坐标、机器人姿态以及机器人id。

进一步地，在所述定位信息投影装置按照固定频率在水平方向循环投影包含坐标信息和机器人id的图像序列之前，所述定位信息投影装置，还用于生成包含坐标信息和机器人id的m+1位bmp位图文件；

其中，bmp位图文件中每个像素点的前m位用于存储坐标信息和机器人id的格雷码序列，最后一位为保留位。

进一步地，设置每一次定位的投影周期为n帧时间，其中，n>m+1，前m帧顺序选择bmp位图文件的前m位，后n-m帧重复选择bmp位图文件的第m+1位；

其中，前m位为数据位，包含坐标信息和机器人id的格雷码序列；后n-m位为标志位，用以划分相邻的两个图像序列。

进一步地，每个光定位解算装置的全局坐标已知，四个光电传感器的安装位置已知；

所述光定位解算装置包括：设于预设位置的4个光电传感器和处理器；

每个光电传感器，用于按照与所述定位信息投影装置投影相同的频率接收图像序列，所述图像序列包括：坐标信息和机器人id的格雷码序列；

所述处理器，用于读取4个光电传感器接收到的格雷码序列并进行解码，确定当前光定位解算装置与所述定位信息投影装置之间的相对坐标，并结合已知的定位信息投影装置与机器人之间的相对坐标和姿态，得到机器人的三维坐标和三个自由度的姿态角。

进一步地，所述机器人，用于通过坐标变换进行实时定位，并通过多个光定位解算装置的位置构建环境地图，实现机器人的自主导航。

进一步地，所述系统还包括：设于机器人上的无线通信收发装置；

当环境中只有一个机器人时，机器人上的无线通信装置作为主节点，每一个光定位解算装置为一个子节点，在主节点与子节点建立的无线通信网络中，每一个光定位解算装置独立的将数据发送至机器人，所述数据包括：机器人的三维坐标和三个自由度的姿态角。

进一步地，当环境中有多个机器人时，添加一个无线通信装置作为主节点，每个机器人上的无线通信收发装置和光定位解算装置均为子节点，光定位解算装置将数据发送至主节点，主节点收到数据后按照机器人id将数据重新打包发送至对应的机器人。

本发明的上述技术方案的有益效果如下：

上述方案中，通过定位信息投影装置投影包含坐标信息和机器人id的图像序列；位于所述定位信息投影装置投影区域内的一个或多个光定位解算装置根据接收到的所述定位信息投影装置投影的图像序列，确定自身与机器人之间的相对坐标和机器人id，并根据确定的机器人id，将自身与机器人之间的相对坐标发送给对应的机器人，这样，对应的机器人通过坐标变换能够在室内环境下的自定位和导航，且具有很高的定位精度和可靠性。

附图说明

图1为本发明实施例提供的室内环境下的机器人定位导航系统的结构示意图；

图2为本发明实施例提供的定位专用图像文件示意图；

图3为本发明实施例提供的定位专用图像截图示意图；

图4为本发明实施例提供的定位信息投影装置配置示意图；

图5为本发明实施例提供的光定位解算装置原理示意图；

图6为本发明实施例提供的位置和姿态估计示意图；

图7为本发明实施例提供的无线通信流程示意图。

具体实施方式

为使本发明要解决的技术问题、技术方案和优点更加清楚，下面将结合附图及具体实施例进行详细描述。

本发明针对现有的仅利用gps导航或其他导航方式进行定位存在定位精度低、可靠性低的问题，提供一种机器人定位导航系统。

如图1所示，本发明实施例提供的机器人定位导航系统，包括：机器人、1个或多个光定位解算装置、设于所述机器人顶部的基于数字光学处理技术(digitallightprocessing，dlp)的定位信息投影装置；其中，

所述定位信息投影装置，用于投影包含坐标信息和机器人唯一标识符(id)的图像序列，其中，所述坐标信息是光定位解算装置在投影图像坐标系中的坐标信息；

位于所述定位信息投影装置投影区域内的一个或多个光定位解算装置，用于根据接收到的所述定位信息投影装置投影的图像序列，确定自身与机器人之间的相对坐标和机器人id，并根据确定的机器人id，将自身与机器人之间的相对坐标发送给对应的机器人，以便于对应的机器人通过坐标变换实现实时定位和导航。

本发明实施例所述的机器人定位导航系统，通过定位信息投影装置投影包含坐标信息和机器人id的图像序列；位于所述定位信息投影装置投影区域内的一个或多个光定位解算装置根据接收到的所述定位信息投影装置投影的图像序列，确定自身与机器人之间的相对坐标和机器人id，并根据确定的机器人id，将自身与机器人之间的相对坐标发送给对应的机器人，这样，对应的机器人通过坐标变换能够在室内环境下的自定位和导航，且具有很高的定位精度和可靠性。

本实施例中，投影的坐标信息指的是：光定位解算装置在投影图像坐标系中的坐标信息。

本实施例中，dlp的核心是dmd芯片，dmd芯片是一种数据微镜装置，通过控制微镜片对光线的偏转来实现显示投影图像的目的。基于dlp的定位信息投影装置具有投影频率高、光强高等特点

如图1所示，可以将基于dlp的定位信息投影装置安装在机器人上，将多个光定位解算装置放置在室内环境中，当机器人运动到某一区域时，其中一个或多个光定位解算装置位于投影区域内，此时，每个光定位解算装置独立计算其与机器人的相对坐标和机器人id，并将相对坐标信息发送至相应的机器人；这样，在一定区域内可以实现机器人在室内环境下的自定位和导航。

在前述机器人定位导航系统的具体实施方式中，进一步地，所述定位信息投影装置包括：投影机、固定所述投影机的固定支架和为所述定位信息投影装置供电的电源；

其中，所述投影机与支架固定后安装在机器人顶部；

所述定位信息投影装置与机器人之间的相对坐标和姿态已知。

本实施例中，所述投影机可以是dlplightcrafter投影机，dlplightcrafter投影机与固定支架固定后安装在机器人顶部，在水平方向循环投影。

本实施例中，所述定位信息投影装置安装完成后，定位信息投影装置与机器人的相对坐标和姿态已知，所述定位信息投影装置是本系统的核心组成部分，是系统正常工作的基础。

在前述机器人定位导航系统的具体实施方式中，进一步地，所述定位信息投影装置，用于按照固定频率在水平方向循环投影包含坐标信息和机器人id的图像序列；

位于所述定位信息投影装置投影区域内的一个或多个光定位解算装置，用于按照与所述定位信息投影装置投影相同的频率接收图像序列并解码，得到自身与机器人的相对坐标、机器人姿态以及机器人id。

本实施例中，在定位时，光定位解算装置在投影区域中的每一个点都有其唯一坐标。

本实施例中，可以设置定位信息投影装置的投影频率为4000hz，且设定每40幅图像为一个图像序列，则每秒可以进行100次定位，其定位频率大大高于视觉定位等其他的定位方式；同时，由于每个点在投影区域内有唯一坐标，保证了其有非常高的定位精度。

在前述机器人定位导航系统的具体实施方式中，进一步地，在所述定位信息投影装置按照固定频率在水平方向循环投影包含坐标信息和机器人id的图像序列之前，所述定位信息投影装置，还用于生成包含坐标信息和机器人id的m+1位bmp位图文件；

其中，bmp位图文件中每个像素点的前m位用于存储坐标信息和机器人id的格雷码序列，最后一位为保留位。

本实施例中，假设，所述定位信息投影装置的投影内容为一个通过预设的算法生成的包含坐标信息和机器人id的24位bmp位图文件，bmp位图文件中每个像素点的坐标信息为一段20位的格雷码序列，并将此格雷码放入像素点24位数据中的0-19位；每个机器人在系统中有一个唯一的id号，第20-22位放入机器人的id号，第23位(最后一位)为固定1，保留此位。

本实施例中，对于投影图像来说，图像的每一个像素点有唯一的二维坐标，将此二维坐标编码为20位格雷码后，存储在图像序列中，循环投影图像序列；光定位解算装置上的每个光电传感器可以读取图像中的一个像素点的图像序列信息，解码后得到的内容是投影图像坐标系中的坐标，即此光电传感器在投影图像坐标系中的坐标由此即可得到整个光定位解算装置与定位信息投影装置之间的相对坐标。

本实施例中，当投影图像的大小为1024x912时，每个点的坐标可以转换为一段长度为20的唯一的格雷码，将本机器人的id转换为一段长度为3的格雷码。将格雷码序列存储与图像像素的24位中；其有益效果是，大大减小了投影装置的运行负荷，将原本24张图像循环投影的任务简化为一幅图像，保证了固定的投影频率，增加了投影设备的稳定性。

在前述机器人定位导航系统的具体实施方式中，进一步地，设置每一次定位的投影周期为n帧时间，其中，n>m+1，前m帧顺序选择bmp位图文件的前m位，后n-m帧重复选择bmp位图文件的第m+1位；

其中，前m位为数据位，包含坐标信息和机器人id的格雷码序列；后n-m位为标志位，用以划分相邻的两个图像序列。

本实施例中，所述定位信息投影装置投影内容为一个包含像素点坐标信息和机器人id的图像序列；每次投影选择24位数据中的一位，此时投影内容可看作颜色位值为1的单色位图，投影内容只由黑色或白色，分别表示数字信号中的0和1。本实施例，可以设置图像序列长度为40帧，其中，前23帧顺序选择bmp位图文件的前23位，后17帧重复选择文件的第23位。此时，前23帧为数据位，包含坐标和机器人id信息，后17位为标志位，用以划分相邻两个图像序列。这样，每次投影的像素点只有两种颜色，对于光电传感器来说，读取时只有黑色和白色对应于数字信号的0和1，在顺序投影完23幅包含格雷码信息的单色图后，加入17个数据为1的位图，可以控制投影图像序列的时序，区分相邻的两个图像序列。

本实施例中，一张24位的bmp图像由三部分组成：

(1)位图头文件数据结构，它包含bmp图像文件的类型、显示内容等信息；

(2)位图信息数据结构，包含bmp图像的宽、高、压缩方法等信息；

(3)位图数据，在24位图中使用rgb。如图2所示，bmp文件的数据位可以分为b、g、r三个通道，每个通道有8位；即对于每一个像素点来说，都可以存储24位数据。

定位专用的图像文件是将每个像素点的20位坐标数据分别写到位图的数据位中。每个像素点的b通道8位数据位存储x轴坐标的高8位；g通道的位0—位1存储x轴坐标的低2位，位2—位7存储y轴坐标的高6位；r通道的位0—位3存储y轴坐标的低四位,位4—6存储机器人id，位7固定设置为1。通过专用算法生成定位专用图像文件(bmp位图文件)，图像文件截图如图3所示。

工作时，dlplightcrafter投影机加载定位专用的图像文件，将投影频率设定为固定的4000hz循环投影。在投影的图像序列中，依次添加b0—b7、g0—g7和r0—r6共23个帧，最后加上17个r7帧；其中，前23位用于传输坐标和机器人id信息，后17位作为时间间隔。使得每一次定位的投影周期为40帧时间，定位频率为每秒钟100次。在投影时，投影机循环投影图像的每一位，即每帧为一幅颜色位值为1的单色位图，位图数据为1时显示为白色，0显示为黑色。

基于dlp的定位信息投影装置的配置图如图4所示，配置完成后，将图像和参数上传至dlplightcrafter投影机，图像和参数保存在dlplightcrafter投影机自带的flash存储中，再次上电即可自动运行，不用重复操作。

在前述机器人定位导航系统的具体实施方式中，进一步地，每个光定位解算装置的全局坐标已知，四个光电传感器的安装位置已知；

所述光定位解算装置包括：设于预设位置的4个光电传感器和处理器；

每个光电传感器，用于按照与所述定位信息投影装置投影相同的频率接收图像序列，所述图像序列包括：坐标信息和机器人id的格雷码序列；

所述处理器，用于读取4个光电传感器接收到的格雷码序列并进行解码，确定当前光定位解算装置与所述定位信息投影装置之间的相对坐标，并结合已知的定位信息投影装置与机器人之间的相对坐标和姿态，得到机器人的三维坐标和三个自由度的姿态角。

本实施例中，在一个机器人定位导航系统中可以包含：一个或多个光定位解算装置；所述光定位解算装置独立于基于dlp的定位信息投影装置，如5所示，光定位解算装置的主体为一个专用于定位的嵌入式电路板，所述嵌入式电路板上设有排列成正方形的四个光电传感器、双向无线通信装置和数据显示模块等组件，这四个光电传感器分别位于光定位解算装置上一个边长为50mm的正方形的四个顶点上，每个光定位解算装置的全局坐标已知，进而其上的四个光电传感器的全局坐标也是已知的。由于四个顶点可以唯一确定一个平面，四个点所组成的正方形固定大小，通过透视变换可以得到此平面相对于机器人的相对坐标和姿态。如果只有3个以下的光电传感器，则只能计算得到机器人的二维坐标和一个旋转角，本发明通过设置四个光电传感器共可以得到机器人六个自由度的信息，其中，包括三维坐标和三个姿态角。

本实施例中，在一个系统中包含：一个或多个独立的光定位解算装置，这样，可以通过不同位置的光定位解算装置可以实现机器人的导航，扩大了机器人的运动范围；同时，当投影区域中有多个光定位解算装置时，可以通过多个相对坐标进行矫正，增加机器人定位的精度。

本实施例中，光定位解算装置上的每个光电传感器按照与所述定位信息投影装置投影相同的频率接收定位信息投影装置投影的图像序列，即可得到包含坐标和机器人id的格雷码序列，图像序列中黑色和白色分别对应于数字信号的0和1。

光定位解算装置上的处理器，用于读取4个光电传感器接收到的格雷码序列并进行解码，运行光定位解算算法确定当前光定位解算装置与所述定位信息投影装置之间的相对坐标，并结合已知的定位信息投影装置与机器人之间的相对坐标和姿态，得到机器人的三维坐标和三个自由度的姿态角。这样，每个光定位解算装置有专用的处理器，可以独立计算得到其与机器人之间的相对坐标和机器人的姿态，并将结果发送至的机器人，保证了多个光定位解算装置之间的同步；同时减少了无线通信网络中的数据量，从而提高机器人的定位和控制频率，提高导航精度和能力。

本实施例中，光电传感器是采用光电元件作为检测元件的传感器，光电传感器可以将光信号转换成电信号，通过电路设定阈值，当光强大于阈值时，传感器输出数字信号1；当光强小于阈值时，传感器输出数字信号0。工作时，光电传感器实时接收投影。当产生上升沿或下降沿时，光电传感器触发中断，由位于光定位解算装置上的处理器读取传感器的值。通过不断的读取，光电传感器即可接收到一段长度为40的格雷码编码，其中，高23位为数据位，低17位为空白位。

本实施例中，四个光电传感器分别放置在边长为50mm的正方形的四个顶点上，四个光电传感器的坐标都可以通过计算得到，由于光定位解算装置在投影坐标系中发生了平移和旋转，因此四个光电传感器的坐标作为顶点组成的四边形不一定是四边形，此时要对光定位解算装置进行空间几何变换，正确估计光定位解算装置与基于dlp的定位信息投影装置之间的变换，即实现从二维到三维的姿态估计。通过估计，找到光定位解算装置与基于dlp的定位信息投影装置之间的一个欧氏空间的变换，该变换仅由旋转和坐标平移构成，实现估计。

在系统工作时，光定位结算装置可能会收到外界光束的影响，导致接收错误。因此，在进行估计之前，先判断传感器坐标是否有误，本发明设定了四个判定条件：

四个传感器按照顺时针方向编号1—4，设定最小边长阈值，检测相邻两个传感器之间的距离，若存在某一边长小于阈值，则判定为接收错误。

设定最小边长之和阈值，计算相邻传感器之间的距离之和。若边长之和小于阈值，则判定为接收错误。

设定最大边长之差阈值，分别计算相邻两边长之差，若存在边长之差小于阈值，则判定为接收错误。

对相邻三个传感器坐标进行两向量叉积运算，判断四次计算之后的符号是否相同，若四次运算结果的符号存在不同，即四个点相连无法组成闭合的四边形，则判定为接收错误。

判定结束后，若判定为接收错误，则终止本次定位，等待下一次定位；否则进行位置和姿态估计。

如图6所示为估计位置和姿态示意图，图6中，点p为dlplightcrafter投影机的中心点，点a1—a4是光电传感器在现实坐标系中的三维点，a1—a4点是a1—a4投影到图像坐标系(投影坐标系)中的坐标点。

姿态估计的目的就是在已知图像平面投影点(a1—a4)和现实坐标系中的点(a1—a4)，得到光定位解算装置和投影机之间的相对变换关系。本系统中，如果将接收板放在xy平面上，即z轴的分量为0。放置在中心时，坐标的中心点为(0,0,0)，四个传感器的坐标分别为b1(-25,-25,0)、b2(25，-25，0)、b3(25,25,0)、b4(-25,25,0)。将(a1—a4)与(b1—b4)的坐标作为参数进行计算，得到(a1—a4)与(b1—b4)两个平面的变换；这种变换由平移向量和旋转向量组成。

由此，可以通过得到平移向量和旋转向量计算光定位解算装置与基于dlp的定位信息投影装置之间的相对坐标和姿态，其中包括机器人在空间内的三维坐标和三个自由度的姿态角，实现对机器人的定位。

在前述机器人定位导航系统的具体实施方式中，进一步地，所述机器人，用于通过坐标变换进行实时定位，并通过多个光定位解算装置的位置构建环境地图，实现机器人的自主导航。

本实施例所述的机器人定位导航系统还包括：设于机器人上的无线通信收发装置；本系统中可以有一个或多个光定位解算装置，每个光定位解算装置可以独立计算机器人相对于自身的相对坐标，多个定位信息投影装置之间建立一个专用的树型无线通信网络；这样，通过专用的树型无线通信网络，多个光定位解算装置可以将数据单跳发送至机器人，保证了定位数据的实时性和稳定性。

所述专用的树型无线通信网络包括以下两种模式：

1)当环境中只有一个机器人时，机器人上的无线通信装置作为主节点，每一个光定位解算装置为一个子节点，在主节点与子节点建立的无线通信网络中，主节点与子节点使用专用的通信协议进行通信，通信流程图如图7所示，其建立起一个稳定、可靠的无线通信网络，每一个光定位解算装置独立的将数据发送至机器人，所述数据包括：机器人的三维坐标和三个自由度的姿态角。

2)当环境中有多个机器人时，添加一个无线通信装置作为主节点，每个机器人上的无线通信收发装置和光定位解算装置均为子节点，光定位解算装置将数据发送至主节点，主节点收到数据后按照机器人id将数据重新打包发送至对应的机器人。

当一个或多个独立的光定位解算装置位于投影区域内，光定位解算装置可以独立计算其与机器人之间的相对坐标和机器人id。并将计算得到的数据通过专用的无线通信网络发送给的机器人，从而使机器人可以进行实时定位，并通过多个光定位解算装置的位置构建环境地图，实现机器人的自主导航。

本实施例中，当环境中有多个机器人时，可以通过机器人id区分机器人，并将数据准确发送至指定机器人。

需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。

以上所述是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明所述原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。