A hybrid human-machine sharing collaborative control method for underwater operation robots

Abstract

The present invention discloses a composite human-machine shared collaborative control method for an underwater working robot, including a robot modeling stage, an information collection stage, a motion mapping stage, a task allocation stage, a task priority separation shared control stage and a force feedback stage, wherein the task allocation stage divides the task into a joint constraint task, a hybrid shared whole-body collaborative control task and a posture optimization task; the joint constraint task is controlled by an autonomous system to prevent the robot from self-collision; the hybrid shared whole-body collaborative control task focuses on the motion control of the end of the underwater manipulator, and the end of the underwater manipulator is controlled by a hybrid shared control method, integrating the input of the operator and the autonomous system, and performing motion allocation for the underwater vehicle and the underwater manipulator; the posture optimization task is controlled by an autonomous system to help the operator adjust the viewing angle. The present invention can automatically realize self-obstacle avoidance and auxiliary posture adjustment, and realize human-machine shared control in the terminal operation task.

Description

Technical Field

[0001] The present invention belongs to the field of underwater robot control, and in particular relates to a composite human-machine shared collaborative control method for an underwater operating robot. Background Art

[0002] The surge in human ocean exploration has led to an increasing demand for underwater tasks. Skilled divers can perform some underwater interventions, but can only complete limited diving tasks. Therefore, underwater operation robots equipped with manipulators are essential for underwater operations. The full automatic control of underwater operation robots to perform tasks cannot be achieved in a short period of time, and professional personnel are required. However, achieving full remote operation control of underwater operation robots requires operators to maintain a high degree of concentration and patience to perform precise movements. This process places high demands on the operator's physical and mental strength, making it challenging to maintain for a long time. In addition, the separate control of underwater vehicles and manipulators is inefficient and difficult to coordinate, which can easily cause the operating objects to destroy or damage the robot. In order to further reduce the operator's control workload, we propose a composite human-machine shared collaborative control method for underwater operation robots, combining separate shared control and hybrid shared control to collaboratively control underwater vehicles and underwater manipulators, reduce the burden on operators, and improve operation efficiency.

[0003] A Chinese patent document with publication number CN118456448A discloses a human-machine shared control method for a heavy-duty robotic arm in an emergency rescue environment, including obtaining the operator's electromyographic signal, blinking frequency, operation time, operation age and operation speed in real time to evaluate the operator's state, obtaining the maximum displacement change difference, acceleration, load, moving speed and rotation angle information at the end of the robotic arm to evaluate the robotic arm state, evaluating the difficulty of the emergency rescue task according to the operation time requirement, target positioning accuracy and load requirement information, and using a combination of weighted and sigmoid functions to construct a human-machine mutual trust model that considers the operator state, robotic arm state and emergency rescue difficulty. The human-machine mutual trust is obtained in real time, and the human-machine mutual trust and the distance between the robotic arm and the target object are used as the input of the TS fuzzy controller. The human-machine shared control weight is dynamically decided based on the fuzzy rules, so as to improve the rescue efficiency of the heavy-duty robotic arm in the emergency rescue environment and ensure the safety of rescue personnel.

[0004] The Chinese patent document with publication number CN114454157A discloses a local trajectory adjustment and human-machine shared control method and system suitable for robots, in order to enhance the autonomy of surgical robots and transform the relationship between humans and robots from master-slave to collaborative. When the difference between the human's instructions and the robot's reference trajectory is large, the robot will combine the human's virtual interaction force to make local active adjustments to its own reference trajectory; when the difference between the human and robot's intentions is small, the instructions of both the human and the robot will be comprehensively considered, and the human-machine hybrid cost function will be dynamically adjusted based on the system safety evaluation index to calculate the optimal control amount and realize human-machine shared control.

[0005] A Chinese patent document with publication number CN110968084A discloses a human-machine shared control method for an autonomous remote-controlled underwater robot, which includes three units: behavior management, basic behavior, and behavior synthesis; the behavior management unit manages the basic behavior through the coordinated management of the operator module and the autonomous management module to achieve the switching of the robot's operating mode; the basic behavior unit calculates the velocity vector of each basic behavior output of the robot based on the information input by the sensor system and the operator; the behavior synthesis unit is used to mix the output of each basic behavior to obtain a control command to act on the robot execution system, thereby achieving human-machine shared control of the autonomous remote-controlled underwater robot.

[0006] However, most existing methods control underwater vehicles and underwater manipulators separately, without considering the coordinated control of robots and manipulators. Moreover, most shared control methods use separate shared control or hybrid shared control, without organically combining the two. In addition, most shared control methods lack the operator's understanding of the autonomous system's intentions. Clear intention feedback can help operators better control the equipment. Summary of the invention

[0007] The present invention provides a composite human-machine shared collaborative control method for an underwater working robot, which can automatically realize self-obstacle avoidance and auxiliary posture adjustment, realize human-machine shared control in terminal operation tasks, and coordinate tasks in a priority manner.

