Multi-source data fusion system and method for laser radar assisted train positioning

Abstract

The invention provides a multi-source data fusion system and method for laser radar assisted train positioning, and the system comprises a front-end odometer and a rear-end diagram optimization device. The front-end odometer comprises a multi-satellite multi-frequency external measuring antenna module, a double-antenna GNSS navigation receiver module, an SINS inertial navigation module and a laser radar information acquisition module; the rear-end map optimization device comprises an inertial navigation calculation module, a loopback detection module, an information fusion navigation module, a map optimization module, a map construction module and a navigation result display module. According to the multi-source data fusion system for laser radar assisted train positioning, a satellite, a gyroscope and a laser signal are fused, and the purpose of improving the positioning reliability and accuracy in a complex environment is achieved.

Description

Technical Field

[0001] The present invention belongs to the field of navigation technology, and in particular relates to a multi-source data fusion system and method for laser radar-assisted train positioning. Background Art

[0002] Train positioning technology is one of the core technologies in rail transit systems. It is mainly used to determine the position, speed and direction of trains in real time to ensure driving safety and efficiency.

[0003] ‌Existing positioning technologies are as follows:

[0004] Track circuit and axle counter positioning uses track circuits to segment track sections, uses current signals to determine train occupancy, and combines this with axle counters to count wheel passes to determine train position. This method is a basic positioning method and relies on trackside equipment.

[0005] With transponder positioning, ground-based transponders store fixed coordinates. Trains use beacon antennas underneath the train to read this data and obtain their precise initial position. This data is then combined with an coded odometer to calculate distance traveled, enabling continuous positioning. 56 For example, densely deploying beacons along subway lines can reduce cumulative errors.

[0006] Speed ​​measurement and positioning technology, coded odometer: Calculates distance traveled by wheel rotations, combined with initial position for positioning. However, this technology is susceptible to wheel spin or slippage and requires regular calibration.

[0007] Doppler radar: It uses the Doppler frequency shift generated by ultrasonic wave reflection to measure speed. Its non-contact nature makes it suitable for complex environments and offers high accuracy.

[0008] Satellite positioning (GPS) directly obtains the longitude and latitude coordinates of the train, but is limited by signals in tunnels, elevated roads, and other scenarios. It is often integrated with inertial navigation (INS) to eliminate noise and accumulated errors through the Kalman filter algorithm.

[0009] Existing technologies are suitable for single environments and have problems with low reliability and accuracy in complex environments. Summary of the Invention

[0010] In response to the problems existing in the prior art, the present invention provides a multi-source data fusion system and method for lidar-assisted train positioning, which at least partially solves the problems of low positioning reliability and accuracy in complex environments existing in the prior art.

[0011] In a first aspect, an embodiment of the present disclosure provides a multi-source data fusion system for laser radar-assisted train positioning, comprising: a front-end odometer and a back-end map optimization device, wherein the front-end odometer comprises: a multi-satellite multi-frequency external measurement antenna module, a dual-antenna GNSS navigation receiver module, a SINS inertial navigation module, and a laser radar information acquisition module;

[0012] The back-end graph optimization device includes: inertial navigation solution module, loop detection module, information fusion navigation module, graph optimization module, map construction module and navigation result display module;

[0013] The output end of the multi-star multi-frequency external measurement antenna module is electrically connected to the input end of the dual-antenna GNSS navigation receiver module, the output end of the dual-antenna GNSS navigation receiver module and the output end of the SINS inertial navigation module are respectively electrically connected to the input end of the inertial navigation solution module, the output end of the SINS inertial navigation module and the output end of the lidar information acquisition module are respectively electrically connected to the input end of the map optimization module, the output end of the map optimization module is electrically connected to the input end of the loop detection module, the output end of the inertial navigation solution module and the output end of the loop detection module are respectively electrically connected to the input end of the information fusion navigation module, the output end of the information fusion navigation module is electrically connected to the input end of the map construction module, and the output end of the map construction module is electrically connected to the input end of the navigation result display module.

[0014] Optionally, the multi-satellite multi-frequency external measurement antenna module adopts a multi-feed point design.

[0015] The data output by the dual-antenna GNSS navigation receiver module includes satellite RTK data, which includes: longitude, latitude, elevation, easting speed, northing speed, celestial speed and track angle;

[0016] The SINS inertial navigation module has a built-in IMU gyroscope.

[0017] Optionally, the inertial navigation solution module is used to perform data solution on the acquired GNSS satellite receiver data and SINS inertial navigation module data to obtain the position, attitude and speed information of the carrier.

[0018] Optionally, the acquired GNSS satellite receiver data and SINS inertial navigation module data are processed, including:

[0019] The angular velocity signals of the carrier in roll, pitch and heading, which are sensed in real time by the IMU gyroscope, are integrated to obtain the attitude angle of the carrier relative to the initial attitude or reference coordinate system, thereby determining the spatial attitude of the carrier.

