! #"%$ &('!)+*%,!-

Control of nonholonomic systems using iterative curvature feedback∗Masato ISHIKAWA, Kyoto University

Abstract– In this paper, we deal with a point-to-point control method for a class of nonholonomic symmetric affinesystems, whose dynamics are described by principal fiber bundle and connection form. Based on the estimate of geometricphase of the system using its curvature differential form, we propose a periodic feedback control method which drivesthe state to a desired one. Finally, we also propose an iterative tuning method to attenuate the estimation error of thegeometric phase, called iterative curvature feedback.

Key Words: nonlinear systems, nonholonomic systems, holonomy theory

1 .0/214352687:9<;>=@?@A2BDC>EGFIH2JLKNMPORQ<S>T2UWV:XMPOZY+[]\!EGFI^`_baNc<d`egfihkjPlDaZm:ngoqpsrutv@;uw@x2y:zpoint-to-point

^`_8tIvsXI%|~iZH2JK@MPORQ`S2T2UNV:9RxGjRlsMPOZY[\EGF2aNc<degfhkjRl8XIk2:<!opsr:N<:`@7kkrG 1);LM2OZY+[\EGFI^`_8<RLaR9G22kPLkr

92¡`¢¤£¦¥R§:¢RPDXI¨:©!ª«x:¬Z­L®Rxb¯Ix q°±oGXZ²RwbruP³2>;DMPOZY´[µM<¶@·¸Y+[^`_D7fihNjRlP¹NGm:nDXR2wbrIº2»@7WRd`eL¼¸;G½k¾Da¿DÀ ¯Á­brÂR§:jRlDaZH%ªÃesĵÅNÆWaZÇ2ÈXGÉ`Ê%ªGcuqwR­L®GË:ÌN9kM:OZY+[]\EGFI^`_4I͸Î:ÏLP°¸±oauÐRÑ`wbr:W¸a>xWrGH:J8KLMPORQ>SRTPU@V:!z`;

chained systemxÒ`Ó>ÔWPÕRÖI×ØÙYWÚ

2)XIÛPÜ@S<T>UkVÝRÔRÞW³27R

Lie ßPà<á 7`u^<_RâL¸ã:äo­r<S`T`U:V (first ordersystem)3, 4) xbÒIuåRæWÆPÓ`çDx<`^>_PâPèké8XIÛWÜ@STuUPV>aHªµeR9:­k³<72a:¼GMuOÙYê[ë\EFµìRÝ<ÇuÈÉuÊN«¨R©<í:î:ïoµ­Pe:ðZ>ÁñkÕuÖq×gصYkÚIòPkcµaÃó`ôNxG^G_`â>è2éWXR¼ÙÜD¼I2aIÜ eN¼Ã;¸2rõZöW÷P`øaR>@rIùDðIú8S>T>UNVPaÜ ek9IîWïWûNü 7 r 5, 6)

<;R¸ZuøD9¸WS<T<U:VDÙRý>þÁEÿLð aPPÆ:xRíg«92x|We x 526L7W9<; @LcqxkWPS2T>UWVkLa 2K<ÿNAÃY >7R9<xÁ;u^G_>â

Lie DaIH oÙp@eG¥R§NÆWx^`_! !"8X$#%Gw@K2ÿLA¸Y!NX$252ªqeu;2q­LX'&!(w8r)>í@X+*,2w8rZÃG^u_Lie

9ujRl-/.:a0 e`Ì1NÆWa û2 «r354F7698XLie ßPàGá ÷k7: @¼Z>7P:rZ 1

ÞWLie ßPàá aH wsr¥P§P^2_! !"D9 y!;Wr Lie ßkà>á!< ÅPª«e5 >=`Ækx?@7L¼Pc@BA ¯Á­brP¼GR7kkrG

7) ;$CuÞW Lie ßRàGá a¸H wrD "DXE r2k9I=F a`°±N7RPrZ'G@I7Z5uî:ïR7R9;IG^u_Lie èkéX-/.53:aHIsªL¼G27Wkru?@A2BDCqYJ

