[发明专利]一种基于多源先验信息的点云匹配高精度定位方法

权利要求书

1.一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，具体步骤如下：

步骤S1，通过安装于智能平台上的IMU、RTK-GPS设备以及激光雷达，在t时刻同步采集IMU数据、GNSS数据和点云数据，通过IMU数据和GNSS数据获得智能平台t时刻的GNSS全局位姿，通过点云数据获得点云观测数据；

步骤S2，根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图S_i，i＝1，2，3...；

步骤S3，根据步骤S2创建的各个三维先验体素子地图的拓扑连通关系建立全局拓扑关系，得到智能平台运行区域的全局先验地图，该全局先验地图保存有各个三维先验体素子地图的N帧多源先验信息；

步骤S4，将步骤S1的GNSS数据与全局先验地图的多源先验信息进行初始数据匹配，得到智能平台在某一三维先验体素子地图S_j中的相对位姿；

步骤S5，智能平台运动过程中不断利用当前点云观测数据与三维先验体素子地图S_j进行数据匹配，得到智能平台的局部位姿，所述局部位姿为智能平台在三维先验体素子地图S_j中的最优相对位姿，然后利用距离智能平台最近的先验路标m_k校正智能平台的位姿，最终得到智能平台的位姿。

2.如权利要求1所述的一种基于多源先验信息的点云匹配高精度定位方法，其特征在于，在步骤S2中，根据GNSS全局位姿和点云观测数据创建各个三维先验体素子地图S_i的具体步骤如下：

步骤S201，根据点云观测数据创建t-1时刻的三维先验体素子地图当i＝1时，将三维先验体素子地图S₁的坐标系的伪全局位姿设为(0，0，0，0，0，0)，并将获取第一帧点云观测数据时的GNSS全局位姿作为三维先验体素子地图的GNSS全局位姿；当i≠1时，以三维先验体素子地图中的第一个先验路标的伪全局位姿和GNSS全局位姿作为该三维先验体素子地图坐标系的伪全局位姿和GNSS全局位姿；

步骤S202，统计三维先验体素子地图的每一体素中的点云分布特征及特征可信度，所述点云分布特征包括：线特征、面特征和团状特征；

步骤S203，采用最大似然估计法将t时刻的点云观测数据与三维先验体素子地图进行数据匹配，得到智能平台在三维先验体素子地图坐标系中的局部位姿

其中，点云观测数据与三维先验体素子地图进行数据匹配的公式如下：

式中，n＝0，1，…N，T_t为t时刻的智能平台与三维先验体素子地图之间的相对位姿，C_n为点云观测数据中的任一点，为距离T_tC_n最近的特征体素中点云分布均值，为距离T_tC_n最近的特征体素中点云分布特征向量矩阵，为的转置矩阵，为距离T_tC_n最近的特征体素中点云分布特征值矩阵的逆矩阵，对于线特征，令中的1/λ₁＝0，对于面特征，令中的1/λ₁＝0，1/λ₂＝0；为点云观测数据中任一点的特征观测误差；e为点云特征观测误差，e^T为e的转置矩阵，为点云特征观测误差协方差矩阵的逆矩阵，为智能平台在三维先验体素子地图坐标系中的局部位姿，即t时刻点云观测数据相对于三维先验体素子地图的最优相对位姿；

步骤S204，根据数据匹配结果和t时刻的点云观测数据更新三维先验体素子地图得到t时刻的三维先验体素子地图并按照步骤S202的方法统计三维先验体素子地图中每一体素的点云分布特征及特征可信度；

步骤S205，对智能平台在三维先验体素子地图坐标系中的局部位姿进行坐标变换得到t时刻智能平台的伪全局位姿，将t时刻智能平台的伪全局位姿和步骤S1中得到的GNSS全局位姿作为第一个先验路标将先验路标m_t记录到三维先验体素子地图中；

步骤S206，在三维先验体素子地图中保存N帧多源先验信息后，将t时刻的三维先验体素子地图S_i保存到计算平台外存中；

步骤S207，重复步骤S201～S206，完成智能平台运行区域内各个三维先验体素子地图S_i的创建，i＝1，2，3...。