[发明专利]一种基于多传感器融合的电力巡检机器人定位方法

权利要求书

1.一种基于多传感器融合的电力巡检机器人定位方法，其特征在于，其步骤如下：

步骤一：分别对机器人系统中的相机、IMU和里程计采集的数据进行预处理，得到图像特征点、IMU预积分和里程计预积分；

S1.1、图像特征点提取与跟踪：

S1.1.1、对M帧图像进行自适应直方图均衡化处理，得到M帧直方图图像；

S1.1.2、利用光流法对步骤S1.1.1中的M帧直方图图像进行处理得到每一帧图像的特征点；

S1.1.3、初始化m＝1，对第m帧图像的m'个特征点与第m+1帧图像的m'个特征点进行相对应，剔除无法被对应上的特征点，得到跟踪特征点b'个；

S1.1.4、根据b'个跟踪特征点的像素位置求出基础矩阵，由基础矩阵去除异常值，再采用随机采样一致性算法去除误匹配的特征点，得到剩余跟踪特征点c'个；

S1.1.5、根据c'个跟踪特征点的被跟踪到的次数按照从大到小排序，得到跟踪特点序列，然后遍历跟踪特点序列，将遍历到的每个特征点的图像掩码的周围半径区域内的掩码设置为0，其中，半径大小为m”个像素；

S1.1.6、在图像掩码不为0的区域检测新的特征点，使用shi-tomasi算法对第m+1帧图像提取m'-c'个角点，获得第m+1帧图像的图像特征点；

S1.1.7、m＝m+1，循环步骤S1.1.3至S1.1.6，直至遍历完M帧直方图图像；

S1.2、IMU预积分：

S1.2.1、利用IMU中的速度计和陀螺仪测量IMU坐标系下机器人的加速度和角速度

其中，a_t为IMU坐标系下机器人的加速度理论值，为速度计的偏置，为世界坐标系到IMU坐标系的旋转矩阵，g^w为世界坐标系下重力向量，n_a为速度计的噪声，ω_t为IMU坐标系下机器人的角速度理论值，为陀螺仪的偏置，n_ω为陀螺仪的噪声；

S1.2.2、对于连续两个关键帧图像，时间间隔为△t＝[j/f,j/f+1]，根据步骤S1.2.1中的加速度和角速度计算下一时刻由IMU得到的世界坐标系下机器人的位置、速度和角度：

其中，世界坐标系是指相机第一帧图像的坐标系，为由IMU得到的世界坐标系下j+1帧时刻的机器人的位置，为由IMU得到的世界坐标系下j帧时刻的机器人的位置，为由IMU得到的世界坐标系下j帧时刻的机器人的速度，为由IMU得到的世界坐标系下t帧时刻的机器人的旋转矩阵，为IMU的加速度计得到的线加速度，g^w为世界坐标系下的重力向量，为由IMU得到的世界坐标系下j+1帧时刻的机器人的速度，为由IMU得到的世界坐标系下j+1帧时刻的机器人的角度，为由IMU得到的世界坐标系下j帧时刻的机器人的角度，为由IMU得到的世界坐标系下t帧时刻的机器人的角度，f表示相机的频率，为IMU的陀螺仪得到的角加速度理论值，为IMU角速度四元数的右乘运算，为角速度四元数的实部向量的反对称矩阵，ω_t,x为角速度四元数在t/f时刻x轴方向上的分量，ω_t,y为角速度四元数在t/f时刻y轴方向上的分量，ω_t,z为角速度四元数在t/f时刻z轴方向上的分量，表示四元数乘法；

S1.2.3、将步骤S1.2.2中世界坐标系下机器人的位置、速度和角度转换到IMU坐标系下，得到IMU的预积分项：

其中，为第j+1帧时刻到第j帧时刻IMU坐标系下的位置增量，为第j+1帧时刻到第j帧时刻IMU坐标系的下速度增量，为第j+1帧时刻到第j帧时刻IMU坐标系下的旋转增量，为t时刻到第j帧时刻IMU坐标系下旋转增量，为IMU坐标系下t/f时刻到第j帧时刻的旋转矩阵；

S1.2.4、分别对步骤S1.2.3中的IMU的预积分项进行一阶近似泰特展开，得到IMU的预积分为：

其中，为IMU预积分位置增量的近似值，为IMU预积分速度增量的近似值，为IMU预积分旋转增量的近似值，为第j帧时刻陀螺仪偏置变化量，均是零偏值在j帧时刻的IMU的预积分测量值的雅可比矩阵；

S1.3、里程计预积分：

S1.3.1、利用里程计测量机器人的线速度和角速度对线速度和角速度进行积分，得到下一时刻由里程计得到的世界坐标系下机器人的位置和角度：

