CN110345944A - 融合视觉特征和imu信息的机器人定位方法 - Google Patents

Abstract

融合视觉特征和IMU信息的机器人定位方法，本发明提出了一种单目视觉与IMU融合的方法。首先视觉前端位姿跟踪采用特征点估计机器人位姿，并利用纯视觉信息对IMU偏差模型，绝对尺度和重力加速度方向进行估计。同时IMU解算得到的高精度位姿信息为优化搜索过程提供初始参照，并作为状态量和视觉导航信息一起参与优化。后端采用了基于滑动窗口的紧耦合非线性优化方法，实时优化位姿和地图，且滑动窗口法计算里程计时候运算复杂度保持固定，提高了算法鲁棒性。

Description

融合视觉特征和IMU信息的机器人定位方法

技术领域

本发明涉及一种机器人室内视觉定位方法，具体是一种融合视觉特征和IMU 信息的机器人定位方法。

背景技术

移动机器人同时定位与地图构建(Simultaneous Localization and Mapping)问题，就是让机器人在环境未加或不确定的情况下，利用自身配备的传感器感知外界信息，对自身位置进行定位，并在此基础上进行地图构建。

视觉定位是当下机器人SLAM研究的热点。视觉定位方法使用所获取的图像序列，通过提取图像特征并根据特征点的运动变化在不同帧中执行特征匹配来提取移动机器人的运动参数。单目SLAM存在初始化的尺度问题和追踪的尺度漂移问题，在复杂的环境下，为了填补单目视觉的缺陷，同时提高单目视觉***的鲁棒性，通常需耍与其它传感器一起使用。惯性导航数据稳定，但积累误差更为严重，这两种技术的融合可以将单目视觉SLAM的高精度与惯性导航数据的稳定性结合起来，取长补短，达到满足定位精度的目的。陈熙源提出一种使用扩展卡尔曼滤波器来融合摄像机和IMU数据进行摄像机姿态跟踪的方法(陈熙源.一种采用迭代扩展卡尔曼滤波与神经网络的惯性/视觉组合导航方法[P].江苏：CN103983263A，2014-08-13.)。但是在跟踪情况下没有使用IMU的先验数据，因此***极易不收敛，导致定位精度较差。王强等提出视觉惯性里程计的实现方法，利用当前图像特征点与地图扩展模块所维护的三维地图点对应的空间约束关系、图像帧之间特征匹配约束关系和图像帧间IMU的约束信息，计算出每一帧图像对应的设备的位置和姿态(王强.视觉惯性里程计的实现方法及***[p].上海： CN108489482A，2018-09-04.)。然而，此方法需要不断地利用视觉算法确定相机的位姿，计算量较大。Qin Tong等提出的基于单目相机和低成本的IMU传感器的六自由度状态估计算法，视觉方面采用的是稀疏光流法进行跟踪定位，论文中提出了一种鲁棒性的视觉惯性联合初始化过程和恢复过程(Tong Q，Peiliang L，Shaojie S.VINS-Mono：A Robust and Versatile Monocular Visual-Inertial StateEstimator[J].IEEE Transactions on Robotics，2018：1-17.)。然而，整个***采用的是最基本的参考帧跟踪模型，无法克服复杂的运动环境。

发明内容

为了克服现有方法上的缺点，本发明提出了一种单目视觉与IMU融台的机器人定位方法。

本发明首先视觉前端位姿跟踪采用特征点估计机器人位姿，并利用纯视觉信息对IMU偏差模型，绝对尺度和重力加速度方向进行估计。同时IMU解算得到的高精度位姿信息为优化搜索过程提供初始参照，并作为状态量和视觉导航信息一起参与优化。后端采用了基于滑动窗口的紧耦合非线性优化方法，实时优化位姿和地图，且滑动窗口法计算里程计时候运算复杂度保持固定，提高了算法鲁棒性。本发明实现了单目视觉和惯导信息融合的机器人SLAM，能够对机器人运动和周围环境进行精确的估计。

融合视觉特征和IMU信息的机器人定位方法，具体步骤如下：

步骤1：使用单目相机进行位姿估计；

