A safe obstacle avoidance method for dual-arm robot nursing transfer tasks

Abstract

The present invention provides a safe obstacle avoidance method suitable for dual-arm robot nursing transfer tasks. First, the robot collects and processes environmental images while performing the task, obtains the coordinates of key points in the human body posture and the coordinates of obstacles; second, the human body posture and the robot's arms are simplified; then, the repulsive force exerted by the obstacle on each link of the dual-arm robot's upper limbs and the attractive force exerted by the target position on each link of the dual-arm robot's upper limbs are calculated; finally, the key angles of the human body posture are calculated, and it is determined whether each angle meets the constraint conditions. If not, an additional force is applied to the joints of the robot arm links in contact with the human body, the applied force is superimposed with the repulsive force, and the superimposed repulsive force and the calculated attractive force are applied to the robot joints. The angular acceleration of each joint is calculated, and the motion of the dual-arm robot joints is then controlled. By improving the artificial potential field method, the present invention makes the robot motion smoother during obstacle avoidance, ensuring the comfort of the person being cared for while avoiding obstacles safely.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot obstacle avoidance, and in particular relates to a safe obstacle avoidance method suitable for dual-arm robot nursing transfer tasks. Background Art

[0002] Robotic obstacle avoidance refers to the process by which a mobile robot or manipulator uses sensors to detect static or dynamic obstacles in its planned path during motion, then replans the path based on an obstacle avoidance algorithm, thereby circumventing the obstacles and reaching the target location. The artificial potential field method is one of the most commonly used obstacle avoidance algorithms. This method establishes a repulsive force field between the obstacle and the robot, and an attractive field between the target point and the robot. Based on the distance between the robot and the obstacle, it calculates the repulsive force of the obstacle on the robot, and the attractive force of the target point on the robot based on the distance between the robot and the obstacle. Finally, the combined force controls the robot's motion, allowing the robot to avoid obstacles while reaching the target point.

[0003] Existing obstacle avoidance strategies based on artificial potential fields are not suitable for dual-arm robot nursing transfer tasks for two reasons. First, the original repulsive field function of the artificial potential field method is a function of the distance between the robot and the obstacle. The repulsive force grows slowly when the distance between the robot and the obstacle is less than or close to the distance threshold, causing the robot to continue moving toward the obstacle even when approaching it, resulting in poor obstacle avoidance. However, when the distance between the two approaches zero, the repulsive force increases dramatically, causing the robot to quickly move away from the obstacle. For dual-arm robot nursing transfer tasks, the target of action is the person being cared for, and sudden changes in the robot's movement speed can reduce human safety. Second, existing artificial potential field methods are typically used for obstacle avoidance in routine robot tasks, considering only two entities, the robot and the obstacle, and modeling only the robot and the obstacle. However, in nursing transfer tasks, the person being cared for is also an important entity, and the safety issues caused by collisions with obstacles cannot be ignored. Therefore, this application proposes a safe obstacle avoidance method suitable for dual-arm robot nursing transfer tasks. Summary of the Invention

[0004] In view of the deficiencies in the prior art, the technical problem to be solved by the present invention is to provide a safe obstacle avoidance method suitable for dual-arm robot nursing transfer tasks.

[0005] The technical solution adopted by the present invention to solve the technical problem is as follows:

[0006] A safe obstacle avoidance method for a dual-arm robot nursing transfer task includes the following steps:

[0007] Step 1: While the dual-arm robot is performing the nursing transfer task, collect environmental images; process the environmental images to obtain the coordinates of key human posture points and obstacle coordinates. The coordinates of key human posture points include the coordinates of the ankle joint, knee joint, waist joint, neck joint, and head; obtain the joint angles of the dual-arm robot's upper limbs, including the left arm joint angles (a1, a2, a3, a4, a5, a6) and the right arm joint angles (b1, b2, b3, b4, b5, b6). a1, a2, a3, a4, a5, a6 are the six joint angles of the left arm from the top to the end, and b1, b2, b3, b4, b5, b6 are the six joint angles of the right arm from the top to the end.

