Navigation method of train coupling test robot

Abstract

The invention discloses a navigation method of a train coupling test robot, and belongs to the technical field of intelligent operation and maintenance of rail transit. According to the method, aiming at the problems of low positioning reliability, insufficient path safety and poor system robustness of the existing navigation technology in a train bottom degradation scene, a map containing structured confidence is pre-constructed, and an improved ICP algorithm fused with the confidence is adopted for repositioning; and planning a global path by using an improved A * algorithm introducing a confidence cost item, carrying out adaptive local trajectory planning and obstacle avoidance based on an omnidirectional platform kinematic model and a multi-target evaluation function, and meanwhile, carrying out monitoring and autonomous recovery on a navigation state through quantitative scoring. According to the method, the navigation precision, safety and overall stability of the robot in a complex structured environment can be effectively improved.

Description

Technical Field

[0001] This invention belongs to the field of intelligent operation and maintenance technology for rail transit, and particularly relates to a navigation method for a train coupling test robot. Background Technology

[0002] With the rapid development of rail transit towards intelligence and automation, utilizing mobile robots to replace manual labor for annual functional testing of electric train coupling couplers has become an important technological direction for improving operation and maintenance efficiency and safety. The core prerequisite for realizing this application is that the robot must possess stable and reliable autonomous navigation capabilities in the specific working environment of the train underside. However, the environment under a train typically presents as a long, narrow, and repetitive passageway, with surfaces mostly consisting of metal or painted surfaces lacking stable geometric features. This is considered a typical degraded scenario in the field of robot simultaneous localization and mapping, posing a severe challenge to navigation technology.

[0003] Currently, navigation technology based on pre-stored maps has some applications in this field, but it still has significant shortcomings. One type of method uses independent localization and path planning modules, failing to fully consider the inherent impact of the structured features of the environment on the reliability of the localization results. This leads to the robot being prone to localization failures in feature-sparse areas, resulting in navigation interruptions. Another type of method, while employing a tightly coupled optimization approach, typically ignores the quality differences in different areas of the pre-built map due to varying feature clarity. These differences directly affect the safety of path planning, making it impossible to guarantee driving safety in complex environments. Furthermore, existing path planning methods mostly use traditional search algorithms, failing to optimize for the highly structured passageways under trains. The planned paths may traverse areas with low reliability, resulting in insufficient reliability. At the motion control level, traditional methods mostly employ fixed parameter strategies, unable to adaptively adjust control parameters according to the robot's location in different areas, limiting maneuver efficiency in confined spaces.

[0004] The root cause of these problems lies in the lack of a systematic approach in current technology that can effectively mine and utilize prior structured environmental information and integrate this information throughout the entire process of localization, planning, and control. In degraded scenarios such as the underside of a train, how to fully utilize limited environmental features to achieve robust localization, how to plan safe paths based on map quality differences, and how to make motion control adapt to the environmental structure have been major challenges that have troubled those skilled in the art. Therefore, there is an urgent need for a navigation method that can deeply integrate prior environmental information, adapt to complex working conditions, and possess adaptive recovery capabilities to improve the navigation reliability, safety, and overall stability of robots in such special scenarios. Summary of the Invention

[0005] To address the aforementioned technical problems, this invention proposes a navigation method for a train coupling test robot, which can effectively improve the robot's navigation accuracy, safety, and overall stability in complex structured environments.

[0006] To achieve the above objectives, the present invention provides a navigation method for a train coupling test robot, comprising: based on a pre-constructed environmental map containing structured confidence information, performing the following process: The robot loads an environment map and uses structured confidence to perform weight registration in pose estimation to determine the robot's initial navigation pose in the environment map. Based on the initial navigation pose and the target pose, a path search incorporating structured confidence cost is performed on the environment map to generate a global navigation path; Based on the global navigation path and real-time perception data, a trajectory is sampled and selected in the velocity space through a multi-objective evaluation function, and motion control commands are output. The system calculates a score that represents the positioning quality and path tracking accuracy in real time, and triggers a pose estimation process or a path search process when the score is lower than the corresponding threshold.

