执行步骤4.6;
[0085]步骤4.6、判断vdi < vl且Vi<vdi是否成立，若成立，表示车辆经过前一控制周期的制动控制后，应当进入低速模式，则令stai = 3，本控制周期将执行低速模式下的控制算法进行纵向车速控制；否则，则意味着车辆仍然未完成制动动作，需要继续使用制动模式下的控制算法进行纵向车速控制，保持运动模式状态值Sta1为制动状态2，并执行步骤5;
[0086]步骤4.7、判断sta1-1= 3是否成立，若成立，表示车辆在上个控制周期处于低速模式，则执行步骤4.8;否则，执行步骤4.9;
[0087]步骤4.8、判断V(I1)vI是否成立，若成立，表示本控制周期车辆需要跳出低速模式，则令Sta1 = O,表示程序需要重新为无人驾驶车辆选择一种除低速模式以外的运动模式；否则，则意味着无人驾驶车辆仍然需要继续使用低速模式下的控制算法进行纵向车速控制，保持运动模式状态值stai为低速状态S3，并执行步骤5;
[0088]步骤4.9、判断sta1-1= S4是否成立，若成立，表示车辆在上个控制周期处于加速模式，则执行步骤4.10;否则，执行步骤4.12；
[0089]步骤4.10、判断vdi >vl且O < vd1-vi < vs是否成立，若成立，表示车辆经过前一控制周期的加速控制后，应当进入调节模式，则令stai = 5，本控制周期将执行调节模式下的控制算法进行纵向车速控制，并执行步骤5;否则，执行步骤4.11；
[0090]步骤4.11、判断vdi < Vi或者Vi < vq是否成立，若成立，表示本控制周期车辆需要跳出加速模式，则令Sta1 = O,表示程序需要重新为无人驾驶车辆选择一种除加速模式以外的运动模式；否则，保持运动模式状态值Sta1为加速状态4，则意味着无人驾驶车辆仍然未完成加速动作，需要继续使用加速模式下的控制算法进行纵向车速控制，并执行步骤5;
[0091 ] 步骤4.12、此时，sta1-1的值必为5，表示车辆在上个控制周期处于调节模式，判断vdi>vl且vd1-vi >va是否成立，若成立，表示本控制周期车辆需要跳出调节模式，贝Ij令stai=0，表示程序需要重新为无人驾驶车辆选择一种除调节模式以外的运动模式;则令Sta1 =so;否则，执行步骤4.13;
[0092]步骤4.13、判断vdi < vl或者Vi < vq是否成立，若成立，贝Ij令stai = so;表示本控制周期车辆需要跳出调节模式，则令Sta1 = O,表示程序需要重新为无人驾驶车辆选择一种除调节模式以外的运动模式;否则，则意味着车辆仍然未完成调节动作，需要继续使用调节模式下的控制算法进行车速控制，执行步骤4.14;
[0093]步骤4.14、保持行驶状态值Sta1为调节状态S5，并执行步骤5;
[0094]步骤5、确定第i个周期T1下的运动模式状态值Sta1，本步骤在两种情况下将会执行:其一是在无人驾驶车辆在步骤4中从一种运动状态跳出但并没有直接进入另一种运动状态的情况下，即复位状态Sta1 = O时执行，此时通过执行该步骤将会为无人驾驶车辆重新选择一种运动模式;其二则是在程序初次执行的时候，此时由于^&1初始值为0，也相当于复位状态；
[0095]步骤5.1、判断Sta1 = O是否成立，若成立，表示需要通过执行该步骤重新为无人驾驶车辆选择一种运动模式，则执行步骤5.2;否则，意味着通过步骤4无人驾驶车辆已经确定了本控制周期下的运动模式，执行步骤6;
[0096]步骤5.2、判断vi<vq且vd>vl是否成立，若成立，表示无人驾驶车辆此时应当选择起步模式，则令s t a i = I，本控制周期将执行起步模式下的控制算法进行纵向车速控制，并执行步骤6;否则，执行步骤5.3;
[0097]步骤5.3、判断vi>vd且vd>vl是否成立，若成立，表示无人驾驶车辆此时应当选择制动模式，则令s t a i = 2，本控制周期将执行制动模式下的控制算法进行纵向车速控制，并执行步骤6;否则，执行步骤5.4;
[0098]步骤5.4、判断vd< vl是否成立，若成立，表示无人驾驶车辆此时应当选择低速模式，则令stai = 3，本控制周期将执行低速模式下的控制算法进行纵向车速控制，并执行步骤6;否则，执行步骤5.5;
[00"] 步骤5.5、判断vd-vi >va且vd>vl是否成立，若成立，表示无人驾驶车辆此时应当选择加速模式，则令s t a i = 4，本控制周期将执行加速模式下的控制算法进行纵向车速控制，并执行步骤6;否则，执行步骤5.6;
[0100]步骤5.6、表示无人驾驶车辆此时应当选择调节模式，令Stai= 5，本控制周期将执行调节模式下的控制算法进行纵向车速控制，并执行步骤6 ；
[0101]步骤6、根据步骤4或者步骤5确定的第i个控制周期T1下的运动模式状态值StaJA行相应的控制程序，计算无人驾驶车辆的油门和制动控制量thri和brai;
[0102]步骤6.1、判断stai= I是否成立，若成立，本控制周期将执行与起步模式相对应的控制算法，则有brai = 0，起步模式下制动控制量始终为最小值，并执行步骤6.1.1;否则，执行步骤6.2 ；
[0?03] 步骤6.1.1、在第i个控制周期Ti下开始计时，获得起步用时t-upi;当t-Upi<tmax时令thri= ω，此时以一个固定的油门控制量驱动无人驾驶车辆进行起步动作，并执行步骤7;为了防止车辆行驶在坡度较大的上坡路段时(尤其是越野路段)可能会出现的无法行驶的情况，当t-UPi>tmax时，执行步骤6.1.2;
[0104]步骤6.1.2、判断vi>vq是否成立，若成立，则认为无人驾驶车辆此时可以正常行驶，贝Ij维持油门控制量否则，意味着上个控制周期的油门控制量已经不足以驱动车辆继续行驶，此时将thr1-1+ωυρ赋值给thri，通过逐步增大油门控制量给车辆提供更大的动力，并执行步骤6.1.3;
步骤6.1.3、判断thri > thrmax是否成立，若成立，表示油门控制量已达最大值，继续发送更大的油门控制量将对执行机构造成损害，则将thrmax赋值给thri并执行步骤7;否则直接执行步骤7 ；
[0106]步骤6.2、判断stai= 2是否成立，若成立，本控制周期将执行与制动运动模式相对应的控制算法，则执行步骤6.2.1;否则，执行步骤6.3;
[0107]步骤6.2.1、制动模式下油门控制量始终为最小值，令thri= 0;
[0108]步骤6.2.2、判断Pl<-Pit是否成立，若成立，则判定无人驾驶车辆处于下坡路面，则执行步骤6.2.3，否则，判定无人驾驶车辆处于非下坡路面，执行步骤6.2.4；
[0109]步骤6.2.3、判断ai^amin是否成立，若不成立，则表示无人驾驶车辆目前的制动加速度还不会对乘车者的舒适感造成较为明显的影响，则将bra1-ι+βι?赋值给brai，本控制周期将进一步增大制动控制量，执行步骤6.2.5；否则，则意味着如果此时继续增大制动控制量则会给乘车者产生较为明显的不舒适感，本控制周期不可以继续增大制动控制量，此时维持brai为bra1-1，并执行步骤6.2.5；
[0110]步骤6.2.4、判断ai 2 amin是否成立，若不成立，则表示车辆目前的制动加速度还不会对乘车者的舒适感造成较为明显的影响，则将bra^+iV。赋值给brai，本控制周期将进一步增大制动控制量；否则，则意味着如果此时继续增大制动控制量则会给乘车者产生较为明显的不舒适感，本控制周期不可以继续增大制动控制量，维持brai为bra1-1;
[0111]步骤6.2.5、判断brai > bramax是否成立，若成立，表示制动控制量已达最大值，继续发送更大的制动控制量将对执行机构造成损害，则将bramax赋值给brai，并执行步骤7 ；否则直接执行步骤7 ；
[0112]步骤6.3、判断stai= 3是否成立，若成立，本周期将执行与低速运动模式相对应的控制算法，则执行步骤6.3.1，若不成立，则执行步骤6.4 ；
[0113]步骤6.3.1、判断lab1-1 = I是否成立，若成立，意味着无人驾驶车辆在上个控制周期处于低速制动状态，则执行步骤6.3.2，若不成立，则表示无人驾驶车辆目前无需制动，则执行步骤6.3.3；
[0?Μ] 步骤6.3.2.、判断v1-vdi<-vds是否成立，若成立，表示无人驾驶车辆已完成低速制动动作，则brai = O，thri = O，Iabi = O，否则，令brai = brai—ι、令thri = thri—ι，无人驾驶车辆继续进行低速制动动作，并执行步骤7 ；
[0115]步骤6.3.3、判断v1-vdi>vds是否成立，若成立，表示无人驾驶车辆需要进行低速制动动作，则令brai = Pis、thri = O，Iabi = I，无人驾驶车辆进入低速制动状态，若不成立，则令thri = O、brai = O，本控制周期车辆将以怠速状态行驶，并执行步骤7;由此，无人驾驶车辆在制动状态下不会在实际车速刚刚低于期望车速的情况下就立即松开制动踏板，也不会在实际车速刚刚超过期望车速时就立即进行制动动作，同时利用车辆自身的怠速状态进行加速动作，这样不仅保证了无人驾驶车辆在低速状态下实际车速可以在期望车速附近保持相对稳定，也避免了制动控制反复介入进而造成的不舒适感。
[0116]步骤6.4、判断stai= 4是否成立，若成立，本周期将执行与加速运动模式相对应的无人车纵向控制算法，则令brai = 0，加速运动模式下制动控制量始终为最小值，并执行步骤6.4.1;若不成立，则执行步骤6.5;
[0117]步骤6.4.1、判断ai< amax是否成立，若成立，则表示车辆目前的加速加速度还不会对乘车者的舒适感造成较为明显的影响，则令thri = thr1-1+coa。，本控制周期将进一步增大制动量，并执行步骤6.4.2，若不成立，则意味着如果此时继续增大制动控制量则会给乘车者产生较为明显的不舒适感，本控制周期不可以继续增大制动控制量，则令thri = thr1-1，并执行步骤7 ；
[0118]步骤6.4.2、判断thri > thrmax是否成立，若成立，表示油门控制量已达最大值，继续发送更大的油门控制量将对执行机构造成损害，则将thrmax赋值给thri，并执行步骤7 ；否贝IJ，直接执行步骤7;
[0119]步骤6.5、调节运动模式下制动控制量始终为最小值，令brai = 0;
[QUO] 步骤6.5.1、计算第i个控制周期Ti下PID调节算法车速反馈误差Erri = vd1-vi;[0121 ] 步骤6.5.2、计算第i个控制周期Ti下PID调节算法比例控制输出量UPi = KP X Erri ；
[0122]步骤6.5.3、计算第i个控制周期Ti下PID调节算法积分控制输出量