Hệ thống điều khiển robot tự hành qua mạng trong môi trường công nghiệp

BỘ GIÁO DỤC VÀ ĐÀO TẠO BỘ CÔNG THƯƠNG VIỆN NGHIÊN CỨU ĐIỆN TỬ, TIN HỌC, TỰ ĐỘNG HÓA -*** - NGUYỄN TRỌNG TRUNG HỆ THỐNG ĐIỀU KHIỂN ROBOT TỰ HÀNH QUA MẠNG TRONG MÔI TRƯỜNG CÔNG NGHIỆP CHUYÊN NGÀNH: KỸ THUẬT ĐIỆN TỬ MÃ SỐ: 9520203 LUẬN ÁN TIẾN SĨ KỸ THUẬT TP Hồ Chí Minh - 2021 LỜI CAM ĐOAN Tôi xin cam đoan nội dung luận án kết nghiên cứu riêng tác giả Tất tham khảo từ nghiên cứu liên quan nêu nguồn gốc cách rõ ràng Những kết nghiên cứu đóng góp luận án chưa cơng bố cơng trình khoa học khác Tác giả luận án Nghiên cứu sinh Nguyễn Trọng Trung i 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 MỞ ĐẦU Cuộc cách mạng công nghiệp lần thứ đánh dấu đời máy nước vào năm 1784 việc sử dụng lượng nước, nước vào giới hóa sản xuất Cuộc cách mạng cơng nghiệp lần thứ hai năm 1870 với việc sử dụng lượng điện, đời dây chuyền sản xuất hàng loạt qui mô lớn Cuộc cách mạng công nghiệp lần thứ ba năm 1969 với lan tỏa việc sử dụng công nghệ điện tử, công nghệ thông tin để đưa vào tự động hóa sản xuất Và từ năm 2015, cách mạng công nghiệp lần thứ tư (Industry 4.0) thức bắt đầu Đặc trưng Industry 4.0 kết hợp xóa bỏ ranh giới hệ thống sản xuất, quản lí quản trị với yếu tố kỹ thuật cốt lõi là: Trí tuệ nhân tạo (Artificial Intelligence – AI), kết nối vạn vật (Internet of Thing – IoT), phân tích liệu lớn (Big Data), bảo mật hệ thống chuỗi (Block Chain) Industry 4.0 tác động vào tất lĩnh vực như: robot, công nghệ sinh học, y sinh, nông nghiệp, công nghệ nano, in 3D, robot tự hành (xe ô tô robot tự hành).v.v Việc tác động Industry 4.0 đến lĩnh vực robot tự hành tạo nên hệ thống vận chuyển hàng nhà xưởng, nhà kho mang tính linh hoạt cao, tạo điều kiện thuận lợi cho hoạt động quản lí, quản trị như: Hoạch định quỹ đạo di chuyển, tối ưu hóa quỹ đạo theo tiêu chí tối thiểu lượng tiêu thụ thời gian di chuyển nhanh nhất, tránh vật cản, kiểm soát lưu lượng hàng hóa, đánh giá lực sản xuất luân chuyển hàng hóa Để thực hóa hệ thống với chức trên, robot tự hành có khả kết nối với điều khiển trung tâm từ xa nhằm chia sẻ thông tin như: vị trí tại, tác vụ hành, tình trạng hoạt động Đồng thời chúng kết nối chia sẻ thông tin với tạo thành mạng chia sẻ ngang hàng chúng với điều khiển trung tâm Với việc triển khai cấu trúc điều khiển qua mạng hệ thống robot tự hành, cho phép nhà quản lí thay đổi thơng số động học vận tốc, quỹ đạo tác vụ robot cách linh hoạt, nhanh chóng Tuy nhiên, hệ thống tập trung nhiều thiết bị với số lượng thông tin chia sẻ lớn, nên gây tổn hao băng thông đường truyền, điều phát sinh hai vấn đề: Mất gói liệu đường truyền thời gian trễ sinh đường truyền Việc gói liệu đường truyền giải giao thức truyền với chế quản lí chất lượng dịch vụ (Quality of Servive – QoS) kiểm sốt tổng số gói tin, u cầu gửi bù gói tin bị thiếu, gửi lại gói tin bị thiếu Tuy nhiên, thời gian trễ sinh đường truyền ngồi yếu tố băng thơng, cịn ngun nhân tốc độ đường truyền, chế QoS, thời gian xử lí thiết bị đầu cuối Mặt khác, thời gian trễ đường truyền làm giảm chất lượng điều khiển hệ thống robot tự hành chí làm tính ổn định hệ thống Do vậy, việc xây dựng hệ thống điều khiển robot tự hành qua mạng nghiên cứu giải quyết, khắc phục vấn đề thời gian trễ đường truyền nhằm giảm ảnh hưởng đến chất lượng điều khiển hệ thống điều cần thiết mang tính khoa học thực tiễn Luận án đặt vấn đề nghiên cứu sau: Mục tiêu nghiên cứu: Tìm giải pháp dựa sở điều khiển, ước lượng với thông tin chia sẻ hệ thống điều khiển robot tự hành qua mạng nhằm giảm ảnh hưởng yếu tố thời gian trễ đến chất lượng điều khiển hệ thống, đảm bảo tính ổn định cho hệ thống hoạt động môi trường công nghiệp với độ tin cậy phạm vi cho phép Đối tượng nghiên cứu: Hệ thống điều khiển robot tự hành với phương thức giao tiếp qua mạng không dây (Internet) Phạm vi nghiên cứu: Luận án đề xuất điều khiển phát triển dựa mơ hình hóa động học động lực học robot tự hành đồng thời kết hợp thuật toán bám theo quỹ đạo, thích nghi theo hệ thống tham chiếu, kỹ thuật thiết kế hệ thống đệ quy, nhằm làm giảm ảnh hưởng thời gian trễ đánh giá hiệu suất hoạt động robot tự hành mô tạo tiền đề cho thực nghiệm Giới hạn luận án: Robot tự hành hệ thống phức hợp nhiều toán gồm điều khiển bám theo quỹ đạo, điều khiển bám theo quỹ đạo đảm bảo yếu tố động lực học, quản lí lượng, định vị môi trường nhà Luận án giới hạn việc xây dựng điều khiển bám theo quỹ đạo diện thời gian trễ thông số hệ thống xác định xác - Luận án khơng tập trung phát triển thuật tốn xác định vị trí cho robot tự hành mà sử dụng thuật tốn truyền thống dead-reckoning, cịn gọi phương pháp odometry - Luận án không tập trung phát triển giải thuật hoạch định tối ưu hóa quỹ đạo cho robot tự hành Phương pháp nghiên cứu: Phân tích thuật tốn lý thuyết đề xuất, nghiên cứu kết hợp thuật toán để tạo thành điều khiển tích hợp nhằm giải vấn đề tồn hệ thống - Xây dựng cấu trúc hệ thống hệ thống điều khiển robot tự hành qua mạng, nghiên cứu phương thức truyền liệu - Nghiên cứu phương pháp thực nghiệm xác định thời gian trễ đường truyền - Nghiên cứu phương pháp ước lượng biến trạng thái mơ hình hóa robot tự hành với thời gian trễ Thiết kế đánh giá điều khiển tích hợp nhiều thuật toán cho robot tự hành nhằm giảm ảnh hưởng thời gian trễ thông số không xác định robot tự hành đến chất lượng điều khiển Đóng góp ý nghĩa khoa học luận án: Luận án đặt hai vấn đề cần phải giải bao gồm: - Cải thiện chất lượng điều khiển robot tự hành qua mạng thời gian trễ sinh đường truyền, thời gian xử lí liệu, đáp ứng trễ cấu chấp hành Cải thiện chất lượng điều khiển robot tự hành ảnh hưởng thông số mơ hình, khơng thể xác định xác, đến q trình xây dựng thuật tốn điều khiển Để giải hai vấn đề trên, luận án đề xuất giải pháp xây dựng điều khiển tích hợp cho robot tự hành điều kiện có thời gian trễ, mơ tả sau: - Tác giả đề xuất điều khiển tích hợp hai kỹ thuật gồm thiết kế điều khiển cho hệ thống có cấu trúc đệ quy, thích nghi theo hệ thống tham chiếu mơ hình ổn định cho robot tự hành - Kỹ thuật thiết kế điều khiển cho hệ thống có cấu trúc đệ quy sử dụng để phát triển thuật tốn điều khiển cho robot tự hành với mơ hình hóa động học động lực học Tín hiệu điều khiển mơ hình động học tín hiệu đầu vào mong muốn mơ hình động lực học - Kỹ thuật thích nghi theo hệ thống tham chiếu mơ hình ổn định sử dụng giải hai yếu tố thời gian trễ thông số mô hình khơng xác Trong đó, tác giả đề xuất hệ thống tham chiếu có cấu trúc mới, khác với cơng bố trước đây, đáp ứng có khả bám theo tín hiệu đầu vào mong muốn, biến thiên theo thời gian có thời gian trễ - Ngồi ra, tác giả cịn đề xuất thơng số độ lợi, cho tín hiệu điều khiển, có khả cập nhật theo thời gian sử dụng hồi tiếp trạng thái tín hiệu đầu vào trễ, giá trị tích lũy đạo hàm tín hiệu đầu vào trễ, để khắc phục sai lệch tín hiệu điều khiển tín hiệu điều khiển trễ nhằm trì ổn định cho điều khiển robot tự hành Ý nghĩa thực tiễn luận án Kết nghiên cứu cho phép áp dụng vào điều khiển trung tâm qua mạng robot tự hành ứng dụng nhiều lĩnh vực như: Nhà kho, nhà 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, [-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]]) [-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", client.publish("ncsntt/command_v_a_r", if start_flag == 1: u.item(0)) u.item(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 ... bày tổng quan hệ thống điều khiển robot tự hành qua mạng gồm cấu trúc hệ thống vật lí ảo cho toán tổng thể sử dụng robot tự hành môi trường công nghiệp, giới thiệu hệ thống điều khiển qua mạng, ... 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. .. dụng hệ thống điều khiển qua mạng giảm khối lượng tính tốn robot tự hành, tiết kiệm lượng pin tiêu thụ robot 23 Hình 1.16 Cấu trúc hệ thống điều khiển robot tự hành qua mạng Về mặt cấu trúc hệ thống