CN115014347A - 一种快速可观测度分析及其指导的多传感器信息融合方法 - Google Patents

Abstract

本发明公开了一种快速可观测度分析及其指导的多传感器信息融合方法，包括步骤A：设计低运算复杂度的可观测度分析算法；步骤B：设计基于可观测度的信息融合机制，并将其集成至多传感器信息融合框架中。本发明与传统算法相比，新算法中的可观测度定义更加合理，且能从理论上设定阈值来判定某个状态量为可观测、弱可观测或不可观测，其低运算复杂度的特性也便于设计基于可观测度指导的多传感器信息融合机制。此外，新方法中的多传感器信息融合部分能够基于传感器的类别、状态量的类别和可观测度对融合框架中的信息分配进行指导，进一步完善了对局部传感器和全局传感器的测量信息融合机制，有效地提升了组合导航系统的综合性能。

Description

一种快速可观测度分析及其指导的多传感器信息融合方法

技术领域

本发明涉及组合导航领域，特别涉及一种快速可观测度分析及其指导的多传感器信息融合方法。

背景技术

在环境复杂的场景中，单一的传感器通常无法在环境变化时持续正常工作，如GNSS在室内无法接收到卫星信号，视觉系统在昏暗或纹理特征缺乏的场景中难以提取足够的视觉特征用于位姿估计，三轴磁强计的测量模型易受外部磁场干扰而变化。通过组合多种传感器，可以一定程度上提高导航系统的可靠性，降低导航系统失效的风险。

然而，当组合导航系统中传感器的组合方式发生变化时，导航系统对各个待估计状态量的估计能力也会相应的发生改变(载体运动状态的改变也会影响部分状态量的估计性能)。特别是在某些传感器失效时，多个状态量可能彻底地无法被有效估计，这将对组合导航系统产生难以预测的影响。

为了研究组合导航系统对各个状态量的估计能力，并为组合导航系统的设计提供理论指导，R.E.Kalman最早提出可观测性的概念。经过学术界和工业界的不断研究和实际验证，关于可观测性的理论研究成果日益丰富，也被越来越多地应用于各种组合导航系统的设计中。

对于线性时不变系统，其可观测性分析算法较为简单，可以通过系统能观性矩阵的秩来判断系统的各个状态量是否具备足够的可观测性，该算法与控制系统中的能观性判别算法类似，后者也已经成为现代控制理论中的基础算法之一。对于“时变”系统，Goshen提出了“分段线性定常”(Piece Wise Constant System，PWCS)的思想，将时变系统在一个较短的时间片段内近似视为时不变系统，这一思想对可观测度的后续研究工作具有深远的影响。

对于“非线性”系统，Dafis基于李导数将非线性模型进行局部线性化处理，但是采用非线性函数求解李导数的运算复杂度较高，更多的研究者倾向于直接使用一阶泰勒公式将非线性模型近似为线性模型，而后再对其进行可观测性分析。

系统的可观测性反应了系统在有限时间内根据已有的测量信息估计出系统状态量的能力，仅能定性地判定系统的某个状态量是否具有足够的可观测性。为了能够对系统各个状态量的可观测性进行度量，研究者基于不同算法定义了可观测度的概念，为设计基于可观测度指导的组合导航系统提供了更丰富的理论工具。

典型地，Ham基于状态估计协方差矩阵的特征值和特征向量定义了系统的可观测度，但是可观测度的计算需要在滤波之后才能进行，无法在滤波前对滤波过程进行指导；程向红首次使用奇异值分解(Singular Value Decomposition，SVD)对能观矩阵进行处理，并结合能观矩阵的奇异值对系统的可观测度进行讨论分析，这一算法目前已成为可观测性分析和可观测度计算的主流算法，但该算法的分析结果会受各个状态量的量纲的影响，马艳红更是针对性地提出了基于SVD理论的可观测度分析算法的若干反例；陈雨从系统初始状态的最小二乘估计出发，基于估计误差传递矩阵的对角元素的衰减情况定义各个状态变量的可观测度，其计算过程依赖滤波初始时的协方差矩阵，且计算结果会随时间推移而不断减小；Dong基于能观矩阵的伪逆分析并计算各个状态量的不可观测度，但是计算矩阵伪逆的运算量极大，不适用于对导航系统的可观测性进行在线分析；Hong和Ge基于加权最小二乘算法对导航系统进行可观测度分析，并从理论上阐释了可观测度与状态估计精度之间的关系；Shen基于能观矩阵的伪逆矩阵与测量向量对可观测度进行了重新定义，相比于SVD算法计算所需的内存更少，但是计算结果依然会受状态向量的量纲的影响；此外，Ge基于李导数构造了非线性状态估计问题的伪状态转移矩阵和伪观测矩阵，理论上比传统算法中的一阶线性化近似具有更高的近似精度，且在后续的研究工作中专门设计了针对无迹卡尔曼滤波的可观测度分析算法，但其计算过程依然是基于对能观矩阵进行奇异值分解实现。

