基于粒子群-遗传混合算法的车辆横向PID控制方法与流程

技术特征：
1.一种基于粒子群
‑
遗传混合算法的车辆横向pid控制方法，其特征在于，通过以下步骤来实现：a).建立车辆运动学模型，设车辆的两前轮和两后轮分别具有同样的运动规律，两前轮胎和两后轮胎各用一个轮胎来描述，只考虑车辆横向运动，且忽略转向机构的影响，基于大地坐标系xoy建立车辆运动学模型，根据运动学定理，得出车辆各状态的更新迭代公式如下：x
t 1
＝x
t
v
t
cos(ψ
t
β)
×
dt
ꢀꢀꢀꢀꢀ
(1)y
t 1
＝y
t
v
t
sin(ψ
t
β)
×
dt
ꢀꢀꢀꢀꢀ
(2)v
t 1
＝v
t
a
×
dt
ꢀꢀꢀꢀꢀꢀꢀ
(4)式中β由公式(5)得出：式中：x
t
、x
t 1
和y
t
、y
t 1
分别为车辆质心在大地坐标系下的横坐标和纵坐标，ψ为车辆当前的偏航角，即车身与x轴的夹角，v为车辆的线速度，δ
f
为车前轮转角，即车前轮与车身的夹角，a为车辆的加速度，l
f
、l
b
分别表示前车轮、后车轮到车辆质心的距离；并建立车辆与目标路径的横向偏差e(t)的计算公式；b).建立pid分段控制参数表，首先判断pid分段控制参数表是否建立，如果已经建立，则根据车辆当前车速v从pid分段控制参数表中获取当前比例控制参数k
p
、积分控制参数k
i
和微分控制参数k
d
；如果pid分段控制参数表没有建立，则通过以下步骤建立pid分段控制参数表：b
‑
1).车速分段，以每段车速长度为1m/s进行分段，将车速划分为[0,1)、[1,2)、
…
、[n
‑
1,n)共计n段；b
‑
2).选取四种最优算法，选取粒子群算法pso、改进的粒子群算法pso
‑
cf、改进的粒子群算法apso以及遗传算法ga，作为求取pid分段控制参数的粒子群
‑
遗传混合算法pcag；b
‑
3).初始化四个算法的种群，首先建立如公式(6)所示的适应度函数：式中，e(t)为车辆与目标路径的横向偏差，t为当前时刻；然后确定每个最优算法的种群规模、迭代次数，并初始化种群的位置；b
‑
4).计算全局最优解，根据四种算法的更新速度和公式规则，更新每个粒子的速度和当前位置，根据适应度函数计算适应度，更新个体最优适应度和全局最优适应度，获取每种算法的全局最优解；b
‑
5).粒子替换，利用步骤b
‑
4)中利用4种算法求取的4个全局最优解，将四种算法中适应度最差的4个粒子替换掉；b
‑
6).收敛判断，判断全局最优解是否达到收敛条件，如果达到，则执行步骤b
‑
7)，如果没有达到收敛条件，则执行步骤b
‑
4)；b
‑
7).迭代次数判断，如果迭代次数没有达到要求，则执行步骤b
‑
4)；如果迭代次数达
到要求，则输出四种算法的最后解作为当前车速分段的pid分段控制参数，执行步骤b
‑
8)；b
‑
8).分段判断，判断所有的车速分段的pid分段控制参数是否已求取完毕，如果求取完毕，则执行步骤c)；如果没有求取完毕，则通过步骤b
‑
2)至步骤b
‑
7)求取下一车速分段的pid分段控制参数；c).获取模糊控制器输出，利用车速v和加速度a作为模糊控制器的输入，获取模糊控制器的输出δk
p
、δk
i
和δk
d
；δk
p
、δk
i
和δk
d
分别作为k
p
、k
i
和k
d
的增量，模糊控制器的输出与pid分段控制参数表输出的参数相加后输入至pid控制器；d).pid控制器输出，利用公式(7)计算出pid控制器的输出：d).pid控制器输出，利用公式(7)计算出pid控制器的输出：式中，b(t)为pid控制器输出的车辆转向角度，e(t)、e(t
‑
1)分别为当前时刻和前一时刻车辆与目标路径的横向偏差；e).车辆控制，将车辆转向角度b(t)输入至车辆控制器，对车辆的转向角度进行控制，以控制车辆与目标路径的横向偏差。2.根据权利要求1所述的基于粒子群
‑
遗传混合算法的车辆横向pid控制方法，其特征在于，步骤c)中模糊控制器通过以下方法进行建立：c
‑
1).输入和输出变量的模糊化，将车辆的车速v和加速度a作为模糊控制器的输入，其中v的论域为[
‑
30，30]，a的论域为[
‑
3，3]，模糊控制器的输出量为pid参数δk
p
、δk
i
和δk
d
；量化等级设为7级，语言变量分别定义为负大nb、负中nm、负小ns、零zo、正小ps、正中pm、正大pb；隶属度函数采用三角形与s形隶属度函数，其中：负大和正大采用s形隶属度函数，用于当有大偏差时快速响应，负中、负小、零、正小、正中均采用分辨率、灵敏度高的三角形隶属度函数；c
‑
2).制定模糊规则，采用“if
‑
then”控制规则，制定如表1所示的控制规则：表1对于输出的语言变量，从左至右分别为δk
p
、δk
i
和δk
d
的模糊规则；c
‑
3).模糊推理和解模糊，采用如公式(8)所示的模糊推理方法：r
i
:if e is e
j and e
c is e
k
then u is u
jk
ꢀꢀꢀꢀꢀꢀ
(8)其中，表示第i条控制规则，e为v，e
c
为a，e
j
、e
k
分别表示第j行和第k列的取值，i＝1,2,...,49，j、k＝1,2,...,7；u
jk
表示第j行、第k列对应的语言变量；
采用公式(9)所示的重心法进行解模糊：式中，m表示输出量化级数，m＝7，u
i
为模糊控制器论域中的值，μ(u
i
)为m的隶属度值，u为解模糊后最终的输出值；通过模糊推理和解模糊，最终得到模糊控制器的pid参数δk
p
、δk
i
和δk
d
。3.根据权利要求1或2所述的基于粒子群
‑
遗传混合算法的车辆横向pid控制方法，其特征在于，步骤b
‑
2)中：b
‑2‑
1).粒子群算法pso、改进的粒子群算法pso
‑
cf和改进的粒子群算法apso，均建立如公式(10)所示的当前位置和飞行速度：其中，x
in
和v
in
分别表示第i个粒子在第n维的位置和速度；算法开始时，先用随机位置x
i
和速度v
i
初始化粒子群，并使用粒子群的位置坐标作为输入值来计算适应度函数；粒子群算法pso的种群中第i个例子的第d维的速度和位置更新方程如公式(11)所示：式中，rand∈(0,1)为随机生成的数，c1和c2为学习因子，ω为惯性权重，p
gd
为所有粒子全局最优的适应度，p
id
为第i个粒子个体最优适应度；b
‑2‑
2).粒子群算法pso
‑
cf的速度更新方程如公式(12)所示：v
id
(t 1)＝λ(v
id
(t) c1×
rand
×
(p
id
‑
x
id
(t)) c2×
rand
×
(p
gd
‑
x
id
(t)))
ꢀꢀꢀꢀꢀꢀꢀꢀꢀ
(12)其中：粒子群算法pso
‑
cf的位置更新方程与粒子群算法pso的相同；b
‑2‑
3).改进的粒子群算法apso速度更新方程如公式(14)所示：式中，s为大于
‑
1的常数，t为当前迭代次数，t为总迭代次数；改进的粒子群算法apso的位置更新方程与粒子群算法pso的相同。4.根据权利要求1或2所述的基于粒子群
‑
遗传混合算法的车辆横向pid控制方法，其特征在于，步骤b
‑
2)所述的遗传算法ga通过以下步骤来实现：步骤1：随机生成个体种群，并计算每个个体的适应度；
步骤2：根据适应度确定该种群中的个体是遗传还是被淘汰；步骤3：将选择出进行遗传的个体进行配对作为父个体，将两个个体的部分码值进行交换，从而得到新的个体；步骤4：根据变异概率，随机变更个体中码值，得到变异后的新个体；步骤5：重新计算适应度，若到达迭代次数，则输出全局最优，否则重复步骤2。5.根据权利要求1或2所述的基于粒子群
‑
遗传混合算法的车辆横向pid控制方法，其特征在于，步骤a)中所述的车辆与目标路径的横向偏差e(t)的建立方法如下：a
‑
1).求质心到路径的最短距离，设车辆的目标路径曲线用公式(15)所示的三次方程表示：y＝ax3 bx2 cx d
ꢀꢀꢀꢀꢀꢀꢀ
(15)设车辆t时刻质心坐标为p
t
(x
t
,y
t
)，p
t
(x
t
,y
t
)到路径曲线最短路径与曲线交点为p(x,y)，则点p
t
(x
t
,y
t
)到曲线的最短距离为：a
‑
2).利用牛顿法求近似解；令：l＝(x
t
‑
x)2 (y
t
‑
y)2ꢀꢀꢀꢀꢀꢀꢀ
(17)将公式(15)带入公式(17)，得：将公式(18)对x一次求导：利用牛顿法求取方程l
′
的近似解：令：迭代初值：x0＝x
t
‑
0.3
ꢀꢀꢀꢀꢀꢀꢀ
(21)迭代公式：x
n 1
＝x
n
‑
f(x
n
)/f
′
(x
n
)
ꢀꢀꢀꢀꢀꢀꢀꢀ
(22)精度阈值为10
‑
12
,迭代次数为100；假定求得的近似解为x＝x
d
，代入式(15)中得到曲线上的一点p
d
＝(x
d
,y
d
)；a
‑
3).求横向偏差，过点p
d
＝(x
d
,y
d
)作路径曲线的切线，得到切线的斜率为为：记θ1为切线与x轴的夹角；则：k＝tanθ1ꢀꢀꢀꢀꢀ
(24)
记θ2为点p与点p
t
两点之间的连线与竖直方向的夹角，根据三角形内角和等于180
°
以及相似三角形定理得：θ1＝θ2；则车辆与目标路径的横向偏差e(t)为：6.根据权利要求1或2所述的基于粒子群
‑
遗传混合算法的车辆横向pid控制方法，其特征在于，步骤b)所建立的pid分段控制参数表为：表2速度参数0～1m/skp0，ki0，kd01～2m/skp1，ki1，kd12～3m/skp2，ki2，kd23～4m/skp3，ki3，kd34～5m/skp4，ki4，kd4……
n～n
1
m/skp
n
，ki
n
，kd
n
。

技术总结
本发明的基于粒子群


技术研发人员：高嵩 高珅琦 潘为刚 王目树 秦石铭 王书新 康超 张允刚 董兴学 徐飞 赵天怀 刘志远 胡浩 李正磊
受保护的技术使用者：山东交通学院
技术研发日：2021.08.30
技术公布日：2021/11/30