λ (ϕ(k + h
p
; c) − ϕ
ref
(k + h
p
))
.
=
λ (ϕ
0
(k + h
p
) − ϕ
ref
(k + h
p
))
|
{z }
∆ϕ
end
+λD
ϕ
c, (21)
where the 1 × 4 row vector D
ϕ
is the last
row of D
X
D
V
, i.e. the derivatives of ϕ(k + h
p
)
with respect to the form-parameters, and λ is the
Lagrange-multiplier. The second term in (21) is
a first order Taylor approximation of an otherwise
nonlinear constraint.
Minimization under fulfilment of the terminal
constraint is then obtained by differentiating with
respect to c and λ and equating the derivative with
zero.
After some algebraic manipulations the linear sys-
tem of equations with dimension 5
1
h
p
D
T
V
D
(ν)T
X
LD
(ν)
X
D
V
+ R D
(ν)T
ϕ
D
(ν)
ϕ
0
c
(ν)
λ
(ν)
=
=
−
1
h
p
D
T
V
D
(ν)T
X
LP
(ν)
0
∆ϕ
(ν)
end
, (22)
written with index ν for the ν-th cycle of the
iteration, is obtained.
With the calculated form-parameters c, the future
reference velocities V
ref
are updated.
v, ω
(ν+1)
ref
(k + 1 + i) = (23)
= v, ω
(ν)
ref
(k + 1 + i) + c
(ν)
1,3
+ c
(ν)
2,4
f(i)
Then, the updated prediction of the position errors
P
(ν+1)
0
and the matrix of derivatives D
(ν+1)
X
at the
new predicted positions are calculated.
After a specified number of iterations, the algorithm
terminates. Usually, a few cycles are sufficient to
achieve convergence.
The weight matrices L and R, the prediction
horizon h
p
, the form factor κ, the sampling time
T
s
and the number of iterations performed are the
design parameters and substantially influence the
performance of the system.
The optimization of a few form-parameters instead
of an entire reference velocity sequence reduces the
calculation time significantly, since only a 5 × 5-
system of equations has to be solved every iteration.
Moreover, the solution is more robust and the sys-
tem’s bandwidth can be adapted selectively.
Prediction and optimization using the estimated
side-slip angle α leads in some situations to unsta-
ble oscillatory behaviour of the side-slip angle during
tracking of stationary curves, while the reference po-
sitions are matched with high accuracy.
On the other hand, when omitting the side-slip angle
in the optimization, position precision is deteriorated.
The controller then attempts to match the attitude an-
gle of the robot with the reference path angle, which
makes it physically impossible to keep the reference
position at large side-accelerations, because the side
force is generated by the side-slip.
Therefore, a compromise between stability and accu-
racy is sought by reducing the estimated side-slip an-
gle α by a relaxation factor µ. A value of about 0.9
to 0.95 provides stability throughout the entire feasi-
ble 2-dimensional velocity-curvature domain for sta-
tionary curves, while keeping position precision at a
reasonable level.
5.4 Application of the control law of
the inner loop
The velocity error
ˆ
e(k + 1) can now be calculated, as
mentioned before. The velocity error integral q(k) is
known from the first step of the prediction.
Therefore, the control law of the inner loop (8), writ-
ten for time k + 1
u(k + 1) = K
"
ˆ
e(k + 1)
e(k)
−q(k)
#
(24)
is now used to compute the PWM-input signals to
the DC-motors, which are applied at the end of the
current sampling interval, i.e. at time k + 1.
This means that the algorithm can consume the en-
tire sampling interval to calculate the output without
any negative effect on the control performance. The
calculation time is thus effectively compensated for,
provided it does not exceed the duration of one sam-
pling interval.
6 RESULTS
To test the tracking algorithm, simulations are carried
out using the nonlinear model.
Changing ground conditions are modeled via low fre-
quency noise or step changes acting on the respective
parameters of the nonlinear model.
The effect of possible modeling inaccuracies is simu-
lated by simply modifying various parameters used in
controller design.
To show the ability of the system to cope with
changing ground conditions, a stationary curve (i.e.
a circle) with a moderate centripetal acceleration
MOBILE ROBOT PREDICTIVE TRAJECTORY TRACKING
117