![](bg2.png)
plied on the interval [t
0
,∆tn+t
0
], known as the reced-
ing step. This process is then repeated on the inter-
val [t
0
+ ∆tn,t
N
+ ∆tn] as the finite horizon moves by
time steps defined by the sampling time ∆tn, yield-
ing a state feedback control scheme strategy. Advan-
tages of the RHC scheme become evident in terms of
adaptation to unknown events and change of strategy
depending on new goals.
In the presented approach, we propose to solve the
collision free trajectory planning and the optimal con-
trol together in one optimization step. We extend the
standard RHC method with one control horizon into
an approach utilizing two finite time intervals T
N
and
T
M
. The first time interval T
N
should provide immedi-
ate control inputs for the formation regarding the local
environment. The difference ∆t(k + 1) = t
k+1
−t
k
be-
tween transition points is kept constant in this time
interval. The second interval T
M
takes into account
information about the global characteristics of the en-
vironment to navigate the formation to the goal. The
transition points in this part can be distributed irregu-
larly to effectively cover the environment. During the
optimization process, more points are automatically
allocated in the regions where a complicated maneu-
ver of the formation is needed. This is enabled due
to the varying values of time ∆t(k + 1) = t
k+1
−t
k
be-
tween the transition points. Both these control inter-
vals, T
N
and T
M
together form a trajectory Ω from
an actual position of the robot into a desired target
through N + M transition points.
The trajectory planning and the static as well
as dynamic obstacle avoidance problem can be then
transformed to the minimization of a single cost func-
tion J(Ω) subject to sets of constraints. During the op-
timization, both control vectors and transition points
act as variables and can be optimized to get the de-
sired solution. The proposed cost function consists of
three parts as J(Ω) = J
total time
(Ω) + αJ
obst dist
(Ω) +
βJ
deviation
(Ω).
The endeavor of the trajectory planning to reach
a desired goal as soon as possible is expressed in the
first part, which represents the total time required for
reaching the goal if using the trajectory Ω. It is a
sum of time differences ∆t(·) between all transition
points of Ω. The second part J
obst dist
(Ω) is an avoid-
ance function, which contributes to the final cost if
an obstacle is closer to the trajectory than a certain
detection radius and it approaches infinity if distance
to the closest obstacle is equal to an avoidance radius.
The part J
deviation
(Ω) is employed only if it is required
to follow a preferred path during reaching the target
(as the following of runway axes shown in Fig. 2-5).
This part represents the biggest deviation of a transi-
tion point from the desired path to follow. If the aim
of the planning is to reach the target independently
on a desired path, this part is neglected (as shown in
Fig. 1). The influence of all parts of the cost function
is adjusted by constants α and β.
The minimization of the cost function is subject
to a set of equality constraints representing a kine-
matic model of the utilized vehicles. This satisfies
that the obtained trajectory stays feasible with respect
to kinematics of nonholonomic robots. Besides, it is
subject to a set of inequality constraints that charac-
terizes bounds on the velocity and curvature of the
virtual leader. These bounds are determined by the
shape of the formation and motion constraints of each
of the follower. Finally, a stability constraint guaran-
teeing that the obtained trajectory will enter the target
has to be employed. This inequality constraint rep-
resents distance between the target and the last tran-
sition point of Ω. The constraint is satisfied if this
distance is bellow a given threshold.
Trajectory Tracking for the Followers. The pre-
sented approach relies on the well known leader-
follower method (Barfoot and Clark, 2004), where the
followers track the leader’s trajectory, which is dis-
tributed within the group. The followers are main-
tained in relative distance to the leader in curvilinear
coordinates. Employing this concept, the trajectory
computed as the result of the previous section will be
used as an input of the trajectory tracking for the fol-
lowers. We apply the classical RHC based method
with one control interval T
N
for a discrete-time trajec-
tory tracking. Such a scheme enables to respond to
events in the environment behind the actual position
of the leader and to incorrect movement of a neigh-
bour in the formation. One can find implementation
details on this approach in (Saska et al., 2011), where
such a trajectory planning has been used for a spline
path following.
3 EXPERIMENTAL RESULTS
AND PARAMETERS SETTING
Let us now discuss the influence of parameters n,
N and M and show performance of the method via
numerical experiments and simulations. In Table 1,
where the situation from Fig. 1 was solved, the quality
of results (values of the cost function) and computa-
tional time
1
are presented. As expected, the quality of
the results increases with growing parameter M, as is
also shown in Fig. 1, but the necessary computational
1
In the 2nd and higher steps of the planning loop, the
computational time is notably decreased due to possible re-
initialization using the result from the previous step.
ICINCO2012-9thInternationalConferenceonInformaticsinControl,AutomationandRobotics
404