Degree-of-freedom implementation device for aquatic inspection robot and control method thereof

Abstract

The invention relates to the technical field of aquatic inspection, in particular to a degree-of-freedom implementation device for an aquatic inspection robot and a control method of the degree-of-freedom implementation device. The method comprises the following steps: based on a target travel distance and a target rotation angle, respectively dividing a lifting motion track and a rotation motion track into seven stages of acceleration S-shaped curves; carrying out joint optimization on parameters of the double-acceleration S-shaped curve by adopting a multi-axis collaborative planning strategy to obtain an optimal double-acceleration S-shaped curve parameter, and then calculating to obtain a seven-stage speed S-shaped curve of a lifting motion track and a rotating motion track as a speed mapping rule; the lifting distance and the rotation angle are obtained in real time, and the actual lifting speed and the actual rotation speed are calculated; on the basis of the actual lifting speed and the actual rotating speed, speed output is dynamically adjusted according to a speed mapping rule, multi-angle detection of the suspension rail type aquatic inspection robot is achieved, meanwhile, the stability of a suspension structure and the smoothness of composite motion can be considered, and the detection precision is effectively improved.

Description

Technical Field

[0001] The present invention relates to the technical field of aquaculture inspection, and particularly relates to a degree-of-freedom implementation device for an aquaculture inspection robot and a control method thereof. Background Art

[0002] Industrialized recirculating aquaculture adopts modern engineering and information technologies to create an ecological environment suitable for the growth of fish through technical means, realizing high-density, high-efficiency, and low-environmental-impact aquaculture. In recent years, the scale of industrialized recirculating aquaculture has developed rapidly, and mechanization, digitization, and intelligence are the inevitable directions for the development of modern green and efficient aquaculture. In the production of industrialized recirculating aquaculture, real-time collection and identification analysis of water quality indicators, fish growth and behavior, and production site conditions in the aquaculture water body can provide data support for intelligent decision-making in production management such as precise water treatment regulation, precise feeding, and risk warning in aquaculture.

[0003] Currently, as an aquaculture inspection robot for collection and identification analysis devices in industrialized recirculating aquaculture production, it has developed to a suspended-rail aquaculture inspection robot, such as Chinese Patent CN216577835U - an aquaculture orbital inspection robot, Chinese Patent CN219027565U - a water quality inspection assembly suitable for industrialized aquaculture, etc., enabling the sensors in the aquaculture inspection robot to stretch up and down, only penetrating into the water when water quality needs to be detected and leaving the water surface when the detection is completed, realizing intermittent detection of multiple aquaculture ponds. Therefore, the accuracy is more stable and the service life is also longer.

[0004] However, the telescopic boom in the prior art can only move vertically up and down, resulting in a fixed detection angle of view and making it difficult to cover multi-angle underwater environments. Even when combined with a camera rotating pan-tilt, the lifting motor and the rotating motor only simply perform independent control according to preset parameters, which has great limitations. Specifically: 1. Static planning parameters are difficult to adapt to dynamic load changes: The parameters of a single motor are preset based on a fixed scenario and cannot be adjusted in real time according to the combined movement of lifting and rotation. When there is multi-axis movement, the start-stop timing sequence and speed change rate of lifting and rotation will cause movement conflicts. For example, when the lifting suddenly stops, the rotation inertia will cause swinging, etc.; 2. It is difficult to balance the stability of the suspension structure and the smoothness of the combined movement: Since the center of gravity of the suspended-rail aquaculture 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 extremely easy to cause mutual interference of torque fluctuations of the two axes in the combined movement. For example, the adverse effect of the lateral centrifugal force during rotation acceleration on the lifting displacement accuracy, and the lateral torque of the lifting mechanism caused by the rotational angular acceleration resulting in unexpected deformation of the telescopic rod, etc. Moreover, when there is multi-axis combined movement, the superposition of the inertia of the two axes will cause resonance of the suspended equipment, thereby exacerbating the swinging. Summary of the Invention

[0005] In view of the above technical problems, the present invention provides a degree-of-freedom implementation device for an aquaculture inspection robot and its control method, aiming to achieve multi-angle detection of a suspended-rail aquaculture inspection robot while taking into account the stability of the suspension structure and the smoothness of compound motion, and effectively improving the detection accuracy.

[0006] In a first aspect, the present application provides a control method for a degree-of-freedom implementation device of an aquaculture inspection robot, including the following steps: Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotation motion trajectory are respectively divided into 7-stage acceleration S-shaped curves to obtain double-acceleration S-shaped curves, and the stages include jerk acceleration, uniform acceleration, deceleration acceleration, uniform speed, acceleration deceleration, uniform deceleration, and jerk deceleration; Adopt a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters; Based on the optimal double-acceleration S-shaped curve parameters, calculate the 7-stage velocity S-shaped curves of the lifting motion trajectory and the rotation motion trajectory as the velocity mapping rules; Obtain the lifting distance and the rotation angle in real time, and calculate the actual lifting speed and the actual rotation speed; Based on the actual lifting speed and the actual rotation speed, use the PID control algorithm to dynamically adjust the speed output according to the speed mapping rules.

