CN104298113A - 两轮机器人自适应模糊平衡控制器 - Google Patents

Abstract

本发明涉及一种两轮机器人自适应模糊平衡控制器。设计方法包括以下步骤：将两轮机器人平衡于地面，打开主机陀螺仪，产生一个初始电压；计算电压偏移量、倾斜角度、左右轮的位置差与速度差，作为自适应模糊控制器的输入变量；控制陀螺仪输出产生新的电压，控制马达输出产生新的倾斜角度；重新计算左右轮的位置差与速度差，以此控制马达输出产生出新的角速度，再重新计算出角度，然后继续对陀螺仪及马达转速编码做自适应模糊控制器的控制，加总后一样对马达做功率输出；如此周而复始，便能让此系统趋于稳定。传统控制方法动态响应不理想、抗干扰能力较差。本发明不但能抑制外界干扰，适应环境变化、系统本身参数变化的影响，还能有效地消除模型化误差等影响。

Description

两轮机器人自适应模糊平衡控制器

技术领域

本发明涉及两轮机器人的平衡控制领域，特别是一种两轮机器人自适应模糊平衡控制器。 

技术背景

机器人在现代社会中起到了越来越重要的作用，其产品在工业、农业、医学、军事等领域都已得到了广泛的应用，而其控制器的设计则是制造机器人的关键技术之一。机器人系统属于比较复杂的非线性系统，容易受到自身和外界一些不确定因素的影响，因此其控制器的设计就成为一个难题。而两轮自平衡机器人结构简单，便于分析，并且是一种非线性、多变量、本质不稳定、强耦合的运动控制系统。因此，研究两轮自平衡机器人对整个机器人领域的控制器设计都具有十分重要的意义，同时也是检验各种控制算法的理想平台。目前，有关两轮机器人平衡方法的研发典型有：陈少斌等2009年提出一种移动机器人轨迹跟踪的最优状态反馈控制方法，采用卡尔曼滤波器对伴有高斯白噪声的系统状态方程进行了状态估计，提出一种基于李亚普诺夫稳定性的最优状态反馈控制策略。给出了补偿输入信号干扰的自适应算法。段学超等2013年提出了一种模糊进化型极点配置(FEPP)控制方法.采用拉格朗日方法进行了两轮自平衡机器人的动力学建模,通过将其在平衡位置进行微偏线性化,得到了系统的线性化标称模型。对该6阶欠驱动系统设计了FEPP控制器,分别实现了两轮自平衡机器人的动态镇定和行走控制。毛欢等2014年提出一种用于双轮自平衡机器人的改进PID算法，通过倾角传感器来检测系统在垂直方向的倾角，采用改进后的PID算法实施控制，利用PWM模块输出配合驱动电路，调整直流电机的运动,能够使系统达到动态平衡。李志超等2014年提出一种两轮自平衡机器人LQR-模糊控制算法，首先利用LQR方法生成的反馈矩阵来构造融合函数，通过融合函数降低系统的输入维数，然后通过模糊控制器来对系统进行控制。仿真和实验结果表明，采用该种控制策略既可以保证机器人的稳定，又可以控制机器人到指定位置和角度。

概括以上专利技术或者文献介绍的系统，两轮机器人自平衡方法大致可分为传统控制方法和智能控制方法：传统控制方法主要采用 PID方法、状态反馈和极点配置法等，主要处理线性已知系统；由于两轮自平衡机器人具有强非线性，因此对机器人模型进行线性化后将导致模型不精确、控制效果不能达到最优；而且传统控制方法动态响应不理想、抗干扰能力较差。智能控制主要采用模糊控制，能够处理非线性未知系统，不要求建立系统数学模型，动态响应性、抗干扰能力有所提高，但是模糊控制方法的控制精度取决于模糊规则库的建立，随着控制程度精确性的提高模糊规则数呈级数增长，控制系统的效果受专家经验的影响非常大。

发明内容

针对 PID、极点配置等传统控制方法，动态响应不理想、抗干扰能力较差，控制效果不能达到最优；模糊控制过分依赖专家经验，模糊规则随控制精度呈级数增长的问题；本发明采用的自适应控制方法，是建立在未知系统的基础上，通过实时地自动调节系统本身控制规律的参数，适应外界环境变化、系统本身参数变化、外界干扰等的影响，使整个系统能按某一性能指标运行在最佳状态下的控制。它不但能抑制外界干扰，适应环境变化、系统本身参数变化的影响，在某种程度上，还能有效地消除模型化误差等影响。因此，本发明采用自适应控制方法设计机器人自平衡控制器，能提高机器人的自平衡性能和抗干扰能力。

