Predictive method for balance of mobile service robots

Transcription

1 Predictive method for balance of mobile service robots Bastings, B.M.; Nijmeijer, H.; Kostic, D.; Kiela, H.J. Published: 1/1/214 Document Version Publisher s PDF, also known as Version of Record (includes final page, issue and volume numbers) Please check the document version of this publication: A submitted manuscript is the author's version of the article upon submission and before peer-review. There can be important differences between the submitted version and the official published version of record. People interested in the research are advised to contact the author for the final version of the publication, or visit the DOI to the publisher's website. The final author version and the galley proof are versions of the publication after peer review. The final published version features the final layout of the paper including the volume, issue and page numbers. Link to publication Citation for published version (APA): Bastings, B. M., Nijmeijer, H., Kostic, D., & Kiela, H. J. (214). Predictive method for balance of mobile service robots. (D&C; Vol ). Eindhoven: Eindhoven University of Technology. General rights Copyright and moral rights for the publications made accessible in the public portal are retained by the authors and/or other copyright owners and it is a condition of accessing publications that users recognise and abide by the legal requirements associated with these rights. Users may download and print one copy of any publication from the public portal for the purpose of private study or research. You may not further distribute the material or use it for any profit-making activity or commercial gain You may freely distribute the URL identifying the publication in the public portal? Take down policy If you believe that this document breaches copyright please contact us providing details, and we will remove access to the work immediately and investigate your claim. Download date: 17. Nov. 218

2 Predictive Method for Balance of Mobile Service Robots Bart Bastings ID: DC Traineeship report Coach(es): Supervisor: H. Kiela, Fontys mechatronics lab D. Kostić, TU/e Prof.dr. H. Nijmeijer, TU/e Technische Universiteit Eindhoven Department Mechanical Engineering Dynamics and Control Technology Group Eindhoven, August, 214

3 CONTENTS i Contents 1 Introduction Problem Description Goals Outline Robot Balance Determining Balance Definition of Balance Measuring Balance Off-line Simulations Prediction During Motion Planning Predictive Methods Literature Overview 6 4 Zero-Moment Point Definition Computed ZMP Cart-Table Model Computed ZMP for Multibody Systems Summary Robot Kinematics Forward Dynamics Summary ZMP Algorithm Input Calculations Summary SimMechanics Test Framework Base Transform ZMP Calculation Robot Links Summary Simulations Pole on Cart Simulation Balance by Torso Leaning SimMechanics Simulation Mobile Service Robot Simulation Simulation for an Intrinsically Balanced Mobile Robot Simulation of a Mobile Robot with a Small Support Polygon Simulation of a Mobile Robot with a Slightly Bigger Support Polygon Concluding Remarks on Comparison Between the ZMP Algorithm and Sim- Mechanics Model Joint Flexibility Normal Maneuver with Joint Flexibility Third Order Motion Profile for Robot with Flexible Dynamics Summary

4 CONTENTS ii 9 Discussion Conclusion Recommendations Coping with Flexibility Appendix A Homogeneous Transformations 43 Appendix B Denavit-Hartenberg Convention 44 Appendix C Forward Dynamics 45 C.1 General Case C.2 Revolute Joint C.3 Prismatic Joint Bibliography 48

5 1 INTRODUCTION 1 1 Introduction In recent years, interest in robotics for care and cure has been growing due to advances in robotics and the increase in elderly people in most western nations. The increasing population of elderly means that more and more people are in need of care, but fewer people (in percentage of the total population) are able to administer this care. One way to deal with this problem is the use of technology in care and cure situations. When robots can perform tasks that are normally being performed by nursing personnel, the same amount of care can be given with less people. The advances in robotics technology, cheap sensors and the availability of sufficient portable computing power, have resulted in increasing number of knowledge institutions being involved in robotics projects in the care and cure sector. Robots could be used for various tasks, from the demanding and dangerous to simple daily ones. The most of attention nowadays is given to robots performing simple daily tasks and assisting nurses in their work. When the field of (care) robotics advances further, more complicated tasks should be performed by these robots. Because most of the focus is in the robots performing simple tasks for the elderly and handicapped, it does not surprise that the majority of the robots for such application look alike. These robots all have a wheeled base for moving inside the house or nursing home and a torso with arms to perform tasks like reaching and grasping, see for instance Figure 1.1. Figure 1.1: Service robot Amigo (left) and Robot Rose (right) The mechatronics lab of Fontys university of applied sciences is also involved into (applied) research on mobile service robots. A major concern there is robot safety. This lab has already some knowledge in the fields of robot navigation, autonomous driving, 3D vision and industrial robotics. Because of the applied nature of the lab at Fontys, for most robotic applications standard existing toolboxes and software are used or adapted to fit requirements of the particular application. 1.1 Problem Description Mobile service robots can be useful in the tasks they are designed for only if they are safe for humans and working environments. All advantages of the use of robots in house, hospital or care homes are worthless if the robot creates an unsafe environment.

