CN113093822B - 两轮单辙载具静态平衡控制系统和方法 - Google Patents

Abstract

本发明提供了一种两轮单辙载具静态平衡控制系统和方法。两轮单辙载具具有双SGCMG并列对称构型，所述两轮单辙载具静态平衡控制系统具有第一框架电机和第二框架以及与第一框架电机和第二框架分别对应的第一控制力矩陀螺和第二控制力矩陀螺，第一框架电机具有第一转子，第二框架具有第二转子；所述两轮单辙载具静态平衡控制系统包括框架去饱和与奇异回避控制器，其执行如下计算和控制：计算框架角度误差；基于框架角度误差计算用于实现框架去饱和与奇异回避的偏置角度；在角度环控制器的输入中加入偏置角度。

Description

两轮单辙载具静态平衡控制系统和方法

技术领域

本发明涉及交通工具领域，具体涉及一种两轮单辙载具静态平衡控制系统和方法。

背景技术

两轮单辙载具其系统类似于一个倒立摆系统，是一个复杂的非线性、强耦合、多变量的自然不稳定系统。其横向动力学特性较汽车来说更为复杂，特别是传统两轮单辙载具在静止时几乎无法靠自身稳定。而两轮单辙载具的自平衡的控制目前有通过转向控制，控制力矩陀螺控制和重心控制三种方法。

但是，目前研究的都是各自单一的控制方法。转向控制无法保证两轮单轨车辆在静态情况下的自平衡，而重心控制大多借助仿人机器人，其结构复杂，且实用性较差。目前来说对于两轮单辙载具的自平衡来说，大多数控制结构和算法其抗扰性差，控制程度有限，而且对于控制力矩陀螺(CMG，Control moment gyroscope)的控制算法较少，无法避免控制力矩陀螺的奇异和去饱和能力。

控制力矩陀螺CMG是角动量交换设备之一，可以产生较大的输出扭矩。它由电动转子和万向架组成。飞轮的旋转轴可以绕与其旋转轴(万向轴)垂直的轴变化。CMG和身体之间的角动量变化率取决于万向节速度。常用在航空航天领域，近年来随着控制力矩陀螺技术的成熟，也用于无人机，船舶的平衡等领域。而两轮车横向不稳定的非线性系统，使研究人员也将控制力矩陀螺技术运用在两轮车自平衡中。

两轮单辙载具目前来说能够实现自平衡的较少，大多数研究处于仿真和理论阶段。国内目前仅实现依靠控制力矩陀螺使两轮单辙载具，实现动态平衡。其扰动较大，稳定性较低。大多数运用LQR，或者结构简单的PID控制算法，这就使两轮单辙载具的平衡稳定性和控制力矩陀螺的奇异与去饱和无法兼顾。或者大多数控制算法，如鲁棒控制，模糊控制等等，其在计算机仿真即如果中可以到达理想的控制效果。但是仿真模型教现实模型来说其干扰性较少，而且其控制结构及算法阶数较高，算法的计算延迟性很难实际的运用在两轮单辙载具的实时平衡控制中。

总而言之，现有的两轮单辙载具其控制结构简单，载具的抗扰性较差，且在保证两轮单辙载具平衡的同时，很难保证控制力矩陀螺的奇异回避和去饱和能力。且很多控制方法计算得迟滞性很难运用在实际在两轮单辙载具的实时平衡控制中。

发明内容

本发明所要解决的技术问题是针对现有技术中存在上述缺陷，提供一种两轮单辙载具静态平衡控制系统和方法，其能够兼顾两轮单辙载具在平衡的同时，保证控制力矩陀螺的奇异回避和去饱和，提高两轮单辙载具的抗外部扰动能力。

