本发明涉及定位方法和装置，具体涉及一种基于惯性数据和视觉特征的定位装置、定位方法及机器人。
背景技术：
机器人实现智能化，一个基础技术是能够自己定位和行走，室内导航技术是其中的关键技术。目前室内导航技术有惯性传感器导航、激光导航、视觉导航、无线电导航等等，各个技术都有自己的优缺点。惯性传感器导航是应用陀螺仪、里程计等进行导航定位，价格低廉，但是存在长时间漂移的问题；激光导航精度高，但是价格比较高，寿命也是一个问题；传统意义上的视觉导航，计算复杂，对于处理器性能要求比较高，功耗和价格会比较高；无线电需要多个固定的无线电发射源，应用不方便，价格也还比较高。多技术的融合，实现低成本和高精度是机器人导航技术的一个发展方向。
现有视觉扫地机产品中，摄像头放置在机器的前面，一般情况下都需要略微突起，才能够获得比较好的视角，然而，这样容易导致摄像头镜片被一些难以检测到的物体碰到，容易刮花镜片；并且机器的前面一般放置的传感器比较多，例如很多机器有碰撞条和圆柱形的360度红外接收装置，这些容易挡住摄像头，导致摄像头的角度需要加大。
技术实现要素：
一种基于惯性数据和视觉特征的定位装置，该定位装置是一种可移动的视觉定位装置，包括图像采集模块、图像处理模块、惯性数据采集模块和融合定位模块；
图像采集模块的摄像头朝向保持向后并放置在所述定位装置的预设位置中，用于避免在所述定位装置前进过程中被障碍物碰撞，其中所述摄像头的数目至少为一个；
图像处理模块，包括图像预处理子模块和特征匹配子模块，用于处理图像采集模块中采集的图像数据；其中，图像预处理子模块用于将所述摄像头采集的数据转换为灰度图像，特征匹配子模块用于从图像预处理子模块预处理的图像中提取出特征数据与地标数据库中的地标图像相关联特征进行匹配；其中所述数据库为图像处理模块内置的地标数据库，该地标数据库包括给定地标相关联区域的图像特征点；
惯性数据采集处理模块，由一系列惯性数据测量单元组成，实时感应惯性传感器的旋转角度信息，加速度信息及平移速度信息；
融合定位模块，用于根据图像处理模块中的特征匹配结果，对惯性数据采集处理模块采集的惯性数据进行数据融合，再通过数据融合结果修正当前位置信息。
进一步地，所述预设位置为所述定位装置顶部表面的中部开口向后的凹入和/或凸出结构处。
进一步地，所述预设位置为所述定位装置顶部表面的尾部开口向后的凹入和/或凸出结构处。
进一步地，所述摄像头的光学轴与所述定位装置顶部表面形成的角度横跨0—80度。
进一步地，所述融合定位模块中，当所述图像处理模块中的特征匹配成功时，根据所述灰度图像特征与实际场景中地标的成像特征几何关系，计算出所述定位装置当前位置坐标，并使用所述惯性数据更新修正；
当所述图像处理模块中的特征匹配失败时，根据所述惯性数据的累积值求出所述惯性传感器与所述摄像头的刚性连接关系，同时结合所述灰度图像特征与地标数据库中的地标图像相关联特征的相对姿态，计算出新的地标并存储记录在所述地标数据库中，完成新路标的创建；
其中，所述惯性传感器到所述摄像头，所述摄像头到所述灰度图像特征或地标图像相关联特征都存在着映射关联，同时特征能够通过所述灰度图像提取获得；所述刚性连接关系是基于所述摄像头采集的相邻两帧图像间所述惯性数据对应的位姿变化建立起来的位置关系。
一种基于惯性数据和视觉特征的定位方法，所述定位方法应用于所述定位装置，包括如下步骤：
将摄像头针对实际场景中地标采集的图像预处理得到灰度图像，并从灰度图像中提取特征点，生成灰度图像特征，确定特征点的描述子；
将生成的灰度图像特征的描述子与存储在地标数据库中的地标图像相关联特征的描述子进行特征匹配；
判断所述灰度图像特征与地标数据库中的地标图像相关联特征是否匹配，是则根据所述灰度图像特征与实际场景中地标的成像特征几何关系，融合惯性数据，得到当前位置坐标；否则根据所述惯性传感器与所述摄像头的刚性连接关系，结合所述灰度图像特征与地标数据库中的地标图像相关联特征的相对姿态，对所述惯性数据进行融合，计算出新的地标并存储记录在所述地标数据库中，完成新路标的创建；
其中所述惯性数据已经过校准滤波处理，所述刚性连接关系是基于所述摄像头采集的相邻两帧图像间所述惯性数据对应的位姿变化建立起来的位置关系。
进一步地，所述特征匹配过程包括：在当前帧图像下，计算所述灰度图像特征的描述子与所述地标数据库的地标图像相关联特征中对应描述子之间的汉明距离；
若所述汉明距离小于预设阈值，则表示采集进来的图像与所述地标数据库中对应的所述地标图像的相关联特征相似度高，视为匹配成功；
其中所述预设阈值对应一个确定的所述相对姿态的数值关系。
进一步地，所述融合惯性数据包括：当所述特征匹配失败时，根据所述摄像头采集的相邻两帧图像间所述惯性数据对应的位姿变化，求出所述惯性传感器与所述摄像头的刚性连接关系，在所述摄像头内参已知的情况下，根据所述惯性传感器与所述摄像头的刚性连接关系计算得到所述惯性传感器预测的当前帧图像的特征点坐标，然后结合所述灰度图像特征与地标数据库中的地标图像相关联特征的相对姿态，将所述惯性传感器预测的当前帧图像的特征点坐标与所述摄像头采集的当前帧图像的特征点坐标对比，对所述摄像头采集的当前图像的特征点坐标进行更新修正。
进一步地，成像特征几何关系是基于所述预设位置处的所述摄像头上采集的当前帧图像中的特征点与采集的实际场景中的路标的位置关系建立起来的相似三角形关系。
一种机器人，所述机器人是一种装设所述定位装置的移动机器人。
相对于现有技术，本发明提供的一个或多个摄像头放置在所述定位装置表面朝后凸出结构处，这样避免机器前进时被没有检测到的障碍物挡住，同时使其安装的位置有更多的选择，更为的灵活。根据所述摄像头在所述定位装置上的定位，本发明使用与地标数据库的图像特征对拍摄的地标图像进行匹配，融合惯性数据，在定位的同时又为地标数据库创建新的地标，提高定位精度，同时缩小地标数据库在定位匹配过程中特征搜索范围，缩短特征搜寻时间，提高导航效率。
附图说明
图1为本发明实施提供的一种基于惯性数据和视觉特征的定位装置的模块框架图；
图2为本发明实施提供的一种基于惯性数据和视觉特征的定位方法流程图；
图3为本发明实施提供的一种基于惯性数据和视觉特征的机器人系统结构图（摄像头定位在所述定位装置表面凸出结构处）；
图4为本发明实施提供的另一种基于惯性数据和视觉特征的机器人系统结构图（摄像头定位在所述定位装置表面凹入结构处）。
具体实施方式
下面结合附图对本发明的具体实施方式作进一步说明：
本发明实施例中的一种基于惯性数据和视觉特征的定位装置以机器人的方式来实施，包括扫地机器人、AGV等等移动机器人。下面假设所述避障装置装设在扫地机器人上。然而本领域技术人员将理解的是，除了特别用于移动机器人之外，根据本发明的实施方式的构造能扩展应用于移动终端。
本发明提供一种基于惯性数据和视觉特征的定位装置，该视觉定位装置是一种可移动的视觉定位装置，如图1所示，包括图像采集模块、图像处理模块、惯性数据采集模块和融合定位模块；图像采集模块的摄像头放置在所述定位装置的预设位置中，用于避免在所述定位装置前进过程中被障碍物碰撞。在所述定位装置前进过程中，如果所述摄像头放置在所述定位装置的前面，一般情况下都需要略微突起，保持预设角度才能够获得比较好的视角，然而，这样容易导致摄像头镜片被一些难以检测到的物体碰到，容易刮花镜片；在本发明实施例中设置有碰撞条和圆柱形的360度红外接收装置，这些容易挡住所述摄像头，导致摄像头保持预设角度才能够获得比较好的视角。
其中所述摄像头的数目可以是多个，装设在所述定位装置顶部表面的中部和/或尾部不同位置处；在所述定位装置前进驱动的过程中，障碍物降低由摄像头捕获的用于在导航定位过程中使用的图像的质量，并且从而减少导航的精准度。为了维持足够水平的导航性能，所述定位装置上装设多个摄像头来采集有效图像信息，从多个视角扩展所述定位装置的感测视野范围，改进所述图像采集模块的系统性能。
优选地，所述预设位置为在所述视觉定位装置的尾部且开口向后的凸出结构处。如图3所示，在本发明实施中所述摄像头106的朝向保持着相对所述定位装置向后，并放置在所述定位装置面盖表面的尾部向后凸出结构处，通过所述摄像头后置避免所述摄像头被碰撞，使得摄像头的镜头具有更精确的成像特性。具体地，所述摄像头106的光学轴与所述定位装置顶部表面形成的角度在本实施例中优选为45度，增加所述定位装置的有效视野，防止不想要的成像问题，诸如可能阻止所述摄像头有效成像特征的光反射和/或折射。
优选地，所述预设位置为在所述视觉定位装置的中部开口向后的凹入结构处。如图4所示，在本发明实施中所述摄像头108的朝向保持着相对所述定位装置向后，并放置在所述定位装置面盖表面的几何中心附近开口向后的凹入结构处，通过所述摄像头后置避免所述摄像头被碰撞，使得摄像头的镜头具有更精确的成像特性。具体地，所述摄像头108的光学轴与所述定位装置顶部表面形成的角度范围在本实施例中优选为45度，增加所述定位装置的有效视野，防止不想要的成像问题，诸如可能阻止所述摄像头有效成像特征的光反射和/或折射。
需要说明的是，所述预设位置为所述视觉定位装置的尾部且开口向后的凹入结构处的实施例可参考图4的结构，将图4中的凹入结构移至所述定位装置的尾部即可；所述预设位置为所述视觉定位装置的中部且开口向后的凸出结构处的实施例可参考图3的结构，将图3中的凸出结构移至所述定位装置的中部即可。所述预设位置处装设摄像头比在所述定位装置表面使用360度全方位相机的情况下获得更高精度的地标。
如图1所示，图像处理模块，包括图像预处理子模块和特征匹配子模块，用于处理图像采集模块中采集的图像数据。其中，图像预处理子模块接收图像采集模块中采集的图像数据以在周围环境内建立重复可识别的唯一的地标，并将所述摄像头采集的彩色图像数据二进制化，转换为灰度图像，完成图像的预处理过程；然后特征匹配子模块从图像预处理子模块预处理的图像中提取出特征数据，并与地标数据库中的地标图像相关联特征进行匹配。
其中，所述地标数据库为图像处理模块内置的地标数据库，该地标数据库包括给定地标相关联区域的图像特征点。所述地标数据库包含关于许多先前观察的地标的信息，所述定位装置可以利用该地标来执行导航定位动作。地标可以被认为是具有具体二维结构的特征的集合。各种特征中的任何一个可以用于识别地标，当所述定位装置被配置为房屋清洁机器人时，地标可能是(但不限于)基于相框的边角的二维结构识别的一组特征。这样的特征基于房间内的静态几何形状，并且虽然特征具有某种照明和尺度变化，但是它们相对于频繁地位移(例如椅子、垃圾桶，宠物等)的环境的较低区域内的物体总体上更容易被辨别和识别为地标。
如图1所示，惯性数据采集处理模块，惯性数据采集处理模块，由一系列惯性数据测量单元组成，实时感应惯性传感器的旋转角度信息，加速度信息及平移速度信息；该模块用于通过惯性传感器采集惯性数据，再进行校准滤波处理传给融合定位模块。所述惯性数据的原始数据处理包括最大值和最小值的屏蔽；静态漂移消除；数据的卡尔曼滤波。其中惯性传感器包括里程计、陀螺仪、加速度计等用于惯性导航的传感器。
如图1所示，融合定位模块，用于根据图像处理模块中的特征匹配结果，对惯性数据采集处理模块采集的惯性数据进行数据融合，再通过数据融合结果修正当前位置信息。该模块基于所述摄像头采集的图像和惯性传感器所获取的行进距离，在新图像与存储在地标数据库中的对应的地标图像匹配，然后进行数据融合以实现定位。
如图1所示，所述融合定位模块中，当所述图像处理模块中的特征匹配成功时，根据所述灰度图像特征与实际场景中地标的成像特征几何关系(相似三角形关系)，计算出所述定位装置当前位置坐标，并使用所述惯性数据更新修正；当所述图像处理模块中的特征匹配失败时，根据所述惯性数据的累积值求出所述惯性传感器与所述摄像头的刚性连接关系，同时结合所述灰度图像特征与地标数据库中的地标图像相关联特征的相对姿态，计算出新的地标并存储记录在所述地标数据库中，完成新路标的创建；其中，所述刚性连接关系是基于所述摄像头采集的相邻两帧图像间所述惯性数据对应的位姿变化建立起来的位置关系；所述惯性传感器到所述摄像头，所述摄像头到所述灰度图像特征或地标图像相关联特征都存在着映射关联，同时特征能够通过所述灰度图像提取获得，依靠刚性连接关系，使用连续两帧图像间的所述惯性传感器数据进行迭代运算，得到所述定位装置当前位置的一个预测，使得特征匹配时的搜索区域更小，匹配速度更快。
进一步地，在本发明实施中所述摄像头可以放置在所述定位装置面盖表面的尾部向后凸出或凹入结构处，通过所述摄像头后置避免所述摄像头被碰撞的问题。在一个具体实施例中，所述惯性数据测量单元中的一个红外碰撞传感器装设于所述定位装置的前方，并感测到障碍物从而得到第一位置坐标，而所述摄像头却拍摄不到，其获得的拍摄物体位于所述定位装置后方，图像特征匹配并计算得出的第二位置坐标与前述的第一位置坐标不一样，存在着基于所述摄像头与所述红外碰撞传感器的刚性连接关系；所述定位装置继续前进，当所述摄像头拍摄到所述障碍物时，图像特征匹配并计算得到所述第一位置坐标，而所述红外碰撞传感器感测到其他物体，得出的位置信息不是所述第一位置坐标，其中根据所述定位装置相对于与地标数据库中相关联的地标姿势的平移和/或旋转的描述，所述图像处理模块对所述图像采集模块的图像数据进行位置关系换算实现进行定位。
进一步地，所述定位装置还可包括不同类型的传感器系统，实现可靠和鲁棒的多传感器数据融合以增强自主移动的鲁棒性。额外的传感器系统可与彼此结合使用以创建所述定位装置的环境的感知，该感知足以允许所述定位装置做出关于将在该环境中采取的动作的智能决定。)、缓冲器传感器系统550 中的接触传感器、激光扫描器和/或成像声纳。其中传感器被典型地放置，使得它们具有在所述定位装置周围的感测区域的实现最大覆盖。其次，传感器以所述定位装置本身造成对传感器的绝对最小的遮挡的方式被典型地放置。第三，传感器的放置和安装不应当侵入平台的工业设计的其余部分。在美学方面，将传感器装设到所述定位装置内不明显的部位更具美观效果。在实用性方面，应当以一种方式安装传感器，以便不干扰正常机器人操作。
基于同一发明构思，本发明实施例还提供了一种基于惯性数据和视觉特征的定位方法，由于该定位方法解决定位问题的硬件装置是基于前述一种基于惯性数据和视觉特征的定位装置，因此该定位方法的实施可以参见前述一种基于惯性数据和视觉特征的定位装置的应用实施，重复之处不再赘述。
在具体实施时，本发明实施例提供的一种基于惯性数据和视觉特征的定位方法，如图2所示，具体包括：
步骤一、将摄像头针对实际场景中地标采集的图像预处理得到灰度图像，并从灰度图像中提取特征点，生成灰度图像特征，确定特征点的描述子；
具体地，在从摄像头拍摄到的环境二维彩色图像上进行高斯滤波处理，去除噪声，再进行灰度化；对灰度图像进行特征点提取，生成图像特征，将提取的特征点与该特征点的领域内256个位置的像素灰度进行比较，以二进制记录结果，0表示特征点的像素灰度小于该特征点的领域内256个像素灰度的其中一个，1表示特征点的像素灰度大于该特征点的领域内256个像素灰度的其中一个，将结果保存在256维的向量中作为该特征点的描述子。该特征点的领域是包含以该特征点为中心，以r为半径的圆平面，r的值根据实际采集的图像灰度决定。
步骤二、将步骤一生成的灰度图像特征的描述子进行匹配，其匹配对象为存储在地标数据库中的地标图像相关联特征，同样对所述地标图像处理得到对应特征点的描述子，将生成的灰度图像特征的描述子与存储在地标数据库中的地标图像特征的描述子进行特征匹配。
步骤三、判断所述灰度图像特征与地标数据库中的地标图像相关联特征是否匹配，是则根据所述灰度图像特征与实际场景中地标的成像特征几何关系，在已知摄像头内参的情况下，计算出所述定位装置在大地坐标系下的当前位置坐标，再结合所述摄像头与惯性传感器之间刚性连接关系，通过所述惯性传感器得到所述定位装置的一个预测位置坐标，并对所述当前位置坐标修正更新，得到一个准确的当前位置坐标；否则根据所述惯性传感器与所述摄像头的刚性连接关系，结合所述灰度图像特征与地标数据库中的地标图像相关联特征的相对姿态，所述灰度图像特征的特征点对应的地标图像坐标，融合所述惯性数据在当前帧地标图像中预测的图像特征点坐标，计算出新的地标并存储记录在所述地标数据库中，完成新路标的创建。其中，其中所述惯性数据已经过校准滤波处理；所述惯性数据包括角速度、加速度和距离信息；所述刚性连接关系是基于所述摄像头采集的相邻两帧图像间所述惯性数据对应的位姿变化建立起来的位置关系。
作为本发明实施的一种方式，所述步骤二中，所述特征匹配过程包括：在当前帧图像下，计算所述灰度图像特征的描述子与所述地标数据库的地标图像相关联特征中对应描述子之间的汉明距离；若所述汉明距离小于预设阈值，则表示采集进来的图像与所述地标数据库中对应的所述地标图像的相关联特征相似度高，视为匹配成功。具体地，在本发明实施中通过计算特征点的描述子的汉明距离进行特征匹配，经过大量实验表明，匹配失败的特征点的描述子的汉明距离在128左右，匹配成功的特征点的描述子的汉明距离则远小于128；即所述图像特征的描述子与所述数据库的图像模板中的描述子的特征编码对应bit位上相同元素的个数小于128的，一定不是配对的；一幅图上特征点与另一幅图上特征编码对应bit位上相同元素的个数最多的特征点配成一对。其中所述预设阈值对应一个确定的所述相对姿态的数值关系，在本发明实施例中设置为128。所述相对姿态取决于所述摄像头采集的图像内可识别的地标相关联的二维空间特征中的一个或多个，对相对姿态的估计还依靠所述定位装置的各种传感器配置变化而变化。在导航过程中所述定位装置可以将新确定的所述相对姿态信息添加到所述地标数据库的地标图像特征信息中，丰富了新创建的地标图像及提高特征识别效率。
作为本发明实施的一种方式，所述步骤三中，所述融合惯性数据包括：当所述特征匹配失败时，根据所述摄像头采集的相邻两帧图像间所述惯性数据对应的位姿变化，求出所述惯性传感器与所述摄像头的刚性连接关系，在所述摄像头内参已知的情况下，根据所述惯性传感器与所述摄像头的刚性连接关系计算得到所述惯性传感器预测的当前帧图像的特征点坐标，然后结合所述灰度图像特征与地标数据库中的地标图像相关联特征的相对姿态，将所述惯性传感器预测的当前帧图像的特征点坐标与所述摄像头采集的当前帧图像的特征点坐标对比，对所述摄像头采集的当前图像的特征点坐标进行更新修正；当所述特征匹配成功时，根据所述灰度图像特征的特征点的所述成像特征几何关系，计算出所述定位装置当前位置坐标，根据所述摄像头与惯性传感器之间的刚性连接关系进行换算后与所述惯性传感器获取的当前位置坐标对比，对所述特征点得到的当前位置坐标进行修正，得到当前位置的准确坐标值。
具体地，所述惯性传感器做预测运动模型，所述摄像头做观测模型，所述惯性传感器和所述摄像头之间的刚体连接作为要估计的参数值；在连续两帧图像之间，计算所述惯性数据的累积值，包括速度和加速度引起的平移以及角速度引起的旋转，因为存在一个所述惯性感器到所述摄像头，所述摄像头到图像，图像到特征的映射关联，同时特征点能够通过图像提取获得，依据同一个特征点在图像中的成像唯一性原则，构建一个最优化方程，以所述惯性感器提供的姿态为初值进行迭代求解。然后利用预测和观测的协方差信息进行信息融合得到一个最小二乘意义下的最优估计，更新修正得到所述定位装置在当前位置下的准确坐标值。
在所述摄像头拍摄连续两帧图像之间，记录所述惯性数据并进行累加运算，获取连续两帧之间所述惯性传感器记录的位姿变换，利用所述惯性感器到所述摄像头之间固定的旋转和平移变换关系，转换成所述摄像头的位姿变换，再根据所述摄像头内参矩阵获得上一帧的特征坐标在当前帧的坐标；当所述特征匹配失败时运用前述转换方法，通过惯性传感器获取的当前位置坐标预测当前图像坐标，并与当前图像特征中的特征点坐标对比，对当前图像特征中的特征点坐标进行更新修正，并存储回所述地标数据库作为当前位置下的创建的新地标。当所述特征匹配成功时，所述图像处理模块预处理得到所述灰度图像特征的坐标通过所述成像特征几何关系运算得到的当前位置坐标，与所述惯性传感器获取的当前位置坐标对比，即通过观测模型对预测模型修正，从而实现由所述特征点得到的当前位置坐标的修正更新。所以当所述定位装置未匹配到输入图像内的己知地标的存在时，可选地尝试创建新地标。
作为本发明实施的一种方式，所述步骤三中，所述成像特征几何关系是基于所述预设位置处的所述摄像头的镜头朝向角度上采集的图像结合所述惯性传感器感测路标所采集的惯性数据对应到的位置关系而建立起来的。所述摄像机模型采用传统的针孔模型，所述摄像机内参为已知，结合所述定位装置前进过程中拍摄的路标上特征的距离和位置作出的三角测量，构建起相似三角形的几何关系，可以计算出路标上相应特征角点在所述摄像机坐标系中的二维坐标。
作为本发明实施的一种机器人实施例，图3中提供一种扫地机器人的结构图，可作为本发明实施中提供的一种基于惯性数据和视觉特征的定位装置的具体应用产品结构图，为了便于说明，仅示出了与本发明实施例相关的部分。所述定位装置中图像处理模块与融合定位模块内置于信号处理板102；图像采集模块包括摄像头106，其中摄像头106装设在机体101的尾部向后凸出结构处，所述摄像头106的光学轴与所述定位装置顶部表面形成一定的倾斜角度，使得所述摄像头106远离碰撞检测传感器105，避免被一些难以检测到的物体碰到；惯性数据采集模块包括碰撞检测传感器105，惯性数据采集模块在运动轮104和万向轮107驱动机体101的作用下进行感测，所述惯性数据采集模块和所述摄像头106采集的数据通过运用所述相对位姿和刚性连接关系进行融合修正位置坐标，进而执行定位导航动作，还可以更新所述地标数据库以作为构建导航地图的依据。最后人机界面103输出信号处理板计算得出的所述扫地机器人的当前位置的准确坐标数值。
作为本发明实施的另一种机器人实施例，图4中提供一种扫地机器人的结构图，可作为本发明实施中提供的一种基于惯性数据和视觉特征的定位装置的具体应用产品结构图，为了便于说明，仅示出了与本发明实施例相关的部分。图4中摄像头108装设在机体101的尾部开口向后的凹入结构处，所述摄像头108的光学轴与所述定位装置顶部表面形成一定的倾斜角度，使得所述摄像头108远离碰撞检测传感器105，从而使得摄像头108具备较好的视角范围。
以上实施例仅为充分公开而非限制本发明，凡基于本发明的创作主旨、未经创造性劳动的等效技术特征的替换，应当视为本申请揭露的范围。