CN107562063A - 一种自平衡机器人姿态控制方法及系统 - Google Patents

Abstract

一种自平衡机器人姿态控制方法，该方法包括：自平衡机器人进行重心矫正操作，并进行上电运行；所述自平衡机器人对系统模块进行初始化设置；所述自平衡机器人驱动电机运行；所述自平衡机器人通过传感器和滤波器检测出机器人的姿态倾角；所述自平衡机器人判断是否平衡；所述自平衡机器人判断是否结束；所述自平衡机器人的PID姿态控制器判断是否需要调整PID参数。本发明还包括一种自平衡机器人姿态控制系统，该系统包括主控制模块、电机驱动模块、传感器模块、参数调整模块和滤波器模块。实施本发明的有益效果：解决对重心固定的两轮自平衡机器人进行姿态控制，并且不断电地更改PID参数以达到最优控制效果。

Description

一种自平衡机器人姿态控制方法及系统

技术领域

本发明涉及机器人控制领域，更具体地说，涉及一种自平衡机器人姿态控制方法及系统。

背景技术

自平衡机器人的想法来自于倒立摆模型，如果摆倒向左边，为了保证平衡，倒立摆的底部必须也向左侧移动。两轮自平衡机器人系统的两个车轮位于同一轴线，并分别由伺服电机独立驱动，机器人的重心在两车轮轴上方，通过运动保持动态自平衡。由于结构特殊，两轮自平衡机器人体积小、运动灵活、适应地形变化能力强、能够方便地实现零半径回转，适于在狭小和危险的工作空间内活动、能够胜任一些复杂环境里的工作。相比传统轮式机器人，两轮自平衡机器人可以进行更为紧凑的结构设计并且减轻自重。此外，由于自平衡机器人独特的结构，一个自平衡机器人可以作为各种控制算法的实验平台，通过机器人的表现性能来对控制算法进行验证。

姿态控制是两轮自平衡机器人研究的核心内容，良好的姿态控制可以使两轮自平衡机器人实现站立及移动，同时为接下来的速度及位置控制提供基础。目前已有的两轮自平衡产品是以载人为设计目标、重心随时可调整，这样的设计使得人体在姿态控制环境内，可以通过人自身的重心调节来达到平衡状态，降低了姿态控制的难度。而对于不载人的自平衡机器人而言，其重心固定无法像载人平衡车一样通过人体调节来改变姿态，而且自平衡机器人由于结构设计原因不能保证重心绝对的垂直于水平面，因此传统姿态控制算法无法解决上述问题。

发明内容

本发明所要解决的技术问题是，提供一种自平衡机器人姿态控制方法中，用以解决现有技术的不足，实现了解决重心偏置设置、姿态检测方法和PID参数可调的姿态控制方法。该方法对于各类不同重心偏置的自平衡机器人均能实现姿态控制。

本发明解决上述技术问题，提供如下解决的技术方案：一种自平衡机器人姿态控制系统，该系统包括主控制模块、电机驱动模块、传感器模块、参数调整模块；所述参数调整模块包括PID控制器，用于机器人控制系统中关于PID参数的调整，其中，所述PID参数包括PID初始量、PID控制量和PID反馈量，所述PID参数通过CAN帧格式初始化，用于实现不断电地实时进行自平衡机器人的重心矫正调试；所述传感器模块、参数调整模块和电机驱动模块分别与主控制模块相互通信连接进行控制。

在本发明的自平衡机器人姿态控制系统中，所述PID初始量为自平衡机器人在静止时检测机器人重心与铅直线的偏置角度，所述PID反馈量为自平衡机器人的传感器采集数据进行滤波后得到的姿态倾角；PID控制量由主控制模块自定义。

在本发明的自平衡机器人姿态控制系统中，该系统还包括滤波器模块，其中，所述滤波器模块采用卡尔曼滤波器滤波，能够有效地滤除传感器采集数据的噪声和干扰，得到机器人稳定的姿态倾角。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块采用数字信号处理芯片作为主控芯片，分别通过不同地通信方式与传感器和电机驱动模块进行通信；所述传感器模块包括加速度计和陀螺仪的六轴运动传感器，并将陀螺仪的输出值作为滤波器模块的状态控制量、加速度计的输出值作为滤波器模块的输入量，该输入量经过滤波器模块得到稳定的机器人姿态倾角。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块通过I2C通信方式与传感器模块相互通信连接，所述主控制模块与滤波器模块相连接，所述主控制模块通过CAN通信方式与电机驱动模块相互通信连接，所述主控制模块通过CAN通信方式与参数调整模块相互通信连接。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块通过I2C通信方式与传感器模块相互通信连接，所述主控制模块通过CAN通信方式接收外部指令，所述主控制模块通过CAN通信方式与电机驱动模块相互通信连接。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块通过CAN帧ID值的不同来区分重心偏置数据和PID参数，其中，所述重心偏置数据为自平衡机器人在运动时检测机器人重心与铅直线的偏置角度。

