CN111089580B - 一种基于协方差交叉的无人战车同时定位与地图构建方法 - Google Patents

Abstract

本发明属于无人战车导航技术，具体为一种基于协方差交叉的无人战车同时定位与地图构建方法。首先建立系统状态估计模型，之后计算环境特征坐标，计算量测矩阵，采用卡尔曼滤波进行系统状态误差估计，根据卡尔曼滤波最优估计结果，对惯导系统误差与特征点误差进行反馈校正，完成校正系统误差后在进行环境特征坐标计算和系统状态误差估计。本方法能够改善惯性系统精度，提高导航精度以及环境模型建模精度。在能获得无人战车上多种传感器信息的情况下，利用多种传感器对环境特征的观测信息，与惯性导航系统进行组合，得到无人战车在各时刻的最优位置估计以及环境特征表示的环境地图模型，实现对无人战车的高精度定位以及周边环境的地图建模功能。

Description

一种基于协方差交叉的无人战车同时定位与地图构建方法

技术领域

本发明属于无人战车导航技术，具体涉及一种基于协方差交叉的无人战车同时定位与地图构建方法。

背景技术

随着地面车辆长时间、远程行驶能力不断得到增强，对导航系统的精度和自主性要求也在不断提高。纯惯性导航系统由于其固有的导航误差随时间而积累的缺点，因而不能完全满足实际应用的需要。构建环境地图模型，其目的是为了得到无人战车在各时刻的最优位置估计，解决长航时无人战车的高精度导航以及对环境态势感知的需求。

地图模型的表示方法有栅格尺度地图、几何尺度地图和拓扑地图，由于栅格尺度地图对于统一描述多平台信息在可行性、可靠性方面具有较大的优势。

栅格尺度地图将整个环境分割为均匀的单元栅格，对于2维的单元栅格，每个栅格被赋予一个0或1的值来表示当前栅格的状态，对于无人车而言，0表示可通行区域，1表示通行截止区域；对于3维的栅格地图，每个栅格的值代表障碍物的高度信息，低于某一阈值的障碍物视为可通行区域。

发明内容

本发明的目的是提供一种基于协方差交叉的无人战车同时定位与地图构建方法。

本发明的技术方案如下：

一种基于协方差交叉的无人战车同时定位与地图构建方法，包括如下步骤：

1)建立系统状态估计模型

式中，X(t)为上述系统状态向量；W(t)为系统白噪声；

状态变量

式中，

为惯导系统在全局坐标系中的位置误差与速度误差，

为已包含的第i个环境特征在全局坐标系中的位置误差与速度误差；

式中，δx_i,δy_i为第i个环境特征在全局坐标系中的位置误差，

为第i个环境特征在全局坐标系中在x轴和y轴方向上的速度误差；

2)计算环境特征坐标

将环境特征在载体坐标系中的坐标转换成全局坐标系中的坐标；

式中，

为3×3维的坐标变换矩阵，即载体坐标系到全局坐标系的转换矩阵， P_i ^m为环境特征在全局坐标系中的坐标；P_i ^b为第i个环境特征在载体坐标系中的位置坐标；

3)计算量测矩阵

确定卡尔曼滤波器的基本量测方程

Z＝HX+V

量测值Z为利用先验信息获得的第i个环境特征点位置

与当前获得的第i 个环境特征点位置P_i ^m之差；

并且

再结合基本量测方程确定量测矩阵H；

4)采用卡尔曼滤波进行系统状态误差估计；

5)系统误差校正

根据卡尔曼滤波最优估计结果，对惯导系统误差与特征点误差进行反馈校正。

6)完成校正系统误差后，继续进行步骤(2)到步骤(5)。

其特征在于：对

进行求导

再结合系统状态模型可得系数矩阵F(t)。

所述步骤2)中：

转换矩阵

的计算公式如下

式中，

为2×2维的载体坐标系到全局坐标系的姿态转换矩阵；

T_x、T_y为载体坐标系到全局坐标系在x和y轴方向上的位置偏移，为载体相对全局坐标系原点的距离。

所述的步骤3)具体为：

计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：

式中：

T_n为导航周期，NT_n为卡尔曼滤波周期，

为在一个卡尔曼滤波周期中，第 i个导航周期的系统矩阵。

状态一步预测

状态估计

滤波增益矩阵

K₁＝wP_k(P_k-1)^-1

一步预测误差方差阵

估计误差方差阵

其中，

为一步状态预测值，

为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，H_k为量测矩阵，Z_k为量测量，K₁和K₂为滤波增益矩阵，R_k为观测噪声阵， P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，Q_k-1为系统噪声阵，w为系数。

