一种基于蚁群算法的车辆路径追踪控制方法与流程

本发明属于车辆自动驾驶控制技术领域，具体涉及一种在无人驾驶车辆路径追踪时计算车辆轮胎转向角和车辆速度的方法。

背景技术：

路径追踪是无人驾驶车辆中的一项关键技术，目前路径追踪主要有以下几种方法：采用pid控制器，有较好的跟踪能力，但是缺乏对复杂工况的适应；采用纯追踪算法，跟踪性能和实时性较好，但是需要建立精确的运动学模型，而且前视距离不易选取，使其在应用方面受到很大的限制；基于模型预测控制(mpc)的方法，该方法不要求建立精确的数学模型，而是通过求解带约束的目标函数，预测系统未来时刻的状态，在线滚动优化，并根据系统实际输出进行反馈校正，修正预测模型，最终使得系统在未来时刻的跟踪误差最小，有独特的预测能力，控制效果好，在许多生产过程中得到了广泛的应用。

然而模型预测控制需要在每一采样时刻，根据当前的测量信息，在线求解一个有限时域开环优化问题，这个过程会消耗很多时间。为了提高计算速度，引入了控制时域的概念。当预测时域大于控制时域时，将不再进行优化求解，而是把最后一个控制时刻求解出的控制量，作为今后时刻的控制输出。但是这样的处理方式在一定程度上降低了优化的自由度。

车辆路径追踪系统需要快速的动态响应，其轨迹跟踪性能主要取决于转向和速度的实时控制，而基于传统的mpc路径追踪方法在实际工程中动态性差，进而影响跟踪精度。

技术实现要素：

发明目的：针对现有技术中存在的问题，本发明提供了一种基于蚁群算法的车辆路径追踪控制方法，该方法计算速度快，能够提高车辆路径追踪的实时性。

技术方案：本发明采用如下技术方案：

一种基于蚁群算法的车辆路径追踪控制方法，包括步骤：

步骤1、建立车辆的运动学模型，包括如下步骤：

(1.1)车辆的坐标与航向角和轮胎转向角之间具有如下的关系：

其中x、y分别为车辆在x轴和y轴的坐标，为车辆航向角，l为车辆的前后轴的轴距，v为车辆的速度，δ为轮胎转向角；分别为车辆在x轴和y轴的速度，为车辆的转向角速度；

(1.2)以为状态量，u＝[δv]^t为控制量，构建车辆的一般运动学模型：

其中，

参考轨迹上当前参考点r处运动学模型的一般形式为：

(1.3)建立离散化跟踪误差模型，如下：

其中，为k时刻的跟踪误差，ak,t和bk,t分别为根据当前时刻t预测k时刻状态的状态系数矩阵和控制系数矩阵；

t为采样周期，k为离散变量，t为当前采样时刻；

步骤2：设计车辆模型预测控制器，包括如下步骤：

(2.1)计算系统的预测输出表达式；

系统的预测输出表达式为：

y(t)＝ψtx(t|t)+θtδu(t)(5)

其中，y(t)为系统预测输出矩阵，ψt为系统状态量预测参数，θt为控制增量序列预测参数，δu(t)为控制增量序列，x(t|t)为当前时刻t的状态量；

设当前时刻t的状态系数矩阵当前时刻t的控制系数矩阵其中i2表示2行2列的单位矩阵，i3表示3行3列的单位矩阵，03×2表示3行2列的全0矩阵；

则nc为控制时域，np为预测时域；

(2.2)根据式(5)的预测模型，目标函数的表达式如下所示：

其中，r、q为权重矩阵，ρ为权重系数，ε为松弛因子，nc为控制时域，np为预测时域，η(t+i|t)为当前时刻t预测t+i时刻的系统输出量，控制增量序列δu(t)为待求解的未知量；

约束条件为：

δumin≤δu(t)≤δumax(7)

其中，δumin、δumax分别为控制时域内控制量变化量最小值、最大值的集合；

步骤3、获取当前时刻车辆的状态，利用蚁群算法求解使目标函数取最小值的δu(t)^*:

δu(t)^*＝[δu(t|t)^*δu(t+1|t)^*…δu(t+nc-1|t)^*]^t，(8)

则当前时刻的最优控制增量为δu(t|t)^*；

步骤4、根据当前时刻的最优控制增量δu(t|t)^*控制轮胎转向角和车辆行驶速度，控制车辆运行，判断是否需要切换参考点r后进入步骤2继续下一个时刻的控制。

所述步骤(3)利用蚁群算法求解使目标函数取最小值的δu(t)^*，包括如下步骤：

(3.1)蚁群算法初始化：

蚁群由m只蚂蚁组成，定义蚂蚁i在第l次迭代时的位置posi(l)为适应度ji(l)为根据当前时刻车辆的坐标、航向角和以posi(l)作为控制增量序列δu(t)计算得到的目标函数值，适应度ji(l)为：ji(l)＝j(posi(l))；