[0007] In some embodiments, adopting a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters includes: Establish a two-axis coupling relationship for vertical force balance and rotational torque balance; Define multi-axis motion constraint conditions, and the constraint conditions include mechanical strength constraints and motion interference constraints; Construct a parameter optimization function with the goal of minimizing the total motion time and inertial impact force, and use the Lagrange multiplier method to solve the parameter optimization function under the constraint conditions to obtain the optimal double S-shaped curve parameters.

[0008] In some embodiments, the two-axis coupling relationship for vertical force balance and rotational torque balance includes the balance between the vertical inertial force and the rotational coupling vertical component and the lifting driving force, and the balance between the rotational inertia torque and the lifting force coupling torque and the rotational torque.

[0009] In some embodiments, the expression of the parameter optimization function is: where T represents the total motion time of the 7 stages, represents the lifting acceleration, represents the rotational acceleration.

[0010] In some embodiments, after "dynamically adjusting the speed output according to the speed mapping rule by using the PID control algorithm based on the actual lifting speed and the actual rotation speed", the following steps are further included: Establish a suspension swing model, which 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 a feedforward control amount based on the swing amplitude. Superimpose the feedforward control amount based on the swing amplitude on the PID speed output as the final speed output to cancel the speed disturbance caused by the predicted swing.

[0011] In some embodiments, the following steps are further included: When the deviation between the actual lifting speed or the actual rotation speed in any stage and the planned value in the speed mapping rule exceeds the preset deviation threshold, terminate the current stage, directly jump to the "deceleration stage", and linearly decay the feedforward control amount to 0.

[0012] In a second aspect, the present application provides a degree-of-freedom implementation device for an aquaculture inspection robot, including a horizontal rotation mechanism, a telescopic lifting mechanism, a multi-device hanging platform, and a control module. The horizontal rotation mechanism is rotatably installed below the aquaculture inspection robot, the telescopic lifting mechanism is fixedly installed at the bottom of the horizontal rotation mechanism so that the telescopic lifting mechanism rotates when the horizontal rotation 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 detection devices required by a plurality of aquaculture inspection robots, and both the horizontal rotation mechanism and the telescopic lifting mechanism are 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, divide the lifting motion trajectory and the rotation motion trajectory into 7-stage acceleration S-curves respectively to obtain a double-acceleration S-curve, and the stages include positive acceleration, constant acceleration, deceleration, constant speed, acceleration and deceleration, uniform deceleration, and deceleration. Adopt a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-curve to obtain the optimal double-acceleration S-curve parameters. Based on the optimal double-acceleration S-curve parameters, calculate the 7-stage speed S-curves of the lifting motion trajectory and the rotation motion trajectory as the speed mapping rule. Obtain the lifting distance and the rotation angle in real time, and calculate the actual lifting speed and the actual rotation speed. Based on the actual lifting speed and the actual rotation speed, dynamically adjust the speed output according to the speed mapping rule by using the PID control algorithm.

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

[0014] 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. The upper bracket is fixedly connected to the lower bracket. The lower bracket is of a polygonal structure and mounting holes are provided on each side of the lower bracket.

[0015] 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 from the bottom of the aquaculture pond in real time, and the waterproof proximity switch is fixedly connected to the control module.

