CN117111441A - 两轮自平衡机器人及其控制方法和装置 - Google Patents

Abstract

本申请公开了一种两轮自平衡机器人的控制方法，所述方法的一具体实施方式包括：采用角度PID与速度设定值为零的速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值；基于所述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将所述指令转换成目标速度设定值，获取两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，控制两轮自平衡机器人轮子的电机转动。该实施方式实现了在保持两轮自平衡机器人平衡的同时，精确的控制两轮自平衡机器人行驶。

Description

两轮自平衡机器人及其控制方法和装置

技术领域

本申请涉及智能控制领域，具体涉及两轮自平衡机器人及其控制方法和装置。

背景技术

两轮自平衡机器人属于倒立摆结构，不稳定。两轮自平衡机器人的结构被改变或姿态改变或负重，都能造成其重心变动。打破原有平衡。在未知新的重心的情况下，控制两轮自平衡机器人平衡行驶比较困难。

发明内容

本申请的目的在于提出一种改进的两轮自平衡机器人及其控制方法和装置，来解决以上背景技术部分提到的技术问题。

本申请提供了一种两轮自平衡机器人的控制方法，所述方法包括：采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，其中，所述速度PID的速度设定值为零；基于所述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将所述指令转换成目标速度设定值，获取所述两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动。

在一些实施例中，所述采用角度PID与速度设定值为零的速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，包括：将默认重心角度作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动，不断调整两轮自平衡机器人的倾斜角度，直到当前倾斜角度可等于所述默认重心角度；以预设时间间隔循环执行如下梯度步骤：获取所述两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，将更新后的当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制所述电机转动，来调整所述两轮自平衡机器人的倾斜角度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值不大于所述阈值，则将当前重心角度设定值输出为待选重心角度设定值；将所述待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，采用所述角度PID与速度PID叠加的双PID控制所述电机转动，不断调整所述两轮自平衡机器人的倾斜角度，如果在预设时间段内，所述两轮自平衡机器人的倾斜角度与所述待选重心角度设定值的差值的绝对值一直小于误差阈值，则将所述待选重心角度设定值作为目标重心角度设定值。

在一些实施例中，所述通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，包括：如果该当前倾斜角度大于当前重心角度设定值，则将前重心角度设定值加上所述梯度值，得到更新后的当前重心角度设定值；如果该当前倾斜角度小于当前重心角度设定值，则将前重心角度设定值减去所述梯度值，得到更新后的当前重心角度设定值。

在一些实施例中，所述将所述指令转换成目标速度设定值，包括：如果所述指令为原地平衡指令，则设置目标速度设定值为零；如果所述指令为加速指令，则将所述当前速度乘以预设加系数，得到目标速度设定值；如果所述指令为减速指令，则将所述当前速度乘以预设减系数，得到目标速度设定值；如果所述指令为转向指令，则根据所述转向指令指示的转动方向，将所述当前速度转换成具有一定差值的右轮的目标速度设定值、左轮的目标速度设定值。

在一些实施例中，角度PID与速度PID叠加的双PID控制，包括：获取所述两轮自平衡机器人当前倾斜角度及当前速度；计算当前倾斜角度与当前重心角度设定值的角度差值，对该角度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到角度控制参数；计算当前速度与所述速度设定值的速度差值，对该速度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到速度控制参数；将所述角度控制参数与所述速度控制参数进行融合，用于控制所述电机转动。

在一些实施例中，所述方法运行于电机中，且所述方法支持双电机主从模式和单电机模式，其中，所述双电机主从模式为采用两个电机驱动所述两轮自平衡机器人中的两个轮，所述单电机模式为采用一个电机驱动所述两轮自平衡机器人的两个轮。

在一些实施例中，所述方法还包括选定所述电机的工作模式，具体的：

检测如果所述电机被开启，则选定默认模式；执行主模式确认步骤：随机延时一段时间，发送竞选主模式信息，并开启监听，用于监听第二电机是否发送其已经接收到所述竞选主模式信息的回应信息，响应于接收到所述回应信息，则切换为主模式，且关闭对所述第二电机的竞选主模式信息的回应信息；响应于没有接收到所述第二电机发送的竞选主模式信息的回应信息，则重新执行所述主模式确认步骤，如果累计未接收到回应信息的次数大于预设数值，则发送非主模式信息，并监听所述第二电机是否发送其已经接收到所述非主模式信息的回应信息，响应于接收到所述非主模式的回应信息，则切换为从模式，否则，切换为单电机模式。

在一些实施例中，在双电机主从模式下，在自适应调整重心角度设定值阶段，角度PID与速度PID叠加的双PID控制，包括如下步骤：主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度，根据内置的编码器，计算所述主模式电机的当前速度，之后，向从模式电机发送获取从模式电机的当前速度的指令；响应于接收到所述从模式电机返回的从模式电机的当前速度，根据所述主模式电机的当前速度、从模式电机的当前速度，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、当前速度、设定设定值零作为输入，用于执行角度PID与速度PID叠加的双PID控制，得到控制参数，并将所述控制参数发送给所述从模式电机，最后，根据所述控制参数，控制主模式电机转动；所述从模式电机接收所述主模式电机发送的获取从模式电机的当前速度的指令，根据内置的编码器，计算所述从模式电机的当前速度，并将所述从模式电机的当前速度发送给所述主模式电机，之后，接收所述主模式电机发送的控制参数，用于控制所述从模式电机转动。

在一些实施例中，在双电机主从模式下，在控制步骤阶段，角度PID与速度PID叠加的双PID控制，包括如下步骤：所述主模式电机接收操控终端发送的指令，将所述指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值，之后，根据内置的六轴姿态传感器，计算当前倾斜角度，之后，向所述从模式电机发送所述从模式电机的目标速度设定值、当前倾斜角度，然后，根据内的编码器，计算主模式电机的当前速度，最后，将所述目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；所述从模式电机接收所述主模式电机发送的所述从模式电机的目标速度设定值、当前倾斜角度，之后，根据内置的编码器，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、所述从模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；所述主模式电机与所述从模式电机约定同步执行双PID控制的约定时间，在所述约定时间，所述主模式电机、所述从模式电机同时分别执行角度PID与速度PID叠加的双PID控制。

本申请还提供了一种两轮自平衡机器人的控制装置，所述装置包括：目标重心角度设定值模块，用于采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，其中，所述速度PID的速度设定值为零；

控制模块，用于基于所述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将所述指令转换成目标速度设定值，获取两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动。

在一些实施例中，所述目标重心角度设定值模块，包括：纠正倾斜角度子模块，用于将默认重心角度作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动，不断调整两轮自平衡机器人的倾斜角度，直到当前倾斜角度可等于所述默认重心角度；修正重心角度设定值子模块，用于以预设时间间隔循环执行如下梯度步骤：获取所述两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，将更新后的当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制所述电机转动，来调整所述两轮自平衡机器人的倾斜角度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值不大于所述阈值，则将当前重心角度设定值输出为待选重心角度设定值；验证重心角度设定值子模块，用于将所述待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，采用所述角度PID与速度PID叠加的双PID控制所述电机转动，不断调整所述两轮自平衡机器人的倾斜角度，如果在预设时间段内，所述两轮自平衡机器人的倾斜角度与所述待选重心角度设定值的差值的绝对值一直小于误差阈值，则将所述待选重心角度设定值作为目标重心角度设定值。

在一些实施例中，所述修正重心角度设定值子模块包括：增大重心角设定值子模块，用于如果该当前倾斜角度大于当前重心角度设定值，则将前重心角度设定值加上所述梯度值，得到更新后的当前重心角度设定值；减小重心角设定值子模块，用于如果该当前倾斜角度小于当前重心角度设定值，则将前重心角度设定值减去所述梯度值，得到更新后的当前重心角度设定值。

在一些实施例中，所述控制模块包括：原地平衡子模块，用于如果所述指令为原地平衡指令，则设置目标速度设定值为零；加速子模块，用于如果所述指令为加速指令，则将所述当前速度乘以预设加系数，得到目标速度设定值；减速子模块，用于如果所述指令为减速指令，则将所述当前速度乘以预设减系数，得到目标速度设定值；转向子模块，用于如果所述指令为转向指令，则根据所述转向指令指示的转动方向，将所述当前速度转换成具有一定差值的右轮的目标速度设定值、左轮的目标速度设定值。

在一些实施例中，双PID控制子模块包括：获取子模块，用于获取所述两轮自平衡机器人当前倾斜角度及当前速度；角度PID子模块，用于计算当前倾斜角度与当前重心角度设定值的角度差值，对该角度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到角度控制参数；速度PID子模块，用于计算当前速度与所述速度设定值的速度差值，对该速度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到速度控制参数；电机控制子模块，用于将所述角度控制参数与所述速度控制参数进行融合，用于控制所述电机转动。

在一些实施例中，所述装置内置于电机中，且所述装置支持双电机主从模式和单电机模式，其中，所述双电机主从模式为采用两个电机驱动所述两轮自平衡机器人中的两个轮，所述单电机模式为采用一个电机驱动所述两轮自平衡机器人的两个轮。

