CN113763252B - 一种用于无人机的大地坐标系与slam坐标系转换方法 - Google Patents

一种用于无人机的大地坐标系与slam坐标系转换方法 Download PDF

Info

Publication number
CN113763252B
CN113763252B CN202111089253.7A CN202111089253A CN113763252B CN 113763252 B CN113763252 B CN 113763252B CN 202111089253 A CN202111089253 A CN 202111089253A CN 113763252 B CN113763252 B CN 113763252B
Authority
CN
China
Prior art keywords
coordinate
slam
gps
data
coordinate system
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111089253.7A
Other languages
English (en)
Other versions
CN113763252A (zh
Inventor
张泽勇
耿虎军
高峰
关俊志
李晨阳
王雅涵
蔡迎哲
柴兴华
陈彦桥
彭会湘
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
CETC 54 Research Institute
Original Assignee
CETC 54 Research Institute
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by CETC 54 Research Institute filed Critical CETC 54 Research Institute
Priority to CN202111089253.7A priority Critical patent/CN113763252B/zh
Publication of CN113763252A publication Critical patent/CN113763252A/zh
Application granted granted Critical
Publication of CN113763252B publication Critical patent/CN113763252B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T3/00—Geometric image transformation in the plane of the image
    • G06T3/60—Rotation of a whole image or part thereof
    • G06T3/604—Rotation of a whole image or part thereof using a CORDIC [COordinate Rotation Digital Compute] device
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S19/00—Satellite radio beacon positioning systems; Determining position, velocity or attitude using signals transmitted by such systems
    • G01S19/38—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system
    • G01S19/39—Determining a navigation solution using signals transmitted by a satellite radio beacon positioning system the satellite radio beacon positioning system transmitting time-stamped messages, e.g. GPS [Global Positioning System], GLONASS [Global Orbiting Navigation Satellite System] or GALILEO
    • G01S19/42—Determining position
    • G01S19/421—Determining position by combining or switching between position solutions or signals derived from different satellite radio beacon positioning systems; by combining or switching between position solutions or signals derived from different modes of operation in a single system
    • G01S19/426—Determining position by combining or switching between position solutions or signals derived from different satellite radio beacon positioning systems; by combining or switching between position solutions or signals derived from different modes of operation in a single system by combining or switching between position solutions or signals derived from different modes of operation in a single system

Abstract

本发明公开了一种用于无人机的大地坐标系与SLAM坐标系转换方法，涉及无人机技术领域。该方法首先获取GPS导航信息以及摄像头图像，然后利用时间戳近似同步规则采集30组数据，计算转换矩阵和平移矩阵，并利用各组数据的欧式距离计算尺度信息；接着利用转换矩阵、平移矩阵和尺度信息将SLAM坐标映射到GPS的笛卡尔坐标，剔除差异过大的点，并重新计算转换矩阵、平移矩阵和尺度信息，实现转换精度的动态提高。

Description

一种用于无人机的大地坐标系与SLAM坐标系转换方法
技术领域
本发明涉及人工智能及无人机技术领域，尤其涉及一种用于无人机的大地坐标系与SLAM坐标系转换方法。
背景技术
随着人工智能技术以及无人机技术的发展，通过使用多种传感器，例如，GPS传感器、摄像头、激光雷达等，进行信息融合，从而提高建图的精准度以及减小定位的误差是无人机导航的重要内容之一。由于SLAM坐标系是笛卡尔坐标系，所以需要将SLAM坐标系转换为大地坐标系。
目前，现有技术普遍采用基于RGB的SLAM算法，初始化时需要足够的特征点，导致SLAM坐标系初始化时，无法确定SLAM坐标系的方向以及初始位置，从而导致无法通过固定转换矩阵，将大地坐标系与SLAM坐标系进行互相转换。
发明内容
有鉴于此，本发明提供一种用于无人机的大地坐标系与SLAM坐标系转换方法，该方法能够动态、精准地计算大地坐标系与SLAM坐标系的转换矩阵以及平移矩阵，从而动态地进行大地坐标系与SLAM坐标系的转换，且可通过不断计算新的转换矩阵以及平移矩阵，保证转换矩阵以及平移矩阵始终为最当前最优解。
为实现上述目的，本发明所采用的技术方案是：
一种用于无人机的大地坐标系与SLAM坐标系转换方法，包括如下步骤：
(1)无人机搭载的GPS导航装置定频发布带有时间戳的GPS坐标，同时，无人机搭载的摄像头发布带有时间戳的图片；
(2)将GPS坐标从大地坐标转换为笛卡尔坐标，得到GPS笛卡尔坐标；
(3)通过SLAM算法对图片进行处理，得到SLAM坐标，并将SLAM坐标的时间戳同步为摄像头获取图片的时间戳；
(4)根据时间戳近似同步的规则采集多组坐标数据以构成数据集，每组坐标数据包括一个GPS笛卡尔坐标以及与GPS笛卡尔坐标时间戳近似同步的SLAM坐标；数据集中的任三个GPS笛卡尔坐标均不共面；
(5)计算数据集中SLAM坐标与GPS笛卡尔坐标之间的转换矩阵和平移矩阵；
(6)计算数据集中各时间戳相邻的两GPS笛卡尔坐标之间的欧氏距离，并求平均，得到第一均值；计算数据集中各时间戳相邻的两SLAM坐标之间的欧氏距离，并求平均，得到第二均值；计算第一均值与第二均值的比值，得到SLAM坐标的尺度信息；
(7)针对数据集中的每组坐标数据，根据转换矩阵、平移矩阵以及尺度信息，反向计算SLAM坐标在笛卡尔坐标系中的位置，然后将反向计算的位置与SLAM坐标对应的GPS笛卡尔坐标做差，将差值超出精度阈值的坐标数据组从数据集中剔除；
(8)根据新的数据集重新计算SLAM坐标与GPS笛卡尔坐标之间的转换矩阵、平移矩阵以及SLAM坐标的尺度信息，实现SLAM坐标与GPS笛卡尔坐标之间转换精度的动态调整。
进一步的，所述摄像头为RGB摄像头；在无人机上升、平移的构图过程中，摄像头采集无人机移动轨迹上的用于SLAM算法的关键点。
进一步的，步骤(4)中，采集数据前，首先剔除数据中的奇异点，具体方式如下：
对于GPS笛卡尔坐标数据，剔除工作模式不正常的点，以及导航星数少于阈值的点；
对于SLAM坐标数据，剔除突变超过阈值的点。
进一步的，步骤(5)的具体方式如下：
通过将原点从相机中心移动到点云的质心来取消平移，同时通过质心归一化，使得SLAM坐标数据与GPS笛卡尔坐标数据有相同的质心，然后使用SVD分解，获取转换矩阵和平移矩阵。
进一步的，步骤(7)中的精度阈值为当前数据集中各组坐标数据依据步骤(7)所求得的差值的平均值。
进一步的，步骤(8)中所述的新的数据集，在步骤(7)所得数据集的基础上，根据精度需求补入了一组或多组新的坐标数据。
本发明的有益效果在于：
1、本发明能够动态、精准地计算大地坐标系与SLAM坐标系的转换矩阵以及平移矩阵，从而动态地进行大地坐标系与SLAM坐标系的转换。
2、本发明通过不断计算新的转换矩阵以及平移矩阵，可保证转换矩阵以及平移矩阵始终为最当前最优解。
3、本发明方法简单，易于实现，适用于无人机的硬件条件，具有较强的实用价值。
附图说明
图1是本发明实施例方法的原理示意图。
图2是本发明实施例方法的流程图。
具体实施方式
下面结合附图，对本发明实施例中的技术方案进行清楚、完整地描述，在下面的描述中为了充分理解此方法，阐述了更多具体的细节，所描述的方法仅为本发明的部分实例，而非全部实例。基于本发明的实例，在此领域中没有突破性进展的前提下，所有其他实例，都属于本发明的保护范围。
如图1和2所示，一种用于无人机的大地坐标系和SLAM坐标系的转换方法，方法步骤如下：
启动无人机，基于ROS操作系统的GPS导航模块开始工作，并发布定频的GPS数据，并剔除掉导航搜星较少，GPS导航非正常工作以及突变较大的坐标点。将GPS所得的经纬度，转换为笛卡尔坐标系，无人机起始点为笛卡尔坐标系的原点。利用ROS操作系统，重新发布GPS的笛卡尔坐标系，时间戳与GPS同步。
与此同时，同步启动摄像头，利用ROS发布摄像头节点，时间戳同步为摄像头接受时间-摄像头延迟时间，SLAM同步接受摄像头节点，并开始尝试初始化。
无人机起飞后，GPS持续工作，GPS的笛卡尔坐标节点持续发布坐标点，同时在无人机飞行中，SLAM不断检测特征点，尝试初始化，等待SLAM初始化成功。
SLAM初始化成功，开始处理结果，发布SLAM处理的坐标点，此时对于SLAM坐标系，初始化成功的点为SLAM坐标系的原点，且时间戳同步为摄像头的时间戳，避免由于SLAM计算时间出现与GPS时间戳不同步的问题。
由于GPS是定频发布的数据，而SLAM节点是根据特征点发布的数据，所以时间戳并不同步，且密度不一，故根据时间戳近似原理，采用时间戳近似缓冲值10ms，将10ms内的最近的GPS数据和SLAM数据视为同一时间，地点发布的数据，可通过调整时间戳近似缓冲值提高转换精度。
采集30组数据后，首先计算此30组数据是否共面，如果发生共面情况，或者近似共面，继续采集10组数据，重复计算是否共面。当不共面时，进一步计算转换矩阵和平移矩阵。
使用单目RGB的SLAM算法没有尺度信息，所以利用两组数据的首尾数据可获取GPS的笛卡尔坐标系的欧式距离，以及SLAM坐标系的欧氏距离，从而得到SLAM坐标系转换到GPS的笛卡尔坐标系的尺度信息。对于使用RGBD和双目的SLAM算法，所生成的SLAM坐标自带尺度信息。
通过所得的转换矩阵，平移矩阵以及尺度信息，反向求得SLAM坐标点在GPS的笛卡尔坐标系中的坐标点。本方法视GPS数据为可靠数据，SLAM数据为不可靠数据，故将反向求得的坐标与GPS的笛卡尔坐标对比，剔除与GPS的坐标点相差较大的SLAM坐标点，重新计算转换矩阵和平移矩阵。
根据需要，继续采集数据，重新计算转换矩阵和平移矩阵，重复以上步骤，在数据量不断变大的同时，可动态的精准的提高转换矩阵和平移矩阵的精度。
下面为一个更具体的例子：
本例基于Ubuntu系统以及ROS操作系统(Robot Operating System，机器人操作系统)，可以更好地管理各个传感器的信息，例如点云信息、GPS信息，以及传感器每次发布信息的时间戳信息。该方法的主要步骤如下：
1)在ROS操作系统框架下发布定频的GPS导航信息，并剔除奇异点。
2)利用GPS所得的大地坐标系，转换为笛卡尔坐标系。
3)在ROS操作系统框架下，启动无人机后，进行基于RGB摄像头的SLAM算法的初始化。进而，采集无人机飞行轨迹上的地图坐标点击，发布SLAM计算后所得的坐标信息，并剔除奇异点。
4)由于GPS传感器定频发布信息，SLAM需要计算特征点，根据特征信息发布计算信息，两者时间戳并不同步。故将GPS的笛卡尔坐标系的信息和SLAM坐标系的信息进行时间戳近似同步，将10ms(时间戳近似缓冲)内GPS发布的坐标系信息与SLAM发布的坐标系信息看作同一时间计算得到的信息。减少时间戳近似缓冲可提高本方法的精度，但同时增加了无人机运行时间。根据时间戳近似同步的数据，一共收集30组GPS数据和SLAM数据用于计算转换矩阵和平移矩阵。
5)利用方程r(k)(t)＝R(k)r(1)(t)+c(k)t＝1，2，3…30，可求得旋转矩阵R(k)和平移矩阵c(k)，r(k)(t)为GPS的笛卡尔坐标系的点集，r(1)(t)为SLAM坐标系的点集。由于点集r(k)(t)和r(1)(t)具有相同的质心，通过将原点从相机中心移动到点云的质心来取消平移。通过以下方程计算r(k)(t)和r(1)(t)的质心：
Figure BDA0003266666200000061
使
Figure BDA0003266666200000062
可得到
Figure BDA0003266666200000063
G(k)＝R(k)G(1)等效为
Figure BDA0003266666200000069
通过SVD分解可得：
Figure BDA0003266666200000064
则在r(k)(t)不共面时，
Figure BDA0003266666200000065
为唯一解。即：
a)
Figure BDA0003266666200000066
b)
Figure BDA0003266666200000067
计算完R(k)，之后可由
Figure BDA0003266666200000068
获取平移矩阵。
6)通过所获取的转换矩阵和平移矩阵可以将SLAM坐标系动态精准的映射到GPS的笛卡尔坐标系，使得SLAM坐标系具有GPS笛卡尔坐标系同样的起点，方向。
7)利用各组数据的欧式距离，可求得SLAM坐标系与GPS的尺度比例信息，利用转换矩阵，平移矩阵以及尺度信息，求得SLAM坐标在GPS的笛卡尔坐标系的位置，与GPS坐标进行对比，剔除差异较大的点，重新计算转换矩阵，平移矩阵以及尺度信息，由此实现动态地提高转换精度。

