Robotic Arm Calibration and Control 6-DOF Powerball LWA 4P

179 Pages·2014·2.77 MB·English

RENSSELAERPOLYTECHNICINSTITUTE
Robotic Arm Calibration and Control
6-DOF Powerball LWA 4P
CurtisBradley
January14,2014
1
1. ACKNOWLEDGMENTS
IwouldliketoexpressmygreatappreciationtoProfessorTrinkleforgivingmesuchcomplete
accesstohislabandrobotalongwithsupportforallaspectsofmyresearch. Histimeandgen-
erositymadethisworknotonlypossible,butenjoyable.
IamgratefultoProfessorBevilacquaforhelpingtomakethisopportunitypossible.
I would like to express my deepest gratitude to Bryant Pong for his tireless pursuit of captur-
ingdataandtherecording,formatted,andoutputtingthereof,withoutwhichIwouldnothave
anythingtoperformresearchon. HisprogrammingskillinvariouslanguagesandROSexper-
tisesolvedproblemsinexecutionofmycodeIcouldnothavehopedtosolveinonesemester.
Thankyouforallyourenthusiasticcodingthatmadethisresearchpossible.
IappreciatethetechnicalsupportreceivedfromJesseHayesatSchunktokeepthearmrunning
smoothly.
ThankyouJunDongforalwayslendingahandwithPythoncoding,generalconstructionand
wordsofencouragement.
ThankyoutoShuaiLifordonatinghismonitoraswellasPythonscriptinghelp.
2. ABSTRACT
The specifics to performing kinematic calibration on the Schunk Powerball Light Weight Arm
(LWA)4.6usingaserialapproachstatingwithCircularPointAnalysis(CPA)followedbyaGauss-
Newtonerrorminimizationmethodarecovered. Thisapproachallowsforasimplifiedregressor
fortheGauss-Newtonmethod.Whilethisserialtechniqueofcombiningthesetwovastlydifferent
parameteridentificationmethodsiscompletelydevelopeditisnotfullynotoptimizedfornumer-
icalaccuracy.Additionallyasolutiontotheinversekinematicsiscompletelysolvedandpresented
asitisneededforfollow-onresearch.
2
CONTENTS
1. Acknowledgments 2
2. Abstract 2
3. Introduction 4
4. Kinematics 5
4.1. ForwardKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
4.2. InverseKinematics . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 8
5. Control 11
6. Kinematic Calibration 16
6.1. CirclePointAnalysisEstimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 16
6.2. Gauss-NewtonEstimation . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
7. Results 27
8. Conclusion 28
A. Inverse Kinematics Matlab Code 30
B. Circular Point Analysis Matlab Code 35
C. Gauss-Newton Minimization Matlab Code 46
D.Jacobian for DH Parameters Matlab Code 54
LIST OF FIGURES
3.1. PowerballLWA4.6. . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 5
4.1. NominalPowerballZeroConfigurationDiagram . . . . . . . . . . . . . . . . . . . . 7
4.2. InverseKinematicsExampleSolution . . . . . . . . . . . . . . . . . . . . . . . . . . . 12
5.1. SystemOverview . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 13
5.2. IndividualJointMotionProfile . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 14
5.3. JointAngleData . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 15
6.1. CircularPointAnalysisResults . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 21
6.2. GaussNewtonSolution . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 25
6.3. CartesianComponentsofCameraDataVs.FK . . . . . . . . . . . . . . . . . . . . . . 26
6.4. 3DplotofCameraDataVs.FK . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 26
6.5. Gauss-NewtonMeanError . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 27
3
LIST OF TABLES
4.1. NominalDHParameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 6
4.2. TestJointAngles(radians) . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 10
4.3. IKSolutionJointAngles . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 11
6.1. FinalDHParameters . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . . 24
3. INTRODUCTION
Thepurposeofthisindependentstudywastogainanin-depthfamiliaritywithacurrenthigh
endroboticmanipulator. TherobotusedwasaSchunkPowerballlightweightarm(LWA)4.6,
thelatest6-Degree-of-Freedom(DOF)armfromSchunk,leadersinmanipulatordesign[1]. In
order to gain this understanding two main tasks were accomplished, namely formulating the
fullinversekinematicsandperformingthekinematiccalibration. Theinversekinematics(IK)
solutionisnecessaryforfastoperationeverywhereintheworldcoordinatesandalsorequired
forsoftcontrolalgorithmslikeimpedancecontrol. Themodelcalibratedisaparametrickine-
maticmodelusingDenavit-Hartenberg(DH)parameters. Thekinematiccalibrationisthefirst
calibrationtypethatmakesalltheproceedingcalibrationsmoreaccurate,likeinertialcalibra-
tionandfrictionidentification.
ThePowerballisanarmdesignedwithindustryandresearchinmind.Ithasahighgearratiofor
eachofitsaxesmakingitinsensitivetogravitationalloading,butsincethegearingisharmonic
andtheratioisnottoohighitcanstillbebackdrivenforimpedancecontrolandfrictioniden-
tification. Thelightcompactandmodulardesignallowedustorecoverfrommultiplefailures
duringinitialset-up. Axis4failedshortlyafterthesystemwasfunctionalandcostusabouta
monthoftestingwaitingforsparepartstobecomeavailable.Asubsequentfailureoftwoaxes,1
and2,wouldhaveendedresearchonthesystemforthesemester,butwewereabletodisman-
tle both the original arm and a replacement arm to re-construct a completely functional arm
withoutsendingeverythingbacktothemanufacturergivingusjustenoughtimetocollectdata
beforetheendofthesemester. Oneoftheonlylimitationsapparentsofaristhelowmaximum
speedmakingcontrolschemeslikeminimum-timecontrolmoot. ThePowerballcanbeused
withanycontrollerthatsupportsmultipleCANOpendevices,wechosetousetheRoboticOp-
eratingSystem(ROS).ROSallowedustoseamlesslyintegratethecameratrackingsystemwith
therobotcontrollerandwillallowfurtherextensionsofthesysteminthefuture. Ahighreso-
lutionrenderingofamodelusedtohelpsimulateandvisualizethePowerball,showninfigure
3.1(a).
ThesolutiontotheIKproblemwassolvedusingthenominalDHparametersdeterminedfrom
themanufacturersdimensionsandfromactuatingthejointstodetermineoffsetsandaxisdi-
rection. Using the nominal parameters allow work in parallel with the kinematic calibration.
Thesolutionisgenerallyapplicableinsidetheworkenvelopeandincludestopologyinforma-
4
(a) SchunkPowerballLab (b) SchunkPowerballRenderedModel
Figure3.1:PowerballLWA4.6
tionfortheconfigurationoftherobotsincethereareingeneraleightsolutionstothePowerball
IK. The IK solution also removes joint solutions from the eight if they are outside joint limits.
Furtherthereisanoptionalinputforthelastknownjointpositionsoonlytheclosestsolution
tothethatpreviouspositionisreturnedtosimplifytrajectorygeneration.
Ajointtrajectoryusingallsixjointsfromonetosixcreatedanendeffectormotionprofilethat
couldbeusedforbothCircularPointAnalysis(CPA)orGauss-Newtonerrorminimizationcan
beusedforkinematiccalibration. CPAwasusedtoorienttheendeffectortrackingdatacap-
turedinthecameraframetotherobotframewheretheDHparametersaredefined. Oncethe
dataisorientedGauss-NewtonerrorminimizationcanbeusedtodirectlysolveforalltheDH
parametersatonetimewithoutaddinganextradegreeoffreedom. Minimizingtheerrorbe-
tweentheforwardkinematics(FK)andthetrackingdatarequiresthesolutionofthesymbolic
versionof the forward kinematics whose complicationis multiplied by every additional kine-
maticframe. ThissimplificationalsoextendstotheJacobianallowingthesimplifiedversionof
theseequationstobecomputationallyfaster.
4. KINEMATICS
Kinematics defines the motion of bodies and in the case of a rigid body robotic manipulator
itdefinestherelationshipbetweenjointconfigurationsandaworkingreferenceframe. Inthe
5
caseofForwardKinematics(FK)mapsjointanglestoapositionandorientationintheworking
referenceframe. TheInverseKinematics(IK)mapsagivenpositionorientationtoasetofjoint
angles. Dependingontheconfigurationoftherobotthemappinginonedirectionorthean-
othermaybemathematicallystraightforwardortheremaybenoclosedformsolutionorthere
could be multiple solutions or no solutions at all. The FK is required for the kinematics cali-
brationwhiletheIKisrequiredforadvancedcontrolalgorithmsanddirectworkspacemotion
planning.Robotstructuresareoftendescribedwithanorderedlistofjointtypes,"R"forarevo-
lutejointand"T"fortranslationaljoint,sothePowerballisanRRRRRRrobotbecauseallsixof
itsjointsarerevoluteshowninfigure4.1.
4.1. FORWARD KINEMATICS
Deriving the FK can be done in a variety of ways each with advantages and disadvantages,
but they all start with a parameterization for the joints positions and orientations. Denavit-
Hartenberg(DH) parameters are one parameterization used in a vast amount of research and
industry. DHparametershavebeenusedthroughoutthisresearchtoallowmorestraightfor-
wardcomparisonstootherresearchinthearea. TheDHparametersareθ,d,a,andα,withθ
theinputvariableforrevolutejointsandd theinputvariablefortranslationaljoints. TheDH
parametersdefinethetransformationfromonejointtothenextjointinthekinematicchain,
θ defines rotation between the z-axis, d defines the distance along the z-axis between joints,
a definesthedistancealongthex-axisbetweenjoints,andαdefinestheanglebetweenz-axes
of joints. Each of the 6 degrees of freedom of the Powerball are a revolute joint making the θ
DHparametertheinputvariableforeachsetoffourDHparametersthatdefineeveryjointaxis.
EventhoughθisanindependentinputvariableaDHparameter,θ ,muststillbeaddedto
offset
θtoformtheexactFKbasedontheactualjointanglesdefinedbythejointaxismotorencoders.
InthecaseofthePowerballtheDHparameterrepresentationisnotuniquebecauseaxespairs
one/two,three/four,andfive/sixarenearintersectingadditionallyaxestwoandthreearenearly
parallel[2].
JointAxis θ d a α
offset
1 0 0.205m 0 −π
2
2 −π 0 0.350m π
2
3 −π 0 0 −π
2 2
π
4 0 0.305m 0
2
5 0 0 0 −π
2
6 0 0.075m 0 0
Table4.1:NominalDHParameters
6
Figure4.1:NominalPowerballZeroConfigurationDiagram
 