[0008] Step 2: Connect the key points of the human body posture through connecting rods in sequence, so that the ankle joint and knee joint form the connecting rod P1P2, the knee joint and waist joint form the connecting rod P2P3, the waist joint and neck joint form the connecting rod P3P4, and the neck joint and head form the connecting rod P4P5; similarly, simplify the two arms of the dual-arm robot to obtain the first connecting rod of the left arm to the sixth connecting rod of the left arm and the first connecting rod of the right arm to the right connecting rod;

[0009] Step 3: Calculate the shortest distance from the obstacle to each link of the dual-arm robot's upper limbs and normalize the shortest distance. Substitute each normalized shortest distance into the repulsive field function to calculate the repulsive force of the obstacle on each link of the dual-arm robot's upper limbs. The repulsive force obtained includes the direction. The repulsive field function is expressed as:

[0010]

[0011] Where, represents the normalized shortest distance, A general symbol for repulsive force, i represents the left and right arms of a dual-arm robot, where l and r represent the left and right arms, respectively, and j = 1, 2, ..., 6 represents the link number.

[0012] Assume that the target values of the left and right arm joint angles of the dual-arm robot are and The difference between the left arm joint angle and the target value is The difference between the right arm joint angle and the target value is Substitute the difference between each joint angle and the target value into the gravitational field function to obtain the gravitational force of the target position on each link of the dual-arm robot's upper limb. The obtained gravitational force includes the direction. The gravitational field function is expressed as:

[0013]

[0014] Where k0 represents a constant coefficient;

[0015] Step 4: Calculate the angle θ1 between the connecting rod P2P3 and the horizontal plane, the angle θ2 between the connecting rod P2P3 and the connecting rod P3P4, and the angle θ3 between the connecting rod P3P4 and the horizontal plane; determine whether the angles θ1, θ2, and θ3 meet the constraints. If they do, no additional force is applied; if θ1 does not meet the constraints, but θ2 and θ3 meet the constraints, then the fifth connecting rod L on the right arm is r5 Apply a horizontal force F0 to the left at the joint; if θ3 does not satisfy, θ1 and θ2 satisfy, then the fifth link L of the left arm l5 Apply a horizontal force F1 to the right at the joint of the right arm; if θ2 does not satisfy or any two or three of θ1, θ2, θ3 do not satisfy, then the fifth link L of the right arm r5 Apply a horizontal force F0 to the right at the joint of the left arm, and l5 A horizontal force F1 is applied to the joint of θ; F0 and F1 are both greater than zero; the constraints are: θ1>15°, θ2>75°, θ3>15°;

[0016] Let the obstacle be on the fifth link L of the left arm of the dual-arm robot l5 And the right arm fifth link L r5 The repulsive forces are and In repulsion The superposition force F1 is the repulsive force In repulsion The superimposed force F0 is the repulsive force The repulsive force The calculated repulsive force of the obstacle on the other links of the dual-arm robot's upper limbs and the attractive force of the target position on the links of the dual-arm robot's upper limbs act on the corresponding joints of the dual-arm robot respectively. The angular acceleration of each joint of the upper limb is obtained through the Jacobian matrix of each joint, and the angular acceleration is used to control the joint movement of the dual-arm robot, so that the dual-arm robot can safely avoid obstacles while ensuring the comfort of the person being cared for.

[0017] Furthermore, assuming that the obstacle coordinates are (x m ,y m ,z m ), the spatial coordinates of the two end points of the first link of the left arm are (x l0 ,y l0 ,z l0 )、(x l1 ,y l1 ,z l1 ), then the distance from the obstacle to the first link of the left arm is:

[0018]

[0019] If R>1, the shortest distance from the obstacle to the first link of the left arm is:

[0020] S l1 =|(x m -x l1 ,y m -y l1 ,z m -z l1 )|

[0021] If R < 0, the shortest distance from the obstacle to the first link of the left arm is:

[0022] S l1 =|(x m -x l0 ,y m -y l0 ,z m -z l0 )

[0023] If 0≤R≤1, the shortest distance from the obstacle to the first link of the left arm is:

[0024]

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

[0026] The modeling process simultaneously considers the dual-arm robot, the person being cared for, and obstacles, establishing a human-robot-environment system model. Specifically for the nursing transfer task, the repulsive field function of the artificial potential field method is improved. This not only provides excellent obstacle avoidance but also makes the robot's motion smoother during the process, preventing safety issues caused by quickly moving away from obstacles. Applying perturbations to the dual-arm robot ensures safe obstacle avoidance while ensuring the comfort of the person being cared for. BRIEF DESCRIPTION OF THE DRAWINGS

[0027] Figure 1 It is the overall flow chart of the present invention;

[0028] Figure 2 A simplified schematic diagram of the human body posture connecting rod of the present invention;

[0029] Figure 3 The curve of the repulsive force growth rate obtained by the original repulsive force field function of the artificial potential field method versus the shortest distance between the fifth link of the left arm of the dual-arm robot and the obstacle;

[0030] Figure 4 The curve of the repulsive force growth rate obtained by improving the repulsive field function of the artificial potential field method and changing with the shortest distance between the fifth link of the left arm of the dual-arm robot and the obstacle. DETAILED DESCRIPTION

[0031] The technical solution of the present invention is described in detail below with reference to the accompanying drawings and specific embodiments, but this does not limit the scope of protection of the present application.

[0032] The present invention provides a safe obstacle avoidance method (hereinafter referred to as the method, see Figures 1 to 4) suitable for a dual-arm robot nursing transfer task. The dual-arm robot nursing transfer task is to hold a person being cared for and transfer the person being cared for from a starting position to a target position. The method specifically includes the following steps:

[0033] Step 1: While the dual-arm robot is performing the nursing transfer task, a camera installed in the working environment is used to capture real-time environmental images. The captured environmental images must include three main subjects: the dual-arm robot, the person being cared for, and obstacles. The person being cared for, the obstacles, and the dual-arm robot are identified in the environmental images to obtain the coordinates of the key points of the human posture and the coordinates of the obstacles, respectively. At the same time, the angles of the dual-arm robot's upper limb joints are obtained based on the kinematic model. The person being cared for, the obstacles, and the dual-arm robot constitute a human-machine-environment system model.

[0034] Human posture recognition uses existing methods such as the Openpose human posture recognition algorithm or the Kinect's built-in human posture estimation algorithm. The coordinates of the key points of the human posture include the coordinates of the ankle joint, knee joint, waist joint, neck joint and head. If the head of the person being cared for is on the left side of the dual-arm robot, the ankle joint and knee joint of the left leg of the human body are detected. If the head of the person being cared for is on the right side of the dual-arm robot, the ankle joint and knee joint of the right leg of the human body are detected. The spatial coordinates of the ankle joint of the human body are marked as P1 = (x1, y1, z1), the spatial coordinates of the knee joint are marked as P2 = (x2, y2, z2), the spatial coordinates of the waist joint are marked as P3 = (x3, y3, z3), the spatial coordinates of the neck joint are marked as P4 = (x4, y4, z4), and the spatial coordinates of the head are marked as P5 = (x5, y5, z5). A target detection algorithm based on deep learning is used, such as the YOLOv5 series neural network, the Transformer series model, etc., for obstacle recognition to obtain the obstacle coordinates, which are recorded as M = (x m ,y m ,z m ); The dual-arm robot adopts a general industrial robot, and its arm is a six-axis manipulator with six joints. Therefore, the upper limb joint angles of the dual-arm robot include the left arm joint angles (a1, a2, a3, a4, a5, a6) and the right arm joint angles (b1, b2, b3, b4, b5, b6). a1, a2, a3, a4, a5, a6 are the six joint angles from the top to the end of the left arm, and b1, b2, b3, b4, b5, b6 are the six joint angles from the top to the end of the right arm.

