CN110568755B - 一种基于低通滤波的倒立摆控制方法、装置及系统 - Google Patents

一种基于低通滤波的倒立摆控制方法、装置及系统 Download PDF

Info

Publication number
CN110568755B
CN110568755B CN201910858296.3A CN201910858296A CN110568755B CN 110568755 B CN110568755 B CN 110568755B CN 201910858296 A CN201910858296 A CN 201910858296A CN 110568755 B CN110568755 B CN 110568755B
Authority
CN
China
Prior art keywords
inverted pendulum
angle
low
pass filtering
motor
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN201910858296.3A
Other languages
English (en)
Other versions
CN110568755A (zh
Inventor
盛峥彦
李磊
宦蕴哲
蒋晨阳
王海滨
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Changzhou Campus of Hohai University
Original Assignee
Changzhou Campus of Hohai University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Changzhou Campus of Hohai University filed Critical Changzhou Campus of Hohai University
Priority to CN201910858296.3A priority Critical patent/CN110568755B/zh
Publication of CN110568755A publication Critical patent/CN110568755A/zh
Application granted granted Critical
Publication of CN110568755B publication Critical patent/CN110568755B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B11/00—Automatic controllers
    • G05B11/01—Automatic controllers electric
    • G05B11/36—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential
    • G05B11/42—Automatic controllers electric with provision for obtaining particular characteristics, e.g. proportional, integral, differential for obtaining a characteristic which is both proportional and time-dependent, e.g. P.I., P.I.D.
    • H—ELECTRICITY
    • H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
    • H02P—CONTROL OR REGULATION OF ELECTRIC MOTORS, ELECTRIC GENERATORS OR DYNAMO-ELECTRIC CONVERTERS; CONTROLLING TRANSFORMERS, REACTORS OR CHOKE COILS
    • H02P7/00—Arrangements for regulating or controlling the speed or torque of electric DC motors
    • H02P7/06—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current
    • H02P7/18—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current by master control with auxiliary power
    • H02P7/24—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current by master control with auxiliary power using discharge tubes or semiconductor devices
    • H02P7/28—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current by master control with auxiliary power using discharge tubes or semiconductor devices using semiconductor devices
    • H02P7/285—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current by master control with auxiliary power using discharge tubes or semiconductor devices using semiconductor devices controlling armature supply only
    • H02P7/29—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current by master control with auxiliary power using discharge tubes or semiconductor devices using semiconductor devices controlling armature supply only using pulse modulation
    • H02P7/2913—Arrangements for regulating or controlling the speed or torque of electric DC motors for regulating or controlling an individual dc dynamo-electric motor by varying field or armature current by master control with auxiliary power using discharge tubes or semiconductor devices using semiconductor devices controlling armature supply only using pulse modulation whereby the speed is regulated by measuring the motor speed and comparing it with a given physical value
    • H—ELECTRICITY
    • H02—GENERATION; CONVERSION OR DISTRIBUTION OF ELECTRIC POWER
    • H02P—CONTROL OR REGULATION OF ELECTRIC MOTORS, ELECTRIC GENERATORS OR DYNAMO-ELECTRIC CONVERTERS; CONTROLLING TRANSFORMERS, REACTORS OR CHOKE COILS
    • H02P2205/00—Indexing scheme relating to controlling arrangements characterised by the control loops
    • H02P2205/07—Speed loop, i.e. comparison of the motor speed with a speed reference

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Power Engineering (AREA)
  • Feedback Control In General (AREA)

Abstract

本发明公开了一种基于低通滤波的倒立摆控制方法、装置及系统，获取倒立摆摆杆角度信息和旋转臂位置信息；根据倒立摆摆杆角度信息，判断当前运动状态；基于当前运动状态，根据得到的倒立摆摆杆角度信息和旋转臂位置信息，结合预设的旋摆杆目标角度、旋转臂目标位置，通过双环低通滤波PID算法计算电机PWM输出值；根据计算得到的电机PWM输出值，发出指令控制电机转速。控制器中直接加入低通滤波器，预测摆杆运动姿态，减小倒立摆惯性与干扰影响，保持摆杆稳定。该装置具有较高的可靠性和稳定度，可实现系统的多样化控制，且整个装置结构简单，成本较低。

Description

一种基于低通滤波的倒立摆控制方法、装置及系统
技术领域
本发明涉及机械控制技术领域，具体涉及一种基于低通滤波的倒立摆控制方法、装置及系统。
背景技术
由于倒立摆系统是一个典型的快速、多变量、非线性、强耦合、自然不稳定系统，在它的控制过程中，能有效反映诸如镇定性、鲁棒性、随动性以及跟踪等许多关键问题，并且作为实验对象，倒立摆结构简单，参数易测，便于数学建模研究算法，因此是进行控制理论教学及开展各种控制策略的理想实验平台，人们常常利用倒立摆检验各种控制算法的不稳定性、非线性和快速系统的控制能力以及各种控制算法的有效性。
倒立摆系统进行研究，不仅具有理论意义，其类似的控制方法在半导体及精密仪器加工、机器人控制技术、人工智能、导弹拦截系统、航天对接控制技术、火箭发射中的垂直控制、卫星飞行器中的姿态控制和一般工业应用等方面都具有广阔的利用开发前景。
发明内容
目的：为了克服现有技术中存在的不足，本发明提供一种基于低通滤波的倒立摆控制方法、装置及系统。
技术方案：为解决上述技术问题，本发明采用的技术方案为：
一种基于低通滤波的倒立摆控制方法，包括：
获取倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)；
根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；
基于当前运动状态，根据得到的倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，结合预设的旋摆杆目标角度wi(k)、旋转臂目标位置Mi(k)，通过双环低通滤波PID算法计算电机PWM输出值；
根据计算得到的电机PWM输出值，发出指令控制电机转速。
所述的基于低通滤波的倒立摆控制方法，倒立摆运动状态包括：预起摆、起摆、倒立状态。
在本发明的一些实施例，根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；包括：
当倒立摆摆杆角度信息wr(k)＜Ws，Ws为预设的倒立摆摆杆角度阈值，为预起摆运动状态；
当倒立摆摆杆角度信息wr(k)≥Ws，Ws为预设的倒立摆摆杆角度阈值，则进入起摆运动状态；
当倒立摆摆杆角度值wr(k)∈{Wc-d,Wc+d}，Wc为预设的倒立摆摆杆倒立角度，d为允许角度偏差，则进入倒立状态。
在本发明的一些实施例，基于当前运动状态，根据倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，基于预设的旋摆杆目标角度、旋转臂目标位置，通过双环低通滤波PID算法计算电机PWM输出值；包括：
倒立状态控制摆杆保持倒立且旋转臂稳定30°范围内；
倒立状态，采用双环低通滤波PID算法，包括角度环负反馈PID和位置环正反馈PID，角度环负反馈PID所得值Ua，位置环正反馈PID所得值Ud，计算电机PWM输出值U＝Ua+Ud；
角度环负反馈PID：
Ua＝UaP(k)+UaI(k)+UaD(k)
wi(k)为预设的摆杆目标角度，摆杆角度偏差ea(k)＝wi(k)-wr(k)；
其中UaP(k)为角度环比例控制器，UaP(k)＝KaP·ea(k)，KaP为比例系数；
UaI(k)为角度环积分控制器，
Figure BDA0002198884760000031
KaI为积分系数；
UaD(k)为微分控制器，UaD(k)＝KaD(1-α)[ea(k)-ea(k-1)]+αUaD(k-1),KaD为微分控制系数，
Figure BDA0002198884760000032
T为角度传感器采样周期，Tf为低通滤波系数；
位置环正反馈PID：Ud＝UdP(k)+UdI(k)+UdD(k)，
Mi(k)为预设的旋转臂目标位置，旋转臂位置偏差ed(k)＝Mi(k)-Mr(k)；
其中，UdP(k)为角度环比例控制器，UdP(k)＝KdP·e(k)，KdP为比例系数；
UdI(k)为角度环积分控制器，
Figure BDA0002198884760000033
KdI为积分系数；
UdD(k)为微分控制器，
UdD(k)＝KdD(1-β)[e(k)-e(k-1)]+βUdD(k-1),KdD为微分控制系数，
Figure BDA0002198884760000034
T’为编码器采样周期，Tf’为低通滤波系数。
在本发明的一些实施例，所述的基于低通滤波的倒立摆控制方法，包括：
预起摆运动状态控制摆杆从自然下垂状态变为单摆简谐运动，近似旋转臂运动状态为简谐运动，预起摆运动状态由位置环负反馈PID控制电机转速；
旋转臂期望位置Position(k)＝A·Famp(k)·sin(Fcyc(k))
其中A为旋转臂长度系数；Famp(k)＝Famp(k-1)+Famp，Famp(k)为简谐运动振幅，逐渐增大，Famp为振幅增长系数；Fcyc(k)为摆动周期系数，
Figure BDA0002198884760000041
T(k)为摆动周期，随着振幅增大摆动周期减小，T(K)＝T(K-1)-Td,其中Td为周期减小系数；
设error(k)＝Position(k)-mr(k)，则位置环正反馈PID控制电机转速为：
U1(k)＝K1P·error(k)+K1D·(error(k)-error(k-1))。
在本发明的一些实施例，所述的基于低通滤波的倒立摆控制方法，包括：起摆运动控制摆杆从单摆运动状态到摆杆接近倒立状态，具体步骤如下：
S1：以固定PWM1波输出使旋转臂向预起摆角度反方向转动1.5S；
S2：以固定PWM2波输出使旋转臂向反方向转动0.3S；
S3:PWM停止输出。
根据本发明的第二方面，提供一种基于低通滤波的倒立摆控制装置，包括：
信息获取模块，用于：获取倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)；
状态判断模块，用于：根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；
计算模块，用于：基于当前运动状态，根据得到的倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，结合预设的旋摆杆目标角度wi(k)、旋转臂目标位置Mi(k)，通过双环低通滤波PID算法计算电机PWM输出值；
输出控制模块，用于：根据计算得到的电机PWM输出值，发出指令控制电机转速。
所述计算模块，包括：
倒立状态，采用双环低通滤波PID算法，包括角度环负反馈PID和位置环正反馈PID，角度环负反馈PID所得值Ua，位置环正反馈PID所得值Ud，计算电机PWM输出值U＝Ua+Ud；
角度环负反馈PID：
Ua＝UaP(k)+UaI(k)+UaD(k)
wi(k)为预设的摆杆目标角度，摆杆角度偏差ea(k)＝wi(k)-wr(k)；
其中UaP(k)为角度环比例控制器，UaP(k)＝KaP·ea(k)，KaP为比例系数；
UaI(k)为角度环积分控制器，
Figure BDA0002198884760000051
KaI为积分系数；
UaD(k)为微分控制器，UaD(k)＝KaD(1-α)[ea(k)-ea(k-1)]+αUaD(k-1),KaD为微分控制系数，
Figure BDA0002198884760000052
T为角度传感器采样周期，Tf为低通滤波系数；
位置环正反馈PID：Ud＝UdP(k)+UdI(k)+UdD(k)，
Mi(k)为预设的旋转臂目标位置，旋转臂位置偏差ed(k)＝Mi(k)-Mr(k)；
其中，UdP(k)为角度环比例控制器，UdP(k)＝KdP·e(k)，KdP为比例系数；
UdI(k)为角度环积分控制器，
Figure BDA0002198884760000061
KdI为积分系数；
UdD(k)为微分控制器，
UdD(k)＝KdD(1-β)[e(k)-e(k-1)]+βUdD(k-1),KdD为微分控制系数，
Figure BDA0002198884760000062
T’为编码器采样周期，Tf’为低通滤波系数。
根据本发明的第三方面，提供一种基于低通滤波的倒立摆控制系统，包括所述的基于低通滤波的倒立摆控制装置，所述的基于低通滤波的倒立摆控制装置包括存储器和处理器，存储器用于存储指令，所述指令用于控制所述处理器进行操作以执行所述的基于低通滤波的倒立摆控制方法。
所述的基于低通滤波的倒立摆控制系统，还包括：电机、旋转臂、摆杆、编码器、角度传感器；
所述旋转臂一端与电机输出端连接，另一端通过角度传感器与摆杆的转轴连接；
所述编码器安装在电机上，用于采集电机输出端的旋转角度，从而获取旋转臂位置信息，并发送给控制装置；
所述角度传感器固定在旋转臂另一端，用于采集摆杆角度信息并发送给控制装置；
所述控制装置的PWM输出口与电机连接，用于控制电机转速。
根据本发明的第四方面，提供一种倒立摆装置，包括所述的基于低通滤波的倒立摆控制系统。
有益效果：本发明提供的基于低通滤波的倒立摆控制方法、装置及系统，采用双环低通滤波PID算法，与传统PID算法相比，在微分控制器中采用数字低通滤波，构成不完全微分PID，微分输出第一个采样周期内脉冲高度下降，随后且按指数形式衰减，具有更理想的控制效果和抗干扰性。控制器中直接加入低通滤波器，预测摆杆运动姿态，减小倒立摆惯性与干扰影响，保持摆杆稳定。有效减小惯性与干扰影响，整个装置具有较高的可靠性和鲁棒性，结构简单，成本低廉。
附图说明
图1为本发明一实施例基于低通滤波的倒立摆装置示意图；
图2为本发明一实施例控制系统的框图。
具体实施方式
下面结合具体实施例对本发明作更进一步的说明。
实施例1
一种基于低通滤波的倒立摆控制方法，包括：
获取倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)；
根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；
基于当前运动状态，根据得到的倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，结合预设的旋摆杆目标角度wi(k)、旋转臂目标位置Mi(k)，通过双环低通滤波PID算法计算电机PWM输出值；
根据计算得到的电机PWM输出值，发出指令控制电机转速。
所述的基于低通滤波的倒立摆控制方法，倒立摆运动状态包括：预起摆、起摆、倒立状态。
在本发明的一些实施例，根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；包括：
当倒立摆摆杆角度信息wr(k)＜Ws，Ws为预设的倒立摆摆杆角度阈值，为预起摆运动状态；
当倒立摆摆杆角度信息wr(k)≥Ws，Ws为预设的倒立摆摆杆角度阈值，则进入起摆运动状态；
当倒立摆摆杆角度值wr(k)∈{Wc-d,Wc+d}，Wc为预设的倒立摆摆杆倒立角度，d为允许角度偏差，则进入倒立状态。
在本发明的一些实施例，基于当前运动状态，根据倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，基于预设的旋摆杆目标角度、旋转臂目标位置，通过双环低通滤波PID算法计算电机PWM输出值；包括：
(1)预起摆运动状态控制摆杆从自然下垂状态变为单摆简谐运动，近似旋转臂运动状态为简谐运动，预起摆运动状态由位置环负反馈PID控制电机转速；
旋转臂期望位置Position(k)＝A·Famp(k)·sin(Fcyc(k))
其中A为旋转臂长度系数；Famp(k)＝Famp(k-1)+Famp，Famp(k)为简谐运动振幅，逐渐增大，Famp为振幅增长系数；Fcyc(k)为摆动周期系数，
Figure BDA0002198884760000081
T(k)为摆动周期，随着振幅增大摆动周期减小，T(K)＝T(K-1)-Td,其中Td为周期减小系数；
设error(k)＝Position(k)-mr(k)，则位置环正反馈PID控制电机转速为：
U1(k)＝K1P·error(k)+K1D·(error(k)-error(k-1))。
(2)起摆运动控制摆杆从单摆运动状态到摆杆接近倒立状态，具体步骤如下：
S1：以固定PWM1波输出使旋转臂向预起摆角度反方向转动1.5S；
S2：以固定PWM2波输出使旋转臂向反方向转动0.3S；
S3:PWM停止输出。
(3)倒立状态控制摆杆保持倒立且旋转臂稳定30°范围内；
倒立状态，采用双环低通滤波PID算法，包括角度环负反馈PID和位置环正反馈PID，角度环负反馈PID所得值Ua，位置环正反馈PID所得值Ud，计算电机PWM输出值U＝Ua+Ud；
角度环负反馈PID：
Ua＝UaP(k)+UaI(k)+UaD(k)
wi(k)为预设的摆杆目标角度，摆杆角度偏差ea(k)＝wi(k)-wr(k)；
其中UaP(k)为角度环比例控制器，UaP(k)＝KaP·ea(k)，KaP为比例系数；
UaI(k)为角度环积分控制器，
Figure BDA0002198884760000091
KaI为积分系数；
UaD(k)为微分控制器，UaD(k)＝KaD(1-α)[ea(k)-ea(k-1)]+αUaD(k-1),KaD为微分控制系数，
Figure BDA0002198884760000092
T为角度传感器采样周期，Tf为低通滤波系数；
位置环正反馈PID：Ud＝UdP(k)+UdI(k)+UdD(k)，
Mi(k)为预设的旋转臂目标位置，旋转臂位置偏差ed(k)＝Mi(k)-Mr(k)；
其中，UdP(k)为角度环比例控制器，UdP(k)＝KdP·e(k)，KdP为比例系数；
UdI(k)为角度环积分控制器，
Figure BDA0002198884760000101
KdI为积分系数；
UdD(k)为微分控制器，
UdD(k)＝KdD(1-β)[e(k)-e(k-1)]+βUdD(k-1),KdD为微分控制系数，
Figure BDA0002198884760000102
T’为编码器采样周期，Tf’为低通滤波系数。
确定双环PID各自比例，积分，微分系数是本系统的关键，当系统表现为在角度环单独作用下，电机输出使误差减小，在位置环单独作用下，电机输出使误差增大时，则系数正负号确定无误。当在角度环单独作用下，摆杆能倒立旋转一周，角度环参数无误，此时加上位置环参数调节，可实现倒立摆的倒立。
实施例2
一种基于低通滤波的倒立摆控制装置，包括：
信息获取模块，用于：获取倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)；
状态判断模块，用于：根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；
计算模块，用于：基于当前运动状态，根据得到的倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，结合预设的旋摆杆目标角度wi(k)、旋转臂目标位置Mi(k)，通过双环低通滤波PID算法计算电机PWM输出值；
输出控制模块，用于：根据计算得到的电机PWM输出值，发出指令控制电机转速。
实施例3
如图1、图2所示，本实施例中，控制装置采用控制器，一种基于低通滤波的倒立摆控制系统，包括控制器、电机2、旋转臂3、摆杆4、编码器5、角度传感器6；
所述旋转臂3一端与电机2输出端连接，另一端通过角度传感器6与摆杆4的转轴连接；
所述编码器5安装在电机上，用于采集电机输出端的旋转角度，从而获取旋转臂位置信息，并发送给控制器；
所述角度传感器6固定在旋转臂3另一端，用于采集摆杆角度信息并发送给控制器；
所述控制器的PWM输出口与电机连接，用于控制电机2转速。
所述控制器包括存储器和处理器，存储器用于存储指令，所述指令用于控制所述处理器进行操作以执行实施例1中所述的基于低通滤波的倒立摆控制方法。
在一些实施例中，如图1所示，还包括支架1，用于支撑安装上述的电机2、旋转臂3、摆杆4、编码器5、角度传感器6等。
支架1呈倒U型，通过底部旋转螺丝固定；支架内部安放电机；旋转臂呈’7’型，包括相互垂直的第一部和第二部，第一部与电机输出端连接，能够在电机的带动下盐水平面方向旋转，第二部一侧安装角度传感器6，另一侧连接摆杆，其中摆杆的转轴通过第二部上的通孔与角度传感器连接，同步旋转。
进一步的，还可以包括显示模块，所述显示模块与控制装置连接，用于将信息在显示屏上进行显示。显示器通过USB接口连接控制器，采用USART通信协议，将旋转臂位置信息，摆杆角度信息，电机PWM数值，系统状态信息上传PC端，不同信息在同一坐标下，以不同颜色显示，便于发现问题，调整参数。
在一些实施例中，控制装置采用意法公司的STM32F1芯片。角度传感器采用GY-25芯片，编码器模块采用增量型霍尔编码器。驱动模块采用TB6612FNG芯片。电机采用ForestD1直流减速电机。电源模块采用电源适配器。
中央控制器采用意法公司的STM32F1芯片，并通过电路设计将TB6612驱动模块和中央控制器插孔对接，信号线间接从驱动模块7个信号管脚引出，使信号线连接十分简洁，不影响倒立摆运动。与传统供电稳压模块相比，电源模块采用电源适配器配置TB6612FNG稳压模块比直流电源配置L298N稳压模块提供更稳定的电流，更大的功率，满足所述直流减速电机的要求。
角度传感器GY-25将采集的摆杆角度信息发送至中央控制器STM32F1，具体措施：中央控制器STM32F1通过USZRT协议，设置波特率115200，向角度传感器发送命令字节，控制角度传感器不同的工作方式，角度传感器接受到命令字节后，开始向中央控制器STM32F1发送采集数据。
所述角度传感器存在角度跃迁，如旋转一周从360°跳回0°，将跃迁角度设置在摆杆水平90°方向。
与传统PID算法相比，在微分控制器中采用数字低通滤波，构成不完全微分PID，微分输出第一个采样周期内脉冲高度下降，随后且按指数形式衰减。低通滤波器的引入使得算法对摆杆的动态演变具有预测作用，从而显著降低电机PWM波输出的超调量及稳态调节耗时，使摆杆能够快速稳定收敛到期望航速。
在一些实施例中，如图1所示，还提供一种倒立摆装置，包括所述的基于低通滤波的倒立摆控制系统。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
以上所述仅是本发明的优选实施方式，应当指出：对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以做出若干改进和润饰，这些改进和润饰也应视为本发明的保护范围。

