A vehicle positioning and navigation system integrating 5G and BeiDou

Abstract

This application discloses a vehicle positioning and navigation system integrating 5G and BeiDou, including: a multi-source heterogeneous positioning module; a high-precision map and dynamic traffic information database; an adaptive multi-layer map matching module; a dynamic path planning module considering steering limitations and real-time position feedback; and a central information fusion and decision-making unit. By integrating multi-source heterogeneous positioning data from BeiDou / GPS, 5G positioning, a high-precision IMU, an onboard atomic clock, and a barometric altimeter, and employing a compact combination algorithm based on Error State Kalman Filter (ESKF) or factor graphs, the system can output continuous, smooth, and high-precision vehicle state estimates in environments with good satellite signals, partial obstruction, or even severe satellite signal loss. Especially when the number of visible satellites is insufficient, by using the onboard atomic clock to predict clock errors and combining this with the elevation constraints of the barometric altimeter, "dual-satellite positioning" can be achieved, improving the availability and robustness of the positioning.

Description

Technical Field

[0001] This application belongs to the field of navigation technology, specifically relating to a vehicle positioning and navigation system that integrates 5G and BeiDou. Background Technology

[0002] With the rapid development of intelligent transportation systems and autonomous driving technologies, high-precision and high-reliability vehicle positioning and navigation have become critical industry requirements. Currently, most mainstream vehicle positioning systems rely on global navigation satellite systems (such as GPS and BeiDou), which can provide good positioning performance in open sky environments. However, in complex environments such as urban canyons, tunnels, and under overpasses, satellite signals are easily blocked and affected by multipath effects, leading to a sharp decline in positioning accuracy or even loss of lock, making it difficult to meet the accuracy requirements of lane-level navigation.

[0003] In existing technologies, inertial navigation systems (INS) are often used for dead reckoning to address the problem of missing satellite signals. However, INS suffers from cumulative errors and is difficult to operate independently for extended periods. Furthermore, traditional map matching algorithms are mostly based on geometric distance or heading consistency, lacking the ability to adapt to fluctuations in positioning accuracy and the complexity of road networks. This makes them prone to mismatches in scenarios such as overlapping elevated and ground-level roads and multiple intersections. Regarding route planning, most systems still operate at the road level, failing to adequately consider lane-level turning restrictions and real-time traffic conditions. This can lead to problems such as "inability to turn" or "illegal turning" during actual route execution.

[0004] Therefore, there is an urgent need for a vehicle positioning and navigation system that can integrate multi-source positioning information, achieve high-precision lane-level matching, and support dynamic path planning, so as to improve positioning reliability, navigation accuracy, and driving safety in complex environments. Summary of the Invention

[0005] This application provides a vehicle positioning and navigation system that integrates 5G and BeiDou, aiming to solve the problems of existing technologies having accumulated errors, difficulty in working independently for a long time, and lack of adaptability to fluctuations in positioning accuracy and the complexity of road networks.

[0006] Firstly, a vehicle positioning and navigation system integrating 5G and BeiDou, comprising:

[0007] The multi-source heterogeneous positioning module receives and fuses data from multiple sensors, and outputs continuous, high-precision vehicle state estimates.

[0008] High-precision maps and dynamic traffic information databases store high-precision digital maps containing lane-level geometric information, road topology relationships, and traffic regulation semantic information, and receive, integrate, and manage real-time dynamic traffic information;

[0009] The adaptive multi-layer map matching module adaptively switches between different matching strategies based on the positioning accuracy index output by the multi-source heterogeneous positioning module and the complexity of the road network, so as to accurately match the vehicle position to a specific lane or road in the high-precision map and dynamic traffic information database.

[0010] The dynamic path planning module, which considers steering limitations and real-time position feedback, is based on a dual graph model and combines real-time lane-level position information from the adaptive multi-layer map matching module with dynamic traffic information from the high-precision map and dynamic traffic information database to plan a legal, efficient and executable driving path.

[0011] The central information fusion and decision-making unit coordinates the data flow and control logic among the multi-source heterogeneous positioning module, the high-precision map and dynamic traffic information database, the adaptive multi-layer map matching module, and the dynamic path planning module.

[0012] Optionally, the multi-source heterogeneous positioning module includes: a BeiDou / GPS positioning submodule, which receives satellite signals and performs positioning calculations;

[0013] The 5G positioning submodule uses Time of Arrival (TOA) and / or Time Difference of Arrival (TDOA) algorithms for positioning in areas with poor satellite signal.

[0014] A high-precision inertial measurement unit (IMU) provides the vehicle's angular velocity and acceleration information for dead reckoning.

[0015] The vehicle-mounted atomic clock and barometric altimeter are used to predict receiver clock errors with high precision to assist positioning when the number of visible satellites is insufficient; the barometric altimeter is used to provide relative elevation information.

[0016] The data fusion center employs a compact combination algorithm based on error state Kalman filter (ESKF) or factor graph to deeply fuse observation information from the BeiDou / GPS positioning submodule, the 5G positioning submodule, the high-precision inertial measurement unit (IMU), and the vehicle-mounted atomic clock and barometric altimeter.

