CN102168973B - Automatic navigating Z-shaft positioning method for omni-directional vision sensor and positioning system thereof - Google Patents

Abstract

The invention discloses an automatic navigating Z-shaft positioning method for an omni-directional vision sensor and a positioning system thereof. The method comprises the following steps: placing four colored identifications at the peripheral corners around an operation area, wherein a rectangle is formed by the four colored identifications; arranging the omni-directional vision sensor on an operation vehicle; acquiring an onsite omni-directional image containing the images of the four colored identifications by the omni-directional vision sensor; calculating the height H of a current omni-directional vision system according to a formula; and finally calculating the difference between the height H and a standard height Hs, namely a positioning coordinate value along the Z-shaft direction, thereby finishing the Z-shaft positioning for operation machinery. The device and method have the advantages that the positioning speed is high, the use scope is wide, the practicability is strong and the cost is competitive.

Description

Omnibearing vision sensor self-navigation Z axle localization method and positioning system thereof

Technical field

The present invention relates to a kind of omnibearing vision sensor self-navigation Z axle localization method and positioning system thereof.

Background technology

At present, people require increasingly high to the automaticity of machinery; The development of mechanical automation operation and robot thereof is following Mechanization Development direction; The location is concerning automatic job vehicle, robot, and is extremely important.Can the position of oneself must be known by vehicle, robot when automatic job, just can determine robot accomplish executing the task of next step on request.GPS is present unique commercialization and uses maximum positioning systems; But the precision of GPS depends on job position, and in city high rise building street or mountain area, GPS is because barriers such as house, the woods, mountain range hinder microwave transmission, and precision obviously descends; Especially GPS can not be in indoor use.Be similar to mammal, realize that through vision the location is one of present robot The Location focus, the vision sensor localization method mainly is divided into geometry location and topology location.Typical geometry location is in the 2D plane, to calculate through Flame Image Process identification marking position or images match is found out the absolute position of robot.The topology location is on the basis of setting up topological map, finds the abstract position of robot; Topology position application scope is wide, does not need manual work that sign is set, but generally sets up the topological map complicacy, and the image information calculated amount is big, and travelling speed is slower.Realize that through artificial sign is set the geometry location method is simple, in the robot location, be widely used.And implement how much absolute fixs through common vision sensor, generally draw the planar positioning result; GPS can obtain the positioning result of robot space three-dimensional.In the rough operating environment of reality, 3D location in space is necessary, can effectively judge the ground environment of robot; A kind of in recent years in addition in the RTK-GPS of the technical development of GPS technology, precision is higher, but costs an arm and a leg.Common vehicle and Work machine be agricultural machinery and agricultural transport vehicle especially, and operating environment place area is more abominable, chamber operation especially, and the GPS technology possibly not realize the self-navigation location.

Omnibearing vision sensor is a kind of novel vision sensor; It can provide the abundant information of 360 degree scopes, and image can not change with the rotation of sensor, helps reducing the part negative effect of wheelslip and vibrations; And low price, be widely used in the robot field; Patent of invention " a kind of autopilot and method " (application number: 200910044412.4) disclose the computing method of plane space two dimension, and will realize three location, also must accomplish the location of Z axle.

Summary of the invention

Technical matters to be solved by this invention is to propose a kind of omnibearing vision sensor self-navigation Z axle localization method and positioning system thereof; The location that this omnibearing vision sensor self-navigation Z axle localization method and positioning system thereof can realize the Z axle; Thereby on the basis of existing two-dimensional localization, accomplish the space three-dimensional location, this method and apparatus locating speed is fast, usable range is wide, practical, have a price advantage.

Technical solution of the present invention is following:

A kind of omnibearing vision sensor self-navigation Z axle localization method,

On the peripheral corner of operating area, place 4 coloured sign of forming rectangle: L ₁, L ₂, L ₃, L ₄, omnibearing vision sensor is arranged on the working truck; Omnibearing vision sensor obtains the on-the-spot omnidirectional images that includes 4 coloured sign imagings;

The relational expression of omnibearing vision sensor system height and omnibearing vision sensor and sign distance, image-forming range is:

$\frac{{(\frac{-2c{x}_i}{X_{0}f+x_{i}H}+c)}^2}{{b_1}^2}-\frac{{\left(\frac{{2\mathrm{cX}}_0{x}_i}{X_{0}f+x_{i}H}\right)}^2}{{a_1}^2}=1,$ X _o，x _i＞0；