[0007] Optionally, the process of weight registration in pose estimation using structured confidence scores includes: Obtain matching point pairs between the current sensor scan point cloud and the environmental map point cloud; We construct a weighted least squares problem with rotation and translation parameters as the optimization objective, where the weight of each matching point pair is determined by the structured confidence of its corresponding map point location and the Euclidean distance between the point pairs. Solve the weighted least squares problem to obtain the 3D spatial pose, and project the 3D spatial pose onto the 2D plane to obtain the initial navigation pose.

[0008] Optionally, in constructing the weighted least squares problem, the weights are calculated by combining the structured confidence scores with the Gaussian function values ​​based on Euclidean distance.

[0009] Optionally, the path search process incorporating structured confidence costs includes: In the cost function of the path search algorithm, a confidence cost term that is negatively correlated with the structured confidence at the path node is introduced; Based on the total cost including the confidence cost term, an optimal path search is performed on the grid of the environment map from the starting point to the destination.

[0010] Optionally, the confidence cost term is equal to one minus the structured confidence at the path node.

[0011] Optionally, the process of sampling and selecting a trajectory in the velocity space using a multi-objective evaluation function includes: Based on the kinematic model of the omnidirectional mobile platform, multiple candidate trajectories are generated by sampling in the velocity space; Each candidate trajectory is scored using a function that includes evaluation terms for the degree of orientation toward the target, distance to obstacles, speed, and lateral motion optimization. The candidate trajectory with the highest score is selected as the basis for outputting the motion control command.

[0012] Optionally, a lateral motion optimization evaluation term is used to optimize the lateral motion efficiency of the omnidirectional moving platform. The lateral motion efficiency is negatively correlated with the magnitude of the lateral velocity component.

[0013] Optionally, the conditions that trigger the pose estimation process or path search process are as follows: When the score representing the positioning quality is lower than the first preset threshold, the pose estimation process is triggered. When the score representing the accuracy of path tracking is lower than the second preset threshold, the path search process is triggered.

[0014] An electronic device, the electronic device comprising: a processor and a memory storing computer program instructions; The processor implements the navigation method for the train coupling test robot when executing the computer program instructions.

[0015] A computer storage medium storing computer program instructions, which, when executed by a processor, implement the navigation method of the train coupling test robot.

[0016] Technical Effects of this Invention: This invention discloses a navigation method for a train coupling test robot. By deeply integrating map structured confidence into the entire navigation process, it achieves several beneficial effects. Map structured confidence is used to improve point cloud matching weight registration in the relocalization algorithm, effectively enhancing positioning accuracy and robustness in degraded scenarios with sparse features, such as the bottom of the train. Introducing a cost term based on this confidence into global path planning enables the robot to actively avoid low-reliability areas and prioritize high-confidence paths with clear features, thus fundamentally enhancing the safety of the navigation process. A local trajectory planner optimized for the characteristics of an omnidirectional mobile platform, through a multi-objective evaluation function including a lateral motion optimization term, fully exploits the platform's motion capabilities, achieving flexible and efficient obstacle avoidance maneuvers in confined spaces. The established navigation status monitoring system evaluates the system status in real time through quantitative scoring and autonomously triggers relocalization or path replanning based on thresholds, endowing the system with anomaly self-detection and autonomous recovery capabilities, ensuring long-term operational stability. Overall, this invention provides a complete solution that ensures high-performance navigation while also possessing high safety and robustness, and has significant engineering application value. Attached Figure Description

[0017] The accompanying drawings, which form part of this application, are used to provide a further understanding of this application. The illustrative embodiments and descriptions of this application are used to explain this application and do not constitute an undue limitation of this application. In the drawings: Figure 1 This is a flowchart illustrating a navigation method for a train coupling test robot according to an embodiment of the present invention. Detailed Implementation

[0018] It should be noted that, unless otherwise specified, the embodiments and features described in this application can be combined with each other. This application will now be described in detail with reference to the accompanying drawings and embodiments.

[0019] It should be noted that the steps shown in the flowchart in the accompanying drawings can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0020] like Figure 1 As shown, this embodiment provides a navigation method for a train coupling test robot, including: Based on a pre-built environment map containing structured confidence information, the following procedure is performed: The robot loads an environment map and uses structured confidence to perform weight registration in pose estimation to determine the robot's initial navigation pose in the environment map. Based on the initial navigation pose and the target pose, a path search incorporating structured confidence cost is performed on the environment map to generate a global navigation path; Based on the global navigation path and real-time perception data, a trajectory is sampled and selected in the velocity space through a multi-objective evaluation function, and motion control commands are output. The system calculates a score that represents the positioning quality and path tracking accuracy in real time, and triggers a pose estimation process or a path search process when the score is lower than the corresponding threshold.

