Method for underwater robot to move along centerline of tunnel
By establishing the thruster layout model of the underwater robot and combining the ranging sonar measurement data, the inertial guide sensor calculates the force difference in each degree of freedom, the underwater robot is realized in the middle line of the tunnel, and the low detection efficiency in traditional methods is solved and high security risks.
Patent Information
- Application Number
- PCT/CN2023/102659
- Authority / Receiving Office
- WO · WO
- Patent Type
- Applications
- Current Assignee / Owner
- Priority Date
- 2023-05-26
- Filing Date
- 2023-06-27
- Publication Date
- 2024-12-05
AI Technical Summary
Traditional underwater robots are difficult to navigate along the middle line independently in narrow tunnels, resulting in problems such as low detection efficiency, high safety risks and high missed detection rates.
By establishing a thruster layout model of the underwater robot, measuring the difference value using the distance measuring components and inertial guide sensors, calculating the force difference in each degree of freedom, and using the PID ring to solve the thruster force to realize the underwater robot's navigation in the middle line of the tunnel control.
Underwater robots are able to independently navigate along the midline in the tunnel, improve detection efficiency, reduce safety risks, and reduce missed detection rates.
Smart Images

Figure CN2023102659_05122024_PF_FP_ABST
Abstract
Description
Method for underwater robot to navigate along tunnel centerline Technical Field
[0001] The present invention relates to the technical field of underwater robots, in particular to a method for an underwater robot to navigate along a tunnel centerline. Background Art
[0002] Water conservancy and hydropower are of great economic and social significance. Defect detection and health assessment of key structures such as diversion tunnels are crucial to the safe operation of large-scale water conservancy and hydropower projects. Traditional inspection methods, such as deep dives by frogmen, climbing ropes after emptying the equipment, and building long ladders, are characterized by long inspection cycles, high safety risks, high rates of missed inspections, and insufficient information. The global underwater robot industry has made significant progress, but the vast majority of applications are in large water bodies such as oceans, lakes, and rivers. Due to the harsh geographical and hydrological conditions, their application in narrow tunnels or confined spaces is still very rare.
[0003] In order to realize the inspection of long-distance, fully enclosed water diversion tunnels and improve the inspection efficiency, a control scheme that can enable underwater robots to autonomously navigate along the center line in the tunnel is essential.
[0004] Summary of the Invention
[0005] The present invention provides a method for an underwater robot to navigate along the centerline of a tunnel, comprising the following steps:
[0006] Step S1: establishing a layout model of the underwater robot propeller;
[0007] Step S2: measuring the difference between the current underwater robot and the tunnel heading angle, pitch angle, vertical center, and horizontal center by a distance measuring component;
[0008] Step S3: The inertial navigation sensor measures the current roll angle and x-axis speed v of the underwater robot. x ;
[0009] Step S4: Calculate the roll angle and the velocity v in the x-axis direction x The difference from the target value, where the target value of the roll angle is 0, v x The target value V dst It is a function of the first four differences, and the differences are used to close the loop and solve the forces on each degree of freedom.
[0010] Step S5: Each thruster propels according to the solved force to achieve motion control of navigation along the centerline of the tunnel.
[0011] Preferably, in step S1, the underwater robot includes four thrusters in the horizontal direction and four thrusters in the vertical direction, which are used to control the underwater robot to achieve six-degree-of-freedom posture adjustment;
[0012] Among them, the four horizontal thrusters include two groups located at the front and two groups located at the rear. The left bow horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot, the right bow horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot, the left stern horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot, and the right stern horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot.
[0013] Preferably, the four horizontal thrusters are located below the center of gravity of the underwater robot.
[0014] Preferably, the thrust axis of the vertical propeller is installed at an angle β to the linear axis and is arranged in vector symmetry with respect to the center of gravity.
[0015] Preferably, the thruster vector arrangement matrix is: B(β,α)
[0016] Preferably, the underwater robot is provided with 9 identical sonars, the first sonar is installed at the bow of the underwater robot to measure the distance to the object in front, the second sonar and the fourth sonar are installed on the left side of the underwater robot, and the third sonar and the fifth sonar are installed on the right side of the underwater robot to measure the distance on both sides from the tunnel; the sixth sonar and the ninth sonar are installed on the top surface of the device, along the x-axis direction of the device, to measure the distance from the top to the tunnel; the seventh sonar and the eighth sonar are installed at the bottom of the device along the y-axis direction, and the seventh sonar is used to measure the distance between the bottom and the inner wall of the tunnel. The seventh sonar and the eighth sonar are combined to assist in measuring the angle between the horizontal direction of the device and the tangent line of the tunnel arc.
[0017] Preferably, in step S4, the difference solution and thrust distribution on the six degrees of freedom include:
[0018] Coarse adjustment of roll angle;
[0019] Six-degree-of-freedom moment calculation;
[0020] Among them, the x-axis torque increment is calculated by the roll angle difference, the y-axis torque increment is calculated by the pitch angle difference, the z-axis torque increment is calculated by the heading angle deviation, the z-axis force increment is calculated by the z-axis distance deviation, the y-axis force increment is calculated by the y-axis distance deviation, and the x-axis force increment is calculated by the x-axis speed deviation.
[0021] Compared with the prior art, the advantages of the present invention are:
[0022] The present invention establishes a vector arrangement model of eight thrusters of an underwater robot, derives a mechanical vector model and a thrust distribution algorithm for the underwater robot's propulsion system, designs an arrangement scheme for nine ranging sonars, utilizes real-time measurement data from the ranging sonar combination, calculates the error values of the underwater robot's left and right positions relative to the centerline, its upper and lower positions relative to the centerline, its tunnel heading angle, its tunnel pitch angle, and its roll angle in the tunnel, uses a PID loop to solve the forces on each degree of freedom of the underwater robot, and sets the thrust of each thruster, thereby achieving the purpose of the underwater robot navigating along the centerline. BRIEF DESCRIPTION OF THE DRAWINGS
[0023] The accompanying drawings are not intended to be drawn to scale. In the drawings, each identical or nearly identical component shown in various figures may be represented by the same reference numeral. For the sake of clarity, not every component is labeled in every figure. Embodiments of various aspects of the present invention will now be described by way of example and with reference to the accompanying drawings, in which:
[0024] FIG1 is a schematic diagram of a propeller layout model shown in the present invention;
[0025] FIG2 is a schematic diagram of a body-attached coordinate system according to the present invention;
[0026] FIG3 a is a top view of the installation arrangement of the horizontal thruster shown in the present invention;
[0027] FIG3 b is a front view of the installation arrangement of the horizontal thruster shown in the present invention;
[0028] FIG3 c is a rear view of the horizontal propeller installation arrangement shown in the present invention;
[0029] FIG4 a is a top view of the vertical thruster installation arrangement shown in the present invention;
[0030] FIG4 b is a front view of the vertical thruster installation arrangement shown in the present invention;
[0031] FIG4c is a rear view of the vertical thruster installation arrangement shown in the present invention;
[0032] FIG5 is a schematic diagram of the thrust breakdown of the horizontal thruster of the present invention;
[0033] FIG6 is a schematic diagram of the decomposition of the thrust of the vertical thruster shown in the present invention;
[0034] FIG7 is a schematic diagram of the distribution of the ranging sonar shown in the present invention;
[0035] FIG8 is a schematic cross-sectional view of the underwater robot of the present invention in a tunnel;
[0036] FIG9 is a top view of the underwater robot shown in the present invention in a tunnel;
[0037] FIG10 is a rear view of the underwater robot shown in the present invention in a tunnel. DETAILED DESCRIPTION
[0038] In order to better understand the technical content of the present invention, specific embodiments are given below in conjunction with the accompanying drawings.
[0039] The requirements for the posture of the equipment when navigating along the center line of the tunnel are that the underwater robot keeps the heading angle and pitch angle consistent with the tunnel (the heading angle and pitch angle of the tunnel) during navigation, the roll angle is 0, and the geometric center of the underwater robot is kept at the vertical midpoint and horizontal midpoint of the tunnel.
[0040] The basic principle of the centerline navigation of the present invention is described as follows: first, an algorithm model for the thrust distribution of the underwater robot is established, and then the distance measuring sonar combination is used to measure the current heading angle, pitch angle, vertical center, and horizontal center difference between the underwater robot and the tunnel, and the inertial navigation sensor installed on the underwater robot is used to measure the current roll angle and x-axis speed v of the underwater robot. x , calculate the roll angle and v x The difference from the target value (the target value of the roll angle is 0, v x The target value V dst It is a function of the first four differences), and the differences (there are six differences in six degrees of freedom) are used to solve the force (torque) on each degree of freedom in a closed loop. Then, the thrusters are set according to the thrust algorithm to realize the centerline navigation motion control.
[0041] The present invention provides a method for an underwater robot to navigate along the centerline of a tunnel, comprising the following steps:
[0042] Step S1: establishing a layout model of the underwater robot propeller;
[0043] Step S2: measuring the difference between the current underwater robot and the tunnel heading angle, pitch angle, vertical center, and horizontal center by a distance measuring component;
[0044] Step S3: The inertial navigation sensor measures the current roll angle and x-axis speed v of the underwater robot. x ;
[0045] Step S4: Calculate the roll angle and the velocity v in the x-axis direction x The difference from the target value, where the target value of the roll angle is 0, v x The target value V dst It is a function of the first four differences, and the differences are used to close the loop and solve the forces on each degree of freedom.
[0046] Step S5: Each thruster propels according to the solved force to achieve motion control of navigation along the centerline of the tunnel.
[0047] Thruster layout model
[0048] As shown in Figure 1, the underwater robot includes four thrusters in the horizontal direction and four thrusters in the vertical direction, which are used to control the underwater robot to achieve six-degree-of-freedom posture adjustment;
[0049] Among them, the four horizontal thrusters include two groups located at the front and two groups located at the rear. The left bow horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot, the right bow horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot, the left stern horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot, and the right stern horizontal thruster is tilted inward and forms an angle α with the longitudinal axis of the underwater robot.
[0050] Preferably, the four horizontal thrusters are located below the center of gravity of the underwater robot.
[0051] The device body coordinate system is established with the mass point of the underwater robot as the origin, the coordinate system ox yz, as shown in Figure 2. The coordinate origin o is taken at the center of mass of the underwater robot, ox points to the front, oy points to the starboard side of the underwater robot, and oz points to the bottom of the underwater robot.
[0052] The underwater robot has six degrees of freedom, namely longitudinal motion along the ox axis, lateral motion along the oy axis, heave motion along the oz axis, roll motion around the ox axis, pitch motion around the oy axis, and yaw motion around the oz axis.
[0053] As shown in Figures 3a-3c, the installation positions and angles of the four horizontal thrusters are shown. T1 is the bow left horizontal thruster, T2 is the bow right horizontal thruster, T3 is the stern left horizontal thruster, and T4 is the stern right horizontal thruster. The horizontal thrusters are installed below the center of gravity of the underwater robot, with their thrust axes β-mounted to the ox axis and arranged in vector symmetry with respect to the center of gravity. The absolute value of the horizontal thruster's coordinate in the body coordinate system is: h 、y h 、z h .
[0054] As shown in Figures 4a-4c, the installation positions and angles of the four vertical thrusters are shown. T5 is the bow left vertical thruster, T6 is the bow right vertical thruster, T7 is the stern left vertical thruster, and T8 is the stern right vertical thruster. The vertical thrusters are installed above the center of gravity of the underwater robot, with their thrust axes α-installed with the oz axis and arranged in vector symmetry with respect to the center of gravity. The absolute value of the vertical thruster's coordinate in the body coordinate system is: x v 、y v 、z v .
[0055] Propulsion system mechanical vector model
[0056] The underwater robot body is subjected to the thrust of four horizontal thrusters, and the angle β between the positive direction of the thrust and the ox axis is β. In Figure 3, T1 is the thrust of thruster No. 1 21, T2 is the thrust of thruster No. 2 22, T3 is the thrust of thruster No. 3 23, and T4 is the thrust of thruster No. 4 25 of the underwater robot.
[0057] After the thrust of the propeller is decomposed, the positive directions of the decomposed forces are shown in Figure 5.
[0058] As shown in Figure 5, T 1x =T1cosβ, T 1y =T1sinβ, T 2x =T2cosβ, T 2y =T2sinβ, T 3x =T3cosβ, T 3y =T3sinβ, T 4x =T4cosβ, T 4y =T4sinβ.
[0059] After thrust decomposition and synthesis, the following relationship can be obtained:
[0060] The sum of the thrust of the horizontal thrusters in the x-axis direction is expressed as: F Xh =T 1x +T 2x -T 3x -T 4x
[0061] The sum of the thrust of the horizontal thrusters in the y-axis direction is expressed as: F yh =T 1y -T 2y +T 3y -T 4y
[0062] The sum of the thrust of the horizontal thrusters in the z-axis direction is expressed as: F zh =0
[0063] The vector torque of the horizontal thruster around the x-axis is expressed as follows: K h =T 1y z h +T 2y z h -T 3y z h +T 4y z h
[0064] The vector torque of the horizontal thruster around the y-axis is expressed as follows: M h =T 1x z h +T 2x zh -T 3x z h -T 4x z h
[0065] The vector torque of the horizontal thruster around the z-axis is expressed as follows: N h =T 1x y h +T 1y x h -T 2x y h -T 2y x h -T 3x y h - T3y x h +T 4x y h +T 4y x h
[0066] The underwater robot body is subjected to the thrust of four vertical thrusters, and the angle α between the positive direction of the thrust and the oz axis is: T5 is the thrust of thruster No. 5 31; T6 is the thrust of thruster No. 6 32; T7 is the thrust of thruster No. 7 34; and T8 is the thrust of thruster No. 8 33.
[0067] After the thrust of the propeller is decomposed, the positive directions of the decomposed forces are shown in Figure 6.
[0068] In Figure 6, T 5y =T5sinα, T 5z =T5cosα, T 6y =T6sinα, T 6z =T6cosα, T 7y =T7sinα, T 7z =T7cosα, T 8y =T8sinα, T 8z =T8cosα. Synthesized by thrust decomposition
[0069] The sum of the thrust of the vertical thrusters in the x-axis direction is expressed as: F Xv =0
[0070] The sum of the thrust of the vertical thrusters in the y-axis direction is expressed as: F yv =-T 5y +T 6y -T 7y +T 8y
[0071] The sum of the thrust of the vertical thrusters in the z-axis direction is expressed as: F zv =T5z +T 6z +T 7z +T 8z
[0072] The sum of the vector torques of the vertical thrusters around the x-axis is expressed as: K v =-T 5y z v -T 5z y v+ T 6y z v -T 6z y v -T 7y z v -T 7z y v +T 8y z v -T 8z y v
[0073] The sum of the vector torques of the vertical thrusters around the y-axis is expressed as: M v =-T 5z x v -T 6z x v +T 7z x v +T 8z x v
[0074] The sum of the vector torques of the vertical thrusters around the z-axis is expressed as: N v =-T 5y x v +T 6y x v +T 7y x v -T 8y x v
[0075] Combined with the above relationship, the thrust vector of the six degrees of freedom generated by the thruster on the underwater robot body can be integrated as follows:
[0076] definition τ is the resultant force (torque) of the 8 thrusters in the six degrees of freedom directions
[0077] Among them F x =Fx h +Fx v =T1cosβ+T2cosβ-T3cosβ-T4cosβ F y =Fy h +Fy v=T1sinβ-T2sinβ+T3sinβ-T4sinβ-T5sinα+T6sinα-T7sinα+T8sinα Fz=Fz h +Fz v =T5cosα+T6cosα+T7cosα+T8cosα K=K h +K v =-T1sinβz h +T2sinβz h -T3sinβz h +T4sinβz h -T5sinαz v -T5cosαy v +T6sinαz v +T6cosαy v -T7sinαz v -T7cosαy v +T8sinαz v +T8cosαy v M=M h +M y =T1cosβz h +T2cosβz h -T3cosβz h -T4cosβz h -T5cosαx v -T6cosαx v +T7cosαx v +T8cosαx v N=N h +N v =T1cosβy h +T1sinβx h -T2cosβy h -T2sinβx h -T3cosβy h -T3sinβx h +T4cosβy h +T4sinβx h -T5sinαx v +T6sinαx v +T7sinαx v -T8sinαx v definition
[0078] If τ=B(β,α)T-----------------(b)
[0079] B(β,α) is the vector arrangement matrix of the thruster. According to formula (a), the B matrix can be obtained as follows:
[0080] Thrust distribution relationship
[0081] According to formula (b), when the thrust of a single thruster is known, how to solve the forces in each dimension of the six degrees of freedom? In actual control, according to the needs of motion control adjustment, the forces (torques) required in each axis are first obtained, and then the forces required to be achieved by each thruster, that is, the thrust distribution, are obtained.
[0082] B(β,α) is actually a fixed value matrix after the underwater robot body design is completed. The pseudo-inverse matrix method can be used to obtain B-1 and define an objective function
[0083] Where N is a positive definite matrix, using Lagrange's rule, and when N is the identity matrix, then B -1 =B T (BB T ) -1
[0084] Thus, when τ is known, the thrust distribution of each thruster is as follows: T = B -1 τ----------(c)
[0085] Ranging sonar combination layout model
[0086] The underwater robot is equipped with 9 identical sonars. The first sonar 41 is installed at the bow of the underwater robot to measure the distance to objects in front. The second sonar 42 and the fourth sonar 44 are installed on the left side of the underwater robot. The third sonar 43 and the fifth sonar 45 are installed on the right side of the underwater robot to measure the distance between the two sides and the tunnel; the sixth sonar 46 and the ninth sonar 49 are installed on the top surface of the equipment, along the x-axis direction of the equipment, to measure the distance from the top to the tunnel; the seventh sonar 47 and the eighth sonar 48 are installed at the bottom of the equipment along the Y-axis direction. The seventh sonar 47 is used to measure the distance between the bottom and the inner wall of the tunnel. The combination of the seventh sonar 47 and the eighth sonar 48 is used to assist in measuring the angle between the horizontal direction of the equipment and the tangent of the tunnel arc.
[0087] Difference solution and thrust distribution in six degrees of freedom
[0088] The applicable conditions of this method are: roll |<θth roll , where θ roll is the roll angle value measured by the inertial sensor in real time, θth roll It is based on a preset roll angle threshold value. When θ rollWhen it is too large, there will be uncertainty in the measurement of ranging sonar in other directions.
[0089] Coarse adjustment of roll angle:
[0090] In motion control, the roll angle is adjusted coarsely first. roll ≥θth roll When the roll angle needs to be adjusted separately, the adjustment method is: according to θ roll Close the loop with the target value 0° to obtain the torque increment in the roll angle direction (around the x-axis direction). The incremental closed-loop calculation formula is as follows: ΔK T =Kp K (E K (k)-E K (k-1))+Ki K E K (k)+Kd K (E K (k)-2E K (k-1)+E K (k-2))
[0091] The force (torque) matrix of the underwater robot is as follows:
[0092] At this time, Fx=0,
[0093] Formula (c) can be used to calculate the distribution value of each thruster.
[0094] Six-degree-of-freedom moment calculation
[0095] When |θ roll |<θth roll , use the height scoring step to solve for the differences in the remaining degrees of freedom:
[0096] a Calculate the x-axis torque increment from the roll angle difference;
[0097] The calculation process is the same as that in the roll angle coarse adjustment, ΔK T =Kp K (E K (k)-E K (k-1))+Ki K E K (k)+Kd K (E K (k)-2E K (k-1)+E K (k-2))
[0098] b. Calculate the y-axis moment increment from the pitch angle difference;
[0099] As shown in Figure 8-10, X1 and X2 are the measurement values of the two longitudinally mounted ranging sonars on the top, and X3 is the measurement value of the bottom ranging sonar. The difference in pitch angle can be calculated by replacing the actual angle deviation value with the difference between X1 and X2. Let EM = X1-X2. When the closed loop is adjusted in real time, ΔM T =Kp M (E M (k)-E M (k-1))+Ki M E M (k)+Kd M (E M (k)-2E M (k-1)+E M (k-2))
[0100] c Calculate the z-axis moment increment ΔN from the heading angle deviation value T =Kp N (E N (k)-E N (k-1))+Ki N E N (k)+Kd N (E N (k)-2E N (k-1)+E N (k-2))
[0101] dCalculate the force increment ΔF in the z-axis direction from the distance deviation in the z-axis direction Z =Kp Z (E Z (k)-E Z (k-1))+Ki Z E Z (k)+Kd Z (E Z (k)-2E Z (k-1)+E Z (k-2))
[0102] eCalculate the force increment ΔF in the y-axis direction from the distance deviation in the y-axis direction Y =Kp Y (E Y (k)-E Y (k-1))+Ki Y E Y (k)+Kd Y (E Y (k)-2E Y (k-1)+E Y (k-2))
[0103] fCalculate the force increment in the x-axis direction ΔFx=Kp from the x-axis velocity deviation X (E X (k)-E X (k-1))+Ki X E X (k)+Kd X (E X (k)-2E X (k-1)+E X (k-2))
[0104] but
[0105] Where: Fx=Fx0+ΔF X ,Fy=Fy0+ΔF Y ,Fz=Fz0+ΔF Z ,K=K0+ΔK T ,M=M0+ΔM T ,N=N0+ΔN T ,
[0106] Among them, Fx0, Fy0, Fz0, K0, M0, and N0 are the setting values of the last force (torque).
[0107] The thrust distribution relationship (c) can be used to calculate the thrust of each thruster.
[0108] In combination with the above embodiments, the present invention establishes a vector arrangement model of the underwater robot's eight thrusters, derives a mechanical vector model and thrust distribution algorithm for the underwater robot's propulsion system, designs an arrangement scheme for nine ranging sonars, and utilizes the ranging sonar combination to measure real-time data. The error values of the underwater robot's left and right positions relative to the centerline, its upper and lower positions relative to the centerline, its tunnel heading angle, its tunnel pitch angle, and its roll angle in the tunnel are calculated. The PID loop is used to solve the forces on each degree of freedom of the underwater robot, and the thrust of each thruster is set, thereby achieving the purpose of the underwater robot navigating along the centerline.
[0109] While the present invention has been disclosed above with reference to preferred embodiments, this is not intended to limit the present invention. Persons skilled in the art will readily appreciate that various modifications and variations can be made without departing from the spirit and scope of the present invention. Therefore, the scope of protection of the present invention shall be determined by the claims.
Claims
1. A method for an underwater robot to navigate along the centerline of a tunnel, characterized in that: The following steps are involved: Step S1, establishing a layout model of the underwater robot thruster; Step S2, measuring the difference between the current underwater robot and the tunnel heading angle, pitch angle, vertical center, and horizontal center through a distance measuring component; Step S3: The inertial navigation sensor measures the current roll angle and x-axis speed v of the underwater robot. x ; Step S4, calculate the rolling angle and the speed v in the x-axis direction x The difference from the target value, where the target value of the roll angle is 0, v x The target value V dst It is a function of the first four differences, and the obtained differences are used to close the loop to solve the forces on each degree of freedom; Step S5: Each thruster propels according to the solved force to achieve motion control of navigation on the centerline of the tunnel.
2. The method for an underwater robot to navigate along a tunnel centerline according to claim 1, characterized in that: In step S1, the underwater robot includes four thrusters in the horizontal direction and four thrusters in the vertical direction, which are used to control the underwater robot to achieve six-degree-of-freedom posture adjustment; Among them, the four horizontal thrusters include two groups located at the front and two groups located at the rear. The left bow horizontal thruster is tilted inward at an angle α to the longitudinal axis of the underwater robot, the right bow horizontal thruster is tilted inward at an angle α to the longitudinal axis of the underwater robot, the left stern horizontal thruster is tilted inward at an angle α to the longitudinal axis of the underwater robot, and the right stern horizontal thruster is tilted inward at an angle α to the longitudinal axis of the underwater robot.
3. The method for an underwater robot to navigate along a tunnel centerline according to claim 2, characterized in that: Four horizontal thrusters are located below the center of gravity of the underwater robot.
4. The method for an underwater robot to navigate along a tunnel centerline according to claim 1, characterized in that: The thrust axis of the vertical propeller is installed at an angle β to the linear axis and is arranged in vector symmetry with respect to the center of gravity.
5. The method for an underwater robot to navigate along a tunnel centerline according to claim 1, characterized in that: The vector arrangement matrix of the thruster: B(β,α) 6. The method for an underwater robot to navigate along a tunnel centerline according to claim 1, characterized in that: The underwater robot is equipped with 9 identical sonars. The first sonar is installed at the bow of the underwater robot to measure the distance of the object in front. The second and fourth sonars are installed on the left side of the underwater robot. The third and fifth sonars are installed on the right side of the underwater robot to measure the distance from the tunnel on both sides. The sixth and ninth sonars are installed on the top surface of the device, along the x-axis direction of the device, to measure the distance from the top to the tunnel. The seventh sonar and the eighth sonar are installed at the bottom of the equipment along the Y-axis direction. The seventh sonar is used to measure the distance between the bottom and the inner wall of the tunnel. The seventh sonar and the eighth sonar are combined to assist in measuring the angle between the horizontal direction of the equipment and the tangent line of the tunnel arc.
7. The method for an underwater robot to navigate along a tunnel centerline according to claim 1, characterized in that: In step S4, the difference solution and thrust distribution on the six degrees of freedom include: Coarse adjustment of roll angle; Six degrees of freedom moment calculation; Among them, the x-axis torque increment is calculated by the roll angle difference, the y-axis torque increment is calculated by the pitch angle difference, the z-axis torque increment is calculated by the heading angle deviation, the z-axis force increment is calculated by the z-axis distance deviation, the y-axis force increment is calculated by the y-axis distance deviation, and the x-axis force increment is calculated by the x-axis speed deviation.
Citation Information
Patent Citations
Water tunnel detection robot and control method thereof
CN113650763A
All-attitude motion control method of underwater robot based on propeller vector layout
CN114217521A
Underwater robot positioning method and device, underwater robot and computer equipment
CN114877897A
Underwater robot adaptive control method and device based on double closed-loop control, computer equipment and storage medium
CN115562315A
Three-dimensional real-time collision avoidance method for autonomous underwater robot
CN116027795A
Cited By
Benthic submersible of deep sea hydrothermal sampling system and state control method
CN120507956A
Deep-sea hydrothermal sampling system benthic submersible and state control method
CN120507956B
