【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】

        要说搜路算法，这个大家都比较好理解。毕竟从一个地点走到另外一个地点，这个都是直觉上可以感受到的事情。但是这条道路上机器人应该怎么走，以什么样的速度、什么样的角速度走，这里面有很大的学问。一方面，机器人本身的机械特性决定了它的速度、角速度这些参数都有一定范围约束的；另外一方面，不同的速度、角速度走出来的轨迹可能是不一样的，特别是拐弯的时候。这个时候，什么样的轨迹最适合我们机器人，就需要设计出一套标准来甄别了。比如，是越快越好，还是越安全越好，还是说离目标越近越好。

        对于客户来说，速度、角速度肯定是越快越好。但是机械的特性决定了很多时候它快不了，比如转弯的时候，甚至是连续转弯的时候，速度快了反而不安全。正因为有了这些需求，所以才会有了dwa算法设计出来帮助我们来解决这些问题。

1、了解机器人的参数

        每一款机器人都有自己独特的参数，比如最小速度、最大速度；最小角速度、最大角速度；最小线加速度、最大线加速度等等。这些数据都需要做很好的了解。不仅如此，我们还需要知道机器人的最小转弯半径。如果可以原地旋转，这固然很好。但是大多数机器人不一定可以做到这一点。

2、了解机器人的运动学模型

        之前我们说过差速轮的运动学模型，假设速度分别为v和w，那么后面小车的轨迹应该是这样的，

x += v * cos(theta) * dt
y += v * cos(theta) * dt
theta += w * dt

        当然这里描述的只是差速轮的运动学模型，其他机器人的运动学模型也可以通过类似的方法进行计算。

3、速度采样、加速度采样

        以速度为例，机器人本身有一个最小速度，还有一个最大速度。此外，它还有一个最小加速度、最大加速度。所以对于任意时刻的速度v，依据加速度的范围可以得到一个数值[v_min, v_max]，但是这个范围不能超过[vmin，vmax]机器人本身要求的范围。所以最终机器人的速度区间应该是在[max(v_min, vmin), min(v_max, vmax)]这个范围之内。加速度也是一样的道理。

4、轨迹评价标准

        本身dwa提供了三个评价标准，分别是目标、速度以及和障碍物的最小距离。当然，这三个标准是不一定适用于所有项目，我们完全可以自己来设计评价标准。

5、测试代码

        dwa算测的测试代码是用python实现的，参考一本ros书上的内容，在此表示感谢。此python代码用python3执行，依赖于库matplotlib，直接输入python3 dwa.py即可。代码内容如下，

import numpy as np
import matplotlib.pyplot as plt
import math

class Info():
    def __init__(self):
        self.v_min = -0.5
        self.v_max = 3.0
        self.w_max = 50.0 * math.pi / 180.0
        self.w_min = -50.0 * math.pi / 180.0
        self.vacc_max = 0.5
        self.wacc_max = 30.0 * math.pi / 180.0
        self.v_reso = 0.01
        self.w_reso = 0.1 * math.pi / 180.0
        self.radius = 1.0
        self.dt = 0.1
        self.predict_time = 4.0
        self.goal_factor = 1.0
        self.vel_factor = 1.0
        self.traj_factor = 1.0
    
def motion_model(x,u,dt):
    x[0] += u[0] * dt * math.cos(x[2])
    x[1] += u[0] * dt * math.sin(x[2])
    x[2] += u[1] * dt
    x[3] = u[0]
    x[4] = u[1]
    return x

def vw_generate(x,info):
    Vinfo = [info.v_min, info.v_max,
            info.w_min, info.w_max]
    
    Vmove = [x[3] - info.vacc_max * info.dt,
              x[3] + info.vacc_max * info.dt,
              x[4] - info.wacc_max * info.dt,
              x[4] + info.wacc_max * info.dt]

    vw = [max(Vinfo[0], Vmove[0]), min(Vinfo[1], Vmove[1]),
          max(Vinfo[2], Vmove[2]), min(Vinfo[3], Vmove[3])]
    return vw


