Fusion positioning method based on beacon correction

Abstract

The invention discloses a fusion positioning method based on beacon correction, and belongs to the technical field of rail transit fusion positioning, the current pose of a train is predicted by using point cloud data monitored by a laser radar and train state information detected by an IMU, and then the predicted train pose is corrected through a beacon mileage value detected by a beacon. The problem that accurate positioning cannot be realized due to the fact that environmental characteristics are seriously degraded and environmental information around the train is difficult to accurately detect in a long tunnel environment is solved; and according to the time difference between the time stamp of the received beacon and the time stamp of the laser radar monitoring point cloud data, time synchronization is carried out on data collected by various sensors of the train state, so that accumulated errors generated in the use process of the sensors are reduced, and pose errors caused by network transmission delay are reduced. And the positioning precision of the train in the tunnel environment is greatly improved.

Description

Technical Field

[0001] The present invention relates to the technical field of rail transit fusion positioning, and in particular to a fusion positioning method based on beacon correction. Background Art

[0002] In recent years, information technology has developed rapidly, and intelligence has become an important development trend in the transportation industry. As an important part of intelligent technology, train autonomous positioning technology can realize accurate perception and judgment of the train's position, providing strong support for the safe operation and intelligent scheduling of trains. Autonomous positioning technology usually uses GPS, RTK, IMU, lidar, camera and other sensors to achieve high-precision positioning, but GPS and RKT are sensitive to signal interference, so it is difficult to work normally in tunnels. IMU is less affected by the external environment, but it will produce cumulative errors, affecting positioning accuracy. At the same time, the features in the tunnel are severely degraded, and lidar and cameras will also have positioning failure problems, so how to improve the positioning effect has become an urgent problem to be solved.

[0003] Chinese patent, publication number: CN112162305A, publication date: January 1, 2021, discloses a fusion positioning method for rail transit, by combining Beidou data and inertial navigation data to construct a Beidou-inertial navigation data structure, combining inertial navigation data and electronic tag data to construct an inertial navigation-electronic tag data structure, and combining odometer data and electronic tag data to construct an odometer-electronic tag data structure; the current fusion positioning structure is determined by preset positioning priority, Beidou status code, inertial navigation status code, odometer status code and electronic tag status code; the current fusion positioning structure is a Beidou-inertial navigation data structure, a Beidou data structure, an inertial navigation-electronic tag data structure or an odometer-electronic tag data structure; the correction value corresponding to the current fusion positioning structure is used to correct the positioning data corresponding to the current fusion positioning structure to obtain fusion positioning data. Although the external environment impact is reduced to a certain extent, it is still difficult to solve the impact of the accumulated error generated during the use of the inertial measurement unit on the positioning result. Summary of the invention

[0004] In order to solve the problem of poor positioning effect caused by degradation of environmental characteristics of trains in tunnel environments and accumulated errors generated during the use of sensors, the present invention provides a fusion positioning method based on beacon correction. The method predicts the current posture of the train by using point cloud data monitored by lidar and train status information detected by IMU, and then corrects the predicted train posture by using the beacon mileage value detected by beacon. This not only solves the problem of inaccurate positioning caused by serious degradation of environmental characteristics and difficulty in accurately detecting environmental information around the train in a long tunnel environment, but also synchronizes the data collected by various sensors of the train status according to the time difference between the timestamp of the beacon received and the timestamp of the point cloud data monitored by lidar, thereby reducing the accumulated errors generated during the use of the sensors, reducing the posture errors caused by network transmission delays, and greatly improving the positioning accuracy of the train in the tunnel environment.

[0005] In a first aspect, a technical solution provided in an embodiment of the present invention is: a fusion positioning method based on beacon correction, comprising the following steps: S1. Collecting the position information of the train in the point cloud map to obtain a first point cloud data set; S2, performing a dedistortion process on the first point cloud data set to obtain a second point cloud data set, and performing environmental feature extraction on the second point cloud data set to obtain environmental feature points; S3, obtaining the initial posture of the train based on the train state information, and obtaining feature points associated with the environmental feature points using the two-dimensional projection area of ​​the initial posture as a matching constraint condition to obtain a relative posture; S4, the posture correction algorithm corrects the relative posture in response to the second point cloud data set and the timestamp of the current beacon mileage value to obtain an estimated posture of the train; S5. The fusion positioning algorithm obtains an optimized train posture estimate in response to the estimated posture and train status information, and obtains the current posture of the train based on the optimized train posture estimate.

