Anchor point detection and positioning method for tethered mobile robot

Abstract

The present invention provides a method for detecting and locating anchor points for a tethered mobile robot. The detection method calculates the distance from the current anchor point to the line along which the tether lies. When the distance between the two is greater than a distance compensation value, a new anchor point is detected. The positioning method utilizes the fact that the tethers always pass through the same anchor point under the tether. The robot's anchor point coordinates are obtained by calculating the intersection points and averaging these intersections. The detection and positioning method of the present invention is simple to implement, highly sensitive, requires minimal computation, and offers high anchor point positioning accuracy. Using this method to detect and locate tethered anchor points can improve the positioning accuracy of a tethered robot within 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 detecting and positioning anchor points of a tethered mobile robot. Background Art

[0002] Many areas of planetary surfaces worth exploring are often located in extreme terrain, including steep slopes and gullies. Most existing robots can only operate and work on relatively flat rocky terrain with slopes less than 30 degrees, making them unsuitable for exploring these extreme terrains. Steep slopes weaken the rover's traction, significantly reducing the terrain's accessibility. Traditional rocker-bogie structures experience significant slippage on steep slopes, making certain areas impossible to access. Gully structures are also extremely dangerous for rovers. Conventional mobile patrol robots can fall on rugged terrain.

[0003] In order to overcome the limitations brought by the above-mentioned extreme terrain areas, the current patrol vehicle designed for steep slopes, gullies and other terrains is a tethered mobile robot. The rope with one end fixed to a fixed mother robot or anchor point provides tension or support. The tethered mobile robot 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] Tether management is crucial for tethered robots. Tether management is defined as the process of managing the tether to reduce inherent problems and gain useful information about the environment. When using the tether to acquire environmental information, the location of the tether's anchor point is essential for the robot's positioning and map updates, significantly impacting its positioning accuracy.

[0005] Existing methods for detecting new anchor points rely on determining when the free length of the tether is greater than the distance between the robot and the current anchor point. Tethered robot anchor point positioning methods assume that the new anchor point lies on an ellipse centered on the robot and the current anchor point, and calculate the coordinates of the new anchor point using an ellipse model and an error angle. This detection and positioning method has low sensitivity, high computational complexity, and low accuracy due to the presence of an error angle. Summary of the Invention

[0006] In order to solve the technical problems mentioned in the above background technology, the present invention proposes a method for detecting and positioning the anchor point of a tethered mobile robot, which can quickly calculate the coordinates of the robot's anchor point.

[0007] In order to achieve the above technical objectives, the technical solution of the present invention is:

[0008] The anchor point detection and positioning method of a tethered mobile robot includes the following steps:

[0009] (1) Acquire data;

[0010] (2) Calculate the equation of the line on which the tether lies;

[0011] (3) Determine whether a new anchor point is detected;

[0012] (4) When a new anchor point is detected, set the new anchor point; otherwise, directly execute step (5);

[0013] (5) Calculate the intersection of the straight line obtained in step (2) and all straight lines passing through the anchor point;

[0014] (6) The coordinates of the anchor point are obtained by averaging the coordinates of all intersection points.

[0015] As a preference, in step (1), the following data must be obtained: the current anchor point coordinate Y n , the robot heading angle θ t , robot coordinate X t , the sensor measures the angle Φ between the tether and the robot t .

[0016] Preferably, in step (2), the equation of the straight line is

[0017] f i :y=k i x+b i

[0018] where k i =tan(θ t -(π-φ t )), b i =y t -k i x t , X t =[x t ,y t ] T , i represents the number of straight lines.

[0019] Preferably, in step (3), when the distance between the straight line and the current anchor point is greater than the distance compensation value, it is determined that a new anchor point is detected; the distance compensation value is obtained by averaging the distance from anchor point 1 to the straight line where the tether anchored at anchor point 1 is located, and its initial value can be set as the step length of the robot.

[0020] Preferably, in step (5), the coordinates of the intersection of the i-th straight line and the j-th straight line are

[0021]

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

[0023] This method detects new anchor points by calculating the distance from the current anchor point to the line along which the tether is currently located, offering high sensitivity. The robot's anchor point coordinates are calculated by calculating the equation of the line through the tether, finding the intersection of all lines passing through the anchor point, and averaging these intersections. This method is simple to implement, has low computational complexity, requires minimal calculations, and achieves high anchor point positioning accuracy. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 Schematic diagram of the robot's motion when a new anchor point is detected for the tethered mobile robot;

[0025] Figure 2 This is the algorithm flow chart of the anchor point detection and positioning method for tethered mobile robots. DETAILED DESCRIPTION

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

[0027] The present invention provides a method for detecting and positioning the anchor point of a tethered mobile robot: the detection method calculates the distance from the current anchor point to the straight line where the tether is located, and when the distance between the two is greater than a distance compensation value, it is determined that a new anchor point has been detected; the positioning method uses the fact that the tether is under the same anchor point and the straight line where the tether is located always passes through the anchor point, and the coordinates of the robot anchor point are obtained by calculating the intersection points of the straight lines and averaging the intersection points.