0≤w≤1，通过设置w使得融合后的协方差矩阵的迹最小。

所述的步骤2)中还包括获得当前时刻的环境特征，对环境特征进行数据关联：

首先，需要将当前时刻来自不同信息平台得到的环境特征点进行数据关联，融合重复的环境特征点，取多平台得到的环境特征点的交集合作为当前时刻的环境特征点库；

接着，将当前时刻的环境特征点库与已使用的环境特征点库去进行匹配，匹配成功的当前时刻特征点将作为对应特征点的观测值去进行量测更新；而未匹配成功的当前时刻特征点将作为新的特征点扩充到已使用的环境特征点库中，完成数据关联的工作。

数据关联中包括对环境特征点的动态或静态情况进行判别；

在全局坐标系中位置不会随时间发生变化的为静态；

在全局坐标系中位置会随时间发生变化的为动态。

本发明的显著效果如下：

本方法尤其适用于解决长航时无人战车的高精度导航以及对环境态势感知的需求。无人战车在未知的环境中从某一个未知开始，通过无人战车上的多种传感器对环境特征的观测信息，同时对无人战车的位置和各类环境特征的位置进行滤波估计。随着无人战车的行进，得到各环境特征的最优位置估计，能够构建出一个完整的由环境特征表示的环境地图模型，并得到无人战车在各时刻的最优位置估计。

采用基于协方差交叉的卡尔曼滤波技术，主要是使用无人战车上的多种传感器信息，用以改善惯性系统的精度，通过软件技术来提高导航精度以及环境模型建模精度。

在能获得无人战车上多种传感器信息的情况下，利用多种传感器对环境特征的观测信息，与惯性导航系统进行组合，得到无人战车在各时刻的最优位置估计以及环境特征表示的环境地图模型，实现对无人战车的高精度定位以及周边环境的地图建模功能。

基于协方差交叉方法的同时定位与地图构建技术采用栅格地图作为地图模型的表达方式，利用激光雷达系统、视觉成像系统的信息进行环境特征的建模与提取，利用信息融合技术将各环境特征进行数据关联，并基于协方差交叉方法的卡尔曼滤波技术进行地图模型库的更新与维护，实现无人战车的高精度定位。

具体实施方式

下面通过具体实施方式对本发明作进一步说明。

将栅格尺度地图作为地图模型的表示方法。使用全局坐标系作为环境建模的坐标系，以无人战车的起始位置为原点，地理北向为全局坐标系的x轴，地理东向为全局坐标系的y轴。

一、激光雷达系统特征建模与提取

首先对激光雷达系统获得的距离信息进行分隔：激光雷达系统在同一时间产生的数据量较多，为了从大量的数据中更加准确的提取环境特征，需要利用聚类算法对扫描点进行聚类，把同一时间产生的数据量分隔为不同的区块，在分隔过程中不能让某个区块过大。分隔后的每个区块包含一组扫描点，计算出扫描点在直角坐标系中的统计特性。包括均值、协方差矩阵、近似椭圆面积、扫描点数、统计点密度等。

激光雷达系统在测量周围障碍物的过程中，由于存在测量误差，获得的扫描点不能完全与实际障碍物的坐标保持一致，可认为是统计意义上的采样点，对于某一障碍物的二维协方差矩阵计算公式如下：

其中，μ_x和μ_y为该区块对应x和y坐标的均值，N为该区块扫描点个数。

由统计学原理可知，协方差矩阵的特征向量总是指向数据方差最大的方向，第一特征向量是数据方差最大的方向，第二特征向量是与第一特征向量垂直方向上数据方差最大的方向，而特征向量对应的特征值表征的是沿特征向量方向的数据方差的大小。因此，可以用协方差矩阵的特征值来衡量该区块扫描点以均值为中心的离散程度。

接着利用区块信息进行环境特征判别，获得某一区块的扫描点后，可用椭圆来模拟该区块的形状，而椭圆的长、短轴分别对应最大和最小的特征值，可计算出椭圆的面积，记为S。则该区块的统计密度为：

ρ＝S/N (2)

因此，可利用统计密度ρ可以表征障碍物的大小，需要根据工程经验以及典型环境的特征设定一个合理的阈值，当ρ大于该阈值，则认为该区块为某一环境特征，即通行截止区域。

二、视觉成像系统环境特征建模与提取技术