6 1 INTRODUCTION 2 Significant concern with respect to the robot safety are the events when a mobile robot gets out of balance and falls over. In such situations humans can be hurt by the falling robot while the robot and/or its environment can get damaged. To avoid such situations, the robot operator has to be sure that the robot remains in balance during its operation. Unlike humans, the existing wheeled robots often lack the ability to detect if they are tipping-over or that they are going to tip-over when conducting a certain movement. Consequently, these robots have to be handled very carefully. This is both inconvenient and risky. Considering the robot safety in general and tip-over detection in particular, the mechatronics lab at Fontys does not have standard toolboxes available that can readily be used. Also general knowledge about these topics is lacking. These are the reasons why some research into these topics has been conducted in the scope of this internship. 1.2 Goals The primary goal of this internship is to work out a method for evaluation of dynamic balance of mobile service robots. This method should be able to determine whether the robot would remain balanced along a motion trajectory supplied by the given motion planner. If the method indicates that the balance cannot be guaranteed during that motion trajectory (the robot could tip-over), a new trajectory should be planned. The resulting method for dynamic balance prediction should be usable at the lab of Fontys by students working on robotics projects without too many adaptations since the focus on applied research demands a more hands-on approach for these students. Furthermore, because of the widespread use of Robotic Operating System (ROS) in the Fontys mechatronics lab, the method for balance prediction should be integrated within the ROS environment. Besides the method for balance prediction itself, a test framework should be developed that could be used to simulate different kind of mobile robots and verify the results of the balance prediction. The last goal is evaluation of dynamic balance of mobile robots that contain flexible robot links or robot joints. In the world of industrial robotics, the robot links and joints are designed for high accuracy and hence are very stiff. For service robots this is not always the case. Also, the base of a mobile robot may contain suspensions of low stiffness that can greatly influence the stiffness of the end-effector of the robot. These are the reasons why flexibilities in the robot dynamics need to be taken into account when predicting dynamic balance. Current research into robotics often neglects these flexibilities, in this internship however some insights on how to cope with this have to be developed. 1.3 Outline In this report, dynamic balance is evaluated for wheeled robots with torso and arms, such as Amigo and Rose service robot depicted in Figure 1.1. Legged robots are not considered in this report. To be able to detect when a robot loses its balance, the meaning of balance has to be defined. This is done in chapter 2. Also some first ideas about a method to evaluate the robot balance are stated there. In chapter 3 relevant literature is reviewed. Chapter 4 explains the Zero-Moment Point as a method to predict robot balance. The Zero-Moment Point relies on the knowledge of the robot dynamics. These dynamics are computed in chapter 5. With the results from chapters 4 and 5 the Zero-Moment Point algorithm for evaluation of dynamic balance of mobile service robots is developed in chapter 6. In chapter 7 a SimMechanics test framework is presented. This framework allows calculation of the center of pressure for a mobile service robot. The center of pressure can be used to validate the calculated ZMP. Also this test framework makes it possible to quickly model a robot with flexible links and inspect the influence of these flexibilities on the location of the center of pressure and the ZMP.

7 1 INTRODUCTION 3 Some simulations are performed to verify the results of the ZMP algorithm in chapter 8. Here a SimMechanics model is used to validate results of the ZMP algorithm and investigate the influence of flexible dynamics on the dynamic balance.

8 2 ROBOT BALANCE 4 2 Robot Balance In the traditional robotics industries (car manufacturing, packaging, electronics assembly), robots are very trustworthy employees. They do their job without complaining and do it fast, 24 hours per day and with very high quality. A limiting aspect is quality of their interaction with people. In most factories the robots are in cages, to prevent human workers to come near the fast moving clumps of metal that aren t aware of their environment. In an assembly line, this is no problem because these lines are designed in such a way that there is no need for robot-human interaction. Safety is guaranteed by keeping the robots and humans separated. Mobile service robots, on the other hand, do their jobs when people are around them, since that is what they are designated for. Thanks to mobility they can move around the house or hospital and have arms and hands (grippers) for grasping objects, to give or receive them from humans. Most robots have camera s on the top of their torso so that they can see their environment, or that an operator on remote site can see what the robot is doing and send out new commands to the robot. Every part of the robot is designed in such a way that the robot is able to help people live their lives. This means that the safety precautions from the assembly line are not going to work for mobile service robots. The mobile service robot operates in almost opposite environment than of that of an industrial robot; there can t be a separation between humans and robots, and even more challenging, most humans the robot needs to work with are untrained in collaborating with robots. Considering the above, it is of utmost importance that a service robot is inherently safe to work with humans. This means that the robot needs to detect unsafe situations to prevent accidents. One unsafe situation that could occur is tipping over of the robot since that can cause harm to anybody in the neighborhood. This tipping over could happen when a robot is driving at high accelerations, when it has to make an emergency stop, when it carries a heavy load or when the robot crosses over an obstacle, or when it is disturbed, that is, when something or somebody applies a force to the robot just hard enough to let it tip over. 2.1 Determining Balance One could think of several ways to determine the balance of the mobile service robot, each one has its pros and cons. In this section several ideas are explored to form the basis of a literature study on methods for determining the balance of mobile service robots Definition of Balance In general, for a service robot to be able to work safely around humans, it is needed to quantify how close it is to tipping over, and adjust the movement such that it will not result in tipping over. In absence of a better definition of tipping over, the term balance will be used. This means that when a mobile robot is balanced, it will touch the ground only with its support, not with any other part of the robot. If a movement causes the robot to touch the ground with another part of its body, it means the robot has tipped over, and thus, was not in balance Measuring Balance One way of measuring the balance is measuring the force on the wheels of the mobile robot. This will give an indication of the center of pressure of the complete robot. Together with knowledge about the support polygon, the area spanned by the wheels of the robot, balance can be determined by checking if the center of pressure is within this support polygon. It is also possible to quantify the level of balance by comparing the position of the center of pressure to the edges of the support polygon. In dynamical situations, where the robot is accelerating, decelerating or turning around its axis, this method is also usable. Dynamic forces are measured so the dynamic center of pressure can be determined. This method can be used to predict when the robot is going out of a balanced situation by looking at the center of pressure and comparing that with the support polygon. This method is reactive because it can only detect if the robot is near the edge of balance when a particular trajectory

