在matplotlib中遇到避障问题

Question

我正在尝试制作一种算法，其中多个代理（蓝色）作为一个团队协同工作，通过在2D网格中执行周围和盘旋战术来捕获稍快的敌方代理（红色）（因此没有图形或节点，实现像A *这样的算法。所以我试图制作一个强大的多智能体算法，允许多智能体捕获智能和更快的敌方代理

然而，敌方特工（红色）只会朝着出口前进，并遇到阻挡它的特工（蓝色）。因此，我试图实现一个基本的避障，其目标是在同时移动代理的同时前往出口。

但是，我在制作良好的实施方面遇到了麻烦

我尝试过的避障算法图

算法描述

基本上发生的事情是，箭头，敌方代理人，将检测其半径内是否有一个代理（形状像一个半圆）。

如果是这样，它将检查自身和出口之间是否有代理（使用等式：y = mx + h）。

如果检测到某些东西，敌方代理将计算检测到的代理的左侧和右侧的兴趣点。然后它将尝试计算哪个兴趣点最接近出口。一旦完成，它将改变其轨迹并转到该兴趣点，而不是冲向出口并进入代理

但问题是，我的敌方特工最终会产生锯齿形图案而不是真正避开任何粒子，我无法找到解决方法。

所以我想知道你们是否可以帮我解决这个问题。

此外，虽然代码很长，但敌方代理使用的唯一功能是goToExit（），因此该函数及其调用的任何函数将是唯一相关的函数。

代码（要避开障碍物，将obstacleAvoidance变量切换为False）：

import numpy as np
from matplotlib import pyplot as plt
from matplotlib import animation
from random import randint

import random
import math

keep = False
keepX = 0
keepY = 0

### Variables that we can play with ###
interestPointVisual = False
huntEnemy = True
numOfAgents = 10

enemyTopSpeed = 0.6
topSpeed = 0.3

secondDoor = False
resultVisual = False

obstacleAvoidance = True

maxFrame = 300
####################################


phaseCount = 0

fig = plt.figure()
fig.set_dpi(100)
fig.set_size_inches(5, 4.5)

# Declaring the enemy and ally agents
ax = plt.axes(xlim=(0, 100), ylim=(0, 100))
enemy = plt.Circle((10, -10), 0.95, fc='r')
agent = plt.Circle((10, -10), 0.95, fc='b')


if interestPointVisual:
    interestColor = 'y'
    interestSize = 0.55

else:
    interestColor = 'w'
    interestSize = 0.55
    #interestSize = 0.000001



midpoint = plt.Circle((10, -10), interestSize, fc=interestColor)

eastpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
northpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
westpoint = plt.Circle((10, -10), interestSize, fc=interestColor)


northeastpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
mideastpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
midwestpoint = plt.Circle((10, -10), interestSize, fc=interestColor)
northwestpoint = plt.Circle((10, -10), interestSize, fc=interestColor)

# Adding the exits
rect_size = 5
x_se_s = 47

x_se = 50
y_se = 0

southExit = plt.Rectangle([x_se_s - rect_size / 2, y_se - rect_size / 2], rect_size + 3, rect_size -2 , facecolor='black', edgecolor='black')

x_ne = 50
y_ne = 101

if secondDoor:
    northExit = plt.Rectangle([x_ne - rect_size / 2, y_ne - rect_size / 2], rect_size + 3, rect_size -2 , facecolor='black', edgecolor='black')


patches_ac = []

if interestPointVisual:
    ax.add_patch(midpoint)
    ax.add_patch(northpoint)
    ax.add_patch(eastpoint)
    ax.add_patch(westpoint)

    ax.add_patch(mideastpoint)
    ax.add_patch(midwestpoint)
    ax.add_patch(northeastpoint)
    ax.add_patch(northwestpoint)


# enemy, north, east, south, west
# 0 represents unoccupied, 1 represent occupied
global occupied_ar
global victory
global agentID
global timeStep

global agentLocationAR



ax.add_patch(agent)

