CN111665838A - 一种自平衡机器人抗持续性外力作用的姿态控制方法 - Google Patents

Abstract

本发明公开了一种自平衡机器人抗持续性外力作用的姿态控制方法，包括定时读取左右轮轴分别对应的编码器的数值，将两编码器的数值相加作为状态参数，所述状态参数具有正负；根据所述状态参数判断当前自平衡机器人的运行状态；根据所述运动状态，采用PID控制器调节自平衡机器人维持原地。本发明通过编码器判断当前自平衡机器人的运行状态，以避免陀螺仪无法正确反映自平衡机器人当前运行状态的弊端，并且通过不同的运行状态采取不同的措施，以保证准确快速的修复自平衡机器人的波动，调节平衡机器人的姿态，以实现始终维持原地平衡。

Description

一种自平衡机器人抗持续性外力作用的姿态控制方法

技术领域

本申请属于自平衡机器人领域，具体涉及一种自平衡机器人抗持续性外力作用的姿态控制方法。

背景技术

常规的两轮自平衡机器人与站在平衡机器人上的人组成了一个完整的系统，乘客可以通过调整自身的重心，来应对不同的状况。但是中小型两轮自平衡机器人在原地维持平衡过程中，当机器人受到一个较大的持续性水平外力影响的时候，为了维持平衡机器人会出现偏移甚至向一个方向移动的情况。通常会在这种情况下就需要自平衡机器人能够判断出外力的方向，并作出相应的反应以防止机器人向一个方向继续偏移，以影响机器人的正常工作。

当前两轮自平衡车普遍通过调整配重块的方式来实现重心的调整。而对于一些用于特殊场合的中小型两轮自平衡机器人来说，如果在原地维持平衡的过程中，遇到持续性水平外力的干扰，调整配重块可能会具有一定的滞后性，并且可能会对其工作造成影响，就需要机器人以其他的方式来改变其重心，以抵消持续性水平外力的干扰。

发明内容

本申请的目的在于提供一种自平衡机器人抗持续性外力作用的姿态控制方法，倾斜回正调整速度快，且在持续性水平外力下自平衡机器人仍能够维持原地平衡的姿态。

为实现上述目的，本申请所采取的技术方案为：

一种自平衡机器人抗持续性外力作用的姿态控制方法，所述自平衡机器人为两轮自平衡机器人且左右轮轴上分别安装有编码器，所述自平衡机器人抗持续性外力作用的姿态控制方法，包括：

步骤S1、定时读取左右轮轴分别对应的编码器的数值，将两编码器的数值相加作为状态参数，所述状态参数具有正负；

步骤S2、根据所述状态参数判断当前自平衡机器人的运行状态，包括：

若所述状态参数的绝对值小于第一临界值，则当前自平衡机器人的运行状态为正常工作状态；

若所述状态参数的绝对值大于第二临界值，则当前自平衡机器人的运行状态为失控偏移状态；

若所述状态参数的绝对值位于第一临界值和第二临界值之间，则当前自平衡机器人的运行状态为可调偏移状态；

步骤S3、根据所述运动状态，采用PID控制器调节自平衡机器人的姿态，包括：

若运行状态为正常工作状态，则不需要重心调节；

若运行状态为可调偏移状态，则通过速度环PI控制器抵消自平衡机器人受的持续性水平外力维持原地平衡；

若运行状态为失控偏移状态，则通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡。

作为优选，若所述自平衡机器人为向前运动，则状态参数为正数；若所述自平衡机器人为向后运动，则状态参数为负数。

作为优选，所述通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡，包括：

建立角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*Angle+Balance_Kd*Gyro

式中，Balance_PWM为角度环PD控制器最终输出的PWM波大小；Balance_Kp为PD环的比例参数；Angle是陀螺仪采集到的自平衡机器人当前倾角；Balance_Kd为PD环的微分参数；Gyro是陀螺仪采集到的自平衡机器人当前倾斜方向的角速度；

则角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Central_Point)+Balance_Kd*Gyro

式中，Central_Point为设置机器人重心点的值，当Central_Point的值取正数时，则调整机器人的重心点向后改变，当Central_Point的值取负数时，则调整机器人的重心点向前改变，并且Central_Point的绝对值越大，机器人的重心点离机器人的物理重心零点越远，即机器人的倾斜角度越大；

