ϕ n n

n

n = 1,..., N

n n

{X I, Y I } {X r, Y r }

(x c, y c ) q r = x a y a θ X r = [x r, y r, θ r ] X I = [x I, y I, θ I ] X I = R(θ)X r R(θ) R(θ) = cosθ sinθ 0 sinθ cosθ 0 0 0 1 Ẋ I = R(θ)Ẋr

y r ẏa r = 0 ẋ a sinθ + ẏ a cosθ = 0 θ R, θ L u DR = R θ R u DL = R θ L R ẋ DR = ẋ a + L θcosθ ẋ DL = ẋ a L θcosθ ẏ DR = ẏ a + L θsinθ ẏ DL = ẏ a L θsinθ

ẋ a cosθ + ẏ a sinθ + θl R θ R = 0 ẋ a cosθ + ẏ a sinθ θl R θ L = 0 sinθ cosθ 0 0 0 cosθ sinθ L R 0 cosθ sinθ L 0 R ẋ a ẏ ȧ θ θ R θ L = Λ(q) q = 0 u = u r + u L 2 = R θ R + θ L 2 ω = u r u L 2L = R θ R θ L. 2 ẋ r a = u, ẏ r a = 0, θ = ω q I = R cosθ R cosθ 2 2 R sinθ R sinθ 2 2 R 2L R 2L [ ] θr θ L

q I = ẋ r a ẏ r a θ = cosθ 0 [ sinθ 0 u ω 0 1 ] u [ u max, u max ] ω [ u max, u max ] ω = u r L L L u L ω max ux = ±ω max u x L u max ωx = ±u max L ω x (ω u) = ± 1 L (u ± u max)

C I X c Y c f C Z c P = (X, Y, Z) p = (x, y) x = f X Z, y = f Y Z (R 3 R 2 ) I P p CP I W H (w, h) (x, y) I (X, Y, Z) w = x p w + w 0 = f p w X Z + w 0 h = y p h + h 0 = f p h Y Z + h 0 p w, p h (w 0, h 0 ) Z c I

I w, p w, f X c Z c θ w W θ w 2 = arctan p 2 w f W 2 θ w = 2arctan p w f (X i, Y i, Z i ) P i P j P j C P i p i

p i p i I P i (w i, h i ) P i X i = (w i w 0 )p w Z i f Y i = (h i h 0 )p h Z i f CP i P i C Y i Y 0 Z i = fy 0 (h i h 0 )p h X i = (w i w 0 )p w Z i f

R j R i R j l ij β ij (u i, ω i ) R i l ij P i, P j ϕ ij R j P I, P j l ij = Z 2 i + X2 i ϕ ij = atan2(z i, X i ) π 2 l ij = (x i x j ) 2 + (y i y j ) 2 ϕ ij = atan2(y i y j, x i x j ) θ j l ij = u i cos(ϕ ij + θ j θ i ) u j cosϕ ij ϕ ij = u isin(ϕ ij + θ j θ i ) u j cosϕ ij l ij ω j