虽然研究者已提出了多种可观测度的定义与分析算法，但是现有算法依然存在以下问题：

(1)可观测度的计算结果包含状态向量的量纲信息，但是不同量纲的状态量之间在理论上不具有可比性；

(2)部分可观测度的分析算法需要在滤波结束后才能进行，无法在滤波前帮助研究者设计滤波器；

(3)现有的可观测度的定义方式无法给出判定某个状态量是否可观测的理论阈值，研究者只能凭借经验判定，并进一步将不可观测或弱可观测的状态量从状态向量中移除；

(4)运算复杂度高，上述算法中涉及到的SVD、伪逆、李导数等运算均需要较多的计算资源，故以其为基础设计的基于可观测度指导的自适应导航系统，仅适合在运算能力较强的机载计算平台上运行。

此外，在结合实际工程需求设计组合导航系统时，现有的融合框架存在以下不足之处：

(1)各个传感器的测量信息参与融合时的信息分配机制不合理。现有的信息分配机制主要基于传感器测量信息的噪声统计特性(在联邦滤波器中也与信息分配系数有关)，而在导航系统实际运行时，部分传感器可能会间隙性地失效或重新使能，导致部分状态量的可观测性发生变化。根据可观测性理论，状态量的可观测性会极大地影响其估计精度，虽然已有部分研究工作设计了基于可观测度自适应调整融合过程的导航系统，但是所设计的自适应机制并没有考虑不同状态量之间的重要性的差异，如多旋翼无人机水平姿态角的重要程度远高于IMU的零偏。

(2)局部和全局测量属性的传感器测量值的融合机制不完善。相机、激光雷达和INS等局部传感器的测量值仅能估计载体在两个时刻间的相对位姿，而GNSS、磁强计等全局传感器则可以直接测量得到载体在导航坐标系中的全局位置、航向角等信息。在两类传感器同时正常工作时，所有传感器的测量值均可以基于自身权重进行加权融合；但是，当全局传感器因故失效一段时间后再次重新使能，并试图与局部传感器的测量值相融合时，现有的加权融合机制会花费较长时间将融合得到的导航结果收敛至全局解。

在上述研究基础上，本专利申请使用一种新颖的、低运算复杂度的可观测度分析算法，能够实现快速在线计算；且将其应用于多传感器信息融合框架来构建导航系统，提高导航系统的稳定性。

发明内容

为了克服现有技术中的不足，本发明提供一种快速可观测度分析及其指导的多传感器信息融合方法，该方法中对可观测度的定义更加合理，不仅与状态量的量纲无关，而且能够以较低的运算量快速、实时地计算导航系统中各个状态量的可观测度，并在线指导多传感器信息融合的过程，提高导航系统在复杂环境中的可靠性。

为了达到上述发明目的，解决其技术问题所采用的技术方案如下：

一种快速可观测度分析及其指导的多传感器信息融合方法，包括以下步骤：

步骤A：设计低运算复杂度的可观测度分析算法，结合待估计状态向量的通解公式，引入QR分解降低对条带化可观测性矩阵进行分析的运算量，并给出相应的可观测度定义、计算公式及可观测度判定阈值；

步骤B：在步骤A的基础上，设计基于可观测度的信息融合机制，并将其集成至多传感器信息融合框架中，各个子滤波器的状态估计结果向主滤波器传输前，需要预先判别各个状态量是否可观测，仅将可观测状态量的估计信息传输给主滤波器；主滤波器将全局滤波的结果反馈至各个子滤波器之前，需要预先判别主滤波器所估计的各个状态量是否融合了某一子滤波器所对应的状态量，仅将状态量中能够被某一子滤波器有效估计的状态量反馈至所有子滤波器中。

一实施例中，步骤A包括以下步骤：

步骤A.1：针对滤波算法的可观测度分析算法设计，考虑有以下的离散线性系统：

其中，x_k∈Rⁿ为系统在t_k时刻的状态向量，维度为n；Φ_k,k-1∈R^n×n为t_k-1时刻到t_k时刻的系统状态转移矩阵；z_k∈R^m为系统测量向量，维度为m；H_k为测量矩阵；v_k为测量值的噪声，满足零均值高斯白噪声模型，且有E[v_k]＝0，

w_k-1为模型噪声，与v_k类似，也满足零均值高斯白噪声模型，且有E[w_k]＝0，

根据上段所定义的离散线性系统，引入如下方程计算k时刻的条带化可观测性矩阵：

