【树莓派5】实现 电机PID闭环调速 & 舵机控制（代码封装+详细代码+调试

🚀 树莓派5初步使用

---

前言

✅ 适合对树莓派开发感兴趣的初学者

==树莓派5 ==作为一款强大的单板计算机，具备较高的处理能力和丰富的 GPIO 资源，能够高效实现 电机速度闭环控制 和 舵机驱动。相比前代产品，树莓派5的多核性能提升以及更快的 GPIO 访问速度让其在实时控制方面表现更加优秀。本篇博客将基于 树莓派5，使用 Python 和 PID 控制算法 来实现 直流电机的闭环调速 和 舵机角度控制。我们将详细介绍硬件连接、PWM 控制、PID 算法实现以及代码调试，对代码进行封装，为智能小车提供运动控制API。

---

一、硬件清单

组件	型号/规格	用途
主控板	树莓派 5 (Raspberry Pi 5)	处理 PID 调速算法、PWM 生成、数据采集
电机驱动	TB6612FNG	2路GPIO控制电机方向，1路PWM控制转速
直流电机	12V 直流电机（带霍尔编码器）	AB 相霍尔编码器用来计算转速和转向
舵机	数字舵机	控制转向，响应更快、精度更高
电源模块	12V 直流电源 + 降压模块	供电，12V 给电机，5V 给树莓派
其他	杜邦线等	连接与稳定电路

二、在树莓派5使用GPIO和PWM

参考文档：
树莓派5官方文档
gpiozero库的使用参考

1. GPIO 基础介绍

• GPIO（通用输入输出） 是树莓派用于控制外设（如电机、舵机等）的引脚。
• PWM（脉冲宽度调制） 用于控制舵机角度、调节电机转速。

---

2. GPIO 和 PWM 使用

2.1 需要使用的库

在树莓派上操作 GPIO，我们可以选择以下两种库：

1. RPi.GPIO：

   • 适用于低级控制，支持 PWM、GPIO 读取/输出等功能。
   • 需要手动设置引脚模式，并在程序结束时清理资源。

2. gpiozero：

   • 更高级、易用的库，适用于快速实验和简单控制。
   • 适用于 LED、舵机、电机等常见外设。

📌 安装库： 一般烧入系统时都有安装这些常用库，如果是使用虚拟环境可以pip install 库名安装

---

2.2 GPIO 控制

GPIO 可用于 高/低电平信号输出，例如控制 LED 或继电器。

示例：用 RPi.GPIO 让 GPIO18 输出高低电平

import RPi.GPIO as GPIO
import time

GPIO.setmode(GPIO.BCM)  # 使用 BCM 引脚编号
GPIO.setup(18, GPIO.OUT)  # 设置 GPIO18 为输出模式

try:
    while True:
        GPIO.output(18, GPIO.HIGH)  # 输出高电平
        time.sleep(1)
        GPIO.output(18, GPIO.LOW)   # 输出低电平
        time.sleep(1)
except KeyboardInterrupt:
    GPIO.cleanup()  # 清理 GPIO

---

2.3 PWM 控制

PWM（脉冲宽度调制）用于 调速电机、控制舵机。

（1）RPi.GPIO 控制 PWM

import RPi.GPIO as GPIO
import time

PWM_PIN = 18  # 选择 GPIO18 作为 PWM 输出
GPIO.setmode(GPIO.BCM)
GPIO.setup(PWM_PIN, GPIO.OUT)

pwm = GPIO.PWM(PWM_PIN, 50)  # 设置 PWM 频率 50Hz（适用于舵机）
pwm.start(0)  # 初始占空比 0%

try:
    while True:
        for duty in range(0, 101, 10):  # 0% ~ 100%
            pwm.ChangeDutyCycle(duty)
            time.sleep(0.5)
except KeyboardInterrupt:
    pwm.stop()
    GPIO.cleanup()

📌 代码说明：

• pwm.ChangeDutyCycle(duty) 控制占空比（0% ~ 100%），可用于舵机角度或电机转速。

