python实现车辆跟随滑模控制的实例

第python实现车辆跟随滑模控制的实例上一篇文章介绍了Python使用OPENCV的目标跟踪算法实现自动视频标注效果，感兴趣的朋友点击查看，使用滑模变结构控制策略来解决汽车跟踪问题，今天通过本文介绍下python实现车辆跟随滑模控制的实例，内容如下所示：

下面分别采用指数趋近律、等速趋近律、准滑模控制的方法完成车辆跟随问题的仿真

importmatplotlib.pyplotasplt

指数趋近律、等速趋近律、准滑模控制的车辆跟随问题仿真,运行结果以图片形式保存在同目录下。

#q1,q2分别是切换函数ei1,ei2前面的系数

q1,q2=2,1

#lan是指数趋近律前面的系数

lan=0.5

#设定期望车间距均为12

l1,l2,l3,l4=12,12,12,12

#设定汽车质量均为1000

m1,m2,m3,m4=1000,1000,1000,1000

#设定动力学模型分子的速度平方项前的系数ci均为0.5(按照模型符号是负的)

c1,c2,c3,c4=0.5,0.5,0.5,0.5

#设定动力学模型分子的常数项系数Fi均为200(按照模型符号是负的)

f1,f2,f3,f4=200,200,200,200

#设定五辆车汽车的位移、速度、加速度

x0,x1,x2,x3,x4=[100.],[90.],[79.5],[68.5],[57.]

v0,v1,v2,v3,v4=[20.],[19.],[18.],[17.],[16.]

a1,a2,a3,a4=[0.],[0.],[0.],[0.]

#设定趋近律

defreaching_law(m:int,s:float,q2:int,mode='exponential'):

mode:指数趋近律exponential|等速趋近律uniform|准滑模控制quasi_sliding

ifmode=='exponential':

return-m*lan*s/q2

ifmode=='uniform':

epslion=0.3

ifs0:

return-m*epslion/q2

ifs==0:

return0

ifs0:

returnm*epslion/q2

ifmode=='quasi_sliding':

delta,epslion=0.8,2.

ifs-delta:

returnm*epslion/q2

ifsdelta:

return-m*epslion/q2

else:

return-m*epslion*s/(delta*q2)

#设定第一辆车的加速度(分段函数),要注意t的长度和a0的长度相等

defget_a0(t:list):

a0=[]

foriint:

ifi4:

a0.append(0)

continue

ifi=4andi7:

a0.append(-0.25*(i-4))

continue

ifi=7andi10:

a0.append(-0.75)

continue

ifi=10andi16:

a0.append(0.25*(i-10)-0.75)

continue

ifi=16andi19:

a0.append(0.75)

continue

ifi=19andi22:

a0.append(0.25*(19-i)+0.75)

continue

ifi=22andi=30:#注意i=30,所以是取两端,故为301份

a0.append(0)

returna0

if__name__=="__main__":

t=[float(i/10)foriinrange(301)]#将30秒划分成301份,[0,0.1,0.2,...,29.9,30]

a0=get_a0(t)

#四辆车的车间距误差ei1列表

e11=[x1[0]-x0[0]+l1]

e21=[x2[0]-x1[0]+l2]

e31=[x3[0]-x2[0]+l3]

e41=[x4[0]-x3[0]+l4]

#四辆车的车间距误差导数ei2的列表

e12=[v1[0]-v0[0]]

e22=[v2[0]-v1[0]]

e32=[v3[0]-v2[0]]

e42=[v4[0]-v3[0]]

#四辆车切换函数的列表

s1=[q1*e11[0]+q2*e12[0]]

s2=[q1*e21[0]+q2*e22[0]]

s3=[q1*e31[0]+q2*e32[0]]

s4=[q1*e41[0]+q2*e42[0]]

#四辆车控制律的列表

u1,u2,u3,u4=[0],[0],[0],[0]

foriinrange(1,301):

#最前车0的速度、加速度更新，可以看出更新时用了直线等效,0.1指的是时间标度(列表t划分的,也是之后绘图打印的x轴)

v0.append(v0[i-1]+0.1*(a0[i]+a0[i-1])*0.5)

x0.append(x0[i-1]+0.1*(v0[i]+v0[i-1])*0.5)

#车1的车间距误差及导数更新

e11.append(x1[i-1]-x0[i-1]+l1)

e12.append(v1[i-1]-v0[i-1])

#车1的切换函数更新

s1.append(q1*e11[i]+q2*e12[i])

#等效控制

u1equ=c1*(e12[i]+v0[i])*(e12[i]+v0[i])-m1*q1*e12[i]/q2+m1*a0[i]+f1

#反馈控制(指数趋近律)

u1n=reaching_law(m1,s1[i],q2)#默认采用指数趋近律,下同

#u1n=reaching_law(m1,s1[i],q2,mode='uniform')#采用等速趋近律

#u1n=reaching_law(m1,s1[i],q2,mode='quasi_sliding')#采用准滑模控制

#更新控制律

u1.append(u1equ+u1n)

#利用控制律更新车1的加速度、速度、位移,加速度是利用动力学模型得到的

a1.append((-c1*v1[i-1]*v1[i-1]+u1[i]-f1)/m1)

v1.append(v1[i-1]+0.1*(a1[i]+a1[i-1])*0.5)

x1.append(x1[i-1]+0.1*(v1[i]+v1[i-1])*0.5)

#车2、3、4过程同车1

e21.append(x2[i-1]-x1[i-1]+l2)

e22.append(v2[i-1]-v1[i-1])

s2.append(q1*e21[i]+q2*e22[i])

u2equ=c2*(e22[i]+v1[i])*(e22[i]+v1[i])-m2*q1*e22[i]/q2+m2*a1[i]+f2

u2n=reaching_law(m2,s2[i],q2)#默认采用指数趋近律

#u2n=reaching_law(m2,s2[i],q2,mode='uniform')#采用等速趋近律

#u2n=reaching_law(m2,s2[i],q2,mode='quasi_sliding')#采用准滑模控制

u2.append(u2equ+u2n)

a2.append((-c2*v2[i-1]*v2[i-1]+u2[i]-f2)/m2)

v2.append(v2[i-1]+0.1*(a2[i]+a2[i-1])*0.5)

x2.append(x2[i-1]+0.1*(v2[i]+v2[i-1])*0.5)

e31.append(x3[i-1]-x2[i-1]+l3)

e32.append(v3[i-1]-v2[i-1])

s3.append(q1*e31[i]+q2*e32[i])

u3equ=c3*(e32[i]+v2[i])*(e32[i]+v2[i])-m3*q1*e32[i]/q2+m3*a2[i]+f3

u3n=reaching_law(m3,s3[i],q2)

#u3n=reaching_law(m3,s3[i],q2,mode='uniform')

#u3n=reaching_law(m3,s3[i],q2,mode='quasi_sliding')

u3.append(u3equ+u3n)

a3.append((-c3*v3[i-1]*v3[i-1]+u3[i]-f3)/m3)

v3.append(v3[i-1]+0.1*(a3[i]+a3[i-1])*0.5)

x3.append(x3[i-1]+0.1*(v3[i]+v3[i-1])*0.5)

e41.append(x4[i-1]-x3[i-1]+l4)

e42.append(v4[i-1]-v3[i-1])

s4.append(q1*e41[i]+q2*e42[i])

u4equ=c4*(e42[i]+v3[i])*(e42[i]+v3[i])-m4*q1*e42[i]/q2+m4*a3[i]+f4

u4n=reaching_law(m4,s4[i],q2)

#u4n=reaching_law(m4,s4[i],q2,mode='uniform')

#u