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 2 Security algorithm with 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 the 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:
Save actual image into $i m a g e$ .
7:
Search for the coordinates of potential human $r e c t s$ in the picture using function
$h o g . d e t e c t M u l t i S c a l e$ .
8:
Assign coordinates $r e c t s$ to $L_{1}$ and $R_{2}$ .
9:
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})$ .
10:
Convert $p o i n t 3 D_{d i f f}$ to grayscale.
11:
Eliminate initial measurement errors.
12:
Scale the $p o i n t 3 D_{d i f f}$ values using Equation (4).
13:
Convert into a binary image using the function $c v 2 . t h r e s h o l d$ .
14:
Perform contour search with the function $c v 2 . f i n d C o n t o u r s$ .
15:
Assign contour coordinates to $L_{2}$ and $R_{2}$ .
16:
Verify that the contour corresponds to the identified person
$d o O v e r l a p (L_{1}, R_{1}, L_{2}, R_{2})$ .
17:
if $d o O v e r l a p = = T r u e$ then
18:
Set $h u m a n P r e s e n c e = T r u e$ .
19:
end if
20:
Calculate the distance 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}}$ , for $d i s t > 0$ .
21:
Find the $m i n_{d i s t}$ of each contour.
22:
Convert $m i n_{d i s t}$ and $m i n_{h u m a n}$ coordinates to RTLS system coordinates.
23:
Find all contours where the RTLS system coordinates match with the contour coordinates and the RTLS tag ID is set to have an exception.
24:
if no other contours are found and $h u m a n P r e s e n c e = = F a l s e$ then
25:
Set $r o b o t_s p e e d = 20$ .
else
26:
find $m i n_{d i s t}$ of the remaining contours.
27:
if $m i n_{d i s t}$ ≤ $d a n g e r_z o n e_s i z e$ then
28:
Set $r o b o t_s p e e d = 0$ for the danger zone.
29:
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
30:
Set $r o b o t_s p e e d = 10$ for the warning zone.
31:
else
32:
Set $r o b o t_s p e e d = 20$ for the safe zone.
33:
end if
34:
end if