本发明便选用两轮机器人作为受控模型，接着以此两轮机器人的机械模型进行状态方程式的推导，并根据两轮机器人的状态方程式进行模糊控制器的设计，本发明推导出一个自适应定律，此自适应定律主要是让本发明所提出的模糊控制器可以近似一个理想控制器，使系统可以追溯响应并让误差收敛，以确保整个闭回路系统的稳定性。该自适应模糊控制器将根据两轮机器人所输出的各个变量值，控制左右伺服马达输出不同的功率(速度)或转动方向以修正车体位置，进而达到直立平衡控制之效果。

本发明的有益效果是： 

本发明所设计之自适应模糊控制器可让两轮机器人在直立状态下持续保持平衡，且其输出响应及平衡效能皆优于典型的PID控制器。 

附图说明

图1为本发明的两轮机器人自适应模糊平衡控制器流程图。

图2为本发明的两轮机器人陀螺仪倾斜角度定义图。

图3为本发明的两轮机器人平衡方法示意图。

图4为本发明的自适应模糊平衡控制系统架构图。

图5为本发明的自适应模糊控制器之前件部归属函数。

图6 为本发明的应用自适应模糊控制器及PID控制器的两轮机器人倾斜角度比较图。

具体实施方式

下面结合附图对本发明两轮机器人自适应模糊平衡控制器做进一步详细阐述。

图1为两轮机器人平衡控制流程图，将两轮机器人平衡于地面。在两轮机器人偏摆时，打开主机陀螺仪，一开始陀螺仪偏移后会产生一个初始电压，将这一个参数先透过陀螺仪比例计算的方式固定出偏移量，然后计算出倾斜角度同时解决角度误差的问题，接着计算左右轮的位置差与速度差，然后推导自适应定律、设计自适应模糊控制器，并以自适应模糊控制器来控制陀螺仪以及马达，经过加总后控制马达的功率输出。马达动作后，经由运算输出产生出新的角速度，再重新计算出角度，然后继续对陀螺仪及马达转速编码做自适应模糊控制器的控制，加总后一样对马达做功率输出，如此周而复始，便能让此系统趋于稳定。 

两轮机器人平衡控制流程具体步骤如下：

步骤一：将两轮机器人平衡于地面

步骤二、打开主机陀螺仪读取初始电压

在两轮机器人偏摆时，打开主机陀螺仪，一开始陀螺仪偏移后会产生一个初始电压

步骤三、计算电压偏移量计算倾斜角度

如（1）式所示，取陀螺仪输出电压值平均值以求得当时陀螺仪在静止时其输出电压的准位，其中                                                、、、为任意取某一陀螺仪输出电压值，、为某一陀螺仪输出电压值其平均值；

如（2）式所示，将所求得的准位减去2.5V后求得其平均的偏移量，其中为实际输出电压偏移量。之后，每次陀螺仪讯号进来以后减去偏移量当作补偿。 

， (1) 

 (2) 

步骤四、计算左右轮的速度差与位置差

 在图2里面G₁为NXT 的中心轴，也同时是陀螺仪里所选的中心轴，Oa 及Ob 各是左右两边圆形轮子的中心点(圆心)，O1是Oa 及Ob 所连起的直线之中心点，同时也是G₁中心轴垂直坐落的重心，另外则是身体前倾或后仰的情况，G₂、G₃是G₁偏移时的轴心，为前倾或后仰的角度，S₁为两颗轮子向前的力量，S₂为两颗轮子向后的力量。两轮机器人一开始是平衡于地面，轮子并无转动，也没有倾斜角度，所以将G₂、G₃设为零如（3）式，S₁、S₂也设为零如（4）式，当两轮机器人使动作后便会产生倾斜角度，其中倾斜角的计算方法如公式（5）所示 

                                   (3)  

                                     (4)

                         (5) 

若所须要的操作角度介于±20°以内的话，则其倾斜角的计算方法可简化为 

                                (6) 

此简化计算的误差范围小于 2％。如此即可免去计算反正弦函数(sin^-1) 的麻烦，并增加程序计算的速度。 

如图3所示，在要求两轮转速一致与控制器设计方式简单的前提下，由运动力学可以得知，移动平台底座所施给机器人的力为两轮所出之力的和U，如果在其中一轮的输出上加上两轮位置差之误差 ，另一轮减掉两轮位置差之误差 ，那么合力依旧为原来的力，不会破坏掉原来平衡方法所需要的力矩。又由于两轮的输出力量不同，会造成速度差，是控制两轮同步的速度差公式如（7）式所示；是控制两轮同步的位置差公式如（8）式所示： 

 (7) 

其中：右轮转速；：左轮转速 

：两轮所出之力的和 

    (8) 

