CN202702247U

CN202702247U - Rapid and accurate positioning system used for indoor mobile robot

Info

Publication number: CN202702247U
Application number: CN 201220375716
Authority: CN
Inventors: 周风余; 田国会; 王然; 闫云章; 韩旭; 台述鹏; 王小龙
Original assignee: Shandong University
Current assignee: Shandong University
Priority date: 2012-07-31
Filing date: 2012-07-31
Publication date: 2013-01-30
Anticipated expiration: 2022-07-31

Abstract

The utility model relates to a fast and precise positioning system for an indoor mobile robot, which includes a positioning sensor arranged on the robot and a plurality of infrared-reflecting passive tags installed on the ceiling of the working area; the positioning sensor includes an image processing chip, It is connected with the storage module, COMS camera, data interface, power supply and infrared emitting module. The infrared emitting module includes a plurality of infrared tubes, which surround the COMS camera and are divided into several groups; the passive tag is a tag with multiple Marking points, marking points are divided into two types, the first type is direction points, to determine the direction of the coordinate axis, which is the only way to determine the direction, only three corners of the four corners of the label can have direction mark points, the second type The code points, that is, the remaining mark points, are used to determine the ID number of the label; reflective infrared materials are pasted on the direction mark points, and reflective infrared materials are pasted on the remaining code mark points according to the coding requirements.

Description

Fast and precise positioning system for indoor mobile robots

技术领域 technical field

本实用新型涉及一种用于室内移动机器人的快速精确定位系统，属于检测技术、图像处理及机器人导航领域。The utility model relates to a fast and precise positioning system for an indoor mobile robot, which belongs to the fields of detection technology, image processing and robot navigation.

背景技术 Background technique

室内定位，是在室内环境下，根据先验环境地图信息、物体位姿的当前估计以及传感器的观测数据等输入信息，经过一定的分析和计算，得到更为准确的物体位姿的估计。对于工作在家庭、医院和办公场所等室内环境下的智能服务机器人，其准确的定位是机器人导航的前提，是完成服务任务的重要保证。Indoor positioning is to obtain a more accurate estimation of the object's pose in an indoor environment, based on input information such as the prior environmental map information, the current estimation of the object's pose, and the observation data of the sensor, after certain analysis and calculation. For intelligent service robots working in indoor environments such as homes, hospitals and offices, their accurate positioning is the prerequisite for robot navigation and an important guarantee for completing service tasks.

室内定位传感器按照所使用的定位技术，可分为绝对定位传感器和相对定位传感器。绝对定位主要采用导航信标、主动或被动标识等技术进行定位；相对定位是测量物体相对于初始位置的距离和方向来确定物体的当前位置，也称为航位推算法。目前室内机器人定位所采用的方法主要有：基于RFID的方法、基于无线传感网络的方法和基于里程计和惯导模块的方法等等。Indoor positioning sensors can be divided into absolute positioning sensors and relative positioning sensors according to the positioning technology used. Absolute positioning mainly uses navigation beacons, active or passive identification and other technologies for positioning; relative positioning is to measure the distance and direction of an object relative to the initial position to determine the current position of the object, also known as dead reckoning. At present, the methods used for indoor robot positioning mainly include: methods based on RFID, methods based on wireless sensor networks, methods based on odometers and inertial navigation modules, and so on.

基于RFID的方法，一般是先读取RFID数据后粗略定位，再利用超声传感器进行距离测量，从而获得定位信息。该方法要求在放置RFID标签时，要充分考虑各种可能性，使用不便，精度较低，适合环境简单，且对精度要求不高场合下的定位。The RFID-based method generally reads the RFID data first and then roughly locates it, and then uses the ultrasonic sensor to measure the distance to obtain the positioning information. This method requires full consideration of various possibilities when placing RFID tags. It is inconvenient to use and has low precision, and is suitable for positioning in simple environments and low precision requirements.

基于无线传感网络的方法，如Wi-Fi技术，Zibgee技术等，利用信号强度进行定位，该方法需要架设无线传感网络，成本高，且无线信号易受干扰，精度较差。Methods based on wireless sensor networks, such as Wi-Fi technology, Zibgee technology, etc., use signal strength for positioning. This method needs to set up a wireless sensor network, which is costly, and the wireless signal is susceptible to interference and has poor accuracy.

基于惯导模块的方法利用陀螺仪、加速度计、磁力计等惯性导航模块，再结合里程计，对物体的航向、速度和加速度进行实时记录，累积里程，通过计算得到物体相对于初始位置的坐标。该方法存在累积误差和漂移，时间一长或路况不佳时，精度难以保证。The method based on the inertial navigation module uses inertial navigation modules such as gyroscopes, accelerometers, and magnetometers, combined with the odometer, to record the course, speed and acceleration of the object in real time, accumulate the mileage, and obtain the coordinates of the object relative to the initial position by calculation . This method has accumulated errors and drifts, and the accuracy is difficult to guarantee when the time is long or the road conditions are not good.

专利号为201110260388.5的发明专利，使用红外发射二极管制作点阵路标，并贴附于室内天花板上，广角红外摄像机固定在移动机器人身上，向上拍摄红外路标，通过机器人身上的计算机进行图像分析、实时计算出机器人的位姿。这种方法具有一定的局限性，其点阵路标属于有源标签，每一个标签都是一块电路板，并需要电源供电，成本高，安装、使用不方便。其次，其图像处理使用机载工控计算机，体积较大，成本较高，对不配置工控计算机的中小型机器人根本无法使用。The invention patent with the patent number 201110260388.5 uses infrared emitting diodes to make dot-matrix road signs and attaches them to the indoor ceiling. The wide-angle infrared camera is fixed on the mobile robot to shoot up the infrared road signs, and the computer on the robot performs image analysis and real-time calculation. Get the pose of the robot. This method has certain limitations. The dot-matrix road signs are active tags. Each tag is a circuit board and needs power supply. The cost is high and installation and use are inconvenient. Secondly, its image processing uses an onboard industrial computer, which is large in size and high in cost, and cannot be used for small and medium-sized robots that do not have an industrial computer.

发明内容Contents of the invention

本实用新型的目的就是为解决上述问题，提供一种用于室内移动机器人的快速精确定位系统及其工作方法，该系统由安装在机器人上的定位传感器和粘贴在工作区域天花板上的多个反射红外线的无源标签组成，系统利用传感器红外发射模块发出红外线，照射天花板上的标签，传感器上的微型CMOS摄像头采集标签光斑图像，TMS320DM642DSP芯片对图像进行处理，获得传感器相对于标签的X坐标值、Y坐标值、航向角及高度等位置信息，实现准确定位。采用红外线的目的是为了有效避免可见光对传感器的影响，提高定位的精度和鲁棒性。The purpose of this utility model is to solve the above problems, to provide a fast and accurate positioning system for indoor mobile robots and its working method. Composed of infrared passive tags, the system uses the infrared transmitter module of the sensor to emit infrared rays to irradiate the tags on the ceiling. The micro CMOS camera on the sensor collects the tag spot image, and the TMS320DM642DSP chip processes the image to obtain the X coordinate value of the sensor relative to the tag. Position information such as Y coordinate value, heading angle and altitude to achieve accurate positioning. The purpose of using infrared is to effectively avoid the influence of visible light on the sensor and improve the accuracy and robustness of positioning.

为实现上述目的，本实用新型采用如下技术方案：In order to achieve the above object, the utility model adopts the following technical solutions:

一种用于室内移动机器人的快速精确定位系统，它包括安装在机器人上的定位传感器和粘贴在工作区域天花板上的多个反射红外线的无源标签；其中，定位传感器包括图像处理芯片，图像处理芯片分别与存储模块、COMS摄像机、数据接口、电源以及红外发射模块连接，红外发射模块包括多个红外管，它们围绕在COMS摄像头周围并分为几组；无源标签为标识标签，在标签上共有15个标志点位置可供粘贴反射红外线材料，标志点被分为两类，第一类为方向点，即来确定坐标轴的方向，为了唯一确定方向，标签的四个角中只能有任意三个角的位置有方向标志点，余下一个顶点不能粘贴反射红外线材料，每个标签上都必须含有这三个方向点；第二类为编码点，即剩余的各标志点，每个编码点都表示二进制的一位，可以通过编码点的组合来确定该标签的ID编号；在方向点上粘有反射红外线材料，在其余编码点上根据编码需要选择全部或部分粘贴反射红外线材料。A fast and precise positioning system for indoor mobile robots, which includes a positioning sensor installed on the robot and a plurality of passive tags that reflect infrared rays pasted on the ceiling of the work area; wherein the positioning sensor includes an image processing chip, image processing The chip is respectively connected with the storage module, COMS camera, data interface, power supply and infrared emission module. The infrared emission module includes a plurality of infrared tubes, which surround the COMS camera and are divided into several groups; the passive label is an identification label, on the label A total of 15 marking points are available for pasting reflective infrared materials. The marking points are divided into two types. The first type is direction points, which are used to determine the direction of the coordinate axis. In order to uniquely determine the direction, there can only be There are direction marking points at any three corners, and the remaining apex cannot be pasted with reflective infrared materials. Each label must contain these three direction points; the second type is code points, that is, the remaining mark points, each code The dots represent one bit of binary, and the ID number of the label can be determined by the combination of coding points; reflective infrared materials are pasted on the direction points, and all or part of the reflective infrared materials can be pasted on the remaining coding points according to the coding needs.

所述数据接口为UART接口，所述存储模块为SDRAM、FLASH、EEPROM。The data interface is a UART interface, and the storage module is SDRAM, FLASH, EEPROM.

所述红外管有12个，每4个分为一组，共3组；开始进行测量时3组全部打开，测得结果后随即关闭一组，再进行测量，如果测量精度没有影响，则再关闭一组，目的是保证精度不受影响且使用的红外管数量最少，达到节省能源，减少发热量的目的。There are 12 infrared tubes, each of which is divided into 4 groups, a total of 3 groups; when the measurement is started, all 3 groups are turned on, and one group is closed immediately after the measurement results are obtained, and then the measurement is carried out. If the measurement accuracy is not affected, then The purpose of turning off one group is to ensure that the accuracy is not affected and the number of infrared tubes used is the least, so as to save energy and reduce heat generation.

该定位系统的工作步骤为：The working steps of the positioning system are:

1）根据需要，在机器人工作区域的天花板上粘贴适当数量的标签，标签之间的距离需大于1.5米，定位传感器与标签之间的距离范围为1米到2.5米；1) According to the needs, paste an appropriate number of labels on the ceiling of the robot's working area. The distance between the labels must be greater than 1.5 meters, and the distance between the positioning sensor and the labels ranges from 1 meter to 2.5 meters;

2）将定位传感器安装在机器人上，上电并初始化；2) Install the positioning sensor on the robot, power on and initialize;

