Sunteți pe pagina 1din 167

UNIVERSITATEA DUNREA DE

JOS DIN GALAI


COALA DOCTORAL A FACULTII DE AUTOMATIC,
CALCULATOARE, INGINERIE ELECTRIC I ELECTRONIC

Tez de doctorat
CONTRIBUTII LA CONDUCEREA,
NAVIGAIA I EVITAREA DE OBSTACOLE A
ROBOILOR MOBILI I VEHICULELOR
AUTONOME

Conductor tiinific
Prof. Univ.Dr. Ing. Adrian Filipescu
Doctorand
Ing. Bogdan Dumitracu

Galai
2012

Investete n oameni !
FONDUL SOCIAL EUROPEAN
Programul Operaional Sectorial Dezvoltarea Resurselor Umane 2007 2013
Axa prioritar 1 Educaie i formare profesional n sprijinul creterii economice i dezvoltrii
societii bazate pe cunoastere
Domeniul major de intervenie 1.5 Programe doctorale i postdoctorale n sprijinul cercetrii
Titlul proiectului: Eficientizarea activitii studentilor din cadrul ciclului de studii doctorale EFICIENT
Numrul de identificare al contractului: POSDRU/88/1.5/S/61445

TEZ DE DOCTORAT
CONTRIBUII LA CONDUCEREA, NAVIGAIA I EVITAREA
DE OBSTACOLE A ROBOILOR MOBILI I VEHICULELOR
AUTONOME
Domeniul: Ingineria Sistemelor
Doctorand: Ing. Bogdan DUMITRASCU
Componena Comisiei de doctorat:
PREEDINTE:

Conf.univ.dr.ing. Emilia PECHEANU


Universitatea Dunrea de Jos din Galai

CONDUCTOR TIINIFIC: Prof.univ.dr.ing. Adrian FILIPESCU


Universitatea Dunrea de Jos din Galai
REFERENT OFICIAL:

Prof.univ.dr.ing. Corneliu LAZR


Universitatea Tehnic Gheorghe Asachi din Iai

REFERENT OFICIAL:

Prof.univ.dr.ing. Octavian-Cezar PSTRVANU


Universitatea Tehnic Gheorghe Asachi din Iai

REFERENT OFICIAL:

Conf.univ.dr.ing. Eugenia MINC


Universitatea Valahia din Trgovite

Cuprins
Prefa ............................................................................................................................... 6
Abstract............................................................................................................................. 7
Lista Figurilor ................................................................................................................... 8
Lista Notaiilor i Abrevierilor ....................................................................................... 15
Capitolul 1. Introducere .................................................................................................. 16
1.1 Stadiul actual......................................................................................................... 17
1.2 Structura lucrrii ................................................................................................... 21
1.3 Lista publicaiilor .................................................................................................. 22
Capitolul 2.Tipologii de senzori utilizai n conducerea roboilor mobili i vehiculelor
autonome ........................................................................................................................ 24
2.1. Encodere .............................................................................................................. 24
2.2. Senzori laser ......................................................................................................... 25
2.3. Uniti de msur inerial ................................................................................... 25
2.3.1. Erorile nregistrate n funcionarea IMU ......................................................... 26
2.3.2. Reprezentarea atitudinii ................................................................................... 31
2.3.3. Filtrul Kalman .................................................................................................. 33
2.2.4. Implementarea practic a filtrului Kalman ...................................................... 34
2.3.5. Rezultate obinute prin implementarea algoritmului de filtrare Kalman n
estimarea atitudinii..................................................................................................... 38
2.4. Concluzii .............................................................................................................. 41
Capitolul 3.Conducerea trajectory-tracking a roboilor mobili i vehiculelor autonome
42
3.1 Determinarea modelului cinematic al roboilor mobili i al vehiculelor autonome
...................................................................................................................................... 42
3.1.1 Determinarea modelului cinematic al roboilor mobili .................................... 42
3.1.2. Determinarea modelului cinematic al vehiculelor autonome .......................... 44
3.2. Conducerea sliding-mode n timp continuu ......................................................... 48
3

3.2.1 Conducerea sliding- mode n timp continuu a roboilor mobili cu 2 roi


motoare i 2 libere (WMR 2DW/2FW) ..................................................................... 53
3.2.2 Conducerea sliding- mode a vehiculului autonom cu 4 roi motoare i 4
directoare (4DW/SW) ................................................................................................ 55
3.2 Conducerea sliding-mode n timp discret ............................................................. 57
3.3.1 Conducerea sliding-mode n timp discret a WMR cu 2 DW/2FW ................... 58
3.3.2 Conducerea sliding-mode n timp discret a vehiculelor autonome .................. 61
3.4. Conducerea backstepping .................................................................................... 64
3.4.1 Conducerea backstepping aWMR cu 2DW/2FW ............................................. 69
3.4.2 Conducerea backstepping a vehiculului autonom cu 4 DW/SW ...................... 70
3.5 Concluzii ............................................................................................................... 72
Capitolul 4.Algoritmi de conducere a vehiculelor autonome i a roboilor mobili,
destinai evitrii obstacolelor .......................................................................................... 74
4.1 Algoritmi de evitare a obstacolelor implementai n conducerea roboilor mobili
...................................................................................................................................... 74
4.2. Algoritmi de evitare a obstacolelor implementai n conducerea vehiculelor
autonome ...................................................................................................................... 82
4.3. Concluzii .............................................................................................................. 87
Capitolul 5.Implementarea

conducerii

timp

real.

Rezultate

experimentale

88
5.1. Simularea conducerii roboilor mobili i vehiculelor autonome ......................... 89
5.2 Rezultatele experimentale ale conducerii n timp real ale roboilor mobili i
vehiculelor autonome ................................................................................................. 121
5.3 Concluzii ............................................................................................................. 126
Capitolul 6. Concluzii................................................................................................... 128
6.1. Contribuii privind conducerea i evitarea obstacolelor utiliznd roboi mobili i
vehicule autonome ..................................................................................................... 128
6.1.1 Estimarea atitudinii .......................................................................................... 129
6.1.2 Conducerea trajectory-tracking a roboilor mobili i vehiculelor autonome ... 129
4

6.1.3 Evitarea obstacolelor........................................................................................ 130


6.1.4 Testarea algoritmilor propui ........................................................................... 131
6.1.5 Direcii de cercetare deschise .......................................................................... 132
6.1.6 Diseminarea rezultatelor .................................................................................. 132
7.

Bibliografie .......................................................................................................... 135

8.

Anexe ................................................................................................................... 143


Anexa 1. Programul folosit la conducerea backsteppping ........................................ 143
Anexa 2. Programul folosit la conducerea sliding-mode.......................................... 147
Anexa 3. Programul principal folosit la estimarea atitudinii .................................... 151
Anexa 4. Funcia pentru calculul atitudinii utiliznd filtrul Kalman ........................ 153
Anexa 5. Funcia folosit pentru citirea datelor de la laser ...................................... 157
Anexa 6. Funcia folosit pentru detecia obstacolelor ............................................. 158
Anexa 7. Funciile folosite la cutarea unei soluii pentru evitarea obstacolului ..... 159
Anexa 8. Funcia pentru generarea traiectoriei de ocolire a obstacolului ................. 161
Anexa 9. Realizri roboi mobili............................................................................... 164

Prefa
Mulumesc tuturor celor care m-au ncurajat i ajutat la elaborarea tezei de doctorat.
n primul rnd i mulumesc profesorului coordonator, dr. ing. Adrian Filipescu,
pentru oportunitatea de a urma studiile de doctorat la Universitatea Dunarea de Jos din
Galai i pentru ajutorul i cunotintele fr de care aceast tez nu ar fi putut fi elaborat.
i mulumesc domnului profesor Urbano Nunes, de la Universitatea din Coimbra,
Institutul de Sisteme i Roboi pentru ajutorul acordat n perioada stagiului extern. Ajutorul
dumnealui a fcut posibil folosirea senzorilor ineriali.
Mulumesc domnului profesor Octavian-Cezar Pstrvanu, domnului profesor
Cornelui Lazr, doamnei confereniar Eugenia Minc pentru ajutorul n elaborarea tezei de
doctorat prin observaiile fcute n calitate de refereni oficiali.
Cercetarea i teza de doctorat s-a fcut cu suportul logistic i financiar din cadrul
proiectului Eficientizarea activitii studenilor din cadrul ciclului de studii doctoraleEFICIENT, Cod proiect POSDRU/88/1.5/S/61445, Id proiect: 61445.
Cercetarea i teza de doctorat s-a facut cu suportul logistic i financiar din cadrul
proiectului UEFISCDI-IDEI, numr proiect PN-II-ID-PCE-2011-3-0641.
Doresc s mulumesc prinilor pentru sprijinul i ncurajrile acordate.

Abstract
Mobile robots and autonomous vehicles have become very important research topics
due to their multitude of possible uses and the technological progress that made it possible to
build increasingly more complex robots at cheaper prices. Autonomous vehicles have the
potential of eliminating the accidents caused by human errors, which represent about 90
percent of all accidents.
Both WMRs(Wheeled Mobile Robots) and AVs( Autonomous Vehicles) need reliable
information about their position and heading, velocity and distance to obstacles. For this
reason, sensors like the GPS, sonar sensors, encoders, lasers and IMU( Inertial Measurement
Units) are used. An indirect Kalman filter for the estimation of the attitude of vehicles using
the data from the IMU has been presented in this thesis.
The calculation of the kinematic models for 2DW/2FW robots and 4DW/SW
autonomous vehicles has been presented.
The trajectory-tracking problem for WMRs and AVs has been solved using slidingmode control in continuous time, sliding-mode control in discrete-time and backstepping
control.
An algorithm for obstacle avoidance of WMRs using sonar sensors to detect obstacles
and one of the proposed control methods to follow an imposed trajectory and only alter
course to avoid an obstacle and return to the initial trajectory afterwards.
An algorithm for obstacle avoidance of autonomous vehicles using lasers to detect
obstacles and one of the control methods that is proposed above, to follow an imposed
trajectory and only alter course to avoid an obstacle and return to the initial trajectory
afterwards.
Simulations and real-time tests have have been performed to prove the proposed
algorithms.

Lista Figurilor
Fig. 2.1 Encoder incremental [69]

24

Fig. 2.2 Senzorul laser SICK LMS111

25

Fig. 2.3 Caracteristica zgomotului conform metodei Allan(adaptare dup lucrarea[48])

27

Fig. 2.4 Deviaia Allan a giroscoapelor

28

Fig. 2.5 Deviaia Allan a accelerometrelor

29

Fig. 2.6 Deviaia Allan pentru senzorii magnetici

30

Fig. 2.7 Schema bloc a algoritmului de filtrare

36

Fig. 2.8 Vehiculul Yamaha colectnd datele pentru fuziune

39

Fig. 2.9 Variaia n timp a unghiului de tangaj

39

Fig. 2.10 Variaia n timp a unghiului de ruliu

40

Fig. 2.11 Variaia n timp a unghiului de direcie

40

Fig. 3.1 Modelul cinematic al WMR cu 2DW/2FW

42

Fig. 3.2 Modelul cinematic al vehiculului autonom SEEKUR

45

Fig. 3.3 Modelul cinematic al bicicletei

47

Fig. 3.4 Interpretarea geometrica a suprefeelor de comutaie

50

Fig. 3.5 Arhitectura conducerii sliding-mode a WMR 2DW/2FW

53

Fig. 3.6 Erorile de urmrire n cazul roboilor mobili

55

Fig. 3.7 Arhitectura conducerii sliding-mode n timp continuu a AV 4DW/SW

56

Fig. 3.8 Erorile de urmrire n cazul vehiculului SEEKUR

57

Fig. 3.9 Arhitectura de conducere sliding-mode n timp discret a WMR cu 2DW/2FW

60

Fig. 3.10 Arhitectura conducerii sliding-mode n timp discret a vehiculului autonom

62

Fig. 3.11 Schema bloc a integratorului backstepping rezultat din sistemului (3.105)

65

Fig. 3.12 Schema bloc a integratorului backstepping rezultat din sistemul (3.108)

66

Fig. 3.13 Schema bloc a integratotului backstepping rezultat din sistemul (3.110)

66

Fig. 3.14 Arhitectura de conducere backstepping a WMR cu 2DW/2FW

70

Fig. 3.15 Arhitectura de conducere backstepping a AV cu 4DW/SW

71

Fig. 4.1 Dispunerea sonarelor la PowerBot

75

Fig. 4.2 Bula de sensibilitate la PowerBot

75

Fig. 4.3 Exemplu de obstacol detectat

76

Fig. 4.4 Traiectoria dorita pentru evitarea obstacolului

78

Fig. 4.5 Schema bloc a algoritmului de conducere cu evitare de obstacole

81

Fig. 4.6 Zona de detecie a laserului

83
8

Fig. 4.7 Traiectoria dorit pentru evitarea obstacolului n cazul utilizrii deteciei cu ajutorul
senzorilor laser

84

Fig. 5.1 Robotul mobil PatrolBot i robotul mobil Powerbot

88

Fig. 5.2 Vehiculul autonom SEEKUR

88

Fig. 5.3. Arhitectura de conducere a AV 4DW/SW SEEKUR n mediul MobileSIM

90

Fig. 5.4 Traiectoria obinut prin simulare n MobileSim la conducerea sliding-mode n timp
continuu a vehiculului autonom 4DW/SW SEEKUR.

91

Fig. 5.5 Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea
sliding-mode n timp continuu a vehiculului autonom 4DW/SW SEEKUR.

91

Fig. 5.6 Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea
sliding-mode n timp continuu a vehiculului autonom 4DW/SW SEEKUR.

92

Fig. 5.7 Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea
sliding-mode n timp continuu a vehiculului autonom 4DW/SW SEEKUR.

92

Fig. 5.8 Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea
sliding-mode n timp continuu a vehiculului autonom 4DW/SW SEEKUR.

92

Fig. 5.9 Suprafaa de comutaie s1 obinut prin simulare n MobileSim la conducerea


sliding-mode n timp continuu a vehiculului autonom 4DW/SW SEEKUR.

93

Fig. 5.10 Suprafaa de comutaie s2 obinut prin simulare n MobileSim la conducerea


sliding-mode n timp continuu a vehiculului autonom 4DW/SW SEEKUR.
Fig. 5.11. Arhitectura de conducere a WMR 2DW/2FW PatrolBot n mediul MobileSIM

93
94

Fig. 5.12. Traiectoria obinut prin simulare n MobileSim la conducerea sliding-mode n


timp discret a robotului 2DW/2FW PatrolBot

94

Fig. 5.13 Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea
sliding-mode n timp discret a robotului 2DW/2FW PatrolBot

95

Fig. 5.14 Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea
sliding-mode n timp discret a robotului 2DW/2FW PatrolBot

95

Fig. 5.15 Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea
sliding-mode n timp discret a robotului 2DW/2FW PatrolBot

96

Fig. 5.16 Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea
sliding-mode n timp discret a robotului 2DW/2FW PatrolBot.

96

Fig. 5.17 Suprafaa de comutaie s1 obinut prin simulare n MobileSim la conducerea


sliding-mode n timp discret a robotului 2DW/2FW PatrolBot.

96

Fig. 5.18 Suprafaa de comutaie s2 obinut prin simulare n MobileSim la conducerea


sliding-mode n timp discret a robotului 2DW/2FW PatrolBot.
9

97

Fig. 5.19. Arhitectura de conducere sliding-mode n timp discret a AV 4DW/SW SEEKUR n


MobileSIM

97

Fig. 5.20 Traiectoria obinut prin simulare n MobileSim la conducerea sliding-mode n timp
discret a vehiculului autonom 4DW/SW SEEKUR.

98

Fig. 5.21 Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea
sliding-mode n timp discret a vehiculului autonom 4DW/SW SEEKUR.

98

Fig. 5.22 Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea
sliding-mode n timp discret a vehiculului autonom 4DW/SW SEEKUR.

99

Fig. 5.23 Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea
sliding-mode n timp discret a vehiculului autonom 4DW/SW SEEKUR.

99

Fig. 5.24 Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea
sliding-mode n timp discret a vehiculului autonom 4DW/SW SEEKUR.

100

Fig. 5.25 Suprafaa de comutaie s1 obinut prin simulare n MobileSim la conducerea


sliding-mode n timp discret a vehiculului autonom 4DW/SW SEEKUR.

100

Fig. 5.26 Suprafaa de comutaie s2 obinut prin simulare n MobileSim la conducerea


sliding-mode n timp discret a vehiculului autonom 4DW/SW SEEKUR.

100

Fig. 5.27 Arhitectura de conducere backstepping a WMR 2DW/2FW PowerBot n


MobileSim

101

Fig. 5.28. Traiectoria generat de planificator pentru testarea conducerii backstepping a


robotului 2DW/2FW Powerbot

101

Fig. 5.29 Traiectoria obinut prin simulare n MobileSim la conducerea backstepping a


robotului 2DW/2FW Powerbot

102

Fig. 5.30. Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea
backstepping a robotului 2DW/2FW Powerbot

102

Fig. 5.31. Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea
backstepping a robotului 2DW/2FW Powerbot

103

Fig. 5.32. Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea
backstepping a robotului 2DW/2FW Powerbot.

103

Fig. 5.33. Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea
backstepping a robotului 2DW/2FW Powerbot

104

Fig. 5.34 Viteza liniar obinut prin simulare n MobileSim la conducerea backstepping a
robotului 2DW/2FW Powerbot

104

Fig. 5.35. Viteza unghiular obinut prin simulare n MobileSim la conducerea backstepping
a robotului 2DW/2FW Powerbot

104
10

Fig. 5.36 Arhitectura algoritmului pentru evitarea obstacolelor a robotului 2DW/2FW


PowerBot n MobileSim

105

Fig. 5.37. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a


obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot

106

Fig. 5.38. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a


obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot i traiectoria impus
106
Fig. 5.39. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

107

Fig. 5.40. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

107

Fig. 5.41. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

107

Fig. 5.42 Suprafaa de alunecare s1 obinut prin simularea n MobileSim a algoritmului de


evitare a obstacolelor, n lipsa obstacolelor , a robotului 2DW/2FW PowerBot.

108

Fig. 5.43. Suprafaa de alunecare s2 obinut prin simularea n MobileSim a algoritmului de


evitare a obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

108

Fig. 5.44. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a


obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot

109

Fig. 5.45. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a


obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot i traiectoria
impus

109

Fig. 5.46. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.
110
Fig. 5.47. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.
110
Fig. 5.48. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.
110
Fig. 5.49. Suprafaa de alunecare s1 obinut prin simularea n MobileSim a algoritmului de
evitare a obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

11

111

Fig. 5.50. Suprafaa de alunecare s2 obinut prin simularea n MobileSim a algoritmului de


evitare a obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

111

Fig. 5.51. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a


obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot

112

Fig. 5.52. Traiectoria de evitare a obstacolului obinut prin simularea n MobileSim a


algoritmului de evitare a obstacolelor, n prezena a dou obstacole, a robotului
2DW/2FW PowerBot i traiectoria dorit.

112

Fig. 5.53. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.
113
Fig. 5.54. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.
113
Fig. 5.55. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului
de evitare a obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.
113
Fig. 5.56 Suprafaa de comutaie s1 obinut prin simularea n MobileSim a algoritmului de
evitare a obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.
114
Fig. 5.57. Suprafaa de comutaie s2 obinut prin simularea n MobileSim a algoritmului de
evitare a obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.
114
Fig. 5.58. Arhitectura pentru evitarea obstacolelor a AV 4DW/SW SEEKUR

115

Fig. 5.59. Harta utilizat pentru testarea evitrii obstacolelor utiliznd vehiculul autonom
4DW/SW SEEKUR

115

Fig. 5.60. Traiectoria generat de planificator pentru vehiculul autonom 4DW/SW SEEKUR
116
Fig. 5.61. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor n cazul absenei obstacolelor a vehiculului autonom 4DW/SW SEEKUR
116
Fig. 5.62. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom 4DW/SW SEEKUR
i traiectoria impus

117

12

Fig. 5.63. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului
pentru evitarea obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom
4DW/SW SEEKUR.

117

Fig. 5.64. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului
pentru evitarea obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom
4DW/SW SEEKUR.

117

Fig. 5.65. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului
pentru evitarea obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom
4DW/SW SEEKUR.

118

Fig. 5.66. Vehiculul autonom 4DW/SW SEEKUR simulnd evitarea obstacolului n


MobileSim

118

Fig. 5.67. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW
SEEKUR

119

Fig. 5.68. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW
SEEKUR i traiectoria dorit.

119

Fig. 5.69. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului
pentru evitarea obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom
4DW/SW SEEKUR.

120

Fig. 5.70. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului
pentru evitarea obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom
4DW/SW SEEKUR.

120

Fig. 5.71. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului
pentru evitarea obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom
4DW/SW SEEKUR.

120

Fig. 5.72. Arhitectura conducerii sliding-mode n timp discret a WMR 2DW/2FW Powerbot
n timp real

121

Fig. 5.73. Traiectoria obinut la conducerea n timp real prin metoda sliding-mode n timp
discret a robotului 2DW/2FW PowerBot

122

Fig. 5.74. Eroarea de urmrire pe axa X obinut la conducerea n timp real prin metoda
sliding-mode n timp discret a robotului 2DW/2FW PowerBot.

123

Fig. 5.75. Eroarea de urmrire pe axa Y obinut la conducerea n timp real prin metoda
sliding-mode n timp discret a robotului 2DW/2FW PowerBot.
13

123

Fig. 5.76. Eroarea de urmrire a direciei obinut la conducerea n timp real prin metoda
sliding-mode n timp discret a robotului 2DW/2FW PowerBot.

123

Fig. 5.77. Suprafaa de alunecare s1 obinut la conducerea n timp real prin metoda slidingmode n timp discret a robotului 2DW/2FW PowerBot.

124

Fig. 5.78. Suprafaa de alunecare s2 obinut la conducerea n timp real prin metoda slidingmode n timp discret a robotului 2DW/2FW PowerBot.

124

Fig. 5.79. Traiectoria n form de S obinut la conducerea n timp real prin metoda slidingmode n timp discret a robotului 2DW/g2FW PowerBot.

125

Fig. 5.81. Eroarea de urmrire a orientrii n cazul traiectoriei n form de S obinut la


conducerea n timp real prin metoda sliding-mode n timp discret a robotului 2DW/2FW
PowerBot.

125

Fig. 5.80. Eroarea de urmrire pe axa X a traiectoriei n form de S obinut la conducerea n


timp real prin metoda sliding-mode n timp discret a robotului 2DW/2FW PowerBot. 125
Fig. 5.82. Suprafaa de alunecare s1 obinut la conducerea n timp real prin metoda slidingmode n timp discret a robotului 2DW/2FW PowerBot pentru urmrirea traiectoriei n
form de S.

126

Fig. 5.83. Suprafaa de alunecare s2 obinut la conducerea n timp real prin metoda slidingmode n timp discret a robotului 2DW/2FW PowerBot pentru urmrirea traiectoriei n
form de S.

126

Fig. 8.1 Aspirator robotic autonom Roomba[41]

164

Fig. 8.2 Robotul autonom pentru splat podele Scooba[41]

164

Fig. 8.3 Robotul autonom de tuns iarba produs de Husqvarna[44]

165

Fig. 8.4 Robotul autonom Robovie-II

165

Fig. 8.5 Vehiculul autonom Sojourner

166

Fig. 8.6 Vehiculul autonom TerraMax

166

Fig. 8.7 Vehiculul autonom VIAC [43]

167

14

Lista Notaiilor i Abrevierilor


2DW/2FW - 2 Driving Wheels/ 2 Free Wheel Casters
3D Tridimensional
4DW/SW 4 Driving Wheels/ Steering Wheels
AV Autonomous Vehicle
AVAR Allan Variance
BRW Bias Random Walk
IMU Inertial Measurement Unit(Unitate de Msur Inerial)
FK Filtru Kalman
MEMS - Micro-Electrical Mechanical Systems
PI Controller Proporional Integrativ
PID Controller Proporional Integrativ Derivativ
WMR Wheeled Mobile Robot
SMC Sliding-Mode Control
VFH Vector Field Histogram

15

Capitolul 1.
1.

Introducere
Roboii mobili i vehiculele autonome au devenit subiecte de cercetare foarte

importante n ultimii ani. Progresul tehnologic a permis crearea de roboi din ce n ce mai
complexi i la preuri din ce n ce mai mici. Din acest motiv, roboii mobili sunt din ce n ce
mai des folosii n industrie i chiar de ctre utilizatorii casnici. n industrie roboii mobili
sunt folosii pentru a transporta componente la liniile de asamblare i n zonele periculoase,
cum ar fi depozitele de deeuri radioactive. Au avantajul c reduc costurile de producie
pentru c pot realiza obiectivele fr a fi nevoie de supravegherea uman. Roboii mobili pot
fi folosii pentru patrularea unor obiective, asistarea persoanelor cu dizabiliti i ajuta n
gospodrii.
Vehiculele autonome pot nlocui oferii umani, astfel fiind eliminat comportamentul
imprevizibil n trafic al oferilor. n prezent cauza major a accidentelor rutiere este eroarea
oferilor. Vehiculele autonome prezint avantajul c pot naviga n condiii maxime de
siguran. Progresul n domeniul vehiculelor autonome poate revoluiona ntreg transportul
uman prin introducerea vehiculelor total autonome capabile s transporte oameni sau obiecte,
pe orice drum, capabile s calculeze traseul necesar pentru a ajunge la destinaie i s
efectueze deplasarea pe ruta calculat pentru a ajunge la destinaie fr controlul operatorilor
umani.
Roboii mobili i vehiculele autonome au nevoie de date despre poziia i orientarea
lor, viteza de deplasare i distana pn la obstacole. Pentru a obine aceste date este nevoie
de senzori, cum ar fi: sonarele, senzorii ineriali, GPS-urile, encoderele. Aceste vehicule nu
au un operator uman care s analizeze datele i s trimit comenzile necesare pentru
realizarea obiectivului i beneficiaz de un sistem de conducere n bucl nchis care
elaboreaz comenzile pe baza informaiilor primite de la senzori.
Informaia de la senzori este perturbat de zgomot i este necesar prelucrarea acestei
informaii pentru a se elimina erorile care ar putea impiedica buna funcionare a sistemului.
Se pot estima datele reale cu ajutorul unei fuziuni a datelor primite de la mai muli senzori
folosind un filtru Kalman.
Funcionarea n regim autonom are nevoie de un modul capabil s detecteze i s
planifice un traseu care s ocoleasc obstacolele detectate. Un alt aspect important este

16

prezena modulului de conducere, care furnizeaz comenzile necesare pentru ca vehiculele


autonome s funcioneze n condiiile dorite.

1.1 Stadiul actual


Un numr mare de companii au construit diverse tipuri de roboi mobili i vehicule
autonome, i un numr mare de cercettori au cercetat acest domeniu. Din acest motiv analiza
tuturor articolelor siinifice sau roboilor mobili i vehiculelor autonome nu este posibil. n
aceast seciune sunt prezentate cteva dintre realizrile notabile din acest domeniu.
Mai multe companii au dezvoltat diveri roboi mobili autonomi capabili s execute
diverse sarcini n gospodrie. Cteva exemple de astfel de roboi sunt prezentate mai jos.
Compania iRobot ,[42], comercializeaz gama Roomba de roboi-aspirator autonomi
capabili s aspire autonom podelele, inclusiv sub i n jurul mobilei i de a-lungul pereilor.
Roomba ( Fig. 8.1) poate detecta tipurile de podele i ajusta setrile pentru cea mai eficient
curare a suprafeei respective.
Compania iRobot, [42], comercializeaz i gama Scooba (Fig. 8.2) de roboi autonomi
proiectai pentru splarea podelelor, care funcioneaz similar roboilor Roomba.
Husqvarna, [45], comercializeaz gama Automower (Fig. 8.3) de roboi autonomi de
tuns iarba.
La institutul japonez pentru cercetarea roboilor, ATR, se afl n curs de dezvoltare
robotul Robovie-II (Fig. 8.4). Robovie-II, [40], este un robot proiectat s realizeze
cumprturile de la magazin. Robotul se deplaseaz n interiorul magazinului i amintete
vnztorului produsele care trebuie cumprate, iar vnztorul le depoziteaz n coul
robotului.
Unul dintre primele vehicule autonome a fost VaMp construit de echipa lui Ernst
Dickmanns de la Universitatea Bundeswehr i Mercedes-Benz n 1990. Vehiculul poate
parcurge distane lungi n trafic intens fr intervenia uman, i s recunoasc obstacole n
micare i s le evite automat. Este un Mercedes 500 SEL modificat pentru a primi comenzi
de la calculator. Comenzile sunt elaborate considernd informaiile primite de la camerele
video i nu folosete GPS.
Vehiculul autonom Sojourner (Fig. 8.5), [41], a fost folosit de NASA pentru a explora
planeta Marte i este un vehicul proiectat s evite automat obstacolele. A fost urmat de
vehiculele Spirit i Oportunity care foloseau acelai sistem de evitare a obstacolelor.

17

Vehiculul autonom TerraMax, [38], (Fig. 8.6) produs de Oshkosh Truck Corporation,
Laboratorul de Viziune artificial i Sisteme inteligente al Universitii din Parma i
Rockwell Collins a terminat proba DARPA Grand Challenge din 2005. Vehiculul utilizeaz 3
lasere LIDAR, 3 camere i 2 sisteme de navigaie GPS.
n 2010 a fost lansat testul Vislab Intercontinental Autonomous Challenge, [43], la
care au participat 4 vehicule autonome, VIAC ( Fig. 8.7), i care au strbatut distana de
15000 km de la Parma, Italia, pn la Shanghai, China, fr intervenia operatorului uman.
Vehiculele au ndurat condiii foarte dificile: osea, vreme, infrastructur, temperatur, trafic
i chiar comportament periculos al participanilor la trafic i ruta a fost necunoscut.
Pentru a rezolva aceste probleme VisLab a gndit 2 vehicule autonome care se
deplaseaz n acelai timp dar cu scopuri diferite. Primul vehicul merge autonom n
majoritatea timpului dar uneori este nevoie de intervenia uman pentru a corecta erorile, iar
al doilea vehicul urmrete primul vehicul i se deplaseaz autonom pe ntreg parcursul
testului.
Roboii mobili i vehiculele autonome sunt sisteme nonholonomice, descrise de
sisteme de ecuaii neliniare. Din acest motiv conducerea folosind regulatoare PID nu d
rezultate satisfctoare. Pentru conducerea proceselor neliniare s-au dezvoltat mai multe
metode, cum ar fi ,metodele backstepping, sliding-mode, fuzzy i reelele neuronale.
Metodele de conducere a roboilor mobili i vehiculelor autonome au ca scop
proiectarea unor controllere capabile s calculeze viteza liniar i viteza unghiular a
vehiculului pentru ca acesta sa urmreasc o traiectorie dorit. Conducerea sistemelor
nonholonomice a fost cercetat de un numr mare de cercettori i diferite regulatoare au fost
propuse.
Cercettorii din [9], [51], [52], [61], [72], [80], [98] i [117] au propus conducerea
backstepping pentru a rezolva problema conducerii sistemelor nonholonomice. Backstepping
este o procedur recursiv care descompune o problem de proiectare a ntregului sistem n
probleme de proiectare pentru sisteme de ordin inferior. Proiectarea backstepping are la baz
integratorul backstepping, acesta fiind aplicat recursiv pentru rezolvarea problemei sistemelor
complexe, procedur descris de Chen, [9], i Khalil, [57].
Alt metod propus pentru conducerea sistemelor nonholonomice este conducerea
sliding-mode. Conducerea sliding-mode a fost analizat de mai muli cercettori, cum ar fi
Utkin, [101], [102], Gao, [31], [32], Khalil, [57], i a fost aplicat de mai muli cercettori n
[11], [12], [13], [14], [47], [49], [63], [91], [92], [93], [94], [95], [96] i [103], pentru
conducerea roboilor mobili i vehiculelor autonome, cu rezultate foarte bune.
18

O metod de proiectare care integreaz conducerea sliding-mode, conducerea fuzzy i


