基于MSRDS机器人仿真平台的多机器人PID编队控制算法

最新推荐文章于 2024-07-28 18:04:30 发布
最新推荐文章于 2024-07-28 18:04:30 发布
阅读量4.6k 收藏 15
点赞数 5
分类专栏： 多机器人系统 机器人仿真 课程-现代控制理论- 文章标签： MSRDS 多机器人编队控制 PID控制
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/ZhangRelay/article/details/51352844
版权

自己调试的编队PID算法，效果也还可以，具体使用教程参考视频链接：

http://v.youku.com/v_show/id_XMTUwNjc3NjMyNA


仿真中三个机器人保持编队，做直线运动，队形和参数都可以调整。

仿真结束后，编队数据直接推送的MATLAB中，用于曲线绘制，并分析。



源代码下载地址：http://download.csdn.net/detail/zhangrelay/9514411

--------

StartSimulationEngine
//zhangrelay formation_control PID
AddSkyEntity  sky
	/VisualCubeTextureFile:"sky.dds"
	/LightingCubeTextureFile:"sky_diff.dds"

AddLightSourceEntity  sun
	/LightType:Directional
	/Color:0.8  0.8  0.8  1
	/Direction:-1.0  -1.0  0.5

AddHeightFieldEntity    ground1

Ruilei Zhang CopyRight


AddiRobotCreate  robot01  	/Position:0  0  0
	/Orientation:0    -90    0

AddiRobotCreate    robot02  	/Position:-3  0  -3
	/Orientation:0    -90    0
//	/Procedure_LRF_SensorNotify:proc_robot_02_LRF	

AddiRobotCreate    robot03  	/Position:-3  0  3
	/Orientation:0    -90    0
//	/Procedure_LRF_SensorNotify:proc_robot_03_LRF	

//GameController    controller1
//	/Procedure_UpdateAxes:proc_ctrl

//SimpleDashboard    simpledashboard1


//AddiRobotCreate    icreate1  	/Position:0  0  0

UpdateCameraView  
	/EyePosition:0  20  5  	/LookAtPoint:0  0  0

FlushScript  
///
WaitForEntityCreation    robot01   
WaitForEntityCreation    robot02   
WaitForEntityCreation    robot03   
//WaitForEntityCreation    robot04   
//WaitForEntityCreation    robot05   
///
double  vtall=0.2    // >0.4
double  wtall=0.00   // >0.6
double dprl=0.2
double leadfy=0.0 
double  vtsign=0
double  wtsign=0
double kpvtf=1.0
double kpwtf=4.0
//double kivtf=0.08
//double kiwtf=0.24
double kivtf=0.1
double kiwtf=0.04
double jdqw=0.04
double jlqw=0.04
double ctrl_x=0.0
double ctrl_y=0.0
double ctrl_z=0.0
double pxsum1=0.0
double pzsum1=0.0
double pxsum2=0.0
double pzsum2=0.0
double kpld1=8.0
double kpld2=8.0
double SimTime=1002.0
float rob2ff=0.0
float rob3ff=0.0
float rob2lrf1=0.0
float rob2lrf2=0.0
float rob2lrf3=0.0
float rob2lrf4=0.0
float rob2lrf5=0.0
float rob3lrf1=0.0
float rob3lrf2=0.0
float rob3lrf3=0.0
float rob3lrf4=0.0
float rob3lrf5=0.0
float j=0
float PadNum=0.0
float ctrl_x=0.0
float ctrl_y=0.0	
float ctrl_z=0.0
matlabtheta1 = Util.CreateArray(double, 1000)  
matlabderr1 = Util.CreateArray(double, 1000)  
matlabtheta2 = Util.CreateArray(double, 1000)  
matlabderr2 = Util.CreateArray(double, 1000) 
matlabtheta3 = Util.CreateArray(double, 1000)  
matlabderr3 = Util.CreateArray(double, 1000) 
matlabtheta4 = Util.CreateArray(double, 1000)  
matlabderr4 = Util.CreateArray(double, 1000) 
 
matlablx = Util.CreateArray(double, 1000)  
matlably = Util.CreateArray(double, 1000)  
matlablz = Util.CreateArray(double, 1000)  
matlabla = Util.CreateArray(double, 1000)  
matlablb = Util.CreateArray(double, 1000)  
matlablc = Util.CreateArray(double, 1000) 
matlabfx1= Util.CreateArray(double, 1000)  
matlabfy1= Util.CreateArray(double, 1000)
matlabfz1= Util.CreateArray(double, 1000)
matlabfx2 = Util.CreateArray(double, 1000)  
matlabfy2 = Util.CreateArray(double, 1000)
matlabfz2= Util.CreateArray(double, 1000)
matlabfx3= Util.CreateArray(double, 1000)  
matlabfy3= Util.CreateArray(double, 1000)
matlabfx4 = Util.CreateArray(double, 1000)  
matlabfy4 = Util.CreateArray(double, 1000)
/
matlablvt = Util.CreateArray(double, 1000)  
matlablwt = Util.CreateArray(double, 1000)  
matlabfvt1 = Util.CreateArray(double, 1000)  
matlabfwt1 = Util.CreateArray(double, 1000) 
matlabfvt2 = Util.CreateArray(double, 1000)  
matlabfwt2 = Util.CreateArray(double, 1000) 
matlabfvt3 = Util.CreateArray(double, 1000)  
matlabfwt3 = Util.CreateArray(double, 1000) 
matlabfvt4 = Util.CreateArray(double, 1000)  
matlabfwt4 = Util.CreateArray(double, 1000) 
matlabt = Util.CreateArray(double, 1000) 
matlabtime = Util.CreateArray(double, 1000)


