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

Abstract

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

Description

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

技术领域

本发明涉及移动机器人智能定位技术领域，具体涉及一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位方法及系统。

背景技术

煤矿产业是我国重要的能源产业，近些年，随着国家经济快速增长，各种工业蓬勃发展，煤矿引入了大量的移动机器人来提高煤矿的安全。机器人具有机动性强、导航自主、可应用传感器种类多、网络交互性强的优势，目前已广泛的应用于煤矿生产之中，极大的增加了煤矿的安全生产，提高了生产效率。

应用于煤矿的移动机器人种类众多，如负责掘进的掘进机器人，负责运输的无人运输机器人，负责监测的巡检机器人以及灾后负责救援的救援机器人等。其中应用最为广泛的是巡检机器人和救援机器人。在煤矿井下，需要进行实时巡检，以确保设备的安全工作和防止事故的发生。另外，煤矿井下许多有害气体，如瓦斯、一氧化碳等，巡检机器人携带多种气体浓度监测传感器，可以更加直接方便的监测井下各种气体的浓度，井下温度和湿度等，防患于未然，并独立的完成煤矿井下的巡检工作；在发生矿难之后，由于煤矿井下环境恶劣，能见度低，危险系数高，使得许多救援任务不能直接进行，需要依靠机器人来进行抢救，救援机器人可以实现在恶劣的环境下完成被困人员的营救。

虽然移动机器人已经广泛的应用于煤矿之中，但考虑到井下复杂的自然环境，由于处于地下环境，一些寻常的定位手段很难直接应用，如井下巷道复杂，很少有笔直的路线，且信号多被煤壁所吸收，对无线电通信非常不利。当井下使用移动机器人进行无人化工作时，由于井下无法使用GPS定位，因此不能让后端工作者实时的对其移动轨迹进行跟踪与监测，只监测到很多数据而无法获知其相应的位置，这无异于盲人摸象，这样的信息是毫无用处的，所以，在煤矿井下，针对移动机器人的导航和实时定位显得极其重要。煤矿移动机器人的导航与定位技术研究是一个复杂且重要的课题，它涉及的领域包括：机器人导航、机器人控制、运动装置、多传感器融合及环境感知等。

由此不难发现，煤矿用移动机器人的导航系统与定位系统的研究，对于解决煤矿移动机器人的定位精度问题，增加移动机器人的机动性和可操作性，提高煤矿工作的安全性，对实现地面人员对井下搜救机器人、巡检机器人的位置跟踪和监测、对掘进机，采煤机等井下工作设备的工作监测、对煤矿井下各种有害气体浓度、环境温度的监测、对煤矿安全生产具有十分重要的研究意义。

发明内容

为解决上述问题，本发明提供了一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航系统，通过图优化算法将惯导数据与单目相机数据进行融合处理，增加系统的定位精度，使用ORB_SLAM算法，在保证定位精度的同时提高了视觉计算的实时性，与现有煤矿移动机器人定位系统相比，具有成本较低，安装简单，定位精度高等优点。

为实现上述目的，本发明采取的技术方案为：

一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航系统，利用捷联惯导系统与视觉相机的特性进行互补，融合两个传感器的数据，对移动机器人进行定位，并绘制出移动轨迹，将数据传给监控系统，实现移动机器人的远程定位导航；具体的：根据机器人在移动的过程中，IMU和单目相机产生的数据，通过无线传输发送给上位机，上位机通过数据处理后将数据融合与优化，并使用处理后的数据，对移动机器人进行定位与运行路线的绘制，监控人员通过观察移动机器人的工作情况，做出判断命令，并下发给机器人，实现移动机器人的远程监控与定位导航。

具体的定位导航方法，包括如下步骤：

S1、在煤矿移动机器人上安装捷联惯导系统与单目视觉相机全套设备，并控制移动机器人在煤矿巷道内移动；

S2、建立捷联惯导系统的世界坐标系(w系)和载体坐标系(b系)，并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵；建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系)，并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵：最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵；

以移动机器人重心为坐标原点，移动机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系(X_b，Y_b，Z_b)；根据移动机器人后轮的起始位置为原点，确定世界坐标系(X_w，Y_w，Z_w)；根据单目相机的安装位置建立相机坐标系(X_c，Y_c，Z_c)；惯导坐标系通过旋转平移得到视觉坐标系，从而将两个传感器联系起来；

S3、将捷联惯导系统和单目相机的原始数据传入计算机，并完成数据的融合与定位建图等处理，具体的：

