透過使用python coding，應用fuzzy 控制車子輪軸角度，達到不論任何的起始位置、角度，都可以將車子開往正確的到車位置。並且使用matplotlib繪圖、tkinter做出簡單的gui方便使用者操作
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| import math; | |
| #import tkinter as tk | |
| import matplotlib.pyplot as plt; | |
| import numpy as np; | |
| import random | |
| from tkinter import * | |
| import matplotlib | |
| from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg | |
| from matplotlib.figure import Figure | |
| x_list = [];# time | |
| y_list = [];# time | |
| Rule_table = [ 'S2','S3', 0 , 0 , 0 , | |
| 'S2','S3','S3','S3', 0 , | |
| 'B1','S1','S2','S3','S2', | |
| 'B2','B2','CE','S2','S2', | |
| 'B2','B3','B2','B1','S1', | |
| 0 ,'B3','B3','B3','B2', | |
| 0 , 0 , 0 ,'B3','B2', | |
| ]; | |
| phi_ulist = [0]*7; | |
| x_ulist = [0]*5; | |
| last_x_pos = 0; | |
| last_y_pos = 0; | |
| #last_phi = 30; | |
| curr_theta = 0; | |
| curr_phi = 0; | |
| b = 4;# length of the truck | |
| #init | |
| def Init_tlist(): | |
| for i in range(0,181,1): | |
| t_list = i; | |
| def math_cos(angle): | |
| if(angle != 180): | |
| angle = angle%180; | |
| angle = angle*math.pi/180; | |
| if(abs(angle - math.pi/2) < 0.0001 ): | |
| return 0; | |
| else: | |
| return math.cos(angle); | |
| def math_sin(angle): | |
| if(angle >= 360): angle = angle -360; | |
| angle = angle*math.pi/180; | |
| if(abs(angle - math.pi) < 0.0001 ): | |
| return 0; | |
| else: | |
| return math.sin(angle); | |
| #formula | |
| def x_trajectory(phi,theta): | |
| global last_x_pos; | |
| result = last_x_pos + math_cos(phi+theta) + math_sin(theta)*math_sin(phi); | |
| last_x_pos = result; | |
| return result; | |
| def y_trajectory(phi,theta): | |
| global last_y_pos; | |
| theta = theta*math.pi/180; | |
| phi = phi*math.pi/180; | |
| result = last_y_pos + math_sin(phi+theta) + math_sin(theta)*math_cos(phi); | |
| last_y_pos = result; | |
| return result; | |
| def new_phi_output(cur_phi,theta): | |
| #if(cur_phi%180 == 0): cur_phi = 0; | |
| if(theta%180 == 0): theta = 0; | |
| cur_phi = cur_phi*math.pi/180; | |
| theta = theta*math.pi/180; | |
| result = cur_phi - math.asin(2*math.sin(theta)/b); | |
| result = result*180/math.pi; | |
| if(result < -90 ): | |
| return 90; | |
| elif(result > 270): | |
| return 270; | |
| else: | |
| return result; | |
| #fuzzy rule | |
| def Triangular_rule(x,a,b): | |
| m = (a+b)/2; | |
| a = m-a; | |
| b = b-m; | |
| if( x >= m - a and x <= m ): | |
| return 1 + (x-m)/a; | |
| elif(x > m and x <= m + b): | |
| return 1 + (m-x)/b; | |
| else: | |
| return 0; | |
| def Trapezood_rule(x,a,m,mode): | |
| if(mode == 'Left'): | |
| slope = 1/(m-a); | |
| if( x > m ): | |
| return 1; | |
| elif(x > a and x < m ): | |
| return slope*(m-x); | |
| else: | |
| return 0; | |
| elif(mode == 'Right'): | |
| slope = -1/(m-a); | |
| if( x > m ): | |
| return 0; | |
| elif(x > a and x < m ): | |
| return slope*(m-x); | |
| else: | |
| return 1; | |
| else: | |
| print('mode is nono'); | |
| return 0; | |
| def BackTriangular_rule(y,a,b): | |
| return (a+b)/2; | |
| def BackHalfTriangular_rule(y,a,b,mode): | |
| if(mode == 'Left'): | |
| x = b - (b-a)*y; | |
| elif(mode == 'Right'): | |
| x = a + (b-a)*y; | |
| else: | |
| return 0; | |
| return (a+x)/2; | |
| def phi_rule(type,x): | |
| return { | |
| 'S3' : lambda x: Triangular_rule(x,-115,-15), | |
| 'S2' : lambda x: Triangular_rule(x,-45,45), | |
| 'S1' : lambda x: Triangular_rule(x,15,90), | |
| 'CE' : lambda x: Triangular_rule(x,80,100), | |
| 'B1' : lambda x: Triangular_rule(x,90,165), | |
| 'B2' : lambda x: Triangular_rule(x,135,225), | |
| 'B3' : lambda x: Triangular_rule(x,195,295), | |
| }.get(type)(x) | |
| def x_rule(type,x): | |
| return { | |
| 'S2' : lambda x: Trapezood_rule(x,1.5,7,'Right'), | |
| 'S1' : lambda x: Triangular_rule(x,4,10), | |
| 'CE' : lambda x: Triangular_rule(x,9,11), | |
| 'B1' : lambda x: Triangular_rule(x,10,16), | |
| 'B2' : lambda x: Trapezood_rule(x,13,18.5,'Left'), | |
| }.get(type)(x) | |
| def Out_rule(type,y): | |
| return { | |
| 'S3' : lambda y: BackHalfTriangular_rule(y,-40,-20,'Left'),#NL | |
| 'S2' : lambda y: BackTriangular_rule(y,-33,-7),#S2 | |
| 'S1' : lambda y: BackTriangular_rule(y,-14, 0),#S1 | |
| 'CE' : lambda y: BackTriangular_rule(y, -4, 4),#CE | |
| 'B1' : lambda y: BackTriangular_rule(y, 0,14),#B1 | |
| 'B2' : lambda y: BackTriangular_rule(y, 7,33),#B2 | |
| 'B3' : lambda y: BackHalfTriangular_rule(y,20,40,'Right'),#B3 | |
| 0 : lambda y: 0, | |
| }.get(type)(y) | |
| def inference(phi,x): | |
| phi_ulist[0] = phi_rule('S3',phi); | |
| phi_ulist[1] = phi_rule('S2',phi); | |
| phi_ulist[2] = phi_rule('S1',phi); | |
| phi_ulist[3] = phi_rule('CE',phi); | |
| phi_ulist[4] = phi_rule('B1',phi); | |
| phi_ulist[5] = phi_rule('B2',phi); | |
| phi_ulist[6] = phi_rule('B3',phi); | |
| ''' | |
| for i in range(0,7,1): | |
| if(phi_ulist[i] > 0 ): | |
| print('inf phi',i,phi_ulist[i]); | |
| ''' | |
| x_ulist[4] = x_rule('B2',x); | |
| x_ulist[3] = x_rule('B1',x); | |
| x_ulist[2] = x_rule('CE',x); | |
| x_ulist[1] = x_rule('S1',x); | |
| x_ulist[0] = x_rule('S2',x); | |
| def RuleTable(k,y): | |
| weight = Out_rule(Rule_table[k],y); | |
| return weight; | |
| def LastOutput(): | |
| last_ulist = [0]*35; | |
| w_sum = 0; | |
| s_sum = 0 | |
| k = 0; | |
| for i in range(0,7,1): | |
| for j in range(0,5,1): | |
| last_ulist[k] = min(phi_ulist[i],x_ulist[j]); | |
| #if(last_ulist[k] > 0.01): | |
| # print('k',k,last_ulist[k]); | |
| k = k + 1; | |
| for k in range(0,35,1): | |
| w_sum += RuleTable(k,last_ulist[k])*last_ulist[k]; | |
| for k in range(0,35,1): | |
| s_sum += last_ulist[k]; | |
| if(s_sum == 0): | |
| print('s_sum == 0'); | |
| return 0; | |
| if(w_sum == 0): | |
| print('w_sum == 0'); | |
| return 0; | |
| output_value = w_sum /s_sum ; | |
| if(output_value < -40): | |
| return -40; | |
| elif(output_value > 40): | |
| return 40; | |
| else: | |
| #print('w_sum: ',w_sum,'sum: ',s_sum,'output_value',output_value); | |
| return output_value; | |
| def main_iteration(input_x,input_phi): | |
| #main init | |
| global curr_theta; | |
| global x_list; | |
| global y_list; | |
| global last_x_pos ; | |
| global last_y_pos ; | |
| global curr_phi ; | |
| last_x_pos = 0; | |
| last_y_pos = 0; | |
| curr_theta = 0; | |
| curr_phi = 0; | |
| last_x_pos = input_x; | |
| curr_phi = input_phi; | |
| del x_list[:] ;# time | |
| del y_list[:];# time | |
| x_list = [0]*180;# time | |
| y_list = [0]*180;# time | |
| #print('input2',last_x_pos,curr_phi,curr_theta); | |
| if(last_x_pos < 0): | |
| last_x_pos = 0; | |
| elif(last_x_pos > 20): | |
| last_x_pos = 20; | |
| if(curr_phi < -90): | |
| curr_phi = -90; | |
| elif(curr_phi > 270): | |
| curr_phi = 270; | |
| #main iteration | |
| for t in range(0,180,1): | |
| x_list[t] = x_trajectory(curr_phi,curr_theta); | |
| curr_phi = new_phi_output(curr_phi,curr_theta); | |
| y_list[t] = y_trajectory(curr_phi,curr_theta); | |
| if(abs(x_list[t] - 10)< 0.05 and abs(curr_phi - 90) < 0.1 ): | |
| print('break!'); | |
| x_list = x_list[:t]; | |
| y_list = y_list[:t]; | |
| break; | |
| else: | |
| inference(curr_phi,x_list[t]); | |
| curr_theta = LastOutput(); | |
| print('t:',t,'x: ',x_list[t],'y: ',y_list[t],'phi: ',curr_phi,'theta: ',curr_theta); | |
| pass; | |
| def drawPic(): | |
| """ | |
| 获取GUI界面设置的参数，利用该参数绘制图片 | |
| """ | |
| #获取GUI界面上的参数 | |
| try: | |
| X_POS = float(inputEntry_x.get()); | |
| PHI_ANGLE = float(inputEntry_phi.get()); | |
| except: | |
| #X_POS = 10; | |
| #PHI_ANGLE = 180; | |
| X_POS = np.random.uniform(0,20); | |
| PHI_ANGLE = np.random.uniform(-90,270); | |
| print('please input number'); | |
| #清空图像，以使得前后两次绘制的图像不会重叠 | |
| drawPic.f.clf() | |
| drawPic.a=drawPic.f.add_subplot(111) | |
| print('input1',X_POS,PHI_ANGLE); | |
| main_iteration(X_POS,PHI_ANGLE); | |
| color=['b','r','y','g'] | |
| drawPic.a.plot(x_list,y_list,'-.',color=color[np.random.randint(len(color))]); | |
| drawPic.a.set_title('Demo: Draw Truck Backer Control') | |
| drawPic.canvas.draw() | |
| if __name__ == '__main__': | |
| matplotlib.use('TkAgg') | |
| root=Tk() | |
| root.title("TruckBackCtrl Simulate") | |
| #在Tk的GUI上放置一个画布，并用.grid()来调整布局 | |
| drawPic.f = Figure(figsize=(5,4), dpi=100) | |
| drawPic.canvas = FigureCanvasTkAgg(drawPic.f, master=root) | |
| drawPic.canvas.draw(); | |
| drawPic.canvas.get_tk_widget().grid(row=0, columnspan=3) | |
| drawPic(); | |
| #放置标签、文本框和按钮等部件，并设置文本框的默认值和按钮的事件函数 | |
| Label(root,text='input x ').grid(row=1,column=0) | |
| Label(root,text='input phi ').grid(row=1,column=1) | |
| inputEntry_x = Entry(root); | |
| inputEntry_x.grid(row=2,column=0); | |
| inputEntry_phi = Entry(root); | |
| inputEntry_phi.grid(row=2,column=1); | |
| Button(root,text='simulate',command=drawPic).grid(row=2,column=2,columnspan=3) | |
| #启动事件循环 | |
| root.mainloop() |
