TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT THÀNH PHỐ HỒ CHÍ MINH
KHOA ĐÀO TẠO CHẤT LƯỢNG CAO

––––????????????????????––––

ĐỒ ÁN TỐT NGHIỆP

Đề Tài:
THIẾT KẾ MÔ HÌNH ROBOT GẮP VẬT CHUYỂN
ĐỘNG VỚI THUẬT TOÁN ĐIỀU KHIỂN VÒNG KÍN
DÙNG DỮ LIỆU HÌNH ẢNH
SVTH

: HUỲNH TẤN PHÚC

18151104

LÊ QUANG PHƯƠNG

18151106

Khóa

: 2018-2022

Ngành

: CÔNG NGHỆ KỸ THUẬT ĐIỀU KHIỂN VÀ TỰ ĐỘNG HÓA

GHVD

: PGS.TS. LÊ MỸ HÀ

Tp. Hồ Chí Minh, tháng 08 năm 2022

LỜI CẢM ƠN
Sau thời gian học tập tại trường Đại Học Sư Phạm Kỹ Thuật cùng với sự hướng dẫn
tận tình của quý Thầy/ Cô, nhóm đồ án đã hoàn thành đồ án tốt nghiệp.
Nhóm em xin gửi lời cảm ơn chân thành đến Thầy Lê Mỹ Hà đã tận tình góp ý,
hướng dẫn nhiệt tình trong quá trình nhóm thực hiện đề tài. Xin được gửi lời cảm ơn đến
Thầy được nhiều sức khỏe và thành công nhiều hơn trong quá trình giảng dạy của mình.
Nhóm em xin chân thành cảm ơn quý Thầy/ Cô khoa Điện - Điện tử đã cho nhóm
kiến thức về lĩnh vực chuyên ngành, những kinh nghiệm về quá trình làm việc nhóm và tạo
điều kiện cho nhóm thực hiện đề tài rất hay này, tạo thêm nhiều hiểu biết cho nhóm trước
khi ra trường. Do trình độ và kiến thức còn hạn chế nên đề tài có thể còn nhiều sai sót mong
Khoa - Bộ môn và quý Thầy/ Cô góp ý để đề tài nhóm thêm hoàn thiện hơn.
Nhóm em cũng xin gửi lời cảm ơn chân thành tới gia đình và bạn bè đã tạo điều
kiện và giúp đỡ nhóm rất nhiều để đạt được kết quả ngày hôm nay.

ĐỒ ÁN TỐT NGHIỆP

i

MỤC LỤC
LỜI CẢM ƠN ...................................................................................................................... i
MỤC LỤC ........................................................................................................................... ii
MỤC LỤC HÌNH ẢNH ..................................................................................................... vi
MỤC LỤC BẢNG BIỂU ................................................................................................... xi
DANH MỤC CÁC TỪ VIẾT TẮT .................................................................................. xii
CHƯƠNG 1. TỔNG QUAN ............................................................................................... 1
TÍNH CẤP THIẾT CỦA ĐỀ TÀI ............................................................................ 1
MỤC TIÊU CỦA ĐỀ TÀI ........................................................................................ 3
ĐỐI TƯỢNG VÀ PHẠM VI NGHIÊN CỨU
...

...
0, 0, 0, 1]])
T3 = np.array(
[[math.cos(math.radians(theta3)),
math.cos(math.radians(theta3))],
[math.sin(math.radians(theta3)),
math.sin(math.radians(theta3))],
[0, 0, 1, 0],
[0, 0, 0, 1]])
T = np.dot(T1, T2)
T = np.dot(T, T3)
pX = T[0][3]
pY = T[1][3]
pZ = T[2][3]
Code động học nghịch:
th1_inv = 0
nx = PX * math.cos(math.radians(th1_inv)) + PY * math.sin(math.radians(th1_inv)) - L1
ny = PY * math.cos(math.radians(th1_inv)) - PX * math.sin(math.radians(th1_inv))
M = (nx * nx + ny * ny - L2 * L2 - L3 * L3) / (2 * (L2 * L3))
N = math.sqrt(abs(1 - M * M))
th3_inv = np.arctan2(N, M) * (180 / math.pi)
th3_inv_1 = %.2f % th3_inv
A = ny * (L3 * math.cos(math.radians(th3_inv)) + L2) - L3 * math.sin(math.radians(th3_inv)) * nx

ĐỒ ÁN TỐT NGHIỆP

92

B = nx * (L3 * math.cos(math.radians(th3_inv)) + L2) + L3 * math.sin(math.radians(th3_inv)) * ny
th2_inv = np.arctan2(A, B) * (180 / math.pi)
th2_inv_1 = %.2f % th2_inv

Code PID:
def PID(set_point, current_point):
global kp, ki, kd, timenow, pid_i, previous_error
# PID calcuation
error = (current_point - set_point) # Centre of the video
print(error, error)
# Proportional
pid_p = kp * error
# Integral
if -500 error 500:
pid_i = pid_i + (ki * error)
print(pid_i: , pid_i)
time_previous = timenow
timenow = time.time()
elapsedTime = timenow - time_previous
print(elapsedTime: , elapsedTime)
pid_d = kd * ((error - previous_error) / elapsedTime)
print(pid_d: , pid_d)
previous_error = error
print(previous_error: , previous_error)
PID = pid_p + pid_i + pid_d
print(PID: , PID)

ĐỒ ÁN TỐT NGHIỆP

93

delta = PID
else:
PID = 30
delta = PID
return delta
Code dự đoán quỹ đạo:
def predit(position_object):
global previous_poision, timenow_predit, position_current
time_previous_predit = timenow_predit
timenow_predit = time.time()
elapsed_Time_predit = timenow_predit - time_previous_predit
if(elapsed_Time_predit2):
next_time = elapsed_Time_predit
else:
next_time = 0.65
print(elapsed_Time: , elapsed_Time_predit)
previous_poision = position_current
position_current = position_object
elapsed_Position_predit = position_current - previous_poision
print(Vitri truoc do: , previous_poision)
print(Vitri hien tai: , position_current)
print(next_time: , next_time)
speed_object = elapsed_Position_predit / elapsed_Time_predit
poistion_future = position_current + speed_object * next_time
print(speed: , speed_object)
return poistion_future - position_current

ĐỒ ÁN TỐT NGHIỆP

94