Autonomous line patrol motion control method of underwater cable detection robot

Abstract

The invention relates to the technical field of underwater robot control, in particular to an autonomous line patrol motion control method of an underwater cable detection robot, which comprises the following steps of: establishing a six-degree-of-freedom kinematics equation and a kinetic model of the robot, and collecting position information of the robot and a current heading angle of the robot; the target heading angle of the robot is calculated through an optimal deflection angle fusion algorithm, the current heading angle, collected by a sensor, of the robot is compared with the target heading angle, an error epsi is obtained, and the error epsi is added into a single-ring PID algorithm for adjustment; and controlling the thrust and direction of a propeller in the underwater cable detection robot according to a control signal tau (t) output by the single-loop PID algorithm and robot position information, judging whether a target heading angle of the underwater cable detection robot is consistent with a current heading angle, if so, completing autonomous line patrol, otherwise, continuing to correct, and if not, completing the detection of the underwater cable detection robot. The attitude control precision and the path navigation stability of the robot in the underwater environment are improved.

Description

Technical Field

[0001] The present invention relates to the technical field of underwater robot control, and in particular to an autonomous line-patrolling motion control method of an underwater cable detection robot. Background Art

[0002] Cable trenches are specialized underground channels used for laying and protecting cables. They are typically designed with a certain width and depth to provide a safe, reliable, and orderly laying environment for the cables, ensuring they are protected from external interference and damage during operation. However, due to their unique geographical location, some areas experience significant year-round waterlogging. Within cable trenches, water flows at high speeds and is mixed with a variety of complex impurities, creating undercurrent disturbances. This environment poses a severe challenge to the detection stability of robots, significantly impacting their safe operation and the stability of their inspection results. Furthermore, with the increasing demand for cable maintenance, the requirements for the maneuverability and maneuverability of underwater robots are becoming increasingly stringent. During their missions, underwater robots are not only required to follow predetermined trajectories despite environmental disturbances, but in many cases, they also require more detailed observation and manipulation of the target object. Existing methods for underwater cable trench inspections mostly rely on manual inspections to identify and record safety hazards. With the development of underwater cable maintenance needs, robots have gradually been applied to this field. The bow of the robot is equipped with a visual sensor, which analyzes the image data obtained by the visual sensor and performs autonomous inspections. However, due to the poor water quality of the underwater cable trench, the clarity of the images transmitted back is poor, resulting in large calculation errors or even inability to calculate.

[0003] In summary, there is an urgent need for an autonomous line patrol motion control method for an underwater cable detection robot to solve the problems in the existing technology. Summary of the Invention

[0004] At present, underwater cable inspection mainly relies on manual or robot visual inspection, but the manual efficiency is low and the risk is high. The turbid underwater environment leads to inaccurate robot visual recognition, large calculation amount, and poor adaptability of a single sensor. The purpose of the present invention is to provide an autonomous line patrol motion control method for an underwater cable inspection robot. The method adopts a multi-sensor fusion method to obtain the robot's absolute position information and heading information and combines the robot's mathematical model with the heading control algorithm to realize the autonomous line patrol motion control of the underwater cable inspection robot.

[0005] The technical solution adopted by the present invention is an autonomous line patrol motion control method of an underwater cable detection robot, the method comprising: S1: establishing a six-degree-of-freedom kinematic equation and a dynamic model of the underwater cable detection robot;

[0006] S2: The robot position information and the current heading angle of the robot are collected through sensors deployed on the robot, and the robot position information is input into the robot;

[0007] S3: Calculate the target heading angle of the robot using the optimal deflection angle fusion algorithm. Compare the current heading angle of the robot collected by the sensor with the target heading angle calculated by the optimal deflection angle fusion algorithm to obtain the error e between the target heading angle and the current heading angle. ψ ;

[0008] S4: Error e between the target heading angle and the current heading angle of the underwater cable detection robot ψ , add single-loop PID algorithm for adjustment;

[0009] S5: Control the thrust and direction of the propeller in the underwater cable detection robot based on the control signal τ(t) output by the single-loop PID algorithm and the robot's position information;

[0010] S6: Determine whether the target heading angle of the underwater cable detection robot is consistent with the current heading angle. If the target heading angle is consistent with the current heading angle, execute S7; otherwise, execute S3.

[0011] S7: The underwater cable inspection robot reaches the target heading angle and realizes autonomous cable inspection.

[0012] Furthermore, the process of constructing the six-degree-of-freedom kinematic equation of the underwater cable detection robot in S1 is specifically as follows:

[0013] S1.1: In the inertial coordinate system, establish the robot position vector function relationship, as shown in formula (1), establish the robot posture vector function relationship, as shown in formula (2), and combine the robot position vector function relationship and the robot posture vector function relationship into the first 6-dimensional column vector, as shown in formula (3):

[0014] η1=[xyz] T (1)

[0015] Where: η1 is the robot position vector in the inertial coordinate system, x is the displacement along the x-axis, y is the displacement along the y-axis, and z is the displacement along the z-axis; T is the transpose;

[0016] η2=[φ θ ψ] T (2)

[0017] Where: η2 is the robot attitude vector in the inertial coordinate system, φ is the roll angle, θ is the pitch angle, and ψ is the heading angle;

[0018] η=[η1 η2] T (3)

[0019] Where: η is the combined 6-dimensional column vector of the inertial system state variables;

[0020] S1.2: In the carrier coordinate system, establish the robot linear velocity vector function relationship, as shown in equation (4), establish the robot angular velocity vector function relationship, as shown in equation (5), and combine the linear velocity vector function relationship and the angular velocity vector function relationship into a second 6-dimensional column vector, as shown in equation (6):

[0021] ν1=[uvw] T (4)

[0022] Where: ν1 is the linear velocity vector, u is the velocity along the x-axis, v is the velocity along the y-axis, and w is the velocity along the z-axis;

[0023] ν2=[pqr] T (5)

[0024] Where: ν2 is the angular velocity vector, p is the angular velocity of rotation around the x-axis, q is the angular velocity of rotation around the y-axis, and r is the angular velocity of rotation around the z-axis;