wait 10000

call proc_main  with concur

Procedure  proc_main

	call proc_robot_01

	call proc_robot_02

	call proc_robot_03

	call proc_robot_04

	call proc_robot_05
	
	call proc_line

//	call proc_ctrl
	
End



Procedure Matlab_Data

	MATLAB    matlab1
///
	matlab1.PutFullMatrix("theta1", "base", matlabtheta1, matlabt)
	matlab1.PutFullMatrix("derr1", "base", matlabderr1, matlabt)
	matlab1.PutFullMatrix("theta2", "base", matlabtheta2, matlabt)
	matlab1.PutFullMatrix("derr2", "base", matlabderr2, matlabt)
	matlab1.PutFullMatrix("theta3", "base", matlabtheta3, matlabt)
	matlab1.PutFullMatrix("derr3", "base", matlabderr3, matlabt)
	matlab1.PutFullMatrix("theta4", "base", matlabtheta4, matlabt)
	matlab1.PutFullMatrix("derr4", "base", matlabderr4, matlabt)
///
	matlab1.PutFullMatrix("lx", "base", matlablx, matlabt)
	matlab1.PutFullMatrix("ly", "base", matlably, matlabt)
    matlab1.PutFullMatrix("lz", "base", matlablz, matlabt)
	matlab1.PutFullMatrix("la", "base", matlabla, matlabt)
	matlab1.PutFullMatrix("lb", "base", matlablb, matlabt)
	matlab1.PutFullMatrix("lc", "base", matlablc, matlabt)
	matlab1.PutFullMatrix("fx1", "base", matlabfx1, matlabt)
	matlab1.PutFullMatrix("fy1", "base", matlabfy1, matlabt)
    matlab1.PutFullMatrix("fz1", "base", matlabfz1, matlabt)
	matlab1.PutFullMatrix("fx2", "base", matlabfx2, matlabt)
	matlab1.PutFullMatrix("fy2", "base", matlabfy2, matlabt)
	matlab1.PutFullMatrix("fz2", "base", matlabfz2, matlabt)
	matlab1.PutFullMatrix("fx3", "base", matlabfx3, matlabt)
	matlab1.PutFullMatrix("fy3", "base", matlabfy3, matlabt)
	matlab1.PutFullMatrix("fx4", "base", matlabfx4, matlabt)
	matlab1.PutFullMatrix("fy4", "base", matlabfy4, matlabt)
/
	matlab1.PutFullMatrix("lvt", "base", matlablvt, matlabt)
	matlab1.PutFullMatrix("lwt", "base", matlablwt, matlabt)
	matlab1.PutFullMatrix("fvt1", "base", matlabfvt1, matlabt)
	matlab1.PutFullMatrix("fwt1", "base", matlabfwt1, matlabt)
	matlab1.PutFullMatrix("fvt2", "base", matlabfvt2, matlabt)
	matlab1.PutFullMatrix("fwt2", "base", matlabfwt2, matlabt)
	matlab1.PutFullMatrix("fvt3", "base", matlabfvt3, matlabt)
	matlab1.PutFullMatrix("fwt3", "base", matlabfwt3, matlabt)
	matlab1.PutFullMatrix("fvt4", "base", matlabfvt4, matlabt)
	matlab1.PutFullMatrix("fwt4", "base", matlabfwt4, matlabt)
	matlab1.PutFullMatrix("mtime", "base", matlabtime, matlabt)
	robot01.Go(0, 0)
	robot02.Go(0, 0)
	robot03.Go(0, 0)
	robot04.Go(0, 0)
	robot05.Go(0, 0)
//	matlab1.PutFullMatrix("linef", "base", matlableaderx, matlableadery)
//	matlab1.PutFullMatrix("derr", "base", matlabdiserror, matlabt)
End

/

