Залуурдагддаггүй тэнхлэг бүхий роботынкоординатыг хугацааны мэдээлэл ашиглаж олох

алгоритм

Г.Ганбат, Д.Нанзадрагчаа, M.БаярпүрэвМонгол Улсын Их Сургуулийн

Мэдээллийн Технологийн СургуульУлаанбаатар хот, Монгол Улс

Email: www.ganbat_net@yahoo.com

Хураангуй—Роботын координатыг өндөр нарийвчлал-тайгаар, real-time олох бодлого нь роботикийн чухаласуудал билээ. Энэхүү судалгааний ажилд роботынзалуурдагддаггүй тэнхлэг дээр байрлуулсан ротариэнкодер ашиглан координатыг тооцоолох алгоритмыг саналболгоно. Эргэлдэгч энкодерийн тасалдал үүсэх хугацаагбүртгэж тухайн цэгийн хурдны фүнкцийг ойролцоолонсэргээнэ. Улмаар үл залуурдагдах тэнхлэг дээрх цэгийнхурд тэнхлэгтэй перпендикуляр байдаг гэдгийг ашигланкинематикийн тооцоолол хийнэ. Алгоритмыг OMAP4430процессор бүхий Panda board ашиглаж туршилтыг хийсэн.

Түлхүүр үгс: Роботикс, координат олох, машин-хэвтробот, зам төлөвлөлт

I. Оршил

Сүүлийн жилүүдэд жолоочгүй машины судалгаахөгжүүлэлт эрчимтэй явагдаж байна. Америкийнбатлан хамгаалах яам ч энэ салбарыг анхаарч жилбүр DARPA Grand Challenge тэмцээнд жолоочгүймашины уралдаан явуулдаг. Түүнчлэн, үйлдвэрлэгчнар ч энэ салбарт өрсөлдөж эхлээд байна. Гүүглкомпаний жолоочгүй машин, Тоётагийн i-unit концептмашин зэрэгийг дурдаж болно. Жолоочгүй машинэсвэл робот нь өөрийн координатыг маш нарийнмэдэж байх шаардлага гардаг. Үүнд GPS хиймэлдагуулыг ашиглаж болох хэдий ч нарийвчлал машмуу болно (Differential GPS ашигласан тохиолдолд1м, бусад үед 7м). Радио долгион ашиглан үүнээснарийн хэмжихийн тулд Ultra-wide-band технологиашиглаж болох ч дэд бүтэц нь үнэтэй, хэрэглэгдэххүрээ багатай болчихдог. Камер ашигласан хиймэлоюун ухааныг хэрэглэж болох хэдийг ч энэ нь нүсэртооцоолол шаардах бөгөөд real-time чанараа алддагдутагдалтай. Машины үл залуурдагдах тэнхлэг дээрбайрлуулсан ротари энкодер ашиглан координат олохалгоритм маань тооцоолол багатай, хямд, real-timeзэрэг давуу талуудтай. Энэ аргыг дээрх аргуудтайхавсран ашиглавал илүү үр дүнтэй. Энэ өгүүлэлийгдараах байдлаар зохион байгуулсан болно: II дэд бүлэгтзалуурдагддагүй тэнхлэг бүхий роботын кинематик,траекторын анализыг хийнэ. III дэд бүлэгт биднийсанал болгож буй ГАЛ алгоритмын талаар тайлбарлах

бөгөөд IV дэд бүлэгт туршилтын үр дүн, V дэд бүлэгтдүгнэлтээ танилцуулна .

II. Залуурдагддагүй тэнхлэг бүхий роботынкинематик

Va

Vb

j

i

-a

Y

X

ra

rb

Зураг 1: Роботын траектор анализ

Дугуй тус бүрийг харгалзан a,b гэвэл түүний үүсгэхзамын илэрхийлэл нь :

~ra(t) =(

xa(t)ya(t)

), ~rb(t) =

(xb(t)yb(t)

)(1)

1 гэе. Эдгээр замын илэрхийлэлээс хурдны вектор тусбүр нь :

