A vehicle positioning method based on vehicle trajectory image matching

Abstract

The present invention discloses a vehicle positioning method based on vehicle travel trajectory image matching. A regional road network map is loaded according to the approximate position of the vehicle at the initial moment; the regional road network map is converted to the northeast celestial geographic coordinate system; the boundary of the regional road network map is calculated for whole-region adjustment, and the base of the road network image pyramid is constructed; a single road element in the image-based road network map is processed; all single road elements in the regional road network are image-processed, and the images of single road elements at each level are fused to construct a road network pyramid image; an initial vehicle travel trajectory is generated and converted to the geographic coordinate system; the initial vehicle travel trajectory is divided into straight trajectories and curved trajectories; until a "straight trajectory sandwiching a curved trajectory" is formed, the straight sections of the vehicle travel trajectory are image-processed; the regional road network image and the vehicle travel trajectory image are matched layer by layer to position the vehicle. The present invention can position the vehicle when the initial positioning information of the vehicle is unclear.

Description

Technical Field

[0001] The present invention belongs to the field of map matching and navigation positioning, and particularly relates to a vehicle positioning method based on vehicle trajectory image matching. Background Art

[0002] In vehicle navigation systems, satellite and inertial integrated navigation systems are widely used due to their excellent positioning performance and high cost performance. However, in urban environments, satellite signals are easily blocked by high-rise buildings, resulting in positioning failures. When the signals bounce off building facades and then reach the ground, multiple paths are formed, seriously affecting positioning accuracy. A pure inertial positioning system can provide accurate positioning information in a short time, but as the driving distance increases, the principle positioning error of inertial navigation accumulates continuously, and in severe cases, it can cause catastrophic positioning consequences. Researchers can compensate for the deficiencies of traditional integrated navigation systems to a certain extent by introducing external positioning systems such as wheel odometers for integrated positioning, but it will inevitably increase the construction cost of vehicle navigation systems and the complexity of data processing.

[0003] With the iterative upgrade of computer hardware and the rapid development of data processing technology, more and more researchers use cameras to achieve vehicle positioning. By inputting a series of images into a Visual Odometry (VO for short) to track the motion state of the vehicle. The visual odometer extracts the feature elements in the image frame, and calculates the pose difference between frames through the feature matching relationship between adjacent image frames. Essentially, it belongs to a kind of dead reckoning method and requires a clear initial pose of the vehicle. As time and driving distance increase, tiny errors will gradually accumulate, leading to a decline in the positioning accuracy of the visual odometer. Most existing methods use the visual odometer to recursively calculate the vehicle pose under the condition that the initial vehicle pose is determined, and introduce external constraints to suppress the cumulative error, without considering the vehicle positioning problem when the initial position is not clear. Summary of the Invention

[0004] Object of the Invention: The object of the present invention is to provide a vehicle positioning method based on vehicle trajectory image matching, which can transform the matching between the unordered point set of the road network and the ordered point set of the vehicle trajectory into an image matching problem, and can position the vehicle when the initial positioning information of the vehicle is not clear.

[0005] Technical Solution: A vehicle positioning method based on vehicle trajectory image matching according to the present invention includes the following steps:

[0006] (1) Load a regional road network map according to the approximate position of the vehicle at the initial moment;

[0007] (2) Convert the regional road network map to the northeast celestial geographic coordinate system;

[0008] (3) Calculate the boundaries of the regional road network map to adjust the entire area and construct the road network image pyramid base;

[0009] (4) Single road element in the visualized road network map;

[0010] (5) Visualize all single road elements in the regional road network and fuse the single road element images at each level to construct a road network pyramid image;

[0011] (6) Generate the initial vehicle trajectory using the visual odometry and convert it to the geographic coordinate system;

[0012] (7) Dividing the initial vehicle trajectory into a long straight trajectory and a curved trajectory according to the angular velocity of the heading angle change between frames;

[0013] (8) Repeating steps (6) and (7) until a “long straight section with a curved section” track is formed, and performing image processing on the long straight section of the vehicle track;

[0014] (9) Match regional road network images and vehicle trajectory images layer by layer to locate vehicles.

[0015] Furthermore, the regional road network map in step (1) is a "point-line" topological map, which is composed of an indefinite number of single road elements Way, and Way is an ordered set composed of a series of nodes Node, and the roads are represented by broken line segments connected by the nodes in the set; the data of a single node element Node includes its latitude lat, longitude lon and altitude alt.

[0016] Furthermore, the implementation process of step (2) is as follows:

[0017] Convert the LLA coordinates of all internal nodes of Way elements to the corresponding Earth-centered Earth-fixed coordinates. For the first node element Node0 in the original data list of the road network, record its LLA coordinates as (lat0, lon0, alt0), and its ECEF coordinates (X0, Y0, Z0) as:

[0018]

[0019]

[0020] Among them, R N represents the principal radius of curvature of the reference ellipsoid, f is the flattening of the reference ellipsoid, R e is the major radius in the equatorial plane;

[0021] Convert all node ECEF coordinates to northeast celestial coordinates, select node Node0 as the reference point, and calculate the transformation matrix from the Earth-centered Earth-fixed coordinate system to the northeast celestial coordinate system based on its LLA coordinates

[0022]

[0023] For the target point Node i , denote its LLA coordinates as (lat i , lon i , alt i ). Calculate its ECEF coordinates as (X i , Y i , Z i ). Then the ENU coordinates (E i , N i , U i ) of the target point are as follows:

[0024]

[0025] Furthermore, the implementation process of step (3) is as follows:

[0026] Traverse the internal nodes of each road element Way in the regional road network, calculate the boundaries of the regional road network map in the ENU coordinate system, and perform whole-region adjustment on it, including the maximum northward coordinate North BMax , the minimum northward coordinate North BMin , the maximum eastward coordinate East BMax , and the minimum eastward coordinate East BMin ;

[0027] Construct the base of the road network image pyramid with Num layers. Set the side length of a single grid from the 1st layer to the Num layer of the image pyramid base to be , and the number of grids increases proportionally; the base data of the pyramid is the data of each square node forming the base. Taking the lower left corner of the pyramid base as the origin, the positive direction of the E-axis is the direction in which the eastward coordinate increases, and the positive direction of the N-axis is the direction in which the northward coordinate increases. The coordinates of the square node in the nth layer, the ith row, and the jth column are calculated as shown in Equation (7), where the value ranges of i and j are as shown in Equation (8):

[0028]

[0029]

[0030]

[0031] Furthermore, the implementation process of step (4) is as follows:

[0032] Quadrilateral expansion is performed on a single road element according to the road width, the single road element is visualized based on the coincidence between the quadrilateral expansion result and the pyramid grids at each level, and finally saved as a single-channel image; a single grid in the pyramid grid is regarded as a pixel, and the pixel value ranges from 0 to 255, where 0 represents black, 255 represents white, and the intermediate values are different levels of gray;

[0033] For the single square node and the single quadrilateral expansion result at the base of the nth layer pyramid, first judge the inclusion relationship between the four vertices of the square node and the quadrilateral expansion result; for the upper left vertex P LU , the four points of the quadrilateral are R A , R B , R C and R D in counterclockwise order. If and have the same sign, that is, all four values are positive or all negative, then it is determined that the point P LU is inside the quadrilateral; then calculate the intersection points of the line segments of each side of the square node and the line segments of each side of the quadrilateral; finally calculate the area S Pol of the polygon formed by all intersection points and the included vertices (if the sum of the number of intersection points and the included vertices is less than 3, then S Pol = 0); the side length of the single square at the base of the nth layer pyramid is BD / 2 n-1 , and the area is S Rec = BD 2 / 2 2n-2 . The pixel value pixel of this square node is:

[0034] piexl = 255 - 255 * S Pol / S Rec (9)

[0035] Traverse the square nodes in the pyramid base to calculate their pixel values, and then the road elements at this level can be visualized.

[0036] Further, the implementation process of step (5) is as follows:

[0037] Design a construction strategy for fusing pyramid images of road networks. For two pixels to be fused at the same position of a single pixel point within the same level: If one of the two pixels to be fused has a pixel value of 255, it means there is no road imaging result at this position, and directly use the pixel value of the other pixel as the fused pixel value; If one of the two pixels to be fused has a pixel value of 0, it means there is a complete road imaging result at this position, and directly use 0 as the fused pixel value; If neither of the two pixels to be fused is 0 or 255, it means there are imaging results with different proportions at this position, and use the average of the two pixel values as the fused pixel value; Traverse the imaging results of each single road element in the regional road network map and fuse them to form the overall regional road network pyramid image.

[0038] Further, the implementation process of step (7) is as follows:

[0039] According to the inter-frame pose change output by the visual odometer, calculate the angular velocity dψ of the inter-frame heading angle change; If dψ < ψ sta , then determine that this frame belongs to a straight trajectory; If dψ > ψ sta , then determine that this frame belongs to a curved trajectory, where ψ sta is a preset value.

