Anchor point detection and track segmentation method for tethered robot based on tether dynamics

Abstract

The invention provides a tethered robot anchor point detection and trajectory segmentation method based on tether dynamics, and belongs to the technical field of mobile robot navigation and positioning. The method is based on tether dynamic analysis, combines a speedometer of a robot and observation data of a tether, and preliminarily determines the position of an anchor point by generating rays corresponding to the direction of the tether and calculating ray intersection points at different moments; the sensitivity and effectiveness of anchor point detection are ensured by adopting a clustering algorithm of preliminary abnormal value elimination and abnormal value rejection; taking the clustering center point as a final anchor point estimation value; and analyzing the accuracy and integrity of track segmentation according to index transfer and track continuity. The innovative technology can provide a more accurate anchor point prior value for a robot positioning algorithm such as a graph optimization algorithm so as to enhance the positioning capability of the robot in a global map.

Description

Technical Field

[0001] The invention belongs to the field of mobile robot navigation and positioning, and in particular relates to a method for real-time anchor point detection and track segmentation of a tethered mobile robot in automatic navigation. Background Art

[0002] Many areas on the planet's surface that are worth exploring are often located in extreme terrain, such as steep slopes and gullies. Most existing robots can only operate and work on relatively flat rocky terrain with slopes less than 30 degrees, and are not suitable for exploring the aforementioned extreme terrain areas. Steep slopes will weaken the traction of the patrol vehicle and greatly reduce the passability of the terrain; the traditional rocker bogie structure will significantly increase the slippage on steep slopes and cannot enter certain areas at all; gullies are also very dangerous for patrol vehicles; on rugged terrain, ordinary patrol mobile robots may fall.

[0003] In order to overcome the limitations brought by the above-mentioned extreme terrain areas, the patrol vehicles currently designed for steep slopes, gullies and other terrains are mostly tethered mobile robots. Through a rope with one end fixed to a fixed mother robot or anchor point to provide tension or support, the tethered mobile robots can reach steep slopes, gullies and other areas that ordinary wheeled robots cannot pass through for fixed-point detection, making it possible to detect these extreme terrain areas.

[0004] Although tether-based robots can solve the detection problem in extreme slope terrain, they also face the risk of tether contact and entanglement with obstacles. To solve this problem, positioning the robot's state and the position of the anchor point through graph optimization technology is an effective way to avoid tether entanglement. However, before performing graph optimization, the robot's trajectory and anchor point prior values ​​must be obtained, and the trajectory must be segmented and associated with the corresponding anchor point. In this process, accurate detection of anchor points and effective segmentation of trajectories are crucial, because these factors directly affect the positioning accuracy of the robot by the graph optimization algorithm. Only through precise anchor point positioning and accurate trajectory segmentation can graph optimization effectively improve the safety and operation efficiency of the robot in complex terrain.

[0005] At present, the research on anchor point detection and trajectory segmentation mainly relies on the Random Sample Consensus (RANSAC) algorithm. These methods usually require a lot of computing resources and have deficiencies in real-time and accuracy. Therefore, how to detect the anchor point position in real time and efficiently and complete trajectory segmentation and association has become an urgent problem to be solved in extreme terrain exploration based on tethered robot systems. Summary of the invention

[0006] The present invention proposes a tethered robot intersection detection and trajectory segmentation method based on tether dynamics. The method dynamically analyzes the interaction between the tether and the anchor point, detects the anchor point position generated by the contact between the tether and the obstacle in real time during the robot movement, and segments the trajectory related to the anchor point. The specific invention content is as follows:

[0007] The principle of the tethered robot intersection detection and trajectory segmentation method based on tether dynamics comes from the fact that, in an ideal state, when the tethered robot moves with the help of an anchor point, the direction of the tether will always point to the anchor point, such as Figure 1 The algorithm consists of the following steps:

[0008] Step 1: Collect data.

[0009] During the robot's motion, the system obtains and calculates the robot's position and posture [x k ,y k ,θ k ] and monitor the total length d of the tether released by the robot through sensors k and the angle with the robot's forward direction

[0010] Whenever the robot moves a certain distance, the system generates a ray based on the current robot position and the tether direction, and calculates the intersection with the ray generated by the previous robot position. The intersection is used as the estimated value of the current anchor point position. The estimated values ​​generated each time are accumulated to obtain a data set of anchor point estimates:

[0011]

[0012] This step relies on the robot's odometry data and angular measurements of the tether, as well as observations related to the tether's orientation.

[0013] Step 2: Preliminary outlier removal.

[0014] Among these estimates of the anchor point positions, there are inevitably some estimated points with large errors. We preliminarily eliminate outliers in the following way.

[0015] Since the initial anchor point is the mother robot or a fixed anchor point, its precise position is known, and the robot always moves away from the anchor point, the distance between the estimated anchor point and the initial fixed anchor point of the tether must be less than or equal to the total tether length d in theory. k Each estimated point Then make the following judgment on the estimated point: If it satisfies Then it is retained as the estimated value of the anchor point; otherwise it is treated as a noise point. iis a coefficient, which decreases with the increase of anchor points to ensure the effectiveness of removing outliers.