for x in range(0, numOfAgents - 1):
    agent_clone = plt.Circle((10, -10), 0.95, fc='b')
    agent_clone.center = (random.randint(1, 100), random.randint(1, 100))
    patches_ac.append(agent_clone)
    ax.add_patch(agent_clone)

ax.add_patch(enemy)

# Adding exit patches
ax.add_patch(southExit)

if secondDoor:
    ax.add_patch(northExit)




def init():
    global occupied_ar
    global agentLocationAR
    global keep
    global keepX
    global keepY

    keep = False
    keepX = 0
    keepY = 0

    #enemy.center = (50, 50)
    enemy.center = (random.randint(1, 100), random.randint(55, 100))
    agent.center = (random.randint(1, 100), random.randint(1, 100))

    occupied_ar = np.zeros([9])
    agentLocationAR = np.zeros((numOfAgents,2))


    for ac in patches_ac:
        ac.center = (random.randint(1, 100), random.randint(1, 100))





    return []

def animationManage(i):
    global occupied_ar
    global agentLocationAR
    global victory
    global agentID
    global timeStep
    global phaseCount
    global maxFrame

    timeStep = i


    agentID = 1
    followTarget(i, agent, enemy)

    agentLocationAR[agentID-1][0], agentLocationAR[agentID-1][1] = agent.center

    for ac in patches_ac:
        agentID = agentID + 1
        followTarget(i, ac, enemy)

        agentLocationAR[agentID-1][0], agentLocationAR[agentID-1][1] = ac.center

    goToExit(i, enemy, southExit)
    # printing tests


    if i >= maxFrame - 1:
        print 'Phase ', phaseCount
        phaseCount += 1
        if resultVisual:
            print occupied_ar
            print 'Victory: ', victory

    return []


def goToExit(i, patch, exit_patch):
    global agentLocationAR
    global keep
    global keepX
    global keepY
    x, y = patch.center
    v_x, v_y = velocity_calc_exit(patch, exit_patch)

    mid_x, mid_y, rad_x, rad_y = getMidDistance(patch, exit_patch)
    rad_size = math.sqrt(rad_x**2 + rad_y**2)

    if obstacleAvoidance:
        if i % 7 == 0:
            keepX = 0
            keepY = 0
            keep = False
            # Change path if an agent is blocking the exit
            change_x, change_y = checkRadius(patch, rad_size)
            #checkRadius(patch, rad_size)

            if ((not (change_x == -9999)) and (not (change_y == -9999))):
                v_x = change_x
                v_y = change_y
                keepX = change_x
                keepY = change_y
                keep = True
        elif keep:

            v_x = keepX
            v_y = keepY

    # x position
    x += v_x

    # y position
    y += v_y


    patch.center = (x, y)


    return patch,

def followTarget(i, patch, enemy_patch):
    x, y = patch.center

    # Will try to follow enemy
    #v_x, v_y = velocity_calc(patch, enemy_patch)

    # Will follow midpoint of enemy & exit
    v_x, v_y = velocity_calc_mid(patch, enemy_patch)  

    #print 'Here:'
    #print interest_ar


    # x position
    x += v_x

    # y position
    y += v_y

    patch.center = (x, y)
    return patches_ac


