PID控制器是工业控制中最广泛使用的反馈控制器。它根据期望值与实际值的偏差，通过比例(P)、积分(I)、微分(D)三个环节计算控制量。
#!/usr/bin/env python3
# pid_controller.py - 通用ROS2 PID控制器
# ✅ Docker验证通过
import rclpy
from rclpy.node import Node
from std_msgs.msg import Float64
from geometry_msgs.msg import Twist
class PIDController(Node):
def __init__(self):
super().__init__('pid_controller')
# PID参数
self.declare_parameter('Kp', 1.0)
self.declare_parameter('Ki', 0.0)
self.declare_parameter('Kd', 0.0)
self.declare_parameter('setpoint', 0.0)
self.declare_parameter('output_min', -1.0)
self.declare_parameter('output_max', 1.0)
self.declare_parameter('update_rate', 50.0)
self.declare_parameter('anti_windup', True)
self.declare_parameter('integral_limit', 1.0)
self.declare_parameter('deadband', 0.01)
# 获取参数
self.Kp = self.get_parameter('Kp').value
self.Ki = self.get_parameter('Ki').value
self.Kd = self.get_parameter('Kd').value
self.setpoint = self.get_parameter('setpoint').value
self.out_min = self.get_parameter('output_min').value
self.out_max = self.get_parameter('output_max').value
self.anti_windup = self.get_parameter('anti_windup').value
self.int_limit = self.get_parameter('integral_limit').value
self.deadband = self.get_parameter('deadband').value
# PID状态
self.integral = 0.0
self.prev_error = 0.0
self.last_time = None
self.current_value = 0.0
# 订阅
self.setpoint_sub = self.create_subscription(
Float64, '/setpoint', self._setpoint_cb, 10)
self.feedback_sub = self.create_subscription(
Float64, '/feedback', self._feedback_cb, 10)
# 发布
self.output_pub = self.create_publisher(Float64, '/control_output', 10)
self.error_pub = self.create_publisher(Float64, '/pid_error', 10)
# 定时更新
rate = self.get_parameter('update_rate').value
self.timer = self.create_timer(1.0/rate, self._update)
self.get_logger().info(
f'🎛️ PID控制器启动 - Kp={self.Kp}, Ki={self.Ki}, Kd={self.Kd}')
def _setpoint_cb(self, msg):
self.setpoint = msg.data
def _feedback_cb(self, msg):
self.current_value = msg.data
def _update(self):
now = self.get_clock().now().nanoseconds / 1e9
if self.last_time is None:
self.last_time = now
return
dt = now - self.last_time
self.last_time = now
if dt <= 0: return
# 计算误差
error = self.setpoint - self.current_value
# 死区
if abs(error) < self.deadband:
error = 0.0
# P项
p_term = self.Kp * error
# I项(含抗积分饱和)
self.integral += error * dt
if self.anti_windup:
self.integral = max(-self.int_limit, min(self.int_limit, self.integral))
i_term = self.Ki * self.integral
# D项
derivative = (error - self.prev_error) / dt if dt > 0 else 0.0
d_term = self.Kd * derivative
# 总输出
output = p_term + i_term + d_term
output = max(self.out_min, min(self.out_max, output))
# 如果输出饱和且误差方向一致，停止积分累积
if self.anti_windup and output in (self.out_min, self.out_max):
if (output == self.out_max and error > 0) or \
(output == self.out_min and error < 0):
self.integral -= error * dt # 回退积分
self.prev_error = error
# 发布
out_msg = Float64(); out_msg.data = output
self.output_pub.publish(out_msg)
err_msg = Float64(); err_msg.data = error
self.error_pub.publish(err_msg)
def main(args=None):
rclpy.init(args=args); rclpy.spin(PIDController()); rclpy.shutdown()#!/usr/bin/env python3
# velocity_pid.py - 速度PID控制应用
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import Twist
from std_msgs.msg import Float64
import math
class VelocityPIDApp(Node):
def __init__(self):
super().__init__('velocity_pid_app')
self.declare_parameter('Kp', 0.5)
self.declare_parameter('Ki', 0.1)
self.declare_parameter('Kd', 0.01)
self.declare_parameter('target_speed', 0.3) # m/s
self.Kp = self.get_parameter('Kp').value
self.Ki = self.get_parameter('Ki').value
self.Kd = self.get_parameter('Kd').value
self.target = self.get_parameter('target_speed').value
self.integral = 0.0
self.prev_error = 0.0
self.current_speed = 0.0
self.cmd_sub = self.create_subscription(Twist, '/odom_twist', self._odom_cb, 10)
self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 10)
self.get_logger().info(f'🏎️ 速度PID: 目标={self.target}m/s')
def _odom_cb(self, msg):
self.current_speed = msg.linear.x
error = self.target - self.current_speed
dt = 0.1 # 假设100ms更新
self.integral += error * dt
derivative = (error - self.prev_error) / dt
output = self.Kp * error + self.Ki * self.integral + self.Kd * derivative
output = max(-0.5, min(0.5, output))
cmd = Twist()
cmd.linear.x = output
self.cmd_pub.publish(cmd)
self.prev_error = error
def main(args=None):
rclpy.init(args=args); rclpy.spin(VelocityPIDApp()); rclpy.shutdown()| 控制器类型 | Kp | Ki | Kd |
|---|---|---|---|
| P | 0.5×Ku | 0 | 0 |
| PI | 0.45×Ku | 0.54×Ku/Tu | 0 |
| PID | 0.6×Ku | 1.2×Ku/Tu | 0.075×Ku×Tu |
Ku=临界增益，Tu=临界周期。步骤：1.设Ki=Kd=0 2.增大Kp直到持续振荡 3.记录Ku和振荡周期Tu
使用Ziegler-Nichols方法整定速度PID参数，记录响应曲线。
对比开启/关闭anti_windup时，大阶跃输入的积分饱和现象。
实现位置-速度串级PID：外环位置PID输出作为内环速度PID的目标。
经验值：+250 XP