Download Thesis

(
y k +1 − y k
y − yk
) x − y + y k − ( k +1
) xk = 0
x k +1 − x k
x k +1 − x k
(2.4)
Hence the relationship between current position on the actual path and the desired
position on the desired path can be established. As shown in the figure above, if ε ⊥ is the
error in the perpendicular distance from actual position to the desired position
and (x p , y p ) is the current position of the vehicle, the error in distance can be calculated as
ε⊥ =
ak x p + bk y p + ck
2
ak + bk
(2.5)
2
Where
a
k
=
y
x
k + 1
k + 1
− y
− x
k
(2.6)
k
bk = −1
ck = y k −
(2.7)
y k +1 − y k
× xk
x k +1 − x k
(2.8)
If the orientation angle at the current position of the vehicle to the x axis is θ p and
orientation angle at the desired position is θ k , the error in orientation can be calculated as,
εθ = θ p − θk
(2.9)
A PID control algorithm often takes integral and derivation terms of error into the
calculation of the output as showing below.
m = k pε + ki ∫ εdt + k d
dε
dt
∆m = k p (ε − ε −1 ) + kiε + k d (ε − ε −1 + ε − 2 )
m = m−1 + ∆m
(3.0)
A PID controller for the path following system should be a total of two PID control
algorithm in terms of both error in distance and error in orientation.
65