一种组合导航传感器的边缘处理方法、装置及设备与流程

1.本发明涉及无人机组合导航技术领域，具体涉及一种组合导航传感器的边缘处理方法、装置及设备。


背景技术：

2.在无人机组合导航领域中，gps与ins结合的方式是最常见的，组合导航的精度也依赖于传感器采集数据的精度。在复杂环境中，gps与惯导传感器在采集数据的过程中会有干扰，影响采集数据的精度。高精度的传感器价格较贵，对于一般需求而言不适用，对于一般传感器存在如下两个问题，第一，在gps信号较强的环境中，gps作为整个组合导航系统的观测值，纠正惯导的累积误差值，那么gps传感器的测量干扰会影响整个组合导航系统的精度；第二，在gps信号较弱的地区，整个组合导航系统只剩下惯导了，惯导一般是由陀螺仪和加速度计组成，在得到位置信息时有累计误差，当gps信号较强时可以清除ins的误差，但是当在复杂环境运行时，惯导传感器测量也存在干扰，同样也会影响整个导航系统的精度。


技术实现要素：

3.为了解决现有无人机组合导航传感器测量过程中因为干扰而影响测量精度的问题，本发明提供了一种组合导航传感器的边缘处理方法。本发明通过引入滑模观测器进行抗干扰，同时与无迹卡尔曼滤波结合，从而提高无人机在复杂环境中的导航精度。
4.本发明通过下述技术方案实现：
5.一种组合导航传感器的边缘处理方法，包括：
6.将组合导航传感器的测量值输入到滑模观测器中；
7.利用滑模观测器得到组合导航传感器测量值的估计值；
8.计算得到估计值与实际测量值之间的误差值；
9.将估计值与误差值反馈给无迹卡尔曼滤波模型进行测量更新。
10.优选的，本发明的滑模观测器的构建方法包括：
11.根据滑模观测器的原理与非线性系统方差构造滑模观测器，如下式：
[0012][0013]
式中，u是已知输入，f和g表示输入的参数矩阵，l是滑模观测器的增益，d为滑模观测器系统中未知的输入量，k是常量，sgn()为符号函数，z是观测值，s为滑模面且x是系统状态真实值，是系统状态估计值，是系统误差；
[0014]
未知输入量d的估计值为：
[0015][0016]
其中：
[0017][0018]
优选的，本发明的sgn(s)可表示为：
[0019][0020]
优选的，本发明的步骤101之前还包括初始化过程，所述初始化过程按照下式选取sigma点：
[0021][0022]
式中，ξ
i，k
‑1满足条件的点，状态估计和误差协方差矩阵a是调谐参数，描述围绕x
k
‑1的sigma点的分布np
k
‑1的第i列。
[0023]
优选的，本发明的步骤102具体为：
[0024]
滑模观测器的输入量d为x
k 1
时，则根据下式计算得到k 1时刻滑模观测器输入量的估计值：
[0025][0026]
通过下式计算得到k 1时刻的状态预测协方差：
[0027][0028]
优选的，本发明的步骤103具体为：
[0029]
计算得到k 1时刻的观测值估计值：
[0030][0031]
式中，为观测矩阵；
[0032]
计算观测值估计值与实际测量值之间的观测误差：
[0033][0034]
式中，v
k 1
是误差值，z
k 1
是观测值，是观测估计值。
[0035]
优选的，本发明的步骤104具体为：
[0036]
将观测值估计值、观测误差值输入到无迹卡尔曼滤波模型中进行测量更新，得到k 1时刻ukf的卡尔曼增益、状态向量估计和过程协方差矩阵为：
[0037][0038]
式中，k
k 1
为增益矩阵，p
k 1|k
为k时刻协方差矩阵，p
k 1|k 1
为k 1时刻协方差矩阵。
[0039]
第二方面，本发明提出了一种组合导航传感器的边缘处理装置，包括滑模观测器和无迹卡尔曼滤波器；
[0040]
所述滑模观测器输入导航传感器测量数据，对其进行观测得到滑模估计值；
[0041]
将滑模估计值与实际测量值之间的误差值、滑模估计值输入到无迹卡尔曼滤波器中进行测量更新。
[0042]
第三方面，本发明提出了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其特征在于，所述处理器执行所述计算机程序时实现本发明所述方法的步骤。
[0043]
本发明具有如下的优点和有益效果：
[0044]
1、本发明针对低成本传感器在复杂环境中的导航遇到的测量干扰，采用了滑模观测器的方式去抑制，同时结合组合导航中的无迹卡尔曼滤波来提高导航精度。
[0045]
2、本发明采用的无迹卡尔曼滤波与滑模观测器的作用对象都是非线性系统，因此本发明可广泛适用于无人机等非线性系统导航领域。
附图说明
[0046]
此处所说明的附图用来提供对本发明实施例的进一步理解，构成本技术的一部分，并不构成对本发明实施例的限定。在附图中：
[0047]
图1为本发明的方法流程示意图。
[0048]
图2为切换面示意图。
[0049]
图3为本发明的滑模观测器与无迹卡尔曼滤波结合的算法结构图。
[0050]
图4为本发明的设备结构示意图。
[0051]
图5为本发明的装置原理框图。
[0052]
图6为本发明的仿真轨迹示意图。
[0053]
图7为本发明的位置误差对比图。
[0054]
图8为本发明的速度误差对比图。
具体实施方式
[0055]
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
[0056]
实施例1
[0057]
针对无人机在复杂环境中的传感器测量干扰问题，本实施例提出了一种组合导航传感器的边缘处理方法，本实施例通过引入滑模观测器进行抗干扰，同时与无迹卡尔曼滤波结合，从而提高无人机在复杂环境中的导航精度。
[0058]
具体如图1所示，本实施例的方法具体包括：
[0059]
步骤101，将组合导航传感器的测量值输入到滑模观测器中，本实施例中的组合导航传感器为gps和ins传感器；
[0060]
步骤102，利用滑模观测器得到组合导航传感器测量值的估计值；
[0061]
步骤103，计算得到估计值与实际测量值之间的误差值；
[0062]
步骤104，将估计值与误差值反馈给无迹卡尔曼滤波模型进行测量更新。
[0063]
滑模观测器是一类动态系统，它的原理主要是指根据整个系统的测量值去得到估计值，测量值主要是输入和输出等外部变量，估计值是根据滑模面的设计得到。其中的滑模面是人为设计的，从而使滑模控制对干扰具有很好的抑制性，同时它具有计算量小，实现简单等优点，比人工智能方法更加适用于无人机等嵌入式设备。设非线性系统为：
[0064][0065]
那么存在如下式(2)的切换函数，个状态空间分为了s>0,s＝0,s<0这3部分，其中s＝0为超曲面空间，为切换面，滑模控制就是依据s(x)与0的大小进行切换。然后根据滑模面的方向分为了平常点、起始点、终止点，分别如图2所示的a、b、c。从图中可知终点就是将系统运动逼向滑模面，当系统达到滑模面附近时。
[0066]
需满足此为滑模面存在的条件。
[0067][0068]
其中f(x,u)是切换函数，s(x)是切换面。
[0069]
在许多导航应用中，一般都是非线性系统，非线性系统可以式(3)表示：
[0070][0071]
本实施例采用的滑模观测器构建过程如下：
[0072]
步骤201，定义系统误差，本实施例定义的系统误差如式(4)所示，表示的是系统状态的真实值与估计值误差：
[0073][0074]
其中，是系统误差，x是系统状态真实值，是系统状态估计值。
[0075]
步骤202，根据滑模观测器的原理与非线性系统误差构造如式(5)所示的滑模观测器。本实施例的选取滑模面
[0076][0077]
其中，u是已知输入，f和g表示输入的参数矩阵。l是滑模观测器的增益，d为滑模观
测器系统中未知的输入量，也可以表示为噪声，k是常量，sgn()为符号函数，z是观测值，sgn(s)可表示为：
[0078][0079]
步骤203，根据式(4)
‑
(6)，可以得到输入量d的最终估计值为：
[0080][0081]
滑模观测器构造完成后，将其与无迹卡尔曼滤波器结合实现测量更新，如图3所示，首先将gps/ins融合的数据分别送入到ukf滤波与滑膜观测器(smo)中,那么gps/ins的状态向量就是smo的输入量，然后smo就会对输入的状态量进行估计得到最优的测量值，最后将最优测量值融入到ukf中，同时将ukf的输出值通过反馈的方式转送到smo中，进而提高传感器测量精度，进而提高导航精度。
[0082]
本实施例在步骤101之前还包括初始化，本实施例的初始化是按照无迹卡尔曼滤波进行的，按照式(8)选取sigma点。
[0083][0084]
其中，ξ
i，k
‑1满足条件的点，状态估计和误差协方差矩阵a是调谐参数，描述围绕x
k
‑1的sigma点的分布np
k
‑1的第i列。
[0085]
本实施例的步骤102具体为：
[0086]
根据式(9)，滑模观测器输入量d为x
k 1
，根据式(7)可知k 1时刻估计结果为式(10)所示：
[0087][0088]
其中表示权重。
[0089][0090]
状态预测协方差p
k 1/k
为k时刻经过滑模面的次数即为k，则下一时刻的状态预测方程为式(11)所示：
[0091][0092]
式中，为预测平均值，为第i次的状态估计值，将观测更新值代入公式(7)进行预测更新。
[0093]
本实施例的步骤103具体为：
[0094]
根据无迹卡尔曼滤波原理可得方程如式(12)所示，根据式(12)的观测值公式可得k 1时刻观测值估计值如式(13)所示：
[0095][0096]
p为协方差矩阵，k为增益，h为观测矩阵，r为过程噪声，z为观测值，下标k与k 1，k
‑
1表示不同时刻。
[0097][0098]
其中，为观测矩阵
[0099]
由上式可得观测值估计值与实际观测值之间的观测误差如式(14)所示：
[0100][0101]
式中，v
k 1
是误差值，z
k 1
是观测值，是观测估计值。
[0102]
本实施例的步骤104具体为：
[0103]
将步骤103获得的观测值估计值、观测误差值输入到无迹卡尔曼滤波模型中进行测量更新，如式(12)所示，则在k 1时刻ukf的卡尔曼增益、状态向量估计和过程协方差矩阵为：
[0104][0105]
式中，k
k 1
为增益矩阵，p
k 1|k
为k时刻协方差矩阵，p
k 1|k 1
为k 1时刻协方差矩阵。
[0106]
本实施例还提出了一种计算机设备，用于执行本实施例的上述方法。
[0107]
具体如图4所示，计算机设备包括处理器、内存储器和系统总线；内存储器和处理器在内的各种设备组件连接到系统总线上。处理器是一个用来通过计算机系统中基本的算术和逻辑运算来执行计算机程序指令的硬件。内存储器是一个用于临时或永久性存储计算
程序或数据(例如，程序状态信息)的物理设备。系统总线可以为以下几种类型的总线结构中的任意一种，包括存储器总线或存储控制器、外设总线和局部总线。处理器和内存储器可以通过系统总线进行数据通信。其中内存储器包括只读存储器(rom)或闪存(图中未示出)，以及随机存取存储器(ram)，ram通常是指加载了操作系统和计算机程序的主存储器。
[0108]
计算机设备一般包括一个外存储设备。外存储设备可以从多种计算机可读介质中选择，计算机可读介质是指可以通过计算机设备访问的任何可利用的介质，包括移动的和固定的两种介质。例如，计算机可读介质包括但不限于，闪速存储器(微型sd卡)，cd
‑
rom，数字通用光盘(dvd)或其它光盘存储、磁带盒、磁带、磁盘存储或其它磁存储设备，或者可用于存储所需信息并可由计算机设备访问的任何其它介质。
[0109]
计算机设备可在网络环境中与一个或者多个网络终端进行逻辑连接。网络终端可以是个人电脑、服务器、路由器、智能电话、平板电脑或者其它公共网络节点。计算机设备通过网络接口(局域网lan接口)与网络终端相连接。局域网(lan)是指在有限区域内，例如家庭、学校、计算机实验室、或者使用网络媒体的办公楼，互联组成的计算机网络。wifi和双绞线布线以太网是最常用的构建局域网的两种技术。
[0110]
应当指出的是，其它包括比计算机设备更多或更少的子系统的计算机系统也能适用于发明。
[0111]
如上面详细描述的，适用于本实施例的计算机设备能执行边缘处理方法的指定操作。计算机设备通过处理器运行在计算机可读介质中的软件指令的形式来执行这些操作。这些软件指令可以从存储设备或者通过局域网接口从另一设备读入到存储器中。存储在存储器中的软件指令使得处理器执行上述的群成员信息的处理方法。此外，通过硬件电路或者硬件电路结合软件指令也能同样实现本发明。因此，实现本实施例并不限于任何特定硬件电路和软件的组合。
[0112]
实施例2
[0113]
本实施例提出了一种组合导航传感器的边缘处理装置，如图5所示，本实施例的装置包括滑模观测器和无迹卡尔曼滤波器；
[0114]
其中，滑模观测器输入导航传感器测量数据，对其进行观测得到滑模估计值；
[0115]
将滑模估计值与实际测量值之间的误差值、滑模估计值输入到无迹卡尔曼滤波器中进行测量更新，即可实现对导航传感器测量数据的修正，提高测量精度和导航精度。
[0116]
滑模估计值为观测器估计的位姿态等信息，模型误差为预测模型与实际值的偏差。
[0117]
实施例3
[0118]
本实施例对上述实施例提出的技术进行仿真实验，本实施例的仿真环境使用gazebo模拟真实的环境，在gazebo中进行飞行并且采集传感器数据，同时全球定位以及飞行估计在qgc地面站中显示，整个无人机飞行轨迹如图6所示。滑模增益l选取为5，分别进行了姿态、位置和速度三个方面进行对比，实验结果如图7所示和图8所示。其中smo
‑
ffk表示滑模观测器与联邦卡尔曼滤波结合的导航算法，ffk表示只用联邦联邦卡尔曼滤波算法。
[0119]
由图7和图8可知，在联邦卡尔曼滤波的基础上加入滑模观测能够有效抑制干扰，在结果图中，无论是速度误差还是位置误差，当出现突然变化的情况时，联邦卡尔曼滤波虽然能够比子滤波器有更好的效果，但是效果只是减小了突变的峰值，不能抑制这种突变情
况。同时为了更好的说明带有滑模观测器的联邦卡尔曼滤波能够提高精度，本实施例将上图中的误差均值统计为表1。
[0120]
表1组合导航平均误差对比
[0121][0122]
由表1可知，结合滑模观测器的联邦卡尔曼滤波的算法经度平均误差提高了21.23％，纬度平均误差提高了21％，高度平均误差提高了12.6％，北向速度平均误差提高了15.17％，东向速度平均误差提高了16.11％，垂直速度平均误差提高了5.5％。通过上述实验说明，加入滑模观测器后能够有效抑制干扰，更加有效的提高导航精度。
[0123]
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。