A method, device and storage medium for underwater dual-arm synchronous predictive control based on cross-coupling strategy

Abstract

The present invention belongs to the technical field of underwater manipulator control, and specifically relates to a method, device and storage medium for underwater dual-arm synchronous predictive control based on a cross-coupling strategy. Step 1: Establish a coupled dynamics model of an underwater dual-arm system based on the Lagrangian energy method; Step 2: Based on the coupled dynamics model, cross-coupling strategy and model predictive control of step 1, design a synchronous predictive controller for the underwater dual-arm system; Step 3: Based on the synchronous predictive controller of step 2, design a disturbance observer to estimate the disturbance and compensate it to the system control input, thereby realizing synchronous predictive control of the underwater dual-arm system and enhancing the robustness of the underwater dual-arm synchronous control system to external interference. The present invention is used to solve the problem that the way of processing constraints in the prior art is not intuitive enough, has poor applicability, and does not consider the impact of underwater environmental uncertainty on the synchronous control of underwater dual arms.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater manipulator control, and in particular relates to an underwater dual-arm synchronous predictive control method, device and storage medium based on a cross-coupling strategy. Background Art

[0002] As an important part of the underwater robot-robotic arm system, the research and application fields of underwater robotic arms are wide-ranging, covering marine science, resource exploration, marine environment monitoring, submarine archaeology, marine energy development and other fields, and also involving mechanical engineering, electronic engineering, control engineering, computer science and other disciplines.

[0003] Unlike the terrestrial environment, the uncertainty of the underwater environment brings many challenges to the research of underwater manipulators. The uncertainty and complexity of the underwater environment and the influence of hydrodynamics make it a difficult task to achieve the autonomy and intelligence of underwater manipulators. When performing complex operations such as submarine pipeline maintenance, underwater archaeology, and deep-sea resource development, the traditional underwater single manipulator has the disadvantages of limited grasping range and insufficient operational flexibility. By introducing the underwater dual-arm system synchronization control technology, the robot can complete underwater tasks more flexibly and efficiently in the underwater environment. Through collaborative operation, the two arms coordinate with each other to adapt to the complex and changeable underwater environment, and improve the success rate and efficiency of operation. The existing technology solves the problem of limited joint space in the synchronization control of underwater dual arms by constructing a time-varying synchronization planning model with constraints. This method introduces constraints in the time-varying nonlinear model, so that the underwater dual-arm manipulator can still accurately complete specific synchronization planning tasks when the joint motion space is limited. However, the way this method handles constraints is not intuitive enough, the applicability is poor, and the influence of underwater environmental uncertainty on the synchronization control of underwater dual arms is not considered.

[0004] The focus of the present invention is to design an underwater dual-arm synchronous control method. Based on the cross-coupling strategy and the principle of model predictive control, a synchronous predictive controller of the underwater dual-arm system is designed to achieve simultaneous control of the robotic arm joint tracking error and the synchronization error. On this basis, a disturbance observer is designed to observe the uncertain disturbances of the underwater environment and the uncertainties of the model parameters and compensate them to the system control input, thereby enhancing the robustness of the underwater dual-arm synchronous control system to external interference. Summary of the invention

[0005] The present invention provides an underwater dual-arm synchronous predictive control method, device and storage medium based on a cross-coupling strategy, which are used to solve the problems that the constraint processing method in the prior art is not intuitive enough, has poor applicability, and does not consider the impact of underwater environmental uncertainty on underwater dual-arm synchronous control.

[0006] The present invention is achieved through the following technical solutions:

[0007] A method for underwater dual-arm synchronous predictive control based on a cross-coupling strategy, the method comprising the following steps:

[0008] Step 1: Establish the coupled dynamic model of the underwater dual-arm system based on the Lagrangian energy method;

[0009] Step 2: Based on the coupled dynamics model, cross-coupling strategy and model predictive control in step 1, design a synchronous predictive controller for the underwater dual-arm system;

[0010] Step 3: Based on the synchronous predictive controller of step 2, a disturbance observer is designed to estimate the disturbance and compensate it to the system control input to realize the synchronous predictive control of the underwater dual-arm system.

