CN111381597A - 一种基于高斯伪谱法的车辆避障轨迹规划方法 - Google Patents

Abstract

本发明属于车辆避障轨迹规划技术领域，特别涉及一种基于高斯伪谱法的车辆避障轨迹规划方法；首先，建立本车的运动学和动力学模型，并将其避障路径规划问题描述为一个最优控制问题，该问题包含了车辆运动学和动力学约束，初末状态约束和路径约束，以避障时间最优为性能指标，然后，基于高斯伪谱方法将该最优控制问题进行离散化处理，使其转化为满足约束条件的非线性规划问题，通过求解该非线性规划问题得到最优的避障轨迹。

Description

一种基于高斯伪谱法的车辆避障轨迹规划方法

技术领域

本发明属于车辆避障轨迹规划技术领域，特别涉及一种基于高斯伪谱法的车辆避障轨迹规划方法。

背景技术

目前，随着汽车保有量的不断增加，随之而来的是道路拥堵，交通事故频发等一系列亟待解决的问题，其中，车辆碰撞导致相当比例的重大交通事故，因此，避障轨迹规划作为自动驾驶车辆的关键部分已经受到学术界的重点关注和深入研究。

目前人工势场法和虚拟力场法是两种进行避障轨迹规划的传统方法，其中人工势场法规划出的轨迹平滑且安全，然而，这种算法在求解时容易陷入局部最小值，然而虚拟力场法是网格法和人工势场法相结合的一种避障轨迹规划算法，它在不确定的动态环境中具有很大的优势，但却很难应用在复杂的交通场景中。

此外，在避障轨迹规划中智能优化算法也得到了发展，但每种算法也都各有利弊，如快速扩展随机数算法虽然有较快的搜索速度，但算法的随机性使得规划出的路径曲率变化过大，甚至出现小范围的直角变化，导致路径不平滑；而遗传算法需要基于大量的驾驶员经验，所以存在无法学习和灵活性差的缺点。

发明内容

为了克服上述问题，本发明提供一种基于高斯伪谱法的车辆避障轨迹规划方法，将换道避障问题描述为一个连续的最优控制问题，通过以高斯伪谱法为基础的数值近似技术将该问题转化为一个非线性规划问题，对非线性规划问题进行数值求解，求得满足各种约束条件的控制变量和状态变量轨迹，即最优的避障路径。

一种基于高斯伪谱法的车辆避障轨迹规划方法，采用数值近似技术方法将连续的最优控制问题转化为一个离散的非线性规划问题，通过对非线性规划问题的数值求解，求得满足各种约束条件下，使得性能指标最优的控制变量轨迹和状态变量轨迹，具体包括以下步骤：

步骤一、三自由度车辆控制建模：

为了描述车辆在避障时的运动学和动力学特性，根据牛顿第二定律描述车辆的运动学和动力学关系来建立三自由度车辆模型；

步骤二、优化问题的建立：

对于车辆行驶前方的障碍物，采取换道避障的方式，建立以最优避障时间为目标，满足车辆运动学和动力学约束、初末状态约束以及路径约束的最优控制问题；

步骤三、基于高斯伪谱法的最优控制问题离散化：

采用基于高斯伪谱法的求解软件GPOPS，将步骤二中连续的最优控制问题转化为离散的非线性规划问题，并数值求解获得满足约束的状态变量和控制变量轨迹，即最优的避障路径。

所述步骤一中建立的三自由度车辆模型为:

车辆运动学方程为：

其中，x,y分别表示车辆在大地坐标系下的纵向位移和侧向位移，v表示车辆的质心速度，θ为航向角，β为质心侧偏角，δ_f表示车辆的前轮转角，a,b分别表示质心到车轮前后轴的距离；

车辆动力学方程为：

其中，m表示车辆的总质量，C_f,C_r分别表示前后轮的侧偏刚度，v_x,v_y分别表示车辆的纵向速度和侧向速度，ω表示车辆的横摆角速度，I_z表示横摆转动惯量，建立车辆的运动学和动力学方程作为最优控制问题中的车辆动力学约束，使之满足车辆的运动特性。

所述步骤二优化问题的建立具体包括：

