一种轮式倒立摆的传感器处理与平衡控制算法

摘要

一种轮式倒立摆的传感器处理与平衡控制算法，它涉及自动控制技术领域。包括传感器处理算法，平衡控制算法，过速度保护算法，应急处理算法；轮式倒立摆由摆杆(1)、带控制器的驱动电机(2)、轮子(3)、控制器和传感器(4)、测速装置(5)组成；能提高倒立摆的工作的稳定性和可靠性，在保持倒立摆平衡的情况下，做了较多的保护措施，使得整个倒立摆系统在复杂的外部环境中运行的更加稳定可靠。

说明书

技术领域：

本发明属于自动控制技术领域，涉及一种轮式倒立摆的传感器处理与平衡控制算法。

背景技术：

轮式倒立摆的控制方法，一直是国内外研究的热点问题。

1988年，日本专利说明书JP63-305082公开了一种只有左右两轮、能自动保持动态平衡的小车。该申请只是一种能演示一级倒立摆的实验方案，离实际使用在技术上尚有一段较远的距离。

2004年，国内专利说明书CN1557668A公开了一种可以用于骑行的两轮自动平衡小车，其核心控制输出为轮子的转矩。

2007年，国内专利说明书CN1952996A公开了一种遥控型两轮自平衡小车，其平衡算法的控制输出仍然是轮子的转矩。

由于不规则地面情况下，轮子的负载有较大变化，因此，单独控制轮子的转矩，将不能有效地对车体的平衡进行控制。因此，本发明提出了直接对车体的加速度进行控制的方法。

上述各专利中，很少提到传感器的融合方法以及对车体在控制策略上的保护上，由于两轮自平衡小车本身就是一个不稳定系统，实际应用中，不可避免地要遇到一些意外，因此本发明从控制策略上提出了一些保护机制，有利于增强整个系统的稳定性，有利于保护所述轮式倒立摆上的设备与人员。

发明内容：

本发明的目的是提供一种轮式倒立摆的传感器处理与平衡控制算法，它能提高倒立摆的工作的稳定性和可靠性，在保持倒立摆平衡的情况下，做了较多的保护措施，使得整个倒立摆系统在复杂的外部环境中运行的更加稳定可靠。

为了解决背景技术所存在的问题，本发明是采用以下技术方案：它包括传感器处理算法，平衡控制算法，过速度保护算法，应急处理算法。

所述传感器处理算法，包括加速度传感器与陀螺仪数据融合算法以及滤波算法。

加速度传感器的输出是当前物体的加速度，当倒立摆静止时，其只收到重力加速度的影响，因此可以在倒立摆静止时通过公式θ＝arcsin(acc)计算出当前倒立摆的偏转角；但是，当倒立摆收到外力作用(如轮子)而具有X方向的加速度时，上述公式就不能计算出当前的偏转角度，因此只使用加速度传感器来测量当前倒立摆的角度时不能满足要求的，但是其主要特点是低频效果较为理想。

陀螺仪的输出是倒立摆偏转的角速度信息，而对角速度值进行积分，即可得到当前的角度值。但是由于零位误差和积分误差的影响，导致使用陀螺仪的积分值所获得的角度存在漂移问题。随着时间的推移，漂移角度的扩大，最终导致所测量的角度误差无法接受，但其高频效果比较理想。

因此单独使用陀螺仪或者加速度传感器无法得到较为准确的角度值，因此需要有效结合两者的优点，对两者进行融合即可。

所述加速度传感器与陀螺仪的数据融合算法，旨在利用加速度传感器和陀螺仪的输出通过一定的融合手段，用以测量出当前倒立摆的偏转角。本算法的特征在于：利用加速度传感器的低频特性，和陀螺仪输出的高频特性，用以得到一个在动态情况下较为稳定的倒立摆偏转角。

所述平衡控制算法，通过使用当前倒立摆的偏转角与倒立摆的偏转角速度，来控制当前车体的加速度和角度。