Z_k＝O_kx_k+V_k,

其中，矩阵O_k表示k时刻的SOM，且有：

其中，V_k的计算依赖于v_k和w_k，都服从零均值高斯白噪声的假设；此外，由于x的维度是n，Z的维度是m，故矩阵O_k是一个mn×n的矩阵，即O_k∈R^mn×n；

基于SOM的计算公式，可以得到关于x_k的通解：

其中，

是O_k的伪逆，α是一个随机的实数向量(α∈Rⁿ)；然后，记：

其中，Δo_k,i是ΔO_k的第i行；同时，定义：

x_k＝[x_k,1 x_k,2 … x_k,N]^T,

则x_k,i可以由两部分组成，具体表达如下：

其中，

可以由

明确计算得到；Δo_k,iα_i是受随机量α_i影响的部分；

如果Δo_k,i的2-范数||Δo_k,i||₂为零或无限接近零，则随机量α_i将几乎不会影响x_k,i，即x_k,i可以被确定性的求解；否则，x_k,i的求解过程会受到随机量α_i的影响，意味着x_k,i会有无数个解；

基于Δo_k,i定义x_k,i的可观测度，由于计算Δo_k,i时需要计算SOM的伪逆矩阵

这将占用过多的计算资源，为了提高运算效率，避免直接计算SOM的伪逆矩阵

考虑将公式中的

改写为：

对矩阵O_k进行QR分解，即：

O_k＝QR,

其中，R∈R^n×n是一个上三角矩阵，Q∈R^mn×n是一个正交矩阵，即满足Q^TQ＝I_mn×mn；

将上述QR分解带入x_k的通解公式中，可以得到：

注意到(R^TR)可能会是奇异矩阵，无法按照上式进行求逆运算，故对上式进行如下改进：

其中，λ是一个较小的正实数，可以避免求逆运算失败的情况；

矩阵

的计算仅与上三角矩阵R有关，根据上述分析，定义系统各个状态量的可观测度为：

范数运算。

一实施例中，步骤A还包括以下步骤：

步骤A.2：针对优化算法的可观测度分析算法设计，记一个优化问题的整体代价函数为F(x)，并对其进行一阶的泰勒展开：

F(x+Δx)≈f(x)+J(x)Δx,

其中，J(x)为F(x)关于x的雅各比矩阵，为求得x的最优估计值，则需要在每次迭代计算中寻找合适的增量Δx，使得‖F(x+Δx)‖²达到最小；

在增加置信域约束项之后，可将原问题转化为以下优化问题：

arg min_Δx‖f(x)+J(x)Δx‖²,s.t.Δx^TΔx≤d,

其中，d表示约束增量Δx的范围，且对于每一步的迭代计算，d是一个已知量；

对上式所描述的约束优化问题求解需要分两种情况进行考虑：

情况1：假设所求的Δx位于Δx^TΔx＜d的范围内，则此时求得的结果等同于无约束情况下的最优解；

情况2：假设所求的Δx位于Δx^TΔx≥d的范围内，则将优化问题视为带等式约束的优化问题：

arg min_Δx‖f(x)+J(x)Δx‖²,s.t.Δx^TΔx＝d,

引入拉格朗日乘数λ构建如下方程：

L(Δx)＝‖f(x)+J(x)Δx‖²+λ(Δx^TΔx-d).

此时对上式求Δx的偏导数并令其等于0，可以得到Δx的计算公式：

(J(x)^TJ(x)+λI)Δx＝J(x)^TF(x).

将J(x)^TJ(x)记为H，将J(x)^TF(x)记为Z，并将λI从方程中删除，可以得到：

HΔx＝Z,

则待估计的状态向量Δx的通解为：

Δx＝H⁺Z+(I-H⁺H)α,

其中，H⁺是H的伪逆，α是一个维数与状态向量相同的实数随机向量，与步骤A.1所提出的可观测度定义及推导公式类似，可以得到各个状态量的可观测度为：

其中，r_(i,j)的计算与步骤A.1中所对应变量的定义相同。

一实施例中，步骤A还包括以下步骤：

步骤A.3：设定可观测度阈值，结合步骤A.1和A.2，本步骤给出了判别系统状态量是否可观测的理论阈值，从而将系统状态量划分为：可观测状态量，弱可观测状态量和不可观测状态量；

定义矩阵R的秩为：

Λ＝rank(R,TOL),

其中，Λ是矩阵R中大于TOL的奇异值数目，且通常TOL在矩阵奇异值分解时被设定为：

其中，

是矩阵R的最大奇异值；

是当前系统中表示浮点数1的最大误差；

显然，矩阵R的秩表示了系统状态量中可观测的个数，根据步骤A.1与步骤A.2，已经计算得到了各个系统状态量的可观测度；同时，结合所得到矩阵R的秩为Λ，则可以设定状态量是否可观测的阈值为：

其中，

是第Λ大的可观测度；

至此，可以给出判别系统状态量可观测性的判别规则：

(1)若D_k,i＝0，则第i维的系统状态量是不可观测的；

(2)若

则第i维的系统状态量是弱可观测的；

(3)若

则第i维的系统状态量是可观测的。

一实施例中，步骤B包括以下步骤：

步骤B.1：各个子滤波器的状态估计结果向主滤波器传输前，需要预先判别各个状态量是否可观测，仅将可观测状态量的估计信息传输给主滤波器；

记第i个子滤波器的待估计状态向量为x_i＝[x_ci,x_bi]^T，其中，x_ci是各个子滤波器的公共状态，包括载体的位置、速度、姿态、三轴加速度计的零偏和三轴陀螺仪的零偏；x_bi则是第i个子滤波器专有的状态量；

另记第i个子滤波器公共状态的估计协方差矩阵为P_ii，且各个子滤波对公共状态的估计互不相关，即P_ij＝0(i≠j)，则主滤波器得到的全局最优估计和其协方差矩阵P_g可以表示为：

其中，n为子滤波器的个数，矩阵L_i是一个对角矩阵，对角元素由1或0构成，当第i个子滤波器的某个公共状态量被判定为可观测时，则矩阵L_i中所对应的对角线元素被设置为1，否则需要将其设置为0。

一实施例中，步骤B还包括以下步骤：

步骤B.2：主滤波器将全局滤波的结果反馈至各个子滤波器之前，需要预先判别主滤波器所估计的各个状态量是否融合了某一子滤波器所对应的状态量，仅将状态量中能够被某一子滤波器有效估计的状态量反馈至所有子滤波器中；

主滤波器估计得到的全局估计值x_g及其相应的协方差矩阵P_g需要反馈到各个子滤波对其进行修正，即：

x_i＝x_g,P_ii＝(β_i[M]^-1)P_g,

其中，信息分配系数β_i＝1/n；[·]^-1运算符表示仅对矩阵的对角元素求倒数，矩阵M可以通过下式进行计算：

根据上式可知，若所有的子滤波器均不能为x_g中某一状态量提供足够的可观测度时，则主滤波器对所有子滤波器进行反馈修正时，将会使其在子滤波中对应的协方差变为无穷大，即认为主滤波器所反馈的信息置信度为零。

本发明由于采用以上技术方案，使之与现有技术相比，具有以下的优点和积极效果：

本发明设计了一种快速可观测度分析及其指导的多传感器信息融合方法，与传统算法相比，新算法中的可观测度定义更加合理，且能从理论上设定阈值来判定某个状态量为可观测、弱可观测或不可观测，其低运算复杂度的特性也便于设计基于可观测度指导的多传感器信息融合机制。此外，新方法中的多传感器信息融合部分能够基于传感器的类别、状态量的类别和可观测度对融合框架中的信息分配进行指导，进一步完善了对局部传感器和全局传感器的测量信息融合机制，有效地提升了组合导航系统的综合性能。

附图说明

为了更清楚地说明本发明实施例的技术方案，下面将对实施例描述中所需要使用的附图作简单的介绍。显而易见，下面描述中的附图仅仅是本发明的一些实施例，对于本领域技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。附图中：

图1是本发明一种快速可观测度分析及其指导的多传感器信息融合方法的示意图。

具体实施方式

下面将结合附图对本发明的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

图1为本发明一种快速可观测度分析及其指导的多传感器信息融合方法示意图，其中惯性导航系统(Inertial Navigation System,INS)作为公共参考系统，向各个子滤波器提供载体坐标系下的角速度和加速度信息；各个子滤波器的局部估计值x_i及其协方差矩阵P_i一起输入至主滤波器中进行融合以得到全局最优的导航信息估计值。与传统的联邦滤波器相比，图中所描述的多传感器信息融合框架能根据传感器测量值是否具有全局属性对信息分配机制进行调整，将其他子滤波器的全局估计信息作为约束，使不具有全局测量属性的传感器所构建的子滤波器的估计值也具备全局属性。

具体的，本实施例公开了一种快速可观测度分析及其指导的多传感器信息融合方法，包括以下步骤：

步骤A：设计低运算复杂度的可观测度分析算法，结合待估计状态向量的通解公式，引入QR分解降低对条带化可观测性矩阵(Stripped Observability Matrix,SOM)进行分析的运算量，并给出相应的可观测度定义、计算公式及可观测度判定阈值；

