Degree of freedom realization device and control method for aquatic inspection robot

Abstract

The present invention relates to the field of aquatic product inspection technology, and more specifically to a device for realizing degrees of freedom for an aquatic product inspection robot and a control method thereof. The method comprises: dividing the lifting motion trajectory and the rotational motion trajectory into seven-stage acceleration S-curves based on a target travel distance and a target rotation angle; jointly optimizing the parameters of the dual-acceleration S-curves using a multi-axis collaborative planning strategy to obtain the optimal dual-acceleration S-curve parameters; and then calculating the seven-stage velocity S-curves of the lifting motion trajectory and the rotational motion trajectory as velocity mapping rules; acquiring the lifting distance and the rotation angle in real time to calculate the actual lifting speed and the actual rotation speed; and dynamically adjusting the velocity output according to the velocity mapping rules based on the actual lifting speed and the actual rotation speed. This method achieves multi-angle detection of the suspended rail aquatic product inspection robot while taking into account the stability of the suspension structure and the smoothness of the compound motion, thereby effectively improving detection accuracy.

Description

Technical Field

[0001] The present invention relates to the technical field of aquatic product inspection, and in particular to a device for realizing degrees of freedom of an aquatic product inspection robot and a control method thereof. Background Art

[0002] Factory-scale recirculating aquaculture utilizes modern engineering and information technology to create an ecological environment suitable for fish growth, achieving high-density, high-efficiency, and low-impact aquaculture. In recent years, factory-scale recirculating aquaculture has grown rapidly, with mechanization, digitization, and intelligentization becoming the inevitable direction for the development of modern, green, and efficient aquaculture. In factory-scale recirculating aquaculture, real-time collection, identification, and analysis of aquaculture water quality indicators, fish growth and behavior, and on-site conditions provide data support for intelligent production management decisions, including precise water treatment and regulation, precise feeding, and risk warning.

[0003] At present, aquatic inspection robots, which serve as data collection, identification and analysis devices in factory-scale recirculating aquaculture production, have developed into suspended track aquatic inspection robots, such as Chinese patent CN216577835U - Aquaculture Track-Type Inspection Robot, and Chinese patent CN219027565U - A Water Quality Inspection Assembly Suitable for Factory-Scale Aquaculture. These robots enable the sensors in the aquatic inspection robots to be extended and retracted, only entering the water when water quality needs to be tested, and leaving the water surface when the test is completed, thus achieving intermittent testing of multiple aquaculture ponds. As a result, the accuracy is more stable and the service life is longer.

[0004] However, the telescopic boom in the existing technology can only be raised and lowered vertically, resulting in a fixed detection angle and difficulty in covering multi-angle underwater environments. Even when combined with a camera rotating pan / tilt head, its lifting motor and rotating motor are simply independently controlled according to preset parameters, which has significant limitations. Specifically:

[0005] 1. Static planning parameters are difficult to adapt to dynamic load changes: The parameters of a single motor are preset based on fixed scenarios and cannot be adjusted in real time according to the combined motion of lifting and rotation. In multi-axis motion, the start and stop timing and speed change rate of lifting and rotation will lead to motion conflicts. For example, when lifting suddenly stops, the rotation inertia causes swinging.

[0006] 2. It is difficult to balance the stability of the suspension structure and the smoothness of the compound motion: Since the center of gravity of the suspended rail aquatic inspection robot is suspended in the air, the mechanical structure is particularly sensitive to inertial disturbances. Therefore, under the independent control of the lifting motor and the rotating motor, it is very easy to cause mutual interference between the torque fluctuations of the two axes in the compound motion. For example, the lateral centrifugal force during rotational acceleration has an adverse effect on the lifting displacement accuracy, and the lateral torque of the lifting mechanism caused by the rotational angular acceleration causes unexpected deformation of the telescopic rod. Moreover, during multi-axis compound motion, the superposition of the inertia of the two axes will cause the suspension equipment to resonate, thereby exacerbating the swing. Summary of the Invention

[0007] In response to the above technical problems, the present invention proposes a degree of freedom realization device and a control method for aquatic inspection robots, aiming to achieve multi-angle detection of suspended rail aquatic inspection robots while taking into account the stability of the suspension structure and the smoothness of the compound motion, thereby effectively improving the detection accuracy.

[0008] In a first aspect, the present application provides a control method for a device for realizing degrees of freedom of an aquatic inspection robot, comprising the following steps:

[0009] Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7 stages of acceleration S-shaped curves, respectively, to obtain a double acceleration S-shaped curve. The stages include acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration.

[0010] The multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual-acceleration S-curve and obtain the optimal dual-acceleration S-curve parameters.

[0011] Based on the optimal double acceleration S-curve parameters, the 7-stage velocity S-curve of the lifting and rotating motion trajectories is calculated as the velocity mapping rule.

[0012] Obtain lifting distance and rotation angle in real time, and calculate actual lifting speed and actual rotation speed;

[0013] Based on the actual lifting speed and the actual rotation speed, the PID control algorithm is used to dynamically adjust the speed output according to the speed mapping rules.

[0014] In some embodiments, a multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual-acceleration S-curve to obtain the optimal dual-acceleration S-curve parameters, including:

[0015] Establish the biaxial coupling relationship of vertical force balance and rotational torque balance;

[0016] Defining multi-axis motion constraints, wherein the constraints include mechanical strength constraints and motion interference constraints;

[0017] A parameter optimization function is constructed with the goal of minimizing the total motion time and inertial impact force. The Lagrange multiplier method is used to solve the parameter optimization function under the constraints to obtain the optimal double S-shaped curve parameters.

