BÁO CÁO MÔ HÌNH ĐIỀU KHIỂN QUADROCOTER

Trang 1

Lời cảm ơn

Kính gửi Thầy TS Chung Tấn Lâm !

Em là Phan Lê Sơn, sinh viên năm 4, ngành Cơ Điện Tử , Trường Dai Hoc Bách Khoa Tp.HCM

Học kỳ này, em rất vinh dự được thực hiện đề tài của đồ án môn học Thiết Kế

Hệ Thống Cơ Điện Tử dưới sự hướng dẫn tận tình của Thây Chính sự nhiệt

tình chỉ dẫn của Thây đã giúp em hoàn thành đồ án khá tốt, đạt được những yêu cầu cơ bản mà để tài đặt ra Và cũng qua đó, em đã học hỏi, tiếp cận được nhiều vấn đề thực tế hơn, bổ sung những “khe hở” kiến thức do việc học thiên về lý thuyết để lại

Em xin chân thành cảm ơn Thầy

Cũng qua đây, em xin gửi lời cảm ơn chân thành của mình đến gia đình, bạn bè

-những người đã động viên, quan tâm hết mực đến em trong suốt quá trình thực hiện đồ án Đó là những động lực thúc đẩy em cố gắng hơn, quyết tâm hơn, để cuối cùng em có được kết quả như ngày hôm nay

Trang 2

BAO CAO MO HINH DIEU KHIEN QUADROCOPTER

Noi dung

Giới thiệu về QuadroCopter

Cầu tạo của QuadroCopter

Nguyên lý hoạt động của QuadroCopter

Xây dựng mô hình QuadroCopter trong VisualNastran

Xây dựng mô hình điêu khiên QuadroCopter trong Simulink cua MatLab Lý thuyết về bộ lọc Kalman

Ứng dụng bộ lọc Kalman vào đo các góc nghiêng của máy bay

Hướng phát triển của đề tài

Phụ Lục

©Ð©

PNAMAYN

Trang 3

I GIỚI THIỆU VỀ QUADROCOPTER:

Quadrocopter là một loại máy bay có 4 cánh cùng nằm trong một mặt phẳng và

đối xứng qua tâm của máy bay Nhờ có 4 cánh nên loại máy bay này dễ điều khiển hơn

và có khả năng giữ thăng bằng tốt hơn Loại máy bay này có rất nhiều ứng dụng thực

tế, nó có thê có kích cỡ lớn để chở người và hàng hoá và cũng có thể có kích cỡ nhỏ

rất năng động có thể hoạt động ngay cả trong nhà Loại máy bay kích cỡ bay trong

nhà hiện nay đang phát triển rất rầm rộ , nhiều sinh viên của Đức và Mỹ đã chế tạo loại máy bay này với nhiều hình dáng và tính năng khác nhau đề nghiên cứu Trong

phân đồ án này nhóm xin được giới thiệu kết quả nghiên cứu về loại Quadrocopter hoạt động trong nhà

Trong đồ án này nhóm đã mô phỏng máy bay Quadrocopter bằng phần mềm

MatLab kết hợp với Visual Nastran Sau đó nhóm đã thiết kế phần cứng máy bay và

hoàn thành một số modul nhỏ của công việc điều khiển máy bay Trong đồ án này có đưa vào modul đo góc nhiêng của máy bay bằng cảm biến gyro và cảm biến gia tốc dài được ước lượng bằng bộ lọc Kalman

Trang 5

II CAU TAO CUA QUADROCOPTER :

- _ Thân: thường có hình đáng giống dấu chữ thập , mỗi cánh của chữ thập đỡ một cánh của máy bay

- _ Cánh: có 4 cánh giống hệt nhau đặt trên 4 góc của thân , 2 cánh đối diện thi quay cùng chiều nhau và ngược chiều với 2 cánh đối diện còn lại Sở đĩ máy bay có

cầu tạo như vậy đê làm hạn chê sự quay của thân khi cánh quay

- _ Bộ điều khiến : bao gồm 2 phần, 1 phần đặt trên máy bay , một phần nằm ở dưới

đất

+ Phân đặt trên máy bay : bao gồm mạch công suất điều khiển 4 động cơ , mạch truyền sóng để kết nối với bộ điều khiển ở dưới đất , mạch chính dùng đề điều

Trang 6

) Để điều khiển máy bay theo chế độ tự động ta còn phải có thêm các cảm biến như cảm biến gyro dé đo độ nghiêng, cảm biến áp suất đề đo độ cao , cảm biển gia tốc đề biết hướng chuyển động , camera xử lý ảnh có nhiệm vụ theo dõi các

đối tượng Ở dưới đất

+ Phần dưới đất : bao gồm bộ điều khiển bằng tay có mạch thu phát tín hiệu sóng VÔ tuyến, mạch kết nối với máy tính hoặc là mạch giao tiếp với người sử dụng để họ có thể thay đổi các thông số và các chế độ điều khién

- _ Động cơ và bộ driver của động cơ : nếu máy bay chỉ hoạt động bay thông thường thì chỉ cần bốn động cơ dùng cho 4 cánh quạt là đủ

