基于树莓派的电机倒立摆控制系统

目录

摘要：

一、倒立摆设计方案

1.1  倒立摆系统建模

        在这里我把一些公式推导就省略了（因为公式太懒得编辑了，如果需要完整的论文及代码可以私我）。

2.2  Simulink仿真

二、硬件设计方案

2.1  整体设计方案

2.2  供电接口电路

2.3  树莓派与传感器接口电路

 三、软件设计方案（需要完整的论文及代码可以私聊获取）

3.1  主函数

 3.2  定时器中断函数

 3.2.1 位置闭环定时器中断函数

3.2.2  角度闭环定时器中断函数

3.2.3  位置角度双闭环定时器函数        

 3.3  编码器输入捕捉程序

3.4  PID算法

 四、具体调试

4.1  PID参数整定

4.2  角度闭环PID参数整定

4.3  位置角度双闭环调试

五、总结 

六、程序代码（需要完整代码私）

6.1  霍尔脉冲程序：

         6.2  PID函数程序：

6.3  电机驱动程序

---

摘要：

        随着现在控制理论在实际生活中的广泛使用，，倒立摆作为一种验证控制理论正确性的一种重要工具，使得对于倒立摆控制的研究越来越有必要。本文在分析了倒立摆的理论模型，基于树莓派linux平台这一强大微型电脑下，控制编码器直流电机驱动和调速以及PID控制，再一次验证了控制科学与工程这一领域相关理论的正确性，以及相关参数对于倒立摆稳定性的影响大小。设计了树莓派与电机及其驱动系统）之间的接口电路，编码器直流电机硬件驱动电路，电源电路，树莓派与电位传感器之间的接口电路，硬件部分由树莓派控制芯片，编码器电机，电位传感器组成，以树莓派为驱动器的控制芯片，软件部分是在树莓派上利用python语言进行驱动电机代码，编码器和电位传感器数据处理的代码。采用L298驱动器进行对电机的驱动，通过Simulink仿真得到控制倒立摆PID的相关参数，采用位置环和速度环双闭环控制倒立摆，编写控制软件，实现倒立摆的控制，包括起摆，倒立稳定控制；编写人机界面，实现对倒立摆控制系统的控制操作，以及对倒立摆数据的实时显示与存储，从而实现对倒立摆的稳定控制。另外设计了编码器接口电路用来连接电机上的霍尔编码器和树莓派，对树莓派通过设置频率和占空比来输出PWM波，驱动编码器电机旋转，通过调节PWM的占空比进而调节电机的电压来实现对速度的调节。通过霍尔编码器来实现对电机的位置和速度的实时反馈，经过驱动器上的编码器接口与树莓派GPIO口相连，从而形成一个闭环控制系统。运用增量式PID算法求出每次控制量的增量值来改变每次控制量输出的大小，从而让电机的实际状态达到所设定的目标状态，实现对倾角和位置的闭环控制，并把电机的实时状态传输到上位机进行实时显示。

关键词：

        倒立摆、树莓派、PID控制、霍尔编码器、Simulink仿真、人机交互界面

---

一、倒立摆设计方案

1.1  倒立摆系统建模

        倒立摆系统是控制理论研究中理想的被控对象，为了便于直观的分析出控制倒立摆运行的各个参数，下面利用控制科学的相关理论对倒立摆系统进行理论上的模型建立，同时也便于接下来倒立摆实物的搭建。

        下面为单级倒立摆系统模型，将根据此模型利用牛顿定律进行受力分析，计算倒立摆系统的传递函数。

图 2-1 单级倒立摆系统

如图2-1所示，一个小车上固定着一个摆杆，在水平面上受到其他外力的作用，构成了一个标准的倒立摆系统，由于在这里现实中摩檫力极小，在此次建模中忽略不计，该系统受到自身重力，驱动力两个外力共同作用，同时也有小车与摆杆内部的相互作用力，这里的驱动力实际上是由连接在小车上的传送带装置提供，控制倒立摆的稳定就是依靠驱动力使各个力之间达到一种动态平衡，使得小车做和倒立摆相似的运动，使摆杆保持稳定垂直状态。

        在这里我把一些公式推导就省略了（因为公式太懒得编辑了，如果需要完整的论文及代码可以私聊我qq2521170001获取）。

