An underwater dredging robot and its control system

Abstract

This invention discloses an underwater dredging robot and its control system, including a robot body, a drive mechanism located at the bottom of the robot body, a cutter head mechanism located at one end of the robot body, a hydraulic station and a slurry pump located on the robot body, and a control system. The control system includes: a data and image transmission module for real-time acquisition and transmission of underwater environmental data and image data; a navigation module for monitoring the positioning of the underwater dredging robot and performing path planning; an attitude analysis module for real-time monitoring and analysis of the attitude state of the underwater dredging robot for stability monitoring and early warning; a motion control module for generating and executing motion commands for the robot; and a power supply control module for managing the energy distribution and power status of the dredging robot.

Description

Technical Field

[0001] This invention relates to the field of dredging equipment technology, and in particular to an underwater dredging robot and its control system. Background Technology

[0002] With continuous social progress and rapid technological development, dredging work, as a crucial link in maintaining the normal operation of various water environments, pipeline systems, and other infrastructure, is becoming increasingly important. However, during dredging operations, equipment safety issues are gradually becoming a significant factor restricting the industry's development.

[0003] Traditional dredging methods, such as manual dredging or dredging using simple mechanical devices, can accomplish dredging tasks to a certain extent, but they pose numerous safety hazards. Simple mechanical dredging equipment is easily affected by external environmental factors during operation, such as water flow speed and underwater obstacles, leading to equipment instability or even malfunctions, and consequently, safety accidents. Furthermore, these simple dredging machines lack effective safety monitoring and data recording methods during operation, making it difficult to trace the cause of an accident and hindering the development of subsequent safety improvement measures. Summary of the Invention

[0004] In order to solve the above-mentioned technical problems, the present invention provides an underwater dredging robot and its control system.

[0005] The technical solution of this invention is implemented as follows:

[0006] An underwater dredging robot includes a robot body, a drive mechanism located at the bottom of the robot body, a cutter mechanism located at one end of the robot body, a hydraulic station and a slurry pump located on the robot body.

[0007] A control system for an underwater dredging robot includes:

[0008] The data and image transmission module collects and transmits underwater environmental data and image data in real time.

[0009] The navigation module monitors the positioning of the underwater dredging robot and performs path planning for it.

[0010] The attitude analysis module is used to monitor and analyze the attitude status of the underwater dredging robot in real time for stability monitoring and early warning.

[0011] The motion control module generates and executes the robot's motion commands;

[0012] The power supply control module manages the energy distribution and power status of the dredging robot.

[0013] Furthermore, the data and image transmission module periodically acquires the latitude and longitude coordinates, depth, speed, and orientation angle data of the underwater dredging robot from the navigation module;

[0014] The robot's roll and pitch angle data are periodically acquired from the attitude analysis module;

[0015] Obtain the status of the currently executed command and the status data of the dredging mechanism from the motion control module;

[0016] Obtain bus voltage, total current, remaining battery power, and temperature alarm signal data from the power supply control module;

[0017] Acquire image data directly from the camera;

[0018] All sensor and control data is packaged into a custom structured data frame, timestamped, compressed, and encrypted to obtain a complete data packet.

[0019] Furthermore, the data and image transmission module multiplexes the video stream, forming a single transmission stream together with the structured data frames.

[0020] Furthermore, the navigation module receives and fuses raw data from the inertial measurement unit (IMU), Doppler log (DVL), and ultra-short baseline positioning system (USBL), and eliminates errors and drift of individual sensors through filtering algorithms;

[0021] Output the global status of the underwater dredging robot.

[0022] The lateral and directional deviations of the underwater dredging robot from the predetermined path are calculated, and guidance commands are generated and sent to the motion control module.

[0023] Furthermore, the process by which the navigation module outputs the global state of the underwater dredging robot includes:

[0024] S1. Based on the angular velocity and linear acceleration data output by the inertial measurement unit (IMU) at the previous moment, the first position information, first velocity information and first attitude information of the underwater dredging robot at the current moment are predicted by mechanical integration method. The predicted state contains accumulated error due to integration operation.

[0025] S2. Simultaneously receive relative velocity observation data from the Doppler log (DVL), absolute depth observation data from the depth gauge, absolute position observation data from the ultra-short baseline positioning system (USBL), and orientation observation data from the magnetometer.

[0026] S3. Based on the predicted state obtained in step S1, and constrained by the various observation data obtained in step S2, the extended Kalman filter algorithm is used to perform data fusion. The fusion process is as follows: calculate the difference between each observation data and the corresponding predicted value, and then use the Kalman gain as the weight to perform weighted correction on the difference, and finally output the optimized global state at the current moment that eliminates the cumulative error of the sensor.

[0027] The global state of the underwater dredging robot includes the fused three-dimensional coordinate position, three-dimensional motion speed, and fused attitude data consisting of roll angle, pitch angle, and yaw angle, used for global positioning and heading control.

[0028] Furthermore, the attitude analysis module independently calculates and outputs roll and pitch angle data based on inertial measurement unit (IMU) data, and determines in real time whether the robot's attitude is within the safe threshold.

