High-precision beidou positioning method and device for armored vehicle driving training examination system

Abstract

The application discloses a high-precision Beidou positioning method and device of an armored vehicle driving training examination system, and the method comprises the following steps: acquiring the current motion state data of the vehicle and the running state data of the positioning module in real time; predicting the predicted vehicle pose of the vehicle within a future preset time window based on the motion state data, generating the intervention strategy of the high-precision positioning module according to the space-time matching relationship between the predicted vehicle pose and the preset examination area; and controlling the working state of the high-precision positioning module in response to the intervention strategy, fusing the multi-mode positioning observation data from the basic positioning module and the high-precision positioning module by using a state estimation filter, and performing real-time deviation compensation on the observation data by using a state vector containing a system deviation variable in the fusion process to obtain continuous positioning data. The application introduces road topological constraint prediction and vector space deviation compensation, and solves the problems of mode switching lag in a curved road scene and non-smooth trajectory in complex maneuvers.

Description

Technical Field

[0001] This invention relates to the field of satellite navigation and integrated positioning technology, and in particular to a high-precision BeiDou positioning method and device for an armored vehicle driver training and assessment system. Background Technology

[0002] In autonomous driving assessment and special vehicle driving evaluation systems, the continuity and accuracy of vehicle positioning data are the technical foundation for automated scoring. The system typically needs to acquire centimeter-level positional information of the vehicle relative to the assessment lines in real time within a closed area for maneuvering maneuvers such as right-angle turns, S-curve driving, and reversing into parking spaces, ensuring the objectivity and fairness of the evaluation results.

[0003] Currently, most existing driving test positioning systems employ a dual-mode combination of basic positioning and high-precision positioning. The approach is as follows: the system continuously runs a low-power basic positioning module (such as single-point BeiDou) to monitor the vehicle's position in real time; when the straight-line distance between the vehicle and the test area is detected to be less than a set threshold, the high-precision positioning module (such as Real-time Dynamic Differential Positioning (RTK) technology) is preheated and activated. At the data processing level, existing technologies use the standard Kalman filter algorithm to smooth the position data based on a uniform linear motion model (CV model) or a constant turning rate model (CTRV), and correct coordinate jumps between different modes by calculating the mean deviation.

[0004] However, the aforementioned existing technologies suffer from predictive model distortion and bias compensation failure in complex assessment scenarios within closed environments, leading to delayed intervention of high-precision models and abrupt changes in the fusion trajectory. Therefore, further research and innovation are needed to address these issues in the existing technologies. Summary of the Invention

[0005] Purpose of the invention: In view of the above-mentioned problems in the prior art, this application provides a high-precision Beidou positioning method and device for an armored vehicle driver training and assessment system.

[0006] Technical solution: On the one hand, a high-precision BeiDou positioning method for an armored vehicle driver training and assessment system includes:

[0007] Real-time acquisition of vehicle and positioning module operating status data;

[0008] Based on motion state data, predict the vehicle's pose within a future preset time window, and generate an intervention strategy for the high-precision positioning module based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area.

[0009] In response to the intervention strategy control of the high-precision positioning module's working state, a state estimation filter is used to fuse multi-mode positioning observation data from the basic positioning module and the high-precision positioning module. During the fusion process, a state vector containing system deviation variables is used to perform real-time deviation compensation on the observation data to obtain continuous positioning data.

[0010] On the other hand, a high-precision BeiDou positioning device for an armored vehicle driver training and assessment system includes:

[0011] The data acquisition module is configured to acquire real-time data on the vehicle's current motion status and the operating status of the positioning module.

[0012] The prediction and decision-making module is configured to predict the vehicle's pose within a preset time window based on motion state data, and generate an intervention strategy for the high-precision positioning module based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area.

[0013] The fusion control module is configured to respond to the intervention strategy and control the working state of the high-precision positioning module. It uses a state estimation filter to fuse multi-mode positioning observation data from the basic positioning module and the high-precision positioning module. During the fusion process, it uses a state vector containing system deviation variables to perform real-time deviation compensation on the observation data to obtain continuous positioning data.

[0014] Beneficial effects: This invention introduces road topology constraint prediction and vector space deviation compensation, solving the problems of prediction model distortion and deviation compensation failure, ensuring timely positioning intervention and smooth trajectory. The related technical effects will be described in detail below with reference to specific embodiments. Attached Figure Description

[0015] Figure 1 A flowchart illustrating a high-precision BeiDou positioning method for an armored vehicle driver training and assessment system provided in this application embodiment.

[0016] Figure 2 This is a flowchart illustrating how to predict a vehicle's pose within a future preset time window based on motion state data, as provided in an embodiment of this application.

[0017] Figure 3 This is a flowchart illustrating how a state estimation filter can be used to fuse multi-mode positioning observation data from a base positioning module and a high-precision positioning module, as provided in an embodiment of this application.

[0018] Figure 4 This is a flowchart illustrating the prediction of a vehicle's pose within a preset time window, as provided in an embodiment of this application.

[0019] Figure 5A flowchart illustrating the formulation of a progressive preheating scheduling plan is provided for an embodiment of this application. Detailed Implementation

[0020] To enable those skilled in the art to better understand the present invention, the technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings of the embodiments of the present invention. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort should fall within the scope of protection of the present invention.

[0021] It should be noted that the terms "first," "second," etc., in the specification and accompanying drawings of this invention are used to distinguish similar objects and are not necessarily used to describe a specific order or sequence. It should be understood that such data can be interchanged where appropriate so that embodiments of the invention described herein can be implemented in orders other than those illustrated or described herein. Furthermore, the terms "including" and "having," and any variations thereof, are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or apparatus that includes a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units not explicitly listed or inherent to such processes, methods, products, or apparatus.

[0022] To address the aforementioned issues, the applicant conducted in-depth searches and analyses, and discovered:

[0023] Correspondingly, the linear extrapolation prediction method based on the current velocity vector does not take into account the geometric constraints of the site road topology, which will produce a large lateral prediction error in S-curves or continuous turn-off sections. This will cause the system to be unable to accurately determine whether the vehicle is about to enter the assessment area, resulting in the phenomenon of missed triggering due to untimely warm-up of the high-precision module.

[0024] Furthermore, existing scalar deviation compensation models neglect the anisotropy and spatial correlation of inertial device errors. In particular, when a vehicle is performing reciprocating motions such as reversing into a parking space, the error model accumulated during forward driving cannot adapt to the characteristics of reverse motion, resulting in a non-physical step in the positioning trajectory at the moment of mode switching.

[0025] To solve these problems, combined with Figures 1 to 5 The present invention will be specifically described through the following embodiments.

[0026] Firstly, this invention provides an exemplary scheme for a high-precision BeiDou positioning method in an armored vehicle driver training and assessment system. In other words, it provides a dual-mode adaptive positioning method for driver assessment scenarios. This scheme is compatible with both basic positioning scenarios and complex assessment scenarios, specifically including:

[0027] Step 101: Acquire the vehicle's current motion status data and the positioning module's operating status data in real time. Alternatively, acquire the vehicle and positioning module's operating status data in real time.

[0028] In this embodiment, the vehicle's current motion state data refers to the set of physical quantities used to describe the vehicle's instantaneous kinematic characteristics. Specifically, this set may include, but is not limited to, parameters such as the vehicle's planar position coordinates, composite velocity, heading angle, composite acceleration, and angular velocity. These parameters are typically acquired in real time via the vehicle's chassis bus (such as the Controller Area Network (CAN bus)) or the onboard inertial measurement unit (IMU). For example, the accelerometer in the IMU can output three-axis acceleration, the gyroscope can output three-axis angular velocity, and the odometer can provide a speed reference.

[0029] Furthermore, the operational status data of the positioning module refers to a set of indicators used to evaluate the health and signal quality of the vehicle-mounted positioning hardware. This set of indicators may include the number of visible satellites, carrier signal-to-noise ratio (CNR), position precision factor (PDOP), differential data age, and positioning solution status (such as single-point solution, floating-point solution, or fixed solution). By monitoring the status data in real time, the system can determine whether the current positioning module is in an available state or whether it meets the prerequisites for high-precision positioning.

[0030] In some optional implementations, the system can periodically read the above data from the hardware interface according to a preset sampling frequency, such as 10Hz, and perform timestamp alignment processing to ensure that the motion state and the positioning state are consistent in the time dimension.

[0031] Step 102: Based on motion state data, predict the vehicle's pose within a future preset time window, and generate an intervention strategy for the high-precision positioning module based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area.

[0032] In this embodiment, the current movement trend is used to predict the future position, achieving proactive control with foresight. Predicted vehicle pose refers to the vehicle's expected position coordinates and heading angle at a future moment or over a period of time. The preset time window is a pre-configured time length parameter, its value typically depending on the typical time required for the high-precision positioning module to complete initialization from a cold start, for example, set to 15 to 30 seconds. The preset assessment area refers to a predefined geographical area within the driving assessment site with a specific spatial range and assessment requirements, such as a reversing parking area or an S-shaped curve area.

