(二) costmap converter基本机制

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC,类继承关系:

1
class CostmapToLinesDBSRANSAC : public CostmapToPolygonsDBSMCCH : public BaseCostmapToPolygons

这个机制太复杂了,还没有搞清楚

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
void workerCallback(const ros::TimerEvent&)
{
updateCostmap2D();
compute();
}


void CostmapToPolygonsDBSMCCH::updateCostmap2D()
{
occupied_cells_.clear();
if (!costmap_->getMutex())
{
ROS_ERROR("Cannot update costmap since the mutex pointer is null");
return;
}
int idx = 0;
costmap_2d::Costmap2D::mutex_t::scoped_lock lock(*costmap_->getMutex());

// get indices of obstacle cells
for(std::size_t i = 0; i < costmap_->getSizeInCellsX(); i++)
{
for(std::size_t j = 0; j < costmap_->getSizeInCellsY(); j++)
{
int value = costmap_->getCost(i,j);
if(value >= costmap_2d::LETHAL_OBSTACLE)
{
double x, y;
costmap_->mapToWorld((unsigned int)i, (unsigned int)j, x, y);
occupied_cells_.push_back( KeyPoint( x, y ) );
}
++idx;
}
}
}


(一) TEB中的costmap converter源码部分

TEB默认情况下不使用Costmap Converter。事实上,此插件可以在复杂场景下极大提高运算效率,尤其是处理激光雷达分散的测量数据时,因为将障碍物视为系列孤立点效率极低。

使用costmap_converter转换障碍,结果Rviz没有显示出来,同时报警

首先确认安装sudo apt-get install -y ros-visualization-msgs,一般安装后就行了

初始化

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
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 treated as point obstacles. Error message: %s", ex.what());
costmap_converter_.reset();
}

配置使用的是costmap_converter::CostmapToLinesDBSRANSAC

computeVelocityCommands 函数中的调用

1
2
3
4
if (costmap_converter_)
updateObstacleContainerWithCostmapConverter();
else
updateObstacleContainerWithCostmap();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
void TebLocalPlannerROS::updateObstacleContainerWithCostmapConverter()
{
if (!costmap_converter_)
return;

//Get obstacles from costmap converter
costmap_converter::ObstacleArrayConstPtr obstacles = costmap_converter_->getObstacles();
if (!obstacles)
return;

for (std::size_t i=0; i<obstacles->obstacles.size(); ++i)
{
const costmap_converter::ObstacleMsg* obstacle = &obstacles->obstacles.at(i);
const geometry_msgs::Polygon* polygon = &obstacle->polygon;

if (polygon->points.size()==1 && obstacle->radius > 0) // Circle
{
obstacles_.push_back(ObstaclePtr(new CircularObstacle(polygon->points[0].x, polygon->points[0].y, obstacle->radius)));
}
else if (polygon->points.size()==1) // Point
{
obstacles_.push_back(ObstaclePtr(new PointObstacle(polygon->points[0].x, polygon->points[0].y)));
}
else if (polygon->points.size()==2) // Line
{
obstacles_.push_back(ObstaclePtr(new LineObstacle(polygon->points[0].x, polygon->points[0].y,
polygon->points[1].x, polygon->points[1].y )));
}
else if (polygon->points.size()>2) // Real polygon
{
PolygonObstacle* polyobst = new PolygonObstacle;
for (std::size_t j=0; j<polygon->points.size(); ++j)
{
polyobst->pushBackVertex(polygon->points[j].x, polygon->points[j].y);
}
polyobst->finalizePolygon();
obstacles_.push_back(ObstaclePtr(polyobst));
}

// Set velocity, if obstacle is moving
if(!obstacles_.empty())
obstacles_.back()->setCentroidVelocity(obstacles->obstacles[i].velocities, obstacles->obstacles[i].orientation);
}
}

TEB所有参数含义

局部代价地图更新频率应当不低于planner和costmap_conveter的转换频率

话题

  • global_plan (nav_msgs/Path)

  • local_plan (nav_msgs/Path) 局部路径,主要是可视化目的

  • teb_poses (geometry_msgs/PoseArray) 当前本地计划的离散姿态列表(SE2),主要用于可视化目的, 个数是 teb.sizePoses() ,每个位姿点其实就是teb.Pose(i)

这两个话题本质都是一样的,发布在TebVisualization::publishLocalPlanAndPoses

  • teb_markers (visualization_msgs/Marker) teb_local_planner通过具有不同名称空间的标记来提供规划场景的其他信息。 Namespaces PointObstacles and PolyObstacles: visualize all point and polygon obstacles that are currently considered during optimization. Namespace TebContainer: Visualize all found and optimized trajectories that rest in alternative topologies (only if parallel planning is enabled). Some more information is published such as the optimization footprint model

话题teb_markers_arrayeb_markers是绑定的,不过不必关注。

话题的发布在TebVisualization::publishRobotFootprintModel

  • teb_feedback (teb_local_planner/FeedbackMsg)
    反馈消息包含计划的轨迹,包括速度轮廓和时间信息以及障碍物列表。主要用于评估和调试。必须启用参数publish_feedback

话题的发布在TebVisualization::publishFeedbackMessage

参数

某些参数不会加载到参数服务器,需要用户在yaml里设置,这是源码决定的

速度

  • max_vel_x: 1.0
  • max_vel_x_backwards: 0.5 最大倒车速度。将此速度设置为0或者负数将导致错误。禁止倒车应在penalty部分将前向行驶的权重设置得极高。 参阅优化参数weight_kinematics_forward_drive

  • acc_lim_x: 0.5 最大线加速度,m²/s

  • max_vel_theta: 1 最大转向角速度,作用在车辆高速行驶时,防止高重心转弯过快翻车
  • acc_lim_theta: 0.3 角加速度限制可以避免车轮打滑,车辆失控。对于低速平稳运行的车辆可以不约束此两项,将二者设为一个足够大的值即可

  • max_vel_y: 0.2

  • acc_lim_y: 0.5

以上几个最大值约束会用于saturateVelocity函数

以下参数仅适用于汽车型机器人
  • min_turning_radius: 0.0 最小转弯半径,对于差动机器人为0,代表可以原地转弯。源码在TebLocalPlannerROS::convertTransRotVelToSteeringAngle, 转弯半径应在低速时生效. 把这个值稍微调大一点,可以防止车采用过分的差速转向策略,而导致速度非常缓慢。 转向半径肯定在一米以内,可以按需设置,转向半径如果大了,U形弯就会外道入弯,能比较轻松过弯,不过小弯道的时候也会贴外侧,这样会比较浪费时间,但是如果小了,小弯会贴内,节省时间,但是U形弯就会内侧入弯,那就有可能过不了弯了。
    这个参数让TEB更适合阿克曼模型。为避免规划出不可能实现的移动路径,请动手测量实际车辆的转弯半径。此参数事实上约束了舵机的最大转角。有些车辆转向性能不佳,前轮实际转过角度小于舵机角度,则应当给指令转角乘上一增益后再控制舵机,否则车辆将总是不能实现设置的最小转弯半径。请注意此项应当与最大角速度配合设置。

  • cmd_angle_instead_rotvel: false # not used, is differential 是否将收到的角速度消息转换为操作上的角度变化。设置成True时,话题cmd_vel.angular.z内的数据是舵机角度。

