CN102419178A - 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 and method based on infrared road sign

Technical field

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

Background technology

The develop rapidly of Along with computer technology, very large scale integration technology, network technology, artificial intelligence technology etc., Robotics have 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: robot will the same huge numbers of families of entering with the PC before 30 years.The Korea S Samsung institute for economic research once predicted, to the year two thousand twenty, world robot market scale be to will reaching 1.4 trillion dollars, Seoul's Information and Communication Ministry even once drew up the surprising target that all there is a robot in the every family of the year two thousand twenty.For intelligent mobile robot, in order from environment, to walk and arrive the destination efficiently, and make it judge it self position, Here it is mobile robot's orientation problem according to some characteristics that oneself knows in the environment.Robot wants and can in circumstances not known, walk automatically, and the location is the most basic problem.

Domestic and international in recent years many researchists use multiple sensors, and mobile robot's self-align problem is furtherd investigate, 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.The sensor that is used to locate comprises: vision sensor, laser, infrared, ultrasonic, code-disc, gyroscope, accelerometer etc.Code-disc, gyroscope, accelerometer are the aiding sensors that is used for local positioning.Infrared, sonac is limit by precision, generally is used for promptly keeping away barrier.The laser sensor cost is higher, is not suitable for civilian popularization.The environmental information that vision sensor obtains is the abundantest, and development space is maximum.The researchist lays particular emphasis on the robotic vision Position Research at present.

Mobile robot's vision localization is generally divided natural landmark and two kinds of patterns of artificial road sign.Natural landmark is meant that utilizing in the environment original scene to serve as a mark positions navigation.Though natural landmark is not destroyed original environment, universality is good, and calculation of complex, robustness are not strong, poor practicability.Artificial road sign is meant that the specific road sign of artificial design is installed in the environment, though artificial road sign changes environment to some extent, calculating is simple, characteristic is stable, practical.Mostly the artificial road sign that present researchist is adopted is the pattern of design special color information or texture structure information, also comprises numeral, letter and two-dimensional bar code etc.These patterns generally all are to use scraps of paper printing and making, make the video camera photosensitive imaging through ambient light illumination, the influence that therefore changed by ambient lighting, thereby less stable.

Summary of the invention

The objective of the invention is provides a kind of mobile robot visual positioning system and method based on infrared road sign for avoiding above-mentioned existing in prior technology weak point, realize to 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 the indoor ceiling, and be 3 * 3 type dot matrixs, the world coordinates value of infrared road sign is confirmed in advance; The wide-angle thermal camera is fixed on one's body the mobile robot, is used to catch infrared image, and the confidential reference items of wide-angle imaging machine and distortion parameter are demarcated its initial homography matrix H in advance ₀Also confirmed in advance; Computer unit is used for receiving said infrared image from the wide-angle thermal camera, and infrared image is carried out distortion correction and image pretreatment operation, and comes the infrared road sign of detection and Identification according to the infrared image after handling, and utilizes initial homography matrix H then ₀, adopt expansion homography matrix localization method that the mobile robot is positioned calculating.

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 card of 3 * 3 types is invested on the ceiling, confirm the world coordinates value of all infrared road signs; The wide-angle thermal camera is fixed on one's body the robot, demarcates the confidential reference items and the distortion parameter of wide-angle imaging machine, calculate the initial homography matrix H of video camera ₀Confirm mobile robot's initial pose; Catch infrared image, go forward side by side line distortion correction and image pretreatment operation; Detect and discern infrared road sign; Utilize initial homography matrix H ₀, adopt expansion homography matrix localization method to position calculating.

Advantage of the present invention is following: the infrared road sign pattern making that the present invention proposes is simple, makes things convenient for 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 in a big way indoor place carry out the mobile robot fast, accurately, the location of robust.

Description of drawings

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

Fig. 2 is the infrared road sign synoptic diagram among the present invention;

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

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

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

Embodiment

For making the object of the invention, technical scheme and advantage clearer, below in conjunction with specific embodiment, and with reference to accompanying drawing, to further explain of the present invention.

Fig. 1 has provided the synoptic 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 the indoor ceiling, 3 * 3 types; Be fixed on robot wide-angle thermal camera 2 on one's body; The computer unit 3 that is used 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 the robot platform top, and to the infrared road sign of photographs, the computer unit 3 that is used for graphical analysis and location Calculation is placed in mobile robot platform inside.The mounting distance of infrared road sign 1 need combine the visual field size of video camera and video camera to consider from the high integrity of ceiling; The dead angle is as the criterion so that there is not the location in video camera; Be that video camera can photograph a road sign Anywhere at least in scene, on ceiling plane, post a plurality of infrared road signs 1.

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, because CCD is to the light sensitivity of the 850nm light wave light wave greater than 940nm, therefore selecting wavelength for use is the infrared-emitting diode of 850nm.The concrete Design Mode of infrared road sign 1 is as shown in Figure 2.Two kinds of point sets are arranged, and all the other points that peripheral area is bigger in the dot matrix of 3*3 at 4.Each road sign all has position identical four a little bigger, is used for confirming the subcoordinate system of road sign, promptly determines the initial point and the XY coordinate axis of road sign dot matrix, and in addition, these 4 are used to calculate homography matrix in location algorithm.The ID value that 5 remaining points are used to calculate road sign is to distinguish each road sign, and number of combinations is 32, promptly can represent 32 kinds of different road signs.Binary-coded mode is adopted in the calculating of ID value, has provided the scale-of-two code value of each point among 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, used video camera must have enough big field range, so infrared wide-angle video camera employing focal length is the big wide-angle lens of 2.8mm or 2.5mm.Camera lens the place ahead mounting center wavelength is the narrow bandpass filter plate of 850nm, thereby only the light wave of 850nm (road sign dot matrix infrared diode corresponding optical wavelength) is carried out 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 and the various CALCULATION OF PARAMETERS of localizing environment.

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