[0029] When the tilt angle data exceeds the preset safety threshold, it is determined that there is a risk of overturning, and an emergency alarm signal is sent to the motion control module.

[0030] Furthermore, the process by which the attitude analysis module calculates and outputs roll and pitch angle data includes:

[0031] Sa1. Preprocess the raw angular velocity data and raw acceleration data output by the inertial measurement unit (IMU). The preprocessing includes calibration and filtering.

[0032] Sa2. Based on the angular velocity data preprocessed in step Sa1, integral calculation is performed using the quaternion method to update and predict the attitude change of the underwater dredging robot in real time, and obtain the first attitude information including roll angle and pitch angle.

[0033] Sa3. The acceleration data preprocessed in step Sa1 is introduced to correct the first attitude information. Specifically, during the time period when the motion acceleration of the underwater dredging robot is less than the preset smooth threshold, the component of the gravity acceleration vector in the body coordinate system is used as the observation reference. The second attitude information of roll angle and pitch angle is calculated by complementary filtering or Kalman filtering algorithm, and the second attitude information is used to correct the drift of the first attitude information caused by integration.

[0034] Sa4. Output the smooth roll and pitch angle data after fusion correction in step Sa3. At the same time, compare the real-time attitude data, i.e. the smooth roll and pitch angle data after fusion correction, with the preset safe attitude threshold. When any angle data exceeds the safe threshold, immediately send a risk alarm signal to the motion control module.

[0035] Furthermore, the motion control module receives guidance instructions generated by the navigation module and task instructions input from the outside, calculates the specific target speed, displacement or force required to drive each actuator, coordinates the synchronous operation of multiple actuators, receives system alarms, and executes the highest priority safety strategy.

[0036] Furthermore, the power supply control module dynamically performs power distribution according to the power request of the motion control module, monitors the bus voltage, current, power and temperature in real time, performs protection operations when abnormal conditions occur, and reports the health status and fault signals of the energy system in real time.

[0037] Compared with the prior art, the present invention has the following advantages:

[0038] This invention constructs a dual environmental perception and adaptive system through a navigation module and an attitude analysis module. Specifically, the navigation module uses multi-sensor (IMU, DVL, USBL, etc.) fusion technology to obtain accurate self-position, speed and orientation in real time, enabling the underwater dredging robot to be accurately located and to perform intelligent planning and tracking according to a preset path, resisting environmental interference such as water flow, maintaining a stable navigation trajectory, and avoiding the deviation from the work area or collision caused by inaccurate positioning of traditional equipment.

[0039] The attitude analysis module is specifically responsible for monitoring the robot's roll and pitch angles, and sensing the robot's own body posture in real time. Once it detects that the robot is tilted too much due to complex underwater terrain or water flow impact, and there is a risk of overturning, it will immediately send an emergency alarm to the motion control module, enabling it to actively respond to the unstable underwater environment and prevent rollover or loss of control. This fundamentally solves the problem of unstable operation caused by environmental interference in traditional equipment.

[0040] This invention constructs a comprehensive real-time monitoring and data traceability system through data and image transmission modules. It collects, packages, and transmits all navigation data, attitude data, equipment status data, energy data, and high-definition video streams to the ground control station in real time. In the control room, the real-time position, operating attitude, operation screen, and system health status of the underwater robot can be clearly monitored, realizing visualized monitoring of the entire operation process. Once any abnormality or accident occurs, the data can be retrieved immediately to accurately trace the time, location, and equipment status of the problem. This provides a solid data foundation for accident analysis and technical improvement, completely changing the dilemma of difficulty in assigning responsibility after traditional dredging. Attached Figure Description

[0041] Figure 1 This is a schematic diagram of the structure of an underwater dredging robot according to the present invention;

[0042] Figure 2 This is a framework diagram of the control system in this invention;

[0043] Figure 3 This is a schematic diagram illustrating the process of the navigation module outputting the global state of the underwater dredging robot in this invention.

[0044] Figure 4 This is a schematic diagram of the process by which the attitude analysis module calculates and outputs roll and pitch angle data in this invention.

[0045] 1. Robot body; 2. Drive mechanism; 3. Cutter mechanism; 4. Hydraulic station; 5. Slurry pump; 6. Underwater lighting. Detailed Implementation

[0046] To make the objectives, features, and advantages of this invention more apparent and understandable, the technical solutions of the embodiments of this invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the embodiments described below are only some embodiments of this invention, and not all embodiments. Based on the embodiments of this invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this invention.

[0047] like Figure 1 As shown, this embodiment provides an underwater dredging robot, including a robot body 1, a drive mechanism 2 disposed at the bottom of the robot body 1, a cutter mechanism 3 disposed at one end of the robot body 1, a hydraulic station 4 and a slurry pump 5 disposed on the robot body 1. The robot body 1 is equipped with at least two underwater high-definition cameras, namely a front-facing and a rear-facing camera, which are used to capture and record videos of the underwater environment when operating in culverts or open channels. Specifically, the specifications of the underwater high-definition cameras are: resolution: 1920x1080, focal length: 2.8mm fixed focus, frame rate: 30FPS, illumination: 0.001 lux, and viewing angle: 110°.