Procedure  proc_robot_01
//leader
	for (i = 0; i < SimTime; i++)
	{
		theta_l=robot01.Orientation.Y
		p_xl = robot01.Position.X 
		p_zl = robot01.Position.Z
	    theta_y=robot01.Orientation.X  //FY
		theta_x=robot01.Orientation.Z  //QX
		p_yl = robot01.Position.Y  //Z			
		vt=vtall    
		wt=wtall 
		if(i>600)
		{
		    vt=(0.2+(i-600.0)/6000.0)
			wt=0.04  
		}
		matlablvt[i]=vt
		matlablwt[i]=wt 
		matlabla[i]=theta_y 
        matlablb[i]=theta_x 
        matlablc[i]=theta_l 
		leadfy=( Math.Cos( theta_y* Math.PI / 180.0)) 
		left= ( vt + dprl * wt ) 
		right= ( vt - dprl * wt ) 
///
		robot01.Go(left, right)
		wait 50
	}
//	FlushScript
//	robot01.Go(0, 0)

End

Procedure  proc_robot_02_LRF

		rob2lrf2=value.DistanceMeasurements[270]
		rob2lrf1=value.DistanceMeasurements[360]
//		print rob2lrf2. ToString()

End

Procedure  proc_robot_03_LRF

		rob3lrf4=value.DistanceMeasurements[90]
		rob3lrf5=value.DistanceMeasurements[0]
		print rob3lrf5. ToString()

End

Procedure  proc_robot_02
//follower01
	for (i = 0; i < SimTime; i++)
	{
		vt_l=1.0*vtall
		wt_l=1.0*wtall
		vtsign= Math.Sign( vt_l )
		wtsign= Math.Sign( wt_l )
//		lpp = 0.8
//		wpp= (- Math.PI /2.0)     //( Math.PI /6.0)  // Math.PI / 6.0    //Math.PI  //left- right +

//		if(rob2lrf2<1500.0)
//		{
//			if(rob2ff<1.0)
//			{
//				if(rob2lrf2>600.0)
//				{
//					lpp = 1.0
//					wpp=( - Math.PI /2.0 + ((Math.PI /2.5)*((-rob2lrf2+1500.0)/900.0)))
//				}
//				else
//				{
//					lpp = 1.0 
					wpp= - Math.PI /6.0
					rob2ff=2.0		
//				}
//			}
//		}
//		else
//		{
//			if(rob2lrf1>2000.0)
//			{
//				lpp = 1.0
//				wpp= (- Math.PI /2.0)
//				rob2ff=0.0
//			}  		
//		}
		if(i<300)
		{
			lpp = 1.0
			wpp= 0		
		}
		else
		{
			if(i<600)
			{
				lpp = 1.0
				wpp= (- Math.PI /6.0)			
			}
			else
			{
				lpp = 1.0
				wpp= (- Math.PI /2.0)
			}
		}
//		rob2ff=0.0
//		print thetadt. ToString()
//basic2D
		theta_l=robot01.Orientation.Y
		theta_f=robot02.Orientation.Y
		p_xl = robot01.Position.X 
		p_zl = robot01.Position.Z
		p_xf = robot02.Position.X 
		p_zf = robot02.Position.Z
/3D
	    theta_yl=robot01.Orientation.X  //FY
		theta_xl=robot01.Orientation.Z  //QX
		p_yl = robot01.Position.Y  //Z	
	    theta_yf=robot02.Orientation.X  //FY
		theta_xf=robot02.Orientation.Z  //QX
		p_yf = robot02.Position.Y  //Z	
/	3D-2D
  //      lpp= Math.Sqrt( 1.0/(1.0+(((p_yf-p_yl)*(p_yf-p_yl))/(((p_xf-p_xl)*(p_xf-p_xl))+((p_yf-p_yl)*(p_yf-p_yl))))))       
//		print lpp. ToString()
		lsin= ( lpp* Math.Sin( wpp))   //L'
		lcos= ( lpp* Math.Cos( wpp))   //L	
		thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
		theta_yl=robot01.Orientation.X * Math.PI /180.0
		theta_yf=robot02.Orientation.X * Math.PI /180.0 
//	 
		theta_=-(theta_l-theta_f)
		if(theta_ > 180.0)
			theta_=-(360.0-theta_)
		if( -theta_ > 180.0)
			theta_=(360.0+theta_) 
		theta_l=robot01.Orientation.Y * Math.PI /180.0
		theta_f=robot02.Orientation.Y * Math.PI /180.0
		theta_=(theta_/180.0)*Math.PI
/ 
		xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp))     //up - //down + this is not right but ...OK
		zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
		p_x_= (xff-p_xf)
		p_z_= (zff-p_zf)		
		p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
		p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f))  
		ptxz= Math.Atan( p_t_x/p_t_z)
		p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
		p_jl= ( p_t_z)
		p_jd= ( p_t_x)
//		print p_jl. ToString()
///
		matlablx[i]=p_xl 
		matlably[i]=p_zl
		matlablz[i]=p_yl  
		matlabfx1[i]=p_xf
		matlabfy1[i]=p_zf
        matlabfz1[i]=p_yf
		matlabtheta1[i]=theta_
		matlabderr1[i]=p_ld
		matlabbt[i]=0      //use matlab
  
		if((-ptxz)>(Math.PI)) 
			ptxz= ( ptxz+ 2.0* Math.PI)
		if(ptxz>(Math.PI)) 
			ptxz= ( ptxz- 2.0* Math.PI)    

