CN113534227A - 一种适用于复杂非合作场景的多传感器融合绝对定位方法 - Google Patents

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表示位置误差，δt_r表示接收机钟差偏差，

表示载波相位整数偏差，δv表示速度误差，b_g表示角速度零值偏差，b_a表示加速度零值偏差，

表示失准角误差，N表述载波相位模糊度参数；

GNSS PPP-RTK/INS紧组合方法的观测方程为：

其中，

和

分别代表智能救援装备所观测的GNSS卫星i的f频率的伪距和载波相位测量值，

和

分别代表智能救援装备所观测的GNSS i卫星f频率的伪距噪声和载波相位噪声，

代表智能救援装备与GNSS卫星i间的几何距离，Iⁱ和Tⁱ分别代表智能救援装备所观测的GNSS卫星i的电离层和对流层延迟信息，δtⁱ和δt分别代表GNSS卫星i的卫星钟差和智能救援装备的接收机钟差，λ_f和

分别代表载波相位波长和GNSS卫星i的载波相位整数模糊度，

代表GNSS i卫星f频率的伪距码相位偏差，

代表GNSS i卫星的整数相位偏差，r₀和r_s分别代表智能救援装备初始概略位置和GNSS卫星的位置矢量，C为光速。

其中，步骤(1)中所述的加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程为：

针对智能救援装备前进方向，基于加速度观测信息的约束方程如下：

针对智能救援装备在侧向和垂直方向经常处于非动态运动状态，依据其速度阈值，采用的约束条件如下：

其中：k为方向，a_k为k方向的加速度，v_k(t₁)和v_k(t₀)分别为t₀和t₁时刻的运动速度，ε_k和v_ε分别为加速度和速度的测量误差阈值，yaw、roll和pitch分别代表惯性坐标系的偏航、横滚和俯仰测量方向，

和

分别代表加速度和角速度观测的噪声矩阵。

其中，所述步骤(2)具体包括以下步骤：

(201)基于零值偏差补偿后的惯性导航角速度信息，构建二阶多项式拟合时间变化的角速度模型，其公式如下：

ω(t₁)＝ω₀+ω₁(t₁-t₀)+ω₂(t₁-t₀)²

其中，ω(t₁)为t₁时刻的零值偏差补偿后的惯性导航角速度，ω₀、ω₁和ω₂分别为时间变化的角速度模型的拟合系数矢量，t₀为二阶多项式拟合的参考时刻；

(202)基于上述时间变化的角速度模型，逐个对激光雷达点云数据Cloud(t₁)中的i激光束的激光点云Cloud(i)进行运动几何畸变补偿，其补偿函数如下：

Clouds(i)＝Cloud(i)*δτ_i*ω(t₁)

Cloudt(i)＝Clouds(i)+(θ+aδτ_i)*δτ_i

其中，τ_scan为激光雷达每周所需的旋转时间，i和Cloud(i)分别为激光测量的标识号i和激光点云数据，δτ_i为i束激光测量的旋转时间，yy_i和xx_i分别为i束激光测量的横坐标值和纵坐标值，θ和a分别为对应时刻的速度和加速度矢量信息，Clouds(i)为i束激光点云旋转补偿后的点云数据，Cloudt(i)为i束激光点云旋转和运动补偿后的点云数据；

基于上述的运动几何畸变补偿后的激光点云数据，进行降采样和滤波处理，获取滤波后的激光点云数据FilterCloud(t₁)。

其中，所述步骤(3)具体包括以下步骤：

(301)将滤波处理后的激光点云数据FilterCloud(t₁)与前一时刻的关键帧的点云KeyCloud(t₁)进行点云匹配，匹配采用快速ICP匹配方法，获得激光点云的里程计信息Odom和两帧之间的相对位姿RelPos；

(302)基于智能救援装备速度θ、时间同步精度δτ_syn和零值偏差补偿后的惯性导航角速度ω信息构建阈值评价函数，其评价函数如下：

Δr＝θ*(δτ_syn+1.0)

Δag＝ω*(δτ_syn+1.0)

其中，Δr为两帧的距离阈值，Δag为两帧的角度阈值；

(303)基于相对位姿RelPos获得前后两帧之间的相对距离矢量dr和相对旋转角度矢量dag，然后采用评价函数进行关键帧的选择，其准则如下：

其中，所述步骤(4)具体实现方式如下：

基于智能救援装备激光测量的关键帧里程计信息Odom，以零偏补偿后的惯性导航角速度ω和加速度信息a作为基础观测信息，构建的智能救援装备相对位姿的约束性观测方程，其方程如下：

a(t₁-t₀)-dθ＝0.0Q_a

dag-ω(t₁-t₀)＝0.0Q_w