cmd_angle_instead_rotvelmin_turning_radius不能同时为0,否则报警 You are mixing a carlike and a diffdrive robot

  • wheelbase: 默认0.0,目前是1,只有在cmd_angle_instead_rotvel为true时才有效

轮廓

  • footprint_model: 可用的有 point, circular, two_circles, line, polygon. point是最简单的,所需的计算时间最少,但准确度也最低;polygon多边形模型最复杂,完全考虑到机器人的轮廓形状,计算准确度最高。circular类型把机器人建模为一个简单的圆,半径为/footprint_model/radius,计算距离和point相似,不同点是每次调用函数时机器人半径会被加入到参数min_obstacle_dist。对圆形轮廓,可以因此选择point类型,然后将半径加到最小障碍物距离来替换,减小计算量

  • is_footprint_dynamic: 目前False,是否footprint为动态的. If true, 在检查trajectory feasibility之前更新footprint

GoalTolerance

  • xy_goal_tolerance: 0.1
  • yaw_goal_tolerance: 0.05
  • free_goal_vel: False 自由目标速度。设为False时,车辆到达终点时的目标速度为0。缺少目标速度约束将导致车辆“全速冲线”,即使前方有一堵墙也是如此 (因为撞墙的时刻不在规划器考虑范围内了)

Trajectory

  • teb_autosize: True 优化期间允许改变轨迹的时域长度,也就是改变 dt_ref

  • dt_ref: 0.3 局部路径规划的解析度; Temporal resolution of the planned trajectory (usually it is set to the magnitude of the 1/control_rate)。 TEB通过状态搜索树寻找最优路径,而dt_ref则是最优路径上的两个相邻姿态(即位置、速度、航向信息,可通过TEB可视化在rivz中看到)的默认距离。 此距离不固定,规划器自动根据速度大小调整这一距离,速度越大,相邻距离自然越大,较小的值理论上可提供更高精度。
    autoResize函数可以看到,当相邻时间差距离(TimeDiff(i) )和dt_ref的差超过正负dt_hysteresis时,规划器将改变这一距离。 增大到0.4,teb_pose个数大大减少,车速明显加快,多数接近最大速度

  • dt_hysteresis: 0.1 允许改变的时域解析度的浮动范围, 一般为 dt_ref 的 10% 左右

  • min_samples: 3,必须大于2。 源码在initTrajectoryToGoalupdateAndPruneTEB以及autoResize

  • max_samples: 500. 源码在optimizeTEB中的autoResize

  • allow_init_with_backwards_motion: 默认true,设置为false. 只影响初始化,而不是路径优化的结果。 源码在initTrajectoryToGoal,只有使用的TebOptimalPlanner::plan第一个参数是initial_plan才有效

  • force_reinit_new_goal_dist: 1.0 源码在TebOptimalPlanner::plan的热启动阶段,判断局部目标(transformed_plan最后一个元素)与teb_poses最后一个元素之差的范数是否达到阈值,其实是判断局部目标是否太远了。如果小于阈值则调用updateAndPruneTEB

  • global_plan_overwrite_orientation: True 覆盖全局路径中的临时局部路径点的朝向,有些全局规划器不考虑局部子目标的朝向,因此在这里校正。将未来一段的规划的姿态角的平均值作为估计的局部子目标的姿态。源码estimateLocalGoalOrientationinitTrajectoryToGoal

  • global_plan_viapoint_sep: 0.3 # 正数才启用。调整全局路径上选取的航迹点的间隔,应根据机器人的尺寸大小调整.

  • max_global_plan_lookahead_dist: 1.5 最大向前看距离. 指定要进行优化的全局计划子集的最大长度 (累积欧几里得距离) 。然后,实际长度由本地成本图大小和此最大界限的逻辑结合确定。设置为零或负数以停用此限制。 此距离 1.应随车辆最大速度的增大而增大 2.不应超过激光雷达等传感器的可靠测量范围 3. 不应超过局部代价地图的大小,即不能要求TEB对局部耗费地图以外的部分进行规划。

TebLocalPlannerROS::transformGlobalPlan()函数中被使用,决定局部规划初始轨迹的最大长度,实际调试发现此参数无需过大,因为局部轨迹在每个控制周期都被更新,实际执行的指令仅是轨迹上第一个点的速度值,这里设置为1.5即可, 过长也可能导致优化结果无法有效收敛

  • global_plan_prune_distance: 0.6 在TebLocalPlannerROS::pruneGlobalPlan()函数中被使用,因为全局路径是从全局起始点到全局目标点的一条轨迹,而初始的局部路径仅是从机器人当前位置到局部目标点的一小段路径,全局路径裁剪其中一部分即局部路径,该参数决定了从机器人当前位置的后面一定距离开始裁剪

  • force_reinit_new_goal_angular: 0.78 原理同上,不过是角度,弧度制

    1
    2
    3
    4
    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 )
  • feasibility_check_no_poses: 3 在判断生成的轨迹是否冲突时使用,此时设置为3,即从轨迹起点开始逐个检查轨迹上的3个点,若3个点均不发生碰撞,则认为本次轨迹有效。若小于0则检查所有路径点。由于teb优化并非硬约束,这里相当于是轨迹生成之后的一层保障, 这个参数根据机器人的速度和环境复杂程度调整,否则极有可能出现在狭窄环境中走走停停的情况,甚至可以减小到1或0 源码在 isTrajectoryFeasible。 如果isTrajectoryFeasible函数检查的局部路径点太多,有的会超出局部代价地图,结果函数一直返回false

  • min_resolution_collision_check_angular: 默认不加载,需要用户赋值,比如 3.14159265。代价地图的检查碰撞所用的最小角分辨率。如果未达到要求,将增加 intermediate samples. 源码在TebOptimalPlanner::isTrajectoryFeasible

  • control_look_ahead_poses 非默认参数,需要用户赋值,比如 2。 ROS Wiki对这个参数的解释其实是min_resolution_collision_check_angular

  • exact_arc_length: false. 一般用true. 如果为true,则在速度,加速度和转弯速率计算中使用精确的弧长(计算三角函数,会增加cpu负担),否则使用两个位姿点的直线距离做近似。

可视化

  • publish_feedback: false. 发布包含完整轨迹和活动障碍物的反馈,在可视化部分,对应TebOptimalPlanner::visualize()

  • visualize_with_time_as_z_axis_scale: 局部路径的z坐标赋值,源码在publishLocalPlanAndPosesTebVisualization::publishObstacles