[0017] Optionally, the adaptive multi-layer map matching module is configured to switch between at least two of the following matching strategies: a high-precision layer, where when the positioning accuracy is higher than a first threshold, performs constraint-based lane-level matching to lock the vehicle to a specific lane;

[0018] In the standard layer, when the positioning accuracy is between sub-meter and meter level, weighted matching based on distance, heading, and topology is performed, and a machine learning model is introduced to dynamically adjust the weight coefficients of each factor.

[0019] The robust layer performs matching based on road topology connectivity and historical trajectory consistency when the positioning accuracy is lower than the second threshold or the signal is severely obstructed, prioritizing the topological correctness of the matching results.

[0020] Optionally, the dynamic path planning module includes: a dual graph construction unit, which converts the physical road network into a dual graph, wherein the nodes of the dual graph represent directed road segments in the original road network, the edges of the dual graph represent allowed turning behaviors between road segments, and the traffic regulation semantic information is characterized as the presence or absence of edges or their weights.

[0021] The real-time constraint integration unit receives real-time, high-confidence lane-level information from the adaptive multi-layer map matching module, and dynamically filters out disallowed turning actions at the next intersection based on the attributes of the lane the vehicle is currently in during the path planning process.

[0022] The improved A* algorithm engine runs on the dual graph, where the edge weights combine real-time travel time, steering penalty, and dynamic event penalty, and path search and planning are performed starting from the real-time lane-level information.

[0023] Optionally, the high-precision map and dynamic traffic information database includes: a high-precision map data model, which stores data in a hierarchical structure, including at least a road layer, a lane layer, a facility layer and a positioning layer, wherein the lane layer accurately records the width, topological connection relationship and the associated traffic regulations of each lane;

[0024] A dynamic information fusion engine is used to receive real-time traffic information via V2X communication and / or mobile networks, and to spatiotemporally correlate the real-time traffic information with map elements in the high-precision map data model.

[0025] The service interface is used to provide map data query, dynamic information retrieval and location assistance services to other modules in the system.

[0026] Secondly, a vehicle positioning and navigation method based on multi-source information fusion and dynamic path optimization is applied to a vehicle positioning and navigation system integrating 5G and Beidou. The method includes: fusing multiple sensor data through a multi-source heterogeneous positioning module to generate continuous and high-precision vehicle state estimation.

[0027] The adaptive multi-layer map matching module adaptively selects a matching strategy based on the accuracy of the vehicle state estimation and the complexity of the current road network, matching the vehicle position to a specific lane or road in the high-precision map and dynamic traffic information database.

[0028] By taking into account steering limitations and real-time position feedback, the dynamic path planning module plans the optimal driving path based on the dual graph model and combined with the matched real-time lane-level position and dynamic traffic information.

[0029] The central information fusion and decision-making unit coordinates the operation of each module and provides navigation guidance.

[0030] Optionally, the fusion of multiple sensor data through the multi-source heterogeneous positioning module includes: when satellite signals are blocked and the number of visible satellites is less than 4, using the clock difference predicted by the vehicle-mounted atomic clock as a known quantity, and using the elevation information provided by the barometric altimeter after initialization and correction as a vertical constraint, to collaboratively achieve positioning calculation with only two visible satellites.

[0031] Optionally, the adaptive selection matching strategy includes: in the standard layer matching strategy, using real-time positioning accuracy, road complexity index and historical matching data as input features, and dynamically outputting the weight adjustment coefficients of distance, heading and topology factors through a lightweight machine learning model to calculate the comprehensive weight of candidate roads.

[0032] Compared with the prior art, this application has at least the following beneficial effects:

[0033] This application integrates multi-source heterogeneous positioning data from BeiDou / GPS, 5G positioning, high-precision IMU, vehicle-mounted atomic clock, and barometric altimeter. By employing a compact combination algorithm based on Error State Kalman Filter (ESKF) or factor graph, the system can output continuous, smooth, and high-precision vehicle state estimates in environments with good satellite signals, partial obstruction, or even severe satellite signal loss. In particular, when the number of visible satellites is insufficient, the system can achieve "dual-satellite positioning" by using the vehicle-mounted atomic clock to predict clock errors and combining them with the elevation constraints of the barometric altimeter, thereby improving the availability and robustness of the positioning.

[0034] This application also systematically designs an adaptive multi-layer map matching mechanism, which can dynamically switch matching strategies according to positioning accuracy and road complexity: in high-precision scenarios, lane-level matching is performed; in standard scenarios, machine learning is introduced to dynamically adjust weights; and in low-precision scenarios, topological correctness is prioritized. This mechanism effectively avoids the mismatch and skipping problems of traditional matching algorithms in scenarios such as complex intersections and parallel roads, and improves the continuity and reliability of navigation.

[0035] This application also utilizes a dual graph model combined with real-time lane-level location information. During path planning, the system can dynamically filter out disallowed turning behaviors, ensuring that the planned path complies with traffic rules and current lane attributes. The improved A* algorithm integrates real-time traffic information and turning penalties, enabling path planning and replanning to be completed within milliseconds to seconds, outputting a legal and efficient driving path, effectively avoiding the problem of "guided violations." Attached Figure Description

