ĐẠI HỌC THÁI NGUYÊN
ĐẠI HỌC CÔNG NGHỆ THÔNG TIN VÀ TRUYỀN THÔNG

BÁO CÁO TỔNG KẾT
ĐỀ TÀI KHOA HỌC VÀ CÔNG NGHỆ CẤP ĐẠI HỌC

NGHIÊN CỨU MỘT SỐ THUẬT TOÁN ĐIỀU KHIỂN ROBOT DI ĐỘNG
SỬ DỤNG THÔNG TIN HÌNH ẢNH
Mã số: ĐH2014 –TN08-02

Chủ nhiệm đề tài: ThS Lê Văn Chung

Thái Nguyên, tháng 5 năm 2017


ĐẠI HỌC THÁI NGUYÊN
ĐẠI HỌC CÔNG NGHỆ THÔNG TIN VÀ TRUYỀN THÔNG

BÁO CÁO TỔNG KẾT
ĐỀ TÀI KHOA HỌC VÀ CÔNG NGHỆ CẤP ĐẠI HỌC

NGHIÊN CỨU MỘT SỐ THUẬT TOÁN ĐIỀU KHIỂN ROBOT DI ĐỘNG
SỬ DỤNG THÔNG TIN HÌNH ẢNH
Mã số: ĐH2014 –TN08-02

Xác nhận của tổ chức chủ trì

Chủ nhiệm đề tài

(ký, họ tên, đóng dấu)



NCS.
Chung

Lê

Chủ nhiệm đề tài,
trƣờng Đại học Công
Văn
thực hiện các nội
nghệ thông tin và Truyền
dung nghiên cứu
thông
liên quan đến đề tài
Chuyên môn: Điều khiển
và tự động hóa.
Đơn vị công tác: Phòng

2

ThS.
Yến

Vƣơng

Thị

KH-CN&HTQT, trƣờng
Thƣ ký hành chính
Đại học Công nghệ Thông

2

MỤC LỤC
MỞ ĐẦU .................................................................................................................... 10
CHƢƠNG 1 ................................................................................................................ 16
GIỚI THIỆU CHUNG VỀ ROBOT DI ĐỘNG, CAMERA XỬ LÝ ẢNH VÀ ROBOT
PAN TILT................................................................................................................... 16
1.1 Giới thiệu camera Eye RIS 2.1................................................................................... 16
1.2. Giới thiệu robot di động Pioner 3DX ........................................................................ 17
1.3. Giới thiệu robot Pan tilt PTU - D48E ....................................................................... 19
CHƢƠNG 2 ................................................................................................................ 21
XÂY DỰNG MÔ HÌNH ĐỘNG HỌC VÀ MÔ HÌNH ĐỘNG LỰC HỌC CHO HỆ
THỐNG ...................................................................................................................... 21
2.1. Mô hình động học hệ robot pan/tilt stereo camera. ................................................... 21
2.2. Xây dụng mô hình động học và động lực học hệ robot di động – bệ pan/tilt stereo
camera .............................................................................................................................. 26
2.2.1. Mô hình động học ............................................................................................... 26
2.2.2. Xác định ma trận Jacobi ảnh.............................................................................. 27
2.2.3. Xác định ma trận Jacobi của hệ và tốc độ bám mục tiêu cho robot di động .... 28
2.2.4. Xác định tốc độ của các bánh xe để cho robot di động tiếp cận mục tiêu ......... 31
2.2.5. Mô hình động lực học ........................................................................................ 32
CHƢƠNG 3 ................................................................................................................ 34
THIẾT KẾ THUẬT TOÁN ĐIỀU KHIỂN VÀ MÔ PHỎNG......................................... 34
HỆ THỐNG ................................................................................................................ 34
3.1. Thiết kế thuật toán điều khiển hệ pan/tilt stereo camera và mô phỏng .................... 34
3.1.1. Thiết kế thuật toán điều khiển động học hệ pan/tilt bám mục tiêu di động ....... 34
3.1.2. Mô phỏng minh họa trên PC ............................................................................. 36
3.1.3. So sánh kết quả và đánh giá .............................................................................. 40
3.2. Thiết kế thuật toán điều khiển hệ robot di động – pan tilt – stereo camera và mô
phỏng. ............................................................................................................................... 41

m

Pixel

Véc tơ đặc trƣng ảnh

Jimag

Ma trận Jacobi ảnh

Jrobot

Ma trận Jacobi robot

J

