Joint space limited underwater dual-arm manipulator synchronous control method and device

Abstract

The application relates to a kind of joint space limited synchronous control method and equipment of underwater double-arm manipulator, method includes the following steps: using D-H parameter method respectively to two underwater manipulators modeling, based on the physical limit of manipulator joint, for the synchronous planning problem of underwater manipulator under the condition of space limitation, construct time-varying nonlinear equation with constraint condition;Based on time-varying nonlinear equation, by introducing non-negative time-varying vector, construct time-varying synchronous planning model based on pseudo-inverse description using neural dynamics design formula;For time-varying synchronous planning model, discrete processing is carried out using difference formula, and discrete synchronous planning model is obtained;The synchronous control of underwater double-arm manipulator is carried out using discrete synchronous planning model.Compared with prior art, the underwater double-arm manipulator can still accurately complete a specific synchronous planning task under the condition of joint motion space limitation, and the synchronous planning precision of underwater double-arm manipulator in practical application can be improved.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of underwater manipulator control, and particularly relates to a joint space limited underwater dual-arm manipulator synchronous control method and device. BACKGROUND

[0002] The underwater dual-arm manipulator is a special mechanical equipment carried on a submersible and driven by a motor or a hydraulic pressure, and is an indispensable tool for detecting and exploiting marine resources. The underwater dual-arm manipulator can simulate human arm functions to complete operation tasks such as submarine cable laying, submarine pipeline maintenance and marine mineral collection in a marine environment. In many application scenarios, how to realize synchronous planning of the underwater dual-arm manipulator under joint limitation is a key content, which relates to whether the dual-arm can complete a specific operation task in a limited area. At present, some synchronous planning schemes have been proposed for the joint space limited underwater dual-arm manipulator. However, the synchronous planning accuracy of these schemes is usually not high, and the corresponding error may be larger when the joint of the manipulator reaches its limit. In other words, the existing synchronous planning schemes often lack consideration for further improving the accuracy, so that they cannot achieve the desired effect in the actual application of the underwater dual-arm manipulator. Therefore, it is very necessary and valuable to design a high-precision synchronous planning scheme for the joint-constrained dual-arm.

[0003] Chinese Patent Publication No. CN 114571448 A discloses a joint limited redundant manipulator pseudo-inverse type repetitive motion planning method. First, considering the joint physical limit of the manipulator, by introducing a non-negative vector, the motion planning problem of the joint limited redundant manipulator is described as a solution problem of a nonlinear equation system. Then, the neural dynamics design formula is used to solve the nonlinear equation system, and a motion planning scheme based on pseudo-inverse description is constructed. Secondly, combining the gradient descent idea and the numerical difference formula, a pseudo-inverse type repetitive motion planning scheme is established. Finally, the motion controller drives the manipulator according to the calculation result of the repetitive motion planning scheme to accurately complete the given planning task, and returns to the initial position of the manipulator at the end of the task.

[0004] The above-mentioned application realizes repetitive motion planning of a single manipulator under motion limitation, but does not solve the problem of low synchronous planning accuracy of the underwater dual-arm manipulator under motion limitation. SUMMARY

[0005] The present application is to overcome the defects of the prior art and provide a joint space limited underwater dual-arm manipulator synchronous control method and device to realize high-precision synchronous control of the underwater dual-arm manipulator under motion limitation.

[0006] The object of the present application can be realized by the following technical solutions.

[0007] In one aspect of the present application, a joint space limited underwater dual-arm manipulator synchronous control method is provided, comprising the following steps:

[0008] S1, D-H parameter method is used to model two underwater manipulators respectively, based on the physical limit of the joints of the manipulator, for the synchronous planning problem of underwater manipulators under space limitation, a time-varying nonlinear equation with constraint conditions is constructed;

[0009] S2, based on the time-varying nonlinear equation, a non-negative time-varying vector is introduced, and a time-varying synchronous planning model based on pseudo-inverse description is constructed by using the neural dynamics design formula;

[0010] S3, for the time-varying synchronous planning model, differential formula is used for discretization processing to obtain a discrete synchronous planning model;

