Positioning method and device based on template matching and optical flow fusion and trolley

Abstract

The application discloses a positioning method and device based on template matching and optical flow fusion and a trolley. The method first calculates a first displacement through template matching of ground textures at two time points, then calculates a more accurate second displacement through optical flow analysis, and finally calculates positioning based on the second displacement. Compared with a simple template matching method, the application fuses optical flow analysis, improves error precision of relative displacement calculation of two ground texture images, thereby reducing cumulative positioning error and making positioning more accurate.

Description

Technical Field

[0001] This invention relates to visual positioning technology. Background Technology

[0002] Existing positioning technologies can be broadly categorized into wireless positioning and visual positioning. Wireless positioning achieves target localization by analyzing the time difference and phase difference of wireless signals, and can be further divided into satellite signal-based positioning, mobile signal-based positioning, and Wi-Fi / Bluetooth-based positioning technologies. Visual positioning achieves target localization by analyzing images captured by cameras, and can be divided into positioning technologies based on wide-angle image target distance analysis and positioning technologies based on ground texture analysis without reference targets. Each type of positioning has its own advantages and disadvantages. In practical applications, multiple positioning technologies are often combined.

[0003] The positioning technology involved in this invention is a ground texture analysis positioning technology without a reference target. This positioning technology calculates the relative displacement by analyzing the displacement difference between two consecutive ground texture images, and then calculates the real-time positioning. The relative displacement calculated from the displacement difference between two consecutive ground texture images is usually obtained through template matching. This positioning technology has at least two drawbacks: First, it requires specific ground texture; ground lacking texture, such as a monochromatic, clean surface or a mirror-like surface, cannot be used for positioning. Second, it suffers from cumulative error; if the relative displacement calculated from two consecutive ground texture images has a large error, the cumulative error amplifies sharply, leading to inaccurate positioning. The first drawback can be considered as an application requirement of the positioning technology and can be disregarded. However, the second drawback requires that the error in the relative displacement calculated from each consecutive ground texture image be as small as possible. In existing technologies, the relative displacement calculated from the displacement difference between two consecutive ground texture images is usually obtained through template matching; however, positioning accuracy is poor when calculated solely through template matching. Summary of the Invention

[0004] The problem this invention aims to solve is to improve the accuracy of relative displacement calculation between two consecutive ground texture images in positioning technology for ground texture analysis without a reference target.

[0005] To solve the above problems, the present invention adopts the following solution:

[0006] According to the localization method based on template matching and optical flow fusion of the present invention, the method includes the following steps:

[0007] Step S1 is used to: continuously receive ground texture images captured by the ground-facing camera;

[0008] Step S2 is used to: determine two rectangular regions as templates by calculating the image entropy of the rectangular regions of the ground texture image at time i and the central region of the ground texture image;

[0009] Step S3 is used to: find two rectangular regions in the ground texture image at time i+1 that have the highest similarity to the template, respectively, as template matching results;

[0010] Step S4 is used to: calculate the displacements (dx1, dy1) and (dx2, dy2) of the two template matching results relative to the two templates based on the center coordinates of the two templates and the center coordinates of the two matching results, and then calculate the first displacement (dx1, dy1) of the first displacement (dx2, dy2). c ,dy c ); where dx c = (dx1 + dx2) ÷ 2; dy c = (dy1 + dy2) ÷ 2;

[0011] dx1 and dy1 are the relative displacements of the x and y coordinates between the center of the first template matching result and the center of the first template.

[0012] dx2 and dy2 are the relative displacements of the x and y coordinates between the center of the second template matching result and the center of the second template.

[0013] Step S5 is used to: calculate the first displacement (dx) using optical flow. c ,dy c The second displacement (dx) is obtained after correction. t ,dy t );

[0014] Step S6 is used to: adjust the second displacement (dx) t ,dy t ) mapped to real-world displacement (dx) v ,dy v );

[0015] Step S7 is used to calculate the heading angle A based on the center coordinates of the two templates and the center coordinates and displacements of the two matching results.

[0016] Among them, D C12 It is the center distance between the two templates;

[0017] Step S8 is used to: determine the heading angle A and the actual displacement dp v =(dx v ,dy v ), the position at time i (x) i ,y i ) Calculate the position (x) at time i+1.i+1 , y i+1 ); where

[0018] x i+1 = x i + x i × cos(A) - y i × sin(A);

[0019] y i+1 = y i + y i × cos(A) + x i × sin(A);

