• SLAM从入门到精通（dwa速度规划算法）

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

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

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

    1、了解机器人的参数

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

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

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

    1. x += v * cos(theta) * dt
    2. y += v * sin(theta) * dt
    3. 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即可。代码内容如下，

    1. import numpy as np
    2. import matplotlib.pyplot as plt
    3. import math
    4.  
    5. class Info():
    6.     def __init__(self):
    7.         self.v_min = -0.5
    8.         self.v_max = 3.0
    9.         self.w_max = 50.0 * math.pi / 180.0
    10.         self.w_min = -50.0 * math.pi / 180.0
    11.         self.vacc_max = 0.5
    12.         self.wacc_max = 30.0 * math.pi / 180.0
    13.         self.v_reso = 0.01
    14.         self.w_reso = 0.1 * math.pi / 180.0
    15.         self.radius = 1.0
    16.         self.dt = 0.1
    17.         self.predict_time = 4.0
    18.         self.goal_factor = 1.0
    19.         self.vel_factor = 1.0
    20.         self.traj_factor = 1.0
    21.     
    22. def motion_model(x,u,dt):
    23.     x[0] += u[0] * dt * math.cos(x[2])
    24.     x[1] += u[0] * dt * math.sin(x[2])
    25.     x[2] += u[1] * dt
    26.     x[3] = u[0]
    27.     x[4] = u[1]
    28.     return x
    29.  
    30. def vw_generate(x,info):
    31.     Vinfo = [info.v_min, info.v_max,
    32.             info.w_min, info.w_max]
    33.     
    34.     Vmove = [x[3] - info.vacc_max * info.dt,
    35.               x[3] + info.vacc_max * info.dt,
    36.               x[4] - info.wacc_max * info.dt,
    37.               x[4] + info.wacc_max * info.dt]
    38.  
    39.     vw = [max(Vinfo[0], Vmove[0]), min(Vinfo[1], Vmove[1]),
    40.           max(Vinfo[2], Vmove[2]), min(Vinfo[3], Vmove[3])]
    41.     return vw
    42.  
    43.  
    44. def traj_calculate(x,u,info):
    45.     ctraj = np.array(x)
    46.     xnew = np.array(x)
    47.     time = 0
    48.     
    49.     while time <= info.predict_time:
    50.         x_new = motion_model(xnew,u,info.dt)
    51.         ctraj = np.vstack((ctraj, xnew))
    52.         time += info.dt
    53.     return ctraj
    54.  
    55.  
    56. def dwa_core(x,u,goal,info, obstacles):
    57.     vw = vw_generate(x,info)
    58.     best_ctraj = np.array(x)
    59.     min_score = 10000.0
    60.     
    61.     for v in np.arange(vw[0], vw[1], info.v_reso):
    62.         for w in np.arange(vw[2], vw[3], info.w_reso):
    63.             ctraj = traj_calculate(x, [v,w], info)
    64.             
    65.             goal_score = info.goal_factor * goal_evaluate(ctraj, goal)
    66.             vel_score = info.vel_factor * velocity_evaluate(ctraj, info)
    67.             traj_score = info.traj_factor * traj_evaluate(ctraj, obstacles,info)
    68.             ctraj_score = goal_score + vel_score + traj_score
    69.             
    70.             if min_score >= ctraj_score:
    71.                 min_score = ctraj_score
    72.                 u = np.array([v,w])
    73.                 best_ctraj = ctraj
    74.  
    75.     return u,best_ctraj
    76.  
    77.  
    78. def goal_evaluate(traj, goal):
    79.     goal_score = math.sqrt((traj[-1,0]-goal[0])**2 + (traj[-1,1]-goal[1])**2)
    80.     return goal_score
    81.     
    82. def velocity_evaluate(traj, info):
    83.     vel_score = info.v_max - traj[-1,3]
    84.     return vel_score
    85.  
    86. def traj_evaluate(traj, obstacles, info):
    87.     min_dis = float("Inf")
    88.     for i in range(len(traj)):
    89.         for ii in range(len(obstacles)):
    90.             current_dist = math.sqrt((traj[i,0] - obstacles[ii,0])**2 + (traj[i,1] - obstacles[ii,1])**2)
    91.             if current_dist <= info.radius:
    92.                 return float("Inf")
    93.             
    94.             if min_dis >= current_dist:
    95.                 min_dis = current_dist
    96.  
    97.     return 1/min_dis
    98.  
    99.  
    100. def obstacles_generate():
    101.     obstacles = np.array([[0,10],
    102.                     [2,10],
    103.                     [4,10],
    104.                     [6,10],
    105.                     [3,5],
    106.                     [4,5],
    107.                     [5,5],
    108.                     [6,5],
    109.                     [7,5],
    110.                     [8,5],
    111.                     [10,7],
    112.                     [10,9],
    113.                     [10,11],
    114.                     [10,13]])
    115.     return obstacles
    116.  
    117. def local_traj_display(x,goal,current_traj, obstacles):
    118.     plt.cla()
    119.     plt.plot(goal[0], goal[1], 'or', markersize=10)
    120.     plt.plot([0,14],[0,0],'-k',linewidth=7)
    121.     plt.plot([0,14],[14,14],'-k',linewidth=7)
    122.     plt.plot([0,0],[0,14],'-k',linewidth=7)
    123.     plt.plot([14,14],[0,14],'-k',linewidth=7)
    124.     plt.plot([0,6],[10,10],'-y',linewidth=10)
    125.     plt.plot([3,8],[5,5],'-y',linewidth=10)
    126.     plt.plot([10,10],[7,13],'-y',linewidth=10)
    127.     plt.plot(obstacles[:,0], obstacles[:,1],'*b',linewidth=8)
    128.     plt.plot(x[0], x[1], 'ob', markersize=10)
    129.     plt.arrow(x[0], x[1], math.cos(x[2]), math.sin(x[2]), width=0.02, fc='red')
    130.     plt.plot(current_traj[:,0], current_traj[:,1], '-g', linewidth=2)
    131.     plt.grid(True)
    132.     plt.pause(0.001)
    133.     
    134. def main():
    135.     x = np.array([2,2,45*math.pi/180,0,0])
    136.     u = np.array([0,0])
    137.     goal = np.array([8,8])
    138.     info = Info()
    139.     obstacles = obstacles_generate()
    140.     global_traj = np.array(x)
    141.     plt.figure('DWA Algorithm')
    142.     
    143.     for i in range(2000):
    144.         u,current_traj = dwa_core(x,u,goal,info,obstacles)
    145.         x = motion_model(x,u,info.dt)
    146.         global_traj = np.vstack((global_traj, x))
    147.         local_traj_display(x, goal, current_traj,obstacles)
    148.         if math.sqrt((x[0]-goal[0])**2 + (x[1]-goal[1])**2 <= info.radius):
    149.             print("Goal Arrived")
    150.             break
    151.     plt.plot(global_traj[:,0], global_traj[:,1], '-r')
    152.     plt.show()    
    153.         
    154. if __name__ == "__main__":
    155.     main()

            motion_model就是小车的运动学模型，dwa_core是算法的核心，而goal_evaluate、velocity_evaluate、traj_evaluate就是三个权重计算函数。在main函数中，dwa是每个时刻都要计算的，所以速度、角速度随时可能会变的。

    6、执行效果

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

  • 相关阅读:
    Selenium启动常用浏览器
    webpack优化篇（四十八）：使用 Tree Shaking 擦除无用的 JavaScript 和 CSS
    Linux三级等保基本设置
    谁不想要一个自己的博客网站呢 - 搭建博客网站wordpress
    Python操作自动化
    dlib 的安装和检测人脸（包括关键68个点位检测）（1）
    微服务架构中间件安装部署
    InternImage的DCNv3编译
    Day01 SpringBoot第一次笔记---运维实用篇
    【JAVA学习笔记】43 - 枚举类
  • 原文地址：https://blog.csdn.net/feixiaoxing/article/details/133896541