Hình 2: Mô hình QuadroCopter

II NGUYÊN LÝ HOẠT ĐỘNG CỦA QUADROCOPTER:

Để điều khiển được hoạt động của máy bay ta phải điều khiển tỷ lể tốc độ tương ứng giữa 4 cánh quạt Sau đây là cách điều khiển cụ thể:

Bay lên:

Bốn cánh quay với vận tốc đều nhau giống như hình vẽ Ở trên hình chúng ta thấy 2 cánh đối diện của máy bay quay cùng chiều với nhau và ngược với 2 cánh còn lại Điều này rất quan trọng vì như vậy nó sẽ không làm máy bay xoay quanh trục

vuông góc với mặt phẳng máy bay, theo nguyên lý bảo toàn momanh động lượng thì 2 cặp cánh quay ngược nhau sẽ tạo ra cặp momanh động lượng đối nghịch triệt

Trang 7

Hình 3 : Điều khiển máy bay bay lên

Bay theo một hướng :

- _ Giữ nguyên tốc độ 2 cánh theo hướng không dịch chuyển - _ Tăng tốc độ cánh nằm ở gốc vector theo phương dịch chuyển - Giảm tốc độ cánh nằm ở đỉnh vector theo phương dịch chuyển Front “i So + L2 (2)

Hình 4 : Điều khiển máy bay bay theo 1 hướng

Trang 8

ngiêng theo phương địch chuyển Chính nhờ góc nghiêng này mà lực nâng của cánh

quạt không còn nằm theo phương thẳng đứng , và tổn tại thành phần lực hướng theo

phương chuyển động Chính thành phần lực này đã làm máy bay bay theo phương ngang

c7

Hình 5 : Lực tác dụng lên thân máy bay khi nó bị nghiêng

Nếu chúng ta muốn máy bay bay theo phương nào của thân thì chúng ta áp dụng nguyên tắc điều khiển trên vào phương đó

Xoay vòng quanh trục vuông góc với mặt phẳng máy bay:

- Không mất tính tổng quát , ta giả sử cần điều khiển máy bay xoay ngược chiều kim

Trang 9

Ễ-r4»rIt

Hình 6 : Điều khiển máy bay bay vòng quanh trục vuông góc với mặt phẳng máy bay

IV XÂY DỰNG MƠ HÌNH QUADRO TRONG VISUAL NASTRAN :

Phần mềm Visual Nastran là phần mềm giúp bạn tạo được môi trường vật lý gần giống với thực tế để mô phỏng hoạt động của robot Với việc dùng phần mềm

Visual Nastran kết hợp với Simulink của Matlab giúp ta có một môi trường mô phỏng

gần với thực tế nhất Trong Visual Nastran các yếu tố trọng lượng , vận tốc, gia tốc, momanh quán tính đều được xét đến

Trang 10

Hình 7: Mô hình cơ khí của máy bay được xây dựng bằng phần mềm Pro/Engineer Mô hình trên là mô hình cải tiến của máy bay mà nhóm đã xây dựng sau nhằm đáp

ứng các yêu cầu thực tế, còn đưới đây là mô hình mà nhóm dùng để mô phỏng trong phan mém Visual Nastran

Giữa thân máy bay và 4 cánh quạt được gắn 4 khớp quay

Khi cánh quạt quay ta xây dựng hàm truyền giữa vận tốc cánh quạt và lực nâng của nó Việc tính toán hàm truyền này được thực hiện trong môi trường MatLab , sau khi tính được lực nâng, ta đưa lực này vào trong môi trương Visual Nastran ( do trong Nastran không có chức năng tạo lực nâng khi cánh quạt quay ) Việc đưa lực vào 4 cánh quạt chỉ được thực hiện với độ chính xác tương đối , nghĩa là các lực này không

đối xứng nhau hoàn toàn Làm như vậy ta có thể kiểm tra được đáp ứng của luật điều

khiển, bởi vì khi máy bay hoạt động trong môi trường thực tế thì các lực nâng này

Trang 11

