沉默杀手HomeArchivesTagsCategories
(十五) 输出速度命令
|路径规划TEB算法|
Word count: 588|Reading time: 3 min

调用是这样的：

1
2
3
4
5
6
7
8
9
10
11
12
// Get the velocity command for this sampling interval
if (!planner_->getVelocityCommand(cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.angular.z, 
		cfg_.trajectory.control_look_ahead_poses,  robot_vel_.linear.x, 
		robot_vel_.angular.z)  )
{
	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;
	return false;
}

getVelocityCommand

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
bool TebOptimalPlanner::getVelocityCommand(double& vx, double& vy, double& omega, 
          int look_ahead_poses, double current_vx, double current_vtheta) const
{
  // TimedElasticBand  teb_; //!< Actual trajectory object
  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;
  }
  // teb_.sizePoses() 在规划中一直很大，除非是接近终点时候
  look_ahead_poses = std::max(1, std::min(look_ahead_poses, teb_.sizePoses() - 1));
  double dt = 0.0;
  /*  double& TimeDiff(int index)
	{
		ROS_ASSERT(index<sizeTimeDiffs()); 
		return timediff_vec_.at(index)->dt();
	} */
  ROS_INFO("\033[45;37m  before  counter, look_ahead_poses: %f  \033[0m", look_ahead_poses);
  for(int counter = 0; counter < look_ahead_poses; ++counter)
  {
		dt += teb_.TimeDiff(counter);
		// TODO: change to look-ahead time? Refine trajectory ?
		if(dt >= cfg_->trajectory.dt_ref * look_ahead_poses)  
		{
		    look_ahead_poses = counter + 1;
		    break;
		}
  }
  if (dt<=0)
  {	
    ROS_ERROR("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);

  double v_new = vx;
  double k = (v_new - current_vx) * (v_new - current_vx) / 
  (  (v_new - current_vx) * (v_new - current_vx)
  	+ (cfg_->robot.acc_lim_x * dt) * (cfg_->robot.acc_lim_x * dt)  );
  vx = current_vx + k * (v_new - current_vx);
  return true;
}

参数control_look_ahead_poses增大，会让look_ahead_poses变量增大，dt也会增大
间接导致最终的look_ahead_poses也比较大。如果参数dt_ref也增大，那此二者就更大了。

这里涉及到的公式太复杂了，实在看不出为什么增大 control_look_ahead_poses 会让速度平滑了

extractVelocity

提取机器人的速度在本体坐标系下的表示

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
// pose2 是 teb_.Pose(look_ahead_poses)
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);
    // 无后退的情况下，sign函数都返回 1
    vx = (double) g2o::sign(dir) * deltaS.norm() / dt;
    vy = 0;
  }
  else  // holonomic robot   ...... 省略

   // rotational velocity
  double orientdiff = g2o::normalize_theta(pose2.theta() - pose1.theta());
  omega = orientdiff / dt;
}

前面两个参数的增大导致deltaS.norm()和dt都增大，从这里直接看不出vx是增大还是减小了，实际运行后，通过日志可以看到vx整体增大了。


1
2
3
4
5
6
inline int sign(T x)
{
  if (x > 0)        return 1;
  else if (x < 0)   return -1;
  else   return 0;
}