[0016] The beneficial technical effects of the present invention at least include: 1. By using the degree-of-freedom realization device and its control method for the aquaculture inspection robot, through the coordinated integration of double S-shaped curve segmented planning, multi-axis parameter joint optimization based on the multi-axis collaborative planning strategy, and real-time closed-loop feedback control, the technical bottlenecks such as multi-axis motion conflicts and dynamic load changes leading to control instability and low detection accuracy that inevitably exist in the existing suspended-rail aquaculture inspection robot during multi-axis motion are creatively broken through. Specifically, in this application, based on the target travel distance and safety threshold, the lifting and rotation trajectories are respectively divided into 7-stage acceleration curves (jerk acceleration → jerk deceleration). By the speed mapping rule, the start-stop timing and speed change rate of the two axes are synchronously planned, so that the uniform speed section of lifting and the acceleration section of rotation are staggered, suppressing the resonance caused by the torque superposition of the two axes, avoiding the swing caused by the rotational inertia during the sudden stop of lifting, and at the same time dynamically calibrating the trajectory in combination with the data feedback of the rotary encoder in real time to ensure that the two axes strictly follow the double-speed S-shaped curve, improving the smoothness of the composite motion, eliminating the motion deviation caused by the load change of the multi-device hanging platform. Through the deep coordination of technical means, the limitation of traditional multi-axis isolated control is broken through, and the "trajectory smoothing - dynamic disturbance resistance" integrated control is realized. While realizing the multi-angle detection of the suspended-rail aquaculture inspection robot, the stability of the suspension structure and the smoothness of the composite motion are taken into account, effectively improving the detection efficiency and detection accuracy of the suspended-rail aquaculture inspection robot under complex working conditions, and providing a highly reliable and full-view underwater environment monitoring ability for factory recirculating aquaculture; 2. The multi-axis collaborative planning strategy designed in this application deeply integrates theoretical mechanics and real-time control algorithms. It quantifies the biaxial coupling effect through the dynamic equations of multi-degree-of-freedom motion, dynamically binds the acceleration / jerk parameters of the 7-stage S-curve to the real-time load (the mass of the suspension device and the elongation of the telescopic sleeve), dynamically distributes 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 the control accuracy, combines constraint optimization to avoid over-limit, jointly optimizes the biaxial parameters to balance time and impact force, maximally shortens the detection cycle, improves the detection efficiency, and can dynamically update and adjust the optimal speed S-curve parameters according to the total mass of the telescopic sleeve and the suspension device, the deflection angle of the horizontal rotating mechanism relative to the vertical direction, and the current elongation length of the telescopic sleeve. It overcomes the technical problem of control instability caused by multi-axis motion interference that inevitably exists in the suspended-rail aquaculture inspection robot during multi-axis motion (such as the additional torque generated by the rotating motor due to inertial coupling during lifting acceleration), reduces the mechanical vibration energy under compound motion, realizes the "global optimal - real-time response - safe and stable" trinity motion control of the suspended-rail aquaculture inspection robot, and effectively improves the detection accuracy, reliability, and efficiency of multi-axis synchronous motion; 3. Through the synergistic effect of the suspension swing model and the feedforward-PID compound control, it creatively solves the problems of suspension swing and mechanical instability caused by lifting acceleration. Specifically, based on the acceleration of the double S-curve to drive the suspension swing model in real time, it directly correlates motion planning and swing prediction, enables the feedforward compensation amount to accurately match the current acceleration stage, actively cancels the theoretical swing, and at the same time dynamically updates the model parameters according to the total mass of the telescopic sleeve and the suspension device, the moment of inertia of the rotating mechanism, and the elongation length, making the feedforward gain adapt to the load change and avoiding mechanical stress over-limit caused by over-compensation. It quickly eliminates the main swing caused by the acceleration mutation through feedforward compensation, superimposes the PID closed-loop to suppress the residual disturbance, further improves the speed smoothness of the double S-curve planning, effectively improves the image acquisition stability, and provides high-reliability support for multi-angle detection in complex underwater environments.

[0017] 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

[0018] The following further describes the present invention with reference to the drawings: Figure 1 It is a flowchart of the control method for the degree-of-freedom implementation device of the aquaculture inspection robot in the embodiment of the present invention.

[0019] Figure 2 It is a schematic structural diagram of the degree-of-freedom implementation device of the aquaculture inspection robot in the embodiment of the present invention.

[0020] Figure 3The bottom view of the degree - of - freedom implementation device for the aquaculture inspection robot in the embodiment of the present invention.

[0021] Figure 4 The structural schematic diagram of the multi - device hanging platform in the embodiment of the present invention. Specific embodiments

[0022] The technical solutions of the embodiments of the present invention will be explained and described below with reference to the accompanying drawings of the embodiments of the present invention. However, the following embodiments are only the preferred embodiments of the present invention, not all of them. Based on the embodiments in the implementation manners, other embodiments obtained by those skilled in the art without creative efforts all fall within the protection scope of the present invention.

[0023] In the following description, terms such as "inner", "outer", "upper", "lower", "left", "right", etc. indicating orientation or position relationship are only for the convenience of describing the embodiments and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and thus cannot be construed as a limitation of the present invention.

[0024] Embodiment 1: Please refer to the attached Figure 1 , Figure 1 which shows the flow schematic diagram of the control method for the degree - of - freedom implementation device of the aquaculture inspection robot provided in an embodiment of this specification.

[0025] As Figure 1 shown, the control method for the degree - of - freedom implementation device of the aquaculture inspection robot can at least include the following steps: Step 101, based on the target travel distance and the target rotation angle, divide the lifting motion trajectory and the rotation motion trajectory into 7 - stage acceleration S - shaped curves respectively to obtain a double - acceleration S - shaped curve. The stages include jerk - up, constant - acceleration, deceleration - jerk, constant - velocity, acceleration - deceleration, constant - deceleration, and jerk - down.

