Train positioning and attitude calculation method based on multi-sensor fusion and storage medium

Abstract

The application discloses a train positioning and posture calculation method based on multi-sensor fusion and a storage medium, and comprises two parts of train positioning initialization based on images and train posture odometry; the train posture odometry realizes real-time measurement of train speed through millimeter wave radar clustering, then rough positioning is obtained by fusing a Kalman filtering algorithm and an IMU; laser radar data is corrected in a frame and matched with a local map based on the rough positioning, and accurate posture mileage is obtained; and the cumulative error is eliminated by setting a plurality of correction position points on a train running route. The application solves the observation error accumulated frame by frame, and can construct a point cloud map of the surrounding environment of the train in the running process in real time by accumulating the point cloud information of the surrounding environment of the train collected in real time according to train positioning.

Description

Technical Field

[0001] This invention relates to the field of train positioning technology, specifically to a train positioning and attitude calculation method and storage medium based on multi-sensor fusion. Background Technology

[0002] With the rapid expansion of urban areas in my country, urban rail transit, which has a large passenger capacity, high transportation efficiency, and low energy consumption, has become an inevitable choice for alleviating urban traffic congestion.

[0003] Urban rail fully automated operation systems are widely used in cities due to their high reliability, high punctuality, and support for driverless operation. However, in GoA3-4 (no fixed driver) conditions, fully automated operation systems cannot rely on a driver's lookout to ensure airspace safety.

[0004] The current relevant technologies for train positioning are as follows:

[0005] Chinese Patent Publication No. CN110588722B, entitled "A Train Positioning Method, Train Positioning Processing Unit, and Beacon Reading Unit," describes a method for train positioning. During train operation, speed and distance are obtained through a speed and distance measuring unit. However, the position obtained by this unit has a cumulative error, requiring continuous position correction during train travel. The train positioning processing unit is used to correct the train's position. In existing technology, position correction is performed by communicating with ground beacons when the train passes them. However, this method requires correction with ground beacons and may be affected by factors such as weather.

[0006] Chinese Patent Publication No. CN110816593B, entitled "Train Positioning System and Train Positioning Method," describes an invention that uses a QR code containing track information placed on the ground and a reader installed on the train. When a controller receives the QR code information from the reader, it determines a target delay time based on the train's real-time speed and the current distance between the reader and the QR code. After the target delay time, the system sends the identified QR code information (i.e., the track information contained in the QR code) to the onboard ATP (Automatic Train Protection) system, thus protecting the train. This ensures that the train's reader passes over the QR code simultaneously when reporting the track information to the ATP, enabling rapid and accurate reporting of the identified track information and facilitating communication between the ground and the train. However, it essentially relies on the identification of relevant markers for train positioning, which may be affected by environmental factors. Summary of the Invention

[0007] The present invention proposes a train positioning and attitude calculation method, device and storage medium based on multi-sensor fusion, which can at least solve one of the technical problems in the background art.

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

[0009] A train localization and attitude calculation method based on multi-sensor fusion includes two parts: image-based train localization initialization and train pose odometer.

[0010] The train pose odometer achieves real-time measurement of train speed through millimeter-wave radar clustering, and then uses a Kalman filter algorithm to fuse with an IMU to obtain a coarse localization. Based on the coarse localization, the lidar data is intra-frame corrected and matched with a local map to obtain a precise pose odometer.

[0011] It also includes eliminating accumulated errors by setting up several correction points along the train route.

[0012] Furthermore, image-based train localization initialization includes fusing local and global features of the image to achieve localization initialization during train operation, thereby obtaining global train localization.

[0013] First, local feature information is collected by sensors and matched with a high-precision map to determine the train's current position on the map. Then, the train's motion data is collected using a train odometer sensor. Based on the train's motion data and local feature information, the train's position is repositioned and calibrated to achieve accurate positioning.

[0014] Based on the train's location information and map information, the train's odometer is updated and optimized. Finally, based on the high-precision map and global feature information, the initial position of the train is located and initialized.

[0015] Furthermore, global image feature extraction includes taking the complete image of the train ahead as a whole, extracting its features, and matching them with global image features in the map;

[0016] MobileNetV3 was chosen as the global feature extraction network. MobileNetV3 proposes a novel Bneck network structure using an inverse residual structure. First, dimensionality is increased using 1×1 convolutions, followed by feature extraction using 3×3 convolutions, and then dimensionality reduction using 1×1 convolutions. The inverse residual structure reduces information loss caused by the activation function. Simultaneously, depthwise separable convolutions significantly reduce network computation. Furthermore, the h-swish activation function is used, reducing computation while improving network performance. The H-swish formula is as follows:

[0017]

[0018] The image is input into the network, and the feature vector is extracted as the global vector of the image.

[0019] Furthermore, local feature extraction includes using ORB features and the FAST algorithm to find key points in the image; the FAST algorithm compares the 16 pixels around the key point p and calculates the number of pixels higher than p, less than p, and equal to p; if there are 8 or more consecutive pixels around the key point p that are greater than or less than p, then point p is a key point.