def traj_calculate(x,u,info):
    ctraj = np.array(x)
    xnew = np.array(x)
    time = 0
    
    while time <= info.predict_time:
        x_new = motion_model(xnew,u,info.dt)
        ctraj = np.vstack((ctraj, xnew))
        time += info.dt
    return ctraj


def dwa_core(x,u,goal,info, obstacles):
    vw = vw_generate(x,info)
    best_ctraj = np.array(x)
    min_score = 10000.0
    
    for v in np.arange(vw[0], vw[1], info.v_reso):
        for w in np.arange(vw[2], vw[3], info.w_reso):
            ctraj = traj_calculate(x, [v,w], info)
            
            goal_score = info.goal_factor * goal_evaluate(ctraj, goal)
            vel_score = info.vel_factor * velocity_evaluate(ctraj, info)
            traj_score = info.traj_factor * traj_evaluate(ctraj, obstacles,info)
            ctraj_score = goal_score + vel_score + traj_score
            
            if min_score >= ctraj_score:
                min_score = ctraj_score
                u = np.array([v,w])
                best_ctraj = ctraj

    return u,best_ctraj


def goal_evaluate(traj, goal):
    goal_score = math.sqrt((traj[-1,0]-goal[0])**2 + (traj[-1,1]-goal[1])**2)
    return goal_score
    
def velocity_evaluate(traj, info):
    vel_score = info.v_max - traj[-1,3]
    return vel_score

def traj_evaluate(traj, obstacles, info):
    min_dis = float("Inf")
    for i in range(len(traj)):
        for ii in range(len(obstacles)):
            current_dist = math.sqrt((traj[i,0] - obstacles[ii,0])**2 + (traj[i,1] - obstacles[ii,1])**2)
            if current_dist <= info.radius:
                return float("Inf")
            
            if min_dis >= current_dist:
                min_dis = current_dist

    return 1/min_dis


def obstacles_generate():
    obstacles = np.array([[0,10],
                    [2,10],
                    [4,10],
                    [6,10],
                    [3,5],
                    [4,5],
                    [5,5],
                    [6,5],
                    [7,5],
                    [8,5],
                    [10,7],
                    [10,9],
                    [10,11],
                    [10,13]])
    return obstacles

def local_traj_display(x,goal,current_traj, obstacles):
    plt.cla()
    plt.plot(goal[0], goal[1], 'or', markersize=10)
    plt.plot([0,14],[0,0],'-k',linewidth=7)
    plt.plot([0,14],[14,14],'-k',linewidth=7)
    plt.plot([0,0],[0,14],'-k',linewidth=7)
    plt.plot([14,14],[0,14],'-k',linewidth=7)
    plt.plot([0,6],[10,10],'-y',linewidth=10)
    plt.plot([3,8],[5,5],'-y',linewidth=10)
    plt.plot([10,10],[7,13],'-y',linewidth=10)
    plt.plot(obstacles[:,0], obstacles[:,1],'*b',linewidth=8)
    plt.plot(x[0], x[1], 'ob', markersize=10)
    plt.arrow(x[0], x[1], math.cos(x[2]), math.sin(x[2]), width=0.02, fc='red')
    plt.plot(current_traj[:,0], current_traj[:,1], '-g', linewidth=2)
    plt.grid(True)
    plt.pause(0.001)
    
def main():
    x = np.array([2,2,45*math.pi/180,0,0])
    u = np.array([0,0])
    goal = np.array([8,8])
    info = Info()
    obstacles = obstacles_generate()
    global_traj = np.array(x)
    plt.figure('DWA Algorithm')
    
    for i in range(2000):
        u,current_traj = dwa_core(x,u,goal,info,obstacles)
        x = motion_model(x,u,info.dt)
        global_traj = np.vstack((global_traj, x))
        local_traj_display(x, goal, current_traj,obstacles)
        if math.sqrt((x[0]-goal[0])**2 + (x[1]-goal[1])**2 <= info.radius):
            print("Goal Arrived")
            break
    plt.plot(global_traj[:,0], global_traj[:,1], '-r')
    plt.show()    
        
if __name__ == "__main__":
    main()

6、执行效果

        代码本身是一个仿真过程，大家可以下载下来在ubuntu环境下测试验证一下。最终实现的效果如下所示，