其中，设置自平衡机器人重心点的值Central_Point的调整公式如下：

Central_Point＝Mechanical_Zero+Angle_Bias

式中，Mechanical_Zero为机器人机体的物理重心零点，且取值为0，Angle_Bias为每隔预设间隔改变自平衡机器人的角度值，如果需要自平衡机器人重心点往后偏移，则Angle_Bias值取正数；如果需要自平衡机器人重心点往前偏移，则Angle_Bias值取负数；

因此，更新角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Mechanical_Zero+Angle_Bias)+Balance_Kd*Gyro

根据角度补偿的角度环PD控制器的输出Balance_PWM，改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡。

作为优选，所述每隔预设间隔改变自平衡机器人的角度值Angle_Bias的取值范围为[-0.05，-0.01]∪[0.01，0.05]。

本申请提供的自平衡机器人抗持续性外力作用的姿态控制方法，通过编码器判断当前自平衡机器人的运行状态，以避免陀螺仪无法正确反映自平衡机器人当前运行状态的弊端，并且通过不同的运行状态采取不同的措施，以保证准确快速的修复自平衡机器人的波动，调节平衡机器人的姿态，以实现始终维持原地平衡。

附图说明

图1为本申请的自平衡机器人抗持续性外力作用的姿态控制方法的流程图；

图2为本申请的状态参数取值范围的示意图。

具体实施方式

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

除非另有定义，本文所使用的所有的技术和科学术语与属于本申请的技术领域的技术人员通常理解的含义相同。本文中在本申请的说明书中所使用的术语只是为了描述具体的实施例的目的，不是在于限制本申请。

其中一个实施例中，提供一种自平衡机器人抗持续性外力作用的姿态控制方法，以克服自平衡机器人在受到较大的持续性水平外力之下无法维持平衡姿态的问题。

这里的持续性水平外力理解为在自平衡机器人运行过程中始终受该水平外力的作用。本申请的方法可辅助自平衡机器人正常完成作业，例如当一款自平衡消防机器人在进行灭火作业时，需要用喷头向外喷洒水雾，此时就可能对机器人的重心造成影响，而利用本申请的方法进行姿态控制即可避免喷洒水务对机器人重心的影响。

本实施例中的自平衡机器人为两轮自平衡机器人且该自平衡机器人的左右轮轴上分别安装有编码器，且还安装有用于采集自平衡机器人的角度和角速度的陀螺仪。这里的编码器主要为了采集左右轮的转数，在实现该功能的前提下，对编码器的安装位置和方式不做严格限制，以上提供的编码器安装在轮轴上仅为一种优选方式。

如图1所示，本实施例的自平衡机器人抗持续性外力作用的姿态控制方法，包括以下步骤：

步骤S1、定时读取左右轮轴分别对应的编码器的数值，将两编码器的数值相加作为状态参数。

为了便于读数，本实施例中自平衡机器人的左右轮轴上安装的为360分度的光电编码器，通过采集光电编码器的数值可以得知左右轮当前的转数，并对两者进行求和处理，公式为：

MotorPulse＝LeftMotorPulse+RightMotorPulse (1)

式中，MotorPulse为采集到的左右轮光电编码器相加后的数值，即状态参数；LeftMotorPulse为左轮光电编码器采集到的数值；RightMotorPulse为右轮光电编码器采集到的数值。

为了保证快速校正自平衡机器人的状态，并且控制计算量，在一实施例中，设置编码器数值定时读取的时间间隔为10ms。

步骤S2、根据所述状态参数判断当前自平衡机器人的运行状态。

通过MotorPulse的大小就可以判断自平衡机器人当前的运行状态。在自平衡机器人没有受到水平外力，并且处在原地的时候，MotorPulse维持在一个固定的范围内，可以将编码器正常工作的范围称为范围1。当MotorPulse值超过另一范围的时候，可以判断机器人持续向某个方向偏移，则可认为机器人受到一个较大的持续性水平外力，可以将机器人发生偏移的范围称为范围2。在范围1到范围2的过程中，还存在一段范围，在该范围内能够在不改变机器人重心点的情况下抵消较小的水平外力，称该范围为范围3。范围1，范围2和范围3的关系如图2所示。