//  coordinate conversion
		if( Math.Abs( wt_l) < 0.01  )
			vt_l=vt_l  
		else
			vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l   //this change some wrong
		p_ld=(kpld1*p_ld / Math.Abs( vt_l))
		if(p_ld> (Math.PI /2) )
			p_ld= Math.PI /2
//  

			kpld1=2.0
			pxsum1=pxsum1+p_jd  
			pzsum1=pzsum1+p_jl
			absjd = Math.Abs( p_jd)	
			absjl = Math.Abs( p_jl)	

					wt_f= ((kpwtf*p_jd+kiwtf*pxsum1)*vtsign+(kpwtf*(theta_-thetadt)*(jdqw/absjd)))     //+ (kpwtf*theta_* Math.Cos( p_ld)))    
					vt_f= ((kpvtf*p_jl/leadfy+kivtf*pzsum1)*(jdqw/absjd))	

			if(absjd<(jdqw/4.0))
			{
				pxsum1=0
//				absjd=jdqw
				wt_f=matlabfwt1[i-1]
			}
			if(absjl<(jlqw/4.0))
			{
				pzsum1=0
//				absjl=jlqw
				vt_f=matlabfvt1[i-1]
			}

/
//		print p_ld. ToString()
//  
		matlabfvt1[i]=vt_f
		matlabfwt1[i]=wt_f 
//  
		left= vt_f + dprl * wt_f 
		right=vt_f - dprl * wt_f 
		if(left>1.0)
			left=1.0
		if(right>1.0)
			right=1.0
		if(-left>1.0)
			left=-1.0
		if(-right>1.0)
			right=-1.0
		robot02.Go(left, right) 
/
		wait 50
//		print p_ld. ToString()
	}

//	robot02.Go(0, 0)

	call Matlab_Data

End

Procedure  proc_robot_03
//follower02
	for (i = 0; i < SimTime; i++)
	{
		vt_l=1.0*vtall
		wt_l=1.0*wtall
		vtsign= Math.Sign( vt_l )
		wtsign= Math.Sign( wt_l )
//		lpp = 0.8
//		wpp= Math.PI /2.0  //(- Math.PI /6.0)  // Math.PI / 6.0    //Math.PI  //left- right +
//		if(rob3lrf4<1500.0)
//		{
//			if(rob3ff<1.0)
//			{
//				if(rob3lrf4>600.0)
//				{
//					lpp = 1.0     //+1.0*((-rob2lrf2+1500.0)/900.0)
//					wpp=( Math.PI /2.0 - ((Math.PI /2.5)*((-rob3lrf4+1500.0)/900.0)))
//				}
//				else
//				{
//					lpp = 1.0
					wpp= Math.PI /6.0	
					rob3ff=2.0		
//				}
//			}
//		}
//		else
//		{
//			if(rob3lrf5>2000.0)
//			{
				tempi=i
//				lpp = 1.0
//				wpp= ( Math.PI /2.0)
//				rob3ff=0.0
//			}  			
//		}
		if(i<300)
		{
			lpp = 2.0
			wpp= 0		
		}
		else
		{
			if(i<600)
			{
				lpp = 1.0
				wpp= ( Math.PI /6.0)			
			}
			else
			{
				lpp = 1.0
				wpp= ( Math.PI /2.0)
			}
		}
//		rob3ff=0.0
		lsin= ( lpp* Math.Sin( wpp))   //L'
		lcos= ( lpp* Math.Cos( wpp))   //L	 
		thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
//basic
		theta_l=robot01.Orientation.Y
		theta_f=robot03.Orientation.Y
		p_xl = robot01.Position.X 
		p_zl = robot01.Position.Z
		p_xf = robot03.Position.X 
		p_zf = robot03.Position.Z 
///
	    theta_yf=robot03.Orientation.X  //FY
		theta_xf=robot03.Orientation.Z  //QX
		p_yf = robot03.Position.Y  //Z	
		theta_=-(theta_l-theta_f)
		if(theta_ > 180.0)
			theta_=-(360.0-theta_)
		if( -theta_ > 180.0)
			theta_=(360.0+theta_) 
		theta_l=robot01.Orientation.Y * Math.PI /180.0
		theta_f=robot03.Orientation.Y * Math.PI /180.0
		theta_=(theta_/180.0)*Math.PI
/ 
		xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp))     //up - //down + this is not right but ...OK
		zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
		p_x_= (xff-p_xf)
		p_z_= (zff-p_zf)		
		p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
		p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f)) 
		ptxz= Math.Atan( p_t_x/p_t_z)
		p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
		p_jl= ( p_t_z)
		p_jd= ( p_t_x)