A wherein ₁, b ₁, c is the structural parameters on hyperboloid surface; F is a focal length; H is the computed altitude of current fully-directional visual system;

X ₀For omnibearing vision sensor on XOY plane arrives spatial point P (X ₀, Z ₀) between distance, X ₀Be spatial point P (X ₀, Z ₀) in subpoint on the XOY plane and the distance of omnibearing vision sensor between the subpoint on the XOY plane;

x _iBe image-forming range, i.e. 4 coloured imaging actual range mean values that are identified to the omnibearing vision sensor projection centre; x _iBy calculating the imaging pixel distance that is identified to the omnibearing vision sensor projection centre respectively, be calculated to be the picture actual range through demarcating pixel distance again;

Calculate the computed altitude H of current fully-directional visual system; Last computed altitude H and calibrated altitude H _sDifference, i.e. the elements of a fix value of Z-direction, thereby the Z axle location of the machinery that fulfils assignment.

The position at four coloured sign places is corner location of the boundary rectangle of operating area.

The position of omnibearing vision sensor projection centre in omnidirectional images fixed.

x _iAsk method following:

The demarcation pixel distance is 0.01389inch/pixel, calculates 4 signs earlier and divides the imaging actual range be clipped to the omnibearing vision sensor projection centre, and the mean value of hoping for success again as actual range obtains x _i

X ₀Ask method following:

According to 4 coloured 4 location points and the location points of omnibearing vision sensor projection centre in image that are identified in the image, calculate every adjacent two coloured signs and the azimuth angle theta of omnibearing vision sensor projection centre in omnidirectional images in four coloured signs ₁～θ ₄

The rectangle that 4 coloured sign L1～L4 form around in the operation area long for a, wide be b; Form principle according to the geometry circular arc, by L1 and coloured identification point of L2 and above-mentioned azimuth angle theta ₁Be that central angle forms arc S ₁, in like manner, L2 and L3, θ ₂Form arc S ₂, L3 and L4, θ ₃Form arc S ₃, L3 and L4, θ ₄Form arc S ₄, arc S ₁～S ₄Two intersect respectively, draw four intersection points, and 4 intersecting point coordinates are:

