CN102419178B - Mobile robot positioning system and method based on infrared road sign - Google Patents

Abstract

The invention discloses a mobile robot indoor positioning system based on an infrared road sign. The system is structurally characterized in that: a dot matrix road sign is manufactured by using an infrared emitting diode and is adhered to an indoor ceiling; a wide-angle infrared camera is fixed on the body of a mobile robot to shoot the infrared road sign upwards; and a computer on the body of the robot performs image analysis and calculates out the position and pose of the robot in real time. The positioning system disclosed by the invention can be used for the positioning of the mobile robot at indoor places with a relatively large range, and has the characteristics of fast calculation speed, high positioning accuracy and strong anti-interference performance.

Description

Mobile robot positioning system based on infrared road sign and method

Technical field

The present invention relates to the infrared imagery technique field, relate to particularly the mobile robot visual field of locating technology.

Background technology

Along with the develop rapidly of computer technology, very large scale integration technology, network technology, artificial intelligence technology etc., Robotics has also obtained the development of advancing by leaps and bounds.The kind of robot is more and more, and range of application is also more and more wider.2007, the president Bill Gates of Microsoft foretold in " Scientific Beauty compatriots " magazine: PC the same the enter huge numbers of families of robot before will be with 30 years.The Korea S Samsung institute for economic research once predicted, arrived the year two thousand twenty, and world robot market scale will be to reaching 1.4 trillion dollars, and Seoul's Information and Communication Ministry was even once drawn up the surprising target that there is a robot in the every family of the year two thousand twenty.For intelligent mobile robot, in order to walk and arrive efficiently destination from environment, and make it judge it self position according to some features that in environment, oneself knows, Here it is mobile robot's orientation problem.Robot wants in circumstances not known, automatically to walk, and location is the most basic problem.

The many researchists of recent domestic use multiple sensors, and mobile robot's self-align problem is conducted in-depth research, and have proposed many method for self-locating.Method for self-locating comprises: topological representation method, dead reckoning, Kalman Filter Estimation, Grid Method, probabilistic method, location and chartography etc. simultaneously.Sensor for location comprises: vision sensor, laser, infrared, ultrasonic, code-disc, gyroscope, accelerometer etc.Code-disc, gyroscope, accelerometer are the aiding sensors for local positioning.Infrared, sonac is limit by precision, is generally used for and promptly keeps away barrier.The laser sensor cost is higher, is not suitable for civilian popularization.The environmental information that vision sensor obtains is the abundantest, the development space maximum.The researchist lays particular emphasis on the robotic vision Position Research at present.

Vision location general minute natural landmark and two kinds of patterns of artificial landmark of mobile robot.Natural landmark refers to that utilizing in environment original scene to serve as a mark positions navigation.Although natural landmark is not destroyed original environment, universality is good, and calculation of complex, robustness are not strong, poor practicability.Artificial landmark refers to that the specific road sign of artificial design is arranged in environment, although artificial landmark changes to some extent to environment, calculating is simple, feature is stable, practical.Mostly the artificial landmark that the researchist adopts at present is the pattern of design special color information or texture structure information, also comprises numeral, letter and two-dimensional bar code etc.These patterns are all generally to use scraps of paper printing and making, by ambient light illumination, make the video camera photosensitive imaging, the impact that therefore changed by ambient lighting, thereby less stable.

Summary of the invention

The objective of the invention is, for avoiding the existing weak point of above-mentioned prior art, provides a kind of mobile robot visual positioning system and method based on infrared road sign, realize for the mobile robot accurately, fast, the location of robust.

Mobile robot's indoor locating system based on infrared road sign of the present invention comprises: dot matrix active infrared road sign, be attached on indoor ceiling, and be 3 * 3 type dot matrixs, the world coordinates value of infrared road sign is determined in advance; The wide-angle thermal camera, be fixed on it the mobile robot, and for catching infrared image, the internal reference of wide-angle imaging machine and distortion parameter are demarcated in advance, its initial homography matrix H ₀also be determined in advance; Computer unit, for from the wide-angle thermal camera, receiving described infrared image, and carry out distortion correction and image pretreatment operation to infrared image, and carry out the infrared road sign of detection and Identification according to the infrared image after processing, and then utilizes initial homography matrix H ₀, adopt expansion homography matrix localization method to position calculating to the mobile robot.