[0006] In this solution, the point cloud map formed by the data detected by the laser radar is used as the initial data for the train posture calculation, thereby solving the problem that various visual sensors of the train in the tunnel environment are difficult to accurately identify the surrounding environment of the train in a dark environment, improving the environmental perception ability, and providing a data basis for posture calculation. By extracting feature points from the point cloud map and using a tree data structure for data storage, the problems of excessive point cloud data volume, slow calculation speed, and difficulty in meeting the real-time requirements of the train posture are solved; the current posture of the train is predicted by using the point cloud data monitored by the laser radar and the train status information detected by the IMU, and the predicted train posture is corrected by the beacon mileage value detected by the beacon. In the long tunnel environment, the accumulated errors generated by various sensors in the continuous working process are reduced, which not only reduces the posture error caused by network transmission delay, but also greatly improves the positioning accuracy of the train in the tunnel environment.

[0007] Preferably, in S1, collecting the position information of the train in the point cloud map to obtain a first point cloud data set includes the following steps: The three-dimensional coordinate points of the train in a frame of point cloud at the current moment are collected in the point cloud map to obtain a first point cloud data set, and the three-dimensional coordinate points in the first point cloud data set are arranged and saved according to the size of the timestamp using a tree data structure.

[0008] In this solution, the kd-tree structure in the tree data structure can be used to save the three-dimensional coordinate points in the first point cloud data set. Kd-tree is a data structure specifically used to organize point sets in k-dimensional space. It constructs a tree structure by recursively dividing the point set into two subsets. In point cloud data, each point contains three-dimensional coordinates, so kd-tree is very suitable for organizing and managing point cloud data. By using the kd-tree structure to organize point cloud data, the efficiency of data retrieval and processing can be optimized. When performing point cloud alignment, filtering, segmentation and other operations, kd-tree can be used to quickly locate relevant point cloud data, reduce the amount of calculation, and provide a structural basis for subsequent feature point matching.

[0009] Preferably, performing dedistortion processing on the first point cloud data set to obtain the second point cloud data set comprises the following steps: Searching for the maximum timestamp and the minimum timestamp of the three-dimensional coordinate points in the first point cloud data set, and converting the first point cloud data set from the laser coordinate system to the IMU coordinate system based on the external parameters of the IMU and the laser radar; The IMU synchronization data set corresponding to the maximum timestamp and the minimum timestamp in the IMU data acquisition process is selected, and the linear interpolation algorithm is used to calculate the compensation transformation matrix of the IMU synchronization data set relative to the minimum timestamp. Based on the compensation transformation matrix, each three-dimensional coordinate point in the first point cloud data set is unified to the moment corresponding to the minimum timestamp to obtain the second point cloud data set.

[0010] In this solution, distortion or deformation of point cloud data caused by movement can be corrected through de-distortion processing, so that the point cloud data can more accurately reflect the actual scene. By converting the point cloud data from the laser coordinate system to the IMU coordinate system and using the IMU data for compensation transformation, the error caused by sensor movement can be eliminated and the accuracy of the point cloud data can be improved. In the processing process, by selecting the IMU synchronization data set containing the maximum timestamp and the minimum timestamp, and using the linear interpolation algorithm to calculate the compensation transformation matrix, all three-dimensional coordinate points in the first point cloud data set can be unified to the moment corresponding to the minimum timestamp, which helps to maintain the temporal consistency of the point cloud data and provides a data basis for subsequent fusion positioning.

[0011] Preferably, based on the external parameters of the IMU and the laser radar, the first point cloud data set is converted from the laser coordinate system to the IMU coordinate system, comprising the following steps: Based on the external parameters of the IMU and the laser radar, including the rotation matrix and the translation vector, a coordinate transformation formula is used to transform the three-dimensional coordinate points in the first point cloud data set from the laser coordinate system to the IMU coordinate system.

[0012] In this scheme, through coordinate transformation, the data from different sensors (lidar and IMU) are unified into the same coordinate system, namely the IMU coordinate system, which eliminates the data inconsistency problem caused by different coordinate systems and facilitates the subsequent data fusion and processing. When the data of lidar and IMU are in a unified coordinate system, the data of the two sensors can be more accurately fused. This fusion can make full use of their respective advantages, such as the high-precision distance measurement of lidar and the high-frequency attitude information of IMU, thereby improving the accuracy and reliability of the overall data fusion.