gain schedulling este propus n [1].
Conducerea utiliznd un controller proporional i un predictor Smith a unui vehicul
autonom 4DW/SW este propus de [4].
n [50] este prezentat o metod de conducere bazat pe inteligena artificial i
folosete o metod de nvare pentru a mbunti politicile de conducere cu ajutorul
experienei i datelor de la sistem.
Un regulator neliniar, bazat pe liniarizarea modelului dinamic al vehiculului autonom,
prezentat n [64] si [69], propune o conducere bazat pe metoda Lyapunov combinat cu
principiul invariaiei LaSalle pentru urmrirea unei traiectorii.
Navigaia const n localizarea robotului i stabilirea unei traiectorii pentru ajungerea
la destinaia dorit.
W. Danwei i Qi Feng, [15], au propus o metod de planificare a traiectoriilor
vehiculelor cu 4 roi directoare bazat pe modelul cinematic i care utilizeaz la maxim
flexibilitarea oferit de roile directoare i ia n consideraie limitrile mecanismului.
Fraichard et al., [27], prezint o metod pentru planificarea traiectoriilor folosind
continous curvature pentru vehiculele autonome.
O strategie de planificare a traseului pentru vehicule autonome aeriene este prezentat
n [8]. Strategia consider puncte intermediare 3D i calculeaz o traiectorie neted
compatibil cu dinamica vehiculului, pe baza micrilor din librria precalculat de micri.
n [85] este prezentat metoda pentru calculul traiectoriei sigure pentru vehicule n
medii parial cunoscute, folosind o arhitectur de control hibrid care combin mai multe
moduri de conducere a vitezei cu un programator de manevre.
Kanarat [55] propune o metod de planificare a traiectoriei care determin un traseu
optimal pentru un robot mobil ntr-un mediu, bazat pe incertitudinea maxim permis, care
reprezint un numr repartizat fiecrei configuraii libere din mediu. Traseul optim
conecteaz punctul iniial de puntul final, prin configuraii care respect constrngerile
nonholonomice i cea mai mare incertitudine permis.
Ge et al. , [34], prezint o abordare hibrid pentru planificarea traiectoriei netede, cu
convergen global pentru roboi nonholonomici.
Garcia et al. [33] trateaz problema planificrii traiectoriei folosind optimizarea
coloniilor de furnici.

19

Foka, [26], propune o formulare ierarhic a proceselor de decizie Markov parial


observabile pentru navigaia roboilor autonomi, care poate fi rezolvat n timp real i este
folosit ca un cadru unificat pentru navigaia n medii dinamice.
Velagic et al., [104], a propus un sistem de navigaie bazat pe 3 subsisteme de
navigaie. Sistemul de nivel sczut se ocup de controlul vitezelor folosind un controller PI.
Poziionarea este calculat de nivelul mediu i este neliniar. Subsistemul de nivel nalt
utilizeaz logica fuzzy i teria Dempster-Shafer pentru proiectarea unei fuziuni de date de la
senzori, construirea hrii i planificarea traiectoriei.
[70] prezint algoritmul pentru generarea manevrelor dinamice posibile pentru
vehiculele autonome care se deplaseaz cu viteze mari i pe distane mari. Generarea se
bazeaz pe cutarea incremental n mulimea spaiului strilor.
Menegatti et al., [71], propune o navigaie bazat pe localizarea robotului prin
compararea imaginilor din locaia curent cu imagini de referin din memorie. Singurul
senzor necesar este o camer omnidirecional. Componentele Fourier ale imaginii
omnidirecionale simplific soluia problemei navigaiei robotului.
n timpul deplasrii robotului pot apare obstacole neprevzute i este nevoie de o
component capabil s evite obstacolele. Lumelsky et al., [66], a propus algoritmul bug
pentru evitarea obstacolelor, care presupune nconjurarea obstacolului pentru determinarea
punctului aflat la distan minim fa de int i continuarea traseului din acest punct.
Frazzoli et al., [28], propune planificarea deplasrii unui vehicul autonom n medii
dinamice necunoscute folosind un automaton hibrid, al crui stri reprezint manevre
fezabile.
Alt algoritm de evitarea obstacolelor este algoritmul cmpului de potenial din [5], [6]
i [58]. Acest algoritm consider c robotul este atras de fore virtuale ctre int i respins de
fore virtuale generate de obstacole, iar traiectoria este rezultatul compunerii acestor fore.
Exist situaii n care soluia nu este gsit de acest algoritm. Situaiile n care nu este gsit
soluia pot fi evitate folosind cmpurile poteniale adaptive din [2], care folosesc mai multe
puncte auxiliare de atracie pentru a permite robotului s evite obstacolele mari sau grupate
foarte aproape unul de altul. Configuraia cmpului potenial optim este calculat de un
algoritm genetic.
Navigarea i evitarea obstacolelor este realizat n [26] utiliznd o structur ierarhic
de procese Markov de decizie parial observabile.

20

Algoritmul Vector Field Hostogram din [65] elimin problema erorilor de msur ale
senzorilor prin utilizarea unei histograme polare care conine mai multe msurri succesive
recente.
Shouwenaars, [85], propune o metod de planificare a traiectoriei pentru vehicule n
medii parial cunoscute, care permite evitarea obstacolelor.
Un algoritm de planificare a traiectoriei n medii necunoscute este prezentat n [34].
Este folosit o abordare hibrid pentru a obine o traiectorie neted cu convergen global
pentru roboi difereniali nonholonomici.

1.2 Structura lucrrii


Structura tezei este urmtoarea:
Capitolul 1 a evideniat Stadiul actual al implementrii structurilor robotice n
diferite tipologii de vehicule autonome.
Capitolul 2, intitulat Tipologii de senzori utilizai n conducerea roboilor mobili i
vehiculelor autonome, a prezentat o descriere scurt a celor mai importani senzori folosii
de WMR i vehiculele autonome i a prezentat n detaliu caracteristicile senzorilor IMU.
Scopul acestui capitol este prezentarea rezultatelor cercetrii obinute prin utilizarea
senzorilor IMU realizat cu sprijinul Institutului de Sisteme i Robotic din cadrul
Universitii din Coimbra, n timpul stagiului extern. Au fost prezentate erorile care
caracterizeaz mrimile msurate de senzorii IMU i a fost analizat senzorul IMU Xsens Mti
aplicnd metoda Allan. n seciunea urmtoare modul de reprezentare a atitudinii vehiculelor
utiliznd quatenioni a fost analizat, apoi a fost prezentat FK, urmat de implementarea folosit
prentru estimarea erorilor atitudinii furnizate de IMU. Filtrul corecteaz erorile furnizate de
giroscoapele 3D utiliznd msurtorile de la accelerometre i senzorii magnetici.n seciunea
urmtoare sunt prezentate atitudinile estimate.
Capitolul 3, intitulat Conducerea trajectory-tracking a roboilor mobili i
vehiculelor autonome, a prezentat determinarea modelului cinematic al WMR cu 2DW/2FW
i modelului cinematic al vehiculului autonom electric SEEKUR(4DW/SW). Modelele
cinematice descriu micarea robotului sau a vehiculului i nu iau n calcul forele care
acioneaz asupra lor. Pentru simplificarea calculelor modelul vehicululuii autonom
SEEKUR a fost simplificat i s-a obinut un model similar modelului bicicletei. Aceste
modele au fost folosite pentru proiectarea a 3 regulatoare pentru conducerea trajectorytracking a roboilor mobili 2DW/2FW i 3 regulatoare pentru conducerea trajectory-tracking
21

a vehiculelor autonome 4DW/SW: Regulatoarele au fost calculate aplicnd metodologia


sliding-mode n timp continuu, metodologia sliding-mode n timp discret i metodologia
backstepping.
Capitolul 4, intitulat Algoritmi de conducere a vehiculelor autonome i a roboilor
mobili, destinai evitrii obstacolelor, a prezentat o metod de evitare de obstacole pentru
roboii mobili care utilizeaz sonarele i o metod de evitare a obstacolelor pentru vehiculele
autonome care folosete detecia laser. Robotul urmrete o traiectorie impus i folosete
una din metodele de conducere menionate n capitolul 4. La fiecare pas se verific dac a
fost detectat un obstacol, iar n cazul detectrii unui obstacol se caut o traiectorie pentru
evitarea obstacolului folosind Quintic equations i se calculeaz vitezele i acceleraiile care
descriu traiectoria de evitare a obstacolelor. Noua traiectorie este urmrita de procedura de
conducere, iar cnd traiectoria iniial nu mai este blocat robotul revine la traiectoria iniial.
n capitolul urmtor, Capitolul 5, intitulat Implementarea conducerii n timp real.
Rezultate experimentale, a fost prezentat implementarea metodelor propuse i rezultate
experimentale pentru validarea metodelor propuse.
Capitolul final prezint Concluzii.

1.3 Lista publicaiilor


Aceast tez are la baza urmtoarele publicaii:
1. [16] Dumitrascu B. and Filipescu A., Discrete-time sliding-mode controller for
wheeled mobile robots, Proceedings of the 18th International Conference on Control
Systems and Computer Science, vol. 1, Bucuresti, pag. 397-403, mai, 2011.
2. [17] Dumitrascu B. and Filipescu A., Sliding mode control of lateral motion for four
driving-steering wheels autonomous vehicle, Annals of the University of Craiova, vol.
7 (34), pag 20-25, 2010.
3. [18] Dumitrascu B. and Filipescu A., Sliding mode controller for steering of fourwheels driving and steering vehicle, Proceedings of the 14th International Conference
on System Theory and Control, pag. 202-206, Sinaia, 2010.
4. [19] Dumitrascu B., Filipescu A., Backstepping control of wheeled mobile robots,
Proceedings of the 15th International Conference on System Theory and Control,
Sinaia, pag. 206-211, 2011.

22

5. [20] Dumitrascu B., Filipescu A., Radaschin A., Filipescu A. Jr., Minca E., Discretetime sliding mode control of wheeled mobile robots, Proceedings of 2011 8th Asian
Control Conference (ASCC) ,Kaohiung, Taiwan, pag. 771-776, mai , 2011.
6. [21] Dumitrascu B., Filipescu A., Vasilache C., Minca E., Filipescu A. Jr., Discretetime sliding-mode control of four driving/steering wheels mobile platform,
Proceedings of the 19th Mediterranean Conference on Control and Automation,
Corfu, Grecia, iunie, 2011, pag. 1076-1081.
7. [22] Dumitrascu B., Filipescu A., Minzu V., Voda A., Minca E.,Discrete-Time
Sliding-Mode Control of Four Driving-Steering Wheels Autonomous Vehicle,
Proceedings of the 30th Chinesse Control Conference, pag. 3620-3625, 2011.
8. [24]Filipescu A., Minzu V., Dumitrascu B. and Filipescu A., Trajectory-tracking and
discrete-time sliding-mode control of wheeled mobile robots, The 2011 IEEE
International Conference on Information and Automation, iunie, 2011.
9. [93] Solea R., Filipescu A., Filipescu S, and Dumitrascu B., Sliding-mode controller
for four- wheel-steering vehicle: trajectory-tracking problem, Proceedings of the 8th
World Congress on Intelligent Control and Automation, Jinan, China, pag. 11851190, 2010.

23

Capitolul 2.
2.

Tipologii de senzori utilizai n conducerea roboilor mobili


i vehiculelor autonome
Vehiculele autonome i roboii mobili sunt dotai cu mai multe tipuri de senzori

pentru a fi posibil determinarea poziiei, orientrii i distanei pn la obstacole, aceste


informaii fiind vitale pentru funcionarea autonom.
Cele mai importante tipuri de senzori sunt: encoderele, senzorii ineriali (IMU),
sonarele i laserele.

2.1. Encodere
Un encoder este un dispozitiv electro-mecanic care convertete micarea unghiular a
roilor vehiculului n cod analog sau digital. Encoderele sunt printre senzorii cei mai folosii
pentru msurarea vitezei de rotire a unui ax. Informaia de la encoder este procesat pentru a
se obine viteza, poziia i orientarea vehiculului. In calculul vitezei i poziiei bazate pe
datele de la encodere pot apare erori datorit alunecrilor.
Exist metode de eliminare a erorilor furnizate de encodere, dintre acestea fiind
prezentate n [75] ,[76] i [78]. Viteza i poziia robotului sunt calculate n funcie de
deplasarea unghiular a roii n jurul axului i din acest motiv nu toate erorile pot fi
minimizate. Pot exista cazuri n care robotul se deplasaz pe o suprafa alunecoas chiar
dac roile sale nu se nvrt, caz n care encoderul raporteaz c robotul este staionar, sau n
cazul n care roile alunec i encoderul raporteaz o deplasare mult mai mare dect cea real.
Pentru a obine informaia real este nevoie de un alt tip de senzor, cum ar fi senzorul IMU.

Fig. 2.1 Encoder incremental [69]

24

2.2. Senzori laser


Senzorii laser sunt folosii pentru detectarea distanei pn la obstacole sau pentru
identificarea unor repere. Laserele msoar durata de timp dintre emiterea unui puls laser i
reflecia lui de ctre un obiect. Pentru c timpul dintre transmisia pulsului i recepia lui este
proporional cu distana se poate calcula distana la care se afl obiectul. Informaia este
trimis n timp real i laserele pot fi folosite n exterior datorit distanei mari de detectare a
obstacolelor(pn la 20 m pentru laserul SICK LMS 111).
Laserele pot msura obstacole aflate la unghiuri de pn la

0.25 ,

n funcie de

rezoluia laserului, oferind o detecie mai bun comparativ cu rezoluia sonarelor.

Fig. 2.2 Senzorul laser SICK LMS111

2.3. Uniti de msur inerial


Unitile de msur inerial(IMU) sunt compuse din accelerometre 3D i giroscoape
3D i uneori senzori magnetici 3D. Aceti senzori pot detecta micarea chiar i n cazurile n
care encoderele nu pot furniza corect datele.
IMU-urile sunt folosite pentru a calcula viteza, poziia i atitudinea vehiculelor,
navelor i avioanelor. Pentru a se obine viteza semnalul de la accelerometru este integrat, iar
pentru a afla poziia acceleraia este dublu integrat. Prin integrarea datelor de la giroscop se
obine unghiul de ruliu, unghiul de tangaj i orientarea. Semnalele primite de la IMU conin
erori care sunt integrate pentru a se obine datele dorite. Din acest motiv IMU reprezint un

25

senzor bun pentru perioade de timp scurte, dar acumuleaz erori din ce n ce mai mari dac
timpul de funcionare este mai lung. Aceste erori trebuie identificate i corectate pentru a
obine date corecte. Corectarea datelor de la IMU se realizeaz utiliznd filtrul Kalman.
Cercetarea senzorilor IMU Xsens Mti a fost realizat cu sprijinul Institutul de Sisteme
i Roboi din cadrul Universitii din Coimbra, unde am efectuat stagiul extern din cadrul
proiectului POSDRU.
2.3.1. Erorile nregistrate n funcionarea IMU
Un IMU ofer date precise despre atitudine i poziie pentru vehicule doar n cazul
perioadelor scurte de timp, n cazul intervalelor de timp lungi integrarea micilor erori ale
IMU conduc la erori mari. Soluia pentru o funcionare corect este identificarea i eliminarea
erorilor care afecteaz senzorul. Erorile nregistrate n funcionarea IMU i modelarea
stohastic a IMU au fost prezentate n [25], [76]i [77]. O metod pentru identificarea erorilor
este metoda Allan. Aceast metod a fost folosit de cercettorii din: [23], [37],[107] i [111].
Metoda Allan este o tehnic de analiz n domeniul timp, care a fost iniial folosit
pentru analiza stabilitii oscilatoarelor, dar apoi a fost folosit pentru analiza senzorilor
ineriali. Are avantajul c se poate identifica caracteristica procesului aleator care genereaz
perturbaia( Fig. 2.3). Aceast metod presupune c incertidudinea este cauzat de diferite
caracteristici ale componentelor zgomotului. Covariana fiecrei componente este evaluat
folosind datele din analiza Allan.
Divergena Allan se calculeaz astfel:
1. Se obine un set lung de date i se mparte n grupuri de lungimea . Setul de date
trebuie s poat fi mprit n minim 9 grupuri pentru a pstra semnificaia;
2. Se formeaz o list cu valoarea medie din fiecare grup ( a 1 , a 2 ,..., a n );
3. Se utilizeaza formula
AVAR

1
a i 1 a i 2 ;

2n 1

(2.1)

4. Caracteristicile zgomotului se determin din graficul deviaiei Allan ca o funcie de

pe o scal logaritmic. Fiecare component a zgomotului este reprezentat de o pant cu


un gradient diferit. Caracteristica general a zgomotului conform metodei Allan din [48] este
prezentat n Fig. 2.3.

26

Fig. 2.3 Caracteristica zgomotului conform metodei Allan(adaptare dup lucrarea[48])

Semnalul de la giroscoape conine viteza de rotaie perturbat de zgomot i deviaie.


Cele mai semnificative erori ale giroscoapelor sunt biasul constant, angle random walk,
flicker noise, efectul temperaturii i eroarea de calibrare. Biasul constant reprezint valoarea
medie msurat cnd giroscopul nu se rotete i se exprim n grade pe or sau radiani pe
secund. Biasul constant produce o eroare care crete liniar cu timpu( e t ).
Valoarea calculat a biasului giroscopului este:

Bgyrox 9.37799e 4 ;
Bgyroy 0.0011 ;

Bgyroz 6.2526e 4 .
Angle random walk este cauzat de integrarea zgomotului alb prezent n semnalul
giroscopului. Aceast eroare introduce o deviaie standard care crete proporional cu

t i

se msoar n / h . Pe graficul Allan este reprezentat de o pant cu gradient -1/2. Valoarea


numeric se citete din grafic la timpul

1.

Stabilitatea biasului unui giroscop se modific n timp datorit zgomotului de


comutaie n componentele electronice. Zgomotul de comutaie are frecvena sczut i apare
ca variaii ale biasului n semnal. Se modeleaz ca random walk. Stabilitatea biasului
specific cum se poate schimba biasul ntr-o perioad de timp, de obicei 100s. Valoarea lui
este exprimat ca o valoare

1 [ / h] . Dac B este biasul la timpul , o stabilitate de 0.01


27

timp de 100s nseamn c B 100 este o variabil aleatoare cu valoarea ateptat B i


deviaia standard 0.01. Uneori se exprim folosind bias random walk

BRW ( / h )

BS ( / h )
t ( h)

(2.2)

n realitate biasul nu crete nelimitat cu creterea timpului i poate fi aproximat ca un


random walk doar pentru intervale de timp scurte. Instabilitatea este reprezentat de zona
plat din zona minimului i valoarea numeric este valoarea minim din grafic.
Rate random walk este un proces cu origini incerte i e reprezentat cu o pant de 1 2 ,
iar magnitudinea poate fi citit la 3.
Rate ramp este o eroare deterministic care apare n timpul intervalelor de timp mari
i e definit[48]
Rt ,

(2.3)

unde R este coeficientul rampei. Rampa este reprezentat de o pant cu gradientul 1 i


amplitudinea poate fi calculat la 2 .
Quantization noise este cauzat de natura digital a circuitului i e reprezentat cu panta
-1 i magnitudinea poate fi citit la 3 .
Graficul Allan pentru cele 3 giroscoape ale Xsens Mti sunt prezentate n Fig. 2.4.

Fig. 2.4 Deviaia Allan a giroscoapelor

28

Zgomotul alb pentru giroscop este:

Gnx 0.00533 ;
Gny 0.056 ;

Gnz 0.00714 .
Instabilitatea giroscopului este:

BIGx 0.001283 ;
Bigy 0.00179 ;

Bigz 0.0171 .
Biasul accelerometrului este valoarea medie a acceleraiei

(m / s 2 )

cnd

accelerometrul nu este supus acceleraiei. Valoarea medie a unui set lung de date dac nu e
afectat de gravitaie este biasul. Poziia este rezultatul dublei integrri a valorilor de la
accelerometru i se obine o eroare care evolueaz astfel
p(t )

t2
.
2

Fig. 2.5 Deviaia Allan a accelerometrelor

Biasul calculat este:

Bx 0.0781 ;
By 0.0430 ;
29

(2.4)

Bz 0.0091 .
Velocity random walk se msoar n m / s / h i e cauzat de integrarea zgomotului alb.
Zgomotul alb genereaz random walk de ordinul doi n poziie, cu medie zero i deviaie
standard [107]

p (t ) t

t
3

(2.5)

Stabilitatea biasului accelerometrului determin variaia biasului n timp.


Deviaia Allan pentru accelerometre este prezentat n Fig. 2.5. Zgomotul alb apare ca
o pant cu valoarea -0.5. Valoarea pentru random walk este citit la

1.

Instabilitatea

biasului apare ca regiunea plat din zona minimului i valoarea este valoarea minima a
curbei.
Zgomotul alb al accelerometrelor este:

Ax 0.000911;

Ay 0.00092 ;
Az 0.001429 .
Instabilitatea biasului accelerometrelor este:

BIAx 0.00021;

BIay 0.00021 ;

BIAz 0.00023 .

Fig. 2.6 Deviaia Allan pentru senzorii magnetici

30

Deviaia Allan pentru senzorii magnetici este prezentat n Fig. 2.6.


Zgomotul alb al senzorilor magnetici este:

Mx 0.0001307 ;

My 0.000179 ;
Mz 0.0001156.
2.3.2. Reprezentarea atitudinii
Atitudinea este format din unghiul de ruliu, unghiul de tangaj i orientarea. n cazul
roboilor mobili i vehiculelor autonome cel mai inportant unghi este unghiul care reprezint
orientarea, dar pentru o estimare mai bun a orientrii trebuie folosite cele 3 unghiuri.
Atitudinea poate fi exprimat folosind unghiurile Euler, dar unghiurile Euler conin o
singularitate. Aceast problem a dus la stabilirea altor reprezentri: direct cosine
formulation, Euler-axis formulation, Euler-Rodrigues quaternion formulation. Aceste
reprezentri sunt prezentate n [79].
Algebra quaternionilor elimin singularitatea i are nevoie de mai puin timp de
calcul, fiind o foarte bun metod de reprezentare a atitudinii. Proprietile quaternionilor
sunt descrise de [44], [67], [86].
Un quaternion e reprezentat de un vector cu 4 elemente

q q0

q1 q 2

q3 q0
T

unde q0 este partea scalar a quaternionului i

T
q .

(2.6)

q este partea vectorial, q0 , q1 , q3 , q4 sunt

numere reale.
Elementele sunt definite astfel

q0
q
1
q 2

q3

cos 2

rx sin
2 .

r sin
y
2

rz sin
2

(2.7)

unde este unghiul necesar pentru a roti sistemul de coordonate A n jurul axei pentru
a obine sistemul de coordonate B, rx , ry , rz definesc componentele vectorului unitate r pe
axele cadrului A.

31

Quaternionul descrie o atitudine cu 3 grade de libertate. Relaia dintre componente


este
q 02 q12 q 22 q32 cos 2

(rx2 ry2 rz2 ) sin 2

1.

(2.8)

Matricea de rotaie a vectorului v de la coordonatele A la coordonatele B este


q 02 q12 q 22 q 32 2 q1 q 2 q 0 q 3 2 q1 q 3 q 0 q 2

C (q) 2 q1 q 2 q 0 q 3 q 02 q12 q 22 q 32 2 q 2 q 3 q 0 q1 .
2 q1 q 3 q 0 q 2 2 q 2 q 3 q 0 q1 q 02 q12 q 22 q 32

2.9)

Un quaternion poate fi definit ca

q q0 q1 i q2 j q3 k .

(2.10)

Adunarea i scderea quaternionilor p i q sunt definite prin adunarea i scderea


componentelor corespunztoare:

q p (q0 p0 ) i (q1 p1 ) j (q2 p2 ) k (q3 p3 ) ;

(2.11)

q p (q0 p0 ) i (q1 p1 ) j (q2 p2 ) k (q3 p3 ) .

(2.12)

Produsul este definit astfel:

q p (q0 q1 i q2 j q3 k ) ( p0 p1 i p2 j p3 k ) ;
q p q0 p0 q1 p1 q2 p2 q3 p3 i q0 p1 q1 p0 q2 p3 q3 p2

(2.13)

j q0 p2 q2 p0 q3 p1 q1 p3 k q0 p3 q3 p0 q1 p2 q2 p1

; (2.14)

q p q p q p .

(2.15)

Magnitudinea unui quaternion este


q q02 q12 q22 q32 .

(2.16)

Conjugatul unui quaternion este:

q q0 q1 i q2 j q3 k ;
q q* q

(2.17)

Un vector poate fi rotit de un quaternion folosind

v' q v q * .

(2.18)

Relaia dintre unghiurile Euler i quaternion este

q2 q3 q0 q1

arctan(
2

)
2

q0 q12 q22 q32
arcsin[ 2 q q q q ]
1
3
0
2 .

arctan(2 q0 q3 q2 q1 )

q02 q12 q22 q32

32

(2.19)

2.3.3. Filtrul Kalman


Filtrul Kalman a fost dezvoltat n 1960. Este o tehnic de estimare recursiv bazat pe
spaiul strilor, n domeniul timp. Estimatorul optimal este un algoritm care proceseaz
msurtorile pentru a obine eroare minim estimat a strii sistemului , folosind cunotine
despre sistem i dinamica msurtorilor, presupuse statistice i zgomotul sistemului, erorile
de msurare i condiiile iniiale din [3]. Filtrul Kalman a devenit tehnica principal de
combinare n sistemele multisenzor. O prezentare detaliat a FK este prezentat n [35].
Modelul filtrului Kalman liniar n timp discret este

xk Fk xk 1 Bk uk wk .

(2.20)

Unde xk este vectorul strilor la momentul k, Fk este matricea de tranziie a strilor de


la vectorul strilor la momentul k-1 la momentul K, uk este vectorul comenzii, Bk reprezint
relaia dintre vectorul de control i vectorul strilor, wk este secvena de zgomot alb necorelat
Gaussian, cu media zero i covariana Qk , adic

Q , i j k
E[ w j wTj ] k
.
0, i j

(2.21)

Vectorul observaiilor este dat de

zk H k xk vk .

(2.22)

unde H k este modelul de observaie, i vk este zgomotul de observaie presupus a fi


zgomot alb necorelat Gaussian, cu media zero i covariana Rk , adic

R , i j k
E[v j vTj ] k
.
0, i j

(2.23)

Filtrul furnizeaz o estimare a strii la momentul k, folosind observaiile pn la


momentul k. Algoritmul are dou etape: etapa de predicie i etapa de estimare.
n etapa de predicie se evalueaz starea folosind observaiile pn la momentul k-1

xk Fk x k1 Bk uk .

(2.24)

Incertitudinea prediciei este reprezentat de matricea de covarian Pk


Pk E[( xk xk ) ( xk xk )T Z k 1 ] Fk Pk1 FkT Qk .

(2.25)

Ecuaiile (2.24), (2.25) se evalueaz la fiecare pas. Cnd o estimare este disponibil
se trece la etapa de estimare, calculat astfel
33

xk xk K k k ,

(2.26)

unde K k este matricea de ctig a filtrului i k este vectorul inovaie egal cu diferena dintre
valoarea observat i valoarea prezis

k zk H k xk .

(2.27)

Ctigul este ales astfel nct s minimizeze eroarea ptrat a estimrii

K k Pk H kT S k1 ,

(2.28)

S k H k Pk H kT Rk .

(2.29)

Matricea de covarian se actualizeaz folosind:

Pk E[( xk xk ) ( xk xk )T Z ] ;
Pk ( I K k H k ) Pk ( I K k H k ) T K k R k K kT .

(2.30)
(2.31)

2.2.4. Implementarea practic a filtrului Kalman


Atitudinea vehiculului este estimat folosind accelerometre 3D, giroscoape 3D i
compasuri magnetice 3D coninute n pachetul Xsens Mti. Datele de la senzori sunt filtrate
folosind filtrul Kalman. FK este folosit foarte pentru estimarea atitudinii n [10], [53], [54],
[60], [66], [68], [82], [84], [100], [107], [109], [110], [113] i [116]. n [59] este prezentat
detectarea inclinaiei senzorului IMU utiliznd accelerometrele din senzorul IMU, i se
constat posibilitatea corectrii orientrii cu ajutorul acceleratiilor nregistrate de
accelerometre.
Pentru determinarea atitudinii se definete o stare nominal i o stare eroare. Schema
bloc a algoritmului de estimare este prezentat n Fig. 2.7.
Folosim Filtrul Kalman indirect pentru a minimiza starea eroare. Primul pas este
prezicerea strii nominale a quaternionului integrnd valorile primite de la giroscoape.
Formula pentru integrare este [53]
q

1
( w) q ,
2

unde

34

(2.32)

0
w
x
( w)
wy

wz

wx
0

wy
wz

wz
wy

0
wx

wz
wy
.
wx

(2.33)

Semnalele giroscoapelor sunt perturbate de zgomot i valoarea prezis trebuie


corectat utiliznd

q q~e q ,

(2.34)

unde q~e este eroarea estimat dintre valoarea real i cea prezis.
Pentru c q~e depinde doar de zgomotul i biasul semnalului i are valori foarte mici,
care permit aproximarea:
1
q~e
qe

(2.35)

Filtrul Kalman estimeaz eroarea i corecteaz quaternionul q folosind ecuaia (2.34).


Eroare este aproximat ca [53]:
1
qe y g qe bg vg
2

(2.36)

unde y g este rata unghiular msurat, bg este biasul giroscoapelor, v g este zgomotul.
Ieirile accelerometrelor, giroscoapelor i senzorilor magnetic au forma
ya C (q) g~ ba va ab

,
y g bg v g

~v

ym C (q)m
m

0
~
g 0
g
cos i cos d
~
m cos i sin d ,
sin i

(2.37)

(2.39)

(2.40)

unde g este acceleraia gravitaional i i este nclinaia vectorului cmpului magnetic


terestru, iar d declinaia, i i d sunt valori specifice zonei n care se efectueaz testul.

35

Fig. 2.7 Schema bloc a algoritmului de filtrare

Definim starea ca
qe
x bg
ba

(2.41)

Ecuaia procesului este


xk 1

0.5v g
Ak xk vbg ,
vba

Ak I A T

1 2 2
A T ,
2

[ y g ] 0.5I
A 0
0
0
0

Fie un vector

0
0 .
0

(2.42)

(2.43)

(2.44)

v R3 , notaia v este definit astfel


0
v v3
v2

v3
0
v1
36

v2
v1 .
0

(2.45)

Ecuaiile msurilor sunt [113]:

ya C (q ) g~ 2C (q ) g~ qe ab va ba ;

(2.46)

~ 2C (q )m
~ q v .
y m C (q )m
e
m

(2.47)

Predicia strii se face utiliznd ecuaia (2.42):

x k1 Ak xk ;

(2.48)

Pk1 Ak Pk ATk Q .

(2.49)

Corecia se face n 2 etape pentru a minimaliza erorile magnetice mari care pot fi
generate cnd senzorul este n apropierea unor obiecte metalice. Prima etap asigur corecia
utiliznd informaiile de la accelerometre i n etapa a doua se aplic corecia magnetic.

Etapa 1

Msurtoarea ya C (q ) g~ este folosit penru corecta estimarea:

Ka, k Pk H aT, k ( H a, k Pk H aT, k Ra Q a, k )1 ;

(2.50)

xa, k xk Ka, k ( za, k H a, k xk ) ;

(2.51)

T
Pa, k ( I Ka, k H a, k ) Pk ( I Ka, k H a, k )T Ka, k Ra Q a, k Ka, k ;

Pk

1
T
Pa , k Pa , k ;
2

(2.52)
(2.53)

H a, k 2C (q ) g~ 0 I .

(2.54)

Q a ,k este definit conform [113]


Q a , k U k ( H a , k Pk H aT, k Ra ) .

(2.55)

U k este simetric i exist vectori eigen ui ,k R 31 i valori corespunztoare eigen

i ,k R astfel nct
3

U k i ,k ui ,k ui ,k .
i 1

37

(2.56)

n cazul n care nu exist acceleraie extern ( max( i ,k ui ,k ) ) Q a ,k este zero. Dac


exist acceleraii externe
3

T
Q a ,k max(i ,k u i ,k ,0)u i ,k u i ,k .

(2.57)

i 1

Dup calculul estimrii corectm atitudinea astfel:

qe xa,k (1 : 3) ;

(2.58)

q q qe ;

(2.59)

q q / q ;

(2.60)

xa ,k (1 : 3) =0.

(2.61)

Etapa 2. Corecia magnetic:

Pm, k Pk ;

(2.62)

Km, k Pm, k H mT , k ( H m, k Pm, k H mT , k Rm )1 ;

(2.63)

xk xa, k Km, k ( za, k H m, k xa, k ) ;

(2.64)

~ 0 0 ;
H m, k 2C (q )m

(2.65)

~;
zm, k ym, k C (qk )m

qe xk (1 : 3) .

(2.66)
(2.67)

Se corecteaz atitudinea folosind (2.59),(2.60).


