Professional Documents
Culture Documents
Free Body Diagram Method. Kinematics and Dynamic Modelling of A Six Leg Robot
Free Body Diagram Method. Kinematics and Dynamic Modelling of A Six Leg Robot
Free Body Diagram Method. Kinematics and Dynamic Modelling of A Six Leg Robot
Abstract y
5 (2)
i
y1
z1
θ
2i 4 0 0 1 0
0 0 0 1
x1
θ
y2 3i
2 3
x2 z2
θ a
z0
6
2 A3 = 6 sin(3 ) cos(3 ) 0 c: sin(3 ) 7
y3 z3
x0 y0
7
c
x3
4 0 0 1 0 5 (3)
leg1 leg2
0 0 0 1
γ 1 =120 γ 2 =60
r~0 = F (1; 2; 3) = 0A1(1):1A2(2):2A3(3 ):[0; 0; 0; 1]t
(4)
L The Denavit-Hartenberg (D-H) method is one of the
most popular technics used in kinematic modeling of
01
yc
d
γ 3 =180 γ 4 =0 leg4
manipulators [1][3][5]. The described robot legs are
leg3
xc
zc
similar to simple manipulators with 3 DOF. Therefore
D-H method can be used to compute the transforma-
γ 5 =240 γ 6 =300
tion matrices between referential frames (Fig.2). De-
leg5 leg6
rived transformation matrices are presented in equa-
tions 1, 2, 3, where a and c are the length of links.
Figure 2: The coordinate systems dened for each leg. Leg direct kinematic problem can be solved by these
Legs' locations around the central body. matrices. Function (4) shows it by computing tip co-
ordinates on the base system given an arbitrary triad
of joint angles (1 ; 2 ; 3).
(1i ) and vertical (2i ) rotation). The third joint is 2 3
located at the knee, connecting the upper and lower , cos(
i ) sin(
i ) 0 d: cos(
i )
link (vertical rotation (3i )). Therefore each leg has 3 cA = 6 0 0 1 0 7
DOF (degree of freedom).Considering six legs and the 0i 6
4 sin(
i ) cos(
i ) 0 ,d: sin(
i ) 7
5 (5)
additional 6 DOF for central body translation and ro- 0 0 0 1
tation, the system has a total of 24 DOF.
The dynamic modeling of the 3D structure with The six legs and the central body must be integrated
six legs is a huge problem that would lead to a great to solve the global kinematic problem. Consider the
amount of equations. Thus, to explain dynamic mod- referential located at the center of the body (Fig.2).
eling using FBD approach, a simplied planar struc- Legi coordinates in body referential are obtained using
ture is considered. However, the formalism of FBD the transformation matrix of equation 5. Note that
method can be extended to 3D structures. The sim- d = L:cos( 6 ) and each leg has a dierent
i associated
plied 2D structure has two legs and a central body. with it.
Each leg is composed by two links and two rotary For simulationpurposes it is important to be able to
joints (1i disappears). Considering central body with compute robot coordinates in an inertial frame located
3 DOF (translation in X and Y and rotation around somewhere in space. The transformation matrix i Ac
Z), the system has a total of 7 DOF. between body referential and the inertial frame de-
pends of the 6 DOF of the robot central body. Three
DOF are the angular positions (x ; y ; z ) of the body
around the inertial axes. The other three are the co-
3 The kinematic equations ordinates of the mass center (Rx ; Ry ; Rz ) in inertial
frame. The considered rotation sequence is (Y,Z,X)
3.1 Direct kinematics of 3D structure (see [1] for more details).
3.2 Direct kinematics of 2D structure
2 3
cos(1 ) 0 sin(1 ) 0 Consider the planar structure composed by a cen-
0 A1 = 6
64 sin(1 ) 0 , cos(1 ) 0 7
7 (1) tral body and a pair of legs (Fig.3). The legs used
0 1 0 05 in this example are the 3 and 4 of the equivalent 3D
0 0 0 1 structure (Fig.2). It is necessary to reach a kinematic
+ φz - C
- B
- Tjy3 B
- Ty3 Tx3 Ty4 Tjy4
00
11
(Rx, Ry) -Tx3 Tx4 -Tx4 Tjx4
00
11
00
11 -Tjy3 Tjx3 -Ty3
Fg
-Ty4 -Tjx4
+ + Fga4
θ θ + -Tjx3 Fga3
23 24 -Tjy4
A
A
Y
θ33 θ 34 Fgc3 Fgc4
- + Y
+ -
N4
N3
Fa3 Fa4
Inertial X
Frame γ = 180 γ =0 Inertial X
3 4 Frame
00
11
(Rpx3, Rpy3)
00
11
11
00
00(Rpx4, Rpy4)
11 lated in equation 10 using scalar product.
Inertial X
Considering the 2D structure, the rotation axis of
the central body (vers ~ cp) and both links of leg 3
Frame
are: the mass centers (gravitical forces), the contact For the upper link of a generic legi (B) (Fig.4):
points of the links(tension forces) and the supports 8 d2 Raxi
(friction forces)(Fig. 5). They are essential to com- >
> ma 2dt2 = Tjxi , Txi
pute the rotation force moments . The arm vectors >
>
>
< m~a dt2 =~ Tjyi , Tyi , ma g
d Rayi
coordinates (ri in equation 8) can be determined by >
subtracting pairs of points coordinates in the inertial Mai = [(Rbi , R~ai)(,T~yi)]+ (12)
frame referential.
>
>
>
> +[(R~ji , R~ai)T~ji ]
With 2D transformation matrices the derivation of
>
>
: d2 2i = vers ~ ai:M~cp
~ i:M~ai , vers
dt2
inertial points coordinates as a function of the seven Ia Icp
independent kinematic variables is straight forward. For the lower link of a generic legi (A) (Fig.4):
These functions describe the restrictions in motion im- 8 d2 Rcxi
posed by structure conguration. In the example of >
>
> mc 2dt2 = Fai , Tjxi
2D structure the ve rigid bodies are connected and, >
>
< mc dt2 = Ni , Tjyi , mc g
> d Rcyi
consequently, the motion of each one is dependent of M~ci = [(R~ji , R~ci)(,T~ji)]+ (13)
the others. >
>
> +[(R~pi , R~ci )(F~fi )]
>
>
5.2 Rotation versors of isolated rigid bod-
> ~ ci:M~ci , vers
: d2 3i = vers
dt2
~ ci:M~ai
ies
Ic Ia
0.600
0.450
0.300
0.150
References
0.000
[1] K. S. Fu,ROBOTICS: Control, Sensing, Vi-
-0.150
sion, and Intelligence,McGraw-Hill Book Com-
-0.300 pany,1987.
[2] David J. Manko,A General Model of Legged Loco-
-0.450
-0.600
motion on Natural Terrain,Kluwer Academic Pub-
-0.750
0.000 0.034 0.068 0.102 0.136 0.170 0.204 0.238 0.272 0.306 0.340
lishers,1993.
Rpx3 Rpy4
27
-45
0.000 0.034
N3
0.068 0.102 0.136 0.170 0.204
Fa4
0.238 0.272 0.306 0.340 sachusetts,1989.
[8] Qiu Xiding, Gao Yimin & Zhuang Jide, \Anal-
Fa3
N4
Figure 7: Graph1: Evolution of tips position of both ysis of the Dynamics of Six-Legged Vehicle",The
legs in inertial frame (Position(m) versus Time(s)). International Journal of Robotics Research,Vol 14,
Graph2: Applied forces on the tips of both legs Pages 1-8, 1995.
(Force(N) versus Time(s)). [9] D J van Wyk, J Spoelstra & J H de Klerk, \Math-
ematical Modeling of the Interaction Between a
7 Discussion and conclusions Tracked Vehicle and the Terrain",Applied Mathe-
matical Modeling, Vol 20, 1996.
Dynamic modeling applying Lagrange Formalism [10] Herbert Goldstein,Classical Mechanics (Chapter
is useful when a state space model is intended. How- 1), Addison-Wesley Publishing Compan,1950.
ever, for the example of the 2D structure, due to the
complexity of the derived expressions, the symbolic [11] Dare A. Wells,Theory and Problems of La-
inversion of matrix D ([1][2]), when possible, needs grangian Dynamics,McGraw-Hill Book Company
a huge amount of computation time. This computa- ,1989.
tion is unpracticable. The option is to make the dis-
crete sampling of the variables, as a way to obtain a
set of non-linear discrete equations that can be solved
with Newton method (as done in FBD). Thus, neither
using FBD nor Lagrange Formalism, the state space
equations are obtained. The mathematical resolution
of Lagrange equations is similar to the FBD method.
The Lagrange dynamic description is more condensed