The Problem of Target Following Based on Range ... - Semantic Scholar
Joint 48th IEEE Conference on Decision and Control and 28th Chinese Control Conference Shanghai, P.R. China, December 16-18, 2009
FrC16.1
The Problem of Target Following Based on Range-only Measurements For Car-like Robots Alexey S. Matveev, Hamid Teimoori, and Andrey V. Savkin Abstract— In this paper, we consider the problem of navigation and guidance of a Dubins-like wheeled robot towards a steady or maneuvering target with range-only information. We propose a range-based sliding mode controller through which the robot follows a target with a constant speed while preserving a predefined range margin from the target. Mathematically rigorous proof of convergence and stability of the proposed guidance law is presented. Simulation results confirm the applicability and performance of the proposed guidance approach.
I. I NTRODUCTION The problems of target following and trajectory tracking for car-like nonholonomic robots have been widely studied in robotics over the past two decades (see e.g. [6], [10], [14], [18] and references therein). These robots have restrictions in mobility due to the particular nature of nonholonomic systems, i.e. wheels rolling without slipping sideways. Because of the deficiency in controllability of their linear models, these systems are not controllable by linear controllers [4]. As a result, nonlinear controllers have been widely studied for this class of systems in recent years. Several sophisticated approaches to target following and trajectory tracking by wheeled robots have been proposed in the literature. A strategy for regulating a car-like robot using a receding horizon controller was proposed in [10]. Soetanto et al. [14] presented an approach to steering a wheeled robot along a desired path, which is based on the Lyapunov and back-stepping techniques. A sliding mode control law for the problem of trajectory tracking by nonholonomic mobile robots has been proposed in [18]. The control scheme is based on the computed torque method combined with feedback linearization of the system model. Another solution for the trajectory tracking problem based on dynamic feedback linearization is presented in [6]. A comprehensive summary of the available algorithms for motion planning and control of wheeled robots can be found in [7]. Among the current approaches, sliding mode control of car-like robots, due to the fast response, good transient performance and robustness against system uncertainties, has been received an increasing attention from researchers. Xu et al. [17] proposed a model-based sliding mode controller for trajectory tracking by underwater robot manipulators. This work was supported by the Australian Research Council. A.S. Matveev is with the Department of Mathematics and Mechanics, Saint Petersburg University, Universitetskii 28, Petrodvoretz, St.Petersburg, 198504, Russia [email protected] H. Teimoori and A.V. Savkin are with the School of Electrical Engineering and Telecommunications, The University of New South Wales, Sydney, 2052, Australia [email protected],
[email protected]
978-1-4244-3872-3/09/$25.00 ©2009 IEEE
An approach to stabilization of nonholonomic systems by means of sliding mode control was proposed by Bloch and Drakunov [3]. For autonomous vehicles, a sliding mode control strategy for trajectory tracking and path following was presented in [15]. Most of the existing approaches to trajectory tracking and target following by wheeled robots assume that both the line-of-sight angle (bearing) and the relative distance (range) are available for navigation and guidance algorithms. There is also a relatively large body of research on navigation and guidance with only bearing measurements [2], [11]. In contrast, only few results on the problem of navigation and guidance towards an unknown target using range-only measurements have been published. The problem of range-only based localization and navigation is motivated by its applications to e.g. wireless networks, unmanned underwater vehicles, surveillance and emergency services [1], [5], [9], where the only available information about the target is the range between it and the pursuer robot. The pursuer-target range is obtained by measuring e.g., the time-of-flight of an acoustics pulse [9] or the strength of the signal received from the target [12], [13]. Such an approach could potentially entail reduction of the complexity and cost of the control hardware. However, a general algorithm is required by which the pursuer robot can be navigated towards the target based on range-only measurements. In this paper, we consider the problem of range-only navigation of a Dubins-like wheeled robot following a maneuvering target at a predefined distance, with the target maneuver being unknown in advance. We propose a sliding mode control strategy whereby the pursuer robot approaches the target using only measurements of the relative distance between them and follows the target at the range assigned by a control parameter. Theoretical analysis and mathematically rigorous justification of the guidance strategy is presented and its stability and performance is discussed. Its applicability is also confirmed by computer simulations. The proposed strategy is in essence similar to the Equiangular Navigation Guidance (ENG) law initially proposed in [16]. In the specific case of a stationary target, the pursuer robot driven by this strategy moves towards the target along a spiral-like curve resembling the so-called equiangular spiral. The reminder of the paper is organized as follows. Problem description and models of the controlled car-like robot and the target are introduced in Section II. Section III introduces the navigation and guidance law. Mathematically rigorous analysis of the proposed strategy is presented in Section IV. Computer simulation results are given in Section V. Finally,
8537
FrC16.1 Assumption 3.1: Both the initial and required distances between the robots are large enough:
the paper is concluded in Section V. II. S YSTEM DESCRIPTION AND PROBLEM SETUP We consider two planar mobile robots modeled as unicycles. They travel with time-varying speeds and turning radii limited by known constants. The linear acceleration is similarly bounded. One of the robots (say robot 1) plays the role of a target, the objective of the other is to follow the target at the given distance d0 . Unlike many works in the area, the pursuer (robot 2) has access to only the current ˙ distance d(t) to robot 1 and its derivative d(t). It is also aware of the bounds on the speed, acceleration, and turning radius of robot 1. At the same time, it has no access to the actual values of these variables and is unaware of the control rule driving robot 1. The kinematic model of the ith robot (i = 1, 2) is as follows x˙i = vi cos θi , vi ∈ [0, vi ], y˙i = vi sin θi , |v˙i | ≤ ai θ˙i = ui ∈ [−ui , ui ].
(1)
Here col (xi , yi ) is the vector of the robot Cartesian coordinates, θi gives its orientation, vi and ui are the speed and the angular velocity, respectively. The upper thresholds vi , ui , and ai are given. The problem to be considered is as follows. Given two robots, find a controller that asymptotically drives robot 2 to the required distance d(t) → d0 to robot 1, irrespective of what is the motion of the latter, i.e., the functions x1 (·), y1 (·), v1 (·) and u1 (·) satisfying (1). In this paper, we examine the following control algorithm for the pursuer (robot 2):
where
v2 (t) ≡ v2 , ˙ u2 (t) = u2 sgn d(t) + χ [d(t) − d0] , γr χ (r) := v∗ := γδ −v∗
(2)
if |r| ≤ δ if r > δ
(3)
if r < −δ
and γ , δ are controller parameters. We also adopt the following assumption, which means that the pursuer is more maneuverable than the target. Assumption 2.1: The following inequalities hold: u2 > u1 and v2 > v1 + au12 . III. T HE MAIN RESULT It employs the following constant that depends on the speed thresholds v1 , v2 from (1) and the parameter v∗ from (3) and is introduced under the assumption that v∗ + v1 < v2 : v q u u (v2 − v2 − v2 ) − v22 − (v1 + v∗ )2 v22 − (v1 − v∗ )2 ∗ u 2 1 q ω := t . (4) (v22 + v2∗ − v21 ) − v22 − (v1 + v∗ )2 v22 − (v1 − v∗ )2
To achieve the control objective, one more assumption is needed, which will be commented in Remark 3.2.
d 0 > R2 d(0) > R2
u2 (ω + 1)2 λ , u2 − u1
v1 u2 3 + 3π + (ω + 1)2 λ v2 − v1 u2 − u1
(5)
(6)
where R2 := uv22 is the minimal turning radius of robot 2 and λ ≥ 1 is a design parameter such that u1 = 0 ⇒ λ > 1. Now we are in a position to state the main result of the paper. Theorem 3.1: Suppose that Assumptions 2.1 and 3.1 hold, the parameters γ > 0 and v∗ = δ γ > 0 of the controller (2) are chosen so that along with the parameter λ from Assumption 3.1, they obey the inequality: 2 a1 + γ (v1 + v2 ) 2 2 (7) 2 + v∗ < (v2 − v1 ) . u2 − λ −1(u2 − u1 )
Then the controller (2) asymptotically drives robot 2 to the required distance to robot 1, i.e., d(t) → d0 as t → ∞. This is true irrespective of the motion of robot 1. The following complement to Theorem 3.1 illuminates the features of the robots relative motion by showing that the pursuer ultimately circulates around the target, with the angular velocity of circulation exceeding a certain lower threshold. To rigorously state this claim, we introduce the moving normally oriented Cartesian coordinate system centered at the characteristic point of the target (robot 1). The axes of the system keep their directions unchanged. Let d and ψ stand for the polar coordinates of the pursuer (robot 2) in this system. In other words, d is the distance between the robots and ψ is the algebraic angle from the x-axis to the vector from the target to the pursuer. Proposition 3.1: Suppose that the assumptions of Theorem 3.1 hold. Then irrespective of the motion of robot 1, either v2 − v1 >0 (8) lim ψ˙ (t) ≥ d0 t→∞ or v2 − v1 lim ψ˙ (t) ≤ − < 0. (9) t→∞ d0 Remark 3.1: i) By Theorem 3.1 and Proposition 3.1, d(t) ≈ d0 ∀t ≥ T and either ψ˙ (t) ≥ ζ ∀t ≥ T or ψ˙ (t) ≤ −ζ ∀t ≥ T for any ζ < d0−1 (v2 − v1 ), ζ ≈ d0−1 (v2 − v1 ) provided that T is large enough. This means that in the above coordinate system centered at the target, the pursuer asymptotically moves around the origin at the required distance d0 at the speed v2 ≥ v2 − v1 > 0. If the target is motionless v1 ≡ 0, this system does not move and the pursuer asymptotically moves at the speed v2 along the circle with the radius d0 centered at the target. ii) The controller (2) exhibits a sliding motion for robot 2. Theorem 3.1 and Proposition 3.1 address the equivalent dynamics [8]. iii) For given λ , inequality (7) describes the interior of an ellipse in the plane of the controller parameters (v∗ , γ ). This ellipse expands as λ ↑; in the marginal cases λ = 1 and λ → ∞, the denominator in (7) converts into u21
8538
FrC16.1 and u22 , respectively. Thanks to the second inequality in Assumption 2.1, this interior intersects the positive octant γ > 0, v∗ > 0 for large λ . So the controller can be tuned so that (7) is satisfied. We also note that (7) implies the condition v∗ + v1 < v2 under which the quantity (4) is well defined. iv) It can be shown that the quantity (4) increases as v∗ ↑. Hence decreasing v∗ improves the lower estimate (6) of the attraction domain, whereas the stability condition (7) does not limit v∗ from below. At the same time, decreasing v∗ may have a detrimental effect on the transient performance. v) By the previous remark, the quantity (ω + 1)2 λ in (5) and (6) increases as v∗ ↑ and λ ↑. Along with (7), this means that the larger the initial and required distances, the more freedom is in the choice of the controller parameters. The proof of Theorem 3.1 is given in Section IV.
IV. P ROOF OF T HEOREM 3.1 We introduce the normally oriented Cartesian coordinate system centered at the characteristic point of robot 1 with the x-axis directed towards robot 2. Let αi denote the algebraic angle between the speed vector of the ith robot and the xaxis. Then the motion equations (1) take the form: d˙ = v2 cos α2 − v1 cos α1 α˙ 1 = u1 − ϕ˙ , α˙ 2 = u2 − ϕ˙ .
ϕ˙ =
We start with technical estimates of expressions encountered here. Their proof is given in Appendix. Lemma 4.1: Whenever |v| ≤ v < v2 − v1 and 0 ≤ v1 ≤ v1 , the following implications hold v2 cos α2 − v1 cos α1 = v ⇒ v | sin α | ≤ v | sin α | − p(v − v )2 − v2 , 1 1 2 2 2 1 v1 | sin α1 | ≤ ω v2 | sin α2 |
A. The steady target By specifying Theorem 3.1 and Proposition 3.1 in the case of the steady target, we arrive at the following. Corollary 3.1: Let the target do not move v1 = 0, u1 = 0, a1 = 0 and d 0 > λ R2 ,
d(0) > [λ + 3]R2 ,
where λ > 1.
Suppose also that the parameters γ > 0 and v∗ = δ γ > 0 of the controller (2) obey the inequality: R22 1 − λ −1
2 2 2 2 γ + v∗ < v2 .
(11)
where ω is given by (4) with v∗ := v. The next lemma reveals conditions under which a discontinuous control law (2) exhibits a sliding motion. Lemma 4.2: For robot 2 driven by the control rule (2), the surface d˙ + χ (d − d0 ) = 0 is sliding in the domain sin α2 > 0 and d > d∗ := R2
Then the controller (2) drives robot 2 to the required distance to robot 1, i.e., d → d0 as t → ∞. Asymptotically, the pursuer circulates around the target at the required distance d0 so that the vector from the target to the pursuer rotates at the angular t→∞ velocity w(t) −−→ w∞ , where |w∞ | = d0−1 v2 . Remark 3.2: i) The controller of the form (2) cannot ensure global convergence d → d0 even for the steady target. For any controller parameters, there is a nonempty domain in the phase space such that whenever robot 2 enters this domain, afterwards it moves with the constant control u2 ≡ ±u2 along a circle of the radius R2 . Then the distance d oscillates and does not converge to d0 . Thus requirements to the robots initial and required states (like (6) and (5)) are unavoidable. ii) In the case of the steady target, a comprehensive analysis of the behavior exhibited by the controller (2) for all possible parameter values can be carried out. It shows that as the parameter v∗ exceeds a certain threshold, the above domain of non-convergence d 6→ d0 becomes infinite: whenever the robots are initially far enough, the convergence does not hold. As v∗ exceeds another and larger threshold, the convergence does not hold for almost all initial states. In the light of these facts, imposing upper bounds on the controller parameters by the key condition (7) seems reasonable.
(10)
v2 sin α2 −v1 sin α1 d
u2 (ω + 1)2 λ u2 − u1
(12)
and two-side repelling in the domain sin α2 < 0 and d > d∗
(13)
whenever (7) holds. Here ω is given by (4). On this surface, p (v2 − v1 )2 − v2∗ > 0, v2 p (v2 − v1 )2 − v2∗ |ϕ˙ | ≥ . (14) d a) Proof: We put v(t) := −χ (d − d0 ) and note that |v(t)| ≤ v∗ and |v| ˙ ≤ γ (v2 + v1 ) by (2). Hence if d ≥ d∗ ,
8539
| sin α2 | ≥
d˙ (10) d + χ (d − d0 ) = d¨ − v˙ == dt − [v2 (u2 − ϕ˙ ) sin α2 − v1 (u1 − ϕ˙ ) sin α1 ] − v˙1 cos α1 − v˙ b
{ v2 sin α2 − v1 sin α1 == + v1u1 sin α1 − v˙1 cos α1 − v˙ d − v2 u2 sin α2 ; 2 v2 | sin α2 | + v1| sin α1 | |b| ≤ + d∗ v1 u1 | sin α1 | + a1 + γ (v2 + v1 ). (10)
z
2
}|
FrC16.1 ˙ If pd + χ (d − d0) = 0, Lemma 4.1 yields that v2 | sin α2 | ≥ µ := (v2 − v1 )2 − v2∗ and v2 (ω + 1)2 |b| ≤ + u1 v2 | sin α2 |−u1 µ +a1 + γ (v2 +v1 ) d∗ (12) == λ −1 [u2 − u1 ] + u1 v2 | sin α2 | − u1 µ + a1 + γ (v2 + v1 ) = λ −1 [u2 − u1 ] + u1 − u2 v2 | sin α2 | − u1 µ + a1+ {z } |
(10) We note that cos α2 = 1 ⇒ d˙ = v2 − v1 cos α1 ≥ v2 − v1 > ˙ v∗ and cos α2 = −1 ⇒ d < −v∗ . In terms of the variables
p := d cos α2 ,
(7)
γ (v2 + v1 ) + u2 v2 | sin α2 | ≤ u2 v2 | sin α2 | − ε ,
ε > 0 is small enough. Hence the signs taken by where d ˙ d + χ (d − d0 ) for u2 = u2 and u2 = −u2 , respectively, dt are opposite. If sin α2 > 0, this sign is opposite to sgn u2 ; if sin α2 < 0, the signs are equal. This implies the first conclusion of the lemma. Inequalities (14) follow from (10) and the first relation in (11). • Lemma 4.3: Let robot 2 be driven by the control rule (2), and inequalities (5) and (7) hold. If the equation d˙ + χ (d − d0 ) = 0 becomes true at some time instant t0 when the second inequality from (12) is satisfied, then d → d0 as t → ∞ and v2 − v1 lim |ϕ˙ | ≥ . (15) d0 t→∞ b) Proof: Lemma 4.2 guarantees that sin α2 > 0 at t = t0 . Afterwards, this inequality is still valid and a sliding motion occurs while the second inequality from (12) is kept true. During this motion, y˙ = −χ (y) for y := d − d0 , where χ (y) > 0 ∀y > 0, χ (y) < 0 ∀y < 0, and χ (0) = 0. It follows that any solution d of the sliding mode differential equation monotonically converges to d0 . This implies that the second inequality from (12) will never be violated. So the sliding mode will never be terminated and d → d0 as t → ∞. So the control u2 coincides with that generated by the controller (2) with arbitrarily small parameter v∗ > 0, v∗ ≈ 0 if t is large enough. Hence (15) follows from (14). • Lemma 4.4: Let the assumptions of Lemma 4.3 hold and v1 v2 R2 , where R2 := . (16) d(0) > 3 1 + π v2 − v1 u2 Then both relations d˙+ χ (d − d0) = 0, sin α2 > 0 become true at some time instant t0 , and for the first such a time, v1 d(t0 ) ≥ d(0) − 3R2 1 + π . (17) v2 − v1 c) Proof: If these relations are true initially, the claim is evident. Otherwise, d˙ + χ (d − d0 ) 6= 0 for t > 0,t ≈ 0, and until the first time instant t0 when the equation becomes true, robot 2 moves with the constant control u2 ≡ ±u2 , v2 ≡ v2 . Let u2 ≡ u2 for the definiteness. We are going to show that if robot 2 is driven by the constant control u2 ≡ u2 , then d˙ > v∗ and d˙ < −v∗ at some time instants t+ and t− . It follows that the equation d˙+ χ (d − d0 ) = 0 becomes true at some time t0 between t− and t+ ; by Lemma 4.2, sin α2 > 0 at t = t0 . We also show that during the time interval with the ends t− and t+ , the distance d is lower bounded by the right hand side of (17), which will complete the proof.
q− := q − R2,
(18)
cos α2 = ±1 ⇔ p = ±d, q− = −R2 and (10) shapes into p˙ = −u2 q− − v1 cos∆, where ∆ := α2 − α1 . q˙− = u2 p − v1 sin ∆, where ∆˙ = u2 − u1
≤0since λ ≥1
γ (v2 + v1 ) + u2 v2 | sin α2 | ≤ λ −1 [u2 − u1 ] − u2 µ + a1 +
q := d sin α2 ,
The polar coordinates ρ , β of col (p, q− ) obey the equations:
ρ˙ β˙
= −v1 cos(β − ∆)
,
= u2 + vρ1 sin(β − ∆)
∆˙ = u2 − u1.
(19)
Let β = − arcsin √ R22
and d > R2 ; we are going to show
q− = R2 + ρ sin β = R2 −
√R22 ρ 2 R2 +d
R2 +d 2
that then cos α2 = 1. Indeed, p = ρ cos β = √ d2ρ
R2 +d 2
, q = R2 +
and
R2 d 2 = p2 + q2 = ρ 2 − 2 q 2 ρ + R22 . 2 2 R2 + d have For d > R2 , the roots ρ1 , ρ2 of this quadratic equation q opposite signs, with the positive root ρ = R22 + d 2 . It follows that p = d, q = 0 ⇒ cos α2 = 1. Similarly, β = π − arcsin √ R22 2 & d > R2 ⇒ cos α2 = −1. R2 +d
Now we note that |ρ − d| ≤ R2 and thanks to (19), ρ˙ ≥ −v1 . Hence ρ > R2 and d > R2 at least until tρ = d| − 3R v−1 t=0 2 . The second equation 1 from(19) shows that until this time, β˙ ≥ u2 − v1 = R−1 v2 − v1 . So inequality R2
2
(16) guarantees that during the time interval [0,tρ ], the angle β continuously runs over an interval whose length exceeds 3π , whereas the values of the continuous functions µ+ (t) := − arcsin √ R22 2 and µ− (t) := π − arcsin √ R22 2 are conR2 +d
R2 +d
fined to [−π /2, 0] and [π /2, π ], respectively. It follows that β (t± ) = µ± (t± ) + 2π j±, j± ∈ Z at some time instants t+ and ˙ + ) > v∗ and d(t ˙ − ) < −v∗ , t− from this interval. Hence d(t and so the equation d˙ + χ (d − d0 ) = 0 does become true at some time instant t0 between t− and t+ . Furthermore, t0 ≤
3 π R2 v2 − v1
and so d(t0 ) ≥ ρ (t0 ) − R2 ≥ ρ (0) − v1t0 − R2 ≥ 3 π R2 v1 d(0) − 2R2 − v1 ≥ d|t=0 − 3R2 1 + π . v2 − v1 v2 − v1 The case where u2 ≡ −u2 is considered likewise. • d) Proof of Theorem 3.1. The convergence d → d0 as t → ∞ is immediate from Lemmas 4.3 and 4.4. Inequality (8) follows from (15) since ψ = −ϕ and the right hand side of (15) is strictly positive. •
8540
FrC16.1 TABLE I S IMULATION PARAMETERS 1 7
Parameter x2 (0) v2 u2 x1 (0) v1 u1
Value [0;0;0]′ 0.5 m/s 1 rad/s [−12m;0; π ]′ 0.15 m/s 0.5 rad/s
6
Comments Pursuer initial posture Pursuer linear velocity Pursuer maximum angular velocity Target initial posture Target linear velocity Target maximum angular velocity
5
4
3
2
1
0
−1
−2
−3
−14
−12
−10
−8
−6
−4
−2
0
V. S IMULATION R ESULTS (a)
To study the performance and effectiveness of the proposed control law, we consider a Dubins-like wheeled robot moving with a constant linear velocity in an obstacle free environment as the pursuer robot. The target is also considered as another wheeled robot moving with a lower linear velocity than that of the pursuer. Simulation parameters are given in Table I. In the first part of the simulation, we consider the case of approaching a stationary target and define d0 = 2m. Given v∗ = δ γ > 0, we choose the parameters v∗ = 0.25, δ = 0.83 and γ = 0.3 satisfying the condition (7). Having applied the proposed strategy, the robot approaches the steady target and goes into a circular trajectory around it such that d(t) → d0 as t → ∞, shown in figures 1(a) and 1(b). The geometry of pursuer motion towards the target during the main part of the trajectory before it goes into a circular trajectory around the target with the guidance law (2) resembles the so-called equiangular spiral. Fig. 1(a) shows the pursuer trajectory while it approaches a target with v∗ = 0.25. As v∗ → (v2 − v1 ) the spiral approaches a straight line which is the optimal trajectory, shown in Fig. 1(b). In the second part of the simulation, the pursuer robot is supposed to follow an unknown maneuvering target. Fig. 2 shows the pursuer trajectory applying the proposed strategy with the same simulation parameters as the case of steady target. We consider v∗ = 0.25. The pursuer follows the target in a circular pattern while preserving the predefined margin d ≃ d0 , shown in Fig. 3.
5
4
3
2
1
0
−1
−2
−3
−4
−5 −14
−12
−10
−8
−6
−4
−2
0
(b)
Fig. 1: Approaching a stationary target (a) with v∗ = 0.25, (b) with v∗ = 0.45 A PPENDIX P ROOF OF L EMMA 4.1 For x := v1 cos α1 ∈ [−v1 , v1 ], the condition from (11) yields y := v2 cos α2 = x + v and so v1 | sin α1 | − v2 | sin α2 | = f (x) :=
q
v21 − x2 −
q
v22 − (x + v)2 . (20)
We observe that the derivative x x+v f ′ (x) = − q +q v21 − x2 v22 − (x + v)2
1v ∈ [−v1 , v1 ] for which vanishes at the point x = v2v−v 1 p 2 2 f (x) = − (v2 − v1 ) − vq. This value is no less than
VI. C ONCLUSION The problem of range-only navigation of a Dubins-like wheeled robot towards a target was considered. A new control law was proposed and the mathematically rigorous proof of convergence and stability of the closed-loop system was presented. The proposed strategy drives the robot to the target and ultimately makes it moving around the target along a circular trajectory. The radius of circle or the preserved margin from the target is predefined with a control parameter. The performance of the proposed method was also confirmed by computer simulations.
max{ f (v1 ); f (−v1 )} = − v22 − (v1 + |v|)2 and does not exp ceed − (v2 − v1 )2 − v2 , which gives the first implication in (11). Similarly q v21 − x2 v1 | sin α1 | = g(x) := gv1 ,v (x) := q . (21) v2 | sin α2 | v2 − (x + v)2 2
Since gv1 ,−v (x) = gv1 ,v (−x) and so maxx∈[−v1 ,v1 ] gv1 ,−v (x) = maxx∈[−v1 ,v1 ] gv1 ,v (x), it suffices to examine v ≥ 0. Then g(−x) ≤ g(x) for x ≥ 0, and so max g(x) over x ∈ [−v1 , v1 ] equals that over x ∈ [0, v1 ]. Hence a grater v gives rise to
8541
FrC16.1 Robot Trajectory
value of gv1 ,v (·) equals r x gv1 ,v (x) = = x+v v q u u (v2 − v2 − v2 ) − v2 − (v1 + v)2 v2 − (v1 − v)2 2 1 2 2 u t q (v22 + v2 − v21 ) − v22 − (v1 + v)2 v22 − (v1 − v)2
4 2 0 −2 −4 −6 −8
⇒
−10 −12
(11).
•
−14
R EFERENCES
−16 −25
−20
−15
−10
−5
0
Fig. 2: Robot trajectory following a maneuvering target 14
12
10
8
6
4
2
0
0
50
100
150
Fig. 3: Pursuer-target range while following a maneuvering target
the grater max, and the ratio from (21) is upper bounded by maxx∈[−v1 ,v1 ] gv1 ,v (x). The change of the variable x = v1 z: max gv1 ,v (x) =
x∈[−v1 ,v1 ]
√ v1 1 − z2
max gv1 ,v (x) = max q z∈[0,1] v22 − (v1 z + v)2
x∈[0,v1 ]
shows that the grater v1 , the grater the max. So the ratio from (21) is upper bounded by maxx∈[−v1 ,v1 ] gv1 ,v (x). Here gv1 ,v (±v1 ) = 0 and the derivative vanishes at the point x where −
x x+v + =0⇔ v21 − x2 v22 − (x + v)2
vx2 − (v22 − v2 − v21 )x + vv21 = 0
(22)
Since |x| should not exceed v1 , we conclude that q (v22 − v2 − v21 ) − v22 − (v1 + v)2 v22 − (v1 − v)2 x= , 2v x+v =
(v22 + v2 − v21 ) −
q v22 − (v1 + v)2 v22 − (v1 − v)2 2v
. (23)
So the first relation from (22) yields that the maximum
[1] A. Arora, P. Dutta, S. Bapat, et al., “A line in the sand: A wireless sensor network for target detection, classification, and tracking,,” Computer Networks (Elsevier), vol. 45, no. 6, pp. 605–634, 2004. [2] K. E. Bekris, A. A. Argyros, and L. E. Kavraki, “Angle-based methods for mobile robot navigation: Reaching the entire plane,” Proceedings of The IEEE International Conference on Robotics and Automation (ICRA), pp. 2373–2378, 2004. [3] A. Bloch and S. Drakunov, “Stabilization of a nonholonomic system via sliding modes,” in Proc. IEEE Int. Conf. Decision Contr., pp. 2961– 2963, Dec. 1994. [4] R. W. Brockett, “Asymptotic stability and feedback stabilization,” Differential Geometric Control Theory (R. W. Brockett, R. S. Millman, and H. J. Sussmann, eds.), pp. 181–191, 1983. [5] R. Crepaldi, A. Harris, A. Scarpa, A. Zanella, and M. Zorzi, “Testbed implementation and refinement of a range-based localization algorithm for wireless sensor networks,” in 3rd IEEE Mobility Conference, Bangkok, Thailand, 2006. [6] B. d’Andrea Novel, G. Bastin, and G. Campion, “Control of nonholonomic wheeled mobile robots by state feedback linearization,” International Journal of Robotics Research, vol. 14, no. 6, pp. 543– 559, 1995. [7] W. E. Dixon, D. M. Dawson, E. Zergeroglu, and A. Behal, Nonlinear Control of Wheeled Mobile Robots. London: Springer-Verlag, 2001. [8] A. Filippov, Differential Equations with Discontinuous Righthand Sides. Kluwer Academic Publishers, 1988. [9] A. Gadre and D. J. Stilwell, “Toward underwater navigation based on range measurements from a single location,” in Proceedings of IEEE International Conference on Robotics and Automation, pp. 4472–4477, 2004. [10] D. Gu and H. Hu, “A stabilizing receding horizon regulator for nonholonomic mobile robots,” IEEE Transactions on Robotics and Automation, vol. 21, no. 5, pp. 1022–1028, October 205. [11] S. G. Loizou and V. Kumar, “Biologically inspired bearing-only navigation and tracking,” Proceedings of the 46th IEEE Conference on Decision and Control, pp. 1386–1391, Dec. 2007. [12] P. N. Pathirana, N. Bulusu, A. V. Savkin, and S. Jha, “Node localization using mobile robots in delay-tolerant sensor networks,” IEEE Trans. on Mobile Computing, vol. 4, no. 4, pp. 285–296, July/August 2005. [13] P. Pathirana, A. Savkin, and S. Jha, “Location estimation and trajectory prediction for cellular networks with mobile base stations,” IEEE Transactions on Vehicular Technology, vol. 53, no. 6, pp. 1903–1913, 2004. [14] D. Soetanto, L. Lapierre, and A. Pascoal, “Adaptive, non-singular pathfollowing control of dynamic wheeled robots,” In Proceedings of the 2003 conference on decision and control, CDC03,, vol. 2, pp. 1765– 1770, 2003. [15] R. Solea and U. Nunes, “Trajectory planning and sliding-mode control based trajectory-tracking for cybercars,” Integr. Comput.-Aided Eng., vol. 14, no. 1, pp. 33–47, 2007. [16] H. Teimoori and A. V. Savkin, “A method for collision-free navigation of a wheeled mobile robot using the range-only information,” in Proceedings of American Control Conference, Seattle, USA, June 2008. [17] B. Xu, S. Pandian, M. Inoue, N. Sakagami, and S. Kawamura, “Modelbased sliding mode control of underwater robot manipulators,” Int. J. Offshore and Polar Engineering, vol. 16, pp. 210–217, 2006. [18] J. M. Yang and J. H. Kim, “Sliding mode control for trajectory tracking of nonholonomic wheeled mobile robots,” IEEE Trans. on Robotics and Automation, vol. 15, no. 3, pp. 578–587, 1999.
8542
FrC16.1
The Problem of Target Following Based on Range-only Measurements For Car-like Robots Alexey S. Matveev, Hamid Teimoori, and Andrey V. Savkin Abstract— In this paper, we consider the problem of navigation and guidance of a Dubins-like wheeled robot towards a steady or maneuvering target with range-only information. We propose a range-based sliding mode controller through which the robot follows a target with a constant speed while preserving a predefined range margin from the target. Mathematically rigorous proof of convergence and stability of the proposed guidance law is presented. Simulation results confirm the applicability and performance of the proposed guidance approach.
I. I NTRODUCTION The problems of target following and trajectory tracking for car-like nonholonomic robots have been widely studied in robotics over the past two decades (see e.g. [6], [10], [14], [18] and references therein). These robots have restrictions in mobility due to the particular nature of nonholonomic systems, i.e. wheels rolling without slipping sideways. Because of the deficiency in controllability of their linear models, these systems are not controllable by linear controllers [4]. As a result, nonlinear controllers have been widely studied for this class of systems in recent years. Several sophisticated approaches to target following and trajectory tracking by wheeled robots have been proposed in the literature. A strategy for regulating a car-like robot using a receding horizon controller was proposed in [10]. Soetanto et al. [14] presented an approach to steering a wheeled robot along a desired path, which is based on the Lyapunov and back-stepping techniques. A sliding mode control law for the problem of trajectory tracking by nonholonomic mobile robots has been proposed in [18]. The control scheme is based on the computed torque method combined with feedback linearization of the system model. Another solution for the trajectory tracking problem based on dynamic feedback linearization is presented in [6]. A comprehensive summary of the available algorithms for motion planning and control of wheeled robots can be found in [7]. Among the current approaches, sliding mode control of car-like robots, due to the fast response, good transient performance and robustness against system uncertainties, has been received an increasing attention from researchers. Xu et al. [17] proposed a model-based sliding mode controller for trajectory tracking by underwater robot manipulators. This work was supported by the Australian Research Council. A.S. Matveev is with the Department of Mathematics and Mechanics, Saint Petersburg University, Universitetskii 28, Petrodvoretz, St.Petersburg, 198504, Russia [email protected] H. Teimoori and A.V. Savkin are with the School of Electrical Engineering and Telecommunications, The University of New South Wales, Sydney, 2052, Australia [email protected],
[email protected]
978-1-4244-3872-3/09/$25.00 ©2009 IEEE
An approach to stabilization of nonholonomic systems by means of sliding mode control was proposed by Bloch and Drakunov [3]. For autonomous vehicles, a sliding mode control strategy for trajectory tracking and path following was presented in [15]. Most of the existing approaches to trajectory tracking and target following by wheeled robots assume that both the line-of-sight angle (bearing) and the relative distance (range) are available for navigation and guidance algorithms. There is also a relatively large body of research on navigation and guidance with only bearing measurements [2], [11]. In contrast, only few results on the problem of navigation and guidance towards an unknown target using range-only measurements have been published. The problem of range-only based localization and navigation is motivated by its applications to e.g. wireless networks, unmanned underwater vehicles, surveillance and emergency services [1], [5], [9], where the only available information about the target is the range between it and the pursuer robot. The pursuer-target range is obtained by measuring e.g., the time-of-flight of an acoustics pulse [9] or the strength of the signal received from the target [12], [13]. Such an approach could potentially entail reduction of the complexity and cost of the control hardware. However, a general algorithm is required by which the pursuer robot can be navigated towards the target based on range-only measurements. In this paper, we consider the problem of range-only navigation of a Dubins-like wheeled robot following a maneuvering target at a predefined distance, with the target maneuver being unknown in advance. We propose a sliding mode control strategy whereby the pursuer robot approaches the target using only measurements of the relative distance between them and follows the target at the range assigned by a control parameter. Theoretical analysis and mathematically rigorous justification of the guidance strategy is presented and its stability and performance is discussed. Its applicability is also confirmed by computer simulations. The proposed strategy is in essence similar to the Equiangular Navigation Guidance (ENG) law initially proposed in [16]. In the specific case of a stationary target, the pursuer robot driven by this strategy moves towards the target along a spiral-like curve resembling the so-called equiangular spiral. The reminder of the paper is organized as follows. Problem description and models of the controlled car-like robot and the target are introduced in Section II. Section III introduces the navigation and guidance law. Mathematically rigorous analysis of the proposed strategy is presented in Section IV. Computer simulation results are given in Section V. Finally,
8537
FrC16.1 Assumption 3.1: Both the initial and required distances between the robots are large enough:
the paper is concluded in Section V. II. S YSTEM DESCRIPTION AND PROBLEM SETUP We consider two planar mobile robots modeled as unicycles. They travel with time-varying speeds and turning radii limited by known constants. The linear acceleration is similarly bounded. One of the robots (say robot 1) plays the role of a target, the objective of the other is to follow the target at the given distance d0 . Unlike many works in the area, the pursuer (robot 2) has access to only the current ˙ distance d(t) to robot 1 and its derivative d(t). It is also aware of the bounds on the speed, acceleration, and turning radius of robot 1. At the same time, it has no access to the actual values of these variables and is unaware of the control rule driving robot 1. The kinematic model of the ith robot (i = 1, 2) is as follows x˙i = vi cos θi , vi ∈ [0, vi ], y˙i = vi sin θi , |v˙i | ≤ ai θ˙i = ui ∈ [−ui , ui ].
(1)
Here col (xi , yi ) is the vector of the robot Cartesian coordinates, θi gives its orientation, vi and ui are the speed and the angular velocity, respectively. The upper thresholds vi , ui , and ai are given. The problem to be considered is as follows. Given two robots, find a controller that asymptotically drives robot 2 to the required distance d(t) → d0 to robot 1, irrespective of what is the motion of the latter, i.e., the functions x1 (·), y1 (·), v1 (·) and u1 (·) satisfying (1). In this paper, we examine the following control algorithm for the pursuer (robot 2):
where
v2 (t) ≡ v2 , ˙ u2 (t) = u2 sgn d(t) + χ [d(t) − d0] , γr χ (r) := v∗ := γδ −v∗
(2)
if |r| ≤ δ if r > δ
(3)
if r < −δ
and γ , δ are controller parameters. We also adopt the following assumption, which means that the pursuer is more maneuverable than the target. Assumption 2.1: The following inequalities hold: u2 > u1 and v2 > v1 + au12 . III. T HE MAIN RESULT It employs the following constant that depends on the speed thresholds v1 , v2 from (1) and the parameter v∗ from (3) and is introduced under the assumption that v∗ + v1 < v2 : v q u u (v2 − v2 − v2 ) − v22 − (v1 + v∗ )2 v22 − (v1 − v∗ )2 ∗ u 2 1 q ω := t . (4) (v22 + v2∗ − v21 ) − v22 − (v1 + v∗ )2 v22 − (v1 − v∗ )2
To achieve the control objective, one more assumption is needed, which will be commented in Remark 3.2.
d 0 > R2 d(0) > R2
u2 (ω + 1)2 λ , u2 − u1
v1 u2 3 + 3π + (ω + 1)2 λ v2 − v1 u2 − u1
(5)
(6)
where R2 := uv22 is the minimal turning radius of robot 2 and λ ≥ 1 is a design parameter such that u1 = 0 ⇒ λ > 1. Now we are in a position to state the main result of the paper. Theorem 3.1: Suppose that Assumptions 2.1 and 3.1 hold, the parameters γ > 0 and v∗ = δ γ > 0 of the controller (2) are chosen so that along with the parameter λ from Assumption 3.1, they obey the inequality: 2 a1 + γ (v1 + v2 ) 2 2 (7) 2 + v∗ < (v2 − v1 ) . u2 − λ −1(u2 − u1 )
Then the controller (2) asymptotically drives robot 2 to the required distance to robot 1, i.e., d(t) → d0 as t → ∞. This is true irrespective of the motion of robot 1. The following complement to Theorem 3.1 illuminates the features of the robots relative motion by showing that the pursuer ultimately circulates around the target, with the angular velocity of circulation exceeding a certain lower threshold. To rigorously state this claim, we introduce the moving normally oriented Cartesian coordinate system centered at the characteristic point of the target (robot 1). The axes of the system keep their directions unchanged. Let d and ψ stand for the polar coordinates of the pursuer (robot 2) in this system. In other words, d is the distance between the robots and ψ is the algebraic angle from the x-axis to the vector from the target to the pursuer. Proposition 3.1: Suppose that the assumptions of Theorem 3.1 hold. Then irrespective of the motion of robot 1, either v2 − v1 >0 (8) lim ψ˙ (t) ≥ d0 t→∞ or v2 − v1 lim ψ˙ (t) ≤ − < 0. (9) t→∞ d0 Remark 3.1: i) By Theorem 3.1 and Proposition 3.1, d(t) ≈ d0 ∀t ≥ T and either ψ˙ (t) ≥ ζ ∀t ≥ T or ψ˙ (t) ≤ −ζ ∀t ≥ T for any ζ < d0−1 (v2 − v1 ), ζ ≈ d0−1 (v2 − v1 ) provided that T is large enough. This means that in the above coordinate system centered at the target, the pursuer asymptotically moves around the origin at the required distance d0 at the speed v2 ≥ v2 − v1 > 0. If the target is motionless v1 ≡ 0, this system does not move and the pursuer asymptotically moves at the speed v2 along the circle with the radius d0 centered at the target. ii) The controller (2) exhibits a sliding motion for robot 2. Theorem 3.1 and Proposition 3.1 address the equivalent dynamics [8]. iii) For given λ , inequality (7) describes the interior of an ellipse in the plane of the controller parameters (v∗ , γ ). This ellipse expands as λ ↑; in the marginal cases λ = 1 and λ → ∞, the denominator in (7) converts into u21
8538
FrC16.1 and u22 , respectively. Thanks to the second inequality in Assumption 2.1, this interior intersects the positive octant γ > 0, v∗ > 0 for large λ . So the controller can be tuned so that (7) is satisfied. We also note that (7) implies the condition v∗ + v1 < v2 under which the quantity (4) is well defined. iv) It can be shown that the quantity (4) increases as v∗ ↑. Hence decreasing v∗ improves the lower estimate (6) of the attraction domain, whereas the stability condition (7) does not limit v∗ from below. At the same time, decreasing v∗ may have a detrimental effect on the transient performance. v) By the previous remark, the quantity (ω + 1)2 λ in (5) and (6) increases as v∗ ↑ and λ ↑. Along with (7), this means that the larger the initial and required distances, the more freedom is in the choice of the controller parameters. The proof of Theorem 3.1 is given in Section IV.
IV. P ROOF OF T HEOREM 3.1 We introduce the normally oriented Cartesian coordinate system centered at the characteristic point of robot 1 with the x-axis directed towards robot 2. Let αi denote the algebraic angle between the speed vector of the ith robot and the xaxis. Then the motion equations (1) take the form: d˙ = v2 cos α2 − v1 cos α1 α˙ 1 = u1 − ϕ˙ , α˙ 2 = u2 − ϕ˙ .
ϕ˙ =
We start with technical estimates of expressions encountered here. Their proof is given in Appendix. Lemma 4.1: Whenever |v| ≤ v < v2 − v1 and 0 ≤ v1 ≤ v1 , the following implications hold v2 cos α2 − v1 cos α1 = v ⇒ v | sin α | ≤ v | sin α | − p(v − v )2 − v2 , 1 1 2 2 2 1 v1 | sin α1 | ≤ ω v2 | sin α2 |
A. The steady target By specifying Theorem 3.1 and Proposition 3.1 in the case of the steady target, we arrive at the following. Corollary 3.1: Let the target do not move v1 = 0, u1 = 0, a1 = 0 and d 0 > λ R2 ,
d(0) > [λ + 3]R2 ,
where λ > 1.
Suppose also that the parameters γ > 0 and v∗ = δ γ > 0 of the controller (2) obey the inequality: R22 1 − λ −1
2 2 2 2 γ + v∗ < v2 .
(11)
where ω is given by (4) with v∗ := v. The next lemma reveals conditions under which a discontinuous control law (2) exhibits a sliding motion. Lemma 4.2: For robot 2 driven by the control rule (2), the surface d˙ + χ (d − d0 ) = 0 is sliding in the domain sin α2 > 0 and d > d∗ := R2
Then the controller (2) drives robot 2 to the required distance to robot 1, i.e., d → d0 as t → ∞. Asymptotically, the pursuer circulates around the target at the required distance d0 so that the vector from the target to the pursuer rotates at the angular t→∞ velocity w(t) −−→ w∞ , where |w∞ | = d0−1 v2 . Remark 3.2: i) The controller of the form (2) cannot ensure global convergence d → d0 even for the steady target. For any controller parameters, there is a nonempty domain in the phase space such that whenever robot 2 enters this domain, afterwards it moves with the constant control u2 ≡ ±u2 along a circle of the radius R2 . Then the distance d oscillates and does not converge to d0 . Thus requirements to the robots initial and required states (like (6) and (5)) are unavoidable. ii) In the case of the steady target, a comprehensive analysis of the behavior exhibited by the controller (2) for all possible parameter values can be carried out. It shows that as the parameter v∗ exceeds a certain threshold, the above domain of non-convergence d 6→ d0 becomes infinite: whenever the robots are initially far enough, the convergence does not hold. As v∗ exceeds another and larger threshold, the convergence does not hold for almost all initial states. In the light of these facts, imposing upper bounds on the controller parameters by the key condition (7) seems reasonable.
(10)
v2 sin α2 −v1 sin α1 d
u2 (ω + 1)2 λ u2 − u1
(12)
and two-side repelling in the domain sin α2 < 0 and d > d∗
(13)
whenever (7) holds. Here ω is given by (4). On this surface, p (v2 − v1 )2 − v2∗ > 0, v2 p (v2 − v1 )2 − v2∗ |ϕ˙ | ≥ . (14) d a) Proof: We put v(t) := −χ (d − d0 ) and note that |v(t)| ≤ v∗ and |v| ˙ ≤ γ (v2 + v1 ) by (2). Hence if d ≥ d∗ ,
8539
| sin α2 | ≥
d˙ (10) d + χ (d − d0 ) = d¨ − v˙ == dt − [v2 (u2 − ϕ˙ ) sin α2 − v1 (u1 − ϕ˙ ) sin α1 ] − v˙1 cos α1 − v˙ b
{ v2 sin α2 − v1 sin α1 == + v1u1 sin α1 − v˙1 cos α1 − v˙ d − v2 u2 sin α2 ; 2 v2 | sin α2 | + v1| sin α1 | |b| ≤ + d∗ v1 u1 | sin α1 | + a1 + γ (v2 + v1 ). (10)
z
2
}|
FrC16.1 ˙ If pd + χ (d − d0) = 0, Lemma 4.1 yields that v2 | sin α2 | ≥ µ := (v2 − v1 )2 − v2∗ and v2 (ω + 1)2 |b| ≤ + u1 v2 | sin α2 |−u1 µ +a1 + γ (v2 +v1 ) d∗ (12) == λ −1 [u2 − u1 ] + u1 v2 | sin α2 | − u1 µ + a1 + γ (v2 + v1 ) = λ −1 [u2 − u1 ] + u1 − u2 v2 | sin α2 | − u1 µ + a1+ {z } |
(10) We note that cos α2 = 1 ⇒ d˙ = v2 − v1 cos α1 ≥ v2 − v1 > ˙ v∗ and cos α2 = −1 ⇒ d < −v∗ . In terms of the variables
p := d cos α2 ,
(7)
γ (v2 + v1 ) + u2 v2 | sin α2 | ≤ u2 v2 | sin α2 | − ε ,
ε > 0 is small enough. Hence the signs taken by where d ˙ d + χ (d − d0 ) for u2 = u2 and u2 = −u2 , respectively, dt are opposite. If sin α2 > 0, this sign is opposite to sgn u2 ; if sin α2 < 0, the signs are equal. This implies the first conclusion of the lemma. Inequalities (14) follow from (10) and the first relation in (11). • Lemma 4.3: Let robot 2 be driven by the control rule (2), and inequalities (5) and (7) hold. If the equation d˙ + χ (d − d0 ) = 0 becomes true at some time instant t0 when the second inequality from (12) is satisfied, then d → d0 as t → ∞ and v2 − v1 lim |ϕ˙ | ≥ . (15) d0 t→∞ b) Proof: Lemma 4.2 guarantees that sin α2 > 0 at t = t0 . Afterwards, this inequality is still valid and a sliding motion occurs while the second inequality from (12) is kept true. During this motion, y˙ = −χ (y) for y := d − d0 , where χ (y) > 0 ∀y > 0, χ (y) < 0 ∀y < 0, and χ (0) = 0. It follows that any solution d of the sliding mode differential equation monotonically converges to d0 . This implies that the second inequality from (12) will never be violated. So the sliding mode will never be terminated and d → d0 as t → ∞. So the control u2 coincides with that generated by the controller (2) with arbitrarily small parameter v∗ > 0, v∗ ≈ 0 if t is large enough. Hence (15) follows from (14). • Lemma 4.4: Let the assumptions of Lemma 4.3 hold and v1 v2 R2 , where R2 := . (16) d(0) > 3 1 + π v2 − v1 u2 Then both relations d˙+ χ (d − d0) = 0, sin α2 > 0 become true at some time instant t0 , and for the first such a time, v1 d(t0 ) ≥ d(0) − 3R2 1 + π . (17) v2 − v1 c) Proof: If these relations are true initially, the claim is evident. Otherwise, d˙ + χ (d − d0 ) 6= 0 for t > 0,t ≈ 0, and until the first time instant t0 when the equation becomes true, robot 2 moves with the constant control u2 ≡ ±u2 , v2 ≡ v2 . Let u2 ≡ u2 for the definiteness. We are going to show that if robot 2 is driven by the constant control u2 ≡ u2 , then d˙ > v∗ and d˙ < −v∗ at some time instants t+ and t− . It follows that the equation d˙+ χ (d − d0 ) = 0 becomes true at some time t0 between t− and t+ ; by Lemma 4.2, sin α2 > 0 at t = t0 . We also show that during the time interval with the ends t− and t+ , the distance d is lower bounded by the right hand side of (17), which will complete the proof.
q− := q − R2,
(18)
cos α2 = ±1 ⇔ p = ±d, q− = −R2 and (10) shapes into p˙ = −u2 q− − v1 cos∆, where ∆ := α2 − α1 . q˙− = u2 p − v1 sin ∆, where ∆˙ = u2 − u1
≤0since λ ≥1
γ (v2 + v1 ) + u2 v2 | sin α2 | ≤ λ −1 [u2 − u1 ] − u2 µ + a1 +
q := d sin α2 ,
The polar coordinates ρ , β of col (p, q− ) obey the equations:
ρ˙ β˙
= −v1 cos(β − ∆)
,
= u2 + vρ1 sin(β − ∆)
∆˙ = u2 − u1.
(19)
Let β = − arcsin √ R22
and d > R2 ; we are going to show
q− = R2 + ρ sin β = R2 −
√R22 ρ 2 R2 +d
R2 +d 2
that then cos α2 = 1. Indeed, p = ρ cos β = √ d2ρ
R2 +d 2
, q = R2 +
and
R2 d 2 = p2 + q2 = ρ 2 − 2 q 2 ρ + R22 . 2 2 R2 + d have For d > R2 , the roots ρ1 , ρ2 of this quadratic equation q opposite signs, with the positive root ρ = R22 + d 2 . It follows that p = d, q = 0 ⇒ cos α2 = 1. Similarly, β = π − arcsin √ R22 2 & d > R2 ⇒ cos α2 = −1. R2 +d
Now we note that |ρ − d| ≤ R2 and thanks to (19), ρ˙ ≥ −v1 . Hence ρ > R2 and d > R2 at least until tρ = d| − 3R v−1 t=0 2 . The second equation 1 from(19) shows that until this time, β˙ ≥ u2 − v1 = R−1 v2 − v1 . So inequality R2
2
(16) guarantees that during the time interval [0,tρ ], the angle β continuously runs over an interval whose length exceeds 3π , whereas the values of the continuous functions µ+ (t) := − arcsin √ R22 2 and µ− (t) := π − arcsin √ R22 2 are conR2 +d
R2 +d
fined to [−π /2, 0] and [π /2, π ], respectively. It follows that β (t± ) = µ± (t± ) + 2π j±, j± ∈ Z at some time instants t+ and ˙ + ) > v∗ and d(t ˙ − ) < −v∗ , t− from this interval. Hence d(t and so the equation d˙ + χ (d − d0 ) = 0 does become true at some time instant t0 between t− and t+ . Furthermore, t0 ≤
3 π R2 v2 − v1
and so d(t0 ) ≥ ρ (t0 ) − R2 ≥ ρ (0) − v1t0 − R2 ≥ 3 π R2 v1 d(0) − 2R2 − v1 ≥ d|t=0 − 3R2 1 + π . v2 − v1 v2 − v1 The case where u2 ≡ −u2 is considered likewise. • d) Proof of Theorem 3.1. The convergence d → d0 as t → ∞ is immediate from Lemmas 4.3 and 4.4. Inequality (8) follows from (15) since ψ = −ϕ and the right hand side of (15) is strictly positive. •
8540
FrC16.1 TABLE I S IMULATION PARAMETERS 1 7
Parameter x2 (0) v2 u2 x1 (0) v1 u1
Value [0;0;0]′ 0.5 m/s 1 rad/s [−12m;0; π ]′ 0.15 m/s 0.5 rad/s
6
Comments Pursuer initial posture Pursuer linear velocity Pursuer maximum angular velocity Target initial posture Target linear velocity Target maximum angular velocity
5
4
3
2
1
0
−1
−2
−3
−14
−12
−10
−8
−6
−4
−2
0
V. S IMULATION R ESULTS (a)
To study the performance and effectiveness of the proposed control law, we consider a Dubins-like wheeled robot moving with a constant linear velocity in an obstacle free environment as the pursuer robot. The target is also considered as another wheeled robot moving with a lower linear velocity than that of the pursuer. Simulation parameters are given in Table I. In the first part of the simulation, we consider the case of approaching a stationary target and define d0 = 2m. Given v∗ = δ γ > 0, we choose the parameters v∗ = 0.25, δ = 0.83 and γ = 0.3 satisfying the condition (7). Having applied the proposed strategy, the robot approaches the steady target and goes into a circular trajectory around it such that d(t) → d0 as t → ∞, shown in figures 1(a) and 1(b). The geometry of pursuer motion towards the target during the main part of the trajectory before it goes into a circular trajectory around the target with the guidance law (2) resembles the so-called equiangular spiral. Fig. 1(a) shows the pursuer trajectory while it approaches a target with v∗ = 0.25. As v∗ → (v2 − v1 ) the spiral approaches a straight line which is the optimal trajectory, shown in Fig. 1(b). In the second part of the simulation, the pursuer robot is supposed to follow an unknown maneuvering target. Fig. 2 shows the pursuer trajectory applying the proposed strategy with the same simulation parameters as the case of steady target. We consider v∗ = 0.25. The pursuer follows the target in a circular pattern while preserving the predefined margin d ≃ d0 , shown in Fig. 3.
5
4
3
2
1
0
−1
−2
−3
−4
−5 −14
−12
−10
−8
−6
−4
−2
0
(b)
Fig. 1: Approaching a stationary target (a) with v∗ = 0.25, (b) with v∗ = 0.45 A PPENDIX P ROOF OF L EMMA 4.1 For x := v1 cos α1 ∈ [−v1 , v1 ], the condition from (11) yields y := v2 cos α2 = x + v and so v1 | sin α1 | − v2 | sin α2 | = f (x) :=
q
v21 − x2 −
q
v22 − (x + v)2 . (20)
We observe that the derivative x x+v f ′ (x) = − q +q v21 − x2 v22 − (x + v)2
1v ∈ [−v1 , v1 ] for which vanishes at the point x = v2v−v 1 p 2 2 f (x) = − (v2 − v1 ) − vq. This value is no less than
VI. C ONCLUSION The problem of range-only navigation of a Dubins-like wheeled robot towards a target was considered. A new control law was proposed and the mathematically rigorous proof of convergence and stability of the closed-loop system was presented. The proposed strategy drives the robot to the target and ultimately makes it moving around the target along a circular trajectory. The radius of circle or the preserved margin from the target is predefined with a control parameter. The performance of the proposed method was also confirmed by computer simulations.
max{ f (v1 ); f (−v1 )} = − v22 − (v1 + |v|)2 and does not exp ceed − (v2 − v1 )2 − v2 , which gives the first implication in (11). Similarly q v21 − x2 v1 | sin α1 | = g(x) := gv1 ,v (x) := q . (21) v2 | sin α2 | v2 − (x + v)2 2
Since gv1 ,−v (x) = gv1 ,v (−x) and so maxx∈[−v1 ,v1 ] gv1 ,−v (x) = maxx∈[−v1 ,v1 ] gv1 ,v (x), it suffices to examine v ≥ 0. Then g(−x) ≤ g(x) for x ≥ 0, and so max g(x) over x ∈ [−v1 , v1 ] equals that over x ∈ [0, v1 ]. Hence a grater v gives rise to
8541
FrC16.1 Robot Trajectory
value of gv1 ,v (·) equals r x gv1 ,v (x) = = x+v v q u u (v2 − v2 − v2 ) − v2 − (v1 + v)2 v2 − (v1 − v)2 2 1 2 2 u t q (v22 + v2 − v21 ) − v22 − (v1 + v)2 v22 − (v1 − v)2
4 2 0 −2 −4 −6 −8
⇒
−10 −12
(11).
•
−14
R EFERENCES
−16 −25
−20
−15
−10
−5
0
Fig. 2: Robot trajectory following a maneuvering target 14
12
10
8
6
4
2
0
0
50
100
150
Fig. 3: Pursuer-target range while following a maneuvering target
the grater max, and the ratio from (21) is upper bounded by maxx∈[−v1 ,v1 ] gv1 ,v (x). The change of the variable x = v1 z: max gv1 ,v (x) =
x∈[−v1 ,v1 ]
√ v1 1 − z2
max gv1 ,v (x) = max q z∈[0,1] v22 − (v1 z + v)2
x∈[0,v1 ]
shows that the grater v1 , the grater the max. So the ratio from (21) is upper bounded by maxx∈[−v1 ,v1 ] gv1 ,v (x). Here gv1 ,v (±v1 ) = 0 and the derivative vanishes at the point x where −
x x+v + =0⇔ v21 − x2 v22 − (x + v)2
vx2 − (v22 − v2 − v21 )x + vv21 = 0
(22)
Since |x| should not exceed v1 , we conclude that q (v22 − v2 − v21 ) − v22 − (v1 + v)2 v22 − (v1 − v)2 x= , 2v x+v =
(v22 + v2 − v21 ) −
q v22 − (v1 + v)2 v22 − (v1 − v)2 2v
. (23)
So the first relation from (22) yields that the maximum
[1] A. Arora, P. Dutta, S. Bapat, et al., “A line in the sand: A wireless sensor network for target detection, classification, and tracking,,” Computer Networks (Elsevier), vol. 45, no. 6, pp. 605–634, 2004. [2] K. E. Bekris, A. A. Argyros, and L. E. Kavraki, “Angle-based methods for mobile robot navigation: Reaching the entire plane,” Proceedings of The IEEE International Conference on Robotics and Automation (ICRA), pp. 2373–2378, 2004. [3] A. Bloch and S. Drakunov, “Stabilization of a nonholonomic system via sliding modes,” in Proc. IEEE Int. Conf. Decision Contr., pp. 2961– 2963, Dec. 1994. [4] R. W. Brockett, “Asymptotic stability and feedback stabilization,” Differential Geometric Control Theory (R. W. Brockett, R. S. Millman, and H. J. Sussmann, eds.), pp. 181–191, 1983. [5] R. Crepaldi, A. Harris, A. Scarpa, A. Zanella, and M. Zorzi, “Testbed implementation and refinement of a range-based localization algorithm for wireless sensor networks,” in 3rd IEEE Mobility Conference, Bangkok, Thailand, 2006. [6] B. d’Andrea Novel, G. Bastin, and G. Campion, “Control of nonholonomic wheeled mobile robots by state feedback linearization,” International Journal of Robotics Research, vol. 14, no. 6, pp. 543– 559, 1995. [7] W. E. Dixon, D. M. Dawson, E. Zergeroglu, and A. Behal, Nonlinear Control of Wheeled Mobile Robots. London: Springer-Verlag, 2001. [8] A. Filippov, Differential Equations with Discontinuous Righthand Sides. Kluwer Academic Publishers, 1988. [9] A. Gadre and D. J. Stilwell, “Toward underwater navigation based on range measurements from a single location,” in Proceedings of IEEE International Conference on Robotics and Automation, pp. 4472–4477, 2004. [10] D. Gu and H. Hu, “A stabilizing receding horizon regulator for nonholonomic mobile robots,” IEEE Transactions on Robotics and Automation, vol. 21, no. 5, pp. 1022–1028, October 205. [11] S. G. Loizou and V. Kumar, “Biologically inspired bearing-only navigation and tracking,” Proceedings of the 46th IEEE Conference on Decision and Control, pp. 1386–1391, Dec. 2007. [12] P. N. Pathirana, N. Bulusu, A. V. Savkin, and S. Jha, “Node localization using mobile robots in delay-tolerant sensor networks,” IEEE Trans. on Mobile Computing, vol. 4, no. 4, pp. 285–296, July/August 2005. [13] P. Pathirana, A. Savkin, and S. Jha, “Location estimation and trajectory prediction for cellular networks with mobile base stations,” IEEE Transactions on Vehicular Technology, vol. 53, no. 6, pp. 1903–1913, 2004. [14] D. Soetanto, L. Lapierre, and A. Pascoal, “Adaptive, non-singular pathfollowing control of dynamic wheeled robots,” In Proceedings of the 2003 conference on decision and control, CDC03,, vol. 2, pp. 1765– 1770, 2003. [15] R. Solea and U. Nunes, “Trajectory planning and sliding-mode control based trajectory-tracking for cybercars,” Integr. Comput.-Aided Eng., vol. 14, no. 1, pp. 33–47, 2007. [16] H. Teimoori and A. V. Savkin, “A method for collision-free navigation of a wheeled mobile robot using the range-only information,” in Proceedings of American Control Conference, Seattle, USA, June 2008. [17] B. Xu, S. Pandian, M. Inoue, N. Sakagami, and S. Kawamura, “Modelbased sliding mode control of underwater robot manipulators,” Int. J. Offshore and Polar Engineering, vol. 16, pp. 210–217, 2006. [18] J. M. Yang and J. H. Kim, “Sliding mode control for trajectory tracking of nonholonomic wheeled mobile robots,” IEEE Trans. on Robotics and Automation, vol. 15, no. 3, pp. 578–587, 1999.
8542