[0020] Optionally, the laser radar information acquisition module is used to process the acquired three-dimensional laser point cloud data using a spherical linear interpolation algorithm so that the point cloud data frequency is consistent with the GNSS and SINS data frequencies.

[0021] Optionally, the information fusion navigation module is used to parse the navigation positioning results according to weight distribution based on the attitude, speed and position of the carrier obtained by the inertial navigation solution module and the attitude, speed and position information analyzed by the lidar information acquisition module.

[0022] Optionally, the map construction module is used to construct a map based on the navigation positioning results and point cloud environment data analyzed by the information fusion navigation module.

[0023] In a second aspect, an embodiment of the present disclosure further provides a multi-source data fusion method for lidar-assisted train positioning, which is applied to any system described in the first aspect, comprising:

[0024] Collect real-time data;

[0025] Analyze the collected real-time data to obtain analytical data;

[0026] Use data error analysis algorithm to remove outliers in analytical data and obtain optimized analytical data;

[0027] Based on the optimized analytical data, inertial navigation is used to calculate the speed, attitude and position information;

[0028] Perform distortion compensation on the acquired point cloud data to obtain compensated point cloud data;

[0029] Perform loop closure detection on the compensated point cloud data to eliminate errors in the point cloud data;

[0030] The velocity, attitude and position information obtained by inertial navigation and the point cloud data with error elimination are fused based on the Kalman filter;

[0031] The data based on information fusion uses the sparsity of the point cloud feature data extraction algorithm to fix the feature point positions, form a matching map, and complete the map construction.

[0032] Optionally, the use of a data error analysis algorithm to eliminate outliers in the analytical data to obtain optimized analytical data includes:

[0033] Define the loss function and convert the parameter estimation problem into the sum of the optimization objective functions;

[0034] After calculating the first-order derivative of the loss function, the parameters are solved by iterative reweighted least squares method to obtain the optimized analytical data.

[0035] Optionally, performing inertial navigation calculation based on the optimized analytical data to obtain speed, attitude, and position information includes:

[0036] Determine the coordinate system;

[0037] Establish an error equation based on the determined coordinate system;

[0038] Establish a posture update algorithm based on the error equation and coordinate system;

[0039] Update the train speed based on the attitude update algorithm and the acquired SINS data;

[0040] The train position is updated based on the updated speed data.

[0041] The present invention provides a multi-source data fusion system and method for laser radar-assisted train positioning. The multi-source data fusion system for laser radar-assisted train positioning, by fusing satellite, gyroscope and laser signals, has the advantages of high precision, real-time performance and strong environmental adaptability. It can provide accurate positioning information in complex environments, make up for the shortcomings of GNSS signals when they are blocked, and locate the train, thereby achieving the purpose of improving positioning reliability and accuracy in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0042] The above and other objects, features and advantages of the present disclosure will become more apparent through a more detailed description of exemplary embodiments of the present disclosure with reference to the accompanying drawings, wherein like reference numerals generally represent like components throughout the exemplary embodiments of the present disclosure.

[0043] Figure 1 A functional block diagram of a multi-source data fusion system for laser radar-assisted train positioning according to an embodiment of the present disclosure;

[0044] Figure 2 A schematic diagram of the process of constructing a map using lidar data provided in an embodiment of the present disclosure;

[0045] Figure 3 A schematic diagram of the image optimization and dedistortion process provided by an embodiment of the present disclosure;

[0046] Figure 4 A schematic diagram of constructing map feature points provided in an embodiment of the present disclosure;

[0047] Figure 5 A schematic diagram of the Kalman filter equation structure provided in an embodiment of the present disclosure. DETAILED DESCRIPTION

[0048] The embodiments of the present disclosure are described in detail below with reference to the accompanying drawings.

[0049] It should be clear that the following embodiments of the present disclosure are described through specific concrete examples, and those skilled in the art can easily understand other advantages and effects of the present disclosure from the contents disclosed in this specification. Obviously, the described embodiments are only a part of the embodiments of the present disclosure, rather than all the embodiments. The present disclosure can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present disclosure. It should be noted that the following embodiments and features in the embodiments can be combined with each other in the absence of conflict. Based on the embodiments in the present disclosure, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present disclosure.

[0050] It should be noted that various aspects of the embodiments within the scope of the appended claims are described below. It should be apparent that the aspects described herein can be embodied in a wide variety of forms, and any specific structure and / or function described herein is merely illustrative. Based on this disclosure, it should be understood by those skilled in the art that an aspect described herein can be implemented independently of any other aspect, and two or more of these aspects can be combined in various ways. For example, any number of aspects described herein can be used to implement an apparatus and / or practice a method. In addition, other structures and / or functionalities other than one or more of the aspects described herein can be used to implement this apparatus and / or practice this method.

