一种基于惯性传感器周期运动轨迹还原的优化方法

1.本发明涉及轨迹还原技术领域，具体为一种基于惯性传感器周期运动轨迹还原的优化方法。


背景技术：

2.基于惯性传感器的运动轨迹还原技术可以获取运动载体的加速度、角速度，为了充分发挥加速度计对长时间内累计误差的控制能力和陀螺仪在短时间内对高频噪声的抑制能力，使用扩展卡尔曼滤波算法将两惯性传感器得到的数据进行融合，得到较为准确的姿态角，然后通过对加速度值进行二次积分得到轨迹曲线，但是由于加速度计信号存在一定偏差，对加速度值进行二次积分会造成误差的累积，导致周期运动还原轨迹与实际轨迹存在偏差。因而控制累积误差的产生，优化周期运动还原轨迹成为首要问题。


技术实现要素：

3.(一)解决的技术问题
4.针对现有技术的不足，本发明提供了一种基于惯性传感器周期运动轨迹还原的优化方法，解决了上述背景技术中所存在的问题。
5.(二)技术方案
6.为实现上述目的，本发明提供如下技术方案：一种基于惯性传感器周期运动轨迹还原的优化方法，包括以下步骤：
7.s1、将装置绕着给定轨迹进行周期性运动，得到速度数据曲线；
8.s2、将所得到的数据曲线与一参考数据曲线的可能部分曲线进行连续或规律地比较，其中，参考曲线的部分曲线分别配设周期运动的一个或多个结束点位置；
9.s3、从可能的部分数据曲线组中排出如下的一个或多个部分数据曲线：其不与数据曲线一致或者其结束部分不与数据曲线一致；
10.s4、将结束位置中的配属于可能的剩余的部分数据曲线的结束位置作为周期运动结束点；
11.s5、找到一个周期内结束点使用斜率补偿的方法优化结束点到起始点的轨迹，从而持续的将积累误差控制在一定范围内。
12.优选的，所述s2中，参考数据曲线包括由针对至少一个完整运动周期运动所生成的数据曲线。
13.优选的，所述s5中，斜率补偿方法为：
14.xr(t)＝x(t)-(t
×
k_x+b_x)
[0015][0016]
b_x＝x(t2)-k_x
×
t2[0017]
其中，xr(t)为优化后位移数据，x(t)为优化前位移数据，t2为结束运动时刻，t1为
起始运动时刻，t为从结束时刻到起始时刻中某一时刻，x(t2)为结束运动时刻位移数据，x(t1)为起始运动时刻位移数据。
[0018]
(三)有益效果
[0019]
本发明提供了一种基于惯性传感器周期运动轨迹还原的优化方法，具备以下有益效果：
[0020]
本发明通过周期运动加速度、速度、角速度等物理量周期性变化的特点找到周期性运动的运动结束点，使用斜率补偿的方法优化从结束点到起始点的轨迹，从而持续的将累积误差控制在一定范围内。就该优化方法可以保证一定的运动周期时间内控制误差较前任工作减小一半。
附图说明
[0021]
图1为圆形周期运动轨迹结束点判定程序图；
[0022]
图2为圆形周期运动轨迹结束点判定结果图；
[0023]
图3为矩形周期运动轨迹结束点判定程序图；
[0024]
图4为矩形周期运动轨迹结束点判定结果图；
[0025]
图5为以x轴为例斜率补偿程序逻辑主体部分图；
[0026]
图6为画倾斜面内矩形轨迹各方向距离及误差图；
[0027]
图7为圆形轨迹斜率补偿前后位移数据对比图；
[0028]
图8为圆形轨迹斜率补偿前后运动还原轨迹对比图。
具体实施方式
[0029]
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
[0030]
结合附图1-8，说明本发明一种基于惯性传感器周期运动轨迹还原的优化方法在周期性圆形曲线运动和周期性矩形直线运动的实例应用。
[0031]
对于一般的周期运动，其开始及结束的速度为0m/s上下，针对这一特点在提前获取标准轨迹运动信息的前提下能够设计对周期运动结束点判定的算法。通过该算法当运动轨迹判定到达结束点的位置时，就能对结束点与起始点直接的轨迹进行补偿优化从而持续的将累计误差控制在一定范围内。
[0032]
用打印出的画有矩形路线的纸黏在一倾斜的板上，再将装置绕着该路线做周期性运动，提前量得装置在x轴方向总移动距离为0.6m，在y轴方向总移动距离为0.25m，在z轴方向总移动距离为0.3m。
[0033]
用打印出画有圆形的纸黏在一倾斜的板上，将装置绕着该路线做周期性运动，对采集到的数据进行处理后，得到优化前后还原轨迹对比。
[0034]
在圆形轨迹的周期运动中，因为速度的正负代表着载体的运动方向，由于可以提前获得标准轨迹的运动信息则可根据已知的x、y、z轴方向上速度的正负变化来判定周期运动的结束点。如本次的圆形标准轨迹x轴方向上的速度是从0m/s到负值到正值到负值再回
归到0m/s上下的一个过程，根据x轴方向速度变化这来判定到达结束点的部分程序如图一所示，圆形轨迹周期运动结束点判定结果如图2所示。
[0035]
在矩形轨迹的周期运动中，同理根据提前获得的矩形标准运动的运动信息，x轴方向的速度先从0m/s变到正值，当x轴方向速度从正值变为0m/s时z轴方向速度开始从0m/s变到正值，当z轴方向速度从正值变为0m/s时x轴方向速度开始从0m/s变到负值，最后z轴方向速度从0m/s变到负值再升到0m/s，根据以上速度变化规律得到的对结束点的判定程序如图3所示，矩形轨迹周期运动结束点判定结果如图4所示。
[0036]
得到周期性运动结束点后，运用周期运动结束点与起始点间矢量为零的调节，根据标准轨迹运动规律，对积分后位移结果设置阈值，若超过阈值，则从该点到运动结束点进行斜率补偿优化，该阈值通过实验数据反复测试得到。
[0037]
令载体起始运动时刻为t1，结束运动时刻为t2，则可构建位移多项式斜率补偿模型
[0038]
xr(t)＝x(t)-kt-b
[0039]
根据起始及结束时刻位移为零的条件得到如下方程：
[0040][0041]
则可得到：
[0042][0043]
代入位移补偿模型，最终得到位移斜率补偿多项式为：
[0044]
xr(t)＝x(t)-(t
×
k_x+b_x)
[0045]
图5为以x轴为例斜率补偿程序逻辑主体部分.
[0046]
将装置绕着上述圆形路线运动，对采集到的数据进行处理后，得到优化前后位移数据对比以及轨迹对比。图7为圆形轨迹斜率补偿前后位移数据对比图；图8为圆形轨迹斜率补偿前后运动还原轨迹对比图。左图均为未进行斜率补偿优化的数据；右图为经过斜率补偿优化后的结果。
[0047]
该基于惯性传感器周期运动轨迹还原的优化方法在进行周期性运动结束点判定、斜率补偿优化控制累积误差后显著提搞了轨迹还原的精度，经过实验数据分析可以发现误差相对前人工作减少一半，运动还原轨迹基本与实际运动轨迹吻合。
[0048]
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。而且，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。
[0049]
尽管已经示出和描述了本发明的实施例，对于本领域的普通技术人员而言，可以理解在不脱离本发明的原理和精神的情况下可以对这些实施例进行多种变化、修改、替换和变型，本发明的范围由所附权利要求及其等同物限定。