其中: 右轮位置；: 左轮位置 

 步骤五、自适应定律的推导及自适应模糊控制器设计

首先，本发明推导自适应定律的过程如下：

一般情况下，用于两轮机器人的理想控制器可表示为：

      （9）

其中为未知向量而为未知矩阵，为受控系统之输出，为控制器之输出，为状态向量。但由于、未知，理想控制器无法实现；

理想控制器（9）表示为：

                                （10）

我们将所设计之模糊控制器及公式(10)的理想控制器一同代入公式(9)的多输入多输出2阶非线性受控系统中，用以取代掉原先的控制器 ，可以得到 

移项后可以得到

      （11）

假设存在一个最佳的模糊控制器，该模糊控制器可以表示为如下形式：

  （12）

假设

              （13）

其中参数、与为我们所假设可获得的最佳控制参数，该参数将使得假设存在的最佳模糊控制器与理想控制器之间存在一个最小的近似误差。因此，我们可以保证系统的稳定性。

然后，本发明设计自适应模糊控制器的过程如下：

如图4，根据以上推导出的自适应定律，本发明设计一个模糊控制器用以近似理想控制器，该模糊控制器可以表示为，为模糊控制器之输入，此输入是根据输出电压（），倾斜角度（），左右轮的位置差（），两轮同步的速度差（）等四个变量与期望值（、、、）之间的误差及误差变化率（、、、），所取得，由于两轮机器人属于一个多输入多输出的系统，这将导致我们难以设计模糊控制器中的模糊规则，因此本发明根据经验以及两轮机器人的实际输出响应，将两轮机器人所有的输出误差做一整合，并将其定义为，而误差总和之变化率则定义为，模糊控制器之输入描述如下： 

其中是两轮机器人的各项输出增益，是本发明根据经验以及两轮机器人的实际输出响应所给定，为输出电压（）增益，为倾斜角度（）增益，为左右轮的位置差（）增益，为两轮同步的速度差（）增益；

则为模糊控制器之输出，也就是模糊控制器根据输入的两轮平衡车误差（）及误差变化（）所修正的控制讯号，该控制讯号将影响左、右伺服马达的输出，模糊控制器之输出表示如下：

 本发明为自适应模糊控制器所订定的模糊规则如表1所示，其中P代表正(Positive)，N代表负(Negative)，Z代表零(Zero)，而自适应模糊控制器中，前件部归属函数的规则如图5所示。

表1自适应模糊控制器之模糊规则

 本发明将所设计的自适应模糊控制器及一个典型的PID控制器分别实际应用在两轮机器人上，在两轮机器人执行平衡控制的同时，取出两个控制器的响应结果及两轮机器人的平衡效能进行比较，其比较结果如下：

自适应模糊控制器及PID控制器根据两轮机器人之各项误差进行修正，经过自适应模糊控制器及PID控制器之控制后，两轮机器人的车身倾斜角度之比较如图6所示，x轴代表时间(ms)，y轴代表两轮机器人车身的倾斜程度，红色数据为PID控制器所控制的两轮机器人之倾斜角度，蓝色数据则为自适应模糊控制器所控制的两轮机器人之倾斜角度。 

由图6可知，由自适应模糊控制器所控制的两轮机器人，在4秒后车身的倾斜程度随着时间的递增而逐步进入稳定状态，两轮机器人的车身倾斜程度都保持在之间；反观由PID控制所控制的两轮机器人，约在第8秒、第15秒及第19秒时，两轮机器人的车身倾斜程度都有突然暴增之迹象，因此本发明可以得知，本发明所提出的自适应模糊控制器，对于控制两轮机器人在直立状态下的平衡能力不仅较典型的PID控制器稳定，平衡效果也优于典型的PID控制器。

Claims (4)

1.两轮机器人自适应模糊平衡控制器，其特征在于，包括以下步骤：

步骤一：将两轮机器人平衡于地面（1-1）；

步骤二、打开主机陀螺仪读取初始电压（1-2）：在两轮机器人偏摆时，打开主机陀螺仪，一开始陀螺仪偏移后会产生一个初始电压；

步骤三、计算电压偏移量计算倾斜角度（1-3）：将这一个参数先透过陀螺仪比例计算的方式固定出电压偏移量，然后计算出倾斜角度同时解决角度误差的问题；

步骤四、计算左右轮的速度差与位置差（1-4）；

步骤五、自适应定律的推导及自适应模糊控制器设计（1-5）；

步骤六、控制马达及陀螺仪的输出（1-6）：以自适应模糊控制器来控制陀螺仪以及马达，经过加总后控制马达的功率输出；

步骤七：产生新的角速度（1-7）：马达动作后，经由运算输出产生出新的角速度，再重新计算出角度，然后继续对陀螺仪及马达转速编码做自适应模糊控制器的控制，加总后一样对马达做功率输出，如此周而复始，便能让此系统趋于稳定。

