Luận văn Nghiên cứu ứng dụng mô hình Quadrotor trong giám sát và cứu hộ

g trình giao diện giao tiếp máy tính để phục vụ test và 
điều khiển. 
 Quadrotor đã có khả năng tự nâng lên, đáp ứng cân bằng. 
6.2 Hiệu quả kinh tế, xã hội, giáo dục, an ninh, quốc phòng,  
Giám sát tình trạng giao thông ở những thành phố lớn, quan sát và chụp ảnh ở 
những thung lũng sâu, rừng rậm, kiểm tra độ ẩm và nhiệt độ ở một vùng miền đất 
nƣớc, hỗ trợ công tác cứu hộ cháy rừng, tai nạn tàu bay 
Các ứng dụng trong quân sự: do thám hoạt động của đối phƣơng, mang vác các 
thiết bị quân sự tải trọng nhẹ. 
Ngoài các thiết kế dành cho quân sự, các ứng dụng cho các lĩnh vực khác cũng 
đƣợc quan tâm nhƣ quan sát núi lửa, điều tra môi trƣờng, bảo dƣỡng thiết bị, gieo 
trồng, phun thuốc trừ sâu trong nông nghiệp Ngày càng có nhiều các ứng dụng 
thƣơng mại đƣợc phát triển với Quadrotor. 
Mặt khác, đề tài này cũng phục vụ đắc lực cho luận án tốt nghiệp khơi nguồn 
sáng tạo và nghiên cứu trong sinh viên khối kỹ thuật. 
6.3 Hƣớng phát triển 
Tích hợp hệ thống định vị toàn cầu GPS. Sau khi Quadrotor đã có thể tự cân 
bằng và có thể tự bay mà không cần ngƣời điều khiển mà chỉ phụ thuộc vào các 
trạm điều khiển mặt đất thông qua sóng wifi. Việc tích hợp thêm hệ thống định vị 
GPS là một bƣớc tiếp theo giúp cho Quadrotor trở nên linh hoạt hơn trong quá trình 
thực hiện nhiệm vụ. 
Ngoài ra, việc tích hợp thêm một số thiết bị phụ trợ phục vụ cho từng loại 
nhiệm vụ cũng cần đƣợc quan tâm nhƣ việc lắp thêm camera cho Quadrotor nhằm 
ghi lại các đặc điểm địa hình do thám hay các cảm biến siêu âm để dò tìm mục 
tiêu 
HU
TE
CH
45 
Phát triển đề tài với các ứng dụng trong quân sự, có thể mang vác các thiết bị 
quân sự. 
Hiện nay ở Việt Nam đã bắt đầu có nhu cầu về loại đồ chơi công nghệ cao dạng 
Quadrotor, vì thế việc nghiên cứu để hoàn thiện và thƣơng mại hóa loại sản phẩm 
này là một hƣớng phát triển tốt. 
HU
TE
CH
46 
PHỤ LỤC 
1. Code Matlab 
% - global constants for quadrotor ----------------------------------------------------------- 
g = 9.81; % [m/s^2] 
b = 3.13e-5; 
d = 9e-7; 
m = 0.4794; % [kg] 
Ix = 0.0086; % [kg.m^2] 
Iy = 0.0086; % [kg.m^2] 
Iz = 0.0172; % [kg.m^2] 
Jr = 3.7404e-5; % [kg.m^2] 
L = 0.225; % [m] 
% - system dynamics for quadrotor --------------------------------------------------------- 
function out = system_dynamics(in) 
% - state variables -----------------------------------------------------------------------------
x1 = in(1); % x position 
x2 = in(2); % xdot velocity 
x3 = in(3); % y position 
x4 = in(4); % ydot velocity 
x5 = in(5); % z position 
x6 = in(6); % zdot velocity 
x7 = in(7); % phi roll angle 
x8 = in(8); % phidot roll velocity 
x9 = in(9); % theta pitch angle 
x10 = in(10); % thetadot pitch velocity 
x11 = in(11); % psi yaw angle 
x12 = in(12); % psidot yaw velocity 
HU
TE
CH
47 
% - system inputs ------------------------------------------------------------------------------- 
Om_1 = in(13); 
Om_2 = in(14); 
Om_3 = in(15); 
Om_4 = in(16); 
U1 = b*(Om_1^2 + Om_2^2 + Om_3^2 + Om_4^2); 
U2 = b*(-Om_2^2 + Om_4^2); 
U3 = b*(Om_1^2 - Om_3^2); 
U4 = d*(-Om_1^2 + Om_2^2 - Om_3^2 + Om_4^2); 
omega = - Om_1 + Om_2 - Om_3 + Om_4; 
% - first order differential equations -------------------------------------------------------- 
xdot = x2; 
xddot = 1/m * (cos(x7)*sin(x9)*cos(x11) + sin(x7)*sin(x11)) * U1; 
ydot = x4; 
yddot = 1/m * (cos(x7)*sin(x9)*sin(x11) - sin(x7)*cos(x11)) * U1; 
zdot = x6; 
zddot = g - (1/m)*cos(x7)*cos(x9) * U1; 
phidot = x8; 
phiddot = x10*x12*(Iyy-Izz)/Ixx + Jr/Ixx*x10*omega + L/Ixx * U2; 
thetadot = x10; 
thetaddot = x8*x12*(Izz-Ixx)/Iyy - Jr/Iyy*x8*omega + L/Iyy * U3; 
psidot = x12; 
psiddot = x8*x10*(Ixx-Iyy)/Izz + U4/Izz; 
% - function output - state variable derivatives -------------------------------------------- 
out(1) = xdot; % xdot velocity 
out(2) = xddot; % xddot acceleration 
out(3) = ydot; % ydot velocity 
out(4) = yddot; % yddot acceleration 
out(5) = zdot; % zdot velocity 
HU
TE
CH
48 
out(6) = zddot; % zddot acceleration 
out(7) = phidot; % phidot roll velocity 
out(8) = phiddot; % phiddot acceleration 
out(9) = thetadot; % thetadot roll velocity 
out(10) = thetaddot; % thetaddot acceleration 
out(11) = psidot; % psidot roll velocity 
out(12) = psiddot; % psiddot acceleration 
% - end of file ----------------------------------------------------------------------------------- 
% Transformation from U commands to Omd desired propellers speed 
% INPUT: U vector (4 elements) 
% OUTPUT: Desired propellers speed (Omega) 
function out=U2Om(in) 
const_global; 
% Control inputs 
U(1)=in(1); 
U(2)=in(2); 
U(3)=in(3); 
U(4)=in(4); 
MM = [1/(4*b), 0, 1/(2*L*b), -1/(4*d); 
1/(4*b), -1/(2*L*b), 0, 1/(4*d); 
1/(4*b), 0, -1/(2*L*b), -1/(4*d); 
1/(4*b), 1/(2*L*b), 0, 1/(4*d)]; 
Omd = sqrt(MM*U'); 
% outputs 
out(1)=Omd(1); % [dec] 
out(2)=Omd(2); 
out(3)=Omd(3); 
out(4)=Omd(4); 
HU
TE
CH
49 
% Altitude control function 
% INPUT: state, desired altitude 
% OUTPUT: desired speeds 
function out=alt_control(in) 
const_global; 
% -------------------- State ---------------------------------------------------------------------- 
x=in(1); 
dotx=in(2); 
y=in(3); 
doty=in(4); 
z=in(5); % [m] 
dotz=in(6); % [m/s] 
roll=in(7); % [rad] 
dotroll=in(8); % [rad/s] 
pitch=in(9); 
dotpitch=in(10); 
yaw=in(11); 
dotyaw=in(12); 
zd=in(13); % desired altitude [m] 
 % Setpoint 
dotzd=0; 
%------------------Control parameters--------------------------------------------------------- 
% ALTITUDE (without motor dynamics) 
kp_z=0.6; 
kd_z=0.5; 
% % ALTITUDE (with motor dynamics) 
% kp_z=1.4; 
% kd_z=1; 
HU
TE
CH
50 
U1=-(m/(cos(roll)*cos(pitch))) * (-g + (zd-z)*kp_z + (dotzd-dotz)*kd_z); 
%out(1)=m*g; % required thrust [N] 
out(1)=U1; % required thrust [N] 
% Rotations control function 
% INPUT: state, (phi,theta,psi)(ref) 
% OUTPUT: 
function out=rot_control(in) 
const_global; 
% --------------------------- State ----------------------------------------------------- 
x=in(1); 
dotx=in(2); 
y=in(3); 
doty=in(4); 
z=in(5); % [m] 
dotz=in(6); % [m/s] 
roll=in(7); % [rad] 
dotroll=in(8); % [rad/s] 
pitch=in(9); 
dotpitch=in(10); 
yaw=in(11); 
dotyaw=in(12); 
% --------------------------------Setpoints ------------------------------------------------------ 
rolld=in(13); % desired ROLL angle [rad] 
pitchd=in(14); % desired PITCH angle [rad] 
yawd=in(15); % desired yaw angle [rad] 
Td=in(16); % desired thrust [N] (from Altitude controller) 
dotrolld=0; 
dotpitchd=0; 
dotyawd=0; 
HU
TE
CH
51 
% ------------------------------ Control parameters ------------------------------------------- 
% ROLL (without motor dynamics) 
%kp_roll=0.8; 
%kd_roll=0.4; 
% PITCH (without motor dynamics) 
%kp_pitch=0.8; 
%kd_pitch=0.4; 
% YAW (with motor dynamics) 
%kp_yaw=0.8; 
%kd_yaw=0.5; 
 % **************************************************************** 
 % ROLL (with motor dynamics) 
 kp_roll=0.1; 
 kd_roll=0.4; 
% PITCH (with motor dynamics) 
 kp_pitch=0.1; 
 kd_pitch=0.4; 
% YAW (with motor dynamics) 
 kp_yaw=0.1; 
 kd_yaw=0.1; 
% ************ PD controllers **************************************** 
U(1)=Td; % [N] 
U(2)=(rolld-roll)*kp_roll + (dotrolld-dotroll)*kd_roll; % ROll 
U(3)=(pitchd-pitch)*kp_pitch + (dotpitchd-dotpitch)*kd_pitch; % PITCH 
U(4)=(yawd-yaw)*kp_yaw + (dotyawd-dotyaw)*kd_yaw; % YAW 
out(1)=U(1); 
out(2)=U(2); 
out(3)=U(3); 
out(4)=U(4); 
HU
TE
CH
52 
2. Sơ đồ nguyên lí. 
2.1 Cảm biến. 
2.1.2 Cảm biến góc nghiêng (Accelerometer ADXL345). 
2.1.2 Cảm biến gia tốc (Gyroscope L3G4200D). 
2.1.3 Cảm biến từ trƣờng (Magnetometer HMC5883). 
HU
TE
CH
53 
2.1.2 Cảm biến áp suất và nhiệt độ (Barometric Pressure / Temperature 
Sensor BMP085 ). 
2.2 Vi diều khiển (Atmega168). 
HU
TE
CH
54 
TÀI LIỆU THAM KHẢO 
[1] Samir Bouabdallah, Pierpaolo Murrieri, Roland Siegwart, “Designand control of 
indoor micro Quadrocopter”. 
[2]  
[3] Gabriel M. Hoffmann (2005), “Multi-Agent Quadrocopter Testbed 
ControlDesign:Intergal sliding mode vs reinforcement learning”, IEEE 
InternationalConference on Intelligent Robots and Systems. 
[4] Scott D. Hanford, Lyle N. Long, and Joseph F. Horn (September 2005). “A 
SmallSemi-Autonomous Rotary-Wing Unmanned Air Vehicle (UAV)”, AIAA, 
Arlington,Virginia. 
[5]  
[6]  
[7] Paul Pounds, Robert Mahony, Joel Gresham (2004), “Towards 
DynamicallyFavorable Quad-Rotor Aerial Robots”, Proceedings of the 2004 
AustralasianConference on Robotics andAutomation, Canberra, Australia. 
[8] Hoffmann, G., Rajnarayan, D.G., Waslander, S.L., Dostal, D., Jang,J.S., Tomlin, 
C.J. 
[9] Samir Bouadallah, Andre Noth and Roland Siegwart (September 2004), “PID vs 
LQControl Techniques Applied to an Indoor Micro Quadrocopter,”Proceedings 
of2004 IEEE/RSJ International Conference on Intelligent Robots and 
Systems,Sendal, Japan. 
[10] Ming Chen and Mihai Huzmezan (2003), “A Combined MBPC/ 2 
DOFControler for a Quad Rotor UAV,” AIAA Guidance, Navigation, and 
ControlConference and Exhibit,Austin, Texas, August. 
[11] Erdinc Altu, James P. Ostrowski and Robert Mahony (May 2004), “Control of 
a Quadrotor Helicopter Using Visual Feedback”,Proceedings of the 2002 IEEE 
International Conference on Robotics and Automation, Washington, DC USA. 
HU
TE
CH
55 
[12] J. Dunfied, M. Tarbouchi and G. Labonte (2004), “Neural Network Based 
Control of a Four Rotor Helicopter,” IEEE International Conference on Industrial 
Technology. 
[13] H. Goldstein (2002) et al., Classical Mechanics. Addison Wesley. 
[14] B. Etkin and L. Reid (1996), Dynamics of Flight: Stability and Control. John 
Wiley and Sons. 
[15] J. Watkinson (2004), The Art of the Helicopter. Elsevier. 
[16] R. Murray (1994)et al., A Mathematical Introduction to Robotic Manipulation. 
CRC. 
[17] S. Arimoto (1996), Control Theory of Non-linear Mechanical Systems. Oxford 
Science.