Cơ sở điện hạt nhân

Đại Học Quốc Gia Tp Hồ Chí Minh TRƯỜNG ĐẠI HỌC BÁCH KHOA PHẠM QUỐC HÙNG ĐIỀU KHIỂN HỆ THỐNG CAMERA ỔN ĐỊNH HAI TRỤC Chuyên ngành: Tự Động Hóa LUẬN VĂN THẠC SĨ TP HỒ CHÍ MINH, tháng 07 năm 2011 CƠNG TRÌNH ĐƯỢC HỒN THÀNH TẠI TRƯỜNG ĐẠI HỌC BÁCH KHOA ĐẠI HỌC QUỐC GIA TP HỒ CHÍ MINH Cán hướng dẫn khoa học : (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : (Ghi rõ họ, tên, học hàm, học vị chữ ký) Cán chấm nhận xét : (Ghi rõ họ, tên, học hàm, học vị chữ ký) Luận văn thạc sĩ bảo vệ HỘI ĐỒNG CHẤM BẢO VỆ LUẬN VĂN THẠC SĨ TRƯỜNG ĐẠI HỌC BÁCH KHOA, ngày tháng năm TRƯỜNG ĐẠI HỌC BÁCH KHOA CỘNG HOÀ XÃ HỘI CHỦ NGHIÃ VIỆT NAM KHOA ĐIỆN - ĐIỆN TỬ Độc Lập - Tự Do - Hạnh Phúc -oOo Tp HCM, ngày tháng năm 2010 NHIỆM VỤ LUẬN VĂN THẠC SĨ Họ tên học viên: Phạm Quốc Hùng Phái: Nam Ngày, tháng, năm sinh: 22/10/1986 Nơi sinh: Tây Ninh Chuyên ngành: Tự Động Hóa MSHV: 09150039 1- TÊN ĐỀ TÀI: ĐIỀU KHIỂN HỆ THỐNG CAMERA ỔN ĐỊNH HAI TRỤC 2- NHIỆM VỤ LUẬN VĂN: -Mơ hình hóa hệ thống camera ổn định hai trục -Thiết kế luật điều khiển phù hợp chất lượng tốt -Mô khảo sát luật điều khiển -Chế tạo mơ hình thực, nhúng giải thuật vào mơ hình dùng vi xử lý DSPF28335 3- NGÀY GIAO NHIỆM VỤ : 4- NGÀY HOÀN THÀNH NHIỆM VỤ : 5- HỌ VÀ TÊN CÁN BỘ HƯỚNG DẪN : TS NGUYỄN VĨNH HẢO Nội dung đề cương Luận văn thạc sĩ Hội Đồng Chuyên Ngành thông qua CÁN BỘ HƯỚNG DẪN CHỦ NHIỆM BỘ MÔN (Họ tên chữ ký) QUẢN LÝ CHUYÊN NGÀNH (Họ tên chữ ký) KHOA QL CHUYÊN NGÀNH (Họ tên chữ ký) Lời Cảm Ơn Em xin chân thành cảm ơn thầy Nguyễn Vĩnh Hảo tận tình giúp đỡ hướng dẫn em học tập suốt trình thực luận văn, ý kiến q báu thầy giúp em học tập khắc phục nhiều thiếu sót để hồn thành luận văn Chân thành cảm ơn q thầy (cơ) Khoa Điện – Điện Tử Trường Đại học Bách Khoa Tp Hồ Chí Minh tận tình truyền đạt kiến thức cho em suốt khóa học trường Cảm ơn bạn bè chia sẻ, trao đổi kiến thức học tập trình thực luận văn Cảm ơn gia đình người thân u ln tạo điều kiện, động viên, giúp đỡ chỗ tựa vững giúp em an tâm học tập vượt qua khó khăn thời gian qua Tp Hồ Chí Minh, tháng 07 năm 2011 Học viên thực PHẠM QUỐC HÙNG TÓM TẮT LUẬN VĂN Luận văn xuất phát từ thực tế, tính ổn định hệ thống camera hai trục Đề tài từ ý tưởng để có hệ thống camera ổn định phục vụ cho nhiều mục đích khác Đề tài biến ý tưởng xuất phát từ thực tế thành cơng việc cụ thể phân tích, mơ hình hóa hệ thống camera ổn định hai trục, mô phỏng, chế tạo mô hình camera hai trục từ đánh giá khảo sát hệ thống thực Ngoài đề tài cịn trình bày phương pháp điều khiển hệ thống camera hai trục sử dụng cảm biến IMU 9dof cụ thể phương pháp sau: - Phương pháp động lực học ngược - Phương pháp động lực học ngược bền vững - Phương pháp trượt Từ mơ khảo sát kết có từ phương pháp điều khiển Thực phần cứng hệ thống camera ổn định hai trục từ phần tiến gần đến mục đích ứng dụng sống MỤC LỤC Trang Các chữ viết tắt ký hiệu dùng luận văn i Hình bảng biểu luận văn iv CHƯƠNG 1: GIỚI THIỆU CHUNG 1.1 ĐẶT VẤN ĐỀ 1.2 CÁC CƠNG TRÌNH NGHIÊN CỨU LIÊN QUAN 1.3 MỤC TIÊU CỦA ĐỀ TÀI .5 1.4 PHẠM VI CỦA ĐỀ TÀI CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 HỆ THỐNG CAMERA ỔN ĐỊNH HAI TRỤC 2.2 LÝ THUYẾT CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHƯƠNG 3: MƠ HÌNH HĨA ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC 3.1 ĐộNG HọC MƠ HÌNH 23 3.2 ĐỘNG LỰC HỌC MƠ HÌNH .27 CHƯƠNG 4: GIỚI THIỆU PHẦN CỨNG HỆ THỐNG 4.1 CẢM BIẾN IMU VÀ PHƯƠNG PHÁP SỬ DỤNG 36 CHƯƠNG 5: ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MÔ PHỎNG 5.1 PHƯƠNG PHÁP ĐỘNG LỰC HỌC NGƯỢC 40 5.2 PHƯƠNG PHÁP ĐộNG LựC HọC NGƯợC BềN VữNG 49 5.3 ĐIềU KHIểN TRƯợT .57 CHƯƠNG 6: KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CÁC CHỮ VIẾT TẮT DÙNG TRONG LUẬN VĂN IMU : Inertial measurement unit DOF : Degree of freedom SISO : Single-Input and Single-Output MIMO : Multiple-input and multiple-output SF : Sliding Surface D-H : Denavit- Hartenberg i CÁC KÝ HIỆU DÙNG TRONG LUẬN VĂN […] : Tài liệu tham khảo (…) : Biểu thức toán học m : Khối lượng e : Sai số tín hiệu so với tín hiệu đặt kp, kd, ki : Hệ số điều khiển PID fdist : Nhiễu moment P0 : Mơ hình chuẫn (mơ hình danh định) PΔ : Mơ hình thực tế với sai lệch Δ so với mơ hình chuẫn fdist : Nhiễu moment S : Mặt trượt (sliding surface) d : Khoảng cách hai trục x d1= d2=0 a : Khoảng cách ngắn hai trục z αi : Góc hai trục zi-1 zi θi (rad) : Góc hai trục xi-1 xi Ji : Ma trận Jacobian khớp thứ i J wi : Ma trận Jacobian vận tốc góc khớp thứ i J vi : Ma trận Jacobian vận tốc dài khớp thứ i K : Động qi : Các biến khớp V : Thế g : Gia tốc trọng trường ii ri : Vector trọng lực thành phần rci : Vector trọng lực trọng tâm thành phần Fi : Lực tổng quát fdist : nhiễu moment iii HÌNH TRONG LUẬN VĂN Trang Hình 1.1: Hệ thống mơ hình APS-50 Hình 1.2: Hệ thống GM-12E .3 Hình 1.3: Hệ thống Pan and Tilt .3 Hình 1.4: Camera dùng để ghi hình cảnh hành động Hình 1.5: Bộ camera trang bị cho xe Hình 1.6: Camera trang bị cho tàu .6 Hình 2.1: Hệ thống camera ổn định hai trục .7 Hình 2.2: Tồn hệ thống camera ổn định hai trục Hình 2.3: Hệ thống vật lý Hình 2.4: Điều khiển vịng kín Hình 2.5: Hệ thống điều khiển tắc (a unit mass) .11 Hình 2.6: Hệ thống điều khiển với quỹ đạo mong muốn 12 Hình 2.7: Hệ thống có nhiễu moment .13 Hình 2.8: Mơ hình điều khiển bền vững 14 Hình 2.9: Biểu diễn hình học định nghĩa ổn định chuyển động 18 Hình 2.10: Sơ đồ hệ tùy động đơn giản 21 Hình 2.11: Đáp ứng miền thời gian hệ học đơn giản 25 Hình 2.12: Mặt phẳng pha hệ học đơn giản 25 Hình 2.13: Quỹ tích số lượng mặt phẳng pha minh họa gia tăng lượng theo thời gian 26 Hình 2.14: Sự thay đổi lượng tốc độ lượng theo thời gian 26 Hình 2.15: Hàm xác định dương 27 Hình 2.16:a) Trạng thái cân ổn định 32 iv CHƯƠNG ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MÔ PHỎNG ⎡ 20 ⎤ K =⎢ ⎥ ⎣ 20 ⎦ ⎡8 ⎤ psi = ⎢ ⎥ ⎣0 ⎦ G1=[25 0;0 25]; Khi nhiễu giá trị đặt Hình 5.18: Giá trị đặt mong muốn Khi khơng có nhiễu Trang 62 CHƯƠNG ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MÔ PHỎNG Hình 5.19: Đáp ứng góc thứ với luật điều khiển trượt khơng có nhiễu Trang 63 CHƯƠNG ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MƠ PHỎNG Hình 5.20: Đáp ứng góc thứ hai với luật điều khiển trượt khơng có nhiễu Khi có nhiễu giá trị đặt cho hai khớp Trang 64 CHƯƠNG ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MƠ PHỎNG Hình 5.21: Đáp ứng góc thứ với luật điều khiển trượt có nhiễu Trang 65 CHƯƠNG ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MƠ PHỎNG Hình 5.22: Đáp ứng góc thứ hai với luật điều khiển trượt có nhiễu Nhận xét: Đáp ứng hệ thống điều khiển phương pháp trượt phương pháp động lực học ngược bền vững tốt nhiều so với phương pháp động lực học ngược Thời gian đáp ứng nhanh hơn, vọt lố nhiễu không ảnh hưởng nhiều đến hệ thống phương pháp điều khiển động lực học ngược Trang 66 CHƯƠNG ÁP DỤNG CÁC PHƯƠNG PHÁP ĐIỀU KHIỂN CHO ĐỐI TƯỢNG CAMERA ỔN ĐỊNH HAI TRỤC VÀ KẾT QUẢ MƠ PHỎNG 5.4 KẾT QUẢ MƠ HÌNH THỰC Do yêu cầu đặt phải điều khiển hệ thống camera ổn định hai trục điều kiện dao động nhiều với tần số cao, góc dao động không lớn (-100 tới 100) Do thời gian nhiều để áp dụng lý thuyết vào thực tế, xin chọn phương pháp điều khiển PID độc lập hai trục, trục gồm có hai vịng (vịng vận tốc vịng vị trí) Sơ đồ khối Hình 5.23 Sơ đồ khối điều khiển mơ hình thực aaa Trang 67 KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN CHƯƠNG Chương KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN 6.1 KẾT LUẬN Đề tài phần giải vấn đề việc thiết kế hệ thống camera ổn định hai trục Từ việc nghiên cứu, mô hệ thống camera ổn định hai trục Tìm hiểu phương pháp điều khiển hệ thống camera ổn định hai trục có chất lượng tốt Đến việc chế tạo mơ hình thực, sử dụng cảm biến IMU 9dof để đo giá trị hai góc hệ thống cho giá trị có chất lượng tốt từ điều khiển Do vấn đề thời gian có hạn gặp nhiều khó khăn với phần cứng, nên kết thực tế mức chấp nhận Hạn chế đề tài: - Mơ hình thực hệ thống chưa chuẫn, vấn đề đầu tư ban đầu chưa chỗ Do không lường trước việc nhiễu cảm biến IMU nên sử dụng vật liệu sắt làm mô hình dẫn tới khơng thể sử dụng giá trị để điều khiển Nên sử dụng vật liệu khác khơng có từ tính gỗ, mica, nhơm để chế tạo mơ hình - Chưa áp dụng luật điều khiển mơ vào mơ hình thực để kiểm tra khảo sát luật điều khiển thực tế -Chưa khảo sát vấn đề thay đổi tải hệ thống ảnh hưởng đến chất lượng điều khiển 6.2 HƯớNG PHÁT TRIểN -Chế tạo lại mơ hình dùng vật liệu nhơm, gỗ mica để tránh tình trạng nhiễu cảm biến -Sử dụng camera đặt lên mơ hình để thu thập hình ảnh từ xử lý sau, đặt lên tàu, máy bay, xe cộ để thu hình, bám đối tượng với mục đích theo mong muốn Trang 68 CHƯƠNG KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN -Áp dụng luật điều khiển mô vào mô hình thực, từ so sánh kết mơ mơ hình thực -Khảo sát vấn đề thay đổi tải, nhiễu tác động đến mơ hình 6.2 LỜI KẾT Do thời gian có hạn gặp nhiều vấn đề khó khăn, phạm vi luận văn khơng tránh khỏi nhiều thiếu sót Vẫn cịn nhiều vấn đề cần nghiên cứu phát triển Trang 69 TÀI LIỆU THAM KHẢO [ 1] “Lý thuyết điều khiển tự động”, Nguyễn Thị Phương Hà, Huỳnh Thái Hoàng, Nhà xuất đại học Quốc gia Thành phố Hồ Chí Minh, 2005 [ 2] “Lý thuyết điều khiển đại”, Nguyễn Thị Phương Hà, Nhà xuất đại học Quốc gia Thành phố Hồ Chí Minh, 2009 [ 3] “Lý thuyết điều khiển nâng cao”, Nguyễn Doãn Phước, Nhà xuất khoa học kỹ thuật, 2007 [ 4] Yun: “An improved quaternion-based kalman filter for real-time tracking”, Xiaoping Mariano Lizarraga, Eric R Bachmann! and Robert B McGhee Naval Postgraduate School, 2003 [ 5] “Design, Implementation, and Experimental Results of a Quaternion-Based Kalman Filter for Human Body Motion Tracking” Xiaoping Yun, Fellow, IEEE, and Eric R Bachmann, Member, IEEE 2006 [ 6] “Quaternion-Based Extended Kalman Filter forDetermining Orientation by Inertial and MagneticSensing”Angelo M Sabatini, Senior Member, IEEE 2006 [ 7] “Inertial and magnetic trackingof limb segment orientation for inserting humans into synthetic enviroments “ by Eric Robert Bachmann December 2000 [ 8] “Visual Self-Calibration of Pan-Tilt Kinematic Structures”Bartosz Tworek, Alexandre Bernardino and Jos´e Santos-Victor [ 9] “Orientation Tracking for Humans and Robots Using Inertial Sensors” [ 10] E R Bachmann, I Duman, U Y Usta, R B McGhee, X P Yun, M J Zyda 1999 [ 11] “Digital Signal Processing using MatLab” Vinay K Ingle John G Proakis [ 12] “Introduction to ROBOTICS mechanics and control” John J Craig, Pearson Education International Trang 70 [ 13] “The agile eye: a high-performance three-degree-of-freedom camera-orienting device”, Clkment M Gosselin and Jean-Franqois Hamel, Dkpartement de Gknie Mkcanique, Universitk Lava1 [ 14] “Stable adaptive control of multivariable servomechanisms, with application to a passive line-of-sight stabilization system”, T H Lee, Member, IEEE, E Koh, and M K Loh [ 15] “Nonlinear Induced Disturbance Rejection in Inertial Stabilization Systems”, Bo Li, David Hullender, and Mike DiRenzo by Eric Robert Bachmann December 2000 [ 16] “ Self-Tuning Controller for Nonlinear Inertial Stabilization Systems” Bo Li and David Hullender [ 17] “ A Study on Robustness Property of Sliding-ModeControllers: A Novel Design and Experimental Investigations”, Kemalettin Erbatur, M Okyay Kaynak, Senior Member, IEEE, and Asif Sabanovic, Member, IEEE [ 18] “Generation of Structural Design Constraints for Spaceborne Precision Pointing Systems“ Gregory Becker, Ronald Cubalchini, Quang Tham, and John Anagnost Communications, Controls, and Navigation Laboratory Sensors and Electronics Systems Raytheon Systems Company El Segundo, [ 19] ” Equations of Motion for a Two-Axes Gimbal System”, YOON1JOHN SUNGPIL B LUNDBERG2, Dept of Aerospace Engineering, Auburn University Auburn, [ 20] “Nonlinear Robust Control For A Passive Line-of-Sight Stabilization System” H Ambrose, Z &U and R Johnso [ 21] Fuzzy Logic Control to Suppress Noises and Coupling Effects in a Laser Tracking System”Ying Bai, Hanqi Zhuang, and Zvi S Roth [ 22] “Robust Control Design for a Two-axis Gimbaled Stabilization System “Ho- Pyeong Lee and Inn-Eark Yoo, Agency for Defense Development, Daejeon 305-600, Republic of Korea Trang 71 PHỤ LỤC PHỤ LỤC Ma trận tổng quát: ⎡cos θi ⎢ sin θ i i −1 ⎢ iT = ⎢ ⎢ ⎣ − cos α i sin θi cos α i cos θi sin α i sin α i sin θi − sin α i cos θi cos α i cos θi ⎤ sin θi ⎥⎥ di ⎥ ⎥ ⎦ Ma trận chuyển đổi từ gốc sang tọa độ khớp thứ là: ⎡cos θ1 ⎢ sin θ ⎢ 1T = ⎢ ⎢ ⎣ − cos α1 sin θ1 cos α1 cos θ1 sin α1 sin α1 sin θ1 − sin α1 cos θ1 cos α1 cos θ1 ⎤ ⎡cos θ1 sin θ1 ⎥⎥ ⎢⎢ sin θ1 = ⎥ ⎢ ⎥ ⎢ ⎦ ⎣ 0 − sin θ1 cos θ1 −1 0 0⎤ 0⎥⎥ 0⎥ ⎥ 1⎦ Ma trận chuyển đổi từ khớp thứ sang khớp thứ hai là: ⎡cos θ ⎢ sin θ ⎢ 2T = ⎢ ⎢ ⎣ − cos α sin θ cos α cos θ sin α sin α sin θ − sin α cos θ cos α a2 cos θ ⎤ ⎡cos θ a2 sin θ ⎥⎥ ⎢⎢ sin θ = ⎥ ⎢ ⎥ ⎢ ⎦ ⎣ 0 sin θ − cos θ 0 Từ ta có ma trận chuyển đổi từ gốc tọa độ sang khớp thứ hai là: T = 01T 21T Trong I1 moment quán tính khớp thứ 0 ⎤ ⎡0.065 ⎢ I1 = ⎢ 0.069 ⎥⎥ 0.07 ⎦⎥ ⎣⎢ Tính thành phần ma trận quán tính D(q) Thành phần thứ Trang 72 0⎤ 0⎥⎥ 0⎥ ⎥ 1⎦ m J T vc1 J vc1 ⎡0 0⎤ ⎡0 0⎤ ⎢ ⎥ = m1 ⎢ ⎥ ⎢0 0⎥ 0 ⎣ ⎦⎢ ⎣ 0 ⎥⎦ ⎡0 ⎤ ⎡0 ⎤ = m1 ⎢ = m1 ⎢ ⎥ ⎥ ⎣0 ⎦ ⎣0 ⎦ Thành phần thứ hai: m2 J T vc J vc ⎡0 0⎤ ⎡0 0⎤ ⎢ = m2 ⎢ 0 ⎥⎥ ⎥ ⎢ ⎣0 0⎦ ⎢ ⎣ 0 ⎥⎦ ⎡0 0⎤ ⎡0 0⎤ = m2 ⎢ = m2 ⎢ ⎥ ⎥ ⎣0 0⎦ ⎣0 0⎦ Thành phần thứ ba: J wTc1 10 R 1I1 10 RT J wc1 ⎡cos θ1 − sin θ1 ⎤ ⎡ I111 ⎢ ⎢ T cosθ1 ⎥⎥ ⎢ R I1 R = ⎢ sin θ1 ⎢⎣ 0 ⎥⎦ ⎢⎢ −1 ⎣ I122 0 ⎤ ⎡ cosθ1 sin θ1 ⎤ ⎥ ⎥ ⎢⎢ 0 −1⎥⎥ ⎥ I1 ⎥ ⎢⎣ − sin θ1 cosθ1 ⎥⎦ 33 ⎦ ⎡ cos θ1I1 − sin θ1I133 ⎤ ⎡ cos θ1 sin θ1 ⎤ 11 ⎢ ⎥ I133 cos θ1 ⎥ ⎢⎢ = ⎢ I111 sin θ1 0 −1⎥⎥ ⎢ ⎥ ⎢ − sin θ cos θ ⎥⎦ − I122 1 ⎢⎣ ⎥⎦ ⎣ ⎡ ⎤ 2 sin(2θ1 )( I111 − I133 ) ⎥ ⎢ I111 cos θ1 + I133 sin θ1 ⎢ ⎥ 2 ⎢ sin(2θ1 )( I111 − I133 ) I111 cos θ1 + I133 sin θ1 ⎥ = ⎢ ⎥ ⎢ 0 I122 ⎥⎥ ⎢ ⎢⎣ ⎦⎥ Trang 73 J T wc1 1 11 T0 R I R J wc1 ⎡ 2 ⎢ I111 cos θ1 + I133 sin θ1 ⎢ ⎡0 ⎤ ⎢ =⎢ sin(2θ1 )( I111 − I133 ) ⎥ ⎣0 0⎦ ⎢⎢ ⎢ ⎣⎢ ⎡0 0⎤ ⎡0 I122 ⎤ ⎢ ⎥ ⎡ I122 =⎢ ⎥ ⎢0 0⎥ = ⎢ ⎣⎢0 0 ⎥⎦ ⎢1 0⎥ ⎢⎣ ⎣ ⎦ sin(2θ1 )( I111 − I133 ) I111 cos θ1 + I133 sin θ1 2 ⎤ ⎥ ⎥ ⎡0 0⎤ ⎥ ⎢⎢0 0⎥⎥ ⎥ ⎢1 0⎥⎦ I122 ⎥⎥ ⎣ ⎦⎥ 0⎤ ⎥ 0⎥⎦ Thành phần thứ tư: ⎡ cos θ1 cos θ ⎢ T R I 2 R = ⎢ sin θ1 cos θ ⎢⎣ − sin θ ⎡ cos θ1 cos θ ⎢ ⎢ − sin θ1 ⎢⎣ cos θ1 sin θ − sin θ1 cos θ1 sin θ ⎤ ⎡ I 211 ⎢ cos θ1 sin θ1 sin θ ⎥⎥ ⎢ 0 cos θ ⎥⎦ ⎢⎢ ⎣ I 222 0 ⎤ ⎥ ⎥ ⎥ I 233 ⎥⎦ sin θ1 cos θ − sin θ ⎤ cos θ1 ⎥⎥ sin θ1 sin θ cos θ ⎥⎦ ⎡ I cosθ cosθ ⎢ 211 ⎢ = I 211 sin θ1 cosθ ⎢ ⎢ − I sin θ2 11 ⎣ − I 222 sin θ1 I 233 cosθ1 sin θ ⎤ ⎡cosθ cosθ sin θ cosθ − sin θ ⎤ ⎥ 2 ⎢ ⎥ I 222 cosθ1 I 233 sin θ1 sin θ ⎥ ⎢ − sin θ1 cosθ1 ⎥ ⎥ ⎥ ⎢⎣ cosθ1 sin θ2 sin θ1 sin θ cosθ ⎥⎦ I cosθ 233 ⎦ ⎡ I211 cos2 θ1 cos2 θ2 + I222 sin2 θ1 + I233 cos2 θ1 sin2 θ2 I211 sinθ1 cosθ1 cos2 θ2 − I222 sinθ1 cosθ1 + I233 sinθ1 cosθ1 sin2 θ2 ⎢ = ⎢I211 sinθ1 cosθ1 cos2 θ2 − I222 cosθ1 sinθ1 + I233 sinθ1 cosθ1 sin2 θ2 I211 sin2 θ1 cos2 θ2 + I222 cos2 θ1 + I233 sin2 θ1 sin2 θ2 ⎢ −I211 sinθ2 cosθ1 cosθ2 + I233 cosθ1 sinθ2 cosθ2 −I211 sinθ1 cosθ2 sinθ2 + I233 cosθ2 sinθ1 sinθ2 ⎢⎣ −I211 cosθ1 cosθ2 sinθ2 + I233 cosθ1 sinθ2 cosθ2 ⎤ ⎥ −I211 sinθ1 sinθ2 cosθ2 + I233 sinθ1 sinθ2 cosθ2 ⎥ ⎥ I211 sin2 θ2 + I233 cos2 θ2 ⎥⎦ J T wc 2 R I 22 T0 R J wc 1⎤ T ⎡ =⎢ ⎥ R I2 R ⎣ − sin θ1 cos θ1 ⎦ ⎡0 − sin θ1 ⎤ 2 ⎢0 cos θ ⎥ ⎡ I 211 sin θ + I 233 cos θ ⎥⎢ ⎢ ⎢⎣1 ⎥⎦ ⎣⎢ Với i=1 2 ⎛ ∂D1 j ∂D jk C1 = ∑∑ ⎜ − ∂θ1 j =1 k =1 ⎝ ∂θ k Với j=1 k=1 Trang 74 ⎞& & ⎟θ jθ k ⎠ ⎤ ⎥ I 222 ⎦⎥ ⎛ ∂D11 ∂D11 ⎞ & & − ⎜ ⎟ θ1θ1 = ⎝ ∂θ1 ∂θ1 ⎠ k=2 ⎛ ∂D11 ∂D12 ⎞ & & − ⎜ ⎟θ1θ = ( I 211 − I 233 )sin(2θ )θ&1θ&2 θ θ ∂ ∂ ⎠ ⎝ j=2 k=1 ⎛ ∂D12 ∂D21 ⎞ & & − ⎜ ⎟ θ1θ = ⎝ ∂θ1 ∂θ1 ⎠ k=2 ⎛ ∂D12 ∂D22 ⎞ & & − ⎜ ⎟ θ 2θ = ⎝ ∂θ 2 ∂θ1 ⎠ C1 = ( I 211 − I 233 )sin(2θ )θ&1θ&2 i=2 2 ⎛ ∂D2 j ∂D ⎞ & & C2 = ∑∑ ⎜ − ⎟θ jθ k ∂θ ⎠ j =1 k =1 ⎝ ∂θ k j=1 k=1 ⎛ ∂D21 ∂D11 ⎞ & & − ⎜ ⎟θ1θ1 = − ( I 211 − I 233 )sin(2θ )θ&1 ⎝ ∂θ1 ∂θ ⎠ k=2 ⎛ ∂D21 ∂D12 ⎞ & & − ⎜ ⎟ θ1θ = ∂ θ ∂ θ ⎝ 2 ⎠ j=2 k=1 ⎛ ∂D22 ∂D21 ⎞ & & − ⎜ ⎟ θ1θ = ⎝ ∂θ1 ∂θ ⎠ k=2 Trang 75 ⎛ ∂D22 ∂D22 ⎞ & & − ⎜ ⎟ θ 2θ = ⎝ ∂θ 2 ∂θ ⎠ C2 = − ( I 211 − I 233 )sin(2θ )θ&12 Tính tốn thán phần trọng lực mơ hình G1 = −∑ m j g T J vcj = − ( m1 g T J vc1 + m2 g T J vc1 ) j =1 ⎡ ⎤ ⎡0 ⎤ ⎡ ⎤ ⎡0⎤ ⎢ ⎥ ⎢ ⎥ = −m1 ⎢ ⎥ ⎢0 ⎥ − m2 ⎢⎢ ⎥⎥ ⎢⎢0 ⎥⎥ = ⎢⎣ − g c ⎥⎦ ⎢⎣0 ⎥⎦ ⎢⎣ − g c ⎥⎦ ⎢⎣0 ⎥⎦ 2 G2 = −∑ m j g T J vcj = − ( m1 g T J vc2 + m2 g T J vc2 ) j =1 ⎡ ⎤ ⎡0 ⎤ ⎡ ⎤ ⎡0⎤ = −m1 ⎢⎢ ⎥⎥ ⎢⎢0 ⎥⎥ − m2 ⎢⎢ ⎥⎥ ⎢⎢0 ⎥⎥ = ⎢⎣ − g c ⎥⎦ ⎢⎣0 ⎥⎦ ⎢⎣ − g c ⎥⎦ ⎢⎣0 ⎥⎦ Trang 76 ... Chương 2: Cơ sở Lý Thuyết Chương 3:Mơ hình hóa đối tượng camera ổn định hai trục Chương 4: Phần cứng hệ thống Chương 5: Kết mô thực tế Chương 6: Kết luận hướng phát triển Trang CHƯƠNG CƠ SỞ LÝ THUYẾT... CHUNG 1.1 ĐẶT VẤN ĐỀ 1.2 CÁC CƠNG TRÌNH NGHIÊN CỨU LIÊN QUAN 1.3 MỤC TIÊU CỦA ĐỀ TÀI .5 1.4 PHẠM VI CỦA ĐỀ TÀI CHƯƠNG 2: CƠ SỞ LÝ THUYẾT 2.1 HỆ THỐNG CAMERA ỔN ĐỊNH... kín vị trí mong muốn Trang CHƯƠNG CƠ SỞ LÝ THUYẾT 2.2.1.1 Luật điều khiển thành phần (control-law partitioning) Chúng ta chia điều khiển thành hai phần phần sở mơ hình (modelbased portion) phần