基于松耦合IMU阵列导航系统的时间同步算法的制作方法

本发明涉及一种基于松耦合imu阵列导航系统的时间同步算法。

背景技术：

导航系统在使用惯性传感器阵列或其他传感器进行数据融合时，使用的融合算法通常假设惯性传感器阵列内的惯性测量单元(imu)具有完美的时间同步，但在实际的使用中，这是很难实现的。造成惯性测量单元阵列时钟不同步的因素有很多种,例如：测量采集时间、预处理时间、通信传输时间、缓冲时间、计算机调度时间、制造工艺和环境因素影响等。

目前解决时间同步误差的问题有三类方法：硬件实现、软件实现和软硬件结合实现。在处理gps/ins组合导航中，研究人员通过硬件检测1pps(pulsespersecond)脉冲的方法解决sins/gnss的时间同步问题。也有基于高精度定时器的时间同步方案，但这些方法都需要增加额外的硬件。软件解决方法是采用同步的外推算法，该方法运算量较大。软硬件结合的方法是利用多项式拟合，该方法同样需要计算大量的数据点。

技术实现要素：

本发明所要解决的技术问题是提供一种基于松耦合imu阵列导航系统的时间同步算法，本算法克服传统时间同步误差解决方案的缺陷，将时间同步误差作为一个待估状态，并扩展到卡尔曼滤波器状态向量实现在线估计，进而将估计出来的时间同步误差反馈到导航系统解算回路中，并在分数延时滤波器中完成数据的时间同步，无需增加额外硬件，降低导航系统的运算量。

为解决上述技术问题，本发明基于松耦合imu阵列导航系统的时间同步算法包括如下步骤：

步骤一、在imu阵列导航系统中设定时间同步误差为td，惯性传感器的采样周期为ts，且系统中时钟之间的偏置和漂移率已经得到补偿，仅时钟之间的初始偏移量未知，即认为时间同步误差为常数；

步骤二、建立imu阵列导航系统的误差方程，选择以各个导航子系统间的误差量作为状态量，建立基于卡尔曼滤波器的间接法滤波，卡尔曼滤波器的输入为两个惯性导航系统对同一导航参数测量输出的差值，经过迭代的滤波计算，估计出各误差量；

设惯性导航系统中由误差构成的状态向量xk为：

其中，δmk由惯性导航系统的位置误差δs、速度误差δv和姿态角误差δψ组成，δbk为惯性传感器的误差，由加速度计的比力误差δf和陀螺仪偏差δw组成；

则惯性导航系统的误差状态空间表达式为：

xk+1＝φkxk+gkek(2)

其中，φk为状态转移矩阵，gk为过程噪声增益矩阵，ek为惯性传感器测量噪声，协方差矩阵为e{·}表示期望函数；

借助惯性导航系统的解算得到惯性阵列中单个惯性传感器对位置的估计，利用imu阵列中任意两个惯性传感器位置估计的差，得到位置向量zk为：

zk＝hkxk(3)

其中，hk为测量矩阵，

hk＝[ii0i,j-i](4)

其中，i和j分别为位置向量zk和状态向量xk的维数；

步骤三、建立imu阵列导航系统含有时间同步误差的系统方程，imu阵列中第一惯性传感器与第二惯性传感器之间存在时间同步误差td，设载体的轨迹方程为s(t)，则第二惯性传感器在采样瞬间k时的位置估计值为：

其中，wk为第二惯性传感器的位置估计误差，协方差为由于ts相比td是一个小量，在t＝kts附近对s(kts-td)进行二阶泰勒展开为：

其中，sk、vk、ak分别表示采样瞬间k时载体的位置、速度、加速度，

将式(6)代入式(5)得到：

基于式(7)，反馈给闭环系统的真实位置向量可以描述为：

其中，为第一惯性传感器在采样时刻瞬间k时的位置估计值；

定义其中xf,k为引入的辅助变量，将其代入式(8)得到关于同步误差系统的位置向量方程：

式(9)中定义dk为位置误差偏移向量：

对比式(9)和式(3)可知，同步误差系统的位置向量不仅包含位置误差hkxf,k和第二惯性传感器的位置估计误差wk，还包含一个位置偏置项dk，该位置偏置项包含时间同步误差td和惯性导航系统的动态特性；

步骤四、建立imu阵列导航系统中imu阵列的时间同步模型，将时间同步误差td作为状态变量引入到状态空间模型中，记时间同步误差td的估计值为惯性导航系统解算过程中用于控制惯性传感器数据的移位操作；

设定扩展后的状态向量xd,k为：

其中，表示在k时刻估计的延时误差，则惯性导航系统的空间表达式为：

xd,k+1＝φd,kxd,k+gd,kek(12)

