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

Abstract

The utility model relates to a rapid and accurate positioning system used for an indoor mobile robot. The rapid and accurate positioning system used for the indoor mobile robot comprises a positioning sensor and a plurality of passive tags, wherein the positioning sensor is arranged on the robot, and the passive tags are arranged on a ceiling in a working region and are used for reflecting infrared rays. The positioning sensor comprises an image processing chip. The image processing chip is connected with a storage module, a complementary metal oxide semiconductor (COMS) camera, a data interface, a power supply and an infrared emission module; the infrared emission module comprises a plurality of infrared tubes; the infrared tubes are arranged around the COMS camera and are divided into several groups; the passive tags are tags; a plurality of mark points are arranged one each tag; the mark points are divided into two types; the first type of the mark points are direction points which are used for determining the direction of a coordinate axis; in order to uniquely determine the direction, the direction mark points are respectively set at only three of the four corners of each corresponding tag; the second type of the mark points are coding points, i.e. the remaining mark points which are used for determining the identification (ID) number of the corresponding tag; reflected infrared materials are respectively pasted on the mark points used for determining the direction; and the reflected infrared materials are pasted on the remaining mark points used for coding according to the coding requirements.

Description

Rapid and accurate positioning system for indoor mobile robot

Technical Field

The utility model relates to a quick accurate positioning system for indoor mobile robot belongs to detection technology, image processing and robot navigation field.

Background

And indoor positioning is to obtain more accurate estimation of the pose of the object by certain analysis and calculation according to input information such as prior environment map information, current estimation of the pose of the object, observation data of a sensor and the like in an indoor environment. For the intelligent service robot working in indoor environments such as families, hospitals, office places and the like, accurate positioning is the premise of robot navigation and is an important guarantee for completing service tasks.

Indoor positioning sensors can be divided into absolute positioning sensors and relative positioning sensors, according to the positioning technology used. The absolute positioning mainly adopts the technologies of navigation beacons, active or passive identifiers and the like for positioning; relative positioning is the measurement of the distance and direction of an object from an initial position to determine the current position of the object, also known as dead reckoning. The current indoor robot positioning method mainly comprises the following steps: RFID-based methods, wireless sensor network-based methods, odometer and inertial navigation module-based methods, and the like.

Generally, an RFID-based method reads RFID data and then performs rough positioning, and then performs distance measurement using an ultrasonic sensor to obtain positioning information. The method requires that various possibilities are fully considered when the RFID tag is placed, and the method is inconvenient to use, low in precision, suitable for positioning under the conditions of simple environment and low precision requirement.

The method based on the wireless sensor network, such as Wi-Fi technology, Zibgee technology and the like, uses signal strength to perform positioning, needs to erect the wireless sensor network, and is high in cost, easy to interfere wireless signals and poor in precision.

The inertial navigation module-based method utilizes inertial navigation modules such as a gyroscope, an accelerometer, a magnetometer and the like, and combines a speedometer to record the course, the speed and the acceleration of an object in real time, accumulate the mileage and obtain the coordinates of the object relative to an initial position through calculation. The method has accumulated errors and drift, and the precision is difficult to guarantee when the time is long or the road condition is not good.

The invention patent No. 201110260388.5 uses an infrared emitting diode to make dot matrix road signs and attaches them to the ceiling of the room, a wide-angle infrared camera is fixed on the body of the mobile robot to shoot the infrared road signs upwards, and the computer on the body of the robot is used to analyze the images and calculate the pose of the robot in real time. The method has certain limitation, the dot matrix road sign belongs to an active label, each label is a circuit board and needs power supply, the cost is high, and the installation and the use are inconvenient. And secondly, the image processing uses an onboard industrial control computer, so that the size is large, the cost is high, and the small and medium-sized robots which are not provided with the industrial control computer cannot be used at all.

Disclosure of Invention

The utility model aims at solving the above problem, the utility model provides a quick accurate positioning system and working method for indoor mobile robot, this system comprises the passive label of installing the position sensor on the robot and pasting a plurality of reflection infrared rays on the work area ceiling, the system utilizes sensor infrared emission module to send the infrared ray, shine the label on the ceiling, miniature CMOS camera on the sensor gathers label facula image, TMS320DM642DSP chip is handled the image, obtain the sensor for the X coordinate value of label, the Y coordinate value, position information such as course angle and height, realize pinpointing. The purpose of adopting infrared ray is in order to effectively avoid the influence of visible light to the sensor, improves the precision and the robustness of location.

In order to achieve the above purpose, the utility model adopts the following technical scheme:

a fast and accurate positioning system for indoor mobile robots includes a positioning sensor mounted on a robot and a plurality of infrared-reflecting passive tags attached to the ceiling of a work area; the positioning sensor comprises an image processing chip, the image processing chip is respectively connected with a storage module, a COMS camera, a data interface, a power supply and an infrared emission module, and the infrared emission module comprises a plurality of infrared tubes which surround the COMS camera and are divided into a plurality of groups; the passive label is an identification label, 15 marking point positions are arranged on the label for pasting reflective infrared materials, the marking points are divided into two types, the first type is a direction point, namely the direction of a coordinate axis is determined, in order to uniquely determine the direction, the position of any three corners in four corners of the label can only have the direction marking point, the rest vertex can not be pasted with the reflective infrared materials, and each label must have the three direction points; the second type is coding points, namely the rest mark points, each coding point represents one bit of a binary system, and the ID number of the label can be determined through the combination of the coding points; the reflecting infrared material is adhered to the direction points, and the reflecting infrared material is adhered to all or part of the other coding points according to coding requirements.

The data interface is a UART interface, and the storage module is SDRAM, FLASH and EEPROM.

The number of the infrared tubes is 12, and 4 infrared tubes are divided into one group, and the total number of the infrared tubes is 3; when the measurement is started, all the 3 groups are opened, one group is closed immediately after the measurement result is obtained, the measurement is carried out again, and if the measurement precision is not influenced, the other group is closed again, so that the purposes that the precision is not influenced and the number of used infrared tubes is the minimum are achieved, and the purposes of saving energy and reducing heat productivity are achieved.

The positioning system comprises the following working steps:

1) according to the requirement, a proper number of labels are pasted on the ceiling of the robot working area, the distance between the labels is more than 1.5 m, and the distance between the positioning sensor and the labels ranges from 1 m to 2.5 m;

2) installing a positioning sensor on the robot, electrifying and initializing;

3) reading the storage module to complete configuration;

4) judging whether to start detection, if not, continuing to wait for a detection command; if yes, turning to step 5);

5) the COMS camera receives an infrared light spot image reflected by the label, preprocesses the image, detects whether an effective label exists in a visual field, if not, turns to 7), and if the number of the effective labels in the visual field is more than one, the optimal label is selected from the visual field to identify the light reflecting point; thereby determining three direction points in the label and a label coordinate system XOY, thereby determining X, Y coordinate information and course angle information of the robot under the label coordinate system, determining height information between the positioning sensor and a ceiling of the pasted label, and simultaneously determining ID information of the label;

6) uploading the processing result to an upper computer of the robot for displaying and controlling;

7) the upper computer determines whether to stop detection, if not, the step 5) is returned; if yes, the detection process is finished.

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:

during calibration, a calibration algorithm based on a 2D plane calibration plate is adopted, and the internal parameters of the camera are assumed to be constant all the time in the calibration process, namely, no matter the camera shoots a plane template from any angle, the internal parameters of the camera are constant, only the external parameters change, and the method comprises the following basic steps:

(1) printing a standard chessboard as a calibration plate, and attaching the calibration plate on a rigid plane;