4+ MS(.viyualHaalr ar (ewklep SMOGO1.vwrm3 Pee : 1 .ẽ ae Cee she TL Wey al Chl nikléel< ey Conmacton ti AT La Neâncty SP £ twhU f= aogier olor) of carts Properties BB toon e ed Aces BA rmáxe pA eee

~ [i] [4] fle SS [ø][m] fim [ 6 Ímm[ 1?12+

Đ mm caencut ctŠ rreLxoet Írroe=B6Š

Hình 8 : Mô hình máy bay trong Visual Nastran

Trong các cửa sổ giao diện của Visual Nastran ta cho hiện lên các cửa sổ thông báo lực tác dụng lên 4 cánh máy bay, góc nghiêng theo 2 phương x và y

V XÂY DỰNG MƠ HÌNH ĐIỀU KHIỂN CỦA QUADROCOPTER TRONG SIMULINK CUA MATLAB :

Nhiệm vụ của phần này là xây dựng một mô hình điều khiển máy bay QuadroCopter Nếu ta muốn điều khiển hướng di chuyển của QuadroCopter thì trước tiên ta phải điêu khiển được độ nghiêng của máy bay theo 2 phương x, y ( oxy là mặt phẳng song song

Trang 12

Hình vẽ ký hiệu các cánh của QuadroCopter

Nếu ta muốn cho máy bay nghiêng theo phương X thì ta phải thay đổi tỷ lệ vận tốc của

cánh A và cánh C ( sự thay đổi vận tốc tương ứng với sự thay đổi lực nâng và lực vuông

góc với trục ) Tương tự nếu ta muốn thay đổi góc nghiêng theo phương Y thì ta phải

thay đổi tỷ lệ vận tốc của 2 cánh B và D

Công thức liên hệ giữa lực nâng F ( vuông góc với mặt phẳng chứa cánh quạt ) và vận tốc g8ôc œ của máy bay là :

F=k.o2

với k là hằng số phụ thuộc vào môi trường và cầu tạo của cánh máy bay

Mô hình điều khiển được xây dựng trên môi trường Simulink của MatLab kết hợp với

VisualNastran

Trang 13

Es-šMð Ì | sẽ Em die MATLAB Purcien > vê bh tam

| one tise 10) Let}

Qeavticert Sart Cedet Tureter Fons Setautis rials Lerier Hualẻl EnEs44+i MT“ B F ii ối sai! guYcl | at | BI TL | Ta] = | ' >>: —w| ru _* yu} : pI #| vs |

| | LJ rei se Rar = | bea.) J

= | Gua rttze Zew-Orde Tiararfar Pow Gatien ior? Font

Homes tere Qed

FrsB Hold Uarrkz sg5Ÿ

Ì ga Ì af ha re te lai [> Lư

Sơ đồ điều khiển trên bao gồm 3 phần :

- _ Phần có khối Controller dùng để điều khiển độ nghiêng theo phương X

- _ Phân có khôi Controllerl dung đê điêu khiên độ nghiêng theo phương Y

- _ Phần có khối controller2 dùng để điều khiển độ cao theo phương Z

Do 2 khối đầu tương tự như nhau nên ta chỉ cần chỉ ra các khối con trong 1 khối là đủ:

Step

-Khéi Step | : chung cho cả 2 khối lớn , đây là khối tạo ra giá trị tham khảo về độ nghiêng của máy bay theo 2 phương X, Y

Trang 14

- Khéi PID: Kp=30, Ki=15, Kn =25 mot 4 | - Khôi = Suantzer Sain maa : Dùng đề lượng tử hoá tín hiệu đầu vào và giữ nó ũ

trong một chu kỳ tính tốn

- Khơi hàm truyên của động cơ : tín hiệu đâu vào là điện áp, tín hiệu đầu ra là vận 1 £“+J0z+ 11 tốc góc Transfer Fen

- _ Khối bão hoà và khối giới hạn mức tăng nhằm giữ cho tín hiệu đầu ra nằm trong

khoảng giới hạn vật lý cho phép của mô hình be be Yr Yr Zé

Saturation Rate Limiter

Khối tính toán : với đầu vào là độ thay đổi vận tốc gốc và đầu ra là sự phan chia

sự thay đôi giả trị đầu vào của vận tôc gôc cho 2 cánh —‡' fen ye Embedded MATLAS Function Nội dung trong khối này như sau: function y = fen(u)

% This block supports an embeddable subset of the MATLAB language % See the help menu for details

Trang 15

y2= (-utsqrt(16-u*u))/2 ; y=l[yl y2]; - _ Khôi hàm truyên từ vận tôc gôc sang lực vuông góc với cảnh : —e u |——— Fen2 - Khéi VisualNastran : Scupsz wÑF lăn† +Ƒ ASMOOO2

Khối này gồm 4 đầu vào đó là lực nâng của 4 cánh quạt Đầu ra là độ nghiêng của máy bay theo phương X và Y

Nêu muôn có cánh quạt quay đê hình ảnh mô phỏng trực quan gân với thực tê hơn ta có

thê thêm vào 4 đầu vào vận tôc cho khôi VisualNastran

Ý tưởng xây đựng khối này là từ góc nghiêng đo về ta so sánh với giá trị tham khảo để đưa ra sai số góc nghiêng , từ sai số góc nghiêng ta đưa vào khối PID tính ra được mức vol chênh lệch cần đưa vào để điều khiển 2 cánh quạt Từ mức Vol này , sau khi

Trang 16

Sau đây là kết quả mô phỏng được thể hiện trên VisualNastran :

x [d*ul yideg) +s time fs)

Đồ thị trên biểu điễn góc nghiêng của máy bay theo phương X và Y theo thời gian ( giá

trị max và min của đồ thị góc trên là từ - 909 đến 90 9 )