Claims (6)

1.一种用于无人机的大地坐标系与SLAM坐标系转换方法，其特征在于，包括如下步骤：
（1）无人机搭载的GPS导航装置定频发布带有时间戳的GPS坐标，同时，无人机搭载的摄像头发布带有时间戳的图片；
（2）将GPS坐标从大地坐标转换为笛卡尔坐标，得到GPS笛卡尔坐标；
（3）通过SLAM算法对图片进行处理，得到SLAM坐标，并将SLAM坐标的时间戳同步为摄像头获取图片的时间戳；
（4）根据时间戳近似同步的规则采集多组坐标数据以构成数据集，每组坐标数据包括一个GPS笛卡尔坐标以及与GPS笛卡尔坐标时间戳近似同步的SLAM坐标；数据集中的任三个GPS笛卡尔坐标均不共面；
（5）计算数据集中SLAM坐标与GPS笛卡尔坐标之间的转换矩阵和平移矩阵；
（6）计算数据集中各时间戳相邻的两GPS笛卡尔坐标之间的欧氏距离，并求平均，得到第一均值；计算数据集中各时间戳相邻的两SLAM坐标之间的欧氏距离，并求平均，得到第二均值；计算第一均值与第二均值的比值，得到SLAM坐标的尺度信息；
（7）针对数据集中的每组坐标数据，根据转换矩阵、平移矩阵以及尺度信息，反向计算SLAM坐标在笛卡尔坐标系中的位置，然后将反向计算的位置与SLAM坐标对应的GPS笛卡尔坐标做差，将差值超出精度阈值的坐标数据组从数据集中剔除；
（8）根据新的数据集重新计算SLAM坐标与GPS笛卡尔坐标之间的转换矩阵、平移矩阵以及SLAM坐标的尺度信息，实现SLAM坐标与GPS笛卡尔坐标之间转换精度的动态调整。
2.如权利要求1所述的一种用于无人机的大地坐标系与SLAM坐标系转换方法，其特征在于，所述摄像头为RGB摄像头；在无人机上升、平移的构图过程中，摄像头采集无人机移动轨迹上的用于SLAM算法的关键点。
3.如权利要求1所述的一种用于无人机的大地坐标系与SLAM坐标系转换方法，其特征在于，步骤（4）中，采集数据前，首先剔除数据中的奇异点，具体方式如下：
对于GPS笛卡尔坐标数据，剔除工作模式不正常的点，以及导航星数少于阈值的点；
对于SLAM坐标数据，剔除突变超过阈值的点。
4.如权利要求1所述的一种用于无人机的大地坐标系与SLAM坐标系转换方法，其特征在于，步骤（5）的具体方式如下：
通过将原点从相机中心移动到点云的质心来取消平移，同时通过质心归一化，使得SLAM坐标数据与GPS笛卡尔坐标数据有相同的质心，然后使用SVD分解，获取转换矩阵和平移矩阵。
5.如权利要求1所述的一种用于无人机的大地坐标系与SLAM坐标系转换方法，其特征在于，步骤（7）中的精度阈值为当前数据集中各组坐标数据依据步骤（7）所求得的差值的平均值。
6.如权利要求1所述的一种用于无人机的大地坐标系与SLAM坐标系转换方法，其特征在于，步骤（8）中所述的新的数据集，在步骤（7）所得数据集的基础上，根据精度需求补入了一组或多组新的坐标数据。
CN202111089253.7A 2021-09-16 2021-09-16 一种用于无人机的大地坐标系与slam坐标系转换方法 Active CN113763252B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111089253.7A CN113763252B (zh) 2021-09-16 2021-09-16 一种用于无人机的大地坐标系与slam坐标系转换方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111089253.7A CN113763252B (zh) 2021-09-16 2021-09-16 一种用于无人机的大地坐标系与slam坐标系转换方法

