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

技术领域

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

背景技术

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

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

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

发明内容

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

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

步骤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

步骤1.7、自对准计算得到初始俯仰角θ

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

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

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

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

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

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

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

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

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

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

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

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

最小二乘配准法是利用最小二乘法通过传感器的量测值估计出配准时刻的值，解决频率配准的问题。现有采样周期不同的传感器，从中任一选取两个传感器A、B，令其采样周期为T

目标状态相邻两次的更新时刻为(k-1)T

传感器A在传感器B的一次量测时间内即(k-1)T

传感器A的任一量测值z

z

用v

Z

式中，W

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

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

可以得到：

方差阵估值为：

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

其中：c

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

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

设非线性模型为：

式中：X

对应时间更新为：

其中：Φ

量测更新为：

设定传感器B的开机时刻滞后于传感器A的开机时刻，且滞后的时间差为Δt，传感器A的量测数据为r

对应的状态方程为：

X

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

Γ＝[T

Q＝σ

考虑系统的量测噪声，选取传感器B在采样时刻t

其中，v

由状态方程可得：

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

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

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

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

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

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

其中，ΔX、ΔY、ΔZ为两坐标系各轴相对平移距离，K为尺度因子，ε

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

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

将经过旋转矩阵变换后的X

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

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

本系统选取东北天导航坐标系作为导航解算坐标系，采用间接法进行状态估计，选用惯性导航系统的导航输出参数误差、惯性仪表误差作为系统状态变量，具体包括为：三向惯性导航平台误差角δΦ，三向速度误差δV，三向位置误差δr，三向陀螺常值漂移ε

X＝[φ

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

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

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

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

在该子系统中，将惯导在导航系下输出的纬度L

b：惯性/视觉子系统2

在该子系统中，将惯导在导航系下输出的俯仰角θ

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

在该子系统中，先将里程计在载体系下获取的前向速度

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

在该子系统中，将惯导在导航系下输出的高度信息h

Z

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

在该子系统中，将惯导在导航系下输出的航向角信息

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

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

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

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

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

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

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

式中：X(k)∈R

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

O＝[H

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

O＝USV

式中：U＝[u

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

则每个子系统状态变量X

可验证，此时的信息分配系数B

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

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

C

将C

/>

为确保滤波器的一致收敛稳定性，要求滤波过程中，误差协方差矩阵是对称阵，很明显，由上式可知，尽管p

令

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

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

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

建立包含n个常用滤波模型的模型集M，在无约束条件下，模型间的转换过程遵循一阶马尔可夫过程，记模型集M为M＝[m

步骤5.2、输入交互：

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

式中：

其中

步骤5.3、模型滤波：

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

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

步骤5.4、模型概率更新：

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

似然函数表达式为：

其中：

模型概率更新表达式为：

式中：

步骤5.5、融合输出：

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

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

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

系统噪声模型集：

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

R

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

R

R

初始模型概率矩阵：μ

初始模型转移概率矩阵：

对应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种传感器进行多源信息融合，在卫星拒止条件下能够保持系统定位精度，提高系统的适应性；采用最小二乘法结合扩展卡尔曼滤波估计和多传感器坐标变换方法有效解决实际车载系统的时空配准问题，解决了平台搭建中出现的固有时空配准问题，提高输入融合信息的准确性；改进传统的联邦分配因子，针对分配因子由传统标量的平均分配转变为针对各子系统的每一维变量的矢量分配，联合系统噪声阵和量测噪声阵，构建噪声模型集，提高了传感器在实际融合中的性能。