CN117191057A - 基于时空配准和多模矢量分配融合的导航平台构建方法 - Google Patents

Abstract

本发明公开了一种基于时空配准和多模矢量分配融合的导航平台构建方法。该方法为：首先安装车载组合导航平台；然后多传感器时空配准；接着建立多源融合子系统框架；再设计自适应矢量分配因子和联邦架构多模噪声集；针对时间误差采用最小二乘法结合扩展卡尔曼滤波估计来解决时间配准中的各传感器采样频率不一致，采样数据量测误差和开机时刻差异的问题；针对空间误差，采用多传感器坐标变换方式来有效的解决各传感器间的空间配准问题；最后执行系统任务。本发明提高了车载导航系统运行环境的可靠性和数据处理效率，提高了系统定位精度和系统的适应性。

Description

基于时空配准和多模矢量分配融合的导航平台构建方法

技术领域

本发明涉及无人车导航技术领域，特别是一种基于时空配准和多模矢量分配融合的导航平台构建方法。

背景技术

现有的导航技术往往采取多个信息源进行融合，而非依靠单一信息源提供导航参数，因此构成了多传感器数据融合系统。目前的市场领域中，无人车导航平台通常采用多传感器融合进行导航定位，用以提升导航精度。在进行无人车载导航平台设计与搭建过程中，车载传感器的时空配准和导航信息融合算法是侧重点。前者涉及到进入融合中心的各传感器数据是否处于同一坐标系，融合数据是否在同一时刻；后者则决定了融合后的导航定位精度。

在实际的多传感器系统中，由于各传感器的执行任务不同、自身性能的优劣、所处环境存在差异等多方面的原因，使得各传感器的量测数据不同步，若直接进行信息融合会使得融合结果不理想甚至发散，因此需要将传感器在不同时间不同空间下的量测数据转换到统一的融合时间和空间，即进行时空配准处理，再将数据发送到融合中心进行融合处理。

当数据发送至融合模块后，需要通过有效的算法将信息量进行融合，输出高精度的导航信息用以载体定位。通过滤波方式来对多传感器进行融合需要借助于联邦架构，而要提升系统精度，得到最优估计值，就要从信息分配因子着手，改变以往单一的标量分配方式，深入到任一子滤波器的每一状态变量，从而充分发挥联邦架构中各子滤波器的性能。此外，由于联邦架构中所采用的子系统滤波器通常是标准卡尔曼滤波器，其要求是量测噪声及过程噪声必须是已知且不变的，但实际应用过程中，器件本身的噪声特性和外界噪声是处于时变状态，这就使得标准卡尔曼滤波器的应用受到了限制。因此如何动态优化信息分配因子，重构时变噪声，成为了重点研究方向。

发明内容

本发明的目的在于提供一种可靠性高、数据处理效率高、定位精度高、适用性强的无人车载导航平台设计方法。

实现本发明目的的技术解决方案为：一种基于时空配准和多模矢量分配融合的导航平台构建方法，包括以下步骤：

步骤1、安装车载组合导航平台；

步骤2、多传感器时空配准，包括时间配准和空间配准；

步骤3、建立多源融合子系统框架；

步骤4、设计自适应矢量分配因子；

步骤5、设计联邦架构多模噪声集；

步骤6、执行系统任务。

本发明与现有技术相比，具有以下优点：(1)采用型号为研华MIC-770V2的工控机，基于Linux下的ROS系统进行应用开发，提升了系统运行环境的可靠性和数据处理效率，避免计算中出现的内存资源不足问题；(2)利用惯导、激光雷达、双目视觉、里程计、高度计、磁强计等6种传感器进行多源信息融合，在卫星拒止条件下能够保持系统定位精度，提高系统的适应性；(3)采用最小二乘法结合扩展卡尔曼滤波估计和多传感器坐标变换方法有效解决实际车载系统的时空配准问题，解决了平台搭建中出现的固有时空配准问题，提高输入融合信息的准确性；(4)改进传统的联邦分配因子，针对分配因子由传统标量的平均分配转变为针对各子系统的每一维变量的矢量分配，联合系统噪声阵和量测噪声阵，构建噪声模型集，提高了传感器在实际融合中的性能。

附图说明

图1是本发明基于时空配准和多模矢量分配融合的导航平台构建方法的流程图。

图2是本发明实施例中基于时空配准和多模矢量分配融合的导航平台的系统框图。

图3是本发明实施例中传感器安装位置的系统框图。

图4是本发明实施例中时间配准功能的模型结构示意图。

图5是本发明实施例中异步传感器采样结果图。

图6是本发明实施例中优化后的矢量信息分配算法框图.

图7是本发明实施例中IMM算法关系图。

图8是本发明实施例中多模噪声集重构算法图

图9是本发明实施例中优化后的多模矢量信息分配软件流程图。

图10是本发明实施例中系统任务调度进程图。

具体实施方式

下面结合附图和具体实施例，对本发明进一步的详细说明。

针对现有技术存在的缺陷及考虑到实际工程领域应用的可行性，本发明给出一种基于时空配准及多模矢量分配融合方法的无人车载导航平台设计。在平台搭建中出现的时空配准问题进行分析。对于时间误差，各传感器所采用的时间基准不同；各传感器的采样周期不同步，开机时刻的不一致；各传感器与主机进行数据传输所采用的通信方式差异产生的时间差。对于空间误差，则主要是不同传感器的坐标系体系在进行定位时产生的偏差；导航传感器使用不同的参考坐标系；各传感器自身的安装误差以及相互间之间的相对位置误差。因此针对时间误差采用最小二乘法结合扩展卡尔曼滤波估计来解决时间配准中的各传感器采样频率不一致，采样数据量测误差和开机时刻差异的问题；针对空间误差，采用多传感器坐标变换方式来有效的解决各传感器间的空间配准问题。

