BỘ GIÁO DỤC VÀ ĐÀO TẠO
TRƯỜNG ĐẠI HỌC SƯ PHẠM KỸ THUẬT TP. HỒ CHÍ MINH
KHOA ĐIỆN – ĐIỆN TỬ
BỘ MÔN TỰ ĐỘNG ĐIỀU KHIỂN


ĐỒ ÁN TỐT NGHIỆP
THIẾT KẾ VÀ LẬP TRÌNH ROBOT TỰ HÀNH
KẾT HỢP MANG CÁNH TAY ROBOT 6 BẬC TỰ DO
ỨNG DỤNG ROBOT OPERATING SYSTEM
VÀ DEPTH CAMERA

GVHD: TS. Nguyễn Văn Thái
SVTH

MSSV

Huỳnh Quốc Toàn

20151160

TP. Hồ Chí Minh, tháng 6 năm 2024

LỜI CẢM ƠN
Em xin bày tỏ lòng biết ơn chân thành tới các cá nhân, tổ chức sau đây đã đóng vai trò
quan trọng cũng như sự hỗ trợ quý báu cho em trong quá trình làm luận văn tốt nghiệp:
• DỰ ÁN NÀY ĐƯỢC TÀI TRỢ MỘT PHẦN THIẾT BỊ BỞI CÔNG TY I&ampC
ENGINEERING SERVICE CO.,LTD
• Quý Thầy, Cô của trường Đại Học Sư Phạm Kỹ Thuật Thành phố Hồ Chí Minh,
đặc biệt là các Thầy, Cô khoa Điện – Điện tử của trường đã tạo điều kiện cho em
học tập ở khoa.
• Em xin chân thành cảm ơn Thầy – TS. Nguyễn Văn Thái đã nhiệt tình hướng
dẫn, tạo điều kiện và dành thời gian theo sát em để hoàn thành đề tài này.
• I&ampC ENGINEERING SERVICE CO.,LTD, cảm ơn sự tài trợ từ Công ty – anh
Tạ Quang Anh đã tạo điều kiện về phần cứng, môi trường học tập cho em có thể
nghiên cứu dự án này.
• Cảm ơn những anh, chị tại Công ty I&ampC Engineering Service Co.,LD. Cũng như
anh Bùi Lê Anh đã tận tình hỗ trợ em trong dự án này.
• Xin chân thành cảm ơn những người bạn cùng khóa đã ở bên cạnh giúp đỡ trong
những ngày tháng khó khăn trong quá trình học tập và khi làm đồ án tốt nghiệp.
• Con xin cảm ơn Ba, Mẹ và gia đình đã giúp đỡ về tài chính và động viên tinh
thần trong lúc khó khăn, tạo điều kiện cho con hoàn thành đồ án tốt nghiệp này.
Em xin chân thành cảm ơn những sự hỗ trợ, hướng dẫn và đóng góp của các cá nhân, tổ
chức trên. Những sự hỗ trợ của mọi người đã thúc đẩy em thực hiện và hoàn thiện đồ án
tốt nghiệp này.
Do trình độ lý luận cũng như kinh nghiệm thực tiễn còn hạn chế nên bài báo cáo khó
tránh khỏi những sai sót, em rất mong nhận được ý kiến đóng góp từ quý Thầy để em
học thêm được nhiều kinh nghiệm và hoàn thiện hơn trong tương lai.
Trường Đại Học Sư Phạm Kỹ Thuật Thành phố Hồ Chí Minh
Khoa Điện – Điện tử
Ngành CNKT Điều khiển và Tự động hóa
Đồ án tốt nghiệp. Học kỳ II. Năm học: 2023 – 2024
Tp. Hồ Chí Minh, tháng 6 năm 2024
Em xin chân thành cảm ơn!
i

LỜI CAM ĐOAN
Em xin cam đoan đề tài “Thiết kế và lập trình Robot tự hành kết hợp mang cánh tay
Robot 6 bậc tự do ứng dụng Robot Operating System và Depth Camera.” là em tự thực
hiện, dựa vào tham khảo một số tài liệu trước đó, dưới sự hỗ trợ giúp đỡ của Thầy – TS
...

...
(rgb_image,
ARUCO_DICT[aruco_type], intrinsic_camera, distortion)
show_resized_image(output, Pose Estimation)
for center, marker_id, rvec, tvec in zip(centers, marker_ids, rvecs, tvecs):
y_marker, x_marker = center
print(fID: {marker_id}, X: {x_marker}, Y: {y_marker}, rvec: {rvec}, tvec:
{tvec})
r_matrix, jacobian = cv2.Rodrigues(rvec)
print(rotation matrix:\\n, r_matrix)
depth_image = np.asarray(rgbd_image.depth)
height, width = depth_image.shape
center_depth = depth_image[x_marker, y_marker]
depth_in_milimet = center_depth
depth_normalized = cv2.normalize(depth_image, None, 0, 200,
cv2.NORM_MINMAX, dtype=cv2.CV_8U)
depth_rgb_image = cv2.applyColorMap(depth_normalized, cv2.COLORMAP_JET)
cv2.circle(

145

PHỤ LỤC
Hàm điều chỉnh tư thế - Pose Adjustment
while not rospy.is_shutdown():
if aruco_detect_node._doJob == 3 and aruco_detect_node._doneJob == 0:
print(tvecs)
rospy.logwarn(JOB UTE)
_frames = kinect.listener.NewFrame()
_color = _frames[color]
_ir = _frames[ir]
_depth = _frames[depth]
kinect.registration.apply(_color, _depth, kinect.undistored,
kinect.registered,
bigdepth=kinect.bigdepth,
color_depth_map=kinect.color_depth_map)
_color = cv2.resize(_color.asarray(), (int(1280), int(720)),
interpolation=cv2.INTER_CUBIC)
output, centers, marker_ids, rvecs, tvecs, _pitch, _roll, _yaw,
_realWorld = _aruco._poseEstimation(_color, kinect.intrinsic_camera,
kinect.distortion)
if len(tvecs) 0:
tvec_str = x = %0.3f z = %0.3f direction = %0.3f %(tvecs[0]*100,
tvecs[2]*100, math.degrees(_roll))
cv2.putText(_color, tvec_str, (20,460), cv2.FONT_HERSHEY_PLAIN, 2,
(0,0,255), 2, cv2.LINE_AA)
twist = robotAlignRoll(math.degrees(_roll), tvecs[2]*100)
velocity_l, velocity_r =
aruco_detect_node.robot.kinematic_(twist[0], twist[2])
if math.degrees(_roll) -10.0 and math.degrees(_roll) 10.0 and
tvecs[2]*100 30:
aruco_detect_node._doneJob = 1
cv2.destroyWindow(test)
else:
velocity_l, velocity_r = aruco_detect_node.robot.kinematic_(0, 0)
vl_ = aruco_detect_node.az.directComand_(velocity_l)
vr_ = aruco_detect_node.az.directComand_(velocity_r)
aruco_detect_node.client_.write_registers(vl_[0], vl_[1], slave =
0x03, skip_encode = True)
aruco_detect_node.client_.write_registers(vr_[0], vr_[1], slave =
0x02, skip_encode = True)
cv2.imshow(test, _color)
kinect.listener.release(_frames)
if aruco_detect_node._doneJob == 1:
aruco_detect_node._doJob = 0
rospy.logwarn(JOB DONE)
aruco_detect_node._doneJob = 0
rate.()

146