S301、完成IMU数据的预积分处理：已知IMU每个时刻的加速度a和角速度w的值，对IMU建立观测模型，有：

式中，

为角速度和加速度的测量值，_Bw_WB(t)，_Wa(t)为角速度和加速度的真实值，

为世界系到IMU坐标系的旋转关系，_Wg为重力加速度，b^g(t)，b^a(t)为IMU的零偏，η^g(t)，η^a(t)为系统的白噪声；

根据运动学方程：

式中，

为机器人旋转、速度、平移的导数，

_Wa、_Wv为机器人的角速度、加速度、速度；

则：

根据i时刻的IMU状态量，求离散时间下j时刻的关键帧对应的IMU预积分公式为：

S302、将单目相机采集到的数据分成关键帧，提取每个关键帧的特征点，计算特征点的FAST得分值，在特征点的邻域内，若有多个特征点，则判断得分值，若p是响应值最大的则保留，否则抑制，得分计算公式如下：

其中，values为p点周围像素点的值，t为FAST算法的设置阈值；

根据提取出的特征点，通过两个关键帧之间特征点的跟踪和比对，求出单应矩阵，并通过矩阵的分解求出旋转矩阵R与位移向量t；

并利用针孔相机模型，通过矩阵的变换，获得空间点在像素坐标系中的像素坐标，将测量值与真实值相减即为重投影误差，转换公式如下所示：

其中，S_i为尺度，K为相机内参；

S303、实现传感器的联合初始化：首先求解单目相机和IMU之间的旋转矩阵

有：

将旋转矩阵写成四元数形式，有：

其次，实现相机坐标系的转换，由于视觉相机最后是在像素坐标系下进行算法定位，因此需要将世界坐标系转换到像平面坐标系下，而世界坐标系与像平面坐标系之间还存在一个相机坐标系，因此，世界坐标系与相机坐标系的转换为：

摄像机坐标系到像平面坐标系转换为：

其中，C_x、C_y为像平面坐标系下远点的平移量，f_x、f_y为焦距；世界坐标系到像平面坐标系转换关系如下所示：

最后，将IMU数据与视觉数据对齐，包括修正惯导系统陀螺仪的bias；初始化惯导系统速度、重力向量和尺度因子；改进重力向量g；

S4、在IMU与视觉传感器初始化完成之后，将数据通过网络传输给上位机监控系统，并进行数据融合处理，具体的：

S401、采用图优化的方法实现数据的融合，将滑动窗口中系统边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化，从而获得机器人准确的位姿信息，需要优化的代价函数为：

其中||r_p-H_pX||²为边缘化信息，其中r_p为边缘化的代价函数、H_p为信息矩阵。

为IMU的测量残差，其中r_B为IMU测量的代价函数。

为视觉的重投影误差。其中r_C视觉测量的代价函数。

在优化过程中，每一次高斯迭代，都会将该代价函数线性化为：

将Λ_p，Λ_B，A_C分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵，把惯导的测量误差与视觉测量误差联立，可将最小化该代建函数简化为求解该线性方程的问题：

(Λ_p+Λ_B+Λ_C)δX＝(b_p+b_B+b_C)

S5、最后加入回环检测，利用词袋的形式来做关键帧数据库的构建与查询，使机器人运动到曾经到达过的位置时，当前帧与词袋模型中的关键帧进行匹配，使地图能够闭合，同时增加定位精度和系统的鲁棒性；

S6、上位机通过观察机器人的运动轨迹与任务完成情况，对机器人发布控制命令，控制命令通过数据传输传给移动机器人，从而实现对煤矿移动机器人的定位与导航。

进一步地，所述步骤S302中，采用FAST算法，若某像素与其周围邻域内足够多的像素点相差较大，则该像素可能是特征点。

进一步地，所述步骤S303中，陀螺仪的bias修正，在修正陀螺仪bias时，构成了如下约束：

其中，

其中

为雅各比矩阵，δbw为机器人的角速度偏移量。对构成的约束进行最小二乘法求解，最终可得到bias的变化量。

对构成的约束进行最小二乘法求解，最终可得到bias的变化量。

进一步地，所述步骤S303中，初始化速度v、重力向量g和尺度因子s时，将系统的状态向量写为：

根据IMU的观测模型，和IMU相对相机的旋转

可以将相机坐标系下的位姿转换到IMU坐标系下，为：

最后经过计算与推导，可以将这个问题推导成求解如下最小二程的问题：