The present invention also provides a kind of mobile robot's indoor orientation method based on infrared road sign, and the method comprising the steps of: the dot matrix active infrared road sign note of 3 * 3 types is invested on ceiling, determine the world coordinates value of all infrared road signs; The wide-angle thermal camera is fixed on it robot, demarcates internal reference and the distortion parameter of wide-angle imaging machine, calculate the initial homography matrix H of video camera ₀; Determine mobile robot's initial pose; Catch infrared image, go forward side by side line distortion correction and image pretreatment operation; Detect and identify infrared road sign; Utilize initial homography matrix H ₀, adopt expansion homography matrix localization method to position calculating.

Advantage of the present invention is as follows: the infrared road sign pattern making that the present invention proposes is simple, facilitates image detection, identification and location Calculation; The location algorithm principle is simple, calculating is quick, accurate positioning.The positioning system that the present invention proposes can be used for indoor place in a big way carry out the mobile robot fast, accurately, the location of robust.

The accompanying drawing explanation

Fig. 1 is that system of the present invention forms schematic diagram;

Fig. 2 is the infrared road sign schematic diagram in the present invention;

The process flow diagram that works offline that Fig. 3 is positioning system of the present invention;

The process flow diagram that works online that Fig. 4 is positioning system of the present invention;

Fig. 5 is the translation rotation relationship figure between true mark and virtual mark.

Embodiment

For making the purpose, technical solutions and advantages of the present invention clearer, below in conjunction with specific embodiment, and, with reference to accompanying drawing, the present invention is described in more detail.

Fig. 1 has provided the schematic diagram of the mobile robot positioning system based on infrared road sign of the present invention.

With reference to Fig. 1, this system comprises: be attached at dot matrix active infrared road signs 1 on indoor ceiling, 3 * 3 types; Be fixed on robot wide-angle thermal camera 2 with it; Computer unit 3 for graphical analysis and location Calculation.

Ceiling plane is parallel to the ground, infrared road sign 1 is attached at ceiling plane, wide-angle thermal camera 2 is fixed on robot platform top, to the infrared road sign of photographs, for the computer unit 3 of graphical analysis and location Calculation, is placed in mobile robot platform inside.The mounting distance of infrared road sign 1 need to be considered from the high integrity of ceiling in conjunction with visual field size and the video camera of video camera, so that there is not location in video camera, dead angle is as the criterion, be that video camera at least can photograph Anywhere a road sign in scene, post a plurality of infrared road signs 1 on ceiling plane.

The dot matrix of infrared road sign 1 is by the luminous realization of SMD infrared-emitting diode.The emission wavelength of infrared-emitting diode generally has two kinds of 850nm and 940nm, due to CCD, the light sensitivity of 850nm light wave is greater than the light wave of 940nm, therefore selects the infrared-emitting diode that wavelength is 850nm.The specific design pattern of infrared road sign 1 as shown in Figure 2.Two kinds of point sets are arranged, and all the other points that peripheral area is larger in the dot matrix of 3*3 at 4.Each road sign has identical four, position a little bigger, for determining the subcoordinate system of road sign, determines initial point and the XY coordinate axis of road sign dot matrix, in addition, these 4 in location algorithm for calculating homography matrix.The ID value that 5 remaining points are used for calculating road sign is to distinguish each road sign, and number of combinations is 32, can mean 32 kinds of different road signs.The calculating of ID value adopts binary-coded mode, has provided the scale-of-two code value of each point in Fig. 2, and ID value scope is 0-31.

For the orientation range that single signpost is covered road sign quantity wide as much as possible, that use is few as much as possible, video camera used must have enough large field range, so the infrared wide-angle video camera adopts the large wide-angle lens that focal length is 2.8mm or 2.5mm.The narrow bandpass filter plate that camera lens the place ahead mounting center wavelength is 850nm, thus only the light wave of 850nm (optical wavelength that road sign dot matrix infrared diode is corresponding) is carried out to photosensitive imaging.

The course of work of the mobile robot's indoor locating system based on infrared road sign of the present invention comprises off-line procedure and at line process.Wherein off-line procedure relates to the configuration of localizing environment and the calculating of various parameters.

With reference to Fig. 3, off-line procedure comprises: internal reference and the distortion parameter of demarcating the wide-angle imaging machine; Determine the world coordinates value of all infrared road signs; Determine mobile robot's initial pose; Calculate the initial homography matrix H of video camera ₀.