2.3.5. Rezultate obinute prin implementarea algoritmului de filtrare Kalman n
estimarea atitudinii
Datele au fost colectate n urma rulrii vehiculului Yamaha (Fig. 2.8) n parcarea
laboratorului. Vehiculul Yamaha este dotat cu un calculator care folosete sistemul de
operare ROS pentru a colecta datele de la toi senzorii echipai i pentru a funciona n regim
autonom. Datele colectate de la vehicul au fost prelucrate n MATLAB utiliznd algoritmul
prezentat n seciunea anterioar. Programul principal MATLAB este trecut n Anexa 3.
Programul principal folosit la estimarea atitudinii i n

38

Anexa 4. Funcia pentru calculul atitudinii utiliznd filtrul Kalman este trecut funcia
care implementeaz filtrul Kalman. Rezultatele unuia din testele efectuate pentru validarea
algoritmului de estimare a atitudinii sunt prezentate n figurile urmtoare. n grafice
atitudinea exprimat n quaternioni este convertit n atitudinea exprimat n grade pentru o
mai bun nelegere a rezultatelor. Conversia de la quaternioni la unghiuri Euler furnizeaz
rezultate aflate n intervalul n intervalul 180,180 .

Fig. 2.8 Vehiculul Yamaha colectnd datele pentru fuziune

Fig. 2.9 Variaia n timp a unghiului de tangaj

39

Fig. 2.9 prezint variaia n timp a unghiului de tangaj estimat de senzorul Xsens
Mti(rou) i variaia n timp a unghiului de tangaj estimat ultiliznd filtrul propus(albastru).
Fig. 2.10 prezint prezint variaia n timp a unghiului de ruliu estimat de senzorul
Xsens(rou) i variaia n timp a unghiului de tangaj estimat ultiliznd filtrul propus(albastru).

Fig. 2.10 Variaia n timp a unghiului de ruliu

Fig. 2.11 Variaia n timp a unghiului de direcie

Fig. 2.11 prezint variaia n timp a unghiului de direcie estimat de senzorul


Xsens(rou) i variaia n timp a unghiului de tangaj estimat ultiliznd filtrul propus(albastru).
Rezulatele filtrului utilizat au furnizat grafice similare celor furnizate de Xsens, se observ c
Xsens folosete valori diferite pentru condiiile iniiale, aceste valori afectnd diferenele
40

dintre grafice. Pentru c unghiul de direcie are valori mai mici de


brusc spre

180 ,

180 se

observ o trecere

cauzat de modul de reprezentare a quaternionilor. Din cele 3 grafice se

observ c filtrul propus realizeaz o bun estimare a atitudinii.

2.4. Concluzii
Roboii mobili i vehiculele autonome au nevoie de senzori, cum ar fi sonarele,
laserele, encoderele sau IMU, prentru a opera n regim autonom. Sonarele i laserele sunt
folosite pentru a determina distana dintre vehicul i obstacole, iar encoderele i IMU sunt
folosite pentru calculul odometriei platformei.
Exist situaii n care encoderele ofer informaii eronate despre odometria
vehiculului i se pot obine informaii corecte utiliznd IMU. Integrarea semnalelor de la
IMU ofer informaii despre odometrie corecte doar n cazul intervalelor scurte de timp. n
cazul intervalelor mari, micile erori ale semnalelor se acumuleaz datorit integrrilor i
rezult erori semnificative. Aceste erori sunt caracterizate utiliznd variaia Allan.
Pentru corectarea erorilor este necesar fuziunea datelor de la mai muli senzori. Am
propus filtrul Kalman indirect pentru a elimina erorile care afecteaz atitudinea platformei.
Filtrul este aplicat strii eroare pentru a minimiza eroarea, apoi eroarea estimat este aplicat
strii nominale a sistemului pentru a obine valorile reale ale atitudinii. Am calculat atitudinea
prin integrarea datelor de la giroscoape i apoi am corectat atitudinea cu ajutorul erorii
calculate de FK.
Am testat filtrul propus utiliznd datele colectate, datele au fost colectate n urma
rulrii vehiculului Yamaha n parcarea laboratorului. Vehiculul Yamaha este dotat cu un
calculator care folosete sistemul de operare ROS pentru a colecta datele de la toi senzorii
echipai i pentru a funciona n regim autonom. Am prelucrat n MATLAB datele colectate
de la vehicul utiliznd algoritmul prezentat n seciunea anterioar. Am convertit atitudinea,
n quaternioni, obinut n urma prelucrrii n atitudine exprimat n grade pentru a uura
nelegerea rezultatelor. Conversia de la quaternioni la unghiuri Euler furnizeaz rezultate
aflate n intervalul n intervalul 180,180 .
Am prezentat rezultate care au artat funcionarea eficient a filtrului Kalman propus.

41

Capitolul 3.
3.

Conducerea trajectory-tracking a roboilor mobili i


vehiculelor autonome

3.1 Determinarea modelului cinematic al roboilor mobili i al


vehiculelor autonome
Exist dou modele folosite pentru conducerea WMR i vehiculelor: modelul cinematic i
modelul dinamic. Modelul cinematic descrie micarea fr a considera forele implicate.
Modelul dinamic descrie micarea consiudernd i forele care acioneaz asupra roboilor.
n continuare vor fi prezentate modelele cinematice ale WMR i vehiculului SEEKUR,
care vor fi folosite ulterior la elaborarea conducerii.

3.1.1 Determinarea modelului cinematic al roboilor mobili


Se consider un WMR cu 2 roi motoare i dou directoare(2DW/2FW) prezentat n
Fig. 3.1, caracterizat de variabila generalizat q (q1, q2 ,..., qn ) . Se presupune c roile
vehiculului se rotesc fr a aluneca, astfel robotul este supus constngerilor nonholonomice
descrise de ecuaia
A(q) q 0 .

(3.1),

unde A(q) este matricea asociat constrngerilor.

Fig. 3.1 Modelul cinematic al WMR cu 2DW/2FW

Considerm modelul folosit n [30] i robotul mobil din Fig. 3.1. Oxy este sistemul de
coordonate, CPX r Yr este sistemul de coordonate ataat robotului, distana dintre CP i centrul

42

de greutate este d, CP se afl la mijlocul distanei dintre cele 2 roi motoare, n acest caz avem
constrngerile:

y r cos r xr sin r d 0 ;

(3.2)

xr cos r yr sin r L R d ;

(3.3)

xr cos r yr sin r L R s .

(3.4)

Matricea A(q) devine

sin r
A(q) cos r
cos r

cos r
sin r

d
L

0
R

sin r

0
0 .
R

(3.5)

Configuraia robotului poate fi reprezentat utiliznd cinci variabile generalizate

q xr

yr r r l , unde xr , yr sunt coordonatele lui CP, r este orientarea, iar

d , s sunt unghiurile roilor motoare. Fie matricea S(q) astfel nct


S T (q) AT (q) 0 ,

(3.4)

R
R

2 L L cos r d sin r 2 L L cos r d sin r


R

L sin r d cos r R L sin r d cos r


2 L
.
S (q) 2 L
R
R

2 L
2 L

1
0

0
1

(3.5)

Modelul cinematic al robotului devine


q S (q) ,

(3.6)

unde [ d s ] reprezint vitezele unghiulare ale roilor motoare.


Din (3.6) se obine

R
cos r
x r 2
y R
r sin r
r 2 R

d 2 L
s 1

cos r
2

R
sin r
2
d .
R
s

2L
0

(3.7)

Se cunoate relaia dintre viteza liniar i viteza unghiul i vitezele unghiulare ale
roilor motoare
43

1
d R
1
s
R

L
R vr .
L
r
R

(3.8)

Utiliznd relaia (3.8) n (3.7) modelul cinematic se poate simplifica astfel


x r cos r
y sin
r
r
r 0 1

0
v
0 r ,
r

(3.9)

n care:
x r - reprezint poziia robotului pe axa Ox;
y r - reprezint poziia robotului pe axa Oy;

r - reprezint orientarea robotului;


v r - reprezint viteza liniar a robotului;

r - reprezint viteza unghiular a robotului.

O alt form pentru sistemul (3.9) este

xr vr cos r

y r vr sin r .

r
r

(3.10)

3.1.2. Determinarea modelului cinematic al vehiculelor autonome


Modelul cinematic al vehiculului autonom electric SEEKUR Robot Base descrie
micarea vehiculului fr a lua n considerare masa sau forele care acioneaz asupra
vehiculului. SEEKUR este un AV care poate fi modelat considernd un WMR cu 4DW/SW.
Modelul cinematic determinat va fi folosit la proiectarea conducerii backstepping i slidingmode.
Modelul cinematic al vehiculului incluznd alunecarea lateral este prezentat in Fig.
3.2. Fiecare roat are un anumit unghi director i i un unghi al alunecrii i. Unghiul de
alunecare defineste ct de mare este alunecarea lateral i se calculeaz n funie de viteza
longitudinal i liniar a roii
v ywi
vx
wi

i tan 1

(3.11)

Subscriptul i denot nr. roii aa cum e artat in Fig. 3.2, (XCG, YCG,) defineste
pozitia si orientarea centrului de greutate al vehiculului, (xwi,ywi) defineste pozitia rotii i, v i
44

vwi sunt vitezele vehiculului si a fiecrei roti. reprezinta unghiul de alunecare a vehiculului,
lf, lr sunt distanele de la centrul de greutate la rotile din fata si din spate.
Lund in considerare alunecrile, constrngerile nonholonomice se pot exprima astfel:

xCG sin y CG cos 0 ;

(3.12)

x wi sin wi wi y wi cos wi wi 0 .

(3.13)

Fig. 3.2 Modelul cinematic al vehiculului autonom SEEKUR

Constrngerile geometrice dintre fiecare roat si centrul de greutate pot fi


scrise astfel:

x w1 X CG l f

x w 2 X CG l f

x w3 X CG l r

x w3 X CG l r

d
sin
2
d
cos sin
2
;
d
cos sin
2
d
cos sin
2
cos

45

(3.14)


y w1 y CG l f

y w 2 y CG l f

y w3 y CG l r

y w 4 y CG l r

d
cos
2
d
sin cos
2
.
d
sin cos
2
d
sin cos
2
sin

(3.15)

Considernd roile 1 si 2 si constrngerile obtinem n forma matriceal

A12 q0 0 ,

sin w1

A12 sin w 2

sin 0

sin( w1 )
2

d
) sin( w 2 ) ,
2

cos w1

l f cos( w1 )

cos w 2