步骤B：在步骤A的基础上，设计基于可观测度的信息融合机制，并将其集成至多传感器信息融合框架中，各个子滤波器的状态估计结果向主滤波器传输前，需要预先判别各个状态量是否可观测，仅将可观测状态量的估计信息传输给主滤波器；主滤波器将全局滤波的结果反馈至各个子滤波器之前，需要预先判别主滤波器所估计的各个状态量是否融合了某一子滤波器所对应的状态量，仅将状态量中能够被某一子滤波器有效估计的状态量反馈至所有子滤波器中。

一实施例中，步骤A包括以下步骤：

步骤A.1：针对滤波算法的可观测度分析算法设计，考虑有以下的离散线性系统：

其中，x_k∈Rⁿ为系统在t_k时刻的状态向量，维度为n；Φ_k,k-1∈R^n×n为t_k-1时刻到t_k时刻的系统状态转移矩阵；z_k∈R^m为系统测量向量，维度为m；H_k为测量矩阵；v_k为测量值的噪声，满足零均值高斯白噪声模型，且有E[v_k]＝0，

w_k-1为模型噪声，与v_k类似，也满足零均值高斯白噪声模型，且有E[w_k]＝0，

根据上段所定义的离散线性系统，引入如下方程计算k时刻的条带化可观测性矩阵(Stripped Observability Matrix,SOM)：

Z_k＝O_kx_k+V_k,

其中，矩阵O_k表示k时刻的SOM，且有：

其中，V_k的计算依赖于v_k和w_k，它们都服从零均值高斯白噪声的假设；此外，由于x的维度是n，Z的维度是m，故矩阵O_k是一个mn×n的矩阵，即O_k∈R^mn×n；

基于SOM的计算公式，可以得到关于x_k的通解：

其中，

是O_k的伪逆，α是一个随机的实数向量(α∈Rⁿ)；然后，记：

其中，Δo_k,i是ΔO_k的第i行；同时，定义：

x_k＝[x_k,1 x_k,2 … x_k,N]^T,

则x_k,i可以由两部分组成，具体表达如下：

其中，

可以由

明确计算得到；Δo_k,iα_i是受随机量α_i影响的部分；

进一步地，如果Δo_k,i的2-范数||Δo_k,i||₂为零或无限接近零，则随机量α_i将几乎不会影响x_k,i，即x_k,i可以被确定性的求解；否则，x_k,i的求解过程会受到随机量α_i的影响，意味着x_k,i会有无数个解；

结合上述原理，可以基于Δo_k,i定义x_k,i的可观测度，由于计算Δo_k,i时需要计算SOM的伪逆矩阵

这将占用过多的计算资源，为了提高运算效率，避免直接计算SOM的伪逆矩阵

考虑将公式中的

改写为：

对矩阵O_k进行QR分解，即：

O_k＝QR,

其中，R∈R^n×n是一个上三角矩阵，Q∈R^mn×n是一个正交矩阵，即满足Q^TQ＝I_mn×mn；

将上述QR分解带入x_k的通解公式中，可以得到：

注意到(R^TR)可能会是奇异矩阵，无法按照上式进行求逆运算，故对上式进行如下改进：

其中，λ是一个较小的正实数，可以避免求逆运算失败的情况；

进一步考察上式可知，矩阵

的计算仅与上三角矩阵R有关，根据上述分析，定义系统各个状态量的可观测度为：

范数运算。

一实施例中，步骤A还包括以下步骤：

步骤A.2：针对优化算法的可观测度分析算法设计，记一个优化问题的整体代价函数为F(x)，并对其进行一阶的泰勒展开：

F(x+Δx)≈f(x)+J(x)Δx,

其中，J(x)为F(x)关于x的雅各比矩阵，为求得x的最优估计值，则需要在每次迭代计算中寻找合适的增量Δx，使得‖F(x+Δx)‖²达到最小；

在增加置信域约束项之后，可将原问题转化为以下优化问题：

arg min_Δx‖f(x)+J(x)Δx‖²,s.t.Δx^TΔx≤d,

其中，d表示约束增量Δx的范围，且对于每一步的迭代计算，d是一个已知量；

对上式所描述的约束优化问题求解需要分两种情况进行考虑：

情况1：假设所求的Δx位于Δx^TΔx＜d的范围内，则此时求得的结果等同于无约束情况下的最优解；

情况2：假设所求的Δx位于Δx^TΔx≥d的范围内，则将优化问题视为带等式约束的优化问题：

arg min_Δx‖f(x)+J(x)Δx‖²,s.t.Δx^TΔx＝d,

引入拉格朗日乘数λ构建如下方程：

L(Δx)＝‖f(x)+J(x)Δx‖²+λ(Δx^TΔx-d).

此时对上式求Δx的偏导数并令其等于0，可以得到Δx的计算公式：

