CN111176284B - 一种用于无人驾驶的车辆路径追踪自适应控制方法和系统 - Google Patents

Abstract

本发明公开了一种用于无人驾驶的车辆路径追踪自适应控制方法和系统，其中控制方法包括：1、建立车辆的Stanley模型：

2、根据车辆当前的运行状态，采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；3、将当前时刻的最优增益参数k^opt代入车辆的Stanley模型中，得到下一时刻车辆的最优期望转向角，控制车辆运行；判断是否要切换参考点，跳转至步骤2继续下一时刻的控制，直到车辆到达参考路径的终点。该方法能够根据车辆运行状态自适应调整Stanley模型参数，得到最优的轮胎转向角，从而控制车辆精确跟踪参考路径。

Description

一种用于无人驾驶的车辆路径追踪自适应控制方法和系统

技术领域

本发明属于无人驾驶路径追踪控制领域，具体涉及一种在智能车辆自动追踪预设路径时，根据车辆实时运行状态自适应调整控制器模型参数，得到最优的轮胎转向角，从而控制车辆运行的方法和系统。

背景技术

路径追踪是车辆无人驾驶的一项关键技术，目前路径追踪主要有以下几种方法：采用PID控制器，跟踪能力较好，对复杂工况的适应性较差；采用纯追踪算法，跟踪性能和实时性较好，但是需要建立精确的数学模型，而且前视距离不易选取，使其在应用方面受到很大的限制；采用模型预测控制的方法，该方法通过求解带约束的目标函数，预测系统未来时刻的状态，在线滚动优化，使跟踪误差最小，但是其计算较为复杂，对硬件要求较高；采用Stanley算法，该算法的核心是一个非线性反馈横向偏差函数，数学模型比较简单，硬件要求低，实时性好。

然而，目前多数Stanley算法中存在一个问题，当车辆行驶速度发生变化时，需要人为改变增益参数k，否则跟踪误差增大。人为调整参数，增加了操作的不便性，而且所调整的参数不一定是最优参数。针对这一问题，有学者采用马尔可夫决策方法(MDP)在线寻找最优增益参数k，该方法能够实现在线优化Stanley算法，但是较为繁琐。开展在不同行驶速度下，Stanley控制算法自适应优化的研究，对提升车辆路径追踪能力具有重要意义。

发明内容

发明目的：本发明旨在提供一种能够根据车辆运行状态自适应调整Stanley模型参数，得到最优的轮胎转向角，从而控制车辆精确跟踪参考路径的方法。

技术方案：本发明一方面公开了一种用于无人驾驶的车辆路径追踪自适应控制方法，包括：

(1)建立车辆的Stanley模型：

其中△u为期望前轮转向角，θ_h为车辆的当前航向角，θ_r为参考路径的角度，d_XTE为当前时刻的横向误差，v为当前时刻的速度，k为当前时刻的增益参数；

(2)根据车辆当前的运行状态，采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；

(3)将当前时刻的最优增益参数k^opt代入车辆的Stanley模型中，得到下一时刻车辆的最优期望转向角，控制车辆运行；判断是否要切换参考点，跳转至步骤2继续下一时刻的控制，直到车辆到达参考路径的终点。

另一方面，本发明公开了实现上述车辆路径追踪自适应控制方法的控制系统，包括：

车辆Stanley模型建立模块，用于建立车辆的Stanley模型；

车辆最优增益参数计算模块，用于根据车辆当前的运行状态，采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；

车辆期望前轮转向角计算模块，用于根据车辆的Stanley模型和最优增益参数k^opt计算车辆的最优期望转向角；

主控模块，用于根据车辆的最优期望转向角，控制车辆运行，并判断是否要切换参考点，控制车辆最优增益参数计算模块和车辆期望前轮转向角计算模块的启停，直到车辆到达参考路径的终点。

有益效果：与现有技术相比，本发明公开的用于无人驾驶的车辆路径追踪自适应控制方法和系统具有如下优点：利用蚁群算法进行参数优化，鲁棒性强，具有更强的全局搜索能力，能够根据实时速度自适应调整Stanley模型的控制器参数，增强算法的自适应能力，使车辆快速地跟踪预设路径。

附图说明

