Multi-sensor fusion train positioning method and system based on factor graph optimization

Abstract

The invention provides a multi-sensor fusion train positioning method and system based on factor graph optimization, and belongs to the technical field of train positioning. 16-dimensional state variables are constructed to serve as factor graph optimization input variables; constructing an INS pre-integral factor; when the attitude change of the train exceeds a set threshold value, selecting the laser radar frame at the moment as a key frame, discarding the other laser radar frames in the two key frames, and constructing a local map by adopting a sliding window; constructing a LiDAR odometer factor; constructing a TLC factor; and adding the INS pre-integration factor, the LiDAR odometer factor and the TLC factor into the factor graph, calling iSAM2 for optimization based on maximum posteriori estimation, and outputting a train pose estimation result under the ENU coordinate system. According to the method, the factor graph model is constructed, and the trackside features such as the transponder, the kilometer post and the hectometer post are utilized, so that high-precision positioning of the train in the satellite navigation signal limited and denial environment is realized.

Description

Technical Field

[0001] This invention relates to the field of train positioning technology, specifically to a multi-sensor fusion train positioning method and system based on factor graph optimization. Background Technology

[0002] In the rail transit industry's train operation control system, accurate train positioning is crucial for ensuring safe and efficient operation. In environments where satellite navigation signals are denied (such as tunnels, stations, and urban canyons), the obstruction of satellite navigation signals by buildings or mountains reduces positioning accuracy and may even render positioning services unavailable. Therefore, in satellite navigation denied scenarios such as tunnels, onboard autonomous positioning technology based on multi-sensor fusion has gradually become a research hotspot. Existing rail transit train autonomous positioning technologies typically use wheel odometers and onboard inertial sensors to calculate the train's travel distance, integrating acceleration and angular velocity to obtain short-term attitude and displacement, further superimposing environmental perception sensors to acquire surrounding geometric features (such as tunnel walls) and track area geometric features, and using methods such as laser odometers and visual odometers to achieve relative pose estimation. Some studies combine train direction constraints, track geometric models, and historical trajectory information to project and constrain the positioning results, calculate the train's two-dimensional coordinate position, and reduce the cumulative error of the train during long-distance operation. Summary of the Invention

[0003] The purpose of this invention is to provide a multi-sensor fusion train positioning method and system based on factor graph optimization, so as to solve at least one of the technical problems existing in the background art.

[0004] To achieve the above objectives, the present invention adopts the following technical solution:

[0005] In a first aspect, the present invention provides a multi-sensor fusion train positioning method based on factor graph optimization, comprising:

[0006] A 16-dimensional state variable is constructed for the train's position, speed, attitude, quaternion, accelerometer, and gyroscope to be estimated, and used as the input variable for factor graph optimization.

[0007] When INS data is acquired at the current observation epoch and the next observation epoch, INS pre-integration calculation is performed to construct the INS pre-integration factor;

[0008] When the train's attitude change exceeds the set threshold, the lidar frame at this time is selected as the key frame, the other lidar frames in the two key frames are discarded, and point cloud distortion is removed using INS pre-integration.

[0009] Edge and planar features are extracted from the distorted point cloud, a local map is constructed using a sliding window, and a feature-based point cloud matching algorithm is used to calculate the relative pose transformation and construct the LiDAR odometry factor.

[0010] When a trackside marker is detected, a global coordinate reference is provided to the system to eliminate accumulated errors, and a TLC factor is constructed based on the absolute position constraints of the train.

[0011] Add the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor graph, and use iSAM2 to optimize based on maximum a posteriori estimation, outputting the train pose estimation results in the ENU coordinate system.

[0012] As a further limitation of the first aspect of the present invention, for the multi-source fusion positioning system with factor graph optimization of INS / LiDAR / TLC, the northeast-sky coordinate system is selected as the reference coordinate system, and 16-dimensional state variables are constructed as follows:

[0013]

[0014] In the formula, This represents the state variable at the k-th epoch, which includes the 3D position of the vehicle in the ENU coordinate system. 3D velocity The attitude of the carrier system in quaternion form relative to the ENU coordinate system Zero bias of accelerometers and gyroscopes in the load system , .

[0015] As a further limitation of the first aspect of the invention, based on INS measurements, the relative motion of the train between adjacent time points is calculated through pre-integration, including changes in speed, position, and rotation; the pre-integration formulas for speed, position, and rotation are as follows:

[0016] ;

[0017] ;

[0018] ;

[0019] In the formula, , , These represent the rotation, velocity, and position change matrices between times i and j, respectively. Indicates a time interval; This represents the angular velocity after zero bias compensation; Indicates time up to the current moment The cumulative rotation matrix; This represents the acceleration after zero bias compensation; Indicates time The cumulative velocity increment up to the current time t;

[0020] Constructing the error function of the INS pre-integral factor as follows:

[0021] ;

[0022] In the formula, , , Representing time respectively and The attitude matrix, velocity, and position; Represents gravitational acceleration; This represents the logarithmic mapping from a Lie group to a Lie algebra.

