对pure pursuit算法的理解和改进

1
2
angular_vel = linear_vel * curvature;

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
//会检查三点是否共线
template <typename T>  
CircleData findCircle2(const T & pt1, const T & pt2, const T & pt3)
{
 
	//令：
	//A1 = 2 * pt2.x - 2 * pt1.x      B1 = 2 * pt1.y - 2 * pt2.y       C1 = pt1.y² + pt2.x² - pt1.x² - pt2.y²
	//A2 = 2 * pt3.x - 2 * pt2.x      B2 = 2 * pt2.y - 2 * pt3.y       C2 = pt2.y² + pt3.x² - pt2.x² - pt3.y²
	float A1, A2, B1, B2, C1, C2, temp;
	A1 = pt1.x - pt2.x;
	B1 = pt1.y - pt2.y;
	C1 = (pow(pt1.x, 2) - pow(pt2.x, 2) + pow(pt1.y, 2) - pow(pt2.y, 2)) / 2;
	A2 = pt3.x - pt2.x;
	B2 = pt3.y - pt2.y;
	C2 = (pow(pt3.x, 2) - pow(pt2.x, 2) + pow(pt3.y, 2) - pow(pt2.y, 2)) / 2;
	//为了方便编写程序，令temp = A1*B2 - A2*B1
	temp = A1*B2 - A2*B1;

  // std::cout << "temp = " << temp << std::endl;
	//定义一个圆的数据的结构体对象CD
	CircleData CD;
	//判断三点是否共线
	if (temp == 0){
		//共线则将第一个点pt1作为圆心
		CD.center.x = pt1.x;
		CD.center.y = pt1.y;
    CD.radius = 5.0;
    
    return CD;
	}
	else{
		//不共线则求出圆心：
		//center.x = (C1*B2 - C2*B1) / A1*B2 - A2*B1;
		//center.y = (A1*C2 - A2*C1) / A1*B2 - A2*B1;
		CD.center.x = (C1*B2 - C2*B1) / temp;
		CD.center.y = (A1*C2 - A2*C1) / temp;
	}

	//共线时将第一个点pt1作为圆心，此时计算出来的半径为0
  //计算圆心到其中一个ie点的距离作为半径
	CD.radius = sqrtf((CD.center.x - pt1.x)*(CD.center.x - pt1.x) + (CD.center.y - pt1.y)*(CD.center.y - pt1.y));
	return CD;
}

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
class RateLimiter {
 public:
  RateLimiter() = default;
  ~RateLimiter() = default;

  double limitRateOfChange(double value);
  void setRisingRate(double maxRisingRate);
  void setFallingRate(double minFallingRate);
  void setTimestep(double dt);
  void reset(double startingValue = 0.0);

 private:
  double dt_ = 0.01;
  double maxRisingRate_ = 1.0;
  double minFallingRate_ = -1.0;
  double valuePrev_ = 0.0;
};


double RateLimiter::limitRateOfChange(double value) {
  double retValue = value;
  if (value > valuePrev_ + dt_ * maxRisingRate_) {
    retValue = valuePrev_ + dt_ * maxRisingRate_;
  }

  if (value < valuePrev_ + dt_ * minFallingRate_) {
    retValue = valuePrev_ + dt_ * minFallingRate_;
  }

  valuePrev_ = retValue;

  return retValue;
}
void RateLimiter::setRisingRate(double maxRisingRate) {
  if (maxRisingRate < 0) {
    throw std::runtime_error("Rising rate cannot be negative.");
  }
  maxRisingRate_ = maxRisingRate;
}
void RateLimiter::setFallingRate(double minFallingRate) {
  if (minFallingRate > 0) {
    throw std::runtime_error("Falling rate cannot be positive");
  }
  minFallingRate_ = minFallingRate;
}
void RateLimiter::setTimestep(double dt) {
  if (dt < 0) {
    throw std::runtime_error("Time step cannot be negative");
  }
  dt_ = dt;
}

void RateLimiter::reset(double startingValue) {
  valuePrev_ = startingValue;
}

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
bool AdaptiveVelocityController::computeVelocity() {
  double referenceVelocity = 0.0;
  switch (drivingDirection_) {
    case DrivingDirection::FWD: {
      referenceVelocity = parameters_.desiredVelocity_;
      break;
    }
    case DrivingDirection::BCK: {
      referenceVelocity = -parameters_.desiredVelocity_;
      break;
    }
  }

  // should be path integral strictly speaking
  const double distanceToGoal = (currentRobotState_.pose_.position_ - currentPathSegment_.point_.back().position_).norm();
  const double dWhenBrakingStarts = parameters_.distanceToGoalWhenBrakingStarts_;//距离目标点多远开始刹车
  if (distanceToGoal <= dWhenBrakingStarts) {
    const double slope = parameters_.desiredVelocity_ / dWhenBrakingStarts;
    const double referenceVelocityMagnitude = slope * distanceToGoal;
    const double direction = sgn(referenceVelocity);
    referenceVelocity = direction * referenceVelocityMagnitude;
  }

  desiredLongitudinalVelocity_ = rateLimiter_.limitRateOfChange(referenceVelocity);

  return true;
}