[0011] Furthermore, the step 1 is specifically as follows: according to the coupling effect in the underwater dual-arm synchronous control process, the uncertain disturbance of the underwater environment and the uncertainty of the model are ignored, and the coupling dynamic model of the underwater dual-arm system is obtained as shown in the following formula:

[0012]

[0013] in, , , , are the angle variables of the two joints of the left robotic arm, are the angle variables of the two joints of the right robotic arm, is the angular velocity variable of the two joints of the left robotic arm, is the angular velocity variable of the two joints of the right robotic arm, is the angular acceleration variable of the two joints of the left robotic arm, is the angular acceleration variable of the two joints of the right robotic arm, represents the generalized inertia matrix of the underwater dual-arm system, represents the generalized centripetal force and Coriolis force matrices of the system, represents the generalized hydrodynamic matrix of the system, represents the generalized equivalent gravity matrix of the system, represents the transpose of the generalized Jacobian matrix of the system, is the generalized force on the target object; It is the driving force matrix of the system robot arm joint motor.

[0014] Furthermore, the step 2 specifically defines the robot arm joint angle tracking error and joint angular velocity tracking error As shown below:

[0015]

[0016] in, is the expected angle variable of each joint of the underwater arms, is the expected angular velocity variable of each joint of the underwater arms;

[0017] Define the robot arm joint angle synchronization error Synchronization error with joint angular velocity As shown below:

[0018]

[0019]

[0020] Next, define the coupling error As shown below:

[0021]

[0022] in, is the tracking error, , is the synchronization error, , is the transformation matrix, is the cross coupling coefficient.

[0023] Furthermore, the system dynamics model in step 1 is linearized and discretized to obtain the discrete time linearized state equation of the underwater dual-arm system as shown below:

[0024]

[0025] in, is the state variable of the robot joint, ; For the robot joint control input, ; , , are all transformation matrices determined by the expected trajectory of the joint state and remain constant during the sampling period; Sampling time, For the system The predicted state of the robot arm joints at this moment, For the system The actual state of the robot arm joint at any moment can be measured by the joint sensor. For the system The control input of the robot arm joint at all times, For the system The output of the robot arm joint at that moment.

[0026] Furthermore, the state predictor is designed according to the discrete time linearized state equation of the underwater dual-arm system as shown in the following formula:

[0027]

[0028] in, is the predicted state sequence of the system in the prediction time domain, ; For system-based The actual state prediction at the moment System status at all times, To predict the control input sequence of the system in the time domain, ; For the system The assumed control input at time; , , Is , The determined transformation matrix, For the prediction time domain.

[0029] Furthermore, the quadratic programming problem is constructed as follows:

[0030]

[0031] in, To optimize the objective function; , They are the joint state error weight matrix and the control input increment weight matrix respectively; is the identity matrix; For the control input increment sequence of the system in the prediction time domain, ; Representation System The assumed control input increment at time, , For system only Time input The column vector formed by is the constant transformation matrix, To predict the tracking error sequence of the system in the time domain, ;

[0032]

[0033] in, is the predicted state sequence of the system in the prediction time domain, To predict the expected state sequence of the system in the time domain, ; , is the joint state limit value, , Enter the limit value for the control, , Enter incremental limits for control; , , Is , The determined transformation matrix;

[0034] By optimizing the objective function The control input increment of the system in the prediction horizon Find the partial derivative equal to zero and solve the optimal control input increment sequence As shown below:

[0035]

[0036]

[0037] in, is the transformation matrix, , They are the joint state error weight matrix and the control input increment weight matrix respectively;

[0038] According to the model predictive control principle, the optimal control input increment vector The first item As a system Control input at all times.

[0039] Furthermore, the system dynamics model obtained in step 1 in step 3 introduces a disturbance term as shown in the following formula:

[0040]

[0041] in, is the total interference, , , and denote the known parts of the corresponding parameter matrices, , , , and They all represent the unknown part of the corresponding parameter matrix.

