Object Tracking Control for an Autonomous Robot System

Object Tracking Control for an Autonomous Robot System

Ming-Chun Lin

Jui-Jen Hsieh

Yin-Tien Wang

1_{Dept. of Electrical Engineering} 2 _{Dept. of Mechanical and Electro-Mechanical Eng.}

Technology and Science Institute of Northern

Taiwan Tamkang University

Peito, Taipei, TAIWAN Tamsui, Taipei Hsien, TAIWAN 25137

Email: [email protected] Email: {695370659@s95, ytwang@mail}.tku.edu.tw

Abstract

implement a non-holonomic wheeled mobile robot that possesses dynamic object-tracking capability by using real-time image processing. Two motion control laws are proposed by using Lyapunov’s direct method and computed-torque method. Simulation results illustrate the effectiveness of the developed schemes. The overall experimental setup of the mobile robot developed in this paper is composed of a Windows based personal computer, Programmable Interface Controllers, a mobile robot, and an omni-directional vision system. The image-based and real-time implementation of the mobile robot demonstrates the feasibility and effectiveness of the proposal schemes.

Keywords: Mobile robot, Object tracking, Motion control, Programmable Interface Controller (PIC), Real-time implementation.

1. Introduction

The problem addressed in the paper is the design of feedback controllers for a wheeled robot with non-holonomic constraints to track an object on the ground using visual feedback. The non- holonomic constraints restrict the admissible velocity space but not that of configuration. Because of these constraints, control design and analysis become substantially more involved. The control of non-holonomic wheeled mobile robot has been widely investigated in the literatures. Some researchers [Laumond 1986, Ma et al. 1999, Coulaud et al. 2006] designed the control on the basis of a kinematic state-space model derived from the constraints, but not taking the internal dynamics of the system into account.

Models that include dynamic effects are required for other purposes, for instance, using torques as control inputs and/or the inertia effect of the robot is not negligible. The approaches based on dynamic models of non-holonomic systems include the works in the literatures [Campion et al. 1990, Bloch et al. 1992, Su and Stepanenko 1994, Sarkar et al. 1994, Shim et al. 1995, Yang and

Kim 1999, Lee 2007]. Campion et al. [1990] and Bloch et al. [1992] derived a full dynamical description of non- holonomic mechanical systems and showed a suitable change of coordinates allows for analysing globally the controllability and the state feedback stabilizability of the system. Sarkar et al. [1994] presented a dynamic path-following algorithm for wheeled mobile robot by using a nonlinear feedback for input-output linearization and decoupling. Su and Stepanenko [1994] developed a reduced dynamic model for simultaneous independent motion and force control of non- holonomic systems. They proposed a robust control law which is a smooth realization of sliding mode control. Shim et al. [1995] proposed a variable structure control law, with which mobile robots converge to reference trajectories with bounded errors of position and velocity. Yang and Kim [1999] developed a sliding mode control law by means of the computed-torque method for solving trajectory tracking problems of non- holonomic mobile robots. Lee [2007] presented a switching control law, which, by utilizing the passivity property and energetic structure of the wheeled mobile robot, can ensure that the robot’s orientation converges to a target value, while driving the robot’s (x, y)-trajectory to a desired position on the plane within a user-specified error- bound.

Visual feedback is a mature and inexpensive technology for servo control of non-holonomic wheeled mobile robots. There exist a lot of image-processing algorithms extracting a map of the environment from the data provided by a camera [Broggi et al. 1999, Taylor et al. 1999]. But the implementation of such sophisticated algorithms is quite complex. Ma et al. [1999] formulated the tracking problem as one of directly controlling the shape of the curve in the image plane, instead of separately considering the estimation from the vision measurements and the design of control strategies. The practical implementation is, however, rather sophisticated, implying an extended Kalman filter to dynamically estimate the image parameters required for

feedback control. Coulaud et al. [2006] proposed a simple solution of the robot tracking problem, which avoids sophisticated image processing and control algorithms. Their practical implementation is straightforward, and can easily be achieved online. While the above works are mainly based on kinematic models of non-holonomic systems.

The dynamic equation of non-holonomic wheeled robot system is derived in this section, and the torque is selected as the control inputs which can be implemented easily in motion control

design. A coordinate frame xmym is set on the

chassis of the planar robot as shown in Figure 1. The robot has two driving wheels and one passive wheel. The robot system is fully described by a set of generalized coordinates

