基于KF/FIR和LS-SVM融合的INS/UWB组合导航算法及系统的制作方法

本发明涉及复杂环境下组合定位技术领域，尤其涉及基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统。

背景技术：

近年来，行人导航(pedestriannavigation,pn)作为导航技术应用的新兴领域，正越来越受到各国学者的重视，并逐渐成为该领域的研究热点。然而在隧道、大型仓库、地下停车场等室内环境下，外界无线电信号微弱、电磁干扰强烈等因素都会对目标行人导航信息获取的准确性、实时性及鲁棒性有很大影响。如何将室内环境下获取的有限信息进行有效的融合以消除室内复杂环境影响，保证行人导航精度的持续稳定，具有重要的科学理论意义和实际应用价值。

在现有的定位方式中，全球卫星导航系统(globalnavigationsatellitesystem,gnss)是最为常用的一种方式。虽然gnss能够通过精度持续稳定的位置信息，但是其易受电磁干扰、遮挡等外界环境影响的缺点限制了其应用范围，特别是在室内、地下巷道等一些密闭的、环境复杂的场景，gnss信号被严重遮挡，无法进行有效的工作。近年来，uwb(ultrawideband)以其在复杂环境下定位精度高的特点在短距离局部定位领域表现出很大的潜力。学者们提出将基于uwb的目标跟踪应用于gnss失效环境下的行人导航。这种方式虽然能够实现室内定位，但是由于室内环境复杂多变，uwb信号十分容易受到干扰而导致定位精度下降甚至失锁；与此同时，由于uwb采用的通信技术通常为短距离无线通信技术，因此若想完成大范围的室内目标跟踪定位，需要大量的网络节点共同完成，这必将引入网络组织结构优化设计、多节点多簇网络协同通信等一系列问题。因此现阶段基于uwb的目标跟踪在室内导航领域仍旧面临很多挑战。

在导航模型方面，目前在室内行人组合导航领域应用较多的为松组合导航模型。该模型具有容易实现的优点，但是需要指出的是，该模型的实现需要参与组合导航的多种技术能够独立的完成导航定位。例如，需要uwb设备能够提供行人的导航信息，这就要求目标行人所处的环境必须能够获取至少3个参考节点信息，这大大的降低了组合导航模型的应用范围，与此同时，参与导航的子技术独立完成定位，也引入了新的误差，不利于组合导航技术精度的提高。为了克服这一问题，学者们提出将紧组合模型应用于室内行人导航领域，紧组合模型直接将参与组合导航的子技术的原始传感器数据应用于最后的导航信息的解算，减少了子技术自行解算引入新误差的风险，提高了组合导航的精度，但是需要指出的是，现有紧组合导航模型均使用集中式模式，这一方式系统容错能力差，并不利于日益精确复杂的组合导航模型。

技术实现要素：

本发明的目的就是为了解决上述难题，提供了一种基于kf/fir和ls-svm融合的ins/uwb组合导航算法及系统。当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估。与此同时，ls-svm算法处于培训模式，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，力求通过培训构造ins位置与位置误差的映射关系。一旦uwb信号不可用，ls-svm算法替代不能工作的kf/fir滤波器，利用构建的映射关系对ins位置误差进行预估。本发明的提出，克服了传统kf/fir滤波器在uwb失锁状态下不能正常工作的缺陷，实现了对导航信息的无缝预估。

为实现上述目的，本发明的具体方案如下：

本发明的第一目的是公开一种基于kf/fir和ls-svm融合的ins/uwb组合导航算法，包括：

以惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；

当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估，同时，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，通过培训构造ins位置与位置误差的映射关系；

一旦uwb信号不可用，利用ins位置与位置误差的映射关系对ins位置误差进行预估，并对ins的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。

进一步地，所述惯性导航器件imu固定在目标行人足部，uwb参考节点预先布置在位置已知的导航环境中，uwb目标节点固定在目标行人身上。

进一步地，惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置的初值根据实际需要自行设定。

进一步地，所述kf/fir滤波器的状态方程为：

其中，分别为k时刻和k-1时刻ins测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；分别为k时刻和k-1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声；t为采样周期。

进一步地，所述kf/fir滤波器的观测方程为：

其中，为k时刻惯性器件imu解算的东向和北向位置；为k时刻uwb解算的东向和北向位置；为k时刻ins测量得到的导航坐标系下目标行人的东向和北向两个个方向的速度误差；为k时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；vk为观测噪声；为k时刻ins东向位置误差的观测值、为k时刻ins北向位置误差的观测值。

进一步地，所述kf/fir滤波器中kf算法的性能评估方法为：

计算信道质量评价指标dk，判断dk与门限door的关系，如果dk<door，则信道质量正常，反之，则信道质量差，引入fir滤波器。

本发明的第二目的是公开一种基于kf/fir和ls-svm融合的ins/uwb组合导航系统，包括：服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：

以惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；

当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估，同时，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，通过培训构造ins位置与位置误差的映射关系；

一旦uwb信号不可用，利用ins位置与位置误差的映射关系对ins位置误差进行预估，并对ins的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。

本发明的第三目的是公开一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：

以惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；

当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估，同时，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，通过培训构造ins位置与位置误差的映射关系；

一旦uwb信号不可用，利用ins位置与位置误差的映射关系对ins位置误差进行预估，并对ins的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。

本发明的有益效果：