[0033] Specifically, the system uses current motion state data (such as velocity and angular velocity) to calculate the vehicle's position at the end of a preset time window using a kinematic extrapolation algorithm. Further, the system analyzes the spatial relationship between this predicted position and various preset assessment areas. If the prediction indicates that the vehicle is about to enter a certain assessment area, meaning a spatiotemporal matching relationship exists, the system generates a corresponding intervention strategy. The intervention strategy specifically refers to a set of instructions controlling the actions of the high-precision positioning module, such as immediately initiating preheating, maintaining sleep mode, or switching to operating mode. Through this mechanism, the system can allow sufficient time for the high-precision positioning module to complete initialization before the vehicle actually arrives at the assessment area, avoiding the loss of assessment data due to insufficient preparation of the positioning equipment.

[0034] Step 103: In response to the intervention strategy control of the working state of the high-precision positioning module, the multi-mode positioning observation data from the basic positioning module and the high-precision positioning module are fused using a state estimation filter. During the fusion process, the observation data are compensated for in real time using a state vector containing system deviation variables to obtain continuous positioning data.

[0035] In this embodiment, the basic positioning module typically refers to a low-power, meter-level positioning satellite positioning module, such as a single-point positioning GPS / BeiDou module; the high-precision positioning module typically refers to a higher-power, centimeter-level differential positioning module, such as an RTK module. The operating states include low-power standby, preheating startup, and full-function operation. The system dynamically switches the state of the high-precision positioning module according to the step-by-step intervention strategy, maintaining it in standby mode to conserve power in non-test areas and waking it up when entering the test area.

[0036] Based on this, the system employs a state estimation filter to fuse multi-source data. The state estimation filter can be implemented using an extended Kalman filter (EKF) or an unscented Kalman filter (UKF). The system bias variable is one or more components in the state vector, used to characterize the systematic error of the base positioning module relative to the high-precision positioning module (or the true value), such as the position drift.

[0037] During the fusion process, when the high-precision positioning module is unavailable, the filter uses a motion model for recursion; when the high-precision positioning module is available, its high-precision observations are used to update the state and correct the system bias variables. Specifically, when the system reverts to a state where only the basic positioning module is operational, the previously estimated system bias variables are used to correct the observation data of the basic positioning module—a process known as bias compensation—eliminating system errors and ensuring that the output positioning data remains smooth and continuous during mode switching. Based on this, the output continuous positioning data is a time-continuous sequence of position, velocity, and heading with the required accuracy.

[0038] On the other hand, this paper describes alternative implementation methods for location switching and fusion in the basic mode, particularly linear prediction methods, distance threshold-based triggering strategies, and scalar deviation fusion methods in scenarios with limited computing resources or open straight roads. These methods are suitable for degraded modes or non-complex road conditions with lower requirements for algorithm complexity. Specifically, this approach includes:

[0039] Step 201: Extract the vehicle's current planar coordinates, composite speed, and heading angle from the motion state data.

[0040] In this embodiment, the system parses the real-time acquired motion status data packets. The current planar coordinates typically refer to the two-dimensional coordinates of the vehicle in a local navigation coordinate system with the test site as the origin, denoted as (x...). _0 y _0 The composite velocity, denoted as v, refers to the scalar velocity of the vehicle along its direction of travel. The heading angle, denoted as θ, is the angle between the vehicle's heading and the north direction (or the X-axis) of the coordinate system. These basic parameters constitute the input reference for subsequent linear prediction.

[0041] Step 202: Decompose the synthesized velocity into an eastward velocity component and a northward velocity component along the heading angle; multiply the eastward velocity component and the northward velocity component by a preset time window to obtain the displacement increment; superimpose the displacement increment onto the current plane coordinates to obtain the predicted vehicle pose.

[0042] Alternatively, the decomposed composite speed is determined by the vehicle's east-west and north-south directions; the distance the vehicle moves in each direction is calculated by multiplying the speeds in both directions by a pre-set time window; this distance is then added to the vehicle's current position to obtain the vehicle's predicted position and direction of travel.

[0043] In this embodiment, the position extrapolation step uses a uniform linear motion model (CV model). Specifically, the system decomposes the scalar velocity v into two orthogonal components: an eastward velocity component v0. _x =v×sin(θ) and the northward velocity component v _y =v×cos(θ), assuming the heading angle is positive north-northeast. Next, using a preset time window T, calculate the displacement increment during that time period: Δx=v _x ×T,Δy=v _y ×T.

[0044] Based on this, the displacement increment is superimposed onto the current coordinates to obtain the predicted coordinates (P). _pred_x P _pred_y ), where P _pred_x =x _0 +Δx, P _pred_y =y _0 +Δy.

[0045] It should be understood that this linear extrapolation method has a relatively low computational cost, making it suitable for fast processing in embedded systems. However, when the vehicle is turning, this model will produce a lateral deviation. For example, if the vehicle is traveling on a circular arc of radius R, after time T, the lateral deviation δ between the actual position and the linearly predicted position is approximately δ = R × (1 - cos(v × T / R)). When v = 5 m / s, T = 20 s, and R = 15 m, the deviation may reach the order of 15 to 25 meters. Therefore, the method in this embodiment is mainly used as an optional solution in non-curving scenarios, or as a backup solution when system resources are limited.

[0046] Step 203: Calculate the shortest geometric distance from the predicted vehicle pose to the polygon boundary of the preset assessment area;

[0047] If the shortest geometric distance is less than the cutting distance threshold, an intervention strategy is generated to either start preheating or switch to high-precision positioning mode.

[0048] If the shortest geometric distance is greater than the cut-out distance threshold, an intervention strategy is generated to maintain or switch to the basic positioning mode;

[0049] Among them, the cutout distance threshold is greater than the cutin distance threshold.

[0050] Accordingly, the preset assessment region is defined as a polygon enclosed by a series of vertex coordinates. In other words, the preset assessment region is defined by the sequence of vertices of the polygon's boundary. The system calculates the shortest Euclidean distance from the predicted coordinate point to all edge segments of the polygon, denoted as d. _min To prevent vehicles from lingering near the critical region and causing frequent mode switching (the ping-pong effect), this step introduces a hysteresis comparison mechanism. The system sets two thresholds: a smaller cut-in distance threshold D... _in For example, 15 meters and a larger cut-out distance threshold D _out For example, 20 meters.

[0051] Furthermore, when a vehicle is detected approaching the assessment area and d _min Less than D _in When the system determines that it is about to enter the high-precision requirement area, it immediately triggers the preheating or switching command of the high-precision module. When it detects that the vehicle is moving away from the assessment area and d _min Greater than D _out When the system determines that it has moved far from the high-precision requirement area, it triggers a hibernation or degradation command. When the distance is within D... _in and D _out During this period, maintain the current state.

[0052] Step 204: When both the base positioning module and the high-precision positioning module output observation data at the same epoch, calculate the Euclidean distance difference between their positioning coordinates; smooth the Euclidean distance difference using an exponentially weighted moving average algorithm and update the system bias variable; during the observation update phase of the state estimation filter, use the updated system bias variable to additively compensate for the observation data from the base positioning module. Here, the system bias variable is a scalar representing the isotropic position error; in other words, it is a scalar characterizing the isotropic position error between the base positioning module and the high-precision positioning module.

[0053] Specifically, the systematic deviation variable is simplified to an isotropic scalar b. _mode δ is used to represent the magnitude of the position drift of the basic positioning module. When the system is in a dual-mode coexistence state, i.e., the high-precision module has completed its preheating and the basic module is also working, the system calculates the distance difference δ between the output coordinates of the two at each time step k. _k =||P _INS_k -P _RTK_k ||;where k represents the time step of the system operation, which is a discretized unit of time measurement, representing the time node for each deviation calculation; P _INS_k The output coordinate values ​​of the time-based positioning module (INS) are the position coordinates calculated by the INS at time step k, and are spatial coordinate vectors; P _RTK_k δ represents the output coordinates of the high-precision module (RTK) at time k. It is the high-precision position coordinate output at time step k after the high-precision module has warmed up, and is a spatial coordinate vector; _k The difference in output coordinate distance between the two types of modules at time k is a scalar quantization result of the dual-mode position deviation, reflecting the degree of positional offset of the basic positioning module relative to the high-precision module at time step k. In the formula, ||…|| is the vector modulus operation, which is used to convert the difference between two spatial coordinate vectors into a scalar Euclidean distance, realizing an intuitive quantization of the dual-mode position deviation.

[0054] Furthermore, the bias estimate is updated using the exponentially weighted moving average (EWMA) algorithm, i.e.:

[0055] b _mode_k =λ×b _mode_k-1 +(1-λ)×δ _k ;

[0056] Here, λ is the forgetting factor, typically ranging from 0.9 to 0.99, used to adjust the dependence on historical data and smooth the response speed. That is, b _mode_k Let b be the scalar value representing the position drift of the base positioning module after the system update at time step k. This scalar value serves as the latest estimate of the position drift magnitude of the base positioning module at that moment. It is an isotropic scalar value that only represents the magnitude of the drift. _mode_k-1δ is the historical scalar value of the system's position drift at time step k-1, which is the position drift estimate obtained by the EWMA algorithm at the previous time step; λ is the forgetting factor, ranging from 0.9 to 0.99, used to adjust the algorithm's dependence on historical drift data and the smoothness of its response to the current deviation. The closer λ is to 1, the higher the weight of historical data, the stronger the algorithm's smoothness, and the smoother the response to sudden deviations; 1-λ is the weighting coefficient of the current distance difference, complementary to the forgetting factor λ, which determines the contribution of the current dual-mode position deviation to this drift scalar update; _k The distance difference between the output coordinates of the basic positioning module and the high-precision module at time step k is denoted as k.