根据本发明，提供了一种两轮单辙载具静态平衡控制系统，其中两轮单辙载具具有双SGCMG并列对称构型，所述两轮单辙载具静态平衡控制系统具有第一框架电机和第一框架电机对应的第一控制力矩陀螺，以及与第二框架电机和第二框架电机对应的第二控制力矩陀螺，第一框架电机具有第一转子，第二框架具有第二转子；所述两轮单辙载具静态平衡控制系统包括框架去饱和与奇异回避控制器，其执行如下计算和控制：根据第一框架电机和第二框架电机分别反馈的框架角度计算框架角度误差；基于框架角度误差计算用于实现框架去饱和与奇异回避的偏置角度；根据车体的平衡点角度、车体当前角度和偏置角度计算出偏置角度的角度误差并输入角度环控制器，以输出目标角速度至角速度环控制器。

优选地，所述两轮单辙载具静态平衡控制系统包括重力矩抑制前馈控制器，用于执行如下计算和控制：接收第一转子和第二转子的转子转速、扩展卡尔曼滤波器估计出的车体倾斜角度、以及车体及陀螺的动力学参数；根据接收的数据计算第一框架电机与第二框架电机的前馈补偿角速度；将计算的第一框架电机与第二框架电机的前馈补偿角速度分布发送给第一框架电机与第二框架电机的角速度环控制。

优选地，所述两轮单辙载具静态平衡控制系统包括双框架角度同步补偿控制器，用于执行如下计算和控制：计算第一框架电机和第二框架电机的角度异步误差，并分别计算计算第一框架电机和第二框架电机的角速度同步补偿值。

优选地，所述两轮单辙载具静态平衡控制系统包括转子恒速控制器，用于实现双转子转速异步补偿、转子转速前馈控制和转子转速油门控制，其中双转子转速异步补偿包括：根据目标转速和转子转速的实时反馈，计算转子转速异步误差；并且根据转子转速异步误差计算第一转子和第二转子的转速异步补偿。

优选地，执行双转子转速异步补偿包括：计算转子转速异步误差，并计算第一转子和第二转子的转速异步补偿。

根据本发明，还提供了一种两轮单辙载具静态平衡控制方法，其中两轮单辙载具具有双SGCMG并列对称构型，所述两轮单辙载具静态平衡控制系统具有第一框架电机和第一框架电机对应的第一控制力矩陀螺，以及与第二框架电机和第二框架电机对应的第二控制力矩陀螺，第一框架电机具有第一转子，第二框架具有第二转子；所述两轮单辙载具静态平衡控制方法包括：根据第一框架电机和第二框架电机分别反馈的框架角度计算框架角度误差；基于框架角度误差计算用于实现框架去饱和与奇异回避的偏置角度；根据车体的平衡点角度、车体当前角度和偏置角度计算出偏置角度的角度误差并输入角度环控制器，以输出目标角速度至角速度环控制器。

优选地，所述两轮单辙载具静态平衡控制方法还包括：接收第一转子和第二转子的转子转速、扩展卡尔曼滤波器估计出的车体倾斜角度、以及车体及陀螺的动力学参数；根据接收的数据计算第一框架电机与第二框架电机的前馈补偿角速度；将计算的第一框架电机与第二框架电机的前馈补偿角速度分布发送给第一框架电机与第二框架电机的角速度环控制。

优选地，所述两轮单辙载具静态平衡控制方法还包括：计算第一框架电机和第二框架电机的角度异步误差，并分别计算计算第一框架电机和第二框架电机的角速度同步补偿值。

优选地，所述两轮单辙载具静态平衡控制方法还包括：根据目标转速和转子转速的实时反馈，计算转子转速异步误差；并且根据转子转速异步误差计算第一转子和第二转子的转速异步补偿。

优选地，所述两轮单辙载具静态平衡控制方法还包括：计算转子转速异步误差，并计算第一转子和第二转子的转速异步补偿。