[0042] definition , , the system dynamics model can be transformed into the following formula:

[0043]

[0044] in, It is the external force on the end effector of the robot arm;

[0045] The disturbance observer is constructed as follows:

[0046]

[0047] in, , They are , The estimated value of .

[0048] Furthermore, the state variable of the observation error is defined as , the disturbance observer is transformed into the following formula:

[0049]

[0050] in, , , , and are all transformation matrices determined by system parameters; , select the appropriate , so that , in order to achieve the total disturbance Observation.

[0051] A computer device comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the above method is implemented.

[0052] A computer-readable storage medium stores a computer program, and the computer program implements the above method when executed by a processor.

[0053] The beneficial effects of the present invention are:

[0054] The present invention designs a synchronous predictive controller for an underwater dual-arm system based on the principle of cross-coupling strategy and model predictive control; the controller can predict the state of the system in a certain time domain in the future according to the system state and system parameters measured by the joint sensor at a certain moment, construct an optimization objective function including the system tracking error and synchronization error and the system control input, solve the optimization objective function to obtain the optimal control input of the system at this moment, and realize the underwater dual-arm synchronous predictive control through a rolling optimization strategy. On the basis of the synchronous predictive controller, a disturbance observer is designed, and the difference between the actual model and the nominal model output of the control system caused by the uncertain disturbance is regarded as the total disturbance of the control system. The observed equal disturbance compensation is introduced into the system control input to eliminate the influence of the disturbance and enhance the robustness of the underwater dual-arm synchronous control system to external interference.

[0055] The method of the present invention gives full play to the ability of model predictive control to explicitly handle constraints, making it more convenient and intuitive to handle system constraints during the synchronous control of the underwater dual-arm system. The method designs a disturbance observer for external uncertain disturbances and internal parameter uncertainties, significantly enhancing the robustness and stability of the system and improving the success rate and efficiency of the underwater dual-arm system in performing underwater tasks. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] Figure 1 The present invention is a flow chart of the method.

[0057] Figure 2 This is a block diagram of the synchronous predictive control system of the underwater dual-arm system of the present invention.

[0058] Figure 3a-3f In order to ignore the uncertain disturbance of the underwater environment and the uncertainty of the model, the simulation results of using the synchronous predictive controller to control the tracking error and synchronization error of the system state are shown in the figure. Figure 3a Tracking curves for the shoulder joints of both arms. Figure 3b is the elbow joint trajectory tracking curve, Figure 3c is the shoulder joint tracking error, Figure 3d is the tracking error of the elbow joints of both arms, Figure 3e is the shoulder joint synchronization error, Figure 3f is the elbow joint synchronization error.

[0059] Figure 4a-4d The simulation results of the present invention are that the tracking error and synchronization error of the system state are controlled by using the disturbance observer considering the uncertain disturbance of the underwater environment and the uncertainty of the model. Figure 4a is the simulation result of shoulder joint tracking error, Figure 4b is the simulation result of elbow joint tracking error, Figure 4c is the simulation result of shoulder joint synchronization error, Figure 4d This is the simulation result of the elbow joint synchronization error. DETAILED DESCRIPTION

[0060] In the following description, specific details such as specific system structures, technologies, etc. are provided for the purpose of illustration rather than limitation, so as to provide a thorough understanding of the embodiments of the present application. However, it should be clear to those skilled in the art that the present application may also be implemented in other embodiments without these specific details. In other cases, detailed descriptions of well-known systems, devices, circuits, and methods are omitted to prevent unnecessary details from obstructing the description of the present application.

[0061] It should be understood that when used in this specification and the appended claims, the term "comprising" indicates the presence of described features, integers, steps, operations, elements and / or components, but does not exclude the presence or addition of one or more other features, integers, steps, operations, elements, components and / or groups thereof.

[0062] It should also be understood that the terms used in this application specification are only for the purpose of describing specific embodiments and are not intended to limit the application. As used in this application specification and the appended claims, the singular forms "a", "an" and "the" are intended to include plural forms unless the context clearly indicates otherwise.

