adaptive control and simulation of a seven-link biped robot for

Adaptive Control and Simulation of A Seven-Link Biped Robot For The Combined Trajectory Motion and Stability Investigations

A. BAGHERI, M. E. FELEZI, P. N. MOUSAVI

Department of Mechanical Engineering Guilan University, Rasht, IRAN

Abstract: In this article, simulation (trajectory planning) and control of seven link biped robot on combined trajectory (three phases), have been investigated. Using mathematical interpolation for trajectory planning and designing software for the robot simulation and implementation from adaptive control process, the joint’s required torques also the ZMP diagrams to stability judgment will be obtained. Key-words: Seven Link Biped Robot, Adaptive Control, Model Reference Method, Mathematical Interpolation, Combined Trajectory 1 Introduction To date, the most concern has been on biped robot single phase [1] trajectory planning and control, whereas in this article combined trajectory planning and control of seven link biped robot have been focused. Designing software with aid of the robot breakpoints which will be passed (Trajectory Planning), the robot kinematics, dynamic and control parameter with implementation from adaptive control process in order to extracting the needed joint’s torques also yielding ZMP diagrams which play as an indicator to the robot stability will be represented. 2 Dynamics of the Robot Using the Lagrangian relation [2] as following:

UTqqdt

d

iii

−=∂∂

−∂∂

=

l

l

&