在一些实施例中，所述装置还包括选定工作模式模块，具体的：检测子模块，用于检测如果所述电机被开启，则选定默认模式；主模式确认子模块，用于执行主模式确认步骤：随机延时一段时间，发送竞选主模式信息，并开启监听，用于监听第二电机是否发送其已经接收到所述竞选主模式信息的回应信息，响应于接收到所述回应信息，则切换为主模式，且关闭对所述第二电机的竞选主模式信息的回应信息；模式切换子模块，用于响应于没有接收到所述第二电机发送的竞选主模式信息的回应信息，则重新执行所述主模式确认步骤，如果累计未接收到回应信息的次数大于预设数值，则发送非主模式信息，并监听所述第二电机是否发送其已经接收到所述非主模式信息的回应信息，响应于接收到所述非主模式的回应信息，则切换为从模式，否则，切换为单电机模式。

在一些实施例中，在双电机主从模式下，在自适应调整重心角度设定值阶段，双PID控制子模块分为：主模式电机控制子模块，用于主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度，根据内置的编码器，计算所述主模式电机的当前速度，之后，向从模式电机发送获取从模式电机的当前速度的指令；响应于接收到所述从模式电机返回的从模式电机的当前速度，根据所述主模式电机的当前速度、从模式电机的当前速度，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、当前速度、设定设定值零作为输入，用于执行角度PID与速度PID叠加的双PID控制，得到控制参数，并将所述控制参数发送给所述从模式电机，最后，根据所述控制参数，控制主模式电机转动；从模式电机控制子模块，用于所述从模式电机接收所述主模式电机发送的获取从模式电机的当前速度的指令，根据内置的编码器，计算所述从模式电机的当前速度，并将所述从模式电机的当前速度发送给所述主模式电机，之后，接收所述主模式电机发送的控制参数，用于控制所述从模式电机转动。

在一些实施例中，在双电机主从模式下，在控制步骤阶段，双PID控制子模块分为：主模式电机双PID控制子模块，用于所述主模式电机接收操控终端发送的指令，将所述指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值，之后，根据内置的六轴姿态传感器，计算当前倾斜角度，之后，向所述从模式电机发送所述从模式电机的目标速度设定值、当前倾斜角度，然后，根据内的编码器，计算主模式电机的当前速度，最后，将所述目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；从模式电机双PID控制子模块，用于所述从模式电机接收所述主模式电机发送的所述从模式电机的目标速度设定值、当前倾斜角度，之后，根据内置的编码器，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、所述从模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；同步子模块，用于所述主模式电机与所述从模式电机约定同步执行双PID控制的约定时间，在所述约定时间，所述主模式电机、所述从模式电机同时分别执行角度PID与速度PID叠加的双PID控制。

本申请还提供了一种两轮自平衡机器人包括权利要求10至18中任一权利要求所述的装置。

本申请公开的两轮自平衡机器人控制方法，先采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值。再接收操控终端发送的指令，将上述指令转换成目标速度设定值，将上述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动。在保持两轮自平衡机器人平衡的同时，精确的控制两轮自平衡机器人行驶。

附图说明

通过阅读参照以下附图所作的对非限制性实施例所作的详细描述，本申请的其它特征、目的和优点将会变得更明显：

图1是本申请一实施例中单电机两轮自平衡机器人的结构示意图；

图2是本申请另一实施例中双电机两轮自平衡机器人的结构示意图；

图3是本申请一实施例中电机的结构示意图；

图4是本申请一实施例中两轮自平衡机器人的控制方法的流程图；

图5是本申请一实施例中自适应调整两轮自平衡机器人的重心角度的流程图；

图6是本申请一实施例中角度PID与速度PID叠加的双PID控制流程图；

图7是本申请一实施例中确定电机工作模式的流程图；

图8是本申请另一实施例中双电机确定工作模式的流程图；

图9是本申请一实施例中双电机主从模式下，在自适应调整重心角度设定值阶段执行双PID控制的流程图；

图10是本申请一实施例中双电机主从模式下，在控制步骤阶段执行双PID控制的流程图；

图11是本申请两轮自平衡机器人的控制装置的一个实施例的框图；

图12是本申请一实施例中目标重心角度设定值模块的框图；

图13是本申请一实施例中控制模块的部分框图；

图14是本申请一实施例中双PID控制子模块的框图；

图15是本申请一实施例中选定工作模式模块的框图。

具体实施方式

下面结合附图和实施例对本申请作进一步的详细说明。可以理解的是，此处所描述的具体实施例仅仅用于解释相关发明，而非对该发明的限定。另外还需要说明的是，为了便于描述，附图中仅示出了与有关发明相关的部分。

需要说明的是，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性。在不冲突的情况下，本申请中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

本申请的两轮自平衡机器人的两个轮子是横向排列的，属于倒立摆结构，不易保持平衡。易出现前倾或后倾，甚至向前或向后跌倒的状况。根据其使用的电机的个数的不同，将两轮自平衡机器人划分成两种，即单电机两轮自平衡机器人、双电机两轮自平衡机器人。

参考图1，该图是本申请一实施例中单电机两轮自平衡机器人的结构示意图。该自平衡机器人的两个轮子是由同一个电机驱动。

继续参考图2，该图是本申请一实施例中双电机两轮自平衡机器人的结构示意图，两轮自平衡机器人的两个轮子由两个不同的电机驱动。

继续参考图3，该图是本申请一实施例中电机的结构示意图。通过上述图1、图2的示意图可知，图1中的单电机两轮自平衡机器人与图2中的双电机两轮自平衡机器人都是由一系列积木件及电子件拼搭而成。他们都使用了图3中的电机这一电子件。

继续参考图4，该图是本申请一实施例中两轮自平衡机器人的控制方法的流程图。包括如下步骤：

在本实施例中，两轮自平衡机器人具有一个名叫主控器的电子件。两轮自平衡机器人的控制方法运行于主控器中。电子件之间通过有线的方式或无线的方式进行通信。

步骤401，采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值。

倾斜角度指两轮机器人的车身相对于竖直线的偏转角度。当将两轮机器人放置于基准平面上，除重力、轮子与地面的摩擦力外，不受其他外力的影响下，可平衡站立时的倾斜角度为重心角度。此时，两轮自平衡机器人水平方向的合力为零，不会引起两轮机器人前倾或后倾。

理想状态下，两轮自平衡机器人的倾斜角度为0，重心角度也为0。即两轮自平衡机器人直直平衡站立，车身垂直于基准平面。此时，重心点的正投影落在两车轮的连线上。但实际上，组装两轮自平衡机器人时会出现偏差，或两轮自平衡机器人的两个手臂从垂直放置变成前伸状态等姿态改变，或机器人负重，导致重心点前移或后移。此时的重心角度不再等于零。即时两轮自平衡机器人被直直的放置于地面上，在不受外力作用下，两轮自平衡机器人会向前倾斜或向后倾斜。但因两轮自平衡机器人的重心点是不可见的，测量难度大，而两轮自平衡机器人的倾斜角度可通过姿态传感器方便的测量出来。例如，采用六轴姿态传感器测量两轮自平衡机器人的前后倾斜角度。所以本申请另辟新径，寻找重心角度。基于重心角度，控制两轮自平衡机器人的平衡。

参考图5，该图是本申请一实施例中自适应调整两轮自平衡机器人的重心角度的流程图。

步骤401-1，将默认重心角度作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动，不断调整两轮自平衡机器人的倾斜角度，直到当前倾斜角度可等于默认重心角度。

在本实施中，因不知道两轮自平衡机器人重心点的位置，所以需要设置一个默认重心角度，可直接设置一个默认重心角度，例如，0度、0.5度、1度。也可选择上次关机前的目标重心角度设定值作为此次的默认重心角度。

当两轮自平衡机器人被用户放置在地面或其他平面上时，放置时的倾斜角度会多种多样。例如，倾斜角度为零度，两轮自平衡机器人被直直放置；或倾斜角度为10度，两轮自平衡机器人有些向前倾斜；或倾斜角度为-12度，两轮自平衡机器人有些向后倾斜。此时，可执行角度PID控制，将倾斜角度拉至默认重心角度。通过角度PID控制两轮自平衡机器人的原理是：让两轮自平衡机器人加速或减速，来调整两轮自平衡机器人的倾斜角度。例如，两轮自平衡机器人的重心角度为5度。而两轮机器人被放置于地面上时的倾斜角度是10度，此时通过角度PID控制电机转动，让两轮自平衡机器人的两个轮子向前加速移动，而两轮自平衡机器人的上半身因为惯性原因，不会同时与两个轮子一起加速移动，从而减少了两轮自平衡机器人的倾斜角度。