1、组合导航方法采用ins/uwb松组合模型，该模型以惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；在此基础上，通过kf/fir滤波将ins和uwb获取的导航信息进行数据融合，最终得到当前时刻最优的导航信息和对参考节点位置信息的预估。提高了数据融合滤波器的精度和鲁棒性。

2、当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估。与此同时，ls-svm算法处于培训模式，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，力求通过培训构造ins位置与位置误差的映射关系。一旦uwb信号不可用，ls-svm算法替代不能工作的kf/fir滤波器，利用构建的映射关系对ins位置误差进行预估。

本发明的提出，克服了传统kf/fir滤波器在uwb失锁状态下不能正常工作的缺陷，实现了对导航信息的无缝预估。

3、可用于室内环境下的行人中高精度定位。

附图说明

图1为uwb信号可用时的算法示意图；

图2为uwb信号不可用时的算法示意图。

具体实施方式：

下面结合附图对本发明进行详细说明：

需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。

为了解决背景技术提出的问题，本发明公开了基于kf/fir和ls-svm融合的ins/uwb组合导航算法，包括：

(1)以惯性导航系统ins的东向和北向的位置和速度误差作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；

组合导航系统主要包括惯性导航系统和uwb无线定位系统两部分。其中，惯性导航系统采用固定在目标行人足部的惯性测量单元(imu)。uwb无线定位系统主要包括目标节点和参考节点两部分，其中，参考节点预先布置在位置已知的导航环境中；目标节点则固定在目标行人身上。

(2)当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估，与此同时，ls-svm算法处于培训模式，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，通过培训构造ins位置与位置误差的映射关系，如图1所示；

其中，kf/fir滤波器的状态方程为：

其中，分别为k时刻和k-1时刻ins测量得到的导航坐标系(n系)下目标行人的东向和北向两个个方向的速度误差；分别为k时刻和k+1时刻导航坐标系下目标行人的东向和北向两个个方向的位置误差；ωk为状态噪声，其协方差阵为q；t为采样周期。

kf/fir滤波器的观测方程为：

其中，为k时刻惯性器件imu解算的东向和北向位置；为k时刻uwb解算的东向和北向位置；vk为观测噪声，其协方差阵为r。

kf/fir滤波器中kf算法的迭代方程为：

pk|k-1＝fkpk-1(fk)^t+q

kk＝pk|k-1(hk)^t(hkpk|k-1(hk)^t+r)^-1

pk＝(i-kkhk)pk|k-1

表示kf在k时刻预估的状态向量，表示kf由k-1时刻到k时刻预估的状态向量，fk表示表示k时刻的系统矩阵，pk|k-1表示kf由k-1时刻到k时刻的最小预测均方误差矩阵；pk表示kf在k时刻的最小预测均方误差矩阵；pk-1表示kf在k-1时刻的最小预测均方误差矩阵；kk表示kf在k时刻的误差增益矩阵；i表示单位阵。

kf/fir滤波器中kf算法的性能评估方法为：

首先计算信道质量评价指标：

在此基础上，判断dk与门限door的关系，如果dk<door，则信道质量正常，反之，则信道质量差，引入fir滤波器。

进行kf算法性能评估的目的主要是看一下kf算法有没有发散，一旦发现其有发散的迹象，就引入fir滤波器。

kf/fir滤波器中fir算法的迭代方程为：

首先，若当前采样时刻大于fir预先设置的窗口n，定义

m＝k-n+1,s＝m+m-1

从第m+m时刻到k时刻，执行下面操作：

最终，

表示fir在j时刻预估的状态向量，表示fir由j-1时刻到j时刻预估的状态向量，fj表示表示j时刻的系统矩阵，表示fir由j-1时刻到j时刻的最小预测均方误差矩阵；表示fir在j时刻的最小预测均方误差矩阵；k表示kf在j时刻的误差增益矩阵；i表示单位阵；

(4)当uwb信号不可用时，ls-svm算法替代不能工作的kf/fir滤波器，利用(3)中构建的映射关系对ins位置误差进行预估，并对ins的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息，如图2所示。

本发明进一步公开了一种基于kf/fir和ls-svm融合的ins/uwb组合导航系统，包括：服务器，所述服务器包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现以下步骤：

以惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；

当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估，同时，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，通过培训构造ins位置与位置误差的映射关系；

一旦uwb信号不可用，利用构建的映射关系对ins位置误差进行预估，并对ins的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。

本发明进一步公开了一种计算机可读存储介质，其上存储有计算机程序，该程序被处理器执行时执行以下步骤：

以惯性导航器件imu的误差向量和各uwb参考节点的东向和北向位置作为状态量，以惯性导航器件imu测量得到的目标行人的东向和北向位置之差作为系统观测量构建ins/uwb松组合导航模型；

当uwb信号可用时，kf/fir滤波器正常工作，为组合导航系统提供导航信息的最优预估，同时，将ins解算的位置信息与kf/fir滤波器给出的ins位置误差的最优预估分别作为ls-svm算法的输入和培训目标，通过培训构造ins位置与位置误差的映射关系；

一旦uwb信号不可用，利用构建的映射关系对ins位置误差进行预估，并对ins的解算误差进行补偿，最终得到当前时刻的行人最优的位置信息。

上述虽然结合附图对本发明的具体实施方式进行了描述，但并非对本发明保护范围的限制，所属领域技术人员应该明白，在本发明的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本发明的保护范围以内。