Teleoperation method of underwater electric manipulator based on constrained optimization and force feedback guidance

Abstract

The present invention discloses a method for remote operation of an underwater electric manipulator based on constrained optimization and force feedback guidance. The method includes: establishing a slave-end constraint planner, inputting the expected joint angular displacement, and outputting the expected trajectory control operation that satisfies the multi-level physical constraints; establishing a slave-end kinematic model, inputting the actual joint angular displacement, and outputting the actual end position; establishing a virtual guiding force feedback, inputting the expected and actual end positions, and outputting the slave-end virtual feedback force; establishing a master-end kinematic model, inputting the slave-end virtual feedback force, outputting the master-end acceleration, and obtaining the master-end end position; establishing a master-slave position mapping model, inputting the master-end end position, and outputting the expected slave-end end position, completing the closed loop to realize remote operation of the underwater electric manipulator. The slave-end constraint planner of the method of the present invention ensures that the slave-end system satisfies the multi-level physical constraints of the underwater electric manipulator, thereby improving the safety and efficiency of the remote operation system; the virtual guiding force feedback can provide guidance for the operator.

Description

Technical Field

[0001] The invention relates to a remote operation method for an underwater electric manipulator arm, and in particular to a remote operation method for an underwater electric manipulator arm based on constrained optimization and force feedback guidance. Background Art

[0002] With the advancement of marine development technology, underwater operations such as marine fishery surveys, oil exploration, submarine pipeline laying and repair, and deep-sea sampling have increased significantly. Despite the significant progress in modern robotics, fully autonomous robotic operations remain extremely difficult. Therefore, teleoperation systems for underwater operations are crucial for improving the operational capabilities of underwater equipment in complex and unknown deep-sea environments.

[0003] A teleoperation system refers to a robot system in which an operator operates a master robot, sends command signals to a slave robot through a communication channel, and the slave robot tracks the received signals to perform the task and feeds back environmental information to the operator through the communication channel.

[0004] In teleoperation systems, slave actuators are inevitably subject to various kinematic and dynamic constraints. Violation of these constraints can lead to instability of the entire motion system and damage to the system hardware. In teleoperation systems, the command trajectory of the slave robot is generated online by the operator manipulating the master robot arm. Its mathematical description cannot be known in advance, making it difficult to ensure the computational efficiency and reliability of existing planners, which in turn degrades the stability of the teleoperation system. Summary of the Invention

[0005] To address the challenges presented by the prior art, this paper provides a method for teleoperation of an underwater electric manipulator based on constrained optimization and force feedback guidance. This method addresses the multi-level physical constraints of the slave underwater electric manipulator in a teleoperation system and provides force feedback guidance to the human operator. Furthermore, it incorporates an autonomous constraint planner to reduce the operator's physical and mental workload, improving the safety and efficiency of the teleoperation system.

[0006] The technical solution adopted in the present invention is:

[0007] The invention provides a remote operation method for an underwater electric manipulator arm based on constrained optimization and force feedback guidance, comprising:

[0008] Step 1: According to the desired end position of the slave underwater electric manipulator, the desired joint angular displacement of the slave underwater electric manipulator is obtained using the inverse kinematics solution method; the inverse kinematics solution is specifically a geometric method.

[0009] Step 2: Considering the multi-level physical constraints of the slave-end underwater electric manipulator, a slave-end constraint planner of the slave-end underwater electric manipulator based on constrained optimization is established, and the desired joint angular displacement of the slave-end underwater electric manipulator is input into the slave-end constraint planner. The slave-end constraint planner outputs the desired trajectory of the slave-end underwater electric manipulator that meets the multi-level physical constraints and inputs it into the slave-end controller of the slave-end underwater electric manipulator to control the operation of the slave-end underwater electric manipulator to ensure the slave-end position tracking performance.

[0010] Step 3: Establish a slave-end kinematic model of the slave-end underwater electric manipulator, input the actual joint angular displacement of the slave-end underwater electric manipulator during operation into the slave-end kinematic model, and the slave-end kinematic model outputs the actual end position of the slave-end underwater electric manipulator.

[0011] Step 4: Considering the end position planning error of the slave-end underwater electric manipulator, a virtual guiding force feedback of the slave-end underwater electric manipulator based on spring damping is established, and the expected end position and actual end position of the slave-end underwater electric manipulator are input into the virtual guiding force feedback. The virtual guiding force feedback outputs the slave-end virtual feedback force of the slave-end underwater electric manipulator.

[0012] Step 5: Establish the master-end kinematic model of the master-end manipulator, input the slave-end virtual feedback force into the master-end kinematic model, and the master-end kinematic model outputs the acceleration of the master-end manipulator, thereby obtaining the end position of the master-end manipulator.

[0013] Step 6: Establish a master-slave position mapping model. Input the end position of the master-end manipulator into the master-slave position mapping model. The master-slave position mapping model outputs the desired end position of the slave-end underwater electric manipulator after position mapping. Return to the first step, use the inverse kinematics solution method to obtain the next desired joint angular displacement of the slave-end underwater electric manipulator after position mapping. Continue the step cycle to complete the closed loop, and finally realize the remote operation of the underwater electric manipulator. The slave-end virtual feedback force and the desired end position of the slave-end underwater electric manipulator after position mapping are transmitted between the master and slave through the communication channel.