$\left\{\begin{array}{c}{\mathrm{Tan}\mathrm{\&theta;}}_1={\mathrm{<figure-callout\; id="1"\; label="BX\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">BX</figure-callout>}}_I/({{\mathrm{<figure-callout\; id="1"\; label="X\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_I}^2+{{\mathrm{<figure-callout\; id="1"\; label="Y\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I}^2-{\mathrm{BY}}_{I1})\\ {\mathrm{Tan}\mathrm{\&theta;}}_2={\mathrm{AY}}_{\mathrm{<figure-callout\; id="1"\; label="I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">I</figure-callout>}1}/({{\mathrm{<figure-callout\; id="1"\; label="X\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_I}^2+{{\mathrm{<figure-callout\; id="1"\; label="Y\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I}^2-{\mathrm{AX}}_{I1})\end{array}\right.;$ Wherein ${\mathrm{<figure-callout\; id="1"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}_1=\frac{b/{{\mathrm{Tan}\mathrm{\&theta;}}_1}^{-a}}{a/{{\mathrm{Tan}\mathrm{\&theta;}}_2}^{-b}};$

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="2"\; label="X\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_I=\frac{b({\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}_2+1/{\mathrm{Tan}\mathrm{\&theta;}}_2)}{1+{{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}_2}^2}\\ {\mathrm{<figure-callout\; id="2"\; label="Y\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I=\frac{{\mathrm{Bc}}_2({\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}_2+1/{\mathrm{Tan}\mathrm{\&theta;}}_2)}{1+{{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}_2}^2}\end{array}\right.,$ Wherein ${\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}_2=\frac{b/{{\mathrm{Tan}\mathrm{\&theta;}}_2}^{-a}}{a/{{\mathrm{Tan}\mathrm{\&theta;}}_3}^{-b}};$

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="3"\; label="X\; I"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">X</figure-callout>}}_I=\frac{b({\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_3+1/{\mathrm{Tan}\mathrm{\&theta;}}_3)}{1+{{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_3}^2}\\ {\mathrm{<figure-callout\; id="3"\; label="Y\; I"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I=\frac{{\mathrm{Bc}}_3({\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_3+1/{\mathrm{Tan}\mathrm{\&theta;}}_3)}{1+{{\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_3}^2}\end{array}\right.,$ Wherein ${\mathrm{<figure-callout\; id="3"\; label="c"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">c</figure-callout>}}_3=\frac{b/{{\mathrm{Tan}\mathrm{\&theta;}}_3}^{-a}}{a/{{\mathrm{Tan}\mathrm{\&theta;}}_4}^{-b}};$

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="4"\; label="X\; I"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">X</figure-callout>}}_I=\frac{b({\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">c</figure-callout>}}_4+1/{\mathrm{Tan}\mathrm{\&theta;}}_4)}{1+{{\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">c</figure-callout>}}_4}^2}\\ {\mathrm{<figure-callout\; id="4"\; label="Y\; I"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I=\frac{{\mathrm{Bc}}_4({\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">c</figure-callout>}}_4+1/{\mathrm{Tan}\mathrm{\&theta;}}_4)}{1+{{\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">c</figure-callout>}}_4}^2}\end{array}\right.,$ Wherein ${\mathrm{<figure-callout\; id="4"\; label="c"\; filenames="HDA0000043604660000011.png,HDA0000043604660000031.png"\; state="\{\{state\}\}">c</figure-callout>}}_4=\frac{b/{{\mathrm{Tan}\mathrm{\&theta;}}_4}^{-a}}{a/{{\mathrm{Tan}\mathrm{\&theta;}}_1}^{-b}};$

Draw intersection point barycentric coordinates P (X ₁, Y ₁), as follows:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="X"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_1=(X_{I1}+X_{I2}+X_{I3}+X_{I4})/4\\ {\mathrm{<figure-callout\; id="1"\; label="Y"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_1=(Y_{I1}+Y_{I2}+Y_{I3}+Y_{I4})/4\end{array}\right.;$

Barycentric coordinates P (X ₁, Y ₁) be the planar location of omnibearing vision sensor;

Calculate P (X at XOY plane according to 2 common range formulas ₁, Y ₁) o'clock to 4 sign L1, L2, L3 and L4 apart from l1, l2, l3 and l4; Obtain at last

A kind of omnibearing vision sensor self-navigation Z axle positioning system is placed 4 coloured sign of forming rectangle: L on the peripheral corner of operating area ₁, L ₂, L ₃, L ₄, the omnibearing vision sensor that is used to the omnidirectional images that includes 4 coloured signs imagings at the scene of obtaining is arranged on working truck; On working truck, be provided with microprocessor; Have the image that is used for obtaining in the microprocessor and carry out the graphics processing unit that data processing is obtained coloured home position coordinate, also have the computing unit that is used for according to the elements of a fix value of aforementioned omnibearing vision sensor self-navigation Z axle localization method and final acquisition Z-direction in the microprocessor according to omnibearing vision sensor.

Omnibearing vision sensor has hyperbolic mirror.

Beneficial effect:

A kind of omnibearing vision sensor self-navigation Z axle localization method of the present invention and positioning system thereof, the operation area forms rectangular-shaped with 4 signs, measure the length and the width of rectangle; Extract through Flame Image Process and to be identified at position in the image, further obtain in image, 4 orientation angles that each sign and sensor projection centre form; Obtain the two-dimensional localization absolute coordinate (X of the space plane XY of relative 4 the sign formation of Work machine through geometric algorithm; Y), adopt cartesian coordinate system, with one in 4 signs as initial point; In reality, be designated initial point with that of the lower left corner in the image.

(application number: 200910044412.4) " a kind of automatic navigating and positioning device and method " Z axle method for calculating and locating of conforming to comprises the relational expression of derivation device system height and omnibearing vision sensor and identifier space distance, image-forming range to the invention provides a kind of and patent of invention; Calculating is identified to the image-forming range of omnibearing vision sensor projection centre; Calculate the space length of omnibearing vision sensor and sign; Calculate Z axle locator value, it is characterized in that:

The relational expression of said derivation device system height and omnibearing vision sensor and identifier space distance, image-forming range comprises: omnibearing vision sensor hyperbolic mirror Hyperbolic Equation is set up, the omnibearing vision sensor image-forming principle is used and the utilization of mathematics geometry; Said omnibearing vision sensor hyperbolic mirror Hyperbolic Equation is set up and is comprised the Hyperbolic Equation that is transformed to the apparatus system height correlation by the plane Hyperbolic Equation; Said omnibearing vision sensor image-forming principle utilization comprises that the spatial point incident ray reflects in the imaging surface image-forming principle through hyperboloid, is characterized in intersecting at projection centre in a bit after incident ray is through the hyperboloid refraction; Space line is imaged as circular arc on imaging surface; The utilization of said mathematics geometry comprises that spatial point omnibearing vision sensor image-forming principle several picture representes that the geometry mathematical relation utilization of intermediate cam shape derives apparatus system height and sensor and identify the relational expression apart from, image-forming range;

The image-forming range that said calculating is identified to the omnibearing vision sensor projection centre comprises that (application number: 200910044412.4) sign of a kind of automatic navigating and positioning device and method is extracted according to patent of invention; The center of gravity representative of extracting identification characteristics and obtaining the identification characteristics pixel through Flame Image Process is identified at the position in the image; Calculating is identified to the imaging pixel distance of omnibearing vision sensor projection centre, goes out to be identified to the imaging actual range of omnibearing vision sensor projection centre through the pixel distance calibrated and calculated; Said pixel distance is demarcated the relation of demarcating actual range and pixel count through experimental result, draws the actual range that single pixel is demarcated, and it is actual range that pixel distance multiply by single pixel demarcation distance.

The space length of said calculating omnibearing vision sensor and sign comprises that (application number: 200910044412.4) a kind of automatic navigating and positioning device and method are obtained (X, Y) coordinate figure on the XY plane according to patent of invention; The sensor plane position utilization air line distance accounting equation of being asked calculates the space length of omnibearing vision sensor and sign;

Said calculating Z axle locator value comprises that apparatus system height that utilization obtains and omnibearing vision sensor and identifier space are apart from the relational expression of, image-forming range be identified to the space length three of image-forming range, omnibearing vision sensor and the sign of omnibearing vision sensor projection centre, calculating Z axle locator value.

In sum, a kind of omnibearing vision sensor self-navigation Z axle method for calculating and locating according to the invention provides a kind of and patent of invention (application number: 200910044412.4) the Z axle method for calculating and locating that conforms to method of a kind of automatic navigating and positioning device; As long as provide a pair to comprise the realtime graphic of sign; Extract identification characteristics and by patent of invention (application number: 200910044412.4) the planar locator value obtained of a kind of automatic navigating and positioning device and method through Flame Image Process; Completion space three-dimensional location; Reach the positioning function same, really realize automated machine or robot self-navigation location with GPS.This method is simple, and travelling speed is fast, and is practical; Automatic navigating and positioning device is simple in structure, and cost is not high, is easy to be extended and applied.

Description of drawings

Fig. 1 omnibearing vision sensor point image-forming principle of the present invention;

Fig. 2 is the mutual relationship principle schematic of system height of the present invention and omnibearing vision sensor and identifier space distance, image-forming range;

Fig. 3 instance omnibearing vision sensor of the present invention calibrated altitude H _sSynoptic diagram;

The process flow diagram of Fig. 4 the inventive method;

The space length of Fig. 5 instance omnibearing vision sensor of the present invention and sign calculates principle schematic.

Label declaration: 1-omnibearing vision sensor, 2-Work machine, 3-imaging plane, 4-hyperboloid.

Embodiment

Below will combine accompanying drawing and specific embodiment that the present invention is explained further details:

Embodiment 1:

Like Fig. 1, during the omnibearing vision sensor imaging, spatial point P (X, Y; Z), the picture point p that forms (x, y) vertical with surface level (XOY) with the plane of projection centre formation, spatial point P (X; Y, the straight line that Z) is linked to be to true origin is α 1 in the projection straight line of XOY plane with respect to the position angle of X axle, picture point p (x; Y) and the line of imaging plane initial point be α 2 with respect to the position angle of imaging plane x axle, α 1 is identical with α 2, is all the θ among Fig. 1.

Like Fig. 2, definition coordinate system XZ; According to the omnibearing vision sensor image-forming principle, space ground point P (X ₀, Z ₀), incident ray i is through focus O _M(0, H) on hyperboloid, put M (x _m, z _m) converge at O after the reflection _c, on imaging surface, form picture point p (x _i, z _i); O _cBe projection centre, f is a focal length; According to general Hyperbolic Equation:

The equation according to the invention (1) of deriving:

$\frac{{(z_m-H+c)}^2}{b^2}-\frac{{x_m}^2}{a^2}=1;$ $c=\sqrt{a^2+b^2}---\left(1\right)$

A, b, c are the structural parameters on hyperboloid surface, and general producer provides standard value; H is an omnibearing vision sensor system height overhead; Can know that from the omnibearing vision sensor image-forming principle spatial point and imaging point xsect can be represented with coordinate system as shown in Figure 2.The definition omnibearing vision sensor is to P (X ₀, Z ₀) between distance be omnibearing vision sensor and (explain: Fig. 2 is a derivation of equation synoptic diagram to sign, has only a sign respectively; If there are four signs the back, then obtain mean value with four sign distances.--the mean value of-4 tag distance refers to 4 mean values that indicate the distance value of omnibearing vision sensor) distance X of projection on XOY plane ₀Be defined as picture point p (x _i, z _i) be image-forming range x to the xoy plan range at omnibearing vision sensor imaging surface center _i, as shown in Figure 2.

Point O _M(0, H), M (x _m, z _m) and P (X _o, Z _o) on same straight line, can be write as (2):

$z_m=H-\frac{H}{X_0}{x}_m---\left(2\right)$

According to how much leg-of-mutton similaritys of mathematics, draw (3):

$\frac{x_i}{x_m}=\frac{f}{z_m-H+2c}---\left(3\right)$

Obtain (4) according to (2) and (3):

$x_m=\frac{{2\mathrm{cX}}_0{x}_i}{X_{0}f+x_{i}H}---\left(4\right)$

In (4) formula substitution (3), can obtain Zm:

$z_m=(1-\frac{{2\mathrm{cx}}_i}{X_{0}f+x_{i}H})H---\left(5\right)$

In (4) formula and (5) substitution (1), obtain the relational expression of following omnibearing vision sensor system height and omnibearing vision sensor and sign distance, image-forming range:

$\frac{{\left(\frac{{-2\mathrm{cx}}_i}{X_{0}f+x_{i}H}\right)}^2}{b^2}-\frac{{\left(\frac{{2\mathrm{cX}}_0{x}_i}{X_{0}f+x_{i}H}\right)}^2}{a^2}=1,(X_o,x_i>0)---\left(6\right)$

Therefore, according to (6) formula, a, b, c and f are definite value, if can draw x _iAnd X ₀Value substitution (6) formula, can instead ask the computed altitude H of fully-directional visual system that obtains this moment.When omnibearing vision sensor was installed, terrain clearance was the constant of fixing, and on Fig. 3 coordinate system Z-direction, is defined as calibrated altitude H _sObtain the difference of computed altitude and calibrated altitude, i.e. the elements of a fix value of Z-direction, the Z axle location of the machinery that fulfils assignment.

Definition capitalization such as XYZ representation space coordinate amount, lowercase such as xyz represent the amount relevant with imaging surface.

Next step explains x _iAnd X ₀Value computation process:

Like Fig. 4, the said position calculating method that is identified in the image of present embodiment comprises artificial sign is set, and opens that sign power supply, view data input, Flame Image Process calculate, the result deposits.Specifically describe as follows:

Sign is set: employing red colouration glass passes through, and bright material is made the inner manual work sign that the power supply light fixture is installed, and centers on 4 red signs of placing on the maximum peripheral corner of operating area: sign L ₁, the sign L ₂, the sign L ₃, the sign L ₄And the composition rectangle, and turn on the power switch enhance color brightness.

View data input: take sign L in real time through the omnibearing vision sensor camera head ₁, the sign L ₂, the sign L ₃, the sign L ₄Comprehensive 360 degree images, and be input in the reservoir in the computing machine through real-time USB interface.

Flame Image Process and calculating: transfer the omnidirectional images that stores in the computing machine, the definition image coordinate, the image upper left corner is initial point (0,0), unit is a pixel; In the Flame Image Process, can carry out Flame Image Process to the redness sign according to the sign color.

Utilize the red brightness of 7 pairs of image pixels of formula to calculate, and program loop calculates, and maximal value is r in the red color intensity of image pixel _Max, be stored in the internal memory;

According to formula 8, in computing machine, set the threshold value t that extracts red identification characteristics amount of color ₁Line by line image is scanned, the brightness of red pixel is greater than t in utilizing formula 7 computed image ₁The time, extract from image and to meet all red character pixels of above condition;

r＝R-(B+G)/2-|B-G| ----7

t ₁＝[r _max-N ₁] ----8

R, G, B: red, green and blue brightness; R: extract red pixel brightness; r _Max: the red color intensity maximal value of pixel in the image; t ₁: the threshold value of extracting red identification characteristics amount of color; N ₁Be testing constant, 10＜N ₁＜50;

Maximum red pixel distance is provided with red pixel apart from maximal value in the image according to being identified at arbitrarily; Calculating extracts Euclidean distance between red pixel; When Euclidean distance between red pixel is judged as a red feature pixel zone less than setting red all pixels during apart from maximal value, be divided into 4 feature pixel zones;

The calculating of single characteristic area pixel; Extract the horizontal ordinate and the ordinate of characteristic area pixel, the image coordinate of i pixel is defined as: Rix and Riy; And the pixel total amount that counts this feature pixel zone is n ₁

Utilize formula 9 to calculate the coordinate mean value of these all pixels of feature pixel zone, promptly obtain the center of gravity in feature pixel zone, the horizontal ordinate of center of gravity and ordinate are defined as Xr and Yr respectively; Coordinate (Xr, Yr) corresponding redness is identified at the location point of representing in the image;

$[\mathrm{Xr},\mathrm{Yr}]=[\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{Rix}/{\mathrm{<figure-callout\; id="1"\; label="n"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">n</figure-callout>}}_1,\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}\mathrm{Riy}/n_1]---9$

In the formula: Rix, Riy: the image coordinate of the i of extraction red identification characteristics pixel;

When the image recognition sign is not equal to 4, then return the image input; Draw every two adjacent images identification point and the orientation angles θ of omnibearing vision sensor projection centre in omnidirectional images through Flame Image Process ₁～θ ₄(in reality, the projection centre of omnibearing vision sensor is under the constant situation of sensor parameters, and the position in image is unique fixing, artificial input data in program)

According to drawing the orientation angles θ of sign with sensor ₁～θ ₄After, like Fig. 5, (application number: the 200910044412.4-publication number is 101660912 according to patent of invention.) a kind of automatic navigating and positioning device and method, sign (rectangle that L1～L4) forms long for a, wide be b; By L1 and coloured identification point of L2 and above-mentioned diagonal angle θ ₁Form arc S ₁, L2 and L3 successively, θ ₂Form arc S ₂, L3 and L4, θ ₃Form arc S ₃, L3 and L4, θ ₄Form arc S ₄, arc S ₁～S ₄Two intersect respectively, draw four intersection points, and intersection point can draw formula (10) through geometric analysis:

$\left\{\begin{array}{c}{\tan \mathrm{\&theta;}}_1={\mathrm{bX}}_{I1}/({X_{I1}}^2+{{\mathrm{<figure-callout\; id="1"\; label="Y\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I}^2-{\mathrm{bY}}_{I1})\\ {\tan \mathrm{\&theta;}}_2={\mathrm{aY}}_{I1}/({X_{I1}}^2+{{\mathrm{<figure-callout\; id="1"\; label="Y\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I}^2-{\mathrm{aX}}_{I1})\end{array}\right.---10$

Formula 1 is carried out conversion, obtains:

$\left\{\begin{array}{c}{X}_{I1}=\frac{b(c+1/{\tan \mathrm{\&theta;}}_1)}{1+c^2}\\ Y_{I1}=\frac{\mathrm{bc}(c+1/{\tan \mathrm{\&theta;}}_1)}{1+c^2}\end{array}\right.---11$

Wherein $c=\frac{b/\mathrm{Tan}{{\mathrm{\&theta;}}_1}^{-a}}{a/{{\mathrm{Tan}\mathrm{\&theta;}}_2}^{-b}}.$

In like manner obtain the coordinate (X of other three intersection points _I2, Y _I2), (X _I3, Y _I3), (X _I4, Y _I4), as follows:

$\left\{\begin{array}{c}{X}_{I2}=\frac{b(c+1/{\tan \mathrm{\&theta;}}_2)}{1+c^2}\\ Y_{I2}=\frac{\mathrm{bc}(c+1/{\tan \mathrm{\&theta;}}_2)}{1+{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}^2}\end{array}\right.,$ $c=\frac{b/\tan {{\mathrm{\&theta;}}_2}^{-a}}{a/{{\tan \mathrm{\&theta;}}_3}^{-b}}$

$\left\{\begin{array}{c}{X}_{I3}=\frac{b(c+1/{\tan \mathrm{\&theta;}}_3)}{1+c^2}\\ Y_{I3}=\frac{\mathrm{bc}(c+1/{\tan \mathrm{\&theta;}}_3)}{1+{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}^2}\end{array}\right.,$ $c=\frac{b/\tan {{\mathrm{\&theta;}}_3}^{-a}}{a/{{\tan \mathrm{\&theta;}}_4}^{-b}}$

$\left\{\begin{array}{c}{X}_{I4}=\frac{b(c+1/{\tan \mathrm{\&theta;}}_4)}{1+c^2}\\ Y_{I4}=\frac{\mathrm{bc}(c+1/{\tan \mathrm{\&theta;}}_4)}{1+{\mathrm{<figure-callout\; id="2"\; label="c"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">c</figure-callout>}}^2}\end{array}\right.,$ $c=\frac{b/\tan {{\mathrm{\&theta;}}_4}^{-a}}{a/{{\tan \mathrm{\&theta;}}_1}^{-b}}$

Draw intersection point barycentric coordinates P (X ₁, Y ₁), as follows:

$\left\{\begin{array}{c}{\mathrm{<figure-callout\; id="1"\; label="X"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_1=(X_{I1}+{\mathrm{<figure-callout\; id="2"\; label="X\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">X</figure-callout>}}_I+{\mathrm{<figure-callout\; id="3"\; label="X\; I"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">X</figure-callout>}}_I+X_{I4})/4\\ {\mathrm{<figure-callout\; id="1"\; label="Y"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_1=(Y_{I1}+{\mathrm{<figure-callout\; id="2"\; label="Y\; I"\; filenames="HDA0000043604660000021.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I+{\mathrm{<figure-callout\; id="3"\; label="Y\; I"\; filenames="HDA0000043604660000011.png"\; state="\{\{state\}\}">Y</figure-callout>}}_I+Y_{I4})/4\end{array}\right.---12$

Barycentric coordinates P (X ₁, Y ₁) be the planar location of omnibearing vision sensor.

Calculate the planar location P (X with respect to sign of camera ₁, Y ₁) after, calculate P (X at XOY plane according to 2 common range formulas ₁, Y ₁) o'clock to 4 sign L1, L2, L3 and L4 apart from l1, l2, l3 and l4.The coordinate of sign is confirmed according to being provided with when identifying.

$X_0=\frac{11+12+13+14}{4}.$

Then, the center of gravity representative of obtaining the identification characteristics pixel according to 9 formulas is identified at the position in the image, calculates the imaging pixel distance (in reality, the sensor projection centre is a steady state value, artificial input) that is identified to the omnibearing vision sensor projection centre.The plain distance of instance acceptance of the bid fixation of the present invention calculates the imaging actual range mean value x that is identified to the omnibearing vision sensor projection centre for 0.01389inch/pixel _i

Draw xi and X0, use 6 formulas to calculate the actual computation value of the omnibearing vision sensor of Work machines such as robot and vehicle, obtain the difference of computed altitude and calibrated altitude, be i.e. the elements of a fix value of Z-direction, the Z axle location of the machinery that fulfils assignment.In conjunction with patent of invention (application number: 200910044412.4) a kind of automatic navigating and positioning device and method, three-dimensional (X, Y, Z) location that both can accomplish the omnibearing vision sensor self-navigation of Work machines such as robot and vehicle.

Claims (2)

1. an omnibearing vision sensor self-navigation Z axle localization method is characterized in that,

On the peripheral corner of operating area, place 4 coloured sign of forming rectangle: L ₁, L ₂, L ₃, L ₄, omnibearing vision sensor is arranged on the working truck; Omnibearing vision sensor obtains the on-the-spot omnidirectional images that includes 4 coloured sign imagings; Omnibearing vision sensor has hyperbolic mirror;

Distance between the computed altitude of fully-directional visual system and omnibearing vision sensor and spatial point, the relational expression of image-forming range are:

$\frac{{(\frac{-2c{x}_i}{X_{0}f+x_{i}H}+c)}^2}{{b_1}^2}-\frac{{\left(\frac{2c{X}_0{x}_i}{X_{0}f+x_{i}H}\right)}^2}{{a_1}^2}=1,$ X _o，x _i＞0；

A wherein ₁, b ₁, c is the structural parameters on hyperboloid surface; F is a focal length; H is the computed altitude of current fully-directional visual system;

X ₀For omnibearing vision sensor on XOY plane arrives spatial point P (X ₀, Z ₀) between distance, be specially spatial point P (X ₀, Z ₀) in subpoint on the XOY plane and the distance of omnibearing vision sensor between the subpoint on the XOY plane;

x _iBe image-forming range, i.e. 4 coloured imaging actual range mean values that are identified to the omnibearing vision sensor projection centre; x _iBy calculating the imaging pixel distance that is identified to the omnibearing vision sensor projection centre respectively, be calculated to be the picture actual range through demarcating pixel distance again;

Calculate the computed altitude H of current fully-directional visual system; Last computed altitude H and calibrated altitude H _sDifference, i.e. the elements of a fix value of Z-direction, thereby the Z axle location of the machinery that fulfils assignment; Calibrated altitude H _sTerrain clearance when being the omnibearing vision sensor installation.

2. omnibearing vision sensor self-navigation Z axle localization method according to claim 1 is characterized in that,

x _iAsk method following:

The demarcation pixel distance is 0.01389inch/pixel, calculates 4 signs earlier and divides the imaging actual range be clipped to the omnibearing vision sensor projection centre, and the mean value of hoping for success again as actual range obtains x _i

X ₀Ask method following:

According to 4 coloured 4 location points and the location points of omnibearing vision sensor projection centre in image that are identified in the image, calculate every adjacent two coloured signs and the azimuth angle theta of omnibearing vision sensor projection centre in omnidirectional images in four coloured signs ₁～θ ₄

The rectangle that 4 coloured sign L1～L4 form around in the operation area long for a, wide be b; Form principle according to the geometry circular arc, by L1 and coloured identification point of L2 and above-mentioned azimuth angle theta ₁Be that central angle forms arc S ₁, in like manner, L2 and L3, θ ₂Form arc S ₂, L3 and L4, θ ₃Form arc S ₃, L3 and L4, θ ₄Form arc S ₄, arc S ₁～S ₄Successively according to S ₁With S ₂, S ₂With S ₃, S ₃With S ₄And S ₄With S ₁Order intersect in twos, draw four intersection points, 4 intersecting point coordinates are:

$\left\{\begin{array}{c}{X}_{I1}=\frac{b(c_1+1/\mathrm{Tan}{\mathrm{\&theta;}}_1)}{1+{c_1}^2}\\ Y_{I1}=\frac{b{c}_1(c_1+1/\mathrm{Tan}{\mathrm{\&theta;}}_1)}{1+{c_1}^2}\end{array}\right.;$ Wherein $c_1=\frac{b/\mathrm{Tan}{\mathrm{\&theta;}}_1-a}{a/\mathrm{Tan}{\mathrm{\&theta;}}_2-b};$

$\left\{\begin{array}{c}{X}_{I2}=\frac{b(c_2+1/\mathrm{Tan}{\mathrm{\&theta;}}_2)}{1+{c_2}^2}\\ Y_{I2}=\frac{b{c}_2(c_2+1/\mathrm{Tan}{\mathrm{\&theta;}}_2)}{1+{c_2}^2}\end{array}\right.,$ Wherein $c_2=\frac{b/\mathrm{Tan}{\mathrm{\&theta;}}_2-a}{a/\mathrm{Tan}{\mathrm{\&theta;}}_3-b};$

$\left\{\begin{array}{c}{X}_{I3}=\frac{b(c_3+1/\mathrm{Tan}{\mathrm{\&theta;}}_3)}{1+{c_3}^2}\\ Y_{I3}=\frac{b{c}_3(c_3+1/\mathrm{Tan}{\mathrm{\&theta;}}_3)}{1+{c_3}^2}\end{array}\right.,$ Wherein $c_3=\frac{b/\mathrm{Tan}{\mathrm{\&theta;}}_3-a}{a/\mathrm{Tan}{\mathrm{\&theta;}}_4-b};$

$\left\{\begin{array}{c}{X}_{I4}=\frac{b(c_4+1/\mathrm{Tan}{\mathrm{\&theta;}}_4)}{1+{c_4}^2}\\ Y_{I4}=\frac{b{c}_4(c_4+1/\mathrm{Tan}{\mathrm{\&theta;}}_4)}{1+{c_4}^2}\end{array}\right.,$ Wherein $c_4=\frac{b/\mathrm{Tan}{\mathrm{\&theta;}}_4-a}{a/\mathrm{Tan}{\mathrm{\&theta;}}_1-b};$

Draw intersection point barycentric coordinates P (X ₁, Y ₁), as follows:

$\left\{\begin{array}{c}{X}_1=(X_{I1}+X_{I2}+X_{I3}+X_{I4})/4\\ Y_1=(Y_{I1}+Y_{I2}+Y_{I3}+Y_{I4})/4\end{array}\right.;$

Barycentric coordinates P (X ₁, Y ₁) be the planar location of omnibearing vision sensor;

Calculate P (X at XOY plane according to 2 common range formulas ₁, Y ₁) o'clock to 4 sign L1, L2, L3 and L4 apart from l1, l2, l3 and l4; Obtain at last

Images