一种基于目标检测的机器人视觉定位模块及方法

1.本发明属于机器人领域，更具体的说涉及一种基于目标检测的机器人视觉定位模块及方法。


背景技术：

2.机器人是自动执行工作的机器装置。它既可以接受人类指挥，又可以运行预先编排的程序，也可以根据以人工智能技术制定的原则纲领行动。它的任务是协助或取代人类工作的工作，例如生产业、建筑业，或是危险的工作。现有的室内定位方法装置大多过于复杂，机器人搭载起来过于笨重而庞大，成本和造价也颇高，机器人移动主要是通过一些定位元件辅助才能进行移动，同时还有设置在机器人上的多个感应探头以及避障器件配合进行移动，然而随着科技的发展普通的定位元件难以满足机器人的高精度移动。
3.然而，现有的基于目标检测的机器人视觉定位方法，均是采用单一的视觉定位，在单一的视觉定位过程中，由于机器人在移动过程中会一直存在误差，导致机器人摄像头在对目标进行视觉检测时，会由于机器人移动造成的累计误差，从而对会对机器人的视觉定位参数误差。机器人自身的当前位置判断主要是基于自身的imu惯性导航单元测量的数据进行判断。如果能将惯性导航单元内的测量数据与视觉定位的数据结合起来，进行机器人的定位。将可以消除机器人移动过程中产生的误差。


技术实现要素：