本发明，能够兼顾两轮单辙载具在平衡的同时，保证控制力矩陀螺的奇异回避和去饱和，提高两轮单辙载具的抗外部扰动能力。而且本次的控制的方法能够运用在单片机上，对两轮单辙载具进行实时的平衡控制。两轮单辙载具的自平衡实现，作为单轨车辆在狭窄和混乱的环境中具有很强的机动性；两轮单辙载具与地面的摩擦较少，具有较高的能源效率；两轮单辙载具的出现，其良好的机动性，可以代替人们，在复杂环境中完成一些危险的工作；为那些步行能力有限的人，也能够驾驶自行车或摩托车，提供便捷的交通支持。

附图说明

结合附图，并通过参考下面的详细描述，将会更容易地对本发明有更完整的理解并且更容易地理解其伴随的优点和特征，其中：

图1示意性地示出了根据本发明优选实施例的双SGCMG并列对称构型的两轮单辙载具侧视图。

图2示意性地示出了根据本发明优选实施例的两轮单辙载具静态平衡控制系统和及其相应控制方法的示意框图。

需要说明的是，附图用于说明本发明，而非限制本发明。注意，表示结构的附图可能并非按比例绘制。并且，附图中，相同或者类似的元件标有相同或者类似的标号。

具体实施方式

为了使本发明的内容更加清楚和易懂，下面结合具体实施例和附图对本发明的内容进行详细描述。

本发明主要基于双SGCMG(单框架控制力矩陀螺，SGCMG)并列对称构型的两轮单辙载具，提出了一种新型的静态平衡控制系统和方法，能有效地解决控制力矩陀螺的奇异与去饱和问题，以及两轮单辙载具静态自平衡过程中的抗扰动问题。本发明提高了两轮单辙载具的平衡稳定性和实用性。

图1示意性地示出了根据本发明优选实施例的双SGCMG并列对称构型的两轮单辙载具侧视图。图2示意性地示出了根据本发明优选实施例的两轮单辙载具静态平衡控制系统和及其相应控制方法的示意框图。

本次发明采用的是双SGCMG并列对称构型的两轮单辙载具，控制力矩陀螺是运用角动量守恒的原理，旋转的陀螺转子整体发生偏转时，会产生进动力矩，从而满足本发明的控制的需要，目前多用在航天器在太空的姿态调整上。参考附图1，旋转的控制力矩陀螺在框架电机的作用下发生偏转，从而在两轮单辙载具的横滚轴上产生进动平衡力矩从而抵消车体倾斜时重力矩的作用，使两轮单辙载具回到平衡位置。

两轮单辙载具具有双SGCMG并列对称构型，具有两个控制力矩陀螺(第一控制力矩陀螺CMG1和第二控制力矩陀螺CMG2)、对应的两个框架电机(第一框架电机1和第二框架电机2)，两个框架电机分布具有第一转子10与第二转子20。

动力学方程如下所示：

控制力矩陀螺产生的进动力矩：

其中：I_r为转子的转动惯量，第一转子10与第二转子20的转速ω_r大小相同方向相反，两个框架电机的角速度

方向相反，因此在两轮单辙载具的横滚轴上产生的进动平衡力矩方向相同。

但是值得注意的是，当控制力矩陀螺在框架角度达到90度时，将无法在两轮单辙载具的横滚轴上产生进动平衡力矩，此时车体将无法控制，这种现象称为框架的奇异。另外当框架无法自动回到零位时，随着扰动的增加，框架角度逐渐增加，框架所能产生的力矩能力逐渐饱和，不利于控制器的控制效果和车体抗击外部扰动的能力。

两轮单辙载具横滚动力学方程：

(2)

其中：I_c为两轮单辙载具横滚方向的转动惯量，

为车体横滚角加速度，m_c为车体质量，h为车体质心高度。

如附图2所示，在控制器的主体结构上，采用角度环和角度速度环的双环控制器，两轮单辙载具移动平台，输出车体倾斜角和倾斜角速度反馈。由于车体刚性，陀螺仪传感器受外界扰动等因素影响，输出的车体倾斜角和倾斜角速度波动较大。为了使控制得到较为精确的反馈数据，提高控制精度，因此在车体倾斜角度和角度速度输出时加入扩展卡尔曼滤波器，使滤波器估计的角度和角速度噪声较小，同时提高车体本身的内部扰动。

