A road network map aided vehicle positioning method based on pose graph optimization

Abstract

This invention discloses a road network map-assisted vehicle localization method based on pose graph optimization, comprising the following steps: S1, loading a regional road network map according to the vehicle's initial global position; S2, converting the regional road network map into a road network primitive map; S3, calculating the connectivity between different road network primitives; S4, determining whether to increase the number of skeleton points within each road network primitive based on the distance between adjacent skeleton points within each road network primitive; S5, calculating the initial transformation matrix T. w g S6, the basic unit of the road network to which the forward-oriented vehicle belongs; S7, according to T w g The visual odometry output pose is converted to the northeast-central coordinate system; S8, discrimination conditions are set to filter map correction points; S9, a pose graph optimization model of the trajectory is constructed to correct historical trajectories; S10, the vehicle pose at the latest moment is predicted; S11, steps S7 to S10 are repeated until the vehicle stops. This invention can improve the positioning accuracy of the vehicle and provide long-term positioning assurance.

Description

Technical Field

[0001] This invention relates to the field of map matching and navigation positioning, and in particular to a method for vehicle positioning assisted by road network maps based on pose graph optimization. Background Technology

[0002] High-precision vehicle navigation and positioning technology is crucial for completing various engineering tasks. The Global Positioning System (GPS) utilizes satellites in Earth orbit to provide location information and is widely used in vehicle navigation systems due to its cost-effectiveness. However, in situations where satellite signals are denied, vehicles face significant positioning difficulties. Cameras, with their ease of use, lightweight design, and cost-effectiveness, are widely used to solve vehicle positioning problems. They track vehicle movement by inputting a series of images into a Visual Odometry (VO) system. Visual Odometry relies on dead reckoning; however, as time and distance increase, small errors gradually accumulate, leading to a decrease in positioning accuracy.

[0003] To address the visual odometry drift problem, some researchers have proposed methods based on Visual Simultaneous Localization and Mapping (VSLAM). However, this method requires the vehicle to repeatedly traverse historical scenes to construct closed loops and reduce localization errors, thus affecting the vehicle's driving planning. Other studies combine visual odometry with additional information from other sensors to mitigate the drift problem, but introducing additional sensors significantly increases the system's hardware cost and data processing complexity. Summary of the Invention

[0004] Purpose of the invention: The purpose of this invention is to provide a road network map-assisted vehicle localization method based on pose graph optimization that can provide stable and accurate vehicle position estimation in real time.

[0005] Technical Solution: The road network map-assisted vehicle positioning method of the present invention, given the initial position, initial attitude angle, and road network primitives to which the vehicle belongs, introduces constraints from the road network map to suppress the cumulative error of visual odometry. Combining the influence of the original visual odometry trajectory and map correction points, a corrected trajectory is output. Finally, the vehicle positioning result is predicted based on the visual odometry calculation result and the corrected trajectory. The method includes the following steps:

[0006] S1, Load the regional road network map based on the vehicle's initial global location;

[0007] S2 uses road intersections as support points and roads between adjacent support points as road network primitives to transform regional road network maps into road network primitive maps.

[0008] S3, calculate the connectivity between different road network primitives based on the positional relationship between the first and last nodes of the road network primitives;

[0009] S4. Based on the distance between adjacent skeleton points within each network element, determine whether it is necessary to increase the number of skeleton points within each network element.

[0010] S5. Calculate the initial transformation matrix based on the vehicle's initial attitude angle and sensor extrinsic parameters.

[0011] S6, based on the initial driving direction of the vehicle, orient the road network element to which the vehicle belongs;

[0012] S7, according to Convert the visual odometry output pose to the northeast-sky coordinate system;

[0013] S8 introduces constraints from the road network map, sets judgment conditions, and filters map correction points;

[0014] S9. Construct a pose graph optimization model for the trajectory, add local visual factors and global map correction point factors, and correct the historical trajectory by solving the pose graph optimization problem of the trajectory.

[0015] S10, predicts the vehicle's latest position and orientation based on visual odometry calculations and corrected trajectories;

[0016] S11, Receive visual odometer output, repeat steps S7 to S10 until the vehicle stops.

[0017] Furthermore, in step S2, the road network support point is defined as an inflection point TP. The first and last nodes of any road network element are inflection points, and the inflection points contain the topological connection relationships between different road network elements. Inside the road network element, the road shape is approximated by a series of nodes connected by lines, and this series of nodes is defined as the skeleton points SP. The structural composition of a single road network element is then represented as follows:

[0018] {TP Start SP1, SP2, ..., SP n ,TP End}

[0019] Among them, TP Start Indicates the inflection point of the road network's foundation, TP End Represents the end inflection point of the road network primitive, {SP1,SP2,…,SP n} represents the skeleton point group of road network primitives;

[0020] If the skeleton point group is empty, then the orientation of the head and tail of this road network element are both... If the skeleton point group is not empty, then the orientation of the head of this road network element is... Tail direction is

[0021] Furthermore, in step S3, for a single road network element, its first inflection point TP is extracted first. Start Tail-end inflection point TP End Iterate through the first and last inflection points of the remaining road network primitives within the road network primitive set:

[0022] If there exists a first inflection point of a certain road network element and TP Start or TP End If they are located at the same road intersection, then these two road network elements are considered to be connected;