[0025] ν=[ν1 ν2] T (6)

[0026] Where: ν is the velocity vector of the robot;

[0027] S1.3: In the carrier coordinate system, establish the robot force vector function relationship, as shown in equation (7), establish the robot torque vector function relationship, as shown in equation (8), and combine the robot force vector function relationship and the robot torque vector function relationship into a third 6-dimensional column vector, as shown in equation (9):

[0028] τ1=[XYZ] T (7)

[0029] Where: τ1 is the force vector representation of the robot, X is the force along the x-axis, Y is the force along the y-axis, and Z is the force along the z-axis;

[0030] τ2=[KMN] T (8)

[0031] Where: τ2 is the torque vector representation of the robot, K is the torque around the x-axis, M is the torque around the y-axis, and N is the torque around the z-axis;

[0032] τ=[τ1 τ2] T (9)

[0033] Where: τ is the force and torque vector of the robot;

[0034] S1.4: Define the linear velocity transformation matrix function relationship from the robot carrier coordinate system to the inertial coordinate system, as shown in equation (10):

[0035]

[0036] in: is the linear velocity transformation matrix, R z,ψ is the rotation matrix around the z axis, R y,θ is the rotation matrix around the y-axis, R x,φ is the rotation matrix around the x-axis, i is the origin in the inertial coordinate system, and b is the center of the body in the carrier coordinate system;

[0037] S1.5: Definition The linear velocity transformation matrix function relationship is shown in formula (11), and the linear velocity rotation matrix J1(η2) function relationship of the robot is defined as shown in formula (12):

[0038]

[0039]

[0040] S1.6: Establish the linear velocity vector ν1 in the carrier coordinate system and the linear velocity vector in the inertial coordinate system The functional relationship is shown in formula (13). According to the projection relationship of the angular velocity vector ν2 in the carrier coordinate system, the functional relationship of the angular velocity vector ν2 in the robot carrier coordinate system is obtained, as shown in formula (14):

[0041]

[0042] in: is the rolling angular velocity, is the pitch angular velocity, is the heading angular velocity;

[0043] S1.7: Define the transformation matrix J, as shown in Equation (15), which transforms the position and attitude change rate of the robot in the inertial coordinate system into By establishing a connection with the robot's velocity vector ν in the carrier coordinate system, the six-degree-of-freedom kinematic equation of the underwater cable detection robot is obtained, as shown in Equation (16):

[0044]

[0045] Where: 0 3×3 is a 3×3 zero matrix, J1 is the linear velocity rotation matrix, and J2 is the angular velocity rotation matrix;

[0046]

[0047] Furthermore, the process of constructing the dynamic model of the underwater cable detection robot in S1 is as follows:

[0048] S2.1: Based on prior knowledge, the robot's six-degree-of-freedom dynamic equation is obtained, as shown in Equation (17):

[0049]

[0050] Where: M is the generalized mass matrix, is the derivative of the velocity vector v, C(v) is the hydrodynamic Coriolis and centripetal force matrix, and D(v b ) is the nonlinear fluid hydrodynamic damping matrix, g(η) is the restoring force and torque vector generated by gravity and buoyancy, τ a is the control input vector, τ a =[T X T Y T Z T K T M T N ] T is the control force on 6 degrees of freedom, τ d Various interference vectors brought by the external environment, τ d =R 6×1 , R represents a real number, R 6×1 represents a 6x1 column vector;

[0051] S2.2: Define the functional relationship of the generalized mass matrix M, as shown in Equation (18), the functional relationship of the hydrodynamic Coriolis and centripetal force matrices C(v), as shown in Equation (19), the functional relationship of the nonlinear fluid hydrodynamic damping matrix, as shown in Equation (20), and the functional relationship of the restoring force and torque vector g(η) generated by gravity and buoyancy, as shown in Equation (21)

[0052] M=M RB +M A (18)

[0053] Where: M RB is the rigid body inertia matrix, M A is the additional mass matrix, M is the generalized mass matrix;

[0054] C(v)=C RB (v)+C A (v) (19)

[0055] Where: C(v) is the hydrodynamic Coriolis and centripetal force matrix, C RB (v) is the Coriolis matrix, C A (v) is the Coriolis and centripetal force matrix caused by the added mass;

[0056] D(v b )=-diag{X u ,Y v ,Z w ,K p ,M q ,N r}-diag{X |u|u |u|,Y |v|v |v|,Z |w|w |w|,K |p|p |p|,M |q|q |q|,N |r|r |r|} (20)

[0057] Where: X u 、Y v 、Z w represents the linear friction coefficient caused by the laminar boundary layer when the robot translates along the x, y, and z axes, K p 、M q 、N r represents the linear friction coefficient caused by the laminar boundary layer when the robot rotates along the x, y, and z axes. |u|u 、Y |v|v 、Z |w|w represents the secondary friction coefficient caused by the turbulent boundary layer when the robot translates along the x, y, and z axes, K |p|p 、M |q|q 、N |r|r represents the secondary friction coefficient caused by the turbulent boundary layer when the robot rotates along the x, y, and z axes. |u|, |v|, |w|, |p|, |q|, and |r| represent the absolute values of u, v, w, p, q, and r. diag is a diagonal matrix.

[0058]

[0059] Where: W is the gravity of the underwater cable detection robot, B is the buoyancy center of the robot, x B is the position of the buoyancy center on the x-axis in the carrier coordinate system, y B is the position of the buoyancy center on the y-axis in the carrier coordinate system, z B is the position of the buoyancy center on the y-axis in the carrier coordinate system;

[0060] S2.3: Define the rigid body inertia matrix M RB The functional relationship of is shown in formula (22), and the additional mass matrix M is defined as A The functional relationship of is shown in formula (23), and the Coriolis matrix C is defined as RB The functional relationship of (v) is shown in Equation (24), which defines the Coriolis and centripetal force matrices C caused by the additional mass. AThe functional relationship of (v) is shown in formula (25):

[0061]