KL ÷MNN$OsX'PQA%ªq;RÓRL¥P§S "8XIó õ T2 »8ypsr:N¸aWc`d`e`;1!UNGjRl:¢`bX`ø@ÆWaV É opsr:íDX* ,kwbrG51!UNujRlP¢`DWQXN7Z­8®<;u`øbaY/ZGwsr'[\s¼u³2WQX@7kkru7>;!fihkjRl8a$]^%ªÃe `_RÎL7k9kMPOZY+[\EGFa!b8aWc<d`e5[\Xc:xg|ÙZÎ:Ï/d]PwbrkWR7ð:r<ok¯Iabf hkjRl@e º2»PÝk;f:ÅsA5gsEh6~a0@¬Wr«Ç2È!i jLt¸vxbÒ¸aZH wbrG ;>Ô@kPlSm

n ¸aGÉpoÃ5[\8X á #!ªÃeu¥R§S "SkW5qsr tuYÚRXu v:w8r KL lSmbMPOZY´[]\EGF¸íDX* ,:wrG2 w7x2.1 y z>|!~S>T>UWVRGjRl-.

MPªÃe>;PW@:ñ@

Q

Lie Gc8¯ è<ÊboÁ­@rÃÐ áS M\>Y^ M = G×QZèkéDXIÛ:Üb¼GkX À r 8) Q

X!-/.b)kujlS4F7698DXξ = (g, q), g ∈ G, q ∈ Q

Pw2G<Ó$Q

ea50W¬PrD!-Q.

TeGX

g!ª;

G

LieJ ~

∀a ∈ GX

GS@a)cs¯+cP¬PrAX À r@;

GWPÍ$/

La : G → G, La(g) := ag

5%¯Á­brG G:ea0@¬:r+W/PªÃe>;

g = TeGZö:÷Q

g → g!goq­k;í8X

DLa

PwRoIe<;u^`_ "uPªÃeP9-/.Ra0N¬:rq½k¾N

4bF68bX ¿8À ¯µ­W;`wDxPyWzq = u

7WWrLwruS>T>UWVR9ZÞNG÷@GjRlS>Î!MLa$juµwbrG

ξ =

[

DLg · Γ(q)I

]

u (1)

k<7I9 /"êGÍoÃÞ@DPÓ/ 87L@r>P³k

Γ : Q → g ⊗ T ∗Q9<;

ga¡bXDGr£¢

QW¤WQR÷

MW7k|~;¥S¦§!¨!©ªg ~2.2 5|!~S«¬@­®¯@°±f:ÅsA5gbEh6XI wbr¸ñ²IG=L?@A2B8C>EGF`ST<UkV> < Å ³S´µ8N5¶X·@75¸2ùkWFSrTPa$¹ª;í!¯«9 < ÅNLAgEh6~/º »@¼@3:ðRa$½%¯«x ¾S¿ªÃ5À 9) XIÛPÜWNL:I9ZjRlS-/.2 M!>\kY^>èWéLa oq­N < ųkÆÁS^SÂÃsa²R% ÅÄÆ@7ÀQL¬8¯Á­brG³Ç

G

M¹PA@X' ªe50s­k9Ij`lk

GÊN 2 ¹N$QAk;uwLx2yPz

∀a ∈ G,∀ (g, q) ∈ MaZH!ªÃeΦa : M → M : (g, q) 7→ (ag, q)

ÃSÈ:wr<8o@¯ZaΦa : M → M