为解决卫星拒止问题，提升系统在多环境下的导航定位性能，本发明采用了除卫导外的几种常用传感器，包括但不限于惯导(SINS)、激光雷达(LIDAR)、双目视觉(VIS)、里程计(ODO)、高度计(ALT)、磁强计(MAG)等6种传感器实现基于多传感器矢量分配的组合导航定位。并且，针对系统和环境噪声时变的特性，设计了针对过程噪声方差阵Q和量测噪声R的模型集。

结合图1，本发明一种基于时空配准和多模矢量分配融合的导航平台构建方法，包括以下步骤：

步骤1、安装车载组合导航平台；

步骤2、多传感器时空配准；

步骤3、建立多源融合子系统框架；

步骤4、设计自适应矢量分配因子；

步骤5、设计联邦架构多模噪声集；

步骤6、执行系统任务。

作为一种具体示例，步骤1中，安装车载组合导航平台，具体如下：

如图2所示为本发明的系统框图，图3为传感器安装示意图系统初始上电后，传感器采集数据信息，并通过相应的通信方式发送到工控机上进行实时解包；车辆静止状态下将原始数据通过先前计算进行传感器的时空配准，然后发送到导航计算机进行解算和融合处理并将其转换为导航数据进行输出。

步骤1.1、将惯性测量单元、激光雷达、双目相机、里程计、高度计、航姿参考系统、组合导航系统固联在车体上，对定位车辆搭载的7种传感器进行自检，确保其正常工作；

进一步地，所述惯性测量单元采用STIM300惯性测量单元，所述激光雷达采用镭神智能C16激光雷达，所述双目相机采用intel D455双目相机，所述里程计采用KYDBL4850-2E里程计，所述高度计采用BMP388高度计，所述航姿参考系统采用HS-AHRS-901航姿参考系统，所述组合导航系统采用SPAN-KVH1750分步式闭环光纤组合导航系统；

将STIM300惯性测量单元，镭神智能C16激光雷达，intel D455双目相机，KYDBL4850-2E里程计，BMP388高度计，HS-AHRS-901航姿参考系统，SPAN-KVH1750分步式闭环光纤组合导航系统固联在车体上，然后对定位车辆搭载的7种传感器进行自检，确保其正常工作；

步骤1.2、将计算处理单元安装在定位车辆车体上，设置以车体中心处东北天导航坐标系作为融合中心处理的坐标系；

例如，所述计算处理单元采用研华MIC-770V2工控机，将其安装在车体上，设置以车体中心处东北天导航坐标系作为融合中心处理的坐标系；

步骤1.3、测量各传感器距融合中心点处的相对位移以及三轴姿态角；

步骤1.4、供电装置对车载传感器和工控机进行供电；激光雷达和里程计采用车载电源，输出电压12V；工控机采用12V直流转220V交流逆变器进行供电；参考卫导基准采用外接电池块进行供电；其余传感器通过串口连接工控机进行供电；

所述供电装置包括12V直流蓄电池、直流12V转220V交流逆变器和220V交流转直流的可调直流电源，工作时使用直流12V转220V交流逆变器将蓄电池的直流逆变为220V交流统一对外供电；用电设备中的直流部分使用可调电源进行转换之后进行供电，220V交流部分使用多孔交流插排拓展之后进行供电。

步骤1.5、调试各传感器与工控机之间的通信连接，惯性测量单元通过RS422串口服务器与工控机进行通信；激光雷达使用百兆以太网UDP/IP协议与工控机进行通信，使用前需要先进行网关地址和子网掩码的配置；双目相机和航姿参考系统通过USB3.0串口连接工控机进行通信；里程计通过RS485总线进行通信；高度计使用I2C通信方式；组合导航基准系统通过CAN总线进行通信；

步骤1.6、接通电源，进行系统上电初始化，车体初始静止2min进行静态数据采集，利用惯性测量单元中加速度计和陀螺仪所采集数据进行零偏计算，设定3轴加速度计和陀螺仪输出数据为A_xi，A_yi，A_zi和w_xi，w_yi，w_zi；各轴平均输出加速度和角速度为 和数据总数N，则有：

步骤1.7、自对准计算得到初始俯仰角θ₀和横滚角γ₀；

步骤1.8、将采集到的各传感器数据以及各传感器启动时间发送到时空配准模块，选取传感器频率中的最低频率作为时间配准频率和信息融合频率，对传感器的量测和开机时差进行时间配准，并且各坐标系转换统一到融合中心；

步骤1.9、将配准后的数据发送到融合中心，进行信息融合，并输出导航信息量。

作为一种具体示例，步骤2中，多传感器时空配准，包括时间配准和空间配准，具体如下：

进一步地，所述时间配准包含观测数据分析、先验知识库以及配准要求、配准方法选择、配准频率选择、时间配准处理、配准数据分析这些模块，其时间配准功能模型图如图4所示。

观测数据分析模块：是进行时间配准前的一步操作，具体包括传感器的具体数目，传感器的类型，传感器的采样周期以及不同采样数据的起始时间差；

先验知识与配准要求模块：包含有在系统中传感器的先验信息，具体有传感器的采样周期以及传感器的采样精度等，同时也包含有时间配准需要达到的配准精度，对于是否要满足实时性等要求；

配准方法选择模块：根据先验知识与配准要求模块的信息提供，来选择最适合的时间配准方法；

配准频率选择模块：选择多传感器系统时间配准的合适的配准频率；

时间配准处理模块：对传感器所提供的量测数据进行时间配准处理；

配准数据分析模块：对时间配准处理模块输出的配准结果进行相应的处理分析，并将结果反馈至先验知识与配准要求模块进行反馈调整。

(2.1)所述时间配准，具体如下：

步骤2.1.1、传感器的采样频率不一致，开机时刻的差异以及各传感器与主机通信的延时会很大程度的影响融合效果，造成导航精度损失，本发明采用一种基于最小二乘法结合扩展卡尔曼滤波估计的方法可以有效改善此类问题。

最小二乘配准法是利用最小二乘法通过传感器的量测值估计出配准时刻的值，解决频率配准的问题。现有采样周期不同的传感器，从中任一选取两个传感器A、B，令其采样周期为T_a,T_b，且有T_a/T_b＝N，其中N为整数，传感器A的采集频率大于传感器，传感器A，B对同一目标的采样结果如图5所示。

