一种机器人运动障碍的判断方法
技术领域
本发明属于机器人运动的路障判断的技术领域,尤其涉及一种机器人运动障碍的判断方法。
背景技术
擦窗机器人主要是凭借自身底部的真空泵或者风机装置,牢牢地吸附在玻璃上的;擦窗机器人借助一定的人工智能,自动探测窗户的边角距离、规划擦窗路径(从左至右,或从下至上的擦拭顺序),并在清洁完毕之后回到初始的放置位置方便人们将其取下。
擦窗机器人在行走时,需要解决障碍的判断处理,障碍包括边框或窗把手等。常用做法是在擦窗机器人的边框加装多个红外、机械等开关,通过检测开关的闭合状态来判断是否遇到障碍,算法非常简单。
虽然采用红外或机械开关的算法非常简单,但需要加装多个开关,边框也需要做成浮动式的,造成结构复杂。此外,如果擦窗机器人进入某些狭窄区域,可能被卡住,这时如果没有碰到开关,就会造成“死机”。
故有必要设计一种新的方法。
发明内容
本发明的目的在提供一种采用两级级滤波算法和尖峰的滤波算法的机器人运动障碍的判断方法。
本发明提供一种机器人运动障碍的判断方法,包括如下步骤:
S1:采集驱动机器人运动的电机的模数转换电路的多组电流数据值;
S2:计算步骤S1的“多组电流数据值”的电流平均值;
S3:计算大于电流平均值的数量;
S4:如果比电流平均值大的数量小于预先设定的数量,将大于电流平均值的电流数据值改成电流平均值,重新计算电流平均值;
S5:初始化历史电流平均值,将当前电流平均值存入历史电流平均值;
S6:计算“历史电流平均值的平均值”;
S7:如果当前电流平均值大于预先设定值且当前电流平均值和“历史电流平均值的平均值”之差大于预先设定的差值的最小值,则推断遇到了障碍,为避免重复判断,将原来的电流平均值全部替换成新的平均值;否则判断未遇到障碍且回到步骤S1重新进行判断;否则判断未遇到障碍且回到步骤S1重新进行判断。
进一步地,步骤S1采用DMA方式读取电机的模数转换电路的电流数据。
进一步地,步骤S2中采用算数方法计算“电流数据”的电流平均值。
进一步地,步骤S2中采用去掉 “电流数据值”的最大值和去掉 “电流数据值”的最小值后再计算电流平均值。
进一步地,步骤S4中的“将大于电流平均值的电流数据值改成电流平均值,重新计算电流平均值”的意思是:大于电流平均值的电流数据值改成电流平均值为a,a和小于电流平均值的电路数据值一起计算电流平均值。
进一步地,对于步骤S4,如果比电流平均值大的数量等于或大于预先设定的数量,则步骤S5为“保存当前电流平均值作为历史电流平均值”。
进一步地,其中步骤S7中,如果当前电流平均值大于预先设定值,代表可能遇到障碍。
进一步地,步骤S7中,“如果当前电流平均值大于预先设定值且比“历史电流平均值的平均值”大某给定值”,代表电流值“显著上升”。
进一步地,所述步骤S1采用了避开浮点运算的算法.
进一步地,步骤S1采集的电流数据量以及步骤S5的历史电流平均值的数量都是2的N次方。
本发明采用多级滤波算法,通过对电流变化进行分析,来判断擦窗机器人是否遇到障碍;本发明虽然算法复杂,且需要较多的实验确定相关参数,但是因为不需要任何检测开关,简化了机器人的整体结构,大量减少了布线,从而大大降低整体故障率。
附图说明
图1是本发明基于电流变化的多级滤波障碍判断方法的流程示意图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白,以下结合附图及实施例,对本发明一种智能跟随运动装置作进一步详细说明。应当理解,此处所描述的具体实施例仅用以解释本发明,并不用于限定本发明。
本发明一种机器人运动障碍的判断方法,其用于擦窗机器人,也可以用于其他可以行走的机器人。
擦窗机器人包括驱动器运动的两个履带驱动电机和控制自动擦窗的风机电机,当擦窗机器人在擦窗过程中遇到障碍时,会造成驱动电机的负载增加,导致驱动电机的电流显著增加,通过读取电流的数据,采用多级滤波算法过滤各种干扰,分析电流数据是否“显著上升”。如果出现“显著上升”,则代表擦窗机器人遇到障碍了。
由于擦窗机器人行走时本身带有负载,不同方向行走的负载不同,而且电流数据本身会有干扰,如何定义“显著上升”比较困难,导致算法复杂,本发明通过多级滤波算法解决这个难题,如图1所示。
采用DMA方式通过采集驱动擦窗机器人运动的电机的模数转换电路的多组电流数据值;计算电流平均值;再计算比电流平均值大的数据量,如果太少且小于预先设定的数量,则代表有尖峰干扰值,就用平均值代替干扰值,并重新计算平均值;初始化历史平均值,将当前平均值存入历史平均值;计算“历史平均值的平均值”;如果当前平均值大于某给定值(代表可能遇到障碍)且比“历史平均值的平均值”大某给定值(代表电流值“显著上升”),则推断遇到了障碍;为避免重复判断,将原来的平均值全部替换成新的平均值。
一种机器人运动障碍的判断方法,包括如下步骤:
S1:采集驱动机器人运动的电机的模数转换电路的多组电流数据值;
S2:计算步骤S1的“多组电流数据值”的电流平均值;
S3:计算大于电流平均值的数量;
S4:如果比电流平均值大的数量小于预先设定的数量,则代表有尖峰干扰值,并采用电流平均值代替干扰值,将大于电流平均值的电流数据值改成电流平均值,重新计算电流平均值;
S5:初始化历史电流平均值,将当前电流平均值存入历史电流平均值;
S6:计算“历史电流平均值的平均值”;
S7:如果当前电流平均值大于预先设定值且当前电流平均值和“历史电流平均值的平均值”之差大于预先设定的差值的最小值,则推断遇到了障碍,为避免重复判断,将原来的电流平均值全部替换成新的平均值;否则判断未遇到障碍且回到步骤S1重新进行判断;否则判断未遇到障碍且回到步骤S1重新进行判断。
其中,步骤S1采用DMA方式读取电机的模数转换电路的电流数据。
DMA方式,Direct Memory Access,也称为成组数据传送方式,有时也称为直接内存操作,DMA方式在数据传送过程中,没有保存现场、恢复现场之类的工作。
步骤S2中采用算数方法是计算电流平均值、或者采用去掉“电流数据值”的最大值和去掉“电流数据值”的最小值后再计算电流平均值、或者采用中位值法取电流平均值、或者中位值的平均取电流平均值、或者滑动平均值法取电流平均值、或者加权滑动平均值取电流平均等滤波算法代替算术平均值滤波算法。
步骤S4中的“将大于电流平均值的电流数据值改成电流平均值,重新计算电流平均值”的意思是:大于电流平均值的电流数据值改成电流平均值为a,a和小于电流平均值的电路数据值一起计算电流平均值。
对于步骤S4,如果比电流平均值大的数量等于或大于预先设定的数量,则步骤S5为“保存当前电流平均值作为历史电流平均值”。
步骤S5的“将当前电流平均值存入历史电流平均值”的意思是将步骤S4的重新计算且得到的电流平均值作为历史电流平均值。
步骤S6中的“计算“历史电流平均值的平均值””的意思是,根据步骤S1至S5的方法取得的多组历史电流平均值,再计算多组历史电流平均值的平均值。
步骤S7的“如果当前电流平均值I1大于预先设定值I2且当前电流平均值I1和历史电流平均值的平均值I3之差大于预先设定的差值的最小值I4,即I1>I2,I1-I3>I4。
其中步骤S7中,如果当前电流平均值大于预先设定值,代表擦窗机器人可能遇到障碍。步骤S7中,“如果当前电流平均值大于预先设定值且当前电流平均值和历史电流平均值的平均值之差大于预先设定的差值的最小值”,代表电流值“显著上升”。
步骤S7中“原来的电流平均值”指的是本判断方法中保存了一批电流平均值,“新的电流平均值”指当前的电流平均值。
由于擦窗机器人往不同的方向行走,负载变化较大,电流值变化较大;且电路本身干扰大,也会造成电流比较剧烈的变化。因此,从本发明采用了两级平均值滤波叠加去尖峰值干扰滤波(即前面两级都是算术平均值滤波算法,去尖峰值算法就是用平均值代替尖峰值),只要参数选择得当,完全可以解决这些难题。
本发明还采用了避开浮点运算的算法(即:不需要进行浮点运算,浮点运算比普通的运算速度要慢很多,所以在对实时性要求较高的地方,如果需要浮点运算,会加FPU(浮点运算单元),成本会高很多。),其中步骤S1采用了避开浮点运算的算法。
浮点运算就是实数运算,浮点运算是很慢的而且会有误差。现在大多数机器都是32位的,也就是说32位都用来表示整数的话,那么对于无符号整数就是0 到 2^32-1,对于有符号的话就是-2^31 到 2^31-1。
本发明可以在普通单片机上快速运行。采集的AD数据不转换成浮点数的电流值,直接采用整数运算;步骤S1采集的电流数据量以及步骤S5的历史电流平均值的数量都是2的N次方(如果要去掉最大、最小值则换成2的N次方+2),通过二进制的移位操作计算平均值。
其中, 二进制右移1位,实际就是除以2,右移2位,就是除以4,右移N位,就是除以2的N次方。所以将数据量设计成2的N次方后,只需右移N位,就完成了除法,除法本来是浮点运算的。
本发明采用的是算术平均值滤波算法以及去尖峰值干扰滤波算法。本发明采用的算术平均值滤波是简单的算术平均值,可以替换成去最大最小值后再计算平均值,或采用中位值、中位值平均、滑动平均值、加权滑动平均值等滤波算法代替算术平均值滤波算法。去尖峰值干扰滤波算法是用平均值代替尖峰值,也可以简单去掉几个尖峰值后再计算平均值。
本发明通过采集驱动机器人运动的电机的电流数据,再通过检测电流的变化来判断是否遇到障碍;本发明采用了“平均值”+“平均值的平均值”两级级滤波算法,再叠加去尖峰的滤波算法,来判断电流是否“显著上升”;本发明采用的两级滤波算法可以根据需要推广到多级,去适应更复杂的干扰环境;每一级滤波算法可以灵活采用多种成熟滤波算法;避免浮点运算,采集的数据量是2的N次方(或2的N次方+2),用二进制的移位操作代替浮点运算。
本发明采用多级滤波算法,通过对电流变化进行分析,来判断擦窗机器人是否遇到障碍。虽然算法复杂,且需要较多的实验确定相关参数,但是因为不需要任何检测开关,简化了机器人的整体结构,大量减少了布线,从而大大降低整体故障率。
以上所述,仅是本发明的较佳实施例而已,并非对本发明作任何形式上的限制,虽然本发明已以较佳实施例揭露如上,然而并非用以限定本发明,任何熟悉本专业的技术人员,在不脱离本发明技术方案范围内,当可利用上述揭示的技术内容做出些许更动或修饰为等同变化的等效实施例,但凡是未脱离本发明技术方案内容,依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰,均仍属于本发明技术方案的范围内。