[0035] Step 2: Since the shapes of the dual-arm robot, obstacles, and the person being cared for are complex and have many configuration surfaces, the human-machine-environment system model is simplified; Figure 2 As shown in the figure, the five key points of the human body posture are connected in sequence to form four connecting rods connected from head to tail, and then the human body posture is simplified. The four connecting rods are the connecting rod P1P2 formed by the ankle joint and the knee joint, the connecting rod P2P3 formed by the knee joint and the waist joint, the connecting rod P3P4 formed by the waist joint and the neck joint, and the connecting rod P4P5 formed by the neck joint and the head; the simplification of the dual-arm robot mainly includes simplifying the connecting rods of the dual arms. First, according to the DH matrix and the joint angle of the dual-arm robot upper limb, the origin of each joint of the dual-arm robot arm is obtained. By connecting the left arm coordinate origin and the six joint origins of the left arm in sequence, six connecting rods connected from head to tail are formed, which are the first connecting rod L l1 , left arm second link L l2 、Left arm third link L l3 、Left arm fourth link L l4 、Left arm fifth link L l5 and the left arm sixth link L l6 Similarly, we get the first link L of the right arm r1 , right arm second link L r2 , right arm third link L r3 , right arm fourth link L r4 、Right arm fifth link L r5 and right arm link L r6 ;

[0036] Step 3: Calculate the repulsive force of the obstacle on each link of the dual-arm robot's upper limb and the attractive force of the target position on each link of the dual-arm robot's upper limb;

[0037] First, calculate the shortest distance from the obstacle to each link of the upper limb of the dual-arm robot; calculate the shortest distance from the obstacle to the first link L of the left arm l1 As an example, assume that the first link L of the left arm l1 The spatial coordinates of the two endpoints are (x l0 ,y l0 ,z l0 )、(x l1 ,y l1 ,z l1 ), then the obstacle to the first link L of the left arm l1 The distance is:

[0038]

[0039] If R>1, the shortest distance from the obstacle to the first link of the left arm is:

[0040] S l1 =|(x m-x l1 ,y m -y l1 ,z m -z l1 )|

[0041] If R < 0, the shortest distance from the obstacle to the first link of the left arm is:

[0042] S l1 =|(x m -x l0 ,y m -y l0 ,z m -z l0 )

[0043] If 0≤R≤1, the shortest distance from the obstacle to the first link of the left arm is:

[0044]

[0045] Similarly, find the obstacles to the second link L of the left arm l2 、Left arm third link L l3 、Left arm fourth link L l4 、Left arm fifth link L l5 And the left arm sixth link L l6 The shortest distance is S l2 、S l3 、S l4 、S l5 、S l6 , the obstacles are respectively to the first link L of the right arm r1 , right arm second link L r2 , right arm third link L r3 , right arm fourth link L r4 、Right arm fifth link L r5 And the right arm sixth link L r6 The shortest distance is S r1 、S r2 、S r3 、S r4 、S r5 、S r6 ;

[0046] Then, the shortest distance from the obstacle to each link of the dual-arm robot upper limb is normalized using the following formula to obtain the normalized shortest distance

[0047]

[0048] Among them, S ij A universal symbol for the shortest distance, d0 represents the distance threshold for safe obstacle avoidance in the artificial potential field method;

[0049] Finally, a suitable repulsive field function is selected to calculate the repulsive force exerted by the obstacle on each link of the dual-arm robot's upper limbs.

[0050] The original repulsive field function and attractive field function of the artificial potential field method are:

[0051]

[0052]

[0053] Where d represents the distance between the target point and the object, and k is a constant coefficient;

