Abstract: 使用 k210 开发板控制SG996舵机使小球平衡在目标点处。
实验目的
用微处理器控制舵机的角度，对平衡板的角度进行调节，从而利用平衡板的倾斜使小球移动，并最终让小球稳定在设定的目标位置处。
器材和准备
器材
基于 k210芯片 的 MAIX Bit 开发板
SG996 舵机
3.7v 锂电池及电池盒
5v 稳压模块
绿色玻璃球
薄木板、 细木棍
开槽或钻孔的不锈钢支架
杜邦线
自制的开发板转接板
胶水等等
准备
用薄木板制作一个形如长方体的无顶木盒作为平衡板和形如桥墩的支架，在木盒的中间位置粘上木棍从而让木盒可以在支架上左右倾斜。
使用不锈钢支架将开发板固定在木盒的上方（该支架与木制支架不相接），舵机转盘上固定有和木盒顶端相接的薄片。
连接电源、稳压模块和舵机等设备。
实践部分
主要难点在于代码的编写，故此处只记录代码相关的内容。
分析
舵机的控制使用简单的 PWM 控制即可，但需要注意限制舵机转动的角度。
实验中使用的小球为绿色玻璃球，颜色鲜明，在光照稳定的条件下，可以使用开发板的色块寻找功能定位小球的位置。
单环的 PID 控制可能不足以实现需要的效果，因此使用双环 PID 来实现功能。
代码编写
PID.py
class PIDClass:
"""PID类"""
def __init__(self, kp=0.1, ki=0, kd=0, option=None):
"""初始化"""
self.__Kp = kp # P参数
self.__Ki = ki # I参数
self.__Kd = kd # D参数
self.__Iterm_max = None # Iterm最大值,用于积分限幅,非int或float表示不限制
self.__mistake_enableIterm = None # 启用I调节的误差最大值,用于积分分离,非int或float表示不限制
self.__Dterm_forward = False # 微分先行,False表示不启用
self.__mistake_deadband = None # 误差死区
if isinstance(option, dict): # 提供了参数字典
self.__Kp = option["Kp"]
self.__Ki = option["Ki"]
self.__Kd = option["Kd"]
self.__Iterm_max = option["Iterm_max"]
self.__mistake_enableIterm = option["mistake_enable_Iterm"]
self.__Dterm_forward = option["Dterm_forward"]
self.__Pterm = 0 # P调节数值
self.__Iterm = 0 # I调节数值
self.__Dterm = 0 # D调节数值
self.__measurement = 0 # 测量量
self.__last_mistake = 0 # 上一次的误差
self.__last_measurement = 0 # 上一次的测量值
self.__target = 0 # 目标值
def __dict__(self):
return {"Kp": self.__Kp, "Ki": self.__Ki, "Kd": self.__Kd, "Iterm_max": self.__Iterm_max,
"mistake_enable_Iterm": self.__mistake_enableIterm, "Dterm_forward": self.__Dterm_forward,
"mistake_deadband": self.__mistake_deadband}
def __str__(self):
return "Kp={0} Ki={1} Kd={2} Iterm_max={3} mistake_enable_Iterm={4} Dterm_forward={5} mistake_deadband={6}".format(self.__Kp, self.__Ki, self.__Kd, self.__Iterm_max, self.__mistake_enableIterm,
self.__Dterm_forward, self.__mistake_deadband)
def setKp(self, kp):
"""设置P参数"""
self.__Kp = kp
def setKi(self, ki):
"""设置I参数"""
self.__Ki = ki
def setLimitForIterm(self, max_value, mistake_enable_value):
"""设置I计算的限制参数"""
self.__Iterm_max = max_value
self.__mistake_enableIterm = mistake_enable_value
def setKd(self, kd):
"""设置D参数"""
self.__Kd = kd
def setTarget(self, target):
"""设置目标值"""
self.__target = target
def setDtermForward(self, state):
"""微分先行设置"""
if isinstance(state, bool):
self.__Dterm_forward = state
def setMistakeDeadband(self, daedband):
"""误差死区设置"""
self.__mistake_deadband = daedband
def getPterm(self):
"""获取P计算值"""
return self.__Pterm
def getIterm(self):
"""获取I计算值"""
return self.__Iterm
def getDterm(self):
"""获取D计算值"""
return self.__Dterm
def update(self, feedback):
"""
迭代计算
在不考虑积分限幅、积分分离和微分先行时,公式为:
$$U_n = K_P{\times}E_n + K_I{\times}{\sum_{i=1}^{n}E_i} + K_D{\times}(E_n-E_{n-1})$$
"""
mistake = self.__target - feedback
if self.__mistake_deadband is not None:
if abs(mistake) < self.__mistake_deadband: # 误差在死区,则认为无误差
mistake = 0
self.__last_measurement = self.__measurement
self.__measurement = feedback
# P计算
self.__Pterm = mistake * self.__Kp
# I计算
if self.__mistake_enableIterm is not None: # 启用积分分离
if mistake <= self.__mistake_enableIterm: # 误差不大于限制值
self.__Iterm = self.__Iterm + mistake
else:
self.__Iterm = self.__Iterm + mistake
if self.__Iterm_max is not None: # 启用积分限幅
self.__Iterm = min(self.__Iterm_max, self.__Iterm)
self.__Iterm *= self.__Ki
# D计算
if self.__Dterm_forward: # 启用微分先行
self.__Dterm = self.__measurement - self.__last_measurement
else:
self.__Dterm = mistake - self.__last_mistake
self.__Dterm *= self.__Kd
self.__last_mistake = mistake
def output(self):
"""获取最终结果"""
return self.__Pterm + self.__Iterm + self.__Dterm
ball_balance.py
import lcd, sensor, image, time
from machine import Timer,PWM
from PID import PIDClass
PWM_OUT = 8 # IO8输出PWM
color_threshold=[(0, 100, -128, -9, -128, 127)] # 色块寻找的阈值
tim = Timer(Timer.TIMER0, Timer.CHANNEL0, mode=Timer.MODE_PWM)
pwm = PWM(tim, freq=50, duty=0, pin=PWM_OUT)
#lcd.init()
sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames()
def cal_target_speed(pp_output): # 计算目标速度
return pp_output
def limit_angle(angle):
angle_range=(41, 73)
if angle<=angle_range[0]:
return angle_range[0]
elif angle>=angle_range[1]:
return angle_range[1]
return int(angle)
def set_angle(angle): # 舵机角度设置
if angle <= 0:
angle = 0
elif angle >= 180:
angle = 180
pwm.duty(angle/18 + 2.5)
def main():
not_found_flag=False
angle=55
last_pos=[0, 0]
now_pos=[0, 0]
speed=0
pos_range=(10, 210)
target_pos=110
target_speed=0
pp_output=0
sp_output=0
pp_var={"Kp": 0.21, "Ki": 0.031, "Kd": 0.05, "Iterm_max": None, "mistake_enable_Iterm": None, "Dterm_forward": False, "mistake_deadband": None}
pos_pid=PIDClass(option=pp_var)
pos_pid.setTarget(target_pos)
sp_var={"Kp": 0.32, "Ki": 0.108, "Kd": 0.072, "Iterm_max": None, "mistake_enable_Iterm": None, "Dterm_forward": True, "mistake_deadband": 4}
speed_pid=PIDClass(option=sp_var)
speed_pid.setTarget(target_speed)
set_angle(angle)
time.sleep(0.2)
while(1):
t = time.ticks_ms()
img=sensor.snapshot()
blobs=img.find_blobs(color_threshold)
# 色块寻找
if blobs:
for j in blobs:
if j.w() > 12 and j.h() > 12:
not_found_flag=False
last_pos=now_pos
now_pos=[j.x(),j.y()] # y有效
#img.draw_rectangle(j.x(), j.y(), 15, 15, color=(255,255,255), thickness=1)
else:
not_found_flag=True
else:
not_found_flag=True
#lcd.display(img)
t = time.ticks_ms() - t
if not_found_flag: # 没有找到
print("not_found_flag")
continue
speed=(now_pos[1]-last_pos[1]) / (t/1000) / 4 # 计算速度 pixel/s,区分正负
pos_pid.update(now_pos[1]) # 位置pid计算
pp_output=pos_pid.output()
target_speed=cal_target_speed(pp_output) # 目标速度计算
speed_pid.setTarget(target_speed) # 速度pid目标更新
speed_pid.update(speed) # 速度pid计算
sp_output=speed_pid.output()
angle=limit_angle(angle+sp_output) # 目标角度计算
set_angle(angle)
print("now_pos={0},speed={1},target_pos={2},target_speed={3},pp_output={4},sp_output={5},angle={6}".format(now_pos,speed,target_pos,target_speed,pp_output,sp_output,angle))
if __name__ == "__main__":
main()
结果分析
预期结果为小球稳定在设定的位置，但是实际上小球只能在目标位置附近来回抖动（误差大约为 18%），推测原因比较复杂。
因为平衡板并不完全水平，导致小球滚动过程中始终与侧边接触，阻碍了小球的运动，出现平衡板倾斜但是小球不滚动的情况。
小球的识别受光亮影响很大，需要给系统额外提供稳定光源。
补充
实验中计算角度的算法的思想是，通过速度环 PID 的输出计算基于上一次角度的增量和减量，这可能是导致明显震荡的重要原因，但是因为实验平台已经拆除，无法再做尝试。
之后若有机会，可以尝试在平衡板的平衡点角度的基础上增减来达到缓解震荡的目的。