def getInterestPoints(enemy_patch, exit_patch):
    # Calculate interest points to attract agents

    x, y = enemy_patch.center

    # Calculate enemy-to-exit midpoint
    mid_x, mid_y, rad_x, rad_y = getMidDistance(enemy_patch, exit_patch)

    interest_ar = np.array([[x,y],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0],[0,0]])

    #north
    interest_ar[1][0] = x - rad_x
    interest_ar[1][1] = y - rad_y


    #east
    interest_ar[3][0] = x - rad_y
    interest_ar[3][1] = y + rad_x


    #south (basically the midpoint)
    interest_ar[5][0] = x + rad_x
    interest_ar[5][1] = y + rad_y


    #west
    interest_ar[7][0] = x + rad_y
    interest_ar[7][1] = y - rad_x




    # northeast
    interest_ar[2][0] = (interest_ar[1][0] + interest_ar[3][0])/2
    interest_ar[2][1] = (interest_ar[1][1] + interest_ar[3][1])/2

    #southeast
    interest_ar[4][0] = (interest_ar[3][0] + interest_ar[5][0])/2
    interest_ar[4][1] = (interest_ar[3][1] + interest_ar[5][1])/2

    #southwest
    interest_ar[6][0] = (interest_ar[5][0] + interest_ar[7][0])/2
    interest_ar[6][1] = (interest_ar[5][1] + interest_ar[7][1])/2

    interest_ar[8][0] = (interest_ar[7][0] + interest_ar[1][0])/2
    interest_ar[8][1] = (interest_ar[7][1] + interest_ar[1][1])/2


    # Setting up visuals
    northpoint.center = (interest_ar[1][0], interest_ar[1][1])
    eastpoint.center = (interest_ar[3][0], interest_ar[3][1])
    midpoint.center = (interest_ar[5][0], interest_ar[5][1])
    westpoint.center = (interest_ar[7][0], interest_ar[7][1])

    mideastpoint.center = (interest_ar[2][0], interest_ar[2][1])
    midwestpoint.center = (interest_ar[4][0], interest_ar[4][1])
    northeastpoint.center = (interest_ar[6][0], interest_ar[6][1])
    northwestpoint.center = (interest_ar[8][0], interest_ar[8][1])


    return interest_ar


def findClosestInterest(agent_patch, in_ar):
    # For some reason, north never gets occupied

    # north east is (north/2) + (south/2)
    global occupied_ar
    global victory
    global agentID
    global timeStep
    global huntEnemy


    victory = False

    index = -1
    smallDis = 999999


    tempAr = np.zeros([9])

    if huntEnemy:
        minDis = 0
    else:
        minDis = 1

    # To check agent's distance of all interest points
    for i in range(minDis,9):
        dis = abs(int(getDistance(agent_patch, in_ar, i)))


       # Add heavy weights to charge at enemy
        if i == 0:
            dis = dis*0.5



        if occupied_ar[i] != 0:
            # we must write a condition so that agent knows it is the
            # one that is occupying it
            dis = dis*5

        # Add heavy weights to avoid the back
        if i == 1 or i == 8 or i == 2:

            if i == 1:
                dis = dis*3
            elif i == 2 or i == 8:
                dis = dis*4


        tempAr[i] = dis




        # When we discover unoccupied shorter distance, replace index        
        if dis < smallDis:

            # index is agent_patch.center[0] < 47 and agent_patch.center[0] > 53the index of interest_array of the closest interest point 
            smallDis = dis
            index = i

    # If the smallest distance is less than 10, we are currently engaged


    if smallDis < 0.5:
        # We are near or at the targeted interest point,
        # now we should update array as occupied


        occupied_ar[index] = agentID

        if occupied_ar[0] != 0:
            victory = True

        #print 'engaged index ', index
    else:
        # Else we are still far away from the index
        if occupied_ar[index] == agentID:
            occupied_ar[index] = 0


            #print 'lost track of index ', index
        #else:
            #print 'far away from index ', index


    return index

def getBypassInterestPoints(user_patch,avoidX, avoidY, exit_x, exit_y):
    # Mainly used by the enemy agent
    # User agent will find a point around the blocking agent that is closest to
    # the exit.
    x,y = user_patch.center
    rad_range = 20

    tempX = x - avoidX
    tempY = y - avoidY

    diffR = math.sqrt(tempX**2 + tempY**2)

    # Calculating our target x and y length
    radX = (rad_range*tempX)/diffR
    radY = (rad_range*tempY)/diffR

    # Now we calculate the main interest points

    # Since we are calculating perpendicular points, we reverse the X and Y
    # in the pt calculation process
    pt1X = avoidX + radY
    pt1Y = avoidY - radX

    ###
    pt2X = avoidX - radY
    pt2Y = avoidY + radX

    # Then we must determine which interest point is closer to the exit

    pt1Dis = int(getDistanceScalar(pt1X, pt1Y,exit_x, exit_y))
    pt2Dis = int(getDistanceScalar(pt2X, pt2Y,exit_x, exit_y))

    '''
    print 'user: ', x, ' ', y
    print 'blockAgent: ', avoidX, ' ', avoidY
    print 'pt1: ', pt1X, ' ', pt1Y
    print 'pt2: ', pt2X, ' ', pt2Y

    exit()
    '''


    #print '(', pt1X, ' and ', pt1Y, ') and (', pt2X, ' and ', pt2Y, ')'
    #print pt1Dis, ' vs ', pt2Dis
    #print int(pt1X), ' vs ', int(pt2X) 

    # If point 1 is closer to the exit than point 2
    if(int(pt1Dis) <= int(pt2Dis)):
        print int(pt1X)
        return pt1X, pt1Y

    print int(pt2X)
    return int(pt2X), int(pt2Y)


