一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统

• 摘要：

  本发明涉及移动机器人智能定位技术领域,具体涉及一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统,该方法利用捷联惯导系统与视觉相机的特性进行互补,融合两个传感器的数据,对移动机器人进行定位,并绘制出移动轨迹,将数据传给监控系统,实现移动机器人的远程定位导航.由于应用在煤矿井下,可以利用惯导的数据来修正由于光线不足而造成的视觉定位不准确,而视觉的数据又可以用来修正惯导长期运行造成的累积误差,两种传感器的数据融合,可以实现对煤矿移动机器人的实时、准确的定位.本发明基于"惯导+"的理念,利用捷联惯导与视觉相机相结合,在复杂、黑暗的煤矿井下环境中,实现煤矿移动机器人的精确定位与实时监测.

• 专利类型：

  发明专利

• 申请/专利号：

  CN202010950320.9

• 申请日期：

  2020.09.10

• 公开/公告号：

  CN112033400A

• 公开/公告日：

  2020-12-04

• 发明人：

  马宏伟 张羽飞 贺媛 毛清华 李磊 石金龙 华洪涛

• 申请人：

  西安科技大学

• 主分类号：

  G01C21/16(2006.01),G,G01,G01C,G01C21

• 分类号：

  G01C21/16(2006.01),G01C21/20(2006.01),G,G01,G01C,G01C21,G01C21/16,G01C21/20

• 主权项：

  1.一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航系统,其特征在于,利用捷联惯导系统与视觉相机的特性进行互补,融合两个传感器的数据,对移动机器人进行定位,并绘制出移动轨迹,将数据传给监控系统,实现移动机器人的远程定位导航. 2.如权利要求1所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法,其特征在于,该方法包括如下步骤: S1、在煤矿移动机器人上安装捷联惯导系统与单目视觉相机全套设备,并控制移动机器人在煤矿巷道内移动; S2、建立捷联惯导系统的世界坐标系(w系)和载体坐标系(b系),并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵;建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系),并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵:最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵; 以移动机器人重心为坐标原点,移动机器人前进的方向作为Y轴正方向,以向上为Z轴正方向,垂直Y轴向右方向为X轴正方向,建立载体坐标系(Xh,Yb,Zb);根据移动机器人后轮的起始位置为原点,确定世界坐标系(Xw,Yw,Zw);根据单目相机的安装位置建立相机坐标系(Xc,Yc,Zc);惯导坐标系通过旋转平移得到视觉坐标系,从而将两个传感器联系起来; S3、将捷联惯导系统和单目相机的原始数据传入计算机,并完成数据的融合与定位建图处理,具体的: S301、完成IMU数据的预积分处理:已知IMU每个时刻的加速度a和角速度w的值,对IMU建立观测模型,有: 式中,为角速度和加速度的测量值,BwWB(t),Wa(t)为角速度和加速度的真实值,为世界系到IMU坐标系的旋转关系,Wg为重力加速度,bg(t),ba(t)为IMU的零偏,ηg(t),ηa(t)为系统的白噪声; 根据运动学方程: 式中,为机器人旋转、速度、平移的导数,Wa、Wv为机器人的角速度、加速度、速度; 则: 根据i时刻的IMU状态量,求离散时间下j时刻的关键帧对应的IMU预积分公式为: S302、将单目相机采集到的数据分成关键帧,提取每个关键帧的特征点,计算特征点的FAST得分值,在特征点的邻域内,若有多个特征点,则判断得分值,若p是响应值最大的则保留,否则抑制,得分计算公式如下: 其中,values为p点周围像素点的值,t为FAST算法的设置阈值; 根据提取出的特征点,通过两个关键帧之间特征点的跟踪和比对,求出单应矩阵,并通过矩阵的分解求出旋转矩阵R与位移向量t; 并利用针孔相机模型,通过矩阵的变换,获得空间点在像素坐标系中的像素坐标,将测量值与真实值相减即为重投影误差,转换公式如下所示: 其中,Si为尺度,K为相机内参; S303、实现传感器的联合初始化:首先求解单目相机和IMU之间的旋转矩阵有: 将旋转矩阵写成四元数形式,有: 其次,实现相机坐标系的转换,由于视觉相机最后是在像素坐标系下进行算法定位,因此需要将世界坐标系转换到像平面坐标系下,而世界坐标系与像平面坐标系之间还存在一个相机坐标系,因此,世界坐标系与相机坐标系的转换为: 摄像机坐标系到像平面坐标系转换为: 其中,Cx、Cy为像平面坐标系下远点的平移量,fx、fy为焦距;世界坐标系到像平面坐标系转换关系如下所示: 最后,将IMU数据与视觉数据对齐,包括修正惯导系统陀螺仪的bias;初始化惯导系统速度、重力向量和尺度因子;改进重力向量g; S4、在IMU与视觉传感器初始化完成之后,将数据通过网络传输给上位机监控系统,并进行数据融合处理,具体的: S401、采用图优化的方法实现数据的融合,将滑动窗口中系统边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化,从而获得机器人准确的位姿信息,需要优化的代价函数为: 其中||rp-HpX||2为边缘化信息,其中rp为边缘化的代价函数、Hp为信息矩阵;为IMU的测量残差,其中rB为IMU测量的代价函数;为视觉的重投影误差;其中,rC为视觉测量的代价函数; 在优化过程中,每一次高斯迭代,都会将该代价函数线性化为: 将Λp,ΛB,ΛC分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵,把惯导的测量误差与视觉测量误差联立,可将最小化该代建函数简化为求解该线性方程的问题: (Λp+ΛB+ΛC)δX＝(bp+bB+bC) S5、最后加入回环检测,利用词袋的形式来做关键帧数据库的构建与查询,使机器人运动到曾经到达过的位置时,当前帧与词袋模型中的关键帧进行匹配,使地图能够闭合,同时增加定位精度和系统的鲁棒性; S6、上位机通过观察机器人的运动轨迹与任务完成情况,对机器人发布控制命令,控制命令通过数据传输传给移动机器人,从而实现对煤矿移动机器人的定位与导航. 3.如权利要求2所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法,其特征在于:所述步骤S302中,采用FAST算法,若某像素与其周围邻域内足够多的像素点相差较大,则该像素可能是特征点. 4.如权利要求2所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法,其特征在于:所述步骤S303中,陀螺仪的bias修正,在修正陀螺仪bias时,构成了如下约束: 其中,一 其中为雅各比矩阵,δbw为机器人的角速度偏移量;对构成的约束进行最小二乘法求解,最终可得到bias的变化量. 5.如权利要求2所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法,其特征在于:所述步骤S303中,初始化速度v、重力向量g和尺度因子s时,将系统的状态向量写为: 根据IMU的观测模型,和IMU相对相机的旋转可以将相机坐标系下的位姿转换到IMU坐标系下,为: 最后经过计算与推导,可以将这个问题推导成求解如下最小二程的问题: 6.如权利要求2所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法,其特征在于:所述步骤S303中,由于地心引力的影响,重力向量会有b1和b2两个正交方向的分量,则有扰动公式: δg＝w1b1+w2b2; 其中,w1,w2是两个扰动方向的大小,最后通过求解该扰动公式,可以求解出纠正后的重力向量.