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

VIỆN HÀN LÂM KHOA HỌC VÀ CÔNG NGHỆ VIỆT NAM BỘ GIÁO DỤC VÀ ĐÀO TẠO 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 Chun ngành: Kỹ thuật điều khiển tự động hóa Mã số: LUẬN ÁN TIẾN SĨ KỸ THUẬT NGƯỜI HƯỚNG DẪN KHOA HỌC: PGS.TSKH Phạm Thượng Cát TS Phạm Minh Tuấn Hà Nội – 2019 LỜI CAM ĐOAN Tôi xin cam đoan cơng trình nghiên cứu riêng tơi Các kết viết chung với tác giả khác đồng ý đồng tác giả trước đưa vào luận án Các kết luận án trung thực chưa công bố cơng trình khác Tác giả luận án Lê Văn Chung i 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 Công nghệ, Viện Công nghệ thông tin - Viện Hàn lâm Khoa học 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 trình học tập, nghiên cứu Tơi xin bày tỏ lòng kính trọng biết ơn sâu sắc tới PGS.TSKH Phạm Thượng Cát TS Phạm Minh Tuấn, hai thầy định hướng tận tình hướng dẫn để tơi hồ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 Truyền thông - Đại học Thái Ngun, Khoa Cơng nghệ tự động hóa đơn vị Nhà trường quan tâm giúp đỡ, tạo điều kiện để tơi thực nghiên cứu Tôi xin cảm ơn cán Phòng Cơng nghệ tự động hóa – Viện Cơng nghệ thông tin, đồng nghiệp thuộc Khoa Công nghệ Tự động hóa - Trường Đại học Cơng nghệ thơng tin truyền thông - Đại học Thái Nguyên động viên trao đổi kinh nghiệm q trình hồn thành luận án Cuối cùng, xin chân thành cảm ơn đến gia đình, người thân, bạn đồng nghiệp - người ln dành cho tơi tình cảm nồng ấm, động viên sẻ chia lúc khó khăn sống tạo điều kiện tốt để tơi hồn thành q trình nghiên cứu Hà Nội, ngày 28 tháng năm 2019 Tác giả luận án Lê Văn Chung ii 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 CHƢƠNG TỔNG QUAN 1.1 Đặt vấn đề 1.2 Tổng quan điều khiển rô bốt sử dụng thơng tin hình ảnh 1.2.1 Tình hình nghiên cứu ngồi nước 11 1.2.2 Tình hình nghiên cứu nước 1.3 Các vấn đề nghiên cứu 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 đ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 tố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 22 CHƢƠNG 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 iii 2.1.1 Xác định ma trận Jacobi ảnh qua tọa độ điểm ảnh thu từ 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 toán điều khiển động học (kinematic control) hệ rô bốtstereo 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 đ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 mô kiểm chứng 35 2.4 Kết luận chƣơng 43 CHƢƠNG 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ệ camera 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 suy biến 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 tham số bất định 53 3.3 dựng đ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 điều khiển 55 3.3.2 Xây dựng cấu trúc lớp đ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ô hệ thống 61 iv 3.5 Kết luận 72 CHƢƠNG 74 PHÁT TRIỂN THUẬT TỐ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 tố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 hệ tốc độ bám mục tiêu cho rô bốt di động 77 4.1.3 Xác định tố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ế điều khiển tối ưu 83 4.3 Mô hệ thống 89 4.3.1 Mô hệ thống với điều khiển tối ưu 89 4.3.2 Mô hệ thống với điều khiển trượt CTC 94 4.4 Kết luận chƣơng 96 KẾT LUẬN 97 5.1 Những nội dung nghiên cứu luận án 97 5.2.Những đóng góp khoa học 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 Mơ hình mơ hệ pan-tilt stereo camera bám mục tiêu di động có sử dụng mơ hình camera 3D ảo .111 v Mơ hình mơ hệ pan-tilt stereo camera bám mục tiêu di động có sử dụng mơ hình camera 3D ảo .118 vi DANH MỤC CÁC KÝ HIỆU VÀ CHỮ VIẾT TẮT Ký hiệu CTC d d(t) f fv f fu H Jimag Jrobot J ˆ J ˆ (m), J robot imag ΔJ imag (m), ΔJ robot (q) J (JJ) ˆ ˆTˆ K k k1 l1 l2 LQR vii J ˆ T LQG m md Q q qr RBF s Tx Ty Tz xyz U, V u1 , u* v C C v Ω vs vd x xs W ε θ1 θ2 m θ1d θ2d ********************************************************** ********** 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 115 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, , 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]; 116 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]; 117 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; Mơ hình mơ hệ phức hợp rô bốt di động mang pan-tilt stereo camera bám mục tiêu di động 118 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; 119 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); 120 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; 121 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 ************************************************************************ function Tor = fcn(vd_dot,vd,v,M1,M2,M3,r,b,Mw,Iw,Lf,L3,theta2,theta3,a_v 122 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 + 123 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 + 124 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) 125 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); 126 s1 = sin(theta1); c12 = cos(theta1 + s12 = sin(theta1 + c3 = cos(theta3); s3 = sin(theta3); t11 = -s12; t12 = -c12*s3; t13 = c12*c3; t14 = c12*(px*c3 - t21 = cos(theta1 + t22 = -sin(theta1 + theta2)*sin(theta3 t23 = sin(theta1 + t24 = s12*(px*c3-py*s3)- c12*pz + Y + t31 =0; t32 = cos(theta3); t33 = sin(theta3); t34 = px*s3 + py*c3 + LZ; T = {{t11, t12, t13, t14; t21, t22, t23, t24; t31, 0, t32, t33, t34; 0, 0, 1]; Oc = T\muctieu; ImaF = -(focus/Oc(3))*[Oc(1); Oc(2)]; 127 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 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 j36 = vl; Jimg = [j11, 0, j13, j14, j15, j16; 0, j31, Jhe = [-s12, -c12*s3,-s12*s3, c12*c3, s12*c3, 0 , , c3,s3,0]; J = Jimg*Jhe; q_dot = [xd; yd; t1d; t2d; t3d]; M_dot = B =0.2; ul_es = vl_es = ur_es = Zb_in_c X_C = B*(ur_es + ul Y_C = B*vl_es/(ur_e 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); 129 ... lý trên, tác giả lựa chọn đề tài: Nghiên cứu điều khiển rô bốt tay máy di động bám mục tiêu sở sử dụng thông tin hình ảnh để phát triển số thuật tốn điều khiển rô bốt theo dõi mục tiêu di độ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 Chun ngành: Kỹ thuật điều khiển tự động hóa Mã số: LUẬN ÁN TIẾN SĨ... hành khác rô bốt thực theo yêu cầu Mục tiêu nghiên cứu Mục tiêu đề tài nghiên cứu phát triển số thuật toán điều khiển tay máy rô bốt bám mục tiêu di động dựa s thơng tin hình ảnh s dụng hai camera