[0062] Where m is the mass of the robot, z g 、y g 、x g is the three-dimensional position of the robot’s center of gravity relative to the robot’s mass center, I x , I y , I z , I xy , I xz , I yz is the moment of inertia of the robot, I xy represents the product moment of inertia of the robot x, y;

[0063]

[0064] in: N is the additional mass coefficient when the robot moves linearly along the z, y, and x axes of the robot. r is the additional inertia of the robot rotating along the z-axis, is the additional inertia of the robot rotating along the y-axis, is the additional inertia of the robot rotating along the x-axis;

[0065]

[0066] S2.4: Based on the distribution structure of the robot's propeller thrusters and the distance between the thrusters and the center of the body, calculate the force and torque of each thruster on each degree of freedom of the body to obtain the control input vector τ a The functional relationship is shown in formula (26):

[0067]

[0068] Where: s() represents the sin() function, c() represents the cos() function, T1, T2, T3, T4, T5, T6 represent the thrust of each propeller, T X 、T Y 、T Z 、T K 、T M 、T N is the control force on 6 degrees of freedom;

[0069] S2.5: Assume that the pitch angle and roll angle of the robot are 0 during the fixed-depth motion. Substitute equations (18)-(26) into equation (17) to obtain the functional relationship of the robot dynamic model, as shown in equation (27):

[0070]

[0071] Furthermore, the specific steps of S2 are: using IMU to collect the current heading angle data of the robot, and using Kalman filtering to filter the heading angle data of the robot at the current moment, using GPS to collect the current position information of the robot, using a depth meter to collect the current depth information of the robot, using Kalman filtering to fuse the current position information and current depth information of the robot to obtain the absolute position information of the robot, and inputting the absolute position information of the robot into the robot.

[0072] Furthermore, the specific steps of S3 are:

[0073] S3.1: The robot is equipped with four ranging sonars. It uses these four ranging sonars to obtain the distance data between the sonar and the trench wall one by one at a dynamic sampling frequency. Let the current heading angle of the robot be ψ FOG , the left front ranging sonar data is d lf , the left rear ranging sonar data is d lb , the right front ranging sonar data is d rf , the right rear ranging sonar data is d rb , the distance between the front and rear sonars is l, and the roll angle is The robot's longitudinal velocity is v x ;

[0074] S3.2: Calculate the angle Δψ between the robot and the trench wall based on the left-side ranging sonar data. l , as shown in formula (28), and the angle Δψ between the robot and the trench wall obtained based on the right-side ranging sonar data r As shown in formula (29);

[0075]

[0076] S3.3: Let the mean value of the left ranging sonar be d l Function expression, as shown in formula (30), the right side ranging sonar mean d r The function expression of is shown in formula (31), and the weight of the angle calculated by the left ranging sonar is ω l , the angle weight calculated by the ranging sonar on the right is ω r , weight ω l With weight ω r The functional relationship is shown in formula (32):

[0077] d l =(d lf +d lb ) / 2 (30)

[0078] d r =(d rf +drb ) / 2 (31)

[0079] ω l +ω r =1 (32)

[0080] S3.4: Conduct a water tank experiment to obtain the accuracy σ of the left sonar angle measurement when the robot is at different distances from the trench wall. l The accuracy of the angle measured with the right sonar σ r The real-time distance between the robot and the trench wall, and the angle weight ω calculated by the error probability model for the left and right sonars according to the real-time distance l 、ω r Correction is performed to obtain the dynamically corrected weights, and the angles Δψ calculated by the left and right sonars are calculated using the dynamically corrected weights. l , Δψ r Perform weighted summation to obtain the fused angle data. Based on the fused angle data, calculate the angle Δψ between the robot and the trench wall at the current moment, as shown in formula (33):

[0081]

[0082] S3.5: Calculate the target heading angle ψ of the robot at the current moment d , as shown in formula (34), calculate the error e between the target heading angle and the current heading angle ψ , as shown in formula (35):

[0083]

[0084] Where: FOG is the bow phase angle at the current center of mass of the robot;

[0085] e ψ =ψ d -ψ FOG (35).

[0086] Furthermore, the dynamic sampling frequency calculation method in S3.1 is:

[0087] S3.1.1: Calculate the error e between the robot's target heading angle and the current heading angle in real time ψ ;

[0088] S3.1.2: Substitute the error e obtained in S3.1.1 ψ Compared with the preset error, if the error e ψ If the error is greater than the preset error, the dynamic adjustment strategy is executed. ψ If the error is less than or equal to the preset error, the ranging sonar sampling frequency is reduced to the minimum frequency.

[0089] Furthermore, the dynamic adjustment strategy in S3.1.2 is to increase the ranging sonar sampling frequency on the side of the robot close to the ditch wall, and at the same time reduce the ranging sonar sampling frequency on the side of the robot far from the ditch wall.

[0090] The application of the technical solution of the present invention has the following beneficial effects:

[0091] 1) Establish a mathematical model for the robot to accurately describe its underwater motion and effectively suppress heading deviations caused by water disturbances or inertial drift;

[0092] 2) Introducing multi-source information fusion to integrate data from multiple sensors, enabling the control strategy to have the advantages of fast response speed and high control accuracy, thereby improving the system's adaptability to complex underwater environments;

[0093] 3) Build a closed-loop control architecture based on the target heading, so that the robot can continuously maintain an ideal linear patrol state in cable trenches with complex environments and significant water disturbances, laying a stable posture and motion foundation for subsequent operations such as target recognition, fault detection, and image acquisition. BRIEF DESCRIPTION OF THE DRAWINGS

[0094] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. 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 any creative work.

[0095] Figure 1 Schematic diagram of the coordinate system of the underwater cable detection robot of the present invention;

[0096] Figure 2 This is a schematic diagram of calculating the target heading angle of the underwater cable detection robot of the present invention;

[0097] Figure 3 This is a schematic diagram of the control system structure of the underwater cable detection robot of the present invention;

[0098] Figure 4 This is a flow chart of target heading calculation of the underwater cable detection robot of the present invention;

