Phát triển hệ Pan tilt – nhiều camera bám mục tiêu di động

Chia sẻ: Thi Thi | Ngày: | Loại File: PDF | Số trang:6

Thêm vào BST

Báo xấu

33
lượt xem 2
download

Download Vui lòng tải xuống để xem tài liệu đầy đủ

Báo cáo này đề xuất một phương pháp điều khiển hệ rô bốt Pan-Tilt cặp camera (stereo) bám mục tiêu di động. Cặp camera được đặt trên đầu tay nắm của một rô bốt Pan-Tilt có 2 bậc tự do.

Chủ đề:

Bình luận(0) Đăng nhập để gửi bình luận!

Lưu

Nội dung Text: Phát triển hệ Pan tilt – nhiều camera bám mục tiêu di động

Lê Văn Chung Tạp chí KHOA HỌC & CÔNG NGHỆ 116 (02): 41 - 46 PHÁT TRIỂN HỆ PAN TILT – NHIỀU CAMERA BÁM MỤC TIÊU DI ĐỘNG Lê Văn Chung* Trường Đại học Công nghệ thông tin và truyền thông – ĐH Thái Nguyên TÓM TẮT Báo cáo này đề xuất một phƣơng pháp điều khiển hệ rô bốt Pan-Tilt cặp camera (stereo) bám mục tiêu di động. Cặp camera đƣợc đặt trên đầu tay nắm của một rô bốt Pan-Tilt có 2 bậc tự do. Thuật toán điều khiển hệ thống bám theo đối tƣợng di chuyển tự do bằng cách điều khiển robot sao cho ảnh thu đƣợc của đối tƣợng di chuyển luôn ở vị trí trung tâm của 2 camera. Bài viết này đƣa ra mô hình động học của hệ Pan-Tilt trên cơ sở bệ Pan-Tilt PTU-D48E-Series, tính toán vị trí của các đối tƣợng trong hình ảnh, khoảng cách và các góc Pan-Tilt và phƣơng pháp điều khiển để giữ đối tƣợng luôn luôn ở trung tâm của hai máy ảnh. Từ khóa: bám mục tiêu, Pan-Tilt, PTU-D48E-Series, rô bốt, cặp camera GIỚI THIỆU* Hệ thống camera bám mục tiêu di động sử dụng hệ pan/tilt thƣờng sử dụng nhiều trong các hệ thống quân sự quốc phòng và an ninh. Theo cấu trúc có thể phân hệ thành nhóm có 1 hoặc 2 camera cố định hoặc điều khiển đƣợc. Hệ thống có 1 camera [7], chỉ xác định đƣợc hƣớng của mục tiêu nên cần sử dụng thêm một máy đo khoảng cách. Nếu sử dụng 2 camera [1], [2] thì thông số khoảng cách hoàn toàn có thể xác định đƣợc từ ảnh trên các camera. Với nhóm camera di động, các camera quay quét với các góc khác nhau [3], [5], [6] có một ƣu điểm là có thể điều khiển hệ camera di chuyển bám theo đối tƣợng để đối tƣợng luôn ở trung tâm của ảnh thu đƣợc nên không gian quan sát lớn, thay đổi đƣợc so với hệ camera gắn cố định trên rô bốt [4], [8]. đƣợc trên 2 camera. Hệ thống đƣợc thiết kế dựa trên mô hình bệ pan/tilt PTU-D48ESeries nhƣ trong hình 1. Nội dung bài báo gồm các phần chính là xây dựng mô hình động học của hệ rô bốt stereo bám mục tiêu di động, thiết kế thuật toán điều khiển hệ servo thị giác, mô phỏng kiểm chứng và kết luận. XÂY DỰNG MÔ HÌNH ĐỘNG HỌC CHO HỆ ĐIỀU KHIỂN ROBOT – STEREO CAMERA BÁM MỤC TIÊU DI ĐỘNG. 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 1. Để bám đƣợc mục tiêu có tốc độ di chuyển khác nhau thì ta cần điều khiển các khớp của bệ Pan-Tilt mang camera thay đổi phù hợp. Để làm đƣợc điều này thì tốc độ di chuyển của mục tiêu phải tính toán hay đo đƣợc, ngoài ra các thông số của hệ thƣờng không hoàn toàn chính xác nên việc bám mục tiêu di động là một bài toán phức tạp. Trong báo cáo này, các góc pan/tilt đƣợc điều chỉnh sao cho ảnh mục tiêu luôn đƣợc duy trì ở vị trí mong muốn trên mô hình của ảnh thu * Tel: 01654 236119, Email: lvchung@ictu.edu.vn Hình 1. Bệ pan/tilt PTU-D48E-Series 41 Lê Văn Chung Tạp chí KHOA HỌC & CÔNG NGHỆ Xác định ma trận Jacobi ảnh qua tọa độ điểm ảnh thu đƣợc từ 2 camera quy vào hệ tọa độ camera OcXcYcZc Dùng 2 camera ta có thể xác định đƣợc các thông số nhƣ khoảng cách, tọa độ của đối tƣợng trong không gian 3D. Hình 2 cho thấy mối quan hệ giữa hệ tọa độ camera và ảnh của nó. Hệ tọa độ camera trái là OLXLYLZL có gốc trùng với tiêu điểm của camera trái, hệ tọa độ camera phải là ORXRYRZR, gốc trùng với tiêu điểm của camera phải và hệ tọa độ camera là OCXCYCZC có gốc ở trung điểm gốc tọa độ 2 camera. Các frame ảnh đƣợc quy định ở phía trƣớc và vuông góc với trục Y tại tâm, các trục u, v song song với các trục Z, X của camera tƣơng ứng. Để giảm độ phức tạp của tính toán, giả sử các thông số nội tại của 2 camera nhƣ tiêu cự f, số pixel…là giống nhau, đƣợc đặt ở độ cao nhƣ nhau và trục quang song song với nhau. Góc Pan là góc θ1 quay quanh trục z0 trong hệ tọa độ gốc của bệ pan/tilt, góc Tilt θ2 là góc quay quanh trục z1 trong hệ tọa độ O1X1Y1Z1 của bệ pan/tilt. Gọi tọa độ ảnh thu đƣợc ở camera trái (UL, VL) và camera phải là (UR ,VR) theo hai trục tọa độ (u, v). Theo giả thiết 2 camera có cùng độ cao nên tọa độ ảnh thu đƣợc từ 2 camera cũng có cùng tọa độ theo trục v hay VR = VL. 116 (02): 41 - 46 Từ hình 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 hình 3, hình 4 ta có các quan hệ hình học trong khung tọa độ OCXCYCZC: f K f f K U L  (  Z ); VL  X ; U R   (  Z ) Y 2 Y Y 2 Với: 1 U L U R  Y f .K 42 (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 độ O C đƣợc tính theo biểu thức sau:   K   VL   2 X        2 K Q   Y   f     2  Z  U R U L     K    (U L  U R )  4  (3) K là khoảng cách giữa 2 trục camera f L  f R  f là tiêu cự ống kính camera Hình 3. Ảnh theo 2 trục Z, Y Hình 2. Mô hình hệ thống camera (1) Hình 4. Ảnh theo 2 trục X, Y Lê Văn Chung Tạp chí KHOA HỌC & CÔNG NGHỆ 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 Ω   XC YC ZC  thì véc tơ vận tốc của điểm Q trong hệ tọa độ camera là: Q  C v  C Ω x Q trong đó các thành phần (4) ZC (5) Z  TZC  Y  XC  X YC (6) YC XC Ta có quan hệ chuyển động tƣơng đối giữa chuyển động của giá mang camera C v, C Ω với chuyển động của T v, T Ω mục tiêu: C v  TX C Ω   X TY TZ    T v Y Z    T Ω Thực hiện vi phân các biểu thức trong (1) rồi thay vào (4), (5), (6) 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:   f    d f K   U   ( (  Z ))   Y  L   dt Y 2           d f  V L    ( X )    0    dt Y          d f K  U R   ( (  Z ))   f    dt Y 2     Y  0 UL Y f Y VL Y 0 UR Y J imag (m) là ma trận Jacobi ảnh:   U L (U L U R ) U LVL 2 f 2 U L2 U LU R  U R U L  0  VL   K fK f 2 f     2 2  U R U L VL (U L U R ) f VL VL (U L U R ) U L U R   J imag (m)   0   (8b) K fK f 2f 2 f     2 2  U R U L  U R (U L U R ) U RVL 2 f U R U LU R  0  VL   K  fK f 2f   u  TX TY TZ X  TXC  Z YC  Y ZC Y  T  Z   X   f 2 U L2 KU L  U LVL   VL   TZ   f f 2Y   TX    f 2 VL2 U LVL KVL Kf   TY    U L     (7) f f 2Y 2Y    Z      X f 2 U R2 KU R U RVL     VL    Y  f f 2Y   Thay (2) vào (7) ta có biểu thức (8a) nhƣ sau:   2 f 2 U L2 U LU R  U U  U L (U L U R ) U LVL 0  VL   TZ     R L   U   K fK f 2f   TX   L       2 2    U R U L VL (U L U R ) f VL VL (U L U R ) U L U R   TY  V L    0       (8a)    K f K f 2 f 2 f  Z           X U R   U U 2 f 2 U R2 U LU R U R (U L U R ) U RVL      R L 0  VL    Y     fK f 2f  K    Nhƣ vậy phƣơng trình (8a) mô tả quan hệ giữa tốc độ thay đổi của điểm ảnh trong 2 camera và tốc độ chuyển động tịnh tiến và quay của mục tiêu. Viết lại (8a) dƣới dạng ma trận Jacobi ta có phƣơng trình biểu diễn tốc độ điểm đặc trƣng ảnh của 2 camera nhƣ sau: (8b) m = J imag (m)u Trong đó: m  [UL VL UR ]T véc tơ đặc trƣng ảnh 3 thành phần (đặc trƣng ảnh quy đổi vào khung OCXLYCZC).  V  U  ]T là tốc độ di chuyển của điểm   [U m L L R ảnh. 116 (02): 41 - 46 X Y Z  véc tơ tốc độ T chuyển động (dài và góc) của camera. Xác định hệ phƣơng trình động học tốc độ rô bốt Pan-Tilt. Trƣớc tiên ta xác định hệ phƣơng trình động học của bệ Pan-Tilt trên cơ sở phép biến đổi đồng nhất giữa các khung tọa độ gắn trên các khớp của bệ [9]. Ma trận biến đổi đồng nhất chuyển hệ tọa độ OC về O1 và từ O1 về O0 (H. 1) lần lƣợt là: C2  S 1 RC   2 0 0  C1 0 l2 C 2      S1 0 l2 S2  0  ; R  1  0 1 0    0 0 1   S2 C2 0 0 0 S1 0 C1 1 0 0 0 0  0  l1  1  Ma trận biến đổi đồng nhất chuyển hệ tọa độ OC về O0 là: C1C2 C1 S2 S1 l2 C1C2     S1C2 S1 S2 C1 l2 S1C2  0 0 1  RC  R1 RC   C2 0 l1  l2 S2   S2  0 0 0 1   (9) Ký hiệu véc tơ biến trong của bệ pan tilt là q  [1 , 2 ]T và véc tơ biến ngoài của đầu tay nắm của bệ là x   x y z X Y Z  , T trong đó X Y Z là các góc quay theo ba trục X,Y,Z. Nhƣ vậy x mô tả vị trí và hƣớng của khung OC X CYC ZC nhìn trong hệ tọa độ rô bốt O0 X 0Y0 Z0 dƣới dạng véc tơ. Từ đây ta có véc tơ vận tốc: x   x y z  X  TX TY TZ  X Y Z   u T Y Z  T (10) Quan hệ tốc độ giữa biến trong và biến ngoài mô tả hệ động học tốc độ qua ma trận Jacobi của rô bốt nhƣ công thức (11) sau: (11) x = J robot (q)q với q  1 2  và J robot là ma trận Jacobi   a hệ pan/tilt đƣợc xác định từ ma trận chuyển hệ tọa độ 0 RC [10] nhƣ sau: T 43 Lê Văn Chung J robot l2C1C2 0     l2 S1C2 0     lS  l 2 2 1    S1   0   C1   0   0   1 Tạp chí KHOA HỌC & CÔNG NGHỆ 116 (02): 41 - 46  d luôn tiệm cận đến 0. Ta chọn trƣng ảnh m thuật điều khiển nhƣ sau: (12) Xây dựng bài toán điều khiển động học (kinematic control) hệ rô bốt-stereo camera bám mục tiêu. Từ véc tơ đặc trƣng ảnh m đo đƣợc và véc tơ đặc trƣng ảnh mong muốn m d ta tính đƣợc sai lệch đặc trƣng ảnh: (13) ε  md m Bài toán điều khiển động học (kinematic control) hệ rô bốt-stereo camera bám mục tiêu là tìm thuật điều khiển q = K(ε) sao cho hệ kín sau ổn định  d - Kε)] q = J + [(m (17) Trong đó J   (J T J )1 J T là ma trận giả nghịch đảo của J Trong đó K là ma trận ma trận đối xứng xác định dƣơng. Lấy đạo hàm (13) ta đƣợc: ˆ  -m  -m  d = Jq d ε = m ε = -Kε (18) Cấu trúc của hệ điều khiển có thể mô tả theo sơ đồ trên hình 6.  = J imag (m)u m u = x = J robot (q)q q = K(ε) Để đơn giản ta giả thiết các tham số của rô bốt và camera là xác định trƣớc. Thuật điều khiển cần tìm sao cho q = K(ε) ε = (m - md )  0. THIẾT KẾ THUẬT TOÁN ĐIỀU KHIỂN Thay thế (11) vào (8b) ta đƣợc (14):  = J imag (m)u  J imag (m)J robot (q)q m (14)   Jq m Trong đó ma trận Jacobi tổng hợp J  J imag (m)J robot (q) (15) Thay thế các ma trận Jacobi ảnh (8b) và Jacobi rô bốt (12) vào (15) ta có ma trận Jacobi tổng hợp biểu diễn trong công thức (16):   2 2  U R U L U (U U ) U L (U L U R ) U LVL 2 f U L U LU R   l2C1C2  L L R l2 S2 VL l1  S1  C1    K fK fK f 2f     2 2  U R U L VL (U L U R ) U L U R VL (U L U R ) f VL VL (U L U R )   J l2 S1C2  l2 S2  l1  S1  C1   K  fK 2f fK f 2f    2 U 2 U U  2 f  U R U L U ( U  U ) U ( U  U ) U V L R  R L R R L R R L R  l CC  l2 S2 VL l1  S C1   K 212 fK fK f 1 2f   (16) Nhƣ vậy ma trận Jacobi tổng hợp J có chiều là 3x2. Vấn đề đặt ra lúc này là phải xác định đƣợc tốc độ của các khớp q là bao nhiêu để đặc 44 Hình 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: 1 V  ε T ε (19) 2 Lấy đạo hàm V theo t ta có: (20) V  εTε  d m   m   Jq Trong đó: ε  m (21) Thay (17) vào (20), đạo hàm V có dạng: V  εT J + JKε (22) Vì J   (J T J )1 J T nên ta có J + J  I Vậy V  εT Kε  0 (23)   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ệ (18) là ổn định tiệm cận và do đó  (t )  m  d hay nói cách khác m(t )  md , m camera bám theo mục tiêu di động với sai lệch bằng 0. MÔ PHỎNG MINH HỌA TRÊN PC 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 trong công thức (16) Lê Văn Chung Tạp chí KHOA HỌC & CÔNG NGHỆ 116 (02): 41 - 46  a1 b1    J   a2 b2  thì J tính đƣợc nhƣ sau:   a b   3 3  a1C  b1 B a3C  b3 B  a2 C  b2 B     M M M J   (J T J )1 J T     a1 B  b1 A a2 B  b2 A a3 B  b3 A      M M M Ký hiệu A  a12  a22  a32 B  a1b1  a2 b2  a3 b3 C  b12  b22  b32 M  (a1b2  a2 b1 ) 2  (a1b3  a3b1 ) 2  (a2 b3  a3b2 ) 2 Chọn các tham số: K  diag  0.45 0.72 1  Và các thông số của hệ pan/tilt camera là: l1= 0.22 m K= 0.2 m l2= 0.1 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] 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ơ đồ trong H.6 ta thu đƣợc một số kết quả sau: Hình 7. Sai lệch tọa độ ảnh mục tiêu (pixel). 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 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). Mô phỏng ta thu đƣợc một số kết quả sau: Hình 8. Sai lệch bám khi mục tiêu di chuyển theo đường thẳng Hình 9. Sai lệch tọa độ ảnh khi mục tiêu di chuyển theo đường thẳng Mô phỏng 3: Mục tiêu cơ động theo cung tròn Mục tiêu đi theo cung tròn tâm tại gốc tọa độ O(0,0), 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). Hình 10. Sai lệch bám quỹ đạo với mục tiêu cơ động theo cung tròn 45