[0023] As a further limitation of the first aspect of the present invention, point cloud distortion correction is performed based on INS data based on the acquired LiDAR keyframes;

[0024] Assuming the LiDAR scan period is Extract the pre-integrated pose sequence within this period;

[0025] For the Each keyframe, its acquisition time Represented as:

[0026] ;

[0027] Interpolation is obtained from the INS pre-integral sequence. Position at any given moment:

[0028] ;

[0029] LiDAR points from Time coordinate system corrected to Time coordinate system:

[0030] ;

[0031] In the formula, , Represents the rotation and translation matrices from LiDAR to INS; express The inverse rotation matrix of the pose at any given time; express The position in the world coordinate system calculated by INS at that moment.

[0032] As a further limitation of the first aspect of the present invention, for point cloud data, after distortion is corrected by INS, edge features and planar features are selected by calculating the curvature of continuous points, and the feature points of the current frame are matched with the previous frame.

[0033] For each edge point of the current frame Find the two nearest neighbor edge points in the local map. Calculate the residual from the point to the line:

[0034] ;

[0035] In the formula, This represents the perpendicular distance from the point to the edge line;

[0036] For each planar point in the current frame Find the three nearest neighbor points on the local map. Calculate the residual from the point to the surface:

[0037] ;

[0038] ;

[0039] In the formula, This represents the perpendicular distance from a point to a plane. Represents the normal vector of the plane;

[0040] Error function for constructing LiDAR odometry factors as follows:

[0041] ;

[0042] In the formula, , These represent the distance residuals from edge feature points to the matching edge lines and the distance residuals from planar feature points to the matching plane, respectively.

[0043] As a further limitation of the first aspect of the present invention, when the lidar is at time... When trackside markers are detected, the cumulative error is corrected using the absolute position information provided by the high-precision map; the observation equation is constructed based on the observation geometry as follows:

[0044] ;

[0045] In the formula, This indicates the location of the sign based on the current position of the train. express The calculated position of the time-tracking train in the world coordinate system; Represents the rotation matrix of the carrier system relative to the ENU coordinate system; and These are the external parameter rotation matrix and translation vector of the lidar coordinate system relative to the carrier system, respectively; This is the three-dimensional coordinate vector of the center point of the sign in the radar coordinate system as measured by the lidar. To observe noise;

[0046] Residual function for constructing TLC factor as follows:

[0047] ;

[0048] In the formula, This refers to the absolute location information of the signboard recorded in the database.

[0049] As a further limitation of the first aspect of the present invention, the system state of the train is estimated based on the maximum a posteriori probability estimation. Represented as:

[0050] ;

[0051] In the formula, Indicates from timestamp 0 to The measurements from all sensors; Indicates from timestamp 0 to All prior information;

[0052] Assuming the noise follows a Gaussian distribution, then according to Bayes' theorem, ... Represented as:

[0053] ;

[0054] In the formula, Indicates time No. The observation error of each sensor; This represents the covariance matrix corresponding to the observation error; The Mahalanobis distance is defined as follows:

[0055] ;

[0056] Therefore, the maximum a posteriori estimation problem is transformed into a nonlinear least squares solution:

[0057] ;

[0058] The global optimization objective function of the multi-source fusion positioning system is obtained as follows:

[0059] ;

[0060] Based on the iSAM2 library of GTSAM, the corresponding factors and state variables are added to the factor graph each time new sensor data is added. Incremental smoothing is used to achieve efficient multi-sensor fusion, suppress cumulative error, and output the optimized train pose estimation result.

[0061] Secondly, the present invention provides a multi-sensor fusion train positioning system based on factor graph optimization, comprising:

[0062] The first construction module is used to construct 16-dimensional state variables for the train position, speed, attitude, quaternion, accelerometer and gyroscope to be estimated, as input variables for factor graph optimization;

[0063] The second construction module is used to perform INS pre-integration calculation and construct INS pre-integration factors when INS data is acquired in the current observation epoch and the next observation epoch.

[0064] The extraction module is used to select the LiDAR frame at the time when the train's attitude change exceeds the set threshold, discard other LiDAR frames between the two key frames, and use INS pre-integration to perform point cloud distortion correction; extract edge features and planar features from the distorted point cloud, construct a local map using a sliding window, and use a feature-based point cloud matching algorithm to calculate the relative pose transformation and construct the LiDAR odometry factor.

[0065] The third building module is used to provide a global coordinate reference for the system when trackside markers are detected, eliminate accumulated errors, and build a TLC factor based on the absolute position constraints of the train.

[0066] The estimation module adds the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor graph, calls iSAM2 for optimization based on maximum a posteriori estimation, and outputs the train pose estimation results in the ENU coordinate system.

[0067] Thirdly, the present invention provides a non-transitory computer-readable storage medium for storing computer instructions, which, when executed by a processor, implement the multi-sensor fusion train positioning method based on factor graph optimization as described in the first aspect.