[0016] Step 3: Use the clustering algorithm that rejects outliers to determine whether new anchor points are generated.

[0017] In this step, the data set of anchor point estimation values ​​is clustered using the Hierarchical Density-Based Spatial Clustering of Applications with Noise (HDBSCAN). This method can automatically determine the number of clusters, and in the clustering process, mark points with low density and not belonging to any valid cluster as noise points, thereby further eliminating outliers. According to the number of clustering results, if the number increases, it means that new anchor points have been generated, otherwise return to step 1 to continue accumulating data as the robot moves forward.

[0018] Step 4: Initialize the old anchor points and segment the track.

[0019] According to the clustering results, the center of each category is calculated as the prior value of each anchor point state. When calculating the anchor point estimate, by passing the index of the pose to the anchor point, the trajectory segmentation and anchor point association can be well realized. However, due to the existence of outliers, some indexes are eliminated, resulting in the index of the anchor point in the clustering result is not continuous. Therefore, the present invention adopts a segmentation method based on trajectory continuity. Specifically, first, according to the clustering results, the trajectory index set corresponding to each anchor point is obtained, and then, by checking the continuity of the index in the set, the outlier index is inserted to ensure that each anchor point is associated with a continuous sub-trajectory, thereby ensuring the integrity of the trajectory segmentation.

[0020] In step 5, the algorithm finally obtains the sub-trajectory associated with each anchor point, which is used to perform graph optimization to achieve accurate estimation of the robot state and anchor point positions.

[0021] The beneficial effects brought by adopting the above technical solution are:

[0022] The main advantage of this technical solution is that it does not rely on the complex and computationally intensive random sampling iteration process of the RANSAC algorithm, thereby significantly reducing the computational complexity and improving real-time performance. By real-time monitoring and determining whether new anchor points are generated every short period of time, this method can quickly segment and optimize the trajectory, significantly improving the robot's real-time positioning capability. Whenever a new sub-trajectory is successfully associated with the corresponding anchor point, graph optimization is immediately performed to further ensure accurate and efficient positioning. BRIEF DESCRIPTION OF THE DRAWINGS

[0023] Figure 1 This is the schematic diagram of anchor point detection for a tethered mobile robot;

[0024] Figure 2 Schematic diagram of the robot's motion when the tethered mobile robot detects a new anchor point;

[0025] Figure 3 Flowchart for anchor point detection and initialization, and trajectory segmentation of tethered mobile robots;

[0026] Figure 4 is the anchor point clustering result;

[0027] Figure 5 Anchor point detection and trajectory segmentation results. DETAILED DESCRIPTION

[0028] The technical solution of the present invention will be described in detail below with reference to the accompanying drawings.

[0029] The present invention provides a method for detecting intersection points and segmenting trajectories of a tethered robot based on tether dynamics. Figure 2 As shown, this method is mainly implemented by the following steps:

[0030] Step 1: Figure 2 As shown in the figure, every time the robot runs a certain distance, the robot's current position and posture are obtained from its odometer data. and observation data of the tether The joint robot's previous position and observation data of the tether We can get two rays with the robot position as the endpoint and coinciding with the tether direction:

[0031] Ray 1: Endpoint (x k-1 ,y k-1 ), angle with the x-axis:

[0032] Ray 2: Endpoint (x k ,y k ), angle with the x-axis:

[0033] The intersection of the two rays is the estimated value of the current anchor point The robot has generated a total of N anchor point estimates since the start of movement, recorded as a set:

[0034]

[0035] Step 2: Each time an anchor point estimate is generated, the following judgment is made on the estimated point:

[0036] like It is retained as the estimated value of the anchor point; otherwise it is treated as a noise point.

[0037] Step 3: Use the HDBSCAN algorithm to cluster the estimated value set of the anchor points, and make the following judgments on the clustering results:

[0038] If the number of clusters is the same as the previous result, it means that no new anchor point is generated, and return to step 1 to continue accumulating data;

[0039] If the number of clusters increases compared to the previous result, it means that new anchor points are generated, and then go to step 4;

[0040] Figure 4 The clustering results of anchor point estimates are shown from experiments using the robot's global motion data and observation data.

[0041] Step 4: After determining that a new anchor point is generated, it means that a complete trajectory segment associated with the old anchor point has been generated. Next, the old anchor point is initialized and the trajectory is segmented.

[0042] By calculating the cluster center point of the old anchor point as its final estimate

[0043]

[0044] By passing the pose index to the anchor point, the pose and anchor point categories are matched one by one, and the trajectory segmentation effect is directly achieved. Among them, the noise point inserts the missing index (caused by the removal of outliers) based on the trajectory continuity to ensure that each anchor point is associated with a continuous sub-trajectory, thereby ensuring the integrity of the trajectory segmentation.

[0045] Figure 4 The clustering results of anchor point estimates are shown using experiments on the robot's global motion data and observation data.

