CN105243366A - Two-dimensional code based vehicle positioning method - Google Patents

Abstract

The present invention discloses a two-dimensional code based vehicle positioning method. According to the method, a location of a vehicle is calculated by depending on two-dimensional code information read from the ground. The vehicle comprises a light source and a visual camera, the camera can be placed at the rear or in the front of the vehicle and the camera faces the ground at a certain angle. Two-dimensional codes are distributed on the floor that the camera faces; when the vehicle operates, at least one two-dimensional code is ensured to be in a shooting area of the camera at any time; and each two-dimensional code corresponds to a unique ID, so that the location of the vehicle in the specific environment can be calculated in real time by an image processing technology, thereby providing location information for navigation of the vehicle.

Description

A kind of vehicle positioning method based on Quick Response Code

Technical field

The present invention relates to the navigation locating method of moving vehicle, particularly relate to the vehicle being furnished with vision positioning system, by identifying that ground Quick Response Code determines the localization method of the positional information of vehicle.

Background technology

The automatic driving vehicle in the present age, all can be equipped with various sensor, identify that the navigation that the information of geographical environment is vehicle provides foundation by these sensors.These sensors comprise: the displacement transducer of sonac, physical contact, laser sensor, and the vision sensor that can gather 2D and 3D.

But method of the prior art also exists or expensive or arithmetic speed is slow or the shortcoming such as low precision, therefore, this area needs that a kind of cost is low, processing speed fast and the method that can meet the demands.

Summary of the invention

The object of the present invention is to provide a kind of vehicle positioning method based on Quick Response Code, mainly rely on the position calculating vehicle place from the feature of surface readout 2 D code information, cost is low, and processing speed is fast.

The technical solution used in the present invention is as follows.

First, from camera reading images information, then pre-service is carried out, as color conversion, image enhaucament, image scaling to the image obtained; Then the Quick Response Code in detected image, the image coordinate information of angle point is obtained from the Quick Response Code image, calculate the distance of camera to Quick Response Code angle point, and identify the ID of special Quick Response Code, obtain the position of vehicle under this map environment and attitude according to the geographical relationship of Quick Response Code ID and place map.

Wherein, when the coordinate determining vehicle, vision system must perceive the existence of at least one Quick Response Code.

Wherein, the Quick Response Code ID in place is ID unique in this place, and is associated with place map; Be supplied to vehicle on-board vision system, thus confirm the locating information of vehicle.

The present invention compared with prior art tool has the following advantages.

1, simple: to use simple method, giving the geography information of the vision system of car and the geometric relationship of map. the automatic identification technology developed rapidly at present---Quick Response Code is as one of the information recording medium of this technology lowermost layer, carry the advantage (low cost of himself, high storage density, hypervelocity identification, stronger error correcting capability), play an important role in automatic identification field, the present invention utilizes the simple, convenient of Quick Response Code and the feature that discrimination is high, identifies the positional information of vehicle.

2, processing procedure fast: method of the present invention carries out pre-service by vision camera to the image obtained, as color conversion, image enhaucament, image scaling, then the Quick Response Code in detected image, the image coordinate information of angle point is obtained from the Quick Response Code image, from the Quick Response Code coordinate file of place, read the volume coordinate of angle point according to Quick Response Code ID, estimate position and the attitude of camera according to the image coordinate of angle point and volume coordinate.Because carried out convergent-divergent, color conversion to image, and only need extract the angle point information of Quick Response Code, so to the calculated amount reduced in image processing process, achieve processing speed fast.

3, feature extraction: according to feature and the frame design of special Quick Response Code, frame angular coordinate can be utilized, calculate locating information; By the decoding of Quick Response Code, obtain unique ID.

4, high accuracy three-dimensional information: the three-dimensional system of coordinate initial point of vision sensor can reach a centimetre rank to X, Y, Z range measurement accuracy of the three-dimensional system of coordinate initial point of Quick Response Code.

5, robustness: for vision system, the change of illumination condition and the uneven of luminance brightness will reduce accuracy of identification, even can cause recognition failures.The present invention starts with from increase secondary light source and algorithm itself, improves the robustness of system, can meet the requirement of general scene.

7, ambient As is simple: little to the environment change amount of vehicle operating.

