ĐỀ BÀI

I. Tìm hiểu và trình bày về một robot dạng người (Xuất xứ, Thông số vật lý,
Tính năng/hạn chế, Minh họa)
II. Mô phỏng dựa trên mô hình robot dạng người Hình 2.19
II.1. Động học thuận:
- Loại bỏ bậc tự do theo nhóm
- Vẽ hình, lập bảng tham số, vẽ sơ đồ cây
- Mô phỏng theo các tham số tiền định cho một số tư thế hợp lý
II.2. Động học ngược:
(Mô hình đầy đủ, tham số tiền định)
- Tạo tư thế mong muốn theo yêu cầu (đưa ra công thức cho tư thế đặc thù nếu có)
- Tính vận tốc khớp theo vận tốc bàn chân
- Tìm 1 tư thế kì dị

1
 I. Tìm hiểu và trình bày về một robot dạng người
1. Xuất sứ:
- Robot humanoid HR-OS5 là sản phẩm của công ty Trossen Robotics, một
công ty công nghệ của Hoa Kỳ

2. Thông số vật lý
- Chiều cao: 68.5cm
- Cân nặng: 6kg
- Bậc tự do: 20
- Tốc độ đi bộ: 30 cm/s
- Thời gian hoạt động: 45 phút
- Vật liệu:
+ Bộ khung bên trong được sản xuất từ nhôm máy bay 5052 theo mô-đun
và có thể mở rộng
+ Lớp vỏ bên ngoài: chất liệu nilin in 3D

2
3. Tính năng/ hạn chế:
- Tính năng:
+ Thiết kế với tỷ lệ và cử động giống con người, giúp ích cho nhiệm vụ
nghiên cứu về lĩnh vực robot và tương tác con người – robot
+ Trang bị nhiều cảm biến tiên tiến giúp robot nhận biết và tương tác với
môi trường xung quanh.
+ Có khả năng di chuyển, duy trì thăng bằng và thực hiện chuyển động
hai chân – điều quan trọng cho các nhiệm vụ yêu cầu di động
+ Được trang bị hệ thống thị giác để nhận biết và theo dõi các đối tượng
và người
+ Được trang bị các bộ công cụ cuối khác nhau cho các nhiệm vụ: lấy và
cầm đồ vật, thao tác các dụng cụ
+ Mã nguồn mở, tương thích với ROS
- Hạn chế:
+ Giá thành cao
+ Hạn chế về thời gian hoạt động
+ Cân nặng và kích thước cồng kềnh
4. Minh họa: https://www.youtube.com/embed/ncI5ccnI1oM
5. Tham khảo: https://www.trossenrobotics.com/HR-OS5
II. Mô phỏng dựa trên mô hình robot dạng người Hình 2.19
II.1. Động học thuận:
- Loại bỏ bậc tự do 8:

3
- Vẽ lại hình, lập bảng tham số, vẽ sơ đồ cây:
+ Vẽ lại hình:

+ Sơ đồ cây:

4
+ Lập bảng tham số:
ID Name Sister Child
1 BODY 0 2
2 RLEG_J0 8 3
3 RLEG_J1 0 4
4 RLEG_J2 0 5
5 RLEG_J3 0 6
6 RLEG_J4 0 7
7 RLEG_J5 0 0
8 LLEG_J1 0 9
9 LLEG_J2 0 10
10 LLEG_J3 0 11
11 LLEG_J4 0 12
12 LLEG_J5 0 0

5
  Sau khi loại bỏ khớp 8:

+ Code matlab cho chương trình bỏ khớp 8:

%%% SetupBipedRobot.m
%%% Set biped robot structure of Figure 2.19, 2.20
%%% Field definition: Table 2.1 Link Parameters

clc
close all
clear all % claer work space

global uLINK

ToDeg = 180/pi;
ToRad = pi/180;
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';

hh=0.6;