本发明首先对单目相机捕获的图像帧，使用ORB特征提取算法，提取丰富的ORB特征点，根据两帧图像之间特征点的像素位置，利用多目几何知识恢复出单目相机的运动过程，估计机器人位姿。

假设某空间点坐标为P_i＝[X_i，Y_i，Z_i]^T，其投影的像素坐标为U_i＝[u_i，v_i]^T，像素位置与空间点位置的关系如下：

s_iU_i＝K exp(ξ^)P_i (1)

上式中，s_i表示深度图的尺度因子，K代表相机的内参矩阵，ξ为相机位姿的李代数表示，exp表示李代数的指数映射。最后通过构建最小二乘问题，计算相机最优位姿，使其误差最小化。计算公式如下：

步骤2：使用IMU进行位姿估计；

根据IMU提供的数据信息，利用基于预积分的IMU动力学模型建立相机的运动模型，采用运动模型对机器人位姿进行实时初步估计。

IMU可以输出加速度a_B和角速度ω_B，定义世界坐标系为W，IMU的参考坐标系为B，通过IMU预积分，根据其在k时刻下的状态，推算出其在k+1时刻下的状态。预积分后，IMU的方向、速度和位置分别更新为：

其中，表示IMU坐标系B在k+1时刻相对于世界坐标系的旋转矩阵；表示IMU坐标系B在k时刻相对于世界坐标系的旋转矩阵；表示IMU坐标系B在k+1时刻相对于世界坐标系的速度；exp表示李代数的指数映射；Δt表示IMU的采样间隔；表示IMU坐标系B在k时刻相对于世界坐标系的速度；g_w表示当前的重力加速度；表示IMU坐标系B在k+1时刻相对于世界坐标系的位移；表示IMU坐标系B在k时刻相对于世界坐标系的位移；ba 与b_g分别表示IMU中陀螺仪和加速度计的偏差。

步骤3：视觉惯性联合初始化；

将步骤1中获得的纯视觉数据和步骤2中获得的IMU数据相结合，并对IMU 偏差模型，绝对尺度和重力加速度方向进行估计。

首先进行陀螺仪偏差估计，根据视觉信息求得的关键帧之间的旋转，对比利用IMU预积分模型求得的旋转，以偏差为变量，最小化两者的差值即可得到关于陀螺仪的偏差：

其中，N代表关键帧的个数； 是由视觉SLAM算法计算得到的相机坐标系相对于世界坐标系的旋转，R_CB是标定矩阵；ΔR_i，i+1为、连续两个关键帧间的陀螺仪积分所得到的旋转矩阵，为ΔR_i，i1随陀螺仪偏差变化方程的一阶近似结果；最后通过高斯-牛顿法求解b_g。

然后，利用上述陀螺仪偏差估计的结果，对尺度和重力加速度进行估计。对于三个相连接的关键帧之间的两两关系，利用两两之间的预积分测量值，得到线性方程：

记第i，i+1，i+2时刻的关键帧标号为1，2，3，则上式各项分别为：

在上式中，表示相机中心C在世界坐标系下的位置，Δt为帧间时间差， I为单位矩阵，R_WC表示相机坐标系C相对于世界坐标系的旋转矩阵；R_WB表示 IMU坐标系B在相对于世界坐标系的旋转矩阵；Δv表示帧间速度；S和g_w分别为所求的尺度和重力加速度估计。

最后，利用尺度和重力加速度估计的结果，对IMU加速度偏差进行估计。考虑3个连续关键帧之间的线性关系，可得：

记第i，i+1，i+2时刻的关键帧标号为1，2，3，则φ(i)，ζ(i)和ψ(i)的求解过程如下：

上式中[]_{(：，1：2)}表示使用矩阵前两列数据；R_WI表示惯性***在世界坐标系的方向；表示在惯性坐标系中的重力方向；R_WC表示相机坐标系C相对于世界坐标系的旋转矩阵；R_WB表示IMU坐标系B在相对于世界坐标系的旋转矩阵；为第2、3关键帧的ΔP_2，3随加速度变化方程的一阶近似结果，为第2、3关键帧的Δv_2，3随加速度变化方程的一阶近似结果Δt为帧间时间差，I为单位矩阵，s 和δθ_xy为需要估计的尺度因子和重力加速度方向角，b_a是加速度偏差。

