本文链接地址: 「链接」机器人导航路径的规划可以分为两种：全局规划和局部规划。
对事先已经知道的静态信息进行的规划为全局规划，如根据高清地图进行的规划，全局规划器就如同平时生活中的地图导航一样，这是一个大尺度的规划。
基于传感器信息进行的机器人控制为局部路径规划，这是一个小尺度的规划。
大范围内的静态信息不易改变，只需要定期更新高清地图就可以保证规划有效性。
局部范围的动态信息则需要摄像头，激光雷达等设备及时回馈，变化频繁。
Content:调用路径planner初始化规划路径核心算法：Dijkstra规划算法获取路径本文介绍ROS的全局规划器：NavfnROS，其默认调用Dijkstra算法进行路径生成。
调用路径在Move Base中有全局规划器的如下调用，完成了全局规划器的实例化：

try { planner_ = bgp_loader_.createInstance(global_planner); planner_->initialize(bgp_loader_.getName(global_planner), planner_costmap_ros_); } catch (const pluginlib::PluginlibException& ex) { ROS_FATAL("Failed to create the %s planner, are you sure it is properly registered and that the containing library is built? Exception: %s", global_planner.c_str(), ex.what()); exit(1); }planner初始化planner_由类NavFn实现，plan_pub_用于发布规划路径，make_plan_srv_提供路径规划服务。
void NavfnROS::initialize(std::string name, costmap_2d::Costmap2D costmap, std::string global_frame){ if(!initialized_){ costmap_ = costmap; global_frame_ = global_frame; planner_ = boost::shared_ptr<NavFn>(new NavFn(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY())); ros::NodeHandle private_nh("~/" + name); plan_pub_ = private_nh.advertise<nav_msgs::Path>("plan", 1); private_nh.param("visualize_potential", visualize_potential_, false); //if we're going to visualize the potential array we need to advertise if(visualize_potential_) potarr_pub_ = private_nh.advertise<sensor_msgs::PointCloud2>("potential", 1); private_nh.param("allow_unknown", allow_unknown_, true); private_nh.param("planner_window_x", planner_window_x_, 0.0); private_nh.param("planner_window_y", planner_window_y_, 0.0); private_nh.param("default_tolerance", default_tolerance_, 0.0); make_plan_srv_ = private_nh.advertiseService("make_plan", &NavfnROS::makePlanService, this); initialized_ = true; } else ROS_WARN("This planner has already been initialized, you can't call it twice, doing nothing"); }规划路径NavfnROS::makePlan方法生成规划路径： bool NavfnROS::makePlan(const geometry_msgs::PoseStamped& start, const geometry_msgs::PoseStamped& goal, double tolerance, std::vector<geometry_msgs::PoseStamped>& plan){ boost::mutex::scoped_lock lock(mutex_); if(!initialized_){ ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); return false; } //clear the plan, just in case plan.clear(); ros::NodeHandle n; //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame if(goal.header.frame_id != global_frame_){ ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), goal.header.frame_id.c_str()); return false; } if(start.header.frame_id != global_frame_){ ROS_ERROR("The start pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), start.header.frame_id.c_str()); return false; }//获取开始位置，并确保开始位置在地图内 double wx = start.pose.position.x; double wy = start.pose.position.y; unsigned int mx, my; if(!costmap_->worldToMap(wx, wy, mx, my)){ ROS_WARN("The robot's start position is off the global costmap. Planning will always fail, are you sure the robot has been properly localized?"); return false; }//清除小车位置的障碍物设定，否则就无法移动了 //clear the starting cell within the costmap because we know it can't be an obstacle clearRobotCell(start, mx, my);//确定导航生成的范围在地图内 //make sure to resize the underlying array that Navfn uses planner_->setNavArr(costmap_->getSizeInCellsX(), costmap_->getSizeInCellsY()); planner_->setCostmap(costmap_->getCharMap(), true, allow_unknown_); int map_start[2]; map_start[0] = mx; map_start[1] = my;//同样，确认目标地点在地图内 wx = goal.pose.position.x; wy = goal.pose.position.y; if(!costmap_->worldToMap(wx, wy, mx, my)){ if(tolerance <= 0.0){ ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal."); return false; } mx = 0; my = 0; } int map_goal[2]; map_goal[0] = mx; map_goal[1] = my;//设置规划起点和目标点 planner_->setStart(map_goal); planner_->setGoal(map_start);//调用calcNavFnDijkstra算法进行路径规划 //bool success = planner_->calcNavFnAstar(); planner_->calcNavFnDijkstra(true); double resolution = costmap_->getResolution(); geometry_msgs::PoseStamped p, best_pose; p = goal;//找到可能的合法点，只要其距离目标点在容忍的距离内，不是障碍物即可 bool found_legal = false; double best_sdist = DBL_MAX; p.pose.position.y = goal.pose.position.y - tolerance; while(p.pose.position.y <= goal.pose.position.y + tolerance){ p.pose.position.x = goal.pose.position.x - tolerance; while(p.pose.position.x <= goal.pose.position.x + tolerance){ double potential = getPointPotential(p.pose.position); double sdist = sq_distance(p, goal); if(potential < POT_HIGH && sdist < best_sdist){ best_sdist = sdist; best_pose = p; found_legal = true; } p.pose.position.x += resolution; } p.pose.position.y += resolution; }//找到合法点，调用getPlanFromPotential方法获取规划plan if(found_legal){ //extract the plan if(getPlanFromPotential(best_pose, plan)){ //make sure the goal we push on has the same timestamp as the rest of the plan geometry_msgs::PoseStamped goal_copy = best_pose; goal_copy.header.stamp = ros::Time::now(); plan.push_back(goal_copy); } else{ ROS_ERROR("Failed to get a plan from potential when a legal potential was found. This shouldn't happen."); } }//如果打开可视化可能点显示，就发布这些点，rviz就可以显示它们 if (visualize_potential_) { // Publish the potentials as a PointCloud2 sensor_msgs::PointCloud2 cloud; cloud.width = 0; cloud.height = 0; cloud.header.stamp = ros::Time::now(); cloud.header.frame_id = global_frame_; sensor_msgs::PointCloud2Modifier cloud_mod(cloud); cloud_mod.setPointCloud2Fields(4, "x", 1, sensor_msgs::PointField::FLOAT32, "y", 1, sensor_msgs::PointField::FLOAT32, "z", 1, sensor_msgs::PointField::FLOAT32, "pot", 1, sensor_msgs::PointField::FLOAT32); cloud_mod.resize(planner_->ny planner_->nx); sensor_msgs::PointCloud2Iterator<float> iter_x(cloud, "x"); PotarrPoint pt; float pp = planner_->potarr; double pot_x, pot_y; for (unsigned int i = 0; i < (unsigned int)planner_->nyplanner_->nx ; i++) { if (pp[i] < 10e7) { mapToWorld(i%planner_->nx, i/planner_->nx, pot_x, pot_y); iter_x[0] = pot_x; iter_x[1] = pot_y; iter_x[2] = pp[i]/pp[planner_->start[1]planner_->nx + planner_->start[0]]20; iter_x[3] = pp[i]; ++iter_x; } } potarr_pub_.publish(cloud); } //publish the plan for visualization purposes publishPlan(plan, 0.0, 1.0, 0.0, 0.0); return !plan.empty(); }核心算法：Dijkstra规划算法下面就是主要的Dijkstra规划算法：算法通过从目标点进行传播，直到起点。
bool NavFn::calcNavFnDijkstra(bool atStart) {//初始化数组准备下一轮传播 setupNavFn(true);//第一个参数为最大传播次数限制，atStart决定是否在起点停止传播，因为算法是从终点Goal出发的。
// calculate the nav fn and path propNavFnDijkstra(std::max(nxny/20,nx+ny),atStart);//计算路径，路径最大含有nxny/2个路径点 // path int len = calcPath(nxny/2); if (len > 0)// found plan { ROS_DEBUG("[NavFn] Path found, %d steps\n", len); return true; } else { ROS_DEBUG("[NavFn] No path found\n"); return false; } }//设置路径可能点数组，开始新一轮算法。
// Set up navigation potential arrays for new propagation void NavFn::setupNavFn(bool keepit) { // reset values in propagation arrays for (int i=0; i<ns; i++) {//设置所有点都是高cost的点，即未计算过的点，无限大的cost。
potarr[i] = POT_HIGH; if (!keepit) costarr[i] = COST_NEUTRAL; gradx[i] = grady[i] = 0.0; }//设置边界点都是障碍物类型的COST，即算法不能突破边界。
// outer bounds of cost array COSTTYPE pc; pc = costarr; for (int i=0; i<nx; i++) pc++ = COST_OBS; pc = costarr + (ny-1)nx; for (int i=0; i<nx; i++) pc++ = COST_OBS; pc = costarr; for (int i=0; i<ny; i++, pc+=nx) pc = COST_OBS; pc = costarr + nx - 1; for (int i=0; i<ny; i++, pc+=nx) pc = COST_OBS;//3种优先级的可能路径点缓冲变量，优先级 curP > nextP > overP 。
// priority buffers curT = COST_OBS; curP = pb1; curPe = 0; nextP = pb2; nextPe = 0; overP = pb3; overPe = 0; memset(pending, 0, nssizeof(bool));//设置目标点的cost为0，传播从目标点开始，k是目标点在数组potarr里面的索引值。
//通过计算目标点的邻接点的cost持续向外传播。
// set goal int k = goal[0] + goal[1]nx;//初始化目标点k的cost为0，并把其邻接点（上下左右）放入当前缓冲器curP中，以便下一次传播。
initCost(k,0);//计算障碍物点的数量。
// find # of obstacle cells pc = costarr; int ntot = 0; for (int i=0; i<ns; i++, pc++) { if (pc >= COST_OBS) ntot++;// number of cells that are obstacles } nobs = ntot; } //初始化目标点k的cost为0，并把其邻接点（上下左右）放入当前缓冲器curP中，以便下一次传播。
// initialize a goal-type cost for starting propagation void NavFn::initCost(int k, float v) { potarr[k] = v; push_cur(k+1); push_cur(k-1); push_cur(k-nx); push_cur(k+nx); }//点n必须不是障碍物才计算cost。
#define push_cur(n) { if (n>=0 && n<ns && !pending[n] && \ costarr[n]<COST_OBS && curPe<PRIORITYBUFSIZE) \ { curP[curPe++]=n; pending[n]=true; }} // // main propagation function // Dijkstra method, breadth-first // runs for a specified number of cycles, // or until it runs out of cells to update, // or until the Start cell is found (atStart = true) ////根据Dijkstra算法开始传播，广度优先，跑cycles次算法直到所有格子都完成更新或到达了起始格子。
bool NavFn::propNavFnDijkstra(int cycles, bool atStart) { int nwv = 0;// max priority block size int nc = 0;// number of cells put into priority blocks int cycle = 0;// which cycle we're on//得到开始点的数组索引值 // set up start cell int startCell = start[1]nx + start[0]; for (; cycle < cycles; cycle++) // go for this many cycles, unless interrupted { //当前缓冲器curP和下次缓冲器nextP都为空，表明所有点都计算过了，退出循环 if (curPe == 0 && nextPe == 0) // priority blocks empty break; // stats 统计计算过的点nc nc += curPe; if (curPe > nwv) nwv = curPe;//重置当前缓冲器中的点的待计算状态 // reset pending flags on current priority buffer int pb = curP; int i = curPe; while (i-- > 0) pending[(pb++)] = false;//调用updateCell计算当前缓冲器中的所有点 // process current priority buffer pb = curP; i = curPe; while (i-- > 0) updateCell(pb++); if (displayInt > 0 && (cycle % displayInt) == 0) displayFn(this);//3个优先级，curP完了，接着nextP // swap priority blocks curP <=> nextP curPe = nextPe; nextPe = 0; pb = curP;// swap buffers curP = nextP; nextP = pb;//nextP完了，接着overP // see if we're done with this priority level if (curPe == 0) { curT += priInc;// increment priority threshold curPe = overPe;// set current to overflow block overPe = 0; pb = curP;// swap buffers curP = overP; overP = pb; }//如果覆盖率起点，结束此轮计算 // check if we've hit the Start cell if (atStart) if (potarr[startCell] < POT_HIGH) break; } ROS_DEBUG("[NavFn] Used %d cycles, %d cells visited (%d%%), priority buf max %d\n", cycle,nc,(int)((nc100.0)/(ns-nobs)),nwv); if (cycle < cycles) return true; // finished up here else return false; } //计算每个点cost的函数 inline void NavFn::updateCell(int n) {//得到左l 右r 上u 下d 4个邻居点的 cost（可能）值。
// get neighbors float u,d,l,r; l = potarr[n-1]; r = potarr[n+1]; u = potarr[n-nx]; d = potarr[n+nx]; // ROS_INFO("[Update] c: %0.1f l: %0.1f r: %0.1f u: %0.1f d: %0.1f\n", // potarr[n], l, r, u, d); // ROS_INFO("[Update] cost: %d\n", costarr[n]); //找到列（column）方向的低cost tc 和交叉（across）方向的低cost ta // find lowest, and its lowest neighbor float ta, tc; if (l<r) tc=l; else tc=r; if (u<d) ta=u; else ta=d; //开始这次的更新，COST必须小于障碍物的COST。
// do planar wave update if (costarr[n] < COST_OBS)// don't propagate into obstacles { float hf = (float)costarr[n]; // traversability factor float dc = tc-ta;// relative cost between ta,tc//dc取绝对值，ta保存为较小值 if (dc < 0) // ta is lowest { dc = -dc; ta = tc; } // calculate new potential//横差值或纵差值的差值大于当前点的cost，则直接累加横差值或纵差值的较小值得到pot，否则用插值公式计算。
float pot; if (dc >= hf)// if too large, use ta-only update pot = ta+hf; else// two-neighbor interpolation update { // use quadratic approximation // might speed this up through table lookup, but still have to // do the divide float d = dc/hf; float v = -0.2301dd + 0.5307d + 0.7040; pot = ta + hfv; } // ROS_INFO("[Update] new pot: %d\n", costarr[n]);//如果计算出的pot值小于当期点的pot值，则表明当前传播路线更优，就更新邻接点。
// now add affected neighbors to priority blocks if (pot < potarr[n]) { float le = INVSQRT2(float)costarr[n-1]; float re = INVSQRT2(float)costarr[n+1]; float ue = INVSQRT2(float)costarr[n-nx]; float de = INVSQRT2(float)costarr[n+nx]; potarr[n] = pot; if (pot < curT)// low-cost buffer block 如果pot值小于当前阈值curT {//邻接点预期计算后的cost会小于当前的cost，则放入nextP进行下一次（curP计算完后）进行计算。
if (l > pot+le) push_next(n-1); if (r > pot+re) push_next(n+1); if (u > pot+ue) push_next(n-nx); if (d > pot+de) push_next(n+nx); } else// overflow block { if (l > pot+le) push_over(n-1); if (r > pot+re) push_over(n+1); if (u > pot+ue) push_over(n-nx); if (d > pot+de) push_over(n+nx); } } } } //根据上面计算出的pot值，算出每个方向的梯度，求出路径 // Path construction // Find gradient at array points, interpolate path // Use step size of pathStep, usually 0.5 pixel // // Some sanity checks: // 1. Stuck at same index position // 2. Doesn't get near goal // 3. Surrounded by high potentials // int NavFn::calcPath(int n, int st) { // test write //savemap("test");//path buf数组不够大时，重新初始化pathx, pathy // check path arrays if (npathbuf < n) { if (pathx) delete [] pathx; if (pathy) delete [] pathy; pathx = new float[n]; pathy = new float[n]; npathbuf = n; }//设置路径起始点，这儿为传入的起点start // set up start position at cell // st is always upper left corner for 4-point bilinear interpolation if (st == NULL) st = start; int stc = st[1]nx + st[0]; // set up offset float dx=0; float dy=0; npath = 0; // go for <n> cycles at most for (int i=0; i<n; i++) { // check if near goal int nearest_point=std::max(0,std::min(nxny-1,stc+(int)round(dx)+(int)(nxround(dy)))); if (potarr[nearest_point] < COST_NEUTRAL)//小于COST_NEUTRAL即表示到达目标点 { pathx[npath] = (float)goal[0]; pathy[npath] = (float)goal[1]; return ++npath;// done! } if (stc < nx || stc > ns-nx) // would be out of bounds { ROS_DEBUG("[PathCalc] Out of bounds"); return 0; } // add to path加入当前点到路径 pathx[npath] = stc%nx + dx; pathy[npath] = stc/nx + dy; npath++;//当前点的前后为同一个点，表示有冲突，无法继续向前 bool oscillation_detected = false; if( npath > 2 && pathx[npath-1] == pathx[npath-3] && pathy[npath-1] == pathy[npath-3] ) { ROS_DEBUG("[PathCalc] oscillation detected, attempting fix."); oscillation_detected = true; } int stcnx = stc+nx; int stcpx = stc-nx; // check for potentials at eight positions near cell if (potarr[stc] >= POT_HIGH || potarr[stc+1] >= POT_HIGH || potarr[stc-1] >= POT_HIGH || potarr[stcnx] >= POT_HIGH || potarr[stcnx+1] >= POT_HIGH || potarr[stcnx-1] >= POT_HIGH || potarr[stcpx] >= POT_HIGH || potarr[stcpx+1] >= POT_HIGH || potarr[stcpx-1] >= POT_HIGH || oscillation_detected) { ROS_DEBUG("[Path] Pot fn boundary, following grid (%0.1f/%d)", potarr[stc], npath); // check eight neighbors to find the lowest int minc = stc; int minp = potarr[stc]; int st = stcpx - 1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st = stc-1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st = stc+1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st = stcnx-1; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } st++; if (potarr[st] < minp) {minp = potarr[st]; minc = st; } stc = minc; dx = 0; dy = 0; ROS_DEBUG("[Path] Pot: %0.1f pos: %0.1f,%0.1f", potarr[stc], pathx[npath-1], pathy[npath-1]); if (potarr[stc] >= POT_HIGH) { ROS_DEBUG("[PathCalc] No path found, high potential"); //savemap("navfn_highpot"); return 0; } } // have a good gradient here else { // get grad at four positions near cell计算梯度值 gradCell(stc); gradCell(stc+1); gradCell(stcnx); gradCell(stcnx+1); // get interpolated gradient float x1 = (1.0-dx)gradx[stc] + dxgradx[stc+1];//计算第一行x的梯度 float x2 = (1.0-dx)gradx[stcnx] + dxgradx[stcnx+1];//计算第二行x的梯度 float x = (1.0-dy)x1 + dyx2; // interpolated x//计算x的梯度 float y1 = (1.0-dx)grady[stc] + dxgrady[stc+1];//计算第一列y的梯度 float y2 = (1.0-dx)grady[stcnx] + dxgrady[stcnx+1];//计算第二列y的梯度 float y = (1.0-dy)y1 + dyy2; // interpolated y//计算y的梯度 // show gradients ROS_DEBUG("[Path] %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f %0.2f,%0.2f; final x=%.3f, y=%.3f\n", gradx[stc], grady[stc], gradx[stc+1], grady[stc+1], gradx[stcnx], grady[stcnx], gradx[stcnx+1], grady[stcnx+1], x, y); // check for zero gradient, failed 0梯度，无法继续，失败 if (x == 0.0 && y == 0.0) { ROS_DEBUG("[PathCalc] Zero gradient"); return 0; } // move in the right direction 增加对应方向的dx, dy float ss = pathStep/hypot(x, y); dx += xss; dy += yss; // check for overflow if (dx > 1.0) { stc++; dx -= 1.0; } if (dx < -1.0) { stc--; dx += 1.0; } if (dy > 1.0) { stc+=nx; dy -= 1.0; } if (dy < -1.0) { stc-=nx; dy += 1.0; } } // ROS_INFO("[Path] Pot: %0.1f grad: %0.1f,%0.1f pos: %0.1f,%0.1f\n", // potarr[stc], x, y, pathx[npath-1], pathy[npath-1]); } // return npath;// out of cycles, return failure ROS_DEBUG("[PathCalc] No path found, path too long"); //savemap("navfn_pathlong"); return 0;// out of cycles, return failure }获取路径 bool NavfnROS::getPlanFromPotential(const geometry_msgs::PoseStamped& goal, std::vector<geometry_msgs::PoseStamped>& plan){ if(!initialized_){ ROS_ERROR("This planner has not been initialized yet, but it is being used, please call initialize() before use"); return false; } //clear the plan, just in case plan.clear(); //until tf can handle transforming things that are way in the past... we'll require the goal to be in our global frame if(goal.header.frame_id != global_frame_){ ROS_ERROR("The goal pose passed to this planner must be in the %s frame. It is instead in the %s frame.", global_frame_.c_str(), goal.header.frame_id.c_str()); return false; } double wx = goal.pose.position.x; double wy = goal.pose.position.y; //the potential has already been computed, so we won't update our copy of the costmap unsigned int mx, my; if(!costmap_->worldToMap(wx, wy, mx, my)){ ROS_WARN_THROTTLE(1.0, "The goal sent to the navfn planner is off the global costmap. Planning will always fail to this goal."); return false; } int map_goal[2]; map_goal[0] = mx; map_goal[1] = my; planner_->setStart(map_goal); planner_->calcPath(costmap_->getSizeInCellsX() 4); //extract the plan 从上面plan中获取x,y的坐标 float x = planner_->getPathX(); float y = planner_->getPathY(); int len = planner_->getPathLen(); ros::Time plan_time = ros::Time::now(); for(int i = len - 1; i >= 0; --i){ //convert the plan to world coordinates 转换成世界坐标 double world_x, world_y; mapToWorld(x[i], y[i], world_x, world_y); geometry_msgs::PoseStamped pose; pose.header.stamp = plan_time; pose.header.frame_id = global_frame_; pose.pose.position.x = world_x; pose.pose.position.y = world_y; pose.pose.position.z = 0.0; pose.pose.orientation.x = 0.0; pose.pose.orientation.y = 0.0; pose.pose.orientation.z = 0.0; pose.pose.orientation.w = 1.0; plan.push_back(pose); //把坐标点加入到plan中 } //publish the plan for visualization purposes publishPlan(plan, 0.0, 1.0, 0.0, 0.0); return !plan.empty(); }