Dual-arm cooperative potential field-based force-guided teleoperation system and control method

Abstract

A dual-arm cooperative potential field-based force-guided teleoperation system and control method. By means of real-time poses of tool center points of two robotic arms, whether the shortest distances dp-s,i between a target object and workspace boundaries of the two arms are lower than a threshold Ds is iteratively determined; if the shortest distances are both greater than the threshold, a dual-arm symmetric cooperation strategy is adopted, and if the shortest distances are lower than the threshold, a single-arm prioritized cooperation strategy is adopted, and the arm is used as a priority arm; cooperation factors δi of the two arms are respectively determined on the basis of the cooperation strategies; a dual-arm cooperative potential field is constructed on the basis of the cooperation factors, the distances between the target object and the tool center points of the two arms, and the position of an obstacle; and corresponding dynamic virtual constraint forces are generated on the basis of the dual-arm cooperative potential field, and two force feedback hand controllers are used to guide and assist an operator in controlling the two robotic arms to complete a dual-arm cooperative task.

Description

Force-guided teleoperation system and control method based on dual-arm collaborative potential field Technical Field

[0001] The present invention belongs to the technical field of space teleoperation control, and mainly relates to a force-guided teleoperation system and a control method based on a dual-arm collaborative potential field. Background Art

[0002] In recent years, teleoperation technology has been widely used in complex and dynamic environments with diverse mission requirements, such as deep space and deep-sea exploration, live power inspections, and nuclear, biological, and chemical facility operations. However, due to the current limitations of robotic control and sensor technology, the development of fully autonomous robots is difficult. Therefore, the trend is to develop teleoperated robots to replace human operators in these hazardous environments. Force feedback hand controllers allow operators to sense the force interaction between the robot and the environment during operation. Force feedback is an important means of enhancing the sense of presence and transparency of teleoperation systems, broadening the scope of human-machine interaction.

[0003] However, the mapping between the operational space and joint space of a robotic arm with more than six degrees of freedom is non-explicit and complex. Furthermore, since the human wrist and arm form a coupled system, their movements often affect each other. Therefore, it is often difficult for a human to directly teleoperate the robotic arm to achieve the desired position and posture, and position errors are inevitable during operation. In recent years, virtual force guidance methods based on traditional virtual fixtures and artificial potential fields have become effective solutions to these problems. However, these methods often only consider the operation of a single robotic arm, and there are no virtual force guidance methods for dual-arm collaborative teleoperation systems. Dual-arm robots have higher degrees of freedom and greater operational flexibility. Compared to traditional single-arm robots, dual-arm robots with coordinated operation capabilities are superior and have broader application prospects when performing complex tasks such as handling and assembly. Consequently, teleoperation of dual-arm robots is more difficult, often requiring the operator to multitask and simultaneously control both robotic arms to complete coordinated tasks, including obstacle avoidance, approaching the target, adjusting the working posture, and closed-loop coordinated motion of the two arms. This places a significant burden on the operator's physical and mental state.

[0004] Furthermore, most current force guidance methods construct virtual fixtures based on predefined structured environments, making them unsuitable for complex and unstructured environments. Alternatively, they construct attractive and repulsive fields based on parabolic functions, which are prone to local optimality, where the combined attractive and repulsive forces acting on the end of the robotic arm are zero, preventing it from reaching the target point. Furthermore, most current force guidance methods fail to consider the impact of the robotic arm's workspace on the task at hand.

[0005] Summary of the Invention

[0006] The present invention addresses the shortcomings of the existing technology and discloses a force-guided teleoperation system and control method based on a dual-arm collaborative potential field. The real-time position and posture of the tool center points of the two robotic arms are obtained through the kinematic model of the robotic arms, and the shortest distance d between the target object and the workspace boundary of the dual arms is cyclically determined. p-s,i Is it below the threshold D? s , if both are higher than the threshold, the two-arm symmetrical collaboration strategy is adopted; if the distance between the boundary of the workspace and the two arms is lower than the threshold, the single-arm priority collaboration strategy is adopted, and the arm is used as the priority arm; the collaboration factor δ of the two arms is determined according to the collaboration strategy i ; The method of the present invention determines the collaboration strategy based on the environmental point cloud image, the posture information of the center point of the dual-arm end tool and the dual-arm workspace, and then dynamically updates the collaboration factor; constructs the dual-arm collaboration potential field according to the collaboration factor, the distance between the target object and the center point of the dual-arm end tool and the position of the obstacle; generates the corresponding dynamic virtual constraint force based on the dual-arm collaboration potential field, and guides and assists the operator to control the two robotic arms to complete the dual-arm collaboration task through two force feedback hand controllers, which can improve the operator's control accuracy, work efficiency and safety factor during human-machine collaboration, and is more adaptable to complex unstructured environments.

[0007] In order to achieve the above-mentioned object, the technical solution adopted by the present invention is as follows: a force-sensing guided teleoperation system based on a dual-arm collaborative potential field, comprising two robotic arms, a force feedback hand controller, a router, a vision unit, a slave computer, a master computer, a display and a mobile platform;

[0008] The two robotic arms are respectively mounted on a mobile platform and connected to a slave computer;

[0009] The force feedback hand controller is located on the operating table and connected to the slave computer. It is used to collect the six-degree-of-freedom posture and control instructions of the robotic arm in the Cartesian coordinate system and send them to the master computer. It receives the dynamic virtual constraint force data generated by the dual-arm collaborative potential field force guidance control module algorithm of the master computer and feeds it back to the operator.

[0010] The router is responsible for establishing a local area network for the entire control system, enabling real-time data exchange between the robotic arm, slave computer, vision unit, router, master computer, and force feedback hand controller;

[0011] The visual unit uses a structured light camera, which is located on a mobile platform, installed between the two robotic arms, and connected to the slave computer. It is used to collect point cloud information of the robot's surrounding environment in real time and transmit it to the master computer;