步骤4：使用滑动窗口法对位姿进行优化；

通过重投影误差与IMU的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和***状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值。***状态向量X表示如下：

其中x₀，x₁，·x_n表示滑动窗口内的n+1个所有相机的状态，分别表示i时刻下IMU坐标系相对于世界坐标系的旋转，速度和位移，b_a，b_g分别表示加速度计偏差和陀螺仪偏差。构造目标函数：

其中为IMU的测量误差，为单目相机的测量误差，B表示IMU 的测量数据集，C表示单目相机的测量数据集，为IMU坐标系下滑动窗口从第i个关键帧到j个关键帧的预积分噪声项协方差，为相机坐标系下滑动窗口第j个关键帧中第l个特征点的噪声协方差。

IMU第i帧和第j帧之间的位移，速度，旋转和IMU偏差的测量残差表示为：

上式中分别表示平移，速度，旋转和IMU偏差带来的误差项；为两帧间旋转变换ΔR相对于重力加速度的一阶近似；为两帧间速度变化Δv相对于重力加速度的一阶近似；为两帧间位移变化Δp相对于重力加速度的一阶近似；为两帧间速度变化Δv相对于加速度的一阶近似；为两帧间位移变化Δp相对于加速度的一阶近似。

视觉残差是重投影误差，对于第l个路标点P，将P从第一次观看到它的第 i个相机坐标系，转换到当前的第j个相机坐标系下的像素坐标，其中视觉误差项为：

上式中，为正切平面上的任意两个正交基；是估计第l个路标点在第j 个相机归一化相机坐标系中的可能坐标；为第l个路标点在第j个相机归一化相机坐标系中的观察到的坐标；和表示第l个路标点在第j个相机中的像素坐标；表示IMU坐标系相对于相机坐标系的位姿变换；表示从世界坐标系到IMU坐标系的位姿变换。

优选地，步骤3中，将步骤1中获得的纯视觉数据和步骤2中获得的IMU 数据相结合，并对IMU偏差模型，绝对尺度和重力加速度方向进行估计。

优选地，步骤4中，使用滑动窗口法对位姿进行优化通过重投影误差与IMU 的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和***状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值。

本发明的优点是：本发明设计的融合视觉特征和IMU信息的机器人定位方法，一方面由于该算法跟踪过程中通过重投影误差与IMU的预积分，建立相邻关键帧之间的约束关系，相比于没有使用IMU先验数据的扩展卡尔曼滤波算法，算法鲁棒性更强，精度更高。另一方面，在融合过程中采用了基于滑动窗口法的后端优化算法，实时优化位姿和地图，且滑动窗口法计算里程计时候运算复杂度保持固定状态，使整个***运行起来更加稳定，提高了鲁棒性。

附图说明

图1是本发明的方法流程图。

图2是本发明的实验平台。

图3是本发明的定位结果展示图。

具体实施方式

下面结合附图进一步说明本发明的技术方案。

融合视觉特征和IMU信息的机器人定位方法，如图1所示，平台组成主耍包括联想Thinkpad电脑1、EAIBOT移动小车2、MYNTEYE惯导相机3。MYNT EYE惯导相机3置于EAIBOT移动小车2上方，两者通过USB连接；Thinkpad 电脑1通过终端远程控制EAIBOT移动小车2。

结台图2，本发明方法的具体实施方式如下：

将MYNT EYE惯导相机与EAIBOT移动小车通过USB相连后，开启联想 thinkpad，输入相机运行指令，启动算法。实例中相机的采样频率为30HZ，关键帧的采样频率更低，IMU的采样频率为100HZ。

步骤1：本发明首先对MYNT EYE相机捕获的图像帧，使用ORB特征提取算法，提取丰富的ORB特征点，根据两帧图像之间特征点的像素位置，利用多目几何知识恢复出单目相机的运动过程，估计机器人位姿。

假设某空间点坐标为P_i＝[X_i，Y_i，Z_i]^T，其投影的像素坐标为U_i＝[u_i，v_i]^T，像素位置与空间点位置的关系如下：

s_iU_i＝K exp(ξ^)P_i (1)

上式中，s_j表示深度图的尺度因子，K代表相机的内参矩阵，ξ为相机位姿的李代数表示。最后通过构建最小二乘问题，计算相机最优位姿，使其误差最小化。计算公式如下：