[0014] In the second step, the slave constraint planner of the slave underwater electric manipulator based on constrained optimization is as follows:

[0015]

[0016] s,t:η - ≤x≤η +

[0017] Where x represents the angular acceleration level optimization variable, represents the angular acceleration of the slave underwater electric manipulator; Ω represents the optimization equivalent coefficient; η + and η - They represent the upper and lower bounds of the angular jerk level optimization variable x.

[0018] The desired trajectory of the slave underwater electric manipulator that satisfies the multi-level physical constraints output by the slave constraint planner includes the constraint values ​​of the desired joint angular displacements. Constraint value of angular velocity and the constrained values ​​of angular acceleration

[0019] The slave constraint planner based on constrained optimization consists of four parts: optimization problem establishment, optimization goal transformation, constraint level unification, and constraint planning update. The details are as follows:

[0020] 1) Optimization problem establishment:

[0021] Considering the multi-level physical constraints of the underwater electric manipulator from the slave end (joint angular displacement q s , angular velocity angular acceleration and angular acceleration The system state constraints are processed by using the angular acceleration level configuration conversion strategy. In order to make the slave underwater electric manipulator reach any configuration at any time and consume the least energy, the following optimization problem can be established:

[0022]

[0023] in, and are the upper and lower limits of the multi-level physical constraints of the actual joints of the underwater electric manipulator from the slave end, q sd is the expected joint angular displacement of the underwater electric manipulator from the end.

[0024] Since the inequality constraints in the optimization problem are multi-level, the following two properties can be used to perform unified constraint target transformation and constraint level unification:

[0025] Property 1. For any time-varying real number w(t), when the positive parameter ξ>>0 is large enough and time t→∞, then w(t)→0 is equivalent to

[0026] Property 2. For any time-varying real number w(t), when the positive parameter ξ>>0 is large enough and time t→∞, then w(t)≤0 is equivalent to

[0027] 2) Optimize target conversion:

[0028] Define the angular displacement level error function δ p =q s -q sd , δ p →0 is equivalent to We can get:

[0029]

[0030] Among them, ξ≥0 is used to affect δ p The convergence rate of .

[0031] Therefore, the angular displacement level optimization target in formula (1a) can be transformed into the equivalent angular velocity level optimization target:

[0032]

[0033] Similarly, define the angular velocity level error function Then δ v →0 is equivalent to We can get:

[0034]

[0035] Therefore, the angular velocity level optimization target in formula (3) can be transformed into the equivalent angular acceleration level optimization target:

[0036]

[0037] Furthermore, the angular acceleration level error function is defined as Then δ a →0 is equivalent to We can get:

[0038]

[0039] Therefore, the angular acceleration level optimization objective in equation (5) can be transformed into the quadratic optimization objective of the equivalent angular acceleration level:

[0040]

[0041] in, represents the angular jerk level optimization variable, Represents the optimization equivalent coefficient.

[0042] According to equations (2) to (7), the optimization objective of the angular displacement level in equation (1a) is transformed into the quadratic optimization objective of the equivalent angular acceleration level shown in equation (7).

[0043] 3) Constraint level unification:

[0044] For the angular displacement level constraint in Eq. (1b), a sufficiently large positive angular displacement parameter ξ is designed. p , then formula (1b) is equivalent to:

[0045]

[0046] Similarly, by continuing to derive from formula (8), the angular displacement level constraint in formula (1b) can finally be transformed into:

[0047]

[0048] in, It represents the lower bound of the angular acceleration level constraint converted from the angular displacement level constraint. Represents the upper bound of the angular acceleration level constraint converted from the angular displacement level constraint.

[0049] Similarly, the angular velocity level constraint in Equation (1c) and the angular acceleration level constraint in Equation (1d) can be converted into the angular jerk level constraint in Equation (10) and Equation (11):

[0050]

[0051] in, It represents the lower bound of the angular acceleration level constraint converted from the angular velocity level constraint. represents the upper bound of the angular acceleration level constraint converted from the angular velocity level constraint, ξ v Represents the positive angular velocity parameter.

[0052]

[0053] in, It represents the lower bound of the angular acceleration level constraint converted from the angular acceleration level constraint. represents the upper bound of the angular acceleration level constraint converted from the angular acceleration level constraint, ξ a Represents the positive angular acceleration parameter.

[0054] According to equations (8)-(11), the multi-level physical constraints in equations (1b)-(1d) are transformed into a unified angular acceleration level constraint: Among them, the unified upper and lower bounds η + and η - Defined as:

[0055]

[0056] Therefore, the optimization problem of formula (1) can be transformed into a standard quadratic programming problem:

[0057]

[0058] s,t:η - ≤x≤η + (14b)

[0059] 4) Constraint update:

[0060] Then in each sampling period, the expected joint angular displacement q of the underwater electric manipulator from the slave end is sd It can be updated in real time through the inverse kinematics solution. Through the above derivation, the original optimization problem with physical constraints at all levels is transformed into a standard quadratic programming problem. Therefore, by solving the quadratic programming problem through online analytical solution, the expected joint angular displacement of the slave end underwater electric manipulator that meets the physical constraints at all levels can be obtained. Angular velocity and angular acceleration

[0061] The optimized equivalent coefficient Ω is specifically as follows:

[0062]

[0063] Where ξ represents the optimization parameter, ξ≥0, which is used to affect δ p The convergence rate of q s 、 and are the joint angular displacement, angular velocity and angular acceleration of the slave underwater electric manipulator, respectively, and q sd represents the desired joint angular displacement of the slave end underwater electric manipulator.

[0064] The upper bound η of the angular jerk level optimization variable x is + and the lower bound η - The details are as follows:

[0065]

[0066] Among them, P + and P - They represent the upper and lower bounds of the angular acceleration level constraint based on the angular displacement level constraint; V + and V - They represent the upper and lower bounds of the angular jerk level constraint based on the angular velocity level constraint, ξ v Represents the positive parameter of angular velocity; A + and A - They represent the upper and lower bounds of the angular acceleration level constraint based on the angular acceleration level constraint, ξ a represents the positive parameter of angular acceleration; and They represent the upper and lower limits of the actual angular acceleration of the slave end underwater electric manipulator, respectively.

[0067] In the third step, the slave-end kinematic model of the slave-end underwater electric manipulator is specifically as follows:

[0068] X s =[x s ,y s ,z s ] T

[0069]

[0070] Among them, X s represents the actual end position of the underwater electric manipulator from the end, x s ,y s ,z s They represent the X-axis, Y-axis and Z-axis coordinates of the slave end position of the slave end underwater electric manipulator respectively; s1 ,q s2 ,q s3 ,……,q sn They represent the first, second, third, ...n joint angular displacements of the slave end underwater electric manipulator respectively; l s1 ,l s2 ,l s3 ,……,l sn They respectively represent the lengths of the first, second, third, ...n connecting rods of the slave end underwater electric manipulator arm.

[0071] q for joint angle of underwater electric manipulator s =[q s1 ,q s2 ,q s3 ,……,q sn ] T Indicated by l, the connecting rod length s =[l s1 ,l s2 ,l s3 ,……,l sn ] T Indicates, n indicates the number of joints of the underwater electric manipulator. The center of the first joint of the underwater electric manipulator is taken as the coordinate origin O s0 , vertically upward is the Z axis S0 When the first joint angle is 0, the rotation axis of the second joint angle is the Y axis Y s0 , the horizontal direction perpendicular to the Y axis is the X axis s0 Establish a 3D coordinate system on the slave side.

[0072] The actual joint angular displacement q of the underwater electric manipulator at the slave end during operation s It includes the first, second, third,...n joint angular displacements of the slave end underwater electric manipulator arm.

[0073] In the fourth step, in order to enable the operator to perceive the motion state of the slave underwater electric manipulator and provide constraint guidance for the operator's manipulation state, a spring damping force feedback generation algorithm based on the slave underwater electric manipulator's workspace and position error is designed; the virtual guidance force feedback of the slave underwater electric manipulator is specifically as follows:

[0074]

[0075] F e =k e [arctan(K p -1 (X sd -X s ))]

[0076]

[0077] Among them, F s F represents the slave end virtual feedback force of the slave end underwater electric manipulator; e and F b represent the error guidance force and virtual boundary spring damping force respectively; X S and X sd They represent the actual end position and the expected end position of the underwater electric manipulator at the slave end, X u Indicates the preset reachable workspace boundary from the end; k e represents the error guiding force amplitude parameter; K p represents the constant scale mapping coefficient matrix; k b represents the virtual boundary spring damping force F b Spring modulus; X b Represents the boundary of the slave end workspace; F0 represents the virtual boundary spring damping force F b The damping coefficient; Indicates the actual end position X of the underwater electric manipulator from the end s The unit vector of .

[0078] In the fifth step, the master end kinematic model of the master end manipulator is as follows:

[0079]

[0080] Among them, F h Indicates the control force exerted by the operator of the master end robot arm; F s represents the slave end virtual feedback force of the slave end underwater electric manipulator; M m Indicates the mass of the master end robotic arm; Indicates the acceleration of the master end robot arm; Z h Indicates the impedance coefficient of the master-side manipulator operator.

[0081] According to the acceleration of the master end robot arm After two 1 / s integrations by the integrator, the end position X of the master end robot is obtained. m .

[0082] In the sixth step, the master-slave position mapping model is specifically as follows:

[0083]

[0084] in, represents the desired end position of the underwater electric manipulator from the end after position mapping; K p Represents the constant scale mapping coefficient matrix; the end position X of the master end manipulator m ;T p Represents the position translation vector.

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

[0086] 1. The slave-side constraint planner based on constrained optimization designed in the present invention can ensure that the slave-side system meets the multi-level physical constraints of the underwater electric manipulator, thereby improving the safety and efficiency of the underwater electric manipulator teleoperation system.