[0021] Furthermore, the process of weight registration in pose estimation using structured confidence scores includes: Obtain matching point pairs between the current sensor scan point cloud and the environmental map point cloud; We construct a weighted least squares problem with rotation and translation parameters as the optimization objective, where the weight of each matching point pair is determined by the structured confidence of its corresponding map point location and the Euclidean distance between the point pairs. Solve the weighted least squares problem to obtain the 3D spatial pose, and project the 3D spatial pose onto the 2D plane to obtain the initial navigation pose.

[0022] Furthermore, in constructing the weighted least squares problem, the weights are calculated by combining the structured confidence scores with the Gaussian function values ​​based on Euclidean distance.

[0023] Specifically, the implementation process of this embodiment includes: Robot relocalization: An improved ICP algorithm incorporating map structured confidence is used to calculate the robot's precise initial pose on the global map. This embodiment constructs a weighted least squares problem based on structured confidence: ; The formula for calculating the weights is as follows: ; in, To match the number of point pairs, index for the next collocation pair , Let k be the coordinates of the k-th point on the map. The coordinates of the k-th point in the current scan. Let be the rotation matrix and the translation vector. For the first The combined weight of each matching pair. For point Structured confidence of location The Euclidean distance between matching points. This is the confidence weighting coefficient, with a value range of [0,1]. The distance influence factor controls the rate of distance weight decay. After solving, the obtained 3D pose is projected onto a 2D plane, and the position coordinates (x, y) and yaw angle are extracted as the 2D navigation pose.

[0024] Furthermore, the path search process that incorporates structured confidence costs includes: In the cost function of the path search algorithm, a confidence cost term that is negatively correlated with the structured confidence at the path node is introduced; Based on the total cost including the confidence cost term, an optimal path search is performed on the grid of the environment map from the starting point to the destination.

[0025] Furthermore, the value of the confidence cost term is equal to one minus the structured confidence at the path node.

[0026] Specifically, the implementation process of this embodiment includes: Global path planning: An improved A algorithm is used to search for the optimal path from the starting point to the target point on a 2D grid map. This embodiment introduces a structured confidence cost term into the traditional cost function. ; Where n is the current search node, The actual cost from the starting point to node n. The estimated cost from node n to the target is calculated using Euclidean distance. The structured confidence cost for the location of node n. This is the confidence weighting coefficient.

[0027] The formula for calculating the structured confidence cost is as follows: ; in Let n be the map structure confidence level.

[0028] The final output global navigation route is derived from the path cost function. Defined, and derived from the structured confidence cost term The corrected path search results.

[0029] Furthermore, the process of sampling and selecting a trajectory in the velocity space using a multi-objective evaluation function includes: Based on the kinematic model of the omnidirectional mobile platform, multiple candidate trajectories are generated by sampling in the velocity space; Each candidate trajectory is scored using a function that includes evaluation terms for the degree of orientation toward the target, distance to obstacles, speed, and lateral motion optimization. The candidate trajectory with the highest score is selected as the basis for outputting the motion control command.

[0030] Furthermore, the lateral motion optimization evaluation term is used to optimize the lateral motion efficiency of the omnidirectional moving platform. The lateral motion efficiency is negatively correlated with the magnitude of the lateral velocity component.

