CarModel.rar_CarModel_java control model_控制算法_自动驾驶_驾驶

/** * */ 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].st