Autonomous navigation control method and system for underwater robot
Through the ranging probe collecting data to judge position and attitude, calculating the deflection angle to adjust the heading, it solves the problem that underwater robots are difficult to navigate along the central axis in the tunnel, and achieves the effect of fast and accurate navigation on the central axis of the tunnel and avoiding damage from hitting walls.
Patent Information
- Application Number
- CN202410929943.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-07-11
- Publication Date
- 2024-11-05
AI Technical Summary
Underwater robots are difficult to navigate along the central axis in narrow environments such as tunnels, and are prone to collision with the tunnel walls, resulting in damage.
The distance data is collected by the distance measuring probe installed on the underwater robot, the position and attitude of the robot are judged, the deflection angle is calculated and input into the PID directional controller, and the heading is adjusted to avoid collisions.
It realizes the rapid and accurate navigation of the underwater robot along the central axis of the tunnel to avoid wall damage, and is suitable for complex environments such as long-distance water transmission tunnels.
Smart Images

Figure CN118897567A_ABST
Abstract
Description
Technical Field
[0001] The present invention relates to the field of autonomous navigation planning of underwater robots, and in particular to an autonomous navigation control method and system for underwater robots. Background Art
[0002] In recent years, natural disasters such as earthquakes and floods have occurred frequently. Post-disaster emergency inspection of long-distance water transfer tunnels, water diversion projects, dams and other hydraulic structures is a major demand related to the national economy and people's livelihood. At present, domestic and foreign research institutions attach great importance to underwater robots that can complete specific tasks in complex environments. When underwater robots operate in narrow environments such as tunnels, they not only need to be able to operate underwater at a specific depth and heading, but also need to avoid colliding with the tunnel wall during the inspection process to prevent damage to the robot.
[0003] Therefore, it is necessary to design an autonomous navigation control algorithm so that the robot can navigate along the central axis of the tunnel. Summary of the invention
[0004] In view of this, an object of the present invention is to provide an autonomous navigation control method for an underwater robot, which utilizes the distance from both sides of the robot to the cave wall to calculate the deflection angle so that the robot can navigate along the central axis.
[0005] To achieve the above object, the present invention provides an underwater robot autonomous navigation control method, the steps comprising:
[0006] Collect the distance data between the underwater robot probe and the tunnel wall;
[0007] Based on the distance data, determining the position and posture of the underwater robot;
[0008] Based on the position and posture of the underwater robot, autonomous navigation control is completed.
[0009] Preferably, the method for collecting the distance data includes: detecting the distance data between the probe and the tunnel wall by means of four ranging probes installed on the underwater robot; the ranging probes are installed at four positions of the underwater robot: front left, front right, rear left, and rear right.
[0010] Preferably, the method for judging the position includes: judging the size of the rangefinder value detected by the front left and front right ranging probes installed on the underwater robot, so as to determine whether the underwater robot is currently located to the left or right of the central axis; the method for judging the posture includes: judging the size of the rangefinder value detected by the front left and rear left ranging probes installed on the underwater robot, so as to determine whether the current posture of the underwater robot is the bow deviating to the left or the bow deviating to the right.
[0011] The preferred control method for completing autonomous navigation includes: calculating the size of the deflection angle; adding the calculated deflection angle to the current heading angle of the underwater robot, and inputting this result into the PID directional controller as the desired heading, calculating the rotational torque; and then distributing the rotational torque to the propeller of the underwater robot.
[0012] Preferably,
[0013] When the underwater robot is currently to the right of the central axis:
[0014] If the bow deviates to the left, the deflection angle is α 3 =α 1 +α 2 ; If the bow deviates to the right, there are two cases. At this time, if α 1 ≥β, the deflection angle is α 3 =-1*|α 1 -α 2 |, if α 1 <β, the deflection angle is α 3 =|α 1 -α 2 |; Among them, α 1 is the angle between the underwater robot and the central axis, which is an acute angle; α 2 is the angle between the straight line formed by the bow of the underwater robot and the target point and the central axis, which is an acute angle; α 3 is the required deflection angle; β is the angle between the straight line formed by the stern of the underwater robot and the target point and the central axis, which is an acute angle.
[0015] Preferably,
[0016] If the underwater robot is currently to the right of the center axis:
[0017] If the bow deflects to the right, the deflection angle is α 3 =α 1 +α 2 ; If the bow deviates to the left, there are two cases. At this time, if α 1 ≥β, the deflection angle is α 3 =|α 1 -α 2 |, if α 1 <β, the deflection angle is α 3 =-1*|α 1 -α 2 |.
[0018] The present invention also provides an underwater robot autonomous navigation control system, the system is used to implement the above method, including: a collection module, a judgment module and a control module;
[0019] The acquisition module is used to collect the distance data between the underwater robot probe and the tunnel wall;
[0020] The judgment module is used to judge the position and posture of the underwater robot based on the distance data;
[0021] The control module is used to complete the control of autonomous navigation based on the position and posture of the underwater robot.
[0022] Preferably, the acquisition module includes four ranging probes, which are installed at four positions of the underwater robot: front left, front right, rear left, and rear right; the distance data between the probes and the tunnel wall is detected by the four ranging probes installed on the underwater robot.
[0023] Compared with the prior art, the present invention has the following beneficial effects:
[0024] The present invention can utilize the distance data measured by the rangefinder carried by the underwater robot to realize the heading control of the underwater robot, so that the robot can navigate along the central axis of the tunnel more quickly and accurately, quickly reach the detection position, and avoid hitting the wall and thus damaging the robot.
[0025] The parameters required by the present invention are simple and easy to obtain, and the maneuverability of the robot can be controlled by modifying the parameters of the target point; the invention has strong applicability and can be used in long-distance water conveyance tunnels, inverted siphon projects, box culverts and other environments; the invention can well solve the problem that the underwater robot will move left and right during the depth determination and orientation process, so that the robot can navigate along the central axis of the tunnel, effectively preventing it from hitting the wall and thus not damaging the robot. BRIEF DESCRIPTION OF THE DRAWINGS
[0026] In order to more clearly illustrate the technical solution of the present invention, the following briefly introduces the drawings required for use in the embodiments. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative labor.
[0027] Figure 1 A schematic diagram of a method flow of an embodiment of the present invention;
[0028] Figure 2 It is an overall control schematic diagram of an embodiment of the present invention;
[0029] Figure 3 It is an overall schematic diagram of the autonomous navigation control algorithm of an embodiment of the present invention;
[0030] Figure 4 This is a schematic diagram of a robot according to an embodiment of the present invention, which is to the left of the central axis, with its bow deviating to the left, and needs to turn right;
[0031] Figure 5This is a schematic diagram of a robot according to an embodiment of the present invention, which is to the left of the central axis, with its bow tilted to the right, and needs to turn left;
[0032] Figure 6 This is a schematic diagram of the robot according to an embodiment of the present invention being to the left of the central axis, with its bow tilted to the right and the robot needing to turn right. DETAILED DESCRIPTION
[0033] The following will be combined with the drawings in the embodiments of the present invention to clearly and completely describe the technical solutions in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.
[0034] In order to make the above-mentioned objects, features and advantages of the present invention more obvious and easy to understand, the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.
[0035] Embodiment 1
[0036] like Figure 1 and Figure 2 As shown, it is a schematic diagram of the method flow and the overall control diagram of this embodiment, and the steps include:
[0037] S1. Collect the distance data between the underwater robot probe and the tunnel wall.
[0038] The distance data between the probes and the tunnel wall are detected by four ranging probes installed on the front left, front right, rear left and rear right of the underwater robot. By judging the size of the front left rangefinder value and the front right rangefinder value, it is concluded whether the underwater robot is currently located to the left or right of the central axis. By judging the size of the front left rangefinder value and the rear left rangefinder value, it is concluded whether the bow of the underwater robot is deviated to the left or to the right.
[0039] In addition, a thruster is installed on the front and back sides of the robot.
[0040] S2. Determine the position and posture of the underwater robot based on the distance data.
[0041] By judging the size of the rangefinder values detected by the front left and front right rangefinder probes installed on the underwater robot, it is determined whether the underwater robot is currently located to the left or right of the central axis; the method for judging the posture includes: judging the size of the rangefinder values detected by the front left and rear left rangefinder probes installed on the underwater robot, it is determined whether the current posture of the underwater robot is the bow deviating to the left or the bow deviating to the right.
[0042] S3. Complete autonomous navigation control based on the position and posture of the underwater robot.
[0043] Calculate the deflection angle; add the calculated deflection angle to the current heading angle of the underwater robot, and input this result into the PID directional controller as the desired heading to calculate the desired rotation torque required to adjust the heading; and then distribute the rotation torque to the propeller of the underwater robot. 1 The current heading angle is determined by the size of β and is output by the inertial navigation system.
[0044] Autonomous navigation control algorithm Figure 3 As shown, follow these steps:
[0045] If the underwater robot is currently to the left of the central axis:
[0046] If the bow deviates to the left, the deflection angle is α 3 =α 1 +α 2 ; If the bow deviates to the right, there are two cases. At this time, if α 1 ≥β, the deflection angle is α 3 =-1*|α 1 -α 2 |, if α 1 <β, the deflection angle is α 3 =|α 1 -α 2 |.
[0047] If the underwater robot is currently to the right of the center axis:
[0048] If the bow deflects to the right, the deflection angle is α 3 =α 1 +α 2 ; If the bow deviates to the left, there are two cases. At this time, if α 1 ≥β, the deflection angle is α 3 =|α 1 -α 2 |, if α 1 <β, the deflection angle is α 3 =-1*|α 1 -α 2 |.
[0049] In this embodiment, when controlling the underwater robot to navigate, the tunnel radius R, the distance d between the front and rear rangefinders, and the target point A need to be given in advance. 3 (0, p).
[0050] Among them, α 1 is the angle (acute angle) between the underwater robot and the central axis; α 2 α is the angle (acute angle) between the straight line formed by the bow of the underwater robot and the target point and the central axis; 3is the required deflection angle; β is the angle (acute angle) between the straight line formed by the stern of the underwater robot and the target point and the central axis; l 1 , l 2 , l 3 , l 4 They are the rear left, front left, rear right and front right rangefinder distance data respectively.
[0051] In this embodiment:
[0052] When the robot is to the left of the central axis and the bow is tilted to the left, the robot needs to turn right. Figure 4 The double solid line represents the tunnel wall, the solid arrow represents the underwater robot, and the coordinate system is established with the tunnel centerline as the y-axis and the perpendicular line of the tunnel centerline as the x-axis. The coordinate point A can be written as 1 (-[l 3 cos(α 1 )-R]),0),A 2 (-[l 3 cos(α 1 )-R+dsin(α 1 )],dcos(α 1 ))、A 3 (0, p), according to the inverse trigonometric function and cosine formula:
[0053]
[0054] The required deflection angle is:
[0055]
[0056] When the robot is to the left of the central axis and the bow is tilted to the right, the robot needs to turn left. Figure 5 shown.
[0057] At this time α 1 ≥β, the deflection angle is α 3 =-1*|α 1 -α 2 |,
[0058] When the robot is to the left of the central axis and the bow is tilted to the right, the robot needs to turn right. Figure 6 shown.
[0059] At this time α 1 <β, the deflection angle is α 3 =|α 1 -α 2 |.
[0060] When the robot is to the right of the central axis, the bow deviates to the right. When the robot needs to turn left, the deflection angle is α 3 =α 1 +α2 ;
[0061] When the robot is to the right of the central axis, the bow is tilted to the left, and the robot needs to turn left:
[0062] At this time α 1 <β, the deflection angle is α 3 =-1*|α 1 -α 2 |,
[0063] When the robot is to the right of the central axis, the bow is tilted to the left, and the robot needs to turn right:
[0064] At this time α 1 ≥β, the deflection angle is α 3 =|α 1 -α 2 |.
[0065] Embodiment 2
[0066] This embodiment also provides an underwater robot autonomous navigation control system, including: an acquisition module, a judgment module and a control module; the acquisition module is used to collect distance data between the underwater robot probe and the tunnel wall; the judgment module is used to judge the position and posture of the underwater robot based on the distance data; the control module is used to complete the control of autonomous navigation based on the position and posture of the underwater robot.
[0067] The following will describe in detail how the present invention solves the technical problems in practical work in conjunction with this embodiment.
[0068] First, the acquisition module is used to collect the distance data between the underwater robot probe and the tunnel wall. The acquisition module includes four ranging probes, which are installed at the front left, front right, rear left, and rear right of the underwater robot; the distance data between the probe and the tunnel wall is detected by the four ranging probes installed on the underwater robot.
[0069] In addition, a thruster is installed on the front and back sides of the robot.
[0070] Afterwards, the judgment module determines whether the underwater robot is currently located to the left or right of the central axis by judging the size of the rangefinder values detected by the front left and front right rangefinder probes installed on the underwater robot; the method for judging the posture includes: determining whether the current posture of the underwater robot is the bow deviating to the left or the bow deviating to the right by judging the size of the rangefinder values detected by the front left and rear left rangefinder probes installed on the underwater robot.
[0071] Finally, the control module completes the control of autonomous navigation based on the position and posture of the underwater robot.
[0072] Calculate the deflection angle; add the calculated deflection angle to the current heading angle of the underwater robot, and input this result into the PID directional controller as the desired heading to calculate the rotation torque; and then distribute the rotation torque to the propeller of the underwater robot. 1 The current heading angle is determined by the size of β and is output by the inertial navigation system.
[0073] Autonomous navigation control algorithm Figure 3 As shown, follow these steps:
[0074] If the underwater robot is currently to the left of the central axis:
[0075] If the bow deviates to the left, the deflection angle is α 3 =α 1 +α 2 ; If the bow deviates to the right, there are two cases. At this time, if α 1 ≥β, the deflection angle is α 3 =-1*α 1 -α 2 |, if α 1 <β, the deflection angle is α 3 =|α 1 -α 2 |.
[0076] If the underwater robot is currently to the right of the center axis:
[0077] If the bow deflects to the right, the deflection angle is α 3 =α 1 +α 2 ; If the bow deviates to the left, there are two cases. At this time, if α 1 ≥β, the deflection angle is α 3 =|α 1 -α 2 |, if α 1 <β, the deflection angle is α 3 =-1*α 1 -α 2 |.
[0078] In this embodiment, when controlling the underwater robot to navigate, the tunnel radius R, the distance d between the front and rear rangefinders, and the target point A need to be given in advance. 3 (0, p).
[0079] Among them, α 1 is the angle (acute angle) between the underwater robot and the central axis; α 2 α is the angle (acute angle) between the straight line formed by the bow of the underwater robot and the target point and the central axis; 3 is the required deflection angle; β is the angle (acute angle) between the straight line formed by the stern of the underwater robot and the target point and the central axis; l1 , l 2 , l 3 , l 4 They are the rear left, front left, rear right and front right rangefinder distance data respectively.
[0080] In this embodiment:
[0081] When the robot is to the left of the central axis and the bow is tilted to the left, the robot needs to turn right. Figure 4 The double solid line represents the tunnel wall, the solid arrow represents the underwater robot, and the coordinate system is established with the tunnel centerline as the y-axis and the perpendicular line of the tunnel centerline as the x-axis. The coordinate point A can be written as 1 (-[l 3 cos(α 1 )-R]),0),A 2 (-[l 3 cos(α 1 )-R+dsin(α 1 )],dcos(α 1 ))、A 3 (0, p), according to the inverse trigonometric function and cosine formula:
[0082]
[0083] The required deflection angle is:
[0084]
[0085] When the robot is to the left of the central axis and the bow is tilted to the right, the robot needs to turn left. Figure 5 shown.
[0086] At this time α 1 ≥β, the deflection angle is α 3 =-1*|α 1 -α 2 |,
[0087] When the robot is to the left of the central axis and the bow is tilted to the right, the robot needs to turn right. Figure 6 shown.
[0088] At this time α 1 <β, the deflection angle is α 3 =|α 1 -α 2 |.
[0089] When the robot is to the right of the central axis, the bow deviates to the right. When the robot needs to turn left, the deflection angle is α 3 =α 1 +α 2 ;
[0090] When the robot is to the right of the central axis, the bow is tilted to the left, and the robot needs to turn left:
[0091] At this time α 1 <β, the deflection angle is α 3 =-1*|α 1 -α 2 |,
[0092] When the robot is to the right of the central axis, the bow is tilted to the left, and the robot needs to turn right:
[0093] At this time α 1 ≥β, the deflection angle is α 3 =|α 1 -α 2 |.
[0094] The embodiments described above are only descriptions of the preferred embodiments of the present invention and are not intended to limit the scope of the present invention. Without departing from the design spirit of the present invention, various modifications and improvements made to the technical solutions of the present invention by ordinary technicians in this field should all fall within the protection scope determined by the claims of the present invention.
Claims
1. A method for controlling autonomous navigation of an underwater robot, characterized in that the steps include: Collect the distance data between the underwater robot probe and the tunnel wall; Based on the distance data, determining the position and posture of the underwater robot; Based on the position and posture of the underwater robot, autonomous navigation control is completed.
2. The underwater robot autonomous navigation control method according to claim 1, characterized in that: The method for collecting the distance data includes: detecting the distance data between the probe and the tunnel wall by installing four distance measuring probes on the underwater robot; the distance measuring probes are installed at four positions of the underwater robot: front left, front right, rear left, and rear right.
3. The underwater robot autonomous navigation control method according to claim 2, characterized in that: The method for judging the position includes: judging the size of the rangefinder value detected by the front left and front right rangefinder probes installed on the underwater robot, thereby obtaining whether the underwater robot is currently located to the left or right of the central axis; the method for judging the posture includes: judging the size of the rangefinder value detected by the front left and rear left rangefinder probes installed on the underwater robot, thereby obtaining whether the current posture of the underwater robot is the bow deviating to the left or the bow deviating to the right.
4. The underwater robot autonomous navigation control method according to claim 3, characterized in that: The control method for completing autonomous navigation includes: calculating the size of the deflection angle; adding the calculated deflection angle to the current heading angle of the underwater robot, and inputting this result into the PID directional controller as the desired heading, calculating the rotation torque; and then distributing the rotation torque to the propeller of the underwater robot.
5. The underwater robot autonomous navigation control method according to claim 4, characterized in that: When the underwater robot is currently to the right of the central axis: If the bow deviates to the left, the deflection angle is α3=α1+α2; if the bow deviates to the right, there are two cases. At this time, if α1≥β, the deflection angle is α3=-1*|α1-α2|, and if α1<β, the deflection angle is α3=|α1-α2|; among them, α1 is the angle between the underwater robot and the central axis, which is an acute angle; α2 is the angle between the straight line formed by the bow of the underwater robot and the target point and the central axis, which is an acute angle; α3 is the required deflection angle; β is the angle between the straight line formed by the stern of the underwater robot and the target point and the central axis, which is an acute angle.
6. The underwater robot autonomous navigation control method according to claim 5, characterized in that: If the underwater robot is currently to the right of the central axis: If the bow deviates to the right, the deflection angle is α3=α1+α2; if the bow deviates to the left, there are two cases. At this time, if α1≥β, the deflection angle is α3=|α1-α2|, and if α1<β, the deflection angle is α3=-1*|α1-α2|.
7. An underwater robot autonomous navigation control system, the system is used to implement the method according to any one of claims 1 to 6, characterized in that: include: Acquisition module, judgment module and control module; The acquisition module is used to collect the distance data between the underwater robot probe and the tunnel wall; The judgment module is used to judge the position and posture of the underwater robot based on the distance data; The control module is used to complete the control of autonomous navigation based on the position and posture of the underwater robot.
8. The underwater robot autonomous navigation control system according to claim 7, characterized in that: The acquisition module includes four ranging probes, which are installed at four positions of the underwater robot: front left, front right, rear left, and rear right; the distance data between the probes and the tunnel wall is detected by the four ranging probes installed on the underwater robot.
Citation Information
Cited By
Autonomous hole passing method for underwater submersible
CN119322527A
