Target Localization and Circumnavigation Using Bearing ...
Target Localization and Circumnavigation Using Bearing Measurements in 2D Mohammad Deghat, Iman Shames, Brian D. O. Anderson and Changbin Yu Abstract— This paper considers the problem of localization and circumnavigation of a slowly drifting target with unknown speed using the agent’s known position and the bearing angle of the target. We first assume that the target is stationary and propose an estimator to localize the target and a control law that forces the agent to move on a circular trajectory around the target such that both the estimator and the control system are exponentially stable. Then the case where the target might experience slow but possibly steady movement is studied.
I. INTRODUCTION A frequently desirable way to accomplish a surveillance mission is to monitor a target by circling around it at a prescribed distance. If the position of the target is known, then the goal is to find a control law that makes one or more agents move to and then on a circular trajectory with prescribed radius around the target. This problem has recently been studied in literature, for example, see [1]–[4]. If the position of the target is initially unknown, then we need an estimator to localize the target as well as a controller to force the agent(s) to circumnavigate the target. Several localization algorithms have studied different cases where a single agent or a group of collaborative agents localizes a target under a variety of assumptions and scenarios and using different information about the relative position of agent(s) and target such as distance and bearing. For instance, in [5], [6] a single agent accomplishes the localization task and in [7], [8] collaborative localization algorithms are studied. This kind of problem in which the aim is to control a system whose characteristics are initially unknown and therefore the identification and control problem should be solved simultaneously is often called a dual control problem [9]. The dual problem of estimation and circumnavigation of an agent trying to move around a target with unknown position using distance measurements alone has previously been studied [10], [11]. But sometimes, it is preferable to use bearing measurements instead of distance measurements since distance measurement techniques are usually active methods in which the agent must transmit signals. In contrast, bearing measurement is a passive measurement technique and may often be preferred for this reason. Mohammad Deghat, Iman Shames and Brian D. O. Anderson are with Research School of Information Sciences and Engineering, The Australian National University, Canberra ACT 0200 and National ICT Australia (NICTA). Changbin Yu is with Research School of Information Sciences and Engineering, The Australian National University.
{Mohammad.Deghat, Iman.Shames, Brian.Anderson, Brad.Yu}@anu.edu.au
In this paper, we propose an estimator that uses only the agent position and the bearing angle of target to solve localization and circumnavigation problem. It is also desirable if not essential to apply controllers and estimators that do not use explicit derivatives of measurements, because differentiation amplifies high-frequency noise. An advantage of the proposed algorithm is that it avoids such differentiation. The rest of this paper is structured as follows. In section II the localization and circumnavigation problem is formally stated and the proposed solution is provided in section III. Section IV contains simulation results and finally, conclusions and future directions are presented in section V. II. PROBLEM STATEMENT Suppose there is a target with unknown position x(t) ∈ R2 at time t and an agent with known trajectory y(s) ∈ R2 for s ≤ t with knowledge of the bearing angle θ(s) for s ≤ t. The case s = t is depicted in Figure 1. Knowing also the desired distance d, our goal is to find an estimator that estimates the unknown position x(t) using measurements up to time t and a control law that makes the agent move on a circle with radius d centered at the point x(t) such that for a stationary target, the estimation error x ˜(t) = x ˆ(t) − x(t)
(1)
in which x ˆ(t) is the estimate of x, exponentially goes to zero and D(t) = ky(t)−x(t)k exponentially goes to d while for a slowly drifting target, x ˜(t) goes to a neighborhood of zero and D(t) gets close to d.
Fig. 1.
The relationship between x, y, θ, ϕ and ϕ ¯
Throughout this paper, k.k denotes the Euclidean norm.
III. PROPOSED ALGORITHM Since this problem is a combined estimation and control problem, we should simultaneously estimate x(t) and find a control law that forces the agent to move on a desired circular trajectory around the target. In this section, we first propose an algorithm for the case that the target is stationary and study the stability of the proposed algorithm. Then, we extend our results to the case that the target moves slowly. A. Stationary target Assume the target is stationary (x˙ = 0). Our first goal is to devise an estimator which does not require the derivative of the measured data and guarantees that x ˜(t) goes to zero exponentially fast. Let ϕ(t) be a unit vector on the line passing through x and y(t) which can be written as x − y(t) x − y(t) = kx − y(t)k D(t) · ¸ cos θ(t) ϕ(t) = sin θ(t)
ϕ(t) =
(2) (3)
where θ(t) is the angle of the unit vector ϕ(t) as shown in Figure 1. It should be noted that the measurement of the bearing angle to the target when D(t) = 0 is not well defined. Moreover, ϕ(t) is not defined for this case as well. Hence, it is desirable that D(t) 6= 0 for all t > 0. A sufficient condition for D(t) to be greater than zero for all t > 0 is given in Lemma 1. Assume k is a constant positive scalar; then the estimator can be defined as ¡ ¢ x ˆ˙ (t) = k I − ϕ(t)ϕT (t) (y(t) − x ˆ(t)). (4) where I is the identity matrix and ϕ(t)ϕT (t) is a projection matrix onto the vector ϕ(t). Considering (1) and (4), the estimation error dynamics can be written as ¡ ¢ x ˜˙ (t) = k I − ϕ(t)ϕT (t) (y(t) − x ˆ(t)) ¡ ¢ T = k I − ϕ(t)ϕ (t) (y(t) − x − x ˆ(t) + x) ¢ (5) (2) ¡ T ˜(t)) = k I − ϕ(t)ϕ (t) (−ϕ(t)D(t) − x ¡ ¢ ϕT ϕ=1 − k I − ϕ(t)ϕT (t) x ˜(t) = Introducing a constant positive scalar α, the control law can be defined as ¡ ¢ ˆ − d ϕ(t) + αϕ(t) y(t) ˙ = D(t) ¯ (6) ˆ where D(t) = ky(t)− x ˆ(t)k and ϕ(t) ¯ ∈ R2 is the unit vector perpendicular to ϕ(t), obtained by π/2 clockwise rotation of ˆ = d, ϕ(t) as shown in Figure 1. It can be seen that if D(t) then the agent does not move toward or away from the target but it just moves on the circle around the target. Lemma 1: Under the estimator (4) and the control law (6), D(t) > 0 ∀t > 0 for D(0) 6= 0 and k˜ x(0)k = kˆ x(0) − xk ≤ d. See Appendix I for the proof.
Now, we should prove that by using the estimator (4) and the control law (6), the estimation error x ˜(t) in (1) exponentially goes to zero and D(t) exponentially goes to d. To this end, we recall the following theorem [12] which will be used to prove the exponential stability of x ˜(t). Proposition 1: Let V (.) : R+ → Rn×r be regulated. Then x˙ = −V V T x
(7)
is exponentially asymptotically stable iff for some positive α1 , α2 , T and for all t0 ∈ R+ Z t0 +T α1 I ≤ V (t)V T (t)dt ≤ α2 I (8) t0
The condition (8) is called persistence of excitation condition and V (.) is said to be persistently exciting (p.e.) if it satisfies (8). Another interpretation of the p.e. condition (8) in scalar form is [13] Z t0 +T ¡ T ¢2 ²1 ≤ U V (t) dt ≤ ²2 ∀t0 ∈ R+ (9) t0
where ²1 and ²2 are positive scalars and U ∈ Rn can be any constant unit vector. Therefore, x˙ = −V V T x is exponentially asymptotically stable if and only if for some positive scalars ²1 , ²2 , T and for all t0 ∈ R+ and all constant unit vectors U , the condition (9) holds. The first step towards the proof of estimator convergence is to write (5) in a form similar to (7) so that we can use Proposition 1. Lemma 2: Considering (3), the estimation error equation (5) can be written as x ˜˙ (t) = −k ϕ(t) ¯ ϕ¯T (t)˜ x(t)
(10)
Proof : The matrix I − ϕ(t)ϕT (t) can be written as · ¸ · ¸ ¤ 1 0 cos θ £ T cos θ sin θ I − ϕ(t)ϕ (t) = − 0 1 sin θ · ¸ sin2 θ − sin θ cos θ = − sin θ cos θ cos2 θ · ¸ (11) ¤ − sin θ £ − sin θ cos θ = cos θ · ¸ ¤ sin θ £ sin θ − cos θ = − cos θ £ ¤T It can be seen that the vector sin θ − cos θ is the unit vector that we previously defined as ϕ(t) ¯ because it can be obtained by π/2 clockwise rotation of ϕ(t) and therefore I − ϕ(t)ϕT (t) = ϕ(t) ¯ ϕ¯T (t). ¥
Lemma 3: Using the control law (6), the signal ϕ(t) ¯ in (10) is persistently exciting for all t ≥ 0 and x ˜(t) exponentially goes to zero. Proof : Considering (10) and Proposition 1, the signal x ˜(t) exponentially goes to zero if and only if ϕ(t) ¯ is persistently exciting and the condition on ϕ(t) ¯ to be p.e. is that there exist some ²1 , ²2 , and T , such that Z t0 +T ¡ T ¢2 ²1 ≤ U ϕ(t) ¯ dt ≤ ²2 (12) t0
is satisfied for all constant unit length U ∈ R2 and all t0 ∈ R+ . Assume γu (t) is the angle from the unit vector U to the vector ϕ(t) ¯ (as shown in Figure 2 for a sample U ). We assume that if the direction of the angles θ(t), ξ(t) or γu (t) are counter-clockwise then they are positive; otherwise, they are negative. For example, in the special case shown in Figure 2, the angle γu (t) is negative while θ(t) and ξ(t) are positive. Therefore (12) can be written as Z t0 +T ²1 ≤ cos2 γu (t)dt ≤ ²2 (13) t0
So, if there exists an upper bound for D(t) such that D(t) ≤ Dmax ∀t ≥ 0 (and the proof of this fact is below) then dγu (t) α ≥ Dmax and consequently dt γu (t + t0 ) ≥ γu (t0 ) +
αt Dmax
∀t ≥ 0
(18)
Therefore, the continuous function γu (t) always increases when time increases and it cannot converge to a constant value. So, we can always find some positive ²1 and T that satisfy (13).
Fig. 3.
ˆ The relationship between k˜ xk, D and D
The last thing to prove is that D(t) ≤ Dmax . Considering (11), the matrix −ϕ(t) ¯ ϕ¯T (t) is symmetric and its eigenvalues are 0 and -1. Therefore by choosing the Lyapunov function V = 12 x ˜T x ˜ and by considering (10), it can be seen that V˙ = −k˜ xT ϕ¯ϕ¯T x ˜ = −kkϕ¯T x ˜k2 is negative semi-definite and (10) is uniformly stable. Therefore x ˜(t) is bounded and k˜ x(t)k ≤ k˜ x(0)k
(19)
According to Figure 3 and using the triangle inequality, we have ˆ |D(t) − D(t)| ≤ k˜ x(t)k ≤ k˜ x(0)k (20) Fig. 2.
The relationship between U , γu , ξ and ϕ
By defining δ(t) and ∆(t) as
Since cos2 (.) ≤ 1, the integral in (13) is always bounded from above and an upper bound for (13) is ²2 = T . On the other hand, cos2 (.) ≥ 0 ∀t ≥ 0. The angle ξ(t) − γu (t) is always constant because U is a constant unit vector and consequently, dγu (t) dξ(t) = dt dt
(14)
On the other hand, the unit vector ϕ(t) ¯ is perpendicular to ϕ(t), obtained by π/2 clockwise rotation of ϕ(t) and therefore dξ(t) dθ(t) = (15) dt dt Since the speed of y(t) along ϕ(t) ¯ is α (see (6)) and the distance between x and y(t) is D(t) we have α dθ(t) = dt D(t)
(16)
α dγu (t) = . dt D(t)
(17)
and consequently
∆(t) = D(t) − d ˆ δ(t) = D(t) − D(t)
(21)
equation (20) can be written as |δ(t)| ≤ k˜ x(t)k ≤ k˜ x(0)k
(22)
Considering (6) and (2), the derivative of ∆(t) can be written as ³ ´ y˙ T (t) y(t) − x ˙ ∆(t) = D(t) ³¡ ´³ ´ ¢ ˆ − d ϕT (t) + αϕ¯T (t) y(t) − x D(t) = D(t) ³¡ ´³ ´ ¢ T ˆ D(t) − d ϕ (t) + αϕ¯T (t) − D(t)ϕ(t) = D(t) ¢ ¡ ˆ = − D(t) − d ¡ ¢ ¡ ¢ ˆ = − D(t) − d + D(t) − D(t) Therefore,
˙ ∆(t) = −∆(t) + δ(t)
(23)
and its solution is
Z
∆(t) = ∆(0)e
−t
t
+
e−(t−τ ) δ(τ )dτ
(24)
0
Since δ(t) is bounded (see (22)), ∆(t) in (24) is also bounded and therefore D(t) is bounded.¥ Having established that the estimation process proceeds satisfactorily, essentially because the control law provides the necessary persistence of excitation, it remains to demonstrate that the control law achieves the required objective. Theorem 1: Using the control law (6) and the estimator (4), D(t) exponentially converges to d. Proof : According to Lemma 3, x ˜(t) exponentially converges to zero and because of (22), the signal δ(t) exponentially goes to zero. So, in the light of (23), ∆(t) also goes to zero exponentially fast. ¥ B. Non-stationary target The analysis of the previous section was based on the assumption that the target is stationary. In this section, we assume that the target can move slowly and we want to show that under this assumption, the estimation error x ˜(t) converges to a neighborhood of zero. So, we suppose the target motion is such that the following assumption holds. Assumption 1: The target trajectory is differentiable and there exists a sufficiently small ε such that kx(t)k ˙ ω
(28)
for some constant positive scalar ω. Then the solution of ¡ ¢ x ˜˙ (t) = −k I − ϕ(t)ϕT (t) x ˜(t) (29) exponentially goes to zero. Proof : The proof is similar to the proof of Lemma 3. Equation (14) and (15) are valid for the non-stationary target case. But (16) may not be valid for the general case where the target can move freely. For instance, if the target moves such that x(t) ˙ = y(t) ˙ then dθ dt = 0. So we impose some constraints on the target speed to ensure that such a situation never occurs. If we assume that D(t) is bounded such that D(t) ≤ Dmax (see the proof below) and if α and ε in (6) and (25) satisfy α−ε>ω >0 (30) then instead of (16) we have
Consider x ˜(t) and x ˆ˙ (t) in (1) and (4). Then, the estimation error dynamics can be written as ¡ ¢ x ˜˙ (t) = −k I − ϕ(t)ϕT (t) x ˜(t) − x(t) ˙ (26) = −k ϕ(t) ¯ ϕ¯T (t)˜ x(t) − x(t) ˙ To prove that x ˜(t) in (26) goes to a neighborhood of zero, we use the following theorem [14]: Proposition 2: If the coefficient matrix A(t) is continuous for all t ∈ [0, ∞) and constants a > 0, b > 0 exist such that for every solution of the homogeneous differential equation x(t) ˙ = A(t)x(t) one has kx(t)k ≤ bkx(t0 )ke−a(t−t0 ) ,
0 ≤ t0 < t < ∞
then for each f (t) bounded and continuous on [0, ∞), every solution of the nonhomogeneous equation x(t) ˙ = A(t)x(t) + f (t),
x(t0 ) = 0
is also bounded for t ∈ [0, ∞). In particular, if kf (t)k ≤ Kf < ∞ then the solution of the perturbed system satisfies kx(t)k ≤ bkx(t0 )ke−a(t−t0 ) +
¢ bKf ¡ 1 − e−a(t−t0 ) (27) a
dθ(t) ω ≥ dt D(t)
(31)
Considering (14) and (15), equation (31) can be written as
and therefore
dγt (t) ω ≥ dt D(t)
(32)
dγu (t) ω ≥ dt Dmax
(33)
and γu (t + t0 ) ≥ γu (t0 ) +
ωt , Dmax
∀t ≥ 0
(34)
Thus, in the case that the target is non-stationary, the signal x ˜(t) in (29) is persistently exciting if D(t) is bounded. The final step is to prove that there exists an upper bound Dmax such that D(t) ≤ Dmax . Similarly to the proof of Lemma 3, it can be seen that (20) and (22) are valid for the case of a non-stationary target. Considering (21), (6) and (2), the derivative of ∆(t) can be written as ³ ´³ ´ y˙ T (t) − x˙ T (t) y(t) − x(t) ˙ ∆(t) = D(t) ¢ ¡ ˆ − d + x˙ T (t)ϕ(t) = − D(t) ¡ ¢ ¡ ¢ ˆ = − D(t) − d + D(t) − D(t) + x˙ T (t)ϕ(t)
Therefore, ˙ ∆(t) = −∆(t) + δ(t) + x˙ (t)ϕ(t) T
and its solution is ∆(t) = ∆(0)e
−t
Z +
t
(35)
³ ´ e−(t−τ ) δ(τ )+ x˙ T (τ )ϕ(τ ) dτ (36)
0
Since δ(t) and x(t) ˙ are bounded (see (22) and (25)) and ϕ(t) is a unit vector, equation (36) can be written as Z t |∆(t)| ≤ |∆(0)|e−t + (k˜ x(0)k + ε) e−(t−τ ) dτ (37) 0
It can be seen that ∆(t) in (37) is bounded and therefore D(t) ≤ Dmax . ¥ Lemma 5: Adopt the hypothesis of Lemma 4. Then x ˜(t) in
¡ ¢ x ˜˙ (t) = −k I − ϕ(t)ϕT (t) x ˜(t) − x(t) ˙
(38)
goes to a neighborhood of zero when t → ∞. Proof : The proof is the direct consequence of Lemma 4 and Proposition 2.¥ Theorem 2: Adopt the hypothesis of Lemma 4 and suppose that the control law (6) is used. Then D(t) goes to a neighborhood of d when t → ∞. Proof : From (36), (22) and (25) we have Z t ³ ´ |∆(t)| ≤ |∆(0)|e−t + e−(t−τ ) k˜ x(t)k + ε dτ (39) 0
According to Lemma 4, x ˜(t) goes to a neighborhood of zero and therefore, |∆(t)| = |D(t) − d| goes to a neighborhood of zero.¥ IV. SIMULATIONS In this section, simulation results for both cases are presented. For the stationary target, we assumed that d = 2, x = [2, 3]T , y(0) = [9, 8]T and the constant k in (4) is 5. As can be seen in Figure 4, the estimation error goes to zero exponentially fast and D(t) exponentially goes to d. Then, we consider the case that the target moves slowly and x(t) = [2 + .025t, 3 + sin(.025t) + .025t]T , y(0) = [9, 8]T , d = 2 and k = 5. According to Lemma 5, we expect that x ˜(t) goes to a neighborhood of x(t). It can be seen in Figure 5 that x ˆ(t) tracks x(t) with a small steady state error which is proportional to the target speed and D(t) gets close to d. V. C ONCLUDING R EMARKS ND F UTURE W ORKS In this paper, we proposed an estimator and a controller for a circumnavigation problem. The estimator estimates an unknown target using only the agent’s position and the bearing angle of the target without any explicit differentiation of the measured data. Stability of both estimator and controller has been studied in the cases where the target is stationary and it moves slowly. Future directions of research include solving the problem in 3-D space, testing the proposed controller and estimator experimentally using a mobile robot, extending the problem to the case where more than one agent is present and the case that the agent is required to move with constant speed.
VI. ACKNOWLEDGEMENTS This work is supported by NICTA, which is funded by the Australian Government as represented by the Department of Broadband, Communications and the Digital Economy and the Australian Research Council through the ICT Centre of Excellence program. Changbin Yu is supported by the Australian Research Council through an Australian Postdoctoral Fellowship under DP-0877562. VII. A PPENDIX I Proof of Lemma 1: Considering (21) and (24) we have Z t ³ ´ ˆ ) dτ D(t) = d+(D(0) − d) e−t + e−(t−τ ) D(τ ) − D(τ 0
(40)
and from (20) we conclude that ˆ − k˜ x(0)k ≤ D(t) − D(t) ≤ k˜ x(0)k Therefore, (40) can be written as
Z
t
e−(t−τ ) dτ − k˜ x(0)k (42) 0 ¢ ¡ −t −t = D(0)e + (d − k˜ x(0)k) 1 − e
D(t) ≥ d + (D(0) − d) e
−t
(41)
Since D(0)e−t is positive, D(t) > 0 ∀t > 0 if the last term of (42) is non-negative i.e. k˜ x(0)k ≤ d. R EFERENCES [1] J. A. Marshall, M. E. Broucke, and B. A. Francis, “Pursuit formations of unicycles,” Automatica, vol. 42, no. 1, pp. 3–12, 2006. [2] ——, “Formations of vehicles in cyclic pursuit,” IEEE Transactions on Automatic Control, vol. 49, no. 11, pp. 1963 – 1974, 2004. [3] T. H. Kim and T. Sugie, “Cooperative control for target-capturing task based on a cyclic pursuit strategy,” Automatica, vol. 43, no. 8, pp. 1426–1431, 2007. [4] I. Shames, B. Fidan, and B. D. O. Anderson, “Close target reconnaissance using autonomous uav formations.” 47th IEEE Conference on Decision and Control, 2008, pp. 1729–1734. [5] M. Fichtner and A. Grobmann, “A probabilistic visual sensor model for mobile robot localisation in structured environments.” IEEE/RSJ International Conference on Intelligent Robots and Systems, 2004, pp. 1980–1985. [6] P. N. Pathirana, N. Bulusu, A. V. Savkin, and S. Jha, “Node localization using mobile robots in delay-tolerant sensor networks,” IEEE Transactions on Mobile Computing, vol. 4, no. 3, pp. 285–296, 2005. [7] N. Patwari, J. N. Ash, S. Kyperountas, A. O. Hero, R. L. Moses, and N. S. Correal, “Locating the nodes: Cooperative localization in wireless sensor networks,” IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 54–69, 2005. [8] S. I. Roumeliotis and G. A. Bekey, “Distributed multirobot localization,” IEEE Transactions on Robotics and Automation, vol. 18, no. 5, pp. 781–795, 2002. [9] A. A. Fel’dbaum, “Dual control theory, parts I and II,” Automation and Remote Control, vol. 21, no. 9 and 11, pp. 874–880 and 1033–1039, 1961. [10] S. H. Dandach, B. Fidan, S. Dasgupta, and B. D. O. Anderson, “A continuous time linear adaptive source localization algorithm, robust to persistent drift,” Systems & Control Letters, vol. 58, no. 1, pp. 7–16, 2009. [11] I. Shames, S. Dasgupta, B. Fidan, and B. D. O. Anderson, “Circumnavigation using distance measurements.” European Control Conference, August 2009, pp. 2444–2449. [12] B. D. O. Anderson, “Exponential stability of linear equations arising in adaptive identification,” IEEE Transactions on Automatic Control, vol. 22, no. 1, pp. 83–88, 1977. [13] S. Sastry and M. Bodson, Adaptive Control: Stability, Convergence, and Robustness. Prentice-Hall, 1994. [14] H. D’Angelo, Linear Time-Varying Systems: Analysis and Synthesis. Allyn and Bacon, 1970.
Agent trajectory (solid line) and target position (+)
Distance between x and its estimate
9
2
8
1.8
7
1.6
6
1.4
5 ||x−x^ ||
1.2 Y (m)
4 3
1 0.8
2 0.6
1 0
0.4
−1
0.2
−2
−2
0
2
4 X (m)
6
8
0
10
0
8
8
7
7
6
6
5
5
4
4
3
3
2
2
50
100
1
150
0
50
time (sec)
Fig. 4.
150
Distance between y and x 9
||y−x||
||y−x^ ||
Distance between y and estimate of x
0
100 time (sec)
9
1
50
100
150
time (sec)
Agent trajectory in X-Y plane, ky(t) − x ˆ(t)k, ky(t) − x(t)k and kx(t) − x ˆ(t)k for the case that the target is stationary. Agent trajectory
Distance between x and its estimate
9
2
8
1.8
7
1.6
6
1.4
5 ||x−x^ ||
1.2 Y (m)
4 3
1 0.8
2 0.6
1 0
0.4
−1
0.2
−2
−2
0
2
4 X (m)
6
8
0
10
0
8
8
7
7
6
6
5
4
3
3
2
2
100 time (sec)
Fig. 5.
5
4
50
150
Distance between y and x 9
||y−x||
||y−x^ ||
Distance between y and estimate of x
0
100 time (sec)
9
1
50
150
1
0
50
100
150
time (sec)
Agent trajectory in X-Y plane, ky(t) − x ˆ(t)k, ky(t) − x(t)k and kx(t) − x ˆ(t)k for the case that the target moves slowly.
I. INTRODUCTION A frequently desirable way to accomplish a surveillance mission is to monitor a target by circling around it at a prescribed distance. If the position of the target is known, then the goal is to find a control law that makes one or more agents move to and then on a circular trajectory with prescribed radius around the target. This problem has recently been studied in literature, for example, see [1]–[4]. If the position of the target is initially unknown, then we need an estimator to localize the target as well as a controller to force the agent(s) to circumnavigate the target. Several localization algorithms have studied different cases where a single agent or a group of collaborative agents localizes a target under a variety of assumptions and scenarios and using different information about the relative position of agent(s) and target such as distance and bearing. For instance, in [5], [6] a single agent accomplishes the localization task and in [7], [8] collaborative localization algorithms are studied. This kind of problem in which the aim is to control a system whose characteristics are initially unknown and therefore the identification and control problem should be solved simultaneously is often called a dual control problem [9]. The dual problem of estimation and circumnavigation of an agent trying to move around a target with unknown position using distance measurements alone has previously been studied [10], [11]. But sometimes, it is preferable to use bearing measurements instead of distance measurements since distance measurement techniques are usually active methods in which the agent must transmit signals. In contrast, bearing measurement is a passive measurement technique and may often be preferred for this reason. Mohammad Deghat, Iman Shames and Brian D. O. Anderson are with Research School of Information Sciences and Engineering, The Australian National University, Canberra ACT 0200 and National ICT Australia (NICTA). Changbin Yu is with Research School of Information Sciences and Engineering, The Australian National University.
{Mohammad.Deghat, Iman.Shames, Brian.Anderson, Brad.Yu}@anu.edu.au
In this paper, we propose an estimator that uses only the agent position and the bearing angle of target to solve localization and circumnavigation problem. It is also desirable if not essential to apply controllers and estimators that do not use explicit derivatives of measurements, because differentiation amplifies high-frequency noise. An advantage of the proposed algorithm is that it avoids such differentiation. The rest of this paper is structured as follows. In section II the localization and circumnavigation problem is formally stated and the proposed solution is provided in section III. Section IV contains simulation results and finally, conclusions and future directions are presented in section V. II. PROBLEM STATEMENT Suppose there is a target with unknown position x(t) ∈ R2 at time t and an agent with known trajectory y(s) ∈ R2 for s ≤ t with knowledge of the bearing angle θ(s) for s ≤ t. The case s = t is depicted in Figure 1. Knowing also the desired distance d, our goal is to find an estimator that estimates the unknown position x(t) using measurements up to time t and a control law that makes the agent move on a circle with radius d centered at the point x(t) such that for a stationary target, the estimation error x ˜(t) = x ˆ(t) − x(t)
(1)
in which x ˆ(t) is the estimate of x, exponentially goes to zero and D(t) = ky(t)−x(t)k exponentially goes to d while for a slowly drifting target, x ˜(t) goes to a neighborhood of zero and D(t) gets close to d.
Fig. 1.
The relationship between x, y, θ, ϕ and ϕ ¯
Throughout this paper, k.k denotes the Euclidean norm.
III. PROPOSED ALGORITHM Since this problem is a combined estimation and control problem, we should simultaneously estimate x(t) and find a control law that forces the agent to move on a desired circular trajectory around the target. In this section, we first propose an algorithm for the case that the target is stationary and study the stability of the proposed algorithm. Then, we extend our results to the case that the target moves slowly. A. Stationary target Assume the target is stationary (x˙ = 0). Our first goal is to devise an estimator which does not require the derivative of the measured data and guarantees that x ˜(t) goes to zero exponentially fast. Let ϕ(t) be a unit vector on the line passing through x and y(t) which can be written as x − y(t) x − y(t) = kx − y(t)k D(t) · ¸ cos θ(t) ϕ(t) = sin θ(t)
ϕ(t) =
(2) (3)
where θ(t) is the angle of the unit vector ϕ(t) as shown in Figure 1. It should be noted that the measurement of the bearing angle to the target when D(t) = 0 is not well defined. Moreover, ϕ(t) is not defined for this case as well. Hence, it is desirable that D(t) 6= 0 for all t > 0. A sufficient condition for D(t) to be greater than zero for all t > 0 is given in Lemma 1. Assume k is a constant positive scalar; then the estimator can be defined as ¡ ¢ x ˆ˙ (t) = k I − ϕ(t)ϕT (t) (y(t) − x ˆ(t)). (4) where I is the identity matrix and ϕ(t)ϕT (t) is a projection matrix onto the vector ϕ(t). Considering (1) and (4), the estimation error dynamics can be written as ¡ ¢ x ˜˙ (t) = k I − ϕ(t)ϕT (t) (y(t) − x ˆ(t)) ¡ ¢ T = k I − ϕ(t)ϕ (t) (y(t) − x − x ˆ(t) + x) ¢ (5) (2) ¡ T ˜(t)) = k I − ϕ(t)ϕ (t) (−ϕ(t)D(t) − x ¡ ¢ ϕT ϕ=1 − k I − ϕ(t)ϕT (t) x ˜(t) = Introducing a constant positive scalar α, the control law can be defined as ¡ ¢ ˆ − d ϕ(t) + αϕ(t) y(t) ˙ = D(t) ¯ (6) ˆ where D(t) = ky(t)− x ˆ(t)k and ϕ(t) ¯ ∈ R2 is the unit vector perpendicular to ϕ(t), obtained by π/2 clockwise rotation of ˆ = d, ϕ(t) as shown in Figure 1. It can be seen that if D(t) then the agent does not move toward or away from the target but it just moves on the circle around the target. Lemma 1: Under the estimator (4) and the control law (6), D(t) > 0 ∀t > 0 for D(0) 6= 0 and k˜ x(0)k = kˆ x(0) − xk ≤ d. See Appendix I for the proof.
Now, we should prove that by using the estimator (4) and the control law (6), the estimation error x ˜(t) in (1) exponentially goes to zero and D(t) exponentially goes to d. To this end, we recall the following theorem [12] which will be used to prove the exponential stability of x ˜(t). Proposition 1: Let V (.) : R+ → Rn×r be regulated. Then x˙ = −V V T x
(7)
is exponentially asymptotically stable iff for some positive α1 , α2 , T and for all t0 ∈ R+ Z t0 +T α1 I ≤ V (t)V T (t)dt ≤ α2 I (8) t0
The condition (8) is called persistence of excitation condition and V (.) is said to be persistently exciting (p.e.) if it satisfies (8). Another interpretation of the p.e. condition (8) in scalar form is [13] Z t0 +T ¡ T ¢2 ²1 ≤ U V (t) dt ≤ ²2 ∀t0 ∈ R+ (9) t0
where ²1 and ²2 are positive scalars and U ∈ Rn can be any constant unit vector. Therefore, x˙ = −V V T x is exponentially asymptotically stable if and only if for some positive scalars ²1 , ²2 , T and for all t0 ∈ R+ and all constant unit vectors U , the condition (9) holds. The first step towards the proof of estimator convergence is to write (5) in a form similar to (7) so that we can use Proposition 1. Lemma 2: Considering (3), the estimation error equation (5) can be written as x ˜˙ (t) = −k ϕ(t) ¯ ϕ¯T (t)˜ x(t)
(10)
Proof : The matrix I − ϕ(t)ϕT (t) can be written as · ¸ · ¸ ¤ 1 0 cos θ £ T cos θ sin θ I − ϕ(t)ϕ (t) = − 0 1 sin θ · ¸ sin2 θ − sin θ cos θ = − sin θ cos θ cos2 θ · ¸ (11) ¤ − sin θ £ − sin θ cos θ = cos θ · ¸ ¤ sin θ £ sin θ − cos θ = − cos θ £ ¤T It can be seen that the vector sin θ − cos θ is the unit vector that we previously defined as ϕ(t) ¯ because it can be obtained by π/2 clockwise rotation of ϕ(t) and therefore I − ϕ(t)ϕT (t) = ϕ(t) ¯ ϕ¯T (t). ¥
Lemma 3: Using the control law (6), the signal ϕ(t) ¯ in (10) is persistently exciting for all t ≥ 0 and x ˜(t) exponentially goes to zero. Proof : Considering (10) and Proposition 1, the signal x ˜(t) exponentially goes to zero if and only if ϕ(t) ¯ is persistently exciting and the condition on ϕ(t) ¯ to be p.e. is that there exist some ²1 , ²2 , and T , such that Z t0 +T ¡ T ¢2 ²1 ≤ U ϕ(t) ¯ dt ≤ ²2 (12) t0
is satisfied for all constant unit length U ∈ R2 and all t0 ∈ R+ . Assume γu (t) is the angle from the unit vector U to the vector ϕ(t) ¯ (as shown in Figure 2 for a sample U ). We assume that if the direction of the angles θ(t), ξ(t) or γu (t) are counter-clockwise then they are positive; otherwise, they are negative. For example, in the special case shown in Figure 2, the angle γu (t) is negative while θ(t) and ξ(t) are positive. Therefore (12) can be written as Z t0 +T ²1 ≤ cos2 γu (t)dt ≤ ²2 (13) t0
So, if there exists an upper bound for D(t) such that D(t) ≤ Dmax ∀t ≥ 0 (and the proof of this fact is below) then dγu (t) α ≥ Dmax and consequently dt γu (t + t0 ) ≥ γu (t0 ) +
αt Dmax
∀t ≥ 0
(18)
Therefore, the continuous function γu (t) always increases when time increases and it cannot converge to a constant value. So, we can always find some positive ²1 and T that satisfy (13).
Fig. 3.
ˆ The relationship between k˜ xk, D and D
The last thing to prove is that D(t) ≤ Dmax . Considering (11), the matrix −ϕ(t) ¯ ϕ¯T (t) is symmetric and its eigenvalues are 0 and -1. Therefore by choosing the Lyapunov function V = 12 x ˜T x ˜ and by considering (10), it can be seen that V˙ = −k˜ xT ϕ¯ϕ¯T x ˜ = −kkϕ¯T x ˜k2 is negative semi-definite and (10) is uniformly stable. Therefore x ˜(t) is bounded and k˜ x(t)k ≤ k˜ x(0)k
(19)
According to Figure 3 and using the triangle inequality, we have ˆ |D(t) − D(t)| ≤ k˜ x(t)k ≤ k˜ x(0)k (20) Fig. 2.
The relationship between U , γu , ξ and ϕ
By defining δ(t) and ∆(t) as
Since cos2 (.) ≤ 1, the integral in (13) is always bounded from above and an upper bound for (13) is ²2 = T . On the other hand, cos2 (.) ≥ 0 ∀t ≥ 0. The angle ξ(t) − γu (t) is always constant because U is a constant unit vector and consequently, dγu (t) dξ(t) = dt dt
(14)
On the other hand, the unit vector ϕ(t) ¯ is perpendicular to ϕ(t), obtained by π/2 clockwise rotation of ϕ(t) and therefore dξ(t) dθ(t) = (15) dt dt Since the speed of y(t) along ϕ(t) ¯ is α (see (6)) and the distance between x and y(t) is D(t) we have α dθ(t) = dt D(t)
(16)
α dγu (t) = . dt D(t)
(17)
and consequently
∆(t) = D(t) − d ˆ δ(t) = D(t) − D(t)
(21)
equation (20) can be written as |δ(t)| ≤ k˜ x(t)k ≤ k˜ x(0)k
(22)
Considering (6) and (2), the derivative of ∆(t) can be written as ³ ´ y˙ T (t) y(t) − x ˙ ∆(t) = D(t) ³¡ ´³ ´ ¢ ˆ − d ϕT (t) + αϕ¯T (t) y(t) − x D(t) = D(t) ³¡ ´³ ´ ¢ T ˆ D(t) − d ϕ (t) + αϕ¯T (t) − D(t)ϕ(t) = D(t) ¢ ¡ ˆ = − D(t) − d ¡ ¢ ¡ ¢ ˆ = − D(t) − d + D(t) − D(t) Therefore,
˙ ∆(t) = −∆(t) + δ(t)
(23)
and its solution is
Z
∆(t) = ∆(0)e
−t
t
+
e−(t−τ ) δ(τ )dτ
(24)
0
Since δ(t) is bounded (see (22)), ∆(t) in (24) is also bounded and therefore D(t) is bounded.¥ Having established that the estimation process proceeds satisfactorily, essentially because the control law provides the necessary persistence of excitation, it remains to demonstrate that the control law achieves the required objective. Theorem 1: Using the control law (6) and the estimator (4), D(t) exponentially converges to d. Proof : According to Lemma 3, x ˜(t) exponentially converges to zero and because of (22), the signal δ(t) exponentially goes to zero. So, in the light of (23), ∆(t) also goes to zero exponentially fast. ¥ B. Non-stationary target The analysis of the previous section was based on the assumption that the target is stationary. In this section, we assume that the target can move slowly and we want to show that under this assumption, the estimation error x ˜(t) converges to a neighborhood of zero. So, we suppose the target motion is such that the following assumption holds. Assumption 1: The target trajectory is differentiable and there exists a sufficiently small ε such that kx(t)k ˙ ω
(28)
for some constant positive scalar ω. Then the solution of ¡ ¢ x ˜˙ (t) = −k I − ϕ(t)ϕT (t) x ˜(t) (29) exponentially goes to zero. Proof : The proof is similar to the proof of Lemma 3. Equation (14) and (15) are valid for the non-stationary target case. But (16) may not be valid for the general case where the target can move freely. For instance, if the target moves such that x(t) ˙ = y(t) ˙ then dθ dt = 0. So we impose some constraints on the target speed to ensure that such a situation never occurs. If we assume that D(t) is bounded such that D(t) ≤ Dmax (see the proof below) and if α and ε in (6) and (25) satisfy α−ε>ω >0 (30) then instead of (16) we have
Consider x ˜(t) and x ˆ˙ (t) in (1) and (4). Then, the estimation error dynamics can be written as ¡ ¢ x ˜˙ (t) = −k I − ϕ(t)ϕT (t) x ˜(t) − x(t) ˙ (26) = −k ϕ(t) ¯ ϕ¯T (t)˜ x(t) − x(t) ˙ To prove that x ˜(t) in (26) goes to a neighborhood of zero, we use the following theorem [14]: Proposition 2: If the coefficient matrix A(t) is continuous for all t ∈ [0, ∞) and constants a > 0, b > 0 exist such that for every solution of the homogeneous differential equation x(t) ˙ = A(t)x(t) one has kx(t)k ≤ bkx(t0 )ke−a(t−t0 ) ,
0 ≤ t0 < t < ∞
then for each f (t) bounded and continuous on [0, ∞), every solution of the nonhomogeneous equation x(t) ˙ = A(t)x(t) + f (t),
x(t0 ) = 0
is also bounded for t ∈ [0, ∞). In particular, if kf (t)k ≤ Kf < ∞ then the solution of the perturbed system satisfies kx(t)k ≤ bkx(t0 )ke−a(t−t0 ) +
¢ bKf ¡ 1 − e−a(t−t0 ) (27) a
dθ(t) ω ≥ dt D(t)
(31)
Considering (14) and (15), equation (31) can be written as
and therefore
dγt (t) ω ≥ dt D(t)
(32)
dγu (t) ω ≥ dt Dmax
(33)
and γu (t + t0 ) ≥ γu (t0 ) +
ωt , Dmax
∀t ≥ 0
(34)
Thus, in the case that the target is non-stationary, the signal x ˜(t) in (29) is persistently exciting if D(t) is bounded. The final step is to prove that there exists an upper bound Dmax such that D(t) ≤ Dmax . Similarly to the proof of Lemma 3, it can be seen that (20) and (22) are valid for the case of a non-stationary target. Considering (21), (6) and (2), the derivative of ∆(t) can be written as ³ ´³ ´ y˙ T (t) − x˙ T (t) y(t) − x(t) ˙ ∆(t) = D(t) ¢ ¡ ˆ − d + x˙ T (t)ϕ(t) = − D(t) ¡ ¢ ¡ ¢ ˆ = − D(t) − d + D(t) − D(t) + x˙ T (t)ϕ(t)
Therefore, ˙ ∆(t) = −∆(t) + δ(t) + x˙ (t)ϕ(t) T
and its solution is ∆(t) = ∆(0)e
−t
Z +
t
(35)
³ ´ e−(t−τ ) δ(τ )+ x˙ T (τ )ϕ(τ ) dτ (36)
0
Since δ(t) and x(t) ˙ are bounded (see (22) and (25)) and ϕ(t) is a unit vector, equation (36) can be written as Z t |∆(t)| ≤ |∆(0)|e−t + (k˜ x(0)k + ε) e−(t−τ ) dτ (37) 0
It can be seen that ∆(t) in (37) is bounded and therefore D(t) ≤ Dmax . ¥ Lemma 5: Adopt the hypothesis of Lemma 4. Then x ˜(t) in
¡ ¢ x ˜˙ (t) = −k I − ϕ(t)ϕT (t) x ˜(t) − x(t) ˙
(38)
goes to a neighborhood of zero when t → ∞. Proof : The proof is the direct consequence of Lemma 4 and Proposition 2.¥ Theorem 2: Adopt the hypothesis of Lemma 4 and suppose that the control law (6) is used. Then D(t) goes to a neighborhood of d when t → ∞. Proof : From (36), (22) and (25) we have Z t ³ ´ |∆(t)| ≤ |∆(0)|e−t + e−(t−τ ) k˜ x(t)k + ε dτ (39) 0
According to Lemma 4, x ˜(t) goes to a neighborhood of zero and therefore, |∆(t)| = |D(t) − d| goes to a neighborhood of zero.¥ IV. SIMULATIONS In this section, simulation results for both cases are presented. For the stationary target, we assumed that d = 2, x = [2, 3]T , y(0) = [9, 8]T and the constant k in (4) is 5. As can be seen in Figure 4, the estimation error goes to zero exponentially fast and D(t) exponentially goes to d. Then, we consider the case that the target moves slowly and x(t) = [2 + .025t, 3 + sin(.025t) + .025t]T , y(0) = [9, 8]T , d = 2 and k = 5. According to Lemma 5, we expect that x ˜(t) goes to a neighborhood of x(t). It can be seen in Figure 5 that x ˆ(t) tracks x(t) with a small steady state error which is proportional to the target speed and D(t) gets close to d. V. C ONCLUDING R EMARKS ND F UTURE W ORKS In this paper, we proposed an estimator and a controller for a circumnavigation problem. The estimator estimates an unknown target using only the agent’s position and the bearing angle of the target without any explicit differentiation of the measured data. Stability of both estimator and controller has been studied in the cases where the target is stationary and it moves slowly. Future directions of research include solving the problem in 3-D space, testing the proposed controller and estimator experimentally using a mobile robot, extending the problem to the case where more than one agent is present and the case that the agent is required to move with constant speed.
VI. ACKNOWLEDGEMENTS This work is supported by NICTA, which is funded by the Australian Government as represented by the Department of Broadband, Communications and the Digital Economy and the Australian Research Council through the ICT Centre of Excellence program. Changbin Yu is supported by the Australian Research Council through an Australian Postdoctoral Fellowship under DP-0877562. VII. A PPENDIX I Proof of Lemma 1: Considering (21) and (24) we have Z t ³ ´ ˆ ) dτ D(t) = d+(D(0) − d) e−t + e−(t−τ ) D(τ ) − D(τ 0
(40)
and from (20) we conclude that ˆ − k˜ x(0)k ≤ D(t) − D(t) ≤ k˜ x(0)k Therefore, (40) can be written as
Z
t
e−(t−τ ) dτ − k˜ x(0)k (42) 0 ¢ ¡ −t −t = D(0)e + (d − k˜ x(0)k) 1 − e
D(t) ≥ d + (D(0) − d) e
−t
(41)
Since D(0)e−t is positive, D(t) > 0 ∀t > 0 if the last term of (42) is non-negative i.e. k˜ x(0)k ≤ d. R EFERENCES [1] J. A. Marshall, M. E. Broucke, and B. A. Francis, “Pursuit formations of unicycles,” Automatica, vol. 42, no. 1, pp. 3–12, 2006. [2] ——, “Formations of vehicles in cyclic pursuit,” IEEE Transactions on Automatic Control, vol. 49, no. 11, pp. 1963 – 1974, 2004. [3] T. H. Kim and T. Sugie, “Cooperative control for target-capturing task based on a cyclic pursuit strategy,” Automatica, vol. 43, no. 8, pp. 1426–1431, 2007. [4] I. Shames, B. Fidan, and B. D. O. Anderson, “Close target reconnaissance using autonomous uav formations.” 47th IEEE Conference on Decision and Control, 2008, pp. 1729–1734. [5] M. Fichtner and A. Grobmann, “A probabilistic visual sensor model for mobile robot localisation in structured environments.” IEEE/RSJ International Conference on Intelligent Robots and Systems, 2004, pp. 1980–1985. [6] P. N. Pathirana, N. Bulusu, A. V. Savkin, and S. Jha, “Node localization using mobile robots in delay-tolerant sensor networks,” IEEE Transactions on Mobile Computing, vol. 4, no. 3, pp. 285–296, 2005. [7] N. Patwari, J. N. Ash, S. Kyperountas, A. O. Hero, R. L. Moses, and N. S. Correal, “Locating the nodes: Cooperative localization in wireless sensor networks,” IEEE Signal Processing Magazine, vol. 22, no. 4, pp. 54–69, 2005. [8] S. I. Roumeliotis and G. A. Bekey, “Distributed multirobot localization,” IEEE Transactions on Robotics and Automation, vol. 18, no. 5, pp. 781–795, 2002. [9] A. A. Fel’dbaum, “Dual control theory, parts I and II,” Automation and Remote Control, vol. 21, no. 9 and 11, pp. 874–880 and 1033–1039, 1961. [10] S. H. Dandach, B. Fidan, S. Dasgupta, and B. D. O. Anderson, “A continuous time linear adaptive source localization algorithm, robust to persistent drift,” Systems & Control Letters, vol. 58, no. 1, pp. 7–16, 2009. [11] I. Shames, S. Dasgupta, B. Fidan, and B. D. O. Anderson, “Circumnavigation using distance measurements.” European Control Conference, August 2009, pp. 2444–2449. [12] B. D. O. Anderson, “Exponential stability of linear equations arising in adaptive identification,” IEEE Transactions on Automatic Control, vol. 22, no. 1, pp. 83–88, 1977. [13] S. Sastry and M. Bodson, Adaptive Control: Stability, Convergence, and Robustness. Prentice-Hall, 1994. [14] H. D’Angelo, Linear Time-Varying Systems: Analysis and Synthesis. Allyn and Bacon, 1970.
Agent trajectory (solid line) and target position (+)
Distance between x and its estimate
9
2
8
1.8
7
1.6
6
1.4
5 ||x−x^ ||
1.2 Y (m)
4 3
1 0.8
2 0.6
1 0
0.4
−1
0.2
−2
−2
0
2
4 X (m)
6
8
0
10
0
8
8
7
7
6
6
5
5
4
4
3
3
2
2
50
100
1
150
0
50
time (sec)
Fig. 4.
150
Distance between y and x 9
||y−x||
||y−x^ ||
Distance between y and estimate of x
0
100 time (sec)
9
1
50
100
150
time (sec)
Agent trajectory in X-Y plane, ky(t) − x ˆ(t)k, ky(t) − x(t)k and kx(t) − x ˆ(t)k for the case that the target is stationary. Agent trajectory
Distance between x and its estimate
9
2
8
1.8
7
1.6
6
1.4
5 ||x−x^ ||
1.2 Y (m)
4 3
1 0.8
2 0.6
1 0
0.4
−1
0.2
−2
−2
0
2
4 X (m)
6
8
0
10
0
8
8
7
7
6
6
5
4
3
3
2
2
100 time (sec)
Fig. 5.
5
4
50
150
Distance between y and x 9
||y−x||
||y−x^ ||
Distance between y and estimate of x
0
100 time (sec)
9
1
50
150
1
0
50
100
150
time (sec)
Agent trajectory in X-Y plane, ky(t) − x ˆ(t)k, ky(t) − x(t)k and kx(t) − x ˆ(t)k for the case that the target moves slowly.
