Biped Robot Report

Modelling and Control of a Biped Robot
Model based Gait Trajectory Generation using a Genetic Algorithm
Department of Control Engineering 8th Semester

Aalborg University 2007
AALBORG
UNIVERSITY
8th Semester Section of Automation and Control Department of Electronic Systems Faculty of Engineering, Science and Medicine Fredrik Bajers Vej 7 C3 DK-9220 Aalborg st Denmark Telephone +45 96 35 87 02 Fax +45 98 15 17 39 http://www.control.aau.dk/
Synopsis: Title:
Modelling and Control of a Biped Robot Modelling and Control
Aalborg University is currently working on a full-scale humanoid biped robot. To gain insight into modelling and controlling a biped robot, this project explores the possibility of obtaining static and dynamic walk of a small-scale biped robot. The project continues a previous project regarding the same robot, where static walk was obtained. The hardware on the robot is changed considerably. The foot and sensor design is changed enabling more accurate center of pressure measurements. Furthermore, batteries and a microcontroller is mounted on the robot to enable autonomy. Models of the robot are derived using kinematics, inverse kinematics, Newton-Euler iterations and Lagrangian mechanics. These models are used o-line for generating gait trajectories for the robot to follow. The gait trajectory generation uses a genetic algorithm to minimize a performance function. By tuning parameters in the performance function, it is possible to control i.e. the range of motion of the limbs, the torque used by the actuators and the speed of the robot. The Lagrangian model is simplied, and is thus less computationally intensive than the Newton-Euler model. Hence, it could be used on-line for a modelbased controller, but due to time constraints, this has not been achieved. Instead, a classic control approach is used consisting of a mid-range controller to stabilize the robot during gait using the center of pressure sensor feedback. The acceptance test shows that the trajectories generated using the static and dynamic models enables the robot to walk. Furthermore the principle of the controller was veried but further tuning is needed in order to achieve substantial improvements to the stability.
Theme: Period:
P8, Spring Semester 2007
Project group:
07gr830
Group members:
Rolf Christensen Nikolaj Fogh Rico Hjerm Hansen Heine Hansen Asger Malte Iversen Mads Schmidt Jensen Louis Schultz Lantow
Supervisor: Copies: 9 Pages: 276
Jan Helbo
Attachments: Enclosed CD-ROM Finished: 31st of May 2007
AALBORG
UNIVERSITY
8. Semester Sektion for Automation og kontrol Institut for Elektroniske Systemer Det Ingenir-, Natur-, og Sundhedsvidenskabelige Fakultet Fredrik Bajers Vej 7 C3 DK-9220 Aalborg st Danmark Telefon +45 96 35 87 02 Fax +45 98 15 17 39 http://www.control.aau.dk/
Synopsis: Titel:
Modelling and Control of a Biped Robot Modellering og kontrol
Denne rapport omhandler modellering og kontrol af en tobenet robot. Projektet er en videreudvikling af et foregende projekt p samme robot, og resultaterne skal ses i et strre perspektiv i forbindelse med Aalborg Universitets udvikling af en tobenet robot i menneske strrelse. Isenkrammet som var tilgngelig p robotten ved projekt start er blevet markant ndret. En microcontroller og batterier er blevet implementeret hvilket muliggr fuldstndig autonomitet. Der er ogs konstrueret nye fdder til robotten for at f en mere prcis mling af robottens trykcenter. I dette projekt er robotten beskrevet matematisk ved at udvikle en kinematisk, invers kinematisk, NewtonEuler samt en Lagrange model. Disse modeller er begrnset til at beskrive robotten nr den sttter p en fod. Modellerne er blevet vericeret og har en prcision der gr dem brugbare til trajektoriedannelse. Trajektoriedannelsen er baseret p en genetisk algoritme som minimerer en performance funktion. Ved at justere parametre i denne performance funktion er det muligt at bestemme f.eks. skridt lngde, gang hastighed og moment leveret af servomotorerne. Dette trajektorie er fremadkoblingsdelen af kontrolleren. En simpliceret Lagrange model skulle gre det muligt at implementere en modelbaseret on-line regulator men grundet tidspres er dette ikke implementeret. I stedet for er der benyttet klassisk regulering ved at implementere en mid-range tilbagekoblingsregulator, hvor denne regulator bruger trykcentret som input. Accepttesten viste at robotten var i stand til at flge de udviklede trajektorier. Princippet i tilbagekoblingsregulatoren virker efter hensigten, men yderligere tuning er ndvendig for at opn betydelig forbedring af stabiliteten.
Tema:
Periode:
P8, Forrssemester 2007
Projektgruppe:
07gr830
Gruppemedlemmer:
Rolf Christensen Nikolaj Fogh Rico Hjerm Hansen Heine Hansen Asger Malte Iversen Mads Schmidt Jensen Louis Schultz Lantow
Vejleder:
Jan Helbo
Kopier: 9 Sider: 276 Bilag: Vedlagt CD-ROM Frdiggjort: 31. Maj 2007
Preface
This report is written by group 07gr830 and documents a project concerning the modelling and control of a biped robot. The project originates in the project proposal, Modelling, Simulation and Control of a Biped Robot proposed by associate professor Jan Helbo at Aalborg University. The report and project is made at the Section of Automation and Control under the Department of Electronic Systems at Aalborg University, and is composed during the period from the 1st of February to the 31st of May 2007 under the theme Modelling and Control. For a complete understanding of the report a technical and scientic level corresponding to that of 8th semester students at the Section of Automation and Control is required. Literature references are written according to the Harvard-method: [Last name of author, year of publication]. If a literary work has more than two authors the rst author's last name is written followed by et al.: [Last name of author et al., year of publication]. A complete bibliography is found at page 153. References to appendices are enumerated as: App. A, B, etc., while attachments are referred to with a number [1], [2], [3], etc. A list of the attachments can be found in the section Appended Documents. The enclosed CD-ROM contains the attachments of this report, including video clips of the biped robot. Throughout the project, Matlab has been used for data processing, large symbolic computations, and for presenting the results. Simulink has been used for implementing and simulating the developed models, and for building the interface to the biped robot. The version of Matlab used is 7.3.0.247 (R2006b) and the Simulink version is 6.5 (R2006b). To help visualizing and verifying the developed models of the biped robot, and to aid the design of control for the robot, the 3D CAD programs, Rhinoceros 4.0 and Autodesk 3ds Max 9.0, have been utilized. The ServoPodTM microcontroller mounted on the biped robot is programmed using the language IsoMax, version 0.82. The report consists of 20 chapters and 10 appendices, which are structured into the six following parts: Part I is an introduction to the project. An overview is given of the context in which the project should be viewed and contains a detailed description of the scope of this project. It introduces the reader to some basic terminology and denitions regarding the area of biped robots. Also, the specication of requirements is set up and the limitations are specied. Part II of the report will focus on a system description and the hardware conguration of the biped robot. This part will describe the changes made to the hardware and the reason why these changes have been made. Part III concerns the mathematical modelling of the biped robot. This includes modelling the servomotors, as well as making static and dynamic models for the biped robot. Part IV of the report contains the development of a controller for the biped robot.
After an introductory analysis of controllers an inverse kinematic model of the robot is developed and gait trajectories are generated. To improve the stability of the robot, a feedback controller is also designed. Part V is the closing part where the result from the acceptance test is discussed and the conclusion of the project is made. Future improvements are also presented. Part VI contains the appendices of the report, which contains further documentation of the project. This includes a human gait analysis, documentation of the conducted tests on the robot and hardware, information about the hardware installed on the biped robot, documentation of the developed software; and lastly, the detailed calculations done in the modelling part. For easy reference, a nomenclature is appended as the second last appendix. This briey describes the meaning of the used abbreviations and symbols, and contains the values of the dierent constant. The general typographic convention of the report is also described in this appendix. The last appendix (last page) is a fold out page, which contains essential illustrations of the robot, including the dierent coordinate systems used in the report. The page is useful to have folded out for easy reference when reading the report. Each part starts with a brief intro, describing the contents of the part, and each part and every major chapter is concluded with a brief summary. The report is copyrighted by the members of the group and Aalborg University. It may be used freely provided that a distinct reference is made.
Aalborg University, 31st of May, 2007
Rolf Christensen
Nikolaj Fogh
Rico Hjerm Hansen
Heine Hansen
Asger Malte Iversen
Mads Schmidt Jensen
Louis Schultz Lantow
Contents
I Introduction
1 The Context and Purpose of the Project
1.1 1.2 1.3 1.4 State of the Art in Biped Robotics . . . . . The AAUBOT Project . . . . . . . . . . . . Studies to support the AAUBOT-1 Project The Purpose of This Project . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
1
2
2 3 4 4
2 Biped Terminology
2.1 2.2
Human Anatomy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Biped Mechanics . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
5 7
3 The Scope of the Project

3.1 3.2 3.3
Review and Summary of the Work of Group 06gr833 . . . . . . . . . Continuing the Work of Group 06gr833 . . . . . . . . . . . . . . . . . Requirement Specication . . . . . . . . . . . . . . . . . . . . . . . .
11
11 13 16
II
System Description and Conguration

4 System Description 5 Hardware
5.1 5.2 5.3 Changes to Existing Sensors and Actuators . . . . . . . . . . . . . . Power Supply to the Biped Robot . . . . . . . . . . . . . . . . . . . . Controlling the Biped Robot . . . . . . . . . . . . . . . . . . . . . . .
19
20 22
23 26 28
Part Summary
31
III
Modelling
33
34 36
. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
6 Modelling Strategy 7 Servomotor Model 8 Kinematic Model

8.1 8.2 8.3 8.4 8.5 Brief Description of Methods to Describe Kinematics Deriving the Kinematics of the Robot . . . . . . . . Verication of Kinematic Model . . . . . . . . . . . . The CoM of the Robot Using the Kinematic Model . Summary of the Kinematic Model . . . . . . . . . .
38
38 42 47 47 49
9 Newton-Euler Based Dynamic Model

9.1 9.2 9.3
The Modelling Approach . . . . . . . . . . . . . . . . . . . . . . . . . Deriving the Newtonian Model . . . . . . . . . . . . . . . . . . . . . Verication of the Newton-Euler Model . . . . . . . . . . . . . . . .
51
51 52 59
10 Lagrangian Mechanics based Dynamic Model

10.1 10.2 10.3 10.4 10.5 Lagrangian Mechanics . . . . . . . . . . Simplication of the Kinematics . . . . . Energy Considerations . . . . . . . . . . The Lagrangian and the Ankle Torques Verication of the Ankle Torques . . . . . . . . . . . . . . . . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
. . . . .
66
67 67 71 72 80
Part Summary
86
IV Control
11 Introductory Analysis 12 Inverse Kinematics
12.1 Calculating the Inverse Kinematic Model . . . . . . . . . . . . . . . . 12.2 Verication of the Inverse Kinematic Model . . . . . . . . . . . . . . 13.1 13.2 13.3 13.4 13.5 13.6 Trajectory Criterias . . . . . . . . . Parameters Dening a Trajectory Set Interpolation . . . . . . . . . . . . . Minimizing Performance Function . . Tuning . . . . . . . . . . . . . . . . . Final Trajectories . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
87
88 92
93 97
13 Trajectories
100
101 105 106 107 110 113
14 Force Measuring to CoP Position
14.1 The CoP Position based on Pressure Measurements . . . . . . . . . . 117 14.2 The CoP Position in DSP Phase . . . . . . . . . . . . . . . . . . . . 118 14.3 Verication of the CoP Mapping . . . . . . . . . . . . . . . . . . . . 120 15.1 Dening the Error Signal for the Controller . . . . . . . . . . . . . . 123 15.2 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 126 15.3 Testing the Designed Controllers . . . . . . . . . . . . . . . . . . . . 131
117
15 ZMP Controller
122
16 Trajectory Supervisor Part Summary
134 137
V System Test and Summary

17 Acceptance Test 18 Conclusion 19 Discussion 20 Closing Statement Bibliography Appended Documents
139
140 146 148 151 153 155
VI Appendix
A Human Gait B Test Scenario Description C CoP Measurement Test D Hardware and Robot Interfacing
D.1 Microcomputer Pin Assignment . . . . . . . . . D.2 IsoMax . . . . . . . . . . . . . . . . . . . . . . . D.3 Programming the ServoPodTM . . . . . . . . . D.4 Interfacing With An External Computer . . . . D.5 Servomotor Calibration . . . . . . . . . . . . . D.6 Servomotor Performance Under Various Loads . D.7 Robot Autonomy Hardware . . . . . . . . . . . D.8 Accelerometer Test . . . . . . . . . . . . . . . . D.9 Strain Gauge Measurements . . . . . . . . . . . D.10 Print Layout . . . . . . . . . . . . . . . . . . . D.11 Simulink Interface . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
157
158 164 169 174
174 176 178 179 183 187 189 192 196 197 198
E Servomotor model F The Newton-Euler Iterations G Lagrange

G.1 G.2 G.3 G.4 Lagrange Model Simplications . . . . . . . . . Determination of Kinetic- and Potential Energy Moments of Inertia . . . . . . . . . . . . . . . . Computation of the Lagrangian, RL1 and RL2 . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
201 206 216

216 232 239 251
H Design of New Feet I Nomenclature

I.1 I.2 I.3 I.4 Abbreviations and Acronyms . . . . . . . Typographic Representation of Quantities Descriptions of Variables . . . . . . . . . . Constants . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . .
262 264
264 265 266 268
J Fold Out Page
276
Part I
Introduction
This part denes the scope of the project and explains important concepts and terminology surrounding biped robots. The rst chapter describes the context of the report in order to specify the area, in which the purpose of this project lies. In the second chapter, the basic terminology regarding human anatomy and biped mechanics are described as these terms will be used extensively to describe the biped robot throughout the report. The terms are also required to describe the scope of the project. Finally, the scope of the project is given in the third chapter, where the work already done on the biped robot is described. This leads to a discussion on how this work is to be continued. Finally, a number of accept test criteria are specied to test the robot and to evaluate the project.
Chapter
The Context and Purpose of the Project

Research in the area of robotics started in the 20th century and has resulted in robots of various types, many being a permanent part of today's industry. These robots perform the jobs that are physically demanding, monotonous or even hazardous to human beings. Also, they increase the work rate and the quality of products in places where factors such as speed and accuracy are essential. Today, robots are found in almost all modern assembly production lines. Usually, these robots use a small set of xed movement patterns which limits their possible interactions with the surroundings. Also, they are seldom in contact with humans. Research concerning robots capable of functioning in normal human surroundings and interacting with the human environment is still at an early stage. A part of this research area includes the development of robots using bipedal locomotion, i.e. walking on two legs, as these robots could function and perform tasks in a human environment without adjusting the surroundings to them. Robots using bipedal locomotion are called biped robots, and are often found within the class of robots known as anthropomorphic robots. Anthropomorphic robots are humanoid robots, where the appearance of the robot resemble that of a human. This is in contrast to the industrial robot, which is simply dened as an automatically controlled, reprogrammable, multipurpose manipulator programmable in three or more axes [Inernational Organization for Standardization, 1994].
1.1 State of the Art in Biped Robotics

The rst full scale humanoid robot was made in 1973 by the Waseda University of Tokyo [Humanoid Robotics Institute, 2007]. One of the rst companies to join the area of anthropomorphic robot research was Honda with the release of their rst experimental model, E0, in 1986. Honda continued developing robots based on this rst model and is one of the leading companies in the area of humanoid robots today with the release of their full scale humanoid robot ASIMO in 2000 [Honda, 2007]. The latest ASIMO model has a height of 130 cm and weighs 54 kg, having a top walking speed of 2.7 km/h and a top running speed of 6.0 km/h. The E0 and ASIMO robots are seen in Fig. 1.1. 2
1.2. THE AAUBOT PROJECT
Figure 1.1: The rst model E0 and ASIMO from Honda.

In [Katic and Vukobratovic, 2003] it is stated that, in spite of the intensive development and experimental verication of various humanoid robots, it is important to further improve their capabilities by using advanced hardware and control software solutions to make humanoid robots more autonomous, intelligent and adaptable to the environment and humans. As a result, Aalborg University (AAU) has joined the research eld of biped robots.
1.2 The AAUBOT Project

In the autumn of 2006 the AAUBOT robot group was founded at AAU with the task of starting the development of human sized biped robots. This lead to the start of the student project of constructing and controlling AAU's rst human sized biped robot, AAUBOT-1 [www.aau-bot.aau.dk]. The mechanical design of the AAUBOT-1 is done as the master thesis of a 3 member group from the Institute of Mechanical Engineering at AAU. The group began their master thesis in September 2006 and have now completed the blueprints for the 180 cm tall AAUBOT-1 having 17 degrees of freedom. Currently, the AAUBOT-1 is at the stage of construction, and is scheduled to take its rst steps in 2008. A sketch of the AAUBOT-1 can be seen in Fig. 1.2 [www.aau-bot.dk].
Figure 1.2: The nal design of the AAUBOT-1 [www.aau-bot.dk].
CHAPTER 1. THE CONTEXT AND PURPOSE OF THE PROJECT
1.3 Studies to support the AAUBOT-1 Project

Hands-on experience and knowledge regarding control and gait design for biped robots would be of great value to the development and implementation of the control part of the AAUBOT-1. As a result, preliminary studies on small scale biped robots have been conducted at AAU and new projects are currently running. After a preliminary research in the area of biped robots done by a student group in autumn 2005, AAU invested in a 29 cm tall small-scale biped robot. This was used as the subject for an 8th semester project carried out by a 6 members group, 06gr833, at the section of Automation and Control, AAU. The project treated subjects such as modelling, simulation and control, which lead to a biped robot which was able to walk slowly with a control algorithm to suppress disturbances whilst walking. In this report, this robot is referred to as the pre-AAUBOT. See Fig. 1.3.
Figure 1.3: The 29 cm tall small-scale biped robot used as the subject for an 8th
semester project in 2006. The robot is in this report named pre-AAUBOT.
Four members of group 06gr833 began their master thesis in autumn 2006, dealing with the construction, modelling and control of another small-scale robot based on their experiences from the former semester. This new and enhanced robot is called Roberto. The project will be completed in the summer of 2007.
1.4 The Purpose of This Project

To obtain more knowledge about biped control strategies, this 8th semester project was established, yielding the results given in this report. The project was also started with the intentions of educating new students, us, in the terminology of biped robots to have students ready to start development of a control strategy for the AAUBOT-1 in the autumn of 2007. This project continues the work on the pre-AAUBOT started by group 06gr833. Available for this project is the 29 cm high small scale biped robot bought in spring 2006, along with all the developed hardware and documentation. Before going into a more detailed description of the scope of this project, some basic terminology and denitions regarding the area of biped robots need to be dened.
Chapter
Biped Terminology
This chapter introduces the basic terminology used in the area of biped robotics. To use intuitive terms, the terminology is based on human anatomy which is connected to the description of biped robots. Afterwards the denitions of other key terms related to biped robots are given.
2.1 Human Anatomy

This section contains a description of the planes in which human motion is described, a model of the human joints and an analysis of human gait. This is done with reference to [Wirhed, 2002].
2.1.1 Planes of Motion

Description of human body motion is done with respect to the anatomical position depicted in Fig. 2.1. The anatomical position is the standing position with the face turned straight forward and the arms hanging along the sides of the body with the palms turned straight forward and the legs stretched with the feet close together. Motions can be described from three perpendicular planes through the body. These planes are depicted in Fig. 2.1, where a coordinate system is placed with the x-axis pointing forward, the y -axis pointing to the left-hand side, and the z -axis pointing upwards. Using this coordinate system, the planes can be dened as:
Frontal plane:
The plane parallel to the yz -plane is called the frontal plane. The plane parallel to the xz -plane and through the Center of Mass (CoM) is called the median plane. All planes parallel to the median plane are called sagittal planes. The plane parallel to the xy -plane is called the horizontal plane.
Median plane:
Horizontal plane:
6
z
CHAPTER 2. BIPED TERMINOLOGY
Median plane
Frontal plane
Horizontal plane CoM
Figure 2.1: The three planes of motion for a human being. The depicted person is
standing in anatomical position.
2.1.2 Joints
Dierent types of joints are located inside the human body where two bones are connected. These joints can be modelled by mechanical joints. Only three joints are needed to model the motion of human joints. These are illustrated in Fig. 2.2 and have the following Degrees of Freedoms (DoF):
Hinge joints: These joints have one DoF and allow motion in one plane. The knee
joint is a example of a hinge joint. joint.
Pivot joints: The pivot joints also have one DoF. The forearm is a example of a pivot Ball-and-socket joints: The ball-and-socket joints have three DoF. These joints allow
rotation in three dimensions. The hip is a example of a ball-and-socket joint.
Figure 2.2: A sketch of the three joints necessary to describe the human joints.
From the left: the hinge joints, the pivot joint, and the ball-and-socket joint.
2.1.3 Walk Analysis

Describing human gait requires some specic terms, which are dened in this section. The gait analysis is inspired from [Pickel, 2003] and [rts et al., 2006].
Double Support: This term is used for situations where a body has two isolated
contact surfaces with the ground. In human gait this situation occurs when the person is supported by both feet.
2.2. BIPED MECHANICS
Single Support: This term is used for situations where a body only has one contact
surface with the ground. In human gait this situation occurs when a person is supporting on only one foot. Both double and single support situations are also valid for biped robots.
Trajectory: A trajectory is the path of a moving point in the three dimensional space.
In gait analysis, trajectories are used to describe the movement of a joint frame, relative to the global frame, which is usually attached to the ground.
Gait Phases: The gait phases of normal dynamic walk consists of eight steps but
only four of them are dierent as the right and left leg executes the exact same motion mirrored in the median plane delayed by half a gait cycle. 1. Release Phase: The innitesimal period of time when the toes of the rear foot breaks contact with the ground. 2. Single Support Phase (SSP): The phase where only one foot has contact with the ground and the other foot swings. Also the SSP is divided into two phases one where the right foot is in contact with the ground (SSP-R) and one where the left foot is in contact with the ground (SSP-L). 3. Impact Phase: The innitesimal period of time when the heel of the swinging foot strikes the ground. 4. Double Support Phase (DSP): The phase where both feet have contact with the ground. The DSP is divided in two phases, one where the weight is shifted from the left to the right foot (DSP-L) and one where the weight is shifted from the right to the left foot (DSP-R). Thus, human walk can be described as a heel-strike-toe-o gait cycle. The four phases are depicted in Fig. 2.3. The average walking speed for a human is about 5 km/h with a average step length of approximately 0.72 m these values are however heavily dependent on the height, weight, age and sex of the human.
2.2 Biped Mechanics

The anatomical terms and concepts which have just been presented can be used to describe biped robots. Still, some specic terms regarding biped mechanics have not yet been explained. These terms are described in the following.
Zero position: The zero position of the biped robot can be seen in Fig. 2.4. It diers
a little from the anatomical position as the palms are facing into the sides of the torso.
Joint: A joint is a mechanical construction where two connected parts are able to
rotate relative to each other. There exist one mechanical joints which can model most of the human joints, this is described in the following:
1 DSP-L Release 2 Impact SSP-R
Left leg
Right leg
SSP-L
Impact 3 Release DSP-R 4
Figure 2.3: The gait of a normal dynamic walk.
frontal view of the robot
sagittal view of the robot
Figure 2.4: The zero position of the biped robot.
Revolute joint:
A revolute joint is a joint, where two connected parts rotate around a common axis. One revolute joint can be used to construct both a hinge joint and a pivot joint. The revolute joint has one DoF. A ball-and-socket joint and can be constructed from three revolute joints thereby giving it three DoF.
Link: A link is a mechanical construction between two joints. When the biped robot
is in zero position the axis of rotation of each joint is parallel to the axes of the body frame. This fact can be used to describe the joint rotations in the following way, where the coordinate system refers to Fig. 2.1:
Roll:
A rotation around any axis parallel to the x-axis, when the biped is in zero position, i.e. a rotation in the frontal plane. A rotation around any axis parallel to the y -axis, when the biped is in zero position,
Pitch:
2.2. BIPED MECHANICS

i.e. a rotation in the sagittal plane.
Yaw:
A rotation around any axis parallel to the z -axis, when the biped is in zero position, i.e. a rotation in the horizontal plane.
These descriptions of rotation in the zero position allows all joint motions to be described uniquely, no matter what position the biped robot is in.
Polygon of Support (PoS) The PoS is the contact surface between the feet and
the ground. In the DSP the surface is the convex area spanned by the contact surfaces, called the convex hull, which can be found by drawing straight lines from the outer rims of each contact surface. In the SSP phase the PoS is equal to the contact surface of the supporting foot. This is illustrated in Fig. 2.5.
right foot
right foot left foot
(a)
(b)
Figure 2.5: Figure (a) illustrates the PoS in the SSP, and gure (b) the PoS in the
DSP.
Center of Pressure (CoP): The CoP is the point on the supporting surface of the
body where the total sum of the contact forces acts, causing a force but no moment. When standing, the part of the body exerted by contact forces is the feet, where the components of the contact forces acting normal to the sole of the supporting foot or feet are called pressure forces [Sardin and Bessonnet, 2004]. An example of the CoP location in SSP phase is seen in Fig. 2.6a.
body body
Fresulting presure force=Fp,1+Fp,2 Fp,1 Fbody,1 Fp,2

dlever arm 1 CoP dlever arm 2 sole tCoP=dlever arm 1 Fp,1 - dlever arm 2 Fp,2 = 0
(a) ZMP
Fbody,2 Fresulting force on the body

sole (b)
Figure 2.6: In (a) the resulting force is equal to the sum of the pressure forces F p,1 and F p,2 acting at the CoP. The CoP is the point on the sole of the foot, where the net torque caused by pressure forces is equal to zero. In (b) the ZMP is the point on the sole, where the forces acting on the body F body,1 and F body,2 results in zero net torque.
10

Where the CoP is related to contact forces, the ZMP is related to the forces acting on the robot caused by gravity and its actuators.
Zero Moment Point (ZMP): The ZMP is the point on the surface where the sum
of all tipping moments due to gravitational and actuators generated forces are equal to zero, where tipping moments are dened as the moments acting tangential to the contact surface. If the supporting surface is horizontal, tipping moment are the moments causing pitch and roll movement, but no yaw movement [Sardin and Bessonnet, 2004]. This is illustrated in Fig. 2.6b.
Coincidence of the CoP and ZMP Location: As long as the contact forces appear
in a single planar surface the ZMP and CoP coincides [Sardin and Bessonnet, 2004]. This result is due to the fact that the vertical component of the resulting force caused by gravitation and actuators acts at the ZMP. Thus, the ZMP must be the same point as the CoP for the supporting foot to remain in equilibrium. Otherwise, the resulting pressure force and the resulting vertical component of the gravitational and actuator forces would apply moment to the foot, i.e. it would rotate, which is not the case.
Static walk: Static walk assumes that a biped robot is always statically stable. This
means that if the motion is stopped the biped robot will stay in a stable position. The static walk speed is low meaning that the dynamics of the system can be ignored. When this is done, the projection of the CoM on the horizontal plane is equal to the CoP and in order to maintain static stability the CoM and CoP should lie within the PoS.
Dynamic walk: Dynamic walk is faster compared to static walk. In dynamic walk
the CoM is allowed to be outside the PoS for a limited time which means that the biped robot will be statically unstable in the swinging phase just before the impact phase. According to [Popovic and Sinkjaer, 2000], for a gait cycle to be called dynamic, it requires the use of the heel-strike-toe-o technique described in Sec. 2.1. Human gait is dynamic.
Pseudo-Dynamic walk: Due to the mechanical construction of the pre-AAUBOT biped robot, i.e. large solid at feet and anatomically incorrect ankle joints, it cannot use the heel-strike-toe-o gait technique, and thus it cannot realize real dynamic walk.
As a result, a pseudo-dynamic walk is dened in this project. The pseudo-dynamic takes into account the dynamics of the robot as the CoM is allowed to be outside the PoS for a limited time, making the biped robot statically unstable in the swinging phase just before the impact phase. No requirements are made concerning the heel-strike-toe-o technique. There is not an absolute criterion that determines whether the pseudo-dynamic walk is stable or not. However, if the biped robot has active ankle joints and always at least one foot on the ground the ZMP can be used as a stability criterion [Cuevas et al., 2005]. As long as the ZMP is inside the PoS, the pseudo-dynamic walk is considered dynamically stable as the foot can control the posture of the biped robot. If the ZMP is outside the PoS there is no resulting contact force acting on its foot, thus leaving the foot with no control of the posture of the robot. As opposed to static walk, a biped robot using pseudo-dynamic walk will not be static when all motions are stopped but rotate around the ZMP. Note that, henceforth, when using the term dynamic walk in the context of the biped robot later in the report, it refers to pseudo-dynamic walk.
Chapter
The Scope of the Project

As mentioned in Ch. 1, this project continues the work, [rts et al., 2006], done on the pre-AAUBOT by group 06gr833 in the spring 2006. The purpose of this chapter is to describe how the work is continued in this project. The scope is explained by rst giving a brief review of [rts et al., 2006], and afterwards dividing the tasks that will be performed in this project into parts.
3.1 Review and Summary of the Work of Group 06gr833

From group 06gr833, the following hardware and documentation have been obtained:
The report, Modelling and Control of a Biped Robot, referred to as [rts et al., 2006]. The pre-AAUBOT, see Fig. 1.3. A working hardware interface to the biped robot.
The group chose and bought the pre-AAUBOT robot from the company, Lynxmotion, and did the project based on the problem: How is it possible to make a two legged robot walk? By reviewing their work it has been chosen to summarize it by dividing it into ve parts and depict the parts in a owchart as seen in Fig. 3.1. A description of the parts is given below:
1: Description and Conguration of Robot: Hardware: The group choose the robot, along with the actuators (servomotors) to
control the joints, and assembled it. Five accelerometers were mounted on the robot, along with four pressure sensors on each foot. To control the servomotors, servo drivers were mounted on the biped. No computer was mounted on the robot, so control signals were sent to the drivers from an external PC using a serial cable. Likewise, analogue data signals from sensors were sent to the PC through a cable, where the signals were sampled and converted. 11
12
CHAPTER 3. THE SCOPE OF THE PROJECT
Description and Configuration of Robot
Modelling
Trajectory Generation
Controller Design and Implementation Fuzzy logic based small disturbance CoM controller
Acceptance Test
Kinematic model Inverse kinematic model Human gait experiment Newtonian model SolidWorks model of the robot Hardware Assembling the robot Sensors Servo control Interfacing with the Robot Implementation of trajectories Servo motor model Lagrangian model Verification of models Static gait design
Stable stand with controller

Static walk with controller Static walk
Heavy disturbance controller Measurements for feedback: - CoP - Acceleration of torso Implementation of controller
Static walk with controller
Stable stand with controller
Figure 3.1: A owchart to summarize the work done in [rts et al., 2006].
SolidWorks Model: A complete 3D model of the biped robot (without wires, servo
drivers and sensors) was implemented in the 3D CAD program, SolidWorks. Using this model, the dimensions, center of masses and inertia tensors of the robot were calculated. These were used in the models of the robot. The SolidWorks model was based on SolidWorks models of the individual parts of the robot, which were available from the Lynxmotion web site. ment was conducted, where the gait of a human, one of the group members, was lmed from eight dierent angles. The recordings were converted into trajectories of the toes, ankles, knees, hips, shoulders, elbows and wrists.
Human gait experiment: To obtain data concerning dynamic walking, an experi-
2: Modelling Kinematic Model: A SSP complete kinematic model of the robot was created for
use in trajectory generation. It is based on the dimensions from the SolidWorks model.
Inverse Kinematic Model: A SSP inverse kinematic model of the robot was created
for use in trajectory design. in the project.
Servo Motor Model: A servomotor model was developed, but was not used or veried Newtonian Model: A SSP newtonian model of the robot's mechanics was created. It
was only partly veried and were not used further in the project.
Lagranian Model: A reduced SSP Lagranian model of the mechanics of the robot,
including only the legs was created and suggestions to a DSP Lagranian model was given. The reduced SSP model was only partly veried and were not used further in the project.
3.2. CONTINUING THE WORK OF GROUP 06GR833
13
3: Trajectory Generation Static gait design: A desired CoP trajectory was found, and at ve stances in a step,
the leg joint angles were manually adjusted to roughly t a gait phase yielding the desired CoP position. The arms were used afterwards to ne tune the CoP position. Given these ve postures in the gait cycle, trajectories for the robot through the entire gait cycle were found by interpolation. Finally, the trajectories were translated to joint angles using inverse kinematics.
4: Controller Design and Implementation Fuzzy logic controller: A fuzzy logic controller was implemented to suppress small
disturbances, i.e. tuning the CoP position during walk and stand. The fuzzy logic controller decided how the dierent limbs should react in dierent stages of the walk. The feedback was based on CoP measurements. i.e. to avoid falling, e.g. when the robot is given a push. The controller works by playing a dierent trajectory when it register a push that would otherwise result in the robot falling over, for example taking a step backwards. The feedback was based on acceleration measurements in the torso.
Heavy disturbance controller: A heavy disturbance controller was implemented,
5: Acceptance Test Stable stand with small disturbance controller: The robot was seemingly able to
stand and stretch the arms in the right direction when pushed to keep balance.
Stable stand with heavy disturbance controller: The robot was sometimes able
to avoid falling by taking a step, when pushed hard. The problem was a delay before the step was taken. correct place. But the robot had a problem with lifting the non-supporting leg, so at least one corner of the non-supporting foot always toughed the ground. stretch the arms in the right direction to keep balance when walking.
Static walk: The robot was able to walk statically and maintaining its CoM in the
Static walk with small disturbance controller: The robot was seemingly able to Static walk with heavy disturbance controller: The robot was sometimes able to
avoid falling when pushed hard while walking. The problem was found in a delay from the push to the controller actually reacted.
3.2 Continuing the Work of Group 06gr833

In this section, the scope and contents of this project is given in the perspective of continuing the work of group 06gr833. The main objective of this project is given and at the end it is described which parts of [rts et al., 2006] is used in this project.
14
3.2.1 The Main Objective

Overall, one of the most important objectives seen in respect to [rts et al., 2006] is to actually use the dynamic Newton-Euler and Lagrangian models in the trajectory generation and control. This is done by looking into design and implementation of pseudo-dynamic walk of the biped robot. Instead of using the Ad-Hoc principle of creating trajectories as used in [rts et al., 2006], it has been chosen to automate the trajectory generation by implementing a genetic trajectory generation algorithm. Thus, the main theme of this project is:
Design and implementation of both static and pseudo-dynamic walk on a biped robot using a genetic algorithm for trajectory generation based on a kinematic, an inverse kinematic and a dynamic model.
As a result, the objective in this project is to obtain the necessary models and create the required hardware to implement and test the genetic algorithm. A simplied owchart describing the contents of the project is given in Fig. 3.2. The boxes in bold are results taken directly from [rts et al., 2006]. Boxes in dashed bold lines are subjects inspired by [rts et al., 2006], but modied or redone. Boxes in thin line are subjects done from the beginning and boxes in dashed line are subjects not covered due to time constraints.
Description and configuration of robot Modelling Genetic trajectory generation Controller design and implenetation CoM controller design Acceptance test Static walk test with controller Dynamic walk test with controller Static walk test Simulation model Model based controller Measurements for feedback, i.e. ZMP Implementation of controller Dynamic walk test
Kinematic model SolidWorks model Mechanical description Human gait experiment Human gait analysis Hardware Sensors Servomotor control Hardware modifications Interfacing with the robot Implementation of trajectories Inverse kinematic model Newtonian model Servomotor model Simplified Lagrangian model Verification of Models Static walk design Dynamic walk design
Figure 3.2: A owchart describing the ow of this project, giving the connecting
thread. The boxes in bold are results taken directly from [rts et al., 2006]. Boxes in dashed bold lines are subjects inspired by [rts et al., 2006], but modied or redone. Boxes in thin line are subjects done from the beginning and boxes in dashed line are subjects not covered due to time constraints.
As seen in Fig. 3.2 only two things are used directly as described in [rts et al., 2006]. These are the SolidWorks models and the data from human gait experiment. The data
3.2. CONTINUING THE WORK OF GROUP 06GR833

from the human gait is, however, treated dierently to nd the joint angles during a gait cycle, instead of showing the positions of the joints. The hardware is also improved due to several problems stated in [rts et al., 2006]. Also, in [rts et al., 2006] the robot is dependent on an external computer and power supply. As a result, it have been chosen to implement an on-board computer and power supply to make it autonomous, i.e. independent of external hardware. Furthermore, the pressure sensor are redone to obtain a better CoP measurement. The reason for not directly using any of the models developed in [rts et al., 2006] is because they have not been veried or because errors has been found during review. These errors will be commented on when the respective subjects are treated in the report. For now, a brief list of the errors are given, in order to justify the time used in this project to develop the models again.
15
Errors in the Kinematic Model:
Errors in the kinematics of the Lagrangian model have been found. To make sure these does not originate from the kinematic model, this is re-developed. An error has been 1 discovered in the transformation matrix RA RA2T which seems to originate in an incorrect calculation of cos( 90 ) and sin( 90 ).
Errors in the Newtonian Model:
The derivation of the Newtonian model is carried out correctly. The model was however not implemented correctly in Simulink . The transformation from the supporting leg to the free limbs is done using incorrect rotation matrices for the arms. Also, wrong center of mass position constants were used for the supporting leg.
Errors in the Lagrangian Model:
The problem in the development of this model originates from describing the kinematics of the legs incorrectly. Joint angles are added directly, even though the joint axis are not always parallel, resulting in a substantial errors.
Errors in the Inverse Kinematic Model:
The errors in this model originate from a faulty approach. Consequently, most equations in this model yields wrong angles. The problem is that the calculations are carried out concerning two dimensions where it should have been carried out in three dimensions. This leads to an increasing error as the posture of the robot moves away from the zero position. Further, a dierent approach is taken concerning the Lagrangian model, which is based on simplifying the kinematics of the biped using the human gait analysis. Concerning trajectory design, a genetic trajectory generation algorithm is designed and implemented as already mentioned. The algorithm uses the kinematic model, the Newtonian model, Lagrangian model and the inverse kinematic model. Lastly, a controller is designed. It was intented, that the Lagrangian model should be used to create a model-based controller, but this was not realized due to time constraints. Instead, a controller is designed based on using the arms and legs in a mid-range controller conguration. The controller is based on moving the CoM of the robot. The controller uses the CoP measurement as the feedback signal. It was intented to test the designed controller on an simulation model of the biped robot, but due to in-adequate servo motor models, this could not be realized.
16
3.3 Requirement Specication

In order to determine whether the main objectives for this report have been achieved a set of requirements is specied. As no metrics have been found regarding the performance of the achieved gait in [rts et al., 2006], it is dicult to measure any improvements. The criteria listed in this report are made from considerations on the abilities of the servomotors, the construction of the biped robot, and its physical size. The criteria are made measurable to the extent possible and thus enable a more clear acceptance test in the end of the report. It also enables a list of further improvements on the biped robot to be held up against the results achieved in this report. The test set procedures and reason for the specications according to speed and step length can be found in App. B. The test cases are divided into four scenarios, which are summarized below. The tests to verify these criteria are in such an order that the more simple tests are veried rst. All the tests are conducted on a horizontal surface as the models made have this as a prerequisite. The tests are documented with graphs in Ch. 17 and video recordings which are on the enclosed CD [1].
Test Scenario One

The tests in the rst test scenario are made to validate if the robot can stand and to validate if the robot follows the trajectories, with feedback control disabled and no external disturbances. 1. Stand stable with no external disturbance and with the controller disabled. 2. Walk statically with no external disturbance and with the controller disabled.
Minimum speed of 0.2 km/h 5.6 cm/s. Minimum step length of 11.6 cm. The swing foot must leave the ground completely. The projection of the CoM onto the supporting plane must be within the PoS during the whole gait cycle.
3. Walk pseudo-dynamically with no external disturbance and with the controller disabled.
Minimum speed of 0.8 km/h 22.4 cm/s. Minimum step length of 11.6 cm. The swing foot must leave the ground completely. The ZMP must be within the PoS during the whole gait cycle.
Test Scenario Two

The tests in the second test scenario are made to validate if the developed feedback controller stabilizes the robot without external disturbance. 4. Walk statically with no external disturbance and with the controller enabled.
The CoP must be within 0.3 cm of the CoP specied for the trajectory.
3.3. REQUIREMENT SPECIFICATION

5. Walk pseudo-dynamically with no external disturbance and with the controller enabled.
17
Test Scenario Three

The tests in this test scenario are made to validate if the implemented feedback controller is able to suppress external disturbances applied to the robot. 6. Stand stable with external disturbance and with the controller enabled.
The robot must have a reasonable response when a step is given in the ZMP reference. The projection of the CoM onto the supporting plane must be within PoS for disturbances up to 4 N.
7. Walk statically with external disturbance and with the controller enabled.
8. Walk pseudo-dynamically with external disturbance and with the controller enabled.
Test Scenario Four

The test in the last test scenario is made for demonstration purpose. 9. Walk autonomously without any wires attached.
3.3.1 Delimitation
The models only concerns the SSP, as a model of the DSP is comprehensive and too time consuming to develop. It is assumed that the DSP can be approximated by the SSP as the DSP only lasts for approximately 8% of the walking cycle cf. App. A. Therefore the transitional phase from one single support phase to another single support phase is disregarded in the models as it seems reasonable that these can still describe the dynamic gait of the biped robot with such a relative short transition phase. The validity of this assumption was veried in the Newton-Euler model described in Ch. 9. Therefore, the DSP is not considered as a modelling problem. This entails that no controllers based on a model for the double support phase are designed. In [rts et al., 2006] a large disturbance controller were designed. The large disturbance controller should make the robot choose an alternative trajectory during gait, if it was given a large push, i.e. taking an extra step to avoid tumbling. The main scope of this project is to improve the gait of the robot. As a result, only a small disturbance controller is explored, thus implementation controllers that make the robot able to withstanding pushes is not the main scope of the project. Further this project is limited to modelling and control of the robot on a planar surface with the gravitational acceleration as a normal.
Part II
System Description and Conguration
In this part a description of the biped robot is presented. The rst chapter contain a description of the robot. This include an introduction to the size, construction, materials and limitations of the robot. This should give an overview of the physical capability of the biped robot. The second chapter introduces a list of suggested improvements proposed by the previous group which worked on the robot. This list mainly contains problems related to the physical design of the robot. This part will describe how this project will solve these problems by improving the robot with new servomotors, a better construction and new sensors. The new hardware mounted on the biped robot in order to achieve better performance and add new possibilities that are available is also described in this part.
Chapter
System Description
The biped robot used in this project is seen in Fig. 4.1. The legs and arms are an assembly set of aluminum servomotor brackets. The body is made from Lexan. It measures approximately 29 cm in height and has a mass of 2240 g. The actuators consist of 20 servomotors resulting in 20 DoF. This chapter will list the most important data describing the biped robot. A complete list is found in App. I. The construction of the biped robot should resemble the human body in order to achieve anthropomorphism and be able to perform human gait. But the design of the biped robot dier in several ways from the human body. First of all the feet of the robot is much larger than human feet compared to the size of the robot. This makes the robot very stable but decreases its ability of perform a human like dynamic gait with heelstrike-toe-o. All of the joints are comprised of servomotors that are revolute joints with one DoF, as standard RC servomotors are used. This means that in order to describe e.g. a hip which has three DoF, three joints are needed. The design of the robot results in legs which do not have the ability to yaw as either the ankle nor either hip is capable of yawing. This means that the robot is only capable of walking straight forward. Moreover the design of the leg diers from that of a human in the fact that the ankle has the same length as the shin. As the servomotors have a 180 range, only one limb on the robot have insucient freedom of movement to resemble a human joint. As the shoulder on a human being is able to move more than 180 this link is limited by the construction of the robot. All other links one the robot have a larger range of motion than that of a human.
20
21
Height: 29.0 cm
Height legs: 20.5 cm
Width feet: 14.0 cm
Height: 29.0 cm
Length feet: 12.6 cm
Figure 4.1: Figure of the biped robot used in this project. An illustration showing
the joints of the robot is superimposed on the picture in the frontal view. This illustration is used throughout the report to represent the robot.
Chapter
Hardware
The assembly of the biped robot and the initial sensor conguration is documented in [rts et al., 2006]. The initial sensor conguration consisted of ve accelerometers - one in each wrist, one in each foot and one in the torso. Additionally, four force sensing resistors were equipped on each foot. The actuators consisted of RC servomotors. The robot was controlled by an external computer interfacing with Simulink via the xPC toolbox and a National Instruments PCI-6024E data acquisition board. Preliminary tests performed on the biped robot and [rts et al., 2006] identies several problems with the hardware. The table below shows which of these have been solved. Improved X X X X X X X Problem 1. The xPC target experiences CPU overloads. 2. The ankle servomotors are too weak. 3. The servomotors have no angular position feedback. 4. The refresh-rate of the servomotors are too slow. 5. The robot begins wobbling after prolonged use due to poor connection between servomotors and brackets. 6. The feet of the robot slides over the surface because of the low friction. 7. The pressure sensors are too inaccurate to properly determine the CoP and the characteristics experience much drift. 8. The feet are too big to easily perform dynamic walk. X X 9. A gyro could be added to measure angular acceleration of the torso. 10. The interface board could be made smaller by using a PCB.
This chapter will focus on the new hardware installed on the robot and the changes done to existing hardware. In the rst part of this chapter, a discussion is given about possible hardware changes to solve some of the problems stated in [rts et al., 2006]. Afterwards, a description of the new hardware installed and the reasons why is given.
22
5.1. CHANGES TO EXISTING SENSORS AND ACTUATORS
23
5.1 Changes to Existing Sensors and Actuators

This section discusses which improvements are made on the robot and what the results of these improvements are. It was determined that the robot needed improved foot force sensors to determine the CoP as this is essential in a controller situation. This automatically lead to a redesign of the foot in order to t the new sensors. Furthermore it was decided that feedback from the servomotors would be of great value as this could be use in numerous situations. It was also decided to change certain parts on the robot to minimize slack. Finally, the sampling board was replaced with a microcontroller, and batteries were mounted on the robot in order to make it free of wires so it could walk around freely. A switch was added to make it possible to use an external power supply.
5.1.1 xPC Target Experiences CPU Overloads

In this project the controller implementation is utilized with the same approach as in [rts et al., 2006] meaning that the controller runs on a Simulink model from a computer running xPC. As mentioned in [rts et al., 2006] a problem occurred with CPU overload using the xPC, but as it has not been possible to determine what caused CPU overloads no measures has been taken to avoid this problem. As no CPU overloads has been experienced, this is not seen as a problem in this project.
5.1.2 Weak Ankle Servomotors

The actuators of the robot consist of 20 Hitec RC servomotors. Initially, 16 of the servomotors, positioned in the arms, torso and legs, were of the type HS-645MG and 4, positioned in the hands, were of the weaker type HS-475HB. In [rts et al., 2006] it was discovered that the ankle servomotors were not strong enough to hold their positions during a gait cycle. This resulted in large angle osets and a gait which swayed from side to side when walking.
Solution
In [rts et al., 2006] it is suggested that an outer control loop could be implemented to add integral control to the servomotors. However, adding a PI controller to the loop will make the servo slower. This can be overcome by using a PID controller. All of these solutions will require a tuning of the controller which will take time. As the fundamental problem is due to the lack of torque given by the servomotors, the problem is instead solved mechanically by replacing the weaker servomotors with a stronger type. The old servomotors has a stall torque of 9.57 kg cm equal to around 0.9 Nm and in [rts et al., 2006] it has been determined that the ankle servomotors experienced as much as 2 Nm. A new stronger servomotor is available from HiTec. The HS-5955TG has a stall torque of 24 kg cm which is equal to 2.35 Nm. Additionally these new servomotors are faster than the old ones as they rotate with 400 /s whereas the old ones rotate with 333 /s. They also have a smaller dead band at 0.45 where the old ones have a dead band at 0.9 . The dead band is a small angle in which the servomotor does not correct an angle error which means that a smaller dead band is equal to less slack in the whole system.
5.1.3 Joint Angular Position Sensors

Addition of feedback from the servomotors is discussed in [rts et al., 2006]. This feedback could be used both for verifying the model and to determine the position of the robots limbs in a control situation.
24
CHAPTER 5. HARDWARE
Solution
Several possibilities are available for measuring the angular position of the servomotors. Some of the possibilities are to attach rotary encoders, tachometers or potentiometers to the shaft of the servomotors. As it is of great importance to keep everything as small as possible, it would be less desirable to mount extra equipment on the robot. Another possibility is to use the potentiometer already mounted internally in the RC servomotors to measure the position of the servomotor. The feedback circuit in the servomotor consists of a potentiometer connected to the shaft which changes resistance according to the angle of the shaft. The circuit can be seen in Fig. 5.1. RC servomotors does not give angular position feedback out of the box but this can be implemented by adding a wire to measure the voltage over the potentiometer. The dashed line in Fig. 5.1 shows where an external ADC measures the position of the servomotor. The solution using the internal potentiometers is chosen, as it enables precise feedback from the servomotors using only an ADC.
To ADC 4.7k To external ADC Vcc 100
Gnd
Motor shaft Gnd
Figure 5.1: The servomotor ADC schematic. Vcc is regulated to 3.3 V
5.1.4 Refresh Rate of the Servomotors

As stated in [rts et al., 2006] the maximum refresh rate of the servos was 20 Hz due to the use of the serial communication with the servomotor controller boards. This slow refresh rate resulted in vibrations during walk.
Solution
A standard RC servomotor is driven by a PWM signal at 50 Hz with a varying duty cycle. However, it is tested that the update frequency can be set to 76 Hz or even higher. Hence, the ServopodTM is congured to drive the servomotors with a PWM signal of 76 Hz, which makes it possible to update the servomotor at 76 Hz. This should result in a smoother walk
5.1.5 Poor Connection between Servomotors and Brackets

The servomotors are attached to the robot with plastic rivets and the rotating part with a plastic horn which is screwed together with thread forming screw. This attachment has become increasingly wobbling with use.
Solution
Instead of plastic rivets the servomotors are bolted to the robot with metal bolts, a metal washer and a self-tightening metal nut. This has two advantages: First o all the servomotors are tightly fastened to the robot and secondly it should not become any looser with time. Furthermore the plastic horns have been replaced with a metal horns which is also tightened with a metal screw. This should all in all reduce the slack in the robot.
5.1. CHANGES TO EXISTING SENSORS AND ACTUATORS
25
5.1.6 Foot Design and Force Sensors

Initially, four force sensing resistors (FSR) where mounted underneath each foot. Their purpose was to measure the weight distribution of the robot. Several problems arose with the initial sensor type and interface selection. First o all, this sensor type is not very accurate. Furthermore, every time the robot took a step the two plates, which constituted the sensor, was pushed sideways with respect to each other. This meant that the sensor experienced a static change in characteristic. Yet another problem was that the signal from the force sensor is lead to a board several meters from the sensor next to other analogue and digital signals. This might have let to substantial noise. The feet also had a very low friction coecient, which caused them to slide across the surface during each step.
Solution
Alternatives to FSR include strain gauges and load cells. A load cell based force sensor is dismissed because of the large size. Compared with a strain gauge, the FSR has a much wider dynamic range: typically over a 0-3 kg force range. This large dynamic range is not needed in this project, as the weight of the robot is low. Additionally, FSRs have a much lower accuracy (typically 10%) [Fraden, 2004]. This lack in precision makes them less than ideal for determining the CoP. Strain gauges will be used instead of the FSR that were mounted from the beginning. Strain gauges are used to measure stretch in materials. These devices are accurate (typically 2%) and can measure small bending in the material it is mounted on [Fraden, 2004]. The strain gauge is less than a millimetre thin and can easily be mounted on the feet of the robot without giving any constraints to the movement of the robot. The sensor consists of a thin metal wire that is enclosed in a ll layer. When the strain gauge is extended, the wire will become longer and therefore the resistance in the wire will change. This change can be measured and maps to the amount the material is bent or stretched. The use of strain gauges instead of FSR have resulted in a new foot design that can be found in App. H. This design also makes it possible to determine a more precise attack point of the force measured by each sensor compared with the old design. The problem with sliding is also resolved, as the new feet has a larger static friction. The strain gauges are mounted in a set-up called a Wheatstone bridge, See gure 5.2 on the following page for schematic of the bridge. This set-up overcomes two problems arising with using strain gauges. One problem is that the output of a strain gauge drifts if the temperature changes. This will normally happen because of the current that run through the strain gauges. In this set-up each strain gauge is equally inuenced by the temperature drift and thus reduce this problem. Furthermore this set-up also overcomes drift caused by the expansion and contraction of the metal, due to changing temperatures, to which the strain gauge is adhered to. The Wheatstone bridge measures the dierence in resistance between the strain gauge pair, i.e. how much the upper side of the metal is longer than the lower side. This conguration eectively doubles the output of the circuit and eliminates temperature drift [Fraden, 2004]. Furthermore the signal from the Wheatstone bridge is converted into a digital signal at the foot and transferred via a serial bus (SPI) to the microcontroller attached at the torso. This should reduce the noise from other PWM and power lines running alongside the strain gauge signals, and result in a more accurate measurement.
26
+ a Strain gauges
R1
CHAPTER 5. HARDWARE
+ R2
a
b
ADC
R3
R4
(a)
(b)
Figure 5.2: Figure (a) shows the placement of the paired strain gauges where it is
seen that strain gauge `a' is lengthened whilst strain gauge `b' is shortened when the beam is bended down. In gure (b) the electrical curcuit of the Wheatstone bridge is showed.
5.2 Power Supply to the Biped Robot

It is attempted to keep the weight of the biped robot as low as possible to keep the stress on the servomotors as low as possible. At the same time it is desirable to keep the robot without wires to the surroundings. This meant that a microcontroller was installed on the biped robot and that it should have the possibility to be driven by batteries. Thus, some considerations have been made upon how to make the electrical system on the robot. All servomotors are supplied with 6 V. The HS-645MG servomotors have a stall current draw of about 1.5 A while the HS-475HBs have a stall current draw of 1.1 A and the HS5955TGs have a stall current draw of about 4.2 A [Hitec, 2006] [rts et al., 2006]. The microcomputer draws about 0.5 A. Hence, the maximum current draw is approximately:
4 1.1 A + 12 1.5 A + 4 4.2 A + 0.5 A = 39.7 A
(5.1)
Fortunately, it is very unlikely that all servomotors stall at the same time. Testing has shown that, on average, the robot draws around 5 A when walking. Currents of more than 10 A has only been experienced when the HS-5995TG servomotors have been stuck, and thus stalled. To avoid that a stuck servomotor permanently damages the robot, but sucient current is available to the servomotors during normal operation, it has been chosen to make the power supply system capable of supplying a continuous current up to 10 A, but not more than that. The power supply schematic is seen in Fig. 5.3. The fuse ensures that the current draw does not exceed 10 A.
Battery or external source 8.4V6V Fuse MCU Power Switch Vcc 5.8V Decoupling diode Unused 5V Regulator Regulator Decoupling Capacitor Vdd 3.3V Microcontroller + Auxiliary supply Vservo 6V Regulator Servo Power Switch Servo supply
Figure 5.3: Power supply circuit at the biped robot which regulate the unregulated
supply voltage to 6V to the servomotors and 3.3V to the microcontroller.
In order to keep the weight as low as possible the electrical system is constructed so only one battery is necessary. The battery used is a Lithium-Ion type, supplying 7.4 V and
5.2. POWER SUPPLY TO THE BIPED ROBOT

1600 mAh. With a discharge current of 5 A, this translates to 1.6 Ah/5 A 20 minutes. This gure, however, is optimistic, as the voltage across the battery will decrease as the battery is discharged, causing the microcontroller to reset. A test have been conducted where a fully charged battery was powering the robot during a normal gait cycle. The battery lasted for approximately 5 minutes. The battery is capable of delivering 20 A of continuous current.
The voltage regulator ensures that Vservo does not exceed 6 V. It is obtained from Lynxmotion inc. The regulator used is rated at 10 A continuous and 20 A peak current. Two switches are added to be able to switch o the servomotors and microcontroller individually. The microcontroller board input voltage is rated at 6 V and includes voltage regulators of its own dropping the voltage to 5 V and 3.3 V (Vdd ) for supplying the digital circuits. To avoid having to add further external voltage regulators for supplying the auxiliary sensor circuits, they are supplied from Vdd . This, however, yields further strain on the on-board regulators, which must be accounted for. The following thermal considerations show that it is possible to supply both the microcontroller and the auxiliary circuits: The total current draw of the microcontroller plus auxiliary circuits is just below 0.5 A, which is the maximum rated amperage of the voltage regulators. The 5 V regulator must dissipate (6 V 5 V) 0.5 A = 0.5 W and the 3.3 V regulator must dissipate (5 V 3.3 V) 0.5 A = 0.85 W. According to the voltage regulator data sheets, this gives a maximal permissible ambient temperature of 125 C 0.85 W 65 C/W = 70 C, which is not expected to be experienced. A voltage drop will occur when large currents are suddenly drawn from the power supply. For example, this happens when the servomotors are switched on. Tests have shown that the microcontroller will only reset when Vcc drops below 4.5 V. This is because the 5 V rail is unused by the ServoPodTM logic, which uses Vdd exclusively. Hence, the 5 V rail is allowed to drop below 5 V without causing erroneous behaviour. A measurement of a voltage drop when the servomotor power is turned on with both battery supply and external power supply is seen in Fig. 5.4. With the battery supply, Vcc never drops below 4.5 V, so the microcontroller will not reset. This, of course, changes as the battery is discharged, but it is not considered a problem, as the robot will always use fully recharged batteries when walking autonomously. To remedy the voltage drop when the robot is supplied using a separate power supply, a capacitor can be used as a temporary power source. Vservo and Vcc are decoupled by a decoupling diode that ensures that current cannot ow from the microcontroller power circuit to the servomotor power circuit. The decoupling diode ensures that the charge stored in the decoupling capacitors ows to the microcontroller and not the servomotors. This way, only the servomotors will experience the voltage drop. The diode is a Schottky diode to make the current drop across it low. The size of the capacitor can be calculated using the formula for the current of a capacitor:
27
I=C
dV dt
[ A ] (5.2)
The voltage drop of the diode at a current draw of 0.5 A is 0.3 V. This gives the maximal permissible voltage drop as 6 V 0.3 V 4.5 V = 1.2 V. The duration of voltage drops that have been measured have all been within 25 ms. Assuming that voltage drops does not have a duration of more than this, the minimal capacitance needed is:
28
CHAPTER 5. HARDWARE
C=
25 ms 0.5 A = 10416 uF 1.2 V

Voltage drop with power supply
(5.3)
10
Voltage [V]
8 6 X: 5 Y: 4.525 4 0 5 10 15 20 25 30 35 40 X: 13.22 Y: 4.51
Voltage drop with battery 10 8 Voltage [V] 6 4 2 0 0 5 10 15 20 Time [ms] 25 30 35 40 X: 17.32 Y: 5.217
Figure 5.4: A voltage drop caused by the sudden current draw from the servomotors.
The solid line is the voltage at the power source while the dashed line is Vcc . The tooltips in the topmost graph, show the times at which the voltage supplied by the external power supply drops below the permissible level. The tooltip in the bottommost graph shows the minimum voltage experienced with battery supplied power.
A capacitor of this size would be too large to be mounted on the robot, so it is decided to have a separate supply for the microcontroller when the robot is running from an external power supply. The robot can be powered using the power supply from [rts et al., 2006] when the robot does not need to be fully autonomous. This supply can deliver 4.5 A. This is insucient for delivering enough current for the walking phase, but is sucient for testing purposes. A supply capable of delivering more current can be used instead of batteries during gait cycles lasting more than 5 minutes.
5.3 Controlling the Biped Robot

The movement of the biped robot is controlled with 20 servomotors which position the limbs of the robot. All of these servomotors need a PWM signal which rotate the servomotors depending on the duty cycle. Besides the 20 servomotor signals 16 of these servomotors has a feedback signal which measure the angle of the servomotor. Furthermore six accelerometers are placed on the robot which delivers 18 signals with information about the acceleration of the legs, hands and the torso in three dimensions. A gyroscope is also placed in the torso which measures angular velocity in two planes. Finally each foot has four strain gauge signals delivering information about the CoP of the robot. All in all this is 64 signals which is either control signals or sensor signals which should all be processed by the microcontroller. Several microcontroller boards
5.3. CONTROLLING THE BIPED ROBOT

were considered, including a Lynxmotion Mini Atom Bot Board and SSC-32 Servo Controller, a Liab nanoLiab and New Micros ServoPodTM . Because of the immediate availability and the ease of interfacing with servomotors and sensors, the ServoPodTM was chosen see App. D.7 for discussion of microcontrollers. This section will discuss how this microcontroller is used in this project and how it communicates with the sensors and actuators on the robot.
29
Gathering Sensor Signals

The sensor signals is gathered in two dierent ways depending on where the sensor is placed on the robot and depending on the noise sensitivity of the signal. An ADC with a serial peripheral interface (SPI) is placed on each foot, each arm and in the torso. But these ADCs only have eight channels which is not enough for all the sensor signals, thus the rest of the sensors is connected directly to the ADCs in the ServoPodTM . SPI is a master/slave based full duplex communication protocol which allows for several slaves to be connected to the same serial line with dierent chip select signals to the slaves. The communication starts with the master asking the slave for data and the slave simply sent back the data. In order to get the twelve bit value from the slave, three bytes needs to be sent each way. These three bytes, which are sent from master to slave and from slave to master, is depicted in Fig.5.5. The information sent from master to slave is a bit selecting between dierential or single ended operation (sgl/di) and three bits selecting one of the eight channels on the ADC. The data from the slave is a start bit and the twelve bit ADC value from the sensors. Before sending the rst byte to the slave the slave needs to be chip selected. This chip select is active low and need to be asserted from before the rst byte is send until the last byte has been received.
First Byte Second Byte Third Byte
Send From Master sgl/ 0 0 0 0 0 D2 diff X X X X X X X X X X
Received From Slave ? ? ? ? ? ? ? ? ? ? ?
D1 D0 X X X X
0 B11B10 B9 B8
B7 B6 B5 B4 B3 B2 B1 B0
Figure 5.5:
This gure shows the three bytes needed to be sent from the ServoPodTM in order to get the twelve bit ADC value via SPI. SGL/DIFF chooses between Single Ended or Dierential operation of the ADC. D0 to D2 is the bit choosing between the eight channels on the ADC and B0 to B11 is the twelve bit ADC value from the ADC.
5.3.1 Microcontroller Programs

Two dierent congurations have been developed in this project which can be uploaded to the ServoPodTM depending on which situation the robot is used in. The IsoMax code can be found at [2]. In one conguration the robot is able to walk without any wires attached at all. A battery supplies the robot and the microcomputer simply plays back some predened trajectory. The program routine is executed at 76 Hz which is the fastest possible update frequency of the servomotors when using s 76 Hz PWM frequency. In the second conguration the robot is connected to a PC running xPC with a RS232
30
CHAPTER 5. HARDWARE
cable. The function of the ServoPodTM in this situation is to gather sensor signals from the numerous sensors on the robot and send this data to the Simulink controller running on the xPC target PC. This PC is running a controller and sends back the actuator signals to the ServoPodTM which nally sends the actuator signals to the respective servomotors. A ow chart of the program is seen in Fig. 5.6.
380Hz 76Hz
Receive Sersor Signals
Mean the Sensor Signals
Receive PWM Signal From External Computer Via RS232
Send Sensor Signals To Simulink Via RS232
Send PWM Signals To Actuators
Figure 5.6: Flow chart of the program when the ServoPodTM is congured with
the second conguration.
As seen in Fig. 5.6 two state-machines are created in this routine. One state-machine gathers sensor signals at 380 Hz. This sampling frequency is due to the anti-aliasing lter implemented which has a cut o frequency of 156 Hz. This means that the sample frequency should be at least 312 Hz. As the servomotors is updated at 76 Hz and due to the way Isomax is structured the sample frequency is chosen as ve times 76 Hz equal to 380 Hz. The second state-machine performs several tasks. It means the ve samples from the sensors, sends this data to the Simulink controller running on the PC, receive the new actuator signals from the Simulink and send this to the respective servomotors. This routine is executed at 76 Hz as mentioned earlier as this is the maximum update frequency.
Part Summary
This part describes the construction and dimensions of the biped robot and the hardware changes which have been carried out on the robot. The rst chapter gives an description of the physical aspects of the robot. The second chapter describes the suggestions made in [rts et al., 2006] to hardware improvements and document which improvements have been realized in this project. One result is a new design of the feet, including new sensors which enable a more precise calculation of the CoP. This is done by replacing the force sensitive resistors from the original design with strain gauges. To make the robot fully autonomous, and to reduce the noise in sensor signals, a microcontroller has been placed on the robot to gather and process signals before they are sent to on-line processing on an external pc. Noise reduction is achieved using SPI interface on the microcontroller to communicate with ADC's distributed on the robot body. The result of this solution is that every signals which have a long signal path is converted into a digital signal thus reducing noise from other analogue signals. In order to keep the weight, as well as the physical size, as small as possible all boards on the robot are made with SMD components on PCB boards. Furthermore, the interface to the servomotors has been changed, so they are controlled directly by PWM generators on the microcontroller. The addition of the microcontroller also enables different congurations of the robot. It can be congured as stand-alone where it simply plays back some predened trajectory, or it can be congured to be connected to a host computer via a serial cable and the host computer is then used to process the received signals. A computer running the Matlab environment xPC is used as a host computer in order to have an easy interface to Simulink. The reason for using Simulink is the easy implementation of controllers, compared to implementing the controller on the microcontroller. Additionally, a gyroscope is added in the torso for measuring angular velocities. The four servomotors in the ankle have been changed to a new type which can deliver a higher torque. The refresh rate of the servomotors has been increased from 20 Hz to 76 Hz which will enable a more smooth gait and ensure a better basis for the development of a controller.
31
Part III
Modelling
In this part a mathematical model of the biped robot is developed. By splitting the model into several sub-models, which are modelled independently, the complexity of the modelling procedure is reduced. An actuator model is created, i.e. a model of the servomotors powering the robot, which describe their kinematics and thereby giving the option of designing control for the actuators. Tests are conducted on the servomotors to identify their parameters. A forward kinematic model of the mechanics of robot is developed rigorously, including a model to determine the CoM of the robot. The kinematic model also creates the necessary framework for developing the rest of the models and discussing the mechanics of the robot. The kinematic model lead to the development of two dynamic models of the biped robot. These are developed by the means of the method of First Principle, where the principles and laws of physics are used to determine the dynamics of the system. The rst dynamic model, the Newton-Euler, is based on a Newtonian approach, i.e. by computing forces and torque, whereas the second is a Lagrangian model, based on energy considerations. Both models calculate the CoP location, which is essential for enabling control design, i.e. stabilizing the robot. These two models are developed to enable design of pseudo-dynamic gaits in the control part. The parameters for all the mechanic models are obtained from a SolidWorks model of the robot, which were created by [rts et al., 2006]. All models are discussed, tested and veried. The kinematic model is veried using 3D software, and the verication of the dynamic models consists of physical tests conducted on the robot.
Chapter
Modelling Strategy
This chapter explains the strategy used for modelling the biped robot, and which aspects of biped modelling which are not considered. The modelling procedure is split into several sub-models which are composed into the full model. The procedure is seen in Fig. 6.1.
p, v, a Newton-Euler based dynamic model CoP
ref
Servomotor model
, ,
Kinematic model Lagrange mechanics based dynamic model CoP
Figure 6.1: Illustration of the model inputs and outputs.

The modelling is done using methods of First Principle and system-identication. The actual choice of modelling method depends on where it is found applicable. First, a model of the servomotors is developed using a system-identication method. It takes an input in the form of the reference angle given to the servomotor, and outputs the angular position, velocity and acceleration of the servomotor shaft. These signals are used by the kinematic model of the biped robot. A kinematic model yields a series of transformation matrices which convert positions, velocities, or accelerations in Cartesian coordinates in one joint of the robot to any joint on the robot. This is done using Denavit-Hartenberg notation for describing each link on the biped robot. The modelling of the mechanics of the biped robot has two dierent approaches. One approach has the goal of deriving a very accurate model which can be used for simulations of the robot. This will enable o-line testing of various control schemes. The other approach will develop a simplied model which can be used for on-line calculations of the torques needed by a controller. The rst approach uses a Newton-Euler based 34
35
model. This model should be able to determine the torques in the joints accurately. The other approach is a Lagrangian mechanics model, where a weighing between precision and computational complexity is made. The Newton-Euler based model determines the torques and forces aecting each link on the biped robot. This is done using Newton-Euler iterations where rst the angular velocity and acceleration, and the linear acceleration are found by starting from the supporting leg moving outward through the torso to the end of each free limb. Then the forces and torques aecting the limbs are found by beginning from the free limbs and then moving inward through the torso to the supporting leg. As mentioned in the previous chapter, the physical robot is able to output the CoP using measurements from the strain gauges placed in the feet. Hence, to verify the models, they must also output this value. This is done using a model based on Newton's second law of motion. This enable a comparison of the model output with output from the physical biped robot. The Lagrange mechanics based model derives the torques from considerations made on the changes in both kinetic- and potential energy. Two constraints are taken into the model to keep the torso of the biped robot in upright position both in the frontal and sagittal plane. As the two torques in the ankle of the supporting leg are the only torques of interest in control purposes, only these are derived in order to keep the model as simple as possible. The models in this project are only considering the SSP phase. Development of a model for the DSP phase is a very comprehensive task compared to development of a SSP model, and it is therefore time consuming to derive. The SSP phase constitutes 92% of a gait cycle, and it is assumed that it is most dicult to maintain balance in a SSP phase as the CoP must be placed within one foot. In a DSP phase the biped robot is more stable and hence a model for this phase is assumed to be a suciently approximated by the SSP phase model. The following chapters will describe the details about the modelling of the sub-models presented in Fig. 6.1. The order of the chapters will follow the order presented in the gure starting with the servomotor model.
Chapter
Servomotor Model
This chapter discusses the development of a model of the actuators in the robot, ie. the servomotors. For reasons that are explained in the latter part of this chapter, the model is not used further in this project, so for brevity the modelling procedure and the obtained results are presented in App. E and only summarized here. It was chosen to extend the servomotor model used in [rts et al., 2006] to include deadzones and a time delay. The parameters was determined experimentally using a systemidentication approach, actuating the servomotors with step and pulse responses. The results of the model with tted parameters is seen in Fig. 7.1.
Hitec HS645MG Servo Response vs. Model Position [rad] 0.6 0.5 0.4 2 Position [rad] 0.04 0 0.04 2 Position [rad] 3 4 5 6 7 8 Hitec HSR5995TG Servo Response vs. Model 9 3 4 5 6 7 8 9
Hitec HS645MG Servo Response vs. Model Error
0.8 0.7 0.6 2 3 4 5 6 7 8 Hitec HSR5995TG Servo Response vs. Model Error 9
Position [rad]
0.04 0 0.04 2 3 4 5 Time [s] 6 7 8 9
Figure 7.1: Test data showing the response from the model vs the response from
the servomotor. The solid lines are the measured values and the dashed lines are the model output.
36
37
As seen in the gure, the model approximates the responses from the servomotors well. However, the data gathered from the servomotors are with very limited load. As the load on the servomotors change during a gait cycle, so will the dynamics of the servomotor. This is seen in the test documented in Appendix D.6. Consequently, apart from the internal inertia of the motor, this servomotor model is only a kinematic model, as it does not take the inertia of the load into consideration. Looking at the results obtained in the test it is clear that this will lead to signicant deviations between the model output to a real servomotor output when a large load is applied. The model will not see the load, and the acceleration and maximum velocity will be unchanged, whereas the real servomotor will have deteriorating performance as the load is increased. In [rts et al., 2006] it is shown that during their gait cycle, the maximum torque exerted on the servomotors were 2 Nm. The maximum torque delivered by the HSR5995TG servomotors are 2.3 Nm, so it will be expected that the performance will be signicantly changed during peak loads, and hence the accuracy of the model will also be deteriorated. To make an actuator model which includes the load torque will require feedback from the dynamic models, making the whole model very complex and calculationally intensive to simulate. It will also require extensive testing to nd the relationship between load torque and servomotor performance. Due to time constraints, this has not been done. The lack of a proper actuator model will not hinder the use of the other models for trajectory generation. It will, however, make a complete simulation model of the robot inaccurate and unsuited for controller testing. With the actuator model complete, the kinematic model will be derived in the next chapter.
Chapter
Kinematic Model
In this section, the forward kinematics of the robot is developed. The kinematics of the robot has also been developed in [rts et al., 2006], but given the fundamental importance of this task, it is also treated and derived in this project. The fundamental importance is due to the fact, that the frames of the robot and the orientation of the joint and servomotor angles are dened here, as is the rotation matrices used in both the Lagrangian and the Newtonian model. Thus, the structure and results of the dynamic models are dependent on the correctness of the kinematics, and the implementation of controllers rely on the denition of these angles and frames. Errors in the kinematics of the dynamic models in [rts et al., 2006] has also been discovered, which furtherly justies a thorough treatment of the kinematics, as the errors may originate from here. The results of the kinematics obtained in this report are compared with those obtained in [rts et al., 2006]. As the frame axment in [rts et al., 2006] seems reasonable, and to ease the comparison, these are reused in this project. The derivation of the kinematics is composed of four sections. First, a brief description of kinematics and the method used to describe kinematics is presented. This includes how positions and orientations of the robot's links can be represented in space. DenavitHartenberg notation and conventions are used for describing links and attachment of frames. In the second section, the methods are applied to the biped to describe its kinematics. In the third section, the kinematics are veried. In the fourth and last section, an expression of the center of mass of the entire biped in any possible posture is derived based on the kinematics. This is used to evaluate static stability in the controller part.
8.1 Brief Description of Methods to Describe Kinematics

In kinematics, motion is treated without regard to the forces that causes it. The entities studied in kinematics are position, velocity, and acceleration. 38
8.1. BRIEF DESCRIPTION OF METHODS TO DESCRIBE KINEMATICS

The robot is regarded as chains of bodies called links connected by joints, where a joint in the case of the robot are 1 Degree of Freedom (DoF) revolute joints. These chains are also called manipulators. To describe every link of the robot in space, both a representation of its position and orientation is needed.
39
Position and Orientation in Space

Given a reference coordinate system {A}, any link p can be located by a 3 1 position vector. To give the orientation of a link, a coordinate system {B } is attached to the link in a known way. The orientation of {B } relative to the reference frame {A} is a linear transformation called the rotation matrix or the direct cosine matrix (DCM). The rotation matrix transforming from {B }-coordinates to {A}-coordinates is written as A BR [Craig, 2003]:
A B
R:AB
B p=A BR p
(8.1)
Where Bp is the link vector p seen from frame {B } and Ap is the link vector p seen from frame {A}.
1 , b 2 If the three principal axes of {B } is described by a set of orthonormal vectors b A and b3 , then the rotation matrix BR can be written as:
A B B T B 1 Note that A . BR = AR = AR
R=
b1
b2
b3
(8.2)
When using a rotation matrix as in Eq. (8.1), the origin of frame {A} and {B } coincides as illustrated in Fig. 8.1a, where frame {A} and {B } is attached to the same link. To attach a frame {B } to another link than {A}, two pieces of informations are required to dene it relative to the frame {A}. These are the rotation matrix A BR and the vector A pB,org pointing to the origin of {B } from frame {A}[Craig, 2003]:
A B A p=A BR p + pB,org
(8.3)
Figure 8.1b shows how this transformation can be applied to several frames.
a2 b2 a2 a1 a3 b3 a1 , b1 (a) (b) a1
A
b2 pB,org c2 b3 b1
B
pC,org c3
c1
Figure 8.1: In (a) the origin of {A} and {B } coincides as they are dened relative
to each other by a rotation matrix. In (b) both a rotation matrix and an oset vector is required to dene {B }. The frame {B } is dened relative to {A}, while frame {C } is dened relative to {B }.
40
CHAPTER 8. KINEMATIC MODEL

For brevity, Eq. (8.3) can be written as a single matrix operation as follows:
A
p = 1
0 0 0
A B
pB,org 1
p 1
(8.4)
The 4 4 matrix in Eq. (8.4) is called a homogeneous transform. This transformation matrix T includes all the necessary information about the position and orientation of a reference frame with respect to another frame. Eq. (8.4) will also be written as,
A B p=A BT p
(8.5)
without indicating p is a 4 1-vector.
The Denavit-Hartenberg Notation

In this section, the Denavit-Hartenberg notation for describing a chain of bodies of a robot is presented. This notation states, that the kinematics of a robot can be represented by four parameters for each link. Two to describe the link itself, and two to describe its connection to the next link [Craig, 2003]. A link is dened as a rigid body which denes the relationship of joint axes of a robot. A joint axis i is a vector direction in space, about which link i rotates relative to link i 1. The relative location of two joint axes can be specied by two parameters, the distance between them and the angle or twist between them. The distance ai1 between the two joint axes i 1 and i is dened as the length of the line mutually perpendicular to both joint axes as illustrated in Fig. 8.2a. The entity ai1 is also known as the link length. The second parameter is the link twist i1 . If both axes are projected onto a plane whose normal is the line mutually perpendicular to both joint axes, the angle between their projections is the link twist angle as illustrated in Fig. 8.2b. Neighboring links have a common joint axis (in the gure, link i 1 and i have the common joint axis i). To describe the connection, the Denavit-Hartenberg notation accommodates two parameters, the link oset di and the joint angle i . The link oset di is the distance between the two link length lines ai1 and ai along the joint axis i. The joint angle i is the angle between ai1 and ai measured about the joint axis. The two entities are illustrated in Fig. 8.2c. The oset di is constant and i is time varying. Using the Denavit-Hartenberg notation, the following convention can be used for axing frames to links.
Convention for Axing Frames to Links

In [Craig, 2003] a convention is given for axing frames to the links of robots or manipulators consisting of a single chain of links attached to some xed base:
i of frame {i} is coincident with joint axis i. 1. The third axis z

2. The origin of frame i is located where link length ai intersects joint axis i.
i point along link length ai in the direction from joint i to i +1. 3. The rst axis x i is formed by the right-hand rule to complete a right-handed 4. The second axis y coordinate system.
8.1. BRIEF DESCRIPTION OF METHODS TO DESCRIBE KINEMATICS

Joint axis i Joint axis i-1
Link Link
41
Joint axis i+1 i zi+1 yi+1
i-1 zi ai xi
zi-1 ai-1 yi-1 xi-1 di
yi
xi+1 ai+1
(a)
Joint axis i-1 ai-1 Joint axis i+1
Joint axis i
Joint axis i-1

Link
i-1 ai qi
Link (b)
ai-1 i
Joint axis i+1

(c)
Figure 8.2: Figure (a) is an illustration of the link length and link displacement pa-
rameters. In (a) the frames are attached using the convention dened in [Craig, 2003]. In (b) the link twist parameter is illustrated for link i 1. In (c) the joint angle for joint i is illustrated.
i . A chain of links with attached The angle i is measured in right-hand sense about x frames is seen in Fig. 8.2a. Also note that the frame attachment is not unique, one of may be chosen. two possible directions of z
The rst link of a chain is the base of the robot, called link 0. The attached frame is called frame {0} and is stationary. Thus, all other link positions may be described in terms of this frame. The coordinate system is attached so it coincides with frame {1} when joint angle 1 is zero. As a result the link parameters d0 , a0 and 0 are always zero. Using the described frame attachment procedure, the link parameters and joint variable can be found as:
i to z i+1 measured along x i. ai = the distance from z i to z i+1 measured about x i. i = the angle between z i1 to x i measured along z i. di = the distance from x i to x i+1 about z i. i = the angle between x
The transformation matrix from frame {i} to {i 1} is found by from the four link parameters ai , i , di and i as:
cos i sin i cos i1 i 1 i T = sin sin i i 1 0
sin i cos i cos i1 cos i sin i1 0
0 sin i1 cos i1 0
ai1 sin i1 di cos i1 di 1
(8.6)
42

Having given a brief description of axing frames and deriving the transformation matrices between them, these methods are applied to the robot to yield a kinematic model of the robot.
8.2 Deriving the Kinematics of the Robot

The task of deriving the kinematics of the robot is carried out in three steps. The rst step is to ax frames to the links of the robot. The second step is to identify the four link parameters for each link, and the third is to nd the transformation matrices describing their relationships.
Axing Frames to the Robot

In [Craig, 2003], a convention is given only for axing frames to a single chain of links with a xed base, whereas the biped has no xed base and consists of multiple chains. However, in SSP the biped may be viewed as having a chain starting from the supporting foot, splitting into three chains: the two arms and the non-supporting leg. Furthermore, the chains of the biped do not have an overall xed base, but do have a xed base during the period of a single SSP. In [rts et al., 2006] the suggested solution is to model the biped as four chains:
Chain 1 consists of the supporting leg, starting from the foot and ending in the hip. Chain 2 consists of the non-supporting leg, starting from its hip joint and ending in the foot. Chain 3 consists of the right arm, starting in the shoulder and ending in the ngers. Chain 4 consists of the left arm, starting in the shoulder and ending in the ngers.
Chain 2, 3 and 4 are attached to chain 1 at the hip joint using transformation matrices. This method is also used in this project, as it is simple and advantageous. It yields a non-moving base frame during the SSP and make use of the well dened procedure in [Craig, 2003] for attaching frames to the four individual chains of links. The frame attachment to the biped is seen in Fig. 8.3a. In the gure, the right foot is the supporting foot. The right leg is abbreviated RL, the left leg LL, the right arm RA and the left arm LA. The frame attachment in Fig. 8.3b depicts the positive direction of rotation of the dierent joint angles. In Fig. 8.3c a front view of the biped robot is seen, showing the link lengths and joint displacements, and the three vectors locating the bases of the arms and the non-supporting legs relative to the hip frame RL5. The zero-frames of the left leg and the arms {LL0 }, {RA0 } and {LA0 } are seen in Fig. 8.4a. These are attached to the body of the biped. The origin of {LL1 }, {RA1 } and {LA1 } coincides with {LL0 }, {RA0 } and {LA0 } respectively, but their orientation is given by a rotation about their z -axes with the joint angles ll1 , ra1 and la1 . Thus, when these angles are zero frame {LL1 }, {RA1 } and {LA1 } coincides with the zero-frames. The base frame {RL0 } is seen in Fig. 8.4b.
8.2. DERIVING THE KINEMATICS OF THE ROBOT
43
RA2
RA2
y z
qRA2 x
LA2 LA2
LA2
y z
RA2
RA1
x y
x
RA1
LA1
qLA2 qRA3 qRA1 qRA4 qRL5 qLA4 qLL1 qLA5 qLL2 qRL3 qLA3 qLA1
RA1
y x
RL5
LA1
LA1
y z
LL1
LA4
qRA5 y
LA3
RA3
y
RA3
RA3
RA4
z z
RA5
y x y
RA4
z z
RL5
LA3
LA4
RL5
y x
LL1
LL1
LA4
z z
LA3
RA4
RA5
x z
RL4
RL4
LA5
y x
LA5
LA5
y
RA5
qRL4
LL2
RL4
z x
LL2
LL2
x z
RL3
RL3
y y
RL3
LL3
z x
LL3
LL3
qRL2 qRL1 qLL5
x z
RL2
qLL3 qLL4
RL2
y y
RL2
LL4
z x
LL3
LL4
x y
RL1
RL1
z z
RL1
LL5
y x (a)
LL5
LL5
(b)
+90
-90
aRA2 dRA3
aRA1
RL5
aLA1
pLA0,org
aLA2 dLA3 aLA3
aRL3
aLL2
RL5
aRA3
+36.8
pRA0,org
RL5
pLL0,org
aRL2
-36.8
aLL3
aRA4
aRL4 aRL3
aRL4
aLL1
aLL1
aLA4
+57.2
-57.2
aRL1
aRL1
aLL4
aLL4
aLL2
(c)
Figure 8.3: In gure (a) the frame attachment to the biped is seen, in this case, the
right foot is the supporting leg. The right leg is abbreviated RL, the left leg LL, the right arm RA and the left arm LA. The zero-frames of the four limbs coincides with the 1-frames in the gure as the joint angles are zero. In gure (b) the positive direction of rotation of the joint is indicated. In (c) the link parameters possible to depict in a front view of the biped are shown.
44
x x z y
RA0
LA0
{RL5}
x z z x y
y y x
RL5
LA0
LA0
y
{LL0}
RA0
RA0
LL1
z ,z
LL0
LL1
y x
LL0
LL1
{RA0}
x y x y (c) z
z y
RL5
RL5
LL0
x y
RL0
RL0
z z
RL0
{LA0}
Right leg
(a)
(b)
Figure 8.4: Figure (a) shows the four body-attached frames {RL5 }, {LL0 }, {LA0 } and {RA0 }, each drawn with dashed lines. Figure (b) shows the base-frame {RL0 } and gure (b) shows the relative orientation of the frames {RL5 }, {LL0 }, {LA0 }.
Identifying Link Parameters

Given the frames in Fig.8.3a, the link parameters are listed in Tab.8.1, Tab.8.2, Tab.8.3 and Tab. 8.4.
i 1 2 3 4 5
rl,i1 [ ] 0 -90 0 0 90
arl,i1 [mm] 0 42.9 42.9 57.2 42.9
drl,i [mm] 0 0 0 0 0
rl,i [ ] rl1 rl2 rl3 rl4 rl5
Table 8.1: Link parameters of the right leg (RL).

i 1 2 3 4 5 ll,i1 [ ] 0 -90 0 0 90 all,i1 [mm] 0 42.9 57.2 42.9 42.9 dll,i [mm] 0 0 0 0 0 ll,i [ ] ll1 ll2 ll3 ll4 ll5
Table 8.2: Link parameters of the left leg (LL).

The vectors rl5pll0,org , ra5pra0,org and la5pla0,org locating the origins of frame {LL0 }, {RA0 } and {LA0 } relative to {RL0 } have the following coordinates: 0 50.5 50.5 rl5 pll0,org = 61.6 rl5pra0,org = 49.1 rl5pla0,org = 110.7 [ mm ] (8.7) 0 0.5 0.5 The orientation of frame {LL0 }, {RA0 } and {LA0 } relative to {RL5 } are obtained by deriving the corresponding rotation matrices. These matrices are found using the result
8.2. DERIVING THE KINEMATICS OF THE ROBOT

i 1 2 3 4 5 ra,i1 [ ] 0 90 -90 90 0 ara,i1 [mm] 0 18.7 29.4 -18.7 31.8 dra,i [mm] 0 1.3 68.8 0 0 ra,i [ ] ra1 ra2 + 90 ra3 ra4 + 36.8 ra5 + 53.2
45
Table 8.3: Link parameters of the right arm (RA).

i 1 2 3 4 5 la,i1 [ ] 0 -90 90 -90 0 ala,i1 [mm] 0 18.7 29.4 -18.7 31.8 dla,i [mm] 0 1.3 68.8 0 0 la,i [ ] la1 la2 90 la3 la4 36.8 la5 53.2
Table 8.4: Link parameters of the left arm (LA).

in Eq. (8.2) and by inspecting Fig. 8.4c: 1 0 0 1 rl5 rl5 0 1 0 0 R = R = ll0 ra0 0 0 1 0
0 0 0 1 1 0
1 rl5 0 R = la0 0
0 0 1
0 1 0
(8.8)
Given these vectors, rotation matrices and link parameters, the transformation matrices can be found.
Derivation of Transformation Matrices

0 The transformation matrix rl rl1T from frame {RL1} to {RL0} can be found by inserting the parameters from Tab. 8.1 into Eq. (8.6) on page 41: cos rl1 sin rl1 0 arl0 sin rl1 cos rl0 cos rl1 cos rl0 sin rl0 drl1 sin rl0 rl0 rl1T = sin rl1 sin rl0 cos rl1 sin rl0 cos rl0 drl1 cos rl0 0 0 0 1 cos rl1 sin rl1 0 0 sin rl1 cos rl1 0 0 = 0 0 1 0 0 0 0 1
Accordingly, the rest of the transformation matrices for the right leg are found: cos rl2 sin rl2 0 arl1 cos rl3 sin rl3 0 arl2 sin rl3 0 0 1 0 cos rl3 0 0 rl1 2 rl rl2T = rl3T = sin rl2 cos rl2 0 0 0 0 1 0 0 0 0 1 0 0 0 1 cos rl4 sin rl4 0 arl3 cos rl5 sin rl5 0 arl4 sin rl4 0 cos rl4 0 0 0 1 0 rl3 rl4 rl4T = rl5T = 0 0 1 0 sin rl5 cos rl5 0 0 0 0 0 1 0 0 0 1 Frame {LL0} is dened by translating frame {RL5 } with the vector rl5pll0,org and 5 rotating it by the matrix rl ll0R given in Eq. (8.8). These two informations are inserted
46

5 in Eq. (8.4) to yield the transformation matrix rl ll0T . The rest of the transformation matrices for the left leg are found by inserting the link parameters listed in Tab. 8.2 into Eq. (8.6):
rl5 ll0
rl5 ll0
T =
rl5
0 0 0
pll0,org 1 0 1 0 0
cos ll2 sin ll2 0 0 ll1 ll2T = sin ll2 cos ll2 0 0 cos ll4 sin ll4 0 sin ll4 cos ll4 0 ll3 ll4T = 0 0 1 0 0 0
all3 0 0 1
all1 0 0 1
cos ll1 sin ll1 ll0 ll1T = 0 0 cos ll3 sin ll3 ll2 ll3T = 0 0 cos ll5 0 ll4 ll5T = sin ll5 0
sin ll1 cos ll1 0 0 sin ll3 cos ll3 0 0 sin ll5 0 cos ll5 0
0 0 1 0 0 0 1 0 0 1 0 0
all0 0 0 1 all2 0 0 1
all4 0 0 1
Accordingly, the transformation matrices for the right and left arm are found:
rl5 ra0
rl5 ra0
rl5
ra1 ra2
ra3 ra4
sin ra2 cos ra2 0 ara1 0 0 1 dra2 = cos ra2 sin ra2 0 0 0 0 0 1 cos(ra4 +36.8 ) sin(ra4 +36.8 ) 0 0 = sin(ra4 +36.8 ) cos(ra4 +36.8 ) 0 0
rl5 la0
0 0 0
pra0,org 1
ra0 ra1
ra2 ra3
0 1 0 0
ara3 0 0 1
ra4 ra5
cos ra1 sin ra1 0 0 sin ra1 cos ra1 0 0 = 0 0 1 0 0 0 0 1 cos ra3 sin ra3 0 ara2 0 0 1 dra3 = sin ra3 cos ra3 0 0 0 0 0 1 cos(ra5 +53.2 ) sin(ra5 +53.2 ) sin(ra5 +53.2 ) cos(ra5 +53.2 ) = 0 0 0 0 cos la1 sin la1 0 0 sin la1 cos la1 0 0 = 0 0 1 0 0 0 0 1 cos la3 sin la3 0 ala2 0 0 1 dla3 = sin la3 cos la3 0 0 0 0 0 1 cos(la5 53.2 ) sin(la5 53.2 ) sin(la5 53.2 ) cos(la5 53.2 ) = 0 0 0 0
0 0 1 0
ara4 0 0 1
rl5 la0
rl5
sin la2 cos la2 0 ala1 0 0 1 dla2 la1 la2T = cos la2 sin la2 0 0 0 0 0 1 cos(la4 36.8 ) sin(la4 36.8 ) 0 0 la3 la4T = sin(la4 36.8 ) cos(la4 36.8 ) 0 0
0 0 0
pla0,org 1
la0 la1
la2 la3
0 1 0 0
ala3 0 0 1
la4 la5
0 0 1 0
ala4 0 0 1
Given these matrices it is possible to compute the position and orientation of all the frames relative to each other. For example, the transformation matrix to give the position of a point in the arm frame {RA3} in the base frame {RL0}, can be found as:
rl0 ra3
0 T = rl rl1T
rl1 rl2
rl2 rl3
rl3 rl4
rl4 rl5
rl5 ra0
ra0 ra1
ra1 ra2
ra2 ra3
(8.9)
Having derived the transformation matrices of the biped robot, these are veried in the following section.
8.3. VERIFICATION OF KINEMATIC MODEL
47
8.3 Verication of Kinematic Model

To verify the kinematics of the biped robot, the links of the robot has been implemented in the 3D design program 3D Studio Max (3ds max) using its bones tool. This tool allows the creation of chain of links, dening their joint axes and to give joint angles as arguments. When giving angles to the joints, 3ds max shows the resulting position and orientation of the links, and can compute their coordinates in a global coordinate system. This is without the user supplying any transformation matrices. This makes it ideal to verify kinematic models of chains of links. The bone skeleton in 3ds max have been constructed using the link parameters in Tab. 8.1, Tab. 8.2, Tab. 8.3 and Tab. 8.4, and the vectors in Eq. (8.7). Two screenshots of the biped implemented as bones in 3ds max are seen in Fig. 8.5. The actual 3ds model can be found in appended document [3] along with a Matlabscript containing the transformation matrices. The Matlab-script takes the input of the 20 joint angles and return the position of all the frames in {RL0} coordinates. By giving the same joint angles to the 3ds max model as the Matlab-script, the kinematics have been veried, as their coordinates of the dierent links coincides. Comparing the result to those found in [rts et al., 2006], their transformation matrix correspond1 ing to RA RA2T diers from the results obtained here. The error seems to originate in an incorrect calculation of cos( 90 ) and sin( 90 ) in [rts et al., 2006]. Otherwise, the results of [rts et al., 2006] complies with the result obtained in this section.
8.4 The CoM of the Robot Using the Kinematic Model

In the case of static walk, the CoP can at any time be found as the projection of the CoM of the robot onto the supporting surface. As a result, an expression for the CoM given any possible posture of the robot is derived, as this can enable the design of static walk. The coordinates of the CoMs of the individual links of the biped are known in their respective joint frames. Thus, the total CoM of the biped can be found by nding the position of the CoMs of the individual links in the base frame {RL0 } and applying the following formula:
pCtot =
where mtot =
1 mtot
mi pCi
(8.10)
mi .
The positions of the CoMs of the links in {RL0 } are found by applying transformation matrices: CoM locations of links in right leg:
rl0
CoM locations of links in left leg:

rl0
pCrl1 = . . .
rl0 rl1
rl1
pCrl1
0 rl5 ll0 ll1 pCll1 = rl pCll1 rl5T ll0T ll1T . . . 0 rl5 ll0 ll5 pCll5 = rl pCll5 rl5T ll0T ll5T
rl0
0 rl1 rl2 rl3 rl4 pCrl4 = rl pCrl4 rl1T rl2T rl3T rl4T
rl0
48
Figure 8.5: Screenshots of the biped implemented in 3ds max as bones using the
link parameters in Tab.8.1, Tab.8.2, Tab.8.3 and Tab.8.4, and the vectors in Eq.(8.7). In gure (a) the joint angles are zero. In (b), dierent joint angles are given to the model, and the global position is in the example found for frame {LL2}.
8.5. SUMMARY OF THE KINEMATIC MODEL

CoM locations of links in right arm:
rl0
49
CoM locations of links in left arm:

rl0
pCra1 = . . .
rl0 rl5
rl5 ra0
ra0 ra1
ra1
pCra1
0 rl5 la0 la1 pCla1 = rl pCla1 rl5T la0T la1T . . . 0 rl5 la0 la5 pCla5 = rl pCla5 rl5T la0T la5T
rl0
0 rl5 ra0 ra5 pCra5 = rl pCra5 rl5T ra0T ra5T
rl0
CoM location of torso:

rl0
CoM location of right foot:

rl0
pCt =
rl0 rl5
rl5
pCt
pCrl,foot = rl0pCrl,foot
These expressions are then inserted into Eq. (8.10) to yield an expression for the CoM of the robot. This expression is implemented in a Matlab -script, see appended document [4]. Given the joint angles, the script computes the CoM of the individual links in {RL0 } and afterwards computes the CoM of the robot. The result is plotted in a 3D plot, where the robot is plotted as a stick gure, along with the CoM of the individual links and the CoM of the robot. An example of this is seen in Fig. 8.6.
[m]
CoM
[m]
CoM
CoP
RL0
RL0
CoP
RL0
[m]
z
(a)
RL0
[m]
RL0
[m]
(b)
RL0
[m]
Figure 8.6: The gures illustrate a stick representation of the robot seen in frame {RL0 }. In (a) all the joint angles are zero, while dierent joint angles are given to the robot in (b). The crosses indicate the CoMs of the individual links and the bold dots are the joints. The illustrations show the calculated CoM of the robot and the resulting CoP, i.e. the projection of the CoM onto the surface.
8.5 Summary of the Kinematic Model

The purpose of a forward kinematic model is to create a mapping between the joint angles of the biped robot to its posture in a Cartesian space. In this chapter, a kinematic model was created for the SSP phase. This was developed by rst giving a method of how the positions and orientations of the links of the robot can be represented in Cartesian space. This method is based on attaching a Cartesian coordinate system or frame to each link of the robot, which
50

follows the motion and orientation of the link. As a result, a complete kinematic model was obtained by nding the position and orientation of all the frames relative to each other as a function of the joint angles. These functions were based on rotation matrices R, also called a direct cosine matrix. The concept of rotation matrices R was extended to transformation matrices T ,
A
p = 1
0 0 0
A B
pB,org 1
p =A BT 1
(8.11)
which transforms the coordinate of point Bp into its coordinates in frame {A}. To derive the transformation matrices between each coordinate system of the robot, a method of describing the links was required. This lead to the use of the DenavitHartenberg notation. The notation states, that the kinematics of a robot can be represented by four parameters for each link, where a link is considered as a rigid body, which denes the relationship of joint axes of a robot. The method used in [Craig, 2003] was used to systemize how frames were attached to the robot. This method axes a frame in each joint of a robot with the z -axis point along the joint axis and the x-axis pointing toward the next frame. A limitation of the frame attachment procedure in [Craig, 2003], is that it only describes axing frames to a single chain of links with a xed base, whereas the biped robot has no xed base and consists of multiple chains. Therefore the biped robot was modelled as four chains: the supporting leg, starting from the supporting foot which was regarded as being xed and ending in the hip; the non-supporting leg, starting from its hip joint and ending in the foot; the right arm, starting in the shoulder and ending in the ngers; and the left arm, starting in the shoulder and ending in the ngers. The three last chains were attached to the supporting leg at the hip joint using transformation matrices. It yielded a non-moving base frame during a SSP and the use of the well dened procedure in [Craig, 2003] for attaching frames to the four individual chains of links. To verify the kinematic model, the links of the robot have been implemented in the 3D design program 3D Studio Max (3ds max). This tool allows the creation of chain of links, dening their joint axes and to give joint angles as arguments. When giving angles to the joints, 3ds max shows the resulting position and orientation of the links, and can compute their coordinates in a global coordinate system. This is without the user supplying any transformation matrices. As a result, the kinematic model was veried using 3ds max, and found correct. Finally, after verifying the kinematic model, the model was applied to give an expression for the CoM of the robot. This expression was implemented in Matlab and gave a reasonable result. As a result, an expression for the CoM given any possible posture of the robot have been obtained, enabling the design of static walk.
Chapter
Newton-Euler Based Dynamic Model

Unlike the kinematic model the dynamic model describes the eects the torques has on the motion of the biped robot. The model will be rather complex as it should contain information about the motion of all the links. The model only concerns the SSP. To keep the calculations as simple as possible without loss of accuracy, the eect from the gravitational acceleration is introduced as a negative acceleration on the supporting foot. Hence no further considerations regarding the gravitational acceleration are needed on other parts of the biped robot [Craig, 2003]. The Newtonian modelling approach is based on mechanical considerations and the use of the Newton-Euler equations. In the rst section, an overall modelling approach is given. In the second section, the dynamic model based on the Newton-Euler equations is derived, this model is referred to as the Newtonian model. The CoP is also calculated in the second section. In the third section, the Newtonian model is veried.
9.1 The Modelling Approach

The Newtonian model is primarily based on [Craig, 2003], but as [Craig, 2003] only concerns manipulators, the transitions in the torso is based on other physics laws than those stated in [Craig, 2003]. This approach is also used in [rts et al., 2006]. However, the model derived in that report could not be veried properly, therefore the Newtonian model is derived from scratch in this report. The Newtonian modelling of the biped robot is devided into ve steps. The rst step is to nd the equations for the angular velocity, angular acceleration and linear acceleration of all the links on the supporting leg. The velocity and acceleration of the torso can be calculated as if the torso was a one link manipulator with the supporting leg as it's base. Thereafter, the linear acceleration of the CoM for each link is found. Step two is to nd the angular velocity, angular acceleration and linear acceleration of all the links in the free limbs (the non-supporting leg and the arms). This is done by considering the free limbs as manipulators, with their base attached to the torso. When the angular 51
52
CHAPTER 9. NEWTON-EULER BASED DYNAMIC MODEL

velocity, angular acceleration and linear acceleration of all the links are calculated, it is possible to nd the force and torque that should be applied to each link to move it in the desired way. The third step is to nd the joint torques from the forces that the free limbs apply to the torso. Step four is to move these forces and torques from the torso down to the supporting foot. The torques in the supporting foot can then be used to calculate the CoP in step ve. The ve steps can be summarized as follows: 1. Find the angular velocity, angular acceleration and linear acceleration of the supporting foot. 2. Calculate the angular velocity, angular acceleration and linear acceleration of the free limbs. 3. Find the torques that the free limbs apply to the torso. 4. Move the torques from the torso down to the supporting foot. 5. Calculate the CoP using the torques in the supporting foot. An illustration of the ve steps is seen in Fig. 9.1.
2
2 3 3
3 2 1 4
Figure 9.1: Illustration of the direction of the Newton-Euler iterations, where step
one and two is outward, step three and four is inward and step ve is the CoP calculation.
9.2 Deriving the Newtonian Model

In this section, each step will be analyzed with the right leg as the supporting leg.
9.2. DERIVING THE NEWTONIAN MODEL
53
Step 1: Velocity and Acceleration of the Links on the Supporting Leg

In step 1 the angular velocity, angular acceleration and linear acceleration is found for each link in the supporting leg.
Angular velocity: The Angular velocity of the center of mass of the links are found
by calculating the angular velocity iteratively from the supporting foot, moving successively, link by link up to the torso.
rl0
rl0 , and
Refer to Fig. 9.2. According to [Craig, 2003] the angular velocity of link i + 1 with respect to frame {0} given in frame {i + 1}, i+1 i+1 , can be expressed in terms of the angular velocity of link i with respect to frame {0} given in frame {i} as:
i+1 +1 i i+1 i+1 z i+1 = i i R i + i+1
(9.1)
+1 where i i R is the rotation matrix that describes frame {i} relative to frame {i + 1}, i+1 i+1 -direction is the joint angle velocity of joint i + 1 and z i+1 is the unit vector in the z for frame {i + 1}.
i+1
Z i+1
Ti+
{i + 1}
lin
Zi
{i}
i+ 1
Figure 9.2: Simplied (two dimensional) illustration of

around which describes angular velocity.
From Eq. (9.1) the angular velocity of each link can be calculated relative to its own frame. The calculations initiate at frame {RL0}. As the zero frame of the supporting leg is attached to the foot, the angular velocity of this frame (rl0 rl0 ) must be zero. The angular velocity of the torso is equal to the last frame of the supporting leg which is rl5 rl5 .
Angular acceleration: The angular acceleration of each link is calculated in a similar
way as the angular velocity. Again, refer to Fig. 9.2. From [Craig, 2003] the angular
link i
{0}
i+1
i+1 and the frame
54

i+1 , is given acceleration of link i + 1 with respect to {0} given in frame {i + 1}, i+1 from the angular acceleration of link i with respect to {0} given in frame {i} as:
i+1 +1 i +1 i i+1 i+1 i+1z i+1 = i i+i i+1 + i+1 z i R i R i i+1
(9.2)
As was the case for the angular velocity, the angular acceleration of the zero frame rl0 ) is also zero. This means that initiating the calculation at the supporting foot, (rl0 the angular acceleration of each link can be found. The angular accelerations of the CoMs are also known since it is equal to the angular acceleration of each link.
Linear acceleration: The linear acceleration of the origin of frame i + 1 with respect
to {0} given in frame {i + 1} is in [Craig, 2003] given as:
i+1 +1 i i i ipi+1 + i i i i ipi+1 +i v i+1 = i v i R
(9.3)
From Eq. (9.3) the linear acceleration for each frame origin of all frames attached to the supporting leg can be calculated. The calculation process is iterative initiating at the rl0 ). The linear acceleration for this frame is set to 9.82 m/s2 in the zero frame (rl0 v x-direction to include the gravitational acceleration as mentioned earlier. The linear acceleration of the CoMs of each link is found as:
i
Ci =i i ipCi + i i i i ipCi +i v i v
(9.4)
where frame {Ci } is located at the CoM of link i and ipCi is the position vector of that frame. From Eq. (9.1), Eq. (9.2), Eq. (9.3) and Eq. (9.4) the linear acceleration of the CoM of each link can be calculated. The rst step is illustrated in Fig. 9.3, it can be seen that the joint angles of the supporting leg is the input. The outputs are the angular velocity, angular acceleration and linear acceleration of the last frame (the torso) which is used in step 2. The angular velocity, angular acceleration and linear acceleration of the CoM of each link is also output.
RL5 RL
Step 1
vRL5, RL5
RL
RL5
, RL5
RL5
vCRL,
RL
Figure 9.3: Illustration of the outward Newton-Euler iterations for step one.
Step 2: Velocity and Acceleration of the Links on the Free Limbs

The torso is seen as a normal link located between the last frame of the supporting leg (frame {RL5}), the zero frames of the left leg (frame {LL0}) and, the arms (frame {RA0} and {LA0}). The angular velocity of the initial frames of the free limbs can be found by transforming the angular velocity from frame {RL5} to frames {LL0}, {RA0}, and {LA0} using rotation matrices.
ll0 ra0
0 rl5 ll0 = ll rl5 rl5R
0 rl5 ra0 = ra rl5 rl5R 0 rl5 la0 = la rl5 rl5R
la0

The angular acceleration can be found in a similar way.
ll0 ra0
55
0 rl5 ll0 = ll rl5 rl5R
0 rl5 ra0 = ra rl5 rl5R

la0 rl5 la0 = rl rl5 5R
la0
The linear acceleration can be found from Eq. (9.3).

ll0 ra0
0 ll0 = ll v rl5R
rl5 rl5
rl5 rl5pll0 + rl5 rl5
rl5 rl5
rl5 rl5 rl5pll0 +rl5 v
0 ra0 = ra v rl5R 0 la0 = la v rl5R
rl5 rl5pra0 + rl5 rl5 rl5 rl5pla0 + rl5 rl5
rl5 rl5 rl5pra0 +rl5 v rl5 rl5 rl5pla0 +rl5 v
la0
rl5
rl5
When the angular velocity, angular and linear acceleration of the origin of the three initial frames are known, the outward iterative Newton-Euler algorithm from step one can be used to nd the acceleration of all the CoMs of the links on the free limbs. The second step is illustrated in Fig.9.4, where the input is the angular velocity, angular acceleration and linear acceleration from step one and the joint angles of the free limbs. The outputs are the angular velocity, angular acceleration and linear acceleration of the CoMs of all the links in the free limbs. The outputs are then used in step 3.
, , vCLL, vCRA, vCLA Step 2
RL5 LL LL
LL RL5
RA
LA
vRL5, RL5
RL5
, RL5
, ,
RA RA
, ,
T
LA
T
LA
Figure 9.4: Illustration of the outward Newton-Euler algorithm in step two.
Step 3: Forces and Torques Applied to the Torso by the Free Limbs
Knowing the linear and angular acceleration of the CoM of each link, the Newton-Euler equations can be applied to calculate the forces (F i ) and torques (N i ) acting at the CoM of each link. The equation for the forces and torques are given by the following two equations.
Ci F i = mi v
where mi is the total mass of link i.
(9.5)
i + i CiI i i N i = CiI i
(9.6)
where CiI i is the inertia tensor of link i with respect to frame {Ci }. Frame {Ci } has its origin at the CoM of link i and has the same orientation as the link frame. Knowing the forces and torques acting on each link in the free limbs the inward NewtonEuler calculation can begin. This is done by writing force balance and torque balance equations based on a free body diagram as depicted in Fig. 9.5. Each link is subjected to a force and a torque by its neighboring links and experiences an inertial force and torque. The calculations are initiated at the last link in the free limbs (link LL5, RA5, and LA5). Summing up the forces acting on link i, yields:
i i+1 F i = if i i f i+1 i+1R
(9.7)
56

Joint axis i+1 Joint axis i Ni fi
{i} Link
ni+1 i
{i+1}
Ni+1
Link i+1 CoM
Joint axis i+2 ni+2

{i+2}
CoM
fi+1
ni Fi
Fi+1
fi+2
Figure 9.5: Free body diagram for a single link.

where if i is the force exerted on link i by link i 1. Summing up the torques about the CoM and setting them equal to zero, results in:
i
N i = ini ini+1 + ipCi if i ipi+1 ipCi if i+1
(9.8)
where ini is the torque exerted on link i by link i 1. Inserting Eq. (9.7) into Eq. (9.8) and adding rotation matrices, results in:
i i+1 i+1 N i = ini i ni+1 ipCi iF i ipi+1 i f i+1 i+1R i+1R
(9.9)
At last the force and torque equations are rearranged so they appear as iterative relationships from link i + 1 to link i.
i i+1 f i = iF i + i f i+1 i+1R
(9.10)
i+1 i+1 ni = iN i + i ni+1 + ipCi iF i + ipi+1 i f i+1 i+1R i+1R
(9.11)
The required joint torques are found by taking the z -component of the torque applied by link i + 1 on link i.
i i = ini iz
where the symbol is used for linear actuator force.
(9.12)
The above calculations are made for all the links on the left leg, the right and left arm. From the calculations the torques that should be applied by the actuators on the free limbs ([ll1 , ..., ll5 , ..., ra1 , ..., ra5 , ..., la1 , ..., la5 ]) are known. Moreover, the eect from the torso on the free limbs can be calculated by: ll0f ll0 , ll0nll0 , ra0f ra0 , ra0nra0 , la0 f la0 , and la0nla0 . The third step is illustrated in Fig. 9.6, where the input is the angular velocity, angular acceleration and linear acceleration of the CoMs of all the links in the free limbs from step 2. The outputs are the forces and torques from the base of the free limbs, these outputs are then used in step 4.
Step 4: Forces and Torques Acting on the Supporting Foot

In this step the forces and torques from the free limbs are moved to the supporting foot. The needed information from step three is the forces and torques exerted on the base of the free limbs by the torso: ll0f ll0 , ll0nll0 , ra0f ra0 , ra0nra0 , la0f la0 , and la0nla0 .
57
vCLL, vCRA, vCLA

LL LL
, ,
RA RA
, ,
T
LA
Step 3
fLL0, fRA0, fLA0
T T
T
LA
nLL0, nRA0, nLA0
Figure 9.6: Illustration of the inward Newton-Euler algorithm in step 3.
f
{RA0}
RL5
{LL0}
CoM C RL5
-n
LA0
-n
RA0
N -f
RA0
LL5
F
{RL5}
RL5
-f
{LL0}
LA0
-n -f
LL0
LL0
RL5
Figure 9.7: Free body diagram for the torso, where the torque and forces acting on the torso is indicated.
These are illustrated in Fig. 9.7. These forces and torques must rst be moved down to the rst frame of the supporting leg. The resulting force acting on the torso can be found by taking the sum of the force exerted on the torso by the supporting leg and the free limbs as shown in Fig. 9.7:
rl5
F rl5 = rl5f rl5 rl5f ll0 rl5f ra0 rl5f la0
Rearranging this equation and introducing the associated rotation matrices the force that should be given from link RL4 to the torso is:
rl5
5 ll0 5 ra0 5 la0 f rl5 = rl5F rl5 + rl f ll0 + rl f ra0 + rl f la0 ll0R ra0R la0R
(9.13)
In the same way the torque balance for the torso can be found.
rl5
N rl5 =rl5nrl5 rl5nll0 rl5nra0 rl5nla0 rl5pCrl5 rl5f rl5

rl5 rl5
pll0 rl5pCrl5 rl5f ll0
rl5
pra0 rl5pCrl5 rl5f ra0

(9.14)
pla0 rl5pCrl5 rl5f la0
Rearranging this equation and introducing the associated rotation matrices the torque that should be given from link RL4 to the torso is:
rl5
5 ll0 5 ra0 5 la0 nrl5 =rl5N rl5 + rl nll0 + rl nra0 + rl nla0 ll0R ra0R la0R 5 ra0 5 la0 + rl5pra0 rl f ra0 + rl5pla0 rl f la0 ra0R la0R 5 ll0 + rl5pCrl5 rl5F rl5 + rl5pll0 rl f ll0 ll0R
(9.15)
The torque the actuator in joint RL5 has to deliver can be found from Eq. (9.12). The calculations are continued down the supporting leg and when the right foot is reached all the torques which has to be delivered by the actuators are known. When these calculated torques are delivered the biped robot should move as specied by the inputs (i ).
58

This fourth step can be seen in Fig. 9.8, where the inputs are the angular velocity, angular acceleration and linear acceleration of the CoMs of each link. Moreover the forces and torques from step three is used as input. The output is the force and the torques in the last link (the foot) of the supporting leg.
vCRL, fLL0, fRA0, fLA0
T T
RL
RL
Step 4
fRL1, nRL1
nLL0, nRA0, nLA0
Figure 9.8: Illustration of the inward Newton-Euler algorithm in step 4.
Step 5: Calculation of the CoP Position

The nal step is to calculate the CoP position based on the now available torques in RL1 and RL2. The forces and torques acting on the foot are listed below and can be seen in Fig. 9.9:
Reaction forces from the surface, i.e. friction and pressure forces. According to the denition of the CoP, these forces can be summarized as 3 components F p , F fric,y and F fric,z acting at the CoP. Reaction forces from the robot at the ankle joint RL1, i.e. F x , F y and F z . The y -component y of the torque acting on joint RL1. The z -component z of the torque acting on joint RL1.
tRL2
qRL1 -ty
qRL1 Fp
yCoP tz
Fy
x y
RL0
RL0
Fp
zCoP
CoMfoot
ty
Fz
x y
RL0
RL0
RL0
Ffric,y
CoP
CoMfoot Ffoot Fx
hankle
hankle
Ffric,z
RL0
CoP Ffoot
Fx
yCoM,foot
Frontal view of right foot
zCoM,foot
Sagittal view of right foot
Figure 9.9: A frontal (a) and sagittal (b) view of the right foot illustrating the
forces and torques acting on the foot. The torques y and z are the torques exerted on the ankle by the robot. The torques rl1 and rl2 are the torques delivered by the servomotors in joint RL1 and RL2 respectively. The forces Ffric,y and Ffric,z are friction forces. The quantities yCoM,foot and zCoM,foot are the y and z -coordinate of the center of mass of the foot.
During the SSP phase the supporting foot is assumed to be static, i.e. no slippage is assumed between the surfaces and the foot. If the foot is to remain static the resulting
9.3. VERIFICATION OF THE NEWTON-EULER MODEL

forces and torques acting on the foot must be zero, hence the following equations must be satised,
59
res,y res,z Fres,x Fres,y Fres,z
= y Ffoot zCoM,foot + Fp zCoP Ffric,z hankle = z + Ffoot yCoM,foot Fp yCoP Ffric,y hankle = Fp Fx Ffoot = Fy Ffric,y = Fz Ffric,z
=0 =0 =0 =0 =0
which gives the following expressions for the CoP,
yCoP = zCoP
Ffoot yCoM,foot + z Fy hankle Fx + Ffoot Ffoot zCoM,foot y + Fz hankle = Fx + Ffoot
(9.16) (9.17)
where yCoP and zCoP are the y and z -coordinate of the CoP. The quantities yCoM,foot and zCoM,foot are the y and z -coordinate of the CoM of the foot given in frame {RL0 }. The torque component z acting on joint RL1 is equal to the torque rl1 delivered by the servomotor with opposite direction, thus z = rl1 . By inspecting Fig. 9.9 the corresponding relation for y can be found:
y =
rl2 cos(rl1 )
(9.18)
Note that Eq. (9.18) becomes unstable or uncertain as rl1 approached 90 , because cos(rl1 ) and rl2 approaches zero. Now the desired expressions are determined. Before they can be used to simulate the biped robot the model must be veried. This is done in the following section.
9.3 Verication of the Newton-Euler Model

In the preceding section the Newton-Euler model of the robot in SSP is derived. Before the entire model is implemented, the validity of the CoP mapping described in Eq.(9.16) and Eq. (9.17) is discussed. A static example is given, where the CoP can be found both as the projection of the CoM on the sole, and by using Eq. (9.16) and Eq. (9.17). In Fig. 9.10 the ankle joint is loaded with a mass m having a lever arm of dy and dz in the frontal and sagittal plane respectively. The lever arm is assumed massless, thus, the CoP is the projection of the CoM of the arrangement, yielding the following CoP:
yCoP = yCoM = zCoP = zCoM =
1 mtot 1 mtot
di mi =
i=1
yCoM,foot mfoot + dy m m + mfoot zCoM,foot mfoot dz m m + mfoot
(9.19) (9.20)
di mi =
i=1
The torques loading the ankle joint are y = Fx dz and z = Fx dy . As the arrangement is static and on a planar surface, friction forces are zero making Fy = Fz = 0 and
60

dz m
Fm Fp
CoP
dy x tz
Ffoot Fx=Fm
RL0
m
Fm
x ty y
RL0
RL0
RL0
RL0
RL0
Ffoot
Fx=Fm
yCoP y CoM,foot
zCoP
zCoM,foot
Frontal view of right foot
Sagittal view of right foot
Figure 9.10: An illustration of the foot with the forces and torques aecting it and
the resulting CoP in the frontal and sagittal plane.
Fx = Fm = mg . Inserting this into Eq. (9.16) and Eq. (9.17) yields the following CoP, yCoP = Ffoot yCoM,foot + z Fy hankle mfoot g yCoM,foot + mgdy mfoot yCoM,foot + mdy = = Fx + Ffoot mg + mfoot g m + mfoot Ffoot zCoM,foot y + Fz hankle mfoot g zCoM,foot mgdz mfoot zCoM,foot mdz zCoP = = = Fx + Ffoot mg + mfoot g m + mfoot
which is equal to those found in Eq. (9.19) and Eq. (9.20). Thus, the mapping from ankle joint forces and torques to CoP is correct. The entire model is veried by implementing it in Simulink . The Simulink model can be found on the appended CD [5]. To verify the Newton-Euler model, the outputs from the ve accelerometers placed on the robot is logged and compared to outputs from the Simulink model. Additionally, the simulated CoP is compared to the CoP position measured by the strain gauges in the feet. To make this comparison, the accelerations of the accelerometers must be extracted from the model along with the CoP position. The accelerometers are not placed in the origin of the joint frame and do therefore not have the same acceleration as this joint frame. The acceleration of the accelerometers depends on two things; the acceleration of the joint frame for the link which they are attached to, and the position of the accelerometers given in the concerned frame. An expression for the acceleration of the accelerometers can be found by rewriting Eq.(9.4).
i
Si =i i ipSi + i i i i ipSi +i v i v
(9.21)
where frame {Si } has its origin in sensor i, and the same orientation as frame {i}. ipSi is the position vector of frame {Si } given in frame {i}. From Eq. (9.21) an expression for the acceleration of the ve accelerometers can be found. Right foot:
rl0
Srl0 =rl0 rl0 rl0pSrl0 + rl0 rl0 v
rl0
rl0 rl0 rl0pSrl0 +rl0 v
Left foot:
ll5
Sll5 =ll5 ll5 ll5pSll5 + ll5 ll5 v
ll5
ll5 ll5 ll5pSll5 +ll5 v
Right arm:
ra5
Sra5 =ra5 ra5 ra5pSra5 + ra5 ra5 v
ra5
ra5 ra5 ra5pSra5 +ra5 v
Left arm:
la5
Sla5 =la5 la5 la5pSla5 + la5 la5 v
la5
la5 la5 la5pSla5 +la5 v

Torso:
rl5
61
Srl5 =rl5 rl5 rl5pSrl5 + rl5 rl5 v
rl5
rl5 rl5 rl5pSrl5 +rl5 v
With the ability to extract accelerometer data and CoP position from the Newtonian model, it can be veried against the response from the physical biped robot. To make a thorough test of the model, the output of the robot should be logged while all of the servomotors are activated one at the time. As this would be a very comprehensive test, it is decided to carry out a subset of the tests. The subsets are selected in such a way that all the free limbs (arms, torso and swinging leg) are moved a considerable degree. This, along with a test of the measured CoP and the simulated CoP, is considered sucient to verify the correctness of the model. Two separate test scenarios are used. In the accelerometer tests, a scenario is used where the biped robot is xed. This means that when testing the left leg and the arms the robot is xed at the torso so that only the respective limb is moving. The right side of the robot is tested in a similar way. When testing the torso the legs are xed and only the torso moves. In the CoP test, the robot is executing a trajectory, and is thus free. The inputs in the accelerometer test is seen in Tab. 9.1. When performing e.g. the test with the left leg, all other servomotors are in the 0 position. Only the two servomotors LL2 and LL5 are used. Test Case Left Arm Right Arm Torso Left Leg Function to servomotor LA2: 0.52 + 0.52cos(x/0.262) LA3: 0.78cos(x/0.131) RA3: 0.78sin(x/0.131) RA2: 0.52 + 0.52sin(x/0.262) RL4: 0.52sin(x/0.525) RL5: 0.52sin(x/0.525) LL2: 0.78sin(x/0.525) LL5: 0.78sin(x/0.525)
Table 9.1: Table showing which servomotor is given which input in which test case.
Only two servomotors are used in every single test case (x 0 : 2).
The results of these four tests can be seen in Fig. 9.11 through Fig. 9.14. In the rst three test cases, which are depicted in Fig. 9.11 through Fig. 9.13, the model output and the output from the robot coincide almost perfectly. The output from the robot has the same characteristic form but deviate a little in amplitude. The deviation in amplitude is assumed to be caused by several dierent things. One problem could be the dead-bands in the servomotors causing the servomotor shaft not to be exactly at the desired angle. Also, as shown in App.D.6, large oset errors can occur when the servomotors are under heavy load. It might also be inaccuracies within the accelerometer measurements. The main problem, however, is suspected to be slack in the robot. To test the slack present in the robot, a servomotor has been positioned at 45 and is manually moved back and forth within the slack of the servomotor. This test is seen in Fig. 9.15 where the dashed line indicates what the acceleration should be and the solid line indicates the actual accelerometer measurement. As seen, the solid line varies around 6.93 m/s due to the slack in the servomotor. This is expected as the slack is equally large to either side of the 45 which is the reference
2
62
Right Arm XRA5 [m/s2] 10 0 10 0 1 2 3 4 Right Arm YRA5 5 6
[m/s2]
10 0 10 0 1 2 3 4 Right Arm ZRA5 5 6
[m/s2]
10 0 10 0 1 2 3 Time [s] 4 5 6
Figure 9.11: Test results for the right arm. The solid line is output from the
All the accelerations are in their respective frames.
physical biped robot whilst the dotted line is the output from the Simulink model.
Left Arm XLA5 [m/s2] 10 0 10 0 1 2 3 4 Left Arm YLA5 5 6
[m/s2]
10 0 10 0 1 2 3 4 Left Arm ZLA5 5 6
[m/s2]
10 0 10 0 1 2 3 Time [s] 4 5 6
Figure 9.12: Test results for the left arm. The solid line is output from the physical
accelerations are in their respective frames.
biped robot whilst the dotted line is the output from the Simulink model. All the
to the servomotor. It can be seen that the acceleration changes as much as 1.3 m/s . This test clearly supports the suspicion that slack is the main problem for inaccurate output from the robot. Another problem is the high sensitivity of the accelerometers when measuring angles around 45 . As seen in App. D.8 the function, mapping an angle
63
Left Foot XLL5 [m/s2] 10 0 10 0 1 2 3 4 Left Foot YLL5 5 6
[m/s2]
10 0 10 0 1 2 3 4 Left Foot ZLL5 5 6
[m/s2]
10 0 10 0 1 2 3 Time [s] 4 5 6
Figure 9.13: Test results for the left leg. The solid line is output from the physical biped robot whilst the dotted line is the output from the Simulink model. All the
Torso XRL5 [m/s2] 10 0 10 0 1 2 3 4 Torso Y RL5 5 6
[m/s2]
10 0 10 0 1 2 3 4 Torso ZRL5 5 6
[m/s2]
10 0 10 0 1 2 3 Time [s] 4 5 6
Figure 9.14: Test results for the torso. The solid line is output from the physical biped robot whilst the dotted line is the output from the Simulink model. All the
to an acceleration, is a sine function thus making small changes in angles change the output drastically around 45 . All of this leads to dierences in amplitude but not in the characteristic of the output. When inspecting Fig. 9.14 the measured output from the robot is clearly smaller in the
64
10 9 8 7 6 [m/s2] 5 4 3 2 1 0 0 5 10 Time [s] 15 20 X: 10.69 Y: 7.549 X: 15.32 Y: 6.172
Figure 9.15: Slack test using servomotor LL2. The dotted line indicate what the
output should have been and the solid line show the actual output
y -direction but larger in the z -direction. This can also be explained by slack in the robot.
The last test concerns the CoP position. The CoP is calculated on-line in Simulink using the strain gauge measurements and is compared to the CoP calculated in the model. The comparison is seen in Fig. 9.16 where the dotted line is the model output and the solid line is the output from the robot. The CoP is plotted in the global frame. As seen in the topmost plot in Fig. 9.16 the CoP moves from a positive length towards zero and back to a positive length again. This is because the CoP is seen from the global frame which is attached to the right foot. So doing a full gait cycle the CoP moves from the middle to the right foot, then to the left foot and back to the middle again. It can be seen that the output from the robot tracks the model output although with some deviations in amplitude. The lower plot in Fig. 9.16 shows CoP in the x-direction. As expected the CoP moves from the back of the foot and forward and back again during one gait cycle, seen from the right foot. Again the output from the robot tracks the model output although with some deviations in amplitude.
Chapter Summary
In this chapter a dynamic SSP model based on Newton-Euler iterations is derived. The SSP model is derived with the iterations initiated at the supporting foot as this link is xed to the ground which means that the link does not have any velocity or acceleration. From the supporting foot the iterations are continued all the way up to the torso. From the torso the iterations are continued outward to the last link of the free limbs by rst transforming the velocity and acceleration from the last link of the supporting foot to the rst link of the free limbs. The force and torque exerted on the last link are then calculated and used to initiate the inward iterations going from the free limbs to the torso. The forces and torques exerted on the torso by the free limbs are then transformed
65
CoP in ydirection seen from the global frame 8 6 [cm] 4 2 0 2 2 8 10 12 Time [s] CoP in xdirection seen from the global frame 4 6
5 0 5 10
[cm]
6 Time [s]
10
12
Figure 9.16: CoP test of the model versus the physical robot. The dotted line is
the model output and the solid line is the output from the robot. The topmost gure shows the CoP in the y -plane and the lower gure shows the CoP in the x-plane in the global frame.
down to the last link of the supporting leg. When these forces and torques are known they are used to initiate the last iterations which give the force and torque exerted on to the supporting foot. When all the iterations are conducted the actuator torque is known for all the actuators in the biped robot. One last step is performed to calculate the CoP position from the torques. These calculations are implemented in Simulink. To verify the model, the acceleration of the accelerometers and the CoP position are included in the model. Dierent tests are then carried out on the physical robot and the results are compared with the outputs from the Simulink model. The test results show that the outputs from the model resemble the outputs from the biped robot satisfactorily. Therefore it is concluded that the mathematical model can be used to describe the biped robot and be used for simulations.
Chapter
10
Lagrangian Mechanics based Dynamic Model

In this chapter a model of the biped robot will be derived using energy considerations. The purpose of deriving a second model is to avoid the very comprehensive calculations the Newton-Euler model introduces. The torques from the dierent parts of the biped robot can be determined exactly using the Lagrange-d'Alembert principle, but it will result in very large expressions and no improvement over the Newton-Euler model would be achieved. Hence, a series of simplications are made based on an analysis of the human gait found in App.A. If a simplication can be implemented introducing less than 10% error it has been carried out. This means that e.g. cosine to angles within 26 is approximated with 1 as cos(26 ) = 0.90. This will reduce the accuracy of the model, but also reduce the amount of computation needed, enabling real time calculations of the CoP. The derivation of the Lagrangian model begins by briey introducing the principles calculating the torques working on the biped robot using energy considerations. Using the kinematics derived in Ch. 8 and the servo angles as generalized coordinates, the positions of the needed links are found. With these positions and their velocities, the energies acting on the biped robot are determined. By further using the introduced principles the torques are found. The Lagrangian model is derived only with the right leg as the supporting leg, but as the biped is symmetric around a sagittal plane through the center of the body, it can also be used when the left leg is the supporting leg. As with the previous models, this model only conserns the SSP. By analyzing human gait it is found that the torso of a walking human is approximately in an upright position during the whole gait cycle. Ensuring the biped robot to do the same leads to two constraints on the input angles in the supporting leg to the model, namely rl1 + rl5 = 0 and rl2 + rl3 + rl4 = 0. These constraints can be included in the mathematical derivation of the simplied equations, but this will introduce some extra calculations. To avoid these and keep the derivation process as simple as possible these constraints are included by introducing the torques they lead to at the end of the derivation using physical considerations on the constraints.
66
10.1. LAGRANGIAN MECHANICS
67
10.1 Lagrangian Mechanics

Lagrangian mechanics is based on considerations on the kinetic energy T and potential energy U in a mechanical system. A mechanical system with n degrees of freedom can be described with n generalized coordinates, denoted q1 , ...qn . When such a system moves from time A to time B, Hamilton's Principle states that the dierence in kinetic and potential energy is always minimal. This means that the following integral has a critical value, where I is minimal.
) = I (q , q
A
)dt L(t, q , q
(10.1)
where I is a functional dened on a set of curves with real values, L = T U and is called the Lagrangian, t is time, and q is the generalized coordinates. The necessary condition for the integral to be minimal is that Eq. (10.2), called the Euler-Lagrange equation, is satised. d L L =0 dt q q (10.2)
Since Hamilton's principle states that Eq.(10.1) has a minimum at a trajectory, Eq.(10.2) is satised along that trajectory for a mechanical system. External forces Q can be taken into account using the Lagrange-d'Alembert Principle yielding d L L =Q dt q q (10.3)
In this project the servomotor angles will be used as generalized coordinates as this will yield the simplest calculations. This also turns the external forces into the torques produced gravity and the movement in the system. Friction in the joints and against the air will be neglected as well as the the spring forces from the wiring. Thus Eq. (10.3) gives d L L = dt where is the actuator torque. These equations are inspired by [Wisniewski, 2007] and constitute the theory behind the Lagrangian model of the biped robot.
(10.4)
10.2 Simplication of the Kinematics

The single support phase described in [rts et al., 2006] is based on generalized coordinates found from adding angles residing in dierent planes. As these additions are invalid and thus results in wrong coordinates, the Lagrangian model is derived from scratch in this report.
68
CHAPTER 10. LAGRANGIAN MECHANICS BASED DYNAMIC MODEL

The base frame for the model is {RL0} which is xed in the rst joint in the supporting leg, here joint RL1, not rotating with rl1 . To enable the description of all the desired points, transformations of these to the base frame is needed and is done using the rotation matrices from Ch. 8. This results in the following coordinates for the legs:
prl1,org = rl0prl1,org prl3,org = prl2,org +
R R rl2prl3,org 0 rl1 rl2 rl3 prl4,org = prl3,org + rl prl4,org rl1R rl2R rl3R rl0 rl1 rl2 rl3 prl5,org = prl4,org + rl1R rl2R rl3R rl4R rl4prl5,org 0 rl1 rl2 rl3 rl4 rl5 0 rl5 pll1,org = prl5,org + rl pll0,org = rl pll0,org rl1R rl2R rl3R rl4R rl5R rl5R rl0 rl5 ll0 ll1 pll2,org = pll1,org + rl5R ll0R ll1R pll2,org 0 rl5 ll0 ll1 ll2 pll3,org = pll2,org + rl pll3,org rl5R ll0R ll1R ll2R rl0 rl5 ll0 ll1 ll2 pll4,org = pll3,org + rl5R ll0R ll1R ll2R ll3R ll3pll4,org 0 rl5 ll0 ll1 ll2 ll3 ll4 pll5,org = pll4,org + rl pll5,org rl5R ll0R ll1R ll2R ll3R ll4R
rl0 rl1 rl1 rl2
0 prl2,org = prl1,org + rl rl1R
rl1
prl2,org
The subscript prl1,org indicates that p is the point located in the joint of frame {RL1}. In this chapter, when no superscript is written to indicate the frame in which a point is given, {RL0} is implied, e.g. prl1 = rl0prl1 . It is assumed that the three outer servomotors in the arms are inactive. Then, only three points in each arm are of interest since they are sucient to describe the torques the arms add to the biped. These points are the rst two joints in the shoulders and the mass center for the entire arm. These points are found from frame {RL5} using the rotation matrices from the kinematics section.
0 rl5 pra1,org = prl5,org + rl rl5R ra0R
ra0 ra0 ra1
pra2,org = pra1,org + pCoM,ra
XCoM,ra 0 rl5 ra0 ra1 YCoM,ra = pra2,org + rl rl5R ra0R ra1R ra2R ZCoM,ra
la0 la0 la1 rl0 rl5
rl0 rl5
rl5 ra0
R ra1pra2,org
pra1,org
0 rl5 pla1,org = prl5,org + rl rl5R la0R
pla2,org = pla1,org + pCoM,la
XCoM,la 0 rl5 la0 la1 YCoM,la = pla2,org + rl rl5R la0R la1R la2R ZCoM,la
R R R la1pla2,org
rl5 la0
pla1,org
From these coordinates it is possible to nd the mass centers of the relevant links. To make the calculations easier the following assumptions are made for the locations of the mass centers.
10.2. SIMPLIFICATION OF THE KINEMATICS
69
CoMrl0 prl1,org CoMrl1 prl2,org CoMrl2 prl3,org CoMrl4 prl4,org
CoMll1 pll2,org CoMll3 pll3,org CoMll4 pll4,org CoMll5 pll5,org
CoMt = pCoM,t CoMra = pCoM,ra CoMla = pCoM,la
In Fig. 10.1 the placements of the actual mass centers are plotted along with the joint centers which are the approximated mass centers in the Lagrange model. This approximation introduce a maximum displacement of 8 mm in the three directions from the actual mass centers. This is considered acceptable as the moment of inertia from each link is changed very little as the lever arm always is measured from the angle of the supporting foot, which means that the displacement introduced by the approximations is less than 10% when a link is more than 8 cm away from the angle.
[m]
[m]
[m]
[m]
Figure 10.1: This gure illustrates the size of the displacement introduced by the
approximation of the mass centers laying in the joint center. The crosses are the actual mass centers and the dots are the approximated mass centers used by the lagrangian model.
The mass centers CoMrl3 and CoMll2 are neglected due to their relative small weight which is about 1/4 of the other links because no servomotor is attached to these links. With these approximations only the pCoM,t is not yet determined and is given by:
pCoM,t
XCoM,t 0 YCoM,t = prl5,org + rl rl5R ZCoM,t
These points contain all the information needed to describe the position of the joints for every possible angle. This information is not needed to describe a gait and hence simplications can be made. To do so, the expressions are written out in their full length and simplied in App. G.1. The simplications have mainly been carried out using the fact that each servomotor angle varies only within limits smaller than 26
70

when walking. These variations have been determined in App. A. This allows for small angle approximations yielding an error of 10% or less. Furthermore, some trigonometric relations have been applied and some physical considerations have been made to bring the expression down to an acceptable size. Finally, for brevity, the following sums have been used.
rl = rl2 + rl3 + rl4 ll1+ = rl2 + rl3 + rl4 + ll2 ll2+ = rl2 + rl3 + rl4 + ll2 + ll3 ll3+ = rl2 + rl3 + rl4 + ll2 + ll3 + ll4 ll1- = rl2 + rl3 + rl4 ll2 ll2- = rl2 + rl3 + rl4 ll2 ll3 ll3- = rl2 + rl3 + rl4 ll2 ll3 ll4
hip = rl5 + ll1 lls2 = ll2 + ll3 + ll4 lls1 = ll2 + ll3 ra+ = rl2 + rl3 + rl4 + ra1 ra- = rl2 + rl3 + rl4 ra1 la+ = rl2 + rl3 + rl4 + la1 la- = rl2 + rl3 + rl4 la1
In the derivation it is not assumed that rl = 0 because this is not always the case for human gait, but as earlier mentioned a constraint on the gait of the biped robot. The '+' or '-' in the dierent subscripts indicate whether the angles are added or subtracted from the angles in the supporting leg. The simplications have resulted in the following coordinates for the legs. 0 0 prl1,org = arl0 0 pll1,org = prl5,org + ahip 1 0 rl rl5 1 1 prl2,org = prl1,org + arl1 rl1 pll2,org = pll1,org + all4 rl1 rl5 ll1 rl 0 1 cos(ll1- ) prl3,org = prl2,org + arl2 rl1 pll3,org = pll2,org + all3 rl5 ll1 sin(ll1 rl2 ) 1 cos(ll2- ) pll4 = pll3,org + all2 ll1 rl1 prl4,org = prl3,org + arl3 ( + ) sin( rl2 rl3 ll2- ) 1 cos(ll3- ) prl5,org = prl4,org + arl4 rl1 pll5,org = pll4,org + all1 ll1 rl sin(ll3- ) For the mass center of the torso the coordinates are
pCoM,t
XCoM,t = prl5,org + YCoM,t rl XCoM,t
For the points in the arm
10.3. ENERGY CONSIDERATIONS
71
pra1,org
pra2,org
pCoM,ra
pla1,org
pla2,org
pCoM,la
50.5 = prl5,org + 49.1 50.5rl 18.7 cos(ra+ ) 0 = pra1,org + 18.7 sin(ra+ ) cos(ra+ )YCoM,ra XCoM,ra = pra2,org + sin(ra+ )YCoM,ra 50.5 = prl5,org + 110.7 50.5rl 18.7 cos(la- ) 0 = pla1,org + 18.7 sin(la- ) cos(la- )YCoM,la XCoM,la = pla2,org + sin(la- )YCoM,la
Now the coordinates for the points of interest have been found, and the redundant information has been discarded. The points are ready to be used to determine the energy in the biped robot.
10.3 Energy Considerations

In order to determine the Lagrangian both the potential and kinetic energy must be known, as stated earlier. The kinetic energy is found using Eq. (10.5)
ki =
1 1 i i p i + I i mi p 2 2 i
ki,l ki,r
(10.5)
The rst part represents the linear part of the kinetic energy, denoted ki,l , and the second part represent the rotational kinetic energy, denoted ki,r [Craig, 2003]. The reason ki,r becomes a scalar is that each joint only rotate around the z -axis which gives
i = 0 0
z,i
i , must be deTo be able to calculate Eq. (10.5) the velocities of each mass center, p termined. This is done in App. G.2. Furthermore, the moments of inertia are found in App. G.3. The complete expressions for the moments of inertia does, like the points for the joints, contain unnecessary information for describing the inertia in a gait cycle and hence they have been simplied in the same way. Additionally, some of the elements in the inertia tensor for the dierent parts of the biped robot contributes with a relative small amount of inertia, and are removed from the expressions.
72

The potential energy of the approximated mass centers is determined using Eq. (10.6). This has been done in App. G.2 using the simplied coordinates to determine the height x. (10.6)
pi = mi gxi
The energies of the system have now been determined enabling the Lagrangian to be calculated, and hence the torques in joints RL1 and RL2 can be found.
10.4 Computation of the Lagrangian and the Ankle Torques

The Lagrangian is given by:
L=
ki
pi
The torque for each link are determined using Eq. (10.7) in two steps: d L L dt
step 1 step 2
(10.7)
The torque determined in Eq. (10.7) is the actuator torque. It has the size of the torque the biped robot applies to the servomotors along the z -axis due to changes in the energies in the robot, with opposite sign. The control of the biped robot is done by adjusting the servomotor angles to obtain a certain output from the strain gauges on the supporting foot which are mapped to a certain torque in the origin of {RL1} and {RL2}. Hence the only two torques of interest are the torque in joint RL1 to determine how the load is distributed on the foot in the frontal plane, and the torque in joint RL2 to determine how the load is distributed in the foot in the sagittal plane. These two torques are the most comprehensive to determine since they have to take the whole robot into account. Consequently, the calculations is divided into three steps, as seen in Eq. (10.8). As step one from Eq. (10.7) consist of both a linear and a rotational part the step is easily divided into two subparts as dierentiation is a linear operation. d Lr L d Ll + rl1 dt rl1 rl1 dt
step 1.1 step 1.2 step 2
rl1 =
(10.8)
rl2
d Ll d Lr L = + dt rl1 dt rl1 rl1
(10.9)
The torques rl1 and rl2 are scalars as they only represent the torque around the z -axis in their respective coordinate system. This leads to the following derivation of the torques caused by the energy changes.
10.4. THE LAGRANGIAN AND THE ANKLE TORQUES
73
Torque in RL1
The expressions for the terms in Eq. (10.8) are derived in App. G.4 and for brevity only the nal equations are presented here.
d Ll d 2 2 = mrl1 a2 rl1 rl1 + mrl2 (arl1 + arl2 ) rl1 + mrl4 (arl1 + arl2 + arl3 ) rl1 rl1 dt dt
2 2 2 2 +mbody (a2 rl sin(rl1 ) + arl )rl1 + mll1 arl sin(rl1 ) rl1
rl1 all1 ( rl5 + ll1 ))(arl1 + arl2 + arl3 ) +((arl1 + arl2 + arl3 ) rl1 + all2 sin(ll1- ) ll1- )(arl sin(rl1 )) +mll3 (arl sin(rl1 ) rl1 (all1 + all2 )( rl5 + ll1 ))(arl1 + arl2 + arl3 ) +((arl1 + arl2 + arl3 ) rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )(arl sin(rl1 )) +mll4 (arl sin(rl1 ) rl1 (all1 + all2 )( rl5 + ll1 ) all3 ll1 )(arl1 + arl2 + arl3 ) +((arl1 + arl2 + arl3 ) +mll5 rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2arl sin(rl1 )
ll3- (arl sin(rl1 )) + (arl1 + arl2 + arl3 ) rl1 +ll4 sin(ll3- ) rl5 + ll1 ) (all3 + all4 ) ll1 (arl1 + arl2 + arl3 ) (all1 all2 )( rl1 + (YCoM,ra 18.7) sin(ra+ ) ra+ )(arl sin(rl1 )) + a2 +marm (arl sin(rl1 ) rl rl1 rl1 (YCoM,la + 18.7) sin(la- ) la- )(arl sin(rl1 )) + a2 +marm (arl sin(rl1 ) rl rl1
(10.10)
and
d Lr d = I + Iyy,rl2 + mrl2 (arl1 + arl2 )2 + Iyy,rl4 + mrl4 (arl1 + arl2 + arl3 )2 rl1 dt rl1 zz,rl1 dt
2 2 2 +2 rl Ixx,t + Izz,t + mt (arl + Xt ) + Izz,ll1 + mll1 (arl1 + arl2 + arl3 ) + ahip 2 2 +Ixx,ll3 + mll3 (arl1 + arl2 )2 + a2 hip + sin (ll3- )Ixx,ll4 + cos (ll3- )Iyy,ll4 2 2 2 2 +mll4 a2 rl1 + ahip + sin (ll3- )Ixx,ll5 + cos (ll3- )Izz,ll5 + mll5 ahip
+marm ((arl + 50.5 + 18.7 cos(ra+ ) YCoM,ra cos(ra+ ))2 + (49.1 + XCoM,ra )2 ) + sin2 (ra+ )Iyy,ra + cos2 (ra+ )Izz,ra + sin2 (la- )Iyy,la + cos2 (la- )Izz,ra +marm ((arl + 50.5 + 18.7 cos(la- ) + YCoM,la cos(la- ))2 + (110.7 XCoM,la )2 ) (10.11)
These two expressions are not dierentiated with respect to time in the report to keep them as simple as possible. The dierentiation is only carried out in Matlab , which is a rather simple task. The nal of the three expressions in Eq. (10.8) is given by.
74
L 2 =mll1 a2 rl sin(rl1 ) cos(rl1 )rl1 + mll3 (arl sin(rl1 )rl1 + all2 sin(ll1- )ll1- )(arl cos(rl1 )rl1 ) rl1 rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )(arl cos(rl1 ) rl1 ) + mll4 (arl sin(rl1 ) rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2+ mll5 arl sin(rl1 ) ll3- (arl cos(rl1 ) rl1 ) +all4 sin(ll3- ) 2 + mbody (a2 rl sin(rl1 ) cos(rl1 )rl1 ) rl1 + (YCoM,ra 18.7) sin(ra+ )ra+ (arl cos(rl1 ) rl1 ) + marm arl sin(rl1 ) rl1 (YCoM,la + 18.7) sin(la+ )la+ (arl cos(rl1 ) rl1 ) + marm arl sin(rl1 ) + (mll1 + mll3 + mll4 + mll5 + mt + mra + mla )garl sin(rl1 )
(10.12)
Inspecting the expressions in Eq. (10.10), Eq. (10.11), and Eq. (10.12) reveal that they seem reasonable. The subparts in Eq. (10.10) all consists of moment of inertia given by the mass of each part multiplied by its displacement, and again multiplied with an angular velocity to get the angular momentum of the whole biped robot due to linear velocity. Dierentiating this with respect to time, the torque is found. The same applies to Eq. (10.11) where the moment of inertia depends on many angles in the biped robot multiplied by the angular velocity of joint RL1. This results in an angular momentum which again is dierentiated with respect to time to get the torque. Finally Eq. (10.12) yields a torque directly as the inertia parts are multiplied with the angular velocity squared. As expected, this expression is dominated by the changes in the potential energy as long as the angular velocity squared is less than the square root of the gravitational acceleration. This means that these expressions do look reasonable, but a further examination is needed to verify the correctness of the expressions. This is done in Sec. 10.5.
Torque in RL2
The torque in the upper angle joint RL2 is given by the expression found in Eq. (10.9) where
d Ll d = (mrl2 a2 rl2 )rl2 + mrl4 (arl2 rl2 arl3 (rl2 + rl3 ))(arl2 arl3 ) dt rl2 dt
rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) (arl2 arl3 ahip rl5 ) + (sin(rl2 )2 rl2 (arl2 + arl3 )2 ) +mll1 arl2
+mll3
rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) + all2 cos(ll1- ) ll1arl2
(arl2 arl3 ahip rl5 + all2 cos(ll1- ))
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- )( sin(rl2 )(arl2 + arl3 ) + all2 sin(ll1- )) +( sin(rl2 )
+mll4
rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) arl2
ll1- + all3 cos(ll2- ) ll2- (arl2 arl3 ahip rl5 + all2 cos(ll1- ) + all3 cos(ll2- )) +all2 cos(ll1- )
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- ) +( sin(rl2 )
( sin(rl2 )(arl2 + arl3 ) + all2 sin(ll1- ) + all3 sin(ll2- )))
+mll5
ll1- + all3 cos(ll2- ) ll2- + all4 cos(ll3- ) ll3+all2 cos(ll1- )
(arl2 arl3 ahip rl5 + all2 cos(ll1- ) + all3 cos(ll2- ) + all4 cos(ll3- )) rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3- ) +( sin(rl2 )
( sin(rl2 )(arl2 + arl3 ) + all2 sin(ll1- ) + all3 sin(ll2- ) + all4 sin(ll3- )))
+mt
rl2 arl3 ( rl2 + rl3 ) (arl4 + XCoM,t )( rl ) arl2
rl2 (arl2 + arl3 )2 (arl2 arl3 arl4 XCoM,t ) + sin(rl2 )2
rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) sin(ra+ ) ra+ )( sin(rl2 )(arl2 + arl3 ) + (YCoM,ra 18.7) sin(ra+ )) +marm ( sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) + arl2
ra+ (arl2 arl3 arl4 50.5 + (YCoM,ra 18.7) cos(ra+ )) +(YCoM,ra 18.7) cos(ra+ )
rl2 (arl2 + arl3 ) + (YCoM,la 18.7) sin(la- ) la- )( sin(rl2 )(arl2 + arl3 ) + (YCoM,la 18.7) sin(la- )) +marm ( sin(rl2 )
75

(10.13)
la- (arl2 arl3 arl4 50.5 (YCoM,la + 18.7) cos(la- )) (YCoM,la + 18.7) cos(la- )
76
and
d d Lr 2 2 2 = rl2 Izz,rl2 + Izz,rl4 + mrl4 (arl2 + arl3 ) + rl5 Ixx,t + Iyy,t + mt ((arl2 + arl3 + arl4 + XCoM,t ) ) dt rl2 dt
2 2 2 +Iyy,ll1 + mll1 ((arl2 + arl3 )2 + a2 hip ) + Izz,ll3 + mll3 arl2 + ahip + Izz,ll4 + mll4 ahip
(hip cos(lls2 ) + ll5 )2 Ixx,ll5 + Iyy,ll5 + mll5 (all4 cos(ll3 ))2 + a2 hip
+Ixx,ra + 2(rl5 ra1 )Izx,ra (rl5 + ra2 )Ixy,ra + (rl5 + ra2 )2 Iyy,ra
+marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra+ ) YCoM,ra cos(ra+ ))2 + (49.1 + XCoM,ra )2 )
(10.14)
+Ixx,la + 2(rl5 la1 )Izx,la (rl5 + la2 )Ixy,la + (rl5 + la2 )2 Iyy,la
+marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la- ) + YCoM,la cos(la- ))2 + (110.7 XCoM,la )2 )
and
L 2 (arl2 + arl3 )2 ) + mll1 arl2 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) =mt ( sin(rl2 ) cos(rl2 ) rl2 rl2 rl5 + ( rl )rl5 ) all1 ( rl ) (ahip rl5 ) + ( sin(rl2 ) cos(rl2 ) 2 (arl2 + arl3 )2 ) ahip ((rl )
rl2
+ mll3
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- ( cos(rl2 ) rl2 (arl2 + arl3 ) + all2 cos(ll1- ) ll1- ) sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) ahip ((rl ) rl5 + ( rl )rl5 ) + arl2
rl ) + all2 cos(ll1- ) ll1- (ahip rl5 all2 sin(ll1- ) ll1- ) all1 ( rl2 (arl2 + arl3 ) cos(rl2 )
+ mll4
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2 sin(rl2 )
ll1- + all3 cos(ll2- ) ll2- + arl2 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) ahip ((rl ) rl5 + ( rl )rl5 ) +all2 cos(ll1- )
rl ) + all2 cos(ll1- ) ll1- + all3 cos(ll2- ) ll2- (ahip rl5 all2 sin(ll1- ) ll1- all3 sin(ll2- ) ll2- ) all1 (
+ mll5
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3 sin(rl2 )
rl2 (arl2 + arl3 ) + all2 cos(ll1- ) ll1- + all3 cos(ll2- ) ll2- + all4 cos(ll3- ) ll3- ) ( cos(rl2 )
rl ) + all2 cos(ll1- ) ll1- + all3 cos(ll2- ) ll2- + all4 cos(ll3- ) ll3all1 (
rl5 all2 sin(ll1- ) ll1- all3 sin(ll2- ) ll2- all4 sin(ll3- ) ll3- ) (ahip
rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) ra+ sin(ra+ )) + marm ( sin(rl2 )
rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) ra+ cos(ra+ )) ( cos(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) + (YCoM,ra 18.7) cos(ra+ ) ra+ ((YCoM,rarm 18.7) sin(ra+ ) ra+ ) + arl2
rl2 (arl2 + arl3 ) (YCoM,la + 18.7) la- sin(la- ))( cos(rl2 ) rl2 (arl2 + arl3 ) (YCoM,la + 18.7) la- cos(la- )) + marm ( sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) (YCoM,la + 18.7) cos(la- ) la- ((YCoM,la + 18.7) sin(la- ) la- ) + arl2
+ mt g (arl2 + arl3 ) sin(rl2 ) + mll1 g (arl2 + arl3 ) sin(rl2 )
+ mll3 g ((arl2 + arl3 ) sin(rl2 ) all2 sin(ll1- )) + mll4 g ((arl2 + arl3 ) sin(rl2 ) all2 sin(ll1- ) all3 sin(ll2- )) + mll5 g ((arl2 + arl3 ) sin(rl2 ) all2 sin(ll1- ) all3 sin(ll2- ) all4 sin(ll3- ))
77
+ marm g ((arl2 + arl3 ) sin(rl2 ) + (18.7 YCoM,ra ) sin(ra+ )) + marm g ((arl2 + arl3 ) sin(rl2 ) + (18.7 + YCoM,la ) sin(la- )) (10.15)
78

The expressions in Eq. (10.13), Eq. (10.14), and Eq.(10.15) do also look reasonable using the same argumentation as for the torques working in joint RL1.
Constraints to keep the Torso Upright

The torque, which is caused by the constraints to the angles in the supporting leg are now taken into account. In order to apply the Lagrange mechanics it is a prerequisite that the generalized coordinates are independent. The constraints to keep the torso in upright position has the results the generalized coordinates are no longer independent as the following dependencies are be introduced, rl1 + rl5 = 0 in the frontal plane and rl2 + rl3 + rl4 = 0 in the sagittal plane. To show why these constraints introduce an extra torque to be taken into account a static example is used, and the case in the frontal plane is illustrated in Fig. 10.2.
h RL5 x
0
m RL5
Epot
1
k 0
RL1
RL1
RL1
0 a.1 a.2 (a) m RL5 x RL1 RL1 RL5 m h
a.3
Epot
RL1
0 b.1 b.2 (b) b.3
Figure 10.2: The case with no constraint is depicted in (a). (a.1) shows the position
of the robot in zero position with a lever arm, x, to the mass center of the biped robot, m. (a.2) shows the case when the robot has maximum potential energy and the lever arm is zero. (a.3) illustrate the energy of the robot in the static case, and the two torques experienced in RL1 from the positions in (a.1) and (a.2). (b) shows the case when the constraint is applied. (b.1) shows the position of the robot in zero position which also is the position where the biped robot has maximum of potential energy and a lever arm x. (b.2) shows the case where the lever arm is zero and the potential energy is smaller than the one in (b.1). (b.3) illustrate the energy of the robot in the static case, and the torque experienced in RL1 from the position in (b.1).
In Fig. 10.2 (a) no constraint are applied. When the biped robot is in zero position the joint RL1 is exposed to a torque 0 due to the lever arm x and the mass m, which can be found from dierentiating according to Eq. (10.16)
0 =
Epot rl1
(10.16)
rl1 =0
which is depicted in (a.3). The case when the biped robot has rotated in RL1 such that the lever arm is zero and the maximal potential energy is reach, is seen in (a.2). In this

case the torque is zero which also comply with (a.3). Hence can the system be described only using Eq. (10.7) when the coordinates are independent. In the case in Fig. 10.2 (b) the constraint are applied. The result is that the maximum potential energy is achieved when the biped robot is in zero position, which states that the torque experienced in joint RL1 should be zero when applying Eq. (10.7), cf. (b.3). However, in (b.1) it is seen that the lever arm is dierent from zero in the zero position, and since the mass always is dierent from zero does joint RL1 experience a torque. Hence is Eq.(10.7) not enough to describe the system when a constraint is applied to the generalized coordinates. Thus is the following torque introduced. The torque due to the constraints arise from the gravitational aect on the torso, arms, and swinging leg working on joint RL1 and RL2, and is not due to energy changes. These are depicted in Fig. 10.3, and in the sagittal plane does this result in the following simplied expression for the torque.
79
sagittal =mt g (asag ZCoM,t ) + (mrl3 + mrl4 + mll1 + mll2 )gasag + (mll4 + mll5 )g (asag + sin(ll2 )all2 + sin(ll2 + ll3 )all3 ) + mll3 g (asag + sin(ll2 )all2 ) + mrl2 g sin(rl2 )arl2 + mra g (asag sin(ra1 )(YCoM,ra ara1 )) + mla g (asag + sin(la1 )(YCoM,la ala1 ))
where asag = (sin(rl2 )arl2 + sin(rl2 + rl3 )arl3 ). The forces depicted in Fig. 10.3 does result in the following simplied expression for the torque in the frontal plane. (10.17)
frontal =mt g (afro ) + YCoM,t + afro ) + mswingleg g (ahip + afro + sin(ll1 )(all1 + all2 )) + mla g (afro +rl5 py,la0,org XCoM,la + YCoM,la sin(ra2 )) + mra g (afro +rl5 py,ra0,org + XCoM,ra YCoM,ra sin(ra2 )) + msupportleg g sin(rl1 )(arl1 + sin(rl2 )arl2 ) (10.18)
where afro = sin(rl1 )(arl1 + (cos(rl2 )arl2 + cos(rl2 + rl3 )arl3 ) + arl4 ) As the torques from the constraints are relatively short expressions, small angle approximations are not used. These torques from the constraints change Eq. (10.8) and Eq. (10.9) to: d L L frontal rl1 rl1 dt
step 1 step 2
rl1 =
(10.19)
rl2
d L L = sagittal rl2 dt rl2
(10.20)
Introducing these constraints causes the gravitational eect to be taken into account twice which will give wrong results. Hence the terms originating in the gravitational acceleration are removed from the expressions in Eq.(10.12) and Eq.(10.15) [Wisniewski, 2007]. As the expressions take up much space they are not written again.
80
FT FRA FLA
FT FRA FLA
RL5
FSUPPORTLEG
RL1 FRONTAL
FSWINGLEG
FSUPPORTLEG
RL2 SAGITTAL
FSWINGLEG
Figure 10.3: This gure illustrates the torque working on the biped robot not due
to energy changes.
10.5 Verication of the Ankle Torques

To verify the expressions for rl1 and rl2 they have been entered in Matlab , and can be found on the enclosed CD [6]. By using the Newton-Euler model the path of the CoP for the trajectory is found. The purpose of this subsection is to see whether the simplied Lagrange model is capable of calculating a path of the CoP satisfactorily. Since the Newton-Euler give the CoP in the global frame the results here are also given in this coordinate system. As the CoP is always situated on the surface on which the biped robot is walking the z -coordinate is always zero and hence of no interest in this section. The CoP of the gait cycle in the y - and x-direction for the Newton-Euler model are shown in Fig. 10.4. To evaluate the correctness of the output of the Lagrangian model, the input to the servomotors in joint RL1 and RL2, rl1 and rl2 are of particular interest, and hence rl1 and rl2 . plotted in Fig. 10.5 together with their derivative, First, the expressions from step one in Eq. (10.19) and Eq. (10.20) are examined. The moment of inertia from the rotational parts are plotted separately and then the angular momenta are plotted i.e. the expressions from step 1.1 and 1.2 before they are dierentiated with respect to time. This is shown in Fig. 10.6. From the topmost graph it is seen that the moment of inertia for both joints are relatively constant, which is also expected as the changes in the posture of the robot does not yield much change in the moment of inertia during a gait cycle. The size of the inertia seen from {RL1} seems reasonable as almost the whole mass of the robot which rotates around the z -axis in joint RL1. The mass of the robot is approximately 2.2 kg and the radius to the approximated mass center of the robot is 0.22 m. Thus the moment of inertia is approximately I = m r2 2.2 kg 0.22 m2 = 0.106 kgm2 . The fact that the moment of inertia from the model is a bit smaller does also seem reasonable as e.g. the swing leg does not rotate exactly around the z -axis in joint RL1. In the same way, it seems reasonable that the inertia seen from {RL2} is approximately half the moment of inertia seen from {RL1}. This is caused by two dierences. First the length from joint RL1 to joint RL2 are approximately 4.2cm shorter which gives a 30% decrease in moment of inertia on average because the radii must be squared. Secondly the fact that in {RL2} the robot rotates around a z -axis parallel to the global y -axis further makes the radii smaller. Hence does the size of the moment of inertia seen from {RL2} seem reasonable.
10.5. VERIFICATION OF THE ANKLE TORQUES
81
CoP in the ycoordinate 0.04 0.03 meter 0.02 0.01 0
0.5
1.5
2.5
3.5
CoP in the xcoordinate 0.1 0.05 meter 0 0.05 0.1
0.5
1.5 2 Time in sec.
2.5
3.5
Figure 10.4: This gure illustrate how the CoP is moving on the surface when the dynamic trajectory for the Newton-Euler model is input to the model.
Angle position and angle velocity of RL1 0.5
0 Angular position Angular velosity 0.5 0 0.5 1 1.5 2 2.5 3 3.5
Angle position and angle velocity of RL2 2 Angular position 1 0 1 2 Angular velosity
0.5
1.5
2.5
3.5
Figure 10.5: This gure shows the trajectory of rl1 and rl2 , and their derivative.
82
Moment of inertia I seen from {RL1} and {RL2} 0.12 Moment of inertia 0.1 0.08 0.06 0.04 0 0.5 1 1.5 2 2.5 3 3.5 I from {RL1} I from {RL2}
Angular Momentum L for RL1 Angular Momentum 0.05
0 L for linear part L for rotational part 0.05 0 0.5 1 1.5 2 2.5 3 3.5
Angular Momentum L for RL2 Angular Momentum 0.05 L for linear part L for rotational part 0
0.05
0.5
1.5 2 Time in sec.
2.5
3.5
frame {RL1} and {RL2} during a gait cycle is shown. In the middle the angular momentums working on the robot are seen from {RL1}, and bottommost the angular momentums working on the robot are seen from {RL2}.
Figure 10.6: Topmost in this gure the inertia of the whole biped robot, seen from

Since the moments of inertia are almost constant the angular momenta from the rota I . Hence the tional parts of step one are easy to verify, as they are given as L = angular velocity from Fig. 10.5 is just multiplied by a constant. This is depicted in Fig. 10.6. The angular momentum from the linear part of step one is harder to verify precisely. It is expected that it will follow the dynamics of the angular velocity and stay at approximately the same size as the rotational part. This expectation arises as moment of inertia must be approximately the same for both parts, and the linear velocity is dependent of the angular velocity.
L L The last parts contributing to the torques in RL1 and RL2, ( , , frontal , and rl1 rl2 sagittal ) gives a torque directly. Hence the four torque components which constitutes the torque in each joint can now be plotted in Fig. 10.7.
83
Torques from step one and two for RL1 0.01 0 0.01 Nm 0.02 0.03 0.04 0.05 0 1 1.1 1.2 2 2 3 0 0 0.2 0.4 0.6
Torque from constrain on RL1
Torques from step one and two for RL2 0.2 1.1 0.15 1.2 Nm 0.1 0.05 0 0.05 0 1 2 Time in sec. 3 2
Torque from constrain on RL2 1 0.5 0 0.5 1 0 1 2 Time in sec. 3
Figure 10.7: Topmost is shown the four torque components working in joint RL1. Topmost the left are the torque from the linear part of step one is denoted 1.1, the torque from the rotational of step one part is denoted 1.2, and the torque from step two is denoted 2 shown. The torque from the constraint is depicted topmost to the right. Likewise for the torque components for RL2 shown bottommost.
From Fig.10.7 it is clear that the angular momenta in Fig.10.6 gives the torques denoted 1.1 and 1.2 as the torques in Fig. 10.7 are the derivatives of the angular momentum with respect to time. Hence, the dynamics and size of these seem reasonable. The part from step two, denoted 2, is very close to zero which is expected as the main components from this part have been removed when the constraints where introduced. The torque from the constraint on RL1 does seem intuitively correct as the gravitational torque on RL1 would initially be large and have a minimum halfway through the cycle and then return to the same value, as the entire robot would be moved in the negative y -direction when the right leg is the supporting leg. Brief considerations on the size of the masses of the limbs, their vertical arm to RL1, and gravitational acceleration states that the size of the torque is also correct as seen in Eq. (10.21). E.g. in the beginning and end
84

of a gait cycle can a rough calculation of the torque show that the size is correct.
g (mt larm,t + mswingleg larm,swingleg + mla larm,la mra larm,ra ) 9.82 m/s (0.9 kg 0.03 m + 0.3 kg 0.06 m + 0.3 kg 0.10 m 0.3 kg 0.05 m) 0.62 Nm (10.21)
The torque from the constraint on RL2 does also seem intuitively correct as the weight of the biped robot will begin behind the supporting foot and then move steadily forward, with approximately the same numerical value at the beginning and at the end of the gait cycle. Making the same considerations as in Eq. (10.21) in the sagittal plane shows that the size is as expected. The torque contributions from the dynamic parts are very small compared to that of the constraints. This means that the biped robot simply does not move fast enough to create large enough changes in the energies to give a signicant torque contribution. This is also the reason why the results from the model when a static gait trajectory is inputted are not presented. This would have shown approximately the same result except from the dynamic parts would have been even smaller. The time for a single step must be less than two seconds before the dynamics will have a signicant impact on the torque in the two joints. A trajectory with such a step time is not derived in this project. Hence the dynamic parts will never contribute signicantly, and thus are they are neglected in the further calculations, as this would make the calculations needed to determine the CoP much smaller. Consequently, only the torques from the constraints are used in Eq.(9.16), which enables calculation of the CoP from the joint torques. As Fx is not an output from the Lagrange model is it approximated by the constant gravitational force generated by the mass of the entire robot except the supporting foot. To make it easier to compare the veried CoP trajectory from the Newton-Euler model with those found here, they are plotted together in Fig. 10.8. From this graph it is concluded that the CoP trajectories follows each other satisfactory, as they follow each other with a maximum of 1.2 mm deviation. This degree of precision from the stationary parts justies the exclusion of the dynamic parts, and veries the expressions from the constraints.
2
Chapter Summary
When modelling a biped robot of this size, weight and speed, the dynamic part of the model can be disregarded. This is due to the fact that the accelerations of the dierent limbs are insignicant compared to the size of the gravitational acceleration g , and the length of the lever arms for these accelerations can not make up for this dierence. The inaccuracy, which was expected from the fact that the input angles to the biped robot is dierent from those seen in the human gait in App. A, cancels out when the dynamic parts were neglected in the expression. Hence there is no precise measure of how large this error is, but it can be concluded that it is insignicant for this biped robot, as the achieved expression has a precision of at least 1.2 mm. However, the graphs in Fig. 10.6 and Fig. 10.7 does, to some degree, justify the simplications despite the dierence in input angles compared to those the model where derived from.
85
CoP comparison in the ycoordinate 0.04 CoP from Lagrangian 0.03 meter 0.02 0.01 0 CoP from Newtonian
0.5
1.5
2.5
3.5
CoP comparison in xcoordinate 0.1 CoP from Lagrangian 0.05 meter 0 0.05 0.1 CoP from Newtonian
0.5
1.5 2 Time in sec.
2.5
3.5
Figure 10.8: Topmost is the CoP trajectory in the y direction for both the Newtonian and Lagrangian model shown. Bottommost is the CoP trajectory in the x direction for both model plotted.
To achieve a higher degree of precision for the dynamic parts the simplications must be made from the actual input angles from a trajectory. Such an approach should be suited for models of larger/faster biped robots where the dynamic parts can not be neglected. Now the model based on Lagrangian mechanics has been derived and veried with satisfying results, and is hence ready to be used for controlling the biped robot.
Part Summary
In this part the models needed to describe the biped robot's kinematic and dynamics have been derived. The servomotors is modelled as rst order systems which is sucient to get a satisfactory kinematic model. The kinematic model does, however, not take the changing load of each servomotor into account. This makes the servomotor model inadequate in the complete model and hence the model is not included in the following. The kinematic model developed in this report, thereby also dening the frames used to describe the robot. The result of the model are numerous transformations matrices, which describes the relation of the frames as a function of the joint angles. The model is veried thoroughly using the 3D CAD program, 3ds Max. Using the kinematics, a model is also developed to determine the CoP of the robot during static gait. A complete dynamic model of the biped robot is derived using the Newton-Euler iterations. This model is not simplied in any way and hence calculates angular velocity/acceleration, linear acceleration, force, and torque precisely for each link in the biped robot. This result makes it very suitable for o-line simulations used in trajectory generation and controller tuning. The verication of the model is carried out comparing the measured output from the accelerometers on the biped robot with the predicted output from the model. In addition is the output from the strain gauges compared with the CoP from the model, using the models derived in Ch.9.2. Both comparisons showed that the model output is coinciding with the measured output. The Lagrangian based dynamic model is derived in order to obtain a model for on-line calculations of the CoP. This is achieved by simplifying the expressions for the torques in joint RL1 and RL2. It is shown that the dynamic expressions had relatively small contributions to the torque and hence is eliminated in order to make the calculations less comprehensive. This simplication is valid for the walking speeds used in this project, which is about 2.8 sec per step. The nal expressions for the two torques is brought down to a few lines of calculations, and when veried against the Newton-Euler based model the dierence stayed within 1.2 mm. This makes the model precise enough for model based control, but this will not be implemented due to time constraints. An expression to determine the CoP from the torques working on the joint RL1 and RL2 is derived and used in both dynamic models. This expression is rst veried from a theoretical experiment. Now the models needed to describe the gait of the biped robot have been derived, and thus enabled trajectory generation and controller design. This will be carried out in the next part.
86
Part IV
Control
This part begins with a discussion of some dierent approaches to implementing a controller. When a control topology has been decided upon the elements of that topology is described. The rst of these elements is an inverse kinematic model for the robot. This element is used to calculate the angles of each joint in the robot, given a set of points which describes the position of the limbs. The techniques used in the feed-forward part of the controller topology is described, namely the trajectory generation. This is the element in the controller that generates the point set needed by the inverse kinematics for all points in time. In the next chapter the development of an algorithm which maps the strain gauge measurements to the CoP position of the robot is found. The nal part of the implemented control topology, the feedback control, is also described in this part. The feedback uses the measured CoP position to stabilize the robot by correcting CoP with respect to a CoP trajectory generated o-line. At last a suggestion to an implementation of a supervisor in the controller is given. This supervisor should be able to correct for larger disturbances by choosing between several o-line generated trajectories to select the most stable trajectory in the situation.
Chapter
11
Introductory Analysis
The goal in this part of the project is to develop a controller that will enable pseudodynamic walk of the biped robot. As there are several approaches to this kind of control a series of solutions will be presented. Depending on the strengths and shortcomings of these solutions the control method used for obtaining pseudo-dynamic walk will be determined. The rst controller solution is used for xed trajectory based static walk. In this controller, the trajectories for the torso and the extremities will be generated o-line, and the controller will focus on tracking these trajectories. A block diagram describing this topology is shown in Fig. 11.1. The trajectories are generated using the models of the robot with the criteria of continuous postural stability throughout the gait cycle. Such trajectories would make the biped walk statically.
Controller Trajectories Inverse kinematics Angle PI
Robot Rotary encoders Servomotors
Figure 11.1: Simple feed-forward control from the trajectory with an angle feedback
control strategy.
In Fig. 11.1 a PI controller is also included. This controller uses the angles outputted by the rotary encoders to correct the servomotor positions. This idea is inspired by the previous project on this robot, as that project group described problems with the tracking capability of the internal controllers in the servomotors. This problem was addressed in Sec. 5.1.2 on page 23 without the use of outer-loop PI controllers. Therefore, the PI controller is not used in this project. 88
89
Ideally, the robot can be made to walk dynamically using the same control strategy by only altering the trajectories. Again o-line generated trajectories are used. These are generated based on new dynamic walk criteria as dened in Sec. 2.2. Both these controllers will work very well under ideal conditions. However, in a realworld situation, perturbations will occur due to model inaccuracies and/or external physical disturbances. Therefore, it will be preferable if the robot was able to deal with these perturbations before they cause irrecoverable consequences. E.g. if the robot is pushed it might take a step back or stop for a while, but it should not fall to the ground. To create such behaviour of the controller, a more complex control strategy must be employed compared to the relatively simple feed-forward strategy mentioned above. A solution to this problem is to add on-line feedback control to maintain balance/stability. To do this the controller needs some input that will provide an indication of the stability of the robot. A quantity that possess these qualities is the zero momentum point (ZMP), which has seen extensive use in exactly this application in [Huang et al., 2001] [Kudoh and Komura, 2003] [Komura et al., 2005] [Sardin and Bessonnet, 2004] [Katic and Vukobratovic, 2003]. The ZMP can be determined by the force sensors (strain gauges) in the feet of the robot [Kim et al., 2005] [Sardin and Bessonnet, 2004], and is thus available as an input to the controller. The position of the ZMP is directly dependent on the posture of the robot, i.e. the joint angles. The controller will thus need a mapping from the angle of the joints to the position of the ZMP. For the controller to have a reference, a desired ZMP trajectory must also be available. This desired ZMP can be generated o-line along with the gait trajectories. A block diagram of this control strategy is shown in Fig. 11.2.
Controller Force to ZMP mapping ZMP trajectory ZMP error to joint angle mapping
Robot Strain gauges Servomotors
Figure 11.2: A ZMP control stratergy.
There exists a problem with one element of the ZMP controller proposed above, which concerns the mapping from the ZMP to the joint angles. As there is only one ZMP and 20 joint angles, there is many dierent ways to map this relationship. To nd the "right" angles more information than just the desired ZMP is needed. This information should include the desired position of the limbs and the range of motion of the angles. The mapping could be simplied further by disregarding some of the joints, and only control the ZMP using a few critical joints. The mapping might also be dierent for dierent states in the gait cycle. By integrating the ZMP controller into the previously mentioned feed-forward controllers, these controllers will be signicantly better equipped to handle perturbations. This scheme is shown in Fig. 11.3.
90
CHAPTER 11. INTRODUCTORY ANALYSIS
Controller Force to ZMP mapping ZMP error to joint angle mapping Inverse kinematics
ZMP trajectory
Trajectories
Figure 11.3: Feed-forward trajectory based control and zmp control combined.
The trajectory of the ZMP should correspond to the other trajectories such that the ZMP controller does not modify the reference angles during unperturbed motion. Only when the robot experiences perturbations should the ZMP controller intervene. While the ZMP controller is able to handle smaller perturbations and could help correct some inaccuracies in the trajectories, in case of larger perturbations, the feed-forward controller would ght the ZMP controller, and the robot might lose balance despite the eorts of the ZMP controller to maintain balance. To deal with this situation, a supervisory mechanism is needed to change the trajectories to some that are better suited for the situation. Evidence from biomechanical studies also indicate that humans have such a mechanism; a number of dierent strategies prepared in advance, and the most appropriate response motion is launched when the perturbation occurs [Komura et al., 2005]. Not only would this improve robustness towards larger perturbations, it would also increase anthropomorphism. This supervisor would need some input related to the perturbation as a basis for its decisions. Here the outputs from the accelerometers could be used, along with the ZMP position error. Fig. 11.4 illustrates such a supervisor incorporated into the previously mentioned feed-forward/ZMP control scheme. The controller designed in this report will be limited to the topology in Fig. 11.3, i.e. no trajectory supervisor is implemented although a description of the ideas in the supervisor will be included. In the following chapters the blocks in Fig. 11.4 will be described in more detail.
91
Controller Trajectory supervisor Force to ZMP mapping ZMP trajectory ZMP error to joint angle mapping
Trajectories Inverse kinematics
ZMP error
Accelerometers
Figure 11.4: Feed-forward and ZMP control with trajectory supervisor.
Chapter
12
Inverse Kinematics
As described in the previous chapter a mapping is needed from a position of an extremity to the angles of the joints in that extremity. This mapping is the inverse of the operation done by the forward kinematic model. The inverse kinematics block is one of the blocks in the constructed controller, see Fig. 12.1.
Controller Accelerometers Trajectory supervisor Force to ZMP mapping ZMP trajectory ZMP error to joint angle mapping Trajectories Inverse kinematics
Strain gauges
ZMP error
Servomotors
Figure 12.1: Figure illustrating the block designed in this chapter and it's context
in the controller.
The forward kinematic model can be expressed as:
P = f(x,y,z) ( )
(12.1)
where f is a function applying the forward kinematic model to the joint angles and P R320 is the position of the links. When the inverse kinematic model has to be determined the inverse of the function f(x,y,z) ( ) has to be calculated. The inverse function can be written as
= f(x,y,z) 1 (T )
(12.2)
here T is a subset of the position vectors p R3 in P . This function calculates based on the desired position of some key joints T . Calculating the inverse kinematic model can at rst sight seem straight forward, but as both none and multiple solutions can appear the calculation can become rather complex. 92
12.1. CALCULATING THE INVERSE KINEMATIC MODEL

For a solution to exist the goal point must lie within the manipulator's workspace, where the workspace is the volume of space that the end-eector of the manipulator can reach. The other problem that can arise is the existence of multiple solutions. An example is a planar arm with three revolute joints which has a large workspace in the plane.This example is illustrated in Fig. 12.2 where six solutions are shown in one plane. The number of solutions increases rapidly if the arm is allowed to move in all three dimensions.
93
Figure 12.2: Six solutions to the inverse kinematic model for a three joint planer
arm.
Dening constraints to the joints can reduce the number of solutions to the inverse kinematic equations. An example could be that the knee joint is only allowed to bend backward in the sagittal plane.
12.1 Calculating the Inverse Kinematic Model

Two kinds of strategies can be used to nd solutions for the inverse kinematic equations, closed-form solutions and numerical solutions. But as the numerical solution is much slower than the closed-form solution, the numerical solution is seldom used to calculate inverse kinematic equations. Closed-form means a solution based on analytical expressions or on the solution of a polynomial [Craig, 2003]. Within the closed-form solutions there are two methods to obtain a solution, algebraic and geometric. These two methods are almost identical and dier only in the approach. The inverse kinematic model in this report is based on the geometric approach as all the link twists (i ) are equal to either 0 or 90 . This means that the spatial geometry problem of the biped robot can be decomposed to several plane geometry problems. The joint angles can then be solved by using simple trigonometric calculations [Craig, 2003]. The geometric approach is also chosen in [rts et al., 2006] to develop the inverse kinematic model. However the inverse kinematic model in [rts et al., 2006] could not be veried in this project. It was found that the model only takes two dimensions into account in some equations that require considerations in all three dimensions. Because of these errors the inverse kinematic model is developed from the beginning in this project using the geometric approach but taking all three dimensions into account.
12.1.1 Constraints
In this section the constraints used to calculate the inverse kinematic model are discussed. These constraints include:
The foot soles are always parallel to the horizontal plane (Only pseudo-dynamic
94
walk is possible).
CHAPTER 12. INVERSE KINEMATICS
The outer joints in the arms are not expected to supply much dynamics to the robot. Hence, only two joints in each arm are used to move the arms, namely the roll and pitch of the shoulder joints. The angle of the torso is zero in the frontal plane meaning that the two hip frames {RL5} and {LL1} have their origin at equal height above ground level. The angle of the torso in the median plane is constant.
Together with the angle ranges dened in App. I these constraints simplify the inverse kinematic modelling.
12.1.2 Prerequisites
Some information is needed to calculate the joint angles, i.e. T in Eq. (12.2). The position of the torso is chosen along with the position of the last link in each chain. T is thus dened as:
T = prl1
prl5
pll5
pra5
pla5
(12.3)
Here, and in the rest of this chapter, pi is given in the global reference frame, i.e. pi = globpi . This frame is obtained by rotating RL0 using: 0 0 1 glob 0 1 0 (12.4) rl0 R = 1 0 0 Thus, the global reference frame has axes parallel to those in gure 2.1 on page 6.
12.1.3 Inverse Kinematic Model of the Legs

In this section the inverse kinematic model for the right leg is developed. The model can also be used for the left leg by changing the indexes. The modelling is spilt into two parts, rst the frontal plane is investigated and next the sagittal plane is investigated. The horizontal plane is not a concern as no joint angles are changing in this plane.
Frontal Plane
The roll angles (rl1 and rl5 ) in the frontal plane are found from Fig. 12.3.
pRL5
RL5
RL1
z y
pRL1 Foot
Figure 12.3: Illustration of the right leg in the frontal plane used to calculate rl1
and rl5 .
rl5 can be calculated using tangent for rectangular triangles as both the coordinates for prl1 and prl5 are known. The coordinates attached to point prl1 are denoted
12.1. CALCULATING THE INVERSE KINEMATIC MODEL

xrl1 , yrl1 , zrl1 and the coordinates attached to point prl5 are denoted xrl5 , yrl5 , zrl5 . The calculation of rl5 can be seen in Eq. (12.5). rl5 = tan1 yrl5 yrl1 zrl5 zrl1
(12.5)
95
Due to the constraint that the foot soles are always parallel to the horizontal plane rl1 can be found from rl5 . See Eq. (12.6).
rl5 = rl1
(12.6)
Sagittal Plane
The pitch angles of the right leg (rl2 , rl3 , and rl4 ) in the sagittal plane are calculated based on Fig. 12.4.
pRL5 aRL4
RL4
L3
pRL4
L 2-4
RL2
aR
RL3
pRL2 aRL1
aRL2
pRL3 z x
pRL1
Figure 12.4: Illustration of the right leg in the sagittal plane used to calculate all
the pitch angles.
The joint angle of the knee can be found as the diagonal from prl2 to prl4 can be calculated. This is done with reference to Fig. 12.5.
pRL5 aRL4
L 2-4
aR
pRL2 aRL1
a RL2
pRL3 z x
L3
pRL4
RL3
pRL1
Figure 12.5: Illustration of the right leg in the sagittal plane used to calculate the
pitch angle of the knee.
96

The diagonal (L24 ) can be calculated as in Eq. (12.7).
L2 4 =
where:
2 + 2 + 2
(12.7)
= xrl5 xrl1 = yrl5 arl4 sin rl5 yrl1 arl1 sin rl1 = zrl5 arl4 cos rl5 zrl1 arl1 cos rl1
The pitch angle of the knee can now be calculated using the cosine rule for arbitrary triangles, see Eq. (12.8).
rl3 = 180 cos1
arl2 2 + arl3 2 L24 2 2 arl2 arl3
(12.8)
The cos1 have two solutions, but since rl3 must be in the range of 0 to 90 only one solution of cos1 can be used. See App. I for the limitations of the dierent angles. The pitch angle of the hip can be calculated using Fig. 12.6 and Eq. (12.9).
pRL5
RL4
pRL2
temp1
L 2-4 aRL2 pRL3
aR
temp2
L3
pRL4
z x
pRL1
Figure 12.6: Illustration of the right leg in the sagittal plane used to calculate the
pitch angle of the hip.
rl4 = 90 cos1
xrl5 xrl1 L2 4
temp1
cos1
2 2 a2 rl3 + L24 arl2 2 L24 arl3 temp2
(12.9)
The cos1 has two solutions, so the right solution must be chosen. The positive solution is chosen for For temp1 as the angle limitations for angle for RL3 and RL4 . For temp2 is the positive solution chosen. Due to the limitations in the angle RL4 , see App. I for the limitations of the dierent angles. At last the pitch angle of the ankle can be found. Due to the constraint that the feet always are horizontal rl2 can be calculated as in Eq. (12.10).
rl2 = rl3 rl4
(12.10)
12.1.4 Inverse Kinematic Model of the Arms

In this section the inverse kinematic model for the right arm is developed. The model can also be used for the left arm by changing the indexes. The modelling is spilt into two
12.2. VERIFICATION OF THE INVERSE KINEMATIC MODEL

parts, rst the sagittal plane is investigated and then the frontal plane is investigated. The horizontal plane is not concern as no joint angles are changing in this plane. As already mentioned, only the shoulder joints are used to model the inverse kinematics as these joints are sucient to maintain the balance of the biped robot.
97
Sagittal Plane
The pitch angle of the right arm (ra1 ) in the sagittal plane is found from Fig. 12.7.
pRA1 z pRA5
RA1
Figure 12.7: Illustration of the right arm in the sagittal plane used to calculate the
pitch angle of the shoulder.
ra1 can be calculated using tangent for rectangular triangles as both the coordinates for pra1 and pra5 are known, see Eq. (12.11). ra1 = tan1 xra1 xra5 zra1 zra5
(12.11)
Frontal Plane
The roll angle of the right arm (ra2 ) in the frontal plane is found from Fig. 12.8
pRA1 z
RA2
pRA5
Figure 12.8: Illustration of the right arm in the frontal plane used to calculate the
roll angle of the shoulder.
ra1 can be calculated using tangent for rectangular triangles as both the coordinates for pra2 and pra5 are known and adding a oset value, see Eq. (12.12). ra2 = tan1 yra1 yra5 zra1 zra5 cos ra1 + ara1 + ra2,oset
(12.12)
In the following section, the developed inverse kinematic model will be veried to ensure that it describes the inverse kinematics of the biped robot accurately.
12.2 Verication of the Inverse Kinematic Model

The inverse kinematic model has been implemented in a Matlab -script which can be found on the appended CD [7]. To verify the inverse kinematic model a simple model of the robot has been made. This model only consists of lines; the lines indicate links
98

and the text of the joints are written, to indicate where the joints are. A gure of this simplied robot is depicted in Fig. 12.9.
RA2 LA2
RA1
LA1
RA3 RA5
RA4 RL5 LL1
LA4
LA3 LA5
RL4
LL2
RL3
LL3
RL2
LL4
RL1
LL5
Figure 12.9: Illustration of the simplied robot used to verify the inverse kinematic
model.
This simplied model is also drawn in the 3D modelling environment Rhinoceros 4.0. A screen shot of this environment is found in Fig. 12.10. The same coordinates are given to the Matlab -script and the 3D environment. The dierent angles are measured in the 3D environment and compared to the output from the inverse kinematic model. To make it possible to transport the coordinates from Rhinoceros to Matlab a plugin for Rhinoceros is written. This plug-in can be found on the CD [8]. When the coordinates are imported into Matlab , the function inverse_kinematic.m is run and the angle of each joint is outputted. This output is compared with the angles measured in Rhinoceros. This procedure is done for ten dierent positions of the legs and arms to ensure that the inverse kinematic model is veried properly. The procedure starts by changing one angle, while keeping the rest of the angles in zero position. This angle is measured in Rhinoceros and the coordinates are exported to Matlab where the inverse kinematic script is run. The output angles of the inverse kinematics must be equal to the angles measured in Rhinoceros. Afterwards another angle is changed till all the angles in the right leg are dierent from the zero position. This is illustrated in Fig. 12.11 where the angles rl2 , rl3 and rl4 are changed and measured. This procedure is done for all the angles. The measured angles in Rhinoceros and the calculated angles in Matlab match, and the inverse kinematic model is thus veried.
Chapter Summary
In this chapter an inverse kinematic model has been derived. The inverse kinematic model is capable of calculating all the joint angles to any reachable position of any link. The model is derived using a geometric approach which means that the spatial geometry
12.2. VERIFICATION OF THE INVERSE KINEMATIC MODEL
99
matic model.
Figure 12.10: Screen shot of the 3D environment used to verify the inverse kine-
Figure 12.11: Illustration of the right leg on the stick robot with measured angles.
problem of the biped robot can be decomposed to several plane geometry problems. The inverse kinematic model is veried satisfactorily using Matlab and the 3D modelling program Rhinoceros. With the use of the inverse kinematic model, trajectories can be derived which is done in the next chapter.
Chapter
13
Controller
Trajectories
This chapter will document the procedure used for generating the trajectories which the robot should follow when walking. These are used in the trajectories block in the controller found in Ch. 11 and depicted in Fig. 13.1.
Accelerometers
Trajectory supervisor Force to ZMP mapping ZMP trajectory ZMP error to joint angle mapping
Trajectories Inverse kinematics
Strain gauges
ZMP error
Servomotors
Figure 13.1: An illustration of the blocks designed in this chapter and its context
in the controller.
The procedure described here generates a trajectory set for one entire SSP. This is the part of the walk covered by the models, and since these are needed to calculate ZMP, the trajectories cannot be generated for a longer period. One SSP is however also sucient. As the DSP only lasts a small fraction of the cycle duration [rts et al., 2006], this phase is not considered. As a result, the SSP for one leg is directly followed by the SSP of the other leg. In practice, using these trajectories, there will be a DSP as slack and torsion in the joints prevent the foot from lifting at exactly the moment planned. To get a trajectory set for the other SSP phase, the generated trajectories are mirrored in the x/z -plane. Trajectories will be given for ve key points on the robot. These points are:
The right foot (origin of RL1 frame: The right hip (origin of RL5 frame: The left foot (origin of LL5 frame:
globp globp
rl1,org
rl5,org
globp
ll5,org
100
13.1. TRAJECTORY CRITERIAS

The right arm (origin of RA5 frame: The left arm (origin of LA5 frame:
globp
101
)
ra5,org
globp
la5,org
For each of these points in space a number (n ) of equally spaced points in time will be given. Thus each point in a trajectory is four dimensional vector containing [x, y, z, time]. A set of trajectories for all these points in space and time is denoted Rn 54 . All the trajectories will be given in the same frame, namely the global reference frame.
13.1 Trajectory Criterias

Before any trajectories can be found, a set of criteria must be established which the trajectories should fulll. This section will dene these criteria along with a performance function that describes how much a given trajectory set respects the criteria.
Realizability
In the huge solution space of this trajectory problem, many solutions are not realizable by the robot simply because the trajectory lies outside the workspace of the robot. For example, a trajectory point for the right hip may not be further from the right foot than the length of the extended leg. Let the function
invKin R53 : C20
(13.1)
map a point in time of a trajectory set to joint angles as described in Ch. 12. This function will then give complex numbers only for those trajectory points that are outside the robots reach. Limiting the trajectories to only those that are realizable for the robot can thus be done by minimizing 13.2.
n
JRealizability =
t=0
{invKin({|time = t})}
(13.2)
Stability
Another important criterion is stability. The trajectories should keep the ZMP inside the stable region dened by the PoS. Furthermore, ZMP should be kept close to the center of the PoS to ensure as large a stability margin as possible. Because these trajectories, as the models, only concerns SSP, the PoS does not move with respect to the global reference frame. The center of the PoS (see appendix I on page 264) is:
RL0 pC,PoS = globpC,PoS = glob pC,PoS RL0R
0.001 = 0.0095 0
(13.3)
Hence this ZMP criteria can be fullled by minimizing 13.4

n
JZMP =
t=0
pC,PoS globpZMP
(13.4)
102
CHAPTER 13. TRAJECTORIES
Energy
It would be desirable to also minimize the energy consumption of the servomotors when the robot is walking. As the energy consumption is proportional to the torque delivered by a servomotor, this can be achieved by minimizing:
n
t
t=0
(13.5)
where t is a column vector containing the torque in all joints at time t. There is however also a maximum torque the servomotors can deliver, if this torque is exceeded in any joint, the trajectory can not be realized. To make the trajectory generation algorithm aware of this limit, the torque is modied by the non-linear function expCap before it is used in the performance function. The function
y = expCap(x, cap)
(13.6)
is linear until the value of x exceeds that of cap. Above cap, y increases exponentially. A plot of expCap as a function of x is shown in Fig. 13.2. The performance term that
15
expCap(x,2)
10
Figure 13.2: A plot of expCap as a function of x.

ensures minimal servomotor overload and energy consumption is thus:
n 20
JEnergy =
t=0 j =0
expCap(|t,j |, MAX,j )
(13.7)
where MAX,j is the stall torque of the servomotor controlling the j'th joint.
Smoothness
To make the robot move more smoothly, the velocity should vary as little as possible. This is implemented by minimizing the angular acceleration of the joints. This will of cause aect the step time for the robot.
n
JSmooth =
t=0
d2 dt2
(13.8)
2
Trajectory length
To ensure that the robot does not make any large unnecessary motions, the total length travelled by all the trajectory points is also included as a term in the performance
13.1. TRAJECTORY CRITERIAS
103
function. This should ensure that the movement of e.g. the arms are kept at a minimum. This term is described in Eq. (13.9)
n 1 5
JLength =
t=0 i=1
pt,i1 pt,i
(13.9)
where pt,i is the point in the i'th trajectory at time t.
Joint Limit Transgression

To ensure that the joints do not move more than the limits described in App. I a term describing the transgression of these limits is added. To aid in the denition of this term the function
softDeadZone R3 : R1 y =softDeadZone(x, limit, sof tness)
(13.10) (13.11)
is used. This function is zero for values of x smaller than limit sof tness. Between limit sof tness and limit the mapping from x to y is quadric (y = x2 ). Above limit y increases linearly with x with a slope given by:
dx2 dy
x=softness
softDeadZone is thus not only dierentiable, its derivative is a continuous function. This property will help convergence of the solution of the minimization problem using some numeric algorithms. Fig. 13.3 describes the function graphically.
1.5 softDeadZone(x,1,0.5)
0.5
0.5
1 x
1.5
Figure 13.3: Plot describing the input output relation for softDeadZone with xed limit (1) and softness (0.5).
Dening t,j as the angle of the j 'th joint at the time t, the joint angle limiting term of the performance function can be described as in Eq. (13.12).
n 20
JJointLimit =
t=0 j =1
softDeadZone t,j , MAX,j ,
+ softDeadZone MIN,j , t,j , 16 16

(13.12)
Here MAX,j is the upper limit of the angle j , and MIN,j is the lower.
104
Foot Lift
To keep the swing foot from touching the ground throughout the SSP, a term is added to the performance function. This term is zero when the trajectory for LL5 is outside a specied region. For this purpose the function inRegion is used. It takes a trajectory and two points as input. The function outputs one number proportional to how far inside the axes aligned box spanned by the two points the trajectory is. In inRegion the softDeadZone function is used again to ensure the continuity of the derivative. The foot lift performance is given by Eq. (13.13).
JFootLift = inRegion(LL5 , pFL1 , pFL2 )
(13.13)
Leg collision
This same method is used to keep the swing foot from colliding with the supporting leg, as described in Eq.(13.14). Here the function outsideRegion, which punishes trajectories that lie outside a specic region i.e. conceptually the inverse of inRegion, is used.
JLegCollision = outsideRegion(LL5 , pLC1 , pLC2 )
(13.14)
The location of pLC1 and pLC2 is such that the trajectory for LL5 starts just to the left of the region. Thus the algorithm initially moves the left foot a little to the left and thereby avoids collision with the right leg. The region is illustrated in Fig. 13.4
z y
z x
y x
Figure 13.4: The axes aligned box spanned pLC1 and pLC2 (red) shown with the
initial trajectory set (green). The frames in the corners represent the orientation of the global reference frame.
Step Time
When generating dynamic trajectories, the generation algorithm should be able to alter the step time. To ensure that the step time is as short as possible, a term is added in
13.2. PARAMETERS DEFINING A TRAJECTORY SET
105
the performance function which punishes long lasting trajectories. This term is dened as:
JTime = tend tstart
(13.15)
Joint Velocity Limit Transgression

To counteract trajectories that exceed the maximum angular velocity of the servomotors (e.g. as a result of trying to minimize JTime ) the term JVelocityLimit is added. It is zero as long as the velocities are below the limits and linearity increasing above. softDeadZone is used to smooth the transition.
JVelocityLimit =
t,j , MAX,j , MAX,j softDeadZone 10 t=0 j =1
20
(13.16)
MAX,j is the maximum angular velocity of joint j ; Here

The nal performance function, i.e. the function that the trajectory generating algorithm should minimize, is simply a weighted sum of the previously mentioned performance functions:
JRealizability JZMP JEnergy JSmooth JLength J =W JJointLimit JFootLift JLegCollision JTime JVelocityLimit
where W is a row vector of weights.
(13.17)
13.2 Parameters Dening a Trajectory Set

To enable minimization of J by some numeric method, some parameters that describe the trajectory must be determined. There should be as few parameters as possible and yet they must uniquely describe all ve trajectories at all times through one SSP. Thus four points in space/time are used to describe each trajectory, and more points are generated for the controller by interpolation. Furthermore, the initial and nal position of each trajectory is constant. The denition of these constants implicitly also dene
106
step length, and double support posture. The trajectories are thus dened by: 0 0 0 0 0 0 0 0 rl1 = (13.18) 0 0 0 0 tend tstart tend tstart tstart 2 tend 3 3 xstart,rl5 g1 g5 xend,rl5 ystart,rl5 g2 g6 yend,rl5 rl5 = (13.19) zstart,rl5 g3 g7 zend,rl5 tstart g4 g8 tend xstart,ll5 g9 g13 xend,ll5 ystart,ll5 g10 g14 yend,ll5 ll5 = (13.20) zstart,ll5 g11 g15 zend,ll5 tstart g12 g16 tend xstart,ra5 g17 g21 xend,ra5 ystart,ra5 g18 g22 yend,ra5 ra5 = (13.21) zstart,ra5 g19 g23 zend,ra5 tstart g20 g24 tend xstart,la5 g25 g29 xend,la5 ystart,la5 g26 g30 yend,la5 la5 = (13.22) zstart,la5 g27 g31 zend,la5 tstart g28 g32 tend where gi is a parameter which will be determined numerically. The column vector g containing all the gi parameters is referred to as the genome of the trajectory set. When used to develop dynamic trajectories the time has increased signicance, and the total step time tend tstart is also a variable parameter. This parameter becomes g33 = tend and tstart is set to 0.
13.3 Interpolation
From the g vector a trajectory set can be computed given initial and nal positions. Using the above matrices yields a trajectory set with n = 4. For the purpose of playing back the trajectories on the robot, a time resolution of 4 is inadequate. Also for verifying that the trajectories are sound (minimize the performance function), more points are preferred. To meet these shortcomings, the trajectories are interpolated. Cubic spline interpolation is used, that is nding the ai , bi , ci , and di for i = 1, 2, ..., n to Eq.(13.23).
S (t) = ai (t ti )3 + bi (t ti )2 + ci (t ti ) + di
that fullls:
for t [ti ti+1 ]
(13.23)
S i (ti ) = ptraj |t=ti S i (ti+1 ) = ptraj |t=ti+1 S i1 (ti ) = S i (ti ) S i1 (ti ) = S i (ti )
(13.24) (13.25) (13.26) (13.27)
Using the constraints in Eq.(13.24) through Eq.(13.27) yields a natural spline as depicted with a dashed curve in gure 13.5. This might however result in some jerky movement at the transition from one phase to the next, as the velocity (the derivative of the curve)
13.4. MINIMIZING PERFORMANCE FUNCTION
107
S(t) Phase Transission t
Phase Transission
Figure 13.5: Figure illustration the dierence between the natural (dashed) and
clamped (solid) spline. The circles represent the points from which the curves are interpolated.
is not continues in this point. An extra constraint is added to address this issue. This constraint simply xes the derivative at the start and end of the entire spline. This constraint, formulated in Eq. (13.28), is used to create the clamped spline in Fig. 13.5.
S 1 (t1 ) = S 4 (t4 )
(13.28)
The trajectories for the dierent limbs require dierent approaches to solving the continuity issue. For the LL5 trajectory it is simply not considered an issue, because the LL5 trajectory will be zero for the next step, and the natural spline interpolation is used. For the arms (RA5 and LA5) a zero slope clamping is used (i.e. Eq. (13.29)) to insure that the end of one trajectory matches the beginning of the other.
S 1 (t1 ) = S 4 (t4 ) = 0
(13.29)
For the torso trajectory, RL5, the spline is clamped using Eq. (13.28) in the y dimension and Eq. (13.30) in the other two dimensions. This matches the slope of the torso trajectory to a version of itself mirrored in the x/z -plane.
S 1 (t1 ) = S 4 (t4 )
(13.30)
13.4 Minimizing Performance Function

The function
evalTraj R32 : R1
(13.31)
calculates the performance function of a trajectory set given its genome g . This is done by rst interpolating n,eval points in time for each trajectory. Inverse kinematics are then used to nd the joint angles at all the n,eval points. These angles are then interpolated again and their rst and second derivatives are computed. These are then fed into the Simulink implementation of the Newtonian model to generate the ZMP trajectory and the torque vectors t . All the performance terms can then be calculated from the available information, as described in Sec. 13.1. Minimizing the function can be achieved by dierent methods. Two of these are explained in the next two sections.
108
13.4.1 Method of Steepest Decent

Starting from some, in principle, arbitrary g the performance can be calculated. The derivative can be approximated using
evalTraj(g ) evalTraj(g + g i ) evalTraj(g ) = gi gi
(13.32)
where g i is a vector of zeros except for a small delta value in the i'th row. Using Eq. (13.32) the gradient is then calculated. By the method of steepest decent it is possible to nd a minimum in the performance function. That is, nding the direction (dimension) in which the performance function declines the most and moving the solution in that direction. After a number of iterations the solution will be at a local minimum in the performance function. The drawback to using steepest decent to determine the best trajectory is that the solution generated may only be a local minimum of the performance function. And since the solution space is 32 dimensional it is bound to have a lot of local minima. The complexity of the performance function also contributes to the large number of local minima.
13.4.2 Genetic Algorithm

The trajectories for the robot are generated using a genetic algorithm. This method implements ideas from evolution to the minimization problem. This section will shortly describe the genetic algorithm as it is applied to the trajectory generation problem, for further information the Matlab les that implements this approach can be found on the CD [9].
Step 1 Initially, a number (npop ) of individuals are created based on some initial
genome ginit . The function
mutate R32 : R32
(13.33)
takes one genome and mutates it by adding some random amount to a random number of the gi s. It is used to create this initial population P 0 R32npop .
P 0 = mutate(ginit ) mutate(ginit ) . . .
mutate(ginit )
(13.34)
Step 2 Then the tness of each individual F is computed using the previously dened performance function. evalTraj(P 1 ) evalTraj(P 2 ) F = (13.35) . . .
evalTraj(P npop )
Step 3 The next generation is the generated. A few (nelite ) of the most t individuals (lowest values in F ) are selected as elites, and continue to the next generation without changes. Other nbreed individuals are selected for breeding. These have their nbreed ! ospring. Recomgenomes recombined with each other, creating nbreed = 2!(n 2 breed 2)! bination is done using the function
breed R322 : R32
(13.36)
13.4. MINIMIZING PERFORMANCE FUNCTION
109
that takes the genomes of the parents and for each element in the child genome randomly selects the value from one or the other parent. The rest of the individuals in the old generation (those with the worst tness) die, and are replaced by random mutations of the elite individuals. Let P i,j be the j'th most t individual in the population of the i'th generation, then the population of the next generation P i+1 is described by Eq. (13.37)
P i+1
P i,nelite breed(P i,nelite +1 , P i,nelite +2 ) breed(P i,nelite +1 , P i,nelite +3 ) . . . breed(P i,nelite +1 , P i,nelite +nbreed ) breed(P i,nelite +2 , P i,nelite +3 ) = . . . all combinations of {P i,nelite +1 . . . P i,nelite +nbreed } . . . breed ( P , P ) i,nelite +nbreed 1 i,nelite +nbreed mutate ( P ) i,nelite +nbreed +1 mutate ( P ) i,nelite +nbreed +2 . . . mutate(P i,npop )
P i,1 . . .
(13.37)
Step 2 and 3 are repeated until an acceptable tness is achieved. The genome of the most t individual in the nal generation is then converted into a trajectory set as described in Sec. 13.2 and 13.3. The genetic approach to trajectory generation described in this section is implemented in Matlab . The nal trajectory set is exported to the 3D modelling software Rhinoceros for manual review and verication. Rhinoceros is also used to dene the initial and nal position i.e.
pstart,rl5 pend,rl5 pstart,ll5 pend,ll5 pstart,ra5 pend,ra5 pstart,la5 pend,la5

which are used as tuning parameters. Tuning is also performed on the parameters pFL1 , pFL2 , pLC1 and pLC2 cf. Eq. (13.13) and Eq. (13.14), these are also dened using Rhinoceros. Other tuning parameters include the weights in the performance function J:
W = w1
w2
w3
w4
w5
w6
w7
w8
w9
w10
(13.38)
and the sizes of the population groups in the genetic algorithm: nbreed npop , nelite , nbreed npop nelite + 2
(13.39)
These are all tuned by hand using the trajectories outputted to Rhinoceros with human walk as a reference.
110
This algorithm is used to generate a trajectory set for one entire SSP. Thus the only thing left to do to get a trajectory for a full gait cycle, is to add a mirrored version of the generated SSP trajectories after the original. The function:
mirrorTraj R20n : R20n
(13.40)
performs this operation. This function operates on the joint angles instead of the trajectories to avoid complications resulting from the reference frame moving between the two SSPs. The mirrored angles ( mir ) is found for each point in time using:
mir,rl1 mir,rl2 mir,rl3 mir,rl4 mir,rl5 mir,ra1 mir,ra2 mir,ra3 mir,ra4 mir,ra5
= = = = = = = = = =
ll5 ll4 ll3 ll2 ll1 la1 la2 la3 la4 la5
mir,ll1 mir,ll2 mir,ll3 mir,ll4 mir,ll5 mir,la1 mir,la2 mir,la3 mir,la4 mir,la5
= = = = = = = = = =
rl5 rl4 rl3 rl2 rl1 ra1 ra2 ra3 ra4 ra5
To summarize, using the genetic algorithm described here, it is possible to generate trajectories for an entire gait cycle. The trajectory set is dened (though not uniquely) by the posture at the start and end of a SSP. The algorithm strives for an optimal trajectory set (one that minimizes J ). Convergence is however not guaranteed, as the algorithm uses genetic analysis. This approach is however more likely to nd the global minimum of J than other methods that could guarantee convergence.
13.5 Tuning
Some extra features are built into the generation algorithm to ease the tuning process. Apart from the utilization of Rhinoceros for visualization, these include the creation of a log le in which the value of each term of the performance function is logged for all elite individuals in each generation. The log also contains the average distance between the elite individuals at all generations.
Performance Function Weights

As some of the terms in the performance function J in Eq. (13.17) contradicts each other, e.g. minimizing the acceleration in Eq. (13.8) and at the same time trying to make the robot walk as fast as possible in Eq. (13.15). These terms cannot be at their minimum at the same time, so where the minimum of their sum lies is dened solely by the weights W . This example demonstrates the importance of the values in W . Considerable resources are therefore spent on tuning W , and this tuning process is documented in this section. Initially, before considering the eects of each term, the goal is to make all the terms have an equal impact on the trajectories. The weights are rst tuned such that each term in the performance of the nal individual is approximately equal. This does not guarantee that the terms have equal inuence as the slope of the terms is also of importance. A steeper slope for one term, will result in a larger dierence in that term between the individuals in a generation, and thus the selection of the elites will be based more
13.5. TUNING
111
heavily on that term. The log is examined to get an indication of the slopes of each term near the nal solution. The weights are adjusted such that the slopes are in the same order of magnitude. This step requires some engineering intuition and trial and error, as it is not as straight forward as it might seem. For one, some of the terms are zero for a large area around the nal solution. JFootLift is an example of such a term. The slopes of such terms are of course zero in the vicinity of the solution. These are instead adjusted to match the slope of other terms in earlier generations when the individuals are not close enough to the nal solution to make these terms zero. Another property that makes this tuning hard is the fact that even with the same weights, two successive executions of the algorithm may provide dierent solutions. Although this will be rare when the weights are not changed, in the tuning process the weights are changed and the solution will move. This may require re-tuning of weights that was tuned to at good value for the previous solution. When the weights are tuned to make an equal impact on the nal trajectories, these are examined to determine further improvement of W . The solution is compared to normal human gait, and corrections are made to W to put emphasis on some terms of the performance function. To give an example of this process, at a point in the tuning process the weights in Eq. (13.41) are used to generate the trajectory set depicted in Fig. 13.6.
W = 5 1
0.0001 1
0.5
0.2
(13.41)
z y
z x
y x
Figure 13.6: Trajectories corresponding to the weights in 13.41. The dark green line is the generated trajectory, the light green is the starting position and the red line is the CoP for the robot.
Analyzing the output shown in this gure, it is determined that the CoP trajectory (red line) is not satisfactorily inside the PoS (i.e. the rectangle spanned by the right foot in
112
the x/y plane). The weighting for JZMP is therefore increased from 0.5 to 35, and the generation algorithm is run again.
W = 5
0.0001 1
35
0.2
(13.42)
The solution using these weights is shown in Fig. 13.7.
z y
z x
y x
Figure 13.7: Trajectories corresponding to the weights in 13.42. The dark green
line is the generated trajectory, the light green is the starting position and the red line is the CoP for the robot.
As seen in Fig. 13.7 the red line has been moved more onto the area of the foot i.e. the CoP is inside the PoS for a longer time period. It should be noted that the solution in Fig. 13.7 by no means is the nal solution, the tuning continues with other attention paid to the other weights.
Population size
Tuning the population size npop is a compromise between precision and generation time. The same compromise also applies to the tuning of ngen . For trajectories that are meant to be played back by the robot npop = 200 and ngen = 50 is used. This results in a generation time of 5 to 7 hours when using the slower Newton-Euler model to calculate ZMP. Lower values are used for tuning purposes. The sizes of the population subgroups nelite and nbreed are chosen such that about half of each population generated by mutation and half by breeding with only a small number of elites. Through testing the used values are determined to nelite = 6 and nbreed = 13.
13.6. FINAL TRAJECTORIES
113
13.6 Final Trajectories

Finally when all the parameters are tuned satisfactorily, the algorithm is run to generate the trajectories that will be used for controlling the robot. In this section these trajectories are documented. Three sets of trajectories are generated. One for each model. The same performance weights are used for all models, these are:
W = 5 0.01
0.35 5
0.0001
700
0.001
(13.43)
The variables dening the initial and nal posture is also the same for all the trajectories.
13.6.1 Kinematics Based Trajectories

For static walk the kinematic model is used to calculate CoP which is required by JZMP , see Eq. (13.4). The trajectories are illustrated in Fig. 13.8.
z y
z x
y x
Figure 13.8: Trajectories generated using the kinematic model. The dark green line is the generated trajectory, the light green is the starting position and the red line is the CoP for the robot.
114
The performance achieved by these trajectories is: JRealizability 1.2044 106 JZMP 1.2027 JEnergy 0 JSmooth 1.4663 JLength 0.41863 J =W = JJointLimit 2.6148 JFootLift 0 JLegCollision 12.4339 JTime 0 JVelocityLimit 0
(13.44)
The step length is xed, as mentioned, and is 11.6 cm. The step time is also xed for these trajectories, and is set to 3 s. The ZMP (red curve in Fig. 13.8) starts between the two feet. As the robot is essentially in double support at this point in time, the PoS includes this point between the feet. Thus this ZMP position is acceptable, although applying the SSP model theory would state that the robot is unstable with such a ZMP position. This paradox is a result of the lack of a DSP model which can provide the theory to calculate ZMP at this point. As a result, the algorithm tries to move ZMP into what it believes to be the PoS (i.e. the area spanned by the right foot) as fast as possible. This will hopefully result in that, the ZMP is moved far enough inside the PoS, before the DSP ends in reality, to maintain stability. To solve this apparent problem, a separate DSP trajectory set could be generated with this algorithm using a DSP model.
13.6.2 Newton-Euler Based Trajectories

For dynamic walk the Newtonian model is used to generate ZMP. This is also the only model which calculates the torques in all joints, and thus these trajectories are the only ones that optimize on the energy, Jenergy found in Eq. (13.7)). The trajectories are illustrated in Fig. 13.9. The same argumentation applies to the ZMP position in this trajectory set as was discussed for the previous one. In the generation of this trajectory set, the step time was also a variable parameter. The algorithm calculated that the optimal step time is 2.577 s. With this step time the robot will not be able to fulll the requirement to walking speed set in Sec. 3.3. For this requirement to be fullled the step time should be 11.6 cm/22.4 cm/s = 0.51 s. The step time can be lowered by altering W , but this would down-prioritise other aspects of the trajectory set, entailing an unusable trajectory.
13.6.3 Lagrangian Based Trajectories

A third set of trajectories are generated using the Lagrangian model to supply ZMP. These trajectories will aid in the verication of the simplications made in the development of the Lagrangian model. The trajectories are illustrated in Fig. 13.10. The same issues as discussed in for the previous two trajectory sets also applies to this one. This trajectory set is generated for verication purposes. It should verify that the application of the simplied Lagrangian model results in a solution similar to that generated of the other models. This verication is deemed successful by visual inspection.
13.6. FINAL TRAJECTORIES
115
z y
z x
y x
Figure 13.9: Trajectories generated using the Newtonian model. The dark green
z y
z x
y x
Figure 13.10: Trajectories generated using the Lagrangian model. The dark green
Chapter Summary
In this chapter the trajectory generation is described. The generation nds a numerical solution to an optimization problem dened through a performance function. The
116
numerical solution is found using a genetic algorithm. This algorithm searches for a minimum of the performance function in a wider area than other numeric methods e.g. steepest decent. The properties of the genetic algorithm also results in a nondeterministic solution, which complicates the tuning process. The verication of the trajectories generated using the algorithm described in this chapter is preformed as part of the accept test in Ch. 17.
Chapter
14
Force Measuring to CoP Position

As mentioned in chapter 11, it is necessary to keep the CoP/ZMP inside the polygon of support in order to balance the robot in both static and dynamic walk . In this chapter, the mapping to CoP/ZMP from the measurements of the forces acting of the sole of the feet is determined, see gure 14.1. This is done by using denition of the CoP and apply it to the feet of the robot.
Strain gauges
ZMP error
Servomotors
Figure 14.1: Illustration of the blocks designed in this chapter and it's context in
the controller.
14.1 The CoP Position based on Pressure Measurements

The contact forces acting between the foot and the surface consist of two components, normal forces and friction forces. This is illustrated in Fig. 14.2a. The normal forces act perpendicular to the sole of the foot, while the friction forces are parallel to the sole. The components normal to the sole are called pressure forces. The feet of the biped robot have four small contact areas as illustrated in Fig. 14.2: the left and right toe, and the left and right heel. In the SSP phase the PoS is the convex area depicted in Fig. 14.2b. 117
118
CHAPTER 14. FORCE MEASURING TO COP POSITION

little toe big toe
Fpressure Fpressure
little toe
Fpressure
contact surfaces
Fpressure
big toe
PoS
Ffriction Ffriction
little heel
big heel
(a)
(b)
Figure 14.2: Right foot of the biped, where the contact forces are illustrated in
(a), i.e. the friction and the pressure forces. In (b) a bottom view of the right foot is given, illustrating the PoS.
Four force sensors are implemented on each foot of the biped robot. These measure the total pressure force at each of the four contact areas. The measured pressure forces are assumed to act in the center of the toes and heels as indicated in Fig. 14.3a. The CoP is determined in the frame {RL0 } attached to the ankle joint. In Fig. 14.3 the point Q is the projection of the origin of frame {RL0 } onto the sole of the foot. According to the denition of the CoP given in Ch.2, the resulting torque due to pressure forces exerted on the foot is zero at the CoP. Hence, at the CoP the y -component y and the z -component z of the resulting torque due to pressures must be equal to zero, which yields the following equations,
y = (Fp,r1 + Fp,r2 )(ltoes zCoP ) + (Fp,r3 + Fp,r4 )(lheels + zCoP ) = 0 z = (Fp,r1 + Fp,r3 )(llittle toe yCoP )+(Fp,r2 + Fp,r4 )(lbig toe + yCoP ) = 0
which give the following expressions for the CoP,
yCoP = zCoP
llittle toe (Fp,r1 + Fp,r3 ) lbig toe (Fp,r2 + Fp,r4 ) Fp,r1 + Fp,r2 + Fp,r3 + Fp,r4 ltoes (Fp,r1 + Fp,r2 ) lheels (Fp,r3 + Fp,r4 ) = Fp,r1 + Fp,r2 + Fp,r3 + Fp,r4
(14.1) (14.2)
where yCoP and zCoP are the y and z -coordinate of the CoP given in frame {RL0 }. Thus, from Eq. (14.1) and Eq. (14.2) the CoP can be determined using the four pressure measurements Fp,R1 , Fp,R2 , Fp,R3 and Fp,R4 .
14.2 The CoP Position in DSP Phase

The dynamic models developed in Ch. 9 and Ch. 10 only considered the SSP phase, but when testing and controlling the robot, it is still valuable to be able to measure the CoP during double support. This section develops a method for estimating the CoP in the DSP using the measured pressure forces. During the DSP phase, Eq. (14.1) and Eq. (14.2) can be used to locate the local CoP of each foot, i.e. the point on the foot where the sum of the four pressure forces acts.
14.2. THE COP POSITION IN DSP PHASE

llitlle toe lbig toe Fp,R1 ltoes
Q
119
llittle toe-yCoP lbig toe+yCoP
x y Fp,R1 Fp,R2
little toe
RL0
RL0
Fp,R2 z
RL0
RL0
l toes-z CoP
Fp,R3 Fp,R4
CoP
CoP
big toe
y ty lheels
RL0
RL0
z CoP
zCoP
pCoP
Q
tz
right foot (a)
Fp,R3
Fp,R4
bottom view of right foot ( c)
bottom view of right foot (b)
Figure 14.3: In (a) the point Q is the projection of the origin of frame {RL0 }
onto the foot. The pressure forces are assumed to act in the center of the toes and heels. In (b) the distances at which the pressure force acts relative to the point Q are shown. In (c) the CoP position is shown along with its coordinates in the frame {RL0 }.
This is illustrated in Fig. 14.4. Thus, the local CoPs are given as, hankle xCoP,R llittle toe (Fp,r1 +Fp,r3 )lbig toe (Fp,r2 +Fp,r4 ) RL0 pCoP,R = yCoP,R = Fp,r1 +Fp,r2 +Fp,r3 +Fp,r4 ltoes (Fp,r1 +Fp,r2 )lheels (Fp,r3 +Fp,r4 ) zCoP,R Fp,r1 +Fp,r2 +Fp,r3 +Fp,r4 hankle xCoP,L llittle toe (Fp,l1 +Fp,l3 )lbig toe (Fp,l2 +Fp,l4 ) LL5 pCoP,L = yCoP,L = Fp,l1 +Fp,l2 +Fp,l3 +Fp,l4 ltoes (Fp,l1 +Fp,l2 )lheels (Fp,l3 +Fp,l4 ) zCoP,L
Fp,l1 +Fp,l2 +Fp,l3 +Fp,l4
heesl+
yCoP
(14.3)
(14.4)
At CoPR the sum Ftot,R of the pressure forces in the right foot acts, and at CoPL the sum Ftot,L of the pressure forces in the left foot acts, where
Ftot,R = Fp,r1 + Fp,r2 + Fp,r3 + Fp,r4 Ftot,L = Fp,l1 + Fp,l2 + Fp,l3 + Fp,l4
It is necessary to describe CoPR and CoPL in the same frame in order to nd the CoP of the robot in DSP. This kinematic model developed in Ch. 8 is developed for the SSP and cannot directly be applied to the DSP. But the joint angles produced by the gait trajectories in Ch. 13 are supposed to create a posture of the robot, where both feet remains horizontal and static in DSP. Hence, a single support kinematic model starting in either foot will still yield the correct position of the other foot. Thus, the local CoPs can be transformed between the two frames {RL0 } and {LL5 } using the transformation matrices derived in the kinematics:
rl0 ll5
0 ll5 pCoP,L = rl pCoP,L ll5T

ll5 rl0
(14.5) (14.6)
pCoP,R =
rl0
pCoP,R
The CoP is where the resulting torque produced by pressure forces is equal to zero, which can be stated as, Fsum,R Fsum,L 0 res = (rl0pCoP,R rl0pCoP ) 0 + (rl0pCoP,L rl0pCoP ) 0 = 0 0 0 0
120
CHAPTER 14. FORCE MEASURING TO COP POSITION
which can be rewritten as the two equations,
yCoP = zCoP
yCoP,R Fsum,R + yCoP,L Fsum,L Fsum,R + Fsum,L zCoP,R Fsum,R + zCoP,L Fsum,L = Fsum,R + Fsum,L
(14.7a) (14.7b)
where all coordinates are given in {RL0 }. The same expressions are valid if all coordinates are given in {LL5 }. These can be obtained using Eq. (14.5) and Eq. (14.6). Thus the CoP is a weighted mean of the two local CoPs, with the pressure forces of the local CoPs as weights. This seems intuitively correct, as the CoP is dominated by the local CoP with the most pressure on it. Further, if the two pressures are equal the CoP is the midpoint of the two local CoPs.
Fp,L1
toes
Fp,L2
CoPL
Fp,R2
RL0
Fp,R1
z
left foot
pCo
LL5
LL5
pCoP CoP
RL0
RL0
LL5
pCoP
RL0
y
pCoP,R
R
RL0
LL5
pCoP,
CoPR
heels
Fp,L4
Fp,L3 Fp,R3 Fp,R4
Figure 14.4: Illustration of the feet in DSP. Both feet have a local CoP denoted
CoPR and CoPL , where the sum of the foot's pressure forces acts. The CoP of the biped robot can then be found as a weighted mean of the local CoPs using the sum of the local pressure forces as weights.
Note that Eq. (14.7) can also be used in SSP phase as the weight of the lifted foot is zero, but the coordinates should be given in the frame of the supporting leg, i.e. {RL0 } or {LL5 }, otherwise the CoP would be described in a frame moving relative to the ground. The solution is to shift the description of the CoP from {RL0 } to {LL5 } and visa versa in every DSP phase.
14.3 Verication of the CoP Mapping

In App. C a test was conducted to see if the measured pressure forces could be used to determine the CoP, i.e. to test the force sensors and verify Eq. (14.1) and Eq. (14.2). The test was conducted by applying known CoP positions by dismounting the foot from the robot and applying 33 dierent weight distributions to it. As a result, the actual CoP was the projection of the CoM on the sole of the foot, and could be used to verify the CoP calculated from the measured pressure forces. The results from the test are seen in Fig. 14.5 along with the CoP calculated using Eq. (14.1) and Eq. (14.2). It is seen that the calculated CoP complies with the actual CoP location within a maximum error of approximately 5 mm in either direction. In Fig. 14.5 the calculated yCoP values have a small oset and seems to be compressed around zero when increasing the CoP distance from zero.
right foot
LL5
pCoP,L
P,L
14.3. VERIFICATION OF THE COP MAPPING
121
This is due to the fact that the locations of the pressure forces Fp,r1 , Fp,r2 , Fp,r3 and Fp,r5 in the toes and heels depends on the location of the CoP. In the CoP model they are assumed to always act in the center of the toes, see Fig. 14.3. Thus, in the y -direction the assumed location of the resulting forces in the toes and heels can dier up to 10 mm from the actual location, as the toes are 20 mm wide. Furthermore, using this assumption, the model cannot yield an CoP outside the center of the heels and toes. In the z -direction, the error of this assumption is small, because the toes and heels are 1.5 mm wide in this direction. As Eq. (14.1) and Eq. (14.2) gives an estimated CoP with a maximum error of 5 mm in a foot of 65 mm width, the results is considered acceptable.
40 30 20 yCoP [mm] zCoP [mm] Actual yCoP Calculated yCoP 5 10 15 20 Test no. 25 30 10 0 10 20 30 40 40 30 20 10 0 10 20 30 40 5 Actual zCoP Calculated zCoP 10 15 20 Test no. 25 30
Figure 14.5: In the two gures the y - and z -coordinates are shown for the actual and
calculated CoP. The actual CoP is found by determining the CoM of the arrangement, while the calculated is determined from the pressure measurement using Eq. (14.1) and Eq. (14.2).
Chapter Summary
In this chapter a model is developed to locate the CoP of the biped robot. The mapping describes the relation between the CoP location and the pressure forces acting on the sole of the foot. This lead to an expression giving the CoP as a function of the four measured pressure forces acting on the foot. This expression is veried experimentally. The model gives the CoP location with an accuracy of 5 mm within a foot of 65 mm width. Part of this error is argued to originate in the simplication that the measured pressure forces always act in the center of the toes and heels, while also the noise in the strain gauge circuits has a signicant impact. A method for measuring the CoP location during the DSP phase is also given. This method uses the expressions for the CoP in the SSP to nd the point in each foot, where the sum of the foot's pressure forces act. These points are then described in the same coordinate system using the transformation matrices from the kinematic model. These two local CoPs can be added as a weighted mean to give the robot's CoP location. The weights used are the sum of the pressure forces for each foot.
Chapter
15
ZMP Controller
The gait cycle signals developed in the previous chapter, should make the robot walk and keep it balanced throughout an entire gait cycle using pure feed-forward. To increase the stability of the biped during gait, i.e. correct for model uncertainties and external disturbance, it would be advantageous to implement some sort of feedback controller. See Ch. 11 for description and Fig. 15.1.
Strain gauges
ZMP error
Servomotors
Figure 15.1: An illustration of the block designed in this chapter and it's context
in the controller.
Based on calculating the ZMP on-line, both during gait and stand, the scope of this chapter is to develop a strategy for making small adjustments to the joint angles provided by the gait cycle signals to increase the stability of the robot. The rst section contains a discussion of how a ZMP measurement and a ZMP reference is used to create a ZMP error signal for the controller. The main issue is that the robot is moving and all the previously dened coordinate systems move along with the biped robot. Thus, a strategy for selecting which reference frame to describe the ZMP in is necessary. In the second section, a controller is developed, which maps the ZMP error into an adjustment of the joint angle provided by the gait cycles signal. In the third section, the controller is tested and discussed. 122
15.1. DEFINING THE ERROR SIGNAL FOR THE CONTROLLER
123
15.1 Dening the Error Signal for the Controller

The purpose of the controller is to keep the robot balanced, both during stand and gait cycle. As a measure of balance and stability of the biped robot, the ZMP is used, as maintaining the ZMP inside the PoS is a valid stability criterion for both stable stand, static walk and dynamic walk. Thus, the input to the controller is chosen to be an error in the location of ZMP. Relating this to the control done in [rts et al., 2006], the input to the controller was the tilt of the robot. The report does not give a precise denition of this tilt, but zero tilt seems to be equivalent to having the ZMP placed in the center of a foot. Thus, the tilt signal is seemingly the same as the dierence between the location of the actual ZMP and the desired ZMP location, which in case of [rts et al., 2006] is in the center of the supporting foot. Thus, the term ZMP-error seems to be more accurate. Another approach is taken in this project, instead of simply using the center of the supporting foot as the ZMP reference. Each gait trajectory generated in Ch. 13 also included the theoretically (desired) ZMP trajectory. Hence, this can be used as a reference for the controller as following this ZMP trajectory would keep the robot stable, and make the robot walk as intended.
15.1.1 Choice of Reference Frames for the ZMP

In the previous chapter, the ZMP trajectory was only calculated for the SSP-R phase, where it was expressed in the global frame, which is located in the right foot. During SSP-R the global frame is xed relative to the ground and therefore easily comprehensible for describing the ZMP, but during SSP-L the global frame is moving. Hence, the interpretation of the ZMP coordinates in the global frame throughout an entire gait cycle is dicult. A solution to this could be describing the ZMP in a static frame locked to the surface, but this solution is inelegant, as it would result in ever increasing values for the ZMP coordinates as the robot walks. To avoid these problem, it have been chosen to describe the ZMP in two dierent frames, depending on the state of the gait cycle:
Frame {RL0 } located in right foot is used to describe ZMP when in SSP-R Frame {LL5 } located in left foot is used to describe ZMP when in SSP-L
Thus, the ZMP is always described in a frame xed to the ground and the frames follow the motion of the robot. The disadvantage of this method is managing the shifting of frames. Another argument for using two coordinate systems can be found by considering how the ZMP is determined. The method for mapping the force measurements to a ZMP was described in Ch. 14, where an expression for nding the ZMP both during SSP and DSP was found, which is repeated here:
yCoP = zCoP
yCoP,R Fsum,R + yCoP,L Fsum,L Fsum,R + Fsum,L zCoP,R Fsum,R + zCoP,L Fsum,L = Fsum,R + Fsum,L
(15.1a) (15.1b)
where all coordinates are given in {RL0 }. The same expressions are valid if all coordinates are given in {LL5 }. The force Fsum,R and Fsum,L are the sum of pressure forces
124
CHAPTER 15. ZMP CONTROLLER
acting on the right and left foot respectively. The coordinates yCoP,R and zCoP,R are the location of the local CoP in the right foot, and yCoP,L and zCoP,L is the local CoP of the left foot. Thus the CoP is a weighted mean of the two local CoPs, with the pressure forces of the local CoPs as weights. In Eq. (15.1) if for example frame {RL0 } is chosen, the position of the local CoP in the left foot is transformed to frame {RL0 } using a chain of transformation matrices, going from the left foot, through the hips, to the right foot. However, this long transformation may introduce some deviation in the ZMP computation from the actual ZMP, because of for example backlash in the legs, which the kinematic model does not take into account. But during SSP the lifted foot does not contribute to the ZMP, as its weight Fsum is zero. Hence, seen from this perspective, it is also advantageous always to switch to the frame located in the supporting foot, as this would reduce the error from transforming between frames. How the frame shifting works is seen in Fig. 15.2, where it is illustrated how the coordinates of the ZMP changes to reect the frame of the support foot. In the illustration the ZMP reference and the measured ZMP are both constant.
SSP-R SSP-L SSP-R SSP-L
left foot
ZMPmes
erry
zRL0
coordinate value
0 -1 -2 -3 -4
Frame {RL0} Frame {LL0} Frame {RL0} Frame {LL0}
zLL0 yLL0
yref ymes
time
errz
yRL0
ZMPref
right foot (a)

SSP-R
coordinate value
(b)
SSP-R SSP-L
coordinate value
SSP-L
SSP-R
SSP-L
SSP-R
SSP-L
2 1 0 zref (c) zmes time
2 1 0
errz=zref-zmes erry=yref-ymes time
-1
-1 (d)
Figure 15.2: To illustrate the idea of switching frames, the ZMP reference and the
measured ZMP are made constant through a gait. In (a) the location of {RL0 } and {LL5 } are shown, as the measured ZMP, ZMPmes , and the reference ZMP, ZM Pref . In (b) and (c) it is shown hown the coordinates changes depending on the frame. In (d) the ZMP errors are shown.
15.1.2 Computing the ZMP-Error

When using frame {RL0 } and {LL5 }, only the y and z coordinate are of interest when considering the ZMP, as the x-axis is always vertical in the supporting foot. Thus, the ZMP-error can be divided into two quantities, one for each direction: the error in the y-direction erry and the error in the z-direction err z , given as the dierence:
err y = yref ymes err z = zref zmes
(15.2a) (15.2b)
15.1. DEFINING THE ERROR SIGNAL FOR THE CONTROLLER
125
where y and z are given in either frame {RL0 } or {LL5 } depending of the state of the gait. Furthermore, to ease the controller design, the errors should always have the properties given in Fig. 15.3. As a result, if err y is for example positive, the controller knows that the ZMP should be adjusted to the right, without knowing the state of the gait.
toes ZMPmes toes ZMPmes toes ZMPref toes ZMPref ZMPref
left foot right foot
ZMPref
left foot right foot
ZMPmes
left foot right foot left foot
ZMPmes
right foot
ZMP to the right of reference ZMP to the left of reference ZMP to the left of reference ZMP in the front of reference ZMP in the front of reference ZMP behind reference erry > 0 erry < 0 erry > 0 errz > 0 errz < 0 errz < 0
ZMP to the right of reference ZMP behind reference erry < 0 errz > 0
Figure 15.3: Illustration of the meaning of the sign of the error signals.
Using Eq. (15.2b), the ZMP-error is illustrated in Fig. 15.2d in both frames. As the y -axis of frame {RL0 } and {LL5 } points in opposite directions, the err y error changes sign depending on which frame is used. To design the error signal so it complies with the denition in Fig. 15.3, the quantity err y is multiplied with -1 during SSP-L. How the creation of the error signal is implemented, is seen in the Simulink model in Fig. 15.4. The frame shifting is performed midways in a gait cycle, i.e. during the DSP phase. As the duration of a complete gait cycle is know from the trajectory, the time of switch is determined by simple switching halfway using a clock or counter.
1
xRL0 yRL0 x zRL0 xLL5 yLL5 zLL5 Phase Signal Phase Signal
If in SSPR
1 Terminator1
y_ZMP
If in SSPL
Switch3 1 y_err Product
1 ZMP Position in both {RL0} and {LL5}
Switch2 Roation matrix from global frame to {RL0} 0 0 0 1 1 0

Phase Signal
z_ZMP
2 z_err
0 lim SSPL
1 0 0
Counter Limited Counts the time of one gait cycle and wraps around
If in SSPR send "1", else send "0"
0 0 1 0 1 0 1 0 0
Switch1
Matrix Multiply Rotate ZMP reference into {RL0} or {LL5}
Terminator2
y_ref z_ref
ZMP_reference 1 SSPR Roation matrix from global frame to {LL5} ZMP trajectory from gait data
Figure 15.4: This gures shows how the ZMP error is implemented in
Simulink . The phase signal is 1 when in SSP-R and 0 when in SSP-L. The signal is generated by restarting a counter for every completed gait cycle, and when the counter reaches half though a cycle, the output value of the switch-function switches from 1 to 0.
Using this conguration, the error signal have the boundaries shown in Fig. 15.5 during
126
SSP. The ZMP reference is not allowed outside the PoS and the measured ZMP is always within the dashed box.
0.124 m
Fp,R3
0.064 m 0.044 m right foot
Fp,R2
During SSP:
erry[-0.054 ; 0.054] errz[-0.124 ; 0.124] Fp,R1
Fp,R4
Figure 15.5: Boundaries of the ZMP-error during SSP. The measured ZMP is
always within the dashed box.
Having created the input signal to the controller, the controller is ready to be designed.
15.2 Controller Design

The small disturbance fuzzy logic controller in [rts et al., 2006] was used to correct the ZMP of the robot by making small adjustment to the gait trajectory, and afterwards using inverse kinematic to calculate joint angles. In [rts et al., 2006] the fuzzy logic was used on-line to choose which limbs could currently be manipulated, as this depended on the state of the gait. Instead of using the same fuzzy logic approach, this section explores a simple and elegant control strategy based on general knowledge of the robot's posture during walk and stand. Secondly, instead of creating the control signal by manipulating trajectories and using inverse kinematics afterwards, the ZMP error is directly mapped by the controller into small adjustment of the joint angles provided by the o-line generated gait cycle signal. A block diagram of the controller is illustrated in Fig. 15.6.
Force to ZMP ZMPmes mapping ZMP reference trajectory ZMPref
S
+
Small disturbance controller ZMPerror ZMP-error to angle qcontroller + + S adjustment mapping
qgait
Joint angles from gait cycle
Robot
Strain gauges
Servomotors
Figure 15.6: The small disturbance controller. The ZMP-error vector is the difference between the measured ZMP and the ZMP reference provided by the gait trajectory. From the ZMP-error the controller computes control joint angles, which are added to the gait cycle angles to remove the ZMP error.
15.2.1 Control Strategy

The control strategy developed in this section is based on adjusting the ZMP position by moving the CoM of the robot. The forces and torques created by accelerating the CoM is ignored in the initial design phase, i.e. the ZMP is always assumed to be the projection of the CoM on the surface. The eect of this assumption is discussed afterwards.
15.2. CONTROLLER DESIGN
127
In the previous section, the ZMP-error signal was divided into two components, the quantity err y describing the error of the ZMP in the frontal plane and err z describing the error in the sagittal plane. Accordingly, the controller is divided into two, a frontal plane controller reducing the err y error and a sagittal plane controller reducing err z . During the design it is considered how the two controllers can be decoupled, i.e. controlling err y without changing err z and visa versa.
Controlling in the Frontal Plane

The gait trajectories designed in the previous chapter have the following properties considering the posture of the robot:
The torso is always vertical. The hips, i.e. joint RL5 and LL1, are always located in the same horizontal plane. The feet are always horizontal.
These properties are illustrated in Fig. 15.7a. It is seen that the joints axes of joint RL1, RL5, LL1 and LL5 are always parallel. Thus, the CoM of the robot can be moved by adjusting joint RL1, RL5, LL1 and LL5 while still keeping a reasonable posture of the robot, i.e. the feet are still horizontal and the orientation of the torso remains the same, as it is a parallel displacement of the torso. This is illustrated in Fig. 15.7b. Further, this collective angle adjustment does not alter the ZMP location in the sagittal plane. When adjusting the joint angles of joint RL1, RL5, LL1 and LL5, it is chosen to adjust them the same amount. As a result, the strategy is also usable during stand or DSP. If the adjustment of RL5 and LL1 did not have the same numerical value, the feet would be forced to slide on the surface when regulating during DSP. The method of giving the same angle adjustment in RL1, RL5, LL1 and LL5 is in this report referred to as torso displacement.
CoM movement RA2 RA1 RL5 LA2 LA1 LA1 LL1 LL1 RL5 RA1
LL5 LL5
RL1
frontal view (a)
RL1 sagittal view frontal view (b)
Figure 15.7: Figure (a) an illustrate the robot's posture during gait. The joints
axes of joint RL1, RL5, LL1 and LL5 are always parallel, as the gait is designed to always keep the torso vertical and the hips in the same horizontal plane. Fig (b) illustrate how the torso can be displaced, and thereby the CoM, using joint RL1, RL5, LL1 and LL5.
Besides the legs, the arms can also be used to adjust the ZMP in the frontal plane. By using the shoulders, i.e. joint RA2 and LA2, the arms can be lifted away from the torso
128
in the frontal plane, thereby manipulating the CoM. Most likely, the arms are faster and more precisely moved compared to using the torso displacement, because the actuators in the arm are not as heavily loaded. But the weight of the arms are small compared the total weight of the robot. Thus, the arms can move the CoM more precisely, but not as much as the as torso displacement, i.e. the arms saturate faster in terms of adjusting the CoM. When having multiple control signals and only one measurement to manipulate, in this case err y , a classic strategy is mid-ranging. The mid-range control uses the fast and precise actuator directly to control the measurement, while the task of the slower actuator is to prevent the control signal of the fast actuator to saturate, i.e. keep the fast actuator in mid-range. The controller is shown in Fig. 15.8.
set point arms
Torso displacement controller: RL1,RL5,LL1 and LL5 Arm controller: RA2 and LA2
qlegs,y
Biped robot
erry
qarms,y
Figure 15.8: The mid-range controller. The error signal is used as input to the arm
controller, while the input to the torso displacement controller is the output from the arm controller. The arm controller should reduce the ZMP error, while the torso displacement controller should prevent the arms from saturating.
As the arm controller, a simple P-controller is used as this give a simple and fast response of the arms. For the torso displacement controller a PI-controller is used with a weak P-term. Thus, given an error the arms react fast, while the legs smoothly adjust the torso position so the arms can be lowered back toward the torso again. The weak Pterm is included for two reasons. The rst is for the leg to respond immediately if the arms are suddenly given a large control signal. The other is due to the anti-windup implementation. If the actuator saturates, the input to the integrator is set to zero, and if no P-control was included, the controller would be in a dead lock, as the integrator would maintain the actuator saturated. The weak P-term is enough to turn o the anti-windup signal, when the error switches sign. The boundaries of the error signal err y during SSP is given in Fig. 15.5. The gait trajectories also apply to the arms, i.e. the arms are not in zero position during the gait, but are lifted slightly from the torso. As a result, the arm controller is chosen to be able to lift the arms 60 in the shoulder joint during SSP, yielding the following gain for the controller:
kP,arm,y =
60 1100 0.054
(15.3)
When adjusting the legs, smaller angles are required. As the legs are 0.0205 m the torso and arms are displaced approximately 0.01 m for every 3 . This corresponds to moving the CoM of the robot approximately the same amount. By tuning, the weak P-term of the torso displacement controller have been chosen to give around 1 , when the arm controller gives maximum output during SSP:
kP,leg,y =
1 0.015 60
(15.4)
15.2. CONTROLLER DESIGN

The gain of the integrator have by tuning been chosen to have the following gain:
129
kI,leg,y = 0.045
(15.5)
If the control signal to the arms is for example 60 for one second, this gain corresponds to adjusting the legs 2.7 during this second, i.e. moving the CoM approximately 0.01 m, which should be fast enough to keep the arms out of saturation, compared to the ZMP error being at maximum 0.054 m during SSP. The mid-range controller for controlling the ZMP in the frontal plane is implemented in Simulink as seen in Fig. 15.9. The control signal to the legs is chosen to be in the range from -15 to 15 to prevent the legs from hitting each other. This range allows the controller to displace the torso from -0.053 m to 0.053 m, which is approximately twice the width of the foot, which should be enough. The sign of the gains in Fig. 15.9 ensures that the arms and legs are moved in the intended direction. It is seen in Fig.15.9 from the saturation blocks, that only one arm is lifted at a time, depending on which direction the ZMP should be moved.
0.01 P gain legs K Ts Antiwindup z1 Integrator 0.45 1 I Gain == Satuation signal Change sign2 Saturation Legs: 15 to 15 degrees 1 Change sign 1 RL1 adjust 2 RL5 adjust 3 LL1 adjust 4 LL5 adjust 5 90 to 0 degrees 1 err_y 1100 P Gain arms 90 to 0 degrees RA2 adjust 6 LA2 adjust
Antiwindup signal
Figure 15.9: The
Simulink implementation of the mid-range controller for adjusting the ZMP in the frontal plane.
Having discussed how the ZMP is controlled in the frontal plane, the following describes how the ZMP is controlled in the sagittal plane.
Controlling in the Sagittal Plane

As in the frontal plane, both the arms and legs can be used to move the ZMP in the sagittal plane to correct the err z . As a result, the mid-range solution as mentioned above is also applied in the sagittal plane. In the arms, the shoulder joints RA1 and LA1 can be used to move the CoM of the robot in the sagittal plane. Again, a simple P-controller is used for the arms. As start value the gain from Eq. (15.3) was used, and through tuning, the following value have been chosen:
kP,arm,z = 1000
(15.6)
Considering the legs, multiple ways exist for displacing the CoM. Three of the joints in each leg have parallel joint axes as illustrated in Fig. 15.10a. To maintain posture of the torso posture, the joint angles of the supporting the leg could be manipulated as illustrated in Fig. 15.10b, where the torso is displaced in SSP-R by keeping the sum of the joint angles RL2, RL3 and RL4 constant. But during DSP, altering the posture
130
of only one leg may lead to an unintended result. As a result, the non-supporting leg must also be altered. To avoid this amount of manipulation of the robot during gait, a simpler solution is used. Here the CoM is moved by tilting the torso as shown in Fig. 15.10c by adjusting joint RL4 and LL2 with the same amount. A consequence of tilting the torso is that the joint axes of joint RL1, RL5, LL1 and LL4 does not remain parallel as assumed in the frontal plane controller. But if the torso is not tilted more than -15 to 15 the strategy have been found to still work in practice, as the lifted feet still remains nearly horizontal during SSP, and during DSP, the feet are able to glide slightly.
CoM movement RA2 RA1 LA2 LA1 CoM movement
LL2 RL4 LL3 RL3 RL2

frontal view (a) sagittal view (b) sagittal view (c)
LL4
Figure 15.10: Figure (a) is an illustration of the robot's posture during gait. The
joints axes of joint RL2, RL3 and RL4 are always parallel, and likewise for LL2, LL3 and LL5. In (b) it is illustrated how the torso can be displaced, while still keeping it vertical. This is done by keeping the sum of the joint angles RL2, RL3 and RL4 constant. In (c) joint RL4 and LL2 are adjusted with the same angle, thereby tilting the torso, but still moving the CoM.
The weak P-term of the torso displacement controller in the sagittal plane is given the same value as in the frontal plane:
kP,leg,z = 0.015
The gain of the integrator is tuned to the following gain:
(15.7)
kI,leg,y = 0.060
(15.8)
If the control signal to the arms are for example 60 for one second, this integral gain corresponds to tilting the torso 3.6 during this second. The CoM of the torso and arms is approximately 8 cm above joint RL4, i.e. adjusting 3.6 corresponds to moving the CoM approximately 0.005 m. This should be fast enough to keep the arms out of saturation. The mid-range controller for controlling the ZMP in the sagittal plane is implemented in Simulink as seen in Fig. 15.11. The sign of the gains are such, that arms and legs are moved in the intended direction.
15.3. TESTING THE DESIGNED CONTROLLERS
131
0.02 P gain legs K Ts Antiwindup z1 Integrator 0.60 I Gain == Satuation signal1 3 RA1 adjust 4 LA1 adjust Saturation Legs: 10 to 10 degrees1 1 RL4 adjust 2 LL2 adjust
Antiwindup signal
90 to 90 degrees1 1 err_z 1000 P Gain arms1 1 Change sign 90 to 90 degrees2
Figure 15.11: The Simulink implementation of the mid-range controller for adjusting the ZMP in the sagittal plane.
15.3 Testing the Designed Controllers

To give an indication of the designed controllers work as intended, two simple step tests are conducted. In the rst test, the robot is placed in zero position, i.e. all joint angles are given zero. The controller is then given a ZMP reference in the left foot, yref = 0.062 m and stepped back to the center between the feet, yref = 0.031 m. This is illustrated in Fig. 15.12a. Both reference values are given in {RL0 }-coordinates. The result of the test is seen in Fig. 15.13. The ZMP reaches the reference in approximately one second and settles. It is seen that the controller immediately gives a large signal to the arms, and the legs are adjusted smoothly afterwards until the arms are given zero again as intended in mid-ranging.
x y
RL0
RL0
x y
(a)
RL0
RL0
RL0
x z
RL0
RL0
z
end ZMP
RL0
initial ZMP reference: -0.062 m
end ZMP reference: -0.031 m
intial ZMP reference: -0.03 m
(b) reference: 0.01 m
Figure 15.12: The performed step test. In the rst test (a) the robot is stepped from a ZMP reference in the left foot to a reference in the center. In the second test, the robot is stepped from a ZMP reference behind the ankle joint, to a reference slightly in front of the ankle.
In the second test, the robot is again placed in zero position. The controller is then given a ZMP reference behind the ankle, zref = 0.03 m and stepped to a reference in front of the ankle, zref = 0.01 m. This is illustrated in Fig. 15.12b. The result of the test is seen in Fig. 15.14. The ZMP reaches the reference in approximately after 5 seconds and settles. It is seen that the controller immediately gives a large signal to the arms, and the legs are adjusted smoothly afterwards until the arms are given zero again as
132
0.02 in RL0 [m] 0.04 0.06 0.08 ZMP reference ZMP position 0 1 2 3 Time [s] 20 Angle [ ]
o
ZMP
0 20 40 Output to arms Output to legs 0 1 2 3 Time [s] 4 5 6 7
Figure 15.13: Results of the step response illustrated in Fig. 15.12a.
intended in mid-ranging.
0.02 zZMP in RL0 [m] 0 0.02 0.04 ZMP reference ZMP position 0 2 4 6 8 10 12 Time [s] 14 16 18 20 22
60 Angle [ o] 40 20 0 20 0 2 4 6 8 10 12 Time [s] 14 16 18 20 22 Output to arms Output to legs
Figure 15.14: Results of the step response illustrated in Fig. 15.12b.

The response of the sagittal controller is not as fast as the frontal plane controller. This is intended as the PoS in SSP is twice the size in the sagittal plane as in the frontal. Hence, control in the frontal plane should be more tight. In Fig. 15.14 it is seen, that when giving a step, the ZMP rst moves slightly in the wrong direction before moving toward the reference. This is due to arms creating a reaction force when acceleration out from the torso. From the responses it seems that the ZMP movement is mainly governed by moving the CoM as intended and not by accelerating inertias.
15.3. TESTING THE DESIGNED CONTROLLERS
133
Chapter Summary
In this chapter a controller for stabilizing the robot is developed. The idea of the controller is to add small adjustment to the joint angles provided by the o-line generated gait cycle trajectories. As a measure of stability of the biped robot, the ZMP is used, as maintaining the ZMP inside the PoS is a valid stability criterion for both stable stand, static walk and dynamic walk. As each o-line generated gait trajectory also includes the theoretically (desired) ZMP trajectory, this ZMP trajectory is used as reference. This should keep the robot stable, and make the robot walk as intended. To describe the ZMP location, it is chosen to switch between two coordinate systems:
Frame {RL0 } located in right foot is used to describe ZMP when in SSP-R Frame {LL5 } located in left foot is used to describe ZMP when in SSP-L
Thus, the ZMP is always described in a frame xed to the ground and the frames follow the motion of the robot. This yields an easy interpretation of the ZMP coordinates. When using frame {RL0 } and {LL5 }, only the y and z coordinates are of interest when considering the ZMP, as the x-axis is always vertical in the supporting foot. The ZMP-error is divided into two quantities, one for each direction: the error in the y -direction erry and the error in the z -direction err z . These are given as the dierence between the ZMP reference and the measured ZMP. Accordingly, the controller is divided into two, a frontal plane controller reducing the err y and a sagittal plane controller reducing err z . The developed control strategy is based on adjusting the ZMP position by moving the CoM of the robot. To adjust the ZMP, both the arms and legs can be used. As a result, mid-range controllers are implemented. The mid-range control strategy uses the arms directly to correct the ZMP-error, while the task of the legs is to prevent the arms from saturating. Thus, the arms react and afterwards the torso is displaced by the legs, so the arms can be lowered back toward the torso again. The controllers are implemented in Simulink and two tests are conducted on the robot, where the controllers are given a step in the ZMP reference. Both the frontal plane and sagittal plane controller is able to reach and settle at the reference, and the mid-range strategy works as intended. The arms reacts immediately to the error, and the legs displaces the torso smoothly afterwards so the arms are lowered back toward the torso. Thus, the controllers are ready to be tested against the requirements specied in Sec.3.3.
Chapter
16
Trajectory Supervisor
This chapter concerns the supervisor described in Ch. 11, see Fig. 16.1. It contains a description of the concepts and ideas behind a trajectory supervisor on a higher level of abstraction. Thus, what is presented here is not a nal design, and no supervisor is implemented in the controller.
Strain gauges
ZMP error
Servomotors
Figure 16.1: The block designed in this chapter and it's context in the controller.
The supervisor can choose between several trajectories. These trajectories contain data for dierent gaits that can be used when experiencing large sudden perturbations in dierent directions. These are illustrated in Fig. 16.2. The supervisor could distinguish between eight dierent directions when determining the direction of the perturbation. These extra trajectories are meant for cases where an external disturbance is so large, that the current trajectories will not be able to maintain balance. The decision to change trajectories could be based on threshold values of the disturbances. The accelerometer in the torso can be used to measure these disturbances. When no larger external perturbations are registered, the optimal trajectory will be used. One trajectory for each of the eight directions the supervisor can choose between will not be adequate, because the disturbance may not be of the same magnitude every time. It should thus be possible to use dierent trajectories, even when the perturbation has the same angle of attack. In Fig. 16.3 the concept is exemplied using the trajectory for LL5. To get a suciently low quantization error, four dierent trajectory sets for each direction of perturbation could be used. This means the supervisor has 32 dierent trajectories to choose between, when an external disturbance requires a trajectory change. 134
135
Figure 16.2: An illustration of the robot and the eight perturbation directions.
The change to another trajectory should be quick to avoid falling over, it should also be
Pertubation
LL5 y z x {GLOB} RL1
Direction of locomotion
Figure 16.3: Example of dierent trajectories available for cases where external
force is applied from the right. The solid line represents the optimal trajectory and the dashed line the alternatives.
done smoothly to minimize the un-modelled dynamic eects of such a quick movement. This blending of the two trajectories is created using a cubic spline interpolation between the two curves, as in the trajectory chapter, see 13.3 on page 106 for details. See Fig. 16.4 where an interpolation is shown. During the blending the trajectories might not be stable, as stability is not taken into account when interpolating. This is a reason for making the duration of the blending small. However the accelerations should not be too great either, which entails longer blending duration. The actual value will have to be determined experimentally. After using the alternative (and more stable in the situation) trajectory set for one step, the supervisor gradually changes back to the optimal trajectory. For this transition a special trajectory is used. Especially when the perturbation is parallel to the walking direction these transition trajectories are important. This case is illustrated in Fig. 16.5. After the longer step taken by LL5, the initial position for the next SSP, moving RL1 forward, is dierent from the one used to generate the optimal trajectory set. Thus using
136
CHAPTER 16. TRAJECTORY SUPERVISOR
ZMP position
Interpolation time
Time External perturbation
Figure 16.4: View of ZMP trajectory, for the optimal trajectory and trajectory
shift when an disturbance occur.
Pertubation LL5 y z x {GLOB} RL1 Direction of locomotion
Figure 16.5: Illustration of a case where the robot takes a longer step to keep
balance when pushed from behind. The solid line indicate the alternative trajectory for LL5/RL5, dashed lines indicate the original/optimal trajectory.
the optimal trajectory set from this point on could make the robot fall. A transition trajectory set is used in this situation. During the next step, this trajectory set moves the robot back to the initial posture, from where the optimal trajectory set can take over. Transition trajectories should be available to the supervisor for each of the alternative trajectories. This way, though the transition to the alternative set might be sudden when the perturbation occurs, the transition back to optimal walk will be seamless.
Part Summary
In this part, the development of the controller implemented on the robot is described. A controller based on o-line trajectory generation is developed and implemented. Trajectories based on a kinematic, Newtonian, and Lagrangian model are developed in Ch. 13. A genetic algorithm is used to develop the trajectories. This algorithm uses a performance function, in which several parameters are included, to gain an optimal trajectory for the robot. In Ch. 14 a Newtonian model is derived, enabling the CoP to be determined from forces acting on the sole of the foot, which is successfully veried experimentally. A method for determining the CoP during DSP is derived, which enables a comparison of the theoretically calculated CoP from the kinematic model with the measured CoP during DSP. A feedback controller is developed in Ch. 15, to make the robot more resilient to disturbances in a limited range. The developed control strategy is based on adjusting the CoP position by moving the CoM of the robot. To adjust the CoP, both the arms and legs are used. This is implemented using mid-range controllers. Furthermore a design of a controller that can change trajectories on-line is proposed. Compared to the controller implemented on the robot, this controller should be able to maintain balance, when larger perturbations occur.
137
Part V
System Test and Summary
In this closing part of the main report the results from the tests which are described in Ch. 3 and dened in App. B are presented. In the conclusion the main results in this report are emphasized. Afterwards the obtained results are discussed along with the project as a whole and the improvements which can be made to the biped robot in this project. Finally a closing statement is given to put this project into a greater perspective regarding the development of biped robotics at AAU.
Chapter
17
Acceptance Test
In this chapter the biped robot is tested against the requirements specied in Sec. 3.3. In order to test if the biped robot reacts as expected a comparison is made of the CoP calculated from strain gauge measurements and the CoP calculated in the Simulink model. This gives a good indication as to whether the robot follows the trajectories developed in Ch. 13. The robot is tested with a trajectory developed from the kinematic model and with a trajectory developed from the dynamic Newton-Euler model. Further more a controller is implemented but this is only tested with the kinematic model. The reason for this is that the impact from the controller is the same using either of the trajectories thus making the eect from the controller visible with only one graph. The tests are carried out under the same test conditions. This means that the tests are always carried out on a horizontal supporting surface. The results are documented with video clips which can be found on the enclosed CD [1]. The precision of the measurements from the strain gauges does not allow for verication of the CoP demands specied in Sec. 3.3 for test 4, 5, 7, and 8. Hence are these not comment further but it can be concluded that the biped robot fail on these demands.
Test Scenarios
The tests presented in this section are divided into four separate scenarios which each cover an area of the agility of the biped robot. The test scenarios are arranged such that the more simple tests are covered rst, i.e. test scenarios increase in complexity. As not every test can be veried by complying with some exact value, some of the tests are simply evaluated with a subjective opinion of this group with a passed or not passed. All the results are presented in the following section including the most important graphs. The test scenarios are discussed one by one and the results are shown in Tab. 17.1. The graphs showing the test information is shown at the end of this section. 140
141
Test Scenario One

The tests in the rst test scenario are made to validate if the robot can stand and to validate if the robot follows the trajectories, with feedback control disabled and no external disturbances.
Results Test One tests the robots ability to stand. This test is passed as the robot stands
without falling.
test is both veried through a video clip, see [10] and with a graph showing the CoP position. The topmost graph in Fig. 17.1 and in Fig. 17.2 shows the test with the static trajectory without a controller. The step length can be seen in Fig. 17.2 which shows the CoP in the x-direction. The step length is the maximum dierence in CoP position which is determined to be 12.5 cm. As it can be seen on the video clip the robot takes approximately 2.6 seconds per step. This leads to a walking speed of 4.8 cm/s. The lifting height of the feet has been determined to be 1 mm by inspecting the video clip. Due to the low walking speed of the robot the dynamics of the system is insignicant, making the CoM a good approximation of the CoP. As the robot does not fall during a gait cycle, the CoP, which is coinciding with the ZMP, is by denition inside the PoS. By examining the topmost graph in Fig. 17.2 closer it shows that a step with the left foot is faster than a step with the right foot. The CoP position in y -direction is more coincident.
CoP Test with Static Walk without Controller in Ydirection 0.1 CoP Position [m] 0.05 0 0.05
Test Two investigates the static walk of the biped robot without a controller. This
10
12
14
16
CoP Test with Static Walk and Controller in Ydirection 0.1 CoP Position [m] 0.05 0 0.05
8 Time [s]
10
12
14
16
Figure 17.1: CoP test while walking static with reference to y in the global frame. The solid line is the measured CoP, and the dotted line is the CoP from the model.
Test Three investigates the dynamic walk of the biped robot without a controller.
This test is both veried through a video clip, see [11] and through a graph showing the CoP position, see Fig. 17.3. First of all the step length is determined from investigating the CoP in the x-direction showed in Fig. 17.3. The step length is determined to be
142
CHAPTER 17. ACCEPTANCE TEST
CoP Test with Static Walk without Controller in Xdirection 0.1 CoP Position [m] 0.05 0 0.05 0.1 0 2 4 6 8 10 12 14 16
CoP Test with Static Walk and Controller in Xdirection 0.1 CoP Position [m] 0.05 0 0.05 0.1 0 2 4 6 8 Time [s] 10 12 14 16
Figure 17.2: CoP test while walking static with reference to x in the global frame.
The solid line is the measured CoP, and the dottet line is the model CoP.
approximately 12.5 cm. From the video clip is has been determined that the robot takes around 2.2 seconds per step. This leads to a walking speed of 5.6 cm/s. The maximum foot lift has been determined to be 2 mm. With the same argumentation as in the previous test case ZMP is by denition inside the PoS as the robot does not fall during a gait cycle. It is seen bottommost in Fig. 17.3, that the CoP is delayed with respect to the estimated CoP in the x-direction when taking a step with the left foot but it is faster when taking a step with the right foot. In the y -direction CoP position is more coincident with the theoretical CoP.
Discussion of Test Scenario One

The tests in this scenario are partially passed. The scaled requirement to step length and foot lift is satised but the requirement regarding speed is not satised. The reason for the low speed has its origin in the genetic algorithm, see Ch. 13 and will not be discussed here. According to the trajectory the robot should lift the feet higher than experienced in the tests. The problem is that the servomotors are not strong enough to deliver the required torque to lift the feet that high. Further more backlash in the servomotors result in a lower lifting height. The dierences in the CoP positions could originate from several sources e.g. slack in the robot and measurement errors from the strain gauges. However, the extremity values are approximately the same and the curve of the CoP is also approximately the same.
Test Scenario Two

The tests in the second test scenario are made to validate if the developed feedback controller stabilizes the robot without external disturbance.
Test Four is veried through a video clip, see [12] and through the bottommost graphs
in Fig. 17.1 and Fig. 17.2 showing the CoP position. The robot still manages to walk with the controller applied to the system and by investigating Fig. 17.1 the CoP does
143
CoP Test with Dynamic Walk in the Ydirection 0.1 CoP Position [m] 0.05 0 0.05
10
12
14
CoP Test with Dynamic Walk in the Xdirection 0.1 CoP position [m] 0.05 0 0.05 0.1 0 2 4 6 8 Time [s] 10 12 14
Figure 17.3: CoP test while walking dynamic, with reference to x and y in the
global frame. The solid line is the measured CoP, and the dottet line is the model CoP.
seem to be closer to the estimated CoP when decreasing but suer from overshoot when increasing.
Test Five is veried through a video clip, see [13]. The robot is still capable of walking
with the controller but CoP measurement is not presented as the result is the exact same as for the static walk.
Discussion of Test Scenario Two

The tests in test scenario two are partly passed. The robot is still capable of walking with the feedback controller implemented but inspecting the CoP reveals that no improvement has been achieved. One of the problems are the delay from the disturbance to the reaction from the controller. This delay limit the eciency of the controller against the fast disturbances it experiences during gait.
Test Scenario Three

The tests in this test scenario are made to validate if the implemented feedback controller is able to suppress external disturbances applied to the robot.
Test Six is veried through a video clip, see [14] and through a graph in Fig. 17.4
showing the CoP position. The graph shows the test which is initialized with the robot balancing on the left foot and hence the CoP reference at 0 cm. After 7 seconds the CoP reference is stepped to right between the feet equal to approximately -3 cm, as seen in Fig. 17.4. As seen the CoP of the robot is stabilizing at 0 cm until applying the step. From the gure it is determined that the rise time is around 0.5 second with an overshoot of 33% and a settling time at around 3 seconds. The biped robot is also able to maintain balance when external disturbances up to 4 N are applied.
144
CHAPTER 17. ACCEPTANCE TEST
Double Support Phase Test with Controller 0.02 0.01 0 CoP Position [m] 0.01 0.02 0.03 0.04 0.05
5 Time [s]
10
15
Figure 17.4: Step test for double support with controller. The CoP position is seen
from LL5.
Test Seven is only veried through a video clip, see [15] with no graphs representing the
results. The robot is able to keep walking although exposed to external disturbances. The robot reacts to direct disturbances as it pushes the arms and tilt the torso in the opposite direction of the disturbance.
Test Eight is only veried through a video clip, see [16] with no graphs representing the
results. The result is the same as in Test Seven. The robot reacts to the disturbances without falling.
Discussion of Test Scenario Three

The controller is able to suppress some disturbances, by correcting the ZMP toward the desired reference when the robot is walking. There is not made any specic requirements to the controller.
Test Scenario Four

The test in the last test scenario is made for demonstration purpose.
Test Nine is only veried through a video clip, see [17] with no graphs representing
the results. In this test all the wires are disconnected, the battery is inserted and the trajectories are uploaded to the robot. As seen in the video clip the robot is able to play back the trajectories and thus walk.
Discussion of Test Scenario Four

The robot can be supplied by a single battery, and walk for about ve minutes before the battery has to be charged. It is not possible to add the feedback control, when the robot is walking without wires, but it is useful for demonstration purpose.
145
Result Summation
To give an overview of the achieved results they are presented in a tabular. Test Case One Two Three Four Five Six Seven Eight Nine Speed X Not Passed Not Passed Not Passed Not Passed X Not Passed Not Passed X Step Length X Passed Passed Passed Passed X Passed Passed X Foot Lift X Passed Passed Passed Passed X Passed Passed X CoM/ZMP Criteria X Passed Passed Not Passed Not Passed Passed Not Passed Not Passed Passed
Table 17.1: Table showing the results from the acceptance test. A "X" denote when no verication test is performed.
Chapter
18
Conclusion
The main purpose of this project was to make the biped robot walk. This goal should be accomplished through modelling the robot and using these models in the trajectory generation. Additionally a controller should be implemented to stabilize the robot under external disturbances. As this project is a follow-up certain hardware improvements were proposed in the last project in order to achieve better performance with the robot. The main improvement is a new foot design with strain gauges as sensors. This resulted in a much more accurate CoP calculation. Further more the implementation of a microcontroller enabled an autonomous gait completely without any wires to the surroundings. A model of the servomotors is developed. This model turned out to lack precision and, due to time constraints, it is not developed further and thus not used in this project. Additionally a kinematic model is developed and veried in 3D Studio Max. The model is able to accurately describe the position and rotation of every link in the robot. The Newton-Euler model describes the dynamics using torques and forces acting in every link of the robot. This approach leads to a very precise but very complex model suitable for o-line simulation where accuracy is valued over computation speed. The Newton-Euler model is veried to a satisfactory degree using measurement from the physical robot. In order to create a less complex model, which could be used in on-line model based controllers, the Lagrangian model is derived. Simplications to this model reduce it to a simple static model as the dynamics can be disregarded as a result of the low gait speed of the robot. A comparison with the Newton-Euler model revealed that the two models produced almost identical results. This leads to the conclusion that the dynamics can be disregarded with only a minor loss of accuracy. An inverse kinematic model is derived in order to convert the desired position of the robot's limbs into actual angles of the servomotors. All these models are used in the genetic trajectory generation algorithm to create the trajectories used to make the robot walk. The application of a genetic algorithm to the trajectory generation problem proves an eective and exible solution. Two trajectories are generated: One static gait and one dynamic gait.
146
147
Finally a feedback controller is developed using CoP/ZMP measurements as the input. The CoM is moved to keep ZMP at a reference determined o-line through the trajectory generation. This ZMP controller implements mid-ranging to move the CoM using both the arms and legs. The principle of mid-ranging proved a simple and eective way to solve the problem of having two actuators that control one system output. The acceptance test revealed that the robot was able to walk with both the static and the dynamic trajectory. Both gaits failed the gait speed requirements, but complied with the requirements regarding foot lift, step length and half of the tests concerning the ZMP criteria. Implementing the controller did not stabilize the robot further during gait. In stand the controller did perform better. Test shows that using the arms to control, do not change CoP enough to maintain balance, when external disturbance occur. Even though the robot never achieved real human-like dynamic heel-strike-toe-o gait, the project is seen as a success as the models are veried and used in the trajectory generation. The controller did not stabilize the robot further but the principle with a mid-range controller showed to work and further improvement to the mechanic of the robot and tuning of the controller is seen as a possibility.
Chapter
19
Discussion
This chapter will discuss the results given in the conclusion by evaluating whether the methods used in this report are applicable for modelling and controlling a biped robot. It will discuss the choice of hardware and also provide a brief summary of the main problems encountered during the development. Further improvements to the robot will also be discussed here. Due to their importance in control aspects, the hardware of primary concern is the feet of the robot which have been improved in several ways in this project. In order to determine the CoP, the use of strain gauges as force sensors has proven a viable alternative to the force sensing resistors. The redesign of the feet resulted in two things. First of all the new design gives a much better signal from the strain gauges and secondly provides more grip when walking. However, further improvement is needed to make the robot able to walk dynamically. The large area of the feet makes the robot very stable statically, even without a controller. As a result, a stability controller has limited use in the robot. Smaller feet will make the robot less stable but help in gaining the heelstrike-toe-o capability required for true dynamic walk. The general hardware problem of achieving faster walking speeds with the pre-AAUBOT is the fact that all the links are entirely rigid. This causes a jolt through the robot as the feet touch the ground. At low walking speeds, these eects are minor, but as the speed increases it causes the robot to become increasingly unstable until it ultimately falls over. It can be compared to a human walking on stilts. Fitting mechanical dampers may prove very dicult. Instead, the damping eects could be obtained by modifying the servomotors to allow control of the generated torque. This way, by applying less torque to some joints in the impact phase the servomotors would move with the stress and absorb the jolt. But with the limited amount of control of the servomotors, it is estimated that, until better feet and/or actuators are tted to the robot, this rigidness will be a limit to the walking speed. Generally, the frame and the servomotors introduce much slack in the robot, which is inherently dicult to model and control. By locating the parts introducing the slack and replacing them with improved hardware, it is assumed that the models of the robot would be more accurate and enable better control. The modelling of the servomotors resulted in a purely kinematic model, which did not take the load torques into account. This resulted in a model in which the servomotors could theoretically have unlimited torque. Tests during a gait cycle and simulation show
148
149
that the load torques at least surpasses the maximal torque delivered by the HS-645MG. To make a model which can be used for o-line simulations of controllers an improved servomotor model must be developed which take the load torque into account. The derived dynamic model based on Newton-Euler iterations has proven quite precise and describe all the forces and torques working on the pre-AAUBOT. This makes it very suited for use in simulations, trajectory generation, and controller tuning. It is, however, comprehensive and time consuming to calculate and hence not a valid option for on-line model based control. The Lagrangian model deals with this problem by simplifying the expressions and only determines the essential torques. Furthermore it is determined that the dynamic have a small impact on the torque on the biped robot. This justify that the dynamic parts of the model are removed, making the model even simpler. Despite the achieved simplicity and precision for this model, it have not been used for on-line simulation, due to time constraint. The SSP model derived in this project can found a base for on-line model based control, due to the simplicity of the achieved expressions. The trajectory generation is based on a genetic algorithm. This approach yields an optimal trajectory according to some performance function. This method is both ecient in the sense that it optimises the gait, and exible in the sense that dierent aspects can be considered for optimality by designing the performance function accordingly. The practical application of the algorithm, though it required some resources to tune satisfactorily, proved eective. Thus, this report conrms the feasibility of the application of a genetic algorithm for trajectory generation. As the models are based on single support, so is the case for trajectory generation. Thus a trajectory for a complete gait cycle is made by mirroring the SSP and inserting it after the rst SSP. This entails insecurity in the transition from one SSP to the next. In an area in time around this transition the robot will, in practise, have two feet on the ground, i.e. be in double support. As this is not how the model interprets this situation, the calculated ZMP will not conform to reality in the transition. However the ZMP is set to be in between the legs at the time of the transition. That is, the transition occurs at the most stable point in the gait cycle. Hence the eects of the inaccuracy in the models at the transition do not lead to instability. The lack of a double support model does however bring other limitations. For the robot to walk truly dynamically, it should, in the transition phase, use the previously supporting leg to push the CoM towards the new supporting foot. This eect can not be modelled using the SSP model. The eects of the push are so signicant, that, without a DSP model, it will be very dicult to implement and still keep the robot stable. A DSP model is therefore advantageous with respect to the implementation of the described push-o feature. The heel-strike-toe-o feature would also require such a DSP model. Both these gait features would be desirable in an anthropomorphic robot, and thus a DSP model would be a valuable addition to the models developed in this report. Some possible issues with the feedback controller is the long delay in the control loop and the inability to precisely control or measure the CoP/ZMP. The delay is mainly caused by the slow lters needed on the input signals to make up for the imprecision in the measurements. The backlash in the servomotors is the main contributor to the imprecision in actuators, and thus in the CoM movement. A more advanced controller could use the gyroscope in the torso for sensor fusion with the accelerometer in the torso. This sensor fusion could give a more exact tilt of the torso which could be used to control the tilt of the robot. An area which should also be investigated further is the real time aspect of the mi-
150
CHAPTER 19. DISCUSSION
crocontroller. The microcontroller is programmed with IsoMaxTM and it has not been investigated fully if any problems arise from using this language which is an interpreted language. It is possible to ash the microcontroller and program it with C via a JTAC cable and maybe gain some speed in processing the programs and enable a feedback controller to be implemented on the microcontroller.
Chapter
20
Closing Statement
The reasons for this project are two-sided. As mentioned in the beginning of this report Aalborg University is currently working on a human-scale biped robot. In order to introduce students to the concept of modelling and controlling biped robots, this smallscale robot project was started. The main objective for this group was to model and control the biped robot and to make the robot walk. Furthermore, the robot is properly also to be used in later projects at Aalborg University, so the results from this project make the foundation for even better control and more human-like gait with the robot. The results are also to be used in a wider perspective. The knowledge gained from this project can be transferred into modelling and controlling a human-scale biped robot, as the techniques for modelling and controlling a small-scale robot are the same as when dealing with large-scale robots. Although the procedure when modelling and developing controllers are the same, some issues need to be treated dierently. The Lagrangian model was found most suited when on-line calculations are necessary, but the model can not be used directly in the present form, when dealing with a large-scale robot. One issue is the dynamic part of the model which has a much greater inuence on a large-scale robot and should of course be included in the model. Further more a model describing the double support phase is a necessity to achieve human-like dynamic walk. Also the knowledge from the controller development is useful when dealing with large-scale robots. Although the controller did not improve the performance of the biped robot the principle and design strategy was found suitable. With a faster controller and better sensor measurements great improvements are possible. To summarize, the knowledge and experience gained from this project is a sound foundation for the development of large-scale biped robots.
151
Bibliography
[Brennan, 1999] Brennan, S. N. (1999). Modeling and control issues associated with scaled vehicles. [Craig, 2003] Craig, J. J. (2003). Introduction to Robotics. Pearson Prentice Hall, 3rd edition. ISBN 0-13-123629-6. [Cuevas et al., 2005] Cuevas, E. V., Zaldivar, D., and Rojas, R. (2005). Bipedal robot description. Control and Applications, 460. [Fraden, 2004] Fraden, J. (2004). "Handbook of Modern Sensors". Springer-Verlag, third edition. [Hitec, 2006] Hitec (2006). http://www.hitecrcd.com/servos/list. Hitec RCD USA. [Honda, 2007] Honda (2007). http://world.honda.com/asimo/history/. Accessed 2903-2007. [Huang et al., 2001] Huang, Q., Yokoi, K., Kajita, S., Kaneko, K., Arai, H., Koyachi, N., and Tanie, K. (2001). Planning walking patterns for a biped robot. IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION, 17:280289. [Humanoid Robotics Institute, 2007] Humanoid Robotics Institute, W. U. (2007). http://www.eu-japanfest.org/english/program/12/lille/robot.html. Accessed 29-032007. [Katic and Vukobratovic, 2003] Katic, D. and Vukobratovic, M. (2003). Survey of intelligent control techniques for humanoid robots. Journal of Intelligent and Robotic Systems, 37:117 141. [Kim et al., 2005] Kim, D., Kim, N.-H., Seo, S.-J., , and Park, G.-T. (2005). Fuzzy modeling of zero moment point trajectory for a biped walking robot. IEEE Proceedings - Control Theory and Applications, 152:411426. [Komura et al., 2005] Komura, T., Leung, H., Kudoh, S., and Kuner, J. (2005). A feedback controller for biped humanoids that can counteract large perturbations during gait. Proceedings of the 2005 IEEE International Conference on Robotics and Automation. [Kudoh and Komura, 2003] Kudoh, S. and Komura, T. (2003). C2 continuous gaitpattern generation for biped robots. Intelligent Robots and Systems, IEEE/RSJ International Conference, 2:1135 1140. 152
BIBLIOGRAPHY
153
[Pickel, 2003] Pickel, A. (2003). "Control for a Biped Robot with Minimal Number of Actuators". None. [Popovic and Sinkjaer, 2000] Popovic, D. and Sinkjaer, T. (2000). Control of Movement for the Physically Disabled. Springer-Verlag. ISBN 1-85233-279-4. [rts et al., 2006] rts, P. F., Chirstensen, J., Nielsen, J. L., Svendsen, M. S., Svendstrup, M. S., and Winther, K. (2006). Modelling and Control of a Biped Robot. none. [Sardin and Bessonnet, 2004] Sardin, P. and Bessonnet, G. (2004). Forces acting on a biped robot. center of pressure-zero moment point. Systems, Man and Cybernetics, Part A, IEEE Transactions, 34:630 637. [Serway and Jewett, 2004] Serway, R. A. and Jewett, J. W. (2004). Physics for Scientist and Engineers. David Harris, 6th edition. ISBN 0-534-40949-0. [Inernational Organization for Standardization, 1994] Inernational Organization for Standardization (1994). ISO 8373:1994 Manipulation Industrial Robots. [Wirhed, 2002] Wirhed, R. (2002). "Anatomi og bevgelsesllre i idrt". Samsprk Forlags AB. [Wisniewski, 2007] Wisniewski, R. (2007). Modelling of mechanical systems ii.
Appended Documents
[1] Video clips of the Biped Robot. CD-ROM path: \Acceptance Test Videos\ [2] The written IsoMax Code for the microcontroller. CD-ROM path: \IsoMax Code\ [3] 3D Studio Max bone model of the biped robot and a Matlab model of the kinematics. These are used for verication of the kinematics. CD-ROM path: \Kinematic Model\ [4] Implementation of the calculation of the CoM of the biped robot in Matlab. CD-ROM path: \Matlab\calc_com.m [5] Implementation of the Newtonian model in Simulink. CD-ROM path: \Matlab\Newtonian\newtonian_model.mdl [6] A Matlab script to verify the expressions for rl1 and rl2 . CD-ROM path: \Matlab\Lagrange\ [7] Implementation of the inverse kinematic model in Matlab. CD-ROM path: \Matlab\Trajectories\inverse_kinematic.m [8] Source code and binary to Rhinoceros plugin. CD-ROM path: \Plugin\ [9] Matlab les that implements the generic algorithm. CD-ROM path: \Matlab\trajectories\generate_trajectories.m [10] Walking static with no controller. CD-ROM path: \Acceptance Test Videos\Test Two\ [11] Walking dynamic with no controller. CD-ROM path: \Acceptance Test Videos\Test Three\ [12] Walking static with controller. CD-ROM path: \Acceptance Test Videos\Test Four\ [13] Walking dynamic with controller. CD-ROM path: \Acceptance Test Videos\Test Five\ [14] Double support phase with controller and disturbances. CD-ROM path: \Acceptance Test Videos\Test Six\ 154
APPENDED DOCUMENTS
[15] Walking static with controller and disturbances. CD-ROM path: \Acceptance Test Videos\Test Seven\ [16] Walking dynamic with controller and disturbances. CD-ROM path: \Acceptance Test Videos\Test Eight\ [17] Walking without any wires. CD-ROM path: \Acceptance Test Videos\Test Nine\
155
[18] Data sheet for the MMA7250Q accelerometer CD-ROM path: \Datasheets\MMA7260Q_large_disturbance_accelerometer.pdf [19] Data sheet for the MCP3208 ADC. CD-ROM path: \Datasheets\mcp3208.mgp
Part VI
Appendix
Appendix
Human Gait
During human gait, the angles of the dierent joints are usually within some boundaries. This knowledge can be used to ease the modelling of the biped by using approximations as the small angle approximations. To determine these boundaries, an analysis of human gait has been carried out. To emphasize the important data for the Lagrangian model it is dierent from the one in [rts et al., 2006] . However, the fraction of the DSP phase found in [rts et al., 2006] to be 8% is used. The data covers half a gait cycle because this will provide all the information needed, since e.g. rl2 and ll4 gives the angles in the sagittal plane through a whole cycle. The reason why they do not exactly start and stop at each others start and stop points is that each half gait cycle is not exactly alike. But as seen in the graphs they are close to be alike and hence, one half gait cycle can represent the gait. To get more precise values of the angles an average of more whole cycles (and dierent persons) should be taken. For the purpose of modelling simplications it is sucient to get an of the span of the angles and hence the data from one half gait cycle is sucient. The numerical value of the angles in each joint from the human gait does not represent the angles in the joints on the biped for two reasons. Firstly, the joints are placed dierently on the biped than on a human, and secondly the reecting balls on the human test person are not placed on a vertical line. The second part results in some osets on the angles measured on the human body, and no information of these osets are given by the group that carried out the experiment. Hence, it is mainly the angle variations which can be derived from the data. The limits within the angles vary are given in degrees in favour of radians as angles in degrees make it more intuitive to visualize the bending of the joints. In the calculations, the angles will always be in radians. First, the angels in the sagittal plane in the legs are examined. By only using the x and z component from the data to nd a vector in each limb (foot, shin, thigh, and torso) the angles in the sagittal plane are found. They are shown in graph A.1. To get rid of redundant information in the Lagrangian model, the sums of these angles are of particular interest. These are shown on graph A.2 and A.3.
158
159
50 servoangle 0 50 20
LL4 RL2
40
60
80
100
120
140
160
50 servoangle 0 50 20
LL3 RL3
40
60
80
100
120
140
160
50 servoangle 0 50 20 LL2 RL4 40 60 80 100 120 Datapoints for a half gait cycle 140 160
Figure A.1: These three graphs shows the angels in each joint of the legs in the
saggital plane.
servoangle
50 0 50 20
LL2
40 +
60
80
100
120
140
160
servoangle
50 0 50 20
LL2
LL3
40 + +
60
80
100
120
140
160
servoangle
50 0 50 20
LL2
LL3
LL4
40
60 80 100 120 Datapoints for a half gait cycle
140
160
Figure A.2: These three graphs shows the sums of the angels in the swing leg, in the saggital plane. A zero lines is also plotted to make it easier to see when the angles change sign.
160
APPENDIX A. HUMAN GAIT
LL2 RL2+RL3+RL4 50 servoangle 0 50 20 40 60 80 100 120

LL2
RL2
RL3
RL4
LL2
140 +
LL3
160
RL2+RL3+RL4 50 servoangle 0 50 20 40 60 80 100 120 140 160 RL2+RL3+RL4 LL2LL3
LL2+LL3+LL4 RL2+RL3+RL4 50 servoangle 0 50 20 40 60 80 100 120 Datapoints for a half gait cycle 140 160 RL2+RL3+RL4 LL2LL3LL4
Figure A.3: These three graphs show the sums of angels in each joint of the legs in
the sagittal plane.
From these graphs it is seen that the angles vary within the following limits given in degrees.
rl2 rl2 + rl3 rl2 + rl3 + rl4 ll2 ll2 + ll3 ll2 + ll3 + ll4 ll2 rl2 rl3 rl4 ll2 + ll3 rl2 rl3 rl4 ll2 + ll3 + ll4 rl2 rl3 rl4
: 15 : 26 : 20 : 12 : 52 : 70
to to to to to to
10 4 15 22 25 10
: 25 to 40 : 45 to 32 : 65
to
22
For the Lagrangian model, small angle approximations can be used on the four rst expressions as these lies within 26 degrees Furthermore, it is seen from the graph A.3 that the angles in the supporting leg are considerably smaller than the angles in the swinging leg, which allows for the following
161
approximation.
ll2 + ll3 rl2 rl3 rl4 ll2 + ll3 ll2 + ll3 + ll4 rl2 rl3 rl4 ll2 + ll3 + ll4
The angles in the frontal plane are found in the same way using the x and y coordinates. The angles are seen on graph A.4 and A.5.
RL1 20 10 0 servoangle in degrees
20
40
60
80 RL5
100
120
140
160
180
10 15 20
20
40
60
80
100 +
RL5
120
140
160
180
RL1
6 8 10
20
40
140
160
180
Figure A.4: The upper two graphs shows the angels in the supporting leg in the
frontal plane, and the third show the sum of these two.
LL1 10 0 10 servoangle in degrees
20
40
60
80 LL5
100
120
140
160
180
20 10 0
20
40
60
RL1
80 +
RL5
100 +
120
140
160
180
LL1
0 5 10
20
40
140
160
180
Figure A.5: These two graphs show the angels in the swing leg in the frontal plane.
162
Here the following variations are found:
APPENDIX A. HUMAN GAIT
rl1 rl5 ll1 ll5 rl1 + rl5
: 4.5 : 10.7 : 1.2 : 4.4 : 6.2
to to to to to to
10.5 18.7 8.1 19.1 8. 8 0
rl1 + rl5 + ll1 : 7.4
Hence, small approximations are very valid for the angles in the legs in the frontal plane. Furthermore, for the angels in the supporting leg it does seem reasonable to approximate a sine to the angles in the supporting leg with a zero since their values are lower than 0.1 rad, in some relations where it makes them insignicant compared to other parts not multiplied with this angle. It is also noticed that the sum of rl1 and rl5 has a variation close to 0 rad and hence keeps the torso in upright position, as expected. The angles in the arms varies as shown on graph A.6 and A.7.
RA1 servoangle in degrees 10
20
40
60
80 RA2
100
120
140
160
180
servoangle in degrees
22 20 18 16 14
20
40
140
160
180
Figure A.6: These two graphs show the angels in the right arm. ra1 is in the
sagittal plane and ra2 is in the frontal plane.
From these graphs the variations are as follows
ra1 :
1.6
to
ra2 : 16.2 to 21.7 la1 : 5.7 to 9.4 la2 : 15.2 to 19.4

These variations are small enough to apply the same small angle approximations as for the angles in the legs in the frontal plane. The angles relative large values are considered due to the previous mentioned unknown oset. When looking at the video recordings of the human gait it seems reasonable to assume that ra2 varies within 0 to 5.5 degrees from vertical instead of the 16.2 - 21.7 the data gives.
163
LA1 servoangle in degrees 10
20
40
60
80 LA2
100
120
140
160
180
servoangle in degrees
20
18
16
14
20
40
140
160
180
Figure A.7: These two graphs show the angels in the left arm. la1 is in the sagittal plane and la2 is in the frontal plane.
Appendix
Test Scenario Description

In this appendix all the test scenarios are described in details along with test objectives which was set up in Ch.3. These test scenarios are carried out to document the achieved abilities of the biped robot. This is done by describing which tests are done and for what purpose. All the tests are carried out on a horizontal surface as the models and controllers are based on this condition. The tests are recorded using a camcorder to document the results and the video clips can be found on the enclosed CD [1]. Each test scenario covers an area of the objectives of the biped robot. The test scenarios are arranged such that the simple objectives are tested rst, and then are the more complicated objectives tested. This order should clarify how well the robot accomplish the objectives and hence whether it complies with the specication of requirements set in Ch. 3.
Test Scenario One

The tests in the rst test scenario are made to validate if the robot can stand and to validate if the robot follows the trajectories, with feedback control disabled and no external disturbances.
Test One
Stand stable with no external disturbance and with the controller disabled.
The biped robot is given a reference signal consisting of constant zeros. This test is carried out to determine the wobble of the servomotors, the interfacing between the servomotors and the microcontroller. The robot is standing in zero position when power is applied. A picture of zero position is found in Fig. B.1
Test Two
Walk static with no external disturbance and with the controller disabled.
164
165
Figure B.1: Zero position of the robot.

The biped robot is given a trajectory for static walk and has to walk one meter without any disturbances or any feedback controller. The speed and gait cycle length is setup from human scaled considerations. Using an average height of 180 cm for a human and a biped robot height of 29 cm gives a scaling factor of approximately 0.16. The gait speed of a human is approximately 5 km/h when the gait is dynamic. It is assumed that the speed of static walk is a fourth of the dynamic walk. This gives a static walking speed of:
vstatic =
5 km/h 100000 cm 29 cm 5.6 cm/s 4 3600 sec 180 cm
The step length of a human is approximately 72 cm scaling this down to the robot gives a static step length of:
lstep = 72 cm
29 cm 11.6 cm 180 cm
An exact measure for the feet lift for a human gait has not been obtained, and hence is the only demand regarding the foot lift that the swing foot must be lifted completely from the ground in the swing phase. At last must the CoM lie within the PoS at all time for the walk to be stable. These requirements can me summarized to the following: 1. Minimum speed of 0.2 km/h 5.6 cm/s. 2. Minimum walking cycle length of 11.6 cm. 3. The foot must leave the ground. 4. The CoP must be within the PoS during the whole gait cycle. This test is carried out to validate that the trajectory is able to make the biped robot walk static. This is done by examine the CoP output from the sensors in the feet. This test will also show whether the servomotors are strong enough to hold the biped robot in the right positions during a gait.
166
APPENDIX B. TEST SCENARIO DESCRIPTION
Test Three
Walk pseudo-dynamic with no external disturbance and with the controller disabled.
The biped robot is given a trajectory for pseudo-dynamic walk and has to walk one meter without any disturbances or any controller. The same considerations and scaling factors from the previous test is used again giving the following requirements: 1. Minimum speed of 0.8 km/h 22.4 cm/s. 2. Minimum walking cycle of 11.6 cm. 3. The foot must leave the ground. 4. The CoP must be within the PoS during the whole gait cycle. This test is carried out to validate that the trajectories are able to make the biped robot walk pseudo-dynamic. This is done by examine the CoP output from the sensors in the feet.
Test Scenario Two

The tests in the second test scenario are made to validate if the developed feedback controller stabilizes the robot without external disturbance.
Test Four
Walk static with no external disturbance and with the controller enabled.
This test is done to verify that the controller does not destabilize the static trajectory. In this test there are no external disturbances. 1. The CoP must be within 0.3 cm of the CoP specied for the trajectory. This test is carried out to clarify the controllers aect on the static gait of the biped robot. This is done by comparing the CoP output with that from test two and examine that it is within the requirements.
Test Five
Walk pseudo-dynamic with no external disturbance and with the controller enabled.
This test is done to determine the objectives of the biped robot with no external disturbances and the controller is enabled in pseudo-dynamic walk. 1. The CoP must be within 0.3cm of the CoP specied for the trajectory. This test is carried out to clarify the controllers aect on the pseudo-dynamic gait of the biped robot. This is done by comparing the Cop output with that from test three and examine that it is within the requirements.
167
Test Scenario Three

The tests in this test scenario are made to validate if the implemented feedback controller is able to suppress external disturbances applied to the robot.
Test Six
Stand stable with external disturbance and with the controller enabled.
The biped robot is given a reference signal consisting of constant zeros and the controller is enabled. The biped robot is then aected with a disturbance in form of a force in each direction in the sagittal plane with the attack point at the center of the torso. The same force is applied from each direction in the frontal plane with attack point at each shoulder. The disturbances are approximately 2-4 Newton. The forces are applied rst as a push and then gradually over a period of approximately 3 seconds. During each force test of the biped robot is the CoP recorded from the strange gauges on the feet, giving the following requirements: 1. The CoP must be held within PoS when applied disturbances around 2-4 N. This test is carried out to determine the controller's ability to maintain CoP within the PoS.
Test Seven
Walk static with external disturbance and with the controller enabled.
This is carried out as test four but with external disturbances aecting the biped robot whilst walking static. The disturbances are created as described in the previous test. This leads to the following requirements: 1. The CoP must be within 0.5 cm of the CoP specied for the trajectory. This test is carried out to clarify the controller capability to suppress external disturbances aecting the biped robot during static walk. This is done by comparing the CoP output with the one from test two and examine that it is within the requirements
Test Eight
This is carried out as test ve with external disturbances aecting the biped robot whilst walking pseudo-dynamic. The disturbances are created as described in the previous tests. This leads to the following requirements: 1. The CoP must be within 0.5 cm of the CoP specied for the trajectory. This test is carried out to clarify the controller capability to suppress external disturbances aecting the biped robot during pseudo-dynamic walk. This is done by comparing the CoP output with the one from test three and examine that it is within the requirements.
168
APPENDIX B. TEST SCENARIO DESCRIPTION
Test Scenario Four

The test in the last test scenario is made for demonstration purpose.
Test Nine
Walk autonomous without any wires attached.
This test scenario covers the tests which are to determine the biped robots abilities to walk autonomous without any wires connected to the biped robot. Tests one to three are repeated with batteries mounted and no wires connected to the biped robot. The requirements for these three tests are the same as described in the respective tests. These tests are carried out to verify that the biped robot can stand and walk both static and pseudo-dynamic using its on-board power supply and microcontroller. The CoP is compared to that of test two and three to show how the changes made by mounting the batteries and using the microcontroller aect the biped robot.
Summary of the Test Scenarios

Test scenario one reveals the biped robots capabilities to stand and walk both static and pseudo-dynamic. In this test scenario no disturbances are applied to the biped robot and no controllers are enabled. Test scenario two reveals the controller's capabilities to hold the CoP at a required value and thus stabilize the biped robot while walking both static and pseudo-dynamic. Test scenario three reveals the controller's capabilities to suppress external disturbances both when standing and when walking. Test scenario four reveals if the biped robot is capable of standing and walking when only using the batteries and the on-board microcontroller.
Appendix
CoP Measurement Test

This appendix describes the test conducted to verify if the position of the CoP can be determined using the pressure sensors mounted on the feet of the robot. The relations for nding the CoP in SSP phase using these pressured measurements are found in Ch. 9. To verify the relations this following test was conducted, where known CoP positions were applied to one foot and compared to the measured CoP positions. The known CoP positions were by placing known weights at the foot in known positions.
Test Description
In the test, the right foot of the biped was dismounted from the robot and instead a test frame consisting of two crossed bars was attached to the ankle servomotor. This is depicted in Fig. C.1. As a result, the foot could be given a known CoP by placing weights on the bars as seen in Fig. C.2. The servo was applied power and a control signal to keep it locked at RL0 = 0.
m1 m5 m3
m4 m2
x y
RL0
RL0
RL0
Figure C.1: The right foot of the biped dismounted from the robot. Instead a test frame consisting of two crossed bars was attached to the ankle servomotor. A known CoP was applied to the foot by placing weights on the bars at the ve indicated places on the gure.
The arrangement consisted of 3 pieces, the foot (including the servomotor), the bracket 169
170
APPENDIX C. COP MEASUREMENT TEST
dm pCbars m5 x
RL0
dm pC bracket
m4
pCRL,foot y
RL0
m3 z
RL0
Saggital view
dm
dm pCbars pC bracket m5
m1
x y
RL0
RL0
m2
z pCRL,foot
RL0
Front view
Figure C.2: The test arrangement consisted of 3 pieces, the foot (including the
servomotor), the bracket mounted on the servo (dark grey), and the two crossed bars mounted on the bracket (light grey). A known CoP was applied to the foot by applying weights m1 , m2 , m3 , m4 and m5 to the bars. The servo was applied a current and a control signal to keep RL1 at zero.
mounted on the servomotor, and the two crossed bars mounted on the bracket. These had the following center of masses given in {RL0 },
pCRL,foot
0.0305 0.0162 0.00515 pC 0 0 = = 0.00859 pCbracket = bars 0.00116 0.00116 0.00455
(C.1)
and the masses:
mfoot = 0.1007 kg
mbracket = 0.0106 kg
mbars = 0.0430 kg
(C.2)
The center of masses of the ve attached weights in Fig. C.2 have the following y and z -coordinates,
pCm1
pCm4
0 = dm pCm2 = dm pCm3 = 0.00116 0.00116 dm 0.00116 0 0 = pCm5 = dm 0.00116 0.00116
where the x-coordinate has been omitted because the CoP only depends on the y and z -coordinates, as it is the projection of the CoM on the sole of the foot.
171
The CoM on the entire test arrangement with applied weights can be found as:
pCtot =
1 mfoot pCRL,foot mfoot + mbracket + mbars + m1 + m2 + m3 + m4 + m5 + pCbracket mbracket + pCbars mbars + pCm1 m1 + pCm2 m2 + pCm3 m3 + pCm4 m4 + pCm5 m5
[m] (C.3)
The CoP is then given as the y and z -coordinate of pCtot as the test arrangement is static. During SSP the CoP can be found by inserting the four pressure measurements on the supporting foot into Eq. (14.1) and Eq. (14.2),
yCoP = zCoP
where:
llittle toe (Fp,r1 + Fp,r3 ) lbig toe (Fp,r2 + Fp,r4 ) Fp,r1 + Fp,r2 + Fp,r3 + Fp,r4 ltoes (Fp,r1 + Fp,r2 ) lheels (Fp,r3 + Fp,r4 ) = Fp,r1 + Fp,r2 + Fp,r3 + Fp,r4
[m] [m]
(C.4) (C.5)
ltoe = 0.0627 m
lheels = 0.0608 m
llittle toe = 0.0315 m
lbig toe = 0.0125 m
The total mass of the robot have been measured to be 2.24 kg. As a result, a total mass between 1.000 kg and 2.010 kg was distributed on the bars during the tests. The microcontroller mounted on the robot was used to sample the pressure measurements from the four strain gauges on the foot. To remove any oset from the output of the force measurements using the strain gauges, their output was rst measured with the foot lifted from the ground and subtracting from the following measurements.
Test Results
In Tab.C.1 the dierent values of the masses placed on test arrangement are listed along with the measured pressure forces. The placement of the masses corresponds to those depicted in Fig. C.1, and forces corresponds to those found in Fig. 14.3. The mass values of Tab. C.1 are inserted into Eq. (C.3) to determine the theoretical CoP location. The force values of Tab. C.1 are inserted into Eq. (C.4) and Eq. (C.5) to nd the CoP location using the measurements. The results are compared in Fig. C.3, where it is seen that the measured CoP follows the position of the calculated CoP. The measured y -coordinate diers with a maximum of 8 mm from the calculated and the measured z -coordinate diers with a maximum of 10 mm from the calculated. Thus, from the measurements it will possible to give a CoP within a radius of 10 mm from the actually CoP. As the foot is 124 mm 64.0 mm it is possible to use the measurement to ensure the CoP is within the PoS.
172
APPENDIX C. COP MEASUREMENT TEST
Test no. 1 2 3 4 5 6 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33
m1 [g] 0 0 100 150 200 250 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 460 0 0 0 100 250 250 1000 0 100 0
m2 [g] 0 0 0 0 0 0 0 0 0 0 0 100 150 200 250 300 0 0 0 0 0 0 0 460 0 0 0 0 200 0 310 0
m3 [g] 0 0 0 0 0 0 100 150 200 250 300 0 0 0 0 0 0 0 0 0 0 0 460 0 0 100 100 150 0 1000 0 200
m4 [g] 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 100 150 200 250 300 0 0 0 460 0 0 0 0 200 0 1000
m5 [g] 0 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1000 1650 1650 1650 1650 1650 1650 1650 650 650 650 650
Fp,R1 [N] 0.374 1.90 2.59 2.93 3.27 3.82 2.39 2.66 2.90 3.12 3.40 1.55 1.41 1.22 0.834 0.365 1.74 1.61 1.55 1.49 1.40 6.21 5.90 0.007 2.59 4.63 5.57 5.78 7.64 6.74 1.39 1.57
Fp,R2 [N] 0.374 4.70 4.33 4.22 4.18 4.03 5.56 6.03 6.44 6.85 7.29 5.87 6.48 7.11 7.96 9.00 4.69 4.64 4.74 4.79 4.73 6.79 11.9 16.4 7.56 8.18 8.00 8.38 2.26 11.8 6.35 4.44
Fp,R3 [N] 0.374 1.68 2.62 3.13 3.57 4.35 1.47 1.40 1.29 1.16 1.07 1.08 0.770 0.474 0.450 0.478 2.16 2.39 2.65 2.91 3.16 6.93 1.60 0.499 5.36 3.53 4.96 4.78 9.93 0.798 0.493 6.05
Fp,R4 [N] 0.374 5.35 4.87 4.63 4.46 4.28 4.15 5.26 5.26 5.26 5.23 5.22 6.53 7.17 7.73 7.96 8.17 6.12 6.53 7.02 7.47 7.82 5.56 7.15 11.8 11.1 6.78 6.24 6.10 4.00 6.60 12.6
Table C.1: The masses placed on the foot along with the corresponding pressure
measurements. A total of 33 tests were conducted.
173
40 30 20 10 0 10 20 30 40 Calculated yCoP Measured yCoP 5 10 15 20 Test no. 25 30
40 30 20 10 0 10 20 30 40 Calculated zCoP Measured zCoP 5 10 15 20 Test no. 25 30
yCoP [mm]
Figure C.3: In the two gures the y - and z -coordinates are shown for the calculated and measured CoP respectively. The calculated CoP is found by determining the CoM of the arrangement, while the measured is determined from the pressure measurement.
zCoP [mm]
Appendix
Hardware and Robot Interfacing

D.1 Microcomputer Pin Assignment
The robot has numerous sensor feedback signals and numerous PWM signals to the servomotors which is to be connected to the ServopodTM . This section will clarify which channels are used for which signals. A list of which servomotor is connected to which pin on the ServopodTM is seen in Tab. D.1. Pin TC0 TD3 TD2 TD1 TD0 LA1 LA2 LA3 LA4 Servomotor LL5 LL4 LL3 LL2 LL1 PWMA0 PWMA1 PWMA2 PWMA3 Pin TC1 TA3 TA2 TA1 TA0 RA1 RA2 RA3 RA4 Servomotor RA1 RA2 RA3 RA4 RA5 PWMB0 PWMB1 PWMB2 PWMB3
Table D.1: Table of the pins on the ServopodTM where the dierent PWM signals
is connected to the servomotors.
The dierent feedback signals are connected in two dierent ways. Some of the signals, which is converted into a digital value, is sent through a serial interface using the Serial Peripheral interface (SPI) and some signals is simply connected directly to the ADC ports on the ServopodTM . Five ADC's with eighth ADC channels and SPI communication is placed on the robot. One on each foot, one on each hand and one in the torso. In order to connect to these chips ve chip selects is needed. PA0 to PA4 is used for this purpose. A table showing all the sensor signals sent via SPI is shown in Tab. D.2. The sensors signals connected directly to the ServopodTM and which pins they are connected to is seen in Tab. D.3. 174
D.1. MICROCOMPUTER PIN ASSIGNMENT
175
Body Part Right Foot -||-||-||-||-||-||-||Left Foot -||-||-||-||-||-||-||Torso -||-||-||-||-||-||-||Right Arm -||-||-||-||Left Arm -||-||-||-||-
Channel CH0 CH1 CH2 CH3 CH4 CH5 CH6 CH7 CH0 CH1 CH2 CH3 CH4 CH5 CH6 CH7 CH0 CH1 CH2 CH3 CH4 CH5 CH6 CH7 CH0 CH1 CH2 CH6 CH7 CH0 CH1 CH2 CH6 CH7
Specic Sensor Strain Gauge Big Toe Strain Gauge Big Heel Strain Gauge Little Toe Strain Gauge Little Heel Acc Horizontal Plane Acc Frontal Plane Acc Sagital Plane Angle RL1 Strain Gauge Little Toe Strain Gauge Little Heel Strain Gauge Big Toe Strain Gauge Big Heel Acc Horizontal Plane Acc Frontal Plane Acc Sagital Plane Angle LL5 Gyro Horizontal Plane Gyro Frontal Plane Acc-Low-Res Sagital Plane Acc-Low-Res Horizontal Plane Acc-Low-Res Frontal Plane Acc-High-Res Frontal Plane Acc-High-Res Horizontal Plane Acc-High-Res Sagital Plane Acc Sagital Plane Acc Frontal Plane Acc Horizontal Plane Angle RA2 Angle RA3 Acc Sagital Plane Acc Frontal Plane Acc Horizontal Plane Angle LA2 Angle LA3
Chip Select PA0 -||-||-||-||-||-||-||PA1 -||-||-||-||-||-||-||PA2 -||-||-||-||-||-||-||PA3 -||-||-||-||PA4 -||-||-||-||-
Table D.2: Table of the pins on the ServopodTM which the dierent sensors is
connected to. This table show where on the body the ADC is situated, which channel on the ADC the sensor is connected to, which specic sensor it is and which pin is used to chip select the ADC. `Acc' means the accelerometer and `Gyro' means the gyroscope. As the torso has both a high resolution and a low resolution accelerometer these are denoted Acc-High-Res and Acc-Low-Res respectively. Angle means the measurement from the potentiometer in the servomotors describing the angle of the servomotors.
176
APPENDIX D. HARDWARE AND ROBOT INTERFACING

Pin ADC4 ADC6 ADC8 ADC9 ADC10 ADC11 ADC12 ADC13 ADC14 ADC15 Sensor Signal Angle LA1 Angle RA1 Angle LL2 Angle RL2 Angle LL4 Angle RL3 Angle LL3 Angle RL4 Angle LL1 Angle RL5
Table D.3: Table of the pins on the ServopodTM with the dierent sensor signals
which is connected directly to the ServopodTM . Angle means the measurement from the potentiometer in the servomotors describing the angle of the servomotors.
D.2 IsoMax
Programming the ServoPodTM is achieved using a programming language called IsoMax. The architecture is called Virtual Parallel Machine Architecture which makes IsoMax ideal when programming real time programs when in need of parallel processes. This section will describe the fundamental idea behind IsoMax programming.
D.2.1 States and Transitions

In order to explain how IsoMax work some denitions need to be claried.
State: When in a state, no outputs or actions are taken. The only activity occurring
when the condition in a state is satised. Then will a transition happens. And when this has happened, it will return into a wait state. This means, the states in IsoMax is dierent wait states. In a state, no outputs are changed. If outputs need to be changed, the wait state is no longer valid. If no outputs need to be changed, the wait state is valid.
Transition: All processing actions take place during a transition from one state to
another. Transitions have four components. The rst component is the state with which they are associated. The second is the condition under which they cause transition. If the condition is met, the current wait is no longer valid. The transition becomes selected. The third part of a transition is the action. Once a condition is valid, actions must be performed. Often this requires processing and output manipulation. This action has normally no waiting component. The transition's action is done as fast as the available hardware can be made to run. The nal component of a transition is the vector, or destination of the transition. As soon as possible, all processing will be accomplished. Re-entry to a condition of real time waiting follows. Therefore, a transition always ends in another wait state.
The whole idea with IsoMax is to create numerous state-machines which is equally important in the sense that they are polled at the exact same frequency. This means that the programmer can create e.g. two state-machines controlling two dierent things. Before creating any state machine it is important to see what a state machine consists of. Every state-machine can consist of numerous states while a state-machine can o course only be in one state at a time. Every state consists of minimum one transition.
D.2. ISOMAX
177
This transition denes how to react and which state to jump to afterwards. Every transition consists of four distinct components as seen in Tab. D.4. One component, IN-STATE, which declares in which state the state-machine should be in before checking the CONDITION clause. This means that despite a state-machine can consist of numerous states, only one of these states is checked to see if a transition should occur. The CONDITION component declares when a transition should occur. When the CONDITION is true a transition occur. The CAUSES component simply causes the actions. This could be to turn o a LED, gather sensor information etc. The THEN-STATE denes which state to set the state-machine after the transition. IN-STATE state CONDITION clause CAUSES action THEN-STATE state
Table D.4: The four components in a transition.

As an example of the principle in programming in IsoMax two state-machines is created. One state-machine which turns on a LED if the temperature is outside a certain threshold and another state-machine which adjust the temperature. This example is illustrated in Fig. D.1.
DIODE: TEMP:
STATE OFF
WITHIN THRESHOLD
TRANSITION TO HOT ON TO COLD
Figure D.1: These gures shows the principle of programming in IsoMax. The circles denote the STATEs in which no actions can be taken. The lines from one STATE too another denote the TRANSITIONs in which every action is taken.
The two state-machines in Fig. D.1 are polled at the same frequency. This means that the two machines are polled at e.g. 100 Hz and every time the machine is polled a wait criterion is checked. Depending on this wait criterion one of two things can happen. If the wait criterion is not true any more, a transition will not occur and if the wait criterion is true, a transition will occur, and it is only during a transition any action can be taken.
178
Lets rst of all see what happens when polling the state-machine called TEMP. If the temperature is within the threshold the CONDITION is not true and this state-machine stays in the WITHIN THRESHOLD state and nothing happens. When polling the statemachine DIODE nothing happens here either as the CONDITION clause in this statemachine is dependent on if the temperature is outside the threshold in the other statemachine. These two state-machines continue to be polled without any actions until the temperature rises or falls outside the threshold. When this happens the CONDITION in the state-machine TEMP is changed to true and this causes a transition where it either turns on the heater and jump to the TO-COLD state or it turns on AC (Air Condition) and jump to the TO-HOT state. Further more it changes a global variable called ERROR to true. This variable is used in the other state-machine DIODE as this variable is the CONDITION clause which forces a transition. When polling the state-machine DIODE again the CONDITION which takes the argument ERROR is true which causes the CONDITION to be true and this causes the state-machine to make a transition in which it turns on a LED and jump to the state ON. The state-machines stay in these two new states until the temperature is within the threshold again which makes the two state-machines return to their initial states. DIODE turn o the LED and TEMP waits for the temperature to go outside the threshold. This simple example shows some of the benets of IsoMax. It is simple to construct numerous state-machines which coexist with the same CPU running as parallel processes. Further more implementing the state-machine diagram as real IsoMax code is simple. As an example the DIODE state-machine is converted. Two states consist within this state-machine, ON and OFF state. In Tab. D.5 the state-machine has been translated from the ow chart in Fig. D.1 into an actual IsoMax program. As seen in Tab. D.5 the same CONDITION clause is used in both states. If the error becomes true when in the o state a transition occur and if the error becomes not true when in the on state a transition occur. IN-STATE o CONDITION error CAUSES LED on THEN-STATE on IN-STATE on CONDITION not error CAUSES LED o THEN-STATE o
Table D.5: The two states in the state-machine DIODE.
D.3 Programming the ServoPodTM

Two dierent programs have been developed in this project which can be uploaded to the ServoPodTM depending on which situation the robot is used in. The IsoMax code can be found at [2]. In one program, the robot is able to walk without any wires attached at all. A battery supplies power to the robot and the microcomputer simply plays back some predened trajectories. The program routine is executed at 76 Hz which is the fastest possible update frequency of the servomotors. The second program is implemented as the processing power of the ServoPodTM is lim-
D.4. INTERFACING WITH AN EXTERNAL COMPUTER
179
ited. Hence, it is important to be able to send and receive data to and from an external PC. This can be used for both logging purposes and for making more computationally intensive controllers. The robot is connected to an external PC with a serial cable. The function of the ServoPodTM in this situation is to gather sensor signals from the numerous sensors on the robot and send this data to the external PC. This PC is running a controller and sends back the actuator signals to the ServoPodTM which nally sends the actuator signals to the respective servomotors. A ow chart of the program is seen in Fig. D.2.
380Hz 76Hz
Receive Sersor Signals
Mean the Sensor Signals
Receive PWM Signal From External Computer Via RS232
Send Sensor Signals To Simulink Via RS232
Send PWM Signals To Actuators
Figure D.2: Flow chart of the program when the ServoPodTM is congured with
the second conguration.
As seen in Fig. D.2, two state-machines are created in this routine. One state-machine gathers sensor signals at 380 Hz. This sampling frequency chosen to avoid aliasing, as the analog lters implemented has a cut o frequency of 156 Hz. This means that the sample frequency should be at least 312 Hz. As the servomotors is updated at 76 Hz and due to the way IsoMax is structured, the sample frequency is chosen as ve times 76 Hz = 380 Hz. The second state-machine performs several tasks. It averages over ve samples from the sensors. This constitutes a lter with the digital transfer function 1/(0.2z + 0.2z 1 + 0.2z 2 + 0.2z 3 + 0.2z 4 ). A bode plot of this lter is seen in Fig. D.3. The averaged samples are sent to the external PC. Also, it receives the new actuator signals from the external PC and sends these to the respective servomotors. This routine is executed at 76 Hz as mentioned earlier as this is the maximum update frequency.
D.4 Interfacing With An External Computer

The interface from the robot to an external computer is done using a serial cable. With a total of 48 sensors with 16 bit resolution and 20 servomotors with 8 bit resolution, an update rate of 76 Hz and one start and stop bit, the bandwidth used is (48 (16 + 4) + 20 (8 + 2)) 76 = 88160 bit/s. Choosing standard serial data rates, a rate of 115200 bit/s can be used. However, because of the limited processing power of the ServoPodTM , it has been chosen not to send all sensor signals. For control purposes, only the strain gauge and accelerometer measurements are needed. Hence, these are the only data sent via the serial interface. To ensure correct reception of data, it is sent in packets enclosed in start and stop bytes.
180
Bode Diagram 0 System: sysz Frequency (rad/sec): 214 Magnitude (dB): 2.99
Magnitude (dB) Phase (deg)
50
100
150 90 45 0 45 90 135 180 10

1 2 3 4
10 10 Frequency (rad/sec)
10
Figure D.3: Bode plot of the digital lter on the ServoPodTM .

This ensures that the data is sent in the right order, and that data is not lost. The packet format is seen in Tab. D.6 and Tab. D.7. As the transmission of data between the external PC and the ServoPodTM program is not fully synchronized when they start sending data, certain measures needs to be taken in order to secure that the data is received in the same order as sent. Otherwise misalignment problems could occur which could cause signals from one servo to be sent to another servo, or even problems regarding the endianess of the data. When sending the data from the ServoPodTM to an external computer, two start bytes (HEX F0) is sent in advance of the data. When receiving data from an external PC, a stop byte (HEX F0) is sent as the last byte. Along with sending this stop byte, the routine which receives data from the serial cable still take measures to avoid the problem with desynchronization. A ow chart of the routine is seen in Fig. D.4. The routine keep on moving bytes from the receive buer to a buer keeping the angles to the servos until one of two things happen. One situation is that is has actually received 19 bytes without receiving the stop byte. This can happen if the serial receive interrupt routine on the ServoPodTM accidentally overwrite the stop byte in the buer. This situation resets the counter and the routine is ready to receive the next 19 bytes. This continues until the stop byte is received and the data is sent to the servos resulting in new angles. As this routine is called with 190 Hz and the data is sent with 76 Hz this routine should eventually receive the 19 bytes in the correct order thus be able to keep the data packets synchronized.
D.4. INTERFACING WITH AN EXTERNAL COMPUTER
181
Byte Sensor Byte Sensor Byte Sensor
1+2 ATHX 17+18 SLLH 33+34 ALFZ
3+4 ATHY 19+20 SLLT 35+36 ARHX
5+6 ATHZ 21+22 SLBT 37+38 ARHY
7+8 SRBH 23+24 ARFX 39+40 ARHZ
9+10 SRLH 25+26 ARFY 41+42 ALHX
11+12 SRLT 27+28 ARFZ 43+44 ALHY
13+14 SLBT 29+30 ALFX 45+46 ALHZ
15+16 SLBH 31+32 ALFY 47 Trailer
ATLX-ATLZ Accelerometer Torso High Sensitivity X-Y-Z SLBT,SRBT Strain Gauge Left/Right Big Toe SLLT,SRLT Strain Gauge Left/Right Little Toe SLBH,SRBH Strain Gauge Left/Right Big Heel SLLH,SRLH Strain Gauge Left/Right Little Heel ARFX-ARFZ Accelerometer Right Foot X-Y-Z ALFX-ALFZ Accelerometer Left Foot X-Y-Z ARHX-ARHZ Accelerometer Right Hand X-Y-Z ALHX-ALHZ Accelerometer Left Hand X-Y-Z Trailer Trailer byte (0xF0)
Table D.6: Packet format used for serial transmission from the ServoPodTM to an
external computer.
Byte Servo Byte Servo Byte Servo
1 Header 11 SLA5 21 SLL5
2 SRA1 12 SRL1 22
3 SRA2 13 SR2 23
4 SRA3 14 SRL3 24
5 SRA4 15 SRL4 25
6 SRA5 16 SRL5 26
7 SLA1 17 SLL1 27
8 SLA2 18 SLL2 28
9 SLA3 19 SLL3 29
10 SLA4 20 SLL4 30
Header Header byte (0xF0) SRA1-SRA5 Servomotor Right Arm 1-5 SLA1-SLA5 Servomotor Left Arm 1-5 SRL1-SRL5 Servomotor Right Leg 1-5 SLL1-SLL5 Servomotor Left Leg 1-5
Table D.7: Packet format used for serial transmission from an external computer
to the ServoPodTM .
182
190Hz
Any data at the receive buffer yes Reset counter Put data into buffer
no
Reset counter
Received 19 bytes no Send new angles to servos Received stop byte yes
yes
no
Figure D.4: Flow chart of the routine receive 19 bytes from an external PC via
RS232 where the last byte is a stop byte.
D.5. SERVOMOTOR CALIBRATION
183
D.5 Servomotor Calibration

In order to control the servomotors on the biped robot a mapping of the servomotors is needed. A mapping is to be created which maps a number between 0 and 180 into a PWM signal which rotates the servomotors to an angle between 0 and 180 , where 90 is the centre position. A mapping is also to be created which map the voltage across the potentiometer in the servomotors, which represent the angle of the servomotors, into a number between 0 and 180 , where 90 is the centre position.
Duty Cycle vs. Angle

In order to calculate which PWM signal should be applied to get the desired angle, each servomotor was applied an increasing PWM signal and the angle of the servomotor was measured. It has been chosen that the centre position of the robots is when standing straight up with its arm slightly out to the side, as seen in Fig.D.5. This position should correspond to an angle of 90 to all servomotors, where the angle can vary 90 from this centre point. As three dierent types of servomotors is used on the robot a mapping function is to be derived for each of these servomotors. The four servomotors LL5, LL4, RL1, RL2 is the Hitec HSR-5995TG servomotor while the rest is the Hitec HS-645MG servomotor except RA4, RA5, LA4, LA5 which is Hitec HS-475HB. As the mapping of HS-645MG and HS-475HB has shown to be exactly the same only two mapping function is needed.
Figure D.5: Centre position of the biped robot.

In order to derive the mapping function from applied PWM to resulting angle, the servomotors is applied an increasing duty cycle while measuring the angle. The duty cycle and corresponding angles can be seen in Fig. D.6 for the two servos. In Matlab these data has been transferred into the following linear function which link the duty cycle (duty) to the angle of the servomotor (ang) using the Hitec HS645MG/HS-475HB: ang = 0.0200 duty where is the oset for the dierent servomotors. The linear function when using the Hitec HSR-5995TG is: ang = 0.0439 duty where is the oset for the dierent servomotors. (D.2) (D.1)
184
Hitec HS645MG 30 20 Angle [] 10 0 10 6800
7000
7200
7400
7600
7800
8000
8200
8400
8600
Hitec HSR5995TG 40 20 Angle [] 0 20 40 6800
7000
7200
7400
7600 Duty Cycle
7800
8000
8200
8400
Figure D.6: Resulting angle of servomotor when applying dierent duty cycles to
the servomotor. The duty cycle is denoted as used with IsoMax.
As the servomotors are not mounted mechanically in the exactly same position, dierent duty cycles orient the dierent servomotors to the robots centre position. These duty cycles has been found using trial and error simply by adjusting the duty cycle until a satisfying result is obtained. The osets are seen in Tab. D.8. Servomotor LL5 LL4 LL3 LL2 LL1 LA1 LA2 LA3 LA4 Oset 7450 7440 7640 7300 7000 7950 5100 7500 9850 Servomotor RL5 RL4 RL3 RL2 RL1 RA1 RA2 RA3 RA4 Oset 7580 7700 8100 7400 7300 8100 9100 7100 5200
Table D.8: Osets for the dierent servomotors positioning them in centre position.
These results enable the creation of a mapping function from the desired angle of the servomotors to the actual duty cycle which should be applied to the servomotors. As an example servomotor LL5 will be used. From Eq. (D.2) it can be seen that changing the duty cycle by 22.7 results in a change in angle of one degree. This enables an easily mapping function: ang 22.7 + 7450 = duty (D.3)
D.5. SERVOMOTOR CALIBRATION
185
Duty Cycle vs. ADC

Tests has also been performed to link applied duty cycle to the output from the potentiometer, which represent the angle of the servomotor, where this output is converted into a digital value. As the feedback from the servomotors is converted using two dierent ADCs, two dierent mappings is needed. One mapping using the MCP3208 ADC and one mapping using the ADC on the ServoPodTM . The reason for using two dierent ADC is that a analogue signals which is to be transferred over long distances along with other analogue signals might interfere with each other. For this reason ADC's has been placed on each foot and in each wrist. But the signals close to the ServoPodTM is connected directly, thus using the ADC on the ServoPodTM . Another problem was that there were simply not enough ADC ports on the ServoPodTM to feedback all the sensor signals. Each servomotor was applied an increasing duty cycle and the output from the potentiometer was measured. A duty cycle with an increment of 50, equal to an increment of 0.07%, was applied to the servomotor.
Hitec HSR5995TG 25000 12000 11000 ADC Value 20000 10000 9000 15000 8000 7000 10000 6500 7000 7500 a) 8000 8500 6000 6000 7000 b) 8000 9000 Hitec HS645MG
3000
1600 1500
ADC Value
2500
1400 1300
2000
1200 1100
1500 6500
7000 7500 8000 c) Duty Cycle
8500
1000 6000
7000 8000 d) Duty Cycle
9000
Figure D.7: ADC values from corresponding duty cycles. The rst column shows
the measurements from the Hitec HSR-5995TG servomotor whilst the second column shows the measurements from the Hitec HS-645MG servomotor. The rst row shows the measurements using the ADC in the ServoPodTM whilst the second row shows the measurements using the MCP3208.
As it is seen in Fig. D.7 four tests have been performed as there are two dierent servo-
186
motors and two dierent ADC's. As seen in Fig. D.7 the functions are approximately linear. These function has been found using Matlab and are listed below. The function mapping a duty cycle applied to the Hitec HSR-5995TG to the ADC in the ServoPodTM , as seen in Fig. D.7a: adc = 5.8 duty + where is the oset of the dierent servomotors. The function mapping a duty cycle applied to the Hitec HSR-5995TG to the MCP3208, as seen in Fig. D.7b: adc = 0.7 duty + (D.5) where is the oset of the dierent servomotors. The function mapping a duty cycle applied to the Hitec HS-645MG to the ADC in the ServoPodTM , as seen in Fig. D.7c: adc = 1.5 duty + where is the oset of the dierent servomotors. The function mapping a duty cycle applied to the Hitec HS-645MG to the MCP3208, as seen in Fig. D.7d: adc = 0.2 duty + (D.7) where is the oset of the dierent servomotors. In order to calculate the exact mapping function the ADC value when the robot is in its center position the ADC has been logged. These values are seen in Tab. D.9: Servo LL5 LL4 LL3 LL2 LL1 LA1 LA2 LA3
position.
(D.4)
(D.6)
Oset 2047 16305 10622 10150 9417 11043 1308 1344
Servo RL5 RL4 RL3 RL2 RL1 RA1 RA2 RA3
Oset 9919 10645 11305 14870 1939 11025 1225 1271
Table D.9: ADC osets for the dierent servomotors when positioned in center
A mapping function from the ADC value to the actual angle can be created using the test results. As an example the LL5 servomotor is used. This servomotor has feedback through an MCP3208. First of all the oset is calculated:
= 2047 + 0.7 7450 = 7262

This is used in the mapping function: duty = adc 7262 0.7
(D.8)
(D.9)
And a combination of Eq. (D.3) and Eq. (D.9) gives the desired mapping function from the digital ADC values to the actual angle: ang = adc 7262 7450 /22.7 0.7 (D.10)
D.6. SERVOMOTOR PERFORMANCE UNDER VARIOUS LOADS
187
The test should have been performed on each and every servomotor resulting in an exact mapping function for every servomotor. But tests showed that the gain of the functions were the same for every servomotor but with dierent osets. This meant that the same function could be used for every servomotor with an oset adjustment.
D.6 Servomotor Performance Under Various Loads

The servomotors used in the robot uses an internal controller to ensure the position of the shaft. This controller should suppress disturbances such as if various loads are given to the servomotor. To test the performance of this controller, a series of tests were conducted on the HS-645MG servomotor in which various loads were applied. The test arrangement is seen in Fig. D.8. The mass ml varied through the tests, where the servomotor was given a step of 70 from 30 to 40 where 0 is horizontal and the rotation is counter clockwise. Because of the change in angle during the test, the torque exerted on the servomotor varies. Ex. during a test with ml = 0.8 kg, the torque varies from 0.69 Nm when the servomotor is in the 30 position to 0.8 Nm when it is in the 0 position. This test, however, is only performed to get an idea of the dependency of the servomotor performance versus the load. Therefore it is not important to get a mapping from various loads to accelerations, but merely to show that there is a correlation. Only one servomotor is tested, as it is assumed that all servomotors will show the same relationships.
Servo
70
ml
l=10.2 cm ml
Figure D.8: Test arrangement

Table D.10 and Fig. D.9 through D.11 summarizes the results of the test. Weight Position error Maximum velocity Maximum acceleration
2
0.0 kg 0.280 kg 0.440 kg 0.800 kg

the servomotor.
0.013 rad 0.033 rad 0.064 rad 0.169 rad
6.912 rad/s 4.846 rad/s 3.803 rad/s 2.022 rad/s
96.91 rad/s 2 74.15 rad/s 2 62.57 rad/s 2 2.023 rad/s
Table D.10: Table summarizing the results of the test with various loads given to
The tests show clearly, that there is a strong dependency between load and the performance of the servomotor. The performance regarding position error, maximal velocity and maximal acceleration is deteriorating as the load is increased. Especially at loads which are close to the stall torque.
188
Positions at various loads 3 Pos. [rad] 2 1 3 Pos. [rad] 2 1 3 Pos. [rad] 2 1 3 Pos. [rad] 2 1 X: 0.001 Y: 2.883 X: 1.201 Y: 1.83 X: 0.001 Y: 2.77 0 0.2 0.4 0.6 0.8 X: 1.201 1 Y: 1.612 1.2 X: 0.001 Y: 2.745 0 0.2 0.4 0.6 0.8 X: 1.201 1 Y: 1.556 1.2 X: 0.001 Y: 2.664 0 0.2 0.4 0.6 0.8 1 X: 1.201 Y: 1.455
1.2
0.2
0.4
0.6 Time [s]
0.8
1.2
Figure D.9: Servomotor positions given a step of 70 at various loads.
Velocities at various loads 2 0 2 4 6 8 2 0 2 4 6 8 2 0 2 4 6 8 2 0 2 4 6 8 Vel. [rad/s] X: 0.222 Y: 6.912 0 0.2 0.4 0.6 0.8 1 1.2
Vel. [rad/s]
X: 0.223 0.2 Y: 4.846 0.4
0.6
0.8
1.2
Vel. [rad/s]
0.2
X: 0.236 Y: 3.803 0.4
0.6
0.8
1.2
Vel. [rad/s]
X: 0.226 Y: 2.022 0 0.2 0.4 0.6 Time [s] 0.8 1 1.2
Figure D.10: Servomotor velocities given a step of 70 at various loads.
D.7. ROBOT AUTONOMY HARDWARE
189
Accelerations at various loads Acc. [rad/s2] 100 0 100 100 0 100 100 0 100 100 0 100 X: 0.163 Y: 62.57 X: 0.151 Y: 74.15 0 0.2 0.4 0.6 0.8 1 1.2 X: 0.144 Y: 96.91 0 0.2 0.4 0.6 0.8 1 1.2
Acc. [rad/s2]
Acc. [rad/s2]
0.2
Acc. [rad/s2]
0.4 X: 0.229 Y: 2.023
0.6
0.8
1.2
0.2
0.4
0.6 Time [s]
0.8
1.2
Figure D.11: Servomotor accelerations given a step of 70 at various loads.
D.7 Robot Autonomy Hardware

One of the feature enhancements for the robot was the inclusion of an on-board microprocessor and batteries to make the biped robot walk without any wires from the robot to the surroundings. This document contains the thoughts that were given to the selection of a microprocessor board and batteries.
Processor board features

Firstly, the physical design of the biped robot gives constraints regarding the size of the processor board. Large and heavy boards would introduce further strain on the already heavily loaded servomotors, and also change the dynamics of the robot. Hence, the board should be as small as possible. To control the servomotors, 20 PWM signal generators are needed. The custom-built board that was on the robot already has 3 servomotor controller boards, each controlling 8 servomotors = 24 servomotors. These boards are interfaced using a serial connection compatible to RS232 (TTL and RS232 voltage levels). Therefore, it is not necessary to have PWM signal generators as this can be done using a serial port. However, the 3 servomotor controllers will have to be added outside the main processor board, giving mounting problems and adding extra weight. The processor must have sucient processing power to control all the 20 servomotors using the sensor inputs from the strain gauges, servomotor positions, gyro and accelerometers. It must be relatively easy to construct and debug software for the microprocessor. A single serial or USB cable to a host computer is preferable.
190
ADCs are not strictly necessary, as this can be done by external hardware linked to the microprocessor via a serial bus (as done for the strain gauges). However, the inclusion of ADCs on the processor board would decrease the amount of external components and decrease development time. Immediate availability of the board is of great importance as waiting time would result in lost project time. Therefore the board should be obtainable within one week.
The candidates
Having looked around on the net, the following candidates have been selected.
Mini Atom Bot Board and SSC-32 Servo Controller

The Atom Bot Board was found on Lynxmotions web-site. The same place where the robot parts were bought. It is a motherboard, in which several small 24 or 28 pin BS2 like processors can be plugged in. This gives a lot of exibility regarding processor usage. Furthermore, it has a speaker, 3 push buttons, 3 status LEDs and an I/O bus. The board has some servomotor controllers, but not enough to interface to the 20 servomotors. Hence, a SSC-32 servomotor controller board is included, which can control 32 servomotors. The BASIC Atom Pro 28-pin processor has 0.1 MIPS and 8 A/D pins The board size is about 5 7 cm This board is available from USA, delivery time approximately 1 week.
Newmicros ServoPod
The ServoPod is a microcontroller board designed for control of a large amount of servomotors. It includes 26 PWM outputs for servomotor control. Apart from this, it has SPI and I2C serial buses. 16 General purpose I/O ports. 2 RS232 and a CAN bus, 16 channel 12-bit A/D and H-bridge, timers and quad encoders. All in all, more than sucient I/O, and most importantly, on-board connectors for all the servomotors. The processor is a DSP56F807 16-bit DSP running at 80 MHz giving 40 MIPS. The connection to the board is achieved using either a RS232 or a USB connection. Also, a JTAG connector is included. The board size is 5.9 7.7 cm for the RS232 version and 5.9 10.2 cm for the USB version. The board is programmed using a language called IsoMax, which is optimized for embedded real time applications. However, it is also possible to ash the processor with custom compiled programs from other languages using the JTAG interface. This board is available from USA, delivery time approximately 1 week.
nanoLIAB
The nanoLIAB is an embedded Linux platform with an Atmel AT91RM9200 microprocessor and Ethernet. The processor includes RS-232 serial communication, USB, PIO, I2C, SPI and 18 general purpose I/O ports.
D.7. ROBOT AUTONOMY HARDWARE
191
Programming is done using an ARM cross compiler delivered with the board. Programming languages includes c and c++. LIAB is situated in Stvring, Denmark, delivery time 1 day.
Choice of Board
The Mini Atom Bot board was opted out because of lack of features. The nanoLIAB and ServoPod had way more functionality without having larger board sizes. The nanoLIAB was available from Denmark and had an Ethernet connection. The Ethernet allowed easy data logging and monitoring. Also, the on-board Linux distribution allows having numerous tools. The nanoLIAB ARM processor is faster than the one on the ServoPod. The ServoPod is able to control up to 26 RC servomotors at a time. It also has on-board ADCs to interface with sensors. When comparing the ServoPod and the nanoLIAB boards, no board had a clear advantage over the other, hence, the boards were chosen according to availability. At the time of purchase, the nanoLiab was not available, so the ServoPod was ordered. However, a nanoLiab board became available while the ServoPod was shipping, so a nanoLiab was also purchased. By having two boards, the one oering the simplest implementation and interfacing or oering the best features could be determined by hands-on experiences.
Evaluation and Final Choice of Board

The boards were tested and evaluated for ease of implementation and features.
ServoPod
The ServoPod was easy to set up using the RS232 cable. Programming in IsoMax proved very easy, and interfacing with the servomotors and ADCs was extremely easy. The PWM ports on the ServoPod had the quirk that the minimum frequency obtainable was 76 Hz, and not 50 Hz as required by the servomotors. This can be overcome by halving the speed of the CPU. However, testing the servomotors with a PWM period of 76 Hz proved that they still work when given signals with this period. Preliminary tests of the real-time performance showed that the ServoPod with IsoMax was able to run a routine with regular intervals without much jitter or delay.
nanoLiab
Connecting to the nanoLiab was easy using both a telnet connection to the on-board Ethernet or the RS232 connection. Both methods yielded a login prompt to the onboard Linux distribution. Programming for the nanoLiab was done on a host Linux computer using the crosscompiler on the nanoLiab CD-ROM. Accessing the microprocessor SPI ports and I/O must be done in kernel-mode, so kernel module programming is necessary. The LED and button module supplied by Liab is an easy to understand example on reading and writing to registers on the nanoLiab. The source for this module is included. However, Linux kernel module programming was considered too time-consuming a task compared to using the IsoMax language available on the ServoPod. Hence, the ServoPod was chosen as microprocessor board.
192
D.8 Accelerometer Test

The ve accelerometers on the biped robot measures acceleration in three planes. In order to use these measurements to either verify the models or to calculate the tilt of the robot in a control situation, a mapping function needs to be constructed. This mapping function should map the output from the accelerometer, which is converted into a digital value, to the tilt of the robot which eventually can be mapped into an acceleration of the robot. A test has been performed on the 5 accelerometers on the robot using the earth gravitationally force as acceleration. Every accelerometer has been tested while it was tilted from an angle of -90 through 0 to 90 with respect to the earth gravitationally force. These tests resulted in the output from the accelerometer 2 2 2 while applying an acceleration from 9.8 m/s through 0 m/s to -9.8 m/s . From the data sheet [18] the output from the accelerometer is suspected to resemble a sine wave, so the data is attempted tted with a sine wave. As a tting tool Matlab's fminsearch is used. This function can minimize multidimensional non-linear functions to a given data set.
Data Treatment
In this section the data from the accelerometer test is tted with a sine wave. As there are 5 accelerometers which each measure in 3 dimensions this leads to 15 tests. The crosses in Fig. D.12 through Fig. D.15 denotes the actual measured values from the accelerometer whilst the solid line is the tted sine curve.
Output from accelerometer Right Foot XRL1 4000 3000 2000 1000 100 4000 3000 2000 1000 100 Left Foot XLL5
0 Right Foot ZRL1
100
0 Left Foot ZLL5
100
Output from accelerometer
4000 3000 2000 1000 100
4000 3000 2000 1000 100
100
o
100
o
Tilt of the accelerometer [ ]
gravity is perpendicular to the sensor and 0 is when the gravity is parallel to the sensor. The title denotes which limb the accelerometers in placed on and which axis it measures in.
Figure D.12: Test results from the accelerometer test. 90 is the situation when
D.8. ACCELEROMETER TEST
193
Right Foot YRL1 4000 3000 2000 1000 100 4000 3000 2000 1000 100
Left Foot YLL5
0 Right Hand ZRA5
100
0 Left Hand ZLA5
100
4000 3000 2000 1000 100
4000 3000 2000 1000 100
100
100
Tilt of the accelerometer [o]
gravity is perpendicular to the sensor and 0 is when the gravity is parallel to the sensor.'
Right Hand XRA5 4000 3000 2000 1000 100 4000 3000 2000 1000 100
Left Hand XLA5
0 Right Hand YRA5
100
0 Left Hand YLA5
100
4000 3000 2000 1000 100
4000 3000 2000 1000 100
100
100
194
Torso Y RL5 4000 3000 2000 1000 100 4000 3000 2000 1000 100
Torso ZRL5
0 Torso XRL5
100
100
Tilt of the accelerometer [o] 4000 3000 2000 1000 100
100
o
Using fminsearch in Matlab lead to the functions describing the accelerometer output when applied varying angles with respect to the gravity, as seen in Tab. D.11. In order to measure the acceleration instead of the tilt a linear mapping is constructed from the same test giving the functions in Tab.D.11. The output is measured at dierent 2 accelerations thus giving an ADC value when the acceleration is 0 m/s and a gain. In order to calculate the actual acceleration a formula describing the ADC value with respect to the analogue input to the ADC is found from the data sheet [19]: ADC 3.3 = Vin 4096 (D.11)
Knowing the input voltage from the accelerometer the gain can be calculated as the ADC value is known for 0 g and for 1 g . These measurements are shown in Tab. D.12. From Tab. D.12 linear function are easily constructed which maps the ADC output to an acceleration. The reason for not showing it for the right leg is that it is always the supporting foot thus there are no acceleration seen from that foot.
D.8. ACCELEROMETER TEST
195
Placement RF RF RF LF LF LF RH RH RH LH LH LH T T T
Direction zRL1 yRL1 xRL1 zLL5 yLL5 xLL5 zRA5 yRA5 xRA5 zLA5 yLA5 xLA5 zRL5 yRL5 xRL5
Function y = 2031.042 + 1033.496 sin(0.0189 x) y = 2075.477 996.9887 sin(0.0178 x) y = 2130.1321 + 972.2518 sin(0.0194 x) y = 1976.379 + 1045.9388 sin(0.0188 x) y = 2133.3736 993.0082 sin(0.0185 x) y = 2146.863 + 995.2353 sin(0.0192 x) y = 2096.153 + 997.7545 sin(0.0163 x) y = 2002.4211 + 982.9864 sin(0.0173 x) y = 2019.1428 1017.7454 sin(0.0164 x) y = 2187.0183 1002.2872 sin(0.0180 x) y = 1983.1052 + 1003.8638 sin(0.0169 x) y = 2111.381 994.0982 sin(0.0175 x) y = 2213.3254 967.4174 sin(0.0170 x) y = 2121.1372 996.5332 sin(0.0165 x) y = 2002.6223 989.073 sin(0.0170 x)
Table D.11: Mapping functions for the 15 dierent accelerometers mapping the output from the accelerometer into an angle in the specied direction. The four dierent placements: Right Foot (RF), Left Foot (LF), Right Hand (RH), Left Hand (LH), Torso (T). The directions of the accelerations is dened by the coordinate systems in Fig. 8.3
Placement LF LF LF RA RA RA LA LA LA T T T
Direction xLL1 yLL1 zLL1 xRA5 yRA5 zRA5 xLA5 yLA5 zLA5 xRL5 yRL5 zRL5
0g 2136 2144 1964 2012 2003 2095 2012 2003 2095 2002 2113 2121
Gain [ADC/g ] 993 993 1043 1018 981 993 1018 981 993 981 956 993
Table D.12: ADC values for 0g and gain for the accelerometers. The four dierent placements: Left Foot (LF), Right Hand (RH), Left Hand (LH), Torso (T). The directions of the accelerations is dened by the coordinate systems in Fig. 8.3
196
D.9 Strain Gauge Measurements

The strain gauges on the feets is used to measure the CoP of the biped robot and for this purpose a mapping is needed which maps the output from the strain gauges to the force exerted on the toes/heels. A test is performed on each toe/heel on both feet where they are subjected to a force of 1-10 N while measuring the output from the strain gauges. This test gives the slope of the function while the o-set has to be determined each time the robot has been walking. This is due to the fact that the bending of the toes/heels changes a little every time the robot has been walking. The test results are seen in Fig. D.16 and Fig. D.17.
Left Little Heel Output from ADC 3000 2000 1000 0 3000 2000 1000 0 Left Big Heel
5 Left Little Toe
10
5 Left Big Toe
10
Output from ADC
3000 2000 1000 0
3000 2000 1000 0
5 Applied Force [N]
10
5 Applied Force [N]
10
Figure D.16: Strain gauge measurements from the right foot. The gure shows the
output from the strain gauges when applying a varying force.
As seen in the gures the functions can be approximated with a linear function for all the strain gauges. As mentioned only the slope of the function is useful from this test as the o-set is varying every time the robot has been walking. The functions is seen in Tab. D.13. Placement RBH RLH RBT RLT LBH LLH LBT LLT Function y = 106 x + y = 135 x + y = 106 x + y = 131 x + y = 140 x + y = 154 x + y = 140 x + y = 126 x +
Table D.13: The eight functions describing the relationship between the applied
force on the strain gauges to the output from the strain gauges. The rst letter in the placement denotes the foot, R for right and L for left and the following letters is Big Heel (BH), Little Heel (LH), Big Toe (BT) and Little Toe (LT).
D.10. PRINT LAYOUT
197
Right Little Heel Output from ADC 3000 2000 1000 0 3000 2000 1000 0
Right Big Heel
5 Right Little Toe
10
5 Right Big Toe
10
Output from ADC
3000 2000 1000 0
3000 2000 1000 0
5 Applied Force [N]
10
5 Applied Force [N]
10
Figure D.17: Strain gauge measurements from the right foot. The gure shows the
output from the strain gauges when applying a varying force.
D.10 Print Layout

In this section the design of the ADC and low-pass lter circuits can be found. The circuits are made to suppress noise by keeping wires from analogue signals short and low-pass ltering them.
Low-pass Filters to Microcontroller's ADC Inputs

A print is made to the low-pass lters which are connected to the microcontroller's ADCs. This print is also used as backplate to the robot. This means that it also connects the power supply to the microcontroller and protect the connections to the microcontroller moreover it protects the microcontroller from scratch if the robot should fall. The schematic of the backplate can be found in Fig. D.18 and the board layout can be found in Fig. D.19.
Accelerometer PCB
The print circuit board (PCB) for the wrist can be found in gure D.20. This board is a double layer print, to ensure small physical size of the board. The PCB board for the foot is found in gure D.21. This board is also a double layer print to ensure small physical size. The accelerometer PCB board for the torso is found in gure D.22. This board is also a double layer print to ensure small physical size.
198
ADC1 1 2 3 4 5 6 7 8 9 10 CON10 POWER 1 2 CON2 ADC2 1 2 3 4 5 6 7 8 9 10 CON10 JTAG 1 2 3 4 5 6 7 8 9 10 CON10 1 2 3 4 5 6 7 8 9 10 CON10 J8 C9 1n C10 1n C11 1n C12 1n C13 1n C14 1n C15 1n C16 1n R11 R12 1k R14 1k R17 1k 1k R13 1k R16 1k 1k R15 1k R18 1 2 3 4 5 6 7 8 9 10 VCC 1 2 CON2 R3 R4 1k R6 1k R9 1k 1k POWER C1 1n C2 1n C3 1n C4 1n C5 1n C6 1n C7 1n C8 1n R5 1k R8 1k 1k R7 1k R10 1 2 3 4 5 6 7 8 9 10
J5
CON10
J6
CON10
Figure D.18: Schematic of the backplate circuit
Figure D.19: Print layout of the backplate circuit
D.11 Simulink Interface

To ease implementation of controllers for the robot and to aid the limited processing power of the on-board microprocessor, a peripheral computer with Simulink is added.
D.11. SIMULINK INTERFACE
199
(a)
(b)
Figure D.20: The accelerometer PCB in the wrist. Figure a is the PCB board from button and b is the board from top
(a)
from top
(b)
Figure D.21: The accelerometer PCB in the foot. Figure a is from button and b is
(a)
(b)
Figure D.22: The accelerometer and gyroscope PCB in the body. Figure a is from button and b is from top
This is done using an xPC setup, where the real-time processing of the Simulink model is done on a dedicated PC while the development of the Simulink model is done on a host computer. The setup is seen in Fig. D.23. The structure of the Simulink model is seen in Fig. D.24. It includes the following sub-blocks. 1. A send and receive block receives the data from the microprocessor and splits the data packet up into the various sensor data. This block outputs the raw data from each sensor. It also formats the servomotor angle input data into a packet, and sends it to the microprocessor. 2. A sensor model block, which translates the raw data into physical values such as ZMP position and tilt. The outputs from this block are the tilt of the limbs and the ZMP of the robot. 3. A controller block, which uses the feedback from the sensors to ensure stability of the robot. It also gets the trajectories and ZMP references which have been generated o-line. The output from this block is the angles given to the servomotors. The main block also logs all the data received from and sent to the microprocessor.
200
Biped robot rs232 Sensor information Servo positions
xPC target Ethernet
Host computer
Figure D.23: The interface between the robot, the xPC target and the host computer.
Send/Receive RS232 block
Raw data
Sensor model block
Sensor information
Controller block
Gait data Gait generator Log data Servo angles
Figure D.24: The structure of the simulink program
Appendix
Servomotor model
In this appendix, models of the servomotors are developed. As the specications regarding the internal structure of the servomotors are unavailable, it is chosen to regard them as a closed system, and determine their dynamic response using a system-identication approach. In [Brennan, 1999] it is concluded that RC servomotors can be approximated by a second order system with a rate-limitation and dead-zones. The rate-limitations refer to the maximal velocity and acceleration of the servomotor. The dead-zone is incorporated into the internal servomotor controller to avoid the DC motor to be constantly activated. In [rts et al., 2006], the servomotors are modeled as acceleration limited, velocity limited rst order systems with no dead-zones. According to the data-sheets for the servomotors, the dead-zones for the HS-645MG and HSR-5995TG are 8 s and 2 s, respectively. This translates to a dead-band of 8 45/400 = 0.9 and 2 90/400 = 0.45 , respectively. Hence, with the addition of dead-zones and a delay, it was found that this servomotor model approximates the real servomotors suciently. Hence, this approach is used instead of a second-order system. A Simulink model is seen in Fig. E.1. Using a state-space representation, the model is:
xk+1 = k+1 k+1 = k k+1 y k+1 =
A xk + B uk 1 Ts 0 c 0 0 0 1 0 0 1/Ts 1/Ts
k 0 0 k c 0 k-1 + 0 ref 0 k 0 0
C xk+1 1 0 0 1 0 0 0 0 0
k+1 k+1 = k+1
k+1 0 k+1 0 k 1 k+1
where xk , yk and uk are the states, the outputs and the inputs at time k and A is the system matrix, B is the input matrix and C is the output matrix. 201
202
APPENDIX E. SERVOMOTOR MODEL
In [rts et al., 2006], nothing is written about a test comparing the model to the actual responses of the servomotors. Parameters regarding maximal velocity can be found in data sheets, but there is no documentation about where the maximal acceleration of the servomotors or the c parameter describing the behaviour of the rst order system have been obtained. It was found out verbally, that the parameters were obtained experimentally. However, to ensure the correctness of the servomotor model other tests will be performed here and documented to verify the data sheet values as well as produce experimentally based parameters for the model.
0 HS5995TG model Constant RT Rate Transition Product Add Dead band switch 15 Constant2 K*u Gain |u| Abs 1 theta 1 z Unit Delay Saturation Rate Limiter RT Rate Transition1 K*u Transport Delay Gain1 2 thetadot 3 thetadotdot
1 theta_ref
Figure E.1: Simulink model of the servomotor.

To nd the maximum velocity and acceleration of the servomotors, a step response of the servomotors are performed. The results are seen in Fig. E.2.
Hitec HS645MG Servomotor 3 Position [rad] 2.5 2 1.5 5 Velocity [rad/s] 0 5 10 Acceleration [rad/s2] 100 X: 0.307 Y: 88.24 X: 0.189 Y: 5.213 0.1 0.2 2 1.5 1 0.5 5 0 5 10 200 X: 0.206 Y: 117 X: 0.134 Y: 0.1 7.6 Hitec HSR5995TG Servomotor
0.1
0.2
0.3
0.4
0.1
0.2
0.3
0.3
0.4
0.2
0.3
100
0 X: 0.133 0.1 Y: 96.61
0.2 0.3 Time [s]
0.4
200
0 X: 0.086 Y: 156.2
0.1 Time [s]
0.2
0.3
Figure E.2: Positions, velocities and accelerations of the two servomotors given a
45 step.
Analyzing the step responses, it is seen that the HS-645MG has a maximal velocity of
203
5.213 rad/s positively. This corresponds well with the data sheet value of 5.235 rad/s which is used in the model. This correspondence adds to the verication of the test. 2 The maximal acceleration is found to be 96.61 rad/s . As there are no data sheet values of the acceleration, this value is used in the model.
The HS-5995TG has a maximal velocity of 7.6 rad/s, which is somewhat larger than the data sheet value of 6.98 rad/s. Even though the measured value is 8.2% larger than the data sheet value the relatively small dierence validates the test suciently. The value obtained from the test is used in the model. The maximal acceleration is found to be 2 156.2 rad/s . As there are no data sheet values of the acceleration, this value is used in the model. With the maximal acceleration and velocity determined, the delay and the rst order approximation are determined by supplying a step of increasing magnitude to the servomotors, and tting the parameters to give the best estimate of the response. It was found, that a delay of 0.021 ms for both models and a c parameter of 25 and 15 for the HS-645MG and the HSR-5995TG respectively gave an adequate t. The measured values and the model response with tted parameters is seen in Fig. E.3. Details in the signal edges can be seen in Fig. E.4 and Fig. E.5.
Hitec HS645MG Servomotor Response vs. Model Position [rad] 0.6 0.5 0.4 2 Position [rad] 0.04 0 0.04 2 Position [rad] 3 4 5 6 7 8 Hitec HSR5995TG Servomotor Response vs. Model 9 3 4 5 6 7 8 Hitec HS645MG Servomotor Response vs. Model Error 9
0.8 0.7 0.6 2 3 4 5 6 7 8 Hitec HSR5995TG Servomotor Response vs. Model Error 9
Position [rad]
0.04 0 0.04 2 3 4 5 Time [s] 6 7 8 9
Figure E.3: Increasing pulse response of the servomotors. The dashed line is the
model response and the solid line is the response of the servomotor. The dotted boxes shows the areas plotted in Fig. E.4 and Fig. E.5.
204
APPENDIX E. SERVOMOTOR MODEL
Hitec HS645MG Servomotor Response vs. Model Position [rad] 0.5 0.45 2.02 Position [rad] 0.52 0.5 0.48 0.46 0.44 0.42 3 Position [rad] 0.55 0.5 0.45 0.4 0.35 8.02 0.55 0.5 0.45 0.4 0.35 9 9.02 9.04 9.06 9.08 9.1 Time [s] 9.12 9.14 9.16 9.18 3.02 3.04 3.06 3.08 3.1 3.12 3.14 3.16 3.18 2.04 2.06 2.08 2.1 2.12 2.14 2.16
8.04
8.06
8.08
8.1
8.12
8.14
8.16
8.18
8.2
Figure E.4: Hitec HS-645MG increasing pulse response details. The dashed line is
the model response and the solid line is the response of the servomotor.
Position [rad]
205
Hitec HSR5995TG Servomotor Response vs. Model Position [rad] 0.76 0.74 0.72 0.7 0.68 2 Position [rad] 0.75 0.7 0.65 3.02 Position [rad] 0.8 0.7 0.6 8 Position [rad] 0.8 0.7 0.6 9.05 9.1 9.15 Time [s] 9.2 9.25 8.05 8.1 8.15 8.2 8.25 3.04 3.06 3.08 3.1 3.12 3.14 3.16 3.18 3.2 3.22 2.05 2.1 2.15 2.2 2.25
Figure E.5: Hitec HSR-5995TG increasing pulse response details. The dashed line
is the model response and the solid line is the response of the servomotor.
Appendix
The Newton-Euler Iterations

In this Appendix all the Equations which has to be calculated to nd the expressions for the Newton-Euler model are set up, these equations are set up as described in Ch. 9. All the equations are implemented in Simulink and can be found on the appended CD [5]. First the outward iterations from the supporting foot to the torso are found.
Outward Iterations for the Supporting Leg

The angular velocity, angular acceleration and linear acceleration are found from link RL1 to link RL5.
Angular Velocity
i+1 +1 i i+1 i+1 i+1 = i z i R i + i+1
(F.1)
i = 0...4:
1 rl0 rl1 rl1z rl1 rl1 = rl rl0 + rl0R . . . rl5 rl5 rl5z rl5 rl5 = rl5R rl4 rl4 +
rl1 rl4
Angular Acceleration
i+1 +1 i +1 i i+1 i+1 i+1z i+1 = i i+i i+1 + i+1 z i R i R i i+1
(F.2)
i = 0...4:
rl1
1 rl0 1 rl0 rl1 rl1z rl1 rl1z rl1 = rl rl0 + rl rl1 + rl1 rl0 rl0R rl0R . . . rl5 rl5 rl5z rl5 rl5z rl5 = rl5R rl4 rl4 + rl5R rl4 rl4 rl5 + rl5
rl4 rl4
206
207
Linear Acceleration
i+1 +1 i i+1 = i i ipi+1 + i i i i ipi+1 +i v i v i R
(F.3)
i = 0...4:
rl1
1 rl1 = rl v rl0R . . . 5 rl5 = rl v rl4R
rl0
rl0 rl0P rl1 + rl0 rl0
rl0
rl0 rl0 rl0P rl1 +rl0 v
rl5
rl4
rl4 rl4P rl5 + rl4 rl4
rl4
rl4 rl4 rl4P rl4 +rl4 v
Transformation from the Supporting Leg to the Free Limbs

The angular velocity, angular acceleration and linear acceleration from the torso (RL5) are then transformed to the initiating link of the free limbs (LL0, RA0 and LA0).
Supporting Leg to Non-supporting leg

ll0 ll0
0 rl5 ll0 = ll rl5 rl5R 0 rl5 ll0 = ll rl5 rl5R 0 ll0 = ll v rl5R
rl5
ll0
rl5 rl5pll0 + rl5 rl5
rl5
rl5 rl5 rl5pll0 +rl5 v
Supporting Leg to Right Arm

ra0 ra0
0 rl5 ra0 = ra rl5 rl5R 0 rl5 ra0 = ra rl5 rl5R 0 ra0 = ra v rl5R
rl5
ra0
rl5 rl5pra0 + rl5 rl5
rl5
rl5 rl5 rl5pra0 +rl5 v
Supporting Leg to Left Arm

la0 la0 la0 rl5 la0 = rl rl5 5R
la0 rl5 la0 = rl rl5 5R
la0
0 la0 = la v rl5R
rl5
rl5 rl5pla0 + rl5 rl5
rl5
rl5 rl5 rl5pla0 +rl5 v
Outward Iteration for the Non-Supporting Leg

The angular velocity, angular acceleration, linear acceleration, linear acceleration of the CoM, force and torque acting at the CoM are found from link LL1 to link LL5 for the non-supporting leg.
208
APPENDIX F. THE NEWTON-EULER ITERATIONS
Angular Velocity
i = 1...5:
ll1
1 ll0 ll1 ll1z ll1 ll1 = ll ll0 + ll0R . . . ll5 ll5 ll5z ll5 ll5 = ll5R ll4 ll4 +
ll4
i = 1...5:
ll1
1 ll0 1 ll0 ll1 ll1z ll1 ll1z ll1 + ll1 ll1 = ll ll0 + ll ll0 ll0R ll0R . . . ll5 ll5 ll5z ll5 ll5z ll5 = ll5R ll4 ll4 + ll5R ll4 ll4 ll5 + ll5
ll4 ll4
Linear Acceleration
i = 1...5:
ll1
1 ll1 = ll v ll0R . . . 5 ll5 = ll v ll4R
ll0
ll0 ll0P ll1 + ll0 ll0
ll0
ll0 ll0 ll0P ll1 +ll0 v
ll5
ll4
ll4 ll4P ll5 + ll4 ll4
ll4
ll4 ll4 ll4P ll4 +ll4 v
Linear Acceleration of the CoM

i
Ci =i i ipCi + i i i i ipCi +i v i v
(F.4)
i = 1...5 :
ll1
Cll1 =ll1 ll1 ll1pCll1 + ll1 ll1 v . . . Cll5 =ll5 ll5 ll5pCll5 + ll5 ll5 v
ll1
ll1 ll1 ll1pCll1 +ll1 v ll5 ll5 ll5pCll5 +ll5 v
ll5
ll5
Forces Acting at the CoM

Ci F i = mi v i = 1...5 :
ll1
(F.5)
F ll1 = mll1 . . . F ll5 = mll5
ll1
Cll1 v
ll5
ll5
Cll5 v
209
Torque Acting at the CoM

i + i CiI i i N i = CiI i i = 1...5:
ll1
(F.6)
ll1
N ll1 = Cll1I ll1 . . .
ll1
ll1 + ll1 ll1 Cll1I ll1 ll5 + ll5 ll5 Cll5I ll5
ll1
ll5
N ll5 = Cll5I ll5
ll5
ll5
ll5
Inward Iteration for the Non-Supporting Leg

The force and torque exerted on a link by a neighbouring link and the required joint torque for link LL5 to LL1 are found for the left leg.
Forces Exerted on a Link by a Neighbouring Link

i i+1 f i = iF i + i f i+1 i+1R
(F.7)
i = 5...1 :
ll5
5 ll6 f ll5 = ll5F ll5 + ll f ll6 ll6R . . .
ll1
1 ll2 f ll1 = ll1F ll1 + ll f ll2 ll2R
Torques Exerted on a Link by a Neighbouring Link

i i+1 i+1 ni = iN i + i ni+1 + ipCi iF i + ipi+1 i f i+1 i+1R i+1R
(F.8)
i = 5...1 :
ll5
5 ll6 5 ll6 nll5 = ll5N ll5 + ll nll6 + ll5pCll5 ll5F ll5 + ll5pll6 ll f ll6 ll6R ll6R . . .
ll1
1 ll2 1 ll2 nll1 = ll1N ll1 + ll nll2 + ll1pCll1 ll1F ll1 + ll1pll2 ll f ll2 ll2R ll2R
Required Joint Torque

i i = ini iz i = 5...1 : ll5 ll5 = ll5nll5 ll5z . . . ll5 ll5 = ll5nll5 ll5z
(F.9)
210
Outward Iteration for the Right Arm

The angular velocity, angular acceleration, linear acceleration, linear acceleration of the CoM, force and torque acting at the CoM are found from link RA1 to link RA5 for the Right Arm.
Angular Velocity
i = 1...5:
ra1
1 ra0 ra1 ra1z ra1 ra1 = ra ra0 + ra0R . . . ra5 ra5 ra5z ra5 ra5 = ra5R ra4 ra4 +
ra4
i = 1...5:
ra1
1 ra0 1 ra0 ra1 ra1z ra1 ra1z ra1 = ra ra0 + ra ra1 + ra1 ra0 ra0R ra0R . . . ra5 ra5 ra5z ra5 ra5z ra5 + ra5 ra4 + ra5R ra4 ra4 ra5 = ra5R ra4
ra4 ra4
Linear Acceleration
i = 1...5:
ra1
1 ra1 = ra v ra0R . . . 5 ra5 = ra v ra4R
ra0
ra0 ra0P ra1 + ra0 ra0
ra0
ra0 ra0 ra0P ra1 +ra0 v
ra5
ra4
ra4 ra4P ra5 + ra4 ra4
ra4
ra4 ra4 ra4P ra4 +ra4 v

i = 1...5 :
ra1
Cra1 =ra1 ra1 ra1pCra1 + ra1 ra1 v . . . Cra5 =ra5 ra5 ra5pCra5 + ra5 ra5 v
ra1
ra1 ra1 ra1pCra1 +ra1 v ra5 ra5 ra5pCra5 +ra5 v
ra5
ra5

i = 1...5 :
ra1
F ra1 = mra1 . . . F ra5 = mra5
ra1
Cra1 v
ra5
ra5
Cra5 v
211

i = 1...5:
ra1
N ra1 = Cra1I ra1 . . . N ra5 = Cra5I ra5
ra1
ra1 + ra1 ra1 Cra1I ra1 ra5 + ra5 ra5 Cra5I ra5
ra1
ra1
ra5
ra5
ra5
ra5
Inward Iteration for the Right Arm

The force and torque exerted on a link by a neighbouring link and the required joint torque for link RA5 to RA1 are found for the right arm.
Force Exerted on a Link by a Neighbouring Link

i = 5...1 :
ra5
5 ra6 f ra5 = ra5F ra5 + ra f ra6 ra6R . . . 1 ra2 f ra2 f ra1 = ra1F ra1 + ra ra2R
ra1
Torque Exerted on a Link by a Neighbouring Link

i = 5...1 :
ra5
5 ra6 5 ra6 nra5 = ra5N ra5 + ra nra6 + ra5pCra5 ra5F ra5 + ra5pra6 ra f ra6 ra6R ra6R . . . 1 ra2 1 ra2 nra1 = ra1N ra1 + ra nra2 + ra1pCra1 ra1F ra1 + ra1pra2 ra f ra2 ra2R ra2R
ra1

i = 5...1 : ra5 ra5 = ra5nra5 ra5z . . . ra5 ra5 = ra5nra5 ra5z
Outward Iteration for the Left Arm

The angular velocity, angular acceleration, linear acceleration, linear acceleration of the CoM, force and torque acting at the CoM are found from link LA1 to link LA5 for the left arm.
212
Angular Velocity
i = 1...5:
la1
1 la0 la1 la1z la1 la1 = la la0 + la0R . . . la5 la5 la5z la5 la5 = la5R la4 la4 +
la4
i = 1...5:
la1
1 la0 la1 la0 la1 la1z la1 la1z la1 = la la0 + la la1 + la1 la0 la0R 0R . . . la5 la5 la5z la5 la5z la5 + la5 la5 = la5R la4 la4 + la5R la4 la4
la4 la4
Linear Acceleration
i = 1...5:
la1
1 la1 = la v la0R . . . 5 la5 = la v la4R
la0
la0 la0P la1 + la0 la0
la0
la0 la0 la0P la1 +la0 v
la5
la4
la4 la4P la5 + la4 la4
la4
la4 la4 la4P la4 +la4 v

i = 1...5 :
la1
Cla1 =la1 la1 la1pCla1 + la1 la1 v . . . Cla5 =la5 la5 la5pCla5 + la5 la5 v
la1
la1 la1 la1pCla1 +la1 v la5 la5 la5pCla5 +la5 v
la5
la5

i = 1...5 :
la1
F la1 = mla1 . . . F la5 = mla5
la1
Cla1 v
la5
la5
Cla5 v

i = 1...5:
la1
N la1 = Cla1I la1 . . . N la5 = Cla5I la5
la1
la1 + la1 la1 Cla1I la1 la5 + la5 la5 Cla5I la5
la1
la1
la5
la5
la5
la5
213
Inward Iteration for the Right Arm

The force and torque exerted on a link by a neighbouring link and the required joint torque for link LA5 to LA1 are found for the left arm.
Force Exerted on a Link by a Neighbouring Link

i = 5...1 :
la5
5 la6 f la5 = la5F la5 + la f la6 la6R . . . 1 la2 f la1 = la1F la1 + la f la2 la2R
la1

i = 5...1 :
la5 la5 la6 5 la6 nla5 = la5N la5 + la nla6 + la5pCla5 la5F la5 + la5pla6 la f la6 6R la6R . . . la1 la2 1 la2 nla1 = la1N la1 + la nla2 + la1pCla1 la1F la1 + la1pla2 la f la2 2R la2R
la1

i = 5...1 : la5 la5 = la5nla5 la5z . . . la5 la5 = la5nla5 la5z
Force and Torque Acting on the Torso

The net force and torque acting on the torso are found.
rl5 rl5
F rl51 = mtorso N rl5 =

Ctorso
Crl5 v
rl5
rl5
I torso
rl5 + rl5 rl5 CtorsoI torso
rl5
rl5
Transformation from Frame {1} to Frame {0} in the Free Limbs

The forces and torques acting on the rst link of the free limbs are transformed to the initiating link.
214
Non-Supporting Leg
ll0 ll0
0 f ll0 = ll ll1R
ll0 ll1
ll1 ll1
f ll1 nll1
nll0 =
Right Arm
ra0 ra0
0 f ra0 = ra ra1R
ra0 ra1
ra1 ra1
f ra1 nra1
nra0 =
Left Arm
la0 la0
0 f la0 = la la1R
la0 la1
la1
f la1 nla1
nla0 =
la1
Collective Forces and Torques in RL5

All the forces and torques acting on the free limbs are colleted in RL5.
rl5 rl5
5 ll0 5 ra0 5 la0 f rl5 = rl5F rl5 + rl f ll0 + rl f ra0 + rl f la0 ll0R ra0R la0R 5 ll0 + rl5pCrl5 rl5F rl5 + rl5pll0 rl f ll0 ll0R
5 ll0 5 ra0 5 la0 nrl5 = rl5N rl5 + rl nll0 + rl nra0 + rl nla0 ll0R ra0R la0R 5 ra0 5 la0 + rl5pra0 rl f ra0 + rl5pla0 rl f la0 ra0R la0R
Outward Iterations of the Supporting Leg

The linear acceleration of the CoM, force and torque acting at the CoM are found from link RL1 to link RL4 for the supporting leg.

i = 1...4 :
rl1
Crl1 =rl1 rl1 rl1pCrl1 + rl1 rl1 v . . . Crl5 =rl5 rl5 rl5pCrl5 + rl5 rl5 v
rl1
rl1 rl1 rl1pCrl1 +rl1 v rl5 rl5 rl5pCrl5 +rl5 v
rl5
rl5
215

i = 1...4 :
rl1
F rl1 = mrl1 . . . F rl5 = mrl5
rl1
Crl1 v
rl5
rl5
Crl5 v

i = 1...4:
rl1
N rl1 = Crl1I rl1 . . .
rl1
rl1 + rl1 rl1 Crl1I rl1 rl5 + rl5 rl5 Crl5I rl5
rl1
rl1
rl5
N rl5 = Crl5I rl5
rl5
rl5
rl5
Inward Iteration for the Supporting Leg

The force and torque exerted on a link by a neighbouring link and the required joint torque for link RL5 to RL1 are found for the supporting leg.
Forces Exerted on a Link by a Neighbouring Link

i = 4...1 :
rl4
4 rl5 f rl5 f rl4 = rl4F rl4 + rl rl5R . . . 1 rl2 f rl2 f rl1 = rl1F rl1 + rl rl2R
rl1

i = 4...1 :
rl4
4 rl5 4 rl5 f rl5 nrl4 = rl4N rl4 + rl nrl5 + rl4pCrl4 rl4F rl4 + rl4prl5 rl rl5R rl5R . . . 1 rl2 1 rl2 nrl1 = rl1N rl1 + rl nrl2 + rl1pCrl1 rl1F rl1 + rl1prl2 rl f rl2 rl2R rl2R
rl1

i = 5...1 : rl5 rl5 = rl5nrl5 rl5z . . . rl1 rl1 = rl1nrl1 rl1z
Now all the equations for the Newtonian model are set up and they can be implemented in Simulink.
Appendix
Lagrange
In this chapter all the calculations and simplications needed in the Ch. 10 are written out to enable the reader to go through them if desired.
G.1 Lagrange Model Simplications

This section does write out the complete expression for the coordinates for each joint, and then simplify them by applying small angle approximations, trigonometrically relations, and a few physical considerations. The points for the joints on the biped robot are found using the kinematic and are written again here. First for the legs.
prl1,org = rl0prl1,org prl3,org = prl2,org +
R R rl2prl3,org 0 rl1 rl2 rl3 prl4,org = prl3,org + rl prl4,org rl1R rl2R rl3R rl0 rl1 rl2 rl3 prl5,org = prl4,org + rl1R rl2R rl3R rl4R rl4prl5,org 0 rl1 rl2 rl3 rl4 rl5 0 rl5 pll1,org = prl5,org + rl pll0,org = rl pll0,org rl1R rl2R rl3R rl4R rl5R rl5R 0 rl5 ll0 ll1 pll2,org = pll1,org + rl pll2,org rl5R ll0R ll1R rl0 rl5 ll0 ll1 pll3,org = pll2,org + rl5R ll0R ll1R ll2R ll2pll3,org 0 rl5 ll0 ll1 ll2 ll3 pll4,org = pll3,org + rl pll4,org rl5R ll0R ll1R ll2R ll3R rl0 rl5 ll0 ll1 ll2 ll3 pll5,org = pll4,org + rl5R ll0R ll1R ll2R ll3R ll4R ll4pll5,org
rl0 rl1 rl1 rl2
0 prl2,org = prl1,org + rl rl1R
rl1
prl2,org
Only three points in the each arm is of interest since this is sucient to describe the torque which the arms do aect the whole biped with. These points are the rst two joints in the shoulders and the mass center for the entire arm. These points are found from the {RL5} frame in the supporting leg using the rotation matrices from the kine216
G.1. LAGRANGE MODEL SIMPLIFICATIONS

matic.
0 rl5 pra1,org = prl5,org + rl rl5R ra0R
ra0 ra0 ra1 rl0 rl5
217
pra2,org = pra1,org + pCoM,ra
XCoM,ra 0 rl5 ra0 ra1 YCoM,ra = pra2,org + rl rl5R ra0R ra1R ra2R ZCoM,ra
la0 la0 la1 rl0 rl5
rl5 ra0
R ra1pra2,org
pra1,org
0 rl5 pla1,org = prl5,org + rl rl5R la0R
pla2,org = pla1,org + pCoM,la
XCoM,la 0 rl5 la0 la1 YCoM,la = pla2,org + rl rl5R ra0R la1R la2R ZCoM,la
R R R la1pla2,org
rl5 la0
pla1,org
And for the body.
pCoM,t
XCoM,t 0 YCoM,t = prl5,org + rl rl5R ZCoM,t
To make these expressions useful they must be written out in their fully length, and simplied. The simplication is manly based on that the expressions contain a lot of redundant information to describe a gait cycle, which can be cut away. E.g. does some joints turn within a limited set of angles when walking, and enabling small angle approximations. Further does some lengths contributes with little information on the position as a whole, and hence can be neglected. To make the following expressions more foreseeable has the following notation been used.
rl =rl2 + rl3 + rl4 ll1+ =rl2 + rl3 + rl4 + ll2 ll2+ =rl2 + rl3 + rl4 + ll2 + ll3 ll3+ =rl2 + rl3 + rl4 + ll2 + ll3 + ll4 ll1- =rl2 + rl3 + rl4 ll2 ll2- =rl2 + rl3 + rl4 ll2 ll3 ll3- =rl2 + rl3 + rl4 ll2 ll3 ll4
Leg Points
First is the equation for the points in the legs found.
218
prl1,org
0 = arl0 0 0
prl2,org
prl3,org
prl4,org
prl5,org
pll1,org
pll2,org
pll3,org
APPENDIX G. LAGRANGE
pll4,org
cos(rl1 ) = prl1,org + arl1 sin(rl1 ) 0 cos(rl2 ) cos(rl1 ) = prl2,org + arl2 cos(rl2 ) sin(rl1 ) sin(rl2 ) cos(rl2 + rl3 ) cos(rl1 ) = prl3,org + arl3 cos(rl2 + rl3 ) sin(rl1 ) sin(rl2 + rl3 ) cos(rl ) cos(rl1 ) = prl4,org + arl4 cos(rl ) sin(rl1 ) sin(rl ) cos(rl ) cos(rl1 ) sin(rl5 ) + sin(rl1 ) cos(rl5 ) = prl5,org + ahip cos(rl ) sin(rl1 ) sin(rl5 ) cos(rl1 ) cos(rl5 ) sin(rl ) sin(rl5 ) cos(rl ) cos(rl1 ) cos(hip ) + sin(rl1 ) sin(hip ) = pll1,org + all1 cos(rl ) sin(rl1 ) cos(hip ) cos(rl1 ) sin(hip ) sin(rl ) cos(hip ) (cos(rl ) cos(rl1 ) cos(hip ) sin(rl1 ) sin(hip )) cos(ll2 ) cos(rl1 ) sin(rl ) sin(ll2 ) = pll2,org + all2 (cos(rl ) sin(rl1 ) cos(hip ) + cos(rl1 ) sin(hip )) cos(ll2 ) sin(rl1 ) sin(rl ) sin(ll2 ) sin(rl ) cos(hip ) cos(ll2 ) cos(rl ) sin(ll2 ) (cos(rl ) cos(rl1 ) cos(hip ) sin(rl1 ) sin(hip )) cos(ll2 + ll3 ) cos(rl1 ) sin(rl ) sin(ll2 + ll3 ) = pll3,org + all3 (cos(rl ) sin(rl1 ) cos(hip ) + cos(rl1 ) sin(hip )) cos(ll2 + ll3 ) sin(rl1 ) sin(rl ) sin(ll2 + ll3 ) sin(rl ) cos(hip ) cos(ll2 + ll3 ) cos(rl ) sin(ll2 + ll3 )
pll5,org
(cos(rl ) cos(rl1 ) cos(hip ) sin(rl1 ) sin(hip )) cos(lls2 ) cos(rl1 ) sin(rl ) sin(lls2 ) = pll4,org + all4 (cos(rl ) sin(rl1 ) cos(hip ) + cos(rl1 ) sin(hip )) cos(lls2 ) sin(rl1 ) sin(rl ) sin(lls2 ) sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )
By examine the human gait in App. A it is determined that the servo angles in the sagittal plane, at any point in the gait, are less than 0.26 rad. This enables two simplications
sin() cos() 1
When these approximations are carried out it is further assumed that
i j 0
This gives the following coordinates in the legs
prl1,org 1
0 = arl0 0 0
prl2,org = prl1,org + arl1 rl1 0 cos(rl2 ) prl3,org = prl2,org + arl2 cos(rl2 )rl1 sin(rl2 ) cos(rl2 + rl3 ) prl4,org = prl3,org + arl3 cos(rl2 + rl3 )rl1 sin(rl2 + rl3 )
219
220
prl5,org
pll1,org
pll2,org
pll3,org
pll4,org
pll5,org
cos(rl ) = prl4,org + arl4 cos(rl )rl1 sin(rl ) cos(rl )rl5 + rl1 1 = prl5,org + ahip sin(rl )rl5 cos(rl ) = pll1,org + all1 cos(rl )rl1 rl5 ll5 sin(rl ) cos(rl ) cos(ll2 ) sin(rl ) sin(ll2 ) = pll2,org + all2 cos(rl )rl1 cos(ll2 ) (rl5 + ll1 ) cos(ll2 ) rl1 sin(rl ) sin(ll2 ) sin(rl ) cos(ll2 ) cos(rl ) sin(ll2 ) cos(rl ) cos(ll2 + ll3 ) sin(rl ) sin(ll2 + ll3 ) = pll3,org + all3 cos(rl )rl1 cos(ll2 + ll3 ) (rl5 + rl5 ) cos(ll2 + ll3 ) rl1 sin(rl ) sin(ll2 + ll3 ) sin(rl ) cos(ll2 + ll3 ) cos(rl ) sin(ll2 + ll3 ) cos(rl ) cos(lls2 ) sin(rl ) sin(lls2 ) = pll4,org + all4 cos(rl )rl1 cos(lls2 ) (rl5 + rl5 ) cos(lls2 ) rl1 sin(rl ) sin(lls2 ) sin(rl ) cos(lls2 ) cos(rl ) sin(lls2 )
Now the following trigonometric relations can be applied:
sin(A) cos(B ) sin(B ) cos(A) = sin(A B ) cos(A) cos(B ) sin(B ) sin(A) = cos(A B )
prl1,org
0 = arl0 0 0
G.1. LAGRANGE MODEL SIMPLIFICATIONS 221
prl2,org = prl1,org + arl1 rl1 0 cos(rl2 ) prl3,org = prl2,org + arl2 cos(rl2 )rl1 sin(rl2 ) cos(rl2 + rl3 ) prl4,org = prl3,org + arl3 cos(rl2 + rl3 )rl1 sin(rl2 + rl3 ) cos(rl ) prl5,org = prl4,org + arl4 cos(rl )rl1 sin(rl ) cos(rl )rl5 + rl1 1 pll1,org = prl5,org + ahip sin(rl )rl5 cos(rl ) pll2,org = pll1,org + all1 cos(rl )rl1 rl5 ll5 sin(rl ) cos(ll1- ) pll3,org = pll2,org + all2 rl1 cos(ll1- ) (rl5 + ll1 ) cos(ll2 ) sin(ll1- ) cos(ll2- ) pll4,org = pll3,org + all3 rl1 cos(ll2- ) (rl5 + ll1 ) cos(ll2 + ll3 ) sin(ll2- ) cos(ll3- ) pll5,org = pll4,org + all4 rl1 cos(ll3- ) (rl5 + ll1 ) cos(lls2 ) sin(ll3- )
222
By analyzing the human gait closely in App. A does the following angles varying within the given limits. to to to to to to to to to
rl2 rl2 + rl3 rl2 + rl3 + rl4 ll4 ll4 + ll3 ll4 + ll3 + ll2 ll2 rl2 rl3 rl4 ll2 + ll3 rl2 rl3 rl4 ll2 + ll3 + ll4 rl2 rl3 rl4
: 15 : 26 : 20 : 12 : 52 : 70 : 25 : 45 : 65
10 4 15 22 25 10 40 32 22
As the four rst sums lies within 26 degrees (0.45 rad) small angel approximations can be applied. Further more are the following approximation found in App. A.
ll3 + ll4 rl2 rl3 rl4 ll2 + ll3 ll2 + ll3 + ll4 rl2 rl3 rl4 ll2 + ll3 + ll4
Applying these simplications gives.
prl1,org
0 = arl0 0 0 1
prl2,org = prl1,org + arl1 rl1 0
prl3,org = prl2,org + arl2 rl1 rl2 1 rl1 prl4,org = prl3,org + arl3 (rl2 + rl3 ) 1 prl5,org = prl4,org + arl4 rl1 (rl ) 0 1 pll1,org = prl5,org + ahip (rl )rl5 1 pll2,org = pll1,org + all1 rl1 rl5 ll1 rl cos(ll1- ) pll3,org = pll2,org + all2 rl1 cos(rl2 + rl3 + rl4 ll2 ) (rl5 + ll1 ) sin(ll1- ) cos(ll2- ) pll4,org = pll3,org + all3 (rl1 + rl5 + ll1 ) cos(ll2 + ll3 ) sin(ll2- ) cos(ll3- ) pll5,org = pll4,org + all4 (rl1 + rl5 + ll1 ) cos(lls2 ) sin(ll3- )
Finally are a couple of physical views applied on the expressions.
The y -components of pllj5 and pllj4 are approximated with ll1 . This is justied by the fact that the sum of rl1 + rl5 is small compared to ll1 . This removes a part which would have made the y-component bit larger and hence is the cos() also removed, since this will make the y-component
223
224
a bit smaller. The two removed parts rl1 + rl5 and cos() are not the exact inverse of each other and hence will introduce some extra error, but it is considered acceptable as long as the angles stays within the limits specied by the human gait, and it will make the further calculations much easier.
This gives the following expression for pll5,org and pll4,org .
pll4,org
pll5,org
cos(ll2- ) = pll3,org + all3 ll1 sin(ll2- ) cos(ll3- ) = pll4,org + all4 ll1 sin(ll3-
With a similar argumentation is pll3,org simplied to:
pll3,org
cos(ll1- ) = pll2,org + all2 rl5 ll1 sin(ll1-
Now are the expressions cut down to hold just enough information to describe the gait specied by the analysis of the human gait, and hence they are ready to be used in calculating the Lagrangian, which is carried out in App. G.2.
These approximations in the supporting leg do however introduce a larger error when more body parts are added. Hence to comply with the needed degree of precision when calculating the height of {RL5} is it adjusted as follows. The height of {RL5} is in the frontal plane approximated with (arl1 + arl2 + arl3 + arl4 ) cos(rl1 ), and in the saggital plane by arl1 + (arl2 + arl3 ) cos(rl2 ) + arl4 . The reason why not one expression is used for both planes is to keep the calculations simpler.
Mass Center of the Body
In this section are the constants XCoM,t , YCoM,t , and ZCoM,t used instead of actual numbers, because these can change as the physical design of the biped changes. This will ease implementation of this model when further development on the biped are carried out.
The position of the mass center written fully out are given below. (cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 ))XCoM,t (cos(rl ) cos(rl1 ) sin(rl5 ) + sin(rl1 ) cos(rl5 ))YCoM,t + cos(rl1 ) sin(rl )ZCoM,t (cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 ))XCoM,t pCoM,t = prl5,org + (cos(rl ) sin(rl1 ) sin(rl5 ) + cos(rl1 ) cos(rl5 ))YCoM,t + sin(rl1 ) sin(rl )ZCoM,t sin(rl ) cos(rl5 )XCoM,t + sin(rl ) sin(rl5 )YCoM,t + cos(rl )ZCoM,t
Applying the same small angel approximation as on the legs gives, cos(rl )XCoM,t (cos(rl )rl5 + rl1 )YCoM,t + sin(rl )ZCoM,t (cos(rl )rl1 + rl5 )XCoM,t + YCoM,t + rl1 ZCoM,t pCoM,t = prl5,org + sin(rl )XCoM,t + sin(rl )rl5 YCoM,t + cos(rl )ZCoM,t
and applying the knowledge that small angle approximation can be used on rl and the sum of rl1 + rl5 is very close to zero states: XCoM,t + ZCoM,t (rl ) YCoM,t + rl1 ZCoM,t pCoM,t = prl5,org + (rl )(YCoM,t rl5 XCoM,t ) + ZCoM,t
Finally is the ZCoM,t in this project insignicant compared to XCoM,t and YCoM,t , and is hence removed from the expression for pCoM,t . Further is the YCoM,t rl5 insignicant compared with XCoM,t and is removed as well. This results in the following expression.
pCoM,t
XCoM,t = prl5,org + YCoM,t rl XCoM,t
Now is this expression also ready to be included in the calculations of the Lagrangian.
225
226
Arm Points
The points in the arms are written out below.
pra1,org
pra2,org
pla1,org
pla2,org
50.5 cos(rl1 ) cos(rl ) cos(rl5 ) 50.5 sin(rl1 ) sin(rl5 ) 49.1 cos(rl ) cos(rl1 ) sin(rl5 ) 49.1 sin(rl1 ) cos(rl5 ) + 0.5 cos(rl1 ) sin(rl ) 50.5 sin(rl1 ) cos(rl ) cos(rl5 ) + 50.5 cos(rl1 ) sin(rl5 ) = prl5,org + 49.1 cos(rl ) sin(rl1 ) sin(rl5 ) + 49.1 cos(rl1 ) cos(rl5 ) + 0.5 sin(rl1 ) sin(rl ) 50.5 sin rl2 + rl3 + rl4 cos(rl5 ) + 49.1 sin(rl ) sin(rl5 ) + 0.5 cos(rl ) 18.7(cos(rl1 ) cos(rl ) cos(rl5 ) sin(rl1 ) sin(rl5 )) cos(ra1 ) 18.7 cos(rl1 ) sin(rl ) sin(ra1 ) +1.3(cos(rl1 ) cos(rl ) cos(rl5 ) sin(rl1 ) sin(rl5 )) sin(ra1 ) + 1.3 cos(rl1 ) sin(rl ) cos(ra1 ) 18.7(sin(rl1 ) cos(rl ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) cos(ra1 ) 18.7 sin(rl1 ) sin(rl ) sin(ra1 ) = pra1,org + +1.3(sin(rl1 ) cos(rl ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) sin(ra1 ) + 1.3 sin(rl1 ) sin(rl ) cos(ra1 ) 18.7 sin(rl ) cos(rl5 ) cos(ra1 ) 18.7 cos(rl ) sin(ra1 ) 1.3 sin(rl ) cos(rl5 ) sin(ra1 ) + 1.3 cos(rl ) cos(ra1 ) 50.5 cos(rl1 ) cos(rl ) cos(rl5 ) 50.5 sin(rl1 ) sin(rl5 ) +110.7 cos(rl ) cos(rl1 ) sin(rl5 ) + 110.7 sin(rl1 ) cos(rl5 ) + 0.5 cos(rl1 ) sin(rl ) 50.5 sin(rl1 ) cos(rl ) cos(rl5 ) + 50.5 cos(rl1 ) sin(rl5 ) = prl5,org + +110.7 cos(rl ) sin(rl1 ) sin(rl5 ) 110.7 cos(rl1 ) cos(rl5 ) + 0.5 sin(rl1 ) sin(rl ) 50.5 sin(rl ) cos(rl5 ) 110.7 sin(rl ) sin(rl5 ) + 0.5 cos(rl ) 18.7(cos(rl1 ) cos(rl ) cos(rl5 ) sin(rl1 ) sin(rl5 )) cos(la1 ) + 18.7 cos(rl1 ) sin(rl ) sin(la1 ) 1.3(cos(rl1 ) cos(rl ) cos(rl5 ) sin(rl1 ) sin(rl5 )) sin(la1 ) + 1.3 cos(rl1 ) sin(rl ) cos(la1 ) 18.7(sin(rl1 ) cos(rl ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) cos(la1 ) + 18.7 sin(rl1 ) sin(rl ) sin(la1 ) = pla1,org + 1.3(sin(rl1 ) cos(rl ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) sin(la1 ) + 1.3 sin(rl1 ) sin(rl ) cos(la1 ) 18.7 sin(rl ) cos(rl5 ) cos(la1 ) + 18.7 cos(rl ) sin(la1 ) +1.3 sin(rl ) cos(rl5 ) sin(la1 ) + 1.3 cos(rl ) cos(la1 )
(((cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 )) cos(ra1 ) cos(rl1 ) sin(rl ) sin(ra1 )) sin(ra2 ) +( cos(rl ) cos(rl1 ) sin(rl5 ) sin(rl1 ) cos(rl5 )) cos(ra2 )) XCoM,ra + (((cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 )) cos(ra1 ) cos(rl1 ) sin(rl ) sin(ra1 )) cos(ra2 ) ( cos(rl ) cos(rl1 ) sin(rl5 ) sin(rl1 ) cos(rl5 )) sin(ra2 )) YCoM,ra +((cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 )) sin(ra1 ) + cos(rl1 ) sin(rl ) cos(ra1 ))ZCoM,ra (((cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) cos(ra1 ) sin(rl1 ) sin(rl ) sin(ra1 )) sin(ra2 ) +( cos(rl ) sin(rl1 ) sin(rl5 ) + cos(rl1 ) cos(rl5 )) cos(ra2 )) XCoM,ra pCoM,ra = pra2,org + + (((cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) cos(ra1 ) sin(rl1 ) sin(rl ) sin(ra1 )) cos(ra2 ) ( cos(rl ) sin(rl1 ) sin(rl5 ) + cos(rl1 ) cos(rl5 )) sin(ra2 )) YCoM,ra +((cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) sin(ra1 ) + sin(rl1 ) sin(rl ) cos(ra1 ))ZCoM,ra (( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) sin(ra2 ) + sin(rl ) sin(rl5 ) cos(ra2 ))XCoM,ra +(( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) cos(ra2 ) sin(rl ) sin(rl5 ) sin(ra2 ))YCoM,ra +( sin(rl ) cos(rl5 ) sin(ra1 ) + cos(rl ) cos(ra1 ))ZCoM,ra (((cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 )) cos(la1 ) + cos(rl1 ) sin(rl ) sin(la1 )) sin(la2 ) +(cos(rl ) cos(rl1 ) sin(rl5 ) + sin(rl1 ) cos(rl5 )) cos(la2 )) XCoM,la + (((cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 )) cos(la1 ) + cos(rl1 ) sin(rl ) sin(la1 )) cos(la2 ) (cos(rl ) cos(rl1 ) sin(rl5 ) + sin(rl1 ) cos(rl5 )) sin(la2 )) YCoM,la +((cos(rl ) cos(rl1 ) cos(rl5 ) sin(rl1 ) sin(rl5 )) sin(la1 ) + cos(rl1 ) sin(rl ) cos(la1 ))ZCoM,la (((cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) cos(la1 ) + sin(rl1 ) sin(rl ) sin(la1 )) sin(la2 ) +(cos(rl ) sin(rl1 ) sin(rl5 ) cos(rl1 ) cos(rl5 )) cos(la2 )) XCoM,la pCoM,la = pla2,org + + (((cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) cos(la1 ) + sin(rl1 ) sin(rl ) sin(la1 )) cos(la2 ) (cos(rl ) sin(rl1 ) sin(rl5 ) cos(rl1 ) cos(rl5 )) sin(la2 )) YCoM,la +((cos(rl ) sin(rl1 ) cos(rl5 ) + cos(rl1 ) sin(rl5 )) sin(la1 ) + sin(rl1 ) sin(rl ) cos(la1 ))ZCoM,la (( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) sin(la2 ) sin(rl ) sin(rl5 ) cos(la2 ))XCoM,la +(( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) cos(la2 ) + sin(rl ) sin(rl5 ) sin(la2 ))YCoM,la +(sin(rl ) cos(rl5 ) sin(la1 ) + cos(rl ) cos(la1 ))ZCoM,la
227
228
Here is the small angle approximation on the angles in the frontal plane on the supporting leg changed to
sin() 0 cos() 1
This is reasonable as there now is a lot more expressions for each coordinate, and hence does one expression multiplied by sinus or cosine to a small angle have small impact on the entire expression. This will, however, make the points in the arms particular fragile to changes in the frontal plane in the supporting leg.
This result in the following expressions
pra1,org
pra2,org
pCoM,ra
50.5 cos(rl ) + 0.5 sin(rl ) 49.1 = prl5,org + 50.5 sin(rl ) + 0.5 cos(rl ) 18.7 cos(rl ) cos(ra1 ) 18.7 sin(rl ) sin(ra1 ) +1.3 cos(rl ) sin(ra1 ) + 1.3 sin(rl ) cos(ra1 ) 0 = pra1,org + 18.7 sin(rl ) cos(ra1 ) 18.7 cos(rl ) sin(ra1 ) 1.3 sin(rl ) sin(ra1 ) + 1.3 cos(rl ) cos(ra1 ) (cos(rl ) cos(ra1 ) sin(rl ) sin(ra1 )) sin(ra2 )XCoM,ra (cos(rl ) cos(ra1 ) sin(rl ) sin(ra1 )) cos(ra2 )YCoM,ra +(cos(rl ) sin(ra1 ) + sin(rl ) cos(ra1 ))ZCoM,ra cos(ra2 )XCoM,ra sin(ra2 )YCoM,ra = pra2,org + ( sin(rl ) cos(ra1 ) cos(rl ) sin(ra1 )) sin(ra2 )XCoM,ra ( sin(rl ) cos(ra1 ) cos(rl ) sin(ra1 )) cos(ra2 )YCoM,ra +( sin(rl ) sin(ra1 ) + cos(rl ) cos(ra1 ))ZCoM,ra
pla1,org
pla2,org
pCoM,la
50.5 cos(rl ) + 0.5 sin(rl ) 110.7 = prl5,org + 50.5 sin(rl ) + 0.5 cos(rl ) 18.7 cos(rl ) cos(la1 ) + 18.7 sin(rl ) sin(la1 ) 1.3 cos(rl ) sin(la1 ) + 1.3 sin(rl ) cos(la1 ) 0 = pla1,org + 18.7 sin(rl ) cos(la1 ) + 18.7 cos(rl ) sin(la1 ) +1.3 sin(rl ) sin(la1 ) + 1.3 cos(rl ) cos(la1 ) (cos(rl ) cos(la1 ) + sin(rl ) sin(la1 )) sin(la2 )XCoM,la +(cos(rl ) cos(la1 ) + sin(rl ) sin(la1 )) cos(la2 )YCoM,la (cos(rl ) sin(la1 ) + sin(rl ) cos(la1 ))ZCoM,la cos( ) X + sin( ) Y = pla2,org + la2 CoM,la la2 CoM,la ( sin(rl ) cos(la1 ) + cos(rl ) sin(la1 )) sin(la2 )XCoM,la +( sin(rl ) cos(la1 ) + cos(rl ) sin(la1 )) cos(la2 )YCoM,la +(sin(rl ) sin(la1 ) + cos(rl ) cos(la1 ))ZCoM,la
Now removing the parts which at most contribute with 1.3mm (and Z), and applying both the small angle approximation on the supporting leg and the following trigonometrical relations,
cos(A) cos(B ) sin(A) sin(B ) = cos(A + B ) sin(A) cos(B ) + sin(B ) cos(A) = sin(A + B ) sin(A) cos(B ) + sin(B ) cos(A) = sin(B A)
229
230
result in:
pra1,org
pra2,org
pCoM,ra
pla1,org
pla2,org
pCoM,la
50.5 49.1 = prl5,org + 50.5(rl ) 18.7 cos(ra+ ) 0 = pra1,org + 18.7 sin(ra+ ) cos(ra+ )(sin(ra2 )XCoM,ra cos(ra2 )YCoM,la ) cos(ra2 )XCoM,la sin(ra2 )YCoM,ra = pra2,org + sin(ra+ )(sin(ra2 )XCoM,ra + cos(ra2 )YCoM,ra ) 50.5 = prl5,org + 110.7 50.5(rl ) 18.7 cos(la- ) 0 = pla1,org + 18.7 sin(la- ) cos(la- )(sin(la2 )XCoM,la + cos(la2 )YCoM,la ) cos(la2 )XCoM,la + sin(la2 )YCoM,la = pla2,org + sin(la- )(sin(la2 )XCoM,la + cos(la2 )YCoM,la )
231
Once again examin the human gait it is seen that the joints in the arms only have small changes in the angles which enables a small angle approximation on the shoulder joints. In fact are the angle in the joints in the frontal plane, pra2,org and pla2,org , very close to zero. For that reason is the following simplications made to the coordinates.
sin(ra1 ) ra1 cos(ra1 ) 1 sin(ra2 ) 0 cos(ra2 ) 1

And likewise for the left arm. This result in
pra1,org
pra2,org
pCoM,ra
pla1,org
pla2,org
pCoM,la
50.5 49.1 = prl5,org + 50.5(rl ) 18.7 cos(ra+ ) 0 = pra1,org + 18.7 sin(ra+ ) cos(ra+ )YCoM,ra XCoM,ra = pra2,org + sin(ra+ )YCoM,ra 50.5 = prl5,org + 110.7 50.5(rl ) 18.7 cos(la- ) 0 = pla1,org + 18.7 sin(la- ) cos(la- )YCoM,la XCoM,la = pla2,org + sin(la- )YCoM,la
Now all the desired points of the biped have been simplied to get rid of redundant information to describe dynamic gait. Then these points are ready to be used in the energy considerations needed to calculate the Lagrangian.
232
G.2 Determination of Kinetic- and Potential Energy

In this section are the kinetic- and potential energies of the biped determined from the simplied coordinates.
Kinetic Energy
In order to determine the Lagrangian the kinetic energies must be known, and these are found using Eq. (G.1)
ki =
1 1 i i + i p mi p I i 2 2 i
ki,l ki,r
(G.1)
The rst part represent the linear part of the kinetic energy, denoted ki,l , and the second part represent the rotational kinetic energy, denoted ki,r [Craig, 2003]. The determination of the kinetic energy is divided into two, rst the linear part and then the rotational part.
The Linear Part

To determine the linear kinetic energy are the points found in the previous section dierentiated with regard to time.
p rl1,org
0 = arl0 0 0
rl1 p rl2,org = p rl1,org + arl1 0 0 rl1 p rl3,org = p rl2,org + arl2 rl2 0 rl1 p rl4,org = p rl3,org + arl3 (rl2 + rl3 ) 0 rl1 p rl5,org = p rl4,org + arl4 rl 0 0 p CoM,t = p rl5,org + rl XCoM,t 0 0 p ll1,org = p rl5,org + ahip rl5 rl rl5 (rl ) 0 rl1 rl5 ll1 p ll2,org = p ll1,org + all1 rl
G.2. DETERMINATION OF KINETIC- AND POTENTIAL ENERGY

ll1- sin(ll1- ) rl5 ll1 =p ll2,org + all2 ll1cos(ll1- ) ll2- sin(ll2- ) ll1 =p ll3,org + all3 ll2cos(ll2- ) ll3- sin(ll3- ) ll1 =p ll4,org + all4 cos(ll3- )ll3 0 0 =p rl5,org + 50.5rl ra 18.7 sin(ra+ ) 0 =p ra1,org + ra 18.7 cos(ra+ ) ra YCoM,ra sin(ra+ ) 0 =p ra2,org + cos(ra+ )ra YCoM,ra 0 0 =p rl5,org + 50.5rl la 18.7 sin(la- ) 0 =p la1,org + la 18.7 cos(la- ) la YCoM,la sin(la- ) 0 =p la2,org + cos(la- )la YCoM,la
233
p ll3,org
p ll4,org
p ll5,org
p ra1,org
p ra2,org
p CoM,ra
p la1,org
p la2,org
p CoM,la
From these velocities of the approximated mass centers of the relevant joints, can the linear kinetic energy be determined. Starting with the supporting leg:
1 rl0,org p rl0,org = 0 krl0,l = mrl0 p 2 1 rl1 )2 krl1,l = mrl1 (arl1 2 y rl1
1 rl1 )2 + (arl2 rl2 )2 krl2,l = mrl2 (y rl1 + arl2 2

y rl2 z rl2
1 rl1 )2 + (z rl2 + rl3 ))2 rl2 + arl3 rl2 arl3 ( krl4,l = mrl4 (y 2
y rl4 z rl4
The energies krl5,l and kll1,l are included in the energy for the body, hence not described individually, but the velocities of prl5 and pll1 are needed. Thus are their components
234
given here.
rl1 x rl5 = (arl1 + arl2 + arl3 + arl4 ) sin(rl1 ) rl2 x rl5,2 = (arl2 + arl3 ) sin(rl2 ) y rl5 z rl5 x ll1 rl1 =y rl4 + arl4 rl =z rl4 arl4 =x rl5
(G.2) (G.3)
rl5 + rl rl5 ) z ll1 = z rl5 ahip ((rl )

The reason why it suddenly appear that x rl5 is dierent from zero as it would be when adding the x-components of the velocities of the joints in the supporting leg is, as mention in Sec. G.1, that the small angle approximations does introduce a too large error when looking at the supporting leg as a whole. Hence is the velocity of xrl5 approximated with two dierent expressions, Eq. (G.2) and Eq. (G.3), to keep the calculations as simple as possible. x rl5 is used when looking at the biped robot in the frontal plane and x rl5,2 is used for the sagittal plane. These velocities origin from the hight approximations xrl5 = (arl1 + arl2 + arl3 + arl4 ) cos(rl1 ) and xrl5,2 = arl1 + (arl2 + arl3 ) cos(rl2 ) + arl4 . In the following derivation of the energies is only xrl5 used to keep the derivation more foreseeable, and further is it rather simple to replace xrl5 with xrl5,2 when needed. The kinetic energies in the swing leg are given by.
1 rl1 + rl5 + ll1 ))2 + (z rl )2 kll1,l = mll1 (x ll1 )2 + (y rl5 all1 ( ll1 + all1 2 kll3,l =
y ll2 z ll2
1 ll1- )2 + (y rl5 + ll1 ))2 + (z ll1- )2 mll3 (x ll1 + all2 sin(ll1- ) ll2 all2 ( ll2 + all2 cos(ll1- ) 2
x ll3 y ll3 z ll3
1 ll2- )2 + (y ll1 )2 + (z ll2- )2 kll4,l = mll4 (x ll3 + all3 sin(ll2- ) ll3 all3 ll3 + all3 cos(ll2- ) 2
x ll4 y ll4 z ll4
1 ll3- )2 + (y ll1 )2 + (z ll3- )2 kll5,l = mll5 (x ll4 + all4 sin(ll3- ) ll4 all4 ll4 + all4 cos(ll3- ) 2
The kinetic energy in the body is given by
1 rl )2 kt = mt (x rl5 )2 + (y rl5 )2 + (z rl5 XCoM,t 2

As stated earlier is one torque from each arm enough to describe its aect on the whole biped robot for this project. To nd the velocity of the mass center of each arm, the velocity contributions from the two shoulder joints must be found.
ra x ra = x rl5 18.7 sin(ra+ ) rl 18.7 cos(ra+ ) ra z ra = z rl5 50.5 la x la = x rl5 18.7 sin(la- ) rl 18.7 cos(la- ) la z la = z rl5 50.5

The linear kinetic energies in the arms are then given by
235
1 ra YCoM,ra )2 + (y ra YCoM,ra )2 kra,l = marm (x ra + sin(ra+ ) rl5 )2 + (z ra + cos(ra+ ) 2 1 la YCoM,ra )2 + (y la YCoM,la )2 kla,l = marm (x la sin(la- ) rl5 )2 + (z la cos(la- ) 2
The Rotational Part

The inertia tensor of the rst link in a chain (e.g. from a foot to the hip) given as:
Ixx,1 I 1 = Iyx,1 Izx,1
Ixy,1 Iyy,1 Izy,1
Ixz,1 Iyz,1 Izz,1
(G.4)
When two links are put together is the inertia given by Eq. (G.4) added by the inertia from the second link which is found using the Parallel Axis Theorem.[Serway and Jewett, 2004] This gives:
Ixx,1 I = Iyx,1 Izx,1
Ixy,1 Iyy,1 Izy,1
Ixz,1 T 1 Iyz,1 + 1 2R I 2 2R + m1 d21 (t)A3 Izz,1
(G.5)
where 1 2R is the rotation matrix rotating the second link, to align it with an axis which is parallel with the z-axis of the rst link, enabling the parallel axis theorem to be applied. A3 is the 33 identity matrix. m1 is the mass of the rst link and is hence constant. d21 (t) is the distance from joint two to the mass center of link one and dependent of the rst joint angle and hence dependent of time. From Eq. (G.1) it is seen that only the Izz part of the matrices found in Eq. (G.5) contribute to the kinetic energy since all joints rotate around the z-axis which gives.
0 i = 0 z,i
Using this approach gives the following expressions for the moments of inertia for the rotational part of the kinetic energies in the swing leg.
I ll0 =I zz,ll0
0 T LL0 I ll1 =I zz,ll1 + LL LL1R Ill0 LL1R + mll5 all4 A3 1 T LL1 LL1 LL0 T LL0 LL1 I ll2 =I zz,ll2 + LL LL2R Ill1 LL2R + mll4 all3 I 3 + (LL2R LL1R ) Ill0 (LL1R LL2R ) + mll5 (all4 + all3 )A3
. . .
As seen do these expressions become large when moving toward the supporting leg. To avoid unnecessary calculations is it rst determined which angles the dierent moments rl1 , of inertia depend on, because later when only dierentiate with regard to rl1 , rl2 , and rl2 , will some moments of inertia appear as constants. Hence not worth to
236
derive as they contribute with zero.
krl0,r =0 1 rl1 krl1,r = I (rl2 , rl3 , rl4 , rl5 , ll1 , ll2 , ll3 , ll4 , ll5 , ra1 , ra2 , la1 , la2 ) 2 rl1 1 rl2 krl2,r = I (rl3 , rl4 , rl5 , ll1 , ll2 , ll3 , ll4 , ll5 , ra1 , ra2 , la1 , la2 ) 2 rl2 1 rl4 krl4,r = I (rl5 , ll1 , ll2 , ll3 , ll4 , ll5 , ra1 , ra2 , la1 , la2 ) 2 rl4 1 ll1, kll1,r = I (ll2 , ll3 , ll4 , ll5 ) 2 ll1 1 ll3 kll3,r = I (ll4 , ll5 ) 2 ll3 1 ll4 kll4,r = I (ll5 ) 2 ll4 1 2 kll5,r = Izz,ll5 2 ll5 1 rl5 kt,r = I (ll1 , ll2 , ll3 , ll4 , ll5 , ra1 , ra2 , la1 , la2 ) 2 rl5 1 ra kra,r = I arm 2 ra 1 la I arm kla,r = 2 la
For this project are only the moments of inertia in joint RL1 and RL2 of interest and hence are only krl1,r and krl2,r needed as the other rotational kinetic energies will appear as constants. The expressions for Irl1,t and Irl2,t are derived in Sec. G.3 and shown below:
Irl1,t =Izz,rl1 + Iyy,rl2 + mrl2 (arl1 + arl2 )2 + Iyy,rl4 + mrl4 (arl1 + arl2 + arl3 )2 + 2 rl Ixx,t + Izz,t + mt (arl1 + arl2 + arl3 + arl4 + Xt )2 + Izz,ll1 + mll1 (arl1 + arl2 + arl3 )2 + a2 hip
2 2 2 2 + Ixx,ll3 + mll3 (arl1 + arl2 )2 + a2 hip + sin (ll2- )Ixx,ll4 + cos (ll2- )Iyy,ll4 + mll4 arl1 + ahip 2 2 + sin2 (ll2- )Ixx,ll5 + cos2 (ll2- )Izz,ll5 + mll5 a2 hip + sin (ra+ )Iyy,ra + cos (ra2+ )Izz,ra
+ marm (((arl1 + arl2 + arl3 + arl4 ) cos(rl1 ) + 50.5 + 18.7 cos(ra ) Yarm cos(ra ))2 + (49.1 + Xra )2 ) + sin2 (la- )Iyy,larm + cos2 (la- )Izz,ra + marm ((arl1 + arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la ) + Yarm cos(ra ))2 + (110.7 Xla )2 )
2 Irl2,t =Izz,rl2 + Izz,rl4 + mrl4 (arl2 + arl3 )2 + rl5 Ixx,t + Iyy,t + mt ((arl2 + arl3 + arl4 + Xt )2 ) 2 2 2 + Iyy,ll1 + mll1 ((arl2 + arl3 )2 + a2 hip ) + Izz,ll3 + mll3 arl2 + ahip + Izz,ll4 + mll4 ahip
(hip cos(lls2 ) + ll5 )2 Ixx,ll5 + Iyy,ll5 + mll5 (all4 cos(ll3 ))2 + a2 hip + Ixx,ra + 2(rl5 ra1 )Izx,ra (rl5 + ra2 )Ixy,ra + (rl5 + ra2 )2 Iyy,ra + marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra ) Yra cos(ra ))2 + (49.1 + Xra )2 ) + Ixx,la + 2(rl5 la1 )Izx,la (rl5 + la2 )Ixy,la + (rl5 + la2 )2 Iyy,la + marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la- ) + Yra cos(la- ))2 + (110.7 Xla )2 )
Note that rl2 is not contained in ll3 , ra+ , and la- as {RL2} is the base frame for Irl2,t . The reason why the index has not been changed is to avoid confusion with new indexes.

These expressions give the following rotational kinetic energies.
237
1 2 krl1,r = Irl1,t 2 rl1 1 2 krl2,r = Irl2,t 2 rl2

Now are the kinetic energies determined, and in order to calculate the Lagrangian are the potential energies found next.
Potential Energy
The potential energies of the approximated mass centres are determined using Eq.(G.6).
pi = mi gxi
First the supporting leg.
(G.6)
prl0 =mrl0 g arl0

xrl0
prl1 =mrl1 g (xrl0 + arl1 )

xrl1

xrl2

xrl4
Before moving to the free limbs and the body, are the heights of joint RL5, LL5, RA1, RA2, LA1, and LA2 found.
xrl5 =(xrl4 + arl4 ) cos(rl1 ) xrl5,2 =(arl1 + (arl2 + arl3 ) cos(rl2 ) + arl4 ) xll5 =xrl5 xra1 =xla1 = xrl5 + 50.5 xra2 =xra1 + 18.7 cos(ra+ ) xla2 =xla1 + 18.7 cos(la- )
Note that the expression for xrl5 is used when considering the biped robot in the frontal plane and xrl5,2 is used when considering the biped robot in the sagittal plane. Then is the potential energy in the swing leg given by
pll1 =mll1 g (xll5 all1 )

xll4
pll3 =mll3 g (xll4 all2 cos(ll1- ))

xll2

xll1
238
The potential energy in the mass center of the body is given by
pCoM,t =mt g (xrl5 + XCoM,t )

The potential energies in the arms are given by
pra =marm g (xra2 YCoM,ra cos(ra+ )) pla =marm g (xla2 + YCoM,la cos(la- ))
Now are the dierent energies on the biped determined, and hence can the Lagrangian be determined.
G.3. MOMENTS OF INERTIA
239
G.3 Moments of Inertia

The inertia tensors used in this section are listed in App. I, and these are given in the mass center of each link aligned with the coordinates system of the link. For the relevant links of the Lagrangian model are the mass centers approximated as the joints, and hence will these inertia tensors introduce an error, but this is evaluate acceptable. In the calculations are symbols used in stead of the numerical values of the inertia to ease changes in the physical appearance of the biped. To make the expressions more foreseeable are the following notation used:
rl =rl2 + rl3 + rl4 ll1+ ll2+ ll3+ ll1ll2ll3=rl2 + rl3 + rl4 + ll2 =rl2 + rl3 + rl4 + ll2 + ll3 =rl2 + rl3 + rl4 + ll2 + ll3 + ll4 =rl2 + rl3 + rl4 ll2 =rl2 + rl3 + rl4 ll2 ll3 =rl2 + rl3 + rl4 ll2 ll3 ll4
Derivation of Moments of Inertia

Using the Parallel-Axis Theorem gives the following expressions for the inertia for the angel and shin in the swing leg [Serway and Jewett, 2004].
I ll5,t = I ll5
5 ll5 I ll4,t = I ll4 + LL LL4R Ill5 ll4R + mll5 all4 I 3 4 ll4 I ll3,t = I ll3 + ll + mll4 all3 I 3 ll3R Ill4 ll3R 4 ll5 ll5 ll4 + (ll ll3R ll4R )Ill5 (ll4R ll3R ) + mll5 (all4 + all3 )I 3
To determine a complete Lagrangian model are the moments of inertia needed for each part of the biped. Initially are only the moments of inertia of the whole biped seen from joint RL1 and RL2 needed, since these expressions are the only one depending on rl1 , rl1 or/and rl2 , rl2 . Hence when calculating the torque of joint RL1 and joint RL2 using the Lagrangian, will the rest of the expressions of the moments of inertia appear as constants, and since they are added to the linear part of the dierent kinetic energies, they will contribute with a zero. For this reason are only the moments of inertia for RL1 and RL2 found. To ease notation a vector is used instead of the scalar di to give the displacement of the mass centers. First the expression for the inertia in joint RL1.
240
1 rl1 I rl1,t = I rl1 + rl + mrl2 (P rl3,org P rl3,org I 3 P rl3,org P rl3,org ) rl2R I rl2 rl2R 1 rl1 T + rl + mrl4 (P rl4,org P rl4,org I 3 P rl4,org P rl4,org ) rl4R I rl4 rl4R 1 rl1 + rl + mt (P t P t I 3 P t P t ) rl5R I t rl5R 1 rl1 + rl + mll1 (P ll2,org P ll2,org I 3 P ll2,org P ll2,org ) ll1R I ll1 ll1R 1 rl1 + rl + mll3 (P ll3,org P ll3,org I 3 P ll3,org P ll3,org ) ll3R I ll3 ll3R 1 rl1 + rl + mll4 (P ll4,org P ll4,org I 3 P ll4,org P ll4,org ) ll4R I ll4 ll4R 1 rl1 + rl + mll5 (P ll5,org P ll5,org I 3 P ll5,org P ll5,org ) ll5R I ll5 ll5R 1 rl1 + rl + marm (P ra P ra I 3 P ra P ra ) ra2R I arm ra2R 1 rl1 + rl + marm (P la P la I 3 P la P la ) la2R I arm la2R
Note that these points to the dierent joints are given from frame {RL1}, and these are simplied when they enter the calculations in the following. By inspecting the equation of the rotational part of the kinetic energy,
ki,r =
1 T I i i 2 i
(G.7)
it is seen that only the Izz part of the inertia tensor I i contribute to the kinetic energy since
0 i = 0 z,i
This is due to the fact that the servomotors only rotate around one axis, which always is the z -axis. Hence does I rl1,t become a scalar Irl1,t . Entering the expression for Irl1,t in Matlab gives the following expression and since it is quite large is it divided it peaces, which should be summed to get the total expression.
First the supporting leg as a whole:
Irl1,t =Izz,rl1
+ (sin(rl2 )Ixx,rl2 + cos(rl2 )Iyx,rl2 ) sin(rl2 ) + (sin(rl2 )Ixy,rl2 + cos(rl2 )Iyy,rl2 ) cos(rl2 ) + mrl2 (arl1 + arl2 )2
+ (sin(rl )Ixx,rl4 + cos(rl )Iyx,rl4 ) sin(rl ) + (sin(rl )Ixy,rl4 + cos(rl )Iyy,rl4 ) cos(rl ) + mrl4 (arl1 + arl2 + arl3 )2
For the torso:
( sin(rl ) cos(rl5 )Ixx,t + sin(rl ) sin(rl5 )Iyx,t + cos(rl )Izx,t ) sin(rl ) cos(rl5 ) + ( sin(rl ) cos(rl5 )Ixy,t + sin(rl ) sin(rl5 )Iyy,t + cos(rl )Izy,t ) sin(rl ) sin(rl5 )
2 + ( sin(rl ) cos(rl5 )Ixz,t + sin(rl ) sin(rl5 )Iyz,t + cos(rl )Izz,t ) cos(rl ) + mt (arl1 + arl2 + arl3 + arl4 + Xt )2 + Yt
For link LL1 in the swing leg:
+ (sin(rl ) cos(hip )Ixx,ll1 sin(rl ) sin(hip )Iyx,ll1 + cos(rl )Izx,ll1 ) sin(rl ) cos(hip ) (sin(rl ) cos(hip )Ixy,ll1 sin(rl ) sin(hip )Iyy,ll1 + cos(rl )Izy,ll1 ) sin(rl ) sin(hip ) + (sin(rl ) cos(hip )Ixz,ll1 sin(rl ) sin(hip )Iyz,ll1 + cos(rl )Izz,ll1 ) cos(rl )
+ mll1 (arl1 + arl2 + arl3 + arl4 all1 )2 + (ahip + all1 (rl5 ll1 ))2
+ ((sin(rl ) cos(hip ) cos(lls1 ) cos(rl ) sin(lls1 ))Ixx,ll3 (sin(rl ) cos(hip ) sin(lls1 ) + cos(rl ) cos(lls1 ))Iyx,ll3 sin(rl ) sin(hip )Izx,ll3 ) (sin(rl ) cos(hip ) cos(lls1 ) cos(rl ) sin(lls1 )) + ((sin(rl ) cos(hip ) cos(lls1 ) cos(rl ) sin(lls1 ))Ixy,ll3 (sin(rl ) cos(hip ) sin(lls1 ) + cos(rl ) cos(lls1 ))Iyy,ll3
sin(rl ) sin(hip )Izy,ll3 ) ( sin(rl ) cos(hip ) sin(lls1 ) cos(rl ) cos(lls1 )) ((sin(rl ) cos(hip ) cos(lls1 ) cos(rl ) sin(lls1 ))Ixz,ll3 (sin(rl ) cos(hip ) sin(lls1 ) + cos(rl ) cos(lls1 ))Iyz,ll3
sin(rl ) sin(hip )Izz,ll3 ) sin(rl ) cos(hip )
+ mll3 (arl1 + arl2 + arl3 + arl4 all1 all2 cos(ll1 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (rl5 ll1 ))2
241
242
+ ((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 ))Ixx,ll4 (sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))Iyx,ll4 sin(rl ) sin(hip )Izx,ll4 ) (sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) + ((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 ))Ixy,ll4 (sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))Iyy,ll4 sin(rl ) sin(hip )Izy,ll4 ) ( sin(rl ) cos(hip ) sin(lls2 ) cos(rl ) cos(lls2 )) ((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 ))Ixz,ll4 (sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))Iyz,ll4 sin(rl ) sin(hip )Izz,ll4 ) sin(rl ) cos(hip )
+ mll4 (arl1 + arl2 + arl3 + arl4 all1 all2 cos(ll1 ) all3 cos(ll2 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (rl5 ll1 ) + all3 (ll1 ))2
+ (((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) cos(ll5 ) sin(rl ) sin(hip ) sin(ll5 ))Ixx,ll5 +((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) sin(ll5 ) sin(rl ) sin(hip ) cos(ll5 ))Iyx,ll5 +(sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))Izx,ll5 )
((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) cos(ll5 ) sin(rl ) sin(hip ) sin(ll5 )) + (((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) cos(ll5 ) sin(rl ) sin(hip ) sin(ll5 ))Ixy,ll5
+((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) sin(ll5 ) sin(rl ) sin(hip ) cos(ll5 ))Iyy,ll5 +(sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))Izy,ll5 ) ((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) sin(ll5 ) sin(rl ) sin(hip ) cos(ll5 ))
+ (((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) cos(ll5 ) sin(rl ) sin(hip ) sin(ll5 ))Ixz,ll5 +((sin(rl ) cos(hip ) cos(lls2 ) cos(rl ) sin(lls2 )) sin(ll5 ) sin(rl ) sin(hip ) cos(ll5 ))Iyz,ll5
+(sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))Izz,ll5 ) (sin(rl ) cos(hip ) sin(lls2 ) + cos(rl ) cos(lls2 ))
+ mll5 (arl1 + arl2 + arl3 + arl4 all1 all2 cos(ll1 ) all3 cos(ll2 ) all4 cos(ll4 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (rl5 ll1 ) + all3 (ll1 ) + all4 (ll1 ))2
For the right arm:
+ (((sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) sin(ra2 ) + sin(rl ) sin(rl5 ) cos(ra2 ))Ixx,ra
+(( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) cos(ra2 ) sin(rl ) sin(rl5 ) sin(ra2 ))Iyx,ra +( sin(rl ) cos(rl5 ) sin(ra1 ) + cos(rl ) cos(ra1 ))Izx,ra (( sin(rl ) cos(rl5 ) cos(ra1 )
cos(rl ) sin(ra1 )) sin(ra2 ) + sin(rl ) sin(rl5 ) cos(ra2 ))) + ((( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) sin(ra2 ) + sin(rl ) sin(rl5 ) cos(ra2 ))Ixy,ra
+(( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) cos(ra2 ) sin(rl ) sin(rl5 ) sin(ra2 ))Iyy,ra +( sin(rl ) cos(rl5 ) sin(ra1 ) + cos(rl ) cos(ra1 ))Izy,ra (( sin(rl ) cos(rl5 ) cos(ra1 )
cos(rl ) sin(ra1 )) cos(ra2 ) sin(rl ) sin(rl5 ) sin(ra2 ))) + ((( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) sin(ra2 ) + sin(rl ) sin(rl5 ) cos(ra2 ))Ixz,ra
+(( sin(rl ) cos(rl5 ) cos(ra1 ) cos(rl ) sin(ra1 )) cos(ra2 ) sin(rl ) sin(rl5 ) sin(ra2 ))Iyz,ra +( sin(rl ) cos(rl5 ) sin(ra1 ) + cos(rl ) cos(ra1 ))Izz,ra ( sin(rl ) cos(rl5 ) sin(ra1 ) + cos(rl ) cos(ra1 )))
+ marm ((arl1 + arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra ) Yarm cos(ra ))2 + (49.1 + Xra )2 )
243
244
For the left arm:
+ (((sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) sin(la2 ) sin(rl ) sin(rl5 ) cos(la2 ))Ixx,la +(( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) cos(la2 ) + sin(rl ) sin(rl5 ) sin(la2 ))Iyx,la +(sin(rl ) cos(rl5 ) sin(la1 ) + cos(rl ) cos(la1 ))Izx,la (( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) sin(la2 ) sin(rl ) sin(rl5 ) cos(la2 ))) + ((( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) sin(la2 ) sin(rl ) sin(rl5 ) cos(la2 ))Ixy,la +(( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) cos(la2 ) + sin(rl ) sin(rl5 ) sin(la2 ))Iyy,la +(sin(rl ) cos(rl5 ) sin(la1 ) + cos(rl ) cos(la1 ))Izy,la (( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) cos(la2 ) + sin(rl ) sin(rl5 ) sin(la2 ))) + ((( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) sin(la2 ) sin(rl ) sin(rl5 ) cos(la2 ))Ixz,la +(( sin(rl ) cos(rl5 ) cos(la1 ) + cos(rl ) sin(la1 )) cos(la2 ) + sin(rl ) sin(rl5 ) sin(la2 ))Iyz,la +(sin(rl ) cos(rl5 ) sin(la1 ) + cos(rl ) cos(la1 ))Izz,la (sin(rl ) cos(rl5 ) sin(la1 ) + cos(rl ) cos(la1 )))
+ marm ((arl1 + arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la ) + Yarm cos(ra ))2 + (110.7 Xla )2 )
To get a useful expression for the inertia in joint RL1 are the small angle approximations, justied by examine the human gait, used. Further are expressions which contribute with an inertia less than 106 kg m removed.
This result in the following simplied expressions. First the supporting leg as a whole:
Irl1,t = Izz,rl1 + Iyy,rl2 + mrl2 (arl1 + arl2 )2 + Iyy,rl4 + mrl4 (arl1 + arl2 + arl3 )2
For the torso:
+ Ixx,t 2 rl + Izz,t
2 + mt (arl1 + arl2 + arl3 + arl4 + Xt )2 + Yt
+ Izz,ll1 + mll1 (arl1 + arl2 + arl3 + arl4 all1 )2 + ((ahip + all1 (rl5 ll1 ))2
For the following is the trigonomic relations also applied.
sin A cos B cos A sin B = sin(A B ) cos A cos B + sin A sin B = cos(A B )
+ sin2 (rl lls1 )Ixx,ll3 + cos2 (rl lls1 )Iyy,ll3
+ mll3 (arl1 + arl2 + arl3 + arl4 all1 all2 cos(ll1 ))2
+((arl1 + arl2 + arl3 + arl4 )rl1 ahip + all1 (rl5 ll1 ) + all2 (rl5 ll1 ))2
+ sin2 (rl lls2 )Ixx,ll4 + cos2 (rl lls2 )Iyy,ll4
+ mll4 (arl1 + arl2 + arl3 + arl4 all1 all2 cos(ll1 ) all3 cos(ll2 ))2
+((arl1 + arl2 + arl3 + arl4 )rl1 ahip + all1 (rl5 ll1 ) + all2 (rl5 ll1 ) + all3 (ll1 ))2
+ sin2 (rl lls2 )Ixx,ll5 + cos2 (rl lls2 )Izz,ll5
+ mll5 (arl1 + arl2 + arl3 + arl4 all1 all2 cos(ll1 ) all3 cos(ll2 ) all4 cos(ll4 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (rl5 ll1 ) + all3 (ll1 ) + all4 (ll1 ))2
The analysis of the human gait in App. A has shown that the small angle approximation also apply on the angles in the arms. For the right arm does it result in:
+ sin2 (rl + ra1 )Iyy,ra + cos2 (rl + ra1 )Izz,ra
+ marm ((arl1 + arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra ) Yarm cos(ra ))2 + (49.1 + Xra )2 )
245
246
For the left arm:
+ sin2 (la1 rl )Iyy,la + cos2 (la1 rl )Izz,ra
+ marm ((arl1 + arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la ) + Yarm cos(ra ))2 + (110.7 Xla )2 )
The x-components in the swing leg have also been shortened by assuming that joint e.g. RL2 has the same height as LL4. This does of course introduce an error, but it is justied by the fact that the displacement of the joints in the swing leg from joint RL1 mainly is in the y -direction. Further have some insignicant parts of the displacement of the joints been cut out. Finally are some rearrangement made on the expressions.
Irl1,t = Izz,rl1 + Iyy,rl2 + mrl2 (arl1 + arl2 )2 + Iyy,rl4 + mrl4 (arl1 + arl2 + arl3 )2
2 2 2 + 2 rl Ixx,t + Izz,t + mt (arl1 + arl2 + arl3 + arl4 + Xt ) + Izz,ll1 + mll1 (arl1 + arl2 + arl3 ) + ahip
2 2 2 2 + Ixx,ll3 + mll3 (arl1 + arl2 )2 + a2 hip + sin (ll2- )Ixx,ll4 + cos (ll2- )Iyy,ll4 + mll4 arl1 + ahip
2 2 + sin2 (ll2- )Ixx,ll5 + cos2 (ll2- )Izz,ll5 + mll5 a2 hip + sin (ra+ )Iyy,ra + cos (ra2+ )Izz,ra
+ marm (((arl1 + arl2 + arl3 + arl4 ) cos(rl1 ) + 50.5 + 18.7 cos(ra ) Yarm cos(ra ))2 + (49.1 + Xra )2 ) + sin2 (la- )Iyy,la
+ cos2 (la- )Izz,ra + marm ((arl1 + arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la ) + Yarm cos(ra ))2 + (110.7 Xla )2 )
Following the same procedure is an expression for the inertia of the biped in joint RL2 found. The inertia is given by:
2 rl2 + rl + mt (P t P t I 3 P t P t ) rl5R I t rl5R 2 rl2 + rl + mll1 (P ll2,org P ll2,org I 3 P ll2,org P ll2,org ) ll1R I ll1 ll1R 2 rl2 + rl + mll3 (P ll3,org P ll3,org I 3 P ll3,org P ll3,org ) ll3R I ll3 ll3R 2 rl2 + rl + mll4 (P ll4,org P ll4,org I 3 P ll4,org P ll4,org ) ll4R I ll4 ll4R 2 rl2 + rl + mll5 (P ll5,org P ll5,org I 3 P ll5,org P ll5,org ) ll5R I ll5 ll5R 2 rl2 + rl + marm (P ra P ra I 3 P ra P ra ) ra2R I arm ra2R 1 rl1 + rl + marm (P la P la I 3 P la P la ) la2R I arm la2R
2 rl2 I rl2,t = I rl2 + rl + mrl4 (P rl4,org P rl4,org I 3 P rl4,org P rl4,org ) rl4R I rl4 rl4R
the points used here are a bit dierent from those used to nd the inertia in joint RL1, as {RL2} now is the base frame, and hence are displacements of the parts of the biped calculated from joint RL2 and not from {RL1}. Using Matlab have the following expressions been achieved.
For the supporting leg:
Irl2,t = Izz,rl2 + Izz,rl4 + mrl4 (arl2 + arl3 )2
For the torso
2 + (sin(rl5 )Ixx,t + cos(rl5 )Ixy,t ) sin(rl5 ) + (sin(rl5 )Ixy,t + cos(rl5 )Iyy,t ) cos(rl5 ) + mt ((arl2 + arl3 + arl4 + Xt )2 + Yt )
For the swing leg. First LL1
( sin(hip )Ixx,ll1 cos(hip )Ixy,ll1 ) sin(hip ) ( sin(hip )Ixy,ll1 cos(hip )Iyy,ll1 ) cos(hip )
+ mll1 ((arl2 + arl3 + arl4 all1 )2 + (ahip + all1 (rl5 ll1 ))2 )
LL3
( sin(hip ) cos(lls1 )Ixx,ll3 + sin(hip ) sin(lls1 )Ixy,ll3 cos(hip )Izx,ll3 ) sin(hip ) cos(lls1 )
+ ( sin(hip ) cos(lls1 )Ixy,ll3 + sin(hip ) sin(lls1 )Iyy,ll3 cos(hip )Izy,ll3 ) sin(hip ) sin(lls1 ) ( sin(hip ) cos(lls1 )Ixz,ll3 + sin(hip ) sin(lls1 )Iyz,ll3 cos(hip )Izz,ll3 ) cos(hip )
+ mll3 (arl2 + arl3 + arl4 all1 all2 cos(ll1 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (ll1 rl5 ))2
LL4
( sin(hip ) cos(lls2 )Ixx,ll4 + sin(hip ) sin(lls2 )Ixy,ll4 cos(hip )Izx,ll4 ) sin(hip ) cos(lls2 ) + ( sin(hip ) cos(lls2 )Ixy,ll4 + sin(hip ) sin(lls2 )Iyy,ll4 cos(hip )Izy,ll4 ) sin(hip ) sin(lls2 )
( sin(hip ) cos(lls2 )Ixz,ll4 + sin(hip ) sin(lls2 )Iyz,ll4 cos(hip )Izz,ll4 ) cos(hip )
+ mll4 (arl2 + arl3 + arl4 all1 all2 cos(ll1 ) all3 cos(ll2 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (ll1 rl5 ) + all3 (ll1 ))2
247
248
LL5
+ (( sin(hip ) cos(lls2 ) cos(ll5 ) cos(hip ) sin(ll5 ))Ixx,ll5 + (sin(hip ) cos(lls2 ) sin(ll5 ) cos(hip ) cos(ll5 ))Ixy,ll5 sin(hip ) sin(lls2 )Izx,ll5 )( sin(hip ) cos(lls2 ) cos(ll5 ) cos(hip ) sin(ll5 )) + (( sin(hip ) cos(lls2 ) cos(ll5 ) cos(hip ) sin(ll5 ))Ixy,ll5 + (sin(hip ) cos(lls2 ) sin(ll5 ) cos(hip ) cos(ll5 ))Iyy,ll5 sin(hip ) sin(lls2 )Izy,ll5 )(sin(hip ) cos(lls2 ) sin(ll5 ) cos(hip ) cos(ll5 )) (( sin(hip ) cos(lls2 ) cos(ll5 ) cos(hip ) sin(ll5 ))Ixz,ll5 + (sin(hip ) cos(lls2 ) sin(ll5 ) cos(hip ) cos(ll5 ))Iyz,ll5 sin(hip ) sin(lls2 )Izz,ll5 ) sin(hip ) sin(lls2 )
+ mll5 (arl2 + arl3 + arl4 all1 all2 cos(ll1 ) all3 cos(ll2 ) all4 cos(ll3 ))2
+(ahip + all1 (rl5 ll1 ) + all2 (ll1 rl5 ) + all3 (ll1 ) + all4 (ll1 ))2
Right arm
+ (( sin(rl5 ) cos(ra1 ) sin(ra2 ) + cos(rl5 ) cos(ra2 ))Ixx,ra + ( sin(rl5 ) cos(ra1 ) sin(ra2 ) + cos(rl5 ) sin(ra2 ))Iyx,ra + sin(rl5 ) sin(ra1 )Izx,ra ) ( sin(rl5 ) cos(ra1 ) sin(ra2 ) + cos(rl5 ) cos(ra2 ))
+ (( sin(rl5 ) cos(ra1 ) sin(ra2 ) + cos(rl5 ) cos(ra2 ))Ixy,ra + ( sin(rl5 ) cos(ra1 ) cos(ra2 ) cos(rl5 ) sin(ra2 ))Iyy,ra + sin(rl5 ) sin(ra1 )Izy,ra ) ( sin(rl5 ) cos(ra1 ) cos(ra2 ) cos(rl5 ) sin(ra2 ))
+ (( sin(rl5 ) cos(ra1 ) sin(ra2 ) + cos(rl5 ) cos(ra2 ))Ixz,ra + ( sin(rl5 ) cos(ra1 ) cos(ra2 ) cos(rl5 ) sin(ra2 ))Iyz,ra + sin(rl5 ) sin(ra1 )Izz,ra ) sin(rl5 ) sin(ra1 )
+ marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra ) Yra cos(ra ))2 + (49.1 + Xra )2 )
Left arm
+ ((sin(rl5 ) cos(la1 ) sin(la2 ) cos(rl5 ) cos(la2 ))Ixx,la + (sin(rl5 ) cos(la1 ) cos(la2 ) + cos(rl5 ) sin(la2 ))Iyx,la sin(rl5 ) sin(la1 )Izx,la ) (sin(rl5 ) cos(la1 ) sin(la2 ) cos(rl5 ) cos(la2 )) + ((sin(rl5 ) cos(la1 ) sin(la2 ) cos(rl5 ) cos(la2 ))Ixy,la + (sin(rl5 ) cos(la1 ) cos(la2 ) + cos(rl5 ) sin(la2 ))Iyy,la sin(rl5 ) sin(la1 )Izy,larm ) (sin(rl5 ) cos(la1 ) cos(la2 ) + cos(rl5 ) sin(la2 )) ((sin(rl5 ) cos(la1 ) sin(la2 ) cos(rl5 ) cos(la2 ))Ixz,la + (sin(rl5 ) cos(la1 ) cos(la2 ) + cos(rl5 ) sin(la2 ))Iyz,la sin(rl5 ) sin(la1 )Izz,la ) sin(rl5 ) sin(la1 )
+ marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la- ) + Yla cos(la- ))2 + (110.7 Xla )2 )
Using the same procedure as for the previous inertia are the following expressions found.
For the supporting leg:
Irl2,t = Izz,rl2 + Izz,rl4 + mrl4 (arl2 + arl3 )2
For the torso

2 + rl5 Ixx,t + Iyy,t + mt ((arl2 + arl3 + arl4 + Xt )2 )
For the swing leg. First LL1
+ Iyy,ll1 + mll1 ((arl2 + arl3 )2 + a2 hip )
LL3
2 +Izz,ll3 + mll3 a2 rl2 + ahip
LL4
+Izz,ll4 + mll4 a2 hip
249
250
LL5
Right arm
+ Ixx,ra + 2(rl5 ra1 )Izx,ra (rl5 + ra2 )Ixy,ra + (rl5 + ra2 )2 Iyy,ra
+ marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra ) Yra cos(ra ))2 + (49.1 + Xra )2 )
Left arm
+ Ixx,la + 2(rl5 la1 )Izx,la (rl5 + la2 )Ixy,la + (rl5 + la2 )2 Iyy,la
+ marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la- ) + Yla cos(la- ))2 + (110.7 Xla )2 )
and the whole expression.

2 2 2 + Iyy,ll1 + mll1 ((arl2 + arl3 )2 + a2 hip ) + Izz,ll3 + mll3 arl2 + ahip + Izz,ll4 + mll4 ahip
2 Irl2,t = Izz,rl2 + Izz,rl4 + mrl4 (arl2 + arl3 )2 + rl5 Ixx,t + Iyy,t + mt ((arl2 + arl3 + arl4 + Xt )2 )
(hip cos(lls2 ) + ll5 )2 Ixx,ll5 + Iyy,ll5 + mll5 (all4 cos(ll3 ))2 + a2 hip + Ixx,ra + 2(rl5 ra1 )Izx,ra (rl5 + ra2 )Ixy,ra + (rl5 + ra2 )2 Iyy,ra + marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(ra ) Yra cos(ra ))2 + (49.1 + Xra )2 ) + Ixx,la + 2(rl5 la1 )Izx,la (rl5 + la2 )Ixy,la + (rl5 + la2 )2 Iyy,la + marm ((arl2 + arl3 + arl4 + 50.5 + 18.7 cos(la- ) + Yla cos(la- ))2 + (110.7 Xla )2 )
Note that for Irl2,t is rl2 not at part of the sums such as ra+ and ll1 . The subscripts have not been changed as this would introduce more variables which would make i harder to follow the calculations.
The moments of inertia seen from the rest of the links can be found in the same way, but for this project are these two sucient.
G.4. COMPUTATION OF THE LAGRANGIAN, RL1 AND RL2
251
G.4 Computation of the Lagrangian, RL1 and RL2

Using the determined energies is an expression for the Lagrangian calculated, and differentiated in order to obtain the joint torque. The torque is only calculated for the two lowest joints in the supporting leg, here the right leg, as only these are needed to determine the CoP. The rest of the torques could be calculated to get a complete picture of the torques working on the whole biped robot, which might form a basis for a more complex controller. This is not within the scope of this project, and will not be calculated. First the Lagrangian is given by summarize over the energies as seen in Eq. (G.8), but in the following calculations will only the energy parts from the Lagrangian which does not give zero when dierentiated be taken into the calculations.
L=
ki
pi
(G.8)
The torque for each link is determined using the following rule:
i =
d L L dt i i
(G.9)
The reason why the generalized forces are torques is that the generalized coordinates are angles. The determined torque is the torque the biped does apply to the servomotors along the z axis. The regulation and control of the biped is done by adjusting the angles to obtain a certain output from the strain gauges on the supporting foot which is mapped to a certain torque in the origin of the base frame. Hence the only two torques of interest is, the torque in joint RL1 to determine how the foot is loaded in the frontal plane, and the torque in joint RL2 to determine how the foot is loaded in the sagittal plane. These two torques are the most complicated to determine since they have to take the whole biped into account, and therefore are the calculations divided into two steps, as seen in Eq. (G.10).
rl1 =
d L L 1 dt 1
step 1 step 2
(G.10)
rl2 =
d L L 2 dt 2
Torque in RL1
First step one for rl1 . To make the calculations more foreseeable are the dierentiations divided into a part with the linear kinetic- and potential energy, and a part with the rotational kinetic energy. Beginning with the linear part of step one. For shortness does arl replace arl1 + arl2 + arl3 + arl4 .
252
d Ll d = k + krl2,l + krl4,l + kll1,l + kll3,l + kll4,l + kll5,l + kt,l + kra,l + kla,l rl1 dt rl1 rl1,l dt d 1 1 2 rl1 )2 + 1 mrl4 ((arl1 + arl2 + arl3 ) rl1 )2 = mrl1 a2 mrl2 ((arl1 + arl2 ) rl1 rl1 + dt rl1 2 2 2 1 rl1 )2 + ((arl ) rl1 )2 + mt (arl sin(rl1 ) 2 1 rl1 )2 + ((arl1 + arl2 + arl3 ) rl1 all1 ( rl5 + ll1 ))2 + mll1 (arl sin(rl1 ) 2 1 rl1 + all2 sin(ll1- ) ll1- )2 + mll3 (arl sin(rl1 ) 2 rl1 (all1 + all2 )( rl5 + ll1 ))2 +((arl1 + arl2 + arl3 )
1 rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )2 + mll4 (arl sin(rl1 ) 2 rl1 (all1 + all2 )( rl5 + ll1 ) all3 ll1 )2 +((arl1 + arl2 + arl3 )
1 rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3- )2 + mll5 (arl sin(rl1 ) 2 rl1 (all1 all2 )( rl5 + ll1 ) (all3 + all4 ) ll1 )2 +((arl1 + arl2 + arl3 )
1 rl1 + (YCoM,ra 18.7) sin(ra+ )ra+ )2 + (arl rl1 )2 + marm (arl sin(rl1 ) 2 1 rl1 (YCoM,la + 18.7) sin(la+ )la+ )2 + (arl rl1 )2 + marm (arl sin(rl1 ) 2
d 2 2 2 2 2 mrl1 a2 rl1 rl1 + mrl2 (arl1 + arl2 ) rl1 + mrl4 (arl1 + arl2 + arl3 ) rl1 + mt (arl sin(rl1 ) + arl )rl1 dt rl1 + ((arl1 + arl2 + arl3 ) rl1 all1 ( rl5 + ll1 ))(arl1 + arl2 + arl3 ) + mll1 a2 sin(rl1 )2
rl
rl1 + all2 sin(ll1- ) ll1- )(arl sin(rl1 )) + mll3 (arl sin(rl1 )
rl1 (all1 + all2 )( rl5 + ll1 ))(arl1 + arl2 + arl3 ) +((arl1 + arl2 + arl3 )
rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )(arl sin(rl1 )) + mll4 (arl sin(rl1 )
rl1 (all1 + all2 )( rl5 + ll1 ) all3 ll1 )(arl1 + arl2 + arl3 ) +((arl1 + arl2 + arl3 )
rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3- )(arl sin(rl1 )) + mll5 (arl sin(rl1 )
rl1 (all1 all2 )( rl5 + ll1 ) (all3 + all4 ) ll1 )(arl1 + arl2 + arl3 ) +((arl1 + arl2 + arl3 )
rl1 + (YCoM,ra 18.7) sin(ra+ ) ra+ )(arl sin(rl1 )) + a2 + marm (arl sin(rl1 ) rl rl1
(G.11)
rl1 (YCoM,la + 18.7) sin(la- ) la- )(arl sin(rl1 )) + a2 + marm (arl sin(rl1 ) rl rl1
253
254
and now for the rotational part of step one.
d Lr d = k rl1 dt rl1 rl1,r dt d 1 2 = I + Iyy,rl2 + mrl2 (arl1 + arl2 )2 + Iyy,rl4 + mrl4 (arl1 + arl2 + arl3 )2 rl1 2 rl1 zz,rl1 dt
2 2 2 +2 rl Ixx,t + Izz,t + mt (arl + Xt ) + Izz,ll1 + mll1 (arl1 + arl2 + arl3 ) + ahip
2 2 2 2 +Ixx,ll3 + mll3 (arl1 + arl2 )2 + a2 hip + sin (ll3- )Ixx,ll4 + cos (ll3- )Iyy,ll4 + mll4 arl1 + ahip
2 2 + sin2 (ll3- )Ixx,ll5 + cos2 (ll3- )Izz,ll5 + mll5 a2 hip + sin (ra+ )Iyy,rarm + cos (ra+ )Izz,ra
+marm ((arl + 50.5 + 18.7 cos(ra+ ) YCoM,ra cos(ra+ ))2 + (49.1 + XCoM,ra )2 ) + sin2 (la- )Iyy,larm
+ cos2 (la- )Izz,ra + marm ((arl + 50.5 + 18.7 cos(la- ) + YCoM,la cos(la- ))2 + (110.7 XCoM,la )2 )
d d Lr 2 2 = rl1 Izz,rl1 + Iyy,rl2 + mrl2 (arl1 + arl2 ) + Iyy,rl4 + mrl4 (arl1 + arl2 + arl3 ) dt rl1 dt
2 2 2 +2 rl Ixx,t + Izz,t + mt (arl + Xt ) + Izz,ll1 + mll1 (arl1 + arl2 + arl3 ) + ahip
2 2 2 2 +Ixx,ll3 + mll3 (arl1 + arl2 )2 + a2 hip + sin (ll3- )Ixx,ll4 + cos (ll3- )Iyy,ll4 + mll4 arl1 + ahip
2 2 + sin2 (ll3- )Ixx,ll5 + cos2 (ll3- )Izz,ll5 + mll5 a2 hip + sin (ra+ )Iyy,ra + cos (ra+ )Izz,ra
+marm ((arl + 50.5 + 18.7 cos(ra+ ) YCoM,ra cos(ra+ ))2 + (49.1 + XCoM,ra )2 ) + sin2 (la- )Iyy,la
(G.12)
+ cos2 (la- )Izz,ra + marm ((arl + 50.5 + 18.7 cos(la- ) + YCoM,la cos(la- ))2 + (110.7 XCoM,la )2 )
255
These expressions are not dierentiated with regard to time because it is easy to do on the computer and it also keeps the expressions simpler and more foreseeable. Now for step two.
L = (kll1,l + kll3,l + kll4,l + kll5,l + kbody,l + kra,l + kla,l rl1 rl1 pCoM,t pll1 pll3 pll4 pll5 pCoM,ra pCoM,la ) 2 1 rl1 + 1 mll3 (arl sin(rl1 ) rl1 + all2 sin(ll1- ) ll1- )2 = mll1 arl sin(rl1 ) rl1 2 2 1 rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )2 + mll4 (arl sin(rl1 ) 2 1 rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3- )2 + mll5 (arl sin(rl1 ) 2 1 rl1 )2 + mt (arl sin(rl1 ) 2 1 rl1 + (YCoM,ra 18.7) sin(ra+ )ra+ )2 + marm (arl sin(rl1 ) 2 1 rl1 (YCoM,la + 18.7) sin(la+ )la+ )2 + marm (arl sin(rl1 ) 2 mt g (arl cos(rl1 ) + XCoM,t ) mll1 g (arl cos(rl1 ) all1 ) mll3 g (arl cos(rl1 ) all1 all2 cos(ll1- )) mll4 g (arl cos(rl1 ) all1 all2 cos(ll1- ) all3 cos(ll2- )) mll5 g (arl cos(rl1 ) all1 all2 cos(ll1- ) all3 cos(ll2- ) all3 cos(ll3- )) mra g (arl cos(rl1 ) + 50.5 + 18.7 cos(ra+ ) YCoM,arm ) mla g (arl cos(rl1 ) + 50.5 + 18.7 cos(la- ) + YCoM,arm )
2 =mll1 a2 rl sin(rl1 ) cos(rl1 )rl1 + mll3 (arl sin(rl1 )rl1 + all2 sin(ll1- )ll1- )(arl cos(rl1 )rl1 ) rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )(arl cos(rl1 ) rl1 ) + mll4 (arl sin(rl1 ) rl1 + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2+ mll5 arl sin(rl1 ) ll3- (arl cos(rl1 ) rl1 ) +all4 sin(ll3- ) 2 + mt (a2 rl sin(rl1 ) cos(rl1 )rl1 ) rl1 + (YCoM,ra 18.7) sin(ra+ )ra+ (arl cos(rl1 ) rl1 ) + marm arl sin(rl1 ) rl1 (YCoM,la + 18.7) sin(la+ )la+ (arl cos(rl1 ) rl1 ) + marm arl sin(rl1 ) + (mll1 + mll3 + mll4 + mll5 + mt + mra + mla )garl sin(rl1 )
(G.13)
Subtracting the Eq.(G.13) from Eq.(G.11) and Eq.(G.12) gives the torque in joint RL1, rl1 , but due to the large subexpressions it will not be written as a single expression.
Torque in RL2
Using the same procedure as for rl1 is rl2 determined, but note that now xrl5,2 is used i favor of xrl5 as RL2 rotate in the sagittal plane. First the linear part of step one. Since arl4 = all1 is the expression shortened a bit.
256
d d Ll = (k + krl4,l + kll1,l + kll3,l + kll4,l + kll5,l + kt,l + kra,l + kla,l ) rl2 dt rl2 rl2,l dt d 1 2 + 1 mrl4 (arl2 rl2 arl3 ( rl2 + rl3 ))2 = m a2 rl2 2 rl2 rl2 rl2 dt 2
2
rl2 (arl2 + arl3 ) + sin(rl2 )
1 mll1 2 1 + mll3 2
2
rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) arl2 rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- )2 + ( sin(rl2 )
ll1+all2 cos(ll1- )
+
2
1 mll4 2
rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) arl2 rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )2 + ( sin(rl2 )
ll1- + all3 cos(ll2- ) ll2+all2 cos(ll1- )
+
2
1 mll5 2
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3- )2 +( sin(rl2 )
1 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) XCoM,t ( rl ))2 + ( sin(rl2 ) rl2 (arl2 + arl3 ))2 mt (arl2 2 2 1 rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) sin(ra+ ) ra + arl2 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) sin(rl2 ) + marm 2
2
rl ) + (YCoM,ra 18.7)(cos(ra+ ) ra ) 50.5(
+
2
1 marm 2
rl2 (arl2 + arl3 ) + (YCoM,la 18.7) sin(la- ) la sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) + arl2
rl ) (YCoM,la + 18.7)(cos(la- ) la- ) 50.5(
d (mrl2 a2 rl2 )rl2 + mrl4 (arl2 rl2 arl3 (rl2 + rl3 ))(arl2 arl3 ) dt rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) (arl2 arl3 ahip rl5 ) + (sin(rl2 )2 rl2 (arl2 + arl3 )2 ) + mll1 arl2
+ mll3
rl2 arl3 ( rl2 + rl3 ) ahip ((rl ) rl5 + ( rl )rl5 ) + all2 cos(ll1- ) ll1arl2
(arl2 arl3 ahip rl5 + all2 cos(ll1- ))
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- )( sin(rl2 )(arl2 + arl3 ) + all2 sin(ll1- )) +( sin(rl2 )
+ mll4
ll1- + all3 cos(ll2- ) ll2- (arl2 arl3 ahip rl5 + all2 cos(ll1- ) + all3 cos(ll2- )) +all2 cos(ll1- )
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- )( sin(rl2 )(arl2 + arl3 ) + all2 sin(ll1- ) + all3 sin(ll2- )) +( sin(rl2 )
+ mll5
(arl2 arl3 ahip rl5 + all2 cos(ll1- ) + all3 cos(ll2- ) + all4 cos(ll3- )) rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + all3 sin(ll2- ) ll2- + all4 sin(ll3- ) ll3- ) +( sin(rl2 )
( sin(rl2 )(arl2 + arl3 ) + all2 sin(ll1- ) + all3 sin(ll2- ) + all4 sin(ll3- )))
+ mt
rl2 arl3 ( rl2 + rl3 ) (arl4 + XCoM,t )( rl ) (arl2 arl3 arl4 XCoM,t ) + sin(rl2 )2 rl2 (arl2 + arl3 )2 arl2
rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) sin(ra+ ) ra+ )( sin(rl2 )(arl2 + arl3 ) + (YCoM,ra 18.7) sin(ra+ )) + marm ( sin(rl2 )
ra+ (arl2 arl3 arl4 50.5 + (YCoM,ra 18.7) cos(ra+ )) +(YCoM,ra 18.7) cos(ra+ )
rl2 (arl2 + arl3 ) + (YCoM,la 18.7) sin(la- ) la- )( sin(rl2 )(arl2 + arl3 ) + (YCoM,la 18.7) sin(la- )) + marm ( sin(rl2 )
257

(G.14)
la- (arl2 arl3 arl4 50.5 (YCoM,la + 18.7) cos(la- )) (YCoM,la + 18.7) cos(la- )
258
and the rotational part of step one.
d Lr d = k rl2 dt rl2 rl2,r dt d 1 2 2 = I + Izz,rl4 + mrl4 (arl2 + arl3 )2 + rl5 Ixx,t + Iyy,t + mt ((arl2 + arl3 + arl4 + Xt )2 ) rl2 2 rl2 zz,rl2 dt
d Lr d 2 = I + Izz,rl4 + mrl4 (arl2 + arl3 )2 + rl5 Ixx,t + Iyy,t + mt ((arl2 + arl3 + arl4 + Xt )2 ) rl2 dt rl2 zz,rl2 dt
(G.15)
Step two for joint RL2.
L = (kt + kll1,l + kll3,l + kll4,l + kll5,l + kra,l + kla,l pt pll1 pll3 pll4 pll5 pra pla ) rl2 rl2 1 rl2 (arl2 + arl3 ))2 = mt ( sin(rl2 ) rl2 2 2 1 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) ahip ((rl ) rl5 + ( rl )rl5 ) all1 ( rl ) + ( sin(rl2 ) rl2 (arl2 + arl3 ))2 mll1 arl2 2 2 1 rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- + arl2 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) + mll3 sin(rl2 ) 2
2
rl5 + ( rl )rl5 ) all1 ( rl ) + all2 cos(ll1- ) ll1ahip ((rl )

2
1 + mll4 2
2
rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) + arl2
rl5 + ( rl )rl5 ) all1 ( rl ) + all2 cos(ll1- ) ll1- + all3 cos(ll2- ) ll2ahip ((rl )
1 + mll5 2
2

2
1 + marm 2
rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) sin(ra+ ) ra+ sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) + (YCoM,ra 18.7) cos(ra+ ) ra+ + arl2
2
1 + marm 2
rl2 (arl2 + arl3 ) (YCoM,la + 18.7) sin(la- ) la sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) (YCoM,la + 18.7) cos(la- ) la+ arl2
259
mt g (arl1 + (arl2 + arl3 ) cos(rl2 ) + arl4 ) mll1 g ((arl2 + arl3 ) cos(rl2 ) + arl4 )
mll3 g ((arl2 + arl3 ) cos(rl2 ) + arl4 all2 cos(ll1- )) mll4 g ((arl2 + arl3 ) cos(rl2 ) + arl4 all2 cos(ll1- ) all3 cos(ll2- )) mll5 g ((arl2 + arl3 ) cos(rl2 ) + arl4 all2 cos(ll1- ) all3 cos(ll2- ) all4 cos(ll3- ))
marm g ((arl1 + (arl2 + arl3 ) cos(rl2 ) + arl4 ) + (18.7 YCoM,ra ) cos(ra+ ))
260
Dierentiate this expression with regrad to rl2 gives L 2 (arl2 + arl3 )2 )+ =mt ( sin(rl2 ) cos(rl2 ) rl2 rl2 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) ahip ((rl ) rl5 + ( rl )rl5 ) all1 ( rl ) (ahip rl5 ) + ( sin(rl2 ) cos(rl2 ) 2 (arl2 + arl3 )2 ) mll1 arl2 rl2
+ mll3
rl2 (arl2 + arl3 ) + all2 sin(ll1- ) ll1- ( cos(rl2 ) rl2 (arl2 + arl3 ) + all2 cos(ll1- ) ll1- ) sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) ahip ((rl ) rl5 + ( rl )rl5 ) all1 ( rl ) + all2 cos(ll1- ) ll1- (ahip rl5 all2 sin(ll1- ) ll1- ) + arl2 rl2 (arl2 + arl3 ) cos(rl2 )
+ mll4
ll1- + all3 cos(ll2- ) ll2- + arl2 rl2 arl3 ( rl2 + rl3 ) arl4 ( rl ) ahip ((rl ) rl5 + ( rl )rl5 ) +all2 cos(ll1- )
rl ) + all2 cos(ll1- ) ll1- + all3 cos(ll2- ) ll2- (ahip rl5 all2 sin(ll1- ) ll1- all3 sin(ll2- ) ll2- ) all1 (
+ mll5
rl2 (arl2 + arl3 ) + all2 cos(ll1- ) ll1- + all3 cos(ll2- ) ll2- + all4 cos(ll3- ) ll3- ) ( cos(rl2 )
rl5 all2 sin(ll1- ) ll1- all3 sin(ll2- ) ll2- all4 sin(ll3- ) ll3- ) (ahip
rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) ra+ sin(ra+ ))( cos(rl2 ) rl2 (arl2 + arl3 ) + (YCoM,ra 18.7) ra+ cos(ra+ )) + marm ( sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) + (YCoM,ra 18.7) cos(ra+ ) ra+ ((YCoM,ra 18.7) sin(ra+ ) ra+ ) + arl2
rl2 (arl2 + arl3 ) (YCoM,la + 18.7) la- sin(la- ))( cos(rl2 ) rl2 (arl2 + arl3 ) (YCoM,la + 18.7) la- cos(la- )) + marm ( sin(rl2 )
rl2 arl3 ( rl2 + rl3 ) (arl4 + 50.5)( rl ) (YCoM,la + 18.7) cos(la- ) la- ((YCoM,la + 18.7) sin(la- ) la- ) + arl2
+ mt g (arl2 + arl3 ) sin(rl2 ) + mll1 g (arl2 + arl3 ) sin(rl2 ) + mll3 g ((arl2 + arl3 ) sin(rl2 ) all2 sin(ll1- )) + mll4 g ((arl2 + arl3 ) sin(rl2 ) all2 sin(ll1- ) all3 sin(ll2- ))
(G.16)
+ mll5 g ((arl2 + arl3 ) sin(rl2 ) all2 sin(ll1- ) all3 sin(ll2- ) all4 sin(ll3- )) + marm g ((arl2 + arl3 ) sin(rl2 ) + (18.7 YCoM,ra ) sin(ra+ )) + marm g ((arl2 + arl3 ) sin(rl2 ) + (18.7 + YCoM,la ) sin(la- ))
By adding Eq. (G.14) and Eq. (G.15) and subtracting Eq. (G.16) is the torque in joint RL2 determined.
G.4. COMPUTATION OF THE LAGRANGIAN, RL1 AND RL2 261
A more detailed knowledge about the other torques on the biped can be achieved using the same procedure, but as this project has no need for these torques they are not determined.
Appendix
Design of New Feet

A new design of the feet's is made. This is done to give a better calculation of the CoP concerning the robot. The bending of the new feets is made for two reasons. First of all to give room for the strain gauges which is mounted both upon and underneath the foot and these should not have contact with the ground. Secondly to get an smaller contact surface to the ground. This will give fewer possible attacking points of the force compared to the old design. See Fig. H.1 for a dawning of a new and old feet.
old
new
Figure H.1: Design of a new and old feet to the robot

The old feets have the force sensing resistors to measure the attacking point. The actual attacking points on one sensor can be found in the grey area on Fig. H.2. And with the new foot design is the possible attacking point shown in Fig. H.3 as the grey area. This more precise decision of the attacking point, will also make it possible to determinate the CoP more precise. Future more has the new feet design toes. This have the advantage, it makes space between the dierent attacking points. So the interference between the sensors is smaller, compared to the old design. Because the width of the foot is split up. As illustrated in Fig. H.4 is the space between the sensors larger with the new design and that help to detect which side of the foot that is most supporting. As it can be seen in the gure, will the new foot design almost only give an output on one sensor, when the force is applied to the foot. This will give a very good identication of where the attacking force is. With the old design, will more than one sensor be involved in the measurements. This will not give any problem, if all the sensors under 262
263
Contact surface
Contact surface
Contact surface
Contact surface
Figure H.2: The possible attacking point with the old feet design
FSR 1 FSR 2 F
Figure H.3: The possible attacking point with the new feet design
(a)
(b)
Figure H.4: In gure a is the dashed area where the force gives output to the sensors. In gure a is it shown for the old design and in b is it for the new design.
the foot give the same output when the same force is applied. But this is not the case, as the sensors drift with temperature and the sensitive for the FSR is typically 10 [Fraden, 2004].
Appendix
Nomenclature
This chapter gives a brief description of the meaning of the symbols and abbreviation used throughout the report. How the dierent quantities and symbols are typographically represented is also given here, as is the meaning and value of all the constants used throughout the report. First, a table containing the used abbreviation is given in Sec. I.1 on page 264. In Sec. I.2, the typographic representation of the dierent quantities and symbols is described. The symbols generally used for dierent physical quantities are also listed. In Sec. I.3 on 266, a description of the joint angles of the biped robot is given. In Sec. I.4 on 268, all the used constants are dened and their value given.
I.1 Abbreviations and Acronyms

The abbreviations and acronyms used throughout this report are given in Tab. I.1.
Table I.1: Table of abbreviations.

Abbreviation ADC CoM CoP DoF DSP FSR glob LA LL org Term Analog-to-Digital Converter Center of mass Center of pressure Degree(s) of freedom Double Support Phase Force Sensing Resistor Global Left Arm Left Leg Origin Abbreviation PCB PoS RA RL SPI SSP SSP-L SSP-R ZMP Term Printed Circuit Board Polygon of support Right Arm Right Leg Serial Peripheral Interface Single Support Phase SSP - left leg supporting SSP - right leg supporting Zero Moment Point
264
I.2. TYPOGRAPHIC REPRESENTATION OF QUANTITIES
265
I.2 Typographic Representation of Quantities

In Tab. I.2, the general typographic representation of quantities is listed.
Quantity Scalar Frame in text Frame in equation Vector Matrix
Table I.2: The typographic representation of quantities.

Representation Lower-case, italic Upper-case, italic, curly brackets Upper-case, italic Lower-case, italic, bold Upper-case, italic, bold
Example b {B } B b B
When a vector is given a prex in superscript, it denotes the frame, in which the vector is given. For example, the following denotes a vector v given in frame {A}:
A
(I.1)
A transformation matrices, denoted T , is used to transform vectors between frames, see page 40. A transformation matrix, mapping a vector from frame {B } to {A}, is represented as A BT . Hence:
A B v=A BT v
(I.2)
Correspondingly, a rotation matrix R mapping the orientation of a vector given in frame {B } into frame {A} coordinates, is denoted A BR . The symbols and typographic used for representing dierent physical vector quantities is seen in Tab. I.3.
Table I.3: The symbols for dierent physical quantities.

Quantity Force vector Angular position Angular velocity vector Angular acceleration vector Position Linear velocity Linear acceleration Torque vectors The unit vectors Representation F or f or or p v or p or p v , N or n , y and z x
If the vector symbols of Tab. I.3 is not typeset in bold during the report, it means the length of the vector. For example:
F =
(I.3)
To name variable of same type, subscripts are used. For example, joint angle of joint RL2 is denoted:
rl2
(I.4)
266
APPENDIX I. NOMENCLATURE
Position vectors describing the position of a frame origin have org in their subscript. For example, the origin of frame {LA0 } seen from {RL5 } is given by the following vector:
rl5
pla0,org
(I.5)
The position vectors describing the location of CoMs have double subscripts with a C in the rst subscript. For example, the CoM location of link RL1 given in frame {RL1 } is:
rl1
pCrl1
(I.6)
I.3 Descriptions of Variables

This given a brief description of the most important variables used in the report. First the joint angles are describes, followed by the important torque and force vectors, and nally, some additional variables.
Joint Angles
The joint angles used through the modelling are given in Tab.I.4. The rst column shows the symbol of the joint angle variable. In the second column the physical meaning of the variables are described where the terms roll, pitch and yaw is used as dened in Ch. 2. The third species the plane of rotation, note that in zero position roll corresponds to a rotation in the frontal plane, a pitch corresponds to a rotation in the sagittal plane, and a yaw corresponds to a rotation in the horizontal plane. In the fourth column the zero position is given. The fth column species the approximate maximum range of the joint angles. Where maximum means that in some cases depending on other joint positions, the joint will have less freedom of rotation.
Table I.4: Joint variables.

Variable rl1 Meaning Roll angle of the right ankle Pitch angle of the right ankle Pitch angle of the right knee Pitch angle of the right hip Roll angle of the right hip Roll angle of the left hip Pitch angle of the left hip Pitch angle of the left knee Pitch angle of the left ankle Plane Frontal Sagittal Sagittal Sagittal Frontal Frontal Sagittal Sagittal Sagittal Zero position Range 0 : xrl1 is coincident with [45 ; 45 ] xrl0 0 : xrl2 is parallel with [45 ; 45 ] xrl1 in the sagittal plane 0 : xrl3 is parallel with [0 ; 90 ] xrl2 in the sagittal plane 0 : xrl4 is parallel with [90 ; 45 ] xrl3 in the sagittal plane 0 : xrl5 is parallel with [90 ; 45 ] xrl4 in the frontal plane 0 : xll1 is parallel with [45 ; 90 ] xrl5 in the frontal plane 0 : xll2 is parallel with [90 ; 45 ] xll1 in the sagittal plane 0 : xll3 is parallel with [0 ; 90 ] xll2 in the sagittal plane 0 : xll4 is parallel with [45 ; 45 ] xll3 in the sagittal plane Continued on next page
rl2 rl3 rl4 rl5 ll1 ll2 ll3 ll4
I.3. DESCRIPTIONS OF VARIABLES Table I.4: Joint variables.

Variable ll5 Meaning Roll angle of the left ankle Pitch angle of the right shoulder Roll angle of the right shoulder Yaw angle of the right arm Roll angle of the right wrist Roll angle of the right ngers Pitch angle of the left shoulder Roll angle of the left shoulder Yaw angle of the left arm Roll angle of the left wrist Roll angle of the left ngers Plane Frontal Sagittal Frontal Horizontal Frontal Frontal Sagittal Frontal Horizontal Frontal Frontal Zero position 0 : xll5 is parallel with xll4 in the frontal plane 0 : xra1 is parallel with xrl5 in the sagittal plane 0 : the angle from xra2 to xra1 is 90 0 : xra3 is parallel with xra2 in the horizontal plane 0 : the angle from xra4 to xra3 is 36.8 0 : The angle from xra5 to xra4 is 53.2 0 : xla1 is parallel with xrl5 in the sagittal plane 0 : the angle from xla2 to xla1 is 90 0 : xla3 is parallel with xla2 in the horizontal plane 0 : the angle from xla4 to xla3 is 36.8 0 : The angle from xla5 to xla4 is 53.2
267
Range [45 ; 45 ] [90 ; 90 ] [90 ; 0 ] [90 ; 90 ] [0 ; 90 ] [90 ; 0 ] [90 ; 90 ] [90 ; 0 ] [90 ; 90 ] [90 ; 0 ] [90 ; 0 ]
ra1 ra2 ra3 ra4 ra5 la1 la2 la3 la4 la5
Forces and Torques

Table I.5: Forces and torques.
Variable RL1 RL2 Fp,r1 Fp,r2 Fp,r3 Fp,r4 Fp,l1 Fp,l2 Fp,l3 Fp,l4 Meaning The torque delivered by the servo motor in joint RL1 The torque delivered by the servo motor in joint RL2 Pressure force on the little toe of the right foot Pressure force on the big toe of the right foot Pressure force on the little heel of the right foot Pressure force on the big heel of the right foot Pressure force on the little toe of the right foot Pressure force on the big toe of the right foot Pressure force on the little heel of the right foot Pressure force on the big heel of the right foot
Other Variables
268
APPENDIX I. NOMENCLATURE Table I.6: Other variables.

Variable y ref z ref mes y Meaning The y component of the ZMP reference in {RL0 } or {LL5 } The z component of the ZMP reference in {RL0 } or {LL5 } The y component of the ZMP measurement in {RL0 } or {LL5 } The z component of the ZMP measurement in {RL0 } or {LL5 } The ZMP error seen from the frontal plane. The ZMP error seen from the sagittal plane. A trajectory set for feet, hip and hands during a single SSP phase.
mes z err y err z
I.4 Constants
In this section the meaning of all the constants used throughout the report is given along with their value. The rst three tables contains the link parameters used in the Denavit-Hartenberg notation: In Tab. I.7 the link lengths are listed, in Tab. I.8 on page 270 the link twists are listed, and in Tab. I.9 on page 271 the link osets are listed. These constants are taken from [rts et al., 2006]. In Tab. I.10 on page 271 the masses of the links of the robot are given. These constants are taken from [rts et al., 2006], except for the torso, where extra weight have been added due to the mounted microcontroller. On page 272 the position of the CoMs for the dierent link are found, and on page 273 the inertia tensors for the links are listed. These are also from [rts et al., 2006]. The position of the accelerometers is given on page 274, and nally some additional lengths concerning the feet is given on page 275.
Link Lengths
Table I.7: Link length.
Constant arl1 Interpretation Link length from the roll axis to the pitch axis of the right ankle. Link length from the pitch axis of the right ankle to the pitch axis of the right knee. Link length from the pitch axis of the right knee to the pitch axis of the right hip. Link length from the pitch axis to the roll axis of the right hip. Link length from the pitch axis to the roll axis of the left hip. Continued on Value 0.0429 m 0.0572 m 0.0429 m 0.0429 m 0.0429 m next page
arl2 arl3 arl4 all1
I.4. CONSTANTS Table I.7: Link length.

Constant all2 Interpretation Link length from the pitch axis of the left hip to the pitch axis of the left knee. Link length from the pitch axis of the left knee to the pitch axis of the left ankle. Link length from the pitch axis to the roll axis of the left ankle. Link length from the pitch axis to the roll axis of the right shoulder. Link length from the roll axis of the rigtht shoulder to the yaw axis of the right elbow. Link length from the yaw axis of the right elbow to the roll axis of the right wrist. Link length from the roll axis of the right wrist to the roll axis of the right ngers. Link length from the pitch axis to the roll axis of the left shoulder. Link length from the roll axis of the rigtht shoulder to the yaw axis of the left elbow. Link length from the yaw axis of the left elbow to the roll axis of the left wrist. Link length from the roll axis of the left wrist to the roll axis of the left ngers. Value 0.0429 m 0.0572 m 0.0429 m 0.0187 m 0.0294 m -0.0187 m 0.0318 m 0.0187 m 0.0294 m -0.0187 m 0.0318 m
269
all3 all4 ara1 ara2 ara3 ara4 ala1 ala2 ala3 ala4
Link Twists
Table I.8: Link Twists.
Constant rl1 Interpretation Value Link twist between the roll axis and the pitch axis 90 of the right ankle. Link twist between the pitch axis of the right an- 0 kle and the pitch axis of the right knee. Link twist between the pitch axis of the right knee 0 and the pitch axis of the right hip. Link twist between the pitch axis and the roll axis 90 of the right hip. Link twist between the roll axis and the pitch axis 90 of the left hip. Link twist between the pitch axis of the left hip 0 and the pitch axis of the left knee. Link twist between the pitch axis of the left knee 0 and the pitch axis of the left ankle. Link twist between the pitch axis and the roll axis 90 of the left ankle. Link twist between the pitch axis and the roll axis 90 of the right shoulder. Continued on next page
rl2 rl3 rl4 ll1 ll2 ll3 ll4 ra1
270
APPENDIX I. NOMENCLATURE Table I.8: Link Twists.

Constant ra2 Interpretation Link twist between the roll axis of the right shoulder and the yaw axis of the right elbow. Link twist between the yaw axis of the right elbow and the roll axis of the right wrist. Link twist between the roll axis of the right wrist and the roll axis of the right ngers. Link twist between the pitch axis and the roll axis of the left shoulder. Link twist between the roll axis of the left shoulder and the yaw axis of the left elbow. Link twist between the yaw axis of the left elbow and the roll axis of the left wrist. Link twist between the roll axis of the left wrist and the roll axis of the left ngers. Value 90
ra3 ra4 la1 la2 la3 la4
90 0 90 90 90 0
Link osets
Table I.9: Link osets.
Constant drl1 Interpretation Link oset between the roll axis and the pitch axis of the right ankle. Link oset between the pitch axis of the right ankle and the pitch axis of the right knee. Link oset between the pitch axis of the right knee and the pitch axis of the right hip. Link oset between the pitch axis and the roll axis of the right hip. Link oset between the roll axis and the pitch axis of the left hip. Link oset between the pitch axis of the left hip and the pitch axis of the left knee. Link oset between the pitch axis of the left knee and the pitch axis of the left ankle. Link oset between the pitch axis and the roll axis of the left ankle. Link oset between the pitch axis and the roll axis of the right shoulder. Link oset between the roll axis of the right shoulder and the yaw axis of the right elbow. Link oset between the yaw axis of the right elbow and the roll axis of the right wrist. Link oset between the roll axis of the right wrist and the roll axis of the right ngers. Link oset between the pitch axis and the roll axis of the left shoulder. Continued on Value 0m 0m 0m 0m 0m 0m 0m 0m 0m 0.0013 m 0.0688 m 0m 0m next page
drl2 drl3 drl4 dll1 dll2 dll3 dll4 dra1 dra2 dra3 dra4 dla1
I.4. CONSTANTS Table I.9: Link osets.

Constant dla2 Interpretation Link oset between the roll axis of the left shoulder and the yaw axis of the left elbow. Link oset between the yaw axis of the left elbow and the roll axis of the left wrist. Link oset between the roll axis of the left wrist and the roll axis of the left ngers. Value 0.0013 m 0.0688 m 0m
271
dla3 dla4
Center of Masses
Table I.10: Center of masses.
Constant mrl,foot mrl1 mrl2 mrl3 mrl4 mll1 mll2 mll3 mll4 mll5 mra1 mra2 mra3 mra4 mra5 mla1 mla2 mla3 mla4 mla5 mt Interpretation The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the The mass of the right foot right ankle right shin right thigh right hip left hip left thigh left shin left ankle left foot right shoulder right overarm right forearm right wrist right ngers left shoulder left overarm left forearm left wrist left ngers torso Value 0.0988 kg 0.08164 kg 0.08164 kg 0.02117 kg 0.0817 kg 0.0817 kg 0.02117 kg 0.08164 kg 0.08164 kg 0.0988 kg 0.01166 kg 0.14211 kg 0.01166 kg 0.13200 kg 0.01385 kg 0.01166 kg 0.14211 kg 0.01166 kg 0.13200 kg 0.01385 kg 0.3537 kg
272
Position of Center of Masses

Right leg
rl,foot
pCrl,foot
rl3
pCrl3
0.00515 = 0.00859 m, 0.00455 0.02819 = 0.00039 m, 0.00087 0.03840 = 0.00463 m, 0.00731 0.00450 = 0.00731 m, 0.00526 0.00584 m, 0 = 0.01526 0.01629 = 0.00083 m, 0.00399 0.00584 m, 0 = 0.01526 0.016231 = 0.00085 m, 0.00398 0.03426 = 0.03084 m 0.00817
rl1
pCrl1
rl4
pCrl4
0.03840 = 0.00573 m, 0.00602 0.00450 = 0.00731 m 0.00541 0.02819 = 0.00039 m, 0.00087 0.00515 = 0.00859 m 0.00455
rl2
pCrl2
0.03840 = 0.00541 m 0.00731
Left leg
ll1
pCll1
ll2
pCll2
ll3
pCll3
0.00450 = 0.00731 m 0.00463
ll4
pCll4
ll5
pCll5
Right arm
ra1
pCra1
ra4
pCra4
0.01472 ra2 pCra2 = 0.01015 m, 0.00784 0.00114 ra5 pCra5 = 0.03463 m 0.00122 0.01472 = 0.01015 m, 0.00784 0.00114 = 0.03463 m 0.00127
ra3
pCra3
0.00584 m 0 = 0.01526
Left arm
la1
pCla1
la2
pCla2
la3
pCla3
0.00584 m 0 = 0.01526
la4
pCla4
la5
pCla5
Torso
rl5
pCrl5
CRA2 CRA3 CRA1 CT CRA4 CRA5 CLL1 CRL4 C LL2 CRL3 C CRL3 LL3 CRL2 CRL1 CRL,foot CLL5 CLL4 CRL2 CLA1 CLA3 CLA4
CLA2
[m]
CLA5
[m]
[m]
[m]
Figure I.1: The center of masses.
I.4. CONSTANTS
273
Inertia Matrices
Right leg
Crl,foot
I rl,foot
Crl1
I rl1
Crl2
I rl2
Crl3
I rl3
Crl4
I rl4
0.00006187 0.00000073 0.00000151 = 0.00000073 0.00004666 0.00000155 kg m2 0.00000151 0.00000155 0.00002896 0.00002354 0.00000156 0.00000104 = 0.00000156 0.00002348 0.00000201 kg m2 0.00000104 0.00000201 0.00002085 0.00002374 0.00000126 0.00000157 = 0.00000126 0.00002529 0.00000181 kg m2 0.00000157 0.00000181 0.00001907 0.00001107 0 0 kg m2 0 0.00001603 0 = 0 0 0.00000713 0.00002374 0.00000158 0.00000126 = 0.00000158 0.00001907 0.00000181 kg m2 0.00000126 0.00000181 0.00002529
Left leg
Cll1
I ll1
Cll2
I ll2
Cll3
I ll3
Cll4
I ll4
Cll5
I ll5
0.00002374 0.00000126 0.00000158 = 0.00000126 0.00002529 0.00000181 kg m2 0.00000158 0.00000181 0.00001907 0.00001107 0 0 kg m2 0 0.00001603 0 = 0 0 0.00000713 0.00002374 0.00000157 0.00000126 = 0.00000157 0.00001907 0.00000181 kg m2 0.00000126 0.00000181 0.00002529 0.00002354 0.00000156 0.00000104 = 0.00000156 0.00002348 0.00000201 kg m2 0.00000104 0.00000201 0.00002085 0.00006187 0.00000073 0.00000151 0.00000155 kg m2 = 0.00000073 0.00004666 0.00000151 0.00000155 0.00002896
274
Right arm
Cra1
I ra1
Cra2
I ra2
Cra3
I ra3
Cra4
I ra4
Cra5
I ra5
0.000007212 0 0.000000885 kg m2 0 0.000002663 0 = 0.000000885 0 0.000006986 0.00003555 0.00000372 0.00000068 = 0.00000372 0.00004920 0.00000021 kg m2 0.00000068 0.00000021 0.00004917 0.000007212 0 0.000000885 kg m2 0 0.000002663 0 = 0.000000885 0 0.000006986 0.00003785 0.00000008 0.00000022 0.00004014 0.00000062 kg m2 = 0.00000008 0.00000022 0.00000062 0.00005021 0.00001174 0.00000087 0 kg m2 0 = 0.00000087 0.00000612 0 0 0.00000777
Left arm
Cla1
I la1
Cla2
I la2
Cla3
I la3
Cla4
I la4
Cla5
I la5
0.000007212 0 0.000000885 kg m2 0 0.000002663 0 = 0.000000885 0 0.000006986 0.00003535 0.00000426 0.00000163 = 0.00000426 0.00004911 0.00000032 kg m2 0.00000163 0.00000032 0.00004903 0.000007212 0 0.000000885 kg m2 0 0.000002663 0 = 0.000000885 0 0.000006986 0.00003789 0.00000044 0.00000025 0.00000060 kg m2 = 0.00000044 0.00004017 0.00000025 0.00000060 0.00005029 0.00001174 0.00000087 0 kg m2 0 = 0.00000087 0.00000612 0 0 0.00000777
Torso
0.0002959 Ct I t = 0.0000019 0.0000225
0.0000019 0.0000225 0.0002573 0.0000029 kg m2 0.0000029 0.0004514
Accelerometer Positions
0.0110 rl0 pSrl0 = 0.0110 m, 0.0470 0.0222 ra4 pSra4 = 0.0004 m, 0.0030 0.0110 = 0.0110 m, 0.0470 0.0212 = 0.0066 m 0.0030 0.0220 = 0.0300 m 0.0500
ll5
pSll5
rl5
pSrl5
la4
pSla4
I.4. CONSTANTS
275
Additional Constants
Table I.11: Additional Constants.
Constant lheels Interpretation The horizontal distance (sagittal plane) to the center of the heels from frame {RL0 } and {LL5 } The horizontal distance (sagittal plane) to the center of the toes from frame {RL0 } and {LL5 } The horizontal distance (frontal plane) to the center of little toe from frame {RL0 } and {LL5 } The horizontal distance (frontal plane) to the center of the big toe from frame {RL0 } and {LL5 } The center of the foot seen in frame {RL0 } Value 0.0608 m 0.0627 m 0.0315 m 0.0125 m [-0:02,0:0095,-0:001]T m
ltoes llittle toe lbig toe pc,PoS
Appendix
Fold Out Page

This fold-out contains ve gures illustrating the Denavit-Hartenberg notation applied on the biped robot. The frame axed to the dierent links of the biped robot is seen in Fig. a. These are the frames used throughout the report. The right leg is abbreviated RL, the left leg LL, the right arm RA and the left arm LA. In Fig. b the positive direction of the joint angles is illustrated. Figure c shows the dierent link parameters possible to depict in a front view of the biped robot. Figure d illustrates the four body-attached frames, RL5, LL0, LA0 and RA0, each drawn with dashed lines. Figure e shows the base-frame RL0, and Fig. f shows the relative orientation of the frames RL5, LL0 and LA0.
276

Biped Robot Report

Diunggah oleh

Informasi Dokumen

Deskripsi Asli:

Judul Asli

Hak Cipta

Format Tersedia

Bagikan dokumen Ini

Bagikan atau Tanam Dokumen

Opsi Berbagi

Apakah menurut Anda dokumen ini bermanfaat?

Apakah konten ini tidak pantas?

Hak Cipta:

Format Tersedia

Biped Robot Report

Diunggah oleh

Hak Cipta:

Format Tersedia

Modelling and Control of a Biped Robot

Model based Gait Trajectory Generation using a Genetic Algorithm

Department of Control Engineering 8th Semester

P8, Spring Semester 2007

Supervisor: Copies: 9 Pages: 276

Attachments: Enclosed CD-ROM Finished: 31st of May 2007

P8, Forrssemester 2007

Aalborg University, 31st of May, 2007

Rico Hjerm Hansen

Asger Malte Iversen

Mads Schmidt Jensen

Louis Schultz Lantow

Human Anatomy . . . . . . . . . . . . . . . . . . . . . . . . . . . . . Biped Mechanics . . . . . . . . . . . . . . . . . . . . . . . . . . . . .

3 The Scope of the Project

System Description and Conguration

6 Modelling Strategy 7 Servomotor Model 8 Kinematic Model

9 Newton-Euler Based Dynamic Model

10 Lagrangian Mechanics based Dynamic Model

101 105 106 107 110 113

14 Force Measuring to CoP Position

16 Trajectory Supervisor Part Summary

V System Test and Summary

E Servomotor model F The Newton-Euler Iterations G Lagrange

201 206 216

H Design of New Feet I Nomenclature

J Fold Out Page

The Context and Purpose of the Project

1.1 State of the Art in Biped Robotics

1.2. THE AAUBOT PROJECT

Figure 1.1: The rst model E0 and ASIMO from Honda.

1.2 The AAUBOT Project

Figure 1.2: The nal design of the AAUBOT-1 [www.aau-bot.dk].

CHAPTER 1. THE CONTEXT AND PURPOSE OF THE PROJECT

1.3 Studies to support the AAUBOT-1 Project

1.4 The Purpose of This Project

2.1 Human Anatomy

2.1.1 Planes of Motion

CHAPTER 2. BIPED TERMINOLOGY

Horizontal plane CoM

2.1.3 Walk Analysis

2.2. BIPED MECHANICS

2.2 Biped Mechanics

CHAPTER 2. BIPED TERMINOLOGY

1 DSP-L Release 2 Impact SSP-R

Impact 3 Release DSP-R 4

Figure 2.3: The gait of a normal dynamic walk.

frontal view of the robot

sagittal view of the robot

Figure 2.4: The zero position of the biped robot.

2.2. BIPED MECHANICS

right foot left foot

Fresulting presure force=Fp,1+Fp,2 Fp,1 Fbody,1 Fp,2

Fbody,2 Fresulting force on the body

CHAPTER 2. BIPED TERMINOLOGY

The Scope of the Project

3.1 Review and Summary of the Work of Group 06gr833

CHAPTER 3. THE SCOPE OF THE PROJECT

Description and Configuration of Robot

Stable stand with controller

Static walk with controller

Stable stand with controller

Human gait experiment: To obtain data concerning dynamic walking, an experi-

System Description and Conguration

Figure 1.1: The rst model E0 and ASIMO from Honda.

Figure 1.2: The nal design of the AAUBOT-1 [www.aau-bot.dk].

3.3 Requirement Specication

System Description and Conguration

Convention for Axing Frames to Links