CN114918923B - A method and system for a robotic arm to avoid the human body in a close-range human-machine collaboration environment - Google Patents

Abstract

The invention proposes a method and system for a robot arm to avoid the human body in a close-range human-machine collaboration environment, and belongs to the technical field of mechanical path planning under human-machine collaboration. Including: obtaining the position information of the upper body joint points of the human body through the IMU inertial sensor; converting the obtained joint point position information of the human body into equivalent obstacles in the working space of the robotic arm; based on the equivalent obstacles, when given a simple task for the robotic arm Based on this, the improved RRT‑CONNECT method is used to generate a collision avoidance working path; the generated working path is optimized and the robotic arm is driven to move. This invention obtains the state of the human body through the IMU, converts the state into the working space of the robotic arm, and then uses the improved RRT-CONNECT method to expand the collision avoidance path of the robotic arm, thereby preventing the operator and the robotic arm from working together at close range. time collision.

Description

A method and system for a robotic arm to avoid the human body in a close-range human-machine collaboration environment

Technical field

The invention belongs to the technical field of mechanical path planning under human-machine collaboration, and in particular relates to a method and system for a robotic arm to avoid the human body in a close-range human-machine collaboration environment.

Background technique

The statements in this section merely provide background technical information related to the present invention and do not necessarily constitute prior art.

Traditional industrial robots have the characteristics of high precision, high speed and high load, so traditional industrial robots have the characteristics of high deployment cost. With the development of society, more and more small-batch, customized, and short-cycle task requirements have emerged. It is too expensive to use traditional industrial robots. Therefore, collaborative robots have emerged. The core of collaborative robots is "human machine collaboration”. Human-machine collaboration refers to humans and robots completing a task together at close range, and safety issues during human-machine collaboration have always been the focus and difficulty in the development of human-machine collaboration. Although some current mainstream collaborative robot arms have included some safety measures. When the robot arm detects a collision, it will make an emergency stop to avoid harm to the human body. However, this method will cause the robot arm to stop and restart, which will affect the efficiency at best and the worst at worst. Otherwise, the mechanical arm will be damaged.

In the collision avoidance of human-machine collaboration, the posture recognition of the human body is particularly important. The current tracking methods of the human body are roughly divided into optical, acoustic, mechanical exoskeleton, inertial sensor IMU and other aspects. Optics obtains human body information through camera tracking of Mark points, which leads to the disadvantage of requiring an unobstructed environment. The acoustic tracking effect is difficult to meet the requirements of collision avoidance. The mechanical exoskeleton is relatively stiff and highly restricted. Although the inertial sensor may be affected by a strong magnetic field, considering that strong magnetic fields will not appear in most scenes, in comparison The practical application value of inertial sensors is even higher.

At present, research on obstacle avoidance of robotic arms is mainly divided into RRT and artificial potential field methods, but they are all applied in simple scenarios to achieve basic obstacle avoidance of stationary obstacles and some simple moving obstacles. There is no way to use it in human-machine collaboration scenarios.

Contents of the invention

In order to overcome the above-mentioned shortcomings of the prior art, the present invention provides a method and system for a robot arm to avoid the human body in a close-range human-machine collaboration environment. The state of the human body is acquired through the IMU, and the state is converted into the working space of the robot arm. The improved RRT-CONNECT method is used to expand the collision avoidance path of the robotic arm to avoid collisions between the operator and the robotic arm when they work together at close range.

To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions:

The first aspect of the present invention provides a method for a robotic arm to avoid the human body in a close human-machine collaboration environment, including:

Obtain the joint point position information of the upper body of the human body;

Convert human body joint point position information into equivalent obstacles in the robotic arm workspace;

Based on equivalent obstacles and given a simple task for the robotic arm, the collision avoidance working path is generated;

Optimize the generated working path and drive the robotic arm to move.

A second aspect of the present invention provides a system for a robot arm to avoid the human body in a close-range human-machine collaboration environment, including:

inertial sensors, processors, and robotic arms;

The inertial sensor is used to obtain the joint point position information of the upper body of the human body and transmit the joint point position information of the upper body of the human body to the processor;

The processor is used to convert human body joint point position information into equivalent obstacles in the robot arm's working space, and based on the equivalent obstacles, based on a simple task given to the robot arm, generates and generates collision avoidance working paths. Optimize and transmit the optimized path instructions to the robotic arm;