~va(t) = ~ra(t)′ =(

xa(t)′

ya(t)′

),~vb(t) = ~rb(t)′ (2)

MMT-2013 Цахим засаглал, Мэдээллийн технологи

103

Дугуй тус бүрийн үүсгэж буй траекторийн тухайнагшин дахь ширгэгч шулуун нь тухайн дугуйн хурднывекторын дагуу байна. Цаашлаад, хэрэв хальтралтыгүл тооцвол тухайн дугуйн чиглэл нь ч мөн энэхүүхурдны векторын дагуу чиглэнэ. Иймээс хойд хоёрдугуйн ~va(t), ~va(t) векторууд хоорондоо параллельбөгөөд AB талтай перпендикуляр чиглэнэ. Тухайлбал,хэрэв роботын A дугуй урагш эргэж байсан гэжүзвэл ~va(t) -ийг цагийн зүүний дагуу 90◦ эргүүлбэл

−→ab

векторын дагуу чиглэнэ. Иймээс,(cosθsinθ

)= 1

| −−→AB |

−−→AB = 1

| ~vA(t) |

(0 1

−1 1

)~vA(t) (3)

буюу (cosθsinθ

)=

yA(t)′

|~vA(t)|

− xA(t)′

|~vA(t)|

(4)

болно. Түүнчлэн,

~rB = ~rA +−−→AB = ~rA + L

| ~vA |

(0 1

−1 0

)~vA (5)

болно. Энэд хувсагч t-ийг орхин товчлов. Ро-та өнцөгхурдыг ω(t) = θ(t)′ гэж тэмдэглэн нормалчлагдсанхурдны вектороос уламжлал авбал

d

dt

(~vA

| ~vA |

)=

(0 −ωω 0

)~vA

| ~vA |, (6)

⇒ ω =∣∣∣∣ d

dt

(~vA

| ~vA |

)∣∣∣∣ (7)

болно. Өөрөөр хэлбэл энэ уламжлал авах үйлдэл ньэнэ векторыг ω дахин сунгаж цагийн зүүний эсрэг90◦ эргүүлэхтэй адилхан. Эндээс, (2) томъёоны хурдыгдахин тооцолбол

~vB = ~vA +(

0 L−L 0

)(0 −ωω 0

)~vA

| ~vA |(8)

⇒ ~vB = ~vA +(

0 −ωω 0

)−−→AB (9)

болно. Эдгээр томъёонуудад эргэлтын матрицуудынбайр солидог чанарыг ашиглав.

Эндээс vB -г :

vB = |~vB | = |~vA|+Lω (10)

болно. Дээрх томъёоноос харахад роботыг өгөгдсөнтраектороор өгөгдсөн хурдтай явуулахын тулд |~vA|(t)болон ω-г тооцоолж даалгавар болгоход хангалттайболох нь илэрхий. ω ро-та өнцөг хурд нь зөвхөн Aдугуйн траектороос хамаарах ба дугуйн хурднуудаасхамаарахгүй. Мөн |~vA|(t) хурдны векторыг траектороосхамааралгүйгээр өгч болно. Энэ хоёр хувьсагчаасбусад бүх хувьсагчдыг гарган авч болно. Хурднывекторуудыг ~vA(t)-тай харьцангуй өнцгөөс нь хамааранабсолют утгыг нь сөрөг утга оноож өгч болно:

sign(vB) = sign(~vA ·~vB) ={

+1, vA > ωL−1, vA ≤ Lω

(11)

Өөрөөр хэлбэл ~vB(t) хурдны векторийн ~vA(t)-тайүүсгэх өнцөг дэлгэмэл бол сөрөг утга авна.

III. Координат тооцоолох алгоритмууд

Бид координат тооцолох гурван алгоритмболовсруулсан. Эдгээрийг ГАЛ, ГАЗ, NF гэж нэрлэсэн.ГАЛ алгоритм нь энкодерын тасалдал болон хугцаанымэдээллийг ашиглаж координатаа олдог.

~rA(t) = ~rA(0)+∫ t

0

∥∥ d

dt~rA(τ)

∥∥(−sinθ(τ)cosθ(τ)

)dτ (12)

⇒ ~rA(0)+∫ t

0

∥∥~vA(τ)∥∥(

−sinθ(τ)cosθ(τ)

)(13)

ГАЗ нь хурдны векторуудыг Диракийн делта δ(t)фүнкцээр ойролцоолж эргэлтүүд A, B дугуй дээрхэргэлтүүд болно.

θ(t) = θ(0)+ 1L

(∫ t

0

∥∥~vB(t)∥∥d(τ)−

∫ t

0

∥∥~vA(t)∥∥d(τ)

)(14)

⇒ θ(0)+ 1L

(∫ t

0~vB(t)d(τ)−

∫ t

0~vA(τ)dτ

)(15)

⇒ θ(0)+ 1L

(sB(t)−sA(t)

)(16)

Алгоритмуудын алдааг үнэлэхийн тулд тооцонгийнаргаар симмүляц хийсэн. Ингэхийн тулд роботын явсанзамыгБизьегийн олон гишүүнтээр өгсөн. P0, P1, P2, P3өгөгдөх Бизьегийн олон гишүүнтийг эхлэн бичвэл:

~rA(t) = At3 +Bt2 +Ct+D, A = P3 −3P2 +3P1 −P0, (17)B = 3P2 −6P+3P0, C = 3P1 −3P0, D = P0

болно. Энэ сегментэд хурдны векторын абсолют утгыгтооцоолбол болохыг хялбархан харж болно:

vA =√

9 | A |2 t4 +Ft3 +Gt2 +Ht+ | C |2

F = 12A · B, G = 4 | B |2 +6A ·C, H = 4B ·C (18)

энэд, · -ээр хоёр векторын скаляр үржвэрыг тэмдэглэв.

Дээрх томъёоноос харахад A дугуйн траекторыгБизье муруйгаар өгсөн тохиолдолд бусад дугуйтраектор нь 1/vA(t) үржэгдэхүүн агуулж байгаа тулолон гишүүнт болж чадахгүй нь илэрхий. Траекторыгбайгуулдаг программыг Java хэл интерактив болонвизуал байдлаар бичиж, дэлгэрэнгүй анализыгтооцонгийн аргаар Matlab программ дээр гүйцэтгэсэн.

A. Траекторыг Тэнцүү Урттай Хэсгүүдэд ХуваахАлгоритмууд

Роботод учруулах тооцооллын ачааллыгбууруулахын тулд тогтмол урттай зам туулсныдараагаар дараагийн даалгарыг өгөх шаардлагагарсан. Үүнийг хэрэгжүүлэхийн тулд аль нэгэндугуйн траекторыг тэнцүү хэсгүүдэд хуваах алгоритмболовсруулах хэрэгцээ гарсан. Энэ бодлогын тавилтыгдоорх байдлаар томъёолж болно:

MMT-2013 Цахим засаглал, Мэдээллийн технологи

104

Хавтгайд A,B,C,D ∈ R2 векторууд өгөгджээ.Тэдгээрээр байгуулагдсан ~rA(t) = At3 + Bt2 + Ct +D, t ∈ [0,1] муруйг өгөгдсөн ∆ ∈ R урттай хэсгүүдэдхуваа. Тодруулбал, хуваалтын t0 = 0, t1, · · · , tK агшиныгол.

A,B,C,D векторуудын талаар дэлгэрэнгүйг ??дэд бүлгээс үзнэ үү. Хувьсагч t нь бодит хугацаагилэрхийлэхгүй бөгөөд Бизьегийн муруйн параметртөдий юм. Жинхэнэ бодит хугацааг болон хурдыгтраекторын тэгшитгэлээс хамааралгүйгээр өгнө.

Бид хоёр алгоритм боловсруулж үзсэн. Эхнийалгоримт нь математик тооцолол байгаатай бөгөөдJava, Visual Studio зэрэг ердийн программчиллынорчинд гүйцэтгэх боломжтой. Нэгдүгээр алгоритмынсанаа энгийн бөгөөд хуваалтын агшныг рекурсивбайдлаар тооцоолно:

t0 = 0, tk = tk−1 + ∆vA(tk−1)

, k = 1,2, · · · (19)

энэд, vA(tk−1) нь хурдны векторын абсолют утгаба (18) томъёогоор өгөгднө. Нэгдүгээр алгоритм ньматематик тооцоолол багатай ч хэсгүүдийн урт ∆ихэсвэл нарийвчлал нь муудах дутагдалтай.

Хоёрдугаар алгоритм нь математик тооцоололихтэй боловч нарийвчлал өндөртэй. Энэ алгоримтнь өгөгдсөн Бизьегийн муруйн натурал параметрыголж түүнийг ∆ урттай хэсгүүдэд хуваах байдлаархугацааны агшинг олно. Натураль параметр гэдэгмуруйн нумын урт s-ээс хамаарсан хугацааны функцt = t(s) юм. Энэ тохиолдолд vA(t(s)) хурдны абсолютутга нь s параметрын хувьд 1-тэй тэнцнэ гэсэн үг юм:

s(t) =∫ t

0vA(τ)dτ, (20)

vA(t(s)) =∥∥∥∥d~rA(t(s))

ds

∥∥∥∥ =∥∥∥∥d~rA(t(s))

dt

dt

ds

∥∥∥∥ =∥∥∥∥d~rA(t(s))dt

∥∥∥∥ dt

ds= vA(t) dt

ds= 1 (21)

Натурал параметрыг олохын тулд (21) дифференциалтэгшитгэлийг бодно. Энэ тэгшитгэлд vA(t)-г (18)томъёоноос орлуулан тавибал дифференциалтэгшитгэлийн гарна:

t′ = 1vA(t)

= 1√9 | A |2 t4 +Ft3 +Gt2 +Ht+ | C |2

(22)

Энэ шугаман бус дифференциал тэгшитгэлийншийд нь гипергеометр функцээр илэрхийлж болохболовч бидний зорилго натурал параметрыг болох бустүүний ∆ урттай хэсгүүд дээрх утга л хэрэгтэй тултэдгээрийг тооцонгий аргаар бодов. Тодруулбал, s =k∆, k = 0,1, · · · ,K дээрх t(n∆), k = 0,1, · · · ,K утгыгMatlab-ын ode45 түүлээр тооцоолов. Эдгээр хуваалтынутгуудыг туршилтад ашиглав.

IV. Туршилт

Траекторыг өмнөх хэсэгт заасан дагуу тэнцүүхэсэгт хуваасан. Хуваалт бүрд харгалзах муруйн t

параметртыг ашиглан ~rA(t) болон ~rB(t) векторуудынкоординатыг хоорондон эрэмблэж дараалал үүсгэсэн.Энэхүү дараалал нь Залуурдагддаггүй тэнхлэгбүхий роботын ротари энкодерын үүсгэх тасалдалындараалалтай жишиж болохуйц мэдээлэл юм. Өөрөөрхэлбэл даалгавар траектор нь роботын туулсанзам мэтээр загварчлаж болох юм аа. Даалгавартраекторыг шулуун,дан нумаас бүрдсэн,өлөн нумагуулсан байдлаар дүрсэлж туршиж үзсэн. Энэхүүтуршилтаар ГАЛ алгоритмыг бусад ГАЗ, NF зэрэгалгоритмтай жишиж алдааг үнлэж харицуулалтхийсэн.Даалгавар траекторыг байгуулахдаа ′′ABURobocon 2013′′ тэмцээний тоглолтын талбайн зургыгсуурь зураг болгон ашиглаж байгаа. Хэмжээний хувьд10:1 харьцаанд оруулсан болно.

Суурь координат дээр даалгавар траекторындүрслэлийг бизьегийн муруйн куб эрэмбийн тэгшитгэлбуюу 4 цэгээр 1 муруйг дүрслэж байгаа. Энэхүү4 цэгийн мэдээлэлийг ашиглаж буцааж муруйгбайгуулах бөгөөд энэ манай роботын А дугуйн туулахтраектор болгон ашиглах юм аа. Роботын траекторилэрхийлэл болох (1) болон (5) тэгшитгэлийг ашиглажбусад траекторыг байгуулдаг.

A. Шулуун траектор

Зураг (2) дээр шулуун траекторыг хэрхэнбайгуулсаныг харж болно. Роботын нэгж алхамын

−500 0 500 1000 1500 2000 2500 3000

7500

8000

8500

9000

9500

10000

x−axis

y−ax

is

My road divition

rArB

Зураг 2: Шулуун траекторын даалгавар

хэмжээ буюу ∆ хэмжээ:1. ротари энкодерын хуваалтын хэмжээ2. энкодерт ашиглаж буй дугуйн радиусаас хамаардаг.Зураг (3) дээр ∆ = 0.4mm үед координат олохалгоритмуудын алдааг харьцуулан харуулсанбайна. GAZ алгоритмын алдаа бусад алгоритмтайхарьцуулхад бага байгаа нь алгоритмын бодолтоосхамаарч байгаа. Энкодерын тасалдалын утга шулуунявах үед нэг нэгээр дараалаж үүсэх нь их бөгөөд энэнь алгоритмын координат олох дараалалтай адилханбайгаатай холбоотой.Зураг (4) дээр ∆ = 0.8mm үед алгоритмуудын алдаахэрхэн өөрчлөгдөснийг харуулж байна. Нийт алдаа ньерөнхийдөө их болсон. Өөрөөр хэлбэл ∆ хэмжээ нь

MMT-2013 Цахим засаглал, Мэдээллийн технологи

105

0 1000 2000 3000 4000 5000 6000 7000 80000

2

4

6

8

10

12

14

16

18

20All algoritm Square error

NFGAZGAL

Зураг 3: Алдааны харьцуулалт, ∆ = 0.4mm

бага байхад аль ч алгоритмын хувьд алдаа нь багсахнь харагдаж байна.

0 500 1000 1500 2000 2500 3000 3500 40000

2

4

6

8

10

12

14

16

18

20All algoritm Square error delta=0.8

NFGAZGAL

Зураг 4: Алдааны харьцуулалт, ∆ = 0.8mm

B. Нэг муруй бүхий траектор

Нэг муруйгаар илэрхийлсэн нум хэлбэртэй траектордаалгаварыг сонгон авсан. Зураг (5) дээр дугуйнкоординатууд хэрхэн байгуулагдсаныг харж болно.Зураг (6) дээр ∆ = 0.4mm үед координат олохалгоритмуудын алдааг харьцуулан харуулсан байна.Энэ үед GAZ алгоритмын алдаа ГАЛ алгоритмыналдааны утгыг бодвол бага байгаа нь тойргоор эргэххөдөлгөөний хувьд ГАЛ алгоритм нь тооцоол илүү сайнхийдэг нь харагдаж байна. Зураг (7) дээр ∆ = 0.1mmүед координат олох алгоритмуудын алдааг харьцууланхаруулсан байна. Энэ үед ГАЗ болон ГАЛ алгоритмыналдаа ойролцоо байгаа нь ∆ хэмжээ бага авсанаасхамаарч байна. Хэдий муруй траектор зурагдсан хэдийч маш бага хуваалтын хувьд шулуун гэж үзэж болно.

C. Олон муруй бүхий траектор

Олон муруйгаар илэрхийлэгдсэн муруйн хувьддаалгавар траектор үүсгэх нь алгоритмуудын онцлогыгтэдгээрийн алдаанаас харж болно. Муруй бүр нь

600 800 1000 1200 1400 1600 1800 2000

8200

8400

8600

8800

9000

9200

x−axis

y−ax

is

My road divition

rArB

Зураг 5: Нэг муруйгаар илэрхийлсэн даалгавартраектор

0 1000 2000 3000 4000 5000 6000 7000 80000

1

2

3

4

5

6All algoritm Square error

NFGAZGAL

Зураг 6: Алдааны харьцуулалт, ∆ = 0.4mm

олон төрлийн өнцөг, байгуулалт бүхий траектор болохюм. Бодит роботын хувьд ч гэсэн олон төрлийнмуруй бүхий замын даалгавараар туулах хэрэгтэйболдог. Зураг (5) дээр дугуйн координатууд хэрхэнбайгуулагдсаныг харж болно. Зураг (9) дээр ∆ = 0.1mmүед координат олох алгоритмуудын алдааг харьцууланхаруулсан байна.

Олон муруй бүхий траекторын хувьд ГАЛалгоритмын алдаа нь ∆ -ын аль утганд бага байгаагнь харагдаж байна. Иймээс ГАЛ алгоритм ньзалуурдагддаггүй тэнхлэг бүхий роботын координатолход илүү тохиромжтой.

V. Дүгнэлт

Алгоритмын харьцуулалт хийж туршилт хийхэд:- Даалгавар траектор байгуулагч- Траекторыг тэнцүү хэсгүүдэд хуваах алгоритм зэрэголон ажилуудыг ашигласан. Зураг (3) дээр ГАЗалгоритм нь ГАЛ алгоритмаас илүү нарийвчлалтайкоординат олж байгаа нь харагдаж байна. Бодиттраектор байгуулалтын хувьд олон муруйгаас тогтсонтраектор байдаг. Зураг (8) нь бодит траектордаалгаварын нэгэн жишээ болох юм. Зураг (8)-ын

MMT-2013 Цахим засаглал, Мэдээллийн технологи

106

0 0.5 1 1.5 2 2.5 3

x 104

0

1

2

3

4

5

6

7All algoritm Square error delta 0.1

NFGAZGAL

Зураг 7: Алдааны харьцуулалт, ∆ = 0.1mm

1500 2000 2500 3000 3500 4000 4500

7000

7500

8000

8500

9000

9500

x−axis

y−ax

is

My road divition

rArB

Зураг 8: Олон муруй бүхий траектор даалгавар

хувьд алгоритмын харьцуулсан үр дүнгүүд нь зураг(9),(??), (11) дээрээс харж болно. Энэхүү харьцуулсаналдааны үзүүлэлтээс ГАЛ алгоритм нь алдааны хувьдбагатайгаар координатыг олож байгааг харж болно.

Талархал

Энэхүү ажлыг одоогын үр дүнд хүрэхэд тусалсанбагш М.Баярпүрэв /Доктор/, багш Д.Нанзадрагчаа/Магистр/, сургалтын инженер Гантулга нартааталархал илэрхийлэё.

Ашигласан ном[1] D.Lee, J.Villasenor, W.Luk, P.Leong ,”Mobile robot localization

and object pose estimation using optical encoder, vision andlaser sensors,” IEEE Intern. Conf. on Automa. and Logist., pp.617 – 622, 2008.

[2] Inigo Thomas, J.Oliensis ,”Automatic position estimation of amobile robot,”Ninth Conf. on Artificial Intelligence for Applica-tions., pp. 438 - 444 , 1993.

[3] Sungbok Kim , Sanghyup Lee,”Mobile robot localization andobject pose estimation using optical encoder, vision and lasersensors,” IEEE Conf. on Digital Object Identifier., pp. 1167 -1172 , 2008.

[4] Gao Qingji, Feng Qi, Zhang Hongxiang,”A robot self-localizationmethod based on perception dominance,” IEEE Intern. Conf.Digital Object Identifier., pp. 2343 – 2346, 2011.

0 2 4 6 8 10 12

x 104

0

5

10

15All algoritm Square error delta 0.1

NFGAZGAL

Зураг 9: Алдааны харьцуулалт, ∆ = 0.1mm

0 0.5 1 1.5 2 2.5 3

x 104

0

2

4

6

8

10

12

14

16

18All algoritm Square error

NFGAZGAL

Зураг 10: Алдааны харьцуулалт, ∆ = 0.4mm

0 2000 4000 6000 8000 10000 12000 140000

5

10

15

20

25

30All algoritm Square error

NFGAZGAL

Зураг 11: Алдааны харьцуулалт, ∆ = 0.8mm

MMT-2013 Цахим засаглал, Мэдээллийн технологи

107