Keyboard shortcuts

Press or to navigate between chapters

Press S or / to search in the book

Press ? to show this help

Press Esc to hide this help

4.3 运动控制算法

PID控制器实现

PID(比例-积分-微分)控制器是工业控制中最常用的算法,PID控制器用于精确控制:

# car_cv.py - PID控制器
from simple_pid import PID

class CarCV:
    def __init__(self):
        # 添加PID控制器
        self.pid_distance = PID(Kp=1.0, Ki=0.1, Kd=0.05, setpoint=1)
        self.pid_distance.output_limits = (1.0, 5.0)  # 限制输出范围
        
        # 速度平滑参数
        self.current_speed = 0.0
        self.max_acceleration = 0.5  # 最大加速度
        self.max_speed = 25  # 最大速度
        self.min_speed = 6   # 最小速度
    
    def handle_target_found(self, x, y, ratio, current_time, node):
        """处理发现网球的情况"""
        # 使用PID控制器计算速度
        if ratio_proportion < 0.6:  # 目标比率阈值
            # 使用PID控制器计算速度调整量
            pid_output = self.pid_distance(ratio_proportion)
            # 使用非线性映射
            speed_factor = ((0.6 - ratio_proportion) / 0.6) ** 0.7
            # 基础速度 + 非线性调整的速度
            speed = self.min_speed + (self.max_speed - self.min_speed) * speed_factor * pid_output
            
            # 应用速度平滑处理
            time_delta = current_time - self.last_speed_update_time
            self.last_speed_update_time = current_time
            
            # 限制速度变化率
            max_speed_change = self.max_acceleration * time_delta
            speed_diff = speed - self.current_speed
            if abs(speed_diff) > max_speed_change:
                speed = self.current_speed + max_speed_change * (1 if speed_diff > 0 else -1)
            
            self.current_speed = speed
        
        # 确保速度在合理范围内
        speed = max(self.min_speed, min(speed, self.max_speed))
        
        # 根据位置偏移控制移动
        if abs(x_offset) > 50:  # 水平偏移较大
            if x_offset > 0:
                return turn_left(node, 8)  # 左转
            else:
                return turn_right(node, 8) # 右转
        elif ratio_proportion > 0.95:  # 非常接近目标
            return stop(node)  # 停止
        elif y_offset > 10:  # 目标在下方
            return back(node, int(speed))
        elif y_offset < -10:  # 目标在上方
            return advance(node, int(speed))
        else:
            return stop(node)