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

( 1)σ -
Thay 1 ε từ (2.27) vào (2.34) ta nhận được : 
 ε
 V εTTT Kε ε() β ε Kε
 ε
 (2.35) 
 T
 ε ε. β ε Kε ε0 ε
Nếu chọn 0 0 ;  0 ta có: 
 -ε + ε0 - ε (2.36) 
Thay (2.36) vào (2.35) ta nhận được: 
 T
 V ε Kε ε 0 (2.37) 
 Ta thấy V 0 khi ε0 và V 0 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 đó mm()t d , mm()t d 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 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 tức là: 
 35 
 UU UUU( ) UUU ( ) UV 2 fUUU22
 RLLLRLLRLLLLRl C C l S V l S C
 K2 1 2 fK 2 2L fK 1 f 12 f 1
 UU VUU()()() UU VUU fV22 VUU
 Jˆ 0.8 RLLLRLRLLRLLLRl S C l S l S C
 K2 1 2 fK 2 222 f fK 1 f 1 f 1
 (2.38) 
 UUUUUUUUUV()() 2 f22 U U U
 RLRLRRLRRLl C C l S V l S RLR C
 K2 1 2 fK 2 2L fK 1 f 1 2 f 1
 ab11
 Jˆ ab
Ký hiệu 22 thì ma trận giả nghịch đảo tính được như sau: 
 ab33
 a1 C b 1 B a 2 C b 2 B a33 C b B
 T 1 T MMM
 JJJJˆ() ˆ ˆ ˆ
 (2.39) 
 a1 B b 1 A a 2 B b 2 A a33 B b A
 MMM
Trong đó: 
 222
 A a1 a 2 a 3
 B a1 b 1 a 2 b 2 a 3 b 3
 222
 C b1 b 2 b 3 (2.40) 
 2 2 2
 M()()() ab12 ab 21 ab 13 ab 31 ab 23 ab 32
 Chọn các tham số tự chọn: 
 1.5; 0.2 (2.41) 
 2
 (1 1)
 1 exp (2.42) 
 25
 2
 (2 2)
 2 exp (2.43) 
 9
 2
 (3 0.5)
 3 exp (2.44) 
 16
 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à: 
 36 
 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: 
 ,
 u1 = 0
 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ù ( ). 
 37 
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. 
 38 
 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ù ( ). 
 ,
 u1 = 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: 
 39 
 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. 
 40 
 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ù 
 ,
 u1 = 0
 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ù ( ). 
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/s2. Tiến hành mô phỏng với sơ đồ như trong hình 2.6 ta thu được kết quả sau: 
 41 
 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. 
 42 
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 u1 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 
 43 
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ố. 
 44 
 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 
 45 
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. 
 46 
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à OL XLYLZL , 
 OR XRYRZR 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à Oc XcYcZc 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 độ O1X1Y1Z1 của hệ pan-tilt. 
 47 
 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
tính được tọa độ của mục tiêu trong không gian 3D xc x y z nhìn trong hệ trục tọa 
độ Oc XcYc Zc được tính theo công thức (3.1): 
 K.VL
 1 
 x x y z T K.f ,
 c   (3.1) 
 UR UL K 
 UR UL 
 2 
 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. 
 fLR f f
 ước 2, Ta định nghĩa một hệ tọa độ liên kết Ov XvYv Zv Có gốc tọa độ trùng với 
hệ tọa độ Oc XcYc Zc . Để chuyển đổi từ hệ tọa độ OC tới OV, ta s dụng ma trận chuyển 
 C
tọa độ Rv (hình 3.2) [20]. 
 48 
 Hình 3.2 Mô hình hệ camera 3D ảo. 
 T
 Tọa độ của mục tiêu xc x y z nhìn trong hệ tọa độ Oc XcYc Zc thì nhìn trong 
 T
hệ tọa độ camera ảo Ov XvYv Zv là xv zv xv yv  (Cai et al., 2013 [20]): 
 v
 xv RC xc , (3.2) 
Với: 
 1 0 0 
 v o
 RC 0 cos sin ; 90 . (3.3) 
 0 sin cos 
 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. 
 49 
 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 Ov XvYv Zv được s dụng để định nghĩa ra hai hệ tọa 
độ Ov1Xv1Yv1Zv1, Ov2 Xv2Yv2Zv2 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 độ Ov XvYv Zv được định nghĩa 
như sau: 
 T T
 zv ; xv  ; zv ; xv  . (3.4) 
 50 
 ướ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 
 T
di n trong hệ tọa độ ảo là: xs zv1 zv2 xv1 . 
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 Ov XvYv Zv . Ta định nghĩa tọa độ của 
của ảnh thu được b i camera ảo thứ nhất Ov1Xv1Yv1Zv1 là V1 (zv1, xv1) và camera ảo thứ 
hai Ov2 Xv2Yv2Zv2 là V2(zv2, xv2): 
 zv
 zv1 fv cz , (3.5) 
  xv
 fv
 zv2 (xv ) cz , (3.6) 
  zv
 fv
 xv2 yv c x . (3.7) 
  zv
 (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) 
 zv 
  xv
 zv1 cz 
 x  
 x z f v c
 s v 2 v z  z (3.8) 
 v 
 xv 2 c x 
 yv 
 z  
 v 
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 Ov XvYv Zv : 
 x s fv Jvimg x v , (3.9) 
 51 
Trong đó Jvimg là ma trận Jacobian ảnh trong hệ tọa độ ảo: 
 1 z 
 v 0
 2 
  xv ( xv ) 
 x  1 
 J v 0
 vimg 2 (3.10)
 (zv ) zv  
 y 1 
 v 0
 2 z  
 (zv ) v 
 Jvimg 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 Ov XvYv Zv . Tọa độ của mục tiêu nhìn trong 
hệ tọa độ gốc là O0 X0Y0Z0 của hệ pan-tilt là: 
 0
 x0 RC xC . (3.11) 
 v 0
Khi đó xv R0 RC x0 , 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: 
  C 0   v 
 xs fv Jvimg Rv RCxv Jimg x0 fv Jvimg RC x0 , (3.12) 
Trong đó: 
 C1C2 C1S2 S1 
 0 
 RC S1C2 S1S2 C1 ; (3.13) 
 S2 C2 0 
 0
 RC là ma trận chuyển hệ tọa độ từ hệ tọa độ OC XCYCZC về hệ tọa độ O0 X0Y0Z0 . C1 là 
cosθ1; C2 là cosθ2; S1 là sinθ1; S2 là sinθ2; 
 C 0
 Jimg fv Jvimg Rv RC . (3.14) 
 T
Thực hiện lấy đạo hàm để lấy quan hệ tốc độ giữa các khớp q q1,q2  và tốc độ ảnh 
mục tiêu nhìn trong khung tọa độ gốc x 0 : 
 C 0
 Jimg fv Jvimg Rv RC (3.15) 
Trong đó Jrobot (q) (3x2) là ma trận Jacobian của hệ pan-tilt nhìn t