l )(τ

which l indicates the differences between the system’s kinetic and potential energies. Constructing the kinetic and potential energies:

∑

∑ ∑

=

= =

=

+==

6

0

0

6

0

6

0

22 )(21

icii

i ii

ici

ici

iii

hgmU

IvmTT ω

Which ci

ici

i Iv , are link’s center of mass velocity and inertia, respectively that are obtained from chain rule as following procedure:

PPvRv

YRi

ii

ii

ii

ii

ii

ici

ii

iiii

iii

11

11

)1()1(

)1(1

11

11

1

][

ˆ+

++

++

++

++

++

++

×+×+=

+=

ωω

ωωω

In abbreviation, the link’s center of mass velocities relative to mass center’s coordinates are extracted as following:

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡=

0

0

0.00 ωmcfc lv

(1)

⎥⎥⎥

⎦

⎤

⎢⎢⎢

⎣

⎡++−

−=

0)()cos(

)sin(

01110

10

11 ωωϕω

ϕω

ce

e

c lqlql

v

(2)

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

+++−++−

−++−

=

0

)()cos()())cos(

)sin()()sin(

2102

1210120

1210120

22 ωωω

ωωϕω

ωωϕω

c

e

e

c lqqlql

qqlql

vL

(3)

Proceedings of the 5th WSEAS International Conference on Applications of Electrical Engineering, Prague, Czech Republic, March 12-14, 2006 (pp232-240)

⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

++++−++−−+−−−

−+++−++−

=

0

)()cos()()cos()()cos(

)sin()()sin()()sin(

32103322102

3110130

322102

3110130

33

ωωωωωωωωωϕω

ωωωωωϕω

c

e

e

c

lqqlqqlql

qqlqqlql

v L

L

(4)

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

+++++−+++

+−++−−+−−−

−+++−−++

+−++−

=

0

)()sin()(

)cos()()cos()()cos(

)sin()()sin()(

)sin()()sin(

432104

4332103

422102

4110140

4332103

422102

4110140

44

ωωωωωωωωω

ωωωωωϕω

ωωωωωωω

ωωϕω

c

e

e

c

lqql

qqlqqlql

qqlqql

qqlql

v

L

L

L

L

L

(5)

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

+++++

+−+−++++

+−+−+++

+−+−++

−−+−+

−−+−−

−+−++++

−−+−+++

−−+−++

+−+−+

+−+−

=

0

)(

)2/cos()(

)2/cos()(

)2/cos()(

)2/cos()(

)2/cos(

)2/sin()(

)2/sin()(

)2/sin()(

)2/sin()(

)2/sin(

43210

4432104

332103

22102

1101

0

4432104

332103

22102

1101

0

55

bcfwing

fswingb

fswingb

fswingb

fswingb

fswingbe

fswingb

fswingb

fswingb

fswingb

fswingbe

c

l

qql

qql

qql

qql

ql

qql

qql

qql

qql

ql

v

ωωωωωω

βπωωωωω

βπωωωω

βπωωω

βπωω

βπϕω

βπωωωωω

βπωωωω

βπωωω

βπωω

βπϕω

L

L

L

L

L

L

L

L

L

(6)

⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

++++++++

+−−−++−−−

++++−−−−+

+−−−

=

0

)()2/cos()(

)2/cos()()2/cos(

)2/sin()()2/sin()(

)2/sin(

210

22102

1101

0

22102

1101

0

torsotorso

torso

torso

torsoe

torso

torso

torsoe

ctorsotorso

lqql

qqlql

qqlqql

ql

v

ωωωωπωωω

πωωπϕω

πωωωπωω

πϕω

L

L

L

L

L

(7)

Now using the above stated terms to obtaining the system’s kinetic energies and implementation from simplified potential expressions as following:

)cos()5.0( 11543211 λ−+++++−= qlmmmmmmgG torso (8))cos()5.0( 2254322 λπ +−++++= qlmmmmmgG torso (9)

)cos()5.0( 335433 λ−++= qlmmmgG (10)

)cos()5.0( 44544 λ−+= qlmmgG (11)))2/cos((55 fswingfswingcfswing qlgmG βλπ −−+−= (12)

)cos(5.0 λπ −−= torsotorsotorsotor qlgmG

(13)

After of the simplification of the relations, the seven link biped robot’s equation becomes as below:

)(),()( ii qGqqqCqqH ++= &&&&τ

(14)

where 6,2,0 L=i and GCH ,, are mass inertia, coriolis and gravitational matrix of system, which can be written as following:

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

=

67666564636261

57565554535251

47464544434241

37363534333231

27262524232221

17161514131211

)(

hhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh

qH

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

=

67666564636261

57565554535251

47464544434241

37363534333231

27262524232221

17161514131211

),(

cccccccccccccccccccccccccccccccccccccccccc

qqC &

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

=

torGGGGGG

qG

5

4

3

2

1

)(

Avoiding from bulk calculations, just the diagonal components are displayed in following:

L

L

L

L

L

L

L

L

+++−+−

−−−−−−−−++−−−−−+−++++

+−−−−−++−−−+−

++++−+−

+−+++−+=

22

21543344242

233241411331

12214433

221124

23

22

214

323231311221

332211

23

22

213122122

1122

21211

21111

([))]cos(2)cos(2

)cos(2)cos(2)cos(2)cos(2)cos()cos()cos()cos(([

))]cos(2)cos(2)cos(2)cos()cos()cos(

([))]cos(2)cos(

)cos(([))]cos(([h

llmqqllqqll

qqllqqllqqllqqllqllqll

qllqllllllm

qqllqqllqqllqllqllqll

lllmqqllqll

qllllmqlllm

cc

c

ece

eec

cc

ecee

ccec

ececc

ϕϕϕϕ

ϕϕϕϕ

ϕϕ


torso

torsoctorsotorsoctorso

torsoctorsoe

eectorso

torfswingfswingcfswing

fswingfswingcfswing

fswingfswingcfswing

fswingfswingcfswing

fswingfswingecfswing

ee

eecfswing

IIIIIIqqllqqll

qqllqllqllqllll

lmqqll

qqllqqllqqll

qqllqqll

qqllqqllqqll

qqllqllqllqll

qllqlllll

+++++++++−−−

+−+−−−+−+−++

++−+−

+−+−+−

+−+−−−−−

−−+−−−−−

−−+−+−−−−−

−−+−+++

54321

2211

1221

221122

2

2133

443443

22

24422332

11

14411331

1221

4433

221122

423

))]2/(cos(2))2/(cos(2)cos(2))2/(cos(

)cos()cos(

([))])2/(cos(2

))2/(cos(2)cos(2))2/(cos(2

)cos(2)cos(2

))2/(cos(2)cos(2)cos(2

)cos(2))2/(cos()cos()cos(

)cos()cos(

L

L

L

L

L

L

L

L

L

L

L

L

πππϕ

ϕϕ

βπ

βπ

βπ

βπ

βπϕϕϕ

ϕϕ

------------------------------------------------------------------------------

torsotorsoctorso

torsoctorso

ctorsotorfswingfswingcfswing

fswingfswingcfswing

fswingfswingcfswing

fswingfswingcfswing

cfswingc

cc

c

cc

ccc

IIIIIqqllqqllqqll

llmqqll

qqllqqll

qqllqqll

qqllqqllqqllqqllqqll

llllmqqll

qqllqqllqqllqqllqqlllllm

qqllqqllqqllllmqqlllm

++++++++−−−+−

+++−+−

+−+−+−

+−+−−−

−−−−+−−−−−−−

+++++−

+−−−−−−−−−+++

+−−−−−+++−+=

543222

111221

22233

443443

222442

233211

144113311221

224

23

2254334

424223324141

1331122124

23

224

323231311221

23

2231221

22222

))]2/(cos(2))2/(cos()cos(

([))])2/(cos(2

))2/(cos(2)cos(2

))2/(cos(2)cos(2

)cos(2))2/(cos()cos()cos()cos(

([))]cos(2

)cos(2)cos(2)cos()cos()cos(([

))]cos(2)cos()cos(([))]cos(([h

ππ

βπ

βπ

βπ

βπ

L

L

L

L

L

L

L

L

L

L

L

------------------------------------------------------------------------------

54333

44

344322

2442233222

4235

433442422332

24

2343232

23333

))])2/(cos(2))2/(cos(2

)cos(2))2/(cos()cos()cos(([

))]cos(2)cos()cos(([))]cos(([h

IIIqqllqqll

qqllqqllqqllqqlllllm

qqllqqllqqllllmqqlllm

fswingfswingcfswing

fswingfswingcfswing

fswingfswingcfswing

cfswing

cc

ccc

+++−+−

+−+−

+−+−+−

−−−−−++

+−+−−−−++−−=

βπ

βπ

βπ

L

L

L

L

L

---------------------------------------------------------------

5433

44

34432442

22454334

24444

))])2/(cos())2/(cos(2

)cos()cos(([))]cos(([h

IIqqllqqll

qqllqqllllmqqlllm

fswingfswingcfswing

fswingfswingcfswing

cfswingcc

++−+−

+−+−+−+−

−++−+=

βπ

βπ L

L

L

--------------------------------------------------------------- 544

2555 ))])2/(cos(([h Iqqlllm fswingfswingcfswingcfswing +−+−+= βπ

--------------------------------------------------------------- 066 =h

In the above stated relations, the geometrical and inertial properties of system are as following (see figures (1), (2) and (3)):

cmffcm qq .. , βλϕβφ ++=+=

Fig.1- The link’s angles and configuration

Fig. 2- The support foot parameters Regarding to figure (2), the similar parameters for swing foot are assumed such as fswingβ and cfswingl . The used parameters for simulation are listed in below: - cT : Total traveling time, including single and double support phases. - dT : Double support phase time, which is approximately equaled to 20% of cT . - mT : The time which ankle joint has maximum height during walking cycle. - k : Step number - aoH : Ankle joint maximum height - aoL : The horizontal traveled distance between ankle joint maximum height position and start point. - sD : Step length

fq cm.β

λ el

mcfl .

1q

2q 3q

4q1l

2l 3l

4l

hipa xx −sup, swingahip xx ,−

,supahip zz −swingahip zz ,−

}0{ }5{ sD


- fb qq , : Foot lift and contact angles with level ground

- gfgs qq , : The ground initial terrain angles -λ : Surface slope

Fig. 3- The foot configuration

3 Adaptive Control After obtaining the robot’s domain nonlinear dynamic relations, control process is used to force the robot to trace the pre-defined trajectory. Adaptive control [3], [4] is one of powerful method that is applicable on trajectory control systems. The main concept of adaptive method refers to online control. In most systems, the domain controlling parameters are pre-known, regarding to physical and geometrical sight. A system is called “known” when the number of poles, zeros and corresponding locations are pre-defined, although is “unknown” while the domain pole and zero’s locations are unknown. For this reasons, in conventional controlling systems, a common control process is implemented, whereas for unknown types a nonlinear control procedure is needed that mostly is called adaptive control. It’s obvious that in the method, the process tries to accommodate with environmental conditions to trace the desired tasks. So, the first priority refers to the unknown parameters estimation to complete a process which is so called “Prefect tracking”. In figure(4), a block diagram of adaptation control (Model Reference Method) process is represented:

Fig. 4: Block diagram of adaptation control (MRAC)

Mainly, adaptive control is used to handle unknown systems, whereas can be implemented to control known type which in this manner is assumed an alternative of computed torque method. In general, biped robot is known system, so following procedure is implemented to control process. Similar to other nonlinear systems, linearization plays main role in control process. Following parameters are used to perform the mentioned operation. Defining combined tracking error as below:

rqqqqs &&& −=Λ+= ~~

(15)

which qqr~,& are reference velocity of q and tracking error,

respectively. Then, linearization based on new known matrix ),,,( rr qqqqYY &&&&= in which:

aqqqqYqgqqqCqqH rrrr ˆ),,,()(),()(ˆ &&&&&&&& =++=τ Following control law is used to perform adaptive control for unknown systems:

sKaYks D−=−= ˆττ (16) where DK is strictly positive definite matrix. For certain seven link biped robot, estimated parameters are replaced with the known types as below:

sKYa D−=τ (17) For seven link biped robot, assuming given mass property constants in below:

)5(.)5(.

)5(.

)5(.

)5(.

)5(.

)5(.

)5(.

543229

5432118

542317

26

255

24544

235433

2254322

21543211

torsoe

torsoe

torso

ctorsotorso

cfswing

torso

torso

mmmmmllammmmmmlla

IIIIIIalma

lma

lmma

lmmma

lmmmmma

lmmmmmma

++++=+++++=

+++++==

=

+=

++=

++++=

+++++=

R e

a

u y

CCoonnttrrooll PPllaanntt

AAddaappttaattiioonn llaaww

MMooddeell RReeffeerreennccee

anl

afl abl


e

torso

torsocfswing

cfswingcfswingcfswing

ecfswingctorsotorsoctorsotorso

ctorsoetorso

torso

ee

lgcaaaaaa

aaaaaaaaaaaaaaaa

aaaaaaaaIaIaIIaIIIa

IIIIIallma

llmallmallma

llmallmallmallmammlla

mmllammmllammllammmlla

mmmmmllammllammmlla

/,,

,

,,,

,

,,

,,),5(.

)5(.),5(.)5(.),5(.

)5(.)5(.),5(.

*13063729536

2854342754333

266543232

765432131

30529542854327

5423264525

352425231522

521220119

18544317

5442165433215

5441145433113

54322112

54411543310

=+=+=

++=+++=+++++=

++++++===+=++=

++++==

=−=−=

−====+=

+−=++−=+−=++−=

++++=+−=++−=

Then rewriting relation (15) in following form:

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

+

⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

+

⎥⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

=

⎥⎥⎥⎥⎥⎥⎥⎥

⎦

⎤

⎢⎢⎢⎢⎢⎢⎢⎢

⎣

⎡

torrtorso

r

r

r

r

r

r

rtorso

r

r

r

r

r

r

tor GGGGGG

qqqqqqq

cc

cc

qqqqqqq

hhhhhh

hhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhhh

5

4

3

2

1

5

4

3

2

1

0

7761

1711

5

4

3

2

1

0

67

57

47

37

27

17

666564636261

565554535251

464544434241

363534333231

262524232221

161514131211

5

4

3

2

1

&

&

&

&

&

&

&

L

MM

O

L

&&

&&

&&

&&

&&

&&

&&

ττττττ

The known matrix components are extracted as below:

])[sin()2222(]222)[cos(

])[sin()222(]222)[cos(

])[sin()222(]222)[cos(

)]()22()()[sin(

]22)[cos()]()22(

)()[sin(]22)[cos()]()22(

)()[sin(]22)[cos()sin()()cos()sin()()cos()sin()()cos(

),cos()sin()()cos(

3434432

10432103410,1

4224432

1043210249,1

322332

103210238,1

432100432

10410114

43210147,1

3210032103

101133210136,1

21002102

10112210125,1

4040044,1

3030033,1

2020022,1

1*11001011,1

rr

rrrrr

rr

rrrrr

rr

rrrr

r

rr

rrrrr

rr

rrrrr

rr

rrrr

rrr

rrr

rrr

rrr

qqqqqqqqqqqqqqqqy

qqqqqqqqqqqqqqqqy

qqqqqqqqqqqqqqy

qqqqqqqqqqqqqqqqq

qqqqqqqyqqqqqqqqqq

qqqqqqqqqqqyqqqqqqqq

qqqqqqqqqqyqqqqqqyqqqqqqyqqqqqqy

qcqqqqqqy

&&&&&

L&&&&&&&&&&&&

&&&&&

L&&&&&&&&&&&&

&&&&

L&&&&&&&&&&

&&&&&&&&&

L&&&&&&

L&&&&&&&&&&

&&&&&&&&&&

L&&&&&&&&&&&

&&&&&&&&

L&&&&&&&&&

&&&&&

&&&&&

&&&&&

&&&&&

−−++

+++++++−=−−++

+++++++−=−−+

++++++−=++++−++

++−+−

+++++−=+++−+++

−+−++++−=++−++

−+−+++−=

−−+−=

−−+−=

−−+−=

−−−−+−=

ϕϕϕϕϕϕ

λϕϕ

torsorfswingrrrrrr

fswingfswingfswingrrswingf

fswingfswingfswingr

rrrrr


fswingfswingfswingr

rrrrr


fswingfswingfswingr

rrrrr

fswingfswingrswingf

rfswingrswingf

fswingfswingfswingr

rrrrr

fswingfswingfswingr

rfswingfswingr

torsotorsotorsorr

torsotorsorrrr

torsor

rtorsotorsortorso

torsotorsorrrr

rtorsortorso

torsor

rr

rrrrr

qyqyqyqyqyqqy

qqqqqqqqqqqqq

qqqqqyqqqqq

qqqqqqqqqqqqqy

qqqqqqqq

qqqqqqqqqqy

qqqqqqqqqqqqqqqqqqq

qqqqqqqqy

qq

qqqqyqqqqqqqqqqqqqqy

qqqqqqqqqqqqqqq

qqqqqqyqqqq

qqyqqqqqqq

qqqqqqqqqy

,24,1,23,1422,1321,1220,11019,1

4,4,4

32104,

4321018,1

3,3,

432103,

4321017,1

2,2,432

102,

4321016,1

10,432

10101,,43210

1,

4321015,1

,

00014,1

2102,2

2,21013,1

2100

110210,1

1,21012,1

,00

011,1

3434432

10432103410,1

,,,,,

))2/(sin())(22222())2/(cos(]

22222[))2/(sin())(

2222())2/(cos(]2222[

))2/(sin())(2

22())2/(cos(]222[

))2/(sin(])()()22[(

))2/(cos(]22[

))2/(sin()

())2/(cos(]222)[)2/(sin()(

))2/(cos(]222[)](

)()22()[)2/(sin())2/(cos(]22[

)())2/(sin())2/(cos(

])[sin()2222(]222)[cos(

&&&&&&&&&&&&&&

&&&&

L&&&&&&

L&&&&&&&&&&

&&&

L&&&&&&&

L&&&&&&&&&&

&&&&&&

L&&&&

L&&&&&&&&&&

&&&&&

L&&&&&&&&&&&&

L&&

L&&&&&&&&&&

&

L&&&&

&&&&&&

L&&&&&&&&

&&&&&

L&&&&&&&&

L&&&&&&&&

&&&

L&&

&&&&&

L&&&&&&&&&&&&

=====+=

−+−++

++++−−+−

+++++=

−+−+

+++++−−+−

+++++=

−+−++++

++−−+−

+++++=

−+−+++

++−+++++++

−−+−

+++++=

−+−

+−−+−=

++++++

−+++++=+++

−+++++−−−

+−−−+++=

+−−−

+−−−=−−++

+++++++−=

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπϕ

βπϕπ

π

π

πϕπ

ϕπ

-----------------------------------------------------------------------

))2/(sin())(())2/(cos(][

))2/(sin()())2/(cos(][),sin()])(2

222[(]2222)[cos()sin())(())(

222[(]222)[cos()sin())(())(

222[(]222)[cos()sin(])[(])[cos(

)sin()]([])[cos(),sin()](2[])[cos(),sin()(

)cos(),sin()()cos()cos()sin(]2[)cos(

0

1,

11011012,2

,00

1011,2344343

210432103410,2

24210432104243

21043210249,2

232103210323

2103210238,2

14411010147,2

13103110136,2121

01010125,24040

044,23030033,2

2*12010021,2

1,2

qqqqqqqqqqy

qqqqqqqyqqqqqq

qqqqqqqqqqyqqqqqqqqqqqqqq

qqqqqqqqqqyqqqqqqqqqqqq

qqqqqqqqqyqqqqqqqqqqy

qqqqqqqqqqyqqqqqqqqqqyqqqq

qqyqqqqqqyqcqqqqqqy

y

torsotorsor

rtorsorr

torsortorsor

torsorrrr

rrrrr

rrrrr

rrrrr

rrrrr

rrrr

rrrr

rrrr

rrrrrr

rrrr

rrr

−−−

+++−−−+=

−−−+

+−−−+=−−+

++++++++−=−++++++−−+

++++++++−=−+++++−−

+++++++−=

−−+++−=

−+−++−=−

++++−=−−

+−=−−+−=

+−+−++−=

=

ππ

ϕπϕπ

ϕϕϕϕλπϕϕ

&

L&&&&&&&

&&&

L&&&&&&&&

L&&&&&&&&&&&&&

&&&&&&&&&&&&

L&&&&&&&&&&&&&

&&&&&&&&&&

L&&&&&&&&&&&

&&&&&&&&

&&&&&&&&&

L&&&&&&&&&&

L&&&&&&&

&&&&&


torsorfswingr

rrrrr


fswingfswingfswingr

rrrrr


fswingfswingfswingr

rrrrr


rrrswingf

fswingfswingfswingr

rrrrr

fswingfswingfswingr

rfswingfswingrr

fswingfswingfswingr

rfswingfswingr

torsotorsorrtorso

rrtorsotorsotorsor

rrr

qyqyqyqyqqqyy

qqqqqqqqqqqqq

qqqqqyqqqqqqq

qqqqqqqqqqqy

qqqqqqq

qqqqqqqqqqqqqqq

qqqqqyqqq

qqqqqqqy

qqqqqqy

qqqqqqqqqqqqqqqqq

qqqy

,24,2,23,2

422,2321,221020,219,2

4,4,43

2104,

4321018,2

3,3,43

2103,

4321017,2

2,2,43

210210,43210

2,

4321016,2

1,

11011015,2

,

00014,2

2,2210

102102,

21013,2

,,,,,0

))2/(sin())(22222())2/(cos(]

22222[))2/(sin())(2

222())2/(cos(]2222[

))2/(sin()])(

222())([())2/(cos(]

222[))2/(sin()

)(())2/(cos()(

))2/(sin()())2/(cos(

))2/(sin()])(222())([())2/(cos(]

222[

&&&&

&&&&&&&&&&

&&&&&

L&&&&&

L&&&&&&&&&&

&&&&&

L&&&&&

L&&&&&&&&&&

&&&&&

L&&&&&&&&&&&&

L&&

L&&&&&&&&&&

&

L&&&&&&&

&

L&&&&

&&&&&&

L&&&&&&&&

L&&&&&&

==

==++==

−+−+++

+++−−+−

+++++=

−+−+++

+++−−+−

+++++=

−+−+++

+++−+++++++

+−+−

+++++=

−+−

++−−+−+=

−+−

+−−+−=

++++++

−++++−++

+++=

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπϕ

βπϕ

ππ

-----------------------------------------------------------------------

))2/(sin()]

)(2222())([(

))2/(cos(]2222[),)2/(sin()]

)([())2/(cos(][),)2/(sin()

)(())2/(cos()(),)2/(sin()

())2/(cos(0),sin()]

)(())(2222[(]2222)[cos(

)sin())((])[cos()sin())((])[cos(

)sin(])[(])[cos()sin()]([])[cos(

0),sin()()cos()cos()sin())((

)sin())(()sin()2()cos(,0,0

3,

3,4321

03210,43210

3,432

1017,32,

221022

1016,31,

11011

015,3,

00014,3

13,312,311,33432

104321043432

10432103410,3

2442210210249,3

2332210210238,3

14411010147,3

13103110136,3

5,34040044,3

3*1232103210

13103210302

10033,32,31,3

fswingfswingfswingr

rswingf

rrrrswingf

fswingfswingfswingrrrr

rrfswingfswingfswingr

rfswingfswingr

rrfswingfswingfswingr

rfswingfswingr

rfswingfswingfswingr

rfswingfswingr

rr

rrrr

rrrrr

rrrrr

rrrrr

rrrr

rrrr

rrr

rrrr

rrrrr

rrr

qqq

qqqqqqqqqqqqqqqqq

qqqqqqqqyqqq

qqqqqqqqqyqqq

qqqqqqqyqq

qqqqyyyyqqqq

qqqqqqqqqqqqqqqqqqqqqy

qqqqqqqqqqqqyqqqqqqqqqqqqy

qqqqqqqqqqyqqqqqqqqqqy

yqqqqqqyqcqqqqqqqqq

qqqqqqqqqqqqqqqyyy

βπ

βπ

βπ

βπ

βπ

βπ

βπϕ

βπϕ

ϕϕλ

ϕϕ

−+−

+++++

+−++++++++

+−+−+++

++=−+−

+++−−+−

++=−+−

++−−+−

+=−+−

+−−+−=

===−+++++++−−++

+++++++−=

−−+++++−=

−−+++++−=

−−+++−=

−+−++−=

=−−+−=−−−+++++

+−+++++−

+++−===

&

L&&&&&&

L&&&&&&&&&&&

L&&&&&&&&

L&&&&&

L&&&&&&

L&&&&&

L&&&&&

L&&&

L&&&&

&&

L&&&&&&&&&&&&

L&&&&&&&&&&&&

&&&&&&&&&&&

&&&&&&&&&&&

&&&&&&&&

&&&&&&&&

&&&&&

&&&&&&&

L&&&&&&&&

L&&&&

0,,,,0),)2/(sin())(

22222())2/(cos(]22222[

24,3,23,3422,3321021,3

20,319,34,4,

432104

,4321018,3

===+++=

==−+−+

+++++−−+−

+++++=

yqyqyqqqqyyyqqqqq

qqqqqqqqqqqqqy

fswingrrrrrr


fswingfswing

fswingrrrrrr

&&&&&&&&&&&&

&&&

L&&&&&

L&&&&&&&&&&&&

βπ

βπ

-----------------------------------------------------------------------

0,,,,0

))2/(sin(])22222(

)())(

[())2/(cos(]22222[

))2/(sin()])((

))2/(cos(][))2/(sin()])(

[())2/(cos(][))2/(sin()

)(())2/(cos()

(),)2/(sin()())2/(cos(,0

],)[cos(],)[cos(0],)[cos(,0,0),cos()sin()

)(2()sin())(2()sin()

)(2()sin()()cos(,0

24,4,23,4

4321022,421,420,419,4

4,,

43210

4432103210,4

32104,

4321018,4

3,33210

3321017,4

2,221

0221016,4

1,

11011

015,4,

00014,413,412,411,4

32103410,4210249,4

8,410147,46,45,44*1343

2103210242

103210141

03210403

21044,43,42,41,4

==

++++====

−+−+++++

−++++−++++

+++++−+−

+++++=

−+−++++

−−+−+++=

−+−++

+−−+−++=

−+−

++−−+−

+=−+−

+−−+−====

+++−=++−=

=+−===−−−

+++++++−++++++−

+++++−

+++−====

yqyqqqqqyyyy

qqqqqqqqq

qqqqqqqqqqqq

qqqqqqqqqqqqy

qqqqqqqq

qqqqqqyqqqqqq

qqqqqqyqqq

qqqqqq

qyqqqqqqyyyyqqqqqqyqqqqqy

yqqqqyyyqcqqq

qqqqqqqqqqqqqqqqqqq

qqqqqqqqqqqqyyyy

fswingr

rrrrr

fswingfswingfswingrswingf

rrrrrswingf

fswingfswingfswingr

rrrrr

fswingfswingfswingrr

fswingfswingrrrr

fswingfswingfswingrr

fswingfswingrrr

fswingfswingfswingr

rfswingfswingr

rfswingfswingfswingr

rfswingfswingr

rrrrrrr

rr

rrrr

rrrr

rrrrr

rrr

&&

L&&&&&&&&&&

&&

L&&&&&

L&&&&&&&&&&&&

L&&&&&&

L&&&&&&&&&&

&&&&&&

L&&&&&&&&

&&&&

L&&&&&&&

&

L&&&&&

L&&&

L&&&&

&&&&&&&&&&&&&&

&&&&&

L&&&&&&&&

L&&&&&&&

L&&&&&&&

&&&&

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπϕ

βπϕ

λ

ϕϕ

-----------------------------------------------------------------------

0,,0

))2/(sin())((

))2/(cos(][

))2/(sin()])((

))2/(cos(][

))2/(sin()])(

[())2/(cos(][

))2/(sin()

)(())2/(cos()(

))2/(cos())2/(sin()

())2/(cos(0

24,5,4321023,522,521,520,519,5

4321043210

44321018,5

342103210

3321017,5

2431021

0221016,5

1432

01011015,5

*14

3210014,5

13,512,511,510,59,58,57,56,55,54,53,52,51,5

=+++++=====

−+−+++++++

+−+−++++=

−+−++++++

+−+−+++=

−+−++++

++−+−++=

−+−++

+++−+−+=

−+−+−+−

++++−+−=

=============

yqqqqqqyyyyy

qqqqqqqqqqq

qqqqqqqy

qqqqqqqqqq

qqqqqqy

qqqqqqqq

qqqqqqy

qqqqq

qqqqqqqy

qcqq

qqqqqqyyyyyyyyyyyyyy

fswingrrrrrr

fswingfswingrrrr

fswingfswingrrrrr

fswingfswingrrrr

fswingfswingrrrr

fswingfswingrrrr

fswingfswingrrr

fswingfswingrrr

rfswingfswingrr

fswingfswingfswingfswingr

rrrfswingfswingr

&&&&&&&&&&&&

&&&&&&&&&

L&&&&&&&&&&

&&&&&&&&

L&&&&&&&&

&&&&&&

L&&&&&&&

&&&

L&&&&&&&

&

L&&&&&&

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπ

βπϕβπϕ

βπϕ

-----------------------------------------------------------------------


torsorrrr

torsorrr

torsorrrtorsorr

torsorrtorso

torsorrtorsor

qqqqyyyyyyyyyyyqqqqqqqq

qqqqqyqqqqqqqqqqyqc

qqqqqqyyyyyyyyyyy

,21024,623,622,621,620,6

19,618,617,616,615,614,62210210

221013,612110

11012,6*1

210011,6

10,69,68,67,66,65,64,63,62,61,6

,0),)2/(sin()])([(

))2/(cos(][),)2/(sin())(())2/(cos(][),cos(

))2/(sin()())2/(cos(,0

&&&&&&&&

L&&&&&&

L&&&&&&&&&&

L&&&&

L&&&&&

+++=====

======++++++

+++++=−−−++

−−−−+=−−

+−−−+−−−−=

==========

πππ

πλπ

ϕπϕπ

With aid of the needed breakpoints in gait cycle and subsequently trajectory planning of the robot in combined moods which contain three phases, two level ground (horizontal plane) and either declined or descending surfaces by using mathematical interpolation, especially VANDERMONDE Matrix strategy and designing software for simulation of the robot in single or combined manners and using the stated adaptive control process, the user’s desired parameters can be obtained easily. More details are in submitted references [1]. 4 Simulation Results As stated above, implementation from the designed software, simulations for four cases, in three phases of trajectory planning, are represented. The used robot’s specifications are listed in table (1). As can be seen from the table, the robot’s inertial and geometrical values have been chosen symmetric and equal whereas one can selects the different properties. Here, just stick diagrams for four slopes: oooo 10,5,8,12 −− involved in the second phase, are displayed in figures (5) to (8). Using the ZMP formulation [1], the obtained diagrams are calculated relative to fixed coordinate systems which are assumed at the bottom of the support feet in each gait. 5 Conclusions After trajectory planning of seven link biped robot on combined manner and designing software to simulation of the robot in MATLAB/SIMULAINK environment and extracting the kinematics and dynamic parameters, then using the adaptive control process for the known systems with the above cited relations, the robot required joint’s torques to trace predefined path over the trajectory have been obtained. Note that in the simulations the robot walk with nominal gait conditions and Moving ZMP where the fixed type and uninominal walking process can be found in given references [1].

References: [1] Mousavi P.N., “Adaptive Control of a 5DOF Biped Robot Moving on a Declined Surface”, Master’s thesis, Guilan University, 2005. [2] John J. G., “Introduction to Robotics: Mechanics and Control”, Addison-Wesley, 1989. [3] Sloting J.J. and Li W.,”Applied Nonlinear Control”, Prentiec – Hall, 1991. [4] Najafi F., Bagheri A., and Farrokhi R., “Adaptive Control Algorithm for Single – Support Phase of Motion of Motion of a New Biped Robot”, Proceeding of MMAR 2003, Poland, P.P. 1003-1010. Table 1 - The simulated robot specifications

.Shl ..Til .Tol anl abl afl

m3.0 m3.0 m3.0 m1.0 m1.0 m13.0

.Shm .Thm .Tom .Fom sD cT

kg7.5

kg10 kg43 kg3.3 m5.0 s9.0

dT mT aoH aoL edx sdx

s18.0

s4.0 m16.0 m4.0 m23.0 m23.0

gsg

gfg minH maxH shankI tightI

0 0 m60.0 m62.0 02.0 kg −

08.0 kg −

torsoI footI

24.1 mkg −

201.0 mkg −


(a)

(b)

(c)

(d)

Fig. 5- (a) Stick Diagram on λ =-12 o , (b) The Moving ZMP on seven steps, (c) Joint’s torques: __: Swing shank joint, --: Swing ankle joint, …: Supp. hip joint, .-: Swing hip joint, (d) Joint’s torques: __: Supp. ankle joint, --: Supp. shank joint

(a)

(b)

(c)

(d)

Fig. 6- (a) Stick Diagram on λ =-8 o , (b) The Moving ZMP on seven steps, (c) Joint’s torques: __: Swing shank joint, --: Swing ankle joint, …: Supp. hip joint, .-: Swing hip joint, (d) Joint’s torques: __: Supp. ankle joint, --: Supp. shank joint


(a)

(b)

(c)

(d)

Fig. 7- (a) Stick Diagram on λ =+5 o , (b) The Moving ZMP on seven steps, (c) Joint’s torques: __: Swing shank joint, --: Swing ankle joint, …: Supp. hip joint, .-: Swing hip joint, (d) Joint’s torques: __: Supp. ankle joint, --: Supp. shank joint

(a)

(b)

(c)

(d)

Fig. 8- (a) Stick Diagram on λ =+10 o , (b) The Moving ZMP on seven steps, (c) Joint’s torques: __: Swing shank joint, --: Swing ankle joint, …: Supp. hip joint, .-: Swing hip joint, (d) Joint’s torques: __: Supp. ankle joint, --: Supp. shank joint


adaptive control and simulation of a seven-link biped robot for

Documents