<%BANNER%>

Vision-based control for autonomous robotic citrus harvesting

University of Florida Institutional Repository

PAGE 1

VISION-BASEDCONTROLFORAUTONOMOUSROBOTICCITRUS HARVESTING By SIDDHARTHASATISHMEHTA ATHESISPRESENTEDTOTHEGRADUATESCHOOL OFTHEUNIVERSITYOFFLORIDAINPARTIALFULFILLMENT OFTHEREQUIREMENTSFORTHEDEGREEOF MASTEROFSCIENCE UNIVERSITYOFFLORIDA 2007

PAGE 2

c 2007SiddharthaSatishMehta

PAGE 3

TomyparentsSatishandSulabha,mysisterShweta, andmyfriendsandfamilymemberswhoconstantly lledmewithmotivationand joy.

PAGE 4

ACKNOWLEDGMENTS Iexpressmymostsincereappreciationtomysupervisorycommitteechair, mentor,andfriend,Dr.ThomasF.Burks.Hiscontributiontomycurrentand ensuingcareercannotbeoveremphasized.Ithankhimfortheeducation,advice, andforintroducingmewiththeinteresting eldofvision-basedcontrol.Special thanksgotoDr.WarrenE.Dixonforhistechnicalinsightandencouragement.I expressmyappreciationandgratitudetoDr.Wonsuk"Daniel"Leeforlendinghis knowledgeandsupport.Itisagreatpriviledgetoworkwithsuchfar-thinkingand inspirationalindividuals.AllthatIhavelearntandaccomplishedwouldnothave beenpossiblewithouttheirdedication. IespeciallythankMr.GregoryPughandMr.MichaelZingarofortheir invaluableguidanceandsupportduringthelastsemesterofmyresearch.Ithank allofmycolleagueswhohelpedduringmythesisresearch:SumitGupta,Guoqiang Hu,Dr.SamuelFlood,andDr.DukeBulanon. MostimportantlyIwouldliketoexpressmydeepestappreciationtomy parentsSatishMehtaandSulabhaMehtaandmysisterShweta.Theirlove, understanding,patienceandpersonalsacri cemadethisdissertationpossible. iv

PAGE 5

TABLEOFCONTENTS page ACKNOWLEDGMENTS.............................iv LISTOFFIGURES.... ............................vii LISTOFTABLES..... ............................ixABSTRACT.............. ......................x CHAPTER 1INTRODUCTION.. ............................1 2FUNDAMENTALSOFFEATUREPOINTTRACKINGANDMULTIVIEWPHOTOGRAMMETRY.......................9 2.1Introduction ...............................9 2.2ImageProcessing ............................9 2.3Multi-viewPhotogrammetry......................12 2.4Conclusion.... ............................16 3OBJECTIVES.... ............................17 4METHODSANDPROCEDURES.....................20 4.1Introduction ...............................20 4.2HardwareSetup ................. ............21 4.3TeachbyZoomingVisualServoControl...............25 4.4 3 TargetReconstructionBasedVisualServoControl.......26 4.5Conclusion.... ............................28 5TEACHBYZOOMINGVISUALSERVOCONTROLFORANUNCALIBRATEDCAMERASYSTEM....................30 5.1Introduction ...............................30 5.2ModelDevelopment...........................33 5.3HomographyDevelopment.......................38 5.4ControlObjective............................41 5.5ControlDevelopment..........................42 5.5.1RotationController.......................42 5.5.2TranslationController......................45 5.6SimulationResults...........................48 v

PAGE 6

5.6.1Introduction...........................48 5.6.2NumericalResults........................48 5.7ExperimentalResults..........................55 5.8Conclusion.... ............................56 6 3 TARGETRECONSTRUCTIONFORVISION-BASEDROBOT CONTROL............ ......................58 6.1ModelDevelopment...........................58 6.23DTargetReconstruction.......................61 6.3ControlDevelopment..........................63 6.3.1ControlObjective.. ......................64 6.3.2RotationController.......................65 6.3.3TranslationController......................67 6.4ExperimentalResults..........................67 6.4.1PerformanceValidationof 3 DepthEstimation.......68 6.4.2ExperimentI...........................69 6.4.3ExperimentII..........................74 6.4.4ExperimentIII..........................75 6.5Conclusion.... ............................76 7CONCLUSION.... ............................77 7.1SummaryofResults..........................77 7.2RecommendationsforFutureWork..................78 APPENDIX AOPEN-LOOPERRORDYNAMICS....................80 A.1RotationController...........................80 A.2TranslationController.........................82 BTARGETIDENTIFICATIONANDFEATUREPOINTTRACKING ALGORITHM.......... ......................84 B.1TargetDetection............................84 B.2FeaturePointTracking.........................103 CTEACHBYZOOMINGVISUALSERVOCONTROLALGORITHM..128 DVISION-BASED 3 TARGETRECONSTRUCTIONALGORITHM..147 REFERENCES............. ......................174 BIOGRAPHICALSKETCH............................177 vi

PAGE 7

LISTOFFIGURES Figure page 21Movingcameralookingatareferenceplane.................12 41Overviewofvision-basedautonomouscitrusharvesting...........21 42ARoboticsResearchK-1207iarticulatedroboticarm. ...........22 43Camerain-handlocatedatthecenteroftherobotend-e ector......23 44OverviewoftheTBZcontrolarchitecture..................27 4Overviewofthe 3 targetreconstruction-basedvisualservocontrol....29 51Cameraframecoordinaterelationships....................35 5Teachbyzoomingvisualservocontrolforaroboticmanipulator......36 53Overviewofteachbyzoomingvisualservocontroller............42 5Imagefromthe xedcamerabeforezooming(subscriptf)andafter zooming(superscript*)...........................52 55RotationErrors. ................... ............52 56TranslationErrors ................... ............53 57Euclideantrajectoryofthefeaturepointsviewedbythecamera-in-hand fromtheinitialpositionandorientation(denotedby+)tothedesired positionandorientation F(denotedbyx),wherethevirtualcamera coordinatesystem Fisdenotedbyo...................53 58Angularcontrolinputvelocityforthecamera-in-hand...........54 59Linearcontrolinputvelocityforthecamera-in-hand............54 510Eliminationofine ectualfeaturepointsvia.targetidenti cation.....55 5Featurepointmatchingbetween xedcameraandcamerain-hand....56 61Cameraframecoordinaterelationships....................59 63Dtargetreconstructionbasedvisualservocontrolforaroboticmanipulator................ ......................60 vii

PAGE 8

6PerspectiveprojectiongeometrymodelforEuclideandepthidenti cation.62 64Controlarchitecturedepictingrotationcontrol... ............64 6Controlarchitecturedepictingtranslationcontrol.. ............65 66Performancevalidationof 3 depthestimation...............69 67Depthestimationerror............................70 68Performancevalidationforrepeatabilitytest.................70 6 3 robottask-spacedepictingrepeatabilityresults. ............72 6Repeatabilityin -planeforthe3Dtargetreconstructionbasedvisual servocontroller. .................... ............73 6Repeatabilityin -planeforthe3Dtargetreconstructionbasedvisual servocontroller. .................... ............73 612PerformancevalidationforexperimentII...................74 613PerformancevalidationforexperimentIII..................75 viii

PAGE 9

LISTOFTABLES Table page 51Listofvariablesforteachbyzoomingvisualservocontrol.........34 61Listofvariablesfor3Dtargetreconstructionbasedvisualservocontrol..59 62Performancevalidationfor3Ddepthestimationmethod..........69 63ActualEuclideantargetpositionexpressedina xedcameraframeand robotbaseframe. ................... ............71 64EstimatedEuclideantargetpositionexpressedina xedcameraframe, robotbaseframe,androbottoolframe...................71 6Initialrobotend-e ectorpositionexpressedinrobotbaseframe.....71 6Finalrobotend-e ectorpositionexpressedinrobotbaseframe......72 67ActualEuclideantargetpositionexpressedina xedcameraframeand robotbaseframe. ................... ............74 6Initialand nalrobotend-e ectorpositionexpressedinrobotbaseframe.75 69ActualEuclideantargetpositionexpressedina xedcameraframeand robotbaseframeandinitialand nalrobotend-e ectorpositionexpressed inrobotbaseframe..............................76 ix

PAGE 10

AbstractofThesisPresentedtotheGraduateSchool oftheUniversityofFloridainPartialFul llmentofthe RequirementsfortheDegreeofMasterofScience VISION-BASEDCONTROLFORAUTONOMOUSROBOTICCITRUS HARVESTING By SiddharthaSatishMehta May2007 Chair:ThomasF.Burks Major:AgriculturalandBiologicalEngineering Vision-basedautonomousroboticcitrusharvestingrequirestargetdetection andinterpretationof3-dimensional(3D)Euclideanpositionofthetargetthrough 2Dimages.Thetypicalvisualservoingproblemisconstructedasateachby showing(TBS)problem.Theteachbyshowingapproachisformulatedasthe desiretoposition/orientacamerabasedonareferenceimageobtainedbyapriori positioningthesamecamerainthedesiredlocation.Anewstrategyisrequiredfor theunstructuredcitrusharvestingapplicationwherethecameracannotbeapriori positionedtothedesiredposition/orientation.Thereforeateachbyzooming approachisproposedwherebytheobjectiveistoposition/orientacamerabased onareferenceimageobtainedbyanothercamera.Forexample,a xedcamera providingaglobalviewofthetreecanopycanzoominonanobjectandrecord adesiredimageforacamerain-hand.Acontrollerisdesignedtoregulatethe imagefeaturesacquiredbyacamerain-handtothecorrespondingimagefeature coordinatesinthedesiredimageacquiredbythe xedcamera.Thecontroller isdevelopedbasedontheassumptionthatparametricuncertaintyexistsinthe cameracalibrationsinceprecisevaluesfortheseparametersaredi culttoobtain x

PAGE 11

inpractice.Simulationresultsdemonstratetheperformanceofthedeveloped controller. However,thenon-identicalfeaturepointmatchingbetweenthetwocameras limitstheapplicationofteachbyzoomingvisualservocontrollertothearti cial targets,wherethefeaturepointinformationisavailable apriori. Therefore,anew visualservocontrolstrategybasedon3Dreconstructionofthetargetfromthe 2Dimagesisdeveloped.The3Dreconstructionisachievedbyusingthestatistical data,viz.themeandiameterofthecitrusfruit,alongwiththetargetimagesize andthecamerafocallengthtogeneratethe3Ddepthinformation.Acontroller isdevelopedtoregulatetherobotend-e ectortothe3DEuclideancoordinates correspondingtothecentroidofthetargetfruit.Theautonomousroboticcitrus harvestingfacilityprovidesarapidprototypingtestbedforevaluatingsuchvisual servocontroltechniques.Experimentalvalidationofthe3Dtargetreconstructionbasedvisualservocontroltechniquedemonstratesthefeasibilityofthecontroller foranunstructuredcitrusharvestingapplication. xi

PAGE 12

CHAPTER1 INTRODUCTION Mechanizationofharvestingoffruitsishighlydesirableindevelopingcountries duetodecreaseinseasonallaboravailabilityandincreasingeconomicpressures. Themechanizedfruitharvestingtechnology haslimitationstoso ftfruitharvesting becauseoftheoccurrenceofexcessivedamagetotheharvest.Mechanicalharvestingsystemsarebasedontheprincipleofshakingorknockingthefruitoutofthe tree.Therearetwobasictypesnamelytrunkshakerandcanopyshake.Thetrunk shakerbasedsystemsattempttoremovethefruitfromthetreebysimplyshaking orvibratingthetreetrunkandallowingtheinducedvibrationsandoscillations tocausethefruittofalloutofthetree.Canopyshakersystemsconsistofnylon rodsthatrotateandshakefoliage.Thesenylonrodsallowforbettershakingof thecanopythanthetrunkshakersalone. However,therearevariousproblems associatedwiththesestrictlymechanicalharvestersystems.Typicallycitrushas strongattachmentbetweenthetreebranchandthefruit,henceitmayrequire largeamountofshakingforharvestingthefruitwhichmaycausedamagetothe tree,suchasbarkremovalandbrokenbranches.Moreover,duetomechanical shakingsystemandconveyorbeltcollectionsystems,mechanicalharvestersystems cancausefruitqualitydeteriorationthusmakingmechanicalharvestingsystems typicallysuitableforjuicefruitquality.Sincethereisstillalargepercentageof citrusthatissoldasfreshmarketfruitandwhichcannotbedamagedinanyway. Analternativetomechanicalharvestingsystems,yieldingsuperiorfruitquality,is theuseofautomatedroboticharvestingsystems. Characteristicsoftheautomatedfruitharvestingrobot:(a)Abletolocate thefruitsonthetreeinthreedimensions(3D)(b)abletoapproachandreachfor 1

PAGE 13

2 thefruit(c)abletodetachthefruitaccordingtoapredeterminedcriterion,and transferittoasuitablecontainerand(d)tobepropelledintheorchardwithease ofmaneuverability,fromtreetotreeandrowtorow,whilenegotiatingvarious terrainsandtopographies.Inaddition,adesirablecapabilityistheversatilityto handleavarietyoffruitcropswithminimalchangesinthegeneralcon guration [30]. Alltheseoperationsshouldbeperformedwithinthefollowingconstrains; Pickingratefasterthan,orequalto,manualpicking(byagroupofworkers), Fruitqualityequaltoorsuperiortothatobtainedwithmanualpicking,and Economicallyjusti able. Robotsperformwellinstructuredenvironment,wheretheEuclideanposition andorientationoftargetandobstaclesisknown.But,inpresenceofunstructured urban/non-urbanenvironmentsthedi cultyisfacedofdeterminingthethree dimensionalEuclideanpositionandorientationofthetargetandpathplanning forobstacleavoidance.Thetraditionalindustrialapplicationofroboticsinvolve structuredenvironment,butrobotics eldisspreadingtoreachnon-traditional applicationareassuchasmedicalrobotsandagriculturalpre-harvestingandpostharvesting.Thesenon-traditionalapplicationsrepresentworkingofarobotinan unstructuredenvironment. Severalautonomous/semi-autonomoustechniqueshavebeenproposedfor thetargetdetectionandrangeidenti cationinunstructuredenvironments.Ceres etal.[3]developedan-aidedfruitharvestingstrategywheretheoperatordrives theroboticharvesterandperformsthedetectionoffruitsbymeansofalaser range nder;thecomputerperformsthepreciselocationofthefruits,computes adequatepickingsequencesandcontrolsthemotionofmanipulatorjoints.The infraredlaserrangenderusedisbasedontheprincipleofphaseshiftbetween emittedandre ectedamplitudemodulatedlasersignal.Since,theprocessof

PAGE 14

3 detectionandthelocalizationofthefruitisdirectedbyahumanoperator,this approachheavilydependsonhumaninterfaceintheharvesting.DEsnonetal.[14] proposedanappleharvestingprototyperobotMAGALI,implementingaspherical manipulator,mountedonahydraulicallyactuatedselfguidedvehicle,servoedby acamerasetmountedatthebaserotationjoint.Whenthecameradetectsafruit, themanipulatorarmisorientatedalongthelineofsightontothecoordinatesofthe targetandismovedforwardtillitreachesthefruit,whichissensedbyaproximity sensor.However,theuseofproximitysensorcouldresultindamagingtheharvest ormanipulator.Moreover,thevisionsystemimplementedrequiresthecamerato beelevatedalongthetreetoscanthetreefrombottomtotop.Rabateletal.[29] initiatedaFrenchSpanishEUREKAprojectnamedCITRUS,whichwaslargely basedonthedevelopmentofFrenchroboticharvesterMAGALI.TheCITRUS roboticsystemusedaprincipleforrobotarmmotion:oncethefruitisdetected, thestraightlinebetweenthecameraopticalcenterandthefruitcanbeusedas atrajectoryforthepickingdevicetoreachthefruit,asthislineisguaranteedto befreeofobstacles.TheprincipleusedhereisthesameasMAGALI,andthus thisapproachalsofailstodeterminethethreedimensional(3-D)positionofthe fruitintheEuclideanworkspace.Bulanonetal.[2]developedthemethodfor determining3-Dlocationoftheapplesforroboticharvestingusingcamerain-hand con guration.Bulanonetal.usedthedi erentialobjectsizemethodandbinocular stereovisionmethodfor3-Dpositionestimation.Indi erentialsizemethod,the changeintheobjectsizeasthecameramovestowardstheobjectwasusedto computethedistance,whereasthebinocularstereovisionmethodcalculatesthe distanceoftheobjectfromthecamerausingtwoimagesfromdi erentviewpoints onthetriangularmeasurementpr inciple.However,rangeidenti cationaccuracy ofthesemethodswasnotsatisfactoryforroboticguidance.InadditionBulanon etal.consideredlaserrangingtechniqueforaccuraterangeidenti cationofthe

PAGE 15

4 object.Murakamietal.[28]proposedavis ion-basedroboticcabbageharvester.The robotconsistsofathreedegreesoffreedom(3-DOF)polarhydraulicmanipulator andacolorCCDcameraisusedformachinevision.Murakamietal.suggested theuseofhydraulicmanipulatorsincetheyhavehighpowertoweightratioand donotrequirehighpowersupplytoautomatethesystem,whichisjusti edfor thecabbageharvestingapplication.Theimageprocessingstrategyistorecognize thecabbageheadsbyprocessingthecolorimagewhichistakenunderunstable lightingconditionsinthe eld.Theimageprocessingusesneuralnetworkto extractcabbageheadsoftheHIStransformedimage.Thelocationanddiameter ofcabbageareestimatedbycorrelationwiththeimagetemplate.Sincecabbage harvestingrepresentsa2-Dvisualservocontrolproblem,itispossibletoapplythe templatematchingtechniquefor3-Dlocationestimation.Butforapplicationslike citrusharvesting,whichrequire3-Dvisualservocontrolofroboticmanipulator, exactthreedimensionalEuclideancoordinatesaretobedetermined.Hayashietal. studiedthedevelopmentofroboticharvestingsystemthatperformsrecognition, approach,andpickingtasksforeggplants[19].Theroboticharvestingsystem consistsofamachinevisionsystem,amanipulatorunit,andanend-e ectorunit. Thearticulatedmanipulatorwith5-DOFwithcamerain-handcon guration wasselectedforharvestingapplication.Afuzzylogicbasedvisualservocontrol techniqueswereadaptedformanipulatorguidance.Thevisualfeedbackfuzzy controlmodelwasdesignedtodeterminetheforwardmovement,verticalmovement androtationalangleofthemanipulatorbasedonthepositionofthedetectedfruit inanimageframe.Whentheareaoccupiedbythefruitismorethan70%ofthe totalimage,theservoingisstoppedandeggplantispicked. Withrecentadvancesincameratechnology,computervision,andcontroltheory,visualservocontrolsystemsexhibitsigni cantpromisetoenableautonomous systemswiththeabilitytooperateinunstructuredenvironments.Althougha

PAGE 16

5 visionsystemcanprovideauniquesenseofperception,severaltechnicalissues haveimpactedthedesignofrobustvisualservocontrollers.Oneoftheseissues includesthecameracon guration(pixelresolutionversus eld-of-view).Forexample,forvisionsystemsthatutilizeacameraina xedcon guration(i.e.,the eye-to-handcon guration),thecameraistypicallymountedatalargeenough distancetoensurethatthedesiredtargetobjectswillremaininthecameras view.Unfortunately,bymountingthecamerainthiscon gurationthetask-space areathatcorrespondstoapixelintheimage-spacecanbequitelarge,resulting inlowresolutionandnoisypositionmeasurements.Moreover,manyapplications areill-suitedforthe xedcameracon guration.Forexample,arobotmaybe requiredtopositionthecameraforclose-uptasks(i.e.,thecamera-in-handcon guration).Forthecamera-in-handcon guration,thecameraisnaturallyclosetothe workspace,providingforhigherresolutionmeasurementsduetothefactthateach pixelrepresentsasmallertask-spacearea;however,the eld-of-viewofthecamera issigni cantlyreduced(i.e.,anobjectmaybelocatedintheworkspacebutbeout ofthecamerasview). Severalresultshavebeenrecentlydevelopedtoaddressthecameracon gurationissuesbyutilizingacooperativecamerastrategy.Theadvantagesofthe cooperativecameracon gurationarethatthe xedcameracanbemountedso thatthecompleteworkspaceisvisibleandthecamera-in-handprovidesahigh resolution,close-upviewofanobject(e.g.,facilitatingthepotentialformore precisemotionforroboticapplications).Speci cally,Flandinetal.[13]madethe rststepstowardscooperativelyutilizingglobalandlocalinformationobtained froma xedcameraandacamera-in-hand,respectively;unfortunately,toprovethe stabilityresults,thetranslationandrotationtasksofthecontrollerweretreated separately(i.e.,thecouplingtermswere ignored)andthecameraswereconsidered tobecalibrated.Dixonetal.[6],[7]developedanewcooperativevisualservoing

PAGE 17

6 approachandexperimentallydemonstratedtouseinformationfrombothanuncalibrated xedcameraandanuncalibratedcamerain-handtoenablerobusttracking bythecamera-in-handofanobjectmovinginthetask-spacewithanunknown trajectory.Unfortunately,acrucialassumptionforthisapproachisthatthecamera andtheobjectmotionbeconstrainedtoaplanesothattheunknowndepthfrom thecameratothetargetremainsconstant.TheapproachespresentedbyDixon etal.[6],[7]andFlandinetal.[13]areincontrasttotypicalmulti-cameraapproaches,thatutilizeastereo-basedcon guration,sincestereo-basedapproaches typicallydonotsimultaneouslyexploitlocalandglobalviewsoftherobotandthe workspace. Anotherissuethathasimpactedthedevelopmentofvisualservosystemsisthe calibrationofthecamera.Forexample,torelatepixelizedimage-spaceinformation tothetask-space,exactknowledgeofthecameracalibrationparametersisrequired, anddiscrepanciesinthecalibrationmatrixresultinanerroneousrelationship. Furthermore,anacquiredimageisafunctionofboththetask-spacepositionof thecameraandtheintrinsiccalibrationparameters;hence,perfectknowledge ofthecameraintrinsicparametersisalsorequiredtorelatetherelativeposition ofacamerathroughtherespectiveimagesasitmoves.Forexample,thetypical visualservoingproblemisconstructedasateachbyshowing(TBS)problem inwhichacameraispositionedatadesiredlocation,areferenceimageistaken (wherethenormalizedtask-spacelocationisdeterminedviatheintrinsiccalibration parameters),thecameraismovedaway fromthereferencelocation,andthen repositionedatthereferencelocationundervisualservocontrol(whichrequires thatthecalibrationparametersdidnotchangeinordertorepositionthecamerato thesametask-spacelocationgiventhesameimage).See[4],[20],[18],and[23]for afurtherexplanationandanoverviewofthisproblemformulation.

PAGE 18

7 Unfortunately,formanypracticalapplicationsitmaynotbepossibletoTBS (i.e.,itmaynotbepossibletoacquirethereferenceimagebyaprioripositioning thecamera-in-handtothedesiredlocation).AsstatedbyMalis[23],theTBS problemformulationiscamera-dependentduetothehypothesisthatthe cameraintrinsicparametersduringtheteachingstage,mustbeequaltothe intrinsicparametersduringservoing.Motivatedbythedesiretoaddressthis problem,thebasicideaofthepioneeringdevelopmentbyMalis[22],[23]istouse projectiveinvariancetoconstructanerrorfunctionthatisinvarianttotheintrinsic parameters,thusenablingthecontrolobjectivetobemetdespitevariationsinthe intrinsicparameters.However,thefundamentalideafortheproblemformulation isthatanerrorsystembeconstructedinaninvariantspace,andunfortunately,as statedbyMalis[22],[23]severalcontrolissuesandarigorousstabilityanalysisof theinvariantspaceapproachhavebeenleftunresolved. InspiredbythepreviousissuesandthedevelopmentbyDixonetal.[7] andMalis[22],[23],thepresentedworkutilizesacooperativecamerascheme toformulateavisualcontrolproblemthatdoesnotrelyontheTBSparadigm. Speci cally,acalibratedcamera-in-handandacalibrated xedcameraareused tofacilitateateachbyzoomingapproachtoformulateacontrolproblem withtheobjectivetoforcethecamera-in-handtotheexactlyknownEuclidean position/orientationofavirtualcamerade nedbyazoomedimagefroma xed camera(i.e.,thecamera-in-handdoesnotneedtobepositionedinthedesired locationaprioriusingthisalternativeapproach).Sincetheintrinsiccamera calibrationparametersmaybedi culttoexactlydetermineinpractice,asecond controlobjectiveisde nedasthedesiretoservoanuncalibratedcamera-in-hand sothattherespectiveimagecorrespondstothezoomedimageofanuncalibrated xedcamera.Foreachcase,the xedcameraprovidesaglobalviewofascenethat canbezoomedtoprovideaclose-upviewofthetarget.Thereferenceimageis

PAGE 19

8 thenusedtoservothecamera-in-hand.Thatis,theproposedteachbyzooming approachaddressesthecameracon gurationissuesbyexploitingthewidescene viewofa xedcameraandtheclose-upviewprovidedbythecamera-in-hand.

PAGE 20

CHAPTER2 FUNDAMENTALSOFFEATUREPOINTTRACKINGANDMULTI-VIEW PHOTOGRAMMETRY 2.1Introduction Thischapterprovidesintroductiontofeaturepointdetectionandtracking techniquescommonlyusedforvision-basedcontrolstrategiesalongwithmultiviewphotogrammetryconceptstodeterminetherelationshipbetweenvarious cameraframecoordinatesystems.Thechapterisorganizedinthefollowing manner.InSection2.2,theimageprocessingalgorithmforreal-timefeature detectionandtrackingisdiscussed.InSection2.3,implementationofamulti-view photogrammetrytechniqueisillustrated,andconcludingremarksareprovidedin Section2.4. 2.2ImageProcessing Manyvision-basedestimationandcontrolstrategiesrequirereal-timedetection andtrackingoffeatures.Inordertoeliminatetheine ectualfeaturepoints, featurepointdetectionandtrackingisperformedonlyonthetargetofinterest. Hence,colorthresholding-basedimagesegmentationmethodsareusedfortarget (i.e.citrusfruit)detection(seeAppendixB.1).Themostimportantsegmentof theimageprocessingalgorithm(seeAppendixB.2)isreal-timeidenti cationof featuresrequiredforcoordinatedguidance,navigationandcontrolincomplex3D surroundingssuchasurbanenvironments.ThealgorithmisdevelopedinC/C++ usingVisualStudio6.0.IntelsOpenSourceComputerVisionLibraryisutilized forreal-timeimplementationofmostoftheimageprocessingfunctions.Also, GNUScienti cLibrary(GSL)isused,whichisanumericallibraryforCandC++ programming.GSLprovidesawiderangeofmathematicalroutinesformostofthe 9

PAGE 21

10 computationinvolvedinthealgorithms.Therestofthissectiondescribesfeature pointdetectionandKanade-Lucas-Tomasi(KLT)featurepointtrackingmethod. ThecolorspaceistransformedfromRGBtoanewcolorspaceCrCgCbhaving componentsofR,G,andB,respectively,utilizingthefollowingtransformation: = 0 721 0 587 0 114 0 2990 413 0 114 0 299 0 5870 886 (21) Colorthresholdingmethodsarethenappliedonthecolortransformedimage fortargetsegmentationandfeaturepointtrackingisperformedonthedetected target. Asstatedin[31],nofeature-basedvisionsystemcanworkunlessgoodfeatures canbeidenti edandtrackedfromframetoframe.Twoimportantissuesneed tobeaddressed;amethodforselectinggoodandreliablefeaturepoints;anda methodtotrackthefeaturepointsframetoframe.Theseissuesarediscussedin detailin[21]and[31],wheretheKanade-Lucas-Tomasi(KLT)trackerispromoted asasolution. Thealgorithmdeveloped(seeAppendixB.2)isbasedontheKLTtrackerthat selectsfeatureswhichareoptimalfortracking.ThebasicprincipleoftheKLT isthatagoodfeatureisonethatcanbetrackedwell,sotrackingshouldnotbe separatedfromfeatureextraction.Asstatedin[21],agoodfeatureisatextured patchwithhighintensityvariationinboth and directions,suchasacorner.By representingtheintensityfunctionin and directionsby and ,respectively, wecande nethelocalintensityvariationmatrix, ,as = 2 2 (22)

PAGE 22

11 Apatchde nedbya7x7pixelswindowisacceptedasacandidatefeatureifin thecenterofthewindowbotheigenvaluesof ,exceedaprede nedthreshold.Two largeeigenvaluescanrepresentcorners,saltandpeppertextures,oranyotherpatternthatcanbetrackedreliably.Twosma lleigenvaluesmeanaroughlyconstant intensitypro lewithinawindow.Alargeandasmalleigenvaluescorrespondtoa unidirectionaltexturepattern.Theintensityvariationsinawindowarebounded bythemaximumallowablepixelvalue,sothegreatereigenvaluecannotbearbitrarilylarge.Inconclusion,ifthetwoeigenvaluesof are 1and 2,weaccepta windowif ( 12) (23) where isaprede nedthreshold. Asdescribedin[31],featurepointtrackingisastandardtaskofcomputer visionwithnumerousapplicationsinnavigation,motionunderstanding,surveillance,scenemonitoring,andvideodatabasemanagement.Inanimagesequence, movingobjectsarerepresentedbytheirfeaturepointsdetectedpriortotracking orduringtracking.Asthescenemoves,thepatternsofimageintensitieschanges inacomplexway.Denotingimagesby ,thesechangescanbedescribedasimage motion: ( + )= ( ( ) ( )) (24) Thusalaterimagetakenattime + (where representsasmalltimeinterval) canbeobtainedbymovingeverypointinthecurrentimage,takenattime ,bya suitableamount.Theamountofmotion =( ) iscalledthedisplacementofthe pointat =( ) .Thedisplacementvector isafunctionofimageposition Tomasiin[31],statesthatpuretranslationisthebestmotionmodelfortracking

PAGE 23

12 Figure2:Movingcameralookingatareferenceplane. becauseitexhibitsreliabilityandaccuracyincomparingfeaturesbetweenthe referenceandcurrentimage,hence = where isthetranslationofthefeatureswindowcenterbetweensuccessiveframes. Thus,theknowledgeoftranslation allowsreliableandoptimaltrackingofthe featurepointswindows. Inthealgorithmdevelopedfortheteachbyzoomingvisualservocontrol,the methodemployedfortrackingisbasedonthepyramidalimplementationofthe KLTTrackerdescribedin[1]. 2.3Multi-viewPhotogrammetry Thefeaturepointsdataobtainedduringmotionthroughthevirtualenvironmentisusedtodeterminetherelationshipbetweenthecurrentandaconstant referencepositionasshowninFigure.21.Thisrelationshipisobtainedbydeterminingtherotationandtranslationbetweencorrespondingfeaturepointsonthe currentandreferenceimageposition.Therotationandtranslationcomponents

PAGE 24

13 relatingcorrespondingpointsofthereferenceandcurrentimageisobtainedby rstconstructingtheEuclideanhomographymatrix.Varioustechniquescanthen beused(e.g.,see[11],[32])todecompos etheEuclideanhomographymatrixinto rotationalandtranslationalcomponents.Followingsectiondescribesmultiview photogrammetry-basedEuclideanreconstructionmethodtoobtainunknown rotationandtranslationvectorsforcameramotion. Considertheorthogonalcoordinatesystems,denoted F and Fthatare depictedinFigure.2.Thecoordinatesystem F isattachedtoamovingcamera. Areferenceplane ontheobjectisde nedbyfourtargetpoints =1 2 3 4 wherethethreedimensional( 3 )coordinatesof expressedintermsof F and Farede nedaselementsof ( ) and R3andrepresentedby ( ) ( ) ( ) ( ) (25) (26) TheEuclidean-spaceisprojectedontotheimage-space,sothenormalizedcoordinatesofthetargetspoints ( ) and arede nedas = = 1 (27) = = 1 (28) underthestandardassumptionthat ( ) and ,where denotesanarbitrarilysmallpositivescalarconstant. Eachtargetpointhaspixelcoordinatesthatareacquiredfromthemoving camera,expressedintermsof F ,denotedby ( ) ( ) R ,andarede nedas elementsof ( ) R3asfollows: 1 .(2)

PAGE 25

14 Thepixelcoordinatesofthetargetpointsatthereferencepositionisexpressedin termsof F(denotedby R )andarede nedaselementsof R3as follows: 1 (2) Thepixelcoordinates ( ) and arerelatedbythefollowingglobalinvertible transformation(i.e.,thepinholemodel)tothenormalizedtask-spacecoordinates ( ) and respectively: = (2) = where istheintrinsiccameracalibrationmatrix. Theconstantdistancefromtheoriginof Ftotheobjectplane alongthe unitnormal isdenotedby R andisde nedas .(2) Thecoordinateframes F and FdepictedinFigure.2areattachedtothe cameraanddenotetheactualandreferencelocationsofthecamera.Fromthe geometrybetweenthecoordinateframes, canberelatedto ( ) asfollows = + (2) In(2), ( ) (3) denotestherotationbetween F and F,and ( ) R3denotesthetranslationvectorfrom F to Fexpressedinthecoordinateframe F Byutilizing(27),(2),and(22),theexpressionsin(23)canbewrittenas follows = + | {z } (2)

PAGE 26

15 In(2), ( ) R3denotesthefollowingscaledtranslationvector = (2) and ( ) denotesthedepthratiode nedas = (2) Aftersubstituting(211)into(24),thefollowingrelationshipscanbedeveloped = 1 | {z } (2) where ( )=[ ( )] =1 2 3 R3 3denotesaprojectivehomography matrix.Afternormalizing ( ) by 33( ) ,whichisassumedtobenon-zerowithout lossofgenerality,theprojectiverelationshipin(217)canbeexpressedasfollows: = 33 (2) where R3 3denotesthenormalizedprojectivehomography.From(2),a setof12linearlyindependentequationsgivenbythe4targetpointpairs ( ( )) with3independentequationspertargetpaircanbeusedtodetermine ( ) and ( ) 33( ) .Basedonthefactthatintrinsiccameracalibrationmatrix isassumed tobeknown,(217)and(28)canbeusedtodetermine 33( ) ( ) .Various techniquescanthenbeused(e.g.,see[11,32])todecomposetheproduct 33( ) ( ) intorotationalandtransla tionalcomponents.Speci cally,thescalefactor 33( ) therotationvector ( ) theunitnormalvector ,andthescaledtranslation vectordenotedby ( ) canallbecomputedfromthedecompositionoftheproduct 33( ) ( ) .Sincetheproduct ( ) 33( ) canbecomputedfrom(2),and 33( ) canbedeterminedthroughthedecompositionoftheproduct 33( ) ( ) ,thedepth ratio ( ) canalsobecomputed.

PAGE 27

16 2.4Conclusion Inthischapter,wediscussedtechniquesforreal-timeidenti cationand trackingoffeaturesrequiredforteachbyzoomingvisualservocontrolmethod. Thedevelopmentincludeddescriptionoffeaturepointtrackingalgorithmsutilizing Kanade-Lucas-Tomastrackeralongwithcolorthresholding-basedimageprocessing methodsandmulti-viewphotogrammetrytechniquesarerealizedtorelatevarious coordinateframes.

PAGE 28

CHAPTER3 OBJECTIVES AsdescribedinChapter1,automatedroboticcitrusharvestingyieldssuperior fruitqualityoverthemechanicalharvestingsystems,whichishighlydesirablefor freshfruitmarket.Characteristicsoftheautomatedfruitharvestingrobot:(a) Abletolocatethefruitsonthetreeinthreedimensions(3D)(b)abletoapproach andreachforthefruit(c)abletodetachthefruitaccordingtoapredetermined criterion,andtransferittoasuitablecontainerand(d)tobepropelledinthe orchardwitheaseofmaneuverability,fromtreetotreeandrowtorow,while negotiatingvariousterrainsandtopographies.Theobjectiveofthepresented workistoaccomplishcharacteristics(a)and(b)oftheautomatedfruitharvesting systemmentionedabove.Colorthresholdi ng-basedtechniqueisrealizedfortarget fruitidenti cation,whereasmulti-cameravisualservocontroltechniquesare developedfor 3 targetpositionestimationandrobotmotioncontrol. Withrecentadvancesincameratechnology,computervision,andcontroltheory,visualservocontrolsystemsexhibitsigni cantpromisetoenableautonomous systemswiththeabilitytooperateinunstructuredenvironments.Roboticharvestingisoneoftheapplicationsofvisualserv ocontrolsystemworkinginunstructured environment.Typicalvisualservoingcontrolobjectivesareformulatedbythedesire toposition/orientacamerausingmulti-sensorstereovisiontechniquesorbasedon areferenceimageobtainedbyapriorypositioningthesamecamerainthedesired location(i.e.,theteachbyshowingapproach).Theshortcomingofstereovision basedvisualservocontroltechniqueislackofthethreedimensionalEuclideantask spacereconstruction.Also,teachbyshowingvisualservocontroltechniqueisnot 17

PAGE 29

18 feasibleforapplicationswherethecameracannotbe apriori positionedtothe desiredposition/orientation. Anewcooperativecontrolstrategy,teachbyzooming(TBZ)visualservo controlisproposedwheretheobjectiveistoposition/orientacamerabased onareferenceimageobtainedbyanothercamera.Forexample,a xedcamera providingaglobalviewofanobjectcanzoominonanobjectandrecordadesired imageforthecamera-in-hand. Atechnicalissuethathasimpactedthedesignofrobustvisualservocontrollersisthecameracon guration(i.e.thepixelresolutionversus eld-of-view). Teachbyzoomingvisualservocontroltechniquee ectivelyidentifyingboththe con gurationsprovideshighresolutioni magesobtainedbycamera-in-handand large eld-of-viewby xedcamera.Anotherissueassociatedwithvisualservo controlsystemsisthecameracalibration.Inordertorelatepixelizedimage-space informationtothethreedimensionaltask-space,exactknowledgeofthecamera calibrationparametersisrequired,anddiscrepanciesinthecalibrationmatrix resultinanerroneousrelationshipbetweentargetimage-spacecoordinatesandthe correspondingEuclideancoordinates.Therefore,thecontrollerisdevelopedbased ontheassumptionthatparametricuncertaintyexistsinthecameracalibration sinceprecisevaluesfortheseparametersaredi culttoobtaininpractice. Homography-basedtechniquesareusedtodecoupletherotationandtranslationcomponentsbetweenthecurrentimagetakenbycamera-in-handanddesired referenceimagetakenbya xedcamera.Sinceteachbyzoomingcontrolobjective isformulatedintermsofimagesacquiredfromdi erentuncalibratedcameras,the abilitytoconstructameaningfulrelationshipbetweentheestimatedandactual rotationmatrixisproblematic.Toovercomethischallenge,thecontrolobjectiveis formulatedintermsofthenormalizedEuclideancoordinates.Speci cally,desired normalizedEuclideancoordinatesarede nedasafunctionofthemismatchinthe

PAGE 30

19 cameracalibration.Thisisaphysicallymotivatedrelationship,sinceanimage isafunctionofboththeEuclideancoordinatesandthecameracalibration.By utilizing,Lyapunov-basedmethods,acontrollerisdesignedtoregulatetheimage featuresacquiredbythecamera-in-handtothecorrespondingimagefeaturecoordinatesinthedesiredimageacquiredbythe xedcamera.Sincetheestimatesof intrinsiccalibrationparametersareusedincontrollerdevelopmentthemethodis robusttothecameracalibrationparamete rs.Also,proposedcontrolschemedoes notrelyonothersensorse.g.ultrasonicsensors,laserbased,sensors,etc. Anothermulti-cameravisualservocontrolstrategy, 3 targetreconstruction basedvisualservocontrol,hasbeenproposedfortargetpositionestimationinthe Euclideanspace.Thetechniqueutilizesstatisticaldataalongwithcameraintrinsic parametersfor 3 positionestimation.Similartoteachbyzoomingvisualservo control, 3 targetreconstructionbasedvisualservocontroltechniquee ectively utilizingboththecon gurationsprovideshighresolutionimagesobtainedby camera-in-handandlarge eld-of-viewby xedcamera.The xedcameraproviding aglobalviewofthetargetisusedfortargetdetectionandimageprocessingthus optimizingthecomputationtime,whileus ingfeedbackfromcamera-in-handduring visualservoing.

PAGE 31

CHAPTER4 METHODSANDPROCEDURES 4.1Introduction Thischapterprovidesanoverviewofthevision-basedcontroltechniques, namelytheteachbyzoomingvisualservocontrollerand 3 targetreconstruction basedvisualservocontroller,realizedforautonomouscitrusharvestingalongwith theexperimentaltestbed.Theexperimentalfacilityprovidesarapidprototyping testbedforautonomousroboticguidanceandcontrol. Thehardwareinvolvedfortheexperimentaltestbedfacilitycanbesubdivided intothefollowingmaincomponents:(1)roboticmanipulator;(2)end-e ector;(3) robotservocontroller;(4)imageprocessingworkstation;and(5)networkcommunication.Thesoftwaredevelopedfortheautonomousroboticcitrusharvestingis basedonimageprocessingandcomputervisionmethods,includingfeaturepoint trackingandmulti-viewphotogrammetrytechniques.Anoverviewofvision-based autonomouscitrusharvestingmethodisillustratedinFigure.41anddescribedas follows: 1.ImagesofthecitrustreecanopyarecapturedbyaCCDcameraandpassed totheimageprocessing/visualservocontrolworkstationthroughaUSBanalogto digitalconverterandframegrabber. 2.Imageprocessingandcomputervisionalgorithmsidentifythetarget,i.e. citrusfruit,fromtheimageofatreecanopy.Featurepointsareidenti edandthe relationshipbetweencurrentanddesiredimagesisdetermined,inteachbyzooming visualservocontrol,togeneratethecontrolcommands.In 3 targetreconstruction technique,theestimatefortheEuclideanpositionofatargetisdeterminedto providetherobotcontrolworkstationwithcontrolcommands. 20

PAGE 32

21 Figure4:Overviewofvision-basedautonomouscitrusharvesting. 3.Positionandorientationcommandsaredevelopedforroboticarmbythe robotcontrolworkstationutilizinglowlevelcontroller. 4.Real-timenetworkcommunicationcontrolisestablishedbetweentheimage processingworkstationandrobotcontrolworkstation. ThehardwaresetupoftheexperimentaltestbedfacilityisdescribedinSection 4.2.TeachbyzoomingvisualservocontrolmethodisdescribedinSection4.3, whileSection4.4illustratesthe 3 targetreconstructionbasedvisualservocontrol techniqueandconcludingremarksareprovidedinSection4.5. 4.2HardwareSetup Thehardwarefortheexperimentaltestbedforautonomouscitrusharvesting consistsofthefollowing vemaincomponents:(1)roboticmanipulator;(2)ende ector;(3)robotservocontroller;(4)imageprocessingworkstation;and(5)

PAGE 33

22 Figure4:ARoboticsResearchK-1207iarticulatedroboticarm. networkcommunication.Therestofthesectionprovidesdetailsabouteachofthe componentsmentionedabove. The rstcomponentistheroboticmanipulator.Theexperimentaltest-bed consistsofaRoboticsResearchK-1207i,a7-axis,kinematically-redundantmanipulatorasshowninFigure.4.TheK-1207imodel,o eringa50inchreachand a35lbcontinuous-dutypayloadisalightweightelectric-drivearticulatedrobotic arm.Theroboticmanipulatorisoperatedinacooperativecameracon guration. Thecooperativecameracon gurationincludesacamerain-hand,whichisattached totheend-e ectorofthemanipulatorasshowninFigure.(4),anda xedcamerahavingzoomingcapabilitiesmountedonthestationarybasejointsuchthatthe targetisalwaysinthe eld-of-view(FOV).The xedcamerathusprovidesaglobal viewofthetreecanopyandcanbeusedtozoom-inonthetarget,viz.acitrus fruit,tocapturethedesiredimageforthecamerain-hand.Thecamerasusedfor thiscon gurationareKT&Cmake(model:KPCS20-CP1) xedfocallength,color

PAGE 34

23 Figure4:Camerain-handlocatedatthecenteroftherobotend-e ector. CCDconepinholecameras.TheimageoutputfromthecamerasisNTSCanalog signalwhichisdigitizedusinguniversalserialbus(USB)framegrabbers. Thesecondcomponentistherobotend-e ector.Theend-e ectorisan accessorydeviceortoolspeci callydesignedforattachmenttotherobotwristor toolmountingplatetoenabletherobott operformitsintendedtask.Theende ectorusedforthisexperimentisa3-linkelectricallyactuatedgrippermechanism developedbyS.FloodattheUniversityofFlorida.Thegripperlinksarepadded withsoftpolymermaterialtoavoiddamagingthefruit.AsseeninFigure.(4), acameraalongwithaninfraredsensorislocatedatthecenterofthegripper.An infraredsensorisusedasaproximitysensortoactivatethegrippermechanism whenthetargetfruitisatthecenteroftheend-e ector.Serialportcommunication isimplementedforaninfraredsensordataacquisitionandgrippermotorcontrol. Theend-e ectorcanalsoaccommodateanultrasonicsensorforrangeidenti cation.

PAGE 35

24 Thethirdcomponentoftheautonomouscitrusharvestingtestbedisrobot servocontrolunit.Theservocontrolunitprovidesalowlevelcontroloftherobot manipulatorbygeneratingtheposition/orientationcommandsforeachjoint. RoboticsResearchR2ControlSoftwareprovidesthelowlevelrobotcontrol.The robotcontrollerconsistsoftwoprimarycomponentsofoperation;theINtime real-timecomponent(R2RTC)andtheNTclient-serveruppercontrollevel component.TheR2RTCprovidesdeterministic,hardreal-timecontrolwith typicallooptimesof1to4milliseconds.Thiscomponentperformstrajectory planning,Cartesiancomplianceandimpedanceforcecontrol,forwardkinematics, andinversekinematics.Thecontrollercanacceptcommandsintheformofhigh levelCartesiangoalpointsdowntolowlevelservocommandsofjointposition, torqueorcurrent.TherobotservocontrolisperformedonaMicrosoftWindows XPplatformbasedIBMpersonalcomputer(PC)with1.2GHzIntelPentium Celeronprocessorand512MBrandomaccessmemory(RAM). Theforthcomponentistheimageprocessingworkstation,whichisusedfor imageprocessingandvision-basedcontrol.Multi-cameravisualservocontrol techniquedescribedhereconsistsofa xedcameramountedonthestationary basejointoftherobotwhereasacamerain-handisattachedtotherobotende ector.The xedcameraprovidesaglobalviewofthetreecanopyandcan beusedtocapturetheimageforthetarge tfruit.MicrosoftDirectXandIntel OpenComputerVision(OpenCV)librariesareusedforimageextractionand interpretation,whereasLucas-Kanade-Tomasi(KLT)basedmulti-resolution featurepointtrackingalgorithm,developedinMicrosoftVisualC++6.0,tracks thefeaturepointsdetectedinthepreviousstageinboththeimages.Multi-view photogrammetrybasedmethodisusedtocomputetherotationandtranslation betweenthecamerain-handframeand xedcameraframeutilizingthetracked featurepointinformation.AnonlinearLyapunov-basedcontroller,developed

PAGE 36

25 inChapter5,isimplementedtoregulatetheimagefeaturesfromthecamera in-handtothedesiredimagefeaturesacquiredbythe xedcamera.Theimage processingandvision-basedcontrolareperformedonaMicrosoftWindowsXP platformbasedPCwith2.8GHzIntelPentium4processorand512MBRAM. The fthcomponentisthenetworkcommunicationbetweentherobotservocontrol workstationandtheimageprocessingworkstation.Adeterministicandhardrealtimenetworkcommunicationcontrolisestablishedbetweenthesecomputersusing INtimesoftware. 4.3TeachbyZoomingVisualServoControl Teachbyzooming(TBZ)visualservocontrolapproachisproposedfor applicationswherethecameracannotbe apriori positionedtothedesired position/orientationtoacquireadesiredimagebeforeservocontrol.Speci cally, theTBZcontrolobjectiveisformulatedtoposition/orientanon-boardcamera basedonareferenceimageobtainedby anothercamera.Anoverviewofthe completeexperimentaltestbedisillustratedinFigure.(4)anddescribedinthe followingsteps: 1)Acquiretheimageofthetargetfruitforthe xedcamera.Thetargetfruit canbeselectedmanuallyorautonomouslybyimplementingcostfunctionbased imageprocessing. 2)Digitallyzoom-inthe xedcameraonthetargetfruittoacquirethe desiredimage,whichispassedtotheimag eprocessingworkstation.Theamount ofmagni cationcanbedecidedbasedonthesizeofafruitintheimageandthe imageaspectratio. 3)RunthefeaturepointextractionandKLT-basedmulti-resolutionfeature pointtrackingalgorithmonthedesiredimageforthe xedcamera. 4)Orienttherobotend-e ectortocapturethecurrentimageofthetargetby thecamerain-hand,whichispassedtotheimageprocessingworkstation.

PAGE 37

26 5)Runthefeaturepointextractionandtrackingalgorithmonthecurrent imageforthecamerain-hand. 6)Implementfeaturepointmatchingtoidentifyatleastfouridenticalfeature pointsbetweenthecurrentimageandthedesiredimage. 7)Computetherotationandtranslationmatrixbetweenthecurrent anddesiredimageframesutilizingthemulti-viewphotogrammetryapproach (homography-baseddecomposition). 8)Rotationandtranslationmatrixcomputedinstep6canbeusedtocompute thedesiredrotationandtranslationvelocitycommandsfortherobotservocontrol. 9)Thelowerlevelcontrollergeneratesthenecessarypositionandorientation commandsfortheroboticmanipulator. 10)Approachthefruitandactivatethegrippermechanismforharvestinga fruit. 4.4 3 TargetReconstructionBasedVisualServoControl The 3 Euclideancoordinatesofthetargetfruitcanbedeterminedbasedon thestatisticalmeandiameterofthefruitasdiscussedlaterinChapter6.Anende ector,andhencethecamerain-hand,canbeorientedsuchthatthetargetfruitis inthe eld-of-viewofthecamerain-hand.Atthispoint,theEuclideancoordinates ofthetargetareagaindeterminedbeforeapproachingthefruit.Aninfraredsensor isusedasaproximitysensorfor nalapproachtowardsthefruitandtoactivate thegrippermechanism.Anoverviewofthe 3 targetreconstructionbasedvisual servocontrolapproachisillustratedinFigure.45. Followingaretheexperimentalstepsortheharvestingsequence: 1)Acquiretheimageofthetargetfruitforthe xedcamera.Thetargetfruit canbeselectedmanuallyorautonomouslybyimplementingcostfunctionbased imageprocessing.

PAGE 38

27 Figure4:OverviewoftheTBZcontrolarchitecture.

PAGE 39

28 2)EstimatetheEuclideanpositionofthetargetexpressedinthe xedcamera coordinatesystembasedonthe 3 targetreconstructiondescribedinChapter6. 3)Determinethetargetpositioninthebaseframeusinganextrinsiccamera calibrationmatrix.Therotationandtranslationcomponentsoftheextrinsic cameracalibrationmatrix R3 4forthe xedcameraareasfollows: = 010 001 100 = 254 00 196 85 381 00 .(4) 4)Orientthecamerain-handsuchthatthetargetfruitisinthe eld-of-viewof thecamerain-hand. 5)EstimatetheEuclideanpositionofthetargetexpressedinthecamera in-handcoordinatesystembasedonthe 3 targetreconstruction. 6)Approachthetargetkeepingthefruitatthecenterofthecamerain-hand image. 7)Reachthefruitharvestingpositionu singaninfraredsensorasaproximity sensorandactivatethegrippermechanism. 4.5Conclusion Inthischapter,wediscusseddevelopmentofatestbedformulti-camera visualservocontroltechniquesforautonomousroboticcitrusharvesting.The rapidprototypingtestbedprovidesaplatformforexperimentalvalidationof multi-viewphotogrammetry-basedteachbyzoomingvisualservocontroland 3 targetreconstructionbasedvisualservocontroltechniques.Thedevelopment includesreal-timeidenti cationoftarget,featurepointidenti cationandtracking, algorithmsformulti-viewphotogrammetrytechniques,and 3 depthidenti cation fortargetreconstruction.Also,thischapterprovidesanoverviewofthevisual servocontroltechniquesrealizedforautonomouscitrusharvesting.

PAGE 40

29 Figure4:Overviewofthe 3 targetreconstruction-basedvisualservocontrol.

PAGE 41

CHAPTER5 TEACHBYZOOMINGVISUALSERVOCONTROLFORANUNCALIBRATED CAMERASYSTEM Theteachbyshowingapproachisformulatedasthedesiretoposition/orient acamerabasedonareferenceimageobtainedby apriori positioningthesame camerainthedesiredlocation.Anewstrategyisrequiredforapplicationswhere thecameracannotbe apriori positionedtothedesiredposition/orientation.In thischapter,ateachbyzoomingapproachispresentedwheretheobjectiveisto position/orientacamerabasedonareferenceimageobtainedbyanothercamera. Forexample,a xedcameraprovidingaglobalviewofanobjectcanzoom-inonan objectandrecordadesiredimageforthecamerain-hand(e.g.acameramounted onthe xedbasejointprovidingagoalimageforanimage-guidedautonomous roboticarm).Acontrollerisdesignedtoregulatetheimagefeaturesacquiredby acamerain-handtothecorrespondingimagefeaturecoordinatesinthedesired imageacquiredbythe xedcamera.Thecontrollerisdevelopedbasedonthe assumptionthatparametricuncertaintyexistsinthecameracalibrationsince precisevaluesfortheseparametersaredi culttoobtaininpractice.Simulation resultsdemonstratetheperformanceofthedevelopedcontroller. 5.1Introduction Recentadvancesinvisualservocontrolhavebeenmotivatedbythedesireto makevehicular/roboticsystemsmoreautonomous.Oneproblemwithdesigning robustvisualservocontrolsystemsistocompensateforpossibleuncertaintyinthe calibrationofthecamera.Forexample,exactknowledgeofthecameracalibration parametersisrequiredtorelatepixelizedimage-spaceinformationtothetaskspace.Theinevitablediscrepanciesinthecalibrationmatrixresultinanerroneous 30

PAGE 42

31 relationshipbetweentheimage-spaceandtask-space.Furthermore,anacquired imageisafunctionofboththetask-spacepositionofthecameraandtheintrinsic calibrationparameters;hence,perfectkno wledgeoftheintrinsiccameraparameters isalsorequiredtorelatetherelativepositionofacamerathroughtherespective imagesasitmoves.Forexample,thetypicalvisualservoingproblemisconstructed asateachbyshowing(TBS)problem,inwhichacameraispositionedata desiredlocation,areferenceimageisacquired(wherethenormalizedtask-space coordinatesaredeterminedviatheintrinsiccalibrationparameters),thecamera ismovedawayfromthereferencelocation,andthenthecameraisrepositioned atthereferencelocationbymeansofvisualservocontrol(whichrequiresthatthe calibrationparametersdidnotchangeinordertorepositionthecameratothe sametask-spacelocationgiventhesameimage).See[4],[20],[18],and[23]fora furtherexplanationandanovervie woftheTBSproblemformulation. FormanypracticalapplicationsitmaynotbepossibletoTBS(i.e.,itmaynot bepossibletoacquirethereferenceimageby apriori positioningacamerain-hand tothedesiredlocation).AsstatedbyMalis[23],theTBSproblemformulation iscamera-dependentduetotheassumptionthattheintrinsiccameraparameters mustbethesameduringtheteachingstageandduringservocontrol.Malis[23], [22]usedprojectiveinvariancetoconstructanerrorfunctionthatisinvariantofthe intrinsicparametersmeetingthecontrolobjectivedespitevariationsintheintrinsic parameters.However,thegoalistoconstructanerrorsysteminaninvariantspace, andunfortunately,asstatedbyMalis[23],[22],severalcontrolissuesandarigorous stabilityanalysisoftheinvariantspaceapproachhasbeenleftunresolved. Inthiswork,ateachbyzooming(TBZ)approach[5]isproposedtoposition/orientacamerabasedonareferenc eimageobtainedbyanothercamera.For example,a xedcameraprovidingaglobalviewofthescenecanbeusedtozoom inonanobjectandrecordadesiredimageforanon-boardcamera.Applications

PAGE 43

32 oftheTBZstrategycouldincludenavigatinggroundorairvehiclesbasedon desiredimagestakenbyothergroundorairvehicles(e.g.,asatellitecapturesa zoomed-indesiredimagethatisusedtonavigateacameraon-boardamicro-air vehicle(MAV),acameracanviewanentiretreecanopyandthenzoominto acquireadesiredimageofafruitproductforhighspeedroboticharvesting).The advantagesoftheTBZformulationarethatthe xedcameracanbemounted sothatthecompletetask-spaceisvisible,canselectivelyzoominonobjectsof interest,andcanacquireadesiredimagethatcorrespondstoadesiredposition andorientationforacamerain-hand.Thecontrollerisdesignedtoregulatethe imagefeaturesacquiredbyanon-boardcameratothecorrespondingimagefeature coordinatesinthedesiredimageacquiredbythe xedcamera.Thecontrolleris developedbasedontheassumptionthatparametricuncertaintyexistsinthecameracalibrationsincetheseparametersaredi culttopreciselyobtaininpractice. SincetheTBZcontrolobjectiveisformulatedintermsofimagesacquiredfrom di erentuncalibratedcameras,theabilitytoconstructameaningfulrelationship betweentheestimatedandactualrotationm atrixisproblematic.Toovercomethis challenge,thecontrolobjectiveisformulatedintermsofthenormalizedEuclidean coordinates.Speci cally,desirednormalizedEuclideancoordinatesarede nedasa functionofthemismatchinthecameracalibration.Thisisaphysicallymotivated relationship,sinceanimageisafunctionofboththeEuclideancoordinatesandthe cameracalibration. Thismethodbuildsonthepreviouse ortsthathaveinvestigatedtheadvantagesofmultiplecamerasworkinginanon-stereopair.Speci cally,Dixonetal. [6],[7]developedanewcooperativevisualservoingapproachandexperimentally demonstratedthatusinginformationfrombothanuncalibrated xedcameraand anuncalibratedon-boardcameraenablesanon-boardcameratotrackanobject movinginthetask-spacewithanunknowntrajectory.ThedevelopmentbyDixon

PAGE 44

33 etal.[6],[7]isbasedonacrucialassumptionthatthecameraandtheobject motionisconstrainedtoaplanesothattheunknowndistancefromthecamerato thetargetremainsconstant.However,incontrasttodevelopmentbyDixonetal. [6],[7],anon-boardcameramotioninthisworkisnotrestrictedtoaplane.The TBZcontrolobjectiveisalsoformulatedsothatwecanleveragepreviouscontrol developmentbyFangetal.[9]toachieveexponentialregulationofanon-board cameradespiteuncertaintyinthecalibrationparameters. Simulationresultsareprovidedtoillustratetheperformanceofthedeveloped controller. 5.2ModelDevelopment Considertheorthogonalcoordinatesystems,denoted F F,and Fthatare depictedinFigure.5andFigure.5.Thecoordinatesystem F isattachedto anon-boardcamera(e.g.,acameraheldbyarobotend-e ector,acameramounted onavehicle).Thecoordinatesystem Fisattachedtoa xedcamerathathasan adjustablefocallengthtozoominonanobject.Animageisde nedbyboththe cameracalibrationparametersandtheEuc lideanpositionofthecamera;therefore, thefeaturepointsofanobjectdeterminedfromanimageacquiredfromthe xed cameraafterzoominginontheobjectcanbeexpressedintermsof Finone oftwoways:adi erentcalibrationmatrixcanbeusedduetothechangeinthe focallength,orthecalibrationmatrixcanbeheldconstantandtheEuclidean positionofthecameraischangedtoavirtualcamerapositionandorientation. Thepositionandorientationofthevirtualcameraisdescribedbythecoordinate system F.Table5-1showstheparametersexpressedinvariouscoordinateframes. Areferenceplane isde nedbyfourtargetpoints =1 2 3 4 wherethe threedimensional( 3 )coordinatesof expressedintermsof F F,and Fare

PAGE 45

34 Table5:Listofvariablesforteachbyzoomingvisualservocontrol. Parameters Frames Description ( ) ( ) F to F Rotationandtranslationvectorfrom F to F ( ) ( ) ( ) F Euclideancoordinatesofatargetin F , F Euclideancoordinatesofatargetin F , F Euclideancoordinatesofatargetin F F Pixelcoordinatesofatargetin F F Pixelcoordinatesofatargetin F F Pixelcoordinatesofatargetin F de nedaselementsof ( ) and R3asfollows: = (51) = = TheEuclidean-spaceisprojectedontotheimage-space,sothenormalized coordinatesofthetargetspoints ( ) ,and canbede nedas = = 1 (52) = = 1 = = 1 wheretheassumptionismadethat ( ) ,and ; denotesapositive (non-zero)scalarconstant.Basedon(5)thenormalizedEuclideancoordinatesof canberelatedto asfollows: = { 1 } (53) wherediag {} denotesadiagonalmatrixofthegivenarguments.

PAGE 46

35 Figure5:Cameraframecoordinaterelationships. Inadditiontohavingnormalizedtask-spacecoordinates,eachtargetpointwill alsohavepixelcoordinatesthatareacquiredfromanon-boardcameraexpressedin termsof F ,denotedby ( ) ( ) R ,andarede nedaselementsof ( ) R3asfollows: 1 .(5) Thepixelcoordinates ( ) andnormalizedtask-spacecoordinates ( ) are relatedbythefollowingglobalinvertibletransformation(i.e.,thepinholemodel): = .(5) Theconstantpixelcoordinates,expressedintermsof F(denoted R ) and F(denoted R )arerespectivelyde nedaselementsof R3and R3asfollows: 1 1 (56)

PAGE 47

36 Figure5:Teachbyzoomingvisualservocontrolforaroboticmanipulator. Thepinholemodelcanalsobeusedtorelatethepixelcoordinates and ( ) to thenormalizedtask-spacecoordinates and ( ) as: = (57) = or = .(5) In(5),the rstexpressioniswheretheEuclideanpositionandorientationof thecameraremainsconstantandthecameracalibrationmatrixchanges,and thesecondexpressioniswherethecalibrationmatrixremainsthesameandthe Euclideanpositionandorientationischanged.In(55)and(57),theintrinsic calibrationmatrices ,and R3 3denoteconstantinvertibleintrinsic cameracalibrationmatricesde nedas 1 1cot 00 2 sin 0001 (59) 1 1cot 0 0 2 sin 0 001 (5)

PAGE 48

37 1 1cot 0 0 2 sin 0 001 (5) In(5),(5),and(5), 0, 0 R and 0 0 R arethepixelcoordinates oftheprincipalpointofanon-boardcameraand xedcamera,respectively. Constants 1, 1, 1, 2, 2, 2 R representtheproductofthecamerascaling factorsandfocallength,and R aretheskewanglesbetweenthecamera axesforanon-boardcameraand xedcamera,respectively. Sincetheintrinsiccalibrationmatrixofacameraisdi culttoaccurately obtain,thedevelopmentisbasedontheassumptionthattheintrinsiccalibration matricesareunknown.Since isunknown,thenormalizedEuclideancoordinates cannotbedeterminedfrom usingequation(57).Since cannotbe determined,thentheintrinsiccalibrationmatrix cannotbecomputedfrom(5 7).FortheTBZformulation, de nesthedesiredimage-spacecoordinates.Since thenormalizedEuclideancoordinates areunknown,thecontrolobjectiveis de nedintermsofservoinganon-boardcamerasothattheimagescorrespond.If theimagefromanon-boardcameraandthezoomedimagefromthe xedcamera correspond,thenthefollowingexpressioncanbedevelopedfrom(55)and(5): = 1 (5) where R3denotesthenormalizedEuclideancoordinatesoftheobject featurepointsexpressedin F,where Fisacoordinatesystemattachedtoan on-boardcamerawhentheimagetakenfromanon-boardcameracorrespondsto theimageacquiredfromthe xedcameraafterzoominginontheobject.Hence, thecontrolobjectivefortheuncalibratedTBZproblemcanbeformulatedasthe desiretoforce ( ) to .Giventhat ( ) ,and areunknown,the

PAGE 49

38 estimates ( ) ,and R3arede nedtofacilitatethesubsequentcontrol development[25] = 1= (5) = 1 = (5) = 1 = (5) where R3 3areconstant,best-guessestimatesoftheintrinsiccamera calibrationmatricesand respectively.Thecalibrationerrormatrices R3 3arede nedas 1 = 11 12 130 22 23001 (5) 1 = 11 12 130 22 23001 .(5) ForastandardTBSvisualservocontrolproblemwherethecalibrationofthe cameradoesnotchangebetweentheteachingphaseandtheservophase, = ; hence,thecoordinatesystems Fand Fareequivalent. 5.3HomographyDevelopment FromFigure.5thefollowingrelationshipcanbedeveloped = + (5) where ( ) R3 3and ( ) R3denotetherotationandtranslation,respectively, between F and F.Byutilizing(51)and(52),theexpressionin(58)canbe

PAGE 50

39 expressedasfollows = | {z } + | {z } (5) where ( ) ,( ) R3and R denotesanunknownconstantdistancefrom Fto alongtheunitnormal .Thefollowingrelationshipcanbedevelopedby substituting(5)and(5)into(5)for ( ) and ,respectively: = (5) where R3 3istheprojectivehomographymatrixde nedas ( ) ( ) 1 Theexpressionsin(5)and(5)canbeusedtorewrite(5)as = 1 (5) Thefollowingexpressioncanbeobtainedbysubstituting(512)into(51) = (5) where ( ) 1 ( ) denotestheEuclideanhomographymatrixthatcanbe expressedas = + where = (5) In(5), ( ) R3 3and ( ) R3denotetherotationandtranslation, respectively,from F to F.Theconstant R in(523)denotesthedistance from Fto alongtheunitnormal R3. Since ( ) and cannotbedeterminedbecausetheintrinsiccamera calibrationmatricesand areuncertain,theestimates ( ) and de nedin (53)and(514),respectively,canbeutilizedtoobtainthefollowing: = (5)

PAGE 51

40 In(5), ( ) R3 3denotesthefollowingestimatedEuclideanhomography [25]: = 1 (5) Since ( ) and canbedeterminedfrom(5)and(5),asetoflinear equationscanbedevelopedtosolvefor ( ) (see[8]foradditionaldetails regardingthesetoflinearequations).Theexpressionin(525)canalsobe expressedasfollows[8]: = + (5) In(5),theestimatedrotationmatrix,denoted ( ) R3 3,isrelatedto ( ) asfollows: = 1(5) and ( ) R3, R3denotetheestimateof ( ) and ,respectively,and arede nedas: = (5) = 1 (5) where R denotesthefollowingpositiveconstant = (5) Although ( ) canbecomputed,standardtechniquescannotbeused todecompose ( ) intotherotationandtranslationcomponentsin(5). Speci cally,from(5) ( ) isnotatruerotationmatrix,andhence,itisnot clearhowstandarddecompositionalgorit hms(e.g.,theFaugerasalgorithm[12]) canbeapplied.Toaddressthisissue,additionalinformation(e.g.,atleastfour vanishingpoints)canbeused.Forexample,asthereferenceplane approaches in nity,thescalingterm alsoapproachesin nity,and ( ) and ( ) approach

PAGE 52

41 zero.Hence,(5)canbeusedtoconcludethat ( )= ( ) ontheplane atin nity,andthefourvanishingpointpairscanbeusedalongwith(5)to determine ( ) .Once ( ) hasbeendetermined,varioustechniques(e.g.,see [10,32])canbeusedalongwiththeoriginalfourimagepointpairstodetermine ( ) and ( ) 5.4ControlObjective Thecontrolobjectiveistoensurethatthepositionandorientationofthe cameracoordinateframe F isregulatedto F.BasedonSection5.3,thecontrol objectiveisachievedif ( ) 3(5) andonetargetpointisregulatedtoitsdesiredlocationinthesensethat ( ) and ( ) (5) where 3 R3 3representsanidentitymatrix. Tocontrolthepositionandorientationof F ,arelationshipisrequiredto relatethelinearandangularcameravelocitiestothelinearandangularvelocities ofthevehicle/robot(i.e.,theactualkinematiccontrolinputs)thatenablesan on-boardcameramotion.Thisrelationshipisdependentontheextrinsiccalibration parametersasfollows[25]: = [ ]0 (5) where ( ) ( ) R3denotethelinearandangularvelocityofthecamera, ( ) ( ) R3denotethelinearandangularvelocityofthevehicle/robot, R3 3denotestheunknownconstantrotationbetweenanon-boardcameraandrobot end-e ectorframes,and [ ] R3 3isaskewsymmetricformof R3,which denotestheunknownconstanttranslationvectorbetweenanon-boardcameraand

PAGE 53

42 Figure53:Overviewofteachbyzoomingvisualservocontroller. vehicle/robotframes.Ablockdiagramofteachbyzoomingvisualservocontroller isshowninFigure.53. 5.5ControlDevelopment 5.5.1RotationController Toquantifytherotationbetween F and F(i.e., ( ) givenin(5)),a rotationerror-likesignal,denotedby ( ) R3,isde nedbytheangleaxis representationas[26]: = (5) where ( ) R3representsaunitrotationaxis,and ( ) R denotestherotation angleabout ( ) thatisassumedtobeconstrainedtothefollowingregion 0 ( ) (5)

PAGE 54

43 Theparameterization ( ) ( ) isrelatedtotherotationmatrix ( ) as = 3+sin [ ]+2sin2 2 [ ]2 (5) where [ ]denotesthe 3 3 skew-symmetricmatrixassociatedwith ( ) .The open-looperrordynamicsfor ( ) canbeexpressedas(RefertoAppendixA.1) = (5) wheretheJacobianmatrix ( ) R3 3isde nedas = 3 2 [ ]+ 1 ( ) 2 2 [ ]2 (5) Inequation(5)the sin ( ) termisgivenby(5)as, sin ( )= sin( ) (5) Sincetherotationmatrix ( ) andtherotationerror ( ) de nedin(534) areunmeasurable,anestimatedrotationerror ( ) R3isde nedas = (5) where ( ) R3, ( ) R representestimatesof ( ) and ( ) ,respectively.Since ( ) issimilarto ( ) (i.e., ( ) hasthesametraceandeigenvaluesas ( ) ), theestimates ( ) and ( ) canberelatedto ( ) and ( ) asfollows[25]: = = (5) where ( ) R denotesthefollowing unknownfunction = 1 .(5)

PAGE 55

44 Therelationshipin(5)allows ( ) tobeexpressedintermsoftheunmeasurableerror ( ) as = (5) Giventheopen-looprotationerrordynamicsin(5),thecontrolinput ( ) isdesignedas = (5) where R denotesapositivecontrolgain,and R3 3denotesaconstant best-guessestimateof .Substituting(543)into(54)andsubstitutingthe resultingexpressioninto(57)givesthe followingexpressionfortheclosed-loop errordynamics[25]: = (5) wheretheextrinsicrotationestimationerror R3 3isde nedas = .(5) Thekinematiccontrolinputgivenin(544)ensuresthat ( ) de nedin (54)isexponentiallyregulatedinthesensethat k ( ) k k (0) k exp( 0 ) (5) providedthefollowinginequalityissatis ed: 0k k2for R3(5) where = (5) = + 2

PAGE 56

45 for R3 and 0 R denotesthefollowingminimumeigenvalue: 0= min + 2 (5) Proof: See[9]. 5.5.2TranslationController Thedi erencebetweentheactualanddesired 3 Euclideancameraposition, denotedbythetranslationerrorsignal ( ) R3,isde nedas (5) where ( ) R3denotestheextendedcoordinatesofanimagepointon expressedintermsof F andisde nedas1, 1( ) 2( ) 3( ) (5) = 1 11 1ln( 1) and R3denotestheextendedcoordinatesofthecorrespondingdesiredimage pointon intermsof Fas 1 2 3(5) = 1 1 1 1ln( 1) 1Todevelopthetranslationcontrollerasinglefeaturepointcanbeutilized. Withoutlossofgenerality,thesubsequentdevelopmentwillbebasedontheimage point 1,andhence,thesubscript 1 willbeutilizedinlieuof .

PAGE 57

46 where ln( ) denotesthenaturallogarithm.Substituting(552)and(553)into (5)yields = 1 1 1 11 1 1 1ln 1 1(5) wheretheratio1 1canbecomputedfrom(5)andthedecompositionofthe estimatedEuclideanhomographyin(5).Since 1( ) and areunknown(since theintrinsiccalibrationmatricesareunknown), ( ) isnotmeasurable.Therefore, theestimateofthetranslationerrorsystemgivenin(54)isde nedas 1 1 2 2ln 1 1(5) where 1( ) 2( ) 1 2 R denoteestimatesof 1( ) 2( ) 1 2, respectively. Todeveloptheclosed-looperrorsystemfor ( ) ,wetakethetimederivative of(5)andthensubstitute(5)intotheresultingexpressionfor ( ) to obtain(RefertoAppendixA.2) = + [ ]+ (5) wherethe ( ) ( ) R3 3arede nedas 1 1 10 10 1 200 1 (5) 1 2 1 2 1 21+ 2 2 1 2 1 2 10 .(5) Tofacilitatethecontroldevelopment,theunknowndepth 1( ) in(5)canbe expressedas 1= 1 1 1(5)

PAGE 58

47 where 1isgivenbythehomographydecomposition. Anestimatefor ( ) canbedesignedas = 1 1 10 10 1 200 1 (5) where 1( ) 2( ) wereintroducedin(55),and 1 R isdevelopedbasedon (5)as 1= 1 1 1 (5) Basedonthestructureoftheerrorsystemin(556)andsubsequentstability analysis,thefollowinghybridtranslationcontrollercanbedeveloped ( )= (5) 1 2 1+ 2 2 1k k2 where ( ) ,and ( ) areintroducedin(5),(5),and(5),respectively, 1, 2 R denotepositiveconstantcontrolgains,and 1( ) isde nedin (5).In(5), ( ) R denotesapositivegainfunctionde nedas = 0+ 2 1 ( 1 2) (5) where 0 R isapositiveconstant,and ( 1 2) isapositivefunctionof 1and 2Thekinematiccontrolinputgivenin(562)ensuresthatthehybridtranslation errorsignal ( ) de nedin(5)isexponentiallyregulatedinthesensethat k ( ) k p 2 0 1 exp( 1 2 ) (5) provided(5)issatis ed,where R3 3isaconstantinvertiblematrix,and 0, 1 R denotepositiveconstants.

PAGE 59

48 Proof: See[9]. 5.6SimulationResults 5.6.1Introduction AnumericalsimulationispresentedtoillustratetheperformanceoftheTBZ controllergivenin(5)and(5).T hesimulationsetupforteachbyzooming visualservocontrolconsistsoffollowingthreemaincomponents:(1)simulation workstation,(2)simulationsoftware,and(3)virtualcamera.Therestofthe sectionprovidesdetailsabouteachofthecomponentsmentionedabove. The rstcomponentisthesimulationworkstation.Thevision-basedcontrol simulationisperformedonMicrosoftWindowsXPplatformbasedIBMpersonal computer(PC)with3.06GHzIntelPentium4processorand764MBrandom accessmemory(RAM). Thesecondcomponentinthesimulationisthesoftwareplatform.MATLAB (TheMathWorks,Inc.)release6.0hasbeenusedfornumericalsimulation,along withSimulinkforinteractivegraphicalenvironment.Simulinkisaplatformfor multidomainsimulationandModel-BasedDesignfordynamicsystems. Thethirdcomponentinthesimulationbeingthevirtualcamera.Theintrinsic cameracalibrationmatrixisgivenin(5 68),whereas(5)and(5)statethe extrinsiccameracalibrationparameters.Intheintrinsiccameracalibrationmatrix itisassumedthatparametricuncertaintyexistsinthecameracalibrationsince theseparametersaredi culttopreciselyobtaininpractice. Thefollowingsectionprovidesnumericalresultsofthesimulation. 5.6.2NumericalResults Theintrinsiccameracalibrationmat rixusedforanon-boardcameraandthe xedcameraaregivenasfollows: 0= 0=120 [pixels]and 0 = 0 =120 [pixels]denotethepixelcoordinatesoftheprincipalpoint; 1=122 5 2=122 5 1=147 2=147 1=294 ,and 2=294 denotetheproductofthe

PAGE 60

49 focallengthandthescalingfactorsforanon-boardcamera, xedcamera,and xedcameraafterzooming(i.e.,thefocallengthwasdoubled),respectively;and = =1 53 [rad]istheskewangleforeachcamera.Theunknownconstant rotationbetweenthecameraandend-e ectorframes,andtheunknownconstant translationbetweenthecameraandend-e ectorframes(i.e.,theextrinsiccamera calibrationparameters and de nedin(533))wereselectedasfollows = 0 95692 0 0655630 28284 0 117250 97846 0 16989 0 265610 195740 944 (5) = 0 020 040 03 .(5) Thebest-guessestimatesfor and wereselectedasfollows: = 0 9220 0 18440 3404 0 34040 8050 0 4858 0 18440 56380 8050 (5) = 120 4122 0121123 001 .(5) Theimagespacecoordinates(allimagespacecoordinatesareinunitsofpixels)of thefourconstantreferencetargetpointsbeforeandafterincreasingthefocallength

PAGE 61

50 ( 2 )wererespectivelyselectedasfollows: 1= 121 9120 41 2= 121 7121 21 3= 121 0121 01 4= 121 2120 31 1= 129 4122 21 2= 128 6125 751 3= 125125 21 4= 125 7121 61 Figure.5illustratesthechangeinpixelcoordinatesfrom to .Theinitial image-spacecoordinatesoftheobjectviewedbyanon-boardcamerawereselected asfollows 1(0)= 113 7113 51 2(0)= 116 41141 3(0)= 115 8115 61 4(0)= 113 2115 21

PAGE 62

51 Thevanishingpointsforthe xedcamerawereselectedas 1= 134 1134 71 2= 135 3105 31 3= 105 9105 31 4= 104 7134 71 whilethevanishingpointsfortheanon-boardcamerawereselectedasfollows: 1(0)= 76 5276 71 2(0)= 1441991 3(0)= 1381921 4(0)= 1431921 Thecontrolgains and wereadjustedtothefollowingvaluestoyieldthebest performance =40 0 =2 0 (5) TheresultingrotationalandunitlesstranslationalerrorsaredepictedinFigure. 5andFigure.5,respectively.FromFigure.5andFigure.5,itcanbe concludedthattheerrorsareexponentiallyregulatedtozero,thusestablishingthe stabilityofthecontroller.TheEuclideantrajectoryofthefeaturepointsviewed byanon-boardcamerafromtheinitialpositionandorientationtothedesired positionandorientation FispresentedinFigure.5.Theangularandlinear controlinputvelocities ( ) and ( ) de nedin(544)and(5),respectively, aredepictedinFigure.58andFigure.59.Itcanbeseenthattheangularand linearcontrolvelocitiesarealwaysbounded.

PAGE 63

52 Figure5:Imagefromthe xedcamerabeforezooming(subscriptf)andafter zooming(superscript*). Figure5:RotationErrors.

PAGE 64

53 Figure5:TranslationErrors. Figure5:Euclideantrajectoryofthefeaturepointsviewedbythecamera-inhandfromtheinitialpositionandorientation(denotedby+)tothedesired positionandorientation F(denotedbyx),wherethevirtualcameracoordinate system Fisdenotedbyo.

PAGE 65

54 Figure5:Angularcontrolinputvelocityforthecamera-in-hand. Figure5:Linearcontrolinputvelocityforthecamera-in-hand.

PAGE 66

55 Figure510:Eliminationofine ectualfeaturepointsvia.targetidenti cation. 5.7ExperimentalResults AnoverviewofthecompleteexperimentaltestbedisillustratedinFigure.(4 4)andanalgorithmforteachbyzoomingvisualservocontrolmethodispresented inAppendixC.Multi-cameravisualservocontroltechniquedescribedhereconsists ofa xedcameramountedonthestationarybasejointoftherobotwhereasa camerain-handisattachedtotherobotend-e ector.The xedcameraprovidesa globalviewofthetreecanopyandcanbezoomedintocapturetheimageforthe targetfruit. Thetwomainchallengesintheimplementationofteachbyzoomingvisual servocontrollerforcitrusharvestingap plicationarediscussedintherestofthe section.The rstchallengeistoidentifythefeaturepointsonthetargetand eliminatetheine ectualfeaturepoints,orotherwise,whichmeansthattoidentify andtrackfeaturepointsonthefruittobeharvestedandpurgethefeaturepoints fromtheenvironment.Thisisnecessaryinordertoestablishtherotationaland translationalinformation,intermsofhomographymatrix,betweenthetwocameras lookingatthetargetfruit.Asolutiontothisproblemisidenti edintheimage processingtechnique.Colorthresholding-basedimageprocessingisusedfor targetdetection,thuspreservingthetargetrestoftheimageispurged.Feature pointidenti cationandtrackingisperformedonlyonthedetectedtarget,thus eliminatingtheissueofine ectualfeaturepointsasshowninFigure.5.

PAGE 67

56 Figure5:Featurepointmatchingbetween xedcameraandcamerain-hand. Thesecondchallengeintheimplementationofthecontrollerisfeaturepoint matchingbetweentheimagestakenbycamerain-handand xedcamera.The featurepointsviewedbythe xedcameracanbedi erentthanthefeaturepoints viewedbythecamerain-handasshowninFigure.51.Non-identicalfeature pointsbetweenthecurrentimageobtainedbyacamerain-handandthedesired imageobtainedbya xedcamerawouldresultinincorrectrotationandtranslation information.Featurepointvectormatchingisrealizedtoaimatidentifyingthe identicalfeaturepointsbetweenthecurrentimageandthedesiredimage.However, practicallyitisdi culttoconsistentlyobtainatleastfouridenticalfeaturepoints, whicharerequiredforhomographydecomposition,andhenceitisrealizedthat teachbyzoomingvisualservocontrolstrategycannotbeimplementedforacitrus harvestingapplication. 5.8Conclusion AnewTBZvisualservocontrolapproac hisproposedforapplicationswhere thecameracannotbe apriori positionedtothedesiredposition/orientationto acquireadesiredimagebeforeservocontrol.Speci cally,theTBZcontrolobjective isformulatedtoposition/orientanon-boardcamerabasedonareferenceimage obtainedbyanothercamera.InadditiontoformulatingtheTBZcontrolproblem, anothercontributionofthisworkistoillustratehowtopreserveasymmetric transformationfromtheprojectivehomographytotheEuclideanhomography

PAGE 68

57 forproblemswhenthecorrespondingimagesaretakenfromdi erentcameras withcalibrationuncertainty.Tothisend,adesiredcameraposition/orientation isde nedwheretheimagescorrespond,buttheEuclideanpositiondi ersasa functionofthemismatchinthecalibrationofthecameras.Simulationresultsare providedtoillustratetheperformanceofthecontroller.Rotationandtranslation errorsareexponentiallyregulatedtozerothusestablishingthestabilityofthe controller,whereastheangularandlinea rcontrolvelocitiesarealwaysbounded. Applicationsofthisstrategycouldinclu denavigatinggroundorairvehicles,based ondesiredimagestakenbyothergroundorairvehicles(e.g.,amicroairvehicle (MAV)capturesazoomedindesiredimagethatisusedtonavigateanon-board camera). Practicallimitationontheimplementationoftheteachbyzoomingvisual servocontrollerforthecitrusharvestingapplicationhasbeenrealizedinthe factthatthefeaturepointsviewedbythe xedcameracanbedi erentthanthe featurepointsviewedbythecamerain-hand.Non-identicalfeaturepointsbetween thecurrentimageandthedesiredimagewouldresultinincorrectrotationand translationinformation.Hencetheteachbyzoomingvisualservocontrollercan beimplementedwherethefeaturepointinformationisavailable apriori ,i.e.,this controllerissuitableforarti cialtargets.

PAGE 69

CHAPTER6 3 TARGETRECONSTRUCTIONFORVISION-BASEDROBOTCONTROL Theteachbyzoomingvisualservocontrolstrategyisdevisedtoposition/orientacamerabasedonarefere nceimageobtainedbyanothercamera. AsseeninChapter5,thisstrategyemployscoordinatedrelationshipbetweenthe xedcameraandcamerain-handusingthefeaturepointmatchingtechnique. However,featurepointmatchingisnotsuitablefornaturaltargetslikecitrusdue tonon-identicalfeaturepointmatching.Therefore,thischapterdescribesavisual servocontrolstrategybasedonthreedimensional(3D)reconstructionofthetarget fromatwodimensional(2D)image.The3Dtargetreconstructionisachievedby usingthestatisticaldata,viz.themeandiameterandthestandarddeviationofthe citrusfruitdiameter,alongwiththetargetimagesizeandthecamerafocallength togeneratethe3Ddepthinformation.Acontrollerisdevelopedtoregulatethe robotend-e ectortothe3DEuclideancoordinatescorrespondingtothecentroidof thetargetfruit. 6.1ModelDevelopment Considertheorthogonalcoordinatesystems,denoted F F,and Fthatare depictedinFigure.6andFigure.6.Thecoordinatesystem F isattachedto anon-boardcamera(e.g.,acameraheldbyarobotend-e ector,acameramounted onavehicle).Thecoordinatesystem Fisattachedtoa xedcamera(e.g.a cameramountedonastationarybasejointofarobot)andthecoordinatesystem Fisattachedtoatargetfruitwithfruitcentroidbeingthecoordinatesystem origin.Table6-1showstheparametersexpressedinvariouscoordinateframes.The originofthecoordinatesystem Fisdenotedas wherethethreedimensional ( 3 )coordinatesof expressedintermsof F and Farede nedaselementsof 58

PAGE 70

59 Figure6:Cameraframecoordinaterelationships. Table6:Listofvariablesfor3Dtargetreconstructionbasedvisualservocontrol. Parameters Frames Description ( ) ( ) F to F Rotationandtranslationvectorfrom F to F ( ) ( ) Fto F Rotationandtranslationvectorfrom Fto F ( ) ( ) ( ) F Euclideancoordinatesofatargetin F , F Euclideancoordinatesofatargetin F F Pixelcoordinatesofatargetin F F Pixelcoordinatesofatargetin F ( ) ( ) ( ) F Estimatedeuclideancoordinatesofatargetin F , F Estimatedeuclideancoordinatesofatargetin F ( ) and R3asfollows: ( )= ( ) ( ) ( ) (61) = (62)

PAGE 71

60 Figure6:3Dtargetreconstructionbasedvisualservocontrolforaroboticmanipulator. TheEuclidean-spaceisprojectedontotheimage-space,sothenormalized coordinatesofthetargetspoints ( ) and canbede nedas = = 1 (63) = = 1 (64) wheretheassumptionismadethat ( ) and ; denotesapositive(nonzero)scalarconstant.Inadditiontohavingnormalizedtask-spacecoordinates,the targetpointwillalsohavepixelcoordi natesthatareacquiredfromanon-board cameraexpressedintermsof F ,denotedby ( ) ( ) R ,andarede nedas elementsof ( ) R3asfollows: 1 .(6) Thepixelcoordinates ( ) andnormalizedtask-spacecoordinates ( ) are relatedbythefollowingglobalinvertibletransformation(i.e.,thepinholemodel): = .(6)

PAGE 72

61 Theconstantpixelcoordinates,expressedintermsof F denoted R ,are de nedaselementsof R3asfollows: 1 (67) Thepinholemodelcanalsobeusedtorelatethepixelcoordinates tothe normalizedtask-spacecoordinates as: = (68) In(66)and(6),theintrinsiccalibrationmatrices and R3 3denote constantinvertibleintrinsiccameracalibrationmatricesde nedas 1 1cot 00 2 sin 0001 1 1cot 0 0 2 sin 0 001 (69) In(6), 0, 0 R and 0 0 R arethepixelcoordinatesoftheprincipal pointofthecamerain-handand xedcamera,respectively.Constants 1, 1, 2, 2 R representtheproductofthecamerascalingfactorsandfocallength,and R aretheskewanglesbetweenthecameraaxesforacamerain-handand xedcamera,respectively. 6.23DTargetReconstruction The3Dtargetreconstructionisachievedbyusingthestatisticalmeandiameterofthetargetalongwiththetargetimagesizeandthecamerafocallengthto generatethe3Ddepthinformation.Themeandiameterofthecitrusfruitcanbe obtainedfromthestatisticaldataas R .Usingtheperspectiveprojections

PAGE 73

62 Figure6:PerspectiveprojectiongeometrymodelforEuclideandepthidenti cation. geometryasshowninFigure.63,therelationshipsforthetargetfruitsizeinthe objectplaneandtheimageplanecanbeobtainedasfollows: = = (6) where R denotetheestimatesforanunknownthreedimensional depthofthetargetplanefromtheimageplane, R istheproductof scalingfactorsandfocallengthofthe xedcameraalongthexandydirections, respectively.In(610),theterm R denotesthetargetdiameterinthe objectplaneobtainedfromthestatisticaldatawhereas R denotesthe targetdiameterintheimageplaneexpressedintermsof F.Utilizing(610),the expressionfortheestimateofanunknownEuclideandepthofthetarget R canbeobtainedasfollows: = ( + ) 2 (6) TheestimatedEuclideancoordinatesofthetarget,expressedintermsof F canbeobtainedfrom(6)and(6)as b = = (6)

PAGE 74

63 Further,theEuclideancoordinatesofthetargetcomputedin(6)canbe expressedwithrespecttotherobotbaseframe Fthroughtheknownextrinsic cameracalibrationmatrix R3 3asfollows: b = b (6) Similarly,theEuclideandepthandhencetheestimatedEuclideancoordinates ofthetargetexpressedintermsof F areobtainedasfollows: = ( + ) 2 (6) b = = (6) In(6), ( ) R denotestheestimatedunknownthreedimensionaldepth ofthetargetplanefromtheimageplane, R isthefocallengthofthe camerain-handalongthexandydirections,respectively,whereas ( ) R denotesthetargetdiameterintheimageplaneexpressedintermsof F .Hence, knowingthestatisticalmeandiameterofthetarget,expressionsin(612)and (65)canbeusedtocomputetheestimatedtargetpositionexpressedin Fand F ,respectively. 6.3ControlDevelopment Thissectiondescribesthevision-basedcontroldevelopmentforroboticcitrus harvesting.The xedcameracanviewanentiretreecanopytoacquireadesired imageofatargetfruit,butthetargetfruitmaynotbeinthe eld-of-viewofthe camerain-handandhencetherobotend-e ector,andhencethecamerain-hand, isorientedalongatargetfruitcentreasshowninFigure.6.Oncetheende ectorisorientedalongthedirectionofthetargetfruit,animagecapturedbythe camerain-handisusedtocalculatetheEuclideancoordinatesofthecitrusfruits inthe eld-of-view.Basedonacostfunction,whichisafunctionofadepthanda diameterofthefruit,thetargetisselected.Theend-e ectoristhenmovedtowards

PAGE 75

64 Figure6:Controlarchitecturedepictingrotationcontrol. thetargetfruitwhilealigningthecentreofthetargetfruitatthecentreofthe camerain-handimage.Aninfraredsensorisusedasaproximitysensorforthe nal positioncontrolasshowninFigure.65. 6.3.1ControlObjective Thecontrolobjectiveistoensurethatthepositionandorientationofthe cameracoordinateframe F isregulatedto F.FromtheFigure6,itcanbe seenthatthecontrolobjectiveisachievedifthetargetfruitcentroid iscollinear withthe ofcamerain-handcoordinatesystem F andthetargetpointis regulatedtoitsdesiredlocationinthesensethat F ( ) F,i.e.mathematicallyit canbestatedasfollows: b ( ) ( ) b (6) where 3 R3 3representsanidentitymatrixand ( ) R4 4istheknownrobot feedbackmatrixasgivenin(6).

PAGE 76

65 Figure6:Controlarchitecturedepictingtranslationcontrol. 6.3.2RotationController Theestimatedtargetpositionexpressedintherobotbaseframe Fcanbe statedinthecamerain-h andcoordinateframe F throughtherobotfeedback matrix ( ) asfollows: b 1 = b 1 (6) wherethefeedbackmatrix ( ) canbewrittenintermsoftherotationand translationvectors ( ) R3 3and ( )= ( ) ( ) ( ) R3 respectively,as: = 01 (6) Theobjectiveoftherotationcontrolleristoalignthe -axisofthecamerain-hand frame F ,i.e.,the -axisoftherobotend-e ector,alongthetargetfruitcentroid. Therotationcontrolobjectivecanbeachievedbyrotatingtherobotend-e ector aboutthe -axisthroughanangle ( ) R and -axisthoughanangle ( ) R .

PAGE 77

66 Therotationangles ( ) and ( ) canbequanti edasbelow =tan 1 (6) = tan 1 (6) Hencetherotationoftherobotend-e ectorabout and axiscanbeexpressedintermsofrotationmatrices ( ) R3 3and ( ) R3 3,respectively, asfollows: = 100 0cos( )sin( ) 0 sin( )cos( ) = cos( )0 sin( ) 010 sin( )0cos( ) (6) where ( ) and ( ) arede nedin(619)and(6),respectively.Further,the rotations ( ) and ( ) canbeexpressedintherobotbaseframe Fas: = = (6) Rotationerrorsignal ( ) R3canbequanti edasthemismatchbetween therobotfeedback ( ) andthedesiredrotationscalculatedin(6)intermsof rotationabout oftherobotbaseframeasfollows:. ( )= ( ) ( ) ( ) (6) Basedontheerrorsystemtherotationcontrolvelocity ( ) R3forthe robotend-e ectorcanbeexpressedasfollows: = (6) where R arethepositiveproportionalandderivativecontrolgains, respectively,and ( ) R3isthetimederivativeoftherotationerrorsignal ( )

PAGE 78

67 6.3.3TranslationController Thedi erencebetweenpixelcoordinatesofthetargetcentreandthepixel coordinatesoftheprincipalpointofthecamerain-hand,denotedbythetranslation errorsignal ( ) ( ) R alongthe and axis,respectively,isde nedas = 0(6) = (6) where ( ) ( ) R arethepixelcoordinatesofthetargetcentrede nedin (6)and 0 R arethepixelcoordinatesoftheprincipalpointofthecamera in-handde nedin(6).Also,thetranslationerrorsignal ( ) R isde ned asthedi erencebetweenthedesiredEuclideandepthandthecurrentdepthas follows: = (6) where ( ) R isthedesiredEuclideandepthde nedin(6)and ( ) R isthefeedback -positionoftheend-e ectorgivenin(6).Basedontheerror systemdevelopedin(6), (6),and(6),thetranslationcontrolvelocity ( ) R3fortherobotend-e ectorcanbeexpressedasfollows: = (6) where R arethepositiveproportionalandderivativecontrolgains, respectively, ( )= ( ) ( ) ( ) R3isthetranslationerrorsignal, and ( ) R3isthetimederivativeofthetranslationerrorsignal ( ) An infraredsensorisusedasaproximitysensortoaccuratelypositiontheend-e ector beforethefruitisharvested. 6.4ExperimentalResults Thissectionisorganizedinthefollowingmanner.Apreliminaryexperiment describingtheperformanceof 3 depthestimationtechniqueisillustratedin

PAGE 79

68 Section6.4.1.Anexperimentalperformancevalidationisdoneinthreeparts. InSection6.4.2,anexperimentisconductedtodescribetherepeatabilityofthe controller.InSection6.4.3,thecontrollerperformanceisdiscussedunderdi erent positionsoftherobotend-e ectorwhilemaintainingtheconstantEuclidean positionofthetargetfruit.InSection6.4.4,thethirdpartoftheexperiment describestheperformanceofthecontrollerundervariedinitialpositionsofthe robotend-e ectoraswellasEuclideanpositionofthetarget.Analgorithmfor 3 depthestimationandtargetreconstructionbasedvisualservocontrolmethodis presentedinAppendixD. 6.4.1PerformanceValidationof 3 DepthEstimation Theperformanceofthe 3 depthestimationstrategyisveri edinthispreliminaryexperiment.Thethreemaincomponentsofthispreliminaryexperiment areasfollows:(1)CCDcamera,(2)imageprocessingworkstation,and(3)depth estimation.ThecameraisKT&Cmake(model:KPCS20-CP1) xedfocallength, colorCCDconepinholecameras.TheimageoutputfromthecameraisNTSC analogsignalwhichisdigitizedusinguniversalserialbus(USB)framegrabber. Thesecondcomponentintheexperimentistheimageprocessingworkstation.The 3 depthestimationisperformedonMicrosoftWindowsXPplatformbasedIBM personalcomputer(PC)with3.06GHzIntelPentium4processorand764MB randomaccessmemory(RAM).Theimagep rocessingworkstationacquiresthe digitizedimagefromtheframegrabberandemployscolorthresholding-basedtechniquesfortargetidenti cation.AsdescribedinSection6.2,utilizingthetargetsize intheimageandstatisticalparameters,thedepthestimationtechniqueidenti es 3 depthofthetargetfromthecameraframe. Thetargetsizeinanobjectplaneismeasuredtobe =74 99 ,and =77 31 along respectively.Theproductofthefocallengthand scalingfactorsforthecameraare =833 57 and =767 02 along

PAGE 80

69 Figure6:Performancevalidationof 3 depthestimation. Table6:Performancevalidationfor3Ddepthestimationmethod. Sr.No. Image-plane ActualDepth Estimateddepth(mm) targetsize (mm) alongx alongy mean (pixels) 1 334 321 200 187 15 184 73 185 94 2 218 205 300 286 73 289 26 288 00 3 161 152 400 388 25 390 12 389 18 4 128 119 500 488 35 498 30 493 32 5 104 98 600 601 04 605 08 603 06 6 88 83 700 710 33 714 44 712 38 7 78 72 800 801 39 823 59 812 49 8 69 64 900 905 92 926 53 916 23 9 63 59 1000 992 20 1005 05 998 634 10 56 53 1100 1116 23 1118 84 1117 53 11 52 50 1200 1202 09 1185 97 1194 03 respectively.Thetargetisstationarywhereasthecameraisdisplacedin incrementalstepsof 100 towardsthetarget,henceitrepresentsacamerainhandcon gurationasshowninFigure.63.Theresultsofthedepthestimationare showninTable6-2andFigure.6illustratesthedepthestimationerrorpattern. Comparingtheactualdepthwiththeestimateddepthitcanbeseenthat 3 depth estimationperformssatisfactorily.FromFigure.6,itcanbeconcludedthatthe depthestimationmethodexhibitlargeestimationerrorsforlowertargetdepthsand viceversa.Thisphenomenoncanbeattributedtothefactthatthetargetdetection e ciencyreducesforlowertargetdepths. 6.4.2ExperimentI Therepeatabilityofapositioningsystemistheextenttowhichsuccessive attemptstomovetoaspeci clocationvaryinposition.Ahighlyrepeatable

PAGE 81

70 Figure6:Depthestimationerror. system(whichmayormaynotalsobeaccurate)exhibitsverylowscatterin repeatedmovestoagivenposition,regardlessofthedirectionfromwhichthepoint wasapproached.Thispartoftheexperimentdiscussestherepeatabilityofthe proposedcontrollerunderthesamestartingpositionoftherobotend-e ectoras showninFigure.68.ActualmeasuredEuclideanpositionofthetargetfruitina xedcameraframeandarobotbaseframeisasshowninTable6-3. Figure6:Performancevalidationforrepeatabilitytest.

PAGE 82

71 Table63:ActualEuclideantargetpositionexpressedina xedcameraframeand robotbaseframe. Coordinateframe (mm) (mm) (mm) Fixedcamera 139 7 12 7 838 2 Robotbaseframe 228 6 1066 8 540 2 Table64:EstimatedEuclideantargetpositionexpressedina xedcameraframe, robotbaseframe,androbottoolframe. Coordinateframe (mm) (mm) (mm) Fixedcamera 65 0 4 0 831 0 Robotbaseframe 237 0 1060 0 395 Robottoolframe 462 0 114 0 110 0 Theestimatedtargetposition b expressedina xedcameracoordinate system, b expressedinarobotbaseframe,and b expressedintherobottool frameisgiveninTable6-4. Initialpositionoftherobotend-e ectororacamerain-handmeasuredina robotbaseframeisgiveninTable6-5. Table6-6showsthe nalpositionoftherobotend-e ectormeasuredinthe robotbaseframeattheendofcontrolloope xecution,whileFigure.6depictsthe resultsofrepeatabilitytestintherobot 3 task-space.Thesuccessofthevisual servocontrollerisconsideredifthetargetfruitisinsidethegripperattheendof controlloopexecution. AsshowninFigure.6andFigure.6thecontrollerexhibitslowscatter inrepeatedmovestoagiventargetpositionin -planeaswellasin -plane,thus indicatingthattheproposedcontrollerisrepeatable. Table6:Initialrobotend-e ectorpositionexpressedinrobotbaseframe Coordinateframe (mm) (mm) (mm) Robotbaseframe 655 0 785 0 251 0

PAGE 83

72 Table6:Finalrobotend-e ectorpositionexpressedinrobotbaseframe. Sr.No. (mm) (mm) (mm) Success/failure 1 238 0 1163 0 561 0 success 2 234 0 1121 0 548 0 success 3 215 0 1111 0 555 0 success 4 226 0 1117 0 546 0 success 5 227 0 1115 0 541 0 success 6 227 0 1118 0 542 0 success 7 228 0 1119 0 548 0 success 8 226 0 1120 0 552 0 success 9 229 0 1114 0 546 0 success 10 229 0 1116 0 551 0 success 11 219 0 1108 0 553 0 success 12 230 0 1118 0 540 0 success 13 220 0 1107 0 544 0 success 14 214 0 1106 0 547 0 success 15 231 0 1116 0 536 0 success Figure6: 3 robottask-spacedepictingrepeatabilityresults.

PAGE 84

73 Figure610:Repeatabilityin -planeforthe3Dtargetreconstructionbased visualservocontroller. Figure611:Repeatabilityin -planeforthe3Dtargetreconstructionbased visualservocontroller.

PAGE 85

74 Figure6:PerformancevalidationforexperimentII. Table67:ActualEuclideantargetpositionexpressedina xedcameraframeand robotbaseframe. Coordinateframe (mm) (mm) (mm) Targetfruit#1 Fixedcamera 139 7 25 7 787 4 Robotbaseframe 266 7 1016 0 540 2 Targetfruit#2 Fixedcamera 25 4 172 72 938 8 Robotbaseframe 68 6 1168 4 375 1 6.4.3ExperimentII Inthissection,abehaviorofthecontrolsystemundernon-identicalinitial positionsoftherobottoolframeisidenti ed.Amultipletargetscenariowas constructedtoverifytheperformanceofthecontrollerwhenonlyonetargetcan beseenbythe xedcamerabutthecamerain-handwillhavetwotargetsinthe eld-of-viewafterorientation.Inthisexperimentthepositionofthetargetfruitsis keptconstantwhiletheinitialpositionoftherobotend-e ectorisvariedasshown inFigure.612.ActualmeasuredEuclideanpositionofthetargetfruitsina xed cameraframeandarobotbaseframeisasshowninTable6-7.

PAGE 86

75 Table6:Initialand nalrobotend-e ectorpositionexpressedinrobotbase frame. Sr.No. Initialposition Finalposition Success/failure 1 (mm) (mm) (mm) (mm) (mm) (mm) 2 549 0 896 0 203 0 75 0 1190 0 384 0 success 3 40 0 979 0 61 0 58 0 1188 0 380 0 success 4 715 0 748 0 47 0 58 0 1194 0 391 0 success Figure6:PerformancevalidationforexperimentIII. Initialand nalpositionsoftherobotend-e ectororcamerain-handmeasured inrobotbaseframeareasshowninTable6-8.Comparingthe nalpositionsofthe robotend-e ectorwiththepositionofthetargetfruit#2intherobotbaseframe itisclearthatthecontrollerperformssatisfactorilyundermultipletargetscenario. 6.4.4ExperimentIII Thispartoftheexperimentdiscussestheperformanceofthecontrollerunder di erenttargetpositions.Inthisexperimentthepositionofthetargetfruitsis variedstartingtherobotend-e ectoratdi erentlocationsasshowninFigure. 613.Table6-9showstheactualmeasure dEuclideancoordinatesofthetarget alongwiththeinitialand nalpositionsoftherobotend-e ector. Underthedi erenttargetpositionsandrobotend-e ectorpositionsthe controllerperformssatisfactorily.

PAGE 87

76 Table69:ActualEuclideantargetpositionexpressedina xedcameraframeand robotbaseframeandinitialand nalrobotend-e ectorpositionexpressedinrobot baseframe. Sr.No. Targetfruitposition Targetfruitposition (Fixedcameraframe) (Robotbaseframe) (mm) (mm) (mm) (mm) (mm) (mm) 1 139 7 25 4 787 4 266 7 1016 0 540 2 2 139 7 12 7 838 2 228 6 1066 8 540 2 3 127 9 171 6 782 55 412 6 1011 4 528 9 Sr.No. Initialposition Finalposition Success (Robotbaseframe) (Robotbaseframe)/failure (mm) (mm) (mm) (mm) (mm) (mm) 1 138 0 1015 223 272 0 964 0 563 0 success 2 538 0 554 0 920 0 240 0 900 0 471 0 success 3 39 0 981 0 69 0 416 0 952 0 514 0 success 6.5Conclusion A 3 targetreconstructionbasedvisualservocontrolapproachisproposed forroboticcitrusharvestingwhere aprior knowledgeofthefeaturepointsisnot availableandfeaturepointmatchingdoesnotperformsatisfactorilythusrendering featurepointmatching-basedteachbyzoo mingvisualservocontrolimpractical. 3 targetreconstructionmethodutilizesstatisticaldataofthetargetsizealong withthecameraintrinsicparameterstogenerateanestimatefortheEuclidean positionofatarget.A 3 depthestimationmethodperformssatisfactorilyfor targetsattributinglargerdepthsfromthecameraframeandhencethecontrol algorithmisswitchedfromvision-basedtoIR-basedwhenthecamerain-hand isclosertothetarget.Thecontrollerexhibitsveryhighsuccessrateintermsof accuratelyreachingthetarget,andalsotherepeatabilitytestshowsgoodposition repeatabilityin and -plane.

PAGE 88

CHAPTER7 CONCLUSION 7.1SummaryofResults Automatedroboticcitrusharvestingyieldssuperiorfruitquality,whichis highlydesirableforfreshfruitmarket .Thepresentedworkaccomplishestwo importantcharacteristicsoftheautomatedfruitharvestingsystem,namelyto locatethefruitsonthethreedimensionalspaceandtoapproachandreachforthe fruit.Colorthresholding-basedtechniqueisrealizedfortargetfruitidenti cation, whereasmulti-cameravisualservocontroltechniques, viz. teachbyzooming visualservocontroland 3 targetreconstructionbasedvisualservocontrol,are developedfor 3 targetpositionestimationandrobotmotioncontrol. Ateachbyzoomingvisualservocontrolapproachisproposedforapplications wherethecameracannotbe apriori positionedtothedesiredposition/orientation toacquireadesiredimagebeforeservocontrol.Speci cally,theteachbyzooming controlobjectiveisformulatedtoposition/orientanon-boardcamerabasedona referenceimageobtainedbyanothercamera.Inadditiontoformulatingtheteach byzoomingcontrolproblem,anothercontributionofthisworkistoillustratehow topreserveasymmetrictransformationfromtheprojectivehomographytothe Euclideanhomographyforproblemswhenthecorrespondingimagesaretakenfrom di erentcameraswithcalibrationuncertainty.Simulationresultsareprovidedto illustratetheperformanceofthecontroller.Rotationandtranslationerrorsare exponentiallyregulatedtozerothusestablishingthestabilityofthecontroller, whereastheangularandlinearcontrolvelocitiesarealwaysbounded.Applications ofthisstrategycouldincludenavigatinggroundorairvehicles,basedondesired imagestakenbyothergroundorairvehicles. 77

PAGE 89

78 Practicallimitationontheimplementationoftheteachbyzoomingvisual servocontrollerforthecitrusharvestingapplicationhasbeenrealizedinthe factthatthefeaturepointsviewedbythe xedcameracanbedi erentthanthe featurepointsviewedbythecamerain-hand.Non-identicalfeaturepointsbetween thecurrentimageandthedesiredimagewouldresultinincorrectrotationand translationinformation.Hencetheteachbyzoomingvisualservocontrollercan beimplementedwherethefeaturepointinformationisavailable apriori ,i.e.this controllerissuitableforarti cialtargets. A 3 targetreconstruction-basedvisualservocontrolapproachisrealized forroboticcitrusharvestingwhere aprior knowledgeofthefeaturepointsisnot availableandfeaturepointmatchingdoesnotperformsatisfactorilythusrendering featurepointmatching-basedteachbyzoo mingvisualservocontrolimpractical. Speci cally,statisticaldataofthetargetsizeisusedfor 3 targetreconstruction forgeneratinganestimatefortheEuclideanpositionofatarget.A 3 depth estimationmethodperformssatisfactor ilyfortargetsattributinglargerdepths fromthecameraframeandhencethecontrolalgorithmisswitchedfromvisionbasedtoIR-basedwhenthecamerain-handisclosertothetarget.Thecontroller exhibitsveryhighsuccessrateintermsofaccuratelyreachingthetarget,alsothe repeatabilitytestshowsgoodpositionrepeatabilityin and -plane.Moreover, theimage-basedvisualservocontrolalongwiththeEuclideandepthinformation isusedandhencethecontrollerisrobusttothetargetstatisticaldataaswellas cameracalibrationparameters. 7.2RecommendationsforFutureWork Oneoftheissueswithteachbyzoomingvisualservocontrollerisofconsistentlyidentifyingatleastfourfeaturepoints,whicharenecessaryforhomography decompositiontoacquirerotationandtranslationinformationbetweenvarious cameracoordinateframes.Thisissuecanbeaddressedbyprojectinganarti cial

PAGE 90

79 gridonthetargetandacquiringfeaturepointsonthegrid. Aprior knowledgeofa gridsegmentcanalsobeutilizedfortargetdepthestimation.Non-identicalfeature pointmatchingissuecanberesolvedbyusinganarti cialgridtoconsistentlyobtainfeaturepointsonthetargetandcontrollingthetrajectoryofacamerain-hand suchthat xedcameraaswellascamerain-handobtainidenticalfeaturepoints. Multi-viewphotogrammetrytechniquesworksundertheassumptionthat thefourfeaturepointsarecoplanarpoints.Imagesegmentationandtexture recognitiontechniquescanbeusedtorecognizedi erentplaneswhichwould helpinde ningtheregionofinterestofthefeaturedetectionalgorithmtoensure featurepointsarecoplanar.Thiswouldalsomakethetrackermoreconsistent andreliabletointensityvariationsinthescene.Anotherissuetobeaddressed istomakesurethatthepointsselectedarenotcollinear.Also,theconditionof thefourfeaturepointsrequiredtobecoplanarandcollinearcanbeeliminatedby implementingtheeightpointalgorithmproposedin[16],wherethefeaturepoints donthavetosatisfythementionedconstraints. Targetdetectiontaskisprimarilyperformedbya xedcameramountedon astationaryrobotbasejointandcamerain-handisservoedbasedontheselected target.Targetdetectione ciencycanbeenhancedbyperformingthetarget detectionby xedcameraaswellascamerain-hand.Moreover,simultaneous localizationandmapping(SLAM)oftargetfruitscanbeachievedbyperforming thedetectiontaskbyacamerain-handalongwitha xedcamerainorderto generateathreedimensionalmapofthescenefore cientharvesting.

PAGE 91

APPENDIXA OPEN-LOOPERRORDYNAMICS A.1RotationController Arotationerror-likesignal,denotedby ( ) R3,isde nedbytheangleaxis representationin(54)asfollows: = (A1) Takingthederivativeof(A1)withrespecttotime = ( ) = + (A2) Multiplyingequation(A2)by ( + 2 ) andusingpropertiesofaskew symmetricmatrixthefollowingexpressioncanbeobtained: ( + 2 ) ( ) = (A3) where R3 3denotesanidentitymatrixand denotesthe 3 3 skew-symmetric matrixassociatedwith ( ) Similarly,multiplyingequation(A2)by ( 2 ) weget, ( 2 ) ( ) = (A4) Theangularvelocity,expressedinthecurrentcameraframe,isde nedas follows: = ( ) ( ) (A5) 80

PAGE 92

81 From(56)andutilizingthepropertiesofaskewsymmetricmatrixthe expressionfor in(A5)canbewrittenasfollows: =sin + +(1 cos )( )(A6) where R3 3denotesthe 3 3 skew-symmetricmatrixassociatedwith ( ) Utilizingthepropertiesdevelopedin(A3)and(A4),theexpressionfor angularvelocity canbeobtainedas, = +[sin +(1 cos ) ] = +[sin + 2 sin 2 2 ] =[ + 2 sin 2 2 +(1 sin ) 2 ] | {z } ( ) (A7) 1 ( ) wheretheJacobianmatrix ( ) R3 3isde nedasfollows: =( 1 ) 1= + 2 sin 2 2 +(1 sin ) 2 (A8) Inequation(A)the sin ( ) termisgivenby(A)as, sin ( )= sin( ) (A9) Basedonthecameraextrinsicparam etersgivenin(53)andexpression (A),theopen-looperrordynamicscanbeobtainedasfollows: = ( ) = (A)

PAGE 93

82 A.2TranslationController Thedi erencebetweentheactualanddesired 3 Euclideancameraposition, denotedbythetranslationerrorsignal ( ) R3,isde nedas (A) where ( ) R3denotestheextendedcoordinatesofanimagepointon expressedintermsof F and R3denotestheextendedcoordinatesofthe correspondingdesiredimagepointon intermsof Fgivenin(5)and(5), respectively. Takingthederivativeof in(A1)withrespecttotime = 1 1 1 2 1 1 1 1 1 2 1 1 1 1 = 1 1 10 1 101 1 1001 1 1 1 = 1 1 10 1 101 1 1001 [ 1] = 1 1 10 1 101 1 1001 1 1 11 1 1 2 1 111+ 2 1 1 11 1 1 110 = + [ ]+ (A)

PAGE 94

83 where(533),(544),andthefollowingfacthavebeenutilized[26] 1= +[ 1] (A) InA12,theJacobian-likematrices ( ) ( ) R3 3arede nedasfollows: 1 1 10 101 2001 (A) 1 21+ 2 1 2 1 2 2 1 2 1 2 10 .(A15)

PAGE 95

APPENDIXB TARGETIDENTIFICATIONANDFEATUREPOINTTRACKING ALGORITHM B.1TargetDetection //************ *************** *************** ************ ************* //PROGRAMSTART //************ *************** *************** ************ ************* //************ *************** *************** ************ ************* //Whatthisprogramdoes: //1)Detectstarget(citrus)basedoncolorthresholdingprinciple //fortheimagetakenby xedcameraaswellascamerain-hand. //2)Estimatesthetargetregionareaalongwiththediameteranddepthof thetarget. //Whatistheinputtothisprogram: //Theinputissequenceofimages(i.e.real-timevideo)takenusingtwo cameras //Whatistheoutputofthisprogram: //Targetregionarea,targetdiameter,estimateoftargetdepth //Thecodegivenbelowdoesnotincludetheentireprogramusedforthe autonomous //roboticcitrusharvester,butshowstheimportantfunctions. //************ *************** *************** ************ ************* #include"StdAfx.h" #include"ImagProcFunctions.h" #include fstream.h 84

PAGE 96

85 #de nePI3.141592654 //enableingdebugginghasNOnotablee ectonprocessingtime #de neDEBUG_THRESH #de neDEBUG_HOLES #de neDEBUG_ISO #de neDEBUG_REGION #de neDEBUG_CENT #de neDEBUG_PER #de neDEBUG_EDGE #de neDEBUG_POSS_CTRS externTBZControllerController; externimg_proc1Docimg_proc; doubleKd=0.025; doubleKr=1.0; doublecost; doubleminCost=99999999999; doubleCFmaxRadius[150]; doubleCiHmaxRadius[150]; //************ *************** *************** ************ ************* //============= ================= ============ //ThisoneFunctioncallsalltheneedfunctionstoperformCentroidbased objectdetection //NOTE:Thresholdfunctionneedstobecalledbeforethis //============= ================= ============ intImagProcFunctions::CFCentroidBasedDetection(unsignedchar*CFlpDib, intMinRegionArea) {

PAGE 97

86 if(img_proc.FLAG_TRACK_0_LOCKED=1) { // ndtheregionsinthethresholdedimageresultsarein m_Regions intnumRegionsCF=CFFindRegions(CFlpDib,m_lpBiThrshImgCF, MinRegionArea); // ndthecentroidoftheregionsresultsareinm_CentCtrs m_nNumCentroidsCF=CFFindRegionCentroid(CFlpDib); } returnm_nNumCentroidsCF; } //************ *************** *************** ************ ************* //============= ================= ============ //ThresholdimageusingColorDi erenceSignals //Italsodoesthefollowing //-intializesboththeBinary(usedinsegmentation)andGrayScale8bit images // //pureorangecolorisRGB=(255,102,0) //lessredmakestheorangecolorappearbrown //moregreenmakestheorangecolorappearyellow //= yellowisRGB=(255,255,0) //morebluemakestheorangecolorappearpastel // //newparametervalueson1/15/04 //CrU=90,CrL=22;//red brown //CgU=25,CgL=-25;//yellow red

PAGE 98

87 //CbU=-5,CbL=-220;//pastel red // //Resultsareplacedin:m_lpBiThrshImg,m_lpCleanedImg, //andm_lpGrayThrshImg //============= ================= ============ voidImagProcFunctions::CFColorDi Threshold(unsignedchar*lpDibCF,int CrU,intCrL,intCgU,intCgL,intCbU,intCbL) { inti; intpixel; intblue,green,red; doubleCr,Cg,Cb; for(pixel=0,i=0;pixel m_nSIZE;pixel++,i=i+3) { blue=i; green=i+1; red=i+2; //Isthecolordi erenceintherightrange? //checkred Cr=0.721*lpDibCF[red]-0.587*lpDibCF[green] -0.114*lpDibCF[blue]; if((Cr CrL) || (Cr CrU)) { m_lpBiThrshImgCF[pixel]=0; m_lpGrayThrshImgCF[pixel]=0; }

PAGE 99

88 else { //checkgreen Cg=-0.299*lpDibCF[red]+0.413*lpDibCF[green] -0.114*lpDibCF[blue]; if((Cg CgL) || (Cg CgU)) { m_lpBiThrshImgCF[pixel]=0; m_lpGrayThrshImgCF[pixel]=0; } else { //checkblue Cb=-0.299*lpDibCF[red] -0.587*lpDibCF[green] +0.886*lpDibCF[blue]; if((Cb CbL) || (Cb CbU)) { m_lpBiThrshImgCF[pixel]=0; m_lpGrayThrshImgCF[pixel]=0; } else { m_lpBiThrshImgCF[pixel]=255; //itcantakeuptoanextra6-8mstodothiscalculationfortheimage

PAGE 100

89 m_lpGrayThrshImgCF[pixel]=(unsignedchar)(0.299*lpDibCF[red]+ 0.587*lpDibCF[green]+0.114*lpDibCF[blue]); //thisisjustfordisplay //colorthepixelintheoriginalimage #ifdefDEBUG_THRESH lpDibCF[blue]=255; lpDibCF[green]=0; lpDibCF[red]=0; #endif } } } } } //************ *************** *************** ************ ************* //============= ================= ============ //Aqueue-basedpaintllroutineforcolorimageusing4connectivity //MinRegionAreaistheminimumregionthatisdesired //Returnsthenumberofregionsfound= Thiscanalsobefoundin m_nNumRegs //Findsthefollowing: //coordstotheseregionscanbefoundinm_lpRegCoords //theregionssize(#ofpixels)isinm_nRegSize //thememoryo setoftheregionspixelsisinm_nRegMemO set //thepixelclosesttotheoriginforeachregionisinm_nRegMinCoord //============= ================= ============

PAGE 101

90 intImagProcFunctions::CFFindRegi ons(unsignedchar*CFlpDib,unsigned char*CFimage,intMinRegionArea) { inti; intpixel; intqh,qt; intcount; intminCoord; unsignedcharpaintOverLabel=255,newPaintLabel=1; //thisisjustfordisplay #ifdefDEBUG_REGION intcPix; #endif //Loopthroughbinaryimagelookingforregions m_RegionsCF[0].memO set=0; m_nNumRegsCF=0; //startchecking for(i=0;i m_nSIZE;i++) { //Checktoseeifinitial/seedpixelisnotabackgroundpixel if((CFimage[i]==paintOverLabel)&&(m_nNumRegsCF MAX_REGIONS)) { //seedpixelisnotbackground= saveinfoonit CFimage[i]=newPaintLabel;

PAGE 102

91 m_lpRegPixQueueCF[0]=i; m_lpRegCoordsCF[m_RegionsCF[m_nNumRegsCF]. memO set]=i; //initvaraibles count=1; qh=1; qt=0; minCoord=i; //startlookingforotherpixelsaroundseedpixel //theloopwillexitafteritcirclesaroundand ndsonlyitselfas aviablepixel while(qt!=qh) { //lookbelow pixel=m_lpRegPixQueueCF[qt]-m_nCOL; //makesurecoordinateisinimageANDitisnotabackgroundpixel if((m_lpRegPixQueueCF[qt]/m_nCOL 0)&&(CFimage[pixel]==paintOverLabel)) { //SetnewfoundpixeltonewPaintLabel CFimage[pixel]=newPaintLabel; //Savenewfoundpixelscoordinatesandintensity m_lpRegCoordsCF[m_RegionsCF[m_nNumRegsCF]. memO set+count]=pixel;

PAGE 103

92 //checktoseeifitistheclosesttotheorigin if(pixel minCoord) { minCoord=pixel; } //incrementnumberofpixelsinregion count++; //setnextpixelinqueuetosearchasthepixelthat wasjustfound m_lpRegPixQueueCF[qh]=pixel; //checktoseeifwehavereachedthemaxnumberof coords //a%bwillalwaysreturnaifa b //thisstatementalsoincrementsqhby1 qh=(qh+1)%MAX_REG_COORDS; } pixel=m_lpRegPixQueueCF[qt]+m_nCOL; if(m_lpRegPixQueueCF[qt]/m_nCOL m_nROW-1&& CFimage[pixel]==paintOverLabel) { CFimage[pixel]=newPaintLabel; m_lpRegCoordsCF[m_RegionsCF[m_nNumRegsCF]. memO set+count]=pixel; if(pixel minCoord) {

PAGE 104

93 minCoord=pixel; } count++; m_lpRegPixQueueCF[qh]=pixel; qh=(qh+1)%MAX_REG_COORDS; } pixel=m_lpRegPixQueueCF[qt]-1; if(m_lpRegPixQueueCF[qt]%m_nCOL 0&&CFimage[pixel]==paintOverLabel) { CFimage[pixel]=newPaintLabel; m_lpRegCoordsCF[m_RegionsCF[m_nNumRegsCF]. memO set+count]=pixel; if(pixel minCoord) { minCoord=pixel; } count++; m_lpRegPixQueueCF[qh]=pixel; qh=(qh+1)%MAX_REG_COORDS; } pixel=m_lpRegPixQueueCF[qt]+1;

PAGE 105

94 if(m_lpRegPixQueueCF[qt]%m_nCOL m_nCOL-1&& CFimage[pixel]==paintOverLabel) { CFimage[pixel]=newPaintLabel; m_lpRegCoordsCF[m_RegionsCF[m_nNumRegsCF]. memO set+count]=pixel; if(pixel minCoord) { minCoord=pixel; } count++; m_lpRegPixQueueCF[qh]=pixel; qh=(qh+1)%MAX_REG_COORDS; } qt++; } //Checkregion //makesureareaislargeenough //ifareaistoosmalleraseitfromimage //makesurenottowriteoutsidearrays if((count MinRegionArea)&&(m_nNumRegsCF MAX_REGIONS)) { //savethenumberofpixelsinregion //arraystartsat0andnumofregionsstartsat1= do thisstepbeforeincrementingnumofregions

PAGE 106

95 m_RegionsCF[m_nNumRegsCF].area=count; //savepixelthatisclosesttoorigin m_RegionsCF[m_nNumRegsCF].minCoord=minCoord; //saveregionslabel m_RegionsCF[m_nNumRegsCF].label=newPaintLabel; //incrementthenumberofusefulregions m_nNumRegsCF++; //seto setfornextsetofcoordinates m_RegionsCF[m_nNumRegsCF].memO set=count+ m_RegionsCF[m_nNumRegsCF-1].memO set; //setnewpaintlabel newPaintLabel=newPaintLabel++; } else { //notaviableregionsodeleteitfromimage for(pixel=0;pixel count;pixel++) { CFimage[m_lpRegCoordsCF[m_RegionsCF [m_nNumRegsCF].memO set+pixel]]=0; m_lpGrayThrshImgCF[m_lpRegCoordsCF [m_RegionsCF[m_nNumRegsCF].memO set+pixel]] =0; //thisisjustfordisplay //colorthepixelintheoriginalimage #ifdefDEBUG_REGION

PAGE 107

96 cPix=3*m_lpRegCoordsCF[m_RegionsCF [m_nNumRegsCF].memO set+pixel]; CFlpDib[cPix]=0; CFlpDib[cPix+1]=0; CFlpDib[cPix+2]=255;//red #endif } } } } returnm_nNumRegsCF; } //************ *************** *************** ************ ************* //============= ================= ============ //Findsthecentroid(akacenterofmass)ofthepreviouslyfoundregions //thelocationofthecentroidissavedin2Dasagrayscalepixelnumbernot RGB //============= ================= ============ intImagProcFunctions::CFFindRegi onCentroid(unsignedchar*CFlpDib) { intreg,pixel; intx,y; intnumCent=0; m_nAvgObjDiameter=67.1576; #ifdefDEBUG_CENT intcPix; intr,c,temp;

PAGE 108

97 #endif //loopthroughalltheregions for(reg=0;reg m_nNumRegsCF;reg++) { //col= xandrow= yforbottomupDIB x=0; y=0; //addupallcoordsinregion //for(pixel=0;pixel m_nRegSize[reg];pixel++) for(pixel=0;pixel m_RegionsCF[reg].area;pixel++) { x+=m_lpRegCoordsCF[m_RegionsCF[reg].memO set+pixel ]%m_nCOL; y+=m_lpRegCoordsCF[m_RegionsCF[reg].memO set+pixel ]/m_nCOL; } //averagevaluestogetcentroid m_CentCtrsCF[reg].Xp[0]=x/m_RegionsCF[reg].area; m_CentCtrsCF[reg].Xp[1]=y/m_RegionsCF[reg].area; // nddistancecentroidisfromcenterofimage(note:0,0isnotthe centeroftheimage) m_CentCtrsCF[reg].dist=(int)sqrt(pow(m_CentCtrsCF[reg].Xp[0](m_nCOL/2),2)+pow(m_CentCtrsCF[reg].Xp[1]-(m_nROW/2),2)); //theisnoradiusassociatedwiththecentroidofaregion //assumeregionsisround,andthusfromarea=PI*radius^2radius=sqrt(area/PI)

PAGE 109

98 m_CentCtrsCF[reg].radius=(int)sqrt(m_RegionsCF[reg].area/PI); m_RegionsCF[reg].depth=(Controller.m_CFCalibrationMatrix. m_Value[0][0]*m_nAvgObjDiameter) /(2*2*m_CentCtrsCF[reg].radius); //2timesradiussincetheimageis320x240insteadof640x480 m_RegionsCF[reg].cost=Kr*(320/ m_CentCtrsCF[reg].radius-1)+ Kd*m_RegionsCF[reg].depth; // ndlowestcost if(m_RegionsCF[reg].cost minCost) { minCost=m_RegionsCF[reg].cost; m_RegionsCF[reg].number=1; m_HarvestFruit.RegionNumCF=reg; } if(m_CentCtrsCF[m_HarvestFruit.RegionNumCF].radius CFmaxRadius[m_HarvestFruit.RegionNumCF]) { CFmaxRadius[m_HarvestFruit.RegionNumCF]= m_CentCtrsCF[m_HarvestFruit.RegionNumCF].radius; m_HarvestFruit.RadiusCF= CFmaxRadius[m_HarvestFruit.RegionNumCF]; m_HarvestFruit.DepthCF=(Controller.m_CFCalibrationMatrix. m_Value[0][0]*m_nAvgObjDiameter) /(2*2*m_HarvestFruit.RadiusCF); } /* OutputDebugString(" \ n");

PAGE 110

99 OutputDebugString("Camera0"); charradius[20]; itoa(m_HarvestFruit.RadiusCF,radius,10); OutputDebugString(radius); OutputDebugString(""); chardepth[20]; itoa(m_HarvestFruit.DepthCF,depth,10); OutputDebugString(depth); OutputDebugString(" \ n"); */ //incrementthenumberofcentroidsfound numCent++; //thisisjustfordisplay //drawcrosshairs #ifdefDEBUG_CENT temp=m_CentCtrsCF[reg].Xp[1]*m_nCOL +m_CentCtrsCF[reg].Xp[0]; for(r=-20;r =20;r++) { cPix=temp+r; if((cPix 0)&&(cPix m_nSIZE)) { cPix=3*cPix; CFlpDib[cPix]=0; CFlpDib[cPix+1]=255; CFlpDib[cPix+2]=0; }

PAGE 111

100 } for(c=-20;c =20;c++) { cPix=temp+c*m_nCOL; if((cPix 0)&&(cPix m_nSIZE)) { cPix=3*cPix; CFlpDib[cPix]=0; CFlpDib[cPix+1]=255; CFlpDib[cPix+2]=0; } } #endif } returnnumCent; } //************ *************** *************** ************ ************* //============= ================= ============ //TransformPixelCoordinatestoCameraCoordinates //-Xparethesuppliedpixelcoordinatesina2elementarray(xp,yp) //-distanceInchistheditancetothesceneininches //-Xnarethenormalcoordsthehavenounitsandrequiretherangefor anyrealuse //-Xcisthereturned3elementarraywithcameracoordinatesininches (xc,yc,zc) //

PAGE 112

101 //TheZaxisforXcisthelineofsightofthecamera:Xp=(0,0)= Xc=(xc,-yc) //Thecenterofthecameraframeistheprinciplepointinpixelframe: Xp=(ppx,ppy)= Xc=(0,0) //============= ================= ============ voidImagProcFunctions::TransformCoordsInv(int*Xp,doubledistanceInch, double*Xn,double*Xc) { inti; doubleR2,R4,R6,K; doubleold[2],delta[2]; //getdata m_Xp.m_Value[0][0]=Xp[0]; m_Xp.m_Value[1][0]=Xp[1]; m_Xp.m_Value[2][0]=1; //convertfrompixelcoordstodistortioncoords m_CamMatInv=m_CameraMatrix^-1; m_Xd=m_CamMatInv*m_Xp; //convertfromdistortioncoordstonormalcoords m_Xn=m_Xd;//initialguessnormal=distortion for(i=0;i 20;i++) { old[0]=m_Xn.m_Value[0][0]; old[1]=m_Xn.m_Value[1][0]; R2=pow(m_Xn.m_Value[0][0],2)+pow(m_Xn.m_Value[1][0],2); R4=R2*R2; R6=R2*R4;

PAGE 113

102 K=1/(1+m_dKcCiH[0]*R2+m_dKcCiH[1]*R4 +m_dKcCiH[4]*R6); //X=Xd-dX m_Xn.m_Value[0][0]=m_Xd.m_Value[0][0](2*m_dKcCiH[2]*m_Xn.m_Value[0][0]*m_Xn.m_Value[1][0] +m_dKcCiH[3]*(R2+2*m_Xn.m_Value[0][0] *m_Xn.m_Value[0][0])); m_Xn.m_Value[1][0]=m_Xd.m_Value[1][0](m_dKcCiH[2]*(R2+2*m_Xn.m_Value[1][0]*m_Xn.m_Value[1][0]) +2*m_dKcCiH[3]*m_Xn.m_Value[0][0]*m_Xn.m_Value[1][0]); m_Xn.m_Value[2][0]=1; //Xn=K*X m_Xn=m_Xn*K; delta[0]=fabs(m_Xn.m_Value[0][0]-old[0]); delta[1]=fabs(m_Xn.m_Value[1][0]-old[1]); } m_Xn.m_Value[2][0]=1; //returnvaluestoXnarray Xn[0]=m_Xn.m_Value[0][0]; Xn[1]=m_Xn.m_Value[1][0]; //convertfromnormalcoordstocameracoords(distanceneedstobein centimeters) m_Xc=m_Xn*distanceInch; Xc[0]=m_Xc.m_Value[0][0]; Xc[1]=m_Xc.m_Value[1][0];

PAGE 114

103 Xc[2]=m_Xc.m_Value[2][0]; } //************ *************** *************** ************ ************* //PROGRAMEND //************ *************** *************** ************ ************* B.2FeaturePointTracking //************ *************** *************** ************ ************* //PROGRAMSTART //************ *************** *************** ************ ************* //************ *************** *************** ************ ************* //Whatthisprogramdoes: //1)Detectsfeaturepointsinthegivenimagebasedonthedi erential intensityalong //xandy-axis. //2)Tracksthefeaturepointsfromoneframetothenext //Whatistheinputtothisprogram: //Theinputissequenceofimages(i.e.real-timevideo)takenusingtwo cameras //Whatistheoutputofthisprogram: //Pixelcoordinatesofthetrackedfeaturepoints //Thecodegivenbelowdoesnotincludetheentireprogramusedforthe autonomous //roboticcitrusharvester,butshowstheimportantfunctions. //************ *************** *************** ************ ************* #pragmaonce #include StdAfx.h #include"cv.h"

PAGE 115

104 #include"highgui.h" #include"FPTracking.h" #include windef.h externTBZControllerController; externimg_proc1Docimg_proc; externCitrusDetectionCitrusDetect; externLinearAlgebram_CiHHarvestFruitPixCoord; externLinearAlgebram_CFHarvestFruitPixCoord; #ifdef_DEBUG #undefTHIS_FILE staticcharTHIS_FILE[]=__FILE__; #de nenewDEBUG_NEW #endif #de nePI3.14159265358979 3238462643 3832795; //Variables: constintsobel=1;//imagesizeinIpl intth_fp=5000,//thresholdforfeaturepoint detection Wx=7,//x-dirtrackingwindowsize (pixels) Wy=7,//y-dirtrackingwindowsize (pixels) x_lim=10,//x-dirminimumdistancefor featurepoints(pixels) y_lim=10,//y-dirminimumdistancefor featurepoints(pixels)

PAGE 116

105 border_x=10,//horizontalimageborderpixels(thisregionisnotprocessed) border_y=10,//verticalimageborderpixels(thisregionisnotprocessed) fp0[320*240][2],//arrayofpossiblefeaturepoint locations fp1[320*240][2],//arrayofpossiblefeaturepoint locations n_res=3,//numberofresolutions (currentmaxis3duetoprogramminglimitations) bound_x=(Wx-1)/2, bound_y=(Wy-1)/2, LostFeatCount=0, CtrImg_horz=80, CtrImg_vert=60; doubletrack_tol=.1,//convergencecriteriafor tracker(pixels) fp_s0[320*240],//minimumeigenvaluesfor possiblefeaturepointlocations fp_s1[320*240];//minimumeigenvaluesfor possiblefeaturepointlocations //************ *************** *************** ************ ************* //FUNCTIONget_feature_points0forCAMERA0 //get_feature_points0:FindsthefeaturepointsintheimagebasedonSobel lter //************ *************** *************** ************ ************* voidFPTracking::get_feature_points0(IplImage*img)

PAGE 117

106 { if(img_proc.FLAG_TRACK_1_LOCKED==0) { img_proc.FLAG_TRACK_0_LOCKED=1; doubleIxx,Iyy,Ixy,single[2],s_min; intfp_count=0; //initializeimagederivativearrays Ix_0=cvCreateImage(img_proc.proc_size,IPL_DEPTH_32F,1); Iy_0=cvCreateImage(img_proc.proc_size,IPL_DEPTH_32F,1); cvSobel(img,Ix_0,1,0,sobel); cvSobel(img,Iy_0,0,1,sobel); //slidewindowoverentireimage(minusborders) for(inti=border_x+bound_x+(img_ proc.n_horz-CtrImg_horz)/2; i img_proc.n_horz-border_x-bound_x-1-(img_proc.n_horz-CtrImg_horz)/2; i++) { for(intj=border_y+bound_y+(img_proc.n_vert-CtrImg_vert) /2;j img_proc.n_vert-border_y-bound_y-1 -(img_proc.n_vert-CtrImg_vert)/2;j++) { //setregionofinterest CvRectwindow=cvRect(i,j,Wx,Wy); cvSetImageROI(Ix_0,window); cvSetImageROI(Iy_0,window); //calculateG Ixx=cvDotProduct(Ix_0,Ix_0);

PAGE 118

107 Iyy=cvDotProduct(Iy_0,Iy_0); Ixy=cvDotProduct(Ix_0,Iy_0); CvMatG,s; doubletemp[2][2]={{Ixx,Ixy},{Ixy,Iyy}}; cvInitMatHeader(&G,2,2,CV_64FC1,temp); //Singularvaluedecomposition s=cvMat(2,1,CV_64FC1,single); cvSVD(&G,&s); s_min=cvGetReal1D(&s,1); if(s_min th_fp) { fp0[fp_count][0]=i+bound_x; fp0[fp_count][1]=j+bound_y; fp_s0[fp_count]=s_min; fp_count++; } } } cvResetImageROI(Ix_0); cvResetImageROI(Iy_0); cvReleaseImage(&Ix_0); cvReleaseImage(&Iy_0); //sortfp0andfp_s0arraysintodescendingorder sortDescend0(fp0,fp_s0,fp_count); } else img_proc.FLAG_TRACK_0_LOCKED=0;

PAGE 119

108 } //************ *************** *************** ************ ************* //FUNCTIONtrack_multi_res0forCAMERA0 //track_multi_res0:FeaturepoonttrackingfunctionbasedonmultiresolutionKLTalgorithm //************ *************** *************** ************ ************* voidFPTracking::track_multi_res0(IplImage*img_prev_0,IplImage* img_curr_0,intgot_features,intframe_count) { if(img_proc.FLAG_TRACK_1_LOCKED==0) { img_proc.FLAG_TRACK_0_LOCKED=1; doublex,y,xn,yn,xi,yi,xf,yf,boundx_res,boundy_res, borderx_res,bordery_res; doubleIxx_sub,Iyy_sub,Ixy_sub,Ixt_sub,Iyt_sub, temp_G[4],temp_b[2],d[2],d0[2]; intxr,yr,n_horz_c,n_vert_c; CvSizeres_size;//imagesizeinIplformat CvSizewin_size=cvSize(Wx,Wy);//windowsizeinIplformat CvMatd_mat,G,b; inttrack_pts=0; intsumx=0; intsumy=0; intCtrImg_CtrX; intCtrImg_CtrY; Ix_0=cvCreateImage(img_proc.proc_size,IPL_DEPTH_32F,1); Iy_0=cvCreateImage(img_proc.proc_size,IPL_DEPTH_32F,1);

PAGE 120

109 cvSobel(img_prev_0,Ix_0,1,0,sobel); cvSobel(img_prev_0,Iy_0,0,1,sobel); //createdecimatedimages if(n_res 0) { n_horz_c=img_proc.n_horz/(int)pow(2,1); n_vert_c=img_proc.n_vert/(int)pow(2,1); res_size=cvSize(n_horz_c,n_vert_c); img_curr_1=cvCreateImage(res_size,IPL_DEPTH_32F,1); img_prev_1=cvCreateImage(r es_size,IPL_DEPTH_32F,1); cvResize(img_prev_0,img_prev_1,CV_INTER_AREA); cvResize(img_curr_0,img_curr_1,CV_INTER_AREA); Ix_1=cvCreateImage(res_size,IPL_DEPTH_32F,1); Iy_1=cvCreateImage(res_size,IPL_DEPTH_32F,1); cvSobel(img_prev_1,Ix_1,1,0,sobel); cvSobel(img_prev_1,Iy_1,0,1,sobel); } if(n_res 1) { n_horz_c=img_proc.n_horz/(int)pow(2,2); n_vert_c=img_proc.n_vert/(int)pow(2,2); res_size=cvSize(n_horz_c,n_vert_c); img_curr_2=cvCreateImage(res_size,IPL_DEPTH_32F,1); img_prev_2=cvCreateImage(r es_size,IPL_DEPTH_32F,1); cvResize(img_prev_1,img_prev_2,CV_INTER_AREA); cvResize(img_curr_1,img_curr_2,CV_INTER_AREA); Ix_2=cvCreateImage(res_size,IPL_DEPTH_32F,1);

PAGE 121

110 Iy_2=cvCreateImage(res_size,IPL_DEPTH_32F,1); cvSobel(img_prev_2,Ix_2,1,0,sobel); cvSobel(img_prev_2,Iy_2,0,1,sobel); } if(n_res 2) { n_horz_c=img_proc.n_horz/(int)pow(2,3); n_vert_c=img_proc.n_vert/(int)pow(2,3); res_size=cvSize(n_horz_c,n_vert_c); img_curr_3=cvCreateImage(res_size,IPL_DEPTH_32F,1); img_prev_3=cvCreateImage(r es_size,IPL_DEPTH_32F,1); cvResize(img_curr_2,img_curr_3,CV_INTER_AREA); cvResize(img_prev_2,img_prev_3,CV_INTER_AREA); Ix_3=cvCreateImage(res_size,IPL_DEPTH_32F,1); Iy_3=cvCreateImage(res_size,IPL_DEPTH_32F,1); cvSobel(img_prev_3,Ix_3,1,0,sobel); cvSobel(img_curr_3,Iy_3,0,1,sobel); } //loopforallfeaturepoints for(inti=0;i img_proc.tracked_pts0;i++) { //roundpastfeaturepointlocations x=fpts0[i][0]; y=fpts0[i][1]; d[0]=0; d[1]=0; xn=x;

PAGE 122

111 yn=y; xr=(int)(x+.5); yr=(int)(y+.5); //multiscaleloop(imageisequaltooriginalwhenres=0) if((xr border_x+bound_x)&&(xr img_proc.n_horzborder_x-bound_x-1)&&(yr border_y+bound_y)&&(yr img_proc.n_vertborder_y-bound_y-1)) { for(intres=n_res;res =0;res) { //calculatenumberofpixelsforcoursegrid n_horz_c=img_proc.n_horz/(int)pow(2,res); n_vert_c=img_proc.n_vert/(int)pow(2,res); //determinefeaturepointlocationincoursegrid xi=(x/pow(2,res))+0.5-(1/pow(2,res+1)); yi=(y/pow(2,res))+0.5-(1/pow(2,res+1)); xf=(xn/pow(2,res))+0.5-(1/pow(2,res+1)); yf=(yn/pow(2,res))+0.5-(1/pow(2,res+1)); boundx_res=bound_x/pow(2,res); boundy_res=bound_y/pow(2,res); borderx_res=border_x/pow(2,res); bordery_res=border_x/pow(2,res); CvPoint2D32fpt_i=cvPoint2D32f(xi,yi); CvPoint2D32fpt_f=cvPoint2D32f(xf,yf); //calculateonlyiffeaturepointisintheimage if(( oor(xf)-boundx_res-borderx_res =0)

PAGE 123

112 &&(ceil(xf)+boundx_res+borderx_res n_horz_c) &&( oor(xi)-boundx_res-borderx_res =0) &&(ceil(xi)+boundx_res+borderx_res n_horz_c) &&( oor(yf)-boundy_res-bordery_res =0) &&(ceil(yf)+boundy_res+bordery_res n_vert_c) &&( oor(yi)-boundy_res-bordery_res =0) &&(ceil(yi)+boundy_res+bordery_res n_vert_c)) { if(res==0) { Ix_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Ix_0,Ix_sub,pt_i); Iy_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Iy_0,Iy_sub,pt_i); img_curr_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_prev_0,img_curr_sub,pt_i); img_prev_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix

PAGE 124

113 (img_curr_0,img_prev_sub,pt_f); It_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvSub(img_curr_sub,img_prev_sub,It_sub); //calculateGandb Ixx_sub=cvDotProduct(Ix_sub,Ix_sub); Iyy_sub=cvDotProduct(Iy_sub,Iy_sub); Ixy_sub=cvDotProduct(Ix_sub,Iy_sub); Ixt_sub=cvDotProduct(Ix_sub,It_sub); Iyt_sub=cvDotProduct(Iy_sub,It_sub);; temp_G[0]=Ixx_sub; temp_G[1]=Ixy_sub; temp_G[2]=Ixy_sub; temp_G[3]=Iyy_sub; temp_b[0]=Ixt_sub; temp_b[1]=Iyt_sub; cvInitMatHeader(&G,2,2,CV_64FC1, temp_G); cvInitMatHeader(&b,2,1,CV_64FC1, temp_b); //solveforcoursedisplacement if((cvDet(&G) 1) || (cvDet(&G) 1)) { d0[0]=0; d0[1]=0; cvInitMatHeader(&d_mat,2,1, CV_64FC1,d0);

PAGE 125

114 cvSolve(&G,&b,&d_mat,CV_SVD); d0[0]=cvGetReal1D(&d_mat,0); d0[1]=cvGetReal1D(&d_mat,1); d[0]=d[0]+d0[0]*pow(2,res); d[1]=d[1]+d0[1]*pow(2,res); xn=x+d[0]; yn=y+d[1]; } else { d0[0]=0; d0[1]=0; } } if(res==1) { Ix_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Ix_1,Ix_sub,pt_i); Iy_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Iy_1,Iy_sub,pt_i); img_curr_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_prev_1,img_curr_sub,pt_i); img_prev_sub=cvCreateImage

PAGE 126

115 (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_curr_1,img_prev_sub,pt_f); It_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvSub(img_curr_sub,img_prev_sub,It_sub); //calculateGandb Ixx_sub=cvDotProduct(Ix_sub,Ix_sub); Iyy_sub=cvDotProduct(Iy_sub,Iy_sub); Ixy_sub=cvDotProduct(Ix_sub,Iy_sub); Ixt_sub=cvDotProduct(Ix_sub,It_sub); Iyt_sub=cvDotProduct(Iy_sub,It_sub); temp_G[0]=Ixx_sub; temp_G[1]=Ixy_sub; temp_G[2]=Ixy_sub; temp_G[3]=Iyy_sub; temp_b[0]=Ixt_sub; temp_b[1]=Iyt_sub; cvInitMatHeader(&G,2,2,CV_64FC1, temp_G); cvInitMatHeader(&b,2,1,CV_64FC1, temp_b); //solveforcoursedisplacement if((cvDet(&G) 1) || (cvDet(&G) 1)) { d0[0]=0; d0[1]=0;

PAGE 127

116 cvInitMatHeader(&d_mat,2,1, CV_64FC1,d0); cvSolve(&G,&b,&d_mat,CV_SVD); d0[0]=cvGetReal1D(&d_mat,0); d0[1]=cvGetReal1D(&d_mat,1); d[0]=d[0]+d0[0]*pow(2,res); d[1]=d[1]+d0[1]*pow(2,res); xn=x+d[0]; yn=y+d[1]; } else { d0[0]=0; d0[1]=0; } } if(res==2) { Ix_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Ix_2,Ix_sub,pt_i); Iy_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Iy_2,Iy_sub,pt_i); img_curr_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix

PAGE 128

117 (img_prev_2,img_curr_sub,pt_i); img_prev_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_curr_2,img_prev_sub,pt_f); It_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvSub(img_curr_sub,img_prev_sub,It_sub); //calculateGandb Ixx_sub=cvDotProduct(Ix_sub,Ix_sub); Iyy_sub=cvDotProduct(Iy_sub,Iy_sub); Ixy_sub=cvDotProduct(Ix_sub,Iy_sub); Ixt_sub=cvDotProduct(Ix_sub,It_sub); Iyt_sub=cvDotProduct(Iy_sub,It_sub); temp_G[0]=Ixx_sub; temp_G[1]=Ixy_sub; temp_G[2]=Ixy_sub; temp_G[3]=Iyy_sub; temp_b[0]=Ixt_sub; temp_b[1]=Iyt_sub; cvInitMatHeader(&G,2,2,CV_64FC1, temp_G); cvInitMatHeader(&b,2,1,CV_64FC1, temp_b); //solveforcoursedisplacement if((cvDet(&G) 1) || (cvDet(&G) 1)) {

PAGE 129

118 d0[0]=0; d0[1]=0; cvInitMatHeader(&d_mat,2,1, CV_64FC1,d0); cvSolve(&G,&b,&d_mat,CV_SVD); d0[0]=cvGetReal1D(&d_mat,0); d0[1]=cvGetReal1D(&d_mat,1); d[0]=d[0]+d0[0]*pow(2,res); d[1]=d[1]+d0[1]*pow(2,res); xn=x+d[0]; yn=y+d[1]; } else { d0[0]=0; d0[1]=0; } } if(res==3) { Ix_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Ix_3,Ix_sub,pt_i); Iy_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Iy_3,Iy_sub,pt_i); img_curr_sub=cvCreateImage

PAGE 130

119 (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_prev_3,img_curr_sub,pt_i); img_prev_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_curr_3,img_prev_sub,pt_f); It_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvSub(img_curr_sub,img_prev_sub,It_sub); //calculateGandb Ixx_sub=cvDotProduct(Ix_sub,Ix_sub); Iyy_sub=cvDotProduct(Iy_sub,Iy_sub); Ixy_sub=cvDotProduct(Ix_sub,Iy_sub); Ixt_sub=cvDotProduct(Ix_sub,It_sub); Iyt_sub=cvDotProduct(Iy_sub,It_sub); temp_G[0]=Ixx_sub; temp_G[1]=Ixy_sub; temp_G[2]=Ixy_sub; temp_G[3]=Iyy_sub; temp_b[0]=Ixt_sub; temp_b[1]=Iyt_sub; cvInitMatHeader(&G,2,2,CV_64FC1, temp_G); cvInitMatHeader(&b,2,1,CV_64FC1, temp_b); //solveforcoursedisplacement

PAGE 131

120 if((cvDet(&G) 1) || (cvDet(&G) 1)) { d0[0]=0; d0[1]=0; cvInitMatHeader(&d_mat,2,1, CV_64FC1,d0); cvSolve(&G,&b,&d_mat,CV_LU); d0[0]=cvGetReal1D(&d_mat,0); d0[1]=cvGetReal1D(&d_mat,1); d[0]=d[0]+d0[0]*pow(2,res); d[1]=d[1]+d0[1]*pow(2,res); xn=x+d[0]; yn=y+d[1]; } else { d0[0]=0; d0[1]=0; } } } elsecontinue; cvReleaseImage(&Ix_sub); cvReleaseImage(&Iy_sub); cvReleaseImage(&It_sub);

PAGE 132

121 cvReleaseImage(&It_sub); cvReleaseImage(&img_curr_sub); cvReleaseImage(&img_prev_sub); } xn=x+d[0]; yn=y+d[1]; //iterationstepforsub_pixelaccuraccy doublenormd=sqrt(d[0]*d[0]+d[1]*d[1]); //ifthereissomedisplacement if(normd 0) { inttest1=0; inttest2=0; while((test1==0)&&(test2 10)) { CvPoint2D32fpt_n=cvPoint2D32f(xn,yn); CvPoint2D32fpt_i=cvPoint2D32f(x,y); //calculateonlyiffeaturepointisintheimage if(( oor(xn)-bound_x-border_x =0) &&(ceil(xn)+bound_x+border_x img_proc.n_horz) &&( oor(yn)-bound_y-border_y =0) &&(ceil(yn)+bound_y+border_y img_proc.n_vert)) {

PAGE 133

122 //printf("FEATUREISINSIDETHE IMAGE \ n"); Ix_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Ix_0,Ix_sub,pt_i); Iy_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix(Iy_0,Iy_sub,pt_i); img_curr_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_prev_0,img_curr_sub,pt_i); img_prev_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvGetRectSubPix (img_curr_0,img_prev_sub,pt_n); It_sub=cvCreateImage (win_size,IPL_DEPTH_32F,1); cvSub(img_curr_sub,img_prev_sub,It_sub); //calculateGandb Ixx_sub=cvDotProduct(Ix_sub,Ix_sub); Iyy_sub=cvDotProduct(Iy_sub,Iy_sub); Ixy_sub=cvDotProduct(Ix_sub,Iy_sub); Ixt_sub=cvDotProduct(Ix_sub,It_sub); Iyt_sub=cvDotProduct(Iy_sub,It_sub); CvMatG0,b0; temp_G[0]=Ixx_sub;

PAGE 134

123 temp_G[1]=Ixy_sub; temp_G[2]=Ixy_sub; temp_G[3]=Iyy_sub; temp_b[0]=Ixt_sub; temp_b[1]=Iyt_sub; cvInitMatHeader(&G0,2,2,CV_64FC1, temp_G); cvInitMatHeader(&b0,2,1,CV_64FC1, temp_b); if((cvDet(&G0) .001) || (cvDet(&G0) -.001)) { d0[0]=0; d0[1]=0; cvInitMatHeader(&d_mat,2,1, CV_64FC1,d0); cvSolve(&G0,&b0,&d_mat,CV_SVD); d0[0]=cvGetReal1D(&d_mat,0); d0[1]=cvGetReal1D(&d_mat,1); doublenormd0=sqrt(d0[0]*d0[0]+ d0[1]*d0[1]); if(normd0 =track_tol) { d[0]=d[0]+d0[0]; d[1]=d[1]+d0[1]; test1=1; }

PAGE 135

124 elseif((normd0 =0)&& (normd0 999999)) { d[0]=d[0]+d0[0]; d[1]=d[1]+d0[1]; xn=x+d[0]; yn=y+d[1]; test2++; } else { test2=99999999; } } else { test2=100; } cvReleaseImage(&Ix_sub); cvReleaseImage(&Iy_sub); cvReleaseImage(&It_sub); cvReleaseImage(&img_curr_sub); cvReleaseImage(&img_prev_sub); } else { test2=99999999;

PAGE 136

125 } } if(test1==1) { CvMatG0,s; temp_G[0]=Ixx_sub; temp_G[1]=Ixy_sub; temp_G[2]=Ixy_sub; temp_G[3]=Iyy_sub; cvInitMatHeader(&G0,2,2,CV_64FC1,temp_G ); if((cvDet(&G0) .001) || (cvDet(&G0) -.001)) { doublesingle[2]={{0},{0}},s_min; //Singularvaluedecomposition s=cvMat(2,1,CV_64FC1,single); cvSVD(&G0,&s); s_min=cvGetReal1D(&s,1); if(s_min th_fp) { x=x+d[0]; y=y+d[1]; if((x 0)&&(x img_proc.n_horz)&& (y 0)&&(y img_proc.n_vert)) { fpts0[track_pts][0]=x; fpts0[track_pts][1]=y;

PAGE 137

126 track_pts=track_pts+1; } } } } } else { fpts0[track_pts][0]=x; fpts0[track_pts][1]=y; track_pts=track_pts+1; } sumx=sumx+x; sumy=sumy+y; } } CtrImg_CtrX=sumx/track_pts; CtrImg_CtrY=sumy/track_pts; cvReleaseImage(&Ix_0); cvReleaseImage(&Iy_0); cvReleaseImage(&It_0); cvReleaseImage(&It_sub); cvReleaseImage(&Ix_sub); cvReleaseImage(&Iy_sub); cvReleaseImage(&img_prev_sub); cvReleaseImage(&img_curr_sub); if(n_res 0)

PAGE 138

127 { cvReleaseImage(&Ix_1); cvReleaseImage(&Iy_1); cvReleaseImage(&img_curr_1); cvReleaseImage(&img_prev_1); } if(n_res 1) { cvReleaseImage(&Ix_2); cvReleaseImage(&Iy_2); cvReleaseImage(&img_curr_2); cvReleaseImage(&img_prev_2); } if(n_res 2) { cvReleaseImage(&Ix_3); cvReleaseImage(&Iy_3); cvReleaseImage(&img_curr_3); cvReleaseImage(&img_prev_3); } img_proc.tracked_pts0=track_pts; } img_proc.FLAG_TRACK_0_LOCKED=0; } //************ *************** *************** ************ ************* //PROGRAMEND //************ *************** *************** ************ *************

PAGE 139

APPENDIXC TEACHBYZOOMINGVISUALSERVOCONTROLALGORITHM //************ *************** *************** ************ ************* //PROGRAMSTART //************ *************** *************** ************ ************* //************ *************** *************** ************ ************* //Whatthisprogramdoes: //1)Homographydecompositionto ndoutrotationandtranslationmatrices //2)Teachbyzoomingvisualservocontrollercomputestherotationand translation //velocitiesfortherobotbasedonhomographydecompositionresults. //Whatistheinputtothisprogram: //PixeldataofthefeaturepointstrackedusingKLTalgorithm //Whatistheoutputofthisprogram: //Rotationandtranslationvelocitiesforthelowlevelcontroller. //************ *************** *************** ************ ************* #pragmaonce //HeaderFiles #include"stdafx.h" #include stdlib.h #include iostream.h #include strstrea.h #include cv.h #include"TBZController.h" #include gsl/gsl_linalg.h 128

PAGE 140

129 //DisableWarnings #pragmawarning(disable:4244) #pragmawarning(disable:4288) #pragmawarning(disable:4551) //GlobalVariables staticgsl_vector*gh_work; staticgsl_matrix*gh_u; staticgsl_vector*gh_s; staticgsl_matrix*gh_v; staticgsl_vector*gh_b; staticgsl_vector*gh_x; staticgsl_vector*dh_work; staticgsl_matrix*dh_u; staticgsl_vector*dh_s; staticgsl_matrix*dh_v; //De nitions #de neERROR_BOUND0.00001 //************ *************** *************** ************ ************* //TBZMainController //************ *************** *************** ************ ************* voidTBZController::TBZControlMain(doublefpts[][2],intrw,intrh,int frame_count,intOrgFeatIndex[]) { //Declarations doublethetaP; intPtTransController=1; doubleLamda_V;

PAGE 141

130 doubleLamda_W=2; doublefFunction; doublekno=1; doublekn1=1; doublekn2=1; doubleZstar_hat=1; doubleZ_hat; //CreateTBZWorkspace(MemoryAllocation) //TBZCreateFixedTBZWor kspace(frame_count); if(frame_count 2) TBZCreateTBZWorkspace(); //InitializeTBZController //TBZControllerInit(); //ReadTargetPixelCoordinates TBZTargetCoord(fpts,rw,rh,frame_count,OrgFeatIndex); //HomographyDecompositionAlgorithmtoGetRotationandTranslationInformation TBZHomography(frame_count); //TBZController m_R_Trans.MatTranspose(m_R); m_p1.Mat2Vector(1,m_pi); m_m1=m_InvCiHCalibrationMatrix*m_p1; m_p1_star.Mat2Vector(1,m_pi_star); m_m1_star=m_InvCiHCalibrationMatrix*m_p1_star; //Angle-AxisRepresentationfortherotationmatrix doubleCosAngle=0.5*(m_R.MatTrace()-1.0);

PAGE 142

131 //ErrorHandling if(CosAngle 1.0) CosAngle=1.0; elseif(CosAngle -1.0) CosAngle=-1.0; thetaP=acos(CosAngle); m_uPx=m_R-m_R_Trans; if(thetaP==0) m_uPx.MatScale(0); else m_uPx.MatScale(1/(2*sin(thetaP))); m_uP.MatAntiSkew(m_uPx); //DepthEstimation Z_hat=Zstar_hat/m_alpha.m_Value[PtTransController-1][0]; //ImageLvMatrix m_Lv.m_Value[0][0]=-1/Z_hat; m_Lv.m_Value[0][1]=0; m_Lv.m_Value[0][2]=m_m1.m_Value[0][0]/Z_hat; m_Lv.m_Value[1][0]=0; m_Lv.m_Value[1][1]=-1/Z_hat; m_Lv.m_Value[1][2]=m_m1.m_Value[1][0]/Z_hat; m_Lv.m_Value[2][0]=0; m_Lv.m_Value[2][1]=0; m_Lv.m_Value[2][2]=-1/Z_hat; m_LvTrans.MatTranspose(m_Lv); //ImageLvwwMatrix

PAGE 143

132 m_Lvw.m_Value[0][0]=m_m1.m_Value[0][0]*m_m1.m_Value[1][0]; m_Lvw.m_Value[0][1]=-1-(m_m1.m_Value[0][0]* m_m1.m_Value[0][0]); m_Lvw.m_Value[0][2]=m_m1.m_Value[1][0]; m_Lvw.m_Value[1][0]=1+(m_m1.m_Value[1][0]* m_m1.m_Value[1][0]); m_Lvw.m_Value[1][1]=-1*m_m1.m_Value[0][0]* m_m1.m_Value[1][0]; m_Lvw.m_Value[1][2]=-1*m_m1.m_Value[0][0]; m_Lvw.m_Value[2][0]=-1*m_m1.m_Value[1][0]; m_Lvw.m_Value[2][1]=m_m1.m_Value[0][0]; m_Lvw.m_Value[2][2]=0; //TranslationControllerGainComputation fFunction=(1/3.0)*(0.5*pow(m_m1.m_Value[0][0],2)+ 0.5*pow(m_m1.m_Value[1][0],2)+ 1-sqrt(pow(0.5*pow(m_m1.m_Value[0][0],2)+ 0.5*pow(m_m1.m_Value[1][0],2)+1,2)-1)); Lamda_V=kno+pow(Z_hat,2)/fFunction; //OpenLoopErrorSystem m_eW=m_uP*thetaP; m_eV=m_m1-m_m1_star; m_eV.m_Value[2][0]=log(m_alpha.m_Value[PtTransController-1][0]); m_eV.MatDisplay("OpenLoopErrorforTranslationController"); m_eW.MatDisplay("OpenLoopErrorforRotationController"); //Controlsignals(CameraLinearandAngularVelocityCommands) m_INTCALC1=m_LvTrans*m_eV; m_INTCALC2=m_INTCALC1;

PAGE 144

133 m_INTCALC1.MatScale(-1*Lamda_V); m_INTCALC2.MatScale(-1*(kn1*pow(Z_hat,2)+kn2*pow(Z_hat,2) *m_eV.VectorNorm())); m_Vc=m_INTCALC1+m_INTCALC2; m_Wc=m_eW*Lamda_W; //DisplayCameravelocities m_Vc.MatDisplay("CameraLinearVelocity"); m_Wc.MatDisplay("CameraAngularVelocity"); //Controlsignals(RobotLinearandAngularVelocityCommands) m_INTCALC6.MatAugment("row",m_Vc,m_Wc); m_Vr=m_INTCALC4*m_INTCALC6; m_Wr=m_INTCALC5*m_INTCALC6; //DisplayRobotvelocities m_Vr.MatDisplay("RobotLinearVelocity"); m_Wr.MatDisplay("RobotAngularVelocity"); //FileWrite //TBZResultFileWrite(frame_count); //ReleaseTBZWorkspace(MemoryDe-Allocation) //TBZReleaseTBZWorkspace(); } //************ *************** *************** ************ ************* //ReadTargetPixelValuesforCamerain-Hand(CiH)andFixedCamera (CF) //************ *************** *************** ************ ************* voidTBZController::TBZTargetCoord(doublefpts[][2],intrw,intrh,int frame_count,intOrgFeatIndex[])

PAGE 145

134 { if(frame_count==1&&OrgFeatIndex[3]!=999) { m_pi_star.m_Value[0][0]=fpts[OrgFeatIndex[0]][0]; m_pi_star.m_Value[1][0]=fpts[OrgFeatIndex[0]][1]; m_pi_star.m_Value[2][0]=1; m_pi_star.m_Value[0][1]=fpts[OrgFeatIndex[1]][0]; m_pi_star.m_Value[1][1]=fpts[OrgFeatIndex[1]][1]; m_pi_star.m_Value[2][1]=1; m_pi_star.m_Value[0][2]=fpts[OrgFeatIndex[2]][0]; m_pi_star.m_Value[1][2]=fpts[OrgFeatIndex[2]][1]; m_pi_star.m_Value[2][2]=1; m_pi_star.m_Value[0][3]=fpts[OrgFeatIndex[3]][0]; m_pi_star.m_Value[1][3]=fpts[OrgFeatIndex[3]][1]; m_pi_star.m_Value[2][3]=1; } m_pi.m_Value[0][0]=fpts[OrgFeatIndex[0]][0]; m_pi.m_Value[1][0]=fpts[OrgFeatIndex[0]][1]; m_pi.m_Value[2][0]=1; m_pi.m_Value[0][1]=fpts[OrgFeatIndex[1]][0]; m_pi.m_Value[1][1]=fpts[OrgFeatIndex[1]][1]; m_pi.m_Value[2][1]=1; m_pi.m_Value[0][2]=fpts[OrgFeatIndex[2]][0]; m_pi.m_Value[1][2]=fpts[OrgFeatIndex[2]][1]; m_pi.m_Value[2][2]=1; m_pi.m_Value[0][3]=fpts[OrgFeatIndex[3]][0]; m_pi.m_Value[1][3]=fpts[OrgFeatIndex[3]][1];

PAGE 146

135 m_pi.m_Value[2][3]=1; /* m_pi_star.m_Value[0][0]=195; m_pi_star.m_Value[1][0]=103; m_pi_star.m_Value[2][0]=1; m_pi_star.m_Value[0][1]=190; m_pi_star.m_Value[1][1]=120; m_pi_star.m_Value[2][1]=1; m_pi_star.m_Value[0][2]=188; m_pi_star.m_Value[1][2]=111; m_pi_star.m_Value[2][2]=1; m_pi_star.m_Value[0][3]=197; m_pi_star.m_Value[1][3]=120; m_pi_star.m_Value[2][3]=1; m_pi.m_Value[0][0]=201; m_pi.m_Value[1][0]=104; m_pi.m_Value[2][0]=1; m_pi.m_Value[0][1]=195; m_pi.m_Value[1][1]=120; m_pi.m_Value[2][1]=1; m_pi.m_Value[0][2]=195; m_pi.m_Value[1][2]=111; m_pi.m_Value[2][2]=1; m_pi.m_Value[0][3]=204; m_pi.m_Value[1][3]=120; m_pi.m_Value[2][3]=1;*/ }

PAGE 147

136 //************ *************** *************** ************ ************* //TBZHomographyDecompositionUsingSVD(GetGnandAlpha_g33) //************ *************** *************** ************ ************* intTBZController::TBZGetHomographySVD() { intnumPoints=0; numPoints=m_pi.Col(); if(m_alpha_g33.GetNumElements()!=numPoints || m_pi_star.Col()!= numPoints || m_pi_star.Row()!=3 || m_pi.Row()!=3) { cout "[getHomographySVD]:Argumentshaveincorrectdimensions." endl; cout "start" endl; cout m_alpha_g33.GetNumElements() endl; cout m_pi.Col() endl; cout m_pi_star.Col() endl; cout m_pi.Row() endl; cout m_pi.Row() endl; return-1; } for(inti=1;i =numPoints;i++) { introw=2*i-1; gsl_matrix_set(gh_u,row-1,0,m_pi_star.m_Value[0][i-1]);

PAGE 148

137 gsl_matrix_set(gh_u,row-1,1,m_pi_star.m_Value[1][i-1]); gsl_matrix_set(gh_u,row-1,2,1.0); gsl_matrix_set(gh_u,row-1,3,0.0); gsl_matrix_set(gh_u,row-1,4,0.0); gsl_matrix_set(gh_u,row-1,5,0.0); gsl_matrix_set(gh_u,row-1,6,-1.0*m_pi_star.m_Value[0][i-1]* m_pi.m_Value[0][i-1]); gsl_matrix_set(gh_u,row-1,7,-1.0*m_pi_star.m_Value[1][i-1]* m_pi.m_Value[0][i-1]); gsl_vector_set(gh_b,row-1,1.0*m_pi.m_Value[0][i-1]); row=2*i; gsl_matrix_set(gh_u,row-1,0,0.0); gsl_matrix_set(gh_u,row-1,1,0.0); gsl_matrix_set(gh_u,row-1,2,0.0); gsl_matrix_set(gh_u,row-1,3,m_pi_star.m_Value[0][i-1]); gsl_matrix_set(gh_u,row-1,4,m_pi_star.m_Value[1][i-1]); gsl_matrix_set(gh_u,row-1,5,1.0); gsl_matrix_set(gh_u,row-1,6,-1.0*m_pi_star.m_Value[0][i-1]* m_pi.m_Value[1][i-1]); gsl_matrix_set(gh_u,row-1,7,-1.0*m_pi_star.m_Value[1][i-1]* m_pi.m_Value[1][i-1]); gsl_vector_set(gh_b,row-1,1.0*m_pi.m_Value[1][i-1]); } if(gsl_linalg_SV_decomp(gh_u,gh_v,gh_s,gh_work)) return-1;

PAGE 149

138 if(gsl_linalg_SV_solve(gh_u,gh_v,gh_s,gh_b,gh_x)) return-1; //Extracteigenvectorforthesmallesteigenvalue m_Gn.m_Value[0][0]=gsl_vector_get(gh_x,0); m_Gn.m_Value[0][1]=gsl_vector_get(gh_x,1); m_Gn.m_Value[0][2]=gsl_vector_get(gh_x,2); m_Gn.m_Value[1][0]=gsl_vector_get(gh_x,3); m_Gn.m_Value[1][1]=gsl_vector_get(gh_x,4); m_Gn.m_Value[1][2]=gsl_vector_get(gh_x,5); m_Gn.m_Value[2][0]=gsl_vector_get(gh_x,6); m_Gn.m_Value[2][1]=gsl_vector_get(gh_x,7); m_Gn.m_Value[2][2]=1.0; //computealpha.G33foreachimagecorrespondance for(i=0;i numPoints;i++) m_alpha_g33.m_Value[i][0]=1.0/(m_Gn.m_Value[2][0]* m_pi_star.m_Value[0][i]+m_Gn.m_Value[2][1]*m_pi_star.m_Value[1][i] +1.0); return0; } //************ *************** *************** ************ ************* //DecomposeHomography(GetRotationandTranslation) //************ *************** *************** ************ ************* intTBZController::TBZDecomposeHomographySimple() { doublesign; doubled_dash; oatst,ct;//sin(theta)andcos(theta). oattypeto

PAGE 150

139 //avoidnumericalerrors doublescaleFactor;//thedinde compositionalg.p290faugeras // //Equations.SeeFaugerasfordescriptionofthealgorithm //R_bar=sign.U.R_dash.V^T //x_f_bar=U.x_dash; //n_star=V.n_dash; //d_star=sign.d_dash; // for(inti=0;i 3;i++) { for(intj=0;j 3;j++) { gsl_matrix_set(dh_u,i,j,m_Hn.m_Value[i][j]); } } //SVDofhomographymatrix if(gsl_linalg_SV_decomp(dh_u,dh_v,dh_s,dh_work)) return-1; for(i=0;i 3;i++) { m_D.m_Value[i][0]=gsl_vector_get(dh_s,i); for(intj=0;j 3;j++) { m_U.m_Value[i][j]=gsl_matrix_get(dh_u,i,j);

PAGE 151

140 m_V.m_Value[i][j]=gsl_matrix_get(dh_v,i,j); } } sign=m_U.MatDeterminant()*m_V.MatDeterminant(); m_TransV.MatTranspose(m_V); m_n_dash=m_TransV*m_nstarActual; //Inthisexperimenttheobjectisalwaysvisibletocamera, //hencedistancetotheobjectplaneisalwayspositive. //Thisdeterminessignofd_dash. scaleFactor=sign*m_D.m_Value[1][0]; if(scaleFactor =0) d_dash=m_D.m_Value[1][0]; else { d_dash=-1*m_D.m_Value[1][0]; scaleFactor=-1*scaleFactor; } //Basedonthesignofd_dash, ndR_dashandx_dash //NOTE:Usingerrorboundsinstead.Is0.9999=1.0001? if((fabs(m_D.m_Value[0][0]-m_D.m_Value[1][0]) ERROR_BOUND) &&(fabs(m_D.m_Value[1][0]-m_D.m_Value[2][0]) ERROR_BOUND)) { if(d_dash 0) { st=(m_D.m_Value[0][0]-m_D.m_Value[2][0])* (m_n_dash.m_Value[0][0]*m_n_dash.m_Value[2][0]) /m_D.m_Value[1][0];

PAGE 152

141 ct=(m_D.m_Value[1][0]*m_D.m_Value[1][0]+ m_D.m_Value[0][0]*m_D.m_Value[2][0]) /(m_D.m_Value[1][0]*(m_D.m_Value[0][0] +m_D.m_Value[2][0])); //setm_R_dash m_R_dash.m_Value[0][0]=ct; m_R_dash.m_Value[0][1]=0; m_R_dash.m_Value[0][2]=-1*st; m_R_dash.m_Value[1][0]=0; m_R_dash.m_Value[1][1]=1; m_R_dash.m_Value[1][2]=0; m_R_dash.m_Value[2][0]=st; m_R_dash.m_Value[2][1]=0; m_R_dash.m_Value[2][2]=ct; //setm_x_dash m_x_dash.m_Value[0][0]=(m_D.m_Value[0][0]m_D.m_Value[2][0])*m_n_dash.m_Value[0][0]; m_x_dash.m_Value[1][0]=0; m_x_dash.m_Value[2][0]=-1*(m_D.m_Value[0][0]m_D.m_Value[2][0])*m_n_dash.m_Value[2][0]; } else { st=(m_D.m_Value[0][0]+m_D.m_Value[2][0])* (m_n_dash.m_Value[0][0]*m_n_dash.m_Value[2][0]) /m_D.m_Value[1][0];

PAGE 153

142 ct=(m_D.m_Value[0][0]*m_D.m_Value[2][0]m_D.m_Value[1][0]*m_D.m_Value[1][0]) /(m_D.m_Value[1][0]*(m_D.m_Value[0][0] -m_D.m_Value[2][0])); //setm_R_dash m_R_dash.m_Value[0][0]=ct; m_R_dash.m_Value[0][1]=0; m_R_dash.m_Value[0][2]=st; m_R_dash.m_Value[1][0]=0; m_R_dash.m_Value[1][1]=-1; m_R_dash.m_Value[1][2]=0; m_R_dash.m_Value[2][0]=st; m_R_dash.m_Value[2][1]=0; m_R_dash.m_Value[2][2]=-1*ct; //setm_x_dash m_x_dash.m_Value[0][0]=(m_D.m_Value[0][0] +m_D.m_Value[2][0])*m_n_dash.m_Value[0][0]; m_x_dash.m_Value[1][0]=0; m_x_dash.m_Value[2][0]=(m_D.m_Value[0][0] +m_D.m_Value[2][0])*m_n_dash.m_Value[2][0]; } } //allequalsingularvalues(purerotation,mostly) if((fabs(m_D.m_Value[0][0]-m_D.m_Value[1][0]) =ERROR_BOUND) &&(fabs(m_D.m_Value[1][0]-m_D.m_Value[2][0]) =ERROR_BOUND)) {

PAGE 154

143 if(d_dash 0) { m_R_dash.m_Value[0][0]=1; m_R_dash.m_Value[0][1]=0; m_R_dash.m_Value[0][2]=0; m_R_dash.m_Value[1][0]=0; m_R_dash.m_Value[1][1]=1; m_R_dash.m_Value[1][2]=0; m_R_dash.m_Value[2][0]=0; m_R_dash.m_Value[2][1]=0; m_R_dash.m_Value[2][2]=1;//unitmatrix m_x_dash.m_Value[0][0]=0; m_x_dash.m_Value[1][0]=0; m_x_dash.m_Value[2][0]=0; } else { //m_R_dash=-I+2n_dash.n_dash^T m_R_dash.m_Value[0][0]=2*m_n_dash.m_Value[0][0]* m_n_dash.m_Value[0][0]-1; m_R_dash.m_Value[0][1]=2*m_n_dash.m_Value[0][0]* m_n_dash.m_Value[1][0]; m_R_dash.m_Value[0][2]=2*m_n_dash.m_Value[0][0]* m_n_dash.m_Value[2][0]; m_R_dash.m_Value[1][0]=2*m_n_dash.m_Value[0][0]* m_n_dash.m_Value[1][0];

PAGE 155

144 m_R_dash.m_Value[1][1]=2*m_n_dash.m_Value[1][0]* m_n_dash.m_Value[1][0]-1; m_R_dash.m_Value[1][2]=2*m_n_dash.m_Value[1][0]* m_n_dash.m_Value[2][0]; m_R_dash.m_Value[2][0]=2*m_n_dash.m_Value[0][0]* m_n_dash.m_Value[2][0]; m_R_dash.m_Value[2][1]=2*m_n_dash.m_Value[1][0]* m_n_dash.m_Value[2][0]; m_R_dash.m_Value[2][2]=2*m_n_dash.m_Value[2][0]* m_n_dash.m_Value[2][0]-1; m_x_dash.m_Value[0][0]=(-2*d_dash) *m_n_dash.m_Value[0][0]; m_x_dash.m_Value[1][0]=0; m_x_dash.m_Value[2][0]=(-2*d_dash) *m_n_dash.m_Value[2][0]; } } //twoequalsingularvalues(translationisnormaltotheplane) if((fabs(m_D.m_Value[0][0]-m_D.m_Value[1][0]) =ERROR_BOUND) || (fabs(m_D.m_Value[1][0]-m_D.m_Value[2][0]) =ERROR_BOUND)) { if(d_dash 0) { m_R_dash.m_Value[0][0]=1; m_R_dash.m_Value[0][1]=0; m_R_dash.m_Value[0][2]=0; m_R_dash.m_Value[1][0]=0;

PAGE 156

145 m_R_dash.m_Value[1][1]=1; m_R_dash.m_Value[1][2]=0; m_R_dash.m_Value[2][0]=0; m_R_dash.m_Value[2][1]=0; m_R_dash.m_Value[2][2]=1;//unitmatrix m_x_dash.m_Value[0][0]=(m_D.m_Value[2][0] -m_D.m_Value[0][0])*m_n_dash.m_Value[0][0]; m_x_dash.m_Value[1][0]=0; m_x_dash.m_Value[2][0]=(m_D.m_Value[2][0] -m_D.m_Value[0][0])*m_n_dash.m_Value[2][0]; } else { m_R_dash.m_Value[0][0]=-1; m_R_dash.m_Value[0][1]=0; m_R_dash.m_Value[0][2]=0; m_R_dash.m_Value[1][0]=0; m_R_dash.m_Value[1][1]=-1; m_R_dash.m_Value[1][2]=0; m_R_dash.m_Value[2][0]=0; m_R_dash.m_Value[2][1]=0; m_R_dash.m_Value[2][2]=1;//unitmatrix m_x_dash.m_Value[0][0]=(m_D.m_Value[2][0] +m_D.m_Value[0][0])*m_n_dash.m_Value[0][0]; m_x_dash.m_Value[1][0]=0; m_x_dash.m_Value[2][0]=(m_D.m_Value[2][0] +m_D.m_Value[0][0])*m_n_dash.m_Value[2][0];

PAGE 157

146 } } m_R.MatMult(m_U,m_R_dash,m_TransV); m_R=m_R*sign; m_x_h=m_U*m_x_dash; m_x_h=m_x_h*(1.0/scaleFactor); if(m_alpha_g33.GetNumElements()!=m_alpha.GetNumElements()) { cout "[decomposeHomographySimple]:Argumentshaveincorrect dimensions." endl; return-1; } for(intindex=0;index m_alpha_g33.GetNumElements();index++) m_alpha.m_Value[index][0]=m_alpha_g33.m_Value[index][0]* scaleFactor; return0; } //************ *************** *************** ************ ************* //PROGRAMEND //************ *************** *************** ************ *************

PAGE 158

APPENDIXD VISION-BASED 3 TARGETRECONSTRUCTIONALGORITHM //************ *************** *************** ************ ************* //PROGRAMSTART //************ *************** *************** ************ ************* //Whatthisprogramdoes: //1)UsesMicrosoftDirectXtocapturetheimagesfromthecamera //2)Executestheharvestingsequence //3)Generatesthecontrolcommandsfortherobot. //Whatistheinputtothisprogram: //Targetregionarea,targetdiameter,andextimatedtargetdepthfromthe cameracoordinatesystem. //Whatistheoutputofthisprogram: //Positionandorientationerrorsignalsforthevisualservocontroller. //************ *************** *************** ************ ************* //HEADERFILES //************ *************** *************** ************ ************* #pragmaonce #de neDRAWDIB_INCLUDE_STRETCHDIB #include"stdafx.h" #include"img_proc1.h" #include"img_proc1Doc.h" #include"MainFrm.h" #include"cv.h" #include"highgui.h" 147

PAGE 159

148 #include wingdi.h #include"GraphBase.h" #include"CitrusDetection.h" #include"FPTracking.h" #include"MainRobot.h" #include"TBZController.h" #include"ImagProcFunctions.h" #include"hispeed.h" #include"robresfun.h" #include"robotics.h" #include"C: \ Sid \ Common \ SERIAL \ FIG.h" #ifdef_DEBUG #de nenewDEBUG_NEW #undefTHIS_FILE staticcharTHIS_FILE[]=__FILE__; #endif //************ *************** *************** ************ ************* //GLOBALDEFINITIONS //************ *************** *************** ************ ************* #de nePI3.141593 #de nePID21.570796 #de nePID40.785398 #de neDEG2RAD0.01745329 #de neRAD2DEG57.2957795 #de neMAXREACH60.0 //************ *************** *************** ************ ************* //FUNCTIONSampleCBforCAMERA0

PAGE 160

149 //SampleCBistheframecallbackfunction.Itwillbecalledoneach //framewhenthevideoisbeingplayed. //SampleTime:thetimestampofcurrentframe //pSample:themediasamplecontainingtheimagedata // //Ifyouwanttoprocesstheimageframes,justaddyourown //processingcodeinthisfunction. //************ *************** *************** ************ ************* classCSampleGrabberCB0:publicISampleGrabberCB { public: //TheinterfaceneededbyISampleGrabberFilter //fakeoutanyCOMrefcounting STDMETHODIMP_(ULONG)AddRef(){return2;} STDMETHODIMP_(ULONG)Release(){return1;} //fakeoutanyCOMQIing STDMETHODIMPQueryInterface(REFIIDriid,void**ppv) { if(riid==IID_ISampleGrabberCB || riid==IID_IUnknown) { img_proc.FLAG_TRACK_0_LOCKED=1; *ppv=(void*)static_cast ISampleGrabberCB* (this); returnNOERROR; } returnE_NOINTERFACE; } //Theframecallbackfunctioncalledoneachframe

PAGE 161

150 //Wecanaddtheprocessingcodeinthisfunction //STDMETHODIMPSampleCB(doubleSampleTime,IMediaSample*pSample); //Anothercallbackfunction.Iamnotusingitnow. STDMETHODIMPBu erCB(doubleSampleTime,BYTE*pBu er,long Bu erLen) { returnE_NOTIMPL; } //WehavetosetthemediaTypeasinitialization,wecannotgetit //throughthepSampleGetMediaType(),itwillbeNULLifthemedia //typedidnotchange. AM_MEDIA_TYPEm_mediaType; STDMETHODIMPSampleCB(doubleSampleTime,IMediaSample *pSample) { //getcurrentmediatype VIDEOINFOHEADER*pvi=(VIDEOINFOHEADER*) m_mediaType.pbFormat; //Getdatafromrgb24image pSampleGetPointer(&pDataCF); SampleImgSize=pSampleGetSize(); H_pDataCF_IP=GlobalAlloc(GMEM_MOVEABLE,(unsigned long)SampleImgSize); pDataCF_IP=(unsignedchar*)GlobalLock(H_pDataCF_IP); memcpy(pDataCF_IP,pDataCF,(unsignedint)SampleImgSize); HarvestStep2();

PAGE 162

151 img_proc.FLAG_TRACK_0_LOCKED=0; FrameCount++; returnNOERROR; } }; CSampleGrabberCB0CB0; //************ *************** *************** ************ ************* //FUNCTIONHarvestStep1 //HarvestStep1:Step1inharvestingoperation //Allthefunctionsareinitialized //************ *************** *************** ************ ************* voidHarvestStep1() { if(OutputDebugStringStatus==true) OutputDebugString("HarvestStep1 \ n"); img_proc.proc_size=cvSize(320,240);//sizeofimagestoprocess img_proc.n_fpts=9;//desirednumberoffeature pointstotrack img_proc.n_horz=img_proc.proc_size.width;//imagewidth (pixels) img_proc.n_vert=img_proc.proc_size.height;//imageheight(pixels) img_proc.FLAG_TRACK_0_LOCKED=0;//Flag indicatingthetrackingforimagefromcamera0 img_proc.FLAG_TRACK_1_LOCKED=0;//Flag indicatingthetrackingforimagefromcamera1 img_proc.tracked_pts0;//numberoftracked featurepoints

PAGE 163

152 img_proc.tracked_pts1;//numberoftracked featurepoints img_proc.OrgFeatIndex[20]; img_proc.numTarg0; img_proc.numTarg1; //initcameratoolframematrix m_CameraToolMatrix.Xi=1; m_CameraToolMatrix.Xj=0; m_CameraToolMatrix.Xk=0; m_CameraToolMatrix.Yi=0; m_CameraToolMatrix.Yj=1; m_CameraToolMatrix.Yk=0; m_CameraToolMatrix.Zi=0; m_CameraToolMatrix.Zj=0; m_CameraToolMatrix.Zk=1; m_CameraToolMatrix.X=+0.0; m_CameraToolMatrix.Y=-2.1; m_CameraToolMatrix.Z=+3.500; //initgrippertoolframematrix m_GripperToolMatrix.Xi=1; m_GripperToolMatrix.Xj=0; m_GripperToolMatrix.Xk=0; m_GripperToolMatrix.Yi=0; m_GripperToolMatrix.Yj=1; m_GripperToolMatrix.Yk=0; m_GripperToolMatrix.Zi=0; m_GripperToolMatrix.Zj=0;

PAGE 164

153 m_GripperToolMatrix.Zk=1; m_GripperToolMatrix.X=0.0; m_GripperToolMatrix.Y=0.5; //m_GripperToolMatrix.Z=6.0; m_GripperToolMatrix.Z=3.5; //Userjog m_bEnableUserJog=false; m_UserJogFrame=TOOL; m_dUserJogVel[0]=0.0; m_dUserJogVel[1]=0.0; m_dUserJogVel[2]=0.0; m_dUserJogVel[3]=0.0; m_dUserJogVel[4]=0.0; m_dUserJogVel[5]=0.0; m_dUserJogVel[6]=0.0; //ResultsFilename char* lename="Results.txt"; //DeletePreviousVersionsoftheFile intFileCheck=remove( lename); if(FileCheck!=0) MessageBox(0,"RemoveFile:Filenotfound","FileWriteError",0); //InitializetheTBZcontrollerandloadthevariables Controller.TBZCreateFixedTBZWorkspace(1); Controller.TBZCreateTBZWorkspace(); Controller.TBZControllerInit(); Controller.TBZReleaseTBZWorkspace();

PAGE 165

154 //Intializeimageprocessing CitrusDetect.Init(); //InitializeMainRobot Robot.Init(); } //************ *************** *************** ************ ************* //FUNCTIONHarvestStep2forCamera0 //HarvestStep2:Step2inharvestingoperation //OrangedetectionanddepthestimationforCamera0 //************ *************** *************** ************ ************* voidHarvestStep2() { inti; doubleR2,R4,R6,K; doubleold[2],delta[2]; if(OutputDebugStringStatus==true) OutputDebugString("HarvestStep2 \ n"); pDataCF_US=(unsignedchar*)pDataCF; img_proc.IPFlag=0; img_proc.numTarg0=CitrusDetect.CFImgProc(CENTROID,pDataCF_US,img_proc.IPFlag); m_CFHarvestFruitPixCoo rd.CreateMemory(3,1); m_CFHarvestFruitNormCoord.CreateMemory(3,1); m_CFHarvestFruit3DCoord.CreateMemory(3,1); m_CFHarvestFruitNormCoordInitGuess.CreateMemory(3,1);

PAGE 166

155 m_CFHarvestFruitPixCoord.m_Value[0][0]=CitrusDetect.m_IP. m_CentCtrsCF[CitrusDetect.m_IP.m_HarvestFruit.RegionNumCF].Xp[0]; m_CFHarvestFruitPixCoord.m_Value[1][0]=CitrusDetect.m_IP. m_CentCtrsCF[CitrusDetect.m_IP.m_HarvestFruit.RegionNumCF].Xp[1]; m_CFHarvestFruitPixCoord.m_Value[2][0]=1; //CalculatetheNormalizedeuclideancoordinatesofthetargetfruit m_CFHarvestFruitNormCoord=Controller.m_InvCFCalibrationMatrix *m_CFHarvestFruitPixCoord; m_CFHarvestFruitNormCoordInitGuess=m_CFHarvestFruitNormCoord; img_proc.m_CFHarvestFruitRadius= CitrusDetect.m_IP.m_HarvestFruit.RadiusCF; for(i=0;i 20;i++) { old[0]=m_CFHarvestFruitNormCoord.m_Value[0][0]; old[1]=m_CFHarvestFruitNormCoord.m_Value[1][0]; R2=pow(m_CFHarvestFruitNormCoord.m_Value[0][0],2)+ pow(m_CFHarvestFruitNormCoord.m_Value[1][0],2); R4=R2*R2; R6=R2*R4; K=1/(1+Controller.m_CFDistortion[0]*R2 +Controller.m_CFDistortion[1]*R4 +Controller.m_CFDistortion[4]*R6); //X=Xd-dX m_CFHarvestFruitNormCoord.m_Value[0][0] =m_CFHarvestFruitNormCoordInitGuess.m_Value[0][0]-(2* Controller.m_CFDistortion[2]*m_CFHarvestFruitNormCoord.m_Value[0][0]*

PAGE 167

156 m_CFHarvestFruitNormCoord.m_Value[1][0]+Controller.m_CFDistortion[3]* (R2+2*m_CFHarvestFruitNormCoord.m_Value[0][0]* m_CFHarvestFruitNormCoord.m_Value[0][0])); m_CFHarvestFruitNormCoord.m_Value[1][0] =m_CFHarvestFruitNormCoordInitGuess.m_Value[1][0]-(Controller.m_CFDistortion[2]*(R2+2*m_CFHarvestFruitNormCoord.m_Value[1][0] *m_CFHarvestFruitNormCoord.m_Value[1][0])+2*Controller.m_CFDistortion[3]*m_CFHarvestFruitNormCoord.m_Value[0][0]* m_CFHarvestFruitNormCoord.m_Value[1][0]); m_CFHarvestFruitNormCoord.m_Value[2][0]=1; //Xn=K*X m_CFHarvestFruitNormCoord=m_CFHarvestFruitNormCoord*K; delta[0]=fabs(m_CFHarvestFruitNormCoord.m_Value[0][0]old[0]); delta[1]=fabs(m_CFHarvestFruitNormCoord.m_Value[1][0]old[1]); } m_CFHarvestFruitNormCoord.m_Value[2][0]=1; CitrusDetect.m_IP.m_HarvestFruit.CFn_X =m_CFHarvestFruitNormCoord.m_Value[0][0]; CitrusDetect.m_IP.m_HarvestFruit.CFn_Y =m_CFHarvestFruitNormCoord.m_Value[1][0]; CitrusDetect.m_IP.m_HarvestFruit.CFn_Z =m_CFHarvestFruitNormCoord.m_Value[2][0];

PAGE 168

157 //Calculatethethreedimensionalpositionandorientationofthetarget fruitintheEuclideanspace m_CFHarvestFruit3DCoord=m_CFHarvestFruitNormCoord*CitrusDetect.m_IP.m_HarvestFruit.DepthCF; CitrusDetect.m_IP.m_HarvestFruit.CFe_X =-m_CFHarvestFruit3DCoord.m_Value[0][0]; CitrusDetect.m_IP.m_HarvestFruit.CFe_Y =-m_CFHarvestFruit3DCoord.m_Value[1][0]; CitrusDetect.m_IP.m_HarvestFruit.CFe_Z =m_CFHarvestFruit3DCoord.m_Value[2][0]; //Calcthetargetspositionintherobotsbaseframe //Conversionbetweencameraslefthandedcoordinatesystemtorobot basejointsrighthandedcoordinatesystem CitrusDetect.m_IP.m_HarvestFruit.CFb_X =CitrusDetect.m_IP.m_HarvestFruit.CFe_Y-254.00; CitrusDetect.m_IP.m_HarvestFruit.CFb_Y =CitrusDetect.m_IP.m_HarvestFruit.CFe_Z+196.85; CitrusDetect.m_IP.m_HarvestFruit.CFb_Z =-CitrusDetect.m_IP.m_HarvestFruit.CFe_X-381.00; HarvestStep3(); } //************ *************** *************** ************ ************* //FUNCTIONHarvestStep3 //HarvestStep3:AllignrobotalongtheaxisjoiningFixedcameracoordinate systemandcentroid //ofthetargetfruit //************ *************** *************** ************ *************

PAGE 169

158 voidHarvestStep3() { doublelinVel[]={15.0,15.0,15.0}; doublerotVel[]={PI,PI,PI}; doublevel[]={15.0,PID2,0.0}; doublefVel[]={0.0,0.0,0.0}; doublealpha,beta; if(OutputDebugStringStatus==true) OutputDebugString("HarvestStep3 \ n"); m_RotationMatX.CreateMemory(3,3); m_RotationMatY.CreateMemory(3,3); RequestToolFeedback(); //Positionoffruitwithrespecttotoolframeafteracountingforthe orientationoftool CitrusDetect.m_IP.m_HarvestFruit.CFt_X =m_laInvRobotFeedbackRot.m_Value[0][0] *CitrusDetect.m_IP.m_HarvestFruit.CFt_X +m_laInvRobotFeedbackRot.m_Value[0][1] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Y +m_laInvRobotFeedbackRot.m_Value[0][2] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Z; CitrusDetect.m_IP.m_HarvestFruit.CFt_Y= m_laInvRobotFeedbackRot.m_Value[1][0] *CitrusDetect.m_IP.m_HarvestFruit.CFt_X +m_laInvRobotFeedbackRot.m_Value[1][1] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Y +m_laInvRobotFeedbackRot.m_Value[1][2]

PAGE 170

159 *CitrusDetect.m_IP.m_HarvestFruit.CFt_Z; CitrusDetect.m_IP.m_HarvestFruit.CFt_Z= m_laInvRobotFeedbackRot.m_Value[2][0] *CitrusDetect.m_IP.m_HarvestFruit.CFt_X +m_laInvRobotFeedbackRot.m_Value[2][1] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Y +m_laInvRobotFeedbackRot.m_Value[2][2] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Z; if(xRotationDone==false || yRotationDone==false) { //RotationaboutX-axistogetthedesiredorientation alpha=atan2(CitrusDetect.m_IP.m_HarvestFruit.CFt_Y,CitrusDetect.m_IP.m_HarvestFruit.CFt_Z);//RotationaboutX-axis m_RotationMatX.m_Value[0][0]=1; m_RotationMatX.m_Value[0][1]=0; m_RotationMatX.m_Value[0][2]=0; m_RotationMatX.m_Value[1][0]=0; m_RotationMatX.m_Value[1][1]=cos(alpha); m_RotationMatX.m_Value[1][2]=sin(alpha); m_RotationMatX.m_Value[2][0]=0; m_RotationMatX.m_Value[2][1]=-sin(alpha); m_RotationMatX.m_Value[2][2]=cos(alpha); m_RotationMatX=m_laRobotFeedbackRot*m_RotationMatX; //Xt m_DesiredMatrix.Xi=m_RotationMatX.m_Value[0][0]; m_DesiredMatrix.Xj=m_RotationMatX.m_Value[1][0]; m_DesiredMatrix.Xk=m_RotationMatX.m_Value[2][0];

PAGE 171

160 //Yt m_DesiredMatrix.Yi=m_RotationMatX.m_Value[0][1]; m_DesiredMatrix.Yj=m_RotationMatX.m_Value[1][1]; m_DesiredMatrix.Yk=m_RotationMatX.m_Value[2][1]; //Zt m_DesiredMatrix.Zi=m_RotationMatX.m_Value[0][2]; m_DesiredMatrix.Zj=m_RotationMatX.m_Value[1][2]; m_DesiredMatrix.Zk=m_RotationMatX.m_Value[2][2]; //DesiredX,Y,andZcoordinates m_DesiredMatrix.X=m_laRobotFeedbackPos.m_Value[0][0]; m_DesiredMatrix.Y=m_laRobotFeedbackPos.m_Value[1][0]; m_DesiredMatrix.Z=m_laRobotFeedbackPos.m_Value[2][0]; //Commandtomovetherobottothesetpointpositionand orientation if(RobotMotionControlOn==true) if(Robot.ServoToSetpoint(BASE,&m_DesiredMatrix,linVel, rotVel)) xRotationDone=true; } RequestToolFeedback(); //Positionoffruitwithrespecttotoolframeafteracountingforthe orientationoftool CitrusDetect.m_IP.m_HarvestFruit.CFt_X =m_laInvRobotFeedbackRot.m_Value[0][0] *CitrusDetect.m_IP.m_HarvestFruit.CFt_X +m_laInvRobotFeedbackRot.m_Value[0][1] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Y

PAGE 172

161 +m_laInvRobotFeedbackRot.m_Value[0][2] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Z; CitrusDetect.m_IP.m_HarvestFruit.CFt_Y =m_laInvRobotFeedbackRot.m_Value[1][0] *CitrusDetect.m_IP.m_HarvestFruit.CFt_X +m_laInvRobotFeedbackRot.m_Value[1][1] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Y +m_laInvRobotFeedbackRot.m_Value[1][2] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Z; CitrusDetect.m_IP.m_HarvestFruit.CFt_Z =m_laInvRobotFeedbackRot.m_Value[2][0] *CitrusDetect.m_IP.m_HarvestFruit.CFt_X +m_laInvRobotFeedbackRot.m_Value[2][1] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Y +m_laInvRobotFeedbackRot.m_Value[2][2] *CitrusDetect.m_IP.m_HarvestFruit.CFt_Z; if(xRotationDone==false || yRotationDone==false) { //RotationaboutY-axistogetthedesiredorientation beta=-atan2(CitrusDetect.m_IP.m_HarvestFruit.CFt_X,CitrusDetect.m_IP.m_HarvestFruit.CFt_Z);//RotationaboutY-axis m_RotationMatY.m_Value[0][0]=cos(beta); m_RotationMatY.m_Value[0][1]=0; m_RotationMatY.m_Value[0][2]=-sin(beta); m_RotationMatY.m_Value[1][0]=0; m_RotationMatY.m_Value[1][1]=1; m_RotationMatY.m_Value[1][2]=0;

PAGE 173

162 m_RotationMatY.m_Value[2][0]=sin(beta); m_RotationMatY.m_Value[2][1]=0; m_RotationMatY.m_Value[2][2]=cos(beta); m_RotationMatY=m_laRobotFeedbackRot*m_RotationMatY; //Xt m_DesiredMatrix.Xi=m_RotationMatY.m_Value[0][0]; m_DesiredMatrix.Xj=m_RotationMatY.m_Value[1][0]; m_DesiredMatrix.Xk=m_RotationMatY.m_Value[2][0]; //Yt m_DesiredMatrix.Yi=m_RotationMatY.m_Value[0][1]; m_DesiredMatrix.Yj=m_RotationMatY.m_Value[1][1]; m_DesiredMatrix.Yk=m_RotationMatY.m_Value[2][1]; //Zt m_DesiredMatrix.Zi=m_RotationMatY.m_Value[0][2]; m_DesiredMatrix.Zj=m_RotationMatY.m_Value[1][2]; m_DesiredMatrix.Zk=m_RotationMatY.m_Value[2][2]; //DesiredX,Y,andZcoordinates m_DesiredMatrix.X=m_laRobotFeedbackPos.m_Value[0][0]; m_DesiredMatrix.Y=m_laRobotFeedbackPos.m_Value[1][0]; m_DesiredMatrix.Z=m_laRobotFeedbackPos.m_Value[2][0]; //Commandtomovetherobottothesetpointpositionand orientation if(RobotMotionControlOn==true) if(Robot.ServoToSetpoint(BASE,&m_DesiredMatrix,linVel, rotVel)) yRotationDone=true;

PAGE 174

163 } m_laRobotFeedbackRot.DeleteMemory(); m_laRobotFeedbackPos.DeleteMemory(); m_laInvRobotFeedbackRot.DeleteMemory(); m_RotationMatX.DeleteMemory(); m_RotationMatY.DeleteMemory(); m_bEndE ectorExtension.DeleteMemory(); m_tEndE ectorExtension.DeleteMemory(); if(xRotationDone==true&&yRotationDone==true) HarvestStep4(); } //************ *************** *************** ************ ************* //FUNCTIONHarvestStep4 //HarvestStep4:Step4inharvestingoperation //Aligncentroidofthefruittotheimagecentre //Approachthefruit //************ *************** *************** ************ ************* voidHarvestStep4() { if(OutputDebugStringStatus==true) OutputDebugString("HarvestStep4 \ n"); doublelinVel[]={15.0,15.0,15.0}; doublerotVel[]={PID2,PID2,PID2}; doublevel[]={15.0,PID2,0.0}; doublefVel[]={0.0,0.0,0.0}; if(maxCiHFruitRadius CitrusDetect.m_IP.m_HarvestFruit.RadiusCiH) zPositionSet=false;

PAGE 175

164 if(zPositionSet==false) { zOldFdbkPosition=m_DesiredMatrix.Z; zDesiredPosition=m_CiHHarvestFruit3DCoord.m_Value[2][0]; maxCiHFruitRadius=CitrusDetect.m_IP.m_HarvestFruit.RadiusCiH; zPositionSet=true; } RequestToolFeedback(); m_CentroidAlignmentError[0]=Con troller.m_CiHCalibrationMatrix .m_Value[0][2]-CitrusDetect.m_IP.m_CentCtrsCiH [CitrusDetect.m_IP.m_HarvestFruit.RegionNumCiH].Xp[0]; m_CentroidAlignmentError[1]=-Controller.m_CiHCalibrationMatrix .m_Value[1][2]+CitrusDetect.m_IP.m_CentCtrsCiH [CitrusDetect.m_IP.m_HarvestFruit.RegionNumCiH].Xp[1]; m_CentroidAlignmentError[2]=zDesiredPosition -fabs(m_laRobotFeedbackPos.m_Value[2][0]-zOldFdbkPosition); //Commandtomovetherobottothesetpointpositionandorientation if(RobotMotionControlOn==true&&CentroidAlign==false&& HarvestPosReached==false) if(Robot.CentroidAlignment(m_CentroidAlignmentError,linVel, rotVel)) CentroidAlign=true; m_laRobotFeedbackRot.DeleteMemory(); m_laRobotFeedbackPos.DeleteMemory(); m_laInvRobotFeedbackRot.DeleteMemory(); m_bEndE ectorExtension.DeleteMemory(); m_tEndE ectorExtension.DeleteMemory();

PAGE 176

165 GlobalFree(H_pDataCF_IP); if(m_CentroidAlignmentError[2] =457.2) { if(IRSensorBasedGuidance==true) CentroidAlign=true;//ForceCentroidAlign agtotruetoget theIRbasedguidance if(IRSensorOn==false) Beep(2000,300); IRSensorOn=true; HarvestStep5(); } } //************ *************** *************** ************ ************* //FUNCTIONHarvestStep5 //HarvestStep5:Step5inharvestingoperation //IRSensorisusedtoprovidetheaccuraterangemeasurements //Activatethegrippertopickthefruit //************ *************** *************** ************ ************* voidHarvestStep5() { if(OutputDebugStringStatus==true) OutputDebugString("HarvestStep5 \ n"); doublelinVel[]={15.0,15.0,15.0}; doublerotVel[]={PID2,PID2,PID2}; doublevel[]={15.0,PID2,0.0}; doublefVel[]={0.0,0.0,0.0}; if(m_IRS =5300&&HarvestPosReached==false)

PAGE 177

166 { FIG.mc[0]=C; FIG.mc[1]=0; FIG.lpCommWrite(FIG.mc,1); Sleep(5); FIG.mc[0]=G; FIG.mc[1]=0; FIG.lpCommWrite(FIG.mc,1); HarvestPosReached=true; if(HarvestPosReached==true) //Beep(2000,300); //Waitfor3secondsandthenopenthegripper Sleep(8000); FIG.mc[0]=W; FIG.mc[1]=0; FIG.lpCommWrite(FIG.mc,1); Sleep(5); FIG.mc[0]=G; FIG.mc[1]=0; FIG.lpCommWrite(FIG.mc,1); R2.JointSpaceTrj(&zCnfLen,pzCmdMot,arJntTar,rAccTime, rJntVelFacApp,rJntVelFacTar); } if(CentroidAlign==true&&HarvestPosReached==false&&IRSensorBasedGuidance==true) { RequestToolFeedback();

PAGE 178

167 m_IRSensorGuidanceError[0]=Con troller.m_CiHCalibrationMatrix .m_Value[0][2]-CitrusDetect.m_IP.m_CentCtrsCiH [CitrusDetect.m_IP.m_HarvestFruit.RegionNumCiH].Xp[0]; m_IRSensorGuidanceError[1]=-Controller.m_CiHCalibrationMatrix .m_Value[1][2]+CitrusDetect.m_IP.m_CentCtrsCiH [CitrusDetect.m_IP.m_HarvestFruit.RegionNumCiH].Xp[1]; m_IRSensorGuidanceError[2]=20;//Couldbeanyvaluesincewe intendtomoveinz-directiontilltheIRsensorisactivated //Commandtomovetherobottothesetpointpositionand orientation if(RobotMotionControlOn==true) if(Robot.CentroidCorrection(m_IRSensorGuidanceError,linVel, rotVel)) { CentroidCorrectionDone=true; } if(RobotMotionControlOn==true&&CentroidCorrectionDone== true) Robot.IRSensorGuidance(m_IRSensorGuidanceError,linVel, rotVel); OutputDebugString("IRGuidance \ n"); } } //************ *************** *************** ************ ************* //FUNCTIONHarvestStep6forCamera1

PAGE 179

168 //HarvestStep6:Step6inharvestingoperation //OrangedetectionandfeaturepointtrackingforCamera1 //************ *************** *************** ************ ************* voidHarvestStep6() { inti; doubleR2,R4,R6,K; doubleold1[2],delta1[2]; if(OutputDebugStringStatus==true) OutputDebugString("HarvestStep6 \ n"); pDataCiH_US=(unsignedchar*)pDataCiH; img_proc.IPFlag=1; img_proc.numTarg1=CitrusDetect.CiHImgProc(CENTROID,pDataCiH_US,img_proc.IPFlag); //DeterminetheapproximateEuclideancoordinatesofthetargetobject m_CiHHarvestFruitPixCoord.CreateMemory(3,1); m_CiHHarvestFruitNormCoord.CreateMemory(3,1); m_CiHHarvestFruit3DCoord.CreateMemory(3,1); m_CiHHarvestFruitNormCoordInitGuess.CreateMemory(3,1); m_CiHHarvestFruitPixCoord.m_Value[0][0]=CitrusDetect.m_IP .m_CentCtrsCiH[CitrusDetect.m_IP.m_HarvestFruit.RegionNumCiH] .Xp[0]; m_CiHHarvestFruitPixCoord.m_Value[1][0]=CitrusDetect.m_IP .m_CentCtrsCiH[CitrusDetect.m_IP.m_HarvestFruit.RegionNumCiH] .Xp[1]; m_CiHHarvestFruitPixCoord.m_Value[2][0]=1;

PAGE 180

169 m_CiHHarvestFruitNormCoord=Controller.m_InvCiHCalibrationMatrix *m_CiHHarvestFruitPixCoord; m_CiHHarvestFruitNormCoordInitGuess =m_CiHHarvestFruitNormCoord; img_proc.m_CiHHarvestFruitRadius=CitrusDetect.m_IP .m_HarvestFruit.RadiusCiH; for(i=0;i 20;i++) { old1[0]=m_CiHHarvestFruitNormCoord.m_Value[0][0]; old1[1]=m_CiHHarvestFruitNormCoord.m_Value[1][0]; R2=pow(m_CiHHarvestFruitNormCoord.m_Value[0][0],2)+ pow(m_CiHHarvestFruitNormCoord.m_Value[1][0],2); R4=R2*R2; R6=R2*R4; K=1/(1+Controller.m_CiHDistortion[0]*R2 +Controller.m_CiHDistortion[1]*R4 +Controller.m_CiHDistortion[4]*R6); //X=Xd-dX m_CiHHarvestFruitNormCoord.m_Value[0][0] =m_CiHHarvestFruitNormCoordInitGuess.m_Value[0][0] -(2*Controller.m_CiHDistortion[ 2]*m_CiHHarvestFruitNormCoord .m_Value[0][0]*m_CiHHarvestFruitNormCoord.m_Value[1][0] +Controller.m_CiHDistortion[3]*(R2+2* m_CiHHarvestFruitNormCoord.m_Value[0][0]* m_CiHHarvestFruitNormCoord.m_Value[0][0])); m_CiHHarvestFruitNormCoord.m_Value[1][0] =m_CiHHarvestFruitNormCoordInitGuess.m_Value[1][0]

PAGE 181

170 -(Controller.m_CiHDistortion[2]*(R2+2* m_CiHHarvestFruitNormCoord.m_Value[1][0]* m_CiHHarvestFruitNormCoord.m_Value[1][0])+ 2*Controller.m_CiHDistortion[3]* m_CiHHarvestFruitNormCoord.m_Value[0][0]* m_CiHHarvestFruitNormCoord.m_Value[1][0]); m_CiHHarvestFruitNormCoord.m_Value[2][0]=1; //Xn=K*X m_CiHHarvestFruitNormCoord=m_CiHHarvestFruitNormCoord*K; delta1[0]=fabs(m_CiHHarvestFruitNormCoord.m_Value[0][0]old1[0]); delta1[1]=fabs(m_CiHHarvestFruitNormCoord.m_Value[1][0]old1[1]); } m_CiHHarvestFruitNormCoord.m_Value[2][0]=1; CitrusDetect.m_IP.m_HarvestFruit.CiHn_X =m_CiHHarvestFruitNormCoord.m_Value[0][0]; CitrusDetect.m_IP.m_HarvestFruit.CiHn_Y =m_CiHHarvestFruitNormCoord.m_Value[1][0]; CitrusDetect.m_IP.m_HarvestFruit.CiHn_Z =m_CiHHarvestFruitNormCoord.m_Value[2][0]; m_CiHHarvestFruit3DCoord=m_CiHHarvestFruitNormCoord* CitrusDetect.m_IP.m_RegionsCiH[CitrusDetect.m_IP .m_HarvestFruit.RegionNumCiH].depth; CitrusDetect.m_IP.m_HarvestFruit.CiHe_X =m_CiHHarvestFruit3DCoord.m_Value[0][0];

PAGE 182

171 CitrusDetect.m_IP.m_HarvestFruit.CiHe_Y =m_CiHHarvestFruit3DCoord.m_Value[1][0]; CitrusDetect.m_IP.m_HarvestFruit.CiHe_Z =m_CiHHarvestFruit3DCoord.m_Value[2][0]; DisplayData(); Sleep(100); GlobalFree(H_pDataCiH_IP); } //************ *************** *************** ************ ************* //FUNCTIONRequestToolFeedback //Requesttoolpositionandorientationfeedbackwithrespecttothebase frame //Accountfortheende ectorsizing //Computethepositionofthefruitwithrespecttothetoolframe //************ *************** *************** ************ ************* voidRequestToolFeedback() { //Getfeedback C3.GetFeedback(); //Creatememoryforlinearalgebravariables m_laRobotFeedbackRot.CreateMemory(3,3); m_laRobotFeedbackPos.CreateMemory(3,1); m_bEndE ectorExtension.CreateMemory(3,1); m_tEndE ectorExtension.CreateMemory(3,1); m_laInvRobotFeedbackRot.CreateMemory(3,3); //Rotation/orientationfeedback(Toolorientationexpressedwithrespect tothebaseframe)

PAGE 183

172 m_laRobotFeedbackRot.m_Value[0][0]=zCSFdk.rXi; m_laRobotFeedbackRot.m_Value[0][1]=zCSFdk.rYi; m_laRobotFeedbackRot.m_Value[0][2]=zCSFdk.rZi; m_laRobotFeedbackRot.m_Value[1][0]=zCSFdk.rXj; m_laRobotFeedbackRot.m_Value[1][1]=zCSFdk.rYj; m_laRobotFeedbackRot.m_Value[1][2]=zCSFdk.rZj; m_laRobotFeedbackRot.m_Value[2][0]=zCSFdk.rXk; m_laRobotFeedbackRot.m_Value[2][1]=zCSFdk.rYk; m_laRobotFeedbackRot.m_Value[2][2]=zCSFdk.rZk; //Feedbackrotationmattrixinverse m_laInvRobotFeedbackRot=m_laRobotFeedbackRot; m_laInvRobotFeedbackRot=m_laInvRobotFeedbackRot^-1; //Positionfeedback(Toolpositionexpressedinbaseframe) m_laRobotFeedbackPos.m_Value[0][0]=zCSFdk.rX; m_laRobotFeedbackPos.m_Value[1][0]=zCSFdk.rY; m_laRobotFeedbackPos.m_Value[2][0]=zCSFdk.rZ; //Multiplythepositions(x,y,and,z)by25.4toconvertintomm m_laRobotFeedbackPos.m_Value[0][0]=25.4 *m_laRobotFeedbackPos.m_Value[0][0]; m_laRobotFeedbackPos.m_Value[1][0]=25.4 *m_laRobotFeedbackPos.m_Value[1][0]; m_laRobotFeedbackPos.m_Value[2][0]=25.4 *m_laRobotFeedbackPos.m_Value[2][0]; //Positioncorrectiontoacounttheextensionoftheende ector m_tEndE ectorExtension.m_Value[0][0]=0; m_tEndE ectorExtension.m_Value[1][0]=0;

PAGE 184

173 m_tEndE ectorExtension.m_Value[2][0]=190.5;//Lengthoftheend e ector m_bEndE ectorExtension=m_laRobotFeedbackRot *m_tEndE ectorExtension; m_laRobotFeedbackPos=m_laRobotFeedbackPos +m_bEndE ectorExtension; m_RobotFeedback[0]=m_laRobotFeedbackPos.m_Value[0][0]; m_RobotFeedback[1]=m_laRobotFeedbackPos.m_Value[1][0]; m_RobotFeedback[2]=m_laRobotFeedbackPos.m_Value[2][0]; //Positionoffruitwithrespecttotoolframe(Orientationoftoolis assumedtobethesameasbaseorientation) CitrusDetect.m_IP.m_HarvestFruit.CFt_X=CitrusDetect.m_IP .m_HarvestFruit.CFb_X-m_laRobotFeedbackPos.m_Value[0][0]; CitrusDetect.m_IP.m_HarvestFruit.CFt_Y=CitrusDetect.m_IP .m_HarvestFruit.CFb_Y-m_laRobotFeedbackPos.m_Value[1][0]; CitrusDetect.m_IP.m_HarvestFruit.CFt_Z=CitrusDetect.m_IP .m_HarvestFruit.CFb_Z-m_laRobotFeedbackPos.m_Value[2][0]; } //************ *************** *************** ************ ************* //PROGRAMEND //************ *************** *************** ************ *************

PAGE 185

REFERENCES [1]J.Y.Bouguet,PyramidalImplementationoftheLucasKanadeFeature TrackerDescriptionoftheAlgorithm,OpenCVDocumentation,MicroprocessorResearchLabs,IntelCorporation,2000. [2]D.M.Bulanon,T.Kataoka,H.Okamoto,S.Hata,Determiningthe3-D LocationoftheAppleFruitDuringHarvest, AutomationTechnologyfor O -RoadEquipment ,Kyoto,Japan,October2004,pp.701P1004. [3]R.Ceres,F.L.Pons,A.R.Jimenez,F.M.Martin,andL.Calderon,Design andimplementationofanaidedfruit-harvestingrobot(Agribot), Industrial Robot ,Volume25,Number5,pp.337-346,1998. [4]P.I.Corke,VisualControlofRobotManipulators-AReview, Visual Servoing:RealTimeControlofRobotManipulatorsBasedonVisualSensoryFeedback ,K.Hashimoto(ed.), WorldScienti cSeriesinRoboticsand AutomatedSystems ,Vol.7,WorldScienti cPress,Singapore,1993. [5]W.E.Dixon,TeachbyZooming:ACameraIndependentAlternativeto TeachByShowingVisualServoControl, ProceedingsoftheIEEEInternationalConferenceonIntelligentRobotsandSystems ,LasVegas,Nevada, October2003,pp.749-754. [6]W.E.DixonandL.J.Love,Lyapunov-basedVisualServoControlfor RoboticDeactivationandDecommissioning, Proceedingsofthe9thBiennial ANSInternationalSpectrumConference ,Reno,Nevada,August2002. [7]W.E.Dixon,E.Zergeroglu,Y.Fang,andD.M.Dawson,ObjectTrackingby aRobotManipulator:ARobustCooperativeVisualServoingApproach, ProceedingsoftheIEEEInternationalConferenceonRoboticsandAutomation Washington,DC,May2002,pp.211-216. [8]Y.Fang,A.Behal,W.E.DixonandD.M.Dawson,Adaptive2.5DVisual ServoingofKinematicallyRedundantRobotManipulators, Proceedingsofthe IEEEConferenceonDecisionandControl, LasVegas,Nevada,Dec.2002 pp. 2860-2865. [9]Y.Fang,W.E.Dixon,D.M.Dawson,andJ.Chen,AnExponentialClassof Model-FreeVisualServoingControllersinthePresenceofUncertainCamera Calibration, ProceedingsoftheIEEEConferenceonDecisionandControl Maui,Hawaii,Dec.20 03,pp.5390-5395. 174

PAGE 186

175 [10]O.FaugerasandF.Lustman,MotionandStructureFromMotionina PiecewisePlanarEnvironment, InternationalJournalofPatternRecognition andArti cialIntelligence, Vol.2,No.3,pp.485-508,1988. [11]O.D.Faugeras,F.Lustaman,andG.Toscani,MotionandStructureFrom PointandLineMatches, ProceedingsoftheInternationalConferenceon ComputerVision ,London,England,June1987,pp.25-33. [12]O.FaugerasandQ.-T.Luong, TheGeometryofMultipleImages, MITPress, 2001. [13]G.Flandin,F.Chaumette,andE.Ma rchand,Eye-in-h and/Eye-to-hand CooperationforVisualServoing, ProceedingsoftheInternationalConference onRoboticsandAutomation ,SanFrancisco,CA,April2000,pp.2741-2746. [14]A.GrandDEsnon,G.Rabatel,R.Pellenc,A.Journeau,andM.J.Aldon, MAGALI:ASelf-PropelledRobottoPickApples, AmericanSocietyof AgriculturalEngineers ,Vol.46,No.3,pp.353-358,June1987. [15]S.Gupta,Lyapunov-BasedRangeAndMotionIdenti cationForA neAnd Non-A ne3dVisionSystems MastersThesis,2006. [16]R.I.Hartley,InDefenseoftheEight-PointAlgorithm, IEEETransactions onPatternAnalysisandMachineIntelligence ,Vol.19,No.6,pp.580-593, 1997. [17]R.HartleyandA.Zisserman, MultipleViewGeometryinComputerVision NewYork,NY:CambridgeUniversityPress,2000. [18]K.Hashimoto(ed.), VisualServoing:RealTimeControlofRobotManipulatorsBasedonVisualSensoryFeedback WorldScienti cSeriesinRoboticsand AutomatedSystems ,Vol.7,WorldScienti cPress,Singapore,1993. [19]ShigehikoHayashi,KatsunobuGan no,YukitsuguIshiiandItsuoTanaka, RoboticHarvestingSystemforEggplants, JapanAgriculturalResearch Quarterly, Vol.36,No.3,pp.163-168,2002. [20]S.Hutchinson,G.D.Hager,andP.I.Corke,ATutorialOnVisualServo Control, IEEETransactionsonRoboticsandAutomation ,Vol.12,No.5,pp. 651-670,1996. [21]B.D.LucasandT.Kanade,AnIterativeImageRegistrationTechniquewith anApplicationtoStereoVision, Proceedingsofthe7thInternationalJoint ConferenceonArti cialIntelligence ,Vancouver,BC,Canada,August1981, pp.674-679. [22]E.Malis,Vision-BasedControlUsingDi erentCamerasforLearningthe ReferenceImageandforServoing, ProceedingsoftheIEEE/RSJInternational

PAGE 187

176 ConferenceonIntelligentRobotsSystems ,Hawaii,November2001,pp.14281433. [23]E.Malis,Visualservoinginvarianttochangesincameraintrinsicparameters, ProceedingsoftheInternationalConferenceonComputerVision Vancouver,Canada,July2001,pp.704-709. [24]E.MalisandF.Chaumette,21/2DVisualServoingWithRespecttoUnknownObjectsThroughANewEstimationSchemeofCameraDisplacement, InternationalJournalofComputerVision ,Vol.37,No.1,pp.79-97,2000. [25]E.MalisandF.Chaumette,TheoreticalImprovementsintheStability AnalysisofaNewClassofModel-FreeVisualServoingMethods, IEEE TransactionsonRoboticsandAutomation ,Vol.18,No.2,pp.176-186,April 2002. [26]E.Malis,F.Chaumette,andS.Bodet,1/2DVisualServoing, IEEE TransactionsonRoboticsandAutomation ,Vol.15,No.2,pp.238-250,April 1999. [27]S.Mehta,W.Dixon,T.Burks,andS.Gupta,TeachbyZoomingVisual ServoControlforanUncalibratedCameraSystem, Proceedingsofthe AmericanInstituteofAeronauticsandAstronauticsGuidanceNavigationand ControlsConference ,SanFrancisco,CA,2005,AIAA-2005-6095. [28]NoriyukiMurakami,KanjiOtsuka,KeiichiInoue,MitsuhoSugimoto,Developmentofroboticcabbageharvester(Part1)-Operationalspeedofthe designedrobot, TheJapaneseSocietyofAgriculturalMachinery ,pp.85-92, 1999. [29]G.Rabatel,A.Bourely,andF.Sevila,F.Juste,RoboticHarvestingofCitrus: State-of-ArtandDevelopmentoftheFrenchSpanishEUREKAProject, ProceedingsoftheInternationalconferenceonHarvestandPostharvest TechnologiesforFreshFruitsandVegetables ,Guanajuanto,Mexico,1995,pp. 232-239. [30]Y.Sarig,RoboticsofFruitHarv esting:AState-of-the-artReview, Journal ofAgricultureEnginnering, Res.54,265-280,1993. [31]C.TomasiandT.Kanade,DetectionandTrackingofPointFeatures, TechnicalReport ,1991. [32]Z.ZhangandA.R.Hanson,ScaledEuclidean3DReconstructionBasedon ExternallyUncalibratedCameras, IEEESymposiumonComputerVision ,pp. 37-42,1995.

PAGE 188

BIOGRAPHICALSKETCH SiddharthaMehtawasborninSangamner,IndiaonMay24,1981.Hereceived hisBachelorofEngineeringdegreeinme chanicalengineeringatGovernment CollegeofEngineeringPune,India,inMay2002. SiddharthajoinedtheUniversityofFloridainAugust2003fortheMasterof Sciencedegreeprograminmechanicalandaerospaceengineeringandagricultural andbiologicalengineering.Duringhismastersprogram,heworkedasagraduate researchassistantwithDr.ThomasBurks. ThefocusofhisresearchwasdesigningLyapunov-basednonlinearcontrollers forautonomousroboticcitrusharvesting.Thearti cialintelligenceisachievedby usingthe3Dvisionsystemsandimplementatingvisualservocontrolusingimage processingandcomputervision-basedtechniques.Currentlyheispursuinghis Ph.D.inmechanicalandaerospaceengineeringundertheguidanceofDr.Warren Dixon,specializinginvisualservocontroltechniques,vision-basedrecedinghorizon control,multi-vehiclecooperativecontrolusingdaisychainingvisualservocontrol. 177


Permanent Link: http://ufdc.ufl.edu/UFE0020600/00001

Material Information

Title: Vision-based control for autonomous robotic citrus harvesting
Physical Description: Mixed Material
Language: English
Creator: Mehta, Siddhartha Satish ( Dissertant )
Burks, Thomas F. ( Thesis advisor )
Dixon, Warren E. ( Thesis advisor )
Publisher: University of Florida
Place of Publication: Gainesville, Fla.
Publication Date: 2007
Copyright Date: 2007

Subjects

Subjects / Keywords: Agricultural and Biological Engineering thesis, M.S
Dissertations, Academic -- UF -- Agricultural and Biological Engineering
Genre: bibliography   ( marcgt )
non-fiction   ( marcgt )
theses   ( marcgt )

Notes

Abstract: Vision-based autonomous robotic citrus harvesting requires target detection and interpretation of 3-dimensional (3D) Euclidean position of the target through 2D images. The typical visual servoing problem is constructed as a 'teach by showing' (TBS) problem. The teach by showing approach is formulated as the desire to position/orient a camera based on a reference image obtained by a priori positioning the same camera in the desired location. A new strategy is required for the unstructured citrus harvesting application where the camera can not be a priori positioned to the desired position/orientation. Therefore, a 'teach by zooming' approach is proposed whereby the objective is to position/orient a camera based on a reference image obtained by another camera. For example, a fixed camera providing a global view of the tree canopy can zoom in on an object and record a desired image for a camera in-hand. A controller is designed to regulate the image features acquired by a camera in-hand to the corresponding image feature coordinates in the desired image acquired by the fixed camera. The controller is developed based on the assumption that parametric uncertainty exists in the camera calibration since precise values for these parameters are difficult to obtain in practice. Simulation results demonstrate the performance of the developed controller. However, the non-identical feature point matching between the two cameras limits the application of teach by zooming visual servo controller to the artificial targets, where the feature point information is available a priori. Therefore a new visual servo control strategy based on 3D reconstruction of the target from the 2D images is developed. The 3D reconstruction is achieved by using the statistical data, viz. the mean diameter of the citrus fruit, along with the target image size and the camera focal length to generate the 3D depth information. A controller is developed to regulate the robot end-effector to the 3D Euclidean coordinates corresponding to the centroid of the target fruit. The autonomous robotic citrus harvesting facility provides a rapid prototyping testbed for evaluating such visual servo control techniques. Experimental validation of the 3D target reconstruction-based visual servo control technique demonstrates the feasibility of the controller for an unstructured citrus harvesting application.
Subject: autonomous, nonlinear, robotics, vision
General Note: Title from title page of source document.
General Note: Document formatted into pages; contains 188 pages.
General Note: Includes vita.
Thesis: Thesis (M.S.)--University of Florida, 2007.
Bibliography: Includes bibliographical references.
General Note: Text (Electronic thesis) in PDF format.

Record Information

Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
System ID: UFE0020600:00001

Permanent Link: http://ufdc.ufl.edu/UFE0020600/00001

Material Information

Title: Vision-based control for autonomous robotic citrus harvesting
Physical Description: Mixed Material
Language: English
Creator: Mehta, Siddhartha Satish ( Dissertant )
Burks, Thomas F. ( Thesis advisor )
Dixon, Warren E. ( Thesis advisor )
Publisher: University of Florida
Place of Publication: Gainesville, Fla.
Publication Date: 2007
Copyright Date: 2007

Subjects

Subjects / Keywords: Agricultural and Biological Engineering thesis, M.S
Dissertations, Academic -- UF -- Agricultural and Biological Engineering
Genre: bibliography   ( marcgt )
non-fiction   ( marcgt )
theses   ( marcgt )

Notes

Abstract: Vision-based autonomous robotic citrus harvesting requires target detection and interpretation of 3-dimensional (3D) Euclidean position of the target through 2D images. The typical visual servoing problem is constructed as a 'teach by showing' (TBS) problem. The teach by showing approach is formulated as the desire to position/orient a camera based on a reference image obtained by a priori positioning the same camera in the desired location. A new strategy is required for the unstructured citrus harvesting application where the camera can not be a priori positioned to the desired position/orientation. Therefore, a 'teach by zooming' approach is proposed whereby the objective is to position/orient a camera based on a reference image obtained by another camera. For example, a fixed camera providing a global view of the tree canopy can zoom in on an object and record a desired image for a camera in-hand. A controller is designed to regulate the image features acquired by a camera in-hand to the corresponding image feature coordinates in the desired image acquired by the fixed camera. The controller is developed based on the assumption that parametric uncertainty exists in the camera calibration since precise values for these parameters are difficult to obtain in practice. Simulation results demonstrate the performance of the developed controller. However, the non-identical feature point matching between the two cameras limits the application of teach by zooming visual servo controller to the artificial targets, where the feature point information is available a priori. Therefore a new visual servo control strategy based on 3D reconstruction of the target from the 2D images is developed. The 3D reconstruction is achieved by using the statistical data, viz. the mean diameter of the citrus fruit, along with the target image size and the camera focal length to generate the 3D depth information. A controller is developed to regulate the robot end-effector to the 3D Euclidean coordinates corresponding to the centroid of the target fruit. The autonomous robotic citrus harvesting facility provides a rapid prototyping testbed for evaluating such visual servo control techniques. Experimental validation of the 3D target reconstruction-based visual servo control technique demonstrates the feasibility of the controller for an unstructured citrus harvesting application.
Subject: autonomous, nonlinear, robotics, vision
General Note: Title from title page of source document.
General Note: Document formatted into pages; contains 188 pages.
General Note: Includes vita.
Thesis: Thesis (M.S.)--University of Florida, 2007.
Bibliography: Includes bibliographical references.
General Note: Text (Electronic thesis) in PDF format.

Record Information

Source Institution: University of Florida
Holding Location: University of Florida
Rights Management: All rights reserved by the source institution and holding location.
System ID: UFE0020600:00001


This item has the following downloads:


Full Text










VISION-BASED CONTROL FOR AUTONOMOUS ROBOTIC CITRUS
HARVESTING
















By
SIDDHARTHA SATISH MEHTA
















A THESIS PRESENTED TO THE GRADUATE SCHOOL
OF THE UNIVERSITY OF FLORIDA IN PARTIAL FULFILLMENT
OF THE REQUIREMENTS FOR THE DEGREE OF
MASTER OF SCIENCE

UNIVERSITY OF FLORIDA


2007


































@ 2007 Siddhartha Satish Mehta

























To nor parents Satish and Sulabha, nor sister Shweta,

and nor friends and family members who constantly filled me with motivation and

.30y.















ACKNOWLEDGMENTS

I express my most sincere appreciation to my supervisory committee chair,

mentor, and friend, Dr.Thomas F. Burks. His contribution to my current and

ensuing career cannot be overemphasized. I thank him for the education, advice,

and for introducing me with the interesting field of vision-based control. Special

thanks go to Dr. Warren E. Dixon for his technical insight and encouragement. I

express my appreciation and gratitude to Dr. Wonsuk "Daniel" Lee for lending his

knowledge and support. It is a great priviledge to work with such far-thinking and

inspirational individuals. All that I have learnt and accomplished would not have

been possible without their dedication.

I especially thank Mr. Gregory Pugh and Mr. Michael Zingaro for their

invaluable guidance and support during the last semester of my research. I thank

all of my colleagues who helped during my thesis research: Sumit Gupta, Guoqiang

Hu, Dr. Samuel Flood, and Dr. Duke Bulanon.

Most importantly I would like to express my deepest appreciation to my

parents Satish Mehta and Sulabha Mehta and my sister Shweta. Their love,

understanding, patience and personal sacrifice made this dissertation possible.
















TABLE OF CONTENTS


ACKNOWLEDGMENTS . . . .

LIST OF FIGURES .. . . . ..

LIST OF TABLES . . . . .

ABSTRACT .. . . . ..

CHAPTER

1 INTRODUCTION.


page

iv

vii

ix

x


2 FUNDAMENTALS OF FEATURE
VIEW PHOTOGRAMMETRY ..

2.1 Introduction ......
2.2 Imagfe Processingf ....
2.3 Multi-view Photogrammetry .
2.4 Conclusion. ......

3 OBJECTIVES

4 METHODS AND PROCEDURES

4.1 Introduction ......


POINT TRACKING AND MULTI-


I I I ( I


Visual Servo Control
ructionn Based Visual Servo Control





4.2 Hardware Setup .
4.3 Teach by Zooming
4.4 3D Target Reconst
4.5 Conclusion ..


5 TEACH BY ZOOMING VISUAL SERVO CONTROL FOR AN UN-
CALIBRATED CAMERA SYSTEM.

5.1 Introduction ................
5.2 Model Development. .......... ..
5.3 Homography Development.
5.4 Control Objective.
5.5 Control Development.
5.5.1 Rotation Controller.
5.5.2 Translation Controller .........
5.6 Simulation Results.










5.6.1 Introduction ......
5.6.2 Numerical Results ....
5.7 Experimental Results .....
5.8 Conclusion. .. ....


6 3D TARGET RECONSTRUCTION FOR VISION-BASED ROBOT
CONTROL.


6.1 Model Development.
6.2 3D Target Reconstruction.
6.3 Control Development.
6.3.1 Control Objective.
6.3.2 Rotation Controller.
6.3.3 Translation Controller .
6.4 Experimental Results.
6.4.1 Performance Validation
6.4.2 Experiment I .
6.4.3 Experiment II.
6.4.4 Experiment III .
6.5 Conclusion .


of 3D


Depth


Estimation


7 CONCLUSION ........


7.1 Summary of Results
7.2 R~ecommendations for Future Work.


APPENDIX


A OPEN-LOOP ERROR DYNJAMICS

A.1 Rotation Controller .. ...
A.2 Translation Controller.


B TARGET IDENTIFICATION AND FEATURE POINT TRACKING
ALGORITHM

B.1 Target Detection.
B.2 Feature Point Tracking ..............

C TEACH BY ZOOMING VISUAL SERVO CONTROL ALGORITHM ..

D VISION-BASED 3D TARGET RECONSTRUCTION ALGORITHM ..

REFERENCES ...................

BIOGRAPHICAL SKETCH ...........



















Figure page

2-1 Moving camera looking at a reference plane. .. .. .. .. .. .. .. 12

4-1 Overview of vision-based autonomous citrus harvesting. .. .. .. .. 21

4-2 A Robotics Research K(-1207i articulated robotic arm. .. .. .. .. .. 22

4-3 Camera in-hand located at the center of the robot end-effector. .. .. 23

4-4 Overview of the TBZ control architecture. .. .. .. .. .. .. .. .. 27

4-5 Overview of the 3D target reconstruction-based visual servo control. .. 29

5-1 Camera frame coordinate relationships. .. .. .. .. .. .. .. .. .. 35

5-2 Teach by zooming visual servo control for a robotic manipulator. .. .. 36

5-3 Overview of teach by zooming visual servo controller. .. .. .. .. .. 42

5-4 Image from the fixed camera before zooming subscriptt 'f') and after
zooming superscriptt '*'). .................. ....... 52

5-5 Rotation Errors. ................... ............ 52

5-6 Translation Errors. ................... ........... 53

57Euclidean trajectory of the feature points viewed by the camera-in-hand
from the initial position and orientation (denoted by '+') to the desired
position and orientation Fad (denoted by 'x'), where the virtual camera
coordinate system FT* is denoted by 'o'. .. .. .. .. .. .. .. .. 53

58Angular control input velocity for the camera-in-hand. .. .. .. .. 54

59Linear control input velocity for the camera-in-hand. .. .. .. 54

5-10 Elimination of ineffectual feature points via. target identification. .. 55

5-11 Feature point matching between fixed camera and camera in-hand. .. 56

6-1 Camera frame coordinate relationships. .. .. .. .. .. .. .. .. .. 59

6-2 3D target reconstruction based visual servo control for a robotic manip-
ulator. ................... .................. 60


LIST OF FIGURES











6-3 Perspective projection geometry model for Euclidean depth identification. 62

6-4 Control architecture depicting rotation control. .. .. .. .. .. 64

6-5 Control architecture depicting translation control. .. .. .. .. .. .. 65

6-6 Performance validation of 3D depth estimation. .. .. .. .. .. .. 69

6-7 Depth estimation error. ................... ........ 70

6-8 Performance validation for repeatability test. .. .. .. .. .. .. .. 70

6-9 3D robot task-space depicting repeatability results. .. .. .. .. .. 72

6-10 Repeatability in my-plane for the 3D target reconstruction based visual
servo controller. ................... ............. 73

6-11 Repeatability in xz-plane for the 3D target reconstruction based visual
servo controller. ................... ............. 73

6-12 Performance validation for experiment II. .. .. .. .. .. .. .. .. 74

6-13 Performance validation for experiment III. .. .. .. .. .. .. .. .. 75

















LIST OF TABLES

Table page

5-1 List of variables for teach by zooming visual servo control. .. .. .. 34

6-1 List of variables for 3D target reconstruction based visual servo control. 59

6-2 Performance validation for 3D depth estimation method. .. .. .. .. 69

6-3 Actual Euclidean target position expressed in a fixed camera frame and
robot base frame. ................... ............ 71

6-4 Estimated Euclidean target position expressed in a fixed camera frame,
robot base frame, and robot tool frame. .. .. .. .. .. .. .. .. 71

6-5 Initial robot end-effector position expressed in robot base frame .. .. 71

6-6 Final robot end-effector position expressed in robot base frame. .. .. 72

6-7 Actual Euclidean target position expressed in a fixed camera frame and
robot base frame. ................... ............ 74

6-8 Initial and final robot end-effector position expressed in robot base frame. 75

6-9 Actual Euclidean target position expressed in a fixed camera frame and
robot base frame and initial and final robot end-effector position expressed
in robot base frame. ................... .......... 76















Abstract of Thesis Presented to the Graduate School
of the University of Florida in Partial Fulfillment of the
Requirements for the Degree of Master of Science

VISION-BASED CONTROL FOR AUTONOMOUS ROBOTIC CITRUS
HARVESTING

By

Siddhartha Satish Mehta

May 2007

Chair: Thomas F. Burks
Major: Agricultural and Biological Engineering

Vision-based autonomous robotic citrus harvesting requires target detection

and interpretation of 3-dimensional (3D) Euclidean position of the target through

2D images. The typical visual serving problem is constructed as a "teach by

showing" (TBS) problem. The teach by showing approach is formulated as the

desire to position/orient a camera based on a reference image obtained by a priori

positioning the same camera in the desired location. A new strategy is required for

the unstructured citrus harvesting application where the camera can not be a priori

positioned to the desired position/orientation. Therefore a "teach by zooming"

approach is proposed whereby the objective is to position/orient a camera based

on a reference image obtained by another camera. For example, a fixed camera

providing a global view of the tree canopy can zoom in on an object and record

a desired image for a camera in-hand. A controller is designed to regulate the

image features acquired by a camera in-hand to the corresponding image feature

coordinates in the desired image acquired by the fixed camera. The controller

is developed based on the assumption that parametric uncertainty exists in the

camera calibration since precise values for these parameters are difficult to obtain










in practice. Simulation results demonstrate the performance of the developed

controller.

However, the non-identical feature point matching between the two cameras

limits the application of teach by zooming visual servo controller to the artificial

targets, where the feature point information is available a priori. Therefore, a new

visual servo control strategy based on 3D reconstruction of the target from the

2D images is developed. The 3D reconstruction is achieved by using the statistical

data, viz. the mean diameter of the citrus fruit, along with the target image size

and the camera focal length to generate the 3D depth information. A controller

is developed to regulate the robot end-effector to the 3D Euclidean coordinates

corresponding to the centroid of the target fruit. The autonomous robotic citrus

harvesting facility provides a rapid prototyping testbed for evaluating such visual

servo control techniques. Experimental validation of the 3D target reconstruction-

based visual servo control technique demonstrates the feasibility of the controller

for an unstructured citrus harvesting application.















CHAPTER 1
INTRODUCTION

Mechanization of harvesting of fruits is highly desirable in developing countries

due to decrease in seasonal labor availability and increasing economic pressures.

The mechanized fruit harvesting technology has limitations to soft fruit harvesting

because of the occurrence of excessive damage to the harvest. Mechanical harvest-

ing systems are based on the principle of shaking or knocking the fruit out of the

tree. There are two basic types namely trunk shaker and canopy shake. The trunk

shaker based systems attempt to remove the fruit from the tree by simply shaking

or vibrating the tree trunk and allowing the induced vibrations and oscillations

to cause the fruit to fall out of the tree. Canopy shaker systems consist of nylon

rods that rotate and shake foliage. These nylon rods allow for better shaking of

the canopy than the trunk shakers alone. However, there are various problems

associated with these strictly mechanical harvester systems. Typically citrus has

strong attachment between the tree branch and the fruit, hence it may require

large amount of shaking for harvesting the fruit which may cause damage to the

tree, such as bark removal and broken branches. Moreover, due to mechanical

shaking system and conveyor belt collection systems, mechanical harvester systems

can cause fruit quality deterioration thus making mechanical harvesting systems

typically suitable for juice fruit quality. Since there is still a large percentage of

citrus that is sold as fresh market fruit and which cannot be damaged in any way.

An alternative to mechanical harvesting systems, yielding superior fruit quality, is

the use of automated robotic harvesting systems.

Characteristics of the automated fruit harvesting robot: (a) Able to locate

the fruits on the tree in three dimensions (3D) (b) able to approach and reach for










the fruit (c) able to detach the fruit according to a predetermined criterion, and

transfer it to a suitable container and (d) to be propelled in the orchard with ease

of maneuverability, from tree to tree and row to row, while negotiating various

terrains and topographies. In addition, a desirable capability is the versatility to

handle a variety of fruit crops with minimal changes in the general configuration

[30].

All these operations should be performed within the following constrains;

Picking rate faster than, or equal to, manual picking (by a group of workers),

Fruit quality equal to or superior to that obtained with manual picking, and

Economically justifiable.

Robots perform well in structured environment, where the Euclidean position

and orientation of target and obstacles is known. But, in presence of unstructured

urban/non-urban environments the difficulty is faced of determining the three

dimensional Euclidean position and orientation of the target and path planning

for obstacle avoidance. The traditional industrial application of robotics involve

structured environment, but robotics field is spreading to reach non-traditional

application areas such as medical robots and agricultural pre-harvesting and post-

harvesting. These non-traditional applications represent working of a robot in an

unstructured environment.

Several autonomous/semi-autonomous techniques have been proposed for

the target detection and range identification in unstructured environments. Ceres

et al. [3] developed an-aided fruit harvesting strategy where the operator drives

the robotic harvester and performs the detection of fruits by means of a laser

rangefinder; the computer performs the precise location of the fruits, computes

adequate picking sequences and controls the motion of manipulator joints. The

infrared laser range-finder used is based on the principle of phase shift between

emitted and reflected amplitude modulated laser signal. Since, the process of










detection and the localization of the fruit is directed by a human operator, this

approach heavily depends on human interface in the harvesting. D'Esnon et al. [14]

proposed an apple harvesting prototype robot- MAGALI, implementing a spherical

manipulator, mounted on a hydraulically actuated self guided vehicle, servoed by

a camera set mounted at the base rotation joint. When the camera detects a fruit,

the manipulator arm is orientated along the line of sight onto the coordinates of the

target and is moved forward till it reaches the fruit, which is sensed by a proximity

sensor. However, the use of proximity sensor could result in damaging the harvest

or manipulator. Moreover, the vision system implemented requires the camera to

be elevated along the tree to scan the tree from bottom to top. Rabatel et al. [29]

initiated a French Spanish EUREK(A project named CITRUS, which was largely

based on the development of French robotic harvester MAGALI. The CITRUS

robotic system used a principle for robot arm motion: once the fruit is detected,

the straight line between the camera optical center and the fruit can be used as

a trajectory for the picking device to reach the fruit, as this line is guaranteed to

be free of obstacles. The principle used here is the same as MAGALI, and thus

this approach also fails to determine the three dimensional (3-D) position of the

fruit in the Euclidean workspace. Bulanon et al. [2] developed the method for

determining 3-D location of the apples for robotic harvesting using camera in-hand

configuration. Bulanon et al. used the differential object size method and binocular

stereo vision method for 3-D position estimation. In differential size method, the

change in the object size as the camera moves towards the object was used to

compute the distance, whereas the binocular stereo vision method calculates the

distance of the object from the camera using two images from different viewpoints

on the triangular measurement principle. However, range identification accuracy

of these methods was not satisfactory for robotic guidance. In addition Bulanon

et al. considered laser ranging technique for accurate range identification of the










object. Murakami et al.[28] proposed a vision-based robotic cabbage harvester. The

robot consists of a three degrees of freedom (3-DOF) polar hydraulic manipulator

and a color CCD camera is used for machine vision. Murakami et al. suggested

the use of hydraulic manipulator since they have high power to weight ratio and

do not require high power supply to automate the system, which is justified for

the cabbage harvesting application. The image processing strategy is to recognize

the cabbage heads by processing the color image which is taken under unstable

lighting conditions in the field. The image processing uses neural network to

extract cabbage heads of the HIS transformed image. The location and diameter

of cabbage are estimated by correlation with the image template. Since cabbage

harvesting represents a 2-D visual servo control problem, it is possible to apply the

template matching technique for 3-D location estimation. But for applications like

citrus harvesting, which require 3-D visual servo control of robotic manipulator,

exact three dimensional Euclidean coordinates are to be determined. Hayashi et al.

studied the development of robotic harvesting system that performs recognition,

approach, and picking tasks for eggplants [19]. The robotic harvesting system

consists of a machine vision system, a manipulator unit, and an end-effector unit.

The articulated manipulator with 5-DOF with camera in-hand configuration

was selected for harvesting application. A fuzzy logic based visual servo control

techniques were adapted for manipulator guidance. The visual feedback fuzzy

control model was designed to determine the forward movement, vertical movement

and rotational angle of the manipulator based on the position of the detected fruit

in an image frame. When the area occupied by the fruit is more than 70%0 of the

total image, the serving is stopped and eggplant is picked.

With recent advances in camera technology, computer vision, and control the-

ory, visual servo control systems exhibit significant promise to enable autonomous

systems with the ability to operate in unstructured environments. Although a










vision system can provide a unique sense of perception, several technical issues

have impacted the design of robust visual servo controllers. One of these issues

includes the camera configuration (pixel resolution versus field-of-view). For ex-

ample, for vision systems that utilize a camera in a fixed configuration (i.e., the

eye-to-hand configuration), the camera is typically mounted at a large enough

distance to ensure that the desired target objects will remain in the camera's

view. Unfortunately, by mounting the camera in this configuration the task-space

area that corresponds to a pixel in the image-space can be quite large, resulting

in low resolution and noisy position measurements. Moreover, many applications

are ill-suited for the fixed camera configuration. For example, a robot may be

required to position the camera for close-up tasks (i.e., the camera-in-hand configu-

ration). For the camera-in-hand configuration, the camera is naturally close to the

workspace, providing for higher resolution measurements due to the fact that each

pixel represents a smaller task-space area; however, the field-of-view of the camera

is significantly reduced (i.e., an object may be located in the workspace but be out

of the camera's view).

Several results have been recently developed to address the camera config-

uration issues by utilizing a cooperative camera strategy. The advantages of the

cooperative camera configuration are that the fixed camera can be mounted so

that the complete workspace is visible and the camera-in-hand provides a high

resolution, close-up view of an object (e.g., facilitating the potential for more

precise motion for robotic applications). Specifically, Flandin et al. [13] made the

first steps towards cooperatively utilizing global and local information obtained

from a fixed camera and a camera-in-hand, respectively; unfortunately, to prove the

stability results, the translation and rotation tasks of the controller were treated

separately (i.e., the coupling terms were ignored) and the cameras were considered

to be calibrated. Dixon et al. [6], [7] developed a new cooperative visual serving










approach and experimentally demonstrated to use information from both an uncali-

brated fixed camera and an uncalibrated camera-in-hand to enable robust tracking

by the camera-in-hand of an object moving in the task-space with an unknown

trajectory. Unfortunately, a crucial assumption for this approach is that the camera

and the object motion be constrained to a plane so that the unknown depth from

the camera to the target remains constant. The approaches presented by Dixon

et al. [6], [7] and Flandin et al. [13] are in contrast to typical multi-camera ap-

proaches, that utilize a stereo-based configuration, since stereo-based approaches

typically do not simultaneously exploit local and global views of the robot and the

workspace.

Another issue that has impacted the development of visual servo systems is the

calibration of the camera. For example, to relate pixelized image-space information

to the task-space, exact knowledge of the camera calibration parameters is required,

and discrepancies in the calibration matrix result in an erroneous relationship.

Furthermore, an acquired image is a function of both the task-space position of

the camera and the intrinsic calibration parameters; hence, perfect knowledge

of the camera intrinsic parameters is also required to relate the relative position

of a camera through the respective images as it moves. For example, the typical

visual serving problem is constructed as a "teach by showing" (TBS) problem

in which a camera is positioned at a desired location, a reference image is taken

(where the normalized task-space location is determined via the intrinsic calibration

parameters), the camera is moved away from the reference location, and then

repositioned at the reference location under visual servo control (which requires

that the calibration parameters did not change in order to reposition the camera to

the same task-space location given the same image). See [4], [20], [18], and [23] for

a further explanation and an overview of this problem formulation.










Unfortunately, for many practical applications it may not be possible to TBS

(i.e., it may not be possible to acquire the reference image by a priori positioning

the camera-in-hand to the desired location). As stated by Malis [23], the TBS

problem formulation is "camera-dependent" due to the hypothesis that the

camera intrinsic parameters during the teaching stage, must be equal to the

intrinsic parameters during serving. Motivated by the desire to address this

problem, the basic idea of the pioneering development by Malis [22], [23] is to use

projective invariance to construct an error function that is invariant to the intrinsic

parameters, thus enabling the control objective to be met despite variations in the

intrinsic parameters. However, the fundamental idea for the problem formulation

is that an error system be constructed in an invariant space, and unfortunately, as

stated by Malis [22], [23] several control issues and a rigorous stability analysis of

the invariant space approach "have been left unresolved."

Inspired by the previous issues and the development by Dixon et al. [7]

and Malis [22], [23], the presented work utilizes a cooperative camera scheme

to formulate a visual control problem that does not rely on the TBS paradigm.

Specifically, a calibrated camera-in-hand and a calibrated fixed camera are used

to facilitate a "teach by zooming" approach to formulate a control problem

with the objective to force the camera-in-hand to the exactly known Euclidean

position/orientation of a virtual camera defined by a zoomed image from a fixed

camera (i.e., the camera-in-hand does not need to be positioned in the desired

location a priori using this alternative approach). Since the intrinsic camera

calibration parameters may be difficult to exactly determine in practice, a second

control objective is defined as the desire to servo an uncalibrated camera-in-hand

so that the respective image corresponds to the zoomed image of an uncalibrated

fixed camera. For each case, the fixed camera provides a global view of a scene that

can be zoomed to provide a close-up view of the target. The reference image is










then used to servo the camera-in-hand. That is, the proposed "teach by zooming"

approach addresses the camera configuration issues by exploiting the wide scene

view of a fixed camera and the close-up view provided by the camera-in-hand.















CHAPTER 2
FUNDAMENTALS OF FEATURE POINT TRACKING AND MULTI-VIEW
PHOTOGRAMMETRY

2.1 Introduction

This chapter provides introduction to feature point detection and tracking

techniques commonly used for vision-based control strategies along with multi-

view photogrammetry concepts to determine the relationship between various

camera frame coordinate systems. The chapter is organized in the following

manner. In Section 2.2, the image processing algorithm for real-time feature

detection and tracking is discussed. In Section 2.3, implementation of a multi-view

photogrammetry technique is illustrated, and concluding remarks are provided in

Section 2.4.

2.2 Image Processing

Many vision-based estimation and control strategies require real-time detection

and tracking of features. In order to eliminate the ineffectual feature points,

feature point detection and tracking is performed only on the target of interest.

Hence, color thresholding-based image segmentation methods are used for target

(i.e. citrus fruit) detection (see Appendix B.1). The most important segment of

the image processing algorithm (see Appendix B.2) is real-time identification of

features required for coordinated guidance, navigation and control in complex 3D

surroundings such as urban environments. The algorithm is developed in C/C++

using Visual Studio 6.0. Intel's Open Source Computer Vision Library is utilized

for real-time implementation of most of the image processing functions. Also,

GNU Scientific Library (GSL) is used, which is a numerical library for C and C++

programming. GSL provides a wide range of mathematical routines for most of the










computation involved in the algorithms. The rest of this section describes feature

point detection and K~anade-Lucas-Tomasi (K(LT) feature point tracking method.

The color space is transformed from RGB to a new color space CrCgCb having

components of R, G, and B, respectively, utilizing the following transformation:

Cr 0.721 -0.587 -0.114 R

Cg = -0.99 0.413 -0.114 GI (2-1)

Cb -0.299 -0.587 0.886 B

Color thresholding methods are then applied on the color transformed image

for target segmentation and feature point tracking is performed on the detected

target.

As stated in [31], no feature-based vision system can work unless good features

can be identified and tracked from frame to frame. Two important issues need

to be addressed; a method for selecting good and reliable feature points; and a

method to track the feature points frame to frame. These issues are discussed in

detail in [21] and [31], where the K~anade-Lucas-Tomasi (K(LT) tracker is promoted

as a solution.

The algorithm developed (see Appendix B.2) is based on the K(LT tracker that

selects features which are optimal for tracking. The basic principle of the K(LT

is that a good feature is one that can be tracked well, so tracking should not be

separated from feature extraction. As stated in [21], a good feature is a textured

patch with high intensity variation in both x and y directions, such as a corner. By

representing the intensity function in x and y directions by g, and g,, respectively,

we can define the local intensity variation matrix, Z, as



Z ~ y9 = ~ (2-2)
Z= 2










A patch defined by a 7 x 7 pixels window is accepted as a candidate feature if in

the center of the window both eigenvalues of Z, exceed a predefined threshold. Two

large eigenvalues can represent corners, salt and pepper textures, or any other pat-

tern that can be tracked reliably. Two small eigenvalues mean a roughly constant

intensity profile within a window. A large and a small eigenvalues correspond to a

unidirectional texture pattern. The intensity variations in a window are bounded

by the maximum allowable pixel value, so the greater eigenvalue cannot be arbi-

trarily large. In conclusion, if the two eigenvalues of Z are Az and A2, we accept a

window if



min(Ay, X2) > (2-3)

where A is a predefined threshold.

As described in [31], feature point tracking is a standard task of computer

vision with numerous applications in navigation, motion understanding, surveil-

lance, scene monitoring, and video database management. In an image sequence,

moving objects are represented by their feature points detected prior to tracking

or during tracking. As the scene moves, the patterns of image intensities changes

in a complex way. Denoting images by I, these changes can be described as image

motion:



I(x, y, t + 7) = I(X P((, y, t, 7), y rl(x, y, t, 7)). (2-4)

Thus a later image taken at time t + 7 (where 7 represents a small time interval)

can be obtained by moving every point in the current image, taken at time t, by a

suitable amount. The amount of motion 6 = ((, rl) is called the displacement of the

point at m = (x, y). The displacement vector 6 is a function of image position m.

Tomasi in [31], states that pure translation is the best motion model for tracking











Object Plane


Reference Location


Cinrent Location


Figure 2-1: 1\oving camera looking at a reference plane.


because it exhibits reliability and accuracy in comparing features between the

reference and current image, hence



6=d

where d is the translation of the feature's window center between successive frames.

Thus, the knowledge of translation d allows reliable and optimal tracking of the

feature points windows.

In the algorithm developed for the teach by zooming visual servo control, the

method employed for tracking is based on the pyramidal implementation of the

K(LT Tracker described in [1].

2.3 Multi-view Photogrammetry

The feature points data obtained during motion through the virtual environ-

ment is used to determine the relationship between the current and a constant

reference position as shown in Figure. 2-1. This relationship is obtained by deter-

mining the rotation and translation between corresponding feature points on the

current and reference image position. The rotation and translation components










relating corresponding points of the reference and current image is obtained by

first constructing the Euclidean homography matrix. Various techniques can then

be used (e.g., see [11], [32]) to decompose the Euclidean homography matrix into

rotational and translational components. Following section describes multiview

photogrammetry-based Euclidean reconstruction method to obtain unknown

rotation and translation vectors for camera motion.

Consider the orthogonal coordinate systems, denoted FT and FT* that are

depicted in Figure. 2-1. The coordinate system FT is attached to a moving camera.

A reference plane xT on the object is defined by four target points Oi Vi = 1, 2, 3,

4 where the three dimensional (3D) coordinates of Oi expressed in terms of FT and

FT* are defined as elements of mi (t) and my E RW3 and represented by


(2-5)


The Euclidean-space is projected onto the image-space, so the normalized coordi-

nates of the targets points mi (t) and my are defined as

mi i yi (
mi 1(
zi zy zy
m m
m*~ ~ Z i1(

under the standard assumption that zy (t) and zf > e, where a denotes an arbitral

ily small positive scalar constant.

Each target point has pixel coordinates that are acquired from the moving

camera, expressed in terms of 7T, denoted by ui (t) I (t) E RW, and are defined as

elements of pi (t) E RW3 aS follows.


pi us I 1 (2


-7)


-8)


r-


-9)


mi~t = t X i (t) zi t)









The pixel coordinates of the target points at the reference position is expressed in

terms of FT* (denoted by uf of E RW) and are defined as elements of pf E RW3 aS

f ollows:



The pixel coordinates pi (t) and pf are related by the following global invertible

transformation (i.e., the pinhole model) to the normalized task-space coordinates

mi (t) and my, respectively:


pi = Ami (2-11)




where A is the intrinsic camera calibration matrix.

The constant distance from the origin of FT* to the object plane xT along the

unit normal n* is denoted by d* ERI and is defined as


d* n n* Af (2-12)


The coordinate frames 7T and FT* depicted in Figure. 2-1 are attached to the

camera and denote the actual and reference locations of the camera. From the

geometry between the coordinate frames, mf can be related to ag (t) as follows


mi = Xf + RmJ. (2-13)


In (2-13), R (t) E SO(3) denotes the rotation between 7T and FT*, and my (t) E RW3
denotes the translation vector from FT to FT* expressed in the coordinate frame FT.

By utilizing (2-7), (2-8), and (2-12), the expressions in (2-13) can be written as
follows
myi = asi (R + .rhn* ) my1.
(2-14)









In (2-14), as (t) E RW3 denotes the following scaled translation vector

my =~ (2-15)

and ag (t) denotes the depth ratio defined as



asi = -4- (2-16)

After substituting (2-11) into (2-14), the following relationships can be developed


(2-17)


where G (t) = [gij(t)], Vi, j = 1, 2, 3 E RW3x3 denotes a projective homography

matrix. After normalizing G(t) by g33(t), which is assumed to be non-zero without

loss of generality, the projective relationship in (2-17) can be expressed as follows:



pi = ai933 '!-'* (2-18)

where Gn E RW3x3 denotes the normalized projective homography. From (2-18), a
se o 1 lnerl idpedet qutinsgie by,,,~, the+:,,:, 4, targe pon pairs (p;,,: pi* (t)

with 3 independent equations per target pair can be used to determine G(t) and

ai(t)g33(t). Based on the fact that intrinsic camera calibration matrix A is assumed
to be known, (2-17) and (2-18) can be used to determine g33(t)H(t). Various

techniques can then be used (e.g., see [11, 32]) to decompose the product g33(t)H(t)

into rotational and translational components. Specifically, the scale factor g33()

the rotation vector R(t), the unit normal vector n*, and the scaled translation

vector denoted by za (t) can all be computed from the decomposition of the product

g33(t)H(t). Since the product ai(t)g33(t) can be computed from (2-18), and g33 t)
can be determined through the decomposition of the product g33(t)H(t), the depth

ratio ag (t) can also be computed.






16


2.4 Conclusion

In this chapter, we discussed techniques for real-time identification and

tracking of features required for teach by zooming visual servo control method.

The development included description of feature point tracking algorithms utilizing

K~anade-Lucas-Tomas tracker along with color thresholding-based image processing

methods and multi-view photogrammetry techniques are realized to relate various

coordinate frames.















CHAPTER 3
OBJECTIVES

As described in Chapter 1, automated robotic citrus harvesting yields superior

fruit quality over the mechanical harvesting systems, which is highly desirable for

fresh fruit market. Characteristics of the automated fruit harvesting robot: (a)

Able to locate the fruits on the tree in three dimensions (3D) (b) able to approach

and reach for the fruit (c) able to detach the fruit according to a predetermined

criterion, and transfer it to a suitable container and (d) to be propelled in the

orchard with ease of maneuverability, from tree to tree and row to row, while

negotiating various terrains and topographies. The objective of the presented

work is to accomplish characteristics (a) and (b) of the automated fruit harvesting

system mentioned above. Color thresholding-based technique is realized for target

fruit identification, whereas multi-camera visual servo control techniques are

developed for 3D target position estimation and robot motion control.

With recent advances in camera technology, computer vision, and control the-

ory, visual servo control systems exhibit significant promise to enable autonomous

systems with the ability to operate in unstructured environments. Robotic harvest-

ing is one of the applications of visual servo control system working in unstructured

environment. Typical visual serving control objectives are formulated by the desire

to position/orient a camera using multi-sensor stereovision techniques or based on

a reference image obtained by a priory positioning the same camera in the desired

location (i.e., the "teach by showing" approach). The shortcoming of stereovision

based visual servo control technique is lack of the three dimensional Euclidean task

space reconstruction. Also, teach by showing visual servo control technique is not










feasible for applications where the camera can not be a priori positioned to the

desired position/orientation.

A new cooperative control strategy, "teach by zooming (TBZ)" visual servo

control is proposed where the objective is to position/orient a camera based

on a reference image obtained by another camera. For example, a fixed camera

providing a global view of an object can zoom in on an object and record a desired

image for the camera-in-hand.

A technical issue that has impacted the design of robust visual servo con-

trollers is the camera configuration (i.e. the pixel resolution versus field-of-view).

Teach by zooming visual servo control technique effectively identifying both the

configurations provides high resolution images obtained by camera-in-hand and

large field-of-view by fixed camera. Another issue associated with visual servo

control systems is the camera calibration. In order to relate pixelized image-space

information to the three dimensional task-space, exact knowledge of the camera

calibration parameters is required, and discrepancies in the calibration matrix

result in an erroneous relationship between target image-space coordinates and the

corresponding Euclidean coordinates. Therefore, the controller is developed based

on the assumption that parametric uncertainty exists in the camera calibration

since precise values for these parameters are difficult to obtain in practice.

Homography-based techniques are used to decouple the rotation and transla-

tion components between the current image taken by camera-in-hand and desired

reference image taken by a fixed camera. Since teach by zooming control objective

is formulated in terms of images acquired from different uncalibrated cameras, the

ability to construct a meaningful relationship between the estimated and actual

rotation matrix is problematic. To overcome this challenge, the control objective is

formulated in terms of the normalized Euclidean coordinates. Specifically, desired

normalized Euclidean coordinates are defined as a function of the mismatch in the










camera calibration. This is a physically motivated relationship, since an image

is a function of both the Euclidean coordinates and the camera calibration. By

utilizing, Lyapunov-based methods, a controller is designed to regulate the image

features acquired by the camera-in-hand to the corresponding image feature coor-

dinates in the desired image acquired by the fixed camera. Since the estimates of

intrinsic calibration parameters are used in controller development the method is

robust to the camera calibration parameters. Also, proposed control scheme does

not rely on other sensors e.g. ultrasonic sensors, laser based, sensors, etc.

Another multi-camera visual servo control strategy, 3D target reconstruction

based visual servo control, has been proposed for target position estimation in the

Euclidean space. The technique utilizes statistical data along with camera intrinsic

parameters for 3D position estimation. Similar to teach by zooming visual servo

control, 3D target reconstruction based visual servo control technique effectively

utilizing both the configurations provides high resolution images obtained by

camera-in-hand and large field-of-view by fixed camera. The fixed camera providing

a global view of the target is used for target detection and image processing thus

optimizing the computation time, while using feedback from camera-in-hand during

visual serving.















CHAPTER 4
METHODS AND PROCEDURES

4.1 Introduction

This chapter provides an overview of the vision-based control techniques,

namely the teach by zooming visual servo controller and 3D target reconstruction

based visual servo controller, realized for autonomous citrus harvesting along with

the experimental testbed. The experimental facility provides a rapid prototyping

testbed for autonomous robotic guidance and control.

The hardware involved for the experimental testbed facility can be subdivided

into the following main components: (1) robotic manipulator; (2) end-effector; (3)

robot servo controller; (4) image processing workstation; and (5) network commu-

nication. The software developed for the autonomous robotic citrus harvesting is

based on image processing and computer vision methods, including feature point

tracking and multi-view photogrammetry techniques. An overview of vision-based

autonomous citrus harvesting method is illustrated in Figure. 4-1 and described as

follows:

1. Images of the citrus tree canopy are captured by a CCD camera and passed

to the image processing/visual servo control workstation through a USB analog to

digital converter and framegfrabber.

2. Image processing and computer vision algorithms identify the target, i.e.

citrus fruit, from the image of a tree canopy. Feature points are identified and the

relationship between current and desired images is determined, in teach by zooming

visual servo control, to generate the control commands. In 3D target reconstruction

technique, the estimate for the Euclidean position of a target is determined to

provide the robot control workstation with control commands.











ORANGE TREE CANOPY


HARVESTING CYCLE


ROBOTIC ARM





CCD CAMERA


ORIENTATION



CONTROL COMMANDS


IMAGE PROCESSING/VISUAL ROBOT CONTROL
SERVO CONTROL WORKSTATION WORKSTATION


Figure 4-1: Overview of vision-based autonomous citrus harvesting.


3. Position and orientation commands are developed for robotic arm by the

robot control workstation utilizing low level controller.

4. Real-time network communication control is established between the image

processing workstation and robot control workstation.

The hardware setup of the experimental testbed facility is described in Section

4.2. Teach by zooming visual servo control method is described in Section 4.3,

while Section 4.4 illustrates the 3D target reconstruction based visual servo control

technique and concluding remarks are provided in Section 4.5.

4.2 Hardware Setup

The hardware for the experimental testbed for autonomous citrus harvesting

consists of the following five main components: (1) robotic manipulator; (2) end-

effector; (3) robot servo controller; (4) image processing workstation; and (5)
































Figure 4-2: A Robotics Research K(-1207i articulated robotic arm.


network communication. The rest of the section provides details about each of the

components mentioned above.

The first component is the robotic manipulator. The experimental test-bed

consists of a Robotics Research K(-1207i, a 7-axis, kinematically-redundant manip-

ulator as shown in Figure. 4-2. The K(-1207i model, offering a 50 inch reach and

a 35 lb continuous-duty payload is a lightweight electric-drive articulated robotic

arm. The robotic manipulator is operated in a cooperative camera configuration.

The cooperative camera configuration includes a camera in-hand, which is attached

to the end-effector of the manipulator as shown in Figure. (4-3), and a fixed cam-

era having zooming capabilities mounted on the stationary base joint such that the

target is always in the field-of-view (FOV). The fixed camera thus provides a global

view of the tree canopy and can be used to zoom-in on the target, viz. a citrus

fruit, to capture the desired image for the camera in-hand. The cameras used for

this configuration are K(T&C make (model: K(PCS20-CP1) fixed focal length, color
































Figure 4-3: Camera in-hand located at the center of the robot end-effector.


CCD cone pinhole cameras. The image output from the cameras is NTSC analog

signal which is digitized using universal serial bus (USB) frame grabbers.

The second component is the robot end-effector. The end-effector is an

accessory device or tool specifically designed for attachment to the robot wrist or

tool mounting plate to enable the robot to perform its intended task. The end-

effector used for this experiment is a 3-link electrically actuated gripper mechanism

developed by S. Flood at the University of Florida. The gripper links are padded

with soft polymer material to avoid damaging the fruit. As seen in Figure. (4-3),

a camera along with an infrared sensor is located at the center of the gripper. An

infrared sensor is used as a proximity sensor to activate the gripper mechanism

when the target fruit is at the center of the end-effector. Serial port communication

is implemented for an infrared sensor data acquisition and gripper motor control.

The end-effector can also accommodate an ultrasonic sensor for range identification.










The third component of the autonomous citrus harvesting testbed is robot

servo control unit. The servo control unit provides a low level control of the robot

manipulator by generating the position/orientation commands for each joint.

Robotics Research R2 Control Software provides the low level robot control. The

robot controller consists of two primary components of operation; the INtime

real-time component (R2 RTC) and the NT client-server upper control level

component. The R2 RTC provides deterministic, hard real-time control with

typical loop times of 1 to 4 milliseconds. This component performs trajectory

planning, Cartesian compliance and impedance force control, forward kinematics,

and inverse kinematics. The controller can accept commands in the form of high

level Cartesian goal points down to low level servo commands of joint position,

torque or current. The robot servo control is performed on a Microsoft Windows

XP platform based IBM personal computer (PC) with 1.2 GHz Intel Pentium

Celeron processor and 512 MB random access memory (RAM).

The forth component is the image processing workstation, which is used for

image processing and vision-based control. Multi-camera visual servo control

technique described here consists of a fixed camera mounted on the stationary

base joint of the robot whereas a camera in-hand is attached to the robot end-

effector. The fixed camera provides a global view of the tree canopy and can

be used to capture the image for the target fruit. Microsoft DirectX and Intel

Open Computer Vision (OpenCV) libraries are used for image extraction and

interpretation, whereas Lucas-K~anade-Tomasi (K(LT) based multi-resolution

feature point tracking algorithm, developed in Microsoft Visual C++ 6.0, tracks

the feature points detected in the previous stage in both the images. Multi-view

photogrammetry based method is used to compute the rotation and translation

between the camera in-hand frame and fixed camera frame utilizing the tracked

feature point information. A nonlinear Lyapunov-based controller, developed










in Chapter 5, is implemented to regulate the image features from the camera

in-hand to the desired image features acquired by the fixed camera. The image

processing and vision-based control are performed on a Microsoft Windows XP

platform based PC with 2.8 GHz Intel Pentium 4 processor and 512 MB RAM.

The fifth component is the network communication between the robot servo control

workstation and the image processing workstation. A deterministic and hard real-

time network communication control is established between these computers using

INtime software.

4.3 Teach by Zooming Visual Servo Control

Teach by zooming (TBZ) visual servo control approach is proposed for

applications where the camera cannot be a priori positioned to the desired

position/orientation to acquire a desired image before servo control. Specifically,

the TBZ control objective is formulated to position/orient an on-board camera

based on a reference image obtained by another camera. An overview of the

complete experimental testbed is illustrated in Figure. (4-4) and described in the

following steps:

1) Acquire the image of the target fruit for the fixed camera. The target fruit

can be selected manually or autonomously by implementing cost function based

image processing.

2) Digitally zoom-in the fixed camera on the target fruit to acquire the

desired image, which is passed to the image processing workstation. The amount

of magnification can be decided based on the size of a fruit in the image and the

image aspect ratio.

3) Run the feature point extraction and K(LT-based multi-resolution feature

point tracking algorithm on the desired image for the fixed camera.

4) Orient the robot end-effector to capture the current image of the target by

the camera in-hand, which is passed to the image processing workstation.










5) Run the feature point extraction and tracking algorithm on the current

image for the camera in-hand.

6) Implement feature point matching to identify at least four identical feature

points between the current image and the desired image.

7) Compute the rotation and translation matrix between the current

and desired image frames utilizing the multi-view photogrammetry approach

(homography-based decomposition).

8) Rotation and translation matrix computed in step 6 can be used to compute

the desired rotation and translation velocity commands for the robot servo control.

9) The lower level controller generates the necessary position and orientation

commands for the robotic manipulator.

10) Approach the fruit and activate the gripper mechanism for harvesting a
fruit .

4.4 3D Target Reconstruction Based Visual Servo Control

The 3D Euclidean coordinates of the target fruit can be determined based on

the statistical mean diameter of the fruit as discussed later in Chapter 6. An end-

effector, and hence the camera in-hand, can be oriented such that the target fruit is

in the field-of-view of the camera in-hand. At this point, the Euclidean coordinates

of the target are again determined before approaching the fruit. An infrared sensor

is used as a proximity sensor for final approach towards the fruit and to activate

the gripper mechanism. An overview of the 3D target reconstruction based visual

servo control approach is illustrated in Figure. 4-5.

Following are the experimental steps or the harvesting sequence:

1) Acquire the image of the target fruit for the fixed camera. The target fruit

can be selected manually or autonomously by implementing cost function based

image processing.





















ZO OMED-IN IMAGE
(DESIRED IMAGE)


FEATURE POINT
TRACKING (FPT)


1IM~AGE
PR~OCESSINTGIVISUAL`
SERVO CONTROL


CONTROL
COMMANDS



ROBOT
CONTROL
WOlRKT SATTOlN



POSITION &t
ORIENTATION


FPT


CAMERA IN-HAND


ROBOTIC ARM


Figure 4-4: Overview of the TBZ control architecture.


FIXED CAMERA










2) Estimate the Euclidean position of the target expressed in the fixed camera

coordinate system based on the 3D target reconstruction described in Chapter 6.

3) Determine the target position in the base frame using an extrinsic camera

calibration matrix. The rotation and translation components of the extrinsic

camera calibration matrix Aef a RW3x4 for the fixed camera are as follows:

0 1 0 -254.00

Ref = 0 0 1 Tef = 196.85 (4-1)

-1 0 0 -381.00

4) Orient the camera in-hand such that the target fruit is in the field-of-view of
the camera in-hand.

5) Estimate the Euclidean position of the target expressed in the camera

in-hand coordinate system based on the 3D target reconstruction.

6) Approach the target keeping the fruit at the center of the camera in-hand

image.

7) Reach the fruit harvesting position using an infrared sensor as a proximity

sensor and activate the gripper mechanism.

4.5 Conclusion

In this chapter, we discussed development of a testbed for multi-camera

visual servo control techniques for autonomous robotic citrus harvesting. The

rapid prototyping testbed provides a platform for experimental validation of

multi-view photogrammetry-based teach by zooming visual servo control and 3D

target reconstruction based visual servo control techniques. The development

includes real-time identification of target, feature point identification and tracking,

algorithms for multi-view photogrammetry techniques, and 3D depth identification

for target reconstruction. Also, this chapter provides an overview of the visual

servo control techniques realized for autonomous citrus harvesting.





IMAGE PROCESSINGIVISUAL
SERVIO CONTROL


3D Target
Reconstruction


ROBOT
CONTROL
WORKS STATION


Align robot


CURRENT IMAGE


IMAGE
PROCESSEYG
WORKS STATION


ROBOT
CONTROL
WORTCSTATTON


Figure 4-5: Overview of the 3D target reconstruction-based visual servo control.


FIXED CAMERA


Target Euclidean
position: (x, y, z)



COMMANDS


CAMERA IN-HAND















CHAPTER 5
TEACH BY ZOOMING VISUAL SERVO CONTROL FOR AN UNCALIBRATED
CAMERA SYSTEM

The teach by showing approach is formulated as the desire to position/orient

a camera based on a reference image obtained by a priori positioning the same

camera in the desired location. A new strategy is required for applications where

the camera can not be a priori positioned to the desired position/orientation. In

this chapter, a "teach by zooming" approach is presented where the objective is to

position/orient a camera based on a reference image obtained by another camera.

For example, a fixed camera providing a global view of an object can zoom-in on an

object and record a desired image for the camera in-hand (e.g. a camera mounted

on the fixed base joint providing a goal image for an image-guided autonomous

robotic arm). A controller is designed to regulate the image features acquired by

a camera in-hand to the corresponding image feature coordinates in the desired

image acquired by the fixed camera. The controller is developed based on the

assumption that parametric uncertainty exists in the camera calibration since

precise values for these parameters are difficult to obtain in practice. Simulation

results demonstrate the performance of the developed controller.

5.1 Introduction

Recent advances in visual servo control have been motivated by the desire to

make vehicular/robotic systems more autonomous. One problem with designing

robust visual servo control systems is to compensate for possible uncertainty in the

calibration of the camera. For example, exact knowledge of the camera calibration

parameters is required to relate pixelized image-space information to the task-

space. The inevitable discrepancies in the calibration matrix result in an erroneous










relationship between the image-space and task-space. Furthermore, an acquired

image is a function of both the task-space position of the camera and the intrinsic

calibration parameters; hence, perfect knowledge of the intrinsic camera parameters

is also required to relate the relative position of a camera through the respective

images as it moves. For example, the typical visual serving problem is constructed

as a teach by showing (TBS) problem, in which a camera is positioned at a

desired location, a reference image is acquired (where the normalized task-space

coordinates are determined via the intrinsic calibration parameters), the camera

is moved away from the reference location, and then the camera is repositioned

at the reference location by means of visual servo control (which requires that the

calibration parameters did not change in order to reposition the camera to the

same task-space location given the same image). See [4], [20], [18], and [23] for a

further explanation and an overview of the TBS problem formulation.

For many practical applications it may not be possible to TBS (i.e., it may not

be possible to acquire the reference image by a priori positioning a camera in-hand

to the desired location). As stated by Malis [23], the TBS problem formulation

is camera-dependent due to the assumption that the intrinsic camera parameters

must be the same during the teaching stage and during servo control. Malis [23],

[22] used projective invariance to construct an error function that is invariant of the

intrinsic parameters meeting the control objective despite variations in the intrinsic

parameters. However, the goal is to construct an error system in an invariant space,

and unfortunately, as stated by Malis [23], [22], several control issues and a rigorous

stability analysis of the invariant space approach has been left unresolved.

In this work, a teach by zooming (TBZ) approach [5] is proposed to posi-

tion/orient a camera based on a reference image obtained by another camera. For

example, a fixed camera providing a global view of the scene can be used to zoom

in on an object and record a desired image for an on-board camera. Applications










of the TBZ strategy could include navigating ground or air vehicles based on

desired images taken by other ground or air vehicles (e.g., a satellite captures a

"zoomed-in" desired image that is used to navigate a camera on-board a micro-air

vehicle (MAV), a camera can view an entire tree canopy and then zoom in to

acquire a desired image of a fruit product for high speed robotic harvesting). The

advantages of the TBZ formulation are that the fixed camera can be mounted

so that the complete task-space is visible, can selectively zoom in on objects of

interest, and can acquire a desired image that corresponds to a desired position

and orientation for a camera in-hand. The controller is designed to regulate the

image features acquired by an on-board camera to the corresponding image feature

coordinates in the desired image acquired by the fixed camera. The controller is

developed based on the assumption that parametric uncertainty exists in the cam-

era calibration since these parameters are difficult to precisely obtain in practice.

Since the TBZ control objective is formulated in terms of images acquired from

different uncalibrated cameras, the ability to construct a meaningful relationship

between the estimated and actual rotation matrix is problematic. To overcome this

challenge, the control objective is formulated in terms of the normalized Euclidean

coordinates. Specifically, desired normalized Euclidean coordinates are defined as a

function of the mismatch in the camera calibration. This is a physically motivated

relationship, since an image is a function of both the Euclidean coordinates and the

camera calibration.

This method builds on the previous efforts that have investigated the advan-

tages of multiple cameras working in a non-stereo pair. Specifically, Dixon et al.

[6], [7] developed a new cooperative visual serving approach and experimentally

demonstrated that using information from both an uncalibrated fixed camera and

an uncalibrated on-board camera enables an on-board camera to track an object

moving in the task-space with an unknown trajectory. The development by Dixon










et al. [6], [7] is based on a crucial assumption that the camera and the object

motion is constrained to a plane so that the unknown distance from the camera to

the target remains constant However, in contrast to development by Dixon et al.

[6], [7], an on-board camera motion in this work is not restricted to a plane. The

TBZ control objective is also formulated so that we can leverage previous control

development by Fang et al. [9] to achieve exponential regulation of an on-board

camera despite uncertainty in the calibration parameters.

Simulation results are provided to illustrate the performance of the developed

controller.

5.2 Model Development

Consider the orthogonal coordinate systems, denoted 7T, -Fy, and FT* that are

depicted in Figure. 5-1 and Figure. 5-2. The coordinate system FT is attached to

an on-board camera (e.g., a camera held by a robot end-effector, a camera mounted

on a vehicle). The coordinate system TFy is attached to a fixed camera that has an

adjustable focal length to zoom in on an object. An image is defined by both the

camera calibration parameters and the Euclidean position of the camera; therefore,

the feature points of an object determined from an image acquired from the fixed

camera after zooming in on the object can be expressed in terms of TFy in one

of two ways: a different calibration matrix can be used due to the change in the

focal length, or the calibration matrix can be held constant and the Euclidean

position of the camera is changed to a virtual camera position and orientation.

The position and orientation of the virtual camera is described by the coordinate

system FT*. Table 5-1 shows the parameters expressed in various coordinate frames.

A reference plane xT is defined by four target points Oi Vi = 1, 2, 3, 4 where the

three dimensional (3D) coordinates of Oi expressed in terms of 7T, TFy, and FT* are









Table 5-1: List of variables for teach by zooming visual servo control.



Parameters Frames Description
R (t), xf (t) FT to FT* Rotation and translation vector from FT to FT*
Xi (t), z(t), Zi (t) FT Euclidean coordinates of a target in 7T
Xfi, Yfi, Zfi -Fy Euclidean coordinates of a target in TFy
Xpi, Yfi, Zzi F* Euclidean coordinates of a target in FT*
us, r Pixel coordinates of a target in 7T
n', Vf v;Fy Pixel coordinates of a target in TFy
Uf ve? Pixel coordinates of a target in FT*


defined as elements of mi (t), m, and mf E RW3 aS follows:


mi = X4 Z,] (5-1)


me =I X p Yfi Z y;

m-i- = X y i 4Z


The Euclidean-space is projected onto the image-space, so the normalized

coordinates of the targets points mi (t), m, and my can be defined as

mi Xi :
mi -1(52
Zi Zi Zi (a
m X ye Yfi
m, 1
zAi X zp Y,i 1
m 1
s ZfZ,* Z~*

where the assumption is made that Zi (t), Zf and Zfi > e; e denotes a positive

(non-zero) scalar constant. Based on (5-2) the normalized Euclidean coordinates of

m, can be related to my as follows:


m,=dig-s s-1mt (5-3)


where diag { denotes a diagonal matrix of the given arguments.










Objea~c Planei







SVirtual Camera



Camera inhn





as~ follows

F[ T






The ~ ~ igr contan pixel ra coordinates exrse ntrso r(elainotedi, v; R



andm of (Tdenoted ufui(t f (t R) are an repetiel defined as elements of pi (t E R W3an

psf o R3 S F: OS




Pfp i Amvi P u v (~56)
The Tostn px codnts xrse tr f T eoTed, f












~ Fixed amera

Vital Camera



Reference Plane of Target Points



Figure 5-2: Teach by zooming visual servo control for a robotic manipulator.




the normalized task-space coordinates m?, and my (t) as:


Pf i A~m,(5-7)

pf = A*m?, or pf = Afmf. (5-8)


In (5-8), the first expression is where the Euclidean position and orientation of

the camera remains constant and the camera calibration matrix changes, and

the second expression is where the calibration matrix remains the same and the

Euclidean position and orientation is changed. In (5-5) and (5-7), the intrinsic

calibration matrices A, Af, and A* E RW3x3 denote constant invertible intrinsic

camera calibration matrices defined as

Az AIcot~ no

AA vo (5-9)
sin





A y 0 o (5-10)
sin 7f
00 1











A( -A( cotf U of

A* O 0 voy (5-11)

0 0 1

In (5-9), (5-10), and (5-11), no, vo ERI and sof v2of ERI are the pixel coordinates

of the principal point of an on-board camera and fixed camera, respectively.

Constants Az, Apl, A ,A2, Xf27 2 ERI represent the product of the camera scaling

factors and focal length, and 4, 4f yER are the skew angles between the camera

axes for an on-board camera and fixed camera, respectively.

Since the intrinsic calibration matrix of a camera is difficult to accurately

obtain, the development is based on the assumption that the intrinsic calibration

matrices are unknown. Since Af is unknown, the normalized Euclidean coordinates

m?, cannot be determined from pfi using equation (5-7). Since m, cannot be

determined, then the intrinsic calibration matrix A* cannot be computed from (5-
7) For the, TBZ formulation, pf defines the desired image-space coordinates. Since

the~ normaI~Ilized EucIIU~lda coordinates~~ my~ are unknown, the control objective is

defined in terms of serving an on-board camera so that the images correspond. If

the image from an on-board camera and the zoomed image from the fixed camera

correspond, then the following expression can be developed from (5-5) and (5-8):


mi = mdi n A-1Afm; (5-12)


where mdi E RW3 denotes the normalized Euclidean coordinates of the object

feature points expressed in Fad, where Fad is a coordinate system attached to an

on-board camera when the image taken from an on-board camera corresponds to

the image acquired from the fixed camera after zooming in on the object. Hence,

the control objective for the uncalibrated TBZ problem can be formulated as the

desire to force milt) to mdi. Given that m4(t)c, mfc, and mdi are unknown, the









estimates is (t) if and ige E RW3 are defined to facilitate the subsequent control

development [25]

my = i-pi = Ami (5-13)


lj2, = A jpf = Aym; (514)

may = -pz = Almdi (5-15)

where A, Af E RW3x3 are contant, best-guess estimates of the intrinsic camera

calibration matrices and Ay, respectively. The calibration error matrices A1,

Af E R~3x3 are defined as

An, A12, Al3
Ai n- A-A= 0 nA2 A23 (5-16)

001

A,,, Afl2 Afl3
Af a aAfA = 0 Af22z /f23 I -7
00 1

For a standard TBS visual servo control problem where the calibration of the

camera does not change between the teaching phase and the servo phase, A = Af;

hence, the coordinate systems Fad and FT* are equivalent.

5.3 Homography Development

From Figure. 5-1 the following relationship can be developed


mi = Rifi + xf (5-18)

where R(t) E RW3x3 and zy(t) E RW3 denote the rotation and translation, respectively,

between 7T and FT*. By utilizing (5-1) and (5-2), the expression in (5-18) can be









expressed as follows

mi = Z~ R + man* my (5-19)


QiH


where as (t) n A E R3 and d* ERW denotes an unknlown constant distance from

FT* to xT along the unit normal n*. The following relationship can be developed by



pi = s Gpf(5-20)

where- ~ GE R~3x3 is the projective homography matrix defined as G(t) AHU/t)A/.

The expressions in (5-5) and (5-8) can be used to rewrite (5-20) as


mi = aiA-1GAym; (5-21)

The following expression can be obtained by substituting (5-12) into (5-21)


mi = ag Hdmdi (5-22)

where Hd(t) n A-1G(t)A denotes the Euclidean homography matrix that can be

expressed as

HdHd a+ mannir where may = Xa (5-23)

In (5-23), Rd(t) E RW3x3 and mya(t) E RW3 denote the rotation and translation,

respectively, from FT to Fa. The constant dd ERI in (5-23) denotes the distance
from Fed to xT along the unit normal nd E RW3
Sice-- mit) and my cannot be determined because the intrinsic camera

calibration matrices and Af are uncertain, the estimates is(t) and Age defined in

(5-13) and (5-14), respectively, can be utilized to obtain the following:


As~ = aiHaday. i


(5-24)









In (5-24), He (t) E R~"x3 denotes the following estimated E~uclidean homography:

[25]:

Hed = AHdA-1. (5-25)

Since is(t) and Age can be determined from (5-13) and (5-15), a set of linear

equations can be developed to solve for Hd (t) (see [8] for additional details

regarding the set of linear equations). The expression in (5-25) can also be

expressed as follows [8]:

He, = 174 + ; ., (5-26)

In (5-26), the estimated rotation matrix, denoted lia (t) E RW3x3, is related to Rd (t)
as follows:

lia = ARdA-1 (5-27)

and Age (t) E RW3, Ti 3 denote the estimate of man (t) and nd, respectively, and
are defined as:

Age = Yiiman (5-28)



nar = -A-"l (-29

where y ERI denotes the following positive constant


7 = A-~ndl (5-30)

Although Ha-(t) can be computed, standard techniques cannot be used

to decompose Hd (t) into the rotation and translation components in (5-26).

Specifically, from (5-27) Rd(t) is not a true rotation matrix, and hence, it is not
clear how standard decomposition algorithms (e.g., the Faugeras algorithm [12])

can be applied. To address this issue, additional information (e.g., at least four

vanishing points) can be used. For example, as the reference plane xT approaches

infinity, the scaling term d* also approaches infinity, and xh(t) and Sa(t) approach









zero. Hence, (5-26) can be used to conclude that Hd(t) = Rd(t) on the plane

at infinity, and the four vanishing point pairs can be used along with (5-24) to

determine Rd(t). Once Rd(t) has been determined, various techniques (e.g., see

[10, 32]) can be used along with the original four image point pairs to determine

Age(t) and Afdt).
5.4 Control Ob jective

The control objective is to ensure that the position and orientation of the

camera coordinate frame 7T is regulated to Fa. Based on Section 5.3, the control

objective is achieved if

Rd(t)~ 13 (5-31)

and one target point is regulated to its desired location in the sense that


milt) mdi and Zy (t) Zdi (5-32)

where I3 E IW3x3 represents an identity matrix.

To control the position and orientation of 7T, a relationship is required to

relate the linear and angular camera velocities to the linear and angular velocities

of the vehicle/robot (i.e., the actual kinematic control inputs) that enables an

on-board camera motion. This relationship is dependent on the extrinsic calibration

parameters as follows [25]:



R, i [(5-33)


where ve(t), cJ(t) E RW3 denote the linear and angular velocity of the camera, vr(t),

LJ,(t) E RW3 denote the linear and angular velocity of the vehicle/robot, R, E RW3x3
denotes the unknown constant rotation between an on-board camera and robot

end-effector frames, and [t,], E RW3x3 is a skew symmetric form of to E IW3, which
denotes the unknown constant translation vector between an on-board camera and









CURRENT IMAGE


ROBOTIC ARM


Figure 5-3: Overview of teach by zooming visual servo controller.


vehicle/robot frames. A block diagram of teach by zooming visual servo controller

is shown in Figure. 5-3.

5.5 Control Development

5.5.1 Rotation Controller

To quantify the rotation between 7T and Fad (i.e., Rd (t) given in (5-23)), a

rotation error-like signal, denoted by e, (t) E RW3, is defined by the angle axis

representation as [26]:


e, = uO


(5-34)


where n (t) E RW3 represents a unit rotation axis, and 8 (t) ERI denotes the rotation

angle about u(t) that is assumed to be constrained to the following region


0< et 0

(535)









The parameterization n (t) 6(t) is related to the rotation matrix Rd (t) as

Rd = 13 s 8 xu] + 2 sin2 2 [n]2 (5-36)

where [n] x denotes the 3 x 3 skew-symmetric matrix associated with u(t). The

open-loop error dynamics for e, (t) can be expressed as (Refer to Appendix A.1)

e:, = L, R, we (5-37)

where the Jacobian matrix L, (t) E RW3x3 is defined as


8 [ x i csin c (0 )2
L, = I3 2 []x 1 [n] (5-38)


In equation (5-38) the sin c(0) term is given by (5-39) as,


sin(0)
sin c(0) = (5-39)

Since the rotation matrix Rd (t) and the rotation error e, (t) defined in (5-34)
are unmeasurable, an estimated rotation error e, (t) E RW3 is defined as

et = iO (5-40)

where i (t) E RW3, 8 (t) ERI represent estimates of a (t) and 8 (t), respectively. Since

Rd (t) is similar to Rd (t) (i.e., Rd (t) has the same trace and eigenvalues as Rd (t)),
the estimates i(t) and 0(t) can be related to u (t) and 8 (t) as follows [25]:

S= 0 it = pr~n (5-41)

where p(t) ERI denotes the following unknown function

p = -.(5-42)
SAnI









The relationship in (5-41) allows e, (t) to be expressed in terms of the unmeasur-
able error e, (t) as


e, = plAe,.


(5-43)


Given the open-loop rotation error dynamics in (5-37), the control input we (t)
is designed as

we, = -A, R, e, (5-44)

where A, ERI denotes a positive control gain, and II, E RW3x3 denotes a constant
best-guess estimate of R,. Substituting (5-43) into (5-44) and substituting the
resulting expression into (5-37) gives the following expression for the closed-loop
error dynamics [25]:


e, = -A,pIL,RTAe,


(5-45)


where the extrinsic rotation estimation error R, E RW3x3 is defined as


R, = R,17 .


(5-46)


The kinematic control input given in (5-44) ensures that e, (t) defined in

(5-34) is exponentially regulated in the sense that


||e, (t)|

(5-47)


provided the following inequality is satisfied:


rCT Ri,A z x > o ||z|2 f01r Vl 6 W3


(5-48)


where


() R


(5-49)


SR, A + R,A> A









for Vz E RW3, and Po ERI denotes the following minimum eigenvalue:


Po = Xmin 2, ,) (5-50)



Proof: See [9].
5.5.2 Translation Controller

The difference between the actual and desired 3D Euclidean camera position,
denoted by the translation error signal e, (t) E RW3, is defined as

e, me mde (5-51)

where me (t) E RW3 denotes the extended coordinates of an image point on xT
expressed in terms of FT and is defined asl


me mer (t) me*2( 1 me3 (t) (5-52)

SX1Z Y~I 1

and mde E RW3 denotes the extended coordinates of the corresponding desired image

point on xT in terms of Fed as

mee ael de2 de3(5-53)

I X,,z Ydl




1 To develop the translation controller a single feature point can be utilized.
Without loss of generality, the subsequent development will be based on the image
point Oz, and hence, the subscript 1 will be utilized in lieu of i.









where In (-) denotes the natural logarithm. Substituting (5-52) and (5-53) into

(5-51) yields
SX1 Xdl Y, Ydl zz1 54
e,=In(54

where the ratio canl be comnputed forom (519) and the decomnposition of thle

estimated Euclidean homography in (5-24). Since milt) and md are unknown (since

the intrinsic calibration matrices are unknown), e, (t) is not measurable. Therefore,

the estimate of the translation error system given in (5-54) is defined as


e, ~~ ~Z, i e ilRe d2I (5-55)

where ie1(t), ie2t) ~del ~de2 ERI denote estimates of me (t), me2 1 mdel, mde2,

respectively.

To develop the closed-loop error system for e, (t), we take the time derivative

of (5-54) and then substitute (5-44) into the resulting expression for we, (t) to

obtain (Refer to Appendix A.2)


e, =LR~, +A,, L, t,] + ,,) ~e,(556i)

where the L, (t), L,, (t) E RW3"3 are defined as

-1 0 mer

L, O I 0 e (5-57)

0 0 -1


melme2 -1 m0 1 Re2

L,, n 1 + m 2 -melme2 -mel (5-58)

-me2 mel 0

To facilitate the control development, the unknown depth Zl(t) in (5-57) can be

expressed as

Z, = Zf (5-59)
01









where al is given by the homography decomposition.

An estimate for L, (t) can be designed as

-1 0 e

Z1

0 0 -1

where ier (t), ie2(t) were introduced in (5-55), and Z1 ERI is developed based on

(5-59) as

Z, = Zf. (5-61)

Based on the structure of the error system in (5-56) and subsequent stability

analysis, the following hybrid translation controller can be developed


v, (t) = A,11, L, e,(562




where 1%,T, 8, (t), and L, (t) are introduced in (5-44), (5-55), and (5-60), respec-

tively, k,1, kn2 ERI denote positive constant control gains, and Zl(t) is defined in

(5-61). In (5-62), A, (t) ERI denotes a positive gain function defined as

z2
A, = kno + (5-63)
/ (Rel, Re2)

where kno ERI is a positive constant, and f (iel, Re2~) is a positive function of iel

and 1e2

The kinematic control input given in (5-62) ensures that the hybrid translation

error signal e, (t) defined in (5-54) is exponentially regulated in the sense that


|,, t | B xp-t) (5-64)


provided (5-48) is satisfied, where Be R 3"3 is a constant invertible matrix, and Po,

(1 ERI denote positive constants.










Proof: See [9].

5.6 Simulation Results

5.6.1 Introduction

A numerical simulation is presented to illustrate the performance of the TBZ

controller given in (5-44) and (5-62). The simulation setup for teach by zooming

visual servo control consists of following three main components: (1) simulation

workstation, (2) simulation software, and (3) virtual camera. The rest of the

section provides details about each of the components mentioned above.

The first component is the simulation workstation. The vision-based control

simulation is performed on Microsoft Windows XP platform based IBM personal

computer (PC) with 3.06 GHz Intel Pentium 4 processor and 764 MB random

access memory (RAM).

The second component in the simulation is the software platform. MATLAB

(The MathWorks, Inc.) release 6.0 has been used for numerical simulation, along

with Simulink for interactive graphical environment. Simulink is a platform for

multidomain simulation and Model-Based Design for dynamic systems.

The third component in the simulation being the virtual camera. The intrinsic

camera calibration matrix is given in (5-68), whereas (5-65) and (5-66) state the

extrinsic camera calibration parameters. In the intrinsic camera calibration matrix

it is assumed that parametric uncertainty exists in the camera calibration since

these parameters are difficult to precisely obtain in practice.

The following section provides numerical results of the simulation.

5.6.2 Numerical Results

The intrinsic camera calibration matrix used for an on-board camera and the

fixed camera are given as follows: no = vo = 120 [pixels] and sof = vof = 120

[pixels] denote the pixel coordinates of the principal point; Az = 122.5, X2 = 122.5,

Ayr = 147, Af2 = 147, AT = 294, and Aj = 294 denote the product of the










focal length and the scaling factors for an on-board camera fixed camera, and

fixed camera after zooming (i.e., the focal length was doubled), respectively; and

S= 7f = 1.53 [rad] is the skew angle for each camera. The unknown constant

rotation between the camera and end-effector frames, and the unknown constant

translation between the camera and end-effector frames (i.e., the extrinsic camera

calibration parameters R, and t, defined in (5-33)) were selected as follows

0.95692 -0.065563 0I -' -'\

R, = 0. 11725 0.97846 -0. 16989 (5-65)

-0.26561 0.19574 0.944

tr = 0.02 0.04 0.03 .' (566)


The best-guess estimates for R, and A were selected as follows:

0.9220 -0. 1844 0.3404

R,= 0.3404 0.8050 -0.4858 (5-67)

-0. 1844 0.5638 0.8050

120 -4 122

A= 0 121 123 (5-68)

001

The image space coordinates (all image space coordinates are in units of pixels) of

the four constant reference target points before and after increasing the focal length







(x2) were respectively selected as follows:

Po = 121.9 12041 1]


Pf2=121.7121.2 1]T
Pf3 = 121.0 121.0 1i
Pf4 = 121.2 120.3

PI =?5 129.4 122 1




P4;O =125.7 121.6 1

as(0 followsI l

92 (0) =Ii 116.4 114








The vanishing points for the fixed camera were selected as

Pi =r~ 134.1 134.7





Pv2 = 135.3 1053 1 T


hlU=[ TI '% L]


whie thel vansigpints forth and onbaramr were selected asth follow vle yl t bs:

Po(0 = 76.5 27.7. 159

The resulPv2(0) 144tina 199 1ntestasainlerr r eitdi iu






Control gins A and A,, weret andjust) eed totefoing v5)alustoyed th2) e bestiey


5-5 ad Figure. 5-rspciel.Fo Figure. 5-5 and Figure. 5-6, it can be e ta h nur d



liercontrolinu velocities wre (t)y a ond ,()defnd i 54)ad(-6) epciey















































































-0.I
U 1 15-----------------I
CI I | |


0: Targe points vieed from fixed camera before zooming i + P2i
+ : Target pairts viewed from fixed camera after zooming
tP3"









d' P2f
P3f P1.

P4*



ey-* Plf
P4f


122



121 .


122 123 124 125 126
X -Axis


127 128 129 130


Figure 5-4: Image from the
zooming superscriptt '*').


fixed camera before zooming subscriptt 'f') and after


Figure 5-5: Rotation Errors.








53













-0.1-


.*








0 1 2 3 4 5 6
Time [sj



Fiur 5-6 TrnsatoEros








4 g+ Iitl poito ofcme Inhn




4.3

4.2


40 1


-0. 7







Figue 5-: Euliden trjectry of~6 thefeature points vee yte aea-n






hand~~4. fro th inta poiiondoiettialpston (denoteda by + t te esre

positions. an orientaedpotion Fa(eoe y'', hr h ita camera coordinat

systm F*is enotd by'o'




































































4-
2 -


Time [s]


Figure 5-8: Angular control input velocity for the camera-in-hand.


Tlm,- I:l


Figure 5-9: Linear control input velocity for the camera-in-hand.





















Figure 5-10: Elimination of ineffectual feature points via. target identification.


5.7 Experimental Results

An overview of the complete experimental testbed is illustrated in Figure. (4-

4) and an algorithm for teach by zooming visual servo control method is presented

in Appendix C. Multi-camera visual servo control technique described here consists

of a fixed camera mounted on the stationary base joint of the robot whereas a

camera in-hand is attached to the robot end-effector. The fixed camera provides a

global view of the tree canopy and can be zoomed in to capture the image for the

target fruit.

The two main challenges in the implementation of teach by zooming visual

servo controller for citrus harvesting application are discussed in the rest of the

section. The first challenge is to identify the feature points on the target and

eliminate the ineffectual feature points, or otherwise, which means that to identify

and track feature points on the fruit to be harvested and purge the feature points

from the environment. This is necessary in order to establish the rotational and

translational information, in terms of homography matrix, between the two cameras

looking at the target fruit. A solution to this problem is identified in the image

processing technique. Color thresholding-based image processing is used for

target detection, thus preserving the target rest of the image is purged. Feature

point identification and tracking is performed only on the detected target, thus

eliminating the issue of ineffectual feature points as shown in Figure. 5-10.




















Figure 5-11: Feature point matching between fixed camera and camera in-hand.


The second challenge in the implementation of the controller is feature point

matching between the images taken by camera in-hand and fixed camera. The

feature points viewed by the fixed camera can be different than the feature points

viewed by the camera in-hand as shown in Figure. 5-11. Non-identical feature

points between the current image obtained by a camera in-hand and the desired

image obtained by a fixed camera would result in incorrect rotation and translation

information. Feature point vector matching is realized to aim at identifying the

identical feature points between the current image and the desired image. However,

practically it is difficult to consistently obtain at least four identical feature points,

which are required for homography decomposition, and hence it is realized that

teach by zooming visual servo control strategy can not be implemented for a citrus

harvesting application.

5.8 Conclusion

A new TBZ visual servo control approach is proposed for applications where

the camera cannot be a priori positioned to the desired position/orientation to

acquire a desired image before servo control. Specifically, the TBZ control objective

is formulated to position/orient an on-board camera based on a reference image

obtained by another camera. In addition to formulating the TBZ control problem,

another contribution of this work is to illustrate how to preserve a symmetric

transformation from the projective homography to the Euclidean homography










for problems when the corresponding images are taken from different cameras

with calibration uncertainty. To this end, a desired camera position/orientation

is defined where the images correspond, but the Euclidean position differs as a

function of the mismatch in the calibration of the cameras. Simulation results are

provided to illustrate the performance of the controller. Rotation and translation

errors are exponentially regulated to zero thus establishing the stability of the

controller, whereas the angular and linear control velocities are always bounded.

Applications of this strategy could include navigating ground or air vehicles, based

on desired images taken by other ground or air vehicles (e.g., a micro air vehicle

(MAV) captures a "zoomed in" desired image that is used to navigate an on-board

camera) .

Practical limitation on the implementation of the teach by zooming visual

servo controller for the citrus harvesting application has been realized in the

fact that the feature points viewed by the fixed camera can be different than the

feature points viewed by the camera in-hand. Non-identical feature points between

the current image and the desired image would result in incorrect rotation and

translation information. Hence the teach by zooming visual servo controller can

be implemented where the feature point information is available a priori, i.e., this

controller is suitable for artificial targets.















CHAPTER 6
3D TARGET RECONSTRUCTION FOR VISION-BASED ROBOT CONTROL

The teach by zooming visual servo control strategy is devised to posi-

tion/orient a camera based on a reference image obtained by another camera.

As seen in Chapter 5, this strategy employs coordinated relationship between the

fixed camera and camera in-hand using the feature point matching technique.

However, feature point matching is not suitable for natural targets like citrus due

to non-identical feature point matching. Therefore, this chapter describes a visual

servo control strategy based on three dimensional (3D) reconstruction of the target

from a two dimensional (2D) image. The 3D target reconstruction is achieved by

using the statistical data, viz. the mean diameter and the standard deviation of the

citrus fruit diameter, along with the target image size and the camera focal length

to generate the 3D depth information. A controller is developed to regulate the

robot end-effector to the 3D Euclidean coordinates corresponding to the centroid of

the target fruit.

6.1 Model Development

Consider the orthogonal coordinate systems, denoted 7T, -Fy, and FT* that are

depicted in Figure. 6-1 and Figure. 6-2. The coordinate system FT is attached to

an on-board camera (e.g., a camera held by a robot end-effector, a camera mounted

on a vehicle). The coordinate system TFy is attached to a fixed camera (e.g. a

camera mounted on a stationary base joint of a robot) and the coordinate system

FT* is attached to a target fruit with fruit centroid being the coordinate system

origin. Table 6-1 shows the parameters expressed in various coordinate frames. The

origin of the coordinate system FT* is denoted as i, where the three dimensional

(3D) coordinates of i expressed in terms of FT and TFy are defined as elements of






















Camera in-hand
J Fixed Camera

Figure 6-1: Camera frame coordinate relationships.
Table 6-1: List of variables for 3D target reconstruction based visual servo control.


Parameters Frames Description
R* (t), xz} (t) FT to FT* Rotation and translation vector from FT to FT*
R)f (t), X) (t) ,Fy to FT* Rotation and translation vector from TFy to FT*
Xi (t), ~i(t), Zi (t) FT Euclidean coordinates of a target in 7T
Xfi, Yfi, Zfi -Fy Euclidean coordinates of a target in TFy
n r Pixel coordinates of a target in 7T
v vy V -Fy Pixel coordinates of a target in TFy
X4(t, (t, Zit) Estimated euclidean coordinates of a target in 7T
Xfi, Yfi, Zfi -Fy Estimated euclidean coordinates of a target in TFy


v*

\ITarget Fruit
%" i F.


mi;

v, (R;,, ;)


(R', x'
X '(


mi (t) and n E RW3 aS follows:



"I T, i i


(6-1)

(6--)


















Reference Plane i T It _-r Points


Figure 6-2: 3D target reconstruction based visual servo control for a robotic ma-
nipulator.

The Euclidean-space is projected onto the image-space, so the normalized

coordinates of the targets points mi (t) and m, can be defined as


m mZi XZi Zi ,'(3

m X ye Yfi
m 1 64


where the assumption is made that Zi (t) and Zfi > e; e denotes a positive (non-

zero) scalar constant. In addition to having normalized task-space coordinates, the

target point will also have pixel coordinates that are acquired from an on-board

camera expressed in terms of 7T, denoted by ui (t) I (t) E RW, and are defined as

elements of pi (t) E RW3 aS follows:


Pi~~ us :-1 (6-5)

The pixel coordinates pi (t) and normalized task-space coordinates mi (t) are

related by the following global invertible transformation (i.e., the pinhole model):


(6-6)


pi = Ami.









The constant pixel coordinates, expressed in terms of TF denoted v y; E RW, are

defined as elements of p ye E RW3 aS follows.


pfi = i U fi ,'(7


The pinhole model can also be used to relate the pixel coordinates p ye to the

normalized task-space coordinates m?, as:


Pfi = Af" (6-8)


In (6-6) and (6-8), the intrinsic calibration matrices A and Af E RW3x3 denote
constant invertible intrinsic camera calibration matrices defined as

Az AIcot~ u o

AA vo
smn

(6-9)

Af = -A i o of

A, 0 0 o


In (6-9), no, vo ERI and sof, v2of ERI are the pixel coordinates of the principal

point of the camera in-hand and fixed camera, respectively. Constants Az, Ayz,A2,

Af~ 2 ER represent the product of the camera scaling factors and focal length, and

~, yfERI are the skew angles between the camera axes for a camera in-hand and

fixed camera, respectively.

6.2 3D Target Reconstruction

The 3D target reconstruction is achieved by using the statistical mean diam-

eter of the target along with the target image size and the camera focal length to

generate the 3D depth information. The mean diameter of the citrus fruit can be

obtained from the statistical data as Do E RW. Using the perspective projections






62


Obj ect plane
Image plane



~ f


Figure 6-3: Perspective projection geometry model for Euclidean depth identifica-
tion.


geometry as shown in Figure. 6-3, the relationships for the target fruit size in the

object plane and the image plane can be obtained as follows:


Do Dfi

~(6-10)
Do Dfi

where~~ Z., .,E denote the estimates for an unknown three dimensional

depth of the target plane from the image plane, ffec, ify ERI is the product of

scaling factors and focal length of the fixed camera along the x and y directions,

respectively. In (6-10), the term Do ERI denotes the target diameter in the

object plane obtained from the statistical data whereas Dfi ERI denotes the

target diameter in the image plane expressed in terms of TFy. Utilizing (6-10), the

expression for the estimate of an unknown Euclidean depth of the target Zfi ERI
can be obtained as follows:

Zy; = (6-11)
2Dfi
The estimated Euclidean coordinates of the target, expressed in terms of 77f,

can be obtained from (6-4) and (6-11) as


i; = m Z i i= Xp 9 Z, (6-12)









Further, the Euclidean coordinates of the target computed in (6-12) can be

expressed with respect to the robot base frame FTb through the known extrinsic

camera calibration matrix Aey E RW3x3 aS follows.


mbi =AeyA(6-13)


Similarly, the Euclidean depth and hence the estimated Euclidean coordinates

of the target expressed in terms of FT are obtained as follows:

(fsi + /yl)Do
Zi = (6-14)
2De

mi = my Zi = [ zi Z, (6-15)


In (6-15), Zi(t) ERI denotes the estimated unknown three dimensional depth

of the target plane from the image plane, f,, ly ERI is the focal length of the

camera in-hand along the x and y directions, respectively, whereas Dy (t) ERI

denotes the target diameter in the image plane expressed in terms of FT. Hence,

knowing the statistical mean diameter of the target, expressions in (6-12) and

(6-15) can be used to compute the estimated target position expressed in TFy and

7T, respectively.

6.3 Control Development

This section describes the vision-based control development for robotic citrus

harvesting. The fixed camera can view an entire tree canopy to acquire a desired

image of a target fruit, but the target fruit may not be in the field-of-view of the

camera in-hand and hence the robot end-effector, and hence the camera in-hand,

is oriented along a target fruit centre as shown in Figure. 6-4. Once the end-

effector is oriented along the direction of the target fruit, an image captured by the

camera in-hand is used to calculate the Euclidean coordinates of the citrus fruits

in the field-of-view. Based on a cost function, which is a function of a depth and a

diameter of the fruit, the target is selected. The end-effector is then moved towards















Target








I` Target




Figure 6-4: Control architecture depicting rotation control.

the target fruit while aligning the centre of the target fruit at the centre of the

camera in-hand image. An infrared sensor is used as a proximity sensor for the final

position control as shown in Figure. 6-5.

6.3.1 Control Ob jective

The control objective is to ensure that the position and orientation of the

camera coordinate frame 7T is regulated to FT*. From the Figure 6-1, it can be

seen that the control objective is achieved if the target fruit centroid i is collinear

with the Z axis of camera in-hand coordinate system FT and the target point is

regulated to its desired location in the sense that FT(t) F ~*, i.e. mathematically it
can be stated as follows:

Ag (t) Tf (t) &bi (6-16)

where I3 E IW3x3 represents an identity matrix and Tf(t) E RW4x4 is the known robot

feedback matrix as given in (6-18).












C Target





Target


c*



Figure 6-5: Control architecture depicting translation control.

6.3.2 Rotation Controller

The estimated target position expressed in the robot base frame FTb can be
stated in the camera in-hand coordinate frame 7T through the robot feedback
matrix Ty (t) as follows:


I ~1 Ty mbi 1(6-17)

where the feedback matrix Tf(t) can be written in terms of the rotation and
translation vectors R, (t) E RW3 x3 and Py (t = x, (t~) y7 (t) z7c (t) F R3
respectively, as:

Tf = RfO Py (6-18)

The objective of the rotation controller is to align the z-axis of the camera in-hand
frame 7T, i.e., the z-axis of the robot end-effector, along the target fruit centroid.
The rotation control objective can be achieved by rotating the robot end-effector
about the x-axis through an angle o(t) ERI and y-axis though an angle P(t) E RW.









The rotation angles a(t) and P(t) can be quantified as below

a = tan-l Z


8 = tan .

Hence the rotation of the robot end-effector about x and

pressed in terms of rotation matrices R,(t) E RW3x3 and Ry(t)
as follows:


(6-19)


(6-20)

y axis can be ex-
E R3x3, respectively,


1 0 0 cos(p) 0 sin(p)
R, = 0 costa) sin(a) R, = 0 1 0 (6-21)

0 sin(a) cos(a) sin(p) 0 cos(p)

where a(t) and P(t) are defined in (6-19) and (6-20), respectively. Further, the

rotations R,(t) and R,(t) can be expressed in the robot base frame FTb aS:


Rbs = f a


Rby = f y.


(6-22)


Rotation error signal e, (t) E RW3 can be quantified as the mismatch between

the robot feedback Rf(t) and the desired rotations calculated in (6-22) in terms of
rotation about x, y, z axis of the robot base frame as follows:.


(6-23)


Based on the error system the rotation control velocity we (t) E RW3 for the
robot end-effector can be expressed as follows:


W, = k~pwew k,,we,


(6-24)


where ky,, k,,, ERI are the positive proportional and derivative control gains,

respectively, and e, (t) E RW3 is the time derivative of the rotation error signal e, (t).


e,(t) = a () (t) ?y(t)









6.3.3 Translation Controller

The difference between pixel coordinates of the target centre and the pixel

coordinates of the principal point of the camera in-hand, denoted by the translation

error signal e,(t), e,(t) ERI along the x and y axis, respectively, is defined as


etc = Ui no (6-25)

ey = '. Vo (6-26)

where ui(t), I (t) ERI are the pixel coordinates of the target centre defined in

(6-5) and no, vo ERI are the pixel coordinates of the principal point of the camera

in-hand defined in (6-9). Also, the translation error signal ez (t) ERI is defined

as the difference between the desired Euclidean depth and the current depth as

follows:

e, = Zy zy (6-27)

where Zi(t) ERI is the desired Euclidean depth defined in (6-15) and zy(t) ERI

is the feedback z-position of the end-effector given in (6-18). Based on the error

system developed in (6-25), (6-26), and (6-27), the translation control velocity

v, (t) E RW3 for the robot end-effector can be expressed as follows:


v, = kypvev k,,vev (6-28)


where ky,, k,, ERI are the positive proportional and derivative control gains,

respectively, e, (t) = x(t g(t) ezi (t) R3is~u ,,~, th rnltonerrsgal

and e,(t) E RW3 is the time derivative of the translation error signal e,(t). An

infrared sensor is used as a proximity sensor to accurately position the end-effector

before the fruit is harvested.

6.4 Experimental Results

This section is organized in the following manner. A preliminary experiment

describing the performance of 3D depth estimation technique is illustrated in










Section 6.4.1. An experimental performance validation is done in three parts.

In Section 6.4.2, an experiment is conducted to describe the repeatability of the

controller. In Section 6.4.3, the controller performance is discussed under different

positions of the robot end-effector while maintaining the constant Euclidean

position of the target fruit. In Section 6.4.4, the third part of the experiment

describes the performance of the controller under varied initial positions of the

robot end-effector as well as Euclidean position of the target. An algorithm for 3D

depth estimation and target reconstruction based visual servo control method is

presented in Appendix D.

6.4.1 Performance Validation of 3D Depth Estimation

The performance of the 3D depth estimation strategy is verified in this pre-

liminary experiment. The three main components of this preliminary experiment

are as follows: (1) CCD camera, (2) image processing workstation, and (3) depth

estimation. The camera is K(T&C make (model: K(PCS20-CP1) fixed focal length,

color CCD cone pinhole cameras. The image output from the camera is NTSC

analog signal which is digitized using universal serial bus (USB) frame grabber.

The second component in the experiment is the image processing workstation. The

3D depth estimation is performed on Microsoft Windows XP platform based IBM

personal computer (PC) with 3.06 GHz Intel Pentium 4 processor and 764 MB

random access memory (RAM). The image processing workstation acquires the

digitized image from the framegrabber and employs color thresholding-based tech-

niques for target identification. As described in Section 6.2, utilizing the target size

in the image and statistical parameters, the depth estimation technique identifies

3D depth of the target from the camera frame.

The target size in an object plane is measured to be Dow = 74.99 mm, and

Doy = 77.31 mm along x, y axis, respectively. The product of the focal length and

scaling factors for the camera are f, = 833.57 pixels, and f, = 767.02 pixels along










Camera M~otion Tre





Figure 6-6: Performance validation of 3D depth estimation.

Table 6-2: Performance validation for 3D depth estimation method.


Sr. No. Image-plane Actual Depth Estimated depth (mm)
target size (mm) along x along y mean
(pixels)
1 334 321 200 187.15 184. 73 185.94
2 218 205 300 286.73 289.26 288.00
3 161 152 400 388.25 390. 12 389. 18
4 128 119 500 488.35 498.30 493.32
5 104 98 600 601.04 605.08 603.06
6 88 83 700 710.33 714.44 712.38
7 78 72 800 801.39 823.59 812.49
8 69 64 900 905.92 926.53 916.23
9 63 59 1000 992.20 1005.05 998.634
10 56 53 1100 1116.23 1118.84 1117.53
11 52 50 1200 1202.09 1185.97 1194.03



x, y axis, respectively. The target is stationary whereas the camera is displaced in

incremental steps of 100 mm towards the target, hence it represents a camera in-

hand configuration as shown in Figure. 6-3. The results of the depth estimation are

shown in Table 6-2 and Figure. 6-7 illustrates the depth estimation error pattern.

Comparing the actual depth with the estimated depth it can be seen that 3D depth

estimation performs satisfactorily. From Figure. 6-7, it can be concluded that the

depth estimation method exhibit large estimation errors for lower target depths and

vice versa. This phenomenon can be attributed to the fact that the target detection

efficiency reduces for lower target depths.

6.4.2 Experiment I

The repeatability of a positioning system is the extent to which successive

attempts to move to a specific location vary in position. A highly repeatable










Depth Estimation Error













0(200 400 6 800 1200 14 300



Actual Depth [mm]


Figure 6-7: Depth estimation error.


system (which may or may not also be accurate) exhibits very low scatter in

repeated moves to a given position, regardless of the direction from which the point

was approached. This part of the experiment discusses the repeatability of the

proposed controller under the same starting position of the robot end-effector as

shown in Figure. 6-8. Actual measured Euclidean position of the target fruit in a

fixed camera frame and a robot base frame is as shown in Table 6-3.






Target





~I lu-Motiion trajectory


Figure 6-8: Performance validation for repeatability test.









Table 6-3: Actual Euclidean target position expressed in a fixed camera frame and
robot base frame.


Coordinate frame x (mm) y (mm) z (mm)
Fixed camera 139.7 12.7 838.2
Robot base frame -228.6 1066.8 -540.2


Table 6-4: Estimated Euclidean target position expressed in a fixed camera frame,
robot base frame, and robot tool frame.


Coordinate frame x (mm) y (mm) z (mm)
Fixed camera 65.0 4.0 831.0
Robot base frame -237.0 1060.0 -395
Robot tool frame 462.0 114.0 110.0


The estimated target position n?, expressed in a fixed camera coordinate

system, Illbi expressed in a robot base frame, and rAg~ expressed in the robot tool

frame is given in Table 6-4.

Initial position of the robot end-effector or a camera in-hand measured in a

robot base frame is given in Table 6-5.

Table 6-6 shows the final position of the robot end-effector measured in the

robot base frame at the end of control loop execution, while Figure. 6-9 depicts the

results of repeatability test in the robot 3D task-space. The success of the visual

servo controller is considered if the target fruit is inside the gripper at the end of

control loop execution.

As shown in Figure. 6-10 and Figure. 6-11 the controller exhibits low scatter

in repeated moves to a given target position in my-plane as well as in xz-plane, thus

indicating that the proposed controller is repeatable.

Table 6-5: Initial robot end-effector position expressed in robot base frame


Coordinate frame x (mm) y (mm) z (mm)
Robot base frame -655.0 785.0 -251.0




















Sr. No. x (mm) y (mm) z (mm) Success/failure
1 -238.0 1163.0 -561.0 success

2 -234.0 1121.0 -548.0 success

3 -215.0 1111.0 -555.0 success
4 -226.0 1117.0 -546.0 success

5 -227.0 1115.0 -541.0 success

6 -227.0 1118.0 -542.0 success

7 -228.0 1119.0 -548.0 success

8 -226.0 1120.0 -552.0 success

9 -229.0 1114.0 -546.0 success

10 -229.0 1116.0 -551.0 success
11 -219.0 1108.0 -553.0 success

12 -230.0 1118.0 -540.0 success

13 -220.0 1107.0 -544.0 success

14 -214.0 1106.0 -547.0 success

15 -231.0 1116.0 -536.0 success


72






Table 6-6: Final robot end-effector position expressed in robot base frame.


111. II~


!


't'
~*~
r
s;


-10*
I
JIXI


2:Y,
raor,
ear
ecg


IW


-armnr)


Figure 6-9: 3D robot task-space depicting repeatability results.



























*TS *


Figure 6-10: Repeatability in my-plane for the 3D target reconstruction based
visual servo controller.


xz-Plane Position Repeatability

-250 -235 -220 -205


73




xy-Plane Position Repeatability


-L


I 1060
~00


-220
x-Position (mm1)


x-Position (mm1)



Figure 6-11: Repeatability in xz-plane for the 3D target reconstruction based
visual servo controller.































Coordinate frame n (mm) y (mm) : (mm)
Target fruit # 1
Fixed camera 139.7 -25.7 787.4
Robot base frame -266.7 1016.0 -540.2
Target fruit # 2
Fixed camera -25.4 172.72 938.8
Robot base frame -68.6 1168.4 -375.1


llr~ ~Target



d:\\ ~:'Motion
Trajectory






Figure 6-12: Performance validation for experiment II.
Table 6-7: Actual Euclidean target position expressed in a fixed camera frame and
robot base frame.


6.4.3 Experiment II

In this section, a behavior of the control system under non-identical initial

positions of the robot tool frame is identified. A multiple target scenario was

constructed to verify the performance of the controller when only one target can

be seen by the fixed camera but the camera in-hand will have two targets in the

field-of-view after orientation. In this experiment the position of the target fruits is

kept constant while the initial position of the robot end-effector is varied as shown

in Figure. 6-12. Actual measured Euclidean position of the target fruits in a fixed

camera frame and a robot base frame is as shown in Table 6-7.









Table 6-8: Initial and final robot end-effector position expressed in robot base
frame.



Sr. No. Initial position Final position Success/failure
1 x (mm) y (mm) : (mm) x (mm) y (mm) : (mm)
2 549.0 896.0 -203.0 -75.0 1190.0 -384.0 success
3 40.0 979.0 61.0 -58.0 1188.0 -380.0 success
4 -715.0 748.0 47.0 -58.0 1194.0 -391.0 success


Target # 2


Figure 6-13: Performance validation for experiment III.

Initial and final positions of the robot end-effector or camera in-hand measured

in robot base frame are as shown in Table 6-8. Comparing the final positions of the

robot end-effector with the position of the target fruit # 2 in the robot base frame

it is clear that the controller performs satisfactorily under multiple target scenario.

6.4.4 Experiment III

This part of the experiment discusses the performance of the controller under

different target positions. In this experiment the position of the target fruits is

varied starting the robot end-effctor at different locations as shown in Figure.

6-13. Table 6-9 shows the actual measured Euclidean coordinates of the target

along with the initial and final positions of the robot end-effctor.

Under the diffrent target positions and robot end-effctor positions the

controller performs satisfactorily.


~~


1B


-r


Taree # 3









Table 6-9: Actual Euclidean target position expressed in a fixed camera frame and
robot base frame and initial and final robot end-effector position expressed in robot
base frame.


Sr. No. Target fruit position Target fruit position
(Fixed camera frame) (Robot base frame)
a (mm) y (mm) z (mm) a (mm) y (mm) z (mm)
1 139.7 -25.4 787.4 -266.7 1016.0 540.2
2 139.7 12.7 838.2 -228.6 1066.8 540.2
3 127.9 -171.6 782.55 -412.6 1011.4 -528.9


Sr. No. Initial position Final position Success
(Robot base frame) (Robot base frame) /failure
n (mm) y (mm) z (mm) a (mm) y (mm) z (mm)
1 -138.0 1015 -223 -272.0 964.0 -563.0 success
2 -538.0 554.0 -920.0 -240.0 900.0 -471.0 success
3 39.0 981.0 69.0 -416.0 952.0 -514.0 success



6.5 Conclusion

A 3D target reconstruction based visual servo control approach is proposed

for robotic citrus harvesting where a prior knowledge of the feature points is not

available and feature point matching does not perform satisfactorily thus rendering

feature point matching-based teach by zooming visual servo control impractical.

3D target reconstruction method utilizes statistical data of the target size along

with the camera intrinsic parameters to generate an estimate for the Euclidean

position of a target. A 3D depth estimation method performs satisfactorily for

targets attributing larger depths from the camera frame and hence the control

algorithm is switched from vision-based to IR-based when the camera in-hand

is closer to the target. The controller exhibits very high success rate in terms of

accurately reaching the target, and also the repeatability test shows good position

repeatability in my and xz-plane.















CHAPTER 7
CONCLUSION

7.1 Summary of Results

Automated robotic citrus harvesting yields superior fruit quality, which is

highly desirable for fresh fruit market. The presented work accomplishes two

important characteristics of the automated fruit harvesting system, namely to

locate the fruits on the three dimensional space and to approach and reach for the

fruit. Color thresholding-based technique is realized for target fruit identification,

whereas multi-camera visual servo control techniques, viz. teach by zooming

visual servo control and 3D target reconstruction based visual servo control, are

developed for 3D target position estimation and robot motion control.

A teach by zooming visual servo control approach is proposed for applications

where the camera cannot be a priori positioned to the desired position/orientation

to acquire a desired image before servo control. Specifically, the teach by zooming

control objective is formulated to position/orient an on-board camera based on a

reference image obtained by another camera. In addition to formulating the teach

by zooming control problem, another contribution of this work is to illustrate how

to preserve a symmetric transformation from the projective homography to the

Euclidean homography for problems when the corresponding images are taken from

different cameras with calibration uncertainty. Simulation results are provided to

illustrate the performance of the controller. Rotation and translation errors are

exponentially regulated to zero thus establishing the stability of the controller,

whereas the angular and linear control velocities are always bounded. Applications

of this strategy could include navigating ground or air vehicles, based on desired

images taken by other ground or air vehicles.










Practical limitation on the implementation of the teach by zooming visual

servo controller for the citrus harvesting application has been realized in the

fact that the feature points viewed by the fixed camera can be different than the

feature points viewed by the camera in-hand. Non-identical feature points between

the current image and the desired image would result in incorrect rotation and

translation information. Hence the teach by zooming visual servo controller can

be implemented where the feature point information is available a priori, i.e. this

controller is suitable for artificial targets.

A 3D target reconstruction-based visual servo control approach is realized

for robotic citrus harvesting where a prior knowledge of the feature points is not

available and feature point matching does not perform satisfactorily thus rendering

feature point matching-based teach by zooming visual servo control impractical.

Specifically, statistical data of the target size is used for 3D target reconstruction

for generating an estimate for the Euclidean position of a target. A 3D depth

estimation method performs satisfactorily for targets attributing larger depths

from the camera frame and hence the control algorithm is switched from vision-

based to IR-based when the camera in-hand is closer to the target. The controller

exhibits very high success rate in terms of accurately reaching the target, also the

repeatability test shows good position repeatability in my and xz-plane. 1\oreover,

the image-based visual servo control along with the Euclidean depth information

is used and hence the controller is robust to the target statistical data as well as

camera calibration parameters.

7.2 Recommendations for Future Work

One of the issues with teach by zooming visual servo controller is of consis-

tently identifying at least four feature points, which are necessary for homography

decomposition to acquire rotation and translation information between various

camera coordinate frames. This issue can be addressed by projecting an artificial










grid on the target and acquiring feature points on the grid. A prior knowledge of a

grid segment can also be utilized for target depth estimation. Non-identical feature

point matching issue can be resolved by using an artificial grid to consistently ob-

tain feature points on the target and controlling the trajectory of a camera in-hand

such that fixed camera as well as camera in-hand obtain identical feature points.

Multi-view photogrammetry techniques works under the assumption that

the four feature points are coplanar points. Image segmentation and texture

recognition techniques can be used to recognize different planes which would

help in defining the region of interest of the feature detection algorithm to ensure

feature points are coplanar. This would also make the tracker more consistent

and reliable to intensity variations in the scene. Another issue to be addressed

is to make sure that the points selected are not collinear. Also, the condition of

the four feature points required to be coplanar and collinear can be eliminated by

implementing the eight point algorithm proposed in [16], where the feature points

don't have to satisfy the mentioned constraints.

Target detection task is primarily performed by a fixed camera mounted on

a stationary robot base joint and camera in-hand is servoed based on the selected

target. Target detection efficiency can be enhanced by performing the target

detection by fixed camera as well as camera in-hand. Moreover, simultaneous

localization and mapping (SLAM) of target fruits can be achieved by performing

the detection task by a camera in-hand along with a fixed camera in order to

generate a three dimensional map of the scene for efficient harvesting.














APPENDIX A
OPEN-LOOP ERROR DYNAMICS

A.1 Rotation Controller

A rotation error-like signal, denoted by e, (t) E RW3, is defined by the angle axis

representation in (5-34) as follows:

e, = ue. (A-1)

Taking the derivative of (A-1) with respect to time

de, d(ue)
uOe + uO. (A-2)
dt dt

Murltiplying equation (A-2), by.~,,,,, (I+ )an sig roetiesof, a skew

symmetric matrix the following expression can be obtained:


(I + n2 ) jLO= ud (A-3)

where le R 3x3 denotes an identity matrix and ux denotes the 3x3 skew-symmetric

matrix associated with u(t).

Similarly, multiplying equation (A-2) by (-u ,) we get,

d(ue)
(-u ) dt = GO.(A4

The angular velocity, expressed in the current camera frame, is defined as
follows:

coex = R(u, B)R (u, 8). (A-5)









From (5-36) and utilizing the properties of a skew symmetric matrix the

expression for LJcx in (A-5) can be written as follows:


Lc x = sin O fi + vi x + (1 cos 8) (ux it) (A-6)

where LJcx E R3x3 denotes the 3 x 3 skew-symmetric matrix associated with LJc(t)-

Utilizing the properties developed in (A-3) and (A-4), the expression for

angular velocity we, can be obtained as,


LJe =
= ue + [sin OI+ sin C2-ux (1
2 2

= [I + sin C2U +1 (1 Sin 8)11 ] (A-7)
2 2 dt




where the Jacobian matrix L, (t) E RW3x3 is defined as follows:

008
L, = (L ')-l = I +- smnC2-Il+ (1-_Sin 8)Z2. (A-8)
w 2 2

In equation (A-7) the sin c(0) term is given by (A-8) as,


sin(0)
sin c(0) = (A-9)

Based on the camera extrinsic parameters given in (5-33) and expression

(A-7), the open-loop error dynamics can be obtained as follows:

e, L, R, w,. (A-10)










A.2 Translation Controller

The difference between the actual and desired 3D Euclidean camera position,

denoted by the translation error signal e, (t) E RW3, is defined as


e, = me mde (A-1l


where me (t) E RW3 denotes the extended coordinates of an image point on xT

expressed in terms of FT and mde E RW3 denotes the extended coordinates of the

corresponding desired image point on xT in terms of Fed given in (5-52) and (5-53),

respectively.

Taking the derivative of e, in (A-11) with respect to time


X1 X1
~z~
YI' Y,
z~zz




X1


S1 Y

O 1 Zz


1
e,=
'"ZT


1
e, =v Z3


X2
Z1
X, Y,
Z,
X 3


Zz
Y 2
Zz + '
Zz


1
e,=
Z3 z


1
oc -
ZT


c,





(A-12)


e, = LRv, + A,, (L, [t,] x + L,,) Re,


1)


Vc -- [mi] x










where (5-33), (5-44), and the following fact have been utilized [26]


mi = -veu + [ml]Ic c. (A-13)


In A-12, the Jacobian-like matrices L, (t), L,, (t) E RW3"3 are defined as follows:

1 0 -mer

L, n 0 1 -m2(A-14)

00 1


-melme2 1 m 1l -me2

L,, n -1- 2Rlme2 mel (A-15)

me2 --mel 0















APPENDIX B
TARGET IDENTIFICATION AND FEATURE POINT TRACKING
ALGORITHM

B.1 Target Detection



// PROGRAM START




// What this program does:

// 1) Detects target (citrus) based on color thresholding principle

// for the image taken by fixed camera as well as camera in-hand.

// 2) Estimates the target region area along with the diameter and depth of
the target.

// What is the input to this program:

// The input is sequence of images (i.e. real-time video) taken using two
cameras

// What is the output of this program:

// Target region area, target diameter, estimate of target depth

// The code given below does not include the entire program used for the
autonomous

// robotic citrus harvester, but shows the important functions.



#include "StdAfx.h"

#include "ImagProcFunctions.h"

#include









#define PI 3.141592654

enablingig debugging has NO notable effect on processing time

#define DEBUGTHRESH

#define DEBUGHOLES

#define DEBUG _ISO

#define DEBUG _REGION

#define DEBUGCENT

#define DEBUGPER

#define DEBUGEDGE

#define DEBUGPOSSCTRS

extern TBZController Controller;

extern img~proclDoc img~proc;

double K~d = 0.025;

double K~r = 1.0;

double cost;

double minCost = 99999999999;

double CFmaxRadius[150];

double CiHmaxRadius[150];




// This one Function calls all the need functions to perform Centroid based

object detection

// NOTE: Threshold function needs to be called before this


int ImagProcFunctions::CFCentroidBasedDetectinusge char* CFlpDib,

int MinRegionArea)









if(img_proc.FLAG TRACK 0 _LOCKED = 1)


//find the regions in the thresholded image -> results are in

m Regions

int numRegionsCF = CFFindRegions(CFlpDib, m_1pBiThrshImgCF,

MinRegionArea) ;

//find the centroid of the regions -> results are in m CentCtrs
m nNumCentroidsCF = CFFindRegionCentroid(CFlpDib) ;


return m nNumCentroidsCF;






// Threshold image using Color Difference Signals

// It also does the following

// intializes both the Binary (used in segmentation) and Gray Scale 8 bit

images


// pure orange color is RGB= (255,102,0)

// less red makes the orange color appear brown

// more green makes the orange color appear yellow

// = > yellow is RGB= (255,255,0)

// more blue makes the orange color appear pastel


// new parameter values on 1/15/04

// CrU=90, CrL=22; // red<->brown

// CgU=25, CgL=-25; // yellow<->red









// CbU=-5, CbL=-220; // pastel<->red


// Results are placed in: m 1pBiThrshImg, m 1peleanedlmg,

// and m_1pGrayThrshImg


void ImagProcFunctions::CFColorDiff~hreshold(unsge char* 1pDibCF, int

CrU, int CrL, int CgU, int CgL, int CbU, int CbL)


int i;

int pixel;
int blue, green, red;

double Cr, Cg, Cb;



for(pixel=0, i=0; pixel

blue = i-

green = i+1;
red = i+2;

// Is the color difference in the right range?

//check red
Cr = 0.721*1pDibCF [red] 0.587*1pDibCF [green]

0.1 14*1pDibCF[blue];

if( (Cr < CrL) | | (Cr > CrU) )

I~ pihs~gF[ie]=0

m _1p~riThrshImgCF [pixel] = 0;









else


//check green
Cg = -0.299*1pDibCF [red] + 0.413*1pDibCF [green]
0.114*1pDibCF [blue];

if( (Cg < CgL) || (Cg > CgU) )

I~ pihs~gF[ie]=0
m _1p~riThrshImgCF [pixel] = 0;



else


//check blue
Cb = -0.299*1pDibCF [red] 0.587*1pDibCF [green]
+ 0.886*1pDibCF[blue] ;

if( (Cb < CbL) | | (Cb > CbU) )

I~ pihs~gC pxl=O
m_1p~riThrshImgCF[pixel] = 0;



else


m_1pBiThrshImgCF [pixel] = 255;


// it can take up to an extra 6-8ms to do this calcula-


tion for the image









m_1pGrayThrshImgCF [pixel] = (unsigned char)(0.299*1pDibCF [red] +
0.587*1pDibCF [green] + 0.114*1pDibCF[blue] );

//this is just for display

//color the pixel in the original image
#ifdef DEBUG _THRESH

1pDibCF [blue] = 255;
1pDibCF[green] = 0;
1pDibCF [red] = 0;
#endif








// Aqueu-basd pin-i rui f cl ia un onetvy









// Aid quuebaed palint-ilruiefr oo mg sng4cnetvt




// -> coords to these regions can be found in m_1pRegeoords
// -> the region's size (# of pixels) is in m_nRegSize
// -> the memory offset of the region's pixels is in m_nRegMemOffset

// -> the pixel closest to the origin for each region is in m nRegMinCoord