[0018] In some embodiments, the dual-axis coupling relationship of vertical force balance and rotational torque balance includes the balance between the vertical inertia force and the vertical component of the rotational coupling and the lifting drive force, and the balance between the rotational inertia torque and the lifting force coupling torque and the rotational torque.

[0019] In some embodiments, the expression of the parameter optimization function is:

[0020]

[0021] Where T represents the total exercise time of the 7 stages, represents the lifting acceleration, Indicates rotational acceleration.

[0022] In some embodiments, after “dynamically adjusting the speed output using a PID control algorithm according to a speed mapping rule based on the actual lifting speed and the actual rotation speed,” the method further includes:

[0023] Establishing a suspension swing model, wherein the suspension swing model is used to predict the swing angle of the suspension device during movement based on the acceleration of the double S curve;

[0024] Set the feedforward gain;

[0025] Multiply the feedforward gain by the predicted swing angle to obtain the feedforward control amount based on the swing amplitude;

[0026] The feedforward control quantity based on the swing amplitude is superimposed on the PID speed output as the final speed output to offset the speed disturbance caused by the predicted swing.

[0027] In some embodiments, the following steps are also included:

[0028] When the deviation between the actual lifting speed or actual rotation speed in any stage and the planned value in the speed mapping rule exceeds the preset deviation threshold, the current stage is terminated and the system directly jumps to the "deceleration stage", and the feedforward control amount is linearly decayed to 0.

[0029] In a second aspect, the present application provides a device for realizing the degree of freedom of an aquatic inspection robot, comprising a horizontal rotation mechanism, a telescopic lifting mechanism, a multi-device hanging platform, and a control module. The horizontal rotation mechanism is rotatably mounted below the aquatic inspection robot, the telescopic lifting mechanism is fixedly mounted at the bottom of the horizontal rotation mechanism so that the telescopic lifting mechanism is driven to rotate when the horizontal rotation mechanism rotates, the multi-device hanging platform is fixedly mounted at the bottom of the telescopic lifting mechanism, the multi-device hanging platform is used to install multiple detection equipment required by the aquatic inspection robot, the horizontal rotation mechanism and the telescopic lifting mechanism are both connected to the control module, and the control module is used to perform the following steps:

[0030] Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7 stages of acceleration S-shaped curves, respectively, to obtain a double acceleration S-shaped curve. The stages include acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration.

[0031] The multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual-acceleration S-curve and obtain the optimal dual-acceleration S-curve parameters.

[0032] Based on the optimal double acceleration S-curve parameters, the 7-stage velocity S-curve of the lifting and rotating motion trajectories is calculated as the velocity mapping rule.

[0033] Obtain lifting distance and rotation angle in real time, and calculate actual lifting speed and actual rotation speed;

[0034] Based on the actual lifting speed and the actual rotation speed, the PID control algorithm is used to dynamically adjust the speed output according to the speed mapping rules.

[0035] In some embodiments, the telescopic lifting mechanism includes several sections of telescopic sleeves, and the cross-section of the telescopic sleeves is elliptical.

[0036] In some embodiments, the multi-device hanging platform includes an upper bracket and a lower bracket. The upper bracket is fixedly connected to the bottom of the telescopic lifting mechanism, and the upper bracket is fixedly connected to the lower bracket. The lower bracket has a polygonal structure and is provided with mounting holes on each side of the lower bracket.

[0037] In some embodiments, a waterproof proximity switch is further included. The waterproof proximity switch is located at the bottom of the multi-device hanging platform. The waterproof proximity switch is used to monitor the distance to the bottom of the breeding pond in real time. The waterproof proximity switch is fixedly connected to the control module.

[0038] The beneficial technical effects of the present invention include at least:

[0039] 1. A degree of freedom realization device and control method for aquatic inspection robots are adopted. Through the coordinated integration of double S-curve segmented planning, multi-axis parameter joint optimization based on multi-axis collaborative planning strategy and real-time closed-loop feedback control, a creative breakthrough is made in the technical bottlenecks of the existing suspended rail aquatic inspection robots, such as multi-axis motion conflicts and dynamic load changes that inevitably exist during multi-axis motion, leading to control instability and low detection accuracy. Specifically, the application divides the lifting and rotation trajectories into 7-stage acceleration curves (acceleration → deceleration) based on the target travel distance and the safety threshold. The dual-axis start-stop timing and speed change rate are synchronously planned through the speed mapping rule, so that the lifting uniform speed segment and the rotation acceleration segment are staggered, thereby suppressing the co-existence caused by the superposition of the two-axis torque. Vibration is eliminated to avoid swinging caused by rotational inertia during lifting and emergency stopping. At the same time, the trajectory is dynamically calibrated based on the real-time feedback data of the rotary encoder to ensure that the two axes strictly follow the dual-speed S-curve, improve the smoothness of compound motion, and eliminate the motion deviation caused by load changes of multiple equipment hanging on the platform. Through the deep coordination of technical means, the limitations of traditional multi-axis isolated control are broken through, and the integrated control of "trajectory smoothing-dynamic anti-interference" is realized. While realizing multi-angle detection of the suspended rail aquatic inspection robot, it takes into account the stability of the suspension structure and the smoothness of compound motion, effectively improving the detection efficiency and detection accuracy of the suspended rail aquatic inspection robot under complex working conditions, and providing highly reliable and full-view underwater environment monitoring capabilities for factory-scale recirculating aquaculture.