8, favorable expandability: the quantity that can increase vision camera according to different needs, and again need not write code, only need revise the parameter of part wherein, meet the demand of many orders identification

Accompanying drawing explanation

Fig. 1 is algorithm flow chart of the present invention.

Fig. 2 be mark in camera and 3 dimension spaces between transformation relation schematic diagram.

Fig. 3 is a kind of embodiment of Quick Response Code used in the present invention.

Embodiment

Shown in accompanying drawing 1 is the process flow diagram of method of the present invention.According to method of the present invention, in the process of vehicle location, first from camera reading images information, then pre-service is carried out, as color conversion, image enhaucament, image scaling to the image obtained.Subsequently, Quick Response Code (MARKER) in detected image, the image coordinate information of angle point is obtained from the Quick Response Code image, calculate the distance of camera to Quick Response Code angle point, and identify the ID of special Quick Response Code, according to the geographical relationship of Quick Response Code ID and place map, obtain the position of vehicle under this map environment and attitude.

When the coordinate determining vehicle, vision system must perceive the existence of at least one Quick Response Code.

Wherein, the Quick Response Code ID in place is ID unique in this place, and is associated with place map; Be supplied to vehicle on-board vision system, thus confirm the locating information of vehicle.

Being described as follows of the step of the image coordinate of angle point is obtained from the MARKER image.

One, Contour extraction is carried out to bianry image.

Determine that in bianry image, the step of profile is as follows:

1) profile starting point is searched:

If 1. f (i, j)=1 and f (i, j-1)=0, then current pixel point (i, j) is the starting point of profile outer rim; And making current outline numbering (CCC) increase by 1, (i2, j2) assignment is (i, j-1);

If 2. f (i, j) >=1 and f (i, j+1)=0, then current pixel point (i, j) is the starting point of profile inner edge, CCC+1; (i2, j2) assignment is (i, j+1); Otherwise, go to step 3).

2) Contour extraction from the starting point of profile:

1., in the neighborhood of current pixel (i, j), start to search first non-zero pixels point from (i2, j2) along clockwise direction, if find pixel, be then designated as (i1, j1); Otherwise, make f (i, j)=-CCC, turn 3);

2. (i2, j2) assignment is (i1, j1), and (i3, j3) assignment is (i, j);

3. in the neighborhood of (i3, j3), start from the next pixel of (i2, j2) in the counterclockwise direction, search first non-zero pixels point, be designated as (i4, j4);

4. the value of pixel (i3, j3) is changed: if f (i3, j3+1) ≠ 0, and f (i3, j3)=1, then f (i3, j3)=CCC; If f (i3, j3+1)=0, and f (i3, j3)=1, f (i3, j3-1)=0, then f (i3, j3)=CCC; If f (i3, j3+1)=0, and

F (i3, j3)=1, then f (i3, j3)=-CCC; Otherwise f (i3, j3) remains unchanged;

If 5. f (i4, j4)=(i, j) and (i3, j3)=(i1, j1), then get back to the starting point of profile, turn 3); Otherwise (i2, j2) assignment is (i3, j3), (i3, j3) assignment is (i4, j4), turns 3.;

3) condition of scanning is judged. start to continue scanning from (i, j+1) pixel, if till the pixel arriving the image lower right corner. j+1 is greater than picture traverse, then put j=0, i=i+1, scans from next line.

Two, the flex point of profile is found.

The method finding the flex point of profile is as follows:

$M=\mathrm{\Σ}w(x,y)\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]\→R^{-1}\left[\begin{array}{cc}{\mathrm{\λ}}_1& 0\\ 0& {\mathrm{\λ}}_2\end{array}\right]R$

By real symmetric matrix diagonalization process, R is regarded as twiddle factor here, it does not affect the change component of orthogonal directions.After diagonalization process, the variable quantity component " extraction " of two orthogonal directionss is out become λ ₁, λ ₂next (eigenwert), just can ideal-like angle point, edge, flat site analyze.

Step above has got accurate mark angle point, can simulate in camera and 3 dimension spaces the conversion between marking thus.In this process, the euclidean transformation between camera and object only comprises rotation and conversion.