[0046] The cluster center results of each anchor point are shown in Table 1. By comparing with the anchor point reference value, it can be seen that the algorithm's estimated results of the anchor point position are highly close to the reference value, and the error gradually increases. Since the optimization is carried out in real time, the subsequent anchor points 2 to 5 will be obtained on the corrected odometer data, so the cluster estimate value can be used as the prior value of each anchor point.

[0047] Table 1 Comparison of anchor point prior values ​​and reference values

[0048]

[0049] During the movement of the robot, in addition to the initial anchor point, it touches 4 anchor points along the way, and the trajectory is divided into 5 sub-trajectory segments. The comparison between the segmentation results obtained by the algorithm and the reference interval is shown in Table 2. It can be found that the deviation of the segmentation results is very small.

[0050] Table 2 Comparison between sub-trajectory estimated interval and reference interval

[0051]

[0052] Step 5: As shown in the figure, the algorithm finally obtains the sub-trajectory associated with each anchor point, which is used to perform graph optimization to achieve accurate estimation of the robot state and anchor point position. Since the present invention only discusses the method of providing initial priors for graph optimization, it will not be discussed here.

Claims

1. A method for anchor point detection and trajectory segmentation of a tethered robot based on tether dynamics, characterized in that: The steps include: Step 1: Collect data. During the robot's motion, the system obtains and calculates the robot's position and posture [x k ,y k ,θ k ] and monitor the total length d of the tether released by the robot through sensors k and the angle with the robot's forward direction Whenever the robot moves a certain distance, the system generates a ray based on the current robot position and the tether direction, and calculates the intersection with the ray generated by the previous robot position. The intersection is used as the estimated value of the current anchor point position. The estimated values ​​generated each time are accumulated to obtain a data set of anchor point estimates: This step relies on the robot's odometry data and angular measurements of the tether, as well as observations related to the tether's orientation. Step 2: Preliminary outlier removal. Among these estimates of the anchor point positions, there are inevitably some estimated points with large errors. We preliminarily eliminate outliers in the following way. Since the initial anchor point is the mother robot or a fixed anchor point, its precise position is known, and the robot always moves away from the anchor point, the distance between the estimated anchor point and the initial fixed anchor point of the tether must be less than or equal to the total tether length d in theory. k Each estimated point Then make the following judgment on the estimated point: If it satisfies Then it is retained as the estimated value of the anchor point; otherwise it is treated as a noise point. i is a coefficient, which decreases with the increase of anchor points to ensure the effectiveness of removing outliers. Step 3: Use the clustering algorithm that rejects outliers to determine whether new anchor points are generated. In this step, the data set of anchor point estimation values ​​is clustered using the Hierarchical Density-Based Spatial Clustering of Applications with Noise (HDBSCAN). This method can automatically determine the number of clusters, and in the clustering process, mark points with low density and not belonging to any valid cluster as noise points, thereby further eliminating outliers. According to the number of clustering results, if the number increases, it means that new anchor points have been generated, otherwise return to step 1 to continue accumulating data as the robot moves forward. Step 4: Initialize the old anchor points and segment the track. According to the clustering results, the center of each category is calculated as the prior value of each anchor point state. When calculating the anchor point estimate, by passing the index of the pose to the anchor point, the trajectory segmentation and anchor point association can be well realized. However, due to the existence of outliers, some indexes are eliminated, resulting in the index of the anchor point in the clustering result is not continuous. Therefore, the present invention adopts a segmentation method based on trajectory continuity. Specifically, first, according to the clustering results, the trajectory index set corresponding to each anchor point is obtained, and then, by checking the continuity of the index in the set, the outlier index is inserted to ensure that each anchor point is associated with a continuous sub-trajectory, thereby ensuring the integrity of the trajectory segmentation. In step 5, the algorithm finally obtains the sub-trajectory associated with each anchor point, which is used to perform graph optimization to achieve accurate estimation of the robot state and anchor point positions.

2. A method for anchor point detection and trajectory segmentation of a tethered robot based on tether dynamics according to claim 1, characterized in that: In step 1, a large number of anchor point estimates are determined by ray intersection based on the robot odometer data and tether observation data.

3. The method for anchor point detection and trajectory segmentation of a tethered robot based on tether dynamics according to claim 1, characterized in that: In step 2, the distance theory of the anchor point estimate is used to determine whether it meets the total tether length range. The estimated points with large errors are removed in combination with the dynamic distribution of the anchor points to ensure the accuracy of the anchor point position.

4. The method for anchor point detection and trajectory segmentation of a tethered robot based on tether dynamics according to claim 1, characterized in that: In step 3, the number of clustering results is used as a criterion for judging whether to generate new anchor points.

5. The method for anchor point detection and trajectory segmentation of a tethered robot based on tether dynamics according to claim 1, characterized in that: In step 4, the center of each category is used as the prior value of each anchor point state, and the accuracy and completeness of the trajectory segmentation are determined based on index transfer and trajectory continuity analysis.

Images