742
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
[6] S. Hutchinson, G. D. Hager, and P. I. Corke, “A tutorial on visual servo control,” IEEE Trans. Robot. Autom., vol. 12, no. 5, pp. 651–670, Oct. 1996. [7] E. Malis, F. Chaumette, and S. Boudet, “2-1/2-d visual servoing,” IEEE Trans. Robot. Autom., vol. 15, no. 2, pp. 238–250, Apr. 1999. [8] P. I. Corke and S. A. Hutchinson, “A new partitioned approach to imagebased visual servo control,” IEEE Trans. Robot. Autom., vol. 17, no. 4, pp. 507–515, Aug. 2001. [9] G. D. Hager, “A modular system for robust positioning using feedback from stereo vision,” IEEE Trans. Robot. Autom., vol. 13, no. 4, pp. 582– 595, Aug. 1997. [10] C. C. Cheah, S. Kawamura, K. Lee, and S. Arimoto, “PID control of robotic manipulator with uncertain jacobian matrix,” in Proc. IEEE Int. Conf. Robot. Autom., Detroit, MI, May 1999, pp. 494–499. [11] C. C. Cheah, K. Lee, S. Kawamura, and S. Arimoto, “Asymptotic stability of robot control with approximate Jacobian matrix and its application to visual servoing,” in Proc. 39th IEEE Conf. Decis. Control, Sydney, N.S.W., Australia, Dec. 2000, pp. 3939–3944. [12] C. C. Cheah, M. Hirano, S. Kawamura, and S. Arimoto, “Approximate Jacobian control for robots with uncertain kinematics and dynamics,” IEEE Trans. Robot. Autom., vol. 19, no. 4, pp. 692–702, Aug. 2003. [13] C. C. Cheah, M. Hirano, S. Kawamura, and S. Arimoto, “Approximate Jacobian control with task-space damping for robot manipulators,” IEEE Trans. Autom. Control, vol. 49, no. 5, pp. 752–757, May 2004. [14] H. Yazarel and C. C. Cheah, “Task-space adaptive control of robotic manipulators with uncertainties in gravity regressor matrix and kinematics,” IEEE Trans. Autom. Control, vol. 47, no. 9, pp. 1580–1585, Sep. 2002. [15] C. C. Cheah, “Task-space pd control of robot manipulators: Unified analysis and duality property,” Int. J. Robot. Res., vol. 27, no. 10, pp. 1152– 1170, 2008. [16] C. C. Cheah, “Approximate Jacobian robot control with adaptive Jacobian matrix,” in Proc. 42nd IEEE Conf. Decis. Control, Maui, HI, Dec. 2003, pp. 5859–5864. [17] M. Galicki, “An adaptive regulator of robotic manipulators in the task space,” IEEE Trans. Autom. Control, vol. 53, no. 4, pp. 1058–1062, May 2008. [18] W. E. Dixon, “Adaptive regulation of amplitude limited robot manipulators with uncertain kinematics and dynamics,” IEEE Trans. Autom. Control, vol. 52, no. 3, pp. 488–493, Mar. 2007. [19] C. C. Cheah, C. Liu, and J. J. E. Slotine, “Adaptive tracking control for robots with unknown kinematic and dynamic properties,” Int. J. Robot. Res., vol. 25, no. 3, pp. 283–296, 2006. [20] C. C. Cheah, C. Liu, and J. J. E. Slotine, “Adaptive jacobian tracking control of robots with uncertainties in kinematic, dynamic and actuator models,” IEEE Trans. Autom. Control, vol. 51, no. 6, pp. 1024–1029, Jun. 2006. [21] D. Braganza, W. E. Dixon, D. M. Dawson, and B. Xian, “Tracking control for robot manipulators with kinematic and dynamic uncertainty,” Int. J. Robot. Autom., vol. 23, no. 2, pp. 117–126, 2008. [22] C. Canudas-De-Wit and J. J. E. Slotine, “Sliding observers for robot manipulators,” Automatica, vol. 27, no. 5, pp. 859–864, 1991. [23] C. Canudas-De-Wit and N. Fixot, “Adaptive control of robot manipulators via velocity estimated feedback,” IEEE Trans. Autom. Control, vol. 37, no. 8, pp. 1234–1237, Aug. 1992. ˚ [24] C. Canudas-De-Wit, K. J. Astrom, and N. Fixot, “Computed torque control via a nonlinear observer,” Int. J. Adaptive Control Signal Process., vol. 4, no. 6, pp. 443–452, 1990. [25] C. Canudas-De-Wit and N. Fixot, “Robot control via robust estimated state feedback,” IEEE Trans. Autom. Control, vol. 36, no. 12, pp. 1497–1501, Dec. 1991. ˚ [26] C. Canudas-De-Wit, N. Fixot, and K. J. Astrom, “Trajectory tracking in robot manipulators via nonlinear estimated state feedback,” IEEE Trans. Robot. Autom., vol. 8, no. 1, pp. 138–144, Feb. 1992. [27] A. F. Filippov, “Differential equations with discontinuous right-hand side,” Amer. Math. Soc. Transl., vol. 42, pp. 199–231, 1964. [28] R. Kelly, “Regulation of manipulators in generic task space: an energy shaping plus damping injection approach,” IEEE Trans. Robot. Autom., vol. 15, no. 2, pp. 381–386, Apr. 1999. [29] S. Arimoto, Control Theory of Nonlinear Mechanical Systems-A PassivityBased and Circuit-Theoretic Approach. Oxford, U.K.: Oxford Univ. Press, 1996. [30] F. L. Lewis, C. T. Abdallah, and D. M. Dawson, Control of Robot Manipulators. New York: Macmillan, 1993. [31] P. A. Ioannou and J. Sun, Robust Adaptive Control. Upper Saddle River, NJ: Prentice-Hall, 1996.
[32] Y. Nakamura and H. Hanafusa, “Inverse kinematic solutions with singularity robustness for robot manipulator control,” Trans. ASME J. Dyn. Syst., Meas., Control, vol. 108, pp. 163–171, 1986. [33] S. Chiaverini, “Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators,” IEEE Trans. Robot. Autom., vol. 13, no. 3, pp. 398–410, Jun. 1997. [34] S. R. Buss and J. S. Kim, “Selectively damped least squares for inverse kinematics,” J. Graph. Tools, vol. 10, no. 3, pp. 37–49, 2005. [35] P. Baerlocher and R. Boulic, “Task-priority formulations for the kinematic control of highly redundant articulated structures,” in Proc. IEEE/RSJ Int. Conf. Intell. Robots Syst., Victoria, B.C., Canada, 1998, pp. 323– 329. [36] N. Mansard and F. Chaumette, “Task sequencing for high level sensorbased control,” IEEE Trans. Robot., vol. 23, no. 1, pp. 60–72, Feb. 2007. [37] J. Peters, M. Mistry, F. Udwadia, J. Nakanishi, and S. Schaal, “A unifying framework for robot control with redundant dofs,” Auton. Robots, vol. 24, pp. 1–12, 2008.
Cooperative Load Transport: A Formation-Control Perspective He Bai and John T. Wen Abstract—We consider a group of agents collaboratively transporting a flexible payload. The contact forces between the agents and the payload are modeled as gradients of nonlinear potentials that describe the deformations of the payload. The load-transport problem is then treated in a similar fashion to the formation-control problem. Decentralized control laws are developed such that without explicit communication, the agents and the payload converge to the same constant velocity; meanwhile, the contact forces are regulated. Experimental results illustrate the effectiveness of our designs. Index Terms—Cooperative manipulators, force control, formation control, multiagent systems.
I. INTRODUCTION Motion coordination and cooperative control have received a lot of attention over the past few years. The main challenge in cooperative Manuscript received August 26, 2009; revised February 4, 2010; accepted May 28, 2010. Date of publication July 8, 2010; date of current version August 10, 2010. This paper was recommended for publication by Associate Editor T. Murphey and Editor L. Parker upon evaluation of the reviewers’ comments. This work was supported in part by the Center for Automation Technologies and Systems, Rensselaer Polytechnic Institute, under a block grant from the New York State Office of Science, Technology, and Academic Research. The work of J. T. Wen was supported in part by the Outstanding Overseas Chinese Scholars Fund of Chinese Academy of Sciences under Grant 2005-1-11. H. Bai is with the Department of Mechanical Engineering, Northwestern University, Evanston, IL 60208 USA (e-mail:
[email protected]). J. T. Wen is with the Department of Electrical, Computer, and Systems Engineering, Rensselaer Polytechnic Institute, Troy, NY 12180 USA (e-mail:
[email protected]). This paper has supplementary downloadable material available at http://ieeexplore.ieee.org. provided by the author. The material includes three videos that demonstrate our typical experiments. The size of the material is 44.1 MB. Contact
[email protected] for further questions about this work. Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TRO.2010.2052169
1552-3098/$26.00 © 2010 IEEE
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
control is to design a decentralized control law that depends on local information to achieve a global behavior. A number of studies, e.g., [1]–[7], have successfully proposed distributed control laws to achieve emergent group behaviors, such as consensus, flocking, and schooling. The local information between agents, such as relative distances, is usually obtained through explicit information flows, including sensor measurements [8] and direct communication [9], [10]. While such explicit information flows exist in many cooperative-control applications, there are situations where the information flows are implicit. For example, suppose that several people move a table and that only one person knows where to go. Then, even without explicitly talking to or seeing each other, those people are able to adjust their velocities and forces and finally succeed in moving the table toward the target. In this example, the communication is implicit, and people receive the information (e.g., where to go, how fast to go) by feeling the contact forces and the trend where the table is going. Through everybody adjusting their forces and velocities, the whole group moves toward the goal. Motivated by this example, we consider a group of agents handling a flexible payload. These agents are modeled as point robots with double-integrator dynamics. As the agents move, the payload may be squeezed or stretched, thereby generating contact forces to the agents. The contact forces between the agents and the payload are modeled as gradients of nonlinear potentials that describe the deformations of the payload. Because all the agents are attached to the payload, the contact forces can be considered as implicit communication between the agents with the payload acting as the “medium.” Our objective is to employ this implicit communication to design decentralized control laws such that the contact forces are regulated at some setpoints and that the agents and the payload move with the same constant velocity in the limit. As a starting case to investigate, we assume that the deformations of the payload are so small that the motion of the payload can be approximated as a rigid body. This assumption is reasonable when a rigid load is surrounded with bumpers or elastic materials. Another illustration of this assumption is multiple grippers grasping a rigid load, where the grippers possess compliance from installed flexible mechanisms. There exists a considerable amount of literature on the control design for the load-transport problem, including the motion/force control [11], [12], event-based control scheme [13], caging-without-force control [14], and leader/follower comply mechanism [15], [16]. Tang et al. [17] employed screw theory to examine a system of two nonholonomic wheeled mobile manipulators holding a common load. Multigripper grasping was considered in [18], where one of the grippers is rigid and the others are flexible with built-in linear springs, and stabilization control laws were developed to achieve both position and force control. Sugar and Kumar [19] considered transporting large (possibly flexible) objects as an impedance-control problem and performed experiments using multiple mobile robots with manipulators to validate their controllers. Tanner et al. [20] studied modeling and manipulating totally deformable objects, and the solution is centralized and is based on finite-element model. In contrast with the existing literature, we attempt to solve the load-transport problem in a similar way to the formation-control problem. We note that formation-control designs usually employ virtual attractive/replusive-force feedback between the agents, while for our load-transport problem, the contact forces play the role of physicalforce feedback between the agents and the payload. With this idea in mind, we consider the case, where a constant desired velocity is available to all the agents. We propose a decentralized controller that guarantees the force regulation and the velocity convergence of the agents and the payload. This controller, which consists of an internalvelocity feedback and an external-force feedback from the payload, exhibits a similar structure to existing formation-control designs, such
743
Fig. 1. Multiple agents are attached to a common flexible load. Initially, the payload is undeformed, and agent i is attached to the point a i . If the load is deformed by agent i, the position of agent i, i.e., xi , is different from a i , and the deformation is approximated by zi = xi − a i .
as in [1] and [6]. Realizing this similarity allows us to apply formationcontrol techniques to the load-transport problem. For example, when no desired velocity is predesigned, we employ the adaptive design [21] from formation-control literature and augment the decentralized controller with an integral control term. The resulting control law recovers the traditional integral-force control and ensures the agents to achieve the same constant velocity and the force regulation without explicit communication. The aim of this paper is to address, in the same framework, the connection between the formation-control problem, where the interaction force is virtual, with the multiagent load-carrying problem, where the interaction force is physical. We have only considered the translational problem. The extension to the rotational case is currently under investigation. Compared with [22], the contribution of this paper lies in the experimental results, where we verify our control laws with two Programmable Universal Machine for Assembly (PUMA) 560 arms holding a soccer ball. The subsequent sections are organized as follows. We formulate our problem in Section II. In Section III, we study the situation where a desired velocity is available to each agent and propose a decentralized control law that achieves our objective. An adaptive control law is developed in Section IV, when no predesigned desired velocity is available. Experimental results are presented in Section V. Conclusions and future work are discussed in Section VI. II. PROBLEM FORMULATION Let us consider N planar agents holding a common flexible load, as shown in Fig. 1. Each agent is modeled as a point robot. Let us suppose that the load is initially undeformed, and that agent i is attached to the load at the point ai , i.e., xi (0) = ai (0) = xc (0) + ri , where xc ∈ R2 , and xi ∈ R2 are the inertial positions for the center of mass of the load and agent i, and ri is a fixed vector in the inertial frame. Assuming that the initial orientation of the load θc is 0, we define
ai (t) := xc (t) + R(θc )ri ,
R(θc ) =
cos θc
− sin θc
sin θc
cos θc
(1)
whose kinematics are given by
a˙ i = x˙ c + θ˙c
− sin θc
− cos θc
cos θc
− sin θc
ri .
(2)
744
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
Note that ai (t) represents the position where agent i would be attached as if the payload were undeformed at time t. As the agents move, however, the flexible payload may be squeezed or stretched, and therefore, xi (t) = ai (t). The deformation of the payload, which is approximated by (3) zi = xi − ai , i = 1, . . . , N
at a setpoint fid . Because the load is eventually moving with a constant velocity, fid ’s are subject to the following constraint:
generates a reaction force fi to agent i. We assume that the reaction force fi is the gradient of a positive-definite potential function Pi (zi ), i.e., (4) fi = ∇Pi (zi ).
The setpoints fid should also be chosen so that the contact forces fi have the desired properties, such as force closure. This requires the knowledge of the payload geometry and the grasping points. In the following sections, we consider our control design in two situations: First, a constant desired velocity v d is predesigned and available to each agent; second, no v d is predesigned.
Note that when zi = 0, the payload is not deformed by agent i. Therefore, no force would be generated to agent i, which implies that Pi (zi ) satisfies the following: Pi (zi ) = 0 ⇐⇒ zi = 0
(5)
∇Pi (zi ) = 0 ⇐⇒ zi = 0.
(6)
We further assume that the deformations are small enough so that the load can be approximated as a rigid body. This assumption is reasonable when a rigid object is surrounded by deformable materials (e.g., bumper), and the agents are attached to those materials. Our model of the payload is different from the one given in [20], since we assume the payload to be a partially flexible object that deforms only around the grasping points, while in [20], the deformations of all particles on the flexible object are considered. The dynamics of the agents and the load, which are restricted to purely translational motion, are given by ¨ i = F i − fi , mi x
i = 1, . . . , N
(7)
N
Mc x ¨c =
fi
(8)
where mi and Mc are the masses of agent i and the load, respectively, Fi is the applied force to agent i, and fi is the contact force, which is defined in (4). The restriction to pure translational motion is a simplifying assumption, which allows us to illustrate, in a basic form, the connection between the formation-control problem and the load-transport problem. Indeed, if the grasp is rigid, the agents are capable of exerting torques to the payload. Then, the dynamics for the orientation of the payload become N i= 1
fid = 0.
(11)
i= 1
III. DECENTRALIZED CONTROL WITH v d PREDESIGNED We note from (4) that the reaction force fi depends on the deformation zi . If zi can be regulated to some desired state, fi would also be maintained accordingly. To this end, we assume that for a given fid , there exists a locally unique deformation zid , such that fid = ∇Pi (zid )
(12)
∇2 Pi (zid ) > 0.
(13)
and Assumption (13) is satisfied by linear spring-force model as well as certain classes of nonlinear models, such as fi = bi |zi |2 zi , with bi > 0. Note that now achieving a desired contact force fid is equivalent to driving the deformation zi in (3) to zid . This is similar to the formationcontrol problem, where the relative positions between agents are driven to some desired values. Proposition 1: Let us consider the decentralized control law given by (14) Fi = −Γi (x˙ i − v d ) + fid where Γi = ΓTi > 0. The equilibrium E, which is given by
i= 1
Ic θ¨c =
N
r i c × fi +
N
τi
(9)
i= 1
where Ic is the inertia of the payload, τi is the torque transmitted to the payload from agent i, ri c = xi − xc , and ri c × fi = rixc fiy − riyc fix
(10)
in which ri c = [rixc , riyc ]T , and fi = [fix , fiy ]T . Our assumption on pure translational motion means that τi ’s may be chosen to stabilize θc to a constant. The design of τi may require the information of ri c and θc . Once θc is stabilized, the remaining motion would be only translational. Therefore, the formulation in this paper only reflects the translational part of the load-transport problem. Our control objective is to design Fi in a decentralized way such that all the agents and the payload converge to the same constant velocity, while the contact forces on the load are regulated, i.e., fi maintained
E = {(x˙ i , x˙ c , fi )|x˙ i = v d ,
x˙ c = v d ,
and
fi = fid }
(15)
is asymptotically stable. Dynamics (7) with the proposed control (14) take the form ¨i = −Γi (x˙ i − v d ) + fid − fi mi x
(16)
which consists of an internal-motion feedback that drives the agent’s velocity to v d and an external-force feedback that regulates the contact force. In the formation-control literature, the external feedback is usually designed as virtual potential reaction forces, which are based on the relative distances between the agents [1], [6], whereas in our problem, the reaction forces between the agents and the payload follow directly from the physical potentials induced by the deformations. The closed-loop systems, i.e., (7), (8), and (14), can be considered as a cooperative system of N + 1 agents, if the payload is treated as an additional agent. The interactions between the N + 1 agents then display a star topology with the payload at the center, as shown in Fig. 2. Therefore, controlling the forces (and, thus, the deformations) between the agents and the payload simultaneously guarantees that the relative positions between the agents are maintained tightly. We also note that the payload is a passive agent that has no direct control input. Without the v d information, this passive agent still achieves v d in the limit. Similar result has been obtained in [21], where an estimation scheme was proposed for those agents without the v d information to reconstruct v d from the local interactions. In our case, the payload recovers the v d information from the contact forces fi , which are, indeed, the local interactions with the other agents.
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
745
IV. DECENTRALIZED CONTROL WITHOUT v d When v d is not preassigned to each agent, we follow the adaptive design in [21] and develop an adaptive control with which each agent estimates the group velocity. We now define vˆid as the velocity estimate for agent i and propose the following update law for vˆid : vˆ˙ di = Λi (fid − fi )
Fig. 2. Implicit communication topology between three agents and the payload.
Proof: Let us consider the following energy-motivated Lyapunov function: V =
N
+
N 1 2
ξiT mi ξi + ξcT Mc ξc
(17)
i= 1
where and
fid
Fi = −Γi (x˙ i − vˆid ) + mi vˆ˙ di + fid
E ∗ = {(x˙ i , x˙ c , fi , vˆid)|x˙ i = v¯,
ξi = x˙ i − v d
vˆid
(23)
and present the following proposition. Proposition 2: Let us consider the decentralized control laws in (22) and (23). The equilibrium E ∗ , which is given by
[Pi (zi ) − Pi (zid ) − (fid )T (zi − zid )]
i= 1
(22)
> 0. Note that stops updating when ≡ fi , in which Λi = i.e., the contact force is regulated at the desired setpoint. This means that zi remains constant, and thus, agent i and the payload have the same velocity. If all the agents have the same velocity as the payload, they move with the same velocity. We next modify the design in (14) as ΛTi
ξc = x˙ c − v d .
fi = fid} (24) is asymptotically stable, where v¯ ∈ R2 is characterized as the weighted average of the initial payload velocity x˙ c (0) and the initial velocity estimates vˆid (0), i = 1, . . . , N :
(18)
Under the assumption that (13) is satisfied, the first term in (17) is positive definite. Then, the time derivative of V yields
v¯ =
N
x˙ c = v¯,
vˆid = v¯,
−1
Λ−1 i
+ Mc
Mc x˙ c (0) +
i= 1
N
and
Λ−1 ˆid (0) i v
.
(25)
i= 1
Expanding dynamics (7) with the controls (22) and (23), we obtain V˙ =
N
N
(fi − fid )T z˙i +
i= 1
#
ξiT mi x ¨i + ξcT Mc x ¨c .
(19)
¨i = −Γi (x˙ i − mi x
vˆdi (0))
+ mi vˆ˙ di + fid − fi + Γi Λi
$
i= 1
(fid − fi )
%&
'
integral-force control
From (2), (3), and the assumption θ˙c = 0, the kinematics of zi are given by (20) z˙i = x˙ i − a˙ i = x˙ i − x˙ c . We next rewrite (19) from (7), (8), (11), (14), and (20) as V˙ =
N
(fi − fid )T (x˙ i − x˙ c ) +
N
i= 1
i= 1
N
=
(fi − fid)T (ξi − ξc) +
N
i= 1
ξiT Γi ξi + ξc
i= 1
=−
N
N
N
fi
N
ξiT (−Γi ξi + fid − fi) + ξcT
Fi = −Γi ξi + mi vˆ˙ di + fid
(27)
ξi = x˙ i − vˆid
(28)
where
i= 1
N
i= 1
=−
ξiT (Fi − fi ) + ξcT
(26) which is of the integral-force-feedback form [11], [12]. Such an integral-force control has been shown in [12] to be robust with respect to small time delay in the force measurements. Proof: We first rewrite (23) as
fi
and
vˆid
is updated as in (22). It follows from (26) that mi ξ˙i = −Γi ξi + fid − fi .
i= 1
We then choose the following Lyapunov function: fid
i= 1
V1 =
N
[Pi (zi ) − Pi (zid ) − (fid )T (zi − zid )] +
i= 1
ξiT Γi ξi ≤ 0
(29)
(21)
i= 1
which implies the stability of the equilibrium E. To conclude the asymptotic stability, we apply the LaSalle invariance principle by investigating the largest invariant set M, where V˙ = 0, i.e., ξi = 0. From (18), we note that ξi = 0 implies that x˙ i = v d . We ¨i = 0, which leads to Fi = fi from further obtain from ξ˙i = 0 that x (7). Thus, it is clear from (14) that fid = fi . We now show that on M, x˙ c = v d . To see this, we note that fid = fi implies that zi = zid . Since zid is constant, we have z˙i = 0 on M, i.e., from (3), x˙ i = a˙ i . Because we only consider the translational motion, and because x˙ i = v d on M, we conclude that a˙ i = x˙ c = v d .
+
N 1 T ξi m i ξi 2 i= 1
N 1 1 d (x˙ c − v¯)T Mc (x˙ c − v¯) + (ˆ vi − v¯)T Λ−1 vid − v¯) i (ˆ 2 2 i= 1
(30) whose time derivative is given by V˙ 1 = −
N
(fid − fi )T z˙i +
i= 1
N
ξiT mi ξ˙i
i= 1
N
+ (x˙ c − v¯)T Mc x ¨c +
i= 1
(fid − fi )T (ˆ vid − v¯).
(31)
746
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
Noting that z˙i = ξi + vˆid − x˙ c
(32)
we rewrite (31) from (11), (27), and (29) as V˙ 1 = −
N
(fid − fi )T (ξi + vˆid − x˙ c ) +
i= 1
(fid − fi )T (ˆ vid − v¯)
i= 1
N
+ (x˙ c − v¯)T
N
fi +
i= 1
N
ξiT (−Γi ξi + fid − fi )
i= 1
N
=−
ξiT Γi ξi ≤ 0
(33)
i= 1
which implies the stability of the equilibrium E ∗ . We again apply the LaSalle invariance principle to investigate the largest invariant set M∗ . On M∗ , V˙ 1 = 0 means ξi = 0, and thus, ξ˙i = 0, which further implies from (29) that fi = fid . Then, from (22), vˆ˙ di = 0. It follows from x ¨i = vˆ˙ di = 0 that x˙ i is constant on M∗ . Since f˙i = f˙id = 0, and z˙i = x˙ i − x˙ c , we conclude x˙ i = x˙ c ∀i, which means that all the agents and the payload have the same constant velocity. Noting from (28) and ξi = 0, we further obtain x˙ c = vˆid , i = 1, . . . , N . Next, from (8) and (22), we compute
Mc x˙ c (t) =
# t N 0
and
#
Proof (Outline): Let us consider the following Lyapunov function: Va =
N
fi (s)ds + Mc x˙ c (0)
[Pi (zi ) − Pi (zid ) − (fid )T (zi − zid )] +
i= 1
+
N 1 T ξi m i ξi 2 i= 1
N 1 1 d T −1 d (x˙ c − v d )T Mc (x˙ c − v d ) + (˜ vi ) Λi v˜i 2 2
(40)
i= 2
(34)
where v˜id = vˆid − v d , ξ1 = x˙ 1 − v d , and ξi , i = 2, . . . , N are defined in (28), and perform a Lyapunov analysis similar to the proof of Proposition 2.
i= 1
t
Λi (fid − fi (s))ds + vˆid (0).
vˆid (t) =
Fig. 3. Experiment testbed of two PUMA 560 arms. Mounted on the wrist of each arm is a 6-DOF force/torque sensor.
(35)
0
V. PRELIMINARY EXPERIMENTS
We rewrite (35) as
# vid (t) Λ−1 i (ˆ
−
vˆid (0))
A. Experiment Hardware
t
fid
=
− fi (s)ds
(36)
(fid − fi (s))ds
(37)
0
and note from (11), (34), and (36) that N
Λ−1 vid (t) − vˆid (0)) = i (ˆ
i= 1
# t N 0
i= 1
= −Mc (x˙ c (t) − x˙ c (0)).
(38)
Because on M∗ , x˙ c and vˆid are equal and constant, we obtain from (38) that x˙ c = x˙ i = vˆid = v¯. A special example of the designs (22) and (23) is when only one agent, say agent 1, has the v d information. In this case, agent 1 can choose to shut off the estimation [see (22)] by selecting Λ1 = 0 and letting vˆ1d (0) = v d . This leads to the same controller in (14) for agent 1. A simple calculation from (25) shows limΛ 1 →0 v¯ = v d , which means that the other agents asymptotically recover the v d information, and the group will eventually move with the velocity v d . The following corollary summarizes this result. Corollary 1: Let us suppose that agent 1 has the v d information and implements (14), while the other agents apply controls (22) and (23). Then, the equilibrium Ea∗ , which is given by Ea∗ ={(x˙ i , x˙ c , fi , vˆid )|x˙ i = v d , x˙ c = v d , vˆid = v d , and fi = fid } is asymptotically stable.
(39)
Our experimental testbed, as shown in Fig. 3, consists of two PUMA 560 robots, each with a 6-degree-of-freedom (DOF) force/torque sensor mounted on the wrist. Two computers running xPC target perform the real-time control of the two robots, and the data acquisition is achieved using Peripheral Component Interconnect (PCI) interface boards. These two xPC computers run at an update rate of 1 kHz and only perform low-level tasks. There is a user-interface host computer that runs MATLAB and communicates with the control computers through an Ethernet cable using User Datagram Protocol (UDP). This configuration allows high-level processing and control to be carried out on the host computer, while the control computer implements the low-level control loop. B. Experiment Implementation Because the end-effector of the PUMA arm is of 6 DOFs, we consider it as a fully actuated agent. To simplify the implementation and reduce the effects due to the uncertainty of the arm inertia, we choose to implement our control laws on the kinematic level rather than on the dynamic level. Motivated by a standard singular perturbation analysis of (16) for small mi , we obtain the following controller by setting the right-hand side of (16) to 0 (i.e., setting x ¨i = 0), and solving for x˙ i : x˙ i = Ki (fid − fi ) + v d ,
i = 1 and 2
(41)
where Ki > 0, and x1 and x2 are the positions of the end-effectors of the right and the left PUMA arms. This controller can also be justified by treating the payload as a virtual massless spring and
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
747
Fig. 6. Trajectories for both end-effectors when they are moving forward with v d = [0.02 0]T m/s.
Fig. 4. (Top view) First two stages of the experiments. (a) Two manipulators first move toward the ball. Once the end-effectors reach the ball, we implement our control (41), with v d = [0 0]T to achieve the static holding in (b). The left (right) PUMA refers to the left (right) manipulator in this figure.
Fig. 5. Force measurements along the squeeze direction when the manipulators move in the x-direction.
invoking a Lyapunov analysis of (41) with the Lyapunov function 2 [Pi (zi ) − Pi (zid ) − (fid )T (zi − zid )]. Likewise, for the adaptive i= 1 design, we implement x˙ i = Ki (fid − fi ) + vˆid
(42)
and keep the update law of vˆid the same as (22). Once the force measurement fi is available, x˙ i is computed from (41) or (42) and transformed to the joint velocities using the pseudoinverse of the Jacobian matrix. The joint positions for the next step are calculated from the joint velocities and tracked by a low-level proportional–integral–differential (PID) controller.
Fig. 7. Estimation of v d . (a) Estimate of v d converges to its true value, while (b) the squeeze-force measurements are well maintained at ±10 N.
Our experiments are performed in the following steps. Approaching: As seen in Fig. 4(a), the two manipulators approach a lightweight soccer ball, which is fed by a person, along the y-direction. In this stage, we tune the positions and the orientations of
748
Fig. 8. When the left PUMA does not have the v d information and the adaptive design (42) is not implemented, the velocities of the end-effectors and v d in (a) show that the tracking of v d is not achieved, and the squeeze-force measurements in (b) show that the contact forces cannot be maintained at the desired ones.
the end-effectors so that they are aligned on the same line. Therefore, their motions will be restricted to the same plane. We also ensure that the end-effectors are normal to the contact surfaces and that the line connecting both end-effectors approximately passes through the center of the ball. This guarantees no rotational motion when the manipulators squeeze the ball in the next stage. Static holding: As the end-effectors reach the ball, as shown in Fig. 4(b), we turn on our controllers in (41) with v d = [0 0]T , which implies that the ball will be held statically and squeezed. The squeeze forces are along the y-direction, and their desired setpoints are chosen to be ±10 N, i.e., f1d = [0 −10]T N, and f2d = [0 10]T N, thereby satisfying (11). These large desired squeeze forces help to maintain the grasp and reduce the rotational motion of the ball. In case that the squeeze force drops below a certain amount (i.e., 6 N in the experiments), we increase the feedback gain Ki to quickly drive fi back. To ensure that the end-effector does not slip on the contact surface, we discard the force measurements along the x-direction, which means that only the squeeze forces are controlled. Once the squeeze forces reach the desired setpoints, we start to move the ball with several basic maneuvers, as discussed below, and evaluate the performances of our proposed controllers in these cases.
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
Fig. 9. (a) Squeeze forces cannot be maintained at the desired value due to the circular motion of the end-effectors. (b) Trajectories of the end-effectors show that the tracking of the desired velocity is not well achieved.
Moving the object: In this step, we examine our proposed controllers in the scenarios of moving with v d available, estimation of v d , and circular motion. In all cases, we set Ki = K = 0.001. 1) Moving With v d Available: After the stage of static holding, we move the object along the x-direction by assigning v d = [0.02 0]T m/s to each end-effector. The force measurements from the sensors on the wrists are shown in Fig. 5. The squeeze forces are maintained at ±10 N at the stage of static holding (i.e., 17.5–30 s) and oscillate more around ±10 N when the end-effectors start moving (i.e., 30–47.5 s). This is partially due to the dynamic effects of the low-level PID tracking controller that we ignored in the implementation. The trajectories of both end-effectors from 30 to 47.5 s are shown in Fig. 6, where no significant rotational motion is observed. Moreover, from Fig. 6, we can compute the approximate average velocities along the x-direction as 0.02 m/s, which is the same as v d . 2) Estimation of v d : We now examine our adaptive design, i.e., (42). We assign v d = −[0 0.005] m/s to the right PUMA, while the left PUMA has no v d information. Therefore, the left PUMA needs to implement the adaptive design (42) to estimate v d . The initial estimate vˆ1d (0) is chosen to be 0. Since the motion is along y-direction, which is the squeeze direction, no rotational motion is generated. The experimental results are shown in Fig. 7, where the estimate from the left PUMA converges to −0.005 m/s; meanwhile, the squeeze forces are well maintained at ±10 N.
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
749
3) Circular Motion: Although the nonadaptive controller (41) is restricted to the case of constant desired velocity, we test it when the end-effectors move in a circular motion. In this case, we choose f1d = [0 −15]T N and f2d = [0 15]T N and show both the force measurements and the trajectories of the two end-effectors in Fig. 9. Fig. 9(a) illustrates that once the end-effectors start circling (i.e., after 40 s), the squeeze forces cannot be maintained at ±15 N due to the periodic motion. The tracking of the desired velocity is not well achieved as the trajectories in Fig. 9(b) are not a perfect circle. This is because the force and the motion control are coupled all the time. To improve the tracking performance, we add deadzone for the force error fid − fi so that the force and motion control are decoupled when fi is around fid . The experimental results in Fig. 10 illustrate the improvement of the tracking of the circles at the price of more fluctuating contact forces in the deadzones. VI. CONCLUSION AND FUTURE WORK We studied a motion-coordination problem, where a group of agents move a flexible payload. The contact forces, which describe the relative information between the agents and the payload, build up implicit communication in the group. When the desired constant velocity is available to each agent, we developed a decentralized controller that achieves the convergence to the desired velocity and the force regulation. We also considered the situation where the desired velocity is not available. We proposed an adaptive control that recovers the nonadaptive results. Both nonadaptive and adaptive control laws were compared with existing formation-control designs. Preliminary experiments were performed and validated our designs. Future work will pursue the theory for tracking time-varying desired velocity and the extension to rotational motion with only contact force (e.g., point contact with friction). Experiments with different payloads and with more manipulators will also be considered. Fig. 10. With deadzones added to the force control, (a) shows that the squeeze force measurements fluctuate more than those in Fig. 9(a) while the trajectories of the end-effectors in (b) show better tracking performance than Fig. 9(b).
As a comparison, we implement the nonadaptive control law (41) with v d = −[0 0.005] m/s for the right PUMA, and v d = [0 0] m/s for the other. Since they do not have the same v d information, the experimental results in Fig. 8 show the existence of steady-state errors in both the desired velocity tracking and the force regulation when the end-effectors are moving (i.e., after 30 s). In fact, along the y direction, the two end-effectors are governed by x˙ 2 , y = K(f2d, y − f2 , y ) + 0 (43) where the subscript y denotes the y-component of each vector. Rewriting (43) as
ACKNOWLEDGMENT The authors would like to thank the anonymous reviewers for their constructive comments. REFERENCES [1] [2] [3]
x˙ 1 , y = K(f1d, y − f1 , y ) − 0.005,
f1d, y
x˙ 1 , y = K
$
f¯1d, y
x˙ 2 , y = K
0.005 − −f1 , y %& 2K '
f2d, y
$
0.005 + −f2 , y %& 2K '
[4] [5]
− 0.0025
(44) [6] [7]
− 0.0025
(45) [8]
f¯2d, y
we see that due to the difference in the desired velocities, the desired setpoints fid, y are shifted to new setpoints f¯id, y . It is easy to calculate f¯id, y as ±12.5 N, which matches our results shown in Fig. 8(b).
[9]
¨ P. Ogren, E. Fiorelli, and N. E. Leonard, “Cooperative control of mobile sensor networks: Adaptive gradient climbing in a distributed network,” IEEE Trans. Autom. Control, vol. 49, no. 8, pp. 1292–1302, Aug. 2004. R. Olfati-Saber and R. M. Murray, “Consensus problems in networks of agents with switching topology and time-delays,” IEEE Trans. Autom. Control, vol. 49, no. 9, pp. 1520–1533, Sep. 2004. H. Tanner, A. Jadbabaie, and G. Pappas, “Flocking in fixed and switching networks,” IEEE Trans. Autom. Control, vol. 52, no. 5, pp. 863–868, May 2007. A. Jadbabaie, J. Lin, and A. S. Morse, “Coordination of groups of mobile autonomous agents using nearest neighbor rules,” IEEE Trans. Autom. Control, vol. 48, no. 6, pp. 988–1001, Jun. 2003. R. S. Smith and F. Y. Hadaegh, “Closed-loop dynamics of cooperative vehicle formations with parallel estimators and communication,” IEEE Trans. Autom. Control, vol. 52, no. 8, pp. 1404–1414, Aug. 2007. M. Arcak, “Passivity as a design tool for group coordination,” IEEE Trans. Autom. Control, vol. 52, no. 8, pp. 1380–1390, Aug. 2007. H. Bai, M. Arcak, and J. T. Wen, “ Rigid body attitude coordination without inertial frame information,” Automatica, vol. 44, no. 12, pp. 3170–3175, Dec. 2008. H. Bai, K. D. Chopin, J. Wason, and J. T. Wen, “Experimental verification of formation control with distributed cameras,” in Proc. 48th IEEE Conf. Decis. Control, Shanghai, China, Dec. 2009, pp. 2996–3001. S. Mastellone, D. M. Stipanovi´c, C. R. Graunke, K. A. Intlekofer, and M. Spong, “Formation control and collision avoidance for multi-agent non-holonomic systems: Theory and experiments,” Int. J. Robot. Res., vol. 27, no. 1, pp. 107–126, 2008.
750
IEEE TRANSACTIONS ON ROBOTICS, VOL. 26, NO. 4, AUGUST 2010
[10] N. Sorensen and W. Ren, “A unified formation control scheme with a single or multiple leaders,” in Proc. Amer. Control Conf., New York, NY, Jul. 2007, pp. 5412–5418. [11] G. Montemayor and J. T. Wen, “Decentralized collaborative load transport by multiple robots,” in Proc. IEEE Int. Conf. Robot. Autom., Barcelona, Spain, Apr. 2005, pp. 372–377. [12] J. T. Wen and K. Kreutz-Delgado, “Motion and force control of multiple robotic manipulators,” Automatica, vol. 28, no. 4, pp. 729–743, 1992. [13] K. Munawar and M. Uchiyama, “Experimental verification of distributed event-based control of multiple unifunctional manipulators,” in Proc. IEEE Int. Conf. Robot. Autom., 1999, pp. 1213–1218. [14] G. A. S. Pereira, V. Kumar, and M. F. M. Campos, “Decentralized algorithms for multirobot manipulation via caging,” Int. J. Robot. Res., vol. 23, pp. 783–795, 2002. [15] Y. Zheng and Y. Luh, “Control of two coordinated robots in motion,” in Proc. 25th IEEE Conf. Decis. Control, Ft. Lauderdale, FL, Dec. 1985, pp. 334–337. [16] Y. Hirata, Y. Kume, Z. Wang, and K. Kosuge, “Decentralized control of multiple mobile manipulators handling a single object in coordination,” in Proc. IEEE Int. Conf. Intell. Robots Syst., Oct. 2002, pp. 2758–2763. [17] C. P. Tang, R. M. Bhatt, M. Abou-Samah, and V. Krovi, “Screw-theoretic analysis framework for cooperative payload transport by mobile manipulator collectives,” IEEE/ASME Trans. Mechatron., vol. 11, no. 2, pp. 169– 178, Apr. 2006. [18] D. Sun and J. K. Mills, “Manipulating rigid payloads with multiple robots using compliant grippers,” IEEE/ASME Trans. Mechatron., vol. 7, no. 1, pp. 23–34, Mar. 2002. [19] T. G. Sugar and V. Kumar, “Control of cooperating mobile manipulators,” IEEE Trans. Robot. Autom., vol. 18, no. 1, pp. 94–103, Feb. 2002. [20] H. G. Tanner, K. J. Kyriakopoulos, and N. I. Krikelis, “Modeling of multiple manipulators handling a common deformable object,” J. Robot. Syst., vol. 15, no. 11, pp. 599–623, 1998. [21] H. Bai, M. Arcak, and J. T. Wen, “Adaptive design for reference velocity recovery in motion coordination,” Syst. Control Lett., vol. 57, no. 8, pp. 602–610, 2008. [22] H. Bai and J. T. Wen, “Motion coordination through cooperative payload transport,” in Proc. Amer. Control Conf., St. Louis, MO, Jun. 2009, pp. 1310–1315.
Sliding-Mode Velocity Control of Mobile-Wheeled Inverted-Pendulum Systems Jian Huang, Zhi-Hong Guan, Takayuki Matsuno, Toshio Fukuda, and Kosuke Sekiyama Abstract—There has been increasing interest in a type of underactuated mechanical systems, mobile-wheeled inverted-pendulum (MWIP) models, which are widely used in the field of autonomous robotics and intelligent vehicles. Robust-velocity-tracking problem of the MWIP systems is investigated in this study. In the velocity-control problem, model uncertainties accompany uncertain equilibriums, which make the controller design become more difficult. Two sliding-mode-control (SMC) methods are proposed for the systems, both of which are capable of handling both parameter uncertainties and external disturbances. The asymptotical stabilities of the corresponding closed-loop systems are achieved through the selection of sliding-surface parameters, which are based on some rules. There is still a steady tracking error when the first SMC controller is used. By assuming a novel sliding surface, the second SMC controller is designed to solve this problem. The effectiveness of the proposed methods is finally confirmed by the numerical simulations. Index Terms—Mobile-wheeled inverted pendulum (MWIP), robust control, sliding-mode control (SMC), stability, underactuated systems.
I. INTRODUCTION Recently, many investigations have been devoted to problems of controlling mobile-wheeled inverted pendulum (MWIP) models, which have been widely applied in the field of autonomous robotics and intelligent vehicles [1]–[12]. The MWIP models are not only of theoretical interest but are also of practical interest. Many practical systems have been implemented that were based on the MWIP models, such as the JOE [7], the Nbot [8], the Legway [9], the B2 [10], the Segway [11], etc. Among these applications, the Segway PT has been a popular personal transporter, since it was invented in 2001. Such systems are characterized by the ability to balance on its two wheels and spin on the spot. This additional maneuverability allows them to easily navigate on various terrains, turn sharp corners, and traverse small steps or curbs. In addition, the compact structure design allows drivers to access most places that can only be accessed by walkers in the past. Moreover, people can drive such vehicles to travel short distances in a small area instead of using cars or buggies that are more pollutive. From the theoretical point of view, the MWIP models have attracted much attention in the field of control theory and engineering because Manuscript received January 25, 2010; revised June 9, 2010; accepted June 9, 2010. Date of publication July 23, 2010; date of current version August 10, 2010. This paper was recommended for publication by Associate Editor T. Murphey and Editor J.-P. Laumond upon evaluation of the reviewers’ comments. This work was supported by the National Natural Science Foundation of China under Grant 60603006 and Grant 60834002 and by the Doctoral Foundation of Ministry of Education of China under Grant 20090142110039. J. Huang and Z.-H. Guan are with the Department of Control Science and Engineering, Huazhong University of Science and Technology, Wuhan 430074, China. They are also with the Key Laboratory for Image Processing and Intelligent Control of Education Ministry of China, Wuhan 430074, China (e-mail:
[email protected];
[email protected]). T. Matsuno is with the Department of Intelligent System Design Engineering, Toyama Prefectural University, Imizu 939-0398, Japan (e-mail:
[email protected]). T. Fukuda and K. Sekiyama are with the Department of Micro-Nano Systems Engineering, Nagoya University, Nagoya 464-8603, Japan (e-mail:
[email protected];
[email protected]). Color versions of one or more of the figures in this paper are available online at http://ieeexplore.ieee.org. Digital Object Identifier 10.1109/TRO.2010.2053732
1552-3098/$26.00 © 2010 IEEE