对于可见光图像，目标所处的环境特征通常会受多种因素影响，例如季节变化、拍摄角度、传感器状态、天气条件等即使同一地区不同时间拍摄得到的图像也可能具有不同的亮度和对比度。以港口目标为例，它是舰船执行运输任务和作战任务的依托。根据性质的不同，港口可以分为军港、商港、军商共用港和各种专用港。对港口目标进行判读时，通常主要依据突堤码头及停泊舰船来判断，另外，也可以根据储存设施、防御设施等设施进行分析。一般而言，港口目标主要关注的设施包括：码头，突堤，防波堤、油库，弹药库，鱼雷库，导弹库、导航站，信号台等等。在光学图像中，港口设施的特点则有为，固定码头一般都有明确的轮廓，并且与水面的色调有着较大的差别；防波堤呈现为整齐的白色窄带状，与海水形成鲜明的对比等等。

再以机场目标为例，军用机场的主要特点是，机场内停放的多为军用飞机，有排列整齐的营房，附近有油库和弹药库；民用机场的主要特点是，停机坪较大，附近有装卸物资的设备和专供旅客上下飞机的蹬机桥等，附近没有弹药库，另外，机库较大而且数量较多。军用机场的设施主要包括：跑道、滑行道、停机坪、飞机掩体(机窝)、营房等。这些组成部分在建筑形式和配置方面都有不同的特点，反映在遥感图像上的特征也有区别。此外,其它的若干类别目标,也均有其独特的属性。

可见光图像中典型机动目标纹理信息较弱，而形状信息丰富，因此主要选择形状特征来描述。对可见光而言，目标在不同尺度的特性差异尤为明显。要准确计算与提取目标特性，首先需要解决目标特征的尺度问题。

相对于传统的图像分析方法，面向对象图像分析的基本处理单元是图像对象，而不是单个像素。甚至分类也在图像对象上进行。面向对象方法的一个推动是许多图像分析任务的期望结果，是提取现实世界中具有恰当形状和恰当类别对象的事实。这类提取不能由普遍的基于像素的方法来完成。

面向对象方法的最大特征是，充分利用了图像对象之间的层次关系而存在循环影响。基于分割，图像对象的尺度和形状、特定的信息可用于图像分割。基于分类，特定处理过程可被激活。在许多应用中，期望的地理信息和感兴趣对象通过分类处理的迭代循环来逐步提取。因此，图像对象作为处理单元可连续地改变其形状、分类和相互联系，为目标检测和识别提供了更高级的处理。

首先，从传统的分类视角来看待面向对象：“面向对象的目标检测方法”本质上就是监督分类与非监督分类的有机结合，先用非监督分类对影像进行分割，将同类的像素聚类成一个个小多边形，即所谓的“影像对象”，然后对这些多边形采取或选择样本进行训练或类别定义建立分类规则或两者结合地进行监督分类。非监督分类创建分类对象，监督分类对分类对象实施具体分类。

其次，从分割的视角来看待面向对象：同时也可看作知识驱动方法(由顶向下)和数据驱动方法(由底向上)的结合。两类方法之间的基本差别是：由底向上通常导致局部结果，由于它们只是标记满足模型描述的像素或者区域，而有底向上方法执行全图像的分割，它们编组像素为满足某种同质性和异质性的空间聚类。分割就是知识驱动的方法，作用在各个局部，生成影像对象；分类是数据驱动方法，通过各类的对样本或隶属函数进行训练，获得相应的特征参数作为先验信息，对分割的结果进行分类或监测。

三、环境特征的数据关联

获得当前时刻的环境特征后，需要对环境特征进行数据关联，数据关联主要包括两个部分的内容：

首先，需要将当前时刻来自不同信息平台得到的环境特征点进行数据关联，融合重复的环境特征点，取多平台得到的环境特征点的交集合作为当前时刻的环境特征点库。

接着，将当前时刻的环境特征点库与已使用的环境特征点库去进行匹配，匹配成功的当前时刻特征点将作为对应特征点的观测值去进行量测更新，进一步提高对应特征点的位置精度，而未匹配成功的当前时刻特征点将作为新的特征点扩充到已使用的环境特征点库中，完成数据关联的工作。

在进行数据关联的过程中，需要注意的是对环境特征点的动态或静态情况进行判别，静态的环境特征点指的是在全局坐标系中位置不会随时间发生变化的环境特征点，而动态的环境特征点指的是在全局坐标系中位置会随时间发生变化的环境特征点，因此不能仅用环境特征点的位置信息作为数据关联的依据，还需要利用视觉等多光谱信息对环境特征点的表面特征进行匹配，保证数据关联的正确性。

四、基于协方差交叉方法的卡尔曼滤波

1)构建模型

