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

Abstract

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

Description

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

技术领域

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

背景技术

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

发明内容

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

附图说明

图1为控制系统流程图；

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

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

具体实施方式

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

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

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

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

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

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

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

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

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

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

其中，

a₁＝-0.2g

a₂＝-0.5g

其中V为相对速度、a₁、a₂为制动减速度；t₁是以a₁为制动减速度时，汽车从开始减速到停止所用时间；t₂是以a₂为制动减速度时，汽车从开始减速到停止所用时间；

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

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

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

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

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

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

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

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

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

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

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

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

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

x(0)＝[0 0 u 0 0 0]

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

x(t_f)＝[0 0 u x_f y_f 0]

其中，t_f是终端时刻；

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

X(t)_min≤X(t)≤X(t)_max

其中，X(t)_min是状态变量变化范围的下界；X(t)_max是状态变量所允许的变化范围的上界；

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

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

其中，J(Z)为性能指标，t₀为初始时刻，t_f为终止时刻；

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

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

其中，v为汽车的侧向速度；u为汽车的纵向速度；ω_r为汽车的横摆角速度；m为整车总质量；I_z为整车绕铅垂轴转动惯量；a、b分别为整车质心至前、后轴的距离；k₁、k₂分别为前、后轮综合侧偏刚度；δ为前轮转角；F_yf为前轮侧偏力；F_yr为后轮侧偏力；F_xf为前轮驱动力/制动力(F_xf≥0为驱动力，F_xf＜0为制动力)；F_xr为后轮驱动力/制动力；F_f为滚动阻力(F_f＝mgf，f为滚动阻力系数)；F_w为空气阻力(F_w＝C_DA(3.6u)²/21.15，C_D为空气阻力系数，A为迎风面积)；

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

其中，θ为航向角；

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

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

min J＝ψ(x(τ_e)，t_e)

C[x(τ)，z(τ)，τ；t₀，t_e]≤0

K阶LGR点是多项式P_K(τ)-P_K-1(τ)的根，其中P_K(τ)即为K阶Legendre多项式。为使节点能覆盖区间端点，Radau伪谱法的节点为配点与初始时间点τ₀＝-1。当节点个数为N时，则配点个数为K(K＝N-1)，即配点取N-1阶LGR点。

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

其中，Lagrange插值多项式函数

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

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

式中，

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

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

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

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

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

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

I＝{m_e+1，…，m}，构造子问题

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

给出

x₁∈Rⁿ，σ＞0，δ＞0，B₁∈R^n×n，ε≥0，k：＝1

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

x_k+1＝x_k+α_kd_k；计算B_k+1；k：＝k+1；

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

s_k＝x_k+1-x_k，

采用BFGS校正公式计算B_k+1

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

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

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

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

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

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

Claims (10)

1.一种基于最优控制与安全距离的自动驾驶最速操纵控制系统，其特征在于：包括依次连接的：

数据采集系统，包括车辆状态感知系统和环境感知系统，用于采集汽车在行驶过程中的环境信息和汽车状态信息，确定汽车行驶的初始状态和终端状态，并输入到安全预警系统中；

安全预警系统，用于根据环境信息和汽车状态信息与安全预警模型判断汽车是否需要横向操纵来避免发生风险，当需要横向操纵时，将汽车初始状态和终端状态输入到最优控制系统中；

最优控制系统，用于利用最优控制模型求解出汽车的最优操控输入，并将最优操控信号经D/A转换输入到执行机构中；

执行机构，用于根据接收到的操纵输入完成纵、横向运动操纵任务，实现自动驾驶汽车。

2.根据权利要求1所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统，其特征在于：所述的数据采集系统为车载传感器系统，包括：

环境感知系统，包括雷达传感器、激光传感器、视觉识别传感器，用于识别车道线方位以及采集汽车行驶时的环境信息；其中雷达传感器置于汽车的两侧用于采集相邻车道行驶车辆的位置信息和车道边界信息；激光传感器置于汽车的前部用于采集自车和前方车辆的距离信息；视觉传感器置于汽车的顶部，用于采集车道线信息；

车辆状态感知系统，包括方向盘转角传感器、车速传感器、陀螺仪、侧向加速度传感器、横摆角速度传感器，用于采集汽车行驶时的汽车状态信息；其中方向盘转角传感器置于方向盘下方，用于采集方向盘转角信息；车速传感器置于变速器的输出端，用于采集车速信息；其余传感器置于车内用于采集包括侧向加速度、横摆角速度、航向角在内的信息。

3.根据权利要求1所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统，其特征在于：所述的安全预警系统为安全距离系统，包括：

纵向安全距离模型，根据汽车与前车的相对距离以及相对速度计算制动安全距离以及操纵安全距离；

横向安全距离模型，通过汽车的行驶状态和环境信息判断汽车行驶所需的横向安全距离；

所述安全距离系统通过纵向安全距离模型和横向安全距离模型的输出结果对驾驶行为进行预警；

所述安全预警模型判断汽车是否需要横向操纵的标准是：D_h>D，其中，D为自车与前车的相对距离，D_h为操纵安全距离。

4.根据权利要求1所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统，其特征在于：所述最优控制系统包括汽车最优控制模型系统、最优控制问题的离散系统和非线性规划问题的求解系统组成，其中最优控制模型系统由边值约束、过程约束、运动微分方程和性能指标组成；将建立的最优控制问题离散成非线性规划问题，最后，求解非线性规划问题，得到汽车的最优操纵输入解。