恢复措施

恢复措施可以尝试将快要撞到障碍或在空隙间不走的机器人或路径规划错误的机器人恢复至正常状态。TEB实现了由move_base规定的振荡恢复方法。 但是,实测中控制器将高频率振荡速度指令(>10Hz),用户应当评估自己的电动机能否承受。 (可在 move_base 配置中关闭)

  • shrink_horizon_backup: true. 在自动检测到问题(plan not feasible)的情况下,允许planner临时缩小范围(50%). TEB将以更近的点作为规划目标,尝试重新规划出可行路径。调试时可关闭,以在可视化界面上观察原出错路径。另请参阅参数shrink_horizo​​n_min_duration

  • shrink_horizon_min_duration: 10. 如果检测到不可行的轨迹,激活缩小的水平线后备模式,本参数为其最短持续时间。个人怀疑这个参数无意义,因为它更新值的地方也都有no_infeasible_plans_变量的更新,判断时,只根据no_infeasible_plans_即可,详见代码configureBackupModes

  • oscillation_recovery: 默认true. Try to detect and resolve oscillations between multiple solutions in the same equivalence class (robot frequently switches between left/right/forward/backwards

  • oscillation_recovery_min_duration: 10 在这个时间内,是否再次发生FailureDetector检测的振荡,调用在configureBackupModes

在小车遇到空隙不向前走时,容易出现下面的信息,然后小车来回振荡,有时能导航成功

1
2
3
4
5
[ WARN] Clearing costmap to unstuck robot (3.00 m).
[ WARN] TebLocalPlannerROS: trajectory is not feasible. Resetting planner...
[ WARN] Rotate recovery behavior started. # 开始振荡 RotateRecovery::runBehavior()
[ INFO] TebLocalPlannerROS: oscillation recovery disabled/expired.
[ WARN] TebLocalPlannerROS: possible oscillation (of the robot or its local plan) detected. Activating recovery strategy (prefer current turning direction during optimization).

  • oscillation_filter_duration: 10 failure_detector_中buffer容器的大小是oscillation_filter_duration * controller_frequency)

  • oscillation_v_eps: (0,1)内的 normalized 线速度的平均值的阈值,默认0.1

  • oscillation_omega_eps: (0,1)内的 normalized 角速度的平均值,默认0.1

Obstacles

  • min_obstacle_dist: 0.05 最小障碍物距离,用于限制机器人与障碍物的最小距离,默认0.5。若设置了车辆的轮廓,则不建议使用此参数。使用车辆轮廓配置footprint_model配合膨胀距离即可实现较好效果。障碍物惩罚权重很高,TEB不会违背这一最小距离约束,因此会把可以通过的缝隙视作不能通过,并终止运行。
    增大min_obstacle_dist可以防止机器人离墙太近,但是对于狭窄通道的情况,反而应该设置的很小。 可以增大机器人轮廓,但减小min_obs_dist,提高防撞性,又保证过窄通道

实际还配合obstacle_association_force_inclusion_factorobstacle_association_cutoff_factor两个参数生效, 参考TebOptimalPlanner::AddEdgesObstacles()函数

  • obstacle_association_force_inclusion_factor: 10.0 the obstacles that will be taken into account are those closer than min_obstacle_dist * factor, if legacy is false

  • obstacle_association_cutoff_factor: 40.0 the obstacles that are further than min_obstacle_dist * factor will not be taken into account, if legacy is false

距离小于min_obstacle_dist * obstacle_association_force_inclusion_factor值的障碍物点,被强制加入优化框架中,距离大于min_obstacle_dist * obstacle_association_cutoff_factor的障碍物点被直接抛弃不再考虑,然后在剩余的障碍物点中计算机器人左侧最小距离和右侧最小距离。这三个参数的设置非常重要,需要根据机器人的外形尺寸小心调整,否则极易出现狭窄空间机器人无法通过或优化不收敛的情况。

  • inflation_dist: 默认0.0 障碍物膨胀距离。这个值必须大于min_obstacle_dist才有效。源码在AddEdgesObstacles. 此膨胀只是降低通过这些区域的优先级,不应当用此距离迫使车辆远离障碍物。障碍物周边的缓冲区,零惩罚代价,缓冲区会导致规划器减速,源码在AddEdgesObstacles,另外参考weight_inflation

  • include_costmap_obstacles: True 这个不是True就不用干了

  • costmap_obstacles_behind_robot_dist: 1.0 # distance at which obstacles behind the robot are taken into account
  • legacy_obstacle_association: false 源码在buildGraph

  • obstacle_poses_affected: 25 对于每一个障碍物(点、线、多边形),距其最近的poses都会被定位出来。参数obstacle_poses_affected个相邻的最近poses都会被考虑进来。略微影响障碍物周围轨迹的平滑度。 如果legacy_obstacle_association是false,则不启用

costmap_converter

  • costmap_converter_plugin: 可取值为costmap_converter::CostmapToPolygonsDBSMCCH, costmap_converter::CostmapToLinesDBSRANSAC, costmap_converter::CostmapToLinesDBSMCCH, costmap_converter::CostmapToPolygonsDBSConcaveHull, “” 空字符串表示不启用

目前所用为CostmapToLinesDBSRANSAC",头文件costmap_to_lines_ransac.h,最终的基类是BaseCostmapToPolygons

决定是否使用costmap_converter插件,原始costmap中障碍物全部以来表示,计算机器人到障碍物的距离实际需要计算机器人到每一个障碍物点的距离,当环境非常复杂时计算代价会非常大。costmap_converter插件的作用是将障碍物预先表示成线段或多边形的形式,可以在一定程度上减轻后续计算距离的压力,尤其是处理激光雷达分散的测量数据时。因为将障碍物视为系列孤立点效率极低。 “costmap_converter::CostmapToPolygonsDBSMCCH”是一个较为精确的方法,它将环境转换为非凸多边形;在将障碍物距离加入g2o优化框架中 (障碍物距离是目标函数之一,描述为超图的边)

  • costmap_converter_spin_thread: True
  • costmap_converter_rate: 10

Optimization

除了撞击障碍物不被允许外,其余的约束在没有可行方案时可被打破(如规划出事实上不可行的转弯半径)

  • no_inner_iterations: 5 图优化optimizer的迭代次数
  • no_outer_iterations: 4 每次外部循环迭代都会根据所需的时间分辨率dt_ref自动调整轨迹的大小,并调用内部优化器

  • penalty_epsilon 0.1 一次性改变所有的惩罚项,为惩罚函数增加一个小的安全余量,以实现硬约束近似. 例如为速度的约束提供一个缓冲的效果,在到达速度限制前会产生一定的惩罚,让其提前减速达到缓冲的效果
    penalty_epsilon=0.1.png
    penalty_epsilon=0.5.png
    0.5时报警.png



