Time-Optimal Online Trajectory Generator for Robotic Manipulators

Transcription

1 Time-Optimal Online Trajectory Generator or Robotic Manipulators Francisco Ramos, Mohanarajah Gajamohan, Nico Huebel and Raaello D Andrea Abstract This work presents a trajectory generator or time-optimal maneuvering o a robotic manipulator in jointspace. This generator allows to adapt these maneuvers depending on external events received by the robot (e.g. rom sensors or human input). Closed-orm equations or these trajectories are provided, resulting on a computationally light algorithm which can update the trajectory at any time taking into account the current state o the robot and the inal desired state. The contribution o this paper is an implementation o the algorithm which is available under LGPL or the use o the community. This implementation has been designed to work with KUKA Fast Research Interace (FRI). As a test-bed, a KUKA Light Weight Robot with seven degrees-o-reedom was used, and the experimental results are summarized in this article. The source code o the trajectory generator is available at https: //github.com/idscethzurich/trajectory-generator I. INTRODUCTION Trajectory design is a topic o capital importance in industrial robotics and, as such, it has been developing since irst robots were ever released and there are chapters devoted to the generation o trajectories in almost every textbook on robotics []. Their importance is tightly related to productivity in assembly chains (the aster the trajectories, the higher the production), but also to the durability o the robot elements, such as motors, or the avoidance o vibrations at the tip o the arm. In this paper we deal with online generation o timeoptimal trajectories. While traditional approaches design the trajectory oline [], [3] and execute it aterwards ignoring any new inormation regarding the environment, the online trajectory generation approach attempts to generate trajectories that can react instantaneously to unoreseen external events. This problem is attracting considerable attention lately [4] as robots are beginning to operate outside o the actories where they perormed repetitive tasks in structured environments. New applications demand rom them the ability to interact with their environments, or at least, react to events that happen in them [5]. Thereore, the trajectories have to be modiied during execution time, depending on external actors such as sensor measurements or human commands. In [6], [7], Kröger and Wahl show a general solution to this problem. Every time instant they calculate the next reerence position based on the current state o the robot and the inal desired state. F. Ramos is with the School o Industrial Engineering, University o Castilla-La Mancha, 37 Ciudad Real, Spain rancisco.ramos@uclm.es G. Mohanarajah, N. Huebel and R. D Andrea are with the Institute or Dynamic Systems and Control, ETH Zurich, 89 Zurich, Switzerland {gajan,nhuebel,rdandrea}@ethz.ch While their work proposes a very general solution, it also requires a not-negligible amount o calculations at every time instant, and the classiication stage grows exponentially with the number o derivatives accounted or in the robot state model. In this work we take a dierent approach that saves computation time. We only recalculate the trajectories, whenever a new event arrives to the trajectory generator, while we can get the position rom evaluating a polynomial at every time step. Furthermore, the classiication depends only on two parameters that can be calculated in a straightorward manner. The article is structured as ollows. First, Section II introduces the problem ramework and the notation and provides a general structure or the solution. Then, the velocity proiles o a speciic case are calculated in Section III, and Section IV states a procedure or synchronizing the trajectories o the joints in a maneuver. Finally, the experimental setup is described and the experimental results are analyzed in Section V beore conclusions are drawn. II. TIME-OPTIMAL TRAJECTORIES We aim to ind a amily o trajectories that solve the problem o moving a robot manipulator with m joints rom an initial state to a inal state in a time-optimal ashion. We deine the state o the system at instant t as where X(t) = ( x(t), x(t),..., m x(t) ), () l x(t) = (l p(t), l v(t) ) () represents the state o a single joint l at time t, which consists o position, l p(t) and velocity, l v(t), o the joint. In addition, we deine the acceleration o joint l, l a(t), as the control input to the system. These variables are bounded by the ollowing joint space and control input limits l a m l a(t) l a M l v m l v(t) l v M l p m l p(t) l t (, t ). (3) p M We only consider the kinematic model o the system in this work. Hence, the maneuver can be decoupled or each joint and calculated separately, so we will drop the l index and assume that we are calculating the time-optimal maneuver or a generic joint. We also assume that the robot cannot collide with itsel as long as we are moving within the joint space, which is a common design requirement or industrial manipulators [8].

