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

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

A method of simultaneous location and map building of UAV Based on covariance cross

【技术实现步骤摘要】
一种基于协方差交叉的无人战车同时定位与地图构建方法
本专利技术属于无人战车导航技术，具体涉及一种基于协方差交叉的无人战车同时定位与地图构建方法。
技术介绍
随着地面车辆长时间、远程行驶能力不断得到增强，对导航系统的精度和自主性要求也在不断提高。纯惯性导航系统由于其固有的导航误差随时间而积累的缺点，因而不能完全满足实际应用的需要。构建环境地图模型，其目的是为了得到无人战车在各时刻的最优位置估计，解决长航时无人战车的高精度导航以及对环境态势感知的需求。地图模型的表示方法有栅格尺度地图、几何尺度地图和拓扑地图，由于栅格尺度地图对于统一描述多平台信息在可行性、可靠性方面具有较大的优势。栅格尺度地图将整个环境分割为均匀的单元栅格，对于2维的单元栅格，每个栅格被赋予一个0或1的值来表示当前栅格的状态，对于无人车而言，0表示可通行区域，1表示通行截止区域；对于3维的栅格地图，每个栅格的值代表障碍物的高度信息，低于某一阈值的障碍物视为可通行区域。
技术实现思路
本专利技术的目的是提供一种基于协方差交叉的无人战车同时定位与地图构建方法。本专利技术的技术方案如下：一种基于协方差交叉的无人战车同时定位与地图构建方法，包括如下步骤：1)建立系统状态估计模型式中，X(t)为上述系统状态向量；W(t)为系统白噪声；状态变量式中，为惯导系统在全局坐标系中的位置误差与速度误差，为已包含的第i个环境特征在全局坐标系中的位置误差与速度误差；式中，δxi,δyi为第i个环境特征在全局坐标系中的位置误差，为第i个环境特征在全局坐标系中在x轴和y轴方向上的速度误差；2)计算环境特征坐标将环境特征在载体坐标系中的坐标转换成全局坐标系中的坐标；式中，为3×3维的坐标变换矩阵，即载体坐标系到全局坐标系的转换矩阵，Pim为环境特征在全局坐标系中的坐标；Pib为第i个环境特征在载体坐标系中的位置坐标；3)计算量测矩阵确定卡尔曼滤波器的基本量测方程Z＝HX+V量测值Z为利用先验信息获得的第i个环境特征点位置与当前获得的第i个环境特征点位置Pim之差；并且再结合基本量测方程确定量测矩阵H；4)采用卡尔曼滤波进行系统状态误差估计；5)系统误差校正根据卡尔曼滤波最优估计结果，对惯导系统误差与特征点误差进行反馈校正。6)完成校正系统误差后，继续进行步骤(2)到步骤(5)。其特征在于：对进行求导再结合系统状态模型可得系数矩阵F(t)。所述步骤2)中：转换矩阵的计算公式如下式中，为2×2维的载体坐标系到全局坐标系的姿态转换矩阵；Tx、Ty为载体坐标系到全局坐标系在x和y轴方向上的位置偏移，为载体相对全局坐标系原点的距离。所述的步骤3)具体为：计算卡尔曼滤波周期到来时的状态一步转移矩阵，其计算公式如下：式中：Tn为导航周期，NTn为卡尔曼滤波周期，为在一个卡尔曼滤波周期中，第i个导航周期的系统矩阵。状态一步预测状态估计滤波增益矩阵K1＝wPk(Pk-1)-1一步预测误差方差阵估计误差方差阵其中，为一步状态预测值，为状态估计矩阵，Φk,k-1为状态一步转移矩阵，Hk为量测矩阵，Zk为量测量，K1和K2为滤波增益矩阵，Rk为观测噪声阵，Pk,k-1为一步预测误差方差阵，Pk为估计误差方差阵，Γk,k-1为系统噪声驱动阵，Qk-1为系统噪声阵，w为系数。0≤w≤1，通过设置w使得融合后的协方差矩阵的迹最小。所述的步骤2)中还包括获得当前时刻的环境特征，对环境特征进行数据关联：首先，需要将当前时刻来自不同信息平台得到的环境特征点进行数据关联，融合重复的环境特征点，取多平台得到的环境特征点的交集合作为当前时刻的环境特征点库；接着，将当前时刻的环境特征点库与已使用的环境特征点库去进行匹配，匹配成功的当前时刻特征点将作为对应特征点的观测值去进行量测更新；而未匹配成功的当前时刻特征点将作为新的特征点扩充到已使用的环境特征点库中，完成数据关联的工作。数据关联中包括对环境特征点的动态或静态情况进行判别；在全局坐标系中位置不会随时间发生变化的为静态；在全局坐标系中位置会随时间发生变化的为动态。本专利技术的显著效果如下：本方法尤其适用于解决长航时无人战车的高精度导航以及对环境态势感知的需求。无人战车在未知的环境中从某一个未知开始，通过无人战车上的多种传感器对环境特征的观测信息，同时对无人战车的位置和各类环境特征的位置进行滤波估计。随着无人战车的行进，得到各环境特征的最优位置估计，能够构建出一个完整的由环境特征表示的环境地图模型，并得到无人战车在各时刻的最优位置估计。采用基于协方差交叉的卡尔曼滤波技术，主要是使用无人战车上的多种传感器信息，用以改善惯性系统的精度，通过软件技术来提高导航精度以及环境模型建模精度。在能获得无人战车上多种传感器信息的情况下，利用多种传感器对环境特征的观测信息，与惯性导航系统进行组合，得到无人战车在各时刻的最优位置估计以及环境特征表示的环境地图模型，实现对无人战车的高精度定位以及周边环境的地图建模功能。基于协方差交叉方法的同时定位与地图构建技术采用栅格地图作为地图模型的表达方式，利用激光雷达系统、视觉成像系统的信息进行环境特征的建模与提取，利用信息融合技术将各环境特征进行数据关联，并基于协方差交叉方法的卡尔曼滤波技术进行地图模型库的更新与维护，实现无人战车的高精度定位。具体实施方式下面通过具体实施方式对本专利技术作进一步说明。本项目将栅格尺度地图作为地图模型的表示方法。使用全局坐标系作为环境建模的坐标系，以无人战车的起始位置为原点，地理北向为全局坐标系的x轴，地理东向为全局坐标系的y轴。一、激光雷达系统特征建模与提取首先对激光雷达系统获得的距离信息进行分隔：激光雷达系统在同一时间产生的数据量较多，为了从大量的数据中更加准确的提取环境特征，需要利用聚类算法对扫描点进行聚类，把同一时间产生的数据量分隔为不同的区块，在分隔过程中不能让某个区块过大。分隔后的每个区块包含一组扫描点，计算出扫描点在直角坐标系中的统计特性。包括均值、协方差矩阵、近似椭圆面积、扫描点数、统计点密度等。激光雷达系统在测量周围障碍物的过程中，由于存在测量误差，获得的扫描点不能完全与实际障碍物的坐标保持一致，可认为是统计意义上的采样点，对于某一障碍物的二维协方差矩阵计算公式如下：其中，μx和μy为该区块对应x和y坐标的均值，N为该区块扫描点个数。由统计学原理可知，协方差矩阵的特征向量总是指向数据方差最大的方向，第一特征向量是数据方差最大的方向，第二特征向量本文档来自技高网...

