ROS中的Tracking PID算法

在ROS的源码中给出了Tracking PID的控制算法，该算法可以用来精确的动态的跟踪轨迹。

在 nav_msgs/Path 中可以动态的插入一个目标点，然后进行速度调节，用来达到预期，以此循环这样的多个节点。

Tracking PID 包含了横向x，纵向y方向正前方，和角速度三个闭环。（在我们的差速机器车中没有横向的PID控制。）

源码在 

https://github.com/nobleo/tracking_pid/blob/master/src/controller.cpp

通过计算误差和pid结合

// calculate the control effort
  proportional_long = Kp_long * filtered_error_long.at(0);
  integral_long = Ki_long * error_integral_long;
  derivative_long = Kd_long * filtered_error_deriv_long.at(0);

  proportional_lat = Kp_lat * filtered_error_lat.at(0);
  integral_lat = Ki_lat * error_integral_lat;
  derivative_lat = Kd_lat * filtered_error_deriv_lat.at(0);

  proportional_ang = Kp_ang * filtered_error_ang.at(0);
  integral_ang = Ki_ang * error_integral_ang;
  derivative_ang = Kd_ang * filtered_error_deriv_ang.at(0);

  // Populate debug output
  pid_debug->proportional.linear.x = proportional_long;
  pid_debug->proportional.linear.y = proportional_lat;
  pid_debug->proportional.angular.z = proportional_ang;

  pid_debug->integral.linear.x = integral_long;
  pid_debug->integral.linear.y = integral_lat;
  pid_debug->integral.angular.z = integral_ang;

  pid_debug->derivative.linear.x = derivative_long;
  pid_debug->derivative.linear.y = derivative_lat;
  pid_debug->derivative.angular.z = derivative_ang;

和上一次的进行“和”

  // Controller logic && overall control effort
  control_effort_long = 0;
  control_effort_lat = 0;
  control_effort_ang = 0;
  if (feedback_long_enabled)
    control_effort_long = control_effort_long + proportional_long + integral_long + derivative_long;
  if (feedforward_long_enabled)
    control_effort_long = control_effort_long + feedforward_long;
  if (feedback_lat_enabled)
    control_effort_lat = control_effort_lat + proportional_lat + integral_lat + derivative_lat;
  if (feedforward_lat_enabled)
    control_effort_lat = control_effort_lat + feedforward_lat;
  if (feedback_ang_enabled)
    control_effort_ang = control_effort_ang + proportional_ang + integral_ang + derivative_ang;
  if (feedforward_ang_enabled)
    control_effort_ang = control_effort_ang + feedforward_ang;

引入最大的限制判断

 // Apply saturation limits
  if (control_effort_long > upper_limit)
    control_effort_long = upper_limit;
  else if (control_effort_long < lower_limit)
    control_effort_long = lower_limit;

  if (control_effort_lat > upper_limit)
    control_effort_lat = upper_limit;
  else if (control_effort_lat < lower_limit)
    control_effort_lat = lower_limit;

  if (control_effort_ang > ang_upper_limit)
    control_effort_ang = ang_upper_limit;
  else if (control_effort_ang < ang_lower_limit)
    control_effort_ang = ang_lower_limit;

输出时判断是否是全向机器人

 // Generate twist message
  if (holonomic_robot_enable)
  {
    output_combined.linear.x = scale_long_control * control_effort_long;
    output_combined.linear.y = control_effort_lat;
    output_combined.linear.z = 0;
    output_combined.angular.x = 0;
    output_combined.angular.y = 0;
    output_combined.angular.z = control_effort_ang;
  }
  else
  {
    output_combined.linear.x = scale_long_control * control_effort_long;
    output_combined.linear.y = 0;
    output_combined.linear.z = 0;
    output_combined.angular.x = 0;
    output_combined.angular.y = 0;
    output_combined.angular.z =
      copysign(1.0, l) * control_effort_ang;  // Take the sign of l for the lateral control effort
  }

开发者增加了error_  和 error_integral_    filtered_error_    filtered_error_deriv_ 几个定义对他们进行的pid.

说白了也是对 x y上的距离和角度求PID ，所以不管是什么erro 误差都可以按照自己的方式去计算误差来求PID了