一种基于自抗扰的双轮自平衡机器人运动控制系统及控制方法

1.本发明属于机器人控制领域，涉及双轮自平衡机器人相关控制，尤其是一种基于自抗扰的双轮自平衡机器人运动控制系统及控制方法。


背景技术：

2.机器人技术作为一个交叉学科，涉及了机械、电子、计算机、控制理论等众多的学科与技术，并且，得益于这些学科领域的发展，机器人技术也越来越成熟，应用范围也越来越广泛。在国内外对机器人的研究中，轮式移动机器人是最热门的研究方向之一。其特殊的双轮结构，使其拥有优异的运动灵活性。在军事、工业生产以及智能家居等领域都有广泛的应用。双轮自平衡机器人是一种特殊的轮式机器人，其结构更加简单，机器人有两个独立的驱动轮，控制器通过控制电机的扭矩来使机器人保持动态平衡状态。
3.目前，在服务机器人、新型代步工具等方面都能看到双轮自平衡机器人的身影。未来，更多的领域会对双轮自平衡机器人产生需求，此外，随着技术的进步，生产双轮自平衡机器人所必需的微控制器、功率器件及传感器性能不断提高的同时成本会下降，这些因素会极大的推动双轮自平衡机器人在各个领域的应用。
4.相较于传统的轮式机器人而言，双轮自平衡机器人控制方式较为复杂，采用合理的控制方法对双轮自平衡机器人控制过程优化显得尤为重要。


技术实现要素：

