A vision-based robot collision warning method

Abstract

The present invention proposes a robot collision warning method based on vision, which relates to the field of robot control methods. According to different safety levels, the robot performs corresponding different actions to avoid direct contact between the robot and surrounding objects or people, thereby achieving the purpose of safety protection. Determine whether there is an obstacle; locate the position and height of any three consecutive equal time intervals of the obstacle; establish an obstacle time motion trajectory model and an in-situ motion model; predict the obstacle's motion trajectory position information at the next moment; set an adaptive safety threshold collision warning rule; determine whether a warning is needed and whether an alarm is needed according to the rule; repeat the above steps, and update the obstacle time trajectory model and in-situ motion model online in real time until the robot finishes working. It can be used in factories, exhibition halls, laboratories and other places where industrial robots or service robots are used, and has the characteristics of safety, efficiency and accurate identification.

Description

Technical Field

[0001] The present invention relates to the field of robot control methods. Background Art

[0002] With the rapid development of science and technology, the application of robots (industrial robots, service robots, etc.) has become more and more common. Robots basically run along the corresponding trajectory according to pre-compiled programs, but they cannot sense and detect moving objects around them. During the movement of robots, there are often people or other equipment working around the robots. If people or objects around enter the working area of ​​the robot, the robot will collide with them, causing damage to people or equipment. In order to avoid this problem, it is necessary to install a fence around the robot.

[0003] The Chinese utility model patent with the authorization announcement number "CN205651378U" and the name "A robot collision detection system" realizes collision switch detection through the principle of capacitive sensing; the Chinese invention patent application with the publication number "CN104985598A" and the name "A method for collision detection of industrial robots" calculates the difference between the theoretical torque value of the joint and the actual sampled torque value of the joint, and it is considered a collision when the torque difference exceeds a certain threshold. Both of the above patents use external sensors to determine whether the robot is in contact with an object or a person, but the disadvantage of this method is that the robot can only sense and stop moving after it touches an object or a person. If the end of the robot is a sharp workpiece and the collision object is a flexible object or a person, it will inevitably cause damage, affecting the operation of the robot and the safety of people and property. Therefore, how to prevent the robot from colliding with people or objects during the operation of the robot is particularly important for the industrial application of robots.

[0004] The Chinese invention patent with the authorization announcement number "CN109746942B" and the title "Robot, motion control system and robot anti-collision method" proposes to measure the distance between the robot and the adjacent objects through a ranging sensor, judge the degree of danger between the robot and the adjacent objects, and set multiple thresholds for safety warning. This patent requires additional configuration of ranging sensors, which increases hardware costs, and relies on human experience to set thresholds. The Chinese invention patent application with the publication number "CN113568407A" and the title "A human-machine collaborative safety warning and system based on deep vision" proposes a human-machine collaborative safety warning method and system based on deep vision, which uses the Yolov5 model to process images of the robot production environment area in real time, locate and mark pedestrians in the image, and calculate the relative distance between all pedestrians and the robot, compare it with the set safety protection level distance, and issue warning signals of different levels. This patent does not require an external ranging sensor, uses a deep learning training model to locate pedestrians, and regards the center point of the detection frame as the position of the person, that is, the person is considered to be a point mass, and does not consider the person's own range of activities. Similarly, the safety threshold is set based on human experience. Summary of the invention

[0005] In view of the above problems, the present invention proposes a vision-based robot collision warning system, which locates moving obstacles such as people or objects around the robot through a visual system, establishes a time motion trajectory model and an in-situ motion trajectory model of the obstacles respectively, updates the models online in real time, adapts to multi-level safety warning thresholds, and makes multi-dimensional real-time judgments on whether the robot is about to (or may) collide with surrounding objects or people. According to different safety levels, the machine performs corresponding different actions to avoid direct contact between the robot and objects or people, thereby achieving the purpose of safety protection.

[0006] The technical solution of the present invention is: early warning is carried out according to the following steps:

[0007] Step 1: Use multiple visual sensors to monitor the robot's surroundings and determine whether there are any obstacles;

[0008] If yes, go to the next step, otherwise keep the robot moving at normal speed;

[0009] Step 2: Locate the position and height of any three consecutive obstacles at equal time intervals;

[0010] Step 3: Establish the obstacle time motion trajectory model and the in-situ motion model;

[0011] Step 4: predict the next moment's movement trajectory position information of the obstacle;

[0012] Step 5: Setting adaptive safety threshold collision warning rules;

[0013] Step 6: Determine whether an early warning is needed and whether an alarm is needed according to the rules of step 5;

[0014] If an alarm is required, the robot will be controlled to stop moving;

[0015] If an early warning is needed, the robot is controlled to slow down;

[0016] If no early warning or alarm is required, the robot will keep moving at a normal speed;

[0017] Repeat the above steps to update the obstacle time trajectory model and in-situ motion model online in real time until the robot finishes its work.

[0018] The robot collision warning method is implemented based on a robot collision warning system, which includes a robot 1, a robot control system 2, a visual system 4, and a plurality of visual sensors 3. The robot 1 is connected to the robot control system 2 and moves under the control of the robot control system 2.

[0019] A plurality of visual sensors 3 are connected to a visual system 4, and the visual system 4 also maintains data exchange with the robot control system 2. The visual system 4 determines whether a warning is needed and whether an alarm is needed according to steps 1 to 6, and then feeds back to the robot control system 2 to control the robot 1 accordingly.

[0020] Furthermore, in step 2, the visual detection system obtains the center position of any three consecutive equal time intervals of the object or person, and the position information is recorded as p t-2 (x t-2 ,y t-2 ,z t-2 ), p t-1 (x t-1 ,y t-1 ,z t-1 ), p t (x t ,y t ,z t ), t≥2; and obtain the height h of any three consecutive equal time intervals of the object or person t-2 ,h t-1 ,h t (t≥2);

[0021] Furthermore, step 3 performs the following operations on the three consecutive position points p obtained in step 2 above: t-2 、p t-1 、p t Perform circle fitting to obtain the radius R of the motion trajectory of the object or person at the current time t t and the center point P t (X t ,Y t ,Zt );

[0022] If p t-2 、p t-1 、p t Not collinear, calculate the center point P of the time motion trajectory separately t and the obstacle position p at time t-2 and t t-2 、p t The angle of and The corresponding angle θ t-2 ,θ t (θ t >θ t-2 );

[0023] Assuming that the obstacle moves at a constant speed for three consecutive moments, the speed of the obstacle is estimated based on the above fitting circle results and the positions of the three points.

[0024] Record the obstacle time motion trajectory model, denoted as

[0025] Step 4: Based on the obstacle time motion trajectory model Estimate the obstacle's position at the next moment

[0026] If p t-2 、p t-1 、p t Collinear, obstacle time trajectory model is approximately R t Infinite circle, speed of movement The predicted position at the next moment is

[0027] Furthermore, when building the in-situ motion model in step 3, the obstacle is considered to be at the current position p t A sphere with a radius r and a center, where the active radius Record the obstacle's in-situ motion model, denoted by m t (p t ,r).

[0028] Further, step 5 sets the adaptive safety threshold collision warning rule according to the following steps;

[0029] Get the coordinates of the robot end at the current moment

[0030] Calculate the relative distance between the robot and the obstacle at the current moment Relative distance from the center of the motion trajectory

[0031]

[0032] in,

[0033]

[0034] Calculate the robot end at the current moment and the obstacle time motion trajectory center P t The angle of Corresponding angle

[0035] According to the above m t (p t ,r)

[0036] , d t and D t As a result, collision safety warning rules are set, and adaptive multi-level thresholds are used as the basis for the robot's next action;

[0037] Then follow the p t-2 、p t-1 、p t Whether they are on the same line, and whether the obstacle is moving in place or stationary, adaptive safety threshold collision warning rules.

