基于最优控制与安全距离的自动驾驶最速操纵控制系统及其控制方法与流程

本发明属于汽车自动驾驶技术领域，具体涉及一种基于最优控制与安全距离的自动驾驶最速操纵控制系统及其控制方法。

背景技术：

随着科技发展与社会进步，自动驾驶技术逐渐成为汽车行业的研究热点问题，一些简单的定速巡航、自适应巡航功能已经在高端车型中应用。然而，目前市面上可以见到的自动驾驶技术尚处于起步阶段，存在些许不足之处。一方面，其自动驾驶程度较低，仅对纵向车速进行控制，不能实现横向转向、加减速的协同控制，无法实现汽车的横向操纵功能；另一方面，自动驾驶车辆可看做存在一个高级的驾驶员模型对汽车进行操纵，但是由于不同驾驶员的预瞄时间、反应时间等的不同，导致很难建立一个统一的驾驶员模型来求解不同驾驶员操纵输入。

技术实现要素：

发明目的：为了克服现有技术中存在的不足，本发明提供一种基于最优控制与安全距离的自动驾驶最速操纵控制系统及其控制方法，该发明可以在避免驾驶员建模的情况下求出汽车的最优操纵输入，可以有效的解决驾驶员建模困难的问题。

技术方案：为实现上述目的，本发明采用的技术方案为：

一种基于最优控制与安全距离的自动驾驶最速操纵控制系统，包括数据采集系统、安全预警系统、最优控制系统、执行机构组成。

数据采集系统包括车辆状态感知系统、环境感知系统。车辆感知系统包括车速传感器、方向盘转角传感器、陀螺仪等，用于记录汽车当前行驶状况；环境感知系统包括视觉识别传感器、雷达传感器、激光传感器，用于识别车道线方位以及环境信息。

安全预警系统由纵向安全距离模型和侧向安全距离模型组成。纵向安全距离模型可根据汽车与前车的相对距离以及相对速度计算制动安全距离以及操纵安全距离。横向安全距离模型通过汽车的行驶状态和环境信息判断汽车行驶所需的横向安全距离。安全距离系统通过纵向安全距离模型和横向安全距离模型的输出结果对驾驶行为进行预警。

最优控制系统包括汽车最优控制模型、最优控制问题的离散方法和非线性规划问题的求解方法组成。其中最优控制模型由边值约束、过程约束、运动微分方程和性能指标组成；之后将建立的最优控制问题离散成非线性规划问题，最后，求解非线性规划问题，得到汽车的最优操纵输入解。

执行机构包括纵向控制器和横向控制器组成。其中，纵向控制器由制动踏板执行器和加速踏板执行器组成；横向控制器由方向盘执行器组成。

同时，本发明还提出了该基于最优控制与安全距离的自动驾驶最速操纵控制系统的控制方法，其控制方法包括以下步骤：

步骤1：数据采集系统采集汽车当前行驶状况、车道线及障碍物方位、周围车辆行驶信息，并采集到的信息包括汽车与障碍物或其他车辆的相对距离d、相对速度v传递到安全预警系统；

步骤2：安全预警系统接收数据采集系统传递的信息，并由纵向安全距离模型分别计算出纵向安全距离dz和操纵安全距离dh。

步骤2.1：d＞dz，此时汽车行驶安全，维持当前行驶状态；

步骤2.2：dz＞d＞dh，汽车进行简单制动操纵，此时控制汽车以较小的制动减速度进行制动；

步骤2.3：dh＞d，此时由横向安全距离模型判断可行性，若横向操纵不可行，则进行紧急制动，以最大的制动减速度进行制动；若横向操纵可行，则汽车进行横向操纵，此时需计算出横向操纵结束时横向安全距离距离xe、纵向安全距离距离ye，作为最优控制系统的终端约束条件，由最优控制系统控制汽车完成相应的横向运动。

步骤3：安全距离系统判定汽车需进行简单制动或紧急制动操纵时，直接将控制信号传送至执行机构，进行相应的纵向控制。

步骤4：安全预警系统判定汽车需进行横向操纵时，将控制命令传送至最优控制系统，其根据安全预警系统制定的操纵策略，给定系统的控制变量、状态变量、约束条件、性能指标，由最优控制模型求解出最速操纵的理想输入。

步骤5：执行机构接收由最优控制系统输出的控制命令，控制纵向控制系统和横向控制系统完成相应的纵横向运动。

步骤6：更新当前的汽车行驶状态以及环境信息，重复以上控制过程。

有益效果：本发明提供的一种基于最优控制与安全距离的自动驾驶最速操纵控制系统及其控制方法，与现有技术相比，具有以下优势：

1、本发明可以在避免驾驶员建模的情况下求解出驾驶员的操纵输入，有效的解决了驾驶员建模困难的问题；

2、本发明所提出的离散方法和插值方法相较于现有的技术具有收敛速度快、求解精度高的优点。

附图说明

图1为控制系统流程图；

图2为安全预警系统工作流程图；

