Vision and RTLS Safety Implementation in an Experimental Human—Robot Collaboration Scenario

View full-text article in PMC

. 2021 Apr 1;21(7):2419. doi: 10.3390/s21072419

Algorithm 1 Security algorithm without human recognition

Input: Stereo stream from the depth camera and tag position from the RTLS system.

Output: Robotic system speed (%).

1:
Initialize depth camera parameters (resolution) and RTLS system (OPC UA).
2:
Set $x_{R - C}$ , $y_{R - C}$ , $z_{R - C}$ , $x_{R T L S - R}$ , $y_{R T L S - R}$ , and $z_{R T L S - R}$ as the known dimensions between the depth camera (C), robotic system (R), and beginning of the RTLS coordinate system.
3:
Save the reference depth image into $p o i n t 3 D_{r e f e r e n c e}$ .
4:
while the robotic workplace is switched on do
5:
Save actual depth image into $p o i n t 3 D_{a c t u a l}$ .
6:
Determine $p o i n t 3 D_{d i f f} = s u b s t r a c t (p o i n t 3 D_{r e f e r e n c e}, p o i n t 3 D_{a c t u a l})$ .
7:
Convert $p o i n t 3 D_{d i f f}$ to grayscale.
8:
Eliminate initial measurement errors.
9:
Scale the $p o i n t 3 D_{d i f f}$ values using Equation (4).
10:
Convert into a binary image using the function $c v 2 . t h r e s h o l d$ .
11:
Contour search with the function $c v 2 . f i n d C o n t o u r s$ .
12:
Perform distance calculation between each value in the contour and the robotic system $d i s t = \sqrt{{(v a l u e_{x} - x_{R - C})}^{2} + {(v a l u e_{y} - y_{R - C})}^{2} + {(v a l u e_{z} - z_{R - C})}^{2}}$ .
13:
Convert $m i n_{d i s t}$ coordinates to RTLS system coordinates.
14:
Find all contours where the RTLS system coordinates match with the contour coordinates and the RTLS tag ID is set to have an exception.
15:
if no other contours are found then
16:
Set $r o b o t_s p e e d = 20$ .
17:
else
18:
find $m i n_{d i s t}$ of the remaining contours.
19:
if $m i n_{d i s t}$ ≤ $d a n g e r_z o n e_s i z e$ then
20:
Set $r o b o t_s p e e d = 0$ for the danger zone.
21:
else if $w a r n i n g_z o n e_s i z e$ ≥ $m i n_{d i s t}$ > $d a n g e r_z o n e_s i z e$ then
22:
Set $r o b o t_s p e e d = 10$ for the warning zone.
23:
else
24:
Set $r o b o t_s p e e d = 20$ for the safe zone.
25:
end if
26:
end if
27:
end while