Python, 24 kroki (prace w toku)
Chodziło o to, aby najpierw rozwiązać ciągły problem, znacznie zmniejszając przestrzeń wyszukiwania, a następnie skwantyzować wynik do siatki (po prostu zaokrąglając do najbliższego punktu siatki i przeszukując otaczające 8 kwadratów)
Parametryzuję ścieżkę jako sumę funkcji trygonometrycznych (w przeciwieństwie do wielomianów, nie rozchodzą się i są łatwiejsze do kontrolowania). Kontroluję także prędkość bezpośrednio zamiast przyspieszenia, ponieważ łatwiej jest wymusić warunek brzegowy, po prostu mnożąc funkcję wagową, która na końcu ma wartość 0.
Moja funkcja celu składa się z:
-wykładniczej oceny przyspieszenia> 10-
wielomianowej oceny odległości euklidesowej między ostatnim punktem a celem
-dużej stałej oceny dla każdego przecięcia ze ścianą, malejącej w kierunku jej krawędzi
Aby zminimalizować wynik, wrzucam to wszystko w optymalizację Neldera-Meada i czekam kilka sekund. Algorytmowi zawsze udaje się dotrzeć do końca, zatrzymując się na nim i nie przekraczając maksymalnego przyspieszenia, ale ma problemy ze ścianami. Ścieżka albo teleportuje się przez rogi i utknie w tym miejscu, albo zatrzymuje się przy ścianie z celem naprzeciwko (lewy obraz)
Podczas testowania miałem szczęście i znalazłem ścieżkę, która została przekręcona w obiecujący sposób (prawy obraz), a po poprawieniu parametrów mogłem użyć jej jako początkowego przypuszczenia dla udanej optymalizacji.
Kwantyzacja
Po znalezieniu ścieżki parametrycznej nadszedł czas, aby usunąć kropki dziesiętne. Patrzenie na dzielnicę 3x3 zmniejsza przestrzeń wyszukiwania z około 300 ^ N do 9 ^ N, ale wciąż jest zbyt duża i nudna do wdrożenia. Zanim poszedłem tą drogą, próbowałem dodać termin „Snap to Grid” do funkcji celu (skomentowane części). Sto kolejnych kroków optymalizacji z zaktualizowanym celem i prostym zaokrągleniem wystarczyło, aby uzyskać rozwiązanie.
[(9, -1), (4, 0), (1, 1), (2, 2), (-1, 2), (-3, 4), (-3, 3), (-2 , 3), (-2, 2), (-1, 1), (0, 0), (1, -2), (2, -3), (2, -2), (3, -5 ), (2, -4), (1, -5), (-2, -3), (-2, -4), (-3, -9), (-4, -4), (- 5, 8), (-4, 8), (5, 8)]
Liczba kroków została ustalona i nie stanowi części optymalizacji, ale ponieważ mamy analityczny opis ścieżki (a ponieważ maksymalne przyspieszenie jest znacznie poniżej 10), możemy ponownie użyć go jako punktu wyjścia do dalszej optymalizacji z mniejszą liczbą terminy
from numpy import *
from scipy.optimize import fmin
import matplotlib.pyplot as plt
from matplotlib.collections import LineCollection as LC
walls = array([[[0,0],[500,0]], # [[x0,y0],[x1,y1]]
[[500,0],[500,400]],
[[500,400],[0,400]],
[[0,400],[0,0]],
[[200,200],[100,200]],
[[100,200],[100,100]],
[[100,100],[200,100]],
[[250,300],[250,200]],
[[300,300],[300,100]],
[[300,200],[400,200]],
[[300,100],[400,100]],
[[100,180],[120, 200]], #debug walls
[[100,120],[120, 100]],
[[300,220],[320, 200]],
#[[320,100],[300, 120]],
])
start = array([120,180])
goal = array([320,220])
###################################
# Boring stuff below, scroll down #
###################################
def weightedintersection2D(L1, L2):
# http://stackoverflow.com/questions/563198/how-do-you-detect-where-two-line-segments-intersect
p = L1[0]
q = L2[0]
r = L1[1]-L1[0]
s = L2[1]-L2[0]
d = cross(r,s)
if d==0: # parallel
if cross(q-p,r)==0: return 1 # overlap
else:
t = cross(q-p,s)*1.0/d
u = cross(q-p,r)*1.0/d
if 0<=t<=1 and 0<=u<=1: return 1-0*abs(t-.5)-1*abs(u-.5) # intersect at p+tr=q+us
return 0
def sinsum(coeff, tt):
'''input: list of length 2(2k+1),
first half for X-movement, second for Y-movement.
Of each, the first k elements are sin-coefficients
the next k+1 elements are cos-coefficients'''
N = len(coeff)/2
XS = [0]+list(coeff[:N][:N/2])
XC = coeff[:N][N/2:]
YS = [0]+list(coeff[N:][:N/2])
YC = coeff[N:][N/2:]
VX = sum([XS[i]*sin(tt*ww[i]) + XC[i]*cos(tt*ww[i]) for i in range(N/2+1)], 0)
VY = sum([YS[i]*sin(tt*ww[i]) + YC[i]*cos(tt*ww[i]) for i in range(N/2+1)], 0)
return VX*weightfunc, VY*weightfunc
def makepath(vx, vy):
# turn coordinates into line segments, to check for intersections
xx = cumsum(vx)+start[0]
yy = cumsum(vy)+start[1]
path = []
for i in range(1,len(xx)):
path.append([[xx[i-1], yy[i-1]],[xx[i], yy[i]]])
return path
def checkpath(path):
intersections = 0
for line1 in path[:-1]: # last two elements are equal, and thus wrongly intersect each wall
for line2 in walls:
intersections += weightedintersection2D(array(line1), array(line2))
return intersections
def eval_score(coeff):
# tweak everything for better convergence
vx, vy = sinsum(coeff, tt)
path = makepath(vx, vy)
score_int = checkpath(path)
dist = hypot(*(path[-1][1]-goal))
score_pos = abs(dist)**3
acc = hypot(diff(vx), diff(vy))
score_acc = sum(exp(clip(3*(acc-10), -10,20)))
#score_snap = sum(abs(diff(vx)-diff(vx).round())) + sum(abs(diff(vy)-diff(vy).round()))
print score_int, score_pos, score_acc#, score_snap
return score_int*100 + score_pos*.5 + score_acc #+ score_snap
######################################
# Boring stuff above, scroll to here #
######################################
Nw = 4 # <3: paths not squiggly enough, >6: too many dimensions, slow
ww = [1*pi*k for k in range(Nw)]
Nt = 30 # find a solution with tis many steps
tt = linspace(0,1,Nt)
weightfunc = tanh(tt*30)*tanh(30*(1-tt)) # makes sure end velocity is 0
guess = random.random(4*Nw-2)*10-5
guess = array([ 5.72255365, -0.02720178, 8.09631272, 1.88852287, -2.28175362,
2.915817 , 8.29529905, 8.46535503, 5.32069444, -1.7422171 ,
-3.87486437, 1.35836498, -1.28681144, 2.20784655]) # this is a good start...
array([ 10.50877078, -0.1177561 , 4.63897574, -0.79066986,
3.08680958, -0.66848585, 4.34140494, 6.80129358,
5.13853914, -7.02747384, -1.80208349, 1.91870184,
-4.21784737, 0.17727804]) # ...and it returns this solution
optimsettings = dict(
xtol = 1e-6,
ftol = 1e-6,
disp = 1,
maxiter = 1000, # better restart if not even close after 300
full_output = 1,
retall = 1)
plt.ion()
plt.axes().add_collection(LC(walls))
plt.xlim(-10,510)
plt.ylim(-10,410)
path = makepath(*sinsum(guess, tt))
plt.axes().add_collection(LC(path, color='red'))
plt.plot(*start, marker='o')
plt.plot(*goal, marker='o')
plt.show()
optres = fmin(eval_score, guess, **optimsettings)
optcoeff = optres[0]
#for c in optres[-1][::optimsettings['maxiter']/10]:
for c in array(optres[-1])[logspace(1,log10(optimsettings['maxiter']-1), 10).astype(int)]:
vx, vy = sinsum(c, tt)
path = makepath(vx,vy)
plt.axes().add_collection(LC(path, color='green'))
plt.show()
Zadanie do wykonania: graficzny interfejs użytkownika, który pozwala narysować początkową ścieżkę, aby uzyskać przybliżone wyczucie kierunku. Wszystko jest lepsze niż losowe próbkowanie z 14-wymiarowej przestrzeni
