[发明专利]一种基于蚁群算法的车辆路径追踪控制方法

权利要求书

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

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

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

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

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

其中，

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

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

其中，为k时刻的跟踪误差，A_k,t和B_k,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的控制系数矩阵其中I₂表示2行2列的单位矩阵，I₃表示3行3列的单位矩阵，0_3×2表示3行2列的全0矩阵；

则N_c为控制时域，N_p为预测时域；

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

其中，R、Q为权重矩阵，ρ为权重系数，ε为松弛因子，N_c为控制时域，N_p为预测时域，η(t+i|t)为当前时刻t预测t+i时刻的系统输出量，控制增量序列ΔU(t)为待求解的未知量；

约束条件为：

ΔU_min≤ΔU(t)≤ΔU_max (7)

其中，ΔU_min、ΔU_max分别为控制时域内控制量变化量最小值、最大值的集合；

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

ΔU(t)^*＝[△u(t|t)^* △u(t+1|t)^* … △u(t+N_c-1|t)^*]^T， (8)

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

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

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

(3.1)蚁群算法初始化：

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

适应度J_i(l)为：J_i(l)＝J(Pos_i(l))；

修正后的适应度为：

其中K_avr(l)是J_i(l)的平均值，K_avr0为[e³,+∞)范围内的随机数；信息素T_i(l)为：

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

初始化蚁群中每只蚂蚁的初始位置Pos_i(0)，Pos_i(0)满足条件：ΔU_min≤Pos_i(0)≤ΔU_max；计算每只蚂蚁初始适应度J_i(0)、初始修正适应度J_i^new(0)和初始信息素T_i(0)；令迭代次数l＝1；

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

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

其中，rands为范围[-1,1]内的随机数，P₀为预设的(0,0.5)范围内的状态转移概率常数，up和low分别为蚂蚁可移动范围的上下限，up＝ΔU_max，low＝ΔU_min，λ为随迭代次数增加而减小的变量；

计算每只蚂蚁位置移动后的适应度J_i(l)、修正适应度和信息素T_i(l)；

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

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

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

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

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

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

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