CN110379285B - 一种四棱锥立体倒立摆装置及控制方法 - Google Patents

Abstract

一种四棱锥立体倒立摆装置及控制方法，包括倒立的四棱锥框架、惯性传感器以及解算器，其中，倒立的四棱锥框架的每个侧面上设置有一个飞轮，并且飞轮与所在侧面平行，并且飞轮的圆心位于等腰三角形的侧面的中线上，每个飞轮连接有一个能够正反调速的电机，在倒立的四棱锥框架的顶面上设置有惯性传感器和解算器，惯性传感器与解算器相连。采用四棱锥的结构以及四个飞轮协同控制姿态的控制方法，使得传感器的设置可以只在倒四棱锥的顶面上设置一个惯性传感器来获取姿态信息，节约了成本。利用该倒立摆研究四旋翼无人机的控制方法，体现了较强的针对性。本发明结构简单，易于实现。

Description

一种四棱锥立体倒立摆装置及控制方法

技术领域

本发明涉及一种倒立摆，具体涉及一种四棱锥立体倒立摆装置及控制方法。

背景技术

倒立摆装置作为一种控制学的教具，已经被广泛应用于各种控制算法仿真验证实验中，对控制算法的研究工作具有很大的帮助。

传统的倒立摆通过控制算法的不断研究和发展，出现了直线型、旋转型等多种形式的倒立摆，并且在阶数方面，已经研究到了五六级倒立摆倒立控制的程度。但传统倒立摆都是在单一自由度上进行控制和研究的，无法通过其研究多自由度机电系统的耦合控制和多自由度协同控制等方法。

而前人所发明的cubli立方体三维倒立摆，打破了倒立摆单自由度控制的限制，可以由立方体三个两两相邻的三个面上所安装的转矩控制飞轮(动量轮)驱动时产生的反转矩，协同起来，控制三维倒立摆在三个旋转自由度方向上的角度。具体控制为在驱动的三个面上各安装一个角度传感器，三个面独立控制，使得三个面各自达到平衡位置时，立方体就可以以三个面的交点为支点，倒立在任意水平面上。但该技术通过自由度解耦，三个自由度由三个驱动器独立控制，并不涉及多自由度的耦合控制，并且无法与实际生活生产中的机电系统的控制形成紧密的联系。

可见，目前传统倒立摆存在耦合控制欠缺、缺乏针对性等问题。

发明内容

本发明针对现有技术中的问题，目的在于提出了一种四棱锥立体倒立摆装置及控制方法。

为实现上述目的，本发明采用如下的技术方案：

一种四棱锥立体倒立摆装置，包括倒立的四棱锥框架、惯性传感器以及解算器，其中，倒立的四棱锥框架的每个侧面上设置有一个飞轮，并且飞轮与所在侧面平行，并且飞轮的圆心位于等腰三角形的侧面的中线上，每个飞轮连接有一个能够正反调速的电机，在倒立的四棱锥框架的顶面上设置有惯性传感器和解算器，惯性传感器与解算器相连。

本发明进一步的改进在于，电机连接有供电系统。

本发明进一步的改进在于，供电系统包括电池以及驱动电路，电池与驱动电路相连，驱动电路与电机相连。

本发明进一步的改进在于，惯性传感器和解算器位于顶面的中心；解算器连接有控制器，控制器连接有混控器，混控器与飞轮相连。

一种四棱锥立体倒立摆装置的控制方法，通过惯性传感器采集位置、角度方向上的状态信息，并反馈给解算器，解算器通过姿态解算算法，获得倒立摆在空间三个旋转自由度上的角度信息，并将角度信息传输给控制器，控制器通过混控器调节四个飞轮。

本发明进一步的改进在于，姿态解算算法为互补滤波算法或卡尔曼滤波算法。

