基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法

1.本发明涉及sins/gnss组合导航系统信息融合和滤波领域，尤其是一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法。


背景技术：

2.组合导航，是将多种导航系统按某种方式组合为一种系统的导航方式，从出现至今因其结合多种传感器数据融合以获得更高导航定位精度的特性而一直属于导航领域的重点研究方向。高精度导航定位效果一方面取决于组合导航中各传感器的信号质量，另一方面则与组合导航所采用的融合滤波算法的精度和鲁棒性密不可分。因此在保证传感器精度的前提下，设计合适的滤波算法提高组合导航系统对外界干扰和内部扰动的鲁棒性，使系统能够面对各种可能的干扰仍能稳定运行具有重要意义和研究价值。
3.近年来，随着组合导航系统的广泛应用，人们开始重视起组合导航系统在实际应用中遇到的各种问题，比如载体运行时环境变化引起的外部扰动、gps等量测传感器信号易受干扰、系统运行时各传感器内部产生的未知粗差以及滤波解算时系统建模不准确引入的误差等等。为了解决这些问题，组合导航鲁棒滤波算法逐渐成为学界重点研究领域之一。但迄今为止的各类研究中，学者们主要针对噪声特性变化或噪声特性未知情况下的组合导航系统鲁棒滤波进行了研究，并且研究主要集中在量测信号出现异常时的情况，没有考虑系统模型除噪声以外的不确定性，也没有充分考虑到实际应用中量测与系统本身同时出现异常的情况。
4.未知输入滤波指在系统含有模型完全未知的输入时，对状态和未知输入同时实现估计的滤波方法，其常用于环境监测、导航与定位等领域中。在导航领域以外，针对未知输入滤波的研究已有了许多成果，从最初的kitanidis提出的无偏最小方差线性估计方法，到后来gillijns等人不断完善总结的三步迭代滤波算法，以及近年来的无导数卡尔曼滤波算法、扩展kitanidis卡尔曼滤波器、基于无迹卡尔曼滤波的未知输入无偏最小方差估计方法、基于自校准技术的无迹卡尔曼滤波算法等。上述各类方法，大多对系统有诸多要求，例如系统完全可观、未知输入维数要小于量测维数以及滤波过程中各环节频率一致等限制条件，而这些条件在常用的sins/gps组合导航系统中往往是无法满足的。


技术实现要素：