步骤2：根据MYNT EYE的IMU提供的数据信息，利用基于预积分的IMU 动力学模型建立相机的运动模型，采用运动模型对机器人位姿进行实时初步估计。

定义世界坐标系为W，IMU的参考坐标系为B，通过IMU预积分，根据其在k时刻下的状态，推算出其在k+1时刻下的状态。预积分后，IMU的方向、速度和位置分别更新为：

其中，a_B和ω_B分别为IMU输出的加速度和角速度，表示IMU坐标系B在 k+1时刻相对于世界坐标系的旋转矩阵；表示IMU坐标系B在k时刻相对于世界坐标系的旋转矩阵；表示IMU坐标系B在k+1时刻相对于世界坐标系的速度；Exp表示李代数的指数映射；Δt表示IMU的采样间隔；表示 IMU坐标系B在k时刻相对于世界坐标系的速度；g_w表示当前的重力加速度；表示IMU坐标系B在k+1时刻相对于世界坐标系的位移；表示IMU 坐标系B在k时刻相对于世界坐标系的位移；b_a与b_g分别表示IMU中陀螺仪和加速度计的偏差。

步骤3：通过键盘控制小车朝一个方向移动一段距离，使相机进入初始化过程。将步骤1中获得的纯视觉数据和步骤2中获得的IMU数据相结合，并对IMU 偏差模型，绝对尺度和重力加速度方向进行估计。

首先进行陀螺仪偏差估计，根据视觉信息求得的关键帧之间的旋转，对比利用IMU预积分模型求得的旋转，以偏差为变量，最小化两者的差值即可得到关于陀螺仪的偏差：

其中，N代表关键帧的个数； 是由视觉SLAM算法计算得到的相机坐标系相对于世界坐标系的旋转，R_CB是标定矩阵；ΔR_i，i+1为连续两个关键帧间的陀螺仪积分所得到的旋转矩阵，为ΔR_i，i+1随陀螺仪偏差变化方程的一阶近似结果；最后通过高斯-牛顿法求解b_g。

然后，利用上述陀螺仪偏差估计的结果，对尺度和重力加速度进行估计。对于三个相连接的关键帧之间的两两关系，利用两两之间的预积分测量值，得到线性方程：

记第i，i-1，i-2时刻的关键帧标号为1，2，3，则上式各项分别为：

在上式中，表示相机中心C在世界坐标系下的位置，Δt为帧间时间差，I为单位矩阵，R_WC表示相机坐标系C相对于世界坐标系的旋转矩阵；R_WB表示IMU 坐标系B在相对于世界坐标系的旋转矩阵；Δv表示帧间速度；S和g_W分别为所求的尺度和重力加速度估计。

最后，利用尺度和重力加速度估计的结果，对IMU加速度偏差进行估计。考虑3个连续关键帧之间的线性关系，可得：

记第i，i+1，i+2时刻的关键帧标号为1，2，3，则φ(i)，ζ(i)和ψ(i)的求解过程如下：

上式中[]_{(：，1：2)}表示使用矩阵前两列数据；R_WI表示惯性***在世界坐标系的方向；表示在惯性坐标系中的重力方向；R_WC表示相机坐标系C相对于世界坐标系的旋转矩阵；R_WB表示IMU坐标系B在相对于世界坐标系的旋转矩阵；为第2、3关键帧的ΔP_2，3随加速度变化方程的一阶近似结果，为第2、3关键帧的Δv_2，3随加速度变化方程的一阶近似结果Δt为帧间时间差，I为单位矩阵，s 和δθ_xy为需要估计的尺度因子和重力加速度方向角，b_a是加速度偏差。

步骤4：通过重投影误差与IMU的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和***状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值。***状态向量X表示如下：

其中x₀，x₁，…x_n表示滑动窗口内的n+1个所有相机的状态，分别表示i时刻下IMU坐标系相对于世界坐标系的旋转，速度和位移，b_a，b_g分别表示加速度计偏差和陀螺仪偏差。构造目标函数：