5.为了弥补现有技术的不足，本发明提供一种基于自抗扰的双轮自平衡机器人运动控制系统及控制方法的技术方案。
6.所述的一种基于自抗扰的双轮自平衡机器人运动控制系统，包括
7.底盘，为安装平台，传感器、电路板通过连接件安装在所述底盘上；
8.驱动轮，独立设置，分布在所述底盘上，并通过直流有刷电机驱动；
9.惯性传感器，接收双轮自平衡机器人的加速度数据以及角速度数据；
10.微控制器，用于控制电机，结合软件程序获取机器人俯仰角，再通过所述微控制器实现机器人自平衡；
11.增量式编码器，用于获取机器人的驱动轮转速，并对机器人的运动速度进行闭环控制，以实现固定轨迹上的自动路径跟踪运动。
12.进一步的，所述底盘上还设置有lc电磁传感器、tof距离传感器、摄像头、显示屏以及上位机，所述微控制器包括控制模块mcu，电机、惯性传感器、lc电磁传感器、tof距离传感器、摄像头、显示屏以及上位机分别与所述控制模块mcu信号连接。
13.一种基于自抗扰的双轮自平衡机器人运动控制方法，包括以下步骤：
14.s1初始化程序，初始化成功后启动主程序；
15.s11初始化失败，则重新进行初始化程序，直至初始化成功；
16.s2判断主程序是否进入中断，若进入中断则通过控制系统进行控制；
17.s3若主程序未进入中断，则判断机器人是否发生故障，发生故障则停止机器人运行；
18.s31未发生故障，则保持主程序运行。
19.进一步的，所述控制系统包括以下步骤：
20.s21对速度进行采集，并进行速度控制；
21.s22对姿态进行解算，并进行平衡控制；
22.s23对路径进行提取，并进行转向控制；
23.s24根据触发主程序进入中断状态的条件，分别进行步骤s21-s23，调节控制后主程序重新运行。
24.进一步的，所述主程序包括数据发送、硬件自检以及人机交互，通过传感器获取数据信息，并发送至上位机中。
25.进一步的，在主程序运行过程中，通过自抗扰控制模型对机器人的整体速度和转向进行控制，所述自抗扰控制器包括
26.跟踪微分器，对输入信号进行跟踪并提取其微分信号，安排过度过程以及滤波；
27.扩张状态观测器，根据系统输入和输出确定系统内部状态变量，并对扰动进行实时估计和补充；
28.非线性误差反馈控制律，基于误差反馈控制，对跟踪信号误差、微分信号误差进行非线性组合。
29.进一步的，所述跟踪微分器的离散表达方程组为：
[0030][0031]
其中fhan(v1,v2,r,h)函数为综合最优控制函数，函数的输出通过以下方程组计算获得：
[0032][0033]
进一步的，扩张状态观测器的形式为：
[0034][0035]
其中，fal函数为：
[0036][0037]
或者fal函数为线性函数，并构成线性扩张状态观测器。
[0038]
进一步的，当系统输出y受到噪声影响，则先对y进行滤波，将滤波后的输出代入到扩张状态观测器中。
[0039]
进一步的，输出控制量为：
[0040][0041]
与现有技术相比，本发明有以下优点：
附图说明
[0042]
图1是本发明的双轮自平衡机器人硬件框图；
[0043]
图2是本发明的双轮自平衡机器人程序流程图；
[0044]
图3是本发明的双轮自平衡机器人路速度倾角自抗扰控制框图；
[0045]
图4是本发明的双轮自平衡机器人路径跟踪运动转向控制框图。
具体实施方式
[0046]
下面结合附图对本发明作进一步说明。
[0047]
如图1所示，一种基于自抗扰的双轮自平衡机器人运动控制系统，包括
[0048]
底盘，为安装平台，传感器、电路板通过连接件安装在底盘上；
[0049]
驱动轮，独立设置，分布在底盘上，并通过直流有刷电机驱动；
[0050]
惯性传感器，接收双轮自平衡机器人的加速度数据以及角速度数据；
[0051]
微控制器，用于控制电机，结合软件程序获取机器人俯仰角，再通过微控制器实现机器人自平衡；
[0052]
增量式编码器，用于获取机器人的驱动轮转速，并对机器人的运动速度进行闭环控制，以实现固定轨迹上的自动路径跟踪运动。
[0053]
底盘上还设置有lc电磁传感器、tof距离传感器、摄像头、显示屏以及上位机，微控制器包括控制模块mcu，电机、惯性传感器、lc电磁传感器、tof距离传感器、摄像头、显示屏以及上位机分别与控制模块mcu信号连接。
[0054]
具体来说双轮自平衡机器人主要由底盘、电路板及传感器等部分组成。底盘是双轮自平衡机器人的各个组成部分的安装平台，传感器、电路板等都通过一定的连接件和紧固件安装在底盘上面。双轮自平衡机器人共有两个独立的驱动轮，每个驱动轮各由一个直流有刷电机驱动，电机轴和和轮子之间通过一对减速齿轮连接，既可以使轮子的转速降低到一个合理的区间，又提高了扭矩。基于上述的硬件系统，开发相应的软件程序，使自主设计的双轮自平衡机器人能够融合惯性传感器数据获得机器人俯仰角，微控制器控制电机实现机器人的自平衡。通过增量式编码器获取机器人的驱动轮的转速，对机器人的运动速度进行闭环控制，实现在固定轨迹上的自动路径跟踪运动。
[0055]
结合图2-4，惯性传感器中加速度计测量姿态角，陀螺仪测量角位移和基于卡尔曼滤波的陀螺仪与加速度计数据融合。
[0056]
加速度计可以测得传感器三个坐标轴上的加速度，当三轴加速度计静止时，三个轴上的加速度矢量和等于重力加速度。通过对三个轴上的加速度进行一定的三角函数计算，即可得到双轮自平衡机器人的俯仰角(pitch)和横滚角(roll)数据。
[0057]
双轮自平衡机器人本体坐标系可以看作是由地面参考坐标系依次绕z-x-y轴旋转得到的，记绕绕z、x和y轴旋转的角度分别为：θz、θ
x
和θy，对应的旋转矩阵分别为rz、r
x
和ry，则：
[0058][0059]
记关于任意轴的旋转矩阵为r，r＝r
yrxrz
，则：
[0060]
本体坐标系下的加速度为[a
x
,ay,az]
t
，在地面参考系下双轮自平衡机器人静止时的受到的加速度为[0,0,g]
t
，g是重力加速度，则可得到以下关系式：
[0061][0062]
解该矩阵方程，便可得到双轮自平衡机器人的姿态角：
[0063][0064][0065]
使用加速度计测量双轮自平衡机器人的姿态角不会引入累积误差，然而，由于双
轮自平衡机器人运动过程中动态加速度、机体震动已经受到冲击等因素都会对加速度传感产生很大的影响，使加速度传感器输出的原始数据包含大量的噪声。因此，单独使用加速度计不能满足双轮自平衡机器人动态角度测量的要求。
[0066]
陀螺仪通常用于测量物体的角速度、角位移等信息，对敏感轴的角速度积分即可得到该轴的角位移信息，在离散系统中，可以通过公式2-10获得双轮自平衡机器人的某个轴的角位移：
[0067]
θk＝θ
k-1
+ωkdt
[0068]
其中：θk为当前时刻的角位移，θ
k-1
为k-1积分获得的角位移，ωk为当前时刻的角速度，dt为积分步长，即角位移测量系统中对角速度的采样周期。
[0069]
对陀螺仪角速度进行离散积分，得到陀螺仪的角位移，通常采样频率越高，获得的角位移数据越准确。陀螺仪输出的角速度原始数据噪声不大，所以，在短时间内通过积分获得的角位移数据较为精准可靠。然而除了离散积分带来误差外，陀螺仪会受到温漂、零飘、线性度误差、震动等因素的干扰，对角速度积分造成的累积误差会越来越大，因此不适合单独用来长时间测量角位移。
[0070]
根据所述的加速度计、陀螺仪的特性和测量角度的原理可知，加速度计和陀螺仪在多个方面都有互补性，主要是：加速度计测量角度没有累积误差，但运动中容易受到干扰，测量数据噪声大；陀螺仪数据稳定，噪声小，但累积误差无法克服。因此，本明基于卡尔曼滤波对二者数据进行融合，力求获得较好的角度测量效果。
[0071]
卡尔曼滤波算法主要分为预测和修正两个部分。在动态离散过程中，在预测阶段，根据上一个时刻的状态变量的值来预测当前时刻的状态变量并估计协方差。在修正阶段，根据估计的不确定性和测量的不确定性对估计量和观测量加权得到状态变量的最优估计，并修正当前时刻的状态向量协方差以在下一个时刻预测新的状态变量。这两个步骤不断迭代，可以获得非常接近于真实数据的最优估计。
[0072]
卡尔曼滤波中线性模型的状态方程和观测方程分别可表示为：
[0073]
xk＝ax
k-1
+buk+wk[0074]
zk＝hxk+vk[0075]
式中：xk为系统在k时刻的状态变量，uk是系统控制量，zk是测量值，a、b、h都是系统参数，wk和vk分别是过程高斯噪声和观测高斯噪声。
[0076]
卡尔曼滤波主要可以分为五个步骤，对陀螺仪与加速度计数据融合的姿态解算每个步骤的具体内容如下：
[0077]
预测阶段：
[0078]
1)估计当前状态向量：
[0079]
系统的状态变量为陀螺仪俯仰角α和陀螺仪y轴漂移β。根据系统模型，可以列出该系统的状态估计方程：
[0080][0081]
式中：dt为采样周期，α
k|k-1
、β
k|k-1
分别为k时刻的俯仰角估计和角速度漂移估计，α
k-1|k-1
、β
k-1|k-1
分别为k-1时刻的最优俯仰角估计和最优角速度漂移估计，uk为k时刻陀螺仪输出的角速度。
[0082]
可将式方程写成如下形式：
[0083]
x
k|k-1
＝ax
k-1|k-1
+buk[0084]
2)估计当前时刻协方差：
[0085]
p
k|k-1
＝ap
k-1|k-1at
+q
[0086]
式中：p
k|k-1
、p
k-1|k-1
分别为k时刻预测的协方差和k-1时刻最优估计协方差。q是过程噪声的协方差矩阵。α、β的预测值还会各自受到噪声的干扰，将这两种噪声都看作均值为0高斯噪声，但互相独立，和分别是它们的高斯分布的方差，调节它们的大小使假设的高斯噪声与实际的噪声分布更相近，从而更好的抑制噪声的影响。
[0087]
修正阶段：
[0088]
3)计算卡尔曼增益：
[0089]kk
＝p
k|k-1ht
(hp
k|k-1ht
+r)-1
[0090]
式中：kk为k时刻卡尔曼滤波增益，在该系统中是一个二维列向量。h在该系统中为[1,0]，r为测量噪声的协方差矩阵，反映服从均值为0的高斯分布的测量噪声分布情况，设测量噪声的方差为δ
z2
，调节它的大小同样也可以调节系统对测量噪声的抑制能力。
[0091]
4)修正状态估计：
[0092]
根据计算出的卡尔曼增益，对估计值和测量值取不同的权重，加权和就是状态变量的最优估计，并修正预测阶段对状态变量的估计。状态向量的最优估计计算公式如下所示：
[0093]
x
k|k
＝x
k|k-1
+kk(z
k-hx
k|k-1
)
[0094]
5)修正当前时刻最优状态估计的协方差：
[0095]
最后，更新当前时刻状态向量的最优估计x
k|k
的协方差，公式如下所示：
[0096][0097]
p
k|k
＝(i-kkh)p
k|k-1
[0098]
按照上述五个步骤，就可以完成一次卡尔曼滤波的迭代，不断重复上述步骤，就可对双轮自平衡机器人的姿态角进行解算和滤波。
[0099]
包括基于误差反馈控制的自抗扰控制模型对整体速度与转向控制系统的优化设计。
[0100]
自抗扰控制器主要由跟踪微分器、扩张状态观测器和非线性误差反馈控制律这三个部分组成。跟踪微分器对输入信号进行跟踪并提取其微分信号，起到安排过渡过程和滤波的作用。扩张状态观测器能够根据系统的输入和输出确定系统内部的状态变量，并且能够对扰动进行实时估计与补偿。非线性误差反馈控制律的核心是基于误差反馈控制，对跟踪信号误差、微分信号误差进行非线性组合。
[0101]
微分跟踪器对输入信号进行跟踪，并提取有效的微分信号，起着安排过渡过程和滤波的作用。在二阶自抗扰控制系统中，一般使用最速跟踪微分器来对输入信号进行跟踪和滤波，它是一种非线性跟踪微分器，其离散表达方程组为：
[0102]
[0103]
上述方程组中，v(k)为k时刻输入信号的大小，v1(k)和v1(k+1)分别为k时刻和k+1时刻最速跟踪微分器输出的跟踪信号，v2(k)和v2(k+1)分别为k时刻和k+1时刻最速跟踪微分器对输入信号的微分，h为积分步长，等于控制器的控制周期，r0为该最速微分跟踪器的的速度因子，决定了跟踪输入信号的快慢，h0为滤波因子，值越大滤波效果越好。fhan(v1,v2,r,h)函数为综合最优控制函数，函数的输出由以下方程组计算得到：
[0104][0105]
其中：sign()为符号函数，输入参数为正数时返回1，为负时返回-1。
[0106]
二阶自抗扰控制器的典型非线性扩张状态观测器的形式如下：
[0107][0108]
其中：z1、z2、z3是扩张状态观测器对系统状态变量x1、x2及a(t)的跟踪。根据上文可知z3是对系统总扰动的估计，动态根据估计的扰动对系统进行补偿，可以极大改善系统的抗扰能力。y是系统的输出，β
01
、β
02
和β
03
是状态观测器的增益，u是自抗扰控制器输出的作用量，b是系统输入增益，其取值也可以近似和b0取一样的值。fal函数是一个非线性函数，其定义如式所示。
[0109][0110]
需要补充说明的是，fal函数也可以用其他函数代替，也可以是线性函数，若是用线性函数代替，就构成线性扩张状态观测器。此外，如果系统的输出y受到噪声影响，可以先对y进行滤波，将滤波后的输出代入到上文的扩张状态观测器中即可。
[0111]
对于二阶自抗扰控制器，其输出的控制量的表达式如下：
[0112][0113]
其中：c是阻尼系数，r1、h1是可调参数，分别对它们适当整定，就可以获得较好的控制效果。相对于经典pid的线性误差反馈控制，自抗扰控制的非线性误差反馈控制律在快速性、稳态误差等方面都有更好的表现。
[0114]
具体来说，本发明通过一个速度-倾角串级控制器来实现双轮自平衡机器人的速度和倾角的双闭环控制。提出的串级控制器的外环为速度环，内环是角度环，速度环的输出作为角度环的输入，角度环动态改变机器人的俯仰角，保持平衡的同时还对机器人的速度进行控制。
[0115]
将自抗扰控制器应用到速度-直立串级控制中，构成一个串级自抗扰控制器。机器人做路径跟踪运动时，一般是匀速运动，即速度环的输入是一个常数。当双轮自平衡机器人在静止时开始运动时，速度环输入信号是一个阶跃信号，会对系统产生很大的冲击。因此，速度环的跟踪微分器的主要作用是安排过渡过程，避免输入信号对系统产生较大的冲击。速度环扩张状态观测器不仅能对速度的稳态误差做出补偿，还能观测双轮自平衡机器人出现打滑、轮子腾空空转等情况对速度环带来的扰动并实时做出补偿，使速度环的抗扰能力得到很大提升。速度环的输出经过限幅得到参考倾角，作为内环的输入，限幅的作用是使参考倾角不至于过大，以防超出角度控制器的控制能力。
[0116]
由于速度环的输出会含有一定的噪声，而跟踪微分器本身就有很好的滤波功能，因此，不需要再增加额外的滤波器，只需要调整角度环的跟踪微分器的滤波因子就能够实现对参考角度信号的滤波。不难看出角度环是一个随动控制器，它应该尽可能快的跟踪输入信号而对超调量、稳态误差等方面的性能要求不高。双轮自平衡机器人路径跟踪运动对转向控制器的快速性和超调量有着较高的要求。转向控制器既要能够灵敏地控制双轮自平衡机器人转向，使机器人沿着弯曲的路径上快速行驶，还要使转向的超调量尽可能的小，以减少过度转向带来的不利影响。
[0117]
自抗扰控制器的扩张状态观测器能够估计系统的受到的扰动并实时做出补偿。实际测试发现本文设计的基于自抗扰控制的速度-角度串级控制器和转向控制器都具有良好的抗扰能力，使双轮自平衡机器人的稳定性和可靠性得到提高。此外，本文设计的自抗扰控制器采用的是非线性误差反馈控制律，在设计的运动控制系统中，俯仰角度环和转向角速度环都采用了自抗扰控制器，响应速度快，超调量小，能够很好的跟踪外环的输出信号，提高了控制系统的响应速度。