///
		matlabfx2[i]=p_xf
		matlabfy2[i]=p_zf
		matlabfz2[i]=p_yf
		matlabtheta2[i]=theta_
		matlabderr2[i]=p_ld
		matlabtime[i]=i
// 
		if((-ptxz)>(Math.PI)) 
			ptxz= ( ptxz+ 2.0* Math.PI)
		if(ptxz>(Math.PI)) 
			ptxz= ( ptxz- 2.0* Math.PI)    

//  coordinate conversion
		if( Math.Abs( wt_l) < 0.01  )
			vt_l=vt_l  
		else
			vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l   //this change some wrong
		p_ld=(kpld2*p_ld / Math.Abs( vt_l))
		if(p_ld> (Math.PI /2) )
			p_ld= Math.PI /2
//  

			kpld2=2.0
			pxsum2=pxsum2+p_jd  
			pzsum2=pzsum2+p_jl	
			absjd = Math.Abs( p_jd)	
			absjl = Math.Abs( p_jl)	
			if(absjd
   
   
    
    1.0)
			left=1.0
		if(right>1.0)
			right=1.0
		if(-left>1.0)
			left=-1.0
		if(-right>1.0)
			right=-1.0
		robot03.Go(left, right)
/				
		wait 50
		print 		thetadt. ToString()
		print 		theta_. ToString()
//		theta_
	} 
//	print p_ld. ToString()
//	robot03.Go(0, 0)

End

Procedure  proc_robot_04
//follower03
	for (i = 0; i < SimTime; i++)
	{
		vt_l=vtall
		wt_l=wtall
		vtsign= Math.Sign( vt_l )
		wtsign= Math.Sign( wt_l )
		if(i<300)
		{
			lpp = 3.0
			wpp= 0		
		}
		else
		{
			if(i<600)
			{
				lpp = 2.0
				wpp= (- Math.PI /6.0)			
			}
			else
			{
				lpp = 2.0
				wpp= (- Math.PI /2.0)  //(- Math.PI /6.0)  // Math.PI / 6.0    //Math.PI  //left- right +
			}
		}
		lsin= ( lpp* Math.Sin( wpp))   //L'
		lcos= ( lpp* Math.Cos( wpp))   //L
		thetadt= ( lcos/ Math.Abs( vt_l/wt_l))	 
//basic
		theta_l=robot01.Orientation.Y
		theta_f=robot04.Orientation.Y
		p_xl = robot01.Position.X 
		p_zl = robot01.Position.Z
		p_xf = robot04.Position.X 
		p_zf = robot04.Position.Z 
		theta_=-(theta_l-theta_f)
		if(theta_ > 180.0)
			theta_=-(360.0-theta_)
		if( -theta_ > 180.0)
			theta_=(360.0+theta_) 
		theta_l=robot01.Orientation.Y * Math.PI /180.0
		theta_f=robot04.Orientation.Y * Math.PI /180.0
		theta_=(theta_/180.0)*Math.PI
/ 
		xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp))     //up - //down + this is not right but ...OK
		zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
		p_x_= (xff-p_xf)
		p_z_= (zff-p_zf)		
		p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
		p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f)) 
		ptxz= Math.Atan( p_t_x/p_t_z)
		p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
		p_jl= ( p_t_z)
		p_jd= ( p_t_x)
///
		matlabfx3[i]=p_xf
		matlabfy3[i]=p_zf
		matlabtheta3[i]=theta_
		matlabderr3[i]=p_ld
//		matlabtime[i]=i
// 
		if((-ptxz)>(Math.PI)) 
			ptxz= ( ptxz+ 2.0* Math.PI)
		if(ptxz>(Math.PI)) 
			ptxz= ( ptxz- 2.0* Math.PI)    

//  coordinate conversion
		if( Math.Abs( wt_l) < 0.01  )
			vt_l=vt_l  
		else
			vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l   //this change some wrong
		p_ld=(kpld2*p_ld / Math.Abs( vt_l))
		if(p_ld> (Math.PI /2) )
			p_ld= Math.PI /2
//  
		if(p_ld>1.56)
		{
			kpld2=4.0
			wt_f= (wt_l+(kpwtf*p_jd)*vtsign+(kpwtf*theta_* Math.Cos( p_ld)))    
			vt_f= (vt_l+kpvtf*p_jl)	
		}
		else
		{
			kpld2=2.0
			pxsum=pxsum+p_jd  
			pzsum=pzsum+p_jl	
			wt_f= (wt_l+(kpwtf*p_jd+kiwtf*pxsum)*vtsign+(kpwtf*(theta_-thetadt)* Math.Cos( p_ld)))     //+ (kpwtf*theta_* Math.Cos( p_ld)))    
			vt_f= (vt_l+kpvtf*p_jl+kivtf*pzsum)
		}
		matlabfvt3[i]=vt_f
		matlabfwt3[i]=wt_f 
		left= vt_f + dprl * wt_f 
		right=vt_f - dprl * wt_f 
		if(left>0.6)
			left=0.6
		if(right>0.6)
			right=0.6
		if(-left>0.6)
			left=-0.6
		if(-right>0.6)
			right=-0.6
		robot04.Go(left, right)