[0012] The slave computer is located inside the mobile platform and includes a robotic arm drive module, a mobile platform control module, and a network communication module, and also includes integration and communication functions between the control modules;

[0013] The main computer is located next to the operating console and includes a force feedback hand controller drive module, a dual-arm collaborative potential field force guidance control module, a robotic arm kinematic model, a robotic arm dynamic model, a point cloud information processing module, and a network communication module. It also includes integration and communication functions between the various control modules.

[0014] The display screen is located in front of the operating console and is used to display point cloud images and virtual restraint force data;

[0015] The mobile platform is used to install the robotic arm, the vision unit and the slave computer.

[0016] In order to achieve the above-mentioned object, the present invention also adopts a technical solution: a force-guided teleoperation control method based on a dual-arm collaborative potential field, comprising the following steps:

[0017] S1: Before the operation begins, the workspaces of the two arms are determined separately using the Monte Carlo method based on the kinematic models of the two arms, and the boundary coordinates of the workspaces of the two arms are solved and recorded;

[0018] S2: Obtain environmental point cloud information and segment the point cloud based on image features; obtain the location of obstacles and target objects based on the point cloud image, and use the bounding box algorithm to determine the location and attitude angle of each target point of the two arms based on the shape of the target object's point cloud bounding box;

[0019] S3: The real-time position of the tool center point of the two robot arms is obtained through the robot arm kinematic model, and the shortest distance d between the target object and the boundary of the dual-arm workspace is determined cyclically. p-s,i Is it below the threshold D? s :

[0020] If the shortest distance d between the target object and the boundary of the dual-arm workspace p-s,i If both are higher than the threshold, a two-arm symmetrical cooperation strategy is adopted. At this time, the cooperation factor δ of the left and right arms i is 1;

[0021] If the shortest distance d between the target object and the boundary of the dual-arm workspace p-s,i Less than threshold D s , then adopt a single-arm priority cooperation strategy, and use this arm as the priority arm, with a cooperation factor of δ1 = 0.5 for the priority arm and a cooperation factor of δ1 = 2 for the other arm;

[0022] S4: Construct the dual-arm collaborative potential field based on the relative posture of the tool center point at the end of the dual-arm and the respective target points, and generate the virtual position attraction F through the force feedback hand controller a,i (P t,i ), guiding the operator to control the end-of-arm tool to approach the target point;

[0023] S5: Loop to determine whether the center point of the tool at the end of the robot arm is close to the obstacle. If so, it enters the obstacle avoidance phase and constructs a repulsive potential field within a certain range outside the obstacle point cloud area. This field is then used to generate a virtual repulsive force F through the force feedback hand controller. r,i,j (d p,i-ob,j ), combined with the virtual position attraction F approaching the target point a,i (P t,i ) assists the operator in controlling the robotic arm to avoid obstacles; otherwise, returns to step S4;

[0024] S6: Circularly judge whether the tool center point at the end of the robot arm reaches the vicinity of the target point. If so, enter the stage of adjusting the working posture. According to the relative posture angle between the end of the robot arm and the corresponding target point, a virtual posture attraction force F is generated through the force feedback hand controller. z,i (r t,i ), and the virtual position attraction F of step S4 a,i (P t,i ) is combined into a virtual posture attraction F vr,i , guide the operator to accurately adjust the end position of the robot arm to perform the task; otherwise, return to step S4;

[0025] S7: Circularly determine whether a certain closed-chain constraint relationship is formed between the two arms. If so, the two arms are in the double-arm closed-chain collaborative motion stage. Record and continuously update the relative posture of the tool center point at the end of the two arms. Generate a double-arm relative impedance model based on the double-arm collaborative potential field. Generate a virtual relative impedance constraint force F through the force feedback hand controller. rc (t) assists the operator in controlling the coordinated movement of the two arms so that the relative positions of the ends of the two arms do not change significantly and maintain synchronous movement.

[0026] As an improvement of the present invention, in step S5, the method for judging whether the center point of the tool at the end of the manipulator is close to the obstacle is as follows: calculating the center point P of the tool at the end of the manipulator t The distance d between the coordinates of and the obstacle p-ob , determine whether the value is lower than the obstacle avoidance threshold D ob , if so, construct a repulsive potential field;

[0027] In step S6, the method for determining whether the center point of the tool at the end of the manipulator reaches the vicinity of the target point is: whether the coordinates of the center point P of the tool at the end of the manipulator are within the point cloud area of ​​the target point, and within the range of the area, a virtual position attraction and a virtual posture attraction are simultaneously generated;

[0028] In step S7, the method for determining whether a certain closed-chain constraint relationship is formed between the two arms is: based on the open and closed state of the two-arm grippers and the area of ​​the region where the point cloud contour of the grippers intersects with the point cloud contour of the target object, cyclically detecting whether the two arms form a whole with the target object and start coordinated movement.

[0029] As another improvement of the present invention, the virtual position attraction F in step S4 a,i (P t,i ) and the virtual repulsive force F in step S5 r,i,j (d p,i-ob,j ) are all 3D forces and do not include torque; the virtual posture attraction F in step S6 vr,i Contains force and torque, which is a 6-dimensional force; the virtual relative impedance constraint force F in step S7 rc (t) includes force and torque, and is a 6-dimensional force.

[0030] As another improvement of the present invention, in step S5, the minimum distance d between the center point of the robot end tool and the obstacle is p-ob The calculation method is as follows:

[0031] S51: Assume that the center coordinate of the end tool of the robot arm is P t =(x t ,y t ,z t ), the vertex Q of the bounding box model of the obstacle point i =(x i ,y i ,z i ), calculate P t Go to each vertex Q of the bounding box respectively i The distance λ1;

[0032] S52: Get the vector n of the bounding box edge i , find P t Vector m to each vertex i , and then judge n i and m i If the angle between them is an obtuse angle, it is discarded; if it is an acute angle, the shortest distance λ2 is calculated as:

[0033] S53: Take any three vertices C1, C2, and C3 on each face, and let P t The foot of the perpendicular to each surface is P c =(x c ,y c ,z c ), according to P t P c ⊥C1C2, P t P c ⊥C2C3, P t P c ⊥C1C3 to find P cThe coordinate value of the vertical foot is determined according to the coordinates of the vertices on the surface, and finally the shortest distance λ3 is obtained as:

[0034] S54: Take the minimum value among λ1, λ2, and λ3 as the distance d from the center point of the tool at the end of the robot arm to the obstacle p-ob : d p-ob =min(λ1,λ2,λ3).

[0035] As another improvement of the present invention, in step S4, the calculation formula of the dual-arm collaboration potential field is as follows:

[0036] Where U a,i (P t,i ) represents the dual-arm collaborative potential field, δ i represents the cooperation factor of the i-th robot arm in the dual-arm cooperation potential field, R represents the distance between the center point of the tool at the end of the i-th robot arm and its corresponding target point, t,i The radius of the point cloud area sphere representing the i-th target point;

[0037] The virtual location attraction F a,i (P t,i ) is calculated as follows:

[0038] The above formula represents the attractive force exerted on the i-th robotic arm, and the force feedback hand controller generates a virtual position attractive force to feed back to the operator.

[0039] As another improvement of the present invention, in step S5, the calculation formula of the repulsive potential field is as follows:

[0040] Where U r,j,i (d p,i-ob,j ) represents the repulsive potential field of the j-th obstacle, ε represents the adjustment factor of the repulsive potential field, d p,i-ob,j Denotes the distance between the j-th obstacle and the center point of the tool at the end of the i-th robot arm, D ob,j represents the range of the repulsive potential field of the j-th obstacle;

[0041] The calculation formula of the virtual repulsive force is as follows:

[0042] The above formula indicates that the i-th robot arm is subjected to the repulsive force of the j-th obstacle; It indicates that the i-th robot arm receives the repulsive force of a total of n obstacles, and generates a virtual repulsive force through the force feedback hand controller and feeds it back to the operator;

[0043] The virtual constraint force formula for the obstacle avoidance task of the i-th robotic arm is as follows:

[0044] As a further improvement of the present invention, in step S6, the calculation formula of the virtual posture attractiveness is as follows:

[0045] Where, re i =r t,i -r o,i , represents the posture angle r of the i-th robot arm at time t t,i The attitude angle r of the corresponding target point o,i The relative posture of represents the relative angular velocity of the two, K is the stiffness coefficient of the Kelvin-Voigt linear model, and B is the damping coefficient of the Kelvin-Voigt linear model;

[0046] The formula for calculating the 6-dimensional virtual posture attraction force on the operator when accurately adjusting the posture of the end of the robotic arm is as follows:

[0047] Where, F vr,i is a 6×1 matrix, F a,i and F z,i Both are 3×1 matrices.

[0048] As a further improvement of the present invention, in step S7, the calculation principle of the virtual relative impedance constraint force is as follows: first, the relative posture ce(0) of the center point of the end tool when the two arms just form a closed chain constraint is recorded as the equilibrium position of the two-arm relative impedance model; then the relative posture ce(0) of the center point of the end tool is updated in a circular manner. The difference between the two positions and the equilibrium position is calculated, and the difference is finally substituted into the double-arm relative impedance model. The calculation formula for the virtual relative impedance constraint force is as follows:

[0049] Where, e(t) = ce(t) - ce(0) is the difference between the relative position of the tool center points of the two manipulators at time t and the relative position ce(0) when the two arms just form a closed chain constraint, where ce(t) is a 6×1 matrix, x(t) and r(t) are both 3×1 matrices, and M d 、B d , K d are the inertia, damping and stiffness coefficients of the double-arm relative impedance model, respectively.

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

[0051] (1) Compared with the traditional artificial potential field method function, the dual-arm collaborative potential field function of the present invention solves the local optimal problem and has a better guiding effect.

[0052] (2) The control method of the present invention decomposes the dual-arm collaboration task into four task stages: obstacle avoidance, target approach, working posture adjustment, and dual-arm closed-chain coordinated movement. For each task stage, a corresponding dynamic virtual constraint force is generated based on the dual-arm collaboration potential field, which can improve the operator's control accuracy, work efficiency, and safety factor during human-machine collaboration, and can better adapt to complex unstructured environments.

[0053] (3) The present invention takes into account the influence of the working space of the robot arm on the dual-arm collaboration strategy, and then dynamically updates the collaboration factor to adjust the attractive force generated by the dual-arm collaboration potential field, which conforms to the control characteristics of the dual-arm collaboration and has strong practicality.

[0054] (4) The remote operation system designed by the present invention has the advantages of low latency, high precision, high stability, high safety, six-dimensional force perception and guidance, and adopts dual-arm collaboration with a high degree of freedom, which can complete more complex tasks and better meet the needs of practical applications. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] FIG1 is an overall block diagram of a force-sensing guided teleoperation system based on a dual-arm collaborative potential field according to the present invention;

[0056] FIG2 is a schematic diagram of force-sensing guidance based on the dual-arm collaborative potential field during the obstacle avoidance stage and the target approaching stage in the method of the present invention;

[0057] FIG3 is a schematic diagram of force guidance based on the double-arm collaborative potential field during the double-arm closed-chain coordinated motion stage in the method of the present invention;

[0058] Among them, 1-structured light camera; 2-mobile platform; 3-left robotic arm; 4-obstacle repulsion field range; 5-target points of both arms; 6-force guidance trajectories of both arms; 7-right robotic arm; 8-origin of the system world coordinate system; 9-relative impedance model of both arms; 10-destination of closed-chain collaborative motion of both arms. DETAILED DESCRIPTION

