一种机器人视觉惯性点线特征定位方法及装置制造方法及图纸

本发明专利技术公开了一种机器人视觉惯性点线特征定位方法及装置包括获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；获取机器人的当前图像和惯性传感器的测量数据等步骤，本发明专利技术利用多传感器的融合，在算法中充分发挥各个传感器的优势信息，最终达到提升定位精度、鲁棒性的效果。

A Robot Vision Inertial Point-Line Feature Location Method and Device

The invention discloses a method and device for locating the feature of inertial point-line in robot vision, which includes acquiring the prior three-dimensional map of the current scene and the measurement data of inertial sensor. The prior three-dimensional map is constructed beforehand and contains the three-dimensional point-line feature, acquiring the current image of the robot and the measurement data of inertial sensor, etc. The invention utilizes the fusion of multi-sensors. In the algorithm, the advantage information of each sensor is fully used to improve the positioning accuracy and robustness.

【技术实现步骤摘要】
一种机器人视觉惯性点线特征定位方法及装置
本专利技术涉及机器人导航技术，具体地说，涉及一种机器人视觉惯性点线特征定位方法及装置。
技术介绍
目前，越来越多不同类型的机器人出现在生产生活的方方面面。对于仓储物流、巡检监控等领域而言，其工作需要机器人能够在一个相对固定的环境内实现长期稳定的运行，且能实现精确的自定位。视觉传感器生产成本低，获取信息量大，相关定位方法受到广泛的研究与应用。传统基于特征点匹配进行相机基础矩阵估计的视觉定位方法容易受到视角变化、动态遮挡、环境光照以及气候变化等问题的影响，造成特征点难以重复检测以及检测到的特征点错误匹配率高等问题。而线特征相比于点特征是一种更高级的语义特征，具有更稳定的检测效果，即使在部分遮挡的情况下也能检测出部分线段，可以很大程度上弥补点特征的不足。另外惯性传感器与视觉的融合也能够为相机位姿的估计问题降低求解维度，从而减少估计相机基础矩阵所需的匹配特征数量，因而有效提高容错率，增强算法鲁棒性。
技术实现思路
为了克服现有技术的不足，本专利技术的目的在于提供一种机器人视觉惯性点线特征定位方法及装置，定位时同时利用环境中的点特征和线特征，最大化利用了视觉提供的信息，应用场景更加广泛；线特征相比于点特征是一种更高级的语义信息，尤其在工厂等视觉点特征不那么丰富的人造环境中，线特征的优势更为明显。同时与惯性传感器的结合降低了相机位姿求解问题的维度，减少了模型估计所需的匹配数量，从而即使在视觉提供的信息存在大量误匹配的情况下也能得到准确的定位结果，有效提高容错率，大大提升鲁棒性。本专利技术是通过以下技术方案来实现的：本专利技术公开了一种机器人视觉惯性点线特征定位方法，包括以下步骤：获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；获取机器人的当前图像和惯性传感器的测量数据；根据当前机器人的惯性传感器的测量数据与所述先验三维地图中的惯性传感器的测量数据计算当前机器人位姿的俯仰角和翻滚角；对机器人当前图像进行二维点线特征的检测，并将当前图像中测量到的二维点线特征与所述先验三维地图中测量到的三维点线特征进行匹配；根据所述匹配到的两对二维到三维点特征或者一对二维到三维匹配的点特征加一对二维到三维匹配的线特征计算当前机器人位姿的其余未知量。作为进一步的改进，本专利技术所述的先验三维地图有如下要求：要包含重力加速度在所述地图坐标系下的测量数据，由惯性传感器测量所得；所述先验三维地图要包含三维点线特征。作为进一步的改进，本专利技术所述的机器人在当前图像中惯性传感器的测量数据为重力加速度在当前图像机器人坐标系下的测量值；所述先验三维地图中的惯性传感器的测量数据为重力加速度在先验三维地图坐标系下的测量值；将重力加速度在两个坐标系下的测量值进行对齐，从而得到地图坐标系相对于当前机器人坐标系的俯仰角和翻滚角，具体根据以下算式进行计算：其中[xwywzw]T为重力加速度在先验三维地图坐标系下的测量值，[xcyczc]T为重力坐标系在当前图像机器人坐标系下的测量值，β,γ分别为所求的先验三维地图坐标系相对于当前图像机器人坐标系的俯仰角和翻滚角。作为进一步的改进，本专利技术所述当前图像测量到的二维点线特征和先验三维地图中的三维点线特征可以进行二维到三维的点线特征的匹配，具体包括以下子步骤：根据所述机器人获取的当前图像检测多个二维特征点，所述二维特征点的检测不限方法，具体可以为ORB，SIFT，LBP或HOG特征点检测算法；根据所述机器人获取的当前图像检测多条二维线段，所述二维线段的检测不限方法，具体可以为LBD，Edline或Hough直线检测；根据所述当前图像的二维点线特征和先验三维地图中的三维点线特征进行匹配，具体表现为计算相应描述子在某度量空间的距离，一般可以取为欧式距离；该距离小于预设阈值范围的特征则认为匹配成功，由此迭代获取多对二维与三维的匹配点线特征。作为进一步的改进，本专利技术根据匹配到的两对二维到三维匹配的点特征可以计算先验三维地图相对于当前图像机器人坐标系的位姿的其余未知量，具体包括以下子步骤：根据地图坐标系{W0}中匹配到的两个三维点P10,引入一个中间坐标系{W1}，其中两个三维点在所述中间坐标系下表示为P1＝(000)T,P2＝(X2Y2Z2)T，而{W0}到{W1}的变换则表示为P10→P1的简单平移；根据所述机器人当前图像中匹配到的二维点和相机标定的内参矩阵K可得二维点在机器人相机坐标系{C}下的归一化平面上的三维坐标：其中C为相机的光心；根据投影几何可得三点(C,D1,RP1+t)共线，且三点(C,D2,RP2+t)共线，其中R为{W1}相对于{C}的旋转变换矩阵，t为{W1}相对于{C}的平移变换向量；根据权利要求3所述{W0}相对于{C}的俯仰角和翻滚角，可将R简化为：其中α为未知量，表示为地图坐标系相对于机器人当前相机坐标系的偏航角，设t＝[t1t2t3]T，则全部未知量一共有四个，分别为α,t1,t2,t3；根据三点(C,D1,RP1+t)共线，可得两个方程如下：a1t2-b1t1＝0a1t3-t1＝0根据三点(C,D2,RP2+t)共线，可得两个方程如下：a2(R21X2+R22Y2+R23Z2+t2)-b1(R11X2+R12Y2+R13Z2+t1)＝0a2(R31X2+R32Y2+R33Z2+t3)-(R11X2+R12Y2+R13Z2+t1)＝0联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿；作为进一步的改进，本专利技术根据匹配到的一对二维到三维匹配的点特征加一对二维到三维匹配的线特征可以计算地图相对于当前机器人坐标系的位姿的其余未知量，具体包括以下子步骤：根据地图坐标系{W0}中匹配到的一个三维点P10和一条三维线段引入一个中间坐标系{W1}，其中三维点在所述中间坐标系下表示为P1＝(000)T，{W0}到{W1}的变换则表示为P10→P1的简单平移，易得三维线段的端点在所述中间坐标系下表示为L2＝(X2Y2Z2)T,L3＝(X3Y3Z3)T；根据所述机器人当前图像中匹配到的一个二维点和一条二维线段，以及相机标定的内参矩阵K可得二维点和二维线段的端点在机器人相机坐标系{C0}下的归一化平面上的三维坐标和引入一个中间坐标系{C1}，使得相机光心以及匹配到的点和线段端点的表示为：其中C为相机的光心；所述中间坐标系{C1}到{C0}的变换可由的变换求得；根据投影几何可得三点(C,D1,RP1+t)共线，四点(C,D2,D3,RL2+t)以及(C,D2,D3,RL3+t)共面，其中R为{W1}相对于{C1}的旋转变换矩阵，t为{W1}相对于{C1}的平移变换向量；根据三点(C,D1,RP1+t)共线，可得两个方程如下：a1t2-b1t1＝0b1t3-t2＝0根据四点(C,D2,D3,RL2+t)共面，可得方程：R21X2+R22Y2+R23Z2+t2＝0根据四点(C,D2,D3,RL3+t)共面，可得方程：R21X3+R22Y3+R23Z3+t2＝0联立以上四个方程即可求得四个未知量，从而求得机器人在地图中的当前位姿。本专利技术还公开了一种机器人视觉惯性点线特征定位装置，包括：第一获取模块，用于获取当前场景的先验三维地图本文档来自技高网...

