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

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.

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 truyền thơng – ĐH Thái Ngun TĨM TẮT Báo cáo đề xuấ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 đầu tay nắm rô bốt Pan-Tilt có bậc tự Thuật tốn điều khiển hệ thống bám theo đối tƣợng di chuyển tự cách điều khiển robot cho ảnh thu đƣợc đối tƣợng di chuyển ln vị trí trung tâm camera Bài viết đƣa mơ hình động học hệ Pan-Tilt sở bệ Pan-Tilt PTU-D48E-Series, tính tốn vị trí đối tƣợng hình ảnh, khoảng cách góc Pan-Tilt phƣơng pháp điều khiển để giữ đối tƣợng ln trung tâm 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 hệ thống quân quốc phòng an ninh Theo cấu trúc phân hệ thành nhóm có camera cố định điều khiển đƣợc Hệ thống có camera [7], xác định đƣợc hƣớng mục tiêu nên cần sử dụng thêm máy đo khoảng cách Nếu sử dụng camera [1], [2] thơng số khoảng cách hồn tồn xác định đƣợc từ ảnh camera Với nhóm camera di động, camera quay quét với góc khác [3], [5], [6] có ƣu điểm điều khiển hệ camera di chuyển bám theo đối tƣợng để đối tƣợng trung tâm ả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 rô bốt [4], [8] đƣợc camera Hệ thống đƣợc thiết kế dựa mơ hình bệ pan/tilt PTU-D48ESeries nhƣ hình Nội dung báo gồm phần xây dựng mơ hình động học hệ rơ bốt stereo bám mục tiêu di động, thiết kế thuật tốn điều khiển hệ servo thị giác, mơ kiểm chứng 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 rô bốt hai bậc tự quay đồng thời theo hai hƣớng pan 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 khớp rô bốt camera đƣợc mơ tả Hình Để bám đƣợc mục tiêu có tốc độ di chuyển khác ta cần điều khiển khớp bệ Pan-Tilt mang camera thay đổi phù hợp Để làm đƣợc điều tốc độ di chuyển mục tiêu phải tính tốn hay đo đƣợc, ngồi thơng số hệ thƣờng khơng hồn tồn xác nên việc bám mục tiêu di động toán phức tạp Trong báo cáo này, góc pan/tilt đƣợc điều chỉnh cho ảnh mục tiêu đƣợc trì vị trí mong muốn mơ hình ảnh thu * Tel: 01654 236119, Email: lvchung@ictu.edu.vn Hình 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ừ camera quy vào hệ tọa độ camera OcXcYcZc Dùng camera ta xác định đƣợc thơng số nhƣ khoảng cách, tọa độ đối tƣợng không gian 3D Hình cho thấy mối quan hệ hệ tọa độ camera ảnh Hệ tọa độ camera trái OLXLYLZL có gốc trùng với tiêu điểm camera trái, hệ tọa độ camera phải ORXRYRZR, gốc trùng với tiêu điểm camera phải hệ tọa độ camera OCXCYCZC có gốc trung điểm gốc tọa độ camera Các frame ảnh đƣợc quy định phía trƣớc vng góc với trục Y tâm, trục u, v song song với trục Z, X camera tƣơng ứng Để giảm độ phức tạp tính tốn, giả sử thơng số nội camera nhƣ tiêu cự f, số pixel…là giống nhau, đƣợc đặt độ cao nhƣ trục quang song song với Góc Pan góc θ1 quay quanh trục z0 hệ tọa độ gốc bệ pan/tilt, góc Tilt θ2 góc quay quanh trục z1 hệ tọa độ O1X1Y1Z1 bệ pan/tilt Gọi tọa độ ảnh thu đƣợc camera trái (UL, VL) camera phải (UR ,VR) theo hai trục tọa độ (u, v) Theo giả thiết camera có độ cao nên tọa độ ảnh thu đƣợc từ camera có tọa độ theo trục v hay VR = VL 116 (02): 41 - 46 Từ hình ta có tọa độ ảnh thu đƣợc camera trái (-UL, VL) camera phải (UR ,VR) với V= VL= VR thực chuyển mặt phẳng theo (Z, Y) (X, Y) nhƣ hình hình 3, hình ta có quan hệ hình học khung tọa độ OCXCYCZC: f K f f K U L  (  Z ); VL  X ; U R   (  Z ) Y Y Y Với: U L U R  Y f K 42 (2) Từ công thức (1), (2) phép biến đổi ta tính đƣợc tọa độ điểm Q(X, Y, Z) [5] hệ tọa độ O C đƣợc tính theo biểu thức sau:   K   VL   X        K Q   Y   f      Z  U R U L     K    (U L  U R )   (3) K khoảng cách trục camera f L  f R  f tiêu cự ống kính camera Hình Ảnh theo trục Z, Y Hình Mơ hình hệ thống camera (1) Hình Ảnh theo trục X, Y Lê Văn Chung Tạp chí KHOA HỌC & CƠNG NGHỆ Khi đối tƣợng di chuyển bệ Pan-Tilt mang camera 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 gốc tọa độ camera C v  TXC TYC TZC  véc tơ tốc độ góc C Ω   XC YC ZC  véc tơ vận tốc điểm Q hệ tọa độ camera là: Q  C v  C Ω x Q 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 chuyển động giá mang camera C v, C Ω với chuyển động T v, T Ω mục tiêu: C v  TX C Ω   X TY TZ    T v Y Z    T Ω Thực vi phân biểu thức (1) thay vào (4), (5), (6) ta đƣợc quan hệ tốc độ điểm đặc trƣng ảnh camera với vị trí hƣớng giá mang camera:   f    d f K   U   ( (  Z ))   Y  L   dt Y           d f  V L    ( X )       dt Y          d f K  U R   ( (  Z ))   f    dt Y     Y  UL Y f Y VL Y UR Y J imag (m) ma trận Jacobi ảnh:   U L (U L U R ) U LVL f U L2 U LU R  U R U L   VL   K fK f f     2  U R U L VL (U L U R ) f VL VL (U L U R ) U L U R   J imag (m)     (8b) K fK f 2f f     2  U R U L  U R (U L U R ) U RVL f U R U LU R   VL   K  fK f 2f   u  TX TY TZ X  TXC  Z YC  Y ZC Y  T  Z   X   f U L2 KU L  U LVL   VL   TZ   f f 2Y   TX    f VL2 U LVL KVL Kf   TY    U L     (7) f f 2Y 2Y    Z      X f 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:   f U L2 U LU R  U U  U L (U L U R ) U LVL  VL   TZ     R L   U   K fK f 2f   TX   L       2    U R U L VL (U L U R ) f VL VL (U L U R ) U L U R   TY  V L          (8a)    K f K f f f  Z           X U R   U U f U R2 U LU R U R (U L U R ) U RVL      R L  VL    Y     fK f 2f  K    Nhƣ phƣơng trình (8a) mô tả quan hệ tốc độ thay đổi điểm ảnh camera tốc độ chuyển động tịnh tiến quay 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 camera nhƣ sau: (8b) m = J imag (m)u Trong đó: m  [UL VL UR ]T véc tơ đặc trƣng ảnh thành phần (đặc trƣng ảnh quy đổi vào khung OCXLYCZC)  V  U  ]T tốc độ di chuyển đ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 góc) 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 bệ Pan-Tilt sở phép biến đổi đồng khung tọa độ gắn khớp bệ [9] Ma trận biến đổi đồng chuyển hệ tọa độ OC O1 từ O1 O0 (H 1) lần lƣợt là: C2  S RC   0 0  C1 l2 C      S1 l2 S2   ; R   0    0   S2 C2 0 S1 C1 0 0   l1   Ma trận biến đổi đồng chuyển hệ tọa độ OC O0 là: C1C2 C1 S2 S1 l2 C1C2     S1C2 S1 S2 C1 l2 S1C2  0  RC  R1 RC   C2 l1  l2 S2   S2  0   (9) Ký hiệu véc tơ biến bệ pan tilt q  [1 , 2 ]T véc tơ biến đầu tay nắm bệ x   x y z X Y Z  , T X Y Z góc quay theo ba trục X,Y,Z Nhƣ x mơ tả vị trí hƣớng khung OC X CYC ZC nhìn hệ tọa độ rơ bốt O0 X 0Y0 Z0 dƣới dạng véc tơ Từ 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 độ biến biến ngồi mơ tả hệ động học tốc độ qua ma trận Jacobi rô bốt nhƣ công thức (11) sau: (11) x = J robot (q)q với q  1 2  J robot ma trận Jacobi   a hệ pan/tilt đƣợc xác định từ ma trận chuyển hệ tọa độ RC [10] nhƣ sau: T 43 Lê Văn Chung J robot l2C1C2     l2 S1C2     lS  l 2    S1     C1       Tạp chí KHOA HỌC & CƠNG NGHỆ 116 (02): 41 - 46  d tiệm cận đến Ta chọn trƣng ảnh m thuật điều khiển nhƣ sau: (12) Xây dựng 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é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 tìm thuật điều khiển q = K(ε) cho hệ kín sau ổn định  d - Kε)] q = J + [(m (17) Trong J   (J T J )1 J T ma trận giả nghịch đảo J Trong K 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 hệ điều khiển mơ tả theo sơ đồ hình  = J imag (m)u m u = x = J robot (q)q q = K(ε) Để đơn giản ta giả thiết tham số rô bốt camera xác định trƣớc Thuật điều khiển cần tìm cho q = K(ε) ε = (m - md )  THIẾT KẾ THUẬT TOÁN ĐIỀU KHIỂN Thay (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 ma trận Jacobi ảnh (8b) Jacobi rơ bốt (12) vào (15) ta có ma trận Jacobi tổng hợp biểu diễn công thức (16):   2  U R U L U (U U ) U L (U L U R ) U LVL f U L U LU R   l2C1C2  L L R l2 S2 VL l1  S1  C1    K fK fK f 2f     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    U U U  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 2f   (16) Nhƣ ma trận Jacobi tổng hợp J có chiều 3x2 Vấn đề đặt lúc phải xác định đƣợc tốc độ khớp q để đặc 44 Hình Cấu trúc 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: V  ε T ε (19) 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ε  (23)   Ta thấy V ε V ε  Theo nguyên lý ổn định Lyapunov ta có sai lệch tốc độ ε  , ε  Nhƣ hệ (18) ổn định tiệm cận  (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 MÔ PHỎNG MINH HỌA TRÊN PC Tiến hành mô hệ thống matlab simulink với J ma trận giả nghịch đảo J 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  J tính đƣợc nhƣ sau:   a b   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 )  (a1b3  a3b1 )  (a2 b3  a3b2 ) Chọn tham số: K  diag  0.45 0.72  Và thông số hệ pan/tilt camera là: l1= 0.22 m K= 0.2 m l2= 0.1 m f= 360 pixel UL, VL, UR, VR: tọa độ ảnh trái phải có đơn vị tính pixel Tọa độ ảnh ban đầu: m=[ UL, VL, UR ] = [0, 0, 0] Mô 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 mục tiêu đứng yên mt=[-20, 0, 20] (pixel); Tiến hành mô với sơ đồ H.6 ta thu đƣợc số kết sau: Hình Sai lệch tọa độ ảnh mục tiêu (pixel) Mô 2: Mục tiêu di chuyển theo đƣờng thẳng Điểm mục tiêu từ điểm A có tọa độ A(0m,1.8m,0m) tới điểm B(-0.3m, 1.8m, 0.5m) mặt phẳng ZCOCXC cách gốc tọa độ khoảng YC = 1.8m hệ tọa độ camera OCXCYCZC Thời gian chuyển động mục tiêu T=30(s) với tốc độ di chuyển v ~ (cm/s) Mô ta thu đƣợc số kết sau: Hình Sai lệch bám mục tiêu di chuyển theo đường thẳng Hình Sai lệch tọa độ ảnh mục tiêu di chuyển theo đường thẳng Mô 3: Mục tiêu động theo cung tròn Mục tiêu theo cung tròn tâm gốc tọa độ O(0,0), bán kính r = mặt phẳng ZCOCXC cách gốc tọa độ khoảng YC = 1.8m hệ tọa độ camera OCXCYCZC Chuyển động mục tiêu theo cung tròn 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 ~ (cm/s) Hình 10 Sai lệch bám quỹ đạo với mục tiêu động theo cung tròn 45 Lê Văn Chung Tạp chí KHOA HỌC & CÔNG NGHỆ 116 (02): 41 - 46 TÀI LIỆU THAM KHẢO Hình 11 Sai lệch tọa độ ảnh với mục tiêu động theo cung tròn KẾT LUẬN Bài báo xây dựng đƣợc mơ hình tốn học hệ robot Pan-Tiltt – thị giác stereo đề xuất phƣơng pháp điều khiển cho hệ servo thị giác stereo Với phƣơng pháp điều khiển hệ thống bám mục tiêu xuất cố định (Hình 7) mục tiêu chuyển động theo đƣờng thẳng (Hình 8) nhƣng có sai lệch Đối với đối tƣợng di chuyển theo cung tròn hay có vận tốc, gia tốc thay đổi việc bám hệ thống đáp ứng có nhiều sai lệch Để hệ thống đáp ứng tốt cần có phƣơng pháp điều khiển đại, khử đƣợc nhiễu ma sát khớp, bù đƣợc thành phần bất định robot đƣợc nghiên cứu tiếp báo cáo sau Huiguang Li, Mei Jin, Liying Zou, (2008) A New Binocular Stereo Visual Servoing Model, IEEE, pp 461-465 Masaaki Shibata, Hideki Eto, Masahide Ito (2011): Visual tracking control for stereo vision robot with high gain controller and high speed cameras IEEE pp 288 – 293 Masaaki Shibata, Hideki Eto, Masahide Ito (2010): Image-based Visual Tracking to Fast Moving Target for Active Binocular Robot IEEE pp 2727 – 2732 Widodo Budiharto, Ari Santoso, Djoko Purwanto, Achmad Jazidie, (2011) A Navigation System for Service Robot using Stereo Vision and Kalman Filtering, International Conference on Control, Automation and Systems pp 1771 – 1776 Hiroyuki Ukida, Yasuhiro Terama and Hiroki Ohnishi.“Object Tracking System by Adaptive Pan-tilt-zoom Cameras and Arm Robot” SICE Annual Conference 2012 pp 1920 – 1925 Ae-Il Kim, Wook Bahn, Changhun Lee, Tae-jae Lee, Muhammad Muneeb Shaikh, and Kwang-soo Kim (2011) “ Vision System for Mobile Robots for Tracking Moving Targets, Based on Robot Motion and Stereo Vision Information” SI International IEEE, pp 634-639 Phạm Thƣợng Cát, Trần Việt Phong, Nghiên cứu phát triển hệ robot-camera tự động tìm kiếm bám đối tƣợng di động VICON Phạm Đức Long, Phạm Thƣợng Cát “Xử lý ảnh để xác định hƣớng khoảng cách giá đỡ xe nâng hàng tự động”, Hội nghị toàn quốc Điều khiển Tự động hoá-VCCA-2011, pp 525-531 Loenzo Sciavicco and Bruno Siciliano, (1976) Modeling and control of robot manipulators, McGrow Hill International Editions SUMMARY DEVELOPMENT PAN TILT - STEREO CAMERA SYSTEM TO TRACKING MOVING OBJECT Le Van Chung* College of Information and Communication Technology - TNU This paper proposes a control method robot - pair cameras to tracking mobile target Pair camera placed on the robot arm, robot arm control cameras has degrees of freedom The algorithms control system tracking moving object with orbits freedom by control pan/tilt robot such a way that the image of a freedom moving object attains the center of two cameras This paper construct kinetic models of the pan/tilt pedestal base on PTU-D48E-Series pan/tilt pedestal, calculate the position of the object in the images, the distances and angles of the base pan, tilt to keep object always in the center of two cameras Key word: tracking target; pan/tilt; PTU-D48E-Series; robot; stereo camera Ngày nhận bài:25/01/2014; Ngày phản biện:10/02/2014; Ngày duyệt đăng: 26/02/2014 Phản biện khoa học: TS Phạm Đức Long – Trường ĐH Công nghệ Thông tin & Truyền thông - ĐHTN * Tel: 01654 236119, Email: lvchung@ictu.edu.vn 46 ... mối quan hệ hệ tọa độ camera ảnh Hệ tọa độ camera trái OLXLYLZL có gốc trùng với tiêu điểm camera trái, hệ tọa độ camera phải ORXRYRZR, gốc trùng với tiêu điểm camera phải hệ tọa độ camera OCXCYCZC... song với Góc Pan góc θ1 quay quanh trục z0 hệ tọa độ gốc bệ pan/ tilt, góc Tilt θ2 góc quay quanh trục z1 hệ tọa độ O1X1Y1Z1 bệ pan/ tilt Gọi tọa độ ảnh thu đƣợc camera trái (UL, VL) camera phải... f tiêu cự ống kính camera Hình Ảnh theo trục Z, Y Hình Mơ hình hệ thống camera (1) Hình Ảnh theo trục X, Y Lê Văn Chung Tạp chí KHOA HỌC & CƠNG NGHỆ Khi đối tƣợng di chuyển bệ Pan- Tilt mang camera