本发明的控制器结构除了主体的双环控制器以外还，还有框架去饱和与奇异回避控制器、重力矩抑制前馈控制器、双框架角度同步补偿控制器、转子恒速控制器，具体参考附图2。

本发明的关键技术点之一在于框架去饱和与奇异回避。为了使车体抗击扰动的能力增加，本发明加入了框架去饱和与奇异回避控制器。要的达到此目的，需要排除框架回零时的力矩对于车体平衡的干扰，因此本发明在角度环控制器的输入中加入了框架去饱和与奇异回避所需要的偏置角度。具体的去饱和与奇异回避控制器执行计算和控制如下所示：

计算框架角度误差：

其中：θ_f1与θ_f2是伺服电机反馈的框架角度。

计算偏置角度：

Δθ_c＝k_Pfe_θf+k_If∫e_θfdt (4)

其中：k_Pf和k_If为框架去饱和与奇异回避控制器的参数。

角度环控制器的输入为平衡点角度，车体当前角度和偏置角度的角度误差，输出为目标角速度。其控制结构如下：

角度误差：

e_θc＝θ_c-(θ_c0+Δθ_c) (5)

其中：θ_c为车体当前角度，θ_c0为平衡点角度，Δθ_c为偏置角度

角度环控制器的输出：

其中：k_p与k_d为角度环PD控制器的参数。

本发明的控制结构中，加入了重力矩抑制前馈控制器实现前馈与反馈结合的控制模式，根据车体倾斜角度计算出回正至所需力矩完全由控制力矩陀螺输出时陀螺理论上所需要的角速度。其输入为转子转速，扩展卡尔曼滤波器估计的车体倾斜角度和车体及陀螺的动力学参数，输出为第一框架电机1与第二框架电机2的前馈补偿角速度。

计算第一框架电机1角速度前馈补偿：

计算第二框架电机2角速度前馈补偿：

角速度环控制器输入为角度环控制器输出的目标角度速度，扩展卡尔曼滤波器估计的当前车体角速度和重力矩抑制前馈控制器输出的第一框架电机1和第二框架电机2的前馈角速度。输出为第一框架电机1和第二框架电机2的角速度控制量。

计算第一框架电机1角速度误差：

其中：

为车体当前角度，u_θ为角度环控制器输出的目标角度速度，Δu_ω1第一框架电机1的前馈角速度。

计算第二框架电机2角速度误差：

其中：

为车体当前角度，u_θ为角度环控制器输出的目标角度速度，Δu_ω2第二框架电机2的前馈角速度。

计算第一框架电机1的角速度控制量：

其中：k_Pω1与k_Iω1为第一框架电机1的角速度环控制器参数。

计算第二框架电机2的角速度控制量：

其中：k_Pω1与k_Iω1为第二框架电机2的角速度环控制器参数。

为了保证控制力矩陀螺框架角度同步，以提高控制器的控制效果，避免不必要的内部扰动。加入了双框架角度异步补偿器。其输入为框架角度异步误差，输出为第一框架电机1和第二框架电机2角速度同步补偿。其控制结构如下：

计算第一框架电机1和第二框架电机2的角度异步误差：

e_df＝θ_f1-θ_f2 (13)

第一框架电机1的角速度同步补偿：

Δu_df1＝-k_df·e_df (14)

第二框架电机2的角速度同步补偿：

Δu_df2＝k_df·e_df (15)

其中：k_df为双框架角度异步补偿器控制参数。

因此，框架电机的控制输入由两部分组成，第一部分为角速度环控制器输出的第一框架电机1和2的角速度控制量，第二部分为双框架角度异步补偿器输出的第一框架电机1和2的角速度同步补偿控制量。

第一框架电机1的角速度控制量：

u_f1＝u_ω1+Δu_df1 (16)

第二框架电机2的角速度控制量：

