CN109202883A - 一种自平衡机器人的位置控制方法 - Google Patents

Abstract

本发明描述了一种电机编码的自平衡机器人的位置控制方法，包括：对自平衡机器人的各个模块进行初始化操作；获取自平衡机器人的传感数据并且进行姿态解算；将姿态解算的姿态信息以及自平衡机器人的位置信息作为自平衡位置控制器的输入信息，进行自平衡机器人的平衡状态判断，从而确定是否更改控制策略；执行模块接收并且执行自平衡位置控制器所输出的控制策略。本发明提出的自平衡机器人的位置控制方法，采用电机编码实时反馈机器人位置信息，进行PID闭环控制，有效地提高机器人的抗干扰能力，尤其对自平衡机器人动态静止时的零点漂移有较明显的抑制效果。

Description

一种自平衡机器人的位置控制方法

技术领域

本发明涉及机器人控制技术领域，具体涉及一种基于电机编码的两轮自平衡机器人位置控制方法。

背景技术

近年来随着科技的发展，机器人从最初应用的工业领域逐渐走进人们日常生活之中，在服务、物流、医疗、教育等领域中已经有一定的应用实例。自平衡机器人是应运而生的一类产品，自平衡机器人具有与自平衡车一样的两轮自平衡结构，与传统轮式机器人相比大幅度缩减了机器人尺寸，同时可以进行更加灵活的运动，能够适用于一些轮式机器人无法工作的狭小空间。

目前自平衡机器人产品较少，主要因为在大型的机器人产品中使用自平衡技术难度较大，尤其是自平衡技术中的控制算法，控制算法能够直接决定机器人能否自平衡站立及运动。目前，两轮自平衡机器人的控制算法主要包括两类：一类是传统的PID方法和极点配置法，主要处理线性已知系统；由于两轮自平衡机器人具有强非线性性，因此对机器人模型进行线性化后将导致模型不精确、抗干扰能力差。另一类是智能控制，主要采用模糊控制，能够处理非线性未知系统，但模糊控制方法的控制精度取决于模糊规则库的建立，控制系统的效果受专家经验影响非常大

针对传统的控制方法、动态响应不理想、抗干扰能力差；智能控制方法过度地依赖专家经验，急需一种简单且抗干扰能力强的自平衡机器人的位置控制方法。

发明内容

本发明要解决的技术问题是提供一种基于电机编码的两轮自平衡机器人的位置控制方法。

为解决上述技术问题，提供一种基于电机编码的自平衡机器人的位置控制方法，所述方法包括：步骤一：对自平衡机器人的各个模块进行初始化操作；步骤二：获取自平衡机器人的传感数据并且进行姿态解算；步骤三：将步骤二中姿态解算的姿态信息以及自平衡机器人的位置信息作为自平衡位置控制器的输入信息，进行自平衡机器人的平衡状态判断，从而确定是否更改控制策略；步骤四：执行模块接收并且执行步骤三中自平衡位置控制器所输出的控制策略；所述执行模块包括电机、驱动轮、以及用于实时反馈自平衡机器人的位置信息的编码器，所述电机连接所述驱动轮，所述编码器连接所述电机，所述编码器将所述电机旋转产生的角度变化转变成自身的码数变化，进而产生电信号的变化，所述电信号的变化作为自平衡机器人的位置信息反馈给所述自平衡位置控制器。

优选的，步骤一中所述的模块初始化包括控制板卡初始化、传感器初始化、自平衡机器人重心初始化、执行模块初始化。

优选的，所述控制板卡初始化包括主控时钟初始化、中断系统初始化、各个通信模块初始化。

优选的，用于获取自平衡机器人的传感数据的传感器为微机电六轴运动传感器，包括加速度计和陀螺仪。

优选的，对所述六轴运动传感器所测的传感数据进行卡尔曼滤波进行姿态解算。