Claims (9)

1.一种基于低通滤波的倒立摆控制方法，其特征在于，包括：
获取倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)；
根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；
基于当前运动状态，根据得到的倒立摆摆杆角度信息wr(k)和旋转臂位置信息mr(k)，结合预设的旋摆杆目标角度wi(k)、旋转臂目标位置Mi(k)，通过双环低通滤波PID算法计算电机PWM输出值；包括：
倒立状态，采用双环低通滤波PID算法，包括角度环负反馈PID和位置环正反馈PID，角度环负反馈PID所得值Ua，位置环正反馈PID所得值Ud，计算电机PWM输出值U＝Ua+Ud；
角度环负反馈PID：
Ua＝UaP(k)+UaI(k)+UaD(k)
wi(k)为预设的摆杆目标角度，摆杆角度偏差ea(k)＝wi(k)-wr(k)；
其中UaP(k)为角度环比例控制器，UaP(k)＝KaP•ea(k)，KaP为比例系数；
UaI(k)为角度环积分控制器，
Figure FDA0003534069870000011
KaI为积分系数；
UaD(k)为微分控制器，UaD(k)＝KaD(1-α)[ea(k)-ea(k-1)]+αUaD(k-1),KaD为微分控制系数，
Figure FDA0003534069870000012
T为角度传感器采样周期，Tf为低通滤波系数；
位置环正反馈PID：Ud＝UdP(k)+UdI(k)+UdD(k)，
Mi(k)为预设的旋转臂目标位置，旋转臂位置偏差ed(k)＝Mi(k)-Mr(k)；
其中，UdP(k)为角度环比例控制器，UdP(k)＝KdP·e(k)，KdP为比例系数；
UdI(k)为角度环积分控制器，
Figure FDA0003534069870000013
KdI为积分系数；
UdD(k)为微分控制器，UdD(k)＝KdD(1-β)[e(k)-e(k-1)]+βUdD(k-1),KdD为微分控制系数，
Figure FDA0003534069870000021
T’为编码器采样周期，Tf’为低通滤波系数；
根据计算得到的电机PWM输出值，发出指令控制电机转速。
2.根据权利要求1所述的基于低通滤波的倒立摆控制方法，其特征在于，倒立摆运动状态包括：预起摆、起摆、倒立状态。
3.根据权利要求1所述的基于低通滤波的倒立摆控制方法，其特征在于，根据倒立摆摆杆角度信息wr(k)，判断当前运动状态；包括：
当倒立摆摆杆角度信息wr(k)＜Ws，Ws为预设的倒立摆摆杆角度阈值，为预起摆运动状态；
当倒立摆摆杆角度信息wr(k)≥Ws，Ws为预设的倒立摆摆杆角度阈值，则进入起摆运动状态；
当倒立摆摆杆角度值wr(k)∈{Wc-d,Wc+d}，Wc为预设的倒立摆摆杆倒立角度，d为允许角度偏差，则进入倒立状态。
4.根据权利要求1所述的基于低通滤波的倒立摆控制方法，其特征在于，包括：
预起摆运动状态由位置环正反馈PID控制电机转速；
旋转臂期望位置Position(k)＝A·Famp(k)·sin(Fcyc(k))
其中A为旋转臂长度系数；Famp(k)＝Famp(k-1)+Famp，Famp(k)为简谐运动振幅，逐渐增大，Famp为振幅增长系数；Fcyc(k)为摆动周期系数，
Figure FDA0003534069870000022
T(k)为摆动周期，随着振幅增大摆动周期减小，T(k)＝T(k-1)-Td,其中Td为周期减小系数；
设error(k)＝Position(k)-mr(k)，则位置环正反馈PID控制电机转速为：
U1(k)＝K1P·error(k)+K1D·(error(k)-error(k-1))。
5.根据权利要求1所述的基于低通滤波的倒立摆控制方法，其特征在于，包括：起摆运动控制摆杆从单摆运动状态到摆杆接近倒立状态：
S1：以固定PWM1波输出使旋转臂向预起摆角度反方向转动1.5S；
S2：以固定PWM2波输出使旋转臂向反方向转动0.3S；
S3:PWM停止输出。
6.一种基于低通滤波的倒立摆控制装置，其特征在于，用于实现如权利要求1-5任一项所述的基于低通滤波的倒立摆控制方法。
7.一种基于低通滤波的倒立摆控制系统，其特征在于，包括权利要求6所述的基于低通滤波的倒立摆控制装置，所述的基于低通滤波的倒立摆控制装置包括存储器和处理器，存储器用于存储指令，所述指令用于控制所述处理器进行操作以执行根据权利要求1-5任一项所述的基于低通滤波的倒立摆控制方法。
8.根据权利要求7所述的基于低通滤波的倒立摆控制系统，其特征在于，还包括：电机、旋转臂、摆杆、编码器、角度传感器；
所述旋转臂一端与电机输出端连接，另一端通过角度传感器与摆杆的转轴连接；
所述编码器安装在电机上，用于采集电机输出端的旋转角度，从而获取旋转臂位置信息，并发送给控制装置；
所述角度传感器固定在旋转臂另一端，用于采集摆杆角度信息并发送给控制装置；
所述控制装置的PWM输出口与电机连接，用于控制电机转速。
9.一种倒立摆装置，其特征在于，包括权利要求7或8所述的基于低通滤波的倒立摆控制系统。
CN201910858296.3A 2019-09-11 2019-09-11 一种基于低通滤波的倒立摆控制方法、装置及系统 Active CN110568755B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201910858296.3A CN110568755B (zh) 2019-09-11 2019-09-11 一种基于低通滤波的倒立摆控制方法、装置及系统

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910858296.3A CN110568755B (zh) 2019-09-11 2019-09-11 一种基于低通滤波的倒立摆控制方法、装置及系统