The robotic arm is used to receive path instructions issued by the processor and move according to the path instructions.

A third aspect of the present invention provides a computer-readable storage medium on which a program is stored. When the program is executed by a processor, the method for a robot arm to avoid the human body in a close human-machine collaboration environment as described in the first aspect of the present invention is implemented. step.

A fourth aspect of the present invention provides an electronic device, including a memory, a processor, and a program stored in the memory and executable on the processor. When the processor executes the program, the near-field method as described in the first aspect of the present invention is implemented. Steps in the method for robot arms to avoid the human body in a human-machine collaboration environment.

One or more of the above technical solutions have the following beneficial effects:

The present invention provides a method and system for a robotic arm to avoid the human body in real time in a specific close-range human-machine collaboration working environment. The state of the human body is obtained through the IMU, and the state is converted into the workspace of the robotic arm. The improved RRT-CONNECT method is then used to expand the collision avoidance path of the robotic arm, thereby avoiding collisions between the operator and the robotic arm when they work together at close range. collision.

Additional advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention.

Description of the drawings

The description and drawings that constitute a part of the present invention are used to provide a further understanding of the present invention. The illustrative embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention.

Figure 1 is a schematic flow chart of a method for an IMU-based robotic arm to avoid the human body in a close-range human-machine collaboration environment provided by an embodiment of the present invention;

Figure 2 is a schematic diagram of the distribution of IMUs in the upper body of the human body and the joint points of the upper body of the human body provided by the embodiment of the present application;

Figure 3 is a schematic diagram of the test system environment and the coordinate system distribution provided by the embodiment of the present application;

Figure 4 is a schematic flowchart of the improved RRT-CONNECT generation of collision avoidance paths provided by the embodiment of the present application;

Figure 5 is an operation flow chart of the test system provided by the embodiment of the present application.

Detailed ways

The present disclosure will be further described below in conjunction with the accompanying drawings and examples.

It should be noted that the following detailed description is illustrative and is intended to provide further explanation of the present application. Unless otherwise defined, all technical and scientific terms used herein have the same meanings commonly understood by one of ordinary skill in the art to which this application belongs.

It should be noted that the terms used herein are only for describing specific embodiments and are not intended to limit the exemplary embodiments according to the present application. As used herein, the singular forms are also intended to include the plural forms unless the context clearly indicates otherwise. Furthermore, it will be understood that when the terms "comprises" and/or "includes" are used in this specification, they indicate There are features, steps, operations, means, components and/or combinations thereof.

In this disclosure, terms such as "upper", "lower", "left", "right", "front", "rear", "vertical", "horizontal", "side", "bottom", etc. The orientation or positional relationship is based on the orientation or positional relationship shown in the drawings. It is only a relative word determined to facilitate the description of the structural relationship of various components or elements of the present disclosure. It does not specifically refer to any component or element of the present disclosure and cannot be understood as a reference to the present disclosure. Public restrictions.

In this disclosure, terms such as "fixed", "connected", "connected", etc. should be understood in a broad sense, indicating that it can be a fixed connection, an integral connection or a detachable connection; it can be directly connected, or it can be connected through an intermediate connection. Media are indirectly connected. For relevant scientific researchers or technicians in this field, the specific meanings of the above terms in this disclosure can be determined according to specific circumstances, and should not be understood as limitations of this disclosure.

Embodiment 1

This embodiment discloses a method for a robotic arm to avoid the human body in a close-range human-machine collaboration environment.

As shown in Figure 1, a method for a robotic arm to avoid the human body in a close human-machine collaboration environment includes the following steps:

(1) Obtain the position information of the joint points of the upper body of the human body;

(2) Convert human body joint point position information into equivalent obstacles in the robotic arm working space;

(3) Based on equivalent obstacles and given a simple task for the robotic arm, generate a collision avoidance working path;

(4) Optimize the generated working path and drive the robotic arm to move.

Explain the coordinate system symbols used below:

G: world coordinate system;

S: Inertial measurement unit coordinate system;

B: Human body-based coordinate system;

R: Cartesian coordinate system of robotic arm workspace;

D: The coordinate system of a selected point in the world coordinate system.

(1) Obtain the joint point position information of the upper body of the human body, specifically:

The position information of the joint points of the upper body of the human body is obtained through the IMU inertial sensor. An IMU sensor is placed on the lower side of the operator's chest, the upper half joint of the left arm, the lower half joint of the left arm, the upper half joint of the right arm, and the lower half joint of the right arm. The IMU is bound to the joints of the human body, and the posture changes of the IMU are used to represent changes in the human body's motion state.

First, obtain the rotation matrix of each single joint: the posture of the human body base coordinate system in the global coordinate scene is expressed as The attitude of the inertial measurement unit coordinate system in the global coordinate system is expressed as/> The posture of the human body base coordinate system in the inertial measurement unit coordinate system is expressed as/>

The IMU can return unit quaternion data Q = [q _x q _y q _z q _w ], which is obtained by converting the quaternion into a rotation matrix:

Then, obtain the joint point position information of the human body, as shown in Figure 2. When the limb size of the human body is known, the upper body posture information of the human body can be represented by 8 point position information from 0 to 7. When the operator sits When sitting on a stool, first sit up straight and perform initial position calibration:

When stationary, the initial positions of 8 points from 0 to 7 can be obtained through the joint length.

The position coordinate of point 0 in the human body coordinate system {B} is P _B0 = [x ₀ y ₀ z ₀ ] ^T . The coordinates of the 0-point position will not change with the operator's actions.

The initial position of point 1 is P _B1 = [x ₁ y ₁ z ₁ ] ^T in the coordinate system of point {B}. Using the posture change of the chest IMU sensor to equivalent the posture change of the upper body trunk of the human body, we can obtain the state change of the human body. The position coordinates of 1 point in the coordinate system of point {B} are

When calculating the position of 2 points, use the 1-point coordinate system as the reference point, and obtain the initial position coordinates of 2 points in the 1-point coordinate system as P ₁₂ = [x ₂ y ₂ z ₂ ] ^T . Same as above, the position coordinates of the two points in the {B} point coordinate system after the human body state changes are In summary, it can be concluded that the position coordinates of the two points in the coordinate system of point {B} are P _B2 ′=P _B1 ′+P ₁₂ ′.

The position coordinate principle of the remaining points is the same as that of 2 points, and a total of 8 joint point coordinates such as P _B3 , P _B4 , P _B5 , P _B6 , and P _B7 can be obtained.

(2) Convert human body joint point position information into equivalent obstacles in the robotic arm working space. The specific process is:

First, the human body joint point position information obtained in step (1) under the human body base coordinate system {B} is converted to the Cartesian coordinate system {R} of the robot arm work space. By overlapping the human body base coordinate system {B} and the {D} coordinate system, the joint point position information in the {D} coordinate system can be obtained as:

P _Di =P _Bi (i＝0,1,2,...,7)

The point position ^D P in the {D} coordinate system and the position ^R P in the {R} coordinate system can be expressed as:

Correspondingly, the position coordinates of the joint points of the upper body of the human body in the {R} coordinate system of the robot arm workspace can be obtained:

Among them, P _Di is the coordinate of the joint point in the {D} coordinate system; P _Bi is the coordinate of the joint point in the {B} coordinate system; P _Ri is the coordinate of the joint point in the {R} coordinate system; is the rotation matrix of the {D} coordinate system relative to the {R} coordinate system; ^R P _DORG is the position vector of the origin of the {D} coordinate system in the {R} coordinate system.

Then, the human body is transformed into an obstacle in the Cartesian coordinate system of the robot arm's workspace. Because when the operator is operating on the workbench, only the large and small arms may collide with the robotic arm, so the robot arm only needs to avoid the human body's large and small arms to avoid collisions.

Using the cylinder envelope method, the two pairs of large and small arms of the human body can be regarded as four equivalent cylinders. The position information of a cylinder can be represented by the coordinates of the center points of the top and bottom surfaces of the cylinder and the radius of the cylinder, so the left The equivalent cylindrical obstacle of the side arm can be expressed as:

Obs_Lb＝[P _R2x P _R2y P _R2z P _R3x P _R3y P _R3z r]

P ₂ is the 2-point coordinate, P ₃ is the 3-point coordinate, and r is the arm radius. In the same way, the other large and small arms can be equivalent to:

Obs_Ls＝[P _R3x P _R3y P _R3z P _R4x P _R4y P _R4z r]

Obs_Rb＝[P _R5x P _R5y P _R5z P _R6x P _R6y P _R6z r]

Obs_Rs＝[P _R6x P _R6y P _R6z P _R7x P _R7y P _R7z r]

(3) Based on equivalent obstacles and given a simple task for the robotic arm, the improved RRT-CONNECT method is used to generate a collision avoidance working path.

This embodiment takes a six-axis robotic arm as an example. The specific process of RRT-CONNECT is shown in Figure 4. The specific process is:

Step1: Given the initial position point q _init and the target position point q _goal in the robot arm joint space, q _init and q _goal are six-dimensional vectors.

Step2: Update the task starting point to the joint position q′ _init of the current robotic arm, and then update the human arm obstacle information obtained in step 2.

Step3: Using the initial joint position q′ _init and the target joint position q _goal as root nodes, generate rapidly expanding random trees TreeA and TreeB.

The random tree contains a set Q_List of joint position vectors and the connection relationship between the joint vectors. Taking TreeA as an example, randomly pick a joint position vector q _sample in the joint space, traverse all joint vectors in the random tree TreeA, and find the joint vector q _near that is closest to q _sample .

Among them, ||qq _sample || is the Euclidean distance between q and q _sample , and the direction from q _near to q _sample is the expansion direction of the new node q _new of the random tree. In terms of the expansion length of q _new , since q _new is a six-dimensional vector, here the six-dimensional vector is regarded as two three-dimensional vectors, and the expansion step is set to L, then the expansion coordinates of the first three-dimensional variable are:

q _new [0]＝q _near [0]+L*cos(θ _z )cos(θ _y )

q _new [1]＝q _near [1]+L*cos(θ _z )sin(θ _y )

q _new [2]＝q _near [2]+L*sin(θ _z )

in:

θ _y =arctan((q _sample [1]-q _near [1])/(q _sample [0]-q _near [0])),

θ _z =arctan((q _sample [2]-q _near [2])/[(q _sample [0]-q _near [0]) ² +(q _sample [1]-q _near [1]) ² ] ^1/2 )

In the same way, the expanded coordinates of the second three-dimensional variable are:

q _new [3]＝q _near [3]+L*cos(θ _z2 )cos(θ _y2 )

q _new [4]＝q _near [4]+L*cos(θ _z2 )sin(θ _y2 )

q _new [5]＝q _near [5]+L*sin(θ _z2 )

in:

θ _y2 = arctan((q _sample [4]-q _near [4])/(q _sample [3]-q _near [3])),

θ _z2 = arctan((q _sample [5]-q _near [5])/[(q _sample [3]-q _near [3]) ² +(q _sample [4]-q _near [4]) ² ] ^1/2 ).

According to this method, the random value of the six-dimensional vector in the joint space can be realized, and the obtained value can reduce to a certain extent the large difference in the speed of each joint caused by the different expansion lengths of different joints.

Step4: Collision detection and determination. The specific content is as follows: Through forward kinematics, the coordinates p _{axis (0,1,2,3,4,5,6,7)} of each joint point of the six-axis robotic arm can be calculated, and the corresponding approximate collision model of the robotic arm can be obtained :

Obs_Axis6＝[P _axis6x P _axis6y P _axis6z P _axis7x P _axis7y P _axis7z r]

Obs_Axis5＝[P _axis5x P _axis5y P _axis5z P _axis6x P _axis6y P _axis6z r]

Obs_Axis4＝[P _axis4x P _axis4y P _axis4z P _axis5x P _axis5y P _axis5z r]

Then calculate the minimum distance d between the equivalent cylinder of the robotic arm and the equivalent cylinder of the human arm in sequence, and set a certain protection range dm (dm>r1+r2). r1 and r2 are the radius lengths of the equivalent obstacles of the human body respectively. The radius length of the obstacle equivalent to the robot arm. When d≤dm, the two are considered to have collided, otherwise they are considered to have collided.

Step5: Determine whether the forward and reverse trees are connected. The specific method is to detect whether there will be a collision in the path between the two new nodes of the forward tree and the reverse tree. If there is a collision, it is not connected. If there is no collision on the path, If a collision occurs, it is connected.

Step6: Integrate forward and reverse random trees to obtain a complete collision avoidance path

(4) Optimize the generated working path and drive the robotic arm to move.

The specific process of optimizing the generated working path and driving the robotic arm to move is:

Since the path obtained by RRT-CONNECT has a certain degree of randomness, certain optimization processing is required. For a joint space position node q _n in the random tree, calculate several joint space position nodes q _k after q _n (k>n) The distance between q _n and q n ||q _k -q _n ||, detect whether the path between the joint q _m and q _{n with the smallest distance q n} will cause collision information. If no collision occurs, _{q m will} be used as q The next joint of _n . This method can effectively shorten the path length and reduce useless random nodes. The optimized path is then interpolated and discretized to drive the robotic arm to move.

The steps to drive the robot arm to move are as follows: detect whether the current joint space position of the robot arm collides with the next moving joint space position. If a collision occurs, re-plan the path; if no collision occurs, drive the robot arm to reach the joint space. Location.

Embodiment 2

This embodiment discloses a system for a robot arm to avoid the human body in a close-range human-machine collaboration environment, including:

inertial sensors, processors, and robotic arms;

The inertial sensor is used to obtain the joint point position information of the upper body of the human body and transmit the joint point position information of the upper body of the human body to the processor;

The processor is used to convert human body joint point position information into equivalent obstacles in the robot arm's working space, and based on the equivalent obstacles, based on a simple task given to the robot arm, generates and generates collision avoidance working paths. Optimize and transmit the optimized path to the robotic arm;

The robotic arm is used to receive the path sent by the processor and move according to the path.

Embodiment 3

The purpose of this embodiment is to provide a computer-readable storage medium.

A computer-readable storage medium has a computer program stored thereon. When the program is executed by a processor, the steps of the method for a robot arm to avoid the human body in a close-range human-machine collaboration environment are implemented as described in Embodiment 1 of the present disclosure.

Embodiment 4

The purpose of this embodiment is to provide electronic equipment.

The electronic device includes a memory, a processor, and a program stored in the memory and executable on the processor. When the processor executes the program, the machine in a close-range human-machine collaboration environment as described in Embodiment 1 of the present disclosure is realized. Steps in the arm avoidance method.

Each step involved in the devices of the above second, third and fourth embodiments corresponds to the first method embodiment. For specific implementation details, please refer to the relevant description part of the first embodiment. The term "computer-readable storage medium" shall be understood to include a single medium or multiple media that includes one or more sets of instructions; and shall also be understood to include any medium capable of storing, encoding, or carrying instructions for use by a processor. The executed instruction set causes the processor to perform any method in the present invention.

Those skilled in the art should understand that each module or each step of the present invention described above can be implemented by a general-purpose computer device. Alternatively, they can be implemented by program codes executable by the computing device, so that they can be stored in a storage device. The device is executed by a computing device, or they are respectively made into individual integrated circuit modules, or multiple modules or steps among them are made into a single integrated circuit module. The invention is not limited to any specific combination of hardware and software.

Although the specific embodiments of the present invention have been described above in conjunction with the accompanying drawings, they do not limit the scope of the present invention. Those skilled in the art should understand that based on the technical solutions of the present invention, those skilled in the art do not need to perform creative work. Various modifications or variations that can be made are still within the protection scope of the present invention.

Claims (8)

1. A method for avoiding human bodies by a mechanical arm in a near man-machine cooperation environment is characterized by comprising the following steps:

acquiring the position information of an articular point of the upper body of a human body;

converting the position information of the joint points of the human body into equivalent barriers in the working space of the mechanical arm;

based on equivalent barriers, generating a collision avoidance working path on the basis of giving a simple task to the mechanical arm;

optimizing the generated working path and driving the mechanical arm to move;

the collision avoidance working path is generated, and the specific process is as follows:

step1: giving an initial position point in the joint space of the mechanical armIs +.>；

Step2: updating the task starting point to be the joint position of the current mechanical armThen updating the obstacle information;

step3: in the initial joint positionTo the target joint position->Expanding a new node by using a new node expansion method to generate a rapid expansion random tree TreeA and a rapid expansion random tree TreeB;

step4: performing collision detection, executing Step5 if no collision is determined, otherwise executing Step3;

step5: judging whether the forward tree and the reverse tree are connected, if the judgment result is that the forward tree and the reverse tree are connected, executing Step6, otherwise, executing Step3;

step6: integrating the forward and reverse random trees to obtain a complete collision prevention path;

the new node expansion method is used for expanding the new node, and the specific contents are as follows:

randomly taking a joint position vector in joint spaceTraversing all joint vectors in the random tree to find distance +.>Nearest joint vector->Then

wherein ,for q and->Euclidean distance between->To->Is the new node of the random tree->Q_List is a set of joint position vectors;

at the position ofIs extended by six-dimensional vector +.>Regarding two three-dimensional vectors, setting the expansion step length in the three-dimensional space as L, and then the expansion coordinate of the first three-dimensional variable is as follows:

wherein ：

，

the extended coordinates of the second three-dimensional variable are:

wherein ：

，

。

2. the method for avoiding human body by using the mechanical arm in the near-distance man-machine cooperation environment according to claim 1, wherein the obtaining of the position information of the joint point of the upper body of the human body is specifically as follows:

and acquiring position information of the lower side of the chest, the upper half joint of the left arm, the lower half joint of the left arm, the upper half joint of the right arm and the lower half joint of the right arm under the human body base coordinate system B.

3. The method for avoiding human bodies by using the mechanical arm in the near-distance man-machine cooperation environment according to claim 2, wherein the method is characterized in that the position information of the joint points of the human bodies is converted into equivalent obstacles in the working space of the mechanical arm, and comprises the following specific processes:

and converting the position information of the human body articulation point under the human body base coordinate system B into a mechanical arm working space Cartesian coordinate system R, and then converting the human body into an obstacle under the mechanical arm working space Cartesian coordinate system R.

4. The robotic arm of claim 1 in a near man-machine collaboration environmentThe human body evading method is characterized in that in the collision detection, the coordinates of each joint point of the six-axis mechanical arm are obtained through positive kinematics calculationThe collision model of the mechanical arm is obtained as follows:

。

5. the method for avoiding human bodies by using the mechanical arm in the near-distance man-machine cooperation environment according to claim 1, wherein the method is characterized in that the generated working path is optimized and the mechanical arm is driven to move, and the specific process is as follows:

detecting whether the joint space position of the current mechanical arm collides with the joint space position of the next movement, and if so, re-planning a path; if no collision occurs, the mechanical arm is driven to reach the joint space position.

6. The utility model provides a robotic arm avoids human system under close-range man-machine cooperation environment which characterized in that includes:

inertial sensor, processor and mechanical arm;

the inertial sensor is used for acquiring the position information of the joint point of the upper body of the human body and transmitting the position information of the joint point of the upper body of the human body to the processor;

the processor is used for converting the position information of the joint points of the human body into equivalent barriers in the working space of the mechanical arm, generating and optimizing a collision avoidance working path on the basis of giving a simple task to the mechanical arm based on the equivalent barriers, and transmitting an optimized path instruction to the mechanical arm;

the mechanical arm is used for receiving a path instruction sent by the processor and moving according to the path instruction;

the collision avoidance working path is generated, and the specific process is as follows:

step1: giving an initial position point in the joint space of the mechanical armIs +.>；

Step2: updating the task starting point to be the joint position of the current mechanical armThen updating the obstacle information;

step3: in the initial joint positionTo the target joint position->Expanding a new node by using a new node expansion method to generate a rapid expansion random tree TreeA and a rapid expansion random tree TreeB;

step4: performing collision detection, executing Step5 if no collision is determined, otherwise executing Step3;

step5: judging whether the forward tree and the reverse tree are connected, if the judgment result is that the forward tree and the reverse tree are connected, executing Step6, otherwise, executing Step3;

step6: integrating the forward and reverse random trees to obtain a complete collision prevention path;

the new node expansion method is used for expanding the new node, and the specific contents are as follows:

randomly taking a joint position vector in joint spaceTraversing all joint vectors in the random tree to find distance +.>Nearest joint vector->Then

wherein ,for q and->Euclidean distance between->To->Is the new node of the random tree->Q_List is a set of joint position vectors;

at the position ofIs extended by six-dimensional vector +.>Regarding two three-dimensional vectors, setting the expansion step length in the three-dimensional space as L, and then the expansion coordinate of the first three-dimensional variable is as follows:

wherein ：

，

the extended coordinates of the second three-dimensional variable are:

wherein ：

，

。

7. a computer readable storage medium having stored thereon a program, which when executed by a processor, performs the steps of the method for robotic arm avoidance of human body in a near man machine collaboration environment according to any of claims 1-5.

8. Electronic device comprising a memory, a processor and a program stored on the memory and executable on the processor, characterized in that the processor, when executing the program, realizes the steps in the method for avoiding a human body by a mechanical arm in a near man-machine cooperation environment according to any one of claims 1-5.