CN103994762A - Mobile robot localization method based on data matrix code - Google Patents

Abstract

The invention discloses a mobile robot localization method based on a data matrix code, relates to the technical field of mobile robots, and solves the technical problems of reducing localization realization cost and improving localization accuracy. The method is as follows: first a plurality of localization points are set in a mobile robot work area, coordinates of each localization point are compiled into a 2D data matrix code to print into a tag, the localization tag of each localization point is pasted on a ceiling; a mobile robot uses a camera to obtain a localization tag mage on the ceiling, then relative coordinates between the camera and the localization tag can be calculated according to coordinates of the localization tag mage in a camera itself image coordinate system, and by combination of coordinates of the localization point marked by the data matrix code in the localization tag, specific coordinates of a current position point of the mobile robot can be calculated. The mobile robot localization method is suitable for mobile localization for the mobile robots.

Description

Based on the method for positioning mobile robot of data matrix code

Technical field

The present invention relates to mobile robot technology, particularly relate to a kind of technology of the method for positioning mobile robot based on data matrix code.

Background technology

Using tag location is one of main method of localization for Mobile Robot, and the method for positioning mobile robot of existing employing tag location mainly contains following several:

1) use REID (RFID) to realize location, realize in the method for localization for Mobile Robot in this use REID, a series of RFID tag is positioned on the floor or ceiling of applied environment, then their installation site is calibrated, while being placed in radio-frequency (RF) identification code reader in robot through these labels, can identify corresponding label and carry out self location according to the position of label.Although RFID tag is cheap, but use radio-frequency identification method to need special specific code reader and the label made, it is embodied as originally still relatively high, and code reader can not directly obtain with respect to position and the angle information of label in identifying, its positioning precision is also lower.

2) based on ultrared localization method, the Stargazer sensor that Korean company Hagisonic produces is a masterpiece realizing location based on infrared ray, it is to remove to irradiate with infrared transmitter the passive-type label being arranged in advance on ceiling, then use a CMOS camera to remove to resolve the infrared image returning from label, this localization method needs special specific sensor and the infrared emitting/receiving trap made, it is embodied as originally higher, the price of whole system exceedes 1,000 dollars, exceed the price of a lot of miniature mobile robots on market, and install also relatively complicated.

Summary of the invention

For the defect existing in above-mentioned prior art, technical matters to be solved by this invention is to provide one, and to realize cost low, the method for positioning mobile robot based on data matrix code that positioning precision is high.

In order to solve the problems of the technologies described above, a kind of method for positioning mobile robot based on data matrix code provided by the present invention, is characterized in that, concrete steps are as follows:

First for mobile work robot region is set one for indicating the earth axes of ground location point, and in mobile work robot region, set multiple location subregion, in the subregion of each location, set an anchor point, on mobile robot, settle camera lens camera upward;

By each anchor point, the coordinate in earth axes is compiled into two-dimentional data matrix code again, and is printed on label, thereby obtains the positioning label of each anchor point;

Under the positioning label of each anchor point being posted in to this anchor point again, locate on the ceiling directly over subregion;

Mobile robot is positioned at a location when subregion, utilize camera to obtain the positioning label image of the anchor point of this location subregion, the coordinate in camera self image coordinate system according to this positioning label image again, calculate the position relationship between camera and this positioning label, and from this positioning label image, extract the data matrix code of anchor point, after extracted data matrix code is converted to the coordinate of anchor point in earth axes, position relationship between combining camera and this positioning label, calculate the concrete coordinate of mobile robot's current location point in earth axes, position relationship between camera and positioning label refers to: the level interval between camera current location point and positioning label, and the grid azimuth of camera current location point in earth axes.

Further, on described positioning label, be also printed with location pattern, and for the alternating pattern of the ranks number of unlabeled data square matrix code;

Described data matrix code is made up of two kinds of squares of black and white, and the shape of two kinds of squares is consistent, and each square in data matrix code is arranged in two-dimentional matrix shape;

Described location pattern is the L shaped pattern of black with two straight flanges, a straight flange is wherein the horizontal edge of location pattern, another straight flange is the longitudinal edge of location pattern, location pattern has three label reference points, one of them label reference point is main datum point, another two label reference point secondary standard points, main datum point is wherein positioned at the junction surface of two straight flanges, and two secondary standard points lay respectively at the free end of two straight flanges;