With reference to Fig. 3, off-line procedure specifically may further comprise the steps:

Step S301 accomplishes earlier camera calibration work, demarcates the confidential reference items and the distortion parameter of wide-angle imaging machine, and the mapping matrix of video camera confidential reference items and distortion correction is stored in respective document T1.

Step S302 makes 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 the point of crossing.

Step S303 is attached at ceiling plane with road sign, and video camera is installed in the mobile robot platform top.

After localizing environment is put up; Remaining work is exactly to set up the parameter that cartographic information and mensuration are used for location Calculation; Cartographic information mainly is that the subcoordinate of each road sign of record ties up to the position in the world coordinates; Calculate the world coordinates value of each road sign through step S304, and deposit respective document T2 in.The world coordinates value of each road sign confirms that method is following: owing on each road sign four anchor points are arranged all, can utilize the homography matrix relation that the position of road sign is confirmed one by one.The world coordinates data storage of road sign is 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 following:

Road sign ID: whether this road sign world coordinates is confirmed (is 1; Deny 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 select ID be 0 road sign as the initial point road sign, promptly with the subcoordinate system of No. 0 road sign as world coordinates.The world coordinates calculation process of other road sign is following: 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 confirm (judging through sign), then utilizes four anchor points of this road sign to calculate the homography matrix H of present image.Utilize H to calculate four anchor point coordinates that other do not confirm the road sign of world coordinates value, deposit data in document then.3. if the road sign in the visual field does not all have to confirm that then the mobile robot restarts the first step.4. after the road sign that can photograph has all been confirmed, 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 explained as follows: in the expansion homography matrix localization method based on the hypothesis of plane mechanism that the present invention adopts, be reference with the camera coordinate system, promptly homography matrix does not change, and road sign produces virtual displacement.The location Calculation of back is all carried out to initial homography matrix.Because all road signs are all unified under the world coordinate system, so four points that initial homography matrix can be chosen wantonly in the road sign calculate.

Definite method of homography matrix is following:

The projection relation that the position of each point and video camera 3 imaging planes constitute in the road sign is the projective rejection of plane to the plane, i.e. homography matrix transformation relation, and formulae express is suc as formula shown in 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 the following formula, H is a homography matrix, m _i(i=1,2 ..., 9) be the matrix element of homography matrix, s _iBe scale-up factor, (u _i, v _i) be the coordinate of road sign point in image, (x _i, y _i) be the world coordinates of road sign point.Formula (2) is expanded into 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)$

Divided by the 3rd equation, second equation is divided by the 3rd equation with first 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,HDA0000088989460000012.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 known 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), then we have 2n the linear equation about the homography matrix element, write out these equations with matrix form below:

$\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, make that equation coefficient is a matrix K, variable is M, is worth to be U, and then 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 calculates the initial position (x of video camera photocentre ₀, y ₀), and deposit document T4 in.The definite method explanation of video camera photocentre initial position is as follows: the video camera confidential reference items are demarcated in advance, and video camera is joined outward, and to be impact point be transformed into the transformed matrix of camera coordinate system from world coordinate system, comprises three rotating vectors and a translation vector.The use equation expression is:

$\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 ₃Be the rotating vector of outer three quadratures of joining of video camera, t is the translation vector of the outer ginseng of video camera.Select for use ID be 31 infrared road sign as the initial point road sign, under road sign, select any pose to calculate homography matrix H ₀, utilize on the road sign 9 pairs of coplanar points to find the solution 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 following:

$\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)

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

With reference to Fig. 4, comprise at line process: video camera 2 is caught the infrared image on the ceilings, 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 discerns 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 may further comprise the steps:

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

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

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

At step S404, calculate road sign ID value.

At step S405, utilize the initial position T3 of the road sign world coordinates value T2 that off-line records, 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 has only two kinds, and parallel move is that turning axle rotates with the vertical line on ground.The principle of expansion homography matrix localization method is: under the prerequisite of the hypothesis of plane mechanism; Utilize the relativity of motion; With being fixed on video camera on the robot platform, be converted into the rotation translation motion of road sign with respect to camera coordinate system with respect to the rotation translation motion on road sign plane; Through the constant position that calculates virtual road sign of hypothesis homography matrix, the pose of being extrapolated video camera by the pose difference between virtual road sign and the 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 be used formula (9) expression, and corresponding be that the motion pose relation of the gauge point of reference can be used formula (10) expression with the video camera.

