- Email: [email protected]
Discussion on: “Trajectory Tracking for aWheeled Mobile Robot Using a Vision Based Positioning System and an Attitude Observer”
European Journal of Control (2012)4:356–362 © 2012 EUCA
Discussion on: “Trajectory Tracking for a Wheeled Mobile Robot Using a Vision Based Positioning System and an Attitude Observer”g Daniele Carnevale∗ , Francesco Martinelli∗∗ Dipartimento di Informatica Sistemi e Produzione, University of Rome “Tor Vergata”, Rome, Italy
The authors of the paper consider a tracking problem for a wheeled mobile robot assuming the possibility of measuring its Cartesian coordinates. These coordinates are fed to a dynamic full information controller together with the orientation of the robot which is estimated through an attitude observer. Theoretical convergence results are proved (under the assumption of perfect measurements) and experimental tests are given in the paper to illustrate the effectiveness of the proposed approach. The observer defined in Proposition 1 belongs to the class of the Immersion and Invariance (I&I) observers [1], [2] and has been designed to obtain asymptotic convergence of the robot angle θ (the estimated quantities are (cos(θ ), sin(θ ))) as stated in Proposition 1, where it is assumed that the robot longitudinal velocity u1 (t) is such that there exist a set T {tk ∈ R ≥ 0 : u1 (tk ) = 0} that is either finite or infinite but countable. Note this assumption implies that the convergence of the estimation error ε(t) is nonuniform. In practice, where usually the robots have zero longitudinal velocity u1 (t) = 0 for compact time intervals yielding a non countable set T ⊂ R, it would be possible to directly rely on the inequality V˙ = −2u1 arctan (u1 )V
(1)
in the proof of Proposition 1, that yields asymptotic convergence of ε(t) to zero as t goes to infinity if lim
t
t→∞ t 0
u1 (τ ) arctan (u1 (τ ))dτ = ∞,
∗ E-mail: [email protected] ∗∗ E-mail: [email protected]
(2)
i.e., u1 (t) arctan (u1 (t)) ∈ / L1 (u1 ≥ 0). Then, if (2) holds, the observer in Proposition 1 guarantees nonuniform convergence to zero of the estimation error ε(t). The stability proof of the output feedback scheme when observers with nonuniform convergence of the estimation error are considered is quite complex. It would be interesting to see if the results [3], [4] would apply to this specific case. Furthermore, it would be interesting to compare the effectiveness of the proposed control law with the general switching controllers in [5], or the sampled data observer in [6]. In the remaining of this discussion we try to focus on the principal issues that may arise in the practical implementation of the proposed tracking algorithm: several factors in fact have to be taken into account when a feedback control law is implemented in a real context. First of all, since the control law is formulated in a continuous time domain, a discrete time approximation must be proposed with a time step mainly constrained by the computational limits and the maximum available rate of measurements [7], [8]. A second important issue is the presence of noise and disturbances in the dynamics and the measurements. From this point of view, it is of particular interest to investigate around the robustness properties of the considered feedback controller: we will perform a numerical analysis by comparing the approach proposed in the paper with a control scheme where the observer is replaced by an Extended Kalman Filter (EKF). The EKF is a standard tool for fusing proprioceptive and exteroceptive measurements in order to estimate the pose of a mobile robot (see e.g., [9] for an application of the EKF in a scenario where the Cartesian coordinates of the robot are measured through a RFID system).
357
Discussions on: “Observer Based Trajectory Tracking for a WMR”
Fig. 2. Noise free system: EKF based control (left) and observer based control (right).
Fig. 1. The desired trajectory (left) and the angular velocity ω along it (right).
To perform this comparison we first introduce a discrete time approximation of the dynamics of the mobile robot (Euler approximation of equations (2) in the paper): x1 (k + 1) = x1 (k) + u1 (k)cos(θ (k)) Ts
(3)
x2 (k + 1) = x2 (k) + u1 (k)sin(θ (k)) Ts
(4)
θ (k + 1) = θ (k) + (u2 (k) + nu2 (k)) Ts
(5)
u1 (k + 1) = u1 (k) + (v1 (k) + nv1 (k)) Ts
(6)
where nu2 ∼ N (0, σu22 ) and nv1 ∼ N (0, σv21 ) are normal random variables characterizing the uncertainties in the actuators of the robot. The discretization step in our simulations is Ts = 6 · 10−4 s. Similarly, the position measurements are corrupted by Gaussian noises: z1 (k) = x1 (k) + n1 (k)
(7)
z2 (k) = x2 (k) + n2 (k)
(8)
being n1 ∼ N (0, σ 2 ) and n2 ∼ N (0, σ 2 ) two normal random variables. Similarly to the paper, we consider a reference trajectory where the robot must follow back and forth an arc of circle and, in the corner (i.e., where there is an inversion of motion), the longitudinal velocity tends to zero. We report in Fig. 1 the reference trajectory (x1d , x2d ) considered in all the simulations: the robot should start from point (2, 0) moving counterclockwise along the arc of√circle until the point (2 cos(π/3), 2 sin(π/3)) = (1, 3). Then it must go back to the initial position. The initial position of the robot is (2.2, 0.1) with an initial orientation of 80◦ . The parameters of the control algorithm and of the observer are like in the paper. The EKF estimates
all the state variables, i.e., x1 , x2 , θ and u1 . The observer proposed in the paper only estimates θ using for x1 and x2 the measurements of these variables. Also u1 is assumed known in the paper. In the noisy implementation of the observer based controller, we assume that the initial value of u1 (which is zero) is known (i.e., uˆ 1 (0) = 0) and that the subsequent values are derived through integration: uˆ 1 (k + 1) = uˆ 1 (k) + v1 (k) Ts , being v1 the acceleration input (notice that the actual velocity evolves according to u1 (k + 1) = u1 (k) + (v1 (k) + nv1 (k)) Ts ). Alternatively, one may assume that the longitudinal velocity is available through a speedometer: in this case a third measurement is available to the EKF (and the observer based controller may directly use it) but clearly an additional sensor is required. Moreover, the results obtained using a speedometer are not significantly different from the ones reported in this discussion. If all the noises are negligible (i.e., σ ≈ 0, σu2 ≈ 0 and σv1 ≈ 0), both the EKF based controller and the one based on the observer described in the paper provide a satisfactory performance (see Fig. 2). As noticed in the paper, the estimate of the orientation appears frozen when the longitudinal speed of the robot approaches zero (where the desired trajectory presents an abrupt change of direction of π radiants). This drawback does not afflict the EKF based approach, even if there is no theoretical guarantee of convergence in this case. A comparison between the tracking errors e(k) =
(x1 (k) − x1d (k))2 + (x2 (k) − x2d (k))2
of the two approaches in the case of negligible noises is reported in Fig. 3. The performance of the observer based controller degrades if we consider a noise in the measurement of x1 and x2 . For example, if we have a measurement noise with standard deviation σ = 0.01 m, the obtained trajectories are reported in Fig. 4: it is apparent how the EKF
358
Discussions on: “Observer Based Trajectory Tracking for a WMR”
Fig. 3. Noise free system: tracking error of the two approaches. Fig. 5. Average tracking error as a function of the standard deviation σ of the noise in the measurements of x1 and x2 .
Fig. 4. Noise in the measurements (σ = 0.01 m): the EKF based control (left) and the observer based control (right).
based approach is not significantly perturbed by the noise unlike the observer based controller. A plot of the average tracking error: J=
T 1 e(k) T k=1
(where T is the duration of the simulation) corresponding to the two approaches is reported in Fig. 5 as a function of the noise intensity. Each value is the average of 100 independent simulation runs, where the average is computed only among the non failing runs (a run is considered failing if J > 0.2 m). The fraction of failures for the two methods is reported in Fig. 6: the plot of the error of the observer based approach (in Fig. 5) is incomplete for large noise levels since, for these values of noise, this approach fails to track the trajectory in all the 100 simulation runs as seen in Fig. 6 (actually, for σ > 0.015 m, the average tracking error of the observer based approach is not simply larger than 0.2 m but becomes unbounded, denoting an unstable behavior).
Fig. 6. Fraction of failures as a function of the standard deviation σ of the noise in the measurements of x1 and x2 .
The situation is different if we consider the noise in the actuators: in this case the observer based controller seems to remain effective and, if the noise acts on the longitudinal velocity actuation (i.e., on the acceleration v1 ), it also appears more reliable than the EKF based controller. In Fig. 7 we report the average tracking error of the two approaches as a function of the standard deviation σu2 which ranges in the interval (0, 0.2). Notice at this regard that a standard deviation of 0.2 rad/s is quite intensive, given that u2 , apart from the values corresponding to the abrupt change in the direction on the corner of the trajectory, normally ranges in the interval (−0.5, 0.5). Nevertheless the effect on the two approaches seems negligible: the tracking error remains really close to the noise free value and the fraction of failures in this case is 0 for both methods. The EKF based approach appears less reliable when the noise acts on the longitudinal velocity actuation. In
359
Discussions on: “Observer Based Trajectory Tracking for a WMR”
Fig. 7. Average tracking error as a function of the standard deviation σu2 of the noise in the steering actuation u2 .
Fig. 9. Fraction of failures as a function of the standard deviation σv1 of the noise in the longitudinal velocity actuation v1 .
References
Fig. 8. Average tracking error as a function of the standard deviation σv1 of the noise in the longitudinal velocity actuation v1 .
Fig. 8 we report the average tracking error of the two approaches as a function of the standard deviation σv1 which ranges in the interval (0, 0.2). Also in this case a standard deviation of 0.2 m/s2 is quite intensive, given that v1 ranges in the interval (−3, 5) with an average absolute value of about 0.7 m/s2 . The performance of the two methods appears satisfactory even if the probability of failure for the EKF approach is not negligible in this case (see Fig. 9).
1. Astolfi A, Karagiannis D, Ortega R. Nonlinear and Adaptive Control with Applications, Springer-Verlag, 2008. 2. Karagiannis D, Carnevale D, Astolfi A. Invariant manifold based reduced-order observer design for nonlinear systems. IEEE Trans Autom Control, 2008; 53(11): 2602–2614. 3. Praly L, Arcak M. A relaxed condition for stability of nonlinear observer-based controllers. Syst Control Lett, 2004; 53(3–4): 311–320. 4. Carnevale D, Karagiannis D, Astolfi A. A condition for certainty equivalence output feedback stabilization of nonlinear systems. IEEE Trans Autom Control, 2010; 55(5): 1180–1185. 5. Casagrande D, Astolfi A, Parisini T. Global asymptotic stabilization of the attitude and the angular rates of an underactuated non-symmetric rigid body. Automatica, 2008; 44(7): 1781–1789. Available Online: http://www. sciencedirect.com/science/article/pii/S0005109807004931. 6. Arcak M, Neši´c D. A framework for nonlinear sampleddata observer design via approximate discrete time models an emulation. Automatica, 2004; 40: 1931–1938. 7. Laila D, Neši´c D, Astolfi A. 3 Sampled-Data Control of Nonlinear Systems, Springer, Berlin, 2006. 8. Neši´c D, Teel AR, Carnevale D. Explicit computation of the sampling period in emulation of controllers for nonlinear sampled-data systems. IEEE Trans Autom Control, 2009; 54(3): 619–624. 9. DiGiampaolo E, Martinelli F. A passive UHF-RFID system for the localization of an indoor autonomous vehicle. IEEE Trans Ind Electron, 2012; 59(10): 3961–3970.
Discussion on: “Trajectory Tracking for a Wheeled Mobile Robot Using a Vision Based Positioning System and an Attitude Observer”g Daniele Carnevale∗ , Francesco Martinelli∗∗ Dipartimento di Informatica Sistemi e Produzione, University of Rome “Tor Vergata”, Rome, Italy
The authors of the paper consider a tracking problem for a wheeled mobile robot assuming the possibility of measuring its Cartesian coordinates. These coordinates are fed to a dynamic full information controller together with the orientation of the robot which is estimated through an attitude observer. Theoretical convergence results are proved (under the assumption of perfect measurements) and experimental tests are given in the paper to illustrate the effectiveness of the proposed approach. The observer defined in Proposition 1 belongs to the class of the Immersion and Invariance (I&I) observers [1], [2] and has been designed to obtain asymptotic convergence of the robot angle θ (the estimated quantities are (cos(θ ), sin(θ ))) as stated in Proposition 1, where it is assumed that the robot longitudinal velocity u1 (t) is such that there exist a set T {tk ∈ R ≥ 0 : u1 (tk ) = 0} that is either finite or infinite but countable. Note this assumption implies that the convergence of the estimation error ε(t) is nonuniform. In practice, where usually the robots have zero longitudinal velocity u1 (t) = 0 for compact time intervals yielding a non countable set T ⊂ R, it would be possible to directly rely on the inequality V˙ = −2u1 arctan (u1 )V
(1)
in the proof of Proposition 1, that yields asymptotic convergence of ε(t) to zero as t goes to infinity if lim
t
t→∞ t 0
u1 (τ ) arctan (u1 (τ ))dτ = ∞,
∗ E-mail: [email protected] ∗∗ E-mail: [email protected]
(2)
i.e., u1 (t) arctan (u1 (t)) ∈ / L1 (u1 ≥ 0). Then, if (2) holds, the observer in Proposition 1 guarantees nonuniform convergence to zero of the estimation error ε(t). The stability proof of the output feedback scheme when observers with nonuniform convergence of the estimation error are considered is quite complex. It would be interesting to see if the results [3], [4] would apply to this specific case. Furthermore, it would be interesting to compare the effectiveness of the proposed control law with the general switching controllers in [5], or the sampled data observer in [6]. In the remaining of this discussion we try to focus on the principal issues that may arise in the practical implementation of the proposed tracking algorithm: several factors in fact have to be taken into account when a feedback control law is implemented in a real context. First of all, since the control law is formulated in a continuous time domain, a discrete time approximation must be proposed with a time step mainly constrained by the computational limits and the maximum available rate of measurements [7], [8]. A second important issue is the presence of noise and disturbances in the dynamics and the measurements. From this point of view, it is of particular interest to investigate around the robustness properties of the considered feedback controller: we will perform a numerical analysis by comparing the approach proposed in the paper with a control scheme where the observer is replaced by an Extended Kalman Filter (EKF). The EKF is a standard tool for fusing proprioceptive and exteroceptive measurements in order to estimate the pose of a mobile robot (see e.g., [9] for an application of the EKF in a scenario where the Cartesian coordinates of the robot are measured through a RFID system).
357
Discussions on: “Observer Based Trajectory Tracking for a WMR”
Fig. 2. Noise free system: EKF based control (left) and observer based control (right).
Fig. 1. The desired trajectory (left) and the angular velocity ω along it (right).
To perform this comparison we first introduce a discrete time approximation of the dynamics of the mobile robot (Euler approximation of equations (2) in the paper): x1 (k + 1) = x1 (k) + u1 (k)cos(θ (k)) Ts
(3)
x2 (k + 1) = x2 (k) + u1 (k)sin(θ (k)) Ts
(4)
θ (k + 1) = θ (k) + (u2 (k) + nu2 (k)) Ts
(5)
u1 (k + 1) = u1 (k) + (v1 (k) + nv1 (k)) Ts
(6)
where nu2 ∼ N (0, σu22 ) and nv1 ∼ N (0, σv21 ) are normal random variables characterizing the uncertainties in the actuators of the robot. The discretization step in our simulations is Ts = 6 · 10−4 s. Similarly, the position measurements are corrupted by Gaussian noises: z1 (k) = x1 (k) + n1 (k)
(7)
z2 (k) = x2 (k) + n2 (k)
(8)
being n1 ∼ N (0, σ 2 ) and n2 ∼ N (0, σ 2 ) two normal random variables. Similarly to the paper, we consider a reference trajectory where the robot must follow back and forth an arc of circle and, in the corner (i.e., where there is an inversion of motion), the longitudinal velocity tends to zero. We report in Fig. 1 the reference trajectory (x1d , x2d ) considered in all the simulations: the robot should start from point (2, 0) moving counterclockwise along the arc of√circle until the point (2 cos(π/3), 2 sin(π/3)) = (1, 3). Then it must go back to the initial position. The initial position of the robot is (2.2, 0.1) with an initial orientation of 80◦ . The parameters of the control algorithm and of the observer are like in the paper. The EKF estimates
all the state variables, i.e., x1 , x2 , θ and u1 . The observer proposed in the paper only estimates θ using for x1 and x2 the measurements of these variables. Also u1 is assumed known in the paper. In the noisy implementation of the observer based controller, we assume that the initial value of u1 (which is zero) is known (i.e., uˆ 1 (0) = 0) and that the subsequent values are derived through integration: uˆ 1 (k + 1) = uˆ 1 (k) + v1 (k) Ts , being v1 the acceleration input (notice that the actual velocity evolves according to u1 (k + 1) = u1 (k) + (v1 (k) + nv1 (k)) Ts ). Alternatively, one may assume that the longitudinal velocity is available through a speedometer: in this case a third measurement is available to the EKF (and the observer based controller may directly use it) but clearly an additional sensor is required. Moreover, the results obtained using a speedometer are not significantly different from the ones reported in this discussion. If all the noises are negligible (i.e., σ ≈ 0, σu2 ≈ 0 and σv1 ≈ 0), both the EKF based controller and the one based on the observer described in the paper provide a satisfactory performance (see Fig. 2). As noticed in the paper, the estimate of the orientation appears frozen when the longitudinal speed of the robot approaches zero (where the desired trajectory presents an abrupt change of direction of π radiants). This drawback does not afflict the EKF based approach, even if there is no theoretical guarantee of convergence in this case. A comparison between the tracking errors e(k) =
(x1 (k) − x1d (k))2 + (x2 (k) − x2d (k))2
of the two approaches in the case of negligible noises is reported in Fig. 3. The performance of the observer based controller degrades if we consider a noise in the measurement of x1 and x2 . For example, if we have a measurement noise with standard deviation σ = 0.01 m, the obtained trajectories are reported in Fig. 4: it is apparent how the EKF
358
Discussions on: “Observer Based Trajectory Tracking for a WMR”
Fig. 3. Noise free system: tracking error of the two approaches. Fig. 5. Average tracking error as a function of the standard deviation σ of the noise in the measurements of x1 and x2 .
Fig. 4. Noise in the measurements (σ = 0.01 m): the EKF based control (left) and the observer based control (right).
based approach is not significantly perturbed by the noise unlike the observer based controller. A plot of the average tracking error: J=
T 1 e(k) T k=1
(where T is the duration of the simulation) corresponding to the two approaches is reported in Fig. 5 as a function of the noise intensity. Each value is the average of 100 independent simulation runs, where the average is computed only among the non failing runs (a run is considered failing if J > 0.2 m). The fraction of failures for the two methods is reported in Fig. 6: the plot of the error of the observer based approach (in Fig. 5) is incomplete for large noise levels since, for these values of noise, this approach fails to track the trajectory in all the 100 simulation runs as seen in Fig. 6 (actually, for σ > 0.015 m, the average tracking error of the observer based approach is not simply larger than 0.2 m but becomes unbounded, denoting an unstable behavior).
Fig. 6. Fraction of failures as a function of the standard deviation σ of the noise in the measurements of x1 and x2 .
The situation is different if we consider the noise in the actuators: in this case the observer based controller seems to remain effective and, if the noise acts on the longitudinal velocity actuation (i.e., on the acceleration v1 ), it also appears more reliable than the EKF based controller. In Fig. 7 we report the average tracking error of the two approaches as a function of the standard deviation σu2 which ranges in the interval (0, 0.2). Notice at this regard that a standard deviation of 0.2 rad/s is quite intensive, given that u2 , apart from the values corresponding to the abrupt change in the direction on the corner of the trajectory, normally ranges in the interval (−0.5, 0.5). Nevertheless the effect on the two approaches seems negligible: the tracking error remains really close to the noise free value and the fraction of failures in this case is 0 for both methods. The EKF based approach appears less reliable when the noise acts on the longitudinal velocity actuation. In
359
Discussions on: “Observer Based Trajectory Tracking for a WMR”
Fig. 7. Average tracking error as a function of the standard deviation σu2 of the noise in the steering actuation u2 .
Fig. 9. Fraction of failures as a function of the standard deviation σv1 of the noise in the longitudinal velocity actuation v1 .
References
Fig. 8. Average tracking error as a function of the standard deviation σv1 of the noise in the longitudinal velocity actuation v1 .
Fig. 8 we report the average tracking error of the two approaches as a function of the standard deviation σv1 which ranges in the interval (0, 0.2). Also in this case a standard deviation of 0.2 m/s2 is quite intensive, given that v1 ranges in the interval (−3, 5) with an average absolute value of about 0.7 m/s2 . The performance of the two methods appears satisfactory even if the probability of failure for the EKF approach is not negligible in this case (see Fig. 9).
1. Astolfi A, Karagiannis D, Ortega R. Nonlinear and Adaptive Control with Applications, Springer-Verlag, 2008. 2. Karagiannis D, Carnevale D, Astolfi A. Invariant manifold based reduced-order observer design for nonlinear systems. IEEE Trans Autom Control, 2008; 53(11): 2602–2614. 3. Praly L, Arcak M. A relaxed condition for stability of nonlinear observer-based controllers. Syst Control Lett, 2004; 53(3–4): 311–320. 4. Carnevale D, Karagiannis D, Astolfi A. A condition for certainty equivalence output feedback stabilization of nonlinear systems. IEEE Trans Autom Control, 2010; 55(5): 1180–1185. 5. Casagrande D, Astolfi A, Parisini T. Global asymptotic stabilization of the attitude and the angular rates of an underactuated non-symmetric rigid body. Automatica, 2008; 44(7): 1781–1789. Available Online: http://www. sciencedirect.com/science/article/pii/S0005109807004931. 6. Arcak M, Neši´c D. A framework for nonlinear sampleddata observer design via approximate discrete time models an emulation. Automatica, 2004; 40: 1931–1938. 7. Laila D, Neši´c D, Astolfi A. 3 Sampled-Data Control of Nonlinear Systems, Springer, Berlin, 2006. 8. Neši´c D, Teel AR, Carnevale D. Explicit computation of the sampling period in emulation of controllers for nonlinear sampled-data systems. IEEE Trans Autom Control, 2009; 54(3): 619–624. 9. DiGiampaolo E, Martinelli F. A passive UHF-RFID system for the localization of an indoor autonomous vehicle. IEEE Trans Ind Electron, 2012; 59(10): 3961–3970.