[0054] The original repulsive field function was derived to obtain the curve of distance and repulsive force growth rate. The analysis found that the repulsive force growth law obtained by the original repulsive force length function is not applicable to the nursing transfer task; see Figure 3 , assuming the distance threshold is 20, when the obstacle is between the left arm and the fifth link L l5 When the shortest distance between them is greater than or equal to 2 and less than 20, the repulsive force growth rate is small, and the repulsive force value is also small. At this time, the dual-arm robot is still moving towards the obstacle, and the obstacle avoidance effect is very small. When the obstacle is close to the fifth link L of the left arm of the dual-arm robot l5 When the shortest distance between them is less than 2, the repulsive force growth rate increases significantly, and the repulsive force value surges at the same time. At this time, the dual-arm robot quickly moves away from the obstacle. Since the dual-arm robot's nursing transfer task targets the person being cared for, the speed during the transfer process is unstable. For example, quickly moving away from an obstacle can reduce the safety of the person being cared for. Therefore, the original repulsive field function of the artificial potential field method is not applicable to nursing transfer tasks.

[0055] From the basic principle of obstacle avoidance using the artificial potential field method, the repulsive field function suitable for the dual-arm robot nursing transfer task should meet three conditions (considering the magnitude and direction of the repulsive force): (1) When the distance between the dual-arm robot and the obstacle is less than or equal to the distance threshold, the repulsive force should be less than or equal to 0; (2) The absolute value of the repulsive force should increase as the distance decreases, that is, the repulsive force decreases as the shortest distance decreases; (3) When the distance approaches infinity, the repulsive force should also approach infinity; Under the premise of meeting the three conditions, the repulsive field function suitable for the nursing transfer task is determined, that is, the repulsive force length function should ensure that: when the shortest distance between each link of the dual-arm robot's upper limb and the obstacle is less than the distance threshold and close to the distance threshold, it should have a large repulsive force growth rate and the repulsive force growth rate should increase steadily, and the repulsive force value should be large but not too large, so as to ensure that the dual-arm robot can start to avoid obstacles when it is far away from the obstacle and has a good obstacle avoidance effect, and the obstacle avoidance process is smooth and safe; Therefore, the present invention selects the tangent function as the repulsive field function, and the relationship curve between the shortest distance and repulsive force can be seen in Figure 4 ;

[0056] The normalized shortest distance Substituting this into the repulsive field function, we can obtain the repulsive force exerted by the obstacle on each link of the dual-arm robot's upper limb. The repulsive force obtained includes the direction. The repulsive field function is expressed as:

[0057]

[0058] At the same time, calculate the gravitational force of the target position on each link of the dual-arm robot's upper limbs; assuming that the target values of the left and right arm joint angles of the dual-arm robot are and The difference between the left arm joint angle and the target value is The difference between the right arm joint angle and the target value is Substitute the difference between each joint angle and the target value into the gravitational field function to obtain the gravitational force of the target position on each link of the dual-arm robot's upper limb. The obtained gravitational force includes the direction. The gravitational field function is expressed as:

[0059]

[0060] Where k0 represents a constant coefficient.

[0061] Step 4: Use the coordinates of the key points of the human body posture to calculate the angle θ1 between the connecting rod P2P3 and the horizontal plane, the angle θ2 between the connecting rod P2P3 and the connecting rod P3P4, and the angle θ3 between the connecting rod P3P4 and the horizontal plane. The calculation formulas for each angle are as follows:

[0062]

[0063]

[0064]

[0065] Where (0,0,1) is the normal vector of the horizontal plane;

[0066] During the transfer process, it is necessary to ensure the comfort of the care recipient while avoiding obstacles safely. Therefore, the corresponding connecting rod of the human body must meet certain constraints to ensure the safety and comfort of the care recipient. Specifically, through the transfer comfort evaluation experiment of the care recipient, it can be known that Figure 2, the angle θ1 between the link P2P3 formed by the human knee joint and waist joint and the horizontal plane must be greater than 15°, the angle θ2 between the link P2P3 and the link P3P4 formed by the waist joint and neck joint must be greater than 75°, and the angle θ3 between the link P3P4 and the horizontal plane must be greater than 15° to ensure the safety and comfort of the person being cared for; since the person being cared for does not have the initiative to control, the body movements of the person being cared for during the transfer process are all achieved by the dual-arm robot. Therefore, the arm movement of the dual-arm robot that is in direct contact with the person being cared for is controlled to ensure that the constraints of the human posture links need to be met;