[0031] Specifically, the implementation process of this embodiment includes: Local trajectory planning and obstacle avoidance: An adaptive dynamic window method is used for local trajectory optimization and real-time obstacle avoidance. First, a kinematic model of the omnidirectional mobile platform is established. Establish a kinematic model for the omnidirectional mobile platform: ; in, For the current moment, For the next moment, For robots in The position coordinates in the global coordinate system at any given time. For robots in The heading angle at any moment, For robots in Position coordinates at that moment for The heading angle at any moment, Let the vertical velocity be the robot's velocity in its own coordinate system. For lateral velocity, Angular velocity, The time step is then defined. A multi-objective evaluation function is subsequently designed: ; in: ; in, As an evaluation item for the degree of orientation toward the target, the robot's current orientation is calculated. angular difference from the target direction The closer the value is to 1, the more accurate the orientation. For obstacle distance evaluation, calculate the minimum distance from all points on the candidate trajectory to the nearest obstacle; the greater the distance, the higher the score. This is a speed-based evaluation item, assessing the ratio of the current speed to the maximum speed to promote efficient movement; For the evaluation of lateral motion optimization, among which To prevent division by zero for small constants, this term optimizes the lateral motion efficiency of the omnidirectional moving platform. to These are the weighting coefficients for each item. Multiple candidate trajectories are generated by sampling the velocity space, and the optimal trajectory is selected based on the evaluation function. Finally, the robot's motion control commands are output.

[0032] Furthermore, the specific conditions that trigger the pose estimation process or path search process are as follows: When the score representing the positioning quality is lower than the first preset threshold, the pose estimation process is triggered. When the score representing the accuracy of path tracking is lower than the second preset threshold, the path search process is triggered.

[0033] Specifically, the implementation process of this embodiment includes: Navigation Status Monitoring and Coordination: Real-time monitoring of the navigation system status, coordination of module operation, and handling of anomalies. This includes calculating a positioning quality score. and path tracking accuracy score And set thresholds to achieve quantitative evaluation of system status.

[0034] Each rating is defined as follows: Positioning quality score Path tracking accuracy score ; in, The trace of the covariance matrix of the state estimate reflects the positioning uncertainty; For actual location, The target location. When When this occurs, a relocation process is triggered, in which... To determine the quality threshold, a value between 0.1 and 0.3 is used. At that time, the global path is replanned, in which This is the path tracking accuracy threshold, with a value range of 0.05-0.15.

[0035] An electronic device, the electronic device comprising: a processor and a memory storing computer program instructions; The processor implements the navigation method for the train coupling test robot when executing the computer program instructions.

[0036] A computer storage medium storing computer program instructions, which, when executed by a processor, implement the navigation method of the train coupling test robot.

[0037] An application example of this invention includes the following steps: The experiment was conducted in an underground parking lot, using an omnidirectional mobile robot platform with a Livox Avia lidar with an integrated IMU mounted on top. The lidar point cloud output frequency was 10Hz, and the built-in IMU sampling frequency was 200Hz.

[0038] In step 1, the robot loads a pre-built 2D grid map containing structured confidence information. Relocalization is performed using an improved ICP algorithm that integrates map structured confidence, with the following parameters: confidence weight coefficient α = 0.7 and distance influence factor σ = 0.2. Relocalization is successful in a typical underground parking lot environment, with an initial pose estimation error of less than 0.1m.

[0039] In step 2, an improved A* algorithm is used for global path planning, with a confidence weight coefficient λ=0.3. The planned path effectively avoids low-confidence areas and prioritizes routes with clearly defined structured features, improving path safety by approximately 40%.

[0040] In step 3, an adaptive dynamic window method is used for local trajectory planning and real-time obstacle avoidance. The evaluation function weight coefficients are set as follows: γ1=0.4, γ2=0.3, γ3=0.2, γ4=0.1. When encountering temporary obstacles, the robot can fully utilize its omnidirectional movement capability to smoothly avoid obstacles.

[0041] In step 4, the navigation status monitoring system operates in real time, with the positioning quality threshold T1 set to 0.15 and the path tracking accuracy threshold T2 set to 0.10. During the 120-meter test path, the system automatically triggered two repositionings and one path replanning, effectively maintaining the stable operation of the navigation system.

[0042] Experimental results show that the method described in this invention has excellent navigation performance in complex indoor environments, and its positioning accuracy, path safety and system robustness all meet the operational requirements of the train coupling test robot.

