原创文章,转载请注明: 转载自慢慢的回味
本文链接地址: 局部规划器-TebLocalPlannerROS
由于Teb算法能很好的支持阿克曼小车,即模型类似于汽车一样的机器人,这儿使用TebLocalPlannerROS进行局部规划。Teb局部规划器包(teb_local_planner)是作为一个局部规划器(base_local_planner)插件的形式融入2D导航栈的。它基于时间的弹性算法来优化局部轨迹,满足:轨迹的时间尽量短即速度和加速度尽量大;和障碍物能明显分离;以及必须满足机器人动力学约束。
在Move Base初始化中通过参数指定base_local_planner为teb_local_planner/TebLocalPlannerROS
//默认的局部规划器为TrajectoryPlannerROS private_nh.param("base_local_planner", local_planner, std::string("base_local_planner/TrajectoryPlannerROS")); //create a local planner try { tc_ = blp_loader_.createInstance(local_planner); ROS_INFO("Created local_planner %s", local_planner.c_str()); //初始化局部规划器 tc_->initialize(blp_loader_.getName(local_planner), &tf_, controller_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", local_planner.c_str(), ex.what()); exit(1); } |
Move Base中通过调用tc_->computeVelocityCommands(cmd_vel)完成速度命令计算。
局部规划器初始化
初始化局部规划器节点等。类TebLocalPlannerROS在代码文件teb_local_planner_ros.cpp中。
void TebLocalPlannerROS::initialize(std::string name, tf2_ros::Buffer* tf, costmap_2d::Costmap2DROS* costmap_ros) { // check if the plugin is already initialized if(!initialized_) { // create Node Handle with name of plugin (as used in move_base for loading) ros::NodeHandle nh("~/" + name); // get parameters of TebConfig via the nodehandle and override the default config //从节点参数中获取参数值覆盖默认值 cfg_.loadRosParamFromNodeHandle(nh); // reserve some memory for obstacles obstacles_.reserve(500); // create visualization instance visualization_ = TebVisualizationPtr(new TebVisualization(nh, cfg_)); //获取小车的参数,这些参数将用于模型优化,路径计算,比如阿克曼小车的最小转弯半径 // create robot footprint/contour model for optimization RobotFootprintModelPtr robot_model = getRobotFootprintFromParamServer(nh); //这儿我们解读TebOptimalPlanner分支,HomotopyClassPlanner可以同时计算出多条轨迹方案 // create the planner instance if (cfg_.hcp.enable_homotopy_class_planning) { planner_ = PlannerInterfacePtr(new HomotopyClassPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); ROS_INFO("Parallel planning in distinctive topologies enabled."); } else { planner_ = PlannerInterfacePtr(new TebOptimalPlanner(cfg_, &obstacles_, robot_model, visualization_, &via_points_)); ROS_INFO("Parallel planning in distinctive topologies disabled."); } // init other variables //得到坐标转换对象和代价地图 tf_ = tf; costmap_ros_ = costmap_ros; costmap_ = costmap_ros_->getCostmap(); // locking should be done in MoveBase. costmap_model_ = boost::make_shared<base_local_planner::CostmapModel>(*costmap_); global_frame_ = costmap_ros_->getGlobalFrameID(); cfg_.map_frame = global_frame_; // TODO robot_base_frame_ = costmap_ros_->getBaseFrameID(); //Initialize a costmap to polygon converter if (!cfg_.obstacles.costmap_converter_plugin.empty()) { try { costmap_converter_ = costmap_converter_loader_.createInstance(cfg_.obstacles.costmap_converter_plugin); std::string converter_name = costmap_converter_loader_.getName(cfg_.obstacles.costmap_converter_plugin); // replace '::' by '/' to convert the c++ namespace to a NodeHandle namespace boost::replace_all(converter_name, "::", "/"); costmap_converter_->setOdomTopic(cfg_.odom_topic); costmap_converter_->initialize(ros::NodeHandle(nh, "costmap_converter/" + converter_name)); costmap_converter_->setCostmap2D(costmap_); costmap_converter_->startWorker(ros::Rate(cfg_.obstacles.costmap_converter_rate), costmap_, cfg_.obstacles.costmap_converter_spin_thread); ROS_INFO_STREAM("Costmap conversion plugin " << cfg_.obstacles.costmap_converter_plugin << " loaded."); } catch(pluginlib::PluginlibException& ex) { ROS_WARN("The specified costmap converter plugin cannot be loaded. All occupied costmap cells are treaten as point obstacles. Error message: %s", ex.what()); costmap_converter_.reset(); } } else ROS_INFO("No costmap conversion plugin specified. All occupied costmap cells are treaten as point obstacles."); //根据小车自身形状,计算最小最大距离,小车是有大小的,距离必须减掉这个最小距离 // Get footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. footprint_spec_ = costmap_ros_->getRobotFootprint(); costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); // init the odom helper to receive the robot's velocity from odom messages //初始化里程计,用来获取小车的速度 odom_helper_.setOdomTopic(cfg_.odom_topic); //配置动态参数重配置,可以在小车运行的时候修改Teb参数值 // setup dynamic reconfigure dynamic_recfg_ = boost::make_shared< dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig> >(nh); dynamic_reconfigure::Server<TebLocalPlannerReconfigureConfig>::CallbackType cb = boost::bind(&TebLocalPlannerROS::reconfigureCB, this, _1, _2); dynamic_recfg_->setCallback(cb); // validate optimization footprint and costmap footprint validateFootprints(robot_model->getInscribedRadius(), robot_inscribed_radius_, cfg_.obstacles.min_obstacle_dist); // setup callback for custom obstacles custom_obst_sub_ = nh.subscribe("obstacles", 1, &TebLocalPlannerROS::customObstacleCB, this); // setup callback for custom via-points via_points_sub_ = nh.subscribe("via_points", 1, &TebLocalPlannerROS::customViaPointsCB, this); // initialize failure detector ros::NodeHandle nh_move_base("~"); double controller_frequency = 5; nh_move_base.param("controller_frequency", controller_frequency, controller_frequency); failure_detector_.setBufferLength(std::round(cfg_.recovery.oscillation_filter_duration*controller_frequency)); // set initialized flag initialized_ = true; ROS_DEBUG("teb_local_planner plugin initialized."); } else { ROS_WARN("teb_local_planner has already been initialized, doing nothing."); } } |
计算速度命令
实现base_local_planner的computeVelocityCommands方法,用于根据当前位姿和速度计算下一速度命令。pose和velocity分别为小车当前位姿和速度,不过这儿没用,而是直接从里程计里重新获取。cmd_vel为返回的速度命令。
uint32_t TebLocalPlannerROS::computeVelocityCommands(const geometry_msgs::PoseStamped& pose, const geometry_msgs::TwistStamped& velocity, geometry_msgs::TwistStamped &cmd_vel, std::string &message) { // check if plugin initialized if(!initialized_) { ROS_ERROR("teb_local_planner has not been initialized, please call initialize() before using this planner"); message = "teb_local_planner has not been initialized"; return mbf_msgs::ExePathResult::NOT_INITIALIZED; } //初始化返回的速度命令 static uint32_t seq = 0; cmd_vel.header.seq = seq++; cmd_vel.header.stamp = ros::Time::now(); cmd_vel.header.frame_id = robot_base_frame_; cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; goal_reached_ = false; // Get robot pose 获取小车位置 geometry_msgs::PoseStamped robot_pose; costmap_ros_->getRobotPose(robot_pose); robot_pose_ = PoseSE2(robot_pose.pose); // Get robot velocity 从里程计中获取小车当前速度 geometry_msgs::PoseStamped robot_vel_tf; odom_helper_.getRobotVel(robot_vel_tf); robot_vel_.linear.x = robot_vel_tf.pose.position.x; robot_vel_.linear.y = robot_vel_tf.pose.position.y; robot_vel_.angular.z = tf2::getYaw(robot_vel_tf.pose.orientation); //修剪plan,去掉已经行驶过的路径 // prune global plan to cut off parts of the past (spatially before the robot) pruneGlobalPlan(*tf_, robot_pose, global_plan_, cfg_.trajectory.global_plan_prune_distance); //从全局规划中获取在当前位姿的局部转换全局规划transformed_plan(离当前位姿小于某个距离的邻接点)。 //下面局部规划器将在transformed_plan的限制下进行局部轨迹规划。即从全局的粗规划得到当前的局部细规划。 // Transform global plan to the frame of interest (w.r.t. the local costmap) std::vector<geometry_msgs::PoseStamped> transformed_plan; int goal_idx; geometry_msgs::TransformStamped tf_plan_to_global; if (!transformGlobalPlan(*tf_, global_plan_, robot_pose, *costmap_, global_frame_, cfg_.trajectory.max_global_plan_lookahead_dist, transformed_plan, &goal_idx, &tf_plan_to_global)) { ROS_WARN("Could not transform the global plan to the frame of the controller"); message = "Could not transform the global plan to the frame of the controller"; return mbf_msgs::ExePathResult::INTERNAL_ERROR; } //把当前transformed_plan经过的点添加进via_points_ // update via-points container if (!custom_via_points_active_) updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep); // check if global goal is reached //通过计算当前机器人位姿robot_pose_和全局目标global_goal的欧几里得距离,检查是否到目的点,包括位置(小于xy_goal_tolerance)和方向(小于yaw_goal_tolerance)。 //如果距离小于容错距离并且容错配置不用完成全局规划(!cfg_.goal_tolerance.complete_global_plan)或者当前经过点为空(via_points_.size() == 0),则认为目标到达,退出局部规划器。 geometry_msgs::PoseStamped global_goal; tf2::doTransform(global_plan_.back(), global_goal, tf_plan_to_global); double dx = global_goal.pose.position.x - robot_pose_.x(); double dy = global_goal.pose.position.y - robot_pose_.y(); double delta_orient = g2o::normalize_theta( tf2::getYaw(global_goal.pose.orientation) - robot_pose_.theta() ); if(fabs(std::sqrt(dx*dx+dy*dy)) < cfg_.goal_tolerance.xy_goal_tolerance && fabs(delta_orient) < cfg_.goal_tolerance.yaw_goal_tolerance && (!cfg_.goal_tolerance.complete_global_plan || via_points_.size() == 0)) { goal_reached_ = true; return mbf_msgs::ExePathResult::SUCCESS; } // check if we should enter any backup mode and apply settings configureBackupModes(transformed_plan, goal_idx); //如果transformed_plan为空,则不能做局部规划,退出局部规划器。 // Return false if the transformed global plan is empty if (transformed_plan.empty()) { ROS_WARN("Transformed plan is empty. Cannot determine a local plan."); message = "Transformed plan is empty"; return mbf_msgs::ExePathResult::INVALID_PATH; } //获取当前局部规划器的机器人目标点 // Get current goal point (last point of the transformed plan) robot_goal_.x() = transformed_plan.back().pose.position.x; robot_goal_.y() = transformed_plan.back().pose.position.y; // Overwrite goal orientation if needed if (cfg_.trajectory.global_plan_overwrite_orientation) { robot_goal_.theta() = estimateLocalGoalOrientation(global_plan_, transformed_plan.back(), goal_idx, tf_plan_to_global); // overwrite/update goal orientation of the transformed plan with the actual goal (enable using the plan as initialization) tf2::Quaternion q; q.setRPY(0, 0, robot_goal_.theta()); tf2::convert(q, transformed_plan.back().pose.orientation); } else { robot_goal_.theta() = tf2::getYaw(transformed_plan.back().pose.orientation); } // overwrite/update start of the transformed plan with the actual robot position (allows using the plan as initial trajectory) if (transformed_plan.size()==1) // plan only contains the goal { transformed_plan.insert(transformed_plan.begin(), geometry_msgs::PoseStamped()); // insert start (not yet initialized) } transformed_plan.front() = robot_pose; // update start //清空当前的障碍数组obstacles_以待更新 // clear currently existing obstacles obstacles_.clear(); //从代价地图中添加障碍物到obstacles_中 // Update obstacle container with costmap information or polygons provided by a costmap_converter plugin if (costmap_converter_) updateObstacleContainerWithCostmapConverter(); else updateObstacleContainerWithCostmap(); //如果有自定义障碍,也添加到obstacles_中 // also consider custom obstacles (must be called after other updates, since the container is not cleared) updateObstacleContainerWithCustomObstacles(); // Do not allow config changes during the following optimization step boost::mutex::scoped_lock cfg_lock(cfg_.configMutex()); // Now perform the actual planning // 调用TebOptimalPlanner::plan方法进行局部规划,下节具体讲解这个方法。 // bool success = planner_->plan(robot_pose_, robot_goal_, robot_vel_, cfg_.goal_tolerance.free_goal_vel); // straight line init bool success = planner_->plan(transformed_plan, &robot_vel_, cfg_.goal_tolerance.free_goal_vel); if (!success) { planner_->clearPlanner(); // force reinitialization for next time ROS_WARN("teb_local_planner was not able to obtain a local plan for the current setting."); ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); last_cmd_ = cmd_vel.twist; message = "teb_local_planner was not able to obtain a local plan"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } //更新机器人的内半径robot_inscribed_radius_和机器人的外半径robot_circumscribed_radius // Check feasibility (but within the first few states only) if(cfg_.robot.is_footprint_dynamic) { // Update footprint of the robot and minimum and maximum distance from the center of the robot to its footprint vertices. footprint_spec_ = costmap_ros_->getRobotFootprint(); costmap_2d::calculateMinAndMaxDistances(footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius); } //检查轨迹是否可行,比如拐弯必须满足机器人的内半径和外半径不能碰到障碍物,不能出地图。 bool feasible = planner_->isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses); if (!feasible) { cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; // now we reset everything to start again with the initialization of new trajectories. planner_->clearPlanner(); ROS_WARN("TebLocalPlannerROS: trajectory is not feasible. Resetting planner..."); ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); last_cmd_ = cmd_vel.twist; message = "teb_local_planner trajectory is not feasible"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } // Get the velocity command for this sampling interval //通过规划的轨迹,获取速度命令,包括x,y,z分量和方向,下节具体讲解这个方法。 if (!planner_->getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses)) { planner_->clearPlanner(); ROS_WARN("TebLocalPlannerROS: velocity command invalid. Resetting planner..."); ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); last_cmd_ = cmd_vel.twist; message = "teb_local_planner velocity command invalid"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } //使速度变得更加平滑 // Saturate velocity, if the optimization results violates the constraints (could be possible due to soft constraints). saturateVelocity(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.robot.max_vel_x, cfg_.robot.max_vel_y, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards); // convert rot-vel to steering angle if desired (carlike robot). // The min_turning_radius is allowed to be slighly smaller since it is a soft-constraint // and opposed to the other constraints not affected by penalty_epsilon. The user might add a safety margin to the parameter itself. if (cfg_.robot.cmd_angle_instead_rotvel) { cmd_vel.twist.angular.z = convertTransRotVelToSteeringAngle(cmd_vel.twist.linear.x, cmd_vel.twist.angular.z, cfg_.robot.wheelbase, 0.95*cfg_.robot.min_turning_radius); if (!std::isfinite(cmd_vel.twist.angular.z)) { cmd_vel.twist.linear.x = cmd_vel.twist.linear.y = cmd_vel.twist.angular.z = 0; last_cmd_ = cmd_vel.twist; planner_->clearPlanner(); ROS_WARN("TebLocalPlannerROS: Resulting steering angle is not finite. Resetting planner..."); ++no_infeasible_plans_; // increase number of infeasible solutions in a row time_last_infeasible_plan_ = ros::Time::now(); message = "teb_local_planner steering angle is not finite"; return mbf_msgs::ExePathResult::NO_VALID_CMD; } } // a feasible solution should be found, reset counter no_infeasible_plans_ = 0; // store last command (for recovery analysis etc.) last_cmd_ = cmd_vel.twist; // Now visualize everything planner_->visualize(); visualization_->publishObstacles(obstacles_); visualization_->publishViaPoints(via_points_); visualization_->publishGlobalPlan(global_plan_); return mbf_msgs::ExePathResult::SUCCESS; } |
规划逻辑
initial_plan为全局Plan中局部部分,start_vel为开始速度,free_goal_vel表示是否允许目标点速度不为0,只要initial_plan的Goal不是全局的Goal,自然允许free_goal_vel为true。
bool TebOptimalPlanner::plan(const std::vector<geometry_msgs::PoseStamped>& initial_plan, const geometry_msgs::Twist* start_vel, bool free_goal_vel) { ROS_ASSERT_MSG(initialized_, "Call initialize() first."); if (!teb_.isInit()) { //初始化轨迹,在起点到目标点之间抽样出规定的位置点和时间差值。 teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); } else // warm start { PoseSE2 start_(initial_plan.front().pose); PoseSE2 goal_(initial_plan.back().pose); if (teb_.sizePoses()>0 && (goal_.position() - teb_.BackPose().position()).norm() < cfg_->trajectory.force_reinit_new_goal_dist && fabs(g2o::normalize_theta(goal_.theta() - teb_.BackPose().theta())) < cfg_->trajectory.force_reinit_new_goal_angular) // actual warm start! teb_.updateAndPruneTEB(start_, goal_, cfg_->trajectory.min_samples); // update TEB else // goal too far away -> reinit { ROS_DEBUG("New goal: distance to existing goal is higher than the specified threshold. Reinitalizing trajectories."); teb_.clearTimedElasticBand(); teb_.initTrajectoryToGoal(initial_plan, cfg_->robot.max_vel_x, cfg_->robot.max_vel_theta, cfg_->trajectory.global_plan_overwrite_orientation, cfg_->trajectory.min_samples, cfg_->trajectory.allow_init_with_backwards_motion); } } if (start_vel) setVelocityStart(*start_vel); if (free_goal_vel) setVelocityGoalFree(); else vel_goal_.first = true; // we just reactivate and use the previously set velocity (should be zero if nothing was modified) //通过构建一个由顶点和边构成的图,然后用G2O优化得到最优的轨迹。 // now optimize return optimizeTEB(cfg_->optim.no_inner_iterations, cfg_->optim.no_outer_iterations); } |
bool TebOptimalPlanner::optimizeTEB(int iterations_innerloop, int iterations_outerloop, bool compute_cost_afterwards, double obst_cost_scale, double viapoint_cost_scale, bool alternative_time_cost) { if (cfg_->optim.optimization_activate==false) return false; bool success = false; optimized_ = false; double weight_multiplier = 1.0; // TODO(roesmann): we introduced the non-fast mode with the support of dynamic obstacles // (which leads to better results in terms of x-y-t homotopy planning). // however, we have not tested this mode intensively yet, so we keep // the legacy fast mode as default until we finish our tests. bool fast_mode = !cfg_->obstacles.include_dynamic_obstacles; for(int i=0; i<iterations_outerloop; ++i) { if (cfg_->trajectory.teb_autosize) { //teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples); teb_.autoResize(cfg_->trajectory.dt_ref, cfg_->trajectory.dt_hysteresis, cfg_->trajectory.min_samples, cfg_->trajectory.max_samples, fast_mode); } //生成G2O图 success = buildGraph(weight_multiplier); if (!success) { clearGraph(); return false; } //优化G2O图 success = optimizeGraph(iterations_innerloop, false); if (!success) { clearGraph(); return false; } optimized_ = true; if (compute_cost_afterwards && i==iterations_outerloop-1) // compute cost vec only in the last iteration computeCurrentCost(obst_cost_scale, viapoint_cost_scale, alternative_time_cost); clearGraph(); weight_multiplier *= cfg_->optim.weight_adapt_factor; } return true; } |
构建图并优化
使用g2o构建一个图并优化:
bool TebOptimalPlanner::buildGraph(double weight_multiplier) { if (!optimizer_->edges().empty() || !optimizer_->vertices().empty()) { ROS_WARN("Cannot build graph, because it is not empty. Call graphClear()!"); return false; } //添加顶点,待优化的对象 // add TEB vertices AddTEBVertices(); //下面的都是边,方程的约束项,每个边都有computeError()方法来计算error,图优化的目的就是尽可能的使总体error最小 。 // add Edges (local cost functions) if (cfg_->obstacles.legacy_obstacle_association) AddEdgesObstaclesLegacy(weight_multiplier); else //添加一个EdgeObstacle边,其计算error的方法如下: // void computeError() // { // const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]); // double dist = robot_model_->calculateDistance(bandpt->pose(), _measurement); // _error[0] = penaltyBoundFromBelow(dist, cfg_->obstacles.min_obstacle_dist, cfg_->optim.penalty_epsilon); //_measurement为obstacle的位置,通过计算出机器人将要路过的点和障碍物的距离dist,判断其是否大于规定的最小距离(min_obstacle_dist+penalty_epsilon), //是就返回0为错误,否则返回一个大于0的数表示错误。 AddEdgesObstacles(weight_multiplier); if (cfg_->obstacles.include_dynamic_obstacles) AddEdgesDynamicObstacles(); //添加轨迹需要途经点的约束EdgeViaPoint,其计算error的方法如下: // void computeError() // { // const VertexPose* bandpt = static_cast<const VertexPose*>(_vertices[0]); // _error[0] = (bandpt->position() - *_measurement).norm(); //error通过计算轨迹点和经过点的距离来表示,因为是途经点,所以这儿要求其必须距离越小越好。 AddEdgesViaPoints(); //添加速度约束EdgeVelocity,其计算error的方法如下: // void computeError() // { // const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]); // const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]); // const VertexTimeDiff* deltaT = static_cast<const VertexTimeDiff*>(_vertices[2]); // const Eigen::Vector2d deltaS = conf2->estimate().position() - conf1->estimate().position(); // double dist = deltaS.norm(); // const double angle_diff = g2o::normalize_theta(conf2->theta() - conf1->theta()); // if (cfg_->trajectory.exact_arc_length && angle_diff != 0) // { // double radius = dist/(2*sin(angle_diff/2)); // dist = fabs( angle_diff * radius ); // actual arg length! // } // double vel = dist / deltaT->estimate(); // vel *= fast_sigmoid( 100 * (deltaS.x()*cos(conf1->theta()) + deltaS.y()*sin(conf1->theta())) ); // consider direction // const double omega = angle_diff / deltaT->estimate(); // _error[0] = penaltyBoundToInterval(vel, -cfg_->robot.max_vel_x_backwards, cfg_->robot.max_vel_x,cfg_->optim.penalty_epsilon); // _error[1] = penaltyBoundToInterval(omega, cfg_->robot.max_vel_theta,cfg_->optim.penalty_epsilon); //_error[0]表示速度vel必须在最小(-cfg_->robot.max_vel_x_backwards)和最大(cfg_->robot.max_vel_x)之间。 //_error[1]表示角度变化omega必须在最小(-cfg_->robot.max_vel_theta)和最大(cfg_->robot.max_vel_theta)之间。 AddEdgesVelocity(); //同速度约束差不多,添加加速度约束EdgeAcceleration,线加速度和角加速度必须在区间之内。 AddEdgesAcceleration(); //添加时间约束EdgeTimeOptimal,到下一点的时间越小越好。 AddEdgesTimeOptimal(); //添加当前点到下一点的路径约束EdgeShortestPath,距离越小越好。 AddEdgesShortestPath(); if (cfg_->robot.min_turning_radius == 0 || cfg_->optim.weight_kinematics_turning_radius == 0) AddEdgesKinematicsDiffDrive(); // we have a differential drive robot else //如果是汽车类型的机器人,还得添加最小转弯半径约束EdgeKinematicsCarlike: // void computeError() // { // const VertexPose* conf1 = static_cast<const VertexPose*>(_vertices[0]); // const VertexPose* conf2 = static_cast<const VertexPose*>(_vertices[1]); // Eigen::Vector2d deltaS = conf2->position() - conf1->position(); // _error[0] = fabs( ( cos(conf1->theta())+cos(conf2->theta()) ) * deltaS[1] - ( sin(conf1->theta())+sin(conf2->theta()) ) * deltaS[0] ); // // limit minimum turning radius // double angle_diff = g2o::normalize_theta( conf2->theta() - conf1->theta() ); // if (angle_diff == 0) // _error[1] = 0; // straight line motion // else if (cfg_->trajectory.exact_arc_length) // use exact computation of the radius // _error[1] = penaltyBoundFromBelow(fabs(deltaS.norm()/(2*sin(angle_diff/2))), cfg_->robot.min_turning_radius, 0.0); // else // _error[1] = penaltyBoundFromBelow(deltaS.norm() / fabs(angle_diff), cfg_->robot.min_turning_radius, 0.0); //_error[0]表示角度越大错误越小,_error[1]表示如果超过最小转弯半径越多错误越大,即这两个错误约束角度变化最大为最小转弯半径为最优。 AddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below. AddEdgesPreferRotDir(); if (cfg_->optim.weight_velocity_obstacle_ratio > 0) AddEdgesVelocityObstacleRatio(); return true; } //调用G2O优化器进行优化图 bool TebOptimalPlanner::optimizeGraph(int no_iterations,bool clear_after) { if (cfg_->robot.max_vel_x<0.01) { ROS_WARN("optimizeGraph(): Robot Max Velocity is smaller than 0.01m/s. Optimizing aborted..."); if (clear_after) clearGraph(); return false; } if (!teb_.isInit() || teb_.sizePoses() < cfg_->trajectory.min_samples) { ROS_WARN("optimizeGraph(): TEB is empty or has too less elements. Skipping optimization."); if (clear_after) clearGraph(); return false; } optimizer_->setVerbose(cfg_->optim.optimization_verbose); optimizer_->initializeOptimization(); int iter = optimizer_->optimize(no_iterations); // Save Hessian for visualization // g2o::OptimizationAlgorithmLevenberg* lm = dynamic_cast<g2o::OptimizationAlgorithmLevenberg*> (optimizer_->solver()); // lm->solver()->saveHessian("~/MasterThesis/Matlab/Hessian.txt"); if(!iter) { ROS_ERROR("optimizeGraph(): Optimization failed! iter=%i", iter); return false; } if (clear_after) clearGraph(); return true; } |
计算速度
通过调用getVelocityCommand,从优化后的位置中选择look_ahead_poses和当前位置计算deltaS和deltaT,最后得到vx, vy和方向omega。
void TebOptimalPlanner::extractVelocity(const PoseSE2& pose1, const PoseSE2& pose2, double dt, double& vx, double& vy, double& omega) const { if (dt == 0) { vx = 0; vy = 0; omega = 0; return; } Eigen::Vector2d deltaS = pose2.position() - pose1.position(); if (cfg_->robot.max_vel_y == 0) // nonholonomic robot { Eigen::Vector2d conf1dir( cos(pose1.theta()), sin(pose1.theta()) ); // translational velocity double dir = deltaS.dot(conf1dir); vx = (double) g2o::sign(dir) * deltaS.norm()/dt; vy = 0; } else // holonomic robot { // transform pose 2 into the current robot frame (pose1) // for velocities only the rotation of the direction vector is necessary. // (map->pose1-frame: inverse 2d rotation matrix) double cos_theta1 = std::cos(pose1.theta()); double sin_theta1 = std::sin(pose1.theta()); double p1_dx = cos_theta1*deltaS.x() + sin_theta1*deltaS.y(); double p1_dy = -sin_theta1*deltaS.x() + cos_theta1*deltaS.y(); vx = p1_dx / dt; vy = p1_dy / dt; } // rotational velocity double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta()); omega = orientdiff/dt; } bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, int look_ahead_poses) const { if (teb_.sizePoses()<2) { ROS_ERROR("TebOptimalPlanner::getVelocityCommand(): The trajectory contains less than 2 poses. Make sure to init and optimize/plan the trajectory fist."); vx = 0; vy = 0; omega = 0; return false; } look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1)); double dt = 0.0; for(int counter = 0; counter < look_ahead_poses; ++counter) { dt += teb_.TimeDiff(counter); if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory? { look_ahead_poses = counter + 1; break; } } if (dt<=0) { ROS_ERROR("TebOptimalPlanner::getVelocityCommand() - timediff<=0 is invalid!"); vx = 0; vy = 0; omega = 0; return false; } // Get velocity from the first two configurations extractVelocity(teb_.Pose(0), teb_.Pose(look_ahead_poses), dt, vx, vy, omega); return true; } |
本作品采用知识共享署名 4.0 国际许可协议进行许可。