本发明进一步的改进在于，通过惯性传感器获取倒立摆整体的三维状态信息，并反馈给解算器，解算器采用姿态解算算法建立起机体坐标系以及世界坐标系，得到倒立摆当前的姿态角度以及角速度，并将得到的倒立摆当前的姿态角度以及角速度反馈给控制器，控制器根据倒立摆当前的姿态角度以及角速度计算，得到俯仰控制量、横滚控制量与偏航控制量，并将俯仰控制量、横滚控制量与偏航控制量传输给混控器，混控器通过转换矩阵，将俯仰控制量、横滚控制量与偏航控制量分配给四个电机。

本发明进一步的改进在于，当倒立摆整体位于初始位置时，启动起跳阶段，或者将倒立摆的姿态调整为接近平衡位置，接近平衡位置与平衡状态有小角度误差，其中，小角度的正弦值等于其弧度值。

本发明进一步的改进在于，当倒立摆一个侧面着地时，通过控制与着地面相邻的两侧面上的飞轮实现倒立摆的起跳，使得倒立摆旋转至平衡位置。

本发明进一步的改进在于，通过控制与着地面相邻的两侧面上的飞轮实现倒立摆的起跳，使得倒立摆旋转至平衡位置的具体过程为：将与着地面相邻两侧面上的飞轮加速到高速旋转状态，然后将电机停转，将动能转化为重力势能，使倒立摆跳起，接近平衡位置；其中，高速旋转状态时的速度为：平衡时倒立摆的重力势能等于飞轮旋转时的动能时的速度。

与现有技术相比，本发明具有的有益效果：一是针对立体倒立摆的各个自由度，都需要采用多个控制电机和飞轮耦合控制，本发明的四棱锥立体倒立摆装置可以作为研究耦合控制的教具；二是四棱锥倒立摆由四个飞轮协同控制，与四旋翼无人机结构和控制逻辑相似，控制思想可以仿照四旋翼无人机的控制方法，同样也可以利用该倒立摆研究四旋翼无人机的控制方法，体现了较强的针对性；三是采用四棱锥的结构，以及四个飞轮协同控制姿态的控制方法，使得传感器的设置可以只在倒四棱锥的顶面上设置一个惯性传感器来获取姿态信息，而不需要像现有的立体倒立摆那样，每个控制平面上都要安装一个传感器，节约了成本。本发明结构简单，易于实现。

本发明在控制时，通过惯性传感器采集位置、角度方向上的状态信息，并反馈给解算器，解算器通过姿态解算算法，获得倒立摆在空间三个旋转自由度上的角度信息，并将角度信息传输给控制器，控制器通过混控器调节四个飞轮，从而实现倒立摆的平衡。

附图说明

图1为本发明的倒立摆装置的立体视图。

图2为倒立摆装置平衡状态的示意图。

图3为倒立摆装置初始状态的示意图。

图4为本发明的倒立摆装置的仰视图。

图5为本发明中处理平衡状态时的立体示意图。

图6为本发明的控制框图。

图中，1为四棱锥框架，2为飞轮，2-1为第一飞轮，2-2为第二飞轮，2-3为第三飞轮，2-4为第四飞轮，3为电机，4为惯性传感器，6为水平面。

具体实施方式

下面结合附图对本发明进行详细说明。

由图1和图2所示，本发明主要包括：倒立的四棱锥框架1、四个飞轮2、四个可正反调速的电机3、惯性传感器4、解算器以及供电系统，倒立的四棱锥框架1的每个侧面(即斜面)上设置有一个飞轮2，每个飞轮2与四棱锥框架1的每个侧面平行。并且飞轮2的圆心安装在斜面等腰三角形的中线上，且每个飞轮的位置相同，每个飞轮2连接有一个可正反调速的驱动电机3，在倒立的四棱锥框架1的顶面上设置有惯性传感器4和解算器，供电系统包括电池以及驱动电路，电池与驱动电路相连，驱动电路与电机3相连并驱动电机3转动。本发明中的惯性传感器为九轴惯性传感器。

参见图4，本发明中的飞轮2包括4个，分别为第一飞轮2-1、第二飞轮2-2、第三飞轮2-3和第四飞轮2-4。由四个飞轮的驱动所产生的反转矩，为四棱锥倒立摆提供各个旋转自由度上的转矩(角加速度)，以此来调整四棱锥倒立摆倒立时的角度，实现四棱锥倒立摆在任意水平面6上以自身椎体顶点为支点的稳定倒立控制。

本发明在控制时可参考多旋翼无人机姿态控制的控制流程和框架，不同点在于，在进行俯仰角和横滚角的控制时，由螺旋桨的升力提供控制力变成了由各个飞轮驱动时的反转矩提供控制力；偏航角控制时与多旋翼无人机原理相同，同时进行三个旋转自由度上的姿态角度控制。

另外，本发明所提出的立体倒立摆不限于四棱锥型结构，也可以是其它多棱锥结构，或者其它适合飞轮控制的结构。立体倒立摆也可以从四棱锥扩展到六棱锥、八棱锥等规则的立体几何棱锥形状，与之相对应的是有六个、八个等飞轮，均匀排布在棱锥的各个斜面三角形的中轴线上。

四棱锥立体倒立摆可作为多旋翼无人机控制算法的半物理仿真验证实验的模型使用。或者该四棱锥立体倒立摆可作为各种控制算法实验验证时所使用的半物理仿真验证实验的模型。

本发明的由多个飞轮2控制的四棱锥型的立体倒立摆，控制方式可用于作为半物理仿真模型及教具，来验证和演示多旋翼无人机的姿态控制，以及其他控制算法研究。

本发明是在立方体倒立摆的基础之上，将倒立的立体几何体改为四棱锥，以四棱锥四个斜面上的四个控制飞轮2，对三个旋转自由度的角度进行冗余控制。控制时四个飞轮2相互不独立，对于每个自由度必须进行混合、协同控制才能完成稳定的倒立控制。

其中，飞轮2由可以进行正反两个方向调速的电机3进行驱动，电机3提供给飞轮转动扭矩的同时，会反馈给四棱锥框架1同样大小的反扭矩，所以实际上立体倒立摆稳定控制的扭矩是电机3提供的，飞轮2的选择就需要保证其转动惯量不能过小，因为电机扭矩变化与载荷无关，但惯量过小会导致飞轮2转速过快，产生空气阻力等干扰，要保证在不过分影响整体质量的情况下惯量较大，可以考虑辐条或内圆镂空等结构，在保证转动惯量的情况下降低重量。

四棱锥框架1的重量选择上，如果过重则会导致转矩产生的角加速度过小，控制响应慢，甚至在偏离平衡位置时转矩产生的角加速度会小于重力产生的角加速度，导致系统不可控；而如果四棱锥框架1重量太轻，会使得电机3产生的一些高频振荡会对整体产生影响，带动整体产生振荡，且不利于系统的抗干扰性；所以整体的重量应该在考虑电机最大驱动力大小的前提下，尽量选择重量较大的材料、结构和尺寸。

四棱锥倒立摆的顶面的中心区域安装惯性传感器，反馈机体的位置角度方向上的状态信息，通过各种姿态解算算法(如互补滤波、卡尔曼滤波算法等)，可以获得倒立摆在空间三个旋转自由度上的角度信息，形成控制的反馈闭环。

惯性传感器3与解算器方面，由于是由四个动量轮即飞轮2对三个旋转自由度进行混控，所以四个飞轮2的控制量不能独立，可选择惯性传感器获取倒立摆整体的三维状态信息，通过解算器采用姿态解算算法(如互补滤波、卡尔曼滤波算法等)，建立起机体坐标系以及世界坐标系，得到倒立摆当前的姿态角度(俯仰角、横滚角与偏航角)以及角速度等信息，并将得到的倒立摆当前的姿态角度以及角速度反馈给控制器。控制器根据倒立摆当前的姿态角度以及角速度，计算得到俯仰控制量、横滚控制量与偏航控制量，并将俯仰控制量、横滚控制量与偏航控制量传输给混控器。混控器通过转换矩阵，将俯仰控制量、横滚控制量与偏航控制量分配给四个电机3。

本发明利用偏差控制的思想，获取到与平衡位置的角度偏差，给出各个自由度上的控制量，再由混控思想，给出各个飞轮的控制量，控制倒立摆的平衡。具体的，四棱锥立体倒立摆的平衡控制方法可以分成两个步骤，第一步是起跳阶段，第二步为平衡控制阶段。