[0008] A hybrid human-machine shared collaborative control method for an underwater operation robot includes the following six stages:

[0009] (1) Robot modeling stage

[0010] The underwater operation robot is divided into underwater vehicle, underwater manipulator and main end manipulator, and the world coordinate system Σ w , underwater vehicle coordinate system Σ v , underwater manipulator base coordinate system Σ ub and the terminal coordinate system Σ ue , the base coordinate system of the master manipulator Σ mb and the terminal coordinate system Σ meAs well as the coordinate transformation relationship between different coordinate systems, and further establish the kinematic model of the underwater operation robot;

[0011] (2) Information Collection Stage

[0012] The real-time images of the underwater robot's own state and the surrounding workspace are collected; the real-time images collected by the camera are provided to the operator to observe the target object and the surrounding environment; on the other hand, the real-time images are processed by image recognition technology to obtain the target object position p g , together with the robot’s own state S g sent to autonomous systems;

[0013] (3) Motion mapping stage

[0014] The main end manipulator collects the operator's input and maps it to the underwater manipulator through coordinate transformation;

[0015] (4) Task allocation stage

[0016] The tasks are divided into joint constraint tasks, operation tasks and posture optimization tasks; among them, the joint constraint tasks are controlled by the autonomous system to prevent the robot from self-collision; the hybrid shared whole-body collaborative control focuses on the motion control of the end of the underwater manipulator, which is controlled by a hybrid shared control method, integrating the input of the operator and the autonomous system, and allocating the motion of the underwater vehicle and the underwater manipulator; the posture optimization task is controlled by the autonomous system to help the operator adjust to the preferred perspective;

[0017] (5) Task priority separation shared control stage

[0018] Introducing task priorities as a method of decoupled shared control to establish a hierarchy between different tasks;

[0019] (6) Force feedback stage

[0020] When the operator operates the main end manipulator to remotely operate the underwater manipulator, force feedback is provided to help the operator understand the robot's intentions and guide the operator's operations.

[0021] The present invention can automatically achieve self-obstacle avoidance and auxiliary posture adjustment, and realize human-machine shared control in the terminal operation task, and coordinate tasks in a priority manner. Combined with the underwater operation robot full-body controller, it further reduces the burden on remote operators, reduces operation time and improves operation efficiency. Real-time force feedback during the control process helps operators understand the machine's intentions and helps to complete the operation quickly.

[0022] Furthermore, in the motion mapping phase, operator input for

[0023]

[0024] Among them, V hd represents the operator's speed input, υ hd and ω hd represents the linear velocity and angular velocity input by the operator, p u,d and R u,d represents the desired position and direction of the end of the underwater manipulator, p u and R u is the current position and direction of the terminal, V m Indicates the speed of the master manipulator; the symbol logR represents the matrix logarithm, and the operator [*] ∨ It means extracting the azimuth error vector from the antisymmetric matrix;

[0025] The desired position p of the end of the underwater manipulator u,d is obtained by matching the displacement of the master manipulator relative to its initial position u,d =p m,0 +k s (p m -p m,0 )

[0026] Among them, p m and p m,0 are the current position and initial position of the master robot arm, respectively, s Represents the scale factor, which is used to adjust the scale of the displacement mapping;

[0027] The desired direction R of the end of the underwater manipulator u,d Mapping

[0028] R u,d =R ub,mb R mb,me R me,ue

[0029] Among them, R ub,mb Represents the coordinate system of the underwater manipulator Σ ub To the main end robot arm base coordinate system Σ mb The rotation transformation, R mb,me Represents the base coordinate system of the slave robot arm Σ mb To the main end robot arm end coordinate system Σ me The rotation transformation, R me,ue Represents the coordinate system of the end arm of the master end Σ me To the underwater manipulator end coordinate system Σ ue The rotation transformation of .

[0030] In the task allocation stage, the joint constraint tasks specifically include:

[0031] The joint constraint objective function of the underwater manipulator is:

[0032]

[0033] in, and Represents the maximum and minimum values ​​of the set joint; q max and q min represents the physical limit of the joint, q th is the safety threshold of the joint; α and β are constants, and α=β=1 is set in use;

[0034] Derivative the joint constraint objective function to obtain the joint constraint Jacobian matrix J cm (q);

[0035] To keep the joints of the robot within their limits, the velocities in task space are

[0036]

[0037] For underwater vehicles, restrict the vehicle's position to avoid collisions

[0038]

[0039] Where d represents the distance to the obstacle, d th represents the safety distance threshold, and the mission speed constrained by the underwater vehicle is

[0040]

[0041] The input of the constraint task is obtained by

[0042]

[0043] Represents the world coordinate system Σ w To the underwater vehicle coordinate system Σ v The velocity transformation matrix; Represents the inverse of the joint constraint Jacobian matrix.

[0044] Task allocation stage, hybrid shared whole body collaborative control, autonomous system input The PID controller is calculated as

[0045]