[0068] Fourthly, the present invention provides a computer device including a memory and a processor, wherein the processor and the memory communicate with each other, the memory stores program instructions executable by the processor, and the processor calls the program instructions to execute the multi-sensor fusion train positioning method based on factor graph optimization as described in the first aspect.

[0069] Fifthly, the present invention provides an electronic device, comprising: a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to cause the electronic device to execute instructions for implementing the multi-sensor fusion train positioning method based on factor graph optimization as described in the first aspect.

[0070] The beneficial effects of this invention are: by integrating information from LiDAR, INS, TLC and other sources to construct a factor graph model, and utilizing trackside features such as transponders, kilometer markers, hexagonal markers, and pre-set signs, high-precision positioning of trains can be achieved in environments where satellite navigation signals are limited or denied.

[0071] The advantages of additional aspects of the invention will be set forth more clearly in the following description or will be learned by practice of the invention. Attached Figure Description

[0072] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the description of the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0073] Figure 1 This is an overall flowchart of the train positioning accuracy improvement method based on factor graph optimization and multi-sensor fusion as described in an embodiment of the present invention.

[0074] Figure 2 This is a flowchart of the multi-sensor fusion-based train positioning method based on factor graph optimization according to an embodiment of the present invention. Detailed Implementation

[0075] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention.

[0076] It will be understood by those skilled in the art that, unless otherwise defined, all terms used herein (including technical and scientific terms) have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains.

[0077] It should also be understood that terms such as those defined in general dictionaries should be understood to have meanings consistent with their meanings in the context of the prior art, and should not be interpreted in an idealized or overly formal sense unless defined as here.

[0078] Those skilled in the art will understand that, unless specifically stated otherwise, the singular forms “a,” “an,” “the,” and “the” used herein may also include the plural forms. It should be further understood that the term “comprising” as used in this specification means the presence of the stated features, integers, steps, operations, elements, and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, and / or groups thereof.

[0079] In the description of this specification, references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of those different embodiments or examples.

[0080] To facilitate understanding of the present invention, the present invention will be further explained and described below with reference to the accompanying drawings and specific embodiments. However, the specific embodiments do not constitute a limitation on the embodiments of the present invention.

[0081] Those skilled in the art should understand that the accompanying drawings are merely schematic diagrams of embodiments, and the components in the drawings are not necessarily essential for implementing the present invention.

[0082] This invention patent proposes a multi-source fusion positioning system that considers factor graph optimization. By combining inputs such as LiDAR (Light Detection and Ranging), Inertial Navigation System (INS), and Train Location Coordinate (TLC), the system improves the continuity, robustness, and accuracy of positioning. Factor graph optimization (FGO), due to its sparsity, robustness, and scalability, has gradually become a popular research direction in multi-sensor fusion positioning.

[0083] Example 1

[0084] like Figure 1 As shown in this embodiment 1, a multi-source fusion positioning accuracy improvement method based on factor graph optimization is provided. By fusing information from LiDAR, INS, TLC and other sources to construct a factor graph model, and utilizing trackside features such as transponders, kilometer markers, hexometer markers and preset signs, high-precision positioning of trains can be achieved in environments where satellite navigation signals are limited or denied.

[0085] The multi-source fusion positioning accuracy improvement method based on factor graph optimization described in this embodiment includes the following steps:

[0086] S1. Construct 16-dimensional state variables for the train's position, speed, attitude, quaternions, accelerometer, and gyroscope data to be estimated. , as input variables for factor graph optimization, where k is the corresponding observation epoch, b a For zero bias of the accelerometer, b g Zero bias for the gyroscope;

[0087] S2. When INS data is obtained at observation epoch k and epoch k+1, INS pre-integration calculation is performed to construct INS pre-integration factors;

[0088] S3. When the train's attitude change exceeds the set threshold, select the lidar frame at this time as the key frame, discard the other lidar frames in the two key frames, and use INS pre-integration to perform point cloud distortion removal.

[0089] S4. Extract edge features and planar features from the distorted point cloud, construct a local map using a sliding window, and calculate the relative pose transformation using a feature-based point cloud matching algorithm to construct the LiDAR odometry factor;

[0090] S5. When a trackside marker is detected, a global coordinate reference is provided to the system to eliminate accumulated errors, and a TLC factor is constructed based on the absolute position constraint of the train;

[0091] S6. Add the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor graph, call iSAM2 for optimization based on maximum a posteriori estimation, and output the train pose estimation results in the ENU coordinate system.

[0092] In step S1, based on the analysis of the multi-sensor fusion positioning, combined structure and coordinate system of INS / LiDAR / TLC, system state variables are selected as factor graph optimization variables.

[0093] For a multi-source fusion positioning system with factor graph optimization for INS / LiDAR / TLC, the East-North-Up (ENU) coordinate system is selected, and the 16-dimensional state variables are constructed as follows:

[0094]