优选的，所述姿态解算的步骤包括利用陀螺仪所获取的角速度构建卡尔曼状态方程，利用加速度计获取各轴加速度分量并计算获取角度而构建卡尔曼测量方程，建立离散化卡尔曼系统方程，计算获得姿态解算结果。

优选的，所述自平衡控制器采用闭环PID控制。

优选的，将所述编码器所反应自平衡机器人的实时位置信息作为自平衡控制器的反馈量，通过位置迭代的方式得到自平衡控制的控制量，将控制量和反馈量的差作为闭环PID控制的输入量。

优选的，所述位置迭代的方式包括：将当前位置和变化位置作和后迭代更新为下一次的当前位置，所述变化位置包括位置前进、位置后退和位置静止。

优选的，所述编码器的码数变化与所述自平衡机器人的位置变化的对应计算公式为：其中，自平衡机器人在初始位置时编码器的码数为A，电机旋转α角度且带动驱动轮运动L_AB距离后，编码器的码数变为B；其中R为驱动轮半径，M为码盘一圈总数。

本发明提出的基于电机编码的两轮自平衡机器人的位置控制方法，采用电机编码实时反馈机器人位置信息，进行PID闭环控制，有效地提高机器人的抗干扰能力，尤其对自平衡机器人动态静止时的零点漂移有较明显的抑制效果。该方法同时也可以提高机器人的运动精度，使自平衡机器人可以进行位置定量的运动，进而解决目前自平衡机器人尚不具备的定位等功能。

下面结合附图和具体实施方式对本发明作进一步详细的说明。

附图说明

图1是本发明一实施例流程框图；

图2是姿态解算流程框图；

图3是实施例中自平衡位置控制器工作流程示意图；

图4是位置信息迭代示意图；

图5是编码器反馈位置信息示意图；

图6是图1所对应实施例的详细实施流程示意图。

具体实施方式

为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

图1为本发明一实施例的基于电机编码的两轮自平衡机器人的位置控制方法流程框图。该实施例的流程步骤主要包括：

步骤S11：对自平衡机器人的各个模块进行初始化操作。该步骤为整个自平衡机器人系统的初始化步骤，是后续步骤以及自平衡机器人其他模块能够正常工作的前提。

初始化操作主要包括控制板卡初始化、传感器初始化、自平衡机器人重心初始化、执行模块初始化。其中，控制板卡初始化主要包括主控时钟初始化、中断系统初始化、各个通信模块初始化。时钟初始化为姿态解算周期，自平衡位置控制器周期等时间相关模块提供稳定的时钟输出，保证实时准确的控制效果；中断初始化能够实时准确地控制嵌入式操作系统，实现一系列功能；通信模块初始化为芯片之间的通信提供了保障。传感器的初始化：对用于检测姿态的传感器的量程、灵敏度等进行初始化，以便能够准确得到机器人的姿态信息。自平衡机器人系统重心初始化：对整个系统的重心进行初始化，以便进行自平衡控制。执行模块的初始化：对电机相关参数初始化，尤其是对执行模块中的编码器的初始值进行记录。

步骤S12:获取自平衡机器人的传感数据并且进行姿态解算。在步骤S11中实现传感器初始化之后，通过姿态检测传感器获取自平衡机器人的实时姿态，用于下一步骤的平衡控制。该步骤通过选取适当的姿态传感器将实时姿态信息传递给自平衡机器人系统，系统对姿态信息进行解算。由于一般姿态传感器的数据无法直接应用于自平衡控制，在该步骤还需要根据实际传感器的选择与系统情况进行滤波、拟合等计算，从而获得稳定可控的姿态信息并传递给下一步骤。该步骤在系统运行后周期性工作，实时对执行模块电机驱动后的机器人姿态(即步骤S14后的姿态)进行检测和解算。