[0059] The present invention will be further described below with reference to the accompanying drawings and specific embodiments. It should be understood that the following specific embodiments are only used to illustrate the present invention and are not used to limit the scope of the present invention.

[0060] Example 1

[0061] A force-guided teleoperation system based on a dual-arm collaborative potential field, as shown in FIG1 , includes a left robotic arm 3, a right robotic arm 7, a force feedback hand controller, a router, a vision unit, a slave computer, a master computer, a display, and a mobile platform 2. The left robotic arm 3 and the right robotic arm 7 are both two six-degree-of-freedom robotic arms, which are respectively mounted on the mobile platform 2 and connected to the slave computer via a network cable. The working parts of the robotic arms can be grippers, suction cups, or other end tools that meet the task requirements. The working parts of the robotic arms are connected to the slave computer via a data cable.

[0062] The force feedback hand controller uses two six-degree-of-freedom force feedback hand controllers, which are located on the operating table and connected to the slave computer via a data cable. They are used to collect the six-degree-of-freedom posture in the Cartesian coordinate system and the end-of-arm tool control instructions and send them to the master computer. They receive the dynamic virtual constraint force data generated by the dual-arm collaborative potential field force guidance control module algorithm of the master computer and feed it back to the operator, realizing the guidance and assistance functions for the operator.

[0063] The router is responsible for establishing the local area network of the entire control system, enabling real-time data exchange between the robotic arm, slave computer, vision system, router, master computer, and force feedback hand controller. The slave computer and master computer are connected to the router wirelessly to transmit control data, robotic arm kinematic solution data, and point cloud information data collected by the vision system.

[0064] The vision unit uses a structured light camera 1, which is located on a mobile platform 2 and installed between the two robotic arms. It is connected to the slave computer via a data cable and is used to collect point cloud information of the robot's surroundings in real time and transmit it to the master computer.

[0065] The slave computer is located inside the mobile platform 2 and integrates the robot drive module, mobile platform control module, network communication module, and also includes the integration and communication functions between the control modules;

[0066] The main computer is located next to the operating console and integrates the force feedback hand controller drive module, the dual-arm collaborative potential field force guidance control module, the robot arm kinematic model, the robot arm dynamic model, the point cloud information processing module, and the network communication module. It also includes the integration and communication functions between the various control modules.

[0067] The display screen is located in front of the operating console and is used to display point cloud images and virtual constraint force data;

[0068] Mobile platform 2 is used to install the robotic arm, vision unit and slave computer.

[0069] When using the system of this embodiment, there are two collaboration strategies: dual-arm symmetrical collaboration and single-arm priority collaboration. These two strategies are targeted at different robot task scenarios and are determined by the pose information of the tool center point at the end of the dual arms, the position of the target object, and the dual-arm workspace. They can be switched dynamically during the robot's operation.

[0070] 1) Symmetrical collaboration of two arms means that when the distance between the target object and the working space boundary of the two arms is greater than the threshold D s When , the two arms should keep moving synchronously and reach the target points of both arms at the same time. At this time, the cooperation factor δ of the left and right arms is i All are 1;

[0071] 2) Single-arm priority collaboration means that when the shortest distance d between the target object and the boundary of the workspace of the two arms is p-s,i Less than threshold D s When , the arm, as the priority arm, should reach its target point first and move the target object to a suitable position to facilitate the other arm to reach the target point. At this time, the attraction of the priority arm should be greater than that of the other arm, and the cooperation factor of the priority arm δ1 = 0.5, and the cooperation factor of the other arm δ1 = 2.

[0072] The system of this embodiment determines the collaboration strategy based on the environmental point cloud image, the posture information of the center point of the dual-arm end tool, and the dual-arm workspace, and then dynamically updates the collaboration factor; constructs a dual-arm collaboration potential field based on the collaboration factor, the distance of the target object from the center point of the dual-arm end tool, and the position of the obstacle; generates a corresponding dynamic virtual constraint force based on the dual-arm collaboration potential field, and guides and assists the operator to control the two robotic arms to complete the dual-arm collaboration task through two force feedback hand controllers, which can improve the operator's control accuracy, work efficiency, and safety factor during human-machine collaboration, and is more adaptable to complex unstructured environments. The remote operation system designed by the present invention has the advantages of low latency, high precision, high stability, high safety, six-dimensional force perception and guidance, and adopts dual-arm collaboration, with a high degree of freedom, and can complete more complex work tasks, which is more in line with the needs of practical applications.

[0073] Example 2

[0074] A control method for a force-guided teleoperation system based on a dual-arm collaborative potential field is disclosed. This control method provides a dynamic virtual constraint force to the operator using a six-degree-of-freedom force feedback hand controller. The six-degree-of-freedom force feedback hand controller can be a Force Dimension Sigma.7 master hand controller. The method specifically includes the following steps:

[0075] Step S1: Analyze the dual-arm workspace. Before the operation begins, the workspaces of the two arms are determined using the Monte Carlo method based on their kinematic models. The coordinates of the workspace boundaries of the two arms are calculated and recorded. In this embodiment, the robotic arm is kinematically simulated using the Robotic Toolbox in MATLAB to calculate and record the coordinate values ​​of the workspace boundaries of the two arms.

[0076] Step S2: Obtaining and processing environmental point cloud information. Use structured light camera 1 to obtain environmental point cloud information and segment the point cloud based on image features. Obtain the positions of obstacles and target objects based on the point cloud image. Use a bounding box algorithm to simplify the point cloud shapes of obstacles and target objects. Use the point cloud bounding box shape of the target object to determine the position and attitude angle of each target point 5 of the two arms.