[0040] 2. The multi-axis collaborative planning strategy designed in this application deeply integrates theoretical mechanics with real-time control algorithms, quantifies the dual-axis coupling effect through the dynamic equations of multi-degree-of-freedom motion, dynamically binds the acceleration / jerk parameters of the 7-stage S-shaped curve with the real-time load (suspension equipment mass, telescopic sleeve elongation), dynamically allocates the vertical driving force output by the lifting motor and the torque output by the rotating motor, accurately compensates for the interference of rotation on lifting, effectively suppresses the swing amplitude, improves control accuracy, combines constraint optimization to avoid over-limit, and jointly optimizes the dual-axis parameters to balance time and impact force, thereby shortening the detection cycle to the maximum extent, improving detection efficiency, and being able to... The optimal speed S-curve parameters are dynamically updated and adjusted based on the total mass of the telescopic sleeve and suspension equipment, the deflection angle of the horizontal rotation mechanism relative to the vertical direction, and the current extension length of the telescopic sleeve. This overcomes the technical problem of multi-axis motion interference leading to control instability that is inevitable in multi-axis motion of suspended rail-mounted aquatic inspection robots (such as the additional torque generated by inertial coupling of the rotating motor during lifting and acceleration), reduces the mechanical vibration energy under compound motion, and realizes the "global optimal - real-time response - safe and stable" trinity motion control of the suspended rail-mounted aquatic inspection robot, effectively improving the detection accuracy, reliability, and efficiency of multi-axis synchronous motion.

[0041] 3. Through the synergistic effect of the suspension swing model and feedforward-PID composite control, the problems of suspension swing and mechanical instability caused by lifting acceleration are creatively solved. Specifically, the acceleration based on the double S curve drives the suspension swing model in real time, directly linking motion planning and swing prediction, so that the feedforward compensation amount accurately matches the current acceleration stage, actively offsetting the theoretical swing. At the same time, the model parameters are dynamically updated according to the total mass of the telescopic sleeve and suspension equipment, the moment of inertia of the rotating mechanism and the elongation length, so that the feedforward gain adapts to the load change and avoids excessive mechanical stress caused by overcompensation. The main swing caused by the sudden acceleration change is quickly eliminated through feedforward compensation, and the PID closed loop is superimposed to suppress residual disturbances, further improving the speed smoothness of the double S curve planning and effectively improving the stability of image acquisition, providing highly reliable support for multi-angle detection in complex underwater environments.

[0042] Other features and advantages of the present invention will be disclosed in detail in the following specific embodiments and drawings. BRIEF DESCRIPTION OF THE DRAWINGS

[0043] The present invention will be further described below with reference to the accompanying drawings:

[0044] Figure 1 This is a flow chart of a control method for a device for realizing degrees of freedom of an aquatic inspection robot according to an embodiment of the present invention.

[0045] Figure 2 The figure is a schematic structural diagram of a device for realizing degrees of freedom of an aquatic inspection robot according to an embodiment of the present invention.

[0046] Figure 3 This is a bottom view of a device for achieving degrees of freedom for aquatic inspection robots according to an embodiment of the present invention.

[0047] Figure 4 This is a schematic diagram of the structure of a multi-device mounting platform according to an embodiment of the present invention. DETAILED DESCRIPTION

[0048] The following is an explanation and description of the technical solutions of the embodiments of the present invention in conjunction with the drawings of the embodiments of the present invention. However, the following embodiments are only preferred embodiments of the present invention and are not exhaustive. Based on the embodiments in the implementation manner, other embodiments obtained by those skilled in the art without creative work are all within the scope of protection of the present invention.

[0049] In the following description, terms such as "inside", "outside", "up", "down", "left", "right", etc. that indicate directions or positional relationships are only used to facilitate the description of the embodiments and simplify the description, and do not indicate or imply that the device or element referred to must have a specific direction, be constructed and operate in a specific direction. Therefore, they should not be understood as limiting the present invention.

[0050] Example 1:

[0051] Please see the attached Figure 1 , Figure 1 A flow chart of a control method for a device for realizing degrees of freedom of an aquatic inspection robot provided in one embodiment of this specification is shown.

[0052] like Figure 1 As shown, the control method of the device for realizing the degree of freedom of the aquatic inspection robot may include at least the following steps:

[0053] In step 101, based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7 stages of acceleration S-shaped curves, respectively, to obtain a double acceleration S-shaped curve, wherein the stages include acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration.

[0054] Specifically, in this embodiment, based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7-stage acceleration S-shaped curves respectively. The implementation method is as follows:

[0055] First, obtain the target travel distance, target rotation angle, and safety distance threshold (minimum allowable distance from the bottom of the fish pond) set by the host computer or built-in task. Read the initial position of the rotary encoder, including the initial lift height and initial rotation angle. Also, collect the distance to the bottom of the fish pond measured in real time by the waterproof proximity switch.

[0056] Then, for the lifting motion (vertical axis) and rotation motion (horizontal axis), a 7-stage acceleration curve is planned, namely acceleration (t1) -> uniform acceleration (t2) -> deceleration (t3) -> uniform speed (t4) -> acceleration and deceleration (t5) -> uniform deceleration (t6) -> deceleration and deceleration (t7). Set the maximum lifting acceleration and the maximum jerk J (jerk indicates the change in acceleration), the acceleration S-curve of the lifting motion trajectory is It can be expressed as:

[0057]

[0058] Set the maximum rotational angular acceleration and maximum jerk, and the acceleration S-curve of the rotational motion trajectory The expression of the lifting motion trajectory is similar to that of the lifting motion trajectory, but in terms of angular displacement Instead of linear displacement.

[0059] Step 102 : Using a multi-axis collaborative planning strategy to jointly optimize the parameters of the dual-acceleration S-curve to obtain the optimal dual-acceleration S-curve parameters.