[0020] The step S5 is to perform iterative calculation on the formula ΔP k = -(M T M) -1 M T (MP k + b) until ΔP k < q, then take dx t = px k , dy t = py k to obtain the second displacement (dx t , dy t );

[0021] where, P k = P k-1 + ΔP k-1 , px0 = dx c , py0 = dy c ;

[0022] S = W × H;

[0023] where, I(P k , 1), I(P k , 2),..., I(P k , S) are the gray value differences of each pixel between the template center displacement P[[ID=8l]] k on the ground texture image at the i-th moment and the template on the ground texture image at the (i + 1)-th moment;

[0024] W is the width of the template, H is the height of the template, and q is a preset threshold.

[0025] Furthermore, according to the positioning method based on template matching and optical flow fusion of the present invention, the step S2 is to calculate the information entropy of a rectangular area with the size of the template and select two rectangular areas with the largest information entropy as the templates.

[0026] The positioning device based on template matching and optical flow fusion according to the present invention includes the following modules:

[0027] Module M1 is used to continuously receive ground texture images captured by the ground-facing camera;

[0028] Module M2 is used to: determine two rectangular regions as templates by calculating the image entropy of the rectangular regions of the ground texture image at time i and the central region of the ground texture image;

[0029] Module M3 is used to: find two rectangular regions in the ground texture image at time i+1 that have the highest similarity to the template, respectively, as template matching results through template matching;

[0030] Module M4 is used to: calculate the displacements (dx1, dy1) and (dx2, dy2) of the two template matching results relative to the two templates based on the center coordinates of the two templates and the center coordinates of the two matching results, and then calculate the first displacement (dx1, dy1) of the matching results. c ,dy c ); where dx c = (dx1 + dx2) ÷ 2; dy c = (dy1 + dy2) ÷ 2;

[0031] dx1 and dy1 are the relative displacements of the x and y coordinates between the center of the first template matching result and the center of the first template.

[0032] dx2 and dy2 are the relative displacements of the x and y coordinates between the center of the second template matching result and the center of the second template.

[0033] Module M5 is used for: calculating the first displacement (dx) via optical flow. c ,dy c The second displacement (dx) is obtained after correction. t ,dy t );

[0034] Module M6 is used to: transfer the second displacement (dx) t ,dy t ) mapped to real-world displacement (dx) v ,dy v );

[0035] Module M7 is used to calculate the heading angle A based on the center coordinates of two templates and the center coordinates and displacements of two matching results.

[0036] Among them, D C12 It is the center distance between the two templates;

[0037] Module M8, for: according to the heading angle A and the true displacement dp v =(dx v , dy v ), calculate the position (x i , y i ) at the (i + 1)-th moment based on the position (x i+1 , y i+1 ) at the i-th moment; where,

[0038] x i+1 =x i +x i × cos(A) - y i × sin(A);

[0039] y i+1 =y i +y i × cos(A)+x i × sin(A);

[0040] The module M5 iteratively calculates the formula ΔP k =-(M T M) -1 M T (MP k +b) until ΔP k <q, and then takes dx t =px k , dy t =py k to obtain the second displacement (dx t , dy t );

[0041] where, P k =P k-1 +ΔP k-1 , px0 = dx c , py0 = dy c ;

[0042] S = W × H;

[0043] where, I(P k , 1), I(P k , 2),..., I(P k , S) are the gray value differences of each pixel between the template center displacement P k at the i-th moment on the ground texture image and the template on the ground texture image at the (i + 1)-th moment;

[0044] W is the width of the template, H is the height of the template, and q is a preset threshold.

[0045] Furthermore, according to the positioning device based on template matching and optical flow fusion of the present invention, the module M2 calculates the information entropy of a rectangular region of template size and selects the two rectangular regions with the largest information entropy as templates.

[0046] According to a vehicle of the present invention, the vehicle includes a camera for capturing ground texture images and a controller; the controller, connected to the camera, includes a processor and a memory; the memory stores a program instruction set; when the program instruction set is loaded and executed by the processor, the processor positions the vehicle according to the above-described positioning method.

[0047] The technical effects of this invention are as follows: Compared with the simple template matching method, this invention integrates optical flow analysis, which improves the error accuracy of the relative displacement calculation between two consecutive ground texture images, thereby reducing the cumulative positioning error and making the positioning more accurate. Attached Figure Description

[0048] Figure 1 This is a flowchart of the positioning method according to an embodiment of the present invention.

[0049] Figure 2 This is a schematic diagram of the structure of the vehicle according to an embodiment of the present invention. Detailed Implementation

[0050] The present invention will now be described in further detail with reference to the accompanying drawings.

