[发明专利]一种基于惯性导航的移动机器人位移计算算法

权利要求书

1.一种基于惯性导航的移动机器人位移计算方法，其特征在于：包括以下步骤：

步骤S1：读取移动机器人的三轴加速度a(a_x,a_y,a_z)；

步骤S2：对三轴加速度采用防脉冲干扰平均滤波法进行滤波，用以滤除由于硬件不稳定产生的跳变数据；

步骤S3：对步骤S2滤波后的数据采用取波峰-波谷算法获取三轴加速度的波峰-波谷值；

步骤S4：对步骤S3中获得的三轴加速度的波峰-波谷值采用最小二乘法获取最佳线性拟合函数，并对拟合函数积分计算出位移；

步骤S5：对位移计算算法进行修正，并用修正的位移计算算法来计算防脉冲干扰平均滤波法滤波后的位移；

步骤S6：对两次位移进行融合消除误差计算最终位移；

其中，所述步骤S3具体包括以下步骤；

步骤S31：检测波峰-波谷，读取最新的a_xn数据，并与上一数据a_x(n-1)相比较，若a_xn＞a_x(n-1)，则记d＝1；若a_xn＜a_x(n-1)，则记d＝-1；若a_xn＝a_x(n-1)记d＝0；

步骤S32：若检测到上次d＝1而此时d＝-1，则说明上次的即为波峰值；若检测到上次d＝-1而此时d＝1，则说明上次的即为波谷值；

步骤S33：用最新的波峰-波谷值作绝对值差，如果该绝对值差大于设定的阈值，则表明不是机器人抖动造成的加速度敏感值，将计算结果纳入下一步计算；

步骤S34：采用步骤S31至S33的方法，分别计算a_y与a_z的波峰-波谷值，并作绝对值差与设定的阈值作比较；

其中，所述步骤S4具体包括以下步骤：

步骤S41：取储存的最新的波峰-波谷或波谷-波峰之间的加速度值作为一组数据值，记为A_xi＝{a_x1,a_x2,…,a_xm},(i＝1,2,...,m)；

步骤S42：计算每组加速度a_x的方差S_xi，记作S_xi＝(S_x1,S_x2,...,S_xm)，采用下式计算：

其中，为平均加速度；

步骤S43：根据最小二乘法公式，用S_xi最小原则求出最佳线性拟合函数，并对该线性拟合函数作二次积分求出波峰-波谷或波谷-波峰之间的这段位移，与之前位移累加算出总位移L'_x；

步骤S44：采用步骤S41至步骤S43的方法计算出Y轴和Z轴的总位移L'_y、L'_z；

其中，所述步骤S5具体为：对防脉冲干扰平均滤波法滤波后的加速度变化设置一个阈值以解决加速度漂移现象，当变化超过这个阈值就根据以下修正公式进行计算速度和位移：

V_n＝2*V_n-1-V_n-2+Δa_n,n-1*Δt；

ΔV＝V_n-V_n-1＝V_n-1-V_n-2+Δa_n,n-1*Δt；

S_n＝2*S_n-1-S_n-2+a_n-1*t²；

其中，V_n、S_n、t、Δa_n,n-1分别表示当前时刻速度、当前时刻总位移、积分时间、当前时刻与上一时刻之间的平均加速度；

由上式可计算出X、Y、Z轴的总位移L”_x,L”_y,L”_z；

其中，所述步骤S6具体为：将每次采集到的加速度作拟合函数积分计算位移算法和修正的位移计算算法分别求出位移L',L”，两数求平均得最终位移L。