其中，

位置向量方程由第一惯性传感器和第二惯性传感器估计位置的差给出，其表示形式为：

其中，为第一惯性传感器数据延时秒的位置解算结果；

在处对zd,k进行二阶泰勒展开，得到：

测量矩阵hd,k定义为:

hd,k＝[hkvk](15)

则位置向量方程表示为：

zd,k＝hd,kxd,k+wk+d'k(16)

其中，

步骤五、采用分数延时滤波器对同步误差td的估计值为进行时间上的移位，分数延迟滤波器的理想频率响应由线性相位全通滤波器给出，其表达式为:

h(e^j2πv)＝e^j2πvdv-normalizedfrequence(17)

其中，j是虚数单位，v是归一化频率，d是移位因子，为需要移位的时间，取

采用拉格朗日插值法插值分数延时滤波器的幅值和相位响应，其中分数延迟滤波器的滤波系数表达式为：

其中，n是滤波器的阶数，且d∈[(n-1)/2,(n+1)/2]为得到的延时；

步骤六、对延时后的惯性传感器数据进行误差补偿，并将经过时间移位且误差补偿后的惯性传感器数据输入到惯性导航系统的导航方程中进行导航求解，得到实际的导航参数。

进一步，步骤五中，当延时d大于(n+1)/2时，在分数延时滤波器前添加单位延时，使d∈[(n-1)/2,(n+1)/2]。

由于本发明基于松耦合imu阵列导航系统的时间同步算法采用了上述技术方案，即本方法首先建立imu阵列导航系统的误差模型，在该模型的基础上建立含有时间同步误差的系统方程，将时间同步误差作为状态变量引入到状态空间模型中作为状态的估计量；然后采用分数延时滤波器对时间同步误差进行时间上的位移，并采用拉格朗日插值法插值分数延时滤波器的幅值和相位响应，得到惯性传感器数据的移位，保证惯性传感器的时间同步；对延时后的惯性传感器数据进行误差补偿，并将经过时间移位且误差补偿后的惯性传感器数据输入到惯性导航系统的导航方程中进行导航求解，得到实际的导航参数。本方法无需增加额外硬件，降低导航系统的运算量。

附图说明

下面结合附图和实施方式对本发明作进一步的详细说明：

图1为本发明基于松耦合imu阵列导航系统的时间同步算法原理框图；

图2为本方法中采用拉格朗日插值法插值分数延时滤波器的幅值示意图；

图3为本方法中采用拉格朗日插值法插值分数延时滤波器的相位响应示意图；

图4为仿真实验中惯性传感器阵列校正后陀螺仪输出示意图；

图5为仿真实验中惯性传感器阵列校正后加速度计输出示意图；

图6为具有时间延迟的四个惯性传感器仿真结果示意图；

图7为采用本算法对具有时间延迟的惯性传感器仿真结果示意图；

图8为应用本算法前后的惯性传感器运动状态估计示意图；

图9为imu阵列中是否考虑时间同步问题得到的运动状态位置估计均值示意图；

图10和图11分别为不同延时时间的惯性传感器仿真结果示意图。

具体实施方式

本发明基于松耦合imu阵列导航系统的时间同步算法包括如下步骤：

步骤一、在imu阵列导航系统中设定时间同步误差为td，惯性传感器的采样周期为ts，且系统中时钟之间的偏置和漂移率已经得到补偿，仅时钟之间的初始偏移量未知，即认为时间同步误差为常数；

步骤二、建立imu阵列导航系统的误差方程，选择以各个导航子系统间的误差量作为状态量，建立基于卡尔曼滤波器的间接法滤波，卡尔曼滤波器的输入为两个惯性导航系统对同一导航参数测量输出的差值，经过迭代的滤波计算，估计出各误差量；

设惯性导航系统中由误差构成的状态向量xk为：

其中，δmk由惯性导航系统的位置误差δs、速度误差δv和姿态角误差δψ组成，δbk为惯性传感器的误差，由加速度计的比力误差δf和陀螺仪偏差δw组成；

则惯性导航系统的误差状态空间表达式为：

xk+1＝φkxk+gkek(2)

其中，φk为状态转移矩阵，gk为过程噪声增益矩阵，ek为惯性传感器测量噪声，协方差矩阵为e{·}表示期望函数；

借助惯性导航系统的解算得到惯性阵列中单个惯性传感器对位置的估计，利用imu阵列中任意两个惯性传感器位置估计的差，得到位置向量zk为：

zk＝hkxk(3)

其中，hk为测量矩阵，

hk＝[ii0i,j-i](4)

其中，i和j分别为位置向量zk和状态向量xk的维数；

步骤三、建立imu阵列导航系统含有时间同步误差的系统方程，imu阵列中第一惯性传感器与第二惯性传感器之间存在时间同步误差td，设载体的轨迹方程为s(t)，则第二惯性传感器在采样瞬间k时的位置估计值为：

其中，wk为第二惯性传感器的位置估计误差，协方差为由于ts相比td是一个小量，在t＝kts附近对s(kts-td)进行二阶泰勒展开为：

其中，sk、vk、ak分别表示采样瞬间k时载体的位置、速度、加速度，

将式(6)代入式(5)得到：

基于式(7)，反馈给闭环系统的真实位置向量可以描述为：

其中，为第一惯性传感器在采样时刻瞬间k时的位置估计值；

定义其中xf,k为引入的辅助变量，将其代入式(8)得到关于同步误差系统的位置向量方程：

式(9)中定义dk为位置误差偏移向量：

对比式(9)和式(3)可知，同步误差系统的位置向量不仅包含位置误差hkxf,k和第二惯性传感器的位置估计误差wk，还包含一个位置偏置项dk，该位置偏置项包含时间同步误差td和惯性导航系统的动态特性；

步骤四、建立imu阵列导航系统中imu阵列的时间同步模型，将时间同步误差td作为状态变量引入到状态空间模型中，记时间同步误差td的估计值为惯性导航系统解算过程中用于控制惯性传感器数据的移位操作；

设定扩展后的状态向量xd,k为：

其中，表示在k时刻估计的延时误差，则惯性导航系统的空间表达式为：

xd,k+1＝φd,kxd,k+gd,kek(12)

其中，

位置向量方程由第一惯性传感器和第二惯性传感器估计位置的差给出，其表示形式为：

其中，为第一惯性传感器数据延时秒的位置解算结果；

在处对zd,k进行二阶泰勒展开，得到：

测量矩阵hd,k定义为:

hd,k＝[hkvk](15)

则位置向量方程表示为：

zd,k＝hd,kxd,k+wk+d'k(16)

其中，

步骤五、步骤四假设了第一惯性传感器数据在时间上进行了秒的移位，然而受惯性导航系统硬件时间戳的限制其无法实现，因此采用分数延时滤波器对同步误差td的估计值为进行时间上的移位，分数延迟滤波器的理想频率响应由线性相位全通滤波器给出，其表达式为:

h(e^j2πv)＝e^j2πvdv-normalizedfrequence(17)

其中，j是虚数单位，v是归一化频率，d是移位因子，为需要移位的时间，取

由于分数延迟滤波器为无限脉冲响应的非因果滤波器，无法在实际应用中实现，需对理想分数延迟滤波器进行近似处理，而拉格朗日插值法是分数延迟滤波器设计的常用技术，因此采用拉格朗日插值法插值分数延时滤波器的幅值和相位响应，其中分数延迟滤波器的滤波系数表达式为：

其中，n是滤波器的阶数，且d∈[(n-1)/2,(n+1)/2]为得到的延时；

步骤六、对延时后的惯性传感器数据进行误差补偿，并将经过时间移位且误差补偿后的惯性传感器数据输入到惯性导航系统的导航方程中进行导航求解，得到实际的导航参数。

进一步，步骤五中，当延时d大于(n+1)/2时，在分数延时滤波器前添加单位延时，使d∈[(n-1)/2,(n+1)/2]。

为了使imu阵列导航系统中每个惯性传感器达到理想的时间同步，本方法构造的闭环松耦合反馈滤波框架如图1所示，首先，把时间同步误差作为状态变量包含在卡尔曼滤波系统的状态空间模型中，然后将由卡尔曼滤波系统估计的时间同步误差反馈到分数延时滤波器中，进而用分数延时滤波器对惯性传感器的数据进行时间移位，从而将延时后的惯性传感器数据进行误差补偿，最后将经过时间移位且补偿后的惯性传感器数据输入到导航方程中进行导航求解，得到估计的导航状态。

图1中imu1和imu2分别为第一惯性传感器和第二惯性传感器，表示被估计的导航状态，表示被估计的传感器误差，和分别表示传感器误差的估计和导航状态的估计，hk表示测量矩阵，和分别表示imu1和imu2在k时刻的位置估计，zd,k表示imu1和imu2在k时刻位置估计的差也就是卡尔曼滤波系统中的观测量，kk表示卡尔曼滤波器中的增益，表示经过扩展卡尔曼滤波系统估计的时间同步误差的估计误差，为经过扩展卡尔曼滤波估计的惯性传感器的误差，表示经过扩展卡尔曼滤波系统估计的位置误差速度误差姿态角误差δψk组成的导航误差估计，表示imu1的测量数据，表示经过分数延时后的数据，表示经过误差补偿的imu1的数据，函数f(·)表示导航方程，d(·)表示分数延时滤波函数，g(·)表示惯性传感器误差补偿。

图2和图3分别为分数延迟滤波器阶数n＝3的拉格朗日插值法插值分数延时滤波器的幅值和相位响应延时，由于惯性传感器具有低通滤波特性，因此利用拉格朗日插值法的低通特性作为惯性传感器数据的理想分数延迟滤波器的近似是较好的选择。

对本方法进行仿真实验和结果分析，在实验阶段，首先对惯性传感器阵列进行校准，并给出了校准后的结果。然后利用matlab仿真工具模拟具有时间同步误差的惯性阵列的运动情况，利用本算法估计matlab工具模拟惯性阵列中的时间同步误差，并给出时间误差补偿后的导航结果。为了进一步说明本算法的有效性，在matlab仿真工具中刻意增加各个惯性传感器的延时时间，仿真结果表明本算法仍能很好的估计出所延时的时间。

在惯性传感器阵列校正阶段，为了平均随机误差和未建模的误差，采用一个具有20面的校正载体，在校正过程中它能够提供20个均匀的方向，从而在很大程度上平均了惯性传感器阵列中的随机误差和未建模的误差。校正过程将多面体的每个面静止5秒收集数据，最后利用高斯牛顿法对校正参数求解。图4和图5给出了惯性传感器阵列校正后陀螺仪和加速度计的输出，所示的数据是惯性传感器阵列经过校准后，让惯性传感器阵列静止正面朝上采集的数据，由于惯性传感器阵列嵌入式板子的正反面都嵌有惯性传感器，因此输出的数据是关于x轴对称的，从图中可以看出，经过校准之后陀螺仪的输出误差为0-2deg/s，加速度计在静止情况下的z轴输出为9.9m/s²，x和y轴的输出接近0m/s²。

然后对含有四个惯性传感器的imu阵列进行了运动状态的仿真估计，在仿真过程中假设四个惯性传感器之间存在不同的时间延时。具体地，以第一惯性传感器作为imu阵列的时间基准，即假设第一惯性传感器imu1延时零秒，然后假设第二惯性传感器imu2具有0.01秒的时间延时，第三惯性传感器imu3具有0.015秒的时间延时，第四惯性传感器imu4具有0.02秒的时间延时。仿真结果如图6所示，该图展示了各个惯性传感器对实际运动的估计，不难看出随着时间的增长，估计值已经远远的偏离真实的轨迹。进而，图7所示是采用本算法对第二惯性传感器、第三惯性传感器和第四惯性传感器的延时估计，从图7中可以看到，本算法不仅具有良好的收敛特性，同时估计的精度也达到了毫秒级别。

然后采用本算法提出的闭环松耦合反馈系统分别对由imu2与imu3和imu4与imu1所构成的闭环导航系统对运动状态的位置估计，图8给出了使用本算法前后的运动状态估计，其中imu12、imu13、imu14分别表示imu2与imu3和imu4与imu1构成的闭环松耦合反馈系统对运动状态的位置估计。图9中的线条1是imu阵列中采用本算法得到运动状态位置估计的均值结果，线条2则是imu阵列中没有考虑时间同步问题的运动状态位置估计的均值，从图9可以看到，时间同步误差对系统的导航性能影响较大，本算法在一定程度上减小了由于惯性传感器的时间不同步造成的导航精度下降。

为了测试本算法对imu阵列中时间同步误差的估计，在仿真实验中又刻意增加了不同惯性传感器之间的时间同步误差的数值，对imu2进行了0.02s和0.03s的延时，对imu3行了0.025s和0.05s的延时，对imu4进行了0.03s和0.09s的延时，分别得到图10和图11的仿真结果示意图，从图10和图11中可以看出本算法不仅对时间延时的估计有很好的收敛特性，还能对所估计的延时达到了很好的估计效果，同样达到了毫秒级别的估计精度范围。

本算法研究了imu阵列导航系统中时间同步误差的问题，选择以惯性阵列组合导航系统中导航子系统的误差量作为状态量的间接滤波法，给出以误差作为卡尔曼滤波状态变量的空间表达式和观测方程，提出将时间同步误差作为状态变量包含在卡尔曼滤波器中，并通过卡尔曼滤波器来估计时间同步误差；在得到时间同步误差的基础上，采用分数延迟滤波器对惯性传感器数据进行时间移位并采用拉格朗日插值的方法来保证不同惯性传感器采集数据的时间同步。经过仿真实验可以得到本算法不仅能很好的估计不同惯性传感器的时间同步误差，还能进行数据的时间同步，从而提高了imu阵列导航系统的导航精度。