視頻效果地址：多無人機編隊飛行使用CoppeliaSim+Python實現

CoppliaSim控制無人車和無人機實現編隊控制

function sysCall_init() particlesAreVisible=truesimulateParticles=truefakeShadow=trueparticleCountPerSecond=430particleSize=0.005particleDensity=8500particleScatteringAngle=30particleLifeTime=0.5maxParticleCount=50-- Detatch the manipulation sphere:targetObj=sim.getObjectHandle('Quadcopter_target')sim.setObjectParent(targetObj,-1,true)-- This control algo was quickly written and is dirty and not optimal. It just serves as a SIMPLE exampled=sim.getObjectHandle('Quadcopter_base')propellerHandles={}jointHandles={}particleObjects={-1,-1,-1,-1}local ttype=sim.particle_roughspheres+sim.particle_cyclic+sim.particle_respondable1to4+sim.particle_respondable5to8+sim.particle_ignoresgravityif not particlesAreVisible thenttype=ttype+sim.particle_invisibleendfor i=1,4,1 dopropellerHandles[i]=sim.getObjectHandle('Quadcopter_propeller_respondable'..i)jointHandles[i]=sim.getObjectHandle('Quadcopter_propeller_joint'..i)if simulateParticles thenparticleObjects[i]=sim.addParticleObject(ttype,particleSize,particleDensity,{2,1,0.2,3,0.4},particleLifeTime,maxParticleCount,{0.3,0.7,1})endendheli=sim.getObjectHandle(sim.handle_self)pParam=2iParam=0dParam=0vParam=-2cumul=0lastE=0pAlphaE=0pBetaE=0psp2=0psp1=0prevEuler=0if (fakeShadow) thenshadowCont=sim.addDrawingObject(sim.drawing_discpoints+sim.drawing_cyclic+sim.drawing_25percenttransparency+sim.drawing_50percenttransparency+sim.drawing_itemsizes,0.2,0,-1,1)end endfunction sysCall_cleanup() sim.removeDrawingObject(shadowCont)for i=1,#particleObjects,1 dosim.removeParticleObject(particleObjects[i])end end function sysCall_actuation() pos=sim.getObjectPosition(d,-1)if (fakeShadow) thenitemData={pos[1],pos[2],0.002,0,0,1,0.2}sim.addDrawingObjectItem(shadowCont,itemData)end-- Vertical control:targetPos=sim.getObjectPosition(targetObj,-1)pos=sim.getObjectPosition(d,-1)l=sim.getVelocity(heli)e=(targetPos[3]-pos[3])cumul=cumul+epv=pParam*ethrust=5.45+pv+iParam*cumul+dParam*(e-lastE)+l[3]*vParamlastE=e-- Horizontal control: sp=sim.getObjectPosition(targetObj,d)m=sim.getObjectMatrix(d,-1)vx={1,0,0}vx=sim.multiplyVector(m,vx)vy={0,1,0}vy=sim.multiplyVector(m,vy)alphaE=(vy[3]-m[12])alphaCorr=0.25*alphaE+2.1*(alphaE-pAlphaE)betaE=(vx[3]-m[12])betaCorr=-0.25*betaE-2.1*(betaE-pBetaE)pAlphaE=alphaEpBetaE=betaEalphaCorr=alphaCorr+sp[2]*0.005+1*(sp[2]-psp2)betaCorr=betaCorr-sp[1]*0.005-1*(sp[1]-psp1)psp2=sp[2]psp1=sp[1]-- Rotational control:euler=sim.getObjectOrientation(d,targetObj)rotCorr=euler[3]*0.1+2*(euler[3]-prevEuler)prevEuler=euler[3]-- Decide of the motor velocities:handlePropeller(1,thrust*(1-alphaCorr+betaCorr+rotCorr))handlePropeller(2,thrust*(1-alphaCorr-betaCorr-rotCorr))handlePropeller(3,thrust*(1+alphaCorr-betaCorr+rotCorr))handlePropeller(4,thrust*(1+alphaCorr+betaCorr-rotCorr)) end function handlePropeller(index,particleVelocity)propellerRespondable=propellerHandles[index]propellerJoint=jointHandles[index]propeller=sim.getObjectParent(propellerRespondable)particleObject=particleObjects[index]maxParticleDeviation=math.tan(particleScatteringAngle*0.5*math.pi/180)*particleVelocitynotFullParticles=0local t=sim.getSimulationTime()sim.setJointPosition(propellerJoint,t*10)ts=sim.getSimulationTimeStep()m=sim.getObjectMatrix(propeller,-1)particleCnt=0pos={0,0,0}dir={0,0,1}requiredParticleCnt=particleCountPerSecond*ts+notFullParticlesnotFullParticles=requiredParticleCnt % 1requiredParticleCnt=math.floor(requiredParticleCnt)while (particleCnt<requiredParticleCnt) do-- we want a uniform distribution:x=(math.random()-0.5)*2y=(math.random()-0.5)*2if (x*x+y*y<=1) thenif (simulateParticles) thenpos[1]=x*0.08pos[2]=y*0.08pos[3]=-particleSize*0.6dir[1]=pos[1]+(math.random()-0.5)*maxParticleDeviation*2dir[2]=pos[2]+(math.random()-0.5)*maxParticleDeviation*2dir[3]=pos[3]-particleVelocity*(1+0.2*(math.random()-0.5))pos=sim.multiplyVector(m,pos)dir=sim.multiplyVector(m,dir)itemData={pos[1],pos[2],pos[3],dir[1],dir[2],dir[3]}sim.addParticleObjectItem(particleObject,itemData)endparticleCnt=particleCnt+1endend-- Apply a reactive force onto the body:totalExertedForce=particleCnt*particleDensity*particleVelocity*math.pi*particleSize*particleSize*particleSize/(6*ts)force={0,0,totalExertedForce}m[4]=0m[8]=0m[12]=0force=sim.multiplyVector(m,force)local rotDir=1-math.mod(index,2)*2torque={0,0,rotDir*0.002*particleVelocity}torque=sim.multiplyVector(m,torque)sim.addForceAndTorque(propellerRespondable,force,torque) end

總結

以上是生活随笔為你收集整理的【CoppeliaSim】使用 Lua 语言控制四旋翼无人机运行的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。