2.2  Simulink仿真

        由以上结论，利用Simulink现有得模型，将式(2-13)、(2-14)代入Matlab进行仿真，建立出倒立摆系统模型，仿真结果可通过示波器，图像，数据的方式显示出来，直观的理解倒立摆相关数据参数对系统稳定性的影响。

                                                        图 2-2 Simulink系统框图

得到的响应的波形如下：

---

二、硬件设计方案

2.1  整体设计方案

        电机驱动器系统整体采用树莓派作为主控芯片，采用按键作为输入控制，再连接驱动芯片L298控制驱动电路，以此来驱动电机。电机上面连接有霍尔编码器，编码器再连接到驱动器上的编码器接口后与单片机相连，从而实现对电机的闭环控制，树莓派再通过串口对电机的状态进行打印显示。

 

                                                              图 2-1 系统整体框图

2.2  供电接口电路

        供电电路采用15V航模电源供电，采用三个降压模块，分别降至12V给直流电机供电，降至5V给编码器供电，降至3V给电位传感器供电，另外树莓派直接采用充电宝5V-2.4A的USB供电。

                                                         图 2-2 供电接口电路设计

2.3  树莓派与传感器接口电路

        树莓派的GPIO口编号采用 BCM规则，该规则是一种更底层的工作方式，其与Broadcom的片上系统中信道编号相对应。其缺点是当换用一个新的板子时，需要重新查找信道号和物理引脚编号之间的对应规则。树莓派的GPIO.22和GPIO.23接到霍尔编码器的A、B相实时采集电机的转动数据，通过GPIO.27和GPIO.28口控制直流电机的正反转，通过GPIO.26给电机相应的占空比控制电机的转速。另外通过GPIO.25口传输数据到上位机，以便实时观测倒立摆的运行参数，电位传感器通过GPIO.24向树莓派传输倒立摆摆杆与平衡位置的角度偏差，使程序能够通过位置和速度双闭环调整摆杆的位置。

                                                           图 3-3 树莓派接口电路图

---

 三、软件设计方案（需要完整的论文及代码可以私聊获取）

3.1  主函数

        在主函数的逻辑中，先对树莓派中的GPIO口进行初始化，将GPIO口设置BCM编号方式，并设置成各GPIO口为输入输出模式，初始化GPIO口为低电平。再进行PWM的初始化，设置频率，调制占空比，复位所有系统中使用到的外设，初始化系统时钟、按键以及串口通信等的配置。对控制电机用到的定时器进行初始化，并配置脉冲输出，使电机状态初始化为零。延时一段时间后，对PID控制器的各个参数进行初始化。随后进入循环状态，对按键的状态进行扫描判断，如果判断启动按键的电平状态为按下的状态，接着后面就进行PID参数的初始化和电机速度的初始化，继而调用启动电机的函数，让电机以一定的PWM输出值运行[8]。这个过程通过编码器函数在中断中实时的采集电机速度值，并将该值传给倾角PID和位置PID函数，进行双闭环控制实时的调节电机的占空比，从而使电机的状态达到目标状态，进而实现倒立摆稳定运行。

图 3-1 主函数流程图

         

 3.2  定时器中断函数

        在树莓派中，python中的中断函数是通过开启一个新的线程定时执行一段固定的函数，函数为 threading.Timer(中断时间, 函数名)，设定时间为50ms，即每过50ms执行进入PID函数，进行一次PID计算，然后将PID计算的占空比赋值给电机，进而控制电机的速度。

 3.2.1 位置闭环定时器中断函数

       在位置闭环控制中，其实就是利用电机旋转时，其上的编码器产生脉冲，然后由定时器计数，累加测量电机的位置信息，并与目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。当计数达到位置环的计时周期之后，就会对编码器捕获到的信号值进行计算，得到当前时刻的位置值。当前时刻的位置状态用总的脉冲数来衡量，总的脉冲数的计算值需要用编码器定时器的计数最大值乘以溢出次数再加上一计数周期内的捕获值而得到，接着就会把PID控制器计算函数返回的增量值加到当前PWM输出值上面，作为下一次位置环的PWM输出值。同时，会对速度进行限幅，防止刚开始的时候速度太大。接着对方向进行判断之后，输出计算完成的PWM，实时传给电机控制电机的占空比。

