A DEVELOPMENT FRAMEWORK FOR COLLABORATIVE ROBOTS USING FEEDBACK CONTROL Salman Ahmed, Mohd N. Karsiti and Herman Agustiawan Electrical and Electronic Engineering, Universiti Teknologi PETRONAS, Bandar Seri Iskandar, 31750 Tronoh, Perak, Malaysia. Email (s):
[email protected];
[email protected];
[email protected]
Abstract: Collaborative robots represent a nonlinear system having nonholonomic constraints. Due to nonholonomic constraints, the collaborative robots are not point stabilizable using continuous time-invariant feedback. Therefore, linear control is ineffective so that innovative design techniques are required. One such possible technique is feedback linearization. This paper proposes a kinematically compatible framework for the development of nonholonomic collaborative robots using feedback control techniques. It presents a comparative assessment of various standard feedback control strategies. The nonholonomic collaborative robotic system is modeled using Simulink and the results are presented confirming the performance of various controllers. From the simulation results, the nonlinear feedback control strategy improves the system performance. Furthermore, the nonlinear control strategy globally-asymptotically stabilizes the system compared to the other feedback strategies. The work presented here is an initial study concerning the applicability of kinematic based control on collaborative robots. Copyright © 2007 IFAC Keywords: Feedback control, mobile robots, nonholonomic.
Intelligent mobile collaborative robots are becoming significant in various industrial and scientific applications such as tele-surgery, underwater robotics, harvesting of agricultural lands, automated construction, planetary explorations, etc. Collaboration among the robot team members is motivated by a need to complete complex tasks that require more capabilities than a single robot can provide. Collaborative wheeled mobile robots are nonholonomic systems due to the constraints imposed on their kinematics. Hence, the control problems involving their kinematics have attracted considerable attraction over the past few years (Kolmanovsky and McClamroch, 1995).
the external disturbances and initial errors are present. Several feedback control strategies for trajectory tracking of collaborative robots have been proposed. A stable tracking control method based on the linearization of the corresponding error model was proposed in (Kanayama, et al., 1991). A globally K – exponentially stable tracking control law was proposed in (Lefeber, et al., 2001). Similarly, a locally uniform exponentially stable tracking control law based on cascaded systems theory was also proposed in (Lefeber, et al., 2001). However, most of these feedback control strategies have been designed for a single robot. For collaborative robots, most of the feedback control laws rely on visual based inputs (Carlos, et al., 2006; Stefan, et al., 2004; Mariottini, et al., 2005).
In automatic control systems, feedback improves the system performance by completing the task even if
This paper extends the idea of feedback control to collaborative nonholonomic mobile robots having
1. INTRODUCTION
communication capabilities. The coordination between robots is guaranteed by the communication protocol. The collaborative robots share information using the Bluetooth Personal Area Network (Morrow, 2002). This paper proposes the development of a framework for collaborative nonholonomic robots based on their kinematics. The development framework is simulated using MATLAB/Simulink. In order to allow a critical assessment, comparative results using various feedback controllers, originally developed for a single robot, are presented. The proposed framework can also be extended to complex robotic systems. 2. FRAMEWORK DEVELOPMENT FOR COLLABORATIVE ROBOTS Collaborative robots need to share information while working in a team. For information sharing, Bluetooth is used which provides various profiles for communication. One such profile is the Personal Area Network or piconet (Morrow, 2002). A piconet works using master-slave architecture and supports at the maximum of eight devices. Inter-slave communication in the piconet is done through the master device. The collaborative robots form a Bluetooth piconet with one robot acting as a master robot. For formation control, leader-follower structure is used (Fujimori, et al., 2005; Desai, et al., 1998). The leader robot is always assumed to be the master robot in the Bluetooth piconet. Feedback control laws are implemented in the leader as well as follower robots to track the correct trajectory. For the leader robot, this framework is shown in Fig. 1. For the follower robots, this framework is similar to the leader robot except that the desired trajectory and feedforward controller are not present. For a desired goal trajectory, the feedforward controller generates the necessary input control commands for the leader robots. At the same time, using Contract Net Protocol appropriate follower robots are selected for trajectory tracking based on a cost function. The cost function used in this case is based on the least distance to the trajectory. In the leader-follower strategy, the input control commands are transmitted to the follower robots by the leader robot using Bluetooth. The feedback
control law is implemented in each robot to improve the system performance by minimizing the error between the desired and the actual trajectory. The simulation platform is implemented using MATLAB/Simulink. A MATLAB session runs on each computer. Each session communicates with other sessions using the Bluetooth piconet. Each session models the proposed framework of Fig. 1. The master robot in the piconet acts as Contract Net Protocol manager while the slave robots act as potential bidders. The message format used for communication conforms to the standard Agent Control Language (ACL) provided by Foundation for Intelligent Physical Agents (FIPA, 2002). 3. KINEMATIC MODELING OF COLLABORATIVE ROBOTS A collaborative robot system can be described by its state, X, which is a composition of all the robots states, given as X = [x1 , x 2 , ... , x n ]T ,
X& = F ( X , t ).
Fig. 2 shows the leader-follower formation for collaborative robots. The kinematic state equations for the leader robot, pl, can be expressed as ⎛ x& l ⎞ ⎛ cos θ l ⎞ ⎛0⎞ ⎜ ⎟ ⎜ ⎟ ⎜ ⎟ p l = ⎜ y& l ⎟ = ⎜ sin θ l ⎟ v l + ⎜ 0 ⎟ ω l . ⎜⎜ & ⎟⎟ ⎜ 0 ⎟ ⎜1 ⎟ ⎝ ⎠ ⎝ ⎠ ⎝θ l ⎠
Contract Net Protocol
(2)
where vl and ωl are the leader robot’s linear and angular velocities, respectively. The wheels of the robots in Eq. 2 are assumed to exhibit purely rolling motion, thus no slipping occurs. Therefore, the wheels of each robot, both leader and followers, observe the nonholonomic constraint given by − x& sin θ + y& cos θ = 0.
(3)
Let d represent the distance between the front castor and the center of axis of each robot wheels as shown in Fig 2. The separation distance between the leader and the follower robot is represented by llf. The position coordinates for the front castor of the follower robot is represented by (x, y).
Feedforward Controller Desired Trajectory
(1)
Leader Follower Strategy
Feedback Control Law
Fig. 1. Development framework for leader robot.
Robot
The state variables, (xf, yf), for the follower robots can be written as
YI
XR
d
x f = x + d cos θ f ,
YR
(4)
y f = y + d sin θ f .
The follower robot can be expressed relatively to the leader robot in a vector form as pf = [l lf , ϕ lf , θ f ]T .
l lf
pl = ( xl , yl , θ l )
( x, y )
d YR
θ
l lf = ( x l − x) 2 + ( y l − y ) 2 ,
ϕ lf = π − arctan 2( y f + d sin θ f − y l ,
θl
XR
The separation bearing angle between the leader and the follower robot is denoted by ϕ lf . From Fig. 2, the separation distance and bearing angle can be obtained as
ϕlf
f
p f = (x f , y f ,θ f )
(5)
XI
(0, 0)
x l − x f − d cos θ f ) − θ l
Fig. 2. Leader-follower formation for collaborative robots.
Differentiating Eq. 5, the state space kinematic equations for the follower robot become l&lf = v f cos γ − vl cos ϕ lf + dω f sin γ ,
ϕ& lf =
vl sin ϕ lf − v f sin γ − ω l llf + dω f cos γ llf
,
θ& f = ω f . (6) where γ = ϕ lf + θ l − θ f , vf and ωf represent the follower robot’s linear and angular velocities respectively, and θf represent the orientation angle of the follower robot. In order to avoid collision between the leader and the follower robots a requirement that l lf > 2d must be ensured. The control inputs for the follower robot can be then derived based on the standard Input-Output linearization techniques as in (Asada and Slotine, 1986). The associated control inputs can be written as
cos γ {k a l lf (ϕ lfd − ϕ lf ) − v l sin ϕ lf + d l lf ω l + ρ sin γ }.
(7)
where
ρ=
In order to track the correct goal trajectory, the feedforward command controller generates the velocity inputs. The inputs commands are generated for the leader robot and transmitted to the follower robots using Bluetooth. The kinematics of the leader and the follower robots is modeled using Simulink. Different feedback control strategies are chosen from the literature based on their properties which are briefly discussed below.
4.1 Feedforward Command Generation Assuming that the collaborative robots follow a [ x d (t ), y d (t )] desired cartesian trajectory with t ∈ [0, T ] . After simple algebraic manipulations using Eq. 2, the following inputs are obtained. v d (t ) = ± x& d2 (t ) + y& d2 (t ) .
wd (t ) =
v f = ρ − dω f tan γ ,
ωf =
4. TRAJECTORY TRACKING
&y&d (t ) x& d (t ) − &x&d (t ) y& d (t ) x& d2 (t ) + y& d2 (t )
cos γ γ = ϕ lf + θ l − θ f .
,
(11)
4.2 Feedback Controller Design
is the desired bearing angle
between the leader and the follower robots.
(10)
(8)
In the control law given by Eq. 7, ka and kb are both the controller gains, l lfd is the desired separation distance, and ϕ lfd
.
Knowing that [ x d (t ), y d (t )] , θ d (t ) can be then calculated offline from
θ d (t ) = tan -1 ( y& d / x& d ). k b (l lfd − l lf ) + v l cos ϕ lf
(9)
The control objective of the feedback controller is to reduce the errors ( x d − x, y d − y, θ d − θ ) to zero. The error is expressed based on the coordinate change as given in (Kanyama, et al., 1991), ⎡ x e ⎤ ⎡ cos θ ⎢ ⎥ e = ⎢ y e ⎥ = ⎢⎢− sin θ ⎢⎣θ e ⎥⎦ ⎢⎣ 0
sin θ cos θ 0
0⎤ ⎡ x d − x ⎤ ⎥ ⎢ 0⎥⎥ ⎢ y d − y ⎥. (12) 1 ⎥⎦ ⎢⎣θ d − θ ⎥⎦
The error dynamics are obtained by differentiating Eq. 12 and combining with the nonholonomic constraint of Eq. 3, as x& e = ωy e − v + v r cos θ e y& e = −ωx e + v r sin θ e θ&e = ω r − ω.
Controller based on Cascaded System Theory: This controller was proposed by (Lefeber, et al., 2001). The control law is given as
v = v r + c 2 x e − c 3ω r y e , c 2 > 0, c 3 > −1 c1 > 0.
(14)
v = v r + c 2 x e − c 3ω r y e , c 2 > 0, c 3 > −1
k 2 = b v d (t ) .
(19)
c1 > 0.
The control law of Eq. 17 does not guarantee asymptotic stability of the error, e, because the system is time varying. Nonlinear Controller Design: A nonlinear control law was proposed in (Samson and Ait-Abderrahim, 1991) which is expressed as: v = vd cos(θ d − θ ) + k1 (vd (t ), ωd (t )) [cosθ ( xd − x) + sin θ ( yd − y )].
ω = ωd + k2vd
sin(θ d − θ ) [cosθ ( yd − y ) − θd − θ
(20)
where k2 is a positive constant, and both k1 and k3 are continuous functions strictly positive in ℜxℜ − (0,0) given by k1 (v d (t ), ω d (t )) = k 3 (v d (t ), ω d (t )) = 2ζa = 2ζ wd2 (t ) + bv d2 (t ) .
(15)
k 2 (v d (t ), ω d (t )) = b , b > 0.
The control law of Eq. 15 results in local uniform exponential stable system (LUES) if v r is bounded and ω r is persistently exciting. Controller Based on Lyapunov Stability: This feedback controller was proposed by (Kanayama, et al., 1991). The control law is given as v = v r cos θ e + K x x e ,
(18)
sin θ ( xd − x)] + k3 (vd , ωd )(θ d − θ ).
The control law of Eq. 14 is K–exponentially stable if v r is bounded and ω r is persistently exciting (Lefeber, et al., 2001). A small modification to this law was also proposed in (Lefeber, et al., 2001) which is
ω = ω r + c1 sin θ e ,
k1 = k 3 = 2ζa = 2ζ wd2 (t ) + bv d2 (t ) .
(13)
The objective is to design a feedback law of the form u = f(e) such that the error converges to zero. There are various approaches to designing the feedback control law which are discussed as follows.
ω = ω r + c1θ e ,
where
K x > 0,
ω = ω r + v r ( K y y e + K θ sin θ e ), K y > 0, K θ > 0. (16) The stability analysis of the control law expressed in Eq. 16 states that if v r > 0, then the system is locally asymptotically stable (LAS). Furthermore, if v r and ω r are both continuous, v r , wr , K x and K θ are all bounded and if v& r and ω& r are both sufficiently small, then the system is locally uniformly asymptotically stable (LUAS). Controller based on approximate linearization: A linear controller designed was proposed based on the linearization at the equilibrium point (Oriolo, et al., 2001). The control law is given as: v = vd cos(θ d − θ ) + k1[cosθ ( xd − x) + sin θ ( yd − y )],
ω = ωd + k2sign (vd )[cosθ ( yd − y ) − sin θ ( xd − x)] + k3 (θ d − θ ). (17)
(21) (22)
This control law globally asymptotically stabilizes the origin, e = 0, which can be demonstrated using Lyapunov stability theory (Oriolo, et al., 2001). 5. RESULTS AND DISCUSSIONS The above discussed feedback control laws were modeled in Simulink and the desired trajectories for the leader robot were defined, x d (t ) = 100 sin( t / 20 ) , y d (t ) = 100 sin( t / 40 ).
(23)
The trajectory of Eq. 23 starts at the origin (0, 0). A full cycle is completed by the trajectory when T = 2π (40) = 251.32 sec . The case for one follower robot was considered. For the follower robot, the following parameters were chosen l lfd = 15
ϕ lfd = π / 3, d = 5, k a = k b = 1.
(24)
Both the leader and the follower robot are assumed to be at the origin (0, 0). To avoid division by zero in Eq. 10, the initial velocities were chosen for the leader robot to be
v d (0) = 0.01 m / s,
(25)
ω d (0) = 0 rad / s
In the first set of simulation, the desired trajectory of Eq. 23 was chosen. The feedback control strategies of Eq. 15, 16, 17 and 20 were modeled using MATLAB/Simulink. For the variables of Eq. 15, the values of c1 = 216.9, c2 = 1.355 and c3 = – 0.414 were selected. In Eq. 16, the values of Kx = 10, Ky = 0.0064 and Kθ = 0.16 were used. Finally, for the gains given in Eq. 18, 19 and 22, ζ = 0.6 and b = 5 were used. The error statistics for the leader robot using different feedback strategies are shown in Table 1.
Table 1 Showing the error statistic for the leader robot (m) Feedback Strategy Cascaded Systems Lyapunov Stability Approximate Linearization Nonlinear Controller
Mean 0.773
Variance Standard Deviation 0.6237 0.7920
0.7157
0.5464
0.7329
0.0227
0.0078
0.0883
0.0043
0.0007
0.0277
globally stabilizes the system more rapidly as compared to the other feedback strategies for the given trajectory. Thus the nonlinear control design is more preferred compared to the feedback control strategies for the given trajectories. 6. CONCLUSIONS AND FUTURE WORK This paper presents a framework for collaborative nonholonomic robots using MATLAB/Simulink. The collaborative robots communicate among each other using the Bluetooth piconet. Feedback control laws are implemented in each robot to track the correct trajectory. From the simulation results, the nonlinear feedback control strategy improves the system performance by efficiently minimizing the error. Furthermore, the nonlinear control strategy globally asymptotically stabilizes the system compared to the other feedback strategies. The proposed framework can be extended to complex robotic systems. In this paper the control problem has been addressed for the first order kinematic model of robots. For massive vehicles and at high speeds, however, the robots dynamics are necessary to be considered for more realistic approach. Similarly, current implementation of Bluetooth piconet does not support roaming protocol, which means the leadership is always static. To make the leadership more dynamic, a roaming and routing protocol for Bluetooth should be designed.
The actual trajectory for the leader and the follower using the nonlinear feedback strategy is shown in Fig 3. The actual separation bearing angle for the follower robot is shown in Fig 4. The actual linear and angular velocities of the leader robot using nonlinear feedback, which are transmitted to the follower robot using the Bluetooth, are shown in Fig 5 and 6, respectively. Another set of simulation was run for the desired trajectory expressed as x d (t ) = 100 sin( t / 20 ) , y d (t ) = 100 cos( t / 20).
(26)
The trajectory of Eq. 26 starts at (0, 100) and a full cycle is completed when T = 2π (20) = 125.66 sec . The same parameters for the variables were chosen as in the first simulation for the trajectory of Eq. 23. The robots are assumed to be at origin (0, 0). It was observed that the feedback strategy based on the cascaded systems theory and Lyapunov stability fails to eliminate the error to zero. Furthermore, these two strategies can not correctly track the trajectory because the robots initial starting position and the trajectory starting position is not the same. However the feedback strategy based on linear approximation and nonlinear design try to eliminate the error to zero. Based on the simulation results, it can be concluded that the error is directly proportional to the difference between the starting point of the robots and the trajectory. Similarly the nonlinear feedback strategy
Fig. 3. Actual trajectory of the leader robot using nonlinear feedback and follower robot using input-output feedback.
Fig. 4. Separation distance for the follower robot.
Fig. 5. Actual linear velocity input used by the leader robot using nonlinear feedback.
Fig. 6. Actual angular velocity input used by the leader robot using nonlinear feedback.
7. ACKNOWLEDGEMENT The authors would like to thank the Malaysian ministry of science, technology and innovation for partial funding of this project via IRPA grant 04-9902-0003 EA001. 8. REFERENCES Asada, H. and Slotine J.-J. E., (1985), Robot Analysis and Control, John Wiley & Sons, New York. Carlos, M. S., et al., (2006), Coordinated control of mobile robots based on artificial vision, International Journal of Computers, Communication & Control, Vol. 1 , No. 2, pp 85-94. Ferber J., (1999), Multi Agent Systems: An Introduction to Distributed Artificial Intelligence, Addison Wesley Longman. FIPA Agent Communication Language Specifications, 2002, [online]. [Accessed 15 April 2007]. Available from World Wide Web: Fujimori, A., et al., (2005), Distributed leaderfollower navigation of mobile robots, International Conference on Control and Automation, Hungary. Desai, J. P., Ostrowski, J. and Kumar, V., (1995), Controlling formations of multiple mobile robots,
IEEE International Conference on Robotics & Automation, Belgium. Kanayama Y., et al., (1991). A stable tracking control method for a non-holonomic mobile robots, IEEE International Workshop on Intelligent Robots and Systems, Japan. Kolmanovsky I., McClamroch N. H., (1995), Developments in nonholonomic control problems, IEEE Control Systems Magazine. Vol. 15 Issue 6, pp. 20–36. Lefeber, E., Jakubiak, J., K. Tcho´n and H. Nijmeijer (2001), Observer based kinematic tracking controllers for a unicycle-type mobile robot, IEEE International Conference on Robotics & Automation, Seoul, Korea. Mariottini, G. L., et al., (2005), Vision-based localization of leader-follower formations, IEEE Conference on Decision and Control and European Control Conference, Spain. Morrow, R., (2002), Bluetooth Operation and Use, McGraw-Hill New York. Oriolo G., et al, (2001), Control of wheeled mobile robotics: An experimental overview, Lecture Notes, Dipartimento di Informatica e Sistemistica, Universit`a degli Studi di Roma “La Sapienza”, Itlay Samson C., K. Ait-Abderrahim, (1991), Feedback control of a nonholonomic wheeled cart in cartesian space, IEEE International Conference on Robotics and Automation Sacramento, CA, pp 1136-1141. Stefan M., et al., (2004), A Real-time Image Recognition System for Tiny Autonomous Mobile Robots, Proceedings of the 10th IEEE Real-Time and Embedded Technology and Application Symposium.