[0063] The following is a clear and complete description of the technical solutions in the embodiments of the present application in conjunction with the drawings of the present application specification. Obviously, the described embodiments are only part of the embodiments of the present application, not all of the embodiments. Based on the embodiments in the present application, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of this application.

[0064] In the following description, many specific details are set forth to facilitate a full understanding of the present application, but the present application may also be implemented in other ways different from those described herein, and those skilled in the art may make similar generalizations without violating the connotation of the present application. Therefore, the present application is not limited to the specific embodiments disclosed below.

[0065] Implementation Method 1

[0066] This embodiment provides an underwater dual-arm synchronous predictive control method based on a cross-coupling strategy. The system includes a base fixed underwater and two underwater manipulators mounted on the base. Each underwater manipulator has two joint degrees of freedom. Based on the cross-coupling strategy and the principle of model predictive control, an underwater dual-arm system synchronous predictive controller is designed to achieve simultaneous control of the manipulator joint tracking error and synchronization error. On this basis, a disturbance observer is designed to observe the uncertain disturbance of the underwater environment and the uncertainty of the model parameters and compensate them to the system control input, thereby enhancing the robustness of the underwater dual-arm synchronous control system to external interference. This method introduces the system coupling error through the cross-coupling strategy, and controls the system tracking error and synchronization error at the same time, thereby achieving synchronous control of the underwater dual manipulators.

[0067] The method comprises the following steps,

[0068] Step 1: Establish the coupled dynamic model of the underwater dual-arm system based on the Lagrangian energy method;

[0069] Step 2: Based on the coupled dynamics model, cross-coupling strategy and model predictive control in step 1, a synchronous predictive controller for the underwater dual-arm system is designed to achieve joint trajectory tracking control and joint synchronization control of the underwater manipulator;

[0070] Step 3: Based on the synchronous predictive controller of step 2, a disturbance observer is designed to estimate the disturbance and compensate it to the system control input in view of the uncertain disturbance of the underwater environment and the uncertainty of the system model, so as to realize the synchronous predictive control of the underwater dual-arm system.

[0071] Furthermore, the step 1 is specifically as follows: the coupling dynamics model considers the dynamic influence of the hydrodynamic force and the coupling effect of the underwater dual-arm on the underwater dual-arm system; according to the coupling effect in the underwater dual-arm synchronous control process, the underwater environmental uncertain disturbance and the model uncertainty are ignored, and the coupling dynamics model of the underwater dual-arm system is obtained as shown in the following formula:

[0072]

[0073] in, , , , are the angle variables of the two joints of the left robotic arm, are the angle variables of the two joints of the right robotic arm, is the angular velocity variable of the two joints of the left robotic arm, is the angular velocity variable of the two joints of the right robotic arm, is the angular acceleration variable of the two joints of the left robotic arm, is the angular acceleration variable of the two joints of the right robotic arm, represents the generalized inertia matrix of the underwater dual-arm system, represents the generalized centripetal force and Coriolis force matrices of the system, represents the generalized hydrodynamic matrix of the system, represents the generalized equivalent gravity matrix of the system, represents the transpose of the generalized Jacobian matrix of the system, is the generalized force on the target object; It is the driving force matrix of the system robot arm joint motor.

[0074] Furthermore, the step 2 specifically defines the robot arm joint angle tracking error and joint angular velocity tracking error As shown below:

[0075]

[0076] in, is the expected angle variable of each joint of the underwater arms, is the expected angular velocity variable of each joint of the underwater arms;

[0077] Define the robot arm joint angle synchronization error Synchronization error with joint angular velocity As shown below:

[0078]

[0079]

[0080] Next, define the coupling error As shown below:

[0081]

[0082] in, is the tracking error, , is the synchronization error, , is the transformation matrix, is the cross coupling coefficient.

[0083] Furthermore, the system dynamics model in step 1 is linearized and discretized to obtain the discrete time linearized state equation of the underwater dual-arm system as shown below:

[0084]