[0040] The implementation process of step (8) is as follows:

[0041] The imaging method of the vehicle trajectory does not consider the marginalization problem. For the single square node and the single quadrilateral expansion result at the base of the nth layer pyramid, if the center point of the square node is included in the quadrilateral expansion result, set its pixel value to 0, indicating that the vehicle passes through this area during driving; If the center point of the square node is not included in the quadrilateral expansion result, set its pixel value to 255, indicating that the vehicle does not pass through this area during driving.

[0042] Further, the implementation process of step (9) is as follows:

[0043] Traverse the regional road network pyramid image, and use the vehicle trajectory image at the corresponding level to match and locate the vehicle; For the road network image and the vehicle trajectory image at the same level, traverse all positions starting from the upper left corner, and calculate the sum of the numerical differences of the pixels in the overlapping part of the vehicle trajectory image and its position in the road network image;

[0044] Give priority to starting the traversal and matching from the lower level of the pyramid. The images at the higher level are slightly offset based on the matching results at the lower level to accelerate the matching and positioning process; Take the average value of the deviation as the final value of the matching and positioning result.

[0045] Beneficial effects: Compared with the prior art, the advantages and beneficial effects of the present invention are:

[0046] 1. The present invention uses the initial vehicle travel trajectory provided by visual odometry as the matching source. By matching it with the road network map, vehicle matching and positioning can be completed when only the approximate range of the vehicle's initial location is known.

[0047] 2. The positioning method designed by the present invention only depends on the "point-line" type road network map, without the need for richer map information. The required data is the position information of road network nodes, and the required map data volume is very small, which is applicable to large-scale scenarios.

[0048] 3. The present invention converts the matching between the unordered point set of the road network and the ordered point set of the vehicle travel trajectory into an image matching problem, and designs pyramid images and matching and positioning methods at different levels, which can simultaneously meet the requirements of real-time and accuracy of positioning through the matching of pyramid images at different levels.

[0049] 4. The present invention designs a method for pyramid image conversion of the regional road network map, which can effectively utilize the topological structure information of the "point-line" type road network map to construct road network images at different levels, and can use the size of pixel values to represent the weight of this position in the road network, providing a reference library for vehicle matching and positioning.

[0050] 5. The present invention designs a method for segmenting the vehicle travel trajectory and a method for pyramid image conversion based on heading change, which can simplify and efficiently utilize the original vehicle travel trajectory data and provide a matching source for vehicle matching and positioning. Description of the Drawings

[0051] Figure 1 is the flowchart of the present invention;

[0052] Figure 2 is the schematic diagram of the OpenStreetMap regional map;

[0053] Figure 3 is the schematic diagram of the regional road network map;

[0054] Figure 4 is the schematic diagram of the regional road network map in the northeast celestial coordinate system;

[0055] Figure 5 is the schematic diagram of the concept of the image pyramid network;

[0056] Figure 6 is the schematic diagram of the result of the expansion of the road quadrilateral;

[0057] Figure 7 is the schematic diagram of the road image pyramid;

[0058] Figure 8 is the schematic diagram of the regional road network image pyramid;

[0059] Figure 9It is a schematic diagram of the initial vehicle travel trajectory segmentation result;

[0060] Figure 10 It is a schematic diagram of the image pyramid of the vehicle travel trajectory on a long straight section;

[0061] Figure 11 It is a schematic diagram of the result of image matching and positioning of the vehicle travel trajectory. Specific implementation mode

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

[0063] As Figure 1 shown, the present invention proposes a vehicle positioning method based on image matching of vehicle travel trajectories. According to the regional road network and the image pyramid result of the vehicle travel trajectory, the initial position of the vehicle is matched and positioned layer by layer; among them, the known conditions are the approximate position and the initial attitude angle of the vehicle at the initial moment, and a relatively long straight section is preferentially selected to start positioning at the initial stage. The specific process is as follows:

[0064] Step 1: Load the regional road network map according to the approximate position of the vehicle at the initial moment.

[0065] Given the approximate position of the vehicle at the initial moment, download the regional road network map from the OpenStreetMap website according to the position information. The regional map of the OpenStreetMap website is as Figure 2 shown. The road network map consists of an indefinite number of single road elements Way. Way is an ordered set composed of a series of nodes Node, and the road is represented by a polyline connected by the nodes in the set; the data of a single node element Node includes its latitude lat, longitude lon and altitude alt. Select a region with a relatively high road network density, and the loaded regional road network map is as Figure 3 shown.