在一具体实施例中，用于获取自平衡机器人的传感数据的传感器为微机电六轴运动传感器，包括加速度计和陀螺仪。微机电六轴运动传感器的成本低于传统用于检测姿态倾角的机械式陀螺仪成本，从而有效地降低自平衡机器人系统的成本。六轴运动传感器中集成的是微机电式陀螺仪，里面没有旋转部件，而是一个石英做的震动机构。当陀螺仪沿敏感轴有旋转运动时，震动机构会产生科里奥利力，这个力会反作用于震动机构中的元件使其产生扭曲。通过检测这个微小的扭曲就能知道沿陀螺仪敏感轴方向的角速度。加速度由感应模块感知传感器在空间坐标系中各个轴方向上的惯性力分量，亦即加速度分量。由于制造工艺所限，六轴运动传感器中的陀螺仪精度与稳定性较差，动态性能较好但零漂严重；加速度计静态性能较好，动态性能差，尤其是在振动环境中容易出现较大的测量偏差。根据六轴运动传感器的上述特性，在一优选实施例中，对所述六轴运动传感器所测的传感数据进行卡尔曼滤波进行姿态解算。具体的姿态解算流程框图如图2所示。

步骤S21：根据陀螺仪动态性能优异的特点，将其获取的角速度值作为卡尔曼滤波器中的状态方程，具体表达式如公式1所示：

其中为卡尔曼滤波器的最优估计值，即对传感器数据融合后的输出姿态倾角；为最优估计值的偏差，用于每次进行卡尔曼滤波时迭代计算；u_gyro(t)为陀螺仪检测到的实时角速度值；w(t)为过程噪声。

步骤S22:根据加速度计静态性能优异不受时间限制的特点，将其输出作为卡尔曼滤波器中的测量方程，具体的表达式如公式2所示：

其中z(t)为加速度计计算得到的角度值，v(t)为对应的测量噪声。优选的，步骤S21和步骤S22可以同步进行，当然也可以顺次或者逆次进行。

步骤S23：在公式1和公式2的基础上，根据自平衡机器人系统的实时性，将传统卡尔曼滤波器离散化。在该实施例中，设定了定时中断Ts，每Ts作为一个周期读取传感器数据，将陀螺仪的输出值作为卡尔曼滤波器中的状态控制量、加速度计的输出值作为测量量。按照卡尔曼滤波器迭代更新格式将过程噪声与测量噪声提取转化为噪声协方差矩阵可以得到卡尔曼滤波器的系统方程，具体表达式如公式3所示：

其中，t＝0,1,2…为离散时间；z(t)为通过加速度计计算得到的实时角度值；u_gyro(t)为陀螺仪检测到的实时角速度值。x₁(t+T_s)为卡尔曼滤波器对传感器数据融合后的输出值(最优估计值)，x₂(t+T_s)为最优估计值的偏差。

在上述数学推导的基础上，通过编写循环程序实现卡尔曼滤波器的迭代计算，并将每一次计算得到的姿态信息传递给步骤S13的自平衡位置控制器，便于进行姿态控制。

步骤S13：将步骤S12中姿态解算的姿态信息以及自平衡机器人的位置信息作为自平衡位置控制器的输入信息，进行自平衡机器人的平衡状态判断，从而确定是否更改控制策略。

自平衡位置控制器的输入为步骤S12输出的姿态信息，以及步骤S14返回的自平衡机器人位置信息。控制器的输出直接给定到步骤S14的执行模块，由电机驱动自平衡机器人完成相应的位置定量运动。在一优选实施例中，自平衡控制器采用闭环PID控制，即自平衡位置控制器的内部为传统的PID控制器，通过合理的设置PID控制参数来完成位置控制的功能。自平衡机器人系统运行后并且没有运动指令需要机器人自平衡站立时，由自平衡位置控制器根据S11步骤中初始化的重心信息以及S12和S14步骤输入的信息进行计算，自动判断机器人是否处于平衡状态，若处于不平衡状态则输出控制指令到步骤S14。如果有运动指令，则更改位置控制策略，计算出执行机构需要的控制指令。

自平衡位置控制器的工作过程的具体步骤流程如图3所示。