/				
		wait 50
	} 

//	robot04.Go(0, 0)

End

Procedure  proc_robot_05
//follower04
	for (i = 0; i < SimTime; i++)
	{
		vt_l=vtall
		wt_l=wtall
		vtsign= Math.Sign( vt_l )
		wtsign= Math.Sign( wt_l )
		if(i<300)
		{
			lpp = 4.0
			wpp= 0		
		}
		else
		{
			if(i<600)
			{
				lpp = 2.0
				wpp= ( Math.PI /6.0)			
			}
			else
			{
				lpp =2.0
				wpp= ( Math.PI /2.0)    //(- Math.PI /6.0)  // Math.PI / 6.0    //Math.PI  //left- right +
			}
		}
		lsin= ( lpp* Math.Sin( wpp))   //L'
		lcos= ( lpp* Math.Cos( wpp))   //L	 
		thetadt= ( lcos/ Math.Abs( vt_l/wt_l))
//basic
		theta_l=robot01.Orientation.Y
		theta_f=robot05.Orientation.Y
		p_xl = robot01.Position.X 
		p_zl = robot01.Position.Z
		p_xf = robot05.Position.X 
		p_zf = robot05.Position.Z 
		theta_=-(theta_l-theta_f)
		if(theta_ > 180.0)
			theta_=-(360.0-theta_)
		if( -theta_ > 180.0)
			theta_=(360.0+theta_) 
		theta_l=robot01.Orientation.Y * Math.PI /180.0
		theta_f=robot05.Orientation.Y * Math.PI /180.0
		theta_=(theta_/180.0)*Math.PI
/ 
		xff= ( p_xl+ lpp* Math.Sin( theta_l+wpp))     //up - //down + this is not right but ...OK
		zff= ( p_zl+ lpp * Math.Cos( theta_l+wpp))
		p_x_= (xff-p_xf)
		p_z_= (zff-p_zf)		
		p_t_x= ( -p_z_* Math.Sin( theta_f) + p_x_ * Math.Cos( theta_f))
		p_t_z= ( -p_z_* Math.Cos( theta_f) - p_x_ * Math.Sin( theta_f)) 
		ptxz= Math.Atan( p_t_x/p_t_z)
		p_ld= Math.Sqrt( p_t_x*p_t_x+ p_t_z*p_t_z )
		p_jl= ( p_t_z)
		p_jd= ( p_t_x)
///
		matlabfx4[i]=p_xf
		matlabfy4[i]=p_zf
		matlabtheta4[i]=theta_
		matlabderr4[i]=p_ld
//		matlabtime[i]=i
// 
		if((-ptxz)>(Math.PI)) 
			ptxz= ( ptxz+ 2.0* Math.PI)
		if(ptxz>(Math.PI)) 
			ptxz= ( ptxz- 2.0* Math.PI)    

//  coordinate conversion
		if( Math.Abs( wt_l) < 0.01  )
			vt_l=vt_l  
		else
			vt_l = ((vt_l/wt_l-lsin)/(vt_l/wt_l))*vt_l   //this change some wrong
		p_ld=(kpld2*p_ld / Math.Abs( vt_l))
		if(p_ld> (Math.PI /2) )
			p_ld= Math.PI /2
//  
		if(p_ld>1.56)
		{
			kpld2=4.0
			wt_f= (wt_l+(kpwtf*p_jd)*vtsign+(kpwtf*theta_* Math.Cos( p_ld)))    
			vt_f= (vt_l+kpvtf*p_jl)	
		}
		else
		{
			kpld2=2.0
			pxsum=pxsum+p_jd  
			pzsum=pzsum+p_jl	
			wt_f= (wt_l+(kpwtf*p_jd+kiwtf*pxsum)*vtsign+(kpwtf*(theta_-thetadt)* Math.Cos( p_ld)))     //+ (kpwtf*theta_* Math.Cos( p_ld)))    
			vt_f= (vt_l+kpvtf*p_jl+kivtf*pzsum)
		}
//		print p_ld. ToString()
		matlabfvt4[i]=vt_f
		matlabfwt4[i]=wt_f 
		left= vt_f + dprl * wt_f 
		right=vt_f - dprl * wt_f 
		if(left>0.6)
			left=0.6
		if(right>0.6)
			right=0.6
		if(-left>0.6)
			left=-0.6
		if(-right>0.6)
			right=-0.6
		robot05.Go(left, right)
/				
		wait 50
	} 

//	robot05.Go(0, 0)

End


Procedure  proc_line

	for (i = 0; i < SimTime; i++)
	{
//     robot01
		p_x01 = robot01.Position.X 
		p_z01 = robot01.Position.Z 
//		robot02
		p_x02 = robot02.Position.X 
		p_z02 = robot02.Position.Z 		
//	   robot03
		p_x03 = robot03.Position.X 
		p_z03 = robot03.Position.Z 
//		robot04
		p_x04 = robot04.Position.X 
		p_z04 = robot04.Position.Z 	
//     robot05
		p_x05 = robot05.Position.X 
		p_z05 = robot05.Position.Z 	
//     robot06
		p_x06 = robot06.Position.X 
		p_z06 = robot06.Position.Z 
	if(i>299)
	{
		i=0
	}	
	if(i==0)
	{
		j++
		AddTextSpriteEntity    sprite1{j}
			/Position:{p_x01}  0.04  {p_z01}
			/Orientation:-90    180    0	
			/FontSize:32
			/Text:●
			/Width:1
			/Height:1
			/TextureWidth:100
			/TextureHeight:100
        	/FontColor:255  255  255  255


		AddTextSpriteEntity    sprite2{j}
			/Position:{p_x02}  0.04  {p_z02}
			/Orientation:-90    180    0
			/FontSize:32
			/Text:●
			/Width:1
			/Height:1
			/TextureWidth:100
			/TextureHeight:100
        	/FontColor:255  255  255  255
			
		AddTextSpriteEntity    sprite3{j}
			/Position:{p_x03}  0.04  {p_z03}
			/Orientation:-90    180    0
			/FontSize:32
			/Text:●
			/Width:1
			/Height:1
			/TextureWidth:100
			/TextureHeight:100
        	/FontColor:255  255  255  255
	
		AddTextSpriteEntity    sprite4{j}
			/Position:{p_x04}  0.04  {p_z04}
			/Orientation:-90    180    0
			/FontSize:32
			/Text:●
			/Width:1
			/Height:1
			/TextureWidth:100
			/TextureHeight:100
        	/FontColor:255  255  255  255


		AddTextSpriteEntity    sprite5{j}
			/Position:{p_x05}  0.04  {p_z05}
			/Orientation:-90    180    0
			/FontSize:32
			/Text:●
			/Width:1
			/Height:1
			/TextureWidth:100
			/TextureHeight:100
        	/FontColor:255  255  255  255


		FlushScript	
	}
		wait 50
	}

End