目标状态相邻两次的更新时刻为(k-1)T_b和kT_b，由于传感器B的采样周期是传感器A的采样周期的整数倍N，则kT_b＝[(k-1)T_b+NT_a],即传感器B在进行一次量测的时间内传感器A已经进行了N次量测更新；根据最小二乘法配准原理，传感器B在进行一次采样的时间内，把传感器A在相同时间内的N次量测数据进行融合，估计出与传感器B采样时刻同步的虚拟量测数据。

传感器A在传感器B的一次量测时间内即(k-1)T_b到kT_b区间内共进行了N次量测，可以用Z_N＝[z₁,z₂,…,z_N]^T来表示，传感器A的第N个量测值z_N和传感器B的量测值的采样时刻是一致的，传感器A的N次量测值进行融合后的结果z及其导数z'可以用U＝[z,z']^T来表示；

传感器A的任一量测值z_i通用表示为：

z_i＝z+(i-N)T_a·z'+v_ii＝1,2,...,N

用v_i表示量测噪声，将上式转换为向量形式：

Z_N＝W_NU+V_N

式中，W_N为对应量测系数矩阵，V_N＝[v₁,v₂,...,v_N]^T，量测噪声的均值为零，方差为其中σ²为量测噪声的方差；

由最小二乘规则得线性最小方差估计的性能指标函数J为：

要使J最小，通过对U求偏导数并使其等于零得：

可以得到：

方差阵估值为：

将N个测量值进行融合，得到传感器A的虚拟量测值以及量测噪声方差分别为：

其中：c₁＝-2/N,c₂＝6/[N(N+1)]

步骤2.1.2、除了传感器的量测数据到来时刻不一致外，另一大主要影响因素则是开机时刻的不同。由于在ROS系统下，不同传感器的开启要输入不同指令，其输入的前后间隔时间差所造成的信息不同步会使得时间配准的误差增大，因此本发明采用一种基于扩展卡尔曼滤波的估计方法，来估计不同传感器的开机时刻差，从而减小时间配准误差。

扩展卡尔曼滤波建立在线性卡尔曼滤波的基础之上，其核心思想是对非线性系统用泰勒级数展开，忽略其高阶项，得到近似线性化模型，然后进行滤波；

设非线性模型为：

式中：X_k是n维状态向量；f(X_k-1)是n维非线性向量函数；Z_k是m维量测向量；h(X_k-1)是m维非线性向量函数；V_k为量测噪声。

对应时间更新为：

其中：Φ_k/k-1，P_k-1，Γ_k-1和Q_k-1是已知的系统结构参数，分别称为n阶状态一步转移矩阵，n×n阶的状态估计均方误差阵，n×l阶的系统噪声分配矩阵和l×l阶的系统噪声矩阵。

量测更新为：

设定传感器B的开机时刻滞后于传感器A的开机时刻，且滞后的时间差为Δt，传感器A的量测数据为r_A(t_k)，其中t_k(k＝1…n)为传感器A的采样时刻，则有传感器B的量测数据为r_B(t_k+Δt)，其中t_k+Δt(k＝1...n)为传感器B的采样时刻，若消除了开机时刻的时间差，则传感器B的采样时刻和传感器A的采样时刻同步，则选取载体的状态量为：

对应的状态方程为：

X_k＝φ_k/k-1X_k+Γ_k/k-1μ_k

系统状态转移矩阵φ、系统噪声矩阵Γ和系统噪声方差阵分别为：

Γ＝[T³/6 T²/2 T 0]

Q＝σ²

考虑系统的量测噪声，选取传感器B在采样时刻t_k+Δt下的量测数据为观测量，则有：

其中，v₁为均值为零方差为的高斯白噪声，v₂为均值为零方差为/>的高斯白噪声，且v₁和v₂互不相关，则量测噪声方差阵R为：

由状态方程可得：

则忽略高阶项后的观测方程为：

再通过非线性卡尔曼滤波的时间更新和量测更新即可获得传感器A和传感器B的开机时刻时间差以及传感器B在消除开机时间差的量测值的估计值；

(2.2)所述空间配准，具体如下：

在搭建实际车载组合导航平台时，基于不同传感器的功能特性，车体平台构造等因素，传感器的安装位置不尽相同，存在空间上的相对安装位置；此外，进行数据融合时，需要将各传感器的坐标系统一到融合中心进行数据融合，而不同传感器其坐标轴的定义也不相同，因此需要对各传感器进行空间上的配准，此处给出一种坐标变换理论的方法，将各传感器进行坐标轴统一。

空间几何理论将导航坐标系统一分成三类：不同系下大地坐标系统一，不同系下直角坐标系统一，同系下大地坐标系和直角坐标系统一。为了便于进行坐标转换，使用不同系下直角坐标系统一。

不同系下直角坐标系统一的数学表达式：

其中，ΔX、ΔY、ΔZ为两坐标系各轴相对平移距离，K为尺度因子，ε_X、ε_Y、ε_Z为欧拉角；X_old,Y_old,Z_old为转换前的三轴坐标；X_new,Y_new,Z_new为转换后的三轴坐标。

设定以车体中线中心作为坐标轴统一的融合中心，确定以东北天地理坐标系为融合中心的坐标系；通过测量，标定出各传感器自身坐标系距融合中心坐标系在3轴上的相对平移距离及3轴相对欧拉角；

针对激光雷达，相机传感器，由于其坐标系定义在Map系中，其坐标系原点以上电初始化所处位置为坐标原点建立坐标系，因此为了确定将这些传感器的坐标系与融合中心坐标系统一，首先需利用初始GNSS测量的导航信息量，即地理系下的3维姿态角构造姿态矩阵变换，将Map系下的初始相对位置信息X_map,Y_map,Z_map转换得到当前时刻的雷达直角坐标系下，其表达式如下：