[0048] Meanwhile, the robot body 1 is equipped with at least 4 high-brightness underwater lights 6, 2 in the front and 2 in the rear, to provide auxiliary light source for the underwater high-definition camera. The specifications of the lights are: maximum brightness: 3500LM, color temperature: 6500K, and working mode: multi-level brightness adjustment.

[0049] Furthermore, the robot body 1 is also equipped with a gimbal system. The gimbal system has an angle feedback function, which can easily adjust the viewing angle of the underwater high-definition camera or image sonar. The gimbal system has a large torque, with a maximum torque of 25 N·m, and has an anti-stalling function to prevent damage to the system under extreme conditions. The specifications of the gimbal system are as follows: spindle material: 316L stainless steel, maximum torque: 25 N·m, signal frequency: 50 Hz, angle range: 200°.

[0050] In this embodiment, the underwater dredging robot relies on cables for long-distance communication. Video signals, sensor information, and control information can be stably transmitted between the ground control system and the underwater dredging robot. On the other hand, the cables can prevent the underwater dredging robot from being lost in harsh environments. Specifically, the cables can be armored umbilical cables, which have a wear-resistant and smooth polyurethane sheath and are reinforced with an armored structure. They contain high-temperature conductors with a specification of 25mm² 660V and high-temperature conductors with a specification of 4mm², as well as 24AWG twisted-pair communication lines.

[0051] Meanwhile, the cable is connected to an electric cable reel system at one end on the ground. The electric cable reel system is simple in structure, convenient and durable, and has a working tensile force of not less than 1t. The cable reel can realize automatic cable laying.

[0052] The electric cable reel system consists of three parts: the electric cable reel body, the cable laying device, and the three-phase power and controller. For the dredging robot system, the electric cable reel body adopts a single roller scheme, and the electric cable reel winding and unwinding speed is 0~25m / min.

[0053] The drive mechanism 2 consists of a travel motor, rubber tracks, drive wheels, support rollers, idler wheels, carrier rollers, and track frame. The travel motor serves as a power source and outputs rotational torque. This motor is typically a hydraulic motor or a waterproof motor, and its speed and direction are adjusted by the control system according to motion commands. The torque output by the travel motor is directly transmitted to the drive wheels.

[0054] The drive wheel is a driving wheel with a toothed structure on its outer circumference that meshes tightly with the meshing holes or patterns on the inner surface of the rubber track. When the drive wheel rotates, it powerfully drives the rubber track through the meshing action of the teeth.

[0055] The rubber track is tensioned on the drive wheel, guide wheel, support wheel and support roller to form a closed circular track. The rotation of the drive wheel drives the rubber track to move in a cycle. The part of the rubber track that contacts the ground or the bottom of the water has a large ground contact area, which converts the driving force into a force on the ground. At the same time, the ground generates a reaction force on the rubber track, that is, traction force. This traction force is transmitted to the robot body 1 through the support wheel and the rubber track frame, which overcomes the resistance and propels the robot forward or backward.

[0056] The majority of the weight of the underwater dredging robot is transferred to the support rollers through the track frame, and then the support rollers press on the track. Finally, the track distributes the pressure to a large contact area, thereby providing good ground support and effectively preventing the robot from sinking into the soft substrate at the bottom of the water.

[0057] The guide wheel is located at the other end of the track ring and is usually connected to the tensioning mechanism. It is used to tension the rubber track and guide its movement direction to keep the rubber track stable. The support rollers are evenly distributed under the track frame, bear the weight of the robot, and roll along with the rubber track as it moves to reduce friction. The support rollers are installed above the track frame to support the upper part of the rubber track and prevent it from sagging excessively or swaying laterally due to its own weight.

[0058] This is achieved by controlling the speed difference between the left and right walking motors. For example, when the speed of the left track is lower than that of the right, the robot turns left; when the speeds of both tracks are equal but in opposite directions, it can turn on the spot.

[0059] Because the rubber tracks distribute the robot's weight over a large area, resulting in a low ground pressure, and the special design of the rubber track frame, the robot can smoothly cross small underwater obstacles and ditches, and adapt to uneven terrain.

[0060] The hydraulic station 4 is equipped with a complete underwater hydraulic drive system, which can meet the working requirements of the walking motor, lifting cylinder, and cutter mechanism 3. The hydraulic drive system is suitable for applications with large output and low movement speed. It adopts a variable flow hydraulic pump for stepless speed regulation, which achieves better control and efficiency improvement. It drives the rubber track and cutter mechanism 3 with greater torque and stronger power. The hydraulic power unit has an input power of 37kW, a working pressure of 21Mpa, and a system displacement of 80L / min. It is installed on the underwater dredging robot body 1 and includes an underwater motor, hydraulic pump, control valve group and oil tank.

[0061] Specifically, the hydraulic oil tank is equipped with an oil tank, an outlet filter, and an underwater oil tank compensator.

