CN114777768A - 一种卫星拒止环境高精度定位方法、系统及电子设备 - Google Patents
一种卫星拒止环境高精度定位方法、系统及电子设备 Download PDFInfo
- Publication number
- CN114777768A CN114777768A CN202210210794.9A CN202210210794A CN114777768A CN 114777768 A CN114777768 A CN 114777768A CN 202210210794 A CN202210210794 A CN 202210210794A CN 114777768 A CN114777768 A CN 114777768A
- Authority
- CN
- China
- Prior art keywords
- coordinate system
- laser radar
- mark
- pose
- geodetic coordinate
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 63
- 238000005457 optimization Methods 0.000 claims abstract description 16
- 238000012937 correction Methods 0.000 claims abstract description 12
- 230000004927 fusion Effects 0.000 claims abstract description 8
- 239000011159 matrix material Substances 0.000 claims description 69
- 239000013598 vector Substances 0.000 claims description 48
- 239000003550 marker Substances 0.000 claims description 23
- 238000013519 translation Methods 0.000 claims description 20
- 230000008569 process Effects 0.000 claims description 17
- 238000006243 chemical reaction Methods 0.000 claims description 13
- 125000003118 aryl group Chemical group 0.000 claims description 10
- 238000001914 filtration Methods 0.000 claims description 8
- 230000009466 transformation Effects 0.000 claims description 7
- -1 hydrogen Chemical class 0.000 claims description 3
- 239000001257 hydrogen Substances 0.000 claims description 3
- 238000004422 calculation algorithm Methods 0.000 description 22
- 238000003860 storage Methods 0.000 description 10
- 238000001514 detection method Methods 0.000 description 9
- 230000001133 acceleration Effects 0.000 description 8
- 238000005259 measurement Methods 0.000 description 8
- 238000004364 calculation method Methods 0.000 description 6
- 230000000007 visual effect Effects 0.000 description 6
- 238000012545 processing Methods 0.000 description 5
- 238000000605 extraction Methods 0.000 description 4
- BULVZWIRKLYCBC-UHFFFAOYSA-N phorate Chemical compound CCOP(=S)(OCC)SCSCC BULVZWIRKLYCBC-UHFFFAOYSA-N 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 230000000052 comparative effect Effects 0.000 description 3
- 238000004590 computer program Methods 0.000 description 3
- 230000008878 coupling Effects 0.000 description 3
- 238000010168 coupling process Methods 0.000 description 3
- 238000005859 coupling reaction Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 230000006870 function Effects 0.000 description 3
- JILPJDVXYVTZDQ-UHFFFAOYSA-N lithium methoxide Chemical compound [Li+].[O-]C JILPJDVXYVTZDQ-UHFFFAOYSA-N 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000011160 research Methods 0.000 description 3
- YCKRFDGAMUMZLT-UHFFFAOYSA-N Fluorine atom Chemical compound [F] YCKRFDGAMUMZLT-UHFFFAOYSA-N 0.000 description 2
- 230000008901 benefit Effects 0.000 description 2
- 230000008859 change Effects 0.000 description 2
- 239000003245 coal Substances 0.000 description 2
- 238000004891 communication Methods 0.000 description 2
- 229910052731 fluorine Inorganic materials 0.000 description 2
- 239000011737 fluorine Substances 0.000 description 2
- 238000009434 installation Methods 0.000 description 2
- 230000007704 transition Effects 0.000 description 2
- RZVHIXYEVGDQDX-UHFFFAOYSA-N 9,10-anthraquinone Chemical compound C1=CC=C2C(=O)C3=CC=CC=C3C(=O)C2=C1 RZVHIXYEVGDQDX-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000015556 catabolic process Effects 0.000 description 1
- 238000000354 decomposition reaction Methods 0.000 description 1
- 238000006731 degradation reaction Methods 0.000 description 1
- 238000009795 derivation Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000005484 gravity Effects 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000003993 interaction Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 230000003287 optical effect Effects 0.000 description 1
- 238000005192 partition Methods 0.000 description 1
- 230000002093 peripheral effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 230000026676 system process Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
- G01C21/1652—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with ranging devices, e.g. LIDAR or RADAR
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/18—Stabilised platforms, e.g. by gyroscope
-
- 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
- G01S17/00—Systems using the reflection or reradiation of electromagnetic waves other than radio waves, e.g. lidar systems
- G01S17/02—Systems using the reflection of electromagnetic waves other than radio waves
- G01S17/06—Systems determining position data of a target
- G01S17/42—Simultaneous measurement of distance and other co-ordinates
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Electromagnetism (AREA)
- Computer Networks & Wireless Communication (AREA)
- Position Fixing By Use Of Radio Waves (AREA)
Abstract
本发明公开了一种卫星拒止环境高精度定位方法、系统及电子设备。本发明所提供的卫星拒止环境高精度定位方法包括获取激光雷达在大地坐标系的位姿;对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合;以及对融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正。本发明通过布置少量的aruco标记就能够在拒止环境中对激光雷达进行全局修正,从而大幅度提高导航精度以及系统的鲁棒性。
Description
技术领域
本发明涉及导航技术领域,尤其是涉及一种卫星拒止环境高精度定位方法、系统及电子设备。
背景技术
目前大多数定位算法需要借助卫星导航系统,可用于拒止环境中的定位算法较为数量较为有限。现有的拒止环境定位算法均为在局部坐标系下或里程计坐标系下运行,例如LIOM、VINS、ORB_SLAM、平面标记的映射与定位(mapping and localization fromplanar marker)、UWB定位系统等等。
其中,LIOM、VINS等里程计算法运行时所绘制的地图为里程计坐标系或局部坐标系下,在拒止环境中无法与全局坐标系建立转换关系。上述算法虽有回环检测等修正方式,但这均为基于局部坐标系的修正,并未真正与全局坐标系产生联系,这就导致当算法长时间运行于体量较大的拒止环境中时,由于误差累计,里程计无法根据所处的真实位置对结果进行有效、真实的修正。
平面标记的映射与定位中导航算法仅可应用于小体量环境中,要求传感器单次必须看到多个标记点,并将标记作为一种便于识别的标记。但是将标记密度布置的极高不仅影响其他任务的作业,也不现实;且该算法同样并不能通过标记反映出里程计在大地坐标系下的位置。
UWB定位系统类似于拒止环境中的卫星导航系统,通过预布置于环境中的电磁波为里程计提供位置信息,但造价高昂,对电磁环境要求极为严格,作用距离短,易受干扰,在大尺度拒止环境中,无法形成使用通过少数基站低成本完成定位。
因此,有必要研究一种能够解决上述技术问题的拒止环境高精度定位方法。
发明内容
针对上述现有技术中存在的问题,本发明提供一种卫星拒止环境高精度定位方法、系统及电子设备。
为了实现上述目的,第一方面,本发明提供一种卫星拒止环境高精度定位方法,其包括:
获取激光雷达在大地坐标系的位姿;
对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合;以及
使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正。
优选地,获取激光雷达在大地坐标系的位姿的过程,包括:
获取至少一个aruco标记四个顶点在像素坐标系的坐标值;
根据至少一个aruco标记四个顶点在像素坐标系的坐标值和对应aruco标记的边长,获取至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵;
根据至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、激光雷达与云台之间的相对位置和至少一个aruco标记在大地坐标系的位姿,解算激光雷达在大地坐标系的位姿。
第二方面,本发明提供一种卫星拒止环境高精度定位系统,其包括:
获取激光雷达在大地坐标系的位姿的模块;
对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合的模块;以及
使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正的模块。
第三方面,本发明提供一种电子设备,其包括:存储器,处理器;
存储器用于存储处理器可执行指令;
处理器用于根据存储器存储的可执行指令,实现第一方面的卫星拒止环境高精度定位方法。
本发明的卫星拒止环境高精度定位方法、系统及电子设备所具有的有益效果包括:
(1)本发明与现有技术适用于卫星导航环境或二维导航相比,更适用于卫星拒止环境的三维导航;
(2)本发明基于aruco标记在拒止环境中能够得到激光雷达相对于真实环境的位姿,从而实现高精度的定位,而现有技术只能得到激光雷达在激光雷达里程计坐标系的位姿,无法得到该坐标系与真实的大地坐标系转换关系;
(3)本发明在拒止环境中能够根据真实环境对激光雷达误差进行修正,解决了现有技术累计误差大且无法修正的问题;
(4)本发明操作简单,只需在环境中布置少量的aruco标记即可实现,无需其他大量的传感器等设备,节约成本。
附图说明
图1是本发明一种优选实施方式的卫星拒止环境高精度定位方法的流程示意图;
图2是本发明一种优选实施方式的卫星拒止环境高精度定位系统的结构示意图;
图3为本发明因子图结构示意图;
图4为本发明实施例、对比例1~2的轨迹结果仿真模拟结果图。
具体实施方式
下面结合附图对本发明的较佳实施例进行详细阐述,以使本发明的优点和特征能更易于被本领域技术人员理解,从而对本发明的保护范围做出更为清楚明确的界定。
需要说明的是,在本文中,术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含,从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素,而且还包括没有明确列出的其他要素,或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下,由语句“包括……”限定的要素,并不排除在包括要素的过程、方法、物品或者设备中还存在另外的相同要素。
目前大多数定位算法需要借助卫星导航系统,可用于拒止环境中的定位算法较为数量较为有限。现有的拒止环境定位算法如LIOM、VINS、ORB_SLAM等均为在局部坐标系下或里程计坐标系下运行,其生成的全局地图与真实的大地坐标系转换关系未知;或者平面标记的映射与定位、UWB定位系统需要提前布置大量标记点或传感器,且对传感器环境要求比较苛刻。
经研究发现,获取激光雷达在真实环境中的位姿,并利用真实位置对其误差进行校正,能够在大场景拒止环境中大幅度提高导航精度。
为了实现上述过程,本发明通过在因子图中增加aruco因子,避免了卫星导航系统仅利用IMU与激光雷达数据进行无人机位姿计算在长时间中产生无法修正的误差。Aruco因子可以将IMU与激光雷达解算时使用的全局坐标系与大地坐标系固连并对正,并通过aruco位置信息对IMU与激光雷达解算所产生的无人机位姿进行误差修正,同时,作为激光雷达匹配退化时维持卫星导航系统正常运行的信息源。
示例性的,对本发明卫星拒止环境高精度定位方法的应用环境进行说明,搭建包含各传感器的无人机系统,具体过程为选定一架无人机(可采用常规型号),在无人机上分别搭载各传感器,包括:视觉惯性里程计(VIO)和激光雷达等等;其中,视觉惯性里程计包含相机(可含云台)和惯性测量单元(IMU),相机镜头方向与无人机前进方向一致。视觉惯性里程计和激光雷达均可以集成在无人机的飞控板上。视觉惯性里程计和激光雷达均可采用常规型号部件,视觉惯性里程计和激光雷达的重量总和不超过无人机的额定载荷。当无人机系统工作时,激光雷达采集点云信息、IMU采集加速度、姿态角(角度)、角速率,相机采集图像信息,云台采集相机(或云台)的姿态角,同时各传感器将采集的信息传输给能够进行数据处理的终端或服务器。
第一方面,本发明提供一种卫星拒止环境高精度定位方法,如图1所示。该方法主要包括以下步骤:
步骤S101、获取激光雷达在大地坐标系的位姿。
其中,拒止环境是指屏蔽卫星信号的环境,在该环境下利用卫星进行定位的导航系统都不能正常接收到卫星信号,不能正常工作。
优选地,在步骤S101之前,可以对各传感器进行标定,分别获得每个传感器对应的参数。例如可以包括:相机的内外参矩阵,IMU的内参数和测量噪声,激光雷达安装参数,相机坐标系和IMU坐标系之间的相对位姿关系、IMU与激光雷达里程计位姿,相机与激光雷达位姿等。其中,相机、IMU、激光雷达等已有成熟的标定方法,比如相机的张正友标定法、IMU的误差分析标定法、激光雷达的交互信息最大化外部自动标定法等。
优选地,在步骤S101之前需要在所选区域(真实环境)内布置aruco标记,并记录各个aruco标记中任一点在大地坐标系下的位姿,为了方便计算优选为周边任一点或中心点,更优选为aruco标记中心点在大地坐标系下的位姿Aruco标记作为基准标记放置在要成像的对象或场景上。它是一个背景为黑色的正方形,正方形内部的白色图案用来表示标记的唯一性,并且存储一些信息,黑色边界的目的是为了提高aruco标记检测的准确性和性能。
本发明对aruco标记的布置和尺寸不作具体的限制,本领域技术人员根据实际情况而确定。其中aruco标记的尺寸可以任意的更改,为了成功检测可根据所选区域大小和场景选择合适的尺寸。在实际使用中,如果标记的尺寸太小,可能无法检测到它,这时可以选择更换较大尺寸的标记,或者将相机离标记更近一些。
示例性的,当所选区域为工厂车间时,可以在工厂车间的地面的四个角落布置aruco标记,为了增加检测的精准性,还可以在地面的四个边每一定间隔处设置一个aruco标记,例如间隔5米。
在本发明的一个优选实施方式中,通过获取激光雷达在大地坐标系的位姿的过程,可以包括以下步骤:
S101-1、获取至少一个aruco标记四个顶点在像素坐标系的坐标值。
本发明中像素坐标系、相机坐标系、大地坐标系、云台坐标系、机体坐标系、世界坐标系都是本领域中已知的坐标系,且各个坐标系之间可以相互转换;其中,像素坐标系是表现相机成像平面的坐标系,相机坐标系是固连于相机的坐标系,云台坐标系为固连于云台基座(与机体相连部分)的坐标系,大地坐标系指与大地固连的坐标系,在本发明中指在已知场景中某个预设的坐标系。机体坐标系是指固定在无人机上的遵循右手法则的三维正交直角坐标系,其原点位于无人机的质心。
具体地,当无人机系统执行任务时,相机采集在所选区域采集图像信息,然后对图像信息依次进行灰度处理,二值化,比特位提取,识别至少一个aruco标记,最后获取至少一个aruco标记四个顶点在像素坐标系的坐标值。
其中,灰度处理为把RGB格式的图像三个原色信号处理为灰度图像的单一颜色信号。
二值化为根据处理后的灰度图像及指定阈值将灰度信号处理为0或255。
比特位提取为在二维码范围内识别每一aruco像素的灰度信号均值,并将均值按照预设阈值进行二值化,并作为该aruco像素的信号值,然后根据各个像素信号值的排列识别aruco标记编号。
通过上述步骤,能够获得aruco标记四个顶点在像素坐标系的准确的坐标值,避免了对aruco标记的误检测。
S101-2、根据至少一个aruco标记四个顶点在像素坐标系的坐标值和对应aruco标记的边长,获取至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵。
由于每个aruco标记在像素坐标系的坐标不同,所以对应的aruco标记在相机坐标系的位置也不同,因此需要计算每个aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵。
具体地,根据相机参数、该aruco标记四个顶点的坐标值和该aruco标记边长使用PNP算法解算出对应aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵,分别记为ri与ti,其中i为aruco标记的编号。
具体地,PNP算法为现有的算法,其计算过程可以参考https://blog.csdn.net/u014709760/article/details/88029841。
S101-3、根据至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、激光雷达与云台之间的相对位置和至少一个aruco标记在大地坐标系的位姿,解算激光雷达在大地坐标系的位姿。
现有的拒止环境定位算法均为在局部坐标系下或里程计坐标系下运行,其生成的全局地图(整个地图)与真实的大地坐标系并不固连,其会导致当算法长时间运行于大体量的拒止环境中时,激光雷达无法根据所处的真实位置对累计误差及偏差进行有效、真实的修正。
因此,为了得到激光雷达在大地坐标系的位姿,以及保证激光雷达在大地坐标系的位姿的准确性,需要对各个传感器的结果进行融合。
具体为激光雷达在大地坐标系的位姿通过式一表示:
其中,表示检测到第i个aruco标记时,激光雷达在大地坐标系的位姿;表示激光雷达与云台之间的相对位置关系的转换矩阵;表示检测第i个aruco标记时,相机在云台坐标系下位姿转换矩阵的逆矩阵;表示第i个aruco标记相对相机坐标系的位姿的转换矩阵;表示第i个aruco标记在大地坐标系中位姿的转换矩阵。
在本发明中,通过式一一系列的坐标系转换就能得到检测到第i个aruco标记时,激光雷达在大地坐标系的位姿。
其中,xg、yg、zg分别表示激光雷达与云台之间的相对位置。
其中,
ri=(rix riy riz)T
其中,i为aruco标记的编号;ri表示第i个aruco标记对相机坐标系的旋转矢量;ti表示第i个aruco标记对相机坐标系的平移矢量;rix、riy、riz分别表示旋转矢量ri中的分量;I表示单位矩阵;αi表示第i个aruco标记对相机坐标系的旋转角度,亦是旋转矢量ri的模长。
在本发明中,通过设置少量的aruco标记就能够获得拒止环境中激光雷达相对于真实环境的位姿,从而适用于卫星拒止环境的三维导航。
步骤S102、对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合。
优选地,对激光雷达在大地坐标系的位姿和IMU采集的信息进行卡尔曼滤波,获得激光雷达在大地坐标系下的位姿和协方差矩阵。
根据本发明,由于激光雷达、IMU和无人机系统固连,故激光雷达的位姿与无人机系统的姿态相同,也就是它们一起旋转平移,但是得到的姿态角等会根据初始位置的不同而不同。因此可通过激光雷达的位姿状态方程(或无人机系统姿态状态方程),即可获得无人机姿态协方差矩阵。
在本发明中,根据检测第i个aruco标记时,其在相机坐标系的位姿以及在相机坐标系下的位置调整所对应的测量噪声协方差矩阵,记为Ri(Ci1(ξi(ri,ti)),Ci2(di))。其中两个影响因子包括:ξi(ri,ti),其通过旋转矢量ri、平移矢量ti与对应aruco标记边长l共同计算得到,其是由位姿导致测量视角不同产生的;和di,其为aruco标记重心距光轴距离,其是由该aruco标记在相机坐标系不同位置导致的。无人机系统运行前,标定相机后,分别根据ξi(ri,ti)和di对测量噪声协方差矩阵进行标定,拟合出Ci1、Ci2函数。
根据上述动态的调整测量噪声协方差矩阵,能够保证卡尔曼滤波的准确性。
具体地,状态方程可以表示为:
观测方程可以表示为:
其中,k表示当前时刻;k-1表示上一时刻;x表示状态向量;xk表示k时刻的状态向量;xk-1表示根据k-1时刻的状态向量;Z表示观测变量;Zk表示k时刻的观测变量;A表示状态转移矩阵,H为状态向量到观测变量的转换矩阵;Vk为观测方程中的噪声,Wk-1为先验噪声。
在卡尔曼滤波时,将k时刻检测到的所有aruco标记所解算出的Ri(Ci1(ξi(ri,ti)),Ci2(di))进行排序,选择并记录Ri(Ci1(ξi(ri,ti)),Ci2(di))最小值所对应的aruco标记的编号i以及其中,表示激光雷达在大地坐标系下的位置,其为中平移矢量部分,表示激光雷达在大地坐标系下的姿态,由中旋转矢量部分解出。aimu、qimu、ωimu分别为IMU观测的加速度、姿态、角速度。
卡尔曼滤波是常用的一种滤波方法,可以表示为:
其中,表示k时刻的激光雷达状态向量的校正值,Pk表示xk对应的协方差矩阵,表示Pk的估计值,Pk-1表示xk-1对应的协方差矩阵,K表示卡尔曼滤波增益系数,∑k表示测量噪声协方差,I表示单位矩阵,Q表示系统过程的协方差矩阵。
将(pzkqzakaimuqimuωimu)T作为观测,将作为状态向量,从而能够得到激光雷达(无人机系统)姿态的协方差矩阵Pk。将k时刻状态xk中的位置与姿态以及Pk中上述两分量所对应的分量组成aruco因子,作为因子图优化的Xk的初值。
在本发明中,当3秒内未检测到aruco标记则停止EKF过程并初始化,在重新检测到aruco标记时继续EKF的计算。
经研究发现,根据上述过程,能够精确的获得激光雷达(无人机系统)姿态的协方差矩阵Pk。
步骤S103、对融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正。
在无人机系统中,还包括IMU等传感器,为了保证无人机系统导航的精度和鲁棒性,可以将多种传感器的信息进行融合。
在本发明中,因子图中还包括预计分因子、激光雷达因子、回环因子等,如图3所示。其中预计分因子、激光雷达因子、回环因子的获得过程可以为现有方法。在k时刻,将aruco因子作为优化初值,并添加k-1与k时刻之间由IMU预计分解算出的预计分因子,k-1与k时刻之间由激光雷达运动估计解算出的激光雷达因子,并通过aruco标记判定回环,若对于j时刻产生回环则继续在因子图中加入j与k时刻的回环因子。
其中,因子图中的无人机k时刻的状态为:
Xk=(pk vk Rk bak bgk)T
其中,pk为无人机在大地坐标系下的平移矢量,vk为无人机在大地坐标系下的速度矢量,Rk为无人机在大地坐标系下的旋转矩阵,bak、bgk分别为IMU中加速度计偏差与陀螺仪偏差量。
优选地,预计分因子的获得过程为:
对IMU所采集的加速度、姿态角、角速度进行预计分;
将预计分结果中的两帧之间位姿TIMU与所对应的协方差CIMU作为IMU因子。
优选地,激光雷达因子的获得过程为:
(1)获取多帧图像:当采集到激光点云时,先对各个点进行运动补偿、对齐时间戳,将一个周期的点云投影到一帧点云图像上,记为第n帧;
(2)特征提取:对该帧点云图像进行特征提取,计算每一线上某一点bk前后五个点(周围点)到该点bk的平均距离kk,如下式所示:
当kk与周围点差距小于预设距离阈值时,则该点附近曲率较小,较为平滑,一般处于平面,作为面特征点,记为Fmn;当kk与周围点差距大于预设距离阈值时,则该点附近曲率较大,突变较厉害,一般为角点,作为边缘特征点,记为Fbn。
(3)确定关键帧:关键帧选取时,第一帧作为关键帧;其余,确定第n帧是否为关键帧时,对比第n帧特征点集合{Fmn,Fbn}与上一关键帧km中特征点的共视关系,当共视关系变化大于设定阈值时将第n帧设置为关键帧,记为km+1。
(4)特征匹配:对于两关键帧,分别使用最新关键帧km+1中的面特征点Fmn与上5个关键帧km中所对应相邻的三个面特征点所组成的平面,计算点到平面距离使用最新关键帧km+1中的边缘特征点Fbn与上5个关键帧km至km中所对应相邻的边缘特征点所组成的直线,计算点到直线距离将作为代价函数进行运动估计,优化两关键帧中的旋转平移变化。若优化求解过程中未出现退化,则在因子图中加入状态Xm+1,将km+1投影在地图中,并将优化结果作为因子图中状态Xm与Xm+1之间的测量;若求解优化过程发生退化,且持续检测到aruco标记,则根据aruco标记解出的激光雷达的位姿将关键帧km+1投影在地图中,但不将优化结果添加为因子图中状态Xm与Xm+1之间的测量。
另外,若读取到激光雷达第n帧时同时检测到第i个aruco标记,即便共视关系未低于预设阈值,亦额外添加该帧为关键帧kai;在后续对第i个aruco标记的连续检测过程中,若出现aruco因子的协方差小于kai时刻对应的协方差,则更新kai;当完成对第i个aruco标记的连续检测后,将kai与其时间最近的由共视特征产生的关键帧km、及km前后相邻的km-2至km+2进行匹配,将kai加入地图。其中对第i个aruco标记检测的不连续的阈值优选设置为3s。
经研究发现,通过引入aruco因子,使得激光雷达因子更加准确,并进一步提高定位的准确性。
优选地,回环因子的获得过程为:
当导航系统第l次采集到对第i个aruco标记的连续检测时,判定为回环。
将l-1次第i个aruco标记检测到回环所对应的的关键帧kai,1、kai,2至kai,l-1,以及其附近由特征共视得到的关键帧{kq-2,kq-1,kq,kq+1,kq+2}、{kw-2,kw-1,kw,kw+1,kw+2}至{ke-2,ke-1,ke,ke+1,ke+2},与离第l次回环关键帧kai,l最近的关键帧kr进行特征匹配,分别进行运动估计,若优化有解且未退化,则加入因子图作为状态Xr与状态Xm之间的测量。
在本发明中,通过融合多种传感器的信息,能够发挥各传感器的优点,从而提高了系统的鲁棒性。
第二方面,本发明提供一种卫星拒止环境高精度定位系统,如图2所示。该系统包括:
获取激光雷达在大地坐标系的位姿的模块201;
对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合的模块202;以及
使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正的模块203。
本发明提供的卫星拒止环境高精度定位系统,可用于执行上述任一实施例描述的一种卫星拒止环境高精度定位方法,其实现原理和技术效果类似,在此不再赘述。
优选地,本发明一种卫星拒止环境高精度定位系统中各模块可直接在硬件中、在由处理器执行的软件模块中或在两者的组合中。
软件模块可驻留在RAM存储器、快闪存储器、ROM存储器、EPROM存储器、EEPROM存储器、寄存器、硬盘、可装卸盘、CD-ROM或此项技术中已知的任何其它形式的存储介质中。示范性存储介质耦合到处理器,使得处理器可从存储介质读取信息和向存储介质写入信息。
处理器可以是中央处理单元(英文:CentralProcessingUnit,简称:CPU),还可以是其他通用处理器、数字信号处理器(英文:DigitalSignalProcessor,简称:DSP)、专用集成电路(英文:ApplicationSpecificIntegratedCircuit,简称:ASIC)、现场可编程门阵列(英文:FieldProgrammableGateArray,简称:FPGA)或其它可编程逻辑装置、离散门或晶体管逻辑、离散硬件组件或其任何组合等。通用处理器可以是微处理器,但在替代方案中,处理器可以是任何常规处理器、控制器、微控制器或状态机。处理器还可实施为计算装置的组合,例如DSP与微处理器的组合、多个微处理器、结合DSP核心的一个或一个以上微处理器或任何其它此类配置。在替代方案中,存储介质可与处理器成一体式。处理器和存储介质可驻留在ASIC中。ASIC可驻留在用户终端中。在替代方案中,处理器和存储介质可作为离散组件驻留在用户终端中。
第三方面,本发明提供一种电子设备,其包括:存储器,处理器;
存储器用于存储处理器可执行指令;
处理器用于根据存储器存储的可执行指令,实现第一方面的卫星拒止环境高精度定位方法。
第四方面,本发明提供一种计算机可读存储介质,其特征在于,计算机可读存储介质中存储有计算机执行指令,计算机执行指令被处理器执行时用于实现如第一方面的卫星拒止环境高精度定位方法。
第五方面,一种程序产品,程序产品包括计算机程序,计算机程序存储在可读存储介质中,至少一个处理器可以从可读存储介质读取计算机程序,至少一个处理器执行计算机程序使执行实施例一描述的卫星拒止环境高精度定位方法。
在本发明所提供的几个实施例中,应该理解到,所揭露的装置和方法,可以通过其它的方式实现。例如,以上所描述的装置实施例仅仅是示意性的,例如,所述单元的划分,仅仅为一种逻辑功能划分,实际实现时可以有另外的划分方式,例如多个单元或组件可以结合或者可以集成到另一个系统,或一些特征可以忽略,或不执行。另一点,所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口,装置或单元的间接耦合或通信连接,可以是电性,机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的,作为单元显示的部件可以是或者也可以不是物理单元,即可以位于一个地方,或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
实施例
在40m*110m的江西省丰城市丰城热电厂一期干煤棚内,在地面除起飞点及附近两个aruco标记外,其余全部布置于距起飞点1.5m高的人行步道上,y轴坐标为-14.8m,间距10m,70m处因有固定设备未放置,一共13个aruco标记,具体布置见图4,range表示真实场景中干煤棚地基范围。无人机搭载相机(型号禅思X5s)、IMU(型号MTI-300)和激光雷达(型号Velodyne-16)。
当无人机执行任务时,相机采集棚内的图像信息,然后对图像信息依次进行灰度处理,二值化,比特位提取,识别每一个aruco标记。
根据相机参数和PNP解算出13个aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵,分别记为ri与ti,其中i为aruco标记的编号。
分别根据13个aruco标记中心点相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、雷达与云台之间的相对位置和13个aruco标记中心点在大地坐标系的位姿,获取激光雷达在大地坐标系的位姿。
激光雷达在大地坐标系的位姿通过式一表示:
其中,表示检测到第i个aruco标记时,激光雷达在大地坐标系的位姿;表示激光雷达与云台之间的相对位置关系的转换矩阵;表示检测第i个aruco标记时,相机在云台坐标系中位姿转换矩阵的逆矩阵;表示第i个aruco标记在相机坐标系中位姿转换矩阵;表示第i个aruco标记在大地坐标系中位姿转换矩阵。
其中,xg、yg、zg分别表示激光雷达与云台之间的相对位置。
其中,
ri=(rix riy riz)T
其中,i为aruco标记的编号;ri表示第i个aruco标记的旋转矩阵;ti表示第i个aruco标记的平移矩阵;rix、riy、riz分别表示旋转矩阵ri中的分量;I表示单位矩阵;αi表示第i个aruco标记的旋转角度。
对激光雷达在大地坐标系的位姿和IMU采集的信息进行卡尔曼滤波,获得激光雷达的误差协方差矩阵和协方差矩阵。
具体地,状态方程可以表示为:
观测方程可以表示为:
其中,k表示当前时刻;k-1表示上一时刻;x表示状态向量;xk表示k时刻的状态向量;xk-1表示根据k-1时刻的状态向量;Z表示观测变量;Zk表示k时刻的观测变量;A表示状态转移矩阵,H为状态向量到观测变量的转换矩阵;Vk为观测方程中的噪声,Wk-1为先验噪声。
在卡尔曼滤波时,将k时刻检测到的所有aruco标记所解算出的Ri(Ci1(ξi(ri,ti)),Ci2(di))进行排序,选择并记录Ri(Ci1(ξi(ri,ti)),Ci2(di))最小值所对应的aruco标记的编号i以及其中,表示激光雷达在大地坐标系下的位置,其为中平移矢量部分,表示激光雷达在大地坐标系下的姿态,由中旋转矢量部分解出。aimu、qimu、ωimu分别为IMU观测的加速度、姿态、角速度。
将(pzkqzakaimuqimuωimu)T作为观测,将作为状态向量,从而能够得到激光雷达(无人机系统)姿态的协方差矩阵Pk。将k时刻状态xk中的位置pk与姿态qk以及Pk中上述两分量所对应的分量组成aruco因子,作为因子图优化的Xk的初值,然后添加k-1与k时刻之间由IMU预计分解算出的预计分因子,k-1与k时刻之间有激光雷达运动估计解算出的激光雷达因子,并通过aruco标记判定回环,若对于j时刻产生回环则继续在因子图中加入j与k时刻的回环因子实现信息融合与全局修正。具体仿真结果见图4。
对比例1
利用现有的loam算法对无人机进行定位,其中loam算法可以参照https://blog.csdn.net/shoufei403/article/details/103664877。具体仿真结果见图4。
对比例2
利用现有的lio_sam算法对无人机进行定位,其中lio_sam算法可以参照https://blog.csdn.net/tiancailx/article/details/109483450。具体仿真结果见图4。rotate表示实施例的结果手动旋转至与loam、lio_sam两种算法近似的状态的结果。
从图4可以看出,loam、lio_sam算法解算得到的轨迹受到了作业时无人机摆放位姿偏差、激光雷达安装角度偏差等影响,且没有对轨迹旋转偏差进行修正的功能,所计算得到的轨迹均与实际飞行中的轨迹差距甚远,在应用中极大地提高了无人机碰撞的风险,且无法满足后续视觉测方等任务,不满足工况需求。而实施例通过aruco标记,完成了对导航系统的全局优化,优化不仅大幅修正了航线的方向,同时还修正了累计误差。
以上结合具体实施方式和范例性实例对本发明进行了详细说明,不过这些说明并不能理解为对本发明的限制。本领域技术人员理解,在不偏离本发明精神和范围的情况下,可以对本发明技术方案及其实施方式进行多种等价替换、修饰或改进,这些均落入本发明的范围内。
Claims (10)
1.一种卫星拒止环境高精度定位方法,其特征在于,包括:
获取激光雷达在大地坐标系的位姿;
对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合;以及
使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正。
2.根据权利要求1所述的卫星拒止环境高精度定位方法,其特征在于,所述获取激光雷达在大地坐标系的位姿的过程,包括:
获取至少一个aruco标记四个顶点在像素坐标系的坐标值;
根据至少一个aruco标记四个顶点在像素坐标系的坐标值和对应aruco标记的边长,获取所述至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵;
根据至少一个aruco标记相对于相机坐标系的旋转矩阵及平移矩阵、云台的姿态角、激光雷达与云台之间的相对位置和所述至少一个aruco标记在大地坐标系的位姿,解算激光雷达在大地坐标系的位姿。
8.根据权利要求1所述的卫星拒止环境高精度定位方法,其特征在于,对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合的过程为:
对激光雷达在大地坐标系的位姿和IMU采集的信息进行卡尔曼滤波。
9.一种卫星拒止环境高精度定位系统,其特征在于,包括:
获取激光雷达在大地坐标系的位姿的模块;
对激光雷达在大地坐标系的位姿和IMU采集的信息进行融合的模块;以及
使用融合后的激光雷达在大地坐标系的位姿通过因子图优化与激光雷达及IMU进行信息融合与全局修正的模块。
10.一种电子设备,其特征在于,包括:存储器,处理器;
所述存储器用于存储所述处理器可执行指令;
所述处理器用于根据所述存储器存储的可执行指令,实现如权利要求1至8中任一项所述的卫星拒止环境高精度定位方法。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210210794.9A CN114777768A (zh) | 2022-03-03 | 2022-03-03 | 一种卫星拒止环境高精度定位方法、系统及电子设备 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202210210794.9A CN114777768A (zh) | 2022-03-03 | 2022-03-03 | 一种卫星拒止环境高精度定位方法、系统及电子设备 |
Publications (1)
Publication Number | Publication Date |
---|---|
CN114777768A true CN114777768A (zh) | 2022-07-22 |
Family
ID=82423805
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202210210794.9A Pending CN114777768A (zh) | 2022-03-03 | 2022-03-03 | 一种卫星拒止环境高精度定位方法、系统及电子设备 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN114777768A (zh) |
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116012377A (zh) * | 2023-03-24 | 2023-04-25 | 四川腾盾科技有限公司 | 一种基于卫星地图的无人机虚拟观测图像生成及定位方法 |
CN117034191A (zh) * | 2023-08-04 | 2023-11-10 | 广东省机场管理集团有限公司工程建设指挥部 | 基于5g云平台的车辆多源信息融合方法、装置及介质 |
-
2022
- 2022-03-03 CN CN202210210794.9A patent/CN114777768A/zh active Pending
Cited By (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN116012377A (zh) * | 2023-03-24 | 2023-04-25 | 四川腾盾科技有限公司 | 一种基于卫星地图的无人机虚拟观测图像生成及定位方法 |
CN117034191A (zh) * | 2023-08-04 | 2023-11-10 | 广东省机场管理集团有限公司工程建设指挥部 | 基于5g云平台的车辆多源信息融合方法、装置及介质 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
US10509983B2 (en) | Operating device, operating system, operating method, and program therefor | |
CN111415387B (zh) | 相机位姿确定方法、装置、电子设备及存储介质 | |
WO2018142900A1 (ja) | 情報処理装置、データ管理装置、データ管理システム、方法、及びプログラム | |
US20160063717A1 (en) | Point cloud position data processing device, point cloud position data processing system, point cloud position data processing method, and program therefor | |
EP3454008A1 (en) | Survey data processing device, survey data processing method, and survey data processing program | |
CN112837352B (zh) | 基于图像的数据处理方法、装置及设备、汽车、存储介质 | |
CN112781586B (zh) | 一种位姿数据的确定方法、装置、电子设备及车辆 | |
CN114777768A (zh) | 一种卫星拒止环境高精度定位方法、系统及电子设备 | |
CN111750896B (zh) | 云台标定方法、装置、电子设备及存储介质 | |
KR102490521B1 (ko) | 라이다 좌표계와 카메라 좌표계의 벡터 정합을 통한 자동 캘리브레이션 방법 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN116184430B (zh) | 一种激光雷达、可见光相机、惯性测量单元融合的位姿估计算法 | |
CN115371665A (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
CN112529957A (zh) | 确定摄像设备位姿的方法和装置、存储介质、电子设备 | |
CN115456898A (zh) | 停车场的建图方法、装置、车辆及存储介质 | |
CN112862818A (zh) | 惯性传感器和多鱼眼相机联合的地下停车场车辆定位方法 | |
CN116952229A (zh) | 无人机定位方法、装置、系统和存储介质 | |
CN115930948A (zh) | 一种果园机器人融合定位方法 | |
CN113790711B (zh) | 一种无人机低空飞行位姿无控多视测量方法及存储介质 | |
CN113301248B (zh) | 拍摄方法、装置、电子设备及计算机存储介质 | |
CN114842224A (zh) | 一种基于地理底图的单目无人机绝对视觉匹配定位方案 | |
JP2018125706A (ja) | 撮像装置 | |
CN113504385A (zh) | 复数相机测速方法及测速装置 | |
CN107796417B (zh) | 一种自适应估计景象匹配与惯导安装误差的方法 | |
CN112347935B (zh) | 一种基于双目视觉slam的自动驾驶车辆定位方法及系统 |
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 |