[0066] Step 2: Convert the regional road network map to the northeast celestial geographic coordinate system.

[0067] The original data of the nodes of the road network map on the OpenStreetMap website is the node latitude-longitude-altitude (LLA) coordinates. For the convenience of subsequent calculations, the present invention converts it to the regional northeast celestial geographic coordinate system. The conversion process is divided into two steps, both of which are mature means in the industry. For the sake of completeness of the content, a necessary description is made. In the first step, convert the LLA coordinates of the internal nodes of all Way elements to the corresponding Earth-centered Earth-fixed (ECEF) coordinates. Taking the first node element Node0 in the original data list of the road network as an example, record its LLA coordinates as (lat0, lon0, alt0), and its ECEF coordinates (X0, Y0, Z0) can be obtained from Equation (1), where R NIndicates the principal curvature radius of the reference ellipsoid, which can be obtained from Equation (2). f = 0.0033528 represents the flattening of the reference ellipsoid, and R e = 6378137 (unit: meter) represents the equatorial plane major radius:

[0068]

[0069]

[0070] In the second step, convert the ECEF coordinates of all nodes into the north-east-down (ENU) coordinates. Select Node0 as the reference point and calculate the transformation matrix from the Earth-centered Earth-fixed coordinate system to the north-east-down coordinate system according to its LLA coordinates as:

[0071]

[0072] For the target point Node i , denote its LLA coordinates as (lat i , lon i , alt i ). The ECEF coordinates (X i , Y i , Z i ) can be calculated. Then, the ENU coordinates (E i , N i , U i ) of the target point can be calculated using Equation (4). Transform a certain regional road network map in the LLA coordinate system shown in Figure 3 to the ENU coordinate system as shown in Figure 4 .

[0073]

[0074] Step 3: Calculate the boundary of the regional road network map and perform whole-region adjustment.

[0075] The present invention traverses the internal nodes of each road element Way in the regional road network and calculates the boundary of the regional road network map in the ENU coordinate system. Denote the boundary of the road network map as East Min , East Max , North Min and North Max , where East Min and East Max represent the minimum and maximum values of the eastward boundary, and North Min and North Max represent the minimum and maximum values of the northward boundary. Taking the regional road network shown in Figure 4 as an example, its eastward boundaries are East Min= -141.923 and East Max = 553.884, the north boundary is respectively North Min = -308.434 and North Max = 664.127. To facilitate the construction of the road network image pyramid network in proportion, the road network boundary is adjusted for the whole area. The new boundary range after the whole area adjustment should include the original road network, and at the same time, the boundary value should be an integer multiple of the preset value Boulen. For the minimum boundary value, taking East Min as an example, its boundary East BMin after the whole area adjustment is calculated as shown in Equation (5), where floor() represents the floor function, and North BMin is calculated in the same way; for the maximum boundary value, taking East Max as an example, its boundary East BMax after the whole area adjustment is calculated as shown in Equation (6), where ceil() represents the ceiling function, and North BMax is calculated in the same way:

[0076] East BMin = floor(East Min ) * Boulen (5)

[0077] East BMax = ceil(East Max ) * Boulen (6)

[0078] Take Boulen = 40, Figure 4 after the whole area adjustment of each boundary of the regional road network shown, its east boundaries are respectively East BMin = -160 and East BMax = 560, and the north boundaries are respectively North BMin = -320 and North BMax = 680.

[0079] Step 4: Construct the base of the image pyramid.

[0080] The present invention constructs the base of the road network image pyramid with Num layers, and sets the side length of a single grid from the 1st layer to the Num layer of the base of the image pyramid as The concept of the formed pyramid grid is as Figure 5 shown. The pyramid base data required by the present invention is the data of each square node constituting the base. Taking the lower left corner of the pyramid base as the origin, the positive direction of the E-axis is the direction in which the eastward coordinate increases, and the positive direction of the N-axis is the direction in which the northward coordinate increases. The coordinates of the square node in the nth layer, the ith row, and the jth column are calculated as shown in Equation (7), where the value ranges of i and j are as shown in Equation (8):

[0081]

[0082]

[0083]

[0084] The number of grids at the bottom layer of the pyramid is small, and the number of grids to be traversed during the matching and positioning process is small, which is beneficial to accelerating the initial matching process and quickly determining the positioning hot zone; the number of grids at the high layer of the pyramid is large. Although the matching and positioning takes a long time, it can be accelerated by the positioning result of the hot zone of the low-layer grid to obtain a more accurate positioning result.

[0085] Step 5: Image a single road element in the road network map.