[0026] Specifically, in this embodiment, the implementation method of dividing the lifting motion trajectory and the rotation motion trajectory into 7 - stage acceleration S - shaped curves based on the target travel distance and the target rotation angle is as follows: First, obtain the target travel distance, target rotation angle, and safety distance threshold (the minimum allowable distance from the bottom of the fish pond) set by the upper computer or the built - in task, read the initial position of the rotary encoder, including the initial lifting height and the initial rotation angle, and collect the distance from the bottom of the fish pond measured by the waterproof proximity switch in real - time; For the lifting motion (vertical axis) and rotational motion (horizontal axis), plan 7-stage acceleration curves respectively, namely jerk acceleration (t1) -> uniform acceleration (t2) -> deceleration acceleration (t3) -> uniform velocity (t4) -> deceleration deceleration (t5) -> uniform deceleration (t6) -> jerk deceleration (t7). Set the maximum lifting acceleration and the maximum jerk J (jerk represents the change in acceleration), then the acceleration S-shaped curve of the lifting motion trajectory can be expressed as: Set the maximum rotational angular acceleration and the maximum jerk, and the acceleration S-shaped curve of the rotational motion trajectory has an expression similar to that of the lifting motion trajectory, but uses angular displacement to replace the linear displacement.

[0027] Step 102, adopt a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters.

[0028] Specifically, in this embodiment, a multi-axis collaborative planning strategy is adopted to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters, including: Step 201, establish a double-axis coupling relationship for vertical force balance and rotational torque balance; Furthermore, in this embodiment, the double-axis coupling relationship for vertical force balance and rotational torque balance includes the balance between the vertical inertial force and the rotational coupling vertical component and the lifting driving force, and the balance between the inertia torque of the moment of inertia and the lifting force coupling torque and the rotational torque. Among them, the rotational coupling vertical component is the component of the centrifugal force in the vertical direction caused by the rotational angular acceleration, and the lifting force coupling torque is the torque generated by the vertical driving force output by the lifting motor on the rotating shaft.

[0029] The double-axis coupling relationship for vertical force balance and rotational torque balance in this embodiment can be expressed as: Among them, represents the vertical driving force output by the lifting motor, m represents the total mass of the telescopic sleeve and the suspension device, represents the vertical inertial force, I represents the moment of inertia of the rotating mechanism (including the load), θ represents the deflection angle of the horizontal rotating mechanism relative to the vertical direction, represents the torque output by the rotating motor, represents the inertia torque of the moment of inertia, and L represents the current extended length of the telescopic sleeve.

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

[0031] 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 elongation length of the telescopic sleeve. It can also be set to re-employ the multi-axis collaborative planning strategy every 5 seconds to jointly optimize and calculate the parameters of the double-acceleration S-shaped curve to obtain the optimal parameters.

[0032] Step 202: Define multi-axis motion constraint conditions, where the constraint conditions include mechanical strength constraints and motion interference constraints.

[0033] It can be understood that the goal of the multi-axis motion constraint conditions is to prevent the composite motion from exceeding the mechanical structure strength limit and solve the mechanical fatigue problem caused by the suspension swing in the suspended-rail aquaculture inspection robot. Therefore, the mechanical strength constraint can be expressed as: where, is the maximum allowable thrust of the motor, is the maximum allowable torque of the worm and worm gear; The motion interference constraint can be expressed as: where, represents the safety threshold of the acceleration product measured by experiments to avoid resonance.

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

[0035] Specifically, in this embodiment, the optimization parameters include: S-shaped curve parameters of the lifting motor: (7-stage time allocation), ; S-shaped curve parameters of the rotating motor: (7-stage time allocation), .

[0036] Under the constraint conditions, use the Lagrange multiplier method to solve the parameter optimization function, so as to achieve the shortest motion time and the minimum impact. The expression of the parameter optimization function in this embodiment is: where, T represents the total motion time of the 7 stages, represents the lifting acceleration, represents the rotational acceleration, and represents a preset weight coefficient. It can be understood that in this embodiment, the impact force accumulation amount is characterized by the square integral of acceleration, that is, the product of acceleration is punished by the integral term, so as to suppress the vibration energy.

[0037] Introduce the Lagrange multiplier to handle the constraints, which can be expressed as: Then, the optimal result of the double-acceleration S-curve parameters can be obtained by solving the functional extremum through numerical methods (such as sequential quadratic programming SQP, etc.). This embodiment will not elaborate here.

[0038] On the other hand, in this embodiment, after obtaining the optimal acceleration S-curve parameters of the lifting motion trajectory and the optimal acceleration S-curve of the rotational motion trajectory it is also possible to design a multi-axis synchronous constraint strategy to determine the time allocation of the optimal acceleration S-curve of the rotational motion trajectory. Specifically, multiply the optimal time allocation of the optimal acceleration S-curve of the lifting motion trajectory by a preset time synchronization coefficient (0.8 - 1.2) as the optimal time allocation of the optimal acceleration S-curve of the rotational motion trajectory, so as to prevent mechanical interference caused by too large a phase difference between the lifting and rotational movements.