[0099] Figure 5 Schematic diagram of propeller arrangement of underwater cable detection robot of the present invention;

[0100] Figure 6 It is the PID (Proportional-Integral-Derivative) heading controller of the present invention;

[0101] Figure 7This is a curve diagram of the heading control process using the PID algorithm of the present invention;

[0102] Figure 8 This is a diagram of the heading data of the underwater cable detection robot of the present invention. DETAILED DESCRIPTION

[0103] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. 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 making creative efforts are within the scope of protection of the present invention.

[0104] like Figures 1-8 As shown, this embodiment discloses an autonomous line patrol motion control method for an underwater cable detection robot, comprising the following steps:

[0105] S1: Establish the six-degree-of-freedom kinematic equations and dynamic model of the underwater cable detection robot;

[0106] In this embodiment, the robot's center of gravity is downward, at its centroid. The robot's frame utilizes a symmetrical, self-stabilizing structure to prevent the robot from rolling or tipping over under the impact of water flow. The robot's exterior design utilizes a box-like frame structure to reduce the robot's water resistance. The robot's layout utilizes a self-stabilizing structure with the center of gravity positioned downward, with heavier thrusters and propellers installed at the bottom of the robot in a tumbler-like design. This positions the center of gravity below the center of buoyancy, maintaining stability. The robot's motion is driven by six thrusters. Two thrusters are located symmetrically about the central axis, controlling the robot's ascent, descent, and pitch. Four thrusters are located at the bottom of the frame, controlling the robot's direction of motion: forward, backward, and spin.

[0107] The robot is equipped with four ranging sonars on the left front, left rear, right front and right rear. The robot is also equipped with an IMU (Inertial Measurement Unit), a GPS (Global Positioning System) and a depth meter.

[0108] exist Figure 1 Under the conditions of the underwater cable detection robot coordinate system shown in FIG, the six-degree-of-freedom kinematics and dynamics model of the underwater cable detection robot is established.

[0109] In the inertial coordinate system, with G as the origin, the robot position vector function relationship is established, as shown in formula (1), the robot attitude vector function relationship is established, as shown in formula (2), and the robot position vector function relationship and the robot attitude vector function relationship are combined into the first 6-dimensional column vector, as shown in formula (3):

[0110] η1=[xyz] T (1)

[0111] Where: η1 is the robot position vector in the inertial coordinate system, x is the displacement along the x-axis, y is the displacement along the y-axis, and z is the displacement along the z-axis;

[0112] η2=[φ θ ψ] T (2)

[0113] Where: η2 is the robot attitude vector in the inertial coordinate system, φ is the roll angle, θ is the pitch angle, and ψ is the heading angle;

[0114] η=[η1 η2] T (3)

[0115] Where: η is the combined 6-dimensional column vector of the inertial system state variables;

[0116] In the carrier coordinate system, with E as the origin, the robot linear velocity vector function relationship is established, as shown in formula (4), the robot angular velocity vector function relationship is established, as shown in formula (5), and the linear velocity vector function relationship and the angular velocity vector function relationship are combined into a second 6-dimensional column vector, as shown in formula (6):

[0117] ν1=[uvw] T (4)

[0118] Where: ν1 is the linear velocity vector, u is the velocity along the x-axis, v is the velocity along the y-axis, and w is the velocity along the z-axis;

[0119] ν2=[pqr] T (5)

[0120] Where: ν2 is the angular velocity vector, p is the angular velocity of rotation around the x-axis, q is the angular velocity of rotation around the y-axis, and r is the angular velocity of rotation around the z-axis;

[0121] ν=[ν1 ν2] T (6)

[0122] Where: ν is the velocity vector of the robot;

[0123] In the carrier coordinate system, the robot force vector function relationship is established as shown in formula (7), and the robot torque vector function relationship is established as shown in formula (8). The robot force vector function relationship and the robot torque vector function relationship are combined into a third 6-dimensional column vector, as shown in formula (9):

[0124] τ1=[XYZ] T (7)

[0125] Where: τ1 is the force vector representation of the robot, X is the force along the x-axis, Y is the force along the y-axis, and Z is the force along the z-axis;

[0126] τ2=[KMN] T (8)

[0127] Where: τ2 is the torque vector representation of the robot, K is the torque around the x-axis, M is the torque around the y-axis, and N is the torque around the z-axis;

[0128] τ=[τ1 τ2] T (9)

[0129] Where: τ is the force and torque vector of the robot;

[0130] Define the linear velocity transformation matrix function relationship from the robot carrier coordinate system to the inertial coordinate system, as shown in formula (10):

[0131]

[0132] in: is the linear velocity transformation matrix, R z,ψ is the rotation matrix around the z axis, R y,θ is the rotation matrix around the y-axis, R x,φ is the rotation matrix around the x-axis, i is the origin in the inertial coordinate system, and b is the center of the body in the carrier coordinate system;

[0133] definition The linear velocity transformation matrix function relationship is shown in formula (11), and the linear velocity rotation matrix J1(η2) function relationship of the robot is defined as shown in formula (12):

[0134]

[0135] Establish the linear velocity vector ν1 in the carrier coordinate system and the linear velocity vector in the inertial coordinate system The functional relationship of , as shown in formula (13), according to the projection relationship of the angular velocity vector ν2 in the carrier coordinate system, the functional relationship of the angular velocity vector ν2 in the robot carrier coordinate system is obtained, as shown in formula (14):

[0136]

[0137] in: is the rolling angular velocity, is the pitch angular velocity, is the heading angular velocity;

[0138] Define the transformation matrix J, as shown in formula (15), which transforms the position and attitude change rate of the robot in the inertial coordinate system into Establish a connection with the robot's velocity vector ν in the carrier coordinate system to obtain the six-degree-of-freedom kinematic equation of the underwater cable detection robot;

[0139]

[0140] Where: 0 3×3 is a 3×3 zero matrix, J1 is the linear velocity rotation matrix, and J2 is the angular velocity rotation matrix;

[0141]

[0142] Based on prior knowledge, the six-degree-of-freedom dynamic equation of the robot is obtained as shown in formula (17):