通过分析四棱锥倒立摆在各种状态下的受力以及动力学特性，可以知道，在倒立摆整体位于初始位置(如图3)时，由于其角度的正余弦三角函数值不可简化为与其角度值相等，所以此时的四棱锥立体倒立摆系统是非线性不稳定系统，难以对其进行偏差控制。所以，此时需要启动起跳阶段，或者直接用人手或其他辅助装置直接将倒立摆的姿态设置为接近平衡位置，接近平衡位置与平衡状态有小角度误差(此原理为当角度误差接近于0时，角度的正弦值(sin约)等于其弧度值，在控制上可视为线性系统)。

起跳阶段：参见图3和图5，倒立摆在无控制时，由于重力作用，会处于一个侧面着地的状态(如图3所示)，此时，可以通过控制与着地面相邻的两侧面上的飞轮2来实现倒立摆的“起跳”，使它迅速达到平衡位置。即，瞬间提供巨大的转矩，使得倒立摆旋转至平衡位置。而由于电机3所提供的转矩有限，所以常用的方法是将着地面相邻两侧面上的飞轮2以不会引起倒立摆整体震动的加速度缓慢加速到高速旋转状态，其中，高速旋转状态时的速度为：平衡时倒立摆的重力势能等于飞轮2旋转时的动能时的速度；然后瞬间利用“刹车”结构将电机3停转，将动能转化为重力势能，使倒立摆“跳起”，接近平衡位置。

平衡控制阶段：当倒立摆通过各种方式(启动起跳阶段，或者直接用人手或其他辅助装置)，达到了接近平衡位置时，由于角度偏差变为小角度，倒立摆系统成为线性系统，易进行平衡控制。

参见图4，该阶段控制可参照无人机姿态控制方式，假如图4中第一飞轮2-1的方向为y轴正方向，则俯仰角度的调节是靠第二飞轮2-2和第三飞轮2-3两个飞轮控制的，在平衡位置时，以水平视线的角度各从外侧看向两个飞轮，第二飞轮2-2和第三飞轮2-3飞轮一个顺时针一个逆时针，转速、转矩相同时，第二飞轮2-2和第三飞轮2-3可以提供倒立摆在俯仰方向上的角加速度，来调节倒立摆装置的俯仰角度；

横滚角度同理靠第一飞轮2-1和第四飞轮2-4两个飞轮控制的，在平衡位置时，以水平视线的角度各从外侧看向两个飞轮，第二飞轮2-2和第三飞轮2-3飞轮一个顺时针一个逆时针，转速、转矩相同时，第一飞轮2-1和第四飞轮2-4飞轮可以提供倒立摆在俯仰方向上的角加速度，来调节倒立摆装置的横滚角度；

而偏航角度控制时，则是第一飞轮2-1、第二飞轮2-2、第三飞轮2-3和第四飞轮2-4四个飞轮同方向并且转速相同时，会提供给机体相反的转矩和角加速度，用以控制倒立摆的偏航角度，参见图6，通过惯性传感器4采集倒立摆的状态信息(加速度信息、地磁场方向信息、角速度信息等)，并将采集的状态信息传送为解算器，解算器进行计算得到俯仰角、横滚角以及偏航角，并传送给控制器，控制器计算出俯仰角控制量、横滚角控制量以及偏航角控制量并传送给混控器，混控器控制第一飞轮2-1、第二飞轮2-2、第三飞轮2-3和第四飞轮2-4。

这是该倒立摆的一个控制方法，但不唯一，也可以通过坐标系旋转的方式设计其他控制方法，同样能达到倒立摆的平衡控制效果。