通过角度PID控制，可将两轮自平衡机器人的倾斜角度调整到当前重心角度设定值，即默认重心角度。将倾斜角度调整到默认重心角度后，即时一直运行角度PID控制，两轮自平衡机器人的倾斜角度也不大可能一直停留在默认重心角度处，除非默认重心角度等于实际的重心角度。当实际的重心角度不等于默认重心角度时，重力产生水平方向的分力，该分力不能完全被水平方向的摩擦力抵消，即两轮自平衡机器人在水平方向的合力不等于零，会导致两轮自平衡机器人倒向重心点所在的方向，即倒向实际的重心角度。但一直执行角度PID控制，会不断调整两轮自平衡机器人向默认重心角度方向倾斜。所以即时采用了角度PID控制，仍会发现两轮自平衡机器人在默认重心角度与实际的重心角度间来回不断晃动。

在单电机两轮自平衡机器人中，即采用一个电机同时驱动两轮自平衡机器人的两个车轮，理论上仅采用角度PID控制，即可精确的将两轮自平衡机器人的倾斜角度调整到默认重心角度。但仅采用角度PID控制，有时导致为了调整倾斜角度，而控制两轮自平衡机器人轮子的电机不断加速。如此，两轮自平衡机器人的速度比较高，行驶快，易受障碍物或高低不平路面的影响，采用速度PID控制，将速度设定值设为零，将电机的速度降为零，将两轮自平衡机器人的活动范围控制在一个较小的范围内。

在双电机两轮自平衡机器人中，即分别采用一个电机驱动两轮自平衡机器人的一个车轮。仅采用角度PID控制，效果并不理想。因不同的电机存在个体差异，就算同一型号的两个电机也存在个体差异。例如，给定两个电机相同的功率或相同的脉冲宽度调制PWM信号，但两个电机的转速并不相同。如在角度PID控制时，给定两个电机相同的功率进行加速，但左轮的电机是每分钟230转，而右轮的电机是每分钟250转，因两个电机的转速不同，导致两轮自平衡机器人出现转向，这不利于调整两轮自平衡机器人的倾斜角度。所以为克服电机的个体差异，除采用角度PID控制外，还采用速度设定值为零的速度PID控制。同时采用叠加的角度PID控制、速度PID控制一起控制两个电机的转动。角度PID控制，会使电机加速。为降低电机个体差异带来的不良影响，通过速度设定值为零的速度PID控制，将两个电机的速度降为零。

继续参考图6，该图是本申请一实施例中角度PID与速度PID叠加的双PID控制流程图。每次执行角度PID控制，都需要获取两轮自平衡机器人当前俯仰角，即本申请中的当前倾斜角度。使用六轴姿态传感器采集角速度、加速度，再通过互补滤波、卡尔曼滤波等数据融合算法，计算电机501的当前倾斜角502，其中，电机501的当前倾斜角度502与两轮自平衡机器人的当前倾斜角度一致。每次执行速度PID控制，都需要获取两轮平衡机器人当前速度508，可根据上述六轴姿态传感器采集的数据，计算当前速度508。在本实施例中，为精确控制电机601的转动，通过内置的光电编码器、霍尔编码器、磁编码器中的一种编码器，计算电机501的当前速度508。

继续参考图6，图6中双PID控制的左半边是角度PID控制，设置当前重心角度设定值503，通过减法模块504，计算当前倾斜角度502与当前重心角度设定值503的角度差值，通过比例P505，对上述角度差值进行比例计算，通过微分D506，对上述角度差值进行微分运算，微分运算能反映角度差值的变化趋势，在角度差值变的太大之前，加快响应，减小调节的时间，有效减小超调。将比例P505、微分D506的输出值，通过加法模块507求和，得到角度控制参数。另外，图6中双PID控制的右半边是速度PID控制。设置一个速度设定值509，通过减法模块510，计算当前速度508与速度设定值509的速度差值，通过比例P511，对上述速度差值进行比例计算，通过积分I512，对上述速度差值进行积分运算，积分运算能消除稳态误差。将比例P511、积分I512的输出值，通过加法模块513求和，得到速度控制参数；将上述角度控制参数与速度控制参数，通过加法模块514求和，用于控制电机501转动。

在本实施例的其他可选的实现方式中，分别对角度差值、速度差值进行比例、微分、积分控制。

在本实施例中，将默认重心角度作为当前重心角度设定值，零作为速度设定值；以预设时间间隔循环执行如下双PID控制步骤：获取两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度不等于默认重心角度，将当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制电机转动，来调整两轮自平衡机器人的倾斜角度，如果该当前倾斜角度等于默认重心角度，则退出上述双PID控制步骤。

在本申请中，不要求当前倾斜角度一直等于默认重心角度，而是在循环执行双PID控制步骤时，只要某次采集的当前倾斜角度等于默认重心角度，则退出循环。

步骤401-2，以预设时间间隔循环执行如下梯度步骤：获取两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，将更新后的当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制电机转动，来调整两轮自平衡机器人的倾斜角度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值不大于所述阈值，则将当前重心角度设定值输出为待选重心角度设定值。

在本实施中，如果当前重心角度设定值不等于实际的重心角度，即时一直执行角度PID控制、速度PID叠加的双PID控制，两轮自平衡机器人的倾斜角度也不会稳定在当前重心角度设定值，而是在当前重心角度设定值与实际的重心角度之间震荡。所以不断获取两轮自平衡机器人当前倾斜角度，如果该当前倾斜角度与当前重心角度的差值的绝对值大于阈值，则说明角度震荡的幅度过大。说明当前重心角度设定值与实际的重心角度的差值也比较大，所以调整当前重心角度设定值的大小，让其靠近实际的重心角度。因实际的重心角度不可知，但当前倾斜角度的大小与方向能在一定程度上表征实际的重心角度的大小与方向。通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度。具体的：如果该当前倾斜角度大于当前重心角度设定值，则将前重心角度设定值加上所述梯度值，得到更新后的当前重心角度设定值；如果该当前倾斜角度小于当前重心角度设定值，则将前重心角度设定值减去所述梯度值，得到更新后的当前重心角度设定值。其中，梯度值设置的比较小，为0.1度、0.15度、0.2度中的一个。让当前重心角度设定值缓慢平稳的变大或变小。

在本实施例中，按照预设的时间间隔，例如40毫秒、50毫秒、60毫秒中的一个时间间隔循环执行梯度步骤，直到当前倾斜角度与当前重心角度的差值的绝对值不大于阈值，才退出循环。其中，阈值设定为0.5度、1度中的一个。每次执行梯度步骤，都获取两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，之后，采用图6角度PID与速度PID叠加的双PID控制电机转动。此时，角度PID控制采用上述更新后的当前重心角度设定值，速度PID控制采用速度设定值零。通过上述叠加的角度PID与速度PID控制电机的转动，来调整两轮自平衡机器人的倾斜角度。

当两轮自平衡机器人的当前倾斜角度与当前重心角度的差值的绝对值不大于阈值，则说明有可能将当前重心角度设置值已经调整到非常靠近实际的重心角度了。但也有可能当前重心角度设置值与实际的重心角度差值还是很大，只是因执行角度PID与速度PID叠加的双PID控制，两轮自平衡机器人在实际的重心角度与当前重心角度设定值之间震荡。而此次获取的当前倾斜角度正好距离当前重心角度设置值比较近，还有距离当前重心角度设置值比较远的当前倾斜角度，只是此次没有获取到。为验证是哪种情况，执行步骤401-3。

特别说明，在循环执行梯度步骤时，使用的角度PID与速度PID叠加的双PID控制，速度PID的速度设定值始终为零。克服了双电机两轮自平衡机器人中两个电机在相同输入控制下，输出的转速不同，影响调整倾斜角度的效果。另外，速度设定值始终为零，可保证在较小的空间范围内实现重心角度调整，避免较大范围中的障碍物、不平衡地面带来的额外不良影响。

步骤401-3，将待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制电机转动，不断调整两轮自平衡机器人的倾斜角度，如果在预设时间段内，两轮自平衡机器人的倾斜角度与待选重心角度设定值的差值的绝对值一直小于误差阈值，则将待选重心角度设定值作为目标重心角度。

在本实施例中，为验证待选重心角度设定值是否是目标重心角度，将待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，一直采用角度PID与速度PID叠加的双PID控制电机转动，不断调整两轮自平衡机器人的倾斜角度。此外，高频率的获取两轮自平衡机器人的当前倾斜角度，如果在预设时间段内，每次获取的当前倾斜角度与待选重心角度设定值的差值的绝对值都小于误差阈值，则等同于两轮自平衡机器人的倾斜角度与待选重心角度设定值的差值的绝对值一直小于误差阈值。则将待选重心角度设定值作为目标重心角度。其中，上述误差阈值为如下一个：0.5度、1度。上述预设时间段为如下一个：15秒、20秒。在该时间段内会获取当前倾斜角度大约300～400次。如果每次当前倾斜角度与待选重心角度设定值的差值的绝对值都小于误差阈值，则等同于两轮自平衡机器人的倾斜角度与待选重心角度设定值的差值的绝对值一直小于误差阈值。用户视觉直观看到的是：在一段时间内，两轮自平衡机器人始终平衡站立，且始终在较小的角度范围内震荡。则说明待选重心角度设定值与实际的重心角度已经很接近了，或待选重心角度设定值等于实际的重心角度。所以将待选重心角度设定值作为目标重心角度，并输出该目标重心角度。

