Sand suction and filling multi-arm cooperative control algorithm based on coupling of kinematics and dynamics

Abstract

The application provides a sand suction and filling multi-arm cooperative control algorithm based on kinematics and dynamics coupling, comprising the following steps: S1: water dynamic disturbance field full working condition data acquisition and feature library construction; S2: multi-arm kinematics modeling and unified mapping of working space; S3: construction of a multi-arm kinematics-dynamics coupling model embedded with a water dynamic disturbance adaptive compensation term; S4: multi-arm task-dynamics coupling distribution based on an improved parasite-prey algorithm; S5: trajectory-task bidirectional coupling planning based on a fractional order pseudospectral method; S6: multi-arm-filling cooperative closed loop control based on a port-controlled Hamilton fractional order passive control; and S7: real-time acquisition of sand suction and filling parameters and full system closed loop feedback. Through deep coupling modeling of kinematics and dynamics and bidirectional cooperative optimization of core control algorithms, the application realizes high-precision, high-stability and high-efficiency cooperative control of multi-arm sand suction and filling operation in a dynamic water environment.

Description

Technical Field

[0001] This invention relates to the field of underwater engineering equipment and multi-robot collaborative control technology, and in particular to a multi-arm collaborative control algorithm for sand suction and filling based on kinematic and dynamic coupling. Background Technology

[0002] With the large-scale development of underwater engineering operations such as inland waterway dredging, near-shore reclamation projects, and water conservancy facility repair, the complexity of working conditions and the requirements for operational precision in sand suction and filling operations continue to increase. Multi-arm collaborative operation systems have become the core equipment for sand suction and filling operations due to their advantages of wide operating range, strong load capacity, and flexible operation.

[0003] While existing multi-arm collaborative control technology has matured in conventional industrial scenarios, it still has significant technical shortcomings for underwater sand suction and filling operations in dynamic waters.

[0004] Firstly, most existing technologies adopt a modeling and control method that decouples kinematics and dynamics. The trajectory is planned based on the kinematic model first, and then the dynamic model is used for compensation and correction. This can easily lead to the planned trajectory exceeding the feasible range of the robotic arm's dynamics. In the dynamic environment of water flow disturbance, this can further lead to joint overload and a significant decrease in trajectory tracking accuracy.

[0005] Secondly, multi-arm task allocation often adopts traditional swarm intelligence algorithms such as particle swarm optimization and genetic algorithms, which only take operation time and path length as optimization objectives, without embedding the dynamic constraints of the robotic arm and the influence of hydrodynamic interference into the optimization process. The resulting allocation scheme has problems such as high energy consumption, failure of interference compensation, and multi-arm coordination conflicts in actual operation.

[0006] Third, the trajectory planning and task allocation adopt a serial processing mode, which can only complete the trajectory planning based on the established task allocation results. It cannot optimize the task allocation scheme in reverse through the feasibility of trajectory planning, resulting in the disconnect between task allocation and trajectory planning, and the overall operation performance cannot reach the global optimum.

[0007] Fourth, the closed-loop control of underwater operations mostly adopts conventional PID, sliding mode control and other algorithms. Under the conditions of strong dynamic water flow disturbance and real-time changes in sand suction load, the robustness and anti-interference ability are insufficient. Moreover, the motion control of the robotic arm and the concentration and flow control of sand suction filling are independent of each other, which cannot achieve coordinated matching between operation actions and operation results, seriously affecting the quality and efficiency of sand suction filling operations.

[0008] Fifth, the compensation for hydrodynamic disturbances often uses fixed compensation coefficients, which cannot adapt to changes in water flow under all operating conditions, resulting in insufficient compensation accuracy and further exacerbating control errors. Summary of the Invention

[0009] This invention provides a multi-arm collaborative control algorithm for sand suction and filling based on kinematic and dynamic coupling. Through deep coupling modeling of kinematics and dynamics, it improves the bidirectional collaborative optimization of three core algorithms: parasitic-predatory algorithm, fractional pseudospectral method, and port-controlled Hamiltonian fractional passive control. It also achieves deep integration of operation control and process control, solving the core pain points of existing technologies such as modeling decoupling, sequential process, weak anti-disturbance, and poor coordination. This enables high-precision, high-stability, and high-efficiency collaborative control of multi-arm sand suction and filling operations in dynamic water environments.

[0010] To achieve the above objectives, the present invention adopts the following technical solution:

[0011] The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling includes the following steps:

[0012] S1: Collect hydrodynamic disturbance data under all working conditions in the sand suction and filling water area. After preprocessing the collected hydrodynamic disturbance data under all working conditions, construct a hydrodynamic disturbance field feature library.

[0013] S2: For each robotic arm in the multi-arm collaborative operation system, construct a single robotic arm kinematic model, and at the same time establish a homogeneous transformation mapping relationship between the base coordinate system of multiple robotic arms and the global operation coordinate system to complete the unified mapping of the multi-arm collaborative operation space.

[0014] S3: Based on the single-arm kinematics model, homogeneous transformation mapping relationship and hydrodynamic disturbance field feature library, construct a multi-arm kinematic-dynamic coupling model with embedded hydrodynamic disturbance compensation term;

[0015] S4: Using the kinematic-dynamic coupling model of the multi-arm as a hard constraint, the improved parasitic-predatory algorithm is used to complete the task-dynamic coupling allocation of the multi-arm sand suction and filling operation, and the optimal task allocation scheme of the multi-arm is obtained.

[0016] S5: Based on the optimal task allocation scheme of the multi-arm and the kinematic-dynamic coupling model of the multi-arm, the fractional pseudospectral method is used to complete the trajectory-task bidirectional coupling planning, generate the optimal trajectory of the multi-arm operation, and feed the trajectory planning results back to S4 to optimize the task allocation scheme.

[0017] S6: Based on the optimal trajectory of multi-arm operation and the kinematic-dynamic coupling model of multi-arm, the port-controlled Hamiltonian fractional-order passive control algorithm is adopted to complete the coordinated closed-loop control of multi-arm motion and sand suction and filling process, and output the mechanical arm joint drive control command.

[0018] S7: Real-time data of sand suction and filling operations are collected synchronously through a multi-sensor fusion architecture. The collected real-time data is filtered and fed back to the multi-arm kinematic-dynamic coupling model of S3, the improved parasitic-predatory algorithm of S4, the fractional pseudospectral method of S5, and the port-controlled Hamiltonian fractional passive control algorithm of S6 to complete the closed-loop dynamic optimization of the entire system.

[0019] In this manual, S1 collects hydrodynamic disturbance data covering the entire range of the work area, including combinations of water flow velocity, water flow direction, and water density. The data collection process covers the entire work space under all work modes, including fixed-point sand suction, point transfer, and obstacle avoidance adjustment by the robotic arm. After removing outliers and performing smoothing filtering preprocessing on the collected data, a hydrodynamic disturbance field feature library is constructed using the work hydrological parameters and the robotic arm motion state parameters as feature dimensions.

[0020] In this specification, in S2, the DH parameter method is used to construct the kinematic model of a single robotic arm. The homogeneous transformation mapping relationship is constructed based on the installation layout of the multi-arm collaborative operation system. At the same time, the effective working range of the single robotic arm kinematic model is limited by combining the coverage of the hydrodynamic interference field feature library.

[0021] In this specification, in S3, the Newton-Euler recursive method is used to construct a multi-arm kinematic-dynamic coupling model. The joint motion parameters of the robotic arm output by the single-arm kinematic model are used as endogenous input variables of the dynamic equation. The embedded hydrodynamic disturbance compensation term can be updated in real time based on the hydrodynamic disturbance field feature library to match the current working conditions. At the same time, motion coupling constraints and force coupling constraints between multiple robotic arms are added to the multi-arm kinematic-dynamic coupling model.

[0022] In this specification, in S4, the improved parasitic-predatory algorithm aims to minimize the overall operating cost of the multi-arm system. The overall operating cost includes the total execution time, total motion path length, total driving energy consumption, and total hydrodynamic interference compensation error. The hard constraints set by the improved parasitic-predatory algorithm include joint torque constraints, joint motion constraints, hydrodynamic compensation constraints, and collision safety constraints. All constraints are derived from the kinematic-dynamic coupling model of the multi-arm system.

[0023] In this specification, in S4, the iterative process of the improved parasitic-predator algorithm sequentially executes the individual position update of the predator stage, the parasitic stage, and the symbiotic stage. In the symbiotic stage, the trajectory planning results output by the fractional pseudospectral method are simultaneously incorporated, and the individual position update strategy is adjusted based on the trajectory optimization effect, thereby simultaneously realizing the bidirectional collaborative optimization of task allocation and trajectory planning.

[0024] In this specification, in S5, the fractional pseudospectral method is constructed based on the Caputo fractional differential operator, with the optimization objective of minimizing trajectory control energy consumption, fractional oscillations, and task allocation deviation. The trajectory optimization constraints include fractional dynamic constraints obtained from the transformation of the multi-arm kinematic-dynamic coupling model.