[0062] The slurry pump 5 is a single-casing centrifugal slurry pump that can be electrically driven. The power source is an external diesel generator or a mains power distribution system. The pump power is about 40kW, the discharge distance is about 220m, and the flow rate is about 400m³ / h. The slurry pump 5 is equipped with a screen at the suction inlet to block debris. It works with the cutter head mechanism 3 to guide sludge into the pump suction inlet.

[0063] The sludge discharge pipe is made of plastic flexible tubing, with each section of tubing paired with a flange for easy disassembly; the diameter of the sludge discharge pipe is approximately 150–200 mm, and it is equipped with an auxiliary float that is easy to disassemble.

[0064] In this embodiment, the power supply and control power are supplied independently to improve power supply reliability. The power supply uses a three-phase AC 660V system, and the control power supply uses a DC 400V system.

[0065] like Figure 2 As shown, an underwater dredging robot control system includes:

[0066] The data and image transmission module collects and transmits underwater environmental data and image data in real time.

[0067] The navigation module monitors the positioning of the underwater dredging robot and performs path planning for it.

[0068] The attitude analysis module is used to monitor and analyze the attitude status of the underwater dredging robot in real time for stability monitoring and early warning.

[0069] The motion control module generates and executes the robot's motion commands;

[0070] The power supply control module manages the energy distribution and power status of the dredging robot.

[0071] Furthermore, the data and image transmission module periodically acquires the latitude and longitude coordinates, depth, speed, and orientation angle data of the underwater dredging robot from the navigation module;

[0072] The robot's roll and pitch angle data are periodically acquired from the attitude analysis module;

[0073] Obtain the status of the currently executed command and the status data of the dredging mechanism from the motion control module;

[0074] Obtain bus voltage, total current, remaining battery power, and temperature alarm signal data from the power supply control module;

[0075] Acquire image data directly from the camera;

[0076] All sensor and control data is packaged into a custom structured data frame, timestamped, compressed, and encrypted to obtain a complete data packet.

[0077] Furthermore, the data and image transmission module multiplexes the video stream, forming a single transmission stream together with the structured data frames.

[0078] Furthermore, the navigation module receives and fuses raw data from the inertial measurement unit (IMU), Doppler log (DVL), and ultra-short baseline positioning system (USBL), and eliminates errors and drift of individual sensors through filtering algorithms;

[0079] Output the global status of the underwater dredging robot.

[0080] Specifically, in this embodiment, the inertial measurement unit (IMU) includes a gyroscope, an accelerometer, a magnetometer, and a depth gauge;

[0081] The lateral and directional deviations of the underwater dredging robot from the predetermined path are calculated, and guidance commands are generated and sent to the motion control module.

[0082] like Figure 3 As shown, further, the process by which the navigation module outputs the global state of the underwater dredging robot includes:

[0083] S1. Based on the angular velocity and linear acceleration data output by the inertial measurement unit (IMU) at the previous moment, the first position information, first velocity information and first attitude information of the underwater dredging robot at the current moment are predicted by mechanical integration method. The predicted state contains accumulated error due to integration operation.

[0084] Specifically, the process of predicting the first position information, first velocity information, and first attitude information of the underwater dredging robot at the current moment can include:

[0085] Input the three-axis angular velocity measured by the gyroscope Where b represents the body coordinate system, attitude can be represented in various ways. This embodiment uses quaternions as an example because they are computationally efficient and have no singularities. The differential equation for attitude update is:

[0086]

[0087] Where q is a quaternion representing the attitude. It is quaternion multiplication;

[0088] Discrete-time computation (actual calculation method):

[0089]

[0090] in, It is the sampling time interval, where k-1 represents the previous time.

[0091] Output: The quaternion of the current pose. This quaternion can be converted into an attitude matrix. (This represents the rotation matrix from the body coordinate system to the navigation coordinate system), and then the roll angle, pitch angle, and yaw angle in the navigation coordinate system are calculated to obtain the first attitude information.

[0092] After obtaining the initial attitude information, the acceleration in the body coordinate system is transformed to the navigation coordinate system, and the influence of gravity is eliminated, thereby obtaining the machine's own motion acceleration. Specifically:

[0093] Input accelerometer to measure triaxial specific force It includes motion acceleration and gravitational acceleration, and introduces an attitude matrix. The differential equation for velocity in the navigation coordinate system is:

[0094]

[0095] in, It is the gravity vector in the navigation coordinate system (g is the gravitational acceleration). It is the acceleration of the body in the navigation coordinate system;

[0096] Discrete-time computation:

[0097]

[0098] Obtain the three-axis velocity in the navigation coordinate system We obtain the first velocity information;

[0099] in, This is the eastward velocity, which is the velocity component of the robot body 1 moving in the direction of geographic due east in the horizontal plane.

[0100] This is the northward velocity, which is the velocity component of the robot body 1 moving in the direction of geographic due north in the horizontal plane.

[0101] The vertical velocity is the component of the robot body 1's velocity along the vertical direction (usually referring to the direction away from the Earth's center).

[0102] Introducing speed Update location information:

[0103]

[0104] Discrete-time computation:

[0105]

[0106] in

[0107] Obtain the three-dimensional position in the navigation coordinate system That is, the first position information.

[0108] S2. Simultaneously receive relative velocity observation data from the Doppler log (DVL), absolute depth observation data from the depth gauge, absolute position observation data from the ultra-short baseline positioning system (USBL), and orientation observation data from the magnetometer.

[0109] S3. Based on the predicted state obtained in step S1, and constrained by the various observation data obtained in step S2, the extended Kalman filter algorithm is used to perform data fusion. The fusion process is as follows: calculate the difference between each observation data and the corresponding predicted value, and then use the Kalman gain as the weight to perform weighted correction on the difference, and finally output the optimized global state at the current moment that eliminates the cumulative error of the sensor.

[0110] Specifically, based on the predicted state of S1 (i.e., the first position information, first velocity information, and first attitude information containing accumulated error), and constrained by the various observation data (DVL velocity, USBL position, depth gauge depth, and magnetometer orientation) obtained in step S2, optimal fusion is performed through the following steps:

[0111] Define the state and observation vectors:

[0112] State vector (x): This is the core variable that needs to be estimated. It directly corresponds to the output of S1 and includes the sensor error state.

[0113]

[0114] in, This refers to the three-dimensional location (latitude, longitude, and depth). For three-dimensional velocity, These are roll angle, pitch angle, and yaw angle, respectively. This is for the zero bias of the accelerometer and gyroscope.

[0115] Observation vector (z): This is the direct measurement value from the external sensor in S2:

[0116]

[0117] Three-dimensional velocity in body coordinate system measured by DVL; The absolute three-dimensional position measured by USBL; Depth measured by a depth gauge; The yaw angle is measured by the magnetometer.

[0118] Using IMU data (angular velocity, acceleration), the predicted state at the current moment is calculated from the optimal state at the previous moment:

[0119]

[0120] The actual observed value of S2 Compared with predicted state Calculated expected observations Comparison:

[0121]

[0122] For example, position residual: USBL measured position - predicted position; velocity residual: DVL measured velocity (after conversion to navigation system) - predicted velocity.

[0123] Using Kalman gain The above residuals are weighted to correct the predicted state of S1, thereby obtaining the optimal estimate:

[0124]

[0125] in, Let represent the Kalman gain matrix at time k, in the update step of the Extended Kalman Filter (EKF). It is a key weight matrix used to optimally fuse state predictions with new observations to calculate the corrected state estimate.

[0126] That is, the final output, optimized global state at the current moment after eliminating sensor cumulative errors.

[0127] The global state of the underwater dredging robot includes the fused three-dimensional coordinate position, three-dimensional motion speed, and fused attitude data consisting of roll angle, pitch angle, and yaw angle, used for global positioning and heading control.

[0128] Furthermore, the attitude analysis module independently calculates and outputs roll and pitch angle data based on inertial measurement unit (IMU) data, and determines in real time whether the robot's attitude is within the safe threshold.

[0129] When the tilt angle data exceeds the preset safety threshold, it is determined that there is a risk of overturning, and an emergency alarm signal is sent to the motion control module.

[0130] like Figure 4 As shown, further, the process by which the attitude analysis module calculates and outputs roll and pitch angle data includes:

[0131] Sa1. Preprocess the raw angular velocity data and raw acceleration data output by the inertial measurement unit (IMU). The preprocessing includes calibration and filtering.

[0132] Sa2. Based on the angular velocity data preprocessed in step Sa1, integral calculation is performed using the quaternion method to update and predict the attitude change of the underwater dredging robot in real time, and obtain the first attitude information including roll angle and pitch angle.

[0133] Sa3. The acceleration data preprocessed in step Sa1 is introduced to correct the first attitude information. Specifically, during the time period when the motion acceleration of the underwater dredging robot is less than the preset smooth threshold, the component of the gravity acceleration vector in the body coordinate system is used as the observation reference. The second attitude information of roll angle and pitch angle is calculated by complementary filtering or Kalman filtering algorithm, and the second attitude information is used to correct the drift of the first attitude information caused by integration.

[0134] For example, the attitude analysis module first determines whether it is appropriate to use the accelerometer for correction, based on the fact that when the robot is in a near-uniform or stationary state, its motion acceleration is very small, and the dominant component of the accelerometer measurement is gravitational acceleration.

[0135] Calculate the vector magnitude of the triaxial accelerometer measurements:

[0136]

[0137] in, The acceleration vector magnitude represents the total amplitude of the current accelerometer measurement value of robot body 1;

[0138] The X-axis specific force measurement value is the specific force component measured by the accelerometer on the X-axis of the robot body coordinate system 1.

[0139] The Y-axis specific force measurement value is the specific force component measured by the accelerometer on the Y-axis of the robot body coordinate system 1.

[0140] The Z-axis specific force measurement value is the specific force component measured by the accelerometer on the Z-axis of the robot body coordinate system 1.

