6自由度水下机器人动力学分析与运动控制

CHINESEJOURNALOFMECHANICALENGINEERING43720077Vol.43No.7Jul.20076*(100083)6TP242.20*[1][2-3][4-5]6∗500750088632001AA42120020060709200701311OxOyOz12O'x'O'y'O'x'O'z'O'x'O'y'1243788coscoscossincossinsinsinsincoscossinsincoscoscossinsinsincossincossinsinsincossincoscosvϕθφϕϕφθϕφϕφθϕθϕφϕφθϕφφϕθθθφθφ−++⎛⎞⎜⎟=+−+⎜⎟⎜⎟−⎝⎠T(1)1tansintancos0cossin0sin/coscos/cosθωφθφφφφθφθ⎛⎞⎜⎟=−⎜⎟⎜⎟⎝⎠T(2)φθϕ3T()uvw′=uT()pqr=Ω322.1T(sincossincoscos000)Gθθϕθϕ⎛⎞=−⎜⎟⎝⎠FGτ(3)G2.2bbbbbbsincossincoscoscossincoscoscoscossinsincossinBzyxzyxθθϕθϕθϕθϕθϕθθθϕ−⎛⎞⎜⎟⎜⎟⎜⎟⎛⎞=⎜⎟⎜⎟−⎜⎟⎝⎠⎜⎟+⎜⎟⎜⎟−−⎝⎠FBτ(4)BTbbb()xyz2.36644T1T2O'x'y'x'z'x'T3T4O'y'z'y'x'y'T5T6O'x'z'z'y'z'462.3.1iiniDiTiKiTiFρ[5]24TiiiTiFnDKρ=(5)2.3.2x'y'z'xT′yT′zT′x'y'z'TKTMTN12xTTTFF′=+(6)34yTTTFF′=+(7)56zTTTFF′=+(8)200776893344TTTTTKRFRF=+(9)5566TTTTTMRFRF=+(10)1122TTTTTNRFRF=+(11)TiRi2.4f′u=Tfff()uvwTflll()uvw′=u=1−vTTfff()uvw(12)f′ur′uTfrrr1()uvw′′′==−uuu(13)2.4.1[5]x'y'z'x'Fy'Fz'F(14)(16)drr0.5xxFCSuuρ′′=(14)d'rr0.5yyFCSvvρ′=(15)drr0.5zzFCSwwρ′′=(16)ρdCdC=0.81.0xS′,yS′,zS′x'y'z'(xK′yK′zK′)x'y'z'xKpp′yKqq′zKrr′2.4.2x'y'z'um&vm&wm&x'y'z'pm&qm&rm&3()(,)()+=MhhNhhIhF&&&(17)T()uvwpqr=h&T()uvwpqr=h&&&&&&&&T()xy'z'TTTTTTKMN′=F(,).Nhh=T((1)(2)(3)(4)(5)(6))NNNNNN11(1)()[()()]xuNFmmqwvrqwvr′=−+−−−&11(2)()[()()]yvNFmmruwpruwp′=−+−−−&(3)N=11()[()()]zwFmmpvuqpvuq′−+−−−&(4)N=bb(cossincoscos)x'KppzyBθϕθϕ+−−b11b11()()mypvuqmzurpw−+−(5)N=bb(coscossin)y'KqqxzBθϕθ+++b11b11()()mxpvuqmzwqvr−−−(6)N=bb(sincossin)z'KrryxBθθϕ−+−b11b11()()mxruwpmywqvr−+−()uvwpxqyrzmmmmmmmImImI+⎛⎞⎜⎟+⎜⎟⎜⎟+=⎜⎟+⎜⎟⎜⎟+⎜⎟⎜⎟+⎝⎠Mh&&&&&&I(h)m(uvwpqr)T()xyzθφϕ==x&&&&&&&T()uvwpqrT(18)T=ω⎛⎞⎜⎟⎝⎠vT00T(19)x&,y&,z&443790T12()=xxx(20)T1()xyz=x(21)T2()φθϕ=x(22)(18)u'=1−vT1x&(23)Ω=1ω−T2x&(24)u'=1−vT1x&&(25)Ω&=1ω−T(2x&&cα)(26)cαωT(23)(26)(17)111c2()()(,)−−⎧⎫⎡⎤⎛⎞⎛⎞⎪⎪=−+⎨⎬⎢⎥⎜⎟⎜⎟⎝⎠⎝⎠⎪⎪⎣⎦⎩⎭xFIhMhTNhhx&&&&&α(27)111vω−−−⎡⎤=⎢⎥⎣⎦T0T0T(28)(17)(27)555Tdd1d2()=xxx(29)Td1ddd()xyz=x(30)Td2ddd()φθϕ=x(31)p1d1=−exxp1d1=−exx&&&p1d1=−exx&&&&&&0n=R()nOa0dddd()n=RnOa[6]oddd1()2=×+×+×ennOOaa(32)(32)o2d2=−exx&&&(33)o2d2=−exx&&&&&&(34)po⎛⎞=⎜⎟⎝⎠eeePo⎛⎞=⎜⎟⎝⎠eee&&&Po⎛⎞=⎜⎟⎝⎠eee&&&&&&d111d2c0(()(,)−−⎧⎫⎡⎤⎛⎞⎛⎞⎪⎪=−+⎨⎬⎢⎥⎜⎟⎜⎟⎝⎠⎝⎠⎪⎪⎣⎦⎩⎭xFIh)MhTNhhx&&&&&α(35)d111d2c0(()(,)−−⎧⎫⎡⎤⎛⎞⎛⎞⎪⎪−+==⎨⎬⎢⎥⎜⎟⎜⎟⎝⎠⎝⎠⎪⎪⎣⎦⎩⎭xIh)MhTNhhFx&&&&&α111c20(()(,)−−⎧⎫⎡⎤⎛⎞⎛⎞⎪⎪−+⎨⎬⎢⎥⎜⎟⎜⎟⎝⎠⎝⎠⎪⎪⎣⎦⎩⎭xIh)MhTNhhx&&&&&α(36)(36)d11d22⎛⎞⎛⎞=⎜⎟⎜⎟⎝⎠⎝⎠xxxx&&&&&&&&(37)d1d11p1dpd2d22o2−⎛⎞⎛⎞⎛⎞⎛⎞=++⎜⎟⎜⎟⎜⎟⎜⎟−⎝⎠⎝⎠⎝⎠⎝⎠xxxexKKxxxex&&&&&&&&&&&&(38)(37)dp0++=eKeKe&&&(39)Kd,KpKdKp()=ee&20077691()00(38)(27)d1d1111dd2d22()()−−⎧⎡−⎛⎞⎛⎞⎪=++⎨⎢⎜⎟⎜⎟−⎝⎠⎝⎠⎪⎣⎩xxxFIhMhTKxxx&&&&&&&&pPoc0(,)⎫⎤⎛⎞⎛⎞⎪−+⎬⎥⎜⎟⎜⎟⎝⎠⎝⎠⎪⎦⎭eKNhhe&α(40)MATLAB(40)FF(27)x6MATLABSIMULINK0.5m6mxyz0.1m/s8m/s(10,10,10)x10m/s[7]exeyez3ex=1mey=1mez=1m(1,1,1)KdKp101000.01s68xyzxyz766x7y8z[1]YUHJ.Designandcontrolofautonomousunderwaterrobotsasurvey[J].AutonomousRobots,2000,8(1)7-24.[2]ZHAOSide,YUHJ.Experimentalstudyonadvancedunderwaterrobotcontrol[J].IEEETransactionsonRobot-ics,2005,21(4)695-703.[3]SAKAQAMIN,INOUEM,KAWAMURAS.Theoreticalandexperimentalstudiesoniterativelearningcontrolforunderwaterrobots[J].InternationalJournalofOffshoreandPolarEngineering,2003,13(2)120-127.[4][D].2001[5][M]437922000[6]LUHJYS,WALKERMW,PAULRPC.Resolvedaccel-erationcontrolofmechanicalmanipulators[J].IEEETransactionsonAutomaticControl,1980,25(3)468-474.[7][M]1992DYNAMICANALYSISANDMOTIONCONTROLOF6-DOFUNDERWATERROBOTBIANYushuGAOZhihuiYUNChao(SchoolofMechanicalEngineeringandAutomation,BeihangUniversity,Beijing100083)AbstractDynamicanalysisandmotioncontrolofthe6-DOFunderwaterrobotareresearched.Firstly,consideringthecom-prehensiveinfluencecausedbyfactorssuchasthegravity,buoyancy,drivingforces,hydrodynamicforces,etc.,dynamicequationsoftheunderwaterrobotareestablished,whichcandescribethecomplicatedunderwaterdynamicbehavioroftherobot.Then,inordertotrackthedesiredtrajectoryaccurately,anonlinearcontroller,whichincludesanouterloopandaninnerloop,isdesignedusingresolvedaccelerationmethod.Intheouterloop,negativefeedbackisusedtocorrectthedeviationbetweentheactualpathanddesiredpath.Intheinnerloop,basedontherobot'sdynamicproperties,thenonlinearcom-pensationisimportedtosimplifytherobotsystemtoalinearsystemandmakeiteasiertocontrol.Finally,simulationsoftheunderwaterrobottrackinganobjectaretaken.Simulationre-sultsshowthatthemethodcanprovidesatisfactorytrackingabilityforunderwaterrobots.KeywordsUnderwaterrobotM[e-mail](86)AbstractACouetterheometerwithflowvisualizationandpressurecontrolismadeboyusingthetechniquesofhigh-precisionmachininganddynamicsealing.Withtherotat-ingoutercylinderandstaticinnercylinder,therheometercanrealizestableanduniformsimpleshearflowoftheshearrateupto105/s.Utilizingthewell-knownviscosity-temperatureformula,theviscositiesofwaterandsiliconeoilsathighshearratesmeasuredbythisrheometeragreewellwiththatobtainedbythecommercialrotationalrheometeratlowshearratesandvarioustemperatures.Atacertainshearstress,cavitationoc-curredforfreshwaterandtwosiliconeoilswithlowandme-diumviscosities.Strongdependenceoftheonsetofshearcavi-tationontheamountofdissolvedgasiso