如果选择过高的值,会影响最终的信息矩阵,可能会出现不收敛的情况

  • weight_max_vel_x 2 满足最大允许平移速度的优化权重,还有最大角速度权重,以及加速度,角加速度权重,在整个运动过程中以主要以高速还是低速运行,则是看这些权重的分配
  • weight_max_vel_theta (double,默认值:1.0) 满足最大允许角速度的优化权重
  • weight_acc_lim_x (double,默认值:1.0) 满足最大允许平移加速度的优化权重
  • weight_acc_lim_theta (double,默认值:1.0) 满足最大允许角加速度的优化重量

  • weight_multiplier

  • weight_obstacle 默认值:50.0. 优化权重以保持与障碍物的最小距离。可以增大至几百,让机器人提前转向,避免卡死

  • obstacle_cost_exponent 无默认值,目前设置为0.65. 源码在EdgeObstacle::computeErrorEdgeInflatedObstacle::computeError,判断是否为1,同时min_obstacle_dist > 0才有效。用于更新g2o的误差函数

  • weight_kinematics_nh 默认值:1000. 用于满足非完整运动学的优化权重. 该参数必须很高,因为运动学方程构成了一个等式约束,即使值1000也不意味着由于与其他成本相比较小的“原始”成本值而导致的矩阵条件不佳

  • weight_kinematics_forward_drive 默认值:1,改为1000。 迫使机器人仅选择前进方向 (正向速度) 。较小的权重,仍然会允许向后行驶。我们无法限制优化器的搜索空间为正的速度,因为优化器不支持硬约束,只能大幅提高weight_kinematics_forward_drive以避免后退速度,但是值太大又会减小收敛速度。不管权重多高,无法规划出前进的路径时,还是可能出现停滞或者倒车。

  • weight_optimaltime 最短时间权重。提高至5时,车辆在直道上线速度明显加快,而且越大越能接近max_vel_x,并靠近路径边缘沿切线过弯。 Optimization weight for contracting the trajectory w.r.t transition time

  • weight_inflation 默认0.1, Optimization weight for the inflation penalty,应当很小,源码在AddEdgesObstacles的信息矩阵里

  • weight_dynamic_obstacle Optimization weight for satisfying a minimum separation from dynamic obstacles

  • weight_dynamic_obstacle_inflation Optimization weight for the inflation penalty of dynamic obstacles (should be small)

  • weight_adapt_factor 2. 源码只有optimizeTEB的最后。 每个外部TEB迭代中,一些特殊权重 (当前为weight_obstacle) 会通过此因子重复缩放 (weight_new = weight_old * factor) 。迭代增加权重而不是设置较大的先验值会导致底层优化问题的更好数值条件。

  • optimization_verbose: 默认false. 图优化的过程是否显示,可以用于调试

    1
    2
    iteration= 1	 chi2= 0.458692	 time= 9.3481e-05	 cumTime= 0.000284497	 edges= 49	 schur= 0
    lambda= 5.804560 levenbergIter= 2

并行规划

  • enable_homotopy_class_planning (bool,默认:true) 激活并行规划(占用更多的CPU资源) 若在单一路径上运行,则没有必要时用此功能

2021-06-11_174533.png
当enable_homotopy_class_planning = False,规划的局部路径会陷入局部最小值。因为路径向上面障碍物或下面障碍物偏,总体的代价值都会增加(时间成本),只有往中间走总体代价值才最低。但此时路径invalid,中间缺少路径点,此时机器人卡死了。
2021-06-11_174540.png
设置True,teb会同时搜寻多条路径并选取一条更可行的,feasibility check环节会抛弃不可行的路径

  • enable_multithreading (bool,默认:true) 激活多个线程以计划不同线程中的每个轨迹

  • max_number_classes (int,默认值:4,设置为2) 指定要考虑的最大不同轨迹数(限制计算工作量),影响CPU

  • selection_cost_hysteresis (double,默认值:1.0) 指定新候选人必须具有多少轨迹成本才能选择之前的轨迹 (如果new_cost < old_cost * factor,则进行选择

  • selection_prefer_initial_plan 默认0.95 Specify a cost reduction in the interval (0,1) for the trajectory in the equivalence class of the initial plan

  • selection_obst_cost_scale (double,默认值:100.0) 仅选择“最佳”候选者的障碍物成本术语的额外缩放

  • selection_viapoint_cost_scale (double,默认值:1.0) 仅为了选择“最佳”候选者而对通孔成本条款进行额外的缩放

  • selection_alternative_time_cost (bool,默认值:false) 如果为true,则将时间成本 (时间差平方的总和) 替换为总过渡时间 (时间差之和)

  • simple_exploration 默认false. 如果为true,不同的 trajectories are explored using a simple left-right approach (pass each obstacle on the left or right side) for path generation, otherwise sample possible roadmaps randomly in a specified region between start and goal.

  • roadmap_graph_no_samples (int,默认值:15) 指定为创建roadmap graph而生成的样本数,前提是simple_exploration为false

  • roadmap_graph_area_width (double,默认值:6) 在起点和目标之间的矩形区域中采样随机关键点/航路点。以米为单位指定该区域的宽度

  • delete_detours_backwards 默认true. planner will discard the plans detouring backwards with respect to the best plan

  • h_signature_prescaler (double,默认值:1.0) 标度内部参数 (H-signature) ,用于区分同伦类。警告:仅当您在本地成本图中观察到障碍物过多的问题时,才减小此参数,请勿将其选得太低,否则障碍物无法彼此区分 (0.2 < 值 <= 1)

  • h_signature_threshold (double,默认值:0.1) 如果实部和复杂部的差都低于指定的阈值,则假定两个H签名相等

  • obstacle_heading_threshold (double,默认值:1.0) 在障碍物航向和目标航向之间指定标量乘积的值,以便将障碍物)考虑在内进行探索

  • visualize_hc_graph (bool,默认:false) 如果为true,在rviz里可看到优化使用的graph

  • viapoints_all_candidates (bool,默认:true) 如果为true,则将不同拓扑的所有轨迹附加到该组通孔点,否则,仅将与初始/全局计划共享相同拓扑的轨迹与它们连接 (对test_optim_node无效)

  • switching_blocking_period (double,默认值:0.0) 指定允许切换到新的等效类之前需要终止的持续时间 (以秒为单位)

提高性能的调试

  • 使用Costmap Converter
  • 减小no_outer_iterations no_inner_iterations
  • 减小局部地图大小
  • 增大dt_ref obstacle_poses_affected
  • 关闭多路径并行规划 (效果非常显著)
  • 降低 max_lookahead_distance (一般)

如何提高线速度

增大max_vel_x, dt_ref, weight_max_vel_x。如果还不见效就增大 weight_optimaltime

TEB参数设置如下:

1
2
3
4
5
6
dt_ref: 0.5
max_vel_x: 3.0
max_vel_x_backwards: 0.02
max_vel_y: 0.0
acc_lim_x: 0.8
weight_optimaltime: 30