[0023] Alternatively, if there exists a tail inflection point of a certain road network element that is related to TP Start or TP End If they are located at the same road intersection, then these two road network elements are considered to be connected.

[0024] Furthermore, in step S4, the detailed steps for determining whether to increase the number of skeleton points within each network element based on the distance between adjacent skeleton points within each network element are as follows:

[0025] S41, for the road network primitive {TP} Start SP1, SP2, ..., SP n ,TP End}, decompose it into a group of line segments {TP Start -SP1, SP1-SP2, ..., SP n -TP End};

[0026] S42, calculate the polyline distance Len for each polyline segment. z , where z = 1, 2, ..., n+1; if Len z <15, indicating a small distance between the two nodes, no additional skeleton points are needed; if Len z If the value is greater than 15, then more skeleton points need to be added.

[0027] Furthermore, in step S6, the implementation process of the road network primitive to which the forward vehicle belongs is as follows:

[0028] The road network primitive element to which the vehicle initially belongs is denoted as RNBE. Init The vehicle is initially stationary, and its initial position is denoted as P. w-init After transformation matrix Position P converted to ENU coordinate system g-init ;

[0029] The position of a vehicle where the cumulative displacement first exceeds 2 meters is recorded as P. w-end Transformed matrix The position is P in the ENU coordinate system.g-end ; Transitioning to the ECEF coordinate system, P g-init and P g-end Convert to LLA coordinates P L-init and P L-end ;

[0030] Calculate the vehicle's direction of travel Positive direction of the corresponding broken line The included angle is θ + Calculate the vehicle's direction of travel Reverse of the corresponding broken line The included angle is θ - :

[0031] If θ + >θ - Maintain RNBE Init The original structure remains unchanged; if θ + <θ - Reverse adjustment of RNBE Init The original structure is aligned with the vehicle's direction of travel.

[0032] Furthermore, the cumulative vehicle displacement LenAll(P) w1 ,P wm The calculation expression for ) is as follows:

[0033] LenAll(P w1 ,P wm ) = Len(P w1 ,P w2 )+Len(P w2 ,P w3 )+…+Len(P wm-1 ,P wm )

[0034] Where Len() represents the straight-line distance between two points, P wm This represents the position of the vehicle in the camera's world coordinate system at frame m.

[0035] Furthermore, in step S8, the filtering correction points include filtering the TP correction points at the end of the road network element and filtering the SP correction points inside the road network element.

[0036] S81, the TP correction point includes a steering TP correction point and a near-straight TP correction point, which correspond to the steering network primitive and the near-straight network primitive, respectively; the implementation process is as follows:

[0037] S811, configured with only a single pre-driving road network element.

[0038] S8111, if the pre-driving road network element is a steering road network element, the orientation of the head of the pre-driving element is the same as that of RNBE. CurThe angle between the tail and the included angle is Calculate the vehicle's current driving direction and RNBE respectively. Cur If the rear-end orientation angle β1 and the vehicle's current driving direction and the pre-driving element's front-end orientation angle β2 are satisfied, then the steering TP correction point screening condition is met; condition A is as follows:

[0039]

[0040] Among them, RNBE Cur This refers to the basic element of the road network to which the vehicle currently belongs;

[0041] After shifting the TP along the orientation of the steering network primitive head by half the road width, it is particle-ized and finally RNBE is used. Cur Using the first TP as a reference, the similarity calculation method is applied to find the particles that meet the set conditions, which are then used as the TP correction points for turning.

[0042] S8112, if the pre-driving primitive is a near-linear primitive, record the vehicle's previous moment and RNBE. Cur The distance of the first TP is Len VO_Last The vehicle's current time and RNBE Cur The distance of the first TP is Len VO_Cur Assuming the vehicle is moving at a constant speed, predict the next moment's distance between the vehicle and RNBE. Cur The distance of the first TP is Len VO_Next RNBE Cur The distance between the first and last TP is Len. TPs If condition B or C is met, then the near-straight TP correction point screening condition is met.

[0043] Condition B is: (Len) VO_Last <Len TPs )&&(Len VO_Cur >Len TPs )

[0044] Condition C is:

[0045] Condition B indicates that the vehicle has passed through the intersection for the first time; Condition C indicates that although the vehicle has not passed through the intersection at the current moment, it is predicted that the vehicle will pass through in the next moment, and the vehicle will be closer to the tail TP in the next moment.

[0046] Granular RNBE Cur The tail TP, and finally RNBE Cur Using the first TP as a reference, the particle that meets the set conditions is obtained by applying the similarity calculation method, which is the near-straight TP correction point;

[0047] S812 has multiple pre-driving primitives.

[0048] If the vehicle meets the steering TP correction conditions, it is determined that the vehicle has entered this steering network element;

[0049] If a vehicle meets the near-straight TP correction conditions, the possibility of the vehicle entering other road network elements is retained until the vehicle passes the tail TP by more than 10 meters and the heading is less than 10° from the head of the near-straight road network element. Then the vehicle is determined to have entered this near-straight road network element.

[0050] S82, SP correction point filtering

[0051] Let the current vehicle belong to the primitive skeleton point group {SP1,SP2,…,SP1}. n}, let SP and RNBE be used. Cur The distance of the first TP is Len SP-TP If condition D or E is met, then the SP correction point screening condition is met.