将经过旋转矩阵变换后的X_{map_rot}，Y_{map_rot}，Z_{map_rot}通过上述式子对齐到融合中心坐标系下。

作为一种具体示例，步骤3中，建立多源融合子系统框架，具体如下：

进行多源信息融合前，首先要根据已有传感器进行子系统构建，将惯导作为公共参考系统，与其余传感器构成子系统包括：惯性/激光雷达、惯性/视觉、惯性/里程计、惯性/高度计、惯性/航姿系统共计5个子系统。

本系统选取东北天导航坐标系作为导航解算坐标系，采用间接法进行状态估计，选用惯性导航系统的导航输出参数误差、惯性仪表误差作为系统状态变量，具体包括为：三向惯性导航平台误差角δΦ，三向速度误差δV，三向位置误差δr，三向陀螺常值漂移ε_b，三向一阶马尔可夫漂移误差ε_r，三向加速度计零偏▽_a，以此构建组合导航系统的状态变量X为：

X＝[φ_e,φ_n,φ_u,δv_e,δv_n,δv_u,δL,δλ,δh,ε_bx,ε_by,ε_bz,ε_rx,ε_ry,ε_rz,▽_x,▽_y,▽_z]

根据系统状态向量X，可列出组合导航系统的状态方程为：

式中，F(t)表示系统的状态一步转移矩阵，G(t)表示系统噪声误差矩阵，W(t)表示系统白噪声随机误差矢量。

不同传感器所采集到的量测信息不同，因此所构建的量测方程也不一致，具体如下：

a：惯性/激光雷达子系统1

在该子系统中，将惯导在导航系下输出的纬度L_sins与经度λ_sins和激光雷达处理转换后的纬度L_lidar与经度λ_lidar分别作差，作为观测量，则量测方程如下：

b：惯性/视觉子系统2

在该子系统中，将惯导在导航系下输出的俯仰角θ_sins，横滚角γ_sins，航向角和位置信息与视觉处理后的俯仰角θ_visual，横滚角γ_visual，航向角/>与纬度L_visual，经度λ_visual分别作差，作为观测量，则量测方程如下：

c：惯性/里程计子系统3

在该子系统中，先将里程计在载体系下获取的前向速度经姿态矩阵/>变换到导航系下，然后与惯导在导航系下输出的三轴速度信息v_sinse，v_sinsn，v_sinsu作差，作为观测量，则量测方程如下：

d：惯性/高度计子系统4

在该子系统中，将惯导在导航系下输出的高度信息h_sins与气压高度计转换后的输出高度h_alt作差，作为观测量，则量测方程如下：

Z₄(t)＝[h_sins-h_alt]＝H₄(t)X(t)+V₄(t)

e：惯性/航姿系统子系统5

在该子系统中，将惯导在导航系下输出的航向角信息与航姿系统输出的航向角/>信息作差，作为观测量，则量测方程如下：

作为一种具体示例，步骤4中，设计自适应矢量分配因子，具体如下：

联邦滤波中决定融合最优估计值的重要因素之一就是信息分配因子，传统的固定分配及动态分配因子方式都是基于标量形式的，无法充分体现出每个状态量的变化，使系统最优估计达到最佳。因此本发明采用一种动态矢量分配算法，先从子系统入手，基于F范数，获得子系统层面的动态分配系数，然后提取子系统可观测矩阵的奇异值，深入到每个状态变量，使得每个状态变量具有各自的分配因子。优化后的矢量分配算法框图如图6所示。

步骤4.1、计算基于系统协方差矩阵F范数的分配系数A_i：

系统的估计精度在很大程度上可由系统协方差矩阵所反映，因此许多关于动态信息分配因子的优化都会从协方差矩阵入手，采用协方差矩阵的F范数来对子系统层面进行信息分配，具体公式如下：

步骤4.2、计算基于可观测性矩阵的分配系数B_i：

可观测度反映了系统状态变量可被观测的程度，通过分析系统的可观测度，可以帮助判断出传感器所观测信息的好坏，从而分析对滤波精度所造成的影响。考虑组合导航系统是时变的，对其进行可观测性分析复杂度较高，因此目前常采用分段线性定常系统(PWSC)的奇异值分解法进行分析。

分段线性定常系统模型如下：

式中：X(k)∈Rⁿ；F_j∈R^n×n；B_j∈P^n×s；U(k)∈R^s；W(k)∈R^l；Γ_j∈R^m×l；Z_j(k)∈R^m；H_j∈R^m×n，j＝1,2,…,q表示系统分段间隔序号；

设定某动态时间段内系统的可观测性矩阵为O，则其表达式如下：

O＝[H^T (HF)^T…(HF^n-1)^T]^T

对可观测性矩阵O进行奇异值分解，如下所示：

O＝USV^T

式中：U＝[u₁ u₂…u_nm]，V＝[v₁ v₂…v_n]均为正交矩阵，Λ_r×r＝diag(σ₁,σ₂,…,σ_r)，其中r，σ_i分别为矩阵O的秩和奇异值；较大的σ_i，对应的状态变量的可观测值也较高，反之，则可能会落入不可观测区间；

由于系统状态变量的可观测性与奇异值成正比，因此量测信息质量越高的子系统，其可观测性就越高，对应的奇异值就越大，相应的子系统的信息分配因子就应该越大；对5个子系统的可观测性矩阵分别进行奇异值分解，可获得对应状态变量X_i的任一分量x_ij的分配系数，公式如下：

则每个子系统状态变量X_i对应的分配系数形式如下：

可验证，此时的信息分配系数B_i满足信息守恒原理：

步骤4.3、计算优化的信息分配系数C_i：

经过子系统层面的信息分配和针对状态量的信息分配，可获得最终的优化信息分配系数C_i：

C_i＝β_i·B_i

将C_i代入信息反馈公式，可得：

/>

为确保滤波器的一致收敛稳定性，要求滤波过程中，误差协方差矩阵是对称阵，很明显，由上式可知，尽管p_g12＝p_g21，但是c_i1≠c_i2，这就导致p_g12/c_i1≠p_g21/c_i2，即优化反馈后的P_i(1,2)≠P_i(2,1)；

