Target Localization and Circumnavigation Using Bearing Measurements in 2D

Transcription

1 Target Localization and Circumnavigation Using Bearing Measurements in D 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 [] []. 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 agents) 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 agents) and target such as distance and bearing. For instance, in [], [] a single agent accomplishes the localization task and in [], [] 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 []. 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 [], []. 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 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 xt) R at time t and an agent with known trajectory ys) R for s t with knowledge of the bearing angle θs) for s t. The case s t is depicted in Figure. Knowing also the desired distance d, our goal is to find an estimator that estimates the unknown position xt) 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 xt) such that for a stationary target, the estimation error xt) ˆxt) xt) ) in which ˆxt) is the estimate of x, exponentially goes to zero and yt) xt) exponentially goes to d while for a slowly drifting target, xt) goes to a neighborhood of zero and gets close to d. Fig.. The relationship between x, y, θ, ϕ and ϕ Throughout this paper,. denotes the Euclidean norm.

2 III. PROPOSED ALGORITHM Since this problem is a combined estimation and control problem, we should simultaneously estimate xt) 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 ẋ ). Our first goal is to devise an estimator which does not require the derivative of the measured data and guarantees that xt) goes to zero exponentially fast. Let ϕt) be a unit vector on the line passing through x and yt) which can be written as ϕt) x yt) x yt) x yt) [ ] cos θt) ϕt) sin θt) where θt) is the angle of the unit vector ϕt) as shown in Figure. It should be noted that the measurement of the bearing angle to the target when is not well defined. Moreover, ϕt) is not defined for this case as well. Hence, it is desirable that for all t >. A sufficient condition for to be greater than zero for all t > is given in Lemma. Assume k is a constant positive scalar; then the estimator can be defined as ) ) ˆxt) k I ϕt)ϕ T t) ) yt) ˆxt)). ) where I is the identity matrix and ϕt)ϕ T t) is a projection matrix onto the vector ϕt). Considering ) and ), the estimation error dynamics can be written as xt) k I ϕt)ϕ T t) ) yt) ˆxt)) k I ϕt)ϕ T t) ) yt) x ˆxt) + x) ) k I ϕt)ϕ T t) ) ϕt) xt)) ϕ T ϕ k I ϕt)ϕ T t) ) xt) Introducing a constant positive scalar α, the control law can be defined as ) ẏt) ˆ d ) ϕt) + α ϕt) ) where ˆ yt) ˆxt) and ϕt) R is the unit vector perpendicular to ϕt), obtained by π/ clockwise rotation of ϕt) as shown in Figure. It can be seen that if ˆ d, then the agent does not move toward or away from the target but it just moves on the circle around the target. Lemma : Under the estimator ) and the control law ), > t > for D) and x) ˆx) x d. See Appendix I for the proof. Now, we should prove that by using the estimator ) and the control law ), the estimation error xt) in ) exponentially goes to zero and exponentially goes to d. To this end, we recall the following theorem [] which will be used to prove the exponential stability of xt). Proposition : Let V.) : R + R n r be regulated. Then ẋ V V T x ) is exponentially asymptotically stable iff for some positive α, α, T and for all t R + α I +T t V t)v T t) α I ) The condition ) is called persistence of excitation condition and V.) is said to be persistently exciting p.e.) if it satisfies ). Another interpretation of the p.e. condition ) in scalar form is [] ɛ +T t U T V t) ) ɛ t R + ) where ɛ and ɛ are positive scalars and U R n can be any constant unit vector. Therefore, ẋ V V T x is exponentially asymptotically stable if and only if for some positive scalars ɛ, ɛ, T and for all t R + and all constant unit vectors U, the condition ) holds. The first step towards the proof of estimator convergence is to write ) in a form similar to ) so that we can use Proposition. Lemma : Considering ), the estimation error equation ) can be written as xt) k ϕt) ϕ T t) xt) ) Proof: The matrix I ϕt)ϕ T t) can be written as [ ] [ ] I ϕt)ϕ T cos θ [cos ] t) θ sin θ sin θ [ sin ] θ sin θ cos θ sin θ cos θ cos θ [ ] sin θ [ ] sin θ cos θ cos θ [ ] sin θ [sin ] θ cos θ cos θ ) It can be seen that the vector [ sin θ cos θ ] T is the unit vector that we previously defined as ϕt) because it can be obtained by π/ clockwise rotation of ϕt) and therefore I ϕt)ϕ T t) ϕt) ϕ T t).