[0038] Furthermore, if p t-2 、p t-1 、p t If they are not collinear, set the following rules:

[0039] when When , the robot operates normally without any warning or alarm;

[0040] when When d t >r, further judgment Is it in Within range: If The collision warning signal is triggered and the robot decelerates;

[0041] On the contrary, if The robot moves normally, with no warning or alarm signals;

[0042] when When d t ≤r, the collision alarm signal is triggered and the robot stops moving;

[0043] when When the collision occurs, the collision alarm signal is triggered and the robot stops moving.

[0044] Furthermore, if p t-2 、p t-1 、p tIf they are collinear, set the following rules:

[0045] When t >r, further judgment Is it in p t and On a line segment: If Then a collision may occur at the next moment, triggering a collision warning signal and causing the robot to decelerate;

[0046] On the contrary, if The robot moves normally.

[0047] When t When ≤r, the collision alarm signal is triggered and the robot stops moving.

[0048] Furthermore, if p t-2 、p t-1 、p t The obstacle can only move in place or remain stationary. At this time, the time trajectory model is R t = 0 circle, P t =p t ,R t =0, D t =d t , then the predicted position at the next moment The collision warning rule is simplified as follows: when d t >r, the robot operates normally; when d t When ≤r, the robot is within the obstacle's original motion range, a collision alarm signal is triggered, and the robot stops moving.

[0049] Advantages of the present invention:

[0050] 1. The present invention establishes a time motion trajectory model of obstacles around the robot, adaptively sets multi-level safety thresholds, and updates them online in real time, without relying on human experience, and has a more scientific basis;

[0051] Second, the present invention approximately establishes an in-situ motion model of obstacles around the robot, and the obstacles are no longer regarded as a particle, so the collision warning result is more accurate and safer;

[0052] 3. The present invention uses a visual system to locate the position of the robot and its surrounding obstacles and calculate the relative distance between the two. No external distance measuring sensor is required, thus saving hardware costs.

[0053] 4. The vision-based collision warning system proposed in the present invention enables the robot to achieve better working efficiency while ensuring safe production.

[0054] The present invention can be used in factories, exhibition halls, laboratories and other places where industrial robots or service robots are used, and has the characteristics of safety, high efficiency and accurate identification. BRIEF DESCRIPTION OF THE DRAWINGS

[0055] Figure 1 It is a schematic diagram of the visual surveillance and early warning system in this case;

[0056] Figure 2 This is the workflow diagram of this case;

[0057] Figure 3 It is a schematic diagram of the obstacle time motion trajectory model and the in-situ motion model in this case;

[0058] Figure 4 It is a schematic diagram of the adaptive multi-level safety threshold collision rule in this case;

[0059] In the figure, 1 is the robot, 2 is the robot control system, 3 is the visual sensor, and 4 is the visual system. DETAILED DESCRIPTION

[0060] In order to clearly illustrate the technical features of this patent, this patent is elaborated in detail below through a specific implementation method and in combination with its accompanying drawings.

[0061] The visual monitoring safety early warning system of the present invention is as follows Figure 1 As shown, the entire safety warning system includes a robot 1, a robot control system 2, a visual sensor 3, and a visual system 4. The visual sensor 3 monitors the action area of ​​the robot 1 in real time, collects image information of obstacles and the robot 1 in the action area of ​​the robot 1, and then transmits it to the visual system 4 by wired or wireless means to identify whether there is a moving object or a person in the robot's motion area. If so, further analysis is performed to determine whether the robot 1 and the object are about to collide, and the safety threshold is set adaptively to generate multi-level warning signals. According to different safety warning signals, the robot controller 2 sends different instructions to the robot to slow down or stop the movement, thereby avoiding direct contact between the robot and the moving object or person, until the moving object or person moves to the safe motion range of the robot, and the controller 2 will send a signal to the robot 1 to let it resume motion.

