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

举报
zhangrelay 发表于 2021/07/15 07:21:21 2021/07/15
【摘要】 自己调试的编队PID算法，效果也还可以，具体使用教程参考视频链接： http://v.youku.com/v_show/id_XMTUwNjc3NjMyNA 仿真中三个机器人保持编队，做直线运动，队形和参数都可以调整。 仿真结束后，编队数据直接推送的MATLAB中，用于曲线绘制，并分析。 源代码下载地址：http://download.csdn.net/detail/z...

自己调试的编队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.blog.csdn.net，作者：zhangrelay，版权归原作者所有，如需转载，请联系作者。

原文链接：zhangrelay.blog.csdn.net/article/details/51352844

【版权声明】本文为华为云社区用户转载文章，如果您发现本社区中有涉嫌抄袭的内容，欢迎发送邮件进行举报，并提供相关证据，一经查实，本社区将立刻删除涉嫌侵权内容，举报邮箱： cloudbbs@huaweicloud.com
机器人
  • 点赞
  • 收藏
  • 关注作者

评论（0）

0/1000
抱歉，系统识别当前为高风险访问，暂不支持该操作

全部回复

上滑加载中

设置昵称

在此一键设置昵称，即可参与社区互动！

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

*长度不超过10个汉字或20个英文字符，设置后3个月内不可修改。

确认 取消

加入云驻计划，成为创作者

  • 华为云周边好礼
  • 免费体验产品
  • 特殊身份标识
  • 线下官方门票
  • 内部专家零距离
  • 与10000+优质创作者共同成长
立即加入