其中，dag为基于两关键帧相对位姿获得的相对角度变化，dθ为两关键帧的测量时间t₁和t₀之间的速度变化量，Q_a和Q_w分别为加速度和角速度测量的协方差矩阵。

其中，所述步骤(5)具体实现方式如下：

(501)基于图优化的松组合融合定位技术，构造GNSS PPP-RTK/INS位置域的约束方程，其实现过程如下：

基于图顶点初始观测时刻的里程计Odom_ref和图顶点当前时刻的历程计信息Odom(t₁)获得约束边条件，即两顶点之间的相对的位置变化量dr_{odom，t1-ref}，并基于GNSS/INS获得两顶点之间的相对位置dr_{GNSS/INS，t1-ref}，其方程如下：

dr_{GNSS/INS，t1-ref}＝r_{GNSS/INS，t1}-r_{GNSS/INS，ref}

其中，r_{GNSS/INS，ref}和r_{GNSS/INS，t1}分别为初始观测的参考时刻和t1时刻基于GNSS/INS获得位置矢量；

基于上述的观测，构造约束方程如下：

dr_{GNSS/INS，t1-ref}＝dr_{odom，t1-ref}Q_dr

其中Q_dr为约束性条件的方差矩阵，其约束条件依据GNSS/INS实时的估计状态进行校准和确定；

上述的Q_dr依赖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表示位置误差，δt_r表示接收机钟差偏差，

表示载波相位整数偏差，δv表示速度误差，b_g表示角速度零值偏差，b_a表示加速度零值偏差，

表示失准角误差，N表述载波相位模糊度参数；

GNSS PPP-RTK/INS紧组合方法的观测方程为：

其中，

和

分别代表智能救援装备所观测的GNSS卫星i的f频率的伪距和载波相位测量值，

和

分别代表智能救援装备所观测的GNSS i卫星f频率的伪距噪声和载波相位噪声，

代表智能救援装备与GNSS卫星i间的几何距离，Iⁱ和Tⁱ分别代表智能救援装备所观测的GNSS卫星i的电离层和对流层延迟信息，δtⁱ和δt分别代表GNSS卫星i的卫星钟差和智能救援装备的接收机钟差，λ_f和

分别代表载波相位波长和GNSS卫星i的载波相位整数模糊度，

代表GNSS i卫星f频率的伪距码相位偏差，

代表GNSS i卫星的整数相位偏差，r₀和r_s分别代表智能救援装备初始概略位置和GNSS卫星的位置矢量，C为光速。

所述的加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程为：

针对智能救援装备前进方向，基于加速度观测信息的约束方程如下：

针对智能救援装备在侧向和垂直方向经常处于非动态运动状态，依据其速度阈值，采用的约束条件如下：

其中：k为方向，a_k为k方向的加速度，v_k(t₁)和v_k(t₀)分别为t₀和t₁时刻的运动速度，ε_k和v_ε分别为加速度和速度的测量误差阈值，yaw、roll和pitch分别代表惯性坐标系的偏航、横滚和俯仰测量方向，

和

分别代表加速度和角速度观测的噪声矩阵。

(2)基于零值偏差补偿后的惯性导航角速度信息，采用二阶多项式，构建时间变化的角速度模型，基于该模型对激光雷达点云数据进行运动几何畸变补偿；然后对运动几何畸变补偿后的激光点云数据进行降采样和滤波处理，获取滤波后的激光点云数据；

具体包括以下步骤：

(201)基于零值偏差补偿后的惯性导航角速度信息，构建二阶多项式拟合时间变化的角速度模型，其公式如下：

ω(t₁)＝ω₀+ω₁(t₁-t₀)+ω₂(t₁-t₀)²

其中，ω(t₁)为t₁时刻的零值偏差补偿后的惯性导航角速度，ω₀、ω₁和ω₂分别为时间变化的角速度模型的拟合系数矢量，t₀为二阶多项式拟合的参考时刻；

(202)基于上述时间变化的角速度模型，逐个对激光雷达点云数据Cloud(t₁)中的i激光束的激光点云Cloud(i)进行运动几何畸变补偿，其补偿函数如下：

