(Luận án tiến sĩ) 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 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Ở ĐẦ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 Nội dung nghiên cứu: - 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 tố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 toá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 toá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à xưởng, bệnh viện, nhà hàng Việc điều khiển robot tự hành qua mạng giúp tiết kiệm thời gian quản lí, vận hành thực linh hoạt tác vụ Cấu trúc luận án gồm phần: Mở đầu, chương nội dung kết luận Phần mở đầu trình bày lý chọn đề tài, mục đích, đối tượng, phạm vi, giới hạn, phương pháp, nội dung nghiên cứu, ý nghĩa khoa học thực tiễn luận án Chương trình 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 tố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, nêu vấn đề tồn cần phải giải quyết, phân tích tình hình nghiên cứu cơng bố trước với hướng nghiên cứu luận án Chương trình bày việc xây dựng hệ thống điều khiển robot tự hành qua mạng, xây dựng mơ hình hóa cho robot tự hành với thời gian trễ Chương trình bày việc thiết kế điều khiển bám theo quỹ đạo dựa sở động học động lực học robot tự hành Trong đó, tác giả trình bày cấu trúc hệ thống tham chiếu mới, hiệu so với hệ thống tham chiếu truyền thống; bước thiết kế điều khiển thích nghi sử dụng hệ thống tham chiếu thơng số độ lợi đề xuất Chương trình bày cấu trúc điều khiển robot tự hành, thiết lập mô kết mô để đánh giá hiệu điều khiển đề xuất cho robot tự hành thông qua kết so sánh với điều khiển thích nghi theo hệ thống tham chiếu truyền thống Phần kết luận tổng hợp lại kết luận án, thảo luận ưu – nhược điểm điều khiển đề xuất, từ đề hướng phát triển tiềm cho nghiên cứu CHƯƠNG 1: TỔNG QUAN VỀ HỆ THỐNG ĐIỀU KHIỂN ROBOT TỰ HÀNH QUA MẠNG Trong chương này, tác giả trình bày nội dung sau: - Cấu trúc thành phần chức hệ thống vật lí ảo cho hệ thống kho vận nội môi trường công nghiệp - Giới thiệu hệ thống điều khiển robot tự hành qua mạng, trình bày vấn đề cần phải giải đối tượng nghiên cứu - Tình hình nghiên cứu giới khoa học dành cho vấn đề đặt - Trên sở đó, tác giả đề xuất nội dung nghiên cứu phương pháp tiến hành thực luận án 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 Sự phát triển công nghệ dẫn đến gia tăng nhu cầu cải tiến cho lĩnh vực sản xuất công nghiệp Các doanh nghiệp mong đợi hoạt động sản xuất họ không tăng hiệu suất, suất mà đa dạng qui mô [14] Để đạt mục tiêu này, hệ thống sản xuất phải hoạt động cách linh hoạt dễ dàng quản lí [41] Cách mạng công nghiệp 4.0 hệ sản xuất công nghiệp với hỗ trợ công nghệ internet [95] để tối ưu hệ thống sản xuất tự động, giúp hệ thống sản xuất hoạt động hiệu quả, tối thiểu hóa thời gian dừng hệ thống cố, phụ thuộc vào hoạt động người hệ thống có khả tự đưa định [94], tạo giới mà hệ thống vật lý ảo chuỗi sản xuất toàn cầu hợp tác với cách linh hoạt Mục tiêu cách mạng công nghiệp 4.0 áp dụng lĩnh vực sản xuất biến nhà máy sản xuất trở nên thông minh [3], cho phép sản phẩm tạo từ nhà máy định u cầu cải tiến cho chu kỳ sản xuất Điều thực cách sử dụng liệu từ cảm biến đo trạng thái vật lí hệ thống sản xuất để tạo hệ thống ảo cho phép theo dõi điều khiển hệ thống [92] Khái niệm hệ thống biết hệ thống vật lí ảo (cyber physical system) [24][32][39][60][84][91] Theo đó, hệ thống vật lí ảo Hình 1.1 tích hợp nhiều công nghệ khác như: hệ thống nhúng, mạng cảm biến không dây, hệ thống điều khiển qua mạng, hệ thống máy tính xử lí tốc độ cao, internet vạn vật hệ thống điều khiển công nghiệp Hệ thống vật lí ảo kết hợp cơng nghệ để phục vụ cho việc tương tác hệ thống vật lí giới thực hệ thống ảo không gian mạng internet Các cảm biến điều khiển triển khai để theo dõi điều khiển hệ thống vật lí giới thực Thế giới ảo cho phép hoạt động với khối lượng tính tốn xử lí lưu trữ lớn phân tích liệu để đưa định cho hệ thống vật lí giới thực Việc trao đổi liệu phần tử hệ thống vật lí ảo cần thiết thực thông qua kết nối internet nhằm đảm bảo liệu có khả sử dụng cho nhiều tầng quản lí, theo dõi, điều khiển Hình 1.1 Cấu trúc hệ thống vật lí ảo Mặt khác, để nâng cao hiệu sản xuất, mắc xích quan trọng cần phải cải tiến hệ thống sản xuất công nghiệp hệ thống kho vận (logistic) Trong hệ thống kho vận, đối tượng đóng vai trị quan trọng robot tự hành vận chuyển hàng hóa, nguyên vật liệu khu vực khác nhà xưởng, nhà máy [45][53][55][67][86][89] Các robot vận hành tự động, định vị - dẫn đường, điều khiển nhiều phương pháp khác Kích thước, hình dáng, cấu trúc, công suất, cấu chấp hành robot tự hành phụ thuộc vào ứng dụng cụ thể chúng nhà xưởng Robot tự hành hoạt động nhà xưởng phải có khả thực tình khác như: thay đổi quỹ đạo di chuyển, thay đổi sơ đồ bố trí quỹ đạo, thay đổi thơng số động lực học q trình di chuyển Trong sản xuất tại, robot tự hành nhà xưởng thiết bị thông minh có khả xử lí trường để đưa định phi tập trung (phân tán) cho hoạt động hoạch định quỹ đạo di chuyển tránh vật cản Tuy nhiên, toán hoạch định quỹ đạo di chuyển [5][16][20][23][64][78][88] xác định nhiệm vụ mang khối lượng tính tốn cao hệ thống vận hành với nhiều robot tự hành Nhiệm vụ thực dễ dàng điều khiển trung tâm với khả xử lí mạnh Để điều khiển robot tự hành bám theo quỹ đạo cho trước với độ xác cao, việc xác định vị trí robot tự hành mang đến cho nhà khoa học thử thách nghiên cứu Việc định vị hoạch định quỹ đạo di chuyển – công nghệ dẫn đường – cho robot tự hành môi trường nhà xưởng thực tế có nhiều khó khăn triển khai lắp đặt thiết bị nhằm giúp định vị xác vị trí robot tự hành [10][17][28][57][75] nhà xưởng tốn chi phí cao Nếu khơng thực việc xác định vị trí xác robot tự hành, việc điều khiển robot tự hành di chuyển theo quỹ đạo khơng thể thực tốt chí nguy hiểm đến hoạt động sản xuất nhà xưởng người làm việc Công nghệ dẫn đường cho robot tự hành chia thành hai nhóm: quỹ đạo di chuyển xác định quỹ đạo di chuyển tự Phương pháp dẫn đường quỹ đạo di chuyển cố định phụ thuộc thiết kế hạ tầng triển khai ban đầu điển hình với hình thức dẫn đường quỹ đạo vạch từ, vạch màu Phương pháp dẫn đường không linh hoạt cần thay đổi quỹ đạo di chuyển tiêu tốn nhiều chi phí cho việc thi công lại quỹ đạo nhà xưởng Trong 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 ... luận án Chương trình 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 tố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. .. dụng hệ thống điều khiển qua mạng giảm khối lượng tính toá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. .. thành phần hệ thống điều khiển gửi gói tin thông qua mạng Hệ thống điều khiển qua mạng nói 13 chung gồm thành phần: điều khiển, cấu chấp hành, cảm biến đường truyền mạng Khái niệm hệ thống điều