u_f2＝u_ω2+Δu_df2 (17)

另外为了保证第一转子10与第二转子20转速恒定且遥控器油门能实时控制转子转速，加入了转子恒速控制器。例如，转子恒速控制器由双转子转速异步补偿、转子转速前馈控制和转子转速油门控制组成。转子恒速控制器输入为目标转速和转子转速的实时反馈，输出为转子油门。

(1)双转子转速异步补偿：

计算转子转速异步误差：

e_dr＝ω_r1-ω_r2 (18)

其中：ω_r1为第一转子10的转速，ω_r2为第二转子10的转速。

计算第一转子10的转速异步补偿：

u_dr1＝-k_dr·e_dr (19)

计算第二转子20的转速异步补偿：

u_dr2＝k_dr·e_dr (20)

其中：k_dr为双转子转速异步补偿控制参数。

(2)转子转速前馈控制：

计算第一转子10前馈控制：

u_rf1＝k_rf1·ω_c+C₁ (21)

计算第二转子20前馈控制：

u_rf2＝k_rf2·ω_c+C₂ (22)

其中：k_rf1、C₁为第一转子10转速前馈控制参数，k_rf2、C₂为第二转子20转速前馈控制参数，ω_c为目标转速。

(3)转子转速油门控制：

第一转子10转速误差：

e_ω1＝ω_r1-(ω_c+u_dr1) (23)

第二转子20转速误差：

e_ω2＝ω_r2-(ω_c+u_dr2) (24)

第一转子10转速油门：

u_r1＝u_rf1+k_r1·∫e_ω1dt (25)

第二转子20转速油门：

u_r2＝u_rf2+k_r2·∫e_ω2dt (26)

其中：k_r1为第一转子10转速油门控制参数，k_r2为第二转子20转速油门控制参数。

通过以上控制结构输出了对于两个框架电机的角速度控制量，控制力两个矩陀螺转子电机的转速油门控制量。对称构型双SGCMG，在电机被控的作用下输出两轮单辙载具自平衡所需要的进动平衡力矩，使两轮单辙载具实现静态自平衡。

本发明关键的技术点和保护点为控制器以及控制结构的设计，使其实现两轮单辙载具实现静态自平衡，并兼顾对称构型双控制力矩陀螺的去饱和与奇异回避，提高了两轮单辙载具的抗外界干扰能力。

本发明，能够兼顾两轮单辙载具在平衡的同时，保证控制力矩陀螺的奇异回避和去饱和，提高两轮单辙载具的抗外部扰动能力。而且本次的控制的方法能够运用在单片机上，对两轮单辙载具进行实时的平衡控制。两轮单辙载具的自平衡实现，作为单轨车辆在狭窄和混乱的环境中具有很强的机动性；两轮单辙载具与地面的摩擦较少，具有较高的能源效率；两轮单辙载具的出现，其良好的机动性，可以代替人们，在复杂环境中完成一些危险的工作；为那些步行能力有限的人，也能够驾驶自行车或摩托车，提供便捷的交通支持。

需要说明的是，除非特别指出，否则说明书中的术语“第一”、“第二”、“第三”等描述仅仅用于区分说明书中的各个组件、元素、步骤等，而不是用于表示各个组件、元素、步骤之间的逻辑关系或者顺序关系等。

可以理解的是，虽然本发明已以较佳实施例披露如上，然而上述实施例并非用以限定本发明。对于任何熟悉本领域的技术人员而言，在不脱离本发明技术方案范围情况下，都可利用上述揭示的技术内容对本发明技术方案作出许多可能的变动和修饰，或修改为等同变化的等效实施例。因此，凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所做的任何简单修改、等同变化及修饰，均仍属于本发明技术方案保护的范围内。

Claims (10)