基于车载传感器系统，当车载传感器系统检测到前方车道上有障碍物时，其采取避开障碍物的方式为换道超车，在这一操作过程中还要考虑周围交通环境中其它车辆的影响；

将这一控制问题描述为：选取状态变量X＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T，选取控制变量

其中

为车辆纵向速度v_x的导数，表示纵向加速度，

为前轮转角δ_f的导数，表示角速度，给定车辆的初始状态x(t₀)，在满足车辆的动力学约束和路径约束的同时，使之运动到给定的目标状态x(t_f)，再由初始状态运动到目标状态的过程中，规划出一条避障时间最短的路径即J＝t_f，其中，J为代价函数，表示执行换道避障这一操作过程中的目标就是使避障时间t_f-t₀最小，采用基于高斯伪谱法的求解软件GPOPS将该控制问题离散化为非线性规划问题，求解得到满足约束条件的状态变量和控制变量轨迹，即规划出的最优避障路径；

其中：t表示时间，t₀表示初始时刻，x(t₀)表示车辆状态的初值，即x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T∈X中各个变量在初始时刻t₀时的值，同理，t_f表示终止时刻，x(t_f)表示车辆的终止状态，即x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T∈X中各个变量在终止时刻t_f时的值，T表示的是一种数学运算，即对矩阵x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T求转置。

本发明的有益效果：

1.本发明采用的轨迹规划方案以高斯伪谱的离散化方法为基础，采用直接的数值方法求解最优控制问题，解决了带有复杂约束的轨迹规划问题。

2.本发明建立了纵向速度可变的车辆三自由度动力学模型，体现了速度变化对智能车轨迹规划的影响。

3.本发明以高斯伪谱法为基础，将连续的最优控制问题转化为一个非线性规划问题进行求解，弥补了传统方法计算复杂的缺点，具有较快的求解速度。

附图说明

图1为避障轨迹规划示意图。

图2为车辆模型示意图。

图3为规划的纵向速度曲线示意图。

图4为规划的位移曲线示意图。

图5为规划的航向角曲线示意图。

图6为规划的前轮转角曲线示意图。

具体实施方式

近年来，数值优化计算方法被广泛应用，其不仅可以考虑多目标的轨迹优化，而且还可以加入车辆状态的动力学约束等，保证优化轨迹的可行性与最优性。高斯伪谱法作为数值优化方法的一个分支，近年来发展极为迅速，并广泛应用于复杂的最优控制问题中。

一种基于高斯伪谱法的车辆避障轨迹规划方法，采用数值近似技术方法将连续的最优控制问题转化为一个离散的非线性规划问题，通过对非线性规划问题的数值求解，求得满足各种约束条件下，使得性能指标最优的控制变量轨迹和状态变量轨迹，具体包括以下步骤：

步骤一、三自由度车辆控制建模：为了描述车辆在避障时的运动学和动力学特性，根据牛顿第二定律描述车辆的运动学和动力学关系来建立三自由度车辆模型；

具体包括运动学模型和动力学模型。

(1)运动学建模。在进行运动学建模时，假设车辆是一个刚性体，如图2所示，图中XOY为惯性坐标系，其运动学模型描述为

其中，x,y分别表示车辆在大地坐标系下的纵向和侧向位置，v表示车辆的质心速度，θ为航向角，β为质心侧偏角，δ_f表示车俩的前轮转角，a,b分别表示质心与车轮前后轴的距离。

(2)动力学建模

一般地，二自由度车辆动力学系统方程假设车辆纵向速度不变，为体现速度对智能车路径规划的影响，本发明建立三自由度车辆模型，忽略空气阻力及轮胎纵向力，包含三个状态变量，纵向速度，侧向速度和横摆角速度，如式(2)所示：

其中，v_x,v_y分别表示车辆的纵向速度和侧向速度，ω表示车辆的横摆角速度，F_sf,F_sr分别表示车辆前后轮的侧向力,I_z表示横摆转动惯量。

假定车辆正常稳定行驶未进入极限工况，车辆侧向轮胎力未达到饱和状态，此时前、后轮侧向力与轮胎侧偏角基本呈线性关系，其表达式为(3)

F_sf＝2C_fα_f

F_sr＝2C_rα_r (3)