Ma trận Jacobi tổng hợp

Tx Ty Tz

m/s

Vận tốc dài của tay nắm camera

 x  y  z m/s

Vận tốc góc của tay nắm camera

x


Hình 2.8.

Ảnh theo 2 trục X, Y................................................................................ 23
Hệ robot di động – pan tilt – stereo camera ............................................. 27
Mô hình hệ thống stereo camera .............................................................. 31
Ảnh theo hai trục X, Z (trái) và Y, Z (phải). ........................................... 31
Vị trí và hƣớng mong muốn của robot di động. ...................................... 31

Hình 3.1. Mạng RBF xấp xỉ hàm f ......................................................................... 35
Hình 3.2. 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.......................................................................................... 36
Hình 3.3. Sai lệch tọa độ ảnh mục tiêu (pixel). ....................................................... 37
Hình 3.4. 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ù ( u1, = 0 ). ........................................................................................................... 37
Hình 3.5. Sai lệch bám tọa độ khi mục tiêu di chuyển theo đƣờng thẳng. .............. 38
Hình 3.6. Sai lệch tọa độ ảnh khi mục tiêu di chuyển theo đƣờng thẳng ................ 38
Hình 3.7. 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 ). .................................................................. 38
Hình 3.8. 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ù ................................................................................ 38
Hình 3.9. Sai lệch bám quỹ đạo khi mục tiêu cơ động theo cung tròn .................... 39
Hình 3.10. Sai lệch tọa độ ảnh khi mục tiêu cơ động theo cung tròn ........................ 39
Hình 3.11. 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ù ................................................................................ 39
Hình 3.12. 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ù ( u1, = 0 ). .................................................................. 40
Hình 3.13. Sai lệch bám quỹ đạo khi mục tiêu di chuyển với vận tốc thay đổi. ....... 40
Hình 3.14. Sai lệch tọa độ ảnh khi mục tiêu di chuyển với vận tốc thay đổi. ........... 40



3. Tính mới và sáng tạo
Các nghiên cứu về hệ robot di động sử dụng camera ở Việt Nam hiện chỉ dùng
1 camera để xử lý ảnh, còn việc tính khoảng cách thì sử dụng các sensor khác. Đề tài
nghiên cứu sử dụng 2 camera để vừa thu nhận ảnh, vừa tính toán khoảng cách tới mục
tiêu.
4. Kết quả nghiên cứu
Kết quả nghiên cứu của đề tài mới dừng lại ở việc mô phỏng và thử nghiệm
thuật toán trên matlab simulink, chƣa cài đặt đƣợc lên robot. Tuy nhiên các kết quả
cũng đã phần nào cho thấy các thuật toán đƣa ra có độ đáp ứng tốt, có tính bền vững
với các tham số bất định, tối ƣu đƣợc năng lƣợng tiêu hao và sai lệch.
5. Sản phẩm
5.1. Sản phẩm khoa học
1. Lê Văn Chung (2014), “Phát triển hệ pan/tilt – nhiều camera bám mục
tiêu di động”, Tạp chí KHCN Đại học Thái Nguyên, 116(02), tr. 41-46.
2. Lê Văn Chung, Ma Thị Hồng Thu (2016), Thiết kế bộ điều khiển port
controller hamilton để điều khiển hệ pan tilt – stereo camera bám mục tiêu
di động", Tạp chí KHCN Đại học Thái Nguyên, 155(10), tr. 105-111.


7

3. Le Van Chung, Pham Thuong Cat (2014), “Robust visual tracking
control of pan tilt – stereo camera system”, VCM conference, pp.167-173.
4. Le Van Chung, Pham Thuong Cat (2016), “Optimal tracking a moving
target for integrated mobile robot – pan tilt – stereo camera”, Advanced
Intelligent Mechatronics AIM IEEE Conference, pp. 530 - 535.
5. Le Van Chung, Pham Thuong Cat (2015), “A new control method for
stereo visual servoing system with pan tilt platform”, Journal of Computer
Science and Cybernetics, 31 (2), pp. 107 – 122.
6. Le Van Chung, Duong Chinh Cuong (2016), “Design Adaptive-CTC