[0046] Among them, V rd represents the velocity input to the autonomous system, ν rd and ω rd represents the linear velocity and angular velocity of the autonomous system, represents the error between the end of the underwater manipulator and the target object; e p ={x G ,y G ,z G} T and e q They are position error and attitude error respectively

[0047]

[0048] Among them, [γ,β,α] T Represents the target relative to the underwater manipulator coordinate system Σ ue Roll, pitch and yaw angles.

[0049] In the task allocation stage, in the hybrid shared whole-body collaborative control, the operator input V is fused by controlling the weight coefficient λ hd and the input of the autonomous system C rd The coefficient λ is the confidence c of the autonomous system established by considering the robot's historical trajectory and the operator's intention r To calculate, the formula is:

[0050]

[0051] Among them, given the initial state p 0 To the current state p c Current trajectory Distribution of target locations Defined as the confidence of the autonomous system, estimated according to the maximum a posteriori probability,

[0052] In the task allocation stage, the underwater vehicle and underwater manipulator are allocated motion in the hybrid shared whole-body collaborative control, specifically:

[0053] Considering the different dynamic characteristics of the underwater vehicle and the underwater manipulator, the motion distribution weight coefficients η and δ are introduced into the Jacobian matrix J uvms Motion allocation is performed in, and the weighted Jacobian matrix is Expressed as

[0054]

[0055] Among them, U x and U q Represents the task space weight matrix W x and the joint space weight matrix W q The decomposition matrix of

[0056]

[0057] Among them, the constant η adjusts the weight between the position and direction of the Cartesian task at the end of the underwater manipulator, and δ adjusts the motion distribution weight between the underwater vehicle and the underwater manipulator; the motion distribution coefficient δ is defined as a function related to the distance to the target object

[0058]

[0059] Among them, d s represents the initial distance between the end of the underwater manipulator and the target, d c It represents the current distance between the end of the underwater manipulator and the target, and L represents the maximum working distance of the underwater manipulator.

[0060] In the task allocation stage, in the posture optimization task, the image-based visual servoing method adjusts the tracking position of the target on the image plane through manual input.

[0061] In the task priority separation shared control stage, the task priorities are as follows: the highest level is the joint constraint task, followed by the operation task, and finally the posture optimization task;

[0062] A hierarchical structure is established between different tasks, and an enhanced null space projection method of task priority is used to perform multi-task hierarchical control.

[0063] In the force feedback stage, the master manipulator is modeled as:

[0064]

[0065] Among them, F m is the force input to the force feedback device by the operator, F h It is the force feedback to the operator; the force feedback function is designed as follows:

[0066]

[0067] Among them, p m,d represents the desired position in the working area of ​​the main manipulator, mapped from the desired position in the working area of ​​the underwater manipulator; the positive definite matrix and represent the stiffness matrix and damping matrix respectively; F l It is the feedback force that guides the operator away from the joint limits.

[0068] Compared with the prior art, the present invention has the following beneficial effects:

[0069] 1. The present invention uses a task priority separation shared control method to coordinate joint constraint tasks, terminal operation tasks and posture optimization tasks, and at the same time uses a hybrid shared control method in the terminal operation task to achieve composite shared control of the underwater working robot.

[0070] 2. In the operation task, the underwater working robot control task is concentrated at the end of the underwater robotic arm through the whole body controller, and a human-machine input fusion strategy and motion weight distribution method related to the robot trajectory are developed to further reduce the burden on the operator.

[0071] 3. Apply image servo-based methods to adjust the posture of the underwater working robot according to the operator's input and develop force feedback to indicate the operator's intention for autonomous control. BRIEF DESCRIPTION OF THE DRAWINGS

[0072] Figure 1 This is a flow chart of a method for composite human-machine sharing and collaborative control of an underwater working robot according to the present invention;

[0073] Figure 2 Schematic diagram of the composition and corresponding coordinate system of the underwater working robot in an embodiment of the present invention;

[0074] Figure 3 Schematic diagram of autonomous control confidence calculation in an embodiment of the present invention;

[0075] Figure 4 Schematic diagram for selecting the best image feature points based on the direction of the underwater robot arm end;

[0076] Figure 5 It is a flow chart of a sea cucumber grabbing experiment in an embodiment of the present invention;

[0077] Figure 6 is the position trajectory of the end effector in the entire grasping process in the embodiment of the present invention;

[0078] Figure 7 Schematic diagram of changes in motion allocation coefficient δ, weight coefficient λ, feedback force and image feature angle in an embodiment of the present invention;

[0079] Figure 8 Schematic diagram of the completion time of grasping, operator input and eye movement data in a water pool grasping experiment according to an embodiment of the present invention. DETAILED DESCRIPTION

[0080] The present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be pointed out that the embodiments described below are intended to facilitate the understanding of the present invention and do not have any limiting effect on the present invention.

[0081] The underwater working robot has highly redundant degrees of freedom, which enables it to handle multiple tasks simultaneously. Following the concept of separated shared control, the present invention divides the entire underwater working robot task into three subtasks and assigns them to the operator and the autonomous system. Considering the safety of the underwater working robot and the dynamic response characteristics of the vehicle and the manipulator, the present invention decomposes the whole-body task into constraint tasks, operation tasks and posture optimization tasks. The autonomous system is responsible for both constraint tasks and posture optimization tasks. The hybrid shared control method is introduced into the operation task, combining the respective advantages of the operator and the autonomous system to effectively perform the operation task.

[0082] like Figure 1 As shown in the figure, a hybrid human-machine shared collaborative control method for an underwater working robot includes a robot modeling stage, an information collection stage, a motion mapping stage, a task allocation stage, a task priority separation shared control stage, and a force feedback stage. In the task allocation stage, the overall control task is divided into a joint constraint task, a hybrid shared whole-body collaborative control task (i.e., an operation task), and a posture optimization task.

[0083] Robot modeling stage

[0084] The system includes an underwater vehicle, an underwater manipulator, a master end controlling the manipulator and the object. Figure 3 Its corresponding coordinate system is shown as follows. w is the world coordinate system, and the underwater manipulator base coordinate system Σ ub and the terminal coordinate system Σ ue ,Σ v is the underwater vehicle coordinate system, Σ mb and Σ me Respectively represent the base coordinate system and the end coordinate system of the master end manipulator, Σ g is the target object coordinate system.

[0085] For the kinematic model, the configuration of a typical 10-DOF underwater working robot (6-DOF underwater vehicle and 4-DOF underwater manipulator) can be expressed as where η v =[x v ,y v ,z v ,γ v ,β v ,α v ] T Represents the underwater vehicle relative to the world coordinate system Σ w The position and roll-pitch-yaw angle. v =[θ 1 ,θ 2 ,θ 3 ,θ 4 ] T Refers to the joint angle of the underwater manipulator. Assume represents the underwater vehicle relative to its coordinate system Σ v Input linear velocity and angular velocity. Speed ​​of underwater robot The relationship between the end velocity of the underwater manipulator and

[0086]

[0087] in Represents the end of the underwater manipulator relative to its coordinate system Σ ue speed, Represents the world coordinate system Σ w To the underwater vehicle coordinate system Σ v The velocity transformation matrix, is the underwater manipulator relative to its own coordinate system Σ ue The Jacobian matrix of .

[0088] Underwater vehicle relative to Σ ue The Jacobian matrix It can be expressed as

[0089]

[0090] Where R ue,v Indicates that from Σ ue to Σ v The rotation transformation, p ue,v Indicates that from Σ ue to Σ v The translation transformation of . It is T ue,v The adjacency matrix of .

[0091] The inverse kinematics of the underwater operation robot is as follows

[0092]

[0093] in Refers to the pseudo-inverse matrix of the Jacobian matrix

[0094] Information collection stage.

[0095] The goal of the information collection stage is to collect real-time images of the underwater robot's own state and the surrounding workspace. Relevant images are collected through cameras installed on the robot or arranged in the working environment. On the one hand, the collected images are provided to the operator to observe the target object and the surrounding environment, providing reference information for the operator's next action; on the other hand, the real-time images are processed by image recognition technology to obtain the target object position p g , together with the robot state S g Sent to the autonomous control system.

[0096] Motion mapping stage.

[0097] The goal of the motion mapping stage is to map the operator's input actions to the end of the underwater robot's manipulator arm. The main end manipulator uses the Geomagic Touch device to collect the operator's input and map it to the underwater manipulator arm through coordinate transformation. Figure 2 The figure shows the coordinate relationship between the master robot and the slave robot.

[0098] Operator Input for

[0099]

[0100] Where V hd represents the operator's speed input, υ hd and ω hd represents the linear velocity and angular velocity input by the operator, p u,d and R u,d represents the desired position and direction of the end of the underwater manipulator, p u and R u is the current position and direction of the end point V m Indicates the speed of the master robot. The symbol logR represents the matrix logarithm, and the operator [*] ∨ Represents the extraction of the azimuth error vector from the antisymmetric matrix.

[0101] The desired position p of the end of the underwater manipulator u,d is obtained by matching the displacement of the master manipulator relative to its initial position u,d =p m,0 +k s (p m -p m,0 )

[0102] where p m and p m,0 are the current position and initial position of the master robot arm, respectively, s Represents the scale factor used to adjust the scale of the displacement map.

[0103] The desired direction R of the end of the underwater manipulator u,d Mapping

[0104] R u,d =R ub,mb R mb,me R me,ue

[0105] Where R ub,mb Indicates that from Σ ub to Σ mb The rotation transformation, R mb,me Indicates that from Σmb to Σ me The rotation transformation, R me,ue Indicates that from Σ me to Σ ue The rotation transformation of .

[0106] Task allocation stage.

[0107] The task allocation stage is due to the redundant degrees of freedom of the underwater operation robot system, which can achieve multiple tasks at the same time. We divide the overall task into joint constraint tasks, terminal hybrid shared control tasks and posture optimization tasks. The joint constraint task is to ensure the safety of the robot and prevent self-collision. The terminal hybrid shared control task is the core task. Due to the complex underwater environment, the operator still needs to participate, and the hybrid shared control method is adopted. The posture optimization task helps the operator adjust to the preferred perspective, which is conducive to underwater operation.

