66 one obtains To incorporate the delayed measurement z ik − Nwhich arrives at the i-th local filter at time instant k+1, a state estimation is created first for instant k − N using Eq.. T
Trang 1Thus, in the case of packet losses, the discrete time Kalman Filter recursion that was described
in Eq (5) (measurement update) is modified as
whereγ k ∈{0, 1} This modification implies that the value of the estimated state vector ˆx(k)
remains unchanged if the a packet drop occurs, i.e whenγ k=0
It is assumed that the system[A, C]is observable Next, the following time sequences{ τ k}
and{ β k }are definedτ1 = inf{ k : k > 1, γ k = 0} Timeτ1 denotes the first time instant when the transmission over the communication channel is interrupted (loss of connection)
On the other hand, time sequence β kis defined as β1 = inf{ k : k > τ1,γ k = 1} Time
β k denotes the k-th time instant in which the transmission over the communication channel
is restored (reestablishment of connection) Therefore, for time sequencesτ kandβ kit holds
1< τ1< β1< τ2< β2< · · · < τ k < β k < · · ·
Thus, 1 is the beginning of tranmission,τ1is the time instant at which the connection is lost for the first time, β1 is the time instant at which the connection is re-established after first interruption,τ2is the time instant at which the connection is lost for second time,β2is the time instant at which the connection is re-established after second interruption,etc The following variable is also definedβ − k =β k −1, whereβ − k is the last time instant in a period of subsequent packet losses Timeβ − k is useful for analyzing the behavior of the Kalman Filter in case of a sequence of packet losses (deterioration of the estimation error covariance matrix) It is noted that in the case of the filtering procedure over the communication network, the sequence of
covariance matrices P β k is stable if sup k>1 E || P β k || <∞ (Xia et al., 2009) Equivalently, it can be
stated that the networked system satisfies the condition of peak covariance stability (Xia et al.,
2009)
Fig 4 Distributed filtering over sensors network with communication delays and packet drops
5.2 Processing of the delayed measurements for an autonomous system
Now, the processing of the delayed measurements for the networked linear Kalman Filter proceeds as follows: it is assumed that for all local filters the packet losses and time delays
Trang 2have the same statistical properties It is also assumed that measurement z i(k − N)should
have arrived at the i-th local filter at time instant k − N Instead of this, the measurement
arrives at time instant k+1 The delayed measurement z i(k − N)must be integrated in the estimation which has been performed by each local Kalman Filter (see Fig 5 and Fig 6)
Fig 5 Distributed filtering diagram implemented with the use of local filters and a master (aggregation) filter
Fig 6 Delayed measurement over the communication channel
This means that the estimation ˆx i(k | k)and the associated state estimation error covariance
matrix P i(k | k)have to be modified The transition matrices between different time instances
of the discrete-time system of Eq (61) are defined
Using the system’s dynamic equation in transition matrix form, i.e
x(k) =A(k, k −1)x(k −1) +w(k, k −1)
one has
where
Trang 3w(k, k − N) =∑N
j=1A(k, k − j+1)w(k − j+1, k − j) (67)
which means that knowing the state estimation x(k − N)and the sequence of noises from time
instant k − N to time instant k one can calculate an estimation of the state vector at time instant
then, from Eq (66) one obtains
To incorporate the delayed measurement z i(k − N)which arrives at the i-th local filter at time instant k+1, a state estimation is created first for instant k − N using Eq (68), i.e.
ˆx i(k − N, k) =Φ1(k − N, k)xˆi(k | k) +wˆa(k − N, k | k) (69)
where ˆx i(k | k)is the state estimation of the i-th local filter at time instant k and ˆ w i(k − N, k | k)is
the noise sequence for the i-th local filter, at time instant k − N For the measurement (output)
equation one has from Eq (65)
while substituting x(k − N)from Eq (68) one gets
z i(k − N) =C i(k − N)Φ1(k − N, k)x(k) +C i(k − N)w a i(k − N, k) +v i(k − N) (71)
Next, using the current state estimate ˆx(k | k) and Eq (71) one can find the measurement
estimate ˆz i(k − N | k)for the i-th local filter, i=1,· · · , M:
ˆz i(k − N) =C i(k − N)Φ1i(k − N, k)ˆx(k | k) +C i(k − N)wˆα i(k − N, k) (72)
Defining, ˜z i(k | j) =z i(k ) − ˆz i(k | j)(innovation), ˜x i(k | j) =x(k ) − ˆx i(k | j)(state estimation error), and ˜w i(k − N, k | k) =w(k − N, k ) − wˆi(k − N, k)(noise estimation error) one obtains
˜z i(k − N | k) =C i(k − N)Φ1i(k − N, k)˜x i(k | k) +C i(k − N)w˜a i(k − N, k | k) +v i(k − N) (73)
The innovation ˜z i(k − N, k)at time instant k − N will be used to modify the estimation ˆx i(k | k)
into
ˆx i(k | k) = ˆx i(k | k) +M i ˜z i(k − N | k) (74)
Thus, one can update (smooth) the state estimate at time instant k by adding to the current state estimate ˆx i(k | k)the corrective term
where M i is a gain matrix to be defined in the sequel, and ˜z i(k − N, k) is the innovation
between the measurement z i(k − N) taken at time instant k − N and the output estimate
ˆz i(k − N)which has been calculated in Eq (72)
The main difficulty in Eq (74) is that one has to calculate first the noise estimation error
˜
w a i(k − N, k | k), which means that one has to calculate an estimate of the process noise ˆw a i(k −
N, k)
Trang 4The following theorem has been stated in (Xia et al., 2009), and is also applicable to the distributed filtering approach presented in this chapter:
Theorem 1: It is assumed that the observation error (innovation) at the i-th information
processing unit (local filter), at time instant k − n where n ∈[ 0, N], is given by
˜z(k − n) =z i(k − n ) − ˆz i(k − n) (76)
and that the covariance matrix of the white process noise w a(k − j+1, k − j)is
while the estimation error for the noise w ai(k − N, k | k)is
˜
Moreover, the covariance matrix for the error of the white estimated noise vector ˜w a i(k −
N, k | k)is
Then, one can obtain the noise estimate ˆw a i(k − N, k)from the relation
ˆ
w a i(k − N, k | k ) = −Φ1(k − N, k)∑n N−1=0
˜
C i(n)[C i(k − n)P i(k − n | k − n −1)C i(k − n)T+R i(k − n)]−1 ˜z
i(k − n) (80)
where
˜
j =n+2 A(k, k − j+1)Q(k − j+1, k − j )×
×[∏m j−1 =n+1 A(k − m+1, k − m)[I − K i(k − m)C i(k − m)]]T } C i(k − n)T (81)
while the covariance matrix of the estimated white noise w α i(k − N, k)is calculated as
Q ∗ i(k − N, k) =Q(k − N, k ) −Φ1(k − N, k )×
×∑n N−1=0C˜i(n)[C i(k − n)P i(k − n | k − n −1)C i(k − n)T+R i(k − n)]−1 ×
× C˜i(n)TΦ1(k − N, k)T
(82) where
Q(k − N, k) =Φ1(k − N, k)[∑N
j=1A(k, k − j+1)×
× Q(k − j+1, k − j)A(k, k − j+1)T]Φ1(k − N, k)T (83)
Next, a theorem is given about the calculation of covariance matrix M i appearing in the modified state estimation of Eq (74) The theorem comes from (Xia et al., 2009) and is also applicable to the distributed filtering approach which is presented in this chapter
Theorem 2: It is assumed that the modified state estimation error at time instant k is
˜x i(k | k) =x(k ) − ˆx i(k | k) (84) and that the covariance matrix of the modified state estimation error is
Trang 5P i ∗(k | k) =E { ˜x i(k | k)˜x i(k | k)T } (85)
and that the cross-covariance between ˜x i(k | k)and ˜w i(k − N, k | k)is
Then, the optimal filter for the processing of the delayed measurements is given by Eq (74), i.e
ˆx i(k | k) = ˆx i(k | k) +M i[z i(k − N ) − ˆz i(k − N | k)] (87) where
In that case, the covariance matrix of the modified state estimation error becomes
P i ∗(k | k) =P i(k | k ) − [ P i ˜x ˜ w+P i(k | k)Φ1(k − N, k)T ]×
× C i(k − N)T W i −1 C i(k − N )×
×[ P i ˜x ˜ w+P i(k | k)Φ1(k − N, k)T]T
(89)
where matrices W i and P ˜x ˜ w
i are defined as
W i=C i(k − N ){Φ1(k − N, k)P i(k | k)Φ1(k − N, k)T+ +Φ1(k − N, k)P ˜x ˜ w
i + [A(k − N, k)P ˜x ˜ w
i ]T+Q ∗ i(k − N, k )}
× C i(k − N) +R i(k − N) (90)
P i ˜x ˜ w=Φ1(k − N, k)∑N−1 n=0P i(k − N | k − N)D i(n)T ×
×[ C i(k − n)P i(k − n | k − n −1)C i(k − n)T+R i(k − n)]−1 ×
× C˜i(n)TΦ1(k − N, k)T − A(k, k − N)Q ∗ i(k − N, k)
(91)
and matrix D T i(n)is defined as
D i(n) =
⎧
⎪
⎪
C i(k − n)A(k − n, k − n −1), if N=1
C i(k − n)A(k − n, k − n −1)∏N−2 j =n [I − K i(k − j −1)C i(k − j −1)]×
× A(k − j − 1, k − j −2), if N >1
(92)
5.3 Processing of the delayed measurements for a linear non-autonomous system
5.3.1 The case of a time-variant linear system
In the case of a linear non-autonomous system, in place of Eq (61) one has
Setting w1(k, k −1) =B(k, k −1)u(k −1) +w(k, k −1)one obtains
and consequently it holds
x(k) =∏N
j=1A(k − j+1, k − j)x(k − N)+
+∑N−1 m=1∏m
j=1A(k − j+1, k − j)w1(k − m, k − m −1) +w1(k, k −1) (95)
Trang 6Thus, one can obtain a more compact form
with
Φ(k, k − N) =∏N
j=1A(k − j+1, k − j), and
w1(k, k − N) =∑m N−1=1∏m
j=1A(k − j+1, k − j)w1(k − m, k − m −1) +w1(k, k −1) (98)
5.3.2 The case of a time-invariant linear system
For a linear time-invariant non-autonomous system
it holds
x(k) =A N x(k − N) +∑N
j=1A
N−j Bu(k − N+j −1) +∑N
j=1A
N−j w(k − N+j −1) (100)
Denoting A N=Φ(k, k − N)one has
x(k) =Φ(k, k − N)x(k − N) +∑N
j=1A
N−j Bu(k − N+j −1) +∑N
j=1A
N−j w(k − N+j −1) (101)
Setting
w1(k, k − N) = ∑N
j=1A
N−j Bu(k − N+j −1) +∑N
j=1A
N−j w(k − N+j −1) (102) one has that Eq (101) can be written in a more compact form as
x(k) =Φ(k, k − N)x(k − N) +w1(k, k − N) (103) Using that matrixΦ(k, k − N)is invertible, one has
x(k − N) =Φ(k, k − N)−1 x(k ) −Φ(k, k − N)−1 w1(k, k − N) (104) The following notation is usedΦ1(k − N, k) = Φ(k, k − N)−1while for the retrodiction of
w1(k, k − N)it holds w a(k − N, k | k ) = −Φ(k, k − N)−1 w
1(k, k − N) Then, to smooth the state
estimation at time instant k − N, using the measurement of output z i(k − N)received at time
instant k+1 one has the state equation
ˆx(k − N, k) =Φ1(k − N, k)xˆ(k | k) +wˆa(k − N, k | k) (105) while the associated measurement equation becomes
Trang 7z(k − N) =Cx(k − N) +v(k − N) (106) Substituting Eq (104) into Eq (106) provides
z(k − N) =CΦ1(k − N, k)x(k) +Cw a(k − N, k) +v(k − N) (107)
and the associated estimated-output at time instant k − N is
ˆz(k − N) =CΦ1(k − N, k)xˆ(k | k) +C ˆ w a(k − N, k) (108) From Eq (108) and Eq (107) the innovation for the delayed measurement can be obtained
˜z(k − N) =z(k − N ) − ˆz(k − N) (109)
i.e ˜z(k − N) = CΦ1(k − N)˜x(k | k) +C ˜ w a(k − N), where ˜x(k | j) = x(k ) − ˆx(k | j)is the state estimation error and ˆw a(k − N, k | k) =w a(k − N, k ) − wˆa(k − N, k)is the estimation error for
w α With this innovation the estimation of the state vector x(k | k)at time instant k is corrected.
The correction (smoothing) relation is
ˆx (k | k) =x(k | k) +M ˜z(k − N, k) (110) Therefore, again the basic problem for the implementation of the smoothing relation provided
by Eq (110) is the calculation of the term w a(k − N, k)i.e w a(k − N) =Φ(k, k − N)−1 w
1(k, k −
N) This in turn requires the estimation of the term w1(k − N, k)which, according to Eq (80),
is provided by
ˆ
w1(k − N, k ) = −Φ1(k − N, k)∑n N−1=0·
· C˜(n)[C(k − n)P(k − n | k − n −1)C(k − n)]T+R(k − n)]−1 ˜z(k − n) (111)
where ˜z(k − n) =z(k − n ) − ˆz(k − n)is the innovation for time-instant k − n, while, as given
in Eq (81)
˜
j =n+2 A(k, k − j+1)Q(k − j+1, k − j )×
×[∏m j−1 =n+1 A(k − m+1, k − m)[I − K i(k − m)C(k − m)]T ]} C(k − n)T (112)
5.4 Derivative-free Extended Information Filtering under time-delays and packet drops
It has been shown that using a suitable transform (diffeomorphism), the nonlinear system of
Eq (15) can be transformed into the system of Eq (17) Moreover, it has been shown that for the systems of Eq (23) and Eq (24) one can obtain a a state-space equation of the form
⎛
⎜
⎜
⎝
˙ζ1
˙ζ2
· · ·
˙ζ n−1
˙ζ n
⎞
⎟
⎟
⎠=
⎛
⎜
⎜
⎜
0 1 0· · ·0
0 0 1· · ·0
. . . .
0 0 0· · ·1
0 0 0· · ·0
⎞
⎟
⎟
⎟
⎛
⎜
⎜
⎝
ζ1
ζ2
· · ·
ζ n
⎞
⎟
⎟
⎠+
⎛
⎜
⎜
⎝
0 0
· · ·
0 1
⎞
⎟
⎟
z= 1 0 0· · ·0
where v(t) = f(x, t) +g(x, t)u(t), with u(t)being the control input of the dynamical system The description of the initial system of Eq (17) in the form of Eq (113) and Eq (114) enables
Trang 8the application of the previous analysis for the compensation of time-delays and packet-drops through smoothing in the computation of the linear Kalman Filter The fact that the system of
Eq (113) and Eq (114) is a time invariant one, facilitates the computation of the smoothing Kalman Filter given in Eq (100) to Eq (112) Thus, one has to use the time invariant matrices
A c and C c defined in Eq (18) and Eq (19), while for matrix B cit holds according to Eq (25)
that B c = [0, 0,· · ·, 0, 1]T The discrete-time equivalents of matrices A c , B c and C care noted
as A d , B d and C d , respectively It is also noted that due to the specific form of matrix B c, the
term Bu(k −1)appearing in Eq (99) is a variable of small magnitude with mean value close
to zero Thus the term w1(k, k −1) =Bu(k −1) +w(k, k −1)differs little from w(k, k −1)
It also becomes apparent that through the description of the initial system of Eq (17) in the form of Eq (113) and Eq (114), the application of the derivative-free Extended Information Filter can be performed in a manner that enables the compensation of time-delays and packet drops Writing the controlled system in the form of Eq (113) and Eq (114) permits to develop local linear Kalman Filters that smooth the effects of delayed sensor measurement or the loss
of measurement packets Moreover, the application of the standard Information Filter for fusing the estimates provided by the local Kalman Filters, permits to avoid the approximation errors met in the Extended Information Filter algorithm
6 Distributed filtering under time-delays and packet drops for sensorless control
6.1 Visual servoing over a network of synchronized cameras
Visual servoing over a network of synchronized cameras is an example where the efficiency of the proposed distributed filtering approach under time delays and packet drops can be seen Applications of vision-based robotic systems are rapidly expanding due to the increase in computer processing power and low prices of cameras, image grabbers, CPUs and computer memory In order to satisfy strict accuracy constraints imposed by demanding manufacturing specifications, visual servoing systems must be fault tolerant This means that despite failures
in its components or the presence of disturbances, the system must continue to provide valid control outputs which will allow the robot to complete its assigned tasks (DeSouza & Kak, 2004),(Feng & Zeng, 2010),(Hwang & Shih, 2002),(Malis et al., 2000)
The example to be presented describes the control of a planar robot with the use of a position-based visual servo that comprises multiple fixed cameras The chapter’s approach relies on neither position nor velocity sensors, and directly sets the motor control current using only visual feedback Direct visual servoing is implemented using a distributed filtering scheme which permits to fuse the estimates of the robot’s state vector computed by local filters, each one associated to a camera in the cameras network (see Fig 7) The cameras’ network can
be based on multiple RS-170 cameras connected to a computer with a frame grabber to form
a vision node Each vision node consists of the camera, the frame grabber and the filter which estimates motion characteristics of the monitored robot joint The vision nodes are connected
in a network to form a distributed vision system controlled by a master computer The master computer is in turn connected to a planar 1-DOF robot joint and uses the vision feedback to perform direct visual servoing (see Fig 7)
The master computer communicates video synchronization information over the network to each vision node Typical sources of measurement noise include charge-coupled device (CCD) noise, analog-to-digital (A/D) noise and finite word-length effects Under ideal conditions, the effective noise variance from these sources should remain relatively constant Occlusions can be also considered as a noise source Finally, communication delays and packet drops
in the transmission of measurements from the vision sensors to the information processing
Trang 9Fig 7 Distributed cameras network and distributed information processing units for visual servoing
nodes induce additional disturbances which should be compensated by the virtual servoing control loop
6.2 Distributed filtering-based fusion of the robot’s state estimates
Fusion of the local state estimates which are provided by filters running on the vision nodes can improve the accuracy and robustness of the performed state estimation, thus also improving the performance of the robot’s control loop (Sun et al., 2011),(Sun & Deng, 2005) Under the assumption of Gaussian noise, a possible approach for fusing the state estimates from the distributed local filters is the derivative-free Extended Information Filter (DEIF) As explained in Section 4, the derivative-free Extended Information Filter provides an aggregate state estimate by weighting the state vectors produced by local Kalman Filters with the inverse
of the associated estimation error covariance matrices
Visual servoing over the previously described cameras network is considered for the nonlinear dynamic model of a single-link robotic manipulator The robot can be programmed to execute
a manufacturing task, such as disassembly or welding (Tzafestas et al., 1997) The position of the robot’s end effector in the cartesian space (and consequently the angle for the robotic link)
is measured by the aforementioned m distributed cameras The proposed multi-camera based
robotic control loop can be also useful in other vision-based industrial robotic applications where the vision is occluded or heavily disturbed by noise sources, e.g cutting In such applications there is need to fuse measurements from multiple cameras so as to obtain redundancy in the visual information and permit the robot to complete safely and within the specified accuracy constraints its assigned tasks (Moon et al, 2006),(Yoshimoto et al., 2010) The considered 1-DOF robotic model consists of a rigid link which is rotated by a DC motor,
as shown in Fig 8 The model of the DC motor is described by the set of equations: L ˙I =
Trang 10Fig 8 Visual servoing based on fusion of state estimates provided by local derivative-free nonlinear Kalman Filters
taken as control input, J : motor inertia, ω : rotor rotation speed, k d : mechanical dumping constant, Γd : disturbance or external load torque It is assumed thatΓd = mgl · sin(θ), i.e
that the DC motor rotates a rigid robotic link of length l with a mass m attached to its end.
Then, denoting the state vector as[x1, x2, x3]T = [θ, ˙θ, ¨θ]T, a nonlinear model of the DC motor
is obtained
˙x=f(x, t) +g(x, t)u (115)
where f(x, t) = [f1(x, t), f2(x, t), f3(x, t)]T is a vector field function with elements: f1(x, t) =
x2, f2(x, t) =x3, f3(x, t ) = − k2
e +k d R
JL x2− RJ +K d L
JL x3− Rmgl
JL sin(x1) − mgl
J cos(x1)x2 Similarly,
for function g(x, t)it holds that g(x, t) = [g1(x, t), g2(x, t), g3(x, t)]T, i.e it is a vector field
function with elements: g1(x, t) = 0, g2(x, t) = 0, g3(x, t) = k e
JL Having chosen the joint’s angle to be the system’s output, the state space equation of the 1-DOF robot manipulator can
be rewritten as
where functions ¯f(x)and ¯g(x)are given by ¯f(x ) = − k2
e +k d R
JL x2− RJ +K d L
JL x3− Rmgl
JL sin(x1) −
mgl
J cos(x1)x2, and ¯g(x) = k e
JL This is a system in the form of Eq (23), therefore a state estimator can be designed according to the previous results on derivative-free Kalman Filtering
The controller has to make the system’s output (angleθ of the motor) follow a given reference
signal x d For measurable state vector x and uncertain functions f(x, t) and g(x, t) an appropriate control law for the 1-DOF robotic model is
g (x,t)[x d (n) − f(x, t ) − K T e+u c] (117)