DWM路径规划（结合Matplotlib.animation）

综述

原文章
本次重写代码，用matplotlib内置的动画模块代替上一篇博文的逐帧显示逻辑，同时修改了上一篇博文的一些错误，改进代码，更具逻辑性。
算法实现逻辑在上一篇博文里面，在这里就不再赘述了。原理我也是看别人的代码和文章学习的，虽然不是原创算法，但是好歹代码是自己依据自己的学习写出来的。

import time
import math
import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation

def distance(point1, point2):
    return np.sqrt((point1[0] - point2[0]) ** 2 + (point1[1] - point2[1]) ** 2)
class state(object):
    def __init__(self, time, x, y, velocity, yaw, yawrate, dyawrate, cost):
        self.time = time
        self.x = x
        self.y = y
        self.velocity = velocity
        self.yaw = yaw
        self.yawrate = yawrate
        self.dyawrate = dyawrate
        self.cost = cost
class obstacle(object):
    def __init__(self,x,y,rand_coeff):
        self.rand_coeef=rand_coeff
        self.locus_x = []
        self.locus_y = []
        self.locus_x.append(x)
        self.locus_y.append(y)
    def update(self):
        alpha = 2 * np.random.random() * np.pi
        self.locus_x.append(self.locus_x[-1] + self.rand_coeef * np.cos(alpha))
        self.locus_y.append(self.locus_y[-1] + self.rand_coeef * np.sin(alpha))
