Abstract: 使用 k210 开发板控制SG996舵机使小球平衡在目标点处

实验目的

用微处理器控制舵机的角度，对平衡板的角度进行调节，从而利用平衡板的倾斜使小球移动，并最终让小球稳定在设定的目标位置处。

器材和准备

器材

  • 基于 k210芯片 的 MAIX Bit 开发板

  • SG996 舵机

  • 3.7v 锂电池及电池盒

  • 5v 稳压模块

  • 绿色玻璃球

  • 薄木板、 细木棍

  • 开槽或钻孔的不锈钢支架

  • 杜邦线

  • 自制的开发板转接板

  • 胶水等等

准备

用薄木板制作一个形如长方体的无顶木盒作为平衡板和形如桥墩的支架，在木盒的中间位置粘上木棍从而让木盒可以在支架上左右倾斜。

使用不锈钢支架将开发板固定在木盒的上方（该支架与木制支架不相接），舵机转盘上固定有和木盒顶端相接的薄片。

连接电源、稳压模块和舵机等设备。

实践部分

主要难点在于代码的编写，故此处只记录代码相关的内容。

分析

舵机的控制使用简单的 PWM 控制即可，但需要注意限制舵机转动的角度。

实验中使用的小球为绿色玻璃球，颜色鲜明，在光照稳定的条件下，可以使用开发板的色块寻找功能定位小球的位置。

单环的 PID 控制可能不足以实现需要的效果，因此使用双环 PID 来实现功能。

代码编写

PID.py

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
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

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
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 的输出计算基于上一次角度的增量和减量，这可能是导致明显震荡的重要原因，但是因为实验平台已经拆除，无法再做尝试。

之后若有机会，可以尝试在平衡板的平衡点角度的基础上增减来达到缓解震荡的目的。