[0108] (1) Joint constraint task

[0109] The joint constraint objective function of the underwater manipulator is:

[0110]

[0111] in and Represents the maximum and minimum values ​​of the joint. max and q min represents the physical limit of the joint, q th is the safety threshold of the joint. α and β are constants, and α=β=1 is set in use.

[0112] Derivative the joint constraint objective function to obtain the joint constraint Jacobian matrix

[0113]

[0114] To keep the joints of the robot within their limits, the velocities in task space are

[0115]

[0116] For underwater vehicles, constrain the vehicle's position (x, y, depth) to avoid collisions

[0117]

[0118] Where d represents the distance to obstacles (walls, seabed, etc.), th Represents the safety distance threshold.

[0119] The mission speed of the underwater vehicle constraint is

[0120]

[0121] The input of the constraint task can be obtained by

[0122]

[0123] Represents the world coordinate system Σ w To the underwater vehicle coordinate system Σ v The velocity transformation matrix; Represents the inverse of the joint constraint Jacobian matrix.

[0124] (2) Hybrid shared whole-body collaborative control task

[0125] The operation task focuses on the motion control of the end effector of the underwater manipulator. The end of the underwater manipulator is controlled by a hybrid shared control method, combining the input of the operator and the autonomous system. At the same time, a full-body controller is used to coordinate the motion of the underwater vehicle and the underwater manipulator, allowing the operator to focus on the control of the end of the manipulator.

[0126] 1. Autonomous control input

[0127] Autonomous control input It can be calculated by PID controller as

[0128]

[0129] Where V rd represents the velocity input of the autonomous system, v rd and ω rd represents the linear velocity and angular velocity of the autonomous system, Represents the error between the end of the underwater manipulator and the target object. p ={x G ,y G ,z G} T and e q They are position error and attitude error respectively

[0130]

[0131] where [γ,β,α] T Indicates that the target is relative to Σ ue Roll, pitch and yaw angles.

[0132] 2. Input Fusion

[0133] Fusion of human input V by controlling weight coefficient λ hd and the robot input V rd The coefficient λ is calculated by considering the robot’s historical trajectory and the human intention to establish the confidence of autonomous control.

[0134] like Figure 3 As shown, given the initial state p 0 To the current state p c Current trajectory Distribution of target locations Defined as the confidence level of autonomous control, estimated according to the maximum a posteriori probability, Then the confidence level of autonomous control c r

[0135]

[0136] Compute the probability by approximating the integral along the trajectory:

[0137]

[0138] in Indicates that from p i to p j The optimal trajectory, the cost function of the trajectory is the cumulative distance of the trajectory. Indicates the degree to which the robot deviates from the optimal trajectory when the target is determined.

[0139] P(g) is the prior probability of the target, which indicates the certainty of the autonomous control on the target. P(g) is formulated based on the operator's actions. The operator's intention is inferred from the movement of the position and direction of the underwater robot end.

[0140]

[0141] in Indicates the current position of the operator input, Indicates the position last entered by the operator, It represents the target position mapped from the end of the underwater manipulator back to the workspace of the main manipulator. p It represents the operator's position intention. P(g) can be expressed as

[0142] P(g)=α*tanh(-2(I p -1))+(1-α)cosθ

[0143] Where θ represents the angle between the Z-axis direction of the end fixture of the robot and the line connecting the target, which expresses the operator's directional intention. The activation function tanh(x) converts I p The change is enlarged to the range of [-1,1]. α is the coefficient that adjusts the weight of position and direction, and is set to α = 0.75.

[0144] Since it is difficult for autonomous control to autonomously determine the appropriate grasping posture, the direction is controlled only by the operator. The mixed input from the operator and autonomous control is as follows:

[0145] V s =λv rd +(1-λ)v hd

[0146] ω s =ω hd

[0147] in Represents the shared control speed of the underwater manipulator end.

[0148] 3. Movement distribution

[0149] Considering the different dynamic characteristics of the underwater vehicle and the underwater manipulator, the motion distribution weight coefficients η and δ are introduced into the Jacobian matrix J uvms Motion allocation is performed in, and the weighted Jacobian matrix is It can be expressed as

[0150]

[0151] Among them U x and U q Represents the task space weight matrix W x and the joint space weight matrix W q The decomposition matrix of

[0152]

[0153] The constant η adjusts the weight between the position and orientation of the Cartesian task at the end of the underwater manipulator, and δ adjusts the weight of the motion distribution between the underwater vehicle and the underwater manipulator. The motion distribution coefficient δ is defined as a function related to the distance to the target object.

[0154]

[0155] where d s represents the initial distance between the end of the underwater manipulator and the target, d c It represents the current distance between the end and the target, and L represents the maximum working distance of the underwater manipulator.

[0156] Finally, the input of the hybrid shared whole-body collaborative control task can be obtained as