l ij ϕ ij θ i θ j u i ω i = u i cos(ϕ ij + θ j θ i ) u j cosϕ ij u i sin(ϕ ij +θ j θ i ) u j cosϕ ij l ij ω i ω j 0 0 ω j + m(t) ẋ(t) = f[x(t), υ(t)] + m(t) ] z(t) = [ lij ϕ ij = h[x(t)] + s(t) m N(0, M) s N(0, S) x M, S x[k + 1] = f(ˆx[k], υ[k]) + F x (x[k] ˆx[k k]) + F υ υ[k] + F m m[k] z[k + 1] = h(ˆx[k]) + H x (ˆx[k + 1 k] x[k]) + H s s[k] F x, F υ, F m, H x, H s f h x[k + 1] z[k + 1] x[k + 1 k] = x[k] ˆx[k + 1 k] = F x x[k k] + F υ υ[k] + F m m[k] z[k + 1 k] = z[k + 1] h[k + 1 k] = H x x + H s s[k] ˆx[k] x[k + 1] ˆx[k + 1 k] = f(ˆx[k], υ[k])

ˆP [k + 1 k] = F x ˆP [k k]f x + F m ˆM[k]F m [k + 1] = z[k + 1] h(ˆx[k + 1 k]) ˆx[k + 1 k] K[k + 1] = ˆP [k + 1 k]h x (H x ˆP [k + 1 k]h x + H s ŜH S ) 1 ˆx[k + 1 k + 1] = ˆx[k + 1 k] + K[k + 1][k + 1] ˆP [k + 1 k + 1] = ˆP [k + 1 k] K[k + 1]H x ˆP [k + 1 k] M S f, h R j R i

rc max 2a c u l < 0 l ψ l = (x l x f ) 2 + (y l y f ) 2 ψ = π arctan2(y f y l, x l x f ) θ l

x l, y l x f, y f θ l l = 1 2l [2(x l x f )(ẋ l x f ) + 2(y l y f )(ẏ l y f )] = 1 l [(x l x f )(u l cosθ l u f cosθ f ) + (y l y f )(u l sinθ l u f sinθ f )] = u f [ x f x l l cosθ f + y f yl sinθ f ] + u l [ x l x f l l cosθ l + y l yf sinθ l ] l l (x l x f ) (y l y f ) x l x f l y l y f l = cos(θ l + ψ) = sin(θ l + ψ) l = u f (cos(θ l + ψ)cosθ f + sin(θ l + ψ)sinθ f ) u l (cos(θ l + ψ)cosθ l + sin(θ l + ψ)sinθ l ) = u f cos(θ l + ψ θ f ) u l cos(θ l + ψ θ l ) = u f cos(ψ + θ l θ f ) u l cosψ

ψ = θ d dt arctan2(y f y l, x l x f ) 1 ( y f ẏ l )(x l x f ) (y f y l )(ẋ l x f ) = ω l 1 + ( y f y l x l x f ) 2 (x l x f ) 2 = ω l 1 l 2 [u f(cosθ f (y f y l ) + sinθ f (x l x f )) + u l (sinθ l (x f x l ) + cosθ l (y l y f ))] = ω l 1 l [u f(cosθ f sin(θ l + ψ) sinθ f cos(θ l + ψ)) + u l (sinθ l cos(θ l + ψ) cosθ l sin(θ l + ψ) = ω l u f sin(ψ + θ l θ f ) l + u l sinψ l l ψ β = cosγ 0 γ l 0 0 1 [ uf ω f ] + cosψ 0 ψ l 1 0 1 [ ul ω l ] ż = Aυ f + Bυ l υ l = [ u l ω l ] υ f = [ u f ω f ] z = [ l ψ β ] R 3 β = θ l θ f γ = β + ψ

ż = Aυ f + Bυ l y = Cz [ ] 1 0 0 C = 0 1 0 y y d z y υ f [ ] z = l ψ R 2 z = Ãυ f + Bυ l β = ω l ω f y = z ẏ = Ãυ f + Bυ l. υ f = Ã 1 (R Bυ l ) R

y R ẏ = R ω f z x r A Ã = [ cosγ d γ γ l d γ l ( sina c a = arctan cosa c r max = d 2 + r max c ] d r max c ) 2 2dr max cosa c c

R R = ke z = [ k1 e l k 2 e ψ k 1 k 2 e z = ] [ e l e ψ ] = [ l d l ψ d ψ z z d = [ l d ψ d ] ] z = k( z d z), β = ω l ω f y = z υ f υ f = [ uf ω f ] = [ ul β + k 1 e l γ (k 2 e ψ + ω l )l γ u l β + k 1e l γ + (k d d 2e ψ + ω l ) l γ d ] z = z d e l e ψ ė l = k 1 (l d l) = k 1 e l e ψ = k 2 (ψ d ψ) = k 2 e ψ V 1 (e z ) = 1 2 (e2 l + e 2 ψ) > 0, e z 0 V 1 (0) = 0

V 1 (e z ) = e l ė l + e ψ e ψ = k 1 e 2 l k 2 e 2 ψ < 0, e z 0 V 1 (0) = 0 a 1 e z λ V 1 (e z ) a 2 e z λ V 1 (e z ) a 3 e z λ a 1 = a 2 = 1, a 2 3 = k 1 + k 2, λ = 2 β = ω l ω f y = k( z d z) l ψ β β C = {l ψ β} R 3 K

C = {l ψ} R 2 K = { z C l < r max, ψ < a} z z d z(t = 0), z d K z K t 0 l r max ϕ a ϕ = β +ψ +π M C R 3 M = {z C h i (z) 0, i = 1, 2} h 1 = l r max h 2 = ϕ a h 1 0 h 2 0 β

u f u l θ f θ l + π e β = β + π e β = ω l ω f = u l d sine β + δ(e β, e tildez, ω l ) δ = ω l [1 + l d cos(e β + ψ)] + 1 d [k 1e l sin(e β + ψ) + k 2 e ψ lcos(e β + ψ)] l ψ e z = 0 e β (e z = 0) = u l d sine β + δ(e β, 0, ω l ) ṅ = A 0 n A 0 H(s) e β = ω l + 1 d [u lsine β + ω l l d cos(e β + ψ d )] = ω l + u l ω l l d sinψ d sine β + ω ll d cosψ d cose β d d = ω l + ρ 1 cos(e β ρ 2 ) (ul ) 2 ( ) 2 ( ) ω l l d sinψ d ωl l d cosψ d ul ω l l d sinψ d ρ 1 = +, ρ 2 = arctan. d d ω l l d cosψ d e eq β = ρ 2 +arccos( ω l ρ 1 )

e eq β e β = f(e β ) e β = f(e eq β ) + f e β (e eq β )(e β e eq β ) = ρ 1sin e β = ρ 1 1 ( ωl ρ 1 ( ( arccos ω )) l e β ρ 1 2 ) e β V 2 ( e β ) = 1 2 2 e β > 0 e β 0 V 2 (0) = 0 V 2 ( e β ) = ρ 1 1 ( ωl ρ 1 V 2 (0) = 0 2 ) 2 e β < 0 e β 0 ρ 1 > 0 e eq β u l = U ω l = 0 e β = U l sine β e eq β = 0 β π ϕ = ψ + β + π ψ d z d K ψ d < a ϕ < a h 2 < 0 ρ 1 0 u l < 0

t > 0 u l = U ω l = Ω ϕ ϕ eq = e eq β ) + ψeq = ρ 2 + arccos ( Ωρ1 + ψ d h 2 < 0 ϕ eq < a ψ d ψ d = 0 ψ d = π 6 ψ d = π 6 l d = 3 l d l d = 3 l d = 1 l d = 6 ψ d = 0 ϕ ϕ ω l κ = ω l u l u l ( 1 ϕ eq κ = arctan l ) dsinψ d d + arccos l d cosψ d ( 1 l ) + ψ d 2 κ dsinψ d + (ld cosψ d ) 2 κ

ψ d ϕ eq 0 = ψ d l d h 2 < 0 δ(e β, e z, ω l ) δ(e β, e z, ω l ) = 0 e β = f(t, e β ) = u l d sine β V 3 (e β ) = 1 2 e2 β > 0 0, V 3 (0) = 0 V 3 (e β ) = e β e β = u l d e βsine β e β u l < 0 e β 0 e β = 0 e β sine β > 0 e β < π V 3 (t, e β ) : [0, ] D R D = {e β R e β < r}

[0, ] D c 1 e 2 β V 3 (t, e β ) c 2 e 2 β V t + V f(t, e β ) c 3 e 2 β e β V e β c 4 e β c 1 = c 2 = 1 2, c 4 = 1 c 3 = u l d r2 3! + r4 5! c 3 D r c 3 u l d e β = 0 D ω l δ(t, e β, e z, ω l ) ϵ < c 3 c1 θr u l θr c 4 c 2 d t > 0 θ < 1 e β (t 0 ) < c1 c 2 r = r e β (t) k ( ζ(t t 0 )) e β (t 0 ) t 0 t < t 1 e β (t) b t t 1 t 1 k = c2 c 1 = 1 ζ = (1 θ)c 3 (1 θ) u l 2c 2 2d b = c 4 c2 ϵ c 3 c 1 θ d ϵ u l θ V (t, ) [0, ] D D = { R 3 e < c} c > 0 V (t, ) 0

h 1 h 2 M z(t) M M ż T z C T z C C z M z(t) M h i, i 1, 2 M dh i dt = h iż < 0 {z C h i (z) = 0} h i J h (z) h = (h 1, h 2 ) : R 3 R 2 J h (z)ż < 0, J h (z) = [ h1 l h 2 l h 1 ψ h 2 ψ h 1 β h 2 β ] M l = r max ϕ = ±a t = 0

l = r max h 1 = 0 ϕ = a h 2 = 0 h 2 ḣ 2 ϕ=a,l=rmax < 0 d(ψ + e β a) ϕ=a,l=rmax < 0 dt k 1 (l d r max )sina d + (k 2 e ψ + ω l )(1 + r max d cosa) + u l d sine β < 0 l d < r max a (0, π) k 1 > (k 2e ψ + ω l )(d + r max cosa) + u l sin(a ψ) (r max l d )sina h 1 < 0 l l d ḣ1 < 0 l = r max, ϕ = a ḣ 2 ϕ= a,l=rmax < 0 d( ψ e β a) ϕ= a,l=rmax < 0 dt k 1 > (k 2e ψ + ω l )(d + r max cosa) u l sin(a + ψ) (l d r max )sina M z(t = 0) M κ ψ d κ < 0 ω l > 0 κ > 0 ω l < 0 ϕ

κ = 0 κ = 0.3 κ = 0.6 ψ d = a 3 κ = 0 κ = 0.3 κ = 0.6 ψ d = a 3 κ = 0 κ = 0.3 κ = 0.6 ψ d = a 3 κ = 0 κ = 0.3 κ = 0.6 ψ d = a 3 k 1 ψ [ a, a] d = 0.5, a = π 4, r max = 5, l d = 2 k 2 = 0.4

d = 0.5 r max = 6 2a a = π 3 L F 1, F 2 l d = 4 ψ d = ± a 3 u l = 2 κ l ϕ κ l ( ϕ a) 0.26 κ l 0.4 0.4 κ l 0.26 F 1 F 2 κ l = 1 κ 10 l = 1 5 κ l = 1 10 κ l = 1 5 L F 1, F 2

e l = l d l, e ψ = ψ d ψ ϕ F 1 F 2 ψ F 1 F 2 F 1 F 2 l d, ψ d ϕ L l ψ ϕ

F 1 F 2 ϕ

O i i = 1,..., n q s = [x s, y s, θ s ] q g = [x g, y g, θ g ] q(τ) τ [0, T ] κ max ω u = ẏ d(atan ẋ) dt ẋ2 + ẏ = 1 2 (1 + ( ) ẏ 2)(ẋ ẋ + ẏ) 1/2 ẋÿ ẍẏ (ẋ 2 + ẏ 2 ) 1 3/2 ρ min d( ẏ ẋ ) dt

ρ min = 1 κ max q(τ) I( q( )) = T 0 ẋ2 (τ) + ẏ 2 (τ)dτ q(0) = q s q(t ) = q g A( q(τ)) O i =, τ [0, T ] Λ( q) q = 0, τ [0, T ] ẋÿ ẍẏ (ẋ 2 + ẏ 2 ) 1 3/2 ρ min A(q(t)) t q = f(q, υ) ẋ = ucosθ x(0) = x s x(t ) = x g ẏ = usinθ y(0) = y s y(t ) = y g θ = κu θ(0) = θ s θ(t ) = θ g ṡ = ẋ 2 + ẏ 2 s(0) = 0 υ = (u, ω) U R 2 U = { 1, +1} [ 1 ρ min, 1 ρ min ] I(ω) = T 0 ṡdτ

O i K i (q) 0, i = 1,..., m q H(q, s, υ, p, λ, m) = pṡ + λ f(q, υ) + m K(q) m i = 0, K i (q) < 0 0, K i (q) = 0. K i (q) = 0 K i (q) < 0 H(q, s, υ, λ) = pṡ + λ 1 ucosθ + λ 2 usinθ + λ 3 κu λ = (λ, p) λ = Hq (q, υ, λ) λ 1 = 0 λ 2 = 0 λ 3 = λ 1 usinθ λ 2 ucosθ ṗ = 0 δ = λ 2 1 + λ 2 2 α = arctan( λ 2 λ 1 ) H(q, s, υ, λ) = pṡ + δucos(θ α) + λ 3 κu λ 3 = δusin(θ α)

υ H(q, s, υ, λ) H(q, s, υ, λ) δucos(θ α) 0 λ 3 κ 0. H = λ κ 3 = 0 λ 3 = 0 θ = α α + π α H κ 0 κ = ± 1 ρ min ρ min ρ min X r d A 1 (q) C = R 2 S 1 S = {(x, y) R 2 x 2 + y 2 = 1} C obs C C obs = {q C A 1 (q) O }

q A 1 (q) O = {O 1,..., O n } C free = C \ C obs ( 2 {q C obs = {q C A i (q) O }) C A1 (q) A 2 (q) } i=1 {q C A 1 (q) A 2 (q) } l d d A(q) h = rmax c + d rmax c C obs = {q C A(q) O } = {q C c(x l, y l, h) O } ψ A(q) ψ [ a, a] n ( π, π ) ψ 2 2 C obs C obs R 2 θ l

O i A(q) (x l, y l ) C obs q int(o) int(a(q)) = q int(c obs ) int q C obs C obs C free C free C free C obs q(τ) C free q s

q g ( υ = [u max, 0] ) ( υ = [u max, ω max ] ) θ s θ s ρ min (x s, y s ) θ s C obs n h n n 1 ω max = u max h < ρ h min ω max = u max ρ min ρ min ρ = max(ρ min, h) ρ min (x s, y s ) θ s (x s, y s ) q g (x g, y g ) θ g (x g, y g )

h A(q) ρ min G G i j

L F 1, F 2 L l d = 3 L κ l = 1 3 F 1 F 2 L F 1 F 2 L L F 1, F 2 h = r c max+d = 6.5 ρ min = 3 Ω max = V max h = 2 6.5

C obs A(q) F 1 F 2 L l d < h L h = l d + d

n G(V, A) V = {r 1,..., r n } A = {(i, j) i, j V, i j l ij r max, ϕ ij a} (i, j) A r j r i G r i N i = {j V (i, j) A} R i V, i = 1,..., m n R 1 = {r 1 } r j R i, i > 1 L k F j r k R i 1 R i

R 1 = {r 1 } R 2 = {r 2, r 3 } R 3 = {r 4, r 5, r 6 } R 4 = {r 7, r 8, r 9 } [ π, π] A(q) ν G h = ν + 1 2 rc max + d, ν h = νr c maxsina + d, ν A(q) e i l = l d i l i, e i ψ = ψd i ψ i e i β = β i+π V i = 1 2 ( (e i l ) 2 + (e i ψ) 2 + (e i β) 2)

L k F i u k < 0 ω k < Ω max k V = n i=2 V i n u j < 0 ω j < Ω max j j V \ R m u k > 0 R 2 r 1 R 1 d l ψ R j j {1,..., m 2 < m n} ( i = 0 (ω 1 = 0) i [2, n]) r 1 ω 1 = ±Ω max 1 r i R 2

L 1 F 2 L 2 F 3 β i r 1 r j R 3 L k F j r k R 2 c j = l j 2(1 cosbk ) b k = e k β eq r j R 4 L k F j, r k R 3 c j = l 2 j + α2 2l j αcosδ α = c 2 k + l2 j 2c kl j cos(ψ j ϕ eq k + ζ) ζ = arccos c k 2l k δ = ζ + ϕ 0 k ψ j λ λ = arccos α2 + c 2 k l2 j 2αc k ϕ 0 k = ψ k ϕ eq k

r 3 R 3 r 4 R 4 s ij r i r j L lead(i) F i L lead(j) F j s ij > c i + c j + 2d c c i = l 2 i + α2 i 2l iα i cosδ i, r i R j, j [3, m] c i = 0, r i R 1 R 2

α i = c 2 lead(i) + l2 i 2c lead(i)l i cos(ψ i ϕ eq lead(i) + ζ i) ζ i = arccos c lead(i) 2l lead(i) λ i = arccos α2 i + c 2 lead(i) l2 i 2α i c lead(i) δ i = ζ i + ψ lead(i) ψ i λ i, r i R j, j [4, m] δ i = e lead(i) β eq, r i R 3 u j, ω j r j L i F j u j = u i cosβ j ω i l d j sinγ j = u i (cosβ j κ i l d j sinγ j ) ω j = u i d sinβ j + ω i l d j d cosγ j = u i d (sinβ j + κ i l d j cosγ j ) κ i = ω i u i r i r j R 2 r i υ = [Vi max, 0] υ =, V max ] ω j u j [V max i i κ max i r i V max j ( ) = Vi max κ max i lj d sin(e j β eq + π + ψj d ) cos(e j β eq + π) Ω max j = V id max κmax i lj d cos(π + ψj d ) ( V i max κ max i e j β eq = ρ j 2 + arccos ρ j 1 r k r j ) V max k = V max j cos(e k β eq + π) ω j ( β k =e k βeq +π ) l d ksin(e k β eq + π + ψ d k) Ω max k = Ω max j l d k d cos(π + ψd k)

(ω u) = ± 1 (u ± u L max) { u max, u max } { ω max, ω max } R m ϕ i a i V \ r 1 r 1 m d = 0.5 r max = 6 2a a = π 3 R 1 = {r 1 } R 2 = {r 2, r 3 } R 3 = {r 4, r 5 } R 2 R 3 l d = [4, 2] ψ d = ±[ π 5, π 6 ] r 1 u 1 = 1 κ l = 0.1 ρ min = 10 h = 2r max sina = 10.4

r 1 r 2 r 3 r 4 r 5 e l = l d l, e ψ = ψ d ψ ϕ

n n G(V, A) G

n