Code number: ĐH2014 –TN08-02.
Coordinator: ThS. Le Van Chung.
Implementing institution: Thai Nguyen University of Information and
Communication Technology.
Duration: from 1/2014 to 12/2015.
2. Objective(s)
The objective of this project is researched and developed some control
algorithms robots that moved by wheels based on information from images obtained
from two cameras. The control algorithms ensure sustainability with parameters
uncertainty and resistance to interference with high processing speed.
3. Creativeness and innovativeness
The research on mobile robot using camera in Vietnam only used one camera
for image processing, and other sensors for detecting distances. This research
projects using 2 cameras for both image processing and calculate the distance to the
target.
4. Research results
Research results of the project have just stopped at the simulation and test the
algorithm on Matlab Simulink, not installed on the real robot. But the results
showed that the algorithm also has a good response, sustainability with uncertain
parameters, optimize energy consumption and errorDomestic Journal 0.5 points: 02
5. Products
5.1. Scientific Products
1. Le Van Chung (2014), “Developing pan/tilt - stereo camera to track a
moving target”, Jounal of Science and Technology Thai Nguyen
University, 116(02), pp. 41-46.
2. Le Van Chung, Ma Thi Hong Thu (2016), "Design Port Controller
Hamilton to Control pan tilt – stereo camera system track a moving
target", Jounal of Science and Technology Thai Nguyen University,
155(10), pp. 105-111.
3. Le Van Chung, Pham Thuong Cat (2014), “Robust visual tracking

MỞ ĐẦU
Cùng với sự phát triển mạnh mẽ của các hệ thống tự động hóa, rô bốt di động
ngày một đƣợc hoàn thiện và càng cho thấy lợi ích của nó trong công nghiệp và sinh
hoạt. Một vấn đề rất đƣợc quan tâm khi nghiên cứu về rô bốt di động là khả năng
nhìn và xử lý thông tin hình ảnh để rô bốt biết đƣợc vị trí nó đang đứng trong môi
trƣờng phi cấu trúc và có thể di chuyển tới một vị trí xác định, đồng thời có thể tự
động tránh đƣợc các chƣớng ngại vật trên đƣờng đi.
Hiện nay, có nhiều nhóm nghiên cứu trên thế giới đã đạt đƣợc những thành
tựu đáng kể về điều khiển rô bốt di động. Nhóm tác giả Widodo Budiharto, Ari
Santoso, Djoko Purwanto, Achmad Jazidie của đại học BINUS và viện công nghệ
Indonesia đã nghiên cứu hệ thống điều khiển chuyển động cho rô bốt dùng nhiều
camera cùng với bộ lọc Kalman để loại bỏ nhiễu ảnh thu từ camera cho độ tin cậy
cao. Rô bôt dựa trên hệ thống này có tên là Srikandi III [2] cho tốc độ chuyển động
nhanh, độ chính xác cao trong phòng thí nghiệm. Cũng dùng 2 camera tốc độ cao
nhóm tác giả Masaaki Shibata, Hideki Eto, Masahide Ito ở phòng nghiên cứu
trƣờng đại học Seikei Nhật Bản đã phát triển hệ thống tìm đƣờng đi cho rô bốt tốc
độ cao [3]. Camera hữu dụng trong tìm đƣờng nhƣng nó cũng rất hữu dụng trong
việc theo dõi chuyển động của các đối tƣợng, nhóm tác giả HiroyukiUkida,
MasayukiKawanami and YasuhiroTerama của trƣờng đại học Waseda – Tokyo –
Nhật bản đã phát triển hệ thống theo dõi chuyển động của đối tƣợng 3D. Hệ thống
dựa trên sự so sánh các ảnh thu đƣợc và dùng phƣơng pháp lọc phần tử để phát hiện
sự chuyển động của đối tƣợng [5]. Trong các đối tƣợng rô bốt di động thì rô bốt tự
hành cũng là một thành phần, nhóm tác giả Tae-Il Kim, Wook Bahn, Changhun
Lee, Tae-jae Lee, Muhammad Muneeb Shaikh and Kwang-soo Kim của Hàn Quốc
đã phát triển hệ thống tìm đƣờng tới mục tiêu cho rô bốt tự hành dựa trên hệ thống
stereo camera, hệ thống này luôn hƣớng tới mục tiêu cùng với đó hệ thống sẽ tính
toán các góc, độ nghiêng từ đó đƣa ra đƣợc vị trí thực tế của mục tiêu trên hệ tọa độ
để tìm đƣợc đƣờng đi tới đích[8].
[1] Eng Swee Kheng , Anwar Hasni Abu Hassan , Ali Ranjbaran , Ting Shyue
Siong (2011), “Range Estimation for Robot arm Applications using Image