本发明的总体布局由四棱锥框架1和四个均匀分布的飞轮2构成一个整体，在任意独立水平面6上可以实现顶点接触的倒立控制。其构造及原理的灵感来自于cubli三维立方体倒立摆，不同点在于：立体倒立摆框架的形状选择四棱锥形状，方便了四个转矩控制飞轮的四角排布，以及顶部平面上惯性传感器4的安装；四个转矩飞轮2的冗余控制，使得倒立摆可以仿照四旋翼无人机的姿态控制来实现倒立摆以四棱锥定点为支点在任意水平面6上的倒立控制；惯性传感器4布置方面，取代了原cubli立方体倒立摆三个装飞轮控制面上各有一个角度传感器的模式，可在四棱锥框架1的上表面的圆形区域内，放置惯性传感单元，通过姿态解算算法来获取倒立摆姿态角度；四个控制飞轮2由可正反调速的电机3提供转矩，通过其提供给四棱锥框架1的反转矩调节倒立摆的姿态角度，实现倒立控制。

Claims (8)

1.一种四棱锥立体倒立摆装置的控制方法，其特征在于，该四棱锥立体倒立摆装置，包括倒立的四棱锥框架（1）、惯性传感器（4）以及解算器，其中，倒立的四棱锥框架（1）的每个侧面上设置有一个飞轮（2），并且飞轮（2）与所在侧面平行，并且飞轮（2）的圆心位于等腰三角形的侧面的中线上，每个飞轮（2）连接有一个能够正反调速的电机（3），在倒立的四棱锥框架（1）的顶面上设置有惯性传感器（4）和解算器，惯性传感器（4）与解算器相连；

惯性传感器（4）和解算器位于顶面的中心；解算器连接有控制器，控制器连接有混控器，混控器与飞轮相连；

通过惯性传感器（4）采集位置、角度方向上的状态信息，并反馈给解算器，解算器通过姿态解算算法，获得倒立摆在空间三个旋转自由度上的角度信息，并将角度信息传输给控制器，控制器通过混控器调节四个飞轮。

2.根据权利要求1所述的四棱锥立体倒立摆装置的控制方法，其特征在于，姿态解算算法为互补滤波算法或卡尔曼滤波算法。

3.根据权利要求1所述的四棱锥立体倒立摆装置的控制方法，其特征在于，通过惯性传感器获取倒立摆整体的三维状态信息，并反馈给解算器，解算器采用姿态解算算法建立起机体坐标系以及世界坐标系，得到倒立摆当前的姿态角度以及角速度，并将得到的倒立摆当前的姿态角度以及角速度反馈给控制器，控制器根据倒立摆当前的姿态角度以及角速度计算，得到俯仰控制量、横滚控制量与偏航控制量，并将俯仰控制量、横滚控制量与偏航控制量传输给混控器，混控器通过转换矩阵，将俯仰控制量、横滚控制量与偏航控制量分配给四个电机。

4.根据权利要求3所述的四棱锥立体倒立摆装置的控制方法，其特征在于，当倒立摆整体位于初始位置时，启动起跳阶段，或者将倒立摆的姿态调整为接近平衡位置，接近平衡位置与平衡状态有小角度误差，其中，小角度的正弦值等于其弧度值。

5.根据权利要求4所述的四棱锥立体倒立摆装置的控制方法，其特征在于，当倒立摆一个侧面着地时，通过控制与着地面相邻的两侧面上的飞轮实现倒立摆的起跳，使得倒立摆旋转至平衡位置。

6.根据权利要求5所述的四棱锥立体倒立摆装置的控制方法，其特征在于，通过控制与着地面相邻的两侧面上的飞轮实现倒立摆的起跳，使得倒立摆旋转至平衡位置的具体过程为：将与着地面相邻两侧面上的飞轮（2）加速到高速旋转状态，然后将电机（3）停转，将动能转化为重力势能，使倒立摆跳起，接近平衡位置；其中，高速旋转状态时的速度为：平衡时倒立摆的重力势能等于飞轮（2）旋转时的动能时的速度。

7.根据权利要求1所述的一种四棱锥立体倒立摆装置的控制方法，其特征在于，电机（3）连接有供电系统。

8.根据权利要求7所述的一种四棱锥立体倒立摆装置的控制方法，其特征在于，供电系统包括电池以及驱动电路，电池与驱动电路相连，驱动电路与电机（3）相连。

Images