[0052] Condition D is: (Len VO_Last <Len SP-TP )&&(Len VO_Cur >Len SP-TP )

[0053] Condition E is:

[0054] Starting from the end of the skeleton point group, the SPs that meet the screening criteria are filtered in reverse order, and then denoted by RNBE. Cur Using the first TP and the last TP as references, the similarity calculation method is applied to obtain the particles that meet the set conditions, which are the SP correction points.

[0055] Furthermore, in step S9, the vehicle pose at each moment is set as a node in the pose graph. The local constraints between two consecutive nodes are provided by the VO system, and the global constraints for indefinite intervals are provided by map correction points. A pose graph optimization problem with only the trajectory is constructed, and the pose graph optimization problem is transformed into a maximum likelihood estimation problem. The maximum likelihood estimation consists of the joint probability distribution of the vehicle over a period of time, and the variable is the global pose of all nodes, T. g ={T g0 ,T g1 ,T g2 ,…,T gl}, where T gl ∈SE(3) represents the vehicle's pose in the geographic system at time l;

[0056] The maximum likelihood estimation problem, assuming all measurement probabilities are independent, is as follows:

[0057]

[0058] Where S is a set of measurements, including the VO system output and map correction point measurements, and T represents the optimal estimate of the global pose of all nodes. This represents the measurement value that can be observed at time t.

[0059] Compared with the prior art, the significant advantages of this invention are as follows:

[0060] 1. This invention provides a road network map-assisted vehicle positioning scheme. Based on the initial trajectory provided by visual odometry, constraints are introduced from the road network map to suppress the cumulative error of visual odometry, which can effectively improve the positioning accuracy of vehicles and provide long-term positioning assurance.

[0061] 2. This invention relies solely on a "point-line" road network map, requiring no richer map information and minimal map data, making it suitable for a wide range of scenarios;

[0062] 3. This invention designs a road network map format—a road network primitive diagram—to meet positioning needs, which can accurately describe the shape of a single road network primitive and the topological relationship between each road network primitive;

[0063] 4. This invention designs a length / angle joint judgment criterion based on the road network primitive map, which comprehensively considers the length similarity and angle similarity between the visual odometry pose and the key nodes of the road network primitive map, and can accurately select map correction points from the road network primitive map.

[0064] 5. This invention constructs a pose optimization factor graph model, which comprehensively considers the short-term accuracy of visual factors and the global consistency of map correction point factors, and can optimize historical trajectories to obtain the best historical trajectory.

[0065] 6. This invention designs an optimized prediction model, which optimizes the original trajectory of the visual odometer by introducing the constraint of the map correction point factor, thereby obtaining the corrected trajectory, and then predicts the vehicle positioning result based on the visual odometer calculation result and the corrected trajectory. Attached Figure Description

[0066] Figure 1 This is a schematic diagram of the overall scheme of the present invention;

[0067] Figure 2 This is a schematic diagram of the OSM map;

[0068] Figure 3 This is a schematic diagram of the road network map;

[0069] Figure 4 This is a schematic diagram of the road network's basic elements;

[0070] Figure 5 This is a schematic diagram of node particleization;

[0071] Figure 6This is a diagram illustrating node similarity.

[0072] Figure 7 This is a diagram illustrating a road intersection.

[0073] Figure 8 A global pose factor map for a road network map-assisted vehicle positioning system;

[0074] Figure 9 The image shows the effect of the correction based on the angle criterion;

[0075] Figure 10 This is a diagram showing the effect of the correction based on the length criterion. Detailed Implementation

[0076] The present invention will now be described in further detail with reference to the accompanying drawings and specific embodiments.

[0077] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. It should also be understood that terms such as those defined in general dictionaries should be understood to have the same meaning as in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as herein.

[0078] The technical solution of the present invention will be further described in detail below with reference to the accompanying drawings:

[0079] like Figure 1 As shown, the essence of this invention is to design an optimized prediction model that, given the vehicle's initial position, initial attitude angle, and the road network primitives to which the vehicle belongs, introduces constraints from the road network map to suppress the accumulated error of visual odometry, providing stable and accurate vehicle position estimation in real time. Detailed steps are as follows:

[0080] Step 1: Load the regional road network map based on the vehicle's initial global location;

[0081] Given the vehicle's initial geographical location, a regional road network map is downloaded from the OpenStreetMap website based on its latitude and longitude. OSM collects data from free sources such as manual surveying, satellite surveying, and aerial photography; this data is available as an open database for use by users worldwide. The OpenStreetMap website map is shown below. Figure 2As shown, the website uses a topological data structure to represent roads, containing three core elements: node, way, and relation. A node represents a point on the map with a geographical location, defined using latitude and longitude, denoted as node = (lat, lon), where lat represents the latitude coordinate of the node and lon represents the longitude coordinate. A way is an ordered list of nodes, representing a polyline segment on the map. A relation represents the relationship between existing nodes and ways. Selecting a region with high road density, the loaded regional road network map is as follows: Figure 3 As shown.

[0082] Step 2: Convert the regional road network map into a road network primitive map;