[0036] Figure 1This application provides a schematic diagram of the module connection for a vehicle positioning and navigation system integrating 5G and BeiDou. Detailed Implementation

[0037] To make the objectives, technical solutions, and advantages of this application clearer, the following detailed description is provided in conjunction with the accompanying drawings and embodiments.

[0038] This application provides a vehicle positioning and navigation system integrating 5G and Beidou, including: a multi-source heterogeneous positioning module, a high-precision map and dynamic traffic information database, an adaptive multi-layer map matching (ML-MM) module, and a dynamic path planning module that considers steering limitations and real-time position feedback.

[0039] The multi-source heterogeneous positioning module receives and fuses data from multiple sensors, outputting continuous, high-precision vehicle state estimates. It includes the following sub-modules:

[0040] The BeiDou / GPS positioning submodule receives satellite signals for basic positioning and serves as the primary positioning source in open sky environments.

[0041] The 5G positioning submodule uses algorithms such as TOA / TDOA for preliminary positioning, serving as an important supplement to areas with poor satellite signals (such as indoors and urban canyons).

[0042] Specifically, algorithms such as TOA / TDOA use signal arrival time or time difference for calculation. To solve NLOS (non-line-of-sight) error, this system will integrate channel impulse response (CIR) information from multiple base stations and use machine learning models (such as lightweight neural networks) to identify and reduce the weight of NLOS measurements.

[0043] The high-precision inertial measurement unit (IMU) provides the vehicle's angular velocity and acceleration information. It obtains attitude changes (pitch, roll, and heading) by integrating the angular velocity and obtains position changes by performing a second integration on the accelerometer measurements (after deducting gravitational acceleration). This information is used for dead reckoning (DR) and maintains short-term positioning when both satellite and 5G signals are temporarily unavailable.

[0044] The vehicle-mounted atomic clock and barometric altimeter are integrated into the vehicle terminal. The atomic clock is used to predict the receiver clock error with high precision. When there are fewer than 4 visible satellites, it assists BeiDou in achieving "quasi-3D" positioning (solving three-dimensional position with clock error as a known quantity). The barometric altimeter provides relative elevation information. After initialization and correction, it serves as a strong constraint in the vertical direction. Together with the atomic clock, it enables "two-satellite-one-positioning" in severely obstructed environments.

[0045] Specifically, when the satellite signal is good (PDOP<6), the receiver clock bias sequence is accurately estimated and recorded. The clock noise conforms to the characteristics of frequency-modulated white noise and is predicted using the endpoint method. That is, when the number of satellites is less than 4, the clock noise value of the last accurate measurement is used. As the current clock difference The predicted value will predict the clock difference. Substituting these known quantities into the pseudorange observation equation reduces the number of unknowns to be solved from four. Reduced to 3 (X, Y, Z), thus achieving 3D positioning with "2 stars + clock difference";

[0046] The initialization and calibration process for a barometric altimeter includes the following steps:

[0047] When the PDOP of satellite positioning is less than 6 and lasts for more than 10 seconds, its elevation solution is considered to be... Reliable, record the original air pressure value measured by the barometric altimeter at this time. And temperature T, using the formula Calculate the theoretical sea level reference pressure P0 at this time (or directly use...) As a reference height H0, As a reference pressure P0, in subsequent measurements, the initial P0 and T0 (or H0 and P0) and the real-time measured P and T are used to calculate the real-time altitude using the pressure height formula. .

[0048] The formula for high pressure is:

[0049] in, Let be the altitude (m) of the measuring point to be determined. Given the known altitude (m) of the reference point. The thermodynamic temperature (K) at the reference point. P is the air pressure value (Pa) measured by the barometer at the measuring point. Here, L represents the air pressure at the reference point (Pa), and L is the vertical temperature lapse rate (K / m), which is taken as 0.0065 K / m in standard atmosphere. Let be the specific gas constant of dry air, and take . , Let be the acceleration due to gravity, and take . ;

[0050] Will As a strong constraint, it is combined with the two-dimensional positioning results from satellite or 5G to solve the three-dimensional position, which is particularly suitable for distinguishing between elevated bridges and ground roads;

[0051] The data fusion center uses a compact combination algorithm based on Kalman filtering to deeply fuse the observation information (pseudorange, carrier phase, 5G ranging values, IMU data, barometric altitude, etc.) from all the above positioning sources, and outputs an optimal, smooth, and continuous full-scene vehicle position, speed, and attitude estimate.

[0052] Specifically, the data fusion center adopts a compact combination scheme based on error state Kalman filtering (ESKF), and its state vector is:

[0053]

[0054] in, For location, For speed, For attitude error, For IMU accelerometer, This refers to the zero bias error of the gyroscope. For receiver clock bias, To account for clock drift error, This is the barometer error (used to model slow changes in air pressure).