9 2 ROBOT BALANCE 5 is already planned and the motion has started. It will be hard if not impossible to guarantee balance and prevent tipping over for a planned trajectory with this method Off-line Simulations A totally different way of determining balance would be simulating the robot and its movements offline and analyze the balance. When an accurate robot model is available this could be a useful method, because it enables making predictions about balance before the real robot even moves. If all the movements the robot is ever going to perform are simulated in advance, a prediction of safe movements could be made. Under the assumption that the robot would never be disturbed by unknown forces, this method would almost guarantee balance as long as the robot only performs those pre-simulated movements. This would greatly limit the usefulness of the robot because it can not perform tasks which where not simulated before. Also this method cannot handle disturbances Prediction During Motion Planning The third balance determination method considered is predicting balance of the robot during motion planning. In this case the robot has a predictive algorithm on board, that can predict balance when given the planned motion of the robot. This method is a variation on the off-line simulation method mentioned above, because it also tries to predict balance instead of measuring it directly like in the first method mentioned. The big difference is that in this method there are no restrictions on the movements the robot could make. The motion planner of the robot creates a motion profile and the balance algorithm calculates balance for that planned motion. This can all be done right before the start of a movement. So in stead of creating a list of safe movements off-line and only performing those movements, every movement is checked just in time. When the algorithm has good predictive value, the robot would know in advance that a movement could lead to unbalance and thus plan a different movement. Without disturbances and a perfect algorithm (model), continuous balance could be guaranteed, even when the robot is creating movements without human intervention. 2.2 Predictive Methods Considering the discussion above, it seems practical to seek a predictive algorithm that can be used online during motion planning. When such a method exists and can effectively used, it has considerable advantages over the other methods mentioned. Also, these methods are not mutually exclusive to each other. When designing a mobile service robot, it is always wise to simulate behavior and balance off-line. Also, even when a predictive algorithm is used, measurements are still useful for considering balance. For instance, perturbations around the planned motion could be detected and accounted for on-line.

10 3 LITERATURE OVERVIEW 6 3 Literature Overview In recent research a few different methods are used to analyze the balance of legged (walking) and wheeled robots. When taking into account the ideas from chapter 2, foremost the ability to predict balance pro-actively and in advance in stead of re-actively, one method clearly stands out: the Zero- Moment Point (ZMP). In short, the ZMP is a virtual point on the floor where all forces and moments on the robot sum to zero. When this point is within the support polygon of the robot, the robot is balanced. Also, when the ZMP is within the support polygon, it coincides with the center of pressure. For a more detailed explanation of the Zero-Moment Point see chapter 4. Vukobratović and Stepanenko [15] were the first to use the term Zero-Moment Point. They used this ZMP to analyze a bipedal walking robot and to design a stable walking gait. The idea of the ZMP was also applied to wheeled robots (mobile manipulators) by Sugano, Huang and Kato [13]. They used the Zero-Moment Point as a way to determine how close a mobile manipulator comes to tipping over while the manipulator is performing a certain task. They state that when the Zero-Moment Point is within the support polygon of the mobile manipulator, the mobile manipulator is balanced. They also introduce the term Stability Degree, which they use to quantify the level of balance. In another article [8] they use the ZMP and the stability degree to create a motion profile for a mobile manipulator that guarantees balance of the mobile manipulator in the ZMP sense. The ZMP of a planned motion of the mobile manipulator is calculated, and in the case of a low stability degree (close to tipping over), a new motion profile is calculated with a potential function (penalty function) to push the ZMP into the support polygon. Huang, Sugano and Tanie [9] take this concept even further and propose a method to compensate a low stability degree with the control of the posture of a redundant manipulator. In this case also the ZMP is used to determine balance of the robot. Furthermore, in [11], the ZMP method is also used to determine stability of bipedal walking robots. A clear distinction is made between the measured ZMP in the case when ground reaction forces are known, and the computed ZMP case when the location of the ZMP is predicted by computing the motion of all parts of the robot and calculate resulting forces and torques from there. Moreover, two forms of this computed ZMP are given. A full 3D dynamics computed ZMP and a simplified Carttable model, where a robot is generalized to one accelerating center of mass. This cart-table model makes some computations easier, but on the other hand still some kinematic calculations have to be performed. Besides the computed ZMP, also two methods to calculate dynamics of the robot are given, a recursive Newton Euler algorithm and a Lagrangian dynamics based method; knowledge of the robot dynamics is required for calculation of the ZMP based on a complete 3D model. A critical discussion about the difference between the recursive Newton Euler algorithm and a Lagrangian based method for modeling robot dynamics can be found in [12]. There it is concluded that computationally either method has no advantages above the other one. Yanjie, Zhenwei and Hua in [16] analyze balance of a wheel based humanoid robot. They also use the Zero-Moment Point as a dynamic stability criterion for their robot. For modeling the robot dynamics, they use a recursive Newton Euler algorithm. Antoska, Jovanović, Petrović, Baščarević and Stankovski [1] continue on the work of Yanjie [16], and use a recursive Newton Euler algorithm for dynamic modeling of a mobile anthropomimetic robot. They analyze the balance of their robot using the ZMP criterion. They also investigate the influence of disturbances on the ZMP for a compliant torso on a wheeled platform. In their article about balance in dynamic situations, Hoyet, Multon, Mombaur and Yoshida [7] compare the center of pressure measured with a force plate with the multibody Zero-Moment Point, an extrapolated center of mass method and the ZMP for a pendulum-like system. They conclude that the extrapolated center of mass method is probably too conservative, as this method may indicate an unbalanced situation while the measured center of pressure is still within the support polygon. The two ZMP methods are also conservative in their measurements, the multibody ZMP performs better than the pendulum-system ZMP. They assume that using a better dynamic model (including inertia) will give better results for the multibody ZMP. A different way to characterize stability of a mobile manipulator is proposed by Papadopoulos