[0060] Specifically, in this embodiment, a multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual-acceleration S-curve to obtain the optimal dual-acceleration S-curve parameters, including:

[0061] Step 201, establishing a biaxial coupling relationship for vertical force balance and rotational torque balance;

[0062] Furthermore, in this embodiment, the biaxial coupling equations for vertical force balance and rotational torque balance include the balance between the vertical inertial force and the vertical component of the rotational coupling and the lift drive force, and the balance between the moment of inertia torque and the lift coupling torque and the rotational torque. The vertical component of the rotational coupling is the vertical component of the centrifugal force caused by the rotational angular acceleration, and the lift coupling torque is the torque exerted on the rotating axis by the vertical drive force output by the lift motor.

[0063] In this embodiment, the biaxial coupling relationship of vertical force balance and rotational torque balance can be expressed as:

[0064]

[0065] in, It represents the vertical driving force output by the lifting motor, m represents the total mass of the telescopic sleeve and the suspension equipment, represents the vertical inertial force, I represents the moment of inertia of the rotating mechanism (including load), θ represents the deflection angle of the horizontal rotating mechanism relative to the vertical direction, Indicates the torque output by the rotating motor, It represents the moment of inertia and inertia torque, and L represents the current extension length of the telescopic sleeve.

[0066] It can be understood that Equation 1 indicates that the lifting driving force needs to overcome the vertical component caused by its own mass acceleration and rotational angular acceleration, and Equation 2 indicates that the rotational torque needs to overcome its own rotational inertia acceleration and the torque coupling of the lifting force on the rotating axis.

[0067] It can be understood that the multi-axis collaborative planning strategy proposed in this embodiment can be dynamically updated and adjusted according to the total mass of the telescopic sleeve and the suspension device, the moment of inertia of the rotating mechanism, the deflection angle of the horizontal rotating mechanism relative to the vertical direction, and the current extension length of the telescopic sleeve. It can also be set to re-adopt the multi-axis collaborative planning strategy every 5 seconds to jointly optimize the parameters of the dual acceleration S-curve and calculate the optimal parameters once.

[0068] Step 202 : defining multi-axis motion constraints, which include mechanical strength constraints and motion interference constraints.

[0069] It is understandable that the goal of the multi-axis motion constraint is to prevent the compound motion from exceeding the strength limit of the mechanical structure and solve the mechanical fatigue problem caused by the suspension swing in the suspended track aquatic inspection robot. Therefore, the mechanical strength constraint can be expressed as:

[0070]

[0071] in, is the maximum thrust allowed by the motor, The maximum torque allowed for the worm gear;

[0072] The motion interference constraint can be expressed as:

[0073]

[0074] in, It represents the experimentally measured safety threshold of acceleration product to avoid resonance.

[0075] Step 203 : constructing a parameter optimization function with the goal of minimizing the total motion time and the inertial impact force, and solving the parameter optimization function using the Lagrange multiplier method under the constraint conditions to obtain the optimal double S-shaped curve parameters.

[0076] Specifically, in this embodiment, the optimization parameters include:

[0077] Lifting motor S-curve parameters: (7-stage time allocation), ;

[0078] Rotating motor S-curve parameters: (7-stage time allocation), .

[0079] Under the constraints, the Lagrange multiplier method is used to solve the parameter optimization function, so as to achieve the shortest movement time and the smallest impact. The expression of the parameter optimization function in this embodiment is:

[0080]

[0081] Where T represents the total exercise time of the 7 stages, represents the lifting acceleration, represents the rotational acceleration, and It is understandable that in this embodiment, the cumulative impact force is represented by the square integral of acceleration, that is, the integral term is used to penalize the acceleration product, thereby suppressing the vibration energy.

[0082] Introducing Lagrange multipliers Processing constraints can be expressed as:

[0083]

[0084] Then, by solving the functional extreme value through a numerical method (such as sequential quadratic programming SQP, etc.), the optimization result of the dual acceleration S-shaped curve parameters can be obtained. This embodiment will not be repeated here.

[0085] On the other hand, in this embodiment, the optimal acceleration S-curve parameters of the lifting motion trajectory and the optimal acceleration S-curve parameters of the rotation motion trajectory are obtained. Afterwards, a multi-axis synchronization constraint strategy can be designed to determine the time allocation of the optimal acceleration S-curve of the rotational motion trajectory. Specifically, the optimal time allocation of the optimal acceleration S-curve of the lifting motion trajectory is multiplied by a preset time synchronization coefficient (0.8-1.2) to obtain the optimal time allocation of the optimal acceleration S-curve of the rotational motion trajectory, thereby preventing mechanical interference caused by excessive phase difference between the lifting and rotation movements.

[0086] The multi-axis collaborative planning strategy designed in this embodiment deeply integrates theoretical mechanics with real-time control algorithms, quantifies the dual-axis coupling effect through the dynamic equations of multi-degree-of-freedom motion, dynamically binds the acceleration / jerk parameters of the 7-stage S-shaped curve with the real-time load (suspension equipment mass, telescopic sleeve elongation), dynamically allocates the vertical driving force output by the lifting motor and the torque output by the rotating motor, accurately compensates for the interference of rotation on lifting, effectively suppresses the swing amplitude, improves control accuracy, combines constraint optimization to avoid over-limit, jointly optimizes dual-axis parameters to balance time and impact force, maximizes the shortening of the detection cycle, improves detection efficiency, and can be based on The optimal speed S-curve parameters are dynamically updated and adjusted according to the total mass of the telescopic sleeve and the suspension equipment, the deflection angle of the horizontal rotation mechanism relative to the vertical direction, and the current extension length of the telescopic sleeve. This overcomes the technical problem of control instability caused by multi-axis motion interference that is inevitable in multi-axis motion of the suspended rail-type aquatic inspection robot (such as the additional torque generated by inertial coupling of the rotating motor during lifting and acceleration), reduces the mechanical vibration energy under compound motion, and realizes the "global optimal-real-time response-safe and stable" three-in-one motion control of the suspended rail-type aquatic inspection robot, effectively improving the detection accuracy, reliability and efficiency of multi-axis synchronous motion.

