ctrlstrategy.java

赛车程序：先对赛道和赛车建立模型

/**
 * 
 */
package cn.tsinghua.ag5.CarModel;

/**
 * @author wfq
 *
 */
public class CtrlStrategy {
	private float m_expV = 18;
	public static final float s_span = 30;
	
	private boolean m_isFirst;

	//速度控制的参数
	private float m_P = 35f;
	private float m_I = 0.4f;
	//跟随车的速度控制参数
	private float m_fP = 50f;
	private float m_fI = 0.2f;
	private double m_integral;
	
	private DynModel m_ctrlTarget, m_target;
	
	private final static double s_sErr = 1.5;
	private final static double s_curPlus = 0.02;
	
	//模糊控制规则，采用加权平均的方法，把去模糊化直接写如控制规则中
	private static final double[][] m_FuzzyRule =
	{
		{-0.20,-0.20,-0.20,-0.20,-0.20,-0.10, 0.05},
		{-0.20,-0.15,-0.10,-0.06,-0.03, 0.03, 0.08},
		{-0.18,-0.08,-0.02,-0.01, 0.005, 0.03, 0.12},
		{-0.15,-0.05,-0.01,   0,  0.01, 0.05, 0.15},
		{-0.12,-0.03,-0.005, 0.01, 0.02, 0.08, 0.18},
		{-0.08,-0.03, 0.03, 0.06, 0.10, 0.15, 0.20},
		{-0.05, 0.10, 0.20, 0.20, 0.20, 0.20, 0.20},
	};
//	{
//			{-0.20,-0.20,-0.20,-0.20,-0.05,    0, 0.05},
//			{-0.12,-0.15,-0.10,-0.07,-0.03, 0.05, 0.10},
//			{-0.05,-0.10,-0.07,-0.03,    0, 0.03, 0.12},
//			{-0.03,-0.07,-0.03,    0, 0.03, 0.07, 0.20},
//			{    0,-0.03,    0, 0.03, 0.07, 0.10, 0.20},
//			{ 0.07,-0.05, 0.03, 0.07, 0.10, 0.15, 0.20},
//			{ 0.20, 0.20, 0.20, 0.20, 0.20, 0.20, 0.20},
//	};
	
	//隶属度函数采用三角形隶属度函数
//	private double t_speedNow = 1;
//	private double m_distErr[] =  {-t_speedNow,-2*t_speedNow/3,-2*t_speedNow/9,0,2*t_speedNow/9,2*t_speedNow/3,t_speedNow};
	private double m_distErr[] = new double[7];
//	private static final double m_included[] = {-0.10,-0.05,-0.03,0,0.03,0.05,0.10};
	private static final double m_included[] = {-0.20,-0.10,-0.06,0,0.06,0.10,0.20};
	
	private int m_roadPartIndex;
	
	//调试信息
	private int d_printLog;
	
	/**
	 * @param p_p
	 * @param p_i
	 */
	public CtrlStrategy(DynModel p_target) {
		super();
//		this.m_P = p_p;
//		this.m_I = p_i;
		m_ctrlTarget = p_target;
//		m_expV = 18; //m/s
		m_integral = 0;
		m_roadPartIndex = 0;
		
		m_isFirst = true;
	}
	
	public CtrlStrategy(DynModel p_target, DynModel p_former) {

		m_ctrlTarget = p_target;
		m_target = p_former;
		
		//m_expV = 50; //m/s
		m_integral = 0;
		m_roadPartIndex = 0;
		
		m_isFirst = false;
	}
	