4.本发明减少优化计算量，采用滑动窗口和边缘化方法对机器人位姿、速度等状态量进行优化更新，从而得到估计的机器人定位信息。算法相比于纯视觉算法对移动机器人的定位估计误差有一定的下降，可满足机器人的定位精度需求。
5.为了实现上述目的，本发明是采用以下技术方案实现的：所述的机器人视觉定位方法包括步骤1、利用视觉与imu松耦合检测出两个连续立体帧中包含的动态特征点，并计算imu在两帧之间的预积分；步骤2、动态点筛选步骤3、对imu与视觉信息紧耦合；步骤4、对步骤2采用滑动窗口法进行优化和边缘化。
6.进一步地，所述的步骤1包括以下步骤：s101、对图像的特征点进行提取与跟踪；s102、对imu进行预积分；s103、根据上述计算得到的数据对重力方向、陀螺仪的角速度偏置、时间同步误差等进行初始化。
7.进一步地，所述的s101采用shi-tomas角点进行图像特征信息的提取，并使用lk光流法来追踪左右目图像帧以及前后两图像帧提取出的特征点，实现图像在时间和空间上的
特征匹配。
8.进一步地，所述的s102，为了得到两帧图像之间的位姿，减少后端的计算量，imu需要先进行预积分处理，得到相邻imu数据之间的位姿，imu预积分更新公式为：要先进行预积分处理，得到相邻imu数据之间的位姿，imu预积分更新公式为：为t时刻的加速度；为i时刻到j时刻的平均角速度；为时刻到时刻的时间间隔；为重力加速度；为时刻到时刻imu的旋转四元数；为时刻到时刻的imu角度预积分；为时刻到时刻的imu速度预积分；为时刻到时刻的imu位移预积分。
9.进一步地，所述的s103初始化采用imu和视觉的松耦合方法，其中视觉利用特征点匹配与pnp算法求解滑动窗口内所有图像帧的位姿以及对应的所有3d路标点，然后与imu预积分得到的值进行对齐，求解出陀螺仪偏置、重力方向以及每一帧所对应的速度。
10.进一步地，所述的步骤2动态点筛选，连续三帧该点均被判定为动态，且该点的世界坐标系的运动速度未产生较大改变，再将其打上动态特征点标签，直到后面某连续两帧图像判定其为静态，再将其标签改为静态特征点。
11.进一步地，所述的步骤3、对imu与视觉信息紧耦合；步骤如下：s301、定义机器人定位求解中的需要优化变量；s302、构建优化残差方程。
12.进一步地，步骤4采用滑动窗口的方法对计算进行简化，通过对以前的数据进行边缘化将信息保存成先验信息，其滑动窗口的删除对象分为对当前帧图像信息和滑动窗口最老一帧图像信息；
当视觉信息积累数量超过滑动窗口的窗口数时，会调用边缘化过程，将拍摄时间最老的图像帧上的特征点与对应的相机位姿删除，并将这些信息转换为先验约束，以继续参加状态量的优化。
13.再一方面，一种基于目标检测的机器人视觉定位模块，所述的模块包括rgb-d摄像模块、imu惯性测量单元、图像识别及主控模块；所述的rgb-d摄像模块与imu惯性测量单元与图像识别及主控模块连接。
14.本发明有益效果：本发明将机器人视觉定位与机器人自身所携带的惯性导航单元检测到的惯性导航数据进行结合定位。通过紧耦合的方法，减少优化计算量，并且采用滑动窗口和边缘化方法对机器人位姿、速度等状态量进行优化更新，从而得到估计的机器人定位信息。算法相比于纯视觉算法对移动机器人的定位估计误差有一定的下降，可满足机器人的定位精度需求，提高了机器人的目标视觉定位能力。
附图说明
15.图1为本发明流程图；图2为本发明紧耦合算法框架图；图3为滑动窗口法的残差约束因子图；图4为imu和视觉测量协方差对系统的选择影响。
具体实施方式
16.为了便于理解本发明，下面将参照相关附图对本发明进行更全面的描述。附图中给出了本发明的典型实施例。但是，本发明可以以许多不同的形式来实现，并不限于本文所描述的实施例。相反地，提供这些实施例的目的是使对本发明的公开内容更加透彻全面。
17.除非另有定义，本文所使用的所有的技术和科学术语与属于本发明的技术领域的技术人员通常理解的含义相同。本文中在本发明的说明书中所使用的术语只是为了描述具体的实施例的目的，不是旨在于限制本发明。
18.所述的机器人视觉定位方法包括步骤1、利用视觉与imu松耦合检测出两个连续立体帧中包含的动态特征点，并计算imu在两帧之间的预积分；所述的步骤1包括以下步骤：s101、对图像的特征点进行提取与跟踪；使用双目相机作为视觉传感器，其相比于单目相机优势在于可以计算得到尺度信息，图像上任一点深度信息也可以通过左右目同一时刻拍摄的两幅图像得到的视差直接获得，并且双目得到的视觉约束更多，尤其是在动态对象较多导致特征不足的场景中。
19.所述的s101采用shi-tomas角点进行图像特征信息的提取，并使用lk光流法来追踪左右目图像帧以及前后两图像帧提取出的特征点，实现图像在时间和空间上的特征匹配。为了尽可能排除特征点错误跟踪点过多的问题，算法采用了反向光流进行外点滤除，右目光流跟踪失败的左目特征点会被删除。这样可保证每个被提取出的特征点都可根据双目基线进行深度恢复。
20.s102、对imu进行预积分；所述的s102，为了得到两帧图像之间的位姿，减少后端的计算量，imu需要先进行预积分处理，得到相邻imu数据之间的位姿，imu预积分更新公式为：预积分处理，得到相邻imu数据之间的位姿，imu预积分更新公式为：为t时刻的加速度；为i时刻到j时刻的平均角速度；为时刻到时刻的时间间隔；为重力加速度；为时刻到时刻imu的旋转四元数；为时刻到时刻的imu角度预积分；为时刻到时刻的imu速度预积分；为时刻到时刻的imu位移预积分。
21.最后可得imu预积分更新式为：
其中、为imu加速度计在时刻的高斯白噪声；为imu陀螺仪在时刻的高斯白噪声。
22.s103、根据上述计算得到的数据对重力方向、陀螺仪的角速度偏置、时间同步误差等进行初始化。
23.所述的s103初始化采用imu和视觉的松耦合方法，其中视觉利用特征点匹配与pnp算法求解滑动窗口内所有图像帧的位姿以及对应的所有3d路标点，然后与imu预积分得到的值进行对齐，求解出陀螺仪偏置、重力方向以及每一帧所对应的速度。
24.首先是视觉求解滑窗位姿和路标点。寻找与当前帧共视点数较多的滑窗内的关键帧作为参考帧，然后通过求基础矩阵来计算当前帧到参考帧的位姿变换，接着对于参考帧与当前帧中的某一帧进行pnp求解该帧的位姿，并三角化。然后重复上述过程，直到滑动窗口内所有的路标点和位姿都被求解出来。最后调用ba优化滑动窗口内所有的位姿。
25.同样imu预积分可以通过式(4)到式(8)求出滑窗内图像帧对应的位姿与速度信息。则可根据以下目标函数求得陀螺仪的偏置。
26.其中，约为，其中为预积分真值，为相对旋转量对的一阶雅可比求导。
27.求解出陀螺仪偏置后，需要再对imu预积分的值重新计算。接着需要对第一帧的图像相机坐标系下（世界坐标系）的重力向量，以及滑动窗口中所有图像帧对应的相机速度bnv进行初始化，其优化变量为：
可将上述优化变量的相关残差定义为imu预积分得到的平移、速度增量与预测值的误差，其中各变量：其中，为到时刻间imu测量数据的积分；x需求解的状态量，即速度、重力向量、陀螺仪偏置；为时刻相机的速度；将第一帧相机所在位置设为世界坐标系，重力在该坐标系上的表示；将上式改成的形式，如下：由于重力常量在小规模位移是模值固定的，所以重力的三个未知量可以变为两个，从而写成其中，重力沿球半径方向的分量；重力沿球面切线方向的分量；将式(13)代入式(12)即可得到与时刻对应的相机速度与重力向量在世界坐标系的方向。
28.步骤2、动态点筛选所述的步骤2动态点筛选，由于每帧图像存在噪声影响，静态特征点与动态特征点存在某一时刻的误判，而物体的运动或者静止状态在时间上是连续的。所以算法对运动进行平滑处理，避免产生较大误差。即如果连续三帧该点均被判定为动态，且该点的世界坐标系的运动速度未产生较大改变，再将其打上动态特征点标签，直到后面某连续两帧图像判定其为静态，再将其标签改为静态特征点。
29.步骤3、对imu与视觉信息紧耦合；所述的步骤3、对imu与视觉信息紧耦合；步骤如下：s301、定义机器人定位求解中的需要优化变量；对图像和imu的紧耦合本质上是相关原始数据的残差方程关于状态量的线性优化求解，定义机器人定位求解中的需要优化变量为s302、构建优化残差方程。
30.其中，表示第n帧时刻机器人的状态表示第k个特征点的逆深度；表示第k帧在世界坐标系下机器人的位移；表示第k帧在世界坐标系下机器人的速度；表示第k帧在世界坐标系下机器人的速度旋转四元数表示第k帧imu的加速度随机游走偏差；表示第k帧imu的角速度随机游走偏差；表示相机到imu坐标系转换的旋转四元数与平移。
31.imu的残差为：其中，为t时刻的加速度；为i时刻到j时刻的平均角速度；为时刻到时刻的时间间隔；为重力加速度；为时刻到时刻imu的旋转四元数；为时刻到时刻的imu角度预积分；为时刻到时刻的imu速度预积分；为时刻到时刻的imu位移预积分。
32.式(15)中的符号表示四元数的乘法。
33.将各残差约束与优化变量转换成为因子图可表示为图3，可以看到历史数据通过先验约束与位姿状态量相连接，视觉重投影误差约束与路标点逆深度和机器人位姿相关联，imu的预积分约束与机器人位姿和速度、加速度计和陀螺仪偏置的状态量相连。
34.其中残差计算通过乘以协方差矩阵来表征imu和图像传感器信号的不确定性，这种使用马氏距离度量的方法可以通过对测量数据添加权重的方式来使状态量得到更准确的更新。因为imu和图像都存在噪声，尤其是imu随着预测的增加观测的不确定度会加大很多，此时根据视觉观测一起进行优化可以将系统的总协方差减小。但短时imu数据不确定度小，此时状态更新应更偏向于根据imu数据来进行更新。如图4视觉噪声大时，通过马氏距离度量残差的算法会偏向于依赖imu，而imu噪声大时，融合后结果更偏向于相信视觉图像得到的数据。
35.步骤4、对步骤2采用滑动窗口法进行优化和边缘化。
36.步骤4采用滑动窗口的方法对计算进行简化，通过对以前的数据进行边缘化将信息保存成先验信息，其滑动窗口的删除对象分为对当前帧图像信息和滑动窗口最老一帧图像信息；当视觉信息积累数量超过滑动窗口的窗口数时，会调用边缘化过程，将拍摄时间最老的图像帧上的特征点与对应的相机位姿删除，并将这些信息转换为先验约束，以继续参加状态量的优化。
37.边缘化的过程数学形式就是舒尔补，其主要是对下面的增量方程进行数学处理。
[0038] （16）式中h表示残差方程关于状态量x的海色矩阵，
∆
x即为要求的状态量增量。
[0039]
将上式用矩阵方法表示如下。
[0040]
上式中、表示希望边缘化的部分与希望保留的部分。对应于算法后端优化，希望边缘化的部分就是需要删除的图像帧对应的相机位姿与对应的路标点，这一部分的删除利用如下的舒尔补进行消元。
[0041]
通过式(4-8)的行列变换，就将的求解单独分列出来，从而将其与的求解分离开，在概率上相当于将联合概率分布转换成条件概率分布，所以上式也是一个概率边缘化的过程，而右边系数矩阵的部分就是要保留的变量的先验信息。
[0042]
当视觉信息积累数量超过滑动窗口的窗口数时，会调用边缘化过程，将拍摄时间
最老的图像帧上的特征点与对应的相机位姿删除，并将这些信息转换为先验约束，以继续参加状态量的优化，同时减少计算和内存资源消耗。
[0043]
实施例二；基于实施例1构建一种基于目标检测的机器人视觉定位模块，所述的模块包括rgb-d摄像模块、imu惯性测量单元、图像识别及主控模块；所述的rgb-d摄像模块与imu惯性测量单元与图像识别及主控模块连接。
[0044]
为验证本发明视觉与imu紧耦合算法对移动机器人的定位准确性，实验控制静态环境下的移动机器人运动，记录机器人运动过程中zed2相机拍摄的图片和对应的imu数据，并对比本发明紧耦合算法和纯视觉算法的定位精度。
[0045]
为得到紧耦合算法的定位精确度，验证本发明算法的有效性，令机器人做直线与旋转运动，利用本发明算法估算的轨迹与纯视觉算法得到的轨迹进行误差评价。
[0046]
移动机器人搭载zed2双目相机在房间内采取直线和旋转运动并采集图像和imu数据，其运动的范围为在顶视相机的可视范围，面积约为3.6m
×
2.2m。机器人的运动总距离大约是25m。
[0047]
发明机器人视觉与imu融合定位准确性，评价视觉定位方法最好方法就是将估计的位姿和真实位姿进行对比，衡量它们之间的偏差程度。对于本发明的机器人，由于其具有平面运动自由度，所以其位姿满足二维变换矩阵李群，可以将其表示为，相机的真实位置和姿态为，则可定义绝对轨迹误差ate。
[0048]
考虑i时刻到i+δt时刻的运动，相对轨迹误差定义为δt时刻内相机相对位姿与真实轨迹相对位姿的误差，适合用来度量视觉里程计的漂移误差，对每个位姿李代数的均方根误差，这种误差可以刻画估计轨迹的旋转和平移误差，适合定位估计结果的评估，可以衡量在一段位移后的定位算法累计估计结果准确度，其形式如下（19）所示：指标具体为绝对轨迹的均方根误差、中位数、最大值、平均数。
[0049]
利用式(19)算出本节算法与纯视觉算法的绝对轨迹误差ate如表1所示：表1某一时刻部分特征点对应运动矢量大小由表1可知本发明紧耦合算法相比于纯视觉算法对移动机器人的定位估计各项误差指标都小，在平均误差指标中误差下降约14%，估计的结果更精确。且机器人运动250cm，轨迹发生的平均漂移仅9.8cm，可以满足机器人的定位精度需求。
[0050]
本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(readonlymemory，rom)或随机存储记忆体(randomabbessmemory，ram)等。
[0051]
上面结合附图对本发明的实施例进行了描述，但是本发明并不局限于上述的具体实施方式，上述的具体实施方式仅仅是示意性的，而不是限制性的，本领域的普通技术人员在本发明的启示下，在不脱离本发明宗旨和权利要求所保护的范围情况下，还可做出很多形式，这些均属于本发明的保护之内。