[0087] Step 103 : Based on the optimal dual acceleration S-curve parameters, a 7-stage velocity S-curve of the lifting motion trajectory and the rotation motion trajectory is calculated and obtained as a velocity mapping rule.

[0088] The optimization results are embedded in the double acceleration S-curve of the original solution. According to the relationship between velocity and acceleration 𝑉=𝑎𝑡, and the relationship between jerk and velocity 𝑉=𝐽𝑡 2 / 2, the 7-stage velocity S-shaped curve of the lifting motion trajectory and the rotation motion trajectory can be calculated and expressed as:

[0089]

[0090] in, , (i=1,2,3,…,7).

[0091] Step 104 , obtaining the lifting distance and the rotation angle in real time, and calculating the actual lifting speed and the actual rotation speed.

[0092] It can be understood that the rotary encoder of the lifting motor feeds back the lifting distance h in real time, and the rotary encoder of the rotating motor feeds back the rotation angle θ in real time, then the actual lifting speed v=Δh / Δt, and the actual rotation speed w=Δθ / Δt.

[0093] Step 105 : Based on the actual lifting speed and the actual rotation speed, the speed output is dynamically adjusted using a PID control algorithm according to a speed mapping rule.

[0094] It can be understood that the motor receives the planned speed value output by the speed mapping rule, realizes stepless speed regulation through PWM duty cycle adjustment, and at the same time the rotary encoder feeds back the actual speed for calibration, so that the lifting motor executes the optimal speed S-curve parameters of the lifting motion trajectory, and the rotating motor executes the optimal speed S-curve parameters of the rotating motion trajectory.

[0095] Furthermore, this embodiment may also include the following steps:

[0096] A waterproof proximity switch is used to detect the distance between the bottom of the aquatic inspection robot and the bottom of the breeding pond in real time;

[0097] When the distance between the aquatic inspection robot and the bottom of the breeding pond is less than the safety distance threshold, an emergency stop is triggered, and the robot is forced to jump directly into the "deceleration stage". At the same time, the integral term of the PID is cleared to avoid overshoot and mechanical collision.

[0098] In summary, this embodiment creatively breaks through the technical bottlenecks of multi-axis motion conflicts and dynamic load changes that inevitably exist in the existing suspended rail aquatic inspection robots during multi-axis motion, such as control instability and low detection accuracy, through the coordinated integration of double S-curve segmented planning, multi-axis parameter joint optimization based on multi-axis collaborative planning strategy, and real-time closed-loop feedback control. Specifically, this embodiment divides the lifting and rotation trajectories into 7-stage acceleration curves (acceleration → deceleration) based on the target travel distance and safety threshold, and synchronously plans the dual-axis start-stop timing and speed change rate through the speed mapping rule, so that the lifting uniform speed section and the rotation acceleration section are executed at staggered times, suppressing the resonance caused by the superposition of the two-axis torque, and avoiding rotation during lifting and stopping suddenly. The swing caused by rotational inertia is dynamically calibrated with the data of real-time feedback from the rotary encoder to ensure that the dual axes strictly follow the dual-speed S-curve, improve the smoothness of the compound motion, and eliminate the motion deviation caused by the load change of the platform with multiple devices. Through the deep coordination of technical means, it has broken through the limitations of traditional multi-axis isolated control and realized the integrated control of "trajectory smoothing-dynamic anti-interference". It can realize multi-angle detection of the suspended rail aquatic inspection robot while taking into account the stability of the suspension structure and the smoothness of the compound motion, effectively improving the detection efficiency and detection accuracy of the suspended rail aquatic inspection robot under complex working conditions, and providing highly reliable and full-view underwater environment monitoring capabilities for factory-scale recirculating aquaculture.

[0099] Example 2:

[0100] This embodiment only compares Figure 1 The difference between the two methods is described in detail in the first embodiment. The technical concept of the remaining method design is similar to that of the first embodiment and will not be repeated in this embodiment. In this embodiment, after "dynamically adjusting the speed output by using a PID control algorithm according to the speed mapping rule based on the actual lifting speed and the actual rotation speed", it also includes:

[0101] Step 106: Establish a suspension swing model. The suspension swing model is used to predict the swing angle of the suspension device during movement based on the acceleration of the double S curve. The suspension swing model can be expressed as:

[0102]

[0103] Where φ represents the swing angle of the suspension device (vertical direction is 0°), m represents the total mass of the telescopic sleeve and the suspension device, L represents the current extension length of the telescopic sleeve, a(t) represents the acceleration of the current segment of the double S curve, and I represents the moment of inertia of the rotating mechanism (including load).

[0104] Step 107: Set the feedforward gain .

[0105] For example, the feedforward gain may be calibrated according to the damping characteristics of the system, or may be adjusted according to the extension of the telescopic sleeve, which is not limited in this embodiment.

[0106] Step 108: Multiply the feedforward gain by the predicted swing angle to obtain the feedforward control value based on the swing amplitude. ;

[0107] Step 109, at the PID speed output The feedforward control quantity based on the swing amplitude is superimposed on the , as the final velocity output, that is , to offset the velocity disturbance caused by the predicted swing.

[0108] This embodiment creatively solves the problems of suspension swing and mechanical instability caused by lifting acceleration through the synergistic effect of the suspension swing model and feedforward-PID composite control. Specifically, the suspension swing model is driven in real time based on the acceleration of the double S curve, which directly links the motion planning and swing prediction, so that the feedforward compensation amount accurately matches the current acceleration stage, actively offsets the theoretical swing, and dynamically updates the model parameters according to the total mass of the telescopic sleeve and the suspension equipment, the moment of inertia of the rotating mechanism, and the elongation length, so that the feedforward gain adapts to the load change and avoids excessive mechanical stress caused by overcompensation. The main swing caused by the sudden acceleration change is quickly eliminated through feedforward compensation, and the PID closed loop is superimposed to suppress residual disturbances, further improving the speed smoothness of the double S curve planning, effectively enhancing the stability of image acquisition, and providing highly reliable support for multi-angle detection in complex underwater environments.

[0109] Furthermore, in this embodiment, the following steps may be included:

[0110] When the deviation between the actual lifting speed or actual rotation speed in any stage and the planned value in the speed mapping rule exceeds the preset deviation threshold, the current stage is terminated and the system directly jumps to the "deceleration stage", and the feedforward control amount is linearly decayed to 0.

[0111] It can be understood that this embodiment proposes a segmented failure compensation strategy, adding dynamic segment switching logic to the 7-stage division of the S-shaped curve, further solving the problem of instability in inter-segment switching caused by external disturbances (such as water flow impact) or sudden load changes that may exist in the embodiment, greatly improving the trajectory smoothness in emergency situations, avoiding mechanical structure jamming, and adjusting the feedforward compensation in a linked manner during the segmented failure compensation stage to avoid conflicts between feedforward compensation and emergency braking.

[0112] Example 3:

[0113] Please see the attached Figure 2 and attached Figure 3 , Figure 2 and Figure 3This is a structural diagram of a device for realizing degrees of freedom for aquatic inspection robots provided in yet another embodiment of this specification.

[0114] like Figure 2 and Figure 3 As shown, the freedom realization device for the aquatic inspection robot may include at least a horizontal rotation mechanism (not shown in the figure), a telescopic lifting mechanism 2, a multi-device hanging platform 3 and a control module 4. The horizontal rotation mechanism is rotatably installed below the aquatic inspection robot 1, and the telescopic lifting mechanism 2 is fixedly installed at the bottom of the horizontal rotation mechanism so that the telescopic lifting mechanism 2 is driven to rotate when the horizontal rotation mechanism rotates. The multi-device hanging platform 3 is fixedly installed at the bottom of the telescopic lifting mechanism 2. The multi-device hanging platform 3 is used to install multiple detection devices required by the aquatic inspection robot 1. The horizontal rotation mechanism and the telescopic lifting mechanism 2 are both connected to the control module 4. The control module 4 is used to perform the following steps:

[0115] Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7-stage acceleration S-shaped curves, respectively, to obtain a double acceleration S-shaped curve. The stages include acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration.

[0116] The multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual-acceleration S-curve and obtain the optimal dual-acceleration S-curve parameters.

[0117] Based on the optimal double acceleration S-curve parameters, the 7-stage velocity S-curve of the lifting and rotating motion trajectories is calculated as the velocity mapping rule.

[0118] Obtain lifting distance and rotation angle in real time, and calculate actual lifting speed and actual rotation speed;

[0119] Based on the actual lifting speed and the actual rotation speed, the PID control algorithm is used to dynamically adjust the speed output according to the speed mapping rules.

[0120] It can be understood that the technical concept of the control module 4 in the degree of freedom implementation device for aquatic inspection robots provided in this embodiment is similar to the technical concept of the control method of the degree of freedom implementation device for aquatic inspection robots provided in the previous embodiment, and this embodiment will not be repeated here.

[0121] For example, in this embodiment, the implementation structure of the horizontal rotation mechanism may include a shell, a bracket, a rotation driver, a rotating table, a rotating motor and a rotary encoder. The horizontal rotation mechanism is provided with a shell on the outside, and a mounting flange hole is provided on the top of the bracket, which can be installed on the bottom of the mobile chassis of the suspended rail type aquatic inspection robot 1 by bolts; a mounting plate is provided on the top of the bracket, and the rotation driver adopts a rotary drive, which is composed of a slewing bearing, a worm and other components. The inner ring of the slewing bearing is connected to the mounting plate on the top of the bracket by screws, and the outer side of the outer ring is provided with worm gear outer teeth, which engage with the worm; bearings are provided at both ends of the worm and connected to the bracket; the rotating table is connected to the outer ring of the slewing bearing by bolts and is suspended below the outer ring of the slewing bearing; the rotating motor adopts a DC synchronous motor, which is connected to the shaft on one side of the worm, can drive the worm to rotate and then drive the outer ring of the slewing bearing and the rotating table to rotate 360°.