修正后的适应度为：

其中kavr(l)是ji(l)的平均值，kavr0为[e³,+∞)范围内的随机数；信息素ti(l)为：

定义最大迭代次数为lmax；i＝1,2,…,m；

初始化蚁群中每只蚂蚁的初始位置posi(0)，posi(0)满足条件：δumin≤posi(0)≤δumax；计算每只蚂蚁初始适应度ji(0)、初始修正适应度和初始信息素ti(0)；令迭代次数l＝1；

(3.2)计算本次迭代中蚂蚁i选择下一个可行位置的概率pi：

其中，ti(l-1)表示在上一次迭代中，蚂蚁i的信息素大小；在第l次迭代中，蚂蚁i按照式(12)所示的规则进行移动：

其中，rands为范围[-1,1]内的随机数，p0为预设的(0,0.5)范围内的状态转移概率常数，up和low分别为蚂蚁可移动范围的上下限，up＝δumax，low＝δumin，λ为随迭代次数增加而减小的变量；计算每只蚂蚁位置移动后的适应度ji(l)、修正适应度和信息素ti(l)；

(3.3)再次更新每只蚂蚁的信息素浓度：

ti^new(l)＝(1-μ)ti^new(l-1)+l×ti(l)(14)

其中，μ为信息挥发系数，μ∈(0,1)，l为信息素收敛常数；ti^new(0)＝ti(0)；

统计本次迭代中所有蚂蚁更新后信息素最大的值，即和取得最大值时的蚂蚁的位置posbest(l)；

(3.4)判断是否终止迭代：

如果满足蚁群算法结束条件，则本次迭代蚂蚁的位置posbest(l)为最优控制增量序列δu(t)^*＝[δu(t|t)^*δu(t+1|t)^*…δu(t+nc-1|t)^*]^t，取δu(t)^*第一个元素δu(t|t)^*为当前时刻t的期望控制增量；转到步骤4；

如果不满足蚁群算法结束条件，令迭代次数l＝l+1，跳转到步骤(3.2)继续计算当前时刻t的期望控制增量。

本发明中，每次迭代中λ的取值为：

蚁群算法结束条件为：

当前迭代次数l>lmax，或最优解满足精度要求：

||posbest(l)-posbest(l-1)||²<ξ，ξ是预设的精度阈值。

所述步骤(4)具体包括如下步骤：

(4.1)根据当前时刻的最优控制增量δu(t|t)^*和前一时刻的控制量计算当前时刻的控制量：

u(t)＝u(t-1)+δu(t|t)^*

由此得到当前时刻轮胎转向角δ(t)和车辆的速度v(t)，来控制车辆运行；

(4.2)判断是否已经到达或超过参考轨迹上当前参考点r，如果到达或超过，则将参考轨迹上下一个点作为参考点r；跳转至步骤2继续下一个时刻的控制。

有益效果：与现有技术相比，本发明公开的基于蚁群算法的车辆路径追踪控制方法具有如下优点：利用蚁群算法作为滚动优化方法，具有更强的全局搜索能力，更快的计算能力，能够提高车辆轮胎转向角和车速的运算速度，更好的满足车辆路径追踪的实时性要求。

附图说明

图1为本发明公开的基于蚁群算法的车辆路径追踪控制方法的流程图；

图2为利用蚁群算法计算当前时刻的最优控制增量的流程图。

具体实施方式

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

如图1所示，本发明公开了一种基于蚁群算法的车辆路径追踪控制方法，包括步骤：

步骤1、建立车辆的运动学模型，包括如下步骤：

(1.1)车辆的坐标与航向角和轮胎转向角之间具有如下的关系：

其中x、y分别为车辆在x轴和y轴的坐标，为车辆航向角，l为车辆的前后轴的轴距，v为车辆的速度，δ为轮胎转向角；分别为车辆在x轴和y轴的速度，为车辆的转向角速度；

(1.2)以为状态量，u＝[δv]^t为控制量，构建车辆的一般运动学模型：

其中，

参考轨迹上当前参考点r处运动学模型的一般形式为：

(1.3)建立离散化跟踪误差模型，如下：

其中，为k时刻的跟踪误差，ak,t和bk,t分别为根据当前时刻t预测k时刻状态的状态系数矩阵和控制系数矩阵；

t为采样周期，k为离散变量，t为当前采样时刻；

步骤2：设计车辆模型预测控制器，包括如下步骤：

(2.1)计算系统的预测输出表达式；

系统的预测输出表达式为：

y(t)＝ψtx(t|t)+θtδu(t)(5)

其中，y(t)为系统预测输出矩阵，ψt为系统状态量预测参数，θt为控制增量序列预测参数，δu(t)为控制增量序列，x(t|t)为当前时刻t的状态量；