其中为IMU的测量误差，为单目相机的测量误差，B表示IMU 的测量数据集，C表示单目相机的测量数据集，为IMU坐标系下滑动窗口从第i个关键帧到j个关键帧的预积分噪声项协方差，为相机坐标系下滑动窗口第j个关键帧中第l个特征点的噪声协方差。

IMU第i帧和第j帧之间的位移，速度，旋转和IMU偏差的测量残差表示为：

上式中分别表示平移，速度，旋转和IMU偏差带来的误差项；为两帧间旋转变换ΔR相对于重力加速度的一阶近似；为两帧间速度变化Δv相对于重力加速度的一阶近似；为两帧间位移变化Δp相对于重力加速度的一阶近似；为两帧间速度变化Δv相对于加速度的一阶近似；为两帧间位移变化Δp相对于加速度的一阶近似。

视觉残差是重投影误差，对于第l个路标点P，将P从第一次观看到它的第 i个相机坐标系，转换到当前的第j个相机坐标系下的像素坐标，其中视觉误差项为：

上式中，为正切平面上的任意两个正交基；是估计第l个路标点在第j 个相机归一化相机坐标系中的可能坐标；为第l个路标点在第j个相机归一化相机坐标系中的观察到的坐标；和表示第l个路标点在第j个相机中的像素坐标；表示IMU坐标系相对于相机坐标系的位姿变换；表示从世界坐标系到IMU坐标系的位姿变换。

最后本发明实时输出小车当前时刻下的位姿变换信息和周围环境地图，如图 3所示。

本说明书实施例所述的内容仅仅是对发明构思的实现形式的列举，本发明的保护范围不应当被视为仅限于实施例所陈述的具体形式，本发明的保护范围也及于本领域技术人员根据本发明构思所能够想到的等同技术手段。

Claims (3)

1.融合视觉特征和IMU信息的机器人定位方法，具体步骤如下：

步骤1：使用单目相机进行位姿估计；

首先对单目相机捕获的图像帧，使用ORB特征提取算法，提取丰富的ORB特征点，根据两帧图像之间特征点的像素位置，利用多目几何知识恢复出单目相机的运动过程，估计机器人位姿；

假设某空间点坐标为P_i＝[X_i，Y_i，Z_i]^T，其投影的像素坐标为U_i＝[u_i，v_i]^T，像素位置与空间点位置的关系如下：

s_iU_i＝Kexp(ξ^)P_i (1)

上式中，s_i表示深度图的尺度因子，K代表相机的内参矩阵，ξ为相机位姿的李代数表示，exp表示李代数的指数映射；最后通过构建最小二乘问题，计算相机最优位姿，使其误差最小化；计算公式如下：

步骤2：使用IMU进行位姿估计；

根据IMU提供的数据信息，利用基于预积分的IMU动力学模型建立相机的运动模型，采用运动模型对机器人位姿进行实时初步估计；

IMU可以输出加速度α_B和角速度ω_B，定义世界坐标系为W，IMU的参考坐标系为B，通过IMU预积分，根据其在k时刻下的状态，推算出其在k+1时刻下的状态；预积分后，IMU的方向、速度和位置分别更新为：

其中，表示IMU坐标系B在k+1时刻相对于世界坐标系的旋转矩阵；表示IMU坐标系B在k时刻相对于世界坐标系的旋转矩阵；表示IMU坐标系B在k+1时刻相对于世界坐标系的速度；exp表示李代数的指数映射；Δt表示IMU的采样间隔；表示IMU坐标系B在k时刻相对于世界坐标系的速度；g_w表示当前的重力加速度；表示IMU坐标系B在k+1时刻相对于世界坐标系的位移；表示IMU坐标系B在k时刻相对于世界坐标系的位移；b_a与b_g分别表示IMU中陀螺仪和加速度计的偏差；

步骤3：视觉惯性联合初始化；

将步骤1中获得的纯视觉数据和步骤2中获得的IMU数据相结合，并对IMU偏差模型，绝对尺度和重力加速度方向进行估计；

首先进行陀螺仪偏差估计，根据视觉信息求得的关键帧之间的旋转，对比利用IMU预积分模型求得的旋转，以偏差为变量，最小化两者的差值即可得到关于陀螺仪的偏差：