（2）使用 gpiozero 控制 PWM(官方推荐）

from gpiozero import PWMOutputDevice
from time import sleep

pwm = PWMOutputDevice(18, frequency=100)  # 100Hz 频率

while True:
    pwm.value = 0.5  # 50% 占空比
    sleep(2)
    pwm.value = 0  # 关闭 PWM
    sleep(2)

📌 优势：

• pwm.value = 0 ~ 1 直接设置占空比，代码更简洁。
  适用于简单的电机调速和 LED 亮度控制。

---

三、用gpiozero库控制电机（带AB相霍尔编码器）和数字舵机及封装

1.数字舵机控制

所使用舵机参数：

• 舵机三根线：电源+（红）、电源-（黑/褐）、控制信号线（黄/白）
  电源：需要5V3A以上的功率，【功率】必须满足！
  控制信号：PWM 控制信号50HZ，脉宽0.5~2.5ms 对应 -90°~90°位置
  实际控制脉宽有所偏差

封装好的舵机代码如下：

from gpiozero import AngularServo
from time import sleep

class servoControl:
    def __init__(self, pwm_pin , angle=0):
        """
        初始化舵机控制，设置引脚和角度范围。
        """
        self.steering = AngularServo(
            pin=pwm_pin,
            min_angle=-90,
            max_angle=90,
            min_pulse_width=0.0006,  # 0.5ms
            max_pulse_width=0.0026,  # 2.5ms
            frame_width=0.02        # 50Hz (1/50=0.02s)
        )
        self.steering.angle = 0
        sleep(1)
        self.steering.angle = None  # 停止控制，信号线PWM输出停止

    def set_angle(self, angle):
        """
        设置舵机到指定的角度。
        """
        self.steering.angle = angle
        sleep(2)
        self.steering.angle = None  # 停止控制，信号线PWM输出停止
        
       
# 示例用法
steering_control = servoControl(22)  # 初始化舵机控制对象
steering_control.set_angle(30)  # 设置舵机角度为30°

问题：

• 官方说明gpiozero库控制输出PWM是采用软件模拟的，有抖动，为了防止舵机抖动舵机转到指定角度后停止PWM输出

---

2. 电机机控制和编码器读取

电机驱动采用的是TB6612，一个电机由2路GPIO控制转向和路PWM控制转速。编码器有AB两相，读取脉冲数可以判断转速、两相相位差可以判断转向。

封装好的电机控制和编码器脉冲数读取代码如下：

from gpiozero import Motor, RotaryEncoder, AngularServo,PWMOutputDevice
from time import sleep
import threading

#-------------------
class MotorController:
    def __init__(self, in1_pin, in2_pin, pwm_pin, pwm_frequency=10000):
        # 初始化电机和PWM设备
        self.direction = Motor(forward=in1_pin, backward=in2_pin)  # 电机方向控制引脚
        self.pwm = PWMOutputDevice(pwm_pin, frequency=pwm_frequency)  # PWM控制引脚
        
    def set_speed(self, speed):
        """
        设置电机速度。
        speed: 一个0到1之间的值，代表PWM占空比，控制电机转速。
        """
        self.pwm.value = speed  # 设置PWM占空比
        
    def forward(self):
        """
        让电机正转。
        """
        self.direction.forward()

    def backward(self):
        """
        让电机反转。
        """
        self.direction.backward()
        
    def forwards(self,pwm):
        """
        让电机正转。
        """
        self.direction.forward()
        self.pwm.value = speed  # 设置PWM占空比
        
    def backwards(self,pwm):
        """
        让电机反转。
        """
        self.direction.backward()
        self.pwm.value = speed  # 设置PWM占空比
        
    def stop(self):
        """
        停止电机。
        """
        self.direction.stop()
        self.pwm.off()  # 停止PWM信号
        
#-----------------
class EncoderControl:
    def __init__(self, pin_a, pin_b, max_steps=1000):
        """
        初始化旋转编码器的控制。
        """
        self.encoder = RotaryEncoder(a=pin_a, b=pin_b, max_steps=max_steps)
    
    def get_steps(self):
        """
        获取当前的步数。并清零
        :return: 当前的步数。
        """
        steps=self.encoder.steps
        self.encoder.steps = 0
        
        return steps

---

三、用增量式pid控制实现电机闭环调速

增量式pid相对于位置式pid控制会更加平滑，因此采取增量式pid

增量式离散PID公式 :
pwm+=Kp[e（k）-e(k-1)]+Ki*e(k)+Kd[e(k)-2e(k-1)+e(k-2)]
e(k)代表本次偏差
e(k-1)代表上一次的偏差 以此类推
pwm代表增量输出

我采取的是pi控制，代码如下：

def pid_motor_left(kp,ki,kd,setpoint,current_speed):     #速度pi
    global last_error_left,pwm_left
    
    error=setpoint - current_speed
    pwm_left+=kp*(error-last_error_left)+ki*error
    
    last_error_left=error
    
    limit_pwm(pwm_left,0.5)    #限幅
    
#     return pwm_left

def pid_motor_right(kp,ki,kd,setpoint,current_speed):     #速度pi
    global last_error_right,pwm_right
    
    error=setpoint - current_speed
    pwm_right+=kp*(error-last_error_right)+ki*error
    
    last_error_right=error
    
    limit_pwm(pwm_right,0.5)

调参：

• 增量式pi的 i可以加快响应速度，p可以抑制超调。
• 可以先调i，使该控制器有较快的响应速度，再调p使控制器超调减小
• 在Thonny中，可以打开绘图器，编写符合格式print函数可以绘制出输出曲线，方便调参

完成代码示例如下：

from gpiozero import Motor, RotaryEncoder, AngularServo,PWMOutputDevice
from time import sleep
import threading

#-------------------
class MotorController:
    def __init__(self, in1_pin, in2_pin, pwm_pin, pwm_frequency=10000):
        # 初始化电机和PWM设备
        self.direction = Motor(forward=in1_pin, backward=in2_pin)  # 电机控制引脚
        self.pwm = PWMOutputDevice(pwm_pin, frequency=pwm_frequency)  # PWM控制引脚
        
    def set_speed(self, speed):
        """
        设置电机速度。
        speed: 一个0到1之间的值，代表PWM占空比，控制电机转速。
        """
        self.pwm.value = speed  # 设置PWM占空比
        
    def forward(self):
        """
        让电机正转。
        """
        self.direction.forward()

    def backward(self):
        """
        让电机反转。
        """
        self.direction.backward()
        
    def forwards(self,pwm):
        """
        让电机正转。
        """
        self.direction.forward()
        self.pwm.value = speed  # 设置PWM占空比
        
    def backwards(self,pwm):
        """
        让电机反转。
        """
        self.direction.backward()
        self.pwm.value = speed  # 设置PWM占空比
        
    def stop(self):
        """
        停止电机。
        """
        self.direction.stop()
        self.pwm.off()  # 停止PWM信号
        
#-----------------
class EncoderControl:
    def __init__(self, pin_a, pin_b, max_steps=1000):
        """
        初始化旋转编码器的控制。
        :param pin_a: 编码器的A引脚（默认为17）。
        :param pin_b: 编码器的B引脚（默认为27）。
        :param max_steps: 最大步数（默认为50）。
        """
        self.encoder = RotaryEncoder(a=pin_a, b=pin_b, max_steps=max_steps)
    
    def get_steps(self):
        """
        获取当前的步数。并清零
        :return: 当前的步数。
        """
        steps=self.encoder.steps
        self.encoder.steps = 0
        
        return steps

#------------------
class servoControl:
    def __init__(self, pwm_pin , angle=0):
        """
        初始化舵机控制，设置引脚和角度范围。
        """
        self.steering = AngularServo(
            pin=pwm_pin,
            min_angle=-90,
            max_angle=90,
            min_pulse_width=0.0006,  # 0.5ms
            max_pulse_width=0.0026,  # 2.5ms
            frame_width=0.02        # 50Hz (1/50=0.02s)
        )
        self.steering.angle = 0
        sleep(1)
        self.steering.angle = None  # 停止控制，舵机不再接收新的角度命令

    def set_angle(self, angle):
        """
        设置舵机到指定的角度。
        """
        self.steering.angle = angle
        sleep(1)
        self.steering.angle = None  # 停止控制，舵机不再接收新的角度命令

#--------------------------------------------------------------------------------------------
left_motor_forward_pin=14
left_motor_backward_pin=15
left_motor_pwm_pin=18
right_motor_forward_pin=2
right_motor_backward_pin=3
right_motor_pwm_pin=4

left_encoder_a_pin=24
left_encoder_b_pin=23
right_encoder_a_pin=17
right_encoder_b_pin=27

servo_pwm_pin=22

# --- 初始化电机 ---
left_motor = MotorController(in1_pin=left_motor_forward_pin, in2_pin=left_motor_backward_pin, pwm_pin=left_motor_pwm_pin)
right_motor= MotorController(in1_pin=right_motor_forward_pin, in2_pin=right_motor_backward_pin, pwm_pin=right_motor_pwm_pin)
# --- 初始化编码器 ---
left_encoder = EncoderControl(pin_a=left_encoder_a_pin, pin_b=left_encoder_b_pin)
right_encoder = EncoderControl(pin_a=right_encoder_a_pin, pin_b=right_encoder_b_pin)

# --- 初始化舵机 ---
servo = servoControl(servo_pwm_pin)
  

# --- 全局变量 ---
last_error_left = 0
last_error_right = 0
pwm_left=0
pwm_right=0

target_speed=0

kp_left=0.06
ki_left=0.03
kd_left=0
kp_right=0
ki_right=0.03
kd_right=0

left_speed=0
right_speed=0


def limit_pwm(a,extent):
    if a>extent:
        a=extent
    elif a<-1*extent:
        a=-1*extent
    

def pid_motor_left(kp,ki,kd,setpoint,current_speed):     #速度pi
    global last_error_left,pwm_left
    
    error=setpoint - current_speed
    pwm_left+=kp*(error-last_error_left)+ki*error
    
    last_error_left=error
    
    limit_pwm(pwm_left,0.5)
    
#     return pwm_left

def pid_motor_right(kp,ki,kd,setpoint,current_speed):     #速度pi
    global last_error_right,pwm_right
    
    error=setpoint - current_speed
    pwm_right+=kp*(error-last_error_right)+ki*error
    
    last_error_right=error
    
    limit_pwm(pwm_right,0.5)
    
#     return pwm_right
    

# --- 速度计算与PID更新线程 ---
def speed_control():
    global target_speed,kp_left,ki_left,kd_left,kp_right,ki_right,kd_right,pwm_left,pwm_right,left_speed,right_speed
    while True:
        dt = 0.1  # 控制周期100ms
        sleep(dt)
        
        # 计算当前速度（steps/s）
        left_steps = left_encoder.get_steps()
        right_steps = right_encoder.get_steps()

        left_speed = left_steps / 390/dt       #一相一圈输出390个脉冲
        right_speed = right_steps / 390/dt     #计算每秒转速
       
        
        # PID计算并设置电机速度
         
        pid_motor_left(kp_left,ki_left,kd_left,target_speed,left_speed)
        pid_motor_right(kp_right,ki_right,kd_right,target_speed,right_speed)
        
        if pwm_left>=0:
            left_motor.forward()
            left_motor.set_speed(pwm_left)  # 设置转速为30%
        else:
            left_motor.backward()
            left_motor.set_speed(-1*pwm_left)  # 设置转速为30%
            
        if pwm_right>=0:
            right_motor.forward()
            right_motor.set_speed(pwm_right)  # 设置转速为30%
        else:
            right_motor.backward()
            right_motor.set_speed(-1*pwm_right)  # 设置转速为30%
        

# --- 主程序 ---
if __name__ == "__main__":
    try:
        # 启动速度控制线程
        control_thread = threading.Thread(target=speed_control, daemon=True)
        control_thread.start()
        
        # 设置阿克曼转向角度（示例：60度）
        servo.set_angle(0)  # 设置舵机角度为30°
        
        target_speed=1
        
        # 主循环（可在此添加其他逻辑）
        while True:
#             print('left{:.2f}   right{:.2f}'.format(left_speed,right_speed))
            print('left',left_speed,'right',right_speed)
#             print(right_speed)
            sleep(1)
            
    except KeyboardInterrupt:
        gpiozero.Factory.close()
        
        motorA.close()
        motorB.close()
        encoderA.close()
        encoderB.close()
        steering.close()

---