[0143]

[0144] Where: M is the generalized mass matrix, is the derivative of the velocity vector v, C(v) is the hydrodynamic Coriolis and centripetal force matrix, and D(v b ) is the nonlinear fluid hydrodynamic damping matrix, g(η) is the restoring force and torque vector generated by gravity and buoyancy, τ a is the control input vector, τ a =[T X T Y T Z T K T M T N ] T Control force on 6 degrees of freedom, τ d ,various interference vectors brought by the external environment;

[0145] Define the functional relationship of the generalized mass matrix M as shown in Equation (18), the functional relationship of the hydrodynamic Coriolis and centripetal force matrix C(v) as shown in Equation (19), and the nonlinear fluid hydrodynamic damping matrix D(v b ), as shown in Equation (20), and the functional relationship between the restoring force and the torque vector g(η) generated by gravity and buoyancy is shown in Equation (21)

[0146] M=M RB +MA (18)

[0147] Where: M RB is the rigid body inertia matrix, M A is the additional mass matrix, M is the generalized mass matrix;

[0148] C(v)=C RB (v)+C A (v) (19)

[0149] Where: C(v) is the hydrodynamic Coriolis and centripetal force matrix, C RB (v) is the Coriolis matrix, C A (v) is the Coriolis and centripetal force matrix caused by the added mass;

[0150] D(v b )=-diag{X u ,Y v ,Z w ,K p ,M q ,N r}-diag{X |u|u |u|,Y |v|v |v|,Z |w|w |w|,K |p|p |p|,M |q|q |q|,N |r|r |r|} (20)

[0151] Where: X u 、Y v , Z w represents the linear friction coefficient caused by the laminar boundary layer when the robot translates along the x, y, and z axes, K p 、M q 、N r represents the linear friction coefficient caused by the laminar boundary layer when the robot rotates along the x, y, and z axes. |u|u 、Y |v|v , Z |w|w represents the secondary friction coefficient caused by the turbulent boundary layer when the robot translates along the x, y, and z axes, K |p|p 、M |q|q 、N |r|r Represents the secondary friction coefficient caused by the turbulent boundary layer when the robot rotates along the x, y, and z axes. |u|, |v|, |w|, |p|, |q|, and |r| represent the absolute values of u, v, w, p, q, and r.

[0152]

[0153] Where: W is the gravity of the underwater cable detection robot, B is the buoyancy center of the robot, xB is the position of the buoyancy center on the x-axis in the carrier coordinate system, y B is the position of the buoyancy center on the y-axis in the carrier coordinate system, z B is the position of the buoyancy center on the y-axis in the carrier coordinate system.

[0154] Define the rigid body inertia matrix M RB The functional relationship of is shown in formula (22), and the additional mass matrix M is defined as A The functional relationship of is shown in formula (23), and the Coriolis matrix C is defined as RB (v) is shown in formula (24), and C is defined as A The functional relationship of (v) is shown in formula (25):

[0155]

[0156] Where m is the mass of the robot, z g 、y g 、x g is the three-dimensional position of the robot’s center of gravity relative to the robot’s mass center, I x , I y , I z , I xy , I xz , I yz is the moment of inertia of the robot, I xy Represents the x- and y-product moment of inertia of the robot.

[0157]

[0158] in: N is the additional mass coefficient when the robot moves linearly along the z, y, and x axes of the robot. r is the additional inertia of the robot rotating along the z-axis, is the additional inertia of the robot rotating along the y-axis, is the additional inertia of the robot rotating along the x-axis;

[0159]

[0160] Based on the robot propeller thruster distribution structure (such as Figure 5 As shown), according to the distance between the propeller and the center of the body, the force and torque of each propeller on each degree of freedom of the body are calculated to obtain the control input vector τ a The functional relationship is shown in formula (26):

[0161]

[0162] Assuming that the pitch angle and roll angle of the robot in the fixed depth motion are 0, substitute equations (18)-(26) into equation (17) to obtain the functional relationship of the robot dynamic model, as shown in equation (27):

[0163]

[0164] S2: The robot position information and the current heading angle of the robot are collected through sensors deployed on the robot, and the robot position information is input into the robot;

[0165] In this embodiment, if Figure 3 As shown in the figure, the robot collects the current heading angle data through the onboard IMU. Due to the noise interference in the collection process, the Kalman filter algorithm is used to filter the data to obtain a more accurate heading angle. At the same time, the current position information of the robot is collected by GPS, and the current depth information is collected by the depth meter. The Kalman filter algorithm is also used to fuse the position information and depth information, and finally the absolute position information of the robot is obtained, and this information is input into the robot control system.

[0166] S3: Calculate the target heading angle of the robot using the optimal deflection angle fusion algorithm. Compare the current heading angle of the robot collected by the sensor with the target heading angle calculated by the optimal deflection angle fusion algorithm to obtain the error e between the target heading angle and the current heading angle. ψ ;

[0167] In this embodiment, the robot is equipped with four ranging sonars, which acquire the distance data between the robot and the trench wall in real time at a dynamic sampling frequency. Figure 2 It can be seen that the current heading angle of the robot is ψ FOG , the left front, left rear, right front, and right rear ranging sonar data are d lf d lb d rf d rb , the distance between the front and rear sonars is l, and the roll angle is The robot's longitudinal velocity is v x The dynamic sampling frequency is adjusted based on the error between the robot's target heading angle and the current heading angle, e ψ , when e ψ When the error is greater than the preset error, a dynamic adjustment strategy is executed, that is, the sampling frequency of the ranging sonar on the side close to the trench wall is increased, while the sampling frequency on the side far from the trench wall is reduced, so as to accurately obtain key information; when e ψ When the error is less than or equal to the preset error, the ranging sonar sampling frequency is reduced to the minimum frequency, thereby reducing energy consumption while ensuring data validity.

[0168] According to the acquired ranging sonar data, calculate the angle Δψ between the robot and the trench wall based on the left ranging sonar data l , as shown in formula (28), and the angle Δψ between the robot and the trench wall obtained based on the right-side ranging sonar data r As shown in formula (29);

