一种失控车辆再稳定的控制方法
技术领域
本发明属于自动驾驶
技术领域
，具体涉及一种失控车辆重新进入稳定状态的控制方法。背景技术
车辆侧滑(尤其是后轮侧滑)导致的车辆失控对安全行车有较大威胁，是造成碰撞、翻车等恶性交通事故的主要原因之一。据统计，有超过一半的侧滑导致的事故是在驾驶员制动和转弯时发生的，因此，车辆稳定性系统的研发被高度重视。
车身电子稳定系统(ESP，Electronic Stability Program)即是通过对各传感器的信息进行分析，然后向防抱死刹车系统(ABS，Anti-lock Brake System)、电子刹车分配力系统(EBD，Electrical Brake Distribution)等发送指令，精确调节各车轮的制动力并对侧向力施加影响，从而提高车辆的操控性能，防止车辆达到其动态极限时失控，有效地检测并预防车辆侧滑。
虽然当前大部分车辆都安装有ESP，但当车辆受到突然的外部刺激或者处于极端场景而导致侧滑已然发生，且产生较大的侧偏角，那么此时ESP是无法有效地预防事故发生的。比如，高速行驶中猛打方向，在下坡或雨雪湿滑路面紧急制动、转弯等，极易发生侧滑；此时，经验丰富的驾驶员可以依赖已有经验沉着应对，但大部分驾驶员可能不知所措，导致事故发生。
为解决车辆侧滑失控的技术问题，目前主要采取下述方案：
(1)中国专利CN108839652A公开的《一种车辆失稳可控域的自动驾驶紧急避让系统》，该专利利用通用的车辆非线性动力学模型，基于分叉理论计算失稳可控区域的联动控制区域，将数据分别传送给前轮控制系统和后轮控制系统，通过转向助力系统EPS和车身电子稳定系统ESP等执行计算结果，最终实现紧急避让障碍物。该发明的主要创新点是基于分叉理论计算车辆失稳可控域及相关的稳定性分析，但未考虑特殊场景下重力分量对于车辆动力学模型的影响，且未讨论环境约束对于失稳状态的影响及车辆退出失稳区域的机制和方法。
(2)英国专利GB2492543B公开的《A vehicle sideslip control system andmethod》，该专利涉及一种侧偏角保持的控制方法，提出了在转弯时保持非零侧偏角的控制系统，该系统包含一种控制器来产生应用于车轮的正或负的驱动力，从而保持相对于期望路径的侧偏角。该专利提出了进入和退出控制系统的条件，但未讨论特殊场景对于车辆动力学模型、大侧偏角状态的转弯半径及控制方法退出的影响。
(3)论文《Steady-state drifting stabilization of RWD vehicles》(Veleniset al.2011)，涉及一种车辆在大侧偏角下维持漂移稳态的控制方法，该方法建立车辆的动力学模型，针对特定稳态，使用backstepping控制器对车辆进行控制，使车辆维持漂移稳态。该方法仅涉及漂移稳态的求解及控制，未考虑在斜坡场景下重力因素对于动力学模型的影响，且无驶出控制方法。
发明内容
本发明的目的在于提供一种失控车辆再稳定的控制方法，该方法针对大侧偏角的失控车辆，在有空间约束的环境中，提出利用车辆漂移控制技术将车从失控状态调整到漂移稳态，再驶出漂移稳态进入可操控状态，该方法有效解决大侧偏角失控车辆无法自动进入稳定状态的技术难题。
为实现上述目的，本发明采用如下技术方案：
一种失控车辆再稳定的控制方法，具体包括以下步骤：
步骤S1、获取传感器信息，利用重力加速度、车辆加速度、轮胎力信息建立路面模型，使用θ角和α角表征当前车身与路面的相对角度关系；其中，θ为路面法向量与世界坐标系Z轴的夹角，α为车头朝向和处于地面切平面上的重力分量的夹角；
步骤S2、依据车辆侧偏角的大小，同时结合后轮所受到的侧向力大小，判断车辆是否处于失控状态；如果是，则考虑重力分量建立车辆的漂移动力学模型，之后以当前车辆状态为初始条件，考虑环境边界，优化求取漂移稳态；
步骤S3、利用反馈控制方法按照步骤S2求得的稳态解控制车辆维持漂移稳态；
步骤S4、获取车道信息，计算车辆与道路的相对姿态，判断是否满足驶出漂移稳态的条件；如果是，则使用驶出控制方法控制车辆脱离漂移状态，进入正常行驶状态；
步骤S5、判断是否满足移交车辆控制权条件；如果是，则向驾驶员或者其他自动驾驶控制模块移交控制权。
作为本发明的优选，步骤S1中θ角和α角的计算过程如下：
考虑到车辆本身的roll和pitch角，建立如下关系式：
maxcosφpitch＝∑Fijx+mg sinθcosα (1)
maycosφroll＝∑Fijy+mg sinθsinα (2)
其中，m为车辆质量，g为重力加速度，θ为路面法向量与世界坐标系Z轴的夹角，α为车头朝向和处于地面切平面上的重力分量的夹角，φroll为车辆绕车辆坐标系x轴的旋转角度，φpitch为车辆绕车辆坐标系y轴的旋转角度，Fijx,Fijy为车轮所受到的纵向力和侧向力，i∈{F，R}，F表示前轮，R表示后轮，j∈{R，L}，R表示右轮，L表示左轮；ax为车辆纵向加速度，ay为车辆侧向加速度；
由(1)和(2)可得到：
对θ进行判断，若|θ|≤θthres，则可认为当前路面为绝对平面，此时α为任意值，其中θthres为预先设定的阈值；
若|θ|＞θthres，α可按公式(4)求解：
α为与车头方向有关的变量，取值范围为(-π，π]，在局部范围内，θ与道路设计有关，可认为是常量，取值范围应满足道路设计指标。
作为本发明的优选，步骤S2中车辆的漂移动力学模型如下：
其中，车辆围绕车辆坐标系z轴的旋转角速度表示为则角加速度为车头朝向和处于地面切平面上的重力分量的夹角α、车辆重心速度V、车身侧偏角β关于时间的导数分别记为和各个车轮的转速及转速关于时间的导数分别记为ωij和i∈{F，R}，F表示前轮，R表示后轮，j∈{R，L}，R表示右轮，L表示左轮；Iz为车身绕车辆坐标系z轴的转动惯量，Iw为车轮的转动惯量，Tij为施加在各车轮上的驱动力矩，i∈{F，R}，F表示前轮，R表示后轮，j∈{R，L}，R表示右轮，L表示左轮；rw为车轮半径；δ为前轮转角；β为车辆侧偏角；D为同一轮轴上两轮的轮心距；Lf为重心与前轮轴水平距离；Lr为重心与后轮轴水平距离；
对于轮胎力Fijx和Fijy，可表示为：
摩擦系数同样可表示为类似函数：
计算载荷转移时，同样需要考虑重力分量的影响：
两个前轮垂向力合力为
两个后轮垂向力合力为
其中，h为车辆重心离地面高度；
若假设
由公式(12)和(13)，可以计算得到四个车轮的垂向力如下：
综上，在倾斜平面上考虑载荷转移的车辆的漂移动力学模型即可建立。
作为本发明的优选，步骤S2中优化求解漂移稳态时，只需考虑单侧边界对于漂移的约束，以向右侧侧滑为例，优化求解漂移稳态的过程如下：
假设右侧道路边界已通过视觉感知系统得到，可以表示为y边界＝C0+C1x+C2x2+C3x3，该多项式曲线为道路边界在车辆坐标系中的表示；其中，点(x，y边界)为边界曲线上的点在车辆坐标系下的坐标，C0，C1，C2，C3为多项式系数；
当车辆以侧偏角β进行逆时针稳态漂移，稳态漂移轨迹可以表示为当前时刻车辆坐标系下的圆，该圆圆心(xO，yo)满足如下关系：
则该圆的方程为：
(x+R sinβ)2+(y漂移-R cosβ)2＝R2 (20)
因车辆向右侧侧滑，所以x＝0时，y边界一定为负，这样仅需令漂移圆满足边界条件：y漂移＞y边界+d膨胀，即：
其中，d膨胀是为安全考虑而预先给定的膨胀深度；此不等式方程不易求解，可以将x在(0，R(1-sinβ)]区间离散化后，建立多个点处的不等式约束；假设将(0，R(1-sinβ)]采样N个点则可建立不等式方程组：
将当前时刻车速V、车辆侧偏角β作为稳态常量，将动力学模型(5)-(18)离散化，建立目标函数
fobj是一个非线性函数，因此选用一个非线性优化求解器对其进行优化求解，同时满足约束条件(22)以及求得稳态的R和其中，是车身横摆角速度绝对值的最大值。
作为本发明的优选，步骤S4中计算车辆与道路的相对姿态的过程如下：
首先需要获取两侧车道线或者道路边界曲线方程，假设通过感知模块感知得到的左侧车道线或者路边界在车辆坐标系中的曲线方程为y＝C0L+C1Lx+C2Lx2+C3Lx3，右侧车道线或者路边界在车辆坐标系中的曲线方程为y＝C0R+C1Rx+C2Rx2+C3Rx3，其中，C0L～C3L、C0R～C3R是各曲线方程的多项式系数；
之后根据道路边界曲线方程计算道路方向ζ，计算道路方向的方法是：
(1)获取加权权值：
(2)将左、右两条曲线在x＝0点处的切线方向与车辆坐标系X轴的夹角分别表示为ζL和ζR，则获取两侧朝向：
(3)将当前道路相对于车辆坐标系的方向角表示为ζ，则可获取道路朝向：
ζ＝pζL+qζR (26)
最后对侧滑方向进行判断，并根据计算得到的道路方向ζ判断车头指向；在对侧滑方向判断时，如是向左侧滑，则判断ζ＞0？；是，则车头指向道路内侧；否，则判断ζ＝0？；是，则车头与道路方向平行；否，则车头指向道路外侧；如不是向左侧滑，则判断ζ＜0？；是，则车头指向道路内侧；否，则判断ζ＝0？；是，则车头与道路方向平行；否，则车头指向道路外侧。
作为本发明的进一步优选，步骤S4中驶出漂移稳态的判断过程如下：
基于得到的车头指向，需要考虑以下三种情况，以判断是否驶出：
(1)判断车头是否朝向道路外侧？如当车头方向指向道路外侧时，则继续进行稳态漂移控制；
(2)判断车头方向与道路方向是否平行？将道路方向角的绝对值记为|ζ|，当|ζ|在误差范围内，则可认为当前车头方向与道路方向平行，可根据当前道路双侧的曲线方程计算理想驶出状态并进行驶出控制；
(3)判断稳态漂移时间是否大于阈值？如当车头方向指向道路内侧且漂移状态持续时间超过规定时间，则驶出状态按照道路单侧曲线方程计算并进行驶出控制。
作为本发明的进一步优选，步骤S4中驶出控制方法的具体控制过程如下：
确定驶出状态，如车头方向与道路方向平行，此时，驶出状态需要与道路方向相适应，驶出的轨迹是圆弧或者直线，具体计算流程如下：
(1)记左、右两条曲线在x＝0点处的曲率为κL和κR，使用双侧车道线或者道路边界曲线方程计算双侧曲率：
(2)记当前道路曲率为κ，使用加权权值计算驶出曲率：
κ＝pκL+qκR (28)
(3)驶出速度为当前车速V，计算驶出角速度：
(4)驶出侧偏角应满足|β|＜βmax，其中βmax为车辆正常行驶时的侧偏角阈值；
如车头方向指向道路内侧，此时的驶出状态仅需考虑单侧的车道线或道路边界约束，驶出状态的轨迹应为圆弧，该圆弧与单侧曲线应只有一个交点或者无交点，可建立如下方程组：
其中，为驶出状态的轨迹半径，若要保证该方程组仅有一个解或无解，R的取值范围不易通过解析的方式求得，所以可采用如下基于采样的方法；
由(30)可得
利用道路方向角ζ进行判断：
若ζ＜0：则此时
若ζ＞0：则此时
欲求目标式的最小值或者最大值，可将x在(0，H)区间内以一定分辨率采样，H为该单侧曲线方程的有效检测距离，假设采样序列为{xk，k＝1，...，N}，则目标式的最小值、最大值可等效于：
此外，还应满足
若上述不等式联立无解，则设定驶出角速度为0，即按照直线驶出；
驶出状态已解出，可以此为初值，使用步骤S2中的非线性优化求解方法，求得驶出状态的稳态解，并选用控制器进行控制。
本发明的优点和积极效果是：
(1)本发明提出一种基于模型和控制理论、利用车辆的漂移特性，解决大侧偏角的侧滑失控问题，该方法能够控制大侧偏角的失控车辆重新回到可操控的稳定状态，而非依赖于人的经验性操作，从而提高人员和车辆的安全性。
(2)本发明利用传感器信息对当前所处路面进行建模，用一种更为简洁的二自由度描述方式对该平面进行表征，使模型表示和求解更加简单。
(3)本发明提供的方法考虑路面平面模型，在车辆漂移动力学模型中考虑重力分量的影响，建立带有重力分量的车辆漂移状态下的动力学模型，使模型更加准确；同时考虑环境的空间约束，优化求解漂移稳态，使得求解出的漂移稳态更加准确，更有利于解决实际驾驶场景下的漂移稳态求解问题。
(4)本发明提供的方法当判断车辆状态满足驶出漂移稳态的条件时，提出一种控制车辆驶出漂移稳态的方法，之后进入正常行驶状态，以便于驾驶员或者其他控制模块接管控制权，在驶出漂移稳态时，考虑道路方向和曲率，解算出适应于道路的驶出状态从而加以控制。
附图说明
图1为本发明车辆再稳定控制方法的流程图；
图2为建立路面模型的流程图；
图3为车辆坐标系与世界坐标系的示意图；
图4为轮胎力及轮胎转角的示意图；
图5为α夹角与θ夹角的示意图；
图6a为不合适的漂移稳态半径导致车辆的行驶轨迹与道路边界相交的示意图；
图6b为环境边界约束下漂移稳态半径的示意图；
图7为控制车辆驶出漂移状态的流程图；
图8为计算车头与道路相对姿态的流程图；
图9为车头方向指向道路外侧的示意图；
图10为车头方向与道路方向平行的示意图；
图11为车头方向指向道路内侧的示意图；
图12为驶出条件判断过程的流程图；
图13为驶出横摆角速度的计算流程图；
图14为本发明车辆再稳定控制方法的总体流程图。
具体实施方式
下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
参阅图1、图14，本发明的目的是提供一种控制失控车辆重新进入可操控状态的方法，该方法具有包括以下步骤：
第一步，获取IMU(惯性测量单元)等传感器信息，利用加速度、轮胎力等信息建立路面模型，使用θ角和α角表征当前车身与路面的相对角度关系；其中，θ为路面法向量与世界坐标系Z轴的夹角，α为车头朝向和处于地面切平面上的重力分量的夹角；
第二步，依据车辆侧偏角的大小，同时结合后轮所受到的侧向力大小，判断车辆是否处于失控状态；如果是，则考虑重力分量建立车辆的漂移动力学模型，之后以当前车辆状态为初始条件，考虑环境边界，优化求取漂移稳态；优化求取漂移稳态时，需要判断是否超出最大求解次数？如是，则提醒驾驶员介入；否则判断是否有解？如是，则判断是否超时？如否，则求得稳态的解；
第三步，利用反馈控制方法按照第二步求得的稳态解控制车辆维持漂移稳态；
第四步，获取车道信息，计算车辆与道路的相对姿态，判断是否满足驶出漂移稳态的条件；如果是，则使用驶出控制方法控制车辆脱离漂移状态，进入正常行驶状态；
第五步，判断是否满足移交车辆控制权条件；如果是，则向驾驶员或者其他自动驾驶控制模块移交控制权。
(1)路面模型
假设局部的行驶路面为平面，该路面平面相对于世界坐标系而言，有一个三维的旋转。考虑到路面的性质，我们只需考虑路面沿道路方向的坡度(slope)和垂直道路方向的倾角(bank angle)。现有方案多只考虑其中的沿道路方向的坡度进行建模，或者考虑二者，但需要使用角速度积分并多次转换坐标系等较为复杂的处理。
本发明利用传感器返回的重力加速度、车辆加速度、轮胎力等信息对路面进行建模，使用两个角度即可表征当前车身与该路面的相对角度关系；图2是本步骤的算法流程图，具体过程如下：
(1.1)获取加速度、轮胎力等信息
首先从加速度传感器获取车辆在纵向和侧向的加速度信息，从重力传感器获取重力加速度方向，利用轮胎模型或者轮胎力传感器获取到轮胎在纵向和侧向所受到的轮胎力，以及轮胎相对于车辆坐标系的转角；车辆坐标系可依据图3建立，车辆绕x轴的旋转角度为roll角，表示为φroll；车辆绕y轴的旋转角度为pitch角，表示为φpitch；
轮胎力、轮胎转角等的表示如图4；关于各符号的说明如下：
Xworld,Yworld,Zworld：世界坐标系的坐标轴
x,y,z：车辆坐标系的坐标轴
D：同一轮轴上两轮的轮心距
Lf：重心与前轮轴水平距离
Lr：重心与后轮轴水平距离
V；车辆重心处的速度
β：车辆侧偏角
δ：前轮转向角
Fijx,Fijy：车轮所受到的纵向力和侧向力，i∈{F，R}，F表示前轮，R表示后轮，j∈{R，L}，R表示右轮，L表示左轮；
车辆纵向加速度表示为ax，侧向加速度表示为ay。
(1.2)路面坡角
考虑到车辆本身的roll和pitch角，我们可以建立如下关系式：
maxcosφpitch＝∑Fijx+mg sinθcosα (1)
maycosφroll＝∑Fijy+mg sinθsinα (2)
其中，m为车辆质量，g为重力加速度，θ为路面法向量与世界坐标系Z轴的夹角，α为车头朝向和处于地面切平面上的重力分量的夹角，如图5所示。
由(1)和(2)可得到：
需要注意的是，求得的θ为三维空间中两个向量的夹角，此时路面法向量的朝向并未得到。
(1.3)重力分量方向
对θ进行判断，若|θ|≤θthres，则可认为当前路面为绝对平面，此时α为任意值，且对后续建模及求解没有影响，其中θthres为预先设定的阈值，为较小量。
若|θ|＞θthres，α可按公式(4)求解：
α为与车头方向有关的变量，取值范围为(-π，π]；而在局部范围内，θ与道路设计有关，可认为是常量，取值范围应满足道路设计指标，通常不会太大。在某一瞬时，斜坡平面相对于车辆的姿态可以被θ和α唯一确定，因此可以使用滤波算法对这两个角分别进行处理，从而使得路面平面在局部范围内更加稳定。
(2)动力学模型
通常可以依据车辆侧偏角的大小，同时结合后轮所受到的侧向力大小来判断车辆是否处于失控状态(后轮侧滑)。
假设车辆围绕车辆坐标系z轴的旋转角速度表示为则角加速度为将车头朝向和处于地面切平面上的重力分量的夹角α、车辆重心速度V、车身侧偏角β关于时间的导数分别记为和如前所述，因为在局部路面下可以认为路面为固定平面，因此重力分量的方向在局部是稳定的，因此有关系式m为车身质量，各个车轮的转速及转速关于时间的导数分别记为ωij和其中，i∈{F，R}，F表示前轮，R表示后轮，j∈{R，L}，R表示右轮，L表示左轮。此时，可建立车辆的漂移动力学模型如下：
其中，Iz为车身绕z轴的转动惯量，Iw为车轮的转动惯量，Tij为施加在各车轮上的驱动力矩，i∈{F，R}，F表示前轮，R表示后轮，j∈{R，L}，R表示右轮，L表示左轮；rw为车轮半径，δ为前轮转角。相比于现有技术方案，本发明在动力学模型中考虑了重力分量。
对于轮胎力Fijx和Fijy，可以挑选任意一种轮胎模型建模，比如Uni-tire或者MF模型等。可表示为：
摩擦系数同样可表示为类似函数：
计算载荷转移时，同样需要考虑重力分量的影响：
两个前轮垂向力合力为
两个后轮垂向力合力为
其中，g为重力加速度，h为车辆重心离地面高度。
若假设
其中，D为同一轮轴上的两轮轮心距，由公式(12)和(13)，可以计算得到四个车轮的垂向力如下：
综上，在倾斜平面上考虑载荷转移的车辆动力学模型即可建立。
(3)优化求解
当车辆处于理想的漂移稳态，如下物理量保持常量：漂移半径R、车速V、横摆角速度(车辆围绕车辆坐标系z轴的旋转角速度)车辆侧偏角β。现有求解漂移稳态的技术方案通常认为漂移场景不受空间约束，因此对漂移半径R已做提前规定。本技术方案依据环境边界求取漂移半径的取值范围，并将其作为一个优化变量，最终与其他参数同时优化。如图6(a)所示，当车辆发生侧滑，若不采取措施或者漂移稳态的半径不合适，则可能导致车辆的行驶轨迹与道路边界相交，从而引发事故；本发明的目的在于控制车辆处于一个符合环境边界约束的漂移稳态，环境边界约束主要体现在对于漂移稳态半径的限制，如图6(b)所示。
事实上，我们只需考虑单侧边界对于漂移的约束，即分如下两种情况：
-当侧滑方向为右侧时，仅需考虑右侧道路边界对于漂移的约束；
-当侧滑方向为左侧时，仅需考虑左侧道路边界对于漂移的约束。
下面以向右侧侧滑为例，假设右侧道路边界已通过视觉感知系统得到，可以表示为y边界＝C0+C1x+C2x2+C3x3，该多项式曲线为道路边界在车辆坐标系中的表示。其中，点(x，y边界)为边界曲线上的点在车辆坐标系下的坐标，C0，C1，C2，C3为多项式系数。
当车辆以侧偏角β进行逆时针稳态漂移，稳态漂移轨迹可以表示为当前时刻车辆坐标系下的圆，该圆圆心(xO，yO)满足如下关系：
则该圆的方程为：
(x+R sinβ)2+(y漂移-R cosβ)2＝R2 (20)
因车辆向右侧侧滑，所以x＝0时，y边界一定为负，这样仅需令漂移圆满足边界条件：y漂移＞y边界+d膨胀，即：
其中，d膨胀是为安全考虑而预先给定的膨胀深度。此不等式方程不易求解，可以将x在(0，R(1-sinβ)]区间离散化后，建立多个点处的不等式约束。假设将(0，R(1-sinβ)]采样N个点则可建立不等式方程组：
将当前时刻车速V、车辆侧偏角β作为稳态常量，将前述动力学模型(5)-(18)离散化，建立目标函数
fobj是一个非线性函数，因此选用一个非线性优化求解器对其进行优化求解，同时满足约束条件(22)以及求得稳态的R和其中，是车身横摆角速度绝对值的最大值，受物理条件限制。
如果求得了最优解，可选用一种控制方法进行控制，控制量为前轮转角δ和各车轮的驱动力矩Tij，控制方法如：MPC(Model Predictive Control)、LQR(Linear QuadraticRegulator)、BackStepping等。
对于向左侧侧滑，处理方法与此类同。
(4)驶出控制
当车辆维持漂移稳态一定时长或到达驶出位置后，即可控制车辆驶出漂移状态，进入正常行驶状态，主要流程如图7，具体步骤为：先获取车道信息，车道信息可通过视觉感知模块或者高精地图得到；之后判断是否满足驶出条件，当满足满足驶出条件时，则计算驶出状态，控制求解，应用控制量，判断是否脱离失控状态，如是，则移交控制权；如否，则继续控制求解。
(4.1)判断是否满足驶出条件
(4.1.1)计算车辆与道路的相对姿态，主要流程如流程图8所示。
首先需要获取两侧车道线或者道路边界曲线方程。假设通过感知模块感知得到的左侧车道线或者路边界在车辆坐标系中的曲线方程为y＝C0L+C1Lx+C2Lx2+C3Lx3，右侧车道线或者路边界在车辆坐标系中的曲线方程为y＝C0R+C1Rx+C2Rx2+C3Rx3，其中，C0L～C3L、C0R～C3R是各曲线方程的多项式系数；
之后根据道路边界曲线方程计算道路方向ζ，计算道路方向的方法是：
(1)获取加权权值：
(2)将左、右两条曲线在x＝0点处的切线方向与车辆坐标系X轴的夹角分别表示为ζL和ζR，则获取两侧朝向：
(3)将当前道路相对于车辆坐标系的方向角表示为ζ，则可获取道路朝向：
ζ＝pζL+qζR (26)
最后对侧滑方向进行判断，并根据计算得到的道路方向ζ判断车头指向；在对侧滑方向判断时，如是向左侧滑，则判断ζ＞0？；是，则车头指向道路内侧；否，则判断ζ＝0？；是，则车头与道路方向平行；否，则车头指向道路外侧；如不是向左侧滑，则判断ζ＜0？；是，则车头指向道路内侧；否，则判断ζ＝0？；是，则车头与道路方向平行；否，则车头指向道路外侧。
(4.1.2)判断驶出条件，主要流程如图12所示：
基于(4.1.1)图8流程中得到的车头指向(车头与道路相对姿态)，需要考虑以下三种情况，以判断是否驶出：
(1)判断车头是否朝向道路外侧？如当车头方向指向道路外侧时(如图9所示)，则继续进行稳态漂移控制；
(2)判断车头方向与道路方向是否平行？将道路方向角的绝对值记为|ζ|，当|ζ|在误差范围内，则可认为当前车头方向与道路方向平行(如图10所示)，可根据当前道路双侧的曲线方程计算理想驶出状态并进行驶出控制(见4.2)，达到驶出状态(比如S点)后移交控制权。
(3)判断稳态漂移时间是否大于阈值？如当车头方向指向道路内侧且漂移状态持续时间超过规定时间(如图11所示)，则驶出状态按照道路单侧曲线方程计算并进行驶出控制，到达驶出状态(比如S点)后移交控制权。
(4.2)计算驶出状态
如(4.1.2)所述，驶出状态分两种，下面分别讨论。
(4.2.1)车头方向与道路方向平行
此时，驶出状态需要与道路方向相适应，驶出的轨迹是圆弧或者直线，具体计算流程如下：
(1)记左、右两条曲线在x＝0点处的曲率为κL和κR，使用(4.1.1)节中的双侧车道线或者道路边界曲线方程计算双侧曲率：
(2)记当前道路曲率为κ，使用(4.1.1)节中的加权权值计算驶出曲率：
κ＝pκL+qκR (28)
(3)驶出速度为当前车速V，计算驶出角速度：
(4)驶出侧偏角应满足|β|＜βmax，其中βmax为车辆正常行驶时的侧偏角阈值。
(4.2.2)车头方向指向道路内侧
此时的驶出状态仅需考虑单侧的车道线或道路边界约束，驶出状态的轨迹应为圆弧，该圆弧与单侧曲线应只有一个交点或者无交点，可建立如下方程组：
其中，为驶出状态的轨迹半径，若要保证该方程组仅有一个解或无解，R的取值范围不易通过解析的方式求得，所以可采用如下基于采样的方法；
由(30)可得
利用(4.1.1)节的道路方向角ζ进行判断：
若ζ＜0：则此时
若ζ＞0：则此时
欲求目标式的最小值或者最大值，可将x在(0，H)区间内以一定分辨率采样，H为该单侧曲线方程的有效检测距离，假设采样序列为{xk，k＝1，...，N}，则目标式的最小值、最大值可等效于：
此外，还应满足
若上述不等式联立无解，则设定驶出角速度为0，即按照直线驶出。
综上，驶出横摆角速度的计算流程如图13所示：
同样，驶出侧偏角应满足|β|＜βmax，其中βmax为车辆正常行驶时的侧偏角阈值。
至此，驶出状态已解出，可以此为初值，使用类似前述非线性优化求解方法，求得驶出状态的稳态解，并选用一种控制器进行控制。
当车辆维持驶出状态时长超过阈值且判断车辆已经脱离失控状态，则向驾驶员或者其他自动驾驶控制模块移交控制权。