[0087] 2. The virtual guidance force feedback designed by the present invention can provide guidance to human operators and help them reduce their physical / mental workload. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 This is a structural diagram of the slave-end underwater electric manipulator arm of the present invention;

[0089] Figure 2 This is a block diagram of the underwater electric manipulator teleoperation planning system designed by the present invention based on constrained optimization and force feedback guidance;

[0090] Figure 3 Schematic diagram of the slave-side constraint planner based on constrained optimization designed by the present invention;

[0091] Figure 4 This is a rendering of the constraint planning of the underwater electric manipulator of the present invention; Figure 4 (a) is the first joint angle effect diagram without constraint planner and with constraint planner. Figure 4 (b) is the effect diagram of the second joint angle without constraint planner and with constraint planner. Figure 4 (c) is the effect diagram of the third joint angle without constraint planner and with constraint planner. Figure 4 (d) is the effect diagram of the angular velocity of each joint after constraint planning. Figure 4 (e) is the effect diagram of the angular acceleration of each joint after constraint planning;

[0092] Figure 5 It is the master-slave terminal position tracking trajectory diagram of the present invention;

[0093] Figure 6 It is a virtual force feedback result diagram of the present invention. DETAILED DESCRIPTION

[0094] In order to make the objectives, technical solutions and advantages of the present invention more clearly understood, the present invention is further described in detail below with reference to the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only intended to illustrate the present invention and are not intended to limit the present invention. In addition, the technical features involved in the various embodiments of the present invention described below may be combined with each other as long as they do not conflict with each other.

[0095] like Figure 2 As shown, the underwater electric manipulator teleoperation method based on constrained optimization and force feedback guidance of the present invention includes:

[0096] Step 1: According to the desired end position of the slave underwater electric manipulator, the desired joint angular displacement of the slave underwater electric manipulator is obtained using the inverse kinematics solution method; the inverse kinematics solution is specifically a geometric method.

[0097] Step 2: Considering the multi-level physical constraints of the slave-end underwater electric manipulator, a slave-end constraint planner of the slave-end underwater electric manipulator based on constrained optimization is established, and the desired joint angular displacement of the slave-end underwater electric manipulator is input into the slave-end constraint planner. The slave-end constraint planner outputs the desired trajectory of the slave-end underwater electric manipulator that meets the multi-level physical constraints and inputs it into the slave-end controller of the slave-end underwater electric manipulator to control the operation of the slave-end underwater electric manipulator to ensure the slave-end position tracking performance.

[0098] In the second step, the slave constraint planner of the slave underwater electric manipulator based on constrained optimization is as follows:

[0099]

[0100] s,t:η - ≤x≤η +

[0101] Where x represents the angular acceleration level optimization variable, represents the angular acceleration of the slave underwater electric manipulator; Ω represents the optimization equivalent coefficient; η + and η - They represent the upper and lower bounds of the angular jerk level optimization variable x.

[0102] The desired trajectory of the slave underwater electric manipulator that satisfies the multi-level physical constraints output by the slave constraint planner includes the constraint values ​​of the desired joint angular displacements. Constraint value of angular velocity and the constrained values ​​of angular acceleration

[0103] The slave constraint planner based on constrained optimization consists of four parts: optimization problem establishment, optimization goal transformation, constraint level unification, and constraint planning update. The details are as follows:

[0104] 1) Optimization problem establishment:

[0105] Considering the multi-level physical constraints of the underwater electric manipulator from the slave end (joint angular displacement q s , angular velocity angular acceleration and angular acceleration The system state constraints are processed by using the angular acceleration level configuration conversion strategy. In order to make the slave underwater electric manipulator reach any configuration at any time and consume the least energy, the following optimization problem can be established:

[0106]

[0107] in, and are the upper and lower limits of the multi-level physical constraints of the actual joints of the underwater electric manipulator from the slave end, q sd is the expected joint angular displacement of the underwater electric manipulator from the end.

[0108] Since the inequality constraints in the optimization problem are multi-level, the following two properties can be used to perform unified constraint target transformation and constraint level unification:

[0109] Property 1. For any time-varying real number w(t), when the positive parameter ξ>>0 is large enough and time t→∞, then w(t)→0 is equivalent to

[0110] Property 2. For any time-varying real number w(t), when the positive parameter ξ>>0 is large enough and time t→∞, then w(t)≤0 is equivalent to

[0111] 2) Optimize target conversion:

[0112] Define the angular displacement level error function δ p =q s -q sd , δ p →0 is equivalent to We can get:

[0113]

[0114] Among them, ξ≥0 is used to affect δp The convergence rate of .

[0115] Therefore, the angular displacement level optimization target in formula (1a) can be transformed into the equivalent angular velocity level optimization target:

[0116]

[0117] Similarly, define the angular velocity level error function Then δ v →0 is equivalent to We can get:

[0118]

[0119] Therefore, the angular velocity level optimization target in formula (3) can be transformed into the equivalent angular acceleration level optimization target:

[0120]

[0121] Furthermore, the angular acceleration level error function is defined as Then δ a →0 is equivalent to We can get:

[0122]

[0123] Therefore, the angular acceleration level optimization objective in equation (5) can be transformed into the quadratic optimization objective of the equivalent angular acceleration level:

[0124]

[0125] in, represents the angular jerk level optimization variable, Represents the optimization equivalent coefficient.

[0126] According to equations (2) to (7), the optimization objective of the angular displacement level in equation (1a) is transformed into the quadratic optimization objective of the equivalent angular acceleration level shown in equation (7).

[0127] 3) Constraint level unification:

[0128] For the angular displacement level constraint in Eq. (1b), a sufficiently large positive angular displacement parameter ξ is designed. p , then formula (1b) is equivalent to:

[0129]

[0130] Similarly, by continuing to derive from formula (8), the angular displacement level constraint in formula (1b) can finally be transformed into:

[0131]

[0132] in, It represents the lower bound of the angular acceleration level constraint converted from the angular displacement level constraint. Represents the upper bound of the angular acceleration level constraint converted from the angular displacement level constraint.

[0133] Similarly, the angular velocity level constraint in Equation (1c) and the angular acceleration level constraint in Equation (1d) can be converted into the angular jerk level constraint in Equation (10) and Equation (11):

[0134]

[0135] in, It represents the lower bound of the angular acceleration level constraint converted from the angular velocity level constraint. represents the upper bound of the angular acceleration level constraint converted from the angular velocity level constraint, ξ v Represents the positive angular velocity parameter.

[0136]

[0137] in, It represents the lower bound of the angular acceleration level constraint converted from the angular acceleration level constraint. represents the upper bound of the angular acceleration level constraint converted from the angular acceleration level constraint, ξ a Represents the positive angular acceleration parameter.

[0138] According to equations (8)-(11), the multi-level physical constraints in equations (1b)-(1d) are transformed into a unified angular acceleration level constraint: Among them, the unified upper and lower bounds η + and η - Defined as:

[0139]

[0140] Therefore, the optimization problem of formula (1) can be transformed into a standard quadratic programming problem:

[0141]

[0142] s,t:η - ≤x≤η + (14b)

[0143] 4) Constraint update:

[0144] Then in each sampling period, the expected joint angular displacement q of the underwater electric manipulator from the slave end is sdIt can be updated in real time through the inverse kinematics solution. Through the above derivation, the original optimization problem with physical constraints at all levels is transformed into a standard quadratic programming problem. Therefore, by solving the quadratic programming problem through online analytical solution, the expected joint angular displacement of the slave end underwater electric manipulator that meets the physical constraints at all levels can be obtained. Angular velocity and angular acceleration

[0145] The optimized equivalent coefficient Ω is as follows:

[0146]

[0147] Where ξ represents the optimization parameter, ξ≥0, which is used to affect δ p The convergence rate of q s 、 and are the joint angular displacement, angular velocity and angular acceleration of the slave underwater electric manipulator, respectively, and q sd represents the desired joint angular displacement of the slave end underwater electric manipulator.

[0148] The upper bound η of the angular jerk level optimization variable x + and the lower bound η - The details are as follows:

[0149]

[0150] Among them, P + and P - They represent the upper and lower bounds of the angular acceleration level constraint based on the angular displacement level constraint; V + and V - They represent the upper and lower bounds of the angular jerk level constraint based on the angular velocity level constraint, ξ v Represents the positive parameter of angular velocity; A + and A - They represent the upper and lower bounds of the angular acceleration level constraint based on the angular acceleration level constraint, ξ a represents the positive parameter of angular acceleration; and They represent the upper and lower limits of the actual angular acceleration of the slave end underwater electric manipulator, respectively.

[0151] Step 3: Establish a slave-end kinematic model of the slave-end underwater electric manipulator, input the actual joint angular displacement of the slave-end underwater electric manipulator during operation into the slave-end kinematic model, and the slave-end kinematic model outputs the actual end position of the slave-end underwater electric manipulator.

[0152] In the third step, the slave kinematic model of the slave underwater electric manipulator is as follows:

[0153] Xs =[x s ,y s ,z s ] t

[0154]

[0155] Among them, X s represents the actual end position of the underwater electric manipulator from the end, x s ,y s ,z s They represent the X-axis, Y-axis and Z-axis coordinates of the slave end position of the slave end underwater electric manipulator respectively; s1 ,q s2 ,q s3 ,……,q sn They represent the first, second, third, ...n joint angular displacements of the slave end underwater electric manipulator respectively; l s1 ,l s2 ,l s3 ,……,l sn They respectively represent the lengths of the first, second, third, ...n connecting rods of the slave end underwater electric manipulator arm.

[0156] q for joint angle of underwater electric manipulator s =[q s1 ,q s2 ,q s3 ,……,q sn ] T Indicated by l, the connecting rod length s =[l s1 ,l s2 ,l s3 ,……,l sn ] T Indicates, n indicates the number of joints of the underwater electric manipulator. The center of the first joint of the underwater electric manipulator is taken as the coordinate origin O s0 , vertically upward is the Z axis s0 When the first joint angle is 0, the rotation axis of the second joint angle is the Y axis Y s0 , the horizontal direction perpendicular to the Y axis is the X axis s0 Establish a 3D coordinate system on the slave side.