l f cos( w 2

cos 0

(3.16)

x CG
q 0 y CG ,

(3.17)

(3.18)

wi wi wi ,

(3.19)

0 .

(3.20)

Considernd o micare planar, corp rigid i fr alunecarea roii, modelul


poate fi aproximat de modelul bicicletei(Fig. 3.3)

sin f cos f l f cos f x CG


sin cos l cos y 0
.
r
r
f
r CG

sin cos

0

(3.21)

Se poate obine vectorul 0 care satisface relaia

cos( )
xCG

y
sin(

v
CG

cos

tan

tan

f
r

l f lr

n care:

arctan

l f tan r l r tan f
l f lr

v - este viteza liniar a vehiculului.


46

(3.22)

Fig. 3.3 Modelul cinematic al bicicletei

Presupunem c unghiul roilor vehiculului este limitat de formula

f , r

(3.23)

Presupunem de asemenea ca vehiculul se deplaseaz utiliznd manevra cu alunecare


lateral zero(Zero-side-slip maneuver), propus de Danwei i Feng[15]. n acest caz unghiul
de alunecare este 0 pe ntreaga durat a deplasrii, iar orientarea vehiculului se potrivete
cu unghiul tangent la traiectoria dorit,

(t ) d (t ), t : 0 t f .

(3.24)

Unghiurile roilor, n acest caz, sunt = , situaie prezentat n Fig. 3.3.


Modelul cinematic al vehiculului (3.22) n cazul manevrei cu alunecare lateral zero
devine

xCG cos( )

y sin(

v .
CG

tan f tan r

2lf

Modelul (3.25) poate fi scris i n forma


47

(3.25)

xCG v cos

y CG v sin ,
v tan / L
f

(3.26)

unde
v- reprezint viteza liniar;
f - reprezint unghiul roii din fa;

( , ) -reprezint coordonatele carteziene ale centrului vehiculului;


-reprezint unghiul dintre direcia de mers i axa Ox;
L 2 l f - reprezint distana dintre roi.

3.2. Conducerea sliding-mode n timp continuu


Chiar dac conducerea sliding-mode a fost inventat n anii 60, folosirea ei n
exteriorul Rusiei a nceput abia dup anii 70. A evoluat ntr-o metod de elaborare a
comenzilor care poate fi aplicat unui numr foarte mare de sisteme i n special pentru
sistemele neliniare.
Conducerea sliding mode este o conducere bazat pe modul alunecare, iar o micare
de alunecare apare cnd starea sistemului trece prin comutaii multiple i este condus spre
starea dorit.
O lege de conducere este definit pentru a conduce traiectoria strii sistemului intr-o
stare prescris a suprafeei n spaiul strilor i s menin traiectoria strii sistemului pe
aceast traiectorie. Aceast suprafat se numete suprafaa de comutaie.
Controlerul sliding mode este un controler n bucl nchis discontinu, care conine
comutaii la frecvene mari. Avantajul major l reprezint robusteea la variaia parametrilor i
perturbaii. Principala problem a controlului sliding mode este apariia fenomenului de
chattering. Fenomenul de chattering poate cauza probleme i se ncearc reducerea lui.
Alt avantaj major al conducerii sliding-mode l reprezint faptul c sistemul se
comport ca un sistem de ordin redus n comparaie cu procesul i faptul c alunecarea pe
suprafeele de comutaie nu este afectat de incertitudini de model sau perturbaii.
Conducerea sliding-mode a fost analizat de cercettori ca Utkin n [101], [102], Gao
n [31], Khalil n [57]. Mai muli cercettori din [11], [12], [14], [17], [18], [49], [63], [65],
[91], [92], [93], [94], [95], [96] i [103] au ales conducerea sliding-mode pentru conducerea
roboilor mobili i vehiculelor autonome, caracterizate de sisteme nonholonomice. n [62] a
fost tratat problema reducerii efectului de chattering n conducerea sliding-mode.
48

Se consider sistemul nelinear affine


x (t ) f (t , x) g (t , x) u(t ) ,

(3.26)

n care: () , () , , ) i , . Semnalul de comand


(, ) are urmtoarea form
u (t , x), dac si 0
ui (t , x) i
, i 1,2,..., m .
ui (t , x), dac si 0

(3.27)

n care: (0) este suprafaa de alunecare cu numrul i i

s( x) s1 ( x) s 2 ( x) .... s m ( x) 0 ,
T

(3.28)

este colectorul de alunecare. Datorit faptului c , este discontinu pe suprafaa ,


aceast suprafa se numete suprafa de comutaie.
Definiie 1: Fie = | = 0 o suprafa de comutaie care include originea
= 0. Dac pentru orice 0 , exist () pentru orice > 0 , atunci () este un
sistem sliding-mode.
Definiie 2: Dac exist sliding-mode pe = | = 0 atunci suprafaa de
comutaie S se numete suprafa de alunecare.
Definiie 3: Existena conducerii sliding-mode necesit stabilitatea traiectoriilor
strilor spre suprafaa de alunecare = | = 0 cel puin n vecintatea lui S. Aceast
condiie de suficien pentru sliding-mode se numete condiia de atingere.
Cea mai mare vecintate a lui S pentru care condiia de atingere e satisfcut se
numete zona de atracie.
Definiie 4: Traiectoria strii condiionat de condiia de atingere se numete faza de
atingere.
Problema conducerii necesit calcularea funciilor continue +i , i suprafaa de
alunecare = 0 astfel nct sistemul n bucl nchis prezint sliding-mode pe suprafaa
de alunecare.
Proiectarea sliding-mode poate fi divizat n 2 faze. Prima faz const n proiectarea
suprafeei de alunecare care asigur evoluia dorit a sistemului. Faza a doua const n
proiectarea legii discontinue de conducere care foreaz sistemul pe traiectoria dorit.
Un sistem de ordinul n cu m intrri va avea 2 1 suprafee de comutaie:

m suprafee de dimensiunea (n-1)

49

Se consider intersecia a 2 suprafee , , . Intersecia lor este o suprafaa de


comutaie de dimensiune (n-2). Numarul total de astfel de intersecii este numrul de
combinaii de m suprafee luate cte 2 n acelai timp

m
m!
mm 1
.
2 m 2!2!
2

(3.29)

Suprafeele de comutaie sunt descrise de

S ij S i S j .

Intersecia a 3 suprafee , i este o suprafa de comutaie de dimensiune


(n-3). Exist

astfel de suprafee.
3

Exist o singur suprafa de dimensiune (n-m), care e intersecia suprafelor


, = 1, , luate mpreun adic
= | = 0 = 1 2 .

(3.30)

SI

SI SJ
SJ

Fig. 3.4 Interpretarea geometrica a suprefeelor de comutaie

Metoda lui Filippov este una din metodele posibile pentru determinarea evoluiei
sistemului n sliding-mode. O alt tehnic aplicabil sistemelor MIMO este conducerea
echivalent propus de Utkin, [102].
Suprafeele de alunecare pot fi att liniare ct i neliniare. Teoria proiectrii
suprafeelor de alunecare pentru sisteme cu dinamici liniare a fost cercetat amnunit, dar n
cazul sistemelor neliniare este nc o problem deschis.
50

Se consider sistemul

x A x B u .

(3.31)

Se presupune c exist o transformat nesingular Q astfel nct


0
QB ,
B2

(3.32)

n care 2 este o matrice m*m nesingular. Sistemul se transform n


x1 A11 x A12 x2

x 2 A21 x A22 x2 B2 u

(3.33)

n care:
1 ;
2 ;
Suprafaa de alunecare poate fi scris ca

s( x) 1 x1 2 x2 .

(3.34)

Se presupune c 2 e nesingular i n regimul sliding-mode avem

1 x1 2 x2 0 .

(3.35)

Dac 2 este dependent liniar de 1 i sistemul satisface


x1 A11 x A12 x2
,

x 2 K x1

(3.36)

n care: = 21 1 .
Se obine astfel un sistem de ordin (n-m), iar dinamica sistemului este

x1 ( A11 x A12 K 2 ) x1 .

(3.37)

Se observ c s-a obinut o simplificare a problemei de proiectare. Dup calcularea lui


K, suprafaa dorit poate fi proiectat astfel

s ( x) 2 [ K , I ] x .

(3.38)

Proiectarea suprafeelor variabile n timp pentru urmrirea traiectoriilor [90] se


realizeaz pentru un sistem cu o intrare, proiectnd suprafaa conform lrgimii de band
dorite
d

s( x, t )
dt

n 1

x 0,

n care:
x este eroarea de urmrire
- este o constant pozitiv care determin lungimea de band n bucl nchis.
51

(3.39)

Se vede c s depinde doar de eroarea x.


Dup calculul suprafeelor sliding-mode este necesar rezolvarea problemei de
aducere a sistemului la suprafaa dorit. Este necesar calcularea unei comenzi n bucl
nchis : , care s conduc starea x spre suprafa i s o menin acolo.
Legile de aducere sunt: metoda comutaiei directe, metoda funciei Lyapunov sau
legea de aducere a lui Gao[31].
Metoda comutaiei directe consider suficient satisfacerea condiiei
< 0, = 1, , .

(3.40)

Este o lege global dar nu garanteaz aducerea n timp finit.


Metoda Lyapunov presupune alegerea unei funcii candidat cum ar fi
1

, = 2 .

(3.41)

Legea de aducere este dat de


, < 0.

(3.42)

Sliding-mode este garantat doar pe intersecia tuturor suprafeelor.


Gao i Hung[31] au propus urmatoarea lege de conducere
s Q sgn( s) P h(s) ,

(3.43)

unde:
Q=diag[q1,q2,,qm]; qi>0;
sgn(s)=[sgn(s1), sgn(s2),,sgn(sm)] T;
P=diag[p1,p2,,pm]; pi>0;
h(s)=[h1(s1),h2(s2),,hm(sm)] T;
si*hi(si)>0;hi(0)=0;

1 dac si 0
sgn( si )
1 dac si 0 .
O form practic a acestei legi poate fi definit ca
s Q sgn( s) P s .

(3.44)

Legea de conducere se poate obine prin metoda legii de aducere, comanda se obine
calculnd derivata lui ():

= ( + = ();
=

+ +

52

( ) .

(3.45)
(3.46)

3.2.1 Conducerea sliding- mode n timp continuu a roboilor mobili cu 2 roi


motoare i 2 libere (WMR 2DW/2FW)
Arhitectura conducerii sliding-mode n timp continuu a WMR cu 2 DW/2FW este
prezentat n Fig. 3.5. Aceast arhitectur de conducere permite robotului s urmreasc o
traiectorie dorit cu un profil de vitez impus. Modulul Odometry din Fig. 3.5 primete datele
de la encoderele robotului i calculeaz poziia, orientarea, viteza liniar i viteza unghiular
a robotului. Acest modul este implementat n softul ARIA de la Mobile Robots i datele
calculate pot fi obinute apelnd n program funciile din ARIA.
Trajectory-tracking consider c robotul urmrete un WMR virtual care se
deplaseaz pe traiectoria dorit cu profilul de vitez impus. Considrnd c traiectoria dorit

qd (t ) xd

yd d este generat de un WMR virtual(Fig. 3.6).


T

Fig. 3.5 Arhitectura conducerii sliding-mode a WMR 2DW/2FW

Modelul cinematic al robotului virtual este

xd vd cos d

y d vd sin d ,

d
d

(3.47)

unde ( xd , y d ) reprezint coordonatele Carteziene ale centrului geometric, v d este viteza


liniar, d reprezint orientarea i d reprezint viteza unghiular.
Cnd robotul este condus pentru a urmri o anumit traiectorie se obin erori de
urmrire(Fig. 3.6). Erorile de urmrire[56] sunt

xe cos d
y sin
d
e
e 0

sin d
cos d
0

Dinamica erorilor de urmrire este

53

0 xr xd
0 yr yd .
1 r d

(3.48)

x e v d v r cos e d y e

.
y e v r sin e d xe

r
d
e

(3.49)

Se presupune c e 2 , astfel orientarea robotului nu este perpendicular pe


traiectoria dorit.
Considernd erorile de urmrire (3.48) i derivata lor (3.49) se definesc suprafeele de
comutaie:

s1 xe k1 xe

s2 y e k 2 ye k0 sgn( ye ) e

(3.50)

unde k1 , k2 , k3 0 sunt parametri constani pozitivi, xe , ye , e sunt erorile de urmrire din


(3.48).
Dac s1 converge la zero, atunci xe converge la zero. Dac s2 converge la 0, atunci

y e k2 ye k0 sgn( ye ) e . Dac ye 0 atunci y e 0 doar dac k 0 k2 ye / e . Se


observ c dac ye i y e converg la zero atunci e converge la zero.
Derivatele suprafeelor devin

s1 xe k1 xe

.
s2 ye k 2 y e k0 sgn( ye ) e

(3.51)

Legea de conducere este folosit n forma


s Q sgn( s) P s ,

Q 0
P
Q 1
, P 1

0 Q2
0

unde

(3.52)

0
, Q1 , Q2 , P1 , P2 0 ,
P2

T
sgn( s) sgn( s1 ) sgn( s2 ) , s s1

s2 .
T

Din (3.48), (3.49), (3.50), (3.51) i (3.52) se obine legea de comand sliding mode:

vc

Q1 sign( s1 ) P1 s1 k1 xe d ye d y e vr e sin e vd
; (3.53)
cos e

c
Fie V

Q2 sign( s2 ) P2 s2 k2 ye vr sin e d xe d xe
d .
vr cos e k0 sgn( ye )

(3.54)

1 T
s s o funcie Lyapunov. Derivata acestei funcii este
2

V s1 s1 s2 s2 s1 Q1 sgn( s1 ) P1 s1 s2 Q2 sgn( s2 ) P2 s2 .

54

(3.55)

Derivata funciei Lyapunov este negativ semi-definit dac alegem parametrii

Qi , Pi 0 .
y

Robot Real

r
ye
(xr,yr, r)
Robot Virtual
d

(xd ,yd ,d )
e
xe

Fig. 3.6 Erorile de urmrire n cazul roboilor mobili

3.2.2 Conducerea sliding- mode a vehiculului autonom cu 4 roi motoare i 4


directoare (4DW/SW)
Calculul comenzii se realizeaz pornind de la modelul cinematic simplificat al
vehiculului din ecuaia(3.26) i considernd un vehicul virtual care se deplaseaz pe
traiectoria dorit. Problema urmririi traiectoriei presupune proiectarea unui controler care s
calculeze viteza liniar i viteza unghiular care s permit vehiculului real s urmreasc
traiectoria vehiculului virtual cu erori de poziionare ct mai mici.
Arhitectura conducerii sliding-mode n timp continuu a AV 4DW/SW este prezentat
n Fig. 3.7. Modulul Odometry din Fig. 3.7 primete datele de la encoderele vehiculului i
calculeaz poziia, orientarea, viteza liniar a vehiculului i unghiul roilor vehiculului. Acest
modul este implementat n softul ARIA de la Mobile Robots i datele calculate pot fi obinute
apelnd n program funciile din ARIA.

55

Fig. 3.7 Arhitectura conducerii sliding-mode n timp continuu a AV 4DW/SW

Modelul cinematic al vehiculului virtual este

x d v d cos d

y d v d sin d ,

v d tan d
d
L

(3.56)

unde ( xd , y d ) reprezint coordonatele Carteziene ale centrului geometric, v d este viteza


liniar, d reprezint orientarea i d reprezint unghiul roilor.
Erorile de urmrire[56] sunt

xe cos d
y sin
d
e
e 0

sin d
cos d
0

0 xr xd
0 yr yd .
1 r d

(3.57)

Dinamica erorilor de urmrire este

vd

x e v d v r cos e L tan d y e

v d tan d
xe
.
y e v r sin e
L

v r tan r v d tan d
e
L
L

(3.58)

Erorile de urmrire ale vehiculului autonom electric SEEKUR, considernd modelul


cinematic simplificat, reprezentat de modelul cinematic al bicicletei sunt descrise n Fig. 3.8.
Considernd erorile de urmrire (3.57) i derivata lor (3.58) se definesc suprafeele de
comutaie:

s1 x e k1 xe ;

(3.59)

s2 y e k2 ye k0 sgn( ye ) e ;

(3.60)

56

fr
y
vr

ye

CG

f d

r
vd

Seekur

CG

d
Virtual vehicle

Path
e

Fig. 3.8 Erorile de urmrire n cazul vehiculului SEEKUR

Legea de conducere este folosit n forma

s Q sgn( s) P s .

(3.61)

Din (3.56), (3.57), (3.58), (3.60) i (3.61) se obine legea de comand sliding mode:

vc
c arctan(

Q1 sign( s1 ) P1 s1 k1 xe d ye d y e vr e sin e vd
; (3.62)
cos e

L (Q2 sign( s2 ) P2 s2 k2 y e vr sin e d xe d xe ) L


d ) . (3.63)
vr cos e k0 sgn( ye )
vr

Fie V

1 T
s s o funcie Lyapunov. Derivata acestei funcii este
2

V s1 s1 s2 s2 s1 Q1 sgn( s1 ) P1 s1 s2 Q2 sgn( s2 ) P2 s2 .

(3.64)

Derivata funciei Lyapunov este negativ semi-definit dac alegem parametrii

Qi , Pi 0 .

3.2 Conducerea sliding-mode n timp discret


Conducerea sliding-mode a fost aplicat cu succes la rezolvarea problemei conducerii
sistemelor nonholonomice. Conducerea sliding-mode n timp discret a fost aplicat mai rar n cazul
sistemelor nonholonomice dect conducerea sliding-mode pentru timp continuu i pentru c
algoritmul de conducere este implementat pe sisteme digitale se va folosi conducerea sliding-

mode pentru timp discret.


57

Conducerea sliding-mode n timp discret a fost propus de [16], [20], [21], [22] [32],
[49], [73], [81], [83], [88], [105], [106] i [108]. n cazul conducerii n timp discret accesul la
informaiile sistemului este restricionat la anumite perioade de timp, iar comenzile pot fi
trimise dor n aceste perioade. Se presupune c semnalele discrete sunt obinute cu metoda zero order
hold. Conducerea sliding-mode n timp discret presupune calculul unei suprafee de alunecare i a
unei legi de conducere care s menin traiectoria n apropierea suprafeei.
Se consider sistemul discret

x[k 1] Ax[k ] b u[k ] ,

(3.65)

unde x este un vector cu n component, u este un scalar, iar A i b au dimensiunile


corespunztoare.
Un controller cu structur variabil n timp discret a crui evoluie pornete de la orice
stare iniial , conduce traiectoria spre un plan de comutaie i ajunge la acest plan n timp
finit i dup ce sistemul a ajuns n planul de comutaie traiectoria descrie o micare n zigzag
n acest plan, se numete quasi-sliding mode.
O lege de aducere convenabil se definete astfel

s[k 1] s[k ] qTs s[k ] Ts sgn( s[k ]) ,

(3.63)

unde , q 0 sunt constant i 1 qTs 0 , Ts este perioada de eantionare.


Suprafaa de comutaie n cazul ideal este

cT Ax[k ] cT bu[k ] s[k ] 0 .

(3.64)

Comanda este dat de formula

u (cT b) 1 cT Ax[k ] .

(3.65)

Ecuaia dinamic pentru quasi-sliding-mode ideal este


x[k 1] [ I b(c T b) 1 c T ] Ax[k ]
.

c T x[k ] 0

(3.66)

Legea de conducere este

s[k 1] s[k ] qTs s[k ] Ts sgn( s[k ]) cT Ax[k ] cT bu[k ] cT x[k ] .

(3.67)

Comanda calculat este

u (cT b) 1[ cT Ax[k ] cT x[k ] qTs cT x[k ] Ts sgn( cT x[k ])] .

(4.46)

3.3.1 Conducerea sliding-mode n timp discret a WMR cu 2 DW/2FW

Controllerul primete de la modulul de planificare a traiectoriei viteza liniar i viteza


unghiular dorite, i erorile de poziionare de la modulul de calcul al erorilor i calculeaz
58

comanda pentru viteza liniar i viteza unghiular. Conducerea sliding-mode n timp discret
are structura din Fig. 3.9. Modulul Odometry din Fig. 3.9 primete datele de la encoderele
robotului i calculeaz poziia, orientarea, viteza liniar i viteza unghiular a robotului.
Acest modul este implementat n softul ARIA de la Mobile Robots i datele calculate pot fi
obinute apelnd n program funciile din ARIA.
Conducerea sliding-mode n timp discret a roboilor mobili cu 2DW/2FW este
realizat pornind de la ecuaiile cinematice ale centrului de greutate.
Considernd periaoada de eantionare Ts i metoda zero order hold sistemul neliniar
(3.10) poate fi scris n timp discret astfel:
x r [k 1] x r [k ] v r [k ] cos r [k ] Ts

y r [k 1] y r [k ] v r [k ] sin r [k ] Ts

r [k 1] r [k ] r [k ] Ts

(3.68)

Conducerea ii propune rezolvarea problemei urmririi traiectoriei. Rezolvarea acestei


probleme necesit proiectarea unui controller capabil de urmrirea traiectoriei dorite. n acest
scop se consider un robot virtual avnd traiectoria dorit qd t xd t yd t d t T i
urmtorul model cinematic
x d cos d 0
v
y sin
0 d ,
d
d

d 0 1 d

(3.69)

n care:
x d - reprezint poziia dorit pe axa Ox;
y d - reprezint poziia dorit pe axa Oy;

d - reprezint orientarea dorit;


v d - reprezint viteza liniar dorit;

d - reprezint viteza unghiular dorit.

Sistemul (3.69) poate fi scris ca

x d v d cos d

y d v d sin d .

d
d

(3.70)

Sistemul (3.70) poate fi scris n timp discret astfel

x d [k 1] x d [k ] v d [k ] cos d [k ] Ts

y d [k 1] y d [k ] v d [k ] sin d [k ] Ts .
[k 1] [k ] [k ] T
d
d
s
d
59

(3.71)

vc[k+1]

vd[k]
d [k]
Trajectory
Planner

vc[k]
c[k]

c[k+1]
Discrete-time
Sliding-mode
Controller

xd[k]

xe[k]

yd[k]
d [k]

ye[k]
e [k]
Error
Calculation

-1

v[k]
[k]

xCG[k]

Odometry

yCG[k]
CG [k]
Fig. 3.9 Arhitectura de conducere sliding-mode n timp discret a WMR cu 2DW/2FW

Erorile de urmrire din Fig. 3.6sunt

xe cos d
y sin
d
e
e 0

sin d
cos d
0

0 x r x d
0 y r y d .
1 r d

(3.72)

Aceste erori pot fi scrise ca

x e [k ] x r [k ] x d [k ] cos e [k ] y r [k ] y d [k ] sin e [k ]

y e [k ] x r [k ] x d [k ] sin e [k ] y r [k ] y d [k ] cos e [k ] .
[k ] [k ] [k ]
r
d
e

(3.73)

Dinamica erorilor poate fi exprimat astfel


x e v d v r cos e d y e

.
y e v r sin e d x e

r
d
e

(3.74)

n timp timp discret dinamica erorilor se exprim

x e [k 1] x e [k ] (v r [k ] cos e [k ] v d [k ] y e [k ] d [k ]) Ts

.
y e [k 1] y e [k ] (v r [k ] sin e [k ] x e [k ] d [k ]) Ts
[k 1] [k ] ( [k ] [k ]) T
e
r
d
s
e

(3.78)

Controlerul sliding-mode n timp discret este un controler cu structur variabil, care


efectueaz msurtori i calculeaz comenzile la intervale regulate de timp i pstreaz
semnalul de comand constant ntre intervale. O important proprietate a conducerii slidingmode n timp discret este controlul discontinuu.
Gao et al. [32] a propus urmtoarea condiie pentru existen sliding-mode
s[k ] (s[k 1] s[k ]) 0 .

Legea de conducere propus de Gao et al. [32] este:


60

(3.79)

s[k 1] (1 q Ts ) s[k ] Ts sgn( s[k ]) ;

(3.80)

0 1 q T 1;

(3.81)

0 Ts 1 ;

(3.82)

n care:

Ts 0 este perioada de eantionare;


0

este viteza de aducere;

q 0 este convergenta exponenial.

Traiectoria sliding-mode evolueaz n cvasi-banda sliding-mode


s[k ]

Ts
1 q Ts

(3.83)

Se definesc suprafeele de alunecare


s1 [k ] xe [k 1] k1 xe [k ]
,

s 2 [k ] y e [k 1] k 2 y e [k ] k 0 sign( y e [k ]) e [k ]

(3.84)

n care:

k0 , k1 , k 2 sunt constante pozitive xe , ye i e sunt erorile de urmrire.


Din (3.80) i (3.84) obinem:
s1 [k 1] xe [k 2] k1 xe [k 1] (1 q Ts ) s1 [k ] Ts sgn( s1 [k ])

s2 [k 1] ye [k 2] k2 ye [k 1] k0 sgn( ye [k ]) e [k ]
(1 q Ts ) s1[k ] Ts sgn( s1[k ])

(3.85)
(3.86)

Din (3.85) i (3.86) se obin comenzile pentru viteza liniar i viteza unghiular astfel:

v[k 1]

1
[(1 q1 Ts ) s1 [k ] 1 Ts sgn( s1 [k ] xe [k 1] (1 k1 )
cos e [k ] * Ts
;(3.87)

(v d [k 1] v r [k ] e [k 1] sin e [k ] wd [k 1] * y e [k ] wd [k ] y e [k 1]) Ts ]
1
[(1 q 2 Ts ) s 2 [k ] 2 Ts sgn( s 2 [k ]) e [k ]
v r [k ] cos e k 0 sgn y e [k 1]
. (3.88)
y e [k 1] (k 2 1) (v r [k 1] sin e [k ] wd [k 1] x e [k ] wd [k ] x e [k 1]) Ts )] wd [k ]

[k ]

3.3.2 Conducerea sliding-mode n timp discret a vehiculelor autonome


Conducerea sliding-mode n timp discret a roboilor mobili i vehiculelor autonome
este realizat pornind de la ecuaiile cinematice ale centrului de greutate. Arhitectura
conducerii sliding-mode n timp discret a vehiculului autonom cu 4 DW/SW este prezentat
n Fig. 3.10. Modulul Odometry din Fig. 3.10 primete datele de la encoderele robotului i
61

calculeaz poziia, orientarea, viteza liniar i viteza unghiular a robotului. Acest modul este
implementat n softul ARIA de la Mobile Robots i datele calculate pot fi obinute apelnd n
program funciile din ARIA.
Considernd periaoada de eantionare Ts i metoda zero order hold sistemul neliniar
(3.26) poate fi scris n timp discret astfel

x r [k 1] x r [k ] v r [k ] cos r [k ] Ts

y r [k 1] y r [k ] v r [k ] sin r [k ] Ts .

v r [k ]
tan r [k ] Ts
r [k 1] r [k ]
L

(3.88)

Fig. 3.10 Arhitectura conducerii sliding-mode n timp discret a vehiculului autonom

Conducerea ii propune rezolvarea problemei urmririi traiectoriei. Rezolvarea acestei


probleme necesit proiectarea unui controller capabil de urmrirea traiectoriei dorite. n acest
scop se consider un robot virtual avnd traiectoria dorit qd t xd t yd t d t T i
urmtorul model cinematic

x d v d cos d

y d v d sin d ,

vd
tan d
d
L

n care:
x d - reprezint poziia dorit pe axa Ox;
y d - reprezint poziia dorit pe axa Oy;

d - reprezint orientarea dorit;


v d - reprezint viteza liniar dorit;

d - reprezint unghiul roilor.


62

(3.89)

Sistemul (3.89) poate fi scris n timp discret astfel

x d [k 1] x d [k ] v d [k ] cos d [k ] Ts

y d [k 1] y d [k ] v d [k ] sin d [k ] Ts .

v d [k ]
tan d [k ] Ts
d [k 1] d [k ]
L

(3.90)

Erorile de urmrire din Fig. 3.6sunt

x e cos d
y sin
d
e
e 0

sin d
cos d
0

0 x r x d
0 y r y d .
1 r d

(3.91)

Aceste erori pot fi scrise ca

xe [k ] x r [k ] x d [k ] cos e [k ] y r [k ] y d [k ] sin e [k ]

y e [k ] x r [k ] x d [k ] sin e [k ] y r [k ] y d [k ] cos e [k ] .
[k ] [k ] [k ]
r
d
e

(3.92)

Dinamica erorilor poate fi exprimat astfel


x e v d v r cos e d y e

.
y e v r sin e d x e

r
d
e

(3.93)

n timp timp discret dinamica erorilor se exprim

v d [k ]

x
[
k

1
]

x
[
k
]

(
v
[
k
]

cos

[
k
]

v
[
k
]

y
[
k
]

tan d [k ]) Ts
e
e
r
e
d
e

v d [k ]
tan d [k ] T ) Ts
. (3.94)
y e [k 1] y e [k ] (v r [k ] sin e [k ] xe [k ]
L

[k 1] [k ] ( v r [k ] tan [k ] v d [k ] tan [k ]) T
e
r
d
s
e
L
L
Controlerul sliding-mode n timp discret este un controler cu structur variabil, care
efectueaz msurtori i calculeaz comenzile la intervale regulate de timp i pstreaz
semnalul de comand constant ntre intervale. O important proprietate a conducerii slidingmode n timp discret este controlul discontinuu.
Gao et al. [32] a propus urmtoarea condiie pentru existen sliding-mode
s[k ] (s[k 1] s[k ]) 0 .

(3.95)

Legea de conducere propus de Gao et al. [32] este:


s[k 1] (1 q Ts ) s[k ] Ts sgn( s[k ]) ;

(3.96)

0 1 q T 1;

(3.97)

0 Ts 1 ;

(3.98)

63

n care:

Ts 0 este perioada de eantionare;


0

este viteza de aducere;

q 0 este convergenta exponenial.

Traiectoria sliding-mode evolueaz n cvasi-banda sliding-mode


s[k ]

Ts
1 q Ts

(3.99)

Se definesc suprafeele de alunecare

s1 [k ] xe [k 1] k1 xe [k ]
,

s 2 [k ] y e [k 1] k 2 y e [k ] k 0 sign( y e [k ]) e [k ]
n care:

(3.100)

k0 , k1 , k 2 sunt constante pozitive xe , ye i e sunt erorile de urmrire.


Din (3.96) i (3.100) obinem:
s1 [k 1] xe [k 2] k1 xe [k 1] (1 q Ts ) s1 [k ] Ts sgn( s1 [k ]) ;
s 2 [k 1] y e [k 2] k 2 y e [k 1] k 0 sgn( y e [k ]) e [k ]
(1 q Ts ) s1 [k ] Ts sgn( s1 [k ])

(3.101)
.(3.102

Din (3.101) i (3.102) se obin comenzile pentru viteza liniar i viteza unghiular
astfel:

v[k 1]

1
[(1 q1 Ts ) s1[k ] 1 Ts sgn( s1[k ] xe [k 1] (1 k1 )
cos e [k ] Ts
;(3.103)

(vd [k 1] vr [k ] e [k 1] sin e [k ] wd [k 1] ye [k ] wd [k ] ye [k 1]) Ts ]

[k ] arctan(

L
[(1 q2 Ts ) s2 [k ] 2 Ts sgn( s2 [k ]) e [k ]
vr [k ] cos e k0 sgn ye [k 1]

L
ye [k 1] (k 2 1) (vr [k 1] sin e [k ] wd [k 1] xe [k ] wd [k ] xe [k 1]) Ts )] wd [k ])
vr

.(3.104)

3.4. Conducerea backstepping


Conducerea backstepping este una dintre cele mai importante tehnici de conducere a
proceselor neliniare i poate fi aplicat la numeroase aplicaii. Acest tip de conducere este
necesar n cazul sistemelor non-holonomice datorit faptului c nu se poate aplica condiia de
stabilitate a lui Brockett, [7].
Probelma conducerii backstepping a mai multor tipuri de roboi nonholonomici a fost
prezentat ntr-un numr mare de lucrri [9], [51], [52], [61], [72], [80], [89], [98] i [117].

64

Backstepping este o procedur recursiv care descompune o problem de proiectare a


ntregului sistem n probleme de proiectare pentru sisteme de ordin inferior. Proiectarea
backstepping are la baz integratorul backstepping, acesta fiind aplicat recursiv pentru
rezolvarea problemei sistemelor complexe, procedur descris de Chen, [9], i Khalil, [57].
Integratorul backstepping, metoda backstepping pentru sisteme i conducerea
backstepping a vehiculului autonom electric SEEKUR Robot Base sunt prezentate n acest
subcapitol.
Integratorul backstepping este un caz special, n proiectarea folosind metoda
backstepping. Se consider sistemul

f ( ) g ( )
,

n care:

(3.105)

+1 este starea, iar este comanda, funciile , : sunt

netede ntr-un domeniu care conine 0 = 0 i 0 = 0.

g( )

f( )

f(.)

Fig. 3.11 Schema bloc a integratorului backstepping rezultat din sistemului (3.105)

Se dorete proiectarea unui controler n bucl nchis care s stabilizeze originea:


= 0, = 0 . Presupunem c funciile f i g sunt cunoscute. Acest sistem poate fi privit ca
o conexiune n cascad a 2 componente( Fig. 3.11). Prima component a sistemului, =
+ (), are ca intrare , iar componenta = reprezint integratorul. Dac prima
component se poate stabiliza cu = (), 0 = 0, atunci originea componentei

f ( ) g ( ) ( )

(3.106)

este asimptotic stabil.


Se consider o funcie Lyapunov neted i pozitiv definit care satisface
inegalitatea
V
f g W , D ,

este pozitiv definit.


65

(3.107)

Prin adunarea i scderea lui n partea dreapt a primei ecuaii din sistemul
(3.105) se obine reprezentarea echivalent

f ( ) g g ( )
.

(3.108)

Aceast reprezentare este prezentat n Fig. 3.12.

g( )

-()
f(.)+g(.) (.)
Fig. 3.12 Schema bloc a integratorului backstepping rezultat din sistemul (3.108)

Se efectueaz schimbarea de variabil

z .

(3.109)

ntroducnd (3.109) n (3.108) se obine sistemul

f g g z
.

z u

g()

(3.110)

-
f(.)+g(.) (.)

Fig. 3.13 Schema bloc a integratotului backstepping rezultat din sistemul (3.110)

Se calculeaz folosind formula

f g .

Se consider v=u- sistemul se reduce la conexiunea n cascad

66

(3.112)

f g g z
.

(3.113)

Sistemul (3.113) este similar sistemului (3.105), cu excepia faptului c prima


component a noului sistem are o origine stabil asimptotic cnd intrarea este 0. Aceast
caracteristic este util n proiectarea lui pentru a stabiliza ntreg sistemul.
Alegem funcia Lyapunov
1

, = + 2 2 ,
=

+ +

(3.114)

+ . (3.115)

Alegnd
=

+ , > 0,

(3.116)

Rezult
2 .

(3.117)

Se observ c originea = 0, = 0 este asimptotic stabil.


Legea de comand se obine nlocuind ,
=

[ ].

(3.118)

Lema 1. Se consider sistemul (3.113). Fie o stare stabilizatoare a sistemului n


bucl nchis cu 0 = 0 i o funcie Lyapunov care satisface (3.117) i o funcie
pozitiv definit (). Atunci comanda dup stare n bucl nchis stabilizeaz originea lui
1

(3.105), cu + 2

ca funcie Lyapunov. Mai mult dac toate presupunerile

sunt valide i este nemrginit, originea va fi global asimptotic stabil.


Se consider un sistem mai general dect integratorul backstepping
= + ()
.
= , + (, )

(3.119)

i sunt funcii netede. Dac (, ) 0 n domeniul de interes atunci


transformata intrrii
=

1
(,)

, ,

(3.120)

reduce ,n (3.119), la forma integratorului = . Dac exist o comand


stabilizatoare i o funcie Lyapunov care satisface condiiile Lemei 1, atunci lema mpreun
cu (3.119) determin = 0 , astfel
=

1
,

{ +

67

, , (3.121)

pentru k>0 i
1

, = + 2

(3.122)

ca funcie Lyapunov pentru sistemul (3.119).


Aplicnd recursiv tehnica integratorului backstepping se pot stabiliza sisteme de
forma

= + ()1
1 = 1 , 1 + 1 (, 1 )2
2 = 2 , 1 , 2 + 1 (, 1 , 2 )3
,

= 1 , 1 , , 1 + 1 (, 1 , , 1 )1
= , 1 , , + 1 (, 1 , , )

(3.123)

n care: i 1 , , , funciile , 1 , , dispar la origine.


Procedura recursiva ncepe cu ecuaia
= + ()1 .

(3.124)

Se determin o funcie stabilizatoare 1 = (), cu 0 = 0, pentru (3.105),


construind o funcie Lyapunov pozitiv definit, nemrginit radial, () astfel nct cu
aceast lege de conducere, derivata n timp

(3.125)

unde este pozitiv definit.


Pasul 1. Se consider subsistemul
= + ()1
.
1 = 1 , 1 + 1 (, 1 )2

(3.126)

Se presupune c 1 este o comand virtual i legea de comand 1 = , 0 = 0


stabilizeaz subsistemul. Considerm sistemul (3.126) un caz special al sistemului (3.119) n
care:
= ;
= 1 ;
= 2 ;
= 1 ;
= 1 .
Comanda stabilizatoare i funcia Lyapunov se obine astfel
1

1 , 1 = [ + 1 1 1 1 ],
1

pentru 1 > 0

68

(3.127)

1 , 1 = + 2 1 1

(3.128)

= + ()1
1 = 1 , 1 + 1 (, 1 )2 ,
2 = 2 , 1 , 2 + 1 (, 1 , 2 )3

(3.129)

Pasul 2. Se consider sistemul

ca un caz special al sistemului (3.126), n care:

= ;
1
= 2 ;
= 3 ;
=

+ 1
;
1

0
;
1

= 2 ;
= 2 .
Comanda stabilizatoare i funcia Lyapunov se obine astfel
1

2 , 1 , 2 = [ 1 + 1 +
2

1
1

(1 + 1 2 ) 1 1 2 2 1 1 ], (3.130)
1

pentru 2 > 0
1

2 , 1 , 2 = 1 , 1 + 2 2 2

(3.131)

Procesul recursiv se repet pn la pasul k pentru obinerea comenzii stabilizatoare


general = (, 1 , 2 , , ) i funcia Lyapunov (, 1 , 2 , , ) .
3.4.1 Conducerea backstepping aWMR cu 2DW/2FW
Arhitectura de conducere backstepping a roboilor mobili cu 2 roi motoare i 2 libere
este prezentat n Fig. 3.14.
Pentru a calcula comanda n cazul roboilor mobili pentru problema urmririi globale
din [52]. considerm modelul cinematic al robotului (3.10), modelul cinematic al robotului
virtual (3.47) erorile de urmrire (3.48) i dinamica erorilor de urmrire(3.49).
Conducerea backstepping presupune calculul legilor de conducere ale vitezelor liniare
i unghiulare avnd forma:

v v( xe , ye ,e , vd , d , vd , d ) ;

(3.132)

( xe , ye ,e , vd , d , vd , d ) ;

(3.133)

69

astfel nct traiectoria n bucl nchis definit de de (4.24), (4.102),(4.103) s fie mrginit
i s convearg spre zero.
vc

vd
d

Trajectory
Planner

Backstepping
Controller

xd

xe

yd
d

ye
e

Error
Calculation

Odometry

xCG
yCG
CG

Fig. 3.14 Arhitectura de conducere backstepping a WMR cu 2DW/2FW

Integratorul backstepping se folosete pentru c y e nu este controlat direct n(3.49).


Funciile xe k1 d ye i e 0 sunt funcii stabilizatoare pentru y e n (3.49).
Folosim schimbarea de variabil

xe xe k1 d ye ,

(3.134)

unde k1 0 este o constant.


Utiliznd (3.134) n (3.49) se obine ecuaia
x e vd vr cos e d ye k1 d ye k1 d (d xe vr sin e ) .

(3.135)

Folosind schimbarea de variabil

u vr cos e d ye k1 d (d xe vr sin e ) .

(3.136)

Rezult comanda backstepping a roboilor mobili:

v u y e d k 2 xe ;
1

r ye vr 0 cos(s e )ds k3 e .

(3.137)
(3.138)

3.4.2 Conducerea backstepping a vehiculului autonom cu 4 DW/SW


Pentru a calcula comanda n cazul roboilor mobili pentru problema urmririi globale
din [52]. Considerm modelul cinematic al robotului (3.26), modelul cinematic al robotului
virtual (3.56) erorile de urmrire (3.57) i dinamica erorilor de urmrire(3.58).
Conducerea backstepping presupune calculul legilor de conducere ale vitezelor liniare
i unghiulare avnd forma:

v v( xe , ye ,e , vd , d , vd , d ) ;

(3.139)

( xe , ye , e , vd , d , vd , d ) ;

(3.140)

70

astfel nct traiectoria n bucl nchis definit de (3.58), (3.139) i (3.140) s fie mrginit i
s convearg spre zero.
Integratorul backstepping se folosete pentru c y e nu este controlat direct n(3.58).
Funciile xe k1

vd
tan d y e i e 0 sunt funcii stabilizatoare pentru y e n (3.58).
L

Folosim schimbarea de variabil


xe xe k1

unde k1 0 este o constant i notm

vd
tan d y e ,
L

(3.141)

vd
tan d cu d .
L

Utiliznd (3.141) n (3.58) se obine ecuaia


xe vd vr cos e d ye k1 d ye k1 d (d xe vr sin e ) .

(3.142)

Folosind schimbarea de variabil

u vr cos e d ye k1 d (d xe vr sin e ) .

(3.143)

Rezult comanda backstepping a roboilor mobili

v u y e d k 2 xe ;

a tan(

1
L
( r y e vr cos(s e )ds k 3 e )) .
0
vr

(3.144)
(3.145)

Fig. 3.15 Arhitectura de conducere backstepping a AV cu 4DW/SW

Arhitectura de conducera este prezentat n Fig. 3.15. Modulul Odometry din Fig.
3.15Fig. 3.10 primete datele de la encoderele robotului i calculeaz poziia, orientarea, viteza

liniar i viteza unghiular a robotului. Acest modul este implementat n softul ARIA de la
Mobile Robots i datele calculate pot fi obinute apelnd n program funciile din ARIA.

71

3.5 Concluzii
n acest capitol am determinat modelul cinematic al WMR cu 2DW/2FW i modelul
cinematic al vehiculului autonom electric SEEKUR (4DW/SW). Modelele cinematice descriu
micarea robotului sau a vehiculului i nu iau n calcul forele care acioneaz asupra lor i
sunt folosite la calculul comenzii pentru conducerea WMR i AV.
Pentru determinarea modelului cinematic al WMR am considerat variabilele
generalizate ale sistemului, rotirea roilor robotului fr alunecare i aplicat constrngerile
nonholomice specifice acestui caz. Modelul rezultat astfel are cinci variabile: 2 variabile
reprezint centrul geometric al WMR, o variabil reprezentnd unghiul de direcie al
robotului i dou variabile reprezentnd unghiurile fiecrei roi. Aceste variabile depind de
vitezele de rotire ale roilor. Am simplificat acest model pentru c am dorit doar calculul
coordonatelor carteziene ale centrului geometric i unghiul de direcie, nlocuind vitezele
celor dou roi cu viteza liniar i viteze unghiular a robotului.
Pentru a determina constngerile nonholonomice ale vehiculului autonom SEEKUR
am pornit de la cazul n care exist alunecare lateral. Astfel am obinut constrngeri
nonholonomice pentru centrul de greutate al vehiculului i pentru fiecare roat de la AV.
Am considerat doar roile 1 i 2 modelul cinematic al vehiculului se simplific la cazul
modelului cinematic al bicicletei. Am considerat doar micarea Zero-side-slip, am obinut
unghiul de alunecare 0 i am obinut modelul cinematic final folosit pentru conducerea
vehiculului autonom.
Am propus trei metode pentru rezolvarea problemei conducerii roboilor mobili i
vehiculelor autonome: conducerea sliding-mode n timp continuu, conducerea sliding-mode
n timp discret i conducerea backstepping.
Am prezentat cazul genaral pentru sinteza comenzii sliding-mode n timp continuu.
De la cazul general i folosind modelul cinematic al WMR cu 2 DW/2SW , modelul erorilor
de urmrire i dinamica erorilor de urmrire am calculat comenzile pentru viteza liniar i
viteza unghiular a WMR. n calcul am folosit suprafee de comutaie i legi de conducere
bazate pe modelul propus de Gao pentru timp continuu n [31].
Utiliznd cazul general i modelul cinematic simplificat al vehiculului autonom
SEEKUR, suprafee de comutaie, legi de conducere similare celor propuse de Gao pentru
timp continuu n [31], modelul erorilor de urmrire i dinamica erorilor de urmrire am
calculat comenzile pentru viteza liniar i unghiul de direcie.

72

Am sintetizat modul de calcul al comenzii utiliznd discrete-time sliding-mode n


cazul general.Utiliznd cazul general am trecut la calculul comenzilor pentru WMR i AV.
Pornind de la modelul cinematic al WMR n timp discret, erorile de urmrire n timp
discret, dinamica erorilor de urmrire n timp discret, suprafeele sliding pentru timp discret i
legea de conducere pentru timp discret am calculat comenzile pentru viteza liniar i viteza
unghiular a WMR.
Utiliznd modelul cinematic simplificat al vehiculului autonomon n timp discret,
erorile de urmrire n timp discret , dinamica erorilor de urmrire n timp discret, suprafeele
sliding pentru timp discret i legea de conducere pentru timp discret am calculat comenzile
pentru viteza liniar i unghiul de direcie a vehicului autonom.
Am prezentat cazul general al conducerii backstepping. n continuare am calculat
comanda pentru conducerea backstepping n cazul WMR utiliznd modelul cinematic al
robotului i dinamica erorilor de urmrire a robotului.
Am calculat comenzile pentru viteza liniar i unghiul de direcie al vehiculului
autonom SEEKUR utiliznd modelul cinematic i dinamica de urmrire a erorilor i
procedura de conducere backstepping.

73

Capitolul 4.
4.

Algoritmi de conducere a vehiculelor autonome i a roboilor


mobili, destinai evitrii obstacolelor
n acest capitol am propus o soluie pentru evitarea obstacolelor de ctre roboii

mobili i vehiculele autonome. Am consider c roboii mobili sunt echipai cu sonare, iar
vehiculele autonome sunt echipate cu senzori laser pentru depistarea obstacolelor i se propun
dou soluii n funcie de senzorii utilizai.

4.1 Algoritmi de evitare a obstacolelor implementai n conducerea


roboilor mobili
Am considerat situaia n care se dorete urmrirea unei traiectorii dorite cu abateri
minore n cazul apariiei unui obstacol i revenirea la traiectoria impus imediat dupa ce
obstacolul a fost depit. n literatur evitarea obstacolelor se face fr a fi impus o
traiectorie strict, se prefer folosirea unor metode pentru a ndrepta robotul spre destinaia
dorit. Astfel de algoritmi au fost prezentai n introducere.
Obstacolele sunt detectate cu ajutorul a 14 sonare dispuse frontal i numerotate de la 0
la 14. Sonarul cu numrul 7 nu a fost folosit.Sonarele sunt dispuse circular ca n Fig. 4.1.
Sonarele acoper o zon de 180 de grade permind detectarea oricrui obstacol care ar putea
bloca navigarea. Pentru detectarea obstacolelor se folosete bula de sensibilitate prezentat n
[97]. Aceast bul este proporional cu viteza de deplasare i permite o bun detecie a
obstacolelor periculoase. Bula de sensibilitate este prezentat n Fig. 4.2. Metoda const n
stabilirea unor coeficieni pentru fiecare sonar i calcularea graniei de siguran prin
multiplicarea coeficienilor cu viteza i timpul de eantionare. Bula de sensibilitate (

Boundry b1 b2 b14 ) se calculeaz utiliznd formula


bi ki v Ts ,

(4.1)

unde - bi- reprezint componenta bulei de sensibilitate corespunztoare sonarului i;


ki- reprezint coeficientul de siguran a bulei de sensibilitate;
v- reprezint viteza robotului;
Ts- reprezint constanta de eantionare.
Dup calcularea bulei de sensibilitate se verific dac un obstacol a ptruns n zona
sensibil. Un obstacol este detectat dac distana msurat de sonar este mai mic dect
valoarea componentei bulei de sensibilitate corespunztoare sonarului respectiv. Ct timp nu
74

sunt detectate obstacole n bula de sensibilitate traiectoria global calculat off-line este
impus modulului de conducere i robotul urmrete aceast traiectorie. Traiectoria este
calculat off-line cu ajutorul unui genrerator de traiectorii care returneaz viteza liniar,
viteza unghiular, acceleraia unghiular i acceleraia unghiular, necesare regulatoarelor din
capitolul 3 pentru a conduce robotul.

Fig. 4.1 Dispunerea sonarelor la PowerBot

Fig. 4.2 Bula de sensibilitate la PowerBot

75

Dac un obstacol a fost detectat n zona de sensibilitate modulului de conducere i este


impus o traiectorie local generat pentru evitarea obstacolului. n Fig. 4.3 este prezentat un
obstacol care este detectat de sonarul cu numrul 6.

Fig. 4.3 Exemplu de obstacol detectat

Traiectoria local urmrete deplasarea robotului la un punct aflat perpendiculara la


direcia de mers dus din punctul n care este detectat cel mai pe apropiat obstacol. Pentru
determinarea acestui punct se caut o zon liber suficient de mare pe perpendiculara la
direcia de mers dus din punctul n care este detectat cel mai pe apropiat obstacol pentru a
permite trecerea robotului. Dac distana minim este msurat de sonarele aflate n partea
stng(0-6) atunci punctul dorit se caut nti n partea dreapt i dac nu se gsete un astfel
de punct se caut n partea stng. Dac distana minim este msurat de sonarele aflate n
partea dreapt(8-14) atunci punctul dorit se caut nti n partea stng i dac nu se gsete
un astfel de punct se caut n partea dreapt.
Algorimul este urmtorul: se determin sonarul care a depistat un obstacol n zona de
sensibilitate i se stie c sonarele cu numrul mai mic dect 7 sunt n partea stng, iar cele
76

mai mari dect 7 sunt n partea dreapt. n funcie de partea pe care se afl obstacolul se
ncearc cutarea n partea opus a unei zone libere suficient de mari pentru a permite
trecerea robotului. n Fig. 4.3 se observ c cel mai uor se poate evita obstacolul cu o ocolire
la dreapta.
n Fig. 4.4 se observ c traiectoria dorit(linia neagr) este blocat i se dorete gsirea
unui punct cu proprietatea c robotul poate ocoli obstacolul dac centrul su de greutate se
deplaseaz spre acel punct. Este cunoscut limea robotului care este considerat distana
minim(hmin) pentru ca robotul s poat trece i ncepnnd cu sonarul 8, i incrementnd
indicele sonarului ales se verific, care este primul sonar care nu detecteaz un obstacol
apropiat n cazul ocolirii prin partea dreapt. n cazul ocolirii prin partea stng se pornete
de la sonarul 6 i se scade indicele sonarului pn se gsete primul sonar care nu depisteaz
un obstacol. Se consider zona de la intersecia direciei de naintare cu perpendiculara dus
din punctul n care s-a detectat cel mai apropiat obstacol pn la punctul determinat de
aceast perpendicular i unghiul sonarului ales o zon de excludere.
Se consider c un obstacol nu depisteaz un obstacol dac:
range[k ] cos(alfa[k ]) min_ range c

(4.2)

Unde range[k ] -reprezint distana msurat de sonarul k,


alfa[k ] -reprezint unghiul sonarului k

min_range- reprezint proiecia distanei pn la cel mai apropiat obstacol pe direcia


de naintare,
c- reprezint o constanta pentru validarea absenei obstacolelor
Se calculeaz proiecia distanei pn la cel mai apropiat obstacol(min_range) i pe
baza unghiului sonarului selectat se calculeaz zona de excludere:

Ze min_ range tan(angle)

(4.3)

Urmeaz determinarea zonei n care poate naviga robotul astfel: se crete(sau scade n
funcie de direcia de cutare) numrul sonarului analizat i se verific dac zona delimitat
de sonarul ales, min_range i sonarul curent este suficient de mare pentru a trece robotul. n
caz afirmativ se alege destinaia punctul aflat la jumtatea distanei. Dac nu se gsete un loc
de ocolire se ncearc schimbarea direciei de ocolire.
Zona delimitat pe perpendiculara la direcia de mers aleas are proprietatea

h min_ range tan(alfa ( j )) Ze h min ,

(4.4)

unde min_range- reprezint proiecia pe direcia de naintare a distanei pn la obstacol,


alfa(j)- reprezint unghiul sonarului j, Ze reprezint zona de excludere(4.3).
77

Fig. 4.4 Traiectoria dorita pentru evitarea obstacolului

Dup determinarea zonei h prin care este posibil evitarea obstacolului se determin
punctul(new_x,new_y) n care se dorete deplasarea centrului de greutate al robotului
new _ x x min_ range cosTh H sin(Th)

,
new _ y y min_ range cosTh H sin(Th)

(4.5)

unde: (x,y) reprezint coordonatele curente ale robotului;


H- reprezint distana de la direcia de naintare la mijlocului segmentului delimitat de h;
Th- reprezint orientarea robotului.
Algoritmul pentru selectarea direciei la dreapta este urmtorul:
j=8
Dac range[ j ] cosalfa[ j ] min_ range 0.1 || range[ j ] max_ range
Atunci
78

Ze min_ range tan(alfa[ j ])


altfel
Ct timp range[ j ] cosalfa[ j ] min_ range 0.1 & & j 14

j j 1;
Ze min_ range tan(alfa[ j ])
h0

Ct timp (h h min) & &( j 14) & &(range[ j ] min_ range)


{ j j 1;

h min_ range * tan(alfa[ j ]) Ze }


Dac

h h min

h
Ze
2

new _ x x min_ range cosTh H sin(Th)


new _ y y min_ range cosTh H sin(Th)
Gasit=1
Altfel Gasit=0
Dup selecia noii destinaii trebuie calculat o nou traiectorie care s permit
deplasarea robotului de la punctul curent la punctul (new_x,new_y). Traiectoria este generat
folosind Quintic equations din[96] pentru a obine traseul dorit apoi se obine traiectoria
dorit calculnd profilul vitezelor pentru c robotul poate urmri noua traiectorie utiliznd
unul din regulatoarele prezentate n capitolul 3. Se consider punctul curent(xc,yc) i unghiul
curent(c) i se dorete calcularea punctelor intermediare care le unesc de punctul calculat i

unghi _ final unghi _ curent pi / 4 reprezentnd orientarea final a robotului, se


determin punctele intermediare care descriu traseul pentru evitarea obstacolului(

P(u) p1 (u), p2 (u),, pn (u) , u 0,1 ). Fiecare punct pi este compus din coordonatele xi,yi
i i.
O curb bazat pe quintic equations se calculeaz astfel[96]:
xi ,i 1 (u ) i 0 i1 u i 2 u 2 i 3 u 3 i 4 u 4 i 5 u 5


pi ,i 1 (u ) yi ,i 1 (u ) i 0 i1 u i 2 u 2 i 3 u 3 i 4 u 4 i 5 u 5 ,

i ,i 1 (u )
i (u )

79

(4.6)

unde:

i 0 xi (0) ;
g1 cos(i (0)) ;
i1

1
g 3 cos( i (0)) g12 ki sin( i (0)) ;
2
3
1

i 3 10 xi1 xi 6 g1 g 3 cos( (0)) 4 g 2 g 4 cos( i 1 (0))


2
2

3
1
g12 ki sin( i (0)) g 22 ki 1 sin( i 1 (0))
2
2
;

i2

3
2

i 4 15 xi1 xi 8 g1 g 3 cos( (0)) 7 g 2 g 4 cos( i 1 (0))


3
g12 ki sin( i (0)) g 22 ki 1 sin( i 1 (0))
2

1
2

1
2

i 5 6 xi1 xi 3 g1 g 3 cos( (0)) 3 g 2 g 4 cos( i1 (0))

1 2
1
g1 ki sin( i (0)) g 22 ki 1 sin( i 1 (0))
2
2

i 0 yi (0) ;
i 2

1
g 3 sin( i (0)) g12 ki cos( i (0)) ;
2

3
2

i 3 10 yi 1 yi 6 g1 g 3 sin( (0)) 4 g 2

1
g 4 sin( i 1 (0))
2

3 2
1
g1 ki cos( i (0)) g 22 ki 1 cos( i 1 (0))
2
2

3
2

i 4 15 yi 1 yi 8 g1 g 3 sin( (0)) 7 g 2 g 4 sin( i1 (0))


3
g12 ki cos( i (0)) g 22 ki 1 cos( i 1 (0))
2

1
2

i 5 6 yi 1 yi 3 g1 g 3 sin( (0)) 3 g 2

1
g 4 sin( i 1 (0))
2

1 2
1
g1 ki cos( i (0)) g 22 ki 1 cos( i 1 (0))
2
2

parametrii ki , ki 1 reprezint curbatura scalar i pot primi valori aleatoare, n acest caz se
consider valoarea 0.Parametrii q1 , q2 , g3 , g 4 influeneaz forma curbei. n acest caz se
consider valorile: g1 g 2 ( xi1 xi ) 2 ( yi1 yi ) 2 , g3 g 4 0 .
80

Controllerul sliding-mode primete la intrare viteza, acceleraia , viteza unghiular i


acceleraia unghiular, iar utiliznd Quintic equations am obinut punctele noului traseu i nu
traiectoria reprezentat de profilul de vitez. Pentru a utiliza acelai regulator trebuie
calculate vitezele, acceleraiile, vitezele unghiulare i acceleraiile unghiulare care permit
robotului s urmreasc traseul generat mai sus..

Fig. 4.5 Schema bloc a algoritmului de conducere cu evitare de obstacole

Pentru fiecare punct al traseului calculm lungimea segmentului pn la punctul


precedent. Obinem viteza mprind aceast lungime la durata de timp n care se dorete
parcurgerea segmentului i acceleraia mprind viteza la durata de timp.
Viteza unghiular se obine astfel

[n] a tan 2( y[n] y[n 1], x[n] x[n 1]) / Ts .


n final obinem acceleraia unghiular mprind viteza unghiular la durata de timp.
Algoritmul pentru calculul acestor parametri este:
De la 1 la 100 repet {
81

l[n]

x[n] x[n 1]2 ( y[n] y[n 1]) 2

Calculul lungimei segmentului

v[n] l[n] / Ts Calculul vitezei


a[n] v[n] / Ts Calculul acceleraiei

[n] a tan 2( y[n] y[n 1], x[n] x[n 1]) / Ts Calculul vitezei unghiulare
a[n] [n] / Ts

Calculul acceleraiei

}.
Se parcurge traiectoria nou generat prin impunerea acestei traiectorii la modulul de
conducere. Se continu deplasarea cu vitez constant n ultima direcie pn cnd obstacolul
este evitat. Se estimeaz poziiile n care s-ar fi aflat robotul dac urma traiectoria iniial i
apoi se calculeaz traiectoria de revenire la traiectoria iniial n locul n cel mai apropiat de
poziia curent a robotului i poziiile estimate utiliznd acelai algoritm folosit la
determinarea traiectoriei locale, i se impune aceast traiectorie modulului de conducere, apoi
se revine la traiectoria impus ncepnd cu punctul selectat anterior. Fig. 4.5 prezint schema
bloc a algoritmului prezentat pentru evitarea de obstacole.

4.2. Algoritmi de evitare a obstacolelor implementai n conducerea


vehiculelor autonome
Se consider un mediu cunoscut n care se dorete urmrirea unei traiectorii impuse
de ctre un vehicul autonom. n acest mediu este posibil apariia unor obstacole neateptate
care trebuie evitate. Se dorete evitarea acestor obstacole abandonnd traiectoria impus doar
ct timp este blocat de un obstacol i revenirea la traiectoria impus dup ce obstacolul a
fost evitat.
Obstacolele sunt detectate cu ajutorul unui laser SICK LMS111. Acest laser permite
detectarea obstacolelor aflate ntr-un arc de la

90

la

90

de centrul robotului. Laserul este

programat sa ofere date cu o rezoluie de 1 , rezultnd un numr total de 181 distane


msurate. Pentru a reduce volumul de date procesate programul folosete o rezoluie de

5 ,

obinut din media aritmetic a 5 distane consecutive. Aceast aproximare garanteaz c nici
un obstacol nu va trece neobservat i c influena unor date eronate este limitat. n Fig. 4.6
este prezentat zona de detecie a laserului.
Micornd rezoluia de la 1 grad la 5 grade obinem 37 distane. La aceste 37 distane
se poate defini o bul de sensibilitate[97],care acoper 180 de grade i permite evitarea
oricrui obstacol. Bula de sensibilitate definete o zon de siguran proporional cu viteza

82

vehicului, n care vehiculul se poate deplasa n siguran. Calculul bulei de siguran se


realizeaz multiplicand coeficienii stabilii pentru fiecare citire cu viteza vehiculului i
perioada de eantionare(4.1).
Procedura de evitare a obstacolelor ncepe dac este detectat un obstacol n interiorul
bulei de sensibilitate. n lipsa obstacolelor conducerea se realizeaz utiliznd conducerea
sliding-mode sau backstepping i traiectoria global generat off-line.
Dac un obstacol este detectat se calculeaz o traiectorie local de ocolire care este
impus modulului de conducere pn cnd obstacolul este evitat. Traiectoria este generat
folosind Quintic equations din [96] pentru a obine traseul dorit apoi se obine traiectoria
dorit calculnd profilul vitezelor pentru c robotul poate urmri noua traiectorie utiliznd
unul din regulatoarele prezentate n capitolul 3. n Fig. 4.7 este prezentat traiectoria dorit
pentru evitarea unui obstacol.
Traiectoria local urmrete deplasarea robotului la un punct aflat perpendiculara la
direcia de mers dus din punctul n care este detectat cel mai pe apropiat obstacol. Pentru
determinarea acestui punct se caut o zon liber suficient de mare pe perpendiculara la
direcia de mers dus din punctul n care este detectat cel mai pe apropiat obstacol pentru a
permite trecerea robotului.

Fig. 4.6 Zona de detecie a laserului

Dac distana minim este se afl n partea stng(i>17) atunci punctul dorit se caut
nti n partea dreapt i dac nu se gsete un astfel de punct se caut n partea stng. Dac

83

distana minim este msurat se afl n partea dreapt(i<17) atunci punctul dorit se caut
nti n partea stng i dac nu se gsete un astfel de punct se caut n partea dreapt.
Algorimul este urmtorul: se determin indexul la care s-a depistat un obstacol n
zona de sensibilitate i se stie c indexurile mai mari dect 17 sunt n partea stng, iar cele
mai mici dect 17 sunt n partea dreapt. n funcie de partea pe care se afl obstacolul se
ncearc cutarea n partea opus a unei zone libere suficient de mari pentru a permite
trecerea robotului.
Pentru a evita obstacolul trebuie gsit o zon fr obstacole suficient de mare pentru
a permite trecerea vehiculului. Se definesc 2 zone de evitare, aflate la stnga i dreapta
direciei de deplasare. Citirile de la 0 la 17 se repartitez n zona din dreapta, iar citirile de la
19 la 36 n zona din partea stng. Se determin la care citire s-a detectat cel mai apropiat
obstacol n zona de sensibilitate. Dac obstacolul se afl n partea stng se ncearc ocolirea
prin partea dreapt, dac nu se obinte o soluie se caut o soluie i n partea stng. Dac
obstacolul se afl n partea dreapt se ncearc ocolirea prin partea stng, dac nu se obinte
o soluie se caut o soluie i n partea dreapt.

Evitarea prin ocolire la dreapta este

prezentat n Fig. 4.7. Se observ c traiectoria dorit(linia roie) este blocat i se calculeaz
traiectoria de ocolire(linia albastr). Punctul spre care se deplaseaz vehiculul pentru evitarea
traiectoriei se calculeaz n funcie de limea robotului plus o marj de siguran.

Fig. 4.7 Traiectoria dorit pentru evitarea obstacolului n cazul utilizrii deteciei cu ajutorul senzorilor laser

84

Se determin primul index a crui distan msurat este mai mare dect distana
minim. Se calculeaz proiecia distanei pn la cel mai apropiat obstacol(min_range) i pe
baza unghiului selectat se calculeaz zona de excludere folosind ecuaia (4.4).
Condiia pentru selectarea primului index este
range[k ] cos(alfa[k ]) min_ range c ,

(4.7)

Unde range[k ] este distana masurat la indexul k, alfa[k ] unghiul la care se msoar
distana.

Urmeaz determinarea zonei n care poate naviga vehiculul astfel: se crete(sau scade
n funcie de direcia de cutare) indexul citirilor analizate i se verific dac zona delimitat
de unghiul ales, min_range i unghiul curent este suficient de mare pentru a trece vehiculul
aplicnd condiia (4.4) n caz afirmativ se alege ca destinaie punctul aflat la jumtatea
distanei. Dac nu se gsete un loc de ocolire se ncearc schimbarea direciei de ocolire.
Coordonatele acestui punct sunt date de (4.5).
Algoritmul pentru selectarea direciei la stnga este urmtorul:
j=18
Dac

range[ j ] * cos(alfa[ j ]) min_ range 0.1 || range[ j ] max_ range


Atunci

Ze min_ range tan(alfa[ j ])


altfel
Ct timp range[ j ] * cos(alfa[ j ]) min_ range 0.1 & & j 36

j j 1;
Ze min_ range tan(alfa[ j ])
h0

Ct timp (h h min) & &( j 36) & &(range[ j ] min_ range)


{ j j 1;

h min_ range * tan(alfa[ j ]) Ze }


Dac

h h min

h
Ze
2

new _ x x min_ range cosTh H sin(Th)

85

new _ y y min_ range cosTh H sin(Th)


Gasit=1
Altfel Gasit=0

Dup selecia noii destinaii trebuie calculat o nou traiectorie care s duc robotul
de la punctul curent la punctul (new_x,new_y). Traiectoria va fi generat folosind Quintic
equations (5.6). Se consider punctul curent i unghiul curent i se dorete calcularea
punctelor

intermediare

care

le

unesc

de

punctul

calculat

unghi _ final unghi _ curent pi / 4 .


Controllerul sliding-mode primete la intrare viteza, acceleraia , viteza unghiular i
acceleraia unghiular. Avnd punctele traseului calculm vitezele, acceleraiile, vitezele
unghiulare i acceleraiile unghiulare care descriu traiectorie necesar modulului de
conducere.
Pentru fiecare punct al traseului calculm lungimea segmentului pn la punctul
precedent. Obinem viteza mprind aceast lungime la durata de timp n care se dorete
parcurgerea segmentului i acceleraia mprind viteza la durata de timp.
Viteza unghiular se obine astfel

[n] a tan 2( y[n] y[n 1], x[n] x[n 1]) / Ts .


n final obinem acceleraia unghiular mprind viteza unghiular la durata de timp.
Algoritmul pentru calculul acestor parametri este:
De la 1 la 100 repet {
l[n]

x[n] x[n 1]2 ( y[n] y[n 1]) 2

Calculul lungimei segmentului

v[n] l[n] / Ts Calculul vitezei


a[n] v[n] / Ts Calculul acceleraiei

[n] a tan 2( y[n] y[n 1], x[n] x[n 1]) / Ts Calculul vitezei unghiulare
a[n] [n] / Ts

Calculul acceleraiei

}.
Se parcurge traiectoria nou generat prin impunerea acestei traiectorii la modulul de
conducere. Se continu deplasarea cu vitez constant n ultima direcie pn cnd obstacolul
este evitat. Se estimeaz poziiile n care s-ar fi aflat robotul dac urma traiectoria iniial i
86

apoi se calculeaz traiectoria de revenire la traiectoria iniial n locul n cel mai apropiat de
poziia curent a robotului i poziiile estimate utiliznd acelai algoritm folosit la
determinarea traiectoriei locale, i se impune aceast traiectorie modulului de conducere, apoi
se revine la traiectoria impus ncepnd cu punctul selectat anterior. Fig. 4.5 prezint schema
bloc a algoritmului prezentat pentru evitarea de obstacole.

4.3. Concluzii
Am propus o soluie pentru problema evitrii obstacolelor n cazul n care se dorete

urmrirea unei traiectorii impuse i abaterea de la aceast traiectorie doar pentru a evita un
obstacol i ntoarcerea la traiectoria impus. Am fost considerat cazul n care robotul este
echipat cu sonare i cazul n care vehiculul este echipat cu senzori laser.
Detecia obstacolelor folosete o bul de sensibilitate pentru a determina dac un
obstacol trebuie evitat. Am folosit aceai metod de conducere n cazul n care nu exist
obstacole i n cazul n care un obstacol este prezent. Ct timp nu este detectat nici un
obstacol care blocheaz traiectoria dorit platforma urmrete traiectoria impus folosind
metoda de conducere aleas din cele 3 prezentate n capitolul anterior. Traiectoria este
compus din vitezele liniare, vitezele unghiulare, acceleratiile liniare i acceleraiile
unghiulare.
Dac un obstacol a fost detectat se caut coordonatele care s permit conducerea n
siguran a platformei i se genereaz o traiectorie bazat pe Quintic equations. Aceast
traiectorie este impus modulului de conducere. Se continu deplasarea pe aceast traiectorie
pn cnd obstacolul a fost evitat. Dup ce obstacolul a fost evitat se calculeaz traiectoria
pentru a reveni la traiectoria impus i modul de conducere urmrete aceast traiectorie apoi
se revine la traiectoria impus iniial.

87

Capitolul 5.
5.

Implementarea conducerii n timp real. Rezultate


experimentale
Activitatea de cercetare s-a desfurat n Laboratorul de Robotic al Facultii de

Automatic, Calculatoare, Inginerie Electric i Electronic din Universitatea Dunrea de


Jos din Galai. Cercetarea s-a realizat pe roboii mobili Pioneer P3-DX, PatrolBot, PowerBot
i vehiculul autonom Seekur de la Adept Mobile Robots.

Fig. 5.1 Robotul mobil PatrolBot i robotul mobil Powerbot

Fig. 5.2 Vehiculul autonom SEEKUR

Roboii mobili Pioneer 3-DX, PatrolBot i PowerBot sunt roboi condui


diferenial(2DW/2FW) i echipai cu sonare, n timp ce Seekur este un vehicul cu traciune
integral i cu 4 roi directoare(4DW/SW).

88

Robotul mobil Pioneer este echipat cu un bra articulat cu 7 grade de libertate i un


gripper. Braul robotului nu este prevzut cu senzori, iar comanda braului poate fi realizat
doar n bucl deschis.
Fiecare platform robotic vine echipat cu motoare i controlere, toate controlate de
un microcontroller ncorporat care acioneaz ca un server i software client pentru roboii
mobili.

Dezvoltarea

de

software

include

Advanced

Robotics

Interface

for

Applications(ARIA) i ArNetworking, dezvoltate sub licen public GNU, i complet cu


librrii complet documentate pentru C++, Java i Python i cod surs.
Roboii sunt prevzui cu o conexiune serial RS232 pentru comunicaia cu exteriorul,
iar conectarea la calculator se realizeaz utiliznd o conexiune wireless, utiliznd un access
point wireless i un universal device server pentru conversia de la protocolul RS232 la
protocolul Ethernet.
Programul este scris n C++ i rulat pe un PC cu o frecven de eantionare de 100
ms. n continuare sunt prezentate simulri i implementri n timp real pentru a valida
metodele propuse. Pentru a trimite comenzile calculate de algoritmii de conducere am folosit
funciile

din

ARIA:

setVel(Velocity),

setVel2(leftVelocity,RightVelocity),

setRotVel(Angular Velocity). Am citit datele de la robot cu ajutorul funciilor ARIA: getX(),


getY(), getTH(), getRotVel(), getVel().

5.1. Simularea conducerii roboilor mobili i vehiculelor autonome


Controlerul propus pentru conducerea roboilor mobili i vehiculelor
autonome a fost testat n simulri realizate cu ajutorul softului MobileSim[39] de la Mobile
Robots. MobileSim este un soft pentru simularea platformelor MobileRobots/ActivMedia i
mediile lor i experimentare cu ARIA. nlocuiete SRIsim distribuit anterior cu ARIA.
MobileSim are la baz simulatorul Stage, creat de Richard Vaughan, Andrew
Howard, i alii ca parte din proiectul Player/Stage, cu nite modificri fcute de
MobileRobots. MobileSim poate simula comportamentul tuturor roboilor produi de
MobileRobots. Pentru simularea evitrii obstacolelor a fost creat o hart n Mapper3basic n
care a fost introdus obstacolul i aceast hart a fost ncrcat n MobileSim. Programele sunt
scrise n C++ i compilate n Visual Studio. Comunicarea cu simulatorul se realizeaz cu
ajutorul funciilor ARIA.

89

Softul Aria realizeaz conectarea automat la simulatorul MobileSim n cazul n care


nu este detectat nici un robot conectat la portul COM1. Simulatorul are implementate
modelele cinematice ale roboilor, funcii pentru simularea sonarelor i laserelor, care sunt
folosite pentru a simula comportamentul unui robot real. Programul scris n C++ apeleaz
funciile ARIA n cazul n care se dorete trimiterea unor comenzi ctre simulator sau citirea
datelor simulate.

Simularea 5.1

n acest test s-a dorit urmrirea unei traiectorii liniare cu viteza de 0.5m/s, urmat de o
traiectorie circular, cu viteza unghiulara 0.2 radiani/s, de ctre vehiculul autonom 4DW/SW
SEEKUR utiliznd conducerea sliding-mode n timp continuu. Parametrii constani folosii n
acest experiment sunt: Q1 0.05 , Q2 0.5 , P1 0.5 , P2 0.75 , k0 30 , k1 1.25 ,

k2 100 . Aceti parametri au fost obinui prin identificare n urma unor simulri succesive
utiliznd diferite valori ale parametrilor. Arhitectura de conducere utiliznd MobileSim este
prezentat n Fig. 5.3. Modulul Trajectory Planner const n acest caz n impunerea unei
traiectorii cu viteza liniar de 0.5m / s i d 0 timp de 20 s, dup 20 s se impune valoarea

d a tan0.2 0.8 / 0.5 , n care 0.2 radiani/s reprezint viteza unghiular, 0.8 lungimea
vehiculului, 0.5 viteza liniar. Modulul Odometry este implementat de softul ARIA i viteza
liniar, unghiul director, poziia i orientarea vehiculului sunt obinute utiliznd funciile
ARIA corespunztoare. Programul folosit pentru implementarea conducerii sliding-mode n
timp continuu este prezentat n Anexa 2. Programul folosit la conducerea sliding-

mode Comunicarea dintre programul de conducere i simulator este realizat de ARIA.

Fig. 5.3. Arhitectura de conducere a AV 4DW/SW SEEKUR n mediul MobileSIM

n continuare sunt prezentate graficele rezultate. n Fig. 5.4 este prezentat simularea n
MobileSim a traiectoriei vehiculului autonom SEEKUR utiliznd conducerea sliding-mode n
90

timp continuu. n Fig. 5.5 este prezentat traiectoria real cu o linie continu roie i
traiectoria dorit cu o linie ntrerupt albastr rezultat n urma simulrii conducerii slidingmode n timp continuu. Fig. 5.6 prezint eroarea pe axa Ox obinut prin simularea
conducerii sliding-mode n timp continuu.
Fig. 5.7 prezint eroarea pe axa Oy generat de simularea conducerii sliding-mode n
timp continuu. Fig. 5.8 prezint eroarea de orientare a robotului n cazul simulrii n
conducerii sliding-mode n timp continuu a vehiculului autonom SEEKUR. n Fig. 5.9 este
prezentat suprafaa de comutaie s1 i n Fig. 5.10 este prezentat suprafaa de comutaie s 2
calculat n timpul simulrii conducerii sliding-mode n timp continuu a vehiculului autonom
SEEKUR.

Fig. 5.4 Traiectoria obinut prin simulare n MobileSim la conducerea sliding-mode n timp continuu a
vehiculului autonom 4DW/SW SEEKUR.

6
Real trajectory
Desired trajectory
5

y[m]

10

12

14

x[m]

Fig. 5.5 Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea sliding-mode n
timp continuu a vehiculului autonom 4DW/SW SEEKUR.

91

Tracking error xe
0.4
0.3
0.2

x[m]

0.1
0
-0.1
-0.2
-0.3
-0.4

10

15

20
time[s]

25

30

35

40

Fig. 5.6 Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea sliding-mode n
timp continuu a vehiculului autonom 4DW/SW SEEKUR.
Tracking error ye
0

-0.05

y[m]

-0.1

-0.15

-0.2

-0.25

10

15

20
time[s]

25

30

35

40

Fig. 5.7 Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea sliding-mode n
timp continuu a vehiculului autonom 4DW/SW SEEKUR.
Heading error
10

Heading error [deg]

-2

-4
0

10

15

20
time[s]

25

30

35

40

Fig. 5.8 Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea sliding-mode n
timp continuu a vehiculului autonom 4DW/SW SEEKUR.

92

0.6

0.4

Sliding surface(s1)

0.2

-0.2

-0.4

-0.6

-0.8

10

15

20
time[s]

25

30

35

40

Fig. 5.9 Suprafaa de comutaie s1 obinut prin simulare n MobileSim la conducerea sliding-mode n timp
continuu a vehiculului autonom 4DW/SW SEEKUR.
25

Sliding surface(s2)

20

15

10

-5

10

15

20
time[s]

25

30

35

40

Fig. 5.10 Suprafaa de comutaie s2 obinut prin simulare n MobileSim la conducerea sliding-mode n timp
continuu a vehiculului autonom 4DW/SW SEEKUR.

Se observ c erorile de poziionare sunt mici, performanele de urmrire a traiectoriei


sunt destul de bune n cazul folosirii conducerii sliding-mode n timp continuu a vehiculului
autonom SEEKUR.

Simularea 5.2.

Urmrirea unei traiectorii liniare urmat de o traiectorie n form de S a fost testat


pentru robotul mobil PatrolBot utiliznd conducerea sliding-mode n timp discret. Parametrii
constani folosii n acest experiment sunt: q1 0.9 , q2 0.9 , 1 0.01 , 2 0.75 , k0 9.5
, k1 0.75 , k 2 15 . Aceti parametri au fost obinui prin identificare n urma unor simulri
succesive utiliznd diferite valori ale parametrilor. Arhitectura de conducere folosit este
prezentat n Fig. 5.11. Modulul Trajectory Planner const n acest caz n impunerea vitezeri
liniare de 0.4m/s i vitezei unghiulare de 0 radiani/s timp de 10s, apoi impunerea vitezei
93

unghiulare de 0.2 radiani/s timp de 15s i n final impunerea vitezei unghiulare de -0.2
radiani/s timp de 15s. Modulul Odometry este implementat de softul ARIA i viteza liniar,
viteza unghiular, poziia i orientarea vehiculului sunt obinute utiliznd funciile ARIA
corespunztoare. Pentru implementarea conducerii sliding-mode n timp discret se folosete
programul prezentat n Anexa 2. Programul folosit la conducerea sliding-mode, n
care se folosesc ecuaiile conducerii n timp discret prezentate n seciunea 3.3.1 Conducerea
sliding-mode n timp discret a WMR cu 2 DW/2FW Comunicarea dintre programul de conducere

i simulator este realizat de ARIA.

Fig. 5.11. Arhitectura de conducere a WMR 2DW/2FW PatrolBot n mediul MobileSIM

n Fig. 5.12 este prezentat simularea n MobileSim a traiectoriei robotului PatrolBot


utiliznd conducerea sliding-mode n timp discret. n Fig. 5.13 este prezentat traiectoria
simulat n MobileSim a conducerii sliding-mode n timp discret a robotului Patrolbot i cu o
linie continu roie i traiectoria dorit cu o linie ntrerupt albastr.

Fig. 5.12. Traiectoria obinut prin simulare n MobileSim la conducerea sliding-mode n timp discret a
robotului 2DW/2FW PatrolBot

94

8
Real trajectory
Desired trajectory

7
6

y[m]

5
4
3
2
1
0

x[m]

Fig. 5.13 Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea sliding-mode n
timp discret a robotului 2DW/2FW PatrolBot

Fig. 5.14 prezint eroarea pe axa Ox de urmrire traiectoriei a robotului PatrolBot


utiliznd conducerea sliding-mode n timp discret. Fig. 5.15 prezint eroarea pe axa Oy de
urmrire traiectoriei a robotului PatrolBot utiliznd conducerea sliding-mode n timp discret.
Fig. 5.16 prezint eroarea de orientare a robotului de urmrire traiectoriei a robotului
PatrolBot utiliznd conducerea sliding-mode n timp discret. n Fig. 5.17 este prezentat
suprafaa de comutaie s1 de urmrire traiectoriei a robotului PatrolBot utiliznd conducerea
sliding-mode n timp discret. n Fig. 5.18este prezentat suprafaa de comutaie s 2 de
urmrire traiectoriei a robotului PatrolBot utiliznd conducerea sliding-mode n timp discret.
Tracking error xe
0.3

0.2

0.1

x[m]

-0.1

-0.2

-0.3

-0.4

10

15

20
time[s]

25

30

35

40

Fig. 5.14 Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea sliding-mode n
timp discret a robotului 2DW/2FW PatrolBot

95

Tracking error ye
0.06
0.05
0.04

y[m]

0.03
0.02
0.01
0
-0.01
-0.02

10

15

20
time[s]

25

30

35

40

Fig. 5.15 Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea sliding-mode n
timp discret a robotului 2DW/2FW PatrolBot
Heading error
6

Heading error [deg]

-2

-4

-6

10

15

20
time[s]

25

30

35

40

Fig. 5.16 Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea sliding-mode n
timp discret a robotului 2DW/2FW PatrolBot.
0.4
0.3
0.2

Sliding surface(s1)

0.1
0
-0.1
-0.2
-0.3
-0.4
-0.5
-0.6

10

15

20
time[s]

25

30

35

40

Fig. 5.17 Suprafaa de comutaie s1 obinut prin simulare n MobileSim la conducerea sliding-mode n timp
discret a robotului 2DW/2FW PatrolBot.

96

Sliding surface(s2)

-1

-2

10

15

20
time[s]

25

30

35

40

Fig. 5.18 Suprafaa de comutaie s2 obinut prin simulare n MobileSim la conducerea sliding-mode n timp
discret a robotului 2DW/2FW PatrolBot.

Din figurile prezentate se observ c robotul urmrete traiectoria dorit cu erori mici.

Simularea 5.3

Testul ii propune urmrirea unei traiectorii n forma din Fig. 5.20 utiliznd vehiculul
autonom SEEKUR folosind conducerea sliding-mode n timp discret. Parametrii constani
folosii n acest experiment sunt: q1 0.9 , q2 0.9 , 1 0.01 , 2 0.75 , k0 30 ,

k1 0.75 , k2 15 . Aceti parametri au fost obinui prin identificare n urma unor simulri
succesive utiliznd diferite valori ale parametrilor. n Fig. 5.19 este prezentat arhitectura
conducerii folosite. n acest caz modulul Trajectory Planner folosete planificatorul de
traiectorii din [96] pentru generarea vitezei unghiulare i unghiului director. Modulul
Odometry este implementat de softul ARIA i viteza liniar, unghiul director, poziia i
orientarea vehiculului sunt obinute utiliznd funciile ARIA corespunztoare.

Fig. 5.19. Arhitectura de conducere sliding-mode n timp discret a AV 4DW/SW SEEKUR n MobileSIM.

97

Pentru implementarea conducerii sliding-mode n timp discret se folosete programul


prezentat n Anexa 2. Programul folosit la conducerea sliding-moden care se
folosesc ecuaiile conducerii n timp discret prezentate n seciunea 3.3.2 Conducerea slidingmode n timp discret a vehiculelor autonome3.3.1 Conducerea sliding-mode n timp discret a WMR
cu 2 DW/2FW .Comunicarea dintre programul de conducere i simulator este realizat de

ARIA.
Fig. 5.20 prezint simularea n MobileSim a traiectoriei generate de conducerea sliding-

mode n timp discret a vehiculului autonom SEEKUR.

Fig. 5.20 Traiectoria obinut prin simulare n MobileSim la conducerea sliding-mode n timp discret a
vehiculului autonom 4DW/SW SEEKUR.

14
Real trajectory
Desired trajectory

12

10

y[m]

0
-2

4
x[m]

10

Fig. 5.21 Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea sliding-mode n
timp discret a vehiculului autonom 4DW/SW SEEKUR.

n Fig. 5.21 este prezentat traiectoria simulat utiliznd conducerea sliding-mode n


timp discret cu o linie continu roie i traiectoria dorit cu o linie ntrerupt albastr a

98

vehiculului autonom SEEKUR. Fig. 5.22 prezint eroarea simulat pe axa Ox utiliznd
conducerea sliding-mode n timp discret a vehiculului autonom SEEKUR. Fig. 5.23 prezint
eroarea simulat pe axa Oy utiliznd conducerea sliding-mode n timp discret a vehiculului
autonom SEEKUR. Fig. 5.24 prezint eroarea simulat de orientare a robotului utiliznd
conducerea sliding-mode n timp discret a vehiculului autonom SEEKUR. n Fig. 5.25 este
prezentat simularea suprafeei de comutaie s1 . n Fig. 5.26 este prezentat simularea
suprafeei de comutaie s 2 .
Tracking error xe
0.5
0.4
0.3
0.2

x[m]

0.1
0
-0.1
-0.2
-0.3
-0.4
-0.5

10

20

30

40
50
time[s]

60

70

80

90

Fig. 5.22 Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea sliding-mode n
timp discret a vehiculului autonom 4DW/SW SEEKUR.
Tracking error ye
0.02
0.015
0.01
0.005

y[m]

0
-0.005
-0.01
-0.015
-0.02
-0.025
-0.03

10

20

30

40
50
time[s]

60

70

80

90

Fig. 5.23 Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea sliding-mode n
timp discret a vehiculului autonom 4DW/SW SEEKUR.

99

Heading error
4

Heading error [deg]

-1

-2

-3
0

10

20

30

40
50
time[s]

60

70

80

90

Fig. 5.24 Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea sliding-mode n
timp discret a vehiculului autonom 4DW/SW SEEKUR.

0.6

0.4

Sliding surface(s1)

0.2

-0.2

-0.4

-0.6

-0.8

10

20

30

40
50
time[s]

60

70

80

90

Fig. 5.25 Suprafaa de comutaie s1 obinut prin simulare n MobileSim la conducerea sliding-mode n timp
discret a vehiculului autonom 4DW/SW SEEKUR.

1.5

Sliding surface(s2)

0.5

-0.5

-1

-1.5

-2
0

10

20

30

40
50
time[s]

60

70

80

90

Fig. 5.26 Suprafaa de comutaie s2 obinut prin simulare n MobileSim la conducerea sliding-mode n timp
discret a vehiculului autonom 4DW/SW SEEKUR.

100

Se observ performane ridicate la urmrirea traiectoriei propuse, cu erori de urmrire


mici n toate graficele, iar suprafeele de alunecare oscileaz n apropierea valorii zero.

Simularea 5.4

Simularea a dorit urmrirea de ctre robotul PowerBot a traiectoriei prezentate n Fig.


5.28 utiliznd conducerea backstepping, aceast traiectorie a fost generat de un planificator.
Parametrii constani folosii n acest experiment sunt:, k1 0.5 , k2 1.25 , k1 0.3 . Aceti
parametri au fost obinui prin identificare n urma unor simulri succesive utiliznd diferite
valori ale parametrilor.

Fig. 5.27 Arhitectura de conducere backstepping a WMR 2DW/2FW PowerBot n MobileSim.

n acest caz modulul Trajectory Planner folosete planificatorul de traiectorii din [96]
pentru generarea vitezei unghiulare i vitezei unghiulare. Modulul Odometry este
implementat de softul ARIA i viteza liniar, viteza unghiular, poziia i orientarea
vehiculului sunt obinute utiliznd funciile ARIA corespunztoare.
Path

1.5

y [m]

0.5

-0.5

0.5

1.5

2.5

3.5

4.5

x [m]

Fig. 5.28. Traiectoria generat de planificator pentru testarea conducerii backstepping a robotului 2DW/2FW
Powerbot

101

Fig. 5.29 Traiectoria obinut prin simulare n MobileSim la conducerea backstepping a robotului 2DW/2FW
Powerbot

n Fig. 5.30 este prezentat traiectoria simulat cu o linie continu roie i traiectoria
dorit cu o linie ntrerupt albastr a robotului PowerBot utiliznd conducerea backstepping.
Fig. 5.31 prezint eroarea de urmrire a robotului PowerBot simulat pe axa Ox utiliznd
conducerea backstepping.
2.5
Real trajectory
Desired trajectory
2

y[m]

1.5

0.5

-0.5
-0.5

0.5

1.5

2
x[m]

2.5

3.5

4.5

Fig. 5.30. Traiectoria obinut prin simulare n MobileSim i traiectoria impus la conducerea backstepping a
robotului 2DW/2FW Powerbot

Fig.

5.32

prezint eroarea de urmrire simulat pe axa Oy a robotului PowerBot

utiliznd conducerea backstepping. Fig. 5.33 prezint eroarea de orientare a robotului


102

simulat utiliznd conducerea backstepping. n Fig. 5.34 este prezentat viteza simulat,
viteza dorit i viteza calculat utiliznd conducerea backstepping a robotului PowerBot. Fig.
5.35 prezint viteza unghiular dorit, viteza unghiular real, viteza unghiular calculat
utiliznd conducerea backstepping a robotului PowerBot.

0.1

0.05

x[m]

-0.05

-0.1

-0.15

-0.2

10

15
Time[s]

20

25

30

Fig. 5.31. Eroarea de urmrire pe axa X obinut prin simulare n MobileSim la conducerea backstepping a
robotului 2DW/2FW Powerbot

0.08
0.06
0.04

y[m]

0.02
0
-0.02
-0.04
-0.06
-0.08

10

15
time[s]

20

25

30

Fig. 5.32. Eroarea de urmrire pe axa Y obinut prin simulare n MobileSim la conducerea backstepping a
robotului 2DW/2FW Powerbot.

103

10
8
6

Heading error [deg]

4
2
0
-2
-4
-6
-8
-10

10

15
Time[s]

20

25

30

Fig. 5.33. Eroarea de urmrire a direciei obinut prin simulare n MobileSim la conducerea backstepping a
robotului 2DW/2FW Powerbot

0.5

Speed[m/s]

0.4

0.3

0.2

0.1
Real Speed
Desired Speed
Calculated Speed

-0.1

10

15
Time[s]

20

25

30

Fig. 5.34 Viteza liniar obinut prin simulare n MobileSim la conducerea backstepping a robotului
2DW/2FW Powerbot

1
0.8
0.6

w[rad/s]

0.4
0.2
0
-0.2
-0.4
Angular Velocity
Desired Angular Velocity
Calculated Angular Velocity

-0.6
-0.8

10

15
Time[s]

20

25

30

Fig. 5.35. Viteza unghiular obinut prin simulare n MobileSim la conducerea backstepping a robotului
2DW/2FW Powerbot

104

Se observ c traiectoria propus este urmrit i erorile de urmrire sunt mici. Din
graficele vitezelor liniare i unghiulare se observ c vitezele dorite, vitezele calculate i
vitezele reale au valori apropiate i aceast metod de urmrire este eficient.
Simularea 5.5
Folosind MobileSim a fost simulat funcionarea arhitecturii de evitare a obstacolelor
propuse att n lipsa obstacolelor ct i n cazul n care e prezent unul sau dou obstacole. O
hart simulnd holul de la intrarea n corpul Y cu i fr obstacole a fost creat utiliznd
Mapper3basic. Traiectoria care se dorete a fi urmrit este prezentat n exemplul fr
obstacole (Fig. 5.37), iar modul n care obstacolele sunt tratate e prezentat n urmtoarele
exemple.

Fig. 5.36 Arhitectura algoritmului pentru evitarea obstacolelor a robotului 2DW/2FW PowerBot n
MobileSim

Modulul de conducere folosit pentru testarea arhitecturii de evitare a obstacolelor


utilizeaz conducerea sliding-mode n timp discret. n acest caz modulul Trajectory Planner
folosete planificatorul de traiectorii din [96] pentru generarea vitezei liniare i vitezei
unghiulare. Modulul Odometry este implementat de softul ARIA i viteza liniar, viteza
unghiular, poziia i orientarea vehiculului sunt obinute utiliznd funciile ARIA
corespunztoare. Conectarea la simulator este realizat de ARIA. Modulul de conducere este
prezentat n Anexa 2. Programul folosit la conducerea sliding-mode.
Parametrii constani sunt q1 0.9 , q2 0.9 , 1 0.01 , 1 0.5 . Aceti parametri au
fost obinui prin identificare n urma unor simulri succesive utiliznd diferite valori ale
parametrilor. Fig. 5.36 prezint arhitectura folosit pentru evitarea obstacolelor a WMR
2DW/2FW PowerBot.
105

Fig. 5.37. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a obstacolelor, n lipsa
obstacolelor, a robotului 2DW/2FW PowerBot

Fig. 5.37. prezint traiectoria rezultat n urma conducerii sliding-mode n timp discret
de ctre robotul PowerBot. Se dorete obinerea unei traiectorii ct mai apropiat de aceasta
n cazurile n care apar obstacole. n Fig. 5.38 este prezentat traiectoria real cu o linie
continu roie i traiectoria dorit cu o linie ntrerupt albastr. Fig. 5.39 prezint eroarea pe
axa Ox. Fig. 5.40 prezint eroarea pe axa Oy. Fig. 5.41 prezint eroarea de orientare a
robotului. n Fig. 5.42 este prezentat suprafaa de comutaie s1 . n Fig. 5.43 este prezentat
suprafaa de comutaie s 2 .

Fig. 5.38. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a obstacolelor, n lipsa
obstacolelor, a robotului 2DW/2FW PowerBot i traiectoria impus

106

Fig. 5.39. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

Fig. 5.40. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

Fig. 5.41. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

107

Fig. 5.42 Suprafaa de alunecare s1 obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n lipsa obstacolelor , a robotului 2DW/2FW PowerBot.

Fig. 5.43. Suprafaa de alunecare s2 obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n lipsa obstacolelor, a robotului 2DW/2FW PowerBot.

Se observ o bun urmrire a traiectoriei impuse cu erori mici, iar suprafeele


de alunecare evolueaz n apropierea valorii zero.
Simularea 5.6
Considerm aceeasi hart i traiectorie ca la simularea 5.5 dar adugm 1 obstacol. n
Fig. 5.51 este prezentat traiectoria de evitare a obstacolului calculat.
108

Fig. 5.44. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a obstacolelor, n
prezena unui obstacol, a robotului 2DW/2FW PowerBot

n Fig. 5.45 este prezentat traiectoria real cu o linie continu roie i traiectoria dorit
cu o linie ntrerupt albastr.

Fig. 5.45. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a obstacolelor, n
prezena unui obstacol, a robotului 2DW/2FW PowerBot i traiectoria impus

Fig. 5.46 prezint eroarea pe axa Ox. Fig. 5.47prezint eroarea pe axa Oy. Fig. 5.48
prezint eroarea de orientare a robotului. n Fig. 5.49 este prezentat suprafaa de alunecare
s1 .

109

Fig. 5.46. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

Fig. 5.47. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

Fig. 5.48. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

110

Fig. 5.49. Suprafaa de alunecare s1 obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

Fig. 5.50. Suprafaa de alunecare s2 obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena unui obstacol, a robotului 2DW/2FW PowerBot.

Se observ c traiectoria dorit este urmrit chiar i n prezena obstacolului, dar se


constat o cretere a erorilor de urmrire cauzat de traiectoria de evitare a obstacolului i
revenirea la traiectoria iniial.
Simularea 5.7.
Simularea 5.7 este asemntoare cu simularea 5.6 dar se mai introduce un obstacol.
111

Fig. 5.51. Traiectoria obinut prin simularea n MobileSim a algoritmului de evitare a obstacolelor, n
prezena a dou obstacole, a robotului 2DW/2FW PowerBot

n Fig. 5.52 este prezentat traiectoria real cu o linie continu roie i traiectoria dorit
cu o linie ntrerupt albastr. Fig. 5.53 prezint eroarea pe axa Ox. Fig. 5.54 prezint eroarea
pe axa Oy. Fig. 5.55 prezint eroarea de orientare a robotului. n Fig. 5.56 este prezentat
suprafaa de comutaie s1 . n Fig. 5.57 este prezentat suprafaa de comutaie s 2 .

Fig. 5.52. Traiectoria de evitare a obstacolului obinut prin simularea n MobileSim a algoritmului de evitare
a obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot i traiectoria dorit.

112

Fig. 5.53. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.

Fig. 5.54. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.

Fig. 5.55. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.

113

Fig. 5.56 Suprafaa de comutaie s1 obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.

Fig. 5.57. Suprafaa de comutaie s2 obinut prin simularea n MobileSim a algoritmului de evitare a
obstacolelor, n prezena a dou obstacole, a robotului 2DW/2FW PowerBot.

Se observ c algoritmul de evitare a obstacolelor propus evit cele dou obstacole i


revine la traiectoria dorit. Evitarea obstacolelor introduce erori semnificative de urmrire i
analiznd graficele se poate determina nceputul fiecrei proceduri de evitare a obstacolelor.
Simularea 5.8.
Se folosete arhitectura pentru evitarea obstacolelor din Fig. 5.58. Se consider un
mediu extern prezentat n harta din Fig. 5.59. Se dorete deplasarea vehiculului conform
traiectoriei din Fig. 5.60. n acest caz se consider c nu exist obstacole n calea vehiculului.
Parametrii constani sunt q1 0.9 , q2 0.9 , 1 0.02 , 1 0.4 . Aceti parametri au fost
114

obinui prin identificare n urma unor simulri succesive utiliznd diferite valori ale
parametrilor i sunt utilizai i la simularea 5.8. n acest caz modulul Trajectory Planner
folosete planificatorul de traiectorii din [96] pentru generarea vitezei liniare i unghiului
director. Modulul Odometry este implementat de softul ARIA i viteza liniar, viteza
unghiular, poziia i orientarea vehiculului sunt obinute utiliznd funciile ARIA
corespunztoare. Conectarea la simulator este realizat de ARIA. Modulul de conducere este
prezentat n Anexa 2. Blocul Range Readings este implementat n Anexa 5. Modulul
Obstacle este implementat n Anexa 6. Modulul Obstacle Avoidance Planner este
implementat n Anexa 8 i utilizeaz funciile din Anexa 7.

Fig. 5.58. Arhitectura pentru evitarea obstacolelor a AV 4DW/SW SEEKUR

Fig. 5.59. Harta utilizat pentru testarea evitrii obstacolelor utiliznd vehiculul autonom 4DW/SW SEEKUR

115

Fig. 5.60. Traiectoria generat de planificator pentru vehiculul autonom 4DW/SW SEEKUR

n Fig. 5.61 este prezentat traiectoria dorit i traiectoria simulat. Fig. 5.62 prezint
simularea n MobileSim. Fig. 5.63 prezint eroarea de urmrire xe.
Fig. 5.64 prezint eroarea de urmrire ye. n Fig. 5.65 este prezentat eroarea de
urmrire a direciei.

Fig. 5.61. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea obstacolelor n
cazul absenei obstacolelor a vehiculului autonom 4DW/SW SEEKUR

116

Fig. 5.62. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea obstacolelor, n
cazul absenei obstacolelor, a vehiculului autonom 4DW/SW SEEKUR i traiectoria impus

Fig. 5.63. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom 4DW/SW SEEKUR.

Fig. 5.64. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom 4DW/SW SEEKUR.

117

Fig. 5.65. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului pentru
evitarea obstacolelor, n cazul absenei obstacolelor, a vehiculului autonom 4DW/SW SEEKUR.

Simularea 5.9.
Se folosete harta din Fig. 5.59, dar n acest caz se introduce un obstacol n prima
intersecie. Parametrii folosii sunt identici cu cei din simularea precedent. Vehiculul trebuie
s ocoleasc acest obstacol i apoi s revin la traiectoria iniial. Fig. 5.66 prezint simularea
MobileSim a depirii obstacolului. n Fig. 5.67 este prezentat traseul simulat.

Fig. 5.66. Vehiculul autonom 4DW/SW SEEKUR simulnd evitarea obstacolului n MobileSim

118

n Fig. 5.68este prezentat traiectoria dorit i traiectoria simulat. prezint simularea n


MobileSim. Fig. 5.69 prezint eroarea de urmrire xe. Fig. 5.70. prezint eroarea de urmrire
ye. n Fig. 5.71 este prezentat eroarea de urmrire a direciei.

Fig. 5.67. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea obstacolelor, n
cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW SEEKUR

Fig. 5.68. Traiectoria obinut prin simularea n MobileSim a algoritmului pentru evitarea obstacolelor, n
cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW SEEKUR i traiectoria dorit.

119

Fig. 5.69. Eroarea de urmrire pe axa X obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW SEEKUR.

Fig. 5.70. Eroarea de urmrire pe axa Y obinut prin simularea n MobileSim a algoritmului pentru evitarea
obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW SEEKUR.

Fig. 5.71. Eroarea de urmrire a direciei obinut prin simularea n MobileSim a algoritmului pentru
evitarea obstacolelor, n cazul prezenei unui obstacol, a vehiculului autonom 4DW/SW SEEKUR.

120

Se observ c vehiculul urmrete traiectoria chiar i n prezena obstacolului, dar


prezena obstacolului introduce erori de urmrire semnificative. Se observ o ntrziere ntre
momentul n care este trimis comanda pentru nceperea evitarii obstacolului i nceperea
evitrii de ctre vehicul.

5.2 Rezultatele experimentale ale conducerii n timp real ale roboilor


mobili i vehiculelor autonome
Experimente n timp real au fost efectuate pe robotul mobil PowerBot pentru a testa
eficiena controlerului sliding-mode n timp discret. Programul, scris n C++, pentru
conducerea robotului ruleaz pe un PC i trimite comenzi robotului i primete datele despre
odometrie utiliznd comenzile ARIA. Comunicarea dintre calculator i robot este gestionat
de softul ARIA. Conectarea la calculator se realizeaz utiliznd o reea wireless. Pentru
conectarea la robot se folosete un router wireless pentru conexiunea la reea i pentru c
robotul are o conexiune RS232 la router se conecteaz un Universal Data Server de la
Lantronix pentru conversia din protocolul RS232 la protocolul Ethernet. Programul scris n
C++ i compilat n Visual Studio este rulat utiliznd CMD. Arhitectura conducerii slidingmode n timp real a WMR 2DW/2FW PowerBot este prezentat n Fig. 5.72.

Fig. 5.72. Arhitectura conducerii sliding-mode n timp discret a WMR 2DW/2FW Powerbot n timp real

121

Modulul Odometry este implementat de softul ARIA, care interpreteaz datele primite
de la robot pentru a calcula poziia, orientarea, viteza liniar, viteza unghiular pe baza
informaiei primite de la encoderele robotului. Pentru implementarea modulului de conducere
se folosete programul din Anexa 2. Programul folosit la conducerea sliding-mode
cu ecuaiile modificate conform seciunii 3.3.1 Conducerea sliding-mode n timp discret a WMR
cu 2 DW/2FW.

Implementarea conducerii n timp real 1

Traiectoria urmrit a fost compus dintr-o traiectorie liniar i o traiectorie circular.


Parametrii constani sunt q1 0.9 , q2 0.85 , 1 0.02 , 1 0.5 . Aceti parametri au fost
identificai experimental. Fig. 5.73 este prezentat traiectoria real cu o linie continu roie i
traiectoria dorit cu o linie ntrerupt albastr. n acest caz modulul Trajectory Planner const
n impunerea vitezei liniare de 0.3 m/s i viteza unghiular de 0 radiani/s timp de 5s, apoi
viteza unghiular impus devine 0.2 radiani/s.
3.5
Real trajectory
Desired trajectory

3
2.5

y[m]

2
1.5
1
0.5
0
-0.5

0.5

1.5

2.5

3.5

x[m]
Fig. 5.73. Traiectoria obinut la conducerea n timp real prin metoda sliding-mode n timp discret a robotului
2DW/2FW PowerBot

122

Fig. 5.74 prezint eroarea pe axa Ox. Fig. 5.75 prezint eroarea pe axa Oy. Fig. 5.76
prezint eroarea de orientare a robotului. n Fig. 5.77 este prezentat suprafaa de alunecare
s1 . n Fig. 5.78 este prezentat suprafaa de alunecare s 2 .
Tracking error xe
0.3

0.2

0.1

x[m]

-0.1

-0.2

-0.3

-0.4

10

15

20

25

time[s]

Fig. 5.74. Eroarea de urmrire pe axa X obinut la conducerea n timp real prin metoda sliding-mode n timp
discret a robotului 2DW/2FW PowerBot.
Tracking error ye
0
-0.005
-0.01

y[m]

-0.015
-0.02
-0.025
-0.03
-0.035
-0.04

10

15

20

25

time[s]

Fig. 5.75. Eroarea de urmrire pe axa Y obinut la conducerea n timp real prin metoda sliding-mode n timp
discret a robotului 2DW/2FW PowerBot.
Heading error
4

Heading error [deg]

-1

-2

10

15

20

25

time[s]

Fig. 5.76. Eroarea de urmrire a direciei obinut la conducerea n timp real prin metoda sliding-mode n
timp discret a robotului 2DW/2FW PowerBot.

123

0.5
0.4
0.3

Sliding surface(s1)

0.2
0.1
0
-0.1
-0.2
-0.3
-0.4
-0.5

10

15

20

25

time[s]

Fig. 5.77. Suprafaa de alunecare s1 obinut la conducerea n timp real prin metoda sliding-mode n timp
discret a robotului 2DW/2FW PowerBot.

1.5

Sliding surface(s2)

0.5

-0.5

-1

-1.5

10

15

20

25

time[s]

Fig. 5.78. Suprafaa de alunecare s2 obinut la conducerea n timp real prin metoda sliding-mode n timp
discret a robotului 2DW/2FW PowerBot.

Se observ o eroare iniial pe axa X la pornirea robotului care este redus dup
aproximativ 5s.Erorile de urmrire obinute sunt mici i dovedesc eficiena conducerii
propuse.

Implementarea conducerii n timp real 2.

Robotul PowerBot a urmat o traiectorie n form de S, prezentat n Fig. 5.79.


Parametrii constani sunt q1 0.9 , q2 0.85 , 1 0.02 , 1 0.5 . Aceti parametri au fost
identificai experimental. Modulul Trajectory planner const n impunerea unei viteze liniare
de 0.3 m/s i a unei viteze unghiulare de 0.2 radiani/s timp de 12.5 s i apoi impunerea vitezei
unghiulare de -0.2 radiani/s timp de 12.5 s.

124

4
Real trajectory
Desired trajectory

3.5
3

y[m]

2.5
2
1.5
1
0.5
0

0.2

0.4

0.6

0.8

1.2

1.4

x[m]

Fig. 5.79. Traiectoria n form de S obinut la conducerea n timp real prin metoda sliding-mode n
timp discret a robotului 2DW/g2FW PowerBot.

Fig. 5.79 prezint traiectoria real i traiectoria dorit. Fig. 5.80 prezint eroarea de
urmrire pe axa X. Fig. 5.81 prezint eroarea de urmrire a direciei
Tracking error xe
0.1

0.05

x[m]

-0.05

-0.1

-0.15

-0.2

-0.25

10

15

20

25

time[s]

Fig. 5.80. Eroarea de urmrire pe axa X a traiectoriei n form de S obinut la conducerea n timp real
prin metoda sliding-mode n timp discret a robotului 2DW/2FW PowerBot.

Heading error
8
6

Heading error [deg]

4
2
0
-2
-4
-6
-8
-10

10

15

20

25

time[s]

Fig. 5.81. Eroarea de urmrire a orientrii n cazul traiectoriei n form de S obinut la conducerea n timp
real prin metoda sliding-mode n timp discret a robotului 2DW/2FW PowerBot.

125

Sliding surface(s1)
0.3

0.2

0.1

-0.1

-0.2

-0.3

-0.4

10

15

20

25

Fig. 5.82. Suprafaa de alunecare s1 obinut la conducerea n timp real prin metoda sliding-mode n
timp discret a robotului 2DW/2FW PowerBot pentru urmrirea traiectoriei n form de S.

Fig. 5.82 prezint suprafaa de comutaie s1 Fig. 5.83 prezint suprafaa de comutaie
s2.
4
3

Sliding surface(s2)

2
1
0
-1
-2
-3
-4
-5

10

15

20

25

time[s]

Fig. 5.83. Suprafaa de comutaie s2 obinut la conducerea n timp real prin metoda sliding-mode n timp
discret a robotului 2DW/2FW PowerBot pentru urmrirea traiectoriei n form de S.

Din grafice reiese o foarte bun urmrire a traiectoriei propuse, chiar dac eroarea de
urmrire pe axa X este mare la nceputul testului, dup aproximativ 5s este redus aproape la
zero. Se observ c cele dou suprafee de alunecare oscileaz n jurul valorii zero.

5.3 Concluzii
Pentru a valida algoritmii propui am prezentat 9 simulri realizate n MobileSim i 2
implementri n timp real.Am scris programele pentru testarea algoritmilor n C++ i i-am
compilat utiliznd Visual Studio. Pentru realizarea simulrilor am folosit softul MobileSim de
la Mobile Robots. MobileSim simuleaz comportamentul fiecrui robot produs de Mobile
126

Robots i detecia laser sau utiliznd sonarele a obstacolelor. Am creat obstacolele simulate n
MobileSim n Mapper3Basic. Mapper3Basic este un utilitar creat pentru a genera hri ale
mediilor dorite pentru a putea fi simulate n MobileSim.
Pentru simularea 5.1 am impus urmrirea de ctre robotul PatrolBot a unei traiectorii
n form de S utiliznd conducerea sliding-mode n timp discret. La simularea 5.2 am dorit
urmrirea unei traiectorii liniare, urmate de una circular de ctre vehiculul SEEKUR,
utiliznd conducerea sliding-mode n timp continuu. Simularea 5.3 a presupus urmrirea unei
traiectorii n form de S de ctre vehiculul SEEKUR, utiliznd conducerea sliding-mode n
timp discret. n simularea 5.4 am testat urmrirea unei traiectorii generate de un planificator
de traiectorii, folosind metoda backstepping i robotul PowerBot. n simularea 5.5 am realizat
urmrirea unei traiectorii de ctre PowerBot n mediul definit fr obstacole. Urmtoarele
dou simulri au testat dac conducerea mai este utilizabil n prezena unor obstacole.
Simularea 5.8 a testat conducerea vehiculului SEEKUR utiliznd sliding-mode n timp discret
i o hart care simuleaz o zona a unui ora.n cadrul simulrii 5.9 am introdus un obstacol
pentru a testa dac este posibil evitarea sa.
La implementarea conducerii n timp real 1 am realizat urmrirea unei traiectorii liniare
urmate de una circular de ctre PowerBot, folosind conducerea sliding-mode n timp-discret,
iar n cadrulimplementrii conducerii n timp real 2 am testat urmrirea unei traiectorii n
form de S.
Rezultatele testelor au artat eficiena metodelor propuse att pentru urmrirea unei
traiectorii utiliznd cele trei metode de conducere: sliding-mode n timp continuu, slidingmode n timp discret i backstepping, ct i pentru algoritmul de evitare a obstacolelor. Am
constatat c urmrirea traiectoriei n absena obstacolelor are erori mai bune, iar prezena unui
obstacol introduce erori de urmrire suplimentare cnd se revine la traiectoria iniial.

127

Capitolul 6
6.

Concluzii
n aceast lucrare am tratat problema conducerii i evitarii de obstacole utiliznd roboi mobili

i vehicule autonome. Pentru o mai buna calculare a odometriei am analizat senzorul IMU Xsens Mti
de la Xsens i am proiectat un filtru Kalman pentru a elimina erorile aprute n calculul atitudinii
furnizat de IMU datorit acumularii erorilor de integrare a semnalului afectat de zgomot. Cercetarea
senzorilor IMU a fost realizat cu suportul Institutului de Sisteme i Robotic , din cadrul
Universitii din Coimbra, sub supervizarea profesorului Urbano Nunes. Am determinat modelele
cinematice ale roboilor mobili i vehiculelor autonome. Am folosit aceste modele pentru a sintetiza
comenzile necesare pentru urmrirea traiectoriilor.

Am folosit trei metode de conducere diferite pentru a realiza conducerea roboilor


mobili i a vehiculelor autonome: conducerea sliding-mode n timp continuu, conducerea
sliding-mode n timp discret i conducerea backsteping. Pentru fiecare tip de conducere am
calculat comenzile specifice pentru roboii mobili i vehiculele autonome. Metodele de
conducere obinute pot fi implementate pe toi roboii din laboratorul de robotic.
Am propus o metod de evitare a obstacolelor care utilizeaz sonarele n cazul WMR
i senzori laser n cazul AV. Am considerat cazul n care se dorete urmrirea traiectoriei
impuse i abatera doar pentru a evita un obstacol i ntoarcerea la traiectoria iniial. Am
folosit un generator de traiectorii online care stabilete o nou traiectorie de urmrit i care
este impus modulului de conducere, fr am fi nevoie de elaborarea unui nou modul de
conducere pentru a evita obstacolul. Metoda propus permite utilizarea oricrei metode de
conducere propuse n aceast lucrare.
Am realizat simulri i implementri n timp real pentru a valida algoritmii propui n
aceast lucrare.

6.1. Contribuii privind conducerea i evitarea obstacolelor utiliznd


roboi mobili i vehicule autonome
Principalele contribuii graviteaz n jurul domeniilor:

Estimarea atitudinii

Conducerea trajectory-tracking a roboilor mobili i vehiculelor autonome

Evitarea obstacolelor

Testarea algoritmilor propui

128

6.1.1 Estimarea atitudinii


Am pornit de la nevoia de a avea date corecte despre orientarea vehiculului i faptul
c IMU poate oferi date despre orientare dac se filtreaz erorile. Pentru c am constatat erori
semnificative la integrarea semnalelor de la senzori pentru obinerea atitudinii am analizat
senzorul IMU Xsens Mti pentru a determina erorile caracteristice. Am analizat senzorul
utiliznd metoda Allan. Am ales quaternionii ca mod de reprezentare a atitudinii pentru c nu
prezint singularitatea specific reprezentrii cu ajutorul unghiurilor Euler, dar pentru
validarea rezultatelor, am trecut de la reprezentarea atitudinii n quaternioni la reprezentarea
Euler pentru o mai bun ntelegere a rezultatelor. Utiliznd caracteristica rezultat am
elaborat un filtru Kalman indirect n dou etape pentru a elimina erorile care se acumuleaz la
calculul orientrii. Am constatat c este necesar folosirea mai multor senzori pentru o
estimare corect a atitudinii. n acest caz am folosit datele de la accelerometre i senzorii
magnetici pentru a obine coreciile necesare. Filtrul folosete datele de la giroscoape pentru a
calcula atitudinea estimat apoi aplic corecii n dou etape. n prima etap sunt aplicate
corecii rezultate de la accelerometre, starea eroare rezultat este apoi folosit la corecia
atitudinii estimate. Etapa a doua aduce coreciile magnetice, dup actualizarea strii eroare se
corecteaz estimarea atitudinii cu noua valoare obinut.

6.1.2 Conducerea trajectory-tracking a roboilor mobili i vehiculelor


autonome
Am tratat problema urmririi traiectoriei n capitolul 3. Am analizat trei metode de
conducere pentru conducerea vehiculelor i roboilor mobili: conducerea sliding-mode n
timp continuu, conducerea sliding-mode n timp discret i conducerea backstepping.
Am propus rezolvarea problemei conducerii roboilor mobili i vehiculelor autonome
utiliznd conducerea sliding-mode n timp continuu, conducerea sliding-mode n timp discret
i conducerea backstepping bazate pentru WMR i AV.
De la cazul general i folosind modelul cinematic al WMR cu 2 DW/2SW , modelul
erorilor de urmrire i dinamica erorilor de urmrire am calculat comenzile pentru viteza
liniar i viteza unghiular a WMR. n calcul am folosit suprafee de comutaie i legi de
conducere bazate pe modelul propus de Gao pentru timp continuu[31].
Utiliznd cazul general i modelul cinematic simplificat al vehiculului autonom
SEEKUR, suprafee de comutaie, legi de conducere similare celor propuse de Gao pentru

129

timp continuu[31], modelul erorilor de urmrire i dinamica erorilor de urmrire am calculat


comenzile pentru viteza liniar i unghiul de direcie.
Am sintetizat modul de calcul al comenzii utiliznd sliding-mode n timp discret
pentru cazul general.Utiliznd cazul general am trecut la calculul comenzilor pentru WMR i
AV.
Pornind de la modelul cinematic al WMR n timp discret, erorile de urmrire n timp
discret , dinamica erorilor de urmrire n timp discret, suprafeele sliding pentru timp discret
i legea de conducere pentru timp discret am calculat comenzile pentru viteza liniar i viteza
unghiular a WMR. Conducerea n timp discret a WMR i AV afost prezentat n
publicaiile: [16], [17], [18], [18], [20], [21], [24] i [93].
Utiliznd modelul cinematic simplificat al vehiculului autonomon n timp discret,
erorile de urmrire n timp discret , dinamica erorilor de urmrire n timp discret, suprafeele
sliding pentru timp discret i legea de conducere pentru timp discret am calculat comenzile
pentru viteza liniar i unghiul de direcie a vehicului autonom.
Am prezenat cazul general al conducerii backstepping. n continuare am calculat
comanda pentru conducerea backstepping n cazul WMR utiliznd modelul cinematic al
robotului i dinamica erorilor de urmrire a robotului. Conducerea backstepping a WMR a
fost publicat n [19].
Am calculat comenzile pentru viteza liniar i unghiul de direcie al vehiculului
autonom SEEKUR utiliznd modelul cinematic i dinamica de urmrire a erorilor i
procedura de conducere backstepping.

6.1.3 Evitarea obstacolelor


O soluie pentru problema evitrii obstacolelor n cazul n care se dorete urmrirea
unei traiectorii impuse i abaterea de la aceast traiectorie doar pentru a evita un obstacol i
ntoarcerea la traiectoria impus este propus. Am considerat cazul n care robotul este
echipat cu sonare i cazul n care vehiculul este echipat cu senzori laser.
Detecia obstacolelor folosete o bul de sensibilitate pentru a determina dac un
obstacol trebuie evitat. Att sonarele ct i laserele au permis definirea unei bule de
sensibilitate care acoper 180 . Este folosit aceai metod de conducere n cazul n care nu
exist obstacole i n cazul n care un obstacol este prezent. Ct timp nu este detectat nici un
obstacol care blocheaz traiectoria dorit platforma urmrete traiectoria impus folosind
metoda de conducere aleas din cele 3 prezentate n capitolul anterior.
130

Dac un obstacol a fost detectat se caut coordonatele care s permit conducerea n


siguran a platformei i se genereaz o traiectorie bazat pe Quintic equations. Nu se dorete
identificarea formei obstacolului ci doar o zon n care acesta poate fi evitat. Aceast
traiectorie este impus modulului de conducere. Se continu deplasarea pe aceast traiectorie
pn cnd obstacolul a fost evitat. Dup ce obstacolul a fost evitat se calculeaz traiectoria
pentru a reveni la traiectoria impus i modul de conducere urmrete aceast traiectorie apoi
se revine la traiectoria impus iniial.

6.1.4 Testarea algoritmilor propui


Problema conducerii prezentat n capitolul 3 i problema evitrii obstacolelor
prezentat n capitolul 4 sunt testate n capitolul 5 cu ajutorul simularilor i testelor n timp
real. Au fost realizate 9 simulri i 2 teste pentru a verifica algoritmii propui.
Am realizat simulrile n MobileSim, un simulator oferit de Mobile Robots care
permite simularea comportamentului tuturor roboilor comercializai de Mobile Robots i a
senzorilor laser sau a sonarelor. MobileSim primete comenzile generate de ARIA, iar
utilizatorul trebuie s foloseasc funciile implementate n ARIA pentru conducerea
robotului.
Pentru a simula evitarea obstacolelor a fost nevoie s creez hri n care s fie
simulate obstacolele dorite. Toate hrile folosite au fost create n Mapper3Basic. Acest
program este oferit de Mobile Robots pentru a crea hri ale mediilor n care se doresc s fie
simulai roboii. Hrile generate n Mapper3Basic sunt compatibile cu MobileSim i sunt
ncrcate n simulator naintea simulrilor.
n cazul testelor n timp real am rulat programul pe un PC cu frecvena de eantionare
de 100ms i am folosit o reea wireless pentru a comunica cu robotul. Pentru c robotul nu are
o conexiune Ethernet a trebuit s folosesc un Universal Data Server de la Lantronix care a
realizat conversia de la comunicaia RS232 a robotului i comunicaia Ethernet a routerului
cu ajutorul cruia am implementat reeaua wireless.
Testarea conducerii sliding-mode n timp continuu a fost realizat n simularea 5.1
utiliznd vehicului SEEKUR i o traiectorie liniar urmat de una circular.
Testarea conducerii sliding-mode n timp discret a fost realizat n simularea 5.2,
simularea 5.3, implementarea conducerii n timp real 1 i implementarea conducerii n timp
real 2. Simularea 1 a realizat urmrirea unei traiectorii n form de S de ctre robotul
PatrolBot. Simularea 5.3 a realizat urmrirea unei traiectorii n form de S utiliznd vehiculul
131

SEEKUR. Conducerea robotului PowerBot a fost testat utiliznd 2 implementri n timp


real. Primul test n timp real a realizat urmrirea unei traiectorii liniare urmate de una
circulara, iar al doilea test n timp real a realizat urmrirea unei traiectorii n form de S.
Simulrile i testele efectuate au dovedit o bun urmrire a traiectoriei dorite, iar
erorile au fost mici, astfel controllerele fiind validate.
Evitarea obstacolelor a fost testat n simulrile 5.5, 5.6, 5.7, 5.8, 5.9. Evitarea
obstacolelor utiliznd robotul PowerBot a fost testat n simulrile 5.5, 5.6, 5.7. Iniial n
simularea 5.5 a fost testat algoritmul n lipsa obstacolelor, iar n simularea urmtoare a fost
adugat un obstacol, iar n simularea 5.7 au fost folosite 2 obstacole. Evitarea obstacolelor
folosind detecia laser i vehiculul SEEKUR a fost testat n simulrile 5.8 i 5.9. Iniial a fost
testat algoritmul fr a exista obstacole, iar n ultima simulare a fost adugat un obstacol.
Simulrile n lipsa obstacolelor au dovedit performane similare cazului n care nu au fost
folosii senzori pentru detectarea obstacolelor. n prezena obstacolelor erorile de urmrire
dup evitarea obstacolelor au crescut i se pot observa n grafice erori semnificative n
momentul nceperii procedurii de evitare a obstacolelor i la teminarea acesteia, dar
conducerea realizeaz traiectoria dorit.
Din testele efectuate reiese c att metodele de conducere ct i de evitare a
obstacolelor ofer rezultate bune.

6.1.5 Direcii de cercetare deschise


O direcie de cercetare poate fi implementarea n timp real a algorimului de evitare a
obstacolelor utiliznd vehiculul autonom SEEKUR.
O alt direcie de cercetare poate fi conducerea vehiculelor autonome utiliznd
atitudinea furnizat de IMU, n timp real i dezvoltarea unui filtru Kalman care s furnizeze i
odometria vehiculului, corectat cu date de la encodere sau GPS.

6.1.6 Diseminarea rezultatelor


Rezultatele cercetrii au fost diseminate n 8 lucrri prezentate la conferine
internaionale i o lucrare publicat n Annals of the University of Coimbra. Din cele 9 lucrari
sunt primul autor la 7 lucrri i coautor la 2 lucrri.
[1] Dumitrascu B. and Filipescu A., Discrete-time sliding-mode controller for
wheeled mobile robots, Proceedings of the 18th International Conference on Control Systems
and Computer Science, vol. 1, Bucuresti, pag. 397-403, mai, 2011.
132

Aceast lucrare propune un controller sliding-mode n timp discret pentru rezolvarea


problemei urmririi traiectoriei robotului PowerBot. Controllerul se bazeaz pe modelul
cinematic calculat n capitolul 3 i este descris n capitolul 3. Teste pentru dovedirea
eficienei conducerii sunt prezentate n capitolul 5.
[2] Dumitrascu B. and Filipescu A., Sliding mode control of lateral motion for four
driving-steering wheels autonomous vehicle, Annals of the University of Craiova, vol. 7 (34),
pag 20-25, 2010.
Aceast lucrare este bazat pe lucrarea [18] prezint un controller sliding-mode n
timp continuu pentru conducerea lateral a vehiculului autonom SEEKUR, controolerul a fost
prezentat n capitolul 3 i folosete modelul cinematic din capitolul 3.
[3] Dumitrascu B. and Filipescu A., Sliding mode controller for steering of fourwheels driving and steering vehicle, Proceedings of the 14th International Conference on
System Theory and Control, pag. 202-206, Sinaia, 2010.
Aceast lucrare prezint un controller sliding-mode n timp continuu pentru conducerea
lateral a vehiculului autonom SEEKUR, controolerul a fost prezentat n capitolul 3, i
folosete modelul cinematic din capitolul 3.
[4] Dumitrascu B., Filipescu A., Backstepping control of wheeled mobile robots,
Proceedings of the 15th International Conference on System Theory and Control, Sinaia, pag.
206-211, 2011.
n aceast lucrare a fost propus conducerea backstepping a robotului mobil
PowerBot, care a fost prezentat n capitolul 3 i testat n capitolul 5. Conducerea este
calculat pornind de la modelul cinematic al robotului prezentat n capitolul 3.
[5] Dumitrascu B., Filipescu A., Minzu V., Voda A., Minca E.,Discrete-Time SlidingMode Control of Four Driving-Steering Wheels Autonomous Vehicle, Proceedings of the
30th Chinesse Control Conference, pag. 3620-3625, 2011.
Lucrarea prezint conducerea sliding-mode n timp discret a vehiculului autonom
SEEUR, din capitolul 3 al acestei teze. Conducerea folosete modelul cinematic simplificat al
vehiculelor 4DW/SW i este testat n capitolul 5.
[6]] Dumitrascu B., Filipescu A., Radaschin A., Filipescu A. Jr., Minca E., Discretetime sliding mode control of wheeled mobile robots, Proceedings of 2011 8th Asian Control
Conference (ASCC) ,Kaohiung, Taiwan, pag. 771-776, mai , 2011.
Aceast lucrare propune un controller sliding-mode n timp discret pentru rezolvarea
problemei urmririi traiectoriei robotului PowerBot. Controllerul se bazeaz pe modelul

133

cinematic calculat n capitolul 3 i este descris n capitolul 3. Teste pentru dovedirea


eficienei conducerii sunt prezentate n capitolul 5.
[7] Dumitrascu B., Filipescu A., Vasilache C., Minca E., Filipescu A. Jr., Discretetime sliding-mode control of four driving/steering wheels mobile platform, Proceedings of
the 19th Mediterranean Conference on Control and Automation, Corfu, Grecia, iunie, 2011,
pag. 1076-1081.
Lucrarea propune metoda de conducere sliding-mode n timp discret pentru problema
urmririi traiectoriei vehiculului autonom cu 4 DW/SW, SEEKUR. Conducerea se bazeaz
pe modelul cinematic calculat n capitolul 3 i este descris n capitolul 3. Teste pentru
dovedirea eficienei conducerii sunt prezentate n capitolul 5.
[8] ]Filipescu A., Minzu V., Dumitrascu B. and Filipescu A., Trajectory-tracking and
discrete-time sliding-mode control of wheeled mobile robots, The 2011 IEEE International
Conference on Information and Automation, iunie, 2011.
Aceast lucrare propune un metoda de conducere sliding-mode n timp discret pentru
rezolvarea problemei urmririi traiectoriei robotului PowerBot. Conducerea se bazeaz pe
modelul cinematic calculat n capitolul 3 i este descris n capitolul 3. Teste pentru dovedirea
eficienei conducerii sunt prezentate n capitolul 5.
[9] Solea R., Filipescu A., Filipescu S, and Dumitrascu B., Sliding-mode controller
for four- wheel-steering vehicle: trajectory-tracking problem, Proceedings of the 8th World
Congress on Intelligent Control and Automation, Jinan, China, pag. 1185-1190, 2010.
Aceast lucrare propune metoda de conducere sliding-mode n timp continuu pentru
rezolvarea problemei urmririi traiectoriei vehiculului autonom SEEKUR Conducerea se
bazeaz pe modelul cinematic calculat n capitolul 3 i este descris n capitolul 3. Teste
pentru dovedirea eficienei conducerii sunt prezentate n capitolul 5.

134

7.

Bibliografie

[1] Andrade Da Silva, Teppa Garrn J. M. and Surez P.A., Sliding mode fuzzy gain scheduling in
sampled data nonlinear systems, 2005 American Control Conference, Portland, OR, USA, pag.
4965-4970, iunie, 2005.
[2] Arambula Cosio F. and Padilla Castaneda M.A., Autonomous robot navigation using adaptive
potential fields, Mathematical and Computer Modelling, vol. 40, pag. 1141-1156, 2004.
[3] Arthur Gelb, Joseph F. Kasper, Raymond A. Nash, Charles F. Price, Arthur A. Sutherland,
Applied optimal estimation, The M.I.T. Pess 2001.
[4] Bakker T., Kees van Asselt, Jan Bontsema, Mller J., Gerrit van Straten, A path following
algorithm for mobile robots, 21 April 2010 This article is published with open access at
Springerlink.com.
[5] Borenstein J., Koren Y., Real-time obstacle avoidance for fast mobile robots, IEEE Transactions
on Systems, Man, and Cybernetics, vol. 19, nr. 5, pag. 1179-1187, 1989.
[6] Borenstein J., Koren Y., The vector field histogram-fast obstacle avoidance for mobile robots,
IEEE Journal of Robotics and Automation, pag. 278-288, vol. 7, 1991.
[7] Brockett R., Control theory and singular Riemannian geometry, New Directions in Applied
Mathematics, Springer-Verlag, pag. 11-27 ,1981.
[8] Carlo L. Bottasso, Leonello D., and Savini B., Path planning for autonomous vehicles by
trajectory smoothing using motion primitives, Paper presented at the AHS International
Specialists Meeting on Unmanned Rotorcraft, Phoenix, AZ, USA, January 2325, 2007.
[9] Chen Chieh, Backstepping control design and its applications to vehicle lateral control in
automated highway systems, University of California at Berkeley, 1996.
[10] Choukroun D., Novel methods for attitude determination using vector observations for attitude
determination, PhD Thesis, Haifa, Mai, 2003.
[11] Chu-qing C., Lian-zheng G. and Rui-feng L., Mobile robots target tracking using finite-time
convergence sliding mode controller, 2010 8th IEEE International Conference on Control and
Automation, Xiamen, China, June 9-11, pag. 460-464, 2010.
[12] Chwa D., Seo J. H., Kim P., and Choi J. Y., Sliding mode tracking control of nonholonomic
wheeled mobile robots, Proceedings of the American Control Conference, Anchorage, pag
3991-3996, mai, 2002.
[13] Chwa D., Sliding-mode tracking control of nonholonomic wheeled mobile robots in polar
coordinates, IEEE Transactions on Control Systems Technology, vol. 12, nr. 4, iulie, pag. 637644, 2004.

135

[14] Corradini M. L., Leo T., Orlando G., Variable structure control of robotic asistance system,
Proceedings of the 5th IEEE Mediterranean Conference on Control and Systems,
http://med.ee.nd.edu/MED5/PAPERS/052/052.PDF.
[15] Danwei W. and Feng Qi, Trajectory planning for a four-wheel steering vehicle, Proceedings of
the IEEE International Conference on Robotics and Automation, Seoul, pag. 3320-3325, 2001.
[16] Dumitrascu B. and Filipescu A., Discrete-time sliding-mode controller for wheeled mobile
robots, Proceedings of the 18th International Conference on Control Systems and Computer
Science, vol. 1, Bucuresti, pag. 397-403, mai, 2011.
[17] Dumitrascu B. and Filipescu A., Sliding mode control of lateral motion for four driving-steering
wheels autonomous vehicle, Annals of the University of Craiova, vol. 7 (34), pag 20-25, 2010.
[18] Dumitrascu B. and Filipescu A., Sliding mode controller for steering of four-wheels driving and
steering vehicle, Proceedings of the 14th International Conference on System Theory and
Control, pag. 202-206, Sinaia, 2010.
[19] Dumitrascu B., Filipescu A., Backstepping control of wheeled mobile robots, Proceedings of the
15th International Conference on System Theory and Control, Sinaia, pag. 206-211, 2011.
[20] Dumitrascu B., Filipescu A., Radaschin A., Filipescu A. Jr., Minca E., Discrete-time sliding
mode control of wheeled mobile robots, Proceedings of 2011 8th Asian Control Conference
(ASCC) ,Kaohiung, Taiwan, pag. 771-776, mai , 2011.
[21] Dumitrascu B., Filipescu A., Vasilache C., Minca E., Filipescu A. Jr., Discrete-time sliding-mode
control of four driving/steering wheels mobile platform, Proceedings of the 19th Mediterranean
Conference on Control and Automation, Corfu, Grecia, iunie, 2011, pag. 1076-1081.
[22] Dumitrascu B., Filipescu A., Minzu V., Voda A., Minca E.,Discrete-Time Sliding-Mode Control
of Four Driving-Steering Wheels Autonomous Vehicle, Proceedings of the 30th Chinesse Control
Conference, pag. 3620-3625, 2011.
[23] El-Sheimy N., Hou H., Niu Xiaoji, Analysis and modelling of inertial sensors using Allan
variance, IEEE Transactions on Instrumentation and Measurement, vol. 57, nr. 1, pag.140-149,
ianuarie, 2008.
[24]Filipescu A., Minzu V., Dumitrascu B. and Filipescu A., Trajectory-tracking and discrete-time
sliding-mode control of wheeled mobile robots, The 2011 IEEE International Conference on
Information and Automation, iunie, 2011.
[25] Flenniken W.S., Wall, J. H., Bevly D.M., Characterization of various IMU error sources and the
effect

on

navigation

performance,

http://www.eng.auburn.edu/~dmbevly/

gavlab/pub-

pre/Flenniken_ION_GNSS_2005.pdf.
[26] Foka A., Trahanias P., Real-time hierarchical POMDPs for autonomous robot navigation,
Robotics and Automatonomous Systems, pag. 561-571, vol. 55, 2007.
[27] Fraichard T. And Scheuer A., From Reeds and Shepps to continuous-curvature paths, IEEE
Transactions On Robotics and Automation, vol. 20, nr. 6,pag 1025-1035, 2004.
136

[28] Frazzoli E., Robust Hybrid Control for Autonomous Vehicle Motion Planning,Massachusetts
Institute of Technology, iunie, 2001.
[29] Freeman J. A., Skapura D. M., Neural networks algorithms, applications, and programming
techniques, Loral Space Information Systems and Adjunct Faculty, School of Natural and
Applied Sciences University of Houston at Clear Lake, 1991.
[30] Fukao T., Nakagawa H., Adachi N., Adaptive tracking control of a nonholonomic mobile robot,
IEEE Transactions on Robotics and Automation, vol. 16, nr.5, pag. 609-615, 2000.
[31] Gao W. and. Hung J. C, Variable structure control of nonlinear systems: A new approach, IEEE
Transactions on Industrial Electronics, 40(1), pag. 45 - 55, 1993.
[32] Gao W., Wang Y. and Homaifa A., Discrete-time variable structure control systems, IEEE
Transaction on Industrial Electronics, vol. 42, 117-122, 1995.
[33] Garcia M. A. P., Montiel O., Castillo O., Sepulveda R., Melin P., Path palanning for autonomous
mobile robot navigation with ant colony optimization and fuzzy cost function evaluation,
Applied Soft Computing, vol.9 , pag 1102-1110, 2009.
[34] Ge S. S., Lai X.-C., Mamun A. Al., Sensor-based path planning for nonholonomic mobile robots
subject to dynamic constraints, Robotics and Autonomous Systems, vol. 57, pag. 513-526, 2007.
[35] Grewal M.S., Andrews A. P., Kalman filtering: theory and practice using MATLAB, second
edition, John Wiley & Sons, Inc., New York, 2001.
[36] Henderson T. C., Minor M., Drake S., Hetrick A., Quist J., Roberts J., Sani H., Bandaru R.,
Rasmussen M., Collins A., Sun Y. ,Suraj, Xiuyi Fan, Ed St. Louis, Shigenori Mikuriya, Ken
Dean, Robust autonomous vehicles,University of Utah, June 2007.
[37]Hou H., Modeling inertial sensors errors using Allan variance. Phd Thesis, University of Calgary,
2004.
[38] http://en.wikipedia.org/wiki/TerraMax_(vehicle)
[39] http://www.mobilerobots.com/software/mobilesim.aspx
[40] http://news.xinhuanet.com/english/2010-01/18/content_12829542_1.htm
[41] http://photojournal.jpl.nasa.gov/catalog/PIA01122
[42] http://store.irobot.com/home/index.jsp
[43] http://viac.vislab.it/
[44] http://www.cs.iastate.edu/~cs577/handouts/quaternion.pdf
[45] http://www.husqvarna.com/us/homeowner/products/robotic-mowers/husqvarna-robotic-mowersfor-homeowners/
[46 ] http://www.tigertek.com/servo-motor-resources/incremental-absolute-encoders.html.
[47] Hung J. Y., Gao W., Hung J. C., Variable structure control: A survey, IEEE Transactions on
Industrial Electronics, vol. 40(1), pag.2-22,1993.

137

[48] IEEE standard, Specification format guide and test procedure for single-axis interferometric fiber
optic gyros, IEEE-SA Standards Board.
[49] Jalili-Kharaajoo M. and Moshiri B., Discrete-time sliding mode control for linear time-varying
systems with application to steering control on highway vehicles, University of Tehran, Iran.
[50] Jeffrey Roderick Norman Forbes, Reinforcement Learning for Autonomous Vehicles,B.S.
(Stanford University), 1993.
[51] Jiang J.P. and Nijmeijer H., Backstepping-based tracking control of nonholonomic chained
systems, European Control Conference, Brussels, iulie, 1997.
[52] Jiang J.P. and Nijmeijer H., Tracking control of mobile robots: a case study in backstepping,
Automatica, vol. 33, nr. 7, pag. 1393-1399, 1997.
[53] Joan Sola, Quaternion Kinematics for the error-state Kalman filter, http://www.joansola.eu/
JoanSola/objectes/notes/kinematics.pdf, august, 2012.
[54] Joao Luis Marins, Xiaoping Yun, Eric R. Bachman, Robert B. McGhee, Michael J. Zyda, An
extended Kalman filter for quaternion-based orientation estimation using MARG sensors,
IEEE/RSJ International Conference on Intelligent Robots and Systems, Maui, USA,octombrie,
2001.
[55] Kanarat A., Motion Planning and robust control for nonholonomic mobile robots under
uncertainties, PhD Thesis, Virginia Polytechnic Institute and State University, Blacksburg,
Virginia, 2004.
[56] Kanayama Y., Kimura Y., Miyazaki F. and Noguchi T., A stable tracking control scheme for an
autonomous mobile robot, Proceedings of the IEEE International Conference on Robotics and
Automation, pag. 384-389,1990.
[57] Khalil H.K., Nonlinear Systems, Second Edition, Prentice Hall, New Jersey, 1996.
[58] Khatib O., Real-time obstacle avoidance for manipulators and mobile robots, IEEE International
Conference on Robotics and Automation, pag. 500-505, martie, 1985.
[59] Kimberly Tuck, Tilt sensing using linear accelerometers, Freescale Semiconductors Application
Note, 2007.
[60] Kong X., Inertial navigation system algorithms for low cost IMU, PhD Thesis, University of
Sydney, 2007.
[61] Kumar U. and Sukavanam N., Backstepping based trajectory tracking control of a four wheeled
mobile robot, International Journal of Advanced Robotic Systems, Vol..5, No.4 , pag 403410,2008.
[62] Lee H., Chattering suppression in sliding mode control system, PhD Thesis M. S. Ohio State
University, 2007.
[63] Lee J. H, Lin C., Lim H. and Jang Myung Lee, Sliding mode control of mobile robot in the RFID
sensor space, International Journal of Control, Automation and Systems, pag 429-435,2009.

138

[64] Luca Caracciolo, Alessandro De Luca Stefano Iannitti, Trajectory Tracking Control of a FourWheel Differentially Driven Mobile Robot, Proceedings of the 1999 IEEE International
Confererence on Robotics & Automation, Detroit, Michigan, May 1999.
[65] Lucet E. , Grand C., Salle D., Bidaud P., Dynamic sliding mode control of a four-wheel skidsteering vehicle in presence of sliding, http://www.doc-center.robosoft.com/@api/ deki/
files/3002/=Dynamic_sliding_mode_control_of_a_four-wheel_skidsteering_vehicle_in_presence_of_sliding.pdf.
[66] Lumelsky V., Skewis T., Incorporating range sensing in the robot navigation function, IEEE
Transactions on Systems, Man, and Cybernetics, vol. 20, nr. 5, pag. 1058-1068, 1990.
[67] Madgwick S. O.H., Harrison A. J.L., Vaidyanathan R., Estimation of IMU and MARG
orientation using a gradient descent algorithm, 2011 IEEE International Conference on
Rehabilitation Robotics, Zurich, Elvetia, pag. 1-7 ,2011.
[68] Malcolm D. Shuster, The quaternion in Kalman filtering, AAS/AIAA Astrodynamics
Conference, British Columbia, august 1993, Advances in the Astronautical Sciences, vol 85, pag.
25-37, 1993.
[69] Manalov O. B., Adaptive steering control for autonomous mobile robots, Proceedings of the 10th
Mediterranean Conference on Control and Automation- Med2002, Portugalia, 2002.
[70] Maxim Likhachev, Dave Ferguson, Planning long dynamically-feasible maneuvers for
autonomous vehicles, http://www.cs.cmu.edu/~maxim/files/planlongdynfeasmotions _rss08.pdf.
[71] Menegatti E., Maeda T., Ishiguro H., Image-based memory for a robot navigation using
properties of omnidirectional images, Robotics and Autonomous Systems, vol. 47 , pag. 251267, 2004.
[72] Mnif F., Recursive backstepping stabilization of a wheeled mobile robot, International Journal
of Advanced Robotic Systems, vol. 1 nr. 4 , pag. 287 294,2004.
[73] Momcilovic O. I., Discrete-time variable structure controller synthesis for third order objects
with finite zero using delta transform, Journal of Electrical Engineering, vol. 55, 3-4, pag . 7176, 2004.
[74] Myungsoo Jun, Atif I. Chaudhry, Raffaello DAndrea, The navigation of autonomous vehicles in
uncertain dynamic environments: a case study, Sibley School of Mechanical and Aerospace
Engineering, Cornell University Ithaca, NY 14853-7501, USA.
[75] Naveen K. Boggarpu, Richard C. Kavanagh, New learning algorithm for high-quality velocity
measurement from low-cost optical encoders, IMTC 2008- IEEE Instrumentation and
Measurement Technology Conference, Victoria, Canada, mai, 2008.
[76] Nassar S., Improving the inertial navigation system (INS) error model for INS and INS/DGPS
applications, PhD Thesis, The University of Calgary, 2003.

139

[77] Park M., Error analysis and stochastic modeling of MEMS based inertial sensors for land vehicle
navigation applications, Master Thesis University of Calgary, 2004.
[78] Petrella R., Tursini M., Peretti L., Zigliotto M., Speed measurement algorithms for lowresolution incremental encoder equipped drives: a comparative a analysis, International Aegean
Conference on Electrical Machines and Power Electronics,Bodrum, Turcia, septembrie, 2007.
[79] Philips W. F., Hailey C.E., and Gebert G.A., Review of attitude representations used for aircraft
kinematics, Journal of Aircraft, 38(4), pag. 718-737, 2001.
[80] Qiang Z., Zengbo L., Yao C., A Back-stepping Based Trajectory Tracking Controller for a Nonchained Nonholonomic Spherical Robot, Chinese Journal of Aeronautics 21, pag. 472-480,
2008.
[81] Qu S., Wang Y., Zhu Q., A discrete sliding variable structure control with dynamic disturbance
compensator, Control 2004, University of Bath, Uk, 2004.
[82] Ren H. And Kazanzides P., Investigation of attitude tracking using an integrated inertial and
magnetic navigation system for hand-held surgical instruments, IEEE/ASME Transactions on
Mechatronics, vol. 17, nr.2, pag. 210-217, 2012.
[83] Rubio J. De Jesus and Yu W., A new discrete-time sliding-mode control with time-varying gain
and neural identification, International Journal of Control, vol. 79, nr. 4, pag. 338348, aprilie,
2006.
[84] Salah Sukkarieh, Low cost, high integrity, aided inertial navigation systems for autonomous land
vehicles, PhD thesis, 2000.
[85] Schouwenaars T., Safe trajectory planning of Autonomous Vehicles, PhD Thesis, Massachusetts
Institute of Technology, 2006.
[86] Sebastian O. H. Madwick, Quaternions, An efficient orientation filter for inertial and
inertial/magnetic sensor arrays, aprilie, 2010.
[87] Seekur Quick Start, Mobile Robots Inc., 2007.
[88] Shaocheng Q., Yongji W., Robust sliding mode control for uncertain discrete time systems,
Journal of Chongqing University-Eng. Ed. vol. 2, nr. 2, pag. 51-54, 2003.
[89] Shi Hongli, A novel scheme for the design of backstepping control for a class of nonlinear
systems, Applied Mathematical Modelling 35 (2011) 18931903.
[90] Slotine J.J.E. and Li W., Applied nonlinear control. Prentice-Hall, London, 1991.
[91] Solea R., Filipeascu A., Stamatescu G., Trajectory-tracking sliding-mode control for wheeled
mobile robots, Energy, Transport and Envinronment Control Applications- International
Workshop, pag 121-134, mai, 2009.

140

[92] Solea R., Filipescu A. and Stamatescu G., Sliding-mode real-time mobile platform control in the
presence of uncertainties, Proceedings of the 48th IEEE Conference on Decision and Control
and 28th Chinese Control Conference, Shanghai, pag. 7747-7752,2009.
[93] Solea R., Filipescu A., Filipescu S, and Dumitrascu B., Sliding-mode controller for four- wheelsteering vehicle: trajectory-tracking problem, Proceedings of the 8th World

Congress on

Intelligent Control and Automation, Jinan, China, pag. 1185-1190, 2010.


[94] Solea R., Filipescu A., Manzu V., Filipescu S., Sliding-mode trajectory-tracking control of a four
wheel steering vehicle, 2010 8th IEEE International Conference on Control and Automation,
Xiamen, China, pag. 382-387,2010.
[95] Solea R., Filipescu A., Nunes U., Sliding-mode control for trajectory-tracking of a wheeled
mobile robot in presence of uncertainties, Proceedings of the 7th Asian Control Conference,
Hong Kong, pag. 1701-1706,2009.
[96] Solea R., Sliding mode control applied in trajectory-tracking of WMRs and autonomous vehicles,
PhD Thesis, University of Coimbra, Portugal, 2009.
[97] Susnea I., Filipescu A., Vasiliu G., Coman G., Radaschin A., The buble rebound obstacle
avoidance algorithm for mobile robots, in Proceedings. 8th IEEE International Conference on
Control and Automation, Xiamen, pag. 540-545, 2010.
[98] Tanner H. G., Kyriakopoulos K. J., Backstepping for nonsmoth systems, Automatica, 39, pag
1259-1265, 2003.
[99] Totha C. K., Paska E., Mobile mapping and autonomous vehicle navigation, Department of Civil
and Environmental Engineering and Geodetic Science, Columbus, USA.
[100] Tsang, Chi Chu, Error reduction techniques for a MEMS accelerometer-based digital input
device, Master Thesis, University of Hong Kong, 2008.
[101] Utkin V.I., Guldner J., and Shi J., Sliding mode control in electromechanical systems, Taylor &
Francis, London, 1999.
[102] Utkin V.I., Sliding modes in optimization and control, Springer-Verlag, New York, 1992.
[103] Vecchio C., Sliding mode control: theoretical developments and applications to uncertain
mechanical systems, PhD Thesis, Universita Degli Studi Di Pavia.
[104] Velagic J., Lacevic B., Perunicic B., A 3-level autonomous mobile robot navigation system
designed by using reasoning/ search approaches, Robotics and Systems, vol. 54, 989-1004,2006.
[105] Vivekanandan C., Prabhakar R., A redefined discrete quasi-sliding mode strategy, International
Journal of Recent Trends, vol. 1, nr. 3, pag 92-96, 2009.
[106] Vivekanandan V., Prabhakar R. and Gnanambigai M., A redefined quasi-sliding mode control
strategy, World Academy of Science, Engineering and Technology, 39, pag 292-295, 2008.
[107] Woodman O.J., An introduction to inertial navigation, Raport tehnic(696), Computer
Laboratory of Cambridge, 2007.
141

[108] Xiao L., Su H., Zhang X., Chu J., A new discrete variable structure control algorithm based on
sliding mode prediction, 2005 American Control Conference, Portland, USA, pag 4643-4648,
2005.
[109] Xiaoping Yun, Bachman E.R., McGhee R. B. and Life Fellow, A simplified quaternion-based
algorithm for orientation estimation from Earth gravity and magnetic field measurements,
Instrumentation, 57(3), pag. 638-650,2008.
[110] Xiaoping Yun, Mariano Lizarraga, Eric R. Bachman, Robert B. McGhee, An improved
quaternion-based Kalman filter for real-life tracking of rigid body orientation, IEEE/RSJ
International Conference on Intelligent Robots and Systems, octombrie, 2003.
[111] Xin Zhang, Yong Li, Peter Mumford, and Chris Rizos, Allan variance analysis on error
characters of MEMS inertial sensors for an FPGA-based GPS/INS system, International
Symposium on GPS/GNSS 2008, Tokyo, Japonia, 2008.
[112] Yang K. and Sukkarieh S., An analytical continuous-curvature path-smoothing algorithm, IEEE
Transactions on Robotics, vol. 26, nr. 3, pag. 561-568, 2010.
[113] Young Soo Suh, Orientation estimation using a quaternion-based indirect Kalman filter with
adaptive estimation of external acceleration, IEEE Transactions on Instrumentation and
Measurement, 59(12), pag. 3296-3305, decembrie, 2010.
[114] Yuan J., Tang G.-Y., Finite-time tracking control algorithms based on variable structure for
mobile robots, Proceedings of the 29th Chinese Control Conference, Beijing, China, pag. 419423, Iunie , 2010.
[115] Yuan Z. P., Wang Z. P. and Chen Q. J., Trajectory tracking control of a nonholonomic mobile
robot, 2010 8th IEEE International Conference on Control and Automation , Xiamen, China,
pag. 2207-2211, iunie, 2010.
[116] Zhen Ziyang, Wang Zhisheng, Hu Yong, Multi-sensor information fusion for aircraft attitude
determination system, 2009 WRI World Congress on Computer Science and Information
Engineering, pag. 474-478, 2009.
[117] Zhou1 B., Han J., Dai X., Backstepping Based Global Exponential Stabilization of a Tracked
Mobile Robot with Slipping Perturbation, Journal of Bionic Engineering, vol. 8 , pag. 69
76,2011.

142

8.

Anexe
n anexe sunt prezentate programele sau funciile folosite pentru testarea metodelor de

conducere i fuziunea datelor.

Anexa 1. Programul folosit la conducerea backsteppping


Este prezentat programul folosit la testarea conducerii backstepping a vehiculului.
#include "Aria.h"
#include "Velocity.h"
// Press escape to shut down Aria and exit.
static double angleLimit(double angle)
{
if (angle > 3.14)
{ printf("\t %4.5f \n", "Angle");
angle = angle - 2*3.14;}
else
if (angle < -3.14){
angle = angle + 2*3.14;
printf("\t %4.5f \n", "Angle");}
return(angle);
}
/* ------------- SIGN & SATURATION FUNCTION -------------- */
double sign(double value){
if (value<0)
return(-1.00);
else
if (value>0)
return(1.00);
else
return(1.00);
}
double satur(double value){
if (value<-1)
return (-1.00);
else if (value > 1)
return (1.00);
else
return (value);
}
/* ------------ END (SIGN & SATURATION FUNCTION) ------------- */
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;

143

ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);
// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 11000, 400);
//ArActionLimiterForwards limiterFarAction("speed limiter far", 600, 2200, 800);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Parse all command line arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Connect to the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
//robot.comInt(ArCommands::SOUNDTOG, 0);
bool first = true;
int goalNum = 0;
/*
while (Aria::getRunning())
{
robot.lock();
*/
FILE *date1;
/* double v, av, w, aw;
/* velocity to drive left wheel*/
double theta_des_next;
/* theta desired */
double ref_xr, ref_yr, ref_Th, ref_w, ref_v, x_des_next, y_des_next;
/* desired position */
/
int i; // n;
double v,av,w,aw;
double x,y,theta;
double xd, yd, theta_d;

144

double v_c, w_c;


double Ts;
double e1, e2, e1_der, e2_der;
double alfa;
double k1, k2, k3;
double timp;
double v_l, v_r;
//------------------------------------double lungime=240;
date1=fopen("d:/test/test1.txt","w+");
timp=0.0;
Ts=0.1;
k1=0.5;
k2=1.25;
k3=0.3;
ref_xr = 0.0;
ref_yr = 0.0;
ref_Th = 0.0;
ref_v = 0.0;
ref_w = 0.0;
//gama0 = gama_y/(3.14/2);
// n = lungime-1;
// n = 3;
robot.lock();
ArUtil::sleep(100);
robot.unlock();
ArUtil::sleep(100);
for (i=1; i<lungime; i++){
robot.lock();
v=velocity[i][0];
av=velocity[i][1];
w=velocity[i][2];
aw=velocity[i][3];

theta_des_next = angleLimit(Ts*w+ref_Th);
x_des_next = Ts*v*cos(ref_Th) + ref_xr;
y_des_next = Ts*v*sin(ref_Th) + ref_yr;

x=(robot.getX()/1000);
y=(robot.getY()/1000);
theta=robot.getTh()*(3.14/180);
e1=x-ref_xr;
e2=y-ref_yr;
e1_der=cos(theta)*(robot.getVel()/1000)-v*cos(ref_Th);
e2_der=sin(theta)*(robot.getVel()/1000)-v*sin(ref_Th);

145

//v_c_der = (temp1 + (robot.getVel()/1000)*theta_e_der*sin(theta_e))/(cos(theta_e));


v_c = (-k1*e1+v*cos(ref_Th))/cos(theta);
// v_c = (-k1*e1+v*cos(ref_Th))/cos(theta)*Ts+ref_v;
alfa=tan(theta)*(v*cos(ref_Th)-k1*e1)-v*sin(ref_Th)+k2*e2;
//w_c =(cos(theta)/v_c)*(tan(theta)*(v*cos(ref_Th)-k1*k1*e1)-v*sin(ref_Th)-k2*k2*e2k2*alfa+k3*alfa);
w_c =(cos(theta)/v_c)*(tan(theta)*(av*cos(ref_Th)-v*ref_Th*sin(ref_Th)-k1*k1*e1)-av*sin(ref_Th)v*ref_Th*cos(ref_Th)-k2*k2*e2-k2*alfa+k3*alfa);
// ROBOT
v_l = v_c + 0.28*w_c;
v_r = v_c - 0.28*w_c;
//
//
//

//0.19 //0.28 //0.24

Simulator
v_r = v_c + 0.19*w_c;
v_l = v_c - 0.19*w_c;

// setVel2 (double leftVelocity, double rightVelocity)


robot.setVel2(1000*v_l, 1000*v_r);
// printf("\t %4.5f \t %4.5f \t %4.5f %4.5f \t %4.5f \n", robot.getX()/1000, robot.getY()/1000,
robot.getTh()*(3.14/180), v_r, v_l);
// printf("\t %4.5f \t %4.5f \t %4.5f \t %4.5f \n", robot.getVel()/100,
robot.getRotVel()*3.14/180, v_c, w_c);
fprintf(date1," %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t
%4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \n",
robot.getX()/1000, robot.getY()/1000, robot.getTh()*(3.14/180), ref_xr, ref_yr, ref_Th, e1, e2,
(robot.getVel()/100), (robot.getRotVel()*3.14/180), v, w, v_c, w_c, timp);
printf("\t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \n", e1, e2,
w,w_c,robot.getRotVel()*3.14/180);
// printf("\t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \n", ref_xr, ref_yr, ref_Th*(180/3.14),
robot.getRobotDiagonal()/1000, robot.getRobotRadius()/1000);
ref_xr = x_des_next;
ref_yr = y_des_next;
ref_Th = theta_des_next;
ref_w = w_c;
ref_v = v_c;
timp=timp+Ts;
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected, shut down
Aria::shutdown();
return 0;
}