In the formula (9), P _Cam, P ' _Cam, P " _CamBe the attained pose of video camera, in the formula (10), P _Mark, P ' _Mark, P " _MarkVirtual pose for road sign.

In conjunction with the parameter of confirming in the off-line procedure, positioning calculation process comprises:

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

In the 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 derive the formula of finding the solution through picture point coordinate solution point coordinate:

$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 _iBe homography matrix H ₀Element, (x _i, y _i) be the world coordinates of object point, (u _i, v _i) be image coordinate.When video camera is in new pose, detect the road sign in the image, promptly obtain image coordinate (u _i, v _i), combine H then ₀Utilize formula (11) to calculate the world coordinates (x of road sign _i, y _i), because the homography matrix that adopts in calculating is the homography matrix of video camera when initial position, so (the x that calculates _i, y _i) be the world coordinates of virtual road sign, three road sign points promptly can constitute virtual road sign pose P " _MarkFig. 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 having only 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 ₀The P that inverse comes 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 calculated two vectors

and

angle difference derived.Will

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

Be normalized to (cos (θ ₂), sin (θ ₂)), θ=θ is then arranged ₁-θ ₂

At last, need displacement calculating S.

Rotation angle θ and p have been calculated above " ₀P " ₁P " ₂, only need p " ₀P " ₁P " ₂With o ' is that the center is rotated counterclockwise θ and promptly obtains p ' ₀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 should equate on the theory; But, can obtain the displacement S of video camera through the equal mode of making even owing to there is the error of calculation.

$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)$

Through the calculating of above rotational angle theta and displacement S, promptly confirmed 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 Filtering Processing, can adopt kalman filter method to carry out.

The foregoing description provides is the basic structure scheme and the algorithm operating flow process of mobile robot positioning system based on infrared road sign proposed by the invention; Protection scope of the present invention is not limited thereto; Also comprise other schemes that adopt this organization plan and location algorithm and do slightly in the periphery to change, the structure or the mobile robot platform that change m*m (m is the integer more than or equal to 3) like the dot matrix road sign into change moving vehicle platform etc. into.

Above-described specific embodiment; The object of the invention, technical scheme and beneficial effect have been carried out further explain, and institute it should be understood that the above is merely specific embodiment of the present invention; Be not limited to the present invention; All within spirit of the present invention and principle, any modification of being made, be equal to replacement, improvement etc., all should be included within protection scope of the present invention.

Claims (12)

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

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

The wide-angle thermal camera is fixed on one's body the mobile robot, is used to catch infrared image, and the confidential reference items of wide-angle imaging machine and distortion parameter are demarcated its initial homography matrix H in advance ₀Also confirmed in advance;

Computer unit is used for receiving said infrared image from the wide-angle thermal camera, and infrared image is carried out distortion correction and image pretreatment operation, and comes the infrared road sign of detection and Identification according to the infrared image after handling, and utilizes initial homography matrix H then ₀, adopt expansion homography matrix localization method that the mobile robot is positioned calculating.

2. positioning system according to claim 1 is characterized in that, said 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 said dot matrix active infrared road sign is 3 * 3 types; Forms with all the other less points by periphery bigger 4,4 bigger detection and location Calculation that are used for road sign, all the other less points are used for the ID value calculating of road sign.

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

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

6. system according to claim 5; It is characterized in that; Said expansion homography matrix localization method is: through the constant position that calculates virtual road sign of hypothesis homography matrix; The pose of being extrapolated video camera by the pose difference between virtual road sign and the actual road sign changes, thereby reaches the purpose of localization for Mobile Robot.

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

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

The wide-angle thermal camera is fixed on one's body the robot, demarcates the confidential reference items and the distortion parameter of wide-angle imaging machine, calculate the initial homography matrix H of video camera ₀

Confirm mobile robot's initial pose;

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

Detect and discern infrared road sign;

Utilize initial homography matrix H ₀, adopt expansion homography matrix localization method to position calculating.

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

9. localization method according to claim 8; It is characterized in that; The dot matrix of said dot matrix active infrared road sign is made up of with all the other less points periphery bigger 4,4 bigger detection and location Calculation that are used for road sign, and all the other less points are used for the ID value calculating of road sign.

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

11. localization method according to claim 10 is characterized in that, said computer unit calculates the detection and Identification road sign through the identification and the ID value of a territory detection, road sign point clustering, road sign selection, four anchor points.

12. localization method according to claim 11; It is characterized in that; Said expansion homography matrix localization method is: through the constant position that calculates virtual road sign of hypothesis homography matrix; The pose of being extrapolated video camera by the pose difference between virtual road sign and the actual road sign changes, thereby reaches the purpose of localization for Mobile Robot.

Images