令可推导得：

此时子系统的误差协方差矩阵为对称阵，则改进后的信息反馈过程为：

作为一种具体示例，步骤5中，设计联邦架构多模噪声集，具体如下：

步骤5.1、多模型算法思想是构建多个并行且模型或系统参数各异的卡尔曼滤波器，任一滤波器之间独立，利用每个滤波器的先验信息及输出的残差信息，依据假设检验规则，得到每个滤波器所对应的模型与当前时刻系统真实模型的匹配概率，称为模型概率，最后通过加权融合算法进行最优估计。多模型算法中应用广泛的就是IMM算法，IMM算法的过程如图7所示；

建立包含n个常用滤波模型的模型集M，在无约束条件下，模型间的转换过程遵循一阶马尔可夫过程，记模型集M为M＝[m₁,m₂,…,m_n]，模型m_i到m_j的转移概率π_ij＝P{m_i/m_j}，一般由先验知识获得，且满足行和为1，通常将对角线上的元素设为较大值，其余元素值较小；

步骤5.2、输入交互：

对模型集中的n个模型进行状态交互混合以获得各模型滤波初值：

式中：和/>分别为模型m_j(j＝1,2,…n)在k-1时刻的初始状态估计和初始状态估计协方差；/>和/>分别为模型m_i(i＝1,2,…n)在k-1时刻的估计值；/>为k-1时刻模型i到j的模型混合概率，其表达式如下：/>

其中为模型m_i在k-1时刻的概率模型；

步骤5.3、模型滤波：

将输入交互中的初始状态估计和初始状态估计协方差作为滤波器的初值输入，对每个模型独立进行卡尔曼滤波，彼此之间相互独立,其过程与标准卡尔曼滤波一致；

设对第j个模型进行滤波，则表达式如下：

步骤5.4、模型概率更新：

此过程采用贝叶斯假设检验的方式，通过求取滤波器残差及其残差协方差矩阵，确定模型概率，完成更新；定义滤波后的测量残差为对应协方差矩阵/>计算公式如下：

似然函数表达式为：

其中：为模型j在k时刻的似然函数值，m为量测向量的维数；

模型概率更新表达式为：

式中：为模型m_j在k时刻的概率模型。

步骤5.5、融合输出：

最终得到的状态估计表达式：

状态估计值对应的协方差阵为：

每增加一个模型，首先就要增加一次滤波计算，还要相应的增加假设检验和数据融合的步骤，其次模型之间的转移概率也会需要作出相应的修改，所以在此种情况下，模型的增加会使得滤波算法过于复杂而且极大地增加了算法的分析难度；但是如果模型过少，那么难以体现出多模型滤波方法自适应的优势，因为模型的数量可以一定程度上影响到多模型滤波方法的自适应性。综合上述因素，针对主要融合子系统1惯性/激光雷达和子系统2惯性/视觉构建系统噪声和量测噪声模型集，每个模型集里共选择3个不同的模型。

系统噪声模型集：

惯性/激光雷达子系统量测噪声模型集：

R_lidar1＝diag((0.3/R_e)² (0.3/R_e)²)，R_lidar2＝3R_lidar1，R_lidar3＝6R_lidar1

惯性/视觉子系统量测噪声模型集：

R_visual1＝diag((0.0035)² (0.0035)² (0.0035)² (0.5/R_e)² (0.5/R_e)²)

R_visual2＝3R_visual1，R_visual3＝6R_visual1

初始模型概率矩阵：μ_k＝[1/3 1/3 1/3]

初始模型转移概率矩阵：

对应2个子系统执行多模噪声集重构算法图如图8所示，联合优化后的多模矢量融合信息分配软件流程图如图9所示。

作为一种具体示例，步骤6中，系统任务执行，结合图10，具体如下：

步骤6.1、对系统进行上电，完成各硬件的初始化工作，随机开启任务调度中心进行任务调度；

步骤6.2、进行传感器的授时工作，各传感器通过与主机通信，采用NTP协议，由ROS的sensor_msgs/TimeReference消息类型发布，获取统一的主机时间信息，完成授时后该进程挂起；

步骤6.3、获取CPU，驱动各传感器进行数据采集，将其发送到主机进行时空配准，任务完成后释放CPU；

步骤6.4、将配准后的导航数据发送至解算和融合处理模块，先进行惯导解算，随即判断是否到达融合时刻，若未到达则进入步骤6.5，若已到达则进入步骤6.6；

步骤6.5、不进入融合中心进行信息融合，进入休眠期等待下一帧惯导解算和判断；

步骤6.6、进入融合模块，针对5个子系统，采用多进程模式来并行处理，将最终融合结果进行输出，随即释放CPU。

本发明采用型号为研华MIC-770V2的工控机，基于Linux下的ROS系统进行应用开发，提升了系统运行环境的可靠性和数据处理效率，避免计算中出现的内存资源不足问题；利用惯导、激光雷达、双目视觉、里程计、高度计、磁强计等6种传感器进行多源信息融合，在卫星拒止条件下能够保持系统定位精度，提高系统的适应性；采用最小二乘法结合扩展卡尔曼滤波估计和多传感器坐标变换方法有效解决实际车载系统的时空配准问题，解决了平台搭建中出现的固有时空配准问题，提高输入融合信息的准确性；改进传统的联邦分配因子，针对分配因子由传统标量的平均分配转变为针对各子系统的每一维变量的矢量分配，联合系统噪声阵和量测噪声阵，构建噪声模型集，提高了传感器在实际融合中的性能。

Claims (10)

1.一种基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，包括以下步骤：

步骤1、安装车载组合导航平台；

步骤2、多传感器时空配准，包括时间配准和空间配准；

步骤3、建立多源融合子系统框架；

步骤4、设计自适应矢量分配因子；

步骤5、设计联邦架构多模噪声集；

步骤6、执行系统任务。