chúng sẽ hoạt động một cách chính xác hơn. Rô bốt đƣợc điều khiển dựa trên xử lý
ảnh từ nhiều camera sẽ có độ chính xác cao hơn từ 1 camera. Vì vậy mục tiêu chung
của đề tài là sử dụng thông tin hình ảnh từ nhiều camera để điều khiển rô bốt.
Mục tiêu lâu dài của hƣớng nghiên cứu là từng bƣớc ứng dụng xử lý ảnh vào quá
trình tự động hóa việc điều khiển cho xe nâng hàng tự động trong kho bãi, bến cảng.
Tuy nhiên để từng bƣớc đạt đƣợc mục tiêu đó, mục tiêu của đề tài này là nghiên cứu
phát triển một số thuật toán điều khiển rô bốt di chuyển bằng bánh xe trên cơ sở
thông tin hình ảnh từ 2 camera. Thuật toán phải đảm bảo có tính bền vững với các
tham số bất định và khả năng kháng nhiễu với tốc độ xử lý cao.
Nói tới rô bốt thì hai thành phần quan trọng nhất của nó là cảm biến và bộ
phận chấp hành, với cảm biến rô bốt có thể dùng nhiều loại từ hồng ngoại, sóng siêu
âm, laser scanner hay camera quang học..v.v, còn bộ phận chấp hành của rô bốt
chuyển động thì nhất thiết phải có cơ cấu chuyển động bằng khớp hay bánh xe,
ngoài ra còn có các cơ cấu chấp hành khác.
Đối tƣợng nghiên cứu chính của đề tài là tập trung vào đối tƣợng rô bốt có cơ
cấu chuyển động bằng bánh xe và dùng cảm biến là các camera với nhiều hơn 1
camera. Nhƣ vậy phạm vi hoạt động của rô bốt hầu nhƣ trong các địa hình tƣơng
đối bằng phẳng, hoặc có độ nghiêng trong khoảng cho phép. Cảm biến hình ảnh thu
đƣợc từ nhiều hơn 1 camera sẽ cho độ nhanh nhậy, góc nhìn và độ chính xác lớn
hơn so với dùng 1 camera. Những hình ảnh thu đƣợc sẽ đƣợc xử lý, tính toán từ đó
ra quyết định điều khiển các cơ cấu chấp hành khác của rô bốt thực hiện theo các
yêu cầu.
Đề tài sẽ thực hiện nghiên cứu các vấn đề sau:
- Nghiên cứu và phát triển một số thuật toán thu nhận ảnh, xử lý ảnh camera, từ
đó xử lý và tính toán các thông số điều khiển cho rô bốt.
- Nghiên cứu về rô bốt có cơ cấu chuyển động bằng bánh xe
- Phát triển một số phƣơng pháp điều khiển rô bốt di động trên cơ sở thông tin
hình ảnh sử dụngnhiều camera.



khiển tốt nhất, phù hợp cho hệ thống.
Để thực hiện đƣợc các công việc đó, trƣớc tiên tác giả sẽ tìm hiểu các công trình
nghiên cứu về rô bốt di động và các phƣơng pháp điều khiển rô bốt di động sử dụng
camera (visual servoing), các phƣơng pháp điều khiển camera để có không gian
quan sát lớn. Việc tìm hiểu sẽ dựa trên các bài báo khoa học, các công trình đã công
bố trong và ngoài nƣớc để từ đó hiểu và đánh giá đƣợc các điểm tốt có thể sử dụng
lại và chƣa tốt còn cải tiến đƣợc trong các nghiên cứu đó để đề xuất một phƣơng
pháp điều khiển tốt hơn có tính mới.
- Để hoàn thiện đề tài nội dung nghiên cứu gồm có:
+ Phần nghiên cứu về thị giác máy camera và xử lý ảnh.
+ Nghiên cứu việc sử dụng hình ảnh để điều khiển bệ pan/tilt mang camera để
điều khiển góc quay theo phƣơng ngang, đứng của bệ giúp hệ thống bám mục tiêu
cần tiếp cận khi mục tiêu di động hay khi rô bốt đi trên địa hình không hoàn toàn
bằng phẳng làm thay đổi phƣơng và góc nhìn tới mục tiêu.


14