1.一种两轮单辙载具静态平衡控制系统，其中两轮单辙载具具有双SGCMG并列对称构型，所述两轮单辙载具静态平衡控制系统具有第一框架电机和第一框架电机对应的第一控制力矩陀螺，以及与第二框架电机和第二框架电机对应的第二控制力矩陀螺，第一框架电机具有第一转子，第二框架具有第二转子；其特征在于，所述两轮单辙载具静态平衡控制系统包括框架去饱和与奇异回避控制器，其执行如下计算和控制：

根据第一框架电机和第二框架电机分别反馈的框架角度计算框架角度误差；

基于框架角度误差计算用于实现框架去饱和与奇异回避的偏置角度；

根据车体的平衡点角度、车体当前角度和偏置角度计算出偏置角度的角度误差并输入角度环控制器，以输出目标角速度至角速度环控制器。

2.根据权利要求1所述的两轮单辙载具静态平衡控制系统，其特征在于，所述两轮单辙载具静态平衡控制系统包括重力矩抑制前馈控制器，用于执行如下计算和控制：

接收第一转子和第二转子的转子转速、扩展卡尔曼滤波器估计出的车体倾斜角度、以及车体及陀螺的动力学参数。

3.根据权利要求1或2所述的两轮单辙载具静态平衡控制系统，其特征在于，所述两轮单辙载具静态平衡控制系统包括双框架角度同步补偿控制器，用于执行如下计算和控制：计算第一框架电机和第二框架电机的角度异步误差，并分别计算第一框架电机和第二框架电机的角速度同步补偿值。

4.根据权利要求1或2所述的两轮单辙载具静态平衡控制系统，其特征在于，所述两轮单辙载具静态平衡控制系统包括转子恒速控制器，用于实现双转子转速异步补偿、转子转速前馈控制和转子转速油门控制，其中双转子转速异步补偿包括：根据目标转速和转子转速的实时反馈，计算转子转速异步误差；并且根据转子转速异步误差计算第一转子和第二转子的转速异步补偿。

5.根据权利要求4所述的两轮单辙载具静态平衡控制系统，其特征在于，执行双转子转速异步补偿包括：计算转子转速异步误差，并计算第一转子和第二转子的转速异步补偿。

6.一种两轮单辙载具静态平衡控制方法，其中两轮单辙载具具有双SGCMG并列对称构型，所述两轮单辙载具静态平衡控制系统具有第一框架电机和第一框架电机对应的第一控制力矩陀螺，以及与第二框架电机和第二框架电机对应的第二控制力矩陀螺，第一框架电机具有第一转子，第二框架具有第二转子；其特征在于，所述两轮单辙载具静态平衡控制方法包括：根据第一框架电机和第二框架电机分别反馈的框架角度计算框架角度误差；基于框架角度误差计算用于实现框架去饱和与奇异回避的偏置角度；根据车体的平衡点角度、车体当前角度和偏置角度计算出偏置角度的角度误差并输入角度环控制器，以输出目标角速度至角速度环控制器。

7.根据权利要求6所述的两轮单辙载具静态平衡控制方法，其特征在于还包括：接收第一转子和第二转子的转子转速、扩展卡尔曼滤波器估计出的车体倾斜角度、以及车体及陀螺的动力学参数；根据接收的数据计算第一框架电机与第二框架电机的前馈补偿角速度；将计算的第一框架电机与第二框架电机的前馈补偿角速度分布发送给第一框架电机与第二框架电机的角速度环控制。

8.根据权利要求6或7所述的两轮单辙载具静态平衡控制方法，其特征在于还包括：计算第一框架电机和第二框架电机的角度异步误差，并分别计算计算第一框架电机和第二框架电机的角速度同步补偿值。

9.根据权利要求6或7所述的两轮单辙载具静态平衡控制方法，其特征在于还包括：根据目标转速和转子转速的实时反馈，计算转子转速异步误差；并且根据转子转速异步误差计算第一转子和第二转子的转速异步补偿。

10.根据权利要求6或7所述的两轮单辙载具静态平衡控制方法，其特征在于还包括：计算转子转速异步误差，并计算第一转子和第二转子的转速异步补偿。

Images