Trong đồ thị ta thấy trong 5 s đầu góc tương ứng theo 2 phương hướng tới trạng thái ổn định ( 0) và sau 5 s thì góc nghiêng chuyển sang 18° Dé thay dugc két qua của luật điều khiến ta so sánh đồi thị trên với dé thi giá trị tham khảo của khối Step đã được đưa ra trong phần khối Step Kết quả mô phỏng băng hình ảnh cho ta thấy ban đầu ( trong 5s đầu ) 2 góc của máy bay là 0° tương ứng với việc giữ thăng bằng trên không của máy bay Và sau 5s góc điều khiển theo 2 phương là 189 nên ta thấy được đáp ứng của máy bay là thay đổi hướng ngay lập tức ứng với phương nghiêng điều khiến

Khối điều khiển độ cao :

Sự khác biệt của khối điều khiển độ cao so với 2 khối kia là ở chỗ , trong mỗi khối

điều khiển góc nghiêng chỉ tác động đến 2 cánh quạt tương ứng với góc nghiêng đó Còn trong khối điều khiển độ cao thì tác động 1 lúc lên 4 cánh quạt Cụ thể như sau,

sau khi đọc giá trị độ cao về ta so sánh với giá trị tham khảo thì thu được sai số độ cao

Trang 17

lại là đầu vào của khối mô hình hoá động cơ, đầu ra của khối mô hình hoá động cơ

chính là vận tốc cần thay đối Vận tốc này được chia đều cho 4 cánh quạt để đảm bảo

không gây ảnh hưởng nhiều tới độ nghiêng

Để việc mô phỏng được trực quan hơn và dễ sử đụng hơn nhóm đã thiêt kế giao diện với người dùng trong môi trường Matlab : Ti eel Close | Simulate | 0 lại | a Lại | a ~ | =|] ee ee ữ | a || a ñ | ! ik i , a | a rae | “h “ Seas ie = Ell II] BI li = J2) ed Bế Se Bae ae oe x Y z MAI V2 Wo wid

Hình vẽ giao diện điều khiển máy bay mô hình

Giao diện có các thanh trượt và các Edit box để người dùng có thể nhập giá trị điều

khiển vào máy bay hoặc quan sát các giá trị đo được từ máy bay

Các thanh trượt X, Y là để điều khiển góc nghiêng theo phương X và Y Thanh trượt Z dùng để điển khiển độ cao theo phương Z

Các thanh trượt V1, V2, V3, V4 là dùng để điều khiển vận tốc của các cánh A, B, C, D của máy bay

Phần Code của giao diện được để ở phần phụ lục

Trang 18

1 Giới thiệu bộ lọc Kalman:

Năm 1976 R.E Kalman đã đưa ra bài viết miêu tả phương pháp giải hồi quy cho

những bài toán lọc tín hiệu rời rạc Kể từ đó, với những ưu điểm lớn trong tính toán,

bộ lọc Kalman là đề tài của nhiều nghiên cứu và ứng dụng, cụ thể là trong lĩnh vực tự

động hay hỗ trợ dò đường

Bộ lọc kalman là tập hợp những phương trình toán học nhằm tính toán các giá trị

trung bình để ước lượng trạng thái của quá trình, bằng cách giảm thiểu giá trị trung bình của bình phương sai số Bộ lọc kalman rất mạnh ở nhiều mặt: nó cung cấp giá trị ước lượng của trạng thái trước, hiện tại, và kế tiếp, và nó có thể hoạt động ngay cả

khi không xác định được trạng thái chính xác của hệ thống được mơ hình hố Bộ lọc kalman được phân làm 2 loại : rời rạc (discrete) và mở rộng (extended)

Trong đồ án này nhóm xin trình bày những nội dung cơ bản về bộ lọc kalman, và

một số ví dụ cụ thể

2 Bộ lọc kalman rời rạc ( Discrete Kalman filer)

Bộ lọc kalman ước lượng quá trình rời rạc có trạng thái xL] R*“, với phương trình update tuyến tính sau:

Xk = Axk-1 + Buk-1 + Wk-1 Và với giá trị đo * là:

Z= Hxk + Vk

Hai biến ngẫu nhiên w và v đặc trưng cho nhiễu quá trình và nhiễu thiết bị đo

Chúng độc lập với nhau, là nhiễu trắng và đều có xác xuất tuân theo phân phối

chuẩn:

p(w) ~ N(O,Q) p(v) ~ N(O,R)

Trong thực tế các ma trận covariance của nhiễu quá trình Q và nhiễu thiết bị

đo R là thay đổi nhưng ở đây ta giả sử chúng không thay đổi

Ma trận vuông nxn A trong phương trình (1) biểu diễn mối quan hệ giữa trạng

thái trước đó và trạng thái hiện tại trong trường hợp không có nhiễu quá trình Chú ý

rằng A thay đổi đối với từng trạng thái nhưng ở đây ta giả sử A không đổi Ma trận B nxI biểu điễn quan hệ giữa biến điều khiển ngõ vào và trạng thái Ma trận H mxn biểu diễn quan hệ giữa giá trị đo và trạng thái của quá trình Trong thực thế, H thay đổi nhưng ta giả sử H không đổi

Quá trình tính toán của bộ lọc:

40