3 Lemma : Using the control law ), the signal ϕt) in ) is persistently exciting for all t and xt) exponentially goes to zero. Proof: Considering ) and Proposition, the signal xt) 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 ɛ, ɛ, and T, such that ɛ +T t U T ϕt) ) ɛ ) is satisfied for all constant unit length U R and all t R +. Assume γ u t) is the angle from the unit vector U to the vector ϕt) as shown in Figure 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, the angle γ u t) is negative while θt) and ξt) are positive. Therefore ) can be written as Fig.. ɛ +T t cos γ u t) ɛ ) The relationship between U, γ u, ξ and ϕ Since cos.), the integral in ) is always bounded from above and an upper bound for ) is ɛ T. On the other hand, cos.) t. The angle ξt) γ u t) is always constant because U is a constant unit vector and consequently, dξt) ) On the other hand, the unit vector ϕt) is perpendicular to ϕt), obtained by π/ clockwise rotation of ϕt) and therefore dξt) dθt) ) Since the speed of yt) along ϕt) is α see )) and the distance between x and yt) is we have and consequently dθt) α ) α. ) So, if there exists an upper bound for such that t and the proof of this fact is below) then and consequently α γ u t + t ) γ u t ) + αt t ) 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 ɛ and T that satisfy ). Fig.. The relationship between x, D and ˆD The last thing to prove is that. Considering ), the matrix ϕt) ϕ T t) is symmetric and its eigenvalues are and -. Therefore by choosing the Lyapunov function V xt x and by considering ), it can be seen that V k x T ϕ ϕ T x k ϕ T x is negative semi-definite and ) is uniformly stable. Therefore xt) is bounded and xt) x) ) According to Figure and using the triangle inequality, we have ˆ xt) x) ) By defining δt) and t) as t) d δt) ˆ equation ) can be written as ) δt) xt) x) ) Considering ) and ), the derivative of t) can be written as ) ẏ T t) yt) x t) ) ) ˆ d ϕ T t) + α ϕ t)) T yt) x ) ) ˆ d ϕ T t) + α ϕ t)) T ϕt) ) ˆ d d ) + ˆ ) Therefore, t) t) + δt) )

4 and its solution is t) )e t + e t τ) δτ)dτ ) Since δt) is bounded see )), t) in ) is also bounded and therefore 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 : Using the control law ) and the estimator ), exponentially converges to d. Proof: According to Lemma, xt) exponentially converges to zero and because of ), the signal δt) exponentially goes to zero. So, in the light of ), 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 xt) converges to a neighborhood of zero. So, we suppose the target motion is such that the following assumption holds. Assumption : The target trajectory is differentiable and there exists a sufficiently small ε such that ẋt) < ε ) Consider xt) and ˆxt) in ) and ). Then, the estimation error dynamics can be written as xt) k I ϕt)ϕ T t) ) xt) ẋt) k ϕt) ϕ T t) xt) ẋt) ) To prove that xt) in ) goes to a neighborhood of zero, we use the following theorem []: Proposition : If the coefficient matrix At) is continuous for all t [, ) and constants a >, b > exist such that for every solution of the homogeneous differential equation one has ẋt) At)xt) xt) b xt ) e at t), t < t < then for each ft) bounded and continuous on [, ), every solution of the nonhomogeneous equation ẋt) At)xt) + ft), xt ) is also bounded for t [, ). In particular, if ft) K f < then the solution of the perturbed system satisfies xt) b xt ) e at t) + bk f e at t ) ) ) a The first step to prove that xt) in ) goes to a neighborhood of zero is to show that xt) I ϕt)ϕ T t) ) xt) is exponentially stable in the case that the target is nonstationary. Then, using Proposition, it will be shown that the solution of ) is bounded and goes to a neighborhood of zero when t. Lemma : Suppose the target is moving such that Assumption holds and assume α and ε in ) and ) satisfy α ε > ω ) for some constant positive scalar ω. Then the solution of xt) k I ϕt)ϕ T t) ) xt) ) exponentially goes to zero. Proof: The proof is similar to the proof of Lemma. Equation ) and ) are valid for the non-stationary target case. But ) may not be valid for the general case where the target can move freely. For instance, if the target moves such that ẋt) ẏt) then dθ. So we impose some constraints on the target speed to ensure that such a situation never occurs. If we assume that is bounded such that see the proof below) and if α and ε in ) and ) satisfy α ε > ω > ) then instead of ) we have dθt) ω ) Considering ) and ), equation ) can be written as and therefore and dγ t t) γ u t + t ) γ u t ) + ω ) ω ) ωt, t ) Thus, in the case that the target is non-stationary, the signal xt) in ) is persistently exciting if is bounded. The final step is to prove that there exists an upper bound such that. Similarly to the proof of Lemma, it can be seen that ) and ) are valid for the case of a non-stationary target. Considering ), ) and ), the derivative of t) can be written as ) ) ẏ T t) ẋ T t) yt) xt) t) ˆ d ) + ẋ T t)ϕt) d ) + ˆ ) + ẋ T t)ϕt)