3）读取存储模块完成配置；3) Read the storage module to complete the configuration;

4）判断是否开始检测，若否，则继续等待检测命令；若是，则转入步骤5）；4) Determine whether to start detection, if not, continue to wait for the detection command; if so, go to step 5);

5）COMS摄像头接收标签反射回来的红外光斑图像，对图像进行预处理，然后检测视野内是否存在有效标签，若没有则转到7），如果视野中有效标签的数量多于一个，则应从中选择最优的标签进行反光点识别；从而确定标签中三个方向点及标签坐标系XOY，从而确定机器人在该标签坐标系下的X、Y坐标信息和航向角信息，并确定定位传感器与粘贴标签天花板之间的高度信息，同时确定标签的ID信息；5) The COMS camera receives the infrared spot image reflected by the label, preprocesses the image, and then detects whether there is a valid label in the field of view. If not, go to 7). If there are more than one valid label in the field of view, the Select the optimal label for reflective point recognition; thus determine the three direction points in the label and the label coordinate system XOY, thereby determine the X, Y coordinate information and heading angle information of the robot in the label coordinate system, and determine the position sensor and paste The height information between the label ceilings and the ID information of the labels are determined at the same time;

6）将处理结果上传到机器人的上位机进行显示和控制；6) Upload the processing results to the host computer of the robot for display and control;

7）由上位机决定是否停止检测，若否，则返回步骤5）；若是，则结束本次检测过程。7) The host computer decides whether to stop the detection, if not, return to step 5); if yes, end the detection process.

所述步骤2）中，在定位传感器投入使用之前必须对微型CMOS摄像机进行标定，来获得内参数和畸变参数：In the step 2), before the positioning sensor is put into use, the micro CMOS camera must be calibrated to obtain internal parameters and distortion parameters:

标定时采用基于2D平面标定板的标定算法，在标定过程中假设摄像机内部参数始终不变，即无论摄像机从任何角度拍摄平面模板，摄像机内部参数都是常数，只有外部参数发生变化，基本步骤为：The calibration algorithm based on the 2D plane calibration board is used in the calibration process. In the calibration process, it is assumed that the internal parameters of the camera remain unchanged, that is, no matter the camera shoots the plane template from any angle, the internal parameters of the camera are constant, and only the external parameters change. The basic steps are as follows: :

（1）打印一张标准的国际象棋盘作为标定板，并将其贴在一个刚性的平面上；(1) Print a standard chess board as a calibration board and stick it on a rigid plane;

（2）从不同角度拍摄多张标定板图像，数量多可使标定更准确；(2) Take multiple images of the calibration board from different angles, and a large number can make the calibration more accurate;

（3）检测标定板上的特征点，并确定图像坐标和实际坐标；(3) Detect the feature points on the calibration board, and determine the image coordinates and actual coordinates;

（4）利用线性模型计算出摄像机的内部参数；(4) Use the linear model to calculate the internal parameters of the camera;

（5）利用畸变模型，对摄像机内参数进行优化，求出所有参数，确定内参数后对图像进行矫正，从而为后续计算做好准备。(5) Use the distortion model to optimize the internal parameters of the camera, find out all the parameters, and correct the image after determining the internal parameters, so as to prepare for the subsequent calculation.

所述步骤5）中，在红外图像中确定有效标签和最优标签的过程为：对获取的红外图像进行预处理，首先对红外图像进行高斯平滑滤波，之后选取合适的阈值进行二值化处理获得二值化图像，提取二值图像中的轮廓，去除较大和较小的轮廓以减小噪声的干扰。In the step 5), the process of determining the effective label and the optimal label in the infrared image is: preprocessing the acquired infrared image, first performing Gaussian smoothing filtering on the infrared image, and then selecting an appropriate threshold for binarization Obtain a binary image, extract the contours in the binary image, remove larger and smaller contours to reduce noise interference.

定位传感器采用对上面获取的二值图像进行水平投影和垂直投影相结合的方法来定位标签在红外图像中的位置，使用最近邻的法则来确定当前的最优标签。The positioning sensor uses the combination of horizontal projection and vertical projection of the binary image acquired above to locate the position of the label in the infrared image, and uses the nearest neighbor rule to determine the current optimal label.

给定一条直线，用垂直该直线的一簇等间距直线将二值图像分割成若干部分，统计每一部分中像素值为1的像素个数为二值图像在给定直线上的该部分的投影；当给定直线为水平或垂直直线时，计算二值图像每一列或每一行上像素值为1的像素数量，就得到了二值图像的水平和垂直投影，在某些应用中投影可以作为物体识别的一个特征，图14a表示了处理后得到的二值图像，图14b和图14c分别表示了二值图像的垂直投影和水平投影，具体的分类可见如下步骤：Given a straight line, a binary image is divided into several parts by a cluster of equally spaced straight lines perpendicular to the straight line, and the number of pixels with a pixel value of 1 in each part is counted as the projection of the part of the binary image on the given straight line ; When the given line is a horizontal or vertical line, calculate the number of pixels with a pixel value of 1 on each column or row of the binary image, and obtain the horizontal and vertical projections of the binary image. In some applications, the projection can be used as A feature of object recognition, Figure 14a shows the binary image obtained after processing, Figure 14b and Figure 14c respectively show the vertical projection and horizontal projection of the binary image, the specific classification can be seen in the following steps:

（1）对获取的垂直投影图像由左向右逐次遍历得到其相邻投影像素簇的间隔距离d₁,d₂…d_n，同样对水平投影图像采取由上向下逐次遍历可得相邻投影像素簇的间隔距离d′₁，d′₂…d′_n，由于标签放置时间隔较远，因而不同标签间的投影像素簇也会间隔较远的距离，可以以间隔距离作为区分标签的依据。(1) Traverse the obtained vertical projection image from left to right to obtain the distances d ₁ , d ₂ ...d _n between adjacent projected pixel clusters, and similarly traverse the horizontal projection image from top to bottom to obtain the adjacent The spacing distance of projected pixel clusters d′ ₁ , d′ ₂ …d′ _n , because the labels are placed far apart, the projected pixel clusters between different labels will also be separated by a long distance, and the spacing distance can be used as the criterion for distinguishing labels in accordance with.

（2）确定投影图像上标签对应的投影区域，取上述垂直投影图像像素簇间隔距离的最小值d和水平投影图像像素簇间隔距离的最小值d′的七倍，即7d和7d′作为阈值进行近邻分类，具体分类方法：对于垂直投影图像由左向右遍历，找到第一个投影像素簇，统计其余投影像素簇到该像素簇的距离，若是小于7d，则属于同一个标签的投影区域，然后以下一个大于7d的投影像素簇为基准，继续向右遍历，仍是小于7d的为第二个标签所在的投影区域，依次向下遍历可得到不同的标签在垂直投影图像中的不同区域，对于水平投影图像，由上向下进行遍历，以7d′为距离基准，采用上述方法可以得到标签在水平投影图像上的对应区域。如图14b和14c所示，可以得到A、B、C、D四个投影区域。(2) Determine the projection area corresponding to the label on the projected image, and take seven times the minimum value d of the distance between the pixel clusters of the vertical projected image and the minimum value d′ of the distance between the pixel clusters of the horizontal projected image, that is, 7d and 7d′ as the threshold Carry out neighbor classification, the specific classification method: For vertical projection images, traverse from left to right, find the first projection pixel cluster, and count the distances from other projection pixel clusters to this pixel cluster, if it is less than 7d, it belongs to the projection area of the same label , and then continue to traverse to the right based on the next projected pixel cluster larger than 7d, and the projected area where the second label is still smaller than 7d, and traverse down in turn to get different labels in different areas in the vertical projection image , for the horizontal projection image, traverse from top to bottom, and take 7d' as the distance reference, and use the above method to obtain the corresponding area of the label on the horizontal projection image. As shown in Figures 14b and 14c, four projection areas A, B, C, and D can be obtained.

（3）找到原图像上的标签区域，以（2）中确定的每个投影区域的边缘做直线，垂直投影区域的直线和水平投影区域的直线在原图像上可以相交得到不同的矩形区域，该矩形区域则为标签可能存在的区域，图14d表示了有相交直线所获取的四个矩形区域，其中有不合理的区域，即有两个矩形区域不存在标签，下一步将介绍如何去除不合理区域，得到有效标签。(3) Find the label area on the original image, and make a straight line with the edge of each projection area determined in (2). The straight line of the vertical projection area and the straight line of the horizontal projection area can intersect on the original image to obtain different rectangular areas. The rectangular area is the area where the label may exist. Figure 14d shows the four rectangular areas obtained by intersecting straight lines. There are unreasonable areas, that is, there are two rectangular areas without labels. The next step will introduce how to remove the unreasonable region, get valid labels.

（4）去除不合理的区域，得到有效标签，经过步骤（3）可以得到标签可能所在的区域，但是需要去除无效区域已得到有效的标签所在区域，主要是排除两方面的干扰：一种是排除相交矩形区域内没有标签的情况，一种是排除标签在图像边缘以至于没有得到完整标签的情况。(4) Remove unreasonable areas and get effective labels. After step (3), you can get the area where the label may be located, but you need to remove the invalid area to get the area where the effective label is located. The main purpose is to eliminate two aspects of interference: one is Exclude the case where there is no label in the intersecting rectangular area, and one is to exclude the case where the label is at the edge of the image so that the complete label is not obtained.

（5）在（3）中已获得可能的投影区域，且通过投影边缘区域的边缘直线可以确定图像中标签可能所在区域的坐标范围，通过检测区域内有无反光点即可判断该区域内是否存在标签，若有反光点则存在标签，若无则不存在。由于在图像边缘处误检测的概率较大，所以，当标签靠近边缘时需要舍弃，通过区域的坐标可以很简单的判断出该标签是否是在图像边缘。排除掉干扰后剩下的即为有效标签。(5) The possible projection area has been obtained in (3), and the coordinate range of the area where the label may be located in the image can be determined by projecting the edge line of the edge area, and whether the area is judged by detecting whether there is a reflective point in the area There is a label, if there is a reflective point, then there is a label, if there is no, then there is no label. Since the probability of misdetection at the edge of the image is high, when the label is close to the edge, it needs to be discarded. Through the coordinates of the area, it can be easily judged whether the label is at the edge of the image. What remains after removing the interference is the valid label.

（6）若图像中有效标签的数量多于一个，需要选择最优标签。图14d表示了获取了标签所在的图像区域。求解两个矩形区域中心坐标a(x_a,y_a)和b(x_b,y_b)，图像中心点坐标o(x_o,y_o)，计算得出oa和ob距离：(6) If the number of valid labels in the image is more than one, the optimal label needs to be selected. Figure 14d shows the region of the image where the labels were acquired. Solve the center coordinates a(x _a ,y _a ) and b(x _b ,y _b ) of two rectangular areas, and the coordinates o(x _o ,y _o ) of the center point of the image, and calculate the distance between oa and ob:

${d d}_{oa oa} = = \sqrt{{(({x x}_{o o} - - {x x}_{a a}))}^{22} + + {(({y the y}_{o o} - - {y the y}_{a a}))}^{22}}$

${d d}_{ob ob} = = \sqrt{{(({x x}_{o o} - - {x x}_{b b}))}^{22} + + {(({y the y}_{o o} - - {y the y}_{b b}))}^{22}}$

取距离相对较小的标签为当前条件下的有效标签，当标签的个数多余两个时，判断方式与上面类似。Take the label with a relatively small distance as the effective label under the current conditions. When the number of labels exceeds two, the judgment method is similar to the above.

所述步骤5）中标签反光点识别过程为：The label reflective point identification process in step 5) is:

（1）首先定义一个梯度：规定一个像素沿某方向的梯度为该像素与该方向下一个像素的灰度值之差，自上而下自左向右搜索，若在某一像素点，右梯度大于设定阈值ε₁，则认为该点是标志区域内一点；(1) First define a gradient: specify that the gradient of a pixel along a certain direction is the difference between the gray value of this pixel and the next pixel in this direction, search from top to bottom from left to right, if at a certain pixel point, right If the gradient is greater than the set threshold ε ₁ , the point is considered to be a point in the marked area;

（2）以该点为起始点，搜索其八邻域内最大灰度值，最终找到标志区域内最大灰度值点；(2) With this point as the starting point, search for the maximum gray value in its eight neighborhoods, and finally find the point with the maximum gray value in the marked area;

（3）以中心点为起始点，向上、下、左、右四个方向搜索，当某点像素灰度值小于设定值并且梯度小于设定阈值时，则认为该点为标志区域的边界点；(3) With the center point as the starting point, search in four directions: up, down, left, and right. When the pixel gray value of a point is less than the set value and the gradient is less than the set threshold, the point is considered to be the boundary of the marked area point;

（4）分别以中心对角线相连的点为起始点开始水平和垂直搜索，直到找到标志区域边界点，其他搜索以此类推；(4) Start horizontal and vertical searches from the points connected by the diagonal lines of the center, until the boundary points of the marked area are found, and so on for other searches;

（5）检测出的区域并不一定都是标志区域，还需要去掉干扰区域，首先计算标志区域内所有像素的平均值，平均值过低则排除，然后计算标志区域的尺寸和边界长度，不符合阈值的排除；(5) The detected areas are not necessarily marked areas, and the interference area needs to be removed. First, calculate the average value of all pixels in the marked area. If the average value is too low, exclude it. Then calculate the size and boundary length of the marked area. Exclusions that meet the threshold;

经过以上步骤确定所有标签反光点，采用建立灰度直方图的方法，选取标志区域和背景区域灰度的峰值间的谷底做为阈值，然后用图像各点灰度值减去该阈值，获得一幅新的图像；确定中心点时，选用灰度加权质心法，所谓灰度加权质心是指，以灰度值为权重，计算图像内所有标志点象素坐标的平均值，公式如下：Through the above steps to determine all the reflective points of the label, adopt the method of establishing a grayscale histogram, select the valley between the peaks of the grayscale of the sign area and the background area as the threshold, and then subtract the threshold from the grayscale value of each point in the image to obtain a A new image; when determining the center point, the gray-scale weighted centroid method is selected. The so-called gray-scale weighted centroid means that the average value of the pixel coordinates of all marker points in the image is calculated with the gray value as the weight. The formula is as follows:

$\{\begin{matrix} {x x}_{00} = = \frac{\underset{((i i,, j j)) &Element; &Element; S S}{Σ Σ} i i {w w}_{i i,, j j}}{\underset{((i i,, j j)) &Element; &Element; S S}{Σ Σ} {w w}_{i i,, j j}} \\ {y the y}_{00} = = \frac{\underset{((i i,, j j)) &Element; &Element; S S}{Σ Σ} j j {w w}_{i i,, j j}}{\underset{((i i,, j j)) &Element; &Element; S S}{Σ Σ} {w w}_{i i,, j j}} \end{matrix}$

公式中x₀和y₀为计算所得中心点的像素坐标，(i,j)表示图像中的某个像素点，i和j分别表示像素点的x轴和y轴的坐标值，w_i，j表示像素点(i,j)处的像素灰度值。In the formula, x ₀ and y ₀ are the pixel coordinates of the calculated center point, (i, j) represents a certain pixel point in the image, i and j represent the coordinate values of the x-axis and y-axis of the pixel point respectively, w _{i, j} represents the pixel gray value at the pixel point (i, j).

所述步骤5）中，确定标签中三个方向点的过程为：计算标签中任意两两标志点之间的连线长度，选取其中最长者，并且提取最长连线所对应的两个标志点，将这两个标志点分别标记为A和B，计算A和B连线的中点，然后分别计算标签中除A和B点外其他所有点到该中点的距离，选取其中最长的，并提取该最长距离所对应的标志点，将其标记为O，则A、O、B三点为三个方向点。In the step 5), the process of determining the three direction points in the label is: calculate the length of the line between any two marker points in the label, select the longest one, and extract the two points corresponding to the longest line. mark points, mark these two mark points as A and B respectively, calculate the midpoint of the line connecting A and B, and then calculate the distances from all points in the label except points A and B to the midpoint, and select the most Long, and extract the mark point corresponding to the longest distance, mark it as O, then A, O, B three points are three direction points.

标签坐标系的确定过程为：O点为标签坐标系的坐标原点，设O点在图像坐标系中的像素坐标为(x₁,y₁)，在A和B中任选一点绕O顺时针旋转90度，设选取的点的像素坐标为(x₂,y₂)，剩余另一点的坐标为(x₃,y₃)，旋转后对应点P像素坐标（x，y）的计算公式为：The process of determining the label coordinate system is as follows: Point O is the coordinate origin of the label coordinate system, and the pixel coordinates of point O in the image coordinate system are (x ₁ , y ₁ ), and a point in A and B is chosen to go clockwise around O Rotate 90 degrees, set the pixel coordinates of the selected point as (x ₂ , y ₂ ), and the coordinates of the other point as (x ₃ , y ₃ ), the calculation formula for the pixel coordinates (x, y) of the corresponding point P after rotation is :

$\{\begin{matrix} x x = = (({x x}_{22} - - {x x}_{11})) cos cos ((α α)) - - (({y the y}_{22} - - {y the y}_{11})) sin sin ((α α)) + + {x x}_{11} \\ y the y = = (({y the y}_{22} - - {y the y}_{11})) cos cos ((α α)) + + (({x x}_{22} - - {x x}_{11})) sin sin ((α α)) + + {y the y}_{11} \end{matrix}$

其中α表示旋转角度。where α represents the rotation angle.

判断δ夹角：Judgment δ included angle:

δ＝(x-x₁)(x₃-x₁)+(y-y₁)(y₃-y₁)δ＝(xx ₁ )(x ₃ -x ₁ )+(yy ₁ )(y ₃ -y ₁ )

若δ>0则两向量之间的夹角为锐角，则选取的点(x₂,y₂)所对应的坐标轴为X轴，(x₃,y₃)点所对应的坐标轴为Y轴，若δ<0则两向量之间的夹角为钝角，则选取的点(x₂,y₂)所对应的坐标轴为Y轴，(x₃,y₃)所对应的坐标轴为X轴，由此对应关系可确定标签坐标系XOY。If δ>0, the angle between the two vectors is an acute angle, then the coordinate axis corresponding to the selected point (x ₂ , y ₂ ) is the X axis, and the coordinate axis corresponding to the point (x ₃ , y ₃ ) is Y axis, if δ<0, the angle between the two vectors is an obtuse angle, then the coordinate axis corresponding to the selected point (x ₂ , y ₂ ) is the Y axis, and the coordinate axis corresponding to (x ₃ , y ₃ ) is The X axis, from which the corresponding relationship can determine the label coordinate system XOY.

所述步骤5）中，位置信息的确定过程为：在定位传感器获取的图像中，R点为定位传感器在图像中的投影位置，该位置是图像中心。在图像中按照确定标签坐标系的规则建立AOB坐标系，其中OA为X轴方向，OB为Y轴方向。In the step 5), the determination process of the position information is: in the image acquired by the positioning sensor, point R is the projection position of the positioning sensor in the image, and this position is the center of the image. In the image, establish the AOB coordinate system according to the rules for determining the label coordinate system, where OA is the X-axis direction, and OB is the Y-axis direction.

求解定位传感器在该标签下的坐标时，使用两次仿射变换的方法，仿射变换是两个平面坐标系之间的转换。When solving the coordinates of the positioning sensor under this label, use the method of two affine transformations, and the affine transformation is the conversion between two plane coordinate systems.

首先是AOB坐标系与图像坐标系的转换，由提取到的标志点的中心点可以确定图像坐标系中AOB三点的坐标值，同时根据各点之间的距离也能确定AOB坐标系中三点的坐标值，将三个坐标带入仿射变换公式中即确定仿射变换矩阵和平移矩阵；The first is the conversion between the AOB coordinate system and the image coordinate system. The coordinate values of the three AOB points in the image coordinate system can be determined from the center point of the extracted marker point, and the three points in the AOB coordinate system can also be determined according to the distance between the points. The coordinate value of the point, bring the three coordinates into the affine transformation formula to determine the affine transformation matrix and translation matrix;

假设求得的仿射矩阵为 $[\begin{matrix} n_{11} & n_{12} \\ n_{21} & n_{22} \end{matrix}],$ 平移矩阵为 $[\begin{matrix} k_{1} \\ k_{2} \end{matrix}],$ 图像中Ｒ点的图像坐标为(u₀,v₀)，则求得Ｒ点在AOB坐标系中的坐标 $[\begin{matrix} x_{r} \\ y_{r} \end{matrix}],$ Suppose the obtained affine matrix is $[\begin{matrix} {no}_{11} & {no}_{12} \\ {no}_{twenty one} & {no}_{twenty two} \end{matrix}],$ The translation matrix is $[\begin{matrix} k_{1} \\ k_{2} \end{matrix}],$ The image coordinates of point R in the image are (u ₀ , v ₀ ), then the coordinates of point R in the AOB coordinate system can be obtained $[\begin{matrix} x_{r} \\ {the y}_{r} \end{matrix}],$

$[\begin{matrix} {x x}_{r r} \\ {y the y}_{r r} \end{matrix}] = = [\begin{matrix} {n no}_{1111} & {n no}_{1212} \\ {n no}_{21 twenty one} & {n no}_{22 twenty two} \end{matrix}] [\begin{matrix} {u u}_{00} \\ {v v}_{00} \end{matrix}] + + [\begin{matrix} {k k}_{11} \\ {k k}_{22} \end{matrix}]$