W@êkªeM:\sQ[8

TMNZö:÷@

DΦ : TM → TM

@ oÙ­bruuw8xRyWzG;ξa0L¬kr½k¾LQ4bFÉ698

∀X ∈ TξMaZH!ªÃe

DξΦa(X) :=d

dtΦa(ξ + tX)

∣

∣

∣

∣

t=07`È oq­brGoIeMÊ

ξ = (g, q)Ta0 e>;`^`_! "8X ¿Àr:W¸aWc<d`eËQ.`ÆWa û/2 «r3>$::»

Hξ := Im

[

DLg · Γ(q)I

]

⊂ TξM

9TM

uÔRÜN5ÌSS^k;>ÜN³ |MW!ÍsX¸¾ÎªÃe rGªÃR:d`e

TΦ(H)92³2

TM5ÌSS^N7|i;

∀a ∈ GaZH!ªÃe

DΦa(Hξ) = Im

[

DLa · DLg · Γ(q)I

]

= HaξX$ÏD<wRDGGâ@ÐDaNc|i;H9

G- Ñ@Ò 7WWrL H

G-P¢W7kNr:N'c¯Ù;

HX$Ó@Ô -/.µwr

MW$R@5!¯­k;RP÷M

Γ : M 7→ g⊗ T ∗M9Γ = adg · (DLg−1 ⊗ dg − Γ(q) ⊗ dq) (2)IxWrGLíL9Õ¢

Mk¤

g¡ WQR÷SMW7kkrGLR7

adg

9gNa0N¬Pr

g ∈ GÖ× Rk7Ps|~;

GbÄ$Ø2Í$

ιg : G → G : h 7→ ghg−1$WQ

( g2¹N

Q)RªÃe`È oq­brP¼G>7kkrG

Γ9«½R¾NÓ!Ô48FÙ6Ú8>º

X ∈ HaIHª

Γ(X) = 0,½k¾@Y ∈ g

5254F76982ºY ∗a¸H!ªÃe

Γ(Y ∗) =YX'Û>wbcLµx

M:

g¡WS>÷SMP7P:r¸

ΓIèRéXÜbr@Ù;pݵY £DݵYJ@èkéN 2 c¯¸ð>³NrÌS

adg , DLg−1

q;>S`T`UkVÞ¸²NßàW7::rÌ1W!2÷M

Γc¯Ê8d`e rG

2.3 áâDãhä'åsæÙç9è é2ªqe`;Fig. 2.3

aê2wcDxÔRÑ:75ëì2ÜðsݸQPFu3Ü>PË oq­Níïî`Sf:ÅsA5gEh6~´ µ

88X$ À r10)`³>ð!ñR7R9Ô òªÙe5ó ô õW 2 X« G:ËÌL95öNeÄ÷N·L7Wkruë ìD9ÉݸQPF<

øÊLau;pݸQ2FL+Ô8x>3G¹N < Å@ 2 X$ùPwscaZê| ÜL¬D¯­Ne rúDzëìNkû@98Ä÷u7:b|i;ü/ý |X8ª«x ¼GbwbrG5þWÓkR ÝZQ2F<ÿoZ9<ÓQÿ82 µwbrGí²î¸Ý¸Q2F 1 L

Link 1

Link 2

Link 3

PSfrag replacements

(x, y)

θ φ1

φ2

X

Y

Fig. 1: snake-like wheeled mobile robot hsX(x, y)

>ªq;LÙ­LX$íîI/º 2ªÃe`wbrÝQRF

1

X aH!ªe<ÊPwX θ,ÝZQRF.PRkH8XG<­<­

φ1, φ2

+0hµujRlS4F7698Xξ :=

[

x, y, θ, φ1, φ2

]T,

+0@ðG;GRÊ8X

g :=[

x, y, θ]T

, q :=[

φ1, φ2

]T

`:wRq95íïîZ<÷PjX$:w2;5@-.

Q = S1 × S

1W7kPrG<Ôg9í²îBöN/º3:ðRXPw>Ê

N7kb|i;pݵY G = SE(2)W7:krG

SE(2)k #L9GÞ@@cqxRÍuÞR¢ ! Lcg¯!#N7:wbrG

Lg :=

cos θ − sin θ x

sin θ cos θ y

0 0 1

(3)

ëìN ü/ý |`ªIx W'c¯35L < ųNÆÁS^N p%¯Á­brG>;`^`_! "2ªÃe

u1 = φ1, u2 = φ2

X ¿À ¯µ­srLÙwsrLÃ;Á!^ÂSÃDGkP³P³(1)M@u÷DjRlS>ÎQMgIxWrGRSªq;

DLg :=

cos θ − sin θ 0sin θ cos θ 0

0 0 1

Γ(q) :=1

∆

− cos(φ1 − φ2) − cosφ1 − cosφ1 − 1sin(φ1 − φ2) + sin φ1 sinφ1

− sin(φ1 − φ2) − sin φ1 − sin φ1

∆ := sin(φ1 − φ2) + sin φ1 − sin φ27kkrG

Remark 1 Γ(q)høDaP8­%r ∆

9ker∆ =

(φ1, φ2) ∈ Q|φ1 = φ2R¡La

0ZxLr<8u`jkl95í²î¸ à j (

QëìLë Qÿ2öD<Ô!Êk7@yNr )³2R9GÔ:ÐGö(!ëì@ë ÿ2ö@Ô )

a2xRd<º2»8a$R8ªq; ÇR­¼'!ëì8a NwbrÁS^SÂÃ85j¹Da2xPd`eª³ijRl@7kkrGu5N7:95ÁS^SÂSÃbX¼<d`eí²î¸ < ųkÆRÅ8X e ru27>;s 7W9 < ÅD"!@ÊZx@r (wDxPy@zu;GW<j:la0¬:r < ÅN5Woq­Lx )

#$Da/c@rPcÙaker∆9

QX

2ÜWZ2Ë>ÊSLa$%!ªÙesªÃ³gÁ ;

QöSX«Å²uc8x < ÅW7P9&!>j2lN5¿_@'¬D¯­@x 5N7k9!!ÊNa()%ª«x cqxbØGQ<Õ2 < Å@2 X+*W>w8rG

•

Ì51kÆPa`9G9

3ÞR;

M9

5Þ:7PRrcs¯Á;

MWg¡WQR÷MW7kkr)2:÷M

Γ9

Γ = adg

[

DLg−1 −Γ(q)]

dx

dy

dθ

dφ1

dφ2

(4)

@cÙa3 × 5

$ ¡ N 75oq­8rGDRZ7

adg := Deιg =

cos θ − sin θ y

sin θ cos θ −x

0 0 1

7kkrG

Remark 2ukDa0 e

24fI9

3#f

−1 ZxPd`e ruL­L9θ = 0

>¡W9θ = −y

¸x@r:¸aGH wbru5j8d`e`Ì1 P:÷M@N¬RX 2 e r@y

θ < ÅL9j¹@7kWrkcÃa5Ü À ruDí%¯X+,-sopsr`a29

SE(2)«R/.&01WG5!ÐkÆP7k:rGD@W)cs¯;

x

y9

ΓG: á `â8X)PQAªÙeG^u_ªq;/º324@9