[0095] In the formula, This represents the state variable at the k-th epoch, which includes the 3D position of the vehicle in the ENU coordinate system. 3D velocity The attitude of the carrier system in quaternion form relative to the ENU coordinate system Zero bias of accelerometers and gyroscopes in the load system , .

[0096] In step S2, based on INS measurements, the relative motion of the train between adjacent time points is calculated through pre-integration, including changes in velocity, position, and rotation. The pre-integration formulas for velocity, position, and rotation are as follows:

[0097]

[0098]

[0099]

[0100] In the formula, , , These represent the rotation, velocity, and position change matrices between times i and j, respectively. Indicates a time interval; This represents the angular velocity after zero bias compensation; Indicates time up to the current moment The cumulative rotation matrix; This represents the acceleration after zero bias compensation; Indicates time The cumulative velocity increment up to the current time t.

[0101] Based on this, an error function for the INS pre-integral factor is constructed. as follows:

[0102]

[0103] In the formula, , , Representing time respectively and The attitude matrix, velocity, and position; Represents gravitational acceleration; This represents the logarithmic mapping from a Lie group to a Lie algebra.

[0104] In step S3, point cloud distortion correction is performed based on the acquired LiDAR keyframes and INS data.

[0105] Assuming the LiDAR scan period is Extract the pre-integrated pose sequence within that period. For the first... Each keyframe, its acquisition time Represented as:

[0106]

[0107] Interpolation is obtained from the INS pre-integral sequence. Position at any given moment:

[0108]

[0109] LiDAR points from Time coordinate system corrected to Time coordinate system:

[0110]

[0111] In the formula, , Represents the rotation and translation matrices from LiDAR to INS; express The inverse rotation matrix of the pose at any given time; express The position in the world coordinate system calculated by INS at that moment.

[0112] Based on the above technical solution, in step S4, for point cloud data, after distortion correction by INS, edge features (high curvature region) and planar features (low curvature region) are selected by calculating the curvature of continuous points, and the feature points of the current frame are matched with the previous frame point cloud.

[0113] For each edge point of the current frame Find the two nearest neighbor edge points in the local map. Calculate the residual from the point to the line:

[0114]

[0115] In the formula, This represents the vertical distance from the point to the edge line.

[0116] For each planar point in the current frame Find the three nearest neighbor points on the local map. Calculate the residual from the point to the surface:

[0117]

[0118]

[0119] In the formula, This represents the perpendicular distance from a point to a plane. This represents the normal vector of the plane.

[0120] Based on this, an error function for the LiDAR odometry factor is constructed. as follows:

[0121]

[0122] In the formula, , These represent the distance residuals from edge feature points to the matching edge lines and the distance residuals from planar feature points to the matching plane, respectively.

[0123] In step S5, when the lidar is at time... When trackside markers are detected, the cumulative error is corrected using the absolute position information provided by the high-precision map. The observation equations are constructed based on the observation geometry as follows:

[0124]

[0125] In the formula, This indicates the location of the sign based on the current position of the train. express The calculated position of the time-tracking train in the world coordinate system; Represents the rotation matrix of the carrier system relative to the ENU coordinate system; and These are the external parameter rotation matrix and translation vector of the lidar coordinate system relative to the carrier system, respectively; This is the three-dimensional coordinate vector of the center point of the sign in the radar coordinate system as measured by the lidar. To observe noise.

[0126] Based on this, the residual function of the TLC factor is constructed. as follows:

[0127]

[0128] In the formula, This refers to the absolute location information of the signboard recorded in the database.

[0129] In step S6, based on the maximum a posteriori probability estimation, the system state of the train is determined. Represented as:

[0130]

[0131] In the formula, Indicates from timestamp 0 to The measurements from all sensors; Indicates from timestamp 0 to All prior information.

[0132] Assuming the noise follows a Gaussian distribution, then according to Bayes' theorem, we can... Represented as:

[0133]

[0134] In the formula, Indicates time No. The observation error of each sensor; This represents the covariance matrix corresponding to the observation error. The Mahalanobis distance is defined as follows:

[0135]

[0136] Therefore, the maximum a posteriori estimation problem can be transformed into a nonlinear least squares solution:

[0137]

[0138] Based on this, the global optimization objective function of the multi-source fusion positioning system can be obtained:

[0139]

[0140] Based on the iSAM2 library of GTSAM, the corresponding factors and state variables are added to the factor graph each time new sensor data is added. Incremental smoothing is used to achieve efficient multi-sensor fusion, suppress cumulative error, and output the optimized train pose estimation result.

[0141] Example 2