	public void update()
	{
		//速度
		double t_speedNow;
		double t_error;
		double t_speedCtrl;

		t_speedNow = Math.hypot(m_ctrlTarget.getVX(), m_ctrlTarget.getVY());
		if(m_isFirst)
		{
			t_error = m_expV - t_speedNow;
//			System.out.println("first car: Speed error: " + t_error + " expected speed : " + m_expV 
//					+ " speed now : " + t_speedNow);
//			if(t_error < 0)
//			{
//				System.out.println(" Too fast!");
//			}
			
			if(Math.abs(t_error) > 1e-2 )
			{
				m_integral += t_error * m_I;
				//积分限幅
				if(m_integral > 3 * MyGUI.s_emStep * DynModel.s_uLimit / 5)
					m_integral = 3 * MyGUI.s_emStep * DynModel.s_uLimit / 5;
				if(m_integral < 3 * MyGUI.s_emStep * DynModel.s_dLimit / 5)
					m_integral = 3 * MyGUI.s_emStep * DynModel.s_dLimit / 5;
				
				t_speedCtrl = (m_P * t_error + m_integral) / MyGUI.s_emStep;
			}else
			{
				t_speedCtrl = m_integral / MyGUI.s_emStep;
			}
			
			if(t_speedCtrl > DynModel.s_uLimit)
			{
				t_speedCtrl = DynModel.s_uLimit;
			}
			if(t_speedCtrl < DynModel.s_dLimit)
			{
				t_speedCtrl = DynModel.s_dLimit;
			}
//			System.out.println("first car: " + t_speedNow + "	:	" + t_speedCtrl);
			
		}else
		{
			if(m_target == null)
			{
				throw new Error("CtrlStrategy: no former target!\n");
			}
			t_error = Math.hypot(m_ctrlTarget.getLocationX() - m_target.getLocationX(), 
					m_ctrlTarget.getLocationY() - m_target.getLocationY()) - s_span;
			t_error += ( Math.hypot(m_target.getVX(), m_target.getVY()) - t_speedNow ) * s_sErr; 
			
			if(Math.abs(t_error) > 1e-1 )
			{
				m_integral += t_error * m_fI;
				//积分限幅
				if(m_integral > 3 * MyGUI.s_emStep * DynModel.s_uLimit / 5)
					m_integral = 3 * MyGUI.s_emStep * DynModel.s_uLimit / 5;
				if(m_integral < 3 * MyGUI.s_emStep * DynModel.s_dLimit / 5)
					m_integral = 3 * MyGUI.s_emStep * DynModel.s_dLimit / 5;
				
				t_speedCtrl = (m_fP * t_error + m_integral) / MyGUI.s_emStep;
			}else
			{
				t_speedCtrl = m_integral / MyGUI.s_emStep;
			}
			
			if(t_speedCtrl > DynModel.s_uLimit)
			{
				t_speedCtrl = DynModel.s_uLimit;
			}
			if(t_speedCtrl < DynModel.s_dLimit)
			{
				t_speedCtrl = DynModel.s_dLimit;
			}
//			System.out.println("Second car: " + t_speedNow + "	:	" + t_speedCtrl);
		}
		
		m_ctrlTarget.setFaif(t_speedCtrl);
		
		//方向
		double t_toend;
		double t_tostart;
		double t_decAngle;
		double t_endAngle;
		double tmp;
		
		tmp = 3;
//		tmp = 2;
		m_distErr[0] = - tmp;
		m_distErr[1] = - 2 * tmp / 3;
		m_distErr[2] = - 2 * tmp / 9;
		m_distErr[3] = 0;
		m_distErr[4] = 2 * tmp / 9;
		m_distErr[5] = 2 * tmp / 3;
		m_distErr[6] = tmp;
		
		//更新归属
		if(GeogInfo.s_road[m_roadPartIndex].strCur)
		{
			t_toend = Math.hypot(m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].endX,
					m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].endY);
			t_tostart = Math.hypot(m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].startX, 
					m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].startY);
			
			if(t_toend < t_tostart && Math.hypot(t_toend, GeogInfo.s_road[m_roadPartIndex].length) < t_tostart)
			{
				m_roadPartIndex++;
			}	
		}else
		{
			t_decAngle = Math.atan2(m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].centerY,
					m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].centerX);
			t_endAngle = Math.atan2(GeogInfo.s_road[m_roadPartIndex].endY - 
					GeogInfo.s_road[m_roadPartIndex].centerY, GeogInfo.s_road[m_roadPartIndex].endX - 
					GeogInfo.s_road[m_roadPartIndex].centerX);
//			t_decAngle = convertPI(t_decAngle);
//			t_endAngle = convertPI(t_decAngle);
			
			if(Math.abs(t_decAngle - t_endAngle) < Math.PI)
			{
				if(t_decAngle >= t_endAngle)
				{
					m_roadPartIndex ++;
				}
			}else
			{
				if( t_decAngle * t_endAngle > 0)
				{
					throw new Error(" CtrlStrategy unexpected error!\n");
				}
				if(t_decAngle < 0)
				{
					m_roadPartIndex ++ ;
				}
				
			}
		}
		
		if(m_roadPartIndex >= GeogInfo.s_roadPartNum)
		{
			m_ctrlTarget.terminate();
			MyGUI.s_cvs.pauseCanvas();
			return;
		}

		if(m_roadPartIndex != d_printLog)
		{
			d_printLog = m_roadPartIndex;
			System.out.print(m_ctrlTarget.getClass().toString() + " chang road part!" + "PartNumber:" +
					m_roadPartIndex + "		" +	GeogInfo.s_road[m_roadPartIndex].strCur + "\n");
		}
		
		//角度左负右正,位置左true，右false
		double t_distErr;
		double t_acrg;
		boolean t_lr = false;
		double t_included;
		
		if(GeogInfo.s_road[m_roadPartIndex].strCur)
		{
			t_toend = Math.hypot(m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].endX,
					m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].endY);
			t_tostart = Math.hypot(m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].startX, 
					m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].startY);
			
			tmp = (t_toend + t_tostart + GeogInfo.s_road[m_roadPartIndex].length) / 2;
			t_acrg = Math.sqrt(tmp * ( tmp - GeogInfo.s_road[m_roadPartIndex].length) * (tmp - t_toend) 
					* (tmp - t_tostart));
			
			t_distErr = 2 * t_acrg / GeogInfo.s_road[m_roadPartIndex].length;
			
			if (t_distErr <= 1e-3)	
			{
				t_distErr = 0;
			}else
			{
				t_lr = dcLeftRight(GeogInfo.s_road[m_roadPartIndex].endX - GeogInfo.s_road[m_roadPartIndex].startX, 
						GeogInfo.s_road[m_roadPartIndex].endY - GeogInfo.s_road[m_roadPartIndex].startY, 
						m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].startX, 
						m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].startY);
			}

			t_included = GeogInfo.tangentDegree(GeogInfo.s_road[m_roadPartIndex].startX, 
					GeogInfo.s_road[m_roadPartIndex].startY, GeogInfo.s_road[m_roadPartIndex].endX, 
					GeogInfo.s_road[m_roadPartIndex].endY) - m_ctrlTarget.getTheta();