[0141] Check if the modulus length is close to the gravitational acceleration g, allowing for a small error range ϵ (i.e., the "preset smooth threshold").

[0142]

[0143] If the conditions are met, the robot's current motion is considered smooth, the accelerometer data is reliable, and the subsequent calibration process is triggered. Otherwise, the calibration is skipped, and the gyroscope integration result is fully trusted.

[0144] After confirming that the conditions are met, use the current accelerometer measurement value. To directly calculate the roll angle and pitch angle It defines the direction of the gravity vector in the collective coordinate system:

[0145]

[0146] Here, arctan2 is the arctangent function in the four quadrants, which provides the correct value of the angle in the range (−π,π], from which the following is obtained. This is known as "second attitude information," which has no drift, but may have transient errors when the robot has motion acceleration.

[0147] The "second attitude information" (accelerometer) that is drift-free but has high high-frequency noise is fused with the "first attitude information" (gyroscope integral) that is smooth but prone to drift. For example, complementary filtering can be used as an example:

[0148] In the frequency domain, the complementary filter is used. Gyroscopes excel at measuring high-frequency dynamic changes, while accelerometers excel at providing low-frequency static references. The complementary filter acts like a "filter bank," allowing gyroscope data to pass through a high-pass filter and accelerometer data through a low-pass filter, and then the two are added together. Specifically:

[0149]

[0150] in, It is the angle after the fusion at the previous moment. It is the angular velocity currently measured by the gyroscope. It is the sampling period. It is a proportional gain that controls the degree to which accelerometer error is used to correct gyroscope integral drift.

[0151] Take Kalman filtering as an example:

[0152] A state-space model is constructed, using the attitude angle and gyroscope bias as state variables and the attitude calculated by the accelerometer as the observation. Through the "prediction-update" step of Kalman filtering, the current attitude is optimally estimated, and the gyroscope bias is estimated simultaneously, thus suppressing drift at its source.

[0153] State vector ,

[0154] in, The roll angle is the angle at which the robot body 1 rotates around the X-axis of its body coordinate system. In this embodiment, the roll angle is obtained by integrating the angular velocity data of the inertial measurement unit (IMU) (quaternion method), and then output after correction by acceleration data.

[0155] The pitch angle is the angle at which the robot body 1 rotates around the Y-axis of its body coordinate system. In this embodiment, the pitch angle is obtained by integrating the angular velocity data of the inertial measurement unit (IMU) (quaternion method), and then output after correction by acceleration data.

[0156] The zero bias of the X-axis gyroscope is the constant output error of the gyroscope on the X-axis, usually measured in rad / s. In this embodiment, it is derived from the constant error (zero bias) of the gyroscope on the X-axis estimated by Kalman filtering during the attitude calculation process.

[0157] The zero bias of the Y-axis gyroscope is the constant output error of the gyroscope on the Y-axis, which is usually measured in rad / s. In this embodiment, it is derived from the constant error (zero bias) of the gyroscope on the Y-axis estimated by Kalman filtering during the attitude calculation process.

[0158] State prediction is performed using gyroscope data (minus the estimated zero bias) and a quaternion dynamics model. When the accelerometer data is reliable, the calculated... The observation is used to update the state vector. The Kalman gain automatically determines the extent to which this observation is trusted, ultimately outputting a corrected, smoothed roll angle. and pitch angle This result retains the gyroscope's ability to respond to high-frequency motion while also possessing the long-term stability of the accelerometer, effectively eliminating the drift caused by pure integration.

[0159] Sa4. Output the smooth roll and pitch angle data after fusion correction in step Sa3. At the same time, compare the real-time attitude data, i.e. the smooth roll and pitch angle data after fusion correction, with the preset safe attitude threshold. When any angle data exceeds the safe threshold, immediately send a risk alarm signal to the motion control module.

[0160] Furthermore, the motion control module receives guidance instructions generated by the navigation module and task instructions input from the outside, calculates the specific target speed, displacement or force required to drive each actuator, coordinates the synchronous operation of multiple actuators, receives system alarms, and executes the highest priority safety strategy.

[0161] Specifically, the process of calculating the specific target rotational speed, displacement, or force required to drive each actuator includes:

[0162] For example, the motion control module first integrates guidance instructions from the navigation module, such as "turn left 5 degrees" and "move forward at a speed of 0.5 m / s", and task instructions from the ground, such as "start dredging" and "retract the robotic arm". Combined with the current system state, it determines the robot's working mode (such as "traveling mode", "dredging mode" and "emergency surfacing mode"), thereby defining the macroscopic goal of control.

[0163] Based on the macroscopic control objective, the built-in robot kinematics and dynamics models are invoked for calculations, converting the desired overall motion into the desired motion quantities of each actuator. For example, for tracked drive, the desired rotational speeds of the left and right tracks are calculated using the tracked robot kinematics model. :

[0164] ,

[0165] Where B is the track spacing and r is the drive wheel radius. This is the forward velocity, which is the linear velocity of the robot's center of mass along its own forward direction (usually the X-axis of the body coordinate system). It is the turning angular velocity, which is the angular velocity of the robot body 1 rotating about a vertical axis (i.e., the Z-axis of the robot body coordinate system) that passes through its center of mass and is perpendicular to the ground / bottom plane.