Ta dinh nghia nx RO 1a trang théi truéc uéc lugng tai bước k, cho biết trang

Trang 19

bước k, và cho ra giá tri do la zx Ta định nghĩa các sai số trước và sau khi ước lượng là: ek =Xx~ OU Xx A x = Xx LI xe Ma tran covariance sai số trước và sau ước lượng sẽ là: Pus =E Y<eenr/f P, = E Yes f

Quá trình xử lý của bộ lọc kalman chia làm hai giai đoạn : cập nhập thời gian ( time update ) và cập nhập giá trị đo ( mesurement update ) Quá trình update tính toán các

ag

giá trị trước ước lượng: ma trận P' và x: cho nên đây còn gọi là quá trình tiên đoán ( predictor) Quá trình measurement update sẽ cho ra những giá trị sau khi ước lượng Px và x: từ những giá trị có được ở time update, cho nên còn gọi là quá trình chỉnh sữa (corrector) Các phương trình tính toán như sau: | +

Time update (“predict”) Measurement update (“ corect”)

1 Ước lượng giá trị kế tiếp: 1.Tính toán độ lợi Kalman:

Xk = Axk-1 + Buk-t + Wk-1 Ky = PH" (APH + R)™ 2 Ước lượng ma trận covariance 2.Cập nhập lại với gid tri do zx kế tiếp: ^^ ^n PP = AP.nAr +Q x:=xi+ Ki(+L Hxi) 3,Cập nhật lại ma trận covariance Pr k =(TLKH)P Khởi tạo 4g Xe va Pe-t

Quá trình tính toán của bộ lọc DKF

Trong đó L chính là hệ số thặng dư của bộ lọc Kalman, hay độ lợi Kalman

Trang 20

Trong quá trình thực thi thực tế của bộ lọc, ma trận covariance của nhiễu thiết bị đo R được xác định trước R được đo off_line băng cách cho một số giá trị đo mẫu, khảo sát và xác định sự biến đổi của nhiễu đo Ta cũng có thể xác định Q từ thực nghiệm nhưng khó khăn hơn nhiều bởi vì đôi khi ta không thể quan sát được trực tiếp quá trình đang xảy ra

Q và R chính là hai thông số để hiệu chỉnh bộ lọc Kalman Quá trình hiệu chỉnh thường được thực hiện off_ line

3 Mô hình hoá gyro:

Gyro là cảm biến đo vận tốc góc

Góc nghiêng theo phương x của máy bay ở trạng thái hiện tại bằng góc theo phương x ở trạng thái trước cộng với tích phân của vận tốc góc Ta có phương trình sau:

(+ =b@)+ |@).9/+bz(@Œ

Trong đó :

@+Ð) là giá trị góc nghiêng ở trạng thái thứ k+l Le) là trạng thái góc nghiêng ở trạng thái trước đó

|) là vận tốc góc đo bởi gỷo ở trạng thái thứ k

@/ là thời gian lấy mẫu

b;(k) là độ trôi của hệ số gỷo bias

3 M6 hinh hod Accelerometer :

Tương tự như gyro, giá trị vị trí ta tính được cũng bị sai số đo hai lần lấy tích phân