步骤S31：接收步骤S14中编码器所返回的位置信息。

步骤S32：进行运动指令的解析。

步骤S33：将所解析的运动指令作为自平衡控制器的输入量。

步骤S34：将自平衡控制器的输出量控制执行模块。其中，执行模块包括编码器。

PID控制器(Proportion Integration Differentiation。比例-积分-微分控制器)，由比例单元P、积分单元I和微分单元D组成。通过Kp，Ki和Kd三个参数的设定。PID控制器主要适用于基本线性和动态特性不随时间变化的系统，是一个在工业控制应用中常见的反馈回路部件。这个控制器把收集到的数据和一个参考值进行比较，然后把这个差别用于计算新的输入值，这个新的输入值的目的是可以让系统的数据达到或者保持在参考值。和其他简单的控制运算不同，PID控制器可以根据历史数据和差别的出现率来调整输入值，这样可以使系统更加准确，更加稳定。可以通过数学的方法证明，在其他控制方法导致系统有稳定误差或过程反复的情况下，一个PID反馈回路却可以保持系统的稳定。

在该实施例中，对机器人的位置量进行闭环控制，将执行模块中的编码器所反应自平衡机器人的实时位置信息作为自平衡控制器的反馈量，通过位置迭代的方式得到自平衡控制的控制量，将控制量和反馈量的差作为闭环PID控制的输入量，计算后将结果输出到执行机构中。

其中，位置迭代的方式包括：将当前位置和变化位置作和后迭代更新为下一次的当前位置，所述变化位置包括位置前进、位置后退和位置静止。具体的示意过程如图4所示，主要包括三种位置变化的迭代：

①当前位置+变化位置

其中，变化位置可能有三种状态，由上位机(控制器)决定：

a)前进。则变化位置为正值，经过该步骤当前位置向正方向增加。

b)后退。则变化位置为负值，经过该步骤当前位置向反方向增加。

c)静止站立。变化位置为零，当前位置不变。

②赋值

将步骤①中计算后的值赋给位置控制器控制量，以便进行PID计算。

③更新

更新当前位置，新的当前位置为步骤②中的位置控制器控制量。新的当前位置用于下一次的迭代计算。

上述算法可以编写代码由嵌入式芯片周期性实现。

步骤S14:执行模块接收并且执行步骤S13中自平衡位置控制器所输出的控制策略。在该实施例中，执行模块包括电机、驱动轮、以及用于实时反馈自平衡机器人的位置信息的编码器。其中，电机连接驱动轮，编码器连接电机，编码器将电机旋转产生的角度变化转变成自身的码数变化，进而产生电信号的变化，电信号的变化作为自平衡机器人的位置信息反馈给所述自平衡位置控制器。

在该实施例中，编码器具有较高精度，用于实时、精准地反馈自平衡机器人的位置信息。如果是增量式编码器，则需要在系统初始化时对上述控制器中所说的“当前位置”清零；如果是绝对式编码器，则需要保留每次下电前的位置信息并在系统再次启动时将该信息赋予上述控制器中所说的“当前位置”。

图5详细描绘了带有编码器(即码盘)的电机如何反馈位置信息：码盘能够将电机旋转产生的角度变化通过其自身的码数变化反应出来，进而产生电信号的变化。将码盘与电机和驱动轮相连，能够通过检测码盘码数的变化来得到电机旋转角度，进而可以获取驱动轮位置变化信息。如图5所示，轮子初始位置码盘数为A，电机向指示方向旋转α角度，带动驱动轮运动L_AB距离后，码盘数变为B。当已知驱动轮半径R及码盘一圈总数M时，具体对应关系的表达式如公式4所示：

其中