[0157]

[0158] (3) Posture optimization task

[0159] The image-based visual servoing (IBVS) method adjusts the tracking position of the target on the image plane through manual input. u ,s v ] T Represents the feature point p projected on the image plane c,o , η v =[x v ,y v ,z v ,γ v ,β v ,α v ] T Represents the camera relative to Σ c Translation and directional speed. The relationship between the camera speed and the image feature point speed can be simplified as follows

[0160]

[0161] Where L p is the image Jacobian matrix, and the image feature point velocity is

[0162]

[0163] where s d =[s u,d ,s v,d ] T and c =ps u,c ,s v,c ] T They represent the expected position and current position of the image feature points respectively.

[0164] Select the best image feature points s d The process is as Figure 4 As shown. We predefine the n feature points on the image plane as [s 1 ,s 2 ,…,s n ] (For demonstration purposes, n = 2). The origin and Z-axis direction of the underwater manipulator end coordinate system projected onto the image plane is s ue and * Z ue , by calculating the vector Angle between Choose Angle The smallest feature point s i As the optimal feature point s i .

[0165]

[0166] The underwater robot automatically adjusts its posture to provide the appropriate camera view according to the grasping direction required by the operator. v Converted to underwater vehicle speed V v

[0167]

[0168] The input of the pose optimization task can be obtained as

[0169]

[0170] Task priority separation shared control stage

[0171] All subtasks together constitute the overall task of the underwater robot, and each subtask has a different priority. Task priority is introduced as a decoupled shared control method to establish a hierarchy between these tasks. Task priority, as a redundant control method, enables auxiliary tasks to be executed in the null space of the main task. This ensures the completion of the main task while minimizing errors in low-priority tasks.

[0172] To ensure the robot's safety and prevent self-collision, we prioritize the highest-level constraint tasks. The hierarchical control architecture focuses on the manipulation task. We use a full-body controller to directly control the end effector of the underwater manipulator. The vehicle autonomously follows the manipulator, and hybrid shared control is used to further reduce the operator burden. The remaining controllable subspace is used to optimize the underwater robot's posture to track the target object.

[0173]

[0174] The u on the left side of the equation represents the total input of the robot, and the right side of the equation contains the inputs of the three tasks. Low-priority tasks will not interfere with the execution of high-priority tasks.

[0175] The enhanced null space projection method with task priority is used to perform multi-task hierarchical control. and The solution of the augmented null space projection is

[0176]

[0177] Among them J i It is task x i The Jacobian matrix of Represents task x 1 The null space of this formula can also be recursively extended to n tasks

[0178]

[0179] in Null space projection matrix representing the augmented Jacobian matrix Projection Matrix P i Can be obtained recursively

[0180]

[0181] Where P 0 =I.

[0182] Force feedback stage.

[0183] When the operator is operating remotely, force feedback is provided to help the operator understand the robot's intentions. The master manipulator is modeled as:

[0184]

[0185] F m is the force input to the force feedback device by the operator, F h It is force feedback to the operator.

[0186] The force feedback is designed with two goals. First, it should guide the operator away from the constraints of the robot system (such as joint limits). Second, the operator should feel the ideal position for sharing the control system. The force feedback function is designed as follows

[0187]

[0188] where p m,d represents the desired position in the workspace of the main manipulator, mapped from the desired position in the workspace of the underwater manipulator. and denote the stiffness matrix and the damping matrix respectively. The attraction hint at the desired location indicates the intention of the shared control system. m,d The calculation is as follows

[0189]

[0190] F l is the feedback force that guides the operator away from the joint limit. Considering that the master manipulator has only three motors to provide force feedback, the Jacobian matrix J is used. m,v The linear part of the joint limit feedback force is projected onto the master end robot workspace

[0191]

[0192] In order to verify the effectiveness of the control method of the present invention, a grasping experiment was conducted in a water pool. The configuration of the underwater working robot used in the experiment reflects the configuration of the simulation, including a 6-DOF underwater vehicle and a 4-DOF Reach5 Mini underwater manipulator. VINS Mono served as the positioning method attempted in this experiment, and hand-eye calibration was performed before the experiment. The drivers for the underwater vehicle and underwater manipulator were implemented using ROS. The console contained the main control computer, the Geometry Touch main manipulator, and the eye tracker for collecting operator data. A one-way analysis of variance (ANOVA) with a significance threshold of α = 0.05 was used to analyze the differences in each indicator under different control methods.

[0193] Figure 5 The process of capturing the sea cucumber model is shown in the figure. Figure 1-6 The process of the underwater robot approaching and grabbing the sea cucumber model is shown respectively.

[0194] Figure 6 represents the position trajectory of the end effector during the entire grasping process. Specifically, Figure 6 (ab) shows the tracking error and 3D trajectory of the end effector. The final stable recognized pose is selected as the target position. Figure 6 As shown in (a), slight manual adjustments were made between about 10 and 15 seconds. During this period, the distance between the end effector and the target continued to increase, and then gradually approached the target. After fine-tuning, the target was successfully grasped in about 40 seconds. The fake sea cucumber grasping experiment verified the feasibility of the composite shared control algorithm for underwater operation robots.