本发明还包括一种自平衡机器人姿态控制方法，该方法包括如下步骤：

S1、自平衡机器人的参数调整模块对该机器人的重心进行重心矫正操作并进行上电运行；

S2、所述自平衡机器人的主控制模块对机器人进行初始化设置；

S3、所述主控制模块驱动电机运行；

S4、所述主控制模块判断定时中断是否产生，如果是则进行步骤S5，否则进行步骤S4；

S5、所述主控制模块通过传感器模块检测出机器人的姿态倾角并通过滤波器模块进行过滤整合得到稳定的姿态倾角；

S6、所述主控制模块判断机器人是否平衡，如果是则进行步骤S7，如果否则步骤S8；

S7、所述自平衡机器人判断是否结束，如果是则结束步骤，如果否则进行步骤S3；

S8、所述自平衡机器人的PID姿态控制器判断是否需要调整PID参数，如果是则进行步骤S2，否则进行步骤S3。

在本发明的自平衡机器人姿态控制方法中，所述步骤S2包括如下步骤：

S21、所述自平衡机器人系统初始化主控制模块的时钟、中断等数据；

S22、所述自平衡机器人初始化PID参数，用于保证在调试机器人姿态控制中能够不断电地随时调整PID参数，优化控制效果；

S23、所述自平衡机器人初始化所述主控制模块与电机驱动模块和传感器模块之间的通信接口。

在本发明的自平衡机器人姿态控制方法中，所述步骤S5包括如下步骤：

S51、设定定时中断Ts，每Ts作为一个检测周期，在每个检测周期内，所述自平衡机器人的控制系统分别读取第一传感器的第一采集数据和第二传感器的第二采集数据；

S52、所述自平衡机器人的控制系统通过滤波器模块对第一采集数据和第二采集数据进行数据融合，得到所述自平衡机器人的姿态倾角。

实施本发明的自平衡机器人姿态控制方法及系统，具有以下有益效果：解决对重心固定的两轮自平衡机器人进行姿态控制，并且不断电地更改PID参数以达到最优控制效果。将该算法应用于我们自制的自平衡试验平台，可以达到较好的姿态控制效果。另外通过在试验平台上更改配重，改变其重心，仍然能够实现较好的姿态控制效果，实现了本发明的通用性。

附图说明

下面将结合附图及实施例对本发明作进一步说明，附图中：

图1为本发明的自平衡机器人姿态控制系统的系统结构图；

图2为本发明的自平衡机器人姿态控制方法流程示意图；

图3为本发明的自平衡机器人的重心校正示意图；

图4为本发明的自平衡机器人的卡尔曼滤波器滤波后的结果示意图；

如附图所示，100、主控制模块，200、电机驱动模块，300、传感器模块，400、参数调整模块，500、滤波器模块，111、PID初始量，222、PID控制量，333、PID反馈量。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定本发明。

如图1所示，为本发明的自平衡机器人姿态控制系统的系统结构图；本发明针对不载人且重心固定的两轮自平衡机器人提出了一种自平衡机器人姿态控制系统，该系统包括主控制模块100、电机驱动模块200、传感器模块300、参数调整模块400；所述参数调整模块400包括PID控制器，用于机器人控制系统中关于PID参数的调整，其中，所述PID参数包括PID初始量111、PID控制量222和PID反馈量333，所述PID参数通过CAN帧格式初始化，用于实现不断电地实时进行自平衡机器人的重心矫正调试；所述传感器模块300、参数调整模块400和电机驱动模块200分别与主控制模块100相互通信连接进行控制。

优选的，数字信号处理芯片选择德州仪器公司的数字信号处理芯片作为主控芯片。