图3为最优控制系统工作流程图。

具体实施方式

本发明为一种基于最优控制与安全距离的自动驾驶最速操纵控制系统，其可根据数据采集系统采集到的自车行驶状态信息及周围环境信息，由安全预警系统制定出纵、横向控制策略；在进行横向操纵时，其根据最优控制理论，给定横向操纵前后的汽车状态，由最优控制算法反求出理想的最速操纵输入，并控制汽车进行预期的横向操纵。

如图1所示，本发明的工作方法为：

步骤1：数据采集系统采集汽车当前行驶状况、车道线及障碍物位、周围车辆行驶信息，并将采集到的信息传递到安全预警系统；

步骤2：安全预警系统接收数据采集系统传递的信息，并由纵向安全距离模型分别计算出制动安全距离dz和操纵安全距离dh。同时，比较d与dz、dh的大小关系，并结合横向安全距离模型制定出纵、横向控制策略。

步骤3：最优控制系统根据安全距离系统制定的纵、横向控制目标，给定系统的控制变量、状态变量、约束条件、性能指标，由最优控制模型求解出最速操纵的理想输入。

步骤4：执行机构接收由最优控制系统输出的控制命令，控制纵向控制器及横向控制器使汽车完成相应的纵、横向运动。

步骤5：更新当前的汽车行驶状态以及环境信息，重复以上控制过程。

如图2所示，安全距离系统的工作方法为：

步骤1：接收数据采集系统信息，并将其传送到纵向安全距离模型、横向安全距离模型。其中，传感器信息主要包括：自车行驶速度、加速度、横摆角速度、方向盘转角、航向角、周围车辆行驶速度、自车与前车的相对距离d、相对速度v，车道线宽度等。

步骤2：纵向安全距离模型由上述传感器信号，计算出纵向安全距离dz、操纵安全距离dh：

其中，

a1＝-0.2g

a2＝-0.5g

其中v为相对速度、a1、a2为制动减速度；t1是以a1为制动减速度时，汽车从开始减速到停止所用时间；t2是以a2为制动减速度时，汽车从开始减速到停止所用时间；

步骤3：判断自车与前车的相对距离与纵向安全距离、操纵安全距离的大小关系：若d＞dz，则此时汽车行驶安全，维持当前行驶状态；

若dz＞d＞dh，则汽车进行简单制动操纵，此时控制汽车以较小的制动减速度进行制动；

若dh＞d，需由横向安全距离模型判断横向操纵可行性，以得到此时的横向操纵策略。

步骤4：横向安全距离模型由当前自车的行驶状态信息、临近车道的车辆行驶状态信息、车道线方位判断此时汽车是否可以进行横向操纵，判断换道可行性：

其中，判断换道可行性的具体方法为：

若相邻车道有车，则换道不可行；

若相邻车道没车，则利用最优控制模型求解换道时所需的操纵输入，若求解出操纵输入则换道可行，通过求解出的操控输入控制汽车完成换道操纵；若求解不出换道操纵则说明在侧向加速度、状态变量、控制变量的约束下汽车不能完成换道操纵，则换道不可行。若横向操纵不可行，则汽车执行紧急制动操作，以汽车的最大制动减速度进行制动；

若横向操纵可行，则汽车执行横向操纵，此时需计算出换道结束时横向安全距离距离xe、纵向安全距离距离ye，作为最优控制系统的终端约束条件。

如图3所示，最优控制系统的工作方法为：

步骤3-1：设定控制变量为方向盘转角δsw(t)。

步骤3-2：设定状态变量为侧向速度、横摆角速度、纵向速度、汽车质心横坐标、汽车质心纵坐标和航向角，即状态变量

x(t)＝{v(t)，ω(t)，u(t)，x(t)，y(t)，θ(t)}^t。

步骤3-3：设定初始边值约束条件，即为当前汽车的行驶状态，此时汽车为直线行驶，只存在纵向速度，即初始边值约束条件

x(0)＝[00u000]

步骤3-4：设定边值约束条件，即为汽车横向操纵完成时的行驶状态及位置信息，即终端边值约束条件

x(tf)＝[00uxfyf0]

其中，tf是终端时刻；

步骤3-5：设定控制变量、状态变量约束条件，受汽车性能及道路条件等因素的影响，汽车要满足一定的状态变量和控制变量约束以保证顺利完成横向操纵。因此，建立如下的约束条件：

x(t)min≤x(t)≤x(t)max

其中，x(t)min是状态变量变化范围的下界；x(t)max是状态变量所允许的变化范围的上界；

步骤3-6：设定过程约束条件，考虑到防止汽车在横向操纵过程中发生侧翻，建立过程约束条件：|ay|≤3m/s²。即u是纵向速度。

步骤3-7：设定性能指标，此时控制目标为以最短时间完成横向操纵。因此其性能指标为：

其中，j(z)为性能指标，t0为初始时刻，tf为终止时刻；

步骤3-8：建立汽车的操纵动力学模型，其中包括汽车运动微分方程。