图1为本发明公开的车辆路径追踪自适应控制方法的流程图；

图2为采用改进的蚁群算法计算当前时刻的最优增益参数的流程图；

图3为本发明公开的车辆路径追踪自适应控制系统的组成图。

具体实施方式

为使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明的具体实施案例做说明。

本发明公开了一种用于无人驾驶的车辆路径追踪自适应控制方法，如图1所示，包括：

步骤1、建立车辆的Stanley模型

Stanley模型的核心是一个非线性反馈横向偏差函数，横向偏差呈指数收敛。根据车辆位姿和参考路径的信息，可以直观的得到期望车轮转角与航向角偏差和横向偏差的关系。

(A)当不考虑横向误差时，期望前轮转角

与航向偏差大小一致，如下式所示：

其中θ_h为车辆的当前航向角，θ_r为参考路径的角度。

(B)当不考虑航向误差时，横向偏差越大，期望前轮转角

越大，假设车辆的预期运动轨迹在距离前轮l处与参考路径相交，由几何关系得：

其中d_XTE为当前时刻的横向误差，即车辆到参考路径的最短距离(规定参考路径左侧为负，右侧为正)，v为当前时刻的速度，k为增益参数，与速度有关，且决定模型的收敛速度。

结合以上两种情况，当横向误差和航向均存在时，期望前轮转向角△u由两部分组成，一部分用来消除航向偏差，另一部分用来消除横向偏差，如下式所示：

联立式(1)、(2)、(3)可得Stanley模型的表达式为：

其中△u为期望前轮转向角，θ_h为车辆的当前航向角，θ_r为参考路径的角度，d_XTE为当前时刻的横向误差，v为当前时刻的速度，k为当前时刻的增益参数；

步骤2、根据车辆当前的运行状态，采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；如图2所示，包括如下步骤：

(2.1)根据k的取值范围和精度确定小数位数和整数位数，设整数位数为I，小数位数为J；确定蚂蚁的活动区域为0≤x≤I+J，0≤y≤9约束的矩形区域Area，x,y均为整数；蚂蚁从坐标原点出发爬向x＝I+J的节点为一次迭代，一次迭代包括I+J步，所经过的路径节点依次为Path＝A(1,y₁),A(2,y₂),…,A(I+J,y_I+J)，y_n∈{0,1,…,9}，n＝1,2,…,I+J；

通常情况下，k为小于10正实数，考虑到0.0001的精度已能够满足车辆运行的控制要求，本实施例中设定k的整数位数为1，即I＝1，小数位数4，即J＝4；即0<k≤9.9999。Area为以(0,9)为左上角，(5,0)为右下角的矩形区域。每次迭代，蚂蚁爬行5步。

(2.2)初始化蚁群参数：设置蚁群规模M，最大迭代次数I_max，信息素挥发系数ρ，蚂蚁迭代一次释放的信息素总量Q，遗留信息素的重要程度α，启发信息的重要程度β，初始化Area中各节点的信息素τ_x,y(0)＝C；ρ、Q、α、β、C均为常数；当前最优路径节点的纵坐标均为0，即

当前迭代次数i＝0，当前爬行步数j＝0；

本实施例中设定信息素挥发系数ρ的取值范围为(0,1)，蚂蚁迭代一次释放的信息素总量Q＝10，遗留信息素的重要程度α和启发信息的重要程度β的取值范围均为(0,5)

(2.3)第i次迭代的第j步中，每一只蚂蚁从x＝j的节点爬向x＝j+1的节点；计算x＝j+1的每个节点的信息素能见度

计算第m只蚂蚁从当前节点A(j,y_j)爬向的节点A(j+1,y_n)的转移概率

其中

为第i次迭代中节点(j,y_n)处的信息素，m＝1,…,M；

选择转移概率

最大的节点为第m只蚂蚁第j步的目的节点A(j+1,y_j+1)；

令j加一，重复步骤2.3，直到j＝I+J，完成一次迭代，记录本次迭代每只蚂蚁的路径节点

(2.4)计算当前迭代的最优增益参数

根据第i次迭代中每只蚂蚁的路径节点

计算M个增益参数k的候选值：

其中

为当前迭代中第m只蚂蚁所经过的路径上的第j个节点的纵坐标；