5.根据权利要求1所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统，其特征在于：所述的执行机构由纵向控制器和横向控制器组成，其中，纵向控制器由制动踏板执行器和加速踏板执行器组成，横向控制器由方向盘执行器组成。

6.根据权利要求1至5任一所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统的控制方法，其特征在于：包括以下步骤：

步骤1：利用数据采集系统采集汽车正常行驶中的汽车状态信息和环境信息，并将采集到的信息传递到安全预警系统；

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

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

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

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

7.根据权利要求6所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统的控制方法，其特征在于：所述步骤2、3的具体方法为：

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

步骤2-2：纵向安全距离模型分别计算出纵向安全距离D_Z和操纵安全距离D_h；

其中，

a₁＝-0.2g

a₂＝-0.5g

其中V为相对速度、a₁、a₂为制动减速度；t₁是以a₁为制动减速度时，汽车从开始减速到停止所用时间；t₂是以a₂为制动减速度时，汽车从开始减速到停止所用时间；

步骤2-3：判断自车与前车的相对距离与纵向安全距离、横向安全距离的大小关系：

若D>D_Z，则此时汽车行驶安全，维持当前行驶状态；

若D_Z>D>D_h，则汽车进行简单制动操纵，此时控制汽车以较小的制动减速度进行制动；

若D_h>D，需由横向安全距离模型判断换道可行性；

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

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

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

8.根据权利要求6或7所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统的控制方法，其特征在于：步骤3中，最优控制系统工作方法为：

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

步骤3-2：设定状态变量为侧向速度v(t)、横摆角速度ω(t)、纵向速度u(t)、汽车质心横坐标x(t)、汽车质心纵坐标y(t)和航向角θ(t)，即状态变量：

X(t)＝{v(t),ω(t),u(t),x(t),y(t),θ(t)}^T；

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

x(0)＝[0 0 u 0 0 0]

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

x(t_f)＝[0 0 u x_f y_f 0]

其中，t_f是终端时刻；

步骤3-5：设定控制变量、状态变量约束条件，建立如下的约束条件：

X(t)_min≤X(t)≤X(t)_max

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

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

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

其中，J(Z)是性能指标，t₀是初始时刻，t_e是终止时刻；

步骤3-8：建立汽车横向操纵的运动微分方程

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

其中，v为汽车的侧向速度；u为汽车的纵向速度；ω_r为汽车的横摆角速度；m为整车总质量；I_z为整车绕铅垂轴转动惯量；a、b分别为整车质心至前、后轴的距离；k₁、k₂分别为前、后轮综合侧偏刚度；δ为前轮转角；F_yf为前轮侧偏力；F_yr为后轮侧偏力；F_xf为前轮驱动力/制动力，F_xf≥0为驱动力，F_xf＜0为制动力；F_xr为后轮驱动力/制动力；F_f为滚动阻力，F_f＝mgf，f为滚动阻力系数；F_w为空气阻力，F_w＝C_DA(3.6u)²/21.15，C_D为空气阻力系数，A为迎风面积；

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

其中，θ是航向角；

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

步骤3-9：利用Radau伪谱法对所述运动微分方程、性能指标和约束条件进行转换，将连续最优控制问题离散化，得到非线性规划问题；

步骤3-10：求解离散后所得的非线性规划问题。

9.根据权利要求8所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统的控制方法，其特征在于：步骤3-9的具体方法为：

步骤3-9.1：由于Radau伪谱法的配点都分布在区间(-1,1]上，将最优控制问题的时间区间t∈[t₀,t_e]转换成τ∈[-1,1]，对时间变量t作变换：τ＝2t/(t_e-t₀)-(t_e+t₀)/(t_e-t₀)，将所述运动微分方程、性能指标和约束条件转换为如下形式的最优控制问题转换式：

min J＝ψ(x(τ_e),t_e)

C[x(τ),z(τ),τ；t₀,t_e]≤0

K阶LGR点是多项式P_K(τ)-P_K-1(τ)的根，其中P_K(τ)即为K阶Legendre多项式；为使节点能覆盖区间端点，Radau伪谱法的节点为配点与初始时间点τ₀＝-1；当节点个数为N时，则配点个数为K(K＝N-1)，即配点取N-1阶LGR点；

步骤3-9.2：采用N个拉格朗日插值多项式L_i(τ)(i＝0,1,…,N-1)为基函数来近似变量：

其中，Lagrange插值多项式函数

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

步骤3-9.3：采用Lagrange插值多项式L^* _i(τ)，(i＝1,…,N-1)作为基函数来近似控制变量，即：

式中，

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

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

其中，g(τ_i)＝(1+τ_i)[P_K(τ_i)-P_K-1(τ_i)]，式中τ_i为节点，τ_k为配点；将步骤3-9.1中的最优控制问题转换式在配点τ_k处离散，将最优控制问题的动力学微分方程约束转换为代数约束，对于k＝1,…,N有：

10.根据权利要求8所述的基于最优控制与安全距离的自动驾驶最速操纵控制系统的控制方法，其特征在于：求解离散后所得的非线性规划问题，用到的优化方法是SQP算法的一种：Wilson-Han-Powell方法，非线性规划问题转化为求解一系列的二次规划问题。