Mô phỏng động học robot đi bộ hai chân

DƯƠNG VĂN ĐỨC BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - DƯƠNG VĂN ĐỨC MÔ PHỎNG ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN CƠ ĐIỆN TỬ LUẬN VĂN THẠC SĨ KỸ THUẬT CƠ ĐIỆN TỬ 2013B Hà Nội – Năm 2015 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - DƯƠNG VĂN ĐỨC MÔ PHỎNG ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN LUẬN VĂN THẠC SĨ KỸ THUẬT CƠ ĐIỆN TỬ Hà Nội – Năm 2015 BỘ GIÁO DỤC VÀ ĐÀO TẠO TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI - DƯƠNG VĂN ĐỨC MÔ PHỎNG ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN Chuyên ngành: Cơ Điện Tử LUẬN VĂN THẠC SĨ KỸ THUẬT CHUYÊN NGÀNH: CƠ ĐIỆN TỬ NGƯỜI HƯỚNG DẪN KHOA HỌC: PGS TS PHAN BÙI KHÔI Hà Nội – Năm 2015 Mô động học robot hai chân LỜI CAM ĐOAN Tôi xin cam đoan nội dung luận văn thực dƣới hƣớng dẫn thầy giáo: PGS TS Phan Bùi Khơi Để hồn thành luận văn này, tơi có sử dụng tài liệu tham khảo đƣợc ghi mục tài liệu tham khảo, ngồi khơng sử dụng tài liệu không đƣợc ghi Nếu sai phạm, tơi xin chịu hình thức kỷ luật theo quy định Học viên thực Dƣơng Văn Đức Mô động học robot hai chân MỤC LỤC LỜI CAM ĐOAN .3 DANH SÁCH HÌNH ẢNH DANH SÁCH BIỂU BẢNG .6 DANH SÁCH KÍ HIỆU LỜI MỞ ĐẦU .8 CHƢƠNG 1: GIỚI THIỆU ROBOT ĐI BỘ HAI CHÂN 1.1 Tổng quan robot hai chân 1.2 Cấu trúc robot hai chân 11 1.2.1 Cấu trúc chung robot hai chân 11 1.2.2 Lựa chọn mơ hình robot hai chân 12 1.3 Mô tả bƣớc tĩnh robot hai chân 16 CHƢƠNG 2: KHẢO SÁT ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN 18 2.1 Cơ sở lý thuyết khảo sát động học robot hai chân 18 2.1.1 Phƣơng pháp ma trận truyền biến đổi 18 2.1.2 Phƣơng pháp Denavit – Hartenberg 24 2.1.3 Mơ hình khảo sát robot hai chân 27 2.2 Thiết kế quỹ đạo chuyển động robot hai chân .33 2.2.1 Mô tả chuyển động robot 33 2.2.2 Xây dựng quỹ đạo bƣớc tĩnh cho robot 35 2.3 Giải toán động học robot hai chân 48 2.3.1 Giải toán động học thuận robot hai chân 48 2.3.2 Giải toán động học ngƣợc robot hai chân 49 CHƢƠNG 3: MÔ PHỎNG ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN 55 3.1 Tính tốn quy luật chuyển động .55 3.1.1 Tính tốn quy luật chuyển động hông robot 56 3.1.2 Tính tốn quy luật chuyển động cổ chân 57 Mô động học robot hai chân 3.1.3 Tính tốn quy luật chuyển động biến khớp 60 3.2 Lập trình mơ chuyển động robot 67 3.2.1 Cấu trúc chƣơng trình mơ hoạt động robot 67 3.2.2 Lập trình chƣơng trình mơ chuyển động robot .68 3.3 Giao diện chƣơng trình mơ chuyển động robot 78 KẾT LUẬN .80 TÀI LIỆU THAM KHẢO 81 Mô động học robot hai chân DANH SÁCH HÌNH ẢNH Hình 1.1 Các hệ ASIMO Hình 1.2 Các hệ robot HRP: Honda P3 (1997), HRP-1 (1997), HRP-2 (1998), HRP-4 (2010) 10 Hình 1.3 Robot KHR-2 11 Hình 1.4 Robot Johnnie 12 Hình 1.5 Mơ hình cấu trúc robot với 13 bậc tự 13 Hình 1.6 Mơ hình cấu trúc robot hai chân 14 Hình 1.7 Sơ đồ động học robot hai chân .15 Hình 1.8 Chu kì bƣớc robot hai chân 17 Hình 2.1 Vị trí vật rắn không gian 18 Hình 2.2 Hệ quy chiếu mơ tả ma trận cosin hƣớng 19 Hình 2.3 Biểu diễn phép quay hệ quy chiếu quanh trục z 20 Hình 2.4 Biểu diễn góc Euler 21 Hình 2.5 Biểu diễn góc Cardan 22 Hình 2.6 Biểu diễn vector 22 Hình 2.7 Quy ƣớc hệ tọa độ theo Denavit – Hartenberg 25 Hình 2.8 Mơ hình robot nối tiếp n khâu 26 Hình 2.9 Hệ trục tọa độ 28 Hình 2.10 Mơ tả chuyển động robot 33 Hình 2.11 Trạng thái robot thời điểm chu kì 34 Hình 2.12 Quỹ đạo bƣớc robot .35 Hình 2.13 Thời điểm bàn chân bắt đầu chạm mặt đất bắt đầu rời mặt đất 36 Hình 2.14 Mơ tả quỹ đạo chuyển động hông cổ chân robot 38 Hình 2.15 Xác định góc khớp 40 Hình 2.16 Xây dựng quỹ đạo chuyển động hông robot 43 Hình 2.17 Xây dựng quỹ đạo chuyển động cổ chân phải 45 Hình 2.18 Xây dựng quỹ đạo chuyển động cổ chân trái 47 Hình 3.1 Đồ thị quỹ đạo chuyển động hông robot 57 Hình 3.2 Đồ thị quỹ đạo chuyển động cổ chân phải 58 Hình 3.3 Đồ thị quỹ đạo chuyển động cổ chân trái 59 Mô động học robot hai chân Hình 3.4 Đồ thị biến khớp chân trái 61 Hình 3.5 Đồ thị vận tốc biến khớp chân trái 62 Hình 3.6 Đồ thị gia tốc biến khớp chân trái .63 Hình 3.7 Đồ thị biến khớp chân phải 64 Hình 3.8 Đồ thị vận tốc biến khớp chân phải 65 Hình 3.9 Đồ thị gia tốc biến khớp chân trái .66 Hình 3.10 Sơ đồ tƣơng tác chƣơng trình mơ 67 Hình 3.11 Sơ đồ trình lập trình chƣơng trình mơ 68 Hình 3.13 Biểu diễn mơ hình robot chƣơng trình mơ 72 Hình 3.14 Sơ đồ giải thuật lập trình chuyển động robot 73 Hình 3.15 Giao diện chƣơng trình mơ chuyển động robot .78 Hình 3.16 Giao diện điều khiển chƣơng trình mơ 79 Mô động học robot hai chân DANH SÁCH BIỂU BẢNG Bảng Kích thƣớc khâu chân robot 15 Bảng Tham số DH chân trái robot hai chân 29 Bảng Tham số DH chân phải robot hai chân 30 Bảng Tham số cấu trúc động học robot hai chân 39 Bảng Vị trí hơng cổ chân robot 39 Bảng Tham số động học robot hai chân .56 Bảng Đặc tính điều khiển 70 Mô động học robot hai chân DANH SÁCH KÍ HIỆU STT Kí hiệu d a2 a3 a4 a5 l1 l2 s T 10 j-1 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 xT yT zT xR yR zR xL yL zL q1 q2 α XT XR XL Xh Xar Xal v a ω γ hT hT max Đơn vị cm cm cm cm cm cm cm cm s Aij cm cm cm cm cm cm cm cm cm 0 0 /s /s2 cm cm Chú giải Chiều dài khâu hông (1-1) khâu hông (2-1) Chiều dài khâu hông (1-2) khâu hông (2-2) Chiều dài khâu chân (1-3) khâu hông (2-3) Chiều dài khâu cẳng chân (1-4) khâu hông (2-4) Chiều dài khâu bàn chân (1-5) khâu hông (2-5) Khoảng cách từ tâm bàn chân C đến gót chân Khoảng cách từ tâm bàn chân C đến mũi chân Khoảng cách hai tâm C bàn chân Chu kì bƣớc di chuyển Ma trận chuyển từ hệ tọa độ gắn với khâu j-1 sang hệ tọa độ gắn với khâu j (j=1,2, 6; i=1,2) Tọa độ thân mơ hình robot theo trục x Tọa độ thân mơ hình robot theo trục y Tọa độ thân mơ hình robot theo trục z Tọa độ bàn chân phải mơ hình robot theo trục x Tọa độ bàn chân phải mơ hình robot theo trục y Tọa độ bàn chân phải mơ hình robot theo trục z Tọa độ bàn chân trái mơ hình robot theo trục x Tọa độ bàn chân trái mơ hình robot theo trục y Tọa độ bàn chân trái mơ hình robot theo trục z Biến khớp chân trái Biến khớp chân trái Góc bàn chân Vector quỹ đạo thân robot Vector quỹ đạo bàn chân phải Vector quỹ đạo bàn chân trái Vector quỹ đạo hông robot Vector quỹ đạo cổ chân phải Vector quỹ đạo cổ chân trái Vận tốc hông robot Gia tốc hơng robot Vận tốc góc Gia tốc góc Độ cao nhỏ đế thân Độ cao lớn đế thân Mô động học robot hai chân OpenGL đƣợc sử dụng nhƣ công cụ để thể lại mơ hình 3D, chƣơng trình dùng kết tính tốn VC++ tác động vào mơ hình 3D thể chuyển động robot hiệu dứng kèm 3.2.2 Lập trình chƣơng trình mơ chuyển động robot Trong phần này, chƣơng trình mơ chuyển động robot đƣợc lập trình mơi trƣờng Visul C++ Dƣới sơ đồ khối mơ tả q trình xây dựng chƣơng trình mơ chuyển động cho robot hai chân (Hình 3.11) Khởi tạo mơi trƣờng OpenGL Thiết kế mơ hình 3D Thiết kế giao diện robot chƣơng trình Xuất file STL Vẽ mơ hình robot Lập trình chuyển động cho robot Hình 3.11 Sơ đồ trình lập trình chương trình mơ Trƣớc hết, visual C++, ta tạo dự án với kiểu dự án MFC Application thêm thƣ viện OpenGL cho ứng dụng Các thƣ viện OpenGL cho phép vẽ lại mô hình 3D thể chuyển động robot Các thƣ viện OpenGL đƣợc sử dụng gồm có: glu.h ST_SplitterWnd.h ST_SplitterWnd.cpp 68 Mô động học robot hai chân ObjectsOpenGL.h InitOpenGL.h InitOpenGL.cpp Thiết lập môi trƣờng đồ họa OpenGL m_pInitGL = new CInitOpenGL(this); //Tạo trỏ động khởi tạo đồ họa m_pInitGL->SetScale(0.18); // Đặt tỉ lệ nhìn 0.18 m_pInitGL->m_TranslateX = -500; // Dịch chuyển hệ trục tọa độ m_pInitGL->m_TranslateY = -100; m_pInitGL->m_TranslateZ = -120; m_pInitGL->m_RotateZ = -20; //Quay hệ trục tọa độ Thêm thuộc tính hàm tạo OnCreate (), hàm hủy OnDestroy() hàm lấy kích thƣớc khung nhìn vào ứng dụng OnSize () Hàm OnCreate () chứa đoạn chƣơng trình khởi tạo mơi trƣờng đồ họa OpenGL m_pInitGL->OnCreateGL(); // Tạo GL m_pInitGL->BeginGLCommand(); // Bắt đầu GL m_Enviroment.EnableLighting(); // Cho phép ánh sáng m_Enviroment.TurnOnLight(); // Bật ánh sáng m_pInitGL->EndGLCommand(); // Kết thúc sử dụng lệnh GL Sau khởi tạo mơi trƣờng OpenGL, giao diện chƣơng trình đƣợc thiết kế Giao diện chƣơng trình đƣợc chia làm hai phần gồm phần hiển thị phần điều khiển (Hình 3.12) Phần hiển thị Phần điều khiển Hình 3.12 Thiết kế giao diện chương trình mơ chuyển động Phần hiển thị hiển thị cấu robot nhƣ chuyển động robot Phần điều khiển gồm có nút ấn cho phép điều khiển chuyển động robot 69 Mô động học robot hai chân Ngoài ra, phần điều khiển, giá trị vị trí thân bàn chân robot nhƣ góc khớp đƣợc hiển thị Edit box Bảng Đặc tính điều khiển Điều khiển Tên kiểu điều khiển Tên thuộc tính Button Run Linear Button Run Spline Button Stop Edit Box xT Edit Box yT Edit Box zT Edit Box xL Edit Box yL Edit Box zL 10 Edit Box xR 11 Edit Box yR 12 Edit Box zR 13 Edit Box Theta11 14 Edit Box Theta12 15 Edit Box Theta13 16 Edit Box Theta14 17 Edit Box Theta15 18 Edit Box Theta21 19 Edit Box Theta22 20 Edit Box Theta23 21 Edit Box Theta24 22 Edit Box Theta25 Ý nghĩa Chạy chƣơng trình với quỹ đạo tuyến tính Chạy chƣơng trình với quỹ đạo Spline bậc Dừng chƣơng trình Hiển thị giá trị tọa độ đế thân robot (xT, yT, zT) Hiển thị giá trị tọa độ đế bàn chân trái (xL, yL, zL) Hiển thị giá trị tọa độ đế bàn chân phải (xR, yR, zR) Hiển thị giá trị biến khớp chân trái (θ11,θ12,θ13, θ14,θ15) Hiển thị giá trị biến khớp chân phải (θ21,θ22,θ23, θ24,θ25) 70 Mô động học robot hai chân Tiếp theo, ta xây dựng hàm vẽ cấu mơ hình robot đƣợc thiết kế Solidworks lƣu dƣới dạng file *.STL Thuộc tính OnInitialUpdate() cho phép cập nhật khâu, khớp nhƣ thay đổi mô hình robot Các file cấu robot nhƣ quỹ đạo chuyển động robot đƣợc cập nhật thuộc tính OnInitialUpdate() m_body.setModel("Body.STL"); // Cập m_link11.setModel("link11.stl"); // m_link12.setModel("link12.stl"); // m_link13.setModel("link13.stl"); // m_link14.setModel("link14.stl"); // m_link15.setModel("link15.stl"); // m_link21.setModel("link21.stl"); // m_link22.setModel("link22.stl"); // m_link23.setModel("link23.stl"); // m_link24.setModel("link24.stl"); // m_link25.setModel("link25.stl"); // nhật Thân robot Cập nhật khâu hông chân Cập nhật khâu hông chân Cập nhật khâu chân trái Cập nhật khâu chân trái Cập nhât bàn chân trái Cập nhật khâu hông chân Cập nhật khâu hông chân Cập nhật khâu chân phải Cập nhật khâu chân phải Cập nhât bàn chân phải trái trái phía phía phải phải phía phía Hàm OnPaint() cho phép vẽ cấu với hàm drawObject() kết nối cấu hàm dịch chuyển hệ tọa độ glTranslated(x,y,z), glRotated(θ,x,y,z) OnPaint() { // Chọn màu vật liệu cho cấu robot m_body.setMaterial(storageMaterial[10]); m_link11.setMaterial(storageMaterial[20]); m_link12.setMaterial(storageMaterial[1]); m_link13.setMaterial(storageMaterial[3]); m_link14.setMaterial(storageMaterial[4]); m_link15.setMaterial(storageMaterial[5]); m_link21.setMaterial(storageMaterial[20]); m_link22.setMaterial(storageMaterial[1]); m_link23.setMaterial(storageMaterial[3]); m_link24.setMaterial(storageMaterial[4]); m_link25.setMaterial(storageMaterial[5]); // Vẽ cấu robot // Vẽ thân robot m_body.drawObject(); // Vẽ chân trái robot //link11 glTranslated(-80,0,0); glRotated(theta11,0,0,1); m_link11.drawObject(); //link12 glTranslated(0,0,-50); glRotated(theta12,0,1,0); m_link12.drawObject(); //link13 glTranslated(0,0,-40); glRotated(theta13,1,0,0); 71 Mô động học robot hai chân m_link13.drawObject(); //link14 glTranslated(0,0,-320); glRotated(theta14,1,0,0); m_link14.drawObject(); //link15 glTranslated(0,0,-350); glRotated(theta15,1,0,0); m_link15.drawObject(); // vẽ chân phải robot //link21 glTranslated(80,0,0); glRotated(theta21,0,0,1); m_link21.drawObject(); //link22 glTranslated(0,0,-50); glRotated(theta22,0,1,0); m_link22.drawObject(); //link23 glTranslated(0,0,-40); glRotated(theta23,1,0,0); m_link23.drawObject(); //link24 glTranslated(0,0,-320); glRotated(theta24,1,0,0); m_link24.drawObject(); //link25 glTranslated(0,0,-350); glRotated(theta25,1,0,0); m_link25.drawObject(); } Nhƣ cấu robot đƣợc liên kết với thành mô hình hồn chình thể chƣơng trình mơ nhƣ Hình 3.13 Hình 3.13 Biểu diễn mơ hình robot chương trình mơ Q trình mơ chuyển động robot q trình vẽ xóa liên tục kết cấu robot Trƣớc tiên, robot đƣợc vẽ trạng thái 1, sau đƣợc xóa đƣợc 72 Mô động học robot hai chân vẽ lại trạng thái lại đƣợc xóa vẽ lại trạng thái Nếu q trình vẽ xóa diễn khoảng 24 lần/giây cho ta trình chuyển động robot Trong chƣơng trình mơ phỏng, để vẽ đƣợc robot vị trí phải có giá trị tọa độ thân robot (xT, yT, zT) góc khớp theta11, theta12, theta13, theta14, theta15, theta21, theta22, theta23, theta24, theta25 Các giá trị tọa độ thân robot biến khớp đƣợc tính tốn chƣơng trình, đồng thời đƣợc cập nhật liên tục để chƣơng trình thực lệnh vẽ robot Giải thuật lập trình chuyển động robot đƣợc mơ tả sơ đồ Hình 3.14 Khởi tạo tham số động học robot thời điểm t = Vẽ robot thời điểm đầu Nút Run Linear/ Run Spline đƣợc ấn Sai Đúng Bật Timer Tăng t+=0.001 Tính tọa độ thân biến khớp Vẽ robot thời điểm Sai t = T (s) Đúng Dừng Hình 3.14 Sơ đồ giải thuật lập trình chuyển động robot 73 Mô động học robot hai chân Chuyển động robot đƣợc lập trình theo hai quy luật chuyển động Một robot chuyển động theo đoạn quỹ đạo thẳng nối liền hai điểm Hai robot bƣớc theo đƣờng cong spline bậc qua điểm đƣợc xác định Trƣớc hết, tham số động học robot đƣợc khởi tạo thời điểm đầu (t = 0) trạng thái robot thời điểm đầu đƣợc vẽ // Tham số số cấu trúc robot d = 50; a2 = 40; a3 = 320; a4 = 350; a5 = 80; l1 = 55; l2 = 195; Pi=3.145926535897932384626433832795; // Thời gian thời điểm t = 0; Tc = 0.6; T1 = 0.05; Tm = 0.15; T2 = Tm-T1; // Khởi tạo giá trị tham số robot hai chân s = 400; alpha = 15*Pi/180; // alpha = 15 độ // Khởi tạo m_theta11 = m_theta12 = m_theta13 = m_theta14 = m_theta15 = m_theta21 = m_theta22 = m_theta23 = m_theta24 = m_theta25 = giá trị biến khớp thời điểm đầu 0.0; 0.0; 0.0; 0.0; 90; 0.0; 0.0; 0.0; 0.0; 90; // Vị yT0 = yT1 = yT2 = yT3 = yT4 = yT5 = yT6 = trí thân s/2; yT0+50; yT0+200; 3*s/2; 3*s/2+50; 3*s/2+200; 5*s/2; zT0 zT1 zT2 zT3 zT4 zT5 zT6 655+a2+d; 660+a2+d; 700+a2+d; zT0; zT1; zT2; zT0; = = = = = = = // vị yR0 = yR1 = yR2 = yR3 = yR4 = yR5 = yR6 = trí cổ chân phải 0; l2*(1-cos(alpha2))+a5*sin(alpha2); 200; 2*s-l1*(1-cos(alpha1))-a5*sin(alpha1); 2*s; 2*s; 2*s; 74 Mô động học robot hai chân zR0 = zR1 = zR2 = zR3 = zR4 = zR5 = zR6 = // vị yL0 = yL1 = yL2 = yL3 = yL4 = yL5 = yL6 = a5; l2*sin(alpha2)+a5*cos(alpha2); 200; l1*sin(alpha1)+a5*cos(alpha1); a5; a5; a5; trí cổ chân trái s-l1*(1-cos(alpha1))-a5*sin(alpha1); s; s; s; s+l2*(1-cos(alpha2))+a5*sin(alpha2); 600; 3*s-l1*(1-cos(alpha1))-a5*sin(alpha1); zL0 zL1 zL2 zL3 zL4 zL5 zL6 l1*sin(alpha1)+a5*cos(alpha1); a5; a5; a5; l2*sin(alpha2)+a5*cos(alpha2); 200; l1*sin(alpha1)+a5*cos(alpha1); = = = = = = = Tọa độ thân góc khớp đƣợc tính tốn hàm CalculateLinear() hàm CalculateSpline() Hàm CalculateLinear() cho phép tính tọa độ thân giá trị góc khớp với quy luật chuyển động theo đoạn thẳng nối hai điểm xác định Hàm CalulateSpline() cho phép tính tọa độ thân giá trị góc khớp với quy luật chuyển động the đƣờng cong Spline bậc void CCTRPANEL::CalculateLinear(void) { // Đoạn quỹ đạo (0 ≤ t = 0) && (t< T1)) { m_yThan = yT0+(yT1-yT0)*t/T1; m_zThan = zT0+(zT1-zT0)*t/T1; m_theta13 = p30+(p31-p30)*t/T1; m_theta14 = p40+(p41-p40)*t/T1; m_theta15 = p50+(p51-p50)*t/T1; m_theta23 = q30+(q31-q30)*t/T1; m_theta24 = q40+(q41-q40)*t/T1; m_theta25 = q50+(q51-q50)*t/T1; } // Đoạn quỹ đạo (T1 ≤ t = T1) && (t< Tm)) { m_yThan = yT1+(yT2-yT1)*(t-T1)/(Tm-T1); m_zThan = zT1+(zT2-zT1)*(t-T1)/(Tm-T1); m_theta13 = p31+(p32-p31)*(t-T1)/(Tm-T1); m_theta14 = p41+(p42-p41)*(t-T1)/(Tm-T1); m_theta15 = p51+(p52-p51)*(t-T1)/(Tm-T1); m_theta23 = q31+(q32-q31)*(t-T1)/(Tm-T1); m_theta24 = q41+(q42-q41)*(t-T1)/(Tm-T1); m_theta25 = q51+(q52-q51)*(t-T1)/(Tm-T1); } // Đoạn quỹ đạo (Tm ≤ t = Tm) && (t< Tc/2)) { m_yThan = yT2+(yT3-yT2)*(t-Tm)/(Tc/2-Tm); 75 Mô động học robot hai chân m_zThan = m_theta13 m_theta14 m_theta15 m_theta23 m_theta24 m_theta25 zT2+(zT3-zT2)*(t-Tm)/(Tc/2-Tm); = p32+(p33-p32)*(t-Tm)/(Tc/2-Tm); = p42+(p43-p42)*(t-Tm)/(Tc/2-Tm); = p52+(p53-p52)*(t-Tm)/(Tc/2-Tm); = q32+(q33-q32)*(t-Tm)/(Tc/2-Tm); = q42+(q43-q42)*(t-Tm)/(Tc/2-Tm); = q52+(q53-q52)*(t-Tm)/(Tc/2-Tm); } // Đoạn quỹ đạo (Tc/2 ≤ t = Tc/2) && (t< Tc/2+T1)) { m_yThan = yT3+(yT4-yT3)*(t-Tc/2)/T1; m_zThan = zT3+(zT4-zT3)*(t-Tc/2)/T1; m_theta13 = p33+(p34-p33)*(t-Tc/2)/T1; m_theta14 = p43+(p44-p43)*(t-Tc/2)/T1; m_theta15 = p53+(p54-p53)*(t-Tc/2)/T1; m_theta23 = q33+(q34-q33)*(t-Tc/2)/T1; m_theta24 = q43+(q44-q43)*(t-Tc/2)/T1; m_theta25 = q53+(q54-q53)*(t-Tc/2)/T1; } // Đoạn quỹ đạo (Tc/2 +T1 ≤ t = Tc/2+T1) && (t< Tc/2+Tm)) { m_yThan = yT4+(yT5-yT4)*(t-Tc/2-T1)/(Tm-T1); m_zThan = zT4+(zT5-zT4)*(t-Tc/2-T1)/(Tm-T1); m_theta13 = p34+(p35-p34)*(t-Tc/2-T1)/(Tm-T1); m_theta14 = p44+(p45-p44)*(t-Tc/2-T1)/(Tm-T1); m_theta15 = p54+(p55-p54)*(t-Tc/2-T1)/(Tm-T1); m_theta23 = q34+(q35-q34)*(t-Tc/2-T1)/(Tm-T1); m_theta24 = q44+(q45-q44)*(t-Tc/2-T1)/(Tm-T1); m_theta25 = q54+(q55-q54)*(t-Tc/2-T1)/(Tm-T1); } // Đoạn quỹ đạo (Tc/2 +Tm ≤ t = Tc/2+Tm) && (tRun(m_xThan, m_yThan, m_zThan, m_theta11, m_theta12, m_theta13, m_theta14, m_theta15,m_theta21, m_theta22, m_theta23, m_theta24, m_theta25); UpdateData(FALSE); InvalidateRect(NULL,FALSE); break; } CFormView::OnTimer(nIDEvent); } Nhƣ vậy, chƣơng trình mơ chuyển động robot đƣợc hoàn thiện 3.3 Giao diện chƣơng trình mơ chuyển động robot Trong phần này, giao diện chƣơng trình mơ chuyển động robot kết mô đƣợc miêu tả trình bày Hình 3.15 mơ tả giao diện hồn chỉnh chƣơng trình mơ chuyển động robot Hình 3.15 Giao diện chương trình mơ chuyển động robot 78 Mô động học robot hai chân Giao diện chƣơng trình mơ gồm có phần: phần điều khiển phần hiển thị Phần điều khiển gồm nút ấn RunLinear cho phép mô hình robot chuyển động theo đoạn quỹ đạo thẳng, nút ấn Run Spine cho phép mơ hình robot chuyển động theo đƣờng cong spline bậc 3, nút ấn Stop có tác dụng dừng chuyển động mơ hình robot (Hình 3.16) Nếu nút Run Linear đƣợc ấn, mơ hình robot chuyển động theo đoạn quỹ đạo thẳng Mặt khác, nút Run Spline đƣợc ấn, mơ hình robot chuyển động theo quỹ đạo cong Spline bậc Phần hiển thị cho phép hiển thị mơ hình quỹ đạo chu kì bƣớc hai chân Hình 3.16 Giao diện điều khiển chương trình mơ Kết mơ chuyển động robot đƣợc thể Hình 3.17 t=0s t = 0.15 s t = 0.05s t = 0.35 s t = 0.45 s t = 0.3 s t = 0.6 s Hình 3.17 Kết mơ chuyển động robot 79 Mô động học robot hai chân KẾT LUẬN Qua trình thực luận văn dƣới bảo tận tình thầy giáo, với nỗ lực tìm tịi, trau dồi kiến thức thân, em hoàn thiện luận văn: Mô động học robot hai chân Với kết đạt đƣợc : - Xây dựng mô hình robot hai chân - Khảo sát động học robot hai chân - Giải tốn xây dựng quỹ đạo chuyển động cho mơ hình robot đƣa - Tính tốn, lập trình chƣơng trình mơ chuyển động cho mơ hình robot Phƣơng pháp khảo sát động học robot đƣợc trình bày phù hợp đặc thù cấu trúc động học thuận lợi cho việc nhận đƣợc hệ phƣơng trình động học robot Áp dụng phƣơng pháp nội suy Spline bậc để thiết lập quỹ đạo trơn cho chuyển động mơ hình robot Mô chuyển động robot từ kết tính tốn quỹ đạo chuyển động mơ hình robot Vì giới hạn luận văn thạc sỹ kỹ thuật nên trình bày tốn mơ động học cho mơ hình robot đơn giản bƣớc tĩnh Từ kết đạt đƣợc mở rộng hƣớng nghiên cứu vào khảo sát động học robot bƣớc động khảo sát động lực học cho robot hai chân Trong q trình hồn thiện luận văn cịn nhiều thiếu sót, em mong nhận đƣợc góp ý q thầy quan tâm tới vấn đề luận văn đề cập tới Em xin chân thành cảm ơn! Học viên Dƣơng Văn Đức 80 Mô động học robot hai chân TÀI LIỆU THAM KHẢO [1] Aye Aye Thant, and Khaing Khaing Aye (2009), “Application of Cubic Spline Interpolation to Walking Patterns of Biped robot”, World Academy of Science, Engineering and Technology, pp 1-5 [2] C Hern ndez-Santos, E Rodriguez-Leal, R Soto and J.L.Gordillo (2012), “Kinematics and Dynamics of a new 16 DOF Humanoid Biped robot with active Toe Joint”, International Journal of Advanced Robotic System [3] D.Ito, T Murakami and K Ohnishi (2002), “An approach to generation of smooth walking pattern for biped robot”, in Proc Of the 7th Workshop on Advanced Motion Control, Maribor, Slovenia, pp 98-103 [4] Eddie Choong, Chee-Meng Chew, Aun-Neow Poo, Geok-Soon Hong (2003), “Mechanical design of an Anthropomorphic bipedal robot”, First Humanoid, Nanotechnology, Information Technology, Communication and control Environment and Management International Conference [5] Erik V Cuevas, Daniel Zaldívar, Rẳl Rojas (2005), “Bipedal robot discription”, Freie Universität Berlin, Institut für Informatik, pp 2-3 [6] GS TSKH Nguyễn Thiện Phúc (2006), Robot công nghiệp, Nhà xuất khoa học kĩ thuật, Hà Nội [7] Jung-Yup Kim, Ill-Woo Park and Jun-Ho Oh, “ Walking control Algorithm of biped humanoid Robot on uneven and inclined floor”, Korea Advanced Institute of Science and Technology [8] K Hirai (1997), “The Honda humanoid robot”, in Proc 1997 IEEE-RSJ Int Conf Inteligent Robots Systems, Grenoble, France, pp 499-508 [9] K Hirai, M Hirose, Y Haikawa, and T Takenaka(1998), “The development of honda humanoid robot”, in Pro IEEE Int Conf Robotics and Automation, pp 1321-1326 [10] K.Kaneko, F.Kanehiro, S.Kajita, H.Hirukawa, T.Kawasaki, M.Hirata, K.Akachi, T.Isozumi (2004), “Humanoid Robot HRP-2”, Proc IEEE, pp 1-3 [11] K.Löffler, M Gienger, F Pfeiffer (2003), “Sensors and Control Concept of Walking Johnnie”, The International Journal of Robotics Research, pp 1-2 81 Mô động học robot hai chân [12] Mattias Wahde and Jimmy Petterson, “A brief review of bipedal robotics research”, Dept of Mechine and Vehicle Systems, Sweden [13] PGS.TS Phan Bui Khoi (2009), Modelling of Robot and Mechatronics System, Ha Noi University of Science and Technology, Hà Nội [14] Q Huang, K Yokoi, S Kajita, K Kaneko, H Arai, N.Kayachi and K.Tanic (2001), “ Planning walking patterns for a biped robot”, IEEE, pp 1-3 [15] R Zhang, P Vadakkepat and Chee-Meng Chew, “An Evolutionary algorithm for trajectory based gait generation of biped robot”, National University of Singapore, pp 1-4 [16] Zhe Tang, Changjiu Zhou, Zenqi Sun (2003), “Trajection Planning for Smooth Transition of a Biped Robot”, Pro of IEEE, pp 1-3 82 ... động học robot hai chân 48 2.3.1 Giải toán động học thuận robot hai chân 48 2.3.2 Giải toán động học ngƣợc robot hai chân 49 CHƢƠNG 3: MÔ PHỎNG ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN 55... bước robot hai chân Các pha chu kì bƣớc robot hai chân đƣợc mơ tả Hình 1.8 17 Mơ động học robot hai chân CHƢƠNG 2: KHẢO SÁT ĐỘNG HỌC ROBOT ĐI BỘ HAI CHÂN 2.1 Cơ sở lý thuyết khảo sát động học robot. .. triển, robot hai chân dần đƣợc hoàn thiện chế di chuyển cấu trúc robot 10 Mô động học robot hai chân 1.2 Cấu trúc robot hai chân 1.2.1 Cấu trúc chung robot hai chân Việc xây dựng cấu trúc robot hai