[0062] The vision-based robot collision warning system works in the following method, which includes the following steps: Figure 2 shown.

[0063] About obstacle identification and positioning;

[0064] like Figure 1As shown, at least one visual sensor is set up in the robot's motion area, and the visual system identifies whether there is an object or a person entering the robot's motion area. If not, the robot moves at a normal speed. If yes, further analysis is performed.

[0065] Specifically: the visual system first models the robot's surrounding environment. If a person or object enters the robot's action area, the visual system will detect abnormalities in the surrounding environment.

[0066] About the establishment of obstacle time motion trajectory model and position prediction;

[0067] When the visual sensor detects a moving object or person within the robot's motion range, the visual detection system obtains the position of the object or person at any three consecutive equal time intervals (the center position of the obstacle), and the three positions are not collinear. The position information is recorded as p t-2 (x t-2 ,y t-2 ,z t-2 ), p t-1 (x t-1 ,y t-1 ,z t-1 ), p t (x t ,y t ,z t ), t≥2;

[0068] According to the three consecutive position points p obtained in the above steps t-2 、p t-1 、p t Perform circle fitting to obtain the radius R of the motion trajectory of the object or person at the current time t t and the center point P t (X t ,Y t ,Z t );

[0069] Calculate the time motion trajectory center point P respectively t and the obstacle position p at time t-2 and t t-2 、p t The angle of and The corresponding angle θ t-2 ,θ t (θ t >θ t-2 );

[0070] Assuming that the obstacle moves at a constant speed for three consecutive moments, the speed of the obstacle is estimated based on the above fitting circle results and the positions of the three points.

[0071] According to the results of the above steps, the obstacle time motion trajectory model is recorded and recorded as like Figure 3 As shown;

[0072] According to the obstacle time motion trajectory model Can estimate the next position of the obstacle

[0073]

[0074] Calculate the center point P of the time motion trajectory t Predicted position of obstacle at time t+1 The angle of Corresponding angle

[0075] About the establishment of obstacle in-situ motion model;

[0076] The visual detection system obtains the height h of an object or person at any three consecutive equal time intervals t-2 ,h t-1 ,h t (t≥2);

[0077] Consider the obstacle as a t A sphere with a radius r and a center, where the active radius The calculation only needs to be initialized once;

[0078] According to the above steps, record the obstacle's in-situ motion model, denoted as m t (p t ,r), such as Figure 3 shown.

[0079] Regarding the adaptive safety threshold collision warning rules; according to the set rules, the control system will send different instructions to make the robot perform different actions, namely normal movement, deceleration movement, and stop movement. At the same time, if an alarm and warning signal is generated, it can be displayed on the teach pendant of the robot control system.

[0080] Get the coordinates of the robot end at the current moment

[0081] Calculate the relative distance between the robot and the obstacle at the current moment Relative distance from the center of the trajectory

[0082]

[0083] in,

[0084]

[0085] Calculate the robot end at the current moment and the obstacle time motion trajectory center P t The angle of Corresponding angle

[0086] According to the above m t (p t ,r)

[0087] , d t and D t Results, such as Figure 4 As shown, set the collision safety warning rules and adaptive multi-level thresholds as the basis for the robot's next action, as follows:

[0088] when When the robot is outside the obstacle's motion trajectory, the robot operates normally without any warning or alarm signals;

[0089] when When d t >r, current robot position There is no collision with the obstacle, and the obstacle is further judged from the current position p t (x t ,y t ,z t ) Move to the predicted position at the next moment Whether a collision will occur during the process, that is, judging Is it in Within range: If The obstacle is currently approaching the robot, and the predicted trajectory position at the next moment Passing the current robot position Then a collision may occur at the next moment, triggering a collision warning signal and causing the robot to decelerate;

[0090] On the contrary, if That is, the current obstacle is approaching the robot and the predicted trajectory position at the next moment Do not pass through the current robot position Or if the obstacle is currently moving away from the robot, the robot will not collide with the obstacle at the next moment, and the robot will move normally without any warning or alarm signal;