2.根据权利要求1所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤1所述的安装车载组合导航平台，具体如下：

步骤1.1、将惯性测量单元、激光雷达、双目相机、里程计、高度计、航姿参考系统、组合导航系统固联在车体上，对定位车辆搭载的7种传感器进行自检；

步骤1.2、将计算处理单元安装在定位车辆车体上，设置以车体中心处东北天导航坐标系作为融合中心处理的坐标系；

步骤1.3、测量各传感器距融合中心点处的相对位移以及三轴姿态角；

步骤1.4、供电装置对车载传感器和工控机进行供电；激光雷达和里程计采用车载电源，输出电压12V；工控机采用12V直流转220V交流逆变器进行供电；参考卫导基准采用外接电池块进行供电；其余传感器通过串口连接工控机进行供电；

步骤1.5、调试各传感器与计算处理单元之间的通信连接，惯性测量单元通过RS422串口服务器与计算处理单元进行通信；激光雷达使用百兆以太网UDP/IP协议与计算处理单元进行通信，使用前需要先进行网关地址和子网掩码的配置；双目相机和航姿参考系统通过USB3.0串口连接计算处理单元进行通信；里程计通过RS485总线进行通信；高度计使用I2C通信方式；组合导航基准系统通过CAN总线进行通信；

步骤1.6、接通电源，进行系统上电初始化，车体初始静止2min进行静态数据采集，利用惯性测量单元中加速度计和陀螺仪所采集数据进行零偏计算，设定3轴加速度计和陀螺仪输出数据为A_xi，A_yi，A_zi和w_xi，w_yi，w_zi；各轴平均输出加速度和角速度为 和/>数据总数N，则有：

步骤1.7、自对准计算得到初始俯仰角θ₀和横滚角γ₀；

步骤1.8、将采集到的各传感器数据以及各传感器启动时间发送到时空配准模块，选取传感器频率中的最低频率作为时间配准频率和信息融合频率，对传感器的量测和开机时差进行时间配准，并且各坐标系转换统一到融合中心；

步骤1.9、将配准后的数据发送到融合中心，进行信息融合，并输出导航信息量。

3.根据权利要求2所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，所述惯性测量单元采用STIM300惯性测量单元，所述激光雷达采用镭神智能C16激光雷达，所述双目相机采用intel D455双目相机，所述里程计采用KYDBL4850-2E里程计，所述高度计采用BMP388高度计，所述航姿参考系统采用HS-AHRS-901航姿参考系统，所述组合导航系统采用SPAN-KVH1750分步式闭环光纤组合导航系统；所述计算处理单元采用研华MIC-770V2工控机；所述供电装置包括12V直流蓄电池、直流12V转220V交流逆变器和220V交流转直流的可调直流电源，工作时使用直流12V转220V交流逆变器将蓄电池的直流逆变为220V交流统一对外供电；用电设备中的直流部分使用可调电源进行转换之后进行供电，220V交流部分使用多孔交流插排拓展之后进行供电。

4.根据权利要求3所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤2所述的时间配准包含观测数据分析、先验知识库以及配准要求、配准方法选择、配准频率选择、时间配准处理、配准数据分析这些模块；

观测数据分析模块：用于进行时间配准前的一步操作，具体包括传感器的具体数目，传感器的类型，传感器的采样周期以及不同采样数据的起始时间差；

先验知识与配准要求模块：包含有在系统中传感器的先验信息，具体有传感器的采样周期以及传感器的采样精度；同时也包含有时间配准需要达到的配准精度，是否要满足实时性的要求；

配准方法选择模块：根据先验知识与配准要求模块的信息提供，来选择时间配准方法；

配准频率选择模块：选择多传感器系统时间配准的配准频率；

时间配准处理模块：对传感器所提供的量测数据进行时间配准处理；

配准数据分析模块：对时间配准处理模块输出的配准结果进行处理分析，并将结果反馈至先验知识与配准要求模块进行反馈调整。

5.根据权利要求4所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤2中多传感器的时间配准，具体如下：

(1)最小二乘配准法是利用最小二乘法通过传感器的量测值估计出配准时刻的值，从采样周期不同的传感器中任意选取两个传感器A、B，采样周期为T_a,T_b，且T_a/T_b＝N，其中N为整数，传感器A的采集频率大于传感器B；

目标状态相邻两次的更新时刻为(k-1)T_b和kT_b，由于传感器B的采样周期是传感器A的采样周期的整数倍N，则kT_b＝[(k-1)T_b+NT_a]，即传感器B在进行一次量测的时间内传感器A已经进行了N次量测更新；根据最小二乘法配准原理，传感器B在进行一次采样的时间内，把传感器A在相同时间内的N次量测数据进行融合，估计出与传感器B采样时刻同步的虚拟量测数据；

传感器A在传感器B的一次量测时间内即(k-1)T_b到kT_b区间内共进行了N次量测，用Z_N＝[z₁,z₂,…,z_N]^T来表示，传感器A的第N个量测值z_N和传感器B的量测值的采样时刻一致，因此传感器A的N次量测值进行融合后的结果z及对应导数z'用U＝[z,z']^T来表示；

传感器A的任一量测值z_i通用表示为：

z_i＝z+(i-N)T_a·z'+v_ii＝1,2,...,N

用v_i表示量测噪声，将上式转换为向量形式：

Z_N＝W_NU+V_N

式中，W_N为对应量测系数矩阵，V_N＝[v₁,v₂,...,v_N]^T，量测噪声的均值为零，方差为其中σ²为量测噪声的方差；

由最小二乘规则得线性最小方差估计的性能指标函数J为：

要使J最小，通过对U求偏导数并使结果等于零得：

得到：

方差阵估值R_U为：

将N个测量值进行融合，得到传感器A在k时刻的虚拟量测值z(k)以及量测噪声方差Var[z(k)]分别为：

其中：c₁＝-2/N,c₂＝6/[N(N+1)]

(2)采用基于扩展卡尔曼滤波的估计方法，来估计不同传感器的开机时刻差；