[0013] Preferably, in S2, extracting environmental features from the second point cloud data set to obtain environmental feature points comprises the following steps: Surface features and edge features are extracted from the three-dimensional coordinate points in the second point cloud data set to obtain surface feature points and edge feature points, and the surface feature points and edge feature points are used as environmental feature points.

[0014] In this solution, by extracting and screening feature points of point cloud data, the analysis of less useful three-dimensional coordinate points can be reduced, thereby concentrating computing power and improving computing efficiency without affecting computing accuracy.

[0015] Preferably, in S3, obtaining the initial position and posture of the train based on the train state information includes the following steps: The linear acceleration, angular velocity and corresponding timestamps of two adjacent frames of the train are collected by IMU. The timestamps of two adjacent frames get the collection time interval. Based on the collection time interval, the angular velocity and linear acceleration of the train are integrated respectively and combined with the IMU noise to get the initial posture of the train.

[0016] In this scheme, a preliminary estimate of the train's posture can be made through the train status information collected by the IMU, which provides a direction for further improving the accuracy of the posture, thereby reducing the calculation steps and improving the calculation efficiency of the posture.

[0017] Preferably, in S3, taking the two-dimensional projection area of ​​the initial posture as a matching constraint condition to obtain feature points associated with the environmental feature points to obtain a relative posture, the following steps are included: Projecting the three-dimensional coordinate points in the second point cloud data set into a map coordinate system based on the initial posture to obtain a map data set, wherein the map coordinate system is a two-dimensional coordinate system; Match the m matching feature points closest to the surface feature points and edge feature points in the map data set, fit the matching feature points into a feature plane, and calculate the distance between the three-dimensional coordinate points in the second point cloud data set and the feature plane. When the deviation between the distance and the distance between the actual position coordinates of the train and the feature plane is the smallest, stop matching and obtain the relative posture.

[0018] In this scheme, by projecting the three-dimensional point cloud into a two-dimensional map coordinate system, the dimension of data processing can be significantly reduced, thereby reducing the amount of calculation and processing time; by fitting the feature plane and calculating the distance from point to plane instead of calculating the distance from point to point, the computational complexity can be further reduced, because plane fitting and point-to-plane distance calculation are usually more efficient than point-to-point distance calculation; this method is not only applicable to flat ground environments, but also because surface feature points and edge feature points can capture the complexity and diversity of the environment, so it can handle environments with complex terrain and structures, thereby improving environmental adaptability; by matching these feature points in the map data set, it can adapt to various types of environmental features, thereby achieving wider applicability.

[0019] Preferably, in S4, the posture correction algorithm corrects the relative posture in response to the second point cloud data set and the timestamp of the current beacon mileage value to obtain the estimated posture of the train, comprising the following steps: Collect the current beacon mileage value when the train passes the beacon, and obtain the beacon time interval based on the timestamp of the train passing the beacon in the current beacon mileage value and the timestamp of the second point cloud data set; The train angular velocity and linear acceleration are integrated at the beacon time interval to obtain the posture correction value, and the relative posture is corrected based on the posture correction value to obtain the estimated posture of the train.

[0020] In this scheme, the train position is continuously corrected by considering the data detected by multiple sensors, thereby improving the train positioning accuracy.

[0021] Preferably, in S5, the fusion positioning algorithm obtains the train posture optimization estimate in response to the estimated posture and the train state information, and obtains the current posture of the train based on the train posture optimization estimate, including the following steps: The train speed is obtained based on the train state information, the train speed is decomposed in three dimensions to obtain an estimated state quantity, and the estimated state quantity is used as the state quantity of the unscented Kalman filter to perform state estimation on the estimated posture of the train to obtain an optimized train posture estimate; Based on the train posture optimization estimate, the estimated posture is optimized to obtain the current posture of the train.

[0022] In this solution, the IMU, lidar, beacon and vehicle speed data are fused and the various sensors are compared and corrected with each other to reduce the cumulative error caused by the positioning of a single sensor. The positioning advantages of multiple sensors are combined to improve the positioning accuracy and environmental adaptability of the train.

[0023] Preferably, the estimated state quantity is an estimated quaternion of the train speed at a corresponding moment.

[0024] In this solution, by corresponding the quaternion with the time corresponding to the train speed, the rotation and translation of the train can be displayed concisely and clearly. Compared with the traditional Euler angle representation, it also avoids the universal lock problem, thereby displaying the status of the train more accurately.