[0051] It should also be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present disclosure. The illustrations only show components related to the present disclosure and are not drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component can be changed at will, and the component layout type may also be more complicated.

[0052] Additionally, in the following description, specific details are provided to provide a thorough understanding of the examples. However, one skilled in the art will appreciate that the aspects described can be practiced without these specific details.

[0053] For ease of understanding, Figure 1 As shown, this embodiment discloses a multi-source data fusion system for laser radar-assisted train positioning, including: a front-end odometer and a back-end map optimization device. The front-end odometer includes: a multi-satellite multi-frequency external measurement antenna module, a dual-antenna GNSS navigation receiver module, a SINS inertial navigation module and a laser radar information acquisition module;

[0054] The back-end graph optimization device includes: inertial navigation solution module, loop detection module, information fusion navigation module, graph optimization module, map construction module and navigation result display module;

[0055] The output end of the multi-star multi-frequency external measurement antenna module is electrically connected to the input end of the dual-antenna GNSS navigation receiver module, the output end of the dual-antenna GNSS navigation receiver module and the output end of the SINS inertial navigation module are respectively electrically connected to the input end of the inertial navigation solution module, the output end of the SINS inertial navigation module and the output end of the lidar information acquisition module are respectively electrically connected to the input end of the map optimization module, the output end of the map optimization module is electrically connected to the input end of the loop detection module, the output end of the inertial navigation solution module and the output end of the loop detection module are respectively electrically connected to the input end of the information fusion navigation module, the output end of the information fusion navigation module is electrically connected to the input end of the map construction module, and the output end of the map construction module is electrically connected to the input end of the navigation result display module.

[0056] Optionally, the multi-satellite multi-frequency external measurement antenna module adopts a multi-feed point design.

[0057] The data output by the dual-antenna GNSS navigation receiver module includes satellite RTK data, which includes: longitude, latitude, elevation, easting speed, northing speed, celestial speed and track angle;

[0058] The SINS inertial navigation module has a built-in IMU gyroscope.

[0059] Optionally, the inertial navigation solution module is used to perform data solution on the acquired GNSS satellite receiver data and SINS inertial navigation module data to obtain the position, attitude and speed information of the carrier.

[0060] Optionally, the acquired GNSS satellite receiver data and SINS inertial navigation module data are processed, including:

[0061] The angular velocity signals of the carrier in roll, pitch and heading, which are sensed in real time by the IMU gyroscope, are integrated to obtain the attitude angle of the carrier relative to the initial attitude or reference coordinate system, thereby determining the spatial attitude of the carrier.

[0062] Optionally, the laser radar information acquisition module is used to process the acquired three-dimensional laser point cloud data using a spherical linear interpolation algorithm so that the point cloud data frequency is consistent with the GNSS and SINS data frequencies.

[0063] Optionally, the information fusion navigation module is used to parse the navigation positioning results according to weight distribution based on the attitude, speed and position of the carrier obtained by the inertial navigation solution module and the attitude, speed and position information analyzed by the lidar information acquisition module.

[0064] Optionally, the map construction module is used to construct a map based on the navigation positioning results and point cloud environment data analyzed by the information fusion navigation module.

[0065] In a specific application scenario, the multi-star multi-frequency external measurement antenna module adopts a multi-feed point design with high unit gain, strong satellite search signal, and strong antenna gain, which makes the entire integrated navigation system have a certain degree of anti-interference ability.

[0066] The data output by the dual-antenna GNSS navigation receiver module includes satellite RTK data, which includes information such as longitude, latitude, elevation, easting speed, northing speed, celestial speed and track angle.

[0067] The SINS inertial navigation module's built-in IMU gyroscope can sense changes in the carrier's angular velocity along three axes (roll, pitch, and heading) in real time. By integrating these angular velocity signals, it can accurately calculate the carrier's attitude angle relative to the initial attitude or reference coordinate system, thereby determining the carrier's spatial attitude. This module does not rely on external signals such as GPS signals and can provide continuous and reliable navigation information to the carrier in various complex environments, such as underground and tunnels, and even when the GPS signal is blocked or interfered with, ensuring that the carrier accurately navigates along the planned route.

[0068] The functions of the inertial navigation solution module include performing data solution on the acquired GNSS satellite receiver data and SINS inertial navigation module data to obtain the position, attitude and speed information of the carrier.

[0069] The laser radar information acquisition module includes acquiring three-dimensional laser point cloud data and processing the laser point cloud data using a spherical linear interpolation algorithm to make the point cloud data frequency consistent with the GNSS and SINS data frequency.

[0070] The image optimization module is mainly used for point cloud data dedistortion processing. It extracts features from the corrected point cloud data to obtain the collective features and intensity information of the point cloud.