技术特征：
1.一种基于自抗扰的双轮自平衡机器人运动控制系统，其特征在于包括底盘，为安装平台，传感器、电路板通过连接件安装在所述底盘上；驱动轮，独立设置，分布在所述底盘上，并通过直流有刷电机驱动；惯性传感器，接收双轮自平衡机器人的加速度数据以及角速度数据；微控制器，用于控制电机，结合软件程序获取机器人俯仰角，再通过所述微控制器实现机器人自平衡；增量式编码器，用于获取机器人的驱动轮转速，并对机器人的运动速度进行闭环控制，以实现固定轨迹上的自动路径跟踪运动。2.根据权利要求1所述的一种基于自抗扰的双轮自平衡机器人运动控制系统，其特征在于所述底盘上还设置有lc电磁传感器、tof距离传感器、摄像头、显示屏以及上位机，所述微控制器包括控制模块mcu，电机、惯性传感器、lc电磁传感器、tof距离传感器、摄像头、显示屏以及上位机分别与所述控制模块mcu信号连接。3.一种基于自抗扰的双轮自平衡机器人运动控制方法，其特征在于包括以下步骤：s1初始化程序，初始化成功后启动主程序；s11初始化失败，则重新进行初始化程序，直至初始化成功；s2判断主程序是否进入中断，若进入中断则通过控制系统进行控制；s3若主程序未进入中断，则判断机器人是否发生故障，发生故障则停止机器人运行；s31未发生故障，则保持主程序运行。4.根据权利要求3所述的一种基于自抗扰的双轮自平衡机器人运动控制方法，其特征在于所述控制系统包括以下步骤：s21对速度进行采集，并进行速度控制；s22对姿态进行解算，并进行平衡控制；s23对路径进行提取，并进行转向控制；s24根据触发主程序进入中断状态的条件，分别进行步骤s21-s23，调节控制后主程序重新运行。5.根据权利要求3或4所述的一种基于自抗扰的双轮自平衡机器人运动控制方法，其特征在于所述主程序包括数据发送、硬件自检以及人机交互，通过传感器获取数据信息，并发送至上位机中。6.根据权利要求5所述的一种基于抗自扰的双轮自平衡机器人运动控制方法，其特征在于在主程序运行过程中，通过自抗扰控制模型对机器人的整体速度和转向进行控制，所述自抗扰控制器包括跟踪微分器，对输入信号进行跟踪并提取其微分信号，安排过度过程以及滤波；扩张状态观测器，根据系统输入和输出确定系统内部状态变量，并对扰动进行实时估计和补充；非线性误差反馈控制律，基于误差反馈控制，对跟踪信号误差、微分信号误差进行非线性组合。7.根据权利要求6所述的一种基于抗自扰的双轮自平衡机器人运动控制方法，其特征在于所述跟踪微分器的离散表达方程组为：
其中fhan(v1,v2,r,h)函数为综合最优控制函数，函数的输出通过以下方程组计算获得：8.根据权利要求6所述的一种基于抗自扰的双轮自平衡机器人运动控制方法，其特征在于扩张状态观测器的形式为：其中，fal函数为：或者fal函数为线性函数，并构成线性扩张状态观测器。9.根据权利要求8所述的一种基于抗自扰的双轮自平衡机器人运动控制方法，其特征在于当系统输出y受到噪声影响，则先对y进行滤波，将滤波后的输出代入到扩张状态观测器中。10.根据权利要求6所述的一种基于抗自扰的双轮自平衡机器人运动控制方法，其特征在于输出控制量为：

技术总结
本发明公开一种基于自抗扰的双轮自平衡机器人运动控制系统及控制方法，包括以下步骤：初始化程序，初始化成功后启动主程序；初始化失败，则重新进行初始化程序，直至初始化成功；判断主程序是否进入中断，若进入中断则通过控制系统进行控制；若主程序未进入中断，则判断机器人是否发生故障，发生故障则停止机器人运行；未发生故障，则保持主程序运行。自抗扰控制器的扩张状态观测器能够估计系统的受到的扰动并实时做出补偿。实际测试发现本文设计的基于自抗扰控制的速度-角度串级控制器和转向控制器都具有良好的抗扰能力，使双轮自平衡机器人的稳定性和可靠性得到提高。机器人的稳定性和可靠性得到提高。机器人的稳定性和可靠性得到提高。


技术研发人员：裘信国 沈彤 郑颖 王志立 黎昌龙 季行健 王晨
受保护的技术使用者：浙江工业大学
技术研发日：2023.05.31
技术公布日：2023/9/14