多连杆冗余移动机械臂建模与运动规划

0，，，［1］.，：，［2］；，［3］.，.，.，，23［4，5］.1，2，1，2，1，2（1.，200237；2.，310018）：，，.，，，.，.，，.：；；；：TP18：A：1001－7119（2010）02－0200－06ModelingandMotionPlanningforMulti-LinkRedundantMobileManipulatorZHANGBotao1，2，LIUShirong1，2，SHIXianpeng1，2（1.ResearchInstituteofAutomation，EastChinaUniversityofScienceandTechnology，Shanghai200237，China；2.ResearchInstituteofAutomation，HangzhouDianziUniversity，Hangzhou310018，China）Abstract：Todealwiththepotentialfieldtrapproblemofartificialpotentialfield（APF）andtheworkspacelimitationofplanarmanipulator，aglobalmotionplanningmethodisproposedforredundantmobilemanipulator.Inthisstudy，repul-sivepotentialfunctionismodifiedbyintroducingasafetyfactorintoAPFtopreventmobilemanipulatorfromgoalun-reachableproblemandtheparametersofpotentialfieldmodelareoptimizedbyquantumgeneticalgorithm（Q-GA）.Toimprovethepositioningaccuracy，anewparticleswarmoptimization（PSO）algorithmisappliedtotheinversekinematicsproblemofmobilemanipulator.SimulationresultshowsthattheproposedmethodcaneffectivelyovercomethedefectsofAPFmodelandreducethetimeanderrorwhiletheend-effectorreachesthedesiredposition.Keywords：mobilerobot；artificialpotentialfield；quantumgeneticalgorithm；particleswarmoptimization：2009-09-15：（60675043）；（2007C21051）；（KYS09150543）：（1982－），，，，。E-mail：[e-mail].26No.2Mar.201026220103BULLETINOFSCIENCEANDTECHNOLOGY21Fig.1Assignmentoflinkframesforthemobilemanipulator［6，7］，，，，.，，.，.，Q-GA，，Arm-PSO..，，Q-GA，；，Arm-PSO、.，.、.：iRobotSUGVEarly、PackBot.1，，，，，。，，，，，1。1.11，Ts，Tr，Tg，T0～T516，T6.：（），、、alength，awidth，aheight.z0，yi，i=1，2，…5….，.w1，w2，w3，w4，w5，w6，h，、：sTr=cosφ-sinφ0xrsinφcosφ0yr00100001!#$%%%%%%%%%&，topT0=10000100001awidth+h0001!#$%%%%%%%%%&.：rT0=cosθ10-sinθ1w1cosθ1sinθ10cosθ1w1sinθ0-1000001!#$%%%%%%%%%&，，φ，xr，yrx，y，θ11.：θi=0，i=1，2，…，6.，yi.，.201266i=2Σθi≤π2.，，［－π/2，π/2］，ki=2Σθi≤π2，k=2，…，6.，，T0，T0（pgx，pgy，pgz），，：p6x=w1+6i=2Σwicos（ij=2Σθj）p6y=0p6z=6i=2Σwisin（ij=2Σθjjjjjjjjjjjjjjjjjjjjjj）.（1），θji.p6x，p6y，p6z.，，，：ki=2Σθi≤π2，k=2，…,6.，，：f（θ1，θ2，…，θn）=ki=2Σθi.（2）1.2Arm-PSO，。：，。，，。，。，，。PSO，，、，，。，PSO，［8］PSACO，（Arm-PSO）.vjt+1=ωtvjt+c1ξ（pjt-θjt）+c2η（pgt-θjt），（3）θjt+1=θjt+vjt+1，（4）ωt=（ωmax-ωmin）×（tmax-t）tmax+ωmin.（5），vjttj，θjt=［θj2，θj2，…，θj6］tj，2～6，pjtj，pgtt，ω，c1c2，ξ、η.，fdis=（p6x－pgx）2＋（p6y－pgy）2＋（p6z－pgz）2姨.（6）22.1Khatib，.，.［9］GNRON，.，，，.，，，EE（eg，ei，q）＝Eatt（eg，q）+l乙Erep（eobsi，q）鄣l=Eatt（eg，q）+ki=1ΣErep（ei，q）.（7），q=［x，y，θ］t，eg，eobsii，ei，i=1，…，k，Eatt（eg，q）；ki=1ΣErep（ei，q）2022.E（eg，ei，q），F=-荦E=-荦Eatt（eg，q）-ki=1Σ荦Erep（ei，q）.（8）E，［9］Eatt（eg，q）=12α1ρ2g（q）.（9），α1，ρg（q），ρg（q）=||q-eg||，eg.，，0，。，，ρg（q）≥ρλ，Eatt（eg，q）=αρ2g（q）ρg≥ρλ－α（1/ρg（q）-ε姨＋γρg（q）ρg＜ρλ姨（10）ρg（q）＜ρλ，Eatt（eg，q）=α（1/ρg（q）-ε姨+γ.（11），ρλ=10R，R，γ.，，，，ρg（q）-ε姨，ε，，.［9］Erep（ei，q）=β2（1ρ-ε-1ρ0）2ρmg（q）ifρ（q，qminobs）≤ρ00ifρ（q，qminobs）≤ρ0≤≤≤≤≤≤≤≤≤（12），m，1≤m≤4；ρ（q）q；β；ρ0.：Eatt（eg，q）=-2α（q-qg）ρ≥ρλ-α/（ρ（q）-ε）－γρ≥ρλ≤，（13）F軋rep（ei，q）=βρm（q-qgoal）||q-qminobs||（1ρ-ε－1ρ0）q-qminobsρ+m2（1ρ-ε－1ρ0）2ρm-1（q-qgoal），ifρ（q，qminobs）≤ρ00ifρ（q，qminobs）＞ρ0≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤≤.（14），qminobs.F軋rep=ki=1ΣErep（ei，q）.（15）2.2Q-GAα，β，ρ0F＝0，.，l.，［10］.，，Q-GA.，.Q-GA，Q-GA.［11］Q-GA.：（1），；（2），.（16），fit1，，fit1fit1=alength=astep×l.（16）（17），fit2，，fit2fit2=bρ0.（17），a，b，length，step，l.，getgoal，missgoal.Fit=fit1+fit2，getgoal0，missgoa≤l.（18）3：（1），.（2），.（3）.：、、alength=0.35m，.20326awidth=0.35m，aheight=0.2m..w1=0.1m，w2=0.2m，w3=0.05m，w4=0.1m，w5=0.2m，w6=0.05m，h=0.145m.3.1，10m×10m，、x-y，1／2，，2、3。，2.，，。，，3。，，（Experience），，1。1，：（times）（mean）（variance），（LBR）；，mean，variance，LBR.3.2，5m×5m×5m，，3.，，，Arm-PSO，.，，，，，θ1=π，θ2=-π／3，θ3=0，θ4=π／2，θ5=π／6，θ6=0.4，1Table1SimulationresultscomparisonofpathplanningMethodTimesLBRMeanVariance1Experience114.7900.61200.0173Q-GA414.4410.84051.501×10-52Experience316.4070.6540.0472Q-GA516.4480.7594.279×10-53Experience510.0100.9580.0626Q-GA510.0001.2191.728×10-64Experience55.6281.9330.1797Q-GA55.6182.1614.33×10-55Experience314.4300.4690.0685Q-GA514.7830.8200.00242Fig.2APFtrapleadstolocalminimumgoalstart3Q-GAAPFFig.3MotionplanningbasedonAPFwithQ-GAstartgoal4Fig.4Globalmotionplanninginthree-dimensionalspace3020100010203001020302042，。3.3、。，，。5，，.5Arm-PSO，200.20，，0.3s1×10-5，1×10-7；5s1×10-5，1×10-14，。2、，210，4000，20.Ts20，N，AC，T。.21×10-4，，.1×10-5，，1-（0.05）2=99.75%.，［6］（mountainclimbing，MC），Arm-PSO：1000，10；MC［6］，Arm-PSO5.Arm-PSO，，Arm-PSOMC，.N，AC，T.4，，，Q-GA，Arm-PSO..，，，，、.：［1］，，.［J］.，2003，25（5）：465-470.［2］YamamotoY，YunXP.Coordinatinglocomotionandmanipulationofamobilemanipulator［C］／／Proceed-ingsofthe31stConferenceonDecisionandcontrol.Arizona，USA：IEEEPress，1992：2643-2648.［3］SerajiH.Motioncontrolofmobilemanipulators［C］／／2、Table2Iterationtimes，positioningtimeandaccuracyTsAC／mArm-PSONT／s201×10-2730.032201×10-33560.079201×10-44360.344181×10-59230.7035Fig.5Positioncontrolofmobilemanipulator0.450.400.350.300.250.200.150.10.050010020030040050060070080090010003Arm-PSOMCTable3ComparisonbetweenQ-GAandGAArm-PSOMCNT／sNT／s10.00281240.0781372.3820.0298310.0311011.7630.0031190.0741933.4540.0261370.03511482.6550.000254240.2972564.58AC／m（232）.205264BDFEKFFig.4TheratioofwidthbetweenBDFandEKFx10.80.60.40.200102030405060709080100x1：Ratio（BDF/EKF）x1