[0011] S4, the discrete synchronous planning model is used to control the underwater dual-arm manipulator synchronously.

[0012] As a preferred technical solution, in S1, the constructed time-varying nonlinear equation is:

[0013]

[0014] Wherein, f(·) represents a nonlinear mapping function, θ ab (t)=[θ a (t);θ b (t)]∈R 2n represents the joint angle of the underwater dual-arm manipulator, n represents the number of degrees of freedom of each manipulator, φ a (·) and φ b (·) represent the forward kinematics mapping functions of the two underwater manipulators according to the D-H parameter method, θ a (t)∈R n and θ b (t)∈R n represent the joint angles of the two underwater manipulators, p ad (t)∈R m and p bd (t)∈R m represent the desired position vectors of the end effectors of the two underwater manipulators in the underwater m-dimensional space for performing specific operation tasks.

[0015] As a preferred technical solution, in S1, the constraint condition is:

[0016] θ - ≤θ ab (t)≤θ+

[0017] Where, θ + ∈R 2n θ - ∈R 2n Let θ represent the physical limit of the joint angles of the underwater dual-arm robotic arm, t>0∈R, and θ represent the time variable. ab (t)=[θ a (t); θ b (t)]∈R 2n The denot represents the joint angles of the underwater dual-arm manipulator, and n represents the number of degrees of freedom of each manipulator.

[0018] As a preferred technical solution, the time-varying synchronization planning model based on pseudo-inverse description in S2 is as follows:

[0019]

[0020] in, Let y(t) represent the joint velocities of the underwater dual-arm manipulator, n represent the number of degrees of freedom of each manipulator, and y(t)∈R. 4n Represents the process variables in the simultaneous planning scheme evolution calculation and It is its time derivative, λ>0∈R represents the design parameters, W=[-I 2n ;I 2n ]∈R 4n×2n Represented by the identity matrix I 2n ∈R 2n×2n The augmented matrix formed by b = [-θ] - ;θ + ]∈R 4n To represent an augmented vector, a nonnegative time-varying vector is introduced. The expression is D(t) = diag{y1(t),…,y 4n (t)}∈R 4n×4n Represents a diagonal matrix. Indicated by p ad (t) and p bd The time derivative of (t), i.e. and p ad (t)∈R m and p bd (t)∈R m Let M represent the augmented vectors consisting of the expected position vectors of the end effectors of two underwater manipulators performing specific tasks in an underwater m-dimensional space. + (t)∈R 6n×(2m+4n) The augmented matrix M(t)∈R is represented as follows. (2m+4n)×6n The pseudo-inverse matrix:

[0021]

[0022] wherein represents the Jacobian matrix of the underwater dual-arm manipulator.

[0023] As a preferred technical solution, the difference formula in the S3 is:

[0024]

[0025]

[0026] wherein k represents the iteration number and k=5, 6, 7…, θ ab,k = θ ab (t k =kδ), y k =y(t k =kδ), δ>0∈R represents a sampling time interval, represents the joint speed of the underwater dual-arm manipulator, n represents the number of degrees of freedom possessed by each manipulator, and θ ab (t)∈R 2n represents the joint angle of the underwater dual-arm manipulator.

[0027] As a preferred technical solution, the discrete synchronous planning model is:

[0028]

[0029] As a preferred technical solution, the S4 is specifically:

[0030] By setting a numerical value pair, the initialization of the discrete synchronous planning model is completed, and the joint angle output realizing synchronous planning of the underwater dual-arm manipulator under the condition of joint motion space limitation is calculated through iteration.

[0031] As a preferred technical solution, the output joint angle is {θ ab,k |k=0, 1, …, (T / δ)}, wherein T represents the period of dual-arm coordination planning, k represents the iteration number, and δ>0∈R represents a sampling time interval, represents the joint speed of the underwater dual-arm manipulator.

[0032] Another aspect of the application provides an electronic device, comprising: one or more processors and a memory, the memory having one or more programs stored therein, the one or more programs comprising instructions for executing the above-mentioned joint space limited underwater dual-arm manipulator synchronous control method.

