懸架模糊控制

應用模糊控制，懸架加速度和速度作為輸入，主動懸架作動力是輸出

程式上還有不少問題，最終懸架位移在白噪聲的作用下竟然沒有收斂，水平實在有限，希望有相關研究的小夥伴可以指正。

匯入相關庫

import numpy as np
import skfuzzy as fuzz
import skfuzzy.control as ctrl
import matplotlib.pyplot as plt
from math import cos as cos

n0, n1, gq, u = 0.1, 0.01, 256.*10**(-6), 25.
l1 = 2 * 3.14 * n0 * (gq * u) ** (1 / 2)
l2 = 2 * 3.14 * n1 * u
Aa, La = 0.1, 1.
x_dzs_range=np.arange(-3,3,1,np.float32)
x_ddzs_range=np.arange(-3,3,1,np.float32)
y_fa_range=np.arange(-30,30,1,np.float32)
# 建立模糊控制變數
x_dzs=ctrl.Antecedent(x_dzs_range, 'dzs')
x_ddzs=ctrl.Antecedent(x_ddzs_range, 'ddzs')
y_fa=ctrl.Consequent(y_fa_range, 'fa')

定義模糊集和其隸屬度函數

x_dzs['NB']=fuzz.zmf(x_dzs_range, -3, -1)
x_dzs['NM']=fuzz.trimf(x_dzs_range,[-3,-2,0])
x_dzs['NS']=fuzz.trimf(x_dzs_range,[-3,-1,1])
x_dzs['ZO']=fuzz.trimf(x_dzs_range,[-2,0,2])
x_dzs['PS']=fuzz.trimf(x_dzs_range,[-1,1,3])
x_dzs['PM']=fuzz.trimf(x_dzs_range,[0,2,3])
x_dzs['PB']=fuzz.smf(x_dzs_range,1,3)

x_ddzs['NB']=fuzz.zmf(x_ddzs_range,-3,-1)
x_ddzs['NM']=fuzz.trimf(x_ddzs_range,[-3,-2,0])
x_ddzs['NS']=fuzz.trimf(x_ddzs_range,[-3,-1,1])
x_ddzs['ZO']=fuzz.trimf(x_ddzs_range,[-2,0,2])
x_ddzs['PS']=fuzz.trimf(x_ddzs_range,[-1,1,3])
x_ddzs['PM']=fuzz.trimf(x_ddzs_range,[0,2,3])
x_ddzs['PB']=fuzz.smf(x_ddzs_range,1,3)

y_fa['NB']=fuzz.zmf(y_fa_range,-30,30)
y_fa['NM']=fuzz.trimf(y_fa_range,[-30,-20,0])
y_fa['NS']=fuzz.trimf(y_fa_range,[-30,-10,10])
y_fa['ZO']=fuzz.trimf(y_fa_range,[-20,0,20])
y_fa['PS']=fuzz.trimf(y_fa_range,[-10,10,30])
y_fa['PM']=fuzz.trimf(y_fa_range,[0,20,30])
y_fa['PB']=fuzz.smf(y_fa_range,10,30)
# 設定輸出powder的解模糊方法——質心解模糊方式
y_fa.defuzzify_method='centroid'

制定規則

