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

BỘ GIÁO DỤC VÀ ĐÀO TẠO VIỆN HÀN LÂM KHOA HỌC 
VÀ CÔNG NGHỆ VIỆT NAM 
HỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ 
----------------------------- 
Lê Văn Chung 
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 
LUẬN ÁN TIẾN SĨ KỸ THUẬT 
Hà Nội – 2019 
BỘ GIÁO DỤC VÀ ĐÀO TẠO VIỆN HÀN LÂM KHOA HỌC 
VÀ CÔNG NGHỆ VIỆT NAM 
HỌC VIỆN KHOA HỌC VÀ CÔNG NGHỆ 
----------------------------- 
Lê Văn Chung 
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 
 Chuyên ngành: Kỹ thuật điều khiển và tự động hóa 
 Mã số: 9.52.02.16 
LUẬN ÁN TIẾN SĨ KỸ THUẬT 
 NGƯỜI HƯỚNG DẪN KHOA HỌC: 
 1. PGS.TSKH Phạm Thượng Cát 
 2. TS. Phạm Minh Tuấn 
Hà Nội – 2019 
i 
LỜI CAM ĐOAN 
Tôi xin cam đoan đây là công trình nghiên cứu của riêng tôi. Các kết quả 
được viết chung với các tác giả khác đều được sự đồng ý của đồng tác giả trước khi 
đưa vào luận án. Các kết quả trong luận án là trung thực và chưa từng được công bố 
trong bất kỳ công trình nào khác. 
 Tác giả luận án 
Lê Văn Chung 
ii 
LỜI CẢM ƠN 
Lời đầu tiên, Tôi xin chân thành cảm ơn Ban Lãnh đạo Học viện Khoa học 
và Công nghệ, Viện Công nghệ thông tin - Viện Hàn lâm Khoa học và Công nghệ 
Việt Nam, Phòng Công nghệ tự động hóa đã tạo điều kiện thuận lợi trong quá trình 
học tập, nghiên cứu. 
Tôi xin bày tỏ lòng kính trọng và biết ơn sâu sắc tới PGS.TSKH. 
Phạm Thượng Cát và TS. Phạm Minh Tuấn, hai thầy đã định hướng và tận tình 
hướng dẫn để tôi có thể hoàn thành luận án. 
Tôi xin cảm ơn Ban Giám hiệu Trường Đại học Công nghệ Thông 
tin và Truyền thông - Đại học Thái Nguyên, Khoa Công nghệ tự động hóa và 
các đơn vị trong Nhà trường đã quan tâm giúp đỡ, tạo điều kiện để tôi có thể thực 
hiện nghiên cứu. 
Tôi xin cảm ơn các cán bộ Phòng Công nghệ tự động hóa – Viện Công nghệ 
thông tin, các đồng nghiệp thuộc Khoa Công nghệ Tự động hóa - Trường Đại học 
Công nghệ thông tin và truyền thông - Đại học Thái Nguyên đã động viên và trao 
đổi kinh nghiệm trong quá trình hoàn thành luận án. 
Cuối cùng, tôi xin chân thành cảm ơn đến gia đình, người thân, các bạn đồng 
nghiệp - những người luôn dành cho tôi những tình cảm nồng ấm, luôn động viên 
và sẻ chia những lúc khó khăn trong cuộc sống và tạo điều kiện tốt nhất để tôi có 
thể hoàn thành quá trình nghiên cứu. 
Hà Nội, ngày 28 tháng 8 năm 2019 
 Tác giả luận án 
 Lê Văn Chung 
iii 
MỤC LỤC 
LỜI CAM ĐOAN ....................................................................................................... i 
LỜI CẢM ƠN ............................................................................................................ ii 
MỤC LỤC ................................................................................................................ iii 
DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT ........................................... vii 
DANH MỤC HÌNH VẼ ............................................................................................ x 
DANH MỤC BẢNG .............................................................................................. xiii 
MỞ ĐẦU .................................................................................................................... 1 
CHƢƠNG 1................................................................................................................ 7 
TỔNG QUAN ............................................................................................................ 7 
1.1 Đặt vấn đề ........................................................................................................ 7 
1.2. Tổng quan về điều khiển rô bốt sử dụng thông tin hình ảnh ..................... 8 
1.2.1 Tình hình nghiên cứu ngoài nước ........................................................... 11 
1.2.2 Tình hình nghiên cứu trong nước .............................................................. 8 
1.3 Các vấn đề nghiên cứu của luận án ............................................................. 19 
1.3.1. Phát triển phương pháp điều khiển rô bốt s dụng thông tin hình ảnh ... 19 
1.3.2. 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
 ........................................................................................................................... 20 