//			System.out.println(" Road tangent: " + (m_ctrlTarget.getTheta() + t_included));

			//此处切换回	右正左负
			if(t_lr)
			{
				t_distErr = -t_distErr;
			}
		}else
		{
			if(GeogInfo.s_road[m_roadPartIndex].turn)
			{
				t_distErr = - GeogInfo.s_road[m_roadPartIndex].curRadius + Math.hypot(m_ctrlTarget.getLocationX() - 
						GeogInfo.s_road[m_roadPartIndex].centerX, m_ctrlTarget.getLocationY() - 
						GeogInfo.s_road[m_roadPartIndex].centerY);
			}else
			{
				t_distErr = GeogInfo.s_road[m_roadPartIndex].curRadius - Math.hypot(m_ctrlTarget.getLocationX() - 
						GeogInfo.s_road[m_roadPartIndex].centerX, m_ctrlTarget.getLocationY() - 
						GeogInfo.s_road[m_roadPartIndex].centerY);
			}
			//t_lr = GeogInfo.s_road[m_roadPartIndex].turn;
			
			t_included = - m_ctrlTarget.getTheta() + GeogInfo.tangentDegree(GeogInfo.s_road[m_roadPartIndex].centerX, 
					GeogInfo.s_road[m_roadPartIndex].centerY, m_ctrlTarget.getLocationX(), 
					m_ctrlTarget.getLocationY(), GeogInfo.s_road[m_roadPartIndex].turn);
			
//			System.out.println(" Road tangent: " + (m_ctrlTarget.getTheta() + t_included));
		}
		
		//用横向误差是错误的，如果延x轴平行方向则误差算不出来
		
		t_included = convertPI(t_included);
		
		//控制算法
		//计算隶属度
		double t_memDist[];
		double t_memInclu[];
		double t_ctrlMete = 0;
