con lắc ngược 2d phương trình com có chiều cao không đổi con lắc ngược tuyến tính phương trình nghiệm

Con lắc ngược tuyến tínhCon lắc ngược 2D: phương trình, CoM có chiều cao không đổi => con lắc ngượctuyến tính phương trình, nghiệm2.. Tư thế ban đầu gần với tư thế của bài động học thuận

TRƯỜNG CƠ KHÍ

NCM CƠ ĐIỆN TỬ THÔNG MINH

BÀI THI CUỐI KỲ

Sinh viên 5: Phan Xuân Chiến Mã SV: 20194924

Nhóm: 02

ĐỀ BÀI

I Tóm tắt bài giữa kỳII Mô phỏng ZMP

1 Con lắc ngược tuyến tính

Con lắc ngược 2D: phương trình, CoM có chiều cao không đổi => con lắc ngượctuyến tính (phương trình, nghiệm)

2 Mô phỏng chuyển động (mô hình đầy đủ, góc quan sát phù hợp):a Mô tả tóm tắt vai trò của các đoạn code chính

b Tư thế ban đầu gần với tư thế của bài động học thuận (bớt DOF) Trình bàyphần tính toán với nhóm đặc thù Nhóm 1-6: chân trái tiếp xúc với sàn; nhóm 7-12: chân phải tiếp xúc với sàn

c Xác định được EndTime max (≥ 0.4s)

d Tìm các tham số (q) của chân chạm đất

3 Áp dụng bỏ DOF theo nhóm

HÀ NỘI, 8/2023

MỤC LỤC

CHƯƠNG 1: TÓM TẮT GIỮA KÌ 4

I TÌM HIỂU VỀ ROBOT ATLAS 4

1 Giới thiệu về Robot Atlas 4

2 Các thông số của robot 4

1 Con lắc ngược tuyến tính 12

2 Tóm tắt vai trò của các đoạn code chính 14

3.Mô phỏng chuyển động 17

TÀI LIỆU THAM KHẢO……… 22

CHƯƠNG 1: TÓM TẮT GIỮA KÌI TÌM HIỂU VỀ ROBOT ATLAS

1 Giới thiệu về Robot Atlas

- Robot Atlas được Boston Dynamics nghiên cứu phát triển ra Robot này

được trang bị hệ thống cơ xương mô phỏng cách cơ thể con người hoạtđộng để tạo ra chuyển động, ban đầu nhằm phục vụ cho mục đích quân sựnhưng do nhiều nguyên nhân mà chuyển hướng sang phục vụ cho đờisống xã hội, sản xuất của con người.

- Có khả năng di chuyển linh hoạt và dẻo dai, đang ngày một hoàn thiện để

có thể áp dụng trong thực tiễn.

- Atlas có rất nhiều phiên bản khác nhau Tuy nhiên trong phạm vi bài tập,

nhóm quyết định lựa chọn thế hệ 2021 để tìm hiểu.

2 Các thông số của robot- Thông số kỹ thuật:

Bảng 0.1 Thông số kỹ thuật của robot Atlas thế hệ 2021

- Được trang bị Lidar và Stereo Vision giúp cho Atlas có cảm quan chính

xác về không gian xung quanh nó, kết hợp với các thuật toán học máy,học sâu được tích hợp sẵn giúp Atlas có khả năng ghi nhớ và cảm nhậnmôi trường xung quanh như con người

- Nhờ sự phát triển mạnh mẽ của công nghệ in 3D, hệ thống khung xương

cũng như các cơ cấu thủy lực của Atlas được thiết kế liền mạch, giảm tốiđa các chi tiết phải áp dụng các biện pháp gia công như hàn, dập,… khiếncơ tính và khả năng truyền động được gia tăng đến mức tối đa.

3 Tính năng và hạn chế

 Tính năng:

- Hỗ trợ các dịch vụ khẩn cấp trong hoạt động tìm kiếm cứu hộ.- Leo trèo, vượt qua các dạng địa hình khó, cao ngang gối.