[0020] In ORB, N corner points are selected as feature points. Simultaneously, to describe the FAST angle of the image...

[0021] The directionality of a point is determined by the direction vector between the geometric center O and the centroid C of the image. To describe;

[0022] After selecting key points, the ORB algorithm describes the key points using the QUIEF algorithm. QUIEF is a binary descriptor, where each binary digit represents the size relationship between two pixels p and q. By randomly selecting the positions of p and q using probability, a 1024-dimensional QUIEF feature descriptor can be obtained.

[0023] The feature vectors extracted using the ORB feature descriptor are used as local vectors of the image, serving as local features.

[0024] Furthermore, the image-based train positioning initialization includes fusing local and global image features to achieve positioning initialization during train operation, obtaining global train positioning, including:

[0025] Using platform images as keyframes, a platform image dataset of train platforms is constructed.

[0026] The global features are regularized to achieve normalization, as shown in the formula below.

[0027]

[0028] Among them, v i For the i-th dimension global feature, It is the i-th global feature after normalization;

[0029] To perform localization initialization, the global feature vector of the i-th image in the platform image dataset, obtained through a neural network... and the global feature vector of the current frame Compare the two feature vectors and calculate the Euclidean distance D between them. i

[0030]

[0031] The similarity to images in the platform image dataset is represented by the calculated Euclidean distance.

[0032] For local features, a bag-of-words model is used for image retrieval. First, a platform image dataset is trained to generate a platform dictionary. The K-means++ algorithm is used to cluster the visual bag of words and construct a dictionary tree structure to facilitate image retrieval.

[0033] Use IDF i Indicates its weight

[0034]

[0035] Where N is the number of words in the platform image dataset, n i For the word η i The number of images in a platform image dataset is important. If a certain feature does not appear frequently in an image, then that feature is more likely to reflect the characteristics of the image, and its discriminative power will be higher. Using TF... i Indicates its weight

[0036]

[0037] Where, n A Let A be the total number of words contained in image A. Let w be the number of times word i appears in image A, then the word weight w is... i Represented by the product of the two

[0038] w i =IDF i ×TF i (6)

[0039] Then the bag-of-words vector of the i-th word in image A is represented as: When calculating the similarity between two images, the L1 norm of the difference between the different bag-of-words vectors of the two images is calculated:

[0040]

[0041] Subsequently, thresholds for global feature similarity and local feature similarity are set respectively. The two features are fused based on whether they are within the threshold range. If the similarity of both features is within the set threshold range, the platform is considered to be the target platform. Train repositioning at the platform is achieved through the above method.

[0042] Furthermore, the coarse localization steps include using the train's onboard millimeter-wave radar to measure the speed of surrounding obstacles, and then using a clustering algorithm to infer the train speed; when the train is running, it detects n types of obstacles O1, O2, ... O in front of it. nThe position and velocity information of the scanned object are obtained through millimeter-wave radar:

[0043]

[0044] Where, x i y represents the lateral distance between the train and the object. i v represents the longitudinal distance between the train and the object. i The speed of the train relative to the obstacle;

[0045] Clustering algorithms are used to group scattered millimeter-wave radar points into clusters;

[0046] First, the radar points are sorted according to the lateral coordinates of the obstacles detected by the millimeter-wave radar. Then, all radar points are traversed to determine radar point O. i Compared to the previous radar point O i-1 The distance d(O) between i-1 O i Is the value of radar point O less than the set threshold T? If the condition is met, then radar point O is determined to be... i-1 With radar point O i Radar points belonging to the same cluster are those that are the same obstacle; otherwise, create a new cluster. Repeat the above steps until all millimeter-wave radar points have been successfully clustered.

[0047]

[0048] Using the clustering algorithm described above, all millimeter-wave radar point clusters C are obtained; based on this, the train's own speed is obtained, and the relative speed between the train and the static obstacle is the train's speed.

[0049] Find the cluster S with the largest number of radar points among all radar point clusters, and its relative speed v with respect to the train. s That is, the train speed

[0050]

[0051] argmax is the index of the largest number in the array;

[0052] Since the velocities of all points in a millimeter-wave radar cluster are not identical, the average of all points is calculated as the velocity of the cluster, i.e., the train speed.

[0053]

[0054] It also includes prediction and correction using the Kalman filter algorithm; the prediction is based on estimating the current state based on the state at the previous time step, and the prediction includes state prediction and error matrix prediction formulas as shown below:

[0055] xk =Ax k-1 +Bu k-1 (12)

[0056] P k =AP k-1 A T +Q (13)

[0057] Correction, on the other hand, combines the estimated state and the observed state at the current moment to estimate the optimal state; correction includes Kalman gain calculation, state correction, and error matrix update, as shown in the following formula:

[0058] K k =P k H T HP k H T +R) -1 (14)