//		double tmp;
		
		//调试
		tmp = 2;
		for(double i : m_included)
		{
			i *= tmp;
		}
		
		t_memDist = calcuMem( m_distErr, t_distErr);
		t_memInclu = calcuMem(m_included, t_included);
		
		for(int i = 0; i<7; i++)
		{
			for(int j = 0; j < 7; j++)
			{
				t_ctrlMete += t_memDist[i] * t_memInclu[j] * m_FuzzyRule[i][j];
			}
		}
		double t_tmp;
		if( ! GeogInfo.s_road[m_roadPartIndex].strCur)
		{
			t_tmp = GeogInfo.s_road[m_roadPartIndex].turn ? s_curPlus * (150 - GeogInfo.s_curRad / 2) /  
					(GeogInfo.s_road[m_roadPartIndex].curRadius - 50): - s_curPlus * (150 - GeogInfo.s_curRad / 2) / 
					(GeogInfo.s_road[m_roadPartIndex].curRadius - 50);
					
			if((tmp = Math.abs(Math.atan2(m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].centerY, 
					m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].centerX) - 
					Math.atan2(GeogInfo.s_road[m_roadPartIndex].startY - GeogInfo.s_road[m_roadPartIndex].centerY,
							GeogInfo.s_road[m_roadPartIndex].startX - GeogInfo.s_road[m_roadPartIndex].centerX)) /
							GeogInfo.s_road[m_roadPartIndex].angle) > 6 / 7)
			{
				t_tmp *= (1 - tmp);
			}else
			{
				t_ctrlMete += t_tmp;
			}

			if(Math.abs(t_ctrlMete) > DynModel.s_steer)
			{
				t_ctrlMete = DynModel.s_steer * Math.signum(t_ctrlMete);
			}
		}else
		{
			//计算直线走了几分之几
			t_tmp = Math.hypot(m_ctrlTarget.getLocationX() - GeogInfo.s_road[m_roadPartIndex].startX, m_ctrlTarget.getLocationY()
					- GeogInfo.s_road[m_roadPartIndex].startY);
			tmp = Math.abs(Math.atan2(m_ctrlTarget.getLocationY() - GeogInfo.s_road[m_roadPartIndex].startY, 
					m_ctrlTarget.getLocationX()- GeogInfo.s_road[m_roadPartIndex].startX) - 
					Math.atan2(GeogInfo.s_road[m_roadPartIndex].endY - GeogInfo.s_road[m_roadPartIndex].startY, 
							GeogInfo.s_road[m_roadPartIndex].endX - GeogInfo.s_road[m_roadPartIndex].startX));
			tmp = t_tmp * Math.cos(tmp) / GeogInfo.s_road[m_roadPartIndex].length;
			
			if(m_roadPartIndex == 0 || GeogInfo.s_road[m_roadPartIndex - 1].strCur || tmp > 1 / 4)
			{
				t_ctrlMete *= 0.4 * t_speedNow / m_expV;
			}else
			{
				t_ctrlMete *= 0.80 * t_speedNow / m_expV;
			}
			
			if(tmp > 0.9 && m_roadPartIndex < GeogInfo.s_roadPartNum && !GeogInfo.s_road[m_roadPartIndex + 1].strCur)
			{
				t_ctrlMete += GeogInfo.s_road[m_roadPartIndex + 1].turn ? s_curPlus / 10 : - s_curPlus / 10;
			}
		}
		
		m_ctrlTarget.setDelta( t_ctrlMete);
		
//		if(m_isFirst)
//		{
//			System.out.println("first car: " + ( t_ctrlMete) + " Dist Error : " + t_distErr 
//					+ " 	Angle Err : " + t_included + " Car angle : " + m_ctrlTarget.getTheta());
//		}else
//		{
//			System.out.println("second car: " + ( t_ctrlMete) + " Dist Error : " + t_distErr 
//					+ " 	Angle Err : " + t_included + " Car angle : " + m_ctrlTarget.getTheta());
//		}
	}
	
	/**
	 * 确定在基准角度的左边还是右边，左为true，右返回false
	 * @param p_normX
	 * @param p_normY
	 * @param p_pointX
	 * @param p_pointY
	 * @return
	 */
	public boolean dcLeftRight(double p_normX, double p_normY, double p_pointX, double p_pointY)
	{
		double t_agn;
		double t_agp;
		
		t_agn = Math.atan2(p_normY, p_normX);
		t_agp = Math.atan2(p_pointY, p_pointX);
		
		if(Math.abs(t_agn - t_agp) < Math.PI)
		{
			if(t_agn > t_agp)	return false;
			else	return true;
		}else
		{
			if(t_agn > 0)	return true;
			else	return false;
		}
	}
	
	/**
	 * 把弧度转化为 -pi 到 pi之间
	 * @param p_radian
	 * @return
	 */
	public double convertPI( double p_radian)
	{
		p_radian %= 2 * Math.PI;
		if(p_radian > - Math.PI )
		{
			if(p_radian <= Math.PI)
			{
				return p_radian;
			}else
			{
				return p_radian - 2 * Math.PI;
			}
		}else
		{
			return p_radian + 2 * Math.PI;
		}
		
	}
	
	/**
	 * 计算隶属度
	 * 如果采取先用二维数组存储界点两侧区域底边长度的办法，模糊算法参数的调整可以更灵活
	 * @param p_mereSt
	 * @param p_x
	 * @return
	 */
	public double[] calcuMem(double []p_mereSt, double p_x)
	{
		if(p_mereSt.length != 7)
		{
			System.out.print("Unexpected Control error!\n");
			return null;
		}
		//0-6: NB, NM, NS, ZERO, PS, PM, PB
		double t_member[] = new double[7];
		if(p_x < p_mereSt[0])
		{
			t_member[0] = 1;
			return t_member;
		}else if(p_x >= p_mereSt[6])
		{
			t_member[6] = 1;
			return t_member;
		}else
		{
			for(int j = 1; j < 7; j++)
			{
				if(p_x >= p_mereSt[j-1] && p_x < p_mereSt[j])
				{
					t_member[j-1] = (p_mereSt[j] - p_x) / (p_mereSt[j] - p_mereSt[j-1]);
					t_member[j] = (p_x - p_mereSt[j-1]) / (p_mereSt[j] - p_mereSt[j-1]);
				}
			}
		}
		return t_member;
	}
}