【技术保护点】
1.一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于，包括如下步骤：/n1)建立系统状态估计模型/n

【技术特征摘要】
1.一种基于协方差交叉的无人战车同时定位与地图构建方法，其特征在于，包括如下步骤：
1)建立系统状态估计模型



式中，X(t)为上述系统状态向量；W(t)为系统白噪声；
状态变量
式中，为惯导系统在全局坐标系中的位置误差与速度误差，为已包含的第i个环境特征在全局坐标系中的位置误差与速度误差；



式中，δxi,δyi为第i个环境特征在全局坐标系中的位置误差，为第i个环境特征在全局坐标系中在x轴和y轴方向上的速度误差；
2)计算环境特征坐标
将环境特征在载体坐标系中的坐标转换成全局坐标系中的坐标；



式中，为3×3维的坐标变换矩阵，即载体坐标系到全局坐标系的转换矩阵，Pim为环境特征在全局坐标系中的坐标；Pib为第i个环境特征在载体坐标系中的位置坐标；
3)计算量测矩阵
确定卡尔曼滤波器的基本量测方程
Z＝HX+V
量测值Z为利用先验信息获得的第i个环境特征点位置与当前获得的第i个环境特征点位置Pim之差；
并且再结合基本量测方程确定量测矩阵H；
4)采用卡尔曼滤波进行系统状态误差估计；
5)系统误差校正
据卡尔曼滤波最优估计结果，对惯导系统误差与特征点误差进行反馈校正。
6)完成校正系统误差后，继续进行步骤(2)到步骤(5)。


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



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


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



式中，为2×2维的载体坐标系到全局坐标系的姿态转换矩阵；
Tx、Ty为载体坐标系到全局坐标系在x和y轴方向上的位置偏移，为载体相对全局坐标系原点的距离。


4.如权利要求1所述的一种基于...

【专利技术属性】
技术研发人员：孙伟，刘冲，李海军，原润，钟润伍，蔡善军，裴玉锋，
申请(专利权)人：北京自动化控制设备研究所，
类型：发明
国别省市：北京;11