ROS Melodic下Turtlebot2视觉巡线功能的PID控制器实现问题

核心问题说明 #

你原来把PID设计为无状态的纯函数是行不通的：PID的积分项需要累计历史误差、微分项需要用到上一时刻的误差，这些状态需要在两次控制周期之间保留，不能每次调用都清零，建议用类实现带状态的PID控制器。 #

具体实现代码 #

第一步：实现PID控制器类

class PIDController:
    def __init__(self, kp, ki, kd, max_integral=0.5):
        self.kp = kp
        self.ki = ki
        self.kd = kd
        # 积分项最大值，避免积分饱和导致转向过度
        self.max_integral = max_integral
        # 状态存储变量
        self.prev_error = 0.0
        self.integral = 0.0
    
    def compute(self, error, dt):
        # 计算比例项
        p_term = self.kp * error
        # 计算积分项
        self.integral += error * dt
        self.integral = max(min(self.integral, self.max_integral), -self.max_integral)
        i_term = self.ki * self.integral
        # 计算微分项
        d_term = self.kd * (error - self.prev_error) / dt
        # 总输出
        output = p_term + i_term + d_term
        # 更新上一次误差
        self.prev_error = error
        return output

第二步：修改GoForward类逻辑

同时修正你原有代码的几个错误：构造方法__init__是双下划线、dt要和控制频率匹配、变量名拼写错误veclocity改为velocity、增加图像读取失败的容错逻辑。

class GoForward():
    # 注意是双下划线
    def __init__(self):
        rospy.init_node('GoForward', anonymous=False)
        rospy.loginfo("To stop TurtleBot CTRL + C")
        rospy.on_shutdown(self.shutdown)
        self.cmd_vel = rospy.Publisher('cmd_vel_mux/input/navi', Twist, queue_size=10)
        # 初始化PID控制器，参数可后续调试
        self.pid = PIDController(kp=0.15, ki=0.02, kd=0.02)
        # 控制频率10Hz，对应dt=0.1s，和你设置的rospy.Rate(10)匹配
        self.control_rate = 10
        self.dt = 1.0 / self.control_rate
        r = rospy.Rate(self.control_rate)
        move_cmd = Twist()
        base_linear_speed = 0.2
        velocity_control_coeff = 0.5

        while not rospy.is_shutdown():
            frame = cv2.imread("image.jpg", 1)
            # 容错逻辑：图片读取失败跳过本次控制，避免崩溃
            if frame is None:
                rospy.logwarn("无法读取image.jpg，跳过本次控制")
                r.sleep()
                continue
            error = extract(frame)
            # 计算角速度输出
            w = self.pid.compute(error, self.dt)
            # 限制角速度范围，避免转向过猛冲出路径
            w = max(min(w, 1.0), -1.0)
            # 线性速度随转角减速，最低速度保留0.05避免停车
            move_cmd.linear.x = max(base_linear_speed - (velocity_control_coeff * abs(w)), 0.05)
            move_cmd.angular.z = -w
            self.cmd_vel.publish(move_cmd)
            r.sleep()
    
    def shutdown(self):
        rospy.loginfo("Stop TurtleBot")
        self.cmd_vel.publish(Twist())
        rospy.sleep(1)

# 注意是双下划线
if __name__ == '__main__':
    try:
        GoForward()
    except:
        rospy.loginfo("GoForward node terminated.")

参数调试建议 #

1. 先调P参数：把ki、kd先设为0，逐步增大kp，直到机器人能跟随路径但出现小幅左右晃动，再把kp稍微调小一点
2. 再调D参数：缓慢增大kd，直到机器人的晃动被抑制，不要设太大否则机器人会出现高频抖动
3. 最后调I参数：如果发现机器人有静态误差（比如总是偏路径一侧走不直），再缓慢增大ki，同时注意max_integral不要设太大，避免积分饱和导致转向过度

内容的提问来源于stack exchange，提问作者sm29