Open Collections

UBC Theses and Dissertations

UBC Theses Logo

UBC Theses and Dissertations

Constraint-aware visual servoing for teaching practical robot motion Chan, Ambrose 2009

You don't seem to have a PDF reader installed, try download the pdf

Item Metadata

Download

Media
[if-you-see-this-DO-NOT-CLICK]
ubc_2009_fall_chan_ambrose.pdf [ 3.58MB ]
Metadata
JSON: 1.0067229.json
JSON-LD: 1.0067229+ld.json
RDF/XML (Pretty): 1.0067229.xml
RDF/JSON: 1.0067229+rdf.json
Turtle: 1.0067229+rdf-turtle.txt
N-Triples: 1.0067229+rdf-ntriples.txt
Original Record: 1.0067229 +original-record.json
Full Text
1.0067229.txt
Citation
1.0067229.ris

Full Text

Constraint-Aware Visual Servoing forTeaching Practical Robot MotionbyAmbrose ChanB.A.Sc., The University of Waterloo, 2006A THESIS SUBMITTED IN PARTIAL FULFILMENT OFTHE REQUIREMENTS FOR THE DEGREE OFMaster of Applied ScienceinThe Faculty of Graduate Studies(Mechanical Engineering)The University Of British Columbia(Vancouver)April, 2009c Ambrose Chan 2009AbstractIn this thesis, a constraint-aware visual servoing control law is proposed.The control law is designed for a robot manipulator with an uncalibratedcamera mounted on its end-e ector. This control law allows the robot toexecute large, collision-free motions with closed-loop positional accuracy.A reference image visually describes the desired end-e ector position withrespect to a target object whose location is initially unknown. The controllaw uses this reference image with online feedback from the camera to directthe trajectory of robot towards the completion of the positioning task. Thecontrol law generates feasible and realistic robot trajectories that respectthe robot’s joint position and velocity limits, even in the presence of largecontrol gains. The control law also explicitly keeps the target object withinthe camera’s  eld of view to provide uninterrupted visual feedback. Thecontrol law avoids potential whole-arm collisions with workspace obstaclesvia planning and control strategies.The visual servoing control law is implemented in a nonlinear modelpredictive control framework, using an estimated model of the eye-in-handcon guration and an estimated location of the target object. Two meth-ods of approximating the object’s location for joint-space path planning aredemonstrated in simulations and experiments. The  rst uses homographyestimation and decomposition on an un-modelled object. The second usesan extended Kalman  lter with a prior object model to improve robustnessagainst image noise and disturbances.Two planning and control strategies are presented. The  rst strat-egy uses an o ine plan-then-servo approach that integrates probabilisticroadmaps with visual servoing. A method to construct paths between tworobot con gurations that keep the target object within the camera’s  eldof view is demonstrated, allowing feasible transitions from planned mo-tion to visual servoing. A method to address pose uncertainty to ensurecollision-free, closed-loop motion is statistically tested with multiple posi-tioning tasks. The second strategy uses an online iterative plan-and-servoapproach that dynamically updates its estimate of the collision-free spacewhile visual servoing. Experiments using an uncalibrated eye-in-hand plat-iiAbstractform demonstrate the ability of the visual servoing control law to achieveclosed-loop positioning via collision-free trajectories, even when the objectlocation is highly uncertain.iiiTable of ContentsAbstract . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . iiTable of Contents . . . . . . . . . . . . . . . . . . . . . . . . . . . . ivList of Tables . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . ixList of Figures . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xNomenclature . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . xiiiAcknowledgements . . . . . . . . . . . . . . . . . . . . . . . . . . . xxiv1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 31.1.1 Industrial Automation: Reduced Setup Costs . . . . . 31.1.2 Medical Robotics: Adaptive Trajectories . . . . . . . 41.1.3 Domestic Service Robots: Ease of Programming . . . 61.2 Robot Bin-Picking: An Example . . . . . . . . . . . . . . . . 71.3 Problem Statement . . . . . . . . . . . . . . . . . . . . . . . 81.4 Research Objectives . . . . . . . . . . . . . . . . . . . . . . . 91.5 Thesis Outline . . . . . . . . . . . . . . . . . . . . . . . . . . 102 Literature Review . . . . . . . . . . . . . . . . . . . . . . . . . 132.1 Advances in Image-Based Visual Servoing . . . . . . . . . . . 132.2 Advances in Position-Based Visual Servoing . . . . . . . . . 152.3 Epipolar-Based Visual Servoing . . . . . . . . . . . . . . . . 172.4 Homography-Based Visual Servoing . . . . . . . . . . . . . . 172.5 Image Path Planning . . . . . . . . . . . . . . . . . . . . . . 182.6 Image Trajectory Tracking . . . . . . . . . . . . . . . . . . . 202.7 Constraint Avoidance with Redundant Manipulators . . . . . 202.8 Predictive Control for Visual Servoing . . . . . . . . . . . . . 212.9 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 22ivTable of Contents3 Image Homography . . . . . . . . . . . . . . . . . . . . . . . . 243.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 243.2 Homography-Based Visual Servoing . . . . . . . . . . . . . . 243.2.1 Overview . . . . . . . . . . . . . . . . . . . . . . . . . 243.2.2 Feature Selection . . . . . . . . . . . . . . . . . . . . 253.2.3 Control Scheme . . . . . . . . . . . . . . . . . . . . . 263.2.4 Servoing Requirements from Images . . . . . . . . . . 283.3 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 303.3.1 Purpose and Setup . . . . . . . . . . . . . . . . . . . 303.3.2 Image-Based Visual Servoing . . . . . . . . . . . . . . 303.3.3 21/2-D Visual Servoing . . . . . . . . . . . . . . . . . 323.4 Experimental Results . . . . . . . . . . . . . . . . . . . . . . 353.4.1 Purpose and Setup . . . . . . . . . . . . . . . . . . . 353.4.2 Extracting and Matching Image Features . . . . . . . 363.4.3 Error Assessment . . . . . . . . . . . . . . . . . . . . 373.5 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 414 Robust Pose Estimation . . . . . . . . . . . . . . . . . . . . . . 424.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 424.2 Controller Design . . . . . . . . . . . . . . . . . . . . . . . . 434.2.1 Plant Model . . . . . . . . . . . . . . . . . . . . . . . 434.2.2 Analysis of Controllability . . . . . . . . . . . . . . . 454.2.3 State-Feedback Controller with Adaptive Gains . . . 464.2.4 Camera Field-of-View Constraints . . . . . . . . . . . 474.3 Observer Design . . . . . . . . . . . . . . . . . . . . . . . . . 474.3.1 Plant Model . . . . . . . . . . . . . . . . . . . . . . . 474.3.2 State Di erence Equations . . . . . . . . . . . . . . . 494.3.3 Output Equations . . . . . . . . . . . . . . . . . . . . 504.3.4 Observer Model Summary . . . . . . . . . . . . . . . 514.3.5 Analysis of Observability . . . . . . . . . . . . . . . . 524.3.6 Extended Kalman Pose Estimation . . . . . . . . . . 524.4 Relationship Between Observer & Controller States . . . . . 524.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 534.5.1 Purpose and Setup . . . . . . . . . . . . . . . . . . . 534.5.2 Position-Based Visual Servoing . . . . . . . . . . . . . 544.5.3 Camera Field-of-View Constraints . . . . . . . . . . . 554.5.4 Visual Servoing Near Robot Singularities . . . . . . . 564.5.5 Tracking performance of EFK and PBVS . . . . . . . 584.6 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 59vTable of Contents5 Path Planning for Constraint Avoidance . . . . . . . . . . . 655.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 655.2 Path Planning Requirements . . . . . . . . . . . . . . . . . . 665.3 Pose Calculations and Modeling . . . . . . . . . . . . . . . . 675.4 Inverse Kinematic Solutions for Path Planning . . . . . . . . 695.4.1 Visibility Modelling . . . . . . . . . . . . . . . . . . . 705.4.2 Simulation Results . . . . . . . . . . . . . . . . . . . . 725.5 Probabilistic Roadmaps . . . . . . . . . . . . . . . . . . . . . 725.5.1 Robot Joint-Limit Constraints . . . . . . . . . . . . . 745.5.2 Whole-Arm Collision Constraints . . . . . . . . . . . 755.5.3 Camera Field-of-View Constraints . . . . . . . . . . . 755.6 Dynamic Generation of Visible Robot Con gurations . . . . 775.6.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . 775.6.2 Vertices Generation using Virtual IBVS . . . . . . . . 775.6.3 Vertices Selection using a Joint-Space Metric . . . . . 785.6.4 Addressing Virtual IBVS Failure . . . . . . . . . . . . 795.7 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 805.8 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 826 Model Predictive Control . . . . . . . . . . . . . . . . . . . . . 866.1 Introduction . . . . . . . . . . . . . . . . . . . . . . . . . . . 866.2 MPC Formulation . . . . . . . . . . . . . . . . . . . . . . . . 876.3 Eye-in-Hand Visual Servoing using MPC . . . . . . . . . . . 896.3.1 System Modelling . . . . . . . . . . . . . . . . . . . . 896.3.2 Constraint Modelling . . . . . . . . . . . . . . . . . . 926.3.3 Control Law Design . . . . . . . . . . . . . . . . . . . 936.4 Summary of the MPC Visual Servo Control Law . . . . . . . 946.5 Simulation Results . . . . . . . . . . . . . . . . . . . . . . . . 956.5.1 Setup . . . . . . . . . . . . . . . . . . . . . . . . . . . 956.5.2 Camera Field-of-View Constraints . . . . . . . . . . . 966.5.3 Robot Joint-Limit Constraints . . . . . . . . . . . . . 1006.5.4 Robot Joint-Velocity Constraints . . . . . . . . . . . 1006.5.5 Tuning Parameters . . . . . . . . . . . . . . . . . . . 1036.6 Whole-Arm Collisions Constraints . . . . . . . . . . . . . . . 1056.6.1 Representation of the Collision-Free Space . . . . . . 1056.6.2 Exploiting DCC for Collision Bounds . . . . . . . . . 1066.7 MPC with O ine Path Planning . . . . . . . . . . . . . . . . 1086.7.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . 1086.7.2 Pose Uncertainty Modelling . . . . . . . . . . . . . . 1096.7.3 Bounds on Closed-Loop Motion . . . . . . . . . . . . 110viTable of Contents6.7.4 Simulation Results . . . . . . . . . . . . . . . . . . . . 1116.7.5 Integration with PRM . . . . . . . . . . . . . . . . . 1126.8 MPC with Online Collision-Avoidance . . . . . . . . . . . . . 1166.8.1 Motivation . . . . . . . . . . . . . . . . . . . . . . . . 1166.8.2 Integration with Online DCC . . . . . . . . . . . . . . 1176.9 Experiments: Collision-Free Visual Servoing . . . . . . . . . 1186.9.1 Experiment Setup . . . . . . . . . . . . . . . . . . . . 1186.9.2 Experiment Results . . . . . . . . . . . . . . . . . . . 1246.9.3 Discussion on Stability . . . . . . . . . . . . . . . . . 1256.9.4 Insights from Simulations and Experiments . . . . . . 1276.10 Summary . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1287 Conclusions and Recommendations . . . . . . . . . . . . . . 1307.1 Summary Remarks . . . . . . . . . . . . . . . . . . . . . . . 1307.2 Contributions . . . . . . . . . . . . . . . . . . . . . . . . . . 1317.2.1 MPC Eye-in-Hand Visual Servoing . . . . . . . . . . 1317.2.2 Integration of MPC Visual Servoing with PRM . . . 1317.2.3 Integration of MPC Visual Servoing with DCC . . . . 1317.2.4 Minor Contributions . . . . . . . . . . . . . . . . . . 1327.3 Future Work . . . . . . . . . . . . . . . . . . . . . . . . . . . 1327.3.1 Interruption of Visual Loop . . . . . . . . . . . . . . . 1327.3.2 Stability Analysis of MPC-DCC . . . . . . . . . . . . 1337.3.3 Model Correction Methods . . . . . . . . . . . . . . . 1347.3.4 Further Evaluation Metrics . . . . . . . . . . . . . . . 134Bibliography . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 135AppendicesA Homography Estimation for Non-Planar Objects . . . . . . 142A.1 Virtual Plane De nition . . . . . . . . . . . . . . . . . . . . . 142A.2 Planar vs. Non-Planar Objects . . . . . . . . . . . . . . . . . 142A.3 Estimation using the Virtual Parallax . . . . . . . . . . . . . 143B Homography Decomposition . . . . . . . . . . . . . . . . . . . 146B.1 Geometric Interpretation . . . . . . . . . . . . . . . . . . . . 146B.2 Decomposition Solutions . . . . . . . . . . . . . . . . . . . . 147B.3 Reinforcing Visibility Constraints . . . . . . . . . . . . . . . 150B.4 Solution for Visual Servoing . . . . . . . . . . . . . . . . . . 150viiTable of ContentsC Damped Least-Squares Inverse . . . . . . . . . . . . . . . . . 151D Extended Kalman Filter . . . . . . . . . . . . . . . . . . . . . 153viiiList of Tables6.1 Uncalibrated Camera Parameters . . . . . . . . . . . . . . . . 1206.2 Joint-position limits of the CRS-A465 6-DoF robot . . . . . . 1216.3 Joint-velocity limits of the CRS-A465 6-DoF robot . . . . . . 1216.4 Feature point coordinates of the target object model . . . . . 1226.5 Dimensions of workspace obstacles . . . . . . . . . . . . . . . 1236.6 Location of workspace obstacles . . . . . . . . . . . . . . . . . 123ixList of Figures1.1 Manipulator Visual Servoing Example . . . . . . . . . . . . . 21.2 Medical Robotics Example . . . . . . . . . . . . . . . . . . . . 51.3 Robot Bin-Picking Example . . . . . . . . . . . . . . . . . . . 73.1 IBVS of a task requiring translations and out-of-plane rotations 313.2 IBVS of a task requiring a rotation of 180 degrees about theoptical axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 323.3 21/2-D visual servoing of a task requiring translations andout-of-plane rotations . . . . . . . . . . . . . . . . . . . . . . 333.4 Joint trajectories for 21/2-D visual servoing . . . . . . . . . . 343.5 21/2-D visual servoing of a task requiring a rotation of 180degrees about the optical axis . . . . . . . . . . . . . . . . . . 353.6 Sequence of images used to assess the accuracy of homography-based methods for visual servoing . . . . . . . . . . . . . . . . 373.7 Number of feature matches available for non-planar homog-raphy estimation . . . . . . . . . . . . . . . . . . . . . . . . . 383.8 Rotational accuracy of homography decomposition for visualservoing . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 403.9 Translational accuracy of homography decomposition for vi-sual servoing . . . . . . . . . . . . . . . . . . . . . . . . . . . 404.1 PBVS of a task requiring translations and out-of-plane rotations 554.2 PBVS of a task requiring a rotation of 180 degrees about theoptical axis . . . . . . . . . . . . . . . . . . . . . . . . . . . . 564.3 Adaptive gains with hierarchical control of camera translationand rotation . . . . . . . . . . . . . . . . . . . . . . . . . . . . 574.4 Transient behaviour of the EKF pose estimate . . . . . . . . 604.5 Transient behaviour of pose estimation errors in the EKF . . 614.6 Transient behaviour of the PBVS controller with an EKFobserving a moving target object . . . . . . . . . . . . . . . . 624.7 Pose-following errors in the PBVS controller with an EKFobserving a moving target object . . . . . . . . . . . . . . . . 63xList of Figures4.8 Transient behaviour in the EKF estimate of the target ob-ject’s velocity . . . . . . . . . . . . . . . . . . . . . . . . . . . 645.1 Two-stage approach for planning and servoing . . . . . . . . . 675.2 Homogeneous transformations between coordinate frames usedfor path planning . . . . . . . . . . . . . . . . . . . . . . . . . 685.3 Oriented bounding box used to represent the target objectfor  eld-of-view visibility planning . . . . . . . . . . . . . . . 715.4 Image trajectories resulting from four di erent inverse kine-matic solutions . . . . . . . . . . . . . . . . . . . . . . . . . . 735.5 PRM representation of the robot path planning problem . . . 745.6 Using the IBVS joint-space trajectory to select candidate vis-ible vertices while reducing joint-space motion . . . . . . . . . 815.7 Using a partial IBVS joint-space trajectory to select candi-date visible vertices while reducing joint-space motion . . . . 815.8 Image trajectories resulting from direct joint-space interpolation 825.9 Image trajectories resulting from virtual IBVS . . . . . . . . 835.10 Image trajectories resulting from the insertion of a visible vertex 836.1 The basic principle of model predictive control . . . . . . . . 886.2 A system diagram illustrating the input and output require-ments of the MPC controller . . . . . . . . . . . . . . . . . . 916.3 Overhead image describing the initial position of the eye-in-hand camera with respect to a target object . . . . . . . . . . 976.4 Reference image describing the desired position of the eye-in-hand camera with respect to a target object . . . . . . . . . . 976.5 Image trajectory generated by MPC visual servoing . . . . . 986.6 Camera Cartesian trajectory generated by MPC visual servoing 996.7 Robot joint trajectory generated by MPC visual servoing . . 1016.8 Robot joint velocities generated by MPC visual servoing . . . 1026.9 Joint-space representation of the collision-free space . . . . . 1066.10 Hyperrectangular collision-free spaces obtained from DCC . . 1096.11 Closed-loop positions for joints 1 & 2 resulting from MPCvisual servoing with object pose perturbations . . . . . . . . . 1136.12 Closed-loop positions for joints 3 & 4 resulting from MPCvisual servoing with object pose perturbations . . . . . . . . . 1146.13 Closed-loop positions for joints 5 & 6 resulting from MPCvisual servoing with object pose perturbations . . . . . . . . . 1156.14 Integration of PRM path planning with MPC visual servoing 1166.15 Integration of MPC, DCC and adaptive bisection . . . . . . . 117xiList of Figures6.16 Eye-in-hand platform for MPC visual servoing experiments . 1186.17 Image describing the initial position of the camera and end-e ector . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 1196.18 Reference image describing the desired position of the end-e ector with respect to a target object . . . . . . . . . . . . . 1196.19 Sequence of camera motions generated by MPC visual servoing1256.20 Sequence of robot motions generated by MPC visual servoing 126xiiNomenclatureAcontrol The system matrix of the open-loop plant in PBVSeA The system matrix used in EKF pose estimation, representing thelinearized state di erence equations of the observer model Pitch; the angle of rotation about the y-axis, applied in the orderz-y-x about current framesb Shorthand for the robot base frame,Fb, when used in a homogeneoustransformation, for example, mTbBcontrol The input matrix of the open-loop plant in PBVS The perpendicular skew angle between imaging plane and optical axisC The camera calibration matrixc Shorthand for the camera frame, Fc, when used in a homogeneoustransformation, for example, mTceC The output matrix used in EKF pose estimation, representing thelinearized state output equations matrix of the observer modelCq The robot inertial matrixCAD Computer Aided DesignCCD Charge-Coupled DeviceC The cost function to be minimized in MPCCPU Central Processing UnitCT Computed Tomographyd the signed perpendicular distance of the virtual plane  to the currentcamera frame; (*) ::: in the desired camera framexiiiNomenclatureed(q) The shortest distance between the target object and the camera frus-trum in con guration q; used in the DVC algorithmd(q) The shortest distance between an obstacle and the robot in con gu-ration q; used in the DCC algorithmDadaptive An adaptive gains matrix used to address camera  eld-of-viewconstraintsDq The robot coriolis and centrifugal matrixdk The disturbance (model) correction term at time k in the generalMPC frameworkDCC Dynamic Collision Checking t Time-step; the sampling period for a discrete-time controller or ob-server q The joint position perturbation vector resulting from closed-loop mo-tion; describes the di erence between the estimated open-loop jointposition and the actual closed-loop joint position maxq The upper bound vector on perturbations in joint positions resultingfrom closed-loop motion minq The lower bound vector on perturbations in joint positions resultingfrom closed-loop motion xyz   An upper bound on the magnitude of the object pose perturbationvectorDistanceKey A SIFT parameter describing the image distance between thekey feature and the neighbourDistanceMatch A SIFT parameter describing the image distance betweenthe key’s matched feature and its neighbourDoF Degree(s) of FreedomDVC Dynamic Visibility Checkinge Shorthand for the robot end-e ector frame, Fe, when used in a ho-mogeneous transformation, for example, mTexivNomenclaturee21/2-D The error vector for 21/2-D visual servoingeIBVS The error vector for IBVSePBVS The error vector for PBVSEKF Extended Kalman Filter ijk The errors at time i predicted at time k in the general MPC frame-work Zero-mean Gaussian noise vector in the nonlinear output equationsof the observer modelF(x;u) The nonlinear state di erence equations of the observer model usedfor pose estimationf The focal length of lensFb The robot base frameFc The camera frameFcontrol The state feedback matrix of the closed-loop plant in PBVSFc The desired camera frameFe The robot end-e ector frameFo The object framefrobot(q) The forward kinematic model of the robot in terms of joint anglesG(x) The nonlinear output equations of the observer model used for poseestimationgq The robot gravity vector Zero-mean Gaussian disturbance vector in the nonlinear state di er-ence equations of the observer model, consisting of 1 and 2 GPC Generalized Predictive ControlH A homography matrix associated with points on plane  , describingthe transformation of image coordinates in the desired image to imagecoordinates in the current imagexvNomenclatureHj The set of 3-D points representing the hull of link j of the robotmanipulator; used for collision testsIBVS Image-Based Visual ServoingJrobot The Jacobian relating robot joint velocities to camera velocities ex-pressed in the camera frameJ   The Jacobian relating the linear and angular velocities of the objectexpressed in the camera frame to the rate of change of X-Y-Z androll-pitch-yaw angles expressed in the camera frameeK The Kalman gain matrix used in EKF pose estimationku The horizontal scale of imaging array in number of pixels per unitlengthkv The vertical scale of imaging array in number of pixels per unit length A damping factor, used in the damped least-squares inverse‘(q(t)) The longest distance traveled by any point on the robot during tra-jectory q(t); used in the DCC algorithme‘(q(t)) The longest distance traveled by any point on the target object withrespect to the camera’s frame of reference during trajectory q(t); usedin the DVC algorithmL21/2-D the interaction matrix for 21/2-D visual servoing relating the cameravelocity to the rate of change of the 21/2-D visual servoing error vectorLIBVS The interaction matrix for IBVS relating the camera velocity to therate of change of the IBVS error vectorLPBVS The interaction matrix for PBVS relating the camera velocity to therate of change of the PBVS error vector The convergence rate of a visual servoing control lawLQG Linear Quadratic GaussianLQR Linear Quadratic Regulatormj The normalized metric image coordinates of feature point j expressedin the current camera frame in homogeneous form; (*) ::: in thedesired camera framexviNomenclatureMIMO Multiple-Input Multiple-OutputMIS Minimally Invasive SurgeriesMPC Model Predictive ControlMRI Magnetic Resonance Imagingn The normal of plane,  , expressed in the current camera frame; (*)::: in the desired camera frameNc The control horizon in the general MPC frameworkNp The prediction horizon in the general MPC frameworko Shorthand for the object frame, Fo, when used in a homogeneoustransformation, for example, mToOriKey A SIFT parameter describing the orientation at which the key fea-ture is detectedOriKeyMatch A SIFT parameter describing the orientation at which thekey’s matched feature is detectedOriNeighbor A SIFT parameter describing the orientation at which the neigh-bour feature is detectedOriNeighborMatch A SIFT parameter describing the orientation at whichtheneighbour’s matched feature is detected(pj)ijk The image coordinates (u,v) of feature j at time i predicted at timek in the MPC visual servoing frameworkpj The image coordinates of feature point j in homogeneous formeP The state covariance matrix obtained from EKF pose estimation,representing the covariance of the estimated statesmP An object model consisting of the 3-D coordinates of all feature pointsexpressed in frame m,mPj The 3-D coordinates of feature point j expressed in frame m in ho-mogeneous formPBVS Position-Based Visual ServoingxviiNomenclaturePCI Periheral Component Interconnect Bus Roll; the angle of rotation about the z-axis, applied in the order z-y-xabout current frames A plane de ned by three 3-D pointsPID Proportional Intergal Derivativep A vector of the image coordinates of all feature points in the MPCvisual servoing frameworkpd A vector of the desired image coordinates of all features in the MPCvisual servoing frameworkpijk A vector of the image coordinates of all features at time i predictedat time k in the MPC visual servoing frameworkpk A vector of the observed image coordinates of all features at time kin the MPC visual servoing frameworkPRM Probabilistic Roadmap Yaw; the angle of rotation about the x-axis, applied in the order z-y-xabout current framesQ A weighting matrix whose elements determine the weight given to theerrors associated with each image feature in the MPC visual servoingcontrol lawq The robot joint position vectorqmaxDCC The upper bound vector on the hyperrectangular collision-free spacereturned from DCCqminDCC The lower bound vector on the hyperrectangular collision-free spacereturned from DCCqmaxDCCk The upper bound vector on the dynamically updated hyperrectan-gular collision-free space returned from DCC at time kqminDCCk The lower bound vector on the dynamically updated hyperrectan-gular collision-free space returned from DCC at time kQdesired The set of all robot joint con gurations that satisfy the desired end-e ector position; Set of all inverse kinematic solutions to the robotxviiiNomenclatureqdesired The desired robot joint con guration; the goal con guration in pathplanningQfree The collision-free space, representing the set of all robot con gura-tions that do not result in physical collisions with workspace obstaclesqIBVS( ) The joint space path, parameterized by  , resulting from IBVSqijk The robot joint positions at time i predicted at time k in the MPCvisual servoing frameworkqinitial The initial robot joint con guration; the start con guration in pathplanningeQ The disturbance covariance matrix used in EKF pose estimation, rep-resenting of the covariance of the disturbances in the state di erenceequations_q The robot joint velocity vector_qijk The robot joint velocities at time i predicted at time k in the MPCvisual servoing framework q The robot joint acceleration vector qj The change in position of joint j between two robot con gurations_qmax An upper bound vector on feasible robot joint velocities_qmin A lower bound vector on feasible robot joint velocities^q The estimated open-loop joint position, resulting from MPC pathplanningqmax An upper bound vector on feasible robot joint positionsqmin A lower bound vector on feasible robot joint positionsQP Quadratic Programmingr The unit vector de ning the axis of rotation of mRn in axis-anglerepresentationrj The greatest distance between the line de ned by the axis of jointrotation zj and a point P; a \moment arm"xixNomenclatureeR The noise covariance matrix used in EKF pose estimation, represent-ing the covariance of the noise in the output equationsmRn A rotation matrix expressing the coordinates of frame n in the coor-dinates of frame m 1 A parameter used in the 21/2-D visual servoing control law, de nedas Zd ROI Region of InterestRTX Real Time Extensions A feature vector selected to de ne the error vector to be minimizedin visual servoingScaleKey A SIFT parameter describing the image scale at which the keyfeature is detectedScaleKeyMatch A SIFT parameter describing the image scale at which thekey’s matched feature is detectedScaleNeighbor A SIFT parameter describing the image scale at which theneighbour feature is detectedScaleNeighborMatch A SIFT parameter describing the image scale at whichthe neighbour’s matched feature is detectedSIFT Scale-Invariant Feature Transform i The ith singular value of a matrix, resulting from SVD xyz   The standard deviation vector corresponding to the object poseperturbation vectorSQP Sequential Quadratic ProgrammingSVD Singular Value DecompositionmTn A homogeneous transformation expressing the coordinates of framen in the coordinates of frame mmtn A translation matrix expressing the coordinates of frame n in thecoordinates of frame mTRx( ) A homogeneous transformation expressing a rotation of  about xxxNomenclatureTRy( ) A homogeneous transformation expressing a rotation of  about yTRz( ) A homogeneous transformation expressing a rotation of  about zTxyz(x;y;z) A homogeneous transformation expressing a translation of x,y, z The robot joint torque vector The angle of rotation about r of mRn in axis-angle representation error The rotation estimation error de ned as the angle of rotation ofcRc cbR 1c in axis-angle representationU The set of possible inputs in the general MPC frameworku A vector of the horizontal image coordinates of all feature pointsu0 The horizontal image coordinate of the principal pointuijk The system input at time i predicted at time k in the general MPCframeworkuj The horizontal image coordinate of feature point juobsv The inputs to the observer model used for pose estimationumax The maximum pixel coordinate of the camera CCD array in the hor-izontal directionumin The mininum pixel coordinate of the camera CCD array in the hor-izontal directionv A vector of the vertical image coordinates of all feature pointsv0 The vertical image coordinate of the principal pointvj The vertical image coordinate of feature point j" The size of the singular region for singularity avoidance; used in thedamped least-squares inverse to determine the damping factorvc The spatial velocity of the camera with respect to the global framevo The spatial velocity of the target object with respect to the globalframe, tracked as states in the pose observerxxiNomenclatureW A weighting matrix whose elements determine the weight given tothe velocities associated with each robot joint in the MPC visualservoing control lawc!o The pose vector of the target object expressed in the current cameraframe, tracked as states in the pose observerX The set of possible states in the general MPC frameworkxijk The system state at time i predicted at time k in the MPC frameworkxj The normalized metric image coordinates of feature point j in thehorizontal direction, expressed in the current camera frame; (*) :::in the desired camera framemXj The x-coodinate of the origin of frame j (or feature point j) expressedin frame mxobsv The states of the observer model used for pose estimationX The 3-D coordinates of a feature point expressed in the current cam-era frame; (*) ::: in the desired camera frameyd The desired system output in the general MPC frameworky(xijk) The system output at time i predicted at time k in the general MPCframeworkyj the normalized metric image coordinates of feature point j in thevertical direction, expressed in the current camera frame; (*) ::: inthe desired camera framemYj The y-coodinate of the origin of frame j (or feature point j) expressedin frame mzj The axis of rotation of revolute joint jZj The \depth" of feature point j; the Z-coodinate of feature point jexpressed in the current camera frame; (*) ::: in the desired cameraframemZj The z-coodinate of the origin of frame j (or feature point j) expressedin frame mzobsv The outputs of the observer model used for pose estimationxxiiNomenclature A path variable where  = 0 corresponds to the beginning of thepath and  = 1 corresponds to the end of the pathxxiiiAcknowledgementsI would like to thank my supervisors, Dr. Elizabeth A. Croft and Dr. JamesJ. Little, for their guidance and support throughout the thesis. I am verygrateful for all the useful advice and learning opportunities that have re-sulted from their patient mentoring.I would also like to thank Dr. Ryozo Nagamune and Dr. Bob Woodhamfor their guidance and suggestions on the thesis.I would like to thank Dr. Simon Leonard for the many hours of fruitfuldiscussions on visual servoing, and for help on setting up the experiments.To my colleagues in the CARIS lab, thank you for your friendship andsupport, and for giving me a healthy perspective on life, when things get alittle hectic.A special thanks goes to my parents for their love, support and encour-agement, and for always giving me the freedom to pursue my interests. Tomy sister, thank you for being there for the family, while I am temporarilyaway for my studies.Finally, I would like to thank Theresa for her love, support, encourage-ment. The many hours spent on editing my thesis, listening to my presenta-tions, and patiently waiting for me to come home on late nights, are greatlyappreciated.To the faculty and sta of the Mechanical Engineering Department,thank you for your assistance and support throughout my learning expe-rience here at UBC. I would also like to acknowledge the  nancial supportof the Natural Sciences and Engineering Research Council of Canada.xxivChapter 1IntroductionThe ability to teach real robots how to carry out physical positioning taskssafely and accurately, using a set of natural human commands, has been alife-long goal of many robotic system designers. Vision is naturally used byhumans for demonstrating to each other spatial locations and multi-bodymotions. As a natural extension of this teach-by-showing paradigm, a humanuser can present a robot with a visual description of a positioning task, whichcan be readily captured using an imaging sensor. The approach removes theneed for tedious and cumbersome numerical robot programming, so thatcomplex tasks can be easily taught to robots by non-technical users.Often, a robot manipulator is required to position its end-e ector withrespect to a target object for grasping. A digital camera can be used by theuser to teach the required task to the robot. Given that this task requiresrelative positioning, it is often advantageous to mount the camera near theend-e ector in an eye-in-hand con guration, to provide a close-up, non-occluded view of the target object for feedback purposes. The user initiatesthe visual command by showing the robot a reference image of the desiredgrasping position. This image describes what the camera should see whenthe end-e ector is correctly positioned relative to the target object. Therobot then compares the current image against the reference image, andgenerates a trajectory that e ciently reduces the di erence between the twoimages at each time step. The task is complete when the observed image isin agreement with the reference image, resulting in the correct positioningof the end-e ector relative to the target object. This method of end-e ectorposition control using feedback from an arm-mounted camera is known aseye-in-hand visual servoing. An example of a positioning task achieved viaeye-in-hand visual servoing is shown in Figure 1.1.This teach-by-showing method is not only intuitive to human users, butalso o ers several technical advantages over traditional, position-based robotprogramming. Vision-based feedback allows the generalization of manipu-lator programming to accommodate frequent changes in the target object’slocation. Exact knowledge of the object’s location and precise camera-robotcalibrations are not required, due to the closed-loop control structure. Ex-1Chapter 1. IntroductionMECH 598 Seminar“Teach by Showing”: Generate Robot Motion from ImagesgJointCommandsCurrent End-Effector PositionDesired End-Effector PositionVisual Servo CtllJoint Commands (q1…qn)ControllerImage Errors (e1…en)fCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate3Current Observed ImageDesired Reference ImageFigure 1.1: Visual servoing uses the image di erences observed by the cam-era as feedback to drive the end-e ector towards a desired position relativeto the target object.pressing the Cartesian positioning task using an image-based metric e ec-tively cancels out calibration errors in the intermediate reference frames.The position of the end-e ector relative to the target object is guaranteedto be accurate, provided the currently observed image corresponds to thereference image.Despite a large body of existing work in this area [1], a fundamental gapstill exists between the current capability of visual servoed robotic systemsand the requirements of real world robotic applications. Beyond its use forcorrecting small planar motions, there has been limited commercial adoptionof visual servoing for manipulator control, primarily due to concerns aroundreliability, safety, and stability. Most research to-date in visual servoinghas focused on the control aspect from the perspective of a free- oatingcamera, paying little or no attention to the physical robot that is usedto drive camera motion. However, robots have mechanical joint limits and nite performance capabilities which restrict where a camera mounted to therobot can go and how fast it can move. Cameras themselves have limited elds and depths of view. During servoing, the image-based control lawcan become unstable if the camera loses sight of the target object, or ifthe required trajectory exceeds the robot’s physical limits. Furthermore,the image-based control law could result in dangerous whole-arm collisions,if the system is unaware of the physical relationships between the robot2Chapter 1. Introductionmanipulator and its environment. These issues must be addressed beforevisual servoing can be reliably used in applications that demand a full rangeof robot motions.This work aims to address the aforementioned practical issues which im-pede the application of manipulator visual servoing in real life applications.For non-trivial robot motions, the stability of a visually servoed system de-pends upon a prudent awareness of the robot’s sensing, control, and physicallimits. Successful design of a visual servoing control law, that can e ectivelymanage these constraints while carrying out its positioning task, will lead toa versatile robotic system that can execute large-range, safe motions whileadapting to observed changes in the target object’s pose.1.1 MotivationAs described above, manipulator visual servoing provides a powerful methodto compensate for uncertainties that exist in robotic systems and their envi-ronments. In an ideal world where system parameters are precisely knownand there are no expectations of disturbances, one can simply use an open-loop approach without the need to servo. However, many engineering sys-tems cannot be simply realized with this approach. In manufacturing appli-cations, costs tend to increase exponentially with increases in required open-loop accuracy, stemming from the need to reduce tolerances recursively ateach design layer. In surgical applications involving complex biological sys-tems, it may not be possible to obtain an accurate open-loop model. Finally,for human-in-the-loop applications, it is impractical to design a system thatrelies upon precise numeric inputs from human users. For such systems,visual servoing provides a natural way for humans to functionally communi-cate with machines without demanding meticulous precision. Some speci cexamples that illustrate the need for reliable visual servoing approaches areprovided in this section.1.1.1 Industrial Automation: Reduced Setup CostsIndustrial robots perform a variety of automated tasks such as componentassembly, pick-and-place, inspection, cutting, painting, welding, and liftingof heavy loads. All of these tasks require the robot end-e ector to be accu-rately positioned with respect to a target object prior to the execution ofthe task. Traditionally, manufacturers have employed record-and-playbackprogramming methods, which rely on mechanical  xtures, positioning de-vices and human labour to present objects to the robot’s pre-programmed3Chapter 1. Introductioninterfacing locations. However, the use of structured work-cells demandsubstantially higher costs stemming from engineering development, capitalinvestment, and system maintenance. For the manufacture of goods withhigh volumes and long lifecycles, these costs are still justi ed. However,the growing demand for mass-customization of durable and non-durableconsumer goods poses a major challenge to this approach. Where produc-tion volume is sensitive to changing customer demands, manufacturers mustmove towards  exible and adaptive production systems that can be quicklyrecon gured to produce a wide array of products using the same infrastruc-ture. Reduction in equipment costs and work-cell setup time are the mainadvantages for using visual servoing in manufacturing environments.Manipulator visual servoing provides a low-cost, highly adaptive, andcalibration-free solution to the relative end-e ector positioning problem.Commercial o -the-shelf cameras are available at a fraction of the cost of arobot, while providing su ciently rich sensory data for the robot to makedecisions for intelligent control. Using software to detect and correct forpositional errors allows the system to be quickly adapted to suit a variety ofdi erent robots, cameras and industrial parts without extensive retooling.The use of direct image-based feedback for trajectory correction makes therobot’s positional accuracy robust to calibration errors.Unfortunately, the reliability of manipulator visual servoing methodsmust be proven before manufacturers will consider adopting them for in-dustrial use. Manufacturers do not have the luxury of employing solutionsthat have appreciable chances of failure, since frequent downtimes can in-terrupt work ow and negate the productivity that is gained by automation.A necessary requirement is that the chance of visual servo failure must re-main near zero. One frequently voiced concern is that visual servoing inuncertain conditions may generate unsafe robot motions resulting in costlydamage to equipment. The visual servo control law must be made awareof its position, velocity, and acceleration limits and provide a guarantee forsafe, collision-free robot motions. The system must be designed such thatif visual input is interrupted due to unforeseen conditions, the position taskcan continue to operate in a stable manner until visual input is restored.1.1.2 Medical Robotics: Adaptive TrajectoriesThe ability to generate robot trajectories that can adapt to patient move-ment is one of the main motivations for using manipulator visual servoing inmedical applications. Advanced robotic surgical systems, such the IntuitiveSurgical da Vinci c shown in Figure 1.2, assist surgeons in performing min-4Chapter 1. IntroductionMECH 598 SeminarCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate31Figure 1.2: The da Vinci c surgical system uses robotic arms to controlthe position of cameras and surgical tools to assist surgeons in performingminimally invasive procedures [2].imally invasive surgeries (MIS) by using its robotic manipulators to controlcameras and laparoscopic tools. The use of a master-slave robotic systemeliminates the awkward inverted manipulation of laparoscopic instrumentsthat is required of the surgeon. However, in MIS, the surgeon cannot di-rectly see the surgical  eld and must rely on scans of the patient’s anatom-ical structures to plan the surgery. Magnetic resonance imaging (MRI) andcomputed tomography (CT) are generally used to provide high resolutionimages of the surgical site prior to the operation. During the surgery, how-ever, any previously identi ed structures may have shifted in location dueto the patient’s movements. Real-time imaging devices, such as ultrasoundand endoscopic cameras, are used to provide visual feedback to the surgeonsto correct for such deviations.Throughout the surgery, the position of these imaging devices must beactively managed in order to provide useful information to the surgeon. Theimaging planes of ultrasound transducers have  nite e ective thicknesses andcameras have limited  elds of view. The position of these sensors must befrequently adjusted to compensate for patient motion which may bring thesurgical  eld outside of the sensor’s e ective limits. Visual servo controlof these sensors can free the surgeon from tedious adjustments, so thatattention can be redirected towards the surgical task. When a new sensorviewpoint is required, the surgeon needs only to move the sensor in the5Chapter 1. Introductioncorrect position once to obtain a reference image. The sensor maintains thisviewpoint through visual servoing, providing the surgeon with a consistentframe of reference, despite patient motions.The proposed visual servoing system for sensor position control must besafe and reliable. In medical applications, any chance of visual servo failureis simply not acceptable because of the inherent dangers to the patient. Atthe most basic level, the manipulator and sensor should be able to maintaina safe distance from its de ned limits during servoing. In the case of anin-vivo sensor, the movement of the sensor should not cause further damageto the surgical site. The ideal servo system should incorporate informationfrom high- delity models built from o ine CT or MRI images to constrainthe robot’s movement from operating in unsafe regions.1.1.3 Domestic Service Robots: Ease of ProgrammingThe ease of robot programming and re-programming is one of the mainadvantages of using visual servoing for domestic robot applications. Do-mestic robots must be designed with usability in mind. Unlike industrialrobots, domestic robots do not have the luxury of precise calibration, northe dedicated support of engineering teams when robot re-programming isrequired. Without resorting to low-level numerical programming, the usermust be able to e ciently re-program the robot when new interaction sce-narios arise. The robot may need to interact with objects that it has notencountered before. For example, the user may want to command the robotto push a light switch, turn a door knob, or pick up a household object, theprecise location of which is unknown. The robot must be able to achievesu cient positional accuracy in the absence of precise camera-robot calibra-tions and without exact a priori knowledge of the target object’s location.If the robot has visual servoing capabilities, the user can train the robotusing a reference image. The user obtains this image  rst, by showing therobot what the  nished task looks like. This image captures the desiredrelative position between the robot and the object of interest. Coupled withvisual servoing, this teach-by-showing method allows the robot to generalizeits programming to accommodate for changes in the target object’s location,permitting task generalization. As in the case with manufacturing and med-ical surgery, the visually servoed manipulator must be aware of its physical,sensing, and control limits in order to ensure safe operation.6Chapter 1. IntroductionFigure 1.3: Robot bin-picking requires the accurate positioning of the robotend-e ector with respect to randomly located parts, without causing colli-sion with workspace objects, such as the container shown. Image courtesyof Braintech Inc. [3].1.2 Robot Bin-Picking: An ExampleThis thesis will return frequently to the following example of robot bin-picking to provide a useful context for manipulator visual servo control.Consider the common industrial task that is required when a container ofun nished parts is presented to a robot for further handling and processing.This task is typically referred to as robot bin-picking. Bins and containersare widely used to store, protect and transport products ranging from au-tomotive parts to agricultural produce. Manufacturers spend considerablee ort and energy on the loading and unloading of parts into and out of binsand constainers, so automation of this repetitive task using programmablerobotic manipulators is highly desirable. An example of a robot bin-pickingprototype is shown in Figure 1.3.Three di erent robot bin-picking implementations are presented below7Chapter 1. Introduction(in the order of increasing use of vision as feedback) to compare the ad-vantages and disadvantages of visual servoing with other commonly usedapproaches. These illustrative examples are representative of the contin-uum of robotic automation in bin-picking applications.The  rst implementation uses a record-and-playback approach, where therobot blindly executes a set of pre-programmed bin-picking motions. Thisrequires prior organization of parts, using pallets or carriages to structurethe parts into rows and columns inside the container. Mechanical  xturesmust be designed for each part variant; as a result, robotic automationcosts are very high. Such automation is limited to long-term, high-volumeproduction lines with limited part changes. Robot manipulation will fail ifthe part-to-part positional tolerances are too large.The second implementation uses a look-then-move approach, where afully calibrated camera and a precise CAD model of the part are used toestimate the part’s pose (position and orientation) from a single sensedimage. In this implementation, a fully calibrated robot is commanded tomove its end-e ector into a calculated con guration to pick up the part. Therobot’s workspace must be free of obstacles, in order to guarantee collision-free motion. Robot manipulation will fail if there are signi cant calibrationerrors in the camera or in the robot (resulting in errors in pose estimationor in robot position control), or if the part has shifted in location since thelast image was taken.The third implementation uses a visual servoing approach to furthereliminate the dependency on precise camera and robot calibration. Exacto ine computations of the part’s pose and the robot’s desired con gurationare not required. Rather, the robot uses online visual feedback to contin-uously correct its trajectory as it approaches the part. The robot has fullcontrol of its trajectory, though it must not exceed the sensing, control, andphysical collision limits of the system in order to provide safe robot motion.This approach ensures that the manipulator achieves a positional accuracythat is robust to modelling errors and disturbances.1.3 Problem StatementThe goal of this research is to enable the use of manipulator visual servoingfor teaching practical robot motions. While traditional visual servoing meth-ods perform well for correcting small planar deviations, they cannot be usedfor teaching general positioning tasks because, by the design of their controllaw, they are inherently unaware of the sensing, control, and physical limits8Chapter 1. Introductionof the robotic-camera system. Naive application of traditional visual ser-voing methods to large robot motions could result in dangerously unstabletrajectories, due to: (i) the target object leaving the sensor’s  eld-of-view;(ii) the robot being commanded to move beyond its mechanical limits, orbeyond it performance limits; or (iii) the robot arm physically colliding intoother objects within its workspace. This work will address all three of theabove problems that are typically encountered when using visual servoingto drive non-trivial robot motions.1.4 Research ObjectivesThe objectives of this work are to design a visual servo control law that:1. Keeps the target object within the camera’s  eld-of-view at all timesduring servoing.2. Generates realistic camera-robot motions that remain within the robot’sdexterous workspace and mechanical limits.3. Generates time-e cient, but safe robot trajectories. These trajectoriesshould not exceed the robot’s velocity limits, but should allow the useof high feedback gains while servoing.4. Avoids manipulator singularities, which a ect the controllability of therobot’s end-e ector position.5. Generates collision-free robot motion, given some knowledge of therobot’s environment.The above objectives are integrated, where possible, in a uni ed frame-work. Further, the research aims to provide insight into the following ques-tions: For an eye-in-hand robot, how should the trajectory be speci ed toprovide a good balance between an ideal robot trajectory (joint coor-dinates follow a straight line) and an ideal image trajectory (featuresmove in straight lines on the image plane), while ensuring feasibilityand task convergence? How does one design a robotic system that incorporates both model-based robot path planning methods (which are computationally e -cient, but calibration-dependent) and reactive visual servoing methods9Chapter 1. Introduction(which are robust to calibration errors, but may get stuck in local min-ima)? Should planning and servoing take place simultaneously, or in sepa-rate stages? If separate stages are required, then when should thepositioning task be handed o from one stage to the next? When using vision-based path planning methods, how does one ensurethat the target object stays within the camera’s  eld of view, whenthe target object’s location is uncertain?1.5 Thesis OutlineThe thesis is organized as follows:This chapter (Chapter 1) outlines the main research questions and givesconcrete examples to motivate why the constraints of an eye-in-hand sytemmust be addressed by the visual servo control law, when it is used to drivelarge robot motions.Chapter 2 contains a review of the state of the art in constraint-awarevisual servoing, and in other related work that has in uenced the contentsof this thesis.The core of the thesis is divided into four main chapters. Chapters 3and 4 contain minor contributions to improvements in visual servoing. Inaddition, these chapters contain methodological work (on the partial andcomplete pose estimation a target object from eye-in-hand images) that isthe basis of the joint-space path planning and visual servoing methods pre-sented in Chapters 5 and 6. Chapters 5 and 6 contain the main contributionsof the thesis, as outlined in Section 7.2. Experiment and simulation resultsare presented within each individual chapter after the appropriate discus-sions on methods. The core chapters of this thesis contain work in each ofthe following areas: Chapter 3 discusses the use of the image homography to improve thepredictability of visual servoing motion. Scaled Euclidean parametersare recovered using feature point correspondences from images only,and are used in visual servoing to provide a decoupled control law.A method of estimating a homography when observing planar andnon-planar objects is discussed. A method of decomposing the ho-mography into its constituent parameters, and of selecting the correctsolution amongst many that are possible, is also discussed. The per-formance of the proposed hybrid visual servo controller is evaluated in10Chapter 1. Introductionsimulation against the image-based visual servoing (IBVS) approach.Experiments are performed using images obtained from an eye-in-handcamera, to evaluate the accuracy of homography estimation and ho-mography decomposition. Chapter 4 discusses the use of an extended Kalman  lter (EKF) witha model of the target object, to improve the robustness of pose estima-tion with respect to image noise and disturbances. Position-based vi-sual servoing (PBVS) is implemented with an adaptive gains controllerto manage the camera’s  eld-of-view limits. A method of generatingacceptable robot trajectories while visual servoing near joint singu-larities is described. The performance of the adaptive gains PBVScontroller working in conjunction with the EKF is evaluated in thepresence of severe image noise and model disturbances. Chapter 5 discusses use of path planning as a prerequisite to visualservoing for managing robot-related constraints. A method for evalu-ating inverse kinematic solutions that considers the trajectory of theeye-in-hand camera is presented. An o ine plan-then-servo collision-avoidance method is discussed, integrating probabilistic roadmaps (PRM)with visual servoing. A method to construct paths between two robotcon gurations that keep the target object within the camera’s  eld ofview is demonstrated to allow feasible transitions from planned motionto visual servoing. The performance of this path planning method isevaluated through simulations. A dynamic collision checking (DCC)algorithm and a dynamic visibility checking (DVC) algorithm is pre-sented for ensuring that system constraints are satis ed during themotion between two robot con gurations. Chapter 6 presents a unifying framework for constraint-aware visualservo control. An image-based visual servo controller is proposed us-ing the model predictive control (MPC) framework, to allow planningand servoing to be executed simultaneously. Simulation results are pre-sented to demonstrate the ability of the MPC visual servoing controllerto manage eye-in-hand constraints while completing large-motion po-sitioning tasks in closed-loop. A discussion on the tuning parametersin the visual servoing controller is given. A method to address poseuncertainty to ensure collision-free, closed-loop motion is statisticallytested with multiple positioning tasks. An online iterative plan-and-servo collision-avoidance method is implemented using dynamicallyupdated estimates of the collision-free space. Experimental results are11Chapter 1. Introductionshown on a CRS-A465 robot with a Sony XC-HR70 camera perform-ing an end-e ector positioning task without calibration, demonstratingcollision-free robot motions in the presence of pose uncertainty.Chapter 7 presents the conclusions of this research. It summarizes themain contributions of the thesis and provides recommendations for futurework in this area.12Chapter 2Literature ReviewThis chapter provides a summary of related work done by other researchersto improve the performance characteristics of visually servoed systems. Vi-sual servo control of robotic manipulators has been an active area of researchfor over 30 years and it is a topic that spans many disciplines. A good in-troduction for visual servo control can be found in [4], while a more recentsurvey paper covering many classes of visual servoing methods can be foundin [1].This literature review will focus on visual servoing methods that aremost relevant to the goals of this research; that is, methods that are capableof handling large-range robot motions without violating system constraints.In this context, the review commences with a description of the major ad-vances in the two main domains of visual servoing, namely image-based andposition-based servoing. Next, two visual servoing methods that exploit theknown camera structure to improve servo motion are discussed: epipolar-based servoing and homography-based servoing. Then, image path planningand image trajectory tracking methods are presented as extensions to clas-sical robot control for minimizing system constraint violation. The chapterends with a discussion of explicit constraint-avoidance in visual servoing forredundant manipulators and predictive control.2.1 Advances in Image-Based Visual ServoingImage-based visual servoing (IBVS) uses direct image measurements as feed-back to control the motion of the robot. The robot’s positioning task isexpressed as an image-based error function to be minimized using a suitablecontrol law. Because IBVS does not explicitly solve for the Cartesian poseof the target object, its performance does not depend on the accuracy of apriori models. However, since the domain of the control law is in image-space, there is no direct control over the Cartesian or joint-space trajectoryof the robot end-e ector. Application of IBVS to control large-range robotmotions typically results in trajectories that are desirable in image space,but are convoluted in the robot’s joint-space and may result in physical13Chapter 2. Literature Reviewtrajectories exceeding the workspace of the robot. To address this prob-lem, researchers have designed some image features and error functions thatare tailored speci cally for certain classes of positioning tasks and targetobjects, which are described below.Classical IBVS uses the displacement between corresponding image fea-ture points expressed in planar Cartesian coordinates to de ne the errorfunction. Chaumette [5] describes the camera retreat problem encounteredwhen classical IBVS is used to control a positioning task that requires arotation about the camera’s optical axis (denoted as the z-axis in the framecentered at and moving with the camera). Rather than commanding cam-era rotation while holding the camera position constant, the control lawforces the camera to translate backward and forward along its optical axiswhile completing the rotation. Not only is such motion not time-optimal, itgreatly increases the potential for robot collision, since this type of motionis neither intended nor expected by the user. In the worst case when therotation required is exactly 180 degrees about the optical axis, the cameraretreats to a distance of in nity and the servoing never reaches its goal.Iwatsuki et al. [6] propose the use of cylindrical coordinates in the de ni-tion of the image error function to avoid camera retreat. Unfortunately, thismethod forces the camera to rotate even for pure translational tasks, result-ing in a problem similar to the original camera retreat, but for translationalmotions. An improvement to the cylindrical coordinate method is presentedin [7] that allows the position of the origin to shift. However, determiningthe correct shift parameters requires an estimation of the rotational motionfrom the di erences between the initial and the desired image, resulting ina partial pose estimation problem.If the target object is known to be planar (and the object and cameraplanes are parallel at the desired position), then several other image fea-tures can be chosen for IBVS to achieve decoupled control. Corke et al. [8]introduce a partitioned approach that decouples the control of z-axis rota-tional and translational components from the remaining degrees of freedom.Z-axis translation is controlled by the square root of the area of the regularpolygon whose vertices are the image feature points, while z-axis rotation iscontrolled by the angle subtended by the line segment connecting the twochosen feature points that are furthest apart. The use of image momentsas visual features is proposed in [9] to avoid the problems of singularitiesand local minima. Good decoupling and stability properties are obtainedthrough careful selection of combinations of moments to control the six de-grees of freedom of the camera, although moments of higher orders tend tobe more sensitive to image noise.14Chapter 2. Literature ReviewCervera et al.[10] introduce the use of three-dimensional features in IBVSvia a stereo camera mounted on the robot end-e ector. A linear control lawis obtained, resulting in a motion of the object along a straight path inthe camera frame. For target objects that are symmetric (e.g., tetrahedron,square, cube), an exact computation of the velocity screw for any rotationcan be obtained. In this formulation, the resulting motion of the camera isthe same regardless of the 3-D model of the object.2.2 Advances in Position-Based Visual ServoingPosition-based visual servoing (PBVS) uses an estimation of the pose ofthe target object with respect to the camera as feedback to control themotion of the robot. The robot positioning task is expressed as an errorfunction composed of pose parameters. Computing that pose from a set ofmeasurements in one image requires full knowledge of the camera intrinsicparameters and a 3-D model of the target object. This is closely related tothe 3-D localization problem in classical computer vision and many solutionshave been presented in the literature [11] [12]. PBVS allows the decoupledcontrol of translational and rotational motions. The resulting camera tra-jectory is a straight line in Cartesian space, while the image trajectories areless satisfactory than those in IBVS. The domain of the control law is inCartesian-space, so there is no direct control over the trajectory of featurepoints on the image plane.From the perspective of practical robot servoing, one of the main draw-backs of PBVS is that the target object (and its image features) may par-tially or entirely exit the camera’s  eld of view during the servoed motion.Servoing can still continue when the feature losses are partial and whereredundant features are still available for pose estimation. Wilson et al. [13]suggest the detection of feature losses to avoid errors in pose estimationand appropriate modi cation to the extended Kalman  lter for tracking.The lost feature’s entry in the covariance matrix is increased to a very largevalue, which has the e ect of eliminating its in uence on the pose estimate.The entry is returned to its normal value when the feature is detected tohave returned back into the  eld of view. Lippiello et al. [14] [15] suggestthe selection of an optimally-de ned subset of image features for PBVS thatis updated online to minimize the loss of image features. Several qualityindices are proposed to take into account the spatial distribution, angulardistribution, visibility and non-coplanarity of the image features, and theproduct of these indices is used as the basis of selection. However, both15Chapter 2. Literature Reviewof these approaches are passive and they do not prevent the target objectfrom leaving the  eld of view entirely, resulting in pose estimation failureand servo failure. Servo failure occurs quite frequently in PBVS when largetranslations and large out-of-plane rotations are simultaneously required.Chesi et al. [16] propose a switching control approach to keep the targetwithin the  eld of view of the camera. PBVS is selected when all featurepoints are inside the  eld of view, implemented as a region of interest (ROI)in the image. When at least one feature point lies on the ROI boundary, aset of rotational and translational control laws is applied to push that fea-ture point back into the region. If all of the above methods fail, the camerais sent away from the target object through a backward translational motionalong the optical axis by a speci ed gain. One drawback of this method isthat appropriate gains and hysteresis bands must be used to avoid chatter-ing, a phenomenon resulting from frequent switching when feature pointsreside near the image boundary. Thuilot et al. [17] propose the use of areference trajectory to coordinate the rate of convergence between transla-tion and rotation motions in PBVS. Rotation errors are allowed to decreaseexponentially without any reference trajectory, while the translational ref-erence frame is adjusted to keep the geometric center of the feature pointsin the camera  eld of view.Cervera [10] and Deng [18] explore the use of alternative coordinateframes for de ning the PBVS error vector to obtain improved servoing char-acteristics. Two methods are discussed. The  rst method de nes the poseerror vector in the stationary target frame. Here, the shortest straight-linetranslation of the camera is achieved at the expense of a highly nonlineartarget pose sequence where image trajectories can easily leave the camera’s eld of view. The second method de nes the pose error vector in the currentcamera frame. If a reference point close to the geometric center of all featurepoints is chosen as the target frame origin, the chances of image trajectoriesleaving the camera  eld of view is signi cantly minimized. However, theability to generate straight-line Cartesian trajectories is lost.Another di culty associated with PBVS is that the stability of the ser-voing system is di cult to study, since it is sensitive to pose estimationerrors. Zanne et al. [19] suggest using sliding mode control to design aPBVS controller that is robust to bounded parametric estimation errors. Aswitching controller is proposed using an appropriate selection of the slid-ing surface, based on the quaternion representation for rotations, to ensurestability.16Chapter 2. Literature Review2.3 Epipolar-Based Visual ServoingWhen a camera views a 3-D object from two distinct positions, there exists anumber of geometric relations between the 3-D points and their projectionsonto the 2-D images that lead to some additional constraints between thetwo sets of observed image points. The epipolar geometry describes suchrelationships between the two resulting views. Conversely, if a set of corre-sponding features in the current image and in the desired image is available,the epipolar geometry can be recovered and used for visual servoing [20].Piazzi [21] and Marotta [22] propose a visual servoing algorithm that usesthe observed epipolar geometry to construct a series of translations androtations that iteratively bring the camera to the desired position. An ad-vantage of this method is that no model of the scene is required and thatthe servoing is compatible with uncalibrated cameras. However, it is shownthat recovering epipoles is often sensitive to image noise. Moreover, nearthe servo target, the epipolar geometry becomes degenerate and it is notpossible to estimate accurately the partial pose between the two views. Analternative is to use homography-based methods which provide more stableestimations when the current and desired images are similar.2.4 Homography-Based Visual ServoingA homography is an invertible transformation that maps points from one3-D plane to another 3-D plane. Since a camera sensor is also a projectiveplane in space, each 3-D plane that is observed by the camera in the currentimage is related to its projection in the desired image by an image homog-raphy. If all the feature points of a target object lie on a 3-D plane, thenthere is a single homography matrix that holds for all the observed featurepoints. This homography can be estimated using a minimum of four pointcorrespondences between the current and desired image. If all feature pointsdo not belong to the same 3-D plane, then three points can be used to de nea virtual plane, and a minimum of  ve additional supplementary points areneeded to estimate the homography [23].Knowledge of this homographic relationship is advantageous because itcan be decomposed to give a partial pose estimation that is useful for visualservoing. The camera rotation and the camera translation (up to a scalefactor) can be recovered from the homography matrix via decomposition[24] [25] without requiring a model of the target object. Thus, some of theuseful properties of PBVS such as decoupled control of camera rotation and17Chapter 2. Literature Reviewtranslation can be obtained using only point correspondences from images.Another advantage of this method is that it works universally for both planarand non-planar target objects.There exist several related approaches that use the recovered rotationand scaled translation parameters for visual servoing. Deguichi et al. [26]use the recovered direction of translation with feedback control to bringthe camera towards its goal in a straight line. Rotation is controlled tocompensate the e ects on the image caused by translation, to keep the targetobject in the  eld of view, and at the same time, to minimize the di erencebetween the current image and the desired image. Malis et al. [27] propose amethod known as 21/2-D visual servoing with a decoupled control law similarto PBVS. Image errors of a chosen reference point are used to control in-plane camera translation, while camera approach and rotation are controlledusing parameters recovered from the homography matrix. Due to the simplestructure of the system, the conditions for global asymptotic stability canbe established. An alternative hybrid visual servoing scheme proposed in[28] is similar to 21/2-D visual servoing, but can tolerate a larger amount ofcalibration errors. However, this method is more sensitive to measurementerrors since the task function is not directly computed as a di erence ofimage data. In [29], the camera is controlled using the recovered directionof translation, while a single image point is chosen to control the x-axisand y-axis rotation, with the rotation matrix controlling the z-axis rotation.This approach results in the image point having a straight line trajectory sothat it always remains in the  eld of view.Homography estimation and decomposition based on three points de n-ing a virtual plane (and additional points to solve for the relationship) canbe extremely sensitive to image noise. In the above cases, the recoveredmotion is used as an approximation for servoing only, rather than for one-step position control. All servoing methods must continuously update theestimate on the homography as new images become available, in order tominimize the impact of estimation errors on the  nal servoing accuracy. Asa result, these hybrid approaches tend to be more computationally intensivethan PBVS and IBVS.2.5 Image Path PlanningMany classical visual servoing methods work well when the initial and de-sired robot positions are close together. The idea behind image path plan-ning methods is to use intermediate reference images to in uence the path18Chapter 2. Literature Reviewof the servoing and to divide the required servoing task into smaller incre-mental tasks. Such intermediate reference images are typically not availablefrom the camera (unless the user has the patience to teach the robot at ev-ery intermediate reference position!) so, in practice, the intermediate featurepositions are generated via partial Euclidean reconstruction of intermediatecamera poses using the initial image and the desired image. In the continu-ous case, an image-based reference trajectory is formed using interpolationin the image, combined with a suitable timing law to produce a time-varyingreference for the visual servo controller to track. This signi cantly improvesthe robustness of visual servoing with respect to calibration and modellingerrors, since the feedback error is always kept small.An image-based path planning method that uses arti cial potential  eldsand homography-based reconstruction to help avoid camera  eld-of-viewlimits and robot joint limits is used in [18] [30] [31] [32]. The path planningtakes place at the beginning of the motion, starting with an estimation of ahomography from the initial image to the desired image. The desired camerapose is found by decomposing the homography matrix into its rotation andtranslation components. A potential- elds approach [33] [34] is used togenerate a path from the initial camera pose to the desired camera pose.An attractive potential  eld is used to bring the camera to its goal, while arepulsive potential  eld pushes the camera away from robot joint limits andanother repulsive potential  eld pushes the camera away from positions thatbring the target object out of view. The planned path is then re-projectedback into the image and interpolated to give a reference image trajectorythat is tracked using IBVS.One disadvantage of the above approach is that the e ectiveness of thepotential  eld planning for camera and robot limit avoidance is highly de-pendent on the accuracy of the initial homography estimation. As discussedabove, homography decomposition for scaled Euclidean reconstruction isknown to be very sensitive to image noise. Unlike the hybrid visual servoingmethods, there is no update to the homography estimation upon subsequentimage observations, as there is no intermediate re-planning step. The actualcamera path may deviate signi cantly from the initially reconstructed cam-era path, so that any repulsive potentials applied to the latter may not bee ective in pushing the actual camera path away from the real robot jointlimits and the real camera  eld-of-view limits.If only the camera  eld-of-view visibility is of concern, Schramm et al.[35] propose an alternative path planning approach that does not require ho-mography decomposition. The algorithm uses the properties of a ne spacesto design a path that ensures the target object is always visible within the19Chapter 2. Literature Reviewcamera’s  eld of view. In [36], a helical trajectory is proposed to harmonizetranslation with rotation to maintain target visibility, without the use ofpotential  eld-based planning.2.6 Image Trajectory TrackingClose tracking of the image reference trajectory is important to ensure thatthe actual image trajectory does not exit the camera’s  eld of view duringservoing, despite the original path planning e orts. Close tracking of theplanned trajectory is also important for ensuring predictable camera motionfor avoiding robot joint limits. To minimize tracking errors, the visual servocontrol law must be modi ed to correctly anticipate the time-variation ofthe reference trajectory. Morel [37] and Zanne [38] propose a robust trackingcontroller for visual servoing that guarantees bounded tracking errors, witha bound that can be speci ed by the user to ensure visibility. Rather thannaively increasing the control gains to achieve the desired precision, thereference velocity is modulated to satisfy an inequality relationship betweenthe error bound, the parametric uncertainties in calibration, and the controlgain. This method achieves bounded tracking errors without the use of largecontrol gains and is robust to calibration errors. The only real drawback isthat the resulting reference velocity may be conservative at times, so thismethod may be more suited to tasks that require precise geometric pathtracking, but at low speeds.2.7 Constraint Avoidance with RedundantManipulatorsIf the required positioning task does not use up all degrees of freedom (DoF)available to the robot, the remaining DoF can be exploited by the visualservoing controller to avoid camera  eld-of-view limits, robot joint limits,singularities and obstacles. Redundancy-based solutions for eye-in-hand po-sitioning and for visual tracking are discussed below.For highly redundant robots completing 6-DoF positioning tasks, a gra-dient projection method is proposed for avoiding constraints related to visualservoing [39][40]. The constraints imposed by the environment are embed-ded as a cost function, and the gradient of this cost function is computedas a secondary task that moves the robot away from these constraints. Thisgradient is then projected onto the set of motions that keep the main taskinvariant and added to the control law that executes the main task. Because20Chapter 2. Literature Reviewthe secondary task cannot have any e ect on the main task in this formu-lation, only the degrees of freedom not controlled by the main task can beexploited to perform constraint avoidance. Mansard et al. [41] propose amethod to improve the performance of the secondary task by enlarging thenumber of DoF available. An equality relationship in the classical redun-dancy formalism is replaced with a norm inequality relationship, to allowthe execution of motions produced by the secondary control law that alsohelp the main task to be completed faster. The amount of secondary mo-tion that is gained by using this approach depends greatly on the degree ofopposition between the task and the constraint.Several methods have been proposed for handling 2-DoF tracking tasksvia visual servoing while avoiding constraints. In [42], an object is visuallytracked with an eye-in-hand camera while the robot simultaneously avoidskinematic singularities and joint limits by moving in directions along whichthe tracking task space is unconstrained or redundant. A manipulabilitymeasure is introduced into the visual tracking task function, such that thecontrol law is able to take into account the con guration of the robot whilethe object is visual tracked. It is shown that for tracking objects with planarmotions, the tracking region of the robot can be greatly increased using thismethod. In [43], a frequency-based framework is used to control a 5-DoFrobot (a 2-DoF pan-tilt unit plus a 3-DoF Cartesian gantry) for tracking anoperator as he walks around a workcell. A partitioning control law is usedto exploit the kinematic and dynamic attributes of each DoF. The high-bandwidth pan-tilt unit is employed for tracking so that the fast-movinghuman target stays within camera’s  eld of view, while the low-bandwidthgantry is used to reduce the bearing angle between the camera and thetarget, reducing the demand on the pan-tilt unit’s range of motion. Analysisof phase characteristics show that the large bandwidth DoF that are visuallyservoed act as lead compensators for the remaining DoF with slow responsetimes.2.8 Predictive Control for Visual ServoingVisual servoing can be formulated as an optimization procedure that com-putes the appropriate control input to the robot given a cost function thatde nes measured image errors. A linear quadratic Gaussian (LQG) controldesign can also be used to choose gains that minimize a linear combina-tion of state and control input [44]. This approach explicitly balances thetrade-o between tracking errors and robot motion. A more recent develop-21Chapter 2. Literature Reviewment is the use of predictive control in visual servoing so that path planningand control can be solved simultaneously. Predictive control makes explicituse of a process model to obtain a control signal by minimizing an objec-tive function. Generalized Predictive Control (GPC) is principally suited forsingle-variable linear control, since the process model is presented in the formof a polynomial transfer function, while Model Predictive Control (MPC)is formulated in state space and is designed for multi-variable control. In[45], GPC is used to provide improved tracking characteristics for a one-DoFpro le-following task, while the remaining DoF are controlled using visualservoing. Sauvee et al. [46] presents a framework for controlling a robotpositioning task using MPC with image feedback from a stationary camera.Successful avoidance of  eld-of-view limits (for the stationary camera ob-serving the robot) and avoidance of joint limits (for the robot completingthe task) are demonstrated in simulation. This approach is extended in [47]to a medical robot controlling a surgical instrument using MPC with imagefeedback from an stationary ultrasound transducer. In both [46] and [47],the imaging sensor is stationary with respect to the robot, and the geometryof the end-e ector tool and the transformation from the camera frame tothe robot base frame are assumed to be known.2.9 SummaryThe chapter presented an overview of the state-of-the-art in constraint-awarevisual servoing. A description of the major advances in the two main do-mains of visual servoing are discussed. When large-range motions are re-quired, it is shown that representing a positioning task as a minimizationof image feature errors, as in IBVS, results in camera motions that natu-rally keep the target object within the  eld of view, but are convoluted inthe joint-space of the robot. On the other hand, representing the position-ing task as a minimization of rotation and translation errors, as in PBVS,produces e cient Cartesian trajectories (but the resulting joint-space mo-tion can still be poor), which may cause the target object to exit the  eld ofview. Some approximate knowledge of the pose of the target object, obtainedthrough homography decomposition or pose estimation, is bene cial for im-proving visual servo motion since it allows path planning to occur. How-ever, path planning must occur in the joint-space of the robot, if avoidanceof robot joint limits and avoidance of whole-arm collisions with workspaceobstacles is required. An explicit representation of system constraints inthe visual servoing control law helps to ensure that robot positioning and22Chapter 2. Literature Reviewconstraint avoidance can be simultaneously achieved.This thesis will build on top of the strengths and weaknesses of the aboveapproaches to arrive at a method for achieving collision-free and constraint-aware visual servo control of an eye-in-hand robot. Homography decom-position and online pose estimation are discussed in Chapters 3 and 4, aspractical methods of obtaining an approximate pose of a target object forservoing and planning purposes. Path planning methods are discussed inChapter 5, using a method inspired by IBVS to address camera  eld-of-view and robot joint-limit constraints, and combined with probablistic roadmaps to address collision constraints. Chapter 6 presents a unifying visualservoing framework that solves the problem of planning and servoing simul-taneously, resulting in a visual servo control law that achieves close-looppositioning while being explicitly aware of system constraints. The cumula-tive goal is a visual servo control law that can be used to drive large-rangerobot motions without losing sight of the target object, violating robot jointlimits or velocity limits, or colliding with workspace obstacles.23Chapter 3Image Homography3.1 IntroductionThe objective of this chapter is to evaluate the suitability of homography-based methods for constraint-aware visual servoing. In particular, the abil-ity of homography-based methods to extract Euclidean motion parameterswithout requiring a model of the observed object is quite attractive for usein the joint-space path planning methods that are presented in Chapter 5and Chapter 6. The method of visual servoing presented and implementedin this chapter is known as 21/2-D visual servoing and it is based on meth-ods proposed in [27]. The goal is to take advantage of the properties of thehomography (as described in Section 2.4) to improve the predictability ofvisual servoing and to minimize the chance that the resulting motion willviolate system contraints. The  rst section gives an overview of the visualservo control law. The second section describes how the requirements of thecontrol law are met using images with matched feature points. In particu-lar, the methods of homography estimation from observations of non-planarobjects and of homography decomposition to recover scaled Euclidean pa-rameters are discussed. Simulation results are shown for several positioningtasks that require large visual servoing motions. The performance of thishomography-based visual servoing method is compared to that of traditionalimage-based visual servoing (IBVS). Finally, experimental results are ob-tained to assess the accuracy of homography estimation and decompositionusing images from an eye-in-hand camera.3.2 Homography-Based Visual Servoing3.2.1 OverviewAs explained in Chapter 1, the goal of eye-in-hand visual servoing is to con-trol the 6-DoF position and orientation of the wrist-mounted camera relativeto a target object, through the regulation of an error function de ned bya set of visual features. This error metric describes the di erence between24Chapter 3. Image Homographythe current camera frame, Fc, and the desired camera frame, Fc . Whenexpressed purely in image space, this error is the di erence in the observedpixel locations of the visual features. When expressed purely in Cartesianspace with respect to the current camera frame, this error is the transla-tion, ctc , de ned in Fc and the rotation, cRc , that describes Fc in thecoordinates of Fc .In 21/2-D visual servoing, the error metric consists of a combination of2-D image features and 3-D parameters. To take advantage of the accuracyand the robustness of image-based techniques, the x-y translation of thecamera is controlled by aligning the projected image coordinates of a real-world reference point P to that of the reference image. To capitalize on thedecoupled nature of position-based techniques, the required camera rotationand the rate of approach in the z direction of the camera frame are estimatedfrom the image using scaled Euclidean reconstruction. This approach doesnot require a model of the target object. All it requires is a minimum ofeight matched points between the two images to estimate the homographyof a virtual plane attached to the object de ned by any three points.3.2.2 Feature SelectionIt is assumed that the object of interest remains rigid during visual servoingand that it can be represented as a distribution of feature points in 3-Dspace. A target point cPj is selected on the object to provide the required2-D image coordinates (x;y) for measuring in-plane camera translations. Anadditional three points cP1, cP2, cP3 are selected on the object to de ne avirtual plane  to act a 3-D reference for measuring the rotation cRc andthe depth Z.In 21/2-D visual servoing, the feature vector s consists of 6 parametersthat derived are from a combination of 2-D and 3-D metrics:s =  XZ YZ logZ  rT T =  x y logZ  rT T ; (3.1)where: (X;Y;Z) are the real-world coordinates of a 3-D target point cPj withrespect to the camera frame. (x;y) are the normalized metric coordinates of the image point corre-sponding to cPj.  and r are, respectively, the angle and axis of rotation matrix cRc associated with the virtual plane  de ned by three target points cP1,cP2, cP3.25Chapter 3. Image HomographyKnowledge of the intrinsic camera parameters is required, since the nor-malized metric coordinates mj =  x y 1 T of point cPj are related to itsmeasured pixel coordinates pj =  u v 1 T by the camera matrix, C:mj = C 1pj; (3.2)where,C =24fku fku cot u00 fkv( 1sin ) v00 0 135; (3.3)where (u0;v0) are the images coordinates of the principal point, f is the focallength,  is the perpendicular skew angle, and ku and kv are the number ofpixels per unit distance in x and y, respectively.The current camera frame Fc is aligned with the desired camera frameFc when the error function e21/2-D = (s s ) is zero in all of its components.Here, s corresponds to the feature measurements that are observed at thedesired camera pose:e21/2-D = (s s ) =  x x y y log( ZZ )  rT T : (3.4)The depth component Z of the reference point cPj cannot be recoveredfrom a single image. However, if the projective homography H of the virtualplane  is available (relating the observed image points of cP1, cP2, cP3 inthe two camera frames), then the ratio log( ZZ ) and the rotation cRc (or  r)can be recovered to complete the last 4 components of the error function.In summary, the goal of the 21/2-D visual servo control scheme is to regulatethis error function, Equation 3.4, to zero.3.2.3 Control SchemeMost vision-based control approaches consider only robot kinematics, sincethe bandwidth for visual control is usually limited by the camera frame rateand image processing times. Robot dynamics are generally handled with alow-level PID controller using joint encoder feedback.The 21/2-D visual servoing controller is designed at the velocity level,based on Jacobian matrices which relate the rate of change between the vi-sual measurements and the robot joint angles. The Jacobian relationship be-tween the spatial velocity of the camera vc =  xvc yvc zvc xwc ywc zwc Tand the time variation of the error function _e21/2-D is described by the inter-action matrix L21/2-D:_e21/2-D = L21/2-Dvc; (3.5)26Chapter 3. Image Homographywhere,L21/2-D = 1d Lv L(v;!)0 L! : (3.6)The 21/2-D error function gives rise to an interaction matrix that isupper triangular, meaning that translation and rotation are decoupled inthe control law. Expressions for the sub-matrices Lv, L(v;!), L! are derivedas follows, where the parameter  1 is recovered from the homography H ofa virtual plane  :Lv = 1 124 1 0 x0  1 y0 0  135; (3.7)where 1 = Zd ; (3.8)L(v;!) =24xy  (1 +x2) y(1 +y2)  xy  x y x 035; (3.9)L! = I3  2  r  + 1 sinc( )sinc2( 2)! r 2 ; (3.10)where  r  is de ned as the skew of the vector r =  r1 r2 r3 T: r  =240  r3 r2r3 0  r1 r2 r1 035: (3.11)Here, a proportional velocity control law is designed to ensure the ex-ponential convergence of each component in the error function towards zeroby imposing:_e21/2-D =  e21/2-D; (3.12)where  is the convergence rate. Substituting the Jacobian relationship_e21/2-D =   e21/2-D into the rate of change of error, an expression for thedesired camera velocity is obtained:vc =  L 121/2-De21/2-D: (3.13)The above expression is always valid because L21/2-D is free of singu-larities in the entire workspace in front of the virtual plane (and is always27Chapter 3. Image Homographyinvertible). The desired camera velocity command is converted to robotjoint velocities _q using the robot Jacobian relationship Jrobot(q). The in-verse relationship is valid when Jrobot(q) is non-singular in the robot jointspace:_q =  J 1robot(q)L 121/2-De21/2-D: (3.14)Finally, the robot joint velocities _q commanded to the robot controllercan be written as a function of the measured error components in e21/2-D:_q =  J 1robot(q) d L 1v  d L 1v L(v;!)0 I3 2664x x y y log( ZZ ) r3775: (3.15)The parameter d represents the distance from virtual plane  to thedesired camera frame Fc . Unfortunately, it cannot be recovered from theimages, so it must be approximated o -line when the desired training im-age is acquired. Within reasonable bounds, this parameter only a ects therelative convergence rate of the camera’s translational and rotational com-ponents.3.2.4 Servoing Requirements from ImagesTo summarize, the parameters that must be estimated from the images tobe used as input into the control law, discussed in Section 3.2.3, are:1. cRc (or in angle-axis form, [ , r] )2. log(Z=Z )3.  1 = Z=d The  rst two metrics de ne the error in 3-D rotation and the error indepth, respectively, between the current camera frame Fc and the desiredcamera frameFc . They are required in order to evaluate the error functione21/2-D. The last metric is used to control the rate of translational conver-gence of the camera and it is required in the sub-matrix Lv of the interactionmatrix L21/2-D. These parameters can be recovered from two images if thehomography H for a common virtual plane  is available.Obtaining an accurate estimate of H is not trivial for images of non-planar objects, where a virtual plane is de ned by three feature points, andadditional feature points that do not necessarily lie on the virtual plane28Chapter 3. Image Homographyare used to recover the homography relationship. The interested readeris directed to Appendix A for a discussion on the homography estimationmethod implemented in the simulations and experiments.Once H is determined, it is decomposed to recover the Euclidean param-eters that are used in the visual servoing control law. The interested readeris directed to Appendix B for a discussion on the homography decompositionmethods implemented in the simulations and experiments. The decomposi-tion solution has the following form, where up to 8 possible solutions exist:H = d (cRc ) + (ctc )n T: (3.16)where cRc is the rotation matrix from frame Fc to Fc, n is the unit vector normal to  expressed in Fc , ctc is the direction of translation from Fc to Fc, and d is the signed distance from  to Fc .In Appendix B.3, a method is presented that uses visibility constraintsto reduce the number of decomposition solutions from 8 to 2. A methodthat is speci c for visual servoing, to determine which of the two remainingsolutions is correct, is presented in Appendix B.4. Once cRc is extractedfrom the homography,  and r can be computed to complete the last threecomponents of the error function e21/2-D. Since H is only de ned up toa scale, the vector ctc only gives the direction of translation (but not itsmagnitude), so ctc cannot be used to directly control camera translation.The remaining parameters  1 and log(Z=Z ) that are required in the controllaw are determined from H as follows: 1 = Zd = 1d  dnTm!= dd 1nTm = det(H) 1ntm!(3.17)log ZZ != log dd n Tm nTm!= log det(H) n Tm nTm!!(3.18)In addition to the direct use of these recovered parameters in the 21/2-Dvisual servoing control law, an accurate knowledge of cRc , n , ctc and d is useful for robot path planning, as discussed in Chapter 5.29Chapter 3. Image Homography3.3 Simulation Results3.3.1 Purpose and SetupThe purpose of these simulations is to evaluate the image trajectories, theCartesian trajectories and the robot trajectories that result from visual ser-voing using homography-based Euclidean reconstruction. These trajectoriesare compared against those obtained using IBVS to discern any improve-ments to the observed motion. Two di erent positioning tasks with signif-icant displacements are considered. The  rst is a translational task thatrequires the robot end-e ector to approach the target object with some out-of-plane rotations. The second is a pure rotational task of 180 degrees aboutthe optical axis. The observed target is a non-planar object with 9 identi -able feature points. The target object is positioned above the robot in theseexperiments to increase the e ective range of motion available to the robotfor servoing, since robot joint limits are not explicitly managed in the servo-ing control law. The simulations are performed on a 6-DoF CRS-A465 robotwith a Hitachi KP-D8 camera amounted on the end-e ector to re ect theequipment available in the Collaborative Advanced Robotics and IntelligentSystems (CARIS) Lab at UBC.3.3.2 Image-Based Visual ServoingThe error to be minimized in image-based visual servoing (IBVS) is the dif-ference between current image feature location (u;v) and the desired imagefeature location (u ;v ), for n chosen feature points:eIBVS =  (u1 u 1) (v1 v 1)    (un u n) (vn v n) : (3.19)A proportional control law is used to drive image coordinates exponen-tially towards their desired locations, with  as the convergence rate:_eIBVS =  eIBVS: (3.20)To achieve the above closed-loop behaviour, the control law for IBVShas the following form:_q =  J+robot(q)L+IBVS(x;y;Z)eIBVS (3.21)where Jrobot(q) is the robot Jacobian corresponding to the eye-in-hand con- guration, and LIBVS is the image Jacobian of the feature points. For mul-30Chapter 3. Image HomographyFigure 3.1: IBVS of a task requiring translations and out-of-plane rotations:image trajectory (left) and Cartesian trajectory (right).tiple feature points, LIBVS is a stacked matrix composed of the following[4]:LIBVS =  LT1    LTn T (3.22)whereLi(xi;yi;Zi) =  1Zi 0xiZi xiyi  (1 +y2i ) yi0  1Zi yiZi 1 +x2i  xiyi  xi : (3.23)The distance Z of a feature point to the image plane is obtained fromthe model of the object. The normalized coordinates (x;y) are calculatedfrom (u;v) using the camera matrix from Equation 3.2.Figure 3.1 shows the results from the  rst robot positioning task asdescribed in Section 3.3.1 using IBVS for control. The point ‘o’ designatesthe start of the motion and the point ‘x’ designates the end of the motion.The image trajectories of the feature points are near-straight lines from thestart-position to the end-position which keeps the target within the  eld ofview. The Cartesian trajectory is also acceptable, due to the simplicity of thetask, which mainly involved translations with some out-of-plane rotations.These results show that the trajectories generated from IBVS are sometimesacceptable for large motions.31Chapter 3. Image HomographyFigure 3.2: IBVS of a task requiring a rotation of 180 degrees about theoptical axis: image trajectory (left) and Cartesian trajectory (right). Thetask is not completed successfully due to the camera retreat problem, whichviolates robot joint limits.Figure 3.2 shows the results from the second robot positioning task as de-scribed in Section 3.3.1 using IBVS for control. The feature points approachtheir desired positions in straight-line trajectories. However, the visual ser-voing stops before the feature points reach their goal, and the positioningtask does not converge. Examination of the Cartesian trajectory shows thatunder IBVS control, the camera is forced to retreat away from the targetobject until a joint limit is reached. These results demonstrate the problemof camera retreat, and illustrate why IBVS cannot be used for positioningtasks involving signi cant rotation about the optical axis.3.3.3 21/2-D Visual ServoingThe method of 21/2-D visual servoing is described in Sections 3.2, whilethe methods of homography estimation and decomposition are described inAppendix A and Appendix B, respectively. Figure 3.3 shows the resultsfrom the  rst robot positioning task as described in Section 3.3.1 using 21/2-D visual servoing for image feedback control. The point ‘o’ designates thestart of the motion and the point ‘x’ designates the end of the motion. In21/2-D visual servoing, the image trajectories of the feature points are morecurved than the near-straight-line trajectories that are obtained using IBVS.32Chapter 3. Image HomographyFigure 3.3: 21/2-D visual servoing of a task requiring translations and out-of-plane rotations: image trajectory (left) and Cartesian trajectory (right).This is due to the decoupling between image-based translation control andhomography-based rotation control. The Cartesian trajectory is shown to beacceptable, as it does not deviate too far from a simple straight-line motion.The robot joint positions and velocities of the resulting motion are shown inFigure 3.4. They exhibit exponential convergence towards the desired robotcon guration, at a rate that is proportionally to the control gain. Theseresults show that while the path generated from 21/2-D visual servoing isacceptable for large translational tasks with some out-of-plane rotations, itstiming must be addressed through the use of a reference trajectory, sinceit is sub-optimal over large-range motions. The joint velocities decreasesexponentially with the image error, resulting in high velocities at the startand negligible velocities towards the end.Figure 3.5 shows the results from the second robot positioning task asdescribed in Section 3.3.1 using 21/2-D visual servoing for image feedbackcontrol. Unlike IBVS, which forces the camera to retreat (resulting in theviolation of joint limits), 21/2-D control is able to correctly recognize that a180-degree rotation is required, using the homography-based methods out-lined in Appendix A and Appendix B. The Cartesan trajectories show that21/2-D visual servo control executes the required rotational motion withoutrequiring any extra translational motion. This property of decoupled con-trol illustrates one of the many bene ts of using partial pose information33Chapter 3. Image HomographyFigure 3.4: 21/2-D visual servoing of a task requiring translations and out-of-plane rotations: robot joint positions (top) and robot joint velocities (bot-tom).34Chapter 3. Image HomographyFigure 3.5: 21/2-D visual servoing of a task requiring a rotation of 180 de-grees about the optical axis: image trajectory (left) and Cartesian trajectory(right).to supplement visual servoing in order to simplify motion. An additionaladvantage of homography estimation and decomposition is that it does notrequire a 3-D model of the observed target object.3.4 Experimental Results3.4.1 Purpose and SetupThis section outlines a method for evaluating the accuracy of homographyestimation and decomposition from image observations of real target objects.This step is important for two reasons. First, the accuracy of the partialpose estimation from the image homography a ects the convergence rateof all visual servoing methods that use these parameters as part of theircontrol schemes. Secondly, the recovered Euclidean parameters must besu ciently accurate if they are to be used for path planning for robot joint-limit avoidance and camera limit avoidance, as proposed by several authors[18] [30] [31] [32].Experiments are performed using real images obtained from an eye-in-hand camera to evaluate the accuracy of the scaled Euclidean parameters(cRc ;ctc ) recovered from the homography H of a virtual plane. In these35Chapter 3. Image Homographyexperiments, a Hitachi KP-D8 miniature CCD camera is attached the end ofa CRS-A465 robot arm to observe a non-planar target object. As the imagesare captured, the corresponding robot joint encoder measurements are alsorecorded so that the change in camera position and orientation are known.The scaled Euclidean parameters (cRc ;ctc ) that are recovered from visionare compared against the ground truth provided by the kinematics of therobot.3.4.2 Extracting and Matching Image FeaturesThe Scale Invariant Feature Transform (SIFT) [48] is used to characterizethe target object into a set of features points. These SIFT feature pointsare used to establish point-to-point correspondences between subsequentimages. SIFT is particularly well-suited to this task, since it generates well-localized features that work well on textured objects. An added advantageis that they are invariant to scale changes and are somewhat invariant tosmall changes in viewpoint, so that the camera is not restricted to movein a planar 2-D fashion. Features between two images are matched usingthe nearest-neighbor algorithm and outliers are removed using semi-localconstraints. For each matched candidate, K out of its N nearest neighborsmust agree with its scale, orientation, and location in order for it to be con-sidered a valid match. K = 8 is chosen because the subsequent estimationof the homography requires at least eight matched points. N = 16 is chosenexperimentally to give good detection rates while minimizing computationtimes. The thresholds for scale, orientation, and location consistency arechosen as follows:23   ScaleKeyScaleKeyMatch! ScaleNeighborScaleNeighborMatch! 32; (3.24) 30  di  di (Orikey;OriKeyMatch);di (OriNeighbor;OriNeighborMatch)  30 ; (3.25)23  Scale(Distancematch&neighbor)(Distancekey&neighbor)  32: (3.26)These parameters were experimentally tested with images and shown toprovide good outlier removal.36Chapter 3. Image Homography 12[4]  Wilson, W, and Hulls, C. W.  Relative End-Effector Control Using Cartesian Position Based Visual Servoing. IEEE Trans. on Robotics and Automation, 12(5):234-246, 1996.  [5]  Deng, L., Janabi-Sharifi, F., and Wilson, W.  Hybrid Motion Control and Planning Strategies for Visual Servoing. IEEE Trans. on Industrial Electronics, 52(4):234-246, 2005.  [6]  Faugeras, O., and Lustman, F. Motion and Structure from Motion in a Piecewise Planar Environment. Int. Journal of Pattern Recognition and Artificial Intelligence, 2(3):485-508, 1988.  [7]  Zhang, Z., and Hanson, A.R. Scaled Euclidean 3D Reconstruction Based on Externally Uncalibrated Cameras. In IEEE Symp. on Computer Vision, Coral Gables, Florida, 1995.  [8]  Chiaverini, S., and Siciliano, B. Review of the Damped Least-Squares Inverse Kinematics with Experiments on an Industrial Robot Manipulator. IEEE Transactions on Control Systems Technology,  2(2):123-134, 1994.  [9] Hartley, R. and Zisserman, A. Multiple View Geometry in Computer Vision, Cambridge University Press, Cambridge, UK, 2000.  Figure 1: Sequence of images used for evaluating the accuracy of the Euclidean parameters (R, t) recovered from the homography of a virtual plane defined by three image points on the object. View 1 View 2View 3View 4View 5 View 6View 7View 8View 9 View 10 View 11 View 12Figure 3.6: Sequence of images used to assess the accuracy of homography-based methods for visual servoing.3.4.3 Error AssessmentThe method of estimating cRc , ctc from the homography of a virtual planeis tested on several household and industrial 3-D objects such as co ee mugs,key chucks, motors, and computer cooling fans. The most comprehensiveresults are obtained from a series of twelve images of a tea box taken atapproximately 10- to 15-degrees increments of out-of-plane 3-D rotations.This sequence of images is shown in 3.6 for visual reference. Although theobject has several planar surfaces, no e ort is spent on ensuring that allmatched features belong to a common plane. In fact, many of the SIFTfeatures matched between the images belong to di erent faces on the teabox. The advantage of homography estimation based on the virtual parallaxis that it applies to both planar and non-planar objects. Regardless of thestructure of the object, three points are automatically selected to de ne areference plane which maximizes the surface area of corresponding trianglesin both images.Figure 3.7 shows the number of matched SIFT features for each image37Chapter 3. Image HomographyFigure 3.7: Number of feature matches available for non-planar homographyestimation for the image sequence shown.pair with viewpoint 7 as the reference. The number of matched SIFT fea-tures between two images decreases quickly as the camera deviates from thereference viewpoint. For out-of-plane rotations greater than 25 degrees, itis di cult to obtain the required number of matching features to accuratelyestimate H. In fact, because only six SIFT features are matched betweenviewpoint 1 and viewpoint 7, in this case, the rotational and translationalparameters cannot be recovered. The quality of the matched SIFT featuresalso decreases in the sense that the locations of the matches are less accu-rate. Since SIFT features are not truly a ne invariant, distortions due tochanges in viewpoints (even for planar features) will cause the location ofthe SIFT features to shift, resulting in an incorrect estimate of H. This isproblematic since in the virtual plane homography estimation method [23]since, regardless of the number of matched features available for homog-raphy estimation, only three feature points are used to de ne the virtualplane.Figure 3.8 show the magnitude of errors in the estimated rotation cbRc for the image sequence shown in Figure 3.6. The magnitude of the actualrotation cRc corresponding to each viewpoint is also shown for comparison.Viewpoint 7 is used as the reference image for establishing the canonical38Chapter 3. Image Homographyframe of reference. The rotation error is de ned as the shortest length ofthe geodesic starting at cRc and ending at cbRc , which corresponds tothe rotation angle  error of the rotation matrix cRc cbR 1c . Note that therotation errors cannot be simply subtracted from the actual camera rotationto obtain the rotation estimate, since their respective axes of rotation do notalign. Finite 3-D rotations are, in general, not vectors and therefore cannotbe summed up vectorially. It is found that the magnitude of errors in cbRc increases quickly as the camera deviates from the reference viewpoint. Forsmall camera motions throughout viewpoints 4-9, the rotation estimationerrors are less than 5 . The largest error is 15 for viewpoint 12, which alsohas very few matched features.Figure 3.9 show the magnitude of errors in the estimated translation cbtc for the image sequence shown in 3.6. The magnitude of the actual trans-lation ctc corresponding to each viewpoint is also shown for comparison.Viewpoint 7 is used as the reference image for establishing the canonicalframe of reference. Since the recovered translation is valid only in direction(not magnitude), the translational error is de ned as the angle  t betweenthe normalized vectors ctc =kctc kand cbtc =kcbtc k. It is found that errors inthe direction of translation (10  20 ) are generally greater than the errorsin rotation (average 3  8 ). However, the magnitudes of the errors in cbtc appear to settle at a constant level (20 degrees on the left, 10 degrees on theright) even as the camera deviates from the reference viewpoint, except forviewpoint 12, for which there is a low number of matched features.One direction for future work is to investigate whether a ne invariantimage features would improve the estimate of H for large changes in cameraviewpoint. If not, then multiple training images of the target object takenat 10- to 20-degree increments of rotation may be required to generate goodestimates of H over large viewing angles. Another direction is to investigatewhether Kalman  ltering would improve the estimates of cRc and ctc byincorporating tracking through successive frames. The current implementa-tion only uses the previous solution to cRc and ctc to help determine thecorrect solution to the decomposition of H. It does not track the pose ofthe virtual plane, nor the rotation and translation parameters of the cam-era. It remains to be veri ed whether additional information provided bythe Kalman  lter would improve the robustness of the estimations in thepresence of image noise.39Chapter 3. Image HomographyFigure 3.8: Rotational accuracy of homography decomposition for visualservoing.Figure 3.9: Translational accuracy of homography decomposition for visualservoing.40Chapter 3. Image Homography3.5 SummaryExperiments using an eye-in-hand camera validate homography-based Eu-clidean reconstruction as a useful method for recovering from images, rota-tional and scaled translation parameters that can be used to improve thepredictability of visual servoed motion. The de nition of a virtual planeformed by any three feature points on an object allows an image-to-imagehomography to be associated with any 3-D object, planar or non-planar.The virtual parallax method uniquely solves for that homography when 5or more additional feature points are available. Analysis and simulations ofvisual servoing tasks in this chapter validate homography-based visual ser-voing as a useful method for positioning a robot to target objects withouta priori models. The 2-D/3-D hybrid de nition of the error metric in 21/2-D visual servoing results in an upper triangular Jacobian so that cameratranslation and rotation can be decoupled in the control law, resulting ine cient camera motion, as shown in simulation reults.However, experimental results using images also show the limits of usinghomography decomposition for open-loop path planning, especially whenprecise Cartesian or joint-space trajectories are required for obstacle avoid-ance. The accuracy of the rotation and translation estimated from a ho-mography tends to su er when the camera displacement is large due to:i) an insu cient number of matched feature points between very di erentviewpoints; and ii) the sensitivity of homography decomposition to imagenoise and to real-image deviations from projections produced by the clas-sical pinhole-camera model. Regardless of the number of matched featuresavailable for homography estimation, only three feature points are used bythe homography estimation to de ne the virtual plane. The reliance of thevirtual parallax method on these three chosen feature points explains whythe recovered parameters are so sensitive to image noise. The next chapterdiscusses an improved method of pose estimation, using a model of the tar-get object with an extended Kalman  lter (EKF) to obtain pose estimatesthat are more robust against image noise and modelling errors.41Chapter 4Robust Pose Estimation4.1 IntroductionIn the previous chapter, it was shown that the partial recovery of Euclideanparameters from images could be used to signi cantly improve the perfor-mance of visually servoed motion. Speci cally, knowledge of the requiredrotation and the direction of translation (recovered from a homography) al-lowed the visual servoing controller to execute the positioning task in a nearstraight-line motion in Cartesian space, resulting in e cient camera mo-tions. Not only did this approach minimize the chance of exceeding robotjoint limits during servoing, it improved the predictability of the servoed mo-tion and avoided the problem of camera retreat. However, the rotation andtranslation parameters recovered from homography estimation and homog-raphy decomposition were found to be extremely sensitive to image noise.One reason for this sensitivity is that the partial pose estimates were ob-tained without tracking and that the estimation was executed without anyassumptions regarding the structure of the observed scene (except that avirtual plane could be formed by any three feature points).This chapter discusses the use of an extended Kalman  lter (EKF), inconjunction with a model of the target object, to improve the robustness ofthe pose estimates against image noise and model disturbances. The EKFis an nonlinear extension of the classical Kalman  lter, which is a linearoptimal estimator. The EKF directly estimates the states of a nonlinearsystem using a bootstrap method. The nonlinear system is linearized aroundthe current Kalman  lter estimate, while the Kalman  lter estimate of thenominal trajectory is, in turn, based on the linearized system. A nonlinearobserver is required for pose tracking, due to the nonlinearity in the cameraphotogrammetric equations and in the parametric representations of 3-Drotations. An added bene t of the EKF is that it can be implemented totrack the velocity of the target object, thus enabling the use of position-based visual servoing (PBVS) with respect to a moving object.The focus of the chapter is the control of an eye-in-hand robot using thePBVS approach. The EKF is used to estimate the pose of a (potentially)42Chapter 4. Robust Pose Estimationmoving target object from images, while a state-feedback controller is usedto drive the eye-in-hand robot to a desired pose with respect to the object.Analysis and synthesis of the observer and the controller is performed withinthe framework of state-space control. The PBVS controller is described inthe  rst section, while the observer is derived in the second section. Follow-ing, the performance of PBVS is compared against that of standard methodsusing a number of visual servoing tasks. A section is devoted to the discus-sion of the transient performance of the EKF in the presence of image noiseand modelling errors, and its e ect on the PBVS controller.A well-known problem encountered in PBVS is that the position-controlledcamera trajectory may cause the target object to exit the  eld of view duringservoing, resulting in pose estimation failure and servo failure. A method us-ing adaptive control gains for PBVS (borrowed from hybrid servoing [27]) isproposed in this chapter to address the  eld-of-view problem. This methodexploits the decoupled control of camera translation and rotation availablein PBVS to ensure that the target stays within the camera’s  eld of view,under the assumption of a su ciently fast image frame rate.Another common problem that is encountered in PBVS is that full con-trollability of the camera is lost when its position is at a robot singularity.Near the singularity, the required servoing motion may also result in largejoint velocities. A practical method of restoring the controllability of visualservoing methods near robot singularities is discussed in this chapter.4.2 Controller Design4.2.1 Plant ModelIn position-based visual servoing (PBVS), the 6-dimensional state vector isan error function, ePBVS(t), consisting of the following Euclidean parame-ters:ePBVS(t) =  c tc; r T (4.1)where c tc are the x-y-z coordinates of the current camera frame expressedin the desired camera frame.  and r are, respectively, the angle and axis of rotation extracted fromc Rc, the rotation matrix that expresses the coordinates of the currentcamera frame in the coordinates in the desired camera frame.43Chapter 4. Robust Pose EstimationWhen the norm of the error function, ePBVS(t), is zero, the currentcamera frame is aligned with the desired camera frame. Since the translationand orientation errors between the current and the desired camera frame donot naturally correct themselves, the autonomous system has the followingform:_ePBVS(t) = AcontrolePBVS(t); (4.2)whereAcontrol = 06 6: (4.3)The inputs to the system are the rate of change of the joint angles con-trolled by the motors of the 6-DoF eye-in-hand robot:_q =  _q1 _q2 _q3 _q4 _q5 _q6 T : (4.4)Most vision-based control approaches consider only robot kinematics,since the bandwidth for visual control is usually limited by the camera framerate (in Hertz) and image processing times (in milliseconds). Robot dynam-ics are generally taken into account with a low-level PID controller usingjoint encoder feedback.To solve for the state di erential equations, it is necessary to derivethe relationship between the input, _q, and the rate of change of the errorfunction _ePBVS(t). At the position level, the error space, the camera spaceand the robot space are all related by nonlinear mappings that are neithersurjective nor injective. Fortunately, the controller is designed at the velocitylevel where they are related by linear Jacobian matrices that are functionsof only the positional state variables. The velocity of the camera has thefollowing three translational and three rotational components:vc =  xvc yvc zvc xwc ywc zwc T : (4.5)The velocity of the camera vc is related to the velocity of the joint angles_q by the robot-manipulator Jacobian: vc = Jrobotjq _q : (4.6)The exact form of Jrobot is a function of the kinematic structure of theeye-in-hand robot. Methods for determining Jrobot can be found in textssuch as [49]. The rate of change of the error function _ePBVS(t) is related tothe velocity of the camera vc by the interaction matrix LPBVS: _ePBVS = LPBVSje vc : (4.7)44Chapter 4. Robust Pose EstimationFor this de nition of ePBVS(t), the interaction matrix has a simple de-coupled form between translational and rotational motions [13]:LPBVS = c Rc 00 L r ; (4.8)whereL r = I3 3  2  r  + 1 sinc( )sinc2( 2)! r 2 : (4.9)Substituting for _ePBVS(t) in terms of _q using the Jacobian relationships,the state di erential equation for the non-autonomous system is obtainedas follows:_ePBVS(t) = AcontrolePBVS(t) + BcontrolFcontrol _q(t); (4.10)whereAcontrol = 06 6; (4.11)Bcontrol = I6 6; (4.12)Fcontrol =  LPBVSje  Jrobotjq : (4.13)4.2.2 Analysis of ControllabilityThe system is completely controllable if, and only if,  I6 6 Acontrol BcontrolFcontrol (4.14)has rank n = 6. Since the eigenvalues of Acontrol are all zero, this matrixevaluates to:  06 6 LPBVSjeJrobotjq : (4.15)Thus, LPBVSje and Jrobotjq are required to have full rank in order forthe system to be completely controllable. Due to the nature of the decou-pled form of LPBVSje and the de nition of the rotation matrix c Rc and itsconstituents  r, LPBVSje has no singularities for the entire error space. Infact, the inverse of LPBVSje always exists and is constant:L 1PBVS = c RTc 00 I3 3 : (4.16)However, the same cannot be said for the robot-manipulator Jacobian.For example, when the con guration of the robot is such that two rotational45Chapter 4. Robust Pose Estimationaxes (or two translational axes) are collinear, there is redundant motionalong the collinear direction. Since the robot only has 6 DoF in total, aDoF must be lost in another direction, reducing the rank of the Jacobian.The Jacobian relationship is also unde ned for camera poses that are outsidethe dexterous workspace of the robot. Therefore, the system is completelycontrollable if and only if the commanded camera pose is not at a manipu-lator singularity (i.e. Jrobotjq has full rank) and does not exceed the robot’sjoint limits or reach. A method to address this partial loss of controllabilityis discussed in Appendix C.4.2.3 State-Feedback Controller with Adaptive GainsThe purpose of PBVS is to introduce state feedback to ensure exponentialconvergence towards zero of each component in the error function:_ePBVS =  ePBVS; (4.17)where  > 0 is the convergence rate. The input to the system from the state-feedback controller is chosen to achieve the closed-loop behaviour de ned byEquation 4.17 for the open-loop system de ned by Equation 4.10. Combin-ing the two equation results, the required input from the state-feedbackcontroller is:_q =   J 1robotjq  L 1PBVSje ePBVS(t) (4.18)From Equation 4.17,   corresponds to the desired eigenvalues of thesystem, which must be in the open left-half plane to achieve asymptotic sta-bility. Due to visual constraints, however, a constant value for  is typicallyinsu cient over the entire course of visual servoing. When the initial cam-era pose di ers signi cantly from the desired camera pose, the target objecthas a large chance of leaving the camera’s  eld of view during the servoingtrajectory, leading to a discontinuity and subsequent failure in the controllaw. An adaptive control law is implemented here to address this issue:_ePBVS =  DadaptiveePBVS (4.19)where Dadaptive is a positive diagonal gains matrix of the following form:D =26641 0 0 00 fu(u) 0 00 0 fu(u)fv(v) 00 0 0 Ifu(u)fv(v)3775: (4.20)46Chapter 4. Robust Pose EstimationEach diagonal element of Dadaptive is a function of the pixel coordinates(u;v) of the object’s center. The bell-curve function f(x) is de ned as:f(x) =8><>:exp   x 12[xmin+xmax] 2n(x xmin)m(xmax x)m!if xmin <x<xmax0 otherwise(4.21)with 0 f(x) 1 and two parameters m and n are used to design the formof the bell-curve. The parameters umin, umax, vmin, vmax correspond to thesize of the CCD array of the camera.Combining Equation 4.19 with Equation 4.10, the input to the systemfrom the state-feedback controller with adaptive gains has the following form:_q =   J 1robotjq  L 1PBVSje  Dadaptive ePBVS(t) (4.22)4.2.4 Camera Field-of-View ConstraintsThe elements of Dadaptive are used to establish hierarchical control for eachcomponent in the error function. They act as a band-pass  lter, ensuringthat the image coordinates (u;v) of the target frame are well away from theperiphery of the image before large changes in depth and orientation areallowed to occur. In other words, if at any point in time during servoingthe target object is about to leave the  eld of view, then the degrees offreedom corresponding to camera rotation and approach in the z directionare temporarily frozen, so that x y translational correction can bring theobject back towards the center of the image.The resulting system using Dadaptive is stable in the sense of Lyapunovfor a su ciently fast camera frame rate. 0  f(x)  1 does not a ect thesign of the original eigenvalues. The largest Jordan block is of size 1, soeven if more than one eigenvalue is set to zero, the system will not becomeunstable. At worst, all gains may be severely attenuated and visual servoingmay be brought to a halt. The transient performance of the control maysu er due to the adaptive gains, but at least it will not become unstable dueto the object exiting the  eld of view.4.3 Observer Design4.3.1 Plant ModelGiven the decoupled form of the control law in Equation 4.22, the challengeof PBVS is in obtaining an accurate estimate of the target pose. Even when47Chapter 4. Robust Pose Estimationthe complete calibration of the camera is given and an accurate model oftarget object is known, the pose estimate can still be very sensitive to imagenoise in the components related to out-of-plane translations and rotations.Since the image measurements are only available on a frame-by-frame basis,a discrete-time state-space dynamic model is proposed to track the pose oversuccessive image frames to increase the robustness of the pose estimates toimage noise and disturbances.The relative pose of the target object is a ected independently by themotion of the camera and by the motion of the target object. Although themotion characteristic of the target object is generally unknown, the cameraself-motion can be calculated from robot kinematics and encoder measure-ments. Therefore, only 12 states are required to represent the relative objectpose and the motion of the target object:xobsv =  cwo vo T (4.23)where cwo is the relative pose of the object with respect to the camera,parameterized as x-y-z translation and roll-pitch-yaw orientation angles:cwo = cXo cYo cZo c o c o c o T : (4.24)The second part of the state vector consists of vo, the velocity of thetarget object with respect to a global reference frame. It is expressed interms of its linear and angular velocity in the x-y-z directions:vo =  xvo yvo zvo xwo ywo zwo T : (4.25)The velocity of the camera vc is modelled as an input into the system,since the camera self-motion can be determined with high certainty fromthe robot kinematics and the joint encoder measurements:uobsv = vc =  xvc yvc zvc xwc ywc zwc T : (4.26)Note that vo and vc have the same form. From linear superposition,they can be subtracted from one another to obtain relative velocities:cvo = vo   vc : (4.27)During visual servoing, the measurement outputs used for pose estima-tion are the image pixel coordinates of the feature points of the object that48Chapter 4. Robust Pose Estimationare visible in the camera’s  eld of view. If n feature points are being used,then the output vector is:zobsv =  u1 v1 u2 v2    un vn T : (4.28)The state di erence equations and output equations are derived in thenext section based on these de nitions of states, inputs and outputs.4.3.2 State Di erence EquationsThe relative pose (cwo)k of the object with respect to the camera at iterationk is a function of its pose at iteration k 1 and its higher order derivatives.Using a Taylor series expansion and a sampling period  t, a constant velocitymodel is chosen where derivatives of 2nd order or higher are approximatedas a zero-mean Gaussian disturbance vector 1 k 1:((cwo)k) ((cwo)k 1) + t((c _wo)k 1) + 1 k 1: (4.29)The above approximation is valid under the assumption that both theobject and the camera have smooth motions and experience low accelera-tions. It is assumed that most accelerations remain near zero and that theprobability of high accelerations fall o with a Gaussian pro le. This modelgenerally results in dynamic modelling errors when there are large changesin the relative velocity. These errors are represented by disturbance inputs1 k 1 to the state equations.The following Jacobian relationship exists between the change of the poseparameters (c _wo)k 1 and the relative velocity of the object with respect tothe camera :(c _wo)k 1 = J 1   j(cwo)k 1 (cvo)k 1 (4.30)266666664c _Xoc _Yoc _Zoc _ oc _ oc _ o377777775=266666641 0 0 0 0 00 1 0 0 0 00 0 1 0 0 00 0 0 cos sin cos sin sin cos 10 0 0  sin cos 00 0 0 cos cos sin cos 03777777526666664cxvocyvoczvocxwocywoczwo37777775: (4.31)The Jacobian is nonlinear in terms of the state variables, and its inverseis unde ned for  =  2 , known as the gimbal lock con guration. This issueis further addressed Section 4.3.5. Using the Jacobian relationship, the state49Chapter 4. Robust Pose Estimationdi erence equation can be expressed in terms of the states (xobsv)k 1, inputs(uobsv)k 1 and disturbance variables  k 1:((cwo)k) ((cwo)k 1) + t J 1   j(cwo)k 1 ((cvo)k 1) + 1 k 1 (4.32)= ((cwo)k 1) + t J 1   j(cwo)k 1 ((vo)k 1 (vc)k 1) + 1 k 1:(4.33)Similarly, the velocity of the object with respect to the global referenceframe at iteration k is a function of its pose at iteration k 1 and its higherorder derivatives. Using a Taylor series expansion and a sampling period  t,a constant velocity model is chosen where derivatives of 2nd order or higherare approximated as a zero-mean Gaussian disturbance vector 2 k 1. Thesecond part of the state di erence equations is as follows:(vo)k = (vo)k 1 + 2 k 1: (4.34)4.3.3 Output EquationsTo derive the output equations for pose estimation, a 3-D model of the targetobject is required. An object frame must be established so that its pose canbe de ned and so that the 3-D geometric relationships between its featurepoints can be described. Let (oXj;oYj;oZj) be the coordinates of featurepoint j with respect to the object frame in the model. The coordinates(oXj;oYj;oZj) of feature point j in the camera frame is a function of therelative target object pose cwo with respect to the camera:2664cXjcYjcZj13775 =2664c c c s s  s c c s c +s s cXos c s s s +c c s s c  c s cYo s c s c c cZo0 0 0 137752664oXjoYjoZj13775: (4.35)Given the camera calibration parameters, a predictive model for the pixelcoordinates (uj;vj) corresponding to the projection of feature point j ontothe image plane can be found:uj = f(cXj)ku(cZj) +u0; vj =f(cYj)kv(cZj) +v0 (4.36)50Chapter 4. Robust Pose Estimationwhere (u0;v0) are the images coordinates of the principal point, f is thefocal length, and ku and kv are the number of pixels per unit distance in xand y, respectively.Finally, an output model describing the relationship between the outputmeasurement (zobsv)k and the states (xobsv)k is obtained, with the imagenoise  k modeled with zero-mean Gaussian probability distributions:zk = G((xobsv)k) + k; (4.37)whereG(xobsv) =  u1 v1    un vn T ; (4.38)and the subscript k denotes the kth observation. Since the measurable pixelcoordinates of the image features are nonlinear functions of the 6-DoF pose ofthe target object, a nonlinear observer is required. Since the measurementsarrive frame-by-frame at  xed sampling intervals, the nonlinear observermust be implemented in discrete-time.4.3.4 Observer Model SummaryIn summary, the nonlinear discrete state-space model for pose estimationand tracking has the following parts:States: (xobsv)k =  (cwo)k (vo)k Input: (uobsv)k =  (vc)k Disturbance:  k =  1 k 2 k TNoise:  k =   k TNonlinear State Di erence Equations:(xobsv)k = F((xobsv)k 1;(uobsv)k 1) + k 1 (4.39)="((cwo)k 1) + t J 1   j(cwo)k 1 ((vo)k 1 (vc)k 1)((vo)k 1)#+ 1 k 12 k 1 (4.40)Nonlinear Output Equations:(zobsv)k = G((xobsv)k) + k: (4.41)51Chapter 4. Robust Pose Estimation4.3.5 Analysis of ObservabilityIt is di cult to obtain a closed form analytic solution for the conditionsthat guarantee complete observability for this nonlinear discrete system.Nonetheless, conditions for which it is not completely observable can bederived from the nonlinear di erence equations. Speci cally, the system willnot be fully observable when  =   2 , since the Jacobian J 1   j(cwo)k 1 loses rank and its inverse does not exist. Physically, when  =   2 , theaxis of rotation of  and the axis of rotation of  are collinear. From theimage output point of view, the two orientation states are indistinguishablefrom each other. Therefore, not all states are completely observable in thissingular con guration. To retain observability near singularities, a damped-least squares inverse [50] of the Jacobian is used, similar in form to themethod of restoring controllability (Appendix C).4.3.6 Extended Kalman Pose EstimationAn extended Kalman  lter is implemented to provide a near-optimal recur-sive update for the nonlinear discrete observer for the system presented inthis section. The complete set of equations for the implementation of EKF isdescribed in Appendix D for the reader’s convenience. For this EKF pose es-timation model, both the state di erence equations F((xobsv)k 1;(uobsv)k 1)and the output equations G((xobsv)k) must be linearized about the currentstate estimate at each iteration k.4.4 Relationship Between Observer & ControllerStatesThe states of the controller and the states of the observer are expressed us-ing di erent parameterizations, each in their own reference frames to bestsuit its own development and analysis. Recall:Controller States: ePBVS =  c tc  r Observer States: xobsv =  cwo vo TThe  nal step is to derive the relationship between the two sets of states.The reference signal for the controller is de ned as cw o, the desired relativepose of the object with respect to the camera frame:cw o = cX o cY o cZ o c  o c  o c  o T : (4.42)52Chapter 4. Robust Pose EstimationThe observed states can be converted into the states required by thecontroller using the following relationships:c tc =24cX ocY ocZ o35 Rdi 24cXocYocZo35; (4.43) r =cos 1 12Tr(Rdi ) 1 2 sin cos 1 12Tr(Rdi ) 1  ; (4.44)whereRdi = R 1(c o;c o;c o)R(c  o;c  o;c  o) =24r11 r12 r13r21 r22 r23r31 r32 r3335: (4.45)Note that vo does not appear in the calculation of ePBVS(t). Strictlyspeaking, the state-feedback controller only requires cwo to be observed.The purpose of including the velocity of the target object vo as part of thestates in the observer is to allow the observer to track a potentially movingtarget object. Camera self-motion is also incorporated as input into theKalman state prediction, so that it can better track the pose of the objectduring fast camera motions. Experimental results show that this methodo ers an improvement over the traditional approach of modeling all motionsas random disturbances.4.5 Simulation Results4.5.1 Purpose and SetupThe purpose of these simulations is to evaluate the image trajectories, theCartesian trajectories and the robot trajectories that result from visual ser-voing using online pose estimation. The observed target is a non-planarobject with 9 identi able feature points. The target object is positionedabove the robot in these experiments to increase the e ective range of mo-tion available to the robot for servoing, since robot joint limits are not ex-plicitly managed in the servoing control law. The simulations are performedon a 6-DoF CRS-A465 robot with a Hitachi KP-D8 camera amounted onthe end-e ector to re ect the equipment available in the lab.The  rst two robot positioning tasks are similar to those de ned in Sec-tion 3.3.1. The  rst is a translational task that requires the robot end-e ector to approach the target object with small out-of-plane rotations. The53Chapter 4. Robust Pose Estimationsecond is a rotational task of 180 degrees about the optical axis. Both tasksare executed by applying PBVS with respect to the stationary target frame.The resulting trajectories are compared against those obtained using IBVS,as presented in Section 3.3.2, to discern any improvements to the observedmotion.The third robot positioning task presents the camera  eld-of-view prob-lem that commonly a ects PBVS. This task is executed by applying PBVSwith respect to the camera frame, and involves signi cant out-of-plane rota-tions and coupled with large translational motions. Due to an initially poorpose estimate, the target object is brought outside of the camera’s  eld ofview during PBVS. An adaptive gains controller is tested with PBVS to de-termine whether the camera  eld-of-view constraints can be satis ed, whilesimultaneously completing the servoing task.The fourth robot positioning task evaluates the tracking performanceof PBVS with an EKF in the presence of severe image noise. The robotis commanded to maintain a relative pose with respect to a moving targetobject. The target object moves with a constant velocity of 0:1m=s in thex direction and the image noise has a standard deviation of  2 pixels.4.5.2 Position-Based Visual ServoingThe form of the state-feedback adaptive gains controller and that of the EKFobserver for position-based visual servoing (PBVS) is described in Sections4.2 and 4.3 respectively.Figure 4.1 shows the results from the  rst robot positioning task as de-scribed in Section 4.5.1 using PBVS for feedback control. The point ‘o’designates the start of the motion and the point ‘x’ designates the end ofthe motion. For this task (mainly translation with some small out-of-planerotations), PBVS generates acceptable image trajectories and Cartesian tra-jectories. Using PBVS, the image trajectories of the feature points are morecurved than the near-straight-line trajectories that are obtained using IBVS,so there is a signi cant chance that the image trajectories will exit the  eld-of-view. Recall that in PBVS, the control metric is expressed purely in theposition domain, so there is no direct control over the image trajectories.Nonetheless, the resulting Cartesian trajectory is extremely e cient, as itis a straight line connecting the camera start-position to the camera end-position. This property makes it easier to predict the motion of the camerausing PBVS for the purpose of collision avoidance and joint-limit avoidance.These results show that the trajectories generated with PBVS are sometimesacceptable, for large translational tasks with some out-of-plane rotations.54Chapter 4. Robust Pose EstimationFigure 4.1: PBVS of a task requiring translations and out-of-plane rotations:image trajectory (left) and Cartesian trajectory (right).Figure 4.2 shows the results from the second robot positioning task asdescribed in Section 4.5.1 using PBVS for feedback control. Unlike IBVSwhich forces the camera to retreat (resulting in the violation of joint limits),PBVS is able to correctly recognize that a 180-degree rotation is required,using the pose estimation obtained from the EKF. PBVS successfully avoidsthe problem of camera retreat. The Cartesan trajectories show that PBVSexecutes the required rotational motion, without inducing any extra trans-lational motion. This property of decoupled control illustrates one of themany bene ts of using pose estimation, when a model of the target objectis available, to simplify visual servoing motion. An additional advantage ofobtaining an accurate pose estimation through a robust method like EKF isthat it can be subsequently used for path planning.4.5.3 Camera Field-of-View ConstraintsResults from the third robot positioning task show that the adaptive-gainsPBVS controller presented in Section 4.2.3 works well for keeping the targetobject within the camera’s  eld of view. Figure 4.3 shows a visual servoingtask in which the target object exits the camera’s  eld of view when PBVS isused. In contrast, when the adaptive gains are applied, the rotational veloc-ities of the camera become severely attenuated as the object moves towards55Chapter 4. Robust Pose EstimationFigure 4.2: PBVS of a task requiring a rotation of 180 degrees about theoptical axis: image trajectory (left) and Cartesian trajectory (right).the camera’s periphery. The object is brought back towards the center ofthe image by the remaining translational degrees of freedom. Control of thecamera’s rotation resumes when the target object returns back to the centerof the region of interest. Although there are partial losses of visual featuresduring the motion, pose estimation is still possible through the modi cationto the EKF. The entries in the covariance matrix that correspond to the lostfeatures are increased to eliminate their in uence on the pose estimate. Theentries are returned to their normal values when the features return backinto the  eld of view. In order for this adaptive-gains approach to work,the visual updates from the camera must be su ciently fast, in order tocapture the violating image point as it enters into the high penalty regionof the camera’s periphery prior to its exit from the  eld of view. Using anexcessively wide penalty region tends to slow down the convergence of theoverall trajectory. The bell-curve parameters m = 5 and n = 5 were chosenexperimentally to give acceptable results.4.5.4 Visual Servoing Near Robot SingularitiesSimulations show that the visual servo control schemes become ill-conditionedand may result in large joint velocities when the camera is driven near thekinematic singularities of the robot. Path planning methods can be used toset up singularity-free waypoints for visual servoing. However, this requires56Chapter 4. Robust Pose EstimationFigure 4.3: Using adaptive gains with hierarchical control of camera trans-lation and rotation to keep the target object within the  eld of view: initialimage (top left); desired image (top right); target object leaves the  eld ofview under PBVS control (bottom left); target object remains in view usingadaptive gains with PBVS (bottom right).57Chapter 4. Robust Pose Estimationthe robot trajectory to be known ahead of time. In many visual servoingapplications, the trajectory of the camera is not known prior to servoing, someasures must be taken to account for unexpected trajectories through robotsingularities. In the simulations, robot singularities were addressed for vi-sual servoing using the damped least squares inverse method using EquationC.1 and Equation C.3 (from Appendix C). The maximum damping factor, max, was experimentally chosen to be 0.02, with the size of the singularregion, ", de ned to be 0.04 to give acceptable results. Larger values tendto bring the target object out of the  eld of view near singularities. Thismethod did not su er from numerical drift even when the inverse kinematicswas implemented in discrete time. The visual inputs acted as feedback tocorrect for positional errors, so that additional feedback correction termswere not required.4.5.5 Tracking performance of EFK and PBVSTo further evaluate the tracking performance of PBVS, a tracking task issimulated with a moving target object. The target object moves with aconstant velocity of 0:1m=s in the x direction and the image noise has astandard deviation of  2 pixels.The transient behavior of the EKF observer in the presence of imagenoise is shown in Figure 4.4. At the start of the task, the pose estimate tendsto oscillate about the true solution, indicating that the initialization for thestate covariance matrix is inaccurate. As the covariance matrix is re nedover the next iterations, the Kalman gain becomes more conservative, andthe estimates become more robust as they gradually converge towards thereal solution. It is found that an arbitrary initiation of the state covariancematrix to the identity matrix is su cient to achieve convergence. Figure 4.5shows the evolution of errors in the pose estimation.The transient behavior of the state-feedback adaptive gains PBVS con-troller while tracking a moving object is shown in 4.6. The oscillations inthe camera motion are directly related to the oscillations in the estimate ofthe object pose, caused by measurement noise in the images. It can be seenthat the accuracy of the camera pose (controller) is limited by the accuracyof the pose estimates (observer). The pose estimates converge quickly andthe camera motion becomes smoother as the true pose solution is reached.Unfortunately, a state-steady error still exists in the X component of thecamera pose due to the form of the proportional velocity controller. Thiserror can be reduced by the use of a proportional-integral controller in visualservoing. Figure 4.7 shows the evolution of pose-following errors.58Chapter 4. Robust Pose EstimationFinally, the transient behavior in the EKF estimate of the target object’svelocity is shown in Figure 4.8. Because the velocity states are not directlyobservable from the output equations, the estimates take many more itera-tions to converge. The target object’s velocity estimate is shown to convergeto 0.1 m/s in the X direction.4.6 SummaryAn extended Kalman  lter (EKF) is implemented in this chapter to providerobust estimates of the target’s 6-D pose from 2-D image measurements.Simulation results show the ability of the EKF observer to obtain accuratepose estimates despite the presence of large image noise and disturbances.The use of an object model and state tracking to regularize the pose estima-tion problem shows that signi cant improvements can be obtained over themethod of homography estimation and decomposition. The EKF observeralso produces a state covariance matrix, which is useful for quantifying thelevel of uncertainty in the target object’s pose. In contrast to the EKF ob-server implemented by Wilson et al. in [13], the EKF observer implementedhere incorporates camera self-motion as an input into the state predictionto improve tracking during fast camera movements. The absolute velocityof the target object is also tracked as a state, so that the object does nothave to be assumed to be stationary.Simulation results validate position-based visual servoing (PBVS) as auseful method for positioning an eye-in-hand robot with respect to a targetobject. The de nition of the error metric based on 3-D parameters results inan upper triangular Jacobian that permits the decoupled control of cameratranslation and rotation. Decoupled control allows an adaptive control lawto be designed to ensure that the target object remains within the  eld ofview during visual servoing. PBVS results in e cient Cartesian motions,although the robot’s joint-space may still be unfeasible. A method of han-dling robot singularities during visual servoing is also presented, using adamped least-squares inverse kinematic solution to restore controllability.However, the remaining issues related to the robot-related constraints suchas joint-limits constraints, joint-velocity constraints, and whole-arm colli-sion constraints still need to be addressed. In the next chapter, the poseestimate that is obtained from EKF will be used to enable joint-space pathplanning for satisfying these constraints.59Chapter 4. Robust Pose EstimationFigure 4.4: Transient behaviour of the EKF pose estimate: translation pa-rameters (top) and rotation parameters (bottom).60Chapter 4. Robust Pose EstimationFigure 4.5: Transient behaviour of pose estimation errors in the EKF: trans-lation errors (top) and rotation errors (bottom).61Chapter 4. Robust Pose EstimationFigure 4.6: Transient behaviour of the PBVS controller with an EKF ob-serving a target object moving at 0:1m=s in the X direction: camera position(top) and camera orientation (bottom).62Chapter 4. Robust Pose EstimationFigure 4.7: Pose-following errors in the PBVS controller with an EKF ob-serving a target object moving at 0:1m=s in the X direction: camera positionerrors (top) and camera orientation errors (bottom).63Chapter 4. Robust Pose EstimationFigure 4.8: Transient behaviour in the EKF estimate of the target object’svelocity: translation rate of change (top) and rotational rate of change (bot-tom).64Chapter 5Path Planning for ConstraintAvoidance5.1 IntroductionIn the previous chapters, it was shown that pose estimation can be used toprovide signi cant improvements to the predictability of the visual servoedrobot motion. Camera  eld-of-view constraints were addressed through theuse of an adaptive controller that acted as a  lter to establish hierarchi-cal control of the six degrees of freedom of the camera to restore visibility.A damped least-squares inverse Jacobian method was implemented to gen-erate acceptable trajectories while servoing near robot joint singularities.However, despite these improvements to the visual servo control law, theavoidance of constraints that are related to the robot, such as joint limitsand whole-arm collisions, could not be formally guaranteed.The chapter explores the use of path planning as a prerequisite to visualservoing for managing robot-related constraints. The idea is to executeas much of the required motion as possible using planned motion, whileusing visual servoing to compensate for uncertainties in the target object’spose, executed in two separate stages. A path planning stage is useful sincethere are a variety of techniques [51] [52] for handling constraints when therobot’s joint-space motion is known a priori, while the same problem is muchmore di cult for visual servoing since the generated motion is di cult topredict. Using this two-stage approach, the servoing motion can be kept toa minimum to avoid possible violations of constraints. The path planningmethods presented in this chapter work well for solving di cult problemsin collision avoidance, such as navigating a manipulator arm in a tightlycluttered workspace. In contrast, the next chapter considers a method thatallows planning and servoing to be executed simultaneously using modelpredictive control (MPC), to avoid whole-arm collisions with obstacles thatdo not tightly constrain the robot motion.65Chapter 5. Path Planning for Constraint Avoidance5.2 Path Planning RequirementsThe objective of path planning is to de ne a sequence of con gurationsthat a robot should follow in order to accomplish its positioning task, whilesatisfying  eld-of-view, joint-limit, and collision constraints. Path planningis typically executed o ine based on some knowledge of the relationshipbetween the robot and its environment. Typically, path planning occursin the robot’s joint-space, rather than in image-space, since many of thephysical constraints (such as whole-arm collision and robot joint-limits) areless sensitive to errors when they are expressed in the joint-space of therobot. Image-based planning techniques [30] [31] [32] based on homographydecomposition, potential  elds planning in Cartesian-space, and reprojectionback into the image, may work well for keeping the image trajectory inthe  eld of view, but they are insu cient for the avoidance of complicatedrobot-related constraints. Homography decomposition is shown to su er inaccuracy when the camera displacement is large. In the presence of errors,motion that is planned in the image-space may not correspond to the desiredmotion in the robot’s joint-space. Even if the homography decomposition iscompletely accurate, repulsive potential  elds applied to the position of thecamera can only prevent camera collisions with obstacles, while whole-armcollisions can still occur. Discussions on path planning in this chapter willconcern the joint-space of the robot.The planning of robot joint-space motion for camera positioning requiresa rough estimate of the target object’s pose. The methods for partial poseestimation using homography decomposition or complete pose estimationusing an extended Kalman  lter were discussed in the previous chapters.The estimated pose may or may not be accurate, so visual servoing mustbe used at the end of the planned motion to compensate for any modellingerrors. However, visual servoing can only be initiated with the target objectin view. The ideal scenario for the two-stage approach is where the targetobject is in the camera’s  eld of view at the end of the planned motion.However, there is no way of knowing this at the planning stage, since by thenature of the problem, the object’s actual location is uncertain. The desiredpath must be planned to keep the target object in view (based on its bestknown position) while reaching the goal, such that if the target object exitsa region of interest representing the  eld of view (due to poor initial assump-tions about it pose), the robot controller can immediately switch to visualservoing to resume control and \close the loop" to the end goal. To allowswitching at anytime, the robot uses a series of linearly interpolated way-points to achieve its planned motion, which can be interrupted between the66Chapter 5. Path Planning for Constraint AvoidanceMECH 598 SeminarModel Predictive Control (MPC) with Probabilistic Road Maps (PRM)p()q2Planned GoalActualGoalVisual ServoingPlannedMotionTarget Object About to Leave Field of Viewq1Planned MotionInitial ConfigurationCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate35gFigure 5.1: Two-stage approach for planning and servoing: joint-space pathplanning addresses collision and joint-limit constraints; visual servoing com-pletes the motion correcting for uncertainties.execution of waypoints. This two-stage approach for planning and servoingis summarized in Figure 5.1.5.3 Pose Calculations and ModelingLetFo be the canonical frame attached to the target object. The eye-in-handrobot starts at an initial con guration qinitial and provides an initial viewof the target object with a pose, represented by Fo, that may vary for eachinstance of robot servoing. For the purpose of the planning algorithm, it isassumed that the target object remains stationary with respect to the robotbase frame Fb during all robot motion. The goal is to position the cameraFc at a desired pose with respect to the target object Fo. This desiredpose is speci ed via an o ine teach-by-showing method. The pose of theobject in the camera frame in the initial and desired images, respectively(cTo)initial and (cTo)desired, are calculated using methods described in theprevious chapters (EKF pose estimation, homography decomposition withknown scale). There exists a  xed geometric relationship between the end-e ector frame Fe and the camera frame Fc, described by the homogeneoustransformation eTc. The target object is within the  eld of view of the67Chapter 5. Path Planning for Constraint AvoidanceeTc(bTe)initialeTc(bTe)desiredTc(e)initialbTo(cTo)desiredbTo(cTo)initialInitial ImageDesiredImagegDesired Image(ready‐to‐grasp position)Figure 5.2: Homogeneous transformations between coordinate frames usedfor path planning: initial end-e ector position (left) and desired end-e ectorposition (right). The corresponding images observed by the camera areshown at the bottom.camera at the start and at the end of the robot motion. A diagram of therelationship between coordinate frames is shown in Figure 5.2.The pose of the object with respect to the robot frame, bTo, can becomputed by the following:bTo = (bTe)initialeTc(cTo)initial; (5.1)bTo = (bTe)desiredeTc(cTo)desired; (5.2)where bTe is the homogeneous transformation mapping the robot base frameFb to the end-e ector frame Fe. This relationship is a function of the well-known forward kinematics of the industrial robot. The initial transformation68Chapter 5. Path Planning for Constraint Avoidanceis computed from the vector of the measured joint angles, q, as:(bTe)initial = frobot(qinitial): (5.3)It is necessary to determine the set of joint con gurations Qdesired whichcorrectly position the camera relative to the target object. As discussedin the following section, typical industrial robots have closed-form analyticsolutions to the inverse kinematics of its end-e ector. Let there be p distinctinverse kinematic solutions available:Qdesired =fq1;q2;:::;qpg (5.4)= f 1robot(bTe)desired (5.5)where (bTe)desired is computed through the following expression:(bTe)desired = (bTe)initialeTc(cTo)initial(cTo) 1desired(eTc) 1 (5.6)5.4 Inverse Kinematic Solutions for PathPlanningThe inverse kinematic calculations allow the path planning problem to beexpressed in the joint-space of the robot. For anthropomorphic robots withwith revolute joints such as the one depicted in Figure 5.2, there may bemultiple robot joint-space con gurations that satisfy the desired relativecamera pose. If collision constraints are satis ed for the motion betweenthe start and end robot con gurations (using a dynamic collision check-ing method presented in Section 5.5.2), then a linear joint interpolationmethod can be used to naturally satisfy robot joint-limit constraints, whileensuring e cient robot motion towards the chosen robot con guration. Forgiven upper-bounds on joint velocities set by the manufacturers of industrialrobots, this solution minimizes the travel time required for the motion.The method presented here is similar to the look-then-move approachdiscussed in Section 1.2 for robot bin-picking, with the exception that visualservoing is used at the end of the planned motion to correct for any mod-elling errors or pose estimation errors. In the presence of errors, the choseninverse kinematic solution must attempt to provide continuous visibility ofthe target object throughout the interpolated path, so that if the target ob-ject exits a region of interest (ROI) representing the camera’s  eld of view69Chapter 5. Path Planning for Constraint Avoidanceat any time when the planned motion is executed, visual servoing can beimmediately activated to \close the loop" to the end goal.It is well known that, due to the robot’s mechanical joint limits, not allclosed-form inverse kinematic solutions correspond to physically realizablecon gurations. Once a mathematical solution is identi ed, it must be fur-ther checked to determine whether it satis es all constraints on the robot’srange of joint motions. An extension of this requirement, for eye-in-handrobots visual servoing, is to check whether the planned interpolated trajec-tory for each inverse kinematic solution satis es the  eld-of-view constraintsof the camera for a given target object. Although the  nal camera pose isidentical for all inverse kinematic solutions, the resulting interpolated cam-era trajectories are quite di erent for each of these joint con gurations, andsome of the interpolated trajectories may bring the target object outside ofthe  eld of view.A criterion for selecting an appropriate inverse kinematic solution forjoint-space path planning is that its interpolated trajectory must keep thetarget object within the camera’s  eld of view, preferably in manner that isrobust to disturbances. The most robustly visible inverse kinematic solutionis de ned as the one that maximizes the distance between the target objectand the camera  eld-of-view limits during the interpolated motion. The aimis to select a robot con guration, which provides the largest bu er againsttarget pose estimation errors, to ensure that a continuously visible path willmost likely result during the execution of the planned path. This selectioncriteria is described more rigorously in the next section.5.4.1 Visibility ModellingLet the target object consist of a number of visual features. The image co-ordinates (u;v) of the n visual features are functions of the joint coordinatesof the robot, where:u =  u1(q)    un(q) T ; (5.7)v =  v1(q)    vn(q) T : (5.8)Let the robot path be parameterized by q( ) with  as the path vari-able, where qinitial = q(0) and qdesired = q(1). Continuous visibility con-straints require that all features remain within the  eld of view of the camerathroughout the interpolated robot path. The problem of selecting the mostrobustly visible inverse kinematic solution is formulated as follows:70Chapter 5. Path Planning for Constraint AvoidanceCollaborative Advanced Robotics &IntelligentSystemsLaboratoryResults: Visual Trajectories fromInverseKinematicSolutions& Intelligent Systems Laboratoryfrom Inverse Kinematic SolutionsInverse Kinematic Solutions 1-4(1)(2)Not Shown:Solutions 5-8, since they violate the robot joint limits (3)(4)||dq|| = 3.6751||dq|| = 5.1867(3)(4)Eight Virtual Featuresdefine the Vertices of the BdiB||dq|| = 2.8607||dq|| = 4.4267Trajectory Specification via Sparse Waypoints for Eye-in-Hand Robots Requiring Continuous Target VisibilityA. Chan, E.A. Croft and J.J. Little11Bounding BoxFigure 5.3: Oriented bounding box used to represent the target object for eld-of-view visibility planning.argmaxqdesired2Qdesired0BB@min0BB@ju(q( )) uminj;ju(q( )) umaxj;jv(q( )) vminj;jv(q( )) vmaxj1CCA1CCA; (5.9)such thatumin  u(q( )) umax; 8 ; (5.10)vmin  v(q( )) vmax; 8 ; (5.11)where umin, umax, vmin, vmax are de ned by the image region of interest(ROI) representing the  eld of view of the camera.To extend this method for handling objects with complicated features(such as areas, lines, and image moments), an oriented bounding box isde ned with respect to the target object frame Fo such that it encloses thetarget object and all its relevant visual features, as shown in Figure 5.3. Aunion of bounding boxes is used to increase the planning resolution wherepossible. In the presence of possible self-occlusion by the target object,this method implicitly assumes that minimum subsets of visual features canalways be observed around the object (when it is within the  eld of view) foruse in visual servoing. The location of the corners of the bounding box(es)are tracked to bound the visual location of the target object. The targetobject is guaranteed to remain within the  eld of view of the camera if thecorners of the bounding box(es) are continuously visible.71Chapter 5. Path Planning for Constraint Avoidance5.4.2 Simulation ResultsA vision-guided positioning task is simulated using a model of the CRS-A465 robot with a Sony XC-HR70 camera mounted on its end-e ector. Thetarget object is a 20cm 20cm 20cm box made up of eight corner featuresplus the geometric centroid. The goal is to select an inverse kinematic so-lution that keeps all these features inside the camera’s  eld of view duringthe interpolated robot motion. The camera starts from an initial overheadview, and is required to approach the target object while performing sig-ni cant out-of-plane rotations. Of the eight closed-form inverse kinematicsolutions that are geometrically available to the CRS-A465 robot to achievethe desired camera pose, four are outside of the robot’s mechanical jointlimits. The image trajectories corresponding to the remaining four inversekinematic solutions are shown in Figure 5.4. The point ‘o’ designates thestart of the motion and the point ‘x’ designates the end of the motion. Themotion is executed using linear joint-space interpolation. Note that onlytwo of the four solutions give satisfactory visual trajectories. Following theconstrained optimization criteria de ned in Section 5.4.1, the solution in thebottom left is chosen for joint-space path planning.If no feasible trajectory (for ensuring target visibility) exists among theinverse kinematic solutions, then additional intermediate robot con gurationmust be speci ed to guide the robot towards its goal, while positioning thetarget object within the camera’s  eld of view at each con guration. Theinsertion of an additional visible robot con guration divides the  eld-of-viewproblem into two sub-problems of the same form but of lesser di culty, sincethe robot’s interpolation distance is reduced. The problem of generatingintermediate robot con gurations that position the target object within the eld of view is discussed in Section 5.6 within the framework of probabilisticroadmaps (PRM).5.5 Probabilistic RoadmapsProbabilistic roadmaps (PRM) [51] [52] have emerged, over the past decade,as one of the popular methods for robot path planning to simultaneouslymanage multiple constraints. The intuition behind PRM consists of ran-domly generating a large sample of robot con gurations in joint-space, sothat various paths can be planned between these con gurations for con-straint avoidance. The sampled robot con gurations make up the verticesin an undirected graph. In the graph, two vertices are connected by an edgeif the trajectory between the two vertices satis es a set of constraints. For72Chapter 5. Path Planning for Constraint AvoidanceFigure 5.4: Image trajectories resulting from four di erent inverse kinematicsolutions: two solutions result in the target object leaving the  eld of view(top), two solutions result in a satisfactory image trajectories (bottom).73Chapter 5. Path Planning for Constraint AvoidanceMECH 598 SeminarModel Predictive Control (MPC) with Probabilistic Road Maps (PRM)p()q2Planned Goal ConfigurationActualGoal ConfigurationPbbilitiRdM(PRM)q1Initial ConfigurationProbabilistic Road Map(PRM)Collision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate32gFigure 5.5: PRM representation of the robot path planning problem: nodesrepresent feasible robot con gurations, edges represent feasible robot mo-tions.each type of constraint, there exists an e cient algorithm to check whetherthe constraint is satis ed, as presented in the next section. Once several con-nections of vertices exist between the starting robot con guration and thegoal robot con guration, the problem becomes a path search in a undirectedgraph with edges weighted by joint-space distance or some other metric. Di-jkstra’s algorithm, a well-known graph search technique, is used to providethe most e cient path through the roadmap. An illustration of the PRMframework in two-dimensional joint-space is presented in Figure 5.5.5.5.1 Robot Joint-Limit ConstraintsPath planning in the joint-space of the robot allows a natural representationof joint-limits as a hyper-rectangular region in which feasible robot con g-urations exists. Robot joint-limit constraints are handled by restricting thesampling of vertices to within the hyper-rectangular region. A separate al-gorithm is not required to check whether robot-limit constraints are satis edbetween two adjacent vertices.74Chapter 5. Path Planning for Constraint Avoidance5.5.2 Whole-Arm Collision ConstraintsA dynamic collision checking (DCC) algorithm proposed by Schwarzer etal. [53] is used to test for collisions between the robot and obstacles in theworkspace. DCC determines if a robot collides with the environment duringa motion between two con gurations. Speci cally, given the initial con gu-ration qi and the  nal con guration qf, DCC determines if the trajectoryde ned by q(t) = qi + (qf qi)t for 0 t 1 results in a collision.As a prerequisite, DCC requires the robot con gurations at the two ver-tices to be collision-free. To guarantee collision-free motion in between thecon gurations, DCC exploits the relationship between the shortest distancebetween the robot and any obstacle, and the longest distance traveled byany point on the robot during a trajectory. Let d(qi) be the shortest dis-tance between an obstacle and the robot in the con guration qi. Similarly,let d(qf) be the shortest distance between an obstacle and the robot in thecon guration qf. Also, given a trajectory q(t), let ‘(q(t)) be the the longestdistance traveled by any point on the robot during the trajectory. That is,as the robot moves according to q(t) each point on the hull of the robotdescribes a 3-D curve and ‘(q(t)) represents the length of the longest ofthese curves. Then, given d(qi), d(qf) and ‘(q(t)), DCC determines thatthe manipulator does not collide with the obstacle if‘(q(t)) <d(qi) +d(qf) (5.12)The intuition behind this inequality is that if every point on the robottravels a short distance during q(t) and the robot is far enough from anobstacle, then it is impossible for the robot to collide with the obstacle.Equation (5.13) is a su cient condition, but it is not a necessary condition.If Equation (5.13) is not satis ed, the algorithm proceeds by dividing thetrajectory q(t) in two trajectories q(u) for 0 u<t=2 and q(v) for t=2 v 1 and Equation (5.13) is evaluated for both trajectories. This procedure isapplied recursively until every trajectory satis es Equation 5.13 or a collisionis detected in the process.Further details on the DCC algorithm are presented in the next chapter,where the speci c form of ‘(q(t)) is discussed within the context of modelpredictive control (MPC).5.5.3 Camera Field-of-View ConstraintsA dynamic visibility checking (DVC) algorithm proposed by Leonard et al.[54] is used to test for visibility of a target object within the camera’s  eld of75Chapter 5. Path Planning for Constraint Avoidanceview. The hollow frustum formed by the camera’s  eld of view is modelledas an physical extension of the robot manipulator. Using a method basedon the DCC algorithm, DVC determines if the hollow frustum \collides"with the target object of interest, bringing the target object outside of thecamera’s  eld of view. Speci cally, given the initial con guration qi and the nal con guration qf, DVC determines if the trajectory de ned by q(t) =qi + (qf qi)t for 0  t 1 results in a \collision" between the camera’sfrustum and the target object.A visible vertex is de ned here as a static robot con guration that hasthe target object within the camera’s  eld of view. DVC requires the twovertices of interest to be visible, similar in form to the prerequisite of theDCC algorithm for collision-free motion. To guarantee visibility between thecon gurations, DVC exploits the relationship between the shortest distancebetween the target object and the camera frustum, and the longest distancetraveled by the target object with respect to the camera’s moving frame ofreference during a trajectory. Let ed(qi) be the shortest distance betweenthe target object and the camera frustum in con guration qi. Similarly, leted(qf) be the shortest distance between the target object and the camerafrustum in con guration qf. Also, given a trajectory q(t), let e‘(q(t)) be thethe longest distance traveled by any point on the target object with respectto the camera’s frame of reference during the trajectory. That is, as thecamera moves according to q(t), each point on the target object describesa 3-D curve with respect to the camera and e‘(q(t)) represents the lengthof the longest of these curves. Then, given ed(qi), ed(qf) and e‘(q(t)), DVCdetermines that the target object does not \collide" with the frustum formedby the camera’s  eld of view ife‘(q(t)) < ed(qi) + ed(qf) (5.13)The intuition behind this inequality is that if every point on the targetobject travels a short distance with respect to the camera during q(t) and thetarget object is far enough from frustum representing the  eld of view limits,then it is impossible for the target object to \collide" with the frustum andleave the camera’s  eld of view. Equation (5.13) is a su cient condition,but it is not a necessary condition. If Equation (5.13) is not satis ed, thealgorithm proceeds by dividing the trajectory q(t) into two trajectories q(u)for 0 u<t=2 and q(v) for t=2 v 1 and Equation (5.13) is evaluated forboth trajectories. This procedure is applied recursively until every trajectorysatis es Equation 5.13 or a \collision" with the frustum is detected in theprocess.76Chapter 5. Path Planning for Constraint Avoidance5.6 Dynamic Generation of Visible RobotCon gurations5.6.1 MotivationIn many PRM implementations, a static roadmap is constructed only once(or very infrequently) to capture the relationship between the robot andthe con guration of obstacles in the workspace. Assuming that the robot’sworkspace does not change and that is not extremely cluttered, it is notdi cult to  nd static robot con gurations (vertices) that are collision-free byrandom sampling. Many implementations of PRM generate their vertices bymethod of uniform random sampling, or by biased random sampling basedon a proximity metric. Collision-free vertices are prerequisites to the DCCalgorithm for determining if the motions in between the vertices are alsocollision-free. So, only vertices that correspond to collision-free static robotcon gurations are retained in the graph.Visible vertices are prerequisites to the DVC algorithm, which deter-mines if the motions in between vertices keep the target object within thecamera’s  eld of view. This prerequisite is more di cult to satisfy in prac-tice, especially if the target object’s location changes very frequently. Inmany vision-based applications, such as robot bin-picking as described inSection 1.2, the target object’s location changes at every pick instance, whilethe workspace of the robot remains static. Removal of all vertices that arenot visible with respect to a speci c target object typically results in a rathersparse graph for path planning. Continued use of the PRM may require thegeneration of new vertices (robot con gurations) that are speci c to the poseof the target object. These new vertices correspond to robot con gurationsthat position the target object within the camera’s  eld of view, while pro-viding a visible joint-space path to connect the start con guration to thegoal con guration. A method of generating and selecting these new verticesdynamically for a target object in any given pose is presented in the nextsection.5.6.2 Vertices Generation using Virtual IBVSThe goal of dynamic vertex generation is to provide a systematic method ofcreating, in the PRM, robot con gurations that:1. Keep the target object within the camera’s  eld of view;2. Provide a connecting path from the starting con guration to the end77Chapter 5. Path Planning for Constraint Avoidancegoal con guration.These vertices must be subsequently checked for the satis cation of col-lision constraints, using the DCC algorithm discussed in Section 5.5.2. Asecondary purpose of these vertices is to act as location biases for furthercon guration sampling in the surrounding joint-space region if the collisionconstraints cannot be met using the original vertices.With respect to camera  eld-of-view constraints, the ideal image trajec-tory of a given feature point is one that travels in a straight line from itslocation within the initial image to its location within the desired image. Ifsuch a trajectory could be realized, then the convexity of the image-spaceguarantees that the feature point remain inside the camera’s  eld of view;namely the IBVS approach.Here, the idea is to apply IBVS virtually with respect to the targetobject, in order to generate candidate visible vertices for PRM planning.However, the joint-space motion imposed by IBVS can be extremely con-voluted; the sequence of con gurations do not necessarily bring the robotcloser to its joint-space goal con guration, as in the problem of camera re-treat. Thus, it is desirable to \short circuit" the IBVS path in joint-spacewhenever possible, in order to generate feasible and e cient robot motion.A discussion of virtual IBVS failure is given in Section 5.6.4IBVS is virtually applied with respect to the object’s feature points togenerate a set of candidate visible vertices for selection. In the case ofcomplex objects, the corners of the oriented bounding box enclosing thetarget object are used as the image features, as discussed in Section 5.4.1.The control law for IBVS is described in Section 3.3.2.5.6.3 Vertices Selection using a Joint-Space MetricLet the IBVS trajectory be parameterized by qIBVS( ) with  as the pathvariable, where qinitial = qIBVS(0) and qdesired = qIBVS(1). The problem ofvertex selection can be formulated as:argmaxqIBVS( ) (5.14)such thatkqIBVS( ) qIBVS(0)k= 12kqIBVS(1) qIBVS(0)k: (5.15)78Chapter 5. Path Planning for Constraint AvoidanceA backward search algorithm is initiated from the end robot con gura-tion, selecting the  rst point along the IBVS trajectory where the joint-spaceerror is half of that between the start and end con guration. If the joint-space path is convoluted, the waypoint is asymmetrically biased towards tothe end con guration rather than the start con guration. Due to the linearapproximation used in the proportional control law in IBVS, joint-space tra-jectories near the end con guration are typically less convoluted than thoseat the start of the servoing path. A graphical illustration of this vertexselection method in two-dimensional joint-space is shown in Figure 5.6.The two joint-space segments created by the selected vertex are checkedfor continuous visibility using DVC. The backward search algorithm is ap-plied recursively for each segment, generating new visible vertices with re-spect to a joint-space metric, until the continuous visibility criterion is metin DVC. Typically, one or two visible vertices (selected from the IBVS path)are all that are required to bring the target back into view and to providecontinuous target visibility between robot con gurations. In the presence ofuncertainties in the pose estimate and in the planning model, the robustnessof this planning method is increased by reducing the size of the region ofinterest (ROI) representing the camera’s  eld of view. The reduced  eld ofview creates additional vertices to sub-divide the joint-space motion, forc-ing the camera to take more conservative motions. The advantage of themethod of vertex selection over the execution of the entire IBVS path isthat the joint-space motion can be shortened whenever possible, resultingin feasible and e cient robot motion.5.6.4 Addressing Virtual IBVS FailureSimulations show that virtual IBVS fails to converge if the trajectory passesnear robot singularities, even though actuation limits do not exist in simula-tion. In the presence of numerical integration errors, the high joint velocitiesthat are generated near singularities tend to bring the trajectory (both inimage-space and in joint-space) away from the desired solution, bringing thetarget object outside of the  eld of view. A damped least-squares inversekinematics solution [50] is implemented to allow virtual IBVS near robotsingularities. Details on the implementation of this method is discussed inSection 4.5.4.Nonetheless, virtual IBVS will fail if joint limits are reached (due toin nite camera retreat), or if the trajectory gets stuck in a local minima (dueto poor selection of image features). In all planning scenarios, the camerapose that is achieved by virtual IBVS is compared with the desired camera79Chapter 5. Path Planning for Constraint Avoidancepose to determine whether the solution converges. In the event of virtualIBVS failure, the vertex selection algorithm is modi ed and applied to thepartial IBVS trajectory. Let the partial IBVS trajectory be parameterizedby qIBVS( ) with  as the path variable, where qinitial = qIBVS(0) and qfail =qIBVS(1). The problem of vertex selection from a partial IBVS trajectory isformulated as:argminqIBVS( )kqIBVS( ) qdesiredk (5.16)A forward (regular) search algorithm is applied to the partial IBVS tra-jectory, selecting the con guration that is closest to the goal con guration injoint-space as the new vertex. The two joint-space segments created by theselected vertex are checked for continuous visibility using DVC. Where a fulltrajectory exists, Equation 5.14 is used to select the visible vertex; wherea partial trajectory exists, Equation 5.16 is used. Because the selection ofvisible vertices is based on a joint-space metric, useful vertices can still befound amongst candidates that bring the robot closer to its con gurationgoal from a partial IBVS trajectory. A graphical illustration of this vertexselection method in two-dimensional joint-space is shown in Figure 5.7.5.7 Simulation ResultsThe vision-guided positioning task is simulated using a model of the CRS-A465 robot with a Sony XC-HR70 camera mounted on its end-e ector. Thetarget object is a 20cm 20cm 20cm box made up of eight corner featuresplus the geometric centroid. The goal is to plan a path, via the insertionof visible robot con gurations, to keep all these features inside the camera’s eld of view during the interpolated robot motion. The camera starts froman initial overhead view, and is required to approach the target object whileperforming signi cant out-of-plane rotations. To account for uncertainties inthe pose estimate and in the planning model, the focal length of the cameramodel used for planning is modi ed to reduce the camera’s  eld of viewby 1=3 in each dimension. This has the same e ect as de ning a region ofinterest (ROI) representing the camera’s reduced  eld of view for visibilityplanning in the presense of increased uncertainty.With the reduced  eld of view, all inverse kinematic solutions fail tomaintain continuous visibility on the target object, when using linear in-terpolation to generate robot motion. The inverse kinematic solution that80Chapter 5. Path Planning for Constraint AvoidanceCollaborative Advanced Robotics &IntelligentSystemsLaboratoryIBVS Joint Space Trajectory& Intelligent Systems LaboratoryChoose a sparse set of waypoints among candidates generated from Virtual IBVS-To reduce data communication overheadq2IBVSaccuracy is asymmetrically biased towards theendconfiguration(linearapproximation)-To remove constraints on joint-space path such that it can be more efficiently executed1argmax()fqqthe end configuration (linear approximation)qdesiredqinitialqvertexBackwards Search1 2.()(0)(1)(0)stffffInsert additional visible + feasiblewaypointq(ζ)Joint Space Trajectory with Virtual IBVSq1Divides problem into two sub-problemsof same form but of lesser difficulty due to reduced joint distancesApply recursivelyuntil visibility criterion is met.Trajectory Specification via Sparse Waypoints for Eye-in-Hand Robots Requiring Continuous Target VisibilityA. Chan, E.A. Croft and J.J. Little23Figure 5.6: Using the IBVS joint-space trajectory to select candidate visiblevertices while reducing joint-space motion.Collaborative Advanced Robotics &IntelligentSystemsLaboratoryIBVS Failure& Intelligent Systems LaboratoryChoose a sparse set of waypoints among candidates generated from Virtual IBVS-To reduce data communication overheadq2IBVSaccuracy is asymmetrically biased towards theendconfiguration(linearapproximation)-To remove constraints on joint-space path such that it can be more efficiently executed1argmax()fqqthe end configuration (linear approximation)qdesiredqinitialRobot Joint Limitqfail1 2.()(0)(1)(0)stffffInsert additional visible + feasiblewaypointqvertexq(ζ)PartialJoint Space Trajectory with Virtual IBVSq1Divides problem into two sub-problemsof same form but of lesser difficulty due to reduced joint distancesApply recursivelyuntil visibility criterion is met.Trajectory Specification via Sparse Waypoints for Eye-in-Hand Robots Requiring Continuous Target VisibilityA. Chan, E.A. Croft and J.J. Little24Figure 5.7: Using a partial IBVS joint-space trajectory to select candidatevisible vertices while reducing joint-space motion.81Chapter 5. Path Planning for Constraint AvoidanceFigure 5.8: Image trajectories resulting from direct joint-space interpolation:target object partially exits the  eld of viewbest keeps the target with the camera’s  eld of view during most of therobot motion is shown in Figure 5.8. The point ‘o’ designates the start ofthe motion and the point ‘x’ designates the end of the motion. Note thatseveral feature trajectories exit the  eld of view at the bottom of the image.Virtual IBVS is applied to the target object to generate visible robot con g-urations for vertices selection in PRM planning. The corresponding IBVSimage trajectory is shown in Figure 5.9. All features trajectories remaininside the  eld of view by the design of the control. Using the vertices se-lection method outlined in Section 5.6.3, a visible robot con guration fromthe IBVS trajectory is chosen as a vertex for path planning. This vertexbrings the target object back into the  eld of view. In this example, the cho-sen vertex also provides continuous target visibility during the interpolatedrobot motion between the three robot con gurations (start, intermediate,end). The image trajectory resulting from the insertion of this intermediaterobot con guration is shown in Figure 5.10.5.8 SummaryThe use of path planning is discussed in this chapter, as a prerequisite tovisual servoing, for managing the multiple constraints that an eye-in-hand82Chapter 5. Path Planning for Constraint AvoidanceFigure 5.9: Image trajectories resulting from virtual IBVS: target objectstays within the  eld of viewFigure 5.10: Image trajectories resulting from the insertion of a visible ver-tex, chosen from the virtual IBVS trajectory using the proposed joint-spacemetric: target object stays within the  eld of view.83Chapter 5. Path Planning for Constraint Avoidancesystem may encounter when the required robot motion is large. A two-stageapproach is presented for planning and servoing. Path planning is used toaddress collision and joint-limit constraints, while visual servoing is usedat the end of the planned motion to correct for uncertainties. Since visualservoing can only be initiated with the target object in view, the plannedpath must continuously keeps the target object within the camera’s  eldof view to allow a feasible transition. In the presence of modelling errors,continuous visibility allows the robot controller to switch to visual servoingto \close the loop" to the end goal, in the event that the target objectunexpectedly begins to leave the camera’s  eld of view during the executionof the planned path. A visible robot con guration is de ned in this chapteras a robot con guration that positions the target object of interest withinthe camera’s  eld of view.Path planning must occur in the robot’s joint-space, rather than inimage-space, in order to accurately capture the constraints related to workspacecollision and robot joint-limits. Selection of an appropriately visible in-verse kinematic solution allows vision-guided path planning to occur in thejoint-space of the robot. Probabilistic roadmaps (PRM) provide an e cientframework for path planning in the presence of multiple constraints. For atarget object in any given pose, a method of generating new vertices in thePRM that correspond to visible robot con gurations is presented. Thesevertices act as location biases for further con guration sampling in the sur-rounding joint-space region if collision constraints are not met. This methodof vertices generation is inspired by image-based visual servoing (IBVS),and it uses a joint-space metric to select visible robot con gurations thatalso bring the robot closer to its end goal con guration. Simulations showthat the insertion of a small number of visible intermediate robot con gura-tions provide the su cient conditions to ensure continuous target visibilitythroughout the entire planned path. The resulting continously visible pathallows a feasible transition from planned motion to visual servoing.In summary, the idea behind this two-stage approach is to execute asmuch of the positioning task as possible using planned joint-space motion,so that the remaining visual servoing motion can be kept to a minimum,thus reducing unexpected motions that may violate robot joint constraints.However, two problems still exist in this approach:1. If the target object exits the  eld of view early on during the plannedpath (due to an extremely poor initial estimate of the target object’spose), then all path planning is voided, since the remaining robotmotion must be executed using visual servoing;84Chapter 5. Path Planning for Constraint Avoidance2. When standard visual servoing techniques are used at the end of theplanned motion to \close the loop" to the end goal, there is still noguarantee that the resulting motion will not violate constraints.These two issues are addressed in the next chapter, using a visual servocontrol law based on Model Predictive Control (MPC).85Chapter 6Model Predictive Control6.1 IntroductionIn the previous chapters, it was shown that pose estimation coupled with aninverse kinematic solution that satis ed path visibility constraints, alloweda part of the visual servoing task to be planned in joint-space to addressrobot-related constraints. One drawback with the proposed two-stage ap-proach was that the uncertainty in the pose of the target object makes itimpossible to predict how much of the vision-guided task will be executedusing planned motion and how much will be executed using visual servo-ing. More importantly, despite the constant availability of visual feedbackand availability of constraint-based models, errors still remained uncorrectedthroughout the  rst stage (planned motion), and avoidance of constraintswas still not guaranteed in the second stage (visual servoing).This chapter presents a novel visual servo control law, based on ModelPredictive Control (MPC), to address the de ciencies of previous planningand servoing approaches. MPC visual servoing provides a uni ed frameworkfor managing both camera-related ( eld of view) and robot-related (joint-limit, joint-velocity, whole-arm collision) constraints associated with eye-in-hand robotic systems. MPC solves the problem of joint-space path planningand eye-in-hand visual servoing simultaneously, by formulating the visualservoing task as a  nite horizon, open-loop, optimal control problem withconstraints that is solved online. At each control iteration, visual feedbackis used to compensate for errors and to re ne the prediction model. Theiterative process of planning and servoing in MPC results in trajectories thatare far less sensitive to the propagation of modelling errors, compared withthe two-stage approach. The MPC framework produces a truly constraint-aware visual servo control law, which can be used to drive large-range robotmotions to achieve close-loop positioning.Although the use of predictive control for visual servoing has been pro-posed recently in [45], [46], [47], none of these methods address the problemof constraint avoidance for eye-in-hand visual servoing. To the author’sknowledge, the use of MPC for simultaneous visual servoing and joint-space86Chapter 6. Model Predictive Controlplanning to avoid whole-arm collisions (amongst other system constraintsrelated to an eye-in-hand robot) is a  rst contribution in the  eld. Thischapter also presents original work in methods for addressing uncertainty inthe target object’s location when using vision-guided path planning methods(such as PRM, as presented in Chapter 5) with MPC.This chapter opens with a basic review of the MPC formulation for thecontrol of a multiple-input multiple-output (MIMO) system. Following, themodelling of the system and the implementation of a MPC controller foreye-in-hand visual servoing is described. Simulation results are given foran eye-in-hand positioning task to demonstrate the successful avoidance ofconstraints during MPC visual servoing. The e ects of the various tuningparameters are discussed. The next section describes a method which ex-ploits the DCC algorithm (previously discussed in 5.5.2) to obtain a usefulrepresentation of the collision-free space for MPC visual servoing. Integra-tion of MPC with PRM, including a method to address uncertainty in thetarget object’s pose is presented next. Integration of MPC with DCC toarrive at an online iterative planning and servoing method is also presented.Experimental results are given for an eye-in-hand visual servoing task in thepresence of workspace obstacles and uncertainty in the target object’s poseto demonstrate the e ciency of the approach.6.2 MPC FormulationThe basic idea of MPC is to use the predictive power of a model to choosea sequence of control actions that is optimal (according to some criterion)for achieving a desired output, while compensating for any prediction er-rors using feedback generated from the observation of the real plant. MPCis formulated to solve, online, a  nite horizon, open-loop, optimal controlproblem. This optimization is subject to the system dynamics and the con-straints related to the states and inputs of the system. An illustration ofthe basic principle of MPC is shown in Figure 6.1.The MPC problem for computing an optimal control sequence is formu-lated as follows:minuijkC( ijk;uijk); (6.1)87Chapter 6. Model Predictive ControlPastFuture(Prediction)PastFuture (Prediction)Desired OutputPredicted outputClosed‐loop output yClosed‐loop iOptimal controlinputukk+δk+δNk+δNkk +δtk +δt Nck +δt NpPrediction horizon NpControl horizon NcFigure 6.1: The basic principle of model predictive control.subject toxi+1jk = f(xijk;uijk) (6.2) ijk = (y(xijk) yd) + dk; i2[1;Np] (6.3)xijk2X; i2[1;Np] (6.4)uijk2U; i2[1;Nc] (6.5)wherex0jk = xk (6.6)uijk = uNcjk; 8i Nc (6.7)dk = y(xkjk 1) yk (6.8)At each iteration k, the MPC controller uses an internal dynamic model(Equation 6.2) to predict the behaviour of the plant over a future predictionhorizon Np of  xed length. The predicted behaviour depends on the currentstate xk and on the assumed sequence of inputs uijk that are applied overa control horizon Nc, where X and U are the sets of feasible inputs and88Chapter 6. Model Predictive Controlstates, respectively. The idea is to select the sequence of inputs that bestforces the predicted outputs y(xijk) to converge to the desired set-point yd,subject to a set of constraints related to the plant. When evaluating thesequence of prediction errors  ijk, a disturbance (model) correction term dkis applied to capture the discrepancy between the latest plant output yk andthe latest model output y(xkjk 1). Once an optimal sequence of inputs ischosen, only the  rst element of the sequence is applied as the input signalto the plant. The whole cycle of output measurement, prediction, and inputtrajectory selection is repeated one sampling interval later. The predictionhorizon remains the same length as before, but slides along by one samplinginterval.The main advantage of MPC lies in its ability to routinely handle con-straints. MPC is the only advanced control technique (that is, more ad-vanced than standard PID control) to have had a signi cant impact in in-dustrial process control, especially where control update rates are infrequentand operation near constraints is necessary to obtain e cient processes.However, it should be noted that proof of closed-loop stability of MPC con-trol of a nonlinear system is still an open research problem. Further detailson MPC theory and analysis can be found in [55].6.3 Eye-in-Hand Visual Servoing using MPCThe MPC framework is implemented to produce a constraint-aware con-trol law for eye-in-hand visual servoing. This section describes the variouscomponents of the MPC visual servoing controller, including the modelling(robot model, camera model, object model), the constraints of the system,and the cost function to be optimized.6.3.1 System ModellingThe following shorthand notation is used to refer to coordinate frames thatare used for system modelling: Robot base frame, Fb; Robot end-e ector frame, Fe; Camera frame, Fc; Target object frame, Fo.89Chapter 6. Model Predictive ControlObject ModelThe object model consists of number of feature points, whose coordinates(oXj;oYj;oZj) are de ne with respect to a canonical object frame. Fora target object made up of n feature points, the object model used forprediction is:oPj =2664oXjoYjoZj13775; j2[1;n]: (6.9)Let bTo de ne the homogeneous transformation that expresses the co-ordinates of the object frame in the coordinates of the robot base frame.Alternatively, where it is more convenient for eye-in-hand pose estimation,cTo can be used to de ne the homogeneous transformation that expressesthe coordinates of the object frame in the coordinates of the camera frame.Robot ModelA forward kinematic model of the robot is used to describe the homogeneoustransformation bTe that expresses the coordinates of the end-e ector framein the coordinates of the robot base frame. bTe is a function of robot jointpositions:bTe = frobot(q): (6.10)For MPC controller design, it is assumed that a low-level controller existsto stabilize the internal dynamics of the robot. This is achieved through thecontrol of motor torques to satisfy the generalized equations of motion forrobots with revolute joints and serial links: = Dq(q) q + Cq(q; _q) + g(q): (6.11)For industrial robots with highly-geared joints to reject external torquedisturbances, independent PID controllers are used to generated the appro-priate torque  to track a smooth position reference signal for each joint.For proposed service robots, which have back-drivable joints that are notgeared (for safety reasons), an inverse dynamic model of the robot is usedto provide feedback linearization. Within this context, the objective of theMPC controller is to generate feasible robot trajectories in the form of jointpositions, velocities, or accelerations that e ciently drive the robot towards90Chapter 6. Model Predictive ControlEye‐in‐Hand System(uv)*Robot Model Camera ModelTime Step Prediction Horizon Control HorizonModel Predictive ControllerReference ImageLow‐Level Robot ControllerObserved Imageq* q(u,v)*Cost Function:Image Error, Input Penaltiesfg(u,v)g,pRobot Joint Position LimitsCameraField‐of‐ViewLimitsSystem Constraints:Camera Field‐of‐View LimitsWhole‐Arm Collision ConstraintsRobot Joint Velocity Limits3Figure 6.2: A system diagram illustrating the input and output requirementsof the MPC controller.completion of the positioning task using visual feedback. These joint com-mands generated by MPC are used as reference signals for the low-leveltorque controllers to track. A system diagram for each of the controllersdescribed is shown in Figure 6.2. Taking into account the low frame-rateof the camera with respect to the high bandwidth of the inner stabilizationloop, most visual servoing researchers use a kinematic model for controllerdesign. The following  rst-order discretization is used for MPC prediction:qk+1 = qk + t _qk: (6.12)Camera ModelA classical pin-hole camera model is used to describe the projection of thetarget object on the camera CCD array. Let C be the camera matrix de nedas:C =24fku fku cot u00 fkv( 1sin ) v00 0 135; (6.13)91Chapter 6. Model Predictive Controlwhere (u0;v0) are the images coordinates of the principal point, f is thefocal length,  is the perpendicular skew angle, and ku and kv are thenumber of pixels per unit distance in x and y, respectively. The camera ismounted on the robot’s distal link, where the position and orientation of itscanonical frame is controlled by the robot. Let eTc de ne the homogeneoustransformation that expresses the coordinates of the end-e ector frame inthe coordinates of the camera frame. The images coordinates (uj;vj) of thefeature points can be modelled as:(pj) =0@ujvj11A = C[I3 3j03 1]cTeeTbbTooPj; j2[1;n]: (6.14)Finally, a vector consisting of the image coordinates of all feature pointsis used for image-based feedback control:p =  (p1)T (p2)T    (pn)T T : (6.15)6.3.2 Constraint ModellingRobot Joint-Limit ConstraintsFor a robot with N independent joint actuators, the robot joint-limit con-straints are modelled as lower bounds qmin and upper bounds qmax on therange of feasible joint positions:q2[qmin;qmax]; qmin;qmax2RN: (6.16)Robot Joint-Velocity ConstraintsFor a robot with N independent joint actuators, the robot joint-velocityconstraints are modelled as lower bounds _qmin and upper bounds _qmax onthe range of feasible joint velocities:_q2[ _qmin; _qmax]; _qmin; _qmax2RN: (6.17)Camera Field-of-View ConstraintsFor a camera with a rectangular imaging sensor array, whose maximum andminimum coordinates are umin and umax in the horizontal axis and vmin andvmax in the vertical axis, the camera  eld-of-view constraints for a targetobject with n features are:92Chapter 6. Model Predictive Controluj(pj)2[umin;umax] 8j2[1;n]; (6.18)vj(pj)2[vmin;vmax] 8j2[1;n]: (6.19)Whole-Arm Collision ConstraintsFor a robot with N independent joint actuators, let Qfree be the collision-free space, where Qfree2RN. Qfree is the set of joint con gurations whichdo not cause the robot to occupy the same physical space as a workspaceobject. In this context, whole-arm collision constraints are represented as:q2Qfree: (6.20)A exact closed-form solution to Qfree is typically not available, except inthe case of trivially simple workspace objects (such as points) with kinemat-ically simpli ed robots (such as Cartesian gantry robots, or robots whereN is limited). The discussion of a representation of collision-free space thatis useful for MPC visual servoing is defered until Section 6.6, where it isdiscussed within the context of the dynamic collision checker (DCC).6.3.3 Control Law DesignThe error function for MPC visual servoing is chosen to be similar in form toIBVS. At time k, the predicted error at time k+i is the di erence betweenthe predicted image coordinates pijk and the desired image coordinates,plus a disturbance (model) correction term dk. The correction term is alsoexpressed in terms of image errors, capturing the di erence between latestplant output pk and the latest model output p(qkjk 1): ijk = (pijk pd) + dk; (6.21)dk = p(qkjk 1) pk: (6.22)The control law for MPC visual servoing is designed to take advantage ofthe robustness of IBVS for error correction while penalizing large joint-spacemotions to give e cient trajectories. Similar to the visible vertex generationand selection algorithm presented in Chapter 5, the idea is to take advantageof the natural ability of IBVS to keep the target within the  eld of view (toavoid local minimas resulting from the nonlinear  eld-of-view constraints)93Chapter 6. Model Predictive Controlwhile minimizing joint motion. The cost function C to be minimized is aquadratic function of image errors  and of joint velocities q:C = 12(Np 1Xi=1 TijkQ ijk + _qTijkW _qijk); (6.23)where Q and W are two symmetric positive de nite matrices, weightingthe predicted image errors against the predicted joint control e orts at timeinstant k as in [47]. The penalization of joint velocities helps to avoid jointsingularities where the control and the prediction of eye-in-hand cameramotion is more sensitive to errors.6.4 Summary of the MPC Visual Servo ControlLawFrom the perspective of manipulator visual servoing, the proposed MPCvisual servoing control law can be seen simultaneously as:1. An optimal controller using IBVS with penalization on large joint mo-tions;2. An online open-loop motion planner that takes into account the futuredynamics and constraints of the robotic system, based on an approxi-mate pose of the target object.The approximate object pose that is required for online open-loop plan-ning in MPC is obtained from one of the following: From the user, in the form of a rough \ballpark" pose of the targetobject, when initiating the robot positioning task; From homography decomposition, in the form of scaled Euclidean pa-rameters with an estimated depth provided by the user (Chapter 3); From EKF pose estimation, in the form a 6-D pose vector and a co-variance matrix describing the uncertainty of the estimated pose ineach dimension (Chapter 4).In this formulation, the cost function can be minimized over a sequence ofqijk or _qijk, depending on the type of input that is accepted by the low-levelrobot controller. Typical industrial controllers accept only joint positionsas input, so minimization over qijk is used for experimental validation here.94Chapter 6. Model Predictive ControlAn added advantage of optimizing over qijk is that the sequence of plannedjoint con guration inputs can be checked for collision-free motion, using theDCC algorithm as outlined in Section 6.8.2. Furthermore, by using qijkas the input for optimization, the MPC controller can be programmed tosolve o -line for the approximate robot joint con guration that completesthe positioning task, based only on the prediction model. The MPC visualservoing formulation is summarized as:minqijk12(Np 1Xi=1 TijkQ ijk + _qTijkW _qijk); (6.24)subject toqijk+1 = qijk + t _qijk; (6.25) ijk = (pijk pd) + dk; (6.26)_qijk2[ _qmin; _qmax]; (6.27)qijk2[qmin;qmax]; (6.28)qijk2Qfree; (6.29)uj((pj)ijk)2[umin;umax]; j2[1;n] (6.30)vj((pj)ijk)2[vmin;vmax]; j2[1;n] (6.31)(6.32)whereq0jk = qk; (6.33)pijk =h(p1)Tijk (p2)Tijk    (pn)TijkiT; (6.34)(pj)ijk = C[I3 3j03 1]cTeeTbbTooPj; j2[1;n] (6.35)eTb = frobot(qijk); (6.36)dk = p(qkjk 1) pk: (6.37)6.5 Simulation Results6.5.1 SetupThe MPC eye-in-hand visual servoing controller is implemented in simula-tion to test its ability to perform large-motion robot positioning tasks inthe presence of constraints and pose estimation errors. The simulations are95Chapter 6. Model Predictive Controldesigned to re ect, as closely as possible, the parameters of the actual ex-perimental test-bed that is used for performing eye-in-hand visual servoingexperiments. In order to avoid repeating the same material, details on thesimulation model (and on the experimental test-bed) can be found in Section6.9.1. The minimization of the cost function for predictive control is solvedusing a sequential quadratic programming (SQP) optimization algorithm[56][55]. This method solves a quadratic programming (QP) subproblem ateach iteration using the method of active sets. To speed up computations,the gradients to the cost function and to the constraints are provided viaa closed-form analytic function composed of the robot Jacobian, the imageJacobian and the measured errors. In the simulations, the actual pose ofthe target object is:bTo = Txyz( 0:28;0:51;0:14)TRz(145 )TRx( 55 )[m];whereas the estimated pose of the target object used by the MPC controllerfor joint-space planning, is:bT^o = Txyz( 0:30;0:50;0:15)TRz(135 )TRx( 45 )[m]:A desired view of the target object is shown to the robot to describe therequired positioning task, and the robot starts the visual servoing task withan initial view containing the target object in sight. The initial view anddesired view corresponding to the positioning task are shown in Figure 6.3and 6.4, respectively. The prediction horizon Np is set to 5 and the controlhorizon Nc is set to 1. The time step  t of 10ms is used for simulations. Q ischosen to be the identity matrix, while W is chosen to be a diagonal matrixwith its diagonal entries equal to 2,500. Note that collision constraintsare not modelled in these simulations. The purpose of these simulations isto demonstrate the ability of MPC visual servoing to manage constraintsthat are represented in the controller:  eld-of-view constraints, joint-limitconstraints and joint-velocity constraints. An appropriate representation ofcollision constraints qijk2Qfree that is useful for MPC visual servoing willbe discussed in Section 6.6. The demonstration of collision avoidance withMPC visual servoing will be shown in experiments with a physical robotsetup in Section 6.9.2).6.5.2 Camera Field-of-View ConstraintsFigure 6.5 shows the trajectory of the target object (as seen by the eye-in-hand camera) for the positioning task achieved via MPC visual servoing.96Chapter 6. Model Predictive ControlInitial ViewDesired View Figure 6.3: Overhead image describing the initial position of the eye-in-handcamera with respect to a target object.Initial ViewDesired View Figure 6.4: Reference image describing the desired position of the eye-in-hand camera with respect to the target object.97Chapter 6. Model Predictive ControlMECH 598 SeminarSimulation Results: Visual Servo to Object Grasping PositionjpgCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate12Figure 6.5: Image trajectory generated by MPC visual servoing, demon-strating the avoidance of camera  eld-of-view limits and the completion ofthe visual servoing taskThe target object, which resembles the conrod shown in Figure 6.18, con-sists of ten non-coplanar feature points. The point ‘o’ designates the startof the motion and the point ‘x’ designates the end of the motion. Throughthe use of an image-based cost function with an eye-in-hand robot con gu-ration, the control law generates motions that naturally satisfy the camera’s eld-of-view constraints. In contrast, this is not true for a stationary cameraobserving a robot completing a position task. Figure 6.5 shows the comple-tion of the task in Cartesian space. The positioning task requires signi canttranslation towards the target object, and additional in-plane and out-of-plane rotations to correct for the orientation errors. All of the above iscompleted in an e cient manner, without camera retreat, despite the use ofan image-based control law. With a high visual update rate, the predictionmodel is quickly corrected to account for the initial pose estimation errors.98Chapter 6. Model Predictive ControlMECH 598 SeminarSimulation Results: Visual Servo to Object Grasping PositionjpgCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate12Figure 6.6: Camera Cartesian trajectory generated by MPC visual servoing,demonstrating reasonable camera motions.99Chapter 6. Model Predictive Control6.5.3 Robot Joint-Limit ConstraintsFigure 6.7 shows the joint-space trajectory of the 6-DoF CRS-A465 robotfor the positioning task achieved via MPC visual servoing. The joint limitsof the robot are shown as dotted lines, where as the joint trajectories areshown as solid lines. The joint trajectories respond to the robot’s joint limitsby approaching them conservatively, while not violating them. The result isthat the robot’s joint-limit constraints are always satis ed throughout theservoing motion. Note that the avoidance of joint-limit constraints cannotbe naively achieved with the use of a joint-position saturator. The di er-ence between the constraint-aware MPC motion and a simple joint-positionsaturator is that the MPC motion continues to complete the task while man-aging constraints, while a joint-position saturator will likely bring the targetobject outside of the  eld of view, causing instability in the control law.6.5.4 Robot Joint-Velocity ConstraintsFigure 6.8 shows the joint velocities of the 6-DoF CRS-A465 robot for thepositioning task achieved via MPC visual servoing. The velocity limits ofthe robot are shown as dotted lines, where as the actual joint velocities areshown as solid lines. Unlike the exponentially decreasing velocity pro lesobserved in other visual servoing control laws (which tend to violate jointvelocity limits. See Figure 3.4), MPC visual servoing makes maximum useof the robot’s output capabilities by keeping the joint velocities near theirlimits, while not violating them. This is a rather aggressive controller. Theinsu cient weight given to the velocity weighting matrix W allows the mag-nitude of pixel errors to dominate the magnitude of joint velocities in thecost function. Note that the avoidance of velocity-limit constraints cannotbe naively achieved with the use of a joint-velocity saturator. The di er-ence between the constraint-aware MPC motion and a simple joint-velocitysaturator is that the MPC motion continues to complete the task while man-aging constraints, while a joint-velocity saturator will likely bring the targetobject outside of the  eld of view, causing instability in the control law. Thejoint velocities still tend to change abruptly in the given simulations. Whereacceleration limits are available for the robot, a higher-order model can beused in the MPC controller, to provide a smoother trajectory for tracking.This is not a major concern in the actual experimental implementation forvisual servoing, since the update rate of the camera is limited, and a tra-jectory interpolator must be used, in any case, to match the update rate ofthe inner PID loop.100Chapter 6. Model Predictive ControlJoint Position History7Figure 6.7: Robot joint trajectory generated by MPC visual servoing,demonstrating the avoidance of robot joint position limits.101Chapter 6. Model Predictive ControlMECH 598 SeminarSimulation Results: Visual Servo to Object Grasping PositionjpgJoint Position HistoryJoint Velocity HistoryCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate13Figure 6.8: Robot joint velocities generated by MPC visual servoing, demon-strating the avoidance of robot joint velocity limits.102Chapter 6. Model Predictive Control6.5.5 Tuning ParametersThe following section describes insights on tuning parameters obtained fromsimulations.Switching Control vs. MPCThe problem with expressing robot joint-limits,  eld-of-view limits, collisionlimits, as explicit constraints in the form of a typical switching visual servocontrol law is that the controller is not aware of the constraints until theyare active. The need to avoid immediate constraints requires large controllere orts. Since the controller does not plan ahead to avoid constraints, manyconstraints will indeed become active, so the modelled constraints mustclosely match the boundaries of the real limits of the plant in order toguarantee safe operation. In the presence of nonlinear constraints, withoutplanning, the system trajectory can easily get stuck in a local minima withinan infeasible region.The motivation for predictive control is to allow the controller to planahead to compute the best sequence of control inputs to avoid these con-straints, while completing the task. Although many of the constraints willbecome active in the prediction model, the control input produced by thepredictive controller that is applied to the plant several steps ahead may ac-tually prevent them from becoming active in the real plant. The predictionin MPC allows signi cant margins of errors in the modelling of the plantfor control. The MPC visual servoing framework allows gains to be tunedto match the controller with the degree of uncertainty in the model (i.e.robot calibration, camera calibration, target object pose). For example, theprediction horizon is a gain that can be tuned to control the aggressivenessof the control scheme to match the precision of the model that is used forplanning. If the model is precise, then a very aggressive control scheme canbe used (short prediction horizon, long time step). The MPC controller canbe interpreted as a constraint-aware switching controller in the limiting casewhere the prediction horizon and control horizon are zero. The followingsection describes the e ect of each tuning parameter within the MPC visualservoing controller when the cost function is optimized over a sequence ofjoint positions qijk.Prediction HorizonThe length of the prediction horizon Np determines the aggressiveness of thecontrol law with respect to constraints. When the prediction horizon is long,103Chapter 6. Model Predictive Controlthe controller predicts further ahead to become aware of constraints thatmay become active in the near future. The controller reacts by producingcontrol actions that avoid those constraints, many steps before they actuallybecome active in the real plant. The result is a conservative control law thatmaintains a wide distance from constraints. The prediction horizon alsoa ects the control e orts, since it is typically chosen to be much longer thanthe control horizon. For a prediction horizon that is longer than a givencontrol horizon, the last control input is held constant for the remainder ofthe prediction horizon. Therefore, a very long prediction horizon tends toproduce more conservative control e orts.Control HorizonThe length of control horizon Nc determines the degrees of freedom that thecontroller has, for choosing how a task is to be completed. When the controlhorizon is long, the controller can a ord to be more aggressive in minimizingthe image errors when required, without fear of violating constraints. Butthe controller can also a ord to be more conservative when required, so as tominimize joint actuation e orts in the cost function, when given the option tocomplete the task in several steps with di erent control inputs. The controlhorizon is typically chosen to be much shorter than the prediction to increasethe speed of computations. When the prediction horizon is longer than thecontrol horizon, the last control input is held constant for the remainder ofthe prediction horizon. A short control horizon (in the presence of a muchlonger prediction horizon) tends to produce more conservative control e orts,since the last control input must be held constant for the remainder of theprediction horizon without overshooting the goal, or violating constraints.Time stepThe length of the time step determines the aggressiveness of the controllaw with respect to the certainty in the target object’s pose. The lengthof the time step is inversely proportionally to the frequency of the visualupdate. When the time step is large, the controller has the opportunityto execute large robot motions in the absence of feedback correction (sinceplanning and servoing are executed iteratively). In the extreme case wherethe time step very large, the controller can be programmed to execute theposition task in a one-step open-loop motion. This is analogous to an open-loop constraint-aware path planner. However, this one-step planner su ersthe same problem as the PRM planner presented in Chapter 5. When the104Chapter 6. Model Predictive Controlpose of the target object is uncertain, a large time step tends to bring thetarget object outside of the  eld of view, before close-loop corrections canhave any e ect on the prediction model. MPC visual servoing addresses this eld-of-view problem by allowing the frequency of re-planning and servoingto be tuned (by adjusting the length of the time step) to match the level ofuncertainty in the target object’s pose.Cost Function: Q and WThe weighting matrices Q and W determine the aggressiveness of the controllaw with respect to the servoing task and control e orts, as inspired fromLQR (Linear Quadric Regulator) control theory. The elements with thematrix Q determine the weight given to the errors associated with eachimage feature, while the elements with the matrix W determine the weightgiven to the velocities associated with each robot joint.6.6 Whole-Arm Collisions ConstraintsPerformance evaluations of the proposed MPC visual servoing controller(Section 6.5) shows that it is capable of visual servoing over large robotmotions without violating the constraints related to an eye-in-hand system.System limits can be avoided as long as the corresponding constraint isincluded the model. The following section describes the modelling of whole-arm collisions constraints, which must be addressed and included in theMPC controller to ensure collision-free visual servoed motion.6.6.1 Representation of the Collision-Free SpaceThe representation of whole-arm collision constraints in joint-space fromknown Cartesian-space geometry is a non-trivial problem. Unlike the inversekinematic solutions for the robot con gurations that solve for a Cartesianframe of a particular point on the robot, there is typically no closed-formsolution to the set of robot con gurations that are collision-free. Collisionscan occur between any combination of points on the obstacle and on therobot. The collision boundary is a complex manifold that depends not onlyon the geometry of the obstacle, but also on the relative geometry of therobot arm. It is easy to check for the absence or presence of collision at aparticular robot con guration, but further information regarding the topol-ogy of the manifold is di cult to obtain. Unfortunately, MPC requires theuse of gradients to e ectively solve the constrained optimization problem.105Chapter 6. Model Predictive ControlMECH 598 SeminarRepresenting the Collision-Free Space{Q free} is a function of Manipulator GeometryANDObstacle Geometry11 2pNTT ikikikikCεQεuRuCost Function:Image Error, Input PenaltiesCollision-Free Space(Complex Manifold)minmaxminmax()(,)(),()(,)tuvutvtuvqqq21ikikikikiQCostuctoageo,puteatesRobot Joint limitsCameraFieldofViewSystem Constraints:minmaxminmax(,)(),()(,)()()freett qq qqqCamera Field of ViewCollision constraints Velocity constraintsCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidateminmax()tqqq36Acceleration constraintsFigure 6.9: Joint-space representation of the collision-free space (right) for aparticular manipulator and obstacle con guration (left). The collision-freespace is represented in 3-dimensional space for 3 joints only.Therefore, point-based representations of the collision-free space, derivedfrom sampling robot con gurations that are free of collisions, cannot be usedas constraints for MPC visual servoing. Figure 6.9 depicts a hypotheticalset of collision-free robot con gurations, represented as a three-dimensionalmanifold in joint-space for 3 out of the 6 available robot joints.6.6.2 Exploiting DCC for Collision BoundsThis section presents a method to exploit the properties of the dynamiccollision checking (DCC) algorithm to obtain a closed-form approximationof the collision-free space, which can be used for MPC visual servoing. TheDCC algorithm was previously presented in Section 5.5.2 to test for thefeasibility of whole-arm collisions with obstacles during the trajectory q(t)between two robot con gurations, qi and qf. The DCC inequality test hasthe following form:‘(q(t)) <d(qi) +d(qf) (6.38)where ‘(q(t)) is the the longest distance traveled by any point on the manip-ulator during the trajectory q(t). d(qi) is the shortest distance between an obstacle and the manipulatorin the con guration qi.106Chapter 6. Model Predictive Control d(qf) is the shortest distance between an obstacle and the manipulatorin the con guration qf.Collision-free motion can be guaranteed if the inequality in Equation 6.38holds for a chosen trajectory q(t). Given a trajectory q(t) and the set of3-D points H representing the hull of the manipulator, ‘(q(t)) is determinedby‘(q(t)) = maxP2HZ 10j_Pj2dt: (6.39)Solving Equation 6.39 is tedious but much can be gained given thatEquation 5.13 is an inequality and it still holds if ‘(q(t)) is replaced by anupper bound [53]. For a manipulator with N actuators, suppose that thetrajectory q(t) only involves rotating the jth actuator by an amount  qj.Then, it is possible to  nd a con guration for the actuators qj+1;:::;qNsuch that‘(q(t)) maxqj+1;:::;qN‘(q(t))= ‘j(q(t)):If the jth actuator is a revolute joint around the axis zj, then‘j(q(t)) = maxqj+1;:::;qNP2Hj[   [HNZ 10j_Pj2dt (6.40)= rjj qjj (6.41)where P 2 Hj[   [HN are the points on the hulls of the links j to Nand rj is the greatest possible distance between the line zj and a point P.Finally, generalizing Equation 6.40 to trajectories involving N actuators weobtain‘(q(t)) NXj=1rjj qjj: (6.42)Notice that the right hand side of Equation 6.42 is \trajectory-free". That is,it does not depend on qi, qf or the trajectory q(t). One notable constraint isthat any trajectory must be bounded within the hyperrectangle with cornersqi and qf.Replacing Equation 6.42 in Equation 5.13 results inNXj=1rjj qjj<d(qi) +d(qf): (6.43)107Chapter 6. Model Predictive ControlIn [53], Equation 6.43 is used to determine if a manipulator collides withobstacles during a trajectory q(t). The conservativeness of the upper bound,however, can be used for trajectories other than the linear interpolation de- ned by q(t). Indeed, if Equation 6.43 is true, then any trajectory boundedby the hyperrectangle with corners qi and qf is collision-free. This is de-termined from Equation 6.40 where any trajectory that is bounded betweenqji and qjf will result in the same upper bound ‘j.Equation 6.43 is used to  nd the collision-free hyperrectangle from theinitial con guration qi. The algorithm is based on evaluating Equation 6.43with the trajectories q(t=2i) for 0  i. Starting with i = 0, the entiretrajectory is tested. If DCC succeeds, then the entire hypperrectangle fromqi to qf is collision-free. If it fails, then the interval is split in half and DCCis evaluated between 0 t 0:5. The procedure is applied recursively untilDCC succeeds, and a hypperrectangluar region of collision-free joint spaceis found.The region of collision-free space between qji and qjf , as obtained fromDCC, can be expressed in the form of joint-limit constraints in the formu-lation of the MPC controller, such that the resulting visual servoed motioncan be guaranteed to be collision-free. In the following sections, this char-acteristic of Equation 6.43 will be used by the MPC controller in two imple-mentations: MPC with PRM for o ine path planning and MPC with DCCfor online collision-avoidance.6.7 MPC with O ine Path Planning6.7.1 MotivationThe ability of the MPC visual servoing controller to handle constraints sug-gests that it should be used in the two-stage planning-then-servoing ap-proach discussed in Chapter 5 to avoid collisions during visual servoing.However, the appropriate bounds on the constraints must still be deter-mined in order for the controller to be e ective. Speci cally, it is desirableto guarantee that the robot will not collide with obstacles or violate joint-limits when visual servoing is activated to \close the loop". Unfortunately,this closed-loop position is typically unknown. The next section discussesa method to determine the appropriate joint-motion constraints to be im-posed on the MPC controller, given a measure of uncertainty in the object’sestimated pose, to achieve collision-free closed-loop motion.108Chapter 6. Model Predictive ControlMECH 598 SeminarModel Predictive Control (MPC) with Probabilistic Road Maps (PRM)p()q2Goal ConfigurationActualGoal ConfigurationCollision-free spaceSince actual object location may be uncertainCollision-free spaceCollision-free spacespacePbbilitiRdM(PRM)pq1Starting Probabilistic Road Map(PRM)Model Predictive Control (MPC) Collision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate18gConfigurationFigure 6.10: Hyperrectangular collision-free spaces obtained from DCC as abyproduct of the form chosen for ‘(q(t)).6.7.2 Pose Uncertainty ModellingLet  xyz   = [ x; y; z;  ;  ;  ]T such that [  xyz   ; xyz   ] repre-sents the bounds on the uncertainty in the 6-D pose of the target object,expressed with respect to the estimated target object frame. If the posevariation has a Gaussian distribution, this bound can be conservatively esti-mated in terms of a number of standard deviations from the pose estimate,derived from the state covariance matrix from EKF pose estimation.Let ^q be the estimated robot joint position which completes the posi-tioning task. Let ^q +  q be the actual robot position which completes thepositioning task, where  q is the change in robot joint positions when theloop is closed. These joint positions are used to de ne the transformationfrom the robot end-e ector frame to the robot base frame:bT^e = frobot(^q); (6.44)bTe = frobot(q) = frobot(^q + q): (6.45)Let bT^o represent the estimated pose of the target object with respectto the base frame. Let bTo represent the actual pose of the target objectwith respect to the base frame. Then,109Chapter 6. Model Predictive ControlbT^o = bT^eeTccT^o;and (6.46)bTo = bTeeTccTo: (6.47)(6.48)Let ^oTo represent the deviation of the actual target object frame fromthe estimated target object frame:^oTo = bT 1^obTo; (6.49)=2664c c c s s  s c c s c +s s ^oXos c s s s +c c s s c  c s ^oYo s c s c c ^oZo0 0 0 13775: (6.50)The parameters ^oXo;^oYo;^oZo;^o o;^o o;^o o describe the deviation of theactual target object frame from the estimated target object frame, whichcauses a change in the robot joint positions  q when the loop is closed.These parameters can be checked to determine whether they are within thebounds de ned by  xyz   . Note that cTo = cT^o when the eye-in-handvisual servoing is complete, regardless of the actual pose of the target objectbTo with respect to the robot base.6.7.3 Bounds on Closed-Loop MotionLet bJ^o(^q) be the Jacobian for bT^o evaluated at ^q. Then an estimate of thelower bounds and upper bounds on the change in joint positions (resultingfrom the closed-loop motion) can be found using the following  rst-orderapproximation: q = bJ 1^o (^q) xyz   : (6.51)To solve for an exact solution for the lower bounds and the uppers boundson the change in joint positions resulting from the closed-loop motion, anonlinear constrained optimization problem is formulated and solved usingthe solution to linear Jacobian approximation as the initial state. Thisoptimization is solved twice for each joint qi (once for the lower bound andonce for the upper bound):110Chapter 6. Model Predictive Controlminq  qi; i2[1;N]; (6.52)maxq  qi; i2[1;N]; (6.53)subject to,^oXo2[  x; x]; (6.54)^oYo2[  y; y]; (6.55)^oZo2[  z; z]; (6.56)^o o2[   ;  ]; (6.57)^o o2[   ;  ]; (6.58)^o o2[   ;  ]: (6.59)Therefore, bounds on the change in joint positions resulting from theclosed-loop motion can be expressed in the form of a hyperrectangular re-gion  q 2 [ minq ; maxq ]. Unlike the symmetric bounds estimated from thelinear approximation, the nonlinear bounds describing this hyperrectangularregion can be asymmetric about the estimated open-loop joint position ^q.6.7.4 Simulation ResultsSimulations were performed on a 6-DoF CRS-A465 robot with a Sony XC-HR70 camera in an eye-in-hand con guration, completing a positioning taskusing MPC visual servoing. The visual servoing task was repeated 100times, while the pose of the target object was perturbed about its initiallyestimated position by a random pose vector with a zero-mean Gaussiandistribution and standard deviation  xyz   . To test the performance limitsof the methods presented in Section 6.7, the value of  xyz   was chosen torepresent a fairly high level of uncertainty in each of the pose dimensions: xyz   =  0:025m 0:025m 0:050m 20 20 10 : T =3The bounds on the maximum uncertainty in the pose are set as xyz   = 3 xyz   to encapsulate 99:7% of all samples. The methods outlined inSection 6.7 are applied to determine the appropriate bounds on the change injoint positions that result from the closed-loop motion (executed to compen-sate the perturbation in the target object’s pose). The results are presented111Chapter 6. Model Predictive Controlon 2-D plots for easy visualization. Figure 6.11 shows the results for joints1 and 2, Figure 6.12 shows the results for joints 3 and 4, and Figure 6.13shows the results for joints 5 and 6. The open-loop estimates of the desiredjoint positions are shown in the center of the  gures (as dotted red lines).The closed-loop joint positions resulting from the object pose perturbationare shown as sample points (in green). The bounds obtained from the linearJacobian approximation (shown as dotted green lines) provide good initialestimates, though they fail to encapsulate possible joint positions that re-sult from close-loop motion. These bounds are used to in uence the searchregion in the nonlinear constrained optimization problems. Results fromthe nonlinear constrained optimization formulation (shown in solid green)show that this method provides bounds that closely match the shape of thesample distribution. More importantly, the bounds encapsulate all samplesgenerated. The bounds obtained from nonlinear constrained optimizationare used to de ne the hyperrectangular region  q2[ minq ; maxq ].6.7.5 Integration with PRMThe hyperrectangular region q2[ minq ; maxq ] obtained from Equations 6.52and 6.53 describing the bounds on the change in joint positions resultingfrom closed-loop motion, can be used with the PRM path planning methods(previously discussed in Section 5) to guarantee collision-free visual servoingduring the second stage of the planning then servoing approach. Since theclosed-loop position qf is unknown during planning, setting d(qf) = 0 inEquation 6.43 results in a modi ed form of the inequality:NXj=1rjj qjj<d(qi): (6.60)If appropriate collision-free joint bounds can be established by the DCCalgorithm (Equation 6.60) to include the hyperrectangular region  q 2[ minq ; maxq ], then collision-free motion can be achieved in closed-loop byexpressing these bounds as the new joint-limit constraints for MPC servo-ing. In the MPC implementation for closed-loop visual servoing, qijk2Qfree(Equation 6.29 from Section 6.4) is replaced with the following collision-freejoint bounds obtained from DCC:qijk2[qminDCC;qmaxDCC]: (6.61)The integration of MPC with PRM using the updated collision-free jointboundaries is illustrated in Figure 6.14 in 2-dimensional joint space. If112Chapter 6. Model Predictive ControlFigure 6.11: Closed-loop positions for joints 1 & 2 resulting from MPCvisual servoing with object pose perturbations: closed-loop positions (greenpoints, n = 100), open-loop position (dotted red), linear bounds (dottedgreen), nonlinear bounds (solid green).113Chapter 6. Model Predictive ControlFigure 6.12: Closed-loop positions for joints 3 & 4 resulting from MPCvisual servoing with object pose perturbations: closed-loop positions (greenpoints, n = 100), open-loop position (dotted red), linear bounds (dottedgreen), nonlinear bounds (solid green).114Chapter 6. Model Predictive ControlFigure 6.13: Closed-loop positions for joints 5 & 6 resulting from MPCvisual servoing with object pose perturbations: closed-loop positions (greenpoints, n = 100), open-loop position (dotted red), linear bounds (dottedgreen), nonlinear bounds (solid green).115Chapter 6. Model Predictive ControlMECH 598 SeminarMethod 1: MPC Visual Servoing withPRMwith PRMActualGoal Configuration (unknownapriori)PlannedGoal ConfigurationCollision-free spaceq2(unknown a priori)Updated Collision-free spacePbbilitiRdM(PRM)q1Probabilistic Road Map(PRM)Model Predictive Control (MPC) Initial ConfigurationCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate33gFigure 6.14: Integration of PRM path planning with MPC visual servoing.Visual servoing is restricted to stay within the collision-free space determinedby DCC algorithm.appropriate collision-free joint bounds cannot be established by the DCCalgorithm (Equation 6.60) to include the hyperrectangular region  q 2[ minq ; maxq ], then the current pose estimate of the target object is too un-certain for the execution of the two-stage planning then servoing approach.Subsequent observations of the target object are required to reduce the un-certainty in the target object’s pose in order to guarantee collision-free mo-tion when closed-loop positioning is achieved. Pose uncertainty can be re-duced through additional observations with the EKF using images generatedfrom new camera positions (as previously discussed in Chapter 4).6.8 MPC with Online Collision-Avoidance6.8.1 MotivationThe two-stage planning then servoing approach (using PRM with MPC) al-lows di cult collision-avoidance problems to be solved o ine in the absenceof computational time constraints. In contrast, for positioning tasks wherecollision-avoidance is necessary, but not the primary issue at hand, an on-line approach can be used. This online approach uses MPC with DCC foriterative planning and servoing to ensure collision-free visual servoing over116Chapter 6. Model Predictive ControlMECH 598 SeminarMethod 2: MPC Visual Servoing + Bounds on Collision-Free SpaceActualGoal ConfigurationCollision-free spaceq2ConfigurationCollision-free spaceCollision-free spaceCollision-free spaceq1Model Predictive Control (MPC) Initial ConfigurationCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate34gFigure 6.15: Integration of MPC, DCC and adaptive bisection for collision-free visual servoing.large robot motions.6.8.2 Integration with Online DCCThe integration of MPC with DCC using a dynamically updated collision-free joint boundary is illustrated in Figure 6.15. During MPC predictionat time instance k, the sequence of planned joint input con gurations qijkis checked to determine if it satis es collision constraints using the DCCalgorithm (Section 6.6.2). If DCC determines that the planned motion iscollision-free, then the  rst element of the sequence is applied as the inputsignal to the plant. If DCC determines that the planned motion cannotbe guaranteed to be collision-free, then the hyperrectangular region that isdetermined to be collision-free by the DCC bisection algorithm, is appliedas new joint-limit constraints for MPC optimization. Speci cally, Equa-tion 6.29 from Section 6.4 is replaced with the following collision-free jointbounds obtained from the hyperrectangular region returned by DCC bisec-tion algorithm:qijk2[qminDCCk;qmaxDCCk]: (6.62)The bounds on the collision-free space are dynamically updated at eachiteration k, as denoted by the subscript.117Chapter 6. Model Predictive ControlMECH 598 SeminarExperimental Validation: Collision-Free Servoing for Object GraspinggjpgExperimental SetupConnecting Rod  g(Engine Part)CRS-A465RobotXC-HR70 CameraTwo-fingered Servo GripperCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate23Figure 6.16: Eye-in-hand platform for MPC visual servoing experiments.6.9 Experiments: Collision-Free Visual Servoing6.9.1 Experiment SetupAn overview of the experimental test-bed is shown in Figure 6.16. The goalof the experiment is to position the eye-in-hand robot with respect to theconnecting rod (which is located at some random location in the workspace),such that the two- ngered gripper is in the correct position for grasping theconnecting rod. MPC visual servoing allows this positioning task to becompleted, without requiring prior hand-eye calibration of the system. Thefollowing sections describe the robot and camera subsystem, as well as theapproximate models that are used in the MPC visual servoing controller.Initial Image and Desired ImageFigure 6.17 shows the initial image that the robot observes, prior to the startof the positioning task. The robot initially observes the workspace from a\bird’s eye" view, with its gripper positioned away from the target object.The robot positioning task is communicated to the robot via an image thatshows the correct relative positioning between the camera and the targetobject, when the desired positioning task is achieved. This reference imageis shown in Figure 6.18. Note that this image also corresponds to the correctpre-grasp position for the two- ngered gripper, which is used to pick up theconrod. The two- ngered gripper can be seen at the bottom of the desiredimage, enclosing the neck of the conrod.118Chapter 6. Model Predictive ControlMECH 598 SeminarExperimental Validation: Collision-Free Servoing for Object Graspinggjpg“Ready to Grasp” View [Desired Robot Configuration]Initial Camera View[Initial Robot Configuration](1) Robot-to-gripper transformation NOTrequired(2) Accurate camera calibration NOTrequiredCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate24(3) Accurate object pose NOTrequiredFigure 6.17: Overhead image describing the initial position of the cameraand end-e ector.MECH 598 SeminarExperimental Validation: Collision-Free Servoing for Object Graspinggjpg“Ready to Grasp” View [Desired Robot Configuration]Initial Camera View[Initial Robot Configuration](1) Robot-to-gripper transformation NOTrequired(2) Accurate camera calibration NOTrequiredCollision-Free Visual Servoing using Model Predictive Control (MPC) with Probabilistic Road Maps (PRM)Ambrose Chan M.A.Sc. Candidate24(3) Accurate object pose NOTrequiredFigure 6.18: Reference image describing the desired position of the end-e ector with respect to a target object.119Chapter 6. Model Predictive ControlTable 6.1: Uncalibrated Camera Parametersf 0.0048 [m](umin;umax) (1,1023)(vmin;vmax) (1,767)(u0;v0) (512,384)ku (4:65 10 6) 1 [pixels/m]kv (4:65 10 6) 1 [pixels/m] 90 CameraThe imaging sensor that is used in the experiments is a Sony XC-HR70monochrome CCD camera, with a 4.8mm lens, as shown in Figure 6.16.The camera acquires images at a resolution of 1024 768 at a maximumframe rate of 30fps. An estimate of the camera’s intrinsic parameters isrequired by the MPC controller to model the image projections in the camerafor predictive control. The camera parameters that are used in the MPCcontroller for experiments can be found in Table 6.1. These parameters areobtained from the product manual without calibration.An estimate of the homogeneous transformation eTc from the cameraframe to the end-e ector frame is also required by the MPC controller,to model the position and orientation of the camera for prediction. Thistypically requires extrinsic camera calibration. An approximate camera-to-end-e ector transformation is obtained using measurements by hand androughly estimating the location of the camera’s optical center. The followingtransformation is used in MPC controller for experiments:eTc = TRz(90 )TRx( 12:5 )Txyz(0; 0:085;0:070)[m]RobotThe robot used in this experiment is a CRS-A465 anthropomorphic robot.It has six independently controlled revolute joints that are highly gearedand not backdrivable. A low-level PID controller controls the output torquefor each joint to track reference trajectories at 1kHz. The low-level trajec-tory interpolator accepts inputs in the form of joint positions at 10Hz andaccepts velocities when speci ed as end conditions for smooth trajectoryinterpolation. A forward kinematic model of the robot is used in the MPCcontroller to estimate for the location of the camera frame, given a set of120Chapter 6. Model Predictive ControlTable 6.2: Joint-position limits of the CRS-A465 6-DoF robotq1 q2 q3 q4 q5 q6[rad] [rad] [rad] [rad] [rad] [rad]qmin -2.9845 0.0000 -3.5605 -3.1416 -1.8151 -3.1416qmax 2.9845 3.1416 0.4189 3.1416 1.8151 3.1416Table 6.3: Joint-velocity limits of the CRS-A465 6-DoF robotq1 q2 q3 q4 q5 q6[rad/s] [rad/s] [rad/s] [rad/s] [rad/s] [rad/s]_qmin -3.14 -3.14 -3.14 -2.99 -3.02 -2.99_qmax +3.14 +3.14 +3.14 +2.99 +3.02 +2.99joint con gurations. Geometric models of the robot, the camera and thegripper (constructed using geometric primitives) are used for the collisiontests. The joint-position limits of the CRS-A465 robot can be found in Table6.2. The joint-velocity limits of the CRS-A465 robot can be found in Table6.3. For the purpose of the experiment, the velocities of the CRS-A465 robothave been limited to 5% of their maximum.Target ObjectThe target object is a conrod (a connecting rod used in a reciprocatingpiston engine) consisting of ten feature points as shown in Figures 6.17 and6.18. A rough point-based model is constructed by measuring the centroidlocation of the feature points on the object. The approximate target objectmodel that is used by the MPC controller can be found in Table 6.4.An estimate of the pose of the target object is required by the MPCcontroller for joint-space path planning. In the experiments, a human userdetermined that the target object has the following \ball-park" pose, ex-pressed with respect to the robot’s base: Translation: (10cm;40cm;15cm) in (x;y;z). Rotation: 90 about z.Therefore, the following approximate target object pose is used by theMPC controller for joint-space path planning:121Chapter 6. Model Predictive ControlTable 6.4: Feature point coordinates of the target object modeloXj oYj oZj[m] [m] [m]oP1 0.076 0.031 0oP2 0.064 0.031 -0.003oP3 0.058 0.015 0oP4 0.054 0 0oP5 0.058 -0.015 0oP6 0.064 -0.031 -0.003oP7 0.076 -0.031 0oP8 0.037 0 -0.008oP9 0.019 0 -0.008oP10 0 0 -0.008bT^o = TRz(90 )Txyz(0:100;0:400;0:150)[m]Signi cant errors have been introduced to this pose estimate in order todemonstrate the e cacy of MPC visual servoing in correcting for modellingerrors. The actual (x;y;z) location of the conrod with respect to the robotbased is closer to (0:130;0:460;0:170)[m]. Also, note that one side of theconrod actually rests on top of another object, as shown in Figure 6.17,resulting in signi cant out-of-plane rotation in the y axis with respect tothe robot’s base. However, this is not captured in the pose estimate that isused by the MPC controller, which only has a rotation of 90 about the zaxis. Also, the rotation the conrod about the z axis is actually closer to 135 .These signi cant pose estimation errors (in addition to the approximationerrors introduced by the rounded \ball-park"  gures, estimated by a humanuser) are designed to test the ability of the MPC controller to compensatefor large errors in its prediction model.Workspace ObstaclesThe obstacles in the workspace include the two textbooks, the black binderand the set of pliers, which are located around the connecting rod. Theobstacles are modelled as simple polygons for collision detection. The di-mensions of the workspace obstacles can be found in Table 6.5. The ho-122Chapter 6. Model Predictive ControlTable 6.5: Dimensions of workspace obstaclesLength Width Height[m] [m] [m]Textbook 1 0.255 0.175 0.025Textbook 2 0.235 0.190 0.025Binder 0.290 0.270 0.040Pliers 0.165 0.075 0.020Table 6.6: Location of workspace obstaclesHomogeneous Transformation[m]Textbook 1 bTwo1 = TRz(60 )Txyz( 0:030;0:570;0:185)Textbook 2 bTwo2 = TRz(95 )Txyz( 0:025;0:450;0:160)Binder bTwo3 = Txyz( 0:040;0:440;0:120)Pliers bTwo4 = TRz(60 )Txyz(0:110;0:440;0:150)mogeneous transformations from the workspace obstacle centroidal frame tothe robot base frame can be found in Table 6.6.Control System ImplementationThe MPC controller is implemented on Pentium Dual-Core 2.0 GHz CPUrunning Windows XP. A Matrox Meteor II acquisition board acquires imagesfrom the Sony XC-HR70 at a frame rate of 30Hz. A visual tracker usingadaptive windowing and thresholding is implemented to track the centroidlocation of the feature points. The maximum control rate that is achieved,with SQP optimization running on one thread and feature tracking runningon another, is 10Hz. The low-level PID, independent joint controller isimplemented on a Pentium 4, 2.8 GHz computer operating with a WindowsRTX extension. The hardware is controlled through a Quanser Multi-Q PCIcard and WinCon software. The inner control loop runs at 1kHz. Quinticpolynomial interpolation is used to provide smooth reference signals for PIDtracking, to account for the di erence in control rates between the inner andthe outer control loops. Communication between controllers is handled viathe Quanser serial block.123Chapter 6. Model Predictive ControlTuning ParametersFor the eye-in-hand visual servoing experiments, the prediction horizon Npis set to 10 and the control horizon Nc is set to 1. The use of a longerprediction horizon in the MPC controller is intended to account for the(possibly signi cant) discrepancy between the real eye-in-hand system andthe approximate model used by the MPC controller for planning. The MPCcontroller time step,  t, is set at 0.1 seconds. Q is an identity matrix andW is a diagonal matrix with its diagonal entries equal to 10,000.6.9.2 Experiment ResultsFigure 6.19 shows the trajectory of the target object, seen from the eye-in-hand camera, as the robot completes the visual servoing task. The ex-periment is designed to test the ability of the image-based visual servoingcontroller to compensate for uncertainties in the model and to e cientlycomplete the positioning task without violating constraints. The image-based control law successfully handles both the signi cant rotation that isrequired about the camera’s optical axis and the translational motion thatis required towards the target object. The camera approaches the target ef- ciently without invoking the camera retreat phenomenon observed in otherimage-based control laws. The MPC controller is able to recognize the out-of-plane rotation required to properly align the gripper with the conrod.Figure 6.20 shows the trajectory of the robot as the visual servoing task isexecuted. The initial robot motions in frames 1 to 7 are very aggressive; theDCC algorithm is able to guarantee collision-free motion several predictionsteps ahead. Robot motions are more conservative when the robot gripperis in proximity to the workspace obstacles next to the conrod. In frames8 to 12, collision-free motion can no longer be guaranteed by the DCCalgorithm between large changes in robot con gurations. As the regularDCC algorithm fails (inequality not satis ed), the MPC controller replacesits joint-limits with the dynamically updated collision-free bounds providedby the DCC bisection algorithm. The MPC controller is optimized overa smaller region in joint-space where collision-free robot motion is feasible.The last few frames show the robot taking very small steps and maintaining agood distance away from the obstacles. The  nal gripper position is achievedconservatively, despite the incorrect object pose and the incorrect eye-in-hand calibration that is used in the MPC prediction model.With such a poorly estimated initial target object pose, the same taskcannot be executed using a one-step look-then-move approach (i.e., using124Chapter 6. Model Predictive ControlFigure 6.19: Sequence of camera motions generated by MPC visual servo-ing to complete a di cult positioning task. The resulting image featuretrajectories satisfy the camera  eld-of-view constraints.a very large time step  t in the MPC controller). Not only does the DCCalgorithm fail to guarantee collision-avoidance for such a large robot motion,it actually detects robot-collision at the open-loop end goal. The poorlyestimated location of the target object puts the robot in virtual collisionwith the surrounding workspace obstacles, so the robot is unable to executeits motion. This problem can be solved by: (i) improving the pose estimatewith the EKF through further observations; (ii) using a smaller time step t, thus allowing the MPC-DCC controller to gradually correct the poseestimate while approaching the target object, as demonstrated above.A similar problem a ects the two-stage planning-then-servoing approach(PRM and MPC, Section 6.7) when the pose of the target object is highlyuncertain.6.9.3 Discussion on StabilityAs previously noted, the proof of closed-loop stability for MPC control ofa nonlinear system with constraints is still an open research problem. Ofparticular interest is the proof of stability when the model is not assumed tobe perfect. Stability is surprising easy to prove under the assumption of a125Chapter 6. Model Predictive ControlFigure 6.20: Sequence of robot motions generated by MPC visual servoing tocomplete a di cult positioning task. The resulting robot trajectory satis esjoint position, joint velocity, and workspace collision constraints.perfect model. Keerthi and Gilbert [57] show that the addition of a \termi-nal constraint" forces the state to take a particular value at the end of theprediction horizon. The equilibrium point can then be proved stable via agiven Lyapunov function, provided that the optimization problem is feasibleand is solved at each step. In MPC visual servoing, the fact that a non-linear eye-in-hand model is used for predictions makes the robust stabilityanalysis of the system quite challenging. In contrast, predictive controllersthat are based on linear models behave linearly provided that the plant isoperating safely away from constraints (however, they behave nonlinearlywhen constraints are nearby). Unfortunately, for eye-in-hand systems thatexhibit severe nonlinearities, the usefulness of predictive control based ona linearized model is very limited. For short prediction horizons, the MPCcontroller can be approximated as a family of switching controllers, witheach controller designed to correspond to the set of constraints which areactive. In this approximation, the key is the  nd a common Lyapunov func-tion for all switching controllers to guarantee stability.In practice, the nominal stability of the closed-loop system is not an issuewhen an accurate model of the plant is available. It is quite easy to obtain126Chapter 6. Model Predictive Controlstability by tuning the parameters in the problem formulation and veryeasy to check that the designed system is stable (assuming the correct plantmodel). This is the typical situation with current applications of predictivecontrol in industrial processes [55].6.9.4 Insights from Simulations and ExperimentsLocal Minima from Nonlinear OptimizationThe eye-in-hand visual servoing experiments demonstrate that parametertuning and feedback correction in the MPC controller provide a signi cantdegree of robustness against modelling errors. Stability of the MPC visualservoing controller is demonstrated for a real robotic system completing po-sitioning tasks that require large-range robot motions, in the presence ofobject pose uncertainty and system constraints. However, each iteration ofthe MPC eye-in-hand visual servoing requires the optimization of a quadraticcost function, subject to constraints that are non-convex. Speci cally, thecamera  eld-of-view constraints are nonlinear functions of the joint con gu-rations of the robot, over which the cost function is minimized. Fortunately,the image-based cost function is designed to naturally keep the image trajec-tories away from the camera’s  eld-of-view constraints, as in the case withIBVS. Simulations show very few instances where the robot controller runsinto local minima due to the nonlinear  eld-of-view constraints.Local Minima from Collision AvoidanceThe avoidance of collision constraints is also a problem that may cause thevisual servoing trajectory to get stuck in local minima. Speci cally, theavoidance of collision constraints may be in direct con ict with the visualservoing goal. In the absence of a random sampling of additional collision-free robot con gurations (such as in PRM path planning) or random per-turbations applied to the current robot con guration (such as in simulatedannealing), the control law may not be able to \side-step" the obstacle,since it requires the trajectory to temporary diverge away from the goal.The collision constraints obtained from DCC are dynamically updated ateach iteration. Using DCC as part of the MPC formulation, the magnitudeof the robot’s joint-space motion is naturally scaled to match the robot’sproximity to obstacles. Collision constraints obtained from DCC are ini-tially designed to be conservative to keep the robot away from obstacles, inorder to prevent the MPC trajectory from getting into a local minimum.On the other hand, integration of MPC with the DCC adaptive bisection127Chapter 6. Model Predictive Controlalgorithm is designed to allow the robot to take smaller steps in an attemptto get out of a local minimum, before the trajectory becomes enclosed by thecollision-constraints in the direction of the goal. In cases where the obstacleavoidance problem is very complex, as in maze solving, the two-stage plan-ning and servoing approach (using PRM for obstacle avoidance and MPCfor closed-loop correction) is recommended over the MPC-DCC approach.An analysis of the MPC-DCC approach within the framework of the ActiveSet method is proposed for future work in Section 7.3.2.6.10 SummaryExperiment and simulation results in this chapter demonstrate the e cacyof MPC visual servoing in managing the contraints related to an eye-in-hand robot. Using MPC visual servoing formulation, large-range positioningtasks can be completed without violation of  eld-of-view, joint-limit, jointvelocity, or collision constraints, even in the presence of large uncertaintiesin the initial estimation of the target object’s pose. Closed-loop positionalaccuracy is achieved with an uncalibrated eye-in-hand system, using theteach-by-showing method with a rough model of the system for prediction.Eye-in-hand visual servoing experiments show that parameter tuning andfeedback correction in the MPC controller provide a signi cant degree ofrobustness against modelling errors. Stability of the MPC visual servoingcontroller is demonstrated for a real robotic system completing a di cultpositioning task.Insights from simulations have been provided for each of the tuning pa-rameters in the MPC visual servoing. The prediction horizon Np determinesthe aggressiveness of the control law with respect to constraints. The controlhorizon Nc determines the degrees of freedom the controller has for choosinghow to complete the task. The length of the time step  t determines theaggressiveness of the controller with respect to the uncertainty in the targetobject’s location. The matrices Q and W weight the priority given to imageerror minimization and control e ort in terms of joint velocities.A method of modelling an eye-in-hand robot and its associated contraintsis presented. A method of modelling collision constraints that is used forMPC visual servoing is also presented. This method exploits a property ofthe DCC algorithm to obtain a hyperrectangular region in joint-space thatis collision-free. The form of these bounds allows them to be included asconstraints in the MPC visual servoing controller for trajectory optimization.Two approaches have been demonstrated for handling collision con-128Chapter 6. Model Predictive Controlstraints from a path planning perspective. The  rst approach integratesMPC with a PRM path planner to allow it to handle complicated obsta-cle avoidance scenarios. The generation of visible vertices within the PRMallows a feasible transition from the PRM path planner to MPC visual ser-voing. A method of estimating the upper bounds on the changes in jointpositions resulting from closed-loop motion is discussed. Simulations of alarge number of MPC positioning tasks with perturbations to the targetobject’s pose validate the accuracy of these upper bounds. The bounds areused with the DCC algorithm to ensure collision-free visual servoing afterthe planned motion is executed. The second approach integrates MPC withDCC to iteratively plan and servo. This method uses DCC to check forcollisions and uses the joint bounds obtained from the DCC adaptive bi-section algorithm to modify the constraints expressed in the MPC visualservoing controller. Experiments demonstrate the ability of this methodto provide e cient visual servoing over large motions, without collisionswith workspace obstacles, and without violating joint-limit, joint-velocityand camera  eld-of-view constraints, even when the target pose’s locationis uncertain.To provide a point of reference, typical eye-in-hand systems using thelook-then-move approach (discussed in Section 1.2) rely on complete cal-ibration of the robot, the camera, the eye-in-hand transformation, and acompletely accurate object pose to achieve its positioning task. The MPCvisual servoing controller is able to achieve the same with an uncalibratedsystem, while compensating for uncertainties in the object’s pose and avoid-ing workspace collisions.129Chapter 7Conclusions andRecommendations7.1 Summary RemarksThis thesis represents the design process for a constraint-aware visual servo-ing control law that can be used to drive large-range robot motions. Uniqueinsights can be gained by examining the improvements that were made ateach stage of the research. First, an estimation of pose (through homogra-phy decomposition or extended Kalman  lter pose estimation) is necessaryfor improving the predictability of image-based control laws when the vi-sually servoed motion is large. However, the direct use of these Euclideanparameters in the visual servoing control law (as implemented in 21/2-D vi-sual servoing and position based visual servoing) may not result in the bestrobot motion. Path planning must occur in the joint-space of the robot,rather than in image-space, to produce feasible trajectories that successfullyavoid joint limits, velocity limits, and whole-arm collisions with workspaceobstacles. A two-stage planning and servoing approach (using probabilis-tic roadmaps with MPC visual servoing) allows di cult collision-avoidanceproblems to be solved, but su ers from the accumulation of errors duringthe execution of the planning stage. An online iterative planning and servo-ing approach (using MPC visual servoing with dynamic collision checking)results in trajectories that are far less sensitive to the propagation of mod-elling errors. Only a rough estimate of the target object’s pose is requiredfor planning purposes. In applications where collisions with workspace ob-stacles are of concern and the target object’s pose is uncertain, MPC visualservoing with dynamic collision checking can be implemented to generatecollision-free robot motions while closing the visual loop. The  nal result ofthis research is a versatile, constraint-aware visual servoing control law thatcan be used to teach practical robot motions.130Chapter 7. Conclusions and Recommendations7.2 ContributionsThe contributions of this thesis are in three main areas:7.2.1 MPC Eye-in-Hand Visual ServoingContribution 1: A visual servo control law for eye-in-hand robots that ef-fectively manages camera  eld-of-view, robot joint-limit, robot velocity-limit,and whole-arm collision constraints while visual servoing over large motions.1. A method to include whole-arm collision constraints in the MPC op-timization problem.2. A method to correct for uncertainty in the target object’s pose usingimage-based feedback.3. A method to optimize the predictions over joint positions as inputs,such that future inputs can be checked against collision constraints.7.2.2 Integration of MPC Visual Servoing with PRMContribution 2: A method to address complicated obstacle avoidance prob-lems while ensuring closed-loop eye-in-hand accuracy, using a two-stageplanning-then-servoing approach.1. A method to ensure a feasible transition between the PRM path plan-ner and MPC visual servoing controller in the presence of large uncer-tainties in the pose of the target object.2. A method to generate new vertices in a PRM that provide visible pathsconnecting the initial robot con guration to the goal robot con gura-tion. The visible path consists of robot con gurations that keep thetarget object within the camera’s  eld of view throughout the entireplanned path.3. A method to address uncertainty in the pose of the target object toguarantee collision-free closed-loop motion during the visual servoingstage.7.2.3 Integration of MPC Visual Servoing with DCCContribution 3: A method to address peripheral obstacle avoidance problemswhile enabling the use of visual servoing over large motions, using an onlineiterative planning and servoing approach.131Chapter 7. Conclusions and Recommendations1. A method to ensure collision-free motion using DCC with MPC visualservoing.2. A method to obtain dynamically updated collision-free bounds forMPC visual servoing using the results from the adaptive DCC bi-section algorithm.7.2.4 Minor ContributionsOther minor contributions of this thesis include: A method of selecting the most robustly visible kinematic solution forjoint-space path planning for eye-in-hand robots. A method of keeping the target within the  eld of view using adaptivegains for PBVS. A method of selecting the correct homography decomposition solutionfor visual servoing. An experimental evaluation of the accuracy of homography estimationand decomposition techniques. An PBVS implementation with EKF which also tracks the velocity ofthe target object.7.3 Future Work7.3.1 Interruption of Visual LoopOne advantage of MPC visual servoing is its ability to handle interruptionsto its visual feedback loop, without becoming unstable like other visual ser-voing controllers. In the presence of temporary object occlusions, the MPCcontroller can be programmed to continue its positioning task by operatingin open-loop (using the latest updated prediction model) until the tempo-rary occlusions have passed and the controller regains sight of the targetobject. The same also applies if the target object temporarily falls outsideof the camera’s  eld of view. However, in the presence of a poorly estimatedinitial target pose and an extended interruption to the visual feedback loop,there is no guarantee that the latest prediction model is correct and thatthe controller will regain sight of the target object. (It may very well belooking at the wrong place!). In the event that the controller cannot see the132Chapter 7. Conclusions and Recommendationstarget object for an extended period of time, it must begin an active searchfor the target object based on its last known location while continuing itspositioning task. A method to handle to these two con icting goals for aneye-in-hand robot remains to be explored.7.3.2 Stability Analysis of MPC-DCCA method to formally guarantee the convergence of the eye-in-hand posi-tioning task when facing di cult collision-avoidance problems is desirable.The current MPC-DCC method for handling collision constraints is similarto the Active Set method that is used for solving QP optimization problems.The Active Set method assumes that a feasible solution is initially available,where some inequality constraints are active and some are inactive. (Forexample, collision constraints are inactive when the robot is far away fromobstacles). At each iteration, the Active Set method  nds an improved solu-tion by taking a step in the direction which minimizes the cost while satisfy-ing the active contraints, without worrying about the inactive constraints.If this new solution is feasible, then it is accepted as the next iteration. (Thisis analogous to when the DCC inequality holds and the MPC controller isable to directly apply the input signal to the plant.) If it is not feasible, thena line-search is made in the direction of the step to locate the point at whichfeasibility is lost - namely the point at which one of the inactive constraintsbecomes active. (This is analogous to when the DCC inequality fails, andthe bisection algorithm is applied to determine the collision-free bounds.)The solution at this point is accepted as the next iteration and the newly ac-tive constraint is added to the active set (just as the collision-free bounds areadded as new constraints to the MPC controller.) Note that the collision-free bounds are approximations to the actual collision-free boundaries in therobot’s joint-space. These collision-free bounds are updated dynamically ateach iteration. In this aspect, the MPC-DCC formulation is most similar tothe SQP algorithm for solving nonlinear optimization problems. The SQPalgorithm solves a QP sub-problem at each iteration by making a local linearapproximation of the nonlinear constraints, with a modi cation to the orig-inal cost function using a quadratic approximation of the Lagrangian. Thecurrent MPC-DCC algorithm does not make modi cations to the cost func-tion when using dynamically updated approximations to the collision-freejoint bounds. Further analysis is required to determine how the MPC-DCCformulation can be modi ed to take advantage of its similarities to the SQPand QP algorithms.133Chapter 7. Conclusions and Recommendations7.3.3 Model Correction MethodsIn addition to using an image-based approach for model correction in MPC,the model correction can also take the form of a partial pose correction usinghomography decomposition or a full pose correction using EKF. The formermethod requires a very good estimation of depth and relatively noise-freeimages, while the later requires a calibrated camera. Both increase the com-putation burdens of the MPC controller. These three di ering approaches(image-based model correction, homography-based model correction, pose-based model correction) remain to be validated in experiments to determinetheir practical merits as well as stability requirements.7.3.4 Further Evaluation MetricsTo date, there has been little research on how to evaluate the relativestrengths and weaknesses of the di erent visual servoing approaches pre-sented in the  eld. Speci cally, there is a lack of a formal method forcomparing the performance of visual servoing controllers in a quantitativemanner for practical systems that must satisfy constraints. The di culty inevaluation lies in the large combination of con gurations that are possiblein the parameter-space and state-space (positioning tasks, robot con gu-rations, target object poses, workspace obstacles, and tuning parameters),and the dependency of visual servoing performance on the speci c con g-uration chosen. In [58], Gans et al. posit a set of preliminary servoingtests and metrics to measure quantitatively the performance of a visualservo controller. However, these metrics are more concerned with the tran-sient performance of the controllers, rather than their ability to react in astable manner to system constraints. The limits imposed by the physicaleye-in-hand system ( eld of view, joint-limits, collision, etc.) in practical ap-plications often render the transient performance of visual servo controllersof secondary importance. For example, for visual servoing systems thatare not constraint-aware, very conservative control gains must be chosento ensure safe operation, which results in poor transient performance. Fu-ture work remains to derive common metrics to evaluate the performancesof constraint-aware visual servoing approaches against the performance ofthose that are not constraint-aware.134Bibliography[1] Danica Kragic and Henrik I. Christensen. Survey on visual servoingfor manipulation. Technical report, Computational Vision and ActivePerception Laboratory, 2002. http://www.societyofrobots.com/robottheory/Survey_on_Visual_Servoing_for_Manipulation.pdf.[2] Intuitive Surgical Inc., 2009. http://www.intuitivesurgical.com/.[3] Braintech Inc., 2009. http://www.braintech.com/.[4] Seth Hutchinson, Gregory D. Hager, and Peter I. Corke. A tutorial onvisual servo control. IEEE Transactions on Robotics and Automation,12(5):651{670, October 1996.[5] Fran cois Chaumette. Potential problems of stability and convergencein image-based and position-based visual servoing. In D. Kriegman,G. Hager, and A.Morse, editors, The Con uence of Vision and Control,volume 237 of Lecture Notes in Control and Information Systems, pages66{78. Springer-Verlag, 1998.[6] Masami Iwatsuki and Norimitsu Okiyama. Rotation-oriented visualservoing based on cylindrical coordinates. In Proceedings of the 2002IEEE International Conference on Robotics and Automation, pages4198{4203, 2002.[7] Masami Iwatsuki and Norimitsu Okiyama. A new formulation of visualservoing based on clindrical coordinate system. IEEE Transactions onRobotics, 21(2):266{273, April 2005.[8] Peter I. Corke and Seth A. Hutchinson. A new partitioned approach toimage-based visual servo control. IEEE Transactions on Robotics andAutomation, 17(4):507{515, August 2001.[9] Fran cois Chaumette. Image moments: a general and useful set of fea-tures for visual servoing. IEEE Transactions on Robotics, 20(4):713{723, August 2004.135Bibliography[10] E. Cervera, A. P. del Pobil, F. Berry, and P. Martinet. Improvingimage-basaed visual servoing with three-dimensional features. The In-ternational Journal of Robotics Research, 22(10-11):821{839, October-November 2003.[11] D. Lowe. Three-dimensional object recognition from single two-dimensional images. Arti cial Intelligence, 31(3):355{395, 1987.[12] D. DeMenthon and L. S. Davis. Model-based object pose in 25 lines ofcode. International Journal of Computer Vision, 15:123{141, 1995.[13] William J. Wilson, Carol C. Williams Hulls, and Graham S. Bell. Rela-tive end-e ector control using cartesian position based visual servoing.IEEE Transactions or Robotics and Automation, 12(5):684{696, Octo-ber 1996.[14] Vincenzo Lippiello, Bruno Siciliano, and Luigi Villani. An experimentalsetup for visual servoing applications on an industrial robotic cell. InProceedings of the 2005 IEEE International Conference on AdvancedIntelligent Mechatronics, pages 1431{1436, Monterey, California, USA,July 2005.[15] Vincenzo Lippiello, Bruno Siciliano, and Luigi Villani. Position-basedvisual servoing in industrial multirobot cells using a hybrid cameracon guration. IEEE Transactions on Robotics, 23(1):73{86, February2007.[16] Graziano Chesi, Koichi Hashimoto, Domenico Prattichizzo, and Anto-nio Vicino. A switching control law for keeping features in the  eld ofview in eye-in-hand visual servoing. In Proceedings of the 2003 IEEE In-ternational Conference on Robotics and Automation, pages 3929{3934,Taipei, Taiwan, September 2003.[17] Benoit Thuilot, Philippe Martinet, Lionel Cordesses, and Jean Gallice.Position based visual servoing : keeping the object in the  eld of vision.In Proceedings of the 2002 IEEE International Conference on Roboticsand Automation, pages 1624{1629, Washington, DC, May 2002.[18] Lingfeng Deng, Farrokh Janabi-Shari , and William J. Wilson. Hy-brid motion control and planning strategies for visual servoing. IEEETransactions on Industrial Electronics, 52(4):1024{1040, August 2005.136Bibliography[19] P. Zanne, G. Morel, and F. Plestan. Robust vision based 3d trajectorytracking using sliding mode control. In Proceedings of the 2000 IEEE In-ternational Conference on Robotics and Automation, pages 2088{2093,San Francisco, CA, April 2000.[20] R. Basri, E. Rivlin, and I. Shimshoni. Visual homing: Sur ng onthe epipoles. International Journal of Computer Vision, 33:117{137,September 1999.[21] Jacopo Piazzi, Domenico Prattichizzo, and Antonio Vincino. VisualServoing Along Epipoles. Springer-Verlag, 2003.[22] A. Marotta, J. Piazzi, D. Prattichizzo, and A. Vicino. Epipole-based3d visual servoing. In Proceedings of the 2002 IEEE/RSJ Interna-tional Conference on Intelligent Robots and Systems, pages 366{371,Lausanne, Switzerland, October 2002.[23] Ezio Malis and Fran cois Chaumette. 2 1=2 d visual servoing with re-spect to unknown objects through a new estimation scheme of cameradisplacement. International Journal of Computer Vision, 37(1), June2000.[24] O. Faugeras and F. Lustman. Motion and structure from motion in apiecewise planar environment. International Journal of Pattern Recog-nition and Arti cial Intelligence, 2(3):485{508, 1988.[25] Z. Zhang and A. R. Hanson. Scaled euclidean 3d reconstruction basedon externally uncalibrated cameras. In Proceedings of the 1995 IEEESymposium on Computer Vision, pages 37{42, Coral Gables, Florida,USA, November 1995.[26] Koichiro Deguchi. Optimal motion control for image-based visual ser-voing by decoupling translation and rotation. In Proceedings of the 1998IEEE/RSJ International Conference on Intelligent Robots and Systems,volume 2, pages 705{711, October 1998.[27] Ezio Malis, Fran cois Chaumette, and Sylvie Boudet. 2-1/2-d visualservoing. IEEE Transactions on Robotics and Automation, 15(2):238{250, April 1999.[28] Ezio Malis and Fran cois Chaumette. Theoretical improvements in thestability analysis of a new class of model-free visual servoing meth-ods. International Journal of Robotics and Automation, 18(2):176{186,2002.137Bibliography[29] Ville Kyrki, Danica Kragic, and Henrik I. Christensen. New shortest-path approaches to visual servoing. In Proceedings of the 2004IEEE/RSJ International Conference on Intelligent Robots and Systems,pages 349{354, Sendai, Japan, September 2004.[30] Youcef Mezouar and Fran cois Chaumette. Path planning for robustimage-based control. IEEE Transactions on Robotics and Automation,18(4):534{549, August 2002.[31] Youcef Mezouara and Fran cois Chaumette. Avoiding self-occlusionsaand preserving visibility by path planning in the image. Robotics andAutonomous Systems, 41:77{87, 2002.[32] Youcef Mezouar and Fran cois Chaumette. Optimal camera trajectorywith imaged-based control. International Journal of Robotics Research,22(10-11):781{803, October-November 2003.[33] O. Khatib. Real-time obstacle avoidance for manipulataors and mobilerobots. International Journal of Robotics Reseach, 5(1):90{98, 1986.[34] J. C. Latombe. Robot Motion Planning. Kluwer Academic, 1991.[35] Florian Schramm, Franck Ge ard, Guillaume Morel, and Alain Mi-caelli. Calibration free image point path planning simultaneously en-suring visibility and controlling camera path. In Proceedings of the 2007IEEE International Conference on Robotics and Automation, pages2074{2079, Roma, Italy, April 2007.[36] Benedetto Allotta and Duccio Fioravanti. 3d motion planning forimage-based visual servoing tasks. In Proceedings of the 2005 IEEE In-ternational Conference on Robotics and Automation, pages 2173{2178,Barcelona, Spain, April 2005.[37] Guillaume Morel, Philippe Zanne, and Franck Plestan. Robust visualservoing: Bounding the task function tracking errors. IEEE Transac-tions on Control Systems Technology, 13(6):998{1009, November 2005.[38] P. Zanne, G. Morel, and F. Plestan. Sensor based robot control in thepresence of uncertainties: Bounding the task function tracking errors.In Proceedings of the 2002 IEEE International Conference on Roboticsand Automation, pages 3775{3780, Washington, DC, May 2002.138Bibliography[39] A. Castano and S. Hutchinson. Visual compliance: task directed vi-sual servo control. IEEE Transactions on Robotics and Automation,10(3):334{342, June 1994.[40] B. Espiau, F. Chaumette, and P. Rives. A new approach to visualservoing in robots. IEEE Transactions on Robotics and Automation,8(3):313{326, June 1992.[41] Nicolas Mansard and Fran cois Chaumette. A new redundancy formal-ism for avoidance in visual servoing. In 2005 IEEE/RSJ InternationalConference on Intelligent Robots and Systems, pages 468{ 474, Edmon-ton, Canada, August 2005.[42] Bradley J. Nelson and Pradeep K. Khosla. Strategies for increasing thetracking region of an eye-in-hand system by singularity and joint limitavoidance. International Journal of Robotics Research, 14(3):255{269,June 1995.[43] Paul Y. Oh and Peter K. Allen. Visual servoing by partitioning degreesof freedom. IEEE Transactions on Robotics and Automation, 17(1):1{17, Febuary 2001.[44] N. Papanikolopoulos, P. Khosla, and T. Kanade. Visual tracking ofa moving target by a camera mounted on a robot: A combination ofvision and control. IEEE Transactions on Robotics and Automation,9(1):14{35, February 1993.[45] Jacques A. Ganglo and Michel F. De Mathelin. Visual servoing of a 6-dof manaipulator for unknown 3-d pro le following. IEEE Transactionson Robotics and Automation, 18(4):511{520, August 2002.[46] M. Sauv ee, P. Poignet, E. Dombre, and E. Courtial. Image based visualservoing through nonlinear model predictive control. In Proceedings ofthe 45th IEEE Conference on Decision and Control, pages 1776{1781,San Diego, CA, USA, December 2006.[47] Micha el Sauv ee, Philippe Poignet, and Etienne Dombre. Ultrasoundimage-based visual servoing of a surgical instrument through nonlin-ear model predictive control. The International Journala of RoboticsResearch, 27(1):25{40, January 2008.[48] David G. Lowe. Distinctive image features from scale-invariant key-points. International Journal of Computer Vision, 60(2):91{110, 2004.139Bibliography[49] Mark W. Spong, Seth Hutchinson, and M. Vidyasagar. Robot Modelingand Control. John Wiley & Sons, Inc., 2006.[50] S. Chiaverini, B. Siciliano, and O. Egeland. Review of the dampedleast-squares inverse kinematics withexperiments on an industrial robotmanipulator. IEEE Transactions on Control Systems Technology,2(2):123{134, 1994.[51] Lydia E. Kavraki, Petr Svestka, Jean-Claude Latombe, and Mark H.Overmars. Probabilistic roadmaps for path planning in high-dimensional con guration space. IEEE Transactions on Robotics andAutomation, 12(4):566{580, August 1996.[52] David Hsu, Jean-Claude Latombe, and Hanna Kurniawati. On theprobabilistic foundation of probabilistic roadmap planning. Interna-tional Journal of Robotics Research, 25(7):627{643, July 2006.[53] Fabian Schwarzer, Mitul Saha, and Jean-Claude Latombe. Adaptivedynamic collision checking for single and multiple articulated robots incomplex environments. IEEE Transactions on Robotics, 21(3):338{353,June 2005.[54] Simon Leonard, Elizabeth A. Croft, and James J. Little. Dynamicvisibility checking for vision-based motion planning. In Proceedings ofthe 2008 IEEE International Conference on Robotics and Automation,pages 2283{2288, Los Angeles, California, May 2008.[55] J. M. Maciejowski. Predictive Control with Constraints. Pearson Edu-cation Limited, 2002.[56] M.J.D. Powell. Variable Metric Methods for Constrained Optimization.Springer-Verlag, 1983.[57] S.S. Keerthi and E.G. Gilbert. Optimal in nite-horizon feedback lawsfor a general class of constrained discrete-time systems: stability andmoving-horizon approximations. Journal of Optimization Theory andApplications, 57:265{293, 1988.[58] Nicholaas R. Gans, Seth A. Hutchinson, and Peter I. Corke. Perfor-mance tests for visual servo control systems, with application to par-titioned approaches to visual servo control. International Journal ofRobotics Reseach, 22(10-11):955{981, 2003.140Bibliography[59] R. Hartley and A. Zisserman. A Multiple View Geometry in ComputerVision. Cambridge University Press, 2000.141Appendix AHomography Estimation forNon-Planar ObjectsA.1 Virtual Plane De nitionRecall that three 3-D points cP1, cP2, cP3 are selected on the target objectto de ne a common virtual plane for each pair of images to act as referencesfor visual servoing. In order to ensure numeric stability, these points arechosen (among all possible combination of available feature points) such thatthey maximize the surface of the corresponding triangles in both images.The resulting three image points m1, m2, m3 in the current camera frameF are related to the corresponding image points m 1, m 2, m 3 in the desiredcamera frame F by a projective homography H such that:mi/Hm i; (A.1)where H is a homogeneous 3 3 matrix. Note that H is only de ned upto a scale factor, so that one of its elements can be set to 1 without loss ofgenerality.A.2 Planar vs. Non-Planar ObjectsIf the target object is known to be planar, then all pairs of image pointsbelonging to the object are related exactly by the projective homographyH. Using an additional feature point on this plane, the 8 unknown elementsof H can be estimated by solving the simple linear homogeneous system ofthe form:mi Hm i = 0; 8i = 1;2;3;4: (A.2)Standard methods exist for solving the homography related to planarobjects where at least 4 feature points are available [59]. However, thisplanar assumption severely restricts the type of target objects that can beused for visual servoing.142Appendix A. Homography Estimation for Non-Planar ObjectsIn the general case where the structure of the object is not planar, theestimation of H is a nonlinear problem which is di cult to solve online at therate required for visual servoing. However, if at least 8 points are available(3 to de ne  , and 5 not belonging to  ), then it is possible to estimate thehomography matrix H using a linearized algorithm based on the principleof virtual parallax, exploiting the property that all epipolar lines intersectat the epipole. (A point in one view projects onto an epipolar line in theother view. An epipole is the image in one view of the camera centre of theother view.) The next section brie y describes the application of this linearalgorithm, as developed in [23].For each 3-D target point cPj which does not belong to the commonlyde ned virtual plane  , its projection m j in the desired camera frame andits projection mj in the current camera frame are not related by the homog-raphy H. If the homography relationship H is virtually applied to m j (thatis, by virtually moving cPj to the plane  while preserving its projection m jin the  rst camera frame), the virtual projection Hm j would not coincidewith the observed projection mj in the second camera frame, due to theparallax e ect. But since m j is preserved in the  rst camera frame, thevirtual projection Hm j must lie on its epipolar line lj in the second cameraframe. By de nition, the same is also true for the observed projection mj.Therefore, the equation of the epipolar line lj can be written as the cross-product of the observed coordinates mj and the virtual coordinates Hm jin projective space:lj = mj Hm j; 8j =2 : (A.3)A.3 Estimation using the Virtual ParallaxThe estimation of H is based on the constraint that all the epipolar linesmust meet at the epipole. For each set of three epipolar lines, a constraintcan be generated by setting the area of the bounded triangle to be zero:det lj lk ll = 0; 8j6= k6= l: (A.4)Using the results from the virtual parallax analysis, this constraint canbe expressed in terms of the 8 unknown elements of the homography matrixH:det (mj Hm j) (mk Hm k) (ml Hm l ) = 0; 8i;j;k =2 :(A.5)143Appendix A. Homography Estimation for Non-Planar ObjectsAlthough the above constraint is non-linear in H, a change of projectioncoordinates can be performed to reduce the number of unknowns to a mini-mum, such that the solution can be solved linearly in two steps. Let M andM be 3 3 transformation matrices formed by the projective coordinatesof the three reference points which de ne the virtual plane:M =  m1 m2 m3 ; (A.6)M =  m 1 m 2 m 3 : (A.7)(A.8)In the new coordinate systems, the projective coordinates of all remain-ing feature points emj and em j are given by:emj = M 1mj; (A.9)em j = M  1m j: (A.10)By de nition, the coordinates of the three reference points form a canon-ical basis in the new coordinate systems:(em1; em2; em3) = (em 1; em 2; em 3) =0@2410035;2401035;24001351A: (A.11)The homography relationship relating the three reference points in thetwo images can be re-expressed in the new coordinate systems as:emi/ eHem i; 8i = 1;2;3: (A.12)Since em1, em2, em3 are orthogonal (and similarly em 1, em 2, em 3), the num-ber of unknowns to solve for in eH are reduced from the original 8 to 3 inthe new coordinate systems. That is:emi =ehiem i; (A.13)andeH = M 1HM =264eh1 0 00 eh2 00 0 eh3375: (A.14)The constraint requiring all the epipolar lines to meet at the epipole canbe written in the new coordinate system as:deth(emj eHem j) (emk eHem k) (eml eHem l )i= 0: (A.15)144Appendix A. Homography Estimation for Non-Planar ObjectsSince the above constraint is homogeneous and of polynomial degreethree, it can be rearranged to form a measurement matrix Ceh relating theseven unknown degree-three polynomials:Cehx = 0; (A.16)wherexT =heh1eh2 eh2eh1 eh1eh3 eh2eh3 eh3eh1 eh3eh2 eh1eh2eh3i: (A.17)Given n matched feature points that do not belong to the virtual plane,there are C(n;3) ways to choose the three di erent epipolar lines. Thismethod generates C(n;3) equations for the 7 unknowns, where:C(n;3) = n!3!(n 3)!: (A.18)Therefore, a minimum of eight matched feature points are required touniquely solve for x (with  ve points not belonging to the plane). Since theconstraints are homogeneous, the solution is found by performing a singularvalue decomposition (SVD) of Ceh = USVT. The solution to x is the columnof V corresponding to the smallest singular value. The unknowns eh1, eh2, eh3are found by solving the following linear homogeneous system, again usingSVD: 266666666664 ex2 ex1 0ex5 0  ex3 ex7 ex3 0ex7 0  xt1 ex4 ex7 0ex4 0  ex2ex6 0  ex70  ex6 ex4377777777775264eh1eh2eh3375 = 0: (A.19)Finally, the homography matrix H is obtained by transforming eH backinto the original coordinate system:H = MeHM  1 = M264eh1 0 00 eh2 00 0 eh3375M  1: (A.20)145Appendix BHomography DecompositionB.1 Geometric InterpretationA 3-D target point cPj expressed in the current camera frame Fc as X = X Y Z projects onto the image point m =  x y 1 in the currentimage through the following relation:Xx =Yy = Z: (B.1)When expressed in the desired camera frame Fc , the same 3-D targetpoint c Pj expressed as X =  X Y Z  projects onto the image pointm =  x y 1 in the desired image through the following relation:X x =Y y = Z : (B.2)The projection m in the desired image is related to the projection min the current image by a linear homography H:m = Hm ; (B.3)24xy135 =24h11 h12 h13h21 h22 h23h31 h32 h333524x y 135: (B.4)Let the 3-D target point be located on the plane  , de ned by the planenormal n expressed in the desired camera frame Fc . The signed perpen-dicular distance d of the plane to the desired camera frameFc is given by:d = n TX : (B.5)The 3-D coordinates X and X of the 3-D target point in the two canon-ical coordinate frames Fc and Fc are related by a rotation cRc and atranslation ctc :X = cRc X + ctc : (B.6)146Appendix B. Homography DecompositionSubstituting the relationship n X d =1:X = cRc +(ctc )n Td !X : (B.7)The above equation can be rewritten in projective coordinates as:m = (d (cRc ) + (ctc )n T)m : (B.8)By comparing the above projective relationship to the homography rela-tionship between m and m , the homography matrix H can be decomposedinto an outer product and sum of four Euclidean entities:H = d (cRc ) + (ctc )n T: (B.9)where cRc is the rotation matrix from frame Fc to Fc, n is the unit vector normal to  expressed in Fc , ctc is the direction of translation from Fc to Fc, and d is the signed distance from  to Fc .B.2 Decomposition SolutionsThe inverse problem of matrix decomposition (to obtain cRc , n , ctc andd from H) may contain multiple solutions and is a non-trivial problem.The method was originally proposed by Faugeras [24] and Zhang [25]. Tosimplify subsequent decompositions, H is  rst re-expressed (using SVD) ina diagonalized form:H = U VT; (B.10)where  is a diagonal matrix consisting of the singular values of H,which are positive and sorted in decreasing order  1   2   3: =24 1 0 00  2 00 0  335: (B.11)The new decomposition equation is = d0R0 + t0n0T: (B.12)147Appendix B. Homography Decompositionwhere d0, R0, t0, n0 are related to the original d , cRc , ctc , n by:cRc = sUR0VT; (B.13)ctc = Ut0; (B.14)n = Vn0; (B.15)d = sd0; (B.16)s = det(U) det(V): (B.17)Writing n0 in terms of its components n0 =  n01 n02 n03 and de ning aset of canonical basis:(e1;e2;e3) =0@2410035;2401035;24001351A: (B.18)The decomposition of  gives three vector equations: iei = d0R0ei + t0n0i; 8i = 1;2;3: (B.19)Eliminating t0 through multiplication byn0j followed by subtraction gives:d0R0(n0jei n0iej) =  i n0jei  jn0iej: (B.20)Since R0 preserves the vector norm, taking the norm of the above equa-tion 8i;j = 1;2;3 gives a set of linear equations in the unknowns n021 , n022 ,n023 :(d02  22)n021 + (d02  21)n022 = 0; (B.21)(d02  23)n022 + (d02  22)n023 = 0; (B.22)(d02  21)n023 + (d02  23)n021 = 0: (B.23)Using the property that n0 has unit norm, the system of equations canbe combined to give 4 di erent solutions to n0 depending on the chosen signsof n01 and n03:n01 = "1s 21  22 21  23; (B.24)n02 = 0; "1;"3 = 1; (B.25)n03 = "3s 22  23 21  23: (B.26)148Appendix B. Homography DecompositionTo solve for d0, note that the determinant of the system of equationsmust be zero to give a non-trivial (non-zero) solution to n021 , n022 , n023 :(d02  21)(d02  22)(d02  23) = 0: (B.27)However, the solutions d0 =  1 or d0 =  3 are impossible, since theyrequire n01 = n02 = n03 = 0 in the system of equations (due to the relativesizes of  1   2   3). Therefore, only two solutions to d0 remain:d0 =  2: (B.28)If d0 = + 2, then the solutions to R0 and t0 are:R0 =24cos 0  sin 0 1 0sin 0 cos 35; t0 = ( 1  3)24n010 n0335; (B.29)wheresin =  1  3 2n01n03 = "1"3p( 21  22)( 22  23)( 1 + 3) 2 ; (B.30)cos =  1n023 + 3n021 2 = 22 + 1 3( 1 + 3) 3: (B.31)If d0 =  2, then the solutions to R0 and t0 are:R0 =24cos 0 sin 0 1 0sin 0  cos 35; t0 = ( 1 + 3)24n010n0335; (B.32)wheresin =  1 + 3 2n01n03 = "1"3p( 21  22)( 22  23)( 1  3) 2 (B.33)cos =  3n021   1n023 2 = 1 3  22( 1  3) 2: (B.34)The indeterminate signs of n1, n3 and d give a total of 8 di erentpossible solutions to the decomposition of H. The solution to the originalEuclidean components d , cRc , ctc , n can be obtained by transformingd0, R0, t0, n0 back into the original coordinate system using U and V fromthe SVD of H.149Appendix B. Homography DecompositionB.3 Reinforcing Visibility ConstraintsFortunately, the knowledge of one point m =  x y 1 on the plane,combined with visibility constraints, can be used to reduce the overall num-ber of solutions from 8 to 2. The visibility constraints of a physical camerarequire Z > 0 and Z > 0, so that the sign of d can be determined fromthe following relationship:ZZ =h31x +h32y +h33d > 0: (B.35)Only 4 solutions remain once the sign of d is determined. The followingconstraint for Z > 0 leaves only 2 of the 4 possible solutions for n :n Tmd > 0: (B.36)B.4 Solution for Visual ServoingOnly one of the two remaining solutions correctly describes the relativegeometry between the desired camera frame Fc , the current camera frameFc, and the plane  . If the incorrect decomposition of H is chosen asinput into the control law, there is no guarantee that visual servoing willconverge. Therefore, a robust method must be devised to eliminate theincorrect solution.One method is to use a third image to determine the common planenormal n . Unfortunately, this strategy cannot be used in the  rst iterationof the control law, since the eye-in-hand robot must be commanded to movebased on the currently observed image and the original training image. Toovercome this de ciency, another virtual reference plane is chosen amongthe available feature points, so that a common solution to cRc and ctc can be determined. In subsequent iterations, the solution nearest to theprevious one is chosen, assuming that the camera motion is small betweenimage frames.150Appendix CDamped Least-SquaresInverseIn section 4.5.4, the problem of robot singularities arose, reducing the com-plete controllability of visual servoing methods. A damped least-squaresinverse kinematics solution [50] is implemented to address robot singulari-ties. This method provides the possibility of ensuring complete controlla-bility throughout the entire robot workspace at the expense of increasedtracking errors. Recall that complete controllability is lost when the inverseof the robot Jacobian J 1robotjq does not exist. J 1robotjq can be rede ned asa pseudo-inverse with the insertion of a small positive diagonal matrix  2Ibefore the matrix inversion to restore its rank. The solution to the modi edinverse always exists, but in a slightly inaccurate form:J 1robotjq   Jrobotjq T Jrobotjq + 2I  1 Jrobotjq T : (C.1)The damping factor  determines the degree of approximation intro-duced with respect to the pure least-squares solution. Small values of  giveaccurate solutions but are not very robust to near-singular con gurations,while large values are robust but result in large tracking errors. Using aconstant value for  is, therefore, inadequate for obtaining a satisfactoryperformance over the entire workspace.  can be adjusted as a function ofa measure of closeness to the singularity. The singular value decomposi-tion of the Jacobian matrix gives insight into the condition of the Jacobianrelationship: Jrobotjq = 6Xi=1 ieuievTi ; (C.2)where eui and evi are the input and output singular vectors and  i are thesingular values, ordered by size so that  1   2       6. The dampingfactor  is adjusted based on the proximity of the smallest singular value  6to " (the size of the singular region). A smooth function is chosen so that151Appendix C. Damped Least-Squares Inversecontinuity of joint velocity is ensured during the transition at the border ofthe singular region [50]: 2 =(0 when  6  "; 1 ( 6" )2  2max otherwise: (C.3)152Appendix DExtended Kalman FilterA nonlinear system can be approximated by linearizing it about the currentstate from which in nitesimally small changes occur. The linearized Kalman lter is based on  nding a linear system whose states represent the devia-tions from a nominal trajectory of a nonlinear system. After linearization,a Kalman  lter can be used to estimate the deviations from the nominaltrajectory and obtain an estimate of the states of the nonlinear system. Animproved version of the linearized Kalman  lter is the extended Kalman lter (EKF), which directly estimates the states of a nonlinear system us-ing a bootstrap method. That is, the nonlinear system is linearized aroundthe Kalman  lter estimate, and the Kalman  lter estimate of the nominaltrajectory is, in turn, based on the linearized system. The complete set ofequations used for the implementation of EKF pose estimation is outlinedbelow:Prediction of State Estimate:(bxobsv)k; = F((bxobsv)k 1;(uobsv)k 1) (D.1)Linearization of State Di erence Equations:eAk = @F(xobsv;uobsv)@xobsv     xobsv=(bxobsv)k; (D.2)Prediction of Covariance Estimate:ePk; = eAk 1ePk 1eATk 1 + eQk 1 (D.3)Linearization of Output Equations:eCk = @G(xobsv)@xobsv     xobsv=(bxobsv)k; (D.4)Correction of Covariance Estimate:ePk = heP 1k 1 + eCTk eR 1k eCki 1 (D.5)153Appendix D. Extended Kalman FilterKalman Gain:eKk = ePkeCkeR 1k (D.6)Correction of State Estimate:(bxobsv)k = (bxobsv)k; + eKk((zobsv)k G((bxobsv)k; )) (D.7)154