[0157] The actual joint angular displacement q of the underwater electric manipulator at the slave end during operation s It includes the first, second, third,...n joint angular displacements of the slave end underwater electric manipulator arm.

[0158] Step 4: Considering the end position planning error of the slave-end underwater electric manipulator, a virtual guiding force feedback of the slave-end underwater electric manipulator based on spring damping is established, and the expected end position and actual end position of the slave-end underwater electric manipulator are input into the virtual guiding force feedback. The virtual guiding force feedback outputs the slave-end virtual feedback force of the slave-end underwater electric manipulator.

[0159] In the fourth step, to enable the operator to perceive the motion state of the slave underwater electric manipulator and provide constraint guidance for the operator's manipulation state, a spring damping force feedback generation algorithm based on the slave underwater electric manipulator's workspace and position error was designed. The virtual guidance force feedback of the slave underwater electric manipulator is as follows:

[0160]

[0161] F e =k e [arctan(K p -1 (X sd -X s ))]

[0162]

[0163] Among them, F s F represents the slave end virtual feedback force of the slave end underwater electric manipulator; e and F b represent the error guidance force and virtual boundary spring damping force respectively; X s and X sd They represent the actual end position and the expected end position of the underwater electric manipulator at the slave end, X u Indicates the preset reachable workspace boundary from the end; k e represents the error guiding force amplitude parameter; K p represents the constant scale mapping coefficient matrix; k b represents the virtual boundary spring damping force F b Spring modulus; X b Represents the boundary of the slave end workspace; F0 represents the virtual boundary spring damping force F b The damping coefficient; Indicates the actual end position X of the underwater electric manipulator from the end s The unit vector of .

[0164] Step 5: Establish the master-end kinematic model of the master-end manipulator, input the slave-end virtual feedback force into the master-end kinematic model, and the master-end kinematic model outputs the acceleration of the master-end manipulator, thereby obtaining the end position of the master-end manipulator.

[0165] In the fifth step, the master end kinematic model of the master end manipulator is as follows:

[0166]

[0167] Among them, F h Indicates the control force exerted by the operator of the master end robot arm; F s represents the slave end virtual feedback force of the slave end underwater electric manipulator; M m Indicates the mass of the master end robotic arm; Indicates the acceleration of the master end robot arm; Z h Indicates the impedance coefficient of the master-side manipulator operator.

[0168] According to the acceleration of the master end robot arm After two 1 / s integrations by the integrator, the end position X of the master end robot is obtained. m .

[0169] Step 6: Establish a master-slave position mapping model. Input the end position of the master-end manipulator into the master-slave position mapping model. The master-slave position mapping model outputs the desired end position of the slave-end underwater electric manipulator after position mapping. Return to the first step, use the inverse kinematics solution method to obtain the next desired joint angular displacement of the slave-end underwater electric manipulator after position mapping. Continue the step cycle to complete the closed loop, and finally realize the remote operation of the underwater electric manipulator. The slave-end virtual feedback force and the desired end position of the slave-end underwater electric manipulator after position mapping are transmitted between the master and slave through the communication channel.

[0170] In the sixth step, the master-slave position mapping model is as follows:

[0171]

[0172] in, represents the desired end position of the underwater electric manipulator from the end after position mapping; K p Represents the constant scale mapping coefficient matrix; the end position X of the master end manipulator m ;T p Represents the position translation vector.

[0173] The specific embodiments of the present invention are as follows:

[0174] like Figure 1 As shown, a three-joint-angle three-link slave underwater electric manipulator is used. Each joint angle of the underwater electric manipulator is q s =[q s1 ,q s2 ,q s3 ,……,q sn ] T Indicated by l, the length of each connecting rod s =[ls1 ,l s2 ,l s3 ,……,l sn ] T Indicates, n indicates the number of joints of the underwater electric manipulator. The center of the first joint of the underwater electric manipulator is taken as the coordinate origin O s0 , vertically upward is the Z axis s0 When the first joint angle is 0, the rotation axis of the second joint angle is the Y axis Y s0 , the horizontal direction perpendicular to the Y axis is the X axis s0 Establish the three-dimensional coordinate system of the slave end, and use X to represent the end position of the underwater electric manipulator. s =[x s ,y s ,z s ] T express.

[0175] The kinematic model of the slave underwater electric manipulator is established, including the forward kinematic model and the inverse kinematic model. The forward kinematic model is as follows:

[0176]

[0177] The inverse kinematics model of the underwater electric manipulator obtained according to the geometric method is as follows:

[0178]

