CN106182000B - 一种基于部分已知参数的两轮自平衡机器人控制方法 - Google Patents

Abstract

本发明公开了一种基于部分已知参数的两轮自平衡机器人控制方法，在主控芯片中设置控制器；获取自平衡机器人的运动参数；以期望速度和实际速度的速度误差e_v作为速度鲁棒控制器和速度滑模控制器的输入信号，获取期望角度θ_r；以期望角度θ_r和实际角度θ的角度误差e_θ和角速度作为角度鲁棒控制器和角度滑模控制器的输入信号，控制输出电压U从而驱动电机系统运动。采用本发明的技术方案，通过差量作为反馈实现角度鲁棒控制器、角度滑模控制器、速度鲁棒控制器和速度滑模控制器的输出控制，从而对于实际两轮自平衡机器人中的一些参数估计存在偏差时，控制器依然能够保持两轮自平衡机器人拥有良好的性能。

Description

一种基于部分已知参数的两轮自平衡机器人控制方法

技术领域

本发明涉及两轮自平衡机器人控制领域，尤其涉及一种基于部分已知参数的两轮自平衡机器人控制方法。

背景技术

两轮自平衡机器人是一种利用传感器感知自身状态，然后通过控制算法控制马达转动，从而实现自平衡。近年来，随着两轮自平衡机器人技术不断完善以及成本不断降低，逐渐成为更多人接受的代步工具，使两轮自平衡机器人开始从实验研究阶段转变为大众型的代步工具，其所面临的环境和任务也越来越复杂。

目前市场上有各种类型的平衡机器人，大多使用PID控制算法，该算法通过采集两轮自平衡机器人当前角度并计算与目标角度的偏差，在将这个偏差进行比例、积分、微分运算计算出马达控制量从而实现两轮自平衡机器人自平衡。这种算法简单实用但并不是最理想的控制器，因为在复杂的运行环境中，该算法在很多时候处理的并不是很好，比如，该方法在外界存在干扰时，就会使控制出现抖震，在干扰特别大时，还会使平衡车失去平衡；同时，PID算法使用比例、积分、微分这三个成员进行线性组合也是不合理的，这种线性组合的方式会使其在系统鲁棒性和系统稳定性上无法两者兼顾，提高鲁棒性会使稳定性降低，反之提高稳定性则降低鲁棒性。

与此同时，两轮自平衡机器人在使用过程中会逐渐老化，或者其运行环境发生巨大变化时，其固有参数会随之发生变化，比如，转子(轮胎)转动惯量J_m，会随着摩擦力变化而改变，以及其他一些物理参量也会在使用过程中发生变化。虽然这些固有参数的变化是缓慢的，但长期积累也会对控制器的输出造成影响，从而使系统变得不稳定，然而，现有技术的控制器并未考虑上述因素对其造成的影响。

故，针对目前现有技术中存在的上述缺陷，实有必要进行研究，以提供一种方案，解决现有技术中存在的缺陷。

发明内容

本发明的目的是提供一种基于部分已知参数的两轮自平衡机器人控制方法，能够在外界条件发生变化或在有些参数发生摄动的情况下依然保持两轮自平衡机器人拥有良好的性能。

为了克服现有技术的缺陷，本发明采用的技术方案为：

一种基于部分已知参数的两轮自平衡机器人控制方法，包括以下步骤：

在主控芯片中设置控制器，所述控制器至少包括角度鲁棒控制器、角度滑模控制器、速度鲁棒控制器和速度滑模控制器；

获取自平衡机器人的运动参数，该运动参数至少包括期望速度实际速度实际角度θ和角速度

以期望速度和实际速度的速度误差e_v作为速度鲁棒控制器和速度滑模控制器的输入信号，获取期望角度θ_r；

以期望角度θ_r和实际角度θ的角度误差e_θ和角速度作为角度鲁棒控制器和角度滑模控制器的输入信号，控制输出电压U从而驱动电机系统运动；

其中，控制器的输出方程为：

其中，U₀为角度滑模控制器的输出量，U₁为角度鲁棒控制器的输出量；

角度鲁棒控制器的输出方程为：

角度滑模控制器的输出方程为：

