[发明专利]基于路标主动重观测的星表巡视器激光雷达定位方法

权利要求书

1.一种基于路标主动重观测的星表巡视器激光雷达定位方法，其特征在于，包括以下步骤：

步骤1，使用激光雷达和惯性测量单元松耦合的方式进行巡视器的定位，激光雷达和惯性测量单元测得实时绝对位姿，记录二者状态估计的差值作为状态估计不确定性的参照；

步骤2，使用奇异值分解的方法对当前帧点云进行划分，每隔固定时间段将当前帧点云划分为四个集合；

步骤3，对于步骤2划分的每个点云集合，构建由该集合点云数量熵和SVD分解奇异值组成的协方差矩阵，求取协方差矩阵的行列式最大值，提取重观测路标；

步骤4，计算激光雷达以及惯性测量单元的状态估计的差值的2-范数，若该范数大于预定阈值，则触发重观测；

步骤5，在巡视器行进过程中实时存储四处重观测路标，在有新的重观测路标被提取时，最早被提取的重观测路标将被释放，在触发重观测后巡视器将停止行进，使用层次分析法对四处重观测路标进行优先级排序，选择一处最佳路标进行配准；

步骤6，将步骤5选取的最佳路标的位置信息与地图中对应的位置信息进行配准，实现系统状态方差的预测值与当前观测值的融合更新。

2.如权利要求1所述的基于路标主动重观测的星表巡视器激光雷达定位方法，其特征在于，在进行步骤1时，巡视器激光雷达和惯性测量单元状态估计的差值求取方法如下：

k时刻巡视器在全局系下的位置表达式：

k到k+1时刻激光雷达测得的位姿变换矩阵，则k+1时刻激光雷达的位姿估计：

惯性测量单元在k+1时刻测得的位姿估计值表达式：

式中，表示速度，表示重力加速度，表示k和k+1时刻的时间间隔，表示加速度计的测量值，和分别表示加速度计的漂移和白噪声，表示陀螺仪的测量值，和表示陀螺仪的漂移和白噪声，和分别表示k和k+1时刻从载体系到全局系得旋转矩阵，二者测量的状态估计的差值计算公式：

。

3.如权利要求2所述的基于路标主动重观测的星表巡视器激光雷达定位方法，其特征在于，在进行步骤2时，对当前帧点云的划分方式如下：

将当前帧点云均匀分成四块区域A、B、C、D，并求取每块区域的中心点Centre_A、Centre_B、Centre_C、Centre_D，以四个中心点为中心，用Range Search算法做范围搜索，逐步扩充点云集合，直至每个集合中囊括了足够数量的点云，这些点云能代表本区域的地貌；将此方法得到的点云集合标记为ContainerA、ContainerB、ContainerC、ContainerD，将每个集合里的点云实时地进行SVD分解，得到三个特征值，，，且，构建表达式：

当S小于阈值时，停止范围搜索。