[0067] Specifically, the fifth link L of the left arm of the dual-arm robot l5 and the fifth link L of the right arm r5 In direct contact with the human body, assuming that the head of the person being cared for is on the left side of the dual-arm robot, the fifth link L of the right arm r5 Contact with connecting rod P2P3, the fifth connecting rod L of the left arm l5 Contact with connecting rod P3P4; when θ1 does not meet the constraint condition, that is, the angle θ1 between connecting rod P2P3 and the horizontal plane is less than 15°, and θ2 and θ3 meet the constraint condition, then the fifth connecting rod L of the right arm r5 Apply a horizontal force F0 to the left at the corresponding joint, so that the angle θ1 between the link P2P3 and the horizontal direction is greater than 15°; when θ3 does not meet the constraint condition, that is, the angle θ3 between the link P3P4 and the horizontal plane is less than 15°, and θ1 and θ2 meet the constraint conditions, then the fifth link L of the left arm l5 Apply a horizontal force F1 to the right at the corresponding joint, so that the angle θ3 between the link P3P4 and the horizontal plane is greater than 15°; when θ2 does not meet the constraint condition or any two or three of the three angles θ1, θ2, and θ3 do not meet the constraint condition, then the fifth link L of the right arm r5 The horizontal force F0 is applied to the corresponding joint, and the fifth link L of the left arm l5 A horizontal force F1 is applied to the corresponding joint to the left. That is, the condition for the existence of force F0 is that one of θ1 and θ2 does not satisfy the constraint condition, or both do not satisfy the constraint condition. The condition for the existence of force F1 is that one of θ2 and θ3 does not satisfy the constraint condition, or both do not satisfy the constraint condition. If θ1, θ2, and θ3 all satisfy the constraint condition, no additional force is applied. F0 and F1 are both greater than zero.

[0068] Let the obstacle be on the fifth link L of the left arm of the dual-arm robot l5 And the right arm fifth link L r5 The repulsive forces are and In repulsion The superposition force F1 is the repulsive force In repulsion The superimposed force F0 is the repulsive force Finally, the repulsive force And the calculated repulsive force of the obstacle on the other links of the dual-arm robot upper limb And the gravitational force of the target position on each link of the dual-arm robot upper limb They act on the corresponding joints of the dual-arm robot respectively, and the angular acceleration of each joint of the upper limb is obtained through the Jacobian matrix at each joint. The joint movement of the dual-arm robot is controlled by the angular acceleration, so that the dual-arm robot can safely avoid obstacles while ensuring the comfort of the person being cared for.

[0069] The “left” and “right” directions in the above content correspond to the left and right of the dual-arm robot respectively, that is, the dual-arm robot is used as the reference.

[0070] Any matters not described in the present invention are applicable to the prior art.

Claims

