[发明专利]一种用于变电站电缆沟道巡检的机器人导航方法及系统

权利要求书

1.一种用于变电站电缆沟道巡检的机器人导航方法，其特征在于，包括：

构建变电站电缆沟道的稠密地图，将所述稠密地图加载到巡检机器人系统中；确定巡检机器人的位置，巡检机器人通过所述稠密地图实现自身的导航；

其中，构建变电站电缆沟道稠密地图的步骤具体包括：

将单目相机设置于巡检机器人上，通过单目相机采集获取变电站电缆沟道的图像数据；根据所述图像数据获取点云，根据获取的点云构建获得变电站电缆沟道的稠密地图；

其中，确定巡检机器人的位置步骤具体包括：

持续跟踪所述单目相机获取的图像数据，根据图像数据获取当前关键帧，根据当前关键帧获取当前单目相机的位姿信息，根据所述位姿信息确定巡检机器人的位置。

2.根据权利要求1所述的一种用于变电站电缆沟道巡检的机器人导航方法，其特征在于，根据当前关键帧获取当前单目相机的位姿信息的步骤包括，通过计算关键帧间的se(3)转换矩阵，得到单目相机位姿信息；其中，se(3)为特殊欧式群。

3.根据权利要求2所述的一种用于变电站电缆沟道巡检的机器人导航方法，其特征在于，获取当前关键帧的步骤包括，当关键帧为创建时，通过深度初始化获得当前关键帧；否则，通过极线匹配获得当前关键帧。

4.根据权利要求1所述的一种用于变电站电缆沟道巡检的机器人导航方法，其特征在于，

持续跟踪所述单目相机获取的图像数据时，关键帧被替换，其对应的深度地图不再进行优化调整；最后将得到的关键帧通过地图优化，合并到全局地图中。

5.根据权利要求2所述的一种用于变电站电缆沟道巡检的机器人导航方法，其特征在于，通过计算关键帧间的se(3)转换矩阵，得到相机位姿，完成相机跟踪功能的算法包括：

当前关键帧用K_i＝(I_i,D_i,V_i)表示；其中I_i表示第i个图像，I:Ω→R，D_i表示第i个逆深度地图上每个像素逆深度，D:Ω→R⁺，V_i表示第i逆深度地图每个像素逆深度方差，V:Ω→R⁺；其中Ω∈R²，Ω是包含相机内参的归一化二维像素坐标点集合；

通过当前关键帧K_i计算下一帧图像I_j三维位置ξ∈se(3)，计算方法采用最小化关键帧K_i图像I_j之间的归一化光度误差；

具体的计算公式是：

r_p(P,ξ):＝I_i(P)-I_j(ω(P,D_i(P),ξ))

其中，E为误差函数，||·||_δ是Huber范数用于计算归一化残差，P表示图像I_i上一点，P＝(P_x,P_y)；光度残差r_P，其对应3D投影点的坐标是P'＝(x',y',z')；ξ∈se(3)为刚体三维位置矩阵；是P点与关键帧K_i对应点间的光度残差方差；是假设服从高斯分布的图像灰度噪声方差；ω(P,D_i(P),ξ)是3D投影偏差函数，其计算公式为：

其中