Thông tin tài liệu
LỜI CẢM ƠN Tơi xin bày tỏ lịng biết ơn sâu sắc đến hai Thầy hướng dẫn: PGS TS Nguyễn Thanh Phương TS Trần Viết Thắng tận tình hướng dẫn tơi thực cơng trình nghiên cứu hồn thành luận án Tơi xin chân thành cảm ơn Thầy, Cô, Cán bộ, Nhân viên Viện Nghiên cứu Điện tử, Tin học, Tự động hóa Phân Viện Nghiên cứu Điện tử, Tin học, Tự động hóa TP.HCM nhiệt tình giúp đỡ, tạo điều kiện thuận lợi cho suốt thời gian học tập nghiên cứu Cuối cùng, xin bày tỏ lòng biết ơn chân thành đến Gia đình tơi, người thân chia sẻ khó khăn, ln động viên tơi vượt qua khó khăn để hồn thành tốt luận án ii MỤC LỤCS LỜI CAM ĐOAN i LỜI CẢM ƠN ii MỤC LỤC iii DANH MỤC CÁC TỪ VIẾT TẮT vi DANH MỤC CÁC ĐƠN VỊ ĐO LƯỜNG viii DANH MỤC HÌNH VẼ ix DANH MỤC BẢNG BIỂU xii DANH MỤC KÍ HIỆU xiii MỞ ĐẦU CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG ĐIỀU KHIỂN ROBOT TỰ HÀNH QUA MẠNG 1.1 Giới thiệu hệ thống vật lí ảo cho hệ thống kho vận nội môi trường công nghiệp 1.2 Giới thiệu hệ thống điều khiển qua mạng cho robot tự hành 13 1.2.1 Giới thiệu chung 13 1.2.2 Hệ thống điều khiển robot tự hành qua mạng 20 1.3 Các vấn đề cần giải 25 1.4 Tình hình nghiên cứu 30 1.4.1 Về hệ thống có thời gian trễ 30 1.4.2 Về điều khiển robot tự hành có thời gian trễ 36 1.5 Kết luận 41 CHƯƠNG 2: XÂY DỰNG MƠ HÌNH HĨA ROBOT TỰ HÀNH TRONG SỰ ẢNH HƯỞNG CỦA THỜI GIAN TRỄ 43 2.1 Mô tả hệ thống 43 2.2 Mơ hình động học robot tự hành 46 2.3 Mơ hình động lực học robot tự hành 47 2.4 Mô hình hóa thời gian trễ 53 iii 2.5 Kết luận chương 57 CHƯƠNG 3: THIẾT KẾ BỘ ĐIỀU KHIỂN BÁM THEO QUỸ ĐẠO MONG MUỐN CHO ROBOT TỰ HÀNH VỚI TÍN HIỆU TRỄ ĐẦU VÀO 59 3.1 Thuật toán bám theo quỹ đạo mong muốn 60 3.2 Hệ thống tham chiếu mơ hình ổn định 65 3.3 Bộ điều khiển thích nghi theo hệ thống tham chiếu cho hệ thống ngõ vào – ngõ 71 3.4 Bộ điều khiển thích nghi theo hệ thống tham chiếu cho robot tự hành 83 3.5 Kết luận chương 92 CHƯƠNG 4: KẾT QUẢ 94 4.1 Cấu trúc hệ thống 94 4.1.1 Cấu hình điều khiển qua mạng 94 4.1.2 Cấu trúc robot tự hành 95 4.1.3 Cấu trúc trao đổi liệu qua mạng 97 4.2 Thiết lập mô 100 4.3 Kết mô Matlab 103 4.3.1 Trường hợp – Khơng có thời gian trễ 103 4.3.2 Trường hợp – Thời gian trễ 0.01 giây 108 4.3.3 Trường hợp – Thời gian trễ 0.05 giây 112 4.3.4 Trường hợp – Thời gian trễ 0.1 giây 115 4.4 Thực nghiệm xác định thời gian trễ 120 4.5 Kết thực nghiệm điều khiển robot tự hành ảo qua mạng 124 4.6 Kết luận chương 130 Kết luận 133 CƠNG TRÌNH CƠNG BỐ TẠP CHÍ 137 CƠNG TRÌNH CÔNG BỐ HỘI NGHỊ 138 TÀI LIỆU THAM KHẢO 139 PHỤ LỤC 152 A.1 Chương trình mơ hệ thống tham chiếu 152 iv A.2 Chương trình mơ điều khiển thích nghi theo hệ thống tham chiếu cho hệ thống ngõ vào – ngõ 154 A.3 Chương trình mơ điều khiển thích nghi theo hệ thống tham chiếu cho robot tự hành – Trường hợp khơng có thời gian trễ 160 A.4 Chương trình mơ điều khiển thích nghi theo hệ thống tham chiếu cho robot tự hành – Trường hợp thời gian trễ 10ms 170 A.5 Chương trình mơ điều khiển thích nghi theo hệ thống tham chiếu cho robot tự hành – Trường hợp thời gian trễ 50ms 180 A.6 Chương trình mơ điều khiển thích nghi theo hệ thống tham chiếu cho robot tự hành – Trường hợp thời gian trễ 100ms 189 A.7 Chương trình phần mềm đo thời gian trễ tầng mạng 199 A.8 Chương trình phần mềm đo thời gian trễ tầng ứng dụng – Thành phần nguồn 201 A.9 Chương trình phần mềm đo thời gian trễ tầng ứng dụng – Thành phần đích 205 A.10 Chương trình phần mềm mơ động học/động lực học robot tự hành 208 A.11 Chương trình phần mềm điều khiển giao diện điều khiển trung tâm 211 v DANH MỤC CÁC TỪ VIẾT TẮT AC ACK AGV CAN CSMA/CD DC DCS FTP HMI HTTP HTTPS ID IMU IP LAN LQR MIMO MRAC MQTT NCS NTP PC PI PID PLC PWM SISO SYN RTS RTT TCP Alternative current Acknowledgement Automated guided vehicle Controlled Area Network Carrier Sense Multiple Access / Collision Detection Direct current Distributed control system File Transfer Protocol Human machine interface Hypertext Transfer Protocol Hypertext Transfer Protocol Secure Identifier Inertia measurement unit Internet protocol Local Area Network Linear Quadratic Regulator Multi input multi output Model reference adaptive control Message Queuing Telemetry Transport Networked control system Network Time Protocol Personal computer Proportional – Integral Proportional – Integral – Derivative Programmable logic control Pulse Width modulation Single input single output Synchronous Request to send Round trip time Transmission Control vi Dòng điện xoay chiều Hồi đáp Phương tiện tự hành Mạng điều khiển nội Dòng điện chiều Hệ thống điều khiển phân tán Giao thức truyền tải tập tin Giao diện người - máy Giao thức truyền tải siêu văn Giao thức truyền tải siêu văn bảo mật Mã định danh Thiết bị đo lường quán tính Giao thức mạng Mạng giao tiếp nội Hệ thống nhiều ngõ vào nhiều ngõ Bộ điều khiển thích nghi theo mơ hình tham chiếu chuẩn Giao thức truyền thơng điệp theo mơ hình cung cấp/th bao Hệ thống điều khiển qua mạng truyền thông Máy tính Bộ điều khiển tỉ lệ - tích phân Bộ điều khiển tỉ lệ - tích phân – vi phân Bộ điều khiển khả trình Phương pháp điều chế độ rộng xung Hệ thống ngõ vào ngõ Đồng Yêu cầu để gửi Thời gian lặp vòng UDP UWB Protocol Universal asynchronous receiver transmitter User Datagram Protocol Ultra-wideband WIFI Wireless fidelity UART Giao thức liệu người dùng Cơng nghệ radio băng thơng siêu rộng Tín hiệu truyền không dây vii DANH MỤC CÁC ĐƠN VỊ ĐO LƯỜNG Khối lượng Chiều dài Thời gian Vận tốc Tốc độ truyền Công suất Độ phân giải kilogram meter, millimeter second, millisecond meter per second, round per minute bit per second Watt pulse per round viii kg m, mm s, ms m/s, rpm bps W ppr DANH MỤC HÌNH VẼ Hình 2.1 Robot tự hành với a) cấu trúc hai bánh vi sai b) cấu trúc cấu lái43 Hình 2.2 Sơ đồ ngun lí robot tự hành với cấu trúc hai bánh xe vi sai 44 Hình 2.3 Các thao tác chuyển động robot tự hành 45 Hình 2.4 Robot tự hành hệ tọa độ gốc 46 Hình 2.5 Sơ đồ vật thể tự bánh xe robot tự hành 48 Hình 2.6 Sơ đồ vật thể tự động - hộp số - bánh xe 49 Hình 2.7 Mạch điện phần ứng động chiều 50 Hình 2.8 Sơ đồ vật thể tự robot tự hành 51 Hình 2.9 Cấu trúc hệ thống điều khiển qua mạng robot tự hành 54 Hình 2.10 Cấu trúc trao đổi thơng tin robot tự hành điều khiển trung tâm 55 Hình 2.11 Thời gian chu trình điều khiển 56 Hình 3.1 Sai số vị trí robot quỹ đạo mong muốn 61 Hình 3.2 Xác định sai số bám theo quỹ đạo mong muốn 62 Hình 3.3 Đáp ứng hệ thống tham chiếu với tín hiệu đầu vào hàm nấc 69 Hình 3.4 Đáp ứng hệ thống tham chiếu với tín hiệu đầu vào hàm sin 70 Hình 3.5 Đáp ứng hệ thống với điều khiển đề xuất điều khiển MRAC 81 Hình 3.6 Thông số độ lợi điều khiển đề xuất 81 Hình 3.7 Tín hiệu điều khiển hệ thống với điều khiển đề xuất 82 Hình 4.1 Sơ đồ cấu hình điều khiển điều khiển trung tâm robot tự hành qua mạng internet 95 Hình 4.2 Robot tự hành Pioneer 3DX 96 Hình 4.3 Kích thước robot tự hành Pioneer 3DX 96 Hình 4.4 Cấu trúc trao đổi liệu qua mạng điều khiển trung tâm robot tự hành 97 Hình 4.5 Đáp ứng vị trí robot tự hành theo quỹ đạo mong muốn – Trường hợp 103 ix Hình 4.6 Đáp ứng vị trí theo thời gian – Trường hợp 104 Hình 4.7 Thành phần vận tốc tịnh tiến đáp ứng động lực học - Trường hợp 105 Hình 4.8 Thành phần vận tốc góc đáp ứng động lực học - Trường hợp 106 Hình 4.9 Thông số độ lợi 𝚯 - Trường hợp 106 Hình 4.10 Thơng số độ lợi 𝝃 - Trường hợp 107 Hình 4.11 Tín hiệu điều khiển - Trường hợp 107 Hình 4.12 Đáp ứng vị trí robot tự hành theo quỹ đạo mong muốn – Trường hợp 108 Hình 4.13 Đáp ứng vị trí theo thời gian – Trường hợp 109 Hình 4.14 Thành phần vận tốc tịnh tiến đáp ứng động lực học - Trường hợp 109 Hình 4.15 Thành phần vận tốc góc đáp ứng động lực học - Trường hợp 110 Hình 4.16 Thơng số độ lợi Θ - Trường hợp 110 Hình 4.17 Thơng số độ lợi ξ - Trường hợp 111 Hình 4.18 Tín hiệu điều khiển - Trường hợp 111 Hình 4.19 Đáp ứng vị trí robot tự hành theo quỹ đạo mong muốn – Trường hợp 112 Hình 4.20 Đáp ứng vị trí theo thời gian – Trường hợp 112 Hình 4.21 Thành phần vận tốc tịnh tiến đáp ứng động lực học - Trường hợp 113 Hình 4.22 Thành phần vận tốc góc đáp ứng động lực học - Trường hợp 113 Hình 4.23 Thơng số độ lợi Θ - Trường hợp 114 Hình 4.24 Thơng số độ lợi ξ - Trường hợp 114 Hình 4.25 Tín hiệu điều khiển - Trường hợp 115 Hình 4.26 Đáp ứng vị trí robot tự hành theo quỹ đạo mong muốn – Trường hợp 115 Hình 4.27 Đáp ứng vị trí theo thời gian – Trường hợp 116 x Hình 4.28 Thành phần vận tốc tịnh tiến đáp ứng động lực học - Trường hợp 116 Hình 4.29 Thành phần vận tốc góc đáp ứng động lực học - Trường hợp 117 Hình 4.30 Thơng số độ lợi Θ - Trường hợp 117 Hình 4.31 Thơng số độ lợi ξ - Trường hợp 118 Hình 4.32 Tín hiệu điều khiển - Trường hợp 118 Hình 4.33 Thông số độ lợi Θ - Trường hợp cải tiến 119 Hình 4.34 Thông số độ lợi ξ - Trường hợp cải tiến 119 Hình 4.35 Tín hiệu điều khiển - Trường hợp cải tiến 120 Hình 4.36 Thời gian trễ tầng mạng - Phép đo 120 Hình 4.37 Thời gian trễ tầng mạng - Phép đo 121 Hình 4.38 Thời gian trễ tầng ứng dụng - Phép đo 122 Hình 4.39 Thời gian trễ tầng ứng dụng - Phép đo 123 Hình 4.40 Cấu trúc trao đổi liệu qua mạng điều khiển trung tâm phần mềm mô động học/động lực học robot tự hành 125 Hình 4.41 Giao diện phần mềm mơ động học/động lực học robot tự hành 126 Hình 4.42 Phần mềm điều khiển giao diện điều khiển trung tâm 127 Hình 4.43 Đáp ứng động lực học robot tự hành ảo 128 Hình 4.44 Đáp ứng động học robot tự hành ảo 129 Hình 4.45 Tín hiệu điều khiển điều khiển đề xuất 130 xi def MQTT_connect(): global controller_start, count if controller_start == 0: broker_address = txt_brokeraddress.get() broker_port = int(txt_port.get()) Output.insert(END, "connecting to broker\n") client.connect(broker_address, port=broker_port, keepalive=60) Output.insert(END, "Subscribing to ncsntt/latency_request\n") client.subscribe("ncsntt/latency_request") pb_connect.config(text="Connecting ") controller_start = else: controller_start = pb_connect.config(text="Connect") Output.delete(1.0, END) Input_txt.delete(1.0, END) client.disconnect() count = txt_count.set(count) client.loop_start() # ===================================================# def on_message(client, userdata, message): global controller_start, count msg = str(message.payload) Input_txt.insert(END, message.topic + " = " + msg + "\n") value = float(message.payload) if message.topic == "ncsntt/latency_request": receive = value if receive == 3: client.publish("ncsntt/latency_reply", client.publish("ncsntt/latency_reply", client.publish("ncsntt/latency_reply", client.publish("ncsntt/latency_reply", client.publish("ncsntt/latency_reply", count = count + txt_count.set(count) 0) 1) 2) 3) receive+1) # =========================================================================== ============================================ window = Tk() window.geometry('300x300') window.configure(bg='white') window.title('Latency Measurement @N T Trung - Destination') window.resizable(0, 0) topFrame = Frame(window, width=500, height=20, background="white") topFrame.grid(row=0, column=0, sticky="NSEW") bottomFrame = Frame(window, width=1000, height=550, background="white") bottomFrame.grid(row=1, column=0, sticky="NSEW") # ROW =========================================================================== ===================================== Label(topFrame, text="Broker Address", font=('Times', 12), 206 bg='white').grid(row=0, column=0, sticky="W") # txt_brokeraddress = StringVar() et_brokeraddress = Entry(topFrame, textvariable=txt_brokeraddress, font=('Times', 12), width=20) et_brokeraddress.grid(row=0, column=1, sticky="NSEW", columnspan=2) et_brokeraddress.insert(0, "broker.mqttdashboard.com") # ROW =========================================================================== ===================================== Label(topFrame, text="Port", font=('Times', 12), bg='white').grid(row=1, column=0, sticky="W") # txt_port = StringVar() et_port = Entry(topFrame, textvariable=txt_port, font=('Times', 12), width=10) et_port.insert(0, "1883") et_port.grid(row=1, column=1, sticky="NSEW") # pb_connect = Button(topFrame, text='Connect', font=('Times', 12), command=MQTT_connect, width=10) pb_connect.grid(row=1, column=2, sticky="NSEW") # ROW =========================================================================== ===================================== Output = Text(topFrame, font=('Times', 12), bg="white", width=30, height=5) Output.grid(row=2, column=0, columnspan=3, sticky="NSEW") # ROW =========================================================================== ===================================== Input_txt = Text(topFrame, font=('Times', 12), bg="white", width=30, height=5) Input_txt.grid(row=3, column=0, columnspan=3, sticky="NSEW") # ROW =========================================================================== ===================================== # pb_start = Button(topFrame, text='Start', font=('Times', 12), command=Start_button, width=10) # pb_start.grid(row=4, column=0, sticky="NSEW") # # # pb_stop = Button(topFrame, text='Stop', font=('Times', 12), command=Stop_button, width=10) # pb_stop.grid(row=4, column=1, sticky="NSEW") # ROW =========================================================================== ===================================== # Label(topFrame, text="Time delay", font=('Times', 12), bg='white').grid(row=5, column=0, sticky=W) # txt_time_delay = StringVar() # Entry(topFrame, textvariable=txt_time_delay, font=('Times', 12), bg='white', width=20).grid(row=5, column=1, # columnspan=2, sticky=W) # ROW =========================================================================== ===================================== Label(topFrame, text="Count", font=('Times', 12), bg='white').grid(row=6, column=0, sticky=W) # txt_count = StringVar() 207 Entry(topFrame, textvariable=txt_count, font=('Times', 12), bg='white', width=20).grid(row=6, column=1, columnspan=2, sticky=W) # =========================================================================== =========================================== Output.insert(END, "creating new instance\n") client = mqtt.Client() # create new instance client.on_message = on_message # attach function to callback window.mainloop() A.10 Chương trình phần mềm mơ động học/động lực học robot tự hành import time import paho.mqtt.client as mqtt # import the client1 from tkinter import * # import everything from tkinter import threading import math import numpy as np #Setting sampling_time = 0.1 #Parameter r_w = 0.0975 #Wheel radius Width = 0.33284 #Lateral wheel distance, [m] M = #Mass of robot, [kg] I = 0.2641 #Moment of inertia of robot, [kg.m^2] d = 0.15 #Distance from P point to C point #DC Motors Parameters inertia = 1.41 viscous = 6.48 kmn_Ra = 10.47 a11_den a22_den a11_num a22_num = = = = viscous viscous inertia + inertia + #n^2*J_m + J_w, [kg.m^2] #n^2*b_m + b_w + km^2*n^2/Ra, [N.s/m] #kmn_Ra = km*n/Ra, [N.m/V] M * np.power(r_w,2)/2 (2*I*np.power(r_w,2))/np.power(Width,2) a11 = -a11_den/a11_num a22 = -a22_den/a22_num b1 = kmn_Ra*r_w /(2*a11_num) b2 = kmn_Ra*r_w /(Width*a22_num) A = [[a11, 0], [0, a22]] B = [[b1, b1], [b2, -b2]] z = [[0],[0]] # =========================================================================== =========================================== xc = yc = theta = v_a_l = v_a_r = control_u = [[0],[0]] 208 controller_start = send_back = receive = # =========================================================================== ============================================ def MQTT_connect(): global controller_start, xc, yc, theta, v_a_r, v_a_l if controller_start == 0: broker_address = txt_brokeraddress.get() broker_port = int(txt_port.get()) Output.insert(END, "connecting to broker\n") client.connect(broker_address, port=broker_port, keepalive=60) Output.insert(END, "Subscribing to ncsntt/command_v_a_l\n") client.subscribe("ncsntt/command_v_a_l") Output.insert(END, "Subscribing to ncsntt/command_v_a_r\n") client.subscribe("ncsntt/command_v_a_r") pb_connect.config(text="Connecting ") controller_start = client.loop_start() controller() else: controller_start = pb_connect.config(text="Connect") client.disconnect() xc = yc = theta = v_a_l = v_a_r = txt_xc.set(0) txt_yc.set(0) txt_theta.set(0) # ===================================================# def on_message(client, userdata, message): global v_a_r, v_a_l, control_u, controller_start, receive, send_back msg = str(message.payload) Input_txt.insert(END, message.topic + " = " + msg + "\n") value = float(message.payload) if message.topic == "ncsntt/command_v_a_l": v_a_l = round(value, 3) txt_v_a_l.set(v_a_l) if message.topic == "ncsntt/command_v_a_r": v_a_r = round(value, 3) txt_v_a_r.set(v_a_r) control_u = [[v_a_l],[v_a_r]] def controller(): global z, xc, yc, theta, A, B, sampling_time z = z + (np.dot(A, z) + np.dot(B, control_u)) * sampling_time velocity = z[0][-1] omega = z[1][-1] xc = xc + velocity * math.cos(theta + omega * sampling_time / 2) * sampling_time yc = yc + velocity * math.sin(theta + omega * sampling_time / 2) * sampling_time theta = theta + omega * sampling_time txt_xc.set(xc) txt_yc.set(yc) 209 txt_theta.set(theta) client.publish("ncsntt/xc", xc) client.publish("ncsntt/yc", yc) client.publish("ncsntt/theta", theta) client.publish("ncsntt/v", velocity) client.publish("ncsntt/omega", omega) if controller_start: threading.Timer(sampling_time, controller).start() # =========================================================================== ============================================ # Get the window and set the size window = Tk() window.geometry('330x300') window.configure(bg='white') window.title('Mobile Robot Agent') window.resizable(0, 0) topFrameLeft = Frame(window, width=400, height=200, background="white") topFrameLeft.grid(row=0, column=0, sticky="NSEW") topFrameMid = Frame(window, width=400, height=300, background="white") topFrameMid.grid(row=1, column=0, sticky="NSEW") topFrameRight = Frame(window, width=400, height=300, background="white") topFrameRight.grid(row=2, column=0, sticky="NSEW") BotFrameLeft = Frame(window, width=800, height=400, background="white") BotFrameLeft.grid(rowspan=3, row=0, column=1, sticky="NSEW") # =========================================================================== ============================================ Label(topFrameLeft, text="Broker Address", bg='white').grid(row=0, column=0, sticky="W") # txt_brokeraddress = StringVar() et_brokeraddress = Entry(topFrameLeft, textvariable=txt_brokeraddress, width=40) et_brokeraddress.grid(row=0, column=1, sticky="NSEW", columnspan=2) et_brokeraddress.insert(0, "broker.mqttdashboard.com") # Label(topFrameLeft, text="Port", bg='white').grid(row=1, column=0, sticky="W") # txt_port = StringVar() et_port = Entry(topFrameLeft, textvariable=txt_port) et_port.insert(0, "1883") et_port.grid(row=1, column=1, sticky="NSEW") pb_connect = Button(topFrameLeft, text='Connect', command=MQTT_connect, width=10) pb_connect.grid(row=1, column=2, sticky="NSEW") Output = Text(topFrameLeft, bg="white", width=30, height=5) Output.grid(row=5, column=0, columnspan=3, sticky="NSEW") Input_txt = Text(topFrameLeft, bg="white", width=30, height=5) Input_txt.grid(row=6, column=0, columnspan=3, sticky="NSEW") # 210 =========================================================================== ========================================= Label(topFrameMid, text="xc", bg='white').grid(row=1, column=0, sticky=W) txt_xc = StringVar() Entry(topFrameMid, textvariable=txt_xc, bg='white', width=10).grid(row=1, column=1, sticky=W) Label(topFrameMid, text="yc", bg='white').grid(row=1, column=2, sticky=W) txt_yc = StringVar() Entry(topFrameMid, textvariable=txt_yc, bg='white', width=10).grid(row=1, column=3, sticky=W) Label(topFrameMid, text="V_ar", bg='white').grid(row=3, column=2, sticky=W) txt_v_a_r = StringVar() Entry(topFrameMid, textvariable=txt_v_a_r, bg='white', width=10).grid(row=3, column=3, sticky=W) Label(topFrameMid, text="theta", bg='white').grid(row=1, column=4, sticky=W) txt_theta = StringVar() Entry(topFrameMid, textvariable=txt_theta, bg='white', width=10).grid(row=1, column=5, sticky=W) Label(topFrameMid, text="V_al", bg='white').grid(row=3, column=4, sticky=W) txt_v_a_l = StringVar() Entry(topFrameMid, textvariable=txt_v_a_l, bg='white', width=10).grid(row=3, column=5, sticky=W) # =========================================================================== ============================================ Output.insert(END, "creating new instance\n") client = mqtt.Client() # create new instance client.on_message = on_message # attach function to callback # =========================================================================== ============================================ window.mainloop() A.11 Chương trình phần mềm điều khiển giao diện điều khiển trung tâm import paho.mqtt.client as mqtt # import the client1 from tkinter import * # import everything from tkinter import threading import time import math from ping3 import ping, verbose_ping from matplotlib.backends.backend_tkagg import FigureCanvasTkAgg import matplotlib.pyplot as plt import matplotlib.animation as animation import numpy as np # Get the window and set the size i = count = 211 controller_start = index = t_time = t_time_vector = np.array([0]) start_flag = # -xs = [] ys = [] xc = xc_vector = np.array([0]) yc = yc_vector = np.array([0]) theta = theta_vector = np.array([0]) v_a_r = v_a_l = R_curve = omega_r = 0.1 xr_vector = np.array([0]) yr_vector = np.array([0]) v_r = omega_r*R_curve k_1 = 1.3 k_2 = 10 k_3 = # z =[[0],[0]] z_vector = np.array([[0],[0]]) u_vector = np.array([[0],[0]]) # A_m = np.array([[-2,0], [0,-2]]) B_m = np.array([[2,0], [0,2]]) P = np.array([[0.001,0], [0,0.001]]) Gamma1 = np.array([[0.025,0.001], [0.02,0.02]]) Gamma2 = np.array([[0.025,0.005], [0.02,0.02]]) signB =np.array([[1, 1], [1, -1]]) # v = omega = v_prev = omega_prev = v_kine = omega_kine = v_vector = np.array([0]) omega_vector = np.array([0]) z_d_vector = np.array([[0],[0]]) z_d_dot_vector = np.array([[0],[0]]) z_d_dot_int = [[0],[0]] z_d_dot_int_vector = np.array([[0],[0]]) z_m_vector = np.array([[0],[0]]) e_vector = np.array([[0],[0]]) 212 z_a_vector = np.array([[0],[0]]) #xi_vector = np.array([[0,0],[0,0]]) xi_vector = np.array([[-3.6263204, 5.06134608], [-7.07102245, 9.86729906]]) delta_u_vector = np.array([[0],[0]]) e_bar_vector = np.array([[0],[0]]) #sigma_vector = np.array([[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]]) sigma_vector = np.array([[ 2.39754952e-05, -8.21909355e-06], [ 2.37460681e-05, -8.14044311e-06], [ 2.44209768e-05, -8.37181010e-06], [ 2.43650760e-05, -8.35264662e-06], [ 2.41273842e-09, -8.27116295e-10], [-1.62932834e-08, 5.58553719e-09]]) sigma_dot_vector = np.array([[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]]) Theta_vector = np.array([[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]]) Theta_vector = np.array([[ 1.40579879, 1.096811 ], [ 0.93451648, 1.52663213], [ 3.9457818, 2.85604037], [ 1.02659284, -0.21302612], [ 0.25371005, 0.03547825], [-0.22807733, -0.05277862]]) #varsigma_vector = np.array([[0,0],[0,0]]) varsigma_vector = np.array([[0.00021128, 0.00015149], [0.00047609, 0.00034136]]) varsigma_dot_vector = np.array([[0,0],[0,0]]) def controller(): global start_flag, index, t_time, xc, xc_vector, yc, yc_vector, theta, theta_vector, xr , xr_vector, yr, yr_vector, theta_r, v_r, omega_r, R_curve, q_e, q_c, q_r, v_vector, omega_vector, v_kine, omega_kine global z, z_vector, z_d, z_d_vector, z_d_dot_vector, z_d_dot_int, z_d_dot_int_vector, z_m_vector, e_vector, z_a_vector, xi_vector, delta_u_vector,e_bar_vector global sigma_vector,sigma_dot_vector, Theta_vector, varsigma_vector, varsigma_dot_vector, u_vector, t_time_vector global v, omega # tau = float(txt_time_delay.get()) sampling_time = float(txt_sampling.get()) p = round(tau / sampling_time) #Reference curve t_time = t_time + sampling_time t_time_vector = np.append(t_time_vector, t_time) xr = R_curve*math.sin(omega_r*t_time) yr = -R_curve*math.cos(omega_r*t_time) theta_r = omega_r*t_time xr_vector = np.append(xr_vector, xr) yr_vector = np.append(yr_vector, yr) q_r = [[xr], [yr], [theta_r]] # v_vector = np.append(v_vector,v) omega_vector = np.append(omega_vector,omega) z_vector = np.append(z_vector,[[v],[omega]], axis = 1) xc_vector = np.append(xc_vector, xc) yc_vector = np.append(yc_vector, yc) theta_vector = np.append(theta_vector, theta) 213 q_c = [[xc], [yc], [theta]] #Calcualte error q_e = np.subtract(q_r, q_c) Trans = [[math.cos(theta), math.sin(theta), 0], [-math.sin(theta), math.cos(theta), 0], [0, 0, 1]] e_q = np.dot(Trans,q_e) #Kinematic controller v_kine = v_r * math.cos(e_q.item(2))+ k_1*e_q.item(0) omega_kine = omega_r + k_2 * v_r * e_q.item(1) + k_3 * v_r * math.sin(e_q.item(2)) #========================================================================== ================================================== # Virtual input z_d = np.array([[v_kine], [omega_kine]]) z_d_vector = np.append(z_d_vector,z_d, axis=1) z_d_dot = np.subtract(z_d_vector[:,[-1]],z_d_vector[:,[2]])/sampling_time z_d_dot_vector = np.append(z_d_dot_vector, z_d_dot, axis = 1) # Modified reference system for j in range(1, p): if (len(z_d_dot_vector)-1-j) >= 0: z_d_dot_int = z_d_dot_int + z_d_dot_vector[:,[-1-j]] * sampling_time else: z_d_dot_int = z_d_dot_int z_d_dot_int_vector = np.append(z_d_dot_int_vector, z_d_dot_int, axis=1) if (len(z_d_vector)-1-p) >=0: z_m = z_m_vector[:,[-1]] + (np.dot(A_m, z_m_vector[:,[-1]]) + np.dot(B_m, z_d_vector[:,[-1-p]]) + z_d_dot_vector[:,[-1]] + np.dot(B_m,z_d_dot_int))*sampling_time else: z_m = z_m_vector[:,[-1]] + (np.dot(A_m, z_m_vector[:,[-1]]) + z_d_dot_vector[:,[-1]] + np.dot(B_m,z_d_dot_int)) * sampling_time z_m_vector = np.append(z_m_vector, z_m, axis=1) #Tracking error e = z - z_m_vector[:,[-1]] e_vector = np.append(e_vector,e,axis=1) #Auxiliary system z_a = z_a_vector[:,[-1]] + (np.dot(A_m,z_a_vector[:,[-1]]) + np.dot(np.dot(B_m,xi_vector[:,[-2,-1]]),delta_u_vector[:,[1]]))*sampling_time z_a_vector = np.append(z_a_vector,z_a,axis=1) #New tracking error e_bar = e + z_a e_bar_vector = np.append(e_bar_vector,e_bar,axis=1) #Saturation if (max(z_d_dot)>=100) or min(z_d_dot) = 0: Zc = np.append(np.append(z, (z_d_vector[:,[-1-p]] + z_d_dot_int), 214 axis=0), z_d_dot_tilde, axis=0) else: Zc = np.append(np.append(z, z_d_dot_int, axis=0), z_d_dot_tilde, axis=0) sigma = np.dot(np.dot(np.dot(np.dot(Zc,e_bar.T),P),signB),Gamma1) sigma_vector = np.append(sigma_vector,sigma, axis = 1) sigma_dot = (sigma_vector[:,[-2,-1]] - sigma_vector[:,[-4,3]])/sampling_time sigma_dot_vector = np.append(sigma_dot_vector,sigma_dot,axis=1) varsigma = np.dot(np.dot(np.dot(np.dot(Gamma2,B_m.T),P),e_bar),delta_u_vector[:,[1]].T) varsigma_vector = np.append(varsigma_vector,varsigma,axis=1) varsigma_dot = (varsigma_vector[:,[-2,-1]] - varsigma_vector[:,[-4,3]])/sampling_time varsigma_dot_vector = np.append(varsigma_dot_vector,varsigma_dot,axis=1) if (len(sigma_vector)-2-p) >=0: Theta = Theta_vector[:,[-2,-1]] - (sigma + sigma_dot + sigma_vector[:,[-2-p,-1-p]])*sampling_time else: Theta = Theta_vector[:, [-2, -1]] - (sigma + sigma_dot) * sampling_time Theta_vector = np.append(Theta_vector, Theta, axis=1) if (len(varsigma_vector)-2-p) >=0: xi = xi_vector[:,[-2,-1]] - (varsigma + varsigma_dot + varsigma_dot_vector[:,[-2-p,-1-p]])*sampling_time else: xi = xi_vector[:,[-2,-1]] - (varsigma + varsigma_dot) * sampling_time xi_vector = np.append(xi_vector, xi, axis=1) #Control input u = np.dot(Theta_vector[:,[-2,-1]].T,Zc) u_vector = np.append(u_vector,u,axis=1) #Calculate Delta_u if (len(u_vector)-2-p) >= 0: delta_u = u_vector[:,[-1]] - u_vector[:,[-2-p]] else: delta_u = u_vector[:,[-1]] delta_u_vector = np.append(delta_u_vector,delta_u, axis=1) # txt_time_total.set(t_time_vector[-1]) xs.append(xc) ys.append(yc) #Send to Broker client.publish("ncsntt/command_v_a_l", u.item(0)) client.publish("ncsntt/command_v_a_r", u.item(1)) if start_flag == 1: threading.Timer(sampling_time, controller).start() def Start_button(): global start_flag start_flag = controller() 215 def Stop_button(): global start_flag, t_time_vector, z_m_vector, z_d_vector start_flag = client.disconnect() plt.figure(2) plt.plot(t_time_vector, z_m_vector[0,:], '-b',label='z_m(1)') plt.plot(t_time_vector, z_d_vector[0,:], ' r', label='z_d(1)') plt.plot(t_time_vector, z_vector[0, :], '-.k', label='z_d(1)') plt.xlabel('Thời gian (s)') plt.ylabel('Đáp ứng động lực học') plt.grid(color='k', linestyle=' ', linewidth=1) plt.legend(framealpha=1, frameon=True) plt.figure(3) plt.plot(t_time_vector, z_m_vector[1,:], '-b',label='z_m(2)') plt.plot(t_time_vector, z_d_vector[1,:], ' r', label='z_d(2)') plt.plot(t_time_vector, z_vector[1, :], '-.k', label='z_d(2)') plt.xlabel('Thời gian (giây)') plt.ylabel('Đáp ứng động lực học') plt.grid(color='k', linestyle=' ', linewidth=1) plt.legend(framealpha=1, frameon=True) plt.figure(4) plt.plot(t_time_vector, xc_vector, '-b',label='x_c') plt.plot(t_time_vector, xr_vector, ' r', label='x_r') plt.xlabel('Thời gian (giây)') plt.ylabel('Vị trí theo phương X') plt.grid(color='k', linestyle=' ', linewidth=1) plt.legend(framealpha=1, frameon=True) plt.figure(5) plt.plot(t_time_vector, yc_vector, '-b',label='y_c') plt.plot(t_time_vector, yr_vector, ' r', label='y_r') plt.xlabel('Thời gian (giây)') plt.ylabel('Vị trí theo phương Y') plt.grid(color='k', linestyle=' ', linewidth=1) plt.legend(framealpha=1, frameon=True) plt.figure(6) plt.plot(t_time_vector, u_vector[0,:], '-b',label='u(1)') plt.plot(t_time_vector, u_vector[1,:], ' r', label='u(2)') plt.xlabel('Thời gian (giây)') plt.ylabel('Tín hiệu điều khiển') plt.grid(color='k', linestyle=' ', linewidth=1) plt.legend(framealpha=1, frameon=True) plt.show() def update(i): line.set_data(xs,ys) return line, def MQTT_connect(): global controller_start, xc, yc, theta if controller_start == 0: broker_address = txt_brokeraddress.get() broker_port = int(txt_port.get()) Output.insert(END, "connecting to broker\n") client.connect(broker_address, port=broker_port, keepalive=60) Output.insert(END, "Subscribing to ncsntt/xc\n") client.subscribe("ncsntt/xc") Output.insert(END, "Subscribing to ncsntt/yc\n") 216 client.subscribe("ncsntt/yc") Output.insert(END, "Subscribing to ncsntt/theta\n") client.subscribe("ncsntt/theta") Output.insert(END, "Subscribing to ncsntt/v\n") client.subscribe("ncsntt/v") Output.insert(END, "Subscribing to ncsntt/omega\n") client.subscribe("ncsntt/omega") pb_connect.config(text="Connecting ") controller_start = else: controller_start = pb_connect.config(text="Connect") Output.delete(1.0, END) client.disconnect() client.loop_start() # ===================================================# def on_message(client, userdata, message): global count, controller_start, xc, yc, theta, xs, ys, v, omega msg = str(message.payload) Input_txt.insert(END, message.topic + " = " + msg + "\n") value = float(message.payload) if message.topic == "ncsntt/xc": xc = round(value, 3) txt_xc.set(xc) if message.topic == "ncsntt/yc": yc = round(value, 3) txt_yc.set(yc) if message.topic == "ncsntt/theta": theta = round(value, 3) txt_theta.set(theta) if message.topic == "ncsntt/v": v = round(value, 3) txt_v.set(v) if message.topic == "ncsntt/omega": omega = round(value, 3) txt_omega.set(omega) xs.append(xc) ys.append(yc) #========================================================================== ============================================= window = Tk() window.geometry('1000x700') window.configure(bg='white') window.title('Central Controller @N T Trung - Source') window.resizable(0, 0) topFrame = Frame(window, width=200, height=800, background="white") topFrame.grid(row=0, column=0, sticky="NSEW") bottomFrame = Frame(window, width=1000, height=800, background="white") bottomFrame.grid(row=0, column=1, sticky="NSEW") # ROW =========================================================================== ===================================== 217 Label(topFrame, text="Broker Address",font = ('Times', 12), bg='white').grid(row=0, column=0, sticky="W") # txt_brokeraddress = StringVar() et_brokeraddress = Entry(topFrame, textvariable=txt_brokeraddress, font = ('Times', 12), width=20) et_brokeraddress.grid(row=0, column=1, sticky="NSEW", columnspan=2) et_brokeraddress.insert(0, "broker.mqttdashboard.com") # ROW =========================================================================== ===================================== Label(topFrame, text="Port",font = ('Times', 12), bg='white').grid(row=1, column=0, sticky="W") # txt_port = StringVar() et_port = Entry(topFrame, textvariable=txt_port, font = ('Times', 12), width=10) et_port.insert(0, "1883") et_port.grid(row=1, column=1, sticky="NSEW") # pb_connect = Button(topFrame, text='Connect',font = ('Times', 12), command=MQTT_connect, width=10) pb_connect.grid(row=1, column=2, sticky="NSEW") # ROW =========================================================================== ===================================== Output = Text(topFrame, font = ('Times', 12), bg="white", width=30, height=5) Output.grid(row=2, column=0, columnspan=3, sticky="NSEW") # ROW =========================================================================== ===================================== Input_txt = Text(topFrame, font = ('Times', 12), bg="white", width=30, height=5) Input_txt.grid(row=3, column=0, columnspan=3, sticky="NSEW") # ROW =========================================================================== ===================================== pb_start = Button(topFrame, text='Start',font = ('Times', 12), command=Start_button, width=10) pb_start.grid(row=4, column=0, sticky="NSEW") # pb_stop = Button(topFrame, text='Stop',font = ('Times', 12), command=Stop_button, width=10) pb_stop.grid(row=4, column=1, sticky="NSEW") # ROW =========================================================================== ===================================== Label(topFrame, text="xc (m)", font = ('Times', 12), bg='white').grid(row=5, column=0, sticky=W) txt_xc = StringVar() Entry(topFrame, textvariable=txt_xc, font = ('Times', 12), bg='white', width=15).grid(row=5, column=1, columnspan=1, sticky=W) # ROW =========================================================================== ===================================== Label(topFrame, text="yc (m)", font = ('Times', 12), bg='white').grid(row=6, column=0, sticky=W) txt_yc = StringVar() Entry(topFrame, textvariable=txt_yc, font = ('Times', 12), bg='white', 218 width=15).grid(row=6, column=1, columnspan=1, sticky=W) # ROW =========================================================================== ===================================== Label(topFrame, text="Theta (rad)", font = ('Times', 12), bg='white').grid(row=7, column=0, sticky=W) txt_theta = StringVar() Entry(topFrame, textvariable=txt_theta, font = ('Times', 12), bg='white', width=15).grid(row=7, column=1, columnspan=1, sticky=W) # ROW =========================================================================== ===================================== Label(topFrame, text="v (m/s)", font = ('Times', 12), bg='white').grid(row=8, column=0, sticky=W) txt_v = StringVar() Entry(topFrame, textvariable=txt_v, font = ('Times', 12), bg='white', width=15).grid(row=8, column=1, columnspan=1, sticky=W) # ROW =========================================================================== ===================================== Label(topFrame, text="omega (rad/s)", font = ('Times', 12), bg='white').grid(row=9, column=0, sticky=W) txt_omega = StringVar() Entry(topFrame, textvariable=txt_omega, font = ('Times', 12), bg='white', width=15).grid(row=9, column=1, columnspan=1, sticky=W) # ROW 10 =========================================================================== ===================================== Label(topFrame, text="Time delay (s)", font = ('Times', 12), bg='white').grid(row=10, column=0, sticky=W) txt_time_delay = StringVar() time_delay = Entry(topFrame, textvariable=txt_time_delay, font = ('Times', 12), bg='white', width=15) time_delay.insert(0,"1") time_delay.grid(row=10, column=1, columnspan=1, sticky=W) # ROW 11 =========================================================================== ===================================== Label(topFrame, text="Sampling (s)", font = ('Times', 12), bg='white').grid(row=11, column=0, sticky=W) txt_sampling = StringVar() sampling = Entry(topFrame, textvariable=txt_sampling, font = ('Times', 12), bg='white', width=15) sampling.insert(0,"0.1") sampling.grid(row=11, column=1, columnspan=1, sticky=W) # ROW 12 =========================================================================== ===================================== Label(topFrame, text="Time total (s)", font = ('Times', 12), bg='white').grid(row=12, column=0, sticky=W) txt_time_total = StringVar() Entry(topFrame, textvariable=txt_time_total, font = ('Times', 12), bg='white', width=15).grid(row=12, column=1, columnspan=1, sticky=W) #========================================================================== ============================================ Output.insert(END, "creating new instance\n") client = mqtt.Client() # create new instance client.on_message = on_message # attach function to callback fig = plt.figure(figsize=[7, 7]) 219 ax = plt.axes(xlim=(-2, 2), ylim=(-2, 2)) plt.xlabel('X (m)') plt.ylabel('Y (m)') plt.grid(color ='k', linestyle=' ', linewidth = 1) line, = ax.plot(xs, ys, 'b,-') # Set up plot to call animate() function periodically graph = FigureCanvasTkAgg(fig, master=bottomFrame) graph.get_tk_widget().grid() graph.draw() ani = animation.FuncAnimation(fig, update, interval=1, blit=True) window.mainloop() 220
Ngày đăng: 25/04/2023, 16:01
Xem thêm:
