技术领域本发明属于机器人领域，尤其涉及一种机器人惯性导航系统的漂移校准方法，用于对惯性导航系统测量的机器人速度进行校准。
背景技术：
作为民用化最成功的机器人技术之一，扫地机器人正在走进越来越多的家庭，智能清扫正在让人们的家居生活变得越来越温馨。追求更高水平的智慧是机器人发展的一条主线。通过各种新功能的传感器，防碰撞、防跌落等一些扫地机器人的常见使用问题已得到成功解决，同时应用加装在充电桩上的信标实现了机器人低电量时的回充导航、极大地方便了人们的使用。目前扫地机器人最新的发展趋势是通过自动规划清扫路线实现100％无死角的屋内环境清洁，这将大大优于目前常用的随机碰撞式寻路模式。自动规划清扫路线的技术核心是机器人要能够自主导航，实时获知自己在屋子中的位置和运动轨迹。惯性导航技术(以下简称惯导，INS)是比较常用的机器人导航方式。它不依赖于外部信息，是一种不易受到干扰的自主式导航系统。惯导通过测量载体在惯性参考系(通常是以机器人本身为基准的参考系)的加速度，利用陀螺测量的角速度将其换算到导航坐标系(对于扫地机器人，一般都默认为屋内地面)中，然后通过积分运算获得载体的瞬时速度和瞬时位置、及偏航角等姿态信息。惯导的优势在于抗干扰，只需给定初始坐标(扫地机器人可以以充电桩作为每次运动的初始点)，运行过程中不需要外部参照就可确定任意时刻机器人的位置、方向及速度。这种方法适用于复杂地理环境和外界干扰严重场合的精确定位和定向。目前国内的青岛海通机器人、新松AGV机器人搬运系统都在进行机器人惯导技术的研发。惯导系统应用于机器人导航的关键是要解决惯导系统随时间的漂移问题，尤其是常用的MEMS惯导系统，尽管MEMS陀螺仪和加速度计具有体积小易于安装、耗电低等众多优势，但陀螺和加速度计的漂移会让惯导算法解算出来的机器人行走路径严重偏离真实路线。惯导的核心器件是陀螺仪和加速度计，利用它们的输出进行坐标变换和积分可解算出机器人行走轨迹。这两种器件的输出都会漂移，而积分运算又会将漂移造成的干扰累积，使得干扰问题进一步严重。本发明所解决的就是发明了一种方法能够抑制惯导系统的漂移。目前比较常用的惯导系统漂移修正方法是零速校正，即寻找机器人两次相邻的停车时刻，停车时速度都为0，若测得停车时车速不为0，则是传感器漂移造成的，通过两次时间间隔和停车时车速的传感器解算值(由于传感器漂移导致不为0)可计算出传感器随时间的漂移量。但零速校正不适合扫地机器人，扫地机器人通常要长时间不停的工作，很难出现静止不动的时刻。尽管会出现撞墙折返、原地转向等接近零速度的时刻，但此时由于机器人的电动机动力系统没有停止工作，受电动机的振动影响，加速度计的输出值在大幅振动，难以通过加速度判别零速时刻。若需要依靠经常性的停机进行零速校正，会极大延长机器人的扫地时间、降低效率，而且经常停机零速校正会造成电机负载严重波动、有损寿命。
技术实现要素：
(一)要解决的技术问题鉴于上述问题，本发明的目的在于提供一种机器人速度校正方法，无需频繁地停机即可进行机器人速度校正，并具有校正精度高的优点。(二)技术方案本发明提供一种机器人惯性导航系统的漂移校准方法，用于对惯性导航系统测量的机器人速度进行校准，方法包括：S1，获取相邻的机器人运动状态变化时刻tn-1和tn，其中，机器人运动状态包括直线运动状态、原定转弯状态、静止状态；S2，获取惯性导航系统在两个状态变化时刻tn-1和tn所测得的速度vn-1和vn，并计算变化斜率vd：vd=vn-vn-1tn-tn-1;]]>S3，根据变化斜率vd，对tn-1～tn时间段的惯性导航系统所测得的速度进行校准，得到校准后的速度vi：vi＝vt-vd(t-tn-1)，其中，vt为校准前惯性导航系统在t时刻所测得的速度，t∈[tn,tn-1]。(三)有益效果本发明通过分析在机器人在行驶时的角速度和加速度，从而提取出机器人的状态变化时刻，即线速度发生变化的时刻作为校正时刻，这时在理论上车速为0，故在这时测得的速度即为速度漂移，从而对该速度漂移进行校正。本方法无需频繁地停机即可对惯性导航系统测得的速度进行校正，并具有校正精度高的优点。附图说明图1为本发明实施例提供的扫地机器人惯性导航系统的漂移校正方法的流程图。图2为本发明实施例中角速度信号、加速度信号及合成信号的波形图。图3为本发明实施例中校正前后的速度在X轴、Y轴和Z轴的效果图。图4为本发明实施例中扫地机器人惯性导航系统在经过速度校正后的轨迹图。具体实施方式本发明提供一种机器人惯性导航系统的漂移校准方法，用于对惯性导航系统测量的机器人速度进行校准，首先获取机器人相邻的两个运动状态变化时刻，然后获取机器人在两个状态变化时刻的速度值，从而计算机器人速度漂移的变化斜率，最后，根据机器人速度漂移的变化斜率，对惯性导航系统测得的速度进行校正，得到校正后的速度。本方法无需频繁地停机即可对惯性导航系统测得的速度进行校正，并具有校正精度高的优点。根据本发明的一种实施方式，机器人惯性导航系统的漂移校准方法包括：S1，获取相邻的机器人运动状态变化时刻tn-1和tn，其中，机器人运动状态包括直线运动状态、原定转弯状态、静止状态，所以运动状态变化时刻是指机器人在直线运动状态、原定转弯状态、静止状态之间进行转变的时刻；S2，获取惯性导航系统在两个状态变化时刻tn-1和tn所测得的速度vn-1和vn，并计算变化斜率vd：vd=vn-vn-1tn-tn-1;]]>S3，根据变化斜率vd，对tn-1～tn时间段的惯性导航系统所测得的速度进行校准，得到校准后的速度vi：vi＝vt-vd(t-tn-1)，其中，vt为校准前惯性导航系统在t时刻所测得的速度，t∈[tn,tn-1]。根据本发明的一种实施方式，步骤S1包括：S11，获取机器人的角速度信号和加速度信号并进行预处理；S12，对预处理后的角速度信号和加速度信号进行合成，得到合成信号，合成信号的信号上沿或信号下沿所对应的时刻为状态变化时刻，具体地，可对角速度信号和加速度信号进行或运算，得到合成信号；S13，获取连续的两个状态变化时刻。根据本发明的一种实施方式，步骤S11包括：将角速度信号与第一阈值进行比较，当角速度信号大于第一阈值时，令该角速度信号处于高电平，否则，令该角速度信号处于低电平，优选地，第一阈值取50度/s；将加速度信号与第二阈值进行比较，当加速度信号大于第二阈值时，令该加速度信号处于高电平，否则，令该加速度信号处于低电平，优选地，第二阈值取0.001m/s2。根据本发明的一种实施方式，校正后的速度vi包括导航坐标系下X轴、Y轴和Z轴的速度分量，方法还包括：S4，对校正后的速度vi中各速度分量进行时间维度积分，获得机器人的各时刻的位置坐标Pi，将各时刻的位置坐标Pi连接，得到机器人的运动轨迹。根据本发明的一种实施方式，机器人速度校正方法还包括：S0，获取机器人在自身坐标系下的加速度信号[ax,bay,baz,b]，并采用欧拉角法进行坐标系转换，得到导航坐标系下的加速度信号[ax,kay,kaz,k]：ax,kay,kaz,k=cosRcosH-sinPsinRsinHcosRsinH+sinPsinRcosHcosPsinR-cosPsinHcosPcosHsinPsinRcosH+sinPcosRsinHsinRsinH-sinPcosRcosHcosPcosRax,bay,baz,b]]>其中，H,P,R分别表示航向角、俯仰角和横滚角，k表示惯性导航系统的信号采样的第k时刻；对导航坐标系下的加速度信号[ax,kay,kaz,k]进行时间维度积分，得到校正前的速度vt：vt=vx,tvy,tvz,t=Σk=1tax,k·ΔtΣk=1tay,k·ΔtΣk=1taz,k·Δt,]]>其中，t表示惯性导航系统的信号采样当前时刻，Δt表示采样时间间隔。由于加速度中的漂移干扰会随积分累加，造成速度的计算结果偏离实际值、呈线性快速增加。根据本发明的一种实施方式，惯性导航系统包括陀螺仪和加速度计，其均为MEMS器件，利用陀螺仪获取机器人的角速度信号，利用加速度计获取机器人的加速度信号。为使本发明的目的、技术方案和优点更加清楚明白，以下结合具体实施例，并参照附图，对本发明进一步详细说明。图1为本发明实施例提供的扫地机器人惯性导航系统的漂移校正方法的流程图，如图1所示，方法包括：S0，通过MEMS加速度计获取扫地机器人在自身坐标系下的原始加速度信号[ax,bay,baz,b]，并采用欧拉角法进行坐标系转换，得到导航坐标系下的加速度信号[ax,kay,kaz,k]：ax,kay,kaz,k=cosRcosH-sinPsinRsinHcosRsinH+sinPsinRcosHcosPsinR-cosPsinHcosPcosHsinPsinRcosH+sinPcosRsinHsinRsinH-sinPcosRcosHcosPcosRax,bay,baz,b,]]>其中，H,P,R分别表示航向角、俯仰角和横滚角，k表示惯性导航系统的信号采样的第k时刻；对导航坐标系下的加速度信号[ax,kay,kaz,k]进行时间维度积分，得到校正前的速度vt：vt=vx,tvy,tvz,t=Σk=1tax,k·ΔtΣk=1tay,k·ΔtΣk=1taz,k·Δt,]]>其中，t表示惯性导航系统的信号采样当前时刻，Δt表示采样时间间隔。S1，获取相邻的机器人运动状态变化时刻tn-1和tn，其中，所述机器人运动状态包括直线运动状态、原定转弯状态、静止状态。具体包括：S11，获取扫地机器人的角速度信号和加速度信号。当角速度信号大于50度/s时，令该角速度信号处于高电平，否则，令该角速度信号处于低电平；当加速度信号大于0.001m/s2时，令该加速度信号处于高电平，否则，令该加速度信号处于低电平。图2是本发明实施例提供的角速度信号、加速度信号及合成信号的波形图。扫地机器人通常是走直线、航向角速度基本为0，而拐弯时先停车再原地转动，此时机器人的行驶速度为0位置坐标保持不变、但转动角速度很大，如图2(a)和图2(b)所示。本实施例利用机器人直线行驶时角速度上限(即50度/s，常用的“弓”型路线扫地方式中机器人转弯时可达100度/秒)作为判别机器人是否转弯的阈值。同时，考虑到扫地机器人在起点和终点都处于停车状态、动力装置的电机未开动，对加速度计无振动干扰，此时加速度计的输出为0，本实施例取一个很小的加速度阈值(即0.001m/s2)判别停车状态，即高电平为启动状态，低电平为停车状态。S12，对预处理后的角速度信号和加速度信号进行或运算，得到合成信号，如图2(c)所示，合成信号的高电平表示扫地机器人线速度为0，低电平表示扫地机器人线速度不为0。故，该合成信号的下降沿可以表示两种情况：机器人从转弯到走直线的时刻、机器人从停止到启动的时刻；该合成信号的上升沿可以表示两种情况：机器人从走直线到转弯的时刻、机器人从启动到停止的时刻。将合成信号的信号上沿或信号下沿所对应的时刻作为状态变化时刻，这时机器人的线速度理论上为0，故在这时刻测得的速度即为速度漂移。S13，获取机器人在转弯时连续的两个状态变化时刻tn-1和tn。S2，获取机器人在两个状态变化时刻tn-1和tn的速度值vn-1和vn，计算机器人速度漂移的变化斜率vd：vd=vn-vn-1tn-tn-1;]]>S3，根据机器人速度漂移的变化斜率vd，对tn-1～tn时间段的速度进行校正，得到校正后的速度vi：vi＝vt-vd(t-tn-1)，其中，vt为校正前的速度，t∈[tn,tn-1]。其中，校正后的速度vi包括导航坐标系下X轴、Y轴和Z轴的速度分量。图3是本发明实施例中校正前后的速度在X轴、Y轴和Z轴的效果图，如图3所示，各方向上校正后的速度基本上消除了速度漂移。S4，对校正后的速度vi中各速度分量进行时间维度积分，获得机器人的各时刻的位置坐标Pi：Pi=Pi,tn-1+Σt∈[tn-1,tn]vi]]>其中，为tn-1时刻的扫地机器人坐标。将各时刻的位置坐标Pi连接，得到机器人的运动轨迹。图4为本发明实施例中扫地机器人惯性导航系统在经过速度校正后的轨迹图，如图4所示，根据惯性导航系统测定的速度，得到机器人的行驶轨迹，可以看出扫地机器人转弯时的角度基本为直角，与真实的扫地机器人弓形运动路径很接近，说明测得的速度校正后基本不存在速度漂移。以上所述的具体实施例，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施例而已，并不用于限制本发明，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。