vfh_algorithm.cc

机器人仿真软件

  Cell_Direction.clear();
  Cell_Base_Mag.clear();
  Cell_Mag.clear();
  Cell_Dist.clear();
  Cell_Enlarge.clear();
  Cell_Sector.clear();

  temp_vec.clear();
  for(x=0;x<WINDOW_DIAMETER;x++) {
    temp_vec.push_back(0);
  }

  temp_vec2.clear();
  temp_vec3.clear();
  for(x=0;x<WINDOW_DIAMETER;x++) {
    temp_vec2.push_back(temp_vec3);
  }

  for(x=0;x<WINDOW_DIAMETER;x++) {
    Cell_Direction.push_back(temp_vec);
    Cell_Base_Mag.push_back(temp_vec);
    Cell_Mag.push_back(temp_vec);
    Cell_Dist.push_back(temp_vec);
    Cell_Enlarge.push_back(temp_vec);
    temp_vec4.push_back(temp_vec2);
  }

  for(x=0;x<NUM_CELL_SECTOR_TABLES;x++)
  {
    Cell_Sector.push_back(temp_vec4);
  }

  Hist = new float[HIST_SIZE];
  Last_Binary_Hist = new float[HIST_SIZE];
  this->SetCurrentMaxSpeed( MAX_SPEED );

  return(1);
}

int VFH_Algorithm::Update_VFH( double laser_ranges[PLAYER_LASER_MAX_SAMPLES][2], 
                               int current_speed, 
                               float goal_direction,
                               float goal_distance,
                               float goal_distance_tolerance,
                               int &chosen_speed, 
                               int &chosen_turnrate ) 
{
  int print = 0;

  this->Desired_Angle = goal_direction;
  this->Dist_To_Goal  = goal_distance;
  this->Goal_Distance_Tolerance = goal_distance_tolerance;

  // 
  // Set current_pos_speed to the maximum of 
  // the set point (last_chosen_speed) and the current actual speed.
  // This ensures conservative behaviour if the set point somehow ramps up beyond
  // the actual speed.
  // Ensure that this speed is positive.
  //
  int current_pos_speed;
  if ( current_speed < 0 )
  {
      current_pos_speed = 0;
  }
  else
  {
      current_pos_speed = current_speed;
  }

  if ( current_pos_speed < last_chosen_speed )
  {
      current_pos_speed = last_chosen_speed;
  }
  // printf("Update_VFH: current_pos_speed = %d\n",current_pos_speed);


  // Work out how much time has elapsed since the last update,
  // so we know how much to increase speed by, given MAX_ACCELERATION.
  timeval now;
  timeval diff;
  double  diffSeconds;
  assert( GlobalTime->GetTime( &now ) == 0 );
  
  TIMESUB( &now, &last_update_time, &diff );
  diffSeconds = diff.tv_sec + ( (double)diff.tv_usec / 1000000 );

  last_update_time.tv_sec = now.tv_sec;
  last_update_time.tv_usec = now.tv_usec;

  if ( Build_Primary_Polar_Histogram(laser_ranges,current_pos_speed) == 0)
  {
      // Something's inside our safety distance: brake hard and
      // turn on the spot
      Picked_Angle = Last_Picked_Angle;
      Max_Speed_For_Picked_Angle = 0;
      Last_Picked_Angle = Picked_Angle;
  }
  else
  {
  if (print) {
    printf("Primary Histogram\n");
    Print_Hist();
  }

  Build_Binary_Polar_Histogram(current_pos_speed);
  if (print) {
    printf("Binary Histogram\n");
    Print_Hist();
  }

  Build_Masked_Polar_Histogram(current_pos_speed);
  if (print) {
    printf("Masked Histogram\n");
    Print_Hist();
  }

  // Sets Picked_Angle, Last_Picked_Angle, and Max_Speed_For_Picked_Angle.
  Select_Direction();

  }

//  printf("Picked Angle: %f\n", Picked_Angle);

  //
  // OK, so now we've chosen a direction.  Time to choose a speed.
  //

  // How much can we change our speed by?
  int speed_incr;
  if ( (diffSeconds > 0.3) || (diffSeconds < 0) )
  {
      // Either this is the first time we've been updated, or something's a bit screwy and
      // update hasn't been called for a while.  Don't want a sudden burst of acceleration,
      // so better to just pick a small value this time, calculate properly next time.
      speed_incr = 10;
  }
  else
  {
      speed_incr = (int) (MAX_ACCELERATION * diffSeconds);
  }

  if ( Cant_Turn_To_Goal() )
  {
      // The goal's too close -- we can't turn tightly enough to get to it,
      // so slow down.
      speed_incr = -speed_incr;
  }

  // Accelerate (if we're not already at Max_Speed_For_Picked_Angle).
  chosen_speed = MIN( last_chosen_speed + speed_incr, Max_Speed_For_Picked_Angle );

  // printf("Max Speed for picked angle: %d\n",Max_Speed_For_Picked_Angle);

  // Set the chosen_turnrate, and possibly modify the chosen_speed
  Set_Motion( chosen_speed, chosen_turnrate, current_pos_speed );

  last_chosen_speed = chosen_speed;

  if (print)
    printf("CHOSEN: SPEED: %d\t TURNRATE: %d\n", chosen_speed, chosen_turnrate);

  return(1);
}

//
// Are we going too fast, such that we'll overshoot before we can turn to the goal?
//
bool VFH_Algorithm::Cant_Turn_To_Goal()
{
    // Calculate this by seeing if the goal is inside the blocked circles
    // (circles we can't enter because we're going too fast).  Radii set
    // by Build_Masked_Polar_Histogram.

    // Coords of goal in local coord system:
    float goal_x = this->Dist_To_Goal * cos( DTOR(this->Desired_Angle) );
    float goal_y = this->Dist_To_Goal * sin( DTOR(this->Desired_Angle) );

// AlexB: Is this useful?
//     if ( goal_y < 0 )
//     {
//         printf("Goal behind\n");
//         return true;
//     }

    // This is the distance between the centre of the goal and
    // the centre of the blocked circle
    float dist_between_centres;

//     printf("Cant_Turn_To_Goal: Dist_To_Goal = %f\n",Dist_To_Goal);
//     printf("Cant_Turn_To_Goal: Angle_To_Goal = %f\n",Desired_Angle);
//     printf("Cant_Turn_To_Goal: Blocked_Circle_Radius = %f\n",Blocked_Circle_Radius);

    // right circle
    dist_between_centres = hypot( goal_x - this->Blocked_Circle_Radius, goal_y );
    if ( dist_between_centres+this->Goal_Distance_Tolerance < this->Blocked_Circle_Radius )
    {
//        printf("Goal close & right\n");
        return true;
    }

    // left circle
    dist_between_centres = hypot( -goal_x - this->Blocked_Circle_Radius, goal_y );
    if ( dist_between_centres+this->Goal_Distance_Tolerance < this->Blocked_Circle_Radius )
    {
//        printf("Goal close & left.\n");
        return true;
    }

    return false;
}

float VFH_Algorithm::Delta_Angle(int a1, int a2) 
{
  return(Delta_Angle((float)a1, (float)a2));
}

float VFH_Algorithm::Delta_Angle(float a1, float a2) 
{
  float diff;

  diff = a2 - a1;

  if (diff > 180) {
    diff -= 360;
  } else if (diff < -180) {
    diff += 360;
  }

  return(diff);
}


int VFH_Algorithm::Bisect_Angle(int angle1, int angle2) 
{
  float a;
  int angle;

  a = Delta_Angle((float)angle1, (float)angle2);

  angle = (int)rint(angle1 + (a / 2.0));
  if (angle < 0) {
    angle += 360;
  } else if (angle >= 360) {
    angle -= 360;
  }

  return(angle);
}


int VFH_Algorithm::Select_Candidate_Angle() 
{
  unsigned int i;
  float weight, min_weight;

  if (Candidate_Angle.size() == 0) 
  {
      // We're hemmed in by obstacles -- nowhere to go, 
      // so brake hard and turn on the spot.
      Picked_Angle = Last_Picked_Angle;
      Max_Speed_For_Picked_Angle = 0;
      Last_Picked_Angle = Picked_Angle;
      return(1);
  }

  Picked_Angle = 90;
  min_weight = 10000000;

  for(i=0;i<Candidate_Angle.size();i++) 
  {
      //printf("CANDIDATE: %f\n", Candidate_Angle[i]);
      weight = U1 * fabs(Delta_Angle(Desired_Angle, Candidate_Angle[i])) +
          U2 * fabs(Delta_Angle(Last_Picked_Angle, Candidate_Angle[i]));
      if (weight < min_weight) 
      {
          min_weight = weight;
          Picked_Angle = Candidate_Angle[i];
          Max_Speed_For_Picked_Angle = Candidate_Speed[i];
      }
  }

  Last_Picked_Angle = Picked_Angle;

  return(1);
}


int VFH_Algorithm::Select_Direction() 
{
  int start, i, left;
  float angle, new_angle;
  std::vector<std::pair<int,int> > border;
  std::pair<int,int> new_border;

  Candidate_Angle.clear();
  Candidate_Speed.clear();

  //
  // set start to sector of first obstacle
  //
  start = -1; 

  // only look at the forward 180deg for first obstacle.
  for(i=0;i<HIST_SIZE/2;i++) 
  {
      if (Hist[i] == 1) 
      {
          start = i;
          break;
      }
  }

  if (start == -1) 
  {
      // No obstacles detected in front of us: full speed towards goal
      Picked_Angle = Desired_Angle;
      Last_Picked_Angle = Picked_Angle;
      Max_Speed_For_Picked_Angle = Current_Max_Speed;

      return(1);
  }

  //
  // Find the left and right borders of each opening
  //

  border.clear();

  //printf("Start: %d\n", start);
  left = 1;
  for(i=start;i<=(start+HIST_SIZE);i++) {
    if ((Hist[i % HIST_SIZE] == 0) && (left)) {
      new_border.first = (i % HIST_SIZE) * SECTOR_ANGLE;
      left = 0;
    }

    if ((Hist[i % HIST_SIZE] == 1) && (!left)) {
      new_border.second = ((i % HIST_SIZE) - 1) * SECTOR_ANGLE;
      if (new_border.second < 0) {
        new_border.second += 360;
      }
      border.push_back(new_border);
      left = 1;
    }
  }

  //
  // Consider each opening
  //
  for(i=0;i<(int)border.size();i++) 
  {
    //printf("BORDER: %f %f\n", border[i].first, border[i].second);
    angle = Delta_Angle(border[i].first, border[i].second);

    if (fabs(angle) < 10) 
    {
        // ignore very narrow openings
        continue;
    }

    if (fabs(angle) < 80) 
    {
        // narrow opening: aim for the centre