hybrid position-based visual servoing

Hybrid Position-Based Hybrid Position-Based Visual ServoingVisual Servoing

Intelligent Robotics Research Centre (IRRC)

Department of Electrical and Computer Systems Engineering

Monash University, Australia

Visual Perception and Robotic Manipulation

Springer Tracts in Advanced Robotics

Chapter 6Chapter 6

Geoffrey Taylor

Lindsay Kleeman

2Taylor and Kleeman, Visual Perception and Robotic Manipulation, Springer Tracts in Advanced Robotics

OverviewOverview

• Motivation for hybrid visual servoing

• Visual measurements and online calibration

• Kinematic measurements

• Implementation of controller and IEKF

• Experimental comparison of hybrid visual servoing with existing techniques


MotivationMotivation

• Manipulation tasks for a humanoid robot are characterized by:– Autonomous planning

from internal models

– Arbitrarily large initial pose error

– Background clutter and occluding obstacles

– Cheap sensors camera model errors

– Light, compliant limbs kinematic calibration errors

Metalman: upper-torso humanoid hand-eye system


Visual ServoingVisual Servoing

• Image-based visual servoing (IBVS):– Robust to calibration errors if target image known– Depth of target must be estimated– Large pose error can cause unpredictable trajectory

• Position-based visual servoing (PBVS):– Allows 3D trajectory planning– Sensitive to calibration errors– End-effector may leave field of view

• Linear approximations (affine cameras, etc)• Deng et al (2002) suggest little difference

between visual servoing schemes


Conventional PBVSConventional PBVS

• Endpoint open-loop (EOL):– Controller observes only the target

– End-effector pose estimated using kinematic model and calibrated hand-eye transformation

– Not affected by occlusion of the end-effector

• Endpoint closed-loop (ECL):– Controller observes both target and end-effector

– Less sensitive to kinematic calibration errors but fails when the end-effector is obscured

– Accuracy depends on camera model and 3D pose reconstruction method


Proposed SchemeProposed Scheme

• Hybrid position-based visual servoing using fusion of visual and kinematic measurements:– Visual measurements provide accurate positioning

– Kinematic measurements provide robustness to occlusions and clutter

– End-effector pose is estimated from fused measure-ments using Iterated Extended Kalman Filter (IEKF)

– Additional state variables included for on-line calibration of camera and kinematic models

• Hybrid PBVS has the benefits of both EOL and ECL control and the deficiencies of neither.


Coordinate FramesCoordinate Frames

EOLECLHybrid


• Conventional approach (Hutchinson et al, 1999).

• Control error (pose error):

• WHE estimated by visual/kinematic fusion in IEKF.

• Proportional velocity control signal:

PBVS ControllerPBVS Controller

OGG

OGG

OG

OGG

k

k

TΩTV

AΩ

2

1

OW

GE

EW

OG H)HH(H 1


ImplementationImplementation


Visual MeasurementsVisual Measurements

• Gripper tracked using active LED features, represented by an internal point model

• IEKF measurement model: iE

EW

WCRL

iRL Gg HHPˆ ,,

Gi

C

gi

image plane

camera centre

3D gripper model

measurements


Camera Model ErrorsCamera Model Errors

• In practical system, baseline and verge angle may not be known precisely.

2b

left camera centre

right camera centre

left image plane

right image plane

reconstruction

2b*

scaled reconstruction

-*

affine reconstruction


• How does scale error affect pose estimation?

• Consider the case of translation only by TE:

– Predicted measurements:

– Actual measurements:

– Relationship between actual and estimated pose:

• Estimated pose for different objects in the same position with same scale error is different!


)ˆ(HHPˆ ,,Ei

EE

WW

CRLi

RL TGg

)(HHP 1,,

EiE

EW

WCRL

iRL K TGg

EEiE KbKf TTGT 11 ),,,(ˆ



• Scale error will cause non-convergence of PBVS!

• Although the estimated gripper and object frames align, the actual frames are not aligned.


• To remove model errors, scale term is estimated by IEKF using modified measurement equation:

• Scale estimate requires four observed points with at least one in each stereo field.

Visual MeasurementsVisual Measurements

)ˆ(HHPˆ 1,,

iE

EW

WCRL

iRL K Gg


• Kinematic measurement from PUMA is BHE

• Measurement prediction (for IEKF):

• Hand-eye transformation BHW is treated as a dynamic bias and estimated in the IEKF

• Estimating BHW requires visual estimation of WHE, and is therefore dropped from the state vector if the gripper is obscured.

Kinematic ModelKinematic Model

EW

WB

EB HHH


• Kalman filter state vector (position, velocity, calibration parameters):

• Measurement vector (visual + kinematic):

• Dynamic models:– Constant velocity model for pose– Static model for calibration parameters

• Initial state from kinematic measurements.

Kalman FilterKalman Filter

TW

BE

WE

W kKkkkk ))(),(),(),(()( 1prpx

TE

BRL kkkk ))(,),(),(()( 00 pggy


ConstraintsConstraints

• Three points required for visual pose recovery

• Stereo measurements required for scale estimation

• LED association required multiple observed LEDs

• Estimation of BHW requires visual observations

• Use a hierarchy of estimators (nL,R = no. points):

– nL,R < 3: EOL control, no estimation of K1 or BHW

– nL > 3 xor nR > 3: Hybrid control, no K1

– nL,R > 3: Hybrid control (visual + kinematic)

• Excluded state variables are discarded by setting rows and columns of Jacobian to zero


LED MeasurementLED Measurement

• LEDs centroids measured with red colour filter

• Measured and model LEDs associated using a global matching procedure.

• Robust global matching requires 3 LEDs.

Predicted LEDs

Observed LEDs


Experimental ResultsExperimental Results

• Positioning experiment:– Align midpoint between

thumb and forefinger at coloured marker A

– Align thumb and forefingeron line between A and B

• Accuracy evaluation:– Translation error: distance between midpoint of

thumb/forefinger and A

– Orientation error: angle between line joining thumb/forefinger and line joining A/B


Positioning AccuracyPositioning Accuracy

Hybrid controller, initial pose(right camera only)

Hybrid controller, final pose(right camera only)



ECL controller, final pose(right camera only)

EOL controller, final pose(right camera only)



• Accuracy measured over 5 trial per controller.


Tracking RobustnessTracking Robustness

Initial pose: gripper outside FOV (ECL control)

Final pose: gripper obscured (Hybrid control, mono)

Gripper enters field of view (Hybrid control, stereo)


Tracking RobustnessTracking Robustness

Translational component of pose error

Estimated scale (camera calibration parameter)

EOLHybridstereo

Hybridmono EOL

Hybridstereo

Hybridmono


Baseline ErrorBaseline Error

• Error introduced in calibrated baseline:– Baseline scaled between 0.7 to 1.5

• Hybrid PBVS performance in presence of error:


Verge ErrorVerge Error

• Error introduced in calibrated verge:– Offset between –6 to +8 degrees

• Hybrid PBVS performance in presence of error:


Servoing TaskServoing Task


ConclusionsConclusions

• We have proposed a hybrid PBVS scheme to solve problems in real-world tasks:– Kinematic measurements overcome occlusions

– Visual measurements improve accuracy and overcome calibration errors

• Experimental results verify the increased accuracy and robustness compared to conventional methods.

hybrid position-based visual servoing

Documents