ROS的路径规划器分为全局路径和局部路径规划,其中局部路径规划器使用的最广的为dwa,个人理解为:
首先全局路径规划会生成一条大致的全局路径,局部路径规划器会把全局路径给分段,然后根据分段的全局路径的坐标,进行局部重新规划,例如:
全局规划后有一组目标点数组【1,2,3,4,5,......】,局部是根据当前速度和时间、约束条件,随机模拟出50条轨迹,选出最佳一条轨迹到达1号目标点,随后重复2,3...,
一直到达到目的地为止。具体的步骤如下:
一、路径存储
1、全局路径存储
在ros move base控制循环中会在规划出新的路径时,将新的全局路径利用setPlan传给DWAPlannerROS,直接保存为global_plan_。
此时的路径时相对于全局地图的全局坐标系的(通常为"map")。
2、局部路径存储
在计算速度前,会先将全局路径映射到局部地图坐标系下(通常为“odom”),在getLocalPlan中,先将较长的全局路径映射并截断到局部地图内(即坐标系转换为局部地图,
且范围完全在局部地图内,超出地图的则抛弃,局部地图为costmap),局部地图使用的是rolling_window。然后,裁减全局路径和局部路径(与机器人当前位置距离超过1m的旧的路径会被裁减掉)。
有了局部的目标点,就可以做规划了。
二、机器人运动模型
运动模型推到是为了进行轨迹推演,具体可参考:http://blog.csdn.net/heyijia0327/article/details/44983551
三、速度采样
机器人的轨迹运动模型有了,根据速度就可以推算出轨迹。因此只需采样很多速度,推算轨迹,然后评价这些轨迹好不好就行了。但是,速度(Vx,Vw)理论上有无穷多组,假定是在两轮差速轮上,
因此根据机器人自身的条件和环境的限制,速度采样范围需要做一定的限制:
1、移动机器人受自身最大速度最小速度的限制
2、 移动机器人受电机性能的影响:由于电机力矩有限,存在最大的加減速限制,因此移动机器人軌迹前向模拟的周期sim_period内,存在一个动态窗口,在该窗口内的速度是机器人能够实际达到的速度:
3、 基于移动机器人安全的考虑:为了能够在碰到障碍物前停下来, 因此在最大减速度条件下, 速度有一个范围:
在获取空间边界时,根据use_dwa参数,采用两套策略。
如果use_dwa==false,首先用当前位置与目标位置的距离处理仿真时间sim_time(模拟仿真时间内匀减速到0,刚好到达目标点的情景)得到max_vel_x和max_vel_y边界,然后基于acc_lim * sim_time得到一种边界(上边界),还有设置的速度极限参数(max_vel_xxx,min_vel_xxx)作为一种边界,然后选取三种边界中空间较小的边界。这种策略,能够获得较大的采样空间(因为用了sim_time)。
如果use_dwa==true,则直接用acc_lim * sim_period得到边界,然后还有设置的速度极限参数作为边界,然后选取两种边界中空间较小的边界。
得到速度空间边界后,根据x,y,theta三个采样个数进行插补,进而组合出整个速度采样空间。
每个维度速度需要采样的养本数存放在vsamples_这个结构体内,vsamples_[0]是x方向样本数,vsamples_[1]是y方向样本数,vsamples_[2]是z方向样本数。首先计算各个方向的最大速度和最小速度,DWA算法只在第一步进行采样,所以最大速度为:
Max_vel=max(max_vel,vel+acc_lim*sim_period)
最小速度为:
Min_vel=min(min_vel,vel-acc_lim*sim_period)
其中max_vel,min_vel为人为设定的最大和最小速度,vel是当前速度,acc_lim是人为设定的最大加速度,sim_period是第一步的模拟时间,由人为设定的局部路径规划频率决定,默认为0.05。
当计算出各个维度的最大最小速度后,就创建三个VelocityIterator类的对象,并传入最大最小速度和样本数目,此对象的构造函数会生成同样数目的速度样本并放入samples_这个容器内。具体做法是先计算步长step_size:
step_size=(max-min)/(nums_samples-1)
max为最大速度,min为最小速度,nums_samples为样本数目。从最小速度每次多累加一次step_size即为一个速度样本,直到达到最大速度。将每个维度的速度样本取得后,再全部循环每个样本组里选择一个组合放入结构体vel_sample,最后将这些vel_sample放入sample_params_的容器里。至此,速度采样就完成了。
四、评价函数
根据采样的速度模拟推演出相对应的轨迹,然后对每一条轨迹进行评价,找出最优的一条轨迹进行局部导航。如何找出最优的轨迹?这里DWA使用的是一个打分系统,
原始的论文只有3个评分项:
最后对每一个项做归一化处理,目的是进行平滑。
评价函数的意义就是:找出一条最优路径,使得机器人在局部导航过程中,避开障碍物,以最快的速度到达目标点。
通过上述几种评分机制,选取最优的一组速度样本,传递给move_base,并发布相应的local plan。move_base如果收到了可用的速度则发布给底盘,否则发布0速度,且如果寻找最优速度的时间超过了限制就会执行障碍物清理模式,state_会变为CLEARING。
dwa算法demo
来源:http://adrianboeing.blogspot.com/2012/05/dynamic-window-algorithm-motion.html BEGIN DWA(robotPose,robotGoal,robotModel) laserscan = readScanner() allowable_v = generateWindow(robotV, robotModel) allowable_w = generateWindow(robotW, robotModel) for each v in allowable_v for each w in allowable_w dist = find_dist(v,w,laserscan,robotModel) breakDist = calculateBreakingDistance(v) if (dist > breakDist) //can stop in time heading = hDiff(robotPose,goalPose, v,w) //clearance与原论文稍不一样 clearance = (dist-breakDist)/(dmax - breakDist) cost = costFunction(heading,clearance, abs(desired_v - v)) if (cost > optimal) best_v = v best_w = w optimal = cost set robot trajectory to best_v, best_w END
五、停止判断
对于DWAPlannerROS的停止处理逻辑,由LatchedStopRotateController类完成,主要包括了停止判断、加速停止、旋转至目标朝向三个大的部分。
在到达目标位置的前提下,还要判断机器人朝向是否满足目标朝向yaw_goal_tolerance的需求,如果也满足,则判断当前速度是否满足停止条件,即x和y的速度小于trans_stopped_velocity,theta速度小于rot_stopped_velocity。
只有到达位置、达到朝向、速度满足停止三个条件都满足的情况下,才算机器人到达了目标位姿,isGoalReached函数才会返回tru