In this paper, we propose a novel control scheme for solving dynamic object-tracking problems of non-holonomic mobile robots using visual feedback. Dynamic models of mobile robots is used to describe their behaviors and, by means of a suitable change of coordinates, dynamics of mobile robots is linearized and two motion control laws based on Lyapunov’s direct method and computed-torque method are applied for stabilizing the robots to a desired position and orientation. Mobile robots with the proposed control laws converge to a given desired position and orientation with asymptotic stability using real-time visual feedback and image processing. While the approach for object-tracking of non- holonomic systems via motion control was presented in the literatures [Sarkar et al. 1994, Yang and Kim 1999], our approach has two contributions: First, in our approach, three- dimensional state variables of position and orientation are explicitly given as control outputs in two control stages, and the control inputs are designed to be applied motor torque. Second, experimental verification is shown. Few empirical studies on dynamic object-tracking of non- holonomic systems have been reported in the literatures, the proposed control laws are implemented on controlling a real non-holonomic wheeled mobile robot that has the dynamic model considered in this paper. Practical issues of visual servo control are addressed and results of the dynamic object-tracking control in the real world are shown.

(

)

Where xy are the axes of the fixed coordinate system; φ is the rotational angle between the robot

frame and fixed coordinate; θi are the rotational

angles of three robot wheels; β is the swinging angle of the passive wheel. The motions of three robot wheels in the direction being perpendicular to the wheel axis are constrained by three pure-rolling conditions,

(1a)

0 cos

sin + + + ₁= −x_& φ y_& φ lφ& rθ&

(1b)

0 cos

sinφ− _& φ+ φ&+ θ&₂= & y l r x 0 sin ) cos( ) sin( + + + − ₃ + _{3 3}= −x_& φ β y_& φ β lφ& β rθ& (1c)

where l is the length from the wheel to the centre

of the robot chassis; l3 is the length from the pivot

of the passive wheel to the centre of the robot chassis. The motions of three robot wheels in lateral direction are constrained by three no- slipping conditions,

0 sin cosφ+y φ=

x& & (2a)

0 sin cos − = −x& φ y& φ (2b) 0 ) cos ( ) sin( )

cos(φ+β + & φ+β + + ₃ β φ&+ β&=

& y d l d x (2c) y x m y m x φ d β 3 l l l 2 θ 1 θ 3 θ L τ R τ R D L D h

This paper is organized as the following sections. In section 2, the dynamic equations of a two-wheeled autonomous robot are derived and the change of coordinates is applied to linearize the dynamics. The designs of robot motion controller are presented in Section 3. In section 4, the proposed dynamic object- tracking behaviour of the non-holonomic mobile robot is described. The experimental setup of mobile robot is also depicted. And section 5 is the experimental results and discussions. We conclude the paper with some remarks in the last section.

Figure 1 Mobile robot system

Since the passive wheel is unpowered, θ3 in

Equation (1c) is free to rotate according to the robot motion. Meanwhile, any lateral motion of the passive wheel in Equation (2c) is balanced by the free rotational angle β. Therefore, Equations (1c) and (2c) do not constrain the motion of the robot.

2. Dynamics of a Wheeled Autonomous

Robot

Therefore, the constraint equations (1a) and (1b) are the pure-rolling conditions of right and left wheels of the robot,

φ φ

η₁=−x&sin +y&cos

φ η₂= &

Where η are the variables in the null space of the system and chosen to be new state variables of the system. The new state equation for the planar robot is derived as ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ ⎥⎦ ⎤ ⎢⎣ ⎡ − − − = ⎥⎦ ⎤ ⎢⎣ ⎡ φ φ φ φ φ ω ω & & & y x l l r L R cos sin cos sin 1 (3)

where and . The non-slipping

constraint equations (2a) and (2b) are dependent and can be expressed as one single equation

1

θ

ω_R = & ω_L =θ&₂ Mη&1=DR+DL

) (

2

0η l DR DL

I & = −

Rearrange and obtain

(4)

[

]

According to the integrability requirements of perfect differentials [D’Souza and Garg 1984, Campion et al. 1996], Equation (4) is a non- holonomic constraint.

where v is the new input vector. Equation (6) completely describes the dynamics of the robot system and complies with the constraint equation.

In this research, the external torques is represented by the driving torques of the motor. The dynamic equations of right and left wheel are expressed as

The dynamic equation of the robot can be derived by using the Lagrange formulation [D’Souza and Garg 1984]:

u q B λ q A q L q L ) ( ) ( + = ∂ ∂ − ⎟⎟ ⎠ ⎞ ⎜⎜ ⎝ ⎛ ∂ ∂ & dt d (5) _{Ιwω&R =τR−rDR−cωR (7a)} L L L L w rD c Ι ω& =τ − − ω (7b)

where λ is the vector of Lagrange multiplier;

A(q)　 is the vector of the generalized forces, which are applied on the system in order to satisfy the kinematic constraints; B(q)u indicate the generalized forces, which are constructed by external forces and torques applied on the system by the actuators; L is the Lagrangian of the system. For the robot in planar motion as shown in Figure 1, it has

Where Iw is the inertia of the wheel system; τR and

τL are the applied torques of the right and left

motors, respectively; c is the coefficient of viscous friction; r is the radius of the wheel. From the pure-rolling conditions of right and left wheels, it has ⎥⎦ ⎤ ⎢⎣ ⎡ ⎥⎦ ⎤ ⎢⎣ ⎡ − = ⎥⎦ ⎤ ⎢⎣ ⎡ 2 1 1 1 1 ηη ω ω l l r L R ₍₈₎ q q M q q q

L( ,&) & ( )& 2

1 T

= Substitute Equation (8) and its first time-derivative

into Equations (6) and (7), it can be obtained the new state equation of the robot system

⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ = 0 0 0 0 0 0 0 ) ( I M M q M τ B η C η M′&+ ′ = ′ (9) ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ + + = ′ r I l I r I Mr I Mr w w 0 2 2 0 2 2 0 0 2 M Where M(q) is the matrix of mass and inertia.; M

and I0 are the mass and inertia of the robot system

about the origin of robot coordinate, respectively. Substitute the Lagrangian into Equation (5), and have ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ = ′ r I cl Mr c 0 2 2 0 0 2 C , ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = ′ 0 0 1 1 I l I l M M B φ φ λcos (D_R D_L)sin x M&&= − + φ φ λsin (D_R D_L)cos y M&&= + +

(

)

DR and DL are the driving forces of the right and

left wheels, respectively. These dynamic equations and the kinematic constraint equations (3) and (4) fully describe the robot system. Unfortunately, the Lagrange multiplier λ can not be measured and controlled by the control system. The concept of null space is utilized to eliminate the Lagrange multiplier in the equations [Campion et al. 1996] by choosing

Note that, the input of the robot dynamic equation becomes the torque of the motor, which can be implemented easily in robot control scheme.

3. Robot Motion Control

In this section, two robot motion control laws are designed based on Lyapunov’s direct method and computed-torque method. By appropriate

choices of output variables, the mobile robot with the designed control laws converge to a given desired position and orientation with asymptotic stability.

3.1 Design of Lyapunov controller

The potential energy and kinetic energy of the robot system is selected as the Lyapunov function candidate, q K q η M η q η ~ _P~ 2 1 2 1 ) ~ , ( T T V = ′ +

The proportional control gain matrix KP is

symmetric and positive definite. q~=qd −q is

defined as the position error, and qd is the desired

position. It has q K q η M η η M η P~ 2 1 T T T

V& = ′&+ &′ − &

η

M &' can be found by Equation (9), and . Therefore, 0 '= M& ) ~ (Bτ S K q η η C ηT T T P V& =− ′ + ′ −

The motion controller is designed as ) ~ (K q K q S τ B′ = T P − D& (10)

The derivative control gain matrix KD is symmetric

and positive definite. Therefore, 0 ≤ − ′ − = η Cη q& KDq& & T T V

3.2 Design of computed-torque controller

In order to control the robot to desired location

[x y]T, the concept of computed-torque method is

utilized to design another motion controller.

Choose the outputs to be

[

]

locates at the pivot of the third wheel as shown in Figure 1, ⎥⎦ ⎤ ⎢⎣ ⎡ − + = ⎥⎦ ⎤ ⎢⎣ ⎡ φ φ cos sin 3 3 l y l x y x

Hence, the time derivative of the outputs will be v B η q q C q&&= ′′( ,&) + ′′ (11) ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ − − − = ′′ φ φ φ φ φ φ φ φ cos sin sin cos ) , ( 3 3 & & & & & l l q q C ⎥⎦ ⎤ ⎢⎣ ⎡− = ′′ _φφ _φφ sin cos cos sin 3 3 l l B

Design the control input of the controller by using the computed-torque method

) ~ ~ ( 1 _{C η q K q K q} B v= ′′− − ′′ + &&_d + _P + _D& (12)

where KP and KD are the proportional and

derivative gain matrices, respectively. From

Equation (12), it can be seen that l3 can not be zero.

Substituting Equation (11) into Equation (12), it becomes 0 ~ ~ ~_{+K q+K q =} q&& _D& _P

The control torque [τR τL]T can be derived as

⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ ⎥⎦ ⎤ ⎢⎣ ⎡ − − − + ⎥⎦ ⎤ ⎢⎣ ⎡ ⎥ ⎥ ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎢ ⎢ ⎣ ⎡ − = ⎥⎦ ⎤ ⎢⎣ ⎡ φ φ φ φ φ τ τ & & & y x l l r c v v l r I Mr l r I Mr L R cos sin cos sin 2 2 2 2 2 1 0 0 ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ ⎥⎦ ⎤ ⎢⎣ ⎡ − − − + ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ ⎥ ⎦ ⎤ ⎢ ⎣ ⎡ − − − − + φ φ φ φ φ φ φ φ φ φ φ φ φ φ && && && & & & & & & & y x l l r I y x r I_{w w} cos sin cos sin 0 sin cos 0 sin cos ₍₁₃₎

4. Object Tracking Behavior

The robot behaviour of dynamic object- tracking is planned using the proposed motion controllers. Before planning the object-tracking behaviour, we simulate the motion control using the proposed Lyapunov controller and computed- torque controller. As shown in Figures 2 and 3, the robot initially locates on eight different positions of a circle with radius of 10m, and the desired

output position is [y x φ]T=[0m 0m 0rad]T. The

results of motion control by the Lyapunov controller are shown in Figure 2. The figure depicts that the motion control can achieve the desired output in position y and orientation 　, while the desired position in x direction can not be stably controlled. On the other hand, Figure 3 depicts the results of motion control from eight positions of a circle by using the computed-torque controller. The results depict that the motion control can achieve the desired output in position x and y, but not in orientation φ.

We propose to plan the dynamic object- tracking behaviour of the robot by using the Lyapunov controller and the computed-torque controller in two stages. As depicted in Figure 4, the tracking behaviour is composed of two motion controllers in these two stages. In stage 1, the robot

moves from the initial position (x0,y0) to the

candidate point (x1,y1) by using the computed-

torque controller. In stage 2, the robot turns with an angle γ to reach the angle ψ by using the Lyapunov controller, and then push the ball to the goal by using the computed-torque controller again. If the orientation of the robot is deviated from the desired direction by a preset threshold value, the Lyapunov controller will be recalled to convert the deviation. ⎥ ⎥ ⎦ ⎤ ⎢ ⎢ ⎣ ⎡ − − − = ′′− φ _φ φ_φ sin 1 cos 1 cos sin 3 3 1 l l B

Figure 2 Motion control by using the Lyapunov controller

Figure 3 Motion control by using the computed-torque controller ψ ) , (xbyb ) , (xg yg ) , (x1y1 1 d γ ) , (x0y0 Goal ectory Robot traj y trajector Reference position initial Ball 1 2 3 position initial Robot

Figure 4 Robot object-tracking behavior

5. Experimental Results

In this section, we present the design and implement of the non-holonomic wheeled robot. The experimental setup of the wheeled robot system, as shown in Figure 5, is composed of a Windows based personal computer (PC), Programmable Interface Controllers (PIC), a mobile robot, and an omni-directional vision

system. The omni-directional vision is utilized to capture the image surrounding the robot, as shown in Figure 6, and calculate the distance from an object to the robot. A PC is in charge of implementing the algorithms including feature recognition and tracking, Simultaneous Localization and Mapping (SLAM), and path planning and obstacle avoidance. Motion and current control laws are implemented in PIC microcontrollers for the driving motors. In the dynamic control of the robot, motor driving torques are chosen as the input commands which are implemented by Pulse Width Modulation (PWM) signals and sent to the motor drive circuits.

The experimental setup of wheeled mobile robot is tested on the ground to track a ball dynamically. Two dynamic motion controls are combined to form the robot object-tracking behaviour. The results are shown in Figures 7 and 8. Figure 7 depicts the continuous tracking motions of the robot. Figure 8 depicts the trajectory of the ball in polar coordinates. Initially, the ball located in the direction of φ=122° and a distance of 85 image pixels. The robot turns firstly, and then approaches to the ball. Finally, the ball is located between the robot and the goal.

Visiondirectional Omni−

Computer Personal

Controller

PIC _DriveMotor

Battery

Figure 5 Wheeled mobile robot

[2] Y. Ma, J. Kosecka, and S.S. Sastry, 1999,Vision Guided Navigation for a Nonholonomic Mobile Robot, IEEE Transactions on Robotics and Automation, vol.15, no.3, pp.521-536.

[3] J.-B. Coulaud, G. Campion, G. Bastin, and M. De Wan, 2006, Stability Analysis of a Vision-Based Control Design for an Autonomous Mobile Robot, IEEE Transactions on Robotics, vol.22, no.5, pp.1062-1069.

[4] G. Campion, B. d’Andrea-Novel and G. Bastin, 1990, Controllability and State Feedback Stability of Nonholonomic Mechanical Systems, In Advanced Robot Control, Proceedings of International Workshop in Adaptive and Nonlinear Control: Issues in Robotics, pp.106-124, Grenoble. Figure 7 Continuous tracking motions of the robot

[5] A M. Bloch, M. Reyhanoglu and N.H. McClamroch, 1992, Control and Stabilization of Nonholonomic Dynamic Systems, IEEE Transactions on Automatic Control, Vol.37, No.11, pp.1746-1757.

[6] C.Y. Su and Y. Stepanenko, 1994, Adaptive control of a Class of Nonlinear systems with Fuzzy logic, IEEE Transactions on Fuzzy Systems, vol.2 no.4, pp.285-294.

[7] N. Sarkar, X. Yun, and V. Kumar, 1994, Control of Mechanical Systems with Rolling Constraints: Application to Dynamic Control of Mobile Robots, The International Journal of Robotics Research, vol.13, no.1, pp.55-69.

[8] H.-S. Shim, J.-H. Kim, and K. Koh, 1995, Variable structure control of nonholonomic wheeled mobile robots, in Proceeding IEEE International Conference on Robotics and Automation, May 1995, pp. 1694–1699.

Figure 8 The trajectory of the ball in polar coordinates

6. Conclusion Remarks

control for trajectory tracking of Nonholonomic Wheeled Mobile Robots, IEEE Transactions on Robotics and Automation, vol.15, no.3, pp.578-587. In this paper, we plan a dynamic object-

tracking behavior for a non-holonomic mobile robot using two developed motion control laws. The mobile robot with the proposed control laws converge to a given reference trajectory with asymptotic stability using real-time visual feedback and image processing. Two major contributions by this research include: The state variables of position and orientation are explicitly given as control outputs in two control stages, and the control inputs are planned by using motor driving torque which is easily implemented in mechatronics system. Meanwhile, the proposed control laws are implemented on controlling a real non-holonomic wheeled mobile robot using a personal computer and PIC microcontrollers. The image-based and real-time implementation of the mobile robot demonstrates the feasibility and effectiveness of the proposal schemes.

[10] D. Lee, 2007, Passivity-Based Switching Control for Stabilization of Wheeled Mobile Robots, Proceedings of Robotics Science and Systems Conference.

[11] A. Broggi, M. Bertozzi, A. Fascioli, and G. Conte, 1999, Automatic Vehicle Guidance: The Experience of the Argo Autonomous Vehicle. Singapore: World Scientific.

[12] C.J. Taylor, J. Koseckà, R. Blasi, and J. Malik, 1999, A comparative study of vision-based lateral control strategies for autonomous highway driving, International Journal of Robotics Research, vol. 18, no. 5, pp. 442-453.

[13] A. F. D’Souza and V. K. Garg, 1984, Advanced dynamics: Modeling and Analysis, Prentice-Hall. [14] G. Campion, G. Bastin, and B. D’Andrea-Novel,

1996, Structural Properties and Classification of Kinematic and Dynamic Models of Wheeled Mobile Robots, IEEE Transactions on Robotics and Automation, vol.12, no.1, pp.47-62.

References

[15] J.-J.E Slotine, and W. Li, 1991, Applied Nonlinear Control, Prentice-Hall Inc.

[1] J.P. Laumond, 1986, Feasible trajectories for mobile robots with kinematic and environment constraints, Proceeding of International Conference on Intelligent Autonomous Systems, pp.346-354.