CN114820768B - 一种大地坐标系与slam坐标系对齐方法 - Google Patents

Abstract

本发明公开了一种大地坐标系与slam坐标系对齐方法，该方法使用粒子滤波器在历史遥感地图上随机生成N个粒子模拟无人机在历史遥感地图的位置，获取无人机高度和相机数据，缩放相机数据，为每个粒子生成历史遥感地图的切片，进行SURF匹配，计算欧式距离作为每个粒子的置信度，对粒子滤波器进行更新，将置信度最高的粒子视为无人机真实位置，由此获得无人机在历史遥感地图上的GPS位置，将多组GPS位置与无人机在SLAM坐标系的位置进行转换，计算转换矩阵、尺度信息，从而得到无人机GPS坐标与SLAM坐标转换关系。本发明能够为无人机提供辅助GPS信息，减少无人机失事的概率，减少视觉SLAM的累计误差。

Description

一种大地坐标系与slam坐标系对齐方法

技术领域

本发明涉及人工智能和无人机技术领域，尤其涉及一种利用历史遥感信息的大地坐标系与SLAM坐标系的对齐方法。

背景技术

随着无人机技术的发展，无人机过分依赖GPS的问题愈发严重，GPS信号丢失，是无人机失事的主要原因。使用多传感器进行信息融合，提高建图精度，减少无人机定位误差，提高无人机定位信号的稳定性是无人机导航的重要内容之一。基于RGB的SLAM坐标系是笛卡尔坐标系，且没有尺度信息，需要将SLAM坐标系与大地坐标系进行对齐，计算转换矩阵，对SLAM坐标进行不断的修正。目前，现有技术中尚没有这样的方案。

发明内容

为了解决大地坐标系与slam坐标系对齐的问题，本发明提出一种大地坐标系与slam坐标系对齐方法，其能够为无人机提供辅助GPS信息，减少无人机失事的概率，可使GPS信息的准确率随着无人机移动越来越精准，同时不断减少视觉SLAM的累计误差。

为了实现上述目的，本发明采用的技术方案为：

一种大地坐标系与SLAM坐标系对齐方法，其包括如下步骤：

S1，无人机起飞到指定高度，SLAM进行初始化，粒子滤波器获取无人机位置和无人机机头方向；

S2，根据无人机位置和无人机机头方向，以无人机位置作为粒子滤波器的中心点位置，以高斯分布的方式随机生成N个具有相同置信度、不同方向的粒子Pⁿ，每个粒子的位置为历史遥感图的GPS值Gⁿ，n＝1,2,...N，完成粒子滤波器的初始化；

S3，通过无人机搭载的相机获取地面图片，根据历史遥感地图获得粒子滤波器中每个粒子的地图切片；将各地图切片与地面图片进行匹配，计算匹配的欧式距离Dⁿ；

S4，更新粒子滤波器，将更新后置信度最高的粒子P视为无人机在历史遥感地图的位置，作为真实无人机的GPS位置G^P；

S5，在无人机运动过程中，实时计算无人机在SLAM坐标系中的位置G^S，并不断采集G^S和G^P，通过坐标转换求得G^S到G^P的转换矩阵和尺度信息，完成大地坐标系与SLAM坐标系的对齐。

进一步的，步骤S1中，首先确认是否有GPS信号，如果有GPS信号，则粒子滤波器直接从当前GPS信息中获取无人机位置和无人机机头方向，否则，手动输入无人机位置和无人机机头方向。

进一步的，步骤S3的具体方式为：

无人机搭载的相机以垂直向下视角获取地面图片A[w,h]，其中，A表示图片区域，w表示图片宽度，h表示图片高度；

利用当前无人机高度，根据相机参数，计算相机视野实际大小，得到w,h的值；