Cite

Citation Scheme:

    

Usage Statistics

Country Views Downloads
United States 16 0
France 13 0
China 10 6
Russia 6 0
Canada 4 6
Vietnam 2 0
Sweden 1 0
India 1 0
Republic of Korea 1 0
Australia 1 0
Poland 1 0
Taiwan 1 1
City Views Downloads
Unknown 22 8
Ashburn 11 0
Beijing 9 0
Vancouver 3 6
Hanoi 2 0
Redmond 2 0
Clarks Summit 2 0
Seoul 1 0
Melbourne 1 0
Wilkes Barre 1 0
Shenzhen 1 6
Montreal 1 0
Stockholm 1 0

{[{ mDataHeader[type] }]} {[{ month[type] }]} {[{ tData[type] }]}
Download Stats

Share

Embed

Customize your widget with the following options, then copy and paste the code below into the HTML of your page to embed this item in your website.
                        
                            <div id="ubcOpenCollectionsWidgetDisplay">
                            <script id="ubcOpenCollectionsWidget"
                            src="{[{embed.src}]}"
                            data-item="{[{embed.item}]}"
                            data-collection="{[{embed.collection}]}"
                            data-metadata="{[{embed.showMetadata}]}"
                            data-width="{[{embed.width}]}"
                            async >
                            </script>
                            </div>
                        
                    
IIIF logo Our image viewer uses the IIIF 2.0 standard. To load this item in other compatible viewers, use this url:
http://iiif.library.ubc.ca/presentation/dsp.24.1-0067229/manifest

Comment

Related Items