[0025] In this specification, in S6, the port-controlled Hamiltonian fractional passive control algorithm designs the control law based on the Hamiltonian energy function of the robotic arm system. The control law includes a feedforward control term, a fractional feedback control term, and a robust compensation term. The feedforward control term is derived from the optimal trajectory of the multi-arm operation, and the robust compensation term is dynamically adjusted based on the hydrodynamic disturbance field feature library and the real-time acquired load disturbance data.

[0026] In this specification, in step S6, based on real-time data and the reference state corresponding to the optimal trajectory of the multi-arm operation, the maximum real-time tracking error over the entire operation cycle is calculated. This maximum real-time tracking error is fed back to the fractional pseudospectral method in step S5 to dynamically adjust the smoothness constraint of trajectory optimization. Based on real-time data, the comprehensive real-time operation cost data is calculated according to the objective function of the improved parasitic-predator algorithm. This comprehensive real-time operation cost data is fed back to step S4 to trigger online dynamic optimization of the task allocation scheme, realizing the pairwise bidirectional collaborative optimization of the improved parasitic-predator algorithm, the fractional pseudospectral method, and the port-controlled Hamiltonian fractional passive control algorithm.

[0027] In this manual, S7 collects real-time data from all dimensions, including sand suction and filling process parameters, robotic arm motion state parameters, and hydrological parameters of the operating area. Based on the collected sand suction and filling process parameters, the real-time load change is calculated and the disturbance term of the multi-arm kinematic-dynamic coupling model constructed in S3 is corrected. All data after filtering is fed back to the corresponding algorithm module in real time, realizing online dynamic optimization and adaptive adaptation of the entire system under all working conditions.

[0028] In summary, the present invention has at least the following beneficial effects:

[0029] This invention uses deep coupling modeling of kinematics and dynamics, taking the kinematic output as an endogenous input variable of the dynamic equation, avoiding the planning and execution disconnect problem caused by traditional decoupled modeling, and ensuring that all control commands are within the dynamic feasible range of the robotic arm, thereby improving the reliability and accuracy of multi-arm collaborative control from the modeling root.

[0030] This invention employs three core algorithms: an improved parasitic-predator algorithm, a fractional pseudospectral method, and a port-controlled Hamiltonian fractional passive control. These algorithms enable bidirectional interaction and collaborative optimization between task allocation, trajectory planning, and closed-loop control, breaking the limitations of traditional serial processing and achieving global optimization of the entire multi-arm collaborative operation process, thus significantly improving the overall operational performance of the system.

[0031] This invention achieves accurate compensation for disturbances in dynamic water flow environments by constructing a feature library of hydrodynamic disturbance fields under all operating conditions and embedding adaptive compensation into the dynamic model. Combined with the strong robustness of fractional-order passive control, it significantly improves the system's anti-interference capability and operational stability under strong disturbances and variable load conditions.

[0032] This invention deeply integrates the motion control of the robotic arm with the process control of sand suction and filling concentration and flow rate, realizing a closed-loop match between the operation action and the operation effect, avoiding the problem of disconnect between motion control and process control, and effectively improving the operation quality and efficiency of sand suction and filling operations.

[0033] This invention achieves online dynamic optimization of the operation process through a closed-loop feedback link of multi-sensor fusion, which can adapt to real-time changes in operating conditions, expands the algorithm's adaptability to all operating conditions, and can be widely applied to multi-arm collaborative sand suction and filling operations in various complex underwater environments. Attached Figure Description

[0034] To more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings used in the following description of the embodiments will be briefly introduced. Obviously, the drawings described below are only some embodiments of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0035] Figure 1 This is a flowchart illustrating the multi-arm collaborative control algorithm for sand suction and filling based on kinematic and dynamic coupling involved in this invention.

[0036] Figure 2 This is a schematic diagram of the basic data acquisition and coupling modeling process involved in this invention.

[0037] Figure 3 This is a schematic diagram of the bidirectional collaborative optimization process of task allocation and trajectory planning involved in this invention.

[0038] Figure 4 This is a schematic diagram of the process of collaborative closed-loop control of the multi-arm motion-filling process involved in this invention. Detailed Implementation

[0039] In the following description, only certain exemplary embodiments are briefly described. As those skilled in the art will recognize, the described embodiments can be modified in various ways without departing from the spirit or scope of the embodiments of the invention. Therefore, the drawings and description are considered to be exemplary in nature and not restrictive.

[0040] The following disclosure provides many different implementations or examples for carrying out different structures of the embodiments of the present invention. To simplify the disclosure of the embodiments of the present invention, specific examples of components and arrangements are described below. Of course, these are merely examples and are not intended to limit the embodiments of the present invention. Furthermore, reference numerals and / or reference letters may be repeated in different examples of the embodiments of the present invention; such repetition is for simplification and clarity and does not in itself indicate a relationship between the various implementations and / or arrangements discussed.

[0041] The embodiments of the present invention will now be described in detail with reference to the accompanying drawings.

[0042] like Figure 1 As shown, this embodiment provides a multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling, including:

[0043] S1: Data Acquisition and Feature Database Construction for Hydrodynamic Disturbance Fields under All Operating Conditions

[0044] This step provides a comprehensive data source for subsequent dynamic model disturbance compensation, task allocation energy consumption optimization, and trajectory planning perturbation constraints. The basic data acquisition and coupled modeling process is as follows: Figure 2 As shown.

[0045] Sand suction and filling operations are typically conducted in dynamic aquatic environments. Water flow disturbance is the core source of reduced accuracy and excessive energy consumption in multi-arm collaborative operations. Therefore, based on the hydrological survey report of the target water area for sand suction and filling operations, the hydrological parameter range covering all operating conditions must first be determined. Specifically, the water flow velocity should cover from 0 m / s to the maximum measured flow velocity in the operating area, with a step size not exceeding 0.2 m / s; the water flow direction should cover the entire circumference from 0° to 360°, with a step size not exceeding 15°; and the water density should cover the density range from freshwater to sand-laden, high-turbidity water, with a step size not exceeding 50 kg / m³. 3 The entire working condition is divided into no less than 20 groups of continuous gradient test conditions, and each group of conditions corresponds to a unique combination of water flow velocity, water flow direction and water density.

[0046] A single robotic arm of the multi-arm collaborative operation system was fixed in a water tank test environment. For each working condition, the water flow speed, direction, and density were precisely controlled by the water circulation system. Real-time data on hydrodynamic disturbance forces of the robotic arm in static, uniform, and variable motion postures were collected. These three postures correspond to the full working modes of sand suction, point transfer, and obstacle avoidance adjustment in sand suction operations. During the data collection, the angle of each joint of the robotic arm covered its entire stroke range, with a step size not exceeding 5°, ensuring that all working postures of each joint had corresponding data support. The data sample size for each working condition was no less than 1,000 sets, covering the entire working space of the robotic arm.

[0047] For the collected hydrodynamic disturbance force data under all working conditions, outliers were removed using the 3σ criterion, and smoothing preprocessing was completed using moving average filtering. A hydrodynamic disturbance field feature library was constructed with water flow velocity, water flow direction, water density, angles of each joint of the robotic arm, and movement speed of each joint as feature dimensions. Each set of feature vectors in the feature library corresponds to a unique measured hydrodynamic disturbance torque data.

[0048] S2: Kinematic Modeling and Unified Mapping of the Workspace for Multi-arm Systems

[0049] This step establishes a unified pose description benchmark for multi-arm collaborative operations, avoiding problems such as accurate pose for a single arm but collisions and pose deviations during multi-arm collaborative operations.

[0050] For each robotic arm in a multi-arm collaborative operation system, the standard DH parameter method is used to define four DH parameters for each joint of the robotic arm. Let be the joint angle of the i-th joint, used to describe the rotation angle of the joint about its own z-axis. The value of i ranges from 1 to n, and n is the total number of joints in a single robotic arm. In this scheme, n is 6, corresponding to a 6-DOF serial robotic arm. Let be the link twist angle of the i-th link, used to describe the spatial angle between the z-axis of the i-th joint and the z-axis of the (i+1)-th joint, that is, the angle of rotation around the x-axis; Let be the link length of the i-th link, used to describe the vertical distance between the z-axis of the i-th joint and the z-axis of the (i+1)-th joint, that is, the translation along the x-axis; Let be the link offset of the i-th joint, used to describe the offset distance between the origin of the (i-1)-th link coordinate system and the origin of the i-th link coordinate system along the z-axis direction of the i-th joint.

[0051] For each joint, based on its four DH parameters, a homogeneous transformation matrix is ​​constructed. This matrix describes the pose transformation relationship between the i-th link coordinate system and the (i-1)-th link coordinate system. It is the core foundation for the kinematic modeling of the serial manipulator, and its matrix expression is as follows:

[0052] ;

[0053] By multiplying the homogeneous transformation matrices of all six joints of a single robotic arm in sequence, the pose matrix of the end effector in its own base coordinate system can be obtained. The pose matrix contains the 3D position coordinates and 3D posture rotation matrix of the end effector, thus completing the construction of the forward kinematics model of the single robotic arm. All subsequent end pose calculations and inverse kinematics solutions of joint angles are based on this forward kinematics model.

[0054] Multiple robotic arms are installed on a sand suction and filling platform. Their respective base coordinate systems are independent and must be unified under the global operating coordinate system to achieve pose constraints, collision detection, and task allocation for collaborative operations. Therefore, based on the installation layout drawings of the multi-arm collaborative operation system, the position of the origin of the base coordinate system of each robotic arm in the global operating coordinate system, as well as the rotation angle of the three axes of the base coordinate system relative to the three axes of the global coordinate system, are accurately measured. A homogeneous transformation mapping matrix between the base coordinate system of each robotic arm and the global operating coordinate system is constructed. Through this mapping matrix, the pose matrix of the end effector of any robotic arm can be converted into a unified pose matrix in the global operating coordinate system, completing the unified mapping of the multi-arm collaborative operation space and ensuring that the pose description of all robotic arms is under the same reference.

[0055] By combining the full-condition hydrodynamic disturbance field data collected in S1 with the corresponding robotic arm working space boundary, the effective working range of the single robotic arm kinematic model is limited, and invalid poses that exceed the coverage of the hydrodynamic disturbance field feature library are eliminated, ensuring that all poses output by the kinematic model can be matched with the corresponding hydrodynamic disturbance data in the feature library of S1.

[0056] S3: Construct a multi-arm kinematic-dynamic coupling model with embedded adaptive compensation terms for hydrodynamic disturbances.

[0057] This step addresses the problem in traditional multi-arm control where kinematics and dynamics are processed separately, leading to trajectory planning results exceeding the dynamically feasible range and situations where planning is feasible but execution is impossible. The kinematic-dynamic coupled model constructed in this step uses the joint angles, angular velocities, and angular accelerations output by kinematics as endogenous input variables of the dynamic equations, achieving deep coupling between kinematics and dynamics. This provides a core model that simultaneously satisfies both kinematic and dynamic constraints for subsequent task allocation, trajectory planning, and closed-loop control.

[0058] The rigid body dynamics equations of the robotic arm are derived using the Newton-Euler recursive method. These equations fully describe the mapping relationship between the joint driving torques and joint motion states of the robotic arm, and are the core foundation of the robotic arm's dynamic control. The equation expression is as follows: ;