[0025] Beneficial effects of the present invention: (1) The present invention uses the point cloud map formed by the data detected by the laser radar as the initial data for train posture calculation, thereby solving the problem that various visual sensors of the train in the tunnel environment are difficult to accurately identify the surrounding environment of the train in a dark environment, improving the environmental perception ability, and providing a data basis for posture calculation. By extracting feature points from the point cloud map and using a tree data structure for data storage, the problem of too large a point cloud data volume, too slow a calculation speed, and difficulty in meeting the real-time requirements of the train posture is solved; (2) The current posture of the train is predicted by combining the point cloud data monitored by the lidar and the train status information detected by the IMU, and the predicted train posture is corrected by the beacon mileage value detected by the beacon. The train posture offset is obtained based on the time difference between the timestamp of the beacon received and the timestamp of the point cloud data monitored by the lidar. In a long tunnel environment, the accumulated errors generated during the continuous operation of various sensors are reduced, which not only reduces the posture error caused by network transmission delay, but also greatly improves the positioning accuracy of the train in the tunnel environment.

[0026] The above invention content is only an overview of the technical solution of the present invention. In order to more clearly understand the technical means of the present invention, it can be implemented according to the contents of the specification. In order to make the above and other purposes, features and advantages of the present invention more obvious and easy to understand, the specific implementation methods of the present invention are listed below. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Other features, objects and advantages of the present invention will become more apparent by reading the detailed description of non-limiting embodiments made with reference to the following drawings. The drawings are only for the purpose of illustrating preferred embodiments and are not to be considered as limiting the present invention. Also, the same reference symbols are used throughout the drawings to represent the same parts.

[0028] Figure 1 The present invention is a flow chart of a fusion positioning method based on beacon correction. DETAILED DESCRIPTION

[0029] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific implementation method described herein is only an optimal embodiment of the present invention, which is only used to explain the present invention and does not limit the scope of protection of the present invention. All other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0030] Before discussing the exemplary embodiments in more detail, it should be mentioned that some exemplary embodiments are described as processes or methods depicted as flow charts. Although the flow charts describe the operations (or steps) as sequential processes, many of the operations (or steps) therein can be implemented in parallel, concurrently, or simultaneously. In addition, the order of the operations can be rearranged. The process can be terminated when its operation is completed, but can also have additional steps not included in the drawings; the process can correspond to a method, function, procedure, subroutine, subprogram, etc.

[0031] Example: Figure 1As shown, in order to solve the problem of poor positioning effect caused by environmental feature degradation of trains in tunnel environments and accumulated errors generated during the use of sensors, this embodiment provides a fusion positioning method based on beacon correction, including the following steps: S1: Collect the position information of the train in the point cloud map to obtain a first point cloud data set.

[0032] In this embodiment, collecting the position information of the train in the point cloud map to obtain a first point cloud data set includes the following steps: The three-dimensional coordinate points of the train in a frame of point cloud at the current moment are collected in the point cloud map to obtain a first point cloud data set, and the three-dimensional coordinate points in the first point cloud data set are arranged and saved according to the size of the timestamp using a tree data structure.

[0033] The kd-tree of this embodiment is a data structure specifically used to organize point sets in k-dimensional space. It constructs a tree structure by recursively dividing the point set into two subsets. In point cloud data, each point contains three-dimensional coordinates, so kd-tree is very suitable for organizing and managing point cloud data. By using the kd-tree structure to organize point cloud data, the efficiency of data retrieval and processing can be optimized. When performing operations such as point cloud alignment, filtering, and segmentation, the kd-tree can be used to quickly locate relevant point cloud data, reduce the amount of calculation, and provide a structural basis for subsequent feature point matching.

[0034] S2: performing a dedistortion process on the first point cloud data set to obtain a second point cloud data set, and performing environmental feature extraction on the second point cloud data set to obtain environmental feature points.

[0035] In this embodiment, performing dedistortion processing on the first point cloud data set to obtain the second point cloud data set includes the following steps: Searching for the maximum timestamp and the minimum timestamp of the three-dimensional coordinate points in the first point cloud data set, and converting the first point cloud data set from the laser coordinate system to the IMU coordinate system based on the external parameters of the IMU and the laser radar; The IMU synchronization data set corresponding to the maximum timestamp and the minimum timestamp in the IMU data acquisition process is selected, and the linear interpolation algorithm is used to calculate the compensation transformation matrix of the IMU synchronization data set relative to the minimum timestamp. Based on the compensation transformation matrix, each three-dimensional coordinate point in the first point cloud data set is unified to the moment corresponding to the minimum timestamp to obtain the second point cloud data set.