系统状态变量包括无人车的位置、速度以及姿态和各环境特征的位置和速度状态，称作增广的系统状态变量。为便于描述同时定位与地图构建技术的基本模型，在仅保留位置信息的情况下，增广的系统状态变量

可表示为：

式中，δx₁,δy₁为无人车在全局坐标系中的位置误差，δx_i,δy_i(i＝2,n)为已包含的第i个环境特征在全局坐标系中的位置误差。

增广系统的协方差矩阵P_nn可表示为：

式中，对角线元素

表示无人车以及环境特征位置估计的不确定性，非对角线元素

表示无人车以及环境特征之间的相互依赖性或相关性。系统状态变量的初始值通常只包括无人车位置，初始的环境地图模型中还未发现环境特征。

为构建统一的环境地图模型，需要实时更新无人车与环境特征的相关性，且发现新的环境特征时，也需要将新环境特征的相关信息纳入到环境地图模型的构建中，系统状态变量和协方差矩阵同时需要进行更新。

式中，X_new为更新之后的增广系统状态变量，X_m为当前更新时刻新加入的状态变量，P_new为更新之后的增广系统协方差矩阵。

2)基于协方差交叉的量测更新

认为各时刻的状态量是是相关的，并且相关性难以度量，即环境特征在每个局部地图中的观测位置是具有某种未知的相关性。将环境特征在当前k时刻观测的位置和方差记作Z_k和R_k，环境特征在上一时刻的位置估计称为先验估计，记作X_k-1和P_k-1，经过数据融合后，环境特征在当前时刻的位置估计称为后验估计，记作X_k和P_k。

假设每次观测的位置是相关的，因此对同一环境特征的先验估计与当前观测的位置是相关的，即

但不能确定实际的相关性度量。运用协方差交叉算法对先验估计和当前观测值进行融合，增益权值K₁、K₂以及后验估计为：

式中，0≤w≤1，通过合理设置w使得融合后的协方差矩阵的迹最小。

每观测到一个局部地图，利用其当前的观测量与其在全局地图中的先验估计进行融合，计算该局部地图在全局地图中的后验估计。随着利用多平台信息对同一环境特征进行多次观测，获得的观测量越多，但由于信息之间具有互相关性，方差下降的趋势不是很明显，但经过协方差交叉融合后的对环境特征的位置估计是一致的。

下面给出本方法具体计算步骤。

步骤1、建立系统状态估计模型

为了进行卡尔曼滤波计算，需要先建立对应的系统状态模型，如下式

式中：X(t)为上述系统状态向量；W(t)为系统白噪声；

系统状态变量X(t)为：

式中，

为惯导系统在全局坐标系中的位置误差与速度误差，

为已包含的第i个环境特征在全局坐标系中的位置误差与速度误差，可表示为下式：

式中，δx_i,δy_i为第i个环境特征在全局坐标系中的位置误差，

为第i个环境特征在全局坐标系中在x轴和y轴方向上的速度误差。

对

进行求导：

再结合系统状态模型可得系数矩阵F(t)。

步骤2、计算环境特征坐标

建立状态模型后，需要利用通过外部传感器，如激光雷达、可见光相机等传感器去提取环境特征，获得第i个环境特征在载体坐标系(b系)中的位置坐标P_i ^b，然后利用载体坐标系到全局坐标系的转换矩阵

将环境特征在载体坐标系中的坐标转换成全局坐标系中的坐标，计算公式如下：

式中，

为3×3维的坐标变换矩阵，P_i ^m和P_i ^b为2×1维的位置坐标。

转换矩阵

的计算公式如下：

式中，

为2×2维的载体坐标系到全局坐标系的姿态转换矩阵，可由载体姿态角计算得到，属于现有技术不再赘述，T_x、T_y为载体坐标系到全局坐标系在x和y轴方向上的位置偏移，为载体相对全局坐标系原点的距离。

步骤3、计算量测矩阵和量测量

卡尔曼滤波器的基本量测方程形式如下：

Z＝HX+V

量测值Z为利用先验信息获得的第i个环境特征点位置

与当前获得的第i 个环境特征点位置P_i ^m之差。

因此可得到下式：

由于

与已观测到的第i个特征点位置相关，而P_i ^m与惯导位置相关，因此上式可推导得到下式：

再结合基本量测方程即可获得量测矩阵H。

步骤4、采用卡尔曼滤波进行系统状态估计

计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：

式中：

T_n为导航周期，NT_n为卡尔曼滤波周期，

为在一个卡尔曼滤波周期中，第 i个导航周期的系统矩阵。

状态一步预测

状态估计

滤波增益矩阵

K₁＝wP_k(P_k-1)^-1

一步预测误差方差阵

估计误差方差阵

其中，

为一步状态预测值，

为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，H_k为量测矩阵，Z_k为量测量，K₁和K₂为滤波增益矩阵，R_k为观测噪声阵， P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，Q_k-1为系统噪声阵，w的范围：0≤w≤1，通过设置w使得融合后的协方差矩阵的迹最小。

步骤5、系统误差校正

根据卡尔曼滤波最优估计结果，对惯导系统误差与特征点误差进行反馈校正，校正公式如下所示：

式中，

和

为第i个环境特征在全局坐标系中的位置坐标，

和

为校正后的第i个环境特征在全局坐标系中的位置坐标。

完成校正系统误差后，继续进行步骤(2)到步骤(5)，从而达到提高导航精度与地图建模精度的目的。

Claims (7)

1.一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于，包括如下步骤：

1)建立系统状态估计模型

式中，X(t)为上述系统状态向量；W(t)为系统白噪声；F(t)为系数矩阵；

状态变量

式中，

为惯导系统在全局坐标系中的位置误差与速度误差，

为已包含的第i个环境特征在全局坐标系中的位置误差与速度误差，其中i＝1,…,n；

式中，δx_i,δy_i为第i个环境特征在全局坐标系中的位置误差，

为第i个环境特征在全局坐标系中在x轴和y轴方向上的速度误差；

2)计算环境特征坐标

将环境特征在载体坐标系中的坐标转换成全局坐标系中的坐标；

式中，

为3×3维的坐标变换矩阵，即载体坐标系到全局坐标系的转换矩阵，P_i ^m为环境特征在全局坐标系中的坐标；P_i ^b为第i个环境特征在载体坐标系中的位置坐标；

3)计算量测矩阵

确定卡尔曼滤波器的基本量测方程

Z＝HX+V

量测值Z为利用先验信息获得的第i个环境特征点位置

与当前获得的第i个环境特征点位置P_i ^m之差；

并且

再结合基本量测方程确定量测矩阵H；

4)采用卡尔曼滤波进行系统状态误差估计；

5)系统误差校正

据卡尔曼滤波最优估计结果，对惯导系统误差与特征点误差进行反馈校正；

6)完成校正系统误差后，继续进行步骤2)到步骤5)。

2.如权利要求1所述的一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于：对

进行求导

再结合系统状态模型可得系数矩阵F(t)。

3.如权利要求1所述的一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于，所述步骤2)中：

转换矩阵

的计算公式如下

式中，

为2×2维的载体坐标系到全局坐标系的姿态转换矩阵；

T_x、T_y为载体坐标系到全局坐标系在x和y轴方向上的位置偏移，为载体相对全局坐标系原点的距离。

4.如权利要求1所述的一种基于协方差交叉的无人战车同时定位与地图构建方法，所述的步骤3)具体为：

计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：

式中：

T_n为导航周期，NT_n为卡尔曼滤波周期，

为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵；

状态一步预测

状态估计

滤波增益矩阵

K₁＝wP_k(P_k-1)^-1

一步预测误差方差阵

估计误差方差阵

其中，

为一步状态预测值，

为状态估计矩阵，Φ_k,k-1为状态一步转移矩阵，H_k为量测矩阵，Z_k为量测量，K₁和K₂为滤波增益矩阵，R_k为观测噪声阵，P_k,k-1为一步预测误差方差阵，P_k为估计误差方差阵，Γ_k,k-1为系统噪声驱动阵，Q_k-1为系统噪声阵，w为系数。

5.如权利要求4所述的一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于：0≤w≤1，通过设置w使得融合后的协方差矩阵的迹最小。

6.如权利要求1所述的一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于，所述的步骤2)中还包括获得当前时刻的环境特征，对环境特征进行数据关联：

首先，需要将当前时刻来自不同信息平台得到的环境特征点进行数据关联，融合重复的环境特征点，取多平台得到的环境特征点的交集合作为当前时刻的环境特征点库；

接着，将当前时刻的环境特征点库与已使用的环境特征点库去进行匹配，匹配成功的当前时刻特征点将作为对应特征点的观测值去进行量测更新；而未匹配成功的当前时刻特征点将作为新的特征点扩充到已使用的环境特征点库中，完成数据关联的工作。

7.如权利要求6所述的一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于：数据关联中包括对环境特征点的动态或静态情况进行判别；

在全局坐标系中位置不会随时间发生变化的为静态；

在全局坐标系中位置会随时间发生变化的为动态。