[0086] A single road element in the original road network map is an ordered node set, and the shape of the road is approximately represented by a broken line formed by connecting the nodes in the set in sequence. The advantage of the "point-line" topological road form is that the required storage is less, and it is suitable for road network modeling in large-scale scenarios. However, in the process of matching and positioning, it is often difficult to determine the corresponding relationship between road nodes and vehicle travel trajectory points, and the positioning accuracy is low or even unable to position.

[0087] The design of the present invention transforms the matching of the unordered point set of the road network and the ordered point set of the vehicle travel trajectory into an image matching problem. First, it is necessary to construct pyramid images of each road element as the matching benchmark. For a single road element, first perform a quadrilateral expansion on it to further simulate the real road shape. "Quadrilateral expansion" is a road simulation method proposed by relevant researchers in the industry. By sequentially performing rectangular expansion and quadrilateral expansion on the node connection line according to the road width, it can effectively restore the road shape. Taking the road shown by the Figure 4 rectangular frame as an example, the quadrilateral expansion result of this road is as shown in Figure 6 the figure.

[0088] The present invention images a single road element according to the coincidence situation between the quadrilateral expansion result of a single road and the pyramid base, and saves it as a single-channel image. The single-channel image is composed of a two-dimensional matrix. The pixel value of each pixel point represents its color depth, and the pixel value is between 0 and 255, where 0 represents black, 255 represents white, and the intermediate value is different levels of gray. For the single square node and the single quadrilateral expansion result of the nth layer pyramid base, first judge the inclusion relationship between the four vertices of the square node and the quadrilateral expansion result. Taking the upper left vertex as an example, denote it as P LU , and the four points of the quadrilateral are R A , R B , R C and R D in counterclockwise order. If and have the same sign, that is, all four values are positive or all four values are negative, then it is determined that the point P LU is located inside the quadrilateral; then calculate the intersection points of the line segments on each side of the square node and the line segments on each side of the quadrilateral; finally, calculate the area S of the polygon formed by all the intersection points and the included vertices Pol (If the sum of the number of intersection points and the included vertices is less than 3, then S Pol = 0). The side length of the single square at the base of the nth layer pyramid is BD / 2 n-1 , and the area is S Rec = BD 2 / 2 2n-2 . The pixel value pixel of this square node is calculated as follows:

[0089] piexl = 255 - 255 * S Pol / S Rec (9)

[0090] Traverse the square nodes in the pyramid base to calculate their pixel values, and then the road elements at this level can be visualized. The overlapping area of the square nodes located inside the quadrilateral expansion is large, and the pixels are shown as black, indicating a high possibility that the vehicle is driving in this area; the overlapping area of the square nodes located at the edge of the quadrilateral expansion is small, and the pixels are shown as gray of different degrees, and the smaller the overlapping area, the closer the pixel representation is to white, indicating a lower possibility that the vehicle is driving in this area, which is in line with the actual vehicle driving situation. Figure 4 For the road shown by the rectangular frame, the visualization results of its levels 1 to 4 are as Figure 7 shown, Figure 7 in (a) to Figure 7 in (d) respectively correspond to the bases of the 1st to 4th levels of the pyramid.

[0091] Step 6: Integrate the visualization results of each single road network element to form a regional road network pyramid image.

[0092] Traverse the visualization results of each single road element in the regional road network map and integrate them to form an overall regional road network pyramid image. For two pixels to be fused at the same position of a single pixel in the same level, the present invention designs the following fusion construction strategy:

[0093] If there is a pixel value of 255 in one of the two pixels to be fused, it means that there is no road visualization result at this position, and directly use the value of the other pixel as the fused pixel value.

[0094] If there is a pixel value of 0 in one of the two pixels to be fused, it means that there is a complete road visualization result at this position, and directly use 0 as the fused pixel value.

[0095] If both of the two pixels to be fused are neither 0 nor 255, it indicates that there are image results with different proportions at this position for both of them. The pixel value after half-and-half fusion is used as the value of the fused pixel.

[0096] For Figure 4 each road element within the regional road network shown, image processing and fusion are performed one by one. The fusion results at levels 1 to 4 are as shown in Figure 8 shown, Figure 8 in (a) to Figure 8 in (d) in

[0097] Step 7: Generate an initial vehicle travel trajectory using visual odometry and convert it to the geographic coordinate system.