Described alternating pattern is the L shaped pattern being formed by two kinds of square alternative arrangements of black and white with two straight flanges, a horizontal edge that straight flange is alternating pattern wherein, another straight flange is the longitudinal edge of alternating pattern, the shape of the each square in alternating pattern is all consistent with the square shape of data matrix code, alternating pattern encloses a rectangle frame with location pattern, described data matrix code is printed in the rectangle frame being enclosed by location pattern and alternating pattern, in alternating pattern, except the square at two straight flange junction surfaces, square row of each the square unlabeled data square matrix code on horizontal edge, a square of each the square unlabeled data square matrix code on longitudinal edge is capable, from positioning label image, extract after the data matrix code of anchor point, the ranks number indicating according to alternating pattern is decoded to data matrix code.

Further, between computing camera and positioning label, the method for position relationship is as follows: first from positioning label image, extract location pattern, and obtain the coordinate of three label reference points in camera image coordinate system of location pattern, the coordinate of initial point in combining camera image coordinate system and the mathematical model of camera, calculate the position relationship between camera and positioning label again.

Further, in each positioning label, horizontal edge, the longitudinal edge of location pattern are parallel with abscissa axis, the axis of ordinates of earth axes respectively;

Abscissa axis, the axis of ordinates of camera local Coordinate System are parallel with abscissa axis, the axis of ordinates of robot local Coordinate System respectively;

The computing formula of calculating mobile robot's current location point concrete coordinate in earth axes is as follows:

$T_{\mathrm{world}}^{\mathrm{robot}}=T_{\mathrm{world}}^{D-M}*{\left(T_{\mathrm{cam}}^{D-M}\right)}^{-1}*{\left(T_{\mathrm{robot}}^{\mathrm{cam}}\right)}^{-1};$

In formula, for the coordinate of mobile robot's current location point in earth axes, for positioning label is with respect to the transition matrix of earth axes, for positioning label is with respect to the transition matrix of camera image coordinate system, for camera is with respect to mobile robot's transition matrix;

matrix Formula be:

$T_{\mathrm{world}}^{D-M}=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{dm}}_x^i\\ 0& 1& 0& {\mathrm{dm}}_y^i\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

In formula, for the horizontal ordinate translational component of positioning label in earth axes, for the ordinate translational component of positioning label in earth axes, with after posting up ceiling, positioning label obtains by demarcation;

matrix Formula be:

$T_{\mathrm{cam}}^{D-M}=\left[\begin{array}{cccc}-\cos \left(\mathrm{\θ}\right)& \sin \left(\mathrm{\θ}\right)& 0& -\frac{z*(u-c_x)}{f_x}\\ \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)& 0& \frac{z*(v-c_y)}{f_y}\\ 0& 0& -1& z\\ 0& 0& 0& 1\end{array}\right];$

In formula, f _xfor the camera focus abscissa value in camera image coordinate system, f _yfor the camera focus ordinate value in camera image coordinate system, z is the vertical spacing between image center point and ceiling, u is the horizontal ordinate of primary standard subpoint in camera image coordinate system, v is the ordinate of main datum point projected image in camera image coordinate system, the projection of the location pattern main datum point that main datum point projected image refers to positioning label in positioning label image, c _xfor the initial point of the camera image coordinate system abscissa value in camera image coordinate system, c _yfor the initial point of the camera image coordinate system ordinate value in camera image coordinate system, θ is the angle between location horizontal edge projected image and camera image coordinate system transverse axis, locates the projection in positioning label image of horizontal edge that horizontal edge projected image refers to the location pattern on positioning label;

matrix Formula be:

$T_{\mathrm{robot}}^{\mathrm{cam}}=\left[\begin{array}{cccc}1& 0& 0& x_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 1& 0& y_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 0& 1& z_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 0& 0& 1\end{array}\right];$

In formula, for image center point and the horizontal ordinate difference of mobile robot's central point in earth axes, for image center point and the horizontal ordinate difference of mobile robot's central point in earth axes, for the vertical spacing between image center point and mobile robot's central point;

Image center point is a point of fixity of demarcating in advance on camera, and mobile robot's central point is a point of fixity of demarcating in advance on mobile robot.