根据历史遥感信息的像素与尺度的比例，将A[w,h]缩放到历史遥感地图大小，得到缩放图片A`[w`,h`]；

对于每个粒子，以粒子位置为中心点，在历史遥感地图中获取与A`[w`,h`]相同大小的地图切片PA`[w`,h`]；

使用SURF加速稳健特征算法，将A`[w`,h`]逐个与PA`[w`,h`]匹配，计算匹配的欧式距离Dⁿ。

进一步的，步骤S4中，更新粒子滤波器的具体方式为：

删除置信度最低的20％的粒子，将剩余粒子的置信度D^m进行归一化，得到剩余粒子的新的置信度B^m,m＝4/5*n；

无人机移动后，通过无人机传感器获取上一帧相机数据至当前帧的位移距离及方向，根据位移距离和方向，同时移动所有粒子的位置；

将B^m进行累加，构成一个含有m个段的0-1的置信度条，在置信度条上使用0-1的随机生成法，生成1/5*n的新粒子，并在每个粒子的位置和方向上随机加上高斯噪音，重新构成含n个粒子的粒子滤波器。

进一步的，步骤S5的具体方式为：

通过SLAM算法实时计算无人机在SLAM坐标系中的位置G^S；

将G^S和G^P进行时间戳同步，在阈值时间内将G^S和G^P视为同一时刻计算出的位置；

在无人机运动中，不断采集G^S和G^P；

在每一时间段内，根据本时间段采集的多组G^S和G^P，进行坐标转换，求得G^S到G^P的转换矩阵和尺度信息，完成大地坐标系与SLAM坐标系的对齐。

本发明的有益效果在于：

(1)本发明利用历史遥感信息与无人机拍摄的图片进行GPS定位，可在无GPS的环境或GPS飞行中丢失的情况下，利用历史遥感信息的GPS为无人机提供辅助GPS信息，从而减少无人机失事的概率。

(2)本发明利用粒子滤波器，根据无人机位置的移动，不断更新粒子滤波器，从而实现历史遥感信息的大地坐标系与slam坐标系对其的转换矩阵以及平移矩阵的不断更新，使得GPS信息的准确率随着无人机移动越来越精准，同时不断减少视觉SLAM的累计误差。

附图说明

图1为本发明实施例方法的总体流程图。

图2为系统粒子滤波器和SLAM定位更新阶段的流程图。

具体实施方式

下面结合附图和具体实施方式对本发明做进一步的说明。

一种大地坐标系与SLAM坐标系对齐方法，该方法可基于Ubuntu操作系统，以及ROS操作系统(Robot Operating System,机器人操作系统)实现，进行任务调度以及界面呈现。如图1所示，该方法主要实现方式如下：

使用粒子滤波器生成N个粒子，模拟飞机在地图中的位置；

将真实无人机位置的垂直视角地图切片A[w,h]进行与历史遥感地图等比例缩放(像素与尺度的比例)获得A`[w`,h`]；

每个粒子获取当前位置的垂直视角的地图切片PⁿA`[w`,h`]；

将A`[w`,h`]与每个粒子的P<sup>n</sup>A`[w`,h`]，进行SURF(Speeded Up RobustFeatures,加速稳健特征)算法匹配，计算每个粒子的地图切片与真实相机场景的SURF欧式距离，将欧式距离归一化后作为每个粒子的置信度；

如图2所示，在更新阶段，对于粒子滤波器做以下处理：利用每个粒子的置信度，删除部分置信度过低的粒子后，利用车轮法重新生成部分粒子，并在位置和方向添加随机高斯噪音，重新构成含N个粒子的粒子滤波器，更新粒子滤波器；

在每个粒子置信度低于阈值时，利用高斯分布，重新生成所有粒子，解决无人机定位失败的重启动定位问题；

利用飞机传感器获得距离及方向等里程计信息，更新粒子的位置；

重复以上步骤保持粒子滤波器更新。

SLAM(simultaneous localization and mapping)与上述步骤并行执行，在无人机运动中，使用原始相机数据，进行建图及计算无人机路径，获得每个时刻的无人机在SLAM坐标系中的位置；

在粒子滤波器获得GPS坐标后，通过GPS与SLAM坐标系进行矩阵转换，求转换矩阵及尺度信息；

将转换矩阵与尺度信息与SLAM坐标进行转换，可将SLAM坐标系转换为大地坐标系。

下面为一个更具体例子：

飞机准备工作，确认是否有GPS信号，如果有GPS信号，采用方法一进行初始化和修正，如果没有GPS信号，采用方法二进行初始化和修正。

方法一，无人机起飞到指定高度，SLAM准备初始化，SLAM初始化完成后，粒子滤波器准备初始化，粒子滤波器中心值采用当前GPS值，方向为飞机机头方向。

方法二，无人机起飞到指定高度，SLAM准备初始化，SLAM初始化完成后，粒子滤波器准备初始化，粒子滤波器中心值采用手动输入的方式初始化GPS信息包括无人机机头方向，无人机位置。

粒子滤波器(particle filter)初始化阶段，使用上阶段所获得的GPS信息以及飞机机头方向在中心点位置以高斯分布的方式随机生成N个具有相同置信度、不同方向的粒子Pⁿ，每个粒子的位置为历史遥感图的GPS值Gⁿ。

粒子滤波器(particle filter)更新阶段：

获取无人机当前位置，相机垂直向下视角的图片切片。具体方式为：利用当前无人机高度，根据相机参数，获取相机视野实际大小A[w,h],A表示当前区域，w表示图像宽度，h表示图像高度。根据历史遥感信息的像素与尺度的比例，缩放A[w,h]到历史遥感地图大小A`[w`,h`]。

获取各个粒子Pⁿ的图像切片，对于每个粒子，以粒子位置为中心点，在历史遥感地图中获取等比例与A`[w`,h`]大小的地图切片PA`[w`,h`]，

图像匹配，将A`[w`,h`]逐个与PA`[w`,h`]使用SURF(Speeded Up RobustFeatures,加速稳健特征)算法匹配，计算匹配的欧式距离Dⁿ。

粒子滤波器置信度更新阶段，删除置信度最低的20％的粒子，将D^m进行归一化后作为每个粒子的置信度B^m,m＝4/5*n。

飞机移动后，粒子滤波器里程计更新阶段：通过飞机传感器获取上一帧相机数据至当前帧的位移距离及方向，根据位移距离和方向，同时移动所有粒子的位置。