11 3 LITERATURE OVERVIEW 7 and Rey [1]. They define a Force-Angle stability measure directed at large forestry machines. This measure uses a resulting force trough the center of mass and the angle between that force and a tipover point. When only considering forces (no torques) this method is very straightforward. When torques are taken into account, they have to be replaced with equivalent forces trough the center of mass, and this criterion starts to look a lot like the ZMP criterion. Also, this force-angle measure has to be evaluated against every tipping point possible. This method does not seem to provide an advantage over the ZMP method. Zutven, Kostić and Nijmeijer [17] recognize the difficulties with the term stability concerning robot balance in contrast to the notion of stability from control systems theory. They propose the following definition: A bipedal walking is stable if it is carried out without falling. A biped falls when any other point than points on the feet come in contact with the ground.. They design two gaits for a planer bipedal robot, one with a ZMP method and the other based on Limit-Cycle Walking. Both gaits are analyzed for stability with two different methods, the ZMP method and the Poincaré Return Map method. The Poincaré Return Map qualified both gaits as stable, which exemplifies the conservative nature of the ZMP criterion. While the ZMP criterion guarantees stability when the ZMP is within the support polygon, the opposite is not true. A disadvantage of the Poincaré Return Map is that it is only usable for periodic gaits. In [5], Guy Dohmen designs a new torso for a mobile service robot. Balance of the robot is also considered. For static balance the center of mass of the robot is used, for dynamic balance the center of mass of the robot is used together with the acceleration of the robot to determine the moments around the possible tipping points of the robot. This is used to calculate the maximum allowable acceleration for a given configuration of the robot. Also the maximum allowable velocity of the robot is calculated by considering the equilibrium between potential and kinetic energy for the robot in case of emergency braking. Dekker gives an overview of methods to analyze balance of a bipedal walking robot [4]: the Zero- Moment Point, Floor Projection of the Center of Mass (FCoM) and the Center of Pressure (CoP). When the ZMP is within the support polygon it coincides with the center of pressure, and thus it is not necessary to consider both methods when evaluating balance. The floor projection of the center of mass may give quite different results compared to the ZMP. According to figure 5.1 in [4], the ZMP seems to be the better method in this regard, the FCoM does not take accelerations into account and thus gives a too optimistic representation of the balance. Bouten [2] researched balance of mobile service robot Rose. The dynamic balance of this robot was determined using the ZMP method without rotational inertia and with a SimMechanics simulation of the center of mass. In both cases only movements of the robotic arms are considered. The robot itself (base and torso) remains at the same location. When the robotic arms move slowly there is only a slight difference between the center of mass and the ZMP, when the arms are moving faster this difference becomes greater. In the SimMechanics simulation, the effects of the suspension of the robot base can be seen. This suspension gives rise to oscillations when the arms start to move, as a result of joint torques. Considering the aforementioned literature and the desire to predict the balance of a robot during motion planning, it seems obvious that the ZMP method is a good candidate. However, while there already has been a lot of research on balance of mobile robots with the ZMP method, there does not seem to be a readily available piece of code that incorporates the ZMP method. Moreover, most of the available literature does only consider rigid robot dynamics when determining balance. That is why in this internship a special attention is given to study influence of flexible dynamics onto the balance of mobile service robots.

