1.一种激光点云匹配的实时位姿校正方法,其特征在于,包括:步骤1:确定导航定位系统失效前的观测历元和失效后的观测历元,获取不同历元下激光移动测量系统的空间位置和姿态信息;
步骤2:根据导航定位系统失效前后的相邻观测历元,通过点云匹配选择激光点云特征点;
步骤3:利用激光点云特征点,在统一的坐标系统下构建空间位置和姿态信息校正关系,采用最小二乘非线性求解得到改正后的空间位置和姿态信息;
步骤4:将改正后的空间位置和姿态信息重新用于失效后一观测历元的激光点云位置计算,重复激光移动测量位姿校正直至导航定位系统恢复准确定位。
2.根据权利要求1所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤1包括:步骤1.1:确定导航定位系统失效的前一观测历元 ,获取历元 下激光移动测量系统的空间位置 和姿态信息 ;
步骤1.2:确定导航定位系统失效后的观测历元,获取历元 下激光移动测量系统的空间位置 和姿态信息 , 为经度、 为纬度、 为高度、为俯仰角、 为侧滚角、 为航偏角,为导航定位系统失效的观测历元。
3.根据权利要求2所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤2包括:步骤2.1:获取导航定位系统失效的前一观测历元 的激光移动测量系统的点云数据集 ,导航定位系统失效后的观测历元 下的激光移动测量系统的点云数据集 ;
步骤2.2:采用点云匹配方法,确定观测历元 下的点云数据集 与观测历元下的点云数据集 的同名特征点集 。
4.根据权利要求3所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤3包括:步骤3.1:根据激光雷达扫描仪坐标系、载体坐标系、当地水平坐标系以及地心地固坐标间的转换关系,构建相邻观测历元间同名特征点集 的空间位置关系:;
式中, 为地心地固坐标系特征点集坐标, 为站心点的地心地固坐标, 为当地水平坐标系特征点集坐标, 为载体坐标系特征点集坐标, 为激光雷达扫描仪坐标系特征点集坐标,为经纬度相关的旋转矩阵,为姿态相关的旋转矩阵, 为安装校准相关的旋转矩阵,为安装校准平移量,和 均通过安装校准获得。
5.根据权利要求4所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤3包括:步骤3.2:利用特征点集激光雷达扫描仪坐标系、地心地固坐标系间的空间位置关系,将观测历元下的激光移动测量系统的经度 、纬度 、高度 、俯仰角 、侧滚角、航偏角 作为未知参数 ,构建位置和姿态改正方程:;
式中, 为未知参数初值, , 为改正前经度、 为改正前纬度、 为改正前高度、 为改正前俯仰角、 为改正前侧滚角、 为改正前航偏角。
6.根据权利要求5所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤3包括:步骤3.3:采用最小二乘间接平差方法对位置和姿态改正方程进行非线性求解:位置和姿态改正方程是理想状态下的关系式,实际情况中未知参数 和之间存在残差向量L:; ;
式中,为线性化后的系数矩阵,也是雅可比矩阵,为线性化后的观测向量,也是残差向量,表示每个数据点的残差。
7.根据权利要求6所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤
3.1包括:
;
。
8.根据权利要求7所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤
3.2包括:位置和姿态改正方程具体为:;
;
;
式中, , , 为地心地固坐标系下坐标, , , 为载体坐标系下坐标, , , 为站心点的地心地固坐标系下坐标。
9.根据权利要求8所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤
3.2包括:
利用参考椭球的相关参数将站心点坐标转换为用经度 、纬度 、高程 进行表示,统一将 , , , , , 作为未知参数进行求解,降低B矩阵的复杂程度:;
;
;
;
其中, ,为椭球的第二偏心率,a为椭球的长半径,b为椭球的短半径。
10.根据权利要求9所述的一种激光点云匹配的实时位姿校正方法,其特征在于,步骤4包括:步骤4.1:利用改正后观测历元 的经度 、纬度 、高度 、俯仰角 、侧滚角 、航偏角 重新用于失效后一观测历元的激光点云位置计算,得到改正后的点云数据集 ;
步骤4.2:对观测历元 和 重复步骤2、步骤3,直至激光移动测量系统的导航定位系统恢复。