5.为了克服现有技术中存在的上述问题，本发明提出一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法。
6.本发明解决其技术问题所采用的技术方案是：基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法，包括以下步骤：
7.步骤一：考虑未知输入d
k-1
∈r
p
和ek∈rm的存在，建立sins/gnss组合导航系统的滤波模型；
8.步骤二：根据量测矩阵hk将d
k-1
分解为可观和不可观的两种未知输入；
9.步骤三：根据指数加权滑动窗估计值、历史均值与方差判断未知输入存在位置；
10.步骤四：利用新息和状态自校准技术设计出未知输入估计器并对系统状态和量测进行修正。
11.上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法，所述步骤1具体方法为：
12.xk＝f
k|k-1
x
k-1
+m
k-1dk-1
+γ
k-1wk-1
13.zk＝hkxk+n
kek
+vk14.式中xk表示系统的状态，zk表示系统量测，d
k-1
和ek表示未知输入干扰向量；f
k|k-1
表示sins的误差一步转移矩阵，由sins的各误差方程(组)系数决定；hk为系统量测矩阵；m
k-1
和nk表示列满秩的未知输入干扰的系数矩阵；w
k-1
表示系统噪声向量，γ
k-1
表示系统噪声分配矩阵，vk量测噪声向量；
15.系统状态的选取如下所示：
16.xk＝[φ
t (δvn)
t (δp)
t (εb)
t (
▽b)
t
]
t
[0017]
其中，φ表示3维姿态失准角误差，δvn表示3维速度误差，δp表示3维位置误差，εb表示3维陀螺随机常值漂移，表示3维加速度计随机常值漂移，共15维状态量；
[0018]
系统量测的选取如下所示：
[0019][0020]
其中，表示sins解算出的位置信息；表示gps接收机给出的定位信息；
[0021]
根据惯性测量单元的误差分析和sins机械编排可以得到忽略高阶小量后的sins姿态误差方程、速度误差方程和位置误差方程(组)分别如下：
[0022][0023][0024][0025]
其中，为sins姿态误差方程，为sins速度误差方程，为sins位置误差方程，φ为3维姿态失准角误差，εb为3维陀螺随机常值漂移，δvn表示3维速度误差，δp表示3维位置误差，表示3维加速度计随机常值漂移；
[0026]
根据(1)-(3)组成的误差模型可以得到：
[0027][0028]
将f(t)离散化可得：
[0029]fk|k-1
≈i
15
×
15
+f(t)
×
δt
[0030]
其中f
k|k-1
表示sins的误差一步转移矩阵，δt为sins采样周期，i
15
×
15
表示15维单位矩阵。
[0031]
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法，所述步骤2
具体方法为：
[0032]
根据量测矩阵hk将d
k-1
分解如下：
[0033][0034]
式中，表示存在于可被直接观测状态中的未知输入干扰向量，对于分解后的其系数矩阵由下式得到，且
[0035][0036]
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法，所述步骤3的具体方法为：
[0037][0038][0039]
wi＝α(1-α)i,i＝0,1,
…
l-2,l-1
ꢀꢀꢀ
(6)
[0040]
式中l表示滑动窗长度，表示k时刻第j维状态的指数加权均值，表示k时刻第j维量测的指数加权均值；wi表示权重；α为权重因子，取值范围为[0,1)；
[0041]
根据式(4)-(6)及对应窗口中的加权数据的方程，当某一维存在未知输入干扰时，其对应的均值必然会在上一时刻均值邻域之外，若对应的均值在邻域范围之内，则不存在未知输入干扰，邻域范围表示如下：
[0042][0043]
式中σ为对应窗口中加权数据的标准差；cu为范围系数，取值与量测信号质量呈正相关；表示k+1时刻第j维的指数加权均值；表示k时刻第j维的指数加权均值。
[0044]
上述的一种基于未知输入观测器的组合导航系统鲁棒抗扰滤波方法，所述步骤4具体方法为：
[0045]
量测信息到来时，卡尔曼滤波量测更新之前，对系统进行如下修正：
[0046][0047]
其中，表示第7～9维系统状态量的一步预测值；nk表示量测中列满秩未知输入干扰的系数矩阵；zk表示系统量测；表示含有未知输入干扰的第7～9维系统状态量的一步预测值，表示含未知输入的一步量测值，m
k-1
表示列满秩的未知输入干扰的系数矩
阵；表示可被直接观测的状态中的未知输入的估计值；表示量测中未知输入干扰向量的估计值
[0048]
其中，当d
k-1
＝0而ek≠0时，
[0049]
令
[0050][0051]
式中，hk表示量测矩阵；表示系统状态一步预测值；表示系数矩阵；
[0052]
当d
k-1
≠0且ek≠0时，
[0053][0054][0055][0056]
式中，表示量测的k-1时刻的指数加权均值。
[0057]
量测信息未到来时，卡尔曼滤波时间更新完成后，对系统进行如下修正：
[0058][0059][0060]
式中，为系统状态一步预测值，表示带有干扰的状态一步预测值，表示含未知输入的一步量测值；表示系统状态量的估计值；m
k-1
表示列满秩的未知输入干扰的系数矩阵；f
k|k-1
表示sins的误差一步转移矩阵。
[0061]
本发明的有益效果是，相比传统滤波融合算法和传统鲁棒算法，本发明提出的滤波方法能够对突变型、斜坡型、交叠型、不同特征随机噪声等类型的未知输入干扰进行准确估计，具有更好的滤波估计效果和更强的鲁棒抗扰性，能够提高组合导航系统针对未知输入干扰的抗扰能力。
附图说明
[0062]
下面结合附图和实施例对本发明进一步说明。
[0063]
图1为仿真实验设计的仿真轨迹；
[0064]
图2为在不含有未知输入干扰时正常滤波的卡尔曼滤波、基于sage-husa自适应滤波的自适应卡尔曼滤波和本发明方法的滤波结果中的定位误差曲线图；
[0065]
图3为含有未知输入干扰时的卡尔曼滤波、基于sage-husa自适应滤波的自适应卡尔曼滤波和本发明方法的滤波结果中的定位误差曲线图；
[0066]
图4为本发明方法中利用新息得到的纬度和经度未知输入干扰估计曲线；
[0067]
图5为本发明方法中利用自校准技术得到的姿态角未知输入干扰估计曲线；
[0068]
图6为本发明方法中利用自校准技术得到速度未知输入干扰估计曲线；
[0069]
图7为采用车载实验数据的含有未知输入干扰时的卡尔曼滤波、基于sage-husa自
适应滤波的自适应卡尔曼滤波和本发明方法的滤波结果中的定位误差曲线图；
[0070]
图8为采用车载实验数据时本发明方法中利用新息得到的纬度和经度未知输入干扰估计曲线；
[0071]
图9为采用车载实验数据时本发明方法中利用自校准技术得到的姿态角未知输入干扰估计曲线；
[0072]
图10为采用车载实验数据时本发明方法中利用自校准技术得到的速度未知输入干扰估计曲线；
[0073]
图11为卡尔曼滤波、基于sage-husa自适应滤波的自适应卡尔曼滤波和本发明方法滤波解算轨迹与gps参考轨迹对比图。
具体实施方式
[0074]
为使本领域技术人员更好的理解本发明的技术方案，下面结合附图和具体实施方式对本发明作详细说明。
[0075]
首先，在步骤一中考虑未知输入d
k-1
∈r
p
和ek∈rm的存在，建立sins/gnss组合导航系统的滤波模型，模型形式为状态误差滤波模型。
[0076]
xk＝f
k|k-1
x
k-1
+m
k-1dk-1
+γ
k-1wk-1
[0077]
zk＝hkxk+n
kek
+vk[0078]
式中xk∈rn表示系统的状态，zk∈rm表示系统量测，d
k-1
∈r
p
和ek∈rm表示未知输入干扰向量；f
k|k-1
∈rn×n表示sins的误差一步转移矩阵，由sins的各误差方程(组)系数决定，hk∈rm×n为系统量测矩阵；m
k-1
∈rn×
p
和nk∈rm×m表示列满秩的未知输入干扰的系数矩阵；w
k-1
∈rs表示系统噪声向量，γ
k-1
∈rn×s表示系统噪声分配矩阵，vk∈rm表示量测噪声向量。
[0079]
系统状态量的选取如下所示：
[0080][0081]
其中，φ表示3维姿态失准角误差，δvn表示3维速度误差，δp表示3维位置误差，εb表示3维陀螺随机常值漂移，表示3维加速度计随机常值漂移，共15维状态量。
[0082]
系统量测量的选取如下所示：
[0083][0084]
其中，和分别表示sins解算出的位置新息和gps接收机给出的定位新息，二者均存在一定误差。
[0085]
根据惯性测量单元的误差分析和sins机械编排可以得到忽略高阶小量后的sins姿态误差方程、速度误差方程和位置误差方程(组)分别如下
[0086]
[0087][0088]
上述3式中，φ为失准角误差，表示导航坐标系相对惯性坐标系的角速度在导航坐标系的投影，表示角速度计算误差，为陀螺测量误差，εb为陀螺常值漂移，表示导航坐标系下的比力值，为地球自转角速度计算误差，为导航系旋转角速度计算误差。
[0089]
根据上述3式构成的误差模型可以得到：
[0090][0091]
将上式离散化可得
[0092]fk|k-1
≈i
15
×
15
+f(t)
×
δt
[0093]
其中δt为sins采样周期。
[0094]
至此，结合前文步骤一中的状态方程和量测方程以及各自的选取值情况，则同时在量测和状态中含有不同种类未知输入的sins/gps组合导航系统状态误差滤波模型建立完成。
[0095]
接下来，由于常见未知输入滤波估计算法需要满足充分条件：对于和p都有
[0096]
rank(hkm
k-1
)＝rank(m
k-1
)＝p
[0097]
且有rank(nk)＝m，因此在组合导航系统中需要根据量测矩阵hk将d
k-1
分解为可观和不可观的两种未知输入。
[0098]
首先假设未知输入d
k-1
和ek不会同时存在，并考虑p＞m且ek＝0的情况，根据hk将d
k-1
分解如下：
[0099][0100]
式中，表示存在于可被直接观测状态中的未知输入，有对于分解后的其系数矩阵由下式得到，且有
[0101][0102]
对于分解后的和将满足上述充分条件式的变形式：
[0103][0104]
这表示将原本对于未知输入的估计分为两个部分进行处理，即能够被新息包含的部分和没有包含的部分。对于前者，将直接利用量测信息得到估计值。
[0105]
进一步地，考虑未知输入同时存在于状态和量测中的情况，根据指数加权滑动窗估计值、历史均值与方差判断未知输入存在位置。
[0106]
本方法模型状态和量测中的导航信息均为误差量，理想情况下应为极小值，且为平稳过程，服从正态分布，故考虑利用指数加权滑动窗求得窗口内某一维的均值如下：
[0107][0108][0109]
wi＝α(1-α)i,i＝0,1,
…
l-2,l-1
[0110]
式中l表示滑动窗长度，和分别表示k时刻第j维状态和量测的指数加权均值；wi表示权重；α为权重因子，取值范围为[0,1)。
[0111]
根据式上述3式及对应窗口中加权数据的方差，当某一维存在干扰时，其对应的均值必然会在上一时刻均值邻域之外，邻域范围表示如下：
[0112][0113]
式中σ为上述方差的标准差；cu为范围系数，取值与量测信号质量呈正相关。又模型中包含的是导航信息的误差量，所以理论上不会受到载体机动的影响，故可以根据是否满足在上式所示范围内来判断未知输入干扰的存在位置——若不满足则该维信息存在未知输入干扰，否则就不存在未知输入。
[0114]
进一步地，需要用新息和状态自校准技术得到未知输入估计器并对系统状态和量测进行修正。
[0115]
为了得到项，将系统状态方程带入量测方程中，并且使等号左侧只保留与有关的项，可得：
[0116][0117]
根据分解结果以及上述充分条件的变形式，令
[0118][0119]
并与前式相乘可得
[0120][0121]
进一步可得的估计为：
[0122][0123]
考虑d
k-1
＝0而ek≠0的情况，此时由信息可得未知输入干扰项变为
[0124]nkek
＝z
k-hk(f
k|k-1
x
k-1
+γ
k-1wk-1
)-vk[0125]
由rank(nk)＝m经简单推导可得：
[0126][0127]
当量测信息到来时，基于本发明步骤一种所建立的滤波模型，由上述各式可得到对未知输入干扰较为准确的估计值，基于此可对系统进行如下修正：
[0128][0129]
式中和表示含未知输入的一步预测和量测值，该修正将在卡尔曼滤波量测更新之前完成。
[0130]
对于量测没有包含的未知输入，无法从新息中解出其估计值，需要从系统状态本身解耦出其估计值。根据所建系统模型，可知当k时刻开始存在未知输入干扰时有一步预测如下：
[0131][0132]
其中表示带有干扰的状态一步预测，又知f
k|k-1
可逆，故：
[0133][0134]
由上式可得到未知输入干扰项的估计值如等式左侧，进一步对状态进行修正即可抑制未知输入干扰的影响：
[0135][0136]
上式所示修正将在卡尔曼滤波时间更新后执行。由上述各式可知当系数矩阵m
k-1
为对角阵且其中元素均取1时可得到d
k-1
的准确估计值，否则无法得到干扰的准确估计。当量测新息到来时，将利用自校准方法所得的未知输入估计值中对应维度替换为利用新息所得的估计值，并重记为即为最终估计结果。
[0137]
当d
k-1
≠0且ek≠0时，新息中做差的量测和状态一步预测均有干扰，表示如下：
[0138][0139]
对于平稳信号指数加权滑动窗具有短期预测的能力，因此当d
k-1
≠0且ek≠0时采取作为k时刻量测信号相对可信的参考值，则有：
[0140]
[0141]
根据上述新息公式有：
[0142][0143]
将上述2个调整后的未知输入干扰估计值代入前文中量测新息到来时的状态和量测修正式即可实现对系统的修正。
[0144]
为了验证本发明的有效性，通过设计对应的sins/gps松组合导航系统的仿真实验以及利用车载实验数据从系统抗扰种类、未知干扰估计结果以及组合导航结果误差等方面验证本发明在理论与实际应用中的有效性。
[0145]
仿真条件设置如下：
[0146]
为了使仿真实验贴近实际情况，设计的仿真轨迹包括加速、减速、匀速、直线、转弯以及转圈等部分，共持续672秒，轨迹如图1所示。设置陀螺零偏为随机游走为0.001
°
/h；加速度计零偏为100μg、随机游走为初始俯仰、横滚和航向失准角分别为0.5'、-0.5'和20'，速度误差均为0.1m/s，位置误差均为3m；系统噪声标准差和量测噪声标准差参考上述各参数设置；sins采用2子样误差补偿方法解算，计算输出频率为50hz，gps接收机输出频率为1hz。由于仿真为相对理想的情况，故设置cu＝1。为方便验证算法，仿真中设置m
k-1
与nk对角线元素均为1，其余元素为0，且列满秩。
[0147]
由图2可见，在不含未知输入干扰时，卡尔曼滤波(kf)、基于sage-husa自适应滤波的自适应卡尔曼滤波(下文中简称akf)与本发明(下文中简称srkfuie)的估计结果和误差精度差别不大，其中本发明在对状态和量测进行修正时会在未知输入估计及系统修正处引入极小误差导致滤波误差较前两者稍大，但在可接受范围内。
[0148]
对于同样的仿真轨迹参数，注入未知输入干扰后采用3种算法进行组合导航滤波，结果如图3所示。由图3可知kf由于没有鲁棒性，在250s处加入多种干扰后滤波误差瞬间增大，滤波发散。akf虽然通过自适应调整rk和qk使得对于噪声中的干扰具有一定的鲁棒性，但表1所示多种干扰还包括野值等非噪声类型。由于akf自适应是基于新息的，状态和量测同时存在误差会导致新息失去反应误差干扰的能力，这会导致akf的自适应rk和qk引入误差，因此akf的滤波误差会显著大于kf。本发明srkfuie对状态中和量测新息中的干扰均有估计和相应的快速修正处理，但基于新息的未知输入干扰估计和处理会慢1拍，因此图3中本发明srkfuie会在有未知输入干扰的时段出现定位误差的波动，但误差值大多在
±
1m以内，可以接受。
[0149]
本方法对于未知输入干扰的估计结果如图4～图6所示，其中dei,i＝1,2对应利用新息得到的未知输入干扰的估计结果，当d
k-1
≠0且ek≠0时表示状态中未知输入干扰与量测中未知输入干扰二者估计之和；dki,i＝1,2,
…
,8表示在系统状态中存在的未知输入干扰，eki,i＝1,2,3表示在系统量测中存在的未知干扰，二者对应于预先的干扰设置；dexi,i＝1,2,
…
,6对应利用自校准得到的未知输入干扰的估计结果，dezi,i＝1,2,3对应式两种未知输入同时存在时的量测未知输入估计结果。通过图4～图6的结果，与预设未知干扰参数进行对比，可见未知输入干扰估计结果较为准确。由上述各图可以看出本发明在系统存在未知输入干扰时仍具有较好的滤波结果，能够实现对未知输入干扰的估计和抑制。
[0150]
为了检验本发明对真实环境下导航数据的有效性，设计并进行了车载实验，并对车载实验数据进行半实物实验验证。实验设备包括汽车，gps接收机、高精度imu以及上位
机，实验地点位于辽宁鞍山。实验中参数参考实验仪器参数设置，如下所示：imu参数设置与仿真实验中相同；gps东向、北向定位误差设置为2m，高度定位误差设置为3m；系统噪声标准差和量测噪声标准差参考上述各参数设置。sins解算频率与gps输出频率与仿真实验中设置均相同。由于实验环境较为复杂，故设置cu＝3。m
k-1
与nk的设置与仿真实验相同。在本实验中，采用精度较高的gps数据作为参考数据计算不同组合导航滤波算法的定位误差。
[0151]
图7中在200s附近东向定位误差出现剧烈波动，该误差由gps经度信号波动引起。此时系统中尚未注入未知输入干扰，属于原始数据自身耦合的小数值未知干扰，akf与kf对其都具有一定程度的抑制，但作用并不明显。本发明srkfuie能够检测出该处干扰(于图8经度未知输入估计中有所体现)大小及存在位置，并及时对系统进行修正，使得定位误差绝对值小于akf和kf，从另一方面验证了本发明的有效性。
[0152]
图8～图10中各图例符号含义与仿真实验中相同，可见本发明应用于实验数据中也能较准确的得到未知输入的估计。200s处由于gps经度信号出现剧烈波动被本算法检测到并作为了未知输入进行处理，因此图8对应时间处出现了估计值的波动，这一现象符合实际情况，是正常的。图11显示出3种算法滤波解算轨迹与gps轨迹对比情况，可以看出在没有未知输入干扰时3那种算法轨迹几乎重合，当干扰到来后kf和akf结果马上出现偏移，本发明结果稍有偏差(《5m)后快速恢复正常。
[0153]
综上，相比传统滤波融合算法和传统鲁棒算法，本发明提出的滤波方法能够对突变型、斜坡型、交叠型、不同特征随机噪声等类型的未知输入干扰进行准确估计，具有更好的滤波估计效果和更强的鲁棒抗扰性。
[0154]
以上实施例仅为本发明的示例性实施例，不用于限制本发明，本发明的保护范围由权利要求书限定。本领域技术人员可以在本发明的实质和保护范围内，对本发明做出各种修改或等同替换，这种修改或等同替换也应视为落在本发明的保护范围内。