6
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(8) = struct('name','LLEG_J1' , 'm', 1, 'sister', 0,
'child',9, 'b',[0 0.1 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);
[uLINK(1).vertex,uLINK(1).face] = MakeBox([0.1 0.3 0.5]
,[0.05 0.15 -0.05] ); % BODY
[uLINK(7).vertex,uLINK(7).face] = MakeBox([0.2 0.1
0.02] ,[0.05 0.05 0.05]); % Foot
[uLINK(12).vertex,uLINK(12).face] = MakeBox([0.2 0.1
0.02] ,[0.05 0.05 0.05]); % Foot

FindMother(1); % Find mother link from sister and child

data

%%% Substitute the ID to the link name variables. For

example, BODY=1.
for n=1:length(uLINK)
7
 eval([uLINK(n).name,'=',num2str(n),';']);
end

uLINK(BODY).p = [0.0, 0.0, hh]';

uLINK(BODY).R = eye(3);
ForwardKinematics(1);

uLINK(BODY).v = [0 0 0]';
uLINK(BODY).w = [0 0 0]';
for n=1:length(uLINK)
uLINK(n).dq = 0; % joitn speed [rad/s]
end

DrawAllJoints(1);
view(38,14)
axis equal
zlim([-0.4 1.3])
grid on

- Mô phỏng theo các tham số tiền định cho một tư thế hợp lí: Tư thể bước đi
+ qR1 = [0 0 0 10 -10 0]; qL1 = [0 -30 15 0 0]

8
 + Code matlab cho tư thế đã chọn:
% fk_random.m
% 2004 Dec.17 s.kajita AIST
clc
close all
clear all % claer work space
global uLINK % allow access from external functions

SetupBipedRobot_8;

figure

qR1 = [0 0 0 10 -10 0]*ToRad;

qL1 = [0 -30 15 0 0]*ToRad;
for n=0:5
uLINK(RLEG_J0+n).q = qR1(n+1);
end

for n=0:4
uLINK(LLEG_J1+n).q = qL1(n+1);
end

uLINK(BODY).p = [0.0, 0.0, 0.7]';

uLINK(BODY).R = eye(3);
ForwardKinematics(1);

clf
DrawAllJoints(1);
view(38,14)
axis equal
zlim([0.1 1.3])
grid on

II.2. Động học ngược robot dạng người

- - Tạo tư thế mong muốn theo yêu cầu (đưa ra công thức cho tư thế đặc thù
nếu có)
- Code Matlab cho mô hình đầy đủ trường hợp một chân duỗi thẳng và nhấc
lên

9
Công thức đưa ra:
L=0.6;
zF=0.4;
xF=sqrt(L^2-(L-zF)^2);
-
% Analytical inverse kinematics with random target
% fk_random.m
% 2004 Dec.17 s.kajita AIST

close all
clear % clear work space
global uLINK % allow access from external functions

SetupBipedRobot; % Set the biped robot of Fig.2.19 and Fig.2.20

%%%%%%%%%%% set non singular posture %%%%%%%%%%%

%
uLINK(RLEG_J2).q = -10.0*ToRad;
uLINK(RLEG_J3).q = 20.0*ToRad;
uLINK(RLEG_J4).q = -10.0*ToRad;

uLINK(LLEG_J2).q = -10.0*ToRad;
uLINK(LLEG_J3).q = 20.0*ToRad;
uLINK(LLEG_J4).q = -10.0*ToRad;

uLINK(BODY).p = [0.0, 0.0, 0.6]';

uLINK(BODY).R = eye(3);

%%%%%%%%%%% random target foot position and orientation %%

%%%%%%%%%%

rand('state',0);
L=0.6;
zF=0.4;
xF=sqrt(L^2-(L-zF)^2);
figure
% while 1
uLINK(BODY).p = [0.0, 0.0, 0.6]';
uLINK(BODY).R = eye(3);

% Rfoot.p = [0, -0.1, 0]' + 0.2*(rand(3,1)-0.5);

10
 % Rfoot.R = RPY2R(1/2*pi*(rand(3,1)-0.5)); % -pi/4 < q < pi/4
%
% Lfoot.p = [0, 0.1, 0]' + 0.1*(rand(3,1)-0.5);
% Lfoot.R = RPY2R(1/2*pi*(rand(3,1)-0.5)); % -pi/4 < q < pi/4
Rfoot.p = [0, -0.1, 0]';% + 0.2*(rand(3,1)-0.5);
Rfoot.R = eye(3); % -pi/4 < q < pi/4

Lfoot.p = [0+xF, 0.1, 0+zF]';% + 0.1*(rand(3,1)-0.5);

Lfoot.R = eye(3); % -pi/4 < q < pi/4
%%% Analytical inverse kinematics solution
qR2 = IK_leg(uLINK(BODY),-0.1,0.3,0.3,Rfoot);
qL2 = IK_leg(uLINK(BODY), 0.1,0.3,0.3,Lfoot);

for n=0:5
uLINK(RLEG_J0+n).q = qR2(n+1);
uLINK(LLEG_J0+n).q = qL2(n+1);
end

ForwardKinematics(1);

clf
DrawAllJoints(1);
view(38,10)
axis equal
zlim([-0.2 1.2])
grid on

% fprintf('Type any key for another pose, Ctrl-C to abort\n');

% pause
% end

% Analytical inverse kinematics with random target

% fk_random.m
% 2004 Dec.17 s.kajita AIST

close all
clear % clear work space
global uLINK % allow access from external functions

SetupBipedRobot; % Set the biped robot of Fig.2.19 and Fig.2.20

11
%%%%%%%%%%% set non singular posture %%%%%%%%%%%%
uLINK(RLEG_J2).q = -10.0*ToRad;
uLINK(RLEG_J3).q = 20.0*ToRad;
uLINK(RLEG_J4).q = -10.0*ToRad;

uLINK(LLEG_J2).q = -10.0*ToRad;
uLINK(LLEG_J3).q = 20.0*ToRad;
uLINK(LLEG_J4).q = -10.0*ToRad;

uLINK(BODY).p = [0.0, 0.0, 0.6]';

uLINK(BODY).R = eye(3);

%%%%%%%%%%% random target foot position and orientation %%%

%%%%%%%%%

rand('state',0);
L=0.6;
zF=0.4;
xF=sqrt(L^2-(L-zF)^2);
figure
% while 1
uLINK(BODY).p = [0.0, 0.0, 0.6]';
uLINK(BODY).R = eye(3);

% Rfoot.p = [0, -0.1, 0]' + 0.2*(rand(3,1)-0.5);

% Rfoot.R = RPY2R(1/2*pi*(rand(3,1)-0.5)); % -pi/4 < q < pi/4
%
% Lfoot.p = [0, 0.1, 0]' + 0.1*(rand(3,1)-0.5);
% Lfoot.R = RPY2R(1/2*pi*(rand(3,1)-0.5)); % -pi/4 < q < pi/4
Rfoot.p = [0, -0.1, 0]';% + 0.2*(rand(3,1)-0.5);
Rfoot.R = eye(3); % -pi/4 < q < pi/4

Lfoot.p = [0+xF, 0.1, 0+zF]';% + 0.1*(rand(3,1)-0.5);

Lfoot.R = eye(3); % -pi/4 < q < pi/4
%%% Analytical inverse kinematics solution
qR2 = IK_leg(uLINK(BODY),-0.1,0.3,0.3,Rfoot);
qL2 = IK_leg(uLINK(BODY), 0.1,0.3,0.3,Lfoot);

for n=0:5
uLINK(RLEG_J0+n).q = qR2(n+1);
uLINK(LLEG_J0+n).q = qL2(n+1);

12
 end

ForwardKinematics(1);

clf
DrawAllJoints(1);
view(38,10)
axis equal
zlim([-0.2 1.2])
grid on

% fprintf('Type any key for another pose, Ctrl-C to abort\n');

% pause
% end

- Tính vận tốc khớp dq dựa khi biết vận tốc bàn chân v
- Code Matlab cho mô hình đầy đủ

13
 % Analytical inverse kinematics with random target
% fk_random.m
% 2004 Dec.17 s.kajita AIST

close all
clear % clear work space
global uLINK % allow access from external functions

SetupBipedRobot; % Set the biped robot of Fig.2.19 and Fig.2.20

idx = FindRoute(LLEG_J5);
%SetJointAngles (idx, [0 0 -pi/3 pi/1.5 -pi/3 0])
SetJointAngles (idx, [0 0 1 0 9 1])
J = CalcJacobian (idx);
dq = J\ [0 0 0.2 0 0 0]'
inv (J)

clf
DrawAllJoints(1);
view(38,10)
axis equal
zlim([-0.2 1.2])
grid on

% fprintf('Type any key for another pose, Ctrl-C to abort\n');

% pause
% end

- Khi chạy code với trường hợp không kỳ dị v = [0 0 -pi/3 pi/1.5 -pi/3 0] ta
được hình dáng robot với vận tốc khớp dq là

14
 dq=[0 0 -0.384900179459751 0.769800358919501 -0.384900179459751 -
0.384900179459751 0]T
- Ta chọn 1 tư thế kì dị khác với hình 2.33 với SetJointAngles [0 0 1 0 9 1]
Ta được hình dáng sau:

15
16