在本发明的自平衡机器人姿态控制系统中，所述PID初始量111为自平衡机器人在静止时检测机器人重心与铅直线的偏置角度，所述PID反馈量333为自平衡机器人的传感器采集数据进行滤波后得到的姿态倾角；PID控制量222由主控制模块自定义。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块100采用数字信号处理芯片作为主控芯片，分别通过不同地通信方式与传感器和电机驱动模块进行通信；所述传感器模块300包括加速度计和陀螺仪的六轴运动传感器，并将陀螺仪的输出值作为滤波器模块500的状态控制量、加速度计的输出值作为滤波器模块500的输入量，该输入量经过滤波器模块500得到稳定的机器人姿态倾角。

优选的，主控制模块100是整个系统的核心，我们选择具有丰富接口资源及计算处理能力强大的数字信号处理器(DSP)作为控制芯片，内嵌姿态解算(卡尔曼滤波器)、姿态控制(PID控制器)等算法程序，并且在初始化程序中设计了PID参数可不断电实时调整的功能。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块100通过12C通信方式与传感器模块300相互通信连接，所述主控制模块100与滤波器模块500相连接，所述主控制模块100通过CAN通信方式与电机驱动模块200相互通信连接，所述主控制模块100通过CAN通信方式与参数调整模块400相互通信连接。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块100通过12C通信方式与传感器模块300相互通信连接，所述主控制模块100通过CAN通信方式接收外部指令，所述主控制模块100通过CAN通信方式与电机驱动模块200相互通信连接。

在本发明的自平衡机器人姿态控制系统中，所述主控制模块100通过CAN帧ID值的不同来区分重心偏置数据和PID参数，其中，所述重心偏置数据为自平衡机器人在运动时检测机器人重心与铅直线的偏置角度。

优选的，利用主控制模块100中主控芯片的CAN通信模块功能，设置有与对外接收CAN数据帧的接口。

如图2所示，为本发明的自平衡机器人姿态控制方法流程示意图；本发明针对不载人且重心固定的两轮自平衡机器人提出了一种姿态控制方法，机器人的重心偏置角度在测量后可以通过CAN通信传递给控制算法，使得该方法能够适用于各类结构不同而导致重心偏角不一致的自平衡机器人；在姿态检测方面使用了卡尔曼滤波融合多传感器检测结果以提高控制精度，解决了单一传感器性能缺陷的弊端；使用PID控制器对姿态倾角进行控制，设计了参数可调整的方式，便于调整控制参数，同时将重心偏置角度加入PID控制器中的PID控制量，使得机器人能够按照当前的重心进行调试，在不垂直于水平面的姿态下也能完成自平衡站立。

本发明的一种自平衡机器人姿态控制方法，该方法包括如下步骤：

S1、自平衡机器人的参数调整模块400对该机器人的重心进行重心矫正操作并进行上电运行；

S2、所述自平衡机器人的主控制模块100对机器人进行初始化设置；

S3、所述主控制模块100驱动电机运行；

S4、所述主控制模块100判断定时中断是否产生，如果是则进行步骤S5，否则进行步骤S4；

S5、所述主控制模块100通过传感器模块300检测出机器人的姿态倾角并通过滤波器模块500进行过滤整合得到稳定的姿态倾角；

S6、所述主控制模块100判断机器人是否平衡，如果是则进行步骤S7，如果否则步骤S8；

S7、所述自平衡机器人判断是否结束，如果是则结束步骤，如果否则进行步骤S3；

S8、所述自平衡机器人的PID姿态控制器判断是否需要调整PID参数，如果是则进行步骤S2，否则进行步骤S3。

在本发明的自平衡机器人姿态控制方法中，所述步骤S2包括如下步骤：

S21、所述自平衡机器人系统初始化主控制模块100的时钟、中断等数据；

S22、所述自平衡机器人初始化PID参数，用于保证在调试机器人姿态控制中能够不断电地随时调整PID参数，优化控制效果；

S23、所述自平衡机器人初始化所述主控制模块100与电机驱动模块200和传感器模块300之间的通信接口。

在本发明的自平衡机器人姿态控制方法中，所述步骤S5包括如下步骤：

S51、设定定时中断Ts，每Ts作为一个检测周期，在每个检测周期内，所述自平衡机器人的控制系统分别读取第一传感器的第一采集数据和第二传感器的第二采集数据；

S52、所述自平衡机器人的控制系统通过滤波器模块500对第一采集数据和第二采集数据进行数据融合，得到所述自平衡机器人的姿态倾角。