cosθ −sinθ cosα sinθ sinα a cosθ
i i i i i i i
i−1T =sinθi cosθicosαi −cosθisinαi aisinθi [3] (4.1)
i  0 sinα cosα d 
 i i i 
0 0 0 1
7
SincethePowerballiscomposedofaseriallylinkedsetofjointsthecompleteFKresultsfromthe
straightforwardmatrixmultiplicationofeachsuccessivejointtransform, forany6-DOFserial
robotthatisequation4.2.
0T =0T 1T 2T 3T 4T 5T (4.2)
6 1 2 3 4 5 6
The4x4homogeneoustransformmatrix,0T canbethoughtofasdividedintoanSO(3)rotation
6
matrix,R ,atranslationvectort ,aperspectivevector,p ,andascalarscalefactor,s .
3x3 3x1 1x3 1x1
Thecompositionof0T isseeninequation 4.3.Forrigidbodyoraffinetransformationsthatin
6
thiscaserepresenttherobotaxestransformationsp equals0 ands equals1.
1x3 1x3 1x1
(cid:183) (cid:184)
R t
0T = 3x3 3x1 (4.3)
6
p s
1x3 1x1
Sogivenallsixjointvalues,[θ ...θ ]andthecorrectsetoftwenty-fourDHparameters,theFK
1 6
solutionisfullydefinedbythehomogeneoustransformmatrix,0T whichdefinestheposition
6
andorientationoftheend-effectorinthesixthaxisofthePowerball. Thegeneralformulation
usingalltwenty-fourDHparametersfor0T willresolvetoamanageablecomplexityasthere
6
areonlysixvariablesandformostrobotsthenominalDHparametershaveseveralzeroentries.
IftheFKmatrix,0T ,retainsasymbolicrepresentationforthenon-zeroDHparametersitwill
6
belargeandifalltwenty-fourDHparametersretaintheirsymbolicrepresentation0T becomes
6
verylarge,morethan25,000charactersperline.
4.2. INVERSE KINEMATICS
Theinversekinematicssolution(IK)willbenecessaryforfastdirectworkspacetrajectorysolu-
tionsandadvancedcontrolalgorithmslikeImpedancecontrol.Theinversekinematicsproblem
istosolveforthejointanglesgiveaspecificrobotendeffectorpositionandorientation.Fora6-
DOFrobotwithasphericalwristthereisaclosedformsolutionforthatproblem;thePowerball
armisofthisclassofrobot.Forsimplicityofimplementationthissolutionusesahomogeneous
transformationmatrixasaninputalongwithanoptional"previousjoint"anglevectorthatwill
givethesolutionclosestinthevectornormsensetothatprevioussetofjointanglesforeasier
(cid:114)
trajectoryimplementation. ThecompletefunctionalimplementationusingMatlab isinap-
pendixAandsolvesnearlyinstantaneously.
The outline for this solution is to solve joint 3 then joints 1 and 2 simultaneously, then solve
joints 5, 4, and 6 as a set of Euler angles which have a straight forward standard solution. To
solve angle 3, first 0T must be moved to the center of the spherical wrist by translating the
6
referenceframebythedistanceoftheconstanttoolvectorlengthinthedirectionofthecurrent
orientationoftheend-effectorusingequation4.4.
 
