Application of Position Sensors for an Adaptive Algorithm for the Movement of a Biped Walking Robot Lebedev Philipp Sergeevich Chudakova Milena Alekseevna Orlov Alexander Vladimirovich Ural Federal University Ural Federal University Ural Federal University Ekaterinburg, Russia Ekaterinburg, Russia Ekaterinburg, Russia Lebedev.phil@gmail.com mila.97@mail.ru A.V.Orlov@mail.ru Abstract This paper is devoted to the algorithm of biped robot walking process which based on transfer of center of mass and algorithm adaptation to changing dynamic characteristics by making corrections from position sensors (accelerometer and gyroscope) with measuring a zero moment point in the walking process. Keywords: biped robot, center of mass, zero moment point, adaptive algorithm, position sensor, task oriented coordinates, task oriented algorithm. 1. Introduction The most important question for to building two-legged robots is the search for balance in the process of movement. Developers should consider the static and dynamic characteristics of the robot. There are several ways to solve the problem of ensuring equilibrium 1. Movement based on the dynamic model of the robot. It takes into account the kinematics, the angles of rotation and the displacement of the center of mass, but don’t taking into account external influences. 2. Determining of zero moment point with position sensors. Method is allows to assess external influences but significantly increase the requirements for speed control system and drive motors. 3. A method that simultaneously takes into account both of the above, the steps of the robot are carried out according to a previously developed motion model, adjusted by the acceleration sensor (accelerometer) and angular velocity (gyroscope). This article describes a method to search a stable position based on the third way and the adaptation of a mathematical model of the robot according using information about external moments in the walking process. The walking robot is considered as a dynamic system with unknown parameters. External influences are measured at each step and the model of the walking process is adjusted on the basis of them. 2. Dynamic of the biped locomotion In the present biped walking process is consisting of two phases. In the first phase (two-support), the center of mass is transferred to the carrier leg, and the stability criterion must be met: the projection of the center of mass must not extend over the support leg area along two axes of coordinates. In the second phase (singl OY OX Phase 1 Phase 2 Phase 1 Phase 2 Figure 1: Two phases of the walking process For this process of movement, a mathematical model has been developed, based on the movement of the center of mass of the robot along two axes, taking into account the following principles: 1. The mass and dimensions of each part of the robot are known and do not change during movement; 2. The surface on which the robot moves is horizontal and has no obstacles; 3. There are not the external impacts on the robot in the process of movement. The main requirements for the walking algorithm: 1. The horizontal projection of center of mass must pass through the surface of the support. It needs for to keep of the stable position. 2. The cycle of a single step should consist of two phases – transfer the center of mass from the back foot to the front (two-support) and moving the swing foot forward to the step (single-support). 3. To avoid jerks in both phases of motion, horizontal velocity of the massive torso must be constant. 4. It is not allowed to straighten the flight foot before separation and when touching the surface in order to avoid the ambiguity of working out the trajectories. 5. The torso should not have a vertical movement. So it weakens the stability of the gait and increases energy consumption. q4 q3 y3 y4 q2 q5 y5 y2 y1 q1 y6 q6 Figure 2: Kinematic scheme of biped robot 2 തതതത࢒; qi is the Using task-oriented coordinates contains: deviations:‫ ܍‬ൌ ሼ‫ ܑ܍‬ሽ, i = 1, k and paths ‫ ܛ‬ൌ ሼ‫ ࢏ܛ‬ሽǡ ࢏ ൌ ૚ǡ angles of robot joints rotation. The total number of conditions for each phase of the movement should be equal to the number of links of the biped (the number of degrees of freedom). k + l = n, in this case n = 3 + 3 = 6. Two-support phase: ଵ ݁ଵ ൌ ‫ݕ‬ଶଶ െ ܿଵ ‫ۓ‬ ଵ ۖ ݁ଶ ൌ ‫ݕ‬ଷଶ െ ܿଵ ۖ ଵ ‫ݏ‬ଵ ൌ ‫ݕ‬ሶ ଶଵ ൌ ܸ௞ ଵ ଶ (2.1) ‫݁۔‬ଷ ൌ ‫ݍ‬ሶ ଵ ൅ ‫ݍ‬ሶଶ െ ܳଵ ۖ ଶ ۖ ݁ଷ ൌ ‫ݕ‬ଷଵ െ ܿଶ ଶ ‫݁ ە‬ସ ൌ ‫ݕ‬ଷଶ െ ܿଶ Single-support phase: ଵ ݁ଵ ൌ ‫ݕ‬ଶଶ െ ܿଵ ‫ۓ‬ ଵ ۖ ݁ଶ ൌ ‫ݕ‬ଷଶ െ ܿଵ ۖ ଵ ‫ݏ‬ଵ ൌ ‫ݕ‬ሶ ଶଵ ൌ ܸ௞ ଶ ଶ (2.2) ‫ ଺݁ ۔‬ൌ ݇ ή ‫ݕ‬ሶ ଶଵ ൅ ‫ݕ‬ሶ ଶଶ െ ܾ ۖ ଵ ۖ ‫ݏ‬ଵ ൌ ‫ݕ‬ሶ ଷଵ ൌ ͵ܸ௞ ଶ ଵ ‫ ଻݁ە‬ൌ ‫ݕ‬ଷଶ െ ‫•‘ ܣ‬ሺ߱ ή ‫ݕ‬ଷଵ ሻ Here, c1 is the vertical coordinate of the foot; Vk is the horizontal velocity of the torso; с2 is the horizontal coordinate of the front foot edge; с3 is the vertical coordinate of the front foot edge; Q is the total angle of the positions of biped; А is the height of the front foot edge lifting; ࣓ is the frequency of a single step; k, b are the angle of inclination and the height of the downward trajectory. The basic model in its general form is a kinematic control object of n links that is described by the following system of vector-matrix equations: ‫ݍ‬ሶ ൌ ‫ݑܤ‬ ൝ ߙ ൌ ܴ‫ݍ‬ (2.3) ‫ ݕ‬ൌ ݄ሺߙሻ Here, control vector is u [n×1]; q [n×1] is the vector of drives rotation angles; α [n×1] is the vector of absolute angles of links rotation; y [n×2] is the vector of coordinates of the end points of the links; B [n×n] is the matrix of the system inputs; R [n×n] is the matrix of transition from relative angles to absolute; h [n×2] is the matrix of recalculation of angular coordinates into linear. The coordinate conversion matrix h is defined by the expression: h(α) = y′ + T T ήz (2.4) The detailed form of the recurrent procedure for calculating the elements of the matrix h is written like this: ‫்ݕ‬ ݄ଵ ‫்ݕ‬ ‫ ் ݖ‬ή ܶ ் ή ሺߙଵ ሻ ‫ ۍ‬ଵ் ‫ې‬ ‫ ۍ‬଴் ‫ ۍ ې‬ଵ் ் ‫ې‬ ‫ݕێ‬ଶ ‫ ۑ‬ൌ ൦ ݄ଶ ൪ ൌ ‫ݕ ێ‬଴ ‫ ۑ‬൅ ‫ݖ ێ‬ଶ ή ܶ ή ሺߙଶ ሻ ‫ۑ‬ (2.5) ‫ێ‬ǥ‫ۑ‬ ǥ ‫ ێ‬ǥ ‫ێ ۑ‬ ǥ ‫ۑ‬ ‫ݕۏ‬௡் ‫ے‬ ݄௡ ் ‫ݕۏ‬௡ିଵ ‫ݖۏ ے‬௡் ή ܶ ் ή ሺߙ௡ ሻ‫ے‬ 3 ‘•ሺߙ௜ ሻ െ•‹ሺߙ௜ ሻ Here, T(ߙ௜ ሻ ൌ ൤ ൨ – rotation matrix. •‹ሺߙ௜ ሻ ‘•ሺߙ௜ ሻ Expressions (2.1) for variables of the i-th link written as: ‫ݍ‬పሶ  ൌ ‫ܤ‬௜ ‫ݑ‬௜ ቐ ߙ௜ ൌ σ௜௝ୀଵ ‫ݍ‬௜ (2.6) ‫ݕ‬௜் ൌ ‫ݕ‬௜ିଵ ் ൅  ‫ݖ‬௜் ή ܶ ் ሺߙ௜ ሻ The control task is to minimize the vector of deviations e (task a). Stabilize the vector of the path traveled s per time - holding the vector of required velocities V * = s ̇ (task b). The synthesis procedure involves obtaining a motion model in Cartesian coordinates, converting to task-oriented coordinates, and then synthesizing controls that solve tasks (a) and (b). After differentiation of the direct kinematics equation (2.3) was obtained: ‫ݕ‬పሶ ் ൌ ‫ݕ‬పିଵ ሶ ் ൅ ߙሶప ‫ݖ‬௜் ܶሺߙ௜ ሻ‫ ܧ‬ൌ ‫ݕ‬పିଵ ሶ ் ൅ ‫ݎ‬௜் ‫ݍ‬ሶ ή ‫ݖ‬௜் ܶሺߙ௜ ሻ‫ ܧ‬ൌ ‫ݕ‬పିଵ ሶ ் ൅ ‫ݎ‬௜் ‫ ݑܤ‬ή ‫ݖ‬௜் ܶሺߙ௜ ሻ‫( ܧ‬2.7) ‫ݕ‬పሶ ் ൌ ‫ݕ‬పିଵ ሶ ் ൅ ‫ݖ‬௜் ‫ ݑܤ‬ή ‫ݎ‬௜் ܶሺߙ௜ ሻ‫ܧ‬ (2.8) Ͳ ͳ Here, Е = ቂ ቃ െͳ Ͳ After simplification obtained: డ௬ డఈ ‫ݕ‬ሶ ൌ ‫ܩ‬௬ ሺߙሻ ή ‫ݑܤ‬, ‫ܩ‬௬ ሺߙሻ ൌ ή ή ‫ݍ‬ሶ (2.9) డఈ డ௤ Task oriented bipedal robot model: ݁ሶ డʣ డʣ ൤ ൨ ൌ ቀ ή ‫ܩ‬௬ ሺߙሻ ή ܴ ൅ ቁ ή ‫ݑܤ‬ (2.10) ‫ݏ‬ሶ డ௬ డ௤ 3. Adaptation of the movement model. Movement mode from previous part is valid only for ideal conditions. The parameters of the robot, such as body mass, hand position, external moments can change, and the model needs to be adjusted for external forces. To solve this problem is used method of determining of the zero moment point. External moments will reject the position of the robot. And changing of position can be measured by sensors. Based on the measurement results, robot controller is change of the drives of robot joints and generates adjusts for the movement model. It allows adapting the movement algorithm to the surrounding environment. The external influence is a random variable, for its assessment was restricted to the following conditions: 1. External impacts are relatively small and do not exceed 15% of the main impact according to the movement algorithm; 2. External influences are constant within the time interval spent by the robot one step. MEMS gyro and accelerometer MPU6050 is using for measurements. Information from sensors after processing the Kalman filter is used to correct the current position of the robot for x deviation, and also taking into account the scale factor m, are introduced into the dynamic equations of the motion process. 4 The mathematical model of the process of movement with adaptations will take the form: ‫ݕ‬పሶ ் ൌ ‫ݕ‬పିଵ ሶ ் ൅ ݇పሶ ‫ݖ‬௜் ܶሺ݇௜ ሻ‫ ܧ‬ൌ ‫ݕ‬పିଵ ሶ ் ൅ ‫ݎ‬௜் ‫ݍ‬ሶ ή ‫ݖ‬௜் ܶሺ݇௜ ሻ‫ ܧ‬ൌ ‫ݕ‬పିଵ ሶ ் ൅ ‫ݎ‬௜் ‫ ݑܤ‬ή ‫ݖ‬௜் ܶሺ݇௜ ሻ‫( ܧ‬3.1) ‫ݕ‬పሶ ் ൌ ‫ݕ‬పିଵ ሶ ் ൅ ‫ݖ‬௜் ‫ ݑܤ‬ή ‫ݎ‬௜் ܶሺ݇௜ ሻ‫ܧ‬ (3.2) Here k = ࢻ ൅ ࢞ ή ࢓ is the required angle with corrections. After simplification obtained the task oriented bipedal robot model with adaptation to the external influences: ݁ሶ డʣ డʣ ൤ ൨ ൌ ቀ ή ‫ܩ‬௬ ሺ݇ሻ ή ܴ ൅ ቁ ή ‫ݑܤ‬ (3.3) ‫ݏ‬ሶ డ௬ డ௤ 4. Conclusion The calculation of the robot joints drives rotation angles is made according to the developed mathematical model, which is based on the movement of the center of mass of the robot. At the same time, at each step, the value of external influences on the robot is estimated along two axes. It’s used to calculate corrections of the robot joints drives rotation angles, and also corrects the motion model. The continuous introduction of adjustments to the motion model allows one to get closer to the actual conditions in which the robot moves. It can increase of the accuracy of the next step of the robot. This algorithm will be use as part of the control program of the STM32F4 microcontroller which used as a controller for a bipedal walking robot developed by the Institute of Radio Electronics and Information Technology. References 1. Analysis, Control and Design of Walking Robots. Gijs van Oort. ISBN 978-90-365-3264-8. DOI 10.3990/1.9789036532648. Copyright c 2011 by G. van Oort, Enschede, The Netherlands. 2. Eric R. Westervelt, Jessy W. Grizzle, Christine Chevallereau and oth. Feedback Control of Dynamic Bipedal Robot Lovomotion. – CRC Press. – London– New York, 2009. 3. Formal'skij A.M. Moving anthropomorphic mechanisms. – M.: Nauka, 1982. – 368 p. 4. Vukobratovic M. Walking robots and anthropomorphic mechanisms. – M.: Mir, 1976. – 539 p. 5. Vukobratovic M., Borovac B. Zero-moment point-thirty five years of its life // International Journal of Humanoid Robotics. – 2004. – № 1. – РР. 157–173. 5