[发明专利]一种面向电力巡检场景的定位方法及系统

权利要求书

1.一种面向电力巡检场景的定位方法，其特征在于，包括：

基于双目相机获取巡检人员的定位点云，将所述定位点云与预先构建的巡检场景中的场景点云进行闭环筛选；

若所述闭环筛选得到场景点云，则基于筛选后的场景点云对当前定位点云进行位姿补偿，确定所述巡检人员的位置；否则，当前巡检人员的位置不在巡检场景范围内。

2.如权利要求1所述的方法，其特征在于，所述巡检场景的构建，包括：

步骤一：通过IMU地磁计获取双目相机的初始位姿；

步骤二：基于所述初始姿态，通过双目相机创建关键帧，并根据所述关键帧构建场景点云；

步骤三：将所述场景点云存入设定的拓扑地图中，并进行闭环检测；若通过所述闭环检测，则以设定次数重复所述步骤二和步骤三，得到巡检场景；否则，重新创建关键帧，并以设定次数重复所述步骤二和步骤三，直到通过闭环检测。

3.如权利要求2所述的方法，其特征在于，所述通过双目相机创建关键帧，并根据所述关键帧构建场景点云，包括：

通过双目相机创建关键帧，如果创建失败，则继续创建关键帧；否则通过所述关键帧采集场景点图像并获取所述场景点的三维深度信息；

将所述三维深度信息通过位姿推算进行位姿校正，并通过视觉里程计构建场景点云。

4.如权利要求1所述的方法，其特征在于，所述基于双目相机获取巡检人员的定位点云，将所述定位点云与预先构建的巡检场景中的场景点云进行闭环筛选，包括：

通过双目相机获取巡检人员定位点云中的所有三维坐标数据；

基于所有三维坐标数据与所有的场景点云的三维坐标数据通过闭环检测进行筛选，得到筛选后的场景点云。

5.如权利要求4所述的方法，其特征在于，所述基于筛选后的场景点云对当前定位点云进行位姿补偿，确定所述巡检人员的位置，包括：

基于所述筛选后的场景点云和所述当前定位点云构建距离函数；

基于所述距离函数对所述当前定位点云中所有的三维坐标进行位姿补偿，确定所述巡检人员的位置。

6.如权利要求5所述的方法，其特征在于，所述相对距离函数的构建，包括：

筛选所述当前定位点云中与所述筛选后的场景点云中距离小于设定阈值的点，构成第一点云；

将所述定位点云和第一点云，通过中心化，计算得到定位点云质心和第一点云质心，并基于所述定位点云质心和第一点云质心构建协方差矩阵；

基于所述协方差矩阵构建对称矩阵，并得到对称矩阵特征值；

选择最大的特征值来求解特征向量；

基于所述特征向量，构建旋转矩阵和平移矩阵，并根据所述旋转矩阵和平移矩阵，得到距离函数。

7.如权利要求6所述的方法，其特征在于，所述基于所述特征向量，构建旋转矩阵和平移矩阵，包括：

根据罗德里格旋转公式将特征向量转化为旋转矩阵；

根据下式求得平移矩阵；

T＝μ′-Rμ

式中，T为平移矩阵，μ′为第一点云质心，μ为当前定位点云质心，R为旋转矩阵。

8.如权利要求7所述的方法，其特征在于，所述定位点云质心μ的计算，如下式所示：

其中，μ为当前定位点云质心，N为当前定位点云中点的数量，A_i为当前定位点云中的点，i＝1，2，3…N；

所述第一点云质心μ′的计算，如下式所示：

其中，N′为第一点云中点的数量，N′＝N，d_i为第一点云中的点，i＝1，2，3…N。

9.如权利要求6所述的方法，其特征在于，所述协方差矩阵的构建，如下式所示：

其中，∑A,d为方差矩阵，N为定位点云中点的数量，A_i为定位点云中的点，d_i为场景点云中的点，μ′为第一点云质心，μ为定位点云质心，T为平移矩阵。