146

Anexa 2. Programul folosit la conducerea sliding-mode


Este prezentat programul folosit la testarea conducerii sliding-mode a vehiculului.
#include "Aria.h"
static double angleLimit(double angle)
{
if (angle > 3.14)
angle = angle - 2*3.14;
else
if (angle < -3.14)
angle = angle + 2*3.14;
return(angle);
}
double fi(double value1, double value2){
if(abs(value1)>value2)
value1=1;
else value1=0;
return (value1);
}
double psi(double value1,double value2){
if(abs(value1)>value2)
value1=1;
else value1=(abs(value1)*abs(value1))/value2;
return (value1);
}
/* ------------- SIGN & SATURATION FUNCTION -------------- */
double sign(double value){
if (value<0)
return(-1.00);
else
if (value>0)
return(1.00);
else
return(1.00);
}
double satur(double value){
if (value<-1)
return (-1.00);
else if (value > 1)
return (1.00);
else
return (value);
}
/* ------------ END (SIGN & SATURATION FUNCTION) ------------- */
int main(int argc, char **argv)
{
Aria::init();
ArArgumentParser parser(&argc, argv);
parser.loadDefaultArguments();
ArSimpleConnector simpleConnector(&parser);
ArRobot robot;
ArSonarDevice sonar;
ArAnalogGyro gyro(&robot);
robot.addRangeDevice(&sonar);

147

// Make a key handler, so that escape will shut down the program
// cleanly
ArKeyHandler keyHandler;
Aria::setKeyHandler(&keyHandler);
robot.attachKeyHandler(&keyHandler);
printf("You may press escape to exit\n");
// Collision avoidance actions at higher priority
ArActionLimiterForwards limiterAction("speed limiter near", 300, 600, 250);
ArActionLimiterForwards limiterFarAction("speed limiter far", 300, 11000, 400);
//ArActionLimiterForwards limiterFarAction("speed limiter far", 600, 2200, 800);
ArActionLimiterTableSensor tableLimiterAction;
robot.addAction(&tableLimiterAction, 100);
robot.addAction(&limiterAction, 95);
robot.addAction(&limiterFarAction, 90);
// Goto action at lower priority
ArActionGoto gotoPoseAction("goto");
robot.addAction(&gotoPoseAction, 50);
// Parse all command line arguments
if (!Aria::parseArgs() || !parser.checkHelpAndWarnUnparsed())
{
Aria::logOptions();
exit(1);
}
// Connect to the robot
if (!simpleConnector.connectRobot(&robot))
{
printf("Could not connect to robot... exiting\n");
Aria::exit(1);
}
robot.runAsync(true);
// turn on the motors, turn off amigobot sounds
robot.enableMotors();
//robot.comInt(ArCommands::SOUNDTOG, 0);
bool first = true;
int goalNum = 0;
/*
while (Aria::getRunning())
{
robot.lock();
*/
FILE *date1;
double v, av, w, aw;
double theta_des_next;
double ref_xr, ref_yr, ref_Th, ref_w, ref_v, x_des_next, y_des_next;
double x_e, y_e, theta_e, x_e_der, y_e_der, theta_e_der;
double s_1, s_2, Q1, Q2, P1, P2, gama0, gama_x, gama_y;
double v_c, v_c_der, w_c, temp1, temp2;
double Ts, v_l, v_r, timp, alpha;
int i; // n;
//------------------------------------date1=fopen("d:/ccc/testcc8.txt","w+");

148

timp=0.0;
Ts=0.1;
Q1 = 0.05;
Q2 = 0.5;
P1 = 0.5;
P2 = 0.75;
alpha = 0.5;
gama0 = 30;
//20
gama_x = 1.25; //1.25
gama_y = 100;

//15

ref_xr = 0.0;
ref_yr = 0.0;
ref_Th = 0.0;
ref_v = 0.0;
ref_w = 0.0;
robot.lock();
ArUtil::sleep(100);
robot.unlock();
ArUtil::sleep(1000);
for (i=1; i<400; i++){
robot.lock();
v=0.5;
av=0;
w=0;
aw=0;
if(i>200){
w=0.2;}
/*

for (i=1; i<400; i++){


robot.lock();
v=0.5;
av=0;
w=0;
aw=0;
if(i>200){
w=-0.2;}
*/
theta_des_next = angleLimit(Ts*w+ref_Th);
x_des_next = Ts*v*cos(ref_Th) + ref_xr;
y_des_next = Ts*v*sin(ref_Th) + ref_yr;
x_e = ((robot.getX()/1000)-ref_xr)*cos(ref_Th)+((robot.getY()/1000)-ref_yr)*sin(ref_Th);
y_e = -((robot.getX()/1000)-ref_xr)*sin(ref_Th)+((robot.getY()/1000)-ref_yr)*cos(ref_Th);
theta_e = angleLimit((robot.getTh()*(2.14/180))-ref_Th);
if (y_e < 0)
{
gama_y = -abs(gama_y);
}
else if( y_e > 0 )
{
gama_y = abs( gama_y );

149

}
x_e_der = -v + (robot.getVel()/1000)*cos(theta_e)+y_e*w;
y_e_der = (robot.getVel()/1000)*sin(theta_e)-x_e*w;
theta_e_der = (robot.getRotVel()*3.14/180)-w;
/* SLIDING SURFACE */
s_1 = x_e_der + gama_x*x_e;
s_2 = y_e_der + gama_y*y_e + gama0*sign(y_e)*(theta_e);
temp1 = -Q1*satur(s_1/0.5) - (P1*s_1) - gama_x*x_e_der - (aw*y_e) - w*y_e_der + av;
temp2 = -Q2*satur(s_2/0.5) - (P2*s_2) - gama_y*y_e_der + (aw*x_e) + w*x_e_der;
v_c_der = (temp1 + (robot.getVel()/1000)*theta_e_der*sin(theta_e))/(cos(theta_e));
v_c = (Ts*v_c_der) + ref_v;
w_c
=
v_c_der*sin(theta_e))/((robot.getVel()/1000)*cos(theta_e)+gama0*sign(y_e))+w;
// ROBOT
v_l = v_c + 0.4*w_c;
v_r = v_c - 0.4*w_c;

(temp2-

//0.19 //0.28 //0.24

robot.setVel(1000*v_c);
robot.setRotVel(w*180/3.14);
fprintf(date1," %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t
%4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \n",
robot.getX()/1000, robot.getY()/1000, robot.getTh()*(2.14/180), ref_xr, ref_yr, ref_Th, x_e, y_e, theta_e,
x_e_der, y_e_der, theta_e_der, (robot.getVel()/1000), (robot.getRotVel()*3.14/180), v, w, v_c, w_c, s_1, s_2,
timp);
printf("\t %4.5f \t %4.5f \t %4.5f \t %4.5f \t %4.5f \n", x_e, y_e, theta_e, robot.getVel()/1000,
w);
ref_xr = x_des_next;
ref_yr = y_des_next;
ref_Th = theta_des_next;
ref_w = w_c;
ref_v = v_c;
timp=timp+Ts;
robot.unlock();
ArUtil::sleep(100);
}
// Robot disconnected, shut down
Aria::shutdown();
return 0;
}