[0033] In another aspect of the present application, a computer-readable storage medium comprising one or more programs for execution by one or more processors of an electronic device is provided, the one or more programs comprising instructions for performing the above-mentioned method for high-precision synchronous control of a joint-space-restricted underwater dual-arm manipulator.

[0034] Compared with the prior art, the present application has the following advantages:

[0035] (1) Achieving high-precision synchronous control of underwater dual-arm manipulators under joint-space restriction: Some existing research has achieved control of a single manipulator under joint-space restriction, but dual-arm action is required in underwater environments, and coordinated motion control of dual-arm manipulators is more complex than single-arm control. How to ensure synchronous control of dual arms under joint-space restriction is an important problem. The present application establishes a high-precision synchronous planning method based on a neural dynamics design formula and a difference formula, which can enable underwater dual-arm manipulators to accurately complete specific synchronous planning tasks under joint-space restriction, and has important value for improving the synchronous planning precision of underwater dual-arm manipulators in practical applications.

[0036] (2) High precision of motion planning: The truncation error of the difference formula of the present application is small, and the number of iterations of the difference formula is higher, so that the precision of the discretized motion planning scheme is high.

[0037] (3) Most technologies consider motion planning of a single manipulator under joint-space restriction, while dual-arm manipulator modeling is more complex, and more joint-space restriction conditions are considered. There is less research on coordinated motion planning of joint-space-restricted underwater dual-arm manipulators, and the present application can provide certain reference value. BRIEF DESCRIPTION OF DRAWINGS

[0038] Figure 1 A flowchart of the dual-arm manipulator synchronous control method in Example 1 is shown. DETAILED DESCRIPTION

[0039] The technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the accompanying drawings in the embodiments of the present application. Obviously, the described embodiments are part of, rather than all of, the embodiments of the present application. Based on the embodiments in the present application, all other embodiments obtained by a person of ordinary skill in the art without creative labor should fall within the scope of protection of the present application.

[0040] Example 1

[0041] Figure 1 As shown, the present embodiment provides a high-precision synchronous control method for a joint-space-restricted underwater dual-arm manipulator, mainly comprising the following steps:

[0042] S1, refine the description of the underwater dual-arm manipulator synchronization planning problem with joint space constraints. The D-H parameter method is used to model the single-arm manipulator, and the joint physical limits of the manipulator are considered. The underwater dual-arm manipulator synchronization planning problem with joint space constraints is described as a constrained time-varying nonlinear equation solving problem.

[0043] For the underwater dual-arm manipulator, the D-H parameter method is used to model the single-arm manipulator, and the joint physical limits of the manipulator are considered. The synchronization planning problem with joint space constraints is described as a constrained time-varying nonlinear equation solving problem as follows:

[0044]

[0045] where f(·) represents a nonlinear mapping function, θ ab (t)=[θ a (t);θ b (t)]∈R 2n represents the joint angles of the underwater dual-arm manipulator (i.e., the joint vector containing the joint angles of the a-arm and b-arm), n represents the number of degrees of freedom possessed by each manipulator; φ a (·) and φ b (·) represent the forward kinematics mapping functions of the a-arm and b-arm according to the D-H parameter method, θ a (t)∈R n and θ b (t)∈R n represent the joint angles of the a-arm and b-arm, p ad (t)∈R m and p bd (t)∈R m represent the desired position vectors of the end-effectors of the a-arm and b-arm in the underwater m-dimensional space to perform a specific operation task; θ ± ∈R 2n represents the physical limits of the joint angles of the underwater dual-arm manipulator, t>0∈R represents the time variable.

[0046] S2, derive the synchronization planning scheme based on pseudo-inverse description. A non-negative time-varying vector is introduced, and a neurodynamic design formula is used to derive the synchronization planning scheme based on pseudo-inverse description.

[0047] For the constrained time-varying nonlinear equation solving problem (1), a non-negative time-varying vector is introduced, and a neurodynamic design formula is used to derive the synchronization planning scheme based on pseudo-inverse description as follows:

[0048]

