CN111273542B - 立方体机器人及控制系统和方法以及设计方法 - Google Patents
立方体机器人及控制系统和方法以及设计方法 Download PDFInfo
- Publication number
- CN111273542B CN111273542B CN202010064287.XA CN202010064287A CN111273542B CN 111273542 B CN111273542 B CN 111273542B CN 202010064287 A CN202010064287 A CN 202010064287A CN 111273542 B CN111273542 B CN 111273542B
- Authority
- CN
- China
- Prior art keywords
- cube
- angle
- inertia wheel
- angular velocity
- cubic
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 53
- 238000013461 design Methods 0.000 title claims abstract description 17
- 238000001914 filtration Methods 0.000 claims abstract description 37
- 230000010354 integration Effects 0.000 claims abstract description 10
- 238000005070 sampling Methods 0.000 claims description 32
- 230000004927 fusion Effects 0.000 claims description 12
- 230000001133 acceleration Effects 0.000 claims description 8
- 230000002159 abnormal effect Effects 0.000 claims description 7
- 238000013178 mathematical model Methods 0.000 claims description 5
- 230000007246 mechanism Effects 0.000 claims description 5
- 230000005484 gravity Effects 0.000 description 10
- 238000004422 calculation algorithm Methods 0.000 description 8
- 230000008859 change Effects 0.000 description 8
- 230000006870 function Effects 0.000 description 8
- 238000011161 development Methods 0.000 description 7
- 230000008569 process Effects 0.000 description 7
- 230000008878 coupling Effects 0.000 description 5
- 238000010168 coupling process Methods 0.000 description 5
- 238000005859 coupling reaction Methods 0.000 description 5
- 230000003044 adaptive effect Effects 0.000 description 4
- 230000000694 effects Effects 0.000 description 4
- 238000005381 potential energy Methods 0.000 description 4
- 238000013139 quantization Methods 0.000 description 4
- 238000010586 diagram Methods 0.000 description 3
- 230000002093 peripheral effect Effects 0.000 description 3
- 238000011105 stabilization Methods 0.000 description 3
- 238000009825 accumulation Methods 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000004069 differentiation Effects 0.000 description 2
- 230000004044 response Effects 0.000 description 2
- 230000006641 stabilisation Effects 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000012360 testing method Methods 0.000 description 2
- WHXSMMKQMYFTQS-UHFFFAOYSA-N Lithium Chemical compound [Li] WHXSMMKQMYFTQS-UHFFFAOYSA-N 0.000 description 1
- 230000002146 bilateral effect Effects 0.000 description 1
- 238000006243 chemical reaction Methods 0.000 description 1
- 238000004891 communication Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 238000002474 experimental method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 229910052744 lithium Inorganic materials 0.000 description 1
- 239000011159 matrix material Substances 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 238000012545 processing Methods 0.000 description 1
- 230000008439 repair process Effects 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
- 238000012795 verification Methods 0.000 description 1
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.
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02P—CLIMATE CHANGE MITIGATION TECHNOLOGIES IN THE PRODUCTION OR PROCESSING OF GOODS
- Y02P90/00—Enabling technologies with a potential contribution to greenhouse gas [GHG] emissions mitigation
- Y02P90/02—Total factory control, e.g. smart factories, flexible manufacturing systems [FMS] or integrated manufacturing systems [IMS]
Landscapes
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Engineering & Computer Science (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及立方体机器人及控制系统和方法以及设计方法，在立方体框架中集成所有部件，各部件之间的连接线或控制线均不伸出于立方体框架；由立方体角度PID控制器和惯性轮角速度自适应模糊PID控制器组成立方体惯性轮角速度及机体角度串级控制系统；外环控制器采用模糊自适应整定PID控制器，测得的惯性轮角速度值应用限幅滤波法得到惯性轮角速度值作为外环的反馈输入，输入惯性轮角速度自适应模糊PID控制器；将陀螺仪和加速度计测得的立方体倾斜角度数据经过一阶卡尔曼滤波姿态角度融合得到的机体角度值作为内环控制的反馈。能够解决目前立方体机器人系统完整性和集成度不高、电路集成度低及平衡控制自适应能力差、控制模型精度不够的问题。
Description
技术领域
本发明涉及智能机器人领域，尤其涉及一种立方体机器人及其模糊PID控制系统，以及控制系统的设计方法和应用。
背景技术
立方体机器人是一种结构和运动方式独特的自平衡机器人，以立方体棱边或者角点为支点实现立方体自平衡。立方体机器人通过内置的刹车装置，实现立方体全向翻转灵活运动。多个立方体机器人组装后可用于桥梁修补、太空探索等领域，具有重要的理论价值和广阔的发展前景。
现有立方体系统外设模块较为分散、部分电子器件位于立方体框架外，如，一些带有飞轮的立方体倒立摆系统，其供电模块、电机驱动模块均需要有排线引出到立方体框架外部，集中程度不高，特别是移动使用的时候控制不便。由于自平衡立方体外观体积及机载系统的限制，其系统设计需综合考虑整体性及实时性要求。此外，立方体机器人是一个复杂的非线性系统，目前常用的对立方体机器人动力学建模，分别采用拉格朗日方法和凯恩方法对自平衡立方体进行动力学模型，也有基于飞轮的单级倒立摆系统的建模与仿真方法，应用二次型最优调节器LQR实现对飞轮倒立摆系统的平衡控制。由于立方体机器人是一种非线性、强耦合、多变量的复杂系统，获取其精确的数学模型难度较大，此外立方体系统易受到外界的干扰，因此经典控制方法具有一定的局限性。
发明内容
本发明要解决的技术问题是，针对目前立方体机器人系统完整性和集成度不高、电路集成度低及平衡控制自适应能力差、精度不够的问题，提供一种内部全集成的立方体机器人；同时在此基础上，针对目前立方体机器人动力学模型基于经典控制方法建模不够精确的局限性，提供一种立方体机器人模糊PID控制系统及控制方法，以及控制系统的设计方法。
为解决上述技术问题，本发明采用如下技术方案：
一种内部全集成的立方体机器人，包括立方体框架；其特征在于：在立方体框架中集成所有部件，各部件之间的连接线或控制线均不伸出于立方体框架，所述部件至少包括：
通过电机固定件悬空固定在立方体框架其中一面中心的电机，
通过法兰联轴器固定在电机的轴上的惯性轮，所述惯性轮平行于所述立方体框架其中一面；
固定于立方体框架另一面上的电控板，电控板包括了姿态传感器、单片机控制板和直流电机伺服驱动器；
固定于立方体框架第三面上的电池固定盒，电池固定盒中容纳电池，电池与所述电控板电连接。
进一步的，惯性轮设置于电机固定件与所述立方体框架其中一面之间。
进一步的，立方体框架六面由位于棱角处的八个直角连接件固定。
一种立方体机器人模糊PID控制系统，其特征在于：由立方体角度PID控制器和惯性轮角速度自适应模糊PID控制器组成立方体惯性轮角速度及机体角度串级控制系统；惯性轮角速度自适应模糊PID控制器作为外环控制器，立方体角度PID控制器作为内环控制器；外环控制器采用模糊自适应整定PID控制器，利用模糊控制规则在线对PID参数进行修改，以满足不同时刻对系统外环PID参数自整定的要求；测得的惯性轮角速度值应用限幅滤波法得到惯性轮角速度值作为外环的反馈输入，输入惯性轮角速度自适应模糊PID控制器；惯性轮角速度自适应模糊PID控制器的输出值作为内环立方体角度PID控制器的输入，同时将陀螺仪和加速度计测得的立方体倾斜角度数据经过一阶卡尔曼滤波姿态角度融合得到的机体角度值作为内环控制的反馈，输入立方体角度PID控制器，最终将立方体角度PID控制器的输出值代入执行机构，控制惯性轮电机旋转而产生立方体自平衡所需的力矩。
一种立方体机器人模糊PID控制系统设计方法，其特征在于包括如下步骤：
S1，采用拉格朗日法对带有惯性轮的自平衡立方体建立数学模型；
S2：将立方体倾斜角度θ、惯性轮角速度
分别作为系统的内、外环的被控量，构造立方体惯性轮角速度及机体角度串级控制系统；惯性轮角速度外环采用模糊PID控制器，利用模糊控制规则在线对惯性轮角速度PID参数进行修改，以满足不同时刻对惯性轮角速度PID参数自整定的要求；立方体倾斜角度内环为内环立方体角度PID控制器；
S3：应用限幅滤波法剔除惯性轮角速度数据中的异常值得到准确的惯性轮角速度数据；将陀螺仪和加速度计测得的数据经过一阶卡尔曼滤波姿态角度融合得到的机体角度值作为内环立方体角度PID控制器的反馈；立方体角度PID控制器的输出值作为执行机构的输入数据，通过控制惯性轮电机旋转而控制立方体自平衡稳定所需要的力矩。
进一步的，步骤S3中，定时对陀螺仪和加速度计得到的机体角度值进行采样，应用一阶卡尔曼滤波进行立方体姿态角度融合，得到可信任的惯性轮自平衡立方体系统角度值作为内环控制的反馈。
进一步的，步骤S3中，应用一阶卡尔曼滤波从陀螺仪积分得到的机体角度数据和加速度计得到的机体倾斜角度中得到可信任的机体角度。
进一步的，步骤S3中，应用限幅滤波法剔除惯性轮角速度数据中的异常值的原理为：确定两次采样允许的最大偏差值，当前采样值与上一次采样值做差值运算，如果差值的绝对值小于最大偏差值，那么视当前采样值为一次正确的采样数据；如果差值的绝对值大于最大偏差值，那么视当前采样值为无效采样，把上一次采样值近似看作当前采样值。
一种立方体机器人控制方法，其特征在于：首先进行系统时钟初始化，各个模块初始化，最后初始化并开启定时中断，完成系统初始化设置，然后在定时中断中进行姿态传感器角速度、角加速度采集，完成一阶卡尔曼滤波姿态角度融合，得到可信任的自平衡立方体的机体角度值，并进行惯性轮角速度限幅滤波，最后实现基于模糊PID的惯性轮自平衡立方体控制。
相对于现有技术，本发明具有如下效果：
本发明的目的是为了解决现有立方体机器人机械结构系统完整性不高，电路集成度低及平衡控制自适应能力差等问题，设计了一种内部高度集成的自平衡立方体结构、并实现了基于模糊PID的惯性轮立方体控制系统。通过机载运行，验证了所设计惯性轮立方体机器人的有效控制。
(1)自平衡立方体系统的所有器件均位于立方体框架内部，无任何其他器件引出到立方体框架外，由此体现了系统的完整性，有效减少了实际运行过程中外界因素对于系统的干扰，利于立方体机器人的翻转运行和二次开发；此外，考虑到硬件开发成本问题，本系统中采用直流有刷电机替代常用的无刷电机，有效降低开发成本。
(2)针对立方体机器人非线性、多变量、强耦合、非自稳的特点，设计了惯性轮角速度模糊PID—机体角度PID串级控制系统。首先，采用拉格朗日法对自平衡立方体建立数学模型；然后，将自平衡立方体角度、惯性轮角速度分别作为系统的内、外环的被控量，构造了立方体惯性轮角速度—机体角度串级控制系统。角速度外环采用模糊PID控制器，提高了PID控制系统的自适应性，利用模糊控制规则在线对惯性轮角速度PID参数进行修改，以满足不同时刻对惯性轮角速度PID参数自整定的要求，控制算法简单，易于机载在线运行。有效避免了对精确模型的依赖，提高了系统的抗扰性能，实现了系统稳、准、快的控制目标。
(3)设计了立方体一阶卡尔曼滤波姿态角度融合算法和惯性轮角速度限幅滤波算法。
在自平衡立方体系统实际应用中，综合考虑带宽，噪声等因素的影响，应用一阶卡尔曼滤波从陀螺仪积分得到的机体角度数据和角速度计得到的机体倾斜角度中得到可信任的机体角度；应用限幅滤波法剔除惯性轮角速度数据中的异常值得到准确的惯性轮角速度数据。准确的机体角度数据和惯性轮角速度数据做为系统的反馈，使控制系统的输出更加精确，电机的输出更加平稳、光滑，防止系统的抖动，提高系统的性能。
(4)实际应用是机载控制系统，验证了算法的有效性，避免算法无法传输而引起的时间时滞和信号干扰，保证实时控制，提高系统的精度。
(5)本发明的带有惯性轮的自平衡立方体机器人基于以上优势，可以作为高校科技活动的教学、实验、验证平台；惯性轮平衡控制方式，可以不依赖于航天器的推力发动机来改变机体姿态，通过惯性轮的控制，实现对航天器姿态的控制；惯性轮立方体利用内置的动力装置和机械执行装置，可以实现立方体的全向移动能力，可应用于探测器中，实现太空探索功能；利用立方体的自稳定性，类似空中机器人一样，将其进行编制队伍，在各类科技文化节中进行活动表演，可以很好的促进观众对科技文化的热爱，激发观众的热情；同时，立方体依靠惯性轮旋转来控制自身的平衡，依靠简单的控制操作实现立方体的平衡，在平衡时具有不倒翁类似的体验感，适合开发成为一种玩具。
附图说明
图1为根据本发明方法实施的内部全集成的立方体机器人结构图示意图。
图2为图1的局部拆分图。
图3为本发明的立方体机器人坐标系示意图。
图4为本发明基于模糊PID的惯性轮立方体机器人控制系统原理框图。
图5为e、ec隶属函数曲线图。
图6Δkp、Δki、Δkd隶属函数曲线图。
图7为本发明模糊自适应整定PID控制算法流程图。
图8为本发明陀螺仪所得角度值、角速度计所得角度值及一阶卡尔曼滤波姿态角度融合所得曲线图。
图9为编码器测得角速度滤波前后数据对比图。
图10为立方体稳态时的机体角度跟随曲线。
图11为立方体稳态时的惯性轮角速度输出曲线。
图12扰动情况下立方体机体角度跟随曲线。
图13扰动情况下惯性轮角速度输出曲线。
图14不同系统参数下立方体机体角度跟随曲线。
图15不同系统参数下惯性轮角速度输出曲线。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图1-12及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
需要指出，根据实施的需要，可将本申请中描述的各个步骤/部件拆分为更多步骤/部件，也可将两个或多个步骤/部件或者步骤/部件的部分操作组合成新的步骤/部件，以实现本发明的目的。
根据本发明实施的一种内部全集成的立方体机器人如图1和2所示，包括六面立方体框架1；其特征在于：在立方体框架1中集成所有部件；立方体机器人至少包括2为直角连接件，3为电控板，4为电机，5为电池固定盒，6为电池固定盒5中设置的电池(本实施例选用12V锂电池)，7为电机固定件，8为法兰联轴器，9为惯性轮。立方体框架1六面由位于棱角处的八个直角连接件2固定，组成立方体框架；惯性轮9通过法兰联轴器8固定在电机4的轴上；电机4通过电机固定件7固定在立方体框架其中一面上；电控板3通过固定件31固定在立方体框架1上，电控板3高度集成，包括了姿态传感器、单片机控制板和直流电机伺服驱动器；电池固定盒5固定在立方体框架一面上，电池6位于电池固定盒5内。
姿态传感器(一般是陀螺仪和加速度计)用于检测倒立摆的倾转角和角加速度，单片机控制板(本实施例基于STM32F103RCT6单片机最小系统)通过无线通信远程与外部操控器或上位机通信，接收姿态传感器的数据，经运算后控制伺服驱动器驱动电机4转动，进而带动惯性轮9回转产生惯性反作用力矩控制立方体机器人的姿态平衡。本实施例中，姿态传感器选用MPU6050姿态传感器，它由陀螺仪和加速度计组成。
立方体机器人框架内部接线，无任何其他器件引出到立方体框架外，由此体现了系统的完整性，利于立方体机器人的翻转运行和二次开发；此外，考虑到硬件开发成本问题，本系统中采用直流有刷电机替代常用的无刷电机，有效降低开发成本。
一种立方体机器人模糊PID控制系统及设计方法，包括以下步骤：
S1：采用拉格朗日法建立数学模型。
如下图3所示。其中M为立方体机器人(包括框架及内部电池和所有元器件，后简称立方体)的总质量，立方体重心G的位置坐标为(X1,Y1)，立方体与地面接触点等效为坐标系原点O进行转动，转动惯量为I1；m1为惯性轮的质量，惯性轮重心P的位置坐标为(X2,Y2)，惯性轮转过的角度为β，惯性轮绕自身重心P的转动惯量为I2。立方体相对于纵轴的偏转角或立方体的倾斜角度为θ，坐标系原点O与立方体重心G的距离为l1，与惯性轮重心P的距离为l2，采用拉格朗日建模法得到如下系统模型。
本实施例中立方体实物的相关参数如表1所示。
表1自平衡立方体参数
| 参数名称 | 参数符号 | 参数数值 |
| 坐标原点O与立方体框架重心P的距离 | l<sub>1</sub> | 0.062m |
| 坐标原点O与惯性轮重心G的距离 | l<sub>2</sub> | 0.085m |
| 立方体绕坐标原点O的转动惯量 | I<sub>1</sub> | 1.754×10<sup>-3</sup>kgm<sup>2</sup> |
| 惯性轮绕自身重心P的转动惯量 | I<sub>2</sub> | 2.061×10<sup>-4</sup>kgm<sup>2</sup> |
| 立方体质量 | M | 0.685kg |
| 惯性轮质量 | m | 0.103kg |
| 立方体绕坐标原点转动的摩擦阻力系数 | k<sub>1</sub> | 0.013 |
| 惯性轮绕转轴转动的摩擦阻力系数 | k<sub>2</sub> | 0.0008 |
为了便于数学推导和处理，作出如下假设：
(1)立方体框架在运动中是不变形的刚体；
(2)忽略空气阻力；
拉格朗日运动方程为：
式中L为拉格朗日函数，Q为系统的广义力，D为系统的耗散能，q为系统的广义坐标，其中，
L＝T-V (2)
T是系统的总动能，包括平动动能和转动动能，V是系统的总势能。
那么，可以根据分析得出：
(1)立方体的动能为：
(2)惯性轮的动能为：
(3)立方体的势能为：
V1＝Mgl1cosθ (5)
(4)惯性轮的势能为：
V2＝mgl2cosθ (6)
(5)整个立方体系统的总动能为：
(6)整个立方体系统的总势能为：
V＝V1+V2＝Mgl1cosθ+mgl2cosθ (8)
(7)拉格朗日函数为：
(8)系统的广义坐标为θ、β，系统的广义力矩为
f＝[0,u]T (10)
其中，u为惯性轮的驱动力矩。
(9)立方体的耗散力做功以及惯性轮的耗散力分别为：
其中，k1为自平衡立方体绕坐标原点的阻力系数，k2为惯性轮自转阻力系数。综合上式，由此可得惯性轮自平衡立方体的动力学方程为：
当θ较小时，可根据泰勒级数展开做近似处理，sinθ的泰勒展开式为
取泰勒级数展开式的第一项，即sinθ≈θ，并代入式(12)，解得：
选取系统的状态变量为
从而得系统的状态方程为：
将立方体系统参数代入系统的状态空间方程得：
S2:惯性轮角速度模糊PID—机体角度PID串级控制系统设计
根据式(16)可知，自平衡立方体系统有四个输入变量，同时对四个变量进行控制，控制器设计复杂，控制难度也较大。由于四个输入变量中，立方体角速度的积分为立方体角度，惯性轮角速度积分为惯性轮角度，因此可选择立方体角度PID控制器和惯性轮角速度PID控制器，同时将设计的两个控制器组成串级控制系统，惯性轮角速度PID控制器作为外环，立方体角度PID控制器作为内环，实现了惯性轮转速控制和机体平衡控制。
由于常规的PID算法对于立方体系统控制过程中负载的变化和外界的干扰难以保证姿态的收敛、系统的平稳，对于控制系统，外环是定值控制系统，内环是随动系统，对于惯性轮角速度的控制尤为重要，因此外环控制器采用模糊自适应整定PID控制器，利用模糊控制规则在线对PID参数进行修改，以满足不同时刻对系统外环PID参数自整定的要求。精确控制的前提是获取精确的立方体角度数据和惯性轮角速度数据，因此对于自平衡立方机体角度数据，采用的方法是将陀螺仪和加速度计采集的数据进行一阶卡尔曼滤波得到一个较为精确的机体角度值；对于惯性轮角速度数据采取限幅滤波法除去测量噪声产生的异值。控制系统设计如图4所示。
惯性轮角速度自适应模糊PID控制器以角速度误差e和误差变化ec作为输入，找出PID三个参数与e和ec之间的模糊关系，在运行中通过不断检测e和ec，根据模糊控制规则来对三个参数进行在线修改，以满足不同时刻的e和ec对PID控制参数的不同需求，而使被控对象有良好的动、静态性能。
惯性轮角速度输入量偏差e及偏差变化率ec通过量化因子Ke、Kec进行一次量化处理，输出量Δkp、Δki、Δkd经过解模糊化比例因子k1、k2、k3进行一次比例变换。取惯性轮角速度输入量偏差e和偏差变化率ec，输出量Δkp、Δki、Δkd模糊子集为{NB,NM,NS,Z,PS,PM,PB}，分别代表负大，负中，负小，零，正小，正中，正大。论域为[-3,3]，量化等级为{-3，-2，-1，0，1，2，3}。输入量偏差e和偏差变化率ec隶属函数选为双边高斯型，输出量Δkp、Δki、Δkd隶属函数选为三角形，使用重心法进行解模糊：
式(17)中，ucen为面积中心对应的横坐标，A(ui)为ui处的隶属度，n为相关隶属度的个数。系统的输入输出的函数隶属度曲线如图5、图6所示。
根据惯性轮角速度环kp、ki、kd三个参数的作用、相互之间的互连关系及对输出特性的影响，可以归纳出在不同e和ec的情况下，惯性轮角速度环kp、ki、kd的自整定要求，从而可得模糊规则：
1.if(e is NB)and(ec is NB)then(Δkpis PB)(Δkiis NB)(Δkdis PS)
2.if(e is NB)and(ec is NM)then(Δkpis PB)(Δkiis NB)(Δkdis NS)
3.if(e is NB)and(ec is NS)then(Δkpis PM)(Δkiis NM)(Δkdis NB)
…
49.If(e is PB)and(ec is PB)then(Δkpis NB)(Δkiis PB)(Δkdis PB)
上述49条模糊控制规则形成一个模糊控制规则矩阵，根据模糊输入量决策出模糊输出量Δkp、Δki、Δkd。代入式(16)，求出系统最终的控制参数惯性轮角速度环kp、ki、kd。
式中，kp0、ki0、kd0为PID初始值，可由工程整定法获得。
S3：立方体一阶卡尔曼滤波姿态角度融合、惯性轮角速度限幅滤波
S31：自平衡立方体姿态角度融合
自平衡立方体姿态角度传感器选用MPU6050姿态传感器，它由陀螺仪和加速度计组成。
陀螺仪是一种检测角速度变化的角速度传感器，假设陀螺仪测得的机体角速度为
则立方体的倾斜角度θ为：
通过式(17)求得的机体角度随着时间的增加，陀螺仪的积分漂移以及温度漂移使机体角度值产生较大的误差累积，但是在短周期内得到的角度值与实际值较为符合。
加速度计对立方体的角加速度变化很灵敏，由于立方体角加速度仅受重力加速度的影响，假设加速度计测得倾斜角加速度在加速度计的X轴和Y轴上的分量分别是Ax和Ay，则立方体的倾斜角度θ为：
由于加速度计受白噪声干扰影响较大，如果直接读取加速度计的瞬时值，由此作为倾角计算出的误差会比较大，不能进行精确的控制，但是在长周期时间选择加速度计比陀螺仪更加准确。
因此，本发明中，根据实际工程经验，定时对加速度计得到的机体角度值进行采样，应用一阶卡尔曼滤波进行立方体姿态角度融合，得到可信任的惯性轮自平衡立方体系统角度值。
S32：惯性轮角速度限幅滤波
惯性轮测速编码器安装在电机上或是采用自带编码器的电机，对电机转速进行采样，由于电机驱动惯性轮转动，惯性轮转速与电机转速相等，因此电机测速编码器的采样值即为惯性轮角速度值。由于编码器采样过程中会产生随机的异值信号，需要把采样值中的错误数据剔除修改，本发明采用的方法是根据实际工程经验，确定两次采样允许的最大偏差值，当前采样值与上一次采样值做差值运算，如果差值的绝对值小于最大偏差值，那么视当前采样值为一次正确的采样数据；如果差值的绝对值大于最大偏差值，那么视当前采样值为无效采样，把上一次采样值近似看作当前采样值。
如图7所示，基于模糊PID的惯性轮立方体机器人控制原理是：电机编码器测得的惯性轮角速度值应用限幅滤波法得到的惯性轮角速度值作为外环的反馈输入，带入惯性轮角速度自适应模糊PID控制器，将惯性轮角速度自适应模糊PID控制器的输出值作为内环立方体角度控制器的输入，同时将陀螺仪和角速度计测得的数据经过一阶卡尔曼滤波姿态角度融合得到的机体角度值做为内环控制的反馈，代入内环立方体角度PID控制器，最终将立方体角度控制器的输出值代入执行机构，控制惯性轮电机旋转产生立方体自稳定控制所需要的力矩。
立方体机器人自平衡控制过程为：基于STM32F103RCT6单片机最小系统，首先进行系统时钟初始化，各个模块初始化，最后初始化并开启定时中断，完成系统初始化设置，然后在定时中断中进行姿态传感器角速度、角加速度采集，完成一阶卡尔曼滤波姿态角度融合，得到可信任的自平衡立方体的机体角度值，惯性轮角速度限幅滤波，最后实现基于模糊PID的惯性轮自平衡立方体控制。
经过试验调试，陀螺仪所得角度值、角速度计所得角度值及一阶卡尔曼滤波姿态角度融合所得角度值如图8所示。由图8可知，经过一阶卡尔曼滤波姿态角度融合得到的立方体机体角度值更接近于实际立方体机体角度值，减少了陀螺仪所得角度值的误差累积，降低了加速度计所得角度值的噪声干扰。
设定惯性轮角速度恒定时，编码器测得角速度滤波前后数据对比如图9所示。由图9可知，经过限幅滤波后的惯性轮角速度采样值可以消除采集过程中的异常值。
根据立方体实际系统，惯性轮取量化因子Ke＝260，Kec＝500，解模糊化因子k1＝5，k2＝2，k3＝0.5。经过试验调试，采样周期为5ms，立方体稳态时的机体角度跟随和惯性轮角速度输出曲线如图10、图11所示。由图10、图11可知，立方体在稳定时，惯性轮的角速度在0附近波动，机体角度值在设定值附近小幅度波动，立方体具有良好的稳定性。
系统在静态稳定过程中，给立方体一个瞬间的干扰，使其机体角度值发生变化，系统的机体角度跟随和惯性轮角速度波形抗扰输出响应如图12、图13所示。由图12、图13可知，对于系统稳定过程中突然出现的外部干扰，立方体系统能快速响应并调整稳定到平衡点，具有一定的抗干扰能力。
系统在稳态过程中，给立方体的立方体框架上悬挂一个重物，改变立方体框架的质量，系统的输出响应如图14、15所示。由图14、图15可知，基于模糊PID的惯性轮立方体机器人控制系统设计，在系统参数发生变化时，仍然对系统的平衡控制具有良好的适应性，验证了控制算法对系统参数发生变化时有良好的鲁棒性。
本发明主要是围绕着惯性轮立方体机器人系统自稳控制，设计了一种基于模糊PID的惯性轮立方体机器人控制系统，实现了对惯性轮立方体机器人的快速稳定控制目标，本专利具有较广泛的推广价值。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
Claims (9)
1.一种立方体机器人模糊PID控制系统，其特征在于：由立方体角度PID控制器和惯性轮角速度自适应模糊PID控制器组成立方体惯性轮角速度及机体角度串级控制系统；惯性轮角速度自适应模糊PID控制器作为外环控制器，立方体角度PID控制器作为内环控制器；外环控制器采用模糊自适应整定PID控制器，利用模糊控制规则在线对PID参数进行修改，以满足不同时刻对系统外环PID参数自整定的要求；测得的惯性轮角速度值应用限幅滤波法得到惯性轮角速度值作为外环的反馈输入，输入惯性轮角速度自适应模糊PID控制器；惯性轮角速度自适应模糊PID控制器的输出值作为内环立方体角度PID控制器的输入，同时将陀螺仪和加速度计测得的立方体倾斜角度数据经过一阶卡尔曼滤波姿态角度融合得到的机体角度值作为内环控制的反馈，输入立方体角度PID控制器，最终将立方体角度PID控制器的输出值代入执行机构，控制惯性轮电机旋转而产生立方体自平衡所需的力矩。
2.一种内部全集成的立方体机器人，包括立方体框架；其特征在于：集成有权利要求1所述的立方体机器人模糊PID控制系统；
在立方体框架中集成所有部件，各部件之间的连接线或控制线均不伸出于立方体框架，所述部件至少包括：
通过电机固定件悬空固定在立方体框架其中一面中心的电机，
通过法兰联轴器固定在电机的轴上的惯性轮，所述惯性轮平行于所述立方体框架其中一面；
固定于立方体框架另一面上的电控板，电控板包括了姿态传感器、单片机控制板和直流电机伺服驱动器；
固定于立方体框架第三面上的电池固定盒，电池固定盒中容纳电池，电池与所述电控板电连接。
3.根据权利要求2所述的内部全集成的立方体机器人，其特征在于：惯性轮设置于电机固定件与所述立方体框架其中一面之间。
4.根据权利要求2所述的内部全集成的立方体机器人，其特征在于：立方体框架六面由位于棱角处的八个直角连接件固定。
5.一种立方体机器人模糊PID控制系统设计方法，其特征在于包括如下步骤：
S1，采用拉格朗日法对带有惯性轮的自平衡立方体建立数学模型；
S2：将立方体倾斜角度
、惯性轮角速度
分别作为系统的内、外环的被控量，构造立方
体惯性轮角速度及机体角度串级控制系统；惯性轮角速度外环采用模糊PID控制器，利用模
糊控制规则在线对惯性轮角速度PID参数进行修改，以满足不同时刻对惯性轮角速度PID参
数自整定的要求；立方体倾斜角度内环为内环立方体角度PID控制器；
S3：应用限幅滤波法剔除惯性轮角速度数据中的异常值得到准确的惯性轮角速度数据；将陀螺仪和加速度计测得的数据经过一阶卡尔曼滤波姿态角度融合得到的机体角度值作为内环立方体角度PID控制器的反馈；立方体角度PID控制器的输出值作为执行机构的输入数据，通过控制惯性轮电机旋转而控制立方体自平衡稳定所需要的力矩。
6.根据权利要求5所述的立方体机器人模糊PID控制系统设计方法，其特征在于步骤S3中，定时对陀螺仪和加速度计得到的机体角度值进行采样，应用一阶卡尔曼滤波进行立方体姿态角度融合，得到可信任的惯性轮自平衡立方体系统角度值作为内环控制的反馈。
7.根据权利要求5所述的立方体机器人模糊PID控制系统设计方法，其特征在于步骤S3中，应用一阶卡尔曼滤波从陀螺仪积分得到的机体角度数据和加速度计得到的机体倾斜角度中得到可信任的机体角度。
8.根据权利要求5所述的立方体机器人模糊PID控制系统设计方法，其特征在于步骤S3中，应用限幅滤波法剔除惯性轮角速度数据中的异常值的原理为：确定两次采样允许的最大偏差值，当前采样值与上一次采样值做差值运算，如果差值的绝对值小于最大偏差值，那么视当前采样值为一次正确的采样数据；如果差值的绝对值大于最大偏差值，那么视当前采样值为无效采样，把上一次采样值近似看作当前采样值。
9.一种立方体机器人控制方法，其特征在于：集成有权利要求1所述的立方体机器人模糊PID控制系统，首先进行系统时钟初始化，各个模块初始化，最后初始化并开启定时中断，完成系统初始化设置，然后在定时中断中进行姿态传感器角速度、角加速度采集，完成一阶卡尔曼滤波姿态角度融合，得到可信任的自平衡立方体的机体角度值，并进行惯性轮角速度限幅滤波，最后实现基于模糊PID的惯性轮自平衡立方体控制。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010064287.XA CN111273542B (zh) | 2020-01-20 | 2020-01-20 | 立方体机器人及控制系统和方法以及设计方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010064287.XA CN111273542B (zh) | 2020-01-20 | 2020-01-20 | 立方体机器人及控制系统和方法以及设计方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111273542A CN111273542A (zh) | 2020-06-12 |
| CN111273542B true CN111273542B (zh) | 2023-03-10 |
Family
ID=71003406
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010064287.XA Active CN111273542B (zh) | 2020-01-20 | 2020-01-20 | 立方体机器人及控制系统和方法以及设计方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111273542B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113110025A (zh) * | 2021-04-08 | 2021-07-13 | 深兰科技（上海）有限公司 | 机器人的行进控制方法、系统、电子设备及存储介质 |
| CN113547507B (zh) * | 2021-06-29 | 2024-09-03 | 武汉科技大学 | 一种可翻转立方体机器人及其建模方法和控制方法 |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103010360A (zh) * | 2011-09-26 | 2013-04-03 | 东莞易步机器人有限公司 | 一种自平衡两轮车的运动控制方法 |
| CN105259902A (zh) * | 2015-09-06 | 2016-01-20 | 江苏科技大学 | 水下机器人惯性导航方法及系统 |
| CN105404296A (zh) * | 2015-10-28 | 2016-03-16 | 南京师范大学 | 一种两轮自平衡智能车姿态控制方法 |
| CN206601625U (zh) * | 2017-01-17 | 2017-10-31 | 北京工业大学 | 一种立方体飞轮倒立摆系统 |
| CN207895756U (zh) * | 2017-09-22 | 2018-09-21 | 北京工业大学 | 一种三飞轮立方体倒立摆系统 |
| CN109709796A (zh) * | 2018-12-20 | 2019-05-03 | 武汉科技大学 | 一种惯性轮自平衡方法 |
| KR101988835B1 (ko) * | 2018-12-12 | 2019-06-12 | 이원석 | 움직임 가능한 큐브형 마우스 |
| CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
| CN110524510A (zh) * | 2019-06-12 | 2019-12-03 | 北京工业大学 | 一种立方体独轮机器人 |
-
2020
- 2020-01-20 CN CN202010064287.XA patent/CN111273542B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103010360A (zh) * | 2011-09-26 | 2013-04-03 | 东莞易步机器人有限公司 | 一种自平衡两轮车的运动控制方法 |
| CN105259902A (zh) * | 2015-09-06 | 2016-01-20 | 江苏科技大学 | 水下机器人惯性导航方法及系统 |
| CN105404296A (zh) * | 2015-10-28 | 2016-03-16 | 南京师范大学 | 一种两轮自平衡智能车姿态控制方法 |
| CN206601625U (zh) * | 2017-01-17 | 2017-10-31 | 北京工业大学 | 一种立方体飞轮倒立摆系统 |
| CN207895756U (zh) * | 2017-09-22 | 2018-09-21 | 北京工业大学 | 一种三飞轮立方体倒立摆系统 |
| KR101988835B1 (ko) * | 2018-12-12 | 2019-06-12 | 이원석 | 움직임 가능한 큐브형 마우스 |
| CN109709796A (zh) * | 2018-12-20 | 2019-05-03 | 武汉科技大学 | 一种惯性轮自平衡方法 |
| CN110524510A (zh) * | 2019-06-12 | 2019-12-03 | 北京工业大学 | 一种立方体独轮机器人 |
| CN110146077A (zh) * | 2019-06-21 | 2019-08-20 | 台州知通科技有限公司 | 移动机器人姿态角解算方法 |
Non-Patent Citations (2)
| Title |
|---|
| 立方体机器人自抗扰平衡控制方法;陈志刚等;《控制与决策》;20180615(第06期);85-92 * |
| 自平衡立方体机器人动力学建模;陈志刚等;《北京工业大学学报》;20171220(第03期);62-67 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111273542A (zh) | 2020-06-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Dong et al. | A dynamic model and control method for a two-axis inertially stabilized platform | |
| CN109240305B (zh) | 基于互补滤波的两轮机器人运动控制系统及方法 | |
| Juang et al. | Design and control of a two-wheel self-balancing robot using the arduino microcontroller board | |
| Nawawi et al. | Real-time control of a two-wheeled inverted pendulum mobile robot | |
| Xu et al. | Reliable tracking control for under-actuated quadrotors with wind disturbances | |
| CN111258216B (zh) | 一种适用于四旋翼飞行器的滑模重复控制器 | |
| CN107479567A (zh) | 动态特性未知的四旋翼无人机姿态控制器及方法 | |
| Yu | Nonlinear PD regulation for ball and beam system | |
| Derafa et al. | Super twisting control algorithm for the four rotors helicopter attitude tracking problem | |
| CN111273542B (zh) | 立方体机器人及控制系统和方法以及设计方法 | |
| González et al. | Real-time attitude stabilization of a mini-uav quad-rotor using motor speed feedback | |
| Sukvichai et al. | Double-level ball-riding robot balancing: From system design, modeling, controller synthesis, to performance evaluation | |
| Emam et al. | Attitude tracking of quadrotor UAV via mixed H2/H∞ controller: An LMI based approach | |
| Shankaran et al. | Improved performance in quadrotor trajectory tracking using MIMO PI λ-D control | |
| Wang et al. | Adaptive trajectory tracking control of a wheeled mobile robot via Lyapunov techniques | |
| Engin | Embedded LQR controller design for self-balancing robot | |
| Kugelberg | Black-box modeling and attitude control of a quadcopter | |
| Schulken et al. | Model-free sliding mode control algorithms including application to a real-world quadrotor | |
| Bicego et al. | Nonlinear model predictive control with actuator constraints for multi-rotor aerial vehicles | |
| Tang et al. | Platform and State Estimation Design of a Small‐Scale UAV Helicopter System | |
| Li et al. | A Nonlinear Trajectory Tracking Control Strategy for Quadrotor With Suspended Payload Based on Force Sensor | |
| Elhennawy | Dynamic modeling and robust nonlinear control of unmanned quadrotor vehicle | |
| Lv et al. | Research and Design of Four Rotor UAV Based on Cascade PID | |
| Jing-Liang et al. | Optimal robust control for attitude of quad-rotor aircraft based on SDRE | |
| Kang et al. | Quaternion based nonlinear trajectory control of quadrotors with guaranteed stability |
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 |