A human-machine collaborative robotic arm control method based on fuzzy decision making

Abstract

The present invention discloses a human-machine collaborative robotic arm control method based on fuzzy decision-making, and the control process includes an environment acquisition stage, an autonomous planning stage, a motion mapping stage, a confidence calculation stage, a fusion and control stage, and a force feedback stage. The present invention uses fuzzy decision-making to simulate human decision-making behavior, determine the nonlinear relationship between confidence and weight, and integrate autonomous control with the operator's instructions for control; during the human-machine collaborative control process, the operator can modify the trajectory and grasping posture of the approaching object; the human-machine collaborative control method can reduce the operating burden of the remote operator, reduce the mental stress of long-term operation, and provide online real-time force feedback during the control process to help the operator understand the intention of autonomous control and provide assistance for subsequent operator decision-making.

Description

Technical Field

[0001] The present invention belongs to the technical field of underwater robots, and in particular relates to a human-machine collaborative robotic arm control method based on fuzzy decision-making. Background Art

[0002] When facing deep-sea or other extreme environment operations, fully automatic control to execute tasks cannot be achieved in a short period of time, and professional personnel are required. In order to ensure personnel safety, cameras are installed on the robot or placed in the operating environment to provide visual information and robot information to the remote operator, who controls the robot to complete the task, avoiding the threat to the operator's personal safety in dangerous environments. However, when the operator is performing remote operation, the operator's burden is heavy, and it is difficult to obtain complete three-dimensional information of the task scene. The missing information can easily cause the operator to misjudge, thereby destroying the operating object or damaging the robot, resulting in the failure of the task.

[0003] The Chinese patent document with application number CN201811159046.2 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 operator module and the autonomous management module to realize the switching of the robot's operation mode. The basic behavior unit calculates the velocity vector of each basic behavior output of the robot according to 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 realizing human-machine shared control of the autonomous remote-controlled underwater robot. This method improves the task performance of the autonomous remote-controlled underwater robot and reduces the workload of the operator.

[0004] The Chinese patent document with application number CN202111575047.7 discloses a local trajectory adjustment and human-machine shared control method and system suitable for robots to enhance the autonomy of surgical robots and transform the relationship between humans and robots from master-slave to collaborative. When the difference between human instructions and the robot's reference trajectory is large, the robot will actively adjust its own reference trajectory locally in combination with human virtual interaction force; when the difference between human and robot intentions is small, the instructions of both humans and robots 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] Most existing methods use autonomous control systems to assist operators in performing tasks, integrating autonomous control system planning with operator actions to reduce the burden on operators. However, most fusion methods are based on the robot's state and trajectory, and rarely consider how to integrate them to better suit the operator's thinking habits. In addition, few methods consider the operator's understanding of the autonomous control intention, as a clear understanding can help operators make better decisions. Summary of the invention

[0006] The present invention provides a human-machine collaborative robotic arm control method based on fuzzy decision-making, which can reduce the operating burden of remote operators and the mental stress of long-term operation, and provide online real-time force feedback during the control process to help operators understand the intention of autonomous control and provide assistance for subsequent operator decision-making.

[0007] A human-machine collaborative robotic arm control method based on fuzzy decision making, the autonomous control system adopted includes a camera, a master manipulator and a slave manipulator; the control process includes the following six stages:

[0008] (1) Environmental acquisition stage

[0009] The goal of the environment acquisition phase is to collect real-time images of the target object and the surrounding workspace through a camera installed on the robot or arranged in the working environment; and obtain the position of the target object p through image recognition technology processing. g and the obstacle position p o , sent to the autonomous control system;

[0010] (2) Autonomous planning stage

[0011] The goal of the autonomous planning stage is to obtain the next target position of the autonomous control system;

[0012] (3) Motion mapping stage

[0013] The goal of the motion mapping stage is to map the operator's input actions to the motion of the slave robot arm; the master robot collects the operator's input and maps it to the slave robot arm through coordinate transformation;

[0014] (4) Confidence calculation stage

[0015] The goal of the confidence calculation phase is to calculate the confidence of autonomous control and the operator for weight allocation in the fusion process;

[0016] (5) Integration and control stage

[0017] The goal of the fusion and control stage is to calculate the weights of autonomous control and the operator by confidence, fuse the target positions of autonomous control and the operator, obtain the final target position of the robot arm, and control the robot arm to reach it;

