本发明涉及一种基于惯性导航的移动机器人位移计算算法，首先通过移动机器人上的惯性测量单元的三轴加速度计读取三轴加速度；接着对三轴加速度采用防脉冲干扰平均滤波法滤除由于硬件不稳定产生的跳变数据；然后对三轴加速度采用取波峰‑波谷算法获取三轴加速度波峰‑波谷值；接着对三轴加速度的波峰‑波谷采用最小二乘法获取最佳线性拟合函数，并对拟合函数积分计算出位移；然后对位移计算算法进行修正，并用修正的位移计算算法来计算防脉冲干扰平均滤波法滤波后的位移；最后对两次位移进行融合消除误差。本发明大大消除了惯性导航加速度计计算位移的误差，实现了单靠加速度计就能精确位移计算，提高了基于惯性导航的移动机器人室内定位的精度。
1.一种基于惯性导航的移动机器人位移计算方法，其特征在于：包括以下步骤：步骤S1：读取移动机器人的三轴加速度a(ax,ay,az)；
步骤S2：对三轴加速度采用防脉冲干扰平均滤波法进行滤波，用以滤除由于硬件不稳定产生的跳变数据；
步骤S3：对步骤S2滤波后的数据采用取波峰-波谷算法获取三轴加速度的波峰-波谷值；
步骤S4：对步骤S3中获得的三轴加速度的波峰-波谷值采用最小二乘法获取最佳线性拟合函数，并对拟合函数积分计算出位移；
步骤S5：对位移计算算法进行修正，并用修正的位移计算算法来计算防脉冲干扰平均滤波法滤波后的位移；
步骤S6：对两次位移进行融合消除误差计算最终位移；
步骤S31：检测波峰-波谷，读取最新的axn数据，并与上一数据ax(n-1)相比较，若axn＞ax(n-1)，则记d＝1；若axn＜ax(n-1)，则记d＝-1；若axn＝ax(n-1)记d＝0；
步骤S32：若检测到上次d＝1而此时d＝-1，则说明上次的即为波峰值；若检测到上次d＝-1而此时d＝1，则说明上次的即为波谷值；
步骤S33：用最新的波峰-波谷值作绝对值差，如果该绝对值差大于设定的阈值，则表明不是机器人抖动造成的加速度敏感值，将计算结果纳入下一步计算；
步骤S34：采用步骤S31至S33的方法，分别计算ay与az的波峰-波谷值，并作绝对值差与设定的阈值作比较；
步骤S41：取储存的最新的波峰-波谷或波谷-波峰之间的加速度值作为一组数据值，记为Axi＝{ax1,ax2,…,axm},(i＝1,2,...,m)；
步骤S42：计算每组加速度ax的方差Sxi，记作Sxi＝(Sx1,Sx2,...,Sxm)，采用下式计算：其中， 为平均加速度；
步骤S43：根据最小二乘法公式，用Sxi最小原则求出最佳线性拟合函数，并对该线性拟合函数作二次积分求出波峰-波谷或波谷-波峰之间的这段位移，与之前位移累加算出总位移L'x；
步骤S44：采用步骤S41至步骤S43的方法计算出Y轴和Z轴的总位移L'y、L'z；
其中，所述步骤S5具体为：对防脉冲干扰平均滤波法滤波后的加速度变化设置一个阈值以解决加速度漂移现象，当变化超过这个阈值就根据以下修正公式进行计算速度和位移：Vn＝2*Vn-1-Vn-2+Δan,n-1*Δt；
ΔV＝Vn-Vn-1＝Vn-1-Vn-2+Δan,n-1*Δt；
其中，Vn、Sn、t、Δan,n-1分别表示当前时刻速度、当前时刻总位移、积分时间、当前时刻与上一时刻之间的平均加速度；
由上式可计算出X、Y、Z轴的总位移L”x,L”y,L”z；
其中，所述步骤S6具体为：将每次采集到的加速度作拟合函数积分计算位移算法和修正的位移计算算法分别求出位移L',L”，两数求平均得最终位移L。
2.根据权利要求1所述的一种基于惯性导航的移动机器人位移计算方法，其特征在于：所述步骤S1具体为：通过Linux系统主控板采用IIC通信方式读取移动机器人上的惯性测量单元的三轴加速度计的三轴加速度a(ax,ay,az)。
3.根据权利要求1所述的一种基于惯性导航的移动机器人位移计算方法，其特征在于：所述步骤S2具体为：对连续采集到的n个三轴加速度数据分别进行排序，并同时存储和更新最近n个数据，对已排序的数据去掉其中最大和最小的m个数据，求取剩余数据的平均值。
一种基于惯性导航的移动机器人位移计算算法
技术领域
[0001] 本发明涉及机器人定位领域，特别是一种基于惯性导航的移动机器人位移计算算法。
背景技术
[0002] 目前用于移动机器人的基于GPS的室外定位系统已相当成熟，但在大型室内建筑由于卫星讯号接收不良，室内定位系统成为应用厂商竞相投入的技术。目前室内定位技术主要以Wi-Fi、蓝牙和惯性导航为主，但Wi-Fi和蓝牙需布建大量的网路系统，建置成本较高，且精度亦受室内障碍物影响，而采用惯性导航的移动机器人室内定位不仅建置成本较低，也可大幅提升精度。
[0003] 惯性导航是指利用多种传感器组成多轴惯性测量单元(IMU，Inertial Measurement Unit)，传感器主要包括三轴加速度计、三轴陀螺仪、三轴磁力计。惯性导航用于移动机器人室内定位一般会与其他定位传感器进行融合，单靠惯性导航的室内定位累积误差大，尤其单靠加速度计进行积分计算位移在移动机器人定位技术中还未能实现。误差产生的主要原因是加速度计较为敏感，加速度计具有较为严重的漂移现象，产生的加速度会随着轻微抖动而起伏变化，由此二次积分得出的位移由于时间累积误差不断变大。
[0004] 目前惯性导航室内定位已成功应用于步行控制的室内定位，采用步长控制计算相对位移，如此产生的加速度由于步行中抬放腿呈有规律的起伏变化，而移动机器人基于平面保持水平运动其加速度呈无规律变化，所以移动机器人平台的位移计算未见较好成果。且目前对移动机器人惯性导航的三轴加速度数据的处理更多采用基本数字滤波算法、Kalman滤波算法、时域滤波算法等，这些算法较好实现了对加速度初始数据的消抖和平滑，但是仍不能解决二次积分后位移累积误差不断变大问题。
[0005] 基于上述基于惯性导航的移动机器人加速度计算位移累积误差不断变大问题，本发明提出一种既能较好处理加速度计数据不稳定问题又能较好通过加速度计算位移的算法。
发明内容
[0006] 有鉴于此，本发明的目的是提出一种基于惯性导航的移动机器人位移计算算法，既能较好处理加速度计数据不稳定的问题又能较好通过加速度计算位移。
[0007] 本发明采用以下方案实现：一种基于惯性导航的移动机器人位移计算算法，具体包括以下步骤：
[0008] 步骤S1：读取移动机器人的三轴加速度a(ax,ay,az)；
[0009] 步骤S2：对三轴加速度采用防脉冲干扰平均滤波法进行滤波，用以滤除由于硬件不稳定产生的跳变数据；
[0010] 步骤S3：对步骤S2滤波后的数据采用取波峰-波谷算法获取三轴加速度的波峰-波谷值；
[0011] 步骤S4：对步骤S3中获得的三轴加速度的波峰-波谷值采用最小二乘法获取最佳线性拟合函数，并对拟合函数积分计算出位移；
[0012] 步骤S5：对位移计算算法进行修正，并用修正的位移计算算法来计算防脉冲干扰平均滤波法滤波后的位移；
[0013] 步骤S6：对两次位移进行融合消除误差计算最终位移。
[0014] 进一步地，所述步骤S1具体为：通过Linux系统主控板采用IIC通信方式读取移动机器人上的惯性测量单元的三轴加速度计的三轴加速度a(ax,ay,az)。
[0015] 进一步地，所述步骤S2具体为：对连续采集到的n个三轴加速度数据分别进行排序，并同时存储和更新最近n个数据，对已排序的数据去掉其中最大和最小的m个数据，求取剩余数据的平均值。
[0016] 进一步地，所述步骤S3具体包括以下步骤；
[0017] 步骤S31：检测波峰-波谷，读取最新的axn数据，并与上一数据ax(n-1)相比较，若axn＞ax(n-1)，则记d＝1；若axn＜ax(n-1)，则记d＝-1；若axn＝ax(n-1)记d＝0；
[0018] 步骤S32：若检测到上次d＝1而此时d＝-1，则说明上次的即为波峰值；若检测到上次d＝-1而此时d＝1，则说明上次的即为波谷值；
[0019] 步骤S33：用最新的波峰-波谷值作绝对值差，如果该绝对值差大于设定的阈值，则表明不是机器人抖动造成的加速度敏感值，将计算结果纳入下一步计算；
[0020] 步骤S34：采用步骤S31至S33的方法，分别计算ay与az的波峰-波谷值，并作绝对值差与设定的阈值作比较。
[0021] 进一步地，所述步骤S4具体包括以下步骤：
[0022] 步骤S41：取储存的最新的波峰-波谷或波谷-波峰之间的加速度值作为一组数据值，记为Axi＝{ax1,ax1,…,axm},(i＝1,2,...,m)；
[0023] 步骤S42：计算每组加速度ax的方差Sxi，记作Sxi＝(Sx1,Sx2,...,Sxm)，采用下式计算：
[0024]
[0025] 其中，为平均加速度；
[0026] 步骤S43：根据最小二乘法公式，用Sxi最小原则求出最佳线性拟合函数，并对该线性拟合函数作二次积分求出该段位移，与之前位移累加算出总位移L′x；
[0027] 步骤S44：采用步骤S41至步骤S43的方法计算出Y轴和Z轴的总位移L′y、L′z。
[0028] 进一步地，所述步骤S5具体为：对防脉冲干扰平均滤波法滤波后的加速度变化设置一个阈值以解决加速度漂移现象，当变化超过这个阈值就根据以下修正公式进行计算速度和位移：
[0029] Vn＝2*Vn-1-Vn-2+Δan,n-1*Δt；
[0030] ΔV＝Vn-Vn-1＝Vn-1-Vn-2+Δan,n-1*Δt；
[0031] Sn＝2*Sn-1-Sn-2+an-1*t2；
[0032]
[0033] 其中，Vn、Sn、t分别表示当前时刻速度、当前时刻总位移、积分时间；
[0034] 由上式可计算出X、Y、Z轴的总位移L″x,L″y,L″z。
[0035] 进一步地，所述步骤S6具体为：将每次采集到的加速度作拟合函数积分计算位移算法和修正的位移计算算法分别求出位移L′,L″，两数求平均得最终位移L。
[0036] 与现有技术相比，本发明有以下有益效果：本发明可有效解决移动机器人采用惯性导航室内定位时位移累积误差不断变大的问题，实现了单靠加速度计就能精确位移计算，提高了基于惯性导航的移动机器人室内定位的精度。
附图说明
[0037] 图1为本发明的算法流程示意图。
[0038] 图2为本发明实施例中的硬件模块图。
[0039] 图3为本发明实施例中获取的加速度波峰-波谷图。
[0040] 图4为本发明实施例中加速度最小二乘法求得最佳拟合函数图。
[0041] 图5为本发明实施例中融合后的最终位移轨迹图。
具体实施方式
[0042] 下面结合附图及实施例对本发明做进一步说明。
[0043] 图1为本发明实施例的算法流程示意图，图2为本发明实施例中的硬件模块图。如图1以及图2所示，本实施例提供的一种基于惯性导航的移动机器人位移计算算法具体包括以下步骤：
[0044] S1：通过Linux系统主控板采用IIC通信方式读取图2所示移动机器人上的惯性测量单元的三轴加速度计的三轴加速度a(ax,ay,az)。
[0045] S2：对三轴加速度采用防脉冲干扰平均滤波法进行滤波。对连续采集到的n个三轴加速度数据a(ax,ay,az)分别进行排序，并同时存储和更新最近n个数据，对已排序数据去掉其中最大和最小的m个数据，将剩余数据进行平均求取平均值。本实施例中，取n＝10，m＝2。
[0046] S3：图3是用取波峰-波谷算法所获取的三轴加速度波峰-波谷图。在本实施例中，具体的用取波峰-波谷算法是：首先检测波峰-波谷，读取最新axn数据，与上一数据ax(n-1)作比较，若axn＞ax(n-1)则记d＝1，axn＜ax(n-1)记d＝-1，axn＝ax(n-1)记d＝0。若检测到上次d＝1而此时d＝-1，则说明上次的ax(n-1)即为波峰值；若检测到上次d＝-1而此时d＝1，则说明上次的ax(n-1)即为波谷值。用最新的波峰-波谷值作绝对值差，如果大于设定的阈值，则表明不是机器人抖动造成的加速度敏感值，纳入下一步计算。用此算法同理计算ay和az的波峰-波谷值，并作绝对值差与阈值作比较。本实施例中，取kx＝10cm/s,ky＝10cm/s,kz＝0cm/s。
[0047] S4：图4是某段波峰-波谷间加速度值采用最小二乘法所获得的最佳线性拟合函数。本实施例中，对S3中所储存最新的波峰-波谷或波谷-波峰之间的加速度值作为一组数据值，记为Axi＝{ax1,ax1,…,axm},(i＝1,2,...,m)；计算每组加速度ax的方差Sxi，记作Sxi＝(Sx1,Sx2,...,Sxm)，其中 为平均加速度。根据最小二乘法公式，用Sxi最小原则求出最佳线性拟合函数，并对该线性拟合函数作二次积分求出该段位移，与之前位移累加算出总位移L′x。用此算法同理计算Y轴和Z轴的中位移L′y和L′z。
[0048] S5：对位移计算算法进行修正，并用修正的位移计算算法来计算防脉冲干扰平均滤波法滤波后的位移。对防脉冲干扰平均滤波法滤波后的加速度变化设置一个阈值以解决加速度漂移现象，当变化超过这个阈值就根据以下修正公式进行计算速度和位移：
[0049] Vn＝2*Vn-1-Vn-2+Δan,n-1*Δt；
[0050] ΔV＝Vn-Vn-1＝Vn-1-Vn-2+Δan,n-1*Δt；
[0051] Sn＝2*Sn-1-Sn-2+an-1*t2；
[0052]
[0053] 由上式可计算出X、Y、Z轴的总位移L″x,L″y,L″z。
[0054] 其中，Vn、Sn、t分别表示当前时刻速度、当前时刻总位移、积分时间；
[0055] S6：图5是对两次位移进行融合后最终位移轨迹图。本实施例中，采用将每次采集到的加速度作S04和S05算法分别求出位移L′,L″，两数求平均得最终位移L。
[0056] 以上所述仅为本发明的较佳实施例，凡依本发明申请专利范围所做的均等变化与修饰，皆应属本发明的涵盖范围。