[0059] x k =x k +K k (z k -Hx k (15)

[0060] P k =(IK k H)P k (16)

[0061] Q and R are the prediction and observation state covariance matrices, respectively. In the train autonomous positioning system, the state dimensions are independent of each other, and Q and R can be set as diagonal matrices. The size of the diagonal elements will affect the filtering result. If the elements of Q are greater than the elements of R, the prediction noise is considered to be large, and the observation value is trusted more; otherwise, the prediction value is trusted more.

[0062] The speed measured by millimeter-wave radar is used as the predicted value, and the train speed obtained by IMU integration is used as the observed value. These are then fed into the Kalman filter algorithm for calculation to obtain the fused train speed. Finally, the train speed is integrated to obtain a rough train location.

[0063] Furthermore, it also includes using intra-frame correction to eliminate the effects of motion in order to eliminate point cloud deformation caused by train movement;

[0064] The pose change relationship between two consecutive frames is considered to be represented as follows:

[0065]

[0066] in, This is the transformation matrix between the two frames; Let R be the rotation matrix between the two frames.k ,T k Let R be the pose of the last point in the current frame. k Let T be the rotation matrix of the current frame. k Let t be the translation matrix of the current frame; assuming t k-1 It is the sampling time of the last point in the previous frame; for any sampling point at time t in the current frame, calculate the proportion s of the current time in the sampling time.

[0067]

[0068] Linear interpolation is performed at time t.

[0069]

[0070] Where θ is The magnitude of the rotation axis, where w is the unit vector of the rotation axis. It is a skew-symmetric matrix of w; the current pose of the radar is represented as

[0071]

[0072] Then, the points at time t can be projected onto the global graph through interpolation pose to achieve intra-frame correction of the point cloud.

[0073] Furthermore, it also includes using point cloud matching to correct the cumulative error;

[0074] Let the train's pose transformation relative to the initial coordinate system at time k be denoted as . The constructed point cloud map is Q k The motion estimation result obtained by inter-frame matching between time k+1 and the previous frame is: The observed point cloud is The pose transformation relationship is expressed as:

[0075]

[0076] pass Observation point cloud Transform to the initial coordinate system and add the newly observed point clouds to the point cloud map to obtain...

[0077] A cube region is captured and saved, and then divided into multiple sub-cube regions. Sub-cubes are retrieved using an index. After obtaining the train's location, the cube index is calculated using the location coordinates. The local map corresponding to the index is matched with the corrected current frame to obtain the train's accurate location. Based on the train's location, the point cloud information of the surrounding environment collected in real time is accumulated to construct a point cloud map of the surrounding environment during the train's operation in real time.

[0078] In another aspect, the present invention also discloses a computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of the method described above.

[0079] In another aspect, the present invention also discloses a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of the method described above.

[0080] As can be seen from the above technical solution, the purpose of this invention is to overcome the shortcomings and deficiencies of the existing technology and provide a train positioning and attitude calculation method based on multi-sensor fusion, solving the problems of high train positioning accuracy requirements and mispositioning in environments with high similarity. To address positioning drift issues in similar scenarios such as tunnels, this invention proposes for the first time a train autonomous positioning and attitude calculation technology, mainly comprising two parts: initial positioning and train pose odometer. The train pose odometer primarily achieves real-time measurement of train speed through millimeter-wave radar clustering, followed by fusion with an IMU using a Kalman filter algorithm to obtain coarse positioning; then, based on the coarse positioning, intra-frame correction is performed on the lidar data and matched with a local map to obtain precise pose odometer. Furthermore, to prevent the accumulation of errors from integral factors in positioning, several correction point locations are set along the train's running route to eliminate accumulated errors. Attached Figure Description

[0081] Figure 1 This is a technical roadmap for the train autonomous positioning system according to an embodiment of the present invention;

[0082] Figure 2 This is a flowchart of the positioning initialization process according to an embodiment of the present invention;

[0083] Figure 3 This is a schematic diagram of a novel Bneck network structure proposed in an embodiment of the present invention;

[0084] Figure 4 This is a schematic diagram of ORB feature extraction according to an embodiment of the present invention. Detailed Implementation

[0085] To make the objectives, technical solutions, and advantages of the embodiments of the present invention clearer, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are some embodiments of the present invention, but not all embodiments.

[0086] like Figure 1 As shown, the train positioning and attitude calculation method based on multi-sensor fusion described in this embodiment includes the following steps:

[0087] Step 1: Image-based train localization initialization

[0088] The positioning initialization part achieves positioning initialization during train operation by fusing local and global features of the image, thus obtaining the global positioning of the train.

[0089] First, local feature information, such as image data and LiDAR data, is collected using sensors. This local feature information is then matched with a high-precision map to determine the train's current position on the map. Next, sensors such as the train's odometer collect motion data, including distance traveled and speed. Based on the train's motion data and local feature information, the train's position is repositioned and calibrated to achieve accurate positioning. The train's odometer is then updated and optimized based on the train's position and map information. Finally, the train's initial position is determined and initialized using the high-precision map and global feature information.

[0090] Step 2: Global Image Feature Extraction. Global feature extraction refers to treating the complete image of the train ahead as a whole, extracting its features, and matching them with global image features in the map. This invention employs a neural network-based extraction method.

[0091] MobileNetV3 was chosen as the global feature extraction network to achieve fast and stable global feature extraction. MobileNetV3 introduces a novel Bneck network structure, such as... Figure 3 As shown, Bneck uses an inverse residual structure. It first uses 1×1 convolutions for dimensionality increase, then 3×3 convolutions for feature extraction, and finally 1×1 convolutions for dimensionality reduction. The inverse residual structure reduces information loss caused by the activation function. Simultaneously, depthwise separable convolutions significantly reduce the network's computational cost. Furthermore, the h-swish activation function is used, further reducing computational cost while improving network performance. The h-swish formula is as follows:

[0092]

[0093] The image is input into the network, and the feature vectors are extracted as the global vectors of the image. This provides the foundation for subsequent feature fusion and localization initialization.

[0094] Step 3: Local Feature Extraction

[0095] ORB features can quickly create feature vectors for keypoints in an image and use these feature vectors to identify objects in the image. ORB features use the FAST algorithm to find keypoints in an image. The FAST algorithm compares the 16 pixels surrounding a keypoint p, calculating the number of pixels higher than, lower than, and equal to p. If there are 8 or more consecutive pixels greater than or less than p in the surrounding area, then p is a keypoint. Since multiple FAST feature points may exist in an image, ORB selects N corner points as feature points. Furthermore, to describe the directionality of the FAST corner points in the image, the direction vector between the image's geometric center O and centroid C is used. To describe. For example... Figure 4 As shown.

[0096] After selecting keypoints, the ORB algorithm uses the QUIEF algorithm to describe them. QUIEF is a binary descriptor. Each binary digit represents the size relationship between two pixels, p and q. By randomly selecting the positions of p and q using probability, a 1024-dimensional QUIEF feature descriptor can be obtained.

[0097] The extracted ORB feature descriptors are rotation, translation, and scaling invariant, and can efficiently describe the features in the image.

[0098] The feature vectors extracted using the ORB feature descriptor are used as local vectors in the image, serving as local features. This provides the foundation for subsequent feature fusion and localization initialization.

[0099] Step 4: Based on the above steps, an image is represented as having global and local features. Subsequently, these two types of features will be used to achieve initial train localization. Platform images will be used as keyframes to construct a platform image dataset for the train.

[0100] For global features, the feature vectors extracted by the neural network are unevenly distributed. Some features may have excessively high weights, reducing the effectiveness of other dimensions and affecting the similarity measurement of global features. Therefore, regularization is performed on global features to normalize them, as shown in the formula below.

[0101]

[0102] Among them, v i For the i-th dimension global feature, It is the i-th global feature after normalization.

[0103] To perform localization initialization, the global feature vector of the i-th image in the platform image dataset, obtained through a neural network... and the global feature vector of the current frame Compare the two feature vectors and calculate the Euclidean distance D between them.i

[0104]

[0105] The similarity between the calculated Euclidean distance and the images in the platform image dataset is used to represent the similarity.

[0106] For local features, a bag-of-words model is used for image retrieval to ensure efficient and accurate matching. First, a platform image dataset is trained to generate a platform dictionary. Since the platform image dataset contains multiple frames of images, and each frame has numerous features, the K-means++ algorithm is used to cluster the visual bag-of-words to reduce the number of words corresponding to these features in the dictionary. A tree structure of the dictionary is then constructed to facilitate image retrieval.

[0107] Although each image is composed of many words, each word contributes differently to distinguishing the images. When a certain feature frequently appears in the platform image dataset, its contribution to image classification is relatively low. For example, features like tracks appear in the train operating environment, but these features do not reflect the characteristics of the train operating environment, so their weight should be relatively low. IDF can be used to further analyze this. i Indicates its weight

[0108]

[0109] Where N is the number of words in the platform image dataset, n i For the word η i The number of features in the platform image dataset. If a certain feature doesn't appear frequently in an image, then that feature is more likely to reflect the characteristics of that image, and its discriminative power is higher. Using TensorFlow... i Indicates its weight

[0110]

[0111] Where, n A Let A be the total number of words contained in image A. Let w be the number of times word i appears in image A. Then the word weight w i It can be expressed as the product of the two.

[0112] w i =IDF i ×TF i (6)

[0113] Then the bag-of-words vector of the i-th word in image A can be represented as: When calculating the similarity between two images, the L1 norm of the difference between the different bag-of-words vectors of the two images can be calculated:

[0114]

[0115] Subsequently, thresholds for global feature similarity and local feature similarity are set separately. The two features are then fused based on whether their similarity falls within the threshold range. If both similarities are within the set threshold range, the platform is considered the target platform. This method enables train repositioning at the platform.

[0116] Step 5: After the train positioning initialization, in order to realize the continuous position calculation of the train, this section designs a coarse positioning module that integrates millimeter-wave radar and IMU, and uses the Kalman filter algorithm to fuse the two types of data to estimate the coarse position of the train.

[0117] This section first designs a millimeter-wave radar speed measurement module. It utilizes a train-mounted millimeter-wave radar to measure the speed of surrounding obstacles, and then uses a clustering algorithm to infer the train's speed. When the train is running, it detects n types of obstacles O1, O2, ... O... n Millimeter-wave radar can obtain the position and velocity information of the scanned object:

[0118]

[0119] Where, x i y represents the lateral distance between the train and the object. i v represents the longitudinal distance between the train and the object. i This represents the speed of the train relative to the obstacle.

[0120] Since millimeter-wave radar typically detects multiple obstacles with varying speeds, a clustering algorithm was designed to group scattered millimeter-wave radar points into clusters.

[0121] First, the radar points are sorted according to the lateral coordinates of the obstacles detected by the millimeter-wave radar. Then, all radar points are traversed to determine radar point O. i Compared to the previous radar point O i-1 The distance d(O) between i-1 O i Is the value less than the set threshold T? If the condition is met, then radar point O is determined to be... i-1 With radar point O i Radar points belonging to the same cluster are radar points that are facing the same obstacle; otherwise, create a new cluster.

[0122] Repeat the above steps until all millimeter-wave radar points have been successfully clustered.

[0123]

[0124] The clustering algorithm described above can be used to obtain all millimeter-wave radar point clusters C. Based on this, the train's own speed can be obtained. Since the train's operating environment is relatively enclosed, most obstacles in rail transit are static. The relative speed between the train and these static obstacles is the train's speed.

[0125] Find the cluster S with the largest number of radar points among all radar point clusters, and its relative speed v with respect to the train. s That is, the train speed

[0126]

[0127] argmax is the index of the largest number in the array.

[0128] Since the velocities of all points in a millimeter-wave radar cluster are not identical, the average of all points is calculated as the velocity of the cluster, i.e., the train speed.

[0129]

[0130] Step 6: The train speed was obtained from the millimeter-wave radar using the clustering algorithm described above. However, the speed information obtained by the clustering algorithm is not accurate enough. To obtain a more accurate train speed, a Kalman filter algorithm is used to fuse the millimeter-wave radar speed measurement data and IMU acceleration data, thereby obtaining the precise train speed.

[0131] The Kalman filter algorithm consists of two steps: prediction and correction. Prediction estimates the current state based on the state at the previous time step. Prediction includes state prediction and error matrix prediction, as shown in the following formulas:

[0132] x k =Ax k-1 +Bu k-1 (12)

[0133] P k =AP k-1 A T +Q (13)

[0134] Correction, on the other hand, combines the estimated state and the observed state at the current moment to estimate the optimal state. Correction includes Kalman gain calculation, state correction, and error matrix update, as shown in the following formula:

[0135] K k =P k H T HP k H T +R) -1 (14)

