Transcript Localization - Test Page for the Apache Web Server on Red

The Hardware Design of the Humanoid Robot RO-PE
and the Self-localization Algorithm in RoboCup
Tian Bo
Control and Mechatronics Lab
Mechanical Engineering
20 Feb 2009 SMC
RoboCup and Team RO-PE

RoboCupTM
is an international joint project to promote
artificial intelligence and robotics.

Team RO-PE
RO-PE (RObot for Personal Entertainment) is
a series of small size humanoid robots
developed by the Legged Locomotion Group
Highlight
Design of the Robot
Hierarchy of the System
PC104
USB
CAMERA
ARM PROCESSOR
SERVOS
Gyroscope
Accelerameter
Self-localization in RoboCup

Global localization problem
-The robot is not told its initial pose, but has to
determine it from the very beginning
Self-localization in RoboCup
Global localization problem
 Kidnapped robot problem

- a well-localized robot is teleported to some other
position without being told
- The kidnapped robot problem is often used to
test a robot’s ability to recover autonomously
form catastrophic localization failures
Self-localization in RoboCup
Global localization problem
 Kidnapped robot problem
 Other difficulties in the humanoid soccer
scenario

- The field of view is limited, due to the human-like
sensor.
- Noisy perceptions and noisy odometry.
- Computational resources are limited. But data
needs to be processed in real-time.
What is Particle Filter
Belongs to the family of Bayesian Filters
(Bayes Filter,Kalman Filter…)
 Bayesian filter techniques provide a
powerful statistical tool to help manage
measurement uncertainty
 Based on the knowledge of previous
state, Bayesian filter probabilistically
estimates a dynamic system’s state from
noisy environment.

What is Particle Filter

Particle filters represent beliefs by a sets of
samples, or particles

It is a probabilistic approach, in which the
current location of the modeled as a density of
the particles.

Each particle can be seen as the hypotheses
of the robot being located at this posture.
What is Particle Filter
The main objective of particle filtering is
to “track” a variable of interest as it
evolves over time, typically with a nonGaussian and potentially multi-modal pdf
 The particle filter algorithm is recursive
in nature and operates in two phases:
prediction and update

Particle Filter Localization
Move all the particles according
to the motion model of the
previous action of the robot
Determine the
probabilities qi
based on
observation model
Resampling
Particle Filter Algorithm
1. Algorithm particle_filter( St-1, ut-1 zt):
2. St  ,
 0
3. For i  1 n
Generate new samples
4.
Sample index j(i) from the discrete distribution given by wt-1
5.
Sample xti from p( xt | xt 1, ut 1 ) using xtj(1i ) and ut 1
6.
wti  p( zt | xti )
Compute importance weight
7.
    wti
Update normalization factor
8.
St  St { xti , wti }
Insert
9. For i  1 n
10.
wti  wti / 
Normalize weights
(Probabilistic Robotics, C4 P98)
Particle Filter for Self-Localization
Loop
 initializeParticles(); //p[i] (x, y, theta, w)
While(sensor reset != 1){
motionModel();
 sensorModel();{


updateWeight();}

resampling();

output();}
Initialization
Motion Model
This is the prediction part
 The particle filter for self-localization
estimates the robot’s pose
 Odometry-based Method
Take x for example:
p[m].x = p[m].x + deltaX *(1+gaussian)
x  x  x  gaussian  x, 
m
i
m
i 1
Motion Model

z
Simplified Leg Model

Step 1
hip_yaw = 0
OE  OB cos  hip _ pitch 
O
y
hip_pitch
hip_roll
E
x
B
ED  BC cos  hip _ pitch  knee _ pitch 
A
D
OD  OE  ED
z  OA  OD cos  hip _ roll 
y  AD  OD sin  hip _ roll 
B’
knee_pitch
C
x  CD  CB  BD  OB sin  hip _ pitch   BC sin  hip _ pitch  knee _ pitch 
Motion Model
Simplified Leg Model

(x’, y’)
Step 2
hip_yaw = θ
(x, y)
C
θ

y
0 A
  hip _ yaw
 x  cos   sin    x   x cos   y sin  
 y   sin  cos    y    x sin   y cos  
  
  

z' z
hip_yaw
x
Motion Model

We do localization when
the left leg just touch the
ground
y
Δ(x,y,θ)

This odometry gets the
data from the motion
commend sent to servo,
it will not be affected by
the control signal. It can
be more accurate if the
servo can feedback its
position.
0
x
Error for the Motion Model(1)
with the steps increased, the error increase.
The largest error is 25%, happens at the 14th step.
Error for the Motion Model(2)
Motion
Real value
Odom value
Side step
y:15cm
y:3.1cm
Hugging
θ:-0.785
θ:-1.8
Turn
θ:2.36
θ:3.6
Possible reason
Neglect of the AR joint effect,
neglect of the length between
HR and HP, AP and AR
The collision of the
themselves during turning
feet
Like walking motion, the real distance has a linear
relationship with the number of steps, so we can
achieve better results through improve our model or
make correction.
Back to Particle Filter
Sensor Model
This is the update part

In the whole field, we only use the two goals and two
poles for self-localization. The world model is known.

We only take the angle from the landmark to the front
of robot into consideration
Sensor Model

We are only using the wide angle camera for landmark
recognition, the information we can abstract from the
camera is limited.
Sensor Model

Once a landmark is observed by the
robot, the function sensorModel() will be
executed. The weight for every particle
will be updated accordingly.
wim  wim1  belief  seen ,  expected 

If several landmarks are observed at
once, the weight will be
wim  wim1  belief1  belief2 
Sensor Model
We can get the expectedTheta
through the position and orientation
of the particle and the world model
 if(blue_goal_found){
/*we can get the percievedTheta from camera,
the coordination of the landmark on the image,
and the position of the panning servo of the
head */
updateWeight(blue_goal)}
Sensor Model

Update Weight
deltaTheta = fabs ( expectedTheta –
perceivedTheta) ;
belief = distribution ( deltaTheta );
p[i].weight = p[i].weight * belief
Normalize(p[i].weight);

Distribution Policy
Now we are using Gaussian distribution.
Resampling

The simplest method of resampling is to
select each particle with a probability
equal to its weight.
Select with Replacement
Linear time Resampling
Resampling by Liu et al.
Resampling
(A Particle Filter Tutorial for Mobile Robot Localization TR-CIM-04-02)
Final Estimation

Finding the Largest Cluster


Calculating the Average


Give the best result but computational
expensive
May affect by the far away particles
Best Weight

Fastest way to give the result, suitable for
the real-time system
Future work
Find out the condition to make sensor
resetting, or else sometimes the particle
will converge to a false point and cannot
recover.
 Including the distance information in
sensor model.
 Try new resampling and weightUpdate
algorithm.

Thank you