动态窗口方法(Dynamic window Approach,DWA)是一种常用的避障规划方法。这是一种选择速度的方法,该速度可以快速到达目标点,同时避免在速度搜索空间中可能与机器人发生碰撞的障碍物。该方法直接在速度空间中搜索机器人最优控制速度,将搜索空间减小到在动态约束下可达的速度,从而将机器人的动力学特性融入到该方法中。
1、 速度空间
我们假设每个时间间隔内速度保持不变,即其加速度为零。这种简化的原因是:(A)缩小的搜索空间是二维的,因而是可处理的;(B)每次时间间隔后都会重复搜索;(C)如果不给出新的命令,速度将自动保持不变。所生成的轨迹需与障碍物不相交。
为了生成从到给定目标点的轨迹,机器人必须确定在
1.1 允许速度范围
在机器人附近的环境区域内,障碍物的存在对机器人的旋转速度和平移速度都有一定的限制。机器人可允许的速度是机器人能够在到达障碍物的位置时及时停止的速度,这样就能够避免碰撞。设
此即为允许机器人在不与障碍物相撞的情况下停止的合理速度范围。
1.2 动态窗口
为了考虑到电机可发挥的有限的加速度,将整个速度搜索空间缩减为动态窗口,动态窗口只包含在下一个时间间隔内可以达到的速度。假设一个动态窗口的时间间隔为t,线加速度和角加速度为,那么动态窗口
动态窗口是以实际速度为中心的,它的扩展取决于可以施加的加速度。动态窗口外的所有轨迹都不能在下一个时间间隔内达到,因此可以不考虑避障。
1.3 结果搜索空间
以上在搜索空间施加的限制区域最终形成了一个结果空间
2、目标函数
在确定好结果搜索空间后,我们从中选择一个能最大化目标函数
Target heading
设
Clearance
函数dist(v,w)是与机器人运动轨迹圆弧相交的最近的障碍物的距离,如果没有障碍物相交,那么这个函数设置为一个很大的数。
Velocity
函数velocity(v,w)用于在相应的轨迹上评估机器人的进度,是平移速度的一个投影。
这三个指标是目标函数的重要组成部分,缺一不可。仅使clearance和velocity最大化,机器人始终在无障碍空间运动,但不会有向目标位置移动的趋势。单独最大化heading,机器人很快就会被阻碍其前进的第一个障碍所阻挡,无法在其周围移动。通过组合三个指标,机器人在上述限制条件下能够快速地绕过碰撞,同时朝着目标方向运动。
3、具体算法流程
下面我们就结合具体的代码详述一下DWA的算法流程。
可以直接运行的DWA算法的Python代码可以在下列链接中下载:
https://github.com/AtsushiSakai/PythonRobotics/blob/master/PathPlanning/DynamicWindowApproach/dynamic_window_approach.pygithub.com这个链接包含有很多经典的规划算法(A*,RRT)及仿真动图。
步骤一:设置初始参数,目标点
步骤二:开始计算速度指令。根据机器人目前姿态和机器人自身的结构参数计算一个矩形动态窗口dw(calc_dynamic_window函数)。
步骤三:动态窗口本身是连续的,因此这个区域内的目标函数难以用公式表达。因此为了获得定义域域为该动态窗口中的目标函数的最小值,(前面讲解原理的时候目标函数是最大化指标,在这个代码的具体实现中,我们采取的最小化指标,即对每个指标取倒数,但是本质是一样的),我们在动态窗口中均匀采点,然后计算每个点对应的机器人姿态,计算该姿态下机器人的三个指标和总目标函数,然后选取目标函数最小的点作为机器人的速度输入。
步骤四:根据步骤三获取的速度输入计算机器人的新姿态,并将新姿态更新为目前姿态。如果目前机器人位置没有到达目标点,那么返回步骤二继续计算。如果到达目标点,那么结束计算。机器人所经过的所有位置即为DWA算法计算出的轨迹。
参考文献:
1、YoonSeok Pyo、HanCheol Cho、RyuWoon Jung、TaeHoon Lim,ROS Robot Programming:From the basic concept to practical programming and robot application.
2、Fox D , Burgard W , Thrun S . The dynamic window approach to collision avoidance[J]. IEEE Robotics & Automation Magazine, 1997, 4(1):23-33.
3、https://github.com/AtsushiSakai/PythonRobotics#dynamic-window-approach
4、zhm-real/PathPlanning