【技术保护点】
1.一种机器人视觉惯性点线特征定位方法，其特征在于，包括以下步骤：获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；获取机器人的当前图像和惯性传感器的测量数据；根据当前机器人的惯性传感器的测量数据与所述先验三维地图中的惯性传感器的测量数据计算当前机器人位姿的俯仰角和翻滚角；对机器人当前图像进行二维点线特征的检测，并将当前图像中测量到的二维点线特征与所述先验三维地图中测量到的三维点线特征进行匹配；根据所述匹配到的两对二维到三维点特征或者一对二维到三维匹配的点特征加一对二维到三维匹配的线特征计算当前机器人位姿的其余未知量。

【技术特征摘要】
1.一种机器人视觉惯性点线特征定位方法，其特征在于，包括以下步骤：获取当前场景的先验三维地图和惯性传感器的测量数据，所述先验三维地图是事先构建的，且包含三维点线特征；获取机器人的当前图像和惯性传感器的测量数据；根据当前机器人的惯性传感器的测量数据与所述先验三维地图中的惯性传感器的测量数据计算当前机器人位姿的俯仰角和翻滚角；对机器人当前图像进行二维点线特征的检测，并将当前图像中测量到的二维点线特征与所述先验三维地图中测量到的三维点线特征进行匹配；根据所述匹配到的两对二维到三维点特征或者一对二维到三维匹配的点特征加一对二维到三维匹配的线特征计算当前机器人位姿的其余未知量。2.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，所述的先验三维地图有如下要求：要包含重力加速度在所述地图坐标系下的测量数据，由惯性传感器测量所得；所述先验三维地图要包含三维点线特征。3.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，所述机器人在当前图像中惯性传感器的测量数据为重力加速度在当前图像机器人坐标系下的测量值；所述先验三维地图中的惯性传感器的测量数据为重力加速度在先验三维地图坐标系下的测量值；将重力加速度在两个坐标系下的测量值进行对齐，从而得到地图坐标系相对于当前机器人坐标系的俯仰角和翻滚角，具体根据以下算式进行计算：其中[xwywzw]T为重力加速度在先验三维地图坐标系下的测量值，[xcyczc]T为重力坐标系在当前图像机器人坐标系下的测量值，β,γ分别为所求的先验三维地图坐标系相对于当前图像机器人坐标系的俯仰角和翻滚角。4.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，根据所述当前图像测量到的二维点线特征和先验三维地图中的三维点线特征可以进行二维到三维的点线特征的匹配，具体包括以下子步骤：根据所述机器人获取的当前图像检测多个二维特征点，所述二维特征点的检测不限方法，具体可以为ORB，SIFT，LBP或HOG特征点检测算法；根据所述机器人获取的当前图像检测多条二维线段，所述二维线段的检测不限方法，具体可以为LBD，Edline或Hough直线检测；根据所述当前图像的二维点线特征和先验三维地图中的三维点线特征进行匹配，具体表现为计算相应描述子在某度量空间的距离，一般可以取为欧式距离；该距离小于预设阈值范围的特征则认为匹配成功，由此迭代获取多对二维与三维的匹配点线特征。5.如权利要求1所述的机器人视觉惯性点线特征定位方法，其特征在于，根据匹配到的两对二维到三维匹配的点特征可以计算先验三维地图相对于当前图像机器人坐标系的位姿的其余未知量，具体包括以下子步骤：根据地图坐标系{W0}中匹配到的两个三维点P10,引入一个中间坐标系{W1}，其中两个三维点在所述中间坐标系下表示为P1＝(000)T,P2＝(X2Y2Z2)T，而{W0}到{W1}的变换则表示为P10→P1的简单平移；根据所述机器人当前图像中匹配到的二维点和相机标定的内参矩阵K可得二维点在机器人相机坐标系{C}下的归一化平面上的三维坐标：其中C为相机的光心；根据投影几何可得三点(C,D1,RP1+t)共线，且...

【专利技术属性】
技术研发人员：王越，焦艳梅，熊蓉，
申请(专利权)人：浙江大学，
类型：发明
国别省市：浙江,33