[0085] in, is the state variable of the robot joint, ; For the robot joint control input, ; , , are all transformation matrices determined by the expected trajectory of the joint state and remain constant during the sampling period; Sampling time, For the system The predicted state of the robot arm joints at this moment, For the system The actual state of the robot arm joint at any moment can be measured by the joint sensor. For the system The control input of the robot arm joint at all times, For the system The output of the robot arm joint at that moment.

[0086] Furthermore, the state predictor is designed according to the discrete time linearized state equation of the underwater dual-arm system as shown in the following formula:

[0087]

[0088] in, is the predicted state sequence of the system in the prediction time domain, ; For system-based The actual state prediction at the moment System status at all times, To predict the control input sequence of the system in the time domain, ; For the system The assumed control input at time; , , Is , The determined transformation matrix, For the prediction time domain.

[0089] Furthermore, the quadratic programming problem is constructed as follows:

[0090]

[0091] in, To optimize the objective function; , They are the joint state error weight matrix and the control input increment weight matrix respectively; is the identity matrix; For the control input increment sequence of the system in the prediction time domain, ; Representation System The assumed control input increment at time, , For system only Time input The column vector formed by is the constant transformation matrix, To predict the tracking error sequence of the system in the time domain, ;

[0092]

[0093] in, is the predicted state sequence of the system in the prediction time domain, To predict the expected state sequence of the system in the time domain, ; , is the joint state limit value, , Enter the limit value for the control, , Enter incremental limits for control; , , Is , The determined transformation matrix;

[0094] By optimizing the objective function Incremental control input Find the partial derivative equal to zero and solve the optimal control input increment sequence As shown below:

[0095]

[0096]

[0097] in, is the transformation matrix, , They are the joint state error weight matrix and the control input increment weight matrix respectively;

[0098] According to the model predictive control principle, the optimal control input increment vector The first item As a system Control input at all times.

[0099] Furthermore, the system dynamics model obtained in step 1 in step 3 introduces a disturbance term as shown in the following formula:

[0100]

[0101] in, is the total interference, , , and denote the known parts of the corresponding parameter matrices, , , , and Both represent the unknown part of the corresponding parameter matrix.

[0102] definition , , the system dynamics model can be transformed into the following formula:

[0103]

[0104] in, It is the external force on the end effector of the robot arm;

[0105] The disturbance observer is constructed as follows:

[0106]

[0107] in, , They are , The estimated value of .

[0108] Furthermore, the state variable of the observation error is defined as , the disturbance observer is transformed into the following formula:

[0109]

[0110] in, , , , and are all transformation matrices determined by system parameters; , select the appropriate , so that , in order to achieve the total disturbance Observation.

[0111] The following is an explanation with the help of the figure:

[0112] Combination Figure 1 The synchronous predictive control process of the underwater dual-arm system is as follows: first, the coupled dynamic model of the underwater dual-arm system is established based on the Lagrangian energy method; then, the underwater dual-arm synchronous predictive controller is designed based on the cross-coupling strategy and the model predictive control principle to realize the synchronous tracking control of the state variables of each joint of the underwater dual-arm; then, according to the uncertain disturbances of the underwater environment and the uncertain terms of the system model, a disturbance observer is designed to estimate the uncertain disturbances and compensate them to the system control input, thereby enhancing the robustness of the underwater dual-arm synchronous control system to external interference.

[0113] Combination Figure 2 The expected state of the system and the actual state of the system measured by the sensor system are used as the input of the synchronous predictive controller. The synchronous predictive controller obtains the control input of the system at this sampling moment by solving the optimization objective function to control the state change of the underwater dual-arm system. Considering the uncertain disturbance of the underwater environment and the uncertainty of the system parameter model, a disturbance observer is designed to observe the equivalent total disturbance, and it is used as the feedback input of the synchronous predictive controller, which effectively improves the robustness of the underwater dual-arm system to external interference.

