WO2021036847A1

WO2021036847A1 - Method for planning boundary movement in monitoring area of industrial robot

Info

Publication number: WO2021036847A1
Application number: PCT/CN2020/109600
Authority: WO
Inventors: 夏正仙; 洪晴; 王杰高
Original assignee: 南京埃斯顿机器人工程有限公司
Priority date: 2019-08-23
Filing date: 2020-08-17
Publication date: 2021-03-04
Also published as: CN110480639B; CN110480639A

Abstract

A method for planning boundary movement in a monitoring area of an industrial robot. The method comprises: calculating a virtual monitoring area according to the monitoring area and a current speed of the robot set by a user; and when the robot moves to enter the virtual monitoring area, carrying out deceleration control, so that the speed of the robot moving to the boundary of the monitoring area is in a relatively low range, thereby preventing damage from excessively high moment caused by emergency braking at a high speed. In deceleration control of the robot, a prospective planned trajectory speed is adjusted by planning the speed ratio of a controller, and the moving trajectory of the robot is not affected. Moreover, by considering the moving trend of the robot, the virtual monitoring area is refreshed dynamically. When the real-time position of the robot is not in the refreshed monitoring area, the moving trajectory of the robot can change in a direction away from the boundary of the monitoring area, and at this moment, the robot gradually restores the speed before deceleration with a certain rule, avoiding the operating efficiency of the robot being reduced excessively while safe braking is guaranteed.

Description

A method for motion planning of industrial robot monitoring area boundary

Technical field

The invention relates to a method for motion planning of an industrial robot monitoring area boundary.

Background technique

Industrial robots are being used more and more widely in the industrial field with their large range of motion, flexible and fast motion capabilities. Usually the working space of an industrial robot is limited, and there may be obstacles near its range of motion; a robot workstation is not only a single robot working, there are other equipment such as a welding machine or other robots working together; the user also operates the robot. A relatively safe operating area is required. Therefore, robot avoiding obstacles and ensuring the safety of other equipment and personnel working in coordination has become an important issue that needs to be solved.

In order to solve the safety problem of the robot working area, the Chinese invention patent case CN105555490A proposes that in the robot working area, according to the degree of danger of the operator and the robot, the working area of different dangerous procedures is divided by different colors, and then the work is based on the different degree of danger. Area, which limits the different moving speeds of the robot. In addition, the Chinese invention patent case CN108656103A builds multiple separation interfaces with three non-collinear points in the work area to form a full separation surface, thereby dividing the work area into a safe area and a prohibited area. In the divided safety zone, the robot can run at full speed and brakes when entering the prohibited zone.

In the case of high-speed robot movement, when entering a prohibited area from a safe working area, direct emergency braking will cause the robot's joints to bear excessive torque, affect the performance of the motor, and even damage the mechanical structure, causing harm. Therefore, the general robot needs to decelerate in advance before entering the forbidden area. None of the above patents mentions the method of decelerating the movement of the robot near the boundary of the area. In addition, the position and speed of the robot are complex and changeable, and the judgment condition of early deceleration may cause misjudgment. If the deceleration process is not repaired in time, it may cause unnecessary shutdown of the robot.

Summary of the invention

The purpose of the present invention is to overcome the shortcomings of the prior art and propose a method of motion planning for the boundary of the monitoring area of an industrial robot. According to the monitoring area set by the user and the current speed of the robot, the virtual monitoring area is calculated. When the robot runs into the virtual When monitoring the area, perform deceleration control, so that when the robot moves to the boundary of the monitoring area, the speed is in a relatively low range to avoid the hazard of excessive torque caused by emergency braking at high speed; the deceleration control of the robot is through planning control The speed multiplier of the robot adjusts the forward-planned trajectory speed, and the robot motion trajectory is not affected; at the same time, taking into account the robot’s motion trend, the virtual monitoring area is dynamically refreshed. When the real-time position of the robot is not in the refreshed virtual monitoring area, it indicates that the robot The motion trajectory may change away from the boundary of the monitoring area. At this time, the speed before the deceleration is gradually restored according to a certain rule to avoid excessive reduction of the robot's operating efficiency while ensuring safe braking.

The purpose of the present invention is to solve the problem of damage to the robot body caused by emergency braking when the robot runs to the boundary of the monitoring area at a high speed and enters the forbidden area. The basic idea is as follows:

The robot monitoring area can be any shape in the three-dimensional space. The robot can work in the area or outside the area, and the user can set it according to the work space. As shown in Figure 1 (the robot works in the monitoring area) and Figure 2 (the robot works outside the monitoring area), area O is the set monitoring area, P is the current position of the robot TCP, and V is the current speed of the robot TCP.

According to the robot speed V and the maximum safe acceleration a _max , by formula (1), calculate the advance deceleration distance △L.

△L=V ² /2a _max (1)

If the robot is operating in the area, the boundary of the monitoring area O will be shifted inward by ΔL, if the robot is operating outside the area, the boundary of the monitoring area O will be shifted outward by ΔL to obtain the virtual monitoring area O' , The shaded area between O'and O is the pre-deceleration zone.

The speed V is _{calculated from the speed V plan of the} robot trajectory plan and the speed multiplier K of the current cycle, as follows:

V=K×V _plan (2)

The speed magnification K before deceleration is equal to the speed magnification K _s set by the original user.

When the robot TCP position P has not reached the virtual monitoring area O', the robot keeps the original speed multiplier K _s movement. When the robot TCP position P enters the virtual monitoring area O', the design rate multiplier adjustment function K _sub (t) is decelerated in advance . The speed override adjustment function K _sub (t) is a polynomial of time, and the boundary conditions are as follows:

The range of time t is 0～T _sub , and T _sub is the adjustment time set by the user. The smaller the value, the shorter the adjustment process. K _cur is the initial value of K _sub (t), that is, the speed multiplier of the current cycle. The K _cur adjusted for the first time is equal to the original multiplier K _s .

During speed adjustment, if K is equal to K _sub (t), the robot will decelerate.

The speed V changes after deceleration, and the changed △L is calculated according to formula (1). The monitoring area O needs to be shifted correspondingly, and the virtual monitoring area O'is refreshed in real time. According to the relative position change between the current TCP point P and the virtual monitoring area O', determine the movement trend of the robot. If the current point P is out of the virtual monitoring area, readjust the speed override adjustment function, and gradually restore to the original speed override K _s , adjust The subsequent speed override adjustment function is _K'sub (t). The whole process implements dynamic acceleration and deceleration to avoid excessively reducing the speed of the robot and improve the working efficiency of the robot as much as possible. The boundary conditions of the adjusted polynomial _K'sub (t) are as follows:

Among them, T _sub , K _cur , and K _s have the same meanings as above.

Based on the above-mentioned technical analysis, the present invention is a technical solution for realizing the purpose of the invention, an industrial robot monitoring area boundary motion planning method, and the steps are as follows:

Step 1. Input the current TCP speed V and the maximum safe acceleration a _max to calculate the advance deceleration distance △L:

△L=V ² /2a _max

If the user sets the robot to work in the area, the set monitoring area O will be offset inward by the offset distance △L to obtain the virtual monitoring area O'; if the user sets the robot to work outside the area, the set offset will be set to the outside The monitoring area O is offset by a distance of △L to obtain a virtual safety area O'.

Step 2. Determine the current TCP position P of the robot:

If the current TCP position point P of the robot is between the virtual monitoring area O'and the monitoring area O, the _{TCP motion speed is reduced according to the designed speed override adjustment function K sub} (t), and the speed is decelerated in advance.

If the current TCP position P of the robot is outside the virtual monitoring area O', judge whether the last motion cycle has been decelerated in advance:

If the last motion cycle did not decelerate earlier, indicating that the robot did not enter the virtual monitoring area in the last cycle, the motion planning will not be interfered and the current speed override will be maintained.

If the last motion cycle has decelerated in advance, it means that the robot was in the virtual monitoring area in the last cycle, that is, the current robot motion trend is far away from the boundary of the monitoring area, or its trajectory is only close to the monitoring area but will not cross the boundary. The redesigned magnification adjustment function _K'sub (t) gradually returns to the original planned speed magnification, so as to avoid excessive reduction of the robot's operating efficiency.

Step 3. The current TCP position P of the robot has reached the boundary of the monitoring area O. At this time, the robot's motion speed V has been decelerated in advance and is already in a relatively low range. Therefore, emergency braking can be implemented and the robot immediately stops moving.

