Đồ án tốt nghiệp - Điều khiển cân bằng và giữ vị trí cho hệ Quadcopter

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

KHÓA LUẬN TỐT NGHIỆP
Đề tài :

ĐIỀU KHIỂN CÂN BẰNG VÀ GIỮ VỊ TRÍ CHO HỆ
QUADCOPTER

GVHD
SVTH
MSSV
Lớp
Chuyên ngành
Hệ đào tạo
Khóa đào tạo

: PGS.TS. NGUYỄN MINH TÂM
: VÕ ANH KHOA
: 16151036
: 16151CL3
: CNKT Điều khiển và Tự động hóa
: Chính Quy
: 2016 - 2020.

TP. Hồ Chí Minh, tháng 08 năm 2020

Khóa Luận Tốt Nghiệp

Lời Cảm Ơn
Để luận văn này đạt kết quả tốt đẹp, tôi đã nhận được sự hỗ trợ, giúp đỡ của các Quý
thầy/cô và các bạn. Với tình cảm sâu sắc, chân thành, cho phép tôi được bày tỏ lòng biết
ơn sâu sắc đến tất cả các cá nhân đã tạo điều kiện giúp đỡ trong quá trình học tập và
nghiên cứu đề tài.
Đặc biệt tôi xin gửi lời cảm ơn chân thành nhất tới PGS. TS. Nguyễn Minh Tâm,
trưởng Khoa Điện – Điện đã quan tâm giúp đỡ, hướng dẫn tôi hoàn thành tốt luận văn
này trong thời gian qua và TS. Nguyễn Văn Đông Hải đã hỗ trợ các trang thiết bị và định
hướng học tập cho tôi từ những năm đầu đại học.
Tôi xin bày tỏ lòng biết ơn đến quý thầy cô Khoa Điện – Điện Tử, khoa Đào tạo
Chất lượng cao đã trực tiếp và gián tiếp giúp đỡ tôi trong suốt quá trình học tập và nghiên
cứu đề tài.
Ngoài ra, tôi xin gửi lời đến gia đình bạn Võ Minh Tài đã tạo điều kiện không gian
cho tôi thực nghiệm đề tài, các bạn Nguyễn Văn Hiển, Võ Minh Tài, Lư Minh Triết đã
bên cạnh giúp đỡ, động viên tôi những ngày vừa qua.
Tôi xin gửi lời cảm ơn đến gia đình đã tạo điều kiện và hỗ trợ tài chính cho tôi sớm
hoàn thành đồ án này.
Với điều kiện thời gian cũng như kinh nghiệm còn hạn chế của một học viên, luận
văn này không thể tránh được những thiếu sót. Tôi rất mong nhận được sự chỉ bảo, đóng
góp ý kiến của các thầy cô để tôi có điều kiện bổ sung, nâng cao ý thức của mình, phục
vụ tốt hơn công tác thực tế sau này.
Tôi xin chân thành cảm ơn!

i

Khóa Luận Tốt Nghiệp

Mục Lục
Lời Cảm Ơn ................................................................................................................. i
Mục Lục ......................................................................................................................ii
Danh Mục Hình Ảnh ................................................................................................. iv
Danh Mục Bảng ......................................................................................................... vi
Danh Mục Từ Viết Tắt ............................................................................................. vii
Tóm Tắt ..
...

...
3*motion_dt
yr_target = -(yaw_error*0.5 + yaw_i_gain)
else:
yaw1_angle = 0
yaw_i_gain = 0
yr_target = 0
if pr_target 50:
pr_target = 50
if rr_target 50:
rr_target = 50
if yr_target 50:
yr_target = 50

#=======================================================================
===============
# Rotation rate PIDs
#=======================================================================
===============
[p_out, i_out, d_out] = pr_pid.Compute(qry, pr_target,
motion_dt, gear, throttle)
pr_out = p_out + i_out + d_out
[p_out, i_out, d_out] = rr_pid.Compute(qrx, rr_target,
motion_dt, gear, throttle)
rr_out = p_out + i_out + d_out
[p_out, i_out, d_out] = yr_pid.Compute(qrz, yr_target,
motion_dt, gear, throttle)
yr_out = p_out + i_out + d_out
if pr_out 50:
pr_out = 50

71

Khóa Luận Tốt Nghiệp
if rr_out 50:
rr_out = 50
if yr_out 50:
yr_out = 50

#

if lipo_volt 9 or lipo_volt 15:
off_rps = 0
else:
off_rps = (12.6-lipo_volt)*15
off_rps=0

esc_1_rps = throttle - pr_out - rr_out + yr_out +
off_rps + PID_pressure
esc_2_rps = throttle - pr_out + rr_out - yr_out +
off_rps + PID_pressure
esc_3_rps = throttle + pr_out + rr_out + yr_out +
off_rps + PID_pressure
esc_4_rps = throttle + pr_out - rr_out - yr_out +
off_rps + PID_pressure
#
esc_1_rps = throttle + yr_out + off_rps
#
esc_2_rps = throttle - yr_out + off_rps
#
esc_3_rps = throttle + yr_out + off_rps
#
esc_4_rps = throttle - yr_out + off_rps
else:
pa_target=0
ra_target=0
pr_target=0
rr_target=0
yr_target=0
pr_out=0
rr_out=0
yr_out=0
esc_1_rps=0
esc_2_rps=0
esc_3_rps=0
esc_4_rps=0
yaw_i_gain=0
pi.set_servo_pulsewidth(ESC1, R2P.Rps2Pulse_M1(esc_1_rps))
pi.set_servo_pulsewidth(ESC2, R2P.Rps2Pulse_M2(esc_2_rps))
pi.set_servo_pulsewidth(ESC3, R2P.Rps2Pulse_M3(esc_3_rps))
pi.set_servo_pulsewidth(ESC4, R2P.Rps2Pulse_M4(esc_4_rps))
#
ser.write(b%f %f %f %f %f
%f\\r\\n%(qax,qay,qaz,qrx,qry,qrz))
#
file.write(str(roll)+,+str(pitch)+,+str(yaw1_angle)+,+str(throttle
)+,+str(h)+,+str(x_pos)+,+str(y_pos)+\\n)
#
ser.write(b%f %f\\r\\n%(roll, pitch))
if elapse_time21:

72

Khóa Luận Tốt Nghiệp
end2=time.time()
print(roll,pitch,yaw1_angle,nfb,throttle,lipo_volt,actual_pressure,PID_p
ressure,h,pa_target,ra_target,x_pos,y_pos)
#
print(pa_target,ra_target,x_pos,y_pos)
except:
pi.set_servo_pulsewidth(ESC1, 1000)
pi.set_servo_pulsewidth(ESC2, 1000)
pi.set_servo_pulsewidth(ESC3, 1000)
pi.set_servo_pulsewidth(ESC4, 1000)
print(Sensor error)
# file.flush()
# time.(5)
# file.close()

73