[0169]

[0170] These two angles reflect the relative angular relationship between the robot and the trench wall derived from sonar data from different sides.

[0171] Calculate the left side ranging sonar mean Function expression, as shown in formula (30), the right side ranging sonar mean The function expression of is shown in formula (31).

[0172] d l =(d lf +d lb ) / 2 (30)

[0173] d r =(d rf +d rb ) / 2 (31)

[0174] In order to more accurately integrate the angles calculated by the sonars on both sides, the concept of weight is introduced. The weight of the angle calculated by the left ranging sonar is set to ω l , the angle weight calculated by the ranging sonar on the right is ω r , and satisfies ω l +ω r =1(32).

[0175] Conduct a water pool experiment to obtain the accuracy σ of the left sonar measurement angle when the robot is at different distances from the ditch wall. l The accuracy of the angle measured with the right sonar σ r And the real-time distance between the robot and the trench wall, according to the real-time distance, the weight ω is adjusted using the pre-established error probability model. l 、ω r Make corrections to obtain the dynamically adjusted weights, which correspond to Figure 4 Then, the angle Δψ calculated by the left and right sonars is calculated by using the modified weights and angle calculation formula. l , Δψ r Re-weighted summation is performed to obtain the fused angle data. Based on the fused angle data, the angle Δψ between the robot and the trench wall at the current moment is calculated, as shown in formula (33):

[0176]

[0177] Use formula (34) to calculate the target heading angle ψ of the robot at the current moment d

[0178]

[0179] Where: FOG is the bow phase angle at the current center of mass of the robot;

[0180] Calculate the error e between the target heading angle and the current heading angle ψ , as shown in formula (35):

[0181] e ψ =ψ d -ψ FOG (35)

[0182] S4: The error e between the target heading angle of the underwater cable detection robot calculated in S3 and the current heading angle ψ , add single-loop PID algorithm, the single-loop PID structure is as follows Figure 6 As shown, make adjustments;

[0183] S5: Control the thrust and direction of the propeller in the underwater cable detection robot based on the control signal τ(t) output by the single-loop PID algorithm and the robot's position information;

[0184] In this embodiment, the thrusters of an underwater cable inspection robot are controlled based on the control signals output by a single-loop PID algorithm, combined with the robot's position information. Specifically, the thrust and direction of the thrusters are adjusted based on the control signals, enabling the robot to move in the desired direction and speed. This allows for precise control of the robot's posture and position, gradually narrowing the gap between the robot's heading angle and the target heading angle.

[0185] S6: Determine whether the target heading angle of the underwater cable detection robot is consistent with the current heading angle. If the determination result is yes, execute S7; otherwise, execute S3.

[0186] S7: The underwater cable inspection robot reaches the target heading angle and realizes autonomous cable inspection.

[0187] In this embodiment, the target heading angle of the underwater cable inspection robot is continuously determined to be consistent with its current heading angle. If they are consistent, the robot has successfully reached the target heading angle and executes S7, achieving autonomous line tracking and completing the underwater cable inspection task. If they are inconsistent, the process returns to S3, resuming sonar data collection, angle calculation, and error adjustment until the robot reaches the target heading angle.

[0188] Example 1: In order to verify the effectiveness of the method of the present invention, the method proposed in the present invention was tested based on the above examples. The test results are as follows: Figure 7 , Figure 8 As shown, from Figure 7 It can be seen that when the longitudinal displacement (X axis) of the expected trajectory (black), the trajectory of this method (red), and the trajectory of the visual method (blue) is small, the lateral displacement (Y axis) changes relatively slowly. As the longitudinal displacement increases, the lateral displacement begins to rise rapidly. The trajectory of this method (red) has a high degree of fit with the expected trajectory (black), indicating that this method performs well in tracking the expected trajectory and can achieve the expected motion trajectory more accurately. Figure 8 It can be seen that from 0 to 10 seconds, the desired heading angle (black curve) remains at 0°, and the target heading phase angle (red curve) fluctuates slightly around 0°. This indicates that during this period, although the target heading phase angle is not completely stable at the desired heading angle, the fluctuation range is small.

[0189] Starting around 10 seconds in, the desired heading angle suddenly jumps to approximately 60° and remains stable. The target heading phase angle responds quickly, rapidly approaching the desired heading angle, reaching close to 60° within a short period of time before fluctuating slightly around 60°. The target heading phase angle reacts quickly to the sudden change in the desired heading angle, quickly approaching the desired heading angle, demonstrating the system's rapid response to angle adjustments and its good dynamic response capability.

[0190] Each embodiment in this specification is described in a related manner. Similar parts between the various embodiments can be referred to in conjunction with each other. Each embodiment focuses on the differences between the other embodiments. In particular, the system embodiment is generally similar to the method embodiment, so the description is relatively simple. For related parts, refer to the description of the method embodiment.

[0191] The above description is only a preferred embodiment of the present invention and is not intended to limit the scope of protection of the present invention. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principles of the present invention are included in the scope of protection of the present invention.

Claims

1. A method for controlling the autonomous patrol motion of an underwater cable detection robot, characterized in that: The following steps are involved: S1: Establish the six-degree-of-freedom kinematic equations and dynamic model of the underwater cable detection robot; S2: The robot position information and the current heading angle of the robot are collected through sensors deployed on the robot, and the robot position information is input into the robot; S3: Calculate the target heading angle of the robot using the optimal deflection angle fusion algorithm. Compare the current heading angle of the robot collected by the sensor with the target heading angle calculated by the optimal deflection angle fusion algorithm to obtain the error e between the target heading angle and the current heading angle. ψ ; S4: Error e between the target heading angle and the current heading angle of the underwater cable detection robot ψ , add single-loop PID algorithm for adjustment; S5: Control the thrust and direction of the propeller in the underwater cable detection robot based on the control signal τ(t) output by the single-loop PID algorithm and the robot's position information; S6: Determine whether the target heading angle of the underwater cable detection robot is consistent with the current heading angle. If the target heading angle is consistent with the current heading angle, execute S7; otherwise, execute S3. S7: The underwater cable inspection robot reaches the target heading angle and realizes autonomous cable inspection.