[0077] Step S3: Determine the collaboration strategy and collaboration factor. The real-time position of the tool center point of the left robot arm 3 and the right robot arm 7 is obtained through the robot arm kinematic model, and the shortest distance d between the target object and the workspace boundary of the two arms is determined cyclically. p-s,i Is it below the threshold D? s , if both are higher than the threshold, the two-arm symmetrical collaboration strategy is adopted; if the distance between the boundary of the workspace and the two arms is lower than the threshold, the single-arm priority collaboration strategy is adopted, and the arm is used as the priority arm; the collaboration factor δ of the two arms is determined according to the collaboration strategy i ;

[0078] There are two collaborative strategies in the actual operation: dual-arm symmetrical collaboration and single-arm priority collaboration. These two strategies are designed for different robot operation scenarios and are determined by the pose information of the tool center point at the end of the dual arms, the position of the target object, and the dual-arm workspace. They can be switched dynamically during the robot's operation.

[0079] 1) Symmetrical collaboration of two arms means that when the distance between the target object and the working space boundary of the two arms is greater than the threshold D s When , the two arms should keep moving synchronously and reach the target points of both arms at the same time. At this time, the cooperation factor δ of the left and right arms is i All are 1;

[0080] 2) Single-arm priority collaboration means that when the shortest distance d between the target object and the boundary of the workspace of the two arms is p-s,i Less than threshold D s When , the arm, as the priority arm, should reach its target point first and move the target object to a suitable position to facilitate the other arm to reach the target point. At this time, the attraction of the priority arm should be greater than that of the other arm. The cooperation factor of the priority arm is δ1 = 0.5, and the cooperation factor of the other arm is δ1 = 2.

[0081] Step S4: Approaching the target stage. Based on the relative posture of the tool center point of the two arms and the respective target points 5, a dual-arm collaborative potential field is constructed, thereby generating a virtual position attraction force F through the force feedback hand controller. a,i (P t,i ), guiding the operator to manipulate the end-of-arm tool to approach the target points 5 of each arm. In the operation of this embodiment, the force-sensing guidance trajectory 6 of each arm is shown in Figure 2;

[0082] Step S5: obstacle avoidance phase. The robot arm end tool center point is judged cyclically to see whether it is close to the obstacle. If so, the robot enters the obstacle avoidance phase and constructs a repulsive potential field within a certain range outside the obstacle point cloud area to form an obstacle repulsive field range 4. The virtual repulsive force F is generated by the force feedback hand controller. r,i,j (d p,i-ob,j ), combined with the virtual position attraction F approaching the target point a,i (P t,i ) Assist the operator to control the robotic arm to avoid obstacles. In the specific implementation operation, the force sense guidance trajectory 6 of each arm is shown in Figure 2; if not, return to step S4;

[0083] The determination of whether the center point of the tool at the end of the manipulator is close to an obstacle is as follows: calculating the center point P of the tool at the end of the manipulator t The distance d between the coordinates of and the obstacle p-ob , determine whether the value is lower than the obstacle avoidance threshold D ob , if so, a repulsive potential field is constructed. d p-ob Calculated by the obstacle bounding box algorithm;

[0084] Step S6: Adjusting the working posture. The process loop determines whether the tool center point at the end of the robot arm reaches the vicinity of the target point. If so, the process enters the adjusting working posture stage. The force feedback hand controller generates a virtual posture attraction force F according to the relative posture angle between the end of the robot arm and the corresponding target point. z,i (r t,i ), and the virtual position attraction F of step S4 a,i (P t,i ) is combined into a virtual posture attraction F vr,i , guide the operator to accurately adjust the end position of the robot arm to perform the task; if not, return to step S4;

[0085] The determination of whether the tool center point of the end-of-arm tool reaches the vicinity of the target point refers to whether the coordinates of the tool center point P of the end-of-arm tool are within the point cloud area of ​​the target point. Within this area, the virtual position attraction and virtual posture attraction are generated simultaneously. The point cloud area of ​​the target point is defined by the point cloud segmentation algorithm and is a point cloud area with the target point point mass center P as the center. o The sphere area with the center as the circle is set to R. t .

[0086] Step S7: Closed-chain coordinated motion of both arms. A loop is run to determine whether a closed-chain constraint relationship is formed between the two arms. If so, the two arms are in the closed-chain coordinated motion stage. The relative position of the tool center points at the end of the two arms is recorded and continuously updated. A relative impedance model 9 is generated based on the potential field of the two-arm collaboration. A virtual relative impedance constraint force F is generated through the force feedback hand controller. rc (t) assists the operator in controlling the coordinated movement of the two arms to the double-arm closed-chain coordinated movement destination 10, so that the relative positions of the two arms do not undergo a large sudden change and synchronized movement is maintained. In the specific implementation operation, the force-guided trajectories of the two arms are shown in FIG3 ;

[0087] The determining whether a closed-chain constraint relationship is formed between the two arms refers to: based on the open and closed state of the gripper and the area of ​​the intersection of the point cloud contour of the gripper and the point cloud contour of the target object, cyclically detecting whether the two arms and the target object form a whole and start coordinated motion;

[0088] In the specific implementation operation, the positions of the obstacles described in the above steps, the positions and attitude angles of the target objects and the target points of the two arms, the positions of the boundaries of the workspaces of the two arms, the postures of the center points of the tools at the end of the two arms, and all posture calculations are unified into the world coordinate system {B} through coordinate system transformation. The world coordinate system {B} conforms to the right-hand rule, as shown in Figure 2. The origin 8 of the system world coordinate system is set as the reference point B on the robot mobile platform 2.

[0089] In the specific implementation operation, when executing the steps S4 and S5, considering the actual task requirements and to reduce the difficulty of the operator, the operator only needs to control the position of the robot arm without controlling the posture. The virtual position attraction F of step S4 is a,i (P t,i ), the virtual repulsive force F of step S5 r,i,j (d p,i-ob,j ) are all 3D forces and do not include torque. When executing step S6, the operator can control the position and posture of the robotic arm when accurately adjusting the posture of the end of the robotic arm. At this time, the virtual posture attraction F vr,i It is a 6-dimensional force including force and moment. When executing step S7, the virtual relative impedance constraint force F rc (t) is a 6-dimensional force including force and torque;