实际的速度最大到2.7,速度曲线如下:
速度实测.png

加大weight_optimaltime到45,最大速度超过了2.8,但修改dt_ref的效果不大了,可见weight_optimaltime提高最大速度的效果比dt_ref更好

尽量避免后退

要求TEB的速度命令只有前进命令是不可能的,因为优化框架不支持硬约束,轻微违反约束是可能的。 应当显著加大weight_kinematics_forward_drive,但是权重太大又会降低收敛速度(这对任何权重都是如此) 。将 max_vel_x_backwards减少到0(或0.02),至少减少到与参数penalty_epsilon相同。


解读 Costmap2DROS 的构造函数

ROS源码里有个costmap_2d_node.cpp,编译成可执行文件costmap_2d_node,注意它的链接库:

1
2
3
4
5
6
target_link_libraries(costmap_2d_node
costmap_2d
${PCL_LIBRARIES}
${Boost_LIBRARIES}
${catkin_LIBRARIES}
)

它的main函数非常简单,实际就是Costmap2DROS的构造函数
1
2
3
4
5
6
7
8
9
10
11
#include <ros/ros.h>
#include <costmap_2d/costmap_2d_ros.h>

int main(int argc, char** argv)
{
ros::init(argc, argv, "costmap_node");
tf::TransformListener tf(ros::Duration(10));
costmap_2d::Costmap2DROS lcr("costmap", tf);
ros::spin();
return (0);
}

获取参数值和tf变换

首先是一些参数的获取。循环等待直到获得机器人底盘坐标系和global系之间的坐标转换。
并获取rolling_windowtrack_unknown_spacealways_send_full_costmap的参数,默认为false。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
Costmap2DROS::Costmap2DROS(std::string name, tf::TransformListener& tf) :
layered_costmap_(NULL),
name_(name), // 局部还是全局代价地图
tf_(tf),
transform_tolerance_(0.3),
map_update_thread_shutdown_(false),
stop_updates_(false),
initialized_(true),
stopped_(false),
robot_stopped_(false),
map_update_thread_(NULL),
last_publish_(0),
plugin_loader_("costmap_2d", "costmap_2d::Layer"),
publisher_(NULL),
dsrv_(NULL),
footprint_padding_(0.0)
{
//初始化old pose
old_pose_.setIdentity();
old_pose_.setOrigin(tf::Vector3(1e30, 1e30, 1e30));
// 如果是全局代价地图,这里name是global_costmap
ros::NodeHandle private_nh("~/" + name);
ros::NodeHandle g_nh;

//获取tf前缀
ros::NodeHandle prefix_nh;
std::string tf_prefix = tf::getPrefixParam(prefix_nh);

//两个坐标系,全局和局部的参数是不同的
private_nh.param("global_frame", global_frame_, std::string("/map"));
private_nh.param("robot_base_frame", robot_base_frame_, std::string("base_link"));

//确保基于tf前缀正确设置了坐标系
//tf::resolve或者TransformListener::resolve的作用就是使用tf_prefix参数将frame_name解析为frame_id
global_frame_ = tf::resolve(tf_prefix, global_frame_);
robot_base_frame_ = tf::resolve(tf_prefix, robot_base_frame_);

ros::Time last_error = ros::Time::now();
std::string tf_error;

//确保机器人global_frame_和robot_base_frame_之间的有效转换, 否则一直停留在这里阻塞
// 这里已经写死为 0.1,有需要可以改变
while (ros::ok()
&& !tf_.waitForTransform(global_frame_, robot_base_frame_, ros::Time(),
ros::Duration(0.1), ros::Duration(0.01), &tf_error))
{
ros::spinOnce();
// 如果5秒内未成功tf转换,报警
if (last_error + ros::Duration(5.0) < ros::Time::now())
{
ROS_WARN("Timed out waiting for transform from %s to %s to become available before running costmap, tf error: %s",
robot_base_frame_.c_str(), global_frame_.c_str(), tf_error.c_str());
last_error = ros::Time::now();
}
// The error string will accumulate and errors will typically be the same, so the last
// will do for the warning above. Reset the string here to avoid accumulation.
tf_error.clear();
}
//检测是否需要“窗口滚动”,从参数服务器获取以下参数
bool rolling_window, track_unknown_space, always_send_full_costmap;
private_nh.param("rolling_window", rolling_window, false);
private_nh.param("track_unknown_space", track_unknown_space, false);
private_nh.param("always_send_full_costmap", always_send_full_costmap, false);

有时出现了下面的报错

这就是因为上面的0.1太小了

LayeredCostmap类的对象

layered_costmap_ = new LayeredCostmap(global_frame_, rolling_window, track_unknown_space);

接下来创建一个LayeredCostmap类实例layered_costmap_,作为规划器使用的master costmap,并通过它管理各层地图。

LayeredCostmap类是Costmap2DROS的类成员,它是master costmap,也能够管理各层地图,因为它 含有指向各层子地图的指针 std::vector<boost::shared_ptr<Layer> > plugins_,能够调用子地图的类方法,开启子地图的更新,各层子地图最后都会合并到master costmap,提供给规划器的使用。

它的构造函数只有:

1
2
3
4
5
6
7
8
9
10
11
// 如果追踪未知信息的cell,默认false
if (track_unknown)
costmap_.setDefaultValue(255);
else
costmap_.setDefaultValue(0);

// 枚举值的含义对应如下:
/* NO_INFORMATION = 255;
LETHAL_OBSTACLE = 254;
INSCRIBED_INFLATED_OBSTACLE = 253;
FREE_SPACE = 0; */

LayeredCostmap含有类成员Costmap2D costmap_,这个类就是底层地图,用于记录地图数据。

继续Costmap2DROS的构造函数代码:

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
// 查看参数服务器里的costmap是否有plugins参数,如果没有,则重置为旧参数
if (!private_nh.hasParam("plugins"))
{
resetOldParameters(private_nh);
}
// 如果加载了plugins,循环将每个plugin即每层子地图添加进layered_costmap_类的
// 指针组对象中,并对每层地图调用其 initialize 类方法,初始化各层地图。
if (private_nh.hasParam("plugins"))
{
XmlRpc::XmlRpcValue my_list;
private_nh.getParam("plugins", my_list);
for (int32_t i = 0; i < my_list.size(); ++i)
{
std::string pname = static_cast<std::string>(my_list[i]["name"]);
std::string type = static_cast<std::string>(my_list[i]["type"]);
ROS_INFO("Using plugin \"%s\"", pname.c_str());

// plugin_loader_类型是 pluginlib::ClassLoader<Layer>,ROS插件机制
boost::shared_ptr<Layer> plugin = plugin_loader_.createInstance(type);
// 添加到容器 plugins_
layered_costmap_->addPlugin(plugin);
// 执行 Layer::initialize, 它向各层地图通知主地图的存在,并调用
// oninitialize方法 (Layer类中的虚函数,被定义在各层地图中)
plugin->initialize(layered_costmap_, name + "/" + pname, &tf_);
}
}

这里得到的插件即为各层子地图。每层子地图使用Pluginlib(ROS插件机制)来实例化,各个层可以被独立的编译,且允许使用C++接口对Costmap做出任意的改变。

footprint话题

订阅footprint话题,回调函数为setUnpaddedRobotFootprintPolygon。当话题上收到footprint时,回调函数会将接收到的footprint根据参数footprint_padding_的值进行膨胀,得到“膨胀”后的padded_footprint_,传递给各级地图。

查看节点可知,/move_base/local_costmap/footprint/move_base/global_costmap/footprint两个话题的发布订阅者都是move_base

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
std::string topic_param, topic;
if (!private_nh.searchParam("footprint_topic", topic_param))
{
topic_param = "footprint_topic";
}
private_nh.param(topic_param, topic, std::string("footprint"));
footprint_sub_ = private_nh.subscribe(topic, 1, &Costmap2DROS::setUnpaddedRobotFootprintPolygon, this);

if (!private_nh.searchParam("published_footprint_topic", topic_param))
{
topic_param = "published_footprint";
}
private_nh.param(topic_param, topic, std::string("oriented_footprint"));
footprint_pub_ = private_nh.advertise<geometry_msgs::PolygonStamped>("footprint", 1);

setUnpaddedRobotFootprint(makeFootprintFromParams(private_nh));


发布代价地图

创建地图的发布器实例,Costmap2DPublisher类是用于地图发布的封装类,在函数mapUpdateLoop中调用

1
2
3
4
5
6
7
8
9
10
11
12
//发布Costmap2D
publisher_ = new Costmap2DPublisher(&private_nh, layered_costmap_->getCostmap(), global_frame_, "costmap", always_send_full_costmap);

//创建地图更新线程的控制量
stop_updates_ = false;
initialized_ = true;
stopped_ = false;

//创建一个timer去检测机器人是否在移动
robot_stopped_ = false;
//回调函数movementCB通过比较前后两个pose的差来判断机器人是否在移动
timer_ = private_nh.createTimer(ros::Duration(.1), &Costmap2DROS::movementCB, this);

开启动态参数配置,它的回调函数Costmap2DROS::reconfigureCB会在节点启动时运行一次,加载.cfg文件的配置参数。这个函数给对应参数赋值,并创建了一个Costmap2DROS::mapUpdateLoop函数的线程,即地图更新线程。

1
2
3
4
5
6
//开启参数动态配置
dsrv_ = new dynamic_reconfigure::Server<Costmap2DConfig>(ros::NodeHandle("~/" + name));
//回调函数reconfigureCB 除了对一些类成员的配置值做赋值以外,还会开启一个更新map的线程
dynamic_reconfigure::Server<Costmap2DConfig>::CallbackType cb = boost::bind(&Costmap2DROS::reconfigureCB, this, _1,_2);
dsrv_->setCallback(cb);
}

这里的代码有点复杂了,mapUpdateLoop函数循环调用UpdateMap函数,更新地图。并以publish_cycle为周期,发布更新后的地图。

UpdateMap首先调用类内getRobotPose函数,通过tf转换,将机器人底盘系的坐标转换到global系,得到机器人的位姿。然后通过layered_costmap_调用LayeredCostmap类的updateMap函数,更新地图。

流程如下
costmap的初始化流程.png

整个costmap初始化,最关键和复杂的就是各层的加载和更新地图


Costmap2D 类

Costmap2DROS, Costmap2D, LayeredCostmap三个类都属于命名空间costmap_2d,互相没有继承关系,而且都是独立的类

Costmap2D 类中实现了用来存储和访问2D代价地图的的基本数据结构. 它的定义在LayeredCostmap类的成员列表初始化里面,cell默认值default_value_的赋值在LayerdCostmap类的构造函数里。

三个类的关系图

地图图像原点在左下角,缺点是会增加理解难度,好处是在生成图像时,y可以 +1 增,符合真实世界中y向北表示增加。要画出图像时,须要上下颠倒,y往往要换成(map.info.height - y - 1)

代价地图的坐标系和索引

A 2D costmap class provides a mapping between points in the world and their associated "costs"

map坐标系原点在该区域左下角坐标(origin_x_, origin_y_)附近。具体是在(origin_x_ + resolution/2, origin_x_ + resolution/2)

在一次导航过程中,world坐标系只有一个,原点固定。map坐标则随着区域出现而出现。由于一个栅格表示一个resolutin X resolution面积的真实区域,栅格坐标对应的world坐标是在该栅格的中心点坐标。


看全局代价地图的各个参数:

1
2
3
4
5
6
7
8
9
int nx = global_costmap->getSizeInCellsX();   //2200
int ny = global_costmap->getSizeInCellsY(); //1600
double global_resolution = global_costmap->getResolution(); // 0.025

double origin_x = global_costmap->getOriginX(); // -1
double origin_y = global_costmap->getOriginY(); // -34.6

double sizeMeterX = global_costmap->getSizeInMetersX(); // 109.975
double sizeMeterY = global_costmap->getSizeInMetersY(); // 79.975

nx, ny就是pgm地图文件的像素尺寸,一共nx X ny个cell

global_resolution, origin_x, origin_y就是map.yaml中的resolution和origin参数

sizeMeterXsizeMeterY 在rviz上用Measure工具测量一下地图的长宽即可得到


局部代价地图,yaml文件的参数是这样设置的:

1
2
3
width: 4.0
height: 4.0
resolution: 0.05

nx, ny都是80,也就是4/0.05, 80x80就是6400个cell

origin_x: 58, origin_y: 1.5 这里机器人的初值位置为(60, 3.5),位于动态窗口的中心,计算就能得到这两个参数

sizeMeterX: 3.975000, sizeMeterY: 3.975000 其实就是4

这里会发现在全局代价地图的yaml里,没有设置width和height,因为它们是地图文件决定的,无需我们设置。而局部代价地图的动态窗口大小是我们决定的。

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
// 就是为 costmap_ 分配内存
void Costmap2D::initMaps(unsigned int size_x, unsigned int size_y)
{
boost::unique_lock<mutex_t> lock(*access_);
delete[] costmap_;
costmap_ = new unsigned char[size_x * size_y];
}

void Costmap2D::resizeMap(unsigned int size_x, unsigned int size_y,
double resolution,
double origin_x, double origin_y)
{
size_x_ = size_x;
size_y_ = size_y;
resolution_ = resolution;
origin_x_ = origin_x;
origin_y_ = origin_y;

initMaps(size_x, size_y);
// reset our maps to have no information
resetMaps();
}

// 将 costmap_ 赋值为默认值
void Costmap2D::resetMaps()
{
boost::unique_lock<mutex_t> lock(*access_);
memset(costmap_, default_value_, size_x_ * size_y_ * sizeof(unsigned char));
}

void Costmap2D::resetMap(unsigned int x0, unsigned int y0, unsigned int xn, unsigned int yn)
{
boost::unique_lock<mutex_t> lock(*(access_));
unsigned int len = xn - x0;
for (unsigned int y = y0 * size_x_ + x0; y < yn * size_x_ + x0; y += size_x_)
memset(costmap_ + y, default_value_, len * sizeof(unsigned char));
}
1
2
3
4
5
6
7
8
9
10
11
12
// 根据像素坐标,获得 costmap_ 数组的索引
inline unsigned int getIndex(unsigned int mx, unsigned int my) const
{
return my * size_x_ + mx;
}

// 根据 costmap_ 数组的索引,获得像素坐标
inline void indexToCells(unsigned int index, unsigned int& mx, unsigned int& my) const
{
my = index / size_x_;
mx = index - (my * size_x_);
}
  • void Costmap2D::deleteMaps(): 释放 costmap_

  • bool saveMap(std::string file_name);

1
2
3
4
5
6
// 输入世界距离(世界坐标的距离,将其转换为单元格),输出均等距离
unsigned int Costmap2D::cellDistance(double world_dist)
{
double cells_dist = max(0.0, ceil(world_dist / resolution_));
return (unsigned int)cells_dist;
}

代价地图保存为pgm格式。全局代价地图保存是这样的
saveMap.png
这里保存的地图图片,其实和OpenCV保存的一样。相当于真实的costmap先逆时针转180°,再对y轴取对称。但是像素值是相反的。局部代价地图打开是一片黑,应该是只有个窗口,看不出来了。

  • void mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
1
2
3
4
5
void Costmap2D::mapToWorld(unsigned int mx, unsigned int my, double& wx, double& wy) const
{
wx = origin_x_ + (mx + 0.5) * resolution_;
wy = origin_y_ + (my + 0.5) * resolution_;
}

把某个代价地图里的坐标转为世界坐标,常用于把机器人全局代价地图的坐标转为map坐标系的坐标。 在TebLocalPlannerROS::updateObstacleContainerWithCostmap()调用过: costmap_->mapToWorld(0, 0, wx, wy);

  • void worldToMap(double robot_x, double robot_y, int mx, int my) const
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// Convert from world coordinates to map coordinates
bool Costmap2D::worldToMap(double wx, double wy, unsigned int& mx, unsigned int& my) const
{
// 这里有时会出现,为什么 ???
if (wx < origin_x_ || wy < origin_y_)
return false;

mx = (int) ((wx - origin_x_) / resolution_);
my = (int) ((wy - origin_y_) / resolution_);
// 出了代价地图的范围
if (mx < size_x_ && my < size_y_)
return true;

return false;
}

把世界坐标转为某个代价地图里的坐标,最常用的是把机器人在map坐标系的坐标转为全局代价地图里的坐标,注意是转为像素坐标,不是base_link或常说的坐标系。 像素坐标系的原点是左下方的点 ,所以是跟size_x_size_y_比较,这两个是地图的长宽,这样才能判断点是否出了代价地图的范围

1
2
3
4
5
tf::Stamped<tf::Pose> robot_pose;
costmap_ros_->getRobotPose(robot_pose);
robot_pose_ = PoseSE2(robot_pose);
unsigned int mx, my;
costmap_ros_->getCostmap()->worldToMap(robot_pose->x(), robot_pose->y(), mx, my);

如果是局部代价地图,窗口边长4,分辨率0.05,那么窗口的像素边长就是80,这里的机器人的坐标 mx my 就在(40, 40)左右,因为它总在局部代价地图中心

1
2
3
4
5
if (!costmap_->worldToMap(x, y, mx, my)) {
ROS_FATAL("The dimensions of costmap is too small to fully include robot's footprint");
return;
}
unsigned char cost = costmap_->getCost(mx, my);

如果worldToMap返回false,那么是mx或my太大了,超出了代价地图的大小。如果不return,继续执行getCost会导致程序退出。还可以return -1,因为代价值是0~255 有时返回false是因为 wx < originx || wy < originy 不明白原因

map坐标系坐标单元是栅格,只有整数才有意义。一般来说,只有在区域内的才有意义,即x范围是[0, size_x_-1],y范围是[0, size_y_-1]。 这就涉及到worldToMapEnforceBoundsworldToMapNoBounds


  • unsigned char getCost (unsigned int mx, unsigned int my) const    获得costmap中某个栅格的代价值。 mx, my是cell的坐标

  • void setCost(unsigned int mx, unsigned int my, unsigned char cost)

设置代价地图中某个cell的代价值,在层的代码中使用很多

  • void setDefaultValue(unsigned char c)

语句只有default_value_ = c;,调用的地方只有LayeredCostmap构造函数

1
2
3
4
if (track_unknown)  // 默认false
costmap_.setDefaultValue(255);
else
costmap_.setDefaultValue(0);

  • unsigned char* getCharMap() const;     return A pointer to the underlying unsigned char array storing cost values     获取到这个地图中存储代价地图的指针,通过指针找到地图信息。主要在层的代码和ClearCostmapRecovery类中使用
1
2
3
4
unsigned char* Costmap2D::getCharMap() const
{
return costmap_;
}

获得指定栅格周围的4个或8个栅格

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
/**
* @brief Determine 4-connected neighbourhood of an input cell, checking for map edges
* @param idx input cell index
* @param costmap Reference to map data
* @return neighbour cell indexes
*/
std::vector<unsigned int> nhood4(unsigned int idx,
const costmap_2d::Costmap2D& costmap)
{
// get 4-connected neighbourhood indexes, check for edge of map
std::vector<unsigned int> out;
unsigned int size_x_ = costmap.getSizeInCellsX(),
size_y_ = costmap.getSizeInCellsY();
if (idx > size_x_ * size_y_ - 1)
{
ROS_WARN("Evaluating nhood for offmap point");
return out;
}
if (idx % size_x_ > 0) {
out.push_back(idx - 1);
}
if (idx % size_x_ < size_x_ - 1) {
out.push_back(idx + 1);
}
if (idx >= size_x_) {
out.push_back(idx - size_x_);
}
if (idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx + size_x_);
}
return out;
}

/**
* @brief Determine 8-connected neighbourhood of an input cell, checking for map edges
* @param idx input cell index
* @param costmap Reference to map data
* @return neighbour cell indexes
*/
std::vector<unsigned int> nhood8(unsigned int idx,
const costmap_2d::Costmap2D& costmap)
{
// get 8-connected neighbourhood indexes, check for edge of map
std::vector<unsigned int> out = nhood4(idx, costmap);

unsigned int size_x_ = costmap.getSizeInCellsX(),
size_y_ = costmap.getSizeInCellsY();

if (idx > size_x_ * size_y_ - 1) {
return out;
}

if (idx % size_x_ > 0 && idx >= size_x_) {
out.push_back(idx - 1 - size_x_);
}
if (idx % size_x_ > 0 && idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx - 1 + size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx >= size_x_) {
out.push_back(idx + 1 - size_x_);
}
if (idx % size_x_ < size_x_ - 1 && idx < size_x_ * (size_y_ - 1)) {
out.push_back(idx + 1 + size_x_);
}
return out;
}

