Bearing only visual servo control of a non-holonomic mobile robot. Robert Mahony

Transcription

1 Bearing only visual servo control of a non-holonomic mobile robot. Robert Mahony Department of Engineering, Australian National University, Australia. Robert.Mahony@anu.edu.au url: Presentation for IRISA, INRIA, Universite de Rennes, Rennes. Non-holonomic IBVS. November 22,

2 ANU (RSISE) Nomad 4000 mobile robot Long term project towards the full automation of a mobile robot. Goal is to have robot running independently for long periods. This presentation considers the development of an automated docking process based on visual sensor system. Non-holonomic IBVS. November 22,

3 Kinematic model of robot ẋ = u cos θ ẏ = u sin θ θ = ω ξ = (x, y) g u linear velocity of the vehicle. θ rotation angle from global frame g to body fixed frame b. ω angular velocity. A docking manoeuvre (x, y, θ) 0 is a state stabilisation problem for the kinematic model. Non-holonomic IBVS. November 22,

4 Key Issues for docking manoeuvres In practice, several sensor systems are available to provide feedback during a docking manoeuvre. Considering just the vision system (and undertaking similar developments based on other sensor configurations) provides redundancy in the control/sensing configuration of the robot. The docking manoeuvre is highly sensitivity to lateral offset from the docking station. In practice lateral accuracy of around 1 cm is required during the docking process. Maintaining good calibration of a visual system for extended periods of operation is a significant problem. Non-holonomic IBVS. November 22,

5 Omni-directional (Panoramic) Camera Allows the robot to observe targets in entire room. Widely spaced targets provides precise positioning of robot despite low precision vision system. Low pixel resolution leads to bearing only measurements. Excellent sensor modality for low cost, highly robust visual positioning tasks. Non-holonomic IBVS. November 22,

6 Multi-coloured visual targets Target identification uses a combination of color segmentation and image moments along with a simple matching criterion to obtain 98% reliable image recognition in a wide range of lighting conditions. Non-holonomic IBVS. November 22,

7 Image space with identified landmarks Non-holonomic IBVS. November 22,

8 Image feature Each identified landmark bearing is represented as a unit vector expressed in the body-fixed frame p i = R(θ) T ξ i ξ ξ i ξ where R(θ) is the rotation matrix from the body-fixed frame to the global frame. Image feature used is the Average Landmark Vector (AVL), defined as q(ξ) = n p i i=1 Body-fixed-frame measurement The global frame version of the image feature is used in some of the analysis q g = R(θ)q. Non-holonomic IBVS. November 22,

9 Average Landmark Vector q 1) Approach inspired by insect navigation. 2) Image based feature - advantages for closed-loop robustness - does not require detailed world model. 3) Image features are discontinuous at target points due to lack of range information in bearing only measurements. Non-holonomic IBVS. November 22,

10 Bio-mimetic vision systems Lambrinos et al. 2000, Robotics and Autonomous Systems. Moeller, 2000, Biological Cybernetics. AVL theory developed in two dimensions. Holonomic kinematic control law V = q q 0 R 2. Goal pose is distinct from landmark points. Requires an inertial direction in addition to visual data. Non-holonomic IBVS. November 22,

11 Image based visual servo-control of dynamic systems Hamel et al., 2001, Transactions on Robotics and Automation. Theory developed in three dimensions. Control of dynamic system - exploit passivity of spherical image plane. Kinematic part of control V des. = q q 0 R 3. Goal pose is distinct from landmark points. Requires an inertial direction in addition to visual data. Non-holonomic IBVS. November 22,

12 Discontinuous control design for non-holonomic systems Discontinuity of the image features implies that an IBVS control will be discontinuous at target points. The docking station is one of the landmark points used. This implies that the control design will be discontinuous at the target pose. The discontinuity is exploited to address the problem of control design for the non-holonomic system. Discontinuous control is inherently non-robust, however, the closed-loop trajectories of the robot do not cross discontinuities of the control law. Non-holonomic IBVS. November 22,

13 Image plane kinematics The kinematics of the ALV are given by q = Ωq Q(ξ)V, D ξ q = Q(ξ) where and Q = Ω = n i=1 (I 2 p i p T i ) ( 0 ω ω 0 ξ i ξ ) > 0, Interaction Matrix, V = R T ξ = R T ( u cos(θ) u sin(θ) ), is the velocity vector of the vehicle in the global frame. Non-holonomic IBVS. November 22,

14 Holonomic Control Design Choose a holonomic control design V des. = q(ξ), v des. = q g (ξ) = Rq(ξ), Body-fixed-frame Base-frame Leads to closed-loop kinematics ξ = q g (ξ), Consider the two Lyapunov functions U(ξ) = n i=1 ξ i ξ Base-frame φ(ξ) = q(ξ) T q(ξ) = q(ξ) 2 Body-fixed-frame Note that φ(ξ) can be measured on-line while U(ξ) requires knowledge of ξ and ξ i. Non-holonomic IBVS. November 22,

15 Holonomic Stability Analysis Stability analysis (Moeller, 2000) d dt U(ξ) = n i=1 d dt ξ i ξ = n i=1 (ξ i ξ) T ξ ξ i ξ = q T g q g = q T q. Stability analysis (Hamel et al., 2001) d dt φ(ξ) = qt q = 2q T Ωq 2q T Q(ξ)V = 2q T Q(ξ)q, Q(ξ) > 0. Note that U(ξ) is convex, U(ξ) 0 and DU(ξ) = 0 at a local minimum of U, φ(ξ) = q 2 = DU(ξ) 2, D 2 U(ξ) = Q(ξ) > 0 Non-holonomic IBVS. November 22,

