SLAM从入门到精通（dwa算法）

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

对于客户来说，速度、角速度肯定是越快越好。但是机械的特性决定了很多时候它快不了，比如转弯的时候，甚至是连续转弯的时候，速度快了反而不安全。正因为有了这些需求，所以才会有了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 mathclass Info():def __init__(self):self.v_min = -0.5self.v_max = 3.0self.w_max = 50.0 * math.pi / 180.0self.w_min = -50.0 * math.pi / 180.0self.vacc_max = 0.5self.wacc_max = 30.0 * math.pi / 180.0self.v_reso = 0.01self.w_reso = 0.1 * math.pi / 180.0self.radius = 1.0self.dt = 0.1self.predict_time = 4.0self.goal_factor = 1.0self.vel_factor = 1.0self.traj_factor = 1.0def 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] * dtx[3] = u[0]x[4] = u[1]return xdef 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 vwdef traj_calculate(x,u,info):ctraj = np.array(x)xnew = np.array(x)time = 0while time <= info.predict_time:x_new = motion_model(xnew,u,info.dt)ctraj = np.vstack((ctraj, xnew))time += info.dtreturn ctrajdef dwa_core(x,u,goal,info, obstacles):vw = vw_generate(x,info)best_ctraj = np.array(x)min_score = 10000.0for 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_scoreif min_score >= ctraj_score:min_score = ctraj_scoreu = np.array([v,w])best_ctraj = ctrajreturn u,best_ctrajdef goal_evaluate(traj, goal):goal_score = math.sqrt((traj[-1,0]-goal[0])**2 + (traj[-1,1]-goal[1])**2)return goal_scoredef velocity_evaluate(traj, info):vel_score = info.v_max - traj[-1,3]return vel_scoredef 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_distreturn 1/min_disdef 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 obstaclesdef 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")breakplt.plot(global_traj[:,0], global_traj[:,1], '-r')plt.show()    if __name__ == "__main__":main()