1. A safety obstacle avoidance method for a dual-arm robot nursing transfer task, wherein the dual-arm robot holds a person being cared for and transfers the person from a starting position to a target position; characterized in that: The method comprises the following steps: Step 1. While the dual-arm robot is performing the nursing transfer task, an environmental image is collected; the environmental image is processed to obtain the coordinates of the key points of the human body posture and the coordinates of the obstacles. The coordinates of the key points of the human body posture include the coordinates of the ankle joint, knee joint, waist joint, neck joint and head; the upper limb joint angles of the dual-arm robot are obtained, including the left arm joint angles (a1, a2, a3, a4, a5, a6) and the right arm joint angles (b1, b2, b3, b4, b5, b6). a1, a2, a3, a4, a5, a6 are the six joint angles of the left arm from the top to the end, and b1, b2, b3, b4, b5, b6 are the six joint angles of the right arm from the top to the end. Step 2: Connect the key points of the human body posture through connecting rods in sequence, so that the ankle joint and knee joint form the connecting rod P1P2, the knee joint and waist joint form the connecting rod P2P3, the waist joint and neck joint form the connecting rod P3P4, and the neck joint and head form the connecting rod P4P5; similarly, simplify the two arms of the dual-arm robot to obtain the first connecting rod of the left arm to the sixth connecting rod of the left arm and the first connecting rod of the right arm to the sixth connecting rod of the right arm; Step 3: Calculate the shortest distance from the obstacle to each link of the dual-arm robot's upper limbs and normalize the shortest distance. Substitute each normalized shortest distance into the repulsive field function to calculate the repulsive force of the obstacle on each link of the dual-arm robot's upper limbs. The repulsive force obtained includes the direction. The repulsive field function is expressed as: Where, represents the normalized shortest distance, The general symbol for repulsive force, i is the symbol for the left and right arms of a dual-arm robot, where l and r represent the left and right arms respectively, and j = 1, 2, ···, 6 represents the link number; Assume that the target values of the left and right arm joint angles of the dual-arm robot are and The difference between the left arm joint angle and the target value is The difference between the right arm joint angle and the target value is Substitute the difference between each joint angle and the target value into the gravitational field function to obtain the gravitational force of the target position on each link of the dual-arm robot's upper limb. The obtained gravitational force includes the direction. The gravitational field function is expressed as: Where k0 represents a constant coefficient; Step 4: Calculate the angle θ1 between the connecting rod P2P3 and the horizontal plane, the angle θ2 between the connecting rod P2P3 and the connecting rod P3P4, and the angle θ3 between the connecting rod P3P4 and the horizontal plane; determine whether the angles θ1, θ2, and θ3 meet the constraints. If they do, no additional force is applied; if θ1 does not meet the constraints, but θ2 and θ3 meet the constraints, then the fifth connecting rod L on the right arm is r5 Apply a horizontal force F0 to the left at the joint; if θ3 does not satisfy, θ1 and θ2 satisfy, then the fifth link L of the left arm l5 Apply a horizontal force F1 to the right at the joint of the right arm; if θ2 does not satisfy or any two or three of θ1, θ2, θ3 do not satisfy, then the fifth link L of the right arm r5 Apply a horizontal force F0 to the right at the joint of the left arm, and l5 A horizontal force F1 is applied to the joint to the left; F0 and F1 are both greater than zero; the constraints are: θ1>15°, θ2>75°, θ3>15°; Let the obstacle be on the fifth link L of the left arm of the dual-arm robot l5 And the right arm fifth link L r5 The repulsive forces are and In repulsion The superposition force F1 is the repulsive force In repulsion The superposition force F0 is based on the repulsive force The repulsive force The calculated repulsive force of the obstacle on the other links of the dual-arm robot's upper limbs and the attractive force of the target position on the links of the dual-arm robot's upper limbs act on the corresponding joints of the dual-arm robot respectively. The angular acceleration of each joint of the upper limb is obtained through the Jacobian matrix of each joint, and the angular acceleration is used to control the joint movement of the dual-arm robot to ensure the comfort of the person being cared for while avoiding obstacles safely.

2. The safe obstacle avoidance method for dual-arm robot nursing transfer tasks according to claim 1 is characterized in that: Assume that the obstacle coordinates are (x m ,y m ,z m ), the spatial coordinates of the two end points of the first link of the left arm are (x l0 ,y l0 ,z l0 )、(x l1 ,y l1 ,z l1 ), then the distance from the obstacle to the first link of the left arm is: If R>1, the shortest distance from the obstacle to the first link of the left arm is: S l1 =(x m -x l1 ,y m -y l1 ,z m -z l1 ) If R < 0, the shortest distance from the obstacle to the first link of the left arm is: S l1 =(x m -x l0 ,y m -y l0 ,z m -z l0 ) If 0≤R≤1, the shortest distance from the obstacle to the first link of the left arm is:

Images