1.3.3. Phát triển thuật toán điều khiển rô bốt di động s dụng thông tin hình 
ảnh. .................................................................................................................... 21 
1.4. Kết luận chƣơng 1 ........................................................................................ 22 
CHƢƠNG 2.............................................................................................................. 23 
PHÁT TRIỂN THUẬT TOÁN ĐIỀU KHIỂN HỆ PAN-TILT SỬ DỤNG 
THÔNG TIN HÌNH ẢNH TỪ HAI CAMERA .................................................... 23 
2.1. Xây dựng mô hình động học hệ pan-tilt-stereo camera bám mục tiêu di 
động với nhiều tham số bất định ................................................................... 24 
iv 
2.1.1 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 độ OcXcYcZc ........................................................................... 24 
2.1.2 Xác định hệ phương trình động học tốc độ hệ pan-tilt. ........................... 28 
2.1.3 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. ............................................................................. 29 
2.2.Thiết kế thuật toán điều khiển ..................................................................... 30 
2.2.1 Xây dựng mô hình bộ điều khiển ............................................................. 30 
2.2.2 Xây dựng thuật toán điều khiển hệ Pan-tilt –2 camera bám mục tiêu di 
động ................................................................................................................... 31 
2.3 Một số kết quả mô phỏng kiểm chứng ........................................................ 35 
2.4. Kết luận chƣơng 2 ........................................................................................ 43 
CHƢƠNG 3.............................................................................................................. 45 
MỘT SỐ CẢI TIẾN TRONG ĐIỀU KHIỂN ....................................................... 45 
HỆ SERVO TH GIÁC BÁM MỤC TIÊU DI ĐỘNG ........................................ 45 
3.1 dựng mô hình 3D cho hệ 2 camera trên hệ pan-tilt ........................... 47 
3.1.1 Mô hình 3D cho hệ stereo camera ........................................................... 47 
3.1.2 Mô hình hệ camera ảo. ............................................................................. 47 
3.1.3 Kiểm soát sự suy biến của ma trận Jacobian ......................................... 53 
3.1.4 Bài toán điều khiển rô bốt bám mục tiêu với nhiều tham số bất định ..... 53 
3.2. dựng hệ động lực học hệ pan-tilt – stereo camera với các tham số 
bất định ............................................................................................................ 53 
3.3. dựng bộ điều khiển nơ ron cho hệ động lực học hệ pan-tilt stereo 
camera bám mục tiêu di động. ...................................................................... 55 
3.3.1 Xây dựng bộ điều khiển .......................................................................... 55 
3.3.2 Xây dựng cấu trúc các lớp bộ điều khiển nơ ron truyền th ng RBF cho hệ 
thống .................................................................................................................. 56 
3.3.3 Tối ưu tham số ......................................................................................... 57 
3.4. Mô phỏng hệ thống ....................................................................................... 61 
v 
3.5. Kết luận ......................................................................................................... 72 
CHƢƠNG 4.............................................................................................................. 74 
PHÁT TRIỂN THUẬT TOÁN ĐIỀU KHIỂN RÔ BỐT DI ĐỘNG .................. 74 
SỬ DỤNG THÔNG TIN HÌNH ẢNH. .................................................................. 74 
4.1. Xây dựng mô hình động học và bài toán hệ điều khiển Rô bốt-Pan-tilt–
Stereo Camera bám mục tiêu di động .......................................................... 75 
4.1.1 Xác định ma trận Jacobi ảnh .................................................................... 75 
4.1.2 Xác định ma trận Jacobi của hệ và tốc độ bám mục tiêu cho rô bốt di 
động. .................................................................................................................. 77 
4.1.3 Xác định tốc độ của các bánh xe cho rô bốt di động để rô bốt tiếp cận 
mục tiêu. ............................................................................................................ 81 
4.2. Thiết kế thuật toán điều khiển .................................................................... 82 
4.2.1 Động lực học hệ rô bốt di động-bệ pan-tilt .............................................. 82 
4.2.2 Thiết kế bộ điều khiển tối ưu ................................................................... 83 
4.3. Mô phỏng hệ thống ....................................................................................... 89 
4.3.1 Mô phỏng hệ thống với bộ điều khiển tối ưu ........................................... 89 
4.3.2 Mô phỏng hệ thống với bộ điều khiển trượt CTC.................................... 94 
4.4. Kết luận chƣơng 4 ........................................................................................ 96 
5. KẾT LUẬN .......................................................................................................... 97 
5.1. Những nội dung nghiên cứu chính của luận án ......................................... 97 
5.2.Những đóng góp khoa học mới của luận án ............................................... 97 
5.3.Định hƣớng nghiên cứu phát triển .............................................................. 98 
DANH MỤC CÁC CÔNG TRÌNH ĐÃ CÔNG BỐ ........................................... 100 
TÀI LIỆU THAM KHẢO .................................................................................... 101 
PHỤ LỤC ............................................................................................................... 111 
1. Mô hình mô phỏng về hệ pan-tilt stereo camera bám mục tiêu di động có 
sử dụng mô hình camera 3D ảo ................................................................... 111 
vi 
2. Mô hình mô phỏng về hệ pan-tilt stereo camera bám mục tiêu di động có 
sử dụng mô hình camera 3D ảo ................................................................... 118 
vii 
DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT 
Ký hiệu 
Đơn vị 
tính 
Mô tả 
CTC Computed Torque Controller – bộ điều khiển dựa 
trên phương pháp tính mô men. 
d m Khoảng cách giữa 2 bánh xe rô bốt di động. 
d(t) Vecto ảnh hư ng của nhi u, các tham số bất định. 
f pixel Tiêu cự camera (1 pixel = 35μm) 
fv pixel Tiêu cự thấu kính của camera ảo. 
f Các thành phần bất định trong mô hình động học hệ 
pan-tilt. 
fu Các thành phần bất định trong mô hình động lực học 
hệ pan-tilt. 
H Hàm Hamilton. 
Jimag Ma trận Jacobi ảnh. 
Jrobot Ma trận Jacobi rô bốt. 
J Ma trận Jacobi tổng hợp. 
ˆ ˆ( ), ( )imag robotJ m J q Các giá trị biết được của các ma trận tương ứng. 
( ), ( )imag robotΔJ m ΔJ q Các đại lượng không biết của các ma trận tương 
ứng. 
 Ma trận giả nghịch đảo của ma trận J; 