设当前时刻t的状态系数矩阵当前时刻t的控制系数矩阵其中i2表示2行2列的单位矩阵，i3表示3行3列的单位矩阵，03×2表示3行2列的全0矩阵；

则nc为控制时域，np为预测时域；

(2.2)根据式(5)的预测模型，目标函数的表达式如下所示：

其中，r、q为权重矩阵，ρ为权重系数，ε为松弛因子，nc为控制时域，np为预测时域，η(t+i|t)为当前时刻t预测t+i时刻的系统输出量，控制增量序列δu(t)为待求解的未知量；

约束条件为：

δumin≤δu(t)≤δumax(7)

其中，δumin、δumax分别为控制时域内控制量变化量最小值、最大值的集合；

步骤3、获取当前时刻车辆的状态，利用蚁群算法求解使目标函数取最小值的δu(t)^*:

δu(t)^*＝[δu(t|t)^*δu(t+1|t)^*…δu(t+nc-1|t)^*]^t，(8)

则当前时刻的最优控制增量为δu(t|t)^*，具体步骤为：

(3.1)蚁群算法初始化：

蚁群由m只蚂蚁组成，蚂蚁的位置作为待优化的控制增量序列δu(t)；

定义蚂蚁i在第l次迭代时的位置posi(l)为适应度ji(l)为根据当前时刻车辆的坐标、航向角和以posi(l)作为控制增量序列δu(t)计算得到的目标函数值；

适应度ji(l)为：ji(l)＝j(posi(l))；(9)

修正后的适应度为：

其中kavr(l)是ji(l)的平均值，kavr0为[e³,+∞)范围内的随机数；修正适应度值的目的在于，避免适应度值过大，导致式(11)解算出的信息素趋于0。信息素ti(l)为：

定义最大迭代次数为lmax；i＝1,2,…,m；

初始化蚁群中每只蚂蚁的初始位置posi(0)，posi(0)满足条件：δumin≤posi(0)≤δumax；根据式(9)-(11)计算每只蚂蚁初始适应度ji(0)、初始修正适应度和初始信息素ti(0)；令迭代次数l＝1；

(3.2)计算本次迭代中蚂蚁i选择下一个可行位置的概率pi：

其中，ti(l-1)表示在上一次迭代中，蚂蚁i的信息素大小；在第l次迭代中，蚂蚁i按照式(13)所示的规则进行移动：

其中，rands为范围[-1,1]内的随机数，p0为预设的(0,0.5)范围内的状态转移概率常数，up和low分别为蚂蚁可移动范围的上下限，up＝δumax，low＝δumin；λ为随迭代次数增加而减小的变量，本发明中取值为：当pi<p0时，属于局部搜索；当pi≥p0时，属于全局搜索。计算每只蚂蚁位置移动后的适应度ji(l)、修正适应度和信息素ti(l)；

(3.3)综合上次迭代信息素剩余量与本次迭代中位置更新后的信息素，再次更新每只蚂蚁的信息素：

ti^new(l)＝(1-μ)ti^new(l-1)+l×ti(l)(14)

其中，μ为信息挥发系数，μ∈(0,1)，l为信息素收敛常数，l越大，信息素收敛越快，过大时会陷入局部最优，也不稳定，经过实验其最佳取值范围是[1,1000]；ti^new(0)＝ti(0)；

统计本次迭代中所有蚂蚁更新后信息素最大的值，即和取得最大值的蚂蚁的位置posbest(l)；

(3.4)判断是否终止迭代：

如果满足蚁群算法结束条件，则本次迭代蚂蚁的位置posbest(l)为最优控制增量序列δu(t)^*＝[δu(t|t)^*δu(t+1|t)^*…δu(t+nc-1|t)^*]^t，取δu(t)^*第一个元素δu(t|t)^*为当前时刻t的期望控制增量；转到步骤4；

如果不满足蚁群算法结束条件，令迭代次数l＝l+1，跳转到步骤(3.2)继续计算当前时刻t的期望控制增量。

所述蚁群算法结束条件为：

当前迭代次数l>lmax，或最优解满足精度要求：

||posbest(l)-posbest(l-1)||²<ξ

ξ是预设的精度阈值。步骤4、根据当前时刻的最优控制增量δu(t|t)^*控制轮胎转向角和车辆行驶速度，控制车辆运行，判断是否需要切换参考点r后进入步骤2继续下一个时刻的控制，具体步骤为：

(4.1)根据当前时刻的最优控制增量δu(t|t)^*和前一时刻的控制量计算当前时刻的控制量：

u(t)＝u(t-1)+δu(t|t)^*

由此得到当前时刻轮胎转向角δ(t)和车辆的速度v(t)，来控制车辆运行；

(4.2)判断是否已经到达或超过参考轨迹上当前参考点r，如果到达或超过，则将参考轨迹上下一个点作为参考点r；跳转至步骤2继续下一个时刻的控制。