根据

计算期望前轮转向角的第m个候选值

并预测下一时刻车辆的位姿(x',y',θ_h')：

x'＝x+v·T·cosθ_h

y'＝y+v·T·sinθ_h

其中T为采样周期，d_carwidth为车辆轴距；

计算下一时刻的横向误差

为车辆位置(x',y')到当前参考点与下一参考点所构成的直线的最短距离；如果当前参考点为参考路径上的最后一个轨迹点，则

为车辆位置到当前参考点的距离；

选择

的最小值

所对应的

为当前迭代的最优增益参数

对应的蚂蚁路径节点为当前迭代的最优路径节点

如果i＝0或

更新最优路径各节点的纵坐标

为

中各节点的纵坐标，并令

(2.5)更新矩形区域Area每个节点的信息素：

τ_xy(i+1)＝(1-ρ)·τ_xy(i)+△τ_xy

其中，(1-ρ)·τ_xy(i)为挥发之后剩余的信息素，△τ_xy为本次迭代中，所有经过节点(x,y)的蚂蚁留下的信息素，由下式表示：

(2.6)令i加一，判断是否达到迭代终止条件：

当前迭代次数i≥I_max，或相邻两次迭代的最优增益参数满足精度要求：

其中ξ是预设的精度阈值；

如不满足上述条件，令j＝0，跳转至步骤2.3进行下一次迭代；

如满足迭代终止条件，将最优路径所对应的增益参数作为当前时刻的最优增益参数k^opt。

步骤3、将当前时刻的最优增益参数k^opt代入车辆的Stanley模型中，得到下一时刻车辆的最优期望转向角，控制车辆运行；

当车辆当前位置到达或超过参考路径上的当前参考点时，将当前参考点切换为参考路径上的下一个轨迹点；

跳转至步骤2继续下一时刻的控制，直到车辆到达参考路径的终点。

本发明还公开了实现上述车辆路径追踪自适应控制方法的控制系统，如图2所示，包括：

车辆Stanley模型建立模块，用于建立车辆的Stanley模型；

车辆最优增益参数计算模块，用于根据步骤2采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；

车辆期望前轮转向角计算模块，用于根据车辆的Stanley模型和最优增益参数k^opt计算车辆的最优期望转向角；

主控模块，用于根据车辆的最优期望转向角，控制车辆运行，并判断是否要切换参考点，控制车辆最优增益参数计算模块和车辆期望前轮转向角计算模块的启停，直到车辆到达参考路径的终点。

Claims (10)

1.一种用于无人驾驶的车辆路径追踪自适应控制方法，其特征在于，包括步骤：

(1)建立车辆的Stanley模型：

其中Δu为期望前轮转向角，θ_h为车辆的当前航向角，θ_r为参考路径的角度，d_XTE为当前时刻的横向误差，v为当前时刻的速度，k为当前时刻的增益参数；

(2)根据车辆当前的运行状态，采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；

(3)将当前时刻的最优增益参数k^opt代入车辆的Stanley模型中，得到下一时刻车辆的最优期望转向角，控制车辆运行；判断是否要切换参考点，跳转至步骤2继续下一时刻的控制，直到车辆到达参考路径的终点；

所述步骤(2)包括：

(2.1)根据k的取值范围和精度确定小数位数和整数位数，设整数位数为I，小数位数为J；确定蚂蚁的活动区域为0≤x≤I+J，0≤y≤9约束的矩形区域Area，x,y均为整数；蚂蚁从坐标原点出发爬向x＝I+J的节点为一次迭代，一次迭代包括I+J步，所经过的路径节点依次为Path＝A(1,y₁),A(2,y₂),…,A(I+J,y_I+J)，y_n∈{0,1,…,9}，n＝1,2,…,I+J；

(2.2)初始化蚁群参数：设置蚁群规模M，最大迭代次数I_max，信息素挥发系数ρ，蚂蚁迭代一次释放的信息素总量Q，遗留信息素的重要程度α，启发信息的重要程度β，初始化Area中各节点的信息素τ_x,y(0)＝C；ρ、Q、α、β、C均为常数；当前最优路径节点的纵坐标均为0，即

当前迭代次数i＝0，当前爬行步数j＝0；

(2.3)第i次迭代的第j步中，每一只蚂蚁从x＝j的节点爬向x＝j+1的节点；计算x＝j+1的每个节点的信息素能见度

计算第m只蚂蚁从当前节点A(j,y_j)爬向的节点A(j+1,y_n)的转移概率

其中

为第i次迭代中节点(j,y_n)处的信息素，m＝1,…,M；

选择转移概率

最大的节点为第m只蚂蚁第j步的目的节点A(j+1,y_j+1)；

令j加一，重复步骤2.3，直到j＝I+J，完成一次迭代，记录本次迭代每只蚂蚁的路径节点

(2.4)计算当前迭代的最优增益参数

根据第i次迭代中每只蚂蚁的路径节点

计算M个增益参数k的候选值：

其中

为当前迭代中第m只蚂蚁所经过的路径上的第j个节点的纵坐标；

根据

计算期望前轮转向角的第m个候选值

并预测下一时刻车辆的位姿(x',y',θ_h')：

x'＝x+v·T·cosθ_h

y'＝y+v·T·sinθ_h

其中T为采样周期，d_carwidth为车辆轴距；

计算下一时刻的横向误差

为车辆位置(x',y')到当前参考点与下一参考点所构成的直线的最短距离；如果当前参考点为参考路径上的最后一个轨迹点，则

为车辆位置到当前参考点的距离；

选择

的最小值

所对应的

为当前迭代的最优增益参数

对应的蚂蚁路径节点为当前迭代的最优路径节点

如果i＝0或

更新最优路径各节点的纵坐标

为

中各节点的纵坐标，并令

(2.5)更新矩形区域Area每个节点的信息素：

τ_xy(i+1)＝(1-ρ)·τ_xy(i)+Δτ_xy

其中，(1-ρ)·τ_xy(i)为挥发之后剩余的信息素，Δτ_xy为本次迭代中，所有经过节点(x,y)的蚂蚁留下的信息素，由下式表示：

(2.6)令i加一，判断是否达到迭代终止条件，如不满足，令j＝0，跳转至步骤2.3进行下一次迭代；

如满足迭代终止条件，将最优路径所对应的增益参数作为当前时刻的最优增益参数k^opt。

2.根据权利要求1所述车辆路径追踪自适应控制方法，其特征在于，所述迭代终止条件为：

当前迭代次数i≥I_max，或相邻两次迭代的最优增益参数满足精度要求：

其中ξ是预设的精度阈值；则满足迭代终止条件。

3.根据权利要求1所述车辆路径追踪自适应控制方法，其特征在于，步骤(2.1)中k的整数位数为1，I＝1，小数位数4，J＝4。

4.根据权利要求1所述车辆路径追踪自适应控制方法，其特征在于，信息素挥发系数ρ的取值范围为(0,1)，蚂蚁迭代一次释放的信息素总量Q＝10，遗留信息素的重要程度α和启发信息的重要程度β的取值范围均为(0,5)。

5.根据权利要求1所述车辆路径追踪自适应控制方法，其特征在于，所述步骤(3)中，当车辆当前位置到达或超过参考路径上的当前参考点时，将当前参考点切换为参考路径上的下一个轨迹点。

6.一种用于无人驾驶的车辆路径追踪自适应控制系统，其特征在于，包括：

车辆Stanley模型建立模块，用于建立车辆的Stanley模型；所述模型为：

其中Δu为期望前轮转向角，θ_h为车辆的当前航向角，θ_r为参考路径的角度，d_XTE为当前时刻的横向误差，v为当前时刻的速度，k为当前时刻的增益参数；

车辆最优增益参数计算模块，用于根据车辆当前的运行状态，采用改进的蚁群算法计算当前时刻的最优增益参数k^opt；

车辆期望前轮转向角计算模块，用于根据车辆的Stanley模型和最优增益参数k^opt计算车辆的最优期望转向角；

主控模块，用于根据车辆的最优期望转向角，控制车辆运行，并判断是否要切换参考点，控制车辆最优增益参数计算模块和车辆期望前轮转向角计算模块的启停，直到车辆到达参考路径的终点；

所述车辆最优增益参数计算模块计算当前时刻的最优增益参数k^opt的步骤包括：

(2.1)根据k的取值范围和精度确定小数位数和整数位数，设整数位数为I，小数位数为J；确定蚂蚁的活动区域为0≤x≤I+J，0≤y≤9约束的矩形区域Area，x,y均为整数；蚂蚁从坐标原点出发爬向x＝I+J的节点为一次迭代，一次迭代包括I+J步，所经过的路径节点依次为Path＝A(1,y₁),A(2,y₂),…,A(I+J,y_I+J)，y_n∈{0,1,…,9}，n＝1,2,…,I+J；

(2.2)初始化蚁群参数：设置蚁群规模M，最大迭代次数I_max，信息素挥发系数ρ，蚂蚁迭代一次释放的信息素总量Q，遗留信息素的重要程度α，启发信息的重要程度β，初始化Area中各节点的信息素τ_x,y(0)＝C；ρ、Q、α、β、C均为常数；当前最优路径节点的纵坐标均为0，即

当前迭代次数i＝0，当前爬行步数j＝0；

(2.3)第i次迭代的第j步中，每一只蚂蚁从x＝j的节点爬向x＝j+1的节点；计算x＝j+1的每个节点的信息素能见度

计算第m只蚂蚁从当前节点A(j,y_j)爬向的节点A(j+1,y_n)的转移概率

其中

为第i次迭代中节点(j,y_n)处的信息素，m＝1,…,M；

选择转移概率

最大的节点为第m只蚂蚁第j步的目的节点A(j+1,y_j+1)；

令j加一，重复步骤2.3，直到j＝I+J，完成一次迭代，记录本次迭代每只蚂蚁的路径节点

(2.4)计算当前迭代的最优增益参数

根据第i次迭代中每只蚂蚁的路径节点

计算M个增益参数k的候选值：

其中

为当前迭代中第m只蚂蚁所经过的路径上的第j个节点的纵坐标；

根据

计算期望前轮转向角的第m个候选值

并预测下一时刻车辆的位姿(x',y',θ_h')：

x'＝x+v·T·cosθ_h

y'＝y+v·T·sinθ_h

其中T为采样周期，d_carwidth为车辆轴距；

计算下一时刻的横向误差

为车辆位置(x',y')到当前参考点与下一参考点所构成的直线的最短距离；如果当前参考点为参考路径上的最后一个轨迹点，则

为车辆位置到当前参考点的距离；

选择

的最小值

所对应的

为当前迭代的最优增益参数

对应的蚂蚁路径节点为当前迭代的最优路径节点

如果i＝0或

更新最优路径各节点的纵坐标

为

中各节点的纵坐标，并令

(2.5)更新矩形区域Area每个节点的信息素：

τ_xy(i+1)＝(1-ρ)·τ_xy(i)+Δτ_xy

其中，(1-ρ)·τ_xy(i)为挥发之后剩余的信息素，Δτ_xy为本次迭代中，所有经过节点(x,y)的蚂蚁留下的信息素，由下式表示：

(2.6)令i加一，判断是否达到迭代终止条件，如不满足，令j＝0，跳转至步骤2.3进行下一次迭代；

如满足迭代终止条件，将最优路径所对应的增益参数作为当前时刻的最优增益参数k^opt。

7.根据权利要求6所述车辆路径追踪自适应控制系统，其特征在于，所述(2.6)中迭代终止条件为：

当前迭代次数i≥I_max，或相邻两次迭代的最优增益参数满足精度要求：

其中ξ是预设的精度阈值；则满足迭代终止条件。

8.根据权利要求6所述车辆路径追踪自适应控制系统，其特征在于，所述(2.1)中k的整数位数为1，I＝1，小数位数4，J＝4。

9.根据权利要求6所述车辆路径追踪自适应控制系统，其特征在于，所述信息素挥发系数ρ的取值范围为(0,1)，蚂蚁迭代一次释放的信息素总量Q＝10，遗留信息素的重要程度α和启发信息的重要程度β的取值范围均为(0,5)。

10.根据权利要求6所述车辆路径追踪自适应控制系统，其特征在于，所述主控模块判断是否要切换参考点具体为：当车辆当前位置到达或超过参考路径上的当前参考点时，将当前参考点切换为参考路径上的下一个轨迹点。

Images