进一步地，所述步骤S303中，由于地心引力的影响，重力向量会有b₁和b₂两个正交方向的分量，则有扰动公式：

δg＝w₁b₁+w₂b₂；

其中，w₁，w₂是两个扰动方向的大小，最后通过求解该扰动公式，可以求解出纠正后的重力向量。

本发明具有以下有益效果：

(1)采用多传感器融合，增加了系统的定位精度与鲁棒性。

(2)利用捷联惯导的数据恢复了单目相机的尺度问题，同时利用单目相机的数据，克服了纯惯导定位的误差累计问题。

(3)使用处理后的数据，将移动机器人的移动轨迹进行绘制，增加了监控人员的可视性。

(4)采用无线通讯进行移动机器人与上位机之间的远程数据传输，实现对煤矿井下移动机器人的实时监测，解放了劳动力，提高了井下工作的安全性。

附图说明

图1为本发明实施例一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法的流程框图。

图2为本发明实施例中的移动机器人的控制系统。

图3为本发明的一个具体应用例的系统结构图。

图4为本发明实施例中的捷联惯导预积分原理图。

图5为本发明实施例中的单目相机成像原理图。

具体实施方式

下面结合具体实施例对本发明进行详细说明。以下实施例将有助于本领域的技术人员进一步理解本发明，但不以任何形式限制本发明。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进。这些都属于本发明的保护范围。

如图1所示，本发明实施例提供了一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法，包括如下步骤：

S1、在煤矿移动机器人上安装捷联惯导系统与单目视觉相机全套设备，并控制移动机器人在煤矿巷道内移动；

S2、建立捷联惯导系统的世界坐标系(w系)和载体坐标系(b系)，并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵；建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系)，并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵：最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵；

以移动机器人重心为坐标原点，移动机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系(X_b，Y_b，Z_b)；根据移动机器人后轮的起始位置为原点，确定世界坐标系(X_w，Y_w，Z_w)；根据单目相机的安装位置建立相机坐标系(X_c，Y_c，Z_c)；惯导坐标系通过旋转平移得到视觉坐标系，从而将两个传感器联系起来；

S3、将捷联惯导系统和单目相机的原始数据传入计算机，并完成数据的融合与定位建图等处理，具体的：

S301、由于单目相机存在尺度问题，需要由惯导来提供，而IMU会受到bias的影响，且IMU与单目相机的频率不同，不能直接进行数据融合，因此要对IMU数据进行预积分。

完成IMU数据的预积分处理：已知IMU每个时刻的加速度a和角速度w的值，对IMU建立观测模型，有：

式中，

为角速度和加速度的测量值，_Bw_WB(t)，_Wa(t)为角速度和加速度的真实值，

为世界系到IMU坐标系的旋转关系，_Wg为重力加速度，b^g(t)，b^a(t)为IMU的零偏，η^g(t)，η^a(t)为系统的白噪声。

根据运动学方程：

式中，

为机器人旋转、速度、平移的导数，

_Wa、_Wv为机器人的角速度、加速度、速度。

则：

根据i时刻的IMU状态量，求离散时间下j时刻的关键帧对应的IMU预积分公式为：

S302、将单目相机采集到的数据分成关键帧，采用FAST算法提取每个关键帧的特征点，若某像素与其周围邻域内足够多的像素点相差较大，则该像素可能是特征点；计算特征点的FAST得分值，在特征点的邻域内，若有多个特征点，则判断得分值，若p是响应值最大的则保留，否则抑制，得分计算公式如下：

其中，values为p点周围像素点的值，t为FAST算法的设置阈值。

根据提取出的特征点，通过两个关键帧之间特征点的跟踪和比对，求出单应矩阵，并通过矩阵的分解求出旋转矩阵R与位移向量t；

并利用针孔相机模型，通过矩阵的变换，获得空间点在像素坐标系中的像素坐标，将测量值与真实值相减即为重投影误差，转换公式如下所示：

其中，S_i为尺度，K为相机内参；

S303、由于IMU的坐标系不同，因此需要预先将传感器的联合初始化。

实现传感器的联合初始化：首先求解单目相机和IMU之间的旋转矩阵

有：

将旋转矩阵写成四元数形式，有：

其次，实现相机坐标系的转换，由于视觉相机最后是在像素坐标系下进行算法定位，因此需要将世界坐标系转换到像平面坐标系下，而世界坐标系与像平面坐标系之间还存在一个相机坐标系，因此，世界坐标系与相机坐标系的转换为：

摄像机坐标系到像平面坐标系转换为：

