Commercial vehicle positioning enhancement and emergency rescue method integrating Beidou system and low-orbit satellite communication

Abstract

The invention provides a commercial vehicle positioning enhancement and emergency rescue method integrating a Beidou system and low-orbit satellite communication, and relates to the technical field of vehicle positioning. According to the method, a vehicle-mounted terminal, an emergency hall platform and a satellite work cooperatively, the vehicle-mounted terminal comprises an RNSS positioning module, a low-orbit satellite communication module and other modules, and a UDP / IP lightweight transmission protocol is adopted to construct a communication protocol. Firstly, coarse positioning is obtained by a Beidou system, and then the positioning precision is improved through low-orbit satellite precision correction and Kalman filtering fusion multi-source data; after an E-CALL system is triggered, key information such as positioning coordinates and MSD data is packaged and transmitted to an emergency hall platform, and the platform judges the type of an accident and dispatches rescue after analyzing the key information. The method solves the problems of inaccurate positioning and information communication loss of the commercial vehicle in the extreme off-line area, the positioning error is less than or equal to 5m, the efficient and accurate rescue is ensured, the cargo loss risk is reduced, and the safe operation of the commercial vehicle in the extreme area is assisted.

Description

TECHNICAL FIELD

[0001] The application relates to the technical field of vehicle positioning, in particular to a commercial vehicle positioning enhancement and emergency rescue method fusing a Beidou system and low-orbit satellite communication. BACKGROUND

[0002] As core equipment in fields such as logistics transportation and engineering operation, commercial vehicles often shuttle in extreme off-network areas (uninhabited areas) such as plateaus, deserts and deep valleys. These areas generally lack ground communication base station coverage, and the terrain is complex and the shielding is serious, so that the signals of the traditional satellite positioning system (such as single GPS) relied on by the commercial vehicle are easily disturbed and attenuated, the positioning accuracy is greatly reduced, and the actual position of the vehicle is often difficult to lock accurately with a deviation of several meters or even tens of meters.

[0003] More importantly, in the extreme off-network environment, the public network communication is completely interrupted, and when the commercial vehicle encounters a sudden condition such as a tire burst, a mechanical failure or a traffic accident, the traditional positioning information transmission mode relying on the public network is completely invalid. The driver cannot send accurate position coordinates to the outside world through a mobile phone or a vehicle-mounted communication system, and it is also difficult to transmit key rescue information such as the type of vehicle failure and the condition of the injured personnel, so that the rescue team is difficult to quickly lock the search and rescue range.

[0004] This double dilemma of inaccurate positioning and information disconnection not only greatly prolongs the rescue response time, but also increases the difficulty of rescue due to the expansion of the search area and the complexity of the terrain, and even endangers the life safety of the driver and passenger. At the same time, for logistics transportation enterprises, the disconnection of the vehicle also causes the interruption of the tracking of goods, increases the risk of loss and damage of goods, and causes serious economic losses. This problem has become a core bottleneck restricting the safe operation and efficient operation of commercial vehicles in extreme areas, and a targeted technical solution is urgently needed to solve it. SUMMARY

[0005] The application aims to solve the problems in the prior art and provides a commercial vehicle positioning enhancement and emergency rescue method fusing a Beidou system and low-orbit satellite communication. The method solves the problems of inaccurate positioning and information disconnection of commercial vehicles in extreme off-network areas, with a positioning error of less than or equal to 5m, ensuring efficient and accurate rescue, reducing the risk of loss of goods, and assisting the safe operation of commercial vehicles in extreme areas.

[0006] To achieve the above-mentioned purpose, the application adopts the following technical solutions:

[0007] A commercial vehicle positioning enhancement and emergency rescue method fusing a Beidou system and low-orbit satellite communication, characterized by comprising a vehicle terminal, an emergency hall platform and a satellite; the vehicle terminal comprises an RNSS positioning module, a low-orbit satellite communication module, an IMU inertial navigation module and an SBOX unit; a UDP / IP lightweight transmission protocol is used to formulate a communication protocol for the vehicle terminal-satellite-emergency hall platform;

[0008] Includes the following steps:

[0009] S1: The BeiDou system obtains coarse positioning;

[0010] Start the RNSS localization module to obtain coarse localization;

[0011] S2: Precise correction for low-orbit satellites;

[0012] S21: Activate the low-Earth orbit satellite communication module to obtain ephemeris, real-time clock difference, and carrier phase observations;

[0013] S22: Accuracy correction for multi-source data fusion;

[0014] First, correct the carrier phase observation value. Then, using the BeiDou coarse positioning as the initial value, the positioning accuracy is corrected by fusing the positioning data corrected by the low-orbit satellite and the data from the inertial navigation module (IMU) through Kalman filtering.

[0015] S3: If the E-CALL system is triggered, the data frame is encapsulated and transmitted to the emergency management platform;

[0016] S4: Emergency Management Platform for Rescue Dispatch;

[0017] After receiving the data frames, the emergency response platform parses out the location coordinates, MSD data, and vehicle ID; based on the MSD data, it determines the type of accident and dispatches rescue teams to the scene.

[0018] Further, step S1 includes: S11: Activate the RNSS positioning module based on the public network signal strength; the vehicle terminal monitors the public network signal strength in real time, and if the signal received power RSRP≤-120dBm, it is determined to be offline; activate the RNSS positioning module to search for GEO / MEO satellites and acquire at least 4 satellites;

[0019] S12: Calculate and obtain coarse positioning coordinates;

[0020] The RNSS positioning module receives satellite navigation messages and extracts satellite ephemeris and clock bias data using the least squares method. Calculate the coarse positioning coordinates (B0, L0, H0) and cache them in the SBOX cell; where G is the geometric matrix and ρ is the pseudorange observation value; the RTKLIB library is loaded by the RNSS positioning module, and G and ρ are directly calculated by the pntpos function; in the coarse positioning coordinates (B0, L0, H0), B0 is the latitude, L0 is the longitude, and H0 is the altitude.

[0021] Furthermore, step S2 includes:

[0022] S221: Data extraction and preprocessing;

[0023] The coarse positioning coordinates (B0, L0, H0) are read from the cache of the SBOX unit, and carrier phase observations are obtained from the low-Earth orbit satellite communication module.

[0024] Read the current acceleration (a) from the IMU inertial navigation module. X ,a Y ,a Z ), angular velocity (ω) X ,ω Y ,ω Z ) and attitude angle; obtain vehicle speed signal, which includes vehicle speed and heading; all data are uniformly aligned to UTC timestamp;

[0025] S222: Error equation, correcting carrier phase observations;

[0026] Constructing error equations to obtain corrected carrier phase observations

[0027] S223: Based on the corrected carrier phase observations Obtain the satellite-corrected positioning coordinates;

[0028] S224: Kalman filter multi-source data fusion.

[0029] Furthermore, step S223 specifically involves first converting the corrected carrier phase observations into geometric distances: using the corrected carrier phase observations... Through formula Convert the phase cycle number into a high-precision geometric distance ρ from the satellite to the terminal;

[0030] Next, the precise position of the satellite, the detailed ephemeris broadcast by the low-Earth orbit satellite, and the SGP4 orbital model are used to calculate the geocentric rectangular coordinates (X, Y, F, G) of the satellite at the time of observation. s ,Y s Z s );

[0031] Then, the terminal coordinates are calculated jointly by multiple satellites: the geometric distance ρ between the three low-Earth orbit satellites and the geocentric rectangular coordinates (X) of the satellites are called. s ,Y s Z s Using the geocentric coordinates obtained from BeiDou coarse positioning as initial values, a system of distance equations is constructed. These equations are then solved iteratively using the least squares method to obtain the precise geocentric coordinates of the vehicle-mounted terminal. Finally, GIS library functions are called to convert the geocentric coordinates of the vehicle-mounted terminal into geodetic coordinates. sat ,L sat H sat () as the corrected positioning coordinates for low-orbit satellites.

[0032] Furthermore, S224 includes:

[0033] Step 1: Initialize filter parameters upon first execution;

[0034] Set the initial value of the state vector Where B0, L0, and H0 are BeiDou coarse positioning coordinates, v B v L v H It is a vehicle speed signal, converted into speed in latitude / longitude / altitude direction, via v. B =v*cos heading; v L =v*sin(heading); v H =0; a B a L a H It is set to 0 upon initial startup and subsequently updated using acceleration data from the IMU inertial navigation module.

[0035] Set the initial state covariance matrix: P0 = diag([1e -4 ,1e -4 ,1e -2 ,1e -3 ,1e -3 ,1e -3 ,1e -2 ,1e -2 ,1e -2 ]);

[0036] Preset process noise variance matrix Q and observation noise variance matrix R;

[0037] Process noise variance matrix:

[0038] Q = diag([1e -8 ,1e -8 ,1e -4 ,1e -6 ,1e -6 ,1e -6 ,1e -4 ,1e -4 ,1e -4 ]);

[0039] Observation noise variance matrix:

[0040] R = diag([1e -7 ,1e -7 ,1e -3 ,1e -5 ,1e -5 ,1e -5 ,1e -3 ,1e -3 ,1e -3]);

[0041] Step 2: Construct the state transition matrix;

[0042]

[0043] Where I3 is a 3×3 identity matrix and O3 is a 3×3 zero matrix; T is the filtering period, which is consistent with the sampling frequency of the IMU inertial navigation module, and is 0.1s;

[0044] Step 3: Predict the state and covariance;

[0045] State prediction; in, Predict the state at the current moment. The state estimate updated at the previous time step; F k This is the state transition matrix;

[0046] Covariance prediction;

[0047] Among them, F k P is the state transition matrix, Q is the process noise variance matrix; k-1 The covariance updated in the previous time step; The current predicted covariance;

[0048] Step 4: Construct the observation vector Z k With observation matrix H k ;

[0049] Among them, B sat ,L sat H sat The corrected positioning coordinates for low-Earth orbit satellites; a X ,a Y ,a z To obtain the acceleration, ω X ,ω Y ,ω Z The obtained angular velocity;

[0050] Where I3 is a 3×3 identity matrix, 03 is a 3×3 zero matrix, and R a R is the acceleration coordinate system transformation matrix; ω This is the transformation matrix from angular velocity to velocity change;

[0051] Step 5: Correct the prediction results using observation data;

[0052] Calculate the filter gain K k ;

[0053] in For the current predicted covariance, H k R is the observation matrix, and R is the observation noise variance matrix;

[0054] Update state estimates;

[0055] Among them, Z k For observation vectors; To predict the state at the current moment, K k H is the filter gain. k The observation matrix;

[0056] Update the covariance matrix;

[0057] Where I is a 9×9 identity matrix. K represents the current predicted covariance. k H is the filter gain. k The observation matrix is ​​updated, and the covariance reflects the uncertainty of the current state, which is used for prediction at the next time step.

[0058] Step 6: Output the position coordinates for the correction accuracy;

[0059] After 5 consecutive iterations, if the state covariance matrix P k The values ​​of the first three diagonal elements are ≤1e -6 If convergence is achieved, the state estimate is obtained. Extracting state estimation The first 3 elements, output position coordinates (B k ,L k H k Otherwise, continue iterating until convergence, and output the position coordinates. If convergence is not achieved after 20 iterations, output the final position coordinates and mark them as the reference position.

[0060] Furthermore, step 4 also includes:

[0061] Calculate R a : Obtain the attitude angles and convert them into rotation matrices;

[0062] Obtain the heading angle ψ, pitch angle θ, and roll angle.

[0063] According to respectively Calculate the roll angle rotation matrix R1;

[0064] Calculate the pitch angle rotation matrix R2;

[0065] Calculate the heading angle rotation matrix R3;

[0066] According to Ra =R3*R2*R1 to obtain the acceleration coordinate system transformation matrix;

[0067] Based on the acceleration coordinate system transformation matrix, the deceleration is converted into a vector. according to Get a B a L a H Update the filter parameters;

[0068] Calculate R ω According to the formula Calculate and obtain R ω .

[0069] Furthermore, the protocol's data frame format is defined, specifically as frame header, vehicle ID, location data, MSD data, checksum, and frame trailer; the total length is ≤64 bytes.

[0070] Furthermore, step S3 specifically involves the following steps: When a serious collision occurs, causing the airbags to deploy or the in-vehicle sensors to detect a major accident and send a trigger command to the vehicle terminal to automatically trigger the E-CALL system, or when the driver presses the SOS physical button to manually trigger and activate the E-CALL system, the SBOX extracts the latest corrected position coordinates, and assembles the MSD data, vehicle ID, etc., cached for the past 30 seconds according to the preset data frame format. The check digit is calculated using the CRC32 algorithm, appended to the end of the data frame, and encapsulated into a data frame. This data frame is then transmitted to the emergency management platform via the vehicle terminal-satellite-emergency management platform communication protocol.

[0071] Furthermore, MSD data includes tire pressure and engine fault codes.

[0072] Furthermore, the type of accident can be determined by the following criteria: airbag activation indicates a collision; a sudden drop in tire pressure indicates a tire blowout; engine fault codes indicate a mechanical failure; and manual activation indicates an emergency.

[0073] Compared with the prior art, the beneficial effects of the present invention are as follows: (1) The present invention obtains coarse positioning through the Beidou system, and combines low-orbit satellite carrier phase correction and Kalman filter multi-source data fusion to control the positioning error of commercial vehicles in uninhabited areas to ≤5m, avoiding the problem of low positioning accuracy and accurately locking the actual position of the vehicle; (2) The present invention relies on the dual-mode architecture of Beidou system and low-orbit satellite communication, and is equipped with UDP / IP lightweight transmission protocol and standardized data frame format. In extreme areas without public network coverage, it ensures that after E-CALL is triggered, key rescue information such as location coordinates, MSD data, and vehicle ID are transmitted without interruption, thus solving the pain point of the traditional positioning information transmission method that relies on the public network completely fails; (3) The present invention is connected to the emergency management platform, and can dispatch rescue according to the fault type. The high-precision positioning coordinates reduce the search and rescue range and rescue difficulty, and ensure the safety of drivers and passengers. (4) In the case of offline, the positioning and data transmission are continuously and stably corrected by low-orbit satellites to realize the full tracking of commercial vehicles in extreme areas, avoid the risk of loss or damage of goods caused by vehicle disconnection, reduce the economic losses of logistics and transportation companies, and help commercial vehicles operate safely and efficiently in extreme areas. Attached Figure Description

[0074] Figure 1 This is a flowchart illustrating the steps of the commercial vehicle positioning enhancement and emergency rescue method integrating the BeiDou system and low-orbit satellite communication of the present invention. Detailed Implementation

[0075] To provide a further understanding of the purpose, structure, features, and functions of the present invention, detailed descriptions are provided below with reference to specific embodiments.

[0076] A method for enhancing commercial vehicle positioning and emergency rescue by integrating the BeiDou Navigation Satellite System and low-Earth orbit (LEO) satellite communication includes an onboard terminal, an emergency response platform, and satellites such as LEO, GEO, and MEO satellites. The onboard terminal includes an RNSS positioning module, a LEO satellite communication module, an IMU inertial navigation module, and an SBOX unit. The RNSS positioning module is a hardware device used to receive satellite navigation signals and achieve autonomous positioning. Its core function is to receive navigation messages broadcast by multiple satellites and, combined with the onboard terminal's own computing power, calculate position, speed, and time information.

[0077] The communication protocol between the vehicle terminal, satellite, and emergency management platform is formulated using the UDP / IP lightweight transmission protocol. The data frame format of the protocol is defined as follows: frame header (2 bytes), vehicle ID (8 bytes), location data (16 bytes), MSD data (32 bytes), check bit (2 bytes), and frame trailer (2 bytes), with a total length of ≤64 bytes.

[0078] Includes the following steps:

[0079] S1: The BeiDou system obtains coarse positioning;

[0080] S11: Activate the RNSS positioning module based on the public network signal strength;

[0081] The vehicle-mounted terminal monitors the public network signal strength in real time. If the signal received power RSRP ≤ -120dBm, it is determined to be offline. The RNSS positioning module is activated to search for Beidou-3 GEO / MEO satellites (at least 4 satellites are captured).

[0082] S12: Calculate and obtain coarse positioning coordinates;

[0083] The RNSS positioning module receives satellite navigation messages and extracts satellite ephemeris and clock bias data using the least squares method. Calculate the coarse positioning coordinates (B0, L0, H0), where G is the geometric matrix and ρ is the pseudorange observation value; G and ρ are obtained directly by loading the RTKLIB library from the RNSS positioning module and solving it directly through the pntpos function; in the coarse positioning coordinates (B0, L0, H0), B0 is the latitude, L0 is the longitude, and H0 is the altitude.

[0084] S13: Mark the coarse positioning data with a timestamp, cache it in the SBOX unit, and trigger the low-orbit satellite communication module to start.

[0085] S2: Precise correction for low-orbit satellites;

[0086] S21: Activate the low-Earth orbit satellite communication module to obtain ephemeris, real-time clock difference, and carrier phase observations;

[0087] After the low-Earth orbit (LEO) satellite communication module is activated, it receives L-band signals broadcast by LEO satellites, acquires at least three satellites, extracts the ephemeris and real-time clock difference transmitted by the LEO satellites, and outputs carrier phase observations through the carrier tracking loop. A carrier tracking loop is used to accurately extract carrier phase information from a received carrier signal.

[0088] S22: Accuracy correction for multi-source data fusion;

[0089] S221: Data extraction and preprocessing;

[0090] The coarse positioning coordinates (B0, L0, H0) are read from the cache of the SBOX unit, and carrier phase observations are obtained from the low-Earth orbit satellite communication module. Read the current acceleration (a) from the IMU inertial navigation module. X ,a Y ,a Z ), angular velocity (ω) X ,ω Y ,ω ZThe attitude angles include the heading angle ψ (the angle between the vehicle's direction of travel and true north), the pitch angle θ (the angle at which the vehicle rolls forward or backward), and the roll angle. (Vehicle tilt angle); Obtain vehicle speed signal, which includes vehicle speed and heading; All data are aligned to UTC timestamps.

[0091] S222: Error equation, correcting carrier phase observations;

[0092] Constructing error equations to obtain corrected carrier phase observations in:

[0093] θ is the carrier wavelength of the low-Earth orbit satellite, calculated from the carrier frequency f: λ = c / f; c is the speed of light, taken as 3 × 10⁻⁶. 8 m / s; f is the carrier frequency of the low-Earth orbit satellite, obtained from the carrier tracking loop;

[0094] d represents the geometric distance from the satellite to the vehicle-mounted terminal; firstly, the satellite's geocentric coordinates (X) are calculated using the ephemeris and the SGP4 orbital model. s ,Y s Z s The SGP4 orbital model is a mathematical model used to calculate the orbital state vector of Earth satellites. Built on open-source code, it uses a simplified perturbation model to efficiently and accurately predict the satellite's position and velocity parameters at any given time. Next, GIS library functions are called to convert the BeiDou coarse positioning coordinates (B0, L0, H0) into geocentric coordinates (X). u ,Y u Z u Finally, based on Obtain the geometric distance d from the satellite to the vehicle terminal;

[0095] Δr1 is the ionospheric delay correction, calculated using the Klobuchar model: during nighttime (22:00-06:00 local time), Δr1 = A × cos(B(t-t0)); during daytime (06:00-22:00 local time), Δr1 = A × [1-0.5×(B(t-t0))]. 2 ]; where: A and B are Klobuchar model coefficients; obtained from satellite broadcast messages, t is the observation time, and t0 is the peak time of the ionosphere (default 14:00).

[0096] Δr2 is the tropospheric delay correction, calculated using the Saastamoinen model, according to the formula: Where P0 is the sea level air pressure, obtained from the barometer built into the vehicle terminal; B0 is the latitude of the coarse positioning coordinates; H0 is the altitude of the coarse positioning coordinates; and θ is the satellite elevation angle, obtained based on the geometric relationship between the satellite position and the coarse positioning coordinates.

[0097] N represents the number of integer cycles of the carrier phase, which is fixed using the LAMBDA algorithm. Mature open-source technologies and standardized protocols, such as the SGP4 orbit model, the LAMBDA algorithm, and the RTKLIB library, are employed to reduce R&D costs.

[0098] ε represents the observation noise, ranging from ±0.005 cycles. It is pre-set by the vehicle-mounted terminal at a base station with known true values, and collects several sets of observations. The standard deviation of the statistical deviation between the observed and true values ​​is calculated to obtain the noise. The observation noise is then stored for later retrieval.

[0099] S223: Based on the corrected carrier phase observations Obtain the satellite-corrected positioning coordinates;

[0100] First, convert the corrected carrier phase observations into geometric distance: using the corrected carrier phase observations Through formula The phase cycle number is converted into a high-precision geometric distance ρ from the satellite to the terminal, where λ is the carrier wavelength of the low-Earth orbit satellite. The calculation method is the same as above.

[0101] Next, the precise position of the satellite is obtained, along with the detailed ephemeris broadcast by the low-Earth orbit satellite. Combined with the SGP4 orbital model, the geocentric coordinates (X, Y, φ) of the satellite at the observation time are calculated. s ,Y s Z s );

[0102] Then, the multi-satellite joint calculation of the terminal coordinates is performed: the geometric distance ρ between the three low-Earth orbit satellites and the geocentric coordinates (X) of the satellites are retrieved. s ,Y s Z s ); Geocentric coordinates (X) based on BeiDou coarse positioning u ,Y u Z u Using the initial values ​​as a reference, a system of distance equations is constructed. This system is then solved iteratively using the least squares method to obtain the precise geocentric coordinates of the vehicle-mounted terminal. Finally, GIS library functions are called to convert the geocentric coordinates of the vehicle-mounted terminal to geodetic coordinates (B). sat ,L sat H sat () as the corrected positioning coordinates for low-orbit satellites.

[0103] Let the observed geometric distances from the three satellites to the terminal be ρ1, ρ2, and ρ3, respectively; and the geocentric coordinates of the three low-orbit satellites be (X... s1 ,Ys1 Z s1 ), (X s2 ,Y s2 Z s2 ) and (X s3 ,Y s3 Z s3 );

[0104] Calculate the initial approximate distance according to the formula. Calculate the distance ρ between the three low-orbit satellites and the geocentric coordinates obtained from BeiDou coarse positioning. 01 ρ 02 and ρ 03 Where i is the i-th satellite, i = 1, 2, 3; X u ,Y u Z u Obtained from coarsely located geocentric coordinates;

[0105] Calculate the distance residual: Δρ i =ρ i -ρ 0i , i=1,2,3; calculate Δρ1, Δρ2 and Δρ3 respectively;

[0106] Construct the Jacobi matrix Each row corresponds to one satellite. The first three columns are the partial derivatives of the range with respect to X, Y, and Z, respectively; the fourth column is the coefficient of the clock error correction (fixed to 1).

[0107] Where ρ 01 ρ 02 and ρ 03 The distance between the low-orbit satellite and the geocentric coordinates obtained from BeiDou coarse positioning, (X) u ,Y u Z u (X) are the coarse geocentric coordinates; s1 ,Y s1 Z s1 ), (X s2 ,Y s2 Z s2 ) and (X s3 ,Y s3 Z s3 These are the geocentric coordinates of the three low-orbit satellites;

[0108] Solve for the coordinate correction ΔX using the least squares method;

[0109] Correction amount Where Δx is the correction amount in the X-axis direction of the vehicle terminal's geocentric coordinates; Δy is the correction amount in the Y-axis direction of the terminal's geocentric coordinates; Δz is the correction amount in the Z-axis direction of the terminal's geocentric coordinates; cΔt is the clock difference correction amount between the terminal and the satellite; where c is the speed of light (3×10⁻⁶). 8m / s), Δt is the clock difference (seconds); the solution is obtained using the least squares method:

[0110] ΔX=(H T H) -1 H T Δρ; where H T Let H be the transpose of the Jacobian matrix H; (H T H) -1 For H T The inverse matrix of H, Let X be the residual vector. Solve to obtain ΔX;

[0111] Iteratively update the terminal coordinates;

[0112] Update coordinates X = X based on the correction amount. u +Δx;Y=Y u +Δy;Z=Z u +Δz;

[0113] Using the updated coordinates as initial values, repeat the above steps to recalculate the initial approximate distance ρ. 0i Distance residual Δρ i Construct the Jacobian matrix H and coordinate correction ΔX, and iteratively update the coordinates until the absolute values ​​of Δx, Δy and Δz are all ≤0.1. The iteration ends at this point, and (X,Y,Z) are the accurate geocentric coordinates.

[0114] S224: Kalman filter for multi-source data fusion;

[0115] Using the coarse BeiDou positioning coordinates as the initial value, the positioning data corrected by low-orbit satellites and IMU inertial data are fused together, and dynamic positioning updates are achieved through Kalman filtering.

[0116] Step 1: Initialize filter parameters upon first execution;

[0117] This includes setting the initial value of the state vector. Where B0, L0, and H0 are BeiDou coarse positioning coordinates, v B v L v H It is a vehicle speed signal, converted into speed in latitude / longitude / altitude direction, via v. B =v*cos heading; v L =v*sin(heading); v H =0; a B a L a H It is set to 0 upon initial startup and subsequently updated using acceleration data from the IMU inertial navigation module.

[0118] Set the initial state covariance matrix:

[0119] P0 = diag([1e -4 ,1e -4 ,1e -2 ,1e -3 ,1e -3 ,1e -3 ,1e -2 ,1e -2 ,1e -2 ]);

[0120] Preset process noise variance matrix Q and observation noise variance matrix R;

[0121] Process noise variance matrix:

[0122] Q = diag([1e -8 ,1e -8 ,1e -4 ,1e -6 ,1e -6 ,1e -6 ,1e -4 ,1e -4 ,1e -4 ]);

[0123] Observation noise variance matrix:

[0124] R = diag([1e -7 ,1e -7 ,1e -3 ,1e -5 ,1e -5 ,1e -5 ,1e -3 ,1e -3 ,1e -3 ]);

[0125] Step 2: Construct the state transition matrix;

[0126]

[0127] Where I3 is a 3×3 identity matrix and O3 is a 3×3 zero matrix; T is the filtering period, which is consistent with the sampling frequency of the IMU inertial navigation module, and is 0.1s;

[0128] Step 3: Predict the state and covariance;

[0129] State prediction;

[0130] in, Predict the state at the current moment. The state estimate updated at the previous time step; F k This is the state transition matrix;

[0131] Covariance prediction;

[0132] Among them, F k P is the state transition matrix, Q is the process noise variance matrix; k-1 The covariance updated in the previous time step; The current predicted covariance;

[0133] Step 4: Construct the observation vector Z k With observation matrix H k ;

[0134] Among them, B sat ,L sat H sat The corrected positioning coordinates for low-Earth orbit satellites; a X ,a Y ,a z To obtain the acceleration, ω X ,ω Y ,ω Z The obtained angular velocity;

[0135] Where I3 is a 3×3 identity matrix, 03 is a 3×3 zero matrix, and R a R is the acceleration coordinate system transformation matrix; ω This is the transformation matrix from angular velocity to velocity change;

[0136] Calculate R a : Obtain the attitude angles and convert them into rotation matrices;

[0137] Obtain the heading angle ψ, pitch angle θ, and roll angle.

[0138] According to respectively Calculate the roll angle rotation matrix R1;

[0139] Calculate the pitch angle rotation matrix R2;

[0140] Calculate the heading angle rotation matrix R3;

[0141] According to R a =R3*R2*R1 to obtain the acceleration coordinate system transformation matrix;

[0142] Based on the acceleration coordinate system transformation matrix, the deceleration is converted into a vector. according to Get a B a L a H Update the filter parameters.

[0143] Calculate R ω According to the formula Calculate and obtain R ω .

[0144] Step 5: Correct the prediction results using observation data;

[0145] Calculate the filter gain K k ;

[0146] in For the current predicted covariance, H k R is the observation matrix, and R is the observation noise variance matrix;

[0147] Update state estimates;

[0148] Among them, Z k For observation vectors; To predict the state at the current moment, K k H is the filter gain. k The observation matrix;

[0149] Update the covariance matrix;

[0150] Where I is a 9×9 identity matrix. K represents the current predicted covariance. k H is the filter gain. k The observation matrix is ​​updated, and the covariance reflects the uncertainty of the current state, which is used for prediction at the next time step.

[0151] Step 6: Output the position coordinates for the correction accuracy;

[0152] After 5 consecutive iterations, if the state covariance matrix P k The values ​​of the first three diagonal elements are ≤1e -6 If convergence is achieved, the state estimate is obtained. Extracting state estimation The first 3 elements, output position coordinates (B k ,L k H k Otherwise, continue iterating until convergence, and output the position coordinates. If convergence is not achieved after 20 iterations, output the final position coordinates and mark them as the reference position.

[0153] S3: If the E-CALL system is triggered, the data frame is encapsulated and transmitted to the emergency management platform;

[0154] When a serious collision occurs, causing the airbags to deploy or the in-vehicle sensors to detect a major accident and send a trigger command to the vehicle terminal to automatically activate the E-CALL system, or when the driver presses the SOS physical button to manually activate the E-CALL system, the SBOX extracts the latest corrected position coordinates, the MSD data cached for the past 30 seconds, the vehicle ID, etc., and assembles them according to the preset data frame format. The check bit is calculated using the CRC32 algorithm, appended to the end of the data frame, and encapsulated into a data frame. The data frame is then transmitted to the emergency management platform through the vehicle terminal-satellite-emergency management platform communication protocol.

[0155] S4: Emergency Management Platform for Rescue Dispatch;

[0156] After receiving the data frames, the emergency response platform parses out the location coordinates, MSD data, and vehicle ID. Based on the MSD data, it determines the accident type: airbag deployment indicates a collision; a sudden drop in tire pressure indicates a tire blowout; engine fault codes indicate mechanical failure; and manual triggering indicates an emergency incident. The platform then dispatches rescue teams to the scene.

[0157] The present invention has been described in the above-described embodiments; however, these embodiments are merely examples for implementing the present invention. It must be noted that the disclosed embodiments do not limit the scope of the present invention. Conversely, any modifications and refinements made without departing from the spirit and scope of the present invention are within the scope of patent protection of the present invention.

Claims

1. A method for enhancing positioning and emergency rescue of commercial vehicles by integrating the BeiDou system and low-orbit satellite communication, characterized in that: It includes a vehicle-mounted terminal, an emergency response platform, and a satellite; the vehicle-mounted terminal includes an RNSS positioning module, a low-orbit satellite communication module, an IMU inertial navigation module, and an SBOX unit; the communication protocol between the vehicle-mounted terminal, the satellite, and the emergency response platform is formulated using the UDP / IP lightweight transmission protocol; Includes the following steps: S1: The BeiDou system obtains coarse positioning; Start the RNSS localization module to obtain coarse localization; S2: Precise correction for low-orbit satellites; S21: Activate the low-Earth orbit satellite communication module to obtain ephemeris, real-time clock difference, and carrier phase observations; S22: Accuracy correction for multi-source data fusion; First, correct the carrier phase observation value. Then, using the BeiDou coarse positioning as the initial value, the positioning accuracy is corrected by fusing the positioning data corrected by the low-orbit satellite and the data from the inertial navigation module (IMU) through Kalman filtering. S3: If the E-CALL system is triggered, the data frame is encapsulated and transmitted to the emergency management platform; S4: Emergency Management Platform for Rescue Dispatch; After receiving the data frames, the emergency response platform parses out the location coordinates, MSD data, and vehicle ID. Based on the MSD data, the type of accident is determined, and rescue teams are dispatched to the scene.

2. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 1, characterized in that: Step S1 includes: S11: Based on the public network signal strength, start the RNSS positioning module; the vehicle terminal monitors the public network signal strength in real time, and if the signal received power RSRP≤-120dBm, it is determined to be offline; start the RNSS positioning module to search for GEO / MEO satellites and acquire at least 4 satellites; S12: Calculate and obtain coarse positioning coordinates; The RNSS positioning module receives satellite navigation messages and extracts satellite ephemeris and clock bias data using the least squares method. Calculate the coarse positioning coordinates (B0, L0, H0) and cache them in the SBOX cell; where G is the geometric matrix and ρ is the pseudorange observation value; the RTKLIB library is loaded by the RNSS positioning module, and G and ρ are directly calculated by the pntpos function; In the coarse positioning coordinates (B0, L0, H0), B0 represents latitude, L0 represents longitude, and H0 represents altitude.

3. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 2, characterized in that: Step S2 includes: S221: Data extraction and preprocessing; The coarse positioning coordinates (B0, L0, H0) are read from the cache of the SBOX unit, and carrier phase observations are obtained from the low-Earth orbit satellite communication module. Read the current acceleration (a) from the IMU inertial navigation module. X ,a Y ,a Z ), angular velocity (ω) X ,ω Y ,ω Z ) and attitude angle; obtain vehicle speed signal, which includes vehicle speed and heading; all data are uniformly aligned to UTC timestamp; S222: Error equation, correcting carrier phase observations; Constructing error equations to obtain corrected carrier phase observations S223: Based on the corrected carrier phase observations Obtain the satellite-corrected positioning coordinates; S224: Kalman filter multi-source data fusion.

4. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 3, characterized in that: Step S223 specifically involves first converting the corrected carrier phase observations into geometric distances: using the corrected carrier phase observations... Through formula Convert the phase cycle number into a high-precision geometric distance ρ from the satellite to the terminal; Next, the precise position of the satellite, the detailed ephemeris broadcast by the low-Earth orbit satellite, and the SGP4 orbital model are used to calculate the geocentric rectangular coordinates (X, Y, F, G) of the satellite at the time of observation. s ,Y s Z s Then, the terminal coordinates are calculated jointly by multiple satellites: the geometric distance ρ between the three low-orbit satellites and the geocentric rectangular coordinates (X) of the satellites are called. s ,Y s Z s Using the geocentric coordinates obtained from BeiDou coarse positioning as initial values, a system of distance equations is constructed. These equations are then solved iteratively using the least squares method to obtain the precise geocentric coordinates of the vehicle-mounted terminal. Finally, GIS library functions are called to convert the geocentric coordinates of the vehicle-mounted terminal into geodetic coordinates. sat ,L sat H sat () as the corrected positioning coordinates for low-orbit satellites.

5. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 3, characterized in that: S224 includes: Step 1: Initialize filter parameters upon first execution; Set the initial value of the state vector Where B0, L0, and H0 are BeiDou coarse positioning coordinates, v B v L v H It is a vehicle speed signal, converted into speed in latitude / longitude / altitude direction, via v. B =v*cos heading; v L =v*sin(heading); v H =0; a B a L a H It is set to 0 upon initial startup and subsequently updated using acceleration data from the IMU inertial navigation module. Set the initial state covariance matrix: P0 = diag([1e -4 ,1e -4 ,1e -2 ,1e -3 ,1e -3 ,1e -3 ,1e -2 ,1e -2 ,1e -2 Preset process noise variance matrix Q and observation noise variance matrix R; Process noise variance matrix: Q = diag([1e -8 ,1e -8 ,1e -4 ,1e -6 ,1e -6 ,1e -6 ,1e -4 ,1e -4 ,1e -4 ]); Observation noise variance matrix: R=diag([1e -7 ,1e -7 ,1e -3 ,1e -5 ,1e -5 ,1e -5 ,1e -3 ,1e -3 ,1e -3 ]); Step 2: Construct the state transition matrix; Where I3 is a 3×3 identity matrix and O3 is a 3×3 zero matrix; T is the filtering period, which is consistent with the sampling frequency of the IMU inertial navigation module, and is 0.1s; Step 3: Predict the state and covariance; State prediction; in, Predict the state at the current moment. The state estimate updated at the previous time step; F k This is the state transition matrix; Covariance prediction; Among them, F k P is the state transition matrix, Q is the process noise variance matrix; k-1 The covariance updated in the previous time step; The current predicted covariance; Step 4: Construct the observation vector Z k With observation matrix H k ; Among them, B sat ,L sat H sat The corrected positioning coordinates for low-Earth orbit satellites; a X ,a Y ,a z To obtain the acceleration, ω X ,ω Y ,ω Z The obtained angular velocity; Where I3 is a 3×3 identity matrix, 03 is a 3×3 zero matrix, and R a R is the acceleration coordinate system transformation matrix; ω This is the transformation matrix from angular velocity to velocity change; Step 5: Correct the prediction results using observation data; Calculate the filter gain K k ; in For the current predicted covariance, H k R is the observation matrix, and R is the observation noise variance matrix; Update state estimates; Among them, Z k For observation vectors; To predict the state at the current moment, K k H is the filter gain. k The observation matrix; Update the covariance matrix; Where I is a 9×9 identity matrix. K represents the current predicted covariance. k H is the filter gain. k The observation matrix is ​​updated, and the covariance reflects the uncertainty of the current state, which is used for prediction at the next time step. Step 6: Output the position coordinates for the correction accuracy; After 5 consecutive iterations, if the state covariance matrix P k The values ​​of the first three diagonal elements are ≤1e -6 If convergence is achieved, the state estimate is obtained. Extracting state estimation The first 3 elements, output position coordinates (B k ,L k H k Otherwise, continue iterating until convergence, and output the position coordinates. If convergence is not achieved after 20 iterations, output the final position coordinates and mark them as the reference position.

6. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 5, characterized in that: Step 4 also includes: Calculate R a : Obtain the attitude angles and convert them into rotation matrices; Obtain the heading angle ψ, pitch angle θ, and roll angle. According to respectively Calculate the roll angle rotation matrix R1; Calculate the pitch angle rotation matrix R2; Calculate the heading angle rotation matrix R3; According to R a =R3*R2*R1 to obtain the acceleration coordinate system transformation matrix; Based on the acceleration coordinate system transformation matrix, the deceleration is converted into a vector. according to Get a B a L a H Update the filter parameters; Calculate R ω According to the formula Calculate and obtain R ω .

7. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 1, characterized in that: Define the data frame format of the protocol, specifically the frame header, vehicle ID, location data, MSD data, checksum, and frame trailer; the total length is ≤64 bytes.

8. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 7, characterized in that: Step S3 is as follows: When a serious collision occurs, causing the airbags to deploy or the in-vehicle sensors to detect a major accident and send a trigger command to the vehicle terminal to automatically trigger the E-CALL system, or when the driver presses the SOS physical button to manually trigger and activate the E-CALL system, the SBOX extracts the latest corrected position coordinates, the MSD data cached for the past 30 seconds, the vehicle ID, etc., and assembles them according to the preset data frame format. The check bit is calculated using the CRC32 algorithm, appended to the end of the data frame to encapsulate it into a data frame, and transmitted to the emergency management platform through the vehicle terminal-satellite-emergency management platform communication protocol.

9. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 1, characterized in that: MSD data includes: tire pressure and engine fault codes.

10. The commercial vehicle positioning enhancement and emergency rescue method integrating BeiDou system and low-orbit satellite communication as described in claim 1, characterized in that: Based on the triggering of the airbag, it is a collision accident; a sudden drop in tire pressure indicates a tire blowout; an engine fault code indicates a mechanical failure; manual triggering indicates an emergency accident and the accident type can be determined.

Images