[0049] where represents the joint velocity of the underwater dual-arm manipulator, y(t)∈R4n represents the process variable of the synchronous planning scheme evolutionary computation and is its time derivative; λ > 0 ∈ R represents the design parameter of the scheme, W = [-I 2n ; I 2n ] ∈ R 4n×2n represents the augmented matrix composed of the unit matrix I 2n ∈ R 2n×2n ; b = [-θ - ; θ + ] ∈ R 4n represents the augmented vector, the non-negative time-varying vector The expression of D(t) = diag{y1(t),…, y 4n (t)} ∈ R 4n×4n represents the diagonal matrix; represents the augmented vector composed of the time derivative of p ad (t) and p bd (t), i.e. and ; M + (t) ∈ R 6n ×(2m+4n) represents the pseudo-inverse matrix of the augmented matrix M(t) ∈ R (2m+4n)×6n :

[0050]

[0051] represents the Jacobian matrix of the underwater dual-arm manipulator.

[0052] S3, Discretization processing of the numerical difference formula. Through the discretization processing of the numerical difference formula, the high-precision synchronous planning scheme of the underwater dual-arm manipulator is established.

[0053] S4, Establishing a high-precision synchronous planning scheme. The lower machine controller plans the joint-constrained dual-arm according to the calculation results of the high-precision scheme to complete the specific operation task synchronously.

[0054] In order to realize the discretization processing of the synchronous planning scheme (2), the following numerical difference formula is adopted:

[0055]

[0056]

[0057] Wherein, k represents the iteration number and k = 5, 6, 7…, θ ab,k = θ ab (t k = kδ), yk = y(t k = kδ), δ > 0 ∈ R represents a sampling time interval.

[0058] The differential formula (3) is used to directly discretize the synchronous planning scheme (2) to establish a high-precision synchronous planning scheme for the underwater dual-arm manipulator as follows:

[0059]

[0060] wherein, For the high-precision synchronous planning scheme (4), six numerical pairs are required to complete the initialization of the iterative calculation, i.e., {θ ab,0 , y0}, {θ ab,1 , y1}, {θ ab,2 , y2}, {θ ab,3 , y3}, {θ ab,4 , y4}, and {θ ab,5 , y5}. In this case, a numerical pair {θ ab,0 , y0} can be given first, and the remaining five numerical pairs are determined according to the following calculation formula:

[0061]

[0062]

[0063]

[0064]

[0065]

[0066] Based on the six numerical pairs determined by the above formula, the joint angles for the underwater dual-arm manipulator to achieve high-precision synchronous planning under the condition of joint motion space limitation can be obtained through the iterative calculation of the high-precision synchronous planning scheme (4), i.e., {θ ab,k | k = 0, 1, …, (T / δ)}, wherein T represents the period of dual-arm coordinated planning.

[0067] S5, inputting the lower computer controller.

[0068] S6, controlling the underwater dual-arm manipulator.

[0069] After obtaining the numerical values of the joint angles of the underwater dual-arm manipulator, the result is transmitted to the lower computer controller to plan the joint-constrained dual-arm to complete the specific operation task synchronously.

[0070] The underwater double-arm manipulator high-precision synchronous planning method is established based on a neural dynamics design formula and a numerical difference formula, and has the following advantages compared with existing methods.

[0071] The underwater double-arm manipulator high-precision synchronous planning method described in the form of iterative calculation can accurately complete a specific synchronous planning task under the condition that the joint motion space of the underwater double-arm manipulator is limited, which is of great value for improving the synchronous planning precision of the underwater double-arm manipulator in practical applications.

[0072] Embodiment 2

[0073] The embodiment provides an electronic device, including one or more processors and a memory, the memory has one or more programs stored therein, and the one or more programs include instructions for executing the double-arm manipulator synchronous control method as described in Embodiment 1.

[0074] Embodiment 3

[0075] The embodiment provides a computer readable storage medium including one or more programs for one or more processors of an electronic device to execute, and the one or more programs include instructions for executing the double-arm manipulator synchronous control method as described in Embodiment 1.

[0076] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited to this, any person skilled in the art can easily think of various equivalent modifications or replacements within the technical range disclosed by the present application, and these modifications or replacements should be covered in the protection scope of the present application. Therefore, the protection scope of the present application should be subject to the protection scope of the claims.