[0039] The multi-axis collaborative planning strategy designed in this embodiment deeply integrates theoretical mechanics and real-time control algorithms, quantifies the two-axis coupling effect through the dynamic equation of multi-degree-of-freedom motion, dynamically binds the acceleration / jerk parameters of the 7-stage S-curve to the real-time load (the mass of the suspension device, the elongation of the telescopic sleeve), dynamically distributes the vertical driving force output by the lifting motor and the torque output by the rotational motor, precisely compensates for the interference of rotation on lifting, effectively suppresses the swing amplitude, improves the control accuracy, combines constraint optimization to avoid over-limitation, jointly optimizes the two-axis parameters to balance time and impact force, maximally shortens the detection cycle, improves the detection efficiency, and can dynamically update and adjust the optimal speed S-curve parameters according to the total mass of the telescopic sleeve and the suspension device, the deflection angle of the horizontal rotation mechanism relative to the vertical direction, and the current elongation length of the telescopic sleeve, overcoming the technical problem of control instability caused by multi-axis motion interference inevitably existing in the suspended-rail aquaculture inspection robot during multi-axis motion (such as the additional torque generated by the rotational motor due to inertial coupling during lifting acceleration), reducing the mechanical vibration energy under compound motion, realizing the "global optimal - real-time response - safe and stable" trinity motion control of the suspended-rail aquaculture inspection robot, and effectively improving the detection accuracy, reliability, and efficiency of multi-axis synchronous motion.

[0040] Step 103: Based on the optimal double-acceleration S-shaped curve parameters, calculate the 7-stage velocity S-shaped curves of the lifting motion trajectory and the rotational motion trajectory as the velocity mapping rules.

[0041] Embed the optimization result into the double-acceleration S-shaped curve of the original scheme. According to the velocity and acceleration relationship 𝑉 = 𝑎𝑡, and the jerk and velocity relationship 𝑉 = 𝐽𝑡 2 / 2, the 7-stage velocity S-shaped curves of the lifting motion trajectory and the rotational motion trajectory can be calculated and expressed as: where , (i = 1, 2, 3, …, 7).

[0042] Step 104: Real-time obtain the lifting distance and the rotational angle, and calculate the actual lifting velocity and the actual rotational velocity.

[0043] It can be understood that the rotation encoder of the lifting motor real-time feedbacks the lifting distance h, and the rotation encoder of the rotational motor real-time feedbacks the rotational angle θ. Then the actual lifting velocity v = Δh / Δt, and the actual rotational velocity w = Δθ / Δt.

[0044] Step 105: Based on the actual lifting velocity and the actual rotational velocity, adopt the PID control algorithm to dynamically adjust the velocity output according to the velocity mapping rules.

[0045] It can be understood that the motor receives the planned velocity value output by the velocity mapping rules, realizes stepless speed regulation through PWM duty cycle adjustment, and at the same time, the rotation encoder feedbacks the actual velocity for calibration, so that the lifting motor executes the optimal velocity S-shaped curve parameters of the lifting motion trajectory, and the rotational motor executes the optimal velocity S-shaped curve parameters of the rotational motion trajectory.

[0046] Furthermore, this embodiment may further include the following steps: Use a waterproof proximity switch to real-time detect the distance from the bottom of the aquatic product inspection robot to the bottom of the aquaculture pond; When the distance from the aquatic product inspection robot to the bottom of the aquaculture pond is less than the safety distance threshold, trigger an emergency stop, directly force a jump into the "deceleration stage", and at the same time clear the integral term of the PID to avoid overshoot and mechanical collision.

[0047] In summary, in this embodiment, through the collaborative integration of dual S-shaped curve segmented planning, multi-axis parameter joint optimization based on the multi-axis collaborative planning strategy, and real-time closed-loop feedback control, the technical bottlenecks such as multi-axis motion conflicts and control instability and low detection accuracy caused by dynamic load changes that inevitably exist in existing suspended aquaculture inspection robots during multi-axis motion are creatively broken through. Specifically, in this embodiment, based on the target travel distance and safety threshold, the lifting and rotation trajectories are respectively divided into 7-stage acceleration curves (jerk acceleration → jerk deceleration). By the speed mapping rule, the start-stop timing and speed change rate of the two axes are synchronously planned, so that the uniform speed section of lifting and the acceleration section of rotation are executed out of phase, suppressing the resonance caused by the torque superposition of the two axes and avoiding the swing caused by the rotational inertia during the sudden stop of lifting. At the same time, the trajectory is dynamically calibrated in combination with the data feedback by the rotary encoder in real time to ensure that the two axes strictly follow the dual-speed S-shaped curve, improving the smoothness of the composite motion, eliminating the motion deviation caused by the load change of the multi-device hanging platform. Through the deep collaboration of technical means, the limitation of traditional multi-axis isolated control is broken through, and the integrated control of "smooth trajectory - dynamic disturbance resistance" is realized. While realizing multi-angle detection of the suspended aquaculture inspection robot, the stability of the suspension structure and the smoothness of the composite motion are taken into account, effectively improving the detection efficiency and detection accuracy of the suspended aquaculture inspection robot under complex working conditions, and providing high-reliability and full-view underwater environment monitoring capabilities for industrialized recirculating aquaculture.