图6是图1所对应实施例的详细实施流程示意图。步骤S61:对自平衡机器人系统进行初始化，初始化的具体内容如上文所述的步骤S11内容。步骤S62：获取自平衡机器人的传感数据并且进行姿态解算，具体的解算过程如上文所述的步骤S12内容。步骤S63：接收步骤S62中姿态解算的姿态信息以及后续判断执行过后的位置信息作为自平衡位置控制器的输入信息。步骤S64：判断自平衡机器人是否处于平衡状态，若处于平衡状态则进入步骤S65，否则进入步骤S69。步骤S65：判断自移动机器人是否需要进行前进或后退的动作，若需要则进入步骤S70，否则进入步骤S66。步骤S66：判断自移动机器人是否需要转向，若需要转向则进入步骤S71，否则进入步骤S67。步骤S67：判断自移动机器人的动作调整是否结束，若结束则结束整个流程，否则再次进入步骤S62进行循环。步骤S71：在步骤S66判定自平衡机器人需要转向时，则调整两轮的给定值，调整完成后进入步骤S69。步骤S70：在步骤S65判定自平衡机器人需要前进或后退时，更改位置控制器的控制量。步骤S69：在步骤S70或步骤S71调整命令完成后，驱动电机执行命令，该步骤完成后进入S68。步骤S68：在步骤S69的驱动电机执行命令完成后，返回电机具体的位置信息，并且将位置信息作为步骤S63的输入。

以上所述仅为本发明的实施方式，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (10)

1.一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，所述方法包括：

步骤一：对自平衡机器人的各个模块进行初始化操作；

步骤二：获取自平衡机器人的传感数据并且进行姿态解算；

步骤三：将步骤二中姿态解算的姿态信息以及自平衡机器人的位置信息作为自平衡位置控制器的输入信息，进行自平衡机器人的平衡状态判断，从而确定是否更改控制策略；

步骤四：执行模块接收并且执行步骤三中自平衡位置控制器所输出的控制策略；所述执行模块包括电机、驱动轮、以及用于实时反馈自平衡机器人的位置信息的编码器，所述电机连接所述驱动轮，所述编码器连接所述电机，所述编码器将所述电机旋转产生的角度变化转变成自身的码数变化，进而产生电信号的变化，所述电信号的变化作为自平衡机器人的位置信息反馈给所述自平衡位置控制器。

2.根据权利要求1所述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，步骤一中所述的模块初始化包括控制板卡初始化、传感器初始化、自平衡机器人重心初始化、执行模块初始化。

3.根据权利要求2述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，所述控制板卡初始化包括主控时钟初始化、中断系统初始化、各个通信模块初始化。

4.根据权利要求1所述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，用于获取自平衡机器人的传感数据的传感器为微机电六轴运动传感器，包括加速度计和陀螺仪。

5.根据权利要求4述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，对所述六轴运动传感器所测的传感数据进行卡尔曼滤波进行姿态解算。

6.根据权利要求4述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，所述姿态解算的步骤包括利用陀螺仪所获取的角速度构建卡尔曼状态方程，利用加速度计获取各轴加速度分量并计算获取角度而构建卡尔曼测量方程，建立离散化卡尔曼系统方程，计算获得姿态解算结果。

7.根据权利要求1所述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，所述自平衡控制器采用闭环PID控制。

8.根据权利要求7所述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，将所述编码器所反应自平衡机器人的实时位置信息作为自平衡控制器的反馈量，通过位置迭代的方式得到自平衡控制的控制量，将控制量和反馈量的差作为闭环PID控制的输入量。

9.根据权利要求8所述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，所述位置迭代的方式包括：将当前位置和变化位置作和后迭代更新为下一次的当前位置，所述变化位置包括位置前进、位置后退和位置静止。

10.根据权利要求1所述的一种基于电机编码的自平衡机器人的位置控制方法，其特征在于，所述编码器的码数变化与所述自平衡机器人的位置变化的对应计算公式为：

其中，

自平衡机器人在初始位置时编码器的码数为A，电机旋转α角度且带动驱动轮运动L_AB距离后，编码器的码数变为B；其中R为驱动轮半径，M为码盘一圈总数。