[0051] Figure 2 An example of a small car is provided. This car is an intelligent car, equipped with a controller 100 implemented by a circuit board, a camera 201 with its lens facing the ground, and wheels for movement. The controller 100 includes a processor 101 and a memory 102 connected together. The processor 101 is connected to the camera 201. The memory 102 stores program instruction sets and data. The processor 101 controls the movement of the car by executing the program instruction set. Specifically, in this embodiment, the processor 101 implements the template matching and optical flow fusion-based positioning method of the present invention by executing the program instruction set stored in the memory 102. The input of this positioning method is a ground texture image captured by the ground-facing camera 201. The processor 101 continuously receives the ground texture images captured by the ground-facing camera 201, and then calculates the real-time position based on the positioning calculation of the ground texture images at previous and subsequent times. The continuous receiving of ground texture images captured by the ground-facing camera 201 in the aforementioned step S1 indicates that the input of the present invention is the ground texture image captured by the ground-facing camera 201. This positioning method, refer to... Figure 1, including a template optimization step, a template matching step, a rough displacement calculation step, an optical flow analysis step, a reality mapping step, a heading angle calculation step, and a real-time position calculation step.

[0052] The template optimization step, that is, the aforementioned step S2, determines two rectangular regions as templates in the central region of the ground texture image at the i-th moment by calculating the image entropy of the rectangular region of the ground texture image. Specifically, the template is a rectangular region on the ground texture image at the i-th moment and is located in the central region of the ground texture image at the i-th moment, with a size of W×H. Here, W is the width of the template, and H is the height of the template. In this embodiment, the central region of the ground texture image at the i-th moment is preferably a rectangular region with a size of w / 2×h / 2 at the center of the image, where w and h are the width and height of the ground texture image, respectively. In addition, it should be noted that the templates determined in this step are two templates, corresponding to two rectangular regions respectively. Thus, obviously, there is usually: W < w / 4, H < h / 4. In addition, the image entropy can be calculated by the following formula:

[0053]

[0054] where R [[ID=​10]] i (P, W, H) represents a rectangular region with a size of W×H centered at point P in the ground texture image at the i-th moment; H(R i (P, W, H)) represents the image entropy of the rectangular region R i (P, W, H); n(R i (P, W, H), j) represents the number of pixels with a pixel gray value of j in the rectangular region R i (P, W, H).

[0055] In addition, it should be pointed out that the templates output in this step are usually represented by the template center coordinates. In this embodiment, the two templates are respectively represented as T1 i =(x T1,i , y T1,i ), T2 i =(x T2,i , y T2,i ); where, T1 <‬ i represents the first template of the ground texture image at the i-th moment, and its center point is (x T1,i , y T1,i ); T2 i represents the second template of the ground texture image at the i-th moment, and its center point is (x T2,i , y T2,i ).

[0056] The template matching step, also known as step S3, involves finding two rectangular regions in the ground texture image at time i+1 that have the highest similarity to the template, respectively, as the template matching results. The specific template matching algorithm for this step is well-known to those skilled in the art and will not be described in detail here. It should be noted that the ground texture images at time i+1 and time i are not necessarily two consecutive images received by the processor 101. For example, at a typical video frame rate, the processor 101 receives 20 frames per second, but time i+1 and time i may differ by 1 second. Therefore, there are 19 discarded frames between the ground texture images at time i+1 and time i. Furthermore, the template corresponding to step S2 is represented by the template center coordinates, and the matching results in this step can also be represented by center coordinates. In this embodiment, the two matching results are: D1 i =(x D1,i+1 ,y D1,i+1 ), D2 i =(x D2,i+1 ,y D2,i+1 ); where D1 i Represented as the ground texture image at time i+1 and the first template T1 i The center point of the template matching result is (x D1,i+1 ,y D1,i+1 D2 i Represented as the ground texture image at time i+1 and the second template T2 i The center point of the template matching result is (x D2,i+1 ,y D2,i+1 ).

[0057] The coarse displacement calculation step, namely step S4 mentioned above, calculates the displacements (dx1, dy1) and (dx2, dy2) of the two template matching results relative to the two templates based on the center coordinates of the two templates and the center coordinates of the two matching results, and then calculates the first displacement (dx1, dy1) of the first displacement (dx2, dy2). c ,dy c ). Among them, dx c = (dx1 + dx2) ÷ 2; dy c = (dy1 + dy2) ÷ 2. Based on the aforementioned template and matching result definitions, dx1, dy1, dx2, and dy2 can be calculated using the following formula:

[0058] dx1=x D1,i+1 -x T1,i ;

[0059] dy1=y D1,i+1 -y T1,i ;

[0060] dx2 = x D2,i+1 -x T2,i ;

[0061] dy2 = y D2,i+1 -y T2,i .

[0062] The optical flow analysis step, which is the aforementioned step S5, corrects the first displacement (dx c , dy c ) through optical flow calculation to obtain the second displacement (dx t , dy t ). Specifically, by iteratively calculating the formula ΔP k = -(M T M) -1 A T (MP k + b) until ΔP k <q, take dx t = px k , dy t = py k to obtain the second displacement (dx t , dy t ). Among them, P k = P k-1 + ΔP k-1 , px0 = dx c , py0 = dy c ;

[0063] S = W × H;

[0064] I(P k , 1), I(P k , 2),..., I(P k , S) are the gray value differences of each pixel between the template center displacement P k on the ground texture image at the i-th moment and the template on the ground texture image at the (i + 1)-th moment. Here, I(P k , 1), I(P k , 2),..., I(P k , S) can be calculated by the following formula:

[0065] I(P k , t) = G(r t |r t ∈ R i+1 (T + P k , W, H)) - G(r t |r t ∈ Ri (T,W,H)),t∈[1..S];

[0066] Among them, R i (T,W,H) represents a rectangular region of size W×H centered at point T in the ground texture image at time i; R i+1 (T+P k (W, H) represents the ground texture image at time i+1 with point T+P. k The region is a rectangular area of ​​size W×H centered at point T; T ,y T () is the center point of the template, which can be T1 i =(x T1,i ,y T1,i ) or T2 i =(x T2,i ,y T2,i Point T+P k The template center displacement P k The center point, T+P k =(x T +px k ,y T +py k );r t ∈R i (T,W,H) represents a rectangular region R. i The t-th pixel in (T,W,H); r t ∈R i+1 (T+P k (W, H) represents a rectangular region R i+1 (T+P k Let G be the t-th pixel in (W, H); G is the grayscale value function of the pixel.

[0067] Step S5 is an iterative calculation process, which is specifically broken down into the following steps in this embodiment:

[0068] Step S51: Construct the initial template displacement center point

[0069] Step S52, based on the template displacement center P k Calculate I(P) k ,1),I(P k ,2),...,I(P k ,S);

[0070] Step S53, based on the calculated I(P) k ,1),I(P k ,2),...,I(P k Construct matrix M and vector b:

[0071]

[0072] Step S54, calculate ΔP according to matrix M and vector b k = -(M T M) -1 M T (MP k + b);

[0073] Step S55, determine whether ΔP k satisfies ΔP k < q; if ΔP k < q, then take dx t = px k , dy t = py k to obtain the second displacement (dx t , dy t ) and end the iterative loop; here q is a pre-set threshold, and ΔP k < q can also be expressed as: Δpx k < q x and Δpy k < q y ; where,

[0074] Step S56, calculate P k+1 = P k + ΔP k , and take k = k + 1, repeat steps S52 to S56.

[0075] The real-world mapping step, which is the aforementioned step S6, maps the second displacement (dx t , dy t ) into the real displacement (dx v , dy v ) in the real world. The real displacement (dx v , dy v ) can be calculated by the following formula:

[0076]

[0077]

[0078] where, hc is the height of the camera 201 lens from the ground; fc x and fc y are the focal lengths of the camera 201 in the x and y directions respectively. Obviously, usually here: fc x = fc y = fc; therefore, the above formula can be simplified to:

[0079]

[0080]

[0081] Where fc is the focal length of camera 201.

[0082] The heading angle calculation step, also known as step S7 mentioned above, involves calculating the heading angle A based on the center coordinates of the two templates and the center coordinates and displacements of the two matching results. The heading angle A can be calculated using the following formula:

[0083] Among them, D C12 It is the center distance between the two templates; specifically, in this embodiment, it is the center point (x) of the first template. T1,i ,y T1,i ) and the center point (x) of the second template T2,i ,y T2,i The distance between ) . Here, dx1, dy1, dx2, dy2 are the same as dx1, dy1, dx2, dy2 in the aforementioned step S3.

[0084] The real-time position calculation step, also known as step S8 mentioned above, is based on the heading angle A and the actual displacement dp. v =(dx v ,dy v ), the position at time i (x) i ,y i ) Calculate the position (x) at time i+1. i+1 ,y i+1 ). Among them, position (x) i+1 ,y i+1 It can be calculated using the following formula:

[0085] x i+i =x i +x i ×cos(A)-y i ×sin(A);