def checkInLine(user_patch, exit_x, exit_y, avoidX, avoidY):
    # Check if an agent is in the user's lined range
    global agentLocationAR
    x1 = exit_x
    y1 = exit_y
    x2, y2 = user_patch.center  # the user agent
    x2 = int(x2)
    y2 = int(y2)

    # avoidX and avoidY are the agents

    # Check other y-intercepts
    #return checkYInterRange(x1,y1,x2,y2, avoidX, avoidY)
    # We will change y intercept to see if anything is near the main line
    xThresh = 5   # the range limit of the used y intercepts
    start = x2 - xThresh
    finish = x2 + xThresh


    # To avoid division by zero error
    if x2-x1 == 0:
        # That means user is directly above the exit
        # They both share the same x value
        for xi in range(start, finish):
            #print avoidX, ' vs ', xi
            # an agent is directly on the vertical line
            if avoidX == xi:

                #print 'There is an agent in the way'
                tarX, tarY = getBypassInterestPoints(user_patch,avoidX, avoidY, exit_x, exit_y)
                return tarX, tarY


        return -9999, -9999


    else:

        for yi in range(start, finish):

            lineEq = ((y2-x2)/(x2-x1))*(avoidX-x1) + yi

            if avoidY == lineEq:
                # There is an agent in its linear path
                # Now we must calculate a point to go to in order to go around it

                tarX, tarY = getBypassInterestPoints(user_patch,avoidX, avoidY, exit_x, exit_y)



                #print 'There is an agent in the way'
                return tarX, tarY

        return -9999, -9999


def getDistanceScalar(x1, y1, x2, y2):
    return math.sqrt((x2 - x1)**2 + (y2 - y1)**2)

def getDistance(agent_patch, in_ar, index):
    x_a, y_a = agent_patch.center
    x_t = in_ar[index][0]
    y_t = in_ar[index][1]


    # get distance between two particles
    return math.sqrt((x_t - x_a)**2 + (y_t - y_a)**2)


def getMidDistance(enemy_patch, exit_patch):
    # Get midpoint between enemy agent and exit

    x, y = enemy_patch.center
    x_e = x_se
    y_e = y_se

    # Get midpoint values
    mid_x = (x + x_e)/2
    mid_y = (y + y_e)/2

    # Get radius values
    rad_x = mid_x - x
    rad_y = mid_y - y

    # Returns (midpoint x and y) values and (radius x and y) values
    return mid_x, mid_y, rad_x, rad_y

def top_speed_regulate(curr_speed, top_speed):

    if curr_speed > top_speed:
        return top_speed
    elif curr_speed < -top_speed:
        return -top_speed
    else:
        return curr_speed

def velocityCalcScalar(x1, y1, x2, y2):

    veloX = top_speed_regulate( (x2 - x1)      ,enemyTopSpeed)
    veloY = top_speed_regulate( (y2 - y1)      ,enemyTopSpeed)

    return veloX, veloY


# Calculate velocity to rush to exit
def velocity_calc_exit(agent_patch, exit_patch):

    x, y = agent_patch.center
    #x_e, y_e = exit_patch.center
    x_e = x_se
    y_e = y_se

    velo_vect = np.array([0.0, 0.0], dtype='f')

    dis_limit_thresh = 1 



    velo_vect[0] = top_speed_regulate( (x_e - x)* dis_limit_thresh    ,enemyTopSpeed)
    velo_vect[1] = top_speed_regulate( (y_e - y)* dis_limit_thresh    ,enemyTopSpeed)

    return velo_vect[0], velo_vect[1]