[0091] when When d t ≤r, the robot is within the obstacle's original motion range, a collision alarm signal is triggered, and the robot stops moving;

[0092] when When the robot is within the obstacle's motion trajectory, a collision alarm signal is triggered and the robot stops moving.

[0093] Repeat all the above steps, update the obstacle time trajectory model and in-situ motion model online in real time, adapt the safety threshold, and realize robot collision warning.

[0094] It is worth noting that the above method also applies to p t-2 、p t-1 、p t For obstacles that are collinear and only move or are stationary, the above time trajectory model and collision warning rules can be further simplified:

[0095] If p t-2 、p t-1 、p t Collinear, obstacle time trajectory model is approximately R t Infinite circle, speed of movement The predicted position at the next moment is The collision warning rule is simplified to:

[0096] When t >r, the current robot position There is no collision with the obstacle, and the obstacle is further judged from the current position p t (x t ,y t ,z t ) Move to the predicted position at the next moment Whether a collision will occur during the process, that is, judging Is it in p t and On a line segment: If The obstacle is currently approaching the robot, and the predicted trajectory position at the next moment Passing the current robot position Then a collision may occur at the next moment, triggering a collision warning signal and causing the robot to decelerate;

[0097] On the contrary, if That is, the current obstacle is approaching the robot and the predicted trajectory position at the next moment Do not pass through the current robot position Or if the current obstacle is moving away from the robot, the robot will not collide with the obstacle at the next moment and the robot will move normally.

[0098] When t When ≤r, the robot is within the obstacle's original motion range, a collision alarm signal is triggered, and the robot stops moving.

[0099] If p t-2 、pt-1 、p t The obstacle can only move in place or remain stationary. At this time, the time trajectory model is R t = 0 circle, P t =p t ,R t =0, D t =d t , then the predicted position at the next moment The collision warning rule is simplified to:

[0100] When t >r, the robot is outside the obstacle's original motion range and outside the obstacle's motion trajectory, then the robot operates normally;

[0101] When t When ≤r, the robot is within the obstacle's original motion range, a collision alarm signal is triggered, and the robot stops moving.

[0102] Repeat all the above steps, update the obstacle time trajectory model and in-situ motion model online in real time, adapt the safety threshold, and realize robot collision warning.

[0103] There are many specific implementation ways of the present invention. The above is only the preferred implementation mode of the present invention. It should be pointed out that for ordinary technicians in this technical field, several improvements can be made without departing from the principle of the present invention. These improvements should also be regarded as the protection scope of the present invention.

Claims

1. A vision-based robot collision warning method, characterized in that: Follow the steps below to issue an alert: Step 1: Use multiple visual sensors to monitor the robot's surroundings and determine whether there are any obstacles; If yes, go to the next step, otherwise keep the robot moving at normal speed; Step 2: Locate the position and height of any three consecutive obstacles at equal time intervals; In step 2, the visual detection system obtains the center position of an object or a person at any three consecutive equal time intervals, and the position information is recorded as p t-2 (x t-2 ,y t-2 ,z t-2 ), p t-1 (x t-1 ,y t-1 ,z t-1 ), p t (x t ,y t ,z t ), t≥2; and obtain the height h of any three consecutive equal time intervals of the object or person t-2 ,h t-1 ,h t (t≥2); Step 3: Establish the obstacle time motion trajectory model and the in-situ motion model; Step 3: For the three consecutive position points p obtained in step 2 above t-2 、p t-1 、p t Perform circle fitting to obtain the radius R of the motion trajectory of the object or person at the current time t t and the center point P t (X t ,Y t ,Z t ); If p t-2 、p t-1 、p t Not collinear, calculate the center point P of the time motion trajectory separately t and the obstacle position p at time t-2 and t t-2 、p t The angle of and The corresponding angle θ t-2 ,θ t (θ t >θ t-2 ); Assuming that the obstacle moves at a constant speed for three consecutive moments, the speed of the obstacle is estimated based on the above fitting circle results and the positions of the three points. The obstacle time motion trajectory model is recorded as Step 3: When establishing the in-situ motion model, the obstacle is considered to be at the current position p t A sphere with a radius r and a center, where the active radius Record the obstacle's in-situ motion model, denoted by m t (p t ,r); Step 4: predict the next moment's movement trajectory position information of the obstacle; Step 4: Based on the obstacle time motion trajectory model Estimate the obstacle's position at the next moment If p t-2 、p t-1 、p t Collinear, obstacle time trajectory model is approximately R t Infinite circle, speed of movement The predicted position at the next moment is Step 5: Setting adaptive safety threshold collision warning rules; Step 6: Determine whether an early warning is needed and whether an alarm is needed according to the rules of step 5; If an alarm is required, the robot will be controlled to stop moving; If an early warning is needed, the robot is controlled to slow down; If no early warning or alarm is required, the robot will keep moving at a normal speed; Repeat the above steps to update the obstacle time trajectory model and in-situ motion model online in real time until the robot finishes its work.