Phương trình trạng thái của accelerometer như sau: v(k+]) = v(k)+ a(k).® + ba(k) xz(k+1) =xa()+v().cos\(&).®r %z(k+1) =ya(k+v(&).sin\(&).®r Trong đó: v(&), v(&+1) lần lượt là vận tốc đài của máy bay ở 2 trạng thái kế tiếp k và k+1

a(k) 1a gia tốc của xe ở trạng thái thứ k b;(k) là hệ số trôi của accelerometer

xa(k), xa(k) lần lượt là vị trí theo phương x của máy bay ở 2 trạng thái kế tiếp k và k+l

ya(k), ya(k+1) lần lượt là vị trí theo phương y của máy bay ở 2 trạng thái kế

Trang 21

4 Thiết kế sơ đồ mạch nguyên lý của øyro và accelerometer

Cảm biến gia tốc mà nhóm sử dụng là cảm biến gia tốc 3 trục , có nghĩa là với cảm biến accelerometer này có thể đo được gia tốc theo 3 phương X, Y, Z Z_DATA - ANY ZOUT 4k LL C3 | 0.1uF R2 — Y_DATA ` YOUT U1 = ^^~ˆ † MMA7280 ‘ik | ~L- c4 BE 0.1uF D0 g-Select1 ~sep_mode R3 = g-Select2 NC7 X DATA = XOUT VDD NC6 = AWN 1 VSS NC5 1K | C5 | 0.1uF

Sơ đồ nguyên lý cảm biến gia tốc

Cảm biến MMA7260 cần nguồn cung cấp là 3.3 V Các chân chúng ta sử dụng ở đây là chân g-Select1 ( chân số 1) , g-Select2 ( chân số 2 ) dùng để chọn chế độ hoạt động

cảm biến Ba chân dữ liệu XOUT (15), YOUT (14), ZOUT (13) là 3 chân đưa ra mức

Vol tỷ lệ với gia tốc theo 3 phương X, Y, Z

Trên mô hình thí nghiệm nhóm sử dụng 2 cảm biến gyro đo theo phương X và Y U6 5V 4 2 et BV Nc Ƒ 3.3V_1 3 4 ————————| 33V C+ ScnD DATA ——x~~ | GV —C

Sơ đồ nguyên lý cảm biến gyro

Trang 22

8.Phu Luc:

Code chương trình thiết kế giao điện bằng phan mém MatLab

function varargout = GUI1(varargin) % GUI1 M-file for GUI1.fig

% GUII, by itself, creates a new GUI1 or raises the existing % singleton*

%

% H= GUII returns the handle to a new GUI] or the handle to % the existing singleton*

%

% GULI(CALLBACK,hObJect,eventData,handles, ) calls the local

% function named CALLBACK in GUI1.M with the g1ven input arguments %

% GUII(Property, Value) ) creates a new GUII or ratses the

% existing singleton* Starting from the left, property value pairs are % applied to the GUI before GUI1_OpeningFunction gets called An % unrecognized property name or invalid value makes property application % stop All inputs are passed to GUI1_OpeningFen via varargin

%

% *See GUIOptions on GUIDE%S Tools menu Choose "GUI allows onÏy one % instance to run (singleton)”

%

% Edit the above text to modify the response to help GUI1 % Last Modified by GUIDE v2.5 28-Feb-2008 13:00:33 % Begin initialization code - DO NOT EDIT

gui_Singleton = 1;

gui_State = struct('gui_Name'’, mfilename,

'gui_Singleton', gui_ Singleton,

'gui_OpeningFcn', @GUII_OpeningEcn,

'gui_OutputFen', @GUI1_OutputFcn,

'gui_LayoutFcn', [],

Trang 23

if nargin && ischar(varargin{ 1 }) gui_State.gui_Callback = str2func(varargin{ 1 }); end if nargout [varargout{ 1:nargout}] = gui_mainfcn(gui_State, varargin{:}); else gui_mainfcn(gui_State, varargin{:}); end

% End initialization code - DO NOT EDIT % - Executes just before GUI is made visible

function GUI1_OpeningFen(hObject, eventdata, handles, varargin) % This function has no output args, see OutputFen

%hObject handle to figure

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA) % varargin command line arguments to GUI1 (see VARARGIN) % Choose default command line output for GUI handles.output = hObject; % Update handles structure guidata(hObject, handles); % UIWAIT makes GUI] wait for user response (see UIRESUME) % uiwait(handles.figure1);

% - Outputs from this function are returned to the command line function varargout = GUI1_OutputFcn(hObject, eventdata, handles) % varargout cell array for returning output args (see VARARGOUT); %hObject handle to figure

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA) % Get default command line output from handles structure varargout{1} = handles.output;

% - Executes on slider movement

function sliderX_Callback(hObject, eventdata, handles)

Trang 24

x=num2str(x);

sef(handles.editX,'string',x);

set_param(son1/StepX,'After',num2str(get(handles.sliderX,'value')/10000)); %hObject handle to sliderX (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB

%handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,' Value’) returns position of slider

% get(hObject,'Min') and get(hObject,’Max’) to determine range of slider

% - Executes during object creation, after setting all properties

function sliderX_CreateFcn(hObject, eventdata, handles)

% hObject handle to sliderX (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: slider controls usually have a light gray background, change % ‘usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 9 9]); else set(hObject, BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

function sliderY_Callback(hObject, eventdata, handles)

Y=get(handles.sliderY,'value');

Y=num2str(Y);

set(handles.editY, string',Y);

seft_param(Œson1/StepY','After',num2str(get(handles.sliderY,'value')/10000)); %hObject handle to sliderY (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider

Trang 25

function sliderY_CreateFcn(hObject, eventdata, handles)

%hObject handle to sliderY (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: slider controls usually have a light gray background, change % ‘usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 9 9]); else set(hObject,' BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

function sliderZ_Callback(hObject, eventdata, handles)

Z=get(handles.sliderZ, value’);

Z=num2str(Z);

set(handles.editZ,'string',Z);

%hObject handle to sliderZ (see GCBO)

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,' Value’) returns position of slider

% get(hObject,'Min’) and get(hObject,'Max’') to determine range of slider

function sliderZ_CreateFcn(hObject, eventdata, handles)

%hObject handle to sliderZ (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1;

if usewhitebg

Trang 26

sef(hObJect,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'`)); end

function editx_Callback(hObject, eventdata, handles)

%hObject handle to editx (see GCBO)

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editx as text