Publications (2)

Publication Number Publication Date
CN113763252A CN113763252A (zh) 2021-12-07
CN113763252B true CN113763252B (zh) 2022-12-09

Family

ID=78796161

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111089253.7A Active CN113763252B (zh) 2021-09-16 2021-09-16 一种用于无人机的大地坐标系与slam坐标系转换方法

Country Status (1)

Country Link
CN (1) CN113763252B (zh)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114063655A (zh) * 2022-01-17 2022-02-18 四川腾盾科技有限公司 无人机真实飞行轨迹的估算方法及装置、设备、存储介质
CN114820768B (zh) * 2022-04-15 2023-03-24 中国电子科技集团公司第五十四研究所 一种大地坐标系与slam坐标系对齐方法
CN116736327B (zh) * 2023-08-10 2023-10-24 长沙智能驾驶研究院有限公司 定位数据优化方法、装置、电子设备和可读存储介质

Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106052674A (zh) * 2016-05-20 2016-10-26 青岛克路德机器人有限公司 一种室内机器人的slam方法和系统
CN110060202A (zh) * 2019-04-19 2019-07-26 湖北亿咖通科技有限公司 一种单目slam算法的初始化方法及系统
CN112504287A (zh) * 2020-10-30 2021-03-16 广东杜尼智能机器人工程技术研究中心有限公司 slam地图坐标系与大地坐标系转换方法、装置、系统及存储介质