[0090] The calculation formula of the bounding box algorithm for the obstacle is as follows: Ψ={L+αl x u x +βl y u y +γl z u z |α,β,γ∈[-1,1]}

[0091] In the formula, Ψ represents the mathematical model of the bounding box, L represents the 3D center vector of the bounding box, and l x 、l y 、l z Indicates the half side length of the bounding box, u x 、u y 、u z Represents the mutually perpendicular unit vectors of the bounding box, α, β, γ are the accuracy parameters of the bounding box algorithm;

[0092] Calculate the minimum distance d between the center point of the robot end tool and the obstacle during the robot operation process p-ob , the calculation principle is as follows:

[0093] 1) Assume that the center coordinate of the end tool of the robot arm is P t =(x t ,y t ,z t ), the vertex Q of the bounding box model of the obstacle point i =(x i ,y i ,z i ), calculate P t Go to each vertex Q of the bounding box respectively i The distance λ1;

[0094] 2) Calculate P t The distance to each edge of the bounding box. First, get the vector n of the edge of the bounding box. i , then find P t Vector m to each vertex i , finally judge n i and m i If the angle between them is an obtuse angle, it is discarded; if it is an acute angle, the shortest distance λ2 is calculated as:

[0095] 3) Calculate P t The distance to each face of the bounding box. Take any three vertices C1, C2, C3 on each face, and let P t The foot of the perpendicular to each surface is P c =(x c ,y c ,z c ), first according to P t P c ⊥C1C2, P t P c ⊥C2C3, P t P c ⊥C1C3 to find P cThe coordinate value of the vertical foot is determined according to the coordinates of the vertices on the surface, and finally the shortest distance λ3 is obtained as:

[0096] 4) Take the minimum value among λ1, λ2, and λ3 as the distance d from the center point of the tool at the end of the robot arm to the obstacle p-ob : d p-ob =min(λ1,λ2,λ3)

[0097] The calculation formula of the dual-arm collaborative potential field is as follows:

[0098] Where U a,i (P t,i ) represents the dual-arm collaborative potential field, δ i represents the cooperation factor of the i-th robot arm in the dual-arm cooperation potential field, R represents the distance between the center point of the tool at the end of the i-th robot arm and its corresponding target point, t,i The radius of the point cloud area sphere representing the i-th target point;

[0099] The virtual location attraction F a,i (P t,i ) is calculated as follows:

[0100] The above formula represents the attractive force exerted on the i-th robotic arm, and the force feedback hand controller generates a virtual position attractive force and feeds it back to the operator;

[0101] The calculation formula of the repulsive potential field is as follows:

[0102] Where U r,j,i (d p,i-ob,j ) represents the repulsive potential field of the j-th obstacle, ε represents the adjustment factor of the repulsive potential field, d p,i-ob,j Denotes the distance between the j-th obstacle and the center point of the tool at the end of the i-th robot arm, D ob,j represents the range of the repulsive potential field of the j-th obstacle;

[0103] The calculation formula of the virtual repulsive force is as follows:

[0104] The above formula indicates that the i-th robot arm is subjected to the repulsive force of the j-th obstacle;

[0105] It indicates that the i-th robot arm receives the repulsive force of a total of n obstacles, and generates a virtual repulsive force through the force feedback hand controller and feeds it back to the operator;

[0106] The virtual constraint force formula for the obstacle avoidance task of the i-th robotic arm is as follows:

[0107] The calculation formula of the virtual posture attractiveness is as follows:

[0108] Where, re i =r t,i -r o,i , represents the posture angle r of the i-th robot arm at time t t,i The attitude angle r of the corresponding target point o,i The relative posture of represents the relative angular velocity of the two, K is the stiffness coefficient of the Kelvin-Voigt linear model, and B is the damping coefficient of the Kelvin-Voigt linear model;

[0109] The calculation formula of the 6-dimensional virtual posture attraction force exerted on the operator when accurately adjusting the posture of the end of the robotic arm is as follows:

[0110] Where, F vr,i is a 6×1 matrix, F a,i and F z,i Both are 3×1 matrices;

[0111] The calculation principle of the virtual relative impedance constraint force is as follows: first, the relative posture ce(0) of the center point of the end tool when the two arms just form a closed chain constraint is recorded as the equilibrium position of the two-arm relative impedance model 9; then the relative posture of the center point of the end tool is updated cyclically. The difference between the two positions and the equilibrium position is calculated, and the difference is finally substituted into the double-arm relative impedance model 9. The calculation formula for the virtual relative impedance constraint force is as follows:

[0112] Where, e(t) = ce(t) - ce(0) is the difference between the relative position of the tool center points of the two manipulators at time t and the relative position ce(0) when the two arms just form a closed chain constraint, where ce(t) is a 6×1 matrix, x(t) and r(t) are both 3×1 matrices, and M d 、B d , K d are the inertia, damping, and stiffness coefficients of the dual-arm relative impedance model 9, respectively. When the relative posture of the dual arms changes, a virtual relative impedance constraint force is generated through the force feedback hand controller and fed back to the operator, prompting him to keep the dual arms moving synchronously to the dual-arm closed-chain collaborative motion destination 10.

[0113] In summary, the present invention targets the characteristics of dual-arm collaborative tasks, determines the collaborative strategy based on the environmental point cloud image, the posture information of the center points of the dual-arm end tools, and the dual-arm workspace, and then dynamically updates the collaboration factor. Based on this, the dual-arm collaborative potential field is designed. Combined with the repulsive potential field, virtual posture attraction, and dual-arm relative impedance model designed by the present invention, a virtual constraint force is generated through two force feedback hand controllers to guide and assist the operator in controlling the two robotic arms to complete the dual-arm collaborative task. This can improve the operator's control accuracy, work efficiency, and safety factor during human-machine collaboration, reduce the operator's operating burden, and better adapt to complex unstructured environments.