[0098] Visual odometry is a mature method in the industry. Usually, a single or multiple cameras are used. Through steps such as image acquisition, feature extraction, inter-frame pose recovery, and pose optimization, the inter-frame pose difference is obtained, and the vehicle pose is recursively recovered to generate the vehicle travel trajectory. In the present invention, it is assumed that the initial position of the vehicle is the position of the first node of the first road element in the road set of the regional road network map. According to visual odometry, the trajectory {T wm , m = 0, 1, …, N f} of the vehicle in the world coordinate system (initial camera coordinate system) is obtained, where W represents the World system of the world coordinate system, and N f represents the number of image frames processed by the visual odometry. The trajectory {T wm , m = 0, 1, …, N f} is converted to the trajectory {T , m = 0, 1, …, N bm} in the body coordinate system (b system), where f represents the transformation matrix from the world coordinate system to the body coordinate system, which is obtained through actual measurement and includes the rotation vector and the translation vector The trajectory {T , m = 0, 1, …, N bm} is converted to the trajectory {T f , m = 0, 1, …, N } in the geographic coordinate system, where gm , m = 0, 1, …, N f} represents the transformation matrix from the body coordinate system to the geographic coordinate system, which is obtained from the known initial attitude angle of the vehicle and includes the rotation matrix and the translation matrix and the translation matrix

[0099] Step 8: Segment the initial vehicle travel trajectory.

[0100] The present invention designs a criterion for segmenting the vehicle driving trajectory, and segments the initial vehicle driving trajectory into straight segments and curved segments. The present invention takes the due north direction as the reference of the heading angle 0°, and the clockwise direction as the direction of increasing the heading angle, and the value range of the heading angle is 0 to 360°. The visual odometer obtains the initial vehicle driving trajectory {T gm , m = 0, 1, …, N f} in the geodetic coordinate system. Denote the coordinates of the vehicle at time t-1 and time t in the geodetic coordinate system as P t-1 =(E t-1 , N t-1 , U t-1 ) and P t =(E t , N t , U t ), where E t-1 and E t represent the eastward coordinates, N t-1 and N t represent the northward coordinates, and U t-1 and U t represent the upward coordinates. Define the vehicle heading direction at time t as the vector . The coordinate differences in its eastward and northward directions are as follows:

[0101] (E △ , N △ ) = (E t - E t-1 , N t - N t-1 ) (10)

[0102] The vector heading can be calculated by Equation (11), where dzero takes the value of 0.00000001.

[0103]

[0104] For the frame to be solved, calculate the difference dψ between its heading direction and the previous moment. If dψ < ψ sta , then it is determined that this frame belongs to a straight trajectory; if dψ > ψ sta , then it is determined that this frame belongs to a curved trajectory, where ψ sta is a preset value. It should be particularly noted that during driving, the vehicle may make short-term sharp turns to avoid emergencies. The present invention only determines that the section where multiple consecutive frames belong to the curved trajectory is a curved section. Segment the initial vehicle driving trajectory, and the segmentation result is as shown in Figure 9 .

[0105] Step 9: Form a "straight segment sandwiching a curved segment" trajectory and visualize the straight segment of the vehicle driving trajectory.

[0106] For a single long straight trajectory, the road section to which the vehicle belongs can be matched according to its heading, but its specific position within the road section cannot be determined. After passing through a curved trajectory, the driving direction of the vehicle changes. The long straight trajectories before and after the curved trajectory can form multiple direction constraints to match and locate the specific position of the vehicle. The present invention repeatedly executes Step 7 and Step 8 until a "long straight sandwiching a curve" trajectory is formed. A typical "long straight sandwiching a curve" trajectory is as Figure 9 shown. There is an obvious heading difference between the two long straight trajectories before and after the curved trajectory.

[0107] Similar to the method of image pyramiding of the road network, calculate the boundary of the vehicle driving trajectory map according to the method of Step 3 and perform whole-region adjustment. Construct the base of the vehicle driving trajectory image pyramid according to the method of Step 4, and perform quadrilateral expansion on the long straight trajectory by using the vehicle width instead of the road width. The long straight trajectory has stability and can provide stable information to be matched. The curved trajectory is greatly affected by the driver's operating habits and actual road conditions and cannot provide stable information to be matched, so it is discarded. The method of image pyramiding of the vehicle driving trajectory is different from the method of image pyramiding of a single road element and does not need to consider the marginalization problem. For a single square node and a single quadrilateral expansion result of the nth layer base of the pyramid, if the center point of the square node is included in the quadrilateral expansion result, set its pixel value to 0, and the pixel appears black, indicating that the vehicle passes through this area during driving; if the center point of the square node is not included in the quadrilateral expansion result, set its pixel value to 255, and the pixel appears white, indicating that the vehicle does not pass through this area during driving. Image pyramiding Figure 9 The "long straight sandwiching a curve" trajectory shown, its image pyramiding results from level 1 to 4 are as Figure 10 shown, Figure 10 in (a) to Figure 10 in (d) respectively correspond to the bases of the 1st to 4th levels of the pyramid.