[0048] Embodiment 2: This embodiment only describes the parts that are different from Figure 1 the corresponding Embodiment 1. The technical concepts of the rest of the method design are similar to those of Embodiment 1, and will not be elaborated here. In this embodiment, after "dynamically adjusting the speed output according to the speed mapping rule using the PID control algorithm based on the actual lifting speed and the actual rotation speed", it further includes: Step 106, establishing a suspension swing model, which is used to predict the swing angle of the suspension device during movement based on the acceleration of the dual S-curve. The suspension swing model can be expressed as: where φ represents the swing angle of the hanging device (0° in the vertical direction), m represents the total mass of the telescopic sleeve and the suspension device, L represents the current extended length of the telescopic sleeve, a(t) represents the acceleration of the current section of the dual S-curve, and I represents the moment of inertia of the rotating mechanism (including the load).

[0049] Step 107, setting the feedforward gain .

[0050] Exemplarily, the feedforward gain can be calibrated according to the system damping characteristics or adjusted according to the extended length of the telescopic sleeve. This embodiment does not make any limitation on this.

[0051] Step 108: multiply the feedforward gain by the predicted swing angle to obtain a feedforward control value based on the swing amplitude. ; Step 109, at the PID speed output The feedforward control quantity based on the swing amplitude is superimposed on the , as the final speed output, that is , to offset the velocity disturbance caused by the predicted swing.

[0052] 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, and the motion planning and swing prediction are directly linked, so that the feedforward compensation amount accurately matches the current acceleration stage, and actively offsets the theoretical swing. At the same time, the model parameters are dynamically updated 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 can adapt to the load change and avoid 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, so as to further improve the speed smoothness of the double S curve planning, effectively improve the stability of image acquisition, and provide high-reliability support for multi-angle detection in complex underwater environments.

[0053] Furthermore, in this embodiment, the following steps may also be 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.

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

[0055] Embodiment three: Please refer to the attached Figure 2 and attached Figure 3 , Figure 2 and Figure 3 This is a schematic structural diagram of a device for realizing degrees of freedom for aquatic inspection robots provided in yet another embodiment of the present specification.

[0056] like Figure 2 and Figure 3As shown, the degree-of-freedom implementation device for the aquatic inspection robot may at least include 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. The telescopic lifting mechanism 2 is fixedly installed at the bottom of the horizontal rotation mechanism so that the telescopic lifting mechanism 2 rotates 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 for the aquatic inspection robot 1. Both the horizontal rotation mechanism and the telescopic lifting mechanism 2 are connected to the control module 4. The control module 4 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 respectively divided into 7-stage acceleration S-shaped curves to obtain a double-acceleration S-shaped curve. The stages include jerk acceleration, uniform acceleration, deceleration acceleration, constant speed, acceleration deceleration, uniform deceleration, and deceleration deceleration; Adopt a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters; Based on the optimal double-acceleration S-shaped curve parameters, calculate the 7-stage speed S-shaped curves of the lifting motion trajectory and the rotation motion trajectory as the speed mapping rules; Real-time obtain the lifting distance and the rotation angle, and calculate the actual lifting speed and the actual rotation speed; Based on the actual lifting speed and the actual rotation speed, adopt the PID control algorithm to dynamically adjust the speed output according to the speed mapping rules.

[0057] It can be understood that the technical concept of the control module 4 in the degree-of-freedom implementation device for the aquatic inspection robot provided in this embodiment is similar to the technical concept of the control method of the degree-of-freedom implementation device for the aquatic inspection robot provided in the foregoing embodiment, and will not be elaborated herein.

[0058] Exemplarily, in this embodiment, the implementation structure of the horizontal rotation mechanism may include a housing, a bracket, a rotary drive, a rotating table, a rotary motor, and a rotary encoder. The horizontal rotation mechanism is externally provided with a housing. The top of the bracket is provided with mounting flange holes and can be installed below the moving chassis of the suspended-rail aquatic inspection robot 1 by bolts; the top of the bracket is provided with a mounting plate. The rotary drive adopts a slewing drive and is composed of components such as a slewing bearing and a worm. The inner ring of the slewing bearing is connected and fixed to the mounting plate at the top of the bracket by screws. The outer ring is provided with external worm gear teeth on the outside, and the external worm gear teeth are meshed with the worm; both ends of the worm are provided with bearings 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 rotary motor adopts a DC synchronous motor and is connected to one side shaft of the worm, and can drive the worm to rotate and then drive the outer ring of the slewing bearing and the rotating table to rotate 360°.