[0057] In subsequent moments, if the system switches back to a mode where only the basic positioning module operates, the filter will use the latest b... _mode_k The position observations output by the basic module are corrected, i.e., P _corrected =P _INS -b _vec , where b _vec It is based on b _mode_k The correction vector P constructed with the current direction of motion _corrected P corresponds to the final position output value of the basic positioning module after error correction. _INS This corresponds to the original position observations that have not been corrected by the basic positioning module. This method can eliminate trajectory jumps caused by system errors during mode switching.

[0058] On the other hand, it describes a specific implementation method for path integral prediction based on road topology constraints, which is applicable to nonlinear motion scenarios such as S-shaped curves and turnaround routes in closed driving test sites, and can eliminate the centrifugal bias caused by linear extrapolation.

[0059] Step 301: Pre-construct a road network topology model; the road network topology model is represented by a directed graph structure, which includes a set of nodes and a set of directed edges; each directed edge in the set of directed edges corresponds to a road segment in the site, and contains geometric parameters describing the shape of the road segment; the geometric parameters include the starting coordinates, azimuth angle, length and curvature information of the road centerline.

[0060] In this embodiment, the system pre-loads a digital map of the closed site and parses it into a directed graph structure G=(N, E). Here, the node set N represents the connection points, endpoints, or branching points of roads; the directed edge set E represents specific road segments. For each directed edge, its associated geometric parameters are used to accurately describe the shape of the road segment. Specifically, for straight road segments, the parameters include the starting coordinates, azimuth angle, and segment length; for circular road segments, the parameters include the center coordinates, radius R, starting phase angle, segment length, and curvature κ. The curvature κ = 1 / R or -1 / R, with the sign used to distinguish between left and right turns. This parametric modeling method can restore the site roads to computable geometric primitives.

[0061] Step 302: Calculate the projection distance from the vehicle's current planar coordinates to each candidate road segment in the road network topology model, and determine the corresponding projection point; calculate the angle between the vehicle's current heading angle and the road tangential heading angle at the projection point; select road segments whose projection distance is less than the distance matching threshold and whose angle is less than the heading consistency threshold as matching road segments; obtain the arc length parameter of the projection point on the matching road segment as the current projection arc length.

[0062] Alternatively, calculate the projected distance from the vehicle's current position to each candidate road, and find the projection point corresponding to each distance; next, calculate the angle difference between the vehicle's current driving direction and the direction of the road where the projection point is located; based on roads whose distance is less than the set standard and whose angle difference meets the requirements, filter out matching road segments; further calculate the arc length of the projection point on that road, which is the current projected arc length.

[0063] In this embodiment, the system performs a map matching operation, snapping vehicles from free coordinate space onto the topological road network. For each candidate road segment, the system calculates the projection of the vehicle's current coordinates onto the centerline of that road segment. If it is a straight segment, the projection point P... _proj The projection point is the foot of the perpendicular from the vehicle's current coordinates onto the straight line; if it is an arc segment, the projection point is the intersection of the line connecting the vehicle's current coordinates to the center of the circle and the arc. In this case, the Euclidean distance between the vehicle's current coordinates and the projection point is the projection distance.

[0064] Further, the system calculates the deviation between the road tangent direction (i.e., road orientation) at the projection point and the vehicle's current measured heading angle, denoted as Δθ. To eliminate mismatches involving reverse or parallel roads, the system retains only road segments that meet the following two conditions: the projected distance is less than a preset distance threshold, such as 3 meters, and Δθ is less than a preset heading threshold, such as 30 degrees. Among the selected candidate segments, the segment with the smallest projected distance is typically chosen as the final matched road segment e. _match Based on this, the system calculates the length along the road segment corresponding to the projection point, and defines it as the current projected arc length s. _match .

[0065] Step 303: Calculate the predicted travel arc length based on the vehicle's current composite speed and the preset time window;

[0066] Starting from the matched road segment and with the current projected arc length as the initial position, perform path integration along the directed edge direction of the road network topology model;

[0067] When the integral length reaches the predicted driving arc length, the corresponding planar coordinates and tangential heading are calculated based on the geometric parameters of the road segment where the endpoint is located, and the predicted vehicle pose is obtained.

[0068] If the endpoint of a road segment is reached during the integration process, the next connecting road segment is selected according to the preset path branch decision rules to continue integration until the remaining predicted driving arc length is zero.

[0069] The prediction process is transformed into a path integration process on the topology graph. Accordingly, the total distance traveled by the vehicle within the prediction window T is calculated, i.e., the predicted travel arc length S. _total =v×T. Then, from the current position (e _match s _match Start moving along the road. System initialization: Remaining distance to be predicted: S _rem =S _total .

[0070] In the integration loop, the remaining available length L of the current road segment is calculated. _avail =e _curr .length-s _curr ;

[0071] In the formula, e _curr .length represents the current target number divided by the total length of the sequence, e _curr The target data / sequence object representing the current moment; `.length` represents the object's inherent property, used to obtain its overall length / total number of elements; s _curr This represents the currently used / offset length, indicating the length of the target data / sequence that has been consumed, read, or offset at the current moment. It is a non-negative scalar value, and its value reflects the degree of data utilization, or in other words, s. _curr The arc length parameter corresponding to the current position.

[0072] If S _rem ≤L _avail This indicates that the predicted endpoint falls within the current road segment, and the arc length parameter of the final position is s. _final =s _curr +S _rem The system directly calculates s based on the geometric parametric equations of the current road segment. _final The coordinates of point P _finalThis is the predicted pose. If S _rem >L _avail This indicates that the predicted path has exceeded the current road segment, and the vehicle will cross the node to enter the next road segment. At this time, the system updates the remaining distance S. _rem =S _rem -L _avail Set the current segment pointer to the next connected segment and reset the arc length s. _curr =0, continue to the next round of scoring.

[0073] When encountering a road fork, i.e., a node with multiple outgoing edges, the system selects the next connecting road segment based on path branching decision rules. These rules may include: prioritizing the straight road segment with the smallest angle to the current driving direction; or, if an assessment task has been loaded, prioritizing road segments that conform to the standard assessment route sequence. Through this integral prediction along the road geometry, even in continuous S-curves or hairpin bends, the predicted trajectory can always adhere to the road centerline, reducing the prediction error from meters to decimeters or even centimeters, which is beneficial for achieving accurate subsequent spatiotemporal matching.

[0074] An example describes an optional technical solution for multi-region parallel progressive preheating scheduling, particularly how to solve the preheating lag or resource conflict caused by a single threshold trigger by calculating comprehensive priorities and phased scheduling when facing multiple consecutive assessment regions. Specifically, it can be implemented by performing the following steps:

[0075] Step 401: Based on the predicted vehicle pose, identify at least one reachable assessment area that the vehicle will pass through within a preset time window.

[0076] Calculate the estimated arrival time of the vehicle to the reachable assessment area;

[0077] The urgency of preheating is calculated based on the estimated arrival time, and the overall preheating priority is calculated based on the positioning accuracy requirements of the reachable assessment area.

[0078] All reachable assessment areas are sorted according to the overall preheating priority to construct a regional preheating priority queue, which provides a basis for generating intervention strategies for high-precision positioning modules.

[0079] The preset assessment area is pre-configured with positioning accuracy requirements.

[0080] Furthermore, based on the path integral prediction results, the system obtains the predicted trajectory of the vehicle over a future period (e.g., 30 seconds). The system overlays this trajectory with the assessment area layout on the electronic map, identifying all areas that intersect with or are adjacent to the predicted trajectory; these areas are the reachable assessment areas. For each reachable assessment area, the system calculates the predicted driving arc length S corresponding to the entry point of that area based on the path integral. _jGiven the current vehicle speed v, calculate the estimated arrival time t. _arrive_j .

[0081] Furthermore, the system calculates quantified ranking indicators for each region. Positioning accuracy requirement level G _j These are parameters pre-configured in the map attributes, with values ​​ranging from 1 to 5. For example, the reverse parking task requires high positioning accuracy and is configured as level 5; while the straight driving task is configured as level 2.

[0082] Among them, the preheating urgency U _j This reflects whether there is sufficient remaining time. Let T be the standard time it takes for a high-precision positioning module to go from cold start to usability. _j For example, 38 seconds, the current time is t. _curr The formula for calculating urgency can then be expressed as:

[0083] U _j =max(0, 1-(t) _arrive_j -t _curr -T _j ) / T _j );

[0084] Here, max(...) corresponds to the maximum value function. When the remaining time is less than the standard preheating time, the urgency level reaches its maximum value of 1; when the remaining time is very ample, the urgency level is 0.

[0085] Based on this, the system calculates the comprehensive preheating priority P for each region. _j The calculation formula is as follows:

[0086] P _j =ω _G ×(G _j / 5)+ω _U ×U _j +ω _S ×S _j ;

[0087] Where, ω _G ω _U ω _S These are preset coefficients for precision weight, urgency weight, and sequence coherence weight, respectively, and the sum of the three is 1. For example, let ω... _G =0.4, ω _U =0.4, ω _S =0.2. S _j It is a Boolean indicator; if the region is the first region on the prediction path, then S... _j Select 1 if the condition is met, otherwise select 0. This formula allows the system to balance importance and urgency, prioritizing areas that are both important and imminent. Based on this, the system processes data according to P... _jThe regions are sorted in descending order of value to generate a region preheating priority queue.