Step 4. Recalculate the parameter △L every cycle and refresh the virtual monitoring area O'. Repeat steps 1-3 to realize the dynamic acceleration and deceleration adjustment of the TCP speed until the movement reaches the planned position or reaches the boundary of the monitoring area.

The method for planning the boundary motion of the industrial robot monitoring area of the present invention calculates the virtual monitoring area according to the motion speed, and decelerates in advance when crossing the virtual monitoring area, and prevents the large torque damage caused by emergency braking while ensuring the safety of the machine and personnel.

The industrial robot monitoring area boundary motion planning method of the present invention performs deceleration control on the robot by adjusting the forward-planned trajectory speed by planning the speed multiplier of the controller. There is no need to re-plan the robot's trajectory and speed, and the speed multiplier algorithm is planned It is simpler, and the trajectory of the robot is not affected, and it does not bring about the problem of uncontrollable trajectory.

The method for planning the boundary motion of the industrial robot monitoring area of the present invention dynamically refreshes the virtual monitoring area in real time, and combines the robot motion trend. If the trend is far away from the boundary of the monitoring area, it gradually restores to the original speed multiplier according to a certain rule, otherwise it continues to decelerate. Through dynamic acceleration and deceleration adjustment, the robot operation efficiency is maximized.

The method for the motion planning of the industrial robot monitoring area boundary of the present invention does not require any additional external devices, is all realized by software, has no hardware cost, and is easy for users to operate.

Description of the drawings

Figure 1 is a schematic diagram of the internal virtual monitoring area.

Figure 2 is a schematic diagram of the external virtual monitoring area.

Figure 3 is a schematic diagram of the dynamic adjustment of the robot speed V; among them: Figure 3 (a) is the case without any speed adjustment, Figure 3 (b) is the case of the motion planning near the monitoring area, and Figure 3 (c) is the speed reduction Then it is far away from the motion planning situation in the monitoring area.

Figure 4 is a flow chart of motion planning for monitoring the boundary of the area.

detailed description

The present invention will be further described in detail below in conjunction with specific embodiments.

The method of motion planning for the boundary of the monitoring area of an industrial robot proposed by the present invention aims to solve the problem of damage to the robot body caused by emergency braking when the robot runs to the boundary of the monitoring area at a high speed and enters the forbidden area. In conjunction with the drawings, the description is as follows:

According to the robot speed V and the maximum safe acceleration parameter a _max , formula (1) is used to calculate the advance deceleration distance △L.

△L=V ² /2a _max (1)

If the robot is operating in the area, the boundary of the monitoring area O will be shifted inward by △L, if the robot is operating outside the area, the boundary of the monitoring area O will be shifted outward by △L to obtain the virtual monitoring area O' , The shaded area between O'and O is the pre-deceleration zone.

V=K×V _plan (2)

When the robot TCP position P has not reached the virtual monitoring area O', the robot keeps the original speed multiplier K _s movement, when the robot TCP position P enters the virtual monitoring area O', the design rate multiplier adjustment function K _sub (t) is decelerated in advance . The speed override adjustment function K _sub (t) is a polynomial of time, and the boundary conditions are as follows:

The speed V changes after deceleration, and the changed ΔL is calculated according to formula (1). The monitoring area O needs to be shifted correspondingly, and the virtual monitoring area O'is refreshed in real time. According to the relative position change between the current TCP point P and the virtual monitoring area O', determine the movement trend of the robot. If the current point P is out of the virtual monitoring area, then readjust the speed override adjustment function to gradually restore the original speed override K _s , and adjust The subsequent speed override adjustment function is _K'sub (t). The whole process implements dynamic acceleration and deceleration to avoid excessively reducing the speed of the robot and improve the working efficiency of the robot as much as possible. The boundary conditions of the adjusted polynomial _K'sub (t) are as follows:

Figure 3 is a schematic diagram of the dynamic adjustment of the robot speed V, where Figure (a) shows that without any speed adjustment, when the robot runs to the boundary of the monitoring area O, it will brake urgently, the output speed will immediately become 0, and the body will bear a huge impact; Figure (b) shows the motion planning situation of the adjacent monitoring area, that is, when the robot enters the virtual monitoring area O', as shown in the paragraphs b'to b", the speed will _{be reduced according to the rate override adjustment function K sub} (t). When the monitoring area boundary O, the speed is already in a relatively low range, and it stops immediately; Figure (c) shows that the virtual monitoring area O′ is refreshed in real time after the speed is reduced, and the refreshed virtual monitoring area is O”. The robot has moved away from the virtual monitoring area O", as shown in the section from c'to c", and will _{accelerate according to the adjusted speed multiplier function K'sub} (t) until it returns to the speed before deceleration.

The specific implementation process of the present invention is shown in Figure 4:

1. Input the current TCP speed V and the parameter a _max , and calculate the virtual monitoring area parameter △L according to formula (1). If the user sets the robot to work in the area, as shown in Figure 1, offset the set monitoring area (solid line frame O) to obtain a virtual monitoring area (dotted line frame O'); if the user sets the robot to work outside the area, as shown below Fig. 2 offsets the set monitoring area (solid line frame O) to obtain a virtual safety area (dotted line frame O').

2. Determine whether the current TCP position P is between the virtual monitoring area O'and the monitoring area O (the shaded area in Fig. 1 and Fig. 2), if it is, as shown in Fig. 3(b) from b'to b”, According to the designed speed override adjustment function K _sub (t), the TCP movement speed is reduced and decelerated earlier.

3. The current TCP position P of the robot is outside the virtual monitoring area O', and it is judged whether the last motion cycle has been decelerated in advance. If the robot has not decelerated in advance, it means that the robot did not enter the virtual monitoring area in the last cycle, and the motion planning is not interfered. The current speed override.

4. The robot's current TCP position P is outside the virtual monitoring area O', and it is judged that the last motion cycle has been decelerated in advance, indicating that the robot was in the virtual monitoring area in the last cycle, that is, the current robot motion trend is far away from the boundary of the monitoring area. Or its trajectory is only close to the monitoring area but will not cross the boundary. At this time, as shown in Figure 3(c), the section c′ to c” should be gradually restored to the original _{according to the redesigned magnification adjustment function K'sub (t)} Plan the speed multiplier to avoid excessively reducing the robot's operating efficiency.

5. The current TCP position P of the robot has reached the boundary of the monitoring area O. At this time, the robot's motion speed V is already in a relatively low range after an advance deceleration process. Therefore, emergency braking can be implemented and the robot immediately stops moving.

6. The parameter △L is recalculated every cycle, and the virtual monitoring area O'is refreshed. Repeat the above steps to realize the dynamic acceleration and deceleration adjustment of the TCP speed until the movement reaches the planned position or reaches the boundary of the monitoring area.

Claims

A method for motion planning of industrial robots to monitor the boundary of an area. The steps are as follows:

Step 1. Input the current TCP speed V and the maximum safe acceleration a max to calculate the advance deceleration distance △L:

△L=V 2 /2a max ;

If the user sets the robot to work in the area, the set monitoring area O will be offset inward by the offset distance △L to obtain the virtual monitoring area O'; if the user sets the robot to work outside the area, the set offset will be set to the outside The monitoring area O of, the offset distance △L, to obtain the virtual safety area O';

Step 2. Determine the current TCP position P of the robot:

If the current TCP position point P of the robot is between the virtual monitoring area O'and the monitoring area O, reduce the TCP motion speed according to the designed speed override adjustment function K sub (t), and decelerate in advance;

If the current TCP position P of the robot is outside the virtual monitoring area O', judge whether the last motion cycle has been decelerated in advance:

a. If the last motion cycle did not decelerate in advance, indicating that the robot did not enter the virtual monitoring area in the last cycle, the motion planning will not be interfered and the current speed override will be maintained;

b. If the last motion cycle has decelerated ahead of time, it means that the robot was in the virtual monitoring area in the last cycle. According to the redesigned magnification adjustment function K'sub (t), it gradually returns to the original planned speed magnification to avoid excessive reduction of the robot. Operational efficiency;

Step 3. The current TCP position P of the robot has reached the boundary of the monitoring area O. At this time, the robot's motion speed V has undergone an early deceleration process, and the robot immediately stops moving;

Step 4. Recalculate the parameter △L every cycle and refresh the virtual monitoring area O'; repeat steps 1-3 to realize the dynamic acceleration and deceleration adjustment of the TCP speed until the movement reaches the planned position or reaches the boundary of the monitoring area.