其中，m表示车辆的总质量，C_f,C_r分别表示车辆前后轮的侧偏刚度，α_f,α_r分别表示车辆前后轮的轮胎侧偏角；根据坐标系规定，车辆前、后轮的轮胎侧偏角可分别被近似描述成：

将公式(3)(4)带入(2)式，得到最终的三自由度车辆动力学模型(5)：

综上，结合式(1)和式(5)车辆的运动学和动力学方程以[x,y,θ,v_x,v_y,ω,β,δ_f]^T为状态变量，由于车辆的速度和转向角不能同时变化,将v_x和δ_f作为控制变量是不连续的,因此选取控制变量

该系统具有复杂的非线性特性，建立的车辆运动学和动力学方程作为最优控制问题中的车辆动力学约束，使之满足车辆的运动特性。

其中

为车辆纵向速度v_x的导数，表示纵向加速度，

为前轮转角δ_f的导数，表示角速度。

步骤二、优化问题的建立：

外界交通环境复杂多变，车辆可能会面对各种静态和动态约束，当车载传感器系统检测到前方车道上有障碍物时，采取避开障碍物的方式通常为减速制动和换道超车，出于交通效率等多方面的考虑，通常优先采用后者来实现避障。

由于研究了生活中最常见的换道避障工况如图1，具体描述如下：车辆A以10m/s的速度在双车道的右边车道上行驶(每条车道宽为3.5米)，其正前方40米处有一低速行驶的车辆C(5m/s)，此时车辆A为了超过前方车辆C，决定换道至左车道，同时左车道后方60米处有一辆车辆以15m/s的车辆B同向而行。上述避障控制问题中，避障操作的开始时刻是已知给定的，且开始时智能车是以稳定状态行驶的，因此初始时刻的车辆状态是已知的，换道至左侧车道后，摆正姿态继续行驶，因此避障完成后期望的车辆状态是已知的，而智能车达到目标状态的时间是未知的，通常希望达到期望目标状态的时间越短越好。综上,智能车的换道避障问题可归结为:初始时刻及状态已知,终端目标状态已知,终端时间待优化的一类约束优化问题。

这一控制问题具体描述如下：

(1)对于智能车避障的控制任务，通常追求避障的快速性，因此，建立以避障时间最短为目标的代价函数：

J＝t_f

其中，J为代价函数，t_f为避障的终端时刻，此函数的含义是使避障过程的时间越短越好。

(2)在智能车避障的过程中，需要满足车辆的动力学约束，即步骤一中建立的车辆运动学和动力学方程，公式(1)和公式(5)。

(3)并且，其初始和终端时刻的车辆状态是受等式约束的，即初始和终端时刻的车辆状态是已知给定的

x(t₀)＝x₀

x(t_f)＝x_f

其中t₀表示初始时刻，x(t₀)表示车辆的初值状态，它等于给定的车辆初始状态值x₀，同理，t_f表示终止时刻，x(t_f)表示车辆的终止状态，它等于给定的车辆终端的目标状态值x_f。此方程作为避障过程的状态约束。

(4)此外，智能车的避障任务是以安全为前提的，因此，本文引入安全距离的概念，即车辆与障碍物之间的纵向位置差，保证最小安全距离为10米做为避障过程中的路径约束

其中，x_A，y_A分别表示车辆A在大地坐标系下的纵向和侧向位置，x_B，y_B分别表示障碍车B在大地坐标系下的纵向和侧向位置，x_C，y_C分别表示障碍车C在大地坐标系下的纵向和侧向位置，d₁表示车辆A与障碍车B的距离，d₂表示车辆A与障碍车C的距离；此方程表示的含义是车辆A与障碍车B和障碍车C的之间的距离都不得小于10米，此方程作为避障过程的路径约束。

综上,智能车的换道避障需求可总结为：选取状态变量X＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T，选取控制变量

其中

为车辆纵向速度v_x的导数，表示纵向加速度，

为前轮转角δ_f的导数，表示角速度，给定车辆的初始状态x(t₀)，在满足车辆的动力学约束、状态约束和路径约束的同时，使之运动到给定的目标状态x(t_f)。再由初始状态运动到目标状态的过程中，车辆有无数条可行的路径，但我们需要规划出一条避障时间最短的路径即J＝t_f，其中，J为代价函数，表示执行换道避障这一操作过程中的目标就是使避障时间t_f-t₀最小。