Method for positioning mobile robot based on data matrix code provided by the invention, the positioning label that employing is printed with two-dimensional data matrix code provides locating information accurately, pattern on this positioning label is only made up of black and white two looks, use common black and white camera, IP Camera, just can identify data matrix code, and the intensive information of can encoding in narrow and small label plane, it is also very extensive that it reads visual angle, can under only having 20% environment, contrast read, error correcting capability is also stronger, in the impaired situation of coding that reaches 25%, still can read the information of coding, compared with existing method for positioning mobile robot, it is of the present invention that to realize cost lower, directly data matrix code is printed on to the making that just can complete positioning label on common paper, also very low to the requirement of camera, and robot can directly obtain anchor point coordinate by positioning label, its positioning precision is also high.

Brief description of the drawings

Fig. 1 is the schematic diagram of the positioning label in the method for positioning mobile robot based on data matrix code of the embodiment of the present invention;

Fig. 2 is the schematic diagram of the location pattern in the method for positioning mobile robot based on data matrix code of the embodiment of the present invention;

Fig. 3 is the schematic diagram of the alternating pattern in the method for positioning mobile robot based on data matrix code of the embodiment of the present invention;

Fig. 4 is the schematic diagram of the data matrix code in the method for positioning mobile robot based on data matrix code of the embodiment of the present invention;

Fig. 5 is the schematic diagram of the each coordinate system in the method for positioning mobile robot based on data matrix code of the embodiment of the present invention.

Embodiment

Below in conjunction with brief description of the drawings, embodiments of the invention are described in further detail, but the present embodiment is not limited to the present invention, every employing analog structure of the present invention and similar variation thereof, all should list protection scope of the present invention in.

A kind of method for positioning mobile robot based on data matrix code that the embodiment of the present invention provides, is characterized in that, concrete steps are as follows:

First for mobile work robot region is set one for indicating the earth axes of ground location point, and in mobile work robot region, set multiple location subregion, in the subregion of each location, set an anchor point, on mobile robot, settle a camera lens towards directly over camera;

The coordinate in earth axes by each anchor point again, according to data matrix code international standard, ECC200 is compiled into two-dimentional data matrix code, and is printed on label, thereby obtains the positioning label of each anchor point;

Under the positioning label of each anchor point being posted in to this anchor point again, locate on the ceiling directly over subregion;

Mobile robot is positioned at a location when subregion, utilize camera to obtain the positioning label image of the anchor point of this location subregion, the coordinate in camera self image coordinate system according to this positioning label image again, calculate position relationship between camera and this positioning label, and from this positioning label image, extract the data matrix code of anchor point, after extracted data matrix code is converted to the coordinate of anchor point in earth axes according to data matrix code international standard ECC200, position relationship between combining camera and this positioning label, calculate the concrete coordinate of mobile robot's current location point in earth axes, position relationship between camera and positioning label refers to: the level interval between camera current location point and positioning label, and the grid azimuth of camera current location point in earth axes,

As Figure 1-Figure 4, in the embodiment of the present invention, on described positioning label, be also printed with location pattern, and for the alternating pattern of the ranks number of unlabeled data square matrix code;

The structure of described data matrix code and compile mode are prior art, data matrix code is made up of two kinds of squares of black and white, and the shape of two kinds of squares is consistent, black square wherein represents a binary one, white square represents a Binary Zero, and each square in data matrix code is arranged in two-dimentional matrix shape;

Described location pattern is the L shaped pattern of black with two straight flanges, a straight flange is wherein the horizontal edge of location pattern, another straight flange is the longitudinal edge of location pattern, location pattern has three label reference points, one of them label reference point is main datum point, another two label reference point secondary standard points, main datum point is wherein positioned at the junction surface (being L shaped corner portion) of two straight flanges, and two secondary standard points lay respectively at the free end (being L shaped both ends) of two straight flanges;

Described alternating pattern is the L shaped pattern being formed by two kinds of square alternative arrangements of black and white with two straight flanges, a horizontal edge that straight flange is alternating pattern wherein, another straight flange is the longitudinal edge of alternating pattern, the shape of the each square in alternating pattern is all consistent with the square shape of data matrix code, alternating pattern encloses a rectangle frame with location pattern, described data matrix code is printed in the rectangle frame being enclosed by location pattern and alternating pattern, in alternating pattern, except the square at two straight flange junction surfaces, square row of each the square unlabeled data square matrix code on horizontal edge, a square of each the square unlabeled data square matrix code on longitudinal edge is capable, from positioning label image, extract after the data matrix code of anchor point, the ranks number indicating according to alternating pattern is decoded to data matrix code.