12 4 ZERO-MOMENT POINT 8 4 Zero-Moment Point For the Zero-Moment point the following holds: ZMP (zero-moment point) is a point on the floor where the resultant moment of the gravity, the inertial force of the mobile manipulator and the external force is zero. If the ZMP is within the support polygon, the mobile manipulator is balanced. [13] This means that when the pose of the robot is known, the movements of the robot are known (the inertial force of the mobile manipulator) and the external forces on the robot are known, it is possible to calculate the point on the floor where all these moments sum to zero. When this point is within the support polygon of the robot (the base, the foot, or the area spanned by the wheels), the robot is balanced in the sense described in section 2.1. Because this zero-moment point involves the inertial forces (acceleration, centripetal forces), it accounts for dynamic effects. This means that the ZMP-method is a suitable method to calculate the dynamic balance of a mobile robot. 4.1 Definition Vukobratović [15] coined the term ZMP: In Figure 4.1 an example of force distribution across the foot is given. As the load has the same sign all over the surface, it can be reduced to the resultant force F P, the point of attack of which will be in the boundaries of the foot. Let the point on the surface of the foot, where the resultant F P passed, be denoted as the zero-moment point, or ZMP in short. z f i z F P x y x r P y r i P Figure 4.1: Force distribution with individual forces f i (left), resultant force F P and point of attack P (right). In the situation shown in Figure 4.1, all reaction forces f i and contact points r i for the reaction forces are known, and can thus be reduced to one resultant force F P at the contact point P with position vector r P. The force vectors f i have the form: and the position vectors r i : f i = [f i,x, f i,y, f i,z ] T (4.1) r i = [r i,x, r i,y, r i,z ] T (4.2) where the components are defined along the axes of the coordinate frame x-y-z depicted in Figure 4.1. The center of pressure (CoP) can be calculated using the following formula [11]: r P = n i=1 r if i,z n i=1 f i,z (4.3)

13 4 ZERO-MOMENT POINT 9 According to this definition, the CoP always exists within the support polygon (shaded area in Figure 4.1), because it uses the reaction forces on this support polygon. When the CoP is known, the torque around it can be calculated: τ = n (r i r P ) f i (4.4) i=1 where r P is the location of the CoP as calculated in (4.3) and r i and f i are the individual position and force of the reaction forces. When substituting components of the vectors in equation 4.4, the following equations are obtained: τ x = τ y = τ z = n (r i,y r P,y ) f i,z i=1 n (r i,z r P,z ) f i,x i=1 n (r i,x r P,x ) f i,y i=1 n (r i,z r P,z ) f i,y (4.5) i=1 n (r i,x r P,x ) f i,z (4.6) i=1 n (r i,y r P,y ) f i,x (4.7) When the support polygon is parallel to the xy-plane the following holds: r i,z = r P,z. Moreover, when the support polygon lies in the xy-plane, these components become r i,z = r P,z =. In either case, the second term in (4.5) and the first term in (4.6) become zero. The definition of the CoP (4.3) can be used to further reduce (4.5) and (4.6): i=1 τ x = τ y = (4.8) The remaining torque around the z-axis (τ z ) is nonzero. The fact that τ z is nonzero has no practical consequences when it is assumed that there is no slip between support polygon and ground. In that case, the vertical torque τ z is canceled by friction forces. The fact that the torques in x and y direction are zero is the reason why this point is also called the Zero-Moment Point. 4.2 Computed ZMP The definition in section 4.1 assumes that there is a finite number of reaction forces that can be summed to one resultant force. If all these are known, the CoP can be calculated. By using pressure sensors on the robot base, this is a convenient way to practically measure the CoP location. This information can be used to control the robot balance online. However, it is not possible to make a prediction about the robot balance resulting from a motion profile using this method. It can only act on already measured forces, not on a planned motion. The Computed ZMP is a way to overcome this problem. It works by using known information about the robot (mass of links, position and type of joints, velocities and accelerations) to calculate the resulting reaction force and torque on the base of the robot, and from there on find a point where the reaction torques become zero. This point is the computed ZMP. In Figure 4.2 the support polygon of the robot is the shaded area. This could be determined by the actual position of a mobile base of a service robot, a foot of a bipedal robot or an ordinary base of a stationary robot. The moving robot exerts forces and moments onto this support polygon, due to accelerating and rotating parts of its body. These forces and moments can be represented by one resultant force F R and one resultant moment M R acting on point R. A reaction force F P and moment M P can be calculated for any point P to keep the robot in balance, i.e. such that all forces and moments sum to zero. There is one location for point P where the reaction moment M P is zero, this is the Zero-Moment Point.

14 4 ZERO-MOMENT POINT 1 z M R F R x P r P O y r R R F P M P Figure 4.2: Resulting force F R and moment M R from the robot on its support polygon (shaded area), and a reaction force F P and moment M P, to keep the robot in balance. To sum up, it is necessary for the forces and moments to sum up to zero for the robot to be in balance. This gives the equations for equilibrium: F P + F R = (4.9) M P + M R + r P F P + r R F R = (4.1) Written out in vector components equation (4.1) becomes: M P,x + M R,x + r P,y F P,z r P,z F P,y + r R,y F R,z r R,z F R,y = (4.11) M P,y + M R,y + r P,z F P,x r P,x F P,z + r R,z F R,x r R,x F R,z = (4.12) M P,z + M R,z + r P,x F P,y r P,y F P,x + r R,x F R,y r R,y F R,x = (4.13) When the base and the points R and P are in the xy-plane, the r z components become zero. Moreover, when it is assumed that there is no slip between base and ground, the forces in the xyplane and the moments around the z-axis are canceled due to friction. This results in: M P,x + M R,x + r P,y F P,z + r R,y F R,z = (4.14) M P,y + M R,y r P,x F P,z r R,x F R,z = (4.15) The ZMP is the point where the moment M P becomes zero (by definition), this reduces the equilibrium equations to: M R,x + r P,y F P,z + r R,y F R,z = (4.16) M R,y r P,x F P,z r R,x F R,z = (4.17) Together with (4.9) these equations can be used to calculate the ZMP (r P ). 4.3 Cart-Table Model A well known approximation to the computed ZMP is the so called cart-table model [11], shown in Figure 4.3. In this figure a mass M is on a massless table, and has an acceleration of ẍ. The position of mass M with respect to origin O is given by r M,x and r M,z in the x and z direction respectively. The ZMP (point P ) can be calculated with (4.9) and (4.17). In this case the resulting forces and moments are taken around point R. Equilibrium around R results in: F R,z = mg (4.18) F R,x = mẍ (4.19) M R,y = (r M,x r R,x )mg r M,z mẍ (4.2)