扩展建图 (二)
abstract Welcome to my blog, enter password to read.
Read more
rosdep 命令

rqt_dep用于查看功能包的依赖关系图。

编译ROS时,有时会报错缺某个包,常常是 could not find *Config.cmake 或者 *-config.cmake,我以前一般的解决方法是:sudo apt-get install ros-kinetic-package_name,其实用rosdep更方便。

安装ROS时,会有一个命令 sudo apt-get install python-rosdep,在安装完成后,还会执行

1
2
sudo rosdep init
rosdep update

src目录下运行rosdep check --from-path package_name,可以查找到所有依赖包。 安装所有依赖包的命令: rosdep install --from-path package_name

rosdep check --from-path src --ignore-src -r -y对整个工作空间检查依赖

最强大的就是rosdep install --from-path src --ignore-src -r -y,在工作空间路径执行,它magically安装了所有 package缺失的依赖项。


pair 和 string

pair

map是将key和value放在一起来保存,pair是更一般化地将2个数据组合成一组数据,另外当一个函数需要返回2个数据的时候,可以选择pair。 pair的实现是一个struct,主要的两个成员变量是first 和 second,因为是使用struct不是class(默认访问权限public),所以可以直接使用这两个成员变量。

头文件 #include <utility>, 类模板:template<class T1,class T2> struct pair。 T1和T2的类型可以不同

1
2
3
4
5
6
7
pair<T1, T2> p1;            //创建一个空的pair对象(使用默认构造),它的两个元素分别是T1和T2类型,采用值初始化。
pair<T1, T2> p1(v1, v2); //创建一个pair对象,它的两个元素分别是T1和T2类型,其中first成员初始化为v1,second成员初始化为v2。
make_pair(v1, v2); // 以v1和v2的值创建一个新的pair对象,其元素类型分别是v1和v2的类型。
p1 < p2; // 两个pair对象间的小于运算,其定义遵循字典次序:如 p1.first < p2.first 或者 !(p2.first < p1.first) && (p1.second < p2.second) 则返回true。
p1 == p2; // 如果两个对象的first和second依次相等,则这两个对象相等;该运算使用元素的==操作符。
p1.first; // 返回对象p1中名为first的公有数据成员
p1.second; // 返回对象p1中名为second的公有数据成员
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
pair<int ,double> p1;
p1.first = 1;
p1.second = 2.5;

pair<int, double> p2;
p2 = make_pair(1, 1.2);



std::pair<std::string, int> getPreson()
{
return std::make_pair("Sven", 25);
}

std::string name;
int ages;
std::tie(name, ages) = getPreson();
std::cout << "name: " << name << ", ages: " << ages << std::endl;

string的find 函数

find()返回值是元素在string中的位置(下标记录),如果没有找到,那么会返回一个特别的标记npos,是一个很大很大的数。返回值可以看成是一个int型的数

1
2
3
4
5
6
7
8
9
10
11
12
13
string s("12345");
string::size_type position;
// 返回34 在 s 中的下标位置
position = s.find("34");
//如果没找到,返回一个特别的标志c++中用npos表示,我这里npos取值是4294967295
if (position != s.npos)
{
printf("position is : %d\n" ,position);
}
else
{
printf("Not found\n");
}

雷达数据的运动畸变

在移动机器人的运动过程中,当激光雷达扫描频率较低时,获得的原始扫描数据会发生畸变,并且随着速度的增加变得越来越严重。雷达旋转有顺时针与逆时针2种情况,但导致运动畸变的效果差异并不明显。

  1. 激光点数据不是瞬时获得的,雷达扫描时在旋转,有个扫描频率的问题
  2. 雷达扫描中,机器人也是运动的。 机器人旋转造成的运动畸变比直线运动大。

比如所用激光雷达的扫描频率为10Hz,当移动机器人以角速度为1 rad/s 进行纯旋转时,在激光雷达扫描一圈得到
一帧数据的时间内(0.1s)移动机器人已经旋转了0.1 rad,约为5.7度,这会影响机器人的定位和路径规划。

由于我以前的机器人是低速机器人,最大线速度才0.4m/s,而雷达的扫描频率至少25Hz,所以雷达不必考虑畸变,实际使用时,从rviz上看,也没发现运动畸变。雷达扫描频率最好达到30Hz以上。但是我现在使用的机器人速度最快1.4m/s,雷达频率才10Hz,这就需要考虑运动畸变了。

使用里程计辅助的方法去除运动畸变。里程计的频率一般比较高,move_base里面默认订阅里程计的频率是20Hz,所以里程计至少也得20Hz,我所用的里程计是50Hz。

平时之所以没有这么严重,是因为有定位程序,比如AMCL或者LOAM

去除激光帧畸变原理:把一帧激光的每个激光点坐标变换到不同时刻的机器人里程计上
里程计辅助方法:imu、轮式里程计
(1) IMU
  直接测量获得角速度和线速度,但对于机器人位移和角度需要积分
  更新速度高 1KHz-8KHz
  具有较高的角速度测量精度,但线加速度精度差
2)轮式里程计
  更新速度高 100-200Hz
  (一般机器人轮式里程计200hz,即可认为满足一定的要求,200hz一帧5ms,1.57rad/s下,误差仅为0.45度,可认为机器人没有运动。)

程序流程

1) main-构造函数
订阅scan话题
调用scancallback处理
2) scancallback
scan话题数据提取到vector数组(包括angles,ranges)
vector数组保存到pcl::PointCloud类型的点云中
调用Lidar_Calibration进行畸变矫正
3) Lidar_Calibration
程序对激光雷达数据进行分段线性插值*
调用Lidar_MotionCalibration激光雷达运动畸变去除分段函数*
调用getLaserPose得到激光雷达在odom中的位姿

参考:
基于里程计修复激光雷达数据
激光雷达移动状态下的数据矫正
激光雷达数据效果对比


马氏距离

马氏距离

数据的协方差距离。它是一种有效的计算两个未知样本集的相似度的方法。与欧氏距离不同的是它考虑到各种特性之间的联系(例如:一条关于身高的信息会带来一条关于体重的信息,因为两者是有关联的)并且是尺度无关的(scale-invariant),即独立于测量尺度。

均方根误差

RMSE = Root Mean Square Error=均方根误差。

含义:衡量观测值与真实值之间的偏差。
用途:常用来作为机器学习模型预测结果衡量的标准
RMSE.png