[0142] In this second embodiment, a multi-sensor fusion train positioning system based on factor graph optimization is first provided, including: a first construction module, used to construct 16-dimensional state variables for the train position, speed, attitude, quaternions, accelerometer, and gyroscope to be estimated, as input variables for factor graph optimization; a second construction module, used to perform INS pre-integration calculation and construct INS pre-integration factors when INS data is acquired in the current observation epoch and the next observation epoch; and an extraction module, used to select the lidar frame at this time as the key frame when the train's attitude change exceeds a set threshold, discard other lidar frames in the two key frames, and utilize INS pre-integration... The system performs point cloud distortion correction; extracts edge and planar features from the distorted point cloud, constructs a local map using a sliding window, and calculates the relative pose transformation using a feature-based point cloud matching algorithm to construct the LiDAR odometry factor; the third construction module provides a global coordinate reference for the system when trackside markers are detected, eliminates accumulated errors, and constructs the TLC factor based on the train's absolute position constraints; the estimation module adds the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor map, calls iSAM2 for optimization based on maximum a posteriori estimation, and outputs the train pose estimation result in the ENU coordinate system.

[0143] like Figure 2 As shown, in this embodiment, the above-described system is used to implement a multi-sensor fusion train localization method based on factor graph optimization. This includes: constructing 16-dimensional state variables for the train's position, speed, attitude, quaternions, accelerometer, and gyroscope data to be estimated, which serve as input variables for factor graph optimization; performing INS pre-integration calculation to construct INS pre-integration factors when INS data is acquired in the current and next observation epochs; and selecting the current lidar frame as a keyframe when the train's attitude change exceeds a set threshold, discarding other lidar frames from the two keyframes, and using INS pre-integration for further optimization. The system performs point cloud distortion correction; extracts edge and planar features from the distorted point cloud, constructs a local map using a sliding window, and calculates the relative pose transformation using a feature-based point cloud matching algorithm to construct the LiDAR odometry factor; when trackside markers are detected, it provides a global coordinate reference for the system to eliminate accumulated errors, and constructs the TLC factor based on the absolute position constraint of the train; adds the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor graph, calls iSAM2 for optimization based on maximum a posteriori estimation, and outputs the train pose estimation result in the ENU coordinate system.

[0144] For a multi-source fusion localization system optimized by factor graphs of INS / LiDAR / TLC, the northeast-sky coordinate system is selected as the reference coordinate system, and the 16-dimensional state variables are constructed as follows:

[0145]

[0146] In the formula, This represents the state variable at the k-th epoch, which includes the 3D position of the vehicle in the ENU coordinate system. 3D velocity The attitude of the carrier system in quaternion form relative to the ENU coordinate system Zero bias of accelerometers and gyroscopes in the load system , .

[0147] Based on INS measurements, the relative motion of the train between adjacent time points is calculated through pre-integration, including changes in velocity, position, and rotation; the pre-integration formulas for velocity, position, and rotation are as follows:

[0148] ;

[0149] ;

[0150] ;

[0151] In the formula, , , These represent the rotation, velocity, and position change matrices between times i and j, respectively. Indicates a time interval; This represents the angular velocity after zero bias compensation; Indicates time up to the current moment The cumulative rotation matrix; This represents the acceleration after zero bias compensation; Indicates time The cumulative velocity increment up to the current time t;

[0152] Constructing the error function of the INS pre-integral factor as follows:

[0153] ;

[0154] In the formula, , , Representing time respectively and The attitude matrix, velocity, and position; Represents gravitational acceleration; This represents the logarithmic mapping from a Lie group to a Lie algebra.

[0155] Point cloud distortion correction is performed based on acquired LiDAR keyframes and INS data.

[0156] Assuming the LiDAR scan period is Extract the pre-integrated pose sequence within this period;

[0157] For the Each keyframe, its acquisition time Represented as:

[0158] ;

[0159] Interpolation is obtained from the INS pre-integral sequence. Position at any given moment:

[0160] ;

[0161] LiDAR points from Time coordinate system corrected to Time coordinate system:

[0162] ;

[0163] In the formula, , Represents the rotation and translation matrices from LiDAR to INS; express The inverse rotation matrix of the pose at any given time; express The position in the world coordinate system calculated by INS at that moment.

[0164] For point cloud data, after distortion correction by INS, edge features and planar features are selected by calculating the curvature of continuous points, and the feature points of the current frame are matched with the point cloud of the previous frame.

[0165] For each edge point of the current frame Find the two nearest neighbor edge points in the local map. Calculate the residual from the point to the line:

[0166] ;

[0167] In the formula, This represents the perpendicular distance from the point to the edge line;

[0168] For each planar point in the current frame Find the three nearest neighbor points on the local map. Calculate the residual from the point to the surface:

[0169] ;

[0170] ;

[0171] In the formula, This represents the perpendicular distance from a point to a plane. Represents the normal vector of the plane;

[0172] Error function for constructing LiDAR odometry factors as follows:

[0173] ;

[0174] In the formula, , These represent the distance residuals from edge feature points to the matching edge lines and the distance residuals from planar feature points to the matching plane, respectively.

[0175] When the lidar is at time When trackside markers are detected, the cumulative error is corrected using the absolute position information provided by the high-precision map; the observation equation is constructed based on the observation geometry as follows:

[0176] ;