其中：t表示时间，t₀表示初始时刻，x(t₀)表示车辆状态的初值，即x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T∈X中各个变量在初始时刻t₀时的值，同理，t_f表示终止时刻，x(t_f)表示车辆的终止状态，即x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T∈X中各个变量在终止时刻t_f时的值，T表示的是一种数学运算，即对矩阵x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T求转置。

T表示的是一种数学运算，即对矩阵x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T求转置，结果如下

二者是等价的，是为了书和排版的方便，所以通常写成上面横向的这种方式。

步骤三、采用基于高斯伪谱法的求解软件GPOPS将该控制问题离散化为非线性规划问题，求解得到满足约束条件的状态变量和控制变量轨迹，即规划出的最优避障路径，即将这些信息写成程序代码的形式作为输入给这个软件。

J＝t_f是以程序代码的形式写进目标函数，它也是程序的一部分，使用Matlab中嵌入的GPOPS软件运行求解。因为系统是离散的，理论上我们求解得到的是无数个点组成的最优的避障路径，它就是我们需要的最优的避障路径。

具体包括以下步骤：

(1)首先进行时域变换

高斯伪谱法的配点分布在τ∈[-1,1]上，对于时间区间为t∈[t₀,t_f]的最优控制问题首先进行时域变换，

t表示的是时间，t₀表示初始时间，t_f表示末端或终止时间，τ表示的是从-1到1的一个虚拟的时间域，高斯伪谱法就是基于这个时间域上的，所以要将我们实际的时间域t转换到τ∈[-1,1]这个域上。

(2)状态变量及控制变量的近似：

高斯伪谱法选取K个LG节点，构成K+1个插值多项式

并以此近似状态变量和控制变量，

在τ∈[-1,1]这个时间域上取K个节点,τ₁,τ₂…，i和j均表示0,1,2,3.......K，这些点构成了插值多项式L_i(τ)来把连续的系统给离散化，离散化的意思就是本来连续的一条曲线，现在在线上取无数个点来近似描述这条曲线，x(τ)表示连续系统的状态变量，X(τ)为离散系统的状态变量，二者是约等的，通过L_i(τ)来进行转换的；同理，u(τ)表示连续系统的控制变量，U(τ)表示离散系统的控制变量。

(3)微分方程约束转换为代数约束：

对

求导可得状态变量导数，

中，其中，微分矩阵D_ki可离线确定；i从0取到K，符号∑表示求和，则上式展开写为

j与i的含义相同；

(4)离散条件下的终端状态约束：

高斯伪谱法中的节点包括K个配点(τ₁…τ_k)，其终端状态应满足方程

将终端状态进行高斯积分近似得

X(τ_f)为在终点τ_f处的状态，

x(τ₀)为在起点τ₀处的状态，ω_k为积分权重。

(5)离散条件性能指标

将性能指标函数中的积分项用高斯积分来近似，

基于上述数值近似方法，高斯伪谱法将连续最优控制问题离散，转换为离散的非线性规划问题，J为代价函数的一般性表达形式；

其中X(τ_k)表示在τ_k点处系统的状态变量，U(τ_k)表示在τ_k点处系统的控制变量；

步骤二中的J＝t_f中的J是针对本发明避障控制问题而选择的具体的代价函数，而此处的J表示的是代价函数的一般性表达方式。

_g是与括号中的参数有关的一个待优化的非线性函数，与整个控制过程有关。函数φ只与控制过程的初态和末态有关。

整个步骤三都是对高斯伪谱法基本原理的叙述，也就是GPOPS软件包将步骤二建立的连续的最优控制问题转化为非线性规划问题的步骤，这是在该软件内部执行的不再需要人为地计算其每个步骤，应用该软件进行求解时，将步骤二中连续的最优控制问题以程序代码的形式表示出来写在Matlab中，使用Matlab中嵌入的GPOPS软件运行求解。

为验证本发明提出的基于高斯伪谱法的车辆避障轨迹规划方法的有效性，下面给出本发明的车辆避障轨迹规划的仿真验证。