class DWM(object):
    def __init__(self,init_state,goal,obstacle_num):
        self.dt = 0.1
        self.maxvelocity = 1.4
        self.minvelocity = 0
        self.maxlinearacc = 0.2
        self.maxdyawrate = 40 * math.pi / 180
        self.velocityres = 0.01
        self.yawrateres = 0.5 * math.pi / 180
        self.predicttime = 3
        self.to_goal_coeff = 1.0
        self.velocity_coeff = 1.0
        self.saferadius = 0.5
        self.goal = goal
        self.arrive = False
        self.states=[]
        self.best_locus_x=[]
        self.best_locus_y=[]

        self.obstacles=[]
        self.fig,self.ax=plt.subplots()

        self.display_obstacle=[]
        self.display_arrow=self.ax.quiver([],[],[],[])
        self.display_pre,=self.ax.plot([],[])
        self.display_locus,=self.ax.plot([],[])
        self.display_goal,=self.ax.plot([],[])
        for i in range(obstacle_num):
            x = np.random.randint(-4, 15)
            y = np.random.randint(-4, 15)
            self.obstacles.append(obstacle(x,y,0.1))
            plot,=self.ax.plot([],[])
            self.display_obstacle.append(plot)
        self.states.append(init_state)

    def cal_lim(self):
        x=[]
        y=[]
        for obs in self.obstacles:
            x.append(obs.locus_x[-1])
            y.append(obs.locus_y[-1])
        x.append(self.states[0].x)
        y.append(self.states[0].y)
        x.append(self.goal[0])
        y.append(self.goal[1])

        return np.min(x),np.max(x),np.min(y),np.max(y)
    def display_init(self):

        x_min,x_max,y_min,y_max=self.cal_lim()
        self.ax.set_xlim(1.5*x_min,1.5*x_max)
        self.ax.set_ylim(1.5*y_min,1.5*y_max)

        return self.display_obstacle[0],\
               self.display_obstacle[1],\
               self.display_obstacle[2],\
               self.display_obstacle[3],\
               self.display_obstacle[4],\
               self.display_obstacle[5],\
               self.display_obstacle[6],\
               self.display_pre,self.display_arrow,self.display_locus,self.display_goal
    def reach(self):
        if distance([self.states[-1].x,self.states[-1].y],self.goal)<0.1:
            return True
        return False
    def display_update(self,frame):
        if not self.reach():
            best_uv,cost=self.search_for_best_uv()
            _=self.motion(best_uv[0],best_uv[1])
            self.states[-1].cost=cost
            for obs in self.obstacles:
                obs.update()

            for i in range(len(self.obstacles)):
                self.display_obstacle[i],=self.ax.plot([self.obstacles[i].locus_x[-1],],
                                                      [self.obstacles[i].locus_y[-1],],"b.-")
            self.display_pre,=self.ax.plot(self.best_locus_x,self.best_locus_y,'g')
            self.display_arrow=self.ax.quiver(self.states[-1].x,
                                              self.states[-1].y,
                                              math.cos(self.states[-1].yaw),
                                              math.sin(self.states[-1].yaw),units='width'
                                             )
            x=[]
            y=[]
            for sta in self.states:
                x.append(sta.x)
                y.append(sta.y)
            self.display_locus,=self.ax.plot(x,y,'y-')
            self.display_goal,=self.ax.plot([self.goal[0],self.states[-1].x],[self.goal[1],self.states[-1].y],"r.")
        else:
            for i in range(len(self.obstacles)):
                self.display_obstacle[i],=self.ax.plot(self.obstacles[i].locus_x,
                                                      self.obstacles[i].locus_y,"blue",linewidth=0.2)
        self.ax.grid(True)

        return self.display_obstacle[0],\
               self.display_obstacle[1],\
               self.display_obstacle[2],\
               self.display_obstacle[3],\
               self.display_obstacle[4],\
               self.display_obstacle[5],\
               self.display_obstacle[6],\
               self.display_pre,self.display_arrow,self.display_locus,self.display_goal
    def motion_windows(self):
        current_velocity = self.states[-1].velocity
        current_yawrate = self.states[-1].yawrate
        maxvelocity = np.min([self.maxvelocity, current_velocity + self.maxlinearacc * self.dt])
        minvelocity = np.max([self.minvelocity, current_velocity - self.maxlinearacc * self.dt])
        maxyawrate = current_yawrate + self.maxdyawrate * self.dt
        minyawrate = current_yawrate - self.maxdyawrate * self.dt

        return np.array([minvelocity, maxvelocity, minyawrate, maxyawrate])
    def motion(self, velocity, yawrate):
        temp_state = state(self.states[-1].time + self.dt,
                           self.states[-1].x + velocity * math.cos(self.states[-1].yaw) * self.dt,
                           self.states[-1].y + velocity * math.sin(self.states[-1].yaw) * self.dt,
                           velocity,
                           self.states[-1].yaw + yawrate * self.dt,
                           yawrate,
                           (yawrate - self.states[-1].yawrate) / self.dt,
                           0)
        self.states.append(temp_state)
        return temp_state

    def cost_goal(self, locus):
        return distance(np.array([locus[-1].x, locus[-1].y]), self.goal) * self.to_goal_coeff

    def cost_velocity(self, locus):
        return (self.maxvelocity - locus[-1].velocity) * self.velocity_coeff

    def cost_obstacle(self, locus):

        dis = []
        for i in locus:
            for ii in self.obstacles:
                dis.append(distance(np.array([i.x, i.y]), np.array([ii.locus_x[-1],ii.locus_y[-1]])))
        dis_np = np.array(dis)
        return 1.0 / np.min(dis_np)

    def cost_total(self, locus):
        return self.cost_goal(locus) + self.cost_velocity(locus) + self.cost_obstacle(locus)
    def search_for_best_uv(self):
        windows = self.motion_windows()
        best_uv = np.array([0, 0])
        currentcost = np.inf

        initship = self.states[:]
        best_locus = []

        for i in np.arange(windows[0], windows[1], self.velocityres):
            for ii in np.arange(windows[2], windows[3], self.yawrateres):
                locus = []
                t = 0
                while (t <= self.predicttime):
                    locus.append(self.motion(i, ii))
                    t = t + self.dt
                newcost = self.cost_total(locus)
                if currentcost > newcost:
                    currentcost = newcost
                    best_uv = [i, ii]
                    # best_locus=copy.deepcopy(locus)
                    # best_locus = locus[:]

                self.states = initship[:]

            self.states = initship[:]

        self.states = initship[:]
        self.best_locus_x.clear()
        self.best_locus_y.clear()
        for point in locus:
            self.best_locus_x.append(point.x)
            self.best_locus_y.append(point.y)
        # self.show_animation(best_locus)
        return best_uv, currentcost


if __name__=="__main__":
    init_state=state(0, 10, 0, 0.2, math.pi / 2, 0, 0, 0)
    goal=np.array([15,15])
    dwm=DWM(init_state,goal,7)
    ani = FuncAnimation(dwm.fig
                        , dwm.display_update
                        , init_func=dwm.display_init
                        , frames=int(10e+100)
                        , interval=10
                        , blit=True
                        )

    plt.show()

当然代码有一些不尽如人意的地方，下面是显示效果：

DWM演示