其中，N代表关键帧的个数； 是由视觉SLAM算法计算得到的相机坐标系相对于世界坐标系的旋转，R_CB是标定矩阵；ΔR_i，i+1为、连续两个关键帧间的陀螺仪积分所得到的旋转矩阵，为ΔR_i，i+1随陀螺仪偏差变化方程的一阶近似结果；最后通过高斯-牛顿法求解b_g；

然后，利用上述陀螺仪偏差估计的结果，对尺度和重力加速度进行估计；对于三个相连接的关键帧之间的两两关系，利用两两之间的预积分测量值，得到线性方程：

记第i，i+1，i+2时刻的关键帧标号为1，2，3，则上式各项分别为：

在上式中，表示相机中心C在世界坐标系下的位置，Δt为帧间时间差，I为单位矩阵，R_WC表示相机坐标系C相对于世界坐标系的旋转矩阵；R_WB表示IMU坐标系B在相对于世界坐标系的旋转矩阵；Δv表示帧间速度；s和g_w分别为所求的尺度和重力加速度估计；

最后，利用尺度和重力加速度估计的结果，对IMU加速度偏差进行估计；考虑3个连续关键帧之间的线性关系，可得：

记第i，i+1，i+2时刻的关键帧标号为1，2，3，则φ(i)，ζ(i)和ψ(i)的求解过程如下：

上式中[]_{(：，1：2)}表示使用矩阵前两列数据；R_WI表示惯性***在世界坐标系的方向；表示在惯性坐标系中的重力方向；R_WC表示相机坐标系C相对于世界坐标系的旋转矩阵；R_WB表示IMU坐标系B在相对于世界坐标系的旋转矩阵；为第2、3关键帧的ΔP_2，3随加速度变化方程的一阶近似结果，为第2、3关键帧的Δv_2，3随加速度变化方程的一阶近似结果Δt为帧间时间差，I为单位矩阵，s和δθ_xy为需要估计的尺度因子和重力加速度方向角，b_a是加速度偏差；

步骤4：使用滑动窗口法对位姿进行优化；

通过重投影误差与IMU的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和***状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值；***状态向量X表示如下：

其中x₀，x₁，…x_n表示滑动窗口内的n+1个所有相机的状态，分别表示i时刻下IMU坐标系相对于世界坐标系的旋转，速度和位移，b_a，b_g分别表示加速度计偏差和陀螺仪偏差；构造目标函数：

其中为IMU的测量误差，为单目相机的测量误差，B表示IMU的测量数据集，C表示单目相机的测量数据集，为IMU坐标系下滑动窗口从第i个关键帧到j个关键帧的预积分噪声项协方差，为相机坐标系下滑动窗口第j个关键帧中第l个特征点的噪声协方差；

IMU第i帧和第j帧之间的位移，速度，旋转和IMU偏差的测量残差表示为：

上式中分别表示平移，速度，旋转和IMU偏差带来的误差项；为两帧间旋转变换ΔR相对于重力加速度的一阶近似；为两帧间速度变化Δv相对于重力加速度的一阶近似；为两帧间位移变化Δp相对于重力加速度的一阶近似；为两帧间速度变化Δv相对于加速度的一阶近似；为两帧间位移变化Δp相对于加速度的一阶近似；

视觉残差是重投影误差，对于第l个路标点P，将P从第一次观看到它的第i个相机坐标系，转换到当前的第j个相机坐标系下的像素坐标，其中视觉误差项为：

上式中，为正切平面上的任意两个正交基；是估计第l个路标点在第j个相机归一化相机坐标系中的可能坐标；为第l个路标点在第j个相机归一化相机坐标系中的现察到的坐标；和表示第l个路标点在第j个相机中的像素坐标；表示IMU坐标系相对于相机坐标系的位姿变换；表示从世界坐标系到IMU坐标系的位姿变换。

2.根据权利要求1所述的一种融合视觉特征和IMU信息的机器人定位方法，其特征在于：步骤3中，将步骤1中获得的纯视觉数据和步骤2中获得的IMU数据相结合，并对IMU偏差模型，绝对尺度和重力加速度方向进行估计。

3.根据权利要求1所述的一种融合视觉特征和IMU信息的机器人定位方法，其特征在于：步骤4中，使用滑动窗口法对位姿进行优化通过重投影误差与IMU的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和***状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值。