[0059] Exemplarily, in this embodiment, the implementation structure of the telescopic lifting mechanism 2 may include a chassis, a tape 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 table of the horizontal rotating mechanism through the mounting hole at the top of the chassis by screws and is hung below the horizontal rotating mechanism. The chassis includes two parts: a chassis shell and a frame. The tape winding device includes one tape winding shaft, two guide shafts, and a limit baffle. The two ends of the tape winding shaft and the guide shafts 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 perpendicular and consistent each time it is wound. The transmission belt can be made of canvas material with a width of 5 cm and less elasticity for lifting the load. One end of the transmission belt is fixed in the groove of the tape winding shaft with screws, and the other end passes through the guide shaft, the limit baffle, and penetrates into the multi-section telescopic cylinder, and is connected to the suspension mounting plate at the bottom of the innermost section of the telescopic cylinder. The lifting motor uses a DC servo motor. The lifting motor is fixed to the frame through a mounting base and is connected to the tape winding shaft. The rotary encoder is connected to the middle guide shaft for calculating the lifting stroke of the device in real time. The multi-section telescopic sleeve 201 is fixed below 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 section of the telescopic cylinder.

[0060] 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 aquaculture inspection robot 1.

[0061] Exemplarily, the implementation manner of the control module 4 may include hardware such as a PCB control board, an I / O input / output interface, a servo driver, a power supply plug, and internal software. The control module 4 is connected to the rotary motor, the lifting motor, the rotary encoder, etc. through control lines, cable lines, etc., and is also responsible for communicating with the host computer, receiving commands from the host computer to control the degrees of freedom of the aquaculture inspection robot 1 to achieve smooth and precise rotation and lifting of the device, and feeding back information such as the motor state and the lifting stroke calculated by the encoder to the host computer.

[0062] Furthermore, in this embodiment, the telescopic lifting mechanism 2 includes several telescopic sleeves 201, and the cross-section of the telescopic sleeve 201 is elliptical.

[0063] In this embodiment, the design of the elliptical sleeve cross-section avoids the problem of the horizontal rotation of the sleeve itself during the telescopic lifting of the sleeve, forming a complement with the horizontal rotating mechanism, which helps to ensure the stability of multi-axis movement.

[0064] Furthermore, in this embodiment, please refer to the appendix 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. The upper bracket 301 is fixedly connected to the lower bracket 302. The lower bracket 302 has a polygonal structure and mounting hole grooves are provided on each side of the lower bracket 302, thus breaking through the single sensor installation limitation of the prior art.

[0065] Exemplarily, the multi-device hanging platform 3 in this embodiment adopts a double-layer bracket design. 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 hole grooves and is connected to the suspension mounting plate at the bottom of the telescopic sleeve 201 through bolts. The lower bracket 302 is hexagonal, and long strip-shaped mounting hole grooves are provided on the sides of the hexagon; the sensor clamp 303 is divided into two parts, each having an arc-shaped concave surface, and can clamp different sensors through long bolts. The long bolts pass through the long strip-shaped mounting hole grooves on the sides of the hexagon and are connected to the bracket; the camera fixing device 304 is arranged at both ends of the hexagon of the bracket and is symmetric left and right. The camera fixing device 304 is provided with upper and lower arc-shaped clamping plates to clamp and hold a waterproof camera through long bolts. The camera fixing device 304 is provided with a horizontal plate, which is connected to the lower bracket 302 through bolts, and the angle can be changed by changing the installation position of the positioning implement. It can be understood that the arc-shaped clamp and bolt fixing design can simplify equipment replacement and meet the requirements of different aquaculture scenarios.

[0066] Furthermore, in this embodiment, a waterproof proximity switch is further 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 device of the aquaculture inspection robot 1 and the bottom of the aquaculture pond in real time. The waterproof proximity switch is fixedly connected to the control module 4.

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

[0068] As mentioned above, it is only the preferred embodiment disclosed in this application and the description of the applied technical principles. Those skilled in the art should understand that the protection scope involved in this disclosure is not limited to the technical solution formed by the specific combination of the above technical features, and should also cover other technical solutions formed by any combination of the above technical features or their equivalent features without departing from the above disclosure concept. For example, the technical solutions formed by mutually replacing the above features with the (but not limited to) technical features with similar functions disclosed in this disclosure.

[0069] In addition, although the operations are depicted in a particular order, this should not be construed as requiring that the operations be performed in the particular order shown or in sequential order. In certain circumstances, multitasking and parallel processing may be advantageous. Similarly, although several specific implementation details are included in the foregoing description, these should not be construed as limiting the scope of the present disclosure. Certain features that are described in the context of separate embodiments may also be implemented in combination in a single embodiment. Conversely, the various features that are described in the context of a single embodiment may also be implemented separately or in any suitable sub-combination in multiple embodiments.

Claims

