融合视觉特征和IMU信息的机器人定位方法
专利汇可以提供融合视觉特征和IMU信息的机器人定位方法专利检索，专利查询，专利分析的服务。并且融合视觉特征和IMU信息的 机器人 定位 方法，本 发明 提出了一种单目视觉与IMU融合的方法。首先视觉前端 位姿 跟踪 采用特征点估计机器人位姿，并利用纯视觉信息对IMU偏差模型，绝对尺度和重 力 加速 度方向进行估计。同时IMU解算得到的高 精度 位姿信息为优化搜索过程提供初始参照，并作为状态量和视觉导航信息一起参与优化。后端采用了基于滑动窗口的紧耦合非线性优化方法，实时优化位姿和地图，且滑动窗口法计算 里程计 时候运算复杂度保持固定，提高了 算法 鲁棒性。，下面是融合视觉特征和IMU信息的机器人定位方法专利的具体信息内容。
1.融合视觉特征和IMU信息的机器人定位方法，具体步骤如下：
步骤1：使用单目相机进行位姿估计；
首先对单目相机捕获的图像帧，使用ORB特征提取算法，提取丰富的ORB特征点，根据两帧图像之间特征点的像素位置，利用多目几何知识恢复出单目相机的运动过程，估计机器人位姿；
假设某空间点坐标为Pi＝[Xi，Yi，Zi]T，其投影的像素坐标为Ui＝[ui，vi]T，像素位置与空间点位置的关系如下：
siUi＝Kexp(ξ^)Pi (1)
上式中，si表示深度图的尺度因子，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时刻相对于世界坐标系的速度；gw表示当前的重力加速度； 表示IMU坐标系B在k+1时刻相对于世界坐标系的位移； 表示IMU坐标系B在k时刻相对于世界坐标系的位移；ba与bg分别表示IMU中陀螺仪和加速度计的偏差；
步骤3：视觉惯性联合初始化；
将步骤1中获得的纯视觉数据和步骤2中获得的IMU数据相结合，并对IMU偏差模型，绝对尺度和重力加速度方向进行估计；
首先进行陀螺仪偏差估计，根据视觉信息求得的关键帧之间的旋转，对比利用IMU预积分模型求得的旋转，以偏差为变量，最小化两者的差值即可得到关于陀螺仪的偏差：
其中，N代表关键帧的个数； 是由视觉SLAM算法计算得到的相机坐标
系相对于世界坐标系的旋转，RCB是标定矩阵；ΔRi，i+1为、连续两个关键帧间的陀螺仪积分所得到的旋转矩阵， 为ΔRi，i+1随陀螺仪偏差变化方程的一阶近似结果；最后通过高斯-牛顿法求解bg；
然后，利用上述陀螺仪偏差估计的结果，对尺度和重力加速度进行估计；对于三个相连接的关键帧之间的两两关系，利用两两之间的预积分测量值，得到线性方程：
记第i，i+1，i+2时刻的关键帧标号为1，2，3，则上式各项分别为：
在上式中， 表示相机中心C在世界坐标系下的位置，Δt为帧间时间差，I为单位矩阵，RWC表示相机坐标系C相对于世界坐标系的旋转矩阵；RWB表示IMU坐标系B在相对于世界坐标系的旋转矩阵；Δv表示帧间速度；s和gw分别为所求的尺度和重力加速度估计；
最后，利用尺度和重力加速度估计的结果，对IMU加速度偏差进行估计；考虑3个连续关键帧之间的线性关系，可得：
记第i，i+1，i+2时刻的关键帧标号为1，2，3，则φ(i)，ζ(i)和ψ(i)的求解过程如下：
上式中[](：，1：2)表示使用矩阵前两列数据；RWI表示惯性系统在世界坐标系的方向； 表示在惯性坐标系中的重力方向；RWC表示相机坐标系C相对于世界坐标系的旋转矩阵；RWB表示IMU坐标系B在相对于世界坐标系的旋转矩阵； 为第2、3关键帧的ΔP2，3随加速度变化方程的一阶近似结果， 为第2、3关键帧的Δv2，3随加速度变化方程的一阶近似结果Δt为帧间时间差，I为单位矩阵，s和δθxy为需要估计的尺度因子和重力加速度方向角，ba是加速度偏差；
步骤4：使用滑动窗口法对位姿进行优化；
通过重投影误差与IMU的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和系统状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值；系统状态向量X表示如下：
其中x0，x1，…xn表示滑动窗口内的n+1个所有相机的状态， 分别表示i时刻下IMU坐标系相对于世界坐标系的旋转，速度和位移，ba，bg分别表示加速度计偏差和陀螺仪偏差；构造目标函数：
其中 为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的预积分残差，建立相邻关键帧之间的约束关系，对传感器测量值和系统状态量建立最小二乘式，使用优化方法，迭代求解出当前帧位姿的最优值。
融合视觉特征和IMU信息的机器人定位方法
技术领域
背景技术
发明内容
RWB表示IMU坐标系B在相对于世界坐标系的旋转矩阵； 为第2、3关键帧的ΔP2，3随加速度变化方程的一阶近似结果， 为第2、3关键帧的Δv2，3随加速度变化方程的一阶近似结果Δt为帧间时间差，I为单位矩阵，s 和δθxy为需要估计的尺度因子和重力加速度方向角，ba是加速度偏差。
为两帧间速度变化Δv相对于加速度的一阶近似； 为两帧间位移变化Δp相对于加速度的一阶近似。
具体实施方式
RWB表示IMU坐标系B在相对于世界坐标系的旋转矩阵； 为第2、3关键帧的ΔP2，3随加速度变化方程的一阶近似结果， 为第2、3关键帧的Δv2，3随加速度变化方程的一阶近似结果Δt为帧间时间差，I为单位矩阵，s 和δθxy为需要估计的尺度因子和重力加速度方向角，ba是加速度偏差。
为两帧间速度变化Δv相对于加速度的一阶近似； 为两帧间位移变化Δp相对于加速度的一阶近似。
| 标题 | 发布/更新时间 | 阅读量 |
|---|---|---|
| 一种非接触测量GIS设备位移反演应力的方法 | 2020-05-11 | 861 |
| 一种三驱动机械臂的视觉跟随系统 | 2020-05-11 | 540 |
| 一种基于多目标追踪框架的无人机视频中车速校正方法 | 2020-05-15 | 896 |
| 阴影莫尔参数标定用结构、标定方法及失调误差消除方法 | 2020-05-13 | 277 |
| 图像处理方法以及电子设备 | 2020-05-14 | 745 |
| 一种标准的虚拟正弦直线振动测量方法 | 2020-05-15 | 906 |
| 车辆拐弯轨迹双向计算方法、系统、装置 | 2020-05-12 | 962 |
| 一种基于双流卷积神经网络和SVM的抽烟动作识别方法 | 2020-05-12 | 767 |
| 基于深度图像的机器人定位与建图方法和装置 | 2020-05-13 | 367 |
| 一种用于柔性内窥镜操作机器人的运动控制方法 | 2020-05-15 | 145 |
高效检索全球专利专利汇是专利免费检索,专利查询,专利分析-国家发明专利查询检索分析平台，是提供专利分析，专利查询，专利检索等数据服务功能的知识产权数据服务商。
我们的产品包含105个国家的1.26亿组数据，免费查、免费专利分析。
专利汇分析报告产品可以对行业情报数据进行梳理分析，涉及维度包括行业专利基本状况分析、地域分析、技术分析、发明人分析、申请人分析、专利权人分析、失效分析、核心专利分析、法律分析、研发重点分析、企业专利处境分析、技术处境分析、专利寿命分析、企业定位分析、引证分析等超过60个分析角度，系统通过AI智能系统对图表进行解读，只需1分钟，一键生成行业专利分析报告。
