- Email: [email protected]
An approach to carton-folding trajectory planning using dual robotic fingers
Robotics and Autonomous Systems 42 (2003) 47–63
An approach to carton-folding trajectory planning using dual robotic fingers Honghai Liu∗ , Jian Dai Department of Mechanical Engineering, King’s College London, Strand, London WC2R 2LS, UK Received 21 February 2002; received in revised form 20 August 2002 Communicated by F.C.A. Groen
Abstract This paper proposes a method for operating robotic fingers to manipulate cartons in a complex folding process in packaging industry. With this method, a carton is modelled as a mechanism whose links present carton panels and joints present carton creases. The carton-folding position can be analysed by this hypothetical mechanism and the trajectory can be generated from the carton motion model and from the sequence analysis. Manipulating different sections of a carton is realised by creating a path connection based on configuration control points (CCPs) and newly introduced geometry guiding points (GGPs). Hence, the cooperative operation of two robotic fingers can be implemented on the generated trajectories. The method will generate a minimum number of fingers required in manipulating a carton and integrate the carton motion into the trajectory of a robotic finger. Having modelled a dexterous packaging unit of dual robotic fingers, the paper presents a simulation of manipulating a complex carton based on the new method. The discussions are then produced on the simulation results. © 2002 Elsevier Science B.V. All rights reserved. Keywords: Carton folding; Position estimation; Trajectory planning; Robotic fingers
1. Introduction During the last three decades, motion planning has emerged as a crucial and productive research area in robotics [1]. Much work has contributed to this topic in the motion planning and control of multiple manipulators and of a multifingered hand [2,3]. A challenging problem is the manipulation planning of cooperative robots with autonomy and dexterity. The robotic system has to comprehend the corresponding environment, to plan and control various kinds of tasks consistently. Sezgin et al. [4,5] introduced the control configuration points (CCP) in the use of redundancy for obstacle avoidance of planar robot manipulators, where the CCPs are selected suitably along the kinematic structure of a manipulator. Sukhan [6] developed the manipulability measures for both ellipsoids, which present desired manipulability and dual-arm manipulability, to identify the volume of intersection between two individual manipulability ellipsoids. Koga and Latombe [7] produced an automatic generation of motion paths for several cooperative robot ∗
Corresponding author. E-mail addresses: [email protected] (H. Liu), [email protected] (J. Dai). 0921-8890/02/$ – see front matter © 2002 Elsevier Science B.V. All rights reserved. PII: S 0 9 2 1 - 8 8 9 0 ( 0 2 ) 0 0 3 1 2 - 3
48
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
arms to manipulate a movable object between two configurations. Li and Latombe [8] proposed a method of decomposing an overall planning problem into subproblems each of which involving a low-dimensional configuration and time space, further orchestrated fast primitives to solve these subproblems. Lazanas and Latombe [2] considered motion planning with uncertainty requirements by selecting a set of states, associated each of these states with a motion command, and synthesised functions to recognise state configuration. Islands of perfection are defined to describe the region where position and motion control are achieved. Hence, a planning of polynomial complexity is presented. Robotic manipulation planning in carton packaging is a new challenge towards packaging automation for rapid changeover and quick adaptability for various complex cartons in the food and perfumery industry. Though many automatic packaging machines have been innovated to tackle some difficult packaging, only a limited range of packaging process can be automated. The difficulty arises when changing-over from one type of carton to another that manual effort is still required. This does not only affect the consistency in quality control but also incurs labour injuries to the wrists due to the constant twisting motion in various packaging processes. Lu and Akella [9] used a fixture approach in folding a carton using robots where certain folding can be implemented in the concept of fixture devices. The work has a general impact but confines itself to a particular type of rectangular carton. Song and Amato [10] modelled foldable objects as tree-like multilink objects which allow applying motion-planning techniques to folding problems from a motion-planning perspective. A feature of this approach is that it allows studying foldability problems, such as, can one object be folded (or unfolded) into another object. Dai and Rees Jones [11] proposed a new model in describing and presenting a carton as a mechanism. Presenting carton creases, which are dominant elements in carton manipulation as revolute joints, and considering carton panels, which do not need to bend during a carton-folding process as mechanism links, can present a carton as an equivalent mechanism. The approach provided a new way of investigating a carton folding from the point of view of mechanism motion and had a general impact in reconfigurability study of carton packaging. In general, the current sequence-finding algorithm was based on a blind search that requested substantial search time. There is a need to generate a sound and straightforward algorithm based on the intricate geometry properties of a carton to generate a trajectory in carton packaging [12]. This paper proposes a method to create a strategy of folding process of cartons by using dual robotic fingers. The work focuses on a more difficult folding action than a traditional packaging machine by modelling a carton as a mechanism. The carton-folding process can hence be described mathematically by the trajectory of a corresponding mechanism model. The motion of each section of a carton is provided by the kinematic model of the section based on the control configuration points (CCPs). By introducing the geometry guiding points (GGPs) to connect the trajectories of different sections of the carton, the folding process of a complete carton can be implemented with a number of trajectories which are fewer than the number of sections of a complex carton. The trajectory generation method of cartons is then applied to a reconfigurable packing unit using dual robotic fingers assisted by a pair movable jaw, and the kinematic PI control model is provided to follow the carton-folding trajectory. The paper is organised as follows. In Section 2, the carton-folding model is introduced and the geometric representation of a carton folding is described based on the equivalent mechanism approach. In Section 3, hereditary connectivity is developed to identify a carton-folding sequence. In Section 4, the folding sequence is attached with the CCPs and the trajectory generation is provided with the CCPs and GGPs. In Section 5, the folding process with dual robotic fingers is described and a corresponding control algorithm is given. In Section 6, trajectory planning simulation is given.
2. Geometric model of carton folding In modelling a carton packaging process, a carton can be described as an articulated robotic mechanism by taking carton panels as mechanism links and creases as joints. The structure of this mechanism changes as construction proceeds through the joining of panels of a carton in a set of successive operations of folding and manipulation.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
49
Fig. 1. An example of a packaging carton.
Configuration control point is introduced by Sezgin et al. [4,5] in 1997. For good performance, CCPs should be selected in midpoint of links. The CCP properties can be concluded in two main points. First, increasing the number of CCPs does not necessarily lead to safer paths, and can in fact reduce safety if the CCPs are located in regions where collisions will not occur. Second, properly locating a limited number of CCPs on each manipulator is sufficient to avoid collisions between them. Due to the fact that cartons are modelled as equivalent mechanisms, of which section fixed to ground is the base section of cartons, carton model can be described by combination of several serial kinematic chains connecting to ground by base panel of carton. In terms of CCP rules, profile of carton-folding process can be provided by trajectories interpolated by selected CCP. Aim of this section is to select minimal CCP along its links of the equivalent mechanism, then generate folding path of each CCP, further connect separated folding path by geometry guiding joints (GGP), finally download the folding trajectories to a multifingered packing unit. In particular, in an example of a complex and articulated carton, as shown in Fig. 1, the model of the carton can be decomposed into base and sections. The base panel is numbered as panel 1 and the remaining panels are numbered counter clockwise. The carton has four sections, section 1 includes panels 5, 10, 13 and 14; section 2 includes panels 2, 6, 7 and 11; section 3 includes panel 3; section 4 includes panels 4, 8, 9 and 12.
Fig. 2. The simplified model in y–z plane of the packaging carton.
50
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 3. The simplified model in x–z plane of the packaging carton.
It can be seen that sections of 1 and 3 form a main section of a folded carton, sections of 2 and 4 contain auxiliary carton panels which do not help in the carton folding, but support a final configuration of the carton in its folded form. The main section of the carton hence dominates the folding process of the carton. The carton model can hence be decomposed into two motions on two perpendicular planes y–z and x–z, as shown in Figs. 2 and 3, the motion of the main section of the carton is illustrated on the y–z plane. In the figure, the solid lines give a shape of one of the possible configurations of the carton during the folding process, the dotted lines give the end configuration. The parameter lij is the length of j th link of ith section of the carton, θij is the relative angle of j th link of ith section and is subject to the initial and end limit of the angle as αijs ≤ θij ≤ αije . Parameters s1 and s2 are the length between the joint 1 and edges of a slot. CCP11 , CCP21 , CCP22 , CCP31 , CCP41 are the configuration control points selected on all sections. The auxiliary part consisting of sections 2 and 4 is shown in its geometric model on the x–z plane. The models give the motion of the sections of the carton and its corresponding folding process, as shown in Fig. 4. The folding sequence can be identified from the operation of the adjacency matrix that gives the carton configuration during the motion. 2.1. Carton motion and configuration Carton motion can be defined by motion of adjacent panels. The relative motion of two panels is given by a homogeneous transformation matrix hi,j . Let i, j be a pair of adjacent panels, k be the joint between two panels,
Fig. 4. An example of graph representation of a packaging carton.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
51
the configuration of each panel can be represented by a panel configuration vector, q, located at the geometric centre of the panel. Hence, the motion between two panels can be represented by the motion between two configuration vectors qj and qi as qi = hi,j · qj = Ti,k (ux , uy , uz ) · Rk (φ, θ, ψ) · Tk,j (vx , vy , vz ) · qj ,
(1)
where Rk (φ, θ, ψ) = Rx (φ) · Ry (θ ) · Rz (ψ) and
1 0 Tk,j vx , vy , vz = 0 0
0 1 0 0
0 0 1 0
vx vy , vz 1
(2)
(3)
where vx , vy , vz represent the linear displacement from panel j to joint k, and ux , uy , uz represent the linear displacement from joint k to panel i. The panel configuration vectors hence form a carton configuration. Suppose the flattened configuration is given by configuration matrix Q at its initial configuration as Qini in the following which is an aggregate of configuration vectors at their starting positions: Qini = [qini 1 , qini 2 , . . . , qini i , . . . , qini n ].
(4)
Suppose the final erected carton has a target configuration as follows: Qgoal = [qgoal 1 , qgoal 2 , . . . , qgoal i , . . . , qgoal n ].
(5)
Carton fold G can be modelled as a compact subset of R3 , with a global reference frame FW and a moving reference frame FG embedded in G, and the configuration changes from Qini to Qgoal can be specified with the position and orientation of FG with respect to FW . This transformation can be represented as the composition of a rotation R(θi ) by angle θi about an axis and a translation T(δi ) by displacement δi along a vector [13]. Hence, Eq. (5) can be represented as follows to describe the configuration change of a panel from the initial configuration to the final configuration as qgoal = T(δi ) ◦ R(θi ) ◦ qini ,
(6)
where a binary operation ◦ is a law of composition. The continuous map τ : s ∈ [0, 1] → τ (s) ∈ C, which is expressed as qgoal = T(sδi ) ◦ R(sθi ) ◦ qini
(7)
is a continuous path from qini i to qgoal i . The continuous map set t is defined as a path of a continuous displacement, {τ1 (s1 ), τ2 (s2 ), . . . , τn (sn )} ∈ G from Qini to Qgoal . 2.2. Hereditary connectivity In this section, the definition of hereditary connectivity matrix is presented based on the adjacency matrix of a graph in order to produce the global folding sequence. The example above is rewritten as a tree-like directed graph, as shown in Fig. 5, in which nodes represent the joints of the mechanism, and directed lines denote links.
52
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 5. The trajectory of xd1 , yd1 , zd1 , for finger 1, with respect to time t.
An adjacency matrix can hence be given, but replacing element ‘1’ in the matrix with hi,j which denotes the configuration transformation from panel i to panel j , an adjacency configuration matrix can be given. The configuration transformation can hence be obtained from the matrix. The adjacency configuration matrix is given as follows: 0 h1,2 h1,3 h1,4 h1,5 0 0 0 0 0 0 0 0 0 h2,1 0 0 0 0 h2,6 h2,7 0 0 0 h2,11 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h3,1 h4,1 0 0 0 0 0 0 h4,8 h4,9 0 0 h4,12 0 0 h5,1 0 0 0 0 0 0 0 0 h5,10 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h6,2 0 0 h7,2 0 0 0 0 0 0 0 0 0 0 0 0 . 0 0 0 h8,4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h9,4 0 0 0 0 0 h10,5 0 0 0 0 0 0 0 h10,13 0 0 h11,2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h12,4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h13,10 0 0 0 h13,14 0 0 0 0 0 0 0 0 0 0 0 0 h14,13 0
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
53
The above sparse matrices should be avoided for reasons of both matrix storage space and computation time, even though some software package support sparse matrix like MATLAB. In such a situation the hereditary connectivity matrix (HCM) is introduced. The number of columns and rows is defined by the largest number of sum of panels at every possible path of the tree-like directed graph. In deciding this number of columns and rows, it is to be noted, if symmetry exist, that the symmetrical links or panels will be ignored in producing the HCM, in return for that the direct parent panel will fold first in this row of HCM. Hence the information of the adjacency configuration matrix is reduced as to the HCM as follows [12]: h1,2 h1,3 h1,4 h1,5 h 0 h4,12 h5,10 2,11 . 0 0 h10,13 0 0 0 0 h13,14 The following rules are set for extracting the folding sequence and trajectory of cartons: (1) identify independent manipulations from the first row of the HCM, this decides how many fingers are at least required to fold this particular type of cartons if using parallel folding paths with a multifingered operation; (2) search column connectivity to generate the first part of the folding paths; (3) search hereditary column to generate the second part of the folding paths; (4) replace connectivity entries with symmetric entries to complete the folding paths. 3. Trajectory generation In developing the algorithm for the trajectory planning of a carton-folding process using multiple robotic fingers, the tips of fingers are presumed to stick to the corresponding configuration control points during a carton-folding process. Hence the trajectory can be generated from the geometric model based on the mechanism analysis. 3.1. Folding sequence with CCPs The control configuration points are certain selected points along a carton panels. The location of the CCP is critically important, even a single CCP is enough to obtain a safe path, provided that this CCP is sufficiently close to the region of potential collision. In this paper, the folded carton region is viewed as one of potential collision. And from the viewpoint of modelling carton as mechanism, each section of a carton is viewed as a virtual kinematic chain, and the corresponding folding motion is represented by one of virtual kinematic chains guided by certain CCPs. Therefore, the problem of the carton-folding process is converted into a cooperation navigation of multiple kinematic chains, that is to say, the folding task can be mathematically described by CCPs based on kinematic chains. For example, as shown in Figs. 2 and 3, the selected continuous CCPs are CCP11 , CCP21 , CCP31 , CCP41 and CCP22 , the equation of CCP41 is given based on Eq. (1): 1 0 0 0 x 0 0 y 0 cθ411 −sθ411 0 0.5l41 −0.5bw = . CCP41 : (8) z 0 sθ411 cθ411 0 0 + 0 0 0 0 1 1 1 1 Hence the other CCPs can be obtained in the same way as follows: x l11 cθ111 + 0.5 cθ1112 + 0.5bl y 0 = , CCP11 : z l11 sθ111 + 0.5 sθ1112 1
1
(9)
54
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
CCP31 :
CCP21 :
CCP22 :
x l11 cθ311 + 0.5 cθ3112 − 0.5bl y 0 = , l11 sθ311 + 0.5 sθ3112 z 1 1 x 0 y l cθ + l cθ 22 2112 + 0.5l23 cθ21123 + 0.5bw 21 211 = , l21 sθ211 + l22 sθ2112 + 0.5l23 sθ21123 z 1 0 x 0 y l cθ + l cθ 22 2112 + l23 cθ21123 + 0.5l24 cθ211234 + 0.5bw 21 211 = , l21 sθ211 + l22 sθ2112 + l23 sθ21123 + 0.5l24 sθ211234 z 1 0
(10)
(11)
(12)
where s stands for sin, c stands for cos; for θsubscription , the first two numbers of subscription denote the name of CCP, the other numbers denote addition of angles relative to the corresponding CCP. For example, sθ21123 stands for sin(θ21 + θ22 + θ23 ) of configuration control point 21 (CCP21 ). For the sake of the folding task that uses a multifingered packaging unit, the CCPs trajectories of the virtual kinematic chain are just those need to be manipulated by robotic fingers. Since one finger deals with one CCP at one time, more than one CCPs in a carton section usually requires more than one finger. However, the sequential operation of CCPs at the same carton section will reduce the use of the number of fingers. Geometrical constraint, G(t), is introduced by the length between Ptip , tip of link l24 , and Pcp , centre point of slot in link l41 . An IF-ELSE control structure is applied here, by introducing k as a constant dependent on the geometrical constraint, in this case k is the length of l24 . If (G(t) ≤ k), then folding motion of robotic finger converts from the trajectory of CCP21 to the trajectory of CCP22 , otherwise, robotic finger continues to follow the trajectory of CCP21 . The geometric constraint is given as follows: 2 2 4 i 4 i G(t) = |Ptip Pcp | = ls cθ41 − l2i c θ2j + ls sθ41 − l2i s θ2j , (13) i=1
j =1
i=1
j =1
where ls = 0.5(s2 + s1 ). 3.2. Trajectory generation with CCPs For the sake of simplicity, some key points are chosen from trajectories of CCPs. These points are dependent on the division of the relative orientation angles θij . These points can hence describe the corresponding carton sections. In this case, a particular selection of key CCPs trajectories is provided in joint space as follows: θ11s + θ11e θ12s + θ12e , θ11e , θ11e + , θ12e + θ12e , θ11s , CCP11 : 2 2 θ31s + θ31e θ32s + θ32e θ31s , CCP31 : , θ31e , θ31e + , θ32e + θ32e , 2 2 θ41s + θ41e , θ41e , θ41s , CCP41 : 2
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
55
θ21s + θ21e θ22s + θ22e θ23s + θ23e , θ21e , θ21e + , θ21e + θ22e , θ21e + θ22e + , 2 2 2 θ21e + θ22e + θ23e } ,
θ21s ,
CCP21 :
CCP22 :
θ21s + θ21e θ22s + θ22e θ23s + θ23e , θ21e , θ21e + , θ21e + θ22e , θ21e + θ22e + , 2 2 2 θ24s + θ24e , θ21e + θ22e + θ23e + θ24e . θ21e + θ22e + θ23e , θ21e + θ22e + θ23e + 2
θ21s ,
The definition of a path γi of ith carton section with CCPi , hence, can be given. A folding path γi of carton section is a continuous mapping γi : [CCPi (0), CCPi (ti )] → C, where γi (CCPi (0)) is the initial point and γi (CCPi (ti )) is the final point, ti is constant time used to fold the carton section. 3.3. Paths connection with GGPs The geometry guiding points (GGPs) are introduced when the problem of how to connect two different paths comes up. Basically GGPs are the points to guide a robotic finger to transit continuously from one path of carton section to another, that is to say, to produce a continuous new path to guide a robotic finger based on key points of CCPs from both separated paths GGPs. Hence GGPs should be those kind of points meeting the following rules: (1) the method of GGP is used to estimate the trajectory in a carton-folding task; (2) the new path produced with GGPs meets the trajectory requirement of separated paths; (3) the robotic finger can be guided to the correct direction of a second path. Let two carton section paths be γ1 with CCP1 and γ2 with CCP2 , the folding time be t1 , t2 , respectively, and let ri (CCPi (ti ))j denote j -coordinate of path ri with CCPi relative to ti time moment. Refer to the coordinate system shown in Figs. 2 and 3, the z-coordinates of GGP have to lower enough than γ2 (CCP2 (0))z , the initial position of path γ2 so that interpolated transit path could continue γ2 in suitable direction due to γ2 (CCP2 (0))z in the initial position of carton. Besides x- and y-coordinates of GGP have to be chosen between (γ1 (CCP1 (t1 ))x , γ1 (CCP1 (t1 ))y ) and (γ2 (CCP2 (0))x , γ2 (CCP2 (0))y ) in order to get as short transit path as it can. Finally a generated new path based data of CCPs and GGPs is what will be fed to dual robotic fingers. In carton-folding case, the selected GGP(x, y, z) for path connection of two carton sections conforms the following formula: γ (CCP (t )) + γ (CCP (0)) x y = z
1
1 1
x
2
2
x
2 γ1 (CCP1 (t1 ))y + γ2 (CCP2 (0))y , 2 γ1 (CCP1 (t1 ))z + γ2 (CCP2 (0))z γ2 (CCP2 (0))z − λ 2 where λ is a constant used to tune the folding direction to path γ2 . Hence GGPs are particular points that not only smoothly connect two paths of carton sections, but also ensure that the packing unit can fold the panels in correct direction. In the example here, let λ be 0.5, so GGP1 , [0.3, 0.3, −0.2] , can be calculated out for connection of carton sections 1 and 2, whose trajectory is followed by finger 1; GGP2 , [−0.3, 0, 0.21] , for connection of sections 3 and 4, whose trajectory is followed by finger 2. The reference path parameterised by s, is produced as xdi = p(s),
(14)
ydi = q(s),
(15)
56
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 6. The trajectory of xd2 , yd2 , zd2 , for finger 2, with respect to time t.
zdi = r(s),
(16)
0 ≤ s ≤ sf .
(17)
Hence the spatial reference path can be given by three parametric equations, where the reference trajectory is parameterised in time. The data based on CCPs and GGPs, for fingers 1 and 2, with respect to time t is changed as a continuous reference path by interpolating the generated CCPs and GGPs. The corresponding paths of fingers 1 and 2 are shown in Figs. 6 and 7, respectively, in which the symbol ‘䊊’ presents x-coordinates of fingers 1 and 2 generated based on the CCPs and GGPs, the symbol ‘+’ for y-coordinates and symbol ‘∗’ for z-coordinates of both fingers. The trajectory of z-coordinates of robotic finger 1 moves from −0.2 to 0.2 mm during section 1 folding of the carton, indicating robotic finger 1 in a correct direction to fold carton after a transit period. The same is true for finger 2. The data produced here will be passed to a dual-robotic-finger control system as a reference path.
Fig. 7. Snapshots of a carton-folding process.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
57
4. Carton folding using dual robotic fingers The environment of an automated packaging unit with dual robotic fingers is provided in this section, a trajectoryfollowing control strategy is constructed to make the dual robotic fingers follow the trajectories produced by the above algorithm. This section consists of three parts. The first part generates the minimum time of a carton-folding process based on the time setting of a carton-folding process. The other two parts produce the modelling and control strategy of the packaging unit. 4.1. Minimum time of carton-folding process The total time T [14] to pass along the path between two points is proportional to the geodesic distance S and can be found as follows: 2 3 9S t = 0.5 . (18) Pmax Hence the assumption is made that a carton has m sections, each has n CCPs, the folding process always starts from a home position of the corresponding robotic finger. A minimum time of the carton-folding process can be formulated as n n m m 2 3 9Sij T = tij = 0.5 , (19) Pmax i=1 i=1
i=1 j =1
where S is the distance from the end points along a path length and Pmax the maximum power. S is the near-minimumtime geometric path under the following constraints: (1) there is an upper limit on the total power consumed by the robot’s torque motors; (2) the initial and final velocities are zeros. 4.2. Modelling of the packaging unit with dual robotic fingers The packaging unit consists of dual robotic fingers, which can be viewed as a Puma 560 robot moving along a rail, respectively, as shown in Figs. 8 and 13. It is straightforward to compute the position of the tip of robotic fingers, relative to the rails, rAB , as cos(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 )) B rA = sin(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 )) , (20) li3 sin(φi2 + φi3 ) + li2 sin(φi2 ) + li1 where i = 1, 2 for dual robotic fingers, indexes A, B the coordinate frames for both the robotic fingers and rails, respectively. For a robotic finger with a guide-rail, the position of the fingertip is given as follows: cos(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 ) + xir ) rAi = sin(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 )) + yir , (21) li3 sin(φi2 + φi3 ) + li2 sin(φi2 ) + li1 where the tip position of the fingers is calculated relative to a fixed, Cartesian coordinate system and rBi = (xir , yir , 0)T is its global position, respectively. The joint velocities are calculated by differentiating rAi = (xAi , yAi , zAi )T and four free control parameters, φ˙ i1 , φ˙ i2 , φ˙ 3i and y˙ir , can be determined as follows: ˙ yir ]T = J −1 (φi )(˙rAi ), [φ, (22) i
T (r r T )−1 . where Ji−1 (φ) is the pseudo-inverse matrix of rAi , defined by rAi Ai Ai
58
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 8. Configuration of robotic finger.
4.3. Control algorithm of the packaging unit with dual robotic fingers The control system applied to the packaging unit is a trajectory-following system, which controls the fingers to follow a desired position trajectory. The trajectories are given by a set of functions, xdi , ydi , zdi , where i = 1, 2, which specified the desired position of the cartons. The servo errors are defined between the desired and actual trajectories, and conventional PI control algorithm is used for both of two robotic fingers of packaging unit as follows: kip (xdi − xi ) + k1i (xdi − xi ) dt uxi (23) ui = uyi = kip (ydi − yi ) + k1i (ydi − yi ) dt , kip (zdi − zi ) + k1i (zdi − zi ) dt uzi where i = 1, 2 for the dual robotic fingers.
5. Simulation results Due to requirement of carton-folding application, the trajectory-following control does not require to make the fingers follow the desired trajectory precisely, but limit the error of the desired and actual trajectory in a specified range in order to implement the carton-folding task. Hence the analysis of simulation results focuses on the problem of whether or not the actual trajectory of fingers meets the desired trajectory within required error. The models in previous sections give the motions of the carton sections and its corresponding folding process. The geometric parameters of the carton are provided see Table 1. The trajectory errors for fingers 1 and 2 are given in Figs. 9 and 10, it can be seen that the errors quickly reduced to zero after the preparation stage. The whole folding process for one robotic finger in y–z plane is divided into four stages: preparation, section 4 folding (from 2 to 7 s), transit, section 1 folding (from 13 to 21 s). In the preparation stage, the robotic finger move from its initial position to the final position of section 4 of the carton, in which the action does not affect the performance of a carton-folding task. The errors in two folding sections from 2nd second
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
59
Table 1 Geometric parameters of the cartona Panel length (mm)
Start angle (◦ )
End angle (◦ )
l11 l12 l21 l22 l23 l24 l31 l32 l41
θ11s θ12s θ21s θ22s θ23s θ23s θ31s θ32s θ41s
θ11e θ12e θ21e θ22e θ23e θ23e θ31e θ32e θ41e
= 320 = 116 = 321 = 123 = 257 = 36 = 320 = 116 = 320 a
=0 =0 =0 =0 =0 =0 =0 =0 =0
= 90 = 90 = 97 = 81 = 82 = 90 = 90 = 90 = 95
Base wide, bw = 518 mm; base length, bl = 198 mm; Slot1, s1 = 102 mm; Slot2, s2 = 147 mm.
to 7th second and from 13th second to 21st second are much smaller than the limited error. Fig. 11 gives the smooth joint trajectories of robotic finger 1. The same is true for the robotic finger 2, as shown in Fig. 12. The movement of finger 2 mainly consists of three folding stages, section 2 folding (from 1 to 5 s), section 3 folding (from 8 to 13 s) of the carton, and a stage to assist finger 1. It is clear that the errors are suitable for the folding task. The environment of the whole folding process using dual robotic fingers is demonstrated in Fig. 13, in which the proposed method of trajectory generation based on CCPs and GGPs meets the requirement of the carton-folding task.
Fig. 9. The trajectory error of X-, Y -, Z-coordinate in turn with respect to t for robotic finger 1.
60
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 10. The trajectory error of X-, Y -, Z-coordinate in turn with respect to t for robotic finger 2.
Fig. 11. The joint trajectory with respect to t for robotic finger 1: (a) the joint trajectory of φ11 , (b) the joint trajectory of φ12 , (c) the joint trajectory of φ13 , (d) the position trajectory of rail1.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
61
Fig. 12. The joint trajectory with respect to t for robotic finger 2: (a) the joint trajectory of φ21 , (b) the joint trajectory of θ22 , (c) the joint trajectory of φ23 , (d) the position trajectory of rail2.
Fig. 13. The environment of the carton-folding process using two robotic fingers.
6. Conclusion An approach to carton-folding trajectory planning using an automated packaging unit with dual robotic fingers has been proposed in this paper. Based on the equivalent mechanism approach, an algorithm based on integration of the CCPs into the GGPs was developed to estimate the trajectory of a carton-folding task. Dual robotic fingers in terms of kinematic models then follow the generated trajectories. The simulation results demonstrated that the
62
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
proposed method is practical to be applied to a multifingered packaging unit and to be used to reduce the number of degrees of freedom required in a complex carton manipulation. Acknowledgements The authors wish to thank the anonymous reviewers for their constructive comments and suggestions which helped in improving the quality of this paper.
References [1] J.C. Latombe, Motion planning: a journey of robots, molecules, digital actors, and other artifacts, International Journal of Robotics and Research 18 (11) (1999) 1119–1128. [2] A. Lazanas, J.C. Latombe, Motion planning with uncertainty: a landmark approach artificial intelligence, Artifical Intelligence 76 (1–2) (1995) 285–317. [3] J. de Vries, A.H. Streppel, E.J.W. Klaassen, L.J. de Vin, H.J.J. Kals, The generation of bending sequences in a capp system for sheet-metal components, Journal of Material Processing Technology 41 (1994) 331–339. [4] U. Sezgin, L.D. Seneviratne, S.W.E. Earles, Redundancy utilization for obstacle avoidance of planar manipulators, Proceedings of Institution of Mechanical Engineers, Journal of Mechanical Engineering Science 211 (4) (1997) 346–355. [5] U. Sezgin, L.D. Seneviratne, S.W.E. Earles, Collision avoidance in multiple-redundant manipulators, International Journal of Robotics and Research 16 (5) (1997) 714–725. [6] L. Sukhan, Dual redundant arm configuration optimization with task-oriented dual arm manipulability, IEEE Transactions on Robotics and Automation 5 (11) (1999) 78–97. [7] Y. Koga, J.C. Latombe, On multi-arm manipulation planning proc, in: Proceedings of IEEE Robotics and Automation Conference (ICRA’1994), San Diego, CA, 1994. [8] T.Y. Li, J.C. Latombe, On-line manipulator planning for two robot arms in a dynamic environment, International Journal of Robotics and Research 16 (2) (1997) 144–167. [9] L. Lu, S. Akella, Folding cartons with frixtures: a motion planning approach, IEEE Transactions on Robotics and Automation 16 (4) (2000) 346–355. [10] S. Guang, M.A. Nancy, A motion planning approach to folding: from paper craft to protein folding, in: Proceedings of IEEE Robotics and Automation Conference (ICRA’2001), Seoul, 2001. [11] J.S. Dai, J. Rees Jones, Mobility in metamorphic mechanisms of foldable/erectable kinds, ASME Transactions 121 (3) (1999) 375–382. [12] H. Liu, J.S. Dai, Manipulation analysis of carton folding task using configuration transformation, Proceedings of Institution of Mechanical Engineers, Journal of Mechanical Engineering Science 216 (2002) 543–555. [13] K.H. Hunt, Kinematic Geometry of Mechanisms, Clarendon Press, Oxford, UK, 1978. [14] Y. Edan, T. Flash, I. Shmulevich, U.M. Peiper, Y. Sarig, Near-minimum-time task planning for fruit-picking robots, IEEE Transactions on Robotics and Automation 7 (1) (1991).
Honghai Liu received his B.Sc. and MEng in Mechatronics Theory and Applications from Wuhan University of Science and Technology and Wuhan Science and Engineering University in 1996 and 1999, respectively. He worked as a project manager in Autotech Automation Engineering Inc., China branch of Siemens-Moore Solution since 1997. He is currently a research associate in the Department of Mechanical Engineering at King’s College London. His current research includes manipulation planning and control using multifingered devices, process control, real-time control.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
63
Jian Dai was born in Shanghai in China. He graduated with a first class degree at Shanghai Jiao Tong University in 1982, received his M.Sc. at the same university in 1985, and obtained his Ph.D. at the University of Salford in UK in 1993. He became a university lecturer in 1985, a visiting scholar to the University of Salford in 1988, a post doctoral research fellow in 1993, a senior research fellow at Unilever research in 1996 and a senior lecturer at the University of Sunderland in 1997. He joined the King’s College London with his research team in 1999. Jian Dai has over 60 refereed publications and was a recipient of the “best paper award” on his ‘metamorphic mechanism’s synthesis’ at the 25th ASME Biennial Mechanism and Robotics Conference in Atlanta in 1998. He has been invited to Italy, Japan and China for many invited lectures and has organised several workshops. Currently, he leads several major research projects and serves on several international committees. His research interests are in mechanisms, manipulation planning, manipulator kinematics and design, multifingered robotic hand, grasping and packaging applications.
An approach to carton-folding trajectory planning using dual robotic fingers Honghai Liu∗ , Jian Dai Department of Mechanical Engineering, King’s College London, Strand, London WC2R 2LS, UK Received 21 February 2002; received in revised form 20 August 2002 Communicated by F.C.A. Groen
Abstract This paper proposes a method for operating robotic fingers to manipulate cartons in a complex folding process in packaging industry. With this method, a carton is modelled as a mechanism whose links present carton panels and joints present carton creases. The carton-folding position can be analysed by this hypothetical mechanism and the trajectory can be generated from the carton motion model and from the sequence analysis. Manipulating different sections of a carton is realised by creating a path connection based on configuration control points (CCPs) and newly introduced geometry guiding points (GGPs). Hence, the cooperative operation of two robotic fingers can be implemented on the generated trajectories. The method will generate a minimum number of fingers required in manipulating a carton and integrate the carton motion into the trajectory of a robotic finger. Having modelled a dexterous packaging unit of dual robotic fingers, the paper presents a simulation of manipulating a complex carton based on the new method. The discussions are then produced on the simulation results. © 2002 Elsevier Science B.V. All rights reserved. Keywords: Carton folding; Position estimation; Trajectory planning; Robotic fingers
1. Introduction During the last three decades, motion planning has emerged as a crucial and productive research area in robotics [1]. Much work has contributed to this topic in the motion planning and control of multiple manipulators and of a multifingered hand [2,3]. A challenging problem is the manipulation planning of cooperative robots with autonomy and dexterity. The robotic system has to comprehend the corresponding environment, to plan and control various kinds of tasks consistently. Sezgin et al. [4,5] introduced the control configuration points (CCP) in the use of redundancy for obstacle avoidance of planar robot manipulators, where the CCPs are selected suitably along the kinematic structure of a manipulator. Sukhan [6] developed the manipulability measures for both ellipsoids, which present desired manipulability and dual-arm manipulability, to identify the volume of intersection between two individual manipulability ellipsoids. Koga and Latombe [7] produced an automatic generation of motion paths for several cooperative robot ∗
Corresponding author. E-mail addresses: [email protected] (H. Liu), [email protected] (J. Dai). 0921-8890/02/$ – see front matter © 2002 Elsevier Science B.V. All rights reserved. PII: S 0 9 2 1 - 8 8 9 0 ( 0 2 ) 0 0 3 1 2 - 3
48
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
arms to manipulate a movable object between two configurations. Li and Latombe [8] proposed a method of decomposing an overall planning problem into subproblems each of which involving a low-dimensional configuration and time space, further orchestrated fast primitives to solve these subproblems. Lazanas and Latombe [2] considered motion planning with uncertainty requirements by selecting a set of states, associated each of these states with a motion command, and synthesised functions to recognise state configuration. Islands of perfection are defined to describe the region where position and motion control are achieved. Hence, a planning of polynomial complexity is presented. Robotic manipulation planning in carton packaging is a new challenge towards packaging automation for rapid changeover and quick adaptability for various complex cartons in the food and perfumery industry. Though many automatic packaging machines have been innovated to tackle some difficult packaging, only a limited range of packaging process can be automated. The difficulty arises when changing-over from one type of carton to another that manual effort is still required. This does not only affect the consistency in quality control but also incurs labour injuries to the wrists due to the constant twisting motion in various packaging processes. Lu and Akella [9] used a fixture approach in folding a carton using robots where certain folding can be implemented in the concept of fixture devices. The work has a general impact but confines itself to a particular type of rectangular carton. Song and Amato [10] modelled foldable objects as tree-like multilink objects which allow applying motion-planning techniques to folding problems from a motion-planning perspective. A feature of this approach is that it allows studying foldability problems, such as, can one object be folded (or unfolded) into another object. Dai and Rees Jones [11] proposed a new model in describing and presenting a carton as a mechanism. Presenting carton creases, which are dominant elements in carton manipulation as revolute joints, and considering carton panels, which do not need to bend during a carton-folding process as mechanism links, can present a carton as an equivalent mechanism. The approach provided a new way of investigating a carton folding from the point of view of mechanism motion and had a general impact in reconfigurability study of carton packaging. In general, the current sequence-finding algorithm was based on a blind search that requested substantial search time. There is a need to generate a sound and straightforward algorithm based on the intricate geometry properties of a carton to generate a trajectory in carton packaging [12]. This paper proposes a method to create a strategy of folding process of cartons by using dual robotic fingers. The work focuses on a more difficult folding action than a traditional packaging machine by modelling a carton as a mechanism. The carton-folding process can hence be described mathematically by the trajectory of a corresponding mechanism model. The motion of each section of a carton is provided by the kinematic model of the section based on the control configuration points (CCPs). By introducing the geometry guiding points (GGPs) to connect the trajectories of different sections of the carton, the folding process of a complete carton can be implemented with a number of trajectories which are fewer than the number of sections of a complex carton. The trajectory generation method of cartons is then applied to a reconfigurable packing unit using dual robotic fingers assisted by a pair movable jaw, and the kinematic PI control model is provided to follow the carton-folding trajectory. The paper is organised as follows. In Section 2, the carton-folding model is introduced and the geometric representation of a carton folding is described based on the equivalent mechanism approach. In Section 3, hereditary connectivity is developed to identify a carton-folding sequence. In Section 4, the folding sequence is attached with the CCPs and the trajectory generation is provided with the CCPs and GGPs. In Section 5, the folding process with dual robotic fingers is described and a corresponding control algorithm is given. In Section 6, trajectory planning simulation is given.
2. Geometric model of carton folding In modelling a carton packaging process, a carton can be described as an articulated robotic mechanism by taking carton panels as mechanism links and creases as joints. The structure of this mechanism changes as construction proceeds through the joining of panels of a carton in a set of successive operations of folding and manipulation.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
49
Fig. 1. An example of a packaging carton.
Configuration control point is introduced by Sezgin et al. [4,5] in 1997. For good performance, CCPs should be selected in midpoint of links. The CCP properties can be concluded in two main points. First, increasing the number of CCPs does not necessarily lead to safer paths, and can in fact reduce safety if the CCPs are located in regions where collisions will not occur. Second, properly locating a limited number of CCPs on each manipulator is sufficient to avoid collisions between them. Due to the fact that cartons are modelled as equivalent mechanisms, of which section fixed to ground is the base section of cartons, carton model can be described by combination of several serial kinematic chains connecting to ground by base panel of carton. In terms of CCP rules, profile of carton-folding process can be provided by trajectories interpolated by selected CCP. Aim of this section is to select minimal CCP along its links of the equivalent mechanism, then generate folding path of each CCP, further connect separated folding path by geometry guiding joints (GGP), finally download the folding trajectories to a multifingered packing unit. In particular, in an example of a complex and articulated carton, as shown in Fig. 1, the model of the carton can be decomposed into base and sections. The base panel is numbered as panel 1 and the remaining panels are numbered counter clockwise. The carton has four sections, section 1 includes panels 5, 10, 13 and 14; section 2 includes panels 2, 6, 7 and 11; section 3 includes panel 3; section 4 includes panels 4, 8, 9 and 12.
Fig. 2. The simplified model in y–z plane of the packaging carton.
50
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 3. The simplified model in x–z plane of the packaging carton.
It can be seen that sections of 1 and 3 form a main section of a folded carton, sections of 2 and 4 contain auxiliary carton panels which do not help in the carton folding, but support a final configuration of the carton in its folded form. The main section of the carton hence dominates the folding process of the carton. The carton model can hence be decomposed into two motions on two perpendicular planes y–z and x–z, as shown in Figs. 2 and 3, the motion of the main section of the carton is illustrated on the y–z plane. In the figure, the solid lines give a shape of one of the possible configurations of the carton during the folding process, the dotted lines give the end configuration. The parameter lij is the length of j th link of ith section of the carton, θij is the relative angle of j th link of ith section and is subject to the initial and end limit of the angle as αijs ≤ θij ≤ αije . Parameters s1 and s2 are the length between the joint 1 and edges of a slot. CCP11 , CCP21 , CCP22 , CCP31 , CCP41 are the configuration control points selected on all sections. The auxiliary part consisting of sections 2 and 4 is shown in its geometric model on the x–z plane. The models give the motion of the sections of the carton and its corresponding folding process, as shown in Fig. 4. The folding sequence can be identified from the operation of the adjacency matrix that gives the carton configuration during the motion. 2.1. Carton motion and configuration Carton motion can be defined by motion of adjacent panels. The relative motion of two panels is given by a homogeneous transformation matrix hi,j . Let i, j be a pair of adjacent panels, k be the joint between two panels,
Fig. 4. An example of graph representation of a packaging carton.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
51
the configuration of each panel can be represented by a panel configuration vector, q, located at the geometric centre of the panel. Hence, the motion between two panels can be represented by the motion between two configuration vectors qj and qi as qi = hi,j · qj = Ti,k (ux , uy , uz ) · Rk (φ, θ, ψ) · Tk,j (vx , vy , vz ) · qj ,
(1)
where Rk (φ, θ, ψ) = Rx (φ) · Ry (θ ) · Rz (ψ) and
1 0 Tk,j vx , vy , vz = 0 0
0 1 0 0
0 0 1 0
vx vy , vz 1
(2)
(3)
where vx , vy , vz represent the linear displacement from panel j to joint k, and ux , uy , uz represent the linear displacement from joint k to panel i. The panel configuration vectors hence form a carton configuration. Suppose the flattened configuration is given by configuration matrix Q at its initial configuration as Qini in the following which is an aggregate of configuration vectors at their starting positions: Qini = [qini 1 , qini 2 , . . . , qini i , . . . , qini n ].
(4)
Suppose the final erected carton has a target configuration as follows: Qgoal = [qgoal 1 , qgoal 2 , . . . , qgoal i , . . . , qgoal n ].
(5)
Carton fold G can be modelled as a compact subset of R3 , with a global reference frame FW and a moving reference frame FG embedded in G, and the configuration changes from Qini to Qgoal can be specified with the position and orientation of FG with respect to FW . This transformation can be represented as the composition of a rotation R(θi ) by angle θi about an axis and a translation T(δi ) by displacement δi along a vector [13]. Hence, Eq. (5) can be represented as follows to describe the configuration change of a panel from the initial configuration to the final configuration as qgoal = T(δi ) ◦ R(θi ) ◦ qini ,
(6)
where a binary operation ◦ is a law of composition. The continuous map τ : s ∈ [0, 1] → τ (s) ∈ C, which is expressed as qgoal = T(sδi ) ◦ R(sθi ) ◦ qini
(7)
is a continuous path from qini i to qgoal i . The continuous map set t is defined as a path of a continuous displacement, {τ1 (s1 ), τ2 (s2 ), . . . , τn (sn )} ∈ G from Qini to Qgoal . 2.2. Hereditary connectivity In this section, the definition of hereditary connectivity matrix is presented based on the adjacency matrix of a graph in order to produce the global folding sequence. The example above is rewritten as a tree-like directed graph, as shown in Fig. 5, in which nodes represent the joints of the mechanism, and directed lines denote links.
52
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 5. The trajectory of xd1 , yd1 , zd1 , for finger 1, with respect to time t.
An adjacency matrix can hence be given, but replacing element ‘1’ in the matrix with hi,j which denotes the configuration transformation from panel i to panel j , an adjacency configuration matrix can be given. The configuration transformation can hence be obtained from the matrix. The adjacency configuration matrix is given as follows: 0 h1,2 h1,3 h1,4 h1,5 0 0 0 0 0 0 0 0 0 h2,1 0 0 0 0 h2,6 h2,7 0 0 0 h2,11 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h3,1 h4,1 0 0 0 0 0 0 h4,8 h4,9 0 0 h4,12 0 0 h5,1 0 0 0 0 0 0 0 0 h5,10 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h6,2 0 0 h7,2 0 0 0 0 0 0 0 0 0 0 0 0 . 0 0 0 h8,4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h9,4 0 0 0 0 0 h10,5 0 0 0 0 0 0 0 h10,13 0 0 h11,2 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h12,4 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 h13,10 0 0 0 h13,14 0 0 0 0 0 0 0 0 0 0 0 0 h14,13 0
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
53
The above sparse matrices should be avoided for reasons of both matrix storage space and computation time, even though some software package support sparse matrix like MATLAB. In such a situation the hereditary connectivity matrix (HCM) is introduced. The number of columns and rows is defined by the largest number of sum of panels at every possible path of the tree-like directed graph. In deciding this number of columns and rows, it is to be noted, if symmetry exist, that the symmetrical links or panels will be ignored in producing the HCM, in return for that the direct parent panel will fold first in this row of HCM. Hence the information of the adjacency configuration matrix is reduced as to the HCM as follows [12]: h1,2 h1,3 h1,4 h1,5 h 0 h4,12 h5,10 2,11 . 0 0 h10,13 0 0 0 0 h13,14 The following rules are set for extracting the folding sequence and trajectory of cartons: (1) identify independent manipulations from the first row of the HCM, this decides how many fingers are at least required to fold this particular type of cartons if using parallel folding paths with a multifingered operation; (2) search column connectivity to generate the first part of the folding paths; (3) search hereditary column to generate the second part of the folding paths; (4) replace connectivity entries with symmetric entries to complete the folding paths. 3. Trajectory generation In developing the algorithm for the trajectory planning of a carton-folding process using multiple robotic fingers, the tips of fingers are presumed to stick to the corresponding configuration control points during a carton-folding process. Hence the trajectory can be generated from the geometric model based on the mechanism analysis. 3.1. Folding sequence with CCPs The control configuration points are certain selected points along a carton panels. The location of the CCP is critically important, even a single CCP is enough to obtain a safe path, provided that this CCP is sufficiently close to the region of potential collision. In this paper, the folded carton region is viewed as one of potential collision. And from the viewpoint of modelling carton as mechanism, each section of a carton is viewed as a virtual kinematic chain, and the corresponding folding motion is represented by one of virtual kinematic chains guided by certain CCPs. Therefore, the problem of the carton-folding process is converted into a cooperation navigation of multiple kinematic chains, that is to say, the folding task can be mathematically described by CCPs based on kinematic chains. For example, as shown in Figs. 2 and 3, the selected continuous CCPs are CCP11 , CCP21 , CCP31 , CCP41 and CCP22 , the equation of CCP41 is given based on Eq. (1): 1 0 0 0 x 0 0 y 0 cθ411 −sθ411 0 0.5l41 −0.5bw = . CCP41 : (8) z 0 sθ411 cθ411 0 0 + 0 0 0 0 1 1 1 1 Hence the other CCPs can be obtained in the same way as follows: x l11 cθ111 + 0.5 cθ1112 + 0.5bl y 0 = , CCP11 : z l11 sθ111 + 0.5 sθ1112 1
1
(9)
54
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
CCP31 :
CCP21 :
CCP22 :
x l11 cθ311 + 0.5 cθ3112 − 0.5bl y 0 = , l11 sθ311 + 0.5 sθ3112 z 1 1 x 0 y l cθ + l cθ 22 2112 + 0.5l23 cθ21123 + 0.5bw 21 211 = , l21 sθ211 + l22 sθ2112 + 0.5l23 sθ21123 z 1 0 x 0 y l cθ + l cθ 22 2112 + l23 cθ21123 + 0.5l24 cθ211234 + 0.5bw 21 211 = , l21 sθ211 + l22 sθ2112 + l23 sθ21123 + 0.5l24 sθ211234 z 1 0
(10)
(11)
(12)
where s stands for sin, c stands for cos; for θsubscription , the first two numbers of subscription denote the name of CCP, the other numbers denote addition of angles relative to the corresponding CCP. For example, sθ21123 stands for sin(θ21 + θ22 + θ23 ) of configuration control point 21 (CCP21 ). For the sake of the folding task that uses a multifingered packaging unit, the CCPs trajectories of the virtual kinematic chain are just those need to be manipulated by robotic fingers. Since one finger deals with one CCP at one time, more than one CCPs in a carton section usually requires more than one finger. However, the sequential operation of CCPs at the same carton section will reduce the use of the number of fingers. Geometrical constraint, G(t), is introduced by the length between Ptip , tip of link l24 , and Pcp , centre point of slot in link l41 . An IF-ELSE control structure is applied here, by introducing k as a constant dependent on the geometrical constraint, in this case k is the length of l24 . If (G(t) ≤ k), then folding motion of robotic finger converts from the trajectory of CCP21 to the trajectory of CCP22 , otherwise, robotic finger continues to follow the trajectory of CCP21 . The geometric constraint is given as follows: 2 2 4 i 4 i G(t) = |Ptip Pcp | = ls cθ41 − l2i c θ2j + ls sθ41 − l2i s θ2j , (13) i=1
j =1
i=1
j =1
where ls = 0.5(s2 + s1 ). 3.2. Trajectory generation with CCPs For the sake of simplicity, some key points are chosen from trajectories of CCPs. These points are dependent on the division of the relative orientation angles θij . These points can hence describe the corresponding carton sections. In this case, a particular selection of key CCPs trajectories is provided in joint space as follows: θ11s + θ11e θ12s + θ12e , θ11e , θ11e + , θ12e + θ12e , θ11s , CCP11 : 2 2 θ31s + θ31e θ32s + θ32e θ31s , CCP31 : , θ31e , θ31e + , θ32e + θ32e , 2 2 θ41s + θ41e , θ41e , θ41s , CCP41 : 2
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
55
θ21s + θ21e θ22s + θ22e θ23s + θ23e , θ21e , θ21e + , θ21e + θ22e , θ21e + θ22e + , 2 2 2 θ21e + θ22e + θ23e } ,
θ21s ,
CCP21 :
CCP22 :
θ21s + θ21e θ22s + θ22e θ23s + θ23e , θ21e , θ21e + , θ21e + θ22e , θ21e + θ22e + , 2 2 2 θ24s + θ24e , θ21e + θ22e + θ23e + θ24e . θ21e + θ22e + θ23e , θ21e + θ22e + θ23e + 2
θ21s ,
The definition of a path γi of ith carton section with CCPi , hence, can be given. A folding path γi of carton section is a continuous mapping γi : [CCPi (0), CCPi (ti )] → C, where γi (CCPi (0)) is the initial point and γi (CCPi (ti )) is the final point, ti is constant time used to fold the carton section. 3.3. Paths connection with GGPs The geometry guiding points (GGPs) are introduced when the problem of how to connect two different paths comes up. Basically GGPs are the points to guide a robotic finger to transit continuously from one path of carton section to another, that is to say, to produce a continuous new path to guide a robotic finger based on key points of CCPs from both separated paths GGPs. Hence GGPs should be those kind of points meeting the following rules: (1) the method of GGP is used to estimate the trajectory in a carton-folding task; (2) the new path produced with GGPs meets the trajectory requirement of separated paths; (3) the robotic finger can be guided to the correct direction of a second path. Let two carton section paths be γ1 with CCP1 and γ2 with CCP2 , the folding time be t1 , t2 , respectively, and let ri (CCPi (ti ))j denote j -coordinate of path ri with CCPi relative to ti time moment. Refer to the coordinate system shown in Figs. 2 and 3, the z-coordinates of GGP have to lower enough than γ2 (CCP2 (0))z , the initial position of path γ2 so that interpolated transit path could continue γ2 in suitable direction due to γ2 (CCP2 (0))z in the initial position of carton. Besides x- and y-coordinates of GGP have to be chosen between (γ1 (CCP1 (t1 ))x , γ1 (CCP1 (t1 ))y ) and (γ2 (CCP2 (0))x , γ2 (CCP2 (0))y ) in order to get as short transit path as it can. Finally a generated new path based data of CCPs and GGPs is what will be fed to dual robotic fingers. In carton-folding case, the selected GGP(x, y, z) for path connection of two carton sections conforms the following formula: γ (CCP (t )) + γ (CCP (0)) x y = z
1
1 1
x
2
2
x
2 γ1 (CCP1 (t1 ))y + γ2 (CCP2 (0))y , 2 γ1 (CCP1 (t1 ))z + γ2 (CCP2 (0))z γ2 (CCP2 (0))z − λ 2 where λ is a constant used to tune the folding direction to path γ2 . Hence GGPs are particular points that not only smoothly connect two paths of carton sections, but also ensure that the packing unit can fold the panels in correct direction. In the example here, let λ be 0.5, so GGP1 , [0.3, 0.3, −0.2] , can be calculated out for connection of carton sections 1 and 2, whose trajectory is followed by finger 1; GGP2 , [−0.3, 0, 0.21] , for connection of sections 3 and 4, whose trajectory is followed by finger 2. The reference path parameterised by s, is produced as xdi = p(s),
(14)
ydi = q(s),
(15)
56
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 6. The trajectory of xd2 , yd2 , zd2 , for finger 2, with respect to time t.
zdi = r(s),
(16)
0 ≤ s ≤ sf .
(17)
Hence the spatial reference path can be given by three parametric equations, where the reference trajectory is parameterised in time. The data based on CCPs and GGPs, for fingers 1 and 2, with respect to time t is changed as a continuous reference path by interpolating the generated CCPs and GGPs. The corresponding paths of fingers 1 and 2 are shown in Figs. 6 and 7, respectively, in which the symbol ‘䊊’ presents x-coordinates of fingers 1 and 2 generated based on the CCPs and GGPs, the symbol ‘+’ for y-coordinates and symbol ‘∗’ for z-coordinates of both fingers. The trajectory of z-coordinates of robotic finger 1 moves from −0.2 to 0.2 mm during section 1 folding of the carton, indicating robotic finger 1 in a correct direction to fold carton after a transit period. The same is true for finger 2. The data produced here will be passed to a dual-robotic-finger control system as a reference path.
Fig. 7. Snapshots of a carton-folding process.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
57
4. Carton folding using dual robotic fingers The environment of an automated packaging unit with dual robotic fingers is provided in this section, a trajectoryfollowing control strategy is constructed to make the dual robotic fingers follow the trajectories produced by the above algorithm. This section consists of three parts. The first part generates the minimum time of a carton-folding process based on the time setting of a carton-folding process. The other two parts produce the modelling and control strategy of the packaging unit. 4.1. Minimum time of carton-folding process The total time T [14] to pass along the path between two points is proportional to the geodesic distance S and can be found as follows: 2 3 9S t = 0.5 . (18) Pmax Hence the assumption is made that a carton has m sections, each has n CCPs, the folding process always starts from a home position of the corresponding robotic finger. A minimum time of the carton-folding process can be formulated as n n m m 2 3 9Sij T = tij = 0.5 , (19) Pmax i=1 i=1
i=1 j =1
where S is the distance from the end points along a path length and Pmax the maximum power. S is the near-minimumtime geometric path under the following constraints: (1) there is an upper limit on the total power consumed by the robot’s torque motors; (2) the initial and final velocities are zeros. 4.2. Modelling of the packaging unit with dual robotic fingers The packaging unit consists of dual robotic fingers, which can be viewed as a Puma 560 robot moving along a rail, respectively, as shown in Figs. 8 and 13. It is straightforward to compute the position of the tip of robotic fingers, relative to the rails, rAB , as cos(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 )) B rA = sin(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 )) , (20) li3 sin(φi2 + φi3 ) + li2 sin(φi2 ) + li1 where i = 1, 2 for dual robotic fingers, indexes A, B the coordinate frames for both the robotic fingers and rails, respectively. For a robotic finger with a guide-rail, the position of the fingertip is given as follows: cos(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 ) + xir ) rAi = sin(φi1 )(li3 cos(φi2 + φi3 ) + li2 cos(φi2 )) + yir , (21) li3 sin(φi2 + φi3 ) + li2 sin(φi2 ) + li1 where the tip position of the fingers is calculated relative to a fixed, Cartesian coordinate system and rBi = (xir , yir , 0)T is its global position, respectively. The joint velocities are calculated by differentiating rAi = (xAi , yAi , zAi )T and four free control parameters, φ˙ i1 , φ˙ i2 , φ˙ 3i and y˙ir , can be determined as follows: ˙ yir ]T = J −1 (φi )(˙rAi ), [φ, (22) i
T (r r T )−1 . where Ji−1 (φ) is the pseudo-inverse matrix of rAi , defined by rAi Ai Ai
58
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 8. Configuration of robotic finger.
4.3. Control algorithm of the packaging unit with dual robotic fingers The control system applied to the packaging unit is a trajectory-following system, which controls the fingers to follow a desired position trajectory. The trajectories are given by a set of functions, xdi , ydi , zdi , where i = 1, 2, which specified the desired position of the cartons. The servo errors are defined between the desired and actual trajectories, and conventional PI control algorithm is used for both of two robotic fingers of packaging unit as follows: kip (xdi − xi ) + k1i (xdi − xi ) dt uxi (23) ui = uyi = kip (ydi − yi ) + k1i (ydi − yi ) dt , kip (zdi − zi ) + k1i (zdi − zi ) dt uzi where i = 1, 2 for the dual robotic fingers.
5. Simulation results Due to requirement of carton-folding application, the trajectory-following control does not require to make the fingers follow the desired trajectory precisely, but limit the error of the desired and actual trajectory in a specified range in order to implement the carton-folding task. Hence the analysis of simulation results focuses on the problem of whether or not the actual trajectory of fingers meets the desired trajectory within required error. The models in previous sections give the motions of the carton sections and its corresponding folding process. The geometric parameters of the carton are provided see Table 1. The trajectory errors for fingers 1 and 2 are given in Figs. 9 and 10, it can be seen that the errors quickly reduced to zero after the preparation stage. The whole folding process for one robotic finger in y–z plane is divided into four stages: preparation, section 4 folding (from 2 to 7 s), transit, section 1 folding (from 13 to 21 s). In the preparation stage, the robotic finger move from its initial position to the final position of section 4 of the carton, in which the action does not affect the performance of a carton-folding task. The errors in two folding sections from 2nd second
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
59
Table 1 Geometric parameters of the cartona Panel length (mm)
Start angle (◦ )
End angle (◦ )
l11 l12 l21 l22 l23 l24 l31 l32 l41
θ11s θ12s θ21s θ22s θ23s θ23s θ31s θ32s θ41s
θ11e θ12e θ21e θ22e θ23e θ23e θ31e θ32e θ41e
= 320 = 116 = 321 = 123 = 257 = 36 = 320 = 116 = 320 a
=0 =0 =0 =0 =0 =0 =0 =0 =0
= 90 = 90 = 97 = 81 = 82 = 90 = 90 = 90 = 95
Base wide, bw = 518 mm; base length, bl = 198 mm; Slot1, s1 = 102 mm; Slot2, s2 = 147 mm.
to 7th second and from 13th second to 21st second are much smaller than the limited error. Fig. 11 gives the smooth joint trajectories of robotic finger 1. The same is true for the robotic finger 2, as shown in Fig. 12. The movement of finger 2 mainly consists of three folding stages, section 2 folding (from 1 to 5 s), section 3 folding (from 8 to 13 s) of the carton, and a stage to assist finger 1. It is clear that the errors are suitable for the folding task. The environment of the whole folding process using dual robotic fingers is demonstrated in Fig. 13, in which the proposed method of trajectory generation based on CCPs and GGPs meets the requirement of the carton-folding task.
Fig. 9. The trajectory error of X-, Y -, Z-coordinate in turn with respect to t for robotic finger 1.
60
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
Fig. 10. The trajectory error of X-, Y -, Z-coordinate in turn with respect to t for robotic finger 2.
Fig. 11. The joint trajectory with respect to t for robotic finger 1: (a) the joint trajectory of φ11 , (b) the joint trajectory of φ12 , (c) the joint trajectory of φ13 , (d) the position trajectory of rail1.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
61
Fig. 12. The joint trajectory with respect to t for robotic finger 2: (a) the joint trajectory of φ21 , (b) the joint trajectory of θ22 , (c) the joint trajectory of φ23 , (d) the position trajectory of rail2.
Fig. 13. The environment of the carton-folding process using two robotic fingers.
6. Conclusion An approach to carton-folding trajectory planning using an automated packaging unit with dual robotic fingers has been proposed in this paper. Based on the equivalent mechanism approach, an algorithm based on integration of the CCPs into the GGPs was developed to estimate the trajectory of a carton-folding task. Dual robotic fingers in terms of kinematic models then follow the generated trajectories. The simulation results demonstrated that the
62
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
proposed method is practical to be applied to a multifingered packaging unit and to be used to reduce the number of degrees of freedom required in a complex carton manipulation. Acknowledgements The authors wish to thank the anonymous reviewers for their constructive comments and suggestions which helped in improving the quality of this paper.
References [1] J.C. Latombe, Motion planning: a journey of robots, molecules, digital actors, and other artifacts, International Journal of Robotics and Research 18 (11) (1999) 1119–1128. [2] A. Lazanas, J.C. Latombe, Motion planning with uncertainty: a landmark approach artificial intelligence, Artifical Intelligence 76 (1–2) (1995) 285–317. [3] J. de Vries, A.H. Streppel, E.J.W. Klaassen, L.J. de Vin, H.J.J. Kals, The generation of bending sequences in a capp system for sheet-metal components, Journal of Material Processing Technology 41 (1994) 331–339. [4] U. Sezgin, L.D. Seneviratne, S.W.E. Earles, Redundancy utilization for obstacle avoidance of planar manipulators, Proceedings of Institution of Mechanical Engineers, Journal of Mechanical Engineering Science 211 (4) (1997) 346–355. [5] U. Sezgin, L.D. Seneviratne, S.W.E. Earles, Collision avoidance in multiple-redundant manipulators, International Journal of Robotics and Research 16 (5) (1997) 714–725. [6] L. Sukhan, Dual redundant arm configuration optimization with task-oriented dual arm manipulability, IEEE Transactions on Robotics and Automation 5 (11) (1999) 78–97. [7] Y. Koga, J.C. Latombe, On multi-arm manipulation planning proc, in: Proceedings of IEEE Robotics and Automation Conference (ICRA’1994), San Diego, CA, 1994. [8] T.Y. Li, J.C. Latombe, On-line manipulator planning for two robot arms in a dynamic environment, International Journal of Robotics and Research 16 (2) (1997) 144–167. [9] L. Lu, S. Akella, Folding cartons with frixtures: a motion planning approach, IEEE Transactions on Robotics and Automation 16 (4) (2000) 346–355. [10] S. Guang, M.A. Nancy, A motion planning approach to folding: from paper craft to protein folding, in: Proceedings of IEEE Robotics and Automation Conference (ICRA’2001), Seoul, 2001. [11] J.S. Dai, J. Rees Jones, Mobility in metamorphic mechanisms of foldable/erectable kinds, ASME Transactions 121 (3) (1999) 375–382. [12] H. Liu, J.S. Dai, Manipulation analysis of carton folding task using configuration transformation, Proceedings of Institution of Mechanical Engineers, Journal of Mechanical Engineering Science 216 (2002) 543–555. [13] K.H. Hunt, Kinematic Geometry of Mechanisms, Clarendon Press, Oxford, UK, 1978. [14] Y. Edan, T. Flash, I. Shmulevich, U.M. Peiper, Y. Sarig, Near-minimum-time task planning for fruit-picking robots, IEEE Transactions on Robotics and Automation 7 (1) (1991).
Honghai Liu received his B.Sc. and MEng in Mechatronics Theory and Applications from Wuhan University of Science and Technology and Wuhan Science and Engineering University in 1996 and 1999, respectively. He worked as a project manager in Autotech Automation Engineering Inc., China branch of Siemens-Moore Solution since 1997. He is currently a research associate in the Department of Mechanical Engineering at King’s College London. His current research includes manipulation planning and control using multifingered devices, process control, real-time control.
H. Liu, J. Dai / Robotics and Autonomous Systems 42 (2003) 47–63
63
Jian Dai was born in Shanghai in China. He graduated with a first class degree at Shanghai Jiao Tong University in 1982, received his M.Sc. at the same university in 1985, and obtained his Ph.D. at the University of Salford in UK in 1993. He became a university lecturer in 1985, a visiting scholar to the University of Salford in 1988, a post doctoral research fellow in 1993, a senior research fellow at Unilever research in 1996 and a senior lecturer at the University of Sunderland in 1997. He joined the King’s College London with his research team in 1999. Jian Dai has over 60 refereed publications and was a recipient of the “best paper award” on his ‘metamorphic mechanism’s synthesis’ at the 25th ASME Biennial Mechanism and Robotics Conference in Atlanta in 1998. He has been invited to Italy, Japan and China for many invited lectures and has organised several workshops. Currently, he leads several major research projects and serves on several international committees. His research interests are in mechanisms, manipulation planning, manipulator kinematics and design, multifingered robotic hand, grasping and packaging applications.