In the embodiment of the present invention, between computing camera and positioning label, the method for position relationship is as follows: first from positioning label image, extract location pattern, and obtain the coordinate of three label reference points in camera image coordinate system of location pattern, the coordinate of initial point in combining camera image coordinate system and the mathematical model of camera again, calculate the position relationship between camera and positioning label, the mathematical model of camera is prior art.

In the embodiment of the present invention, in each positioning label, horizontal edge, the longitudinal edge of location pattern are parallel with abscissa axis, the axis of ordinates of earth axes respectively, and the rotational component that makes each positioning label is zero.

In the embodiment of the present invention, the abscissa axis of camera local Coordinate System, axis of ordinates are parallel with abscissa axis, the axis of ordinates of robot local Coordinate System respectively, and making camera is zero with respect to mobile robot's rotational component.

In the embodiment of the present invention, the computing formula of calculating mobile robot's current location point concrete coordinate in earth axes is as follows:

$T_{\mathrm{world}}^{\mathrm{robot}}=T_{\mathrm{world}}^{D-M}*{\left(T_{\mathrm{cam}}^{D-M}\right)}^{-1}*{\left(T_{\mathrm{robot}}^{\mathrm{cam}}\right)}^{-1};$

In formula, for the coordinate of mobile robot's current location point in earth axes, for positioning label is with respect to the transition matrix of earth axes, for positioning label is with respect to the transition matrix of camera image coordinate system, for camera is with respect to mobile robot's transition matrix;

matrix Formula be:

$T_{\mathrm{world}}^{D-M}=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{dm}}_x^i\\ 0& 1& 0& {\mathrm{dm}}_y^i\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

In formula, for the horizontal ordinate translational component of positioning label in earth axes, for the ordinate translational component of positioning label in earth axes, with after posting up ceiling, positioning label obtains by demarcation, for example use the laser with vertical emission function to indicate main datum point on the positioning label forward projection point on ground, spacing between this forward projection point and the abscissa axis of earth axes is the horizontal ordinate translational component of positioning label in earth axes, and the spacing between this forward projection point and the axis of ordinates of earth axes is the ordinate translational component of positioning label in earth axes;

matrix Formula be:

$T_{\mathrm{cam}}^{D-M}=\left[\begin{array}{cccc}-\cos \left(\mathrm{\θ}\right)& \sin \left(\mathrm{\θ}\right)& 0& -\frac{z*(u-c_x)}{f_x}\\ \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)& 0& \frac{z*(v-c_y)}{f_y}\\ 0& 0& -1& z\\ 0& 0& 0& 1\end{array}\right];$

In formula, f _xfor the camera focus abscissa value in camera image coordinate system, f _yfor the camera focus ordinate value in camera image coordinate system, z is the vertical spacing between image center point and ceiling, image center point obtains by demarcation, u is the horizontal ordinate of primary standard subpoint in camera image coordinate system, v is the ordinate of main datum point projected image in camera image coordinate system, the projection of the location pattern main datum point that main datum point projected image refers to positioning label in positioning label image, c _xfor the initial point of the camera image coordinate system abscissa value in camera image coordinate system, c _yfor the initial point of the camera image coordinate system ordinate value in camera image coordinate system, θ is the angle between location horizontal edge projected image and camera image coordinate system transverse axis, locates the projection in positioning label image of horizontal edge that horizontal edge projected image refers to the location pattern on positioning label;

matrix Formula be:

$T_{\mathrm{robot}}^{\mathrm{cam}}=\left[\begin{array}{cccc}1& 0& 0& x_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 1& 0& y_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 0& 1& z_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 0& 0& 1\end{array}\right];$

In formula, for image center point and the horizontal ordinate difference of mobile robot's central point in earth axes, for image center point and the horizontal ordinate difference of mobile robot's central point in earth axes, for the vertical spacing between image center point and mobile robot's central point, mobile robot's central point obtains by demarcation, obtain by measuring the installation site of camera on mobile robot;

Image center point can Shi Qi mass centre point, also can be the point of fixity that on camera one is easy to measure relative distance, mobile robot's central point can Shi Qi mass centre point, can be also the point of fixity that on mobile robot one is easy to measure relative distance.