K m Khoảng cách 2 camera. 
k m khoảng cách giữa hai bánh xe rô bốt di động. 
k1 m Khoảng cách giữa rô bốt di động và mục tiêu. 
l1 m Chiều dài khớp pan củahệ pan-tilt. 
l2 m Chiều dài khớp tilt củahệ pan-tilt. 
LQR Linear–Quadratic Regulator. 
T 1 Tˆ ˆ ˆ ˆ( )J J J J
T 1 Tˆ ˆ ˆ ˆ( )J J J J
viii 
LQG Linear–Quadratic–Gaussian. 
m Pixel Véc tơ đặc trưng ảnh. 
md Pixel Véc tơ đặc trưng ảnh mong muốn. 
Q Tọa độ mục tiêu. 
q Véc tơ vị trí góc của khớp pan, tilt 
r
q Véc tơ vị trí góc của khớp pan, tilt 
RBF Radial basis function neural network 
 s Véc tơ sai số giữa giá trị mong muốn và đo được. 
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 
U, V Pixel Tọa độ ảnh của đối tượng 
,
1u Thành phần điều khiển nơ ron 
u* Thành phần điều khiển tối ưu 
 v véc tơ biến ngoài của đầu tay nắm của bệ pan-tilt 
(trùng với gốc tọa độ camera OC) 
C
v Véc tơ vận tốc dài của đầu tay nắm của bệ pan-tilt 
CΩ Véc tơ vận tốc góc của đầu tay nắm của bệ pan-tilt 
vs Véc tơ vận tốc đo được tại 2 bánh rô bốt di động và 
2 khớp của bệ pan-tilt. 
vd Véc tơ vận tốc mong muốn đặt lên 2 bánh rô bốt di 
động và 2 khớp của bệ pan-tilt. 
 x Véc tơ mô tả vị trí và hướng của camera. 
 xs Véc tơ tọa độ mục tiêu nhìn trong hệ tọa độ camera 
ảo. 
 W Ma trận trọng số của mạng nơ ron 
 ε Sai số giữa tham số mong muốn và đo được 
θ1 rad Góc quay của khớp pan 
θ2 rad Góc quay của khớp tilt 
m rad Góc hướng của rô bốt di động 
θ1d rad Góc quay mong muốn của khớp pan 
θ2d rad Góc quay mong muốn của khớp tilt 
ix 
md rad Góc hướng mong muốn của rô bốt di động 
 m Khoảng cách từ gốc tới vị trí đặt camera ảo 