1. Control method for a degree-of-freedom implementation device of an aquatic inspection robot, characterized in that, It includes the following steps: Based on the target travel distance and the target rotation angle, the lifting motion trajectory and the rotational motion trajectory are respectively divided into 7-stage acceleration S-shaped curves to obtain double-acceleration S-shaped curves, and the stages include jerk acceleration, uniform acceleration, deceleration acceleration, constant speed, acceleration deceleration, uniform deceleration, and jerk deceleration; Adopt a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters; Based on the optimal double-acceleration S-shaped curve parameters, calculate the 7-stage velocity S-shaped curves of the lifting motion trajectory and the rotational motion trajectory as the velocity mapping rule; Obtain the lifting distance and the rotation angle in real time, and calculate the actual lifting speed and the actual rotation speed; Based on the actual lifting speed and the actual rotation speed, dynamically adjust the speed output using the PID control algorithm according to the velocity mapping rule.

2. The control method of the degree-of-freedom implementation device for an aquaculture inspection robot according to claim 1, characterized in that Adopting a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters, including: Establish a two-axis coupling relationship for vertical force balance and rotational torque balance; Define multi-axis motion constraint conditions, and the constraint conditions include mechanical strength constraints and motion interference constraints; Construct a parameter optimization function with the goal of minimizing the total motion time and inertial impact force, and use the Lagrange multiplier method to solve the parameter optimization function under the constraint conditions to obtain the optimal double S-shaped curve parameters.

3. The control method of the degree-of-freedom implementation device for an aquaculture inspection robot according to claim 2, characterized in that The two-axis coupling relationship for vertical force balance and rotational torque balance includes the balance between the vertical inertial force and the rotational coupling vertical component and the lifting driving force, and the balance between the rotational inertia torque and the lifting force coupling torque and the rotational torque.

4. The control method of the degree-of-freedom implementation device for an aquaculture inspection robot according to claim 2, characterized in that The expression of the parameter optimization function is: where T represents the total motion time of the 7 stages, represents the lifting acceleration, represents the rotational acceleration.

5. The control method of the degree-of-freedom implementation device for an aquaculture inspection robot according to claim 1, characterized in that After "Based on the actual lifting speed and the actual rotation speed, dynamically adjust the speed output using the PID control algorithm according to the velocity mapping rule", it further includes: Establish a suspension swing model, which is used to predict the swing angle of the suspension device during motion based on the acceleration of the double S curve; Set the feedforward gain; Multiply the feedforward gain by the predicted swing angle to obtain a feedforward control quantity based on the swing amplitude; Superimpose the feedforward control quantity based on the swing amplitude on the PID speed output as the final speed output to cancel the speed disturbance caused by the predicted swing.

6. The control method of the degree-of-freedom implementation device for an aquaculture inspection robot according to claim 5, characterized in that It further includes the following steps: When the deviation between the actual lifting speed or the actual rotation speed in any stage and the planned value in the speed mapping rule exceeds the preset deviation threshold, terminate the current stage, directly jump to the "deceleration stage", and linearly decay the feedforward control amount to 0.

7. Degree-of-freedom implementation device for an aquatic inspection robot, characterized in that, It includes a horizontal rotation mechanism, a telescopic lifting mechanism, a multi-device hanging platform, and a control module. The horizontal rotation mechanism is rotatably installed under the aquaculture inspection robot. The telescopic lifting mechanism is fixedly installed at the bottom of the horizontal rotation mechanism so that the horizontal rotation mechanism drives the telescopic lifting mechanism to rotate when rotating. 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 the detection devices required for multiple aquaculture inspection robots. Both the horizontal rotation mechanism and the telescopic lifting mechanism are 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, divide the lifting motion trajectory and the rotation motion trajectory into 7-stage acceleration S-shaped curves respectively to obtain double-acceleration S-shaped curves. The stages include positive acceleration, uniform acceleration, deceleration, constant speed, acceleration and deceleration, uniform deceleration, and deceleration. Adopt a multi-axis collaborative planning strategy to jointly optimize the parameters of the double-acceleration S-shaped curve to obtain the optimal double-acceleration S-shaped curve parameters. Based on the optimal double-acceleration S-shaped curve parameters, calculate the 7-stage speed S-shaped curves of the lifting motion trajectory and the rotation motion trajectory as the speed mapping rule. Obtain the lifting distance and the rotation angle in real time, and calculate the actual lifting speed and the actual rotation speed. Based on the actual lifting speed and the actual rotation speed, dynamically adjust the speed output according to the speed mapping rule using the PID control algorithm.

8. The degree-of-freedom realization device for an aquaculture inspection robot according to claim 7, wherein The telescopic lifting mechanism includes several telescopic sleeves, and the cross-section of the telescopic sleeve is elliptical.

9. The degree-of-freedom realization device for an aquaculture inspection robot according to claim 7, wherein 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 is of a polygonal structure and mounting holes are provided on each side of the lower bracket.

10. The degree-of-freedom realization device for an aquaculture inspection robot according to claim 7, wherein It further includes a waterproof proximity switch. 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 from the bottom of the aquaculture pond in real time. The waterproof proximity switch is fixedly connected to the control module.

Images