zhangrelay
关注
专栏目录
多机器人编队控制
08-22
多机器人编队控制
多移动机器人编队控制
01-04
多移动机器人编队控制及协作搬运策略
领航者-跟随者编队算法 定义+特性+原理+公式+Python示例代码（带详细注释）
最新发布
qq_51929160的博客
07-28 1375
本文深入解析了领航者-跟随者编队算法，探讨了其基本原理、实现步骤、实际应用以及优化与挑战。领航者-跟随者编队算法通过定义领航者和跟随者的相对位置与运动轨迹，实现多智能体系统的协调控制。该算法广泛应用于无人机编队、自动驾驶车队和搜救任务中，具有鲁棒性强、扩展性好和分布式控制等优势。然而，实际应用中仍面临通讯可靠性、动态环境适应性和多智能体协作等挑战。未来研究应致力于提升算法效率、优化能量消耗和增强环境适应性，以推动其在更多实际场景中的应用和发展。
多机器人编队控制matlab仿真程序
03-31
多机器人系统的群集编队控制理论仿真程序，采用人工势场法，使多个机器人在向目标点运动过程中能保持队形，并能适应环境约束。对于初学者非常有用
【目标识别】自适应多机器人编队，可在运动和能见度约束下包围和跟踪目标（Matlab代码实现）
weixin_61181717的博客
03-27 328
近年来，以机器人为代表的人工智能产业逐渐走进了各行各业，单机器人系统在很多方面不能满足人们生产和生活的需要，多机器人协作控制成为了当今机器人研究的热门方向。多机器人编队控制是多机器人协作研究的基础，传统多机器人编队的控制方式相对复杂，实现起来较为困难，且不能很好的适应外界环境。在实际应用中，多机器人队形需要面临复杂的环境，且经常需要进行队形变换，实际的需求给我们研究多机器人编队控制算法的自适应方面提出了更高的要求。这些策略基于保持具有可变尺寸的地层形状，或者基于以灵活的形状保持地层的大小。
多机器人编队PID算法_SPL_MRF_demo
05-09
基于微软机器人仿真平台开发。 多机器人编队PID算法_SPL_MRF_demo
Matlab实现多个机器人的编队控制
07-01
这个脚本可能包含了定义机器人模型、编队规则、控制算法和仿真过程。常见的编队控制方法包括虚拟结构法、拓扑控制法和优化方法等。在Matlab中，通常会用到ODE求解器（如ode45）来模拟机器人的动态行为，以及PVTOL...
多机器人编队控制,多机器人编队控制matlab仿真程序,matlab源码.zip
10-15
MATLAB作为一种强大的数值计算和仿真工具，被广泛用于设计和验证多机器人编队控制的算法。 在MATLAB中进行多机器人编队控制的仿真，通常会涉及到以下几个关键知识点： 1. **机器人模型**：首先，需要为每个机器人...
l_f.rar_matlab 编队_多机器人MATLAB_机器人 编队_编队 领航_领航 跟随
07-14
在机器人技术领域，编队控制是一项重要的研究主题，特别是在多机器人系统中。"l_f.rar_matlab 编队_多机器人MATLAB_机器人 编队_编队 领航_领航 跟随"这个压缩包内容显然是关于使用MATLAB实现的一种多机器人领航跟随...
编队控制仿真程序
06-27
总的来说，这个编队控制仿真程序是一个实践多机器人协调控制的好例子，它结合了控制理论、数学建模和软件编程，对于学习和研究多智能体系统的协同行为具有很高的价值。通过这个项目，不仅可以掌握编队控制策略，还能...
多机器人协同编队算法原理及实现
热门推荐
Redglass的博客
04-24 3万+
ROS及SLAM进阶教程（八）机器人协同编队算法原理及实现图论基础基于一致性的编队控制算法编队控制算法原理编队算法仿真实现 多机器人协同编队需要将理论和实践紧密地结合起来，其应用包括编队队形生成、保持、变换和路径规划与避障等等都是基于图论的理论基础完成的。 图论基础 控制协同多智能体动态系统是通过通信图进行相互联系的动力学问题，通信图表明了各个节点之间的信息流。协同控制的目标是为各个节点设计控制协...
多个机器人编队控制 形成统一队形到达目标点的程序.zip
05-31
在静态环境中，使用人工势场法进行多机器人编队，使多机器人形成统一队形后到达目标点，从而实现编队加避障
一种多机器人编队行进控制算法.pdf
08-13
#资源达人分享计划#
人工智能算法 多机器人路径规划协同避障 MATLAB 仿真平台，可直接运行
03-19
MATLAB 可直接运行 具有说明文档 欢迎下载，持续更新
基于ROS的多差速移动机器人编队控制仿真实现
qq_41701758的博客
08-04 3477
基于ROS实现移动机器人编队仿真，可以在gazebo和rviz中显示多机器人，并可以发布cmd_vel指令单独控制其中某个机器人
编队控制方法
u010743448的博客
07-14 1万+
简介   目前，实现多 AUV 系统编队航行的控制方法主要包括基于领航者-跟随者的方法、 基于虚拟结构的方法、基于人工势场的方法、基于行为的方法和基于路径跟随的方法等。 基于领航者-跟随者（Leader-follower）的编队控制方法   领航者-跟随者的概念最早由 Cruz提出，并且由 Wang等将其成功应用于移动机器人的编队控制中。作为目前最为常用的一种编队控制方法，其基本思想是：所有编队成员被指定为领航者或跟随者这两种角色，领航者通过沿着预定或者临时设定的路径航行，掌控整个编队的运动趋势，跟随者依
【无人机编队】分布式编队控制算法三无人机编队协同作业【含Matlab源码 3070期】
Matlab武动乾坤的博客
02-22 1012
分布式编队控制算法三无人机编队协同作业 完整的代码，方可运行；可提供运行操作视频！适合小白！
机器人编队控制总结
漫长IT路
01-09 2万+
文章目录简介基于领航者-跟随者（Leader-follower）的编队控制方法基于虚拟结构（Virtual Structure）的编队控制方法基于人工势场（Artificial Potential Field）的编队控制方法基于行为（Behavior）的编队控制方法 简介   目前，实现多 AUV 系统编队航行的控制方法主要包括基于领航者-跟随者的方法、 基于虚拟结构的方法、基于人工势场的方法、...
多机器人系统仿真平台(摘要与目录)
狂战士之家
01-29 2475
来源：http://www.robotdiy.com/article.php?sid=125 如何实现多个机器人之间的协调与协作已经成为当前机器人学研究的新课题之一。这篇论文针对多机器人系统的运动规划和仿真系统建模与实现展开研究工作。这篇论文根据通用的多机器人仿真平台所应该具有的特点，采用面向对象模型化技术OMT（Object Modeling Technique）,OMT
基于群体觅食行为的多机器人分布式避障编队控制
接着，研究者对群体觅食行为模型进行了扩展，提出了基于势场的多机器人分布式编队控制模型。势场法是一种常见的路径规划和避障方法，它将环境中的目标和障碍物转化为虚拟的引力和斥力场，使机器人能够根据场力的引导...
评论 17
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包

打赏作者

zhangrelay

你的鼓励将是我创作的最大动力

¥1 ¥2 ¥4 ¥6 ¥10 ¥20
扫码支付：¥1
获取中
扫码支付

您的余额不足，请更换扫码支付或充值

打赏作者

实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值