15 4 ZERO-MOMENT POINT 11 M ẍ r M,z g z O x r R,x R P r P,x r M,x Figure 4.3: Cart-table model Assuming no-slip conditions, the force in x direction vanishes. Using (4.9) and (4.17) gives: F P,z = mg (4.21) r P,x mg = (r M,x r R,x )mg r M,z mẍ + r R,x mg (4.22) r P,x = r M,x r M,z g ẍ (4.23) This shows that the ZMP can be anywhere on the x axis, depending on the acceleration ẍ of the mass. Also, from (4.22) it can be seen that the (arbitrarily chosen) location of point R has no consequence in the resulting ZMP position. As long as the computed ZMP is within the foot of the table, this system is in balance. When the ZMP leaves the foot of the table there is no equilibrium anymore, which will result in the table tipping over. This cart-table model can be used to represent an entire robot, the mass M represents the mass of the entire robot while x and ẍ denote motion and acceleration of the actual center of mass of the robot in the x-direction 4.4 Computed ZMP for Multibody Systems In the case of a service robot, the computed ZMP method needs to be adapted to handle multibody systems. In essence this means that a method is needed to compute the resultant force F R and resultant moment M R indicated in Figure 4.2 from the known kinematic information (position, velocity, acceleration, mass and inertia) of each robot link. Figure 4.4 shows a mobile robot consisting of n rigid parts (links), each with its own mass m i and body fixed inertia tensor I i. Furthermore, r ci, R i and ω i are the position vector, the rotation matrix and angular velocity of the center of mass of the i-th link respectively. The total mass and the center of mass of the robot can be calculated with: m = r c = n m i (4.24) i=1 n i=1 m i r ci m (4.25) Assuming that all kinematic information is available (measured or calculated by robot kinematics) and that the no-slip condition holds, the linear and angular momenta of this system of rigid bodies can be used to calculate the resultant force and moment with respect to the origin O. For these

16 4 ZERO-MOMENT POINT 12 R i ω i m i I i r ci z P x O y r P Figure 4.4: Multibody model of a mobile robot and reference frame O. calculations, Newton-Euler equations can be used that state that the sum of external forces and the sum of external moments result in change of linear and angular momenta respectively [14]: F = Ṗ (4.26) M = Ḣ (4.27) where Ṗ and Ḣ are change in Linear momentum and angular momentum respectively. These quantities can be computed using forward kinematics relationships for the robot. For now it is just assumed that Ṗ and Ḣ are known. When the mobile robot is in balanced equilibrium, the external forces and moments (F E and M E ) that cause the movement of the robot and consequently the change in P and H, are in equilibrium with a resulting force (F R ) and moment (M R ). These are related to each other via: F E = F R and M E = M R. Together with gravitational effects, this result in: F = F E + mg = F R + mg = Ṗ (4.28) M = M E + r c mg = M R + r c mg = Ḣ (4.29) Here g is a 3D gravitational acceleration vector: g = [g x, g y, g z ] T. The resulting force and moment are given by: F R = Ṗ + mg (4.3) M R = Ḣ + r c mg (4.31) All these forces and moments are given with respect to the origin O, which means that point R in Figure 4.2 coincides with the origin O, resulting in r R =. Combining this with (4.9) and (4.1) gives: F P = F R (4.32) M P = Ḣ r c mg r P (mg Ṗ ) (4.33) Now, by definition, the first and second component of M P are zero. When written in vector components this results in: M P,x = = Ḣx r c,y mg z + r c,z mg y r P,y ( P z mg z ) + r P,z ( P y mg y ) (4.34) M P,y = = Ḣy r c,z mg x + r c,x mg z r P,z ( P x mg x ) + r P,x ( P z mg z ) (4.35)