技术特征：
1.一种基于目标检测的机器人视觉定位方法，其特征在于：所述的基于目标检测的机器人视觉定位方法包括以下步骤：步骤1、利用视觉与imu松耦合检测出两个连续立体帧中包含的动态特征点，并计算imu在两帧之间的预积分；步骤2、动态点筛选；步骤3、对imu与视觉信息紧耦合；步骤4、对步骤2采用滑动窗口法进行优化和边缘化。2.根据权利要求1所述的一种基于目标检测的机器人视觉定位方法，其特征在于：所述的步骤1包括以下步骤：s101、对图像的特征点进行提取与跟踪；s102、对imu进行预积分；s103、根据上述计算得到的数据对重力方向、陀螺仪的角速度偏置、时间同步误差等进行初始化。3.根据权利要求2所述的一种基于目标检测的机器人视觉定位方法，其特征在于：所述的s101采用shi-tomas角点进行图像特征信息的提取，并使用lk光流法来追踪左右目图像帧以及前后两图像帧提取出的特征点，实现图像在时间和空间上的特征匹配。4.根据权利要求2所述的一种基于目标检测的机器人视觉定位方法，其特征在于：所述的s102，为了得到两帧图像之间的位姿，减少后端的计算量，imu需要先进行预积分处理，得到相邻imu数据之间的位姿，imu预积分更新公式为：到相邻imu数据之间的位姿，imu预积分更新公式为：为t时刻的加速度；为i时刻到j时刻的平均角速度；为时刻到时刻的时间间隔；为重力加速度；为时刻到时刻imu的旋转四元数；为时刻到时刻的imu角度预积分；
为时刻到时刻的imu速度预积分；为时刻到时刻的imu位移预积分。5.根据权利要求2所述的一种基于目标检测的机器人视觉定位方法，其特征在于：所述的s103初始化采用imu和视觉的松耦合方法，其中视觉利用特征点匹配与pnp算法求解滑动窗口内所有图像帧的位姿以及对应的所有3d路标点，然后与imu预积分得到的值进行对齐，求解出陀螺仪偏置、重力方向以及每一帧所对应的速度。6.根据权利要求1所述的一种基于目标检测的机器人视觉定位方法，其特征在于：所述的步骤2动态点筛选，连续三帧该点均被判定为动态，且该点的世界坐标系的运动速度未产生较大改变，再将其打上动态特征点标签，直到后面某连续两帧图像判定其为静态，再将其标签改为静态特征点。7.根据权利要求1所述的一种基于目标检测的机器人视觉定位方法，其特征在于：所述的步骤3、对imu与视觉信息紧耦合步骤如下：s301、定义机器人定位求解中的需要优化变量；s302、构建优化残差方程。8.根据权利要求1所述的一种基于目标检测的机器人视觉定位方法，其特征在于：步骤4采用滑动窗口的方法对计算进行简化，通过对以前的数据进行边缘化将信息保存成先验信息，其滑动窗口的删除对象分为对当前帧图像信息和滑动窗口最老一帧图像信息；当视觉信息积累数量超过滑动窗口的窗口数时，会调用边缘化过程，将拍摄时间最老的图像帧上的特征点与对应的相机位姿删除，并将这些信息转换为先验约束，以继续参加状态量的优化。9.一种基于目标检测的机器人视觉定位模块，所述的模块适用于如权利要求1-8中任一项所述的方法，其特征在于：所述的模块包括rgb-d摄像模块、imu惯性测量单元、图像识别及主控模块；所述的rgb-d摄像模块与imu惯性测量单元与图像识别及主控模块连接。

技术总结
本发明公开了一种基于目标检测的机器人视觉定位模块及方法，属于机器人领域，所述的机器人视觉定位方法包括步骤1、利用视觉与IMU松耦合检测出两个连续立体帧中包含的动态特征点，并计算IMU在两帧之间的预积分；步骤2、对IMU与视觉信息紧耦合；步骤3、进行滑动窗口优化与边缘化。本发明减少优化计算量，采用滑动窗口和边缘化方法对机器人位姿、速度等状态量进行优化更新，从而得到估计的机器人定位信息。算法相比于纯视觉算法对移动机器人的定位估计误差有一定的降低，可满足机器人的定位精度需求。度需求。度需求。


技术研发人员：栾东丽 赵之喜
受保护的技术使用者：济宁市技师学院
技术研发日：2023.06.19
技术公布日：2023/7/21