[0136] x k=x k +K k (z k -Hx k (15)

[0137] P k =(IK k H)P k (16)

[0138] Q and R are the covariance matrices of the predicted and observed states, respectively. In the autonomous train positioning system, the states are independent of each other, and Q and R can be set as diagonal matrices. The size of the diagonal elements will affect the filtering result. If the elements of Q are greater than the elements of R, the prediction noise is considered to be large, and the observed value is trusted more; conversely, the predicted value is trusted more.

[0139] The speed measured by millimeter-wave radar is used as the predicted value, and the train speed obtained by IMU integration is used as the observed value. These are then fed into a Kalman filter algorithm for calculation to obtain the fused train speed. Finally, the train speed is integrated to obtain a rough train location.

[0140] Step 7: LiDAR, capable of providing remote environmental perception and precise 3D measurement, is becoming an indispensable sensor in autonomous positioning applications. However, LiDAR sampling time is relatively long, and as the train moves at high speed, the point cloud information collected by LiDAR will exhibit significant distortion. To eliminate the point cloud deformation caused by train motion, intra-frame correction is needed to eliminate the effects of motion.

[0141] The pose change relationship between two consecutive frames can be represented as follows:

[0142]

[0143] in, This is the transformation matrix between the two frames; Let R be the rotation matrix between the two frames. k ,T k Let R be the pose of the last point in the current frame. k Let T be the rotation matrix of the current frame. k Let t be the translation matrix for the current frame. k-1 It is the sampling time of the last point in the previous frame. For any sampling point at time t in the current frame, calculate the proportion s of the sampling time at the current time.

[0144]

[0145] Linear interpolation is performed at time t.

[0146]

[0147] Where θ is The magnitude of the rotation axis, where w is the unit vector of the rotation axis. It is a slant-symmetric matrix of w. The current pose of the radar can be represented as...

[0148]

[0149] Then, the points at time t can be projected onto the global graph through interpolation pose to achieve intra-frame correction of the point cloud.

[0150] Step 8: Due to the unavoidable observation noise of the train sensors, the pose estimation module will have certain errors. Since train positioning estimates the current position of the train by incrementally accumulating the pose estimation results, the observation error accumulates frame by frame.

[0151] To address this issue, this paper employs point cloud matching (matching the current frame point cloud with a local point cloud map) to correct the accumulated error.

[0152] Let the train's pose transformation relative to the initial coordinate system at time k be denoted as . The constructed point cloud map is Q k The motion estimation result obtained by inter-frame matching between time k+1 and the previous frame is: The observed point cloud is The pose transformation relationship can then be expressed as:

[0153]

[0154] It can be done Observation point cloud Transform to the initial coordinate system and add the newly observed point clouds to the point cloud map to obtain... To reduce the computational load of point cloud registration, a cubic region is extracted and saved, and then divided into multiple sub-cube regions. Sub-cubes are retrieved using indices. After obtaining the train's location, the cube index is calculated using the location coordinates. The local map corresponding to this index is matched with the corrected current frame to obtain the train's accurate location. Based on the train's location, the point cloud information of the surrounding environment collected in real time is accumulated to construct a point cloud map of the surrounding environment during the train's operation.

[0155] In summary, the embodiments of the present invention have the following characteristics:

[0156] (1) The image-based positioning initialization module of the present invention can not only realize positioning initialization, but also be used for positioning correction during operation to eliminate the error of the train's position odometer. Thus, the initial positioning of the train is determined.

[0157] (2) The present invention provides a train position and attitude calculation method based on multi-sensor fusion, and proposes a train pose odometer based on multi-sensor fusion, which includes a pose estimation module, an intra-frame correction module, and a point cloud matching module to realize autonomous train positioning.

[0158] (3) The point cloud matching method of the present invention (matching the current frame point cloud and the local point cloud map) solves the observation error of frame-by-frame accumulation. Based on the train positioning, the point cloud information of the surrounding environment collected in real time by the train is accumulated, so that the point cloud map of the surrounding environment during the train operation can be constructed in real time.

[0159] In another aspect, the present invention also discloses a computer-readable storage medium storing a computer program, which, when executed by a processor, causes the processor to perform the steps of the method described above.

[0160] In another aspect, the present invention also discloses a computer device, including a memory and a processor, wherein the memory stores a computer program, and when the computer program is executed by the processor, the processor performs the steps of the method described above.

[0161] In another embodiment provided in this application, a computer program product containing instructions is also provided, which, when run on a computer, causes the computer to execute any of the train positioning and attitude calculation methods based on multi-sensor fusion in the above embodiments.

[0162] It is understood that the system provided in the embodiments of the present invention corresponds to the method provided in the embodiments of the present invention, and the explanation, examples and beneficial effects of the relevant content can be referred to the corresponding parts of the above methods.

[0163] This application also provides an electronic device, including a processor, a communication interface, a memory, and a communication bus, wherein the processor, communication interface, and memory communicate with each other via the communication bus.

[0164] Memory, used to store computer programs;

[0165] When the processor executes the program stored in the memory, it implements the above-mentioned train positioning and attitude calculation method based on multi-sensor fusion.

[0166] The communication bus mentioned in the aforementioned electronic devices can be a Peripheral Component Interconnect (PCI) bus or an Extended Industry Standard Architecture (EISA) bus, etc. This communication bus can be divided into address bus, data bus, control bus, etc.

[0167] The communication interface is used for communication between the aforementioned electronic devices and other devices.

[0168] The memory may include random access memory (RAM) or non-volatile memory (NVM), such as at least one disk storage device. Optionally, the memory may also be at least one storage device located remotely from the aforementioned processor.

[0169] The processors mentioned above can be general-purpose processors, including central processing units (CPUs), network processors (NPs), etc.; they can also be digital signal processors (DSPs), application-specific integrated circuits (ASICs), field-programmable gate arrays (FPGAs), or other programmable logic devices, discrete gate or transistor logic devices, or discrete hardware components.

[0170] In the above embodiments, implementation can be achieved entirely or partially through software, hardware, firmware, or any combination thereof. When implemented using software, it can be implemented entirely or partially in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded and executed on a computer, all or part of the processes or functions described in the embodiments of this application are generated. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium or transmitted from one computer-readable storage medium to another. For example, the computer instructions can be transmitted from one website, computer, server, or data center to another website, computer, server, or data center via wired (e.g., coaxial cable, fiber optic, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.) means. The computer-readable storage medium can be any available medium that a computer can access or a data storage device such as a server or data center that integrates one or more available media. The available medium can be a magnetic medium (e.g., floppy disk, hard disk, magnetic tape), an optical medium (e.g., DVD), or a semiconductor medium (e.g., solid state disk (SSD)).

[0171] It should be noted that, in this document, relational terms such as "first" and "second" are used only to distinguish one entity or operation from another, and do not necessarily require or imply any such actual relationship or order between these entities or operations. Furthermore, the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such a process, method, article, or apparatus. Without further limitations, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes said element.

[0172] The various embodiments in this specification are described in a related manner. Similar or identical parts between embodiments can be referred to mutually. Each embodiment focuses on describing the differences from other embodiments. In particular, the system embodiments are basically similar to the method embodiments, so the description is relatively simple; relevant parts can be referred to the descriptions of the method embodiments.

[0173] The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit it. Although the present invention has been described in detail with reference to the foregoing embodiments, those skilled in the art should understand that modifications can still be made to the technical solutions described in the foregoing embodiments, or equivalent substitutions can be made to some of the technical features. Such modifications or substitutions do not cause the essence of the corresponding technical solutions to deviate from the spirit and scope of the technical solutions of the embodiments of the present invention.

Claims

1. A train positioning and attitude calculation method based on multi-sensor fusion, characterized in that, It includes two parts: image-based train positioning initialization and train pose odometer. The train pose odometer achieves real-time measurement of train speed through millimeter-wave radar clustering, and then uses a Kalman filter algorithm to fuse with an IMU to obtain a coarse localization. Based on the coarse localization, the lidar data is intra-frame corrected and matched with a local map to obtain a precise pose odometer. It also includes eliminating accumulated errors by setting up several correction points along the train route.

2. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that: Image-based train localization initialization involves fusing local and global features of the image to achieve train localization initialization during operation, thereby obtaining the train's global localization. First, local feature information is collected by sensors and matched with a high-precision map to determine the train's current position on the map. Then, the train's motion data is collected using a train odometer sensor. Based on the train's motion data and local feature information, the train's position is repositioned and calibrated to achieve accurate positioning. Based on the train's location information and map information, the train's odometer is updated and optimized. Finally, based on the high-precision map and global feature information, the initial position of the train is located and initialized.

3. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 2, characterized in that: Global image feature extraction involves taking the complete image of the train ahead as a whole, extracting its features, and matching them with global image features in the map; MobileNetV3 was chosen as the global feature extraction network. MobileNetV3 proposes a novel Bneck network structure using an inverse residual structure. First, dimensionality is increased using 1×1 convolutions, followed by feature extraction using 3×3 convolutions, and then dimensionality reduction using 1×1 convolutions. The inverse residual structure reduces information loss caused by the activation function. Simultaneously, depthwise separable convolutions significantly reduce network computation. Furthermore, the h-swish activation function is used, reducing computation while improving network performance. The H-swish formula is as follows: The image is input into the network, and the feature vector is extracted as the global vector of the image.

4. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 3, characterized in that: Local feature extraction includes using ORB features and the FAST algorithm to find key points in the image; The FAST algorithm compares the 16 pixels surrounding the key point p to calculate the number of pixels that are higher than p, lower than p, and equal to p. If there are 8 or more consecutive pixels in the surrounding area that are greater than or less than p, then point p is a key point; In ORB, N corner points are selected as feature points. Simultaneously, to describe the directionality of the FAST corner points of the image, the direction vector between the image geometric center O and the image centroid C is used. To describe; After selecting key points, the ORB algorithm describes the key points using the QUIEF algorithm. QUIEF is a binary descriptor, where each binary digit represents the size relationship between two pixels p and q. By randomly selecting the positions of p and q using probability, a 1024-dimensional QUIEF feature descriptor can be obtained. The feature vectors extracted using the ORB feature descriptor are used as local vectors of the image, serving as local features.

5. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 4, characterized in that: The image-based train positioning initialization includes fusing local and global image features to achieve positioning initialization during train operation, obtaining global train positioning, including... The platform images are used as keyframes to construct a platform image dataset for train stations; The global features are regularized to achieve normalization, as shown in the formula below. Among them, v i For the i-th dimension global feature, It is the i-th global feature after normalization; To perform localization initialization, the global feature vector V obtained from the i-th image in the platform image dataset through a neural network is... Ii and the global feature vector of the current frame Compare the two feature vectors and calculate the Euclidean distance D between them. i The similarity to images in the platform image dataset is represented by the calculated Euclidean distance. For local features, a bag-of-words model is used for image retrieval. First, a platform image dataset is trained to generate a platform dictionary. The K-means++ algorithm is used to cluster the visual bag-of-words and construct a dictionary tree structure to facilitate image retrieval. Use IDF i Indicates its weight Where N is the number of words in the platform image dataset, n i For the word η i The number of images in a platform image dataset is important. If a certain feature does not appear frequently in an image, then that feature is more likely to reflect the characteristics of the image, and its discriminative power will be higher. Using TF... i Indicates its weight Where, n A Let A be the total number of words contained in image A. Let w be the number of times word i appears in image A, then the word weight w is... i Represented by the product of the two w i =IDF i ×TF i (6) Then the bag-of-words vector of the i-th word in image A is represented as: When calculating the similarity between two images, the L1 norm of the difference between the different bag-of-words vectors of the two images is calculated: Subsequently, thresholds for global feature similarity and local feature similarity are set respectively. The two features are fused based on whether they are within the threshold range. If the similarity of both features is within the set threshold range, the platform is considered to be the target platform. Train repositioning at the platform is achieved through the above method.

6. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that: The rough localization steps include using the train's onboard millimeter-wave radar to measure the speed of surrounding obstacles, and then using a clustering algorithm to infer the train's speed; when the train is running, it detects n types of obstacles O1, O2, ... O n The position and velocity information of the scanned object are obtained through millimeter-wave radar: Where, x i y represents the lateral distance between the train and the object. i v represents the longitudinal distance between the train and the object. i The speed of the train relative to the obstacle; Clustering algorithms are used to group scattered millimeter-wave radar points into clusters; First, the radar points are sorted according to the lateral coordinates of the obstacles detected by the millimeter-wave radar. Then, all radar points are traversed to determine radar point O. i Compared to the previous radar point O i-1 The distance d(O) between i-1 O i Is the value of radar point O less than the set threshold T? If the condition is met, then radar point O is determined to be... i-1 With radar point O i Radar points belonging to the same cluster are those that are the same obstacle; otherwise, create a new cluster. Repeat the above steps until all millimeter-wave radar points have been successfully clustered. Using the clustering algorithm described above, all millimeter-wave radar point clusters C are obtained; based on this, the train's own speed is obtained, and the relative speed between the train and the static obstacle is the train's speed. Find the cluster S with the largest number of radar points among all radar point clusters, and its relative speed v with respect to the train. s That is, the train speed S=arg max i∈C {num i } (10) argmax is the index of the largest number in the array; Since the velocities of all points in a millimeter-wave radar cluster are not identical, the average of all points is calculated as the velocity of the cluster, i.e., the train speed. It also includes prediction and correction using the Kalman filter algorithm; the prediction is based on estimating the current state based on the state at the previous time step, and the prediction includes state prediction and error matrix prediction formulas as shown below: x k =Ax k-1 +Bu k-1 (12) P k =AP k-1 From T +Q (13) Correction, on the other hand, combines the estimated state and the observed state at the current moment to estimate the optimal state; correction includes Kalman gain calculation, state correction, and error matrix update, as shown in the following formula: K k =P k H T (HP k H T +R) -1 (14) x k =x k +K k (z k -Hx k ) (15) P k =(I-K k H)P k (16) Q and R are the prediction and observation state covariance matrices, respectively. In the train autonomous positioning system, the state dimensions are independent of each other, and Q and R can be set as diagonal matrices. The size of the diagonal elements will affect the filtering result. If the elements of Q are greater than the elements of R, the prediction noise is considered to be large, and the observation value is trusted more; otherwise, the prediction value is trusted more. The speed measured by millimeter-wave radar is used as the predicted value, and the train speed obtained by IMU integration is used as the observed value. These are then fed into the Kalman filter algorithm for calculation to obtain the fused train speed. Finally, the train speed is integrated to obtain a rough train location.

7. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 1, characterized in that: It also includes using intra-frame correction to eliminate the effects of motion in order to eliminate point cloud deformation caused by train movement; The pose change relationship between two consecutive frames is considered to be represented as follows: in, This is the transformation matrix between the two frames; Let R be the rotation matrix between the two frames. k ,T k Let R be the pose of the last point in the current frame. k Let T be the rotation matrix of the current frame. k Let t be the translation matrix of the current frame; assuming t k-1 It is the sampling time of the last point in the previous frame; for any sampling point at time t in the current frame, calculate the proportion s of the current time in the sampling time. Linear interpolation is performed at time t. Where θ is The magnitude of the rotation axis, where w is the unit vector of the rotation axis. It is a skew-symmetric matrix of w; the current pose of the radar is represented as Then, the points at time t can be projected onto the global graph through interpolation pose to achieve intra-frame correction of the point cloud.

8. The train positioning and attitude calculation method based on multi-sensor fusion according to claim 7, characterized in that: It also includes using point cloud matching to correct cumulative errors; Let the train's pose transformation relative to the initial coordinate system at time k be denoted as . The constructed point cloud map is Q k The motion estimation result obtained by inter-frame matching between time k+1 and the previous frame is: The observed point cloud is The pose transformation relationship is expressed as: pass Observation point cloud Transform to the initial coordinate system and add the newly observed point cloud data to the point cloud map to obtain... A cube region is captured and saved, and then divided into multiple sub-cube regions. Sub-cubes are retrieved using an index. After obtaining the train's location, the cube index is calculated using the location coordinates. The local map corresponding to the index is matched with the corrected current frame to obtain the train's accurate location. Based on the train's location, the point cloud information of the surrounding environment collected in real time is accumulated to construct a point cloud map of the surrounding environment during the train's operation in real time.

9. A computer-readable storage medium storing a computer program that, when executed by a processor, causes the processor to perform the steps of the method as claimed in any one of claims 1 to 8.

10. A computer device comprising a memory and a processor, the memory storing a computer program that, when executed by the processor, causes the processor to perform the steps of the method as claimed in any one of claims 1 to 8.

Images