[0071] The loop detection module mainly includes the point cloud data features of two adjacent frames. The purpose is to use the loop detection results and the inertial navigation prior pose to correct the odometry error.

[0072] The information fusion navigation module mainly includes the attitude, velocity, position of the carrier analyzed by inertial navigation and the attitude, velocity, position analyzed by lidar, and the navigation positioning results are analyzed according to weight distribution.

[0073] The map construction module mainly uses the carrier's attitude, speed, position information and point cloud environment data solved by the information fusion module to construct the map display. The process of constructing the map with lidar data is as follows: Figure 2 shown.

[0074] The function of the navigation result display module of the vehicle-mounted host computer is to store the received data and display the navigation results.

[0075] The system disclosed in this embodiment includes two parts: a multi-front-end odometer and a back-end map optimization system. The front-end odometer mainly includes a multi-satellite multi-frequency external measurement antenna module, a dual-antenna GNSS navigation receiver module, a SINS inertial navigation module, and a lidar information acquisition module. The back-end map optimization system mainly includes a map optimization module, a loop detection module, an information fusion navigation module, an inertial navigation solution module, a map construction module, and a navigation result display module. The modules are electrically connected to each other, and each module operates independently, including the collection, analysis, and solution of each part of the data, and is powered by a safe power supply.

[0076] This embodiment also discloses a multi-source data fusion method for laser radar-assisted train positioning, which is applied to the system disclosed in this embodiment, including:

[0077] Collect real-time data;

[0078] Analyze the collected real-time data to obtain analytical data;

[0079] Use data error analysis algorithm to remove outliers in analytical data and obtain optimized analytical data;

[0080] Based on the optimized analytical data, inertial navigation is used to calculate the speed, attitude and position information;

[0081] Perform distortion compensation on the acquired point cloud data to obtain compensated point cloud data;

[0082] Perform loop closure detection on the compensated point cloud data to eliminate errors in the point cloud data;

[0083] The velocity, attitude and position information obtained by inertial navigation and the point cloud data with error elimination are fused based on the Kalman filter;

[0084] The data based on information fusion uses the sparsity of the point cloud feature data extraction algorithm to fix the feature point positions, form a matching map, and complete the map construction.

[0085] Optionally, the use of a data error analysis algorithm to eliminate outliers in the analytical data to obtain optimized analytical data includes:

[0086] Define the loss function and convert the parameter estimation problem into the sum of the optimization objective functions;

[0087] After calculating the first-order derivative of the loss function, the parameters are solved by iterative reweighted least squares method to obtain the optimized analytical data.

[0088] Optionally, performing inertial navigation calculation based on the optimized analytical data to obtain speed, attitude, and position information includes:

[0089] Determine the coordinate system;

[0090] Establish an error equation based on the determined coordinate system;

[0091] Establish a posture update algorithm based on the error equation and coordinate system;

[0092] Update the train speed based on the attitude update algorithm and the acquired SINS data;

[0093] The train position is updated based on the updated speed data.

[0094] In a specific application scenario, a multi-source data fusion method for lidar-assisted train positioning is characterized by including:

[0095] Step 1: Collect real-time data, which includes data transmitted from the satellite multi-frequency external measurement antenna module to the dual-antenna GNSS navigation receiver, including latitude, longitude and elevation data. SINS data includes three-axis Gyroscope data, three-axis Accelerometer data, lidar point cloud data.

[0096] Step 2: Data analysis. The analyzed data includes dual-antenna GNSS navigation receiver data, three-axis gyroscope data and three-axis accelerometer data output by SINS, and three-dimensional point cloud data collected by lidar.

[0097] Step 3: Establishment of data error analysis algorithm. Due to external and internal errors in the multi-source data, the collected data contain outliers, so the M estimation algorithm is used to eliminate outliers.

[0098] Step 3.1: Define the loss function and convert the parameter estimation problem into the optimization of the sum of objective functions, as shown in Formula 1.

[0099] (1)

[0100] in, are model parameters, is the parameter to be estimated, For the observations, Is the loss function, used to measure the single observation value and parameter degree of deviation.

[0101] Step 3.2: Solve the optimization problem. First, calculate the first-order derivative of the loss function, and then solve the parameters through iterative reweighted least squares (IRLS), as shown in Formula 2 and Formula 3.

[0102] (2)

[0103] (3)

[0104] in, is the influence function, is the residual. is the sample size.

[0105] Step 4: Inertial Navigation Solution: Use the optimized data for inertial navigation solution to obtain the speed, attitude, and position information required by the navigation system. The specific steps are as follows.