因此判断当前自平衡机器人的运行状态，具体包括：

a、若所述状态参数的绝对值小于第一临界值(即处于范围1内)，则当前自平衡机器人的运行状态为正常工作状态。

b、若所述状态参数的绝对值大于第二临界值(即处于范围2内)，则当前自平衡机器人的运行状态为失控偏移状态。

c、若所述状态参数的绝对值位于第一临界值和第二临界值之间(即处于范围3内)，包括第一临界值和第二临界值，则当前自平衡机器人的运行状态为可调偏移状态。

在编码器的实际读数中，对于自平衡机器人而言，具有前后两种运行方向，其中一个运行方向对应的编码器读数为正，另一个运行方向对应的编码器读数为负。

在一个实例中，设置自平衡机器人处在向前运动时，LeftMotorPulse与RightMotorPulse值为正，即状态参数为正数；设置自平衡机器人为向后运动时，LeftMotorPulse与RightMotorPulse值为负，即状态参数为负数。

并且为了降低运行状态误判情况，令范围1与范围3的第一临界值为edge1，范围2与范围3的第二临界值为edge2。那么当连续N次采集到的数据均为MotorPulse<-edge2时可以认为机器人因为受到一个较大的持续性水平外力在向后偏移；当连续N次采集到的数据均为MotorPulse>edge2时可以认为机器人因为受到一个较大的持续性水平外力在向前偏移。

优选的，在一实施例中，设置edge1＝10，edge2＝22，N＝10，能够得到较优的原地维持效果。该设置下当MotorPulse的取值连续10次始终在-10到10之间此时认为机器人处于正常工作状态，也就是范围1。当MotorPulse的取值连续10次出现在大于22或小于-22的时候，判定机器人在受到外力，且发生失控偏移状态，也就是范围2。当MotorPulse的取值连续10次出现在-22到-10或10到22之间时，认为机器人受到外力，但可以不需要改变重心原点就可以抵消外力，发生可调偏移状态，即处于范围3。

步骤S3、根据所述运动状态，采用PID控制器调节自平衡机器人的姿态。本申请对自平衡机器人的姿态控制主要在于控制自平衡机器人在受持续性外力作用下维持原地平衡，从而保证自平衡机器人的正常作业。具体调控过程包括：

a、若运行状态为正常工作状态，即MotorPulse值在范围1内，可以认定机器人处于正常工作状态，此时不需要进行重心调节，通过自身的平衡处理(例如采用不加补充的常规的PID调节)即可维持原地平衡。

b、若运行状态为可调偏移状态，即MotorPulse值在范围3内，通过速度环PI控制器抵消自平衡机器人受的持续性水平外力维持原地平衡，即当通过编码器判断轮子出现偏移的趋势时，减小轮子向偏移方向的输出功率，以消除偏移。

c、若运行状态为失控偏移状态，通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡。

目前自平衡机器人基本依靠陀螺仪进行重心调节，在推力比较小时，机器人会倾斜向推力方向产生反作用力，此时机器人不会移动；当水平外力足够大时，机器人会被推动，但是倾斜角度变化不大。相当于力量超过了机器人自身能维持角度的值以后，机器人倾斜角度变化不大，而且会发生偏移，此时陀螺仪无法准确判断机器人的倾角，即无法使机器人通过陀螺仪的数据维持原地平衡。

本实施例提供的通过角度补偿的角度环PD控制器能够很好的克服上述缺陷，维持自平衡机器人维持原地平衡具体包括以下步骤：

自平衡机器人由PID控制器来实现自平衡，PID控制器包括了角度环PD控制器和速度环PI控制器。通过PD控制器可以控制机器人对角度的反应速度，通过PI控制器可以控制机器人对轮子转速的反应速度。

在一个实施例中，设置自平衡机器人向前倾斜，陀螺仪采集到的角度和角速度信息为正；自平衡机器人向后倾斜，则陀螺仪采集到的角度和角速度为负，那么建立PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*Angle+Balance_Kd*Gyro (2)

式中，Balance_PWM为角度环PD控制器最终输出的PWM波大小；Balance_Kp为PD环的比例参数；Angle是陀螺仪采集到的自平衡机器人当前倾角，即角度；Balance_Kd为PD环的微分参数；Gyro是陀螺仪采集到的自平衡机器人当前倾斜方向的角速度。

当需要改变自平衡机器人的重心点，让机器人往一个方向倾斜时，只需要对Angle的值进行补偿，则可以将公式(2)进行修改，得到角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Central_Point)+Balance_Kd*Gyro (3)

式中，Central_Point为设置机器人重心点的值，当Central_Point的值取正数时，则调整机器人的重心点向后改变，当Central_Point的值取负数时，则调整机器人的重心点向前改变。并且Central_Point的绝对值越大，机器人的重心点离机器人的物理重心零点越远，即机器人的倾斜角度越大。

当自平衡机器人重心往前的时候，机器人会向前倾斜，会有向前移动的趋势，产生一个向前的力；反之当自平衡机器人重心往后的时候，机器人会向后倾斜，则有向后移动的趋势，产生一个向后的力。

由于根据状态参数MotorPulse的正负可以判断当前自平衡机器人处于向前运动或向后运动，因此在进行自平衡机器人重心点调整之前，先根据状态参数判断偏移方向，即运动方向。

如果确定自平衡机器人在推力作用下向后偏移，则需要减少Central_Point的值使机器人的重心往前增加；反之如果确定机器人在推力作用下向前偏移，则需要增加Central_Point的值使机器人的重心往后增加。

其中，设置自平衡机器人重心点的值Central_Point的调整公式如下：

Central_Point＝Mechanical_Zero+Angle_Bias (4)

式中，Mechanical_Zero为机器人机体的物理重心零点，通常取值为0。Angle_Bias为每隔预设间隔改变自平衡机器人的角度值，如果需要自平衡机器人重心点往后偏移，则Angle_Bias值取正数；如果需要自平衡机器人重心点往前偏移，则Angle_Bias值取负数。

Angle_Bias如果过大，则容易导致改变过度，并容易影响平衡效果，如果过小则反应过慢，会有较大的偏移量，因此在一实施例中，设置Angle_Bias的取值范围为[-0.05，-0.01]∪[0.01，0.05]。并且为了防止对平衡产生过大的干扰，设置改变自平衡机器人的角度值的预设间隔为每10ms改变一次。

这里改变自平衡机器人的角度值的预设间隔指机器人已判断位于某范围内，并且明确需要从角度A改变到角度B情况下的预设间隔。例如编码器前面10次的10ms判断机器人的状态都位于范围3内，那么接下来每10ms就可以改变一次角度值，除非里面有一次编码器的判断进入到范围1内，则需要重新连续判断10次确定机器人的状态。在范围3内每10ms去改变一次Angle_Bias的值时，比如要从0度改到1度，每次改0.05度，在没有进入到范围1内的前提下，就需要连续改20次，耗时200ms。将公式(4)代入公式(3)更新角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Mechanical_Zero+Angle_Bias)+Balance_Kd*Gyro (5)

根据角度补偿的角度环PD控制器的输出Balance_PWM，改变自平衡机器人的重心。通过调整重心点，当机器人倾斜角度达到一个值后，其产生的力与受到水平外力基本相同，此时机器人则不再发生偏移，MotorPulse值在范围3内，但是为了使机器人具有更好的平衡效果，需要使MotorPulse值维持在范围1内，即连续10次采集的MotorPulse值均处在范围1内，则判断机器人不再继续进行偏移，此时，令Angle_Bias的值为0，停止机器人重心角的变化。

本实施例提供编码器反映自平衡机器人的运行状态，即自平衡机器人被拖动以后做出反应，增加反作用力维持原地，这个过程可能会被推动一段非常小的距离，然后机器人会在几百毫秒内立刻做出反应，相当于维持原地平衡。

本实施例提供的自平衡机器人抗持续性外力作用的姿态控制方法，在自平衡机器人在原地维持平衡过程中，当受到一个较大的持续性水平外力的时候，通过对编码器的变化情况来判断机器人受到外力方向，并通过改变维持机器人自平衡的物理重心原地的大小，使机器人向外力来源方向倾斜，以此来产生一个与外力方向相反的力来抵消外力，以达到减小或消除机器人偏移的目的，该方法对自平衡机器人的调节准确且快速，能够维持其位于原地，提高自平衡机器人姿态控制的准确性和高效性。

以上所述实施例的各技术特征可以进行任意的组合，为使描述简洁，未对上述实施例中的各技术特征所有可能的组合都进行描述，然而，只要这些技术特征的组合不存在矛盾，都应当认为是本说明书记载的范围。

以上所述实施例仅表达了本申请的几种实施方式，其描述较为具体和详细，但并不能因此而理解为对发明专利范围的限制。应当指出的是，对于本领域的普通技术人员来说，在不脱离本申请构思的前提下，还可以做出若干变形和改进，这些都属于本申请的保护范围。因此，本申请专利的保护范围应以所附权利要求为准。

Claims (4)

1.一种自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，所述自平衡机器人为两轮自平衡机器人且左右轮轴上分别安装有编码器，所述自平衡机器人抗持续性外力作用的姿态控制方法，包括：

步骤S1、定时读取左右轮轴分别对应的编码器的数值，将两编码器的数值相加作为状态参数，所述状态参数具有正负；

步骤S2、根据所述状态参数判断当前自平衡机器人的运行状态，包括：

若所述状态参数的绝对值小于第一临界值，则当前自平衡机器人的运行状态为正常工作状态；

若所述状态参数的绝对值大于第二临界值，则当前自平衡机器人的运行状态为失控偏移状态；

若所述状态参数的绝对值位于第一临界值和第二临界值之间，则当前自平衡机器人的运行状态为可调偏移状态；

步骤S3、根据所述运动状态，采用PID控制器调节自平衡机器人的姿态，包括：

若运行状态为正常工作状态，则不需要重心调节；

若运行状态为可调偏移状态，则通过速度环PI控制器抵消自平衡机器人受的持续性水平外力维持原地平衡；

若运行状态为失控偏移状态，则通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡。

2.如权利要求1所述的自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，若所述自平衡机器人为向前运动，则状态参数为正数；若所述自平衡机器人为向后运动，则状态参数为负数。

3.如权利要求1所述的自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，所述通过角度补偿的角度环PD控制器改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡，包括：

建立角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*Angle+Balance_Kd*Gyro

式中，Balance_PWM为角度环PD控制器最终输出的PWM波大小；Balance_Kp为PD环的比例参数；Angle是陀螺仪采集到的自平衡机器人当前倾角；Balance_Kd为PD环的微分参数；Gyro是陀螺仪采集到的自平衡机器人当前倾斜方向的角速度；

则角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Central_Point)+Balance_Kd*Gyro

式中，Central_Point为设置机器人重心点的值，当Central_Point的值取正数时，则调整机器人的重心点向后改变，当Central_Point的值取负数时，则调整机器人的重心点向前改变，并且Central_Point的绝对值越大，机器人的重心点离机器人的物理重心零点越远，即机器人的倾斜角度越大；

其中，设置自平衡机器人重心点的值Central_Point的调整公式如下：

Central_Point＝Mechanical_Zero+Angle_Bias

式中，Mechanical_Zero为机器人机体的物理重心零点，且取值为0，Angle_Bias为每隔预设间隔改变自平衡机器人的角度值，如果需要自平衡机器人重心点往后偏移，则Angle_Bias值取正数；如果需要自平衡机器人重心点往前偏移，则Angle_Bias值取负数；

因此，更新角度补偿的角度环PD控制器的输出公式为：

Balance_PWM＝Balance_Kp*(Angle+Mechanical_Zero+Angle_Bias)+Balance_Kd*Gyro

根据角度补偿的角度环PD控制器的输出Balance_PWM，改变自平衡机器人的重心，抵消受到的持续性水平外力维持原地平衡。

4.如权利要求3所述的自平衡机器人抗持续性外力作用的姿态控制方法，其特征在于，所述每隔预设间隔改变自平衡机器人的角度值Angle_Bias的取值范围为[-0.05，-0.01]∪[0.01，0.05]。

Images