其中，C_x、C_y为像平面坐标系下远点的平移量，f_x、f_y为焦距；世界坐标系到像平面坐标系转换关系如下所示：

最后，将IMU数据与视觉数据对齐，包括修正惯导系统陀螺仪的bias；初始化惯导系统速度、重力向量和尺度因子；改进重力向量g；

(1)陀螺仪的bias修正，在修正陀螺仪bias时，构成了如下约束：

其中，

其中

为雅各比矩阵，δbw为机器人的角速度偏移量。对构成的约束进行最小二乘法求解，最终可得到bias的变化量。对构成的约束进行最小二乘法求解，最终可得到bias的变化量。

(2)初始化速度、重力向量g和尺度因子时，将系统的状态向量写为：

根据IMU的观测模型，和IMU相对相机的旋转

可以将相机坐标系下的位姿转换到IMU坐标系下，为：

最后经过计算与推导，可以将这个问题推导成求解如下最小二程的问题：

(3)所述步骤S303中，由于地心引力的影响，重力向量会有b₁和b₂两个正交方向的分量，则有扰动公式：

δg＝w₁b₁+w₂b₂；

其中，w₁，w₂是两个扰动方向的大小，最后通过求解该扰动公式，可以求解出纠正后的重力向量。

S4、在IMU与视觉传感器初始化完成之后，将数据通过网络传输给上位机监控系统，并进行数据融合处理，具体的：

S401、采用图优化的方法实现数据的融合，将滑动窗口中系统边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化，从而获得机器人准确的位姿信息，需要优化的代价函数为：

其中||r_p-H_pX||²为边缘化信息，其中r_p为边缘化的代价函数、H_p为信息矩阵。

为IMU的测量残差，其中r_B为IMU测量的代价函数。

为视觉的重投影误差。其中r_C视觉测量的代价函数。

在优化过程中，每一次高斯迭代，都会将该代价函数线性化为：

将Λ_p，Λ_B，Λ_C分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵，把惯导的测量误差与视觉测量误差联立，可将最小化该代建函数简化为求解该线性方程的问题：

(Λ_p+Λ_B+Λ_C)δX＝(b_p+b_B+b_C)

S5、最后加入回环检测，利用词袋的形式来做关键帧数据库的构建与查询，使机器人运动到曾经到达过的位置时，当前帧与词袋模型中的关键帧进行匹配，使地图能够闭合，同时增加定位精度和系统的鲁棒性；

S6、上位机通过观察机器人的运动轨迹与任务完成情况，对机器人发布控制命令，控制命令通过数据传输传给移动机器人，从而实现对煤矿移动机器人的定位与导航。

本具体实施通过单目视觉相机与捷联惯导配合，减小了安装空间，增加了应用范围。通过IMU地数据，恢复单目视觉相机的尺度问题，通过单目相机的数据，校正捷联惯导系统的误差累积，利用图优化算法，实现对捷联惯导与单目相机的数据进数据融合处理，增加了系统的定位精度，同时加入回环检测，增加煤矿移动机器人视觉与惯导组合定位系统的定位精度与鲁棒性；利用处理过后的数据，对移动机器人的移动轨迹进行绘制，增加了控制系统的可视性，方便控制人员做出决策；将捷联惯导与视觉相机数据通过无线传输传给上位机，方便工作人员远程进行操控。

以上对本发明的具体实施例进行了描述。需要理解的是，本发明并不局限于上述特定实施方式，本领域技术人员可以在权利要求的范围内做出各种变化或修改，这并不影响本发明的实质内容。在不冲突的情况下，本申请的实施例和实施例中的特征可以任意相互组合。

Claims (6)

1.一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航系统，其特征在于，利用捷联惯导系统与视觉相机的特性进行互补，融合两个传感器的数据，对移动机器人进行定位，并绘制出移动轨迹，将数据传给监控系统，实现移动机器人的远程定位导航。

2.如权利要求1所述的一种基于捷联惯导与视觉组合的煤矿移动机器人智能定位导航方法，其特征在于，该方法包括如下步骤：

S1、在煤矿移动机器人上安装捷联惯导系统与单目视觉相机全套设备，并控制移动机器人在煤矿巷道内移动；

S2、建立捷联惯导系统的世界坐标系(w系)和载体坐标系(b系)，并构建载体坐标系(b系)与世界坐标系(w系)之间的转换矩阵；建立单目相机的世界坐标系(w系)、相机坐标系(c系)和像平面坐标系(i系)，并构建世界坐标系(w系)、相机坐标系(c系)与像平面坐标系(i系)之间的转换矩阵：最后确定惯导的载体坐标系(b系)与单目相机的相机坐标系(c系)之间的旋转矩阵；