[0086] y i+1 =y i +y i ×cos(A)+x i ×sin(A).

[0087] The location here (x) i ,y iThe position (x0, y0) is stored in memory. Initially, that is, the position at time 0 is usually assigned through other positioning methods. How the position (x0, y0) is specifically assigned is not within the scope of this invention and will not be elaborated here.

[0088] Furthermore, the device referred to in this invention is a virtual device corresponding to the aforementioned method, and its modules are the steps corresponding to the method, which will not be described again.

Claims

1. A localization method based on template matching and optical flow fusion, characterized in that, The method includes the following steps: Step S1 is used to: continuously receive ground texture images captured by the ground-facing camera; Step S2 is used to: determine two rectangular regions as templates by calculating the image entropy of the rectangular regions of the ground texture image at time i and the central region of the ground texture image; Step S3 is used to: find two rectangular regions in the ground texture image at time i+1 that have the highest similarity to the template, respectively, as template matching results; Step S4 is used to: calculate the displacement of the two template matching results relative to the two templates based on the center coordinates of the two templates and the center coordinates of the two matching results. and Then the first displacement is calculated. ;in, ; ; and It is the relative displacement of the horizontal and vertical coordinates between the center of the first template matching result and the center of the first template; and It is the relative displacement of the horizontal and vertical coordinates between the center of the second template matching result and the center of the second template; Step S5 is used to: calculate the first displacement using optical flow. The second displacement was obtained after correction. ; Step S6 is used to: adjust the second displacement Mapped to real-world displacement ; Step S7 is used to calculate the heading angle A based on the center coordinates of the two templates and the center coordinates and displacements of the two matching results. ;in, It is the center distance between the two templates; Step S8 is used to: determine the heading angle A and the actual displacement. The position at time i Calculate the position at time i+1. ;in, ； ； Step S5 involves adjusting the formula. Iterative calculation until Time to take , The second displacement is obtained ; in, , ; ; , ; ； ； ； in, The template center displacements on the ground texture image at time i are respectively. The difference in grayscale values ​​between individual pixels on the ground texture image up to time i+1; W is the width of the template, H is the height of the template, and q is a preset threshold.

2. The localization method based on template matching and optical flow fusion as described in claim 1, characterized in that, Step S2 calculates the information entropy of the rectangular regions of the template size and selects the two rectangular regions with the largest information entropy as templates.

3. A positioning device based on template matching and optical flow fusion, characterized in that, The device includes the following modules: Module M1 is used to continuously receive ground texture images captured by the ground-facing camera; Module M2 is used to: determine two rectangular regions as templates by calculating the image entropy of the rectangular regions of the ground texture image at time i and the central region of the ground texture image; Module M3 is used to: find two rectangular regions in the ground texture image at time i+1 that have the highest similarity to the template, respectively, as template matching results through template matching; Module M4 is used to calculate the displacement of the matching result of the two templates relative to the two templates based on the center coordinates of the two templates and the center coordinates of the two matching results. and Then the first displacement is calculated. ;in, ; ; and It is the relative displacement of the horizontal and vertical coordinates between the center of the first template matching result and the center of the first template; and It is the relative displacement of the horizontal and vertical coordinates between the center of the second template matching result and the center of the second template; Module M5 is used for: calculating the first displacement via optical flow. The second displacement was obtained after correction. ; Module M6 is used to: transfer the second displacement Mapped to real-world displacement ; Module M7 is used to calculate the heading angle A based on the center coordinates of two templates and the center coordinates and displacements of two matching results. ;in, It is the center distance between the two templates; Module M8 is used to: determine the heading angle A and the actual displacement. The position at time i Calculate the position at time i+1. ;in, ； ； The module M5, through the formula Iterative calculation until Time to take , The second displacement is obtained ; in, , ; ; , ; ； ； ； in, The template center displacements on the ground texture image at time i are respectively. The difference in grayscale values ​​between individual pixels on the ground texture image up to time i+1; W is the width of the template, H is the height of the template, and q is a preset threshold.

4. The positioning device based on template matching and optical flow fusion as described in claim 3, characterized in that, The module M2 calculates the information entropy of a rectangular region of template size and selects the two rectangular regions with the highest information entropy as templates.

5. A small vehicle, characterized in that, The vehicle includes a camera for capturing images of ground texture and a controller; the controller, connected to the camera, includes a processor and a memory; the memory stores a set of program instructions; when the set of program instructions is loaded and executed by the processor, the processor positions the vehicle according to the positioning method according to claim 1 or 2.

Images