图 3-2 位置闭环中断回调函数

3.2.2  角度闭环定时器中断函数

        这是最核心的控制，其他的控制相对于倾角控制而言都是干扰。

        在倾角闭环控制里，实际上就是根据单位时间获取的脉冲数测量电机的转过的角度信息，并与电位器平衡位置的目标值进行比较，得到控制偏差，然后通过对偏差的比例、积分、微分进行控制，使偏差趋向于零的过程。当达到倾角环的计时周期之后，树莓派会先计算当前电机转过的角度。当前角度值的计算是将前后采集到的两次编码器作差计算，这样计算得到的当前角度值的是每样本周期采集到的脉冲数。将编码器的值转化成角度值，下面还需要对当前角度计算值的单位进行转化，转化为和设定的目标角度值（即角度传感器的平衡位置）同样的单位，即和设定的目标角度值一样单位的当前角度值。接着，计算PWM输出值，并判断方向后，输出计算完成的PWM，实时传给电机控制电机的占空比。

图 3-3 角度闭环中断回调函数

3.2.3  位置角度双闭环定时器函数        

        位置角度双闭环的程序中，位置环和速度环都有各自的计算周期，达到各自的计时周期之后，就会各自进行相应的计算，当位置环和角度环分别进行计算之后，然后会将两者计算的PWM做差，由于倾角环是最核心的控制，因此在这里，最终的PWM计算值为倾角环PWM值减去位置环的PWM值，然后把还需要将该值进行PWM值限幅，避免占空比100%带来的不稳定因素，才会输出最终的PWM输出值，赋值给电机。

图 3-4 位置角度双闭环回调函数流程图 

 3.3  编码器输入捕捉程序

        采用的编码器为13线的霍尔编码器，电机的减速比为1：20，故同步轮转一圈，电机可以输出260个脉冲，编码器集成了上拉电阻和比较整形功能，可以直接输出方波。电机转动时，霍尔传感器就会根据磁钢转动产生的磁场作用而输出脉冲信号，增大电机转轴上所均匀固定的磁钢的块数，那么，电机旋转一周所产生的脉冲个数就会随之而增加，单片机可以对编码器产生的信号进行捕捉[9]，进而通过计算得到电机的位置和速度信息。将编码器对于的GPIO口设置为输入模式，实时连续的检测脉冲，采用的是边缘检测方式。在边缘检测方式中，add_event_detect()函数运行后，会为回调函数另外开启一个线程，与主程序并发运行，因此不容易错过当 CPU 忙于处理其它事物时输入状态的改变。但是同一进程内也不能有太过耗费CPU时间的部分，否则仍会导致脉冲的丢失。

                                                     图 3-5 部分编码器输入捕获程序

3.4  PID算法

        PID控制器计算函数的输入是被控对象的当前值，对于位置环来说就是当前位置，对于角度环来说就是当前角度。先定义偏差和增量值，接着计算偏差，偏差等于被控对象的目标值减去当前实际值，并设定允许的偏差范围。接着根据公式计算增量值，增量值等于P乘以当前误差减去I乘以上次的误差加上D乘以上上次的误差，并做一个存储误差的操作，把上次的误差值赋值给上上次，当前的误差值赋值给上次。最后返回计算得到的输出量的增值大小。

图 3-5 增量式PID算法流程图

---

 四、具体调试

4.1  PID参数整定

略

4.2  角度闭环PID参数整定

略

4.3  位置角度双闭环调试

略

在这里我把一些公式推导就省略了（因为公式太懒得编辑了，如果需要完整的论文及代码可以私）

---