Clouds(i)＝Cloud(i)*δτ_i*ω(t₁

Cloudt(i)＝Clouds(i)+(θ+aδτ_i)*δτ_i

其中，τ_scan为激光雷达每周所需的旋转时间，i和Cloud(i)分别为激光测量的标识号i和激光点云数据，δτ_i为i束激光测量的旋转时间，yy_i和xx_i分别为i束激光测量的横坐标值和纵坐标值，θ和a分别为对应时刻的速度和加速度矢量信息，Clouds(i)为i束激光点云旋转补偿后的点云数据，Cloudt(i)为i束激光点云旋转和运动补偿后的点云数据；

基于上述的运动几何畸变补偿后的激光点云数据，进行降采样和滤波处理，获取滤波后的激光点云数据FilterCloud(t₁)。

(3)将滤波处理后的激光点云数据与前一时刻的关键帧采用快速ICP匹配方法进行点云匹配，获得激光点云的里程计信息和两帧之间的相对位姿，然后，基于智能救援装备速度、时间同步精度、零值偏差补偿后的惯性导航角速度和相对位姿信息构建阈值评价函数，进行关键帧的选择；

具体包括以下步骤：

(301)将滤波处理后的激光点云数据FilterCloud(t₁)与前一时刻的关键帧的点云KeyCloud(t₁)进行点云匹配，匹配采用快速ICP匹配方法，获得激光点云的里程计信息Odom和两帧之间的相对位姿RelPos；

(302)基于智能救援装备速度θ、时间同步精度δτ_syn和零值偏差补偿后的惯性导航角速度ω信息构建阈值评价函数，其评价函数如下：

Δr＝θ*(δτ_syn+1.0)

Δag＝ω*(δτ_syn+1.0)

其中，Δr为两帧的距离阈值，Δag为两帧的角度阈值；

(303)基于相对位姿RelPos获得前后两帧之间的相对距离矢量dr和相对旋转角度矢量dag，然后采用评价函数进行关键帧的选择，其准则如下：

(4)基于智能救援装备激光测量的关键帧里程计信息，以零值偏差补偿后的惯性导航角速度和加速度信息作为基础观测信息，构建智能救援装备里程计相对位姿的约束性观测方程；

具体实现方式如下：

基于智能救援装备激光测量的关键帧里程计信息Odom，以零偏补偿后的惯性导航角速度ω和加速度信息a作为基础观测信息，构建的智能救援装备相对位姿的约束性观测方程，其方程如下：

a(t₁-t₀)-dθ＝0.0Q_a

dag-ω(t₁-t₀)＝0.0Q_w

其中，dag为基于两关键帧相对位姿获得的相对角度变化，dθ为两关键帧的测量时间t₁和t₀之间的速度变化量，Q_a和Q_w分别为加速度和角速度测量的协方差矩阵。

(5)采用图优化的松组合融合定位方法，采用GNSS/INS实时校准和优化的协方差矩阵信息，构造GNSS PPP-RTK/INS位置域的约束方程，然后，联合智能救援装备激光点云关键帧里程计信息和惯性导航的约束性测量方程，获得智能救援装备融合定位的结果。

具体包括以下步骤：

(501)基于图优化的松组合融合定位技术，构造GNSS PPP-RTK/INS位置域的约束方程，其实现过程如下：

基于图顶点初始观测时刻的里程计Odom_ref和图顶点当前时刻的历程计信息Odom(t₁)获得约束边条件，即两顶点之间的相对的位置变化量dr_{odom，t1-ref}，并基于GNSS/INS获得两顶点之间的相对位置dr_{GNSS/INS，t1-ref}，其方程如下：

dr_{GNSS/INS，t1-ref}＝r_{GNSS/INS，t1}-r_{GNSS/INS，ref}

其中，r_{GNSS/INS，ref}和r_{GNSS/INS，t1}分别为初始观测的参考时刻和t1时刻基于GNSS/INS获得位置矢量；

基于上述的观测，构造约束方程如下：

dr_{GNSS/INS,t1-ref}＝dr_{odom，t1-ref}Q_dr

其中Q_dr为约束性条件的方差矩阵，其约束条件依据GNSS/INS实时的估计状态进行校准和确定；

上述的Q_dr依赖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表示位置误差，δt_r表示接收机钟差偏差，

表示载波相位整数偏差，δv表示速度误差，b_g表示角速度零值偏差，b_a表示加速度零值偏差，

表示失准角误差，N表述载波相位模糊度参数；

GNSS PPP-RTK/INS紧组合方法的观测方程为：

其中，

和

分别代表智能救援装备所观测的GNSS卫星i的f频率的伪距和载波相位测量值，

和

分别代表智能救援装备所观测的GNSS i卫星f频率的伪距噪声和载波相位噪声，

代表智能救援装备与GNSS卫星i间的几何距离，Iⁱ和Tⁱ分别代表智能救援装备所观测的GNSS卫星i的电离层和对流层延迟信息，δtⁱ和δt分别代表GNSS卫星i的卫星钟差和智能救援装备的接收机钟差，λ_f和

分别代表载波相位波长和GNSS卫星i的载波相位整数模糊度，

代表GNSS i卫星f频率的伪距码相位偏差，

代表GNSS i卫星的整数相位偏差，r₀和r_s分别代表智能救援装备初始概略位置和GNSS卫星的位置矢量，C为光速。

3.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法，其特征在于，步骤(1)中所述的加速度先验信息和智能救援装备运动特征约束的PPP-RTK快速收敛方程为：

针对智能救援装备前进方向，基于加速度观测信息的约束方程如下：

针对智能救援装备在侧向和垂直方向经常处于非动态运动状态，依据其速度阈值，采用的约束条件如下：

其中：k为方向，a_k为k方向的加速度，v_k(t₁)和v_k(t₀)分别为t₀和t₁时刻的运动速度，ε_k和v_ε分别为加速度和速度的测量误差阈值，yaw、roll和pitch分别代表惯性坐标系的偏航、横滚和俯仰测量方向，

和

分别代表加速度和角速度观测的噪声矩阵。

4.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法，其特征在于，所述步骤(2)具体包括以下步骤：

(201)基于零值偏差补偿后的惯性导航角速度信息，构建二阶多项式拟合时间变化的角速度模型，其公式如下：

ω(t₁)＝ω₀+ω₁(t₁-t₀)+ω₂(t₁-t₀)²

其中，ω(t₁)为t₁时刻的零值偏差补偿后的惯性导航角速度，ω₀、ω₁和ω₂分别为时间变化的角速度模型的拟合系数矢量，t₀为二阶多项式拟合的参考时刻；

(202)基于上述时间变化的角速度模型，逐个对激光雷达点云数据Cloud(t₁)中的i激光束的激光点云Cloud(i)进行运动几何畸变补偿，其补偿函数如下：

Clouds(i)＝Cloud(i)*δτ_i*ω(t₁)

Cloudt(i)＝Clouds(i)+(θ+aδτ_i)*δτ_i

其中，τ_scan为激光雷达每周所需的旋转时间，i和Cloud(i)分别为激光测量的标识号i和激光点云数据，δτ_i为i束激光测量的旋转时间，yy_i和xx_i分别为i束激光测量的横坐标值和纵坐标值，θ和a分别为对应时刻的速度和加速度矢量信息，Clouds(i)为i束激光点云旋转补偿后的点云数据，Cloudt(i)为i束激光点云旋转和运动补偿后的点云数据；

基于上述的运动几何畸变补偿后的激光点云数据，进行降采样和滤波处理，获取滤波后的激光点云数据FilterCloud(t₁)。

5.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法，其特征在于，所述步骤(3)具体包括以下步骤：

(301)将滤波处理后的激光点云数据FilterCloud(t₁)与前一时刻的关键帧的点云KeyCloud(t₁)进行点云匹配，匹配采用快速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(t₁-t₀)-dθ＝0.0 Q_a

dag-ω(t₁-t₀)＝0.0 Q_w

其中，dag为基于两关键帧相对位姿获得的相对角度变化，dθ为两关键帧的测量时间t₁和t₀之间的速度变化量，Q_a和Q_w分别为加速度和角速度测量的协方差矩阵。

7.根据权利要求1所述的一种适用于复杂非合作场景的多传感器融合绝对定位方法，其特征在于，所述步骤(5)具体包括以下步骤：

(501)基于图优化的松组合融合定位技术，构造GNSS PPP-RTK/INS位置域的约束方程，其实现过程如下：

基于图顶点初始观测时刻的里程计Odom_ref和图顶点当前时刻的历程计信息Odom(t₁)获得约束边条件，即两顶点之间的相对的位置变化量dr_{odom，t1-ref}，并基于GNSS/INS获得两顶点之间的相对位置dr_{GNSS/INS，t1-ref}，其方程如下：

dr_{GNSS/INS，t1-ref}＝r_{GNSS/INS，t1}-r_{GNSS/INS，ref}

其中，r_{GNSS/INS，ref}和r_{GNSS/INS，t1}分别为初始观测的参考时刻和t1时刻基于GNSS/INS获得位置矢量；

基于上述的观测，构造约束方程如下：

dr_{GNSS/INS，t1-ref}＝dr_{odom，t1-ref}Q_dr

其中Q_dr为约束性条件的方差矩阵，其约束条件依据GNSS/INS实时的估计状态进行校准和确定；

上述的Q_dr依赖GNSS/INS实时的估计状态

进行校准和确定，其确定的准则如下：

其中，Eye(3)为3维的单位矩阵，GNSS/INS的实时估计状态依据GNSS收敛状态分为固定解Fixed、浮点解Float和单点定位解SPP；

(502)基于步骤(501)的方程和约束条件，联合智能救援装备激光点云里程计Odom和智能救援装备相对位姿的约束性观测方程，估计获得智能救援装备融合定位的结果。

Images