[0177] In the formula, This indicates the location of the sign based on the current position of the train. express The calculated position of the time-tracking train in the world coordinate system; Represents the rotation matrix of the carrier system relative to the ENU coordinate system; and These are the external parameter rotation matrix and translation vector of the lidar coordinate system relative to the carrier system, respectively; This is the three-dimensional coordinate vector of the center point of the sign in the radar coordinate system as measured by the lidar. To observe noise;

[0178] Residual function for constructing TLC factor as follows:

[0179] ;

[0180] In the formula, This refers to the absolute location information of the signboard recorded in the database.

[0181] Based on maximum a posteriori probability estimation, the system state of the train is... Represented as:

[0182] ;

[0183] In the formula, Indicates from timestamp 0 to The measurements from all sensors; Indicates from timestamp 0 to All prior information;

[0184] Assuming the noise follows a Gaussian distribution, then according to Bayes' theorem, ... Represented as:

[0185] ;

[0186] In the formula, Indicates time No. The observation error of each sensor; This represents the covariance matrix corresponding to the observation error; The Mahalanobis distance is defined as follows:

[0187] ;

[0188] Therefore, the maximum a posteriori estimation problem is transformed into a nonlinear least squares solution:

[0189] ;

[0190] The global optimization objective function of the multi-source fusion positioning system is obtained as follows:

[0191] ;

[0192] Based on the iSAM2 library of GTSAM, the corresponding factors and state variables are added to the factor graph each time new sensor data is added. Incremental smoothing is used to achieve efficient multi-sensor fusion, suppress cumulative error, and output the optimized train pose estimation result.

[0193] Example 3

[0194] This embodiment 3 provides a non-transitory computer-readable storage medium for storing computer instructions. When the computer instructions are executed by the processor, they implement the multi-sensor fusion train localization method based on factor graph optimization as described above. This method includes: constructing 16-dimensional state variables for the train's position, speed, attitude, quaternions, accelerometer readings, and gyroscope readings to be estimated, as input variables for factor graph optimization; performing INS pre-integration calculations to construct INS pre-integration factors when INS data is acquired in the current and next observation epochs; and selecting the current lidar frame as the keyframe and discarding it when the train's attitude change exceeds a set threshold. Other LiDAR frames in the two keyframes are used, and INS pre-integration is used for point cloud distortion correction. Edge and planar features are extracted from the distorted point cloud, a local map is constructed using a sliding window, and a feature-based point cloud matching algorithm is used to calculate the relative pose transformation and construct the LiDAR odometry factor. When a trackside marker is detected, a global coordinate reference is provided to the system to eliminate accumulated errors, and a TLC factor is constructed based on the absolute position constraint of the train. The INS pre-integration factor, LiDAR odometry factor, and TLC factor are added to the factor map, and iSAM2 is called for optimization based on maximum a posteriori estimation to output the train pose estimation result in the ENU coordinate system.

[0195] Example 4

[0196] This embodiment 4 provides a computer device, including a memory and a processor. The processor and the memory communicate with each other. The memory stores program instructions that can be executed by the processor. The processor calls the program instructions to execute the multi-sensor fusion train localization method based on factor graph optimization as described above. The method includes: constructing 16-dimensional state variables for the train position, speed, attitude, quaternions, accelerometer, and gyroscope to be estimated, as input variables for factor graph optimization; performing INS pre-integration calculation to construct INS pre-integration factors when INS data is acquired in the current observation epoch and the next observation epoch; and selecting the laser radar at this time when the train's attitude change exceeds a set threshold. The first keyframe is the LiDAR frame. Other LiDAR frames in the two keyframes are discarded, and INS pre-integration is used to distort the point cloud. Edge features and planar features are extracted from the distorted point cloud. A local map is constructed using a sliding window, and a feature-based point cloud matching algorithm is used to calculate the relative pose transformation and construct the LiDAR odometry factor. When a trackside marker is detected, a global coordinate reference is provided to the system to eliminate accumulated errors. Based on the absolute position constraint of the train, a TLC factor is constructed. The INS pre-integration factor, LiDAR odometry factor, and TLC factor are added to the factor graph. Based on maximum a posteriori estimation, iSAM2 is called for optimization, and the train pose estimation result in the ENU coordinate system is output.

[0197] Example 5

[0198] This embodiment 5 provides an electronic device, including: a processor, a memory, and a computer program; wherein, the processor is connected to the memory, and the computer program is stored in the memory. When the electronic device is running, the processor executes the computer program stored in the memory to enable the electronic device to execute instructions to implement the multi-sensor fusion train positioning method based on factor graph optimization as described above. The method includes: constructing 16-dimensional state variables for the train position, speed, attitude, quaternions, accelerometer, and gyroscope to be estimated, as input variables for factor graph optimization; when INS data is acquired in the current observation epoch and the next observation epoch, performing INS pre-integration calculation to construct INS pre-integration factors; when the train's attitude change exceeds a set... The threshold is set, and the LiDAR frame at this time is selected as the key frame. Other LiDAR frames in the two key frames are discarded, and INS pre-integration is used to dedorthogonalize the point cloud. Edge features and planar features are extracted from the dedorthogonal point cloud, a local map is constructed using a sliding window, and a feature-based point cloud matching algorithm is used to calculate the relative pose transformation and construct the LiDAR odometry factor. When a trackside marker is detected, a global coordinate reference is provided to the system to eliminate accumulated errors, and a TLC factor is constructed based on the absolute position constraint of the train. The INS pre-integration factor, LiDAR odometry factor, and TLC factor are added to the factor graph, and iSAM2 is called for optimization based on maximum a posteriori estimation to output the train pose estimation result in the ENU coordinate system.