[0122] For example, in this embodiment, the implementation structure of the telescopic lifting mechanism 2 may include a chassis, a belt winding device, a transmission belt, a multi-section telescopic sleeve 201, a lifting motor and a rotary encoder. The telescopic lifting mechanism 2 is connected to the bottom of the rotating platform of the horizontal rotation mechanism through the mounting hole on the top of the chassis by screws and is hung under the horizontal rotation mechanism. The chassis includes a chassis shell and a frame. The belt winding device includes a belt winding shaft, two guide shafts and a limit baffle. The two ends of the belt winding shaft and the guide shaft are respectively connected to the frame through bearings and bearing seats. Limit baffles are provided on both sides of the shaft to ensure that the transmission belt is vertically consistent each time it is rolled up. The transmission belt can be A 5cm wide, less elastic canvas material is used to lift the load. One end of the transmission belt is tightened and fixed in the groove of the take-up shaft with a screw, and the other end passes through the guide shaft and the limit baffle, and enters from the multi-section telescopic cylinder, and is connected to the suspension mounting plate at the bottom of the innermost telescopic cylinder; the lifting motor adopts a DC servo motor, which is fixed to the frame through a mounting base and connected to the take-up shaft. The rotary encoder is connected to the middle guide shaft for real-time calculation of the lifting stroke of the device; the multi-section telescopic sleeve 201 is fixed to the bottom of the frame by bolts, and can be made of corrosion-resistant and lightweight materials such as fiberglass. A suspension mounting plate is provided at the bottom of the innermost telescopic cylinder.

[0123] Among them, the multi-device hanging platform 3 is used to hang water quality sensors such as dissolved oxygen, water temperature, pH, waterproof proximity switches and underwater cameras required by the aquatic inspection robot 1.

[0124] For example, the implementation of the control module 4 may include hardware and internal software such as a PCB control board, an I / O input and output interface, a servo driver, and a power plug. The control module 4 is connected to the rotating motor, the lifting motor, the rotary encoder, etc. through control lines, cables, etc., and is responsible for communicating with the host computer, accepting commands from the host computer to control the degree of freedom of the aquatic inspection robot 1 to achieve smooth and precise rotation and lifting, and feedback to the host computer information such as the motor status and the lifting stroke calculated by the encoder.

[0125] Furthermore, in this embodiment, the telescopic lifting mechanism 2 includes a plurality of telescopic sleeves 201 , and the cross section of the telescopic sleeves 201 is elliptical.

[0126] This embodiment avoids the problem of horizontal rotation of the sleeve itself when the sleeve is extended and lowered through the elliptical sleeve cross-section design, complementing the horizontal rotation mechanism and helping to ensure the stability of multi-axis motion.

[0127] Further, in this embodiment, please refer to the attached Figure 4 The multi-device hanging platform 3 includes an upper bracket 301 and a lower bracket 302. The upper bracket 301 is fixedly connected to the bottom of the telescopic lifting mechanism 2, and the upper bracket 301 is fixedly connected to the lower bracket 302. The lower bracket 302 has a polygonal structure and is provided with mounting holes on each side of the lower bracket 302, thereby breaking through the single sensor installation limitation of the existing technology.

[0128] For example, the multi-device hanging platform 3 in this embodiment adopts a double-layer bracket design, and its implementation structure may include an upper bracket 301, a lower bracket 302, a sensor clamp 303 and a camera fixing device 304. The upper bracket 301 is provided with multiple holes and slots, which are connected to the suspension mounting plate at the bottom of the telescopic sleeve 201 by bolts. The lower bracket 302 is hexagonal, and the sides of the hexagon are provided with long strip mounting holes and slots; the sensor clamp 303 is divided into two parts, each of which is provided with an arc-shaped concave surface, which can be clamped with different sensors by long bolts, and the long bolts pass through the long strip mounting holes and slots on the sides of the hexagon to be connected to the bracket; the camera fixing device 304 is arranged at both ends of the hexagon of the bracket, and is symmetrical on the left and right. The camera fixing device 304 is provided with upper and lower arc-shaped clamping plates, which are clamped with long bolts to clamp the waterproof camera. The camera fixing device 304 is provided with a horizontal plate, which is connected to the lower bracket 302 by bolts, and the angle can be changed by changing the installation position of the positioning implementation. It is understandable that the arc-shaped clamp and bolt fixing design can simplify equipment replacement and adapt to the needs of different breeding scenarios.

[0129] Furthermore, in this embodiment, a waterproof proximity switch is also included. The waterproof proximity switch is located at the bottom of the multi-device hanging platform 3. The waterproof proximity switch is used to monitor the distance between the bottom end of the detection equipment of the aquatic inspection robot 1 and the bottom of the breeding pond in real time. The waterproof proximity switch is fixedly connected to the control module 4.

[0130] Among them, the waterproof proximity switch is arranged on the lower bracket 302, combined with the safety control strategy of the control module 4 (that is, when the distance between the detection equipment of the aquatic inspection robot 1 and the bottom of the fish pond is less than the safety distance threshold, an emergency stop is triggered, and the robot is forced to jump directly into the "deceleration stage", and the integral term of the PID is cleared to avoid overshoot and mechanical collision), providing a structural basis for the safety protection mechanism.

[0131] The above description is merely a description of the preferred embodiments disclosed in this application and the technical principles employed. Those skilled in the art should understand that the scope of protection provided by this disclosure is not limited to technical solutions formed by a specific combination of the aforementioned technical features, but also encompasses other technical solutions formed by any combination of the aforementioned technical features or their equivalents without departing from the scope of the disclosure. For example, a technical solution formed by replacing the aforementioned features with (but not limited to) technical features with similar functions disclosed in this disclosure.

[0132] In addition, although each operation is described in a specific order, this should not be understood as requiring these operations to be performed in the specific order shown or in a sequential order. Under certain circumstances, multitasking and parallel processing may be advantageous. Similarly, although some specific implementation details have been included in the above discussion, these should not be interpreted as limiting the scope of the present disclosure. Some features described in the context of a separate embodiment can also be implemented in a single embodiment in combination. On the contrary, the various features described in the context of a single embodiment can also be implemented in multiple embodiments individually or in any suitable sub-combination mode.

Claims

1. A control method for a device for realizing degrees of freedom of an aquatic inspection robot, characterized in that: The following steps are involved: Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7 stages of acceleration S-shaped curves, respectively, to obtain a double acceleration S-shaped curve. The stages include acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration. The multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual-acceleration S-curve and obtain the optimal dual-acceleration S-curve parameters. Based on the optimal double acceleration S-curve parameters, the 7-stage velocity S-curve of the lifting and rotating motion trajectories is calculated as the velocity mapping rule. Obtain lifting distance and rotation angle in real time, and calculate actual lifting speed and actual rotation speed; Based on the actual lifting speed and actual rotation speed, the PID control algorithm is used to dynamically adjust the speed output according to the speed mapping rules; Among them, a multi-axis collaborative planning strategy is used to jointly optimize the parameters of the dual acceleration S-shaped curve to obtain the optimal dual acceleration S-shaped curve parameters, including: Establish the biaxial coupling relationship of vertical force balance and rotational torque balance; Defining multi-axis motion constraints, wherein the constraints include mechanical strength constraints and motion interference constraints; A parameter optimization function is constructed with the goal of minimizing the total motion time and inertial impact force. The Lagrange multiplier method is used to solve the parameter optimization function under the constraints to obtain the optimal double S-shaped curve parameters.

2. The control method for the device for realizing the degree of freedom of the aquatic inspection robot according to claim 1, characterized in that: The biaxial coupling relationship of vertical force balance and rotational torque balance includes the balance between the vertical inertia force and the vertical component of the rotational coupling and the lifting driving force, and the balance between the rotational inertia torque and the lifting force coupling torque and the rotational torque.

3. The control method of the device for realizing the degree of freedom of the aquatic inspection robot according to claim 1, characterized in that: The expression of the parameter optimization function is: ， Where T represents the total exercise time of the 7 stages, represents the lifting acceleration, Indicates rotational acceleration.

4. The control method for the device for realizing the degree of freedom of the aquatic inspection robot according to claim 1, characterized in that: After the aforementioned “dynamically adjusting the speed output using a PID control algorithm based on the actual lifting speed and the actual rotation speed according to the speed mapping rules”, it also includes: Establishing a suspension swing model, wherein the suspension swing model is used to predict the swing angle of the suspension device during movement based on the acceleration of the double S curve; Set the feedforward gain; Multiply the feedforward gain by the predicted swing angle to obtain the feedforward control amount based on the swing amplitude; The feedforward control quantity based on the swing amplitude is superimposed on the PID speed output as the final speed output to offset the speed disturbance caused by the predicted swing.

5. The control method for the device for realizing the degree of freedom of the aquatic inspection robot according to claim 4, characterized in that: The following steps are also included: When the deviation between the actual lifting speed or actual rotation speed in any stage and the planned value in the speed mapping rule exceeds the preset deviation threshold, the current stage is terminated and the system directly jumps to the "deceleration stage", and the feedforward control amount is linearly decayed to 0.

6. A device for realizing the degree of freedom of aquatic inspection robots, characterized in that: It includes a horizontal rotating mechanism, a telescopic lifting mechanism, a multi-device hanging platform and a control module. The horizontal rotating mechanism is rotatably installed under the aquatic inspection robot. The telescopic lifting mechanism is fixedly installed at the bottom of the horizontal rotating mechanism so that the telescopic lifting mechanism is driven to rotate when the horizontal rotating mechanism rotates. The multi-device hanging platform is fixedly installed at the bottom of the telescopic lifting mechanism. The multi-device hanging platform is used to install multiple detection equipment required by the aquatic inspection robot. The horizontal rotating mechanism and the telescopic lifting mechanism are both connected to the control module. The control module is used to perform the following steps: Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are divided into 7 stages of acceleration S-shaped curves, respectively, to obtain a double acceleration S-shaped curve. The stages include acceleration, uniform acceleration, deceleration, uniform speed, acceleration and deceleration, uniform deceleration, and deceleration. Establish the biaxial coupling relationship of vertical force balance and rotational torque balance; Defining multi-axis motion constraints, wherein the constraints include mechanical strength constraints and motion interference constraints; A parameter optimization function is constructed with the goal of minimizing the total motion time and inertial impact force. The Lagrange multiplier method is used to solve the parameter optimization function under the constraints to obtain the optimal double S-shaped curve parameters. Based on the optimal double acceleration S-curve parameters, the 7-stage velocity S-curve of the lifting and rotating motion trajectories is calculated as the velocity mapping rule. Obtain lifting distance and rotation angle in real time, and calculate actual lifting speed and actual rotation speed; Based on the actual lifting speed and the actual rotation speed, the PID control algorithm is used to dynamically adjust the speed output according to the speed mapping rules.

7. The device for realizing the degree of freedom of aquatic inspection robots according to claim 6, characterized in that: The telescopic lifting mechanism comprises a plurality of telescopic sleeves, and the cross section of the telescopic sleeves is elliptical.

8. The device for realizing the degree of freedom of aquatic inspection robots according to claim 6, characterized in that: The multi-device hanging platform includes an upper bracket and a lower bracket. The upper bracket is fixedly connected to the bottom of the telescopic lifting mechanism. The upper bracket is fixedly connected to the lower bracket. The lower bracket has a polygonal structure and is provided with mounting holes on each side of the lower bracket.

9. The device for realizing the degree of freedom of aquatic inspection robots according to claim 6, characterized in that: It also includes a waterproof proximity switch, which is located at the bottom of the multi-device hanging platform. The waterproof proximity switch is used to monitor the distance between the platform and the bottom of the breeding pond in real time. The waterproof proximity switch is fixedly connected to the control module.

Images