# Calculate velocity to chase down enemy
def velocity_calc(agent_patch, enemy_patch):

    x, y = agent_patch.center
    x_e, y_e = enemy_patch.center

    velo_vect = np.array([0.0, 0.0], dtype='f')

    dis_limit_thresh = 1 



    velo_vect[0] = top_speed_regulate( (x_e - x)* dis_limit_thresh    ,topSpeed)
    velo_vect[1] = top_speed_regulate( (y_e - y)* dis_limit_thresh    ,topSpeed)

    return velo_vect[0], velo_vect[1]

# Calculate velocity to arrive at midpoint between enemy and exit
def velocity_calc_mid(agent_patch, enemy_patch):


    x, y = agent_patch.center
    x_e, y_e, _, _ = getMidDistance(enemy_patch, southExit)

    # We get location of interest points as well as animate the interest points
    interest_ar = getInterestPoints(enemy_patch, southExit)

    interest_index = findClosestInterest(agent_patch, interest_ar)




    x_e = interest_ar[interest_index][0]
    y_e = interest_ar[interest_index][1]




    velo_vect = np.array([0.0, 0.0], dtype='f')

    dis_limit_thresh = 1 

    topSpeed = 0.3

    velo_vect[0] = top_speed_regulate( (x_e - x)* dis_limit_thresh    , topSpeed)
    velo_vect[1] = top_speed_regulate( (y_e - y)* dis_limit_thresh    , topSpeed)

    return velo_vect[0], velo_vect[1]


def checkRadius(user_patch, r):
    global agentLocationAR
    r = 10
    for i in range(0,numOfAgents-1):
        x = int(agentLocationAR[i][0])
        y = int(agentLocationAR[i][1])

        if(inSemiRadius(user_patch, x, y, r)):
            # if an agent is in the user's radius
            tarX, tarY = checkInLine(user_patch, int(x_se),int(y_se) , x, y)

            if (not (tarX == -9999)) and (not (tarY == -9999)):
                # We have detected an agent
                # Now we must change the velocity direction of the user
                userX = user_patch.center[0]
                userY = user_patch.center[1]

                tarX, tarY = velocityCalcScalar(userX, userY, tarX, tarY)
                #print 'Detected agent ', i, ' at (', x,',',y ,') while at (', user_patch.center[0], ' ', user_patch.center[1], ')'

                return tarX, tarY

    return -9999, -9999 

def inRadius(self_patch, pointX, pointY, r):
    # Helps determine if there is something near the using agent

    x, y = self_patch.center # agent emitting the radius
    # agent we are trying to avoid

    h = pointX
    k = pointY    
    # Equation of circle
    # (x-h)^2 + (y-k)^2 <= r^2

    tempX = (x - h)**2
    tempY = (y - k)**2

    r_2 = r**2

    if tempX + tempY <= r_2:
        # It is within the radius
        return True
    else:
        return False


def inSemiRadius(self_patch, pointX, pointY, r):
    # Helps determine if there is something near the using agent

    h, k = self_patch.center # agent emitting the radius
    # agent we are trying to avoid
    x = pointX
    y = pointY
    # Equation of semicircle

    tempTerm = r**2 - (x-h)**2

    if tempTerm < 0:
        # if this term is negative, that means agent to avoid is out of range
        return False

    tempEq = k - math.sqrt(tempTerm)


    if y <= tempEq:
        # It is within the radius
        return True
    else:
        return False



def animateCos(i, patch):
    x, y = patch.center
    x += 0.1

    y = 50 + 30 * np.cos(np.radians(i))
    patch.center = (x, y)
    return patch,


anim = animation.FuncAnimation(fig, animationManage,
                               init_func=init,
                               frames=maxFrame,
                               interval=1,
                               blit=True,
                               repeat=True)


plt.show()

有谁知道如何解决这个问题？这是否可以解决，或者我是否必须完全抓住这一点并着眼于另一种避障算法？