2. The autonomous line patrol motion control method of an underwater cable detection robot according to claim 1, characterized in that: The specific process of constructing the six-degree-of-freedom kinematic equations of the underwater cable detection robot in S1 is: S1.1: In the inertial coordinate system, establish the robot position vector function relationship, as shown in formula (1), establish the robot posture vector function relationship, as shown in formula (2), and combine the robot position vector function relationship and the robot posture vector function relationship into the first 6-dimensional column vector, as shown in formula (3): η1=[x y z] T (1) Where: η1 is the robot position vector in the inertial coordinate system, x is the displacement along the x-axis, y is the displacement along the y-axis, and z is the displacement along the z-axis; T is the transpose; η2=[φ θ ψ] T (2) Where: η2 is the robot attitude vector in the inertial coordinate system, φ is the roll angle, θ is the pitch angle, and ψ is the heading angle; n=[n1 n2] T (3) Where: η is the combined 6-dimensional column vector of the inertial system state variables; S1.2: In the carrier coordinate system, establish the robot linear velocity vector function relationship, as shown in equation (4), establish the robot angular velocity vector function relationship, as shown in equation (5), and combine the linear velocity vector function relationship and the angular velocity vector function relationship into a second 6-dimensional column vector, as shown in equation (6): ν1=[u v w] T (4) Where: ν1 is the linear velocity vector, u is the velocity along the x-axis, v is the velocity along the y-axis, and w is the velocity along the z-axis; ν2=[pqr] T (5) Where: ν2 is the angular velocity vector, p is the angular velocity of rotation around the x-axis, q is the angular velocity of rotation around the y-axis, and r is the angular velocity of rotation around the z-axis; n=[n1n2] T (6) Where: ν is the velocity vector of the robot; S1.3: In the carrier coordinate system, establish the robot force vector function relationship, as shown in equation (7), establish the robot torque vector function relationship, as shown in equation (8), and combine the robot force vector function relationship and the robot torque vector function relationship into a third 6-dimensional column vector, as shown in equation (9): τ1=[X Y Z] T (7) Where: τ1 is the force vector representation of the robot, X is the force along the x-axis, Y is the force along the y-axis, and Z is the force along the z-axis; τ2=[KMN] T (8) Where: τ2 is the torque vector representation of the robot, K is the torque around the x-axis, M is the torque around the y-axis, and N is the torque around the z-axis; τ=[τ1τ2] T (9) Where: τ is the force and torque vector of the robot; S1.4: Define the linear velocity transformation matrix function relationship from the robot carrier coordinate system to the inertial coordinate system, as shown in equation (10): in: is the linear velocity transformation matrix, R z,ψ is the rotation matrix around the z axis, R y,θ is the rotation matrix around the y-axis, R x,φ is the rotation matrix around the x-axis, i is the origin in the inertial coordinate system, and b is the center of the body in the carrier coordinate system; S1.5: Definition The linear velocity transformation matrix function relationship is shown in formula (11), and the linear velocity rotation matrix J1(η2) function relationship of the robot is defined as shown in formula (12): S1.6: Establish the linear velocity vector ν1 in the carrier coordinate system and the linear velocity vector in the inertial coordinate system The functional relationship is shown in formula (13). According to the projection relationship of the angular velocity vector ν2 in the carrier coordinate system, the functional relationship of the angular velocity vector ν2 in the robot carrier coordinate system is obtained, as shown in formula (14): in: is the rolling angular velocity, is the pitch angular velocity, is the heading angular velocity; S1.7: Define the transformation matrix J, as shown in Equation (15), which transforms the position and attitude change rate of the robot in the inertial coordinate system into By establishing a connection with the robot's velocity vector ν in the carrier coordinate system, the six-degree-of-freedom kinematic equation of the underwater cable detection robot is obtained, as shown in Equation (16): Where: 0 3×3 is a 3×3 zero matrix, J1 is the linear velocity rotation matrix, and J2 is the angular velocity rotation matrix; 3. The autonomous line patrol motion control method of an underwater cable detection robot according to claim 1, characterized in that: The specific process of constructing the dynamic model of the underwater cable detection robot in S1 is as follows: S2.1: Based on prior knowledge, the robot's six-degree-of-freedom dynamic equation is obtained, as shown in Equation (17): Where: M is the generalized mass matrix, is the derivative of the velocity vector v, C(v) is the hydrodynamic Coriolis and centripetal force matrix, and D(v b ) is the nonlinear fluid hydrodynamic damping matrix, g(η) is the restoring force and torque vector generated by gravity and buoyancy, τ a is the control input vector, τ a =[T X T Y T Z T K T M T N ] T is the control force on 6 degrees of freedom, τ d Various interference vectors brought by the external environment, τ d =R 6×1 , R represents a real number, R 6×1 represents a 6x1 column vector; S2.2: Define the functional relationship of the generalized mass matrix M as shown in Equation (18), the functional relationship of the hydrodynamic Coriolis and centripetal force matrices C(v) as shown in Equation (19), and the nonlinear fluid hydrodynamic damping matrix D(v) b ), as shown in Equation (20), and the functional relationship between the restoring force and the torque vector g(η) generated by gravity and buoyancy is shown in Equation (21) M=M RB +M A (18) Where: M RB is the rigid body inertia matrix, M A is the additional mass matrix, M is the generalized mass matrix; C(v)=C RB (in)+C A (in) (19) Where: C(v) is the hydrodynamic Coriolis and centripetal force matrix, C RB (v) is the Coriolis matrix, C A (v) is the Coriolis and centripetal force matrix caused by the added mass; D(v b )=-diag{X u ,Y v ,Z w ,K p ,M q ,N r }-diag{X |u|u |u|,Y |v|v |v|,Z |w|w |w|,K |p|p |p|,M |q|q |q|,N |r|r |r|} (20) Where: X u 、Y v , Z w represents the linear friction coefficient caused by the laminar boundary layer when the robot translates along the x, y, and z axes, K p 、M q 、N r represents the linear friction coefficient caused by the laminar boundary layer when the robot rotates along the x, y, and z axes. |u|u 、Y |v|v , Z |w|w represents the secondary friction coefficient caused by the turbulent boundary layer when the robot translates along the x, y, and z axes, K |p|p 、M |q|q 、N |r|r represents the secondary friction coefficient caused by the turbulent boundary layer when the robot rotates along the x, y, and z axes. |u|, |v|, |w|, |p|, |q|, and |r| represent the absolute values of u, v, w, p, q, and r. diag is a diagonal matrix. Where: W is the gravity of the underwater cable detection robot, B is the buoyancy center of the robot, x B is the position of the buoyancy center on the x-axis in the carrier coordinate system, y B is the position of the buoyancy center on the y-axis in the carrier coordinate system, z B is the position of the buoyancy center on the y-axis in the carrier coordinate system; S2.3: Define the rigid body inertia matrix M RB The functional relationship of is shown in formula (22), and the additional mass matrix M is defined as A The functional relationship of is shown in formula (23), and the Coriolis matrix C is defined as RB The functional relationship of (v) is shown in Equation (24), which defines the Coriolis and centripetal force matrices C caused by the additional mass. A The functional relationship of (v) is shown in formula (25): Where m is the mass of the robot, z g 、y g 、x g is the three-dimensional position of the robot’s center of gravity relative to the robot’s mass center, I x , I y , I z , I xy , I xz , I yz is the moment of inertia of the robot, I xy represents the product moment of inertia of the robot x, y; in: is the additional mass coefficient when the robot moves linearly along the z, y, and x axes of the robot, is the additional inertia of the robot rotating along the z-axis, is the additional inertia of the robot rotating along the y-axis, is the additional inertia of the robot rotating along the x-axis; S2.4: Based on the distribution structure of the robot's propeller thrusters and the distance between the thrusters and the center of the body, calculate the force and torque of each thruster on each degree of freedom of the body to obtain the control input vector τ a The functional relationship is shown in formula (26): Where: s() represents the sin() function, c() represents the cos() function, T1, T2, T3, T4, T5, T6 represent the thrust of each propeller, T X 、T Y 、T Z 、T K 、T M 、T N is the control force on 6 degrees of freedom; S2.5: Assume that the pitch angle and roll angle of the robot are 0 during the fixed-depth motion. Substitute equations (18)-(26) into equation (17) to obtain the functional relationship of the robot dynamic model, as shown in equation (27):

4. The autonomous line patrol motion control method of an underwater cable detection robot according to claim 1, characterized in that: The specific steps of S2 are: using IMU to collect the current heading angle data of the robot, and using Kalman filtering to filter the current heading angle data of the robot, using GPS to collect the current position information of the robot, using a depth meter to collect the current depth information of the robot, using Kalman filtering to fuse the current position information and current depth information of the robot to obtain the absolute position information of the robot, and inputting the absolute position information of the robot into the robot.

5. The autonomous line patrol motion control method of an underwater cable detection robot according to claim 1, characterized in that: The specific steps of S3 are: S3.1: The robot is equipped with four ranging sonars. It uses these four ranging sonars to obtain the distance data between the sonar and the trench wall one by one at a dynamic sampling frequency. Let the current heading angle of the robot be ψ FOG , the left front ranging sonar data is d lf , the left rear ranging sonar data is d lb , the right front ranging sonar data is d rf , the right rear ranging sonar data is d rb , the distance between the front and rear sonars is l, and the roll angle is The robot's longitudinal velocity is v x ; S3.2: Calculate the angle Δψ between the robot and the trench wall based on the left-side ranging sonar data. l , as shown in formula (28), and the angle Δψ between the robot and the trench wall obtained based on the right-side ranging sonar data r As shown in formula (29); S3.3: Set the left ranging sonar mean Function expression, as shown in formula (30), the right side ranging sonar mean The function expression of is shown in formula (31), and the weight of the angle calculated by the left ranging sonar is ω l , the angle weight calculated by the ranging sonar on the right is ω r , weight ω l With weight ω r The functional relationship is shown in formula (32): oh l +oh r =1 (32) S3.4: Conduct a water tank experiment to obtain the accuracy σ of the left sonar angle measurement when the robot is at different distances from the trench wall. l The accuracy of the angle measured with the right sonar σ r The real-time distance between the robot and the trench wall, and the angle weight ω calculated by the error probability model for the left and right sonars according to the real-time distance l 、ω r Correction is performed to obtain the dynamically corrected weights, and the angles Δψ calculated by the left and right sonars are calculated using the dynamically corrected weights. l , Δψ r Perform weighted summation to obtain the fused angle data. Based on the fused angle data, calculate the angle Δψ between the robot and the trench wall at the current moment, as shown in formula (33): S3.5: Calculate the target heading angle ψ of the robot at the current moment d , as shown in formula (34), calculate the error e between the target heading angle and the current heading angle ψ , as shown in formula (35): Where: FOG is the bow phase angle at the current center of mass of the robot; e ψ =ψ d -ψ FOG (35).

6. The autonomous line patrol motion control method of an underwater cable detection robot according to claim 5, characterized in that: The dynamic sampling frequency calculation method in S3.1 is: S3.1.1: Calculate the error e between the robot's target heading angle and the current heading angle in real time ψ ; S3.1.2: Substitute the error e obtained in S3.1.1 ψ Compared with the preset error, if the error e ψ If the error is greater than the preset error, the dynamic adjustment strategy is executed. ψ If the error is less than or equal to the preset error, the ranging sonar sampling frequency is reduced to the minimum frequency.

7. The autonomous line patrol motion control method of an underwater cable detection robot according to claim 6, characterized in that: The dynamic adjustment strategy in S3.1.2 is to increase the ranging sonar sampling frequency on the side of the robot close to the ditch wall, and at the same time reduce the ranging sonar sampling frequency on the side of the robot far from the ditch wall.

Images