CN108627152B - 一种微型无人机基于多传感器数据融合的导航方法 - Google Patents

Abstract

本发明实施例公开了一种微型无人机基于多传感器数据融合的导航方法。所述方法包括：获取惯性传感器IMU、GPS、磁罗盘、光流传感器、雷达输出的信号；使用软件时间对准技术对多传感器的信号进行时间对准；对获取的信号进行滑动噪声估计；将所述滑动噪声估计输入卡尔曼滤波器中，作为量测校正时的权重；并且，根据所述滑动噪声估计进行同量纲数据的增益分配；将处理后的数据、滑动噪声估计输入卡尔曼滤波器，进行速度、位置的解算。本发明实施例的技术方案融合多种传感器的数据，能够提高导航的精确性；同时，对传感器数据进行滑动噪声估计和增益分配进一步提高了系统的准确性。

Description

一种微型无人机基于多传感器数据融合的导航方法

技术领域

本发明实施例涉及导航领域，尤其涉及一种微型无人机基于多传感器数据融合的导航方法。

背景技术

现代无人机普遍装有多种传感器如惯性传感器IMU、气压计、超声波、磁罗盘、光流传感器、GPS等。使用多种传感器会使得噪声愈发多样性和复杂性，而目前通常使用的固定噪声估计，不能有效克服多种传感器在不同环境下产生的噪声对系统的干扰，将会导致导航效果变差，严重的会发生安全事故。例如，可能出现在GPS定位情况较好的情况下，采用了较大的固定噪声，使得GPS数据在融合时所占比重过低，或是GPS定位情况较差的情况下，采用了较小的固定噪声，使得GPS数据在融合时所占比重过高。并且，多种传感器会输出同量纲的物理量，一般的组合导航中，往往忽视同量纲数据的融合，或是机械地采用固定参数进行融合，导致系统准确性较差。另外，传统的组合导航滤波器并没有考虑到磁偏角以及磁倾角，而在工程实践中发现，如不考虑磁偏角和磁倾角的影响，则会导致在全球不同地区的导航效果出现较大差异。以上因素都大大制约了多传感器的数据融合，以及基于多传感器数据融合的导航方法的精度。

发明内容

有鉴于此，因此本发明提出一种微型无人机基于多传感器数据融合的导航方法，对多传感器数据进行时间对准，并对传感器的噪声进行动态滑动估计，将所述滑动噪声估计输入卡尔曼滤波器中，作为量测校正时的权重，并且可对相同量纲的数据进行增益分配，提高算法的精度；同时使用改进的卡尔曼滤波方法，充分考虑磁偏角和磁倾角的影响，所述方法具有运算量小，扩展方便，解算精度高等优点。

本发明实施例提供了一种微型无人机基于多传感器数据融合的导航方法，所述微型无人机包括多传感器、控制单元、存储单元及总线，所述多传感器包括惯性传感器IMU、GPS、磁罗盘、光流传感器和雷达，所述方法包含以下步骤：

步骤一：信号获取，获取惯性传感器IMU、GPS、磁罗盘、光流传感器和雷达的输出信号；

步骤二：时间对准，对所述获取的信号进行时间对准，实现相位对齐；

步骤三：滑动噪声估计，基于滑动窗口对所述多传感器的数据进行噪声估计，得到滑动噪声估计量；

步骤四：信号增益分配，基于所述滑动噪声估计量对所述输出信号中相同量纲的数据进行增益分配，得到融合后的数据；

步骤五：将所述融合后的数据、所述输出信号和所述滑动噪声估计量输入至控制单元；

步骤六：所述控制单元执行卡尔曼滤波，解算所述微型无人机的速度和位置。

优选地，所述步骤二中，采用基于时间戳的软件缓冲区时间对准技术进行时间对准。

优选地，所述步骤三中，滑动噪声估计的具体步骤为：在所述存储单元中建立5个长度为n的环形缓冲区，用于存放所述多传感器的数据，设置采样窗口为n，从任一传感器采样得到的数据被对应地存储于所述缓冲区中，记作buf[n]＝[x₁ … x_n]，x₁ … x_n表示采样得到的数据；计算采样数据的均值和方差：

所述方差即为滑动噪声估计量。

优选地，所述步骤四中信号增益分配的具体步骤包括：

获取同量纲的输出信号，记作{M₁，M₂…M_n}，获取所述同量纲的输出信号的方差，记作{S₁，S₂…S_n}，其中1<n<5,n为正整数；