[0036] Specifically, the specific calculation process of performing dedistortion processing on the first point cloud data set in this embodiment is as follows: Step 1: Set the first point cloud data set points of the current frame mapEach point in the time stamp is t_points n (n=0, 1, 2, ..., k), where n is the nth point, k is the number of points in the current point cloud frame, and t_points is calculated n The maximum and minimum timestamps in the point cloud are recorded as t_start and t_end, respectively, and the corresponding points in the point cloud are recorded as

[0037] Step 2: Use the external parameters of IMU and LiDAR to transfer the point cloud from the laser coordinate system to the IMU coordinate system, recorded as

[0038] Step 3: Select the IMU synchronization data set corresponding to the maximum timestamp t_start and the minimum timestamp t_end in the IMU data acquisition process, and use the linear interpolation algorithm to calculate the compensation transformation matrix of the IMU synchronization data set relative to the minimum timestamp t_end. Based on the compensation transformation matrix, the first point cloud data set points map Each three-dimensional coordinate point in is unified to the time t corresponding to the minimum timestamp t_end end Get the second point cloud data set

[0039] This embodiment can correct the distortion or deformation of point cloud data caused by movement through de-distortion processing, so that the point cloud data can more accurately reflect the actual scene. By converting the point cloud data from the laser coordinate system to the IMU coordinate system and using the IMU data for compensation transformation, the error caused by sensor movement can be eliminated and the accuracy of the point cloud data can be improved. In the processing process, by selecting an IMU synchronization data set containing a maximum timestamp and a minimum timestamp, and using a linear interpolation algorithm to calculate the compensation transformation matrix, all three-dimensional coordinate points in the first point cloud data set can be unified to the moment corresponding to the minimum timestamp, which helps to maintain the temporal consistency of the point cloud data and provides a data basis for subsequent fusion positioning.

[0040] In this embodiment, based on the external parameters of the IMU and the laser radar, the first point cloud data set is converted from the laser coordinate system to the IMU coordinate system, including the following steps: Based on the external parameters of the IMU and the laser radar, including the rotation matrix and the translation vector, a coordinate transformation formula is used to transform the three-dimensional coordinate points in the first point cloud data set from the laser coordinate system to the IMU coordinate system.

[0041] Specifically, the coordinate transformation formula is as follows: Here, R is the 3x3 rotation matrix and T is the 3x1 translation vector.

[0042] This embodiment uses coordinate transformation to unify data from different sensors (lidar and IMU) into the same coordinate system, namely the IMU coordinate system, thereby eliminating the data inconsistency problem caused by different coordinate systems and facilitating subsequent data fusion and processing. When the data of the lidar and IMU are in a unified coordinate system, the data of the two sensors can be more accurately fused. This fusion can make full use of their respective advantages, such as the high-precision distance measurement of the lidar and the high-frequency attitude information of the IMU, thereby improving the accuracy and reliability of the overall data fusion.

[0043] In this embodiment, environmental feature extraction is performed on the second point cloud data set to obtain environmental feature points, including the following steps: extracting surface features and edge features from the three-dimensional coordinate points in the second point cloud data set to obtain surface feature points surf And edge feature points pointsadge, the surface feature points pointssurf and edge feature points pointsedge are used as environment feature points.

[0044] This embodiment can reduce the analysis of less useful three-dimensional coordinate points by extracting and screening feature points from point cloud data, thereby concentrating computing power and improving computing efficiency without affecting computing accuracy.

[0045] S3: obtaining an initial posture of the train based on the train state information, and acquiring feature points associated with environmental feature points in a two-dimensional projection area of ​​the initial posture to obtain a relative posture.

[0046] In this embodiment, obtaining the initial position of the train based on the train state information includes the following steps: The linear acceleration, angular velocity and corresponding timestamps of two adjacent frames of the train are collected by IMU. The timestamps of two adjacent frames get the collection time interval. Based on the collection time interval, the angular velocity and linear acceleration of the train are integrated respectively and combined with the IMU noise to get the initial posture of the train.

[0047] Specifically, the specific calculation process of calculating the initial posture in this embodiment is as follows: This embodiment performs an integration operation based on two adjacent frames of IMU data to obtain an IMU odometer. When performing IMU integration, the initial value of the IMU integration is calculated in advance through IMU pre-integration to ensure that the integration result is independent of the initial value. In the subsequent iterative optimization process, the optimization variables are reduced and the calculation speed is accelerated. The motion equation of the IMU integration is as follows: Where R wj is the rotation in the world coordinate system at time j, V j is the speed at time j, P wj is the position at time j; and are the bias noise of the gyroscope and accelerometer, respectively, and Represent the discrete white noise of the gyroscope and accelerometer respectively; Δt is the acquisition time interval.