步骤3-8.1：建立汽车三自由度运动模型

其中，v为汽车的侧向速度；u为汽车的纵向速度；ωr为汽车的横摆角速度；m为整车总质量；iz为整车绕铅垂轴转动惯量；a、b分别为整车质心至前、后轴的距离；k1、k2分别为前、后轮综合侧偏刚度；δ为前轮转角；fyf为前轮侧偏力；fyr为后轮侧偏力；fxf为前轮驱动力/制动力(fxf≥0为驱动力，fxf＜0为制动力)；fxr为后轮驱动力/制动力；ff为滚动阻力(ff＝mgf，f为滚动阻力系数)；fw为空气阻力(fw＝cda(3.6u)²/21.15，cd为空气阻力系数，a为迎风面积)；

步骤3-8.2：建立汽车运行轨迹方程

其中，θ为航向角；

步骤3-8.3：建立航向角运动微分方程：

步骤3-9：利用radau伪谱法将连续最优控制问题离散化。由于radau伪谱法的配点都分布在区间(-1，1]上，因此采用radau伪谱法对以上最优控制问题进行求解时需将最优控制问题的时间区间t∈[t0，te]转换成τ∈[-1，1]，对时间变量t作变换：τ＝2t/(te-t0)-(te+t0)/(te-t0)，将上述的运动微分方程、性能指标和约束条件转换为如下形式的最优控制问题转换式：

minj＝ψ(x(τe)，te)

c[x(τ)，z(τ)，τ；t0，te]≤0

k阶lgr点是多项式pk(τ)-pk-1(τ)的根，其中pk(τ)即为k阶legendre多项式。为使节点能覆盖区间端点，radau伪谱法的节点为配点与初始时间点τ0＝-1。当节点个数为n时，则配点个数为k(k＝n-1)，即配点取n-1阶lgr点。

采用n个拉格朗日插值多项式li(τ)(i＝0，1，…，n-1)为基函数来近似变量：

其中，lagrange插值多项式函数

使得节点上的近似状态与实际状态相等，即x(τi)＝x(τi)。

采用lagrange插值多项式l^*i(τ)，(i＝1，…，n-1)作为基函数来近似控制变量，即：

式中，

将动力学微分方程约束转换为代数约束，即：

其中微分矩阵dki表达式为：

其中，g(τi)＝(1+τi)[pk(τi)-pk-1(τi)]，式中τi为节点，τk为配点。将上述步骤3-9中的最优控制问题转换式在配点τk处离散。这样，可将最优控制问题的动力学微分方程约束转换为代数约束，对于k＝1，…，n有：

步骤3-10：求解步骤3-9中的非线性规划问题。用到的优化方法是sqp算法的一种：wilson-han-powell方法，非线性规划问题转化为求解一系列的二次规划问题。

考虑一般非线性约束最优控制问题

式中，f(x)，ci(x)都是实值连续函数并且至少两者有其一是非线性的，e＝{1，2，…，me}，

i＝{me+1，…，m}，构造子问题

其中gk是函数f(x)在点xk的梯度，bk是拉格朗日函数的海色阵的近似，d是n维状态向量。记上述子问题的解为dk。

给出

x1∈rⁿ，σ＞0，δ＞0，b1∈r^n×n，ε≥0，k：＝1

求解上述子问题得到dk；如果||dk||≤ε，则停；求αk∈[0，δ]使得

xk+1＝xk+αkdk；计算bk+1；k：＝k+1；

用拟牛顿公式逐步迭代来计算bk+1，取

sk＝xk+1-xk，

采用bfgs校正公式计算bk+1

对于本文时变系统的最优控制问题，可利用上述直接配置法思想转换成有限维非线性规划问题来求解：

1：对给定的划分区间n等分，得到n+1个节点。

2：sj作为所选节点处控制变量的估计值，若控制变量值已知，则利用状态变量的初值xj顺次迭代求得各节点的状态变量值，故可求得xn+1及性能指标j。所以把状态方程的解及性能指标值看成是各节点控制变量的函数。利用非线性规划算法求出汽车所需的最优操纵输入。

步骤3-11：将步骤3-10中得到的最优操纵输入信号经d/a转换传给执行机构，执行机构通过控制信号完成相应的纵、横向操纵任务。

本发明公开了一种基于最优控制与安全距离的自动驾驶最速操纵控制系统。该系统包括数据采集系统、安全预警系统、最优控制系统和执行机构。其具体控制方法为：通过车载数据采集系统获取周围环境信息和汽车行驶状态。并通过采集到的信息确定汽车行驶的初始状态和终端状态；结合周围环境信息与安全预警模型判断汽车是否需要横向操纵；当需要横向操纵时，将汽车初始状态和终端状态输入到最优控制系统中，利用最优控制模型求解出汽车的最优操控输入，之后，将最优操控信号经d/a转换输入到执行机构中，由执行机构控制汽车完成纵、横向运动。本发明的控制系统及控制方法提高了自动驾驶汽车纵横向运动的安全性、快速性。

以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。