jjc , Trọng tâm và độ rộng khoảng cách tính từ tâm của 
lớp n thứ j radial basis function của mạng nơ ron. 
φl , φr rad Góc quay của bánh xe trái, phải trên rô bốt di động 
Stereo camera Hệ 2 camera được gắn trên khung cố định củahệ 
pan-tilt và có các quan hệ hình học được xác định 
trước. 
x 
DANH MỤC HÌNH VẼ 
Hình 1.1 Các ứng dụng điều khiển rô bốt s dụng thông tin hình ảnh. .................... 11 
Hình 1.2 Một số phương pháp điều khiển rô bốt s dụng thông tin hình ảnh .......... 14 
Hình 2.1 Bệ pan-tilt PTU-D48E-Series và biểu di n hệ tọa độ ................................ 24 
Hình 2.2 Mô hình hệ thống camera ........................................................................... 25 
Hình 2.3 Ảnh theo 2 trục Z, Y .................................................................................. 26 
Hình 2.4 Ảnh theo 2 trục X, Y .................................................................................. 26 
Hình 2.5 Mạng RBF xấp xỉ hàm ........................................................................... 32 
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 .......................................................................................... 34 
Hình 2.7 Sai lệch tọa độ ảnh mục tiêu (pixel). .......................................................... 37 
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 ). ........................................................................................................ 37 
Hình 2.9 Sai lệch bám tọa độ khi mục tiêu di chuyển theo đường ... ae-I Kim, Wook Bahn, Changhun Lee, Tae-jae Lee, Muhammad Muneeb 
Shaikh, and Kwang-soo Kim, Vision System for Mobile Robots for Tracking 
Moving Targets, Based on Robot Motion and Stereo Vision Information, IEEE 
International 2011, pp. 634 – 639. 
108 
[68] Thuan Hoang Tran, Van Tinh Nguyen, Minh Tuan Pham, Thuong Cat Pham, 
Trajectory Tracking Control of a Mobile Robot by computed torque method with 
on-line learning neural network, IEEE 2013 on Control and Automation, pp 
1184-1189. 
[69] T. Hu and S. Yang, A novel tracking control method for a wheeled mobile robot, 
in: Proc. of 2nd Workshop on Conference Kinematics. Seoul, Korea, May 20-22, 
2001, pp. 104-116. 
[70] T. Hu and S. Yang, A novel tracking control method for a wheeled 
mobile robot, Proc. of 2nd Workshop on Computational Kinematics, 
Seoul, Korea, May 20-22, 2001, pp. 104-116. 
[71] T. Hu, S. Yang, F. Wang, G. Mittal, A neural network for a 
nonholonomic mobile robot with unknown robot parameters. Proc. of 
the 2002 IEEE Int. Conf. on Robotics & Automation, Washington 
DC., May 2002. 
[72] T. Fukao, H. Nakagawa, and N. Adachi, “Adaptive tracking control of a 
nonholonomic mobile robot”, IEEE Trans. Robot. Autom, vol. 16, no. 1, pp. 609-
615, 2000. 
[73] Tinh Nguyen, Linh Le, “Neural network-based adaptive tracking control for a 
nonholonomic wheeled mobile robot with unknown wheel slips, model 
uncertainties, and unknown bounded disturbances”, Turkish Journal of Electrical 
Engineering & Computer Sciences, (2018) Vol 26: 378 – 392. 
[74] Tinh Nguyen, Thuong Hoang, Minhtuan Pham & Namphuong Dao, “A Gaussian 
wavelet network-based robust adaptive tracking controller for a wheeled mobile 
robot with unknown wheel slips”, International Journal of Control, online 
version https://doi.org/10.1080/00207179.2018.1458156. 
[75] Tinh Nguyen, Kiem Nguyentien, Tuan Do, Tuan Pham, “Neural Network-based 
Adaptive Sliding Model Control Method for Tracking of a Nonholonomic 
Wheeled Mobile Robot with Unknown Wheel Slips, Model Uncertainties, and 
Unknown Bounded External Disturbances”, Acta Polytechnica Hungarica Vol. 
15, No. 2, 2018, pp. 103-123. 
109 
[76] Wang C L, Mei Y C, Liang Z Y, Jia Q W. “Dynamic feedback tracking control of 
non-holonomic mobile robots with unknown camera parameters”. Transactions 
of the Institute of Measurement and Control, 2010, 32(2): 155-169. 
[77] Wang Y, Lang H X, Silva C W. “A hybrid visual servo controller for robust 
grasping by wheeled mobile robots”. IEEE/ASME Transactions on 
Mechatronics, 2010, 15(5): 757-769 
[78] Wang H S, Liu Y H, Zhou D X. “Adaptive visual servoing using point and line 
features with an uncalibrated eye-inhand camera”. IEEE Transactions on 
robotics, 2008, 24(4): 843-857. 
[79] WenYua, Marco A Moreno-Armendariz, Robust Visual Servoing of Robot 
Manipulators with Neuro Compensation, Published by Elsevier Ltd 2005, pp. 
824-838. 
[80] WenYu and XiaoouLi, System Identification Using Adjustable R F Neural 
Network with Stable Learning Algorithms, International Symposium on Neural 
Networks, 2004, pp. 212-217. 
[81] Widodo Budiharto, Ari Santoso, Djoko Purwanto, Achmad Jazidie, A Navigation 
System for Service Robot using Stereo Vision and Kalman Filtering, International 
Conference on Control, Automation and Systems, 26-29, 2011 in KINTEX, 
Gyeonggi-do, Korea, pp. 1771 – 1776. 
[82] Wook Bahn, Jaehong Park, Chang-hun Lee, Tae-il Kim, Teajae Lee, Muhammad 
Muneeb Shaikh, Kwang-soo Kim, and Dong-il “Dan” Cho, A Motion-
Information-Based Vision-Tracking System with a Stereo Camera on Mobile 
Robots, 2011 IEEE, pp. 252-257. 
[83] Yang, Z., Che, Y., EricCheng, K. W., 2007, Genetic Algorithm-Based RBF 
Neural Network Load Forecasting Model, IEEE Power Engineering Society 
General Meeting, 1298-1303. 
[84] Yang Fang, WANG Chao-Li, Adaptive Stabilization for Uncertain 
Nonholonomic Dynamic Mobile Robots Based on Visual Servoing Feedback, 
ACTA Automatic sinica, Vol. 37, No. 7, pp 857-864. 
110 
[85] Yong Huan. Yang, Yu Lin. Xu, Xin. Li, Wan Mi. Chen,Yan Kai. Chao, Stereo 
vision-based arm control for service robot, Control and Decision Conference 
(CCDC), 2011 Chinese, pp. 4048 – 4053. 
[86] Y. M. Zhao; W. F. Xie; X. W. Tu, Multiple cameras-multiple target points visual 
servoing in large scale 3D manufacturing systems, 2011 6th IEEE Conference on 
Industrial Electronics and Applications, pp. 2107 – 2113. 
111 
PHỤ LỤC 
1. Mô hình mô phỏng về hệ pan-tilt stereo camera bám mục tiêu di động có sử 
dụng mô hình camera 3D ảo 
Bộ điều khiển động lực học 
112 
#code bộ điều khiển động lực học 
function {{tor, AA] = fcn(epsilon_dot,J,e,M2,KD, 
KP,theta1d,theta2d,I1,I2,Lc2,theta1,theta2,J_dot) 
%#codegen 
a1 = J(1,1); 
b1 = J(1,2); 
a2 = J(2,1); 
b2 = J(2,2); 
a3 = J(3,1); 
b3 = J(3,2); 
A = a1^2+a2^2+a3^2; 
B = a1*b1+a2*b2+a3*b3; 
C = b1^2+b2^2+b3^2; 
M = (a1*b2-a2*b1)^2+(a1*b3-a3*b1)^2+(a2*b3-a3*b2)^2; 
j11 = 0.8*(a1*C-b1*B)/M; 
j12 = 0.8*(a2*C-b2*B)/M; 
j13 = 0.8*(a3*C-b3*B)/M; 
j21 = 0.8*(b1*A-a1*B)/M; 
j22 = 0.8*(b2*A-a2*B)/M; 
j23 = 0.8*(b3*A-a3*B)/M; 
Jinv = {{j11, j12, j13;... 
 j21, j22, j23] 
s1 = sin(theta1); 
c2 = cos(theta2); 
s2 = sin(theta2); 
m11 = I1+I2*s2^2+I1*c2^2; 
m22 = I2; 
m13 = (I1-I2)*s2*c2; 
113 
matrixM = {{m11, 0;... 
 0, m22]; 
c11 = -m13*theta2d; 
c12 = -m13*theta1d; 
c21 = m13*theta1d; 
matrixC = {{c11, c12;... 
 c21, 0]; 
g21= -9.8*M2*Lc2*cos(theta2); 
velG = {{0;g21]; 
qd = {{theta1d;theta2d]; 
AA = matrixM*Jinv; 
b = -matrixM*Jinv*J_dot*qd + matrixC*qd + velG; 
tor = matrixM*Jinv*(-KD*epsilon_dot-KP*e)+ b + 0.01*s1; 
*********************************************************** 
#code bộ điều khiển nơ ron 
 function TORnn = fcn(e,SIGMA,delta,eta,w1,w2,w3,A) 
%#codegen 
W = {{w1,w2,w3]; 
TORnn = A*((eta+1)*W*SIGMA - delta*e/sqrt(e(1)^2 + e(2)^2 + 
e(3)^2)); 
114 
Động học hệ pan-tilt
function Out = fcn(theta1,theta2,q_d,I1,I2,M2,Lc2) 
%#codegen 
m13 = (I1-I2)*sin(theta2)*cos(theta2); 
theta1d=q_d(1); 
theta2d=q_d(2); 
c11 = -m13*theta2d; 
c12 = -m13*theta1d; 
c21 = m13*theta1d; 
matrixC = {{c11, c12;... 
 c21, 0]; 
g21= -9.8*M2*Lc2*cos(theta2); 
velG = {{0;g21]; 
Out = matrixC*q_d + velG + 0.01*sin(theta1); 
115 
**********************************************************
********** 
function q_dot = fcn(input,I1,I2,theta2) 
%#codegen 
c2 = cos(theta2); 
s2 = sin(theta2); 
m11 = I1+I2*s2^2+I1*c2^2; 
m22 = I2; 
%m13 = (I1-I2)*s2*c2; 
matrixM = {{m11, 0;... 
 0, m22]; 
q_dot = matrixM\input; 
%v_dot = tor; 
Mô hình camera 3D ảo 
116 
function {{X_S_dot,J] = fcn(lamda,fv, xyz_vd, xyz_vdot, 
theta1, theta2,l1,l2, theta1d, theta2d) 
%#codegen 
Rcv=[1, 0, 0;... 
 0, 0, 1;... 
 0, -1, 0]; 
xv = xyz_vd(1); 
yv = xyz_vd(2); 
zv = xyz_vd(3); 
j11 = 1/(lamda-xv); 
j12 = zv/(lamda-xv)^2; 
j21 = -(xv-lamda)/(zv+lamda)^2; 
j22 = 1/(zv+lamda); 
j31 = -(yv)/(zv+lamda)^2; 
j33 = 1/(zv+lamda); 
Jvimg=[j11, j12, 0;... 
 j21, j22, 0;... 
 j31, 0 , j33]; 
c1 = cos(theta1); 
c2 = cos(theta2); 
s1 = sin(theta1); 
s2 = sin(theta2); 
Rco = {{c1*c2, -c1*s2, s1;... 
 s1*c2, - s1*s2, -c1;... 
 s2, c2, 0]; 
117 
Jrobot=[l2*c1*c2, 0;... 
 l2*s1*c2, 0;... 
 l2*s2, l1]; 
J=Jvimg*Rcv*Rco*Jrobot; 
thetad = {{theta1d;theta2d]; 
X_S_dot=fv*Jvimg*Rcv*Rco*xyz_vdot; 
Mô hình stereo camera 
function {{X_S, X_C]= fcn(J, q_dot, fv, t,lamda) 
%#codegen 
Rcv=[1, 0, 0;... 
 0, 0, 1;... 
 0, -1, 0]; 
118 
X_S = J*q_dot; 
Xs = X_S(1); 
Ys = X_S(2); 
Zs = X_S(3); 
Xv =lamda*(Ys*fv+Xs+fv)/(Xs+fv^2); 
Zv = Xs*(lamda-Xv)/fv; 
Yv = Zs*(lamda+Zv)/fv; 
X_V = {{Xv;Yv;Zv]; 
X_C = Rcv\X_V; 
 2. Mô hình mô phỏng về hệ phức hợp rô bốt di động mang pan-tilt stereo 
camera bám mục tiêu di động 
119 
Bộ điều khiển tối ƣu 
Hàm tính u* 
function usao = fcn(e,e_dot) 
%#codegen 
R=[ 0.25 , 0, 0, 0;... 
 0, 0.25, 0, 0;... 
 0, 0, 0.25, 0;... 
 0, 0, 0, 0.25]; 
alpha1=[15.5,0,0,0;... 
 0,15.6,0,0;... 
 0,0,15.6,0;... 
 0,0,0,15.4]; 
e2=e_dot+alpha1*e; 
usao=-R\e2; 
120 
Bộ điều khiển động lực học hệ rô bốt di động – pan-tilt – stereo camera 
Vector a dot 
function a_vector_es_dot = 
fcn(v,vd,vd_dot,r,b,theta3,Learn) 
%#codegen 
c2 = cos(theta2); 
s2 = sin(theta2) 
c3 = cos(theta3); 
s3 = sin(theta3); 
v1d_dot = vd_dot(1); 
v2d_dot = vd_dot(2); 
v3d_dot = vd_dot(3); 
v4d_dot = vd_dot(4); 
121 
v1d = vd(1); 
v2d = vd(2); 
v3d = vd(3); 
v4d = vd(4); 
e = v - vd; 
theta3_dot = v(4); 
theta2_dot = v(3); 
theta1_dot = (r/b)*(v(1) - v(2)); 
y11 = (v1d_dot - v2d_dot)*(r/b)^2; 
y12 = (v1d_dot - v2d_dot)*(r/b)^2 + v3d_dot*(r/b); 
y13 = 0.5*(1 - cos(2*theta3))*(r/b)*((v1d_dot - 
v2d_dot)*(r/b) + v3d_dot) + ... 
 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d - 
v2d)*(r/b) + v3d) + 0.5*(r/b)*sin(2*theta3)*(theta1_dot + 
theta2_dot)*v4d; 
y14 = 0.5*(1 + cos(2*theta3))*(r/b)*((v1d_dot - 
v2d_dot)*(r/b) + v3d_dot) - ... 
 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d - 
v2d)*(r/b) + v3d) - 0.5*(r/b)*sin(2*theta3)*(theta1_dot + 
theta2_dot)*v4d; 
y15 = 0; 
y21 = -y11; 
y22 = -y12; 
y23 = -y13; 
y24 = -y14; 
y25 = 0; 
122 
y31 = 0; 
y32 = (v1d_dot - v2d_dot)*(r/b) + v3d_dot; 
y33 = 0.5*(1-cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + 
v3d_dot) + ... 
 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + 
v3d) + (theta1_dot + theta2_dot)*v4d); 
y34 = 0.5*(1+cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + 
v3d_dot) - ... 
 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + 
v3d) + (theta1_dot + theta2_dot)*v4d); 
y35 = 0; 
y41 = 0; 
y42 = 0; 
y43 = -0.5*sin(2*theta3)*(theta1_dot + 
theta2_dot)*((r/b)*(v1d - v2d) + v3d); 
y44 = 0.5*sin(2*theta3)*(theta1_dot + 
theta2_dot)*((r/b)*(v1d - v2d) + v3d); 
y45 = v4d_dot; 
Y = {{y11, y12, y13, y14, y15;... 
 y21, y22, y23, y24, y25;... 
 y31, y32, y33, y34, y35;... 
 y41, y42, y43, y44, y45]; 
a_vector_es_dot = -Learn*(Y')*e; 
Tor 1 
************************************************************************ 
function Tor = 
fcn(vd_dot,vd,v,M1,M2,M3,r,b,Mw,Iw,Lf,L3,theta2,theta3,a_v
123 
ector_es,Kp) 
%#codegen 
c2 = cos(theta2); 
s2 = sin(theta2); 
c3 = cos(theta3); 
s3 = sin(theta3); 
v1d_dot = vd_dot(1); 
v2d_dot = vd_dot(2); 
v3d_dot = vd_dot(3); 
v4d_dot = vd_dot(4); 
v1d = vd(1); 
v2d = vd(2); 
v3d = vd(3); 
v4d = vd(4); 
e = v - vd; 
theta3_dot = v(4); 
theta2_dot = v(3); 
theta1_dot = (r/b)*(v(1) - v(2)); 
y11 = (v1d_dot - v2d_dot)*(r/b)^2; 
y12 = (v1d_dot - v2d_dot)*(r/b)^2 + v3d_dot*(r/b); 
y13 = 0.5*(1 - cos(2*theta3))*(r/b)*((v1d_dot - 
v2d_dot)*(r/b) + v3d_dot) + ... 
 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d - 
v2d)*(r/b) + v3d) + 0.5*(r/b)*sin(2*theta3)*(theta1_dot + 
124 
theta2_dot)*v4d; 
y14 = 0.5*(1 + cos(2*theta3))*(r/b)*((v1d_dot - 
v2d_dot)*(r/b) + v3d_dot) - ... 
 0.5*(r/b)*sin(2*theta3)*theta3_dot*((v1d - 
v2d)*(r/b) + v3d) - 0.5*(r/b)*sin(2*theta3)*(theta1_dot + 
theta2_dot)*v4d; 
y15 = 0; 
y21 = -y11; 
y22 = -y12; 
y23 = -y13; 
y24 = -y14; 
y25 = 0; 
y31 = 0; 
y32 = (v1d_dot - v2d_dot)*(r/b) + v3d_dot; 
y33 = 0.5*(1-cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + 
v3d_dot) + ... 
 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + 
v3d) + (theta1_dot + theta2_dot)*v4d); 
y34 = 0.5*(1+cos(2*theta3))*((r/b)*(v1d_dot - v2d_dot) + 
v3d_dot) - ... 
 0.5*sin(2*theta3)*(theta3_dot*((r/b)*(v1d - v2d) + 
v3d) + (theta1_dot + theta2_dot)*v4d); 
y35 = 0; 
y41 = 0; 
y42 = 0; 
y43 = -0.5*sin(2*theta3)*(theta1_dot + 
theta2_dot)*((r/b)*(v1d - v2d) + v3d); 
y44 = 0.5*sin(2*theta3)*(theta1_dot + 
125 
theta2_dot)*((r/b)*(v1d - v2d) + v3d); 
y45 = v4d_dot; 
Y = {{y11, y12, y13, y14, y15;... 
 y21, y22, y23, y24, y25;... 
 y31, y32, y33, y34, y35;... 
 y41, y42, y43, y44, y45]; 
Z1 = 0.25*(M1 + M2 + M3)*(v1d_dot + v2d_dot)*r^2 + (Mw*r^2 
+ Iw)*v1d_dot ... 
 + ((M2 + M3)*Lf^2 - 
2*M3*Lf*L3*c2*s3)*(r/b)^2*(v1d_dot - v2d_dot) ... 
 - M3*Lf*L3*c2*s3*(r/b)*v3d_dot + 
M3*Lf*L3*(s2*s3*theta2_dot - 
c2*c3*theta3_dot)*(r/b)^2*(v1d - v2d)... 
 +M3*Lf*L3*(r/b)*((s2*s3*theta2_dot - 
c2*c3*theta3_dot)*v3d + theta1_dot*(s2*s3*v3d - 
c2*c3*v4d)); 
Z2 = 0.25*(M1 + M2 + M3)*(v1d_dot + v2d_dot)*r^2 + (Mw*r^2 
+ Iw)*v2d_dot ... 
 - ((M2 + M3)*Lf^2 - 
2*M3*Lf*L3*c2*s3)*(r/b)^2*(v1d_dot - v2d_dot) ... 
 + M3*Lf*L3*c2*s3*(r/b)*v3d_dot - 
M3*Lf*L3*(s2*s3*theta2_dot - 
c2*c3*theta3_dot)*(r/b)^2*(v1d - v2d)... 
 - M3*Lf*L3*(r/b)*((s2*s3*theta2_dot - 
c2*c3*theta3_dot)*v3d + theta1_dot*(s2*s3*v3d - 
c2*c3*v4d)); 
Z3 = -M3*Lf*L3*c2*s3*(r/b)*(v1d_dot - v2d_dot) - 
M3*Lf*L3*s2*s3*theta1_dot*(r/b)*(v1d - v2d) - 
126 
M3*Lf*L3*c2*c3*theta1_dot*v4d; 
%G = {{0; 0; 0; -9.8*M3*L3*sin(theta3)]; 
Z4 = M3*L3^2*v4d_dot* + 
M3*Lf*L3*c2*c3*theta1_dot*(r/b)*(v1d - v2d) + 
M3*Lf*L3*c2*c3*theta1_dot*v3d -9.8*M3*L3*sin(theta3); 
Z = {{Z1; Z2; Z3; Z4]; 
Tor = Y*a_vector_es + Z - Kp*e; 
Động học hệ rô bốt di động – hệ pan-tilt – stereo camera 
function {{Zb_in_c, ImaF3, ImaF,X_0_es,Y_0_es,Z_0_es]= 
fcn(theta1,X,Y,theta2,theta3,muctieu,Lf,focus,px,py,pz,LZ, 
M) 
%#codegen 
c1 = cos(theta1); 
127 
s1 = sin(theta1); 
c12 = cos(theta1 + theta2); 
s12 = sin(theta1 + theta2); 
c3 = cos(theta3); 
s3 = sin(theta3); 
t11 = -s12; 
t12 = -c12*s3; 
t13 = c12*c3; 
t14 = c12*(px*c3 - py*s3) + s12*pz + X + Lf*c1; 
t21 = cos(theta1 + theta2); 
t22 = -sin(theta1 + theta2)*sin(theta3); 
t23 = sin(theta1 + theta2)*cos(theta3); 
t24 = s12*(px*c3-py*s3)- c12*pz + Y + Lf*s1; 
t31 =0; 
t32 = cos(theta3); 
t33 = sin(theta3); 
t34 = px*s3 + py*c3 + LZ; 
 T = {{t11, t12, t13, t14;... 
 t21, t22, t23, t24;... 
 t31, t32, t33, t34;... 
 0, 0, 0, 1]; 
Oc = T\muctieu; 
ImaF = -(focus/Oc(3))*[Oc(1); Oc(2)]; 
128 
ImaF3 = -(focus/Oc(3))*[0.1+Oc(1); Oc(2);Oc(1)-0.1]; 
ul =ImaF3(1); 
vl =ImaF3(2); 
ur =ImaF3(3); 
//------------------------------------// 
B =0.2; 
j11 = (ur-ul)/B; 
j13 = ul*(ul-ur)/(focus*B); 
j14 = ul*vl/focus; 
j15 = -(2*focus^2+ul^2+ul*ur)/(2*focus); 
j16 = vl; 
j22 = j11; 
j23 = vl*(ul-ur)/(focus*B); 
j24 = (focus^2+vl^2)/focus; 
j25 = -vl*(ur+ul)/(2*focus); 
j26 = -(ul+ur)/(2*focus); 
j31 = j11; 
j33 = ur*(ul-ur)/(focus*B); 
j34 = ur*vl/focus; 
j35 = -(2*focus^2+ur^2+ul*ur)/(2*focus); 
j36 = vl; 
Jimg = [j11, 0, j13, j14, j15, j16;... 
 0, j22, j23, j24, j25, j26;... 
 j31, 0, j33, j34, j35, j36]; 
Jhe = [-s12, -c12, 0, 0, 0;... 
 -c12*s3,-s12*s3, 0, 0, 0;... 
 c12*c3, s12*c3, 0, 0, 0;... 
 0 , 0 , 0, 0, -1;... 
 0 , 0 , c3,s3,0;... 
129 
 0 , 0 , c3,s3,0]; 
J = Jimg*Jhe; 
q_dot = [xd; yd; t1d; t2d; t3d]; 
M_dot = J*q_dot; 
B =0.2; 
ul_es = M(1); 
vl_es = M(2); 
ur_es = M(3); 
Zb_in_c = focus*B/(ur_es - ul_es); 
X_C = B*(ur_es + ul_es)/(2*(ur_es - ul_es)); 
Y_C = B*vl_es/(ur_es - ul_es); 
Ball_C_estimate = {{X_C; Y_C; Zb_in_c; 1]; 
Ball_0_estimate = T*Ball_C_estimate ; 
X_0_es = Ball_0_estimate(1); 
Y_0_es = Ball_0_estimate(2); 
Z_0_es = Ball_0_estimate(3);