rule1=ctrl.Rule(antecedent=((x_dzs['PM'] & x_ddzs['PS'])|(x_dzs['PM'] & x_ddzs['PM'])|(x_dzs['PM'] & x_ddzs['PB'])|(x_dzs['PB'] & x_ddzs['ZO'])|(x_dzs['PB']&x_ddzs['PS'])|(x_dzs['PB'] & x_ddzs['PM'])|(x_dzs['PB'] & x_ddzs['PB'])),consequent=y_fa['NB'])
rule2=ctrl.Rule(antecedent=((x_dzs['ZO'] & x_ddzs['PM'])|(x_dzs['ZO'] & x_ddzs['PB'])|(x_dzs['PM'] & x_ddzs['NS'])|(x_dzs['PM'] & x_ddzs['ZO'])|(x_dzs['PB']&x_ddzs['NS'])),consequent=y_fa['NM'])
rule3=ctrl.Rule(antecedent=((x_dzs['NS'] & x_ddzs['PM'])|(x_dzs['NS'] & x_ddzs['PB'])|(x_dzs['ZO'] & x_ddzs['PS'])|(x_dzs['PS'] & x_ddzs['ZO'])|(x_dzs['PS']&x_ddzs['PS'])|(x_dzs['PS'] & x_ddzs['PM'])|(x_dzs['PS'] & x_ddzs['PB'])),consequent=y_fa['NS'])
rule4=ctrl.Rule(antecedent=((x_dzs['NB'] & x_ddzs['PM'])|(x_dzs['NB'] & x_ddzs['PB'])|(x_dzs['NM'] & x_ddzs['PB'])|(x_dzs['NM'] & x_ddzs['PM'])|(x_dzs['NS']&x_ddzs['PS'])|(x_dzs['ZO'] & x_ddzs['ZO'])|(x_dzs['PS'] & x_ddzs['NS'])|(x_dzs['PM'] & x_ddzs['NB'])|(x_dzs['PM'] & x_ddzs['NM'])|(x_dzs['PB'] & x_ddzs['NB'])|(x_dzs['PB'] & x_ddzs['NM'])),consequent=y_fa['ZO'])
rule5=ctrl.Rule(antecedent=((x_dzs['ZO'] & x_ddzs['NS'])|(x_dzs['PS'] & x_ddzs['NB'])|(x_dzs['PS'] & x_ddzs['NM'])),consequent=y_fa['PS'])
rule6=ctrl.Rule(antecedent=((x_dzs['NB'] & x_ddzs['PS'])|(x_dzs['NM'] & x_ddzs['ZO'])|(x_dzs['NM'] & x_ddzs['PS'])|(x_dzs['NS'] & x_ddzs['NB'])|(x_dzs['NS']&x_ddzs['NM'])|(x_dzs['NS'] & x_ddzs['NS'])|(x_dzs['NS'] & x_ddzs['ZO'])|(x_dzs['ZO'] & x_ddzs['NB'])|(x_dzs['ZO'] & x_ddzs['NM'])),consequent=y_fa['PM'])
rule7=ctrl.Rule(antecedent=((x_dzs['NB'] & x_ddzs['NB'])|(x_dzs['NB'] & x_ddzs['NM'])|(x_dzs['NB'] & x_ddzs['NS'])|(x_dzs['NB'] & x_ddzs['ZO'])|(x_dzs['NM']&x_ddzs['NB'])|(x_dzs['NM'] & x_ddzs['NM'])|(x_dzs['NM'] & x_ddzs['NS'])),consequent=y_fa['PB'])

# 構建系統
system = ctrl.ControlSystem(rules=[rule1, rule2, rule3, rule4, rule5, rule6, rule7])
sim = ctrl.ControlSystemSimulation(system)

在中間一個時間段新增高斯白噪聲

t = [i * 0.01 for i in range(1000)]
x = [i for i in range(1000)]
# x :原始訊號
# snr 訊雜比
def wgn(x,snr):
    snr=10 ** (snr/10.)
    xsum=0
    for i ,d in enumerate(x):
        xsum = xsum + abs(d)**2
    xpower=xsum / len(x)
    npower=xpower / snr
    
    l=len(x)
    a=np.random.randn(l)*np.sqrt(npower)
    a=np.array(a)
    a=a.reshape([l,1])
    return a

y = wgn(np.array(x),500).tolist()

設定路面干擾輸入

zr = [0.]
dzr = [0.]
for i in range(1, 1000):
    dzr.append(l1 * y[i][0] - l2 * zr[i-1])
    if i < 300 and i > 300 + 100 * La / u:
        zr.append(zr[i-1] + 0.01 * dzr[-1])
    else:
        zr.append(Aa/2 * (1-cos(2 * 3.14 * u /La * (i* 0.01 - 3))))

用1/4懸架模型迭代，具體模型可以去網上搜搜比如這個部落格

zs, zu, dzs, dzu, ddzs, ddzu = [0], [0], [0], [0], [0], [0]
ms, mu, ks, cs, kt = 1000., 125., 45000., 2350., 650000.
lan1, lan2, lan3 = 20., 10., 1.
fa = []
for i in range(1, 1000):
    vz = lan2 * dzs[i-1]
    az = lan3 * ddzs[i-1]
    sim.input['dzs'] = vz
    sim.input['ddzs'] = az
    sim.compute()
    fa.append(sim.output['fa'])
    ddzs.append(1 / ms * (cs * dzu[i-1]-dzs[i-1] + ks * (zu[i-1]-zs[i-1])+fa[-1]))
    dzs.append(dzs[i-1] + ddzs[i] * 0.01)
    zs.append(zs[i-1] + dzs[i] * 0.01)
    ddzu.append(1 / mu * (-cs * (dzu[i-1]-dzs[i-1]) - ks * (zu[i-1]-zs[i-1])-fa[-1] + kt * (zr[i-1]-zu[i-1])))
    dzu.append(dzu[i-1] + ddzu[i] * 0.01)
    zu.append(zu[i-1] + dzu[i]*0.01)

下圖是懸架速度與時間的關係，發現用這種方法並不收斂。。。。

下圖是懸架加速度與時間的關係，也不收斂。。。。