更新粒子滤波器：将B^m,进行累加，构成一个含有m个段的0-1的置信度条，称之为车轮，在车轮上使用0-1的随机生成法，生成1/5*n的新粒子，并在每个粒子的位置和方向上随机加上高斯噪音，重新构成含n个粒子的粒子滤波器。置信度最高的粒子P视为无人机在历史遥感地图的位置，作为真实无人机GPS位置G^P。

重复粒子滤波器的更新阶段。

为解决无人机绑架问题，如果Dⁿ均低于阈值，则定位失败，在最后一次欧氏距离大于阈值的点周围利用高斯分布，重新生成N个粒子，重复粒子滤波器的更新阶段。

SLAM算法与粒子滤波器的更新阶段并行执行，在无人机运动过程中，SLAM算法实时计算无人机在SLAM坐标系中的位置，由于SLAM坐标系的初始点为SLAM算法通过两帧相机数据的位移量及关键点匹配度所决定的，初始化点与真实坐标系存在方向与尺度的差距。由于SLAM所计算出的无人机在SLAM坐标系中的位置G^S，与GPS信息G^P，存在时间差，故将G^S和G^P进行时间戳同步，在一定时间内的G^S和G^P视为同一时刻计算出的位置。

采集多组G^S和G^P后进行坐标转换，求得G^S到G^P的转换矩阵、尺度信息，由于SLAM算法存在累计误差，在无人机运动中，不断采取多组数据，作为不同时段的转换矩阵、尺度信息，以提高转换精度。

Claims (4)

1.一种大地坐标系与SLAM坐标系对齐方法，其特征在于，包括如下步骤：

S1，无人机起飞到指定高度，SLAM进行初始化，粒子滤波器获取无人机位置和无人机机头方向；

S2，根据无人机位置和无人机机头方向，以无人机位置作为粒子滤波器的中心点位置，以高斯分布的方式随机生成N个具有相同置信度、不同方向的粒子Pⁿ，每个粒子的位置为历史遥感图的GPS值Gⁿ，n＝1,2,...N，完成粒子滤波器的初始化；

S3，通过无人机搭载的相机获取地面图片，根据历史遥感地图获得粒子滤波器中每个粒子的地图切片；将各地图切片与地面图片进行匹配，计算匹配的欧式距离Dⁿ；

S4，更新粒子滤波器，将更新后置信度最高的粒子P视为无人机在历史遥感地图的位置，作为真实无人机的GPS位置G^P；更新粒子滤波器的具体方式为：

删除置信度最低的20％的粒子，将剩余粒子的置信度D^m进行归一化，得到剩余粒子的新的置信度B^m,m＝4/5*n；

无人机移动后，通过无人机传感器获取上一帧相机数据至当前帧的位移距离及方向，根据位移距离和方向，同时移动所有粒子的位置；

将B^m进行累加，构成一个含有m个段的0-1的置信度条，在置信度条上使用0-1的随机生成法，生成1/5*n的新粒子，并在每个粒子的位置和方向上随机加上高斯噪音，重新构成含n个粒子的粒子滤波器；

S5，在无人机运动过程中，实时计算无人机在SLAM坐标系中的位置G^S，并不断采集G^S和G^P，通过坐标转换求得G^S到G^P的转换矩阵和尺度信息，完成大地坐标系与SLAM坐标系的对齐。

2.根据权利要求1所述的一种大地坐标系与SLAM坐标系对齐方法，其特征在于，步骤S1中，首先确认是否有GPS信号，如果有GPS信号，则粒子滤波器直接从当前GPS信息中获取无人机位置和无人机机头方向，否则，手动输入无人机位置和无人机机头方向。

3.根据权利要求1所述的一种大地坐标系与SLAM坐标系对齐方法，其特征在于，步骤S3的具体方式为：

无人机搭载的相机以垂直向下视角获取地面图片A[w,h]，其中，A表示图片区域，w表示图片宽度，h表示图片高度；

利用当前无人机高度，根据相机参数，计算相机视野实际大小，得到w,h的值；

根据历史遥感信息的像素与尺度的比例，将A[w,h]缩放到历史遥感地图大小，得到缩放图片A`[w`,h`]；

对于每个粒子，以粒子位置为中心点，在历史遥感地图中获取与A`[w`,h`]相同大小的地图切片PA`[w`,h`]；

使用SURF加速稳健特征算法，将A`[w`,h`]逐个与PA`[w`,h`]匹配，计算匹配的欧式距离Dⁿ。

4.根据权利要求1所述的一种大地坐标系与SLAM坐标系对齐方法，其特征在于，步骤S5的具体方式为：

通过SLAM算法实时计算无人机在SLAM坐标系中的位置G^S；

将G^S和G^P进行时间戳同步，在阈值时间内将G^S和G^P视为同一时刻计算出的位置；

在无人机运动中，不断采集G^S和G^P；

在每一时间段内，根据本时间段采集的多组G^S和G^P，进行坐标转换，求得G^S到G^P的转换矩阵和尺度信息，完成大地坐标系与SLAM坐标系的对齐。

Images