[0106] Step 4.1: Determine the coordinate system. The main coordinate systems are the geocentric inertial coordinate system, the earth coordinate system, the navigation coordinate system, and the carrier coordinate system. The geocentric inertial coordinate system is a quasi-inertial coordinate system with the origin at the center of the earth. and The axis is in the plane of the Earth's equator, The axis points to the vernal point (one of the intersection points where the intersection of the equator and the ecliptic plane intersects the celestial sphere). The axis is the Earth's rotation axis and points to the North Pole. The output of the inertial sensor is based on this coordinate system. The origin is the center of the Earth, and The axis is in the Earth's equatorial plane, where Pointing to the prime meridian, is the Earth's rotation axis, pointing to the North Pole. The angular motion of the Earth's coordinate system relative to the inertial coordinate system is the Earth's rotation angular rate, which is =7.2921151467*10 -5 rad / s=15.0410671786° / h. The navigation coordinate system moves with the movement of the carrier on the surface of the earth, and the "east-north-sky" direction is selected as the navigation coordinate system. The carrier coordinate system is used It indicates that its origin is the center of gravity of the carrier, The axis is to the right along the horizontal axis of the carrier, The shaft moves forward along the longitudinal axis of the carrier, The shaft is upward along the vertical axis of the carrier, and the b system is fixedly connected to the carrier.

[0107] Step 4.2: Establish the error equation. The error equation is the basis for subsequent filtering and graph optimization fusion solutions.

[0108] (4)

[0109] in, They are position error, velocity error, attitude angle error, and bias error respectively.

[0110] Step 4.3: Establish attitude update algorithm. Select the "East-North-Sky" geographic coordinate system as the navigation reference coordinate system of this system, denoted as System, then The attitude differential equation of the system as a reference system is

[0111] (5)

[0112] Among them, the matrix Indicates the carrier system ( relative to The posture array of the system, express Relative to The rotation of the navigation system caused by the rotation of the Earth and The system moves near the Earth's surface due to the curvature of the Earth's surface. The system rotates, that is, ,in and It can be obtained by formulas 6 and 7.

[0113] (6)

[0114] (7)

[0115] Then the discretization method is used, assuming that the gyroscope is in the time period Two equally spaced samples were taken within the and , using the two-sample cone compensation algorithm, there is

[0116] (8)

[0117] Typically during the navigation update cycle It can be considered that it is caused by speed and position The change is small, visible is a constant value, denoted as , then Formula 9 is the attitude update algorithm of the numerical recursion of this navigation system.

[0118] (9)

[0119] Step 4.4: Velocity Update. Velocity update is a key step in improving navigation accuracy and reliability. It not only reflects the object's motion in real time, helping the system accurately predict and correct its position, but also provides an important basis for multi-sensor fusion optimization. Velocity update can be performed by calculating the velocity using Equation 10 based on SINS data.

[0120] (10)

[0121] in, For time period and The inertial velocity at the moment, Time periods The internal navigation system compares the velocity increment and the velocity increment of harmful acceleration.

[0122] Step 4.5: Position update. Using the speed information and GNSS data, the distance traveled is calculated using the trapezoidal integration method and the position is updated, as shown in Formulas 11 and 12.

[0123] (11)

[0124] (12)

[0125] in, 、 、 Represents the local latitude and altitude respectively. 、 They are the radius of curvature on the meridian circle and the radius of curvature on the meridian circle respectively.

[0126] Step 5: Image optimization. Distortion compensation is performed on the point cloud data to make the map construction more accurate. Due to the noise interference of the train running environment, the generated lidar data is distorted (caused by rotation and translation). By compensating the coordinates of each laser point, the compensation amount is the change of the radar coordinates relative to the start time of the frame. According to Euler's rotation law, the coordinates of two points in space can be completed by one rotation and translation. The specific structure is as follows: Figure 3 Assume that in a frame of point cloud, the radar pose at the initial moment is as shown in Formula 13.

[0127] (13)

[0128] No. When collecting laser points, the radar's posture is shown in Formula 14.

[0129] (14)

[0130] No. The coordinates of the laser points are: , then The coordinate transformation formula for compensating the distortion of a laser point is shown in Equation 15.

[0131] (15)

[0132] in is the coordinate after distortion compensation, is the rotation matrix, is the displacement from the previous coordinate to the next coordinate.

[0133] Step 6: Loop closure detection. Because LiDAR data collection is subject to various errors and data drift, errors increase with driving time. Therefore, a point-to-point ICP matching algorithm based on SVD is used for loop closure detection and error elimination. First, a point set is determined and an objective function is established. The rotation and translation matrices between the interrelated point sets are determined by selecting the point set. This is shown in Equations 16 and 17.

[0134] (16)

[0135] (17)

[0136] in, The solution is shown in Equations 18-20.

[0137] (18)

[0138] in, and are the centroids of point sets X and Y respectively, , , .

[0139] make , , then, for any R, we can find a t such that ,Right now .Right now

[0140] (19)

[0141] make but ,in, , so the problem is transformed into finding a suitable ,make Reach the maximum. According to the theorem: if there is a positive definite matrix , then for any orthogonal matrix ,have .Pick , transform the problem into finding the .

[0142] First, perform SVD decomposition on H and take , , then we have the following formula 20

[0143] (20)

[0144] get Afterwards, there is Get the translation vector .

[0145] Step 7: Information fusion based on the Kalman filter. The filter problem is generally "prediction + observation = fusion result". Combined with the actual point cloud data, the prediction is provided by SINS, and the observation is the fusion of the LiDAR point cloud data, map matching, and GNSS calculated pose and position information, as shown below.

[0146] The state space model of the random system is:

[0147] (twenty one)

[0148] Where, For Time has come The state transfer matrix at time t; is the state vector; is the observation vector; is the system error vector; is the observation error vector; is the coefficient matrix; , Satisfy the following properties:

[0149] ; ; ; (twenty two)

[0150] , ; , ; (twenty three)

[0151] The classic Kalman filter algorithm is described as:

[0152] (twenty four)

[0153] (25)

[0154] (26)

[0155] (27)

[0156] (28)

[0157] in, 、 They are The state vector at the moment and the corresponding posterior variance matrix; 、 They are The one-step prediction value of the state at time t and the corresponding covariance matrix; for The filter gain at time t.

[0158] Step 8: Map construction. As the LiDAR moves, the map size will gradually increase, resulting in reduced computational efficiency. Therefore, the sparsity of the point cloud feature data extraction algorithm is used to fix the location of feature points to form a matching map.

[0159] (29)

[0160] in, represents the camera's pose, and the edge of the map is the estimate of the relative motion between two pose nodes. The estimate is analyzed by feature analysis and outliers are removed. Figure 4 shown.

[0161] According to the optimization idea, Formula 30 is an approximation, so Formula 30 is expressed as follows:

[0162] (30)

[0163] Constructing the error term ,

[0164] (31)

[0165] Setting the overall objective function as shown in Formula 24 will result in a set of all edges.

[0166] (32)

[0167] Step 9: Construct the Kalman filter equation. Considering the update of the lidar posture and the data loss when the satellite signal is blocked, the system model is constructed as shown in formulas 33-37. The Kalman filter equation structure is as follows: Figure 5 shown.

[0168] (33)

[0169] (34)

[0170] (35)

[0171] (36)

[0172] (37)

[0173] in, , , , .

[0174] Step 10: Data output and display. The train's running trajectory and map construction are output to the onboard host computer attitude display module for display and storage.

[0175] The multi-source data fusion method for lidar-assisted train positioning in complex environments proposed in this embodiment can effectively solve the problem of train positioning function degradation in complex environments. At the same time, the system is easy to build and easy to promote.

[0176] The multi-source data fusion method for LiDAR-assisted train positioning in complex environments proposed in this embodiment effectively addresses the problem of train trajectory divergence caused by a single GNSS / SINS integrated navigation algorithm when satellite signal lock is lost by coordinating with the GNSS / SINS integrated navigation system and assigning different weights. This method effectively achieves real-time navigation in model rooms and urban high-rise environments, and simultaneously constructs a model within the field of view.

[0177] The multi-source data fusion method for lidar-assisted train positioning in complex environments proposed in this embodiment can fuse multi-source sensor data in real time and provide a navigation solution.

[0178] The multi-source data fusion method for lidar-assisted train positioning in complex environments proposed in this embodiment can not only calculate a more accurate navigation solution when the satellite signal is good, but also effectively solve the problem of trajectory divergence by allowing the lidar to take over the system navigation control when the satellite signal is lost.

[0179] The multi-source data fusion method for lidar-assisted train positioning in complex environments proposed in this embodiment is combined with the existing navigation algorithm and innovatively integrated into the lidar module. By eliminating abnormal data and optimizing the posture, it accurately draws the characteristic map of the external environment of the train operation, calculates the train position information and displays it on the on-board host computer.

[0180] The multi-source data fusion method for lidar-assisted train positioning in complex environments proposed in this embodiment is combined with existing navigation algorithms. In underwater and other location environments and when satellite signals are completely lost for a long time, navigation and positioning requirements can also be met and driving maps can be drawn through lidar and IMU data.

[0181] The fusion method proposed in this embodiment integrates multi-source sensor data to develop a positioning algorithm capable of handling complex signal occlusion environments. The input data for this method includes IMU data, longitude, latitude, altitude, northeast celestial velocity, track angle information, and three-dimensional environmental perception data provided by a dual-antenna GNSS receiver. The system adopts a loosely coupled design pattern, integrating GNSS, SINS, and lidar data while also prioritizing data: when the GNSS signal is normal, GNSS data is prioritized; when the GNSS signal loses lock, lidar data is switched to, thereby building a reliable navigation and positioning algorithm.

[0182] like Figure 2 As shown, the method proposed in this embodiment, in which the dual-antenna GNSS navigation receiver module, the SINS inertial measurement unit and the lidar information acquisition module operate in coordination, presents a series of steps for trajectory alignment, constraint construction and optimization. The process first performs the trajectory alignment operation, then introduces the relative pose information of the odometer, and performs loop detection. If a loop is detected, the corresponding loop constraint is added; if no loop is detected, a priori constraints are added. Then, it is determined whether the optimization cycle is completed: if not, it returns to continue detecting loops; if it is completed, it enters the optimization stage. Through continuous constraint detection and addition, the entire process ultimately achieves the optimization goal of the system.

[0183] like Figure 3 As shown, the point cloud is first preprocessed, followed by the nearest neighbor points. The point cloud is then transformed by solving for the rotation matrix R and the translation vector t. R and t are gradually optimized, iterating until the termination condition is met. The optimized R and t are finally output.

[0184] like Figure 4 As shown, there are some outliers in the initial data. These are identified and removed using the data error analysis algorithm. After processing, only key feature points are retained, which form a more accurate and concise feature map, thereby improving the quality and usability of the map.

[0185] Figure 5Figure 1 shows the pose estimation process based on a Kalman filter. It begins with pose initialization, followed by initialization of the Kalman filter parameters. Next, inertial calculations are performed using inertial sensor data to obtain the current pose information, and the predicted pose is obtained using the Kalman prediction update step. When observation data is available, the Kalman measurement update phase begins, using this observation data to optimize and adjust the predicted pose, calculate the posterior pose, and reset the state variables. If no observation data is available, the predicted pose is directly output as the result.

[0186] The basic principles of the present disclosure have been described above in conjunction with specific embodiments. However, it should be noted that the advantages, strengths, and effects mentioned in this disclosure are merely illustrative and not restrictive, and should not be construed as necessarily possessed by each embodiment of the present disclosure. Furthermore, the specific details disclosed above are provided for illustrative purposes and to facilitate understanding, rather than as limitations. These details do not limit the present disclosure to necessarily being implemented using these specific details.

[0187] In the present disclosure, relational terms such as first and second, etc. are merely used to distinguish one entity or operation from another entity or operation, and do not necessarily require or imply that there is any such actual relationship or order between these entities or operations. The block diagrams of the devices, devices, equipment, and systems involved in the present disclosure are merely illustrative examples and are not intended to require or imply that they must be connected, arranged, or configured in the manner shown in the block diagrams. As will be appreciated by those skilled in the art, these devices, devices, equipment, and systems can be connected, arranged, or configured in any manner. Words such as "including," "comprising," "having," and the like are open-ended words, meaning "including but not limited to," and can be used interchangeably therewith. The words "or" and "and" used herein refer to the words "and / or" and can be used interchangeably therewith, unless the context clearly indicates otherwise. The word "such as" used herein refers to the phrase "such as but not limited to," and can be used interchangeably therewith.

[0188] Additionally, as used herein, "or" used in a list of items beginning with "at least one" indicates a separate list, so that, for example, a list of "at least one of A, B, or C" means A or B or C, or AB or AC or BC, or ABC (i.e., A and B and C). Furthermore, the word "exemplary" does not mean that the example described is preferred or better than other examples.

[0189] It should also be noted that in the system and method of the present disclosure, each component or each step can be decomposed and / or recombined. Such decomposition and / or recombination should be regarded as equivalent solutions of the present disclosure.

[0190] Various changes, substitutions, and modifications may be made to the technology described herein without departing from the teachings defined by the appended claims. Moreover, the scope of the claims of this disclosure is not limited to the specific aspects of the processes, machines, manufactures, compositions of things, means, methods, and actions described above. Currently existing or later developed processes, machines, manufactures, compositions of things, means, methods, or actions that perform substantially the same function or achieve substantially the same results as the corresponding aspects described herein may be utilized. Accordingly, the appended claims include within their scope such processes, machines, manufactures, compositions of things, means, methods, or actions.

[0191] The above description of the disclosed aspects is provided to enable any person skilled in the art to make or use the present disclosure. Various modifications to these aspects will be readily apparent to those skilled in the art, and the general principles defined herein may be applied to other aspects without departing from the scope of the present disclosure. Therefore, the present disclosure is not intended to be limited to the aspects shown herein, but rather to be accorded the widest scope consistent with the principles and novel features disclosed herein.

[0192] The above description has been provided for the purpose of illustration and description. In addition, this description is not intended to limit the embodiments of the present disclosure to the forms disclosed herein. Although a number of example aspects and embodiments have been discussed above, those skilled in the art will recognize certain variations, modifications, alterations, additions, and sub-combinations thereof.

Claims

1. A multi-source data fusion system for laser radar-assisted train positioning, characterized in that: include: Front-end odometer and back-end map optimization device. The front-end odometer includes: multi-star multi-frequency external measurement antenna module, dual-antenna GNSS navigation receiver module, SINS inertial navigation module and lidar information acquisition module; The back-end graph optimization device includes: inertial navigation solution module, loop detection module, information fusion navigation module, graph optimization module, map construction module and navigation result display module; The output end of the multi-star multi-frequency external measurement antenna module is electrically connected to the input end of the dual-antenna GNSS navigation receiver module, the output end of the dual-antenna GNSS navigation receiver module and the output end of the SINS inertial navigation module are respectively electrically connected to the input end of the inertial navigation solution module, the output end of the SINS inertial navigation module and the output end of the lidar information acquisition module are respectively electrically connected to the input end of the map optimization module, the output end of the map optimization module is electrically connected to the input end of the loop detection module, the output end of the inertial navigation solution module and the output end of the loop detection module are respectively electrically connected to the input end of the information fusion navigation module, the output end of the information fusion navigation module is electrically connected to the input end of the map construction module, and the output end of the map construction module is electrically connected to the input end of the navigation result display module.

2. The multi-source data fusion system for laser radar-assisted train positioning according to claim 1 is characterized in that: The multi-satellite multi-frequency external measurement antenna module adopts a multi-feed point design. The data output by the dual-antenna GNSS navigation receiver module includes satellite RTK data, which includes: longitude, latitude, elevation, easting speed, northing speed, celestial speed and track angle; The SINS inertial navigation module has a built-in IMU gyroscope.

3. The multi-source data fusion system for laser radar-assisted train positioning according to claim 2 is characterized in that: The inertial navigation solution module is used to solve the acquired GNSS satellite receiver data and SINS inertial navigation module data to obtain the position, attitude and speed information of the carrier.

4. The multi-source data fusion system for laser radar-assisted train positioning according to claim 3 is characterized in that: The acquired GNSS satellite receiver data and SINS inertial navigation module data are processed, including: The angular velocity signals of the carrier in roll, pitch and heading, which are sensed in real time by the IMU gyroscope, are integrated to obtain the attitude angle of the carrier relative to the initial attitude or reference coordinate system, thereby determining the spatial attitude of the carrier.

5. The multi-source data fusion system for laser radar-assisted train positioning according to claim 4 is characterized in that: The laser radar information acquisition module is used to process the acquired three-dimensional laser point cloud data using a spherical linear interpolation algorithm to make the point cloud data frequency consistent with the GNSS and SINS data frequency.

6. The multi-source data fusion system for laser radar-assisted train positioning according to claim 5 is characterized in that: The information fusion navigation module is used to analyze the navigation positioning results according to weight distribution based on the attitude, speed and position of the carrier obtained by the inertial navigation solution module and the attitude, speed and position information analyzed by the lidar information acquisition module.

7. The multi-source data fusion system for laser radar-assisted train positioning according to claim 6 is characterized in that: The map construction module is used to construct a map based on the navigation positioning results and point cloud environment data analyzed by the information fusion navigation module.

8. A multi-source data fusion method for laser radar-assisted train positioning, applied to the system according to any one of claims 1 to 7, characterized in that: include: Collect real-time data; Analyze the collected real-time data to obtain analytical data; Use data error analysis algorithm to remove outliers in analytical data and obtain optimized analytical data; Based on the optimized analytical data, inertial navigation is used to calculate the speed, attitude and position information; Perform distortion compensation on the acquired point cloud data to obtain compensated point cloud data; Perform loop closure detection on the compensated point cloud data to eliminate errors in the point cloud data; The velocity, attitude and position information obtained by inertial navigation and the point cloud data with error elimination are fused based on the Kalman filter; The data based on information fusion uses the sparsity of the point cloud feature data extraction algorithm to fix the feature point positions, form a matching map, and complete the map construction.

9. The multi-source data fusion method for laser radar-assisted train positioning according to claim 8, characterized in that: The method of using a data error analysis algorithm to eliminate outliers in the analytical data to obtain optimized analytical data includes: Define the loss function and convert the parameter estimation problem into the sum of the optimization objective functions; After calculating the first-order derivative of the loss function, the parameters are solved by iterative reweighted least squares method to obtain the optimized analytical data.

10. The multi-source data fusion method for laser radar-assisted train positioning according to claim 9, characterized in that: The inertial navigation solution based on the optimized analytical data is used to obtain speed, attitude and position information, including: Determine the coordinate system; Establish an error equation based on the determined coordinate system; Establish a posture update algorithm based on the error equation and coordinate system; Update the train speed based on the attitude update algorithm and the acquired SINS data; The train position is updated based on the updated speed data.

Images