[0018] (6) Force feedback stage

[0019] The operator obtains autonomous control force feedback during the process of controlling the robotic arm, obtains the intention of autonomous control, and improves the collaboration between the operator and the robotic arm.

[0020] The present invention infers the confidence of the operator and autonomous control from the operator's input, the robot's historical trajectory and the robot's current state; uses fuzzy decision-making to simulate human decision-making behavior, determines the nonlinear relationship between confidence and weight, and integrates autonomous control with the operator's instructions for control; provides real-time online force feedback to assist the operator in issuing instructions and making decisions.

[0021] Furthermore, in the autonomous planning stage, the specific process of obtaining the next target position of the autonomous control system is as follows:

[0022] The model of the target object is defined as

[0023] X(k)=AX(k+1)+BU(k)+W(k)

[0024] Z(k)=HX(k)+V(k)

[0025] Among them, X(k) is the state variable of the object at time k, U(k) is the control value of the external input of the object at time k, A and B are the system matrix and input matrix, W(k) is the process noise, Z(k) is the measurement value at time k, H is the measurement matrix, and V(k) is the measurement noise; the state variable X is set to

[0026]

[0027] Among them, {x G ,y G ,z G} T Indicates the position of the target object in the terminal coordinate system. represents the linear velocity of the target in the terminal coordinate system, {γ G ,β G ,α G} T and Represents the Euler angle and angular velocity of the target object in the terminal coordinate system; the coefficient matrix of the model is set to

[0028] A={a,a,a,a,a,a}

[0029]

[0030] B=0

[0031] H=diag[1,0,1,0,1,0,1,0,1,0,1,0]

[0032] The error between the target and the end of the robot arm is e = {e p ,e q}∈R 6 , e p and e q They are position error and attitude error respectively

[0033] e p ={x G ,y G ,z G} T

[0034]

[0035] The end velocity of the robot under autonomous control is calculated by the PID controller, and the object velocity estimate is obtained by the Kalman filter. Delay introduced by PID controller as feedforward compensation

[0036]

[0037] Where x=(p,q)∈R 3 ×SO(3) represents the position and posture of the end of the robot arm; is the next target point of the autonomous control system in the robot end coordinate system. After coordinate transformation, the next target position p in the robot base coordinate system is obtained. r

[0038]

[0039] in, Represents the transformation matrix from the robot base coordinate system to the end coordinate system.

[0040] In the autonomous planning stage, when the target position p r When approaching an obstacle, in order to avoid collision, a local obstacle avoidance strategy is used for real-time motion control; specifically:

[0041] Define a spherical surface with the obstacle as the center to ensure that the target position moves along the spherical surface without touching the obstacle. According to the distance between the target position and the obstacle, define the target remapping function as

[0042]

[0043] In the formula, p o ∈R 3 is the center of the obstacle, p r ∈R 3 is the target position of the end of the robot arm, r is the radius of the inner circle, and R is the radius of the outer circle.

[0044] In the motion mapping phase, the end position of the slave manipulator is obtained by calculating the displacement of the master manipulator relative to its initial position. h (t) = p h (t-1)+s(p m (t)-p m (t-1))

[0045] Among them, p h (t) and p h (t-1) are the current position and the last position of the slave robot arm, respectively, p m (t) and p m (t-1) are the current position and the last position of the master manipulator, s is the scale factor, which is used to adjust the scale of the displacement mapping; the end direction mapping of the slave manipulator is

[0046]

[0047] in, Indicates that from C SB to C MB The rotation transformation, Indicates that the end of the main end robot is at C MB The direction of the next Indicates that the end of the slave robot arm is at C ME Downward direction; C SB represents the base coordinate system of the slave robot, C MB represents the base coordinate system of the master manipulator, C ME Represents the end coordinate system of the master robot.

[0048] In the confidence calculation stage, the process of calculating the confidence of autonomous control is as follows:

[0049] Given an initial state p 0 The current trajectory to the current state p, the distribution of the target position 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 for

[0050]

[0051]

[0052] in, That is, from p i to p j The optimal trajectory, the cost function of the trajectory is the distance traveled by the trajectory; p 0 is the initial position, p is the current position, and p GIndicates the location of the target point. Indicates p to p G The optimal trajectory of .