本试验环境为Matlab2014b中的高斯伪谱法求解软件包GPOPS。通过优化计算，共选取42个LG节点，换道避障的最优时间t_f求解为5.63秒，纵向速度、位移变化、航向角和前轮转角曲线分别如图3、图4、图5和图6所示。

从仿真结果可以看出，智能车在0-4s内以10m/s的速度保持直行，满足给定的初始状态，大约在第4s与前方障碍车达到设定的给定的最小安全距离，此时，智能车开始执行换道动作，侧向位移从-1.75平滑上升，航向角与前轮转角相应地开始增加，直至侧向位移稳定在1.75(换道至左侧车道)，航向角与前轮转角回到0的状态，即智能车在到达左侧车道后摆正姿态继续行驶，满足给定的换道避障的要求，求解计算时间为2.26秒，避障时间t_f为5.63秒，该方法具有较快的求解速度。

Claims (3)

1.一种基于高斯伪谱法的车辆避障轨迹规划方法，其特征在于，采用数值近似技术方法将连续的最优控制问题转化为一个离散的非线性规划问题，通过对非线性规划问题的数值求解，求得满足各种约束条件下，使得性能指标最优的控制变量轨迹和状态变量轨迹，具体包括以下步骤：

步骤一、三自由度车辆控制建模：

为了描述车辆在避障时的运动学和动力学特性，根据牛顿第二定律描述车辆的运动学和动力学关系来建立三自由度车辆模型；

步骤二、优化问题的建立：

对于车辆行驶前方的障碍物，采取换道避障的方式，建立以最优避障时间为目标，满足车辆运动学和动力学约束、初末状态约束以及路径约束的最优控制问题；

步骤三、基于高斯伪谱法的最优控制问题离散化：

采用基于高斯伪谱法的求解软件GPOPS，将步骤二中连续的最优控制问题转化为离散的非线性规划问题，并数值求解获得满足约束的状态变量和控制变量轨迹，即最优的避障路径。

2.根据权利要求1所述的一种基于高斯伪谱法的车辆避障轨迹规划方法，其特征在于，所述步骤一中建立的三自由度车辆模型为:

车辆运动学方程为：

其中，x,y分别表示车辆在大地坐标系下的纵向位移和侧向位移，v表示车辆的质心速度，θ为航向角，β为质心侧偏角，δ_f表示车辆的前轮转角，a,b分别表示质心到车轮前后轴的距离；

车辆动力学方程为：

其中，m表示车辆的总质量，C_f,C_r分别表示前后轮的侧偏刚度，v_x,v_y分别表示车辆的纵向速度和侧向速度，ω表示车辆的横摆角速度，I_z表示横摆转动惯量，建立车辆的运动学和动力学方程作为最优控制问题中的车辆动力学约束，使之满足车辆的运动特性。

3.根据权利要求2所述的一种基于高斯伪谱法的车辆避障轨迹规划方法，其特征在于，所述步骤二优化问题的建立具体包括：

基于车载传感器系统，当车载传感器系统检测到前方车道上有障碍物时，其采取避开障碍物的方式为换道超车，在这一操作过程中还要考虑周围交通环境中其它车辆的影响；

将这一控制问题描述为：选取状态变量X＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T，选取控制变量

其中

为车辆纵向速度v_x的导数，表示纵向加速度，

为前轮转角δ_f的导数，表示角速度，给定车辆的初始状态x(t₀)，在满足车辆的动力学约束和路径约束的同时，使之运动到给定的目标状态x(t_f)，再由初始状态运动到目标状态的过程中，规划出一条避障时间最短的路径即J＝t_f，其中，J为代价函数，表示执行换道避障这一操作过程中的目标就是使避障时间t_f-t₀最小，采用基于高斯伪谱法的求解软件GPOPS将该控制问题离散化为非线性规划问题，求解得到满足约束条件的状态变量和控制变量轨迹，即规划出的最优避障路径；

其中：t表示时间，t₀表示初始时刻，x(t₀)表示车辆状态的初值，即x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T∈X中各个变量在初始时刻t₀时的值，同理，t_f表示终止时刻，x(t_f)表示车辆的终止状态，即x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T∈X中各个变量在终止时刻t_f时的值，T表示的是一种数学运算，即对矩阵x＝[x,y,θ,v_x,v_y,ω,β,δ_f]^T求转置。

Images