Publications (2)

Publication Number Publication Date
CN110568755A CN110568755A (zh) 2019-12-13
CN110568755B true CN110568755B (zh) 2022-04-26

Family

ID=68779109

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910858296.3A Active CN110568755B (zh) 2019-09-11 2019-09-11 一种基于低通滤波的倒立摆控制方法、装置及系统

Country Status (1)

Country Link
CN (1) CN110568755B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101590323A (zh) * 2009-07-08 2009-12-02 北京工业大学 一种独轮机器人系统及其控制方法
CN105904456A (zh) * 2016-04-29 2016-08-31 西北工业大学 手持式力反馈装置以及利用该装置实现力觉再现的方法
CN108572590A (zh) * 2018-04-18 2018-09-25 陕西理工大学 一种基于蓝牙自平衡小车控制倒立摆实验装置
CN110161848A (zh) * 2019-03-12 2019-08-23 广东省智能制造研究所 一种基于时间自动机的一阶直线倒立摆控制方法及系统

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4681016B2 (ja) * 2008-03-10 2011-05-11 株式会社豊田中央研究所 倒立振子型車輪移動体

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101590323A (zh) * 2009-07-08 2009-12-02 北京工业大学 一种独轮机器人系统及其控制方法
CN105904456A (zh) * 2016-04-29 2016-08-31 西北工业大学 手持式力反馈装置以及利用该装置实现力觉再现的方法
CN108572590A (zh) * 2018-04-18 2018-09-25 陕西理工大学 一种基于蓝牙自平衡小车控制倒立摆实验装置
CN110161848A (zh) * 2019-03-12 2019-08-23 广东省智能制造研究所 一种基于时间自动机的一阶直线倒立摆控制方法及系统

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
基于MATLAB/Simulink的随动控制系统建模与仿真;王桂霞;《船舶工程》;20120615;第第34卷卷(第03期);第55页第3段-第57页第3段 *