[0053] P(g) is the prior probability of the target:

[0054]

[0055] in, It is the angle between the vector from the current robot end position to the target position and the current end Z axis, and k is the coefficient.

[0056] In the confidence calculation stage, the confidence calculation formula of the operator is:

[0057]

[0058] Among them, p h ∈R 3 Indicates the current position of the master robot, p h0 ∈R 3 Indicates the initial position of the master manipulator, p hG ∈R 3 Indicates the target position of the slave robot arm mapped back to the master robot arm.

[0059] The process of the fusion and control stage is as follows:

[0060] A fuzzy decision method is used to determine the nonlinear relationship between confidence and weight. The fuzzy decision method includes two inputs and one output. The inputs are the confidence c of the autonomous control and the r ∈[0,1] and the operator's confidence c h ∈[0,1], the output is the weight coefficient λ∈[0,1] of autonomous control; the fuzzy decision method uses the standard triangular membership function, and based on the priority principle, the priority of the operator confidence is higher than the priority of the autonomous control confidence. Finally, in the defuzzification stage, the weight λ is obtained by the centroid method;

[0061] After the weight λ is obtained, the target position is linearly mixed by the autonomously controlled target position and the operator's target position, while the direction is controlled solely by the operator;

[0062] Get the fused target position p s and posture q s Finally, the joint positions of the robot arm are solved by the Track-Ik tool, and then the joint commands are sent to each joint of the robot arm to finally reach the target posture.

[0063] In the force feedback stage, the first three degrees of freedom of the master manipulator are equipped with motors to generate force feedback. In the human-machine collaborative control process, the force feedback is designed as follows:

[0064]

[0065] Among them, λ is the weight coefficient of autonomous control, p G and are the target position of the robot arm and the obstacle position, N is the number of obstacles, and s is the scale factor; the first part on the right side of the equation is the product of the attraction and the weight coefficient; the second part is the repulsion force, which guides the operator to avoid possible collisions with obstacles.

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

[0067] During the human-machine collaborative control process of the present invention, the operator can modify the trajectory and grasping posture of the approaching object; the human-machine collaborative control method can reduce the operating burden of the remote operator, reduce the mental stress of long-term operation, and provide online real-time force feedback during the control process, helping the operator to understand the intention of autonomous control and provide assistance for subsequent operator decision-making. BRIEF DESCRIPTION OF THE DRAWINGS

[0068] Figure 1 This is a control flow chart of a human-machine collaborative robotic arm control method based on fuzzy decision-making according to the present invention;

[0069] Figure 2 Schematic diagram of an autonomous control system according to an embodiment of the present invention;

[0070] Figure 3 is a two-dimensional diagram of the spherical enclosing surface of the obstacle in the embodiment of the present invention;

[0071] Figure 4 Schematic diagram of the entire obstacle avoidance process of the robotic arm in an embodiment of the present invention;

[0072] Figure 5 Schematic diagram of the three-dimensional trajectory of the end of the robot arm and the position of obstacles in an embodiment of the present invention;

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

[0074] Figure 7 A schematic diagram of a membership function and a fuzzy reasoning surface in an embodiment of the present invention;

[0075] Figure 8 It is a flow chart of the algorithm of human-machine collaborative control in an embodiment of the present invention;

[0076] Fig. 9 It is a schematic diagram of the test system during the experimental test of the present invention;

[0077] Fig.10It is a schematic diagram of the test task during the experimental test of the present invention;

[0078] Fig.11 It is a schematic diagram of position error and attitude error during the experimental test of the present invention;

[0079] Fig.12 It is a schematic diagram of the change of confidence and weight during the experimental test of the present invention;

[0080] Fig.13 It is a schematic diagram of the change of feedback force during the experimental test of the present invention. DETAILED DESCRIPTION

[0081] 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.

[0082] like Figure 1 As shown in the figure, a human-machine collaborative robot control method based on fuzzy decision-making includes six stages: environment acquisition, autonomous planning, motion mapping, confidence calculation, fusion and control, and force feedback. The autonomous control system used includes a camera, a master manipulator, and a slave manipulator. The coordinate system and its relative relationship are shown in Figure 2 shown.

[0083] Environmental acquisition phase.

[0084] The goal of the environment acquisition phase is to collect real-time images of the target object and the surrounding workspace. Relevant images are collected through cameras installed on the robot or arranged in the work 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 position of the target object. g and the obstacle position p o , sent to the autonomous control system.