17 4 ZERO-MOMENT POINT 13 With the assumption that the floor lies in the xy-plane (r P,z = ), the ZMP can be calculated: r P,x = Ḣy + r c,z mg x r c,x mg z (4.36) P z mg z r P,y = Ḣx r c,y mg z + r c,z mg y (4.37) P z mg z From this, it can be seen that when the robot is stationary the ZMP coincides with the projection of the center of mass. Important to notice here is that it is assumed that the x and y component of the gravity vector are not always zero. This is a result of the xy plane of the reference frame being defined such that the floor is in this plane. In case of an inclined floor, the gravity vector is not aligned with the z-axis of the reference frame anymore. The final step in these calculations is the determination of the change in linear and angular momenta. With respect to the origin, the linear momentum is given by [11]: P = where ṙ ci is the linear velocity of the center of mass of the i-th link. Furthermore, the angular momentum is given by [11]: H = n m i ṙ ci (4.38) i=1 n r ci m i ṙ ci + I i ω i (4.39) i=1 where I i is the inertia tensor of link i with respect to the ground fixed reference frame with origin O. This is calculated from the body fixed inertia tensor I i using: I i = R i I i R T i (4.4) Here, R i is the rotation matrix that describes the rotation of link i with respect to the reference frame, see appendix B. The rates of change in the linear and angular momenta are the time derivatives of (4.38) and (4.39) respectively. The inertia matrix I i in equation (4.39) has a time dependency (due to the rotation matrix R i ), so the time derivative of this matrix is also needed [14]: which results in a change of linear momentum: and for the change in angular momentum: Ḣ = = İω = ω Iω (4.41) Ṗ = n m i r ci (4.42) i=1 n ṙ ci m i ṙ ci + r ci m i r ci + I i ω i + ω i (I i ω i ) (4.43) i=1 n r ci m i r ci + I i ω i + ω i (I i ω i ) (4.44) i=1 where the cross product ṙ ci m i ṙ ci is always zero. To sum up, the ZMP can be calculated with (4.36), (4.37), (4.42) and (4.44) when the following quantities are known: m i : Mass of link i

18 4 ZERO-MOMENT POINT 14 I i : Body fixed inertia matrix for link i r ci : Position vector of center of mass of link i, with respect to frame O R i : Rotation matrix for link i, with respect to frame O ω i : Angular velocity of link i, with respect to frame O r ci : Acceleration of center of mass of link i, with respect to frame O ω i : Time derivative of angular velocity of link i, with respect to frame O Mass m i and inertia I i are known for each robot link, the other quantities need to be calculated with the forward kinematics relationships. 4.5 Summary In this chapter the Zero-Moment Point is defined and a method of calculating this ZMP from reaction forces is described. Next the computed ZMP method is explained and adapted to multibody systems. This computed ZMP for multibody systems uses the knowledge about the velocities and accelerations of all robot links to compute the change in linear and angular momenta. This in turn enables the computation of the ZMP. The next step is to calculate the link velocities and accelerations from known joint coordinates. This will be the topic of chapter 5.

19 5 ROBOT KINEMATICS 15 5 Robot Kinematics In the previous chapter it is shown that the ZMP can be computed via the change in linear (Ṗ ) and angular (Ḣ) momenta when the kinematics of the robot are known. The quantities that are needed for the ZMP computation are r ci, r ci, ω i and ω i. Also, the rotation matrix R i is needed. A suitable method for the calculation of these quantities from known robot joint coordinates is by using the Denavit-Hartenberg parameters [12, 3] to get homogeneous transformation matrices. These matrices can then be used to compute the dynamics of the robot. Homogeneous transformations are treated in detail in [12, 3], the Denavit-Hartenberg convention can also be found in these sources. In appendices A and B an overview of homogeneous transformations and Denavit-Hartenberg parameters respectively is given. 5.1 Forward Dynamics By the knowledge of the robot forward dynamics, the joint coordinates (d i or θ i ) and their timederivatives can be related to the link velocities (ṙ i, ω i ) and accelerations ( r i, ω i ) needed to compute the change in linear (4.42) and angular (4.44) momenta. The relations between link dynamics and joint coordinates for revolute and prismatic joints are treated in [12] and are also derived for both cases in appendix C. The results are summarized below using the frames and position vectors defined in Figure 5.1. z i 1 Joint i y i 1 z i x i 1 r i 1 i 1,i y i z r i 1 x i r i y x Figure 5.1: Linked frames and their position vectors. In general, independent of joint type, the location of the origin of frame i is given by r i : r i = r i 1 + R i 1r i 1 i 1,i (5.1) = r i 1 + r i 1,i (5.2) where R i 1 is the homogeneous transformation between reference frame and link frame i 1, computed with Denavit-Hartenberg parameters as in equation B.1. For a revolute joint the following relations are found: and for a prismatic joint: ω i = ω i 1 + θ i z i 1 (5.3) ω i = ω i 1 + ω i ω i 1,i + θ i z i 1 (5.4) r i = r i 1 + ω i r i 1,i + ω i (ω i r i 1,i ) (5.5) ω i = ω i 1 (5.6) ω i = ω i 1 (5.7) r i = r i 1 + ω i r i 1,i + ω i (ω i r i 1,i ) + 2ω i d i z i 1 + d i z i 1 (5.8)

20 5 ROBOT KINEMATICS Summary In chapter 4 the computations for the Zero-Moment Point are laid out. These computations demand the knowledge of all link positions, velocities and accelerations.the forward kinematics of a kinematic chain are determined with the use of the Denavit-Hartenberg convention and the knowledge of the joint coordinates. The forward kinematics are used to calculate the forward dynamics of a robot link. For each joint type, prismatic and revolute, equations are derived and used in the ZMP algorithm in chapter 6.