[0166] Specifically, if >0, the speed of the left track is less than the speed of the right track, the robot turns left; if If < 0, the robot turns right; if =0 and ≠0, the speeds of the left and right tracks are equal in magnitude and opposite in direction, and the robot body 1 achieves rotation in place.

[0167] Considering underwater resistance, gravity, buoyancy, and the robot's dynamics, calculate the total force and torque required to achieve the above motion. ;

[0168] The required force and torque are distributed to the specific actuators, and the final control command is calculated, running a PID controller or other advanced control algorithm. The controller uses the difference between the desired motion state (e.g., desired speed) and the actual state fed back by the sensors (e.g., actual speed). As input, calculate the control output. :

[0169] ,

[0170] in, The differential gain is used to predict the future trend of error changes, that is, by calculating the differential / derivative of the error.

[0171] For example, control allocation is performed for each actuator:

[0172] Total force and torque The target thrust for each actuator is obtained by allocating it to each propeller and track drive mechanism according to optimization criteria. or torque For the dredging mechanism, the target rotational speed of the cutter head 3 is directly calculated based on the intensity of the task instruction. and the target flow rate of slurry pump 5 or power Ultimately, these physical quantities are converted into target rotational speed, target displacement, or target force that the actuator driver can receive.

[0173] Throughout the process, the motion control module continuously monitors system alarms from other modules, such as attitude alarms and energy failures. Once it receives the highest priority alarm, such as the risk of overturning, it immediately interrupts the current calculation process and forcibly executes preset safety strategies, such as "zero speed command for all motors", "release ballast blocks", and "emergency buoyancy", overriding normal control outputs.

[0174] Furthermore, the power supply control module dynamically performs power distribution according to the power request of the motion control module, monitors the bus voltage, current, power and temperature in real time, performs protection operations when abnormal conditions occur, and reports the health status and fault signals of the energy system in real time.

[0175] For example, the power supply control module samples key parameters of the energy system at a fixed frequency (e.g., 100 times per second):

[0176] Bus voltage V is used to monitor whether the power supply output voltage is stable.

[0177] Bus current I monitors the total load current of the system.

[0178] The remaining battery charge (SOC) is estimated using a coulomb counter or a voltage-capacity model.

[0179] Temperature T is used to monitor the temperature of critical components such as batteries and power devices.

[0180] Based on the real-time status, dynamically calculate the total power that can be safely output at present.

[0181]

[0182] For example, when the battery is low or the temperature is too high, It will be lowered to protect the battery and extend its lifespan.

[0183] Dynamic power distribution:

[0184] Receive power requests from the motion control module. For example, the request typically includes the power values ​​and priority identifiers of each actuator, such as the travel motor, cutter head 3, slurry pump 5, etc.

[0185] For example:

[0186] Scenario 1: Total requested power ≤ Total power budget ( ≤ Power will be allocated in full upon request to ensure all functions operate normally;

[0187] Scenario 2: Total requested power > Total power budget ( > The priority scheduling algorithm is activated, and non-critical loads are derated or shut down according to a preset priority order, such as ensuring safety and navigation > ensuring movement > dredging operations. For example, the speed of the cutter head mechanism 3 or the flow rate of the slurry pump 5 can be reduced to ensure that the track drive and control system have sufficient power to prevent the robot from losing position or control.

[0188] The allocation decision is translated into specific control signals, such as adjusting the output power of the motor driver through PWM (Pulse Width Modulation) signals; controlling the on / off state of solid-state relays or MOSFETs to connect or disconnect the power supply to non-critical circuits.

[0189] Fault protection and reporting:

[0190] The monitored parameters (V, I, T) are compared with preset safety thresholds. Once any parameter exceeds the limit, the corresponding protection mechanism is immediately triggered, and its severity increases with the severity of the fault.

[0191] Level 1 alarm: If the temperature is slightly too high, limit the maximum output power;

[0192] Secondary protection: If the voltage is severely low or the current is severely overloaded, immediately disconnect the power supply to non-critical loads, such as the dredging mechanism;

[0193] Level 3 protection: In the event of a short circuit or severe overheating, system-level power failure will be implemented;

[0194] Status reporting: Regardless of whether it is normal or not, the module will report the health status of the energy system (including SOC, (Fault codes, etc.) are encapsulated into data frames and reported to the ground control system in real time through the data and image transmission module.

[0195] The specific embodiments of the invention have been described in detail above, but these are merely examples. The invention is not limited to the specific embodiments described above. Those skilled in the art should understand that the embodiments and descriptions in the specification are only illustrative of the principles of the invention. Various changes and modifications can be made to the invention without departing from its spirit and scope, and all such changes and modifications fall within the scope of the invention as claimed. The scope of protection of this invention is defined by the appended claims and their equivalents.

Claims

1. A control system for an underwater dredging robot, characterized in that, include: The data and image transmission module collects and transmits underwater environmental data and image data in real time. The navigation module monitors the positioning of the underwater dredging robot and performs path planning for it. The attitude analysis module is used to monitor and analyze the attitude status of the underwater dredging robot in real time for stability monitoring and early warning. The navigation module receives and fuses raw data from the inertial measurement unit, Doppler log, and ultra-short baseline positioning system, and eliminates errors and drift from a single sensor through a filtering algorithm; Output the global status of the underwater dredging robot; The lateral and directional deviations of the underwater dredging robot from the predetermined path are calculated, and guidance commands are generated and sent to the motion control module. The process by which the navigation module outputs the global status of the underwater dredging robot includes: S1. Based on the angular velocity and linear acceleration data output by the inertial measurement unit at the previous moment, the first position information, first velocity information and first attitude information of the underwater dredging robot at the current moment are predicted by mechanical integration method. The predicted state contains cumulative error due to integration operation. S2. Simultaneously receive relative velocity observation data from the Doppler log, absolute depth observation data from the depth gauge, absolute position observation data from the ultra-short baseline positioning system, and orientation observation data from the magnetometer. S3. Based on the predicted state obtained in step S1, and constrained by the various observation data obtained in step S2, the extended Kalman filter algorithm is used to perform data fusion. The fusion process is as follows: calculate the difference between each observation data and the corresponding predicted value, and then use the Kalman gain as the weight to perform weighted correction on the difference, and finally output the optimized global state at the current moment that eliminates the cumulative error of the sensor. The global state of the underwater dredging robot includes the fused three-dimensional coordinate position, three-dimensional motion speed, and fused attitude data consisting of roll angle, pitch angle, and yaw angle, used for global positioning and heading control.

2. The underwater dredging robot control system according to claim 1, characterized in that, The data and image transmission module periodically acquires the latitude and longitude coordinates, depth, speed, and orientation angle data of the underwater dredging robot from the navigation module; The robot's roll and pitch angle data are periodically acquired from the attitude analysis module; Obtain the status of the currently executed command and the status data of the dredging mechanism from the motion control module; Obtain bus voltage, total current, remaining battery power, and temperature alarm signal data from the power supply control module; Acquire image data directly from the camera; All sensor and control data are packaged into a custom structured data frame, timestamped, compressed, and encrypted to obtain a complete data packet; The data and image transmission module multiplexes the video stream, forming a single transmission stream together with the structured data frames.

3. The underwater dredging robot control system according to claim 1, characterized in that, The attitude analysis module independently calculates and outputs roll and pitch angle data based on inertial measurement unit data, and determines in real time whether the robot's attitude is within the safe threshold. When the tilt angle data exceeds the preset safety threshold, it is determined that there is a risk of overturning, and an emergency alarm signal is sent to the motion control module.

4. The underwater dredging robot control system according to claim 3, characterized in that, The process by which the attitude analysis module calculates and outputs roll and pitch angle data includes: Sa1. Preprocess the raw angular velocity data and raw acceleration data output by the inertial measurement unit. The preprocessing includes calibration and filtering. Sa2. Based on the angular velocity data preprocessed in step Sa1, integral calculation is performed using the quaternion method to update and predict the attitude change of the underwater dredging robot in real time, and obtain the first attitude information including roll angle and pitch angle. Sa3. The acceleration data preprocessed in step Sa1 is introduced to correct the first attitude information. Specifically, during the time period when the motion acceleration of the underwater dredging robot is less than the preset smooth threshold, the component of the gravity acceleration vector in the body coordinate system is used as the observation reference. The second attitude information of roll angle and pitch angle is calculated by complementary filtering or Kalman filtering algorithm, and the second attitude information is used to correct the drift of the first attitude information caused by integration. Sa4. Output the smooth roll and pitch angle data after fusion correction in step Sa3. At the same time, compare the real-time attitude data, i.e. the smooth roll and pitch angle data after fusion correction, with the preset safe attitude threshold. When any angle data exceeds the safe threshold, immediately send a risk alarm signal to the motion control module.

5. The underwater dredging robot control system according to claim 1, characterized in that, The control system further includes: The motion control module generates and executes the robot's motion commands; The power supply control module manages the energy distribution and power status of the dredging robot.

6. The underwater dredging robot control system according to claim 5, characterized in that, The motion control module receives guidance commands generated by the navigation module and task commands input from the outside, calculates the specific target speed, displacement or force required to drive each actuator, coordinates the synchronous operation of multiple actuators, receives system alarms, and executes the highest priority safety strategy.

7. The underwater dredging robot control system according to claim 5, characterized in that, The power supply control module dynamically distributes power according to the power request from the motion control module, monitors the bus voltage, current, power, and temperature in real time, performs protection operations when abnormal conditions occur, and reports the health status and fault signals of the energy system in real time.

8. An underwater dredging robot, equipped with the underwater dredging robot control system according to any one of claims 1-7, characterized in that: It includes the robot body, as well as a drive mechanism located at the bottom of the robot body, a cutter head mechanism located at one end of the robot body, a hydraulic station and a slurry pump located on the robot body.

Images