[0085] Autonomous planning stage.

[0086] The goal of the autonomous planning stage is to obtain the next target position of the autonomous control system. After obtaining the position of the target object, the Kalman filter is introduced to reduce the jitter noise of target recognition. The model of the target object is defined as

[0087] X(k)=AX(k+1)+BU(k)+W(k)

[0088] Z(k)=HX(k)+V(k)

[0089] Among them, X(k) is the state variable of the object at time k, U(k) is the control value of the external input of the object at time k, A and B are the system matrix and input matrix, W(k) is the process noise, Z(k) is the measurement value at time k, H is the measurement matrix, and V(k) is the measurement noise. The state variable X is set to

[0090]

[0091] Among them, {x G ,y G ,z G} T Indicates the position of the target object in the terminal coordinate system. represents the linear velocity of the target in the terminal coordinate system, {γ G ,β G ,α G} T and represents the Euler angle and angular velocity of the target object in the terminal coordinate system. The coefficient matrix of the model is set as

[0092] A={a,a,a,a,a,a}

[0093]

[0094] B=0

[0095] H=diag[1,0,1,0,1,0,1,0,1,0,1,0]

[0096] The error between the target and the end of the robot arm is e = {e p ,e q}∈R 6 , respectively position error and attitude error

[0097] e p ={x G ,y G ,z G} T

[0098]

[0099] The PID controller is used to calculate the end velocity of the robot under autonomous control, and the Kalman filter obtains the object velocity estimate Delay introduced by PID controller as feedforward compensation

[0100]

[0101] Where x = (p, q)∈R 3 ×SO(3) represents the position and posture of the end of the robot arm. It is approximately the next target point of the autonomous control system in the robot end coordinate system. After coordinate transformation, the next target position p in the robot base coordinate system can be obtained. r .

[0102]

[0103] in, Represents the transformation matrix from the robot base coordinate system to the end coordinate system.

[0104] When the target position p r In order to avoid collision when approaching an obstacle, we developed a local obstacle avoidance strategy for real-time motion control. Similar to the artificial potential field method, we defined a spherical surface with the obstacle as the center to ensure that the target position moves along the spherical surface without touching the obstacle. The two-dimensional image of the sphere is as follows: Figure 3 As shown, where p o ∈R 3 is the center of the obstacle, p r ∈R 3 is the target position of the robot end, r is the radius of the inner circle, R is the radius of the outer circle, and according to the distance between the target position and the obstacle, the target remapping function is defined as

[0105]

[0106] Figure 4 This is a schematic diagram of the entire obstacle avoidance process of the robot arm. When the UR10 robot arm approaches an obstacle, it bypasses the obstacle according to the set envelope surface. Figure 1-6 The entire obstacle avoidance process of the robotic arm is demonstrated. Figure 5 The three-dimensional trajectory of the end of the robotic arm and the location of obstacles are shown.

[0107] Motion mapping stage.

[0108] The goal of the motion mapping phase is to map the operator's input actions to the motion of the robot arm. The six-degree-of-freedom master robot device (Geomagic Touch) collects the operator's input and maps it to the six-degree-of-freedom slave robot arm through coordinate transformation. Figure 2 The coordinate relationship between the master end manipulator and the slave end manipulator is shown in Figure 1. The end position of the slave end manipulator is obtained by calculating the displacement of the master end manipulator relative to its initial position. h (t) = p h (t-1)+s(p m (t)-p m (t-1))

[0109] Among them, p h (t) and p h(t-1) are the current position and the last position of the slave robot arm, respectively, p m (t) and p m (t-1) are the current position and the last position of the master manipulator, respectively. s represents the scale factor, which can adjust the scale of the displacement mapping. The end direction of the slave manipulator is mapped as

[0110]

[0111] in, Indicates that from C SB to C MB The rotation transformation, Indicates that the end of the main end robot is at C MB The direction of the next Indicates that the end of the slave robot arm is at C ME Down direction.

[0112] Confidence calculation stage.

[0113] The goal of the confidence calculation stage is to calculate the confidence of autonomous control and the operator, which is used for weight allocation in the subsequent fusion process. Figure 6 As shown, given the initial state p 0 The current trajectory to the current state p, the distribution of the target position 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