[0043] This invention discloses a navigation method for a train coupling test robot. By deeply integrating map structured confidence into the entire navigation process, it achieves several beneficial effects. Map structured confidence is used to improve point cloud matching weight registration in the relocalization algorithm, effectively enhancing positioning accuracy and robustness in degraded scenarios with sparse features, such as the bottom of the train. Introducing a cost term based on this confidence into global path planning enables the robot to actively avoid low-reliability areas and prioritize high-confidence paths with well-defined features, thus fundamentally enhancing the safety of the navigation process. A local trajectory planner optimized for the characteristics of the omnidirectional mobile platform fully leverages the platform's motion capabilities through a multi-objective evaluation function that includes lateral motion optimization terms, achieving flexible and efficient obstacle avoidance maneuvers in confined spaces. The established navigation status monitoring system evaluates the system status in real time through quantitative scoring and autonomously triggers relocalization or path replanning based on thresholds, endowing the system with anomaly self-detection and autonomous recovery capabilities, ensuring long-term operational stability. Overall, this invention provides a complete solution that guarantees high-performance navigation while also possessing high safety and robustness, possessing significant engineering application value.

[0044] The above are merely preferred embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.

Claims

1. A navigation method for a train coupling test robot, characterized in that, include: Based on a pre-built environment map containing structured confidence information, the following procedure is performed: The robot loads an environment map and uses structured confidence to perform weight registration in pose estimation to determine the robot's initial navigation pose in the environment map. Based on the initial navigation pose and the target pose, a path search incorporating structured confidence cost is performed on the environment map to generate a global navigation path; Based on the global navigation path and real-time perception data, a trajectory is sampled and selected in the velocity space through a multi-objective evaluation function, and motion control commands are output. The system calculates a score that represents the positioning quality and path tracking accuracy in real time, and triggers a pose estimation process or a path search process when the score is lower than the corresponding threshold.

2. The navigation method for the train coupling test robot as described in claim 1, characterized in that, The process of weight registration in pose estimation using structured confidence scores includes: Obtain matching point pairs between the current sensor scan point cloud and the environmental map point cloud; We construct a weighted least squares problem with rotation and translation parameters as the optimization objective, where the weight of each matching point pair is determined by the structured confidence of its corresponding map point location and the Euclidean distance between the point pairs. Solve the weighted least squares problem to obtain the 3D spatial pose, and project the 3D spatial pose onto the 2D plane to obtain the initial navigation pose.

3. The navigation method for the train coupling test robot as described in claim 2, characterized in that, In constructing the weighted least squares problem, the weights are calculated by combining the structured confidence scores with the Gaussian function values ​​based on Euclidean distance.

4. The navigation method for the train coupling test robot as described in claim 1, characterized in that, The path search process that incorporates structured confidence costs includes: In the cost function of the path search algorithm, a confidence cost term that is negatively correlated with the structured confidence at the path node is introduced; Based on the total cost including the confidence cost term, an optimal path search is performed on the grid of the environment map from the starting point to the destination.

5. The navigation method for the train coupling test robot as described in claim 4, characterized in that, The value of the confidence cost term is equal to one minus the structured confidence at the path node.

6. The navigation method for the train coupling test robot as described in claim 1, characterized in that, The process of sampling and selecting a trajectory in velocity space using a multi-objective evaluation function includes: Based on the kinematic model of the omnidirectional mobile platform, multiple candidate trajectories are generated by sampling in the velocity space; Each candidate trajectory is scored using a function that includes evaluation terms for the degree of orientation toward the target, distance to obstacles, speed, and lateral motion optimization. The candidate trajectory with the highest score is selected as the basis for outputting the motion control command.

7. The navigation method for the train coupling test robot as described in claim 6, characterized in that, The lateral motion optimization evaluation term is used to optimize the lateral motion efficiency of the omnidirectional moving platform. The lateral motion efficiency is negatively correlated with the magnitude of the lateral velocity component.

8. The navigation method for the train coupling test robot as described in claim 1, characterized in that, The specific conditions that trigger the pose estimation process or path search process are as follows: When the score representing the positioning quality is lower than the first preset threshold, the pose estimation process is triggered. When the score representing the accuracy of path tracking is lower than the second preset threshold, the path search process is triggered.

9. An electronic device, characterized in that, The electronic device includes: a processor and a memory storing computer program instructions; When the processor executes the computer program instructions, it implements the navigation method of the train coupling test robot as described in any one of claims 1-8.

10. A computer storage medium, characterized in that, The computer storage medium stores computer program instructions, which, when executed by a processor, implement the navigation method of the train coupling test robot as described in any one of claims 1-8.

Images