[0048] The above IMU integration formula needs to calculate the R, V, P at the previous moment when calculating the R, V, P at the j moment. If the state quantity at the i moment changes, the state quantity at the j moment needs to be recalculated. Therefore, pre-integration is used, that is, when the system state is adjusted during the optimization process, there is no need to re-integrate the system state at the next moment. The specific steps are as follows: Step 1: Use the pre-integration measurement model to calculate the IMU pre-integration increment between two IMU frames. The increment values ​​corresponding to R, V, and P are as follows: Step 2: Separate the IMU noise from the IMU pre-integrated increment value. The increment value after separation is as follows: in δV ij ,δP ij The separated noise.

[0049] This embodiment can make a preliminary estimate of the train's posture by using the train status information collected by the IMU, providing a direction for further improving the accuracy of the posture, thereby reducing the calculation steps and improving the calculation efficiency of the posture.

[0050] In this embodiment, acquiring feature points associated with environmental feature points in the two-dimensional projection area of ​​the initial pose to obtain a relative pose includes the following steps: Projecting the three-dimensional coordinate points in the second point cloud data set into a map coordinate system based on the initial posture to obtain a map data set, wherein the map coordinate system is a two-dimensional coordinate system; Match the m matching feature points closest to the surface feature points and edge feature points in the map data set, fit the matching feature points into a feature plane, and calculate the distance between the three-dimensional coordinate points in the second point cloud data set and the feature plane. When the deviation between the distance and the distance between the actual position coordinates of the train and the feature plane is the smallest, stop matching and obtain the relative posture.

[0051] This embodiment can significantly reduce the dimension of data processing by projecting the three-dimensional point cloud into a two-dimensional map coordinate system, thereby reducing the amount of calculation and processing time; by fitting feature planes and calculating the distance from point to plane instead of calculating the distance from point to point, the computational complexity can be further reduced, because plane fitting and point-to-plane distance calculation are usually more efficient than point-to-point distance calculation; this method is not only applicable to flat ground environments, but also because surface feature points and edge feature points can capture the complexity and diversity of the environment, so that it can handle environments with complex terrain and structures, thereby improving environmental adaptability; by matching these feature points in a map data set, it can adapt to various types of environmental features, thereby achieving wider applicability.

[0052] S4: Collect the current beacon mileage value when the train passes the beacon, and the posture correction algorithm obtains the estimated posture in response to the first point cloud data set and the timestamp of the current beacon mileage value.

[0053] In this embodiment, the posture correction algorithm corrects the relative posture in response to the second point cloud data set and the timestamp of the current beacon mileage value to obtain the estimated posture of the train, including the following steps: Collect the current beacon mileage value when the train passes the beacon, and obtain the beacon time interval based on the timestamp of the train passing the beacon in the current beacon mileage value and the timestamp of the second point cloud data set; The train angular velocity and linear acceleration are integrated at the beacon time interval to obtain the posture correction value, and the relative posture is corrected based on the posture correction value to obtain the estimated posture of the train.

[0054] Specifically, the calculation process of obtaining the estimated pose is as follows: Step 1: Get the timestamp t of the current laser frame lidar , the timestamp t of the current beacon beacon , calculate the time interval Δt between the two.

[0055] Step 2: Convert the beacon mileage value to the laser coordinate system to obtain pose beacon , quaternion beacon .

[0056] Step 3: Calculate t lidar The posture at the moment is as follows: Among them, Δpose, Δraw, Δpitch, and Δyaw are obtained by integrating the IMU as the initial pose, and the cumulative integration time is Δt. Project it onto the high-precision map to get the estimated pose, which is represented by quaternion and recorded as pose lidar , quaternionlidar .

[0057] Step 4: Pose lidar , quaternion lidar The estimated pose of the point cloud is used for matching and correction.

[0058] This embodiment continuously corrects the train posture by considering data detected by multiple sensors, thereby improving the train positioning accuracy.

[0059] S5: The fusion positioning algorithm obtains an optimized train posture estimate in response to the estimated posture and train status information, and obtains the current train posture based on the optimized train posture estimate.