[0088] Step 402: Generate the intervention strategy for the high-precision positioning module, specifically including formulating a progressive preheating scheduling plan based on the regional preheating priority queue; the process of formulating the progressive preheating scheduling plan includes:

[0089] The preheating and startup process of the high-precision positioning module is broken down into the signal acquisition stage, the differential data synchronization stage, and the ambiguity resolution stage.

[0090] For the region with the highest priority in the regional preheating priority queue, the ideal start time of each stage is calculated in reverse based on its expected arrival time and the typical time consumption of each stage.

[0091] A sequence of control commands containing the start times of each stage is generated as an intervention strategy to trigger the high-precision positioning module to perform preheating operations in stages.

[0092] Furthermore, the system breaks away from the traditional one-click full-on warm-up mode and adopts a phased, gradual strategy. Specifically, the initialization process of the high-precision positioning module is broken down into the following independent logical stages (typical time consumption examples are in parentheses):

[0093] Phase 1 Φ _1 Satellite signal acquisition and tracking (5 seconds);

[0094] Phase Two Φ _2 Pseudorange single-point positioning calculation (3 seconds);

[0095] Phase Three Φ _3 Carrier phase tracking loop established (10 seconds);

[0096] Phase Four Φ _4 Differential data link connection and synchronization (5 seconds);

[0097] Phase 5 Φ _5 ,Full-cycle fuzzy search and fixation (15 seconds).

[0098] The total time for each stage is approximately 38 seconds.

[0099] Based on this, the system selects the head region in the regional preheating priority queue and reads its estimated arrival time t. _arrive_top The system is set with a safety margin T. _margin For example, 5 seconds, the ideal start time t for each stage is calculated using a reverse calculation algorithm. _start_k This ensures that the module is in optimal condition when the vehicle arrives at the area. The calculation logic is as follows:

[0100] t _end_total =t_arrive_top -T _margin ;t _start_Φ_5 =t _end_total -15 seconds; t _start_Φ_4 =t _start_Φ_5 -5 seconds; ...; t _start_Φ_1 =t _start_Φ_2 -5 seconds; and so on, the system obtains a timeline control sequence accurate to the second. When the system clock reaches t... _start_Φ_1 At that time, only the RF front-end is triggered to capture the signal; when t is reached... _start_Φ_4 Only then is a network connection established to receive differential data.

[0101] Above, t _end_total The ideal time for the completion of all preheating stages of the corresponding positioning module is a scalar, which is the time endpoint reference for the execution of each preheating stage, t. _start_Φ_q (q=1, 2, 3, 4, 5) corresponds to the ideal start-up time of the q-th preheating stage, and is a scalar.

[0102] Optionally, if there are multiple regions with very close time intervals in the priority queue, such as intervals less than 10 seconds, the system will adopt a parallel sharing strategy. That is, for Φ _1 To Φ _4 In the foundational stage, multiple regions share the same pre-heating results, only in Φ _5 The stage performs independent fuzzy resolution optimization for different regional geometric features, further improving resource utilization efficiency.

[0103] Another example describes an alternative implementation of high-precision fusion based on a spatial vector database. This embodiment utilizes three-dimensional vectors to expand the state space and incorporates historical prior information using a spatial database based on road arc lengths, maintaining high positioning accuracy even in scenarios where satellite signals are blocked or multipath effects are severe. Specifically, this embodiment may include:

[0104] Step 501: The system deviation variable is a three-dimensional deviation vector, which contains three independent deviation components corresponding to the eastward, northward, and elevation directions, respectively.

[0105] The state transition process of the state estimation filter uses an anisotropic deviation dynamic model, which is configured with different error accumulation correlation time constants and process noise intensity parameters for the three independent deviation components.

[0106] The anisotropic deviation dynamic model includes a deviation accumulation coefficient function.

[0107] When performing state prediction, the state estimation filter reads the vehicle's composite speed, rate of change of heading angle, and composite acceleration in real time.

[0108] The process noise intensity parameter is dynamically adjusted using the deviation cumulative coefficient function, so that the process noise intensity parameter is positively correlated with the composite velocity, the rate of change of heading angle, and the composite acceleration.

[0109] In this embodiment, the system abandons the scalar error model and constructs a three-dimensional deviation vector b. _mode =[b _E , b_N b _U ] T Among them, b _E This represents an eastward positional deviation. b_N Represents the northward position deviation, b _U Represents elevation deviation. T This represents the transpose operation. This vectorized representation takes into account the drift independence of inertial devices (gyroscopes and accelerometers) in different axes. For example, the zero-bias drift of a yaw gyroscope mainly causes horizontal position error to accumulate orthogonally over time, while the zero-bias of a vertical accelerometer mainly affects elevation error.

[0110] Furthermore, this embodiment introduces an anisotropic bias dynamic model. In the state prediction stage of the Extended Kalman Filter (EKF), the state transition equation for the bias vector is expressed as:

[0111] b _mode (k+1)=F _b ×b _mode (k)+w _b (k);

[0112] Among them, F _b It contains different time decay factors (β) _E ,β _N ,β _U A diagonal matrix of . w _b (k) is the process noise vector, and its covariance matrix Q _b This reflects the degree of uncertainty the system has about the current deviation prediction value, b _mode (k+1) is the predicted value of the deviation vector at time k+1, b _mode (k) corresponds to the current value of the deviation vector at time k.

[0113] Optionally, the system introduces a deviation accumulation coefficient function γ(m). This function is based on the real-time acquired motion state data m=[v, ω, a]. T To perform the calculation, that is:

[0114] γ(m)=γ _0 ×(1+k _v ×|v|+k _ω ×|ω|+k _a ×|a|);

[0115] Where, γ _0 k is the static reference coefficient. _v k _ω k _a These are the weighting coefficients for velocity, angular velocity (rate of change of heading), and acceleration, respectively, where v, ω, and a correspond to velocity, angular velocity (rate of change of heading), and acceleration, respectively. The system uses this function to dynamically adjust the process noise intensity Q. _b_current =Q _b_base ×(γ(m)) 2 ; where Q _b_base The corresponding process noise covariance basis matrix is ​​the system's preset benchmark value.

[0116] Through this mechanism, when the vehicle is in a state of violent maneuvering (such as sharp turns, rapid acceleration and deceleration), γ(m) increases, resulting in increased process noise. This forces the filter to be more inclined to believe the observed data (or prior data) rather than the predicted data in subsequent steps. Conversely, when traveling at a constant speed in a straight line, the filter relies more on the predicted smoothing.

[0117] In step 502, during the process of fusing multi-mode positioning observation data using state estimation filters, a pre-built bias spatial database is also utilized.

[0118] The deviation spatial database adopts a one-dimensional index structure based on roads, using the unique identifier of road segments and the road arc length parameter as the primary index key, and stores the statistical feature values ​​of the three-dimensional deviation vector estimated at the corresponding location at historical times;

[0119] The fusion process includes: determining the current matching road segment identifier and the current projected arc length of the vehicle, and then retrieving candidate deviation records in the neighborhood from the deviation space database based on these (matching road segment identifier and current projected arc length).

[0120] The system pre-builds and maintains a deviation spatial database. Unlike traditional two-dimensional databases based on latitude and longitude grids, this embodiment uses a one-dimensional index structure based on road topology. The database's primary key consists of two parts: {Road_ID, Arc_Length}. Road_ID is the unique identifier for the road segment, and Arc_Length is the arc length coordinate along the road's centerline. For each index location, the database stores the statistical characteristic values ​​of the three-dimensional deviation vector calculated historically when passing through that location, mainly including the deviation mean vector and the deviation covariance matrix.

[0121] This one-dimensional index structure is used to address the problem of spatial proximity but topological disparity. For example, in U-shaped or S-shaped curve scenarios, two geographically adjacent points (such as roads on either side of the curve) may correspond to different vehicle motion states and error accumulation patterns. If a two-dimensional grid index is used, the error characteristics of the two locations can easily be confused; however, by using a road arc length index, the error characteristics of the curve's entrance and exit segments can be distinguished.

[0122] Step 503: For each index position, the deviation space database stores forward driving deviation sub-records and reverse driving deviation sub-records respectively; retrieving candidate deviation records in the neighborhood also includes: real-time identification of the vehicle's current driving direction, extracting data from the forward driving deviation sub-records when the vehicle is traveling along the road segment's defined direction, and extracting data from the reverse driving deviation sub-records when the vehicle is traveling against the road segment's defined direction.

[0123] Accordingly, considering that armored vehicle driving assessments include numerous reversing maneuvers, such as reversing into a parking space and parallel parking, the vehicle's error accumulation characteristics are highly correlated with the direction of travel. For example, the odometer scaling factor error often exhibits different signs or magnitudes when moving forward and backward, and the projection direction of the position error by heading drift also reverses. Therefore, the database employs a layered design in its physical storage. For the same {Road_ID, Arc_Length} index, the system maintains separate forward and reverse sub-records.

[0124] During operation, the system detects the vehicle's longitudinal velocity v. _long The current driving direction is identified by symbols or gear shift signals. If v _long If v ≥ 0, the system only retrieves forward sub-records; if v _long If the value is less than 0, the system only retrieves the reverse sub-record. This mechanism ensures that when performing operations such as reversing into a parking space, the system does not introduce historical errors from forward driving as prior knowledge, thus guaranteeing the physical correctness of deviation compensation.

[0125] Step 504: Retrieve candidate deviation records in the neighborhood from the deviation space database, including: calculating the shortest path distance between the vehicle's current location and each sampling point in the deviation space database along a pre-constructed road network topology model; calculating spatial relevance weights based on the shortest path distances, with the spatial relevance weights decreasing as the shortest path distance increases; and performing weighted fusion of the retrieved candidate deviation records according to the spatial relevance weights to obtain the spatial prior deviation of the current location.

[0126] Accordingly, the system needs to search for nearby records in the database to obtain the prior bias of the current location. Here, "nearby" does not refer to proximity in Euclidean distance, but rather proximity along the road travel path. The system utilizes a pre-loaded road network topology model and employs Dijkstra's algorithm or a pre-computed distance lookup table to calculate the shortest path distance d from the current vehicle's location to each sampling point in the database. _path_i , where i is the index of the sampling point.

[0127] Furthermore, the system calculates the spatial relevance weight w for each candidate record. _i The weighting function uses a Gaussian decay form, specifically:

[0128] w _i =exp(-(d _path_i ) 2 / (2×(L _corr ) 2 ));

[0129] Among them, L _corr Here, is a spatial correlation length constant, for example, 10 meters, and exp(...) is an exponential function. This formula shows that the greater the distance along the road path, the lower the correlation of historical errors. In some implementations, the weight calculation can also incorporate a time decay factor, i.e., the longer the time since the last update, the lower the weight. Based on this, the system performs a weighted average of all valid candidate records within the retrieval range:

[0130] b _prior =(Σw _i ×μ _b_i ) / (Σw _i );

[0131] Σ _prior =(Σ(w _i ) 2 ×Σ _b_i ) / (Σw _i ) 2 ;

[0132] Among them, b _prior The corresponding spatial prior bias has an uncertainty of Σ. _prior μ _b_i Σ _b_i These correspond to the mean vector and covariance matrix of the i-th deviation, respectively. Alternatively, a valid candidate record refers to a candidate record that has been filtered to meet requirements such as data completeness, timeliness, and matching degree, after removing invalid / abnormal / exceeding threshold data. This yields the spatial prior bias and its uncertainty at the current location.

[0133] Optionally, calculate the comprehensive relevance weight w. _iAlternatively, the following spatiotemporal joint attenuation formula can be used:

[0134] w _i =exp(-(d _path_i ) 2 / (2×(L _corr ) 2 ))×exp(-Δt _i / τ _decay );

[0135] Where, Δt _i τ represents the time difference between the current time and the last time this record was updated in the database; _decay This is a preset time decay constant, such as 7 days or 168 hours. The formula clarifies that when records in the database become too old, their contribution to the current fusion will decrease exponentially, giving higher confidence to more recent data.

[0136] Step 505 involves fusing multi-mode positioning observation data using a state estimation filter. Specifically, this includes: using spatial prior bias as virtual observation values ​​to construct a prior observation equation; using the prior observation equation to measure and update the predicted state vector of the state estimation filter to obtain the corrected system bias variable estimate; and using the corrected system bias variable estimate to compensate for the observation data of the basic positioning module when the high-precision positioning module data is unavailable.

[0137] In this step, the system treats spatial prior bias as virtual observations. During the EKF measurement update phase, the following prior observation equations are constructed:

[0138] z _prior =H _prior ×X+v _prior ;

[0139] Among them, z _prior The value is b _prior The state vector X contains the three-dimensional bias components to be estimated; the observation matrix H _prior It is the selection matrix, used to extract the deviation components from the state vector; v _prior It is observation noise, and its covariance matrix R _prior The value is Σ _prior .

[0140] Or rather, z _prior Let H be the prior virtual observation vector, a three-dimensional spatial vector, whose value is the preprocessed spatial prior bias. _prior The observation selection matrix is ​​a dimension-matched linear matrix, X is the EKF predicted state vector, which is a multidimensional vector and represents the prior state values ​​obtained by the filter during the state prediction stage. _prioris the virtual observation noise vector, a three-dimensional random vector representing the random errors introduced during the virtual observation process, such as prior bias estimation errors and model adaptation errors.

[0141] Furthermore, the system performs standard Kalman filter update steps, calculates the Kalman gain K, and uses prior observations z. _prior The predicted state is corrected. This process uses historical experience to constrain the current deviation estimate. Especially when the high-precision positioning module has not yet warmed up or the signal is briefly blocked, this spatial prior-based correction can quickly bring the system deviation back to the true level. Based on this, the system uses the corrected deviation variable b. _mode_corrected The original coordinates P output by the basic positioning module _INS To compensate, i.e.: P _final =P _INS -b _mode_corrected It outputs high-precision continuous positioning results. Or, in other words, P _final This corresponds to high-precision continuous positioning results.

[0142] Another example provides a specific implementation scheme for robust maintenance and closed-loop management of the system, especially the online incremental update mechanism for the deviation space database and the trajectory smoothing strategy when the positioning mode degrades. This helps the system achieve self-evolutionary capability of becoming more accurate with use over long-term operation, as well as a fallback protection mechanism under abnormal conditions. Accordingly, this embodiment can be implemented in the following way:

[0143] Accordingly, online incremental updates are performed on the deviation space database. That is, when the high-precision positioning module is in an effective working state, the residual between the current state estimate and the observed value is calculated. Based on the residual, the matching road segment identifier where the vehicle is currently located, and the current projected arc length, the target deviation record in the deviation space database is located. Using the incremental mean update algorithm and the online covariance update algorithm, the statistical feature value of the three-dimensional deviation vector in the target deviation record is updated according to the residual.

[0144] Furthermore, the system not only utilizes the database to assist in positioning, but also uses the high-precision positioning results to correct the database, forming a closed loop. When the RTK module is in a fixed solution state and the signal quality is good, the system considers the current observation value to be close to the true value. At this time, the system calculates the predicted position P calculated by the inertial navigation system (or the basic positioning module). _INS With RTK observation location P _RTK The difference between them yields the instantaneous deviation observation value r=P _INS -P _RTKTo incorporate this observation into the historical record, the system searches for the corresponding target deviation record unit in the database based on the vehicle's current matching result (Road_ID, Arc_Length).

[0145] Based on this, this embodiment uses the Welford online algorithm for incremental updates of statistics. This algorithm only needs to store the current mean vector μ and auxiliary matrix M2, without needing to save all historical samples. The update logic is as follows:

[0146] Accordingly, update the sampling count n _new =n _old +1.

[0147] Furthermore, the difference between the current residual and the historical mean is calculated as δ = r - μ. _old .

[0148] Furthermore, update the mean vector μ _new =μ _old +δ / n _new .

[0149] Next, update the auxiliary matrix M2. _new =M2 _old +δ×(r-μ _new ) T .

[0150] Based on this, the updated covariance matrix Σ is calculated. _new =M2 _new / (n _new -1).

[0151] It should be understood that the above multiplication is the outer product operation of vectors. _new Represents the updated cumulative sample count, n _old The cumulative sample count before the update is represented by δ, which represents the difference vector between the current residual and the historical mean, used to quantify the deviation of the current sample residual from the historical mean; r represents the residual vector to be updated, μ _old This represents the historical mean vector before the update, symbolizing the average level of all samples up to the previous sampling time, μ. _new M2 represents the updated mean vector. _new The corresponding updated auxiliary matrix stores information related to the cumulative squared sum of sample deviations from the mean, M2. _old The corresponding auxiliary matrix before the update, Σ _new The corresponding updated covariance matrix is ​​a symmetric positive definite square matrix, which quantifies the dispersion of the sample data and the correlation between various dimensions.

[0152] Through streaming computing, the deviation model in the database can absorb the latest observation data in real time, gradually approximating the actual error characteristics of vehicles on specific road sections. Moreover, the numerical calculation process has high stability and is less prone to precision loss due to the addition of large numbers.

[0153] Optionally, in addition to database updates, the system also includes a degradation protection mechanism for RTK signal anomalies. When the system detects that the RTK differential data age exceeds a preset threshold (e.g., 5 seconds) or the solution state reverts to a floating-point solution, the system will automatically determine that the high-precision mode is unreliable and immediately switch back to the basic positioning mode, i.e., use prior bias for compensation.

[0154] Based on this, the system uses a cubic spline interpolation algorithm to smoothly connect the trajectories before and after the switching point, in order to eliminate possible position jumps that may occur during mode switching. Specifically, the system uses the high-precision position and velocity within the time window before the switch, as well as the position and velocity in the basic mode after the switch, to construct a spline function, ensuring that the position and velocity (first derivative) at the splicing point are continuous, thus preventing misjudgments by the downstream assessment system due to data jumps.

[0155] According to one aspect of this application, after performing incremental updates to the mean and covariance matrices, the system also performs a timestamp update operation, i.e., t _last_update_new =t _current In other words, the updated timestamp is the timestamp of the current moment.

[0156] The updated timestamp is written back to the target record in the deviation spatial database. This enables the database to not only reflect the spatial distribution of errors but also to dynamically track the time-varying characteristics of errors as they drift over time, such as the slow drift of sensor zero bias caused by seasonal temperature changes.

[0157] According to another aspect of this application, an exemplary scheme for a dual-mode adaptive positioning device is provided, which is used to implement the hardware device architecture and functional module division of the method of the present invention. Specifically:

[0158] Step 701, the data acquisition module is configured to acquire the vehicle's current motion status data and the positioning module's operating status data in real time.

[0159] The device can be implemented as an in-vehicle embedded terminal or an edge computing box. The data acquisition module specifically includes hardware interface circuitry and drivers. The hardware interface circuitry can include a CAN bus interface, RS-232 serial port, or Ethernet interface, used to connect to the vehicle chassis controller, inertial measurement unit (IMU), and BeiDou positioning receiver. The drivers are responsible for parsing the raw messages according to the communication protocols of each sensor, extracting key data such as synthesized velocity, heading angle, and satellite signal-to-noise ratio, and storing them in a shared memory area for subsequent modules to access.

[0160] Step 702, the prediction decision module is configured to predict the vehicle's pose within a future preset time window based on motion state data, and generate an intervention strategy for the high-precision positioning module based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area.

[0161] In other words, the predictive decision-making module can make predictions based on the vehicle's driving status and, combined with the road conditions, obtain the next decision based on the vehicle's location.

[0162] In this embodiment, the prediction and decision-making module is the brain of the system, typically running on a high-performance microprocessor (such as the ARM Cortex-A series). This module integrates a map engine and a prediction algorithm library. The map engine is responsible for loading a pre-built road network topology model and providing spatial query services. The prediction algorithm library stores linear extrapolation algorithms and path integration algorithms. During runtime, this module selects an appropriate prediction algorithm based on configuration parameters, calculates the vehicle trajectory for the next 15 to 30 seconds, and determines whether the trajectory intersects with a preset assessment area polygon. If they intersect, the module further calls a multi-area scheduling subroutine to calculate the warm-up priority of each area and generate a sequence of control instructions containing accurate timestamps, i.e., the intervention strategy.

[0163] Step 703: The fusion control module is configured to respond to the intervention strategy to control the working state of the high-precision positioning module. It uses a state estimation filter to fuse multi-mode positioning observation data from the basic positioning module and the high-precision positioning module. During the fusion process, the state vector containing system deviation variables is used to perform real-time deviation compensation on the observation data to obtain continuous positioning data.

[0164] In other words, the fusion control module follows the intervention strategy, adjusts the corresponding state of the high-precision positioning module, fuses the data of basic positioning and high-precision positioning, performs real-time deviation compensation during fusion, and obtains continuous positioning data.

[0165] The fusion control module is responsible for executing the specific positioning calculation tasks. At the control level, this module controls the RTK module's power management chip via general-purpose input / output (GPIO) pins or serial port commands, enabling switching between low-power and full-function modes. At the algorithm level, this module runs an extended Kalman filter (EKF) program. This program maintains a high-dimensional state space containing position, velocity, attitude, and three-dimensional deviation vectors. When high-precision observation data triggered by an intervention strategy is received, the filter performs measurement updates; when only basic observation data is available, the filter compensates for spatial prior biases.

[0166] In addition, this module includes a database maintenance sub-thread responsible for updating the deviation spatial database in the background using the Welford algorithm. Based on this, the module outputs a smoothed, continuous positioning data stream to the assessment system via UDP or in-vehicle Ethernet.

[0167] Optionally, the apparatus may include a memory and a processor. The memory stores computer programs, as well as a pre-built road network topology model and deviation spatial database; the processor executes the computer programs to implement the methods of the embodiments described above. The memory may specifically include high-speed random access memory (RAM) and non-volatile memory (such as Flash memory or embedded multimedia card eMMC). The processor may specifically include a digital signal processor (DSP), a field-programmable gate array (FPGA), or a central processing unit (CPU). This hardware-software co-design enables the system to provide high-precision positioning services even under high-speed vehicle movement and complex electromagnetic environments.

[0168] In this invention, some methods may also be:

[0169] The driving assessment scenario configuration parameters are extracted from the driving training tasks issued by the backend server. The current assessment subject type, the geographical boundaries of the training ground, and the spatial definition information of each key assessment point are analyzed. Key assessment points include the fixed-point parking judgment line, the boundary of the reversing parking area, and the start and end points of obstacle passage. Each key assessment point is described by a sequence of polygon vertex coordinates, and the corresponding positioning accuracy requirement level for that area is labeled. The above analysis results are structured and organized to generate a scene feature parameter set, including the boundary coordinate sequence of each key assessment point, accuracy level labels, and area type identifiers.

[0170] Furthermore, the positioning coordinates, instantaneous velocity, and heading angle information of the current epoch are read from the raw positioning observation data, and a short-term vehicle position prediction model is constructed based on the assumption of uniform linear motion. The vehicle position at the current moment is assumed to be in planar coordinate form, the velocity to be a scalar value, and the heading angle to be measured clockwise from true north. The prediction time window is set to 15 to 30 seconds based on the typical warm-up time of the RTK module. The raw positioning observation data is the original form of multi-mode positioning observation data from the basic positioning module and the high-precision positioning module.

[0171] The position prediction calculation employs the following method: the current speed is decomposed into eastward and northward components along the heading angle, multiplied by the prediction time window, and then superimposed onto the current coordinates to obtain the vehicle's position at the predicted time. The prediction formula is expressed as follows:

[0172] P _pred_x =P _current_x +v×T _pred ×sin(θ);

[0173] P_pred_y =P _current_y +v×T _pred ×cos(θ);

[0174] Among them, P _pred_x With P _pred_y These are the east and north coordinates of the predicted location, P. _current_x With P _current_y Let v be the current position coordinates, v be the current vehicle speed, and T be the current vehicle speed. _pred The forecast time window is θ, where θ is the heading angle.

[0175] The predicted location coordinates are encapsulated and output as vehicle predicted location data, while the current location coordinates are retained for subsequent fence matching.

[0176] Furthermore, the vehicle's predicted location data and the scene feature parameter set are read, and geofence matching is performed on the current location and the predicted location, respectively. For the polygon boundary of each key assessment point in the scene feature parameter set, the ray method is used to determine whether the target point is located inside the polygon: a ray is drawn from the target point in any direction, and the number of intersections between the ray and each side of the polygon is counted. If the number of intersections is odd, the point is determined to be inside the polygon; otherwise, it is determined to be outside.

[0177] Based on the fence matching results between the current location and the predicted location, the movement trend of the vehicle relative to the key assessment area is determined, and area state variables are defined to distinguish four scenarios:

[0178] When both the current position and the predicted position are within the critical assessment area, it is marked as a persistent state within the area;

[0179] When the current location is outside the region but the predicted location is inside the region, it is marked as about to enter the state;

[0180] When both the current location and the predicted location are outside the region, it is marked as a persistent state outside the region;

[0181] When the current location is inside the region but the predicted location is outside the region, it is marked as an upcoming departure state.

[0182] The regional state variables and their corresponding key assessment point identifiers are encapsulated and output as regional trend determination results.

[0183] Furthermore, the regional trend determination results and dual-mode status monitoring data are read, and the RTK module preheating control logic is executed based on the regional status variables. When the regional status variables indicate that the vehicle is about to enter the critical assessment area, and the current RTK module is in a low-power standby state, a preheating wake-up command is sent to the RTK module to initiate the satellite signal reacquisition and differential data link establishment process, and the preheating start time is recorded.

[0184] During the warm-up period, the main control module maintains the current BeiDou normal positioning mode and continuously extracts the real-time status information of the RTK module from the dual-mode status monitoring data, focusing on monitoring the acquisition of fixed solutions. The condition for completing the warm-up is defined as: the number of epochs in which the RTK module continuously outputs fixed solutions reaches a preset threshold, which is recommended to be 5 or more consecutive epochs, so that the differential calculation enters a stable state.

[0185] If the vehicle changes its direction of travel during the warm-up period, causing the area state variable to change from the state about to enter the area to the state outside the area, the current warm-up process is canceled, a sleep command is sent to the RTK module to return it to the standby state, and the system power consumption budget is released.

[0186] The preheating completion flag, preheating duration, and RTK fixed decryption status are encapsulated and output as RTK preheating status data.

[0187] Furthermore, by reading RTK preheating status data and regional trend determination results, a joint triggering condition for the formal switching of positioning mode is constructed. The formal switching trigger must simultaneously meet the following three conditions:

[0188] The RTK preheating completion flag is true, indicating that the RTK module has entered a stable fixed solution output state;

[0189] The area state variable indicates whether the vehicle is in an upcoming state or a continuous state within the area, indicating that the need for high-precision positioning does indeed exist.

[0190] The preheating duration exceeds the minimum preheating time threshold to avoid misjudgment immediately after preheating starts.

[0191] Optionally, the joint decision logic can be expressed as follows:

[0192] Trigger _switch =Flag _preheat_done ∧(R _status ∈{1,2})∧(t _current -t _preheat ≥T _min );

[0193] Among them, Flag _preheat_done R is the sign that preheating is complete. _status Encoding the region state variable, t _current For the current time, t _preheat For the preheating and startup time, T _min This is the minimum preheating time threshold. (Trigger) _switch This represents the module switching trigger flag, a Boolean variable (value: true / false). When the joint determination result is true, a mode switching trigger signal is output; otherwise, the current positioning mode remains unchanged, and various conditions continue to be monitored. The determination result is encapsulated and output as the mode switching trigger status.

[0194] Furthermore, by reading the mode switching trigger state, vehicle predicted position data, and scene feature parameter set, a hysteresis threshold mechanism is introduced before executing the mode switch to prevent frequent switching caused by boundary jitter. An asymmetric switching distance threshold is defined for the boundaries of key assessment areas: an in-cut distance threshold is used when switching to RTK mode, and an out-of-cut distance threshold is used when switching to normal mode. The out-of-cut distance threshold is greater than the in-cut distance threshold, and the difference between the two is the hysteresis distance.

[0195] Among them, the shortest distance d from the current vehicle position to the nearest critical assessment area boundary is calculated. _boundary The minimum distance from a point to each line segment of the polygon is used:

[0196] d _boundary =min{dist(P _current Edge _i )}, i=1,2,...,n;

[0197] Among them, Edge _i Let n be the i-th edge of the polygon, and n be the total number of edges. The function `min{…}` is used to find the minimum value, and `dist(…)` is the spatial distance calculation function. P _current The corresponding spatial location of the vehicle.

[0198] When the mode switching trigger state is true and the boundary distance meets the cut-in threshold condition, a mode switching command to switch to RTK mode is generated. The RTK positioning module is activated through the main control chip, and the BeiDou ordinary module is switched to standby mode. When the area status variable indicates that it is about to leave and the boundary distance exceeds the cut-out threshold, a mode switching command to switch to ordinary mode is generated, and the reverse switching operation is performed.

[0199] After the switch is completed, the identifier of the current effective positioning mode is updated, and fields such as latitude and longitude, elevation, speed, heading, and positioning timestamp are read from the positioning module corresponding to the effective mode according to the set period. The data is then encapsulated to generate the original positioning observation data and transmitted to the subsequent trajectory smoothing process.

[0200] On the other hand, some methods of the present invention can also be:

[0201] If the current effective positioning mode indicator indicates that the system is operating in RTK differential positioning mode, the differential status fields specific to the RTK module are extracted from the raw positioning observation data. These include differential age, fixed solution and floating-point solution status flags, differential base station communication status, and carrier phase residuals. The above status parameters are continuously collected at a fixed monitoring cycle, and the historical record of the status sequence is maintained within a sliding time window. The RTK signal status parameters are then output for subsequent anomaly detection.

[0202] Optionally, RTK signal status parameters are read, and a multi-condition joint judgment mechanism is used to detect differential signal anomalies and determine whether degradation protection is triggered. Three types of anomaly triggering conditions are set:

[0203] Differential age exceeds the limit. When the differential age value continuously exceeds the preset threshold and the duration reaches the specified threshold, it is judged as abnormal.

[0204] The loss of a fixed solution is considered abnormal when a fixed solution is not obtained for multiple consecutive positioning epochs and the floating-point solution accuracy estimate exceeds the allowable range.

[0205] When base station communication is interrupted, it is immediately identified as an anomaly when the data link with the differential base station is detected to be disconnected.

[0206] A logical OR operation is performed on three types of conditions, and the degradation process is initially triggered when any one of the conditions is met. To avoid frequent mode switching caused by short-term signal fluctuations, a stabilization delay mechanism is introduced. Degradation is only formally confirmed and triggered when the abnormal state persists for more than the stabilization time threshold.

[0207] The degradation trigger flag, trigger reason code, and trigger time timestamp are encapsulated and output as the degradation trigger determination result.

[0208] Optionally, the original positioning observation data and the currently active positioning mode identifier are read to construct a vehicle motion state vector for extended Kalman filtering. The state vector is defined as a seven-dimensional vector, including the east and north coordinate components of the planar position, elevation coordinates, the east and north components of the velocity, the heading angle, and the estimated system bias between positioning modes.

[0209] The state vector is expressed as:

[0210] X=[p _x p _y p _h v _x v _y ,θ,b _mode ] T ;

[0211] Where, p _x With p _y p represents the planar coordinate components. _h For elevation, v _x With v _y Let b be the velocity component, θ be the heading angle, and b be the yaw rate. _mode This is the system bias estimate between the BeiDou standard mode and the RTK mode.

[0212] Based on this, the state covariance matrix is ​​initialized, and the diagonal elements are set according to the initial uncertainty of each state component. The initial variance of the position component is determined according to the nominal accuracy of the current positioning mode, and the initial variance of the deviation estimate is set to a large value to allow subsequent online estimation convergence.

[0213] The initialized state vector and covariance matrix are encapsulated and output as a filtered state vector.

[0214] Optionally, the filtered state vector from the previous epoch is read, and a constant rotation rate and velocity motion model is used to perform one-step state prediction. The position increment is calculated based on the current velocity component and the sampling period, the acceleration component is estimated based on the rate of change of velocity for velocity prediction, and the angular velocity is estimated based on the rate of change of heading for heading prediction. The mode bias estimate is assumed to remain constant over a short period of time.

[0215] The state prediction equation is expanded into a recursive form for each component, specifically:

[0216] p _x_k+1|k =p _x_k +v _x_k ×Δt;

[0217] p _y_k+1|k =p _y_k +v _y_k ×Δt;

[0218] p _h_k+1|k =p _h_k ;

[0219] v _x_k+1|k =v _x_k +a _x_k ×Δt;

[0220] v _y_k+1|k =v _y_k +a _y_k ×Δt;

[0221] θ _k+1|k =θ _k +ω _k ×Δt;

[0222] b _mode_k+1|k =b _mode_k ;

[0223] Where, the subscript k represents the current epoch, the subscript k+1|k represents the prediction of the k+1 epoch based on the information of the k epoch, Δt is the sampling period, and a _x With a _y Let ω be the acceleration component and ω be the angular velocity of the heading.

[0224] The state prediction covariance update employs the standard Kalman filter framework, namely:

[0225] P _k+1|k =F _k ×P _k|k ×F _k T +Q;

[0226] Among them, F _k Let P be the state transition Jacobian matrix, P be the state covariance matrix, and Q be the process noise covariance matrix. _k|k Let P represent the updated state covariance matrix at epoch k. _k+1|k The covariance matrix of the predicted state at epoch k+1.

[0227] Based on this, the predicted state vector and covariance matrix are encapsulated and output as the predicted state vector.

[0228] Optionally, the predicted state vector and the original positioning observation data are read, and the observation data from the two modes that may coexist are fused during the mode switching transition period. The observation vector is defined to include three coordinate components: eastward, northward, and elevation. A mode identification coefficient is introduced when constructing the observation matrix: when the observation originates from the RTK mode, the mode identification coefficient is zero, and the observed value directly corresponds to the position component in the state vector; when the observation originates from the normal mode, the mode identification coefficient is one, and the bias estimate in the state vector needs to be superimposed in the observation equation for compensation.

[0229] The Kalman gain calculation is expressed as follows:

[0230] K _k =P _k+1|k ×H T ×(H×P _k+1|k ×H T +R _k ) -1 ;

[0231] Where H is the observation matrix, R _k To observe the noise covariance matrix. K _k This represents the Kalman gain matrix for epoch k, used to weigh the predicted state against the observed data. −1 This represents the inverse operation of a matrix. The observation noise covariance is dynamically set according to the positioning mode of the current observation source: the observation noise variance takes a smaller value in RTK mode, corresponding to centimeter-level positioning accuracy; the observation noise variance takes a larger value in normal mode, corresponding to meter-level positioning accuracy.

[0232] Furthermore, the state update and covariance update are performed using the standard Kalman filter procedure, as follows:

[0233] X ＃ _k+1|k+1 =X ＃_k+1|k +K _k ×(z _k -H×X ＃ _k+1|k );

[0234] P _k+1|k+1 =(IK _k ×H)×P _k+1|k ;

[0235] Among them, z _k Let I be the observation vector for the current epoch, and let I be the identity matrix. ＃ For the _hat symbol, X ＃ _k+1|k+1 The optimal state vector X after updating at epoch k+1. ＃ _k+1|k For the predicted state vector at epoch k+1, P _k+1|k+1 The state covariance matrix after updating at epoch k+1.

[0236] Based on this, the updated state vector and covariance matrix are encapsulated and output as the updated filtered state vector.

[0237] Optionally, the filtered state vector and the original positioning observation data are read, and during the RTK module warm-up phase, the system deviation between the modes is estimated online using the time window when both modes simultaneously output effective positioning. When it is detected that both the BeiDou ordinary module and the RTK module output effective positioning coordinates at the same epoch, the instantaneous coordinate difference between the two is calculated as a deviation sample.

[0238] Furthermore, the bias estimate is updated using an exponentially weighted moving average method, with the specific formula as follows:

[0239] b ＃ _mode_k+1 =λ×b ＃ _mode_k +(1-λ)×δ _k ;

[0240] Where, δ _k Let be the instantaneous bias sample for the current epoch, and λ be the forgetting factor, with a suggested value of 0.9 to 0.95 to allow the bias estimate to adapt to environmental changes while maintaining a certain smoothness. ＃ _mode_k+1 Let b represent the estimated system bias after updating at epoch k+1. ＃ _mode_k This represents the current system bias estimate at epoch k.

[0241] Based on this, the updated bias estimate is injected into the corresponding component of the filtered state vector. In subsequent measurement updates, bias compensation is automatically applied to observations in the normal mode, eliminating coordinate step phenomena caused by system bias during mode switching. The output is a filtered state vector containing the corrected bias estimate.

[0242] Optionally, the filtered state vector sequence and the degradation trigger determination result are read, and interpolation compensation is performed for data missing epochs that may occur during mode switching. The presence of missing points with discontinuous timestamps in the filtered output sequence is detected. If missing points exist, the position coordinates of the missing epochs are estimated using cubic spline interpolation based on the effective state vectors adjacent to the missing points.

[0243] The interpolation calculation uses the missing point time as the independent variable and solves the coefficients of a cubic polynomial by using the position and velocity information of multiple valid points before and after the missing point. This ensures that the interpolation curve is continuous in position and first derivative at the boundary points, guaranteeing the geometric smoothness of the trajectory and the physical consistency of the velocity.

[0244] A continuity check is performed on the compensated complete sequence. The check includes: the timestamp sequence is strictly increasing and without omissions; the change in coordinates between adjacent epochs is within a reasonable range of vehicle speed; and the estimated position accuracy is within an acceptable range. If abnormal jump points are found during the check, they are marked and corrected using the mean of adjacent points.

[0245] After the verification is passed, the position, velocity, and heading components are extracted from the filtered state vector. A positioning mode identifier, accuracy level, and data source label are added to each epoch. The continuous positioning data sequence is organized and output in chronological order to ensure that the trajectory data obtained by the downstream assessment and evaluation module is complete and physically feasible.

[0246] This application employs a path integral prediction method based on road network topology constraints. This method abandons traditional free-space linear extrapolation, constraining vehicle motion to the known geometric topology of the site. Through integration along the road centerline, it eliminates centrifugal prediction bias caused by S-curves and continuous turnaround sections. Combined with a multi-region progressive scheduling strategy, it enables the prediction of vehicle arrival times in the assessment area, allowing the high-precision positioning module to complete signal acquisition and ambiguity resolution in advance.

[0247] Furthermore, a three-dimensional anisotropic deviation vector model and a spatial deviation database based on road arc length were constructed. Shortest path distance weights based on road topology were introduced, and a hierarchical storage structure for forward and reverse directions was established, resolving the issues of inertial navigation error model flipping and the failure of traditional Euclidean distance indexes in scenarios such as reversing into parking spaces. This allows the system to utilize spatial priors for compensation during positioning mode switching, ensuring the smoothness and consistency of the output trajectory in terms of position and dynamics.

[0248] The optional embodiments of the present invention have been described in detail above. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various equivalent transformations can be made to the technical solution of the present invention, and these equivalent transformations all fall within the protection scope of the present invention.

Claims

1. A high-precision BeiDou positioning method for an armored vehicle driver training and assessment system, characterized in that, include: Real-time acquisition of vehicle and positioning module operating status data; Based on motion state data, predict the vehicle's pose within a future preset time window, and generate an intervention strategy for the high-precision positioning module based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area. In response to the intervention strategy control of the working state of the high-precision positioning module, the multi-mode positioning observation data from the basic positioning module and the high-precision positioning module are fused using a state estimation filter. During the fusion process, the observation data are compensated for in real time using a state vector containing system deviation variables to obtain continuous positioning data. Before generating the intervention strategy for the high-precision positioning module, a road network topology model is also pre-built. The road network topology model is represented by a directed graph structure, which includes a set of nodes and a set of directed edges. Each directed edge in the set of directed edges corresponds to a road segment in the site and contains geometric parameters describing the shape of the road segment. The geometric parameters include the starting coordinates, azimuth, length, and curvature information of the road centerline. Based on motion state data, the vehicle's predicted pose within a future preset time window is predicted, including performing vehicle road matching, specifically: Calculate the projection distance from the vehicle's current planar coordinates to each candidate road segment in the road network topology model, and determine the corresponding projection point; Calculate the angle between the vehicle's current heading angle and the road tangential heading angle at the projection point; Road segments with a projected distance less than the distance matching threshold and an angle less than the heading consistency threshold are selected as matching road segments; Obtain the arc length parameter of the projection point on the matching road segment, and use it as the current projection arc length; The systematic deviation variable is a three-dimensional deviation vector, which contains three independent deviation components corresponding to the eastward, northward, and elevation directions, respectively. The state transition process of the state estimation filter uses an anisotropic deviation dynamic model, which is configured with different error accumulation correlation time constants and process noise intensity parameters for the three independent deviation components. The anisotropic deviation dynamic model includes a deviation accumulation coefficient function. When performing state prediction, the state estimation filter reads the vehicle's composite speed, rate of change of heading angle, and composite acceleration in real time. The process noise intensity parameter is dynamically adjusted using the deviation cumulative coefficient function, so that the process noise intensity parameter is positively correlated with the composite velocity, the rate of change of heading angle, and the composite acceleration.

2. The method according to claim 1, characterized in that, Based on motion state data, predict the vehicle's pose within a future preset time window, including: Extract the vehicle's current planar coordinates, composite velocity, and heading angle from the motion state data; The composite velocity is decomposed into an eastward velocity component and a northward velocity component along the heading angle; The displacement increment is obtained by multiplying the eastward velocity component and the northward velocity component by a preset time window. The displacement increment is then superimposed on the current plane coordinates to obtain the predicted vehicle pose.

3. The method according to claim 1 or 2, characterized in that, The pre-defined assessment area is defined by a sequence of polygon boundary vertices; Based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area, an intervention strategy for the high-precision positioning module is generated, including: Calculate the shortest geometric distance from the predicted vehicle pose to the polygon boundary of the preset assessment area; If the shortest geometric distance is less than the cutting distance threshold, an intervention strategy is generated to either start preheating or switch to high-precision positioning mode. If the shortest geometric distance is greater than the cut-out distance threshold, an intervention strategy is generated to maintain or switch to the basic positioning mode; Among them, the cutout distance threshold is greater than the cutin distance threshold.

4. The method according to claim 1, characterized in that, The systematic deviation variable is a scalar representing the isotropic positional error; The state estimation filter is used to fuse multi-mode positioning observation data from the base positioning module and the high-precision positioning module, including: When both the basic positioning module and the high-precision positioning module output observation data at the same epoch, calculate the difference in the Euclidean distance between their positioning coordinates. The Euclidean distance difference is smoothed using an exponentially weighted moving average algorithm, and the system bias variable is updated accordingly. During the observation update phase of the state estimation filter, the updated system bias variable is used to additively compensate the observation data from the base positioning module.

5. The method according to claim 1, characterized in that, After determining the matching road segment and the current projected arc length, the predicted vehicle pose within a future preset time window is calculated, specifically including: Calculate the predicted travel arc length based on the vehicle's current composite speed and the preset time window; Starting from the matched road segment and with the current projected arc length as the initial position, perform path integration along the directed edge direction of the road network topology model; When the integral length reaches the predicted driving arc length, the corresponding planar coordinates and tangential heading are calculated based on the geometric parameters of the road segment where the endpoint is located, and the predicted vehicle pose is obtained. If the endpoint of a road segment is reached during the integration process, the next connecting road segment is selected according to the preset path branch decision rules to continue integration until the remaining predicted driving arc length is zero.

6. The method according to claim 1, characterized in that, The preset assessment area is pre-configured with positioning accuracy requirements. Based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area, an intervention strategy for the high-precision positioning module is generated, including: Based on the predicted vehicle pose, identify at least one reachable assessment area that the vehicle will pass through within a preset time window. Calculate the estimated arrival time of the vehicle to the reachable assessment area; The urgency of preheating is calculated based on the estimated arrival time, and the overall preheating priority is calculated in combination with the positioning accuracy requirements of the reachable assessment area. All reachable assessment areas are sorted according to the overall preheating priority to construct a regional preheating priority queue, which provides a basis for generating intervention strategies for high-precision positioning modules.

7. The method according to claim 6, characterized in that, The intervention strategy for generating high-precision positioning modules specifically includes formulating a progressive preheating scheduling plan based on a regional preheating priority queue. The process of developing a gradual preheating schedule includes: The preheating and startup process of the high-precision positioning module is broken down into the signal acquisition stage, the differential data synchronization stage, and the ambiguity resolution stage. For the region with the highest priority in the regional preheating priority queue, the ideal start time of each stage is calculated in reverse based on its expected arrival time and the typical time consumption of each stage. A sequence of control commands containing the start times of each stage is generated as an intervention strategy to trigger the high-precision positioning module to perform preheating operations in stages.

8. A high-precision Beidou positioning device for an armored vehicle driver training and assessment system, used to implement the method as described in any one of claims 1 to 7, characterized in that, include: The data acquisition module is configured to acquire real-time data on the vehicle's current motion status and the operating status of the positioning module. The prediction and decision-making module is configured to predict the vehicle's pose within a preset time window based on motion state data, and generate an intervention strategy for the high-precision positioning module based on the spatiotemporal matching relationship between the predicted vehicle pose and the preset assessment area. The fusion control module is configured to respond to the intervention strategy and control the working state of the high-precision positioning module. It uses a state estimation filter to fuse multi-mode positioning observation data from the basic positioning module and the high-precision positioning module. During the fusion process, it uses a state vector containing system deviation variables to perform real-time deviation compensation on the observation data to obtain continuous positioning data.

Images