其中，期望角度θ_r满足 为速度滑模控制器的输出量，为速度鲁棒控制器的输出量；

速度鲁棒控制器的输出方程为：

速度滑模控制器的输出方程为：

优选地，通过检测速度手柄输出的线性霍尔信号获取期望速度

优选地，通过陀螺仪采集角速度信号以及通过加速度计采集加速度信号和地磁信号，获取实际角度θ和角速度

优选地，所述陀螺仪的型号为L3G420D。

优选地，所述加速度计的型号为LSM303D。

优选地，通过编码器获取实际速度

优选地，通过通讯模块实现自平衡机器人与外部设备进行数据通讯。

优选地，通过设置转向杆线性霍尔传感器实现自平衡机器人转向控制。

优选地，所述主控芯片采用DSP芯片。

优选地，所述通讯模块采用无线数据传输模块。

与现有技术相比较，本发明能够利用部分已知参数实现优化和改善两轮自平衡机器人的性能，即便在外界条件发生剧烈变化或在有些参数发生摄动的情况下，依然能够保持两轮自平衡机器人的稳定控制。

说明书附图

图1为本发明基于部分已知参数的两轮自平衡机器人控制方法的流程框图；

图2为本发明所采用的控制器的结构框图；

图3为本发明中倒立摆模型的结构框图；

图4为本发明中两轮自平衡机器人控制系统的结构框图；

图5为本发明中两轮自平衡机器人控制系统的执行流程图；

图6为仿真时参数摄动规律图；

图7为仿真时本发明速度跟踪与传统PID速度跟踪对比图；

图8为仿真时本发明速度误差与传统PID速度误差对比图；

图9为仿真时本发明角度跟踪与传统PID角度跟踪对比图；

图10为仿真时本发明角度误差与传统PID角度误差对比图；

具体实施方式

参见图1，所示为本发明一种基于部分已知参数的两轮自平衡机器人控制方法的流程框图，包括以下步骤：

步骤S1：在主控芯片中设置控制器，参见图2，所示为本发明两轮自平衡机器人控制器的原理框图，控制器至少包括角度鲁棒控制器、角度滑模控制器、速度鲁棒控制器和速度滑模控制器；

步骤S2：获取自平衡机器人的运动参数，该运动参数至少包括期望速度实际速度实际角度θ和角速度

步骤S3：以期望速度和实际速度的速度误差e_v作为速度鲁棒控制器和速度滑模控制器的输入信号，获取期望角度θ_r；

步骤S4：以期望角度θ_r和实际角度θ的角度误差e_θ和角速度作为角度鲁棒控制器和角度滑模控制器的输入信号，根据角度误差e_θ调节控制输出；

步骤S5：控制输出电压U从而驱动电机系统运动。

其中，控制器的输出方程为：

其中，U₀为角度滑模控制器的输出量，U₁为角度鲁棒控制器的输出量；

进一步的，角度鲁棒控制器的输出方程为：

进一步的，角度滑模控制器的输出方程为：

其中，期望角度θ_r满足 为速度滑模控制器的输出量，为速度鲁棒控制器的输出量；

速度鲁棒控制器的输出方程为：

速度滑模控制器的输出方程为：

上述控制器的设计原理如下：

两轮自平衡机器人的系统可以等效看作是一个倒立摆模型，参见图3，所示的倒立摆模型为现有技术通用的动力模型结构。从能量和动量角度分析，利用拉格朗日动力学理论，可以得到以下描述：

U＝-mgl+mglcosθ (2)

(1)式和(2)式中，m为车身质量，M_w为转子(轮胎)质量，l为摆杆长度、J_e为平衡车转动惯量、J_m为转子(轮胎)转动惯量，平衡车轮胎转速，R为平衡车轮胎半径，这些参量都为自平衡机器人的固有参量，取决于自平衡机器人机械架构；在倒立摆模型下的不同机械架构，上述参量会发生变化。

其中，X_w为路程、为速度、θ为角度和为角速度为自平衡机器人的运动参量，这些数据可以通过传感器采集到。

在两轮自平衡机器人控制中，θ变化范围很小所以cosθ可以近似为1，sinθ可以近似为θ，然后根据(1)、(2)两个方程联立可以得到：