下面求AOB坐标系与标签坐标系之间的关系，选用标签的三个方向点，由于标签的大小和标志点之间的实际距离已知，所以标签坐标系中各点的坐标可以求得同时三个方向点在AOB坐标系中的坐标可以求得，假设求得仿射矩阵为 $[\begin{matrix} m_{11} & m_{12} \\ m_{21} & m_{22} \end{matrix}],$ 由于图像中O点以及标签坐标下原点的坐标为（0,0）所以平移矩阵为 $[\begin{matrix} 0 \\ 0 \end{matrix}],$ 在图像中可求得R点在图像坐标系AOB中的中的坐标，设为 $[\begin{matrix} x_{r} \\ y_{r} \end{matrix}],$ 变换后的坐标为 $[\begin{matrix} x_{r}^{,} \\ y_{r}^{,} \end{matrix}],$ 则：Next, find the relationship between the AOB coordinate system and the label coordinate system, and select the three direction points of the label. Since the size of the label and the actual distance between the marker points are known, the coordinates of each point in the label coordinate system can be obtained at the same time The coordinates of the three direction points in the AOB coordinate system can be obtained, assuming that the obtained affine matrix is $[\begin{matrix} m_{11} & m_{12} \\ m_{twenty one} & m_{twenty two} \end{matrix}],$ Since the coordinates of point O in the image and the origin of the label coordinates are (0,0), the translation matrix is $[\begin{matrix} 0 \\ 0 \end{matrix}],$ In the image, the coordinates of point R in the image coordinate system AOB can be obtained, set as $[\begin{matrix} x_{r} \\ {the y}_{r} \end{matrix}],$ The transformed coordinates are $[\begin{matrix} x_{r}^{,} \\ {the y}_{r}^{,} \end{matrix}],$ but:

$[\begin{matrix} {x x}_{r r}^{,,} \\ {y the y}_{r r}^{,,} \end{matrix}] = = [\begin{matrix} {m m}_{1111} & {m m}_{1212} \\ {m m}_{21 twenty one} & {m m}_{22 twenty two} \end{matrix}] [\begin{matrix} {x x}_{r r} \\ {y the y}_{r r} \end{matrix}]$

由此确定定位器在该标签下的坐标(x′_r,y′_r)。由坐标(x′_r,y′_r)即可求得在标签坐标系中定位器与X轴和Y轴的夹角，由此即可确定航向角。The coordinates (x' _r , y' _r ) of the locator under the label are thus determined. From the coordinates (x′ _r , y′ _r ), the angle between the locator and the X-axis and Y-axis in the tag coordinate system can be obtained, and the heading angle can be determined accordingly.

所述步骤5）中，确定定位传感器与粘贴标签天花板之间的高度信息的过程为：根据相机的投影原理，L为实际物体的长度，经过投影后，在图像平面上投影后为l，f为焦距，Z为物体与相机的距离，则有：In the step 5), the process of determining the height information between the positioning sensor and the sticker ceiling is as follows: According to the projection principle of the camera, L is the length of the actual object, after projection, it is l, f after projection on the image plane is the focal length, Z is the distance between the object and the camera, then:

$\frac{l l}{L L} = = \frac{f f}{Z Z}$

所以

其中f通过相机标定内参数获得，L和l分别由标签和图像计算得到，由此便可确定定位传感器与粘贴标签天花板之间的高度信息；so

Among them, f is obtained by calibrating the internal parameters of the camera, L and l are obtained by calculating the label and the image respectively, so that the height information between the positioning sensor and the ceiling where the label is pasted can be determined;

对标签的ID号进行识别：在一张标签在图像坐标系下的任意分布，图像坐标系X′O′Y′的坐标原点O′为图像的左上角像素点，X轴水平向右，Y轴垂直向下，以O、A、B、三点构成了标签在图像中的坐标系AOB，对应像素坐标分别为(x_o,y_o)(x_a,y_a)和(x_b,y_b)，其中O为标签坐标系原点，OA指向x轴，OB指向y轴，M点和N点是A点和C点之间的两个等分点，N点用虚圈标出表示在该标签上该点处并没有感光材料，除了方向点之外的其他感光点为标签的编码点，用来确定标签的ID信息，采用下面的步骤来确定标签编码点的位置。Identify the ID number of the label: in any distribution of a label in the image coordinate system, the coordinate origin O' of the image coordinate system X'O'Y' is the pixel point in the upper left corner of the image, the X axis is horizontal to the right, and the Y The axis is vertically downward, and O, A, B, and three points constitute the coordinate system AOB of the label in the image, and the corresponding pixel coordinates are (x _o ,y _o )(x _a ,y _a ) and (x _b ,y _b ), where O is the origin of the label coordinate system, OA points to the x-axis, OB points to the y-axis, M and N points are two equal points between A and C, and N is marked with a dotted circle to indicate that There is no photosensitive material at this point on the label, and other photosensitive points except the direction point are the coding points of the label, which are used to determine the ID information of the label. The following steps are used to determine the location of the label coding point.

（1）确定直线OB上的编码点的位置(1) Determine the position of the coding point on the straight line OB

确定向量

求取除O、A、B三点之外的其余点的与O点构成的向量与

的夹角θ，如另一向量那么

和

的夹角θ为：determine the vector

Find the vector sum of the other points except O, A, and B and the O point

The included angle θ, such as another vector So

and

The included angle θ is:

$θ θ = = arccos arccos ((\frac{(({x x}_{b b} - - {x x}_{o o})) (({x x}_{f f} - - {x x}_{o o})) + + (({y the y}_{b b} - - {y the y}_{o o})) (({y the y}_{f f} - - {y the y}_{o o}))}{\sqrt{{(({x x}_{b b} - - {x x}_{o o}))}^{22} + + {(({y the y}_{b b} - - {y the y}_{o o}))}^{22}} \sqrt{{(({x x}_{f f} - - {x x}_{o o}))}^{22} + + {(({y the y}_{f f} - - {y the y}_{o o}))}^{22}}}))$

选取判定阈值，当夹角θ小于该阈值时认为该点处在OB线上，此时判定M点在OB线上。Select the decision threshold, when the angle θ is smaller than the threshold, it is considered that the point is on the OB line, and at this time it is judged that point M is on the OB line.

利用向量长度之间的关系确定M点的具体位置，设

其长度为：Use the relationship between the vector lengths to determine the specific position of point M, set

Its length is:

$| | \overset{&RightArrow; &Right Arrow;}{OM om} | | = = \sqrt{{(({x x}_{m m} - - {x x}_{o o}))}^{22} + + {(({y the y}_{m m} - - {y the y}_{o o}))}^{22}}$

此时求取向量

与向量

的长度比值确定点M的具体位置。Find the vector at this time

with vector

The length ratio of determines the specific position of point M.

对于不存在感光材料的N点，由于判别其他感光点时会用到，所以要求出其坐标，设定 $\overset{&RightArrow;}{ON} = (x_{n} - x_{o}, y_{n} - y_{o}),$ $\overset{&RightArrow;}{ON} = 2 / 3 \overset{&RightArrow;}{OB},$ 则有：For point N where there is no photosensitive material, since it will be used to distinguish other photosensitive points, its coordinates are required to be set. $\overset{&Right Arrow;}{ON} = (x_{no} - x_{o}, {the y}_{no} - {the y}_{o}),$ $\overset{&Right Arrow;}{ON} = 2 / 3 \overset{&Right Arrow;}{OB},$ Then there are:

$\{\begin{matrix} {x x}_{n no} - - {x x}_{o o} = = \frac{22}{33} (({x x}_{b b} - - {x x}_{o o})) \\ {y the y}_{n no} - - {y the y}_{o o} = = \frac{22}{33} (({y the y}_{b b} - - {y the y}_{o o})) \end{matrix}$

求出N点坐标(x_n,y_n)，若是M点也没有感光材料，那么M点坐标也以此方法确定。Find the coordinates (x _n , y _n ) of point N. If there is no photosensitive material at point M, then the coordinates of point M are also determined in this way.

（2）确定直线OA上的编码点的位置(2) Determine the position of the coding point on the straight line OA

求得

依据（1）中所述方法求取其余向量与

的夹角，可判断D点为OA线上的点，利用向量长度关系可以确定D点的位置。obtain

Calculate the remaining vectors and

It can be judged that the point D is a point on the OA line, and the position of the point D can be determined by using the vector length relationship.

（3）确定其余编码点的位置(3) Determine the position of the remaining code points

其余感光点的位置确定方法和上述类似，只不过向量选取有所不同，以确定直线L2上感光点位置为例，对于图13所示标签，取M点为向量起点，其余未确定位置的点为向量终点确定四个向量，如

将得到的向量均计算与

的夹角，通过阈值判断E点为l₂上的点，通过向量

与向量

的长度关系可以得到E的位置。同理可得知处在直线L3和L4上的点及其位置。The methods for determining the positions of other photosensitive points are similar to the above, except that the selection of vectors is different. Take determining the position of photosensitive points on the straight line L2 as an example. For the label shown in Figure 13, point M is taken as the starting point of the vector, and the rest of the undetermined points Determine the four vectors for the vector endpoints, such as

Compute the resulting vectors with

The included angle, through the threshold to judge point E is a point on l ₂ , through the vector

with vector

The length relationship can get the position of E. Similarly, the points and their positions on the straight lines L3 and L4 can be known.

（4）确定标签ID(4) Determine the tag ID

标签上编码点的不同位置对应着二进制数的不同位，沿标签构成的坐标系x轴方向遍历十二个点，依次对应的是二进制数的第一位，第二位…第十二位；利用得到的感光点位置唯一的确定一个二进制数，其就为该标签的ID。The different positions of the coding points on the label correspond to different bits of the binary number. Twelve points are traversed along the x-axis direction of the coordinate system formed by the label, which in turn corresponds to the first bit, the second bit...the twelfth bit of the binary number; Use the obtained photosensitive point position to uniquely determine a binary number, which is the ID of the tag.

本实用新型涉及的是一种可快速准确定位室内移动机器人的精密传感器及其工作方法，移动机器人携带该传感器可以实现精确室内定位，从而为机器人的正确导航提供基础条件，具有很高的应用价值和经济效益。The utility model relates to a precision sensor capable of quickly and accurately positioning an indoor mobile robot and its working method. The sensor carried by the mobile robot can realize precise indoor positioning, thereby providing basic conditions for the correct navigation of the robot, and has high application value and economic benefits.

附图说明 Description of drawings

图1为定位传感器系统整体框图；Figure 1 is the overall block diagram of the positioning sensor system;

图2为红外发射管布设示意图；Figure 2 is a schematic diagram of the layout of infrared emission tubes;

图3为标签示意图；Figure 3 is a schematic diagram of the label;

图4为图像坐标系；Fig. 4 is image coordinate system;

图5为摄像机坐标系与世界坐标系；Figure 5 shows the camera coordinate system and the world coordinate system;

图6为标志点图像及灰度值分布；Fig. 6 is a marker point image and gray value distribution;

图7为中心点搜索图；Figure 7 is a center point search diagram;

图8为边界搜索图；Figure 8 is a boundary search diagram;

图9为方向点识别示意图；Fig. 9 is a schematic diagram of direction point recognition;

图10为标签坐标轴确定示意图；Fig. 10 is a schematic diagram of label coordinate axis determination;

图11为位置信息确定示意图；Fig. 11 is a schematic diagram of location information determination;

图12为投影原理图；Figure 12 is a schematic diagram of projection;

图13为标签在图像坐标系下的分布图；Figure 13 is a distribution diagram of labels in the image coordinate system;

图14a-d为标签红外图像处理图；Figure 14a-d is a diagram of label infrared image processing;

图15为传感器工作流程图。Figure 15 is a flow chart of the sensor work.

其中，1.图像处理芯片，2.COMS摄像头，3.UART接口，4.SDRAM，5.FLASH，6.EEPROM，7.红外管，8.电源。Among them, 1. Image processing chip, 2. COMS camera, 3. UART interface, 4. SDRAM, 5. FLASH, 6. EEPROM, 7. Infrared tube, 8. Power supply.

具体实施方式 Detailed ways

下面结合附图与实施例对本实用新型做进一步说明。Below in conjunction with accompanying drawing and embodiment the utility model is described further.

图1中，它包括图像处理芯片1，图像处理芯片1分别与存储模块、COMS摄像头3、数据接口、电源8以及红外发射模块连接，红外发射模块包括多个红外管7，它们围绕在COMS摄像头2周围并分为几组。所述数据接口为UART接口3，所述存储模块为SDRAM4、FLASH5、EEPROM6。所述红外管有12个，每4个分为一组，共3组，开始进行测量时3组全部打开，测得结果后随即关闭一组，再进行测量，如果测量精度没有影响，则再关闭一组，目的是保证精度不受影响且使用的红外管7数量最少。In Fig. 1, it comprises image processing chip 1, and image processing chip 1 is connected with storage module, COMS camera 3, data interface, power supply 8 and infrared emission module respectively, and infrared emission module comprises a plurality of infrared tubes 7, and they surround COMS camera 2 around and divided into groups. The data interface is UART interface 3, and the storage modules are SDRAM4, FLASH5, EEPROM6. There are 12 infrared tubes, each of which is divided into 4 groups, a total of 3 groups, all 3 groups are opened when the measurement is started, and one group is closed immediately after the measurement results are obtained, and then the measurement is performed. If the measurement accuracy is not affected, then One group is closed to ensure that the accuracy is not affected and the number of infrared tubes 7 used is the minimum.

1 标签光斑图像处理1 Label spot image processing

图像处理芯片采用TMS320DM642 DSP处理器，其主频范围为480MHz~720MHz，在600MHz的主频下，处理能力可达4800MIPS。DM642带有3个专用的可配置视频接口VideoPort(VP)，为视频数据的采集和处理提供了极大的便利。图1所示为定位传感器整体框图，TMS320DM642通过EMIF接口外扩SDRAM、FLASH、EEPROM，其中EEPROM用于传感器信息配置的存储和读取；通过Video Port读取图像传感器经AD转换过来的数据；图像数据经TMS320DM642进行算法处理后，得出定位信息（空间坐标值和航向角），通过UART串行数据口发送至机器人控制器。The image processing chip adopts TMS320DM642 DSP processor, and its main frequency range is 480MHz~720MHz. Under the main frequency of 600MHz, the processing capacity can reach 4800MIPS. DM642 has 3 dedicated configurable video interfaces VideoPort (VP), which provides great convenience for the collection and processing of video data. Figure 1 shows the overall block diagram of the positioning sensor. TMS320DM642 expands SDRAM, FLASH, and EEPROM through the EMIF interface, and the EEPROM is used for storing and reading sensor information configuration; read the data converted by the image sensor through AD through the Video Port; After the data is processed by the algorithm of TMS320DM642, the positioning information (space coordinate value and heading angle) is obtained, which is sent to the robot controller through the UART serial data port.

2 红外发射模块2 Infrared transmitter module

红外发射模块由12个红外管及控制电路组成，为了获得最佳的图像质量和定位精度，红外管最佳布置如图2所示，图中红色部分为红外发射管，黑白色部分为COMS摄像头。红外管布设在COMS摄像头周围，其个数虽不影响定位精度，但个数少了会缩小定位传感器的采集范围，个数多了又会增加功耗，导致传感器发热。为解决上述矛盾，实际应用中，采用了红外管分组控制的方法，每4个红外管为一组，共分为3组，开始进行测量时3组全部打开，测得结果后随即关闭一组，再进行测量，如果测量精度没有影响，则再关闭一组，目的是保证精度不受影响且使用的红外管数量最少。The infrared emitting module is composed of 12 infrared tubes and control circuits. In order to obtain the best image quality and positioning accuracy, the best layout of the infrared tubes is shown in Figure 2. The red part in the figure is the infrared emitting tube, and the black and white part is the COMS camera. . The infrared tubes are arranged around the COMS camera. Although the number does not affect the positioning accuracy, a small number will reduce the acquisition range of the positioning sensor, and a large number will increase power consumption and cause the sensor to heat up. In order to solve the above contradictions, in practical application, the infrared tube group control method is adopted. Every 4 infrared tubes are divided into 3 groups. When the measurement is started, all 3 groups are turned on, and one group is closed immediately after the measurement result is obtained. , and then measure, if the measurement accuracy is not affected, then turn off another group, the purpose is to ensure that the accuracy is not affected and the number of infrared tubes used is the least.

3 图像传感器3 image sensor

图像传感器采用COMS感光阵列，由于传感器体积的限制，本设计将图像传感器集成在主板上。The image sensor adopts CMOS photosensitive array. Due to the limitation of sensor volume, this design integrates the image sensor on the main board.

4 电源4 power supply

传感器电源输入为DC8.4V～12V，系统需要的1.4V、3.3V和5V三个不同电压等级的电源由DCDC稳压芯片产生。由于红外发射模块功耗最大，因此采用了大电流开关型稳压芯片LM2576。The sensor power input is DC8.4V～12V, and the power supplies of three different voltage levels of 1.4V, 3.3V and 5V required by the system are generated by the DCDC voltage regulator chip. Since the infrared transmitter module consumes the most power, a high-current switching voltage regulator chip LM2576 is used.

5 通信接口5 communication interface

通信接口作为传感器的数据输出接口，必须满足一般机器人控制器计算机的接口需要。本实用新型采用UART接口，接线端子选用9针D型串口头和3针2mm间距接线端子，同时输出232电平和TTL电平，以满足不用类型上位计算机的需要。此外，UART接口还可连接显示设备，进行数据的本地显示。As the data output interface of the sensor, the communication interface must meet the interface requirements of the general robot controller computer. The utility model adopts the UART interface, and the connection terminals are 9-pin D-type serial ports and 3-pin 2mm spacing terminals, and output 232 level and TTL level at the same time to meet the needs of different types of upper computers. In addition, the UART interface can also be connected to a display device for local display of data.

图3为本实用新型需要的反射红外线标签示意图，其大小为15cm×15cm，上面15个小圆点的直径均为15mm。图中三个白色圆点粘贴有反射红外线材料，用于传感器定位，所有标签都要有这样三个圆点，其余十二个圆点，通过在不同位置粘贴不同数量的反射红外线材料构造标签属性的ID号，ID号由图3中所示的值相加计算而得。属性信息可以根据需要自己设定。Fig. 3 is a schematic diagram of a reflective infrared label required by the present invention, its size is 15cm * 15cm, and the diameters of the 15 small dots on it are all 15mm. The three white dots in the picture are pasted with reflective infrared material for sensor positioning. All labels must have these three dots, and the remaining twelve dots are constructed by pasting different amounts of reflective infrared material at different positions. ID number, which is calculated by adding the values shown in Figure 3. Attribute information can be set as required.

标签识别原理Principle of label recognition

1 相机标定1 Camera Calibration

理想的摄像机模型是针孔摄像机模型，但真实的针孔不能为快速曝光提供足够的光线，所以眼睛和摄像机都要使用透镜而不是单单一个点来收集更多光线。但利用透镜模型就背离了简单的针孔几何模型，并且引入了透镜的畸变等影响。所以为了得到理想的图像，必须对图像进行矫正，需要知道相机的畸变系数，同时为了计算相机和标签之间的距离，还需要知道内参数焦距f_o所以在传感器投入使用之前必须对相机进行标定，来获得内参数和畸变参数。The ideal camera model is a pinhole camera model, but a real pinhole doesn't provide enough light for a quick exposure, so both the eye and the camera use a lens rather than a single point to gather more light. However, using the lens model deviates from the simple pinhole geometric model, and introduces effects such as lens distortion. Therefore, in order to obtain an ideal image, the image must be corrected, and the distortion coefficient of the camera needs to be known. At the same time, in order to calculate the distance between the camera and the label, the internal parameter focal length f _o must be known, so the camera must be calibrated before the sensor is put into use. , to obtain internal parameters and distortion parameters.

首先要建立三个坐标系：图像坐标系、摄像机坐标系和世界坐标系。数字图像在计算机内以M×N的二维数组的形式存储，M行N列的图像中的每个元素称为像素。在图像上定义直角坐标系UOV，每个像素的坐标(u,v)表示该像素在图像中的行数和列数，该坐标系称为图像坐标系，如图4所示。同时还需建立一个以物理单位表示的图像坐标系X₁O₁Y₁，该坐标系以摄像机光轴与图像平面的交点O₁为坐标原点，X₁轴Y₁轴分别与U、V轴平行。First of all, three coordinate systems should be established: image coordinate system, camera coordinate system and world coordinate system. Digital images are stored in the form of M×N two-dimensional arrays in the computer, and each element in the image of M rows and N columns is called a pixel. The Cartesian coordinate system UOV is defined on the image. The coordinates (u, v) of each pixel represent the number of rows and columns of the pixel in the image. This coordinate system is called the image coordinate system, as shown in Figure 4. At the same time, it is necessary to establish an image coordinate system X ₁ O ₁ Y ₁ expressed in physical units. The coordinate system takes the intersection point O ₁ of the camera optical axis and the image plane as the coordinate origin, and the X ₁ axis and the Y ₁ axis are respectively connected to the U and V axes. parallel.

原点O₁在U、V坐标系中的坐标为(u₀,v₀)，每个像素在X轴与Y轴方向上的物理尺寸为dx，dy，则图像中任意一个像素在两个坐标系中的关系为：The coordinates of the origin O ₁ in the U and V coordinate system are (u ₀ , v ₀ ), and the physical size of each pixel in the direction of the X-axis and Y-axis is dx, dy, then any pixel in the image is in the two coordinates The relationship in the department is:

$\{\begin{matrix} u u = = \frac{x x}{{d d}_{x x}} + + {u u}_{00} \\ v v = = \frac{y the y}{{d d}_{y the y}} + + {v v}_{00} \end{matrix}$

用齐次坐标和矩阵形式表示为：Expressed in homogeneous coordinates and matrix form as:

$[\begin{matrix} u u \\ v v \\ 11 \end{matrix}] = = [\begin{matrix} \frac{11}{{d d}_{x x}} & 00 & {u u}_{00} \\ 00 & \frac{11}{{d d}_{y the y}} & {v v}_{00} \\ 00 & 00 & 11 \end{matrix}] [\begin{matrix} x x \\ y the y \\ 11 \end{matrix}]$

摄像机成像的几何关系如图5所示，其中，O_cX_cY_cZ_c为摄像机坐标系，O_cO₁为摄像机的焦距，O_c为摄像机光心，X_c轴和Y_c轴分别与图像X₁和Y₁轴平行，Z_c轴为摄像机的光轴，与图像平面垂直。The geometric relationship of camera imaging is shown in Figure 5, where O _c X _c Y _c Z _c is the camera coordinate system, O _c O ₁ is the focal length of the camera, O _c is the optical center of the camera, and the X _c axis and the Y _c axis are respectively Parallel to the image _X1 and _Y1 axes, the _Zc axis is the optical axis of the camera and is perpendicular to the image plane.

世界坐标系是摄像机安装环境中描述位置的一个基准，摄像机坐标系和世界坐标系的关系称为外参数，因为只是在标定过程中会用到此关系，系统实际使用时不会涉及外参数。若空间点p在世界坐标系和摄像机坐标系下的齐次坐标分别为(x_w,y_w,z_w,1)和(x_c,y_c,z_c,1),则存在如下关系:The world coordinate system is a reference for describing the position in the camera installation environment. The relationship between the camera coordinate system and the world coordinate system is called an external parameter, because this relationship is only used in the calibration process, and the external parameters will not be involved in the actual use of the system. If the homogeneous coordinates of the space point p in the world coordinate system and the camera coordinate system are (x _w , y _w , z _w , 1) and (x _c , y _c , z _c , 1), then the following relationship exists:

$[\begin{matrix} {x x}_{c c} \\ {y the y}_{c c} \\ {z z}_{} \\ c c \\ 11 \end{matrix}] = = [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}] [\begin{matrix} {x x}_{w w} \\ {y the y}_{w w} \\ {z z}_{} \\ w w \\ 11 \end{matrix}] = = {M m}_{22} [\begin{matrix} {x x}_{w w} \\ {y the y}_{w w} \\ {z z}_{} \\ w w \\ 11 \end{matrix}]$

根据理想的针孔成像模型，空间任意一点P与其在图像上的投影位置p之间的关系为：According to the ideal pinhole imaging model, the relationship between any point P in space and its projected position p on the image is:

$\{\begin{matrix} x x = = \frac{f f {x x}_{c c}}{{z z}_{c c}} \\ y the y = = \frac{f f {y the y}_{c c}}{{z z}_{c c}} \end{matrix}$

（x，y）为p点物理图像坐标，(x_c,y_c,z_c)为摄像机坐标系下坐标。对以上各式化简得：(x, y) are the physical image coordinates of point p, and (x _c , y _c , z _c ) are the coordinates in the camera coordinate system. Simplify the above formulas to get:

${z z}_{c c} [\begin{matrix} u u \end{matrix}] [\begin{matrix} v v \\ 11 \end{matrix}] = = [\begin{matrix} \frac{11}{{d d}_{x x}} & 00 & {u u}_{00} \\ 00 & \frac{11}{{d d}_{y the y}} & {v v}_{00} \\ 00 & 00 & 11 \end{matrix}] [\begin{matrix} f f & 00 & 00 & 00 \\ 00 & f f & 00 & 00 \\ 00 & 00 & 11 & 00 \end{matrix}] [\begin{matrix} R R & T T \\ 00 & 11 \end{matrix}] [\begin{matrix} {x x}_{w w} \\ {y the y}_{w w} \\ {z z}_{} \\ w w \\ 11 \end{matrix}]$

$= = [\begin{matrix} {a a}_{x x} & x x & {u u}_{00} \\ 00 & {a a}_{y the y} & {v v}_{00} \\ 00 & 00 & 11 \end{matrix}] [\begin{matrix} R R & T T \end{matrix}] [\begin{matrix} {x x}_{w w} \\ {y the y}_{w w} \\ {z z}_{} \\ w w \\ 11 \end{matrix}]$

其中a_x、a_y、u₀、v₀只与摄像机内部结构有关，称为内部参数。Among them, a _x , a _y , u ₀ , and v ₀ are only related to the internal structure of the camera, and are called internal parameters.

实际上，实际的成像并不是理想的成像，会带有不同程度畸变，理想的成像点由于畸变的存在会发生偏移，假设理想成像点（x_u，y_u），畸变成像点（x_d，y_d），可用如下公式描述畸变模型：In fact, the actual imaging is not the ideal imaging, it will have different degrees of distortion, the ideal imaging point will shift due to the existence of distortion, assuming the ideal imaging point (x _u , y _u ), the distorted image point (x _d , y _d ), the distortion model can be described by the following formula:

$\{\begin{matrix} {x x}_{u u} = = {x x}_{d d} + + {δ δ}_{x x} (({x x}_{d d},, {y the y}_{d d})) \\ {y the y}_{u u} = = {y the y}_{d d} + + {δ δ}_{y the y} (({x x}_{d d},, {y the y}_{d d})) \end{matrix}$

其中，δ_x、δ_y是非线性畸变值，它与图像点在图像中的位置有关。首先是径向畸变，其数学模型如下：Among them, δ _x and δ _y are nonlinear distortion values, which are related to the position of the image point in the image. The first is radial distortion, and its mathematical model is as follows:

$\{\begin{matrix} {δ δ}_{x x} (({x x}_{d d},, {y the y}_{d d})) = = {x x}_{d d} (({k k}_{11} {r r}_{}^{d d} + + {k k}_{22} {r r}_{}^{d d} + + {k k}_{33} {r r}_{}^{d d} + + \cdot \cdot \cdot &Center Dot; \cdot \cdot)) \\ {δ δ}_{y the y} (({x x}_{d d},, {y the y}_{d d})) = = {y the y}_{d d} (({k k}_{11} {r r}_{}^{d d} + + {k k}_{22} {r r}_{}^{d d} + + {k k}_{33} {r r}_{}^{d d} + + \cdot \cdot \cdot &Center Dot; \cdot \cdot)) \end{matrix}$

其中，

k₁、k₂、k₃为径向畸变系数，一般取径向的前两阶就可满足要求。in,

k ₁ , k ₂ , and k ₃ are radial distortion coefficients, and generally the first two orders in the radial direction can meet the requirements.

离心畸变是由于摄像机光学系统中各镜头的光轴中心并不严格共面，离心畸变包括径向和切向畸变，可用如下模型表示：Centrifugal distortion is due to the fact that the optical axis centers of the lenses in the camera optical system are not strictly coplanar. Centrifugal distortion includes radial and tangential distortion, which can be expressed by the following model:

$\{\begin{matrix} {δ δ}_{x x} (({x x}_{d d},, {y the y}_{d d})) = = {p p}_{11} ((33 {x x}_{}^{d d} + + {y the y}_{d d}^{22})) + + 22 {p p}_{22} {x x}_{d d} {y the y}_{d d} \\ {δ δ}_{y the y} (({x x}_{d d},, {y the y}_{d d})) = = {p p}_{22} ((33 {y the y}_{}^{d d} + + {x x}_{d d}^{22})) + + 22 {p p}_{11} {x x}_{d d} {y the y}_{d d} \end{matrix}$

其中，p₁、p₂为离心畸变系数。Among them, p ₁ and p ₂ are centrifugal distortion coefficients.

还有一种畸变是由于透镜设计、生产不完善或装备不完善引起的，可用如下模型表示：Another kind of distortion is caused by lens design, imperfect production or imperfect equipment, which can be represented by the following model:

$\{\begin{matrix} {δ δ}_{x x} (({x x}_{d d},, {y the y}_{d d})) = = {s the s}_{11} (({x x}_{}^{d d} + + {y the y}_{d d}^{22})) \\ {δ δ}_{y the y} (({x x}_{d d},, {y the y}_{d d})) = = {s the s}_{22} (({x x}_{}^{d d} + + {y the y}_{d d}^{22})) \end{matrix}$

其中，s₁、s₂为畸变系数。Among them, s ₁ and s ₂ are distortion coefficients.

通过以上总结，完整的畸变模型为：Through the above summary, the complete distortion model is:

$\{\begin{matrix} {δ δ}_{x x} (({x x}_{d d},, {y the y}_{d d})) = = {x x}_{d d} (({k k}_{11} {r r}_{}^{d d} + + {k k}_{22} {r r}_{d d}^{44})) + + {p p}_{11} ((33 {x x}_{}^{d d} + + {y the y}_{d d}^{22})) + + 22 {p p}_{22} {x x}_{d d} {y the y}_{d d} + + {s the s}_{11} (({x x}_{}^{d d} + + {y the y}_{d d}^{22})) \\ {δ δ}_{y the y} (({x x}_{d d},, {y the y}_{d d})) = = {y the y}_{d d} (({k k}_{11} {r r}_{}^{d d} + + {k k}_{22} {r r}_{d d}^{44})) + + {p p}_{22} ((33 {y the y}_{}^{d d} + + {x x}_{d d}^{22})) + + 22 {p p}_{11} {x x}_{d d} {y the y}_{d d} + + {s the s}_{22} (({x x}_{}^{d d} + + {y the y}_{d d}^{22})) \end{matrix}$

本系统中摄像机标定时采用了张正友提出的基于2D平面标定板的标定算法，在标定过程中假设摄像机内部参数始终不变，即无论摄像机从任何角度拍摄平面模板，摄像机内部参数都是常数，只有外部参数发生变化。基本步骤为：In this system, the calibration algorithm based on the 2D plane calibration board proposed by Zhang Zhengyou is used in the camera calibration. In the calibration process, it is assumed that the internal parameters of the camera are always constant, that is, no matter the camera shoots the plane template from any angle, the internal parameters of the camera are constant. Only External parameters changed. The basic steps are:

（1）打印一张标准的国际象棋盘作为标定板，并将其贴在一个刚性的平面上。(1) Print a standard chess board as a calibration board and stick it on a rigid plane.

（2）从不同角度拍摄多张标定板图像，数量多可使标定更准确。(2) Take multiple images of the calibration plate from different angles, and a large number can make the calibration more accurate.

（3）检测标定板上的特征点，并确定图像坐标和实际坐标。(3) Detect the feature points on the calibration board, and determine the image coordinates and actual coordinates.

（4）利用线性模型计算出摄像机的内部参数。(4) Use the linear model to calculate the internal parameters of the camera.

（5）利用畸变模型，对摄像机内参数进行优化，求出所有参数。(5) Use the distortion model to optimize the internal parameters of the camera and find out all the parameters.

确定内参数后就可以对图像进行矫正，并且为后续计算做好准备。After the internal parameters are determined, the image can be corrected and prepared for subsequent calculations.

2 有效标签和最优标签确认2 Valid label and optimal label confirmation

3 标签反光点识别3 Label reflective point recognition

反光点使用反光材料加工而成，能将入射光线反射回光源处，在近轴光源照射下能在像片上形成灰度反差明显的“准二值”图像，特别适合用作摄影测量的高精度特征点。The reflective point is made of reflective material, which can reflect the incident light back to the light source. Under the illumination of the paraxial light source, it can form a "quasi-binary" image with obvious grayscale contrast on the photo, which is especially suitable for high-precision photogrammetry. Feature points.

由于相机成像等各种原因的影响，实际检测到的标志点并不是完整的圆形，所以本系统中采用了基于像素搜索的方法来识别标志区域。以图6为例，识别过程如下：Due to the influence of various reasons such as camera imaging, the actual detected mark points are not a complete circle, so the system uses a method based on pixel search to identify the mark area. Taking Figure 6 as an example, the identification process is as follows:

（1）为方便计算，首先我们定义一个梯度。规定一个像素沿某方向的梯度为该像素与该方向下一个像素的灰度值之差。自上而下自左向右搜索，若在某一像素点，右梯度大于设定阈值ε₁，则认为该点是标志区域内一点。(1) For the convenience of calculation, first we define a gradient. It is stipulated that the gradient of a pixel along a certain direction is the difference between the gray value of this pixel and the next pixel in this direction. Search from top to bottom and from left to right. If at a certain pixel point, the right gradient is greater than the set threshold ε ₁ , then the point is considered to be a point in the marker area.

（2）以该点为起始点，搜索其八邻域内最大灰度值，最终找到标志区域内最大灰度值点。若图中ε₁设为10，则起始点为（5,8），即灰度值为7的点。(2) With this point as the starting point, search for the maximum gray value in its eight neighborhoods, and finally find the point with the maximum gray value in the marked area. If ε ₁ in the figure is set to 10, then the starting point is (5,8), that is, the point with a gray value of 7.

（3）以中心点为起始点，向上、下、左、右四个方向搜索，当某点像素灰度值小于设定值并且梯度小于设定阈值时，则认为该点为标志区域的边界点。(3) With the center point as the starting point, search in four directions: up, down, left, and right. When the pixel gray value of a point is less than the set value and the gradient is less than the set threshold, the point is considered to be the boundary of the marked area point.

（4）分别以中心对角线相连的点为起始点开始水平和垂直搜索，直到找到标志区域边界点。其他搜索以此类推，如图7、图8所示。(4) Start the horizontal and vertical search from the points connected by the diagonal lines of the center respectively until the boundary point of the marked area is found. Other searches can be deduced by analogy, as shown in Figure 7 and Figure 8.

（5）检测出的区域并不一定都是标志区域，还需要去掉干扰区域，首先计算标志区域内所有像素的平均值，平均值过低则排除，然后计算标志区域的尺寸和边界长度，不符合阈值的排除，因为标志区域为圆形，虽成像可能有所变形，但不会有太大影响，所以若标志区域的x和y方向的尺度比例相差过大则应排除。(5) The detected areas are not necessarily marked areas, and the interference area needs to be removed. First, calculate the average value of all pixels in the marked area. If the average value is too low, exclude it. Then calculate the size and boundary length of the marked area. Exclusions that meet the threshold, because the marked area is circular, although the imaging may be deformed, but it will not have much impact, so if the scale ratio of the marked area in the x and y directions is too large, it should be excluded.

经过以上步骤基本可以确定视野范围内所有标志点，但考虑到成像时背景区域并不是理想的黑色，所以其灰度值并不为零，为了使计算标志中心更准确，需要排除背景的干扰。本系统中采用建立灰度直方图的方法，选取标志区域和背景区域灰度的峰值间的谷底做为阈值，然后用图像各点灰度值减去该阈值，获得一幅新的图像。After the above steps, all marker points within the field of view can be basically determined. However, considering that the background area is not ideally black during imaging, its gray value is not zero. In order to make the calculation of the center of the marker more accurate, it is necessary to eliminate the interference of the background. In this system, the method of establishing a gray histogram is adopted, and the valley between the peaks of the gray value of the sign area and the background area is selected as the threshold value, and then the threshold value is subtracted from the gray value of each point in the image to obtain a new image.

确定中心点时，选用灰度加权质心法，所谓灰度加权质心是指，以灰度值为权重，计算图像内所有标志点象素坐标的平均值，公式如下：When determining the center point, the gray-scale weighted centroid method is used. The so-called gray-scale weighted centroid means that the average value of the pixel coordinates of all marker points in the image is calculated with the gray value as the weight. The formula is as follows:

公式中x₀和y₀为计算所得中心点的像素坐标，(i,j)表示图像中的某个像素点，i和j分别表示像素点的x轴和y轴的坐标值，w_i,j表示像素点(i,j)处的像素灰度值。In the formula, x ₀ and y ₀ are the pixel coordinates of the calculated center point, (i, j) represents a certain pixel point in the image, i and j represent the coordinate values of the x-axis and y-axis of the pixel point respectively, w _{i, j} represents the pixel gray value at the pixel point (i, j).

4确定三个方向点及坐标系4 Determine the three direction points and coordinate system

在一张标签中，标志点被分为两类，第一类为方向点，即来确定标签坐标轴的方向，为了唯一确定方向，标签四个角中只能有三个角有标志点。第二类为编码点，用来确定该标签的编号。In a label, the marker points are divided into two categories. The first category is the direction point, that is, to determine the direction of the label coordinate axis. In order to uniquely determine the direction, only three corners of the four corners of the label can have marker points. The second category is the code point, which is used to determine the number of the label.

如图9所示，在上一节中已经找到了各个标志点的中心点，下一步要区分方向点和编码点，首先计算标签中任意两两标志点之间的长度，选取其中最长者，并且提取最长连线所对应的两个标志点，将这两个标志点分别标记为A和B，计算A和B连线的中点，然后分别计算标签中除A和B点外其他所有点到该中点的距离，选取其中最长的，并提取该最长距离所对应的标志点，将其标记为O，则A、O、B三点为三个方向点。As shown in Figure 9, the center point of each mark point has been found in the previous section. The next step is to distinguish the direction point and the code point. First, calculate the length between any two mark points in the label, and select the longest one. , and extract the two marker points corresponding to the longest connection, label these two marker points as A and B respectively, calculate the midpoint of the connection between A and B, and then calculate the other points in the label except A and B respectively Select the longest distance from all points to the midpoint, and extract the mark point corresponding to the longest distance, and mark it as O, then A, O, and B are three direction points.

其中α为旋转角度。where α is the rotation angle.

判断δ夹角：Judgment δ included angle:

5 位置信息的确定5 Determination of location information

位置信息的确定过程为：在定位传感器获取的图像中，R点为定位传感器在图像中的投影位置，该位置是图像中心。在图像中按照确定标签坐标系的规则建立AOB坐标系，其中OA为X轴方向，OB为Y轴方向。The determination process of the position information is: in the image acquired by the positioning sensor, point R is the projection position of the positioning sensor in the image, and this position is the center of the image. In the image, establish the AOB coordinate system according to the rules for determining the label coordinate system, where OA is the X-axis direction, and OB is the Y-axis direction.

求解定位器在该标签下的坐标时，使用了两次仿射变换的方法，仿射变换是两个平面坐标系之间的转换。仿射变换的基本原理是：假设P₁、P₂、P₃是平面内不共线的任意三个点，P'₁、P’₂、P'₃也是平面内不共线的三个点，那存在也只存在一个仿射变换T，使T(P_i)＝P’_i（i=1,2,3）。可用如下公式表示：When solving the coordinates of the locator under this label, two methods of affine transformation are used. Affine transformation is the conversion between two plane coordinate systems. The basic principle of affine transformation is: suppose P ₁ , P ₂ , P ₃ are any three points that are not collinear in the plane, and P' ₁ , P' ₂ , P' ₃ are also three points that are not collinear in the plane , then there is only one affine transformation T, so that T(P _i )=P' _i (i=1,2,3). It can be expressed by the following formula:

$[\begin{matrix} {x x}^{,,} \\ {y the y}^{,,} \end{matrix}] = = [\begin{matrix} {a a}_{1111} & {a a}_{1212} \\ {a a}_{21 twenty one} & {a a}_{22 twenty two} \end{matrix}] [\begin{matrix} x x \\ y the y \end{matrix}] + + [\begin{matrix} {b b}_{11} \\ {b b}_{22} \end{matrix}]$

其中， $[\begin{matrix} x \\ y \end{matrix}],$ $[\begin{matrix} x^{,} \\ y^{,} \end{matrix}]$ 分别为变换前后的坐标。 $[\begin{matrix} a_{11} & a_{12} \\ a_{21} & a_{22} \end{matrix}]$ 为仿射矩阵，产生旋转等变换。 $[\begin{matrix} b_{1} \\ b_{2} \end{matrix}]$ 为平移矩阵。由仿射矩阵和平移矩阵可唯一确定一个仿射变换。in, $[\begin{matrix} x \\ the y \end{matrix}],$ $[\begin{matrix} x^{,} \\ {the y}^{,} \end{matrix}]$ are the coordinates before and after the transformation, respectively. $[\begin{matrix} a_{11} & a_{12} \\ a_{twenty one} & a_{twenty two} \end{matrix}]$ It is an affine matrix, which produces transformations such as rotation. $[\begin{matrix} b_{1} \\ b_{2} \end{matrix}]$ is the translation matrix. An affine transformation can be uniquely determined by an affine matrix and a translation matrix.

首先是AOB坐标系与图像坐标系的转换，可以确定图像坐标系中A、O、B三点的坐标值，同时也可以确定AOB坐标系中三点的坐标值，将三个坐标带入放射变换公式中即可确定放射变换矩阵和平移矩阵。The first is the conversion between the AOB coordinate system and the image coordinate system, which can determine the coordinate values of A, O, and B in the image coordinate system, and at the same time determine the coordinate values of the three points in the AOB coordinate system, and bring the three coordinates into the radiation The radiation transformation matrix and translation matrix can be determined in the transformation formula.

假设求得的放射矩阵为 $[\begin{matrix} n_{11} & n_{12} \\ n_{21} & n_{22} \end{matrix}],$ 平移矩阵为 $[\begin{matrix} k_{1} \\ k_{2} \end{matrix}],$ 可知图像中Ｒ点的图像坐标为(u₀,v₀)，则可求得Ｒ点在AOB坐标系中的坐标 $[\begin{matrix} x_{r} \\ y_{r} \end{matrix}],$ Assume that the radiation matrix obtained is $[\begin{matrix} {no}_{11} & {no}_{12} \\ {no}_{twenty one} & {no}_{twenty two} \end{matrix}],$ The translation matrix is $[\begin{matrix} k_{1} \\ k_{2} \end{matrix}],$ It can be seen that the image coordinates of point R in the image are (u ₀ , v ₀ ), then the coordinates of point R in the AOB coordinate system can be obtained $[\begin{matrix} x_{r} \\ {the y}_{r} \end{matrix}],$

下面求AOB坐标系与标签坐标系之间的关系，标签坐标系和AOB坐标系已知，假设求得仿射矩阵为 $[\begin{matrix} m_{11} & m_{12} \\ m_{21} & m_{22} \end{matrix}],$ 由于图像中O点以及标签坐标系下原点的坐标为（0,0）所以平移矩阵为 $[\begin{matrix} 0 \\ 0 \end{matrix}],$ 在图像中可求得R点在AOB坐标系中的坐标，设为 $[\begin{matrix} x_{r} \\ y_{r} \end{matrix}],$ 变换后的坐标为 $[\begin{matrix} x_{r}^{,} \\ y_{r}^{,} \end{matrix}],$ 则：The following is the relationship between the AOB coordinate system and the label coordinate system. The label coordinate system and the AOB coordinate system are known. Assume that the affine matrix obtained is $[\begin{matrix} m_{11} & m_{12} \\ m_{twenty one} & m_{twenty two} \end{matrix}],$ Since the coordinates of point O in the image and the origin in the label coordinate system are (0,0), the translation matrix is $[\begin{matrix} 0 \\ 0 \end{matrix}],$ In the image, the coordinates of point R in the AOB coordinate system can be obtained, set as $[\begin{matrix} x_{r} \\ {the y}_{r} \end{matrix}],$ The transformed coordinates are $[\begin{matrix} x_{r}^{,} \\ {the y}_{r}^{,} \end{matrix}],$ but:

由此便可确定定位器在该标签下的坐标(x′_r,y′_r)。由坐标(x′_r,y′_r)即可求得在标签坐标系中定位器与X轴和Y轴的夹角，由此即可确定航向角。From this, the coordinates (x′ _r , y′ _r ) of the locator under the label can be determined. From the coordinates (x′ _r , y′ _r ), the angle between the locator and the X-axis and Y-axis in the tag coordinate system can be obtained, and the heading angle can be determined accordingly.

6 确定定位器距离顶棚的垂直距离6 Determine the vertical distance from the locator to the ceiling

图12所示为相机的投影原理，确定定位传感器与粘贴标签天花板之间的高度信息的过程为：根据相机的投影原理，L为实际物体的长度，经过投影后，在图像平面上投影后为l，f为焦距，Z为物体与相机的距离，则有：Figure 12 shows the projection principle of the camera. The process of determining the height information between the positioning sensor and the label ceiling is as follows: According to the projection principle of the camera, L is the length of the actual object. After projection, it is projected on the image plane as l, f is the focal length, Z is the distance between the object and the camera, then:

$\frac{l l}{L L} = = \frac{f f}{Z Z}$

所以

其中f通过相机标定内参数获得，L和l分别由标签和图像计算得到，由此便可确定定位传感器与粘贴标签天花板之间的高度信息。so

Among them, f is obtained by calibrating the internal parameters of the camera, and L and l are calculated by the label and the image respectively, so that the height information between the positioning sensor and the label-attached ceiling can be determined.

7 标签ID号的识别7 Identification of tag ID number

本部分主要介绍如何利用平面坐标系下的向量运算快速准确的确定感光点的位置分布情况以确认标签的ID信息。This part mainly introduces how to quickly and accurately determine the position distribution of photosensitive points by using the vector operation in the plane coordinate system to confirm the ID information of the label.

图13表示了一张标签在图像坐标系下的任意分布情况，图像坐标系X′O′Y′的坐标原点O′为图像的左上角像素点，X轴水平向右，Y轴垂直向下，以O、A、B、三点构成了标签在图像中的坐标系AOB，对应像素坐标分别为(x_o,y_o)(x_a,y_a)和(x_b,y_b)，其中O为标签坐标系原点，OA指向x轴，OB指向y轴，M点和N点是A点和C点之间的两个等分点，N点用虚圈标出表示在该标签上该点处并没有感光材料，除了方向点之外的其他反光点为标识标签的编码点，用来确定标签的ID信息，采用下面的步骤来确定标签编码点的位置。Figure 13 shows the arbitrary distribution of a label in the image coordinate system. The coordinate origin O' of the image coordinate system X'O'Y' is the pixel point in the upper left corner of the image, the X axis is horizontal to the right, and the Y axis is vertically downward. , with O, A, B, and three points forming the coordinate system AOB of the label in the image, the corresponding pixel coordinates are (x _o ,y _o )(x _a ,y _a ) and (x _b ,y _b ), where O is the origin of the label coordinate system, OA points to the x-axis, and OB points to the y-axis. Points M and N are two equally divided points between points A and C. Point N is marked with a dotted circle to indicate that the There is no photosensitive material at the point, and the other reflective points except the direction point are the code points of the identification label, which are used to determine the ID information of the label. The following steps are used to determine the position of the label code point.

确定向量

求取除O、A、B三点之外的其余点的与O点构成的向量与

的夹角θ，如另一向量

那么

和

的夹角θ为：determine the vector

Find the vector sum of the other points except O, A, and B and the O point

The included angle θ, such as another vector

So

and

The included angle θ is:

选取判定阈值，当夹角θ小于该阈值时认为该点处在OB线上，此时判定M点在OB线上；Select the judgment threshold, when the included angle θ is less than the threshold, it is considered that the point is on the OB line, and at this time it is judged that point M is on the OB line;

利用向量长度之间的关系确定M点的具体位置，设

Its length is:

此时求取向量

与向量

的长度比值确定点M的具体位置；Find the vector at this time

with vector

The length ratio of determines the specific position of point M;

求出N点坐标(x_n,y_n)，若是M点也没有感光材料，那么M点坐标也以此方法确定；Find the coordinates of point N (x _n , y _n ), if there is no photosensitive material at point M, then the coordinates of point M are also determined in this way;

求得

依据（1）中所述方法求取其余向量与的夹角，可判断D点为OA线上的点，利用向量长度关系可以确定D点的位置；obtain

Calculate the remaining vectors and The included angle can determine that point D is a point on the OA line, and the position of point D can be determined by using the vector length relationship;

（3）确定其余感光点的位置(3) Determine the position of the remaining photosensitive points

将得到的向量均计算与

的夹角，通过阈值判断E点为l₂上的点，通过向量

与向量

的长度关系可以得到E的位置。同理可得知处在直线L3和L4上的感光点及其位置。The methods for determining the positions of other photosensitive points are similar to the above, except that the selection of vectors is different. Take determining the position of photosensitive points on the straight line L2 as an example. For the label shown in Figure 13, point M is taken as the starting point of the vector, and the rest of the undetermined points Determine the four vectors for the vector endpoints, such as

Compute the resulting vectors with

with vector

The length relationship can get the position of E. Similarly, the photosensitive points and their positions on the straight lines L3 and L4 can be known.

（4）确定标签ID(4) Determine the tag ID

标签上编码点的不同位置对应着二进制数的不同位，沿标签构成的坐标系x轴方向遍历十二个点，依次对应的是二进制数的第一位，第二位…第十二位；利用得到的感光点位置唯一的确定一个二进制数，其就为该标签的ID。对于图13表示的标签，其对应二进制数为101010010101，则该标签的ID号为2709。The different positions of the coding points on the label correspond to different bits of the binary number. Twelve points are traversed along the x-axis direction of the coordinate system formed by the label, which in turn corresponds to the first bit, the second bit...the twelfth bit of the binary number; Use the obtained photosensitive point position to uniquely determine a binary number, which is the ID of the tag. For the tag shown in Figure 13, its corresponding binary number is 101010010101, and the ID number of the tag is 2709.

本实用新型工作流程：The working process of the utility model:

安装本定位传感器的机器人在工作区域运动时，传感器红外发射模块发出红外线，照射天花板上的标签，传感器上的微型CMOS摄像头采集标签光斑图像，TMS320DM642 DSP芯片采用先进的算法对图像进行处理，获得传感器相对于标签的X坐标值、Y坐标值、航向角及高度等位置信息，其工作流程如图15所示。When the robot installed with this positioning sensor is moving in the working area, the infrared emission module of the sensor emits infrared rays to irradiate the label on the ceiling. The micro CMOS camera on the sensor collects the spot image of the label, and the TMS320DM642 DSP chip uses advanced algorithms to process the image to obtain the sensor The workflow is shown in Figure 15 relative to location information such as the X coordinate value, Y coordinate value, heading angle, and altitude of the tag.

Claims

1. A fast and accurate positioning system for indoor mobile robots is characterized in that it includes a positioning sensor installed on the robot and a plurality of passive tags that reflect infrared rays on the ceiling of the work area; wherein the positioning sensor includes Image processing chip, the image processing chip is respectively connected with the storage module, COMS camera, data interface, power supply and infrared emission module, the infrared emission module includes a plurality of infrared tubes, which surround the COMS camera and are divided into several groups; the passive label is There are multiple marking points on the marking label. The marking points are divided into two categories. The first type is the direction point, which is to determine the direction of the coordinate axis. In order to uniquely determine the direction, only the four corners of the marking label can be There are direction marking points at any three corners; the second type is coding points, that is, the remaining marking points, and the ID number of the label is determined by the combination of the remaining marking points; reflective infrared materials are glued on the direction points , choose to paste reflective infrared material in whole or in part according to coding needs on the remaining coding points.

2. the fast and accurate positioning system for indoor mobile robot as claimed in claim 1, is characterized in that, described infrared tube has 12, every 4 is divided into one group, totally 3 groups; 3 groups when starting to measure Open all, close one group immediately after the measurement result, and then measure, if the measurement accuracy is not affected, then close another group.