五、总结 

        本文设计了基于树莓派倒立摆的方案模型，利用matlab中的Simulink进行了倒立摆的仿真，验证了方案的实际可行性，并且学习了树莓派的python编程方法，掌握了部分linux系统，在控制倒立摆稳定的过程中，并且以PID控制器的算法思想为理论基础，设计了基于树莓派的电机位置角度双闭环控制的软件程序，同时利用工程整定法的试凑法对各个PID的参数进行多次实验，最终得到满意的控制效果。

---

六、程序代码（需要完整代码私）

6.1  霍尔脉冲程序：

#部分霍尔脉冲读取函数
 GPIO_A=22 #编码器A相
 GPIO_B=23 #编码器B相
 GPIO.setup(GPIO_A, GPIO.IN,pull_up_down=GPIO.PUD_UP)   #通过22号引脚读取A相脉冲数据
 GPIO.setup(GPIO_B, GPIO.IN,pull_up_down=GPIO.PUD_UP)   #通过23号引脚读取B相脉冲数据
 counter=0      #A相脉冲初值
 counter1=0     #B相脉冲初值
 def my_callback(channel):          #边缘检测回调函数
     global counter                 #设置为全局变量
     if GPIO.event_detected(GPIO_A):        #检测到一个脉冲则脉冲数加1
         counter=counter+1
 def my_callback1(channel1):            #这里的channel和channel1无须赋确定值
     global counter1
     if GPIO.event_detected(GPIO_B):
         counter1=counter1+1
 GPIO.add_event_detect(GPIO_A,GPIO.RISING,callback=my_callback) #在引脚上添加上升临界值检测再回调
 GPIO.add_event_detect(GPIO_B,GPIO.RISING,callback=my_callback1)

6.2  PID函数程序：

import pyb
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
        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

6.3  电机驱动程序

class Motor:
    def __init__(self,direction,speed):
        self.IN1 = 27 # 电机A的正极IO口
        self.IN2 = 28
        self.ENA = 16   # 电机的PWM输入IO口
        self.hz = 500   #定义频率变量（等一下用）
        self.pwm = 0
        self.speed = speed    #定义占空比变量（等下用）运行时默认的占空比为0
        self.direction = direction
        
              
    def setup(self):
        print('Begin setup IN1 IN2 ENA')
        GPIO.setwarnings(False)  #忽略警告
        GPIO.setmode(GPIO.BCM)    # 使用BCM编号方式
        GPIO.setup(self.ENA, GPIO.OUT)  # 将连接ENA的GPIO引脚设置为输出模式
        GPIO.setup(self.IN1, GPIO.OUT)   # 将连接IN1的GPIO引脚设置为输出模式
        GPIO.setup(self.IN2, GPIO.OUT)   # 将连接IN2的GPIO引脚设置为输出模式
        self.pwm = GPIO.PWM(self.ENA, self.hz)   # 设置向ENA输入PWM脉冲信号，频率为hz并创建PWM对象
        self.pwm.start(self.speed)         # 以speed的初始占空比开始向ENA输入PWM脉冲信号
        print('Setup IN1 IN2 ENA over')

    def destroy(self):
        GPIO.output(self.IN1, GPIO.LOW)
        GPIO.output(self.IN2, GPIO.LOW)
        GPIO.output(self.ENA, GPIO.LOW)
        GPIO.cleanup()
        
    def motor_run(self,direction,speed):
        '''
        函数功能：控制方向，控制速度
        '''
        #self.setup()
        if direction == 0:     #如果键盘输入的数值为0，则电机正转
            GPIO.output(self.IN1, GPIO.LOW)
            GPIO.output(self.IN2, GPIO.HIGH)
            self.pwm.ChangeDutyCycle(speed)   # 改变PWM占空比
        if direction == 1:      #如果键盘输入的数值为1，则电机反转
            GPIO.output(self.IN1, GPIO.HIGH)
            GPIO.output(self.IN2, GPIO.LOW)
            self.pwm.ChangeDutyCycle(speed)        
        if direction == 2:      #如果键盘输入的数值为2，则电机停止转动
            GPIO.output(self.IN1, GPIO.LOW)
            GPIO.output(self.IN2, GPIO.LOW)
            GPIO.output(self.ENA, GPIO.LOW)