[0114] Throughout this specification, references to terms such as "one embodiment," "example," or "specific example" indicate that the specific features, structures, materials, or characteristics described in conjunction with that embodiment or example are included in at least one embodiment or example of the present invention. In this specification, schematic representations of these terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in any one or more embodiments or examples.

[0115] It should be noted that the above content merely illustrates the technical idea of ​​the present invention and cannot be used to limit the scope of protection of the present invention. For ordinary technicians in this technical field, several improvements and modifications can be made without departing from the principles of the present invention. These improvements and modifications all fall within the scope of protection of the claims of the present invention.

Claims

1. A force-guided teleoperation system based on a dual-arm collaborative potential field, characterized by: Includes two robotic arms, force feedback hand controller, router, vision unit, slave computer, master computer, display and mobile platform; The two robotic arms are respectively mounted on a mobile platform and connected to a slave computer; The force feedback hand controller is located on the operating table and connected to the slave computer. It is used to collect the six-degree-of-freedom posture and control instructions of the robotic arm in the Cartesian coordinate system and send them to the master computer. It receives the dynamic virtual constraint force data generated by the dual-arm collaborative potential field force guidance control module algorithm of the master computer and feeds it back to the operator. The router is responsible for establishing a local area network for the entire control system, enabling real-time data exchange between the robotic arm, slave computer, vision unit, router, master computer, and force feedback hand controller; The visual unit uses a structured light camera, which is located on a mobile platform, installed between the two robotic arms, and connected to the slave computer. It is used to collect point cloud information of the robot's surrounding environment in real time and transmit it to the master computer; The slave computer is located inside the mobile platform and includes a robotic arm drive module, a mobile platform control module, and a network communication module, and also includes integration and communication functions between the control modules; The main computer is located next to the operating console and includes a force feedback hand controller drive module, a dual-arm collaborative potential field force guidance control module, a robotic arm kinematic model, a robotic arm dynamic model, a point cloud information processing module, and a network communication module. It also includes integration and communication functions between the various control modules. The display screen is located in front of the operating console and is used to display point cloud images and virtual restraint force data; The mobile platform is used to install the robotic arm, the vision unit and the slave computer.

2. A force-guided teleoperation control method based on a dual-arm collaborative potential field using the system as claimed in claim 1, characterized in that: The steps include: S1: Before the operation begins, the workspaces of the two arms are determined separately using the Monte Carlo method based on the kinematic models of the two arms, and the boundary coordinates of the workspaces of the two arms are solved and recorded; S2: Obtain environmental point cloud information and segment the point cloud based on image features; obtain the location of obstacles and target objects based on the point cloud image, and use the bounding box algorithm to determine the location and attitude angle of each target point of the two arms based on the shape of the target object's point cloud bounding box; S3: The real-time position of the tool center point of the two robot arms is obtained through the robot arm kinematic model, and the shortest distance d between the target object and the boundary of the dual-arm workspace is determined cyclically. p-s,i Is it below the threshold D? s : If the shortest distance d between the target object and the boundary of the dual-arm workspace p-s,i If both are higher than the threshold, a two-arm symmetrical cooperation strategy is adopted. At this time, the cooperation factor δ of the left and right arms i is 1; If the shortest distance d between the target object and the boundary of the dual-arm workspace p-s,i Less than threshold D s , then adopt a single-arm priority cooperation strategy, and use this arm as the priority arm, with a cooperation factor of δ1 = 0.5 for the priority arm and a cooperation factor of δ1 = 2 for the other arm; S4: Construct the dual-arm collaborative potential field based on the relative posture of the tool center point at the end of the dual-arm and the respective target points, and generate the virtual position attraction F through the force feedback hand controller a,i (P t,i ), guiding the operator to control the end-of-arm tool to approach the target point; S5: Loop to determine whether the center point of the tool at the end of the robot arm is close to the obstacle. If so, it enters the obstacle avoidance phase and constructs a repulsive potential field within a certain range outside the obstacle point cloud area. This field is then used to generate a virtual repulsive force F through the force feedback hand controller. r,i,j (d p,i-ob,j ), combined with the virtual position attraction F approaching the target point a,i (P t,i ) assists the operator in controlling the robotic arm to avoid obstacles; otherwise, returns to step S4; S6: Circularly judge whether the tool center point at the end of the robot arm reaches the vicinity of the target point. If so, enter the stage of adjusting the working posture. According to the relative posture angle between the end of the robot arm and the corresponding target point, a virtual posture attraction force F is generated through the force feedback hand controller. z,i (r t,i ), and the virtual position attraction F of step S4 a,i (P t,i ) is combined into a virtual posture attraction F vr,i , guiding the operator to accurately adjust the end position of the robotic arm to perform the task; Otherwise, return to step S4; S7: Circularly determine whether a certain closed-chain constraint relationship is formed between the two arms. If so, the two arms are in the double-arm closed-chain collaborative motion stage. Record and continuously update the relative posture of the tool center point at the end of the two arms. Generate a double-arm relative impedance model based on the double-arm collaborative potential field. Generate a virtual relative impedance constraint force F through the force feedback hand controller. rc (t) assists the operator in controlling the coordinated movement of the two arms so that the relative positions of the ends of the two arms do not change significantly and maintain synchronous movement.

