专利名称：一种轮式倒立摆的传感器处理与平衡控制算法的制作方法
技术领域：
本发明属于自动控制技术领域，涉及一种轮式倒立摆的传感器处理与平衡控制算 法。
背景技术：
轮式倒立摆的控制方法，一直是国内外研究的热点问题。1988年，日本专利说明书JP63-305082公开了一种只有左右两轮、能自动保持动 态平衡的小车。该申请只是一种能演示一级倒立摆的实验方案，离实际使用在技术上尚有 一段较远的距离。2004年，国内专利说明书CN1557668A公开了一种可以用于骑行的两轮自动平衡 小车，其核心控制输出为轮子的转矩。2007年，国内专利说明书CN1952996A公开了一种遥控型两轮自平衡小车，其平衡 算法的控制输出仍然是轮子的转矩。由于不规则地面情况下，轮子的负载有较大变化，因此，单独控制轮子的转矩，将 不能有效地对车体的平衡进行控制。因此，本发明提出了直接对车体的加速度进行控制的 方法。上述各专利中，很少提到传感器的融合方法以及对车体在控制策略上的保护上， 由于两轮自平衡小车本身就是一个不稳定系统，实际应用中，不可避免地要遇到一些意外， 因此本发明从控制策略上提出了一些保护机制，有利于增强整个系统的稳定性，有利于保 护所述轮式倒立摆上的设备与人员。
发明内容
本发明的目的是提供一种轮式倒立摆的传感器处理与平衡控制算法，它能提高倒 立摆的工作的稳定性和可靠性，在保持倒立摆平衡的情况下，做了较多的保护措施，使得整 个倒立摆系统在复杂的外部环境中运行的更加稳定可靠。为了解决背景技术所存在的问题，本发明是采用以下技术方案它包括传感器处 理算法，平衡控制算法，过速度保护算法，应急处理算法。所述传感器处理算法，包括加速度传感器与陀螺仪数据融合算法以及滤波算法。加速度传感器的输出是当前物体的加速度，当倒立摆静止时，其只收到重力加速 度的影响，因此可以在倒立摆静止时通过公式9 =arCSin(aCC)计算出当前倒立摆的偏转 角；但是，当倒立摆收到外力作用(如轮子)而具有X方向的加速度时，上述公式就不能计 算出当前的偏转角度，因此只使用加速度传感器来测量当前倒立摆的角度时不能满足要求 的，但是其主要特点是低频效果较为理想。陀螺仪的输出是倒立摆偏转的角速度信息，而对角速度值进行积分，即可得到当 前的角度值。但是由于零位误差和积分误差的影响，导致使用陀螺仪的积分值所获得的角 度存在漂移问题。随着时间的推移，漂移角度的扩大，最终导致所测量的角度误差无法接受，但其高频效果比较理想。因此单独使用陀螺仪或者加速度传感器无法得到较为准确的角度值，因此需要有 效结合两者的优点，对两者进行融合即可。所述加速度传感器与陀螺仪的数据融合算法，旨在利用加速度传感器和陀螺仪的 输出通过一定的融合手段，用以测量出当前倒立摆的偏转角。本算法的特征在于利用加速 度传感器的低频特性，和陀螺仪输出的高频特性，用以得到一个在动态情况下较为稳定的 倒立摆偏转角。所述平衡控制算法，通过使用当前倒立摆的偏转角与倒立摆的偏转角速度，来控 制当前车体的加速度和角度。控制过程的输入参数是当其车体运动的速度，当其车体的偏转角，以及偏转角速 度。输出为车体的加速度，计算公式如下Speed _ acc = kl* angle + k2* anglerate + A:3* | angle其中SpeecLacc为轮子的输出加速度，angle为车体偏转角度，anglerate是偏转 的角速度，kl，k2，k3是相关参数，其具体的大小与实际的物理参数及其他因素。使用平衡 控制算法得到加速度之后，由相应的电机控制器来进行执行。所述的过速度保护算法，即当速度高于一定速度阈值时，该算法会输出一个与速 度有关的角度偏移量。该偏移量将附加在所述传感器融合算法所计算出来的角度上，用于 整个车体的减速，当速度减小到一定阈值时，将逐渐撤去该偏移量，使得系统恢复正常。所述的应急处理算法是在紧急情况下，如车体出现故障时，系统将自动启动应急 处理算法，该算法的实质与过速度保护算法的实质相同，不同之处在于，其速度阈值为0附 近，控制目标是将车体的速度迅速降下来。以保证车载设备的安全。本发明能提高倒立摆的工作的稳定性和可靠性，在保持倒立摆平衡的情况下，做 了较多的保护措施，使得整个倒立摆系统在复杂的外部环境中运行的更加稳定可靠。
图1是本发明轮式倒立摆的结构示意图；图2是本发明平衡控制算法的示意图。
具体实施例方式参看图1-2，本具体事实方式采用以下技术方案它是由摆杆1、带控制器的驱动 电机2、轮子3、控制器和传感器4、测速装置5组成；摆杆1为倒立摆杆，也可以认为是整个 倒立摆小车的负载；控制器和传感器4通过读取传感器的信号进行融合计算之后生成速度 控制量发送到带控制器的驱动电机2中，带控制器的驱动电机2根据收到的控制量通过读 取测速装置5所输出的速度信息进行反馈控制驱动电机，电机通过传动装置驱动轮子3从 而最终控制倒立摆的运动。控制器使用ST公司的C0MTEX-M3内核的处理器，传感器使用ADI公司的陀螺仪和 加速度传感器。电机的速度使用编码器测量。传感器的输出通过控制器内部ADC读取后转换为数字量。陀螺仪的输出为 anglerate，加速度传感器的输出为acc，anglerate通过高通滤波器之后值为anglerate_f，acc通过低通滤波器之后值为acc_f，则最终结合两个传感器的输出融合的角度值为 控制器将融合的角度值angle，以及陀螺仪输出的角速度值anglerate，使用如下计算出轮子的输出加速度SpeecLacc，然后将加速度输入到电机控制器，控制器执 行加速度命令。当倒立摆运行速度较大时，将导致电机输出达到功率上限，从而影响最终的平衡 控制，因此，需要对倒立摆的运动速度进行限制。本实例使用电机的输入电压为门限，当输 入电压值快要达到最大时，开启过速度保护过程。使用如下公式计算出角度偏移量angle_offset = k氺(CurVoltage-ThresholdVoltage)式中，anglejffset是角度偏移量，用来控制当前车体的角度偏移。CurVoltage 为当前电压，ThresholdVoltage为门限电压，k为比例系数。通过上述公式，即可以得到当前倒立摆所需要的偏移角度，从而控制倒立摆减速。当倒立摆检测出故障时，需要尽快停下来，此时，需要使用ThresholdVoltage为0 过速度保护过程，可以让车体尽快停车，以达到保护效果。
angle = anglerate_f*dt+arcsin(acc_f)。
公式 Speed — acc 二籴1 * angle + k2* anglerate + k3
权利要求
一种轮式倒立摆的传感器处理与平衡控制算法，其特征在于它包括传感器处理算法，平衡控制算法，过速度保护算法，应急处理算法。
2.根据权利要求1所述的一种轮式倒立摆的传感器处理与平衡控制算法，其特征在于 所述的所述传感器处理算法，包括加速度传感器与陀螺仪数据融合算法以及滤波算法；力口 速度传感器与陀螺仪的数据融合算法，旨在利用加速度传感器和陀螺仪的输出通过一定的 融合手段，用以测量出当前倒立摆的偏转角；本算法的特征在于利用加速度传感器的低 频特性，和陀螺仪输出的高频特性，用以得到一个在动态情况下较为稳定的倒立摆偏转角。
3.根据权利要求1所述的一种轮式倒立摆的传感器处理与平衡控制算法，其特征在于 所述平衡控制算法，通过使用当前倒立摆的偏转角与倒立摆的偏转角速度，来控制当前车 体的加速度和角度；控制过程的输入参数是当其车体运动的速度，当其车体的偏转角，以及 偏转角速度；输出为车体的加速度。
4.根据权利要求1所述的一种轮式倒立摆的传感器处理与平衡控制算法，其特征在于 所述过速度保护算法，即当速度高于一定速度阈值时，该算法会输出一个与速度有关的角 度偏移量；该偏移量将附加在所述传感器融合算法所计算出来的角度上，用于整个车体的 减速，当速度减小到一定阈值时，将逐渐撤去该偏移量，使得系统恢复正常。
5.根据权利要求1所述的一种轮式倒立摆的传感器处理与平衡控制算法，其特征在于 所述的应急处理算法是在紧急情况下，如车体出现故障时，系统将自动启动应急处理算法， 该算法的实质与过速度保护算法的实质相同，不同之处在于，其速度阈值为O附近，控制目 标是将车体的速度迅速降下来，以保证车载设备的安全。
全文摘要
一种轮式倒立摆的传感器处理与平衡控制算法，它涉及自动控制技术领域。包括传感器处理算法，平衡控制算法，过速度保护算法，应急处理算法；轮式倒立摆由摆杆(1)、带控制器的驱动电机(2)、轮子(3)、控制器和传感器(4)、测速装置(5)组成；能提高倒立摆的工作的稳定性和可靠性，在保持倒立摆平衡的情况下，做了较多的保护措施，使得整个倒立摆系统在复杂的外部环境中运行的更加稳定可靠。
文档编号B60W30/04GK101823485SQ20101013214
公开日2010年9月8日 申请日期2010年3月25日 优先权日2010年3月25日
发明者周伟, 李一鹏, 林敬桥, 郭盖华, 闫学凯, 陈志发 申请人:武汉若比特机器人有限公司