With reference to Fig. 3, off-line procedure specifically comprises the following steps:

Step S301, first complete camera calibration work, demarcates internal reference and the distortion parameter of wide-angle imaging machine, and the mapping matrix of video camera internal reference and distortion correction is stored in to respective document T1.

Step S302, make road sign, particularly, adopts the strip circuit plate to be barricaded as sphere of movements for the elephants type shelf, locates soldering surface mounted infrared-emitting diode in point of crossing.

Step S303, be attached at ceiling plane by road sign, and video camera is arranged on to the mobile robot platform top.

After localizing environment is put up, remaining work is exactly to set up cartographic information and measure the parameter for location Calculation, cartographic information is mainly that the subcoordinate that records each road sign ties up to the position in world coordinates, calculate the world coordinates value of each road sign by step S304, and deposit respective document T2 in.The world coordinates value of each road sign determines that method is as follows: due to four anchor points being arranged on each road sign, so can utilize the homography matrix relation that the position of road sign is determined one by one.The world coordinates data of road sign are stored in a file, and form is:

0：1，0 460，0 0，460 0，460 230

1：0，0 0，0 0，0 0，0 0

2：0，0 0，0 0，0 0，0 0

… … …

… … …

… … …

31：0，0 0，0 0，0 0，0 0 (1)

The format description of each row of data is as follows:

Road sign ID: whether this road sign world coordinates is determined (is 1, no 0), the world coordinates value of anchor point 0 (between each coordinate with space-separated), the world coordinates value of anchor point 1 (between each coordinate with space-separated), the world coordinates value of anchor point 2 (between each coordinate with space-separated), the world coordinates value of anchor point 3 (between each coordinate with space-separated).

Suppose and select road sign that ID is 0 as the initial point road sign, using the subcoordinate system of No. 0 road sign as world coordinates.The process that the world coordinates of other road sign calculates is as follows: 1. detect all road signs in the shooting visual field, and identify their ID value.2. the road sign that identifies of search from road sign world coordinates document, if there is the world coordinates of certain road sign to determine (judging by sign), utilize four anchor points of this road sign to calculate the homography matrix H of present image.Utilize H to calculate other and do not determine four anchor point coordinates of the road sign of world coordinates value, then deposit data in document.3., if the road sign in the visual field does not all have to determine, the mobile robot, restart the first step.4. after the road sign that can photograph has all been determined, calculate and finish.

Next, calculate initial homography matrix H at step S305 ₀, and deposit result of calculation in H ₀document T3.Definite method of initial homography matrix is described as follows: in the expansion homography matrix localization method based on the hypothesis of plane mechanism that the present invention adopts, with camera coordinates, be reference, homography matrix does not change, and road sign produces virtual displacement.The location Calculation of back is all carried out for initial homography matrix.Because all road signs are all unified under world coordinate system, so four points of initial homography matrix in can an optional road sign are calculated.

Definite method of homography matrix is as follows:

The projection relation that in road sign, the position of each point and video camera 3 imaging planes form is the projective rejection of plane to plane, i.e. homography matrix transformation relation, and formulae express is as shown in Equation 2.

$s_i\left[\begin{array}{c}{u}_i\\ v_i\\ 1\end{array}\right]=H\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]=\left[\begin{array}{ccc}{m}_1& m_2& m_3\\ m_4& m_5& m_6\\ m_7& m_8& m_9\end{array}\right]\left[\begin{array}{c}{x}_i\\ y_i\\ 1\end{array}\right]---\left(2\right)$

In above formula, H is homography matrix, m _i(i=1,2 ..., 9) be the matrix element of homography matrix, s _ifor scale-up factor, (u _i, v _i) coordinate for road sign point in image, (x _i, y _i) be the world coordinates of road sign point.Formula (2) is expanded into to three equations,

