[发明专利]融合单目视觉与IMU的煤矿巷道巡检机器人系统及导航方法

权利要求书

1.一种融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，用于巡检的机器人系统包括：单目相机、巡检机器人、机载惯性测量单元、机载处理器和照明灯；巡检机器人载体用于搭载机载处理器、照明灯设备，构成导航系统的硬件部分；单目相机用于拍摄煤矿井下巷道的高清图片，构成系统的数据采集部分；机载惯性测量单元融合单目相机，对齐后获得尺度信息；照明灯用于巷道内部的照明，提高单目相机成像质量；机载处理器用于图像处理、计算巡检机器人的偏航角和机器人姿态解算；

其特征是：首先将惯性测量单元与单目相机对齐，进行初始化操作，获得相机的真实尺度信息；然后通过单目相机拍摄的高清图片，经过一系列图像处理，获得巷道地平面与两侧墙壁的交线在图像中的交线方程，在图像中交线通过延长后会相交于一点；将两个方程联立可求得交点的图像坐标，查找距离交点最近的匹配特征点恢复此帧相机坐标系的三维坐标，通过光轴向量和所求的特征点向量求得巡检机器人的偏航角，最后根据求得偏航角进行机器人的航向控制；

具体步骤如下：

步骤1)单目相机标定内参；

步骤2)单目相机与惯性测量单元初始化，求解尺度信息参数，将相机坐标系对齐到世界坐标系；

步骤3)根据单目相机拍摄的巷道高清图片，检测出巷道地平面与两侧墙壁的交线；

步骤4)结合步骤3)对检测的两条交线构建方程，求解巡检机器人在巷道中的偏航角；

步骤5)根据机器人的偏航角进行航向调节。

2.根据权利要求1所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤1)，具体步骤如下：

11)采用张氏标定法标定相机内参；

12)单目相机内参标定后，得到内参矩阵为和相机的畸变向量V＝[k₁k₂ k₃ q₁ q₂]，其中f_x、f_y分别为单目相机在x、y方向上的焦距，(c_x，c_y)为像素坐标中相机光轴所对应的坐标，k₁、k₂、k₃为相机的切向畸变系数，q₁、q₂为相机径向畸变系数。

3.根据权利要求1所述的融合单目视觉与IMU的煤矿巷道巡检机器人系统的导航方法，其特征是，所述的步骤2)，具体步骤如下：

21)单目相机与惯性测量单元构建坐标系方程：其中，下标c、b分别表示相机坐标系和惯性测量单元坐标系，s表示尺度因子，表示非米制单位轨迹，为IMU坐标系到相机坐标系的旋转；所述的惯性测量单元英文表示为Inertial Measurement Unit，英文缩写为IMU；

22)利用步骤21)第一个公式估计外参数q_bc：对k、k+1时刻，IMU旋转积分为视觉测量为满足公式公式进一步可写成其中[·]_L、[·]_R表示四元数的左右算子；累计不同时刻的上述方程，加入鲁棒核权重，最后通过奇异值分解Singular Value Decomposition，SVD，所求解即为最小奇异向量；

23)利用21)中第一个公式求解陀螺仪偏置：外参q_bc已经由22)求解出，根据公式公式为最小二乘问题，对其求雅可比矩阵Jacobi Matrix，构建HX＝b求解；其中，B是所有图像帧集合，表示通过相机求取的b_k和b_k+1的旋转的逆与陀螺仪求取的差异，表示取旋转四元数的虚部；

24)初始化尺度、速度和重力：待估计的变量为其中，为k时刻IMU坐标系中的速度，表示重力向量在相机初始帧中的表示，s为尺度因子；把世界坐标系换为相机坐标系c₀，IMU的预积分为：

把21)中所述公式代入预积分，并且将要求的变量整理到方程的右边，对其进行最小二乘求解：其中，

25)相机坐标系转换为世界坐标系：相机坐标系到世界坐标系的转换矩阵为其中把相对于初始相机坐标系下全部变量转换到世界坐标系，根据24)中求得的尺度因子恢复真实尺度。