写成状态空间形式：

然后我们可以另 于是动力学模型可简化为

即简写形式：

由状态空间方程，可得到：

令

a₄₃＝a₄₃₀+Δa₄₃

a₂₃＝a₂₃₀+Δa₂₃

b₁＝b₁₀+Δb₁

b₂＝b₂₀+Δb₂ (9)

其中a₄₃₀、a₂₃₀、b₁₀、b₂₀为已知部分，Δa₄₃、Δa₂₃、Δb₁、Δb₂为未知部分。于是公式可重写为：

其中，

P₁＝Δa₄₃+Δb₂U

P₂＝Δa₂₃+Δb₁U (11)

定义角度误差：

e_θ＝θ-θ_r (12)

代入(10)式可得：

考虑不确定性因素公式(13)可简化为：

其中，

为了使系统稳定且误差能够被快速稳定，将角度鲁棒控制器可被设计为：

其中k₁₁与k₁₂的选取必须满足赫维兹稳定性判据。

在控制部分中U主要由两部分组成，即U满足：

将公式(17)与(16)代入到公式(14)中可得到：

令

这里η₁₁、η₁₂分别为Δa₄₃、Δb₂的上界。

引入滑模变量：

于是本发明就可以设计出角度滑模控制器：

应用李雅普诺夫稳定性原理，构造能量函数：

当s₁＝0时取“＝”。由此可知，该控制方法能够使两轮自平衡机器人保持稳定。

在直立得到控制的前提下，才可以对速度进行控制，根据模型可以构建速度与角度关系θ_r＝β·e_v(23)，其中e_v＝v-v_r(24)

这里，v和v_r表示的是瞬时速度，期望速度和实际速度表示的是平均速度，两者实际的物理意义是相同。

根据公式(23)与(10)可以得到：

不考虑误差的情况下，公式可以简化为:

为了使速度可以有效被控制，设计速度鲁棒控制器为：

根据赫维兹稳定性判据可以确定k₂₁，k₂₂的值。

速度控制输出量和角度控制一样由两部分组成，即

将公式(29)与(28)代入公式(26)可以得到：

为了使速度能够有效收敛，利用滑模控制技术，设计滑模变量：

s₂＝e_v+λ₂·e_iv

其速度滑模控制器为

根据李雅普诺夫稳定性原理构造能量函数：

由此证明，速度控制也是收敛并能够使系统稳定的。

采用上述技术方案，通过差量作为反馈实现角度鲁棒控制器、角度滑模控制器、速度鲁棒控制器和速度滑模控制器的输出控制，从而对于实际两轮自平衡机器人中的一些参数估计存在偏差时，控制器依然能够保持两轮自平衡机器人拥有良好的性能。在这些参数偏差较小时期鲁棒控制器起主导作用，在偏差较大时滑模控制器起主导作用，两者结合就可以应对外界参数发生的变化，如两轮自平衡机器人使用过程中的老化或者运行环境而导致系统固有参数发生变化。

参见图4，所述为本发明两轮自平衡机器人的系统框图，包括传感器测量模块、主控芯片、通讯模块、转向杆线性霍尔传感器、速度手柄和电机系统，其中，传感器测量模块至少包括陀螺仪、加速度计和编码器，编码器安装在电机系统中，用于测量电机的转速，控制芯片通过编码器获取实际速度陀螺仪用于采集角速度信号以及加速度计用于采集加速度信号和地磁信号，主控芯片由此获取实际角度θ和角速度其中，陀螺仪的型号为L3G420D，加速度计的型号为LSM303D；速度手柄用于输出的线性霍尔信号，主控芯片通过该线性霍尔信号获取期望速度主控芯片采用DSP芯片，并在其中设置通过上述方法设计的控制器；通讯模块采用串口通信模块或者无线数据传输模块，用于与外部设备进行数据通讯，以便于系统调试和维修检测；转向杆线性霍尔传感器用于实现自平衡机器人转向控制；电机系统至少包括无刷电机及其驱动电路。