[0199] Those skilled in the art will understand that embodiments of the present invention can be provided as methods, systems, or computer program products. Therefore, the present invention can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, the present invention can take the form of a computer program product embodied on one or more computer-usable storage media (including, but not limited to, disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0200] This invention is described with reference to flowchart illustrations and / or block diagrams of methods, apparatus (systems), and computer program products according to embodiments of the invention. It will be understood that each block of the flowchart illustrations and / or block diagrams, and combinations of blocks in the flowchart illustrations and / or block diagrams, can be implemented by computer program instructions. These computer program instructions can be provided to a processor of a general-purpose computer, special-purpose computer, embedded processor, or other programmable data processing apparatus to produce a machine, such that the instructions, which execute via the processor of the computer or other programmable data processing apparatus, generate instructions for implementing the flowchart illustrations and / or block diagrams. Figure 1 One or more processes and / or boxes Figure 1 A device that provides the functions specified in one or more boxes.

[0201] These computer program instructions may also be stored in a computer-readable storage medium that can direct a computer or other programmable data processing device to function in a particular manner, such that the instructions stored in the computer-readable storage medium produce an article of manufacture including instruction means, which are implemented in a process Figure 1 One or more processes and / or boxes Figure 1 The function specified in one or more boxes.

[0202] These computer program instructions may also be loaded onto a computer or other programmable data processing equipment, whereby a series of operational steps are performed to produce a computer-implemented process, thereby providing instructions that execute on the computer or other programmable equipment for implementing the process. Figure 1 One or more processes and / or boxes Figure 1 The steps of the function specified in one or more boxes.

[0203] While the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, this is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that, based on the technical solutions disclosed in the present invention, various modifications or variations that can be made by those skilled in the art without creative effort should be included within the scope of protection of the present invention.

Claims

1. A multi-sensor fusion train positioning method based on factor graph optimization, characterized in that, include: A 16-dimensional state variable is constructed for the train's position, speed, attitude, quaternion, accelerometer, and gyroscope to be estimated, and used as the input variable for factor graph optimization. When INS data is acquired at the current observation epoch and the next observation epoch, INS pre-integration calculation is performed to construct the INS pre-integration factor; When the train's attitude change exceeds the set threshold, the lidar frame at this time is selected as the key frame, the other lidar frames in the two key frames are discarded, and point cloud distortion is performed using INS pre-integration; edge features and planar features are extracted from the distorted point cloud, a local map is constructed using a sliding window, and a feature-based point cloud matching algorithm is used to calculate the relative pose change and construct the LiDAR odometry factor. When a trackside marker is detected, a global coordinate reference is provided to the system to eliminate accumulated errors, and a TLC factor is constructed based on the absolute position constraints of the train. Add the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor graph, and use iSAM2 to optimize based on maximum a posteriori estimation, outputting the train pose estimation results in the ENU coordinate system.

2. The multi-sensor fusion train positioning method based on factor graph optimization according to claim 1, characterized in that, For a multi-source fusion localization system optimized by factor graphs of INS / LiDAR / TLC, the northeast-sky coordinate system is selected as the reference coordinate system, and the 16-dimensional state variables are constructed as follows: ； In the formula, This represents the state variable at the k-th epoch, which includes the 3D position of the vehicle in the ENU coordinate system. 3D velocity The attitude of the carrier system in quaternion form relative to the ENU coordinate system Zero bias of accelerometers and gyroscopes in the load system , .

3. The multi-sensor fusion train positioning method based on factor graph optimization according to claim 1, characterized in that, Based on INS measurements, the relative motion of the train between adjacent time points is calculated through pre-integration, including changes in speed, position, and rotation. Its velocity, position, and rotation pre-integral formulas are as follows: ； ； ； In the formula, , , These represent the rotation, velocity, and position change matrices between times i and j, respectively. Indicates a time interval; This represents the angular velocity after zero bias compensation; Indicates time up to the current moment The cumulative rotation matrix; This represents the acceleration after zero bias compensation; Indicates time The cumulative velocity increment up to the current time t; Constructing the error function of the INS pre-integral factor as follows: ； In the formula, , , Representing time respectively and The attitude matrix, velocity, and position; Represents gravitational acceleration; This represents the logarithmic mapping from a Lie group to a Lie algebra.