其中，为由里程计得到的世界坐标系下j+1帧时刻的机器人的位置，为由里程计得到的世界坐标系下j帧时刻的机器人的位置，为t时刻里程计坐标系相对于世界坐标系的旋转矩阵，为里程计速度测量值，n_v为速度测量值的高斯噪声，为由里程计得到的世界坐标系下j+1帧时刻的机器人的角度，为由里程计得到的世界坐标系下j+1帧时刻的机器人的位置，n_ωd为角速度测量值的高斯噪声，为里程计角速度四元数的右乘运算，ω_d为里程计角速度理论值，为里程计角速度四元数的实部向量的反对称矩阵，ω_d,x为里程计角速度四元数在x轴方向上的分量，ω_d,y为里程计角速度四元数在y轴方向上的分量，ω_d,z为里程计角速度四元数在z轴方向上的分量；

S1.3.2、将步骤S1.3.1中的世界坐标系下机器人的位置和角度转换到里程计坐标系下，得到的里程计的预积分项：

其中，为第j+1帧时刻到第j帧时刻里程计坐标系下的位置增量，为里程计速度理论值，为第j+1帧时刻到第j帧时刻里程计坐标系下的旋转增量，为t帧时刻到第j帧时刻里程计坐标系下旋转增量，为t帧时刻到第j帧时刻里程计坐标系下旋转矩阵，为里程计测得的角速度理论值；

S1.3.3、将里程计的预积分项中的噪声项分离后，对里程计的预积分项进行一阶近似泰特展开，可得里程计的预积分：

其中，为里程计预积分位置增量的近似值，为里程计预积分旋转增量的近似值，为第j帧时刻里程计速度测量值的高斯噪声，均是噪声在j时刻里程计的预积分测量值的雅可比矩阵；

步骤二：根据步骤一中的图像特征点提取关键帧图像，并利用ENFT-SFM算法对关键帧图像进行求解，得到视觉位姿，其中，视觉位姿包括位姿的旋转矩阵和平移向量；

S2.1、将第一帧图像设置为关键帧，将第一帧图像添加到滑窗中；

S2.2、计算当前帧与前一个关键帧的平均视差c：

其中，是指当前帧第i个特征点的像素坐标，是指前一个关键帧与当前帧第i个特征点相匹配的特征点的像素坐标，c'为剩余跟踪特征点；

S2.3、判断平均视差c是否大于视差阈值c_d，若是，则当前帧为关键帧，并将当前帧添加到关键帧数据库中，否则，执行步骤S2.4；

S2.4、判断匹配特征点数c'是否大于匹配阈值c”，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，执行步骤S2.5；

S2.5、计算当前帧与前一个关键帧的时间间隔△t'；

S2.6、判断时间间隔△t'是否大于间隔阈值t_d，若是，则当前帧为关键帧，并将当前帧添加到滑窗中，否则，转至下一帧图像，循环执行步骤S2.2至步骤S2.6，直至遍历完M帧图像；

步骤三：利用权重计算法分别对相机与IMU的外参、相机与里程计的外参进行标定；

步骤四：利用动态加权法将视觉位姿与IMU预积分和里程计预积分进行对齐处理，完成了机器人系统的初始化；

步骤五：对初始化后的机器人系统进行数据的采集，并对采集的数据进行预处理，得到实时图像特征点、实时IMU预积分和实时里程计预积分；

步骤六：根据步骤五中的实时图像特征点提取关键帧，并利用非线性优化法对关键帧进行优化，得到实时视觉位姿；

步骤七：根据步骤六中的实时视觉位姿对机器人的位置、速度、角度和IMU中的陀螺仪的偏置进行后端优化，得到机器人的实时位姿；

步骤八：将步骤六中的关键帧添加到关键帧数据库，循环步骤五至步骤八，直到关键帧数据库中的关键帧的数量大于H，完成关键帧数据库的构建；

步骤九：根据步骤五和步骤六获得当前帧图像，并计算当前帧图像与关键帧数据库中的所有关键帧的相似度，若存在相似度为1的关键帧，则将与当前帧图像相似度为1的关键帧作为闭环帧，执行步骤十，否则，将当前帧图像插入关键帧数据库，输出机器人的实时位姿；

其中，s(·)为相似度函数，m₁为当前帧图像的向量表示形式，n″_a′为关键帧数据库中的第a'个关键帧的向量表示形式，W表示字典；

步骤十：对闭环帧进行重定位后，再对关键帧数据库中的关键帧进行闭环优化，输出闭环优化后的位姿，并将当前帧图像插入关键帧数据库，完成对机器人的定位。