[0055] The data fusion center has built-in observation vectors and observation models, directly using the raw observation values ​​from each sensor, corresponding to the observation pseudorange. and carrier phase Compared to TOA / TDOA measurements from multiple base stations, angular velocity and specific force (acceleration), as well as barometric altitude. ;

[0056] The data fusion center has an adaptive mechanism that dynamically adjusts the value of the observation noise matrix R in the Kalman filter based on the real-time signal-to-noise ratio (SNR) of each observation, the geometry of the satellite / base station (DOP), and the innovative sequence (the difference between the predicted and observed values).

[0057] A high-precision map and dynamic traffic information database stores high-precision digital maps containing lane-level geometric information, road topology relationships, and traffic regulation semantic information. It also receives, integrates, and manages real-time dynamic traffic information. The high-precision map and dynamic traffic information database includes:

[0058] The high-precision map data model stores high-precision digital maps containing lane-level geometric information, road topology relationships, and traffic regulation semantic information, and has a dynamic traffic information database to receive, integrate, and manage real-time dynamic traffic information;

[0059] Specifically, high-precision map data models employ a hierarchical, structured data model, typically following international standards such as OpenDRIVE or AutowareVectorMap, or extending from them. Their core layers are as follows:

[0060] The first layer is the road layer, which mainly depicts the macro framework of the road. Each road is given a unique ID and name, and its level (such as highway, urban arterial road or internal road of a community) and direction of traffic (one-way or two-way) are indicated. The geometry of the road is defined by a reference line, which is essentially the center line of the road. It is composed of a series of points with three-dimensional coordinates, and the points are connected by straight lines, arcs or smoother spline curves.

[0061] The second layer is the most crucial and detailed lane layer. The lane layer directly describes the actual driving space of the vehicle, and it is further divided into two parts:

[0062] First, it describes the lane lines themselves, recording the precise three-dimensional geometric position of each lane line, and also recording its physical properties in detail, such as whether it is a solid line, a dashed line, or a double solid line, whether the color is white or yellow, and whether the material is ordinary paint or reflective cat's eye stone. The above information directly determines where the driver can legally change lanes.

[0063] Secondly, a complete definition of the lane as a driving unit is required. Since the width of a lane is not constant (for example, it gradually widens as it approaches an intersection), a complete lane needs to be divided into multiple consecutive lane units. Each unit has a unique ID and indicates its sequence number from left to right in the entire road (for example, the leftmost lane is lane number 1). At the same time, the width change pattern of the unit relative to the road reference line is recorded, and the "preceding" and "successor" lanes of each lane unit are clearly recorded to form the route basis for path planning. In addition, each lane unit is also bound to rich attributes, such as the maximum and minimum speed limits, lane function (whether it is a regular driving lane, a bus lane, or a left / right turn lane), and key traffic regulation information.

[0064] Thirdly, there is the facility layer, which describes various objects on the road. This facility layer is equivalent to creating a file for static facilities in the road environment. For example, for each traffic sign (speed limit sign, no entry sign, etc.) and traffic light, we record its type, specific content, precise three-dimensional location, and which lane or area it specifically affects. The data for traffic lights includes its phase sequence, and key obstacles that may affect the safety of autonomous driving, such as curbs, guardrails, and bridge piers, are labeled using three-dimensional bounding boxes.

[0065] Fourth is the positioning layer, which helps vehicles achieve accurate positioning even when satellite signals are weak. Unique "positioning features" are pre-embedded in the map, such as unique texture feature point clouds on a certain road surface, the corner of a landmark building, or the signal fingerprint of a 5G / Bluetooth base station. When the vehicle passes by, the onboard camera, LiDAR and other sensors scan the surrounding environment, and then match the scanned features with the features pre-stored in the map to calculate the vehicle's current precise position and attitude.

[0066] The dynamic traffic information database is responsible for receiving, integrating, managing, and distributing various real-time traffic information. The database acquires data through multiple channels and processes it so that it can be effectively utilized by other modules of the navigation system.

[0067] Regarding information sources, the system primarily accesses real-time data through two methods:

[0068] One type is based on V2X vehicle-to-everything (V2X) communication. The system receives basic safety messages from surrounding vehicles through V2V vehicle-to-vehicle communication. These basic safety messages contain key information such as the location, speed, heading, and braking status of surrounding vehicles, which can be used to detect micro-traffic events such as sudden braking or sudden congestion ahead. At the same time, through V2I vehicle-to-infrastructure (V2I) communication, the system can receive traffic light status and timing information from roadside units, roadside sensor detection results (such as pedestrian crossing warnings), and local high-precision map data. These V2X communications are currently mainly implemented based on two technical standards: DSRC or C-V2X.

[0069] Another approach involves accessing the system via mobile internet. This system obtains regional-level traffic flow data from a traffic big data platform, including macro-level information such as average vehicle speed, congestion index, and travel time. It also receives event information from traffic management departments and third-party platforms, including traffic accidents, road construction, temporary traffic control measures, and severe weather warnings. This type of data is typically transmitted using PEG or similar standard formats. Additionally, the vehicle's operational data, after anonymization, is also uploaded to the cloud platform as floating car data, providing data support for macro-level traffic flow calculations.

