Introduction
This is a 2D navigation sample code with Dynamic Window Approach.
[图片上传失败...(image-9983e3-1561134998649)]
动态窗口方法(Dynamic window Approach,DWA)是一种常用的避障规划方法。
DWA是一种选择速度的方法。DWA结合了机器人的动力学特性,通过在速度空间 中采样多组速度,并对该速度空间进行缩减,模拟机器人在这些速度下一小段时间间隔内的运动轨迹,要求:
机器人避开可能发生碰撞的每一个障碍物
机器人在该时间间隔内可以达到这一速度(受限于机器人的动态约束,i.e. 加速度)
机器人可以快速到达目标点
之后通过一个评价函数:
- 生成轨迹与参考路径的距离(贴合程度)
- 生成轨迹与参考路径终点的距离
- 生成轨迹上是否存在障碍物(若有则抛弃这条轨迹)
对这些轨迹评价,在速度空间中搜索最优轨迹所对应的机器人最优控制速度,并通过这一速度来驱动机器人运动。
优点
- 反应速度较快,计算不复杂,通过速度组合(线速度与角速度)可以快速得出下一时刻规划轨迹的最优解.
- 可以将优化由横向与纵向两个维度向一个维度优化
缺点
- 此算法是由机器人避障衍生而来,机器人的路径规划算法主要体现在较高的灵活性和其静态环境,譬如某些特种车辆的工作环境,是一个较大的开阔广场,因此是比较适用此算法的
- 无人驾驶则要求较高的稳定性和动态环境,主要是结合车道线信息来实现规划算法
- 较高的灵活性会极大的降低行驶的平稳性
- 可能是避障算法载体由机器人向无人车平台移植过程中会产生的主要问题
Motion Model
模型1:直线轨迹模型
简单,程序中最常见。
机器人不是全向移动的
即不能纵向移动,只能前进和旋转。
计算机器人轨迹时,先考虑两个相邻时刻,如下图所示。
为简单起见,由于机器人相邻时刻内,运动距离短,因此可以忽略加速度当作匀速直线运动处理,将两相邻点之间的运动轨迹看成直线。
[站外图片上传中...(image-5f0529-1561134998649)]
则只需将该段距离分别投影在世界坐标系x轴和y轴上得到在世界坐标系中坐标移动的位移:
以此类推,推算一段时间内的轨迹,只需要将这段时间的位移增量累计求和:
机器人是全向运动的
即机器人有轴速度,则同理,只需将机器人在机器人坐标y轴移动的距离投影到世界坐标系即可得:
推算一段时间内的轨迹,则对之前的公式进行修改得:
在ROS的轨迹推演中就使用的该公式,base_local_planner的轨迹采样程序也是使用的该公式(line 256 in simple_trajectory_generator.cpp)。
模型2:圆弧轨迹模型(DWA论文中的运动模型)
上面的计算中,假设相邻时间段内机器人的轨迹是直线,这是不准确的,更准确的做法是用圆弧来代替。
假设机器人不是全向运动的,且机器人的平移和旋转速度可以独立控制(扭矩有限)。通过推导出近似,将速度建模为时间上的分段常数函数,则机器人轨迹由有限多个圆圈段的序列组成。 这种表示对于碰撞检查非常方便,因为障碍物与圆圈的交叉点易于检查。
General Motion Equations
作出如下定义:
- : 某一时刻
- : 从到的时间间隔
- : 时刻下,机器人的坐标
- : 时刻下,机器人的坐标
- : 时刻下,机器人的朝向(orientation / heading direction)
- : 该三元组描述机器人的运动学配置(kinematic configuration)
- : 时刻下,机器人的平移速度(translational velocity),其方向与机器人的朝向相同
- : 时刻下,机器人的旋转速度(rotational velocity)
- : 时刻下,机器人的平移加速度
- : 时刻下,机器人的旋转加速度
则有:
引入加速度和初始速度,则有(同理可得):
由于机器人只能由有限多个加速度命令控制,作出如下定义:
- : 这一时间间隔内的时间刻度(time ticks)的总数,即有
- : 这一时间间隔内的某一时间下,机器人的平移加速度,是常数
- : 这一时间间隔内的某一时间下,机器人的选装加速度,是常数
- : 即,其中
则有:
Approximate Motion Equations
通过将时间间隔内的机器人速度近似为常数值来进行简化上式。
在这个假设下看到的那样,机器人的轨迹可以通过分段圆弧(和直线弧)来近似。
由于机器人在时间间隔内的运动是平稳的,上式中项可以由进行近似。同理,则可以由进行近似,其中
则有:
其中:
同理可得:
其中:
当 ,机器人将沿直线运动。
当 ,机器人的运动轨迹可被描述为一个圆,满足:
即机器人的第个轨迹是一个以为圆心的圆,其半径为:
模型3:自行车模型
作出以下假设:
- 车辆在垂直方向的运动被忽略掉了,也就是说我们描述的车辆是一个二维平面上的运动物体(可以等价与我们是站在天空中的俯视视角)
- 我们假设车辆的结构就像自行车一样,也就是说车辆的前面两个轮胎拥有一直的角度和转速等,同样后面的两个轮胎也是如此,那么前后的轮胎就可以各用一个轮胎来描述
- 我们假设车辆运动也和自行车一样,这意味着是前面的轮胎控制这车辆的转角
[图片上传失败...(image-9a83e2-1561134998649)]
自行车运动学模型将前后轮胎分别用一个轮胎来描述并且将轮胎置于前后中心线上。假定车轮没有横向漂移且只有前向车轮是可以转向的。
由于限制该模型在平面上运动,前后轮的非完整约束方程为:
其中:
- : 后轮的全局坐标
- : 前轮的全局坐标
- : 车辆的纵向速度
- : 车辆在方向的偏转角度
- : 车辆的转向角度
由于车轮距离(前后轮胎之间的距离)为,所以可以表示为:
带入上式则有:
可以通过纵向速度来表示:
则有:
又由于车辆的瞬时曲率半径R是由以及来决定的:
结合上式,可得:
最终,以上运动学模型可以通过矩阵形式表达出来:
Velocity Space Search
根据现有的运动轨迹模型,可以推算出机器人的运动轨迹。通过对速度进行对组采样,可对推算生成的轨迹进行评价。
Circular trajectories
动态窗口方法仅考虑由平移和旋转速度的对唯一确定的圆形轨迹(curvatures),受自身最大速度最小速度的限制:
得到一个二维的搜索空间,且满足的轨迹要求满足轨迹不与障碍物相交。
例如,在论文中的场景:
[站外图片上传中...(image-942e8e-1561134998649)]
可得到速度空间如下:
[站外图片上传中...(image-52780c-1561134998649)]
可以发现:
- 缩小的搜索空间是二维的,因此易于处理
- 每次时间间隔后都会重复搜索
- 如果没有给出新的命令,机器人速度将自动保持不变
为了使优化成为可能,动态窗口方法仅考虑第一时间间隔,并假设剩余个时间间隔内的速度是恒定的,即相当于假设为零加速度。
Admissible Velocities
为了保证安全,机器人能够在碰到障碍物前停下来,受限于机器人的最大加(减)速度,速度有一个范围:
其中为速度对应轨迹上离障碍物最近的距离。
Note: 这个条件并不是在采样一开始就能得到的。需要我们模拟出来机器人轨迹以后,找到障碍物位置,计算出机器人到障碍物之间的距离,然后看当前采样的这对速度能否在碰到障碍物之前停下来,如果能够停下来,那这对速度就是可接受的admissible。如果不能停下来,这对速度就得抛弃掉。
Dynamic Window (动态窗口)
依据机器人的加减速性能限定速度采用空间在一个可行的动态范围内,如图所示。
[站外图片上传中...(image-2051e9-1561134998649)]
考虑到电机可发挥的有限的加速度,整个搜索空间减少到动态窗口,该窗口仅包含下一个时间间隔内可以达到的速度。
动态窗口是以实际速度为中心的,它的扩展取决于可以施加的加速度。动态窗口外的所有轨迹都不能在下一个时间间隔内达到,因此可以不考虑避障。
动态窗口采样的轨迹如下图所示:
[站外图片上传中...(image-6dbe54-1561134998649)]
Resulting Search Space
缩减后的速度空间为:
Optimization
根据得到的采样空间,采用评价函数的方式为每条轨迹进行评价:
动态窗口外的评价设为。
Target heading
方位角评价函数测量机器人与目标方向的对齐,即评价机器人在当前设定的采样速度下,达到模拟轨迹末端时的朝向和目标之间的角度差距,差距越小,评价得分越高:
[图片上传失败...(image-2e0a56-1561134998649)]
由于该方向随着不同的速度而变化,因此针对机器人的预测位置计算。为了确定预测位置,我们假设机器人在下一个时间间隔内以所选速度移动。为了对目标航向进行实际测量,我们必须考虑旋转的dynamics。 因此,在机器人在下一个间隔之后施加最大减速度时将到达的位置处计算。
当机器人绕过障碍物时,这使得机器人的行为平稳地转向目标。
[图片上传失败...(image-529ea6-1561134998649)]
其中,非允许(non admissible)速度的值设置为零
Clearance
表示与curvature相交的最近障碍物的距离。若没有障碍物,则将该值设置为大常数。
[图片上传失败...(image-41673c-1561134998649)]
Velocity
用于评估机器人在相应轨迹上的进度,它只是对平移速度的投影。
[站外图片上传中...(image-4f446e-1561134998649)]
Smoothing
平滑处理,即归一化,上面三个部分计算出来以后不是直接相加。而是每个部分在归一化以后,再相加。
通过最大化间隙和速度,机器人将始终进入自由空间,但没有动力朝着目标位置移动。 通过单独最大化目标航向,机器人很快就会被阻挡其路径的第一个障碍物阻挡,无法绕过它。
总结起来三者构成的评价函数的物理意义是:在局部导航过程中,使得机器人避开障碍,朝着目标以较快速度行驶,缺一不可。通过组合所有三个组件,机器人在上面列出的约束条件下尽可能快地绕过碰撞,同时仍然朝着达到目标的方向前进。
[站外图片上传中...(image-ff4be7-1561134998649)]
归一化过程如下:
其中,为采样的所有轨迹,为待评价的当前轨迹。
归一化的目的是smoothing:例如障碍物距离,机器人传感器检测到的最小障碍物距离在二维空间中是不连续的,这条轨迹能够遇到障碍,旁边那边不一定能遇到。并且这条轨迹最小的障碍物距离是1m,旁边那条就是10m。那么障碍物距离的这种评价标准导致评价函数不连续,也会导致某个项在评价函数中太占优势,如这里的离障碍物距离10m相对于1m就太占优势。这样,都变成同一百分比了,每个障碍物最小距离是这100份中的一份。
Role of the Current Velocity
例:
假设加速度为和,给定直线运动,平移速度分别为和,图12示出了动态窗口和:
[站外图片上传中...(image-9ac7ba-1561134998649)]
在图13和14中,示出了动态窗口的目标函数,动态窗口外的评估设为:
[站外图片上传中...(image-19d314-1561134998649)]
在第一种情况下,当前速度为时,机器人移动得太快,无法通过打开的门进行向右急转弯。 图13中,反映为导致向右转弯的速度不可接受。因此, 在动态窗口中的速度中,具有最大评估的速度产生直线运动,机器人沿着走廊继续前进,如图13中的垂直线和图12中的顶部的交叉标记所示。
[图片上传失败...(image-351b95-1561134998649)]
在第二种情况下,如果当前速度是,则动态窗口包括通过门的轨迹(见图12和14)。 由于对角度和距离的更好评估,所选择的速度是向右转动最极端的速度,通过门到达目标。
Dependency on the Accelerations
受限制于搜索空间,评估功能和动态窗口也取决于给定的加速度。
例:
考虑加速度为和的示例的速度空间,如图15所示。
[站外图片上传中...(image-c0599b-1561134998649)]
由于加速度小,允许速度的空间小于图4。
[站外图片上传中...(image-ec1578-1561134998649)]
因此,我们只考虑速度至和。 具有的平移速度的直线运动的动态窗口由白色区域表示。 该窗口的大小减小,因为它取决于加速度。
图16包含整个目标函数:
[站外图片上传中...(image-9373a7-1561134998649)]
限制在动态窗口中的速度的空间的评估如图17所示:
[站外图片上传中...(image-45a1fc-1561134998649)]
同理,此时机器人速度太快而不能急剧转入门,直线沿走廊运动的速度具有最大的评估。
Algorithm
[站外图片上传中...(image-4b6e5d-1561134998649)]
Implementation Details
- Rotate away mode:在极少数情况下,我们观察到机器人卡在局部最小值。 如果没有允许的轨迹允许机器人平移,则就是这种情况。 当这种情况发生时。 这很容易被发现。 机器人远离障碍物旋转,直到它能够再次平移。
- Speed dependent side clearance:为了使机器人的速度根据侧面间隙适应障碍物,我们在机器人周围引入了一个安全边缘,它随机器人的平移速度线性增长。 因此,机器人将通过走廊高速行进,并在通过狭窄的门行驶时减速。 同时,可以考虑前面在关于近似误差上限的部分中描述的近似误差的可能偏差。
References
- Fox, D., Burgard, W., & Thrun, S. (1997). The Dynamic Window Approach to Collision Avoidance. IEEE Robotics & Automation Magazine, 4(1), 23–33.
- 这道题我不会做啊啊啊. 路径规划与避障算法——DWA算法流程: 1, 2, 3
- 张天佳. ros导航-动态窗口方法(Dynamic Window Approach)
- 白巧克力亦唯心. 机器人局部避障的动态窗口法(dynamic window approach)
- peakzuo. DWA算法分析
- 苏碧落. Dynamic Window Approach_机器人局部避障的动态窗口法
- http://wiki.ros.org/dwa_local_planner
- https://github.com/AtsushiSakai/PythonRobotics