+ Nghiên cứu phát triển một số phƣơng pháp điều khiển rô bốt di động trên cơ
sở thông tin hình ảnh sử dụng nhiều camera để tính toán điều khiển cho rô bốt di
chuyển, tiếp cận tới mục tiêu biết trƣớc.
Chi tiết nội dung nghiên cứu gồm có
Nghiên cứu về thị giác máy camera và xử lý ảnh: từ đó phát triển một số
thuật toán xử lý ảnh dựa trên ảnh từ các camera để trích rút các thông số cần thiết
cho điều khiển. Trƣớc tiên tác giả sẽ nghiên cứu các camera có khả năng xử lý song
song, nhƣ EyeRIS 1.3, EyeRIS 2.1 vì những camera này có tốc độ xử lý cao, thích
hợp cho nhiều ứng dụng.
Tuy nhiên những camera xử lý song song này còn chƣa đƣợc phổ biến rộng
rãi, việc đặt mua, sửa chữa và hỗ trợ kỹ thuật vẫn còn hạn chế. Trong trƣờng hợp
gặp khó khăn các camera thông dụng sẽ đƣợc sử dụng thay thế nhƣng vẫn đảm bảo

toán điều khiển cần bù đƣợc độ bất định của các tham số nội tại của camera, ống
kính, độ bất định của các khớp, trục bánh xe…
Chƣơng trình xử lý ảnh là một chƣơng trình phức tạp, do đó một máy tính
riêng, tốt cần đƣợc chuẩn bị trên rô bốt để nhận các ảnh, xử lý chúng và đƣa ra tín
hiệu điều khiển các cơ cấu chấp hành khác điều khiển rô bốt tiếp cận mục tiêu.


16

CHƢƠNG 1
GIỚI THIỆU CHUNG VỀ ROBOT DI ĐỘNG, CAMERA XỬ LÝ ẢNH VÀ
ROBOT PAN TILT
Hệ thống đƣợc nghiên cứu trong đề tài là một robot di động có mang bệ
pan/tilt. Trên bệ pan/tilt có gắn 2 camera (stereo camera) dùng để quan sát. Mục tiêu
của hệ thống là di chuyển và tiếp cận đối tƣợng di chuyển biết trƣớc. trong các mục
sau sẽ lần lƣợt giới thiệu về các thiết bị dùng trong hệ.
1.1 Giới thiệu camera Eye RIS 2.1.
Eye RIS 2.1 là một camera CNN của hãng Ana focus. Đây là 1 camera xử lý
tốc độ cao, có thể hoạt động độc lập hoặc kết nối với máy tính.

Hình 1.1. Camera Eye-RIS v2.1
Đặc điểm và thông số kỹ thuật
- Tốc độ bắt ảnh: 10.000 frame/s.
- Độ phân giải 176x144 pixels.
- Tấm cảm biến C-mount 1/2
- Kích thƣớc 47x47x26.53 mm
- Phạm vi hoạt động (50 db – 120 db).
- Cảm biến tích hợp 4 ADCs và 4 DACs 8 bit – 50Hz cho ảnh xám.
-


trên Nios II sẽ tiếp tục xử lý ảnh, trích đặc trƣng, lƣu trữ, truyền dữ liệu về máy tính
hay ra các quyết định xử lý, điều khiển trực tiếp cho các thiết bị chấp hành.
Hệ thống các thƣ viện và cách lập trình trên Eye-RIS đƣợc giới thiệu rõ hơn
trong tài liệu tham khảo về xử lý ảnh cho sinh viên.
1.2. Giới thiệu robot di động Pioner 3DX
Robot Pioneer 3DX là 1 robot di động thông minh có đặc điểm cấu tạo nhƣ
hình 1 với các thông số kỹ thuật nhƣ sau:


18

Hình 1.3. Robot di động Pioneer 3DX
Một số tính năng, cổng giao tiếp và nguồn điện
- 4 cổng giao tiếp nối tiếp RS-232 tốc độ từ 9.6 đến 115.2 kilobaud
-

4 đến 8 cặp sensor phát hiện vật cản Sonar trên mỗi robot.
2 cổng vào số 8-bit.
Cổng nguồn và cổng giao tiếp tƣơng tự
Cảm biến nghiên và quay Tilt/roll.
Đèn báo nguồn, báo dung lƣợng pin dự trữ
Cho phép chuyển đổi nguồn nuôi 5/12 V