控制过程的输入参数是当其车体运动的速度，当其车体的偏转角，以及偏转角速度。输出为车体的加速度，计算公式如下：

$>\mathrm{Speed}\_\mathrm{acc}=k1*\mathrm{angle}+k2*\mathrm{anglerate}+k3*{\int }_0^t\mathrm{angle}$>

其中Speed_acc为轮子的输出加速度，angle为车体偏转角度，anglerate是偏转的角速度，k1，k2，k3是相关参数，其具体的大小与实际的物理参数及其他因素。使用平衡控制算法得到加速度之后，由相应的电机控制器来进行执行。

所述的过速度保护算法，即当速度高于一定速度阈值时，该算法会输出一个与速度有关的角度偏移量。该偏移量将附加在所述传感器融合算法所计算出来的角度上，用于整个车体的减速，当速度减小到一定阈值时，将逐渐撤去该偏移量，使得系统恢复正常。

所述的应急处理算法是在紧急情况下，如车体出现故障时，系统将自动启动应急处理算法，该算法的实质与过速度保护算法的实质相同，不同之处在于，其速度阈值为0附近，控制目标是将车体的速度迅速降下来。以保证车载设备的安全。

本发明能提高倒立摆的工作的稳定性和可靠性，在保持倒立摆平衡的情况下，做了较多的保护措施，使得整个倒立摆系统在复杂的外部环境中运行的更加稳定可靠。

附图说明：

图1是本发明轮式倒立摆的结构示意图；

图2是本发明平衡控制算法的示意图。

具体实施方式：

参看图1-2，本具体事实方式采用以下技术方案：它是由摆杆1、带控制器的驱动电机2、轮子3、控制器和传感器4、测速装置5组成；摆杆1为倒立摆杆，也可以认为是整个倒立摆小车的负载；控制器和传感器4通过读取传感器的信号进行融合计算之后生成速度控制量发送到带控制器的驱动电机2中，带控制器的驱动电机2根据收到的控制量通过读取测速装置5所输出的速度信息进行反馈控制驱动电机，电机通过传动装置驱动轮子3从而最终控制倒立摆的运动。

控制器使用ST公司的COMTEX-M3内核的处理器，传感器使用ADI公司的陀螺仪和加速度传感器。电机的速度使用编码器测量。

传感器的输出通过控制器内部ADC读取后转换为数字量。陀螺仪的输出为anglerate，加速度传感器的输出为acc，anglerate通过高通滤波器之后值为anglerate_f，acc通过低通滤波器之后值为acc_f，则最终结合两个传感器的输出融合的角度值为angle＝anglerate_f*dt+arcsin(acc_f)。

控制器将融合的角度值angle，以及陀螺仪输出的角速度值anglerate，使用如下公式：

$>\mathrm{Speed}\_\mathrm{acc}=k1*\mathrm{angle}+k2*\mathrm{anglerate}+k3*{\int }_0^t\mathrm{angle}$>

计算出轮子的输出加速度Speed_acc，然后将加速度输入到电机控制器，控制器执行加速度命令。

当倒立摆运行速度较大时，将导致电机输出达到功率上限，从而影响最终的平衡控制，因此，需要对倒立摆的运动速度进行限制。本实例使用电机的输入电压为门限，当输入电压值快要达到最大时，开启过速度保护过程。使用如下公式计算出角度偏移量：

angle_offset＝k*(CurVoltage-ThresholdVoltage)

式中，angle_offset是角度偏移量，用来控制当前车体的角度偏移。CurVoltage为当前电压，ThresholdVoltage为门限电压，k为比例系数。

通过上述公式，即可以得到当前倒立摆所需要的偏移角度，从而控制倒立摆减速。

当倒立摆检测出故障时，需要尽快停下来，此时，需要使用ThresholdVoltage为0过速度保护过程，可以让车体尽快停车，以达到保护效果。