扩展卡尔曼滤波建立在线性卡尔曼滤波的基础上，对非线性系统用泰勒级数展开，忽略高阶项，得到近似线性化模型，然后进行滤波；

设非线性模型为：

式中：X_k是n维状态向量；f(X_k-1)是n维非线性向量函数；Z_k是m维量测向量；h(X_k-1)是m维非线性向量函数；V_k为量测噪声；

对应时间更新为：

其中：Φ_k/k-1，P_k-1，Γ_k-1和Q_k-1是已知的系统结构参数，分别称为n阶状态一步转移矩阵，n×n阶的状态估计均方误差阵，n×l阶的系统噪声分配矩阵和l×l阶的系统噪声矩阵；

量测更新为：

设定传感器B的开机时刻滞后于传感器A的开机时刻，且滞后的时间差为Δt，传感器A的量测数据为r_A(t_k)，其中t_k为传感器A的采样时刻，则有传感器B的量测数据为r_B(t_k+Δt)，k＝1…n；其中t_k+Δt为传感器B的采样时刻，若消除了开机时刻的时间差，则传感器B的采样时刻和传感器A的采样时刻同步，则选取载体的状态量为：

对应的状态方程为：

X_k＝φ_k/k-1X_k+Γ_k/k-1μ_k

系统状态转移矩阵φ、系统噪声矩阵Γ和系统噪声方差阵Q分别为：

Γ＝[T³/6T²/2T 0]

Q＝σ²

考虑系统的量测噪声，选取传感器B在采样时刻t_k+Δt下的量测数据为观测量，则有：

其中，v₁为均值为零方差为的高斯白噪声，v₂为均值为零方差为/>的高斯白噪声，且v₁和v₂互不相关，则量测噪声方差阵R为：

由状态方程得：

则忽略高阶项后的观测方程为：

再通过非线性卡尔曼滤波的时间更新和量测更新，即获得传感器A和传感器B的开机时刻时间差，以及传感器B在消除开机时间差的量测值的估计值。

6.根据权利要求5所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤2中多传感器的空间配准，具体如下：

为了进行坐标转换，采用不同系下直角坐标系统一，数学表达式为：

其中，ΔX，ΔY，ΔZ为两坐标系3轴相对平移距离，K为尺度因子；ε_X、ε_Y、ε_Z为欧拉角；X_old,Y_old,Z_old为转换前的三轴坐标；X_new,Y_new,Z_new为转换后的三轴坐标；

设定以车体中线中心作为坐标轴统一的融合中心，确定以东北天地理坐标系为融合中心的坐标系；通过测量，标定出各传感器自身坐标系距融合中心坐标系在3轴上的相对平移距离及3轴相对欧拉角；

针对激光雷达、相机传感器，由于坐标系定义在Map系中，坐标系原点以上电初始化所处位置为坐标原点建立坐标系，因此为了确定将这些传感器的坐标系与融合中心坐标系统一，首先需利用初始GNSS测量的导航信息量，即地理系下的3维姿态角构造姿态矩阵变换，将Map系下的初始相对位置坐标信息X_map,Y_map,Z_map转换得到当前时刻的雷达直角坐标系下，表达式如下：

将经过旋转矩阵变换后的X_{map_rot}、Y_{map_rot}、Z_{map_rot}通过上述式子对齐到融合中心坐标系下。

7.根据权利要求6所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤3所述的建立多源融合子系统框架，具体如下：

将惯导作为公共参考系统，与其余传感器构成子系统，包括惯性/激光雷达、惯性/视觉、惯性/里程计、惯性/高度计、惯性/航姿系统共计5个子系统；

选取东北天导航坐标系作为导航解算坐标系，采用间接法进行状态估计，选用惯性导航系统的导航输出参数误差、惯性仪表误差作为系统状态变量，具体为：

三向惯性导航平台误差角δΦ，三向速度误差δV，三向位置误差δr，三向陀螺常值漂移ε_b，三向一阶马尔可夫漂移误差ε_r，三向加速度计零偏以此构建组合导航系统的状态变量X为：

根据系统状态向量X，列出组合导航系统的状态方程为：

式中F(t)表示系统的状态一步转移矩阵，G(t)表示系统噪声误差矩阵，W(t)表示系统白噪声随机误差矢量；

不同传感器所采集到的量测信息不同，所构建的量测方程也不一致，具体如下：

a：惯性/激光雷达子系统1

在惯性/激光雷达子系统1中，将惯导在导航系下输出的纬度L_sins与经度λ_sins和激光雷达处理转换后的纬度L_lidar与经度λ_lidar分别作差，作为观测量，则量测方程如下：

b：惯性/视觉子系统2

在惯性/视觉子系统2中，将惯导在导航系下输出的俯仰角θ_sins，横滚角γ_sins，航向角和位置信息与视觉处理后的俯仰角θ_visual，横滚角γ_visual，航向角/>与纬度L_visual，经度λ_visual分别作差，作为观测量，则量测方程如下：

c：惯性/里程计子系统3

在惯性/里程计子系统3中，先将里程计在载体系下获取的前向速度经姿态矩阵/>变换到导航系下，然后与惯导在导航系下输出的三轴速度信息v_sinse，v_sinsn，v_sinsu作差，作为观测量，则量测方程如下：

d：惯性/高度计子系统4

在惯性/高度计子系统4中，将惯导在导航系下输出的高度信息h_sins与气压高度计转换后的输出高度h_alt作差，作为观测量，则量测方程如下：

Z₄(t)＝[h_sins-h_alt]＝H₄(t)X(t)+V₄(t)

e：惯性/航姿系统子系统5

在惯性/航姿系统子系统5中，将惯导在导航系下输出的航向角信息与航姿系统输出的航向角信息/>作差，作为观测量，则量测方程如下：

。

8.根据权利要求7所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤4所述的设计自适应矢量分配因子，具体如下：

采用动态矢量分配算法，先从子系统入手，基于F范数，获得子系统层面的动态分配系数，然后提取子系统可观测矩阵的奇异值，深入到每个状态变量，使得每个状态变量具有各自的分配因子，具体如下：

