因为打算用PYB做机器人核心板，那么作为机器人底盘的最基本控制算法PID必然不可或缺。我自己写了一个PID算法的小py文件，可以在主函数中直接进行调用，采用PID算法可以让机器人的行驶速度达到设定的速度，使机器人实现闭环控制。
硬件基础
采用PYB、L298N、编码器电机进行实战验证，接线和编码器电机的使用可以参考我的这篇博客。
Micropython——基于PYB的霍尔编码器电机测速与使用
需要注意的是，在霍尔编码器电机的PID控制中，我们需要用到硬件模块：
- PYB
- L298N 或 TB6612
- 霍尔编码器电机
- 12V电源或可调电压源
电机的接线如下（如果是有两个电机需要进行接线，参考仓库中的接线图）：
在这里需要注意的是：接ENA引脚时，需要将跳线帽拔掉，再将PWM引脚接到靠外的ENA引脚上，不要接到靠里边的引脚上！
PID算法理论基础
这里可以直接参阅我写的这篇关于PID控制理论的博客，读者如果还是觉得不太懂，也可以再找找有其他大佬的关于PID理论的博客进行深入学习。
PID控制的相关代码模块准备
在pid控制前，需要准备好如下部分代码：
- 电机驱动模块
- 编码器读取模块
- PID模块
电机驱动模块
在这里我们只是用一个电机进行控制，因为我做的是一个差速的机器人底盘，所以写的就是两个电机的驱动，这里可以只使用单个的即可，同时你也可以在此基础上自行改动。
motor.py
from pyb import Pin, Timer
import pid
class Motor:
def __init__(self, pwm_range):
# 左轮A， 右轮B
self.io_p_A = Pin("Y5", Pin.OUT_PP) # 电机A的正极IO口
self.io_n_A = Pin("Y6", Pin.OUT_PP)
self.io_p_B = Pin("X11", Pin.OUT_PP) # 电机B的正极IO口
self.io_n_B = Pin("X12", Pin.OUT_PP)
self.io_PWM_A = Pin("Y3", Pin.OUT_PP) # 电机A、B的PWM输入IO口
self.io_PWM_B = Pin("Y4", Pin.OUT_PP)
timer_M = Timer(4, freq=1000) # PWM输出定时器
self.ch_PWM_A = timer_M.channel(3, Timer.PWM, pin=self.io_PWM_A)
self.ch_PWM_B = timer_M.channel(4, Timer.PWM, pin=self.io_PWM_B)
self.pwm_range = pwm_range # PWM范围
def motor_run(self, pwm_A, pwm_B):
'''
函数功能：电机驱动
入口参数：两个电机的PWM值（范围为pwm_range）
'''
self.ch_PWM_A.pulse_width_percent(100*(pwm_A/self.pwm_range)) # 给电机PWM占空比，默认pwm_range为100
self.ch_PWM_B.pulse_width_percent(100*(pwm_B/self.pwm_range))
# print("pwm_A:", 100*(pwm_A / self.pwm_range))
# print("pwm_B:", 100*(pwm_B / self.pwm_range))
def direction_control(self, pwm_A, pwm_B):
'''
函数功能：控制方向，并将PWM值转换为正值
入口参数：两个电机的PWM值（范围为pwm_range）
返回值 ：两个电机的正PWM值
'''
def ahead_m_A():
self.io_p_A.high(); self.io_n_A.low();
# self.io_p_A.low(); self.io_n_A.high();
def back_m_A():
self.io_p_A.low(); self.io_n_A.high();
# self.io_p_A.high(); self.io_n_A.low();
def ahead_m_B():
self.io_p_B.high(); self.io_n_B.low();
# self.io_p_B.low(); self.io_n_B.high();
def back_m_B():
self.io_p_B.low(); self.io_n_B.high();
# self.io_p_B.high(); self.io_n_B.low();
def stop():
self.io_p_A.low(); self.io_n_A.low();
self.io_p_B.low(); self.io_n_B.low();
if (pwm_A > 0 and pwm_B >= 0) or (pwm_A >= 0 and pwm_B > 0): # 向前
ahead_m_A()
ahead_m_B()
# print("ahead")
elif (pwm_A < 0 and pwm_B <= 0) or (pwm_A <= 0 and pwm_B < 0): # 向后
back_m_A()
back_m_B()
# print("back")
elif pwm_A < 0 and pwm_B > 0: # 原地左转，只有当没有其他动作时才会原地左转
back_m_A()
ahead_m_B()
# print("left")
elif pwm_A > 0 and pwm_B < 0: # 原地右转，只有当没有其他动作时才会原地右转
ahead_m_A()
back_m_B()
# print("right")
else:
stop()
# print("stop")
pwm_A = abs(pwm_A)
pwm_B = abs(pwm_B)
return pwm_A, pwm_B
增量式PID模块
写好后已经测试过没有问题，可以作为包import进来直接使用。
pid.py
import pyb
'''
PID使用说明：
输入参数：当前的编码器值，目标速度对应的编码器值
输出参数：此时应当的给电机的pwm值
'''
class PID:
def __init__(self, pwm_range, kp_A, ki_A, kd_A, kp_B, ki_B, kd_B):
pwm_A = 0
pwm_B = 0
err = 0
err_A = 0
err_B = 0
last_err_A = 0
last_err_B = 0
self.pwm_range = pwm_range
self.kp_A = kp_A
self.ki_A = ki_A
self.kd_A = kd_A
self.kp_B = kp_B
self.ki_B = ki_B
self.kd_B = kd_B
self.pwm_A = 0
self.pwm_B = 0
self.err = err
self.err_A = err_A
self.err_B = err_B
self.last_err_A = last_err_A
self.last_err_B = last_err_B
# 增量PID原理代码
def incremental_pid(self, now, target):
'''
函数功能：增量PI控制器
入口参数：当前的编码器值，目标速度对应的编码器值
返回值 ：电机PWM
根据增量式离散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代表增量输出。
'''
err = target - now
pwm = pwm + self.kp*(err - last_err) + self.ki*err + self.kd*(err - last_err)
if (pwm >= self.pwm_range): # 限幅，防止pwm值超出100
pwm = self.pwm_range
if (pwm <= -self.pwm_range):
pwm = -self.pwm_range
last_err = err
return pwm
def pid_A(self, now, target):
self.err_A = target - now
self.pwm_A = self.pwm_A + self.kp_A*(self.err_A - self.last_err_A) + self.ki_A*self.err_A + self.kd_A*(self.err_A - self.last_err_A)
if (self.pwm_A >= self.pwm_range):
self.pwm_A = self.pwm_range
if (self.pwm_A <= -self.pwm_range):
self.pwm_A = -self.pwm_range
self.last_err_A = self.err_A
# print("pwm_A", self.pwm_A)
return self.pwm_A
def pid_B(self, now, target):
self.err_B = target - now
self.pwm_B = self.pwm_B + self.kp_B*(self.err_B - self.last_err_B) + self.ki_B*self.err_B + self.kd_B*(self.err_B - self.last_err_B)
if (self.pwm_B >= self.pwm_range):
self.pwm_B = self.pwm_range
if (self.pwm_B <= -self.pwm_range):
self.pwm_B = -self.pwm_range
self.last_err_B = self.err_B
return self.pwm_B
编码器读取计数模块
写好后测试没问题，也直接可以使用，但需要改变对应的引脚，因为大家的定时器、引脚使用可能不同。同时，在改变引脚的时候一定要注意，要选择高级定时器，因为只有高级定时器才可以配置为编码器模式。
encoder.py
import pyb
from pyb import Timer, Pin
import cmath
"""
入口参数：编码器读取频率
# 编码器电机参数参考
1.光电编码器电机(MG513P30_12V)的参数：
减速比：1:30
分辨率（电机驱动线数）：500ppr（编码器精度，转一圈输出的脉冲数）
2.霍尔编码器电机()的参数：
减速比：1:30
分辨率：13ppr
# 底盘机械参数参考
标准轮式机器人硬件尺寸参数：
轮子直径：6.5cm
两轮间距：16cm
履带式机器人硬件尺寸参数：
轮子直径：4cm
两轮间距：23cm
"""
class Encoder:
'''
机器人自身及编码器电机配置
'''
def __init__(self, encoder_freq):
'''————————————————————————————可调参数——————————————————————————————————'''
# -------机器人机械参数--------
self.wheel_distance = 0.160 # 轮间距，单位:cm
self.Wheel_diameter = 0.065 # 轮直径：单位:cm
# -------机器人编码器电机参数--------
reduction_ratio = 1/30 # 减速比，
resolution_ratio = 500 # 分辨率，单位:ppr
'''————————————————————————————系统参数——————————————————————————————————'''
#配置编码器AB相读取模式,利用定时器中断进行读取
p1_A = Pin("Y1"); p2_A = Pin("Y2");
tim_A = Timer(8)
tim_A.init(prescaler=0,period=10000)
ch1_A = tim_A.channel(1,Timer.ENC_AB,pin=p1_A); ch2_A = tim_A.channel(2,Timer.ENC_AB, pin=p2_A);
p1_B = Pin("X1"); p2_B = Pin("X2");
tim_B = Timer(5)
tim_B.init(prescaler=0,period=10000)
ch1_B = tim_B.channel(1,Timer.ENC_AB,pin=p1_B); ch2_B = tim_B.channel(2,Timer.ENC_AB, pin=p2_B)
# 初始化系统参数
freq_multiplier = 4 # 倍频数
self.pi = cmath.pi # pi的值
self.encoder_freq = encoder_freq
self.encoder_precision = freq_multiplier * resolution_ratio / reduction_ratio # 编码器精度 = 倍频数*编码器精度（电机驱动线数）*电机减速比
self.tim_A = tim_A
self.tim_B = tim_B
self.encoder_A = 0
self.encoder_B = 0
# 定时器中断：固定时间读取编码器数值
timer_read = Timer(10,freq=self.encoder_freq,callback=self.encoder_cb)
def encoder_cb(self, encoder):
'''
函数功能：通过定时器中断读取编码器A、B计数值，并重置编码器
'''
if self.tim_B.counter() > 5000:
self.encoder_B = 10000 - self.tim_B.counter()
else:
self.encoder_B = -self.tim_B.counter()
self.tim_B.counter(0)
if self.tim_A.counter() > 5000:
self.encoder_A = -(10000 - self.tim_A.counter())
else:
self.encoder_A = self.tim_A.counter()
self.tim_A.counter(0)
def target_enc_process(self, speed):
'''
函数功能：将目标速度值转化为目标编码器值
入口参数：目标速度值
返回值 ：目标编码器值
# 目标编码器值=(目标速度*编码器精度)/(轮子周长*控制频率)
'''
enc = (speed*self.encoder_precision) / ( (self.pi*self.Wheel_diameter) * self.encoder_freq)
return enc
def enc_to_speed(self, enc):
'''
函数功能：将编码器值转化为速度值
入口参数：编码器值
返回值：速度值
速度值=(采集到的脉冲数/编码器精度)*轮子周长*控制频率
'''
speed = (enc / self.encoder_precision) * (self.pi*self.Wheel_diameter) * self.encoder_freq
return speed
'''
# 已知量，单位m
v = 0 # 机器人的线速度
w = 0 # 机器人的角速度
v_l = 0 # 左轮速度
v_r = 0 # 右轮速度
D = 0.2 # 轮间距
d = D/2
# 速度、角速度、左右轮速的关系式
v_l = v + wd
v_r = v - wd
v = (v_l + v_r)/2
w = (v_l - v_r)/D
'''
def speed_to_anglin(self, speed_A, speed_B):
'''
函数功能：将两轮速度值转化为角速度和线速度值
入口参数：两轮速度值
返回值：角速度，线速度
'''
linear_vel = (speed_A + speed_B)/2
angular_vel = (speed_B - speed_A)/self.wheel_distance
return angular_vel, linear_vel
def anglin_to_speed(self, angular_vel, linear_vel):
'''
函数功能：将角速度和线速度值转化为两轮速度值
入口参数：角速度，线速度
返回值：两轮速度值
'''
speed_A = linear_vel - angular_vel * self.wheel_distance/2
speed_B = linear_vel + angular_vel * self.wheel_distance/2
return speed_A, speed_B
PID控制电机
在这里的PID控制器中，我们采用：
- 输入：目标编码器值和当前编码器值
- 输出：电机预期的PWM值
- 参数：P、I、D值
这里需要注意的是：目标编码器值是将目标速度经过编码器速度公式计算得出，公式可以参考encoder.py中的target_enc_process()函数，在这里我已经将公式封装成了一个函数直接进行调用即可。
主控代码
main.py
import pyb
from pyb import LED, Timer
import encoder
import pid
import motor
def mainTimer_cb(cb):
'''
函数功能：main函数的定时器中断函数，用于总体程序逻辑的定时执行
修改：修改对应的程序时间，以及程序的功能标志位(flag)
'''
global count, reset_flag, enc_flag, pid_flag, toggle_flag
LED(3).toggle()
count += 1
if (count%1000) == 0: # 重置计数 1s
reset_flag = True
if (count%10) == 0: # pid刷新并控制电机 15ms
pid_flag = True
if (count%500) == 0: # LED闪烁 500ms
toggle_flag = True
if __name__ == '__main__':
# 初始化
count = 0 # 程序定时计数初值
reset_flag = False
toggle_flag = False
pid_flag = False
# 初始化系统参数
pwm_range = 100 # PWM的范围（0-100）,默认值，不要改动
encoder_freq = 100 # 编码器读取频率,和电机控制频率相同
target_enc_A = 10 # 初始化时的 速度值
target_enc_B = 10
encoder_freq = 100 # 编码器读取频率
'''
机器人参数整定：
1.先在encoder.py中调整机器人机械参数、编码器参数
2.调整以下PID参数，一般只用PI即可。
'''
# -------编码器PID参数--------
kp_A = 0.8
ki_A = 0.3
kd_A = 0
kp_B = 1.0
ki_B = 0.3
kd_B = 0
# 定时器初始化：主程序逻辑控制，频率为1000Hz，每次计时1ms
mainTimer = Timer(1, freq=1000, callback=mainTimer_cb)
print("定时器初始化----")
# 编码器初始化
enc = encoder.Encoder(encoder_freq)
print("编码器初始化----")
# 初始化PID控制器
pid = pid.PID(pwm_range, kp, ki, kd)
print("PID初始化----")
# 初始化电机
motor = motor.Motor(pwm_range)
print("电机初始化----")
#-----------------------逻辑循环--------------------------#
#主程序逻辑
while True:
# 重置计数器 1s
if reset_flag == True:
count = 0
reset_flag = False
# pid刷新并控制电机 15ms
if pid_flag == True:
enc_A = enc.encoder_A
enc_B = enc.encoder_B
target_enc_A = enc.target_enc_process(target_speed_A)
target_enc_B = enc.target_enc_process(target_speed_B)
if target_enc_A > 0 or target_enc_B > 0:
pwm_A = pid.pid_A(cur_enc_A, target_enc_A)
pwm_B = pid.pid_B(cur_enc_B, target_enc_B)
elif target_enc_A < 0 or target_enc_B < 0:
pwm_A = pid.pid_A(cur_enc_A, target_enc_A)
pwm_B = pid.pid_B(cur_enc_B, target_enc_B)
else:
pwm_A = 0
pwm_B = 0
pwm_A = pid.pid_A(cur_enc_A, target_enc_A)
pwm_B = pid.pid_B(cur_enc_B, target_enc_B)
pwm_A, pwm_B = motor.direction_control(pwm_A, pwm_B)
self.motor.motor_run(pwm_A, pwm_B)
pid_flag = False
效果展示
在只加了P的情况下，大家可以自行改变参数进行调节验证，这里仅做原理性的讲解和功能性演示。同时利用自己写的可视化绘图工具实时显示了一下pid调节情况：
下图是我用单个电机进行试验的画面，在上图中我是使用的两个编码器电机的底盘进行调节，所以如果大家使用单个电机，那么只需要大家需要微调一下上边的代码即可，将A或B任意的量全部置零即可。
PID调节技巧
最后给大家大概说一下PID的调节方法：
- P,I,D置 0，逐渐增大 P，直至响应时间达到预期效果并出现震荡
- 增大微分项 D，直至波动减到预期
- 若存在过冲，减小 P
- 增大积分项 I
如果有想要完整PYB差速底盘主控代码的同学可以直接去我的仓库下载，我已经将项目公开，并会不断更新： PYB差速主控底盘
1万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