2 A. Problem solution The time optimal problem or each joint can be stated as: Find the optimal input a (t) that minimizes t dt subject to the ollowing system equations v(t) = dp(t) dt the initial and inal constraints a(t) = dv(t) dt x i = x() = (p i, ) x = x(t ) = (p, v ), and the state and input constraints given by equation (3). This is a well-known control problem that can be solved using Pontryagin s Minimum Principle [9] and the direct adjoining approach to handle the constraints. Then, rom corollary 5. in [], it can be demonstrated that the the control input that provides a time-optimal solution is a piecewise bang-zero-bang proile. The state and control proiles are given by the ollowing polynomial equations (4) (5) p j (t) = a j(t T j ) + v j (t T j ) + p j (6) v j (t) = a j (t T j ) + v j (7) a j (t) = a j, (8) where p j, v j and a j are the initial values o the states and the control input in the j-th piece, and T j is the initial time o the piece. Assuming that actuator limits are identical in both positive and negative directions, the constraints rom (3) become a(t) a M, v(t) v M, p(t) p M, t (, t ), (9) B. Note on the inal velocities The use o inal velocities dierent rom zero raises two additional problems, as not all states that satisy (9) can be reached without implying a previous or posterior violation o those same constraints: Non-decelerable inal state: these states are reachable rom within the easible region o the state-space, but they will conlict with the position constraints ater the end o the trajectory. Assuming that our inal velocity is v, the minimum distance covered when decelerating to zero would be p dec = v v a M. () As the position at rest must be within the limits given by (9), it ollows that p + p dec p M, () or we would not be able to stop the movement without violating the position constraints. Non-reachable inal state: these states are within the easible region o the phase space deined by (9), but cannot be reached without leaving that easible region previously. That is, the distance needed to accelerate to v rom is given by Eq. (), with p acc = p dec. Thereore, or the inal state to be reachable, the position, at rest, rom where we approach it must ulil p p acc p M, () p i p v p + Δpdec p i v p p - Δpacc Fig. : Outline o non-decelerable and non-reachable inal states due to inal velocities From conditions () and () the ollowing additional easibility condition can be derived or the desired inal state o the trajectory can be joined in a single statement as max ( p + p dec, p p acc ) p M. (3) III. VELOCITY PROFILES Depending on the values o the initial state and the inal desired state, we will obtain dierent trajectory shapes. In this work we will use the velocity proile to classiy the trajectories into three cases: critical, over-critical and undercritical. For simpliication, both initial and inal velocities o the trajectory will be considered positive in the analysis, but the results extend to any pair o velocities within the limits given by (9) and (3). A. Critical proile We deine a critical proile that consists o a linear velocity proile between initial and inal velocities at a constant acceleration a M (see Fig. a). This proile deines a threshold between accelerating and decelerating rom the initial state. Let us deine p = p p i and v = v. Then the duration o this proile is t crit = s v v a M (4) and the distance covered during the maneuver is given by ) s v (v p crit = v t crit =, (5) a M where s v = sign( v). In the case p = p crit, no urther calculations are needed, and the piecewise polynomial solution consists o a single piece a (t) = s v a M v (t) = s v a M t + p (t) = s va M t + t + p i t [, t crit ]. (6)

3 v (rad/s) v vi t v (rad/s) vm v vi t t v (rad/s) v vi vm t t p (rad) p pi p crit t t (s) (a) Critical velocity proile p (rad) p p pi t t t (s) (b) Over-critical proiles p (rad) pi p p t t t (s) (c) Under-critical proiles Fig. : Velocity proiles classiication depending on the comparison between p and p crit. The duration o a critical proile is the minimum time that a trajectory with such values o initial and inal velocity can take. When the distance to cover is longer or even shorter than p crit, the duration o the trajectory will be longer than the critical, as we will discuss in the ollowing subsections. B. Over-critical proile ( p > p crit ) In this case, as the distance to cover is longer than the critical, the maneuver will require more time to be accomplished. Let us irst assume that the velocity proile is triangular and, hence, is divided into two parts: ) acceleration, and ) deceleration (see Fig. b). The peak velocity achieved during the motion is given by v po = p a M + ( ) v +. (7) The triangular proile will be easible i v po < v M. Otherwise, the system will ollow a trapezoidal proile with three parts: ) acceleration, ) constant speed at v M, and 3) deceleration (see Fig. b). Equations or both proiles will be treated in Section III-D. C. Under-critical proile ( p < p crit ) This case requests a shorter position movement and, thereore, we may be inclined to think that its duration should be also shorter. However, the inal velocity restriction causes the joint irst to move backwards to reach point = p p crit at velocity, and then perorm a critical maneuver in order to achieve the inal desired state. Hence, the total trajectory will also be longer than the critical. However, the proile shape is similar to that o the overcritical proile. Again, we determine the proile to be triangular or trapezoidal (see Fig. (c))depending on the peak velocity, which, in this case, is given by p crit i v pu = D. Equations o a generic non-critical proile ( ) v + p a M. (8) Under and over-critical proiles can be expressed in a single ormulation by deining the sign o the trajectory as s = sign ( p p crit) (9) and the peak speed (assuming a triangular proile) as ( ) v p = v + + s p a M. () Equation (9) also gives the sign o the initial acceleration o the proile, while () provides a criterion to classiy the proile. ) Triangular proile (v p v M ): This is a bang-bang acceleration proile with two parts: First piece, t (, T ), rom initial velocity to peak velocity a (t) = sa M v (t) = sa M t + p (t) = sa M t + t + p i. () Second piece, t (T, t ], rom peak velocity to inal velocity a (t) = sa M v (t) = sa M (t T ) + sv p p (t) = sa M (t T ) + sv p (t T ) + p (T ). () Switching times: to completely deine this trajectory we only need to calculate T and t, which are given by T = sv p sa M t = T + v sv (3) p. sa M ) Trapezoidal proile (v p > v M ): In this case, the distance to cover is long enough such that the velocity reaches its maximum admissible value. Hence, the proile will have three parts: First piece, t [, T ], rom initial to maximum velocity a (t) = sa M v (t) = sa M t + p (t) = sa M t + t + p i. (4) Second piece t (T, T ], motion at constant velocity sv M a (t) = v (t) = sv M (5) p (t) = sv M (t T ) + p (T ).