[0108] Step 10: Layer by layer, match the regional road network and the vehicle driving trajectory pyramid image to locate the vehicle.

[0109] The present invention traverses the pyramid images of the regional road network, and matches and locates the vehicle by taking the vehicle trajectory images at the corresponding levels. For the road network images and vehicle trajectory images at the same level, starting from the upper left corner, all positions are traversed, and the sum of the numerical differences of the pixels in the overlapping part of the vehicle trajectory image and its position in the road network image is calculated. The present invention is designed to start the traversal and matching from the lower levels of the pyramid, and the images at the higher levels are slightly offset based on the matching results at the lower levels to accelerate the matching and positioning process. Since the width of the vehicle is smaller than the road width, there are multiple groups of sums of pixel numerical differences that are the same and are the smallest during the traversal process. The present invention takes the average value of the deviation amounts as the final value of the matching and positioning result, and the center coordinates of the square node corresponding to the pixel point corresponding to the starting point of the vehicle trajectory are the initial position of the vehicle. In a road network with a relatively high similarity, there may be multiple matching results for a simple "long straight with a bend" trajectory. Taking Figure 4 the road network shown and Figure 9 the vehicle trajectory shown as an example, there are multiple matching and positioning results as shown by the Figure 11 circle markings. By increasing the number of subsequent long straight trajectories, incorrect matching items can be eliminated. As shown in Figure 11 , the three long straight trajectories are mutually constrained, and the initial position of the vehicle can be effectively matched and located. The part with higher transparency in the figure is the road network, and the part with lower transparency is the matching and positioning result of the vehicle trajectory on the road network.

Claims

1. A vehicle positioning method based on vehicle trajectory image matching, characterized in that, The following steps are involved: (1) Loading a regional road network map based on the approximate location of the vehicle at the initial moment; (2) Convert the regional road network map to the Northeastern geographic coordinate system; (3) Calculate the boundaries of the regional road network map to adjust the entire area and construct the road network image pyramid base; (4) Single road element in the visualized road network map; (5) Visualize all single road elements in the regional road network and fuse the single road element images at each level to construct a road network pyramid image; (6) Generate the initial vehicle trajectory using the visual odometry and convert it to the geographic coordinate system; (7) Dividing the initial vehicle trajectory into a long straight trajectory and a curved trajectory according to the angular velocity of the heading angle change between frames; (8) Repeating steps (6) and (7) until a "long straight section with a curved section" track is formed, and performing image processing on the long straight section of the vehicle track; (9) Match regional road network images and vehicle trajectory images layer by layer to locate vehicles; The implementation process of step (4) is as follows: A single road element is quadrilaterally expanded according to the road width, and a single road element is visualized according to the quadrilateral expansion result and the overlap of pyramid grids at each level, and finally saved as a single-channel image; a single grid in the pyramid grid is regarded as a pixel, and the pixel value is between 0 and 255, where 0 represents black, 255 represents white, and the intermediate values are different levels of gray; For the single square node and the single quadrilateral expansion result of the base of the nth layer pyramid, first judge the inclusion relationship between the four vertices of the square node and the quadrilateral expansion result; for the upper left vertex P LU , the four points of the quadrilateral are R A , R B , R C and R D in counterclockwise order. If and have the same sign, that is, all four values are positive or all four values are negative, then it is determined that the point P LU is located inside the quadrilateral; then calculate the intersection points of the line segments of each side of the square node and the line segments of each side of the quadrilateral; finally calculate the area S Pol of the polygon formed by all the intersection points and the included vertices; the side length of the single square at the base of the nth layer pyramid is BD / 2 n-1 , and the area is S Rec = BD 2 / 2 2n-2 . The pixel value pixel of this square node is as follows: piexl = 255 - 255 * S Pol / S Rec (9) Traverse the square nodes within the pyramid base to calculate their pixel values, and the road elements at this level can be visualized accordingly.

2. The vehicle positioning method based on vehicle trajectory image matching according to claim 1, characterized in that, The regional road network map in step (1) is a "point-line" topological map, which is composed of an indefinite number of single road elements Way. Way is an ordered set composed of a series of nodes Node, and the roads are represented by broken line segments connected by the nodes in the set; the data of a single node Node includes its latitude lat, longitude lon and altitude alt.

