CN113534227B - 一种适用于复杂非合作场景的多传感器融合绝对定位方法 - Google Patents
一种适用于复杂非合作场景的多传感器融合绝对定位方法 Download PDFInfo
- Publication number
- CN113534227B CN113534227B CN202110841655.1A CN202110841655A CN113534227B CN 113534227 B CN113534227 B CN 113534227B CN 202110841655 A CN202110841655 A CN 202110841655A CN 113534227 B CN113534227 B CN 113534227B
- Authority
- CN
- China
- Prior art keywords
- gnss
- point cloud
- rescue equipment
- ins
- information
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 46
- 230000004927 fusion Effects 0.000 title claims abstract description 40
- 230000001133 acceleration Effects 0.000 claims description 37
- 238000005259 measurement Methods 0.000 claims description 35
- 239000011159 matrix material Substances 0.000 claims description 15
- 238000011156 evaluation Methods 0.000 claims description 12
- 238000012545 processing Methods 0.000 claims description 11
- 238000001914 filtration Methods 0.000 claims description 9
- 238000009616 inductively coupled plasma Methods 0.000 claims description 9
- 238000005070 sampling Methods 0.000 claims description 7
- 238000005457 optimization Methods 0.000 claims description 5
- 238000005516 engineering process Methods 0.000 claims description 4
- 230000001419 dependent effect Effects 0.000 claims description 3
- 238000007667 floating Methods 0.000 claims description 3
- 239000005433 ionosphere Substances 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 3
- 239000005436 troposphere Substances 0.000 claims description 3
- 238000004891 communication Methods 0.000 abstract description 3
- 238000007499 fusion processing Methods 0.000 abstract description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000007812 deficiency Effects 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000010295 mobile communication Methods 0.000 description 1
- 230000001052 transient effect Effects 0.000 description 1
Images
Classifications
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/485—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an optical system or imaging system
-
- 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/86—Combinations of lidar systems with systems other than lidar, radar or sonar, e.g. with direction finders
-
- 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/48—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system
- G01S19/49—Determining position by combining or switching between position solutions derived from the satellite radio beacon positioning system and position solutions derived from a further system whereby the further system is an inertial position system, e.g. loosely-coupled
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- General Physics & Mathematics (AREA)
- Electromagnetism (AREA)
- Navigation (AREA)
Abstract
本发明公开可一种适用于复杂非合作场景的多传感器融合绝对定位方法,适用于复杂非合作场景的多传感器融合绝对定位,它针对遮挡条件GNSS高精度绝对定位失效、广域动态场景激光雷达SLAM无法实现绝对定位以及通信中断与缺失的灾害环境,构建惯性导航为媒介的GNSSPPP‑RTK/INS、激光雷达分布式融合处理体系,设计了GNSSPPP‑RTK和惯性导航融合绝对定位方法,在此基础上,设计惯性导航辅助的激光雷达实时畸变在线补偿和图优化相对位姿约束融合估计方案,提升智能救援装备无缝、高精度绝对定位的可靠性。
Description
技术领域
本发明公开了一种适用于复杂非合作场景的多传感器融合绝对定位,适用于复杂地质灾害等遮挡、广域作业环境条件下的智能救援平台高精度绝对定位。
背景技术
基于GNSS的RTK技术能为智能救援装备在开阔和广域环境下提供高精度的定位性能,但是针对遮挡、漫反射等复杂的地质灾害环境环境条件,如何解决该广域、非合作环境的高精度绝对定位问题,是提升应急救援装备的智能化、实现自主作业的重要信息支撑。
基于GNSS-RTK、INS和激光雷达的多传感器融合已成为当前智能驾驶的主要手段,但是仍然无法适用于当前广域、非合作的地质灾害环境需求,主要体现在以下方面:①GNSS-RTK依赖于地面移动通信传输的基准站的差分信息,不利于机动较强、通信中断与缺失的地质灾害环境;②当前面向无人驾驶的多传感器融合定位方法主要针对城市等合作环境,需要依赖先验的地图信息,不适用于地质灾害等陌生、瞬变的环境条件;③基于惯性导航和激光雷达SLAM的融合定位研究,集中在小尺度的室内环境,融合的技术方法无法满足智能救援装备在广域作业环境下的连续性绝对定位需求。
发明内容
本发明针对复杂地质灾害等遮挡、广域作业环境下GNSS高精度定位失效、融合定位无法提供绝对位置及定位结果存在的中断等问题,提出一种复杂非合作场景多传感器融合绝对定位方法。
本发明是通过下述技术方案实现的:
本发明依据惯性导航连续、高频的测量信息,充分利用惯性导航的不间断性和高采样特征,构建惯性导航为媒介的GNSS PPP-RTK/INS、激光雷达分布式融合处理体系,设计惯性导航辅助的激光雷达实时畸变在线补偿和图优化相对位姿约束融合估计方案,最终实现智能救援装备无缝、可靠的高精度绝对定位。
一种适用于复杂非合作场景的多传感器融合绝对定位方法,具体包括以下步骤:
(1)以智能救援装备的GNSS伪距、载波相位观测数据和惯性导航测量数据为基础,采用GNSS PPP-RTK/INS紧组合方法,联合加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程,估计当前智能救援装备的角速度零值偏差、加速度零值偏差、位置、速度和姿态,然后,基于角速度零值偏差和加速度零值偏差对惯性导航测量数据进行补偿,获得零值偏差补偿后的惯性导航角速度和加速度信息;
(2)基于零值偏差补偿后的惯性导航角速度信息,采用二阶多项式,构建时间变化的角速度模型,基于该模型对激光雷达点云数据进行运动几何畸变补偿;然后对运动几何畸变补偿后的激光点云数据进行降采样和滤波处理,获取滤波后的激光点云数据;
(3)将滤波处理后的激光点云数据与前一时刻的关键帧采用快速ICP匹配方法进行点云匹配,获得激光点云的里程计信息和两帧之间的相对位姿,然后,基于智能救援装备速度、时间同步精度、零值偏差补偿后的惯性导航角速度和相对位姿信息构建阈值评价函数,进行关键帧的选择;
(4)基于智能救援装备激光测量的关键帧里程计信息,以零值偏差补偿后的惯性导航角速度和加速度信息作为基础观测信息,构建智能救援装备里程计相对位姿的约束性观测方程;
(5)采用图优化的松组合融合定位方法,采用GNSS/INS实时校准和优化的协方差矩阵信息,构造GNSS PPP-RTK/INS位置域的约束方程,然后,联合智能救援装备激光点云关键帧里程计信息和惯性导航的约束性测量方程,获得智能救援装备融合定位的结果。
其中,步骤(1)中所述的GNSS PPP-RTK/INS紧组合方法的***状态方程为:
其中,F表示***状态转移矩阵,G表示***噪声驱动阵,w表示***噪声向量,X和为***状态向量及其导数,δr表示位置误差,δtr表示接收机钟差偏差,表示载波相位整数偏差,δv表示速度误差,bg表示角速度零值偏差,ba表示加速度零值偏差,表示失准角误差,N表述载波相位模糊度参数;
GNSS PPP-RTK/INS紧组合方法的观测方程为:
其中,和分别代表智能救援装备所观测的GNSS卫星i的f频率的伪距和载波相位测量值,和分别代表智能救援装备所观测的GNSS i卫星f频率的伪距噪声和载波相位噪声,代表智能救援装备与GNSS卫星i间的几何距离,Ii和Ti分别代表智能救援装备所观测的GNSS卫星i的电离层和对流层延迟信息,δti和δt分别代表GNSS卫星i的卫星钟差和智能救援装备的接收机钟差,λf和分别代表载波相位波长和GNSS卫星i的载波相位整数模糊度,代表GNSS i卫星f频率的伪距码相位偏差,代表GNSS i卫星的整数相位偏差,r0和rs分别代表智能救援装备初始概略位置和GNSS卫星的位置矢量,C为光速。
其中,步骤(1)中所述的加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程为:
针对智能救援装备前进方向,基于加速度观测信息的约束方程如下:
针对智能救援装备在侧向和垂直方向经常处于非动态运动状态,依据其速度阈值,采用的约束条件如下:
其中:k为方向,ak为k方向的加速度,vk(t1)和vk(t0)分别为t0和t1时刻的运动速度,εk和vε分别为加速度和速度的测量误差阈值,yaw、roll和pitch分别代表惯性坐标系的偏航、横滚和俯仰测量方向,和分别代表加速度和角速度观测的噪声矩阵。
其中,所述步骤(2)具体包括以下步骤:
(201)基于零值偏差补偿后的惯性导航角速度信息,构建二阶多项式拟合时间变化的角速度模型,其公式如下:
ω(t1)=ω0+ω1(t1-t0)+ω2(t1-t0)2
其中,ω(t1)为t1时刻的零值偏差补偿后的惯性导航角速度,ω0、ω1和ω2分别为时间变化的角速度模型的拟合系数矢量,t0为二阶多项式拟合的参考时刻;
(202)基于上述时间变化的角速度模型,逐个对激光雷达点云数据Cloud(t1)中的i激光束的激光点云Cloud(i)进行运动几何畸变补偿,其补偿函数如下:
Clouds(i)=Cloud(i)*δτi*ω(t1)
Cloudt(i)=Clouds(i)+(θ+aδτi)*δτi
其中,τscan为激光雷达每周所需的旋转时间,i和Cloud(i)分别为激光测量的标识号i和激光点云数据,δτi为i束激光测量的旋转时间,yyi和xxi分别为i束激光测量的横坐标值和纵坐标值,θ和a分别为对应时刻的速度和加速度矢量信息,Clouds(i)为i束激光点云旋转补偿后的点云数据,Cloudt(i)为i束激光点云旋转和运动补偿后的点云数据;
基于上述的运动几何畸变补偿后的激光点云数据,进行降采样和滤波处理,获取滤波后的激光点云数据FilterCloud(t1)。
其中,所述步骤(3)具体包括以下步骤:
(301)将滤波处理后的激光点云数据FilterCloud(t1)与前一时刻的关键帧的点云KeyCloud(t1)进行点云匹配,匹配采用快速ICP匹配方法,获得激光点云的里程计信息Odom和两帧之间的相对位姿RelPos;
(302)基于智能救援装备速度θ、时间同步精度δτsyn和零值偏差补偿后的惯性导航角速度ω信息构建阈值评价函数,其评价函数如下:
Δr=θ*(δτsyn+1.0)
Δag=ω*(δτsyn+1.0)
其中,Δr为两帧的距离阈值,Δag为两帧的角度阈值;
(303)基于相对位姿RelPos获得前后两帧之间的相对距离矢量dr和相对旋转角度矢量dag,然后采用评价函数进行关键帧的选择,其准则如下:
其中,所述步骤(4)具体实现方式如下:
基于智能救援装备激光测量的关键帧里程计信息Odom,以零偏补偿后的惯性导航角速度ω和加速度信息a作为基础观测信息,构建的智能救援装备相对位姿的约束性观测方程,其方程如下:
a(t1-t0)-dθ=0.0Qa
dag-ω(t1-t0)=0.0Qw
其中,dag为基于两关键帧相对位姿获得的相对角度变化,dθ为两关键帧的测量时间t1和t0之间的速度变化量,Qa和Qw分别为加速度和角速度测量的协方差矩阵。
其中,所述步骤(5)具体实现方式如下:
(501)基于图优化的松组合融合定位技术,构造GNSS PPP-RTK/INS位置域的约束方程,其实现过程如下:
基于图顶点初始观测时刻的里程计Odomref和图顶点当前时刻的历程计信息Odom(t1)获得约束边条件,即两顶点之间的相对的位置变化量drodom,t1-ref,并基于GNSS/INS获得两顶点之间的相对位置drGNSS/INS,t1-ref,其方程如下:
drGNSS/INS,t1-ref=rGNSS/INS,t1-rGNSS/INS,ref
其中,rGNSS/INS,ref和rGNSS/INS,t1分别为初始观测的参考时刻和t1时刻基于GNSS/INS获得位置矢量;
基于上述的观测,构造约束方程如下:
drGNSS/INS,t1-ref=drodom,t1-refQdr
其中Qdr为约束性条件的方差矩阵,其约束条件依据GNSS/INS实时的估计状态进行校准和确定;
其中,Eye(3)为3维的单位矩阵,GNSS/INS的实时估计状态依据GNSS收敛状态分为固定解Fixed、浮点解Float和单点定位解SPP;
(502)基于步骤(501)的方程和约束条件,联合智能救援装备激光点云里程计Odom和智能救援装备相对位姿的约束性观测方程,估计获得智能救援装备融合定位的结果。
本发明与现有技术相比所取得的有益效果为:
本发明提出了一种适用于复杂非合作场景多传感器融合绝对定位方法,针对通信中断与缺失的地质灾害环境,设计GNSS PPP-RTK和惯性导航融合绝对定位方法,针对非合作、广域的地质灾害环境需求,构建惯性导航为媒介的GNSS PPP-RTK/INS、激光雷达分布式融合处理体系,解决遮挡环境下GNSS高精度绝对定位失效、广域动态场景激光雷达SLAM无法实现绝对定位等问题,设计惯性导航辅助的激光雷达实时畸变在线补偿和图优化相对位姿约束融合估计方案,提升智能救援装备无缝、高精度绝对定位的可靠性。
附图说明
图1为本发明定位传感器在智能救援挖掘机上的安装部署图。
具体实施方式
为了更好的说明本发明的目的和优点,下面结合附图1对本发明的技术方案作进一步说明。在本具体实施方式中,以智能救援挖掘机基于多传感器进行融合绝对定位为例进行方法说明。本发明中设备包括:GNSS天线、激光雷达设备、惯性导航设备、GNSS接收机和综合处理板,示意图如图1所示,GNSS接收机基于GNSS天线跟踪捕获得到载波相位测量和伪距测量数据,然后发送给综合处理板,同时,该处理板接收激光雷达测量的点云数据,联合实时接收惯性导航测量的角速度与加速度信息,进行多传感器融合绝对定位的处理,整个处理包括如下步骤:
(1)以智能救援装备的GNSS伪距、载波相位观测数据和惯性导航测量数据为基础,采用GNSS PPP-RTK/INS紧组合方法,联合加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程,估计当前智能救援装备的角速度零值偏差、加速度零值偏差、位置、速度和姿态,然后,基于角速度零值偏差和加速度零值偏差对惯性导航测量数据进行补偿,获得零值偏差补偿后的惯性导航角速度和加速度信息;
其中,所述的GNSS PPP-RTK/INS紧组合方法的***状态方程为:
其中,F表示***状态转移矩阵,G表示***噪声驱动阵,w表示***噪声向量,X和为***状态向量及其导数,δr表示位置误差,δtr表示接收机钟差偏差,表示载波相位整数偏差,δv表示速度误差,bg表示角速度零值偏差,ba表示加速度零值偏差,表示失准角误差,N表述载波相位模糊度参数;
GNSS PPP-RTK/INS紧组合方法的观测方程为:
其中,和分别代表智能救援装备所观测的GNSS卫星i的f频率的伪距和载波相位测量值,和分别代表智能救援装备所观测的GNSS i卫星f频率的伪距噪声和载波相位噪声,代表智能救援装备与GNSS卫星i间的几何距离,Ii和Ti分别代表智能救援装备所观测的GNSS卫星i的电离层和对流层延迟信息,δti和δt分别代表GNSS卫星i的卫星钟差和智能救援装备的接收机钟差,λf和分别代表载波相位波长和GNSS卫星i的载波相位整数模糊度,代表GNSS i卫星f频率的伪距码相位偏差,代表GNSS i卫星的整数相位偏差,r0和rs分别代表智能救援装备初始概略位置和GNSS卫星的位置矢量,C为光速。
所述的加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程为:
针对智能救援装备前进方向,基于加速度观测信息的约束方程如下:
针对智能救援装备在侧向和垂直方向经常处于非动态运动状态,依据其速度阈值,采用的约束条件如下:
其中:k为方向,ak为k方向的加速度,vk(t1)和vk(t0)分别为t0和t1时刻的运动速度,εk和vε分别为加速度和速度的测量误差阈值,yaw、roll和pitch分别代表惯性坐标系的偏航、横滚和俯仰测量方向,和分别代表加速度和角速度观测的噪声矩阵。
(2)基于零值偏差补偿后的惯性导航角速度信息,采用二阶多项式,构建时间变化的角速度模型,基于该模型对激光雷达点云数据进行运动几何畸变补偿;然后对运动几何畸变补偿后的激光点云数据进行降采样和滤波处理,获取滤波后的激光点云数据;
具体包括以下步骤:
(201)基于零值偏差补偿后的惯性导航角速度信息,构建二阶多项式拟合时间变化的角速度模型,其公式如下:
ω(t1)=ω0+ω1(t1-t0)+ω2(t1-t0)2
其中,ω(t1)为t1时刻的零值偏差补偿后的惯性导航角速度,ω0、ω1和ω2分别为时间变化的角速度模型的拟合系数矢量,t0为二阶多项式拟合的参考时刻;
(202)基于上述时间变化的角速度模型,逐个对激光雷达点云数据Cloud(t1)中的i激光束的激光点云Cloud(i)进行运动几何畸变补偿,其补偿函数如下:
Clouds(i)=Cloud(i)*δτi*ω(t1
Cloudt(i)=Clouds(i)+(θ+aδτi)*δτi
其中,τscan为激光雷达每周所需的旋转时间,i和Cloud(i)分别为激光测量的标识号i和激光点云数据,δτi为i束激光测量的旋转时间,yyi和xxi分别为i束激光测量的横坐标值和纵坐标值,θ和a分别为对应时刻的速度和加速度矢量信息,Clouds(i)为i束激光点云旋转补偿后的点云数据,Cloudt(i)为i束激光点云旋转和运动补偿后的点云数据;
基于上述的运动几何畸变补偿后的激光点云数据,进行降采样和滤波处理,获取滤波后的激光点云数据FilterCloud(t1)。
(3)将滤波处理后的激光点云数据与前一时刻的关键帧采用快速ICP匹配方法进行点云匹配,获得激光点云的里程计信息和两帧之间的相对位姿,然后,基于智能救援装备速度、时间同步精度、零值偏差补偿后的惯性导航角速度和相对位姿信息构建阈值评价函数,进行关键帧的选择;
具体包括以下步骤:
(301)将滤波处理后的激光点云数据FilterCloud(t1)与前一时刻的关键帧的点云KeyCloud(t1)进行点云匹配,匹配采用快速ICP匹配方法,获得激光点云的里程计信息Odom和两帧之间的相对位姿RelPos;
(302)基于智能救援装备速度θ、时间同步精度δτsyn和零值偏差补偿后的惯性导航角速度ω信息构建阈值评价函数,其评价函数如下:
Δr=θ*(δτsyn+1.0)
Δag=ω*(δτsyn+1.0)
其中,Δr为两帧的距离阈值,Δag为两帧的角度阈值;
(303)基于相对位姿RelPos获得前后两帧之间的相对距离矢量dr和相对旋转角度矢量dag,然后采用评价函数进行关键帧的选择,其准则如下:
(4)基于智能救援装备激光测量的关键帧里程计信息,以零值偏差补偿后的惯性导航角速度和加速度信息作为基础观测信息,构建智能救援装备里程计相对位姿的约束性观测方程;
具体实现方式如下:
基于智能救援装备激光测量的关键帧里程计信息Odom,以零偏补偿后的惯性导航角速度ω和加速度信息a作为基础观测信息,构建的智能救援装备相对位姿的约束性观测方程,其方程如下:
a(t1-t0)-dθ=0.0Qa
dag-ω(t1-t0)=0.0Qw
其中,dag为基于两关键帧相对位姿获得的相对角度变化,dθ为两关键帧的测量时间t1和t0之间的速度变化量,Qa和Qw分别为加速度和角速度测量的协方差矩阵。
(5)采用图优化的松组合融合定位方法,采用GNSS/INS实时校准和优化的协方差矩阵信息,构造GNSS PPP-RTK/INS位置域的约束方程,然后,联合智能救援装备激光点云关键帧里程计信息和惯性导航的约束性测量方程,获得智能救援装备融合定位的结果。
具体包括以下步骤:
(501)基于图优化的松组合融合定位技术,构造GNSS PPP-RTK/INS位置域的约束方程,其实现过程如下:
基于图顶点初始观测时刻的里程计Odomref和图顶点当前时刻的历程计信息Odom(t1)获得约束边条件,即两顶点之间的相对的位置变化量drodom,t1-ref,并基于GNSS/INS获得两顶点之间的相对位置drGNSS/INS,t1-ref,其方程如下:
drGNSS/INS,t1-ref=rGNSS/INS,t1-rGNSS/INS,ref
其中,rGNSS/INS,ref和rGNSS/INS,t1分别为初始观测的参考时刻和t1时刻基于GNSS/INS获得位置矢量;
基于上述的观测,构造约束方程如下:
drGNSS/INS,t1-ref=drodom,t1-refQdr
其中Qdr为约束性条件的方差矩阵,其约束条件依据GNSS/INS实时的估计状态进行校准和确定;
其中,Eye(3)为3维的单位矩阵,GNSS/INS的实时估计状态依据GNSS收敛状态分为固定解Fixed、浮点解Float和单点定位解SPP;
(502)基于步骤(501)的方程和约束条件,联合智能救援装备激光点云里程计Odom和智能救援装备相对位姿的约束性观测方程,估计获得智能救援装备融合定位的结果。
总之,本发明解决了复杂地质灾害等遮挡、广域作业环境下GNSS高精度定位失效、融合定位无法提供绝对位置及定位结果存在的中断等问题,大大提升了广域、非合作的复杂地质灾害环境下的高精度绝对定位的连续性和可靠性,对于复杂、陌生环境下的救援车辆和无人驾驶等车辆的导航控制,具有重要的工程实际应用价值。
Claims (7)
1.一种适用于复杂非合作场景的多传感器融合绝对定位方法,其特征在于,包括以下步骤:
(1)以智能救援装备的GNSS伪距、载波相位观测数据和惯性导航测量数据为基础,采用GNSS PPP-RTK/INS紧组合方法,联合加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程,估计当前智能救援装备的角速度零值偏差、加速度零值偏差、位置、速度和姿态,然后,基于角速度零值偏差和加速度零值偏差对惯性导航测量数据进行补偿,获得零值偏差补偿后的惯性导航角速度和加速度信息;
(2)基于零值偏差补偿后的惯性导航角速度信息,采用二阶多项式,构建时间变化的角速度模型,基于该模型对激光雷达点云数据进行运动几何畸变补偿;然后对运动几何畸变补偿后的激光点云数据进行降采样和滤波处理,获取滤波后的激光点云数据;
(3)将滤波处理后的激光点云数据与前一时刻的关键帧采用快速ICP匹配方法进行点云匹配,获得激光点云的里程计信息和两帧之间的相对位姿,然后,基于智能救援装备速度、时间同步精度、零值偏差补偿后的惯性导航角速度和相对位姿信息构建阈值评价函数,进行关键帧的选择;
(4)基于智能救援装备激光测量的关键帧里程计信息,以零值偏差补偿后的惯性导航角速度和加速度信息作为基础观测信息,构建智能救援装备里程计相对位姿的约束性观测方程;
(5)采用图优化的松组合融合定位方法,采用GNSS/INS实时校准和优化的协方差矩阵信息,构造GNSS PPP-RTK/INS位置域的约束方程,然后,联合智能救援装备激光点云关键帧里程计信息和惯性导航的约束性测量方程,获得智能救援装备融合定位的结果。
2.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法,其特征在于,步骤(1)中所述的GNSS PPP-RTK/INS紧组合方法的***状态方程为:
其中,F表示***状态转移矩阵,G表示***噪声驱动阵,w表示***噪声向量,X和为***状态向量及其导数,δr表示位置误差,δtr表示接收机钟差偏差,表示载波相位整数偏差,δv表示速度误差,bg表示角速度零值偏差,ba表示加速度零值偏差,表示失准角误差,N表述载波相位模糊度参数;
GNSS PPP-RTK/INS紧组合方法的观测方程为:
4.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法,其特征在于,所述步骤(2)具体包括以下步骤:
(201)基于零值偏差补偿后的惯性导航角速度信息,构建二阶多项式拟合时间变化的角速度模型,其公式如下:
ω(t1)=ω0+ω1(t1-t0)+ω2(t1-t0)2
其中,ω(t1)为t1时刻的零值偏差补偿后的惯性导航角速度,ω0、ω1和ω2分别为时间变化的角速度模型的拟合系数矢量,t0为二阶多项式拟合的参考时刻;
(202)基于上述时间变化的角速度模型,逐个对激光雷达点云数据Cloud(t1)中的i激光束的激光点云Cloud(i)进行运动几何畸变补偿,其补偿函数如下:
Clouds(i)=Cloud(i)*δτi*ω(t1)
Cloudt(i)=Clouds(i)+(θ+aδτi)*δτi
其中,τscan为激光雷达每周所需的旋转时间,i和Cloud(i)分别为激光测量的标识号i和激光点云数据,δτi为i束激光测量的旋转时间,yyi和xxi分别为i束激光测量的横坐标值和纵坐标值,θ和a分别为对应时刻的速度和加速度矢量信息,Clouds(i)为i束激光点云旋转补偿后的点云数据,Cloudt(i)为i束激光点云旋转和运动补偿后的点云数据;
基于上述的运动几何畸变补偿后的激光点云数据,进行降采样和滤波处理,获取滤波后的激光点云数据FilterCloud(t1)。
5.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法,其特征在于,所述步骤(3)具体包括以下步骤:
(301)将滤波处理后的激光点云数据FilterCloud(t1)与前一时刻的关键帧的点云KeyCloud(t1)进行点云匹配,匹配采用快速ICP匹配方法,获得激光点云的里程计信息Odom和两帧之间的相对位姿RelPos;
(302)基于智能救援装备速度θ、时间同步精度δτsyn和零值偏差补偿后的惯性导航角速度ω信息构建阈值评价函数,其评价函数如下:
Δr=θ*(δτsyn+1.0)
Δag=ω*(δτsyn+1.0)
其中,Δr为两帧的距离阈值,Δag为两帧的角度阈值;
(303)基于相对位姿RelPos获得前后两帧之间的相对距离矢量dr和相对旋转角度矢量dag,然后采用评价函数进行关键帧的选择,其准则如下:
6.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法,其特征在于,所述步骤(4)具体实现方式如下:
基于智能救援装备激光测量的关键帧里程计信息Odom,以零偏补偿后的惯性导航角速度ω和加速度信息a作为基础观测信息,构建的智能救援装备相对位姿的约束性观测方程,其方程如下:
a(t1-t0)-dθ=0.0 Qa
dag-ω(t1-t0)=0.0 Qw
其中,dag为基于两关键帧相对位姿获得的相对角度变化,dθ为两关键帧的测量时间t1和t0之间的速度变化量,Qa和Qw分别为加速度和角速度测量的协方差矩阵。
7.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法,其特征在于,所述步骤(5)具体包括以下步骤:
(501)基于图优化的松组合融合定位技术,构造GNSS PPP-RTK/INS位置域的约束方程,其实现过程如下:
基于图顶点初始观测时刻的里程计Odomref和图顶点当前时刻的历程计信息Odom(t1)获得约束边条件,即两顶点之间的相对的位置变化量drodom,t1-ref,并基于GNSS/INS获得两顶点之间的相对位置drGNSS/INS,t1-ref,其方程如下:
drGNSS/INS,t1-ref=rGNSS/INS,t1-rGNSS/INS,ref
其中,rGNSS/INS,ref和rGNSS/INS,t1分别为初始观测的参考时刻和t1时刻基于GNSS/INS获得位置矢量;
基于上述的观测,构造约束方程如下:
drGNSS/INS,t1-ref=drodom,t1-refQdr
其中Qdr为约束性条件的方差矩阵,其约束条件依据GNSS/INS实时的估计状态进行校准和确定;
其中,Eye(3)为3维的单位矩阵,GNSS/INS的实时估计状态依据GNSS收敛状态分为固定解Fixed、浮点解Float和单点定位解SPP;
(502)基于步骤(501)的方程和约束条件,联合智能救援装备激光点云里程计Odom和智能救援装备相对位姿的约束性观测方程,估计获得智能救援装备融合定位的结果。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110841655.1A CN113534227B (zh) | 2021-07-26 | 2021-07-26 | 一种适用于复杂非合作场景的多传感器融合绝对定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN202110841655.1A CN113534227B (zh) | 2021-07-26 | 2021-07-26 | 一种适用于复杂非合作场景的多传感器融合绝对定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN113534227A CN113534227A (zh) | 2021-10-22 |
CN113534227B true CN113534227B (zh) | 2022-07-01 |
Family
ID=78120732
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN202110841655.1A Active CN113534227B (zh) | 2021-07-26 | 2021-07-26 | 一种适用于复杂非合作场景的多传感器融合绝对定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN113534227B (zh) |
Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008083777A (ja) * | 2006-09-26 | 2008-04-10 | Tamagawa Seiki Co Ltd | 無人搬送車の誘導方法及び装置 |
CN101344391A (zh) * | 2008-07-18 | 2009-01-14 | 北京工业大学 | 基于全功能太阳罗盘的月球车位姿自主确定方法 |
WO2018219361A1 (zh) * | 2017-05-31 | 2018-12-06 | 中国矿业大学 | 一种采煤机绝对位姿检测方法 |
CN109946732A (zh) * | 2019-03-18 | 2019-06-28 | 李子月 | 一种基于多传感器数据融合的无人车定位方法 |
CN110687559A (zh) * | 2019-11-04 | 2020-01-14 | 中国电子科技集团公司第五十四研究所 | 适用于机载的gnss无缝高精度定位和完好性评估方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN111288984A (zh) * | 2020-03-04 | 2020-06-16 | 武汉大学 | 一种基于车联网的多车联合绝对定位的方法 |
CN112362051A (zh) * | 2020-10-16 | 2021-02-12 | 无锡卡尔曼导航技术有限公司 | 一种基于gnss/ins/lidar-slam信息融合的移动机器人导航定位*** |
CN112950781A (zh) * | 2021-03-19 | 2021-06-11 | 中山大学 | 特种场景的多传感器动态加权融合的点云地图构建方法 |
CN112987065A (zh) * | 2021-02-04 | 2021-06-18 | 东南大学 | 一种融合多传感器的手持式slam装置及其控制方法 |
CN112985386A (zh) * | 2021-01-26 | 2021-06-18 | 浙江吉利控股集团有限公司 | 一种自动驾驶多源融合定位方法、装置、设备及存储介质 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN108732603B (zh) * | 2017-04-17 | 2020-07-10 | 百度在线网络技术(北京)有限公司 | 用于定位车辆的方法和装置 |
WO2018214978A1 (zh) * | 2017-05-26 | 2018-11-29 | 苏州宝时得电动工具有限公司 | 定位装置及方法以及自动行走设备 |
-
2021
- 2021-07-26 CN CN202110841655.1A patent/CN113534227B/zh active Active
Patent Citations (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
JP2008083777A (ja) * | 2006-09-26 | 2008-04-10 | Tamagawa Seiki Co Ltd | 無人搬送車の誘導方法及び装置 |
CN101344391A (zh) * | 2008-07-18 | 2009-01-14 | 北京工业大学 | 基于全功能太阳罗盘的月球车位姿自主确定方法 |
WO2018219361A1 (zh) * | 2017-05-31 | 2018-12-06 | 中国矿业大学 | 一种采煤机绝对位姿检测方法 |
WO2020087846A1 (zh) * | 2018-10-31 | 2020-05-07 | 东南大学 | 基于迭代扩展卡尔曼滤波融合惯性与单目视觉的导航方法 |
CN109946732A (zh) * | 2019-03-18 | 2019-06-28 | 李子月 | 一种基于多传感器数据融合的无人车定位方法 |
CN110687559A (zh) * | 2019-11-04 | 2020-01-14 | 中国电子科技集团公司第五十四研究所 | 适用于机载的gnss无缝高精度定位和完好性评估方法 |
CN111288984A (zh) * | 2020-03-04 | 2020-06-16 | 武汉大学 | 一种基于车联网的多车联合绝对定位的方法 |
CN112362051A (zh) * | 2020-10-16 | 2021-02-12 | 无锡卡尔曼导航技术有限公司 | 一种基于gnss/ins/lidar-slam信息融合的移动机器人导航定位*** |
CN112985386A (zh) * | 2021-01-26 | 2021-06-18 | 浙江吉利控股集团有限公司 | 一种自动驾驶多源融合定位方法、装置、设备及存储介质 |
CN112987065A (zh) * | 2021-02-04 | 2021-06-18 | 东南大学 | 一种融合多传感器的手持式slam装置及其控制方法 |
CN112950781A (zh) * | 2021-03-19 | 2021-06-11 | 中山大学 | 特种场景的多传感器动态加权融合的点云地图构建方法 |
Non-Patent Citations (8)
Title |
---|
《Performance Enhancement of INS/GNSS/Refreshed-SLAM Integration for Acceptable Lane-Level Navigation Accuracy》;Kai-Wei Chiang etal.;《IEEE Transactions on Vehicular Technology》;20200115;2463-2476 * |
《Precise Point Positioning Algorithm for Pseudolite Combined with GNSS in a Constrained Observation Environment》;Chuanzhen Sheng,Xingli Gan,Baoguo Yu;《Sensors》;20200218;1-16 * |
《Robust and Precise Vehicle Localization Based on Multi-Sensor Fusion in Diverse City Scenes》;G Wan et al.;《2018 IEEE International Conference on Robotics and Automation (ICRA)》;20180525;4670-4677 * |
《一种多线激光雷达与GNSS/INS***标定方法》;叶珏磊 , 周志峰 , 王立端 , 庞正雅;《激光与红外》;20200322;30-36 * |
《高精度组合导航及其测绘车应用技术》;马国驹;蔚保国;智奇楠;《测绘科学》;20151220;144-148+152 * |
基于三维激光雷达与RTK融合的SLAM研究;倪志康等;《制造业自动化》;20200725(第07期);全文 * |
张少将.《基于多传感器信息融合的智能车定位导航***研究》.《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》.2021, * |
杨立财.《GNSS/INS融合定位滤波方法研究及应用》.《中国优秀硕士学位论文全文数据库 信息科技辑》.2021, * |
Also Published As
Publication number | Publication date |
---|---|
CN113534227A (zh) | 2021-10-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN106443746B (zh) | 一种低成本双天线gnss/ahrs组合测姿方法 | |
US9784582B2 (en) | Method and apparatus for navigation with nonlinear models | |
CN102508275B (zh) | 多天线gps/gf-ins深度组合定姿方法 | |
CN110779521A (zh) | 一种多源融合的高精度定位方法与装置 | |
CA2802445C (en) | Moving platform ins range corrector (mpirc) | |
CN111102978A (zh) | 一种车辆运动状态确定的方法、装置及电子设备 | |
CN113203418B (zh) | 基于序贯卡尔曼滤波的gnssins视觉融合定位方法及*** | |
Konrad et al. | Advanced state estimation for navigation of automated vehicles | |
CN109506660B (zh) | 一种用于仿生导航的姿态最优化解算方法 | |
CN111024074B (zh) | 一种基于递推最小二乘参数辨识的惯导速度误差确定方法 | |
CN110702091A (zh) | 一种沿地铁轨道移动机器人的高精度定位方法 | |
WO2020124624A1 (zh) | 一种紧耦合的自动驾驶感知方法及*** | |
CN115327588A (zh) | 一种基于网络rtk的无人自动化作业特种车高精度定位方法 | |
CN105910623B (zh) | 利用磁强计辅助gnss/mins紧组合***进行航向校正的方法 | |
CN108151765B (zh) | 一种在线实时估计补偿磁强计误差的定位测姿方法 | |
Li et al. | Continuous and precise positioning in urban environments by tightly coupled integration of GNSS, INS and vision | |
CN115683094A (zh) | 一种复杂环境下车载双天线紧耦合定位方法及*** | |
Wen et al. | 3D LiDAR aided GNSS real-time kinematic positioning | |
Qian et al. | A LiDAR aiding ambiguity resolution method using fuzzy one-to-many feature matching | |
Xiang et al. | A SINS/GNSS/2D-LDV integrated navigation scheme for unmanned ground vehicles | |
CN108205151B (zh) | 一种低成本gps单天线姿态测量方法 | |
Chiang et al. | Performance of LiDAR-SLAM-based PNT with initial poses based on NDT scan matching algorithm | |
Nebot | Sensors used for autonomous navigation | |
CN113534227B (zh) | 一种适用于复杂非合作场景的多传感器融合绝对定位方法 | |
CN115523920B (zh) | 一种基于视觉惯性gnss紧耦合的无缝定位方法 |
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 |