(J(x)^TJ(x)+λI)Δx＝J(x)^TF(x).

上式中的λI项可以一定程度上避免因线性方程组的系数矩阵病态而使得求解过程数值计算不稳定；但是弱可观测或不可观测状态量无法基于代价函数得到有效的、精确的估计，且当它们与其他可观测状态量存在耦合关系时，会极大的影响其他可观测状态量的估计结果；

现将J(x)^TJ(x)记为H，将J(x)^TF(x)记为Z，并将λI从方程中删除，可以得到：

HΔx＝Z,

则待估计的状态向量Δx的通解为：

Δx＝H⁺Z+(I-H⁺H)α,

其中，H⁺是H的伪逆，α是一个维数与状态向量相同的实数随机向量，与步骤A.1所提出的可观测度定义及推导公式类似，可以得到各个状态量的可观测度为：

其中，r_(i,j)的计算与步骤A.1中所对应变量的定义相同。

一实施例中，步骤A还包括以下步骤：

步骤A.3：设定可观测度阈值，结合步骤A.1和A.2，本步骤给出了判别系统状态量是否可观测的理论阈值，从而将系统状态量划分为：可观测状态量，弱可观测状态量和不可观测状态量；

定义矩阵R的秩为：

Λ＝rank(R,TOL),

其中，Λ是矩阵R中大于TOL的奇异值数目，且通常TOL在矩阵奇异值分解时被设定为：

其中，

是矩阵R的最大奇异值；

是当前系统中表示浮点数1的最大误差；

显然，矩阵R的秩表示了系统状态量中可观测的个数，根据步骤A.1与步骤A.2，已经计算得到了各个系统状态量的可观测度；同时，结合所得到矩阵R的秩为Λ，则可以设定状态量是否可观测的阈值为：

其中，

是第Λ大的可观测度；

至此，可以给出判别系统状态量可观测性的判别规则：

(1)若D_k,i＝0，则第i维的系统状态量是不可观测的；

(2)若

则第i维的系统状态量是弱可观测的；

(3)若

则第i维的系统状态量是可观测的。

一实施例中，步骤B包括以下步骤：

步骤B.1：各个子滤波器的状态估计结果向主滤波器传输前，需要预先判别各个状态量是否可观测，仅将可观测状态量的估计信息传输给主滤波器；

记第i个子滤波器的待估计状态向量为x_i＝[x_ci,x_bi]^T，其中，x_ci是各个子滤波器的公共状态，包括载体的位置、速度、姿态、三轴加速度计的零偏和三轴陀螺仪的零偏；x_bi则是第i个子滤波器专有的状态量，如二维图像中特征点的逆深度(视觉导航系统)；

另记第i个子滤波器公共状态的估计协方差矩阵为P_ii，且各个子滤波对公共状态的估计互不相关，即P_ij＝0(i≠j)，则主滤波器得到的全局最优估计和其协方差矩阵P_g可以表示为：

其中，n为子滤波器的个数，矩阵L_i是一个对角矩阵，对角元素由1或0构成，当第i个子滤波器的某个公共状态量被判定为可观测时，则矩阵L_i中所对应的对角线元素被设置为1，否则需要将其设置为0。

一实施例中，步骤B还包括以下步骤：

步骤B.2：主滤波器将全局滤波的结果反馈至各个子滤波器之前，需要预先判别主滤波器所估计的各个状态量是否融合了某一子滤波器所对应的状态量，仅将状态量中能够被某一子滤波器有效估计的状态量反馈至所有子滤波器中；

主滤波器估计得到的全局估计值x_g及其相应的协方差矩阵P_g需要反馈到各个子滤波对其进行修正，即：

x_i＝x_g,P_ii＝(β_i[M]^-1)P_g,

其中，信息分配系数β_i＝1/n；[·]^-1运算符表示仅对矩阵的对角元素求倒数，矩阵M可以通过下式进行计算：

显然，根据上式可知，若所有的子滤波器均不能为x_g中某一状态量提供足够的可观测度时，则主滤波器对所有子滤波器进行反馈修正时，将会使其在子滤波中对应的协方差变为无穷大，即认为主滤波器所反馈的信息置信度为零。

与传统的联邦滤波器相比，附图1所描述的多传感器信息融合框架能根据传感器测量值是否具有全局属性对信息分配机制进行调整，将其他子滤波器的全局估计信息作为约束，使不具有全局测量属性的传感器所构建的子滤波器的估计值也具备全局属性。如图1中，计算机视觉系统参与的子滤波器也融合了来自整体系统输出的全局回环定位信息，从而保证其估计结果具备全局性。

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应该以权利要求的保护范围为准。

Claims (6)