[0083] To simplify and efficiently utilize road network map information, this invention proposes a road network model that uses road intersections as support points and roads between adjacent support points as RoadNet Basic Elements (RNBEs). Furthermore, road network support points are defined as Turning Points (TPs), and the first and last nodes of any RoadNet Basic Element are turning points, containing the topological connections between different RoadNet Basic Elements. Within each RoadNet Basic Element, the road shape is approximated by a series of connected nodes; this series of nodes is defined as Skeleton Points (SPs). In summary, the structural composition of a single RoadNet Basic Element is represented as follows:

[0084] {TP Start SP1, SP2, ..., SP n ,TP End} (1)

[0085] Among them, TP Start Indicates the inflection point of the road network's foundation, TP End Represents the end inflection point of the road network primitive, {SP1,SP2,…,SP n} represents the skeleton point group of the road network primitive. If the skeleton point group is empty, then the orientation of the head and tail of this road network primitive is both empty. If the skeleton point group is not empty, then the orientation of the head of this road network element is... Tail direction is

[0086] Intersections of way elements within a region are road network junctions. These intersections are extracted as inflection points, and the road network is divided into multiple road network primitives. Nodes within these primitives are skeleton points. For example... Figure 4The diagram shows the road network of a certain area under the road network model designed in this embodiment. The original road network is represented by 12 independent road network primitives. Road network primitives with the same inflection point endpoints are connected. The internal shape of the road network primitives is approximated by the connection of skeleton points. There are fewer skeleton points inside the near-straight road network primitives and more skeleton points inside the arc road network primitives, so as to maximize the similarity between the interconnected polyline segments of skeleton points and the original road trajectory.

[0087] Step 3: Calculate the connectivity between different road network primitives;

[0088] For a single road network primitive, first extract its first inflection point TP. Start Tail-end inflection point TP End Iterate through the first and last inflection points of the remaining road network primitives within the set of road network primitives. If there exists a first or last inflection point of a certain road network primitive that is related to TP... Start (or TP) End If two road network primitives are located at the same road intersection, then these two examples are considered connected. Repeat the above process, traversing the set of road network primitives, and calculate the connectivity between all road network primitives within the regional road network.

[0089] Step 4: Increase the number of skeleton points within each network element;

[0090] Step 2 mentions that the number of skeleton points within the near-straight road network primitives is relatively small because the road shape in near-straight road segments is simple, and a small number of node connections can accurately reconstruct the road shape. This invention increases the number of skeleton points within each road network primitive, providing more data support for trajectory correction.

[0091] For the road network element {TP} Start SP1, SP2, ..., SP n ,TP End It can be decomposed into a group of broken line segments:

[0092] {TP Start -SP1, SP1-SP2, ..., SP n -TP End} (2)

[0093] Using the broken line segment TP Start Taking SP1 as an example, the method for adding skeleton points is explained as follows:

[0094] TP Start The coordinates are (lat1, lon1), where lat1 represents TP. Start The latitude coordinates, lon1 represents TP Start The longitude coordinates of SP1. Let the coordinates of SP1 be (lat2, lon2), where lat2 represents the latitude coordinates of SP1 and lon2 represents the longitude coordinates of SP1. This broken line segment TP Start-SP1 The latitude distance is shown in Equation (3), and the longitude distance is shown in Equation (4), where Meter_Latitude represents the distance difference of 1 second in latitude, which is generally taken as 30.8702623.

[0095] Len lat =|lat1-lat2|*3600*Meter_Latitude (3)

[0096] Len lon =|lon1-lon2|*3600*Meter_Latitude*cos(lat1) (4)

[0097] The expression for the broken line distance is as follows:

[0098]

[0099] If Len1 < 15, the distance between the two nodes is small, and no additional skeleton points are needed; if Len1 > 15, the number of skeleton points to be added is calculated as follows:

[0100] Num = (Len1 / 10 + 0.5) floor (6)

[0101] in,() floor This indicates rounding down to the nearest integer.

[0102] The coordinates of the added skeleton points are:

[0103]

[0104] Where i = 1, 2, ..., Num.

[0105] The method for adding skeleton points to other polyline segments is the same as for polyline segment TP. Start -SP1.

[0106] Step 5: Calculate the initial transformation matrix based on the vehicle's initial attitude angles and sensor extrinsic parameters.

[0107] Visual odometry outputs the pose as the pose of the current frame relative to the camera coordinate system (world coordinate system) of the first frame, denoted as T. w Including the rotational component R w Translation component t w The initial attitude angles of the vehicle are known, and the transformation matrix can be obtained from the initial heading angle ψ, pitch angle θ, and roll angle γ. The vehicle body coordinate system is taken as the common "right front upper" coordinate system, and the world coordinate system is taken as the common "right lower front" coordinate system, from which the rotation matrix can be obtained. The translation matrix can be obtained from the camera's mounting position. United Initial transformation matrix The calculation method is as follows:

[0108]

[0109] in,() -1 This represents finding the inverse of a matrix.

[0110] Step 6: Based on the initial travel direction of the vehicle, orient the road network elements to which the vehicle belongs;