21 6 ZMP ALGORITHM 17 6 ZMP Algorithm The basics for the predictive algorithm to determine robot balance stated in section 2.2, are laid out in sections 4 and 5. The mathematical background in these chapters is used to design an algorithm that calculates the Zero-Moment Point from known joint coordinates. In this chapter the algorithm itself is presented. First the input for the algorithm is discussed, then the algorithm itself is presented and finally the output of the algorithm is treated. 6.1 Input From section 5.1 it is clear that the calculation of the link velocities and accelerations are dependent on a description of the robot. Next to these kinematic parameters, the mass, inertia and also the center of mass of each link need to be known. These quantities are the first set of inputs to the algorithm. link angle offset length twist type CoM mass inertia 1 θ 1 d 1 a 1 α 1 σ 1 r 1 1,c1 m 1 I 1 2 θ 2 d 2 a 2 α 2 σ 2 r 2 2,c2 m 2 I n θ n d n a n α n σ n r n n,cn m n I n Table 6.1: table with link properties In table 6.1 a kinematic description of a mobile robot is shown. For every link there are parameters for its geometric properties: angle, offset, length and twist, defined in the DH-convention [12] or [3]. The joint type σ denotes the type of joint, revolute or prismatic, and thus tells the algorithm what the actuated parameter is (θ or d). The center of mass r i i,ci is defined with respect to frame i, also the inertia is defined around the center of mass with respect to this frame as shown in Figure 6.1. link i-1 z i 1 joint i y i 1 x i 1 x z O y m i I i link i r i i,ci joint i+1 z i y i x i Figure 6.1: Link of a robotic arm (link i), reference frame O, link frame i and center of mass i.

22 6 ZMP ALGORITHM 18 The second part of the input to the algorithm are the joint coordinates. These are: θ i, θ i and θ i for a revolute joint and d i, d i and d i for a prismatic joint. 6.2 Calculations The calculations needed to compute the actual Zero-Moment Point are described below, see Figure 6.2 for details about the used position vectors. All calculations are performed for each subsequent link in the robot. link i-1 z i 1 joint i y i 1 x i 1 r i 1,i x z O o i 1 y r i 1,ci r ci o i m i I i link i r i i,ci joint i+1 z i y i x i Figure 6.2: Position vectors for link i. First, the homogeneous transformation matrix T i between the reference frame O and frame i is constructed: T i = T i 1A i (6.1) where A i is given by (B.1) and depends on the joint coordinate of joint i. T i 1 is the transformation between the reference frame and frame i 1, which is the body attached frame for the previous link. Next, the z i 1 -axis is determined, this axis is given by the first three elements in the third column of T i 1: z i 1 = T i 1(1 : 3, 3) (6.2) The vector r ci from the reference frame to the center of mass of link i is given by: [ ] rci = T 1 i [ ] r i i,ci 1 like in (A.3), where r i i,ci is given as input in table 6.1. The vector r i 1,ci from frame i 1 to the center of mass of link i is calculated with: (6.3) r i 1,ci = r ci o i 1 (6.4) = r ci T i 1(1 : 3, 4) (6.5)

23 6 ZMP ALGORITHM 19 In similar fashion, the vector r i 1,i from frame i 1 to frame i is calculated using: r i 1,i = o i o i 1 (6.6) = T i (1 : 3, 4) T i 1(1 : 3, 4) (6.7) When all these position vectors are calculated, it is possible to calculate the rotational and linear velocities and accelerations for the center of mass and end of each link. For the calculation of the change in linear and angular momenta (Ṗ and Ḣ), the velocity and acceleration of the center of mass are of interest, but from section 5.1 it is clear that the velocity and acceleration of the (end of) previous link are also needed. That is, the velocity and acceleration of frame i 1 with position vector r i 1 = o i 1 in Figure 6.2. For a revolute joint this results in (see also appendix C.2): ω i = ω i 1 + θ i z i 1 (6.8) ω i = ω i 1 + ω i θ i z i 1 + θ i z i 1 (6.9) r ci = r i 1 + ω i r i 1,ci + ω i (ω i r i 1,ci ) (6.1) r i = r i 1 + ω i r i 1,i + ω i (ω i r i 1,i ) (6.11) For a prismatic joint this gives (see also appendix C.3): ω i = ω i 1 (6.12) ω i = ω i 1 (6.13) r ci = r i 1 + ω i r i 1,ci + ω i (ω i r i 1,ci ) + 2ω i d i z i 1 + d i z i 1 (6.14) r i = r i 1 + ω i r i 1,i + ω i (ω i r i 1,i ) + 2ω i d i z i 1 + d i z i 1 (6.15) With these velocities and accelerations, the changes in linear and angular momenta can be calculated. The changes in the linear and angular momenta are summations over all links, see (4.42) and (4.44). This gives for link i: Ṗ i = Ṗ i 1 + m i r ci (6.16) Ḣ i = Ḣi 1 + r ci m i r ci + I i ω i + ω i (I i ω i ) (6.17) where I i is given by (4.4) as I i = R i I i R T i, and I i is an input from table 6.1. Now the position of the Zero-Moment Point can be calculated using (4.36) and (4.37): r P,x = Ḣy + r c,z mg x r c,x mg z P z mg z r P,y = Ḣx r c,y mg z + r c,z mg y P z mg z Here, m and r c are calculated using (4.24) and (4.25). 6.3 Summary The ZMP method described in chapter 4 together with the forward kinematics and dynamics described in chapter 5 are used to design an algorithm that computes the location of the ZMP. This algorithm needs a description of the robot joint using the DH-convention, mass and inertia parameters and joint coordinates to calculate link dynamics and the location of the ZMP. This algorithm is implemented in Matlab and used to simulate some robot movements, see chapter 8. The developed ZMP algorithm can be integrated in ROS, which is a requirement from the Fontys mechatronics lab.