1.一种快速可观测度分析及其指导的多传感器信息融合方法，其特征在于，包括以下步骤：

步骤A：设计低运算复杂度的可观测度分析算法，结合待估计状态向量的通解公式，引入QR分解降低对条带化可观测性矩阵进行分析的运算量，并给出相应的可观测度定义、计算公式及可观测度判定阈值；

步骤B：在步骤A的基础上，设计基于可观测度的信息融合机制，并将其集成至多传感器信息融合框架中，各个子滤波器的状态估计结果向主滤波器传输前，需要预先判别各个状态量是否可观测，仅将可观测状态量的估计信息传输给主滤波器；主滤波器将全局滤波的结果反馈至各个子滤波器之前，需要预先判别主滤波器所估计的各个状态量是否融合了某一子滤波器所对应的状态量，仅将状态量中能够被某一子滤波器有效估计的状态量反馈至所有子滤波器中。

2.根据权利要求1所述的一种快速可观测度分析及其指导的多传感器信息融合方法，其特征在于，步骤A包括以下步骤：

步骤A.1：针对滤波算法的可观测度分析算法设计，考虑有以下的离散线性系统：

其中，x_k∈Rⁿ为系统在t_k时刻的状态向量，维度为n；Φ_k,k-1∈R^n×n为t_k-1时刻到t_k时刻的系统状态转移矩阵；z_k∈R^m为系统测量向量，维度为m；H_k为测量矩阵；v_k为测量值的噪声，满足零均值高斯白噪声模型，且有E[v_k]＝0，

w_k-1为模型噪声，与v_k类似，也满足零均值高斯白噪声模型，且有E[w_k]＝0，

根据上段所定义的离散线性系统，引入如下方程计算k时刻的条带化可观测性矩阵：

Z_k＝O_kx_k+V_k,

其中，矩阵O_k表示k时刻的SOM，且有：

其中，V_k的计算依赖于v_k和w_k，都服从零均值高斯白噪声的假设；此外，由于x的维度是n，Z的维度是m，故矩阵O_k是一个mn×n的矩阵，即O_k∈R^mn×n；

基于SOM的计算公式，可以得到关于x_k的通解：

其中，

是O_k的伪逆，α是一个随机的实数向量(α∈Rⁿ)；然后，记：

其中，Δo_k,i是ΔO_k的第i行；同时，定义：

x_k＝[x_k,1 x_k,2 ... x_k,N]^T,

则x_k,i可以由两部分组成，具体表达如下：

其中，

可以由

明确计算得到；Δo_k,iα_i是受随机量α_i影响的部分；

如果Δo_k,i的2-范数||Δo_k,i||₂为零或无限接近零，则随机量α_i将几乎不会影响x_k,i，即x_k,i可以被确定性的求解；否则，x_k,i的求解过程会受到随机量α_i的影响，意味着x_k,i会有无数个解；

基于Δo_k,i定义x_k,i的可观测度，由于计算Δo_k,i时需要计算SOM的伪逆矩阵

这将占用过多的计算资源，为了提高运算效率，避免直接计算SOM的伪逆矩阵

考虑将公式中的

改写为：

对矩阵O_k进行QR分解，即：

O_k＝QR,

其中，R∈R^n×n是一个上三角矩阵，Q∈R^mn×n是一个正交矩阵，即满足Q^TQ＝I_mn×mn；

将上述QR分解带入x_k的通解公式中，可以得到：

注意到(R^TR)可能会是奇异矩阵，无法按照上式进行求逆运算，故对上式进行如下改进：

其中，λ是一个较小的正实数，可以避免求逆运算失败的情况；

矩阵

的计算仅与上三角矩阵R有关，根据上述分析，定义系统各个状态量的可观测度为：

其中，r_k,(i,j)是矩阵(R^TR+λI_N×N)^-1R^TR中第i行，第j列的元素，‖·‖₂表示2范数运算。

3.根据权利要求2所述的一种快速可观测度分析及其指导的多传感器信息融合方法，其特征在于，步骤A还包括以下步骤：

步骤A.2：针对优化算法的可观测度分析算法设计，记一个优化问题的整体代价函数为F(x)，并对其进行一阶的泰勒展开：

F(x+Δx)≈f(x)+J(x)Δx,

其中，J(x)为F(x)关于x的雅各比矩阵，为求得x的最优估计值，则需要在每次迭代计算中寻找合适的增量Δx，使得‖F(x+Δx)‖²达到最小；

在增加置信域约束项之后，可将原问题转化为以下优化问题：

arg min_Δx‖f(x)+J(x)Δx‖²,s.t.Δx^TΔx≤d,

其中，d表示约束增量Δx的范围，且对于每一步的迭代计算，d是一个已知量；

对上式所描述的约束优化问题求解需要分两种情况进行考虑：