[0179] A kinematic model of the slave underwater electric manipulator is established using a geometric approach. Based on this model, a constrained optimization-based slave constraint planner is designed, taking into account the multi-level physical constraints of the slave underwater electric manipulator. This planner then inputs the desired trajectory that satisfies the constraints into the slave controller to ensure the slave's position tracking performance. Finally, considering the position planning and control errors of the slave underwater electric manipulator's end position, a virtual force feedback system is designed to provide guidance to the human operator. Therefore, the proposed teleoperation planning method for an underwater electric manipulator, based on constrained optimization and force feedback guidance, effectively addresses the multi-level physical constraints of the slave underwater electric manipulator in the teleoperation system. Furthermore, the designed virtual force feedback guidance further enhances the operator's sense of presence, thereby improving the safety and efficiency of the teleoperation system.

[0180] The proposed teleoperation constraint planning method was tested on a five-function underwater electric manipulator platform and compared with an unconstrained planning method to verify the planning effectiveness of the proposed method. To visually verify the effectiveness and reliability of the proposed planning method, the fourth rotational joint of the slave underwater electric manipulator was fixed. During the verification, the physical constraints at each level in the constraint planner were selected as follows:

[0181]

[0182] The convergence parameters are selected as: ξ=10, ξ p =ξ v =ξ a = 5. Constant scale mapping coefficient matrix K p and position translation vector T p Select: K p =diag([2.6,2.8,2.9]),T p =[0,0,-0.005] T ; The virtual force feedback amplitude parameter is selected as: k e =2, k b =0.6.

[0183] The constraint planning effect of the underwater electric manipulator designed by the slave constraint planner based on constrained optimization is as follows: Figure 4 As shown, Figure 4 (a) Figure 4 (b) and Figure 4 The solid lines in (c) represent the three joint angles without the constraint planner, and the dotted lines represent the joint angles with the constraint planner. Figure 4 (d) and Figure 4 (e) shows the effect diagram of angular velocity and angular acceleration of each joint after constraint planning. The master-slave terminal position tracking trajectory result of the present invention is as follows: Figure 5 As shown, the thick black line represents the actual position of the end of the master end manipulator, the light dotted line represents the master end position after transmission to the slave end, the black dotted line represents the expected position of the slave end after proportional mapping, and the black wide dotted line represents the actual position of the end of the slave end manipulator. Finally, the virtual force feedback x-axis, y-axis and z-axis results of the present invention are shown in FIG. Figure 6 As shown. Figure 4 , the joint angles of the underwater electric manipulator with the constraint planner can effectively track the desired joint angles, and the planned joint angular velocity and angular acceleration meet the preset constraints (joint angular velocity is within the range of ±1rad / s, joint angular acceleration is within the range of ±2rad / s 2 within the scope); according to Figure 5 The master-slave position mapping model ensures that the working space of the slave underwater electric manipulator is fully utilized, and the position error of the slave underwater electric manipulator end after planning by the slave constraint planner is within 8mm; according to Figure 6 When the slave end reaches the workspace boundary, the operator can feel a significant feedback force, thereby constraining the operator's behavior. The constraint planning effect and virtual force feedback result diagram show that the underwater electric manipulator teleoperation planning method based on constrained optimization and force feedback guidance can handle all levels of physical constraints of the underwater electric manipulator and smoothly track the master end's desired trajectory that meets the constraints. In addition, virtual force feedback can effectively constrain the operator's behavior.

[0184] Compared with the unconstrained planning method, the angular displacement, acceleration and angular acceleration of each joint after planning in the present invention all meet the preset constraint range, demonstrating that the underwater electric manipulator remote operation planning method based on constrained optimization and force feedback guidance designed in the present invention can help human operators reduce the physical / mental workload and improve the safety and efficiency of the remote operation system.

[0185] The above content is only the technical idea of ​​the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the claims of the present invention.

Claims