As shown in Figure 2, O is the center being expressed as camera, and A, B, C, D are the points of 3 dimensions of world coordinates axle, and a, b, c, d are their projections in camera image plane.Object below uses inherent matrix and goes for out the conversion between the position of 3 dimension space known mark and camera O at the point that the plane of delineation is known.

Owing to marking always square and all summits all at same plane, in order to obtain the position coordinates being marked at 3 dimension spaces, therefore define their angle point as shown in Figure 3.

Shown in Fig. 3 is an embodiment of Quick Response Code used in the present invention, and it preferably uses has standard-sized black and white frame and form, to improve the success ratio of identification.

Mark is placed in xy coordinate system (i.e. z=0), and the central point of mark is placed in (0,0,0) place.The starting point of left side system is exactly the central point of mark.

After having identified Quick Response Code, the point set namely tieed up by 2 dimensions-3 has found out the position of camera.

The algorithm flow finding the three-dimensional reconstruction of the position of camera is as follows.

Suppose attitude to be asked, comprise rotation matrix R and translation vector T, be respectively

$R=\left[\begin{array}{ccc}{R}_{11}& R_{12}& R_{13}\\ R_{21}& R_{22}& R_{23}\\ R_{31}& R_{32}& R_{33}\end{array}\right]=\left[\begin{array}{c}{R}_1^T\\ R_2^T\\ R_3^T\end{array}\right]$

$T=\left[\begin{array}{c}{T}_x\\ T_y\\ T_z\end{array}\right]$

Perspective projection transformation is:

$x=\frac{f}{Z_c}{X}_c$

$y=\frac{f}{Z_c}{Y}_c$

(x, y) in formula is the coordinate of the image coordinate system in units of millimeter, (X _c, Y _c, Z _c) be the coordinate of camera coordinate system in units of millimeter.

F in above formula is the focal length of video camera, and its occurrence is unimportant, the ratio importantly between f and x and y, fx and fy according to camera internal reference matrix number can obtain this ratio.Directly can make f=1 in actual computing, but corresponding x and y also proportionally to set.Such as, be the camera of [fx, fy, u0, v0] for intrinsic parameter, if the position of a pixel is (u, v), then corresponding x and y should be

x＝(u-u ₀)f/f _x

y＝(v-v ₀)f/f _y

If in world coordinate system is a bit (X _w, Y _w, Z _w), then