150

Anexa 3. Programul principal folosit la estimarea atitudinii


Este prezentat programul principal scris n MATLAB, folosit la fuziunea datelor de la senzorul
IMU Xsens pentru determinarea atitudinii:
clc
clear all
%Log=OpenAndProcessLogs4('log-18_9_29_3_2012_front_back_LITcars.txt');
Log=OpenAndProcessLogs4('log-18_0_29_3_2012_firetruck_front.txt');
Acc=[Log.IO.Xsens_acc_X Log.IO.Xsens_acc_Y Log.IO.Xsens_acc_Z]';
Acc(3,:)=Acc(3,:);
Gyro=[Log.IO.Xsens_gyro_X Log.IO.Xsens_gyro_Y Log.IO.Xsens_gyro_Z]';
Mag=[Log.IO.Xsens_compass_X Log.IO.Xsens_compass_Y Log.IO.Xsens_compass_Z]';
Calibrated=[Log.IO.Xsens_quaternion_X Log.IO.Xsens_quaternion_Y Log.IO.Xsens_quaternion_Z
Log.IO.Xsens_quaternion_W]';
r=eulr2qua([0 0 pi/2]);
for i=1:length(Calibrated)
%Calibrated(:,i)=quaternionmul(Calibrated(:,i)',r);
xsens(:,i)=quaternion2euler(Calibrated(:,i));
end
R=[0 1 0; -1 0 0; 0 0 1];
ym=Mag;
%ym(1,:)=Mag(1,:);
%ym(2,:)=Mag(2,:);
%ym(3,:)=Mag(3,:);
yg=Gyro;
%yg(1,:)=Gyro(1,:);
%yg(2,:)=Gyro(2,:);
%yg(3,:)=Gyro(3,:);
ya=Acc;
%ya(1,:)=Acc(1,:);
%ya(2,:)=Acc(2,:);
%ya(3,:)=-Acc(3,:);
tt=0:.1:(length(ya)-1)*0.1;
D2R = pi/180;
R2D = 180 / pi;
Rg = 1e-4 * eye(3);
Ra = 1e-2 * eye(3);
Rm = 1e-3*eye(3);
[q4, eulercom4, bahat, bghat, bmhat] = computeorientation (yg,ya,ym,tt,Rg,Ra,Rm);
%
% estimation result
%
figure(3)
title('Pitch')
plot(tt,xsens(2,:)*R2D,'r--',tt,eulercom4(2,:)*R2D);
ylabel('pitch \phi (deg)');
xlabel('time (sec)');

151

legend('true value','estimated value')


figure(4)
plot(tt,xsens(1,:)*R2D,'r--',tt,eulercom4(1,:)*R2D);
title('Roll');
ylabel('roll \theta (deg)');
xlabel('time (sec)');
legend('true value','estimated value')
figure(5)
plot(tt,xsens(3,:)*R2D,'r--',tt,eulercom4(3,:)*R2D);
ylabel('yaw \psi (deg)');
xlabel('time (sec)');
title('Heading');
legend('true value','estimated value')

152

Anexa 4. Funcia pentru calculul atitudinii utiliznd filtrul Kalman


Funcia MATLAB computerorientation returneaz atitudinea reprezentat n quaternion (q4),
atitudinea reprezentat n unghiuri Euler(eulercom4) i biasurile estimate utiliznd filtrul Kalman.
Funcia primete ca parametri datele de la giroscoape(yg), datele de la accelerometre(ya), datele de la
senzorii magnetici(ym), vectorul timp(tt) i matricile de covarian corespunztoare senzorilor.
function [q4, eulercom4, bahat, bghat, bmhat] = computeorientation (yg,ya,ym,tt,Rg,Ra,Rm)
D2R = pi/180;
% number of data: N (>= 3)
N = max(size(ya));
% gravitation acceleration
g = 9.8;
gtilde = [ 0 ; 0 ; g];

% Magnetic Inclination angle at COIMBRA (degree)


inc = (54+49/60) * D2R;
% Magnetic Declination angle at COIMBRA (degree)
dec=-3 * D2R;
mtilde = [cos(inc)*cos(dec);cos(inc)*sin(dec); -sin(inc) ];
% dip angle at Ulsan (degree)
%alpha = 50 * D2R;
%mtilde = [ cos(alpha) ; 0 ; -sin(alpha) ];
% bias update covariance
Qbg = eye(3) * 1e-8; %allan plot bias instabilitty for gyro 10^-4
Qba = eye(3) * 9e-8; %allan plot bias instabilitty for accelerometer 3*10^-4
Qbm = eye(3) * 1e-10; %allan plot bias instabilitty for magnet 10^-4 Xsense
Qbg0 = eye(3)* 1e-4; %initial covariance
Qba0 = eye(3)* 1e-4; %initial covariance
Qbm0 = eye(3)* 1e-5; %initial covariance
% -------------------------------------------------------% Filtrul Kalman
% -------------------------------------------------------% q4 : quaternion
q4 = zeros(4,N);
% eulercom4 : euler angles
eulercom4 = zeros(3,N);
bghat = zeros(3,1);
bahat = zeros(3,1);
bmhat = zeros(3,1);
yabar = ya(:,1) / norm(ya(:,1));
ymbar = ym(:,1) / norm(ym(:,1));
f1 = cross(yabar,ymbar) / norm( cross(yabar,ymbar) );
C = [ -cross(yabar,foo1) , f1 , yabar ] ;
q4(:,1) = dcm2quaternion(C);

153

x = zeros(12,1);
P = zeros(12,12);
Q = zeros(12,12);
Q(1:3,1:3) = 0.025 * Rg
Q(4:6,4:6) = Qbg;
Q(7:9,7:9) = Qba;
Q(10:12,10:12) = Qbm;
% intial error covariance
P(1:3,1:3) = 0.01*eye(3);
P(4:6,4:6) = Qbg0;
P(7:9,7:9) = Qba0;
P(10:12,10:12) = Qbm0;
% A matrix
A = zeros(12,12);
A(1:3,4:6) = -0.5 * eye(3);
% initializarea quaternionului pt integrare
wx = yg(1,1);
wy = yg(2,1);
wz = yg(3,1); %
oldomega4 =[ 0 ,-wx ,-wy ,-wz ;
wx , 0 , wz ,-wy ;
wy ,-wz , 0 , wx ;
wz , wy ,-wx , 0];

% variabile folosite in algorimul de adaptare


r2count = 100;
% H1 and H2 for measurement update
H1 = zeros(3,12);
H1(1:3,7:9) = eye(3);
H2 = zeros(3,12);
H2(1:3,10:12) = eye(3);
% parametrii pentru algoritmul adaptiv
M1 = 3;
M2 = 3;
gamma = 0.1;
R = zeros(3,3*N);
% bucla filtrului Kalman
for i = 2:N
T = tt(i) - tt(i-1);
Cq = quaternion2dcm(q4(:,i-1));
A(1:3,1:3) = -vec2product(yg(:,i-1) - bghat);
dA = eye(12) + A * T + A * A * T^2 / 2;
x = dA * x;
Qd = Q*T + 0.5*T*T*A*Q + 0.5*T*T*Q*A';
Qd = 0.5*(Qd + Qd');
P = dA * P * dA' + Qd;
yyg = yg(:,i-1) - bghat;

154

wx = yyg(1);
wy = yyg(2);
wz = yyg(3);
omega4 = [ 0 , -wx , -wy , -wz ; ...
wx , 0 , wz , -wy ; ...
wy , -wz , 0 , wx ; ...
wz , wy , -wx , 0];
q4(:,i) = ( eye(4) + 0.75 * omega4 * T - 0.25 * oldomega4 * T - (1/6) * norm(yyg)^2 * T^2 * eye(4) (1/24) * omega4 * oldomega4 * T^2 - (1/48)*norm(yyg)^2 *omega4 * T^3) * q4(:,i-1);
q4(:,i) = q4(:,i) / norm(q4(:,i));
oldomega4 = omega4;
Cq = quaternion2dcm(q4(:,i));
% ---------------------------------------------------% update in 2 pasi
% ---------------------------------------------------H1(1:3,1:3) = 2 * vec2product(Cq*gtilde);
z1 = [ ya(:,i) - bahat - Cq*gtilde ];
% adaptive algorithm
fR1 = (z1 - H1*x) * (z1 - H1*x)';
R(:,3*(i-1)+1:3*i) = fR1;
uk = fR1;
for j = i-1:min([i-(M1-1),1])
uk = uk + R(:,3*(j-1)+1:3*j);
end
uk = uk / M1;

fR2 = H1*P*H1' + Ra;


[u,s,v] = svd(uk);
u1 = u(:,1);
u2 = u(:,2);
u3 = u(:,3);
lam = [ s(1) , s(2) , s(3)];
mu = [ u1' * fR2 * u1 , u2' * fooR2 * u2 , u3' * fR2 * u3];
if ( max(lam - mu) > gamma )
r2count = 0;
Qstar = max(lambda(1)-mu(1),0)*u1*u1' + max(lam(2) -mu(2),0)*u2*u2' + max(lam(3)mu(3),0)*u3*u3';
else
r2count = r2count + 1;
if ( r2count < M2 )
Qs = max(lam(1)-mu(1),0)*u1*u1' + max(lam(2) -mu(2),0)*u2*u2' + max(lam(3)mu(3),0)*u3*u3';
else
Qs = zeros(3,3);
end
end
P1 = P;
K1 = P1 * H1' * inv(H1 * P1 * H1' + Ra + Qs);
x = x + K1 * (z1 - H1 * x);

155

P = (eye(12) - K1*H1)*P*(eye(12)-K1*H1)' + K1*(Ra+Qs)*K1';


P = 0.5 * (P + P');
qe = [ 1 ; x(1:3) ];
q4(:,i) = quaternionmul(q4(:,i),qe);
q4(:,i)= q4(:,i) / norm(q4(:,i));
x(1:3) = zeros(3,1);
Cq = quaternion2dcm(q4(:,i));
H2(1:3,1:3) = 2 * vec2product(Cq*mtilde);
z2 = [ym(:,i) - bmhat - Cq*mtilde];

P2 = P;
K2 = P2 * H2' * inv(H2 * P2 * H2' + Rm);
x = x + K2 * (z2 - H2 * x);
P = P - K2 * H2 * P - P * H2' * K2' + K2*(H2*P*H2'+Rm)*K2';
P = 0.5 * (P + P');
bghat = bghat + x(4:6);
x(4:6) = zeros(3,1);
bahat = bahat + x(7:9);
x(7:9) = zeros(3,1);
bmhat = bmhat + x(10:12);
x(10:12) = zeros(3,1);
qe = [ 1 ; x(1:3) ];
q4(:,i) = quaternionmul(q4(:,i),qe);
q4(:,i)= q4(:,i) / norm(q4(:,i));
x(1:3) = zeros(3,1);
eulercom4(:,i) = quaternion2euler(q4(:,i));
end

156

Anexa 5. Funcia folosit pentru citirea datelor de la laser


n aceast seciune este prezentat funcia getLaser, care este folosit pentru citirea datelor de la
laser. Aceast funcie primete datele de la laser i apelnd funciile Aria pentru procesarea datelor de la lasere
calculeaz distanele msurate de laser utiliznd funcia getRange() i memoreaz citirile din 5 n 5 grade n
vectorul range , pentru fiecare distan din vectorul range se memoreaz unghiul la care se efectueaz msurarea
apoi se calculeaz bula de sensibilitate n vectorul boundry.
void getLaser(ArSick *thisLaser)
{
const std::list<ArSensorReading *> *readingsList; //
std::list<ArSensorReading *>::const_iterator it;
int i = -1;
//
readingsList = thisLaser->getRawReadings();

//
//

c=0;
for (it = readingsList->begin(); it != readingsList->end(); it++)
{
i++; j++;
double ra=(*it)->getRange();
if (j==5){
range[c]=ra/1000;
double angle=(*it)->getSensorTh();
alfa[c]=angle*(3.14/180);
boundry[c]=k[c]*vrobot*Ts;
j=0;c++; }
}
}

157

int j=4; int

Anexa 6. Funcia folosit pentru detecia obstacolelor


Este prezentat funcia folosit pentru a determina dac a fost depistat un obstacol.
Funcia obstacle returneaz 1 dac exist un obstacol n zona de sensibilitate sau 0 altfel.
Aceast funcie compar distanele msurate cu distanele din vectorul boundry i dac exist
distane mai mici dect distana corespunztoare din vectorul boundry se salveaz poziia la
care distana este minim n variabila pos.
int obstacle(double range[37], double boundry[37]) {
int i,j;
j=0;pos=0;
min_range=15;
printf("test");
for (i=1;i<37;i++) {
if(i!=18) {
if (range[i]<boundry[i]){
if (min_range>range[i])
{
min_range=range[i];
pos=i;
}
j=1;
}
}
}
return (j);
}

158

Anexa 7. Funciile folosite la cutarea unei soluii pentru evitarea


obstacolului
Funciile searchleft() i searchright() caut o soluie pentru evitarea obstacolului. Aceste
funcii caut punctul (new_x,new_y) care permite trecerea vehiculului.
void searchright()
{ double hb,h,H;
int j;
j=17;
if (((range[j]*cos(alfa[j]))>=min_range+0.1)||(range[j]==maxrange))
{
hb=abs(min_range*tan(alfa[j]));
}
else {
while (((range[j]*cos(alfa[j]))<min_range+0.1)&&(j>0))//////
{ j=j-1;}
hb=abs(min_range*tan(alfa[j]));
}
h=0;
while ((h<hmin)&&(j>0)&&(range[j]>min_range))
{ j=j-1;
h=abs(min_range*tan(alfa[j]))-hb;
printf("%4.5f \n",h);
}
if (h>=hmin){
H=h/2+hb;
new_x=new_x+min_range*cos(angleb)-H*sin(angleb);
new_y=new_y-min_range*sin(angleb)-H*cos(angleb);
found=1;
printf("Cautarea la dreapta a gasit o solutie=");
}
else {
found=0;
printf("Cautarea la dreapta nu a gasit o solutie pt evitare");
}
fprintf(date3," %4.5f \t %4.5f \t %4.5f \t %4.5f \n",j, hb, new_x, new_y,
found);
}
void searchleft()
{ double hb,h,H;
int j;
j=19;
if
(((range[j]*cos(alfa[j]))>=min_range+0.1)||(range[j]==maxrange))
{
hb=abs(min_range*tan(alfa[j]));
}
else {
while (((range[j]*cos(alfa[j]))<min_range+0.1)&&(j<36))//////

159

{ j=j+1;}
hb=abs(min_range*tan(alfa[j]));
}
h=0;
while ((h<hmin)&&(j<36)&&(range[j]>min_range))
{ j=j+1;
h=abs(min_range*tan(alfa[j]))-hb;
}
if (h>=hmin){
H=h/2+hb;
new_x=new_x+min_range*cos(angleb)-H*sin(angleb);
new_y=new_y+min_range*sin(angleb)-H*cos(angleb);
found=1;
printf("Cautarea la stanga a gasit o solutie=");
printf("Pozitian n_X, n_Y=");
printf("%4.5f \t %4.5f \n" , new_x, new_y);
}
else {
found=0;
printf("Cautarea la stanga nu a gasit o solutie pt evitare");
}
fprintf(date3," %4.5f \t %4.5f \t %4.5f \t %4.5f \n",j, hb, new_x, new_y,
found);
}

160

Anexa 8. Funcia pentru generarea traiectoriei de ocolire a


obstacolului
Funcia generare() genereaz traiectoria de ocolire a obstacolului care este impus n locul
traiectoriei dorite. Funcia calculeaz o traiectorie pentru evitarea obstacolului folosind Quintic
ecuations, de la punctul curent la punctul (new_x, new_y) i asociaz un profil de vitez acestei
traiectorii. Traiectoria care urmeaz o fi urmrit de modulul de conducere este coninut n vectorii
vel, acc, angvel i accang.
void generare()
{
angleb=robot.getTh();
new_x=robot.getX()/1000;
new_y=robot.getY()/1000;
int po=pos;
min_range=min_range*cos(alfa[po]);
if (pos<18) {
printf("Cautare stanga");
searchleft();
if (found==0)
{
printf("Cautare dreapta");
searchright();
}
}
else {
printf("Cautare dreapta");
searchright();
if (found==0)
{
printf("Nu a fost gasita solutie");
printf("Cautare stanga");
searchleft();
}
}
if (found==1){
printf("directie selectata");
double tetaA=angleb; // unghi initial
double tetaB=angleb+3.14/4; // unghi final
// robot.lock();
double xA=robot.getX()/1000;
double yA=robot.getY()/1000;
printf("PozitianX, Y=");
printf("%4.5f \t %4.5f \n",xA, yA);
//
robot.unlock();
double xB=new_x;
double yB=new_y;
double kA=0;
double kB=0;

161

double kA_d=0;
double kB_d=0;
double g1=sqrt(pow((xA-xB),2)+pow(yA-yB,2));
double g2=g1;
double g3=0;
double g4=-g3;
double x0 = xA;
double x1 = g1*cos(tetaA);
double x2 = 1/2*(g3*cos(tetaA)-ow((g1),2)*kA*sin(tetaA));
double x3 = 10*(xB-xA)-(6*g1 + 3/2*g3)*cos(tetaA) - (4*g2 - (1/2)*g4 )*cos(tetaB) +
(3/2)*pow((g1),2)*kA*sin(tetaA) - (1/2)*pow((g2),2)*kB*sin(tetaB);
double x4 = -15*(xB - xA) + (8*g1 + (3/2)*g3)*cos(tetaA) + (7*g2 - g4)*cos(tetaB) (3/2)*pow((g1),2)*kA*sin(tetaA) + pow((g2),2)*kB*sin(tetaB);
double x5 = 6*(xB - xA) - (3*g1 + (1/2)*g3)*cos(tetaA) - (3*g2 - (1/2)*g4)*cos(tetaB) +
(1/2)*pow((g1),2)*kA*sin(tetaA)-(1/2)*pow((g2),2)*kB*sin(tetaB);
double y0 = yA;
double y1 = g1*sin(tetaA);
double y2 = 1/2*(g3*sin(tetaA)+pow(g1,2)*kA*cos(tetaA));
double y3 = 10*(yB-yA)-(6*g1 + (3/2)*g3)*sin(tetaA) - (4*g2 - (1/2)*g4 )*sin(tetaB) (3/2)*pow((g1),2)*kA*cos(tetaA) + (1/2)*pow((g2),2)*kB*cos(tetaB);
double y4 = -15*(yB - yA) + (8*g1 + (3/2)*g3)*sin(tetaA) + (7*g2 - g4)*sin(tetaB) +
(3/2)*pow((g1),2)*kA*cos(tetaA) - pow((g2),2)*kB*cos(tetaB);
double y5 = 6*(yB - yA) - (3*g1 + (1/2)*g3)*sin(tetaA) - (3*g2 - (1/2)*g4)*sin(tetaB) (1/2)*pow((g1),2)*kA*cos(tetaA)+(1/2)*pow((g2),2)*kB*cos(tetaB);
printf("x1=");
printf("%4.5f \n",x3);
n=1;
l[0]=0;
double u;
double alfa1[101],beta[101];
double x_d;
double y_d;
double x_d_d;
double y_d_d;
double KK[101];
double lungime=0;
u=0;
for (n=0;n<=100;n++)
{
u=u+0.01;
printf("u=");
printf("%4.5f \n",x0);
alfa1[n]=x0+x1*u+x2*pow(u,2)+x3*pow(u,3)+x4*pow(u,4)+x5*pow(u,5);
beta[n]=y0+y1*u+y2*pow(u,2)+y3*pow(u,3)+y4*pow(u,4)+y5*pow(u,5);
printf("alfa1=");
printf("%4.5f \t %4.5f \n",alfa1[n], beta[n]);
if (n > 0)
//
l[n] = l[n-1] + sqrt(pow((alfa1[n] - alfa1[n-1]),2) + pow((beta[n] beta[n-1]),2));
l[n] = sqrt(pow((alfa1[n] - alfa1[n-1]),2) + pow((beta[n] - beta[n-1]),2));

162

x_d = x1 + 2*x2*u + 3*x3*pow(u,2) + 4*x4*pow(u,3) + 5*x5*pow(u,4);


y_d = y1 + 2*y2*u + 3*y3*pow(u,2) + 4*y4*pow(u,3) + 5*y5*pow(u,4);
x_d_d = 2*x2 + 6*x3*u + 12*x4*pow(u,2) + 20*x5*pow(u,3);
y_d_d = 2*y2 + 6*y3*u + 12*y4*pow(u,2) + 20*y5*pow(u,3);
KK[n]=(x_d*y_d_d - x_d_d*y_d)/((sqrt(pow((x_d),2) + pow((y_d),2)),3));
lungime=lungime+l[n];
}
for (n=1; n<100;n++)
{
vel[n]=l[n]/Ts;
acc[n]=vel[n]/Ts;
angvel[n]=vel[n]*KK[n]*l[n];
accang[n]=angvel[n]*vel[n];
}
}
}

163

Anexa 9. Realizri roboi mobili

Fig. 8.1 Aspirator robotic autonom Roomba[41]

Fig. 8.2 Robotul autonom pentru splat podele Scooba[41]

164

Fig. 8.3 Robotul autonom de tuns iarba produs de Husqvarna[44]

Fig. 8.4 Robotul autonom Robovie-II

165

Fig. 8.5 Vehiculul autonom Sojourner

Fig. 8.6 Vehiculul autonom TerraMax

166

Fig. 8.7 Vehiculul autonom VIAC [43]

167