莱芜高端网站设计建设,wordpress文字加效果,个性化网站建设公司电话,网站ip和pv的比例teb算法的基本思路之前已经看完了#xff0c;今天主要看一下teb算法的参数配置文件#xff0c;分析一下每个配置参数的作用#xff1a;
teb的参数主要可以包含以下几个部分#xff1a;
1、Trajectory
Trajectory的参数顾名思义#xff0c;就是对路径生效的一些参数今天主要看一下teb算法的参数配置文件分析一下每个配置参数的作用
teb的参数主要可以包含以下几个部分
1、Trajectory
Trajectory的参数顾名思义就是对路径生效的一些参数仔细看一下每个参数的作用
1.1、teb_autosize if (cfg_-trajectory.teb_autosize){//teb_.autoResize(cfg_-trajectory.dt_ref, cfg_-trajectory.dt_hysteresis, cfg_-trajectory.min_samples, cfg_-trajectory.max_samples);//该函数可以新插入或是删除路径中的点初始化后的路径并且一次只能操作一个点。 //1.障碍物添加或移除时会造成teb曲线的延长或缩短teb为了继续遵循dt所以要重排路径。 //2. 在接近终点时dt会变小不懂这会导致优化运算负荷大且不稳定。第二点还看不懂。 //添加点的规则是 if dtdt(ref) dt(hysteresis)) 时插点 ifdtdt(ref) - dt(hysteresis) 时删点// 这样可以让点到点所需的时间dt维持在dtref附近。两个条件有难以同时满足程序中定义了最大迭代次数以避免卡死。//查新值的方法是插入两点的平均值作为中间点。 删点的方法是删去i点的时间i1点的位置。//dt_ref 预设的dt dt_hysteresis dt的缓冲区间 min_sample max_samples 最小和最大路径点数 fast_mode 是否自动迭代重复resize直到整条路径resize完成的布尔值。teb_.autoResize(cfg_-trajectory.dt_ref, cfg_-trajectory.dt_hysteresis, cfg_-trajectory.min_samples, cfg_-trajectory.max_samples, fast_mode);}teb_autosize参数用于是否自动更新/插入或是删除路径中的点初始化后的路径 trajectory.teb_autosize函数使用。teb的优化是基于g2o的而g2o本身需要点与边的概念这里的点就是局部路径规划的位姿点为了保证优化的稳定性需要保证两个位姿之间距离不能太远也不能太近如果该参数为true时会对路径长度进行优化。
1.2、dt_ref
这个参数在两个地方用到了
第一个地方
//dt_ref 预设的dt dt_hysteresis dt的缓冲区间 min_sample max_samples 最小和最大路径点数 fast_mode 是否自动迭代重复resize直到整条路径resize完成的布尔值。teb_.autoResize(cfg_-trajectory.dt_ref, cfg_-trajectory.dt_hysteresis, cfg_-trajectory.min_samples, cfg_-trajectory.max_samples, fast_mode);这个地方跟1.1参数配合使用用于判断是否插入新的点位。即两个点之间的距离/最大速度超过dt_ref则会插入新的点位
第二个地方
for(int counter 0; counter look_ahead_poses; counter){//计算一个运动时间从当前点到前向多少个点的理论时间和dt teb_.TimeDiff(counter);//dt_ref为两个点之间允许的最大时间如果超过这个时间会进行点的插入if(dt cfg_-trajectory.dt_ref * look_ahead_poses) // TODO: change to look-ahead time? Refine trajectory?{look_ahead_poses counter 1;break;}}这里是在优化位姿之后速度计算的地方因为位姿变化了所以需要重新计算一下点与点之间的时间如果时间差比较大则需要重新插入点位。
1.3、dt_hysteresis
预设时间缓冲区与dt_ref联用如果两点之间的运动时间超过dt_refdt_hysteresis会进行点的插入 如果两点之间的运动时间超过dt_ref-dt_hysteresis会进行点的删除。trajectory.teb_autosize函数使用。
1.4、global_plan_overwrite_orientation
//根据当前local planer 的goal的后面几个waypoint计算平均角度用这个角度来重写当前local planer 的goal的角度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)// (enable using the plan as initialization) 用真实的目标点朝向覆盖transformed_plan的目标点朝向tf2::Quaternion q;q.setRPY(0, 0, robot_goal_.theta());tf2::convert(q, transformed_plan.back().pose.orientation);} 这个参数用于决定是否重写每个点位的朝向因为考虑到某些全局路径规划中不会给出中间点位的朝向则这里会重写一下每个点的朝向。
1.5、allow_init_with_backwards_motion
// 将起点终点固定并计算每两个点之间的理想时间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);如果为true则当机器人初始位姿在起点位姿前面时机器人可能通过倒退走到起点位置。
1.6、max_global_plan_lookahead_dist
单次local plan 考虑的全局plan的最大距离。从全局路径到局部路径优化时需要截取长度长度由costmap与max_global_plan_lookahead_dist共同决定取其中更小的那个
1.7、force_reinit_new_goal_dist
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路径 裁剪掉node 顶点 更新起点或者终点。teb第一次初始化后后续每次仅需更新起点和终点即可teb_.updateAndPruneTEB(start_, goal_, cfg_-trajectory.min_samples); // update TEBelse // 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);}参数1.7与参数1.8合用一个是距离阈值一个是角度阈值当目标点的距离超过一定距离时会更新轨迹这里的目标点应该是局部路径规划的终点这个点是会一直随着路径更新的。
1.8、force_reinit_new_goal_angular
与1.7联用一个是距离阈值一个是角度阈值。如果上一个目标更新的旋转差大于指定的弧度值则强制规划器重新初始化轨迹
1.9、feasibility_check_no_poses
bool feasible planner_-isTrajectoryFeasible(costmap_model_.get(), footprint_spec_, robot_inscribed_radius_, robot_circumscribed_radius, cfg_.trajectory.feasibility_check_no_poses);用于优化后的轨迹检查前向检查多少个位姿点的可行性。判断优化后的一段轨迹需要判断是否可以运动这个参数用于判断可以运动的轨迹的长度。
1.10、exact_arc_length
用于g2o中添加边约束是使用如果为true则规划器在速度、加速度和转弯率计算中使用精确的弧长[增加的cpu时间]否则使用欧氏近似。
1.11、publish_feedback
用于可视化发布发布包含完整轨迹和活动障碍物列表的计划器反馈应仅出于评估或调试目的启用
1.12、control_look_ahead_poses
if (!planner_-getVelocityCommand(cmd_vel.twist.linear.x, cmd_vel.twist.linear.y, cmd_vel.twist.angular.z, cfg_.trajectory.control_look_ahead_poses))这个参数用于在速度计算时取前向多少个点的位姿作为终点计算。teb的速度比较简单就是受cfg_.trajectory.control_look_ahead_poses影响取向前多少个位姿点的姿态。然后用当前点的位姿与目标点做差。在前面运动优化部分有计算过每个位姿之间的理论运动时间。例如这里如果要取前向第五个点那么时间就是前面计算过的点1-2-3-4-5的时间和距离就是点1与点5的距离差。速度的话就是两点间距离/前面计算过的点与点之间理想时间和。
1.13、prevent_look_ahead_poses_near_goal
与control_look_ahead_poses联用control_look_ahead_poses在整条路径上使用 prevent_look_ahead_poses_near_goal在终点附近做限制。
//取前向多少个姿态点进行计算在局部路径中取点的数量由cfg_.trajectory.control_look_ahead_poses决定在目标点附近受cfg_-trajectory.prevent_look_ahead_poses_near_goal参数决定look_ahead_poses std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1 - cfg_-trajectory.prevent_look_ahead_poses_near_goal));1.14、visualize_with_time_as_z_axis_scale
该值用于可视化参数如果该值大于0则使用时间作为按该值缩放的z轴在3d中可视化轨迹和障碍物。对动态障碍物最有用
2、ViaPoints
ViaPoints相关的参数是跟路径点相关的一些参数
2.1、global_plan_viapoint_sep
if (!custom_via_points_active_)updateViaPointsContainer(transformed_plan, cfg_.trajectory.global_plan_viapoint_sep);遍历transformed_plan将其中二点间隔大于global_plan_viapoint_sep的点加入到 ViaPointContainer。也就是从全局路径中提取局部路径。
2.2、via_points_ordered
这个参数在g2o添加边约束时使用如果为true则计划器将遵守存储容器中通过点的顺序。
3、Robot
Robot的参数自然就是跟机器人相关的啦
3.1、max_vel_x
顾名思义向前运动的最大速度。最大速度在几个地方使用到
首先在轨迹初始化的时候会用于计算点与点之间的理想时间
// 将起点终点固定并计算每两个点之间的理想时间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);其次速度硬约束保证计算结果不超过设定值
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_trans, cfg_.robot.max_vel_theta, cfg_.robot.max_vel_x_backwards);3.2、max_vel_x_backwards
后退时候的最大速度这个参数跟3.1第二个使用点一样的作用对于计算后的速度发布前做最后的判断。
3.3、max_vel_theta
机器人运动的最大角速度作用同3.1
3.4、acc_lim_x
最大加速度仅在EdgeAcceleration类中使用用于g2o添加运动学约束边。
3.5、acc_lim_theta
最大角加速度与3.4一样在EdgeAcceleration类中使用用于g2o添加运动学约束边。
3.6、is_footprint_dynamic
是否动态更新footprintfootprint代表的是机器人的模型如果机器人模型是变化的则需要设置为true一般我们机器人模型都是固定的。
3.7、use_proportional_saturation
这个参数在速度计算完成后进行速度约束时使用作用是当机器人的速度、角速度超过一定阈值时会进行限幅。考虑一种情况当我的线速度x超过最大值时但是其他两项在范围内时我是只对x单独进行速度限幅还是将另外两个速度也等比例限制
if (cfg_.robot.use_proportional_saturation){double ratio std::min(std::min(ratio_x, ratio_y), ratio_omega);vx * ratio;vy * ratio;omega * ratio;}else{vx * ratio_x;vy * ratio_y;omega * ratio_omega;}这里参数如果设置为true则会对所有参数作同比例限幅如果是false则是各管各的。
3.8、transform_tolerance
ros::Duration(cfg_.robot.transform_tolerance)。在TF获取的时候设置的时间参数。
geometry_msgs::TransformStamped plan_to_global_transform tf.lookupTransform(global_frame, ros::Time(), plan_pose.header.frame_id, plan_pose.header.stamp,plan_pose.header.frame_id, ros::Duration(cfg_.robot.transform_tolerance));4、Carlike
Carlike参数主要是用于类似于汽车的机器人的运动学控制什么是类似于汽车的就是那种不能原地转向的小车。
4.1、min_turning_radius
顾名思义就是最小转弯半径
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);在速度发布前会根据该参数确定角速度如果速度/角速度得到的转弯半径小于设定值*系数0.95则会相应的修改角速度。
if (fabs(radius) min_turning_radius)radius double(g2o::sign(radius)) * min_turning_radius; 4.2、wheelbase
驱动轴和转向轴之间的距离仅适用于启用“cmd_angle_instead_rotvel”的车载机器人对于后轮机器人该值可能为负值 同样用于角速度修正不论4.1参数对角速度是否进行修正这里都会根据wheelbase做二次修正 double radius v/omega;if (fabs(radius) min_turning_radius)radius double(g2o::sign(radius)) * min_turning_radius; return std::atan(wheelbase / radius);4.3、cmd_angle_instead_rotvel
用于判断是否为carlike机器人只有该参数为true时参数4.1、4.2才生效。
5、Omnidirectional
全向轮机器人
5.1、max_vel_y
全向轮在y方向的最大速度作用与x方向的速度类似两个作用一个是用于添加运动学约束二是限制y方向的最大速度。
5.2、max_vel_trans
机器人的最大线速度。当它们的线性组合超过其值时将限制max_vel_x和max_vel_y。当设置为默认值0.0时它将被设置为等于max_vel_x。作用与5.1类似也是在这两个地方分别起作用。
5.3、acc_lim_y
y方向的最大加速度用于g2o的加速度约束项。
6、GoalTolerance
这个类别下的参数主要是对于目标点的一些参数设定
6.1、xy_goal_tolerance
允许到终点位置的最终欧几里得距离。这个值用于判断当前机器人是否到达目标点位置如果机器人到达目标点则当前姿态与目标点的欧式距离应小于该值。
if(fabs(std::sqrt(dx*dxdy*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) (base_local_planner::stopped(base_odom, cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel)|| cfg_.goal_tolerance.free_goal_vel)){goal_reached_ true;return mbf_msgs::ExePathResult::SUCCESS;}当然到终点的判定不止这一个这只是其中一个判定要求。
6.2、yaw_goal_tolerance
允许的目标方向的最终方向误差作用与6.1类似。
6.3、free_goal_vel
出于规划目的允许机器人的速度为非零机器人可以以最大速度到达目标。如果为false则判定机器人到达目标点的条件会严格一点需要判断当前速度是否也为一个很小的值cfg_.goal_tolerance.theta_stopped_vel, cfg_.goal_tolerance.trans_stopped_vel。同时planner函数在终点前面也会做对应的减速处理如果为true则对终点不做减速处理也不会判断终点附近的速度。
6.4、trans_stopped_vel
当6.3为false时对于是否到达目标点需要做速度判断作为到达终点的条件。判定的条件即是该值小于设定值6.4.低于我们认为机器人在平移中停止的最大速度
6.5、theta_stopped_vel
作用同6.4但是这里是对角速度做判断。用于到达目标点的角速度判断机器人到达目标点的判定条件之一当前角速度低于该值。
7、Obstacles
障碍物相关参数
7.1、min_obstacle_dist
与障碍物的最小期望间距参数在g2o优化项AddEdgesObstacles中使用作为姿态点的约束。
7.2、inflation_dist
膨胀距离。具有非零惩罚成本的障碍物周围的缓冲区应大于min_obstacle_dist才能生效
bool inflated cfg_-obstacles.inflation_dist cfg_-obstacles.min_obstacle_dist;
if (inflated){EdgeInflatedObstacle* dist_bandpt_obst new EdgeInflatedObstacle;dist_bandpt_obst-setVertex(0,teb_.PoseVertex(index));dist_bandpt_obst-setInformation(information_inflated);dist_bandpt_obst-setParameters(*cfg_, robot_model_.get(), obstacle);optimizer_-addEdge(dist_bandpt_obst);}else{EdgeObstacle* dist_bandpt_obst new EdgeObstacle;dist_bandpt_obst-setVertex(0,teb_.PoseVertex(index));dist_bandpt_obst-setInformation(information);dist_bandpt_obst-setParameters(*cfg_, robot_model_.get(), obstacle);optimizer_-addEdge(dist_bandpt_obst);};这个参数跟7.1参数在同一个地方使用判断添加的约束类型。
7.3、dynamic_obstacle_inflation_dist
动态障碍物的膨胀半径在g2o优化添加约束过程中如果参数7.4为true则约束中会添加动态障碍物约束。
7.4、include_dynamic_obstacles
这个参数有两个作用
首先用于判断是否需要添加动态障碍物约束 //添加动态障碍物边if (cfg_-obstacles.include_dynamic_obstacles)AddEdgesDynamicObstacles();其次也用于判断路径更新模式
bool fast_mode !cfg_-obstacles.include_dynamic_obstacles;
teb_.autoResize(cfg_-trajectory.dt_ref, cfg_-trajectory.dt_hysteresis, cfg_-trajectory.min_samples, cfg_-trajectory.max_samples, fast_mode);autoResize函数会更新局部路径点根据参数include_dynamic_obstacles决定是只更新前面的一个点还是更新一段路径上的点。
7.5、include_costmap_obstacles
是否添加costmap障碍物如果为true时会在更新障碍物地图时添加costmap的内容。
7.6、legacy_obstacle_association
用于判断以何种方式添加障碍物边 如果为true则使用旧的关联策略对于每个障碍物找到最近的TEB姿势否则使用新的关联策略。对于每个TEB姿势只找到“相关”障碍物。 // 添加障碍物边if (cfg_-obstacles.legacy_obstacle_association)AddEdgesObstaclesLegacy(weight_multiplier);elseAddEdgesObstacles(weight_multiplier);7.7、obstacle_association_force_inclusion_factor
在优化期间仅将相关障碍物与离散轨迹连接强制包括指定距离内的所有障碍物作为min_obstacle_dist的倍数例如选择2.0以考虑半径为2.0*min_obstocle_dist内的障碍物。 double dist robot_model_-calculateDistance(teb_.Pose(i), obst.get());// force considering obstacle if really close to the current poseif (dist cfg_-obstacles.min_obstacle_dist*cfg_-obstacles.obstacle_association_force_inclusion_factor){iter_obstacle-push_back(obst);continue;}添加障碍物约束的时候考虑一定距离内的障碍物添加到约束。
7.8、obstacle_association_cutoff_factor
裁减距离在添加障碍物边约束的时候如果障碍物距离超过这个值就直接不考虑了。 // cut-off distanceif (dist cfg_-obstacles.min_obstacle_dist*cfg_-obstacles.obstacle_association_cutoff_factor)continue;7.7与7.8两个分别代表了添加障碍物的最近距离与最远距离。如果这两个值不想等则它们中间还有一部分空间不满足这两个条件的则会再做其他判断是否添加到障碍物约束中。
7.9、costmap_obstacles_behind_robot_dist
添加代价地图中的障碍物时考虑的朝后距离用于障碍物添加函数updateObstacleContainerWithCostmap中。
if (costmap_-getCost(i,j) costmap_2d::LETHAL_OBSTACLE){Eigen::Vector2d obs;costmap_-mapToWorld(i,j,obs.coeffRef(0), obs.coeffRef(1));// check if obstacle is interesting (e.g. not far behind the robot)Eigen::Vector2d obs_dir obs-robot_pose_.position();if ( obs_dir.dot(robot_orient) 0 obs_dir.norm() cfg_.obstacles.costmap_obstacles_behind_robot_dist )continue;obstacles_.push_back(ObstaclePtr(new PointObstacle(obs)));}7.10、obstacle_poses_affected
用于函数AddEdgesObstaclesLegacy添加障碍物边约束似乎影响了障碍物会被添加到哪个顶点作为约束项。 if (cfg_-obstacles.obstacle_poses_affected teb_.sizePoses())index teb_.sizePoses() / 2;elseindex teb_.findClosestTrajectoryPose(*(obst-get()));7.11、obstacle_proximity_ratio_max_vel
由于接近静态障碍物而降低速度时用作上限的最大速度的比率。用于AddEdgesVelocityObstacleRatio函数这个函数在参数weight_velocity_obstacle_ratio0时添加到动力学约束中。
if (cfg_-optim.weight_velocity_obstacle_ratio 0)AddEdgesVelocityObstacleRatio();7.12、obstacle_proximity_lower_bound
用于AddEdgesVelocityObstacleRatio函数作用类似于7.11。
7.13、obstacle_proximity_upper_bound
用于AddEdgesVelocityObstacleRatio函数作用类似于7.11。 以上三个参数在同一个函数中被使用且同时需要参数weight_velocity_obstacle_ratio0时才会用到用于添加额外的约束项。
8、Optimization
优化相关参数
8.1、no_inner_iterations
内循环迭代的次数。optimizeTEB里面有两层循环,分别叫外部循环和内部循环。外部循环通过调用TimedElasticBand::autoResize()来根据时间分辨率调整轨迹。内部循环调用optimizeGraph()进行优化。
8.2、no_outer_iterations
外循环迭代的次数。作用同上内循环每次优化完位姿后在外循环中调用TimedElasticBand::autoResize()来根据时间分辨率调整轨迹。对于距离远的点插入一下新的位姿对于距离近的点删一删。
8.3、optimization_activate
g2o优化参数是否激活优化项感觉似乎没啥意义肯定得开着不是。
8.4、optimization_verbose
好像是一个专门用于设置优化器属性的参数
optimizer_-setVerbose(cfg_-optim.optimization_verbose);8.5、penalty_epsilon
这个参数使用的地方还是挺多的在添加障碍物边、动态障碍物边以及速度边的时候都用到了。说是为硬约束近似的惩罚函数添加一个小的安全裕度。不是特别理解后面慢慢研究一下。
8.6、weight_max_vel_x
满足最大允许平移速度的优化权重。在函数AddEdgesVelocity中使用对运动学约束添加权重。
8.7、weight_max_vel_y
作用同上但是只在全向轮机器人上会存在y方向速度所以使用的时候需要约束一下同样作为权重使用。
if (cfg_-robot.max_vel_y 0) // non-holonomic robot
{information(0,0) cfg_-optim.weight_max_vel_x;information(1,1) cfg_-optim.weight_max_vel_theta;information(0,1) 0.0;information(1,0) 0.0;
}
else
{information(0,0) cfg_-optim.weight_max_vel_x;information(1,1) cfg_-optim.weight_max_vel_y;information(2,2) cfg_-optim.weight_max_vel_theta;
}8.8、weight_max_vel_theta
角度方向的权重使用与前两个是一样的。
8.9、weight_acc_lim_x
作用与前三个类似但是使用的地点不同前面是添加速度边的时候的权重这里则是在添加加速度边AddEdgesAcceleration的时候生效
8.10、weight_acc_lim_y
加速度权重y方向
8.11、weight_acc_lim_theta
角速度权重
8.12、weight_kinematics_nh
满足非完整运动学的优化权值用于机器人的约束条件。
if (cfg_-robot.min_turning_radius 0 || cfg_-optim.weight_kinematics_turning_radius 0)AddEdgesKinematicsDiffDrive(); // we have a differential drive robotelseAddEdgesKinematicsCarlike(); // we have a carlike robot since the turning radius is bounded from below.void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
{if (cfg_-optim.weight_kinematics_nh0 cfg_-optim.weight_kinematics_forward_drive0)return; // if weight equals zero skip adding edges!8.13、weight_kinematics_forward_drive
与上面的参数是一样的用法但是这个参数需要在差速轮下才能够使用。
void TebOptimalPlanner::AddEdgesKinematicsDiffDrive()
{if (cfg_-optim.weight_kinematics_nh0 cfg_-optim.weight_kinematics_forward_drive0)return; // if weight equals zero skip adding edges!// create edge for satisfiying kinematic constraintsEigen::Matrixdouble,2,2 information_kinematics;information_kinematics.fill(0.0);information_kinematics(0, 0) cfg_-optim.weight_kinematics_nh;information_kinematics(1, 1) cfg_-optim.weight_kinematics_forward_drive;
8.14、weight_kinematics_forward_drive
与上面的类似但是这个参数是用在carlike机器人上的权重参数
void TebOptimalPlanner::AddEdgesKinematicsCarlike()
{if (cfg_-optim.weight_kinematics_nh0 cfg_-optim.weight_kinematics_turning_radius0)return; // if weight equals zero skip adding edges!// create edge for satisfiying kinematic constraintsEigen::Matrixdouble,2,2 information_kinematics;information_kinematics.fill(0.0);information_kinematics(0, 0) cfg_-optim.weight_kinematics_nh;information_kinematics(1, 1) cfg_-optim.weight_kinematics_turning_radius;8.15、weight_optimaltime
时间约束边的权重。
if (cfg_-optim.weight_optimaltime0) return; // if weight equals zero skip adding edges!Eigen::Matrixdouble,1,1 information;information.fill(cfg_-optim.weight_optimaltime);for (int i0; i teb_.sizeTimeDiffs(); i){EdgeTimeOptimal* timeoptimal_edge new EdgeTimeOptimal;timeoptimal_edge-setVertex(0,teb_.TimeDiffVertex(i));timeoptimal_edge-setInformation(information);timeoptimal_edge-setTebConfig(*cfg_);optimizer_-addEdge(timeoptimal_edge);}8.16、weight_shortest_path
最短路径边的权重
if (cfg_-optim.weight_shortest_path0)return; // if weight equals zero skip adding edges!Eigen::Matrixdouble,1,1 information;information.fill(cfg_-optim.weight_shortest_path);for (int i0; i teb_.sizePoses()-1; i){EdgeShortestPath* shortest_path_edge new EdgeShortestPath;shortest_path_edge-setVertex(0,teb_.PoseVertex(i));shortest_path_edge-setVertex(1,teb_.PoseVertex(i1));shortest_path_edge-setInformation(information);shortest_path_edge-setTebConfig(*cfg_);optimizer_-addEdge(shortest_path_edge);}8.17、weight_obstacle
添加障碍物边的权重在AddEdgesObstacles函数中使用
8.18、weight_inflation
膨胀层的权重与8.18是一起使用的 bool inflated cfg_-obstacles.inflation_dist cfg_-obstacles.min_obstacle_dist;Eigen::Matrixdouble,1,1 information;information.fill(cfg_-optim.weight_obstacle * weight_multiplier);Eigen::Matrixdouble,2,2 information_inflated;information_inflated(0,0) cfg_-optim.weight_obstacle * weight_multiplier;information_inflated(1,1) cfg_-optim.weight_inflation;information_inflated(0,1) information_inflated(1,0) 0;8.19、weight_dynamic_obstacle
动态障碍物权重与8.20联合使用用于添加动态障碍物权重。 Eigen::Matrixdouble,2,2 information;information(0,0) cfg_-optim.weight_dynamic_obstacle * weight_multiplier;information(1,1) cfg_-optim.weight_dynamic_obstacle_inflation;information(0,1) information(1,0) 0;8.20、weight_dynamic_obstacle_inflation
动态障碍物膨胀层权重与8.19联合使用添加动态障碍物权重
8.21、weight_velocity_obstacle_ratio
这个参数有两个地方使用 第一个是用于判断是否添加额外约束
if (cfg_-optim.weight_velocity_obstacle_ratio 0)AddEdgesVelocityObstacleRatio();第二个是函数AddEdgesVelocityObstacleRatio中添加对应额外约束的权重
information(0,0) cfg_-optim.weight_velocity_obstacle_ratio;information(1,1) cfg_-optim.weight_velocity_obstacle_ratio;information(0,1) information(1,0) 0;auto iter_obstacle obstacles_per_vertex_.begin();for (int index 0; index teb_.sizePoses() - 1; index){for (const ObstaclePtr obstacle : (*iter_obstacle)){EdgeVelocityObstacleRatio* edge new EdgeVelocityObstacleRatio;edge-setVertex(0,teb_.PoseVertex(index));edge-setVertex(1,teb_.PoseVertex(index 1));edge-setVertex(2,teb_.TimeDiffVertex(index));edge-setInformation(information);edge-setParameters(*cfg_, robot_model_.get(), obstacle.get());optimizer_-addEdge(edge);}}8.22、weight_viapoint
经过路径点边的权重用于函数AddEdgesViaPoints
8.23、weight_adapt_factor
权重的自适应因子在TebOptimalPlanner::optimizeTEB函数的外循环中每次内循环结束后内循环权重都会乘以自适应因子 //构建图success buildGraph(weight_multiplier);//优化图success optimizeGraph(iterations_innerloop, false);weight_multiplier * cfg_-optim.weight_adapt_factor;8.24、obstacle_cost_exponent
非线性障碍物成本的指数成本linear_cost*障碍物成本导出。设置为1以禁用非线性成本默认值
9、HCPlanning
这边的参数是Homotopy方法的一些配置参数teb中的一些函数存在两种方法其中TebOptimalPlanner类是单一的TEB规划器。使用全局规划器生成的初始轨迹来初始化TEB局部规划器。HomotopyClassPlanner类像是多个TebOptimalPlanner类实例的组合。HomotopyClassPlanner类中也会实例化一个由全局规划器生成的路径作为参考的对象。除此之外它还会使用probabilistic roadmap (PRM) methods在障碍物周边采样一些keypoints将这些keypoints连接起来去除方向没有朝向目标点的连接和与障碍物重叠的连接。这样就形成了一个网络然后将起始点和终点接入到这个网络。
使用Depth First Search深度优先方法搜索所有可行的路径。将这些路径作为参考实例化多个TebOptimalPlanner类的实例。采用多线程并行优化得到多条优化后的路径。将这些路径进行可行性分析选出代价值最小的最优路径。不得不说HomotopyClassPlanner类里的方法是一个鲁棒性和可靠性更高的方法。因为单一的TEB规划 (TEB without homology class exploration)在某些场景会陷入局部最小值可能出现卡死的情况。
具体分析参看《对ROS局部运动规划器Teb的理解》
9.1、enable_multithreading
是否激活多线程并行规划多个轨迹
9.2、max_number_classes
指定允许的替代homotopy类的最大数量限制计算工作量
9.3、max_number_plans_in_current_class
要尝试的与当前最佳轨迹在同一homotopy类中的轨迹的最大数量将其设置为2或更多有助于避免局部极小值。必须小于等于max_number_classes
9.4、selection_cost_hysteresis
指定新的候选轨迹必须具有相对于先前选择的轨迹小于一定范围的轨迹成本才能被选择如果new_costold_cost*因子则选择
9.5、selection_prefer_initial_plan
在初始计划的等价类中指定轨迹的间隔
9.6、selection_obst_cost_scale
仅用于选择“最佳”候选者的障碍成本术语的额外缩放new_obst_cost:obst_cost*因子
9.7、selection_viapoint_cost_scale
通过点成本条款的额外扩展仅用于选择“最佳”候选人。new_viapt_cost:viapt_cost*因子
9.8、selection_alternative_time_cost
如果为true则时间成本将替换为总转换时间。
9.9、selection_dropping_probability
在每个计划周期除了当前“最佳”的TEB之外TEB将以这种概率随机丢弃。防止“专注”于次优替代同源物。
9.10、switching_blocking_period
指定允许切换到新等效类之前需要过期的持续时间以秒为单位
9.11、roadmap_graph_no_samples
如果simple_exploration处于关闭状态则指定为创建路线图而生成的样本数
9.12、roadmap_graph_area_width
指定将在起点和终点之间生成采样的区域的宽度[m]高度等于起点到终点的距离
9.13、roadmap_graph_area_length_scale
矩形区域的长度由起点和终点之间的距离决定。此参数进一步缩放距离使几何中心保持相等
9.14、h_signature_prescaler
为了允许大量的障碍物缩放障碍物的数值。不要选择极低否则无法区分障碍物0.2H1
9.15、h_signature_threshold
如果实部和复部的差值都低于指定的阈值则假设两个h符号相等
9.16、obstacle_heading_threshold
指定障碍物航向和目标航向之间的标准化标量乘积的值以便在探索时考虑它们障碍物
9.17、viapoints_all_candidates
如果为true则不同拓扑的所有轨迹都附加到过孔点集否则仅附加与初始/全局计划共享相同轨迹的轨迹在test_optim_node中没有影响。
9.18、visualize_hc_graph
可视化为探索新的homotopy类而创建的图
10、Recovery
故障恢复下使用的几个参数
10.1、shrink_horizon_backup
允许计划器在自动检测到问题的情况下临时缩小范围50%。teb局部路径规划有时候也会失败。此时需要进入故障恢复丢弃一部分路径这个参数决定了是否丢弃一部分路径如果设置为true会裁减掉50%。 if (cfg_.recovery.shrink_horizon_backup goal_idx (int)transformed_plan.size()-1 // we do not reduce if the goal is already selected (because the orientation might change - can introduce oscillations)(no_infeasible_plans_0 || (current_time - time_last_infeasible_plan_).toSec() cfg_.recovery.shrink_horizon_min_duration )) // keep short horizon for at least a few seconds{ROS_INFO_COND(no_infeasible_plans_1, Activating reduced horizon backup mode for at least %.2f sec (infeasible trajectory detected)., cfg_.recovery.shrink_horizon_min_duration);// Shorten horizon if requested// reduce to 50 percent:int horizon_reduction goal_idx/2;10.2、shrink_horizon_min_duration
这个参数在TebLocalPlannerReconfigure.cfg列表中是没有的但是在函数中被使用与10.1在同一位置。用于判断当前时间与上一次成功规划之间的时间间隔如果时间间隔超过该值就不做继续的处理的说明失败有一段时间了该进入错误模式了。
10.3、oscillation_recovery
跟前两个参数一样都是在恢复模式下使用当该参数设置为true时会进行机器人振荡行为判断。判断机器人是否在某个位置来回振荡运行。即始终在一个小范围内活动且角速度来回反复横跳。
10.4、oscillation_recovery_min_duration
振荡判断的时间参数作用同10.2
11、Divergence Detection
发散性检测相关参数
11.1、divergence_detection_enable
是否使能收敛性判断对teb优化后的位姿进行收敛性判断。判断其与全局路径之间是否收敛。
11.2、divergence_detection_max_chi_squared
收敛性判断依据。
bool TebOptimalPlanner::hasDiverged() const
{// Early returns if divergencedetection is not active// 如果发散检测未激活则提前返回if (!cfg_-recovery.divergence_detection_enable)return false;//这里涉及到一个g2o中的返回函数batchStatistics这里似乎存放了优化后的评价指标auto stats_vector optimizer_-batchStatistics();// No statistics yetif (stats_vector.empty())return false;// Grab the statistics of the final iterationconst auto last_iter_stats stats_vector.back();//最后一个点的评价指标不能超过设定值//chi2函数来获得加权最小二乘误差等这些可以用来判断某条边的误差是否过大。return last_iter_stats.chi2 cfg_-recovery.divergence_detection_max_chi_squared;
}chi2函数来获得加权最小二乘误差等这些可以用来判断某条边的误差是否过大。
11.3、enable_homotopy_class_planning
是否激活homotopy类规划算法。如果为true算法会使用homotopy方法进行路径优化false则使用TebOptimalPlanner类。 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.);}teb参数列表一共97个参数至此应该就差不多了。HCPlanning相关参数由于那块代码还没细看后面代码看完了再补充一下。
