A Vision Based Algorithm for a Path Following Problem
A Vision Based Algorithm for a Path Following Problem
Image ex Path xw uT , uϕ
Processing ey yw Controller uϑ , uψ Drone
Planner
Fig. 2: Snapshot extracted from the virtual scenario. A
dashed circle is used to indicated the drone position. Sec. III-A Sec. III-B Competition Organizers
IMG
4 3
Centered
Centered
Fig. 5: State machine implemented. State 1: Take-off. State
2: Following. State 3: End-Marker. State 4: Landing.
Fig. 6: Original frame (upper left), converted and binarized
frame (upper right), and eroded frame (lower).
in the Following state, hence the path following starts. As
soon as the IPS detects the End-Marker, the state machine
exits from the Following state and goes into the End-Marker In Figure 6 the overall process is reported for a single sample
state. At this stage the mission stops, and the drone starts the frame.
landing. In the following subsections the implementation of Then, the obtained reference path is used in a twofold
the image processing system and path planner modules are way: (i) to identify a new VTP belonging to the track; (ii)
detailed. to detect the landing marker. The two tasks are described in
the pseudocode reported in Algorithm 1.
A. Image processing system Looking at the algorithm, the first three functions (i.e.,
channelConv, binarization, and erosion) take
Starting from the camera frames, the Image Processing
care of extracting the path information from the frame. Then,
System takes care of separating the features of the pre-
the detectTrack and detectMarker functions deal
established path from that of the environment.
with raising a flag when the path (Flag VTP) or the End-
The IPS receives frames of width W and height H from the
Marker (Flag marker) are detected. The path following
camera sensor at each TIPS = 0.2 s, i.e., the camera sampling
algorithm starts with the IPS that computes the errors (ex and
time. The image format is RGB with 8 bits for each color
ey ) between the drone position and the VTP point for the PP
channel. The path is 0.01 m in width, while the landing
by using a circular arc mask centered in the drone Center of
marker is circular with a diameter of 0.02 m. The path color
Mass (CoM)2 with thickness Rmax − Rmin 3 .
is red, and this information is taken into consideration in all
In Figure 7, the arc mask considering the VTP po-
the elaborations to filter out the background scenario. The
sition at time tk is depicted, where tk denotes the k-
procedure consists of the following steps: first, the RGB
element of the time interval vector defined as t =
frame is converted into an intensity level frame represen-
(0, TIPS , . . . , NTIPS )> ∈ RN+1 , with k ∈ N0 . The orientation
tation as follows
angle ϑ = arctan2(xVTP , yVTP ) is calculated with respect to
fG (n, m) fB (n, m) the frame coordinates, where the arctan2 function is the
F(n, m) = fR (n, m) − − , (1)
GG GB four-quadrant inverse of the tangent function. A portion Θ
of the arc mask is established by taking into account the
where the pair (n, m) represents the pixel located at row
previous VTP’s orientation. In particular, we set up two
n ∈ {1, 2, . . . , H} and column m ∈ {1, 2, . . . ,W } of the image
semi-arcs with width Θ/2, namely Field of View (FoV), in
frame and fi , with i ∈ {R, G, B}, provides the intensity level
counter-clockwise and clockwise directions from ϑ . Then,
representation of the corresponding red, green and blue
the arc mask is applied to the eroded image obtaining
channels. An heuristic approach was used to tune the GG,
the VTP point at tk+1 . The function VTP calculates xVTP ,
GB ≥ 1 parameter values. These parameters help to detect the
yVTP , and ϑ which represent the frame coordinates and angle
pixels belonging to the path. Further, a binarization process
orientation of the VTP at tk+1 , respectively. Subsequently the
based on a KT threshold value refines the process removing
corresponding errors with respect to the center of mass, i.e.,
artifacts from the elaboration. The binarized frame can be
ex and ey , are computed inside the frame coordinates. Finally,
described by the binary function Fbin : (n, m) → {0, 1} whose
output is one when the pixel belongs to the path and zero 2 The assumption that the CoM being in the center of the reference frame,
otherwise. Finally, an erosion operation is performed through i.e., xCoM = H/2 and yCoM = W/2, is taken into consideration.
a square kernel to shrink and regularize the binarized frame. 3R
max and Rmin are the outer and inner radius, respectively.
Algorithm 1 Image Processing System
1: IMG ← channelConv(IMG),
2: IMG ← binarization(IMG),
3: IMG ← erosion(IMG),
4: Flag VTP ← detectTrack(IMG),
5: Flag marker ← detectMarker(IMG)
6: if Flag VTP then
7: xVTP , yVTP ← vtp(frame)
8: ex ← xVTP − xCoM
9: ey ← yVTP − yCoM Fig. 8: Frame after the application of the Arc mask (left).
10: else Extracted pixels belonging to the path (right).
11: if Flag marker then
12: xMARK , yMARK ← cgMarker(frame)
13: ex ← xMARK − xCoM
14: ey ← yMARK − yCoM
15: return ex , ey , Flag VTP, Flag marker
Y [m]
Y [m]
2:
3: xk+1 ← xk + αex 1 1
4: yk+1 ← yk + αey
0 0
5: zk+1 ← zH −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0 −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0
6: if Flag marker then X [m] X [m]
Y [m]
Y [m]
11: else 1 2.4
12: xk+1 ← xk + β ex 2.2
13: yk+1 ← yk + β ey 0
2
14: zk+1 ← zH −3.5 −3 −2.5 −2 −1.5 −1 −0.5 0 −3.7 −3.65 −3.6 −3.55 −3.5 −3.45 −3.4
X [m] X [m]
15: xw ← xk+1 , yw ← yk+1 , zw ← zk+1
(c) (d)
16: return xw , yw , zw
Fig. 10: Trajectory plots. From left to right: the desired and
TABLE I: Parameter values. the drone paths for various values of α are represented. The
Sym Value mission time Ts and a comparison between the considered α
Sampling time TPP 5 × 10−3 s values are also reported.
Sampling time TIPS 0.2 s
PP constant β 18 × 10−3 m pixel−1
Frame height H 120 pixel 0.4
Frame width W 160 pixel
Velocity [m s−1 ]