3. The force-guided teleoperation control method based on dual-arm collaborative potential field according to claim 2, characterized in that: In step S5, the method for determining whether the center point of the tool at the end of the manipulator is close to an obstacle is as follows: calculating the center point P of the tool at the end of the manipulator t The distance d between the coordinates of and the obstacle p-ob , determine whether the value is lower than the obstacle avoidance threshold D ob , if so, construct a repulsive potential field; In step S6, the method for determining whether the center point of the tool at the end of the manipulator reaches the vicinity of the target point is: whether the coordinates of the center point P of the tool at the end of the manipulator are within the point cloud area of ​​the target point, and within the range of the area, a virtual position attraction and a virtual posture attraction are simultaneously generated; In step S7, the method for determining whether a certain closed-chain constraint relationship is formed between the two arms is: based on the open and closed state of the two-arm grippers and the area of ​​the region where the point cloud contour of the grippers intersects with the point cloud contour of the target object, cyclically detecting whether the two arms form a whole with the target object and start coordinated movement.

4. The force-guided teleoperation control method based on dual-arm collaborative potential field according to claim 3, characterized in that: The virtual location attraction F in step S4 a,i (P t,i ) and the virtual repulsive force F in step S5 r,i,j (d p,i-ob,j ) are all 3D forces and do not include torque; the virtual posture attraction F in step S6 vr,i Contains force and torque, which is a 6-dimensional force; the virtual relative impedance constraint force F in step S7 rc (t) includes force and torque, which is a 6-dimensional force.

5. The force-sensing guided teleoperation control method based on dual-arm collaborative potential field according to claim 3, characterized in that: In step S5, the minimum distance d between the center point of the robot end tool and the obstacle p-o The calculation method is as follows: S51: Assume that the center coordinate of the end tool of the robot arm is P t =(x t ,y t ,z t ), the vertex Q of the bounding box model of the obstacle point i =(x i ,y i ,z i ), calculate P t Go to each vertex Q of the bounding box respectively i The distance λ1; S52: Get the vector n of the bounding box edge i , find P t Vector m to each vertex i , and then judge n i and m i If the angle between them is an obtuse angle, it is discarded; if it is an acute angle, the shortest distance λ2 is calculated as: S53: Take any three vertices C1, C2, and C3 on each face, and let P t The foot of the perpendicular to each surface is P c =(x c ,y c ,z c ), according to P t P c ⊥C1C2, P t P c ⊥C2C3, P t P c ⊥C1C3 to find P c The coordinate value of the vertical foot is determined according to the coordinates of the vertices on the surface, and finally the shortest distance λ3 is obtained as: S54: Take the minimum value among λ1, λ2, and λ3 as the distance d from the center point of the tool at the end of the robot arm to the obstacle p-ob : d p-ob =min(λ1,λ2,λ3).

6. The force-guided teleoperation control method based on dual-arm collaborative potential field according to claim 5, characterized in that: In step S4, the calculation formula of the dual-arm collaborative potential field is as follows: Where U a,i (P t,i ) represents the dual-arm collaborative potential field, δ i represents the cooperation factor of the i-th robot arm in the dual-arm cooperation potential field, R represents the distance between the center point of the tool at the end of the i-th robot arm and its corresponding target point, t,i The radius of the point cloud area sphere representing the i-th target point; The virtual location attraction F a,i (P t,i ) is calculated as follows: The above formula represents the attractive force exerted on the i-th robotic arm, and the force feedback hand controller generates a virtual position attractive force to feed back to the operator.

7. The force-guided teleoperation control method based on dual-arm collaborative potential field according to claim 6, characterized in that: In step S5, the calculation formula of the repulsive potential field is as follows: Where U r,j,i (d p,i-ob,j ) represents the repulsive potential field of the j-th obstacle, ε represents the adjustment factor of the repulsive potential field, d p,i-ob,j Denotes the distance between the j-th obstacle and the center point of the tool at the end of the i-th robot arm, D ob,j represents the range of the repulsive potential field of the j-th obstacle; The calculation formula of the virtual repulsive force is as follows: The above formula indicates that the i-th robotic arm is subjected to the repulsive force of the j-th obstacle; It indicates that the i-th robot arm receives the repulsive force of a total of n obstacles, and generates a virtual repulsive force through the force feedback hand controller and feeds it back to the operator; The virtual constraint force formula for the obstacle avoidance task of the i-th robotic arm is as follows:

8. The force-sensing guided teleoperation control method based on dual-arm collaborative potential field according to claim 7, characterized in that: In step S6, the calculation formula of the virtual posture attractiveness is as follows: Where, re i =r t,i -r o,i , represents the posture angle r of the i-th robot arm at time t t,i The attitude angle r of the corresponding target point o,i The relative posture of represents the relative angular velocity of the two, K is the stiffness coefficient of the Kelvin-Voigt linear model, and B is the damping coefficient of the Kelvin-Voigt linear model; The formula for calculating the 6-dimensional virtual posture attraction force on the operator when accurately adjusting the posture of the end of the robotic arm is as follows: Where, F vr,i is a 6×1 matrix, F a,i and F z,i Both are 3×1 matrices.

9. The force-sensing guided teleoperation control method based on dual-arm collaborative potential field according to claim 8, characterized in that: In step S7, the calculation principle of the virtual relative impedance constraint force is as follows: first, the relative posture ce(0) of the center point of the end tool when the two arms just form a closed chain constraint is recorded as the equilibrium position of the two-arm relative impedance model; then the relative posture ce(0) of the center point of the end tool is updated in a circular manner. The difference between the two positions and the equilibrium position is calculated, and the difference is finally substituted into the double-arm relative impedance model. The calculation formula for the virtual relative impedance constraint force is as follows: Where, e(t) = ce(t) - ce(0) is the difference between the relative position of the tool center points of the two manipulators at time t and the relative position ce(0) when the two arms just form a closed chain constraint, where ce(t) is a 6×1 matrix, x(t) and r(t) are both 3×1 matrices, and M d 、B d , K d are the inertia, damping and stiffness coefficients of the double-arm relative impedance model, respectively.

Images