SE(2)G=k2âbX'P!A!ªÃe5Rwr@ 25NÆkx^<_Q6D`ÉW³G­Ne urG

•

3 798;:=<?>A@CBEDGFH=I9JÞW7`È oq­rPcx<;5>:÷MΓ¾R¢KWQ

ΩX À rG

Ω(X1, X2) := dΓ(hX1, hX2) (5)

2Z7∀X1, X2

9MI½:¾@4DFÙ698RºW7ks|~;

hX9X

H¹WLMD7kkrG

Ω9 ¢

MW¤

g¡

2Þ

W/P÷MN7kg| ;NO©Qªg'k®G­bru¶·D7¸kùWΓZèkéDX'PQA2wbr@Ù;

Ω9

Ω = adgΩ (6)

Ω := dΓ − [Γ(·), Γ(·)]g (7)

o­rGWR¸7dΓ9uÌ1 2R÷MW ¢

Qk72¤KW:7P2rZ«³<

ΩX¸Ì1 KL ÷Mg ` ; ¢

Q:¤

g¡

2ÞW!2÷ Mk7::rP

2MkP

2 Q [·, ·]g9sݵY

Jgk7`oÙ­br

Lie á ( á )

7P:d`e>;g

#@G=k2âa!÷R>wbr Q 7kkrGí²î`A5gEh6~GºR»LèkÆkxk@95ð@ñ:@ca2xWrG

dΓ =

2(1 + cosφ1 + cosφ2 + cosφ1 cosφ2)−sinφ1 + sin φ2 + sin(φ1 + φ2)

sin φ1 + sin φ2 + sin(φ1 + φ2)

dφ1 ∧ dφ2

∆2

(8)

[Γ(·), Γ(·)] =[

0, −∆, 0]T dφ1∧dφ2