1. A method for remote operation of an underwater electric manipulator based on constrained optimization and force feedback guidance, characterized in that: include: Step 1: According to the desired end position of the slave underwater electric manipulator, the desired joint angular displacement of the slave underwater electric manipulator is obtained using the inverse kinematics solution method; Step 2: Establish a slave-end constraint planner for the slave-end underwater electric manipulator based on constrained optimization, input the desired joint angular displacement of the slave-end underwater electric manipulator into the slave-end constraint planner, and the slave-end constraint planner outputs the desired trajectory of the slave-end underwater electric manipulator that satisfies the multi-level physical constraints and inputs the desired trajectory into the slave-end controller of the slave-end underwater electric manipulator to control the operation of the slave-end underwater electric manipulator; Step 3: Establish a slave-end kinematic model of the slave-end underwater electric manipulator, input the actual joint angular displacement of the slave-end underwater electric manipulator during operation into the slave-end kinematic model, and the slave-end kinematic model outputs the actual end position of the slave-end underwater electric manipulator; Step 4: Establish a virtual guidance force feedback of the slave-end underwater electric manipulator, input the desired end position and actual end position of the slave-end underwater electric manipulator into the virtual guidance force feedback, and the virtual guidance force feedback outputs the slave-end virtual feedback force of the slave-end underwater electric manipulator; Step 5: Establish a master-end kinematic model of the master-end manipulator, input the slave-end virtual feedback force into the master-end kinematic model, and the master-end kinematic model outputs the acceleration of the master-end manipulator, thereby obtaining the end position of the master-end manipulator; Step 6: Establish a master-slave position mapping model, input the end position of the master-end manipulator into the master-slave position mapping model, and the master-slave position mapping model outputs the expected end position of the slave-end underwater electric manipulator after position mapping. Return to the first step, use the inverse kinematics solution method to obtain the next expected joint angular displacement of the slave-end underwater electric manipulator after the expected end position of the slave-end underwater electric manipulator after position mapping, continue the step cycle to complete the closed loop, and finally realize the remote operation of the underwater electric manipulator; In the second step, the slave constraint planner of the slave underwater electric manipulator based on constrained optimization is as follows: s,t:h - ≤x≤η + Where x represents the angular acceleration level optimization variable, represents the angular acceleration of the slave underwater electric manipulator; Ω represents the optimization equivalent coefficient; η + and η - They represent the upper and lower bounds of the angular jerk level optimization variable x respectively; The desired trajectory of the slave underwater electric manipulator that satisfies the multi-level physical constraints output by the slave constraint planner includes the constraint values ​​of the desired joint angular displacements. Constraint value of angular velocity and the constrained values ​​of angular acceleration The upper bound η of the angular jerk level optimization variable x is + and the lower bound η - The details are as follows: Among them, P + and P - They represent the upper and lower bounds of the angular jerk level constraint based on the angular displacement level constraint, ξ p Represents the positive parameter of angular displacement; V + and V - They represent the upper and lower bounds of the angular jerk level constraint based on the angular velocity level constraint, ξ v Represents the positive parameter of angular velocity; A + and A - They represent the upper and lower bounds of the angular acceleration level constraint based on the angular acceleration level constraint, ξ a represents the positive parameter of angular acceleration; and They represent the upper and lower limits of the actual angular acceleration of the slave end underwater electric manipulator, respectively.

2. The underwater electric manipulator teleoperation method based on constrained optimization and force feedback guidance according to claim 1, characterized in that: The optimized equivalent coefficient Ω is specifically as follows: Where ξ represents the optimization parameter, ξ≥0; q s 、 and are the joint angular displacement, angular velocity and angular acceleration of the slave underwater electric manipulator, respectively, and q sd represents the desired joint angular displacement of the slave end underwater electric manipulator.

3. The underwater electric manipulator teleoperation method based on constrained optimization and force feedback guidance according to claim 1, characterized in that: In the third step, the slave-end kinematic model of the slave-end underwater electric manipulator is specifically as follows: X s =[x s ,y s ,z s ] T Among them, X s represents the actual end position of the underwater electric manipulator from the end, x s ,y s ,z s They represent the X-axis, Y-axis and Z-axis coordinates of the slave end position of the slave end underwater electric manipulator respectively; s1 ,q s2 ,q s3 ,……,q sn They represent the first, second, third, ...n joint angular displacements of the slave end underwater electric manipulator respectively; l s1 ,l s2 ,l s3 ,……,l sn represent the lengths of the first, second, third, ...n connecting rods of the slave end underwater electric manipulator arm respectively; The actual joint angular displacement q of the underwater electric manipulator at the slave end during operation s It includes the first, second, third, ...n joint angular displacements of the slave end underwater electric manipulator arm.

4. The underwater electric manipulator teleoperation method based on constrained optimization and force feedback guidance according to claim 1, characterized in that: In the fourth step, the virtual guidance force feedback of the underwater electric manipulator arm at the slave end is specifically as follows: F e =k e [arctan(K p -1 (X sd -X s ))] Among them, F s F represents the slave end virtual feedback force of the slave end underwater electric manipulator; e and F b represent the error guidance force and virtual boundary spring damping force respectively; X s and X sd They represent the actual end position and the expected end position of the underwater electric manipulator at the slave end, X u Indicates the preset reachable workspace boundary from the end; k e represents the error guiding force amplitude parameter; K p represents the constant scale mapping coefficient matrix; k b represents the virtual boundary spring damping force F b Spring modulus; X b Represents the boundary of the slave end workspace; F0 represents the virtual boundary spring damping force F b The damping coefficient; Indicates the actual end position X of the underwater electric manipulator from the end s The unit vector of .

5. The underwater electric manipulator teleoperation method based on constrained optimization and force feedback guidance according to claim 1, characterized in that: In the fifth step, the master end kinematic model of the master end manipulator is as follows: Among them, F h Indicates the control force exerted by the operator of the master end robot arm; F s represents the slave end virtual feedback force of the slave end underwater electric manipulator; M m Indicates the mass of the master end robotic arm; Indicates the acceleration of the master end robot arm; Z h Indicates the impedance coefficient of the master-end manipulator operator; According to the acceleration of the master end robot After two 1 / s integrations by the integrator, the end position X of the master end robot is obtained. m .

6. The underwater electric manipulator teleoperation method based on constrained optimization and force feedback guidance according to claim 1, characterized in that: In the sixth step, the master-slave position mapping model is specifically as follows: in, represents the desired end position of the underwater electric manipulator from the end after position mapping; K p Represents the constant scale mapping coefficient matrix; the end position X of the master end manipulator m ;T p Represents the position translation vector.

Images