- Thực hiện được các động tác khó như nhào lộn, đứng vững bằng 1 chân,

quay 180 độ khi nhảy…

- Hoạt động độc lập mà không cần điều khiển bởi máy tính.

II MÔ PHỎNG DỰA TRÊN MÔ HÌNH ROBOT 1 Động học thuận

1.1.Vẽ hình, lập bảng tham số, vẽ sơ đồ cây

- Loại bỏ bậc tự do số 3:

Hình 1.1 Hình vẽ khi loại bỏ bậc tự do số 3Bảng 1.2 Tham số của robot

1.2.Mô phỏng theo các tham số tiền định cho một tư thế hợp lý

- Bộ tham số cho trước:

qR1 = [0 pi/3 pi/6 -pi/3 0]'; qL1 = [0 0 -pi/12 pi/4 pi/3 0]';

- Tư thế tương ứng với tham số trên:

Hình 1.3 Tư thế theo tham số tiền định

2 Động học ngược

2.1.Tạo tư thế mong muốn theo yêu cầu

- Thông số về vị trí và góc quay của bàn chân:

Rfoot.p = [0, -0.1, 0]’;

Rfoot.R = rpy2rot(ToRad*0, ToRad*30 ,ToRad*0); Lfoot.p = [-0.2, 0.1, 0.2]’;

Lfoot.R = rpy2rot(ToRad*0, ToRad*30 ,ToRad*0);

- Tư thế tương ứng với tham số trên:

Hình 1.4 Tư thế mong muốn

2.2.Tính vận tốc khớp theo vận tốc bàn chân

- Xét tư thế với chân trái đứng yên, chân phải nâng lên với vector góc khớp

chân phải và trái như sau:qR1 = [0 0 -pi/6 pi/4 -pi/12 0]';qL1 = [0 0 0 0 0 0]';

- Tư thế tương ứng với tham số trên:

Hình 1.5 Tư thế bàn chân phải nâng lên

- Giả sử bàn chân phải nâng lên với vận tốc theo phương z là 0,1m/s, từ đó

- Ta được tư thế kỳ dị như hình dưới đây:

Hình 0.6 Tư thế kì dị

CHƯƠNG 2: MÔ PHỎNG ZMP1 Con lắc ngược tuyến tính

 

(2.2)Động năng của cơ hệ có dạng:

Tính các đạo hàm:sin

. 0

( )

Mr 

M rr

2

  

M rd tr

   

 

Thay vào phương trình lagrange ta được:

2 2 sin /

r  r r gr   M

Hình 0.8 Lý do cho quỹ tích nằm ngang của CoM.

Xét hình 2.2 thành phần nằm ngang tăng tốc CoM theo chiều ngang:

x : điều kiện đầu

2 Tóm tắt vai trò của các đoạn code chính

- Mục đích:

+ Mô phỏng chuyển động của robot khi tư thế ban đầu mất cân bằng vớiđiều kiện đầu cho trước là một chân robot giữ nguyên trên mặt đất, mộtchân nhấc lên, chiều cao của trọng tâm robot không đổi

+ Xác định ZMP và trọng tâm dựa trên nguyên lý con lắc ngược tuyến tính với chiều cao trạng tâm không đổi

- Giải thích các đoạn code chính:Sử dụng file SetupBipedRobot2.m:

uLINK = struct('name','BODY' , 'm', 10, 'sister', 0, 'child', 2, 'b',[0 0 0.7]','a',UZ,'q',0);

uLINK(2) = struct('name','RLEG_J0' , 'm', 5, 'sister', 8, 'child', 3, 'b',[0 -0.1 0]' ,'a',UZ,'q',0);

uLINK(3) = struct('name','RLEG_J1' , 'm', 1, 'sister', 0, 'child', 4, 'b',[0 0 0]','a',UX,'q',0);

uLINK(4) = struct('name','RLEG_J2' , 'm', 5, 'sister', 0, 'child', 5, 'b',[0 0 0]' ,'a',UY,'q',0);

uLINK(5) = struct('name','RLEG_J3' , 'm', 1, 'sister', 0, 'child', 6, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(6) = struct('name','RLEG_J4' , 'm', 6, 'sister', 0, 'child', 7, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(7) = struct('name','RLEG_J5' , 'm', 2, 'sister', 0, 'child', 0, 'b',[0 0 0 ]' ,'a',UX,'q',0);

uLINK(8) = struct('name','LLEG_J0' , 'm', 5, 'sister', 0, 'child', 9, 'b',[0 0.1 0]' ,'a',UZ,'q',0);

uLINK(9) = struct('name','LLEG_J1' , 'm', 1, 'sister', 0, 'child',10, 'b',[0 0 0]','a',UX,'q',0);

uLINK(10)= struct('name','LLEG_J2' , 'm', 5, 'sister', 0, 'child',11, 'b',[0 0 0]' ,'a',UY,'q',0);

uLINK(11)= struct('name','LLEG_J3' , 'm', 1, 'sister', 0, 'child',12, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(12)= struct('name','LLEG_J4' , 'm', 6, 'sister', 0, 'child',13, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(13)= struct('name','LLEG_J5' , 'm', 2, 'sister', 0, 'child', 0, 'b',[0 0 0 ]' ,'a',UX,'q',0);

uLINK(n).dq = 0; % vận tốc góc khớp [rad/s] uLINK(n).ddq = 0; % gia tốc góc khớp [rad/s^2] uLINK(n).c = [0 0 0]'; % toạ độ khối tâm [m]

uLINK(n).I = zeros(3,3); % tenxo quán tính khối tâm [kg.m^2] uLINK(n).Ir = 0.0; % mô men quán tính [kg.m^2]

uLINK(n).gr = 0.0; % tỉ số truyền [-] uLINK(n).u = 0.0; % mô men xoắn [Nm]

end

% khai báo các điều kiện ban đầu của robotuLINK(BODY).p = [0.0, 0.0, 0.5]';

uLINK(BODY).R = eye(3);uLINK(BODY).v = [0, 0, 0]';uLINK(BODY).w = [0, 0, 0]';Rfoot.p = [0, -0.05, 0.1]';Rfoot.R = eye(3);

Rfoot.v = [0 0 0]';Rfoot.w = [0 0 0]';

Lfoot.p = [0, 0.1, 0.05]'; % chân trái cố địnhLfoot.R = eye(3);

Lfoot.v = [0 0 0]';Lfoot.w = [0 0 0]';% động học ngược

InverseKinematicsAll(RLEG_J5, Rfoot);InverseKinematicsAll(LLEG_J5, Lfoot);ForwardVelocity(1);

Zc = com(3); % chiều cao con lắc ngược tuyến tính Tc = sqrt(Zc/G); % hằng số thời gian của LIMP

cx0 = com(1);cy0 = com(2);

% khai báo tham số đưa vào vòng lặp

EndTime = 0.48;

time = 0:Dtime:EndTime;tsize = length(time);com_m = zeros(tsize,3);zmp_m = zeros(tsize,2);dqL_m = zeros(tsize,6);figure

k = 1;zmpz = 0.0;

for k = 1:tsize

% tính vận tốc khối tâm robot dựa trên mo hình con lắc ngược tuyến tính [px,vx] = LIPM(time(k),Lfoot.p(1),cx0,Tc);

[py,vy] = LIPM(time(k),Lfoot.p(2),cy0,Tc); uLINK(BODY).p = [px, py, 0.5]';

uLINK(BODY).v = [vx, vy, 0.0]';

InverseKinematicsAll(LLEG_J5, Lfoot); % giữ chân trái tiếp xúc với sàn ForwardVelocity(1);

%%% tính toán CoM và ZMP

com = calcCoM; % Center of mass P = calcP(1); % Linear momentum L = calcL(1); % Angular momentum

dP = (P-P1)/Dtime; dL = (L-L1)/Dtime;

[zmpx,zmpy] = calcZMP(com,dP,dL,zmpz); P1 = P;

L1 = L;

com_m(k,:) = com';

zmp_m(k,:) = [zmpx, zmpy]; dqL_m(k,:)=vq';

for i=1:13

KQ(i,k)=uLINK(i).q; end

- Chọn tư thế ban đầu của robot:

uLINK(BODY).p = [0.0, 0.0, 0.5]';uLINK(BODY).R = eye(3);

uLINK(BODY).v = [0, 0, 0]';uLINK(BODY).w = [0, 0, 0]';Rfoot.p = [0, -0.05, 0.1]';Rfoot.R = eye(3);

Rfoot.v = [0 0 0]';Rfoot.w = [0 0 0]';

Lfoot.p = [0, 0.1, 0.05]'; % chân trái cố địnhLfoot.R = eye(3);

Lfoot.v = [0 0 0]';Lfoot.w = [0 0 0]';

Thời gian EndTime max = 0.46 (s):

- Các tham số của chân chạm đất:

+ Các tham số vận tốc khớp của chân chạm đất được xác định như sau:

InverseKinematicsAll(LLEG_J5, Lfoot);dqL_m(k,:)=vq';

+ Các tham số về góc khớp của hai chân đươc xác định như sau:

3.2 Mô hình loại bỏ 1 DOF (khớp 3)

Sửa file SetupBipedRobot2.m:

uLINK = struct('name','BODY' , 'm', 10, 'sister', 0, 'child', 2, 'b',[0 0 0.7]','a',UZ,'q',0);

uLINK(2) = struct('name','RLEG_J0' , 'm', 5, 'sister', 7, 'child', 3, 'b',[0 -0.1 0]' ,'a',UZ,'q',0);

% uLINK(3) = struct('name','RLEG_J1' , 'm', 1, 'sister', 0, 'child', 4, 'b',[0 0 0]','a',UX,'q',0);

uLINK(3) = struct('name','RLEG_J2' , 'm', 5, 'sister', 0, 'child', 4, 'b',[0 0 0]' ,'a',UY,'q',0);

uLINK(4) = struct('name','RLEG_J3' , 'm', 1, 'sister', 0, 'child', 5, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(5) = struct('name','RLEG_J4' , 'm', 6, 'sister', 0, 'child', 6, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(6) = struct('name','RLEG_J5' , 'm', 2, 'sister', 0, 'child', 0, 'b',[0 0 0 ]' ,'a',UX,'q',0);

uLINK(7) = struct('name','LLEG_J0' , 'm', 5, 'sister', 0, 'child', 8, 'b',[0 0.1 0]' ,'a',UZ,'q',0);

uLINK(8) = struct('name','LLEG_J1' , 'm', 1, 'sister', 0, 'child',9, 'b',[0 0 0]','a',UX,'q',0);

uLINK(9)= struct('name','LLEG_J2' , 'm', 5, 'sister', 0, 'child',10, 'b',[0 0 0]' ,'a',UY,'q',0);

uLINK(10)= struct('name','LLEG_J3' , 'm', 1, 'sister', 0, 'child',11, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(11)= struct('name','LLEG_J4' , 'm', 6, 'sister', 0, 'child',12, 'b',[0 0 0.3]' ,'a',UY,'q',0);

-uLINK(12)= struct('name','LLEG_J5' , 'm', 2, 'sister', 0, 'child', 0, 'b',[0 0 0 ]' ,'a',UX,'q',0);

Thời gian EndTime max = 0.45 (s):

- Các tham số của chân chạm đất:

+ Các tham số vận tốc khớp của chân chạm đất được xác định như sau:

InverseKinematicsAll(LLEG_J5, Lfoot);dqL_m(k,:)=vq';

+ Các tham số về góc khớp của hai chân đươc xác định như sau:

TÀI LIỆU THAM KHẢO

[1] TS Bùi Hải Lê, Slide bài giảng Robot dạng người

[2] Design of a Momentum-Based Control Framework and Application to theHumanoid Robot Atlas.

[3] Optimization Based Full Body Control for the Atlas Robot.