以移动机器人重心为坐标原点，移动机器人前进的方向作为Y轴正方向，以向上为Z轴正方向，垂直Y轴向右方向为X轴正方向，建立载体坐标系(X_h，Y_b，Z_b)；根据移动机器人后轮的起始位置为原点，确定世界坐标系(X_w，Y_w，Z_w)；根据单目相机的安装位置建立相机坐标系(X_c，Y_c，Z_c)；惯导坐标系通过旋转平移得到视觉坐标系，从而将两个传感器联系起来；

S3、将捷联惯导系统和单目相机的原始数据传入计算机，并完成数据的融合与定位建图处理，具体的：

S301、完成IMU数据的预积分处理：已知IMU每个时刻的加速度a和角速度w的值，对IMU建立观测模型，有：

式中，

为角速度和加速度的测量值，_Bw_WB(t)，_Wa(t)为角速度和加速度的真实值，

为世界系到IMU坐标系的旋转关系，_Wg为重力加速度，b^g(t)，b^a(t)为IMU的零偏，η^g(t)，η^a(t)为系统的白噪声；

根据运动学方程：

式中，

为机器人旋转、速度、平移的导数，

_Wa、_Wv为机器人的角速度、加速度、速度；

则：

根据i时刻的IMU状态量，求离散时间下j时刻的关键帧对应的IMU预积分公式为：

S302、将单目相机采集到的数据分成关键帧，提取每个关键帧的特征点，计算特征点的FAST得分值，在特征点的邻域内，若有多个特征点，则判断得分值，若p是响应值最大的则保留，否则抑制，得分计算公式如下：

其中，values为p点周围像素点的值，t为FAST算法的设置阈值；

根据提取出的特征点，通过两个关键帧之间特征点的跟踪和比对，求出单应矩阵，并通过矩阵的分解求出旋转矩阵R与位移向量t；

并利用针孔相机模型，通过矩阵的变换，获得空间点在像素坐标系中的像素坐标，将测量值与真实值相减即为重投影误差，转换公式如下所示：

其中，S_i为尺度，K为相机内参；

S303、实现传感器的联合初始化：首先求解单目相机和IMU之间的旋转矩阵

有：

将旋转矩阵写成四元数形式，有：

其次，实现相机坐标系的转换，由于视觉相机最后是在像素坐标系下进行算法定位，因此需要将世界坐标系转换到像平面坐标系下，而世界坐标系与像平面坐标系之间还存在一个相机坐标系，因此，世界坐标系与相机坐标系的转换为：

摄像机坐标系到像平面坐标系转换为：

其中，C_x、C_y为像平面坐标系下远点的平移量，f_x、f_y为焦距；世界坐标系到像平面坐标系转换关系如下所示：

最后，将IMU数据与视觉数据对齐，包括修正惯导系统陀螺仪的bias；初始化惯导系统速度、重力向量和尺度因子；改进重力向量g；

S4、在IMU与视觉传感器初始化完成之后，将数据通过网络传输给上位机监控系统，并进行数据融合处理，具体的：

S401、采用图优化的方法实现数据的融合，将滑动窗口中系统边缘化的先验信息、IMU的测量残差和视觉的观测残差和视觉的观测残差三者一起进行优化，从而获得机器人准确的位姿信息，需要优化的代价函数为：

其中||r_p-H_pX||²为边缘化信息，其中r_p为边缘化的代价函数、H_p为信息矩阵；

为IMU的测量残差，其中r_B为IMU测量的代价函数；

为视觉的重投影误差；其中，r_C为视觉测量的代价函数；

在优化过程中，每一次高斯迭代，都会将该代价函数线性化为：

将Λ_p，Λ_B，Λ_C分别代表边缘先验量、IMU的测量残差和视觉的测量残差的信息矩阵，把惯导的测量误差与视觉测量误差联立，可将最小化该代建函数简化为求解该线性方程的问题：

(Λ_p+Λ_B+Λ_C)δX＝(b_p+b_B+b_C)

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中，由于地心引力的影响，重力向量会有b₁和b₂两个正交方向的分量，则有扰动公式：

δg＝w₁b₁+w₂b₂；

其中，w₁，w₂是两个扰动方向的大小，最后通过求解该扰动公式，可以求解出纠正后的重力向量。

Images