路径规划与避障算法（⼋）---DWA算法流程之四---路径选择评价函数
版权声明：本⽂为博主原创⽂章，原创不易, 转载请联系博主。
本篇博客将重点介绍DWA算法所采⽤的评价函数中与参考路径相关的评价函数
评价函数:
轨迹主要依据以下三条准则进⾏评分，综合评分后选取分数最⼩的路径作为下⼀时刻选择路径:
Obstacle_costs 轨迹上是否存在障碍物以及距离障碍物的距离
Path_costs 轨迹上点距离局部参考路径最近距离
元宝树Goal_costs 轨迹上点距离局部参考路径终点最近距离
上篇博客路径规划与避障算法(七)—DWA算法流程之三—碰撞检测评价函数中已经介绍了与障碍物碰撞检测相关(Collision Detection)的评价函数，本篇博客将重点介绍与参考路径相关的评价函数。
由于⽆⼈车感知层输出的costmap是基于激光雷达等传感器的实时数据计算得出，因此costmap的尺⼨⼤⼩是有⼀定限制的，此时的路径规划需要分成两种情况来讨论：
参考路径全程需要规划
仅避障时需要规划新路径
当我们需要利⽤DWA算法依据全程参考轨迹进⾏实时平滑拟合时，需要添加⼀个截图模块，这个模块的功能主要是根据车辆当前位置以及costmap的⼤⼩实时的将全局参考轨迹裁剪适配到costmap中。
Path_Costs
Path_Costs主要评价采样轨迹上的点距离costmap上参考路径的最近距离，此标准的建⽴主要是希望选择运动趋势尽可能贴近参考轨迹的采样轨迹作为⽆⼈车下⼀时刻的运动轨迹。
Goal_Costs
Goal_Costs主要评价采样轨迹距离costmap上参考路径终点的最近距离，此标准的建⽴主要是希望选择运动朝向尽可能贴近参考路径的采样轨迹作为⽆⼈车下⼀时刻的运动轨迹。
模块代码
以下代码为基于Costmap的碰撞检测部分代码，仅供交流，由于属于项⽬开发，不能提供全套代码，望见谅
利⽤costmap进⾏轨迹评价时，需要将轨迹点先转换⾄costmap的栅格中，具体转换⽅法如下所⽰：
void AstarSearch::poToIndex(const geometry_msgs::Po &po,int*index_x,int*index_y,int*index_theta) {
*index_x =(po.position.x - costmap_.igin.position.x)/costmap_.solution;
*index_y =(po.position.y - costmap_.igin.position.y)/costmap_.solution;
tf::Quaternion tf_q;
tf::ientation,tf_q);
鹫峰>破房子
double yaw = tf::getYaw(tf_q);
if(yaw <0)
{
yaw = yaw +2*M_PI;
}
el if(yaw >=2*M_PI )
{
yaw = yaw -2*M_PI;
}
static double one_angle_range =2*M_PI / angle_size_;
*index_theta = yaw / one_angle_range;
*index_theta %= angle_size_;
}
对采样轨迹进⾏评价时，需要对当前costmap进⾏预计算：
/****
@预计算
@goal_costs: 计算轨迹路径计算地图中所有点到局部地图末端点的最短距离
@path_costs: 计算轨迹路径计算地图中所有点到整个局部地图路径的最短路径
****/
石榴哥哥/****
预计算,is_local_goal_function=true ==> goal_costs
is_local_goal_function = fal ==> path_costs
****/
bool MapGridCostFunction::prepare(){
map_.retPathDist();
if(is_local_goal_function_){
map_.tLocalGoal(*costmap_, target_pos_);
}el{
map_.tTargetCells(*costmap_, target_pos_);
}
return true;
}
Path_Costs
/****
@计算地图中所有点到整个局部地图路径的最短距离
****/
void MapGrid::tTargetCells(const costmap_2d::Costmap2D& costmap,
const std::vector<geometry_msgs::PoStamped>& global_plan){
SizeInCellsX(), SizeInCellsY());
bool started_path =fal;
queue<MapCell*> path_dist_queue;
std::vector<geometry_msgs::PoStamped> adjusted_global_plan;
adjustPlanResolution(global_plan, adjusted_global_plan, Resolution());
if(adjusted_global_plan.size()!= global_plan.size()){
ROS_DEBUG("Adjusted global plan resolution, added %zu points", adjusted_global_plan.size()- global_plan.size());
}
小米手机如何unsigned int i;
for(i =0; i < adjusted_global_plan.size();++i){
double g_x = adjusted_global_plan[i].po.position.x;
double g_y = adjusted_global_plan[i].po.position.y;
unsigned int map_x, map_y;
if(costmap.worldToMap(g_x, g_y, map_x, map_y)&& Cost(map_x, map_y)!= costmap_2d::NO_INFORMATION){ MapCell& current =getCell(map_x, map_y);
current.target_dist =0.0;
current.target_mark =true;
path_dist_queue.push(¤t);
started_path =true;
}el if(started_path){
break;
}
}
computeTargetDistance(path_dist_queue, costmap);
}
Goal_Costs
/****
@计算地图中所有点到末端终点的最短距离
@brief Update what cell is considered the next local goal
@Param costmap Information of the costmap
@Param global_plan Input of the reference trajecotry
****/
void MapGrid ::tLocalGoal (const costmap_2d ::Costmap2D & costmap ,
const std ::vector <geometry_msgs ::PoStamped >& global_plan ) {
sizeCheck (costmap .getSizeInCellsX (), costmap .getSizeInCellsY ());
int local_goal_x = -1;
睫毛卷翘int local_goal_y = -1;
bool started_path = fal ;
std ::vector <geometry_msgs ::PoStamped > adjusted_global_plan ;
adjustPlanResolution (global_plan , adjusted_global_plan , costmap .getResolution ());
// skip global path points until we reach the border of the local map
for (unsigned int i = 0; i < adjusted_global_plan .size (); ++i ) {
double g_x = adjusted_global_plan [i ].po .position .x ;
double g_y = adjusted_global_plan [i ].po .position .y ;
unsigned int map_x , map_y ;
if (costmap .worldToMap (g_x , g_y , map_x , map_y ) && costmap .getCost (map_x , map_y ) != costmap_2d ::NO_INFORMATION ) {
local_goal_x = map_x ;
local_goal_y = map_y ;
started_path = true ;
} el {
if (started_path ) {
break ;
}// el we might have a non pruned path, so we just continue
}
}
queue <MapCell *> path_dist_queue ;
if (local_goal_x >= 0 && local_goal_y >= 0) {
MapCell & current = getCell (local_goal_x , local_goal_y );
costmap .mapToWorld (local_goal_x , local_goal_y , goal_x_, goal_y_);
current .target_dist = 0.0;
current .target_mark = true ;
path_dist_queue .push (¤t );
}
//计算地图中所有点到局部路径终点的值
computeTargetDistance (path_dist_queue , costmap );
}
轨迹评分
当依据提出的三条标准：Obstacle_costs 轨迹上是否存在障碍物以及距离障碍物的距离
Path_costs 轨迹上点距离局部参考路径最近距离
系统的特征包括Goal_costs 轨迹上点距离局部参考路径终点最近距离
对每条采样轨迹进⾏打分后，在对每项标准提供⼀个权重值，⼯程师可以根据⾃⾝项⽬以及场景需求对每项权重值进⾏设置，最终得到的加权乘积值之和即为该条采样轨迹的最终评分，对每条轨迹的分数进⾏⽐较筛选，其中最⼩分数的轨迹即为下⼀时刻路径⾏驶的实时轨迹。公式如下：
其中 分别表⽰各个评分标准的权重值
Score =α∗Costs +Obstacle β∗Costs +Path Γ∗Costs Goal
五环破冰α,β,Γ
以上已经完成DWA算法在⽆⼈车避障功能中的全部讲解，整个系列都是笔者依据⾃⾝项⽬场景以及功能需求得出的⼀些经验，若有不完善以及不正确的地⽅还希望⼤家指正包涵。也很欢迎⼤家随时沟通⽆⼈车规划⽅向的问题。