4 Third piece t (T, t ], rom maximum to inal velocity a 3 (t) = sa M v 3 (t) = sa M (t T ) + sv M p 3 (t) = sa M (t T ) + sv M (t T ) + p (T ). (6) Switching times: T, T and t can be calculated rom T = sv M sa[ M T = v + vi sv ] M + s p v M a M t = T + v sv M sa M. (7) This is a compact, straightorward ormulation or the eventdriven online trajectory generation problem that unctions or any set o initial and inal conditions (p i, p, and v ) satisying (9) and (3). IV. TRAJECTORY SYNCHRONIZATION Most maneuvers o the robot will have dierent durations or each joint trajectory and the shorter ones will already have inished their motions while the longer ones are still moving. Hence, there is no need or the shorter trajectories to be perormed in a time-optimal way and they could use some o this extra time to make their proiles less demanding or the actuators, hence extending their lietime. This procedure is what we will deine as synchronization: a maneuver is synchronized i all joints arrive at their target positions at the same time instant. To achieve this, new synchronized trajectory proiles that take the inal time as an input have been developed. The synchronization procedure consists o three steps Calculate the minimum time o every joint proile and choose the synchronizing time as the maximum o them t sync = max l t, l =,..., m. (8) Determine the shape o synchronized velocity proile or every joint. Calculate the parameters and coeicients o the proiles. The irst step is straightorward rom the equations given in previous Section, and hence it will not be urther detailed. However, the shape o the synchronized proile is a rich problem in terms o the choice o the design parameters. As we now relax the optimal-time requirement and only ix the inal time o the trajectory, we have a new degree o reedom in the equations, which could be used in a number o ways (i.e. reducing the maximum demanded acceleration). In this paper we approach the problem by minimizing the value o the velocity at the constant velocity piece o each individual joint, v c, such that all joints reach the inal position at time t sync while keeping the acceleration value at its maximum, a M, during the acceleration and deceleration pieces. However, the duration o these pieces will be reduced. With these constraints in mind, the dierent synchronized proiles have been outlined in Fig. 3. For the trajectory v M t opt t lim time optimal trapezoidal limit double ramp Fig. 3: Dierent shapes o the velocity proiles depending on the synchronization time synchronization, we will assume that the inal velocity is zero. We can see that there is a limit case, which separates trapezoidal rom double ramp proiles, when v c =. The duration o this limit case has to be calculated separately or each joint, yielding t lim = p + s a M. (9) Then, comparing this value with t sync shape o the trajectory: trapezoidal proile i t sync, we can determine the < t lim or double-ramp proile otherwise. The expression o v c will dier or each proile Trapezoidal proile: [ b v c = where b = a M t sync + s. Double-ramp proile: b 4 s a M p v i ], (3) s p a v c = M t sync v. (3) i s a M The equations o the synchronized trajectory will be very similar to those o the trapezoidal proile, only substituting v M by v c and including a new sign s t = sign(t lim t sync ), that makes a general ormulation valid or both trapezoidal and double-ramp proiles, yielding First piece, t (, T ): initial to constant velocity. a (t) = s s t a M v (t) = s s t a M t + p (t) = s s ta M t + t + p i. Second piece, t (T, T ): constant velocity at sv M. a (t) = v (t) = sv c p (t) = sv c (t T ) + p (T ). (3) (33) Third piece, t (T, t sync ): rom sv M to inal velocity. a 3 (t) = sa M v 3 (t) = sa M (t T ) + sv c p 3 (t) = sa M (t T ) + sv c (t T ) + p (T ). (34)

5 Obviously, the switching times will be dierent or this proile than those o the optimal trajectory T = sv c s s t a M T = t sync v c. a M V. EXPERIMENTS (35) To show the perormance o our trajectory generator, experimental results will be presented in this section. A. Experimental Setup The trajectory generator has been tested on a KUKA Light Weight Robot (LWR) with seven degrees o reedom. For our sotware setup we were using the Robot Operating System (ROS) [] and the Open Robot Control Sotware (OROCOS) []. We were using a ROS node to generate random joint positions at a rate o s. These positions were sent to our trajectory generator OROCOS component. To couple our trajectory generator with the KUKA FRI the OROCOS LWR FRI component was used. More inormation on this component can be ound at wiki/lwr_ri. ROS OROCOS KRC Next Final Joint Position [ s] Trajectory Generator Commanded Joint Position [ ms] Measured Robot State [ ms] LWR_FRI Fig. 4: Experimental setup UDP Packets [ ms] Whenever the trajectory generator component receives a new inal desired position, it gets the current state o the robot rom the LWR FRI component and calculates the new trajectory to reach the new inal position taking into account the current position and velocity o the robot. Ater calculating the trajectory polynomials, they can be evaluated at the current time whenever the FRI is requesting a new position. This event-based architecture was chosen to avoid synchronization problems between the FRI running on the KUKA Robot Controller (KRC) and the OROCOS and ROS components running on the remote computer. B. Source Code o the Trajectory Generator The trajectory generator is available at com/idscethzurich/trajectory-generator, under LGPL. This repository contains all nodes and components required to run the setup described above (except or the FRI, which must be provided by KUKA). I a real KUKA LWR is not available or the behavior o newly developed code using the trajectory generator should irst be tested without endangering the real robot, a script ile oers to reroute the output to the ROS visualization environment RVIZ (http: // Please be aware that this is only a visualization o the kinematics, which does not take into account the dynamics o the robot. README iles, veloticies(rad/s) angles(rad) (a) Time-optimal veloticies(rad/s) angles(rad) (b) Synchronized Fig. 5: Single maneuver o the robot explaining the content, setup, and use o each package, are provided as well. I you should encounter problems or have questions, eel ree to contact the authors. C. Results Figure 5 shows a single maneuver perormed by the robot using time optimal trajectories or all joints (Fig. 5a) and using synchronized trajectories (Fig. 5b). Notice that when we use the synchronization procedure, the acceleration periods o each joint (except or the longest time-optimal trajectory) are reduced with respect to the set o original time-optimal proiles, while the overall maneuver duration remains the same. The most interesting eature o this work is the ability to interrupt trajectories depending on unoreseen events (human or sensor driven). In Fig. 6, two excerpts o the proiles generated during longer runs o the robot are shown. Fig. 6a shows the joint velocity proiles when the time-optimal trajectories are used in the robot, while Fig. 6b gives an example o the velocity proiles when the joint trajectories are synchronized. The position proiles are smooth during the whole sequence o maneuvers even when they are interrupted by new events. This shows the advantage o considering the current state in the updating o the trajectories, which is one o the main goals o this work. Maneuvers are clearly more abrupt when they are not synchronized, and there are also more rest periods or the joints, while the synchronized motions are smoother but there are no rests in the whole sequence o maneuvers. Velocities are also more steady in the synchronized maneuvers, while they are almost constantly changing in the time-optimal case, showing that the use o acceleration (and hence, motor torques) is reduced with synchronization. VI. CONCLUSIONS In this paper, a trajectory generator or robotic arms that takes into account the kinematics o the arm has been studied. The trajectory proiles have been calculated in a time-optimal manner, obtaining a compact, computationally light ormulation which works or any set o initial and inal states within the robot limits. Due to this, the trajectory generator can be event-driven and dynamically update the current trajectory o each joint whenever a new desired inal state arrives. This is a convenient eature o a trajectory

6 FRI VELOCITIES Robot Side Solid:commanded Dashed:measured joint velocities (rad/s) joint angles (rad) FRI POSITIONS Robot Side Solid:commanded Dashed:measured (a) Non-synchronized maneuvers. joint velocities (rad/s) joint angles (rad) FRI POSITIONS Robot Side Dashed:commanded Solid:measured FRI VELOCITIES Robot Side Dashed:commanded Solid:measured (b) Synchronized maneuvers. Fig. 6: Continuous motion o each joint o the KUKA LWR. New targets state every second. generator, as there are a number o applications in which we would like to modiy the original trajectory based on sensor inormation, especially or service robotics, where the robot must dynamically react to changes in the environment. The trajectory generator has been implemented to work with some o the most used sotware packages in the robotics community (ROS, OROCOS), and is available under LGPL in a public git repository at com/idscethzurich/trajectory-generator. Even i no physical robot is available, the trajectory generator operation and perormance can be checked in simulation ollowing the instructions ound in the code repository. The same approach taken in this paper will be extended in uture works to more general trajectories that include synchronization with non-zero inal velocities, and higher order control inputs (jerk, snap). VII. ACKNOWLEDGMENTS This research was unded by the European Union Seventh Framework Programme FP7/7-3 under grant agreement no RoboEarth. [3] H. Geering, L. Guzzella, S. Hepner, and C. Onder, Time-optimal motions o robots in assembly tasks, Automatic Control, IEEE Transactions on, vol. 3, no. 6, pp. 5 58, jun 986. [4] J. Kim, S.-R. Kim, S.-J. Kim, and D.-H. Kim, A practical approach or minimum-time trajectory planning or industrial robots, Industrial Robot: An International Journal, vol. 37, no., pp. 5 6,. [5] J. Lloyd and V. Hayward, Trajectory generation or sensor-driven and time-varying tasks, The International Journal o Robotics Research, vol., no. 4, pp , 993. [6] T. Kröger, A. Tomiczek, and F. M. Wahl, Towards on-line trajectory computation, in Proc. o the IEEE/RSJ International Conerence on Intelligent Robots and Systems, Beijing, China, oct 6, pp [7] T. Kröger and F. M. Wahl, On-line trajectory generation: Basic concepts or instantaneous reactions to unoreseen events, IEEE Trans. on Robotics, vol. 6, no., pp. 94, eb. [8] M. Shimizu, H. Kakuya, W.. Yoon, K. Kitagaki, and K. Kosuge, Analytical inverse kinematic computation or 7-do redundant manipulators with joint limits and its application to redundancy resolution, IEEE Transactions on Robotics, vol. 4, no. 5, pp. 3 4, 8. [9] D. P. Bertsekas, Dynamic Programming and Optimal Control. Athena Scientiic, 7. [] H. Maurer, Optimal-control problems with bounded state variables and control appearing linearly, SIAM Journal on Control and Optimization, vol. 5, no. 3, pp , 977. [] Willow Garage, Robot Operating System (ROS), 9, [Last visited ]. [] H. Bruyninckx, Open Robot COntrol Sotware,, [Last visited ]. REFERENCES [] B. Siciliano, L. Sciavicco, L. Villani, and G. Oriolo, Robotics. Modelling, Planning and Control. Springer, 9. [] K. G. Shin and N. D. McKay, Minimum-time control o robotic manipulators with geometric path constraints, Automatic Control, IEEE Transactions on, vol. 3, no. 6, pp , jun 985.