5 Therefore, and its solution is t) )e t + t) t) + δt) + ẋ T t)ϕt) ) e t τ) ) δτ)+ẋ T τ)ϕτ) dτ ) Since δt) and ẋt) are bounded see ) and )) and ϕt) is a unit vector, equation ) can be written as t) ) e t + x) + ε) e t τ) dτ ) It can be seen that t) in ) is bounded and therefore. Lemma : Adopt the hypothesis of Lemma. Then xt) in xt) k I ϕt)ϕ T t) ) xt) ẋt) ) goes to a neighborhood of zero when t. Proof: The proof is the direct consequence of Lemma and Proposition. Theorem : Adopt the hypothesis of Lemma and suppose that the control law ) is used. Then goes to a neighborhood of d when t. Proof: From ), ) and ) we have t) ) e t + e t τ) ) xt) + ε dτ ) According to Lemma, xt) goes to a neighborhood of zero and therefore, 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, x [, ] T, y) [, ] T and the constant k in ) is. As can be seen in Figure, the estimation error goes to zero exponentially fast and exponentially goes to d. Then, we consider the case that the target moves slowly and xt) [ +.t, + sin.t) +.t] T, y) [, ] T, d and k. According to Lemma, we expect that xt) goes to a neighborhood of xt). It can be seen in Figure that ˆxt) tracks xt) with a small steady state error which is proportional to the target speed and gets close to d. V. CONCLUDING REMARKS ND FUTURE WORKS 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 -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-. VII. APPENDIX I Proof of Lemma : Considering ) and ) we have d+d) d) e t + e t τ) Dτ) ˆDτ) ) dτ and from ) we conclude that ) x) ˆ x) ) Therefore, ) can be written as d + D) d) e t x) e t τ) dτ D)e t + d x) ) e t) ) Since D)e t is positive, > t > if the last term of ) is non-negative i.e. x) d. REFERENCES [] J. A. Marshall, M. E. Broucke, and B. A. Francis, Pursuit formations of unicycles, Automatica, vol., no., pp.,. [], Formations of vehicles in cyclic pursuit, IEEE Transactions on Automatic Control, vol., no., pp.,. [] T. H. Kim and T. Sugie, Cooperative control for target-capturing task based on a cyclic pursuit strategy, Automatica, vol., no., pp.,. [] I. Shames, B. Fidan, and B. D. O. Anderson, Close target reconnaissance using autonomous uav formations. th IEEE Conference on Decision and Control,, pp.. [] 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,, pp.. [] 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., no., pp.,. [] 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., no., pp.,. [] S. I. Roumeliotis and G. A. Bekey, Distributed multirobot localization, IEEE Transactions on Robotics and Automation, vol., no., pp.,. [] A. A. Fel dbaum, Dual control theory, parts I and II, Automation and Remote Control, vol., no. and, pp. and,. [] 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., no., pp.,. [] I. Shames, S. Dasgupta, B. Fidan, and B. D. O. Anderson, Circumnavigation using distance measurements. European Control Conference, August, pp.. [] B. D. O. Anderson, Exponential stability of linear equations arising in adaptive identification, IEEE Transactions on Automatic Control, vol., no., pp.,. [] S. Sastry and M. Bodson, Adaptive Control: Stability, Convergence, and Robustness. Prentice-Hall,. [] H. D Angelo, Linear Time-Varying Systems: Analysis and Synthesis. Allyn and Bacon,.

6 Agent trajectory solid line) and target position +) Distance between x and its estimate. Y m) x x^ X m) Distance between y and estimate of x Distance between y and x y x^ y x Fig.. Agent trajectory in X-Y plane, yt) ˆxt), yt) xt) and xt) ˆxt) for the case that the target is stationary. Agent trajectory Distance between x and its estimate Y m) x x^ X m) Distance between y and estimate of x Distance between y and x y x^ y x Fig.. Agent trajectory in X-Y plane, yt) ˆxt), yt) xt) and xt) ˆxt) for the case that the target moves slowly.