UIi = KI XUPi+UIi;
[0123]步骤6.5.4、计算第i个控制周期Ti下PID调节算法微分控制输出量UDi = KDX (UP1-UP1-1 )；
[0124]步骤6.5.5、计算第1个控制周期1\下？10调节算法总输出量?01^ =即1+1^+1^;
[0125]步骤6.5.6、判断v-vd<vf是否成立，若成立，则表示实际车速已经非常接近期望车速，为了防止车速超调，此时将油门控制量置为最小值，则令thri = 0，车辆将在短时间内处于滑行状态，若不成立，则令thri = Pouti;
[0126]步骤7、将第i个控制周期Ti下所得油门控制量thri和制动控制量brai通过CAN 口发送至执行机构控制车辆速度并执行步骤8;
[0127]步骤8、将i+Ι赋值给i;并返回步骤3执行，进入下一个控制周期。
【主权项】
1.一种基于运动模式判断的无人车辆纵向控制方法，是应用于装配有自动驾驶仪、GPS/INS定位系统和工控机的无人驾驶车辆上，其特征是，所述无人车纵向控制方法是按如下步骤进行: 步骤1、定义变量i，初始化i = l;当i<l时，令i = l; 步骤2、设置第i个周期1\下所述无人驾驶车辆的油门控制量为thr1、制动控制量为bra1、运动模式状态值为sta1、PID调节算法比例控制输出量为UP1、PID调节算法车速反馈误差量为Em、PID调节算法积分控制输出量为UI1、PID调节算法微分控制输出量为UD1、PID调节算法总控制输出量为Pout1、低速制动标志位为Iabi；并有staie {so,si,S2,S3,S4,S5} ,so表示复位模式;SI表示起步模式;S2表示制动模式;S3表示低速模式;S4表示加速模式;S5表示调节模式； 定义低速模式切换阈值为V1、低速模式制动切换阀值为vds、调节模式出口阀值以及加速模式入口阀值为Va，调节模式入口阀值为VS，并有va> VS、起步模式出口阈值为vc、调节模式防止超调阀值为vf，PID调节算法比例项系数KP，PID调节算法积分项系数KI，PID调节算法微分项系数KD; 定义有效行驶速度阈值为vq、坡度阈值为Pit、起步模式油门控制量初始值为ω、油门最大控制量为thrmax、起步模式油门控制量补偿增量为ω up、起步模式下的时间阈值为tmax、加速模式油门控制量补偿增量为ω ac、制动模式加速度限值为amin、加速模式加速度限值为amax、制动模式下坡工况制动控制补偿增量为扮?、制动模式非下坡工况制动控制补偿增量为、制动模式最大控制量为bramax，低速模式制动控制量为^;初始化 thr1、bra1、sta1、Lup1、UP1、Err1、UI1、UD1、Pouti 各为一个定值，初始化 Iabi = O;步骤3、从所述GPS/INS定位系统获取第i个周期Ti下所述无人驾驶车辆的车速V1、加速度&1和坡度p1;从所述工控机获取第i个周期T1下所述无人驾驶车辆的期望车速vcU、第1-1个周期IV1T所述无人驾驶车辆的油门控制量为thr^、制动控制量为bra^、运动模式状态值为sta1-1、比例控制输出量为Lup1-1和低速制动标志位lab1-1; 步骤4、判断第i个周期1\下所述无人驾驶车辆是否对运动模式状态值^&1进行切换: 步骤4.1、判断sta1-1 = so是否成立，若成立，则执行步骤5;否则，执行步骤4.2 ； 步骤4.2、判断sta1-1 = Si是否成立，若成立，则执行步骤4.3;否则，执行步骤4.4 ； 步骤4.3、判断vdi> vl且Vi > vc是否成立，若成立，则令stai = so ；否则，保持运动模式状态值stai为起步状态Si ；并执行步骤5 ； 步骤4.4、判断sta1-1 = S2是否成立，若成立，则执行步骤4.5;否则，执行步骤4.7 ； 步骤4.5、判断vdi>vl且0<vdi_vi<vs是否成立，若成立，贝Ij令stai = s5;并执行步骤5;否则，执行步骤4.6; 步骤4.6、判断vdi < vl且vi<vdi是否成立，若成立，则令stai = S3;否则，保持运动模式状态值stai为制动状态S2，并执行步骤5 ； 步骤4.7、判断sta1-1 = S3是否成立，若成立，则执行步骤4.8;否则，执行步骤4.9 ； 步骤4.8、判断vdi>vl是否成立，若成立，则令stai = SQ;否则，保持运动模式状态值stai为低速状态S3，并执行步骤5; 步骤4.9、判断sta1-1 = S4是否成立，若成立，则执行步骤4.10;否则，执行步骤4.12； 步骤4.10、判断￥(^>￥1且00(^-￥108是否成立，若成立，贝1|令8七31 = 85，并执行步骤.5;否则，执行步骤4.11; 步骤4.11、判断vdi < vl或者Vi < vq是否成立，若成立，则令Stai = So;否则，保持运动模式状态值Stai为加速状态S4，并执行步骤5 ； 步骤4.12、判断vdi > vl且vd1-vi> va是否成立，若成立，则令stai = so;否则，执行步骤.4.13； 步骤4.13、判断vdi < vl或者Vi < vq是否成立，若成立，则令Stai = So;否则，执行步骤.4.14； 步骤4.14、保持行驶状态值Sta1为调节状态S5，并执行步骤5; 步骤5、确定第i个周期Ti下的运动模式状态值sta1: 步骤5.1、判断stai = so是否成立，若成立，则执行步骤5.2 ；否则，执行步骤6 ； 步骤5.2、判断Vi<vq且vd>vl是否成立，若成立，则令stai = Si，并执行步骤6;否则，执行步骤5.3 ； 步骤5.3、判断Vi > vd且vd> vl是否成立，若成立，则令stai = S2，并执行步骤6;否则，执行步骤5.4; 步骤5.4、判断vd < vl是否成立，若成立，则令stai = S3，并执行步骤6;否则，执行步骤.5.5； 步骤5.5、判断vd_vi>va且vd>vl是否成立，若成立，则令stai = s4，并执行步骤6;否贝IJ，执行步骤5.6; 步骤5.6、令stai = S5，并执行步骤6 ； 步骤6、根据第i个周期T1下的运动模式状态值StaJA行相应的控制程序； 步骤6.1、判断stai = Si是否成立，若成立，则有brai = β，并执行步骤6.1.1;否则，执行步骤6.2; 步骤6.1.1、在第i个周期Ti下开始计时，获得起步用时t-upi;当t-upi<tmax时令thri =ω并执行步骤7;当t-upi>tmax时，执行步骤6.1.2; 步骤6.1.2、判断Vi>vq是否成立，若成立，则维持油门控制量thri为ω ;否则，将thr1-1+ω up赋值给thri，并执行步骤6.1.3； 步骤6.1.3、判断thri > thrmax是否成立，若成立，则将thrmax赋值给thri并执行步骤7;否则直接执行步骤7 ； 步骤6.2、判断stai = S2是否成立，若成立，则执行步骤6.2.1;否则，执行步骤6.3 ；步骤 6.2.1、令 thri = α ； 步骤6.2.2、判断pi<-pit是否成立，若成立，则执行步骤6.2.3，否则，执行步骤6.2.4；步骤6.2.3、判断ai > amin是否成立，若不成立，则将bra1-ι+βι。》赋值给brai，执行步骤.6.2.5;否则，维持brai为brai—!，并执行步骤6.2.5； 步骤6.2.4、判断ai > amin是否成立，若不成立，则将bra1-1+0gr。赋值给brai;否则，维持brai 为 bra1-1 ； 步骤6.2.5、判断brai > bramax是否成立，若成立，则将bramax赋值给brai，并执行步骤7 ；否则直接执行步骤7; 步骤6.3、判断stai = S3是否成立，若成立，则执行步骤6.3.1，若不成立，则执行步骤.6.4； 步骤6.3.1、判断lab1-1 = I是否成立，若成立，则执行步骤6.3.2，若不成立，则执行步骤.6.3.3； 步骤6.3.2.、判断vi_vdi<-vds是否成立，若成立，则brai = β，thri = α，Iabi = O，否则，令 brai = brai—1、令 thri = thri—ι，并执行步骤 7 ； 步骤6.3.3、判断v1-vdi > vds是否成立，若成立，则令brai = βι8、thri = a，Iabi = I，若不成立，贝1J令thri = a、brai = P并执行步骤7 ； 步骤6.4、判断stai = S4是否成立，若成立，则令brai = β，并执行步骤6.4.1；若不成立，则执行步骤6.5; 步骤6.4.1、判断ai < amax是否成立，若成立，则令thri = thri—ι+ω3。，并执行步骤6.4.2，若不成立，则令thri = thr1-1，并执行步骤7 ； 步骤6.4.2、判断thri > thrmax是否成立，若成立，则将thrmax赋值给thri，并执行步骤7 ；否则，直接执行步骤7;步骤 6.5、令 brai = β ； 步骤6.5.1、计算第i个周期Ti下PID调节算法车速反馈误差Em = vd1-vi; 步骤6.5.2、计算第i个周期Ti下PID调节算法比例控制输出量UPi = KPXErri; 步骤6.5.3、计算第i个周期Ti下PID调节算法积分控制输出量UIi = KI XUPi+UIi; 步骤6.5.4、计算第i个周期Ti下PID调节算法微分控制输出量UDi = KDX (UP1-UPi—!); 步骤6.5.5、计算第i个周期Ti下PID调节算法控制总输出量PoutpUPi+UIi+UDi; 步骤6.5.6、判断v-vd<vf是否成立，若成立，则令thri = a，若不成立，则令thri =Pouti； 步骤7、将第i个周期T1下所得油门控制量thri和制动控制量brai通过CAN 口发送至执行机构控制车辆速度并执行步骤8; 步骤8、将i+Ι赋值给i;并返回步骤3执行。
【专利摘要】本发明公开了一种基于运动模式判断的无人车辆纵向控制方法，是应用于装配有自动驾驶仪、GPS/INS定位系统和工控机的无人驾驶车辆上，其特征是首先通过对无人驾驶车辆实际车速与期望车速相对关系的分析，确定无人驾驶车辆运动模式状态值；在确认无人驾驶车辆运动模式的基础上执行与该运动模式相对应的控制算法进行车速控制从而实现无人驾驶车辆的纵向控制。本发明是能在保证跟踪精度的同时，使无人驾驶车辆纵向速度的控制更加平滑、舒适，控制过程更加符合人类的驾驶习惯。
【IPC分类】G05D1/02
【公开号】CN105511475
【申请号】CN201610073978
【发明人】宋彦, 蔡宗亮, 许铁娟, 梁华为, 梅涛
【申请人】中国科学院合肥物质科学研究院
【公开日】2016年4月20日
【申请日】2016年1月29日