在本实施例的其他可选的实现方式中，为验证待选重心角度设定值是否是目标重心角度，将待选重心角度设定值作为当前重心角度设定值，零作为速度设定值。开启计时器计时，循环执行角度PID与速度PID叠加的双PID控制电机转动。在每次执行双PID控制的过程中，进行判断，如果获取的当前倾斜角度与待选重心角度设定值的差值的绝对值小于误差阈值，则判断计时器的值是否大于预设时间段，如果大于，则待选重心角度设定值作为目标重心角度。如果获取的当前倾斜角度与待选重心角度设定值的差值的绝对值不小于误差阈值，则关闭计时器，并输出待选重心角度设定值不是目标重心角度。

另外一种方案，是通过判断两轮自平衡机器人是否能在一段时间内原地平衡，来确定当前重心角度设定值是否已经等于实际的重心角度。但这只能在理想情况下才能实现，实际情况是，即时当前重心角度设定值等于实际的重心角度，在角度PID与速度PID叠加的双PID控制下，两轮自平衡机器人也会不断前后小幅移动，通过姿态传感器或编码器采集的数据，计算出来的位移并不为零。一方面，因任何一种滤波也不能完全滤除噪声；另一方面，姿态传感器或编码器本身会积累误差。

在本实施例的其他可选的实现方式中，在步骤401-1与步骤401-2之间增加一步，在步骤401-1之后，如果一直执行角度PID与速度PID叠加的双PID控制，两轮自平衡机器人则在默认重心角度与实际的重心角度之间晃动。为更快速的找准目标重心角度设定值，在上述实施例步骤401-1与步骤401-2之间增加如下步骤：以默认重心角度作为当前重心角度设定值，零作为速度设定值，在预设一段时间内，循环执行角度PID与速度PID叠加的双PID控制，将每次采集的当前倾斜角度，组成一个当前倾斜角度序列。对该序列中的数值进行大小排序。因当前倾斜角度是在默认重心角度与实际的重心角度之间晃动，所以上述序列的一端的值等于默认重心角度，另一端的值接近实际的重心角度。所以需在序列远离默认重心角度的一端选取重心角度设定值。为剔除掉噪声干扰，剔除掉序列两端3％～5％的当前倾斜角度，然后在远离默认重心角度的一端，选择端点的当前倾斜角度作为当前重心角度设定值，之后，采用上述步骤401-2进行微调整当前重心角度设定值，最后，通过上述步骤401-3进行验证。其中，上述预设一段时间的值为10秒或15秒，要求在上述预设一段时间内，两轮自平衡机器人在默认重心角度与实际的重心角度之间晃动多次，而采集的当前倾斜角度在200～300个。

步骤402，基于上述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将上述指令转换成目标速度设定值，获取两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动上述两轮自平衡机器人轮子的电机转动。

在本实施例中，两轮自平衡机器人与操控终端通过无线的方式进行通信，两轮自平衡机器人接收操控终端发送的指令。其中，操控终端包括但不限于：智能手机、平板、笔记本电脑、台式电脑等。

因两轮机器人属于倒立摆结构，不稳定，所以在控制两轮自平衡机器人时需要一直运行角度PID控制。另外，通过速度PID控制实现原地平衡、加速、减速、匀速、转向等操控，所以需继续执行如图6所示的角度PID与速度PID叠加的双PID控制。

在本实施例中，如果从终端接收的指令为原地平衡指令，则设置目标速度设定值为零。如果上述指令为匀速指令，如果当前速度作为目标速度设定值。如果上述指令为加速指令，如果当前速度为零，则给定一个目标速度设定值，例如：每分钟200转，如果当前速度不为零，则将当前速度乘以预设加系数，得到目标速度设定值，其中，预设加系数为如下一个：1.5、2.0、2.5、3。如果指令为减速指令，则将当前速度乘以预设减系数，得到目标速度设定值，其中，预设减系数为如下一个：0.8、0.6、0.4。如果指令为转向指令，如果是向右转的指令，则将当前速度作为左轮的目标速度设定值，用当前速度减去设定值得到右轮的目标速度设定值。因左轮的目标速度设定值与右轮的目标速度设定值之间具有差值，所以经循环多次角度PID与速度PID叠加的双PID控制后，两轮自平衡机器人会缓慢右转。同理，如果是向左转的指令，则将当前速度作为右轮的目标速度设定值，用当前速度减去设定值得到左轮的目标速度设定值。其中，上述设定值等于当前速度乘以设定值系数。其中，设定值系数为如下一个：0.2、0.3、0.4。在其他实施例中，上述设定值可为一个固定数值。

在本实施例的其他可选的实现方式中，对加速、减速、转向可设置不同的档位。不同的档位对应不同的系数，例如，慢加速、中加速、快加速、超快加速分别对应如下预设加系数：1.5、2.0、2.5、3。

在本实施例中，通过角度PID与速度为零的速度PID叠加的双PID控制，在较小的地面等轨迹范围内，快速而准确的找到目标重心角度设定值，之后，接收来至终端的指令，将指令转换成目标速度设定值，最后，基于上述目标重心角度设定值、目标速度设定值，通过角度PID与速度PID叠加的双PID，控制两轮自平衡机器人保持平衡的同时，实现了操控终端精准的控制两轮自平衡机器人行驶。

通过图1、图2的示意图可知，图1中的单电机两轮自平衡机器人与图2中的双电机两轮自平衡机器人都是由一系列积木件及电子件拼搭而成。他们都使用了图3中的电机这一电子件。在一些实施例中，本申请两轮自平衡机器人的控制方法运行于电机中。因是采用积木拼搭，用户即可以采用上述电机拼搭单电机两轮自平衡机器人，也可采用上述两个电机拼搭双电机两轮自平衡机器人。所以电机必须支持两种模式，即单电机模式，双电机主从模式。所以通电后，在执行两轮自平衡机器人的控制方法之前，电机需要进行模式判断，选定电机的工作模式。不管有几个电机，电机开机后，都执行相同的确定电机工作模式的步骤。上述电机内部至少集成了如下电子元件：微控制单元MCU、六轴传感器、电机驱动电路、电机、用于测速的编码器、蓝牙芯片。本申请的两轮自平衡机器人的控制方法运行在微控制单元MCU上。

参考图7，该图是本申请一实施例中确定电机工作模式的流程图。包括如下步骤：

步骤601，检测到本电机被开启，则选定默认模式。

在本实施例中，微控制单元MCU检测到电压或电流后，则确定本电机被开启，选定默认模式。其中，默认模式即不是单电机模式，也不是主电机模式、从电机模式。

步骤602，随机延时一段时间，发送竞选主模式信息。

在本实施例中，选定默认模式后，随机延时一段时间，向其他电机发送竞选主模式信息，其中，竞选主模式信息携带了本电机的唯一标识等信息。其他电机在收到竞选主模式信息后，返回回应信息，该回应信息表示赞成对方电机切换为主模式。

步骤603，监听回应信息n秒。

在本实施例中，发送竞选主模式信息后，则开启监听，监听其他电机返回的竞选主模式信息对应的回应信息。最多监听n秒。

步骤604，是否接收到其他电机发送的回应信息。

在本实施例中，n秒过后，判断是否接收到其他电机发送的回应信息。如果接收到回应信息，则执行步骤605，如果没有接收到回应信息，则执行步骤606。

步骤605，切换为主模式，且关闭对其他电机竞选主模式信息的回应。

在本实施例中，响应于接收到其他电机发送的回应信息，则将默认模式切换成主模式，且关闭对其他电机竞选主模式信息的回应。之后，即时接收到其他电机发送的竞选主模式信息，也不再向其他电机发送回应信息。如果发送了回应信息，因为其他电机执行相同的步骤，其他电机一旦也接收到回应信息，则同样将默认模式切换成主模式。而两个电机同为主模式是不对的。在双电机两轮自平衡机器人中，一个电机为主模式电机，另外一个电机为从模式电机。

步骤606，未回应次数+1。

在本实施例中，如果在步骤604中，在n秒内没有接收到其他电机发送的回应信息，则将未回应次数的参数值加1。

步骤607，未回应次数是否大于阈值。

在本实施例中，判断未回应次数的参数值是否大于阈值，如果大于阈值，则执行步骤608，如果不大于阈值，则跳转执行步骤602，再次随机延时一段时间发送竞选主模式信息，以防上次发送的竞选主模式信息丢失或回应信息丢失。其中，阈值为如下一个：4、5、6。

步骤608，发送非主模式信息。

在本实施例中，发送了上述阈值次数的竞选主模式信息，但一直没有接收到回应信息，则一方面默认对方电机已经关闭了对竞选主模式信息的回应，另一方面默认可能不存在其他电机。为进一步确定是哪种情况，对外发送非主模式信息。

步骤609，监听非主模式信息的回应信息n秒。

在本实施例中，发送非主模式信息后，如果存在其他切换到主模式的电机接收到该非主模式信息，则返回非主模式信息对应的回应信息，该回应信息表示同意对方电机切换为从模式。

为确定是否有主模式电机发送回应信息，则监听回应信息，最长监听n秒。如果接收到回应信息，则不在继续监听；如果没有接收到回应信息，则n秒后，就结束监听。

步骤610，是否接收到非主模式的回应信息。

在本实施例中，n秒后，判定是否接收到非主模式信息的回应信息，如果接收到，则执行步骤611，否则执行步骤612。

步骤611，切换为从模式。

如果接收到非主模式信息对应的回应信息，则说明已经有电机切换成主模式了，且该主模式电机同意本电机切换为从模式，则将本电机从默认模式切换为从模式。

步骤613，切换为单电机模式。

如果没有接收到非主模式信息对应的回应信息，则说明不存在其他主模式电机，则将本电机从默认模式切换为单电机模式。

通过上述的各步骤可知，电机开机后是默认模式，通过发送竞选主模式信息，如果接收到其他电机返回回应信息，则切换为主模式，如果多次尝试，都没有接收到回应信息，则发送非主模式信息，已经切换到主模式的电机接收到该非主模式信息，则返回回应信息，接收到该回应信息后，可切换为从模式。如果没有电机给出回应信息，则默认只有自己一个电机，则切换为单电机模式。

为直观说明两个电机是如何确定工作模式的，参考图8，该图是本申请另一实施例中双电机确定工作模式的流程图。电机A与电机B通过有线或无线的方式进行通信，例如，通过通用串行数据总线UART(Universal Asynchronous Receiver/Transmitter，通用异步收发器)、蓝牙。

步骤701，如果检测到本电机被开启，则选定默认模式。

在本实施例中，不论电机A，还是电机B检测到本电机被通电开启后，则选定默认模式。

步骤702，随机延时一段时间，发送竞选主模式信息。

在本实施例中，不论电机A，还是电机B选定默认模式后，都随机延时一段时间，通过有线或无线的方式，对外发送竞选主模式信息。其中，随机延时一段时间为随机生成的0-4秒之间的数值，例如，2秒。

步骤703，开始监听。

在本实施例中，不论电机A，还是电机B在发送完竞选主模式信息后，都开启监听，等待接收到其他电机发送的竞选主模式信息的回应信息。

在本实施例的其他可选的实现方式中，可调换步骤702与步骤703的顺序。

步骤704，电机B接收到电机A发送的竞选主模式信息。

在本实施例中，因电机A早于电机B发送竞选主模式信息，所以电机B先于电机A通过监听，接收到对方发送的竞选主模式信息。

步骤705，发送回应信息。

电机一旦接收到对方电机发送的竞选主模式信息后，需向对方电机发送表示赞成的回应信息。在本实施例中，电机B接收到电机A发送的竞选主模式信息后，就向电机A发送回应信息。

步骤706，是否接收到其他电机发送的回应信息。

在本实施例中，不论电机A，还是电机B发送竞选主模式信息后，监听等待一段时间后，都判断是否接收到其他电机发送的回应信息。电机A已经接收到电机B返回的回应信息，则执行步骤707。

步骤707，电机A不向电机B发送回应信息，且切换为主模式。

电机一旦接收到竞选主模式信息的回应信息，则可切换为主模式，即使是双电机两轮自平衡机器人也不能拥有两个同为主模式的电机，在本实施例中，电机A已经收到电机B发送的回应信息，则电机A不再向电机B发送回应信息。同时，电机A从默认模式切换为主模式。

步骤708，电机B累计未收到回应信息的次数是否大于预设数值。

在本实施例中，电机A因已经接收电机B发送的回应信息，所以切换为主模式。为不让电机B也切换为主模式，电机A不管接收到电机B发送的几次竞选主模式信息，都不发送回应信息。电机B没有接收到回应信息，为避免发送的竞选主模式信息或回应信息出现丢失情况，跳转到步骤702，随机延时一段时间后，再发送竞选主模式信息。但电机B不会一直随机延时后，重发竞选主模式信息，而是设置一个预设数值，如果累计未收到回应信息的次数大于预设数值，则执行步骤709，否则，继续随机延时一段时间后，再发送竞选主模式信息。

步骤709，电机B向电机A发送非主模式信息。

步骤710，电机A向电机B返回非主模式的回应信息。

电机A接收到电机B发送的非主模式信息后，向电机B发送回应信息。

步骤711，电机B是否接收到非主模式信息的回应信息。

判断电机B是否接收到非主模式信息的回应信息。接收到，则执行步骤712，否则，执行步骤713。

步骤712，电机B切换为从模式。

如果电机B已经接收到电机A发送的非主模式的回应信息后，则切换为从模式。

步骤713，电机B切换为单电机模式。

如果电机B未接收到电机A发送的非主模式的回应信息后，而且之前也未收到电机A发送的竞选主模式的回应信息，则认为不存在其他电机，所以从默认模式切换为单电机模式。

通过上面的各步骤可知，电机A、电机B都发送竞选主模式信息，因电机A先收到对方回应信息的，则切换为主模式，电机B为判断要选从模式，还是单电机模式，向电机A发送非主模式信息，如果收到电机A的针对非主模式信息的回应信息，则说明存在一个主模式电机，则将自己切换为从模式，否则，因前后都未收到回应信息，切换为单电机模式。

在本实施例的其他可选的实现方式中，如果标明某电机只能用于单电机两轮自平衡机器人。标明某电机只能用于主模式电机，某电机只能用于从模式电机。这样电机在执行两轮自平衡机器人的控制方法之前就不需要进行模式判断。

对于单电机两轮自平衡机器人，电机经模式确认，切换为单电机模式后，该单电机执行两轮自平衡机器人的控制方法。如执行上述图4、图5中的技术方案。其中，单电机根据内置于电机中的六轴姿态传感器，计算电机的当前倾斜角度。根据内置于电机中的光电编码器，计算电机的当前转速。电机的当前倾斜角度、当前转速等同于两轮自平衡机器人的当前倾斜角度、当前转速。

对于双电机两轮自平衡机器人，两个电机经模式确认后，一个切换为主模式，一个切换为从模式。而因电机存在个体差异，在相同输入下，主模式电机、从模式电机的转速有可能不同。相对于单电机模式，执行双PID控制存在一些不同的技术特征。

参考图9，该图是本申请一实施例中双电机主从模式下，在自适应调整重心角度设定值阶段执行双PID控制的流程图。包括如下步骤：

步骤801，主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度。

因主模式电机、从模式电机的当前倾斜角度一样，所以根据内置于主模式电机中的六轴姿态传感器，计算当前倾斜角度。

步骤802，主模式电机根据内置的编码器，计算主模式电机的当前速度。

在本实施例中，内置的编码器为如下一种：光电编码器、霍尔编码器、磁编码器。

步骤803，主模式电机向从模式电机发送获取从模式电机的当前速度的指令。

步骤804，从模式电机根据内的编码器，计算从模式电机的当前速度。

步骤805，从模式电机向主模式电机发送从模式电机的当前速度。

步骤806，主模式电机根据主模式电机的当前速度、从模式电机的当前速度，计算当前速度。

在本实施例中，取主模式电机的当前速度、从模式电机的当前速度的均值，得到当前速度。

步骤807，主模式电机执行角度PID与速度PID叠加的双PID控制，得到控制参数。

在本实施例中，主模式电机计算当前倾斜角度与当前重心角度设定值的角度差值，对该角度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到角度控制参数；计算当前速度与速度设定值零的速度差值，对该速度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到速度控制参数；将所述角度控制参数与所述速度控制参数进行求和，用于控制电机转动。

步骤808，主模式电机向从模式电机发送控制参数。

在本实施例中，需要主模式电机、从模式电机同进同退，所以采用相同的控制参数，控制主模式电机、从模式电机转动。虽然主模式电机、从模式电机存在个体差异，但因速度PID控制的速度设定值为零，所以在零速度附近，主、从模式电机的差异可以忽略不计。

步骤809，主模式电机根据控制参数，控制主模式电机转动。

在本实施例中，主模式电机根据控制参数，生成PWM(Pulse width modulation，脉冲宽度调制)信号，用于控制主模式电机转动。

步骤810，从模式电机根据控制参数，控制从模式电机转动。

在本实施例中，从模式电机根据控制参数，生成PWM(Pulse width modulation，脉冲宽度调制)信号，用于控制从模式电机转动。

在本实施例中，主模式电机负责采用角度PID与速度PID叠加的双PID控制，自适应调整重心角度设定值，找到目标重心角度设定值，从模式电机只负责接收并执行主模式电机发送的获取当前速度的指令，或根据发送的控制参数，控制从模式电机转动。这样可以协同主模式电机、从模式电机，实施例简单。虽然主模式电机、从模式电机存在个体差异，但因速度设定值为零，所以在零速度附近，主、从模式电机的差异可以忽略不计。但在控制步骤，目标速度设定值不为零的情况，主、从模式电机的差异比较显著不能忽略不计。

参考图10，该图是本申请一实施例中双电机主从模式下，在控制步骤阶段执行双PID控制的流程图。包括如下步骤：

步骤901，主模式电机接收操控终端发送的指令，将上述指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值。

在本实施例中，双电机主从模式下，主模式电机、从模式电机分别驱动一个轮子。主模式电机通过内置的无线通信模块与操控终端进行通信，接收操控终端发送的指令。如果上述指令为原地平衡指令，则主模式电机的目标速度设定值、从模式电机的目标速度设定值都为零；如果上述指令为加速指令，则将主模式电机的当前速度乘以预设加系数，得到主模式电机的目标速度设定值。此时，从模式电机的目标速度设定值与主模式电机的目标速度设定值相等。如果上述指令为减速指令，则将主模式电机的当前速度乘以预设减系数，得到主模式电机的目标速度设定值。此时，从模式电机的目标速度设定值与主模式电机的目标速度设定值相等。如果上述指令为转向指令，确定主模式电机、从模式电机分别驱动左轮还是右轮。根据转动的方向，将主模式电机的当前速度作为一个目标速度设定值，被转向的那边的目标速度设定值等于主模式电机的当前速度乘以转向系数。根据主模式电机、从模式电机与左轮、右轮的关系，确定哪一个是主模式电机的目标速度设定值，确定哪一个是从模式电机的目标速度设定值。其中，转向系数为如下一个：0.8、0.6、0.4。

步骤902，主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度。

在本实施例中，两轮自平衡机器人作为一个整体，其当前倾斜角度只有一个。即主模式电机、从模式电机的当前倾斜角度相同。虽然主模式电机、从模式电机都内置六轴姿态传感器，但两六轴姿态传感器因被放置的位置、姿态不同，还有累计误差等因素，造成通过两六轴姿态传感器分别计算的当前倾斜角度不同。与实际情况不同。在本实施例中，只让主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度。

步骤903，主模式电机向从模式电机发送上述从模式电机的目标速度设定值、当前倾斜角度。

步骤904，主模式电机根据内的编码器，计算主模式电机的当前速度。

步骤905，主模式电机执行角度PID与速度PID叠加的双PID控制，控制主模式电机转动。

在本实施例中，在步骤908约定时间，将目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为输入，用于执行角度PID与速度PID叠加的双PID控制，控制主模式电机转动。

步骤906，从模式电机接收从模式电机的目标速度设定值、当前倾斜角度。

步骤907，从模式电机根据内的编码器，计算从模式电机的当前速度。

步骤908，同步。

在本实施例中，为精准的操控两轮自平衡机器人加速、减速、匀速、转向等，需要协同主模式电机、从模式电机同时开始执行角度PID与速度PID叠加的双PID控制。以防应该在直行加速、减速、匀速行驶的过程中，因主模式电机、从模式电机执行角度PID与速度PID叠加的双PID控制的时间早晚不同，造成主模式电机与从模式电机的转速不同，而出现两轮自平衡机器人转向。所以在本实施例中，主模式电机、从模式电机根据时钟约定执行双PID控制的约定时间，实现主模式电机与从模式电机同步开始执行双PID控制。

步骤909，从模式电机执行角度PID与速度PID叠加的双PID控制，控制从模式电机转动。

在本实施例中，在步骤908约定时间，将目标重心角度设定值、当前倾斜角度、从模式电机的目标速度设定值、当前速度作为输入，用于执行角度PID与速度PID叠加的双PID控制，控制从模式电机转动。

在本实施例中，为避免主模式电机、从模式电机计算出不一致的当前倾斜角度，只有主模式电机计算当前倾斜角度，再把该当前倾斜角度发送给从模式电机。为避免主模式电机、从模式电机计算出不一致的主模式电机的目标速度设定值、从模式电机的目标速度设定值，由主模式电机将指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值。为精准控制主模式电机、从模式电机的转动，根据约定时间，主模式电机、从模式电机同步执行双PID控制。实现精准控制两轮自平衡机器人的行驶。

在本实施例的其他可选的实现方式中，主模式电机、从模式电机还同步约定计算自己当前速度的时间。即同时执行步骤904与步骤907。

在其他实施例中，双电机主从模式下，在控制步骤阶段执行双PID控制时，主模式电机执行角度PID与速度PID叠加的双PID控制，从模式电机只执行速度PID。具体的：主模式电机接收操控终端发送的指令，将指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值，之后，根据内置的六轴姿态传感器，计算当前倾斜角度，之后，向从模式电机发送从模式电机的目标速度设定值，然后，根据内的编码器，计算主模式电机的当前速度，最后，将目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入。从模式电机接收主模式电机发送的上述从模式电机的目标速度设定值，之后，根据内置的编码器，计算当前速度，然后，将上述从模式电机的目标速度设定值、当前速度作为速度PID控制的输入。最后，主模式电机与从模式电机约定同步执行PID控制的约定时间，在上述约定时间，主模式电机执行角度PID与速度PID叠加的双PID控制，从模式电机执行速度PID控制。因主模式电机执行的是角度PID与速度PID叠加的双PID控制，而从模式电机仅执行速度PID控制，所以即时在相同的主模式电机的目标速度设定值、从模式电机的目标速度设定值时，主模式电机、从模式电机输出的转速也不相同。理论上存在角度PID控制这一差值。可通过调整从模式电机PID控制的比例、积分、分参数，来实现在主模式电机的目标速度设定值、从模式电机的目标速度设定值相同时，主模式电机、从模式电机的输出转速相同，不会出现转向。

在其他实施例中，不管是在采用角度PID与速度PID叠加的双PID控制，自适应调整重心角度设定值，找到目标重心角度设定值的阶段、还是在基于目标重心角度设定值，执行如下控制步骤的阶段，双电机主从模式下，主模式电机、从模式电机都执行角度PID与速度PID叠加的双PID控制。在每次执行双PID控制之前，主模式电机将从模式电机的目标速度设定值、当前重心角度设定值、当前倾斜角度发送给从模式电机，从模式根据内置的编码器计算当前速度。此外，在每次执行双PID控制之前，主模式电机、从模式电机约定同步执行双PID控制的约定时间。

与前述方法实施例相对应，本申请还公开了两轮自平衡机器人的控制装置。参考图11，该图是本申请两轮自平衡机器人的控制装置的一个实施例的框图。包括目标重心角度设定值模块1101、控制模块1102。其中，目标重心角度设定值模块1101用于采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，其中，所述速度PID的速度设定值为零。控制模块1102用于基于上述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将所述指令转换成目标速度设定值，获取两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动上述两轮自平衡机器人轮子的电机转动。

继续参考图12，该图是本申请一实施例中目标重心角度设定值模块的框图。其中，目标重心角度设定值模块包括纠正倾斜角度子模块1101-1、修正重心角度设定值子模块1101-2、验证重心角度设定值子模块1101-3。纠正倾斜角度子模块1101-1，用于将默认重心角度作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制电机转动，不断调整两轮自平衡机器人的倾斜角度，直到当前倾斜角度可等于所述默认重心角度。修正重心角度设定值子模块1101-2，用于以预设时间间隔循环执行如下梯度步骤：获取上述两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，将更新后的当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制电机转动，来调整上述两轮自平衡机器人的倾斜角度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值不大于上述阈值，则将当前重心角度设定值输出为待选重心角度设定值。验证重心角度设定值子模块1101-3，用于将上述待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，采用所述角度PID与速度PID叠加的双PID控制电机转动，不断调整上述两轮自平衡机器人的倾斜角度，如果在预设时间段内，上述两轮自平衡机器人的倾斜角度与所述待选重心角度设定值的差值的绝对值一直小于误差阈值，则将上述待选重心角度设定值作为目标重心角度设定值。其中，修正重心角度设定值子模块1101-2包括：增大重心角设定值子模块，用于如果该当前倾斜角度大于当前重心角度设定值，则将前重心角度设定值加上所述梯度值，得到更新后的当前重心角度设定值；减小重心角设定值子模块，用于如果该当前倾斜角度小于当前重心角度设定值，则将前重心角度设定值减去所述梯度值，得到更新后的当前重心角度设定值。

继续参考图13，该图是本申请一实施例中控制模块的部分框图。其中，控制模块中将指令转换成目标速度设定值，包括原地平衡子模块1102-1、加速子模块1102-2、减速子模块1102-3、转向子模块1102-4。其中，原地平衡子模块1102-1，用于如果上述指令为原地平衡指令，则设置目标速度设定值为零。加速子模块1102-2，用于如果上述指令为加速指令，则将当前速度乘以预设加系数，得到目标速度设定值。减速子模块1102-3，用于如果上述指令为减速指令，则将当前速度乘以预设减系数，得到目标速度设定值。转向子模块1102-4，用于如果上述指令为转向指令，则根据上述转向指令指示的转动方向，将当前速度转换成具有一定差值的右轮的目标速度设定值、左轮的目标速度设定值。

在本实施例中，目标重心角度设定值模块1101、纠正倾斜角度子模块1101-1、修正重心角度设定值子模块1101-2、验证重心角度设定值子模块1101-3、控制模块1102都调用了双PID控制子模块。参考图14，该图是本申请一实施例中双PID控制子模块的框图。如图所示，双PID控制子模块包括获取子模块1401、角度PID子模块1402、速度PID子模块1403、电机控制子模块1404。其中，上述获取子模块1401，用于获取两轮自平衡机器人当前倾斜角度及当前速度；角度PID子模块1402，用于计算当前倾斜角度与当前重心角度设定值的角度差值，对该角度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到角度控制参数；速度PID子模块1403，用于计算当前速度与上述速度设定值的速度差值，对该速度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到速度控制参数；电机控制子模块1404，用于将上述角度控制参数与上述速度控制参数进行融合，用于控制电机转动。其中，将上述角度控制参数与上述速度控制参数按照一定的比例求和。如角度控制参数乘以系数0.8，速度控制参数乘以系数1.2。

在本实施例中，两轮自平衡机器人的控制装置内置于电机中，该装置支持双电机主从模式和单电机模式，其中，双电机主从模式为采用两个电机分别驱动两轮自平衡机器人中的两个轮，单电机模式为采用一个电机驱动两轮自平衡机器人的两个轮。所以上述装置还包括选定工作模式模块。参考图15，该图是本申请一实施例中选定工作模式模块的框图。如图15所示，选定工作模式模块包括检测子模块1501、主模式确认子模块1502、模式切换子模块1503。其中，检测子模块1501，用于检测如果装置被开启，则选定默认模式。主模式确认子模块1502，用于执行主模式确认步骤：随机延时一段时间，发送竞选主模式信息，并开启监听，用于监听第二电机是否发送其已经接收到上述竞选主模式信息的回应信息，响应于接收到上述回应信息，则切换为主模式，且关闭对上述第二电机的竞选主模式信息的回应信息。模式切换子模块1503，用于响应于没有接收到上述第二电机发送的竞选主模式信息的回应信息，则重新执行上述主模式确认子模块1502，如果累计未收到回应信息的次数大于预设数值，则发送非主模式信息，并监听上述第二电机是否发送其已经接收到上述非主模式信息的回应信息，响应于接收到上述非主模式的回应信息，则切换为从模式，否则，切换为单电机模式。如此，装置确定本电机是主模式、从模式、单电机模式中的哪一个模式。

在双电机主从模式下，因在自适应调整重心角度设定值，寻找目标重心角度设定值阶段，采用的是角度PID与速度设定值为零的速度PID叠加的双PID控制。因速度PID控制的速度设定值为零，为实施简单，节省开发时间，设置主模式电机执行角度PID与速度PID叠加的双PID控制，而从模式电机不执行角度PID与速度PID叠加的双PID控制，只接收主模式电机控制子模块的指令。如此，双PID控制子模块分为主模式电机控制子模块、从模式电机控制子模块。具体的：主模式电机控制子模块，用于主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度，根据内置的编码器，计算所述主模式电机的当前速度，之后，向从模式电机发送获取从模式电机的当前速度的指令；响应于接收到所述从模式电机返回的从模式电机的当前速度，根据所述主模式电机的当前速度、从模式电机的当前速度，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、当前速度、设定设定值零作为输入，用于执行角度PID与速度PID叠加的双PID控制，得到控制参数，并将所述控制参数发送给所述从模式电机，最后，根据所述控制参数，控制主模式电机转动。从模式电机控制子模块，用于所述从模式电机接收所述主模式电机发送的获取从模式电机的当前速度的指令，根据内置的编码器，计算所述从模式电机的当前速度，并将所述从模式电机的当前速度发送给所述主模式电机，之后，接收所述主模式电机发送的控制参数，用于控制所述从模式电机转动。

在双电机主从模式下，在控制阶段，为更准确的控制两轮自平衡机器人行驶。双PID子模块分为主模式电机双PID控制子模块、从模式电机双PID控制子模块、同步子模块。具体的：主模式电机双PID控制子模块，用于所述主模式电机接收操控终端发送的指令，将所述指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值，之后，根据内置的六轴姿态传感器，计算当前倾斜角度，之后，向所述从模式电机发送所述从模式电机的目标速度设定值、当前倾斜角度，然后，根据内的编码器，计算主模式电机的当前速度，最后，将所述目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入。从模式电机双PID控制子模块，用于所述从模式电机接收所述主模式电机发送的所述从模式电机的目标速度设定值、当前倾斜角度，之后，根据内置的编码器，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、所述从模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入。同步子模块，用于所述主模式电机与所述从模式电机约定同步执行双PID控制的约定时间，在所述约定时间，所述主模式电机、所述从模式电机同时分别执行角度PID与速度PID叠加的双PID控制。

根据本公开实施例的一方面，还提供一种两轮自平衡机器人，该两轮自平衡机器人包括上面描述的根据本公开实施例的控制装置。

以上描述仅为本申请的较佳实施例以及对所运用技术原理的说明。本领域技术人员应当理解，本申请中所涉及的发明范围，并不限于上述技术特征的特定组合而成的技术方案，同时也应涵盖在不脱离所述发明构思的情况下，由上述技术特征或其等同特征进行任意组合而形成的其它技术方案。例如上述特征与本申请中公开的(但不限于)具有类似功能的技术特征进行互相替换而形成的技术方案。

Claims (19)

1.一种两轮自平衡机器人的控制方法，其特征在于，所述方法包括：

采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，其中，所述速度PID的速度设定值为零；

基于所述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将所述指令转换成目标速度设定值，获取所述两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动。

2.根据权利要求1所述的方法，其特征在于，所述采用角度PID与速度设定值为零的速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，包括：

将默认重心角度作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动，不断调整两轮自平衡机器人的倾斜角度，直到当前倾斜角度可等于所述默认重心角度；

以预设时间间隔循环执行如下梯度步骤：获取所述两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，将更新后的当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制所述电机转动，来调整所述两轮自平衡机器人的倾斜角度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值不大于所述阈值，则将当前重心角度设定值输出为待选重心角度设定值；

将所述待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，采用所述角度PID与速度PID叠加的双PID控制所述电机转动，不断调整所述两轮自平衡机器人的倾斜角度，如果在预设时间段内，所述两轮自平衡机器人的倾斜角度与所述待选重心角度设定值的差值的绝对值一直小于误差阈值，则将所述待选重心角度设定值作为目标重心角度设定值。

3.根据权利要求2所述的方法，其特征在于，所述通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，包括：

如果该当前倾斜角度大于当前重心角度设定值，则将前重心角度设定值加上所述梯度值，得到更新后的当前重心角度设定值；

如果该当前倾斜角度小于当前重心角度设定值，则将前重心角度设定值减去所述梯度值，得到更新后的当前重心角度设定值。

4.根据权利要求1所述的方法，其特征在于，所述将所述指令转换成目标速度设定值，包括：

如果所述指令为原地平衡指令，则设置目标速度设定值为零；

如果所述指令为加速指令，则将所述当前速度乘以预设加系数，得到目标速度设定值；

如果所述指令为减速指令，则将所述当前速度乘以预设减系数，得到目标速度设定值；

如果所述指令为转向指令，则根据所述转向指令指示的转动方向，将所述当前速度转换成具有一定差值的右轮的目标速度设定值、左轮的目标速度设定值。

5.根据权利要求1-4任一所述的方法，其特征在于，角度PID与速度PID叠加的双PID控制，包括：

获取所述两轮自平衡机器人当前倾斜角度及当前速度；

计算当前倾斜角度与当前重心角度设定值的角度差值，对该角度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到角度控制参数；

计算当前速度与所述速度设定值的速度差值，对该速度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到速度控制参数；

将所述角度控制参数与所述速度控制参数进行融合，用于控制所述电机转动。

6.根据权利要求1所述的方法，其特征在于，所述方法运行于电机中，且所述方法支持双电机主从模式和单电机模式，其中，所述双电机主从模式为采用两个电机驱动所述两轮自平衡机器人中的两个轮，所述单电机模式为采用一个电机驱动所述两轮自平衡机器人的两个轮。

7.根据权利要求6所述的方法，其特征在于，所述方法还包括选定所述电机的工作模式，具体的：

检测如果所述电机被开启，则选定默认模式；

执行主模式确认步骤：随机延时一段时间，发送竞选主模式信息，并开启监听，用于监听第二电机是否发送其已经接收到所述竞选主模式信息的回应信息，响应于接收到所述回应信息，则切换为主模式，且关闭对所述第二电机的竞选主模式信息的回应信息；

响应于没有接收到所述第二电机发送的竞选主模式信息的回应信息，则重新执行所述主模式确认步骤，如果累计未接收到回应信息的次数大于预设数值，则发送非主模式信息，并监听所述第二电机是否发送其已经接收到所述非主模式信息的回应信息，响应于接收到所述非主模式的回应信息，则切换为从模式，否则，切换为单电机模式。

8.根据权利要求7所述的方法，其特征在于，在双电机主从模式下，在自适应调整重心角度设定值阶段，角度PID与速度PID叠加的双PID控制，包括如下步骤：

主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度，根据内置的编码器，计算所述主模式电机的当前速度，之后，向从模式电机发送获取从模式电机的当前速度的指令；响应于接收到所述从模式电机返回的从模式电机的当前速度，根据所述主模式电机的当前速度、从模式电机的当前速度，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、当前速度、设定设定值零作为输入，用于执行角度PID与速度PID叠加的双PID控制，得到控制参数，并将所述控制参数发送给所述从模式电机，最后，根据所述控制参数，控制主模式电机转动；

所述从模式电机接收所述主模式电机发送的获取从模式电机的当前速度的指令，根据内置的编码器，计算所述从模式电机的当前速度，并将所述从模式电机的当前速度发送给所述主模式电机，之后，接收所述主模式电机发送的控制参数，用于控制所述从模式电机转动。

9.根据权利要求8所述的方法，其特征在于，在双电机主从模式下，在控制步骤阶段，角度PID与速度PID叠加的双PID控制，包括如下步骤：

所述主模式电机接收操控终端发送的指令，将所述指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值，之后，根据内置的六轴姿态传感器，计算当前倾斜角度，之后，向所述从模式电机发送所述从模式电机的目标速度设定值、当前倾斜角度，然后，根据内的编码器，计算主模式电机的当前速度，最后，将所述目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；

所述从模式电机接收所述主模式电机发送的所述从模式电机的目标速度设定值、当前倾斜角度，之后，根据内置的编码器，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、所述从模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；

所述主模式电机与所述从模式电机约定同步执行双PID控制的约定时间，在所述约定时间，所述主模式电机、所述从模式电机同时分别执行角度PID与速度PID叠加的双PID控制。

10.一种两轮自平衡机器人的控制装置，其特征在于，所述装置包括：

目标重心角度设定值模块，用于采用角度PID与速度PID叠加的双PID控制，驱动两轮自平衡机器人轮子的电机转动，自适应调整重心角度设定值，找到目标重心角度设定值，其中，所述速度PID的速度设定值为零；

控制模块，用于基于所述目标重心角度设定值，执行如下控制步骤：接收操控终端发送的指令，将所述指令转换成目标速度设定值，获取两轮自平衡机器人的当前倾斜角度及当前速度，将所述目标重心角度设定值、当前倾斜角度、目标速度设定值、当前速度作为输入，用于运行角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动。

11.根据权利要求10所述的装置，其特征在于，所述目标重心角度设定值模块，包括：

纠正倾斜角度子模块，用于将默认重心角度作为当前重心角度设定值，零作为速度设定值，采用角度PID与速度PID叠加的双PID控制，驱动所述两轮自平衡机器人轮子的电机转动，不断调整两轮自平衡机器人的倾斜角度，直到当前倾斜角度可等于所述默认重心角度；

修正重心角度设定值子模块，用于以预设时间间隔循环执行如下梯度步骤：获取所述两轮自平衡机器人当前倾斜角度及当前速度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值大于阈值，则通过加减梯度值，更新当前重心角度设定值的大小，让更新后的当前重心角度设定值靠近当前倾斜角度，将更新后的当前重心角度设定值、当前倾斜角度、速度设定值零、当前速度作为角度PID与速度PID叠加的双PID控制的输入，控制所述电机转动，来调整所述两轮自平衡机器人的倾斜角度，如果该当前倾斜角度与当前重心角度设定值的差值的绝对值不大于所述阈值，则将当前重心角度设定值输出为待选重心角度设定值；

验证重心角度设定值子模块，用于将所述待选重心角度设定值作为当前重心角度设定值，零作为速度设定值，采用所述角度PID与速度PID叠加的双PID控制所述电机转动，不断调整所述两轮自平衡机器人的倾斜角度，如果在预设时间段内，所述两轮自平衡机器人的倾斜角度与所述待选重心角度设定值的差值的绝对值一直小于误差阈值，则将所述待选重心角度设定值作为目标重心角度设定值。

12.根据权利要求11所述的装置，其特征在于，所述修正重心角度设定值子模块包括：

增大重心角设定值子模块，用于如果该当前倾斜角度大于当前重心角度设定值，则将前重心角度设定值加上所述梯度值，得到更新后的当前重心角度设定值；

减小重心角设定值子模块，用于如果该当前倾斜角度小于当前重心角度设定值，则将前重心角度设定值减去所述梯度值，得到更新后的当前重心角度设定值。

13.根据权利要求10所述的装置，其特征在于，所述控制模块包括：

原地平衡子模块，用于如果所述指令为原地平衡指令，则设置目标速度设定值为零；

加速子模块，用于如果所述指令为加速指令，则将所述当前速度乘以预设加系数，得到目标速度设定值；

减速子模块，用于如果所述指令为减速指令，则将所述当前速度乘以预设减系数，得到目标速度设定值；

转向子模块，用于如果所述指令为转向指令，则根据所述转向指令指示的转动方向，将所述当前速度转换成具有一定差值的右轮的目标速度设定值、左轮的目标速度设定值。

14.根据权利要求10-13任一所述的装置，其特征在于，双PID控制子模块包括：

获取子模块，用于获取所述两轮自平衡机器人当前倾斜角度及当前速度；

角度PID子模块，用于计算当前倾斜角度与当前重心角度设定值的角度差值，对该角度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到角度控制参数；

速度PID子模块，用于计算当前速度与所述速度设定值的速度差值，对该速度差值进行比例运算、积分运算、微分运算中的至少两个运算，得到速度控制参数；

电机控制子模块，用于将所述角度控制参数与所述速度控制参数进行融合，用于控制所述电机转动。

15.根据权利要求10所述的装置，其特征在于，所述装置内置于电机中，且所述装置支持双电机主从模式和单电机模式，其中，所述双电机主从模式为采用两个电机驱动所述两轮自平衡机器人中的两个轮，所述单电机模式为采用一个电机驱动所述两轮自平衡机器人的两个轮。

16.根据权利要求15所述的装置，其特征在于，所述装置还包括选定工作模式模块，具体的：

检测子模块，用于检测如果所述电机被开启，则选定默认模式；

主模式确认子模块，用于执行主模式确认步骤：随机延时一段时间，发送竞选主模式信息，并开启监听，用于监听第二电机是否发送其已经接收到所述竞选主模式信息的回应信息，响应于接收到所述回应信息，则切换为主模式，且关闭对所述第二电机的竞选主模式信息的回应信息；

模式切换子模块，用于响应于没有接收到所述第二电机发送的竞选主模式信息的回应信息，则重新执行所述主模式确认步骤，如果累计未接收到回应信息的次数大于预设数值，则发送非主模式信息，并监听所述第二电机是否发送其已经接收到所述非主模式信息的回应信息，响应于接收到所述非主模式的回应信息，则切换为从模式，否则，切换为单电机模式。

17.根据权利要求16所述的装置，其特征在于，在双电机主从模式下，在自适应调整重心角度设定值阶段，双PID控制子模块分为：

主模式电机控制子模块，用于主模式电机根据内置的六轴姿态传感器，计算当前倾斜角度，根据内置的编码器，计算所述主模式电机的当前速度，之后，向从模式电机发送获取从模式电机的当前速度的指令；响应于接收到所述从模式电机返回的从模式电机的当前速度，根据所述主模式电机的当前速度、从模式电机的当前速度，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、当前速度、设定设定值零作为输入，用于执行角度PID与速度PID叠加的双PID控制，得到控制参数，并将所述控制参数发送给所述从模式电机，最后，根据所述控制参数，控制主模式电机转动；

从模式电机控制子模块，用于所述从模式电机接收所述主模式电机发送的获取从模式电机的当前速度的指令，根据内置的编码器，计算所述从模式电机的当前速度，并将所述从模式电机的当前速度发送给所述主模式电机，之后，接收所述主模式电机发送的控制参数，用于控制所述从模式电机转动。

18.根据权利要求17所述的装置，其特征在于，在双电机主从模式下，在控制步骤阶段，双PID控制子模块分为：

主模式电机双PID控制子模块，用于所述主模式电机接收操控终端发送的指令，将所述指令转换成主模式电机的目标速度设定值、从模式电机的目标速度设定值，之后，根据内置的六轴姿态传感器，计算当前倾斜角度，之后，向所述从模式电机发送所述从模式电机的目标速度设定值、当前倾斜角度，然后，根据内的编码器，计算主模式电机的当前速度，最后，将所述目标重心角度设定值、当前倾斜角度、主模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；

从模式电机双PID控制子模块，用于所述从模式电机接收所述主模式电机发送的所述从模式电机的目标速度设定值、当前倾斜角度，之后，根据内置的编码器，计算当前速度，然后，将所述目标重心角度设定值、当前倾斜角度、所述从模式电机的目标速度设定值、当前速度作为角度PID与速度PID叠加的双PID控制的输入；

同步子模块，用于所述主模式电机与所述从模式电机约定同步执行双PID控制的约定时间，在所述约定时间，所述主模式电机、所述从模式电机同时分别执行角度PID与速度PID叠加的双PID控制。

19.一种两轮自平衡机器人，其特征在于，该两轮自平衡机器人包括权利要求10至18中任一权利要求所述的装置。