Pose and Motion Estimation from Vision Using Dual
Total Page:16
File Type:pdf, Size:1020Kb
POSE AND MOTION ESTIMATION FROM VISION USING DUAL QUATERNION-BASED EXTENDED KALMAN FILTERING A Dissertation Presented for the Doctor of Philosophy Degree The University of Tennessee, Knoxville James Samuel Goddard, Jr. December 1997 Copyright c 1997 by James Samuel Goddard, Jr. All rights reserved ii ACKNOWLEDGEMENTS I would like to thank my adviser, Dr. Mongi Abidi, whose continued encouragement and guidance have enabled me to bring this extensive effort to a successful conclusion. In addition, I would like to express gratitude to my committee members, Drs. Rajiv Dubey, Walter Green, Michael Roberts, and Ross Whitaker for their extremely helpful assistance and comments while completing my dissertation. Lastly, I would like to express special appreciation to my wife and children for their support and patience during this long process. iii ABSTRACT Determination of relative three-dimensional (3–D) position, orientation, and rela- tive motion between two reference frames is an important problem in robotic guidance, manipulation, and assembly as well as in other fields such as photogrammetry. A solution to this problem that uses two-dimensional (2–D), intensity images from a single camera is desirable for real-time applications. Where the object geometry is unknown, the esti- mation of structure is also required. A single camera is advantageous because a standard video camera is low in cost, setup and calibration are simple, physical space requirements are small, reliability is high, and low-cost hardware is available for digitizing and processing the images. A difficulty in performing this measurement is the process of projecting 3–D object features to 2–D images, a nonlinear transformation. Noise is present in the form of perturbations to the assumed process dynamics, imperfections in system modeling, and errors in the feature locations extracted from the 2–D images. This dissertation presents solutions to the remote measurement problem for a dynamic system given a sequence of 2–D intensity images of an object where feature positions of the object are known relative to a base reference frame and where the feature positions are unknown relative to a base reference frame. The 3–D transformation is modeled as a nonlinear stochastic system with the state estimate providing six degree-of-freedom motion and position values. The stochastic model uses the iterated extended Kalman filter as an estimator and as a screw representation of the 3–D transformation based on dual quaternions. Dual quaternions provide a means to represent both rotation and translation in a unified notation. The method has been implemented and tested with both simulated and actual experimental data. Simulation results are provided along with comparisons to a point-based method using rotation and translation to show the relative advantages of the proposed method. Experimental test results using a camera mounted on the end effector of a robot arm to demonstrate relative motion and position control are also presented. iv TABLE OF CONTENTS Chapter 1. Introduction ::::::::::::::::::::::::::::::::: 1 1.1ProblemStatement............................. 1 1.2Motivation.................................. 5 1.3RelatedWork ............................... 7 1.3.1 Pose Using Point-Based Methods . ............... 8 1.3.2 Methods for Real-Time Pose Determination . ..... 10 1.3.3 Pose Using Model-Based and Higher-Level Primitives Methods 17 1.3.4 Methods Incorporating Noise Estimation and Uncertainty . 26 1.3.5 Methods Using Kalman Filtering for Direct Pose and Motion Estimation............................. 29 1.4SynopsisofDissertation.......................... 33 Chapter 2. Theoretical Foundation ::::::::::::::::::::::::::: 35 2.1CoordinateFrameTransformations.................... 35 2.2Quaternions................................. 36 2.3DualNumbers................................ 38 2.4DualNumberQuaternions......................... 40 2.5 3-D to 2-D Perspective Projection . ............... 41 2.6Representationof3-DRotationbyQuaternions............. 43 2.7 Representation of 3–D Rotation and Translation by Dual Number Quaternions................................. 45 2.8IteratedExtendedKalmanFilter..................... 47 v Chapter 3. Pose and Motion Estimation Method ::::::::::::::::::: 54 3.1 Pose and Motion Estimation Using IEKF and Dual Number Quaternions 55 3.2StateAssignment.............................. 55 3.3SystemModel................................ 57 3.4MeasurementModel............................ 60 3.5RepresentationofLinesinaPlane.................... 62 3.6LinearizationoftheStateTransitionFunction.............. 65 3.7LinearizationoftheMeasurementFunction............... 67 3.8IteratedExtendedKalmanFilterRepresentation............ 72 3.9 Extension of Iterated Extended Kalman Filter to Estimate Structure . 73 3.9.1 StructureRepresentation..................... 73 3.9.2 ExtensiontoStateTransition................... 74 3.9.3 ExtensiontoMeasurementUpdate................ 74 Chapter 4. Noise Analysis ::::::::::::::::::::::::::::::: 76 4.1AnalysisofLineNoise........................... 76 4.1.1 Probability Density Function of Line-Point Parameters ..... 77 4.1.2 Covariance of xlp and ylp ..................... 87 4.2Real-TimeImplementation........................ 93 Chapter 5. Experimental Results :::::::::::::::::::::::::::: 96 5.1SimulationResults............................. 96 5.1.1 DynamicModel........................... 97 5.1.2 Model Trajectory . ...................... 99 5.1.3 CameraModel........................... 100 5.1.4 ObjectModel............................ 101 5.1.5 EstimationModel......................... 101 vi 5.1.6 ComparisonModel......................... 102 5.1.7 InitialConditions.......................... 102 5.1.8 MeasurementNoiseModel..................... 102 5.1.9 TestResults............................. 103 5.1.10SimulationUsingAdaptiveNoiseEstimation.......... 130 5.2RobotArmTests ............................. 132 5.2.1 ExperimentalSetup........................ 136 5.2.2 DynamicModel........................... 136 5.2.3 MeasurementModel........................ 136 5.2.4 InitialConditions.......................... 139 5.2.5 MeasurementNoiseResults.................... 139 5.2.6 TargetMotionResults....................... 139 5.3TestResultsFromUnknownObjectGeometry............. 148 5.3.1 StructureEstimation........................ 148 5.3.1.1 Calculation of Actual Structure for Reference . 150 5.3.1.2 InitialStateEstimate.................. 151 5.3.1.3 Transformation of Structure Estimate Frame to Ref- erenceFrame ...................... 153 5.3.2 ResponsetoStepChangeinMotion............... 154 Chapter 6. Summary and Conclusions ::::::::::::::::::::::::: 159 6.1Summary.................................. 159 6.2ConclusionsandContributions...................... 160 6.3FutureWork................................ 164 References ::::::::::::::::::::::::::::::::::::::::: 166 Vita :::::::::::::::::::::::::::::::::::::::::::: 179 vii LIST OF TABLES 5.1 Actual and assumed initial states of the extended Kalman filter for the four-point simulation tests. The initial states are the same for both the dualquaternionmethodandthepointmethod................ 104 5.2 Dual quaternion method and point method initial error covariance matrix and process noise matrix diagonal terms of the extended Kalman filter for thefour-pointsimulationtests......................... 104 5.3 Actual and assumed initial states of the dual quaternion and point-based extended Kalman filter for simulation comparison with the adaptive mea- surement noise line method, the nonadaptive line method, and the com- parisonpointmethod.............................. 131 5.4 Dual quaternion method and point method initial error covariance matrix and process noise matrix diagonal terms of the extended Kalman filter for simulation comparison with the adaptive measurement noise line method, thenonadaptivelinemethod,andthecomparisonpointmethod...... 131 5.5 Reference pipe line data as calculated by two image views separated by an x translation of 50 mm. The data show the ~l and m~ components of the 3–Dlines..................................... 151 viii LIST OF FIGURES 1.1 Transformation between two reference frames illustrating the fundamental problemtobesolved.............................. 3 1.2 Block diagram of a robot arm feedback control loop showing an application for pose and motion estimation. A camera mounted on the end effector of therobotarmisusedasthemeasurementsensor. ............. 6 2.1 3–D to 2–D perspective projection model assumed in pose estimation prob- lem. The center of projection is the camera reference origin. ..... 42 2.2 Diagram showing screw form of 3–D transformation between two reference frames. Eight parameters are needed to specify the complete transforma- tion. ...................................... 46 2.3 Extended Kalman filter block diagram showing functionality along with the inputs and outputs. The filter is seen to have a closed-loop feedback controlwherethemeasurementerrorisminimized.............. 52 3.1 Functional block diagram of estimation problem showing operations in- volved in pose estimation method from optical imaging to state estimate producedbytheiteratedextendedKalmanfilter............... 58 3.2 Line point for a line in 2-D image plane is defined as the intersection point ofthelineandaperpendicularlinepassingthroughtheorigin....... 64 4.1 Diagram showing a point pair with a noise radius of one standard deviation and the corresponding range of variation of the line connecting the points.