$\left[\begin{array}{c}{Z}_{c}x\\ Z_{c}y\\ Z_c\end{array}\right]=\left[\begin{array}{c}f{X}_c\\ {\mathrm{fY}}_c\\ Z_c\end{array}\right]=\left[\begin{array}{cc}{\mathrm{fR}}_1^T& {\mathrm{fT}}_x\\ {\mathrm{fR}}_2^T& {\mathrm{fT}}_y\\ R_3^T& T_z\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$

Both sides are simultaneously divided by T _z, obtain

$\left[\begin{array}{c}wx\\ wy\\ w\end{array}\right]=\left[\begin{array}{cc}{\mathrm{sR}}_1^T& {\mathrm{sT}}_x\\ {\mathrm{sR}}_2^T& {\mathrm{sT}}_y\\ R_3^T/T_z& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$ (original equation)

Wherein

w＝Z _c/T _z

W＝[X _wY _wZ _w] ^T

s＝f/T _z

The concrete meaning of above rotation matrix R and translation vector T is described as follows:

I-th row of R represents the coordinate of vector of unit length in world coordinate system of i-th change in coordinate axis direction in camera coordinate system;

The coordinate of vector of unit length in camera coordinate system of i-th change in coordinate axis direction in world coordinate system is shown in i-th list of R;

T is just in time the coordinate of initial point at camera coordinate system of world coordinate system, and especially, Tz represents " degree of depth " of the initial point in camera coordinate system of world coordinate system.

Suppose " thickness " of object in Z-direction, i.e. the Z changes in coordinates scope of body surface each point in camera coordinate system, much smaller than the mean depth of this object in Z-direction.Described " thickness " and " degree of depth " are all for the Z axis of camera coordinate system.Can think that mean depth is exactly the Tz component in translation vector T when immediate vicinity at object of the initial point of world coordinate system, namely the mean value of the Zc of each point is Tz, and the variation range of Zc is very little relative to Tz, therefore can think, Zc all the time near Tz, Zc ≈ Tz.

According to this approximation relation, can obtain

W＝Z _c/T _z≈1

The initial value of iteration that Here it is.In this initial situation, suppose that the institute of object is a little in the same degree of depth, perspective transform at this moment is just degenerated in order to a scaled orthographic projection POS.Namely, iteration at this moment starts from a scaled orthographic projection.

Obtain above:

$\left[\begin{array}{c}wx\\ wy\\ w\end{array}\right]=\left[\begin{array}{cc}{\mathrm{sR}}_1^T& {\mathrm{sT}}_x\\ {\mathrm{sR}}_2^T& {\mathrm{sT}}_y\\ R_3^T/T_Z& 1\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$

Because w has had an estimated value, therefore first can be regarded as known quantity, delete the third line (just lacked 4 unknown quantitys in this spline equation, more convenient solve), obtain

$\left[\begin{array}{c}wx\\ wy\end{array}\right]=\left[\begin{array}{cc}{\mathrm{sR}}_1^T& {\mathrm{sT}}_x\\ {\mathrm{sR}}_2^T& {\mathrm{sT}}_y\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{sR}}_{11}& {\mathrm{sR}}_{12}& {\mathrm{sR}}_{13}& {\mathrm{sT}}_x\\ {\mathrm{sR}}_{21}& {\mathrm{sR}}_{22}& {\mathrm{sR}}_{34}& {\mathrm{ST}}_y\end{array}\right]\left[\begin{array}{c}{X}_w\\ Y_w\\ Z_w\\ 1\end{array}\right]$

$\left\{\begin{array}{c}wx=s{R}_{11}{X}_w+s{R}_{12}{Y}_w+s{R}_{13}{Z}_w+s{T}_x\\ wy={\mathrm{sR}}_{21}{X}_w+{\mathrm{sR}}_{22}{Y}_w+{\mathrm{sR}}_{23}{Z}_w+{\mathrm{sT}}_y\end{array}\right.$ (iterative equation)

Because w is counted as known, the iterative equation therefore can be regarded as 8 unknown quantitys, is sR respectively ₁₁sR ₁₂sR ₁₃sT _xsR ₂₁sR ₂₂sR ₂₃sT _y,

These 8 unknown quantitys can split into 3 vectors, respectively:

${\mathrm{sR}}_1=\left[\begin{array}{c}s{R}_{11}\\ {\mathrm{sR}}_{12}\\ {\mathrm{sR}}_{13}\end{array}\right],{\mathrm{sR}}_2=\left[\begin{array}{c}s{R}_{21}\\ {\mathrm{sR}}_{22}\\ {\mathrm{sR}}_{23}\end{array}\right],sT=\left[\begin{array}{c}s{T}_x\\ {\mathrm{sT}}_y\end{array}\right]$

After given a pair coordinate, (one is the coordinate of world coordinate system, one is the coordinate of image coordinate system, their corresponding same points), just can obtain 2 independently equations, needs 8 independent equations altogether, therefore at least need given 4 pairs of coordinates, and these 4 points of correspondence can not be coplanar in world coordinate system.If the 4th point and first three point coplanar, so " homogeneous coordinates " of this point just can by " homogeneous coordinates " linear expression of other three points, and the right side of iterative equation uses is exactly homogeneous coordinates, not just independent equation by the 4th equation that point obtains like this.Here why emphasize " homogeneous coordinates ", as long as be because three points not conllinear, " ordinary coor " of every other point (even if not coplanar) can by " ordinary coor " linear expression of these three points, but " homogeneous coordinates " then require coplanar.

If obtained 4 not coplanar points and coordinate thereof, and obtained 8 unknown quantitys by iterative equation.At this moment just vectorial sR can be calculated ₁and sR ₂mould long.And due to R ₁and R ₂itself be all vector of unit length, namely mould length is 1.Therefore can obtain s, and then try to achieve R ₁and R ₂and Tz=f/s:

More than show and describe ultimate principle of the present invention, principal character and advantage of the present invention.Those skilled in the art should understand the present invention and not be restricted to the described embodiments; what describe in above-described embodiment and instructions just illustrates principle of the present invention; without departing from the spirit and scope of the present invention; the present invention also has various changes and modifications, and these changes and improvements all fall in claimed scope.

Claims (8)

1. a vehicle positioning method, described vehicle being provided with the camera for detecting ground Quick Response Code, it is characterized in that, said method comprising the steps of:

Step 1, from camera reading images information;

Step 2, carries out pre-service to the image obtained;

Step 3, the Quick Response Code in detected image, obtains the image coordinate information of angle point from the Quick Response Code image;

Step 4, calculates the distance of camera to Quick Response Code angle point;

Step 5, identifies the ID of Quick Response Code, according to the geographical relationship of Quick Response Code ID and place map, obtains the position of vehicle under this map environment and attitude.

2. method according to claim 1, is characterized in that, described pre-service comprises color conversion, image enhaucament and/or image scaling.

3. method according to claim 1, is characterized in that, described step 3 comprises further:

Step 3.1, carries out Contour extraction to bianry image;

Step 3.2, finds the flex point of profile.

4. method according to claim 3, is characterized in that, described step 3.1 comprises further:

1) profile starting point is searched:

If 1. view data f (i, j)=1 and f (i, j-1)=0, then current pixel point (i, j) is the starting point of profile outer rim; And making current outline numbering CCC increase by 1, (i2, j2) assignment is (i, j-1);

If 2. view data f (i, j) >=1 and f (i, j+1)=0, then current pixel point (i, j) is the starting point of profile inner edge, CCC+1; (i2, j2) assignment is (i, j+1); Otherwise, go to step 3);

2) Contour extraction from the starting point of profile:

1., in the neighborhood of current pixel (i, j), start to search first non-zero pixels point from (i2, j2) along clockwise direction, if find pixel, be then designated as (i1, j1); Otherwise, make f (i, j)=-CCC, turn 3);

2. (i2, j2) assignment is (i1, j1), and (i3, j3) assignment is (i, j);

3. in the neighborhood of (i3, j3), start from the next pixel of (i2, j2) in the counterclockwise direction, search first non-zero pixels point, be designated as (i4, j4);

4. the value of pixel (i3, j3) is changed: if f (i3, j3+1) ≠ 0, and f (i3, j3)=1, then f (i3, j3)=CCC; If f (i3, j3+1)=0, and f (i3, j3)=1, f (i3, j3-1)=0, then f (i3, j3)=CCC; If f (i3, j3+1)=0, and

F (i3, j3)=1, then f (i3, j3)=-CCC; Otherwise f (i3, j3) remains unchanged;

If 5. f (i4, j4)=(i, j) and (i3, j3)=(i1, j1), then get back to the starting point of profile, turn 3); Otherwise (i2, j2) assignment is (i3, j3), (i3, j3) assignment is (i4, j4), turns 3.;

3) condition of scanning is judged. start to continue scanning from (i, j+1) pixel, if till the pixel arriving the image lower right corner. j+1 is greater than picture traverse, then put j=0, i=i+1, scans from next line.

5. method according to claim 3, is characterized in that, the method finding the flex point of profile in described step 3.2 is as follows:

$M=\mathrm{\Σ}w(x,y)\left[\begin{array}{cc}{I}_x^2& I_x{I}_y\\ I_x{I}_y& I_y^2\end{array}\right]\→R^{-1}\left[\begin{array}{cc}{\mathrm{\λ}}_1& 0\\ 0& {\mathrm{\λ}}_2\end{array}\right]R$

By real symmetric matrix diagonalization process, R is twiddle factor, and it does not affect the change component of orthogonal directions.After diagonalization process, the variable quantity component extraction of two orthogonal directionss is out become eigenvalue λ ₁, λ ₂, next angle point, edge, flat site are analyzed.

6. method according to claim 1, is characterized in that, described Quick Response Code is formed by having standard-sized black and white frame.

7. method according to claim 1, is characterized in that, described camera is placed on the afterbody of vehicle and/or the front portion of vehicle, and described camera presses certain angle towards ground.

8. method according to claim 7, is characterized in that, described camera towards ground on be distributed with Quick Response Code, to ensure during vehicle operating that any time all has a Quick Response Code at least in the shooting area of camera.