0
dx=0T6(1:3,1:3) 0  (4.4)
d
6
8
Similarlythevectorfromtherobotbasetothecommoncenterofjoints1and2,shoulderjoint
center,mustalsoberemovedfromtheend-effectorvectoryieldingthevectorfromthecenter
oftheshoulderjointtothecenterofthesphericalwristfromequation4.5,
 
0
delbow =0T6(1:3,4)−dx− 0 . (4.5)
d
1
With the distance d all three sides of the triangle formed by the two primary sections of
elbow
thePowerballarm,a andd ,andthevectord canbeusedtosolvethelawofcosinesfor
2 4 elbow
theta asinequation4.6,
3
(cid:195) (cid:33)
a2+d2−|d |2
θ =± π−arccos 2 4 elbow . (4.6)
3 2∗a ∗d
2 4
Theangleθ ,the"elbow",hastwosolutionsclassicallytermed"elbowup"and"elbowdown",
3
thesesolutionsareindependentofallotherjointsandsowassolvedforfirst. Thetopologyfor
thesolutionisretainedasabinarynumberwhereeachbinarydigitstoresthetopologyofthat
solution,inthecaseofthata a1intheseconddigitmeansthesolutionhastheelbowup.This
3
allowsforbettertrajectoryplanningtoavoidunnecessarilyjumpsbetweentopologies.
Themethodchosentosolvejoints1and2usesthesecondPaden-Kahanstandardsubproblem
andhastwosolutionsforeachofthetwosolutionsforθ ,foratotaloffoursolutionssofar.The
3
secondPaden-Kahanstandardsubproblemalsocalledjustsubproblem2simultaneouslysolves
theproblemofrotatingavectortwoconsecutiverotationsabouttwoarbitraryunitvectorsto
anothervector[4]. Thearbitraryrotationunitvectorsaretheaxesforaxis1whichisalsothez
axisandtheotherisaxis2,theyaxis.Tooutlineasolutiontosubproblem2theaxesandvectors
neededtosolveitareshowninequation4.7,
 0   0   0   −d sinθ 
