原创文章,转载请注明: 转载自慢慢的回味
本文链接地址: 全局规划器-NavfnROS
机器人导航路径的规划可以分为两种:全局规划和局部规划。对事先已经知道的静态信息进行的规划为全局规划,如根据高清地图进行的规划,全局规划器就如同平时生活中的地图导航一样,这是一个大尺度的规划。基于传感器信息进行的机器人控制为局部路径规划,这是一个小尺度的规划。大范围内的静态信息不易改变,只需要定期更新高清地图就可以保证规划有效性。局部范围的动态信息则需要摄像头,激光雷达等设备及时回馈,变化频繁。
Content:
本文介绍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_->ny*planner_->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(nx*ny/20,nx+ny),atStart); //计算路径,路径最大含有nx*ny/2个路径点 // path int len = calcPath(nx*ny/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, ns*sizeof(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)((nc*100.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.2301*d*d + 0.5307*d + 0.7040; pot = ta + hf*v; } // 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(nx*ny-1,stc+(int)round(dx)+(int)(nx*round(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] + dx*gradx[stc+1];//计算第一行x的梯度 float x2 = (1.0-dx)*gradx[stcnx] + dx*gradx[stcnx+1];//计算第二行x的梯度 float x = (1.0-dy)*x1 + dy*x2; // interpolated x//计算x的梯度 float y1 = (1.0-dx)*grady[stc] + dx*grady[stc+1];//计算第一列y的梯度 float y2 = (1.0-dx)*grady[stcnx] + dx*grady[stcnx+1];//计算第二列y的梯度 float y = (1.0-dy)*y1 + dy*y2; // 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 += x*ss; dy += y*ss; // 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(); } |
本作品采用知识共享署名 4.0 国际许可协议进行许可。