将所述方差数据进行归一化处理，具体计算方式为：

其中，S_j∈{S₁,S₂...S_n}，计算融合后的所述输出信号的值，具体计算方式为：

优选地，所述卡尔曼滤波器采用四元数q＝[q₀ q₁ q₂ q₃]，三个位置p＝[p_n p_ep_d]，三个速度v＝[v_n v_e v_d]，三轴加速度计零偏b_a＝[b_ax b_ay b_az]，三轴陀螺零偏b_ω＝[b_ωxb_ωy b_ωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为状态量，记作

X＝[q₀ q₁ q₂ q₃ p_n p_e p_d v_n v_e v_d b_ax b_ay b_az b_ωx b_ωy b_ωz α γ]；

记角速度向四元数的转换矩阵为

机体系向导航系的转换矩阵为

其转置为

三轴加速度为f＝[f_x f_y f_z]，三轴角速度为ω＝[ω_x ω_y ω_z]；

系统的状态方程可以表示为：

其中

为系统状态的导数，f(x)为系统状态的函数，w为系统噪声，Z为系统的量测状态，h(x)为系统的量测状态函数，v为量测噪声。

记

上式具体化为：

设量测数据中位置Z_p＝[Z_pn Z_pe Z_pd]^T，速度为Z_v＝[Z_vn Z_ve Z_vd]^T，机体的三轴磁场强度为Z_m＝[Z_mx Z_my Z_mz]^T，B_e为地球磁场矢量和，其为一已知常量；

则系统的量测向量可以表示为

Z＝[Z_pn Z_pe Z_pd Z_vn Z_ve Z_vd Z_mx Z_my Z_mz]^T (9)

对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，可得系统线性化后的状态方程

优选地，其特征在于，所述卡尔曼滤波包括时间更新和量测修正；

所述时间更新的具体步骤包括：

其中，向量

为系统状态向量的一步预测值，向量

为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，矩阵P_k/k-1为状态向量的一步预测方差矩阵，矩阵P_k/k-1为状态向量的上一时刻方差矩阵，矩阵Q_k为系统噪声方差阵；

所述量测修正的具体步骤包括：

P_k＝(I-K_kH_k)P_k/k-1 (15)

其中，向量

为系统状态向量的当前最优估算值，矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差阵，矩阵I为单位矩阵。

优选地，所述滑动噪声估计量还作为量测校正时，测量数据的权重：

其中，所述滑动噪声估计量与W成反比。

优选地，在进行卡尔曼滤波时，为了消除数值误差的影响，对非负定方差矩阵坐如下分解：

设矩阵D为n阶方阵，其只有对角线元素为非0元素，通过使用归一化的方法，将矩阵D元素的数据限定在[0,1]中，防止因为数据尺度的不同造成发散，具体归一化方法如下：

此时矩阵D即完成了尺度变换。

本发明提出一种微型无人机基于多传感器数据融合的导航方法，对多传感器数据进行时间对准，并对传感器的噪声进行动态滑动估计，将所述滑动噪声估计输入卡尔曼滤波器中，作为量测校正时的权重，并且基于所述滑动噪声估计量可对相同量纲的数据进行增益分配，提高了算法的精度；同时使用改进的卡尔曼滤波方法，充分考虑磁偏角和磁倾角的影响，具有较高鲁棒性和扩展性；并且针对UD序贯更新的卡尔曼滤波器，考虑到在进行UD序贯更新时，会出现对矩阵D对角线元素的除法计算，如果对角线元素之间的差异较大，可能会出现计算误差，因此对对角线元素进行尺度变换，计算完成之后，再变换回之前的尺度。所述方法具有运算量小，扩展方便，解算精度高等优点。

附图说明

图1是本发明实施例一提供的微型无人机的硬件结构图；

图2是本发明实施例二提供的一种微型无人机基于多传感器数据融合的导航方法流程图；

图3是本发明实施例三提供的一种基于低成本惯性传感器在机动环境下的位置速度更新方法的流程图

具体实施方式

为了使本发明的目的、技术方案和优点更加清楚，下面结合附图对本发明具体实施例作进一步的详细描述。可以理解的是，此处所描述的具体实施例仅仅用于解释本发明，而非对本发明的限定。

另外还需要说明的是，为了便于描述，附图中仅示出了与本发明相关的部分而非全部内容。在更加详细地讨论示例性实施例之前应当提到的是，一些示例性实施例被描述成作为流程图描绘的处理或方法。虽然流程图将各项操作(或步骤)描述成顺序的处理，但是其中的许多操作可以被并行地、并发地或者同时实施。此外，各项操作的顺序可以被重新安排。当其操作完成时所述处理可以被终止，但是还可以具有未包括在附图中的附加步骤。所述处理可以对应于方法、函数、规程、子例程、子程序等等。

实施例一

本发明实施例一具体可以应用在需要高精度导航的产品，如无人机定位，自动驾驶，数字城市，机器人导航等。图1为本发明实施例一提供的一种微型无人机的硬件结构图。本实施例所述的微型无人机具体包括：多传感器、控制单元(160)、存储单元及总线，所述多传感器包括惯性传感器IMU(110)，GPS(120)、磁罗盘(130)、光流传感器(140)和雷达(150)。控制单元(160)执行卡尔曼滤波，进行量测修正，解算所述微型无人机的速度(170)和位置(180)。

实施例二

本发明实施例二具体可以应用在需要高精度导航的产品，如无人机定位，自动驾驶，数字城市，机器人导航等。图2为本发明实施例一提供的一种微型无人机基于多传感器数据融合的导航方法的流程图。本实施例的方法具体包括：

210、信号获取，获取惯性传感器IMU、GPS、磁罗盘、光流传感器和雷达的输出信号。

220、时间对准，对所述获取的信号进行时间对准，实现相位对齐。

在本实施例中，采用基于时间戳的软件缓冲区时间对准技术进行时间对准。

230、滑动噪声估计，基于滑动窗口对所述多传感器的数据进行噪声估计，得到滑动噪声估计量。

在本实施例中，滑动噪声估计的具体步骤为：在所述存储单元中建立5个长度为n的环形缓冲区，用于存放所述多传感器的数据，设置采样窗口为n，从任一传感器采样得到的数据被对应地存储于所述缓冲区中，记作buf[n]＝[x₁ … x_n]，x₁ … x_n表示采样得到的数据；计算采样数据的均值和方差：

所述方差即为滑动噪声估计量。

240、信号增益分配，基于所述滑动噪声估计量对所述输出信号中相同量纲的数据进行增益分配，得到融合后的数据。

在本实施例中，信号增益分配的具体步骤包括：

获取同量纲的输出信号，记作{M₁，M₂…M_n}，获取所述同量纲的输出信号的方差，记作{S₁，S₂…S_n}，其中1<n<5,n为正整数；

将所述方差数据进行归一化处理，具体计算方式为：

其中，S_j∈{S₁,S₂...S_n}，计算融合后的所述输出信号的值，具体计算方式为：

具体地，假设当前光流的速度方差为S_flow，当前光流的速度为V_flow，GPS输出为水平速度方差为S_gps，当前GPS的水平速度为V_gps，融合后的速度为V_f。首先将方差数据归一化，则归一化光流速度方差为:

归一化后GPS的速度方差为:

计算融合后的速度为：

250、速度位置解算，将所述融合后的数据、所述输出信号和所述滑动噪声估计输入至控制单元，所述控制单元执行卡尔曼滤波，进行量测修正，解算所述微型无人机的速度和位置。

本发明实施例二提供了一种微型无人机基于多传感器数据融合的导航方法，对多传感器数据进行时间对准，并对传感器的噪声进行动态滑动估计，基于所述滑动噪声估计量对相同量纲的数据进行增益分配，提高了算法的精度。

实施例三

图3是本发明实施例三提供的一种微型无人机基于多传感器数据融合的位置速度更新方法的流程图。本实施例的方法具体包括：

设置载体坐标系，所述载体坐标系三轴表示为记作xyz，所述载体坐标系与载体固联，坐标系符合右手法则，原点在载体重心处，x轴指向载体前进方向，y轴由原点指向载体右侧，z轴方向根据xy轴由右手法则确定。

通过惯性传感器确定当前时刻的载体姿态，记作att＝[φ θ ψ]，其中φ代表滚转角，θ代表俯仰角，ψ代表航向角。

确定载体坐标系向导航坐标系的转换矩阵，记作

简化转换矩阵为以下形式

构建卡尔曼滤波器，所述卡尔曼滤波器采用四元数q＝[q₀ q₁ q₂ q₃]，三个位置p＝[p_n p_e p_d]，三个速度v＝[v_n v_e v_d]，三轴加速度计零偏b_a＝[b_ax b_ay b_az]，三轴陀螺零偏b_ω＝[b_ωx b_ωy b_ωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为状态量，记作系统状态

X＝[q₀ q₁ q₂ q₃ p_n p_e p_d v_n v_e v_d b_ax b_ay b_az b_ωx b_ωy b_ωz α γ]；

系统的状态方程可以表示为：

其中

为系统状态的导数，f(x)为系统状态的函数，w为系统噪声，Z为系统的量测状态，h(x)为系统的量测状态函数，v为量测噪声。

记

上式具体化为：

设量测数据中位置Z_p＝[Z_pn Z_pe Z_pd]^T，速度为Z_v＝[Z_vn Z_ve Z_vd]^T，机体的三轴磁场强度为Z_m＝[Z_mx Z_my Z_mz]^T，B_e为地球磁场矢量和，其为一已知常量；

则系统的量测向量可以表示为

Z＝[Z_pn Z_pe Z_pd Z_vn Z_ve Z_vd Z_mx Z_my Z_mz]^T (3-6)

对f(x)和h(x)其求解雅克比矩阵得到矩阵F和H，可得系统线性化后的状态方程

在本实施例中，所述卡尔曼滤波包括时间更新和量测修正。

310、时间更新，在本实施例中，时间更新具体步骤包括：

其中，向量

为系统状态向量的一步预测值，向量

为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，矩阵P_k/k-1为状态向量的一步预测方差矩阵，矩阵P_k/k-1为状态向量的上一时刻方差矩阵，矩阵Q_k为系统噪声方差阵。

320、尺度归一化，在本实施例中，尺度归一化的具体步骤包括：

在进行卡尔曼滤波时，为了消除数值误差的影响，对非负定方差矩阵坐如下分解：

设矩阵D为n阶方阵，其只有对角线元素为非0元素，通过使用归一化的方法，将矩阵D元素的数据限定在[0,1]中，防止因为数据尺度的不同造成发散，具体归一化方法如下：

此时矩阵D即完成了尺度变换。

330、时间更新完成；

340、判定是否进行量测更新，如果否，则进行速度位置解算；如果是，则进行量测校正；

350、量测校正，在本实施例中，所述量测校正的具体步骤包括：

P_k＝(I-K_kH_k)P_k/k-1 (3-15)

其中，向量

为系统状态向量的当前最优估算值，矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差阵，矩阵I为单位矩阵。

进一步地，所述滑动噪声估计量还作为量测校正中测量数据的权重：

其中，所述滑动噪声估计量与W成反比。

360、获得速度位置解算结果。

本发明实施例三提供了一种基于低成本惯性传感器在机动环境下的位置速度更新方法，使用改进的卡尔曼滤波方法，充分考虑磁偏角和磁倾角的影响，具有较高鲁棒性和扩展性；并且针对UD序贯更新的卡尔曼滤波器，考虑到在进行UD序贯更新时，会出现对矩阵D对角线元素的除法计算，如果对角线元素之间的差异较大，可能会出现计算误差，因此对对角线元素进行尺度变换，计算完成之后，再变换回之前的尺度。另外，利用计算的滑动噪声估计量作为量测校正中测量数据的权重，可以提高系统的适应性，提高导航的精度。所述方法具有运算量小，扩展方便，解算精度高等优点。

在此请注意，上述仅为本发明的较佳实施例及所运用技术原理。本领域技术人员会理解，本发明不限于这里所述的特定实施例，对本领域技术人员来说能够进行各种明显的变化、重新调整和替代而不会脱离本发明的保护范围。因此，虽然通过以上实施例对本发明进行了较为详细的说明，但是本发明不仅仅限于以上实施例，在不脱离本发明构思的情况下，还可以包括更多其他等效实施例，而本发明的范围由所附的权利要求范围决定。

Claims (7)

1.一种微型无人机基于多传感器数据融合的导航方法，所述微型无人机包括多传感器、控制单元、存储单元及总线，所述多传感器包括惯性传感器IMU、GPS、磁罗盘、光流传感器和雷达，其特征在于，所述方法包含以下步骤：

步骤一：信号获取，获取惯性传感器IMU、GPS、磁罗盘、光流传感器和雷达的输出信号；

步骤二：时间对准，对所述获取的信号进行时间对准，实现相位对齐；

步骤三：滑动噪声估计，基于滑动窗口对所述多传感器的数据进行噪声估计，得到滑动噪声估计量；

步骤四：信号增益分配，基于所述滑动噪声估计量对所述输出信号中相同量纲的数据进行增益分配，得到融合后的数据；

步骤五：将所述融合后的数据、所述输出信号和所述滑动噪声估计量输入至控制单元；

步骤六：所述控制单元执行卡尔曼滤波，解算所述微型无人机的速度和位置；

所述执行卡尔曼滤波包括以下步骤：采用四元数q＝[q₀ q₁ q₂ q₃]，三个位置p＝[p_n p_ep_d]，三个速度v＝[v_n v_e v_d]，三轴加速度计零偏b_a＝[b_ax b_ay b_az]，三轴陀螺零偏b_ω＝[b_ωxb_ωy b_ωz]，一个磁偏角α，一个磁倾角γ这18个状态量作为状态量，记作X＝[q₀ q₁ q₂ q₃ p_np_e p_d v_n v_e v_d b_ax b_ay b_az b_ωx b_ωy b_ωz α γ]；

记角速度向四元数的转换矩阵为

机体系向导航系的转换矩阵为

三轴加速度为f＝[f_x f_y f_z]，三轴角速度为ω＝[ω_x ω_y ω_z]；

系统的状态方程可以表示为：

其中

为系统状态的导数，f(X)为系统状态的函数，w为系统噪声，Z为系统的量测状态，h(X)为系统的量测状态函数，v为量测噪声；

记

公式(1)具体化为：

设量测数据中位置Z_p＝[Z_pn Z_pe Z_pd]^T，速度为Z_v＝[Z_vn Z_ve Z_vd]^T，机体的三轴磁场强度为Z_m＝[Z_mx Z_my Z_mz]^T，B_e为地球磁场矢量和，其为一已知常量；

则系统的量测向量可以表示为

Z＝[Z_pn Z_pe Z_pd Z_vn Z_ve Z_vd Z_mx Z_my Z_mz]^T

对f(X)和h(X)求解雅克比矩阵得到矩阵F和H，可得系统线性化后的状态方程

2.根据权利要求1所述的方法，其特征在于，所述步骤二中，采用基于时间戳的软件缓冲区时间对准技术进行时间对准。

3.根据权利要求1所述的方法，其特征在于，所述步骤三中，滑动噪声估计的具体步骤为：在所述存储单元中建立5个长度为n的环形缓冲区，用于存放所述多传感器的数据，设置采样窗口为n，从任一传感器采样得到的数据被对应地存储于所述缓冲区中，记作buf[n]＝[x₁ … x_n]，x₁ … x_n表示采样得到的数据；计算采样数据的均值和方差：

所述方差即为滑动噪声估计量。

4.根据权利要求3所述的方法，其特征在于，所述步骤四中信号增益分配的具体步骤包括：

获取同量纲的输出信号，记作{M₁，M₂…M_n}，获取所述同量纲的输出信号的方差，记作{S₁，S₂…S_n}，其中1<n<5,n为正整数；

将所述方差数据进行归一化处理，具体计算方式为：

其中，S_j∈{S₁,S₂...S_n}，计算融合后的所述输出信号的值，具体计算方式为：

5.根据权利要求1所述的方法，其特征在于，所述卡尔曼滤波包括时间更新和量测修正；

所述时间更新的具体步骤包括：

其中，向量

为系统状态向量的一步预测值，向量

为上一时刻的估计值，矩阵Φ为矩阵F的离散化形式，Φ_k/k-1为一步转移矩阵，矩阵P_k/k-1为状态向量的一步预测方差矩阵，矩阵P_k-1为状态向量的上一时刻方差矩阵，矩阵Q_k为系统噪声方差阵；

所述量测修正的具体步骤包括：

P_k＝(I-K_kH_k)P_k/k-1 (13)

其中，向量

为系统状态向量的当前最优估算值，矩阵K_k为增益矩阵，矩阵H_k为量测矩阵，矩阵R_k为量测噪声序列方差阵，矩阵I为单位矩阵。

6.根据权利要求5所述的方法，其特征在于，所述滑动噪声估计量还作为量测校正时，测量数据的权重：

其中，所述滑动噪声估计量与W成反比。

7.根据权利要求6所述的方法，其特征在于，在进行卡尔曼滤波时，为了消除数值误差的影响，对非负定方差矩阵做如下分解：

设矩阵D为n阶方阵，其只有对角线元素为非0元素，通过使用归一化的方法，将矩阵D元素的数据限定在[0,1]中，防止因为数据尺度的不同造成发散，具体归一化方法如下：

此时矩阵D即完成了尺度变换。

Images