Also Published As

Publication number Publication date
CN110568755A (zh) 2019-12-13

Similar Documents

Publication Publication Date Title
EP3342559A1 (en) Robot system
JP4043996B2 (ja) サーボモータ駆動制御装置
CN106671091B (zh) 一种机器人液压驱动转动关节闭环控制系统
CN101859096A (zh) 采用pid控制器的控制方法及控制装置与机器人
US9354651B2 (en) Sizing and tuning methodology for optimized motion control components and energy efficiency
CN108196442A (zh) 基于模糊神经pid控制和绝对编码器的舵机控制系统及方法
CN1932705A (zh) 一种高精度低功耗磁悬浮控制力矩陀螺框架伺服系统数字控制装置
CN104199460A (zh) 基于图像识别跟踪的四旋翼飞行器智能侦察系统
CN111736471A (zh) 一种旋转倒立摆的迭代反馈整定控制及其鲁棒优化方法
CN110568755B (zh) 一种基于低通滤波的倒立摆控制方法、装置及系统
CN109375584A (zh) 臂架关节控制方法及系统及包含该系统的工程机械
CN106773652A (zh) 一种pid系统及其参数自动调整方法
CN104459636A (zh) 多雷达天线协同控制方法
CN108983699B (zh) 基于plc的三轴数控钻床控制系统及其控制方法
CN103812392A (zh) 电机驱动控制器及其控制双电机的方法
CN208384428U (zh) 基于反向位移补偿的稳定转台系统
CN106385209B (zh) 电机角度控制方法、系统及无人机
Howimanporn et al. Comparison between PID and sliding mode controllers for rotary inverted pendulum using PLC
CN112019104B (zh) 一种步进电机驱动方法、步进电机驱动装置及小型机械臂
CN2900632Y (zh) 机电一体化挖掘机
JP2005094964A (ja) モータの制御装置及び制御方法
CN104271950B (zh) 马力限制装置及马力限制方法
CN111722636A (zh) 一种基于多新息卡尔曼滤波的自平衡车设计方案
CN113696328A (zh) 调节制砂楼给料速度的控制方法、控制装置和制砂楼
CN108382571B (zh) 一种基于嵌入式的墙面打孔及雕刻的四旋翼装置

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant