Dynamic modeling of a human-inspired robot based on a Newton-Euler approach

Dynamic modeling of a human-inspired robot based on a Newton-Euler approach Fernando Gon¸calves1, Tiago Ribeiro2, A. Fernando Ribeiro2, Gil Lopes3, and Paulo Flores1 1Department of Mechanical Engineering, University of Minho, Guimar˜aes, Portugal [email protected] (F.G.) 2Department of Industrial Electronics, University of Minho, Guimar˜aes, Portugal 3Department of Communication Sciences and Information Technologies, University of Maia, Maia, Portugal Abstract. This work deals with the modeling process of a new three dimensional human-like robot for an inverse dynamic analysis. This robot intends to be utilized by caregivers to assist persons with reduced mobility (such as the elderly). The model under analysis is composed by 24 rigid bodies: 3 to represent the robot’s base and locomotion, 4 for the lower limbs and torso, 7 for each arm, and 3 for the head. The resulting multibody system has 19 degrees-of-freedom driven by 4 linear actuators and 15 revolute motors. The proposed approach was implemented using an in-house computational code, and validated against a commercial software for a general spatial motion. The outcomes achieved show that the proposed formulation is computationally effective both in terms of efficiency and accuracy. The general findings of this study are promising and useful for the mechanical design and construction of a real human-like robot prototype. Keywords: Human-inspired robot, Multibody dynamics, Newton-Euler formulation 1 Introduction Inverse dynamics refers to the process of determining the forces and torques applied in a body, or set of bodies, that result in a known motion (kinematics). There are three main formulations for studying a dynamics problem, all of them used in the multibody analysis of robots: Newton-Euler equations [1,2], Lagrange equations [3], and Hamilton equations [4,5]. Due to their simplicity and ease of implementation, recursive Newton-Euler algorithms are favored in the dynamic analysis of robotic systems [6]. Dynamic studies in computational environments often use one of two main methods: implementing the problem in an already developed multibody dynamics simulation software, or using computational tools to solve the equations of motion defined manually by the researcher. Simulation software are the most common resource for robotics research [7] due to: their simplicity, their convenient tools and interfaces, and their capability to tackle complex problems 2 Fernando Gon¸calves et al. without requiring more in-depth studies. Implementing the equations of motion on an integrated development environment is a more laborious process, but it provides its own set of advantages. The resulting models have higher compatibility with other software, they provide more flexible solutions (giving more freedom to researchers), and they can be more thoroughly optimized for specific problems. This study extends the authors’ previous work [8] to include a complete dynamic analysis of CHARMIE, a colaborative robot for elderly care [9]. The main novelty of this work is the use of an in-house code to adapt, apply and validate the recursive algorithm of [6] for obtaining the multibody model of the considered robot. This model will play a pivotal role in optimizing the mechanical design and developing the physical prototype of the robot. 2 Description of the multibody model The multibody model of CHARMIE is composed of 24 rigid bodies connected by 35 joints — 4 prismatic and 31 revolute — in a configuration that results in 19 degrees-of-freedom (Figure 1.b). The system is fully actuated, driven by 4 linear actuators and 15 revolute motors. Additionally, tension springs apply forces to reduce actuator loads: two identical springs are placed between bodies 3 and 4, and one between bodies 4 and 5, with 105.5; 77 mm free lenght (L0), 1380; 1029 N/m spring rate (k), and 27.1; 15.1 N initial force (Fi) respectively. Figure 1 labels the bodies of the multibody system, showing that 6a-12a correspond to the left arm, 6b-12b to the right arm, 6c-8c to the head, and 4 and 5 to the torso. Bodies 4sl and 4sr are auxiliary, used for the application of the tension spring between bodies 3 and 4. Bodies 1 to 3 correspond to the robot’s base and locomotion. At this stage, the behaviour of the omnidirectional wheels and suspension system was not considered. Bodies 1 and 2 (not represented in the CAD model) are massless fictitious links that allow the robot’s base to slide and rotate while traveling along the floor plane (using 2 linear actuators and 1 revolute motor). The work in [8] details the degrees-of-freedom associated with each main joint of the robot. From the CAD model, the masses and inertia of all the robot’s bodies were obtained, listed on Table 1. The robot’s inverse dynamics were analysed using two methods. The first was an in-house computational code that applied an altered version of the recursive Newton-Euler algorithm presented in [10]; the in-house code was programmed in Python, using the numpy and matplotlib libraries for mathematical operations and to obtain a visual representation of the robot. The second was a commercial simulation software. The resulting computational models are shown in Figure 2. In Figure 2.a the robot is in its starting position (zero position reference). In this configuration, all revolute actuators on the arms and head have a rotation of 0, and the robot’s base is aligned with the global reference and placed in its origin. In this setup, the two linear actuators have a total length of 480 mm (lower actuator) and 400 mm (upper actuator). Dynamic modeling of a human-inspired robot 3 Fig. 1. The CHARMIE robot: (a) CAD model with its bodies labeled, (b) kinematic diagram Table 1. Geometric and inertia properties of the multibody system. The origins are defined in relation to the local coordinates of the preceding link in the kinematic chain. Body Origin Mass Inertia [kg.mm2] [mm] [kg] Ixx Iyy Izz Ixy Ixz Iyz 1 [d1, 0, 0] 0 0 0 0 0 0 0 2 [0, d2, 0] 0 0 0 0 0 0 0 3 [0, 0, 0] 37.1 1187000 1304000 1610000 11450 3586 −51860 4sr = 4sl [±245, 0, 341] 0 0 0 0 0 0 0 4 [0, 0, 290 + d4] 7.5 118300 167400 94970 19.62 1.907 −7536 5 [0, 0, 434] 4.6 77230 80350 32960 2316 -229.9 4803 6a = 6b [±200, 3, 460] 0.3 32.39 32.39 19.93 0 0 0 7a = 7b [0, 0, 30] 0.3 102.5 102.5 19.61 0 0 0 8a = 8b [0, 0, 61.5] 0.4 710.8 710.8 26.54 0 0 0 9a = 9b [0, 0, 145] 0.4 710.8 710.8 26.54 0 0 0 10a = 10b [0, 0, 145] 0.4 663.9 663.9 26.56 0 0 0 11a = 11b [0, 0, 140] 0.4 663.9 663.9 26.56 0 0 0 12a = 12b [0, 0, 140] 0.3 498.6 498.6 19.94 0 0 0 6c [0, 0, 495] 0.3 145.8 132.1 246.5 0 0 4.732 7c [0, 0, 46.3] 0.4 296.0 268.2 522.0 2.606 −0.822 6.866 8c [14.8, −5.3, 18.3] 0.7 8548 7189 4981 31.79 143.1 −222.0 4 Fernando Gon¸calves et al. Fig. 2. Computational models of the CHARMIE robot’s dynamics: (a) in a commercial multibody dynamic analysis software (frame t = 0s), (b) in the in-house developed code (frame t = 1.5s) with bold arrows representing the main directions of motion. For comparison, the same generic motion was studied in both environments. All joints started with zero velocity. The motion lasted for 5 seconds, and was controlled via the accelerations of the 19 actuators. The accelerations were defined as: 0.032 m/s2for the base xand ylinear motions, 2π/9 rad/s2for the base rotation around the zaxis, 0.0032 m/s2for the linear actuators on the body, and 2π/9 rad/s2for the revolute motors on the arms and head. These accelerations were multiplied with a sign function that has a negative value for the first half of the simulation, and positive for the second half. 3 Equations of motion The formulation of multibody system dynamics adopted in this work follows closely that of [10], in which a recursive Newton-Euler algorithm is used to derive the spatial system’s equations of motion. These equations were implemented using the outputs from the forward kinematic analysis of [8] (orientations defined by rotation matrices, point coordinates, joint positions, velocities and accelerations). The algorithm is divided into two stages. The first stage is preparatory, and converts the data from the kinematic analysis into the relevant inputs for the dynamic calculations (body linear acceleration, angular velocity, and angular acceleration). Iterations progress from the global reference (body 0) to the robotic end-effector, modeling the behaviour of each body using the calculated properties of the previous kinematic link i−1, and the relative motion between link i and link i−1. The calculation of the linear accelerations includes the Coriolis, centrifugal, and Euler accelerations for computing the fictitious forces. Dynamic modeling of a human-inspired robot 5 The second stage uses the information from the first stage to determine the reaction forces and torques applied by each body i−1 on body i, progressing from the end-effectors to the robot’s base. To allow tackling more complex structures (not only serial assemblies of links) the original algorithm had to be altered. The modified formulation can directly tackle any body which has only one unknown reaction force and moment vector (applied by link i−1). If this condition is not met, a manual analysis is required (such is the case for bodies 4 and 5 of the CHARMIE robot). To simplify the computational implementation, forces and torques applied by link ion links i+1 are considered positive, while forces applied by links i−1 on link iare negative. In the following equations, the superscript next to each variable represents the reference frame they are expressed in. The sum of the naunknown fi ja forces applied by the ja links i−1 on link iis determined using Euler’s first law of motion, written as: na X ja=1 fi ja =mi¨ pi Ci + nb X jb=1 Ri jbfjb jb(1) where miis the mass of body iand ¨ pi Ci the linear acceleration of its center of mass. The nbforces applied by bodies fjb jb in body iare determined in the previous iterations i+ 1, so they are expressed in the local coordinates of their respective bodies. Before adding these forces, they are rotated to the orientation of body iusing the Ri jb rotation matrices. In order to determine the sum of the nareaction torques µi ja applied by the jalinks i−1 on link i, Euler’s second law of motion is used: na X ja=1 µi ja =¯ Ii i˙ ωi i+ωi i×(¯ Ii iωi i) + na X ja=1 −fi ja ×ri ja,Ci + nb X jb=1 Ri jbfjb jb ×ri jb,Ci +Ri jbµjb jb (2) The change in angular momentum of body iis determined from the inertia matrix ¯ Ii i, its angular velocity ωi i, and its angular acceleration ˙ ωi i. The sum of torques considers both directly applied torques, as well as the effect from forces not aligned with the body’s center of mass. This results in three known groups of torques: the sum of the nacross products between the fi ja forces and the respective ri ja,Ci vectors that define the displacement from the forces’ point of application to the body’s center of mass; the sum of the nbcross products between the fjb jb forces converted to the orientation of iusing the Ri jb rotation matrices, and the respective ri jb,Ci vectors that define the displacement from the forces’ point of application to the body’s center of mass; and the sum of the µi jb torques applied by body iin bodies jbwith their orientations converted to that of body iusing the Ri jb rotation matrices. 6 Fernando Gon¸calves et al. 4 Inverse dynamics for closed and overconstrained loops The equations of motion automatically deduce the applied forces and torques if a body has only one unknown reaction force and moment vector at its base when traversing the chain from the tip to the base. However, if this condition is not met (in closed or overconstrained loops), only the sum of unknown forces is calculated, and a more detailed study must determine their distribution. This occurs for bodies 4 and 5 of CHARMIE. For simplification, these reactions were analysed based on the mechanism formed by these bodies and the tolerances of their joints. Since this section tackles specific sections of the robot, not generalizable systems, a different nomenclature is used to facilitate reading the equations. The forces and torques are expressed using their x,yand zcomponents, and the orientations between bodies is defined using a single angle (there is only rotation around a single axis between two consecutive bodies). Euclidean vectors associated with distances are highlighted by accenting the corresponding variable with a right arrow. Most equations used in this sections result from developing a single line of equations (1) or (2). The notation used in both sections is interchangeable using the following equations:   Fxjb Fyjb Fzjb  =fjb jbRi jb;  µxjb µyjb µzjb  =µjb jbRi jb (3) for forces and torques applied by link ion the links i+ 1, and:   Fxja Fyja Fzja  =fi ja;  µxja µyja µzja  =µi ja (4) for the forces and torques applied by links i−1 on link i. Body 5 of CHARMIE (Figure 3) has three unknown forces/torques applied by: the tension spring s5, the revolute joint j5, and the linear actuator act5. Their distribution was determined using the following seven steps: 1. The tangential Fts5 component of s5 can be determined directly from the spring initial force Fi, spring rate kand it’s extension (Ls5 −(L0)s5). This force’s yand zcomponents are determined from the spring orientation β5. The spring does not apply any torque directly. Fts5 = (Fi)s5 +ks5(Ls5 −(L0)s5) (5) Fys5 =Fts5sin(β5); Fzs5 =Fts5cos(β5) (6) 2. Both j5and act5are revolute joints around the xaxis, so they do not apply any torque on it. The force from act5must nullify the remaining µxtorque around j5; this force’s component on the yz plane must be tangential to the Dynamic modeling of a human-inspired robot 7 Fig. 3. Kinematic representation of body 5 of the CHARMIE robot (a) and corresponding free body diagram (b). In (b), known forces and torques are colored in red/orange, unknown forces are colored green, and displacement vectors in blue. The x′ 5y′ 5z′ 5axes represent the orientations used in all calculations of body 5 actuator’s orientation. The following three equations allow determining the yand zcomponents of Fact5. µxaux =¯ I5 5˙ ω5 5[1,0,0]T+ω5 5×(¯ I5 5ω5 5)[1,0,0]T −Fys5(−→ e5z− −−→ f5z) + Fzs5(−→ e5y− −−→ f5y) + Fy6a(−→ a5z− −−→ f5z) −Fz6a(−→ a5y− −−→ f5y) + µx6a +Fy6b(−→ b5z− −−→ f5z)−Fz6b(−→ b5y− −−→ f5y) +µx6b +Fy6c(−→ c5z− −−→ f5z)−Fz6c(−→ c5y− −−→ f5y) + µx6c (7) Ftact5 =µxaux sin(α5)(−→ d5z− −−→ f5z)−cos(α5)(−→ d5y− −−→ f5y)(8) Fyact5 =Ftact5sin(α5); Fzact5 =Ftact5cos(α5) (9) 3. The yand zcomponents of j5can now be determined with the sum of forces. Fyj5 =m5¨py5 C5+Fy6a +Fy6b +Fy6c −Fyact5 −Fys5 (10) Fzj5 =m5¨pz5 C5+Fz6a +Fz6b +Fz6c −Fzact5 −Fzs5 (11) 8 Fernando Gon¸calves et al. 4. Since j5and act5are similar in terms of construction and tolerances, it was assumed that the remaining xreaction force is divided equally among them. Fxj5 =Fxact5 =m5¨px5 C5+Fx6a +Fx6b +Fx6c 2(12) 5. All unknown forces are determined. 6. The sum of unknown µxmust be 0 at this stage. Due to the tolerance similarities between j5and act5, it was estimated that each will apply half of the µyand µztorques obtained using equation (2). 2µyj5 = 2µyact5 =¯ I5 5˙ ω5 5[0,1,0]T+ω5 5×(¯ I5 5ω5 5)[0,1,0]T −Fzs5 −→ e5x+Fxs5 −→ e5z−Fzact5 −→ d5x+Fxact5 −→ d5z −Fzj5 −−→ f5x+Fxj5 −−→ f5z+Fz6a −→ a5x−Fx6a −→ a5z+µy6a +Fz6b −→ b5x−Fx6b −→ b5z+µy6b +Fz6c −→ c5x−Fx6c −→ c5z+µy6c (13) 2µzj5 = 2µzact5 =¯ I5 5˙ ω5 5[0,0,1]T+ω5 5×(¯ I5 5ω5 5)[0,0,1]T −Fxs5 −→ e5y+Fys5 −→ e5x−Fxact5 −→ d5y+Fyact5 −→ d5x −Fxj5 −−→ f5y+Fyj5 −−→ f5x+Fx6a −→ a5y−Fy6a −→ a5x+µz6a +Fx6b −→ b5y−Fy6b −→ b5x+µz6b +Fx6c −→ c5y−Fy6c −→ c5x+µz6c (14) 7. The distribution of unknown torques is fully determined. Body 4 of CHARMIE (Figure 4) has four unknown forces/torques: the ones originating from the tension springs srt4and slt4, the prismatic joint j4along the zaxis, and the linear actuator act4. Their distribution was determined using the following nine steps: 1. The spring forces srt4and slt4required an analysis of the auxiliary bodies 4sr and 4sl (Figure 5). The spring force (sr4or sl4) is determined directly from the spring position and orientation. Fts4 = (Fi)s4 +ks4(Ls4 −(L0)s4) (15) Fxs4 =Fts4sin(γ4); Fzs4 =Fts4cos(γ4) (16) 2. Forces srt4and slt4are along the zaxis, and can be calculated by using them to nullify the torque µyreaction on joint jr4or jl4. Fzsrt4 =Fzslt4 =Fzsr4 −−→ h4x−Fxsr4 −→ h4z −→ j4x (17) Dynamic modeling of a human-inspired robot 9 Fig. 4. Kinematic representation of body 4 of the CHARMIE robot (a) and corresponding free body diagram (b). In (b), known forces and torques are colored in red/orange, unknown forces are colored green, and displacement vectors in blue. The x′ 4y′ 4z′ 4axes represent the orientations used in all calculations of body 4. Fig. 5. Free body diagram of body 4sr with unknown forces and torques colored in green, and displacement vectors in blue. 3. For modeling body 3, the joint reactions (jr4or jl4) are determined with the sum of forces within these auxiliary bodies. Fxjr4 =Fxs4;Fzjr4 =Fzs4 +Fzsrt4 (18) 4. Since act4is the only unknown zreaction on body 4, and its orientation in the yz plan is known, its yand zcomponents can be determined. Fzact4 =m4¨pz4 C4+Fzact5 +Fzj5 +Fzs5 −Fzsrt4 −Fzslt4 (19) Fyact4 =Fzact4tan(α4) (20)