[0070] The system is equipped with a data fusion and processing engine for data processing. This engine includes a data parser, which is responsible for parsing and standardizing data from different sources and protocols, and converting it into a standardized format within the system. The spatiotemporal correlation module is responsible for accurately associating received dynamic events with specific elements in the high-precision map. For example, it can associate an accident report with "the 2nd and 3rd lanes of road A from south to north, about 150 meters away from node X", or quantify congestion information into its impact on the traffic speed of a specific road segment or lane.

[0071] To ensure data reliability, the system also establishes a confidence level and lifecycle management mechanism. Each piece of dynamic information is assigned a confidence score, which is comprehensively evaluated based on factors such as the reliability of the information source and the freshness of the data. At the same time, the system sets a lifespan for various types of information. Information that has not been updated within the time limit will be automatically marked as expired and removed from the system, thereby ensuring the timeliness and accuracy of traffic information.

[0072] The adaptive multi-layer map matching module, based on the positioning accuracy index output by the multi-source heterogeneous positioning module and the complexity of the road network, adaptively switches different matching strategies to accurately match the vehicle position to a specific lane or road in the high-precision map and dynamic traffic information database.

[0073] Specifically, the adaptive multi-layer map matching module continuously monitors real-time data from multiple key modules in the system to comprehensively evaluate the current positioning quality and the complexity of the road environment.

[0074] The positioning module obtains various indicators reflecting the positioning status, including the position estimate and its corresponding covariance matrix. These data reveal the accuracy of the positioning results, as well as the magnitude and direction of the uncertainty range. At the same time, the positioning mode identifier is also an important input, such as the RTK fixed solution with centimeter-level accuracy, the RTK floating-point solution with decimeter-level accuracy, the single-point solution with meter-level accuracy, or the dead reckoning mode that relies on inertial calculation. It can be seen that the number of satellites and the accuracy factor (such as the PDOP value) are also taken into consideration. Generally, a PDOP value greater than 6 indicates that the current spatial geometry distribution of satellites is poor. The internal quality assessment data from the 5G positioning submodule also serves as an important reference for measuring the reliability of positioning.

[0075] On the other hand, the sensor also obtains feature information about the road environment from high-precision maps. The system pre-calculates or queries in real time the complexity index of the road network surrounding the vehicle's current location. This index classifies roads into different levels of complexity: for example, one-way or two-way road segments without adjacent parallel roads are classified as simple road segments; areas with multiple intersections, roundabouts, etc., are classified as complex intersections; and areas that are prone to causing location confusion, such as overpasses overlapping with ground roads, multiple parallel roads (such as the coexistence of main roads and auxiliary roads), or ramp intersections, are marked as high-risk areas requiring special attention.

[0076] The adaptive multi-layer map matching module has a multi-layer matching strategy, in which:

[0077] Layer 1 is a high-precision layer - constraint-based lane-level matching, triggered by the localization mode. And the horizontal positioning error (major axis of the covariance ellipse) is less than 0.3 meters;

[0078] Its core algorithms include:

[0079] Candidate lanes are generated by searching all lane lines in the high-precision map with the positioning point as the center and a radius of 3 times the positioning error.

[0080] Projection and constraint filtering projects the location points onto the centerlines of all candidate lanes. Strict constraints are applied for filtering, including:

[0081] Lateral distance constraint: The lateral distance between the projection point and the lane centerline must be less than (lane width / 2 + positioning error).

[0082] Heading angle constraint: the angle between the vehicle heading (from IMU / fusion results) and the lane direction must be less than a certain threshold (e.g., 10°).

[0083] Topological rationality constraint: the current candidate lane must be topologically connected to the matched lane at the previous time step;

[0084] Finally, the optimal choice is made: among all lanes that meet the constraints, the lane with the smallest weighted sum of lateral distance and heading angle difference is selected as the matching result.

[0085] Layer 2 is the standard layer - road-level matching based on adaptive weighting. Its triggering conditions are that the positioning accuracy is sub-meter to meter level (such as RTK_FLOAT, STANDALONE), or it is located at a complex intersection but the positioning is still acceptable.

[0086] Its core algorithm includes candidate road generation, which searches all road segments (Links) within a fixed location information region.

[0087] For each candidate path i, calculate three initial weights, which include:

[0088] Distance weight Based on the perpendicular distance from the location point to the road, the formula is:

[0089]

[0090] in, It is the distance expansion factor, which can be obtained empirically;

[0091] It is a scale factor determined by vertical distance, defined by the following rules:

[0092]

[0093] Course weight Based on the angle between the vehicle's heading and the road direction The calculation formula is as follows: ;

[0094]