3. A vehicle positioning method based on vehicle trajectory image matching according to claim 1, characterized in that, The implementation process of step (2) is as follows: Convert the LLA coordinates of all internal nodes of the Way element to the corresponding Earth-centered Earth-fixed coordinates. For the first node Node0 in the original road network data list, its LLA coordinates are (lat0, lon0, alt0), and its ECEF coordinates (X0, Y0, Z0) are: where R N represents the principal curvature radius of the reference ellipsoid, f is the flattening of the reference ellipsoid, and R e is the major radius of the equatorial plane; Convert the ECEF coordinates of all nodes to the north-east-down coordinates. Select Node0 as the reference point and calculate the transformation matrix from the Earth-centered Earth-fixed coordinate system to the north-east-down coordinate system based on its LLA coordinates For the target point Node i , denote its LLA coordinates as (lat i , lon i , alt i ). Calculate its ECEF coordinates as (X i , Y i , Z i ). Then the ENU coordinates (E i , N i , U i ) of the target point are as follows:

4. A vehicle positioning method based on vehicle trajectory image matching according to claim 1, characterized in that, The implementation process of step (3) is as follows: Traverse the internal nodes of each road element Way in the regional road network, calculate the boundaries of the regional road network map in the ENU coordinate system, and perform whole-region adjustment on it, including the maximum north coordinate North BMax , the minimum north coordinate North BMin , the maximum east coordinate East BMax and the minimum east coordinate East BMin ; Construct the base of the road network image pyramid with Num layers, and set the side length of a single grid from the 1st layer to the Num layer of the image pyramid base to be The number of grids increases proportionally; the pyramid base data is the data of each square node that makes up the base. Taking the lower left corner of the pyramid base as the origin, the positive direction of the E-axis is the direction in which the eastward coordinate increases, and the positive direction of the N-axis is the direction in which the northward coordinate increases. The coordinates of the square node in the nth layer, the ith row, and the jth column are calculated as shown in Equation (7), where the value ranges of i and j are shown in Equation (8):

5. The vehicle positioning method based on vehicle travel trajectory image matching according to claim 1, characterized in that, The implementation process of step (5) is as follows: Design a road network pyramid image fusion construction strategy. For two pixels to be fused at a single pixel position in the same level: if one of the two pixels to be fused has a pixel value of 255, it means that there is no road imaging result at this position, and the other pixel value is directly used as the fused pixel value; if one of the two pixels to be fused has a pixel value of 0, it means that there is a complete road imaging result at this position, and 0 is directly used as the fused pixel value; If both pixels to be fused are not 0 or 255, it means that the two have different proportions of visualization results at this position, and the half-fused pixel value is used as the fused pixel value; the visualization results of each single road element in the regional road network map are traversed and fused to form the overall regional road network pyramid image.

6. The vehicle positioning method based on vehicle travel trajectory image matching according to claim 1, characterized in that, The implementation process of step (7) is as follows: According to the inter-frame pose change output by the visual odometer, calculate the angular velocity of the inter-frame heading angle change dψ; if dψ < ψ sta , then determine that this frame belongs to a long straight trajectory; if dψ > ψ sta , then determine that this frame belongs to a curved trajectory, where ψ sta is a preset value.

7. A vehicle positioning method based on vehicle trajectory image matching according to claim 1, characterized in that The implementation process of step (8) is as follows: The method of visualizing vehicle trajectories does not consider the marginalization problem. For a single square node and a single quadrilateral expansion result at the base of the nth layer pyramid, if the center point of the square node is included in the quadrilateral expansion result, set its pixel value to 0, indicating that the vehicle passes through this area during driving; if the center point of the square node is not included in the quadrilateral expansion result, set its pixel value to 255, indicating that the vehicle does not pass through this area during driving.

8. A vehicle positioning method based on vehicle track image matching according to claim 1, characterized in that The implementation process of step (9) is as follows: Traverse the pyramid image of the regional road network, and take the vehicle trajectory image corresponding to the same level to match and locate the vehicle; for the road network image and the vehicle trajectory image at the same level, start traversing all positions from the upper left corner, and calculate the sum of the numerical differences of the pixels in the overlapping part of the vehicle trajectory image and its position in the road network image; Priority is given to starting the traversal and matching from the lower levels of the pyramid. The images at the higher levels are slightly offset based on the matching results at the lower levels to accelerate the matching and positioning process; take the average value of the deviation as the final value of the matching and positioning result.

Images