2. A vision-based robot collision warning method according to claim 1, characterized in that: The robot collision warning method is implemented based on a robot collision warning system, wherein the robot collision warning system comprises a robot (1), a robot control system (2), a visual system (4) and a plurality of visual sensors (3), wherein the robot (1) is connected to the robot control system (2) and moves under the control of the robot control system (2); A plurality of visual sensors (3) are connected to a visual system (4), and the visual system (4) also maintains data exchange with the robot control system (2). The visual system (4) determines whether a warning is required and whether an alarm is required according to steps 1 to 6, and then feeds back to the robot control system (2) to control the robot (1).

3. The vision-based robot collision warning method according to claim 1, characterized in that: Step 5: Set the adaptive safety threshold collision warning rule according to the following steps; Get the coordinates of the robot end at the current moment Calculate the relative distance between the robot and the obstacle at the current moment Relative distance from the center of the motion trajectory in, Calculate the robot end at the current moment and the obstacle time motion trajectory center P t The angle of Corresponding angle According to the above m t (p t ,r), d t and D t As a result, collision safety warning rules are set, and adaptive multi-level thresholds are used as the basis for the robot's next action; Then follow the p t-2 、p t-1 、p t Whether they are on the same line, and whether the obstacle is moving in place or stationary, adaptive safety threshold collision warning rules.

4. A vision-based robot collision warning method according to claim 3, characterized in that: If p t-2 、p t-1 、p t If they are not collinear, set the following rules: when When , the robot operates normally without any warning or alarm signal; when When d t >r, further judgment Is it in Within range: If The collision warning signal is triggered and the robot decelerates; On the contrary, if The robot moves normally, with no warning or alarm signals; when When d t ≤r, the collision alarm signal is triggered and the robot stops moving; when When the collision occurs, the collision alarm signal is triggered and the robot stops moving.

5. The vision-based robot collision warning method according to claim 3, characterized in that: If p t-2 、p t-1 、p t If they are collinear, set the following rules: When t >r, further judgment Is it in p t and On a line segment: If Then a collision may occur at the next moment, triggering a collision warning, and the robot will decelerate; On the contrary, if Then the robot moves normally; When t When ≤r, the collision alarm signal is triggered and the robot stops moving.

6. The vision-based robot collision warning method according to claim 3, characterized in that: If p t-2 、p t-1 、p t The obstacle can only move in place or remain stationary. At this time, the time trajectory model is R t = 0 circle, P t =p t ,R t =0, D t =d t , then the predicted position at the next moment The collision warning rule is simplified as follows: when d t >r, the robot operates normally; when d t When ≤r, the robot is within the obstacle's original motion range, a collision alarm signal is triggered, and the robot stops moving.

Images