Fig. 5 is the schematic diagram of the each coordinate system in the method for positioning mobile robot based on data matrix code of the embodiment of the present invention, abscissa axis Xw in Fig. 5, axis of ordinates Yw, the coordinate axis that ordinate axle Zw is earth axes, abscissa axis Xd, axis of ordinates Yd, ordinate axle Zd is the coordinate axis of positioning label coordinate system, the setting means of positioning label coordinate system is prior art, abscissa axis Xi, axis of ordinates Yi is the coordinate axis of camera self image coordinate system, abscissa axis Xc, axis of ordinates Yc, ordinate axle Zc is the coordinate axis of camera local Coordinate System, abscissa axis Xr, axis of ordinates Yr, ordinate axle Zr is the coordinate axis of mobile robot's local Coordinate System, the location pattern main datum point that P point is positioning label, P ' point is the projection of P in positioning label image.

In two kinds of indoor environments, utilize existing two mobile robot to test the method for the embodiment of the present invention;

The ceiling of test environment 1 is 2.9 meters to the distance on ground, and positioning label is of a size of 10 centimetres of x10 centimetre, and the laying spacing between positioning label is 1.5 meters, and the camera of settling on mobile robot is that model is the IP Camera of Logitech c200;

When existing iRobot Create robot tests in test environment 1, to the processing speed of positioning label image 10 frames that are greater than per second, the positioning precision of robot is less than 5 centimetres;

When existing MobileRobots Pioneer P3DX robot tests in test environment 1, the maximum translational speed of robot is 1.5 meter per seconds, and to the processing speed of positioning label image 10 frames that are greater than per second, the positioning precision of robot is less than 5 centimetres;

The ceiling of test environment 2 is 4 meters to the distance on ground, and positioning label is of a size of 20 centimetres of x20 centimetre, and the laying spacing between positioning label is 1.5 meters, and the camera of settling on mobile robot is that model is the IP Camera of Logitech c200;

When existing iRobot Create robot and MobileRobots Pioneer P3DX robot test in test environment 2, can obtain equally the positioning precision that is less than 5 centimetres.

Claims (4)

1. the method for positioning mobile robot based on data matrix code, is characterized in that, concrete steps are as follows:

First for mobile work robot region is set one for indicating the earth axes of ground location point, and in mobile work robot region, set multiple location subregion, in the subregion of each location, set an anchor point, on mobile robot, settle camera lens camera upward;

By each anchor point, the coordinate in earth axes is compiled into two-dimentional data matrix code again, and is printed on label, thereby obtains the positioning label of each anchor point;

Under the positioning label of each anchor point being posted in to this anchor point again, locate on the ceiling directly over subregion;

Mobile robot is positioned at a location when subregion, utilize camera to obtain the positioning label image of the anchor point of this location subregion, the coordinate in camera self image coordinate system according to this positioning label image again, calculate the position relationship between camera and this positioning label, and from this positioning label image, extract the data matrix code of anchor point, after extracted data matrix code is converted to the coordinate of anchor point in earth axes, position relationship between combining camera and this positioning label, calculate the concrete coordinate of mobile robot's current location point in earth axes, position relationship between camera and positioning label refers to: the level interval between camera current location point and positioning label, and the grid azimuth of camera current location point in earth axes.

2. the method for positioning mobile robot based on data matrix code according to claim 1, is characterized in that: on described positioning label, be also printed with location pattern, and for the alternating pattern of the ranks number of unlabeled data square matrix code;

Described data matrix code is made up of two kinds of squares of black and white, and the shape of two kinds of squares is consistent, and each square in data matrix code is arranged in two-dimentional matrix shape;

Described location pattern is the L shaped pattern of black with two straight flanges, a straight flange is wherein the horizontal edge of location pattern, another straight flange is the longitudinal edge of location pattern, location pattern has three label reference points, one of them label reference point is main datum point, another two label reference point secondary standard points, main datum point is wherein positioned at the junction surface of two straight flanges, and two secondary standard points lay respectively at the free end of two straight flanges;