% str2double(get(hObject, String')) returns contents of editx as a double

function editx_CreateFcn(hObject, eventdata, handles)

%hObject handle to editx (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispe set(hObject, BackgroundColor','white'); else set(hObject, BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

function editY_Callback(hObject, eventdata, handles)

%hObject handle to editY (see GCBO)

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editY as text

% str2double(get(hObject, String')) returns contents of editY as a double y=get(handles.editY,'string’);

y=str2num(y);

Trang 27

function editY_CreateFcn(hObject, eventdata, handles)

%hObject handle to editY (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispe set(hObject, BackgroundColor','white’); else set(hObject,' BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

function editZ_Callback(hObject, eventdata, handles)

%hObject handle to editZ (see GCBO)

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editZ as text

% str2double(get(hObject, String')) returns contents of editZ as a double z=get(handles.editZ,'string’);

z=str2num(z);

set(handles.sliderZ,'value',z);

function editZ_CreateFcn(hObject, eventdata, handles)

%hObject handle to editZ (see GCBO)

Trang 28

function sliderV 1_Callback(hObject, eventdata, handles)

%hObject handle to sliderV1 (see GCBO)

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'Value') returns position of slider

% get(hObject,'Min') and get(hObject,'Max’) to determine range of slider VI=get(handles.sliderV 1,'value');

V1=num2str(V 1);

set(handles.editV 1,'string’, V1);

set_param(‘son1/Pulse

Generator 1','Amplitude',num2str(1000* get(handles.sliderV 1,'value'))); % - Executes during object creation, after setting all properties

function sliderV 1_CreateFcn(hObject, eventdata, handles)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: slider controls usually have a light gray background, change % 'usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 9 9]); else set(hObject,' BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

function sliderV2_Callback(hObject, eventdata, handles)

% hObject handle to sliderV2 (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,' Value’) returns position of slider

% get(hObject,'Min’) and get(hObject, Max) to determine range of slider V2=get(handles.sliderV2,' value’);

V2=num2str(V2);

Trang 29

set_param(‘son1/Pulse

Generator2','Amplitude',num2str(1000* get(handles.sliderV2,'value')));

function sliderV2_CreateFcn(hObject, eventdata, handles)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: slider controls usually have a light gray background, change % ‘usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1; if usewhitebg set(hObject,`BackgroundColor',[.9 9 9]); else set(hObJect,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'`)); end

% get(hObject,'Min') and get(hObject,’Max’) to determine range of slider V3=get(handles.sliderV3,'value’); V3=num2str(V3); sef(handles.editV3, string',V3); set_param(‘son1/Pulse Generator3','Amplitude',num2str(1000* get(handles.sliderV3,'value')));

% hObject handle to sliderV3 (see GCBO)

Trang 30

% Hint: slider controls usually have a light gray background, change % ‘usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 9 9]); else set(hObject,'BackgroundColor', get(0,'defaultUicontrolBackgroundColor')); end

function editv1_Callback(hObject, eventdata, handles)

%hObject handle to editvl (see GCBO)

%handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editv1 as text

% str2double(get(hObject,'String')) returns contents of editv1 as a double

function editv1_CreateFcn(hObject, eventdata, handles)

% hObject handle to editv1 (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER 1Í ISpC set(hObject, BackgroundColor',"white); else sef(hObJect,'BackgroundColor',get(0,'defaultUicontrolBackgroundColor'`)); end

%hObject handle to editv2 (see GCBO)

Trang 31

% Hints: get(hObject,'String') returns contents of editv2 as text

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispe set(hObject, BackgroundColor','white'); else set(hObject, BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editv3 as text

% hObject handle to editv3 (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER

1Í ISpC

set(hOblject, BackgroundColor,'white);

else

Trang 32

end

% ge((hObJect,Min) and get(hObject, Max') to determine range of slider V4=get(handles.sliderV4,'value`); V4=num2str(V4); set(handles.editV4,'string', V4); set_param(‘son1/Pulse Generator4','Amplitude',num2str(1000# get(handles.sliderV4,'value`)));

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFcns called % Hint: slider controls usually have a light gray background, change % ‘usewhitebg' to 0 to use default See ISPC and COMPUTER usewhitebg = 1; if usewhitebg set(hObject,'BackgroundColor',[.9 9 9]); else set(hObject,'BackgroundColor', get(0,'defaultUicontrolBackgroundColor')); end

function editV4_Callback(hObject, eventdata, handles)

%hObject handle to editV4 (see GCBO)

Trang 33

% Hints: get(hObject,'String') returns contents of editV4 as text

% str2double(get(hObject, 'String')) returns contents of editV4 as a double V4=get(handles.sliderV4, value’);

V4=num2str( V4);

set(handles.editV4,'string', V4);

function editV4_CreateFcn(hObject, eventdata, handles)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispe set(hObject, BackgroundColor','white'); else set(hObject,'BackgroundColor', get(0,'defaultUicontrolBackgroundColor')); end

function editV 1_Callback(hObject, eventdata, handles)

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV1 as text

% str2double(get(hObject,'String')) returns contents of editV1 as a double

function editV 1_CreateFcn(hObject, eventdata, handles)

Trang 34

% Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispe set(hObject, BackgroundColor','white’); else set(hObject, BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV3 as text

% str2double(get(hObject,'String')) returns contents of editV3 as a double % - Executes during object creation, after setting all properties

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFens called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispe set(hObject, BackgroundColor','white'); else set(hObject,' BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editV2 as text

% str2double(get(hObject, String')) returns contents of editV2 as a double % - Executes during object creation, after setting all properties

Trang 35

% eventdata reserved - to be defined in a future version of MATLAB % handles empty - handles not created until after all CreateFcns called % Hint: edit controls usually have a white background on Windows % See ISPC and COMPUTER if ispc set(hObject, BackgroundColor’, white’); else set(hObject,' BackgroundColor’, get(0,'defaultUicontrolBackgroundColor')); end

function editX_Callback(hObject, eventdata, handles)

%hObject handle to editX (see GCBO)

% handles _ structure with handles and user data (see GUIDATA) % Hints: get(hObject,'String') returns contents of editX as text

% str2double(get(hObject,'String')) returns contents of editX as a double x=get(handles.editX, string’);

x=str2num(x);

set(handles.sliderX,'value',x);

function editX_CreateFcn(hObject, eventdata, handles)

%hObject handle to editX (see GCBO)

Trang 36

set_param(‘son1/Pulse Generator2','Amplitude',num2str(1000* get(handles.sliderV2,'value'))); set_param(‘son1/Pulse Generator3','Amplitude',num2str(1000* get(handles.sliderV3,'value'))); set_param(‘son1/Pulse Generator4','Amplitude',num2str(1000* get(handles.sliderV 4, 'value'))); set_param('son1/StepX’,'After',num2str(get(handles.sliderX,'value')/10000)); set_param(‘son1/StepY','After',num2str(get(handles.sliderY,'value')/10000));

function SimulateButton_Callback(hObject, eventdata, handles)

% hObject handle to SimulateButton (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA)

model_open(handles);

set_param(‘son1', 'SimulationCommand,, 'start')

function SimulateButton_CreateFcn(hObject, eventdata, handles)

%hObject handle to SimulateButton (see GCBO)

%handles empty - handles not created until after all CreateFens called %them ma

set(hObject,'String’,'Simulate’');

% - Executes on button press in ExitButton

function ExitButton_Callback(hObject, eventdata, handles)

%hObject handle to ExitButton (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB % handles _ structure with handles and user data (see GUIDATA)

close(guil);

function ExitButton_CreateFcn(hObject, eventdata, handles)

%hObject handle to ExitButton (see GCBO)

% eventdata reserved - to be defined in a future version of MATLAB %handles empty - handles not created until after all CreateFcns called % them ma

Trang 37

Code của hàm tính toán góc nhiêng dựa vào thuật toán Kalman extern float angle;

extern float q_bias;

extern float rate; extern void state_update( const float q_m /* Pitch gyro measurement */ ); extern void kalman_update(

const float ax_m, /* X acceleration */

const float az_m /* Z acceleration */ ); #endif Ham kalman_update #include <math.h> [* * thời gian lấy mẫu */ static constfloat dt = ( 1024.0 * 256.0) / 8000000.0; /*

* Ma trận covariance , ma trận này được cập nhập sau mỗi chu kỳ tính

Trang 38

* gdc nhiéng * bias * tốc độ */ float angle; float q_bias; float rate; /*

* R là ma trận covarian của nhieỗ đo Trong trường hợp nay

* là ma trận 1x1 , chúng ta đặt là 0.3 rad ( thông số này đo được từ thực nghiệm) */ static const float R_angle = 0.3; /* * Q là ma trận 2x2 trình bày nhiễu quá trình */

static const float Q_angle = 0.001;

static const float Q gyro = 0.003;

X= [ angle, gyro_bias ]

Xdot = [ angle_dot, gyro_bias_dot ]

angle_dot = gyro - gyro_bias gyro_bias_dot =0 x %X% *%x *%

Pdot = A*P + P*¥A'+Q

Trang 39

* / void state_update( ) { [*

const float q_m /* Pitch gyro measurement */

const float q=q_m- q_bias; [* Pdot = A*P + P*A'+Q A'=[00, -1, 0] Q_angle và Q_gyro có trong ma trận Q **/ const float Pdot[2 * 2] = { Q_angle - P[O][1] - P[1][0], /* 0,0 */ -P[IIH /* 0,1 */ -P[IIH /#1,0#/ Q_gyro /* 1,1 */ b * * * A=[0-1,00] * * rate = q; pe

Trang 40

* C=[d(angle_m)/d(angle) d(angle_m)/d(gyro_bias) ] * =[10] */ void kalman_update(

const float ax_m, /* X acceleration */

const float az_m /* Z acceleration */

) {

/* tính góc đo và sai số ước lượng */

const float angle_m = atan2( -az_m, ax_m );

const float § angle_err = angle_m - angle; /* *(€_ 0 trình bày sự liên quan giữa giá trị đo và trạng thái ước lượng * * The C_1 trình bày sự liên quan giữa trạng thái đo với hệ số bias của gyro */ const float C_0=1; /* const float C_1=0; */ /* * PCt<2,1>= P<2,2> * C'<2,1> */

const float PCt_0 = C_O * P[0][0]; /* + C_1 * P[O][1] =0 */

Định dạng
Số trang	41
Dung lượng	6,37 MB