2.根据权利要求1所述的方法：所述步骤三、计算电压偏移量计算倾斜角度的方式为：

如（1）式所示，取陀螺仪输出电压值平均值以求得当时陀螺仪在静止时其输出电压的准位，其中                                                、、、为任意取某一陀螺仪输出电压值，、为某一陀螺仪输出电压值其平均值；

如（2）式所示，将所求得的准位减去2.5V后求得其平均的偏移量，其中为实际输出电压准位；之后，每次陀螺仪讯号进来以后减去偏移量当作补偿；

，                                       (1) 

                                (2) 。

3.根据权利要求1所述的方法：所述步骤四、计算左右轮的速度差与位置差的方式为：

G₁为NXT 的中心轴，也同时是陀螺仪里所选的中心轴，Oa 及Ob 各是左右两边圆形轮子的中心点(圆心)，O1是Oa 及Ob 所连起的直线之中心点，同时也是G₁中心轴垂直坐落的重心，另外则是身体前倾或后仰的情况，G₂、G₃是G₁偏移时的轴心，为前倾或后仰的角度，S₁为两颗轮子向前的力量，S₂为两颗轮子向后的力量；两轮机器人一开始是平衡于地面，轮子并无转动，也没有倾斜角度，所以将G₂、G₃设为零如（3）式，S₁、S₂也设为零如（4）式，当两轮机器人使动作后便会产生倾斜角度，其中倾斜角的计算方法如公式（5）所示 

 (3) (4)

 (5) 

若所须要的操作角度介于±20°以内的话，则其倾斜角的计算方法可简化为 

 (6) 

此简化计算的误差范围小于 2％；如此即可免去计算反正弦函数(sin^-1) 的麻烦，并增加程序计算的速度；

 在要求两轮转速一致与控制器设计方式简单的前提下，由运动力学可以得知，移动平台底座所施给机器人的力为两轮所出之力的和U，如果在其中一轮的输出上加上两轮位置差之误差 ，另一轮减掉两轮位置差之误差 ，那么合力依旧为原来的力，不会破坏掉原来平衡方法所需要的力矩；又由于两轮的输出力量不同，会造成速度差，是控制两轮同步的速度差公式如（7）式所示；是控制两轮同步的位置差公式如（8）式所示； 

 (7) 

其中：右轮转速；：左轮转速 

：两轮所出之力的和 

    (8) 

其中: 右轮位置；: 左轮位置。

4. 根据权利要求1所述的方法：所述步骤五、自适应定律的推导及自适应模糊控制器设计的方式为：

首先，推导自适应定律的过程如下：

一般情况下，用于两轮机器人的理想控制器可表示为：

      （9）

其中为未知向量而为未知矩阵，为受控系统之输出，为控制器之输出，为状态向量；但由于、未知，理想控制器无法实现；

理想控制器（9）表示为：

                                （10）

我们将所设计之模糊控制器及公式(10)的理想控制器一同代入公式(9)的多输入多输出2阶非线性受控系统中，用以取代掉原先的控制器 ，可以得到 

移项后可以得到

     （11）

假设存在一个最佳的模糊控制器，该模糊控制器可以表示为如下形式：

                   （12）

假设

              （13）

其中参数、与为我们所假设可获得的最佳控制参数，该参数将使得假设存在的最佳模糊控制器与理想控制器之间存在一个最小的近似误差；因此，我们可以保证系统的稳定性；

 然后，设计自适应模糊控制器的过程如下：

根据以上推导出的自适应定律，设计一个模糊控制器用以近似理想控制器，该模糊控制器可以表示为，为模糊控制器之输入，此输入是根据输出电压（），倾斜角度（），左右轮的位置差（），两轮同步的速度差（）等四个变量与期望值（、、、）之间的误差及误差变化率（、、、），所取得，由于两轮机器人属于一个多输入多输出的系统，这将导致我们难以设计模糊控制器中的模糊规则，因此根据经验以及两轮机器人的实际输出响应，将两轮机器人所有的输出误差做一整合，并将其定义为，而误差总和之变化率则定义为，模糊控制器之输入描述如下： 

其中是两轮机器人的各项输出增益，是根据经验以及两轮机器人的实际输出响应所给定，为输出电压（）增益，为倾斜角度（）增益，为左右轮的位置差（）增益，为两轮同步的速度差（）增益；

则为模糊控制器之输出，也就是模糊控制器根据输入的两轮平衡车误差（）及误差变化（）所修正的控制讯号，该控制讯号将影响左、右伺服马达的输出，模糊控制器之输出表示如下：

      。