Smooth Autonomous Patrolling for a Differential-Drive Mobile Robot in Dynamic Environments

. 2023 Aug 25;23(17):7421. doi: 10.3390/s23177421

Algorithm 1 Obstacle detection

1:
Input: $l a s e r R a n g e s$ , $p_{c}$ , $P P$ , $S P P$
2:
Output: $n e x t G o a l$
3:
for $\forall l a s e r R a n g e s$ do
4:
transform $l a s e r R a n g e s$ from the local robot frame to the global map frame
5:
end for
6:
if $g l o b a l < d_{o}$ then
7:
$n e x t G o a l X = x_{c} + g l o b a l \cdot c o s (φ_{c})$
8:
$n e x t G o a l Y = y_{c} + g l o b a l \cdot s i n (φ_{c})$
9:
end if
10:
for $\forall p_{s} \in S P P$ do
11:
for $\forall l a s e r R a n g e s$ do
12:
if ${∥ S P P - g l o b a l ∥}_{1} < d_{m i n}$ and ${∥ g l o b a l - p_{c} ∥}_{1} < d_{o}$ then
13:
$r e p l a n n i n g = 1$
14:
end if
15:
end for
16:
end for
17:
if $r e p l a n n i n g$ then
18:
for $\forall p_{i} \in P P$ do
19:
$s d$ = ${∥ p_{i + 1} - p_{i} ∥}_{1}$
20:
$s d_{1}$ = ${∥ p_{c} - p_{i} ∥}_{1}$
21:
$s d_{2}$ = ${∥ p_{i + 1} - p_{c} ∥}_{1}$
22:
if $(s d_{1} + s d_{2}) \in (s d - 0.5, s d + 0.5)$ then
23:
if $s d_{2} < d_{o}$ then
24:
$n e x t G o a l = P P [i + 2]$
25:
else
26:
$n e x t G o a l = P P [i + 1]$
27:
end if
28:
end if
29:
end for
30:
end if