Luận án Nghiên cứu điều khiển rô bốt tay máy di động bám mục tiêu trên cơ sở sử dụng thông tin hình ảnh

, ε ε 0 n u t c đ các khớp b Pan-Tilt được điều khiển theo thu t toán (2.25), (2.26), (2.27) và lu t học (2.28) như au: ˆ ˆ ˆ + , + + , d 1 d 1q J [(m - Kε)+u ] J (m -Kε)+J u 0 1= u +u (2.25) 0 ˆ + du = J m -Kε (2.26) 1 ˆ , 1 + , 1 1 ε u = ( )Wσ - ε u = J u (2.27) T W εσ (2.28) Trong đó các tham số tự chọn K là ma trận đối xứng xác định dương T K = K > 0 các hệ số 1, 0   . Cấu trúc của hệ điều khiển có thể mô tả theo sơ đồ trên hình 2.6. Tín hiệu q gồm hai thành phần chính: 0u là thành phần phản hồi, 1u là thành phần có mạng nơron với thuật học on-line để bù các thành phần bất định. Phương pháp điều khiển này được chứng minh b ng phương pháp ổn định Lyapunov đảm bảo tính ổn định tiệm cận toàn cục của cả hệ thống như sau: 34 Hình 2.6 Cấu trúc của hệ visual servoing điều khiển camera bám mục tiêu di động có nhiều tham số bất định Chứng minh: Ta chọn hàm ứng tuyển Lyapunov xác định dương như sau: 3 1 1 2 i i i V T Tε ε w w (2.29) Trong đó iw là véc tơ cột thứ i của ma trận trọng W. Ta có V > 0 khi , i ε w 0 ; V = 0 khi và chỉ khi , i ε w 0 i=1,2,3; V  khi , i ε w . Lấy đạo hàm V theo t ta có: 3 T i i i 1 V Tε ε w w (2.30) Thay (2.21) vào (2.30), đạo hàm V có dạng: 3 T i i 1i V T T , 1 ε Kε ε u f w w (2.31) Với thuật học on-line (2.28) ta có i i  w ; i = 1, 2,3; (2.32) Và ta có thể xác định được: 3 3 3 T T T T i i i i i 1 1 1 i i i i w w w ε ε w ε Wσ (2.33) Thay (2.33),(2.23) và (2.24) vào (2.31) ta có: 35 + ( 1)V T T , 1 ε Kε ε u Wσ β (2.34) Thay ( 1) - , 1 ε u Wσ ε từ (2.27) vào (2.34) ta nhận được : 0 ( ) . V T T T T ε ε Kε ε β ε Kε ε ε ε β ε Kε ε ε (2.35) Nếu chọn 0 0 ; 0  ta có: 0- + -ε ε ε (2.36) Thay (2.36) vào (2.35) ta nhận được: 0V Tε Kε ε (2.37) Ta thấy 0V  khi ε 0 và 0V  khi và chỉ khi ε 0 .Theo nguyên lý ổn định Lyapunov ta có sai lệch tốc độ ε 0 , ε 0 . Như vậy hệ (2.21) là ổn định tiệm cận và do đó ( )t dm m , ( )t dm m hay nói cách khác camera bám theo mục tiêu di động với sai lệch b ng 0. Phương pháp điều khiển 1 cũng như tính ổn định tiệm cận toàn cục của hệ stereo camera bám mục tiêu s dụng mạng nơ ron mô tả trong hình 2.6 đã được chứng minh. 2.3 Một số kết quả mô phỏng kiểm chứng Kích thước ảnh trong mô phỏng được lấy theo kích thước ảnh chu n của camera EyeRIS 2.1 có kích thước ảnh là 176x144 pixels. Trong đó 1 pixel ≈ 35µm. Sai số trong mô phỏng được tính theo đơn vị pixel. Các kết quả mô phỏng đã được công bố trong mục [cb6] và [cb2] trong mục các công trình công bố. Tiến hành mô phỏng hệ thống trên matlab simulink với ˆ  J là ma trận giả nghịch đảo của Jˆ với Jˆ là phần ước lượng được của ma trận Jacobi tổng hợp J (2.16b). Ta giả thiết chỉ ước lượng được 80% giá trị của J tức là: 36 2 2( ) ( ) 2 2 1 2 2 2 1 1 12 2 2( ) ( ) ( ) ˆ 0.8 2 1 2 2 2 1 1 12 2 ( ) ( ) 2 1 2 2 2 1 1 U U U U U U U U U V f U U U R L L L R L L R L L L L Rl C C l S V l S C LK fK fK f f U U V U U U U V U U f V V U U R L L L R L R L L R L L L Rl S C l S l S C K fK f fK f f U U U U U U U U U V R L R L R R L R R Ll C C l S V l S LK fK fK f J 2 22 12 f U U U R L R C f (2.38) Ký hiệu 1 1 2 2 3 3 ˆ a b a b a b J thì ma trận giả nghịch đảo tính được như sau: 3 31 1 2 2 T 1 T 3 31 1 2 2 ˆ ˆ ˆ ˆ( ) a C b Ba C b B a C b B M M M a B b Aa B b A a B b A M M M J J J J (2.39) Trong đó: 2 2 2 1 2 3 1 1 2 2 3 3 2 2 2 1 2 3 2 2 2 1 2 2 1 1 3 3 1 2 3 3 2 ( ) ( ) ( ) A a a a B a b a b a b C b b b M a b a b a b a b a b a b (2.40) Chọn các tham số tự chọn: 1.5; 0.2 (2.41) 2 1 1 ( 1) exp 25 (2.42) 2 2 2 ( 2) exp 9 (2.43) 2 3 3 ( 0.5) exp 16 (2.44) K = 0.5I với I là ma trận đơn vị Và các thông số của hệ pan-tilt camera là: 37 l1= 0.22 m l2= 0.1 m K= 0.2 m f= 360 pixel UL, VL, UR, VR: là tọa độ ảnh trái và phải có đơn vị tính là pixel. Tọa độ ảnh ban đầu: m = [ UL, VL, UR ] = {{0, 0, 0] (2.45) Mô phỏng 1: Mục tiêu cố định Tâm camera thời điểm ban đầu là: m(0) =[-40, 30, 0] (pixel); tọa độ ảnh của mục tiêu đứng yên mt=[-20, 0, 20] (pixel); Tiến hành mô phỏng với sơ đồ như trong hình 2.6 ta thu được một số kết quả sau: Hình 2.7 Sai lệch tọa độ ảnh mục tiêu (pixel). Hình 2.8 Sai lệch tọa độ ảnh mục tiêu (pixel) khi bộ điều khiển không có mạng nơ ron bù ( , 1u = 0 ). 38 Mô phỏng 2: Mục tiêu di chuyển theo đƣờng thẳng Điểm mục tiêu đi từ điểm A có tọa độ A(0m,1.8m,0m) tới điểm B(-0.3m, 1.8m, -0.5m) trên mặt phẳng ZCOCXC và cách gốc tọa độ một khoảng YC = 1.8m trong hệ tọa độ camera OCXCYCZC. Thời gian chuyển động của mục tiêu T=30(s) với tốc độ di chuyển đều v ~ 2 (cm/s). Tiến hành mô phỏng với sơ đồ như trong hình 2.6 ta thu được một số kết quả sau: Hình 2.9 Sai lệch bám tọa độ khi mục tiêu di chuyển theo đường thẳng. Hình 2.10 Sai lệch tọa độ ảnh khi mục tiêu di chuyển theo đường thẳng Sai lệch bám tọa độ hình 2.9 cho ta thấy trong không gian 3D hệ pan-tilt camera bám mục tiêu với sai lệch nhỏ. Nhưng trên hình 2.10 các sai lệch đó được thể hiện cụ thể trên từng pixel ảnh thì các sai lệch đó chưa triệt tiêu. Điều này có thể do tốc độ bám của hệ pan-tilt theo mục tiêu còn chưa đáp ứng tốt. 39 Hình 2.11 Sai lệch bám tọa độ khi mục tiêu di chuyển theo đường thẳng với bộ điều khiển không có mạng nơ ron bù ( , 1u = 0 ). Hình 2.12 Sai lệch tọa độ ảnh khi mục tiêu di chuyển theo đường thẳng với bộ điều khiển không có mạng nơ ron bù Sai lệch bám tọa độ hình 2.11 cho ta thấy trong không gian 3D hệ pan-tilt camera vẫn di chuyển đúng theo hướng của mục tiêu cho dù các sai số nhìn thấy là rõ ràng. Tuy vậy trên hình 2.12 các sai lệch đó có xu hướng bất ổn và tăng thời điểm cuối của mô phỏng. Điều này có thể do các sai số không được triệt tiêu mà tích lũy lại gây ra. Mô phỏng 3: Mục tiêu cơ động theo cung tròn Mục tiêu đi theo cung tròn có tọa độ tâm tại gốc tọa độ O(0,0) và bán kính r = 1 trên mặt phẳng ZCOCXC và cách gốc tọa độ một khoảng YC = 1.8m trong hệ tọa độ camera OCXCYCZC. Chuyển động của mục tiêu theo cung tròn đi từ điểm A(0m,1.8m, 1m) tới điểm B(1m,1.8m,0m) với thời gian T = 30s. Tốc độ v ~ 3 (cm/s). Tiến hành mô phỏng với sơ đồ như trong hình 2.6 ta thu được kết quả sau: 40 Hình 2.13 Sai lệch bám quỹ đạo khi mục tiêu cơ động theo cung tròn Hình 2.14 Sai lệch tọa độ ảnh khi mục tiêu cơ động theo cung tròn Hình 2.13 cho thấy hệ bám theo mục tiêu di chuyển theo cung tròn khá tốt các sai lệch được triệt tiêu dần và sai lệch giảm dần về 0. Tuy nhiên tốc độ hội tụ còn chậm. Trong thực tế có thể giảm thời gian đáp ứng của hệ thống nhưng khi đó hệ sẽ kém ổn định hơn. Với bộ điều khiển s dụng mạng nơ ron với thuật học on-line thì khi sai số càng lớn hệ sẽ học và đưa ra các tín hiệu điều khiển để giảm sai lệch càng nhanh nhưng khi sai lệch nhỏ thì các đáp ứng chậm hơn. Nhưng nếu không có thành phần mạng nơ ron trong bộ điều khiển thì các sai lệch sẽ tích tụ dần như trong hình 2.15 và càng ngày càng lớn hơn khiến cho hệ mất bám, điều này thể hiện rõ hơn trong mô phỏng thứ 4 khi mục tiêu di chuyển đổi hướng liên tục theo cung tròn và có tốc độ cũng thay đổi khi nhanh khi chậm. 41 Hình 2.15 Sai lệch bám quỹ đạo với mục tiêu cơ động theo cung tròn khi bộ điều khiển không có mạng nơ ron bù Hình 2.16 Sai lệch tọa độ ảnh với mục tiêu cơ động theo cung tròn khi bộ điều khiển không có mạng nơ ron bù ( , 1u = 0 ). Mô phỏng 4: Mục tiêu di chuyển theo cung tròn có vận tốc tha đổi Mục tiêu đi theo cung tròn tương tự trong mô phỏng 3 nhưng trong 1/6 cung đầu vận tốc tăng dần đều với gia tốc 1cm/s2 sau 3 giây nó chuyển động đều với vận tốc 3cm/s. Tới 1/6 cung tròn cuối nó chuyển động vận tốc giảm dần đều với gia tốc - 1cm/s 2 . Tiến hành mô phỏng với sơ đồ như trong hình 2.6 ta thu được kết quả sau: 42 Hình 2.17 Sai lệch bám quỹ đạo khi mục tiêu di chuyển với vận tốc thay đổi. Hình 2.18 Sai lệch tọa độ ảnh khi mục tiêu di chuyển với vận tốc thay đổi. Hình 2.18 cho thấy các sai lệch tăng đột biến các thời điểm tốc độ di chuyển của mục tiêu thay đổi (tại giây thứ 3 và giây thứ 51) nhưng các sai lệch này nhanh chóng bị triệt tiêu, hệ dần quay tr lại trạng thái ổn định. Với hệ thống mà bộ điều khiển không có thành phần mạng nơ ron thì ngay tại giây thứ 3 khi mục tiêu tăng tốc đã xảy ra hiện tượng mất bám do sai số rất lớn. 43 So sánh kết quả mô phỏng và đánh giá Kết quả mô phỏng cho thấy hệ thống với bộ điều khiển có mạng nơ ron bù các đại lượng bất định , 1 u có thời gian đáp ứng và sai lệch ít hơn so với hệ có bộ điều khiển không có mạng nơ ron bù . Đối với mục tiêu cố định (hình 2.7) và chuyển động thẳng (hình 2.9) ta thấy sai lệch tiệm cận về không. Khi mục tiêu cơ động theo cung tròn độ bám cho kết quả tốt (hình 2.13). Do các tham số liên tục được mạng nơ ron học bù nên tác động của các nhi u và các tham số bất định được kh , hệ thống giữ được trạng thái ổn định và sai lệch bám triệt tiêu. Với mục tiêu di chuyển có vận tốc thay đổi (hình 2.17) hệ thống vẫn có khả năng bám tốt. Khi vật chuyển động có gia tốc hệ thống vẫn bắt bám. Khi vật chuyển sang chuyển động đều, mới đầu sai lệch hệ bám thay đổi tăng, nhưng nhanh chóng bám lại được. Cuối cung tròn, vật chuyển động chậm dần đều nhưng sai lệch của hệ bám thay đổi ít và bị triệt tiêu nên độ bám hầu như không bị ảnh hư ng. Hệ thống với bộ điều khiển không có mạng nơ ron có thể bám mục tiêu cố định (hình 2.8) và mục tiêu chuyển động theo đường thẳng (hình 2.11) nhưng có sai lệch lớn. Tuy nhiên khi mục tiêu cơ động với cung tròn (hình 2.15) thì kết quả bám có sai lệch lớn thậm chí mất bám giai đoạn cuối. 2.4. Kết luận chƣơng 2 Chương 2 đã đề xuất một phương pháp mới cho hệ servo thị giác stereo s dụng mạng nơ ron với luật học on-line trong bộ điều khiển để bù cho các tham số bất định không biết rõ như mô men quán tính, ma trận Jacobi, ma sát các khớp, nhi u tác động vv giúp cho hệ thống hoạt động ổn định và sai lệch bám triệt tiêu. Thụât toán điều khiển hệ thống có tính tự thích nghi cao và có khả năng chống được các nhi u tác động lên hệ thống. Độ ổn định toàn cục của hệ thống được chứng minh b ng thuyết ổn định Lyapunov. Các mô phỏng với độ bất định lên đến 20% khi mục tiêu cố định, chuyển động theo đường thẳng hay cơ động đổi hướng liên tục theo cung tròn đều cho kết quả ổn định, sai lệch bám tiếp cận về 0, phù hợp với kết quả lý thuyết. Tính khả thi của việc cài đặt ứng dụng mạng nơ ron này lên các microcontroller là hoàn toàn khả thi. Do luật học cập nhật liên tục được thực hiện chỉ với một số phép 44 nhân và tích phân (cộng dồn) nên khối lượng tính toán không quá lớn và mặt khác các chip ví x lý 32 và 64 bit hiện nay có tốc độ x lý ngày càng cao nên tốc độ x lý sẽ không là rào cản cho việc ứng dung trong thực tế sau này. Các kết quả của nghiên cứu chương này đã được công bố tại hội thảo [cb6] và tạp chí Tin học và điều khiển học [cb2] trong mục các công trình công bố. 45 CHƢƠNG 3 MỘT SỐ CẢI TIẾN TRONG ĐIỀU KHIỂN HỆ SERVO TH GIÁC BÁM MỤC TIÊU DI ĐỘNG Như trong chương 2 tác giả đã nhận thấy, để điều khiển hệ pan-tilt thị giác stereo camera bám mục tiêu di động hoạt động được tốt hơn nữa thì bài toán điều khiển có một số vấn đề cần giải quyết như sau: - Thứ nhất là cần xây dựng được ma trận Jacobian ảnh vuông để khi thực hiện nghịch đảo và các biến đổi khác không xuất hiện các điểm kỳ dị dẫn tới mất bám. - Thứ hai là xây dựng bộ điều khiển động lực học kết hợp với mạng nơ ron để bù ảnh hư ng của các tham số bất định bên trong mô hình cũng như nhi u bên ngoài. - Thứ ba là tối ưu một số tham số trong mạng nơ ron để thu được kết quả đầu ra tốt hơn. Trong các hệ điều khiển rô bốt có s dụng camera [21], [43] hay [18], [37], [29], [81] để bám mục tiêu di động thì ma trận Jacobian ảnh Jimg đóng một vai trò quan trọng cụ thể trong các kết quả nghiên cứu [18], [59], [65], [49]. Nhưng trong hầu hết các nghiên cứu về bám mục tiêu di động có s dụng thị giác stereo camera thì ma trận Jacobian ảnh là không vuông. Đây là một trong những nguyên nhân gây nên những điểm mất bám của hệ thống theo dõi mục tiêu. Để cải tiến điều này, Một số tác giả cũng đã xây dựng ma trận Jacobian ảnh đầy đủ (vuông) cho hệ stereo camera cố định để quan sát mục tiêu trong khoảng không gian cho trước [20]. Trong chương này tác giả đi xây dựng ma trận Jacobian ảnh đầy đủ cho hệ stereo camera trên hệ pan-tilt và tìm thuật điều khiển để hệ thống bám được mục tiêu. Thuật toán điều khiển rô bốt di động cũng cần được quan tâm. Một số tác giả s dụng phương pháp động học [16], [19], [29], trong khi một số tác giả khác s dụng phương pháp động lực học [20], [64]. Trong những năm gần đây nhiều tác giả đã phát triển rất nhiều phương pháp điều khiển khác nhau cho hệ rô bốt camera bám mục tiêu di động. Từ bộ điều khiển cổ điển PID [37], tới những bộ điều khiển hiện đại như điều 46 khiển trượt [20], điều khiển thích nghi [59], [34], hay kết hợp với mạng nơ ron như [21], [33], [25], [83] Hay bộ điều khiển mờ [66]. Để có kết quả tốt hơn, gần đây các tác giả còn đi thực hiện tối ưu các tham số trong các bộ điều khiển nói trên [27], [40]. Trong chương này, tác giả đi xây dựng bộ điều khiển nơ ron cho hệ hệ pan-tilt stereo camera bám mục tiêu di động trong đó có chú ý tới các yếu tố bất định bên trong mô hình cũng như ảnh hư ng của nhi u. Mạng nơ ron truyền thẳng RBF được tác giả s dụng cùng với thuật học online và các tham số được tối ưu để cho ra kết quả tốt hơn. Trong hầu hết các nghiên cứu thì phương pháp ổn định Lyapunov cũng như bổ đề Barbalat s lemma được s dụng nhiều trong các chứng minh sự ổn định của toàn hệ thống [20], [37]. Tuy nhiên phương pháp Hamiltonian cũng là một phương pháp ổn định có thể s dụng [18]. Trong chương 1 và nghiên cứu trước của tác giả , mà trận Jacobian ảnh là không vuông do vậy nó gây nên những điểm kỳ dị khi lấy giả nghịch đảo. Điều này được nhận thấy qua thực nghiệm khi các mô phỏng bám mục tiêu đều không đi qua được điểm - khi mục tiêu đi theo cung tròn. Mặt khác trong chương1 bộ điều khiển được dùng cho hệ động học nên không thể theo dõi kiểm soát được l i bám về tốc độ cũng như các tham số động lực học của rô bốt, các tham số bất định trong mô hình động lực học rô bốt cũng như ảnh hư ng của nhi u. Kết quả mô phỏng bám mục tiêu trong chương 1 cũng chưa thực sự thuyết phục khi các l i bám mục tiêu chưa hội tụ về 0. Trong chương này, vấn đề bám mục tiêu được tiếp cận theo một vài hướng mới đó là: Thứ nhất là tác giả đi xây dựng mô hình 3D cho hệ stereo camera để thu được ma trận Jacobian ảnh là đầy đủ (vuông). Thứ hai, xây dựng mô hình động lực học cho hệ hệ pan-tilt stereo camera. Thứ ba là xây dựng bộ điều khiển nơ ron với thuật học online để giảm sự ảnh hư ng của nhi u và sự bất định của các tham số trong mô hình động lực học. Trong phần 3.1 tác giả đi xây dựng mô hình 3D cho hệ 2 camera để có được ma trận Jacobian đầy đủ. Trong phần 3.2, tác giả xây dựng bộ điều khiển động lực học s dụng mạng nơ ron với các tham số được tối ưu, sự ổn định của hệ được chứng minh b ng phương pháp Lyapunov và bổ đề Barballat. Phần 3.3 là các kết quả mô phỏng. Cuối cùng là một số kết luận. 47 3.1 Xây dựng mô hình 3D cho hệ 2 camera trên hệ pan-tilt 3.1.1 hình 3 cho h stereo camera Mô hình 3D cho hệ stereo camera được xây dựng b ng cách tạo nên một hệ camera ảo trong đó dùng các phép biến đổi hình học để đưa ảnh trong hệ tọa độ camera thực sang hệ hai camera ảo. Trong hệ camera ảo này các thành phần tọa độ ảnh được biến đổi và trích rút các đặc trưng sao cho không mất đi các thông tin tọa độ ảnh đồng thời tạo ra ma trận Jacobian đầy đủ 3x3. Trong hệ thống này, rô bốt di động Pioneer 3DX là một rô bốt di chuyển b ng 3 bánh xe 2 bánh dẫn động, 1 bánh dẫn hướng còn bệ pan-tilt PTU-D48E là một rô bốt hai bậc tự do có thể quay đồng thời theo hai hướng pan và tilt, góc pan thay đổi được từ -1880 tới +1880, góc tilt từ -900 tới +300. Các khung tọa độ được gắn trên các khớp rô bốt và camera được mô tả trong hình 3.1. 3.1.2 hình h cam ra o. Quan hệ giữa hệ camera thực và camera ảo được biểu di n cụ thể trong hình 3.1. Trong đó hệ tọa độ của camera trái và camera phải được đặt lần lượt là LLLL ZYXO , RRRR ZYXO với gốc của hệ được đặt tại tâm thấu kính của m i camera. Hệ tọa độ chung của hệ stereo camera là cccc ZYXO với gốc được đặt tại trung điểm của đường nối hai gốc tọa độ camera trái và phải. Ta quy ước các thông số của hai camera là giống nhau như tiêu cự f số điểm ảnh...v.v. Giả s hai camera được đặt trên hệ pan-tilt là có cùng độ cao và song song với nhau. Gọi góc pan là θ1, trục xoay của góc pan là xung quanh trục Z0 trong hệ tọa độ gốc của hệ pan-tilt, góc tilt là θ2, trục xoay của góc pan là xung quanh trục Z1 trong hệ tọa độ 1111 ZYXO của hệ pan-tilt. 48 Hình 3.1 Hệ trục tọa độ của hệ Pan-tilt – stereo cameras Ta gọi tọa độ ảnh thu được b i camera trái là (UL, VL) và tọa độ ảnh thu b i camera phải (UR, VR) theo hai trục (U, V). Theo quy ước về độ cao của hai camera là giống nhau trên thì tọa độ ảnh theo trục V của hai camera là như nhau hay VR = VL = V. Từ hình 3.1, ta có điểm đặc trưng ảnh mục tiêu trên camera trái (UL , VL) và camera phải (UR, VR) được chiếu lên hệ tọa độ phẳng (Z, Y) và (X, Y) như biểu di n trong hình 3.2. Tiếp theo hệ tọa độ camera 3D ảo được xây dựng theo các bước như sau: ước 1, Từ quan hệ hình học giữa mục tiêu và đặc trưng ảnh trên hai camera ta tính được tọa độ của mục tiêu trong không gian 3D  Tc zyxx nhìn trong hệ trục tọa độ cccc ZYXO được tính theo công thức (3.1): (3.1) Với K là khoảng cách giữa 2 trục camera OL và OR. là tiêu cự ống kính camera. ước 2, Ta định nghĩa một hệ tọa độ liên kết Có gốc tọa độ trùng với hệ tọa độ . Để chuyển đổi từ hệ tọa độ OC tới OV, ta s dụng ma trận chuyển tọa độ (hình 3.2) [20].     , 2 . . 1                LR L LR T c UU K fK VK UU zyxx L Rf f f vvvv ZYXO cccc ZYXO v CR 49 Hình 3.2 Mô hình hệ camera 3D ảo. Tọa độ của mục tiêu nhìn trong hệ tọa độ thì nhìn trong hệ tọa độ camera ảo là (Cai et al., 2013 [20]): cC v v R xx  , (3.2) Với: o C v R 90; cossin0 sincos0 001                . (3.3) Mục tiêu của hệ tọa độ ảo Ov là để làm đơn giản hóa mô hình máy ảo trong một hướng cụ thể của trục y và z khi xác định hai khung máy ảo trên bước tiếp theo.  Tc zyxx cccc ZYXO vvvv ZYXO   T vvvv yxzx 50 Hình 3.3 Tọa độ mục tiêu nhìn theo hai trục tọa độ Y và Z Hình 3.4 Tọa độ mục tiêu nhìn theo hai trục tọa độ Y và Z ước 3, Hệ tọa độ camera ảo vvvv ZYXO được s dụng để định nghĩa ra hai hệ tọa độ 1111 vvvv ZYXO , 2222 vvvv ZYXO của hệ camera ảo. Gốc của hai hệ tọa độ được đặt trên hai trục Xv và Zv đặt xa gốc Ov một khoảng λ. Tọa độ của gốc tọa độ Ov1 và Ov2 nhìn trong hệ tọa độ vvvv ZYXO được định nghĩa như sau:  Tvv xz ; ;   T vv xz ; . (3.4) 51 ước 4, Mô hình camera ảo được kết hợp với mô hình camera trực quan 3D để tạo ra không gian 3D Cartesian ảo có tọa độ điểm đặc trưng của mục tiêu được biểu di n trong hệ tọa độ ảo là: . Mô hình camera pinhole (Shibata et al., 2011 [49]) được s dụng để tìm ra mối quan hệ giữa tọa độ mục tiêu trong hệ tọa độ thực và ảo vvvv ZYXO . Ta định nghĩa tọa độ của của ảnh thu được b i camera ảo thứ nhất 1111 vvvv ZYXO là V1 (zv1, xv1) và camera ảo thứ hai 2222 vvvv ZYXO là V2(zv2, xv2): z v v vv c x z fz     1 , (3.5) zv v v v cx z f z    )(2   , (3.6) xv v v v cy z f x     2 . (3.7) (cz , cx) là tọa độ tâm hệ tọa độ frame ảnh ảo nhìn trong hệ . fv là tiêu cự thấu kính của camera ảo. là khoảng cách từ hệ gốc tọa độ camera ảo tới gốc tọa độ ảo Ov1, Ov2 theo hai trục z, x. Viết lại (3.5), (3.6), (3.7) ta thu được (3.8)                                              x z z v v v v v v v v v v s c c c z y z x x z f x z z     2 2 1 x (3.8) Với xs là véc tơ tọa độ mục tiêu nhìn trong hệ tọa độ camera ảo. Khi mục tiêu hoặc hệ pan-tilt di chuyển thì điểm đặc trưng của ảnh mục tiêu cũng di chuyển và điểm ảnh đặc trưng trong . Giả s các quan hệ giữa hệ tọa độ camera hệ pan-tilt và hệ camera ảo được đảm bảo khi đứng yên và di chuyển. Thực hiện lấy đạo hàm (3.8) ta thu được (3.9) là công thức biểu di n mối quan hệ về tốc độ giữa mục tiêu và ảnh của nó trong hệ camera ảo : vvimgvs f xJx   , (3.9)  Tvvvs xzz 121x vvvv ZYXO vvvv ZYXO vvvv ZYXO 52 Trong đó vimgJ là ma trận Jacobian ảnh trong hệ tọa độ ảo:                              vv v vv v v v v vimg zz y zz x x z x 1 0 )( 0 1 )( 0 )( 1 2 2 2 J (3.10) vimgJ mô tả quan hệ tốc độ giữa ảnh mục tiêu thu được b i hai camera ảo và camera thực nhìn trong hệ tọa độ camera ảo vvvv ZYXO . Tọa độ của mục tiêu nhìn trong hệ tọa độ gốc là 0000 ZYXO của hệ pan-tilt là: CCR xx 0 0 . (3.11) Khi đó 0 0 0 xx C v v RR , Với x0 là tọa độ của mục tiêu nhìn trong hệ tọa độ gốc O0. Phương trình (3.9) có thể viết lại thành: 00 0 xJxJxJx C v vimgvimgvCv C vimgvs RfRRf   , (3.12) Trong đó: ; 022 12121 12121 0              CS CSSCS SSCCC RC (3.13) CR 0 là ma trận chuyển hệ tọa độ từ hệ tọa độ CCCC ZYXO về hệ tọa độ 0000 ZYXO . C1 là cosθ1; C2 là cosθ2; S1 là sinθ1; S2 là sinθ2; Cv C vimgvimg RRf 0JJ  . (3.14) Thực hiện lấy đạo hàm để lấy quan hệ tốc độ giữa các khớp  Tqq 21,  q và tốc độ ảnh mục tiêu nhìn trong khung tọa độ gốc 0x : Cv C vimgvimg RRf 0JJ  (3.15) Trong đó )(qJrobot (3x2) là ma trận Jacobian của hệ pan-tilt nhìn trong hệ tọa độ của camera mà trận Jacobian rô bốt được định nghĩa như sau (Siciliano et al., 2009 [19]):              22 212212 212212 0 )( Sl SClSCl CClSSl robot qJ (3.16) 53 Thay công thức (3.15) vào (3.11) thu được: 0 000 )( xJqJ xJqqJJxJxJx C v vimgv C v vimgvrobotimgC v vimgvimgs Rf RfRf     , (3.17) Trong đó )(qJJJ robotimg Là ma trận Jacobian (3x2) của hệ pan-tilt nhìn trong hệ tọa độ ảo. 3.1.3 iểm oát ự u i n c a ma tr n aco ian Ta thấy ma trận chuyển hệ tọa độ CR 0 và v CR là không suy biến, Do đó sự suy biến của ma trận ảnh imgJ phụ thuộc vào vimgJ có tồn tại hay không. vimgJ sẽ không tồn tại khi λ = xv hay zv = -λ. Do vậy ta có thể tránh được b ng cách chọn tham số λ sao cho thỏa mãn λ > max (xv, zv). Tương tự như vậy ta có thể tránh sự suy biến của ma trận Jacobian rô bốt robotJ b ng cách giới hạn trước không gian hoạt động của mục tiêu. Như vậy ma trận Jacobian của hệ thống chỉ phụ thuộc vào ma trận Jacobian rô bốt robotJ 3.1.4 ài toán điều khiển r t ám m c tiêu với nhiều tham ất đ nh Giả s các tham số của hệ là không xác định được chính xác. Ta chia làm hai phần là phần có thể xác định được và phần bất định JJJ  ˆ , (3.18) Trong đó Jˆ là phần có thể xác định được, J là phần không xác định được của ma trận Jacobian. Thay công thức (3.18) vào (3.17) ta có: 0) ˆ( xJqJJx C v vimgvs Rf   . (3.19) 3.2. dựng hệ động lực học hệ pan-tilt – stereo camera với các tham số bất định Hệ động lực học của rô bốt n-link với sự ảnh hư ng của các tham số nhi u và các tham số bất định có thể biểu di n b ng phương trình (3.20) như sau [19]: 54 τdqgqqqCqqM  )()(),()( t (3.20) Trong đó M(q) là ma trận moment quán tính xác định dương kích thước (n x n). q là véc tơ của các khớp quay trong rô bốt kích thức (n x 1). qqqC  ),( là ma trận mô ment hướng tâm và Coriolis có kích thước (n x n). g(q) là vecto moment lực trọng trường và τ là vec tơ lực moment đặt lên các khớp của rô bốt. Trong hệ thống này, hệ pan-tilt là một rô bốt mang camera có hai khớp, n = 2. d(t) là vecto ảnh hư ng của nhi u và các tham số bất định tác động lên hệ động lực học của rô bốt. Giả s , d(t) là liên tục và bị chặn, khi đó: max)( dt d . (3.21) Từ phương trình (3.20) ta có (Cat and Minh, 2009 [35]): 1( )[ ( , ) ( ) ( )]t   q M q τ C q q q g q d . (3.22) L i theo dõi điểm tính năng của ảnh mục tiêu được định nghĩa như sau: sds xxε  ; trong đó 3Rs x là vec tơ của điểm ảnh thu được trong hệ không gian ảo, 3Rsd x là điểm ảnh mong muốn cần đạt đến của mục tiêu trong không gian 3D ảo được đưa ra b i hệ stereo camera. Như vậy vec tơ biểu di n l i theo dõi ảnh trong không gian 3D ảo. sds xxε  . (3.23) Lấy đạo hàm của , đạo hàm bậc 1 và bậc 2 ta có: qJqJxε   ˆs , (3.24) qJqJqJqJx