[0028] like Figure 1 As shown, at time t-1, the robot is at point X t-1 The robot's heading angle is θ t-1 , the angle between the tether and the robot is Φ t-1 , the current anchor point coordinate is Y n At time t, the robot is at point X t The robot's heading angle is θ t , the angle between the tether and the robot is Φ t , the current anchor point coordinate is Y n At this time, X t =[x t ,y t ] T , Y n =[x n ,y n ] T .

[0029] like Figure 2 As shown in the figure, the algorithm flow chart of the anchor point detection and positioning method of the tethered mobile robot includes the following calculation steps:

[0030] (1) Get the following data: Current anchor point coordinate Y n , the robot heading angle θt , robot coordinate X t , the sensor measures the angle Φ between the tether and the robot t .

[0031] (2) Using the positional relationship between the robot and the anchor point after the robot anchors the anchor point, the robot’s position in X t The equation of the line on which the tether lies at the time is

[0032] f i :y=k i x+b i

[0033] where k i =tan(θ t -(π-φ t )), b i =y t -k i x t , i represents the line number.

[0034] (3) When the distance between the straight line and the current anchor point is greater than the distance compensation value d s When , it is considered that a new anchor point is detected; the distance compensation value d s The average distance from anchor point 1 to the straight line where the tether is anchored at anchor point 1 is obtained, and its initial value is set as the robot's step length. The distance formula from the current anchor point to the straight line is

[0035]

[0036] The calculation formula for the distance compensation value is:

[0037]

[0038] Where m represents the number of lines where the tethers are anchored at anchor point 1.

[0039] (4) When a new anchor point is detected, n=n+1, set and initialize the new anchor point; otherwise, directly execute step (5)

[0040] (5) Calculate the intersection of all straight lines passing through the anchor point. The coordinates of the intersection of the i-th straight line and the j-th straight line are

[0041]

[0042] (6) The coordinates of the anchor point are obtained by averaging the coordinates of the intersections of all the straight lines passing through the anchor point. The coordinates of the anchor point are

[0043]

[0044] Where k represents the number of straight lines.

[0045] In an embodiment of the present invention, a tethered mobile robot detects a new anchor point by calculating the distance from the anchor point to the line along which the tether is located. When the distance between the two is greater than a distance compensation value, the new anchor point is detected. Because the line along which the tether is located always passes through the same anchor point, the positioning algorithm for the new anchor point uses a calculation method that averages the intersections of the lines, achieving good anchor point estimation accuracy. This method can be applied to tether management technology for tethered mobile robots that obtain useful environmental information via a rope with one end fixed to a mother robot / anchor location. It can effectively improve anchor point detection and positioning accuracy, thereby effectively preventing rope entanglement and enabling robot positioning using the rope. Such robots have broad application prospects in fields such as planetary surface exploration and disaster relief.

[0046] The present invention has many specific application paths. The above is only a preferred embodiment of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements made without departing from the principle of the present invention should also be regarded as the scope of protection of the present invention.

Claims

1. A method for detecting and positioning anchor points of a tethered mobile robot, comprising the following steps: (1) Get data, including the current anchor point coordinates , robot heading angle , robot coordinates , the sensor measures the angle between the tether and the robot ; (2) Based on the data, the equation of the line on which the tether lies is calculated, which is expressed as: ， in , , , i represents the number of straight lines; (3) Calculate the distance between the straight line and the current anchor point to determine whether a new anchor point is detected; (4) When a new anchor point is detected, set the new anchor point and execute step (5); otherwise, execute step (5) directly; (5) Calculation steps The intersection of the resulting straight line and all straight lines passing through the anchor point; (6) The coordinates of the anchor point are obtained by averaging the coordinates of all intersection points.

2. The anchor point detection and positioning method of a tethered mobile robot according to claim 1, characterized in that: In step (3), when the distance between the straight line and the current anchor point is greater than the distance compensation value, it is determined that a new anchor point is detected.

3. The anchor point detection and positioning method of a tethered mobile robot according to claim 2, characterized in that: Distance compensation value The distance from the anchor point to the line where the tether is anchored is averaged. The distance formula from the current anchor point to the line is: , the distance compensation value is expressed as: , where m represents the number of lines anchored at the anchor point. The current anchor point coordinates are ; Distance compensation value The initial value is set as the robot's step size.

4. The anchor point detection and positioning method of a tethered mobile robot according to claim 1 or 2, characterized in that: In step (5), the coordinates of the intersection of the i-th line and the j-th line are 。 5. The anchor point detection and positioning method of a tethered mobile robot according to claim 4, characterized in that: In step (6), the coordinates of the intersection points of all the straight lines passing through the anchor point are averaged to obtain the coordinates of the anchor point. The coordinates of the anchor point are: ， Where k represents the number of straight lines.

Images