在本发明的自平衡机器人姿态控制方法中，所述滤波器采用卡尔曼滤波器；其中，为了在数字芯片中能够实现卡尔曼滤波器，将卡尔曼滤波器离散化，令T_s为系统采样周期，亦即获取陀螺仪和加速度计数据的时间间隔，同时按照卡尔曼滤波器迭代更新格式将过程噪声与测量噪声提取转化为噪声协方差矩阵可以得到卡尔曼滤波器的系统方程；其中，所述卡尔曼滤波器采用线性系统状态的最优估计算法对第一采集数据和第二采集数据进行数据合并，过滤噪声。

如图3所示，为本发明的自平衡机器人的重心校正示意图，所述PID参数包括PID初始量111、PID控制量222和PID反馈量333；首先在静止时检测机器人重心与铅直线的偏置角度，将该值记为PID初始量111。对姿态进行PID控制，在PID控制器中，将PID初始量111加入PID控制量222，令每个周期得到的卡尔曼滤波后的角度值为PID反馈量333，进行PID控制。在调试中，可以根据初始化中设计好的CAN帧格式传入PID参数，实现不断电地实时调试。在自平衡机器人的PID姿态控制器的控制中，将该PID反馈量333作为补偿实现该自平衡机器人的重心矫正。

在本发明的自平衡机器人姿态控制方法中，所述步骤S8包括如下步骤：

S81、通过步骤S4得到的姿态倾角，采用PID控制器对其进行控制，将PID控制器输出结果转换为发送给电机的指令来完成自平衡机器人的闭环控制；

S82、所述PID控制器判断是否需要调整PID参数，如果是则进行在步骤S2中对PID参数进行初始化设置，否则直接进行步骤S3，驱动电机启动。

优选的，对于上一步骤得到的姿态倾角，我们采用绝对式PID控制器对其进行控制。将步骤中检测到的角度偏差作为补偿值加入PID控制量中，将PID控制器输出结果转换为发送给电机的指令来完成自平衡机器人的闭环控制。同时由于主控制模块中设置了PID参数可实时调整的功能，调试PID控制器时可以直接更改参数来观察控制效果。

如图4所示，为本发明的自平衡机器人的卡尔曼滤波器滤波后的结果示意图，从图上能够看出该方法充分发挥陀螺仪动态特性优异以及加速度计测量准确的特点，同时抑制了陀螺仪漂移严重和加速度计易受振动影响的弊端，准确的检测两轮自平衡机器人实时姿态。

在本发明的自平衡机器人姿态控制系统中，该系统还包括滤波器模块500，其中，所述滤波器模块500采用卡尔曼滤波器滤波，能够有效地滤除传感器采集数据的噪声和干扰，得到机器人稳定的姿态倾角。

其中，所述自平衡机器人控制系统中的主控制模块100，设定了定时中断Ts，每Ts作为一个周期读取传感器数据。所述传感器模块300采用陀螺仪和加速度计采集数据，并且使用卡尔曼滤波器对两种传感器获得的数据进行融合，得到机器人姿态倾角。

卡尔曼滤波是一种利用线性系统状态方程，通过系统输入输出观测数据，对系统状态进行最优估计的算法。由于观测数据中包括系统中的噪声和干扰的影响，所以最优估计也可看作是滤波过程。为了在数字芯片中能够实现卡尔曼滤波器，将卡尔曼滤波器离散化，令T_s为系统采样周期，亦即获取陀螺仪和加速度计数据的时间间隔，同时按照卡尔曼滤波器迭代更新格式将过程噪声与测量噪声提取转化为噪声协方差矩阵可以得到卡尔曼滤波器的系统方程为：

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

在本发明的自平衡机器人姿态控制方法中，所述第一传感器采用陀螺仪采集第一采集数据，所述陀螺仪的动态特性优异并且采用滤波器同时抑制了陀螺仪漂移严重的问题；所述第二传感器采用加速度传感器采集第二采集数据，所述加速度传感器的优点在于测量准确，采用滤波器同时抑制了加速度传感器易受振动影响。

实施本发明的自平衡机器人姿态控制方法及系统，具有以下有益效果：解决对重心固定的两轮自平衡机器人进行姿态控制，并且不断电地更改PID参数以达到最优控制效果。将该算法应用于我们自制的自平衡试验平台，可以达到较好的姿态控制效果。另外通过在试验平台上更改配重，改变其重心，仍然能够实现较好的姿态控制效果，验证了本发明的通用性。

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

Claims (10)