情况1：假设所求的Δx位于Δx^TΔx＜d的范围内，则此时求得的结果等同于无约束情况下的最优解；

情况2：假设所求的Δx位于Δx^TΔx≥d的范围内，则将优化问题视为带等式约束的优化问题：

arg min_Δx‖f(x)+J(x)Δx‖²,s.t.Δx^TΔx＝d,

引入拉格朗日乘数λ构建如下方程：

L(Δx)＝‖f(x)+J(x)Δx‖²+λ(Δx^TΔx-d).

此时对上式求Δx的偏导数并令其等于0，可以得到Δx的计算公式：

(J(x)^TJ(x)+λI)Δx＝J(x)^TF(x).

将J(x)^TJ(x)记为H，将J(x)^TF(x)记为Z，并将λI从方程中删除，可以得到：

HΔx＝Z,

则待估计的状态向量Δx的通解为：

Δx＝H⁺Z+(I-H⁺H)α,

其中，H⁺是H的伪逆，α是一个维数与状态向量相同的实数随机向量，与步骤A.1所提出的可观测度定义及推导公式类似，可以得到各个状态量的可观测度为：

其中，r_(i,j)的计算与步骤A.1中所对应变量的定义相同。

4.根据权利要求3所述的一种快速可观测度分析及其指导的多传感器信息融合方法，其特征在于，步骤A还包括以下步骤：

步骤A.3：设定可观测度阈值，结合步骤A.1和A.2，本步骤给出了判别系统状态量是否可观测的理论阈值，从而将系统状态量划分为：可观测状态量，弱可观测状态量和不可观测状态量；

定义矩阵R的秩为：

Λ＝rank(R,TOL),

其中，Λ是矩阵R中大于TOL的奇异值数目，且通常TOL在矩阵奇异值分解时被设定为：

其中，

是矩阵R的最大奇异值；ò是当前系统中表示浮点数1的最大误差；

显然，矩阵R的秩表示了系统状态量中可观测的个数，根据步骤A.1与步骤A.2，已经计算得到了各个系统状态量的可观测度；同时，结合所得到矩阵R的秩为Λ，则可以设定状态量是否可观测的阈值为：

其中，

是第Λ大的可观测度；

至此，可以给出判别系统状态量可观测性的判别规则：

(1)若D_k,i＝0，则第i维的系统状态量是不可观测的；

(2)若

则第i维的系统状态量是弱可观测的；

(3)若

则第i维的系统状态量是可观测的。

5.根据权利要求1所述的一种快速可观测度分析及其指导的多传感器信息融合方法，其特征在于，步骤B包括以下步骤：

步骤B.1：各个子滤波器的状态估计结果向主滤波器传输前，需要预先判别各个状态量是否可观测，仅将可观测状态量的估计信息传输给主滤波器；

记第i个子滤波器的待估计状态向量为x_i＝[x_ci,x_bi]^T，其中，x_ci是各个子滤波器的公共状态，包括载体的位置、速度、姿态、三轴加速度计的零偏和三轴陀螺仪的零偏；x_bi则是第i个子滤波器专有的状态量；

另记第i个子滤波器公共状态的估计协方差矩阵为P_ii，且各个子滤波对公共状态的估计互不相关，即P_ij＝0(i≠j)，则主滤波器得到的全局最优估计和其协方差矩阵P_g可以表示为：

其中，n为子滤波器的个数，矩阵L_i是一个对角矩阵，对角元素由1或0构成，当第i个子滤波器的某个公共状态量被判定为可观测时，则矩阵L_i中所对应的对角线元素被设置为1，否则需要将其设置为0。

6.根据权利要求5所述的一种快速可观测度分析及其指导的多传感器信息融合方法，其特征在于，步骤B还包括以下步骤：

步骤B.2：主滤波器将全局滤波的结果反馈至各个子滤波器之前，需要预先判别主滤波器所估计的各个状态量是否融合了某一子滤波器所对应的状态量，仅将状态量中能够被某一子滤波器有效估计的状态量反馈至所有子滤波器中；

主滤波器估计得到的全局估计值x_g及其相应的协方差矩阵P_g需要反馈到各个子滤波对其进行修正，即：

x_i＝x_g,P_ii＝(β_i[M]^-1)P_g,

其中，信息分配系数β_i＝1/n；[·]^-1运算符表示仅对矩阵的对角元素求倒数，矩阵M可以通过下式进行计算：

根据上式可知，若所有的子滤波器均不能为x_g中某一状态量提供足够的可观测度时，则主滤波器对所有子滤波器进行反馈修正时，将会使其在子滤波中对应的协方差变为无穷大，即认为主滤波器所反馈的信息置信度为零。

Images