[0114] Combination Figure 3a-3fIn the relevant simulation, in order to verify the effectiveness of the synchronous predictive controller, the expected trajectory of the system state is input in the simulation, the control input of the system is calculated by the synchronous predictive controller, and it is applied to the system model to obtain the actual trajectory of the system state. The actual trajectory of the system state is consistent with the expected trajectory, and the tracking error and synchronization error are kept within the allowable range. Therefore, this method effectively realizes the synchronous control of the underwater dual-arm system.

[0115] Combination Figure 4a-4d In the relevant simulation, in order to verify the effectiveness of the disturbance observer, random disturbances are introduced into the control input to simulate the uncertain disturbances of the underwater environment. The disturbance observer takes the system control input and the actual system output as input, estimates the equivalent disturbance term of the system, and feeds it back to the synchronous predictive controller, which suppresses the influence of the uncertain disturbance to a certain extent, and obtains the actual trajectory of the system state in this case. The tracking error and synchronization error are kept within the allowable range. Therefore, this method significantly enhances the robustness of the underwater dual-arm synchronous control system to external disturbances.

[0116] Implementation Method 2

[0117] This embodiment provides a system for synchronous predictive control of an underwater dual-arm system, the system uses the underwater dual-arm synchronous predictive control method based on a cross-coupling strategy described in the first embodiment, the system includes:

[0118] Coupled dynamics model building module: Establish the coupled dynamics model of the underwater double-arm system based on the Lagrangian energy method;

[0119] Synchronous predictive controller design module: Based on the coupled dynamics model, cross-coupling strategy and model predictive control of the coupled dynamics model building module, the synchronous predictive controller of the underwater dual-arm system is designed;

[0120] Control input compensation module: Based on the synchronous predictive controller of the synchronous predictive controller design module, a disturbance observer is designed to estimate the disturbance and compensate it to the system control input to realize the synchronous predictive control of the underwater dual-arm system.

[0121] Implementation Method 3

[0122] An embodiment of the present invention provides an electronic device, which includes a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the memory is used to store software programs and modules, and the processor executes various functional applications and data processing by running the software programs and modules stored in the memory. The memory and the processor are connected via a bus. Specifically, the processor implements any step in the first embodiment above by running the computer program stored in the memory.

[0123] It should be understood that in the embodiments of the present invention, the processor may be a central processing unit (CPU), and the processor may also be other general-purpose processors, digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. A general-purpose processor may be a microprocessor or the processor may also be any conventional processor, etc.

[0124] The memory may include a read-only memory, a flash memory, and a random access memory, and provides instructions and data to the processor. A part or all of the memory may also include a nonvolatile random access memory.

[0125] It can be seen from the above that the electronic device provided by the embodiment of the present invention can implement the underwater dual-arm synchronous predictive control method as described in embodiment one by running a computer program. By designing an underwater dual-arm synchronous predictive controller based on cross-coupling and model predictive control, and designing a disturbance observer for underwater uncertainties, the problem of underwater dual-arm system synchronous control is effectively solved while considering system constraints and uncertain disturbances in the underwater environment. The difference between the actual model and the nominal model output of the control system caused by the uncertain disturbance is regarded as the total disturbance of the control system. The observed equal disturbance compensation is introduced into the system control input to eliminate the influence of the disturbance and enhance the robustness of the underwater dual-arm synchronous control system to external interference.

[0126] It should be understood that if the above-mentioned integrated module / unit is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the present invention implements all or part of the processes in the above-mentioned implementation method, and can also be completed by instructing the relevant hardware through a computer program. The above-mentioned computer program can be stored in a computer-readable storage medium. When the computer program is executed by the processor, the steps of the above-mentioned method implementation can be implemented. Among them, the above-mentioned computer program includes computer program code, and the above-mentioned computer program code can be in source code form, object code form, executable file or some intermediate form. The above-mentioned computer-readable medium may include: any entity or device capable of carrying the above-mentioned computer program code, recording medium, U disk, mobile hard disk, disk, optical disk, computer memory, read-only memory (ROM), random access memory (RAM), electric carrier signal, telecommunication signal and software distribution medium. It should be noted that the content contained in the above-mentioned computer-readable storage medium can be appropriately increased or decreased according to the requirements of legislation and patent practice in the jurisdiction.