16 Placing the targets. A target must be placed at the docking station. A unique minimum ξ of U and φ exists and lies in the closed convex hull of the landmark features. By placing the targets appropriately it is possible to locate ξ at the docking station. If the targets cannot be placed appropriately then a target weighting scheme has been developed to place ξ at the docking station (Wei, et al., 2005). Non-holonomic IBVS. November 22,

17 Structure of φ at ξ. The graph of φ at an optimal landmark. The cost surface φ is discontinuous at landmark points. Locating the minimum ξ at a discontinuity creates a singular trough in the cost function approaching ξ. The holonomic control design causes smooth decrease of φ. Expect that trajectories of closed-loop system will approach ξ along asymptotic direction of trough in cost function. Non-holonomic IBVS. November 22,

18 Simulation of closed-loop trajectories for holonomic control strategy Note that the closed loop system converges to the target point along a suitable trajectories to complete the docking manoeuvre. Non-holonomic IBVS. November 22,

19 Non-holonomic control strategy For the true system the holonomic velocity control v des. = q g will not normally satisfy the non-holonomic constraints. The approach to the non-holonomic control design is to design a control that will approximately track the desired behaviour ξ = q g This is similar to designing a controller to track a family of trajectories, any one of which will satisfy the control requirements. Non-holonomic IBVS. November 22,

20 Tracking error Tracking error ɛ track. = ξ v des. Define normalized vector directions q g = q g q g, v g = v g v g. Robot scalar velocity is u = v g. Schematic showing angle γ and the definition of the tracking error. Normalized tracking error and tracking angle = q g v g, γ = ( v g, q g ). Non-holonomic IBVS. November 22,

21 Consider the Lyapunov function L = U + q g + q g (1 cos(γ)) Stability Analysis The first term is the stability function introduced by Moeller The second term q = φ is highly sensitive to the discontinuous structure of the cost function. The third term is a close approximation of the tracking error. The C 0 nature of the cost terms q in L are crucial to obtaining the bounds. Non-holonomic IBVS. November 22,

22 Derivative of the cost function Differentiating L along trajectories L = u q T g Q q g + u ( T Q q g, v g ) ω q g, A v g, A = ( ) Using T = tan(γ/2) and making some approximations one obtains L u q T g Q q g + ( u 4nT 2 r 0 (1 + T 2 ) q g 1 ) T2 1 + T }{{ 2 } α ω 2T 1 + T 2 q g, } {{ } β Defining α and β as suitable functions of the observed quantities T and q g L = u q T g Q q g (uα + ωβ) q g (uα + ωβ) q g. Non-holonomic IBVS. November 22,

23 Control design The simplified form of the Lyapunov derivative is L (uα + ωβ) q g Minimize the right-hand side subject to a control constraints 1 2 u k ω B q g 2, u 0. Control inputs u = c 0α q kα 2 +β 2 for α 0 u = 0 for α < 0 ω = c 0β q g for α 0 kα 2 +β 2 ω = sgn(β)c 0 q for α < 0 Non-holonomic IBVS. November 22,

24 Convergence analysis Several calculations yield the following bound on the decrease in the Lyapunov function L c 0 q g 2.5 r 0 (t) 2 (k + 1)nr 0 (t) + 4kn The constant r 0 (t) is a bound r 0 (t) min{ ξ ξ i }. Applying Lyapunov s 2nd method shows that ξ ξ. With some care it is also possible to show that γ 0 along the closed-loop trajectory. Non-holonomic IBVS. November 22,

25 Asymptotic analysis The nature of the Lyapunov function allows one to obtain the following bound close to the limit point L A q, ξ ξ ɛ Consequently asymptotically L BL 3 One obtains L(t) 1 (c 1 t + c 2 ) The convergence implied in ξ ξ 0 is slower than exponential. Non-holonomic IBVS. November 22,

26 Experimental Configuration Non-holonomic IBVS. November 22,

27 Results of 15 experimental runs Non-holonomic IBVS. November 22,

28 Final convergence results The statistics are Mean St. Dev. x 0.1cm 0.91 cm y 0.3 cm 0.91cm γ -0.7 deg. 2.5 deg. Final position and orientation for a set of 15 experiments. Non-holonomic IBVS. November 22,

29 Video Demonstration Closed-loop behaviour of the robot in a typical test. Unwrapped image sequence from on-board camera. Raw image sequence from the on-board camera Non-holonomic IBVS. November 22,

30 Conclusions Precise position information obtained from low quality vision system by using a panoramic camera and widely spaced landmarks. Inherent discontinuity of measurements used to construct a discontinuous error feature that leads to discontinuous stabilising control for non-holonomic model of unicycle. Non-holonomic control design based on maximising rate of decrease in a control Lyapunov function subject to steering and speed constraints. Excellent experimental results. Non-holonomic IBVS. November 22,

31 Work presented was undertaken in collaboration with Ran Wei PhD student, Department of Engineering, Australian National University, Australia David Austin Department of Systems Engineering, Research School of Information Science and Engineering, Australian National University, Australia Non-holonomic IBVS. November 22,