[0114]

[0115] The maximum entropy inverse optimal control surface, minimizing the worst-case prediction loss will result in a model where the probability decreases exponentially with the increase in cost, that is, P(ξ|g)∝exp(-C g (ξ)), so it can be computed by marginalizing the path from the start to the goal.

[0116]

[0117] In the actual calculation process, we follow the method of Dragan and Srinivasa and use the Laplace method to approximate the integral on the trajectory:

[0118]

[0119] in, That is, from p i to p j The optimal trajectory, the cost function of the trajectory is the distance traveled by the trajectory

[0120] P(g) is the prior probability of the target:

[0121]

[0122] in, It is the angle between the vector from the current robot end position to the target position and the current end Z axis, and k is the coefficient.

[0123] Similar to the confidence calculation of autonomous control, the confidence calculation formula of the operator is:

[0124]

[0125] Among them, p h ∈R 3 Indicates the current position of the master robot, p h0 ∈R 3 Indicates the initial position of the master manipulator, p hG ∈R 3 Indicates the target position of the slave robot arm mapped back to the master robot arm.

[0126] Autonomous control confidence c r The larger it is, the more credible the autonomous control system is; the greater the operator confidence, the greater the operator's willingness to control and the greater the willingness to resist the guidance of the autonomous control system.

[0127] Integration and control stage.

[0128] The goal of the fusion and control stage is to calculate the weights of autonomous control and the operator by confidence, fuse the target positions of autonomous control and the operator, obtain the final target position of the robotic arm, and control the robotic arm to reach it.

[0129] The fuzzy decision-making method is introduced to simulate the human thinking mode and determine the nonlinear relationship between confidence and weight. The proposed fuzzy decision-making method contains two inputs and one output. The inputs are the confidence c of autonomous control and the output of r ∈[0,1] and the operator's confidence c h ∈[0,1], the output is the weight coefficient λ∈[0,1] of autonomous control. Figure 7 As shown, the fuzzy decision method uses a standard triangular membership function, and the fuzzy set includes negative large (NB), negative medium (NM), negative small (NS), zero (ZO), positive small (PS), positive medium (PM) and positive large (PB). The rule base of fuzzy decision is shown in Table 1 below, and based on the priority principle, the priority of operator confidence is higher than that of autonomous control confidence, that is, when both confidences are high, the operator confidence requirement is met first. Finally, in the defuzzification stage, the weight λ is obtained by the centroid method.

[0130] Table 1 Fuzzy decision rule base

[0131]

[0132]

[0133] After the weight λ is obtained, the target position is linearly mixed by the autonomous control target position and the operator's target position, while the direction is controlled by the operator alone. This is because it is difficult for autonomous control to obtain a suitable grasping posture, so it is all left to the operator.

[0134] p s =λp r +(1-λ)p h

[0135] q s =q h

[0136] Get the fused target position p s and posture s Finally, the joint positions of the robot arm are solved by the Track-Ik tool, and then the joint commands are sent to each joint of the robot arm to finally reach the target posture.

[0137] Force feedback stage.

[0138] The goal of the force feedback stage is to allow the operator to obtain the intention of autonomous control and improve the collaboration between the operator and the robot arm. The first three degrees of freedom of the main robot device (Geomagic Touch) are equipped with motors to generate force feedback. In the process of human-machine collaborative control, the force feedback design is:

[0139]

[0140] Where λ is the weight of autonomous control, p G and are the target position of the robot and the obstacle position, N is the number of obstacles, and s is the scale factor. The first part on the right side of the equation is the product of the attraction and the weight coefficient. The second part is the repulsion force, which guides the operator to avoid possible collisions with obstacles. Online force prompts can help operators predict the movement of the robot, which can provide more decision-making time and correct errors in a timely manner.

[0141] The algorithm flow of human-machine collaborative control is as follows: Figure 8 shown.

[0142] Experimental testing part.

[0143] The experimental system consists of Fig. 9As shown in the figure, the camera and force feedback device are connected to a Linux system computer via USB, and the maximum frame rate of the camera is 60 frames. UR10 is connected to the PC via Ethernet, and the entire system is developed based on the Robot Operating System (ROS). Another PC provides the real-time image of the camera as visual feedback for the operator to observe.