Patent Citations (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN106052674A (zh) * 2016-05-20 2016-10-26 青岛克路德机器人有限公司 一种室内机器人的slam方法和系统
CN110060202A (zh) * 2019-04-19 2019-07-26 湖北亿咖通科技有限公司 一种单目slam算法的初始化方法及系统
CN112504287A (zh) * 2020-10-30 2021-03-16 广东杜尼智能机器人工程技术研究中心有限公司 slam地图坐标系与大地坐标系转换方法、装置、系统及存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
关于无人机飞行坐标系与桥梁坐标系的转换;张武毅等;《公路交通科技(应用技术版)》;20171115(第11期);全文 *
基于SLAM的连续运动测量坐标转换算法研究;訾安琪等;《计算机测量与控制》;20160125(第01期);全文 *

Also Published As

Publication number Publication date
CN113763252A (zh) 2021-12-07

Similar Documents

Publication Publication Date Title
CN113763252B (zh) 一种用于无人机的大地坐标系与slam坐标系转换方法
EP3469306B1 (en) Geometric matching in visual navigation systems
CN110389348B (zh) 基于激光雷达与双目相机的定位与导航方法及装置
CN112859051B (zh) 激光雷达点云运动畸变的矫正方法
CN107084727B (zh) 一种基于高精度三维地图的视觉定位系统及方法
US11908152B2 (en) Acceleration method of depth estimation for multiband stereo cameras
CN110675450B (zh) 基于slam技术的正射影像实时生成方法及系统
CN111161179A (zh) 一种基于法向量的点云平滑光顺滤波方法
CN111366148B (zh) 适用于机载光电观瞄系统多次观察的目标定位方法
CN111829532B (zh) 一种飞行器重定位系统和重定位方法
CN110889899B (zh) 一种数字地表模型的生成方法及装置
CN114565863B (zh) 无人机图像的正射影像实时生成方法、装置、介质及设备
WO2022247306A1 (zh) 一种基于毫米波雷达的无人机定位方法
WO2020258217A1 (zh) 可移动平台状态估计方法、系统、可移动平台及存储介质
CN103985154A (zh) 一种基于全局线性方法的三维模型重建方法
CN111694012A (zh) 一种基于机载激光雷达的三维地形在线生成方法及系统
CN110706329A (zh) 一种三维场景重构方法和装置
CN112529962A (zh) 一种基于视觉算法的室内空间关键定位技术方法
WO2024005858A3 (en) Lidar system with gyroscope-aided focus steering
JPH07199804A (ja) 干渉型合成開口レーダ装置から得た３次元情報を用いる地形図作成装置
CN110160503A (zh) 一种顾及高程的无人机景观匹配定位方法
CN113779012B (zh) 一种用于无人机的单目视觉slam尺度恢复方法
CN110906922A (zh) 无人机位姿信息的确定方法及装置、存储介质、终端
CN114429515A (zh) 一种点云地图构建方法、装置和设备
CN114459474A (zh) 一种基于因子图的惯性/偏振/雷达/光流紧组合导航的方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant