一种机器人室内定位系统的制作方法

本实用新型涉及一种机器人室内定位系统。

背景技术：

机器人要能正常使用于实际生活中，除了复杂的机械制造、驱动装置、控制系统以外底层的算法是其智能化的核心所在。机器人的关键技术是能够实现自主导航定位功能，尤其是机器人在真实环境中进行移动工作时，至少需要知道关于自身的三件事：“我在什么地方”、“我的周围是什么样的”，“我要怎么样才能到达指定的地点”。

视觉传感器是最接近人类获取外界图像的方式，通过视觉传感器(一般为相机)能够给机器人提供丰富的环境特征信息，而且相机的数据不会有漂移。通过特征匹配算法可以求得两帧之间相机运动状态，并且还可以对三维环境信息进行重建。但是，当场景区域纹理少或者相机运动过快时，都会导致相机获取的图像，进行特征匹配时造成误匹配率过高或者匹配失败，从而使计算出的位姿信息出现错误，并且难以用于场景的三维重建。

惯性传感器进行定位可以提供一个较好的初始运动状态，并且惯性传感器可以提供重力方向。但是只使用惯性传感器进行定位时，对于短时间内惯性传感器的定位精度较高，但长时间的定位会使得结果出现过度的漂移，一般的惯性传感器对于角度的测量漂移相对于时间呈线性关系，而对于位置的测量漂移相对于时间呈现平方级别的变化。

公开号CN107560613A的发明申请公开了一种基于九轴惯性传感器的机器人室内轨迹跟踪系统及方法，其中方法包括为使用引入PI控制算法，利用陀螺仪计算出的姿态矩阵，对加速度计和磁力计测量值进行坐标系转换，转换时产生的转换误差，可以用来修正姿态矩阵，从而提高整体的定位精度。该方法有以下几个缺点：1)定位结果长时间计算存在累计误差；2)只依赖惯性传感器数据，对于电磁干扰强度大的环境下，惯性数据不可靠；3)只能对机器人的轨迹进行跟踪，而不能对环境地图进行重建，无法满足机器人进行导航定位时对环境地图的需求。

为了提高机器人室内定位的精度和满足机器人导航定位时对环境地图的需求，融合惯性传感器和视觉传感器两者的优点来提高定位算法的精度和鲁棒性能，并且能够重建出场景的三维地图。

技术实现要素：

本实用新型的目的是为了提高机器人室内定位的精度和鲁棒性能以及场景的三维地图，提供一种机器人室内定位系统及定位方法。

本实用新型采用由三轴加速度计、三轴陀螺仪和三轴磁力计组成的九轴惯性传感器作为惯性传感器模块。引入基于四元数的坐标系旋转关系描述方法，使用三轴加速度计和三轴磁力计的输出值来修正三轴陀螺仪的输出值，以获得精确的旋转关系。精确的旋转关系中还包含机器人的运动方向信息。并对获取的三轴加速度值进行坐标系转换以及去除重力处理，将处理后的三轴加速度值进行积分运算可获得机器人移动过程中的速度信息。采用一种静止时期(即零速点)判断方法，用于检测出机器人移动过程中的静止时期，结合判断出的静止时期对速度进行修正，对修正后的速度再次进行积分运算则可获得机器人移动过程中的位移信息。

本实用新型首先对固定在机器人上的相机进行标定处理，标定完成后使用标定后的参数，对获取到的场景图像进行校正处理，以去除图像畸变。对校正后的图像使用特征提取算法，提取相邻帧与帧之间图像中的特征点，通过特征点之间的变化关系来估计出机器人的运动状态。再通过构建位姿图来描述载体机器人运动与传感器的观测信息之间的关系，使用图优化求取出更加精确的初始位姿，并通过g2o图优化求解库，计算该图优化问题，以求解出更加精确的机器人的位姿等信息，并在此基础上构建场景三维地图，最后融合惯性数据和视觉数据对机器人定位进行处理。

为了实现上述的目的，本实用新型采用如下技术方案予以实现。

一种机器人室内定位系统，包括可移动的机器人，依次相连设置在所述机器人上的惯性传感器，传感器控制模块和上位机，其特征在于：所述上位机还分别连接有设置在机器人上的视觉传感器；当所述机器人移动时，所述惯性传感器实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述视觉传感器实时采集运动过程中的场景图像；所述传感器控制模块与惯性传感器相连接，用于对惯性传感器进行运行控制以及数据的收发工作，所述上位机与传感器控制模块相连接，用于对所获取的数据进行处理，所述上位机与视觉传感器连接通信，用于获取环境图像信息并对信息进行处理，所述惯性传感器与视觉传感器用于对机器人的移动轨迹进行定位与跟踪；其中所述惯性传感器为整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，将它们的数据进行组合处理以达到九轴惯性传感器的效果的惯性传感器；所述传感器控制模块为ARM处理器、单片机或微处理器；所述视觉传感器为单目相机、双目相机、RGB-D相机或者其他可以获取场景图像的视觉设备。

本实用新型的一种机器人室内定位系统的定位方法，包括如下步骤：

步骤1、通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；具体方法是：

步骤1-1、所述上位机以采样频率Fs读取九轴惯性传感器数据，将读取的数据保存到csv文件中；

步骤1-2、将保存完成的csv文件导入上位机软件中，并对其中的三轴加速度值采用巴特沃兹低通滤波器进行滤波处理；

步骤2、使用四元数法描述传感器模块的输出值由载体坐标系转换到导航坐标系之间的旋转矩阵；具体方法是：

步骤2-1、在导航系统中选取三轴加速度计、三轴磁力计和三轴陀螺仪组成的惯性测量单元所在坐标系为载体坐标系b，该坐标系的原点为载体的重心，3个轴分别对应载体的横轴、纵轴和竖轴；选取地理坐标系为导航所参考坐标系，用n表示，使用四元数法表示n系至b系之间的旋转关系；

步骤2-2、采用四元数微分方程求解出旋转四元数更新方程；

步骤2-3、采用四元数更新方程求解出旋转矩阵更新方程；

步骤3、使用三轴加速度值和三轴磁场强度值修正三轴角速度值以获得更加准确的旋转矩阵；具体方法是：

步骤3-1、使用三轴加速度计数据，利用如下公式求得误差矩阵erra；

式中，vx、vy、vz为导航坐标系(n系)下的重力加速度通过旋转矩阵转换至载体坐标系(b系)后在三轴的计算值，ax、ay、az为载体坐标系中的传感器模块测得的重力加速度在三轴的输出值；

步骤3-2、使用三轴磁力计数据，利用如下公式求得误差矩阵errm；

式中，mx、my、mz为导航坐标系(n系)下的磁场强度值通过旋转矩阵转换至载体坐标系(b系)后在三轴的计算值，cx、cy、cz为载体坐标系中的传感器模块测得的磁场强度在三轴输出值；

步骤3-3、对误差矩阵erra和errm使用矩阵加法运算，可求得总的误差err；

步骤3-4、使用PI算法，利用如下公式，则可通过误差err去修正三轴陀螺仪的输出值；

gyro_new＝gyro+kp·err+ki·err·Δt

式中，gyro为三轴陀螺仪的输出值，gyro_new为修正后的三轴陀螺仪计算值，kp为PI算法中的比例增益记为，ki为PI算法中的积分增益，Δt为采样周期；

步骤3-5、将修正后的三轴陀螺仪数据代入上述步骤(2)中，利用四元数法则可获得更加准确的旋转矩阵；

步骤4、通过旋转矩阵将载体坐标系下的三轴加速度值转换到导航坐标系，对转换后的三轴加速度值进行积分运算，获得机器人移动过程中的速度信息；具体方法是：

步骤4-1、将载体坐标系上的三轴加速度值通过旋转矩阵转换到地理坐标系上；

步骤4-2、对三轴加速度值进行单位转换，去除重力影响后获得速度更新方程，去除积分漂移后修正速度更新方程，求解步骤如下：

步骤4-2-1、将三轴加速度值的单位转换为m/s²；

步骤4-2-2去除Z轴加速度值固有的1g垂直重力分量影响；

步骤4-2-3对去除重力影响后的三轴加速度值进行积分，获得速度更新方程；

步骤5、通过三轴加速度值判断机器人是否处于静止时期，即是否是零速度，利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息；具体方法是：

步骤5-1、设定阈值amin，利用三轴加速度值通过数值比较求取静止时期；

步骤5-2、利用如下公式，计算静止时期速度不为0产生的积分漂移d；

d＝dr*dt

式中，dr为漂移速率，dt为静止时期的长度，即漂移时长。

步骤5-4、将步骤4中所求的速度减去所求的积分漂移，则可求得修正后的速度更新方程；

步骤5-5、对修正后的速度再次进行积分运算，则可求得位移更新方程；

其特征在于还包括：

步骤6、对相机进行预处理，获取相机参数用于图像校正；

步骤7、提取校正后图像中的特征点，并进行特征匹配与去除外点；

步骤8、使用匹配后的特征点，通过特征点之间的几何关系解算出相机运动变换关系；

步骤9、通过获取到的运动变换关系构建位姿图；

步骤10、求解构建的位姿图优化问题，修正相机运动位姿，并重建场景地图；

步骤11、对惯性数据和视觉数据进行融合处理以完成对机器人的精确定位。

进一步优选的，步骤6中所述获取相机参数进行预处理的具体步骤是：

第一步：使用打印出的黑白棋盘网格图案纸张作标定板；

第二步：利用相机在不同角度拍摄标定板，保存拍摄的图像；

第三步：使用角点检测算法，检测图像中的角点，并保存角点坐标；

第四步：通过保存的角点坐标与原始标定板内角点的真实坐标之间的关系，进行相机参数标定；

第五步：利用标定出的相机参数进行图像校正，校正后的图像可用于后期的特征点提取步骤。

进一步优选的，步骤7中所述特征点的提取与匹配并去除外点的具体步骤是：

第一步：对校正后的图像使用特征点提取算法处理，提取图像中的特征点；

第二步：对两幅图像之间的特征点，使用特征点匹配算法进行处理，以获取图像间相匹配的特征点；

第三步：对匹配出的特点点进行误匹配剔除处理，去除匹配结果中的误匹配点。

进一步优选的，步骤8中所述相机运动变换关系的解算具体步骤是：

第一步：通过正确的匹配特征点之间的内在射影关系，构建它们之间的对极几何关系；

第二步：通过对极几何关系进行分析，可以分解出包含相机运动信息的基础矩阵和本质矩阵，；

第三步：根据基础矩阵和本质矩阵的数学关系，选择本质矩阵进行计算处理，可以求得相机运动。

进一步优选的，步骤9和步骤10中通过位姿图修正相机运动具体步骤是：

第一步：通过图优化算法构建相机运动与图像数据之间的约束关系，利用这个约束关系构建最小二乘关系式；

第二步：通过最小化该关系式，使相机运动估计值与真实值之间的误差最小；

第三步：不断迭代计算，求取误差最小时相机的运动信息，即包含相机位置和姿态的信息；

第四步：在获取最优相机运动信息基础上，使用三角测量法计算特征点的深度信息，以获得特征点的三维坐标，通过三维坐标点可以构建三维点云地图。

进一步优选的，步骤11中通过条件约束，对惯性数据求解出的机器人运动状态与通过视觉数据求解出的运动状态进行融合处理，以使得对机器人的定位更加精准。

与现有技术相比，本实用新型具有如下优点和有益效果：

(1)本实用新型结合惯性导航定位与视觉传感器的优势，完成室内机器人运动定位与跟踪，并能够对环境地图进行重建；

(2)本实用新型不但可以对机器人进行定位与跟踪，还能对场景进行三维重建，重建后的场景图可以用于对机器人运动的路径规划；

(3)本实用新型克服只使用惯性传感器的单一数据不可靠情况，具有较高实用价值。

附图说明

图1一种机器人室内定位系统的结构示意图；

图2一种机器人室内定位方法部分惯性数据定位流程示意图；

图3是计算旋转矩阵部分的流程图；

图4是对旋转矩阵进行修正部分流程图；

图5是计算速度更新方程部分流程图；

图6是对速度更新方程进行修正部分流程图；

图7是计算位移更新方程部分流程图；

图8是相机标定流程图；

图9是对极几何关系图；

图10是构建的位姿图。

具体实施方式

以下将结合附图，对本实用新型的技术方案进行详细说明。

参考图1，为本实用新型的一种机器人室内定位系统的结构示意图。该机器人室内定位系统，包括可移动的机器人，依次相连设置在所述机器人上的惯性传感器，传感器控制模块和上位机，其特征在于：所述上位机还分别连接有设置在机器人上的视觉传感器；当所述机器人移动时，所述惯性传感器实时产生三轴加速度值、三轴角速度值以及三轴磁场强度值，所述视觉传感器实时采集运动过程中的场景图像；所述传感器控制模块与惯性传感器相连接，用于对惯性传感器进行运行控制以及数据的收发工作，所述上位机与传感器控制模块相连接，用于对所获取的数据进行处理，所述上位机与视觉传感器连接通信，用于获取环境图像信息并对信息进行处理，所述惯性传感器与视觉传感器用于对机器人的移动轨迹进行定位与跟踪；其中所述惯性传感器为整合了三轴加速度计、三轴陀螺仪和三轴磁力计的九轴惯性传感器，或整合三轴加速度、三轴陀螺仪的六轴惯性传感器加上一个单独的三轴磁力计以组成九轴惯性传感器，或使用单独封装的三轴加速度计、三轴陀螺仪和三轴磁力计，将它们的数据进行组合处理以达到九轴惯性传感器的效果的惯性传感器；所述传感器控制模块为ARM处理器、单片机或微处理器；所述视觉传感器为单目相机、双目相机、RGB-D相机或者其他可以获取场景图像的视觉设备。

本方案以单目相机为例，通过USB将单目相机与上位机相连，用于实时接收机器人移动过程中所拍摄到的场景图像。

传感器控制模块可采用ARM处理器、单片机或微处理器，将传感器控制模块的输入端通过串口与惯性传感器输出端相连接，用于对惯性传感器的工作模式进行配置，并接收惯性传感器产生的数据，最后将接收到的数据通过USB传输到上位机。

上位机优先选用个人笔记本电脑，上位机中安装有对实验数据进行分析处理的软件。

机器人可选用任何一款可移动的机器人，本文使用美国Willow Garage公司出产的小型移动机器人turtlebot为例，该机器人小巧灵活，并且为上中下三层结构，下层为移动机器人底盘，中层和上层自带有可以载物的圆盘，三层之间通过支架支撑固定，可将惯性传感器和传感器控制模块固定在机器人的中层圆盘上，而个笔记本则放在上层圆盘上，视觉传感器则可以固定在笔记本上。

本实用新型的一种机器人室内定位系统的定位方法，参考图2，为本实用新型一种机器人室内定位系统的定位方法的惯性数据定位部分的流程示意图；参考图3，为计算旋转矩阵部分的流程图；参考图4，为旋转矩阵进行修正部分流程图；参考图5，为计算速度更新方程部分流程图；参考图6，为速度更新方程进行修正部分流程图；参考图7，为计算位移更新方程部分流程图；参考图8，为相机标定流程图；参考图9，为特征点之间的对极几何关系图；参考图10，为构建的位姿图，具体包括如下步骤：

(1)通过上位机获取机器人移动过程中产生的九轴惯性数据，并对数据进行预处理，消除数据中的噪声干扰；

具体包括如下内容：

a)将九轴惯性传感单元固定在机器人上，上位机以采样频率Fs读取机器人移动过程中九轴惯性传感单元产生的数据，并将数据保存为csv文件。

b)使用MATLAB软件读取保存的csv文件，为了能够通过三轴加速度值检测出机器人移动过程中的静止时期，需要对三轴加速度值进行滤波预处理。为了能够实时观察到机器人所处的环境，控制机器人移动时速度介于正常人行走速度和跑步速度之间，而正常行走时人的步频处于1Hz至2.4Hz之间,跑步时步频最高为3.5Hz,为了能够精确的检测出机器人在移动过程中所有的静止时期，采用通带截止频率为3Hz,阻带截止频率为10Hz的巴特沃斯低通滤波器对三轴加速度值进行滤波处理，该滤波器的滤波窗口如图2所示。

(2)使用四元数法来描述传感器值由载体坐标系转换到导航坐标系之间的旋转矩阵；

具体步骤如下：

第一步：通常在导航系统中选取三轴加速度计、三轴磁力计和三轴陀螺仪组成的惯性测量单元所在坐标系为载体坐标系b，该坐标系的原点为载体的重心，3个轴分别对应载体的横轴、纵轴和竖轴；选取地理坐标系为导航所参考坐标系，一般用n表示。使用四元数法可以很方便的表示n系至b系之间的转换关系；

式中，为n系至b系的旋转矩阵表示为：

第二步：采用四元数微分方程可以求解出旋转四元数；

其中表示四元数乘法，为载体坐标系b相对于导航坐标系n的三轴角速度。假设采样周期为Δt，那么四元数q的更新方程为:

通过四元数q的更新方程可以达到对旋转矩阵进行更新的目的。

(3)使用三轴加速度计和三轴磁力计的输出值去修正三轴陀螺仪的输出值以获得更加准确的旋转矩阵；

具体修正步骤如下：

假设重力加速度向量在导航坐标系(n系)中的理论输出值为(上标^表示单位化处理)，通过旋转矩阵变换后在载体坐标系(b系)下的值表示为同时载体坐标系中的三轴加速度计测得的重力加速度的值表示为假设准确，则但是由于存在误差所以使用该误差来修正对两个向量做叉积，则有误差矩阵：

假设n系中磁场强度为经过旋转矩阵变换后在b系中的输出值为而载体坐标系中三轴磁力计测得的磁场强度为则有误差矩阵：

则总的转换误差为：

err＝erra+errm (7)

设补偿后的三轴陀螺仪计算值记为gyro_new，PI算法中的比例增益记为kp，积分增益记为ki，则有：

gyro_new＝gyro+kp·err+ki·err·Δt (8)

将修正后的三轴陀螺仪计算值代入式(3)中，则可对旋转四元数进行修正，从而对旋转矩阵进行修正。

(4)通过旋转矩阵将载体坐标系下的三轴加速度值转换到导航坐标系，对转换后的三轴加速度值进行积分运算，从而获得机器人移动过程中的速度信息。

具体步骤如下：

第一步：将载体坐标系上的三轴加速度值通过步骤(3)获得修正后的旋转矩阵的逆变换矩阵转换到地理坐标系上，则有：

式中：an为地理坐标系下的三轴方向上的加速度输出值，ab为载体坐标系下三轴方向上的加速度输出值。为载体坐标系(b系)到地理坐标系(n系)的旋转矩阵，该旋转矩阵的四元数表示为：

第二步：对an进行单位转换，去除重力影响后获得速度更新方程，去除积分漂移修正速度更新方程。求解步骤如下：

步骤①：将an的单位转换为m/s²，转换单位后的三轴加速度值输出记为acc，则有：

acc＝an*9.81 (11)

步骤②：去除Z轴固有的1g垂直重力分量影响，将acc中Z轴的加速度计输出值记为accz，则有：

accz＝accz-9.81 (12)

步骤③：对去除重力影响后的三轴加速度值进行积分，获得速度更新方程，则有：

vel(t)＝vel(t-1)+acc(t)·Δt (13)

式中，Δt为采样周期，acc(t)为t时刻的三轴加速度值，vel(t)为所求的t时刻的速度值。

(5)通过三轴加速度值判断机器人是否处于静止时期，即是否是零速度。利用这个静止时期对速度进行修正，对修正后的速度再次积分运算，则可获得机器人移动过程中的位移信息。具体步骤如下：

第一步：通过三轴加速度值判断静止时期。求解步骤如下：

设t时刻x、y、z三轴加速度值分别为accx(t)、accy(t)、accz(t)，那么t时刻的三轴加速度值矢量和为：

设定一个最小阈值amin，当t时刻的三轴加速度值矢量和模值为0或小于等于这个阈值时则认为此时机器人处于静止时期；用stationary表示静止时期，当满足静止时期检测标准时stationary为1，其他情况下stationary为0，那么则有：

第二步：计算静止时期速度不为0产生的积分漂移d。求解步骤如下：

d＝dr*dt (16)

式中，dr为漂移速率。dt为静止时期的长度，即漂移时长。i和j分别为静止时期的开始与截止。velstationary(j-1)为静止前的一次采样速度。

第三步：利用积分漂移修正步骤(4)中所求的速度，修正后的t时刻的速度记为v(t)，则有：

上式中，当stationary为1即静止时期时此时的速度置为0，其他时期则根据更新公式计算速度的值。

第四步：对修正后的速度再次进行积分运算，则t时刻位移pos(t)更新方程：

pos(t)＝pos(t-1)+v(t)·Δt (19)

(6)对相机进行标定处理，将标定后的相机内外参数用于校正获取的图像。具体步骤如下：

第一步：本案列标定相机使用张正友标定法为例，采用黑白象棋网格图案来制作标定板，使用7×9的正方形黑白棋盘网格，每个网格的真实大小为30mm×30mm，棋盘中角点的总数量为48个，单目相机的分辨率为752×480像素；

第二步：为降低标定板的使用难度，将黑白棋盘网格打印在纸上后固定在墙上，从不同角度和方向控制相机拍摄至少20张该纸张图像，并保存；

第三步：使用角点检测算法，检测图像中的角点并进行标记，保存角点的坐标信息；

第四步：使用保存的角点坐标估计值与角点坐标真实值之间的关系，进行相机标定，可标定出相机的内参矩阵和径向畸变与切向畸变参数；

第五步：使用标定出的参数作用于单目相机拍摄的场景图像，可对图像进行校正处理以去除图像中的畸变情况。

(7)对校正后的图像进行特征点提取与匹配并去除外点。具体步骤如下：

第一步：常使用的特征点提取算法有SIFT、SURF、FAST和ORB算法，使用任一算法都可以对图像进行特征点提取；

第二步：使用快速近似最近邻搜索库对帧与帧之间图像的特征点进行特征点匹配；

第三步：使用随机抽样一致算法可以剔除匹配出的特征点中的误匹配点即外点。

(8)通过步骤(7)中获取的正确匹配点对来获取相机的运动变换关系。具体步骤如下：

第一步：通过帧与帧之间图像中匹配正确的特征点，通过内在射影关系可以构建如图9所示的对极几何关系：

两帧图像所在的图像平面分别为Il、Ir，由Il帧运动到Ir帧之间需要经过一个旋转变换R以及平移变换t。Ol、Or分别为相机中心在不同帧图像下所处的位置。假设Pl为Il平面内的一个特征点，通过特征匹配算法匹配到对应点，在Ir平面内所处的位置为Pr。假设Pl和Pr之间匹配正确，不存在误匹配，那么可以知道Pl和Pr是同一个空间点在两个成像平面上的投影，此时连接线段和延长两个线段交汇于点P，点P、Ol、Or三个点所构成的平面称为极平面。OlOr连线称为基线，OlOr连线相交于平面Il、Ir于点el、er，点el、er被称为极点，射线和分别为极平面与成像平面Il、Ir之间的交线。

第二步：对极几何中包含基础矩阵与本质矩阵，从而恢复出两帧图像之间相机的运动变换关系，该关系中包含旋转变换R以及平移变换t。求解步骤如下：

步骤①：假设空间点P在Il平面所在坐标系下的坐标为：

P＝[X,Y,Z]^T (20)

步骤②：单目相机通常使用针孔相机模型，通过该模型可以计算出两个特征点Pl和Pr的像素位置，通过齐次化处理后为：

上式中，K为通过步骤(6)标定后的相机内参矩阵，R和t为两帧之间的相机所作运动。

步骤③：Pl和Pr的像素坐标进行归一化处理后的坐标设为c1和c2，那么：

步骤④：将式(22)代入式(21)，得：

c2＝Rc1+t (23)

步骤⑤：将t的反对称矩阵记为t^。式(23)两侧同时左乘t^，则得：

t^c2＝t^Rc1 (24)

步骤⑥：式(24)两侧同时都左乘得：

由于Ol、P、Or共面，则上式的左侧根据垂直向量的乘积结果为0，得：

步骤⑦：将式(22)代入上式，得：

步骤⑧：式(26)和式子(27)为对极约束的两种形式，对极约束中包含旋转和平移关系。将基础矩阵，记为F，本质矩阵，记为E，则有：

由式(28)可知，基础矩阵F和本质矩阵E之间只相差一个相机的内参矩阵K，而相机的内参矩阵已经通过通过标定获得的，因此通过对本质矩阵的解算可以获得相机旋转运动矩阵R和平移运动向量t。

第三步：对本质矩阵进行解算，可以获取相机旋转运动矩阵R和平移运动向量t。具体步骤如下：

本质矩阵E为一个3×3的矩阵，其中包含9个待求解的未知参数。本质矩阵E的解具有内在性质：经过奇异值分解(SVD)后，解的形式必定为[σ,σ,0]^T。由于单目相机无法确定尺度信息，即本质矩阵E乘以任意一个非零常数后依然满足它的定义式，这个常数可以看成不同尺度下的缩放因子，那么可以直接将本质矩阵的奇异值设为(1,1,0)，那么E中就只剩8个未知数。设E的SVD分解为：

E＝UΣV^T (29)

式(29)中，U、V为正交矩阵，Σ为奇异值矩阵，由上文可知，Σ＝diag(1,1,0)。通过SVD分解后，对于上式中E的解，可以有两组可能的R和t的解：

式(30)中，RZ(π/2)表示旋转矩阵R围绕Z轴做了90°的旋转所得到的矩阵。由于对于t和-t所得到的本质矩阵E和-E是等价的，所以，从E中分解R和t可以得到4组可能的解，然而任意一空间点P在相机成像平面的前方，因此只需要通过分解出的R和t，计算出空间点与相机之间的深度值，若通过某一组R和t求解出的所有空间点的深度值都为大于0的值，那么这组解就是所需要的R和t。

(9)通过步骤(8)求取帧与帧之间的运动变换关系后，通过构建位姿图来描述载体机器人或者相机的运动与相机传感器的观测信息之间的关系，以此来约束两帧之间的相机运动，以获取更加精确的运动变换关系。具体步骤如下：

图优化是通过图论原理去描述非线性优化问题，非线性优化问题可以使用图的方式来表示，图可以由若干个顶点，以及与这些顶点相连的若干条边组成。位姿图中节点可以为每帧图像对应的相机位姿，或者通过变换矩阵计算求得的机器人位姿，而边可以用两帧之间的运动变换的约束信息来表示。假设Pi为一组通过动态的变换矩阵计算求得的动态位姿序列，该变换矩阵由一个3×3的旋转矩阵R和一个3×1的平移向量t组成，因此该位姿序列点具有6个自由度。假设相邻两个位姿i-1和i之间的变换矩阵用表示，那么点Pi-1变换到Pi可用公式求得。对于一组连续的图像帧，假设初始帧对应的相机位姿用P0表示，之后的任意一帧k相机对应的位姿Pk可以用下面这个式子计算：

构建的位姿图示意图如图10所示，具体的构建步骤如下所示：

步骤①：选择一个初始位姿P0，将其添加到位姿图中，作为图中的起始节点；

步骤②：利用公式(31)在通过前端处理后知道第一帧和第二帧之间的运动变换矩阵T0¹的基础上，求得此时的机器人位姿P1，将它加入到位姿图中作为第二个节点，并且通过该变换矩阵建立第一节点与第二个节点之间的变换约束关系，建立第一条边E0；

步骤③：同理，将获得的第二帧和第三帧之间的运动变换矩阵T1²，再次由公式(31)计算出第三帧对应的机器人位姿P2，将其加入位姿图中建立第三个节点，并以运动变换矩阵T1²建立第二个节点与第三个节点之间的约束关系作为位姿图中的第二条边E1；

步骤④：以此类推，将获得的第N-1帧和第N帧之间的运动变换矩阵由公式(31)计算出第N帧对应的机器人位姿PN，将其加入位姿图中建立第N个节点，再以运动变换矩阵建立第N-1个节点与第N个节点之间的约束关系作为位姿图中的第N条边EN-1。

(10)步骤(9)构建的位姿图的实质是最小二乘问题，通过g2o图优化库求解该最小二乘问题，以最小化位姿误差，求解更加精确的位姿信息。具体步骤如下：

第一步：使用g2o进行位姿图优化处理：

g2o为通用图优化开源框架，为图优化问题提供所需的操作并能够对位姿图中的节点位置进行优化调整。该开源框架集成了大部分的最小二乘问题的迭代求解算法，适用于绝大多数的非线性优化问题。

调用g2o对图优化问题进行优化求解，有以下5步：

步骤①：选择或自主定义合适的顶点和边的类型；

步骤②：构建位姿图；

步骤③：选择g2o中的线性方程求解器；

步骤④：选择g2o中的矩阵块求解算法和迭代策略；

步骤⑤：不断迭代计算直到求解出优化结果。

通过g2o框架对位姿图进行优化求解，可以对最终求解的节点位姿进行一个调整，使求解出的位姿精确和具有鲁棒性。

第二步：通过三角测量原理来估计相机的深度，从而确定特征点的三维坐标，当获得大量特征点的三维坐标后通过PCL点云库则可以绘制三维点云场景地图。具体步骤如下：

假设相机平面Il下建立相机坐标系I1，选定I1坐标系为参考世界坐标系，此时空间点P在此坐标系下的坐标设为P＝(X,Y,Z,1)^T，相机坐标系与世界坐标系之间的转换存在一个变换矩阵T，T为相机的外参矩阵，内部包含g2o优化后的旋转矩阵R和平移向量t。由于是以相机坐标系作为参考世界坐标系，因此两个坐标系之间无旋转和平移发生，定义第一帧图像两个坐标系之间的外参矩阵为单位矩阵，记为T0，得：

T0为固定且标准的外参矩阵。同理，相机坐标系I2以相机坐标系I1为参考坐标系，那么此时I2坐标系下的外参矩阵记为T，由于T相对于标准的外参矩阵T0做了相对的旋转和平移，可以表示为：

由点Pl和Pr与空间点P的投影关系，得：

上式中，T和T′分别为在两个相机坐标系下通过本质矩阵解算出来的相对于标准外参矩阵T0的外参矩阵，再结合特征点匹配算法所求得的Pl和Pr两点的二维坐标，则可通过式中对应关系解算出点P在世界坐标系的三维坐标。再通过PCL点云库则可以绘制可视化的场景点云图。

(11)通过条件约束，对惯性数据与视觉数据的求解的机器人运动定位结果进行融合处理。具体约束条件如下：

步骤①：当机器人静止时，使用惯性数据求解出的定位结果，并使用静止检测算法检测机器人静止状态，舍弃此时的相机求解数据；

步骤②：当机器人运动时，若拍摄的图像中提取的特征点丰富，且正确的匹配特征点数量与所有特征点之间的比值大于设定的阈值，则对惯性数据求解的机器人运动位姿与视觉数据求解的运动位姿进行加权平均处理，具体的加权比例根据实际情况进行设定，使用加权平均后的位姿对机器人进行定位解算；

步骤③：当机器人运动时，若拍摄的图像中提取的特征点稀少，或正确的匹配特征点数量与所有特征点之间的比值小于设定的阈值，则舍弃视觉数据使用惯性数据对机器人进行定位解算。

以上实施案例仅为说明本实用新型的技术思想，不能以此限定本实用新型的保护范围，凡是按照本实用新型提出的技术思想，在技术方案基础上所做的任何改动，均落入本实用新型保护范围之内。