[0111] The road network primitive element to which the vehicle initially belongs is denoted as RNBE. Init The vehicle's segment can be further determined based on the output of step 4, let's assume it's SP. a -SP b The vehicle is initially stationary, and its initial position is denoted as P. w-init After transformation matrix Position P converted to ENU (Northeast-Northwest) coordinate system g-init At this time, due to the visual odometry error, the vehicle position moves irregularly near the starting point. The cumulative displacement of the vehicle from the initial frame to the m-th frame is calculated as shown in equation (9):

[0112] LenAll(P w1 ,P wm ) = Len(P w1 ,P w2 )+Len(P w2 ,P w3 )+…+Len(P wm-1 ,P wm (9)

[0113] Where Len() represents the straight-line distance between two points, P wm This represents the position of the vehicle in the camera's world coordinate system at frame m.

[0114] The location of the vehicle where the cumulative displacement first exceeds 2 meters is designated as P. w-end Transformed matrix Position P converted to ENU coordinate system g-end The coordinates of the road network's basic nodes are LLA (latitude, longitude, and altitude) coordinates. The transformation between the LLA and ENU coordinate systems is non-linear. The P coordinate system is then transitioned to the ECEF (Earth-Centered, Earth-Fixed) coordinate system. g-init and P g-end Convert to LLA coordinates P L-init and P L-end Calculate the vehicle's direction of travel separately. Positive direction of the corresponding broken line The included angle is θ + Calculate the vehicle's direction of travel Reverse of the corresponding broken line The included angle is θ - If θ + >θ - , keep RNBE Init The original structure remains unchanged; if θ + <θ - Reverse adjustment of RNBE Init The original structure is aligned with the vehicle's direction of travel.

[0115] Step 7, based on the transformation matrix Convert the visual odometry output pose to the northeast-sky coordinate system;

[0116] The world coordinate system pose T output by the visual odometry w Conversion to Northeast Celestial Coordinate System Pose T g Including rotation vector R w Turn R g Translation vector t w Turn t g It should be noted that here... The transformation matrix from the camera world coordinate system to the northeast sky coordinate system is obtained in step 5 at the initial moment.

[0117] Step 8: Import constraints from the road network map, set judgment conditions, and filter map correction points;

[0118] The selection of correction points includes the selection of TP correction points at the end of the road network element and the selection of SP correction points within the road network element. The TP correction point will also be responsible for the replacement of the road network element to which the vehicle currently belongs. The selection process for the two types of correction points is explained below.

[0119] First, we introduce a common particle-based method and similarity calculation method for TP (Track Point) and SP (Site Point) correction points. For point-to-line road network maps, TP is usually located at the center of road intersections, and SP is usually located at the road centerline. Due to the uncertainty of vehicle movement, it is difficult to ensure that vehicles travel on the road centerline. This invention designs particle-based road network nodes to cover all possible vehicle positions.

[0120] like Figure 5 As shown, the central pentagram represents a road network node, and the dots represent particles derived from that node. Assume the node coordinates are (lat... z ,lon z ), where lat z This represents the latitude coordinates of this node, lon. zThis represents the longitude coordinates of the node. The distance Dis from the particle to the node follows a Gaussian distribution with a mean of 0 and a standard deviation of one-sixth of the road width (Road_Width). Since the node is located in the center of the road, half the road width is the distance from the node to the road edge. Utilizing the property of the Gaussian distribution—99.73% of the data points falling within the interval (μ-3σ, μ+3σ)—this method ensures that the vast majority of particles are within the road area, with more particles closer to the road centerline. The angle between the line connecting the node and the particle and the positive longitude direction is a random variable ranging from 0 to 2π. Assuming we take θ, then the latitude of the particle is lat. p The calculation is shown in equation (10), longitude lon p The calculation is shown in equation (11):

[0121] lat p =lat z +Dis·sinθ / Meter_Latitude / 3600 (10)

[0122] lon p =lon z +Dis·cosθ / Meter_Latitude / 3600 / cos(lat p (11)

[0123] Meter_Latitude represents the distance difference in latitude per second, typically taken as 30.8702623.

[0124] like Figure 6 As shown, P1 and P2 are two derived particles of the skeleton point, and T1 and T2 are the beginning and end inflection points of this road network primitive. Considering the direction of the connection, P1 is more similar to the output position of VO (Visual Odometry); considering the length of the connection, P2 is more similar to the output position of VO.

[0125] With T1 as the reference point, the relative error between P1 and VO is shown in equation (12), and the angular error is shown in equation (13):

[0126]

[0127]

[0128] Similarity measurement between P1 and VO relative to reference point T1 According to the error definition settings in equations (12) and (13), the calculation method is shown in equation (14).

[0129]

[0130] in α represents the length factor, which takes a value of 0 to 1.

[0131] The following sections will introduce the methods for selecting TP correction points and SP correction points.

[0132] (1) TP correction point selection method

[0133] like Figure 7 The image shows a road intersection area on a road network map. The arrows indicate the direction of vehicle travel. The current road network primitive is denoted as RNBE. Cur In the intersection area, vehicles have three selectable road network primitives, labeled RNBE1, RNBE2, and RNBE3. When a vehicle's assigned road network primitive changes, pre-driving road network primitives are obtained based on the connectivity calculated in step 3 and then forward-oriented. Pre-driving road network primitives with a head-to-tail angle difference greater than 40° from the current road network primitive are labeled as turning road network primitives, such as... Figure 7 RNBE1 and RNBE3 in the diagram; pre-driving road network elements with an angle difference of less than 40° are marked as near-straight road network elements, such as... Figure 7 RNBE2 in.

[0134] The TP correction points designed in this invention include steering TP correction points and near-straight TP correction points, corresponding to steering road network primitives and near-straight road network primitives, respectively. Assuming only a single pre-driving road network primitive, then:

[0135] If the pre-driving road network element is a steering road network element, the orientation of the head of the pre-driving road network element is the same as that of RNBE. Cur The angle between the tail and the included angle is Calculate the vehicle's current driving direction and RNBE respectively. Cur Calculate the rear-end orientation angle β1 and the angle β2 between the current driving orientation of the vehicle and the front orientation of the pre-driving element. If equation (15) is satisfied, then the steering TP correction point screening condition is met.

[0136]

[0137] The above conditions can only be met after the vehicle has completed most of the steering actions. This invention particles the TP after shifting it along the steering network element head by half the road width, such as... Figure 7 The black circles mark RNBE1 and RNBE3. Finally, RNBE... Cur Using the first TP as a reference, the particle with the highest similarity is obtained by applying the similarity calculation method, which is the turning TP correction point.

[0138] If the pre-driving road network primitive is a near-straight road network primitive, record the vehicle's previous time step and RNBE. Cur The distance of the first TP is Len VO_Last The vehicle's current time and RNBE CurThe distance of the first TP is Len VO_Cur By assuming the vehicle is moving at a constant speed, the relationship between the vehicle and the RNBE at the next moment can be predicted. Cur The distance of the first TP is Len VO_Next RNBE Cur The distance between the first and last TP is Len. TPs If the above variables satisfy equation (16) or equation (17), then the near-straight TP correction point screening condition is met. Equation (16) indicates that the vehicle has passed the intersection for the first time, and equation (17) indicates that although the vehicle has not yet passed the intersection, it is predicted that the vehicle will pass the intersection at the next moment, and the vehicle will be closer to the tail TP at the next moment.

[0139] (Len VO_Last <Len TPs )&&(Len VO_Cur >Len TPs (16)

[0140]

[0141] The above conditions are met when the vehicle reaches the road intersection. This invention, particle-based RNBE, addresses this issue. Cur Tail TP, such as Figure 7 The black triangle mark is located at RNBE2. Finally, RNBE... Init Using the first TP as a reference, the particle with the highest similarity is obtained by applying the similarity calculation method, which is the near-straight TP correction point.

[0142] If there are multiple pre-driving road network elements, once a vehicle meets the steering TP correction condition, it is determined that the vehicle has entered this steering road network element; if the vehicle only meets the near-straight TP correction condition, the possibility of the vehicle entering other road network elements is retained until the vehicle has passed the tail TP by more than 10 meters and the heading is less than 10° from the head of the near-straight road network element, then it is determined that the vehicle has entered this near-straight road network element.

[0143] (2) Method for selecting SP correction points

[0144] For the SP correction point, this invention employs a determination method similar to that for near-straight TP. VO_Last Len VO_Cur and Len VO_Next As defined above, let SP and RNBE be denoted as SP and RNBE respectively. Cur The distance of the first TP is Len SP-TP If the above variables satisfy equation (18) or equation (19), then the SP correction point screening condition is satisfied.

[0145] (Len VO_Last <Len SP-TP )&&(Len VO_Cur >LenSP-TP (18)

[0146]

[0147] To avoid the influence of near-straight TP determination, this invention performs reverse screening starting from the end of the skeleton point group, particleizing SPs that meet the screening criteria, and then using RNBE. Cur Using the first TP and the last TP as references, the particle with the highest similarity is obtained by applying the similarity calculation method, which is the SP correction point.

[0148] Step 9: Construct a pose graph optimization model for the trajectory, add local visual factors and global map correction point factors, and correct the historical trajectory by solving the pose graph optimization problem of the trajectory.

[0149] The global attitude diagram structure of the road network map-assisted vehicle positioning system designed in this invention is as follows: Figure 8 As shown, the vehicle pose at each moment is treated as a node in the pose graph. The local constraints between two consecutive nodes are provided by the VO system, and the global constraints for indefinite intervals are provided by map correction points.

[0150] This invention constructs a pose graph optimization problem with only trajectories, which is essentially a maximum likelihood estimation problem. The maximum likelihood estimate consists of the joint probability distribution of the vehicle over a time period, and the variable is the global pose of all nodes, T. g ={T g0 ,T g1 ,T g2 ,…,T gl}, where T gl ∈SE(3) represents the vehicle's pose in the geographic system at time l. Assuming all measurement probabilities are independent, the maximum likelihood estimation problem can be derived as shown in Equation (20), where S is a set of measurements, including visual odometry output and map correction point measurements, and T represents the optimal estimate of the global pose of all nodes:

[0151]

[0152] in, This represents the measurement value that can be observed at time t.

[0153] This invention assumes that visual odometry (VO) is accurate in the short term, an assumption applicable to most existing VO algorithms. The VO output is accurate over a small range, utilizing the relative pose between two frames, considering consecutive frames t-1 and t. T is used respectively... i enu and The VO output pose transformation at times i and j, and the transformation relationship between the two frames can be expressed as: The form of Lie algebra is as follows: in[] ∨ The expression represents the transformation from a four-dimensional matrix to a six-dimensional vector, and ln() represents the natural logarithm to the base e.

[0154] Based on this, give ε i and ε j Add a left perturbation δε to each i and δε j As shown in equation (21), where ε i and ε j T represents respectively i enu and The Lie algebra form.

[0155]

[0156] in, This represents the transformation term after adding the perturbation. ∧ This represents the transformation from a six-dimensional vector to a four-dimensional matrix, where exp() represents the natural exponential function and I represents the identity matrix.

[0157] Error with respect to T i enu and The Jacobian matrix is ​​shown in equations (22) and (23) below:

[0158]

[0159]

[0160] in φ e ε ij The rotational component, ρ e Represents ε ij The translation component. R x Indicates pose T x The rotational component, t x Indicates pose T x The translation component.

[0161] The TP and SP correction points selected in step 8 only contain longitude and latitude information. This invention assigns vehicle position and height information that meets the selection criteria to the correction points, and calculates the ENU coordinates of the correction points as map correction point factors to be added to the pose graph.

[0162] After the global pose graph is constructed, its optimization process is equivalent to searching for the node configuration that best matches all edges. This invention uses a sliding window method to reserve a large computational window for the pose graph, thereby obtaining accurate and globally drift-free pose estimation. The selection of turning point corrections (TP) mainly relies on angle judgment criteria, exhibiting high stability, and primarily corrects the trajectory length, such as... Figure 9 As shown. This invention designs its calculation window to begin with the previous steering TP correction point, using the vehicle's initial position as the first steering TP correction point in the initial stage. The selection of SP correction points and near-straight TP correction points mainly relies on length judgment criteria, primarily correcting the trajectory direction, such as... Figure 10 As shown. For the same driving distance, a small deviation in heading can lead to a large positioning error. The calculation window for SP correction point and near-straight TP correction point is set to the first 1000 frames before the current time. If there are less than 5 turning TP correction points in the first 1000 frames, it is extended to 1500 frames. If there are still insufficient corrective points, the actual number of frames available is used as the window.

[0163] Step 10: Predict the vehicle's pose at the latest moment based on the visual odometry calculation results and the corrected trajectory;

[0164] Assuming the vehicle pose at time l meets the correction point selection criteria, the corrected trajectory pose at time l after pose graph optimization is T. l enu Two frames of images captured by the camera at time l and time l+1 are input into the visual odometry. The visual odometry outputs the incremental motion estimate between the two frames, which is defined as... Then the vehicle pose at time l+1 can be predicted:

[0165]

[0166] in, This represents the vehicle's position and pose at time l+1.

Claims

1. A method for vehicle localization assisted by road network map based on pose graph optimization, characterized in that, Given the vehicle's initial position, initial attitude angle, and the road network primitives to which the vehicle belongs, constraints are introduced from the road network map to suppress the cumulative error of visual odometry. Combining the influence of the original visual odometry trajectory and map correction points, a corrected trajectory is output. Finally, the vehicle's positioning result is predicted based on the visual odometry calculation result and the corrected trajectory. The steps are as follows: S1, Load the regional road network map based on the vehicle's initial global location; S2 uses road intersections as support points and roads between adjacent support points as road network primitives to transform regional road network maps into road network primitive maps. S3, calculate the connectivity between different road network primitives based on the positional relationship between the first and last nodes of the road network primitives; S4. Based on the distance between adjacent skeleton points within each network element, determine whether it is necessary to increase the number of skeleton points within each network element. S5. Calculate the initial transformation matrix based on the vehicle's initial attitude angle and sensor extrinsic parameters. ; S6, based on the initial driving direction of the vehicle, orient the road network element to which the vehicle belongs; S7, according to Convert the visual odometry output pose to the northeast-sky coordinate system; S8 introduces constraints from the road network map, sets judgment conditions, and filters map correction points; S9. Construct a pose graph optimization model for the trajectory, add local visual factors and global map correction point factors, and correct the historical trajectory by solving the pose graph optimization problem of the trajectory. S10, predicts the vehicle's latest position and orientation based on visual odometry calculations and corrected trajectories; S11, Receive visual odometer output, repeat steps S7 to S10 until the vehicle stops; In step S2, the road network support point is defined as the inflection point TP. The first and last nodes of any road network element are inflection points, and the inflection points contain the topological connection relationships between different road network elements. Inside the road network element, the road shape is approximated by a series of nodes connected by lines, and this series of nodes is defined as the skeleton point SP. The structural composition of a single road network element is then represented as follows: ， in, This indicates a turning point in the road network infrastructure. Indicates the inflection point of the road network primitive. Represents the skeleton point group of road network primitives; If the skeleton point group is empty, then the orientation of the head and tail of this road network element are both... If the skeleton point group is not empty, then the orientation of the head of this road network element is... The tail is facing ; In step S8, the filtering map correction points include road network primitive tails. Correction point selection and internal network elements Correction point filtering; S81, the Correction points include steering. Correction point and near straight The correction points correspond to the turning road network primitive and the near-straight road network primitive, respectively; the implementation process is as follows: S811, setting up a case with only a single pre-driving road network element; S8111, if the pre-driving road network element is a turning road network element, the orientation of the head of the pre-driving element is... The angle between the tail and the included angle is Calculate the vehicle's current driving direction and Tail facing the angle The angle between the vehicle's current driving direction and the direction of the front of the pre-driving element. If condition A is met, then the turning condition is satisfied. Correction point filtering criteria; Criterion A is as follows: ， in, This refers to the basic element of the road network to which the vehicle currently belongs; Will After shifting the head of the turning road network primitive by half the road width, it is particle-ized, and finally... head For reference, a similarity calculation method is used to find particles that meet the set conditions, which are then used for steering. Correction point; S8112, if the pre-driving primitive is a near-linear primitive, record the vehicle's previous moment and... head The distance is The vehicle's current time and head The distance is Assuming the vehicle is moving at a constant speed, predict the next moment when the vehicle and... head The distance is , First and last The distance between them is If condition B or C is met, then it is determined that the near-straight line is satisfied. Correction point filtering criteria; Condition B is: ; Condition C is: ; Condition B indicates that the vehicle has passed through the intersection for the first time; Condition C indicates that although the vehicle has not yet passed through the intersection, it is predicted that the vehicle will pass through in the next moment, and the distance between the vehicle and the tail in the next moment is... Closer; Particle tail Finally head For reference, a similarity calculation method is used to find particles that meet the set conditions, which are near-straight Correction point; S812 contains multiple pre-driving primitives; If the vehicle meets the steering requirements Correct the conditions to determine if the vehicle has entered this turning network element; If the vehicle meets the near-straight line The condition is modified to retain the possibility of the vehicle entering other road network elements until the vehicle passes the tail. If the distance exceeds 10 meters and the heading is less than 10° from the head of the near-straight road network element, it is determined that the vehicle has entered this near-straight road network element. S82, Filtering correction points; Let the current vehicle belong to the basic skeleton point group as ,remember and head The distance is If condition D or E is satisfied, then it is determined that the condition is satisfied. Correction point filtering criteria; Condition D is: ; Condition E is: ; Starting from the end of the skeleton point group, the selection is reversed, and particles that meet the selection criteria are identified. , respectively head Watsuo For reference, similarity calculation methods are used to find particles that meet the set conditions. Correction point.

2. The method for road network map-assisted vehicle localization based on pose graph optimization according to claim 1, characterized in that, In step S3, for a single road network element, its first inflection point is extracted first. Tail Turning Point Iterate through the first and last inflection points of the remaining road network primitives within the road network primitive set: If there exists a first inflection point of a certain road network element and or If they are located at the same road intersection, then these two road network elements are considered to be connected; Alternatively, if there exists a tail inflection point of a certain road network element and or If they are located at the same road intersection, then these two road network elements are considered to be connected.

3. The method for road network map-assisted vehicle localization based on pose graph optimization according to claim 1, characterized in that, In step S4, the detailed steps for determining whether to increase the number of skeleton points within each network element based on the distance between adjacent skeleton points within each network element are as follows: S41, for road network elements Decompose it into a group of broken line segments ; S42, calculate the polyline distance for each polyline segment. Where z = 1, 2, ..., n+1; if If the distance between two nodes is small, no additional skeleton points are needed; if Then, it is necessary to add skeleton points.

4. The method for road network map-assisted vehicle localization based on pose graph optimization according to claim 1, characterized in that, In step S6, the implementation process of the road network primitive to which the vehicle belongs in the forward direction is as follows: Given the road network primitive element to which the vehicle initially belongs, denoted as ; The vehicle is initially stationary; its initial position is denoted as . After transformation matrix Position converted to ENU coordinate system ; The position of a vehicle whose cumulative displacement first exceeds 2 meters is recorded as follows: Transformed matrix The position after conversion to the ENU coordinate system is ; Transitioning via ECEF coordinate system, and Convert to LLA coordinates and ; Calculate the vehicle's direction of travel Positive direction of the corresponding broken line The included angle is Calculate the vehicle's direction of travel Reverse of the corresponding broken line The included angle is : like ,Keep The original structure remains unchanged; if Reverse adjustment The original structure is aligned with the vehicle's direction of travel.

5. The road network map-assisted vehicle localization method based on pose graph optimization according to claim 4, characterized in that, Vehicle cumulative displacement The calculation expression is as follows: ， in, This represents the straight-line distance between two points. Indicates the first The position of the vehicle in the camera's world coordinate system at a given time frame.

6. The method for road network map-assisted vehicle localization based on pose graph optimization according to claim 1, characterized in that, In step S9, the vehicle pose at each moment is set as a node in the pose graph. Local constraints between two consecutive nodes are provided by the VO system, and global constraints for indefinite intervals are provided by map correction points. A pose graph optimization problem with only trajectory is constructed and transformed into a maximum likelihood estimation problem. The maximum likelihood estimation consists of the joint probability distribution of the vehicle over a period of time, with the global pose of all nodes as the variable. ,in Indicates the first The vehicle's pose in the geographic system at a given time frame; The maximum likelihood estimation problem, assuming all measurement probabilities are independent, is as follows: ， in, It is a set of measurements, including VO system output and map correction point measurements. This represents the optimal estimate of the global pose of all nodes. express The measurements that can be observed at any given moment.

Images