[0144] The experimental tasks were designed and performed using three methods: operator manual control, automatic control, and human-machine collaborative control based on fuzzy decision making. The results were compared and discussed.

[0145] The experiment is divided into two stages: tracking and pouring. A cup filled with ten small balls is fixed on the end of the UR10 robot. Three different methods are used to control the robot to track the empty cup on a fixed track and avoid obstacles, and pour the small balls into the empty cup at the end of the track. Fig.10 As shown, the dotted line is a fixed trajectory, Figure 1-6 It shows the process from the starting point to the end point.

[0146] Fig.11 In the figure, (a) and (b) are the position error and attitude error of the three methods during the experiment. During the tracking process, when encountering an obstacle, the expected position deviates from the target according to the obstacle avoidance algorithm; in the dumping stage, the expected position is the upper left corner of the target position. Since each experiment is placed at the initial position by a person, it is impossible to ensure that the position is consistent each time, resulting in different initial errors for different methods. Finally, the position error decreases, the dumping begins, and the attitude error gradually increases. When the dumping is completed, the attitude error is the largest.

[0147] The confidence and weight changes during the experiment are as follows: Fig.12 As shown in the figure. The initial value of confidence is related to the distance from the initial position of the end robot to the expected position. When the operator's control intention reaches the maximum, λ is the minimum (point D). As the operator's control intention gradually decreases, λ slowly increases and finally remains at around 0.7, so that the operator can adjust it. As can be seen from the figure, the weight and the operator's confidence curve have opposite trends. As the operator's confidence increases, the weight coefficient decreases, and vice versa. This shows that this method is easy to understand the operator's intention and easy for the operator to control.

[0148] The feedback force changes during the experiment are as follows: Fig.13 As shown. There are two minimum points F and G in the feedback force change curve, which are Fig.12 Points D and E in the figure correspond exactly. At point F, the operator's control intention is very strong (c h =0.84), λ drops to the minimum value. According to the design formula of force feedback, the influence of force feedback on the operator will become very small. At point G, λ reaches the maximum value, but due to the p in the formula G -p rThe smaller the force feedback value, the smaller the force feedback value. This result indicates that the force feedback system provides clear intention and guidance to the operator.

[0149] 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 human-machine collaborative robotic arm control method based on fuzzy decision making, It is characterized in that The autonomous control system used includes a camera, a master manipulator and a slave manipulator; the control process includes the following six stages: (1) Environmental acquisition stage The real-time image of the target object and the surrounding workspace is collected by the camera installed on the robot or arranged in the working environment; and the position of the target object p is obtained through image recognition technology processing. g and the obstacle position p o , sent to the autonomous control system; (2) Autonomous planning stage Get the next target position of the autonomous control system; (3) Motion mapping stage The operator's input action is mapped to the movement of the slave end robot arm; the master end robot collects the operator's input and maps it to the slave end robot arm through the coordinate transformation relationship; (4) Confidence calculation stage The confidence of autonomous control and operator is calculated for weight allocation in the fusion process. The process of calculating the confidence of autonomous control is: Given an initial state p 0 The current trajectory to the current state p, the distribution of the target position 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 for in, That is, from p i to p j The optimal trajectory, the cost function of the trajectory is the distance traveled by the trajectory; P(g) is the prior probability of the target: in, is the angle between the vector from the current robot end position to the target position and the current end Z axis, and k is the coefficient; The calculation formula for calculating the operator's confidence is: Among them, p h ∈R 3 Indicates the current position of the master robot, p h0 ∈R 3 Indicates the initial position of the master manipulator, p hG ∈R 3 Indicates the target position of the slave robot arm mapped back to the master robot arm; (5) Integration and control stage The weights of autonomous control and operator are calculated by confidence, and the target positions of autonomous control and operator are integrated to obtain the final target position of the robot arm, and the robot arm is controlled to reach it. (6) Force feedback stage The operator obtains autonomous control force feedback during the process of controlling the robot arm, obtains the intention of autonomous control, and improves the cooperation between the operator and the robot arm; In the force feedback stage, the first three degrees of freedom of the master manipulator are equipped with motors to generate force feedback. In the human-machine collaborative control process, the force feedback is designed as follows: Among them, λ is the weight coefficient of autonomous control, p G and are the target position of the robot arm and the obstacle position, N is the number of obstacles, and s is the scale factor; the first part on the right side of the equation is the product of the attraction and the weight coefficient; the second part is the repulsion force, which guides the operator to avoid possible collisions with obstacles.