∆2 (9)S>T>U@V2`>^`_Pâ%S L ÷M@ÄÆD9`;>ÞNR?@ABLCUT@V8aWc<d`eGËQ¼W¬8¯Á­brG

XY1 (Ambrose-Singer11))

Êξ ∈ M

a0 e V ɪ«r[Z\³kÆ!RL$:P»W;<wLx`ykz`?NA2BLCYJh(ξ)9u;

ξcb¯Ó/]^SöL7GË2ùr'öWeRÊ::P»X

H(ξ) ⊂MµwbrWZðG;

Ω(H(ξ))a_!ª

•

M@<\NY^>èWéPbX %ªÃ<H2JLK@MPORQS>TRUWVPa0 e>;k?@A2BDCqYJL9``^`_Pâ8ÄÆ!` öLa¸H wrZ'è :Æ:a`9h(ξ)

95Êξcb¯aZ\S³:Æ

QR8aSb!Pªe û r[bkÐ3R54F7698D$::»X!ªÙe0|~;

h(ξ) = gNc8qa

G$!-Q.«Í oÁÞSXk¼e:®<`^`_N;

h(ξ) = 0xb¯c`Sö@?@A2B@C>EGFL

¸W¸a2xWrGíîIéN7R9G;

∆(q) 6= 07:Prdg|9G;½:¾La$Xgox

ξufea0 e

Ω(H(ξ)) = se(2)7kkrjDd<e2;

QWÂR§/ºDX$QÊPªÃZ½k¾DaX oZxgÇ2ÈXhrPkIakcL|i;

se(2)a0N¬Prq½:¾W3`Z\³:Æ$

RX V Ék7Wð:rG

4 798jilknm=oAHqpcrtsvuxwzy4.1 |~/&/"/A&f/"^x ¢¡£¤¦¥§¨© [ª«¬­®v¯U°;±+²³v´µ93E¡¶ ±¸·^¹º»½¼¿¾9ÀÁ[ÂÃxÄÅÆ¡ÇÈ"É·/ÊfËÍÌ΢ÏÐ ÀxÑU[µz±ÒÔÓÕÖ¸×Ø"¡Ç

Q Ù ÚÛ ¥[ÜÝ A±¦­/Þß

∂AàlÒ¦Ó¬áâ"ãäå

Γæ"ç/Ó/Ê

Stokesèé ∫

∂AΓ =

∫

AΩ£^êìëí ã"îfï í ã ðfñ¢ ®A¡Ç

Q Ù ·Ê ¥óò M Ù â"ãäå ·"ô"¡3õö äåΓæç/Ó/Ê ¶ ÷+øEf­ùùú"çA¡ ¶ ±[Ê&·û ¥ ç î ¬

Aü ãý ¥þÿ

rA

æ ä Ü"Ý ± ®¬&vÊA¡ äå

ΩA

±πr2

A

íó·f´"ó£ òó·û/¡ÇFig. 2

^£ ¤ Q Ù 3

æ ä Ü/ÝA, B, C

±ÒÓʬԭ®®"&¡ äå î ¥^ò ± ë ä ¥ ¡£ ¤ ¥ /+ª! ¥ [® ¥#"¸¥ çǶ¶ ·Ê¬¸­®® $î&% òÔ¥ ¡£ ¤ (0,−π

2 ),(1,−1), (2,−2)

'(^v±)ÓçA¡Ç/f/""η ∈ se(2)

àlÒÔÓ¬

[ ΩA ΩB ΩC ]

sgn(dirA) · πr2A

sgn(dirB) · πr2C

sgn(dirC) · πr2C

= η (10)

*+fÆri

,Aجsgn(diri)

fA/-#)Ó ä ÜÝ ÞßA.0/1¡ ¶ ±±Ò ¬ ¶ ®ó1 2 ÀÑÔ±cÆ¡Ç34 ±ÒÔÓÆ¡[""AÊ56

η/ç ± ¥ ¡[Ç ¶ 7¬

Q Ù ·&Ê §+¨© 8çÓç¡9 " ¬ ¶:;&<=·φ1

±φ2

Êð>jÒ ¥ ç/Ç

C

singular shapes

A

Q

B

PSfrag replacements

φ1

φ2

π

2

π

2

φ1 = −φ2

Fig. 2: generating an se(2) motion

4.2 ?@0ABDCEFHGJILKM&NO(x, y, θ)

ÅP^9±&¯ì°Í±Ô[²&³´µÆv¡£ ¤ηQ#R¡ó±¸¬

1 S §T »U V/»WYXZ&[f ±Æv¡¹&º+» ¼ ¾¢ÀÁ+ÂÃ]\ ¤ ¶ ± î ·"û¡ÇFig.3

^ Ò_+ Êf¬`a0bcξ(0) = (0, 0,−π

4 , π2 , π

2 )T9 "¯ °0bc

ξd = (0, 0, π4 , π

2 ,−π2 )T d Âfì[Æ ¥feg ­h·

π2 [rad]

S&i \ e +fµ^·"ô"¡Ç Fig. 3ç/Ó¬jÄëvÊf­®®

x, y, θ k7lm¬0n&o0b ^ ¦®+ (fëvÊ ¨&©p æ p æAÄ\/Æ¡<óf¬/"óq 4 0 äå ú"ç/Órè9Ò]+v·"ô¡Ç

3 sft ÀuUvw¢ ®v¡ 1 2 ÀÑyx"±+A¯ °ó zç/ÓçA¡ ¶ ± î e 9^¡Ç ¶ µ^·/Ê5 2 ÀxÑ ·56x¯°bcv|zÒÔÓçA¡fÇ~7

Q Ù ·v¡^±¸¬ Fig. 4 ^ Æ£ ¤ ¬`a(q(0) = (π

2 ,−π2 )T(E±Ò3Ó¬

3æ Ü"Ý

A, B, CÞ/ßv.v1)ÓçA¡fÇ

A, B, Cà l/ÆE¡¦ó&ó ¨&© î " ®"¬/󹺻½¼ ¾9ÀÁ£vê,¢¦®E¡ §¨© þÿ î00zÒÔÓç/ÓçA¡ ¶ ± î e 9^¡Ç

-1.5

-1

-0.5

0

0.5

1

1.5

2

0 20 40 60 80 100 120 140 160

x0

estimate

-3

-2.5

-2

-1.5

-1

-0.5

0

0.5

1

0 20 40 60 80 100 120 140 160

y0

estimate

-1

-0.5

0

0.5

1

1.5

2

0 20 40 60 80 100 120 140 160

thetapi/2

estimate

Fig. 3: P-t-P control: Time response of x, y, θ

-3

-2.5

-2

-1.5

-1

-0.5

0

-0.5 0 0.5 1 1.5 2 2.5 3

phi2

phi1

phi1-phi2singular

Fig. 4: Periodic motion on Q-spaceù+¬Aú"ç+rèÊáw ÆE¡ &·ô¡·¬+A±R] ¥ V& T Ôúç +±Ò Ó 9 ¥!" ³ î PÆA¡Ç3­ ¶ ·1 2 ÀóÑ \Dx/±Ô¬äå £&¡]rè=± Ä"=±+f³v+ízÒ¸ÓÆ¡ ¶ ±+]¡Ç

j 2 ÀAѸ¯¦ ³ó ej

±u ò Çk 2 ÀÑ[¯+&ç/Ó/ʬ

(10)·f,óØf+

[r1, r2, r3]TàzÒÔÓ

[r1, r2, r3]T +

k−1∑

j=1

kIej (11)

±0óDR+ /[úçó¡ ¶ ±±aÆ¡Ç ¶&¶ ·kI

Ê ¥ èf^·"ôxê ¬PIÂÃví ã Z Æ¡ &·"ô"¡Ç

ËlÌ Î¢ÏÐ ÀxÑ ·0WD!Ô»WYXZ+µz±"ÒÔÓ¬ <µl± ¡y`ab cξ(0) = (0, 0,−π

4 , π2 , π

2 )T9 " ¬¢E¯ °bc

ξd = (0, 100,−π4 , π

2 ,−π2 )TQ!R¡ ¶ ±£Eê ¬

y £ A"9)Ó"ù0)Æ¥¤ur/×zÔx¡[Â&â\)+Ç ¶¶ · ³0 ZÊkI = 0.05

±Ò]+&ÇFig.5

Ê ¶ ±û¦ §ó ¨¨ x− y © ï ^ Ò_+ &·"ô"¡Çù+

Fig.6Êrè ³

ej

0^ª«v ^ÒÔÓçA¡¬­`óA " ®¡x £ ó d e 9 ¥¼®¹=Ñ î ¬ ³q 4 £f)Ó¯°¢3®ÓçA¡ª« î e 9^¡fÇ ¥ 0± ¶ ²0³óÊË9Ì&´µvàzÒÔÓ¶f· ¥ d r×^·ôê ¬/£ ò¹¸º" ®+ serpenoid

ëEz ò » 12) ¥¼ ·Ä/ÅÍ3®¢¡²0³z±Ê½ ò_¾¥ )+ &·"ô"¡Ç

-2

0

2

4

6

8

10

-0.4 -0.2 0 0.2 0.4 0.6 0.8 1 1.2 1.4 1.6

y

x

head position

Fig. 5: Iterative curvature feedback : x-y trajectory

-0.01-0.005

0 0.005 0.01

0.015 0.02

0.025 0.03

0.035 0.04

0 50 100 150 200 250 300

erro

r

x

xy

theta

Fig. 6: Iterative curvature feedback : estimation error

5 ¿DÀÂÁÃÄÅ ·/ʬÆÇ ÏÈ ÀÁW s tÉ &!/¡&Ç ÏȦ»Êl± äå ÷[øEË"úzÒ¦¬0[fAÌaÍÎvÏ ÐÑfÒ e E¡ ¶ ±£f)Ó¬/&bcð>^x x¡+0»vÓÔjÒ+Ç " ¬ äå ú"ç"Ó/""rèÆ¡]Õ0Ö󷺡¡ ³ p S ×Lx±º¡]+ ³v+ízÒ¦¬fÌaÍÎØÙÚ »ÛÜÝË"úÆ¡ ×¹"º»½¼¿¾9ÀÁ»ÓÔjÒ]+Ç¥ Ä Þß Ê¬àá0âfãâ/ Þßä åæ0ç èêéëÞß(B) No.17760349 ì £&¡íîï#Ó\ e ®+Ƕ¶ ðñòvóÆ¡Ç

ôöõø÷¥ù1) R.W. Brockett. Asymptotic stability and feedback stabilization.

In Differential Geometric Control Theory, Vol. 27, pp. 181–191.Springer Verlag, 1983.

2) úüû . ýÿþ – –. / !" / #$ , Vol. 45, No. 9, pp. 536–543, 2001.

3) %& , ú û , ' . () * !" ,+.- /01 2435687 359:<; = !" – > ?A@BC!" 7D8EFHGJILKNMPO !" QR –. STVUXW !" YZ[ \] , Vol. 38, No. 10, pp. 839–844, 2002.4) ^ _` , ab , ced . () * !"gf ýþP hi4 P435A9 :j; i !" – 4 kl 74* !" monqp4rs . t 33 u!" v[ 6wx,yi , pp. 363–366, 2004.5) R.M. Murray. Nilpotent basis for a class of nonintegrable distri-

butions with applications to trajectory generation for nonholonomicsystems. Math. Contr. Signals, Syst., pp. 58–74, 1994.

6) ú û . zPf * !" m +8-/ ýþ| | !" . ST nX!" , Vol. 42, No. 10, pp. 841–846, 2003.7) c . ý~!="k – W ii48 !=" –. A , 2000.8) A.M. Bloch. Nonholonomic Mechanics and Control, Vol. 24 of In-

terdisciplinary Applied Mathematics (IAM). Springer Verlag, 2004.9) J. Ostrowski and J. Burdick. The geometric mechanics of undu-

latory robotic locomotion. The International Journal of RoboticsResearch, Vol. 17, No. 7, pp. 683–701, 7 1998.

10) P. Prautsch and T. Mita. Control and analysis of the gait of snakerobots. In International Conference of Control Applications, pp.502–507, 1999.

11) K. Nomizu and S. Kobayashi. Foundations of Differential Geome-try. John Wiley & Sons, 1963.

12) . Y . Z , 1987.