参见图5，所述为本发明两轮自平衡机器人的系统执行流程图，该系统在开始执行后首先进行初始化，然后分两条不同频率的任务，一条是方向控制，执行周期为20ms；另一条为本发明的平衡控制，执行周期为5ms。其中平衡控制首先通过传感器(陀螺仪和加速度计)采集角速度信号和加速度信号，检测速度手柄的速度输入，然后通过姿态计算计算出两轮自平衡机器人角度，然后通过速度鲁棒控制器和速度滑模控制器计算出速度输出，速度输出计算完成后直接用作角度控制的期望信号，然后计算出角度输出，最后将直立控制和方向控制的控制输出进行叠加然后滤波从而控制电机输出。

测试数据及说明(补充)

参见图6是本发明在进行仿真时为了模拟实际情况，并且测试极端状况对实际模型中的a₄₃加入1/60Hz的正弦震荡并对包括a₄₃在内的所有参数加入幅度为1的噪声信号。本发明仿真时使用与传统PID进行对比，并且本发明与传统PID是在相等地仿真条件(输入信号都为图6的模拟信号)下进行。

参见图7与图8，在图7中左图为本发明速度跟踪曲线，右图为传统PID跟踪曲线，其中虚线为速度期望信号，实线为实际速度。在图8中左图为本发明速度误差变化曲线，右图为传统PID速度跟踪误差变化曲线；由两者对比可以很明显地发现本发明速度输出在外界参数发生摄动地情况下比较平滑稳定，而传统PID会由于系统本非线性产生抖动。

参见图9与图10，在图7中左图为本发明角度跟踪曲线，右图为传统PID角度跟踪曲线，其中虚线为速度控制部分输出地期望角度参考信号，实线为实际角度响应情况。在图10中左图为本发明角度误差变化曲线，右图为传统PID角度误差变化曲线。由本发明角度仿真波形与传统PID仿真波形对比可以明显发现本发明在速度可控地情况下角度依然可以得到有效控制，而传统PID在速度可控时角度控制并不完美产生了较大地相位差并且在角度幅度控制上传统PID也无法有效精确控制。

以上实施例的说明只是用于帮助理解本发明的方法及其核心思想。应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以对本发明进行若干改进和修饰，这些改进和修饰也落入本发明权利要求的保护范围内。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本发明中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本发明所示的这些实施例，而是要符合与本发明所公开的原理和新颖特点相一致的最宽的范围。

Claims (10)

1.一种基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，包括以下步骤：

在主控芯片中设置控制器，所述控制器至少包括角度鲁棒控制器、角度滑模控制器、速度鲁棒控制器和速度滑模控制器；

获取自平衡机器人的运动参数，该运动参数至少包括期望速度实际速度实际角度θ和角速度

以期望速度和实际速度的速度误差e_v作为速度鲁棒控制器和速度滑模控制器的输入信号，获取期望角度θ_r；

以期望角度θ_r和实际角度θ的角度误差e_θ和角速度作为角度鲁棒控制器和角度滑模控制器的输入信号，控制输出电压U从而驱动电机系统运动；

其中，控制器的输出方程为：

其中，U₀为角度滑模控制器的输出量，U₁为角度鲁棒控制器的输出量；

角度鲁棒控制器的输出方程为：

角度滑模控制器的输出方程为：

其中，期望角度θ_r满足 为速度滑模控制器的输出量，为速度鲁棒控制器的输出量；

速度鲁棒控制器的输出方程为：

速度滑模控制器的输出方程为：

2.根据权利要求1所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，通过检测速度手柄输出的线性霍尔信号获取期望速度

3.根据权利要求1所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，通过陀螺仪采集角速度信号以及通过加速度计采集加速度信号和地磁信号，获取实际角度θ和角速度

4.根据权利要求3所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，所述陀螺仪的型号为L3G420D。

5.根据权利要求3所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，所述加速度计的型号为LSM303D。

6.根据权利要求1所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，通过编码器获取实际速度

7.根据权利要求1所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，通过通讯模块实现自平衡机器人与外部设备进行数据通讯。

8.根据权利要求1所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，通过设置转向杆线性霍尔传感器实现自平衡机器人转向控制。

9.根据权利要求1所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，所述主控芯片采用DSP芯片。

10.根据权利要求7所述的基于部分已知参数的两轮自平衡机器人控制方法，其特征在于，所述通讯模块采用无线数据传输模块。