[0060] In this embodiment, the fusion positioning algorithm obtains the train posture optimization estimate in response to the estimated posture and train state information, and obtains the current posture of the train based on the train posture optimization estimate, including the following steps: The train speed is obtained based on the train state information, the train speed is decomposed in three dimensions to obtain an estimated state quantity, and the estimated state quantity is used as the state quantity of the unscented Kalman filter to perform state estimation on the estimated posture of the train to obtain an optimized train posture estimate; Based on the train posture optimization estimate, the estimated posture is optimized to obtain the current posture of the train.

[0061] Specifically, the specific calculation process of fusion positioning is as follows: Step 1: Fuse IMU, LiDAR, beacon and vehicle speed to obtain the vehicle speed Decompose into v x 、v y 、v z , the decomposition formula is as follows: The decomposed speed is used as the estimated state quantity of the filter for state estimation.

[0062] Where quaternion is the estimated quaternion of the vehicle speed at the corresponding moment.

[0063] Step 2: Obtain the covariance matrix based on the estimated state quantity, and generate Sigma points through untraceable transformation. The formula is as follows: in is the estimated state quantity, P x is the covariance matrix.

[0064] Step 3: Update the predicted value, pass these Sigma points through the nonlinear state equation, and obtain the predicted value of the train state.

[0065] Step 4: Update the Sigma point. Same as step 2. The updated Sigma value will be used to calculate the train posture optimization estimate.

[0066] Step 5: Update the train posture optimization estimate according to the following formula and output the current train posture: Where K is the gain, is the estimated state quantity, P x，k is the covariance matrix.

[0067] This embodiment combines the data of IMU, lidar, beacon and vehicle speed, and reduces the cumulative error caused by the positioning of a single sensor through mutual comparison and correction between various sensors. It combines the positioning advantages of multiple sensors and improves the positioning accuracy and environmental adaptability of the train.

[0068] In this embodiment, the estimated state quantity is the estimated quaternion of the train speed at the corresponding moment.

[0069] This embodiment uses quaternions to correspond to the time corresponding to the train speed, which can concisely and clearly display the rotation and translation of the train. Compared with the traditional Euler angle representation, it also avoids the universal lock problem, thereby displaying the status of the train more accurately.

[0070] It can be seen from the above embodiments that at least the following substantial effects are achieved: (1) The present invention uses the point cloud map formed by the data detected by the laser radar as the initial data for train posture calculation, thereby solving the problem that various visual sensors of the train in the tunnel environment are difficult to accurately identify the surrounding environment of the train in a dark environment, improving the environmental perception ability, and providing a data basis for posture calculation. By extracting feature points from the point cloud map and using a tree data structure to store data, the problem of too large a point cloud data volume, too slow a calculation speed, and difficulty in meeting the real-time requirements of the train posture is solved; (2) The current posture of the train is predicted by combining the point cloud data monitored by the lidar and the train status information detected by the IMU, and the predicted train posture is corrected by the beacon mileage value detected by the beacon. The train posture offset is obtained based on the time difference between the timestamp of the beacon received and the timestamp of the point cloud data monitored by the lidar. In a long tunnel environment, the accumulated errors generated during the continuous operation of various sensors are reduced, which not only reduces the posture error caused by network transmission delay, but also greatly improves the positioning accuracy of the train in the tunnel environment.

[0071] The specific implementation described above is a preferred implementation of a fusion positioning method based on beacon correction of the present invention, and is not intended to limit the specific implementation scope of the present invention. The scope of the present invention includes but is not limited to this specific implementation. All equivalent changes made in accordance with the shape and structure of the present invention are within the protection scope of the present invention.

Claims

1. A fusion positioning method based on beacon correction, characterized in that: The following steps are involved: S1. Collecting the position information of the train in the point cloud map to obtain a first point cloud data set; S2, performing a dedistortion process on the first point cloud data set to obtain a second point cloud data set, and performing environmental feature extraction on the second point cloud data set to obtain environmental feature points; S3, obtaining an initial posture of the train based on the train state information, and obtaining feature points associated with environmental feature points in a two-dimensional projection area of ​​the initial posture to obtain a relative posture; S4, the posture correction algorithm corrects the relative posture in response to the second point cloud data set and the timestamp of the current beacon mileage value to obtain an estimated posture of the train; S5. The fusion positioning algorithm obtains an optimized train posture estimate in response to the estimated posture and train status information, and obtains the current posture of the train based on the optimized train posture estimate.