步骤4.1、计算基于系统协方差矩阵F范数的分配系数A_i：

采用协方差矩阵的F范数来对子系统层面进行信息分配，公式如下：

步骤4.2、计算基于可观测性矩阵的分配系数B_i：

采用分段线性定常系统PWSC的奇异值分解法进行分析，分段线性定常系统模型如下：

式中：X(k)∈Rⁿ；F_j∈R^n×n；B_j∈P^n×s；U(k)∈R^s；W(k)∈R^l；Γ_j∈R^m×l；Z_j(k)∈R^m；H_j∈R^m×n，j＝1,2,…,q，表示系统分段间隔序号；

设定动态时间段内系统的可观测性矩阵为O，则表达式如下：

O＝[H^T(HF)^T…(HF^n-1)^T]^T

对可观测性矩阵O进行奇异值分解，如下所示：

O＝USV^T

式中：U＝[u₁ u₂ … u_nm]，V＝[v₁ v₂ ... v_n]均为正交矩阵，Λ_r×r＝diag(σ₁,σ₂,…,σ_r)，其中r，σ_i分别为矩阵O的秩和奇异值；

对5个子系统的可观测性矩阵分别进行奇异值分解，获得对应状态变量X_i的任一分量x_ij的分配系数，公式如下：

则每个子系统状态变量X_i对应的分配系数形式如下：

此时的信息分配系数B_i满足信息守恒原理：

步骤4.3、计算优化的信息分配系数C_i：

经过子系统层面的信息分配和针对状态量的信息分配，获得最终的优化信息分配系数C_i：

C_i＝β_i·B_i

将C_i代入信息反馈公式，得：

滤波过程中，误差协方差矩阵是对称阵；

由上式知，p_g12＝p_g21，c_i1≠c_i2，导致p_g12/c_i1≠p_g21/c_i2，即优化反馈后的P_i(1,2)≠P_i(2,1)；

令得：

此时子系统的误差协方差矩阵为对称阵，改进后的信息反馈过程为：

。

9.根据权利要求8所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤5所述的设计联邦架构多模噪声集，具体如下：

步骤5.1、构建多个并行且模型或系统参数各异的卡尔曼滤波器，滤波器之间独立，利用每个滤波器的先验信息及输出的残差信息，依据假设检验规则，得到每个滤波器所对应的模型与当前时刻系统真实模型的匹配概率，称为模型概率，最后通过加权融合算法进行最优估计；

建立包含n个常用滤波模型的模型集M，在无约束条件下，模型间的转换过程遵循一阶马尔可夫过程；记模型集M为M＝[m₁,m₂,…,m_n]，模型m_i到m_j的转移概率π_ij＝P{m_i/m_j}，由先验知识获得，且满足行和为1，对角线上的元素值大于其余元素值；

步骤5.2、输入交互：

对模型集中的n个模型进行状态交互混合以获得各模型滤波初值：

式中：和/>分别为模型m_j在k-1时刻的初始状态估计和初始状态估计协方差，j＝1,2,…n；/>和/>分别为模型m_i在k-1时刻的估计值，i＝1,2,…n且i≠j；/>为k-1时刻模型m_i到模型m_j的模型混合概率，表达式如下：

其中为模型m_i在k-1时刻的概率模型；

步骤5.3、模型滤波：

将输入交互中的初始状态估计和初始状态估计协方差作为滤波器的初值输入，对每个模型独立进行卡尔曼滤波，彼此之间相互独立，过程与标准卡尔曼滤波一致；

设对第j个模型进行滤波，则表达式如下：

步骤5.4、模型概率更新：

采用贝叶斯假设检验的方式，通过求取滤波器残差及其残差协方差矩阵，确定模型概率，完成更新；

定义滤波后的测量残差为对应协方差矩阵/>计算公式如下：

似然函数表达式为：

其中：为模型j在k时刻的似然函数值，m为量测向量的维数；

模型概率更新表达式为：

式中：为模型m_j在k时刻的概率模型；

步骤5.5、融合输出：

最终得到的状态估计表达式：

状态估计值对应的协方差阵P_k为：

针对融合惯性/激光雷达子系统1和惯性/视觉子系统2构建系统噪声和量测噪声模型集，每个模型集里共选择3个不同的模型；

系统噪声模型集：

惯性/激光雷达子系统量测噪声模型集：

R_lidar1＝diag((0.3/R_e)²(0.3/R_e)²)，R_lidar2＝3R_lidar1，R_lidar3＝6R_lidar1

惯性/视觉子系统量测噪声模型集：

R_visual1＝diag((0.0035)² (0.0035)² (0.0035)² (0.5/R_e)² (0.5/R_e)²)

R_visual2＝3R_visual1，R_visual3＝6R_visual1

初始模型概率矩阵：μ_k＝[1/3 1/3 1/3]

初始模型转移概率矩阵：。

10.根据权利要求9所述的基于时空配准和多模矢量分配融合的导航平台构建方法，其特征在于，步骤6所述的执行系统任务，具体如下：

步骤6.1、对系统进行上电，完成各硬件的初始化工作，随机开启任务调度中心进行任务调度；

步骤6.2、进行传感器的授时工作，各传感器通过与主机通信，采用NTP协议，由ROS的sensor_msgs/TimeReference消息类型发布，获取统一的主机时间信息，完成授时后该进程挂起；

步骤6.3、获取CPU，驱动各传感器进行数据采集，发送到主机进行时空配准，任务完成后释放CPU；

步骤6.4、将配准后的导航数据发送至解算和融合处理模块，先进行惯导解算，然后判断是否到达融合时刻，若未到达则进入步骤6.5，若已到达则进入步骤6.6；

步骤6.5、不进入融合中心进行信息融合，进入休眠期等待下一帧惯导解算和判断；

步骤6.6、进入融合模块，针对5个子系统，采用多进程模式来并行处理，将最终融合结果进行输出，随即释放CPU。