$\left\{\begin{array}{c}{s}_i{u}_i=m_1{x}_i+m_2{y}_i+m_3\\ s_i{v}_i=m_4{x}_i+m_5{y}_i+m_6\\ s_i=m_7{x}_i+m_8{y}_i+m_9\end{array}\right.---\left(3\right)$

By first equation, divided by the 3rd equation, second equation is divided by the 3rd equation, and s disappears _iobtain two equations:

$\left\{\begin{array}{c}{x}_i{\mathrm{<figure-callout\; id="1"\; label="m"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">m</figure-callout>}}_1+y_i{\mathrm{<figure-callout\; id="2"\; label="m"\; filenames="HDA0000088989460000011.png,HDA0000088989460000031.png"\; state="\{\{state\}\}">m</figure-callout>}}_2+m_3-u_i{x}_i{m}_7-u_i{y}_i{m}_8=u_i{m}_9\\ x_i{m}_4+y_i{m}_5+m_6-v_i{x}_i{m}_7-v_i{y}_i{m}_8=v_i{m}_9\end{array}\right.---\left(4\right)$

If the n on the known object plane point, know their volume coordinate (x _i, y _i) (i=1,2 ..., n), and known their picture point coordinate (u _i, v _i) (i=1,2 ..., n), we have 2n the linear equation about the homography matrix element, below with matrix form, write out these equations:

$\left[\begin{array}{cccccccc}{x}_1& {\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_1& 1& 0& 0& 0& -u_1{x}_1& -u_1{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_1\\ 0& 0& 0& x_1& {\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_1& 1& -v_1{x}_1& -v_1{\mathrm{<figure-callout\; id="1"\; label="y"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_1\\ & & ...& ...& ...& & & \\ & & ...& ...& ...& & & \\ x_{\mathrm{<figure-callout\; id="1"\; label="n\; y\; n"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">n</figure-callout>}}& y_n{}_{}& 1& 0& 0& 0& -u_n{x}_n& -u_{\mathrm{<figure-callout\; id="0"\; label="n\; y\; n"\; filenames="HDA0000088989460000012.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">n</figure-callout>}}{y}_n{}_{}\\ 0& 0& 0& x_n& y_n& 1& -v_n{x}_n& -v_n{y}_n\end{array}\right]\left[\begin{array}{c}{m}_1\\ m_2\\ m_3\\ m_4\\ m_5\\ m_6\\ m_7\\ m_8\end{array}\right]=\left[\begin{array}{c}{u}_1{m}_9\\ v_1{m}_9\\ ...\\ ...\\ ...\\ ...\\ u_n{m}_9\\ v_n{m}_9\end{array}\right]---\left(5\right)$

Can make m ₉=1, making equation coefficient is matrix K, and variable is M, is worth for U, and equation can be written as KM=U, and the least square solution of equation is:

M＝(K ^TK) ^-1K ^TU (6)

There are 8 unknown numbers, can pass through four pairs of some solving equations, thereby obtain each matrix element value of homography matrix.

Last completing steps S306, calculate the initial position (x of video camera photocentre ₀, y ₀), and deposit document T4 in.Definite method of video camera photocentre initial position is described as follows: the video camera internal reference is demarcated in advance, and the outer ginseng of video camera is that impact point is transformed into the transformed matrix of camera coordinate system from world coordinate system, comprises three rotating vectors and a translation vector.With equation expression, be:

$\left[\begin{array}{c}{X}_c\\ Y_{\mathrm{<figure-callout\; id="1"\; label="c\; Z\; c"\; filenames="HDA0000088989460000011.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">c</figure-callout>}}& \\ Z_c{}_{}\\ 1\end{array}\right]=M_2\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{r}_1& r_2& r_3& \mathrm{<figure-callout\; id="0"\; label="t"\; filenames="HDA0000088989460000012.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">t</figure-callout>}\\ 0& 0& 0& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]---\left(7\right)$

(X _c, Y _c, Z _c) be the coordinate figure of impact point under camera coordinate system, (X _w, Y _w, Z _w) be the coordinate figure of impact point under world coordinate system.M ₂for video camera is joined matrix, r outward ₁, r ₂, r ₃for the rotating vector of outer three quadratures of joining of video camera, the translation vector that t is the outer ginseng of video camera.Select infrared road sign that ID is 31 as the initial point road sign, under road sign, select free-position to calculate homography matrix H ₀, utilize on road sign 9 pairs of coplanar points to solve video camera simultaneously and join M outward ₂.The location of video camera photocentre is to calculate the coordinate figure of photocentre under world coordinate system.Computing formula is as follows:

$\left[\begin{array}{c}{x}_0\\ y_0\\ 0\\ 1\end{array}\right]={M_2}^{-1}\left[\begin{array}{c}{X}_c\\ Y_c\\ Z_c\\ 1\end{array}\right]$ (X _c＝0，Y _c＝0，Z _c＝0)

(8)

The process that works online that Fig. 4 is the mobile robot's indoor locating system based on infrared road sign of the present invention.Working online of task is mainly to utilize the various parameters that work offline and measure, and detects and identify infrared road sign, adopts expansion homography matrix localization method to carry out the location Calculation of video camera.

With reference to Fig. 4, at line process, comprise: the infrared image that video camera 2 catches on ceiling, and the image of catching is sent to computer unit 3; The image of 3 pairs of receptions of computer unit carries out distortion correction and image pretreatment operation; Computer unit 3 detects and identifies infrared road sign; Computer unit 3 utilizes initial homography matrix H ₀, adopt expansion homography matrix localization method to position calculating.

With reference to Fig. 4, off-line procedure specifically comprises the following steps:

At first, at step S401, by after being arranged on video camera in robot and obtaining the road sign image on ceiling, utilize correction matrix T1 to carry out the pre-service work of image, comprise image rectification, image filtering, binaryzation, morphology processing etc.

At step S402, detect all road signs (the sphere of movements for the elephants road sign be constituted by a diode) in the visual field and select to be positioned in the middle of image road sign as the initial point road sign.

At step S403, carry out the detection of four peripheral anchor points for selected road sign.

At step S404, calculate road sign ID value.

At step S405, the initial position T3 of the road sign world coordinates value T2 that utilizes off-line to record, initial homography matrix T3 and video camera photocentre, adopt expansion homography matrix localization method to position calculating.

The prerequisite of expansion homography matrix localization method is the hypothesis of plane mechanism: 1. road sign is attached at ceiling plane, and video camera is fixed in the mobile platform top, and world coordinates XY plane is based upon on the road sign plane; 2. camera motion plane and road sign plane parallel; 3. the motion of video camera only has two kinds, parallel, and the vertical line on ground of take rotates as turning axle.The principle of expansion homography matrix localization method is: under the prerequisite of the hypothesis of plane mechanism, utilize the relativity of motion, by being fixed on video camera on the robot platform rotary flat shifting movement with respect to the road sign plane, be converted into the rotary flat shifting movement of road sign with respect to camera coordinate system; By the hypothesis constant position that calculates virtual road sign of homography matrix, the pose of being extrapolated video camera by the pose difference between virtual road sign and actual road sign changes, thereby reaches the purpose of localization for Mobile Robot.Relation between former and later two poses of the motion of video camera can use formula (9) to mean, and corresponding motion pose relation of take the gauge point that video camera is reference can use formula (10) expression.

In formula (9), P _cam, P ' _cam, P " _camfor the attained pose of video camera, in formula (10), P _mark, P ' _mark, P " _markvirtual pose for road sign.

In conjunction with parameter definite in off-line procedure, positioning calculation process comprises:

At first, calculate the virtual pose P of road sign " _mark.

In motion positions computation process under the hypothesis of plane mechanism, homography matrix remains unchanged, and the motion of mobile platform is equivalent to the reciprocal translation of road sign/rotation.Suppose that road sign opposite direction translation/postrotational pose is P " _mark.

In conjunction with initial homography matrix H ₀, can pass through the formula of picture point coordinate solution point coordinate by derived for solving:

$x_i=\frac{(u_i-m_3)(m_5-v_i{m}_8)-(v_i-m_6)(m_2-u_i{m}_8)}{(m_1-u_i{m}_7)(m_5-v_i{m}_8)-(m_4-v_i{m}_7)(m_2-u_i{m}_8)}$

$y_i=\frac{(u_i-m_3)(m_4-v_i{m}_7)-(v_i-m_6)(m_1-u_i{m}_7)}{(m_2-u_i{m}_8)(m_4-v_i{m}_7)-(m_5-v_i{m}_8)(m_1-u_i{m}_7)}---\left(11\right)$

Wherein, m _ifor homography matrix H ₀element, (x _i, y _i) be the world coordinates of object point, (u _i, v _i) be image coordinate.During in new pose, detect the road sign in image when video camera, obtain image coordinate (u _i, v _i), then in conjunction with H ₀utilize formula (11) to calculate the world coordinates (x of road sign _i, y _i), due to the homography matrix adopted in the calculating homography matrix that is video camera when the initial position, so (the x calculated _i, y _i) be the world coordinates of virtual road sign, three road sign points can form virtual road sign pose P " _mark.Fig. 5 has provided the graph of a relation of original road sign and virtual road sign, p ₀p ₁p ₂3 is real road sign point, p ' ₀p ' ₁p ' ₂virtual road sign point when only having translation motion, p " ₀p " ₁p " ₂for translation adds postrotational virtual road sign point.O ' is the initial position of video camera photocentre with respect to world coordinates.Utilize H ₀inverse P out " _markbe corresponding p " ₀p " ₁p " ₂.

Secondly, calculate rotational angle theta.

P ₀p ₁p ₂with p " ₀p " ₁p " ₂between differential seat angle be exactly rotation angle θ.θ can be by calculating two vectors

with

differential seat angle draw.Will

be normalized to (cos (θ ₁), sin (θ ₁)),

be normalized to (cos (θ ₂), sin (θ ₂)), θ=θ is arranged ₁-θ ₂.

Finally, need displacement calculating S.

The above has calculated rotation angle θ and p " ₀p " ₁p " ₂, only need be by p " ₀p " ₁p " ₂be rotated counterclockwise θ and obtain p ' centered by o ' ₀p ' ₁p ' ₂.Formula (12) is p " ₀rotation obtains p ' ₀formula, other 2 are similar.

$\left[\begin{array}{c}{x}_{{\mathrm{<figure-callout\; id="0"\; label="p"\; filenames="HDA0000088989460000012.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">p</figure-callout>}}_0^{\&prime;}}\\ {\mathrm{<figure-callout\; id="0"\; label="y\; p"\; filenames="HDA0000088989460000012.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_{p_{}^{}}\end{array}\right]=\left[\begin{array}{cc}\cos \mathrm{\&theta;}& -\sin \mathrm{\&theta;}\\ \sin \mathrm{\&theta;}& \cos \mathrm{\&theta;}\end{array}\right](\left[\begin{array}{c}{\mathrm{<figure-callout\; id="0"\; label="x\; p"\; filenames="HDA0000088989460000012.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">x</figure-callout>}}_{p_{}^{}}\\ {\mathrm{<figure-callout\; id="0"\; label="y\; p"\; filenames="HDA0000088989460000012.png,HDA0000088989460000021.png"\; state="\{\{state\}\}">y</figure-callout>}}_{p_{}^{}}\end{array}\right]-\left[\begin{array}{c}{x}_{o^{\&prime;}}\\ y_{o^{\&prime;}}\end{array}\right])+\left[\begin{array}{c}{x}_{o^{\&prime;}}\\ y_{o^{\&prime;}}\end{array}\right]---\left(12\right)$

Three displacements on theory

should equate, but, owing to there being the error of calculation, can obtain by the mode be averaged the displacement S of video camera.

$x_S=\frac{1}{3}\underset{i=0}{\overset{2}{\mathrm{\&Sigma;}}}(x_{p_i}-x_{p_i^{\&prime;}})$ $y_S=\frac{1}{3}\underset{i=0}{\overset{2}{\mathrm{\&Sigma;}}}(y_{p_i}-y_{p_i^{\&prime;}})---\left(13\right)$

By the calculating of above rotational angle theta and displacement S, determined the pose of robot with respect to the initial point road sign, reached the purpose of space relative positioning.

At step S406, positioning result is carried out to the filtering processing, can adopt kalman filter method to carry out.

Above-described embodiment provides is basic structure scheme and the algorithm operating flow process of the mobile robot positioning system based on infrared road sign proposed by the invention; protection scope of the present invention is not limited to this; also comprise this organization plan of employing and location algorithm and slightly make other schemes of changing in periphery, the structure or the mobile robot platform that change m*m (m is more than or equal to 3 integer) as the dot matrix road sign into change moving vehicle platform etc. into.

Above-described specific embodiment; purpose of the present invention, technical scheme and beneficial effect are further described; institute is understood that; the foregoing is only specific embodiments of the invention; be not limited to the present invention; within the spirit and principles in the present invention all, any modification of making, be equal to replacement, improvement etc., within all should being included in protection scope of the present invention.

Claims (10)

1. the mobile robot's indoor locating system based on infrared road sign, this system comprises:

Dot matrix active infrared road sign, be attached on indoor ceiling, is m * m type dot matrix, and the world coordinates value of infrared road sign is determined in advance, and wherein m is more than or equal to 3 integer;

The wide-angle thermal camera, be fixed on it the mobile robot, and for catching infrared image, the internal reference of wide-angle imaging machine and distortion parameter are demarcated in advance, its initial homography matrix H ₀also be determined in advance;

Computer unit, for from the wide-angle thermal camera, receiving described infrared image, and infrared image is carried out to distortion correction and image pretreatment operation, and carry out the infrared road sign of detection and Identification according to the infrared image after processing, then utilize initial homography matrix H0, adopt expansion homography matrix location to position calculating to the mobile robot

Wherein world coordinates XY plane is based upon on the road sign plane, camera motion plane and road sign plane parallel, the motion of video camera comprises parallel and take the vertical line on ground and rotates as turning axle, expansion homography matrix location further comprises: the relativity of utilizing motion, to be fixed on the rotary flat shifting movement of robot video camera with it with respect to the road sign plane, be converted into the rotary flat shifting movement of road sign with respect to camera coordinate system; By the hypothesis constant position that calculates virtual road sign of homography matrix, the pose of being extrapolated video camera by the pose difference between virtual road sign and actual road sign changes, thereby reaches the purpose of localization for Mobile Robot.

2. positioning system according to claim 1, is characterized in that, described dot matrix active infrared road sign is made by SMD infrared-emitting diode, and near-infrared wavelength is 850nm.

3. positioning system according to claim 2, it is characterized in that, the dot matrix of described dot matrix active infrared road sign is 3 * 3 types, by periphery, larger 4 and all the other less points form, larger 4 detections for road sign and location Calculation, all the other less points are for the ID value calculating of road sign.

4. positioning system according to claim 3, is characterized in that, described infrared wide-angle video camera adopts the large wide-angle lens that focal length is 2.8mm or 2.5mm, the narrow bandpass filter plate that camera lens the place ahead mounting center wavelength is 850nm.

5. system according to claim 4, is characterized in that, described computer unit calculates the detection and Identification road sign by identification and the ID value of a territory detection, road sign point clustering, Selecting landmarks, four anchor points.

6. the mobile robot's indoor orientation method based on infrared road sign, the method comprising the steps of:

The dot matrix active infrared road sign note of 3 * 3 types is invested on ceiling, determine the world coordinates value of all infrared road signs;

The wide-angle thermal camera is fixed on it robot, demarcates internal reference and the distortion parameter of wide-angle imaging machine, calculate the initial homography matrix H of video camera ₀;

Determine mobile robot's initial pose;

Catch infrared image, go forward side by side line distortion correction and image pretreatment operation;

Detect and identify infrared road sign;

Utilize initial homography matrix H ₀adopt expansion homography matrix location to position calculating, wherein world coordinates XY plane is based upon on the road sign plane, camera motion plane and road sign plane parallel, the motion of video camera comprises parallel and take the vertical line on ground and rotates as turning axle, expansion homography matrix location further comprises: the relativity of utilizing motion, to be fixed on the rotary flat shifting movement of robot video camera with it with respect to the road sign plane, be converted into the rotary flat shifting movement of road sign with respect to camera coordinate system; By the hypothesis constant position that calculates virtual road sign of homography matrix, the pose of being extrapolated video camera by the pose difference between virtual road sign and actual road sign changes, thereby reaches the purpose of localization for Mobile Robot.

7. localization method according to claim 6, is characterized in that, described dot matrix active infrared road sign is made by SMD infrared-emitting diode, and near-infrared wavelength is 850nm.

8. localization method according to claim 7, it is characterized in that, the dot matrix of described dot matrix active infrared road sign larger by periphery 4 and all the other less points form, larger 4 detections for road sign and location Calculation, and all the other less points are for the ID value calculating of road sign.

9. localization method according to claim 8, is characterized in that, described infrared wide-angle video camera adopts the large wide-angle lens that focal length is 2.8mm or 2.5mm, the narrow bandpass filter plate that camera lens the place ahead mounting center wavelength is 850nm.

10. localization method according to claim 9, is characterized in that, described computer unit calculates the detection and Identification road sign by identification and the ID value of a territory detection, road sign point clustering, Selecting landmarks, four anchor points.

Images