【 声明：版权所有，欢迎转载，请勿用于商业用途。 联系信箱：feixiaoxing @163.com】
要说搜路算法，这个大家都比较好理解。毕竟从一个地点走到另外一个地点，这个都是直觉上可以感受到的事情。但是这条道路上机器人应该怎么走，以什么样的速度、什么样的角速度走，这里面有很大的学问。一方面，机器人本身的机械特性决定了它的速度、角速度这些参数都有一定范围约束的；另外一方面，不同的速度、角速度走出来的轨迹可能是不一样的，特别是拐弯的时候。这个时候，什么样的轨迹最适合我们机器人，就需要设计出一套标准来甄别了。比如，是越快越好，还是越安全越好，还是说离目标越近越好。
对于客户来说，速度、角速度肯定是越快越好。但是机械的特性决定了很多时候它快不了，比如转弯的时候，甚至是连续转弯的时候，速度快了反而不安全。正因为有了这些需求，所以才会有了dwa算法设计出来帮助我们来解决这些问题。
1、了解机器人的参数
每一款机器人都有自己独特的参数，比如最小速度、最大速度；最小角速度、最大加速度；最小线加速度、最大线加速度等等。这些数据都需要做很好的了解。不仅如此，我们还需要知道机器人的最小转弯半径。如果可以原地旋转，这固然很好。但是大多数机器人不一定可以做到这一点。而且根据v=w*r，一旦转弯半径定了，v和w只要一个确定了，另外一个也就确定了。
2、知晓机器人的运动学模型
之前我们说过差速轮的运动学模型，假设速度分别为v和w，那么后面小车的轨迹应该是这样的，
x += v * cos(theta) * dt
y += v * cos(theta) * dt
theta += w * dt当然这里描述的只是差速轮的运动学模型，其他机器人的运动学模型也可以通过类似的方法进行计算。
3、速度采样、加速度采样
以速度为例，机器人本身有一个最小速度，还有一个最大速度。此外，它还有一个最小加速度、最大加速度。所以某个时刻速度v，依据加速度的范围可以得到一个数值[v_min, v_max]，但是这个范围不能超过[vmin，v_max]机器人本身要求的范围。所以最终机器人的速度应该是在[max(v_min, vmin), min(v_max, vmax)]这个范围之内。加速度也是一个道理。
4、轨迹评价标准
本身dwa提供了三个评价标准，分别是目标、速度以及和障碍物的最小距离。当然，这三个标准是不一定的，我们完全可以自己来设计。
5、测试代码
dwa算测的测试代码是用python实现的，参考一本ros书上的内容，在此表示感谢。此python代码用python3执行，依赖于库matplotlib。代码内容如下，
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环境下测试验证一下。最终实现的效果如下所示，