Claims

1. A method for synchronous control of an underwater dual-arm manipulator with limited joint space, characterized in that, Includes the following steps: S1. The DH parameter method is used to model the two underwater manipulators respectively. Based on the physical limits of the manipulator joints, a time-varying nonlinear equation with constraints is constructed for the synchronous planning problem of underwater manipulators under space constraints. S2, Based on the time-varying nonlinear equation, by introducing a non-negative time-varying vector, a time-varying synchronization planning model based on pseudo-inverse description is constructed using the neurodynamic design formula; S3. For the time-varying synchronization planning model, the difference formula is used to discretize it to obtain a discrete synchronization planning model. S4. The discrete synchronization programming model is used to perform synchronous control of the underwater dual-arm manipulator. In S1, the constructed time-varying nonlinear equation is as follows: in, Represents a nonlinear mapping function. This indicates the joint angles of the underwater dual-arm robotic arm. This indicates the number of degrees of freedom possessed by each robotic arm. and These represent the forward kinematic mapping functions of two underwater manipulators established according to the DH parameter method. and These represent the joint angles of the two underwater robotic arms. and These represent the end effectors of two underwater robotic arms underwater. The expected position vector in a 3D space for performing a specific operation. In S2, the time-varying synchronization planning model based on pseudo-inverse description is as follows: in, This indicates the joint velocity of the underwater dual-arm robotic arm. This indicates the number of degrees of freedom possessed by each robotic arm. Represents the process variables in the simultaneous planning scheme evolution calculation and It is its time derivative. Indicates design parameters, Represented by the identity matrix The augmented matrix formed To represent an augmented vector, a nonnegative time-varying vector is introduced. The expression is , Represents a diagonal matrix. Indicates by and The time derivative, i.e. and , and These represent the end effectors of two underwater robotic arms underwater. An augmented vector consisting of the expected position vectors for performing a specific operation in dimensional space. The following augmented matrix is ​​represented as The pseudo-inverse matrix: in The Jacobian matrix represents the underwater dual-arm robotic arm.

2. The synchronous control method for an underwater dual-arm manipulator with limited joint space according to claim 1, characterized in that, In S1, the constraint condition is: in, , This represents the physical limit of the joint angles of an underwater dual-arm robotic arm. Represents a time variable. This indicates the joint angles of the underwater dual-arm robotic arm. This indicates the number of degrees of freedom that each robotic arm possesses.

3. The synchronous control method for an underwater dual-arm manipulator with limited joint space according to claim 1, characterized in that, In S3, the difference formula is: in, Indicates the number of iterations and , , , , , Indicates the sampling time interval. This indicates the joint velocity of the underwater dual-arm robotic arm. This indicates the number of degrees of freedom possessed by each robotic arm. This indicates the joint angles of the underwater dual-arm robotic arm.

4. The synchronous control method for an underwater dual-arm manipulator with limited joint space according to claim 3, characterized in that, The discrete synchronization planning model is as follows: 。 5. The synchronous control method for an underwater dual-arm manipulator with limited joint space according to claim 1, characterized in that, Specifically, S4 is: The discrete synchronous planning model is initialized by setting numerical pairs, and the joint angles of the underwater dual-arm manipulator are output to achieve synchronous planning under the condition of limited joint movement space through iterative calculation.

6. The synchronous control method for an underwater dual-arm manipulator with limited joint space according to claim 5, characterized in that, The output joint angle is ,in This indicates the cycle of bi-arm coordination planning. Indicates the number of iterations. Indicates the sampling time interval. , This indicates the joint velocity of the underwater dual-arm robotic arm.

7. An electronic device, characterized in that, include: One or more processors and a memory, wherein the memory stores one or more programs, the one or more programs including instructions for executing the joint space-constrained underwater dual-arm manipulator synchronization control method as described in any one of claims 1-6.

8. A computer-readable storage medium, characterized in that, It includes one or more programs that are executed by one or more processors of an electronic device, the one or more programs including instructions for performing the synchronized control method for an underwater dual-arm manipulator with confined joint space as described in any one of claims 1-6.

Images