- Cổng giao tiếp điều khiển động cơ với chế độ băm xung PWM/ trực tiếp
qua cổng điều khiển số 8 bit.
- Giao tiếp I2C interface hỗ trợ LCD 4x20.
Hoạt động
Robot Pioneer 3DX có thể hoạt động với các chế độ nhƣ sau:
- Lập trình điều khiển trên PC và giao tiếp điều khiển qua cổng Ethernet.
- Lập trình điều khiển trên PC và giao tiếp điều khiển qua wifi.

• Khả năng tải trọng lớn đến 15 Lbs
• Định vị cực kỳ chính xác (để 0.006 ° với động cơ microstep)
• Đa dạng các pan tốc độ 0.006 ° / giây đến 100 °, điều khiển chính xác
• Pan 360 liên tục (với tùy chọn vòng trƣợt)
• Kiểm soát vị trí, tốc độ và tăng tốc thời gian thực
• Thiết kế worm gear cứng cung cấp ổn định khi định vị trong môi trƣờng gió
• Kết nối duy nhất cho cả tín hiệu điều khiển, tín hiệu tải trọng
• Linh hoạt lắp ráp tự do (hoặc bên trên thiết bị khác)
• Bảo về khép kín, an toàn cho các ứng dụng ngoài trời / biển (IP67)
• Phạm vi nhiệt độ hoạt động rộng
• Dải điện áp đầu vào DC rộng (12VDC đến 30VDC) có chạy pin điều khiển
năng lƣợng thân thiện môi trƣơng.
• Mã hóa kỹ thuật số chính xác
• Nhiều giao diện điều khiển: RS-232/485/422 và Ethernet
• Nhiều lệnh giao thức giao diện: FLIR-MCS, Nexus, Pelco-D
Bệ PTU-D48 E Series đã đƣợc chứng minh hoạt động tổ trong một loạt các
ứng dụng quan trọng cho việc định vị của máy ảnh, laser, ăng-ten, hoặc máy móc
khác. Nó hoạt động đƣợc trong cả hai môi trƣờng tĩnh và di động. Nó đƣợc thiết kế
cho các nhiệm yêu cầu hoạt động chính xác và tin cậy cao, lien tục 24/7 tại tất cả
các môi trƣờng thời tiết khắc nghiệt. Series PTU-D48 E sẽ giúp đảm bảo dự án của
bạn và hoàn thành đúng thời gian và đúng thời gian. Một số hình ảnh ứng dụng thực

Hình 1.7. Ứng dụng của bệ Pan/Tilt PTU D48E
Lập trình cho bệ pan/tilt PTU D48E:
Ngôn ngữ sử dụng cho lập trình cho bệ pan/tilt PTU D48E là C++/ Visual
C++. Nhà sản xuất cũng đã cung cấp các thƣ viện để ngƣời lập trình sử dụng trong
các ứng dụng của mình.


21

Từ hình 2.2 ta có tọa độ ảnh thu đƣợc ở camera trái (-UL, VL) và camera phải
là (UR ,VR) với V= VL= VR thực hiện chuyển về mặt phẳng theo (Z, Y) và (X, Y)
nhƣ hình 2.3, hình 2.4 ta có các quan hệ hình học trong khung tọa độ OCXCYCZC:
UL =

f K
f
f K
( + Z ); VL = X ; U R = - ( - Z ) (1) Với:
Y 2
Y
Y 2

1 UL - UR
=
Y
f .K

(2)

Từ công thức (1), (2) bằng các phép biến đổi ta tính đƣợc tọa độ của điểm Q(X, Y,
Z) [5] trong hệ tọa độ OC đƣợc tính theo biểu thức sau:
é
ù
K
ê
ú
VL
ê
ú

fL = fR = f

là tiêu cự ống kính camera.

(3)


23

Hình 2.3. Ảnh theo 2 trục Z, Y

Hình 2.4. Ảnh theo 2 trục X, Y
Khi đối tƣợng di chuyển thì bệ Pan-Tilt mang camera cũng cần đƣợc điều
khiển đẻ bám theo. Nếu ta biểu diễn véc tơ vận tốc dài của gốc tọa độ camera là
C

v = [TXC

TYC

TZC ]và

véc tơ tốc độ góc là

C

Ω = [wXC

wYC



v, T Ω

mục tiêu:

C

v = [TX

C

Ω = [wX

TY
wY

TZ ]= -

T

v

wZ ]= - T Ω

Thực hiện vi phân các biểu thức trong (1) rồi thay vào (3), (4), (5) ta đƣợc quan hệ
tốc độ giữa điểm đặc trƣng trên ảnh 2 camera với vị trí và hƣớng giá mang camera
(6):