[0127] The above description of the disclosed embodiments enables one skilled in the art to implement or use the present invention. Various modifications to these embodiments will be apparent to one skilled in the art, and the general principles defined herein may be implemented in other embodiments without departing from the spirit or scope of the present invention. Therefore, the present invention will not be limited to the embodiments shown herein, but rather to the widest range consistent with the principles and novel features disclosed herein.

[0128] Those skilled in the art can clearly understand that for the convenience and simplicity of description, only the division of the above-mentioned functional units and modules is used as an example for illustration. In actual applications, the above-mentioned function allocation can be completed by different functional units and modules as needed, that is, the internal structure of the above-mentioned device can be divided into different functional units or modules to complete all or part of the functions described above. The functional units and modules in the implementation method can be integrated into a processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit. The above-mentioned integrated unit can be implemented in the form of hardware or in the form of software functional units. In addition, the specific names of the functional units and modules are only for the convenience of distinguishing each other, and are not used to limit the scope of protection of the present invention. The specific working process of the units and modules in the above-mentioned system can refer to the corresponding process in the aforementioned method implementation method, which will not be repeated here.

[0129] It should be noted that the methods and detailed examples provided in the above embodiments can be combined with the devices and equipment provided in the embodiments, and references can be made to each other, and no further details will be given.

[0130] Those skilled in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the present invention.

[0131] In the embodiments provided by the present invention, it should be understood that the disclosed device / terminal equipment and method can be implemented in other ways. For example, the device / equipment implementation described above is only illustrative, for example, the division of the above modules or units is only a logical function division, and in actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed.

[0132] The above-mentioned embodiments are only used to illustrate the technical solutions of the present invention, rather than to limit them. Although the present invention has been described in detail with reference to the above-mentioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the above-mentioned embodiments, or replace some of the technical features therein by equivalents. These modifications or replacements do not deviate the essence of the corresponding technical solutions from the spirit and scope of the technical solutions of the various embodiments of the present invention, and should all be included in the protection scope of the present invention.

Claims

1. An underwater dual-arm synchronous predictive control method based on a cross-coupling strategy, characterized in that: The method comprises the following steps, Step 1: Establish the coupled dynamic model of the underwater dual-arm system based on the Lagrangian energy method; Step 2: Based on the coupled dynamics model, cross-coupling strategy and model predictive control in step 1, design a synchronous predictive controller for the underwater dual-arm system; Step 3: Based on the synchronous predictive controller of step 2, a disturbance observer is designed to estimate the disturbance and compensate it to the system control input to realize the synchronous predictive control of the underwater dual-arm system; The step 2 specifically defines the robot arm joint angle tracking error. and joint angular velocity tracking error As shown below: in, is the expected angle variable of each joint of the underwater arms, is the expected angular velocity variable of each joint of the underwater arms; Define the robot arm joint angle synchronization error Synchronization error with joint angular velocity As shown below: Next, define the coupling error As shown below: Where e is the tracking error, e = [e1 e2] T , ε is the synchronization error, ε=[ε1 ε2] T , is the conversion matrix, λ is the cross-coupling coefficient; The system dynamics model in step 1 is linearized and discretized to obtain the discrete time linearized state equation of the underwater dual-arm system as shown below: Among them, x is the state variable of the robot joint, u is the robot joint control input, u = τ; are all transformation matrices determined by the expected trajectory of the joint state and remain constant during the sampling period; at sampling time t = k, x(k+1) is the predicted state of the robot joint of the system at time k+1, x(k) is the actual state of the robot joint of the system at time k, which can be measured by the joint sensor, u(k) is the control input of the robot joint of the system at time k, and y(k+1) is the output of the robot joint of the system at time k+1; The state predictor is designed based on the discrete time linearized state equation of the underwater dual-arm system as shown below: X k =Ψx(k)+ΘU k +ΓG C Among them, X k is the predicted state sequence of the system in the prediction time domain, X k =[x(k+1|k) x(k+2|k) … x(k+N|k)] T ; x(k+i|k) is the system state at time k+i predicted based on the actual state of the system at time k, U k To predict the control input sequence of the system in the time domain, U k =[u(k) u(k+1) … u(k+N-1)] T ; u(k+i-1) is the assumed control input of the system at time k+i-1; By A c , B c The determined transformation matrix, N is the prediction time domain; The system dynamics model obtained in step 1 in step 3 introduces the disturbance term as shown below: Among them, τ d is the total interference, and denote the known parts of the corresponding parameter matrices, and Both represent the unknown part of the corresponding parameter matrix; definition f2=τ d , the system dynamics model can be transformed into the following formula: Among them, F end It is the external force on the end effector of the robot arm; The disturbance observer is constructed as follows: Among them, z1 and z2 are the estimated values ​​of f1 and f2 respectively, and e ob =f1-z1; Define the state variable for the observation error The disturbance observer is transformed into the following formula: in, and are all transformation matrices determined by system parameters; L ob =[l0 l1] T , select the appropriate L according to the system parameters ob , so that To achieve the total disturbance τ d Observation.