(2) a plurality of calibration plate images are shot from different angles, and the calibration is more accurate due to a large number of calibration plate images;

(3) detecting characteristic points on the calibration plate, and determining image coordinates and actual coordinates;

(4) calculating internal parameters of the camera by using the linear model;

(5) and optimizing the internal parameters of the camera by using the distortion model, solving all the parameters, and correcting the image after determining the internal parameters so as to prepare for subsequent calculation.

In the step 5), the process of determining the effective tag and the optimal tag in the infrared image is as follows: preprocessing the acquired infrared image, firstly performing Gaussian smooth filtering on the infrared image, then selecting a proper threshold value to perform binarization processing to obtain a binarized image, extracting the contour in the binarized image, and removing a larger contour and a smaller contour to reduce the interference of noise.

The positioning sensor positions the position of the label in the infrared image by adopting a method of combining horizontal projection and vertical projection of the acquired binary image, and determines the current optimal label by using a nearest neighbor rule.

Giving a straight line, dividing the binary image into a plurality of parts by using a cluster of straight lines which are perpendicular to the straight line and have equal intervals, and counting the number of pixels with the pixel value of 1 in each part as the projection of the part of the binary image on the given straight line; when a given straight line is a horizontal or vertical straight line, calculating the number of pixels with a pixel value of 1 in each column or each row of the binary image, so as to obtain horizontal and vertical projections of the binary image, wherein the projection can be used as a feature for object identification in some applications, fig. 14a shows the binary image obtained after processing, fig. 14b and fig. 14c show the vertical projection and the horizontal projection of the binary image, respectively, and the specific classification can be seen in the following steps:

(1) successively traversing the acquired vertical projection images from left to right to obtain the spacing distance d between adjacent projection pixel clusters₁,d₂…d_nSimilarly, the horizontal projection image is successively traversed from top to bottom to obtain the spacing distance d 'of the adjacent projection pixel clusters'₁，d′₂…d′_nSince the labels are placed at a longer interval, the projection pixel clusters between different labels are also spaced at a longer distance, and the spacing distance can be used as a basis for distinguishing the labels.

(2) Determining a projection area corresponding to a label on a projection image, and taking the minimum value d of the pixel cluster spacing distance of the vertical projection image and seven times of the minimum value d 'of the pixel cluster spacing distance of the horizontal projection image, namely 7d and 7 d' as threshold values to carry out neighbor classification, wherein the specific classification method comprises the following steps: traversing the vertical projection image from left to right, finding a first projection pixel cluster, counting the distances from the rest projection pixel clusters to the pixel cluster, if the distances are smaller than 7d, belonging to the projection area of the same label, then continuously traversing to the right by taking the next projection pixel cluster larger than 7d as a reference, still being smaller than 7d and being the projection area where a second label is located, sequentially traversing downwards to obtain different areas of different labels in the vertical projection image, traversing from top to bottom for the horizontal projection image, and taking 7 d' as a distance reference, and obtaining the corresponding area of the label on the horizontal projection image by adopting the method. As shown in fig. 14b and 14c, A, B, C, D four projection areas can be obtained.

(3) Finding out the label area on the original image, making a straight line at the edge of each projection area determined in the step (2), enabling the straight line of the vertical projection area and the straight line of the horizontal projection area to intersect on the original image to obtain different rectangular areas, wherein the rectangular areas are areas where labels may exist, and fig. 14d shows four rectangular areas obtained by intersecting straight lines, wherein unreasonable areas exist, namely two rectangular areas do not have labels, and how to remove the unreasonable areas is introduced in the next step to obtain effective labels.

(4) Removing unreasonable areas to obtain valid labels, and obtaining areas where the labels may be located through the step (3), but removing the areas where the invalid areas are located to obtain valid labels is required, mainly to eliminate interference in two aspects: one is to exclude the case where no label is present in the intersecting rectangular region, and one is to exclude the case where the label is at the edge of the image so that a complete label is not obtained.

(5) And (3) obtaining a possible projection area, determining the coordinate range of the area where the label is possibly located in the image according to the edge straight line of the projection edge area, judging whether the label exists in the area or not according to the existence of the reflection point in the detection area, and if the label exists in the area, judging that the label exists in the area, otherwise, judging that the label does not exist in the area. Because the probability of false detection at the edge of the image is high, the label needs to be discarded when being close to the edge, and whether the label is at the edge of the image can be simply judged through the coordinates of the area. And the remaining label after the interference is eliminated is the effective label.

(6) If the number of the effective labels in the image is more than one, the optimal label needs to be selected. Fig. 14d shows the image area where the label is acquired. Solving two rectangular area center coordinates a (x)_a,y_a) And b (x)_b,y_b) Coordinate o (x) of center point of image_o,y_o) And calculating the oa and ob distances:

$d_{\mathrm{oa}}=\sqrt{{(x_o-x_a)}^2+{(y_o-y_a)}^2}$

$d_{\mathrm{ob}}=\sqrt{{(x_o-x_b)}^2+{(y_o-y_b)}^2}$

and taking the label with relatively small distance as an effective label under the current condition, and when the number of the labels is more than two, judging in a manner similar to the above.

The identification process of the label reflecting points in the step 5) comprises the following steps:

(1) first a gradient is defined: defining the gradient of a pixel along a certain direction as the difference of the gray values of the pixel and the next pixel in the direction, searching from top to bottom from left to right, if at a certain pixel point, the right gradient is greater than a set threshold epsilon₁If the point is a point in the mark area, the point is considered to be a point in the mark area;

(2) searching the maximum gray value in eight neighborhoods of the point by taking the point as a starting point, and finally finding out the maximum gray value point in a mark region;

(3) searching in four directions of up, down, left and right by taking the central point as a starting point, and when the pixel gray value of a certain point is smaller than a set value and the gradient is smaller than a set threshold value, considering the point as a boundary point of the mark area;

(4) respectively starting horizontal and vertical search by taking points connected with central diagonal lines as starting points until a boundary point of the mark area is found, and performing other searches in the same way;

(5) the detected areas are not necessarily all mark areas, and interference areas need to be removed, the average value of all pixels in the mark areas is calculated firstly, if the average value is too low, the mark areas are excluded, and then the size and the boundary length of the mark areas are calculated, and the exclusion is not in accordance with the threshold value;

determining all label light reflection points through the steps, selecting valley bottoms between peak values of the gray levels of the mark area and the background area as a threshold value by adopting a method for establishing a gray level histogram, and then subtracting the threshold value from the gray level value of each point of the image to obtain a new image; when the central point is determined, a gray-scale weighted centroid method is selected, wherein the gray-scale weighted centroid is that the average value of pixel coordinates of all mark points in an image is calculated by taking a gray-scale value as weight, and the formula is as follows:

$\left\{\begin{array}{c}{x}_0=\frac{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}i{w}_{i,j}}{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}{w}_{i,\mathrm{<figure-callout\; id="0"\; label="j\; y"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">j</figure-callout>}}}& \\ y_{}{}_0=\frac{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}j{w}_{i,j}}{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}{w}_{i,j}}\end{array}\right.$

x in the formula₀And y₀To calculate the pixel coordinates of the resulting center point, (i, j) represents a certain pixel point in the image, i and j represent the coordinate values of the pixel point's x-axis and y-axis, respectively, w_i，jRepresenting the pixel grey value at pixel point (i, j).

In the step 5), the process of determining three direction points in the label is as follows: calculating the length of a connecting line between any two mark points in the label, selecting the longest one, extracting two mark points corresponding to the longest connecting line, marking the two mark points as A and B respectively, calculating the middle points of the A and B connecting lines, then calculating the distances from all the points except the A and B points in the label to the middle point respectively, selecting the longest one, extracting the mark point corresponding to the longest distance, marking the mark point as O, and then A, O, B defining three points as three direction points.

The determination process of the label coordinate system comprises the following steps: point O as label coordinateThe coordinate origin of the system is set as the pixel coordinate of the O point in the image coordinate system as (x)₁,y₁) In A and B, a point is selected to rotate clockwise by 90 degrees around O, and the pixel coordinate of the selected point is (x)₂,y₂) The coordinates of the other remaining point are (x)₃,y₃) The calculation formula of the pixel coordinate (x, y) of the corresponding point P after rotation is as follows:

$\left\{\begin{array}{c}x=(x_2-x_1)\cos \left(\mathrm{\&alpha;}\right)-(y_2-y_1)\sin \left(\mathrm{\&alpha;}\right)+x_1\\ y=(y_2-y_1)\cos \left(\mathrm{\&alpha;}\right)+(x_2-x_1)\sin \left(\mathrm{\&alpha;}\right)+{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">y</figure-callout>}}_1\end{array}\right.$

wherein α represents the rotation angle.

And (4) judging the delta included angle:

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

if delta>0 is the acute angle, the point (x) is selected₂,y₂) The corresponding coordinate axis is the X axis, (X)₃,y₃) The coordinate axis corresponding to the point is Y-axis, if delta<0 is an obtuse angle, the point (x) is selected₂,y₂) The corresponding coordinate axis is Y axis, (x)₃,y₃) The corresponding coordinate axis is the X axis, and the label coordinate system XOY can be determined by the correspondence.

In the step 5), the determining process of the position information is as follows: in the image acquired by the positioning sensor, the R point is the projected position of the positioning sensor in the image, and the position is the center of the image. And establishing an AOB coordinate system in the image according to a rule for determining a label coordinate system, wherein OA is the X-axis direction, and OB is the Y-axis direction.

And when the coordinates of the positioning sensor under the label are solved, a method of twice affine transformation is used, wherein the affine transformation is the transformation between two plane coordinate systems.

Firstly, converting an AOB coordinate system and an image coordinate system, determining coordinate values of three points of the AOB in the image coordinate system by using the extracted central point of the mark point, determining the coordinate values of the three points in the AOB coordinate system according to the distance between the points, and substituting the three coordinates into an affine transformation formula to determine an affine transformation matrix and a translation matrix;