→− 4 3 →− −−−−→
axisk1= 0 , axisk2= 1 , p = 0 + 0 , q =delbow. (4.7)
1 0 a d cosθ
2 4 3
→−
Subproblem2solvesfortherotationof p,thezeroconfigurationofthevectorfromtheshoul-
→−
dertothewrist,firstrotatedaboutk thenaboutk whichwillalignitwith q,theactualvector
1 2
fromshouldertowrist. Thismustbedoneforbothθ solutions. Thespecificnumericalmeth-
3
odsusedtosolvesubproblem2areclearlylabeledinappendixAasthefunction"subproblem
2".Thetwosolutionsaretraditionallytermed"shoulderleft"and"shoulderright",thesetopolo-
giesarestoredintheleastsignificantdigit,theright-most-bit,1forrightshoulder.
Withtheanglesforthefirstthreejointstheend-effectororientationcanberotatedbacktothe
base coordinate system which will then only represent the spherical wrist transform, also an
Euleranglerotation.TheSO(3)rotationmatrixformedbythesethreerotations,roll,pitch,roll,
9
hasastraightforwardclosedformsolutiondirectlyfromtherotationmatrix,4R ,equationfrom
6
thehomogeneoustransformationmatrix,.
4R =(cid:161)0R1R2R (cid:162)T 0R (4.8)
6 1 2 3 6
 cosθ cosθ cosθ −sinθ sinθ −cosθ sinθ −cosθ cosθ sinθ −cosθ sinθ 
4 5 6 4 6 6 4 4 5 6 4 5
4R6= cosθ4sinθ6+cosθ5cosθ6sinθ4 cosθ4cosθ6−cosθ5sinθ4sinθ6 −sinθ4sinθ5 
cosθ sinθ −sinθ sinθ cosθ
6 5 5 6 5
(4.9)
Thesolutionisthentouseequation4.8onthegiven0R rotationmatrixportionofthetrans-
6
formmatrix0T withtherotationmatricesforthefirstthreejointsalreadyfound. Theninthe
6
(3,3)positionfortheSO(3)rotationmatrixfromequation4.9isthecosineofjointangle5.While
theθ =atan2((2,3),(1,3))andθ =atan2((3,2),−(3,1)). AllEulerangletransformshave2so-
4 6
lutionsandinthiscasetheyrepresentthe"wristup"and"wristdown"configurations, multi-
plyingthetotalnumberofsolutionsto8ingeneral,limitedonlybyjointlimitsandsingularities.
Thewristuptopologyisrepresentedbya1inthemostsignificantbit,theleftmostbit.
ToillustratetheIKsolution6randomanglesaregeneratedwithinthejointlimitsthenusethe
FK equation 4.2 to generate the homogeneous transformation matrix, 0T , which is the input
6
to the IK function. One set of sample joint angles and the corresponding solution are in the
followingtablesforreference.
θ θ θ θ θ θ
1 2 3 4 5 6
-1.950 -0.717 -2.081 2.575 1.634 0.938
Table4.2:TestJointAngles
 −0.834 −0.118 −0.538 −65.739 
 −0.200 0.975 0.095 −56.379 
0T = 
6  0.514 0.187 −0.837 468.762 
 
0 0 0 1
The complete set of 8 solutions all exist for this particular set of starting joint angles and are
shownintable4.3. Theillustratedsolutionisshowninfigure4.2wheretheactualsolutionis
highlightedinblueandthesubplottitlesarethebinarynumbersrepresentingthetopologyof
eachsolution. Everysolutionshowstheend-effectorinthesamelocationandorientation,but
therobotusesadifferentsetofjointanglestoachievethepose. WiththePowerballthissimple
linedrawingisdifficulttoseethedifferencebecausetherearethreepairsofintersectingaxes,
makingleftshoulderandrightshoulderlookidentical.
10