2. According to claim 1, a method for underwater dual-arm synchronous predictive control based on a cross-coupling strategy is characterized in that: Specifically, the step 1 is to obtain the coupled dynamics model of the underwater dual-arm system according to the coupling effect in the underwater dual-arm synchronous control process, ignoring the underwater environmental uncertainty disturbance and the model uncertainty term, as shown in the following formula: Where θ=[θ L θ R ] T , θ L =[θ 1L θ 2L ] T is the angle variable of the two joints of the left robot arm, θ R =[θ 1R θ 2R ] T are the angle variables of the two joints of the right robotic arm, is the angular velocity variable of the two joints of the left robotic arm, is the angular velocity variable of the two joints of the right robotic arm, is the angular acceleration variable of the two joints of the left robotic arm, is the angular acceleration variable of the two joints of the right robotic arm, represents the generalized inertia matrix of the underwater dual-arm system, represents the generalized centripetal and Coriolis force matrices of the system, represents the generalized hydrodynamic matrix of the system, represents the generalized equivalent gravity matrix of the system, represents the transpose of the generalized Jacobian matrix of the system, is the generalized force on the target object; It is the driving force matrix of the system robot arm joint motor.

3. According to claim 1, the underwater dual-arm synchronous predictive control method based on the cross-coupling strategy is characterized in that: The quadratic programming problem is constructed as follows: Among them, J(ΔU k ) is the optimization objective function; are the joint state error weight matrix and the control input increment weight matrix respectively; I is the unit matrix; ΔU k is the control input increment sequence of the system in the prediction time domain, ΔU k =[Δu(k) … Δu(k+N-1)] T ; Δu(k)+i-1) represents the assumed control input increment of the system at time k+i-1, U k =U k-1 +ΦΔU k , U k-1 is a column vector consisting only of the system input u(k-1) at time k-1, Φ is a constant transformation matrix, E k To predict the tracking error sequence of the system in the time domain, E k =[e(k+1) e(k+2) … e(k+N)] T ; Among them, X k is the predicted state sequence of the system in the prediction time domain, To predict the expected state sequence of the system in the time domain, x min 、x max is the joint state limit value, u min 、u max is the control input limit, Δu min , Δu max Enter incremental limits for control; By A c , B c The determined transformation matrix; By optimizing the objective function J(ΔU k ) for the control input increment ΔU k Find the partial derivative equal to zero and solve the optimal control input increment sequence ΔU k * As shown below: in, is the transformation matrix, They are the joint state error weight matrix and the control input increment weight matrix respectively; According to the model predictive control principle, the optimal control input increment vector ΔU is selected k * The first term Δu * (k) serves as the control input of the system at time k.

4. A computer device, characterized in that: The method comprises a memory, a processor and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the method according to any one of claims 1 to 3 is implemented.

5. A computer-readable storage medium, characterized in that: The computer-readable storage medium stores a computer program, and when the computer program is executed by a processor, the method according to any one of claims 1 to 3 is implemented.

Images