4. The multi-sensor fusion train positioning method based on factor graph optimization according to claim 1, characterized in that, Point cloud distortion correction is performed based on acquired LiDAR keyframes and INS data. Assuming the LiDAR scan period is Extract the pre-integrated pose sequence within this period; For the Each keyframe, its acquisition time Represented as: ； Interpolation is obtained from the INS pre-integral sequence. Position at any given moment: ； LiDAR points from Time coordinate system corrected to Time coordinate system: ； In the formula, , Represents the rotation and translation matrices from LiDAR to INS; express The inverse rotation matrix of the pose at any given time; express The position in the world coordinate system calculated by INS at that moment.

5. The multi-sensor fusion train positioning method based on factor graph optimization according to claim 1, characterized in that, For point cloud data, after distortion correction by INS, edge features and planar features are selected by calculating the curvature of continuous points, and the feature points of the current frame are matched with the point cloud of the previous frame. For each edge point of the current frame Find the two nearest neighbor edge points in the local map. Calculate the residual from the point to the line: ； In the formula, This represents the perpendicular distance from the point to the edge line; For each planar point in the current frame Find the three nearest neighbor points on the local map. Calculate the residual from the point to the surface: ； ； In the formula, This represents the perpendicular distance from a point to a plane. Represents the normal vector of the plane; Error function for constructing LiDAR odometry factors as follows: ； In the formula, , These represent the distance residuals from edge feature points to the matching edge lines and the distance residuals from planar feature points to the matching plane, respectively.

6. The multi-sensor fusion train positioning method based on factor graph optimization according to claim 1, characterized in that, When the lidar is at time When trackside markers are detected, the cumulative error is corrected using the absolute position information provided by the high-precision map; the observation equation is constructed based on the observation geometry as follows: ； In the formula, This indicates the location of the sign based on the current position of the train. express The calculated position of the time-tracking train in the world coordinate system; Represents the rotation matrix of the carrier system relative to the ENU coordinate system; and These are the external parameter rotation matrix and translation vector of the lidar coordinate system relative to the carrier system, respectively; This is the three-dimensional coordinate vector of the center point of the sign in the radar coordinate system as measured by the lidar. To observe noise; Residual function for constructing TLC factor as follows: ； In the formula, This refers to the absolute location information of the signboard recorded in the database.

7. The multi-sensor fusion train positioning method based on factor graph optimization according to claim 1, characterized in that, Based on maximum a posteriori probability estimation, the system state of the train is... Represented as: ； In the formula, Indicates from timestamp 0 to The measurements from all sensors; Indicates from timestamp 0 to All prior information; Assuming the noise follows a Gaussian distribution, then according to Bayes' theorem, ... Represented as: ； In the formula, Indicates time No. The observation error of each sensor; This represents the covariance matrix corresponding to the observation error; The Mahalanobis distance is defined as follows: ； Therefore, the maximum a posteriori estimation problem is transformed into a nonlinear least squares solution: ； The global optimization objective function of the multi-source fusion positioning system is obtained as follows: ； Based on the iSAM2 library of GTSAM, the corresponding factors and state variables are added to the factor graph each time new sensor data is added. Incremental smoothing is used to achieve efficient multi-sensor fusion, suppress cumulative error, and output the optimized train pose estimation result.

8. A multi-sensor fusion train positioning system based on factor graph optimization, characterized in that, include: The first construction module is used to construct 16-dimensional state variables for the train position, speed, attitude, quaternion, accelerometer and gyroscope to be estimated, as input variables for factor graph optimization; The second construction module is used to perform INS pre-integration calculation and construct INS pre-integration factors when INS data is acquired in the current observation epoch and the next observation epoch. The extraction module is used to select the LiDAR frame at the time when the train's attitude change exceeds the set threshold, discard other LiDAR frames between the two key frames, and use INS pre-integration to perform point cloud distortion correction; extract edge features and planar features from the distorted point cloud, construct a local map using a sliding window, and use a feature-based point cloud matching algorithm to calculate the relative pose transformation and construct the LiDAR odometry factor. The third building module is used to provide a global coordinate reference for the system when trackside markers are detected, eliminate accumulated errors, and build a TLC factor based on the absolute position constraints of the train. The estimation module adds the INS pre-integration factor, LiDAR odometry factor, and TLC factor to the factor graph, calls iSAM2 for optimization based on maximum a posteriori estimation, and outputs the train pose estimation results in the ENU coordinate system.

9. A computer device, characterized in that, The system includes a memory and a processor, which communicate with each other. The memory stores program instructions that can be executed by the processor, and the processor calls the program instructions to execute the multi-sensor fusion train positioning method based on factor graph optimization as described in any one of claims 1-6.

10. An electronic device, characterized in that, include: The device includes a processor, a memory, and a computer program; wherein the processor is connected to the memory, the computer program is stored in the memory, and when the electronic device is running, the processor executes the computer program stored in the memory to cause the electronic device to execute instructions for implementing the multi-sensor fusion train positioning method based on factor graph optimization as described in any one of claims 1-6.

Images