1.一种自平衡机器人姿态控制系统，其特征在于，该系统包括主控制模块、电机驱动模块、传感器模块、参数调整模块；所述参数调整模块包括PID控制器，用于机器人控制系统中关于PID参数的调整，其中，所述PID参数包括PID初始量、PID控制量和PID反馈量，所述PID参数通过CAN帧格式初始化，用于实现不断电地实时进行自平衡机器人的重心矫正调试；所述传感器模块、参数调整模块和电机驱动模块分别与主控制模块相互通信连接进行控制。

2.根据权利要求1所述的自平衡机器人姿态控制系统，其特征在于，所述PID初始量为自平衡机器人在静止时检测机器人重心与铅直线的偏置角度，所述PID反馈量为自平衡机器人的传感器采集数据进行滤波后得到的姿态倾角；PID控制量由主控制模块自定义。

3.根据权利要求1所述的自平衡机器人姿态控制系统，其特征在于，该系统还包括滤波器模块，其中，所述滤波器模块采用卡尔曼滤波器滤波，能够有效地滤除传感器采集数据的噪声和干扰，得到机器人稳定的姿态倾角。

4.根据权利要求1或2所述的自平衡机器人姿态控制系统，其特征在于，所述主控制模块采用数字信号处理芯片作为主控芯片，分别通过不同地通信方式与传感器和电机驱动模块进行通信；所述传感器模块包括加速度计和陀螺仪的六轴运动传感器，并将陀螺仪的输出值作为滤波器模块的状态控制量、加速度计的输出值作为滤波器模块的输入量，该输入量经过滤波器模块得到稳定的机器人姿态倾角。

5.根据权利要求1或3或4所述的自平衡机器人姿态控制系统，其特征在于，所述主控制模块通过I2C通信方式与传感器模块相互通信连接，所述主控制模块与滤波器模块相连接，所述主控制模块通过CAN通信方式与电机驱动模块相互通信连接，所述主控制模块通过CAN通信方式与参数调整模块相互通信连接。

6.根据权利要求8所述的自平衡机器人姿态控制系统，其特征在于，所述主控制模块通过I2C通信方式与传感器模块相互通信连接，所述主控制模块通过CAN通信方式接收外部指令，所述主控制模块通过CAN通信方式与电机驱动模块相互通信连接。

7.根据权利要求1所述的自平衡机器人姿态控制系统，其特征在于，所述主控制模块通过CAN帧ID值的不同来区分重心偏置数据和PID参数，其中，所述重心偏置数据为自平衡机器人在运动时检测机器人重心与铅直线的偏置角度。

8.一种自平衡机器人姿态控制方法，其特征在于，该方法包括如下步骤：

S1、自平衡机器人的参数调整模块对该机器人的重心进行重心矫正操作并进行上电运行；

S2、所述自平衡机器人的主控制模块对机器人进行初始化设置；

S3、所述主控制模块驱动电机运行；

S4、所述主控制模块判断定时中断是否产生，如果是则进行步骤S5，否则进行步骤S4；

S5、所述主控制模块通过传感器模块检测出机器人的姿态倾角并通过滤波器模块进行过滤整合得到稳定的姿态倾角；

S6、所述主控制模块判断机器人是否平衡，如果是则进行步骤S7，如果否则步骤S8；

S7、所述自平衡机器人判断是否结束，如果是则结束步骤，如果否则进行步骤S3；

S8、所述自平衡机器人的PID姿态控制器判断是否需要调整PID参数，如果是则进行步骤S2，否则进行步骤S3。

9.根据权利要求8所述的自平衡机器人姿态控制方法，其特征在于，所述步骤S2包括如下步骤：

S21、所述自平衡机器人系统初始化主控制模块的时钟、中断等数据；

S22、所述自平衡机器人初始化PID参数，用于保证在调试机器人姿态控制中能够不断电地随时调整PID参数，优化控制效果；

S23、所述自平衡机器人初始化所述主控制模块与电机驱动模块和传感器模块之间的通信接口。

10.根据权利要求8所述的自平衡机器人姿态控制方法，其特征在于，所述步骤S5包括如下步骤：

S51、设定定时中断Ts，每Ts作为一个检测周期，在每个检测周期内，所述自平衡机器人的控制系统分别读取第一传感器的第一采集数据和第二传感器的第二采集数据；

S52、所述自平衡机器人的控制系统通过滤波器模块对第一采集数据和第二采集数据进行数据融合，得到所述自平衡机器人的姿态倾角。