2. According to the fuzzy decision-making based human-machine collaborative robotic arm control method of claim 1, It is characterized in that In the autonomous planning stage, the specific process of obtaining the next target position of the autonomous control system is as follows: The model of the target object is defined as X(k)=AX(k+1)+BU(k)+W(k) Z(k)=HX(k)+V(k) Among them, X(k) is the state variable of the object at time k, U(k) is the control value of the external input of the object at time k, A and B are the system matrix and input matrix, W(k) is the process noise, Z(k) is the measurement value at time k, H is the measurement matrix, and V(k) is the measurement noise; the state variable X is set to Among them, {x G ,y G ,z G } T Indicates the position of the target object in the terminal coordinate system. represents the linear velocity of the target in the terminal coordinate system, {γ G ,β G ,α G } T and represents the Euler angle and angular velocity of the target object in the terminal coordinate system; the coefficient matrix of the model is set to A = {a,a,a,a,a,a} B＝0 H=diag[1,0,1,0,1,0,1,0,1,0,1,0] The error between the target and the end of the robot arm is e = {e p ,e q }∈R 6 , e p and e q They are position error and attitude error respectively e p ={x G ,y G ,z G } T The end velocity of the robot under autonomous control is calculated by the PID controller, and the object velocity estimate is obtained by the Kalman filter. Delay introduced by PID controller as feedforward compensation Where x = (p, q)∈R 3 ×SO(3) represents the position and posture of the end of the robot arm; is the next target point of the autonomous control system in the robot end coordinate system. After coordinate transformation, the next target position p in the robot base coordinate system is obtained. r in, Represents the transformation matrix from the robot base coordinate system to the end coordinate system.

3. According to the fuzzy decision-making based human-machine collaborative robotic arm control method of claim 2, It is characterized in that In the autonomous planning stage, when the target position p r When approaching an obstacle, in order to avoid collision, a local obstacle avoidance strategy is used for real-time motion control; specifically: Define a spherical surface with the obstacle as the center to ensure that the target position moves along the spherical surface without touching the obstacle. According to the distance between the target position and the obstacle, define the target remapping function as In the formula, p o ∈R 3 is the center of the obstacle, p r ∈R 3 is the target position of the end of the robot arm, r is the radius of the inner circle, and R is the radius of the outer circle.

4. According to the fuzzy decision-making based human-machine collaborative robotic arm control method of claim 1, It is characterized in that In the motion mapping phase, the end position of the slave manipulator is obtained by calculating the displacement of the master manipulator relative to its initial position. h (t) = p h (t-1)+s(p m (t)-p m (t-1)) Among them, p h (t) and p h (t-1) are the current position and the last position of the slave robot arm, respectively, p m (t) and p m (t-1) are the current position and the last position of the master manipulator, s is the scale factor, which is used to adjust the scale of the displacement mapping; the end direction mapping of the slave manipulator is in, Indicates that from C SB to C MB The rotation transformation, Indicates that the end of the main end robot is at C MB The direction of the next Indicates that the end of the slave robot arm is at C ME Downward direction; C SB represents the base coordinate system of the slave robot, C MB Represents the base coordinate system of the master manipulator, C ME Represents the end coordinate system of the master robot.

5. According to the fuzzy decision-making based human-machine collaborative robotic arm control method of claim 1, It is characterized in that The process of the fusion and control stage is as follows: A fuzzy decision method is used to determine the nonlinear relationship between confidence and weight. The fuzzy decision method includes two inputs and one output. The inputs are the confidence of autonomous control and c r ∈[0,1] and the operator's confidence c h ∈[0,1], the output is the weight coefficient of autonomous control λ∈[0,1]; The fuzzy decision method uses the standard triangular membership function and is based on the priority principle. The priority of the operator confidence is higher than that of the autonomous control confidence. Finally, in the defuzzification stage, the weight λ is obtained by the centroid method; After the weight λ is obtained, the target position is linearly mixed by the autonomously controlled target position and the operator's target position, while the direction is controlled solely by the operator; Get the fused target position p s and posture q s Finally, the joint positions of the robot arm are solved by the Track-Ik tool, and then the joint commands are sent to each joint of the robot arm to finally reach the target posture.

Images