control algorithms development for locomotion of a humanoid robot
DESCRIPTION
Control Algorithms Development for Locomotion of a Humanoid Robot. Milton Ruas 1 Filipe M. T. Silva 1 Vítor M. F. Santos 2. 1 Department of Electronics and Telecommunications 2 Department of Mechanical Engineering University of Aveiro, PORTUGAL. Overview. Introduction - PowerPoint PPT PresentationTRANSCRIPT
Centre for Mechanical Technology and Automation
Institute of Electronics Engineering and Telematics
TEMA
IEETA http://www.mec.ua.pt/robotics
Control Algorithms Development for Locomotion of a Humanoid Robot
Milton Ruas1
Filipe M. T. Silva1
Vítor M. F. Santos2
1 Department of Electronics and Telecommunications2 Department of Mechanical Engineering
University of Aveiro, PORTUGALUniversity of Aveiro, PORTUGAL
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Overview Introduction Humanoid Platform: Overview
Actuation Element: the Servomotor Experimental Setup Speed Control Open and Closed Loop Performance Torque Measurement Algorithms Application on the Humanoid Balance Control
Conclusions and Open Issues
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Introduction Project ’s Motivation
Develop a humanoid platform for research on control, navigation and perception
Offer opportunities for under & pos-graduate students to apply engineering methods and techniques
The utopia of Man to develop an artificial being with some of its own capabilities…
Objectives Describe how an external microprocessor can read the
shaft position and current consumption of RC servomotors Improve servo’s performance towards a control system
that provides variable velocity and compensates for load variations
Implement an example of local control based on force sensors present in both feet
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Humanoid Platform Complete humanoid model
22 degrees of freedom Weight - 5 kg Height - 60 cm Max. width - 25 cm Foot print - 20 8 (cm2)
Actuation Servomotors with transmission belts
Sensors Servos’ internal potentiometers Sensitive foot Accelerometers Gyroscopes
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Control System Architecture
Distributed control system A network of controllers connected
by a CAN bus Master/multi-slave arrangement:
• 8 slave units for joints actuation and sensors reading
• 1 master unit for interface between main unit and slaves
Asynchronous communications Between master and slaves: CAN
bus at 833.3 Kbit/s Between master and high level
controller: currently serial RS232 at 115200 baud
Main ControlMain Control
RS23RS2322MasteMaste
rr
CAN CAN BUSBUS
1
23 1
2
3
1
2
31
2 1
2
3
1
2
3
1
2
3
1
2
SlaveSlavess
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Local Control
Power resistor (0.47)
16:1 multiplexer
CAN connector
Piggy-back socket
PIC Cristal oscillator
CAN driver
PIC
Unit CAN Address
PWM plugs
Servo fuse
Fuse status LED
Piggy-back board 2
Piggy-back board 1
Connector to sensor
CAN bus Power plug
Power regulator Reset button
RS232 plug
Connector to sensor
Each slave controller is made of a PIC 18F258 device with I/O interfacing
All slave units: Connect up to 3 servomotors Have a common base (a piggy-
back unit can add I/O sensors)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
The Servomotor
1-2 ms 20 ms
PWM
PWM: Period: 20ms (50 Hz) Duty cycle: 1-2 ms
Why servomotors? Small and compact Relatively inexpensive Position control included
Servomotor parts: DC motor Gearbox Controlling electronics Position feedback mechanism
Characteristics Motion excursion: 180º Position control: digital signal, PWM
Some constraints: Doesn’t offer velocity control Doesn’t consider the external load
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Experimental Setup Main goals
To study the servomotor’s performance with high loads and/or velocities
Improve the system’s behaviour by software compensation
Only one physical intervention: Connection of an extra output wire to the
servo internal potentiometer Servomotor
Feedback position y(t)
Applying position u(t)
Computer
CAN bus
RS-232
Master unit
Slave unit
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Servo’s Parameters Measurement Potentiometer outputs shaft
position (feedback signal)
Different voltage “grounds” for external measurement and internal controller can produce measuring fluctuations
For high loads/fast motions a pulse above the position voltage is added: Fixed amplitude Synchronized to PWM Pulse width related with current
consumption!
Position and current measured from the same output: Position: minimal value Current: pulse width For high current draining, position
reading can be inhibited!
Input PWM pulse
Motor position
(variable)
“current” pulse
Amplitude fixed at
maximum
20 ms
Pulse width proportional to torque/current
τg = Ka.I = m.g.L.cos(Θ)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Programming Issues Each slave unit controls up to 3 servos. Actuation:
Each servo has associated to it a variable to store its PWM width
μC Timer2 interrupts compares a counter to each variable to apply PWM fall-down
Potentiometer signal
PWM
Potentiometer reading Interrupts (Timer 0)
ADCconversion
start
ADCreading
PWM for actuation
High frequency interrupts forPWM fall down (Timer 2)
Interrupt for PWMrise up (Timer 1)
5V
20 ms
1 to 2 ms
ADC starts conversion
Converted voltage is processed
Select next Servo (change MUX input)
Timer 0 generates an interrupt
ADC finishesconversion and
generates an interruptConversion Time: 40μs
Processing Time:10μs
MUX stabilization + Acquisition time:70μs
Sensing Timer0 interrupts used to measure
feedback data Servo output sensing is multiplexed in
time (120μs*3).
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Velocity Control Application of a position step
Servo drives to the commanded position at maximum speed
User cannot directly control velocity!
How to control velocity with only position control? Trajectory planning!
Slope input Application of successive “small” step Amplitude and time delay of each step defines
the average speed Existence of speed discontinuities!
3ª order polynomial x(t)=a0+a1t+a2t2+a3t3
Total period defines speed Null initial and final speed Finite initial and final acceleration
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Open Loop Performance Step response: -45º → +45º
Steady state error varies with the load (around 8 degrees for 1129g)
Response delay varies with the load Servo executes a fast and continuous
motion to the final position Unstable position readings with the
maximum load!
Ramp response: Δt=100ms Δx=5º T=1.8s Steady state error still present Transient response improved
Static analysis Steady state error depends of
gravitational terms
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Low-Level Control Objectives
Eliminate steady state error Reduce response time lag … using software solutions
An external controller is implemented using: Servo’s own potentiometer for feedback Incremental algorithm of a digital PID controller
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Integral Control Ramp:
-45º → +45º Δt=100ms, Δx=5º, T=1.8s
Open loop vs. closed loop (KI=0.2) Steady state error eliminated Time lag reduced
Ramp: -90º → +50º Δt=40ms, Δx=5º, T=1.12s
KI=0.06 vs. KI=0.10 (KP=0.04) Time delay reduced Overshoot arises
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
PID Control Ramp response
-90º → +50º Δt=40ms, Δx=5º, T=1.12s
KP=0.04 VS KP=0.30 (KI=0.10) Overshoot eliminated No interference with time delay
Polynomial response (T=1s): Increasing KP… Overshoot is reduced Time delay diminishes Establishment time increases
Adding Derivative component: Transient state smoothed Overshoot and time delay remains
unchanged Sensitive to noise
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
P+I+D Terms Integrator term
Eliminates steady state error Improves time lag Deteriorates overshoot
Proportional term Reduces overshoot Improves time delay Deteriorates establishment time
Derivative term Smoothes transient response Very sensitive to noise
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Servo’s Torque Measurement For a Humanoid platform, the load
seen by each actuator can vary rapidly and substantially
For each task it is required to apply an adequate set of control parameters to ensure good performance
Two approaches can be followed: Parameters determination and manual
application from the main unit for each task Local parameter automatic adaptation,
through torque analysis
Torque estimation can be done from current measurement (τ=K*I)
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Humanoid Locomotion The robot is able to stand, lean on sides, for/backward
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Humanoid Locomotion Load weight: 2.1Kg
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Humanoid Locomotion
0 2 4 6 8-60
-50
-40
-30
-20
-10
0
10
time (s)
Hip
po
sitio
n (
de
gre
es)
Open Loop
Ki=0.10, Kp=0.80
Expected trajectory
0 2 4 6 8-80
-60
-40
-20
0
20
40
time (s)
Kn
ee
po
sitio
n (
de
gre
es)
Ki=0.10, Kp=0.80
Open Loop
Expected trajectory
0 2 4 6 80
10
20
30
40
50
60
70
time (s)
Fo
ot p
osi
tion
(d
eg
ree
s)
Open Loop
Expected trajectory
Ki=0.10, Kp=0.80
Flexion movement
Foot roll jointFoot roll joint
Kn
ee join
tK
nee join
tH
ip join
tH
ip join
t
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Humanoid Locomotion Lateral movement
0 1 2 3 4 50
5
10
15
20
25
30
35
time (s)
Fo
ot p
osi
tion
(d
eg
ree
s)
Open Loop
Expected trajectory
Ki=0.15, Kp=0.20
Foot tilt jointFoot tilt joint
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Balance Control Our goal is to balance Humanoid’s body using
Force sensors Force sensors on each foot are used to…
measure ground reaction forces calculate the location of the Center of Pressure (CoP)
4 Extensometers used as force sensors!
Sensor’s outputAmplifier
Sensor’s outputAmplifier
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Balance Control Control Law: Δq = K·JT·e
Δq: velocity to apply to the leg joints K: constant JT: transposed Jacobian of the CoP
• For lower velocities CoP≈CoM e is the error between the desired and the
actual CoP
Velocity application: pos(n)=pos(n-1)+Δq
knee
z
footroll
z
foottilt
z
knee
y
footroll
y
foottilt
y
knee
x
footroll
x
foottilt
x
CoMCoMCoM
CoMCoMCoM
CoMCoMCoM
J
Senso
r 1
Senso
r 2
Senso
r 3
Senso
r 4
y
x
FootFoot
4
1
4
1
sensorsensor
sensorsensorsensor
F
rF
CoM
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Balance Control Application Pression on each force sensor
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Balance Control Application Changing support plane sloping
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Plane Sloping Change Balance through the yy axis
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Balance Control Application Introducing perturbations
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Conclusions Procedures were presented to measure servos’
position and torque with a servomotor… without velocity control whose speed and steady-state error is dependent of motor load
Methods were described on how… to introduce velocity control through trajectory planning to correct time lag and steady-state error through an external
controller using position reading as feedback …all this without hardware interventions!
Nevertheless, controller needs to be updated to maintain efficiency: To build an adaptive PID controller whose parameters are based
on torque estimation is the next objective to pursue
A special case of local control was presented: Leg’s balance was implemented with very reactive responses!
UNIVERSITY OF AVEIRO, PORTUGALCentre for Mechanical Technology and AutomationInstitute of Electronics Engineering and Telematics
Future Perspectives
Use torque estimation to compensate inertial variations on the joints;
Improve materials quality used to sense the reaction forces for the balance control;
Combine position/velocity control with balance control;
Substitute the main controller unit (PC) with an embedded motherboard based on a PC104, nano-ITX or similar;
Participate in the next edition of Robocup in 2007 – Humanoid League!