[0059] Where q is the joint angle vector of the robotic arm, and each element of the vector is the joint angle of the corresponding joint. The dimension is 6 rows and 1 column; Let be the joint angular velocity vector of the robotic arm, and q be the first derivative of q with respect to time, with a dimension of 6 rows and 1 column; Let be the joint angular acceleration vector of the robotic arm, and be the second derivative of q with respect to time, with dimensions of 6 rows and 1 column; M(q) is the inertia matrix of the robotic arm, with dimensions of 6 rows and 6 columns, used to describe the inertial characteristics of the robotic arm under different joint poses, and its elements are calculated from the mass, moment of inertia, center of mass position of each link of the robotic arm and joint angle q; C(q, This is the Coriolis and centrifugal force matrix of the robotic arm, with dimensions of 6 rows and 6 columns. It describes the additional inertial force effect generated by the joint rotational motion, and its elements consist of the joint angle q and the joint angular velocity. Together they are calculated to obtain G(q), which is the gravity term vector of the robotic arm with a dimension of 6 rows and 1 column. It is used to describe the torque effect of gravity on each joint of the robotic arm. Its elements are calculated by the joint angle q and the mass and center of mass of each link. It is a hydrodynamic disturbance torque vector with a dimension of 6 rows and 1 column. It is used to describe the disturbance torque generated by the water flow on each link of the robotic arm. It is the most important external disturbance in dynamic water operations. is the driving torque vector of each joint of the robotic arm, with a dimension of 6 rows and 1 column, is the control torque output by the joint actuator, and is the final output of the dynamic control.

[0060] Traditional underwater robotic arm dynamics models typically treat hydrodynamic disturbances as fixed compensation terms, which cannot adapt to changes in water flow under all operating conditions. Therefore, this scheme embeds a real-time updated hydrodynamic disturbance force term into the rigid body dynamics equations, expressed as follows: ;

[0061] in The hydrodynamic disturbance compensation factor matrix has a dimension of 6 rows and 3 columns. It is used to describe the mapping relationship between the water flow velocity and the disturbance torque of each joint of the robotic arm in the global coordinate system. It is the core parameter to be calibrated. The velocity vector is 3 rows and 1 column, used to describe the velocity components of the water flow in the working area along the x, y, and z axes in the global coordinate system. Its value is obtained by matching the hydrodynamic disturbance field feature library in step S1 with the current working conditions.

[0062] For each robotic arm in the multi-arm collaborative operation system, the kinematic-dynamic coupling model of the single robotic arm is constructed as described above. Then, based on the homogeneous transformation mapping matrix between the multi-arm base coordinate system and the global operation coordinate system output in step S2, the overall kinematic-dynamic coupling model of the multi-arm system is constructed. The model incorporates motion coupling constraints and force coupling constraints between the multiple robotic arms. The motion coupling constraint ensures that the minimum spatial distance between the multiple robotic arms in the global coordinate system is not less than the safe collision distance. The force coupling constraint ensures that when multiple robotic arms collaboratively operate the same sand suction and filling end effector, the resultant force at the end of each robotic arm is balanced with the work load, ensuring that the motion and force of all robotic arms are under the constraints of a unified coupling model during multi-arm collaborative operation.

[0063] S4: Multi-arm task-dynamic coupling assignment based on improved parasitic-predatory algorithm

[0064] This step addresses the problem that traditional multi-arm task allocation only considers task time and path length, neglecting the influence of dynamic constraints and hydrodynamic interference. This leads to problems such as excessive energy consumption, joint overload, and interference compensation failure in actual operation. This step employs an improved parasitic-predator algorithm, a swarm intelligence optimization algorithm rarely used in the field of multi-arm robotic collaboration. Compared to traditional particle swarm optimization and genetic algorithms, it has stronger global search and local exploitation capabilities, and is less prone to getting trapped in local optima. Furthermore, this algorithm embeds the kinematic-dynamic coupling model of step S3 as a hard constraint into the objective function, achieving deep coupling between task allocation and dynamic characteristics. The task allocation-trajectory planning bidirectional collaborative optimization process is as follows: Figure 3 As shown.

[0065] The overall task of the sand suction and filling operation is to perform full-coverage sand suction and filling within the work area. This overall task is broken down into N independent sand suction and filling sub-tasks. Each sub-task corresponds to a unique sand suction point location, task execution duration, preset sand suction and filling concentration value, and preset sand suction and filling flow rate value. The value of N is determined by the area of ​​the work area and the coverage range of a single sub-task. The decision variable X is an N x 1 vector, where each element of the vector... To assign the robotic arm number to perform the j-th subtask, j ranges from 1 to N, the robotic arm number ranges from 1 to m, and m is the total number of robotic arms in the multi-arm system.

[0066] The objective function for constructing the improved parasitic-predatory algorithm is to minimize the overall operating cost of the multi-arm system, and its expression is:

[0067] ;

[0068] To improve the objective function value of the parasitic-predator algorithm, a smaller value indicates a lower overall operating cost; , , , These are the weight coefficients of the objective function. The sum of all weight coefficients is 1. The value of each weight coefficient ranges from 0 to 1. The weight priority can be adjusted according to the task requirements. The total execution time for the multi-arm system to complete all sub-tasks is the maximum time for all robotic arms to complete the assigned sub-task sequence, used to measure operational efficiency. The total motion path length for the multi-arm system to complete all sub-tasks is the sum of the motion path lengths of all robotic arms executing sub-tasks, used to measure the motion loss of the robotic arms. The total drive energy consumption for the multi-arm system to complete all sub-tasks is the sum of the integrals of the drive torques of all joints of the robotic arms, expressed as:

[0069] ;

[0070] k is the number of the robotic arm, and its value ranges from 1 to m; The total time for the k-th robotic arm to complete the assigned sub-tasks; The joint driving torque vector of the k-th robotic arm at time t is calculated by the kinematic-dynamic coupling model in step S3, ensuring that the energy consumption calculation is based entirely on the dynamic model, rather than empirical estimation. The total hydrodynamic interference compensation error for the multi-arm system to complete all sub-tasks is expressed as the sum of squared errors between the hydrodynamic interference compensation values ​​and the measured values ​​for each joint of the robotic arm, and is given by:

[0071] ;

[0072] The calculated hydrodynamic disturbance torque vector of the k-th robotic arm at time t is derived from the calibrated... Calculated; Let be the measured hydrodynamic disturbance torque vector of the k-th robotic arm at time t. It is obtained by matching the current working condition with the hydrodynamic disturbance field feature library in step S1. This index is used to measure the adaptability of the task scheme to hydrodynamic disturbance.

[0073] The algorithm is configured with hard constraints, all derived from the kinematic-dynamic coupling model: The first constraint is the joint torque constraint, where the absolute value of the driving torque of all joints of all robotic arms during the entire operation time does not exceed the maximum allowable driving torque of that joint; the second constraint is the joint motion constraint, where the absolute values ​​of the joint angle, joint angular velocity, and joint angular acceleration of all joints of all robotic arms during the entire operation time do not exceed their respective maximum allowable values; the third constraint is the hydrodynamic compensation constraint, where the absolute value of the hydrodynamic disturbance torque of all joints of all robotic arms during the entire operation time does not exceed the maximum allowable compensation torque of that joint, to avoid situations where the disturbance exceeds the compensation capacity; and the fourth constraint is the collision safety constraint, where the minimum spatial distance between any two robotic arms during the entire operation time is not less than the preset safe collision distance, to avoid collisions during multi-arm collaborative operations.

[0074] The algorithm comprises three core evolutionary stages. The first stage is the predation stage, which simulates the hunting behavior of predators in an ecosystem. This stage enables the algorithm to perform a global search and avoid getting trapped in local optima. The individual position update formula during the predation stage is:

[0075] ;

[0076] Let p be the position vector of the p-th individual in the t-th iteration, where p ranges from 1 to NP and NP is the population size of the algorithm. Let be the position vector of the p-th individual at the (t+1)-th iteration; Let be the optimal individual position vector in the population at the t-th iteration, corresponding to the objective function. The minimum value; Let be the convergence factor during the predation phase, expressed as:

[0077] ;

[0078] The convergence control factor decreases linearly from 2 to 0 with the number of iterations, and its expression is: , This represents the maximum number of iterations for the algorithm. A random number between 0 and 1, used to increase the randomness of the search.

[0079] The second stage is the parasitic stage, which simulates the parasitic behavior of organisms on their hosts in an ecosystem. This stage is used to enable the algorithm's local exploitation capabilities and improve its solution accuracy. The individual position update formula in the parasitic stage is:

[0080] ;

[0081] and Let r1 and r2 be the position vectors of two different individuals randomly selected from the population at the t-th iteration. r1 and r2 are random integers between 1 and NP, and r1 is not equal to r2 or p. The step size factor for the parasitic phase is a random number between 0 and 1.

[0082] The third stage is the symbiotic stage, which simulates the mutualistic symbiotic behavior among species in an ecosystem. This stage aims to achieve a dynamic balance between the algorithm's global search and local exploitation, and also enables bidirectional interaction between this algorithm and the S5 step fractional pseudospectral method. The individual position update formula in the symbiotic stage is:

[0083] ;

[0084] Let be the average vector of the positions of all individuals in the population at the t-th iteration; The synergistic factor in the symbiotic phase is expressed as follows: As the number of iterations decreases linearly from 1 to 0, a balance is achieved between early global search and later local development. , where is the interaction coupling factor, and is a preset fixed constant; For the t-th iteration, the fractional pseudospectral method in step S5 calculates the trajectory optimization objective function value based on the task allocation scheme corresponding to the current individual position; The reference threshold is set as the objective function reference for trajectory optimization. This formula realizes bidirectional interaction between the two algorithms. The individual position update of this algorithm is directly affected by the trajectory optimization result of the fractional pseudospectral method. If the trajectory optimization objective function value corresponding to the current task allocation scheme exceeds the reference threshold, it indicates that the trajectory feasibility of the task scheme is poor. The algorithm will automatically adjust the individual position and select a task scheme with higher trajectory feasibility. At the same time, the task allocation scheme corresponding to the individual position of this algorithm is also the input boundary condition of the fractional pseudospectral method trajectory optimization. Changes in the task scheme will directly affect the trajectory optimization result. The two form a bidirectional coupled interactive relationship, rather than the traditional unidirectional process of first allocating tasks and then planning trajectories.

[0085] The iterative solution process of the algorithm is as follows: First, initialize the algorithm parameters, set the population size NP, and the maximum number of iterations. Weighting coefficient , , , Inter-coupling factor The first step involves determining the safe collision distance, threshold parameters for all constraints, and the values ​​of all parameters based on the robot arm's body parameters and operational requirements. The second step is to initialize the population. Within the feasible region of the decision variables, NP initial position vectors are randomly generated for each individual. Each individual's position vector corresponds to a multi-arm task allocation scheme, ensuring the initial population covers a sufficiently wide feasible solution space. The third step is to calculate the objective function value for each individual. For each individual's corresponding task allocation scheme, the fractional pseudospectral method from step S5 is used to calculate the trajectory optimization objective function value corresponding to that scheme. Simultaneously, based on the kinematic-dynamic coupling model of step S3, the corresponding scheme is calculated. , , , Substituting into the objective function formula, we obtain the values ​​for each individual. The objective function value is calculated to ensure that the objective function value of each individual simultaneously considers the effects of task allocation, trajectory planning, and dynamic constraints. The fourth step involves updating the optimal individual in the population by comparing the objective function values ​​of all individuals and finding the individual with the smallest objective function value, which is then selected as the optimal individual for the current iteration. The fifth step involves updating the position vectors of all individuals during the predation phase, according to the formula for the predation phase. Feasibility region constraints are then applied to the updated position vectors to ensure that all individuals' positions are within the feasible region of the decision variables, eliminating invalid individuals that exceed the constraints. The sixth step involves updating the position vectors of all individuals during the parasitic phase, according to the formula for the parasitic phase, and applying feasible region constraints to the updated position vectors. The seventh step involves updating the position vectors of the symbiotic phase, according to the formula for the symbiotic phase, combined with the position vectors returned by the fractional pseudospectral method in step S5. The value is updated, and the position vectors of all individuals are then subjected to feasible region constraints to complete one complete population iteration. The eighth step is to determine if the iteration count has reached the maximum number of iterations. If the objective function value of the optimal individual in the population does not decrease for 10 consecutive iterations, it indicates that the algorithm has converged to the optimal solution, and the iteration is terminated, outputting the optimal task allocation scheme for the multi-arm corresponding to the optimal individual; if the termination condition is not met, return to step 3 and continue to the next iteration.

[0086] During the application of the algorithm, the optimal task allocation scheme obtained after iterative convergence is output to the fractional pseudospectral method in step S5 as the boundary condition for trajectory planning. At the same time, the sub-task sequence, task pose, and task execution time window in the optimal task allocation scheme are output to the port-controlled Hamiltonian fractional passive control module in step S6 as the task reference value for closed-loop control. Simultaneously, the actual operation data returned by step S6 is received. When the deviation between the comprehensive objective function value of the actual operation and the optimal objective function value obtained by the algorithm exceeds a preset threshold, the algorithm is triggered to iterate and optimize again, realizing the online dynamic update of the task allocation scheme.

[0087] S5: Trajectory-Task Coupled Planning Based on Fractional Pseudospectral Method

[0088] This step addresses the shortcomings of traditional robotic arm trajectory planning, which can only guarantee the continuity of the trajectory by integer order, making it unable to adapt to the disturbance suppression requirements in dynamic water environments. Furthermore, it can only perform unidirectional trajectory planning based on task allocation results and cannot optimize the task allocation scheme in reverse. This step adopts the fractional pseudospectral method, which is an optimal control method rarely used in the field of multi-arm cooperative trajectory planning. Compared with traditional integer-order trajectory planning methods, it can achieve fractional-order smoothness of the trajectory and effectively suppress trajectory oscillations caused by water flow disturbances.

[0089] First, we define the Caputo fractional differential operator, which is the mathematical foundation of the fractional pseudospectral method. For a continuously differentiable function f(t), its... The expression for the fractional derivative of order Caputo is:

[0090] ;

[0091] The order of the fractional derivative ranges from 0 to 2; in this scheme, it is set to 1.2. This order achieves an optimal balance between trajectory smoothness and dynamic response characteristics; n is greater than... The smallest integer n is n, and in this scheme, n is 2; This is the gamma function, used to calculate fractional factorials. For a positive real number z, ; Let f(τ) be the nth integer derivative of the function f(τ) with respect to τ. The core advantage of Caputo fractional derivatives is that their initial conditions are consistent with those of integer derivatives, both being the integer derivative values ​​of the function at the initial time. This makes them very suitable for solving trajectory optimization and optimal control problems with determined initial values.

[0092] Define the state and control variables for trajectory optimization. For a single robotic arm, the state variable x(t) is a 12-row, 1-column vector, where the first 6 elements are the joint angle vector q(t) and the last 6 elements are the joint angular velocity vector. This corresponds to the full motion state of a 6-DOF robotic arm; the control variable u(t) is a 6x1 vector, representing the joint angular acceleration vector. , is the control input for trajectory optimization.

[0093] Constructing the objective function for trajectory optimization The objective is to minimize the trajectory control energy consumption, fractional-order oscillations, and task assignment deviation, expressed as:

[0094] ;

[0095] , , The weight coefficients are used to optimize the objective function of the trajectory. The sum of all weight coefficients is 1, and the value of each weight coefficient ranges from 0 to 1. T is the execution time window of the current subtask, which is allocated by the improved parasitic-predator algorithm in step S4. Improve the objective function value of the parasitic-predator algorithm for the S4 step corresponding to the current trajectory; This formula provides the optimal objective function reference value for the improved parasitic-predator algorithm in step S4. It realizes a bidirectional interaction between the two algorithms. The objective function of the current algorithm is directly affected by the objective function value of the improved parasitic-predator algorithm. If the overall task cost corresponding to the currently planned trajectory exceeds the optimal reference value, it indicates that the trajectory will lead to a decrease in the overall performance of the task allocation scheme. The algorithm will automatically adjust the direction of trajectory optimization to reduce the overall task cost. Simultaneously, the calculation of the objective function of the improved parasitic-predator algorithm requires the trajectory optimization result of the current algorithm. The quality of the task allocation scheme is directly verified by the trajectory planning result. These two elements form a bidirectional coupled interactive relationship, achieving collaborative optimization of task allocation and trajectory planning, rather than a traditional serial process.

[0096] The constraints for trajectory optimization are set as follows: The first term is a fractional-order dynamic equation constraint. Based on the kinematic-dynamic coupling model of step S3, the integer-order dynamic equations are converted into fractional-order forms to achieve deep coupling between trajectory planning and the dynamic model. The expression is as follows:

[0097] ;

[0098] joint angle vector The fractional-order Caputo derivative corresponds to the fractional-order angular acceleration of the robotic arm; joint angle vector The Caputo fractional derivative corresponds to the fractional angular velocity of the robotic arm. This constraint ensures that the planned trajectory fully satisfies the fractional dynamics characteristics of the robotic arm, avoiding trajectories that are feasible for integer-order planning but not for fractional-order dynamics.

[0099] The second term is the boundary condition constraint. The initial and final states of the trajectory are determined by the subtask initial pose and target pose assigned by the improved parasitic-predator algorithm in step S4. The expression is:

[0100] , ;

[0101] This is the initial state vector of the subtask, corresponding to the joint angles and joint angular velocities of the initial pose; This is the termination state vector of the subtask, corresponding to the joint angles and joint angular velocities of the target pose.

[0102] The third constraint is the control variable constraint. The range of values ​​for the control variable u(t) satisfies the minimum and maximum limits of the joint angular acceleration, ensuring that the planned angular acceleration is within the output capability range of the robotic arm's joint actuator. The fourth constraint is the state variable constraint. The range of values ​​for the state variable x(t) satisfies the minimum and maximum limits of the joint angle and joint angular velocity, ensuring that the planned trajectory is within the effective working range of the robotic arm. The fifth constraint is the obstacle avoidance constraint. The minimum distance between the robotic arm's end effector and all obstacles in the workspace is not less than the preset obstacle avoidance safety distance, ensuring that the robotic arm will not collide with obstacles during movement. The sixth constraint is the closed-loop tracking error constraint. The upper limit of the trajectory tracking error is determined by the real-time tracking error returned by the port-controlled Hamiltonian fractional-order passive control module in step S6, expressed as:

[0103] ;

[0104] This is the reference state vector for the trajectory; This is the maximum tracking error threshold returned by the port-controlled Hamiltonian fractional-order passive control module in step S6. This constraint enables bidirectional interaction between the two algorithms. The trajectory optimization constraint of this algorithm is directly affected by the actual tracking error of the closed-loop control module. If the actual tracking error of the closed-loop control exceeds the preset value, the algorithm will automatically tighten the trajectory smoothness constraint to reduce the tracking difficulty. At the same time, the optimal trajectory output by this algorithm is also the reference input of the closed-loop control module. The characteristics of the trajectory directly affect the tracking accuracy of the closed-loop control. The two form a bidirectional coupled interactive relationship, realizing the collaborative optimization of trajectory planning and closed-loop control.

[0105] The core steps in solving optimal control problems using the fractional pseudospectral method are completing the collocation and discretization model construction. The first step is time interval mapping, mapping the actual operation's time interval [0,T] to the standard computation interval [-1,1] of the pseudospectral method. The mapping formula is as follows: ;

[0106] τ represents the coordinates of the collocation point within the standard interval, and t represents the time variable within the original time interval.

[0107] The second step is collocation selection. Within the standard interval [-1, 1], N Legendre-Gauss-Lobart collocation points are selected. These points are the roots of the derivative of the Nth-order Legendre polynomial. Including the interval endpoints -1 and 1, there are a total of N+1 collocation points, numbered from 0 to N. The core advantage of the Legendre-Gauss-Lobart collocation is its high numerical accuracy and fast convergence speed, making it very suitable for solving smooth optimal control problems. The third step is to construct the fractional differential matrix. The matrix has dimensions of (N+1) rows and (N+1) columns, used to convert continuous fractional differential operations into discrete matrix multiplication operations, significantly reducing the difficulty of solving the problem. The elements of the matrix... The Lagrange interpolation basis function at the i-th collocation point The value of the Caputo fractional derivative at the j-th collocation point is obtained through pre-calculation to ensure the efficiency of the solution process. The fourth step involves discretizing the state and control variables at the collocation points. The discrete values ​​of the state variables at the collocation points are... The discrete values ​​of the control variables at the collocation points are , Let be the coordinates of the i-th collocation point. The fifth step is to discretize the fractional-order dynamic constraints, transforming the continuous fractional-order dynamic equality constraints into discrete algebraic constraints, expressed as:

[0108] ;

[0109] Let be the discrete value of the state variable at the i-th collocation point; The product of the fractional-order differential matrix at the i-th collocation point and the discrete value of the state variable corresponds to the fractional-order angular acceleration. for The product of the fractional-order differential matrix and the discrete values ​​of the state variables corresponds to the fractional-order angular velocity. The discrete value of the hydrodynamic disturbance torque at the i-th collocation point is obtained by matching the feature library of the hydrodynamic disturbance field in step S1. Let be the discrete value of the control variable at the i-th collocation point.

[0110] Step 6: Discretize the objective function using the Gaussian integral formula, transforming continuous integration into a weighted sum at collocation points. The expression is:

[0111] ;

[0112] The Gaussian integral weights corresponding to the i-th collocation point are pre-calculated using the Legendre-Gauss-Lobart integral formula. Through discretization, the original continuous-time optimal control problem is transformed into a standard nonlinear programming problem, which can be solved using a mature nonlinear programming solver.

[0113] The algorithm's solution process is as follows: First, initialize the algorithm parameters and set the fractional order. Number of points N, weighting coefficient , , The first step involves determining the threshold parameters for all constraints and the maximum number of iterations. The second step involves obtaining the optimal task allocation scheme output by the improved parasitic-predator algorithm module in step S4, extracting the initial pose, target pose, and execution time window for each subtask as boundary conditions for trajectory optimization. The third step involves initializing the discrete values ​​of the state and control variables using linear interpolation based on the boundary conditions, ensuring that the initial values ​​satisfy the boundary constraints. The fourth step involves constructing a nonlinear programming problem, using the discretized state and control variables as optimization variables and a discretized objective function. To optimize the objective, discretized fractional-order dynamic constraints, boundary condition constraints, control variable constraints, state variable constraints, obstacle avoidance constraints, and closed-loop tracking error constraints are used as constraints. The fifth step involves calling a nonlinear programming solver and employing a sequential quadratic programming algorithm to solve the constructed nonlinear programming problem, obtaining the optimized discrete values ​​of the state and control variables. Sequential quadratic programming is an efficient algorithm for solving nonlinear programming problems, with fast convergence and high accuracy. The sixth step involves accuracy verification: substituting the optimized discrete values ​​into the fractional-order dynamic equations and calculating the residuals of the dynamic constraints. If the residuals are less than a preset accuracy threshold, the solution is valid; if the residuals are greater than the accuracy threshold, the number of collocation points is increased, and the process returns to the third step to resolve, ensuring the solution accuracy meets the job requirements. The seventh step involves trajectory reconstruction: based on the optimized discrete values ​​of the state and control variables, Lagrange interpolation is used to reconstruct a continuous optimal trajectory, including continuous time series of joint angles, joint angular velocities, and joint angular accelerations, as well as the corresponding joint driving torque sequences. The eighth step involves bidirectional interactive feedback: the objective function value corresponding to the reconstructed optimal trajectory is then fed back. The feedback is sent to the improved parasitic-predator algorithm module in step S4 for updating the individual position during the symbiotic phase of the algorithm; the tracking error threshold requirement of the optimal trajectory is fed back to the port-controlled Hamiltonian fractional passive control module in step S6 for adjusting the parameters of the closed-loop control law.

[0114] During the application of the algorithm, the reconstructed continuous optimal trajectory is used as the feedforward reference input and output to the port-controlled Hamiltonian fractional passive control module in step S6. At the same time, the feedforward value of the joint driving torque corresponding to the optimal trajectory is output to the joint actuator of the robotic arm as the feedforward control quantity. Simultaneously, the real-time tracking error data returned from step S6 is received to update the closed-loop tracking error constraint of trajectory optimization, triggering online replanning of the trajectory and realizing dynamic optimization of the trajectory.

[0115] S6: Multi-arm-filling cooperative closed-loop control based on port-controlled Hamiltonian fractional-order passive control

[0116] The core function of this step is to address the problems of poor robustness and low tracking accuracy of traditional robotic arm closed-loop control in dynamic, highly disturbed water environments, and the inability to achieve coordinated control between robotic arm motion control and sand suction / filling operation control, which are independent of each other. This step employs a port-controlled Hamiltonian fractional-order passive control algorithm, a nonlinear control method rarely used in the field of underwater multi-arm coordinated control. Compared to traditional control methods, it designs the control law based on the system's energy conservation characteristics, possessing inherent robustness and stability. Furthermore, the introduction of fractional-order integrals and derivatives significantly improves the system's disturbance suppression capability and tracking accuracy. This algorithm, along with the improved parasitic-predator algorithm in step S4 and the fractional-order pseudospectral method in step S5, achieves bidirectional interaction, realizing deep coupling between closed-loop control, task allocation, and trajectory planning, while simultaneously achieving coordinated closed-loop control of multi-arm motion control and sand suction / filling operation control. The multi-arm motion-filling process coordinated closed-loop control flow is as follows: Figure 4 As shown.

[0117] First, define the Hamiltonian energy function. This function is the core of the port-controlled Hamiltonian system, used to describe the total energy of the robotic arm system, including kinetic and potential energy, and its expression is:

[0118] ;

[0119] x is the system's state vector, which is completely consistent with the state variables defined in step S5. It is a 12-row, 1-column vector, with the first 6 elements being the joint angle vector q and the last 6 elements being the joint angular velocity vector. Ensure that the state variable definitions are consistent throughout the entire scheme; Let be the gravitational potential energy function of the system, satisfying G(q) is the gravity term vector defined in step S3, ensuring that the potential energy function perfectly matches the gravity term of the dynamic model.

[0120] The kinematic-dynamic coupling model of step S3 is transformed into the standard form of a port-controlled Hamiltonian system. This form clearly separates the system's energy exchange, dissipation, input, and disturbance, facilitating the design of control laws based on passive control theory. The expression is:

[0121] ;

[0122] ;

[0123] J(x) is the interconnection matrix of the system, which is an antisymmetric matrix used to describe the energy exchange relationships within the system. Its expression is:

[0124] ;

[0125] It is a 6x6 zero matrix. It is a 6x6 identity matrix. The antisymmetric nature of the matrix ensures that energy exchange within the system does not generate or consume energy, which conforms to the law of conservation of energy.

[0126] R(x) is the damping matrix of the system. It is a symmetric positive semi-definite matrix used to describe the energy dissipation characteristics of the system. In this scheme, R(x) is taken as a diagonal matrix, and the diagonal elements are the viscous damping coefficients of each joint. The symmetric positive semi-definite characteristic ensures that the energy dissipation of the system is always non-negative, which is consistent with the actual physical characteristics.

[0127] Let be the partial derivative of the Hamiltonian function with respect to the state vector, and let be a 12-row, 1-column vector. Its expression is:

[0128] ;

[0129] This vector describes the gradient of the system's energy as the state changes and is the core feedback quantity of the port-controlled Hamiltonian control law.

[0130] g(x) is the system's input matrix, a 12-row, 6-column matrix, expressed as:

[0131] ;

[0132] The control input vector is used to describe the influence of the control input on the system state; u is the system's control input vector, a 6-row, 1-column vector, corresponding to the joint driving torque τ of the robotic arm, and is the final output of the control law; d(x) is the system's disturbance vector, a 12-row, 1-column vector, used to describe external disturbances caused by hydrodynamic interference and changes in sand suction and filling load, and its expression is:

[0133] ; The hydrodynamic disturbance torque vector defined in step S3. The disturbance torque vector caused by the change in sand suction and filling load is acquired in real time by the multi-sensor fusion module in step S7; y is the output vector of the system, which is a 6-row, 1-column vector, corresponding to the energy output of the system, and is the core feedback output of passive control.

[0134] This design incorporates a fractional-order passive control law based on passive control theory to ensure the passive characteristics of the closed-loop system, thereby guaranteeing its global asymptotic stability. Furthermore, fractional integrals and derivatives are introduced to improve the system's tracking accuracy and disturbance suppression capability. The expression for the control law is as follows:

[0135] ;

[0136] The feedforward control term is obtained from the joint driving torque feedforward value corresponding to the optimal trajectory output by the fractional pseudospectral method module in step S5. It is used to realize the reference trajectory feedforward tracking of the system, offset the nominal dynamic characteristics of the system, and reduce the burden of feedback control. This is a fractional-order feedback control term used to achieve passive stabilization of the system and eliminate trajectory tracking errors. Its expression is:

[0137] ;

[0138] , , The proportional, integral, and derivative gain matrices are all diagonal positive definite matrices with 6 rows and 6 columns, ensuring the stability of the feedback control. Let x be the state tracking error vector, and let x be the actual state vector of the system and the reference state vector output from step S5. The difference is expressed as: ; For tracking error The first-order Caputo fractional integral is used to eliminate the steady-state tracking error of the system. The order of the fractional integral ranges from 0 to 1. In this scheme, it is set to 0.8. This order can eliminate steady-state error while avoiding the problem of integral saturation. For tracking error The first-order Caputo fractional derivative is used to predict the changing trend of tracking error in advance, thereby improving the dynamic response characteristics of the system.

[0139] This is a robust compensation term used to compensate for hydrodynamic disturbances and load disturbances, improving the system's robustness. Its expression is:

[0140] ;

[0141] is the robust gain coefficient, which is a constant greater than 0; It is a symbolic function; The 2-norm of the disturbance vector is used for compensation to ensure that the system maintains stable tracking performance even in the presence of external disturbances.

[0142] The design incorporates a bidirectional interactive coupling term between the control law and modules S4 and S5, enabling pairwise bidirectional interaction among the three niche algorithms. This is one of the core innovations of this scheme. The interaction with the fractional pseudospectral method module in step S5 feeds back the maximum real-time tracking error to the fractional pseudospectral method module in step S5, updating the closed-loop tracking error constraint for trajectory optimization. The expression is:

[0143] ;

[0144] , where is the error amplification factor, a constant greater than 1, used to ensure that the constraint boundary of trajectory optimization includes the actual tracking error range. The logic of this interaction is as follows: if the actual tracking error of the closed-loop control increases, it indicates that the tracking difficulty of the current trajectory is too high, and the error threshold fed back to the trajectory planning module will increase. The trajectory planning module will automatically adjust the smoothness of the trajectory to reduce the tracking difficulty and improve the tracking accuracy. At the same time, the change in trajectory smoothness output by the trajectory planning module will directly affect the tracking error of the closed-loop control, and the two form a two-way coupled collaborative optimization.

[0145] The interaction with the improved parasitic-predatory algorithm module in step S4 feeds real-time task data back to the module, updating the objective function parameters for task allocation. The expression is:

[0146] ;

[0147] This refers to the actual task execution time; This represents the actual length of the motion path. This represents the actual total drive energy consumption; The actual total hydrodynamic compensation error is calculated from the real-time data collected by this module. The logic of this interaction is as follows: when the deviation between the comprehensive objective function value of the actual operation and the optimal objective function value obtained by the improved parasitic-predatory algorithm exceeds a preset threshold, it indicates that the performance of the current task allocation scheme has deteriorated in the actual operation, triggering the improved parasitic-predatory algorithm module in step S4 to re-optimize the task allocation, realizing the online dynamic update of the task allocation scheme; at the same time, the update of the task allocation scheme will directly affect the results of trajectory planning and closed-loop control, forming a two-way coupled collaborative optimization.

[0148] The parameter tuning and adaptive adjustment process of the control law is as follows: First, initialize the system parameters and set the fractional order. The initial parameters of the Hamiltonian function, the initial value of the damping matrix R(x), and the parameters of the input matrix g(x) are all determined based on the kinematic-dynamic coupling model in step S3, ensuring that the system model is consistent with the actual physical characteristics. The second step is to initialize the control law parameters and set the feedforward control term. The initial value, the scaling gain matrix Integral gain matrix Differential gain matrix Initial value, robust gain coefficient The initial value, error amplification factor The initial values ​​are then determined. The third step is offline parameter tuning. Based on the kinematic-dynamic coupling model from step S3, the Lyapunov stability theorem is used to tune the control law parameters, ensuring the global asymptotic stability of the closed-loop system. The Lyapunov function is chosen as the system's Hamiltonian energy function plus a quadratic function of the tracking error, expressed as:

[0149] ;

[0150] To obtain the fractional integral vector of the tracking error, the time derivative of the Lyapunov function is calculated. Combining this with the equations and control law formulas of the port-controlled Hamiltonian system, the time derivative of the Lyapunov function is ensured to always be less than or equal to 0, satisfying the stability requirements of passive control and thus guaranteeing the global asymptotic stability of the closed-loop system. The fourth step involves online adaptive parameter adjustment based on the real-time tracking error. The gradient descent method is used to adjust the gain matrix online. , , The parameters are adjusted using the following formula:

[0151] ;

[0152] ;

[0153] ;

[0154] , , The learning rate used for parameter adjustment is a constant greater than 0. This adaptive adjustment process can optimize the control law parameters in real time, adapting to changes in system characteristics during operation and improving control performance. The fifth step involves bidirectional interactive parameter updates, updating the feedforward control term in real time based on the changes in the optimal trajectory output by the fractional pseudospectral method module in step S5. To ensure the synchronization of feedforward control and trajectory planning, the load parameters and constraint boundaries of the Hamiltonian function are updated in real time based on the changes in the task allocation scheme output by the improved parasitic-predator algorithm module in step S4, ensuring the matching between the control law and the task allocation scheme. The sixth step is to calibrate the disturbance compensation parameters, using the hydrodynamic disturbance field feature library from step S1 to calibrate the gain coefficient of the robust compensation term. This ensures that the compensation accuracy for hydrodynamic disturbances meets operational requirements and avoids undercompensation or overcompensation.

[0155] During the application of the algorithm, the designed fractional-order passive control law outputs real-time joint drive control commands to the joint actuators of the robotic arm, realizing collaborative trajectory tracking control of multiple arms. Simultaneously, the real-time collected sand suction and filling concentration and flow rate data are compared with the preset values ​​allocated by the improved parasitic-predatory algorithm module in step S4. By adjusting the power of the sand suction pump and the opening of the pipeline valves, closed-loop control of the sand suction and filling parameters is achieved, ensuring that the concentration and flow rate of sand suction and filling remain stable within the preset range. Real-time tracking error, energy consumption data, and disturbance compensation data are fed back to modules S4 and S5, achieving bidirectional coupling closed-loop optimization of the entire system, ensuring the accuracy, efficiency, and stability of multi-arm collaborative operation.

[0156] S7: Real-time acquisition of sand suction and filling parameters and closed-loop feedback of the entire system

[0157] This step serves as the core data link for closed-loop optimization of the entire system, providing real-time operational status data to all upstream steps. This ensures that all calculations in upstream task allocation, trajectory planning, and closed-loop control modules are based on real operational data, while simultaneously achieving deep coupling between multi-arm motion control and sand suction and filling operation control.

[0158] Through a distributed multi-sensor fusion architecture, real-time data from all dimensions of the sand suction and filling operation system is collected synchronously. This includes real-time data on mortar concentration in the sand suction and filling pipeline collected by concentration sensors, real-time data on mortar flow rate in the pipeline collected by flow sensors, real-time pressure data at the inlet and outlet of the pipeline collected by pressure sensors, real-time pose data of the robotic arm end effector collected by attitude sensors, real-time data on the angle and angular velocity of each joint of the robotic arm collected by joint encoders, real-time data on the driving torque of each joint collected by torque sensors, and real-time water flow velocity and direction data of the operating area collected by flow meters. The timestamps of all sensor data are completely synchronized, with a synchronization error of no more than 1ms.

[0159] Based on the hydrodynamic interference field feature library output from step S1, the water flow interference features corresponding to the current working condition are extracted, and the filtering parameters of the adaptive Kalman filter algorithm are constructed. Interference filtering is performed on all collected real-time data to filter out high-frequency interference noise caused by water flow fluctuations, robotic arm vibration, and pipeline pressure pulsation. This results in accurate real-time data on sand suction and filling concentration and flow rate, as well as accurate real-time data on robotic arm movement status, ensuring that all data used by upstream modules are authentic, smooth, and noise-free.

[0160] Based on the filtered real-time data of sand suction and filling concentration and flow rate, the actual load change of the current sand suction and filling operation is calculated. Changes in mortar concentration and flow rate directly lead to changes in the load mass of the end effector. The real-time mortar mass change is calculated using mortar density, pipeline cross-sectional area, and flow rate data, and then converted into the load torque change at the end of the robotic arm. This load change is used to correct the disturbance vector of the controlled Hamiltonian system at the S6 step port, providing a basis for real-time compensation of the kinematic-dynamic coupling model and ensuring the real-time performance and accuracy of the dynamic model.

[0161] All filtered real-time data, including the robotic arm's motion state data, joint driving torque data, sand suction and filling concentration and flow rate data, and water flow velocity data, are fed back in real-time to the kinematic-dynamic coupling model in step S3 to achieve online real-time model updates; to the improved parasitic-predatory algorithm module in step S4 to achieve dynamic optimization of the task allocation scheme; to the fractional pseudospectral method module in step S5 to achieve online trajectory replanning; and to the port-controlled Hamiltonian fractional passive control module in step S6 to achieve real-time adaptive adjustment of the control law, forming a closed-loop feedback link for the entire system. This ensures that the entire multi-arm collaborative control system can adapt to changes in the working environment in real time and always maintain optimal operational performance.

[0162] In some embodiments, the interaction coupling factor of the improved parasitic-predator algorithm is... The value range is 0.01 to 0.2, and its calibration method is as follows: using a preset reference threshold for the trajectory optimization objective function. Based on this, using orthogonal experimentation, five sets of gradient distribution test values ​​were selected within the range of values. These values ​​were then substituted into the algorithm to complete the iterative solution, and the comprehensive operating cost of the task allocation scheme was selected. With trajectory optimization objective function value The experimental value with the smallest overall deviation is taken as... The final calibration value; in a preferred embodiment, Take 0.05.

[0163] In some embodiments, the robust gain coefficient of the port-controlled Hamiltonian fractional-order passive control algorithm The value range is 0.8 to 1.5, and its calibration method is as follows: based on the maximum disturbance vector norm under the maximum water flow velocity condition in the hydrodynamic disturbance field feature library, the calibration is completed by trial and error to ensure that the output torque of the robust compensation term does not exceed 30% of the maximum allowable driving torque of the joint; in the preferred embodiment, Take 1.2.

[0164] In some embodiments, the error amplification factor of the port-controlled Hamiltonian fractional-order passive control algorithm The value range is 1.2 to 2.0, and its calibration method is as follows: it is determined based on the response bandwidth of the robotic arm joint drive system and the steady-state error requirement of trajectory tracking. When the system response bandwidth is low and the steady-state error requirement is high, a larger value is selected. Value; in a preferred embodiment, Take 1.5.

[0165] In some embodiments, the motion coupling constraint between multiple robotic arms is a collision safety distance constraint in the global coordinate system, the specific mathematical expression of which is:

[0166] ;

[0167] in, for Time of the first The set of spatial positions of the links of the robotic arm in the global working coordinate system. for Time of the first The set of spatial positions of the links of the robotic arm in the global working coordinate system. , The preset safe collision distance, ranging from 0.2m to 0.5m, ensures that the minimum spatial distance between any two robotic arms throughout the entire operation cycle is not less than the safe collision distance, thus avoiding collisions during multi-arm collaborative operations.

[0168] In some embodiments, the force coupling constraint between multiple robotic arms is the end-effector force balance constraint during collaborative operation, and its specific mathematical expression is as follows:

[0169] ;

[0170] ;

[0171] in, This represents the total number of robotic arms in the multi-arm system. for Time of the first The force vector of the end effector of the robotic arm in the global working coordinate system. for Time of the first The output torque vector of the end effector of the robotic arm in the global working coordinate system. for The total load force vector during the sand suction and filling operation. for The total load torque vector of the sand suction and filling operation at all times. This constraint ensures that when multiple robotic arms work together to operate the same sand suction and filling end effector, the resultant force at the end of each robotic arm is balanced with the operating load.

[0172] In some embodiments, the core difference between the improved parasitic-predatory algorithm of the present invention and the existing conventional parasitic-predatory algorithm lies in:

[0173] Firstly, existing conventional parasitic-predator algorithms only include individual position updates during the predator and parasitic phases. This invention adds an individual position update step during the symbiotic phase and simultaneously incorporates the trajectory planning results output by the fractional pseudospectral method during the symbiotic phase. Based on the trajectory optimization effect, the individual position update strategy is adjusted to achieve bidirectional collaborative optimization of task allocation and trajectory planning. This solves the problem that existing algorithms can only complete unidirectional task allocation and cannot match the feasibility of trajectory planning.

[0174] Secondly, existing conventional parasitic-predator algorithms only optimize path length and operation time. This invention embeds the joint torque constraints, joint motion constraints, hydrodynamic compensation constraints, and collision safety constraints of the multi-arm kinematic-dynamic coupling model as hard constraints into the objective function and iterative process of the algorithm. At the same time, it incorporates driving energy consumption and hydrodynamic interference compensation errors into the optimization objectives, ensuring that the output task allocation scheme fully matches the dynamic feasible range of the robotic arm. This solves the problems of joint overload, excessive energy consumption, and interference compensation failure that easily occur in the existing algorithm allocation scheme in actual operation.

[0175] Third, the existing conventional parasitic-predator algorithm has an independent closed-loop iteration process. The algorithm iteration process of this invention can synchronously receive real-time comprehensive operation cost data fed back by the closed-loop control module. When the deviation between the actual operation cost and the optimal objective function value exceeds a preset threshold, the algorithm can be triggered to iterate and optimize online, realize the dynamic update of the task allocation scheme, and adapt to the real-time changes in the operation conditions.

[0176] In some embodiments, a water tank test was conducted to verify the technical solution of the present invention. The test used a multi-arm collaborative sand suction and filling operation system composed of four 6-DOF tandem robotic arms. The water flow velocity in the operating area ranged from 0 to 1.5 m / s, and the water flow direction covered the entire circumference from 0° to 360°. The control algorithm of the present invention was compared with the existing conventional control scheme, and the quantitative test data obtained are as follows:

[0177] 1. Tracking accuracy: The algorithm of this invention has a maximum tracking error of ±2.3mm at the end of the robotic arm under all working conditions, which is 81.9% higher than the ±12.7mm of the existing conventional solution.

[0178] 2. Work efficiency: To complete sand suction and filling operations in the same area, the algorithm of this invention has a total operation time of 42 minutes, which is 38.2% higher than the existing conventional solution of 68 minutes.

[0179] 3. Drive energy consumption: To complete the same task, the total drive energy consumption of the algorithm of this invention is 18.7 kWh, which is 36.2% lower than the 29.3 kWh of the existing conventional solution;

[0180] 4. Anti-interference stability: Under the condition of maximum water flow velocity of 1.5m / s, the algorithm of the present invention has no joint overload or trajectory instability during operation. The sand suction and filling concentration fluctuation rate is ±3.2%, which is 72.4% higher than the ±11.6% of the existing conventional solution.

[0181] In some embodiments, the preset thresholds for the above-mentioned tests are set as follows: the maximum allowable value for trajectory tracking error is ±5mm, and the maximum allowable value for sand suction and filling concentration fluctuation is ±5%. The algorithm of the present invention meets the preset operational accuracy and stability requirements under all test conditions.

Claims

1. A multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling, characterized in that, Includes the following steps: S1: Collect hydrodynamic disturbance data under all working conditions in the sand suction and filling water area. After preprocessing the collected hydrodynamic disturbance data under all working conditions, construct a hydrodynamic disturbance field feature library. S2: For each robotic arm in the multi-arm collaborative operation system, construct a single robotic arm kinematic model, and at the same time establish a homogeneous transformation mapping relationship between the base coordinate system of multiple robotic arms and the global operation coordinate system to complete the unified mapping of the multi-arm collaborative operation space. S3: Based on the single-arm kinematics model, homogeneous transformation mapping relationship and hydrodynamic disturbance field feature library, construct a multi-arm kinematic-dynamic coupling model with embedded hydrodynamic disturbance compensation term; S4: Using the kinematic-dynamic coupling model of the multi-arm as a hard constraint, the improved parasitic-predatory algorithm is used to complete the task-dynamic coupling allocation of the multi-arm sand suction and filling operation, and the optimal task allocation scheme of the multi-arm is obtained. S5: Based on the optimal task allocation scheme of the multi-arm and the kinematic-dynamic coupling model of the multi-arm, the fractional pseudospectral method is used to complete the trajectory-task bidirectional coupling planning, generate the optimal trajectory of the multi-arm operation, and feed the trajectory planning results back to S4 to optimize the task allocation scheme. S6: Based on the optimal trajectory of multi-arm operation and the kinematic-dynamic coupling model of multi-arm, a port-controlled Hamiltonian fractional-order passive control algorithm is adopted to complete the coordinated closed-loop control of multi-arm motion and sand suction and filling process, and output the mechanical arm joint drive control command.

2. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, It also includes S7: synchronously collecting real-time data of sand suction and filling operations through a multi-sensor fusion architecture, filtering the collected real-time data, and feeding the processed real-time data back to the multi-arm kinematic-dynamic coupling model of S3, the improved parasitic-predatory algorithm of S4, the fractional pseudospectral method of S5, and the port-controlled Hamiltonian fractional passive control algorithm of S6 to complete the closed-loop dynamic optimization of the entire system.

3. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S2, the DH parameter method is used to construct the kinematic model of a single robotic arm. The homogeneous transformation mapping relationship is constructed based on the installation layout of the multi-arm collaborative operation system. At the same time, the effective working range of the single robotic arm kinematic model is limited by combining the coverage of the hydrodynamic interference field feature library.

4. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S3, the Newton-Euler recursive method is used to construct a multi-arm kinematic-dynamic coupling model. The joint motion parameters of the manipulator output by the single manipulator kinematic model are used as the endogenous input variables of the dynamic equation. The embedded hydrodynamic disturbance compensation term is updated in real time based on the hydrodynamic disturbance field feature library to match the current working condition. At the same time, motion coupling constraints and force coupling constraints between multiple manipulators are added to the multi-arm kinematic-dynamic coupling model.

5. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S4, the improved parasitic-predatory algorithm aims to minimize the overall operating cost of the multi-arm system. The overall operating cost includes the total execution time, total motion path length, total driving energy consumption, and total hydrodynamic interference compensation error. The hard constraints set by the improved parasitic-predatory algorithm include joint torque constraints, joint motion constraints, hydrodynamic compensation constraints, and collision safety constraints. All constraints are derived from the kinematic-dynamic coupling model of the multi-arm system.

6. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S4, the iterative process of the improved parasitic-predator algorithm sequentially executes the individual position update of the predator stage, the parasitic stage, and the symbiotic stage. In the symbiotic stage, the trajectory planning results output by the fractional pseudospectral method are simultaneously incorporated, and the individual position update strategy is adjusted based on the trajectory optimization effect, thereby simultaneously realizing the bidirectional collaborative optimization of task allocation and trajectory planning.

7. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S5, the fractional pseudospectral method is constructed based on the Caputo fractional differential operator. The optimization objectives are to minimize trajectory control energy consumption, fractional oscillations, and task allocation deviation. The trajectory optimization constraints include fractional dynamic constraints obtained from the transformation of the multi-arm kinematic-dynamic coupling model.

8. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S6, the port-controlled Hamiltonian fractional passive control algorithm designs the control law based on the Hamiltonian energy function of the robotic arm system. The control law includes a feedforward control term, a fractional feedback control term, and a robust compensation term. The feedforward control term is derived from the optimal trajectory of the multi-arm operation, and the robust compensation term is dynamically adjusted based on the hydrodynamic disturbance field feature library and the real-time acquired load disturbance data.

9. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 1, characterized in that, In S6, based on the reference state corresponding to the optimal trajectory of the multi-arm operation, the maximum real-time tracking error in the entire operation cycle is calculated. This maximum real-time tracking error is fed back to the fractional pseudospectral method in S5 to dynamically adjust the smoothness constraint of trajectory optimization. Based on real-time data, the comprehensive cost data of real-time operations is calculated according to the objective function of the improved parasitic-predatory algorithm. This comprehensive cost data is then fed back to S4 to trigger online dynamic optimization of the task allocation scheme, thereby achieving pairwise bidirectional collaborative optimization of the improved parasitic-predatory algorithm, the fractional pseudospectral method, and the port-controlled Hamiltonian fractional passive control algorithm.

10. The multi-arm cooperative control algorithm for sand suction and filling based on kinematic and dynamic coupling as described in claim 2, characterized in that, In S7, the real-time data collected includes sand suction and filling process parameters, robotic arm motion state parameters, and hydrological parameters of the operating water area. Based on the collected sand suction and filling process parameters, the real-time load change is calculated and the disturbance term of the multi-arm kinematic-dynamic coupling model constructed in S3 is corrected. All data after filtering are fed back to the corresponding algorithm module in real time, realizing online dynamic optimization and adaptive adaptation of the entire system under all working conditions.

Images