Described alternating pattern is the L shaped pattern being formed by two kinds of square alternative arrangements of black and white with two straight flanges, a horizontal edge that straight flange is alternating pattern wherein, another straight flange is the longitudinal edge of alternating pattern, the shape of the each square in alternating pattern is all consistent with the square shape of data matrix code, alternating pattern encloses a rectangle frame with location pattern, described data matrix code is printed in the rectangle frame being enclosed by location pattern and alternating pattern, in alternating pattern, except the square at two straight flange junction surfaces, square row of each the square unlabeled data square matrix code on horizontal edge, a square of each the square unlabeled data square matrix code on longitudinal edge is capable, from positioning label image, extract after the data matrix code of anchor point, the ranks number indicating according to alternating pattern is decoded to data matrix code.

3. the method for positioning mobile robot based on data matrix code according to claim 2, it is characterized in that, between computing camera and positioning label, the method for position relationship is as follows: first from positioning label image, extract location pattern, and obtain the coordinate of three label reference points in camera image coordinate system of location pattern, the coordinate of initial point in combining camera image coordinate system and the mathematical model of camera, calculate the position relationship between camera and positioning label again.

4. the method for positioning mobile robot based on data matrix code according to claim 3, is characterized in that: in each positioning label, horizontal edge, the longitudinal edge of location pattern are parallel with abscissa axis, the axis of ordinates of earth axes respectively;

Abscissa axis, the axis of ordinates of camera local Coordinate System are parallel with abscissa axis, the axis of ordinates of robot local Coordinate System respectively;

The computing formula of calculating mobile robot's current location point concrete coordinate in earth axes is as follows:

$T_{\mathrm{world}}^{\mathrm{robot}}=T_{\mathrm{world}}^{D-M}*{\left(T_{\mathrm{cam}}^{D-M}\right)}^{-1}*{\left(T_{\mathrm{robot}}^{\mathrm{cam}}\right)}^{-1};$

In formula, for the coordinate of mobile robot's current location point in earth axes, for positioning label is with respect to the transition matrix of earth axes, for positioning label is with respect to the transition matrix of camera image coordinate system, for camera is with respect to mobile robot's transition matrix;

matrix Formula be:

$T_{\mathrm{world}}^{D-M}=\left[\begin{array}{cccc}1& 0& 0& {\mathrm{dm}}_x^i\\ 0& 1& 0& {\mathrm{dm}}_y^i\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right];$

In formula, for the horizontal ordinate translational component of positioning label in earth axes, for the ordinate translational component of positioning label in earth axes, with after posting up ceiling, positioning label obtains by demarcation;

matrix Formula be:

$T_{\mathrm{cam}}^{D-M}=\left[\begin{array}{cccc}-\cos \left(\mathrm{\θ}\right)& \sin \left(\mathrm{\θ}\right)& 0& -\frac{z*(u-c_x)}{f_x}\\ \sin \left(\mathrm{\θ}\right)& \cos \left(\mathrm{\θ}\right)& 0& \frac{z*(v-c_y)}{f_y}\\ 0& 0& -1& z\\ 0& 0& 0& 1\end{array}\right];$

In formula, f _xfor the camera focus abscissa value in camera image coordinate system, f _yfor the camera focus ordinate value in camera image coordinate system, z is the vertical spacing between image center point and ceiling, u is the horizontal ordinate of primary standard subpoint in camera image coordinate system, v is the ordinate of main datum point projected image in camera image coordinate system, the projection of the location pattern main datum point that main datum point projected image refers to positioning label in positioning label image, c _xfor the initial point of the camera image coordinate system abscissa value in camera image coordinate system, c _yfor the initial point of the camera image coordinate system ordinate value in camera image coordinate system, θ is the angle between location horizontal edge projected image and camera image coordinate system transverse axis, locates the projection in positioning label image of horizontal edge that horizontal edge projected image refers to the location pattern on positioning label;

matrix Formula be:

$T_{\mathrm{robot}}^{\mathrm{cam}}=\left[\begin{array}{cccc}1& 0& 0& x_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 1& 0& y_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 0& 1& z_{\mathrm{robot}}^{\mathrm{cam}}\\ 0& 0& 0& 1\end{array}\right];$

In formula, for image center point and the horizontal ordinate difference of mobile robot's central point in earth axes, for image center point and the horizontal ordinate difference of mobile robot's central point in earth axes, for the vertical spacing between image center point and mobile robot's central point;

Image center point is a point of fixity of demarcating in advance on camera, and mobile robot's central point is a point of fixity of demarcating in advance on mobile robot.