assuming an affine matrix of $\left[\begin{array}{cc}{n}_{11}& n_{12}\\ n_{21}& {\mathrm{<figure-callout\; id="22"\; label="n"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">n</figure-callout>}}_{22}\end{array}\right],$ The translation matrix is $\left[\begin{array}{c}{k}_1\\ {\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2\end{array}\right],$ The image coordinate of the R point in the image is (u)₀,v₀) Then, the coordinates of the R point in the AOB coordinate system are obtained $\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right],$

$\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right]=\left[\begin{array}{cc}{n}_{11}& n_{12}\\ n_{21}& n_{22}\end{array}\right]\left[\begin{array}{c}{u}_0\\ {\mathrm{<figure-callout\; id="0"\; label="v"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">v</figure-callout>}}_0\end{array}\right]+\left[\begin{array}{c}{k}_1\\ {\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2\end{array}\right]$

The relation between the AOB coordinate system and the label coordinate system is obtained, three direction points of the label are selected, and the size of the label and the actual distance between the mark points are known, so that the coordinates of each point in the label coordinate system can be obtained, and the coordinates of the three direction points in the AOB coordinate system can be obtained, and the assumption that the obtained affine matrix is the affine matrix $\left[\begin{array}{cc}{m}_{11}& m_{12}\\ m_{21}& {\mathrm{<figure-callout\; id="22"\; label="m"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{22}\end{array}\right],$ Since the coordinates of the origin point under the O point and the label coordinates in the image are (0, 0), the translation matrix is $\left[\begin{array}{c}0\\ 0\end{array}\right],$ The coordinates of the R point in the image coordinate system AOB can be obtained in the image and are set as $\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right],$ The transformed coordinates are $\left[\begin{array}{c}{x}_r^{,}\\ y_r^{,}\end{array}\right],$ Then:

$\left[\begin{array}{c}{x}_r^{,}\\ y_r^{,}\end{array}\right]=\left[\begin{array}{cc}{m}_{11}& m_{12}\\ m_{21}& m_{22}\end{array}\right]\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right]$

from this, the coordinates (x ') of the locator under the tag are determined'_r,y′_r). From the coordinates (x'_r,y′_r) The included angle between the locator and the X axis and the Y axis in the label coordinate system can be obtained, and therefore the course angle can be determined.

In the step 5), the process of determining the height information between the positioning sensor and the ceiling of the sticker label comprises the following steps: according to the projection principle of the camera, L is the length of the actual object, after projection, L is the focal length after projection on the image plane, and Z is the distance between the object and the camera, then there are:

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

therefore, it is not only easy to use

F is obtained by calibrating internal parameters through a camera, and L and L are respectively obtained by calculating labels and images, so that the height information between the positioning sensor and the ceiling on which the labels are stuck can be determined;

identifying the ID number of the tag: in the random distribution of a label in an image coordinate system, the origin of coordinates O 'of an image coordinate system X' O 'Y' is a pixel point at the upper left corner of the image, the X axis is horizontally rightward, the Y axis is vertically downward, O, A, B and three points form a coordinate system AOB of the label in the image, and the corresponding pixel coordinates are (X) respectively_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, and M and N are points A andtwo equally-divided points between the points C, the points N are marked by dotted circles to indicate that no photosensitive material exists at the point on the label, the other photosensitive points except the direction point are coding points of the label and are used for determining the ID information of the label, and the following steps are adopted to determine the position of the coding point of the label.

(1) Determining the position of an encoded point on a straight line OB

Determining a vector

The sum of vectors of points other than O, A, B and O point is obtained

By an angle theta, e.g. another vectorThen

And

the included angle theta is as follows:

$\mathrm{\&theta;}=\arccos \left(\frac{(x_b-x_o)(x_f-x_o)+(y_b-y_o)(y_f-y_o)}{\sqrt{{(x_b-x_o)}^2+{(y_b-y_o)}^2}\sqrt{{(x_f-x_o)}^2+{(y_f-y_o)}^2}}\right)$

and selecting a judgment threshold, and when the included angle theta is smaller than the threshold, considering that the point is on the OB line, and judging that the point M is on the OB line.

Determining the specific position of the M point by using the relation between the vector lengths

The length of the utility model is as follows:

$\left|\stackrel{\&RightArrow;}{\mathrm{OM}}\right|=\sqrt{{(x_m-x_o)}^2+{(y_m-y_o)}^2}$

at this time, the vector is obtained

And vector

Determines the specific location of point M.

For the N points where no photosensitive material exists, since it is used for discriminating other photosensitive points, the coordinates thereof are determined and set $\stackrel{\&RightArrow;}{\mathrm{ON}}=(x_n-x_o,y_n-y_o),$ $\stackrel{\&RightArrow;}{\mathrm{ON}}=2/3\stackrel{\&RightArrow;}{\mathrm{OB}},$ Then there are:

$\left\{\begin{array}{c}{x}_n-x_o=\frac{2}{3}(x_b-x_o)\\ y_n-y_o=\frac{2}{3}(y_b-y_o)\end{array}\right.$

finding the coordinates (x) of the N points_n,y_n) If the M point has no photosensitive material, the coordinate of the M point is determined by the method.

(2) Determining the position of a coding point on a straight line OA

To obtain

Calculating the sum of the remaining vectors according to the method described in (1)

The D point can be judged to be a point on the OA line, and the position of the D point can be determined by utilizing the vector length relation.

(3) Determining the positions of the remaining code points

The remaining photosites are positioned in a similar manner as described above, except that the vectors are selected differently, for example, by determining the location of the photosite point on line L2, for the label shown in FIG. 13, taking point M as the starting point of the vector, and the remaining points with undetermined positions as the ending points of the vector, four vectors are determined, e.g.

Calculating the average of the obtained vectors

The point E is judged to be l through a threshold value₂Point of (1) throughMeasurement of

And vector

The length relationship of (a) can result in the location of E. The points and their positions on the straight lines L3 and L4 are known by the same token.

(4) Determining tag 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 a coordinate system formed by the label, and the first bit and the second bit … twelfth bit of the binary number correspond to each other in sequence; a binary number is uniquely determined using the resulting photosite location, which is the ID of the tag.

The utility model relates to a can pinpoint indoor mobile robot's precision sensor fast and operating method, mobile robot carries this sensor and can realize accurate indoor location to for the correct navigation of robot provides basic condition, have very high using value and economic benefits.

Drawings

FIG. 1 is an overall block diagram of a positioning sensor system;

FIG. 2 is a schematic diagram of the arrangement of infrared emission tubes;

FIG. 3 is a schematic view of a label;

FIG. 4 is an image coordinate system;

FIG. 5 is a camera coordinate system and a world coordinate system;

FIG. 6 shows a distribution of the image and gray values of the mark points;

FIG. 7 is a center point search diagram;

FIG. 8 is a graph of a boundary search;

FIG. 9 is a schematic view of direction point identification;

FIG. 10 is a schematic view of a label coordinate axis determination;

FIG. 11 is a schematic view of position information determination;

FIG. 12 is a schematic projection diagram;

FIG. 13 is a distribution diagram of labels in an image coordinate system;

FIGS. 14a-d are diagrams of tag infrared image processing;

FIG. 15 is a sensor work flow diagram.

The device comprises an image processing chip 1, a COMS camera 2, a UART interface 3, an SDRAM4, a FLASH5, an EEPROM6, an infrared tube 7 and a power supply 8.

Detailed Description

The present invention will be further explained with reference to the accompanying drawings and examples.

In fig. 1, it includes an image processing chip 1, the image processing chip 1 is respectively connected with a storage module, a cmos camera 3, a data interface, a power supply 8 and an infrared emission module, the infrared emission module includes a plurality of infrared tubes 7, which surround the cmos camera 2 and are divided into several groups. The data interface is a UART interface 3, and the storage modules are SDRAM4, FLASH5 and EEPROM 6. The number of the infrared tubes is 12, 4 infrared tubes are divided into one group, the number of the infrared tubes is 3, all the 3 groups are opened when the measurement is started, one group is closed immediately after the measurement result is obtained, the measurement is carried out, and if the measurement precision is not influenced, the other group is closed again, so that the purpose that the precision is not influenced and the number of the used infrared tubes 7 is the minimum is ensured.

1 tag speckle image processing

The TMS320DM642DSP processor is adopted by the image processing chip, the dominant frequency range is 480 MHz-720 MHz, and the processing capacity can reach 4800MIPS under the dominant frequency of 600 MHz. DM642 with 3 dedicated configurable video interfaces (videoports) (vps) provides great convenience for video data acquisition and processing. FIG. 1 is a block diagram of a whole positioning sensor, TMS320DM642 externally extends SDRAM, FLASH and EEPROM through an EMIF interface, wherein the EEPROM is used for storing and reading sensor information configuration; reading the data from the image sensor after AD conversion through a Video Port; after the image data is processed by the TMS320DM642 in an algorithm mode, positioning information (a space coordinate value and a course angle) is obtained and is sent to the robot controller through the UART serial data port.

2 infrared emission module

The infrared emission module consists of 12 infrared tubes and a control circuit, in order to obtain the best image quality and positioning accuracy, the infrared tubes are optimally arranged as shown in figure 2, the red part in the figure is an infrared emission tube, and the black and white part is a COMS camera. The infrared tubes are arranged around the COMS camera, the number of the infrared tubes does not affect the positioning precision, but the number of the infrared tubes is less, so that the acquisition range of the positioning sensor is reduced, and the number of the infrared tubes is more, so that the power consumption is increased, and the sensor is heated. In order to solve the contradiction, in practical application, a method of infrared tube grouping control is adopted, every 4 infrared tubes are in one group and are divided into 3 groups in total, 3 groups are all opened when measurement is started, one group is closed immediately after a measurement result is obtained, measurement is carried out again, and if the measurement precision is not influenced, the other group is closed again, so that the purpose that the precision is not influenced and the number of used infrared tubes is minimum is ensured.

3 image sensor

The image sensor adopts a COMS photosensitive array, and is integrated on a main board due to the limitation of the size of the sensor.

4 power supply

The power supply input of the sensor is DC8.4V-12V, and three power supplies with different voltage levels of 1.4V, 3.3V and 5V required by the system are generated by a DCDC voltage stabilizing chip. Because the infrared emission module consumes the most power, a high-current switch type voltage stabilizing chip LM2576 is adopted.

5 communication interface

The communication interface is used as a data output interface of the sensor and must meet the interface requirement of a common robot controller computer. The utility model discloses a UART interface, binding post choose for use 9 needle D type cluster head and 3 needle 2mm interval binding post, export 232 level and TTL level simultaneously to satisfy the needs without type host computer. In addition, the UART interface can also be connected with a display device to locally display data.

Fig. 3 is a schematic view of the reflective infrared label of the present invention, which is 15cm × 15cm, and the diameter of the upper 15 small dots is 15 mm. In the figure, three white dots are pasted with infrared ray reflecting materials for positioning the sensor, all labels are provided with the three dots, and the rest twelve dots are pasted with different numbers of infrared ray reflecting materials to construct ID numbers of label attributes, wherein the ID numbers are obtained by adding the values shown in figure 3. The attribute information may be set by itself as needed.

Principle of tag identification

1 Camera calibration

The ideal camera model is a pinhole camera model, but a real pinhole does not provide enough light for a fast exposure, so both the eye and the camera use lenses instead of a single point to collect more light. However, the use of lens models deviates from simple pinhole geometry models and introduces lens distortion and other effects. Therefore, in order to obtain an ideal image, the image must be corrected, the distortion coefficient of the camera needs to be known, and the internal parameter focal length f needs to be known in order to calculate the distance between the camera and the label_oThe camera must be calibrated to obtain the intrinsic and distortion parameters before the sensor is put into use.

First, three coordinate systems are established: an image coordinate system, a camera coordinate system, and a world coordinate system. Digital images are stored in a computer in the form of an M x N two-dimensional array, each element in an image of M rows and N columns being referred to as a pixel. Defining on an imageRectangular coordinate system UOV, the coordinate (u, v) of each pixel represents the number of rows and columns of the pixel in the image, and this coordinate system is called the image coordinate system, as shown in fig. 4. At the same time, an image coordinate system X expressed by physical unit is established₁O₁Y₁The coordinate system is defined by the intersection O of the optical axis of the camera and the image plane₁Is the origin of coordinates, X₁Axis Y₁The axes are parallel to the U, V axes, respectively.

Origin O₁The coordinate in the U, V coordinate system is (u)₀,v₀) And the physical size of each pixel in the directions of the X axis and the Y axis is dx, dy, then the relationship of any one pixel in the image in two coordinate systems is:

$\left\{\begin{array}{c}u=\frac{x}{d_x}+u_0\\ v=\frac{y}{d_y}+{\mathrm{<figure-callout\; id="0"\; label="v"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">v</figure-callout>}}_0\end{array}\right.$

expressed in homogeneous coordinates and matrix form as:

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& {\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">u</figure-callout>}}_0\\ 0& \frac{1}{{\mathrm{<figure-callout\; id="0"\; label="d\; y\; v"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">d</figure-callout>}}_y}& v_{}{}_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}x\\ \mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">y</figure-callout>}\\ 1\end{array}\right]$

the geometry of the camera imaging is shown in FIG. 5, where O_cX_cY_cZ_cAs camera coordinate system, O_cO₁Is the focal length of the camera, O_cIs the optical center, X, of the camera_cAxis and Y_cAxis is respectively associated with image X₁And Y₁Axis parallel, Z_cThe axis is the optical axis of the camera and is perpendicular to the image plane.

The world coordinate system is a reference for describing the position in the camera installation environment, and the relationship between the camera coordinate system and the world coordinate system is called an external parameter, because the relationship is only used in the calibration process, and the external parameter is not involved in the actual use of the system. If the homogeneous coordinate of the space point p in the world coordinate system and the camera coordinate system is (x) respectively_w,y_w,z_w1) and (x)_c,y_c,z_c1), then the following relationship exists:

$\left[\begin{array}{c}{x}_c\\ y_{\mathrm{<figure-callout\; id="1"\; label="c\; z\; c"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">c</figure-callout>}}& \\ z_c{}_{}\\ 1\end{array}\right]=\left[\begin{array}{cc}\mathrm{<figure-callout\; id="0"\; label="R\; T"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">R</figure-callout>}& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_{\mathrm{<figure-callout\; id="1"\; label="w\; z\; w"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">w</figure-callout>}}& \\ z_w{}_{}\\ 1\end{array}\right]=M_2\left[\begin{array}{c}{x}_w\\ y_{\mathrm{<figure-callout\; id="1"\; label="w\; z\; w"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">w</figure-callout>}}& \\ z_w{}_{}\\ 1\end{array}\right]$

according to an ideal pinhole imaging model, the relationship between any point P in space and its projection position P on the image is:

$\left\{\begin{array}{c}x=\frac{f{x}_c}{z_c}\\ y=\frac{f{y}_c}{z_c}\end{array}\right.$

(x, y) is p-point physical image coordinates, (x)_c,y_c,z_c) The coordinates are in the camera coordinate system. The above formulas are simplified to obtain:

$z_{\mathrm{<figure-callout\; id="1"\; label="c\; u\; v"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">c</figure-callout>}}\left[\begin{array}{c}u\\ v\end{array}\right]\left[\begin{array}{c}\\ 1\end{array}\right]=\left[\begin{array}{ccc}\frac{1}{d_x}& 0& {\mathrm{<figure-callout\; id="0"\; label="u"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">u</figure-callout>}}_0\\ 0& \frac{1}{{\mathrm{<figure-callout\; id="0"\; label="d\; y\; v"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">d</figure-callout>}}_y}& v_{}{}_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cccc}\mathrm{<figure-callout\; id="0"\; label="f"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">f</figure-callout>}& 0& 0& 0\\ 0& \mathrm{<figure-callout\; id="0"\; label="f"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">f</figure-callout>}& 0& 0\\ 0& 0& 1& 0\end{array}\right]\left[\begin{array}{cc}\mathrm{<figure-callout\; id="0"\; label="R\; T"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">R</figure-callout>}& T\\ 0& 1\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_{\mathrm{<figure-callout\; id="1"\; label="w\; z\; w"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">w</figure-callout>}}& \\ z_w{}_{}\\ 1\end{array}\right]$

$=\left[\begin{array}{cc}{a}_{\mathrm{<figure-callout\; id="0"\; label="x\; x\; u"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">x</figure-callout>}}& x& u_{}{}_0\\ 0& a_{\mathrm{<figure-callout\; id="0"\; label="y\; v"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">y</figure-callout>}}& v_{}{}_0\\ 0& 0& 1\end{array}\right]\left[\begin{array}{cc}R& T\end{array}\right]\left[\begin{array}{c}{x}_w\\ y_{\mathrm{<figure-callout\; id="1"\; label="w\; z\; w"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">w</figure-callout>}}& \\ z_w{}_{}\\ 1\end{array}\right]$

wherein a is_x、a_y、u₀、v₀Only with respect to the internal structure of the camera, called internal parameters.

In fact, the actual imaging is not ideal imaging and has different distortion degrees, the ideal imaging point can shift due to the distortion, and the ideal imaging point (x) is assumed_u，y_u) Distortion of image point (x)_d，y_d) Can be used asThe distortion model is described by the following equation:

$\left\{\begin{array}{c}{x}_u=x_d+{\mathrm{\&delta;}}_x(x_d,y_d)\\ y_u=y_d+{\mathrm{\&delta;}}_y(x_d,y_d)\end{array}\right.$

wherein, delta_x、δ_yA non-linear distortion value that is related to the position of the image point in the image. First is the radial distortion, the mathematical model of which is as follows:

$\left\{\begin{array}{c}{\mathrm{\&delta;}}_x(x_d,y_d)=x_d(k_1{\mathrm{<figure-callout\; id="2"\; label="r\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+k_2{\mathrm{<figure-callout\; id="4"\; label="r\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+k_3{\mathrm{<figure-callout\; id="6"\; label="r\; d"\; filenames="HDA00001958160700011.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+\&CenterDot;\&CenterDot;\&CenterDot;)\\ {\mathrm{\&delta;}}_y(x_d,y_d)=y_d(k_1{\mathrm{<figure-callout\; id="2"\; label="r\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+k_2{\mathrm{<figure-callout\; id="4"\; label="r\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+k_3{\mathrm{<figure-callout\; id="6"\; label="r\; d"\; filenames="HDA00001958160700011.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+\&CenterDot;\&CenterDot;\&CenterDot;)\end{array}\right.$

wherein,

k₁、k₂、k₃for radial distortion coefficients, the first two radial orders are generally taken to meet the requirements.

The centrifugal distortion is caused by the fact that the optical axis centers of all lenses in the optical system of the camera are not strictly coplanar, and the centrifugal distortion comprises radial distortion and tangential distortion, and can be represented by the following model:

$\left\{\begin{array}{c}{\mathrm{\&delta;}}_x(x_d,y_d)=p_1(3{\mathrm{<figure-callout\; id="2"\; label="x\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">x</figure-callout>}}_d^{}+y_d^2)+2p_2{x}_d{y}_d\\ {\mathrm{\&delta;}}_y(x_d,y_d)=p_2(3{\mathrm{<figure-callout\; id="2"\; label="y\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">y</figure-callout>}}_d^{}+x_d^2)+2p_1{x}_d{y}_d\end{array}\right.$

wherein p is₁、p₂The centrifugal distortion coefficient.

Yet another distortion is due to lens design, manufacturing imperfections, or equipment imperfections, and can be represented by the following model:

$\left\{\begin{array}{c}{\mathrm{\&delta;}}_x(x_d,y_d)=s_1({\mathrm{<figure-callout\; id="2"\; label="x\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">x</figure-callout>}}_d^{}+y_d^2)\\ {\mathrm{\&delta;}}_y(x_d,y_d)=s_2({\mathrm{<figure-callout\; id="2"\; label="x\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">x</figure-callout>}}_d^{}+y_d^2)\end{array}\right.$

wherein s is₁、s₂Is the distortion coefficient.

By summarizing above, the complete distortion model is:

$\left\{\begin{array}{c}{\mathrm{\&delta;}}_x(x_d,y_d)=x_d(k_1{\mathrm{<figure-callout\; id="2"\; label="r\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+k_2{r}_d^4)+p_1(3{\mathrm{<figure-callout\; id="2"\; label="x\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">x</figure-callout>}}_d^{}+y_d^2)+2p_2{x}_d{y}_d+s_1({\mathrm{<figure-callout\; id="2"\; label="x\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">x</figure-callout>}}_d^{}+y_d^2)\\ {\mathrm{\&delta;}}_y(x_d,y_d)=y_d(k_1{\mathrm{<figure-callout\; id="2"\; label="r\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">r</figure-callout>}}_d^{}+k_2{r}_d^4)+p_2(3{\mathrm{<figure-callout\; id="2"\; label="y\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">y</figure-callout>}}_d^{}+x_d^2)+2p_1{x}_d{y}_d+s_2({\mathrm{<figure-callout\; id="2"\; label="x\; d"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">x</figure-callout>}}_d^{}+y_d^2)\end{array}\right.$

in the system, a calibration algorithm based on a 2D plane calibration plate provided by Zhangzhen is adopted during camera calibration, and the internal parameters of the camera are assumed to be constant all the time in the calibration process, namely, the internal parameters of the camera are constant no matter the camera shoots the plane template from any angle, and only the external parameters are changed. The method comprises the following basic steps:

(1) a standard checkerboard is printed as a calibration plate and attached to a rigid flat surface.

(2) A plurality of calibration plate images are shot from different angles, and the calibration can be more accurate due to the large quantity of the calibration plate images.

(3) Feature points on the calibration plate are detected, and image coordinates and actual coordinates are determined.

(4) And calculating the internal parameters of the camera by using the linear model.

(5) And optimizing the parameters in the camera by using the distortion model to calculate all the parameters.

Once the intrinsic parameters are determined, the image can be rectified and prepared for subsequent calculations.

2-active tag and optimal tag validation

In the step 5), the process of determining the effective tag and the optimal tag in the infrared image is as follows: preprocessing the acquired infrared image, firstly performing Gaussian smooth filtering on the infrared image, then selecting a proper threshold value to perform binarization processing to obtain a binarized image, extracting the contour in the binarized image, and removing a larger contour and a smaller contour to reduce the interference of noise.

The positioning sensor positions the position of the label in the infrared image by adopting a method of combining horizontal projection and vertical projection of the acquired binary image, and determines the current optimal label by using a nearest neighbor rule.

Giving a straight line, dividing the binary image into a plurality of parts by using a cluster of straight lines which are perpendicular to the straight line and have equal intervals, and counting the number of pixels with the pixel value of 1 in each part as the projection of the part of the binary image on the given straight line; when a given straight line is a horizontal or vertical straight line, calculating the number of pixels with a pixel value of 1 in each column or each row of the binary image, so as to obtain horizontal and vertical projections of the binary image, wherein the projection can be used as a feature for object identification in some applications, fig. 14a shows the binary image obtained after processing, fig. 14b and fig. 14c show the vertical projection and the horizontal projection of the binary image, respectively, and the specific classification can be seen in the following steps:

(1) successively traversing the acquired vertical projection images from left to right to obtain the spacing distance d between adjacent projection pixel clusters₁,d₂…d_nSimilarly, the horizontal projection image is successively traversed from top to bottom to obtain the spacing distance d 'of the adjacent projection pixel clusters'₁，d′₂…d′_nSince the labels are placed at a longer interval, the projection pixel clusters between different labels are also spaced at a longer distance, and the spacing distance can be used as a basis for distinguishing the labels.

(2) Determining a projection area corresponding to a label on a projection image, and taking the minimum value d of the pixel cluster spacing distance of the vertical projection image and seven times of the minimum value d 'of the pixel cluster spacing distance of the horizontal projection image, namely 7d and 7 d' as threshold values to carry out neighbor classification, wherein the specific classification method comprises the following steps: traversing the vertical projection image from left to right, finding a first projection pixel cluster, counting the distances from the rest projection pixel clusters to the pixel cluster, if the distances are smaller than 7d, belonging to the projection area of the same label, then continuously traversing to the right by taking the next projection pixel cluster larger than 7d as a reference, still being smaller than 7d and being the projection area where a second label is located, sequentially traversing downwards to obtain different areas of different labels in the vertical projection image, traversing from top to bottom for the horizontal projection image, and taking 7 d' as a distance reference, and obtaining the corresponding area of the label on the horizontal projection image by adopting the method. As shown in fig. 14b and 14c, A, B, C, D four projection areas can be obtained.

(3) Finding out the label area on the original image, making a straight line at the edge of each projection area determined in the step (2), enabling the straight line of the vertical projection area and the straight line of the horizontal projection area to intersect on the original image to obtain different rectangular areas, wherein the rectangular areas are areas where labels may exist, and fig. 14d shows four rectangular areas obtained by intersecting straight lines, wherein unreasonable areas exist, namely two rectangular areas do not have labels, and how to remove the unreasonable areas is introduced in the next step to obtain effective labels.

(4) Removing unreasonable areas to obtain valid labels, and obtaining areas where the labels may be located through the step (3), but removing the areas where the invalid areas are located to obtain valid labels is required, mainly to eliminate interference in two aspects: one is to exclude the case where no label is present in the intersecting rectangular region, and one is to exclude the case where the label is at the edge of the image so that a complete label is not obtained.

(5) And (3) obtaining a possible projection area, determining the coordinate range of the area where the label is possibly located in the image according to the edge straight line of the projection edge area, judging whether the label exists in the area or not according to the existence of the reflection point in the detection area, and if the label exists in the area, judging that the label exists in the area, otherwise, judging that the label does not exist in the area. Because the probability of false detection at the edge of the image is high, the label needs to be discarded when being close to the edge, and whether the label is at the edge of the image can be simply judged through the coordinates of the area. And the remaining label after the interference is eliminated is the effective label.

(6) If the number of the effective labels in the image is more than one, the optimal label needs to be selected. Fig. 14d shows the image area where the label is acquired. Solving two rectangular area center coordinates a (x)_a,y_a) And b (x)_b,y_b) Coordinate o (x) of center point of image_o,y_o) And calculating the oa and ob distances:

$d_{\mathrm{oa}}=\sqrt{{(x_o-x_a)}^2+{(y_o-y_a)}^2}$

$d_{\mathrm{ob}}=\sqrt{{(x_o-x_b)}^2+{(y_o-y_b)}^2}$

and taking the label with relatively small distance as an effective label under the current condition, and when the number of the labels is more than two, judging in a manner similar to the above.

3 tag reflection point identification

The reflection points are processed by using a reflection material, can reflect incident light rays back to a light source, can form a quasi-binary image with obvious gray contrast on an image under the irradiation of a paraxial light source, and is particularly suitable for being used as a high-precision characteristic point for photogrammetry.

Because the actually detected mark point is not a complete circle due to various reasons such as camera imaging, the system adopts a pixel search-based method to identify the mark area. Taking fig. 6 as an example, the identification process is as follows:

(1) for ease of calculation, we first define a gradient. The gradient of a pixel along a certain direction is defined as the difference between the gray value of the pixel and the gray value of the pixel next to the direction. Searching from top to bottom from left to right, if the right gradient is larger than a set threshold epsilon at a certain pixel point₁Then the point is considered to be a point within the marker region.

(2) And searching the maximum gray value in eight neighborhoods of the point by taking the point as a starting point, and finally finding out the maximum gray value point in the mark region. If ε in the figure₁Set to 10, the starting point is (5, 8), i.e. the point where the gray value is 7.

(3) Searching in four directions of up, down, left and right by taking the central point as a starting point, and when the pixel gray value of a certain point is smaller than a set value and the gradient is smaller than a set threshold value, considering the point as a boundary point of the mark area.

(4) And respectively starting horizontal and vertical searches by taking the points connected with the central diagonal as starting points until the boundary point of the mark area is found. Other searches are analogized as shown in fig. 7 and 8.

(5) The detected areas are not all mark areas, but interference areas need to be removed, the average value of all pixels in the mark areas is calculated firstly, if the average value is too low, the average value is eliminated, then the size and the boundary length of the mark areas are calculated, and the elimination of the threshold value is not met.

All the mark points in the visual field range can be basically determined through the steps, but the gray value of the background area is not zero considering that the background area is not ideally black during imaging, and the interference of the background needs to be eliminated in order to calculate the mark center more accurately. The system adopts a method of establishing a gray level histogram, selects the valley bottom between the peak values of the gray levels of the mark area and the background area as a threshold value, and then subtracts the threshold value from the gray level value of each point of the image to obtain a new image.

When the central point is determined, a gray-scale weighted centroid method is selected, wherein the gray-scale weighted centroid is that the average value of pixel coordinates of all mark points in an image is calculated by taking a gray-scale value as weight, and the formula is as follows:

$\left\{\begin{array}{c}{x}_0=\frac{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}i{w}_{i,j}}{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}{w}_{i,\mathrm{<figure-callout\; id="0"\; label="j\; y"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">j</figure-callout>}}}& \\ y_{}{}_0=\frac{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}j{w}_{i,j}}{\underset{(i,j)\&Element;S}{\mathrm{\&Sigma;}}{w}_{i,j}}\end{array}\right.$

x in the formula₀And y₀To calculate the pixel coordinates of the resulting center point, (i, j) represents a certain pixel point in the image, i and j represent the coordinate values of the pixel point's x-axis and y-axis, respectively, w_i,jRepresenting the pixel grey value at pixel point (i, j).

4 determining three direction points and coordinate system

In a label, the mark points are divided into two types, the first type is direction points, namely the direction of the coordinate axis of the label is determined, and only three of the four corners of the label have mark points for uniquely determining the direction. The second type is a code point used to determine the number of the tag.

As shown in fig. 9, the central point of each mark point is found in the previous section, next, the direction point and the code point are distinguished, the length between any two mark points in the label is calculated, the longest mark point is selected, the two mark points corresponding to the longest connecting line are extracted, the two mark points are respectively marked as a and B, the middle point of the connecting line of a and B is calculated, the distances from all points except the point a and the point B in the label to the middle point are respectively calculated, the longest mark point is selected, the mark point corresponding to the longest distance is extracted, the mark point is marked as O, and then A, O, B three direction points are three direction points.

The determination process of the label coordinate system comprises the following steps: setting the O point as the coordinate origin of the label coordinate systemThe pixel coordinate in the image coordinate system is (x)₁,y₁) In A and B, a point is selected to rotate clockwise by 90 degrees around O, and the pixel coordinate of the selected point is (x)₂,y₂) The coordinates of the other remaining point are (x)₃,y₃) The calculation formula of the pixel coordinate (x, y) of the corresponding point P after rotation is as follows:

$\left\{\begin{array}{c}x=(x_2-x_1)\cos \left(\mathrm{\&alpha;}\right)-(y_2-y_1)\sin \left(\mathrm{\&alpha;}\right)+x_1\\ y=(y_2-y_1)\cos \left(\mathrm{\&alpha;}\right)+(x_2-x_1)\sin \left(\mathrm{\&alpha;}\right)+{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA00001958160700011.png,HDA00001958160700022.png"\; state="\{\{state\}\}">y</figure-callout>}}_1\end{array}\right.$

wherein α is the rotation angle.

And (4) judging the delta included angle:

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

if delta>0 is the acute angle, the point (x) is selected₂,y₂) The corresponding coordinate axis is the X axis, (X)₃,y₃) The coordinate axis corresponding to the point is Y-axis, if delta<0 is an obtuse angle, the point (x) is selected₂,y₂) The corresponding coordinate axis is Y axis, (x)₃,y₃) The corresponding coordinate axis is the X axis, and the label coordinate system XOY can be determined by the correspondence.

5 determination of position information

The determination process of the position information comprises the following steps: in the image acquired by the positioning sensor, the R point is the projected position of the positioning sensor in the image, and the position is the center of the image. And establishing an AOB coordinate system in the image according to a rule for determining a label coordinate system, wherein OA is the X-axis direction, and OB is the Y-axis direction.

When the coordinates of the locator under the label are solved, a method of twice affine transformation is used, and the affine transformation is the transformation between two plane coordinate systems. The basic principle of affine transformation is: suppose P₁、P₂、P₃Is any three points, P ', that are not collinear in plane'₁、P’₂、P'₃Also three points that are not collinear in the plane, there is also only one affine transformation T, let T (P)_i)＝P’_i(i =1,2, 3). Can be expressed by the following formula:

$\left[\begin{array}{c}{x}^{,}\\ y^{,}\end{array}\right]=\left[\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]\left[\begin{array}{c}x\\ y\end{array}\right]+\left[\begin{array}{c}{b}_1\\ {\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">b</figure-callout>}}_2\end{array}\right]$

wherein, $\left[\begin{array}{c}x\\ y\end{array}\right],$ $\left[\begin{array}{c}{x}^{,}\\ y^{,}\end{array}\right]$ respectively before and after transformation. $\left[\begin{array}{cc}{a}_{11}& a_{12}\\ a_{21}& a_{22}\end{array}\right]$ For affine matrices, transformations such as rotation are generated. $\left[\begin{array}{c}{b}_1\\ {\mathrm{<figure-callout\; id="2"\; label="b"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">b</figure-callout>}}_2\end{array}\right]$ Is a translation matrix. From affine matrix and translation matrixAn affine transformation can be uniquely determined.

Firstly, the transformation of the AOB coordinate system and the image coordinate system can determine the coordinate values of A, O, B three points in the image coordinate system, and simultaneously can also determine the coordinate values of three points in the AOB coordinate system, and the radiation transformation matrix and the translation matrix can be determined by substituting the three coordinates into a radiation transformation formula.

Assuming that the obtained radiation matrix is $\left[\begin{array}{cc}{n}_{11}& n_{12}\\ n_{21}& {\mathrm{<figure-callout\; id="22"\; label="n"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">n</figure-callout>}}_{22}\end{array}\right],$ The translation matrix is $\left[\begin{array}{c}{k}_1\\ {\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2\end{array}\right],$ The image coordinate of the R point in the image is known as (u)₀,v₀) Then, the coordinates of the R point in the AOB coordinate system can be obtained $\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right],$

$\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right]=\left[\begin{array}{cc}{n}_{11}& n_{12}\\ n_{21}& n_{22}\end{array}\right]\left[\begin{array}{c}{u}_0\\ {\mathrm{<figure-callout\; id="0"\; label="v"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">v</figure-callout>}}_0\end{array}\right]+\left[\begin{array}{c}{k}_1\\ {\mathrm{<figure-callout\; id="2"\; label="k"\; filenames="HDA00001958160700011.png,HDA00001958160700012.png"\; state="\{\{state\}\}">k</figure-callout>}}_2\end{array}\right]$

The relationship between the AOB coordinate system and the tag coordinate system is found, the tag coordinate system and the AOB coordinate system are known, and the affine matrix is assumed to be obtained as $\left[\begin{array}{cc}{m}_{11}& m_{12}\\ m_{21}& {\mathrm{<figure-callout\; id="22"\; label="m"\; filenames="HDA00001958160700022.png"\; state="\{\{state\}\}">m</figure-callout>}}_{22}\end{array}\right],$ Since the coordinates of the O point in the image and the origin in the label coordinate system are (0, 0), the translation matrix is $\left[\begin{array}{c}0\\ 0\end{array}\right],$ The coordinates of the R point in the AOB coordinate system can be obtained in the image and are set as $\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right],$ The transformed coordinates are $\left[\begin{array}{c}{x}_r^{,}\\ y_r^{,}\end{array}\right],$ Then:

$\left[\begin{array}{c}{x}_r^{,}\\ y_r^{,}\end{array}\right]=\left[\begin{array}{cc}{m}_{11}& m_{12}\\ m_{21}& m_{22}\end{array}\right]\left[\begin{array}{c}{x}_r\\ y_r\end{array}\right]$

from this, the coordinates (x ') of the locator under the tag can be determined'_r,y′_r). From the coordinates (x'_r,y′_r) The included angle between the locator and the X axis and the Y axis in the label coordinate system can be obtained, and therefore the course angle can be determined.

6 determining the vertical distance of the locator from the ceiling

Fig. 12 shows the principle of projection of the camera, and the process of determining the height information between the position sensor and the ceiling of the sticker label is: according to the projection principle of the camera, L is the length of the actual object, after projection, L is the focal length after projection on the image plane, and Z is the distance between the object and the camera, then there are:

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

therefore, it is not only easy to use

Wherein f is obtained by calibrating internal parameters with a camera, and L and L are respectively obtained by calculating the label and the image, so that the height information between the positioning sensor and the ceiling of the pasted label can be determined.

7 identification of tag ID number

This section mainly describes how to determine the location distribution of the photosites quickly and accurately by using vector operation in a planar coordinate system to confirm the ID information of the tag.

Fig. 13 shows an arbitrary distribution of labels in an image coordinate system, where the origin O 'of coordinates of the image coordinate system X' O 'Y' is the pixel point at the upper left corner of the image, the X-axis is horizontally to the right,the Y axis is vertically downward, O, A, B three points form a 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) Wherein O is the origin of the coordinate system of the label, OA points to the x axis, OB points to the y axis, M and N points are two equal points between the A point and the C point, the N point is marked by a dotted circle to indicate that no photosensitive material exists at the point on the label, other reflective points except the direction point are coding points for marking the label and are used for determining the ID information of the label, and the following steps are adopted for determining the position of the coding points of the label.

(1) Determining the position of an encoded point on a straight line OB

Determining a vector

The sum of vectors of points other than O, A, B and O point is obtained

By an angle theta, e.g. another vector

Then

And

the included angle theta is as follows:

$\mathrm{\&theta;}=\arccos \left(\frac{(x_b-x_o)(x_f-x_o)+(y_b-y_o)(y_f-y_o)}{\sqrt{{(x_b-x_o)}^2+{(y_b-y_o)}^2}\sqrt{{(x_f-x_o)}^2+{(y_f-y_o)}^2}}\right)$

selecting a judgment threshold value, considering the point to be on an OB line when the included angle theta is smaller than the threshold value, and judging that the point M is on the OB line at the moment;

determining the specific position of the M point by using the relation between the vector lengths

The length of the utility model is as follows:

$\left|\stackrel{\&RightArrow;}{\mathrm{OM}}\right|=\sqrt{{(x_m-x_o)}^2+{(y_m-y_o)}^2}$

at this time, the vector is obtained

And vector

The specific position of the point M is determined by the length ratio of (a);

for the N points where no photosensitive material exists, since it is used for discriminating other photosensitive points, the coordinates thereof are determined and set $\stackrel{\&RightArrow;}{\mathrm{ON}}=(x_n-x_o,y_n-y_o),$ $\stackrel{\&RightArrow;}{\mathrm{ON}}=2/3\stackrel{\&RightArrow;}{\mathrm{OB}},$ Then there are:

$\left\{\begin{array}{c}{x}_n-x_o=\frac{2}{3}(x_b-x_o)\\ y_n-y_o=\frac{2}{3}(y_b-y_o)\end{array}\right.$

finding the coordinates (x) of the N points_n,y_n) If the M point has no photosensitive material, the coordinate of the M point is determined by the method;

(2) determining the position of a coding point on a straight line OA

To obtain

Calculating the sum of the remaining vectors according to the method described in (1)The included angle of the D point can be judged to be a point on an OA line, and the position of the D point can be determined by utilizing the vector length relation;

(3) determining the position of the remaining photosites

The remaining photosites are positioned in a similar manner as described above, except that the vectors are selected differently, for example, by determining the location of the photosite point on line L2, for the label shown in FIG. 13, taking point M as the starting point of the vector, and the remaining points with undetermined positions as the ending points of the vector, four vectors are determined, e.g.

Calculating the average of the obtained vectors

The point E is judged to be l through a threshold value₂Point of (a) by a vector

And vector

The length relationship of (a) can result in the location of E. The same reasoning holds for the photosites and their positions on the lines L3 and L4.

(4) Determining tag 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 a coordinate system formed by the label, and the first bit and the second bit … twelfth bit of the binary number correspond to each other in sequence; a binary number is uniquely determined using the resulting photosite location, which is the ID of the tag. For the tag shown in fig. 13, the corresponding binary number is 101010010101, and the ID number of the tag is 2709.

The utility model discloses work flow:

when the robot provided with the positioning sensor moves in a working area, the infrared emission module of the sensor emits infrared rays to irradiate a label on a ceiling, a micro CMOS camera on the sensor collects a light spot image of the label, the TMS320DM642DSP chip processes the image by adopting an advanced algorithm to obtain position information of the sensor relative to the label, such as an X coordinate value, a Y coordinate value, a course angle, a height and the like, and the working flow is shown in FIG. 15.

Claims (2)

1. A fast and accurate positioning system for indoor mobile robot is characterized in that it comprises a positioning sensor mounted on the robot and a plurality of passive tags reflecting infrared rays and adhered on the ceiling of a working area; the positioning sensor comprises an image processing chip, the image processing chip is respectively connected with a storage module, a COMS camera, a data interface, a power supply and an infrared emission module, and the infrared emission module comprises a plurality of infrared tubes which surround the COMS camera and are divided into a plurality of groups; the passive tag is an identification tag, a plurality of marking points are arranged on the identification tag, the marking points are divided into two types, the first type is a direction point, namely the direction of a coordinate axis is determined, and in order to uniquely determine the direction, direction marking points are arranged at any three angles in four angles of the identification tag; the second type is coding points, namely the rest mark points, and the ID number of the label is determined by the combination of the rest mark points; the reflecting infrared material is adhered to the direction points, and the reflecting infrared material is adhered to all or part of the other coding points according to coding requirements.

2. The fast precise positioning system for indoor mobile robots according to claim 1, wherein the number of said infrared tubes is 12, and 4 of said infrared tubes are grouped into 3 groups; when the measurement is started, all the 3 groups are opened, one group is closed immediately after the measurement result is obtained, the measurement is carried out again, and if the measurement precision is not influenced, the other group is closed again.

Images