2. The beacon correction-based fusion positioning method according to claim 1, characterized in that: In S1, the location information of the train in the point cloud map is collected to obtain a first point cloud data set, including the following steps: The three-dimensional coordinate points of the train in a frame of point cloud at the current moment are collected in the point cloud map to obtain a first point cloud data set, and the three-dimensional coordinate points in the first point cloud data set are arranged and saved according to the size of the timestamp using a tree data structure.

3. The beacon correction-based fusion positioning method according to claim 1, characterized in that: In S2, dedistortion processing is performed on the first point cloud data set to obtain a second point cloud data set, including the following steps: Searching for the maximum timestamp and the minimum timestamp of the three-dimensional coordinate points in the first point cloud data set, and converting the first point cloud data set from the laser coordinate system to the IMU coordinate system based on the external parameters of the IMU and the laser radar; The IMU synchronization data set corresponding to the maximum timestamp and the minimum timestamp in the IMU data acquisition process is selected, and the linear interpolation algorithm is used to calculate the compensation transformation matrix of the IMU synchronization data set relative to the minimum timestamp. Based on the compensation transformation matrix, each three-dimensional coordinate point in the first point cloud data set is unified to the moment corresponding to the minimum timestamp to obtain the second point cloud data set.

4. The beacon correction-based fusion positioning method according to claim 3, characterized in that: Based on the external parameters of the IMU and the laser radar, the first point cloud data set is converted from the laser coordinate system to the IMU coordinate system, including the following steps: Based on the external parameters of the IMU and the laser radar, including the rotation matrix and the translation vector, a coordinate transformation formula is used to transform the three-dimensional coordinate points in the first point cloud data set from the laser coordinate system to the IMU coordinate system.

5. The beacon correction-based fusion positioning method according to claim 1, characterized in that: In S2, environmental feature extraction is performed on the second point cloud data set to obtain environmental feature points, including the following steps: Surface features and edge features are extracted from the three-dimensional coordinate points in the second point cloud data set to obtain surface feature points and edge feature points, and the surface feature points and edge feature points are used as environmental feature points.

6. The beacon correction-based fusion positioning method according to claim 1, characterized in that: In S3, the initial position of the train is obtained based on the train state information, including the following steps: The linear acceleration, angular velocity and corresponding timestamps of two adjacent frames of the train are collected by IMU. The timestamps of two adjacent frames get the collection time interval. Based on the collection time interval, the angular velocity and linear acceleration of the train are integrated respectively and combined with the IMU noise to get the initial posture of the train.

7. The beacon correction-based fusion positioning method according to claim 5, characterized in that: In S3, acquiring feature points associated with environmental feature points in the two-dimensional projection area of ​​the initial pose to obtain a relative pose includes the following steps: Projecting the three-dimensional coordinate points in the second point cloud data set into a map coordinate system based on the initial posture to obtain a map data set, wherein the map coordinate system is a two-dimensional coordinate system; Match the m matching feature points closest to the surface feature points and edge feature points in the map data set, fit the matching feature points into a feature plane, and calculate the distance between the three-dimensional coordinate points in the second point cloud data set and the feature plane. When the deviation between the distance and the distance between the actual position coordinates of the train and the feature plane is the smallest, stop matching and obtain the relative posture.

8. The beacon correction-based fusion positioning method according to claim 6, characterized in that: In S4, the posture correction algorithm corrects the relative posture in response to the second point cloud data set and the timestamp of the current beacon mileage value to obtain the estimated posture of the train, including the following steps: Collect the current beacon mileage value when the train passes the beacon, and obtain the beacon time interval based on the timestamp of the train passing the beacon in the current beacon mileage value and the timestamp of the second point cloud data set; The train angular velocity and linear acceleration are integrated at the beacon time interval to obtain the posture correction value, and the relative posture is corrected based on the posture correction value to obtain the estimated posture of the train.

9. The beacon correction-based fusion positioning method according to claim 1, characterized in that: In S5, the fusion positioning algorithm obtains the train posture optimization estimate in response to the estimated posture and the train state information, and obtains the current posture of the train based on the train posture optimization estimate, including the following steps: The train speed is obtained based on the train state information, the train speed is decomposed in three dimensions to obtain an estimated state quantity, and the estimated state quantity is used as the state quantity of the unscented Kalman filter to perform state estimation on the estimated posture of the train to obtain an optimized train posture estimate; Based on the train posture optimization estimate, the estimated posture is optimized to obtain the current posture of the train.

10. A fusion positioning method based on beacon correction according to claim 9, characterized in that: The estimated state quantity is the estimated quaternion of the train speed at the corresponding moment.

Images