[0095] in, (The difference between the vehicle's heading angle and the azimuth angle of the candidate road segment)

[0096] It is the extension factor of the course;

[0097] The value of is in the range of [-1, 1], and is 1 when the headings are exactly the same and -1 when they are completely opposite.

[0098] Topological weights The calculation formula is based on the connectivity between the current road and historically matched roads. .

[0099]

[0100] Its connectivity judgment logic is as follows:

[0101] Directly connected, the current candidate road segment is the direct successor of the historically matched road segment;

[0102] Indirect connectivity: The current candidate road segment can be connected to a historically matched road segment through an intermediate road segment;

[0103] Disconnected: The current candidate road segment has no connection with the historically matched road segments;

[0104] in, This is the expansion factor for the topological relationship;

[0105] We can optimize weights based on machine learning and design a lightweight online neural network (such as a single-hidden-layer MLP) or a gradient boosting decision tree model. The input features are real-time localization covariance, road complexity index, vehicle speed, and historical matching success rate.

[0106] The output consists of the three basic weights. , , Dynamic adjustment coefficient , , ;

[0107] Subsequently, in the background, the system continuously collects "matching decision" and "post-verification" data, and uses this data to fine-tune the model online so that it can better adapt to the current road characteristics and driving habits of the city;

[0108] Overall weight calculation:

[0109] choose The highest road segment is used as the matching result, and a matching confidence score is calculated. If the highest score and the second highest score are too close, the confidence score is low.

[0110] The final output includes the road segment ID, projection point, and matching confidence score.

[0111] Layer 3 is a robust layer based on topology and historical trajectory matching. Its triggering conditions are a significant decrease in positioning accuracy (a sharp increase in the covariance ellipse), signal loss (entering DR mode), or a persistently low confidence level in Layer 2 matching.

[0112] The core algorithms of layer 3 include: historical trajectory deduction, based on the road topology of a high-precision map, starting from the high-confidence matching point of the previous moment, deducing all possible subsequent paths to form a "possible path network";

[0113] And particle filtering or Hidden Markov Model (HMM), where particle filtering involves scattering points (particles) on a "network of possible paths", with each particle representing a possible vehicle state (position, speed). The particles are moved based on the dead reckoning results from the IMU and the map topology, and the particle weights are updated based on coarse localization observations (even if there is a lot of noise). Finally, the road where the cluster of particles with the highest weights is located is the matching result.

[0114] HMM treats road segments as hidden states and coarse localization points as observations. The transition probability is determined by the road topology and reasonable vehicle speed, while the launch probability is determined by the localization error model. The Viterbi algorithm is used to find the most likely road sequence.

[0115] Its core principle is that at this layer, it is better to stay on a path that may be slightly off but is topologically correct than to jump to a path that is geometrically closest but topologically unworkable, sacrificing local accuracy to preserve the correctness of the global path.

[0116] The final output is the most likely road segment, marked as a low-precision match result;

[0117] The adaptive multi-layer map matching module has a built-in adaptive switching and decision manager, which is responsible for layer switching. Its switching process includes:

[0118] The initial state typically starts from layer 2;

[0119] If the triggering conditions of Layer 1 are met for N consecutive cycles (e.g., N=5), and the map shows that the current road segment supports lane-level matching, then the system will be upgraded to Layer 1.

[0120] If the localization covariance exceeds the threshold, or the IMUDR mode is activated, or the matching confidence of layer 2 is continuously lower than the threshold, then it will be downgraded to layer 3.

[0121] When recovering from layer 3 to layer 2, the "most likely path" output by layer 3 can be the topological weights of layer 2. It provides very strong prior information, accelerating the convergence of layer 2;

[0122] The high-confidence matching results of layers 1 and 2 can be used as "ground truth" to correct the localization module (especially the drift of the IMU), forming a beneficial feedback loop.

[0123] The dynamic path planning module, which considers steering limitations and real-time position feedback, is based on a dual graph model and combines real-time lane-level position information from an adaptive multi-layer map matching module with dynamic traffic information from a high-precision map and a dynamic traffic information database to plan a legal, efficient and executable driving path.

[0124] The construction of the dual graph model includes:

[0125] Node definition: Define each road segment (directed road unit, such as "eastbound direction from intersection A to intersection B") in the high-precision map as a node in the dual graph;

[0126] An edge is defined as a directed edge connecting two nodes in the dual graph, representing a permitted turning action in the original road network.

[0127] The refined definition of weights, specifically the weight of each edge in the dual graph, is a comprehensive cost function calculated based on high-precision maps and a dynamic traffic information database.

[0128] in, (Travel time) is the basic travel time, calculated from the length of Link_i and the standard speed corresponding to the road class. It is updated in real time from a dynamic traffic information database. Real-time passage time.

[0129] (Turning penalty), the penalty conditions include: going straight, the penalty is 0 or very small;

[0130] Turn right, with a smaller penalty (such as +5 seconds), simulating waiting for pedestrians / cyclists;

[0131] Turning left incurs a significant penalty (e.g., +15 seconds) to simulate waiting for oncoming traffic.

[0132] Turning around incurs a severe penalty (such as +30 seconds).

[0133] For illegal turns, if the turn i->j is prohibited by traffic rules, its cost is set to infinity (INF) to ensure that the algorithm will never choose it.

[0134] (Dynamic event penalty): If If a traffic accident or construction occurs on the road, a huge penalty cost is added, guiding the algorithm to detour.

[0135] , , It is a weighting coefficient that can be adjusted according to user preferences (fastest path, fewest turns);

[0136] The real-time position feedback of the dynamic path planning module, which considers steering limitations and real-time position feedback, includes the following process:

[0137] The starting point is dynamically determined, and the map matching module outputs the current dual graph node.

[0138] For example, the ML-MM module outputs: the vehicle is currently in For the second lane (straight lane), the starting point of the route planning is the node. ;

[0139] Lane-level constraints maintain a list of currently executable steering actions, which is determined by two factors:

[0140] The attributes of the lane the vehicle is currently in (from the ML-MM module);

[0141] At the next intersection, the permitted turning distance for this lane (from a high-precision map).

[0142] In a specific embodiment, the application process of lane-level constraints is as follows:

[0143] When the A* algorithm starts from the current node (Representing the current road segment) When preparing to expand its neighboring nodes, it first queries the high-precision map: "At the next intersection, Which subsequent road sections are allowed to turn into from the second lane?

[0144] The map returns a list of allowed subsequent nodes, for example: (Straight ahead only), and (Turn left) and (Turn right) Not permitted.

[0145] When expanding nodes, the A* algorithm will directly ignore... and These two options will only Add to the open list.

[0146] This ensures that the first instruction planned (from the current road segment to the next road segment) can be legally executed by vehicles immediately, avoiding the embarrassment and danger of "please turn left at the next intersection, but you are currently in the straight lane";

[0147] The implementation process of the A* algorithm includes:

[0148] Initialization: OpenList is a priority queue, and ClosedList is a set of explored nodes. The starting node S (i.e., the vehicle's current dual graph node) is added to OpenList. The cost (from the starting point to the current node) is 0. .

[0149] Main loop: Retrieve from OpenList if it is not empty. The smallest node is Current;

[0150] If Current is the target node, backtrack the path and the algorithm ends; otherwise, add Current to ClosedList.

[0151] Expanding the Current node: Traverse each of Current's neighbor nodes Next (i.e., each allowed turn) in the dual graph.

[0152] Real-time constraint check: If Current is the starting node, the "lane-level steering filter" is invoked, retaining only valid neighboring nodes. For non-starting nodes, this check can be relaxed, but basic steering restrictions must still be observed;

[0153] Calculate the cost from Current to Next: cost = Cost(Current->Next) (using the aforementioned comprehensive cost function).

[0154] Calculate the temporary of Next ;

[0155] If Next is not in OpenList or ClosedList, or if a new one is added... Smaller than known: then update Next. for ;

[0156] The heuristic function h = heuristic(Next, Goal) is calculated. Here, the Euclidean distance calculated on the original road network is divided by the maximum speed limit as the time heuristic value to ensure its applicability.

[0157] Set Next ;

[0158] The parent node for recording Next is Current;

[0159] Add Next to (or update it in) OpenList;

[0160] Path backtracking: Starting from the target node, backtrack along the parent node pointer to the starting point to obtain a series of dual graph nodes. Transform these dual graph nodes back into the road segment sequence in the original road network and attach the suggested lane and turning instructions for each road segment.

[0161] The dynamic route planning module, which takes into account steering limitations and real-time location feedback, also has a dynamic replanning mechanism. Its triggering conditions include: vehicle deviating from the path, the ML-MM module detects that the vehicle is continuously not traveling on the planned path; sudden changes in traffic information, the dynamic traffic information database notifies the planning module that a new severe congestion or accident has occurred ahead of the current path; and the user requests to change the destination.

[0162] A replanning strategy is implemented, including gentle replanning, which involves rerunning Algorithm A from the vehicle's new real-time location to the original destination. Due to the efficiency of Algorithm A and the stability of the dual graph structure, the replanning can be completed within milliseconds to seconds.

[0163] Path continuity is ensured; new paths will naturally emerge from the vehicle's current location and smoothly connect with historical paths, without giving users a sense of jumping.

[0164] The central information fusion and decision-making unit coordinates the data flow and control logic between the multi-source heterogeneous positioning module, the high-precision map and dynamic traffic information database, the adaptive multi-layer map matching module, and the dynamic path planning module.

[0165] The technical features of the above embodiments can be combined in any way. For the sake of brevity, not all possible combinations of the technical features in the above embodiments are described. However, as long as there is no contradiction in the combination of these technical features, they should be considered to be within the scope of this specification.

Claims

1. A vehicle positioning and navigation system integrating 5G and BeiDou, characterized in that, include: The multi-source heterogeneous positioning module receives and fuses data from multiple sensors, and outputs continuous, high-precision vehicle state estimates. The multi-source heterogeneous positioning module includes: a BeiDou / GPS positioning submodule, which receives satellite signals and performs positioning calculations; The 5G positioning submodule uses Time of Arrival (TOA) and / or Time Difference of Arrival (TDOA) algorithms for positioning in areas with poor satellite signal. A high-precision inertial measurement unit (IMU) provides the vehicle's angular velocity and acceleration information for dead reckoning. The vehicle-mounted atomic clock and barometric altimeter are used to predict receiver clock errors with high precision to assist positioning when the number of visible satellites is insufficient; the barometric altimeter is used to provide relative elevation information. The data fusion center employs a tight combination algorithm based on error state Kalman filter (ESKF) or factor graph to deeply fuse observation information from the BeiDou / GPS positioning submodule, the 5G positioning submodule, the high-precision inertial measurement unit (IMU), and the vehicle-mounted atomic clock and barometric altimeter. High-precision maps and dynamic traffic information databases store high-precision digital maps containing lane-level geometric information, road topology relationships, and traffic regulation semantic information, and receive, integrate, and manage real-time dynamic traffic information; The adaptive multi-layer map matching module adaptively switches between different matching strategies based on the positioning accuracy index output by the multi-source heterogeneous positioning module and the complexity of the road network, so as to accurately match the vehicle position to a specific lane or road in the high-precision map and dynamic traffic information database. The adaptive multi-layer map matching module is configured to switch between at least two of the following matching strategies: high-order accuracy layer, when the positioning accuracy is higher than a first threshold, performs constraint-based lane-level matching to lock the vehicle to a specific lane; In the standard layer, when the positioning accuracy is between sub-meter and meter level, weighted matching based on distance, heading, and topology is performed, and a machine learning model is introduced to dynamically adjust the weight coefficients of each factor. The robust layer performs matching based on road topology connectivity and historical trajectory consistency when the positioning accuracy is lower than the second threshold or the signal is severely blocked, prioritizing the topological correctness of the matching results. The dynamic path planning module, which considers steering limitations and real-time position feedback, is based on a dual graph model and combines real-time lane-level position information from the adaptive multi-layer map matching module with dynamic traffic information from the high-precision map and dynamic traffic information database to plan a legal, efficient and executable driving path. The central information fusion and decision-making unit coordinates the data flow and control logic among the multi-source heterogeneous positioning module, the high-precision map and dynamic traffic information database, the adaptive multi-layer map matching module, and the dynamic path planning module.

2. The vehicle positioning and navigation system integrating 5G and BeiDou as described in claim 1, characterized in that, The dynamic path planning module includes: a dual graph construction unit, which converts the physical road network into a dual graph, wherein the nodes of the dual graph represent directed road segments in the original road network, the edges of the dual graph represent allowed turning behaviors between road segments, and the traffic regulation semantic information is characterized as the presence or absence of edges or their weights. The real-time constraint integration unit receives real-time, high-confidence lane-level information from the adaptive multi-layer map matching module, and dynamically filters out disallowed turning actions at the next intersection based on the attributes of the lane the vehicle is currently in during the path planning process. The improved A* algorithm engine runs on the dual graph, where the edge weights combine real-time travel time, steering penalty, and dynamic event penalty, and path search and planning are performed starting from the real-time lane-level information.

3. The vehicle positioning and navigation system integrating 5G and BeiDou as described in claim 1, characterized in that, The high-precision map and dynamic traffic information database includes: a high-precision map data model, which stores data in a hierarchical structure, including at least a road layer, a lane layer, a facility layer and a positioning layer, wherein the lane layer accurately records the width, topological connection relationship and the associated traffic regulations of each lane; A dynamic information fusion engine is used to receive real-time traffic information via V2X communication and / or mobile networks, and to spatiotemporally correlate the real-time traffic information with map elements in the high-precision map data model. The service interface is used to provide map data query, dynamic information retrieval and location assistance services to other modules in the system.

4. A vehicle positioning and navigation method based on multi-source information fusion and dynamic path optimization, characterized in that, Applied to the system as described in any one of claims 1-3, the method includes: fusing multiple sensor data through a multi-source heterogeneous positioning module to generate a continuous, high-precision vehicle state estimate; The adaptive multi-layer map matching module adaptively selects a matching strategy based on the accuracy of the vehicle state estimation and the complexity of the current road network, matching the vehicle position to a specific lane or road in the high-precision map and dynamic traffic information database. By taking into account steering limitations and real-time position feedback, the dynamic path planning module plans the optimal driving path based on the dual graph model and combined with the matched real-time lane-level position and dynamic traffic information. The central information fusion and decision-making unit coordinates the operation of each module and provides navigation guidance.

5. The vehicle positioning and navigation method according to claim 4, characterized in that, The method of fusing multiple sensor data through a multi-source heterogeneous positioning module includes: when satellite signals are blocked and the number of visible satellites is less than 4, using the clock difference predicted by the vehicle-mounted atomic clock as a known quantity, and using the elevation information provided by the barometric altimeter after initialization and correction as a vertical constraint, to collaboratively achieve positioning calculation with only two visible satellites.

6. The vehicle positioning and navigation method according to claim 4, characterized in that, The adaptive selection matching strategy includes: in the standard layer matching strategy, real-time positioning accuracy, road complexity index and historical matching data are used as input features, and a lightweight machine learning model is used to dynamically output the weight adjustment coefficients of distance, heading and topology factors to calculate the comprehensive weight of candidate roads.

Images