[0195] Figure 7 The changes of motion distribution coefficient δ, weight coefficient λ, feedback force and image feature angle are shown. As the distance decreases, Figure 7 The motion weight coefficient in (a) changes toward the underwater manipulator. The operator's intervention at about 15 seconds causes the motion distribution weight coefficient to change. Figure 7 (b) shows that the control weight of autonomous control starts from a low value of about 0.25 in the initial stage. Subsequently, the weight of autonomous control increases rapidly. The operator intervenes at about 15 seconds, causing the control weight coefficient of autonomous control to drop rapidly. During this stage, the control weight gradually shifts to the operator, consistent with the phenomenon described previously. After the operator's adjustment, the control weight coefficient begins to increase until the robot completely dominates the control at the end of the stage.

[0196] The change of feedback force is as follows Figure 7 As shown in (c), the feedback force shows a trend of gradually decreasing. Figure 7The change of the position error shown in (a) is consistent. In addition, due to the operator's control, the feedback force will experience a slight fluctuation at about 15 seconds. Figure 7 (d) depicts the behavior of the feature angle. After about 10 seconds of operator adjustment, the pose optimization task adjusts towards the desired camera view.

[0197] The present invention recruited 9 volunteers to participate in the pool grasping experiment. The volunteers completed grasping by both full manual control and shared control, and the achieved data were compared. The grasping completion time, operator input and eye movement data are shown in Figure 2. Figure 8 shown.

[0198] like Figure 8 As shown in (a), the average task completion time for manual control is 40.128 seconds, and the average task completion time for shared control is 33.894 seconds. The average task runtime is reduced by 15.53%. There is a significant difference in completion time between the two control methods (F(1,16)=7.7, p=0.0135).

[0199] Figure 8 (b) shows that the average input trajectory length of the operator input for manual control is 0.43247, and the operator input for shared control is 0.30669, which is a 29.08% reduction in average input trajectory length. There is a significant difference in input trajectory length between the two control methods (F(1,16)=7.03, p=0.0174).

[0200] Figure 8 (c) shows that the average eye track length of the operator is 211670.4 for manual control and 140559.4 for shared control, and the average eye track length is reduced by 33.59%. There is a significant difference in the average eye track length between the two control methods (F(1,16)=9.84, p=0.0064).

[0201] It can be seen from the comparative experiments that the shared control of the present invention can effectively reduce the operation time and operator input, reduce the burden on operators and improve efficiency.

[0202] The embodiments described above provide a detailed description of the technical solutions and beneficial effects of the present invention. It should be understood that the above are only specific embodiments of the present invention and are not intended to limit the present invention. Any modifications, supplements and equivalent substitutions made within the scope of the principles of the present invention should be included in the protection scope of the present invention.

Claims

1. A hybrid human-machine shared collaborative control method for an underwater working robot, characterized in that: It includes the following six stages: (1) Robot modeling stage The underwater operation robot is divided into underwater vehicle, underwater manipulator and main end manipulator, and the world coordinate system ∑ w , underwater vehicle coordinate system∑ v , the base coordinate system of the underwater manipulator ∑ ub and the terminal coordinate system∑ ue , the base coordinate system of the main end manipulator∑ mb and the terminal coordinate system∑ me As well as the coordinate transformation relationship between different coordinate systems, and further establish the kinematic model of the underwater operation robot; (2) Information Collection Stage Collect real-time images of the underwater robot's own state and surrounding workspace; the real-time images collected by the camera are provided to the operator to observe the target object and the surrounding environment; on the other hand, the real-time images are processed by image recognition technology to obtain the position of the target object, and sent to the autonomous system together with the robot's own state; (3) Motion mapping stage The main end manipulator collects the operator's input and maps it to the underwater manipulator through coordinate transformation; (4) Task allocation stage The tasks are divided into joint constraint tasks, hybrid shared whole-body collaborative control tasks and posture optimization tasks; among them, the joint constraint tasks are controlled by the autonomous system to prevent the robot from white collision; the hybrid shared whole-body collaborative control tasks focus on the motion control of the end of the underwater manipulator, which is controlled by a hybrid shared control method, integrating the input of the operator and the autonomous system, and allocating the motion of the underwater vehicle and the underwater manipulator; the posture optimization task is controlled by the autonomous system to help the operator adjust to the preferred perspective; (5) Task priority separation shared control stage Introducing task priorities as a method of decoupled shared control to establish a hierarchy between different tasks; (6) Force feedback stage When the operator operates the main end manipulator to remotely operate the underwater manipulator, force feedback is provided to help the operator understand the robot's intentions and guide the operator's operations.

2. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 1, characterized in that: Motion Mapping Phase, Operator Input for Among them, V ha represents the operator's velocity input, v hd and ω hd represents the linear velocity and angular velocity input by the operator, p u，d and R u，d represents the desired position and direction of the end of the underwater manipulator, p u and R u is the current position and direction of the end, V m Indicates the speed of the master manipulator; the symbol logR represents the matrix logarithm, and the operator [*] ∨ It means extracting the azimuth error vector from the antisymmetric matrix; The desired position p of the end of the underwater manipulator u，d is obtained by matching the displacement of the master manipulator relative to its initial position p u，d =p m,0 +k s (p m -p m,0 ) Among them, p m and p m,0 are the current position and initial position of the master robot arm, respectively, s Represents the scale factor, which is used to adjust the scale of the displacement mapping; The desired direction R of the end of the underwater manipulator u，d Mapping R u，d =R ub,mb R mb,me R me，ue Among them, R ub，mb Represents the coordinate system of the underwater manipulator ∑ ub To the main end robot arm base coordinate system∑ mb The rotation transformation, R mb,me Represents the base coordinate system of the slave robot arm ∑ mb To the main end robot arm end coordinate system∑ me The rotation transformation, R me，ue Represents the coordinate system of the end arm of the master end ∑ me To the underwater manipulator end coordinate system∑ ue The rotation transformation of .

3. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 1, characterized in that: In the task allocation stage, the joint constraint tasks specifically include: The joint constraint objective function of the underwater manipulator is: in, and Represents the maximum and minimum values ​​of the set joint; q max and q min represents the physical limit of the joint, q th is the safety threshold of the joint; α and β are constants, and α=β=1 is set in use; Derivative the joint constraint objective function to obtain the joint constraint Jacobian matrix J cm (q); To keep the joints of the robot within their limits, the velocities in task space are For underwater vehicles, restrict the vehicle's position to avoid collisions Where d represents the distance to the obstacle, d th represents the safety distance threshold, and the mission speed constrained by the underwater vehicle is The input of the constraint task is obtained by Represents the world coordinate system ∑ w To the underwater vehicle coordinate system∑ v The velocity transformation matrix; Represents the inverse of the joint constraint Jacobian matrix.

4. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 1, characterized in that: Task allocation stage, hybrid shared whole body collaborative control, autonomous system input The PID controller is calculated as Among them, V rd represents the velocity input of the autonomous system, u rd and ω rd represents the linear velocity and angular velocity of the autonomous system, represents the error between the end of the underwater manipulator and the target object; e p ={x G ,y G , z G } T and e q They are position error and attitude error respectively Among them, [γ, β, α] T Represents the target relative to the underwater manipulator coordinate system ∑ ue Roll, pitch and yaw angles.

5. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 4, characterized in that: In the task allocation stage, in the hybrid shared whole-body collaborative control, the operator input V is fused by controlling the weight coefficient λ hd and the autonomous system input V rd The coefficient λ is the confidence c of the autonomous system established by considering the robot's historical trajectory and the operator's intention r To calculate, the formula is: Among them, given from the initial state p0 to the current state p c Current trajectory Distribution of target locations Defined as the confidence of the autonomous system, estimated according to the maximum a posteriori probability, 6. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 5, characterized in that: In the task allocation stage, the underwater vehicle and underwater manipulator are allocated motion in the hybrid shared whole-body collaborative control, specifically: Considering the different dynamic characteristics of the underwater vehicle and the underwater manipulator, the motion distribution weight coefficients η and δ are introduced into the Jacobian matrix J uvms Motion allocation is performed in, and the weighted Jacobian matrix is Expressed as Among them, U x and U q Represents the task space weight matrix W x and the joint space weight matrix W q The decomposition matrix of the weight matrix is ​​designed as Among them, the constant η adjusts the weight between the position and direction of the Cartesian task at the end of the underwater manipulator, and δ adjusts the motion distribution weight between the underwater vehicle and the underwater manipulator; the motion distribution coefficient δ is defined as a function related to the distance to the target object Among them, d s represents the initial distance between the end of the underwater manipulator and the target, d c It represents the current distance between the end of the underwater manipulator and the target, and L represents the maximum working distance of the underwater manipulator.

7. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 1, characterized in that: In the task allocation stage, in the posture optimization task, the image-based visual servoing method adjusts the tracking position of the target on the image plane through manual input.

8. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 1, characterized in that: In the task priority separation shared control stage, the task priorities are as follows: the highest level is the joint constraint task, followed by the operation task, and finally the posture optimization task; A hierarchical structure is established between different tasks, and an enhanced null space projection method of task priority is used to perform multi-task hierarchical control.

9. The hybrid human-machine shared collaborative control method for underwater working robots according to claim 1, characterized in that: In the force feedback stage, the master manipulator is modeled as: Among them, F m is the force input to the force feedback device by the operator, F h It is the force feedback to the operator; the force feedback function is designed as follows: Among them, p m，d represents the desired position in the working area of ​​the main manipulator, mapped from the desired position in the working area of ​​the underwater manipulator; the positive definite matrix and represent the stiffness matrix and damping matrix respectively; F z It is the feedback force that guides the operator away from the joint limits.

Images