本发明涉及一种三维建模方法及系统，具体涉及一种建筑测量机器人多传感器融合三维建模方法及系统。
背景技术：
三维建模技术是智能体携带其传感器在运动过程中对自身进行定位，同时以合适的方式描述周围的环境并建立真实世界的三维模型。三维建模能够比传统的文字、图像和视频等方式更高效、直观地呈现信息；在建筑工地等复杂环境下，传统的基于gps定位可能不能正常使用，那么三维建模技术可以作为一种有效的替代方案实现在未知复杂环境中的环境感知、场景建模以及实时导航和定位。
目前，经典的三维建模技术按照传感器可以分为：基于激光雷达的三维建模技术、基于深度传感器的三维建模技术、以及基于视觉传感器的三维建模技术等。
激光雷达的优点是数据的精度较高，后期处理的方式简单。然而其缺点是：第一，价格昂贵，这限制了它的广泛使用；第二，测量数据稀疏，对环境的感知较少；第三，测量距离有限，在空旷区无法使用。
深度传感器(rgbd相机)，这种传感器能够实时通过红外结构光获得空间结构信息，这有利于实时建立稠密的三维地图。然而，由于rgbd摄像机使用的是红外结构光，因此对于光线充足的室外场景，我们几乎无法通过rgbd摄像机实现定位建图。
视觉传感器是基于图像内容来获得空间结构信息的，因此室内室外的建模用视觉传感器可以有效的解决，但其三维成像的精度较低，且建模能力严重依赖于环境的几何结构和纹理信息。
综上所述，基于单一传感器的三维建模技术都存在些许不足，因此，基于多传感器融合的三维建模技术的应用需求越来越受到关注。多传感器信息融合是指对不同传感器在相近时间段获取的时间进行结合，在某种对应准则情况下，利用相关技术分析数据，消除传感器间的误差，并融合互补，完成最终结果处理的过程。将该技术应用于建筑测量机器人三维建模应用领域，结合不同种传感器信息的互补性，可以获取较为完整的数据，从而适应不同复杂程度的建筑工地环境，保证所获取的三维模型的准确性和稳定性。
技术实现要素：
鉴于此，本发明提供了一种建筑测量机器人多传感器融合三维建模方法，包括：
步骤1，采集单个测量站点在各个旋转角度方向上的rgb图像和深度图像，融合得到单个测量站点处的三维点云数据以及rgb高清图像数据；
步骤2，多传感器融合自动导航定位，得到所述建筑测量机器人的实时定位信息；
步骤3，根据步骤1采集到的三维点云数据，并基于步骤2得到的建筑测量机器人的定位信息，对两个相邻测量站点的三维点云数据进行初始配准；
步骤4，根据初始配准后的三维点云数据，对相邻测量站点的全景rgb图像和深度图像与三维点云中的融合特征进行提取和匹配；
步骤5，根据提取和匹配的结果，完成三维点云拼接，最终完成三维建模。
作为本发明进一步的改进，步骤1具体包括：
步骤101，利用双轴旋转云台在各个旋转角度上的旋转，带动双目结构光深度相机的旋转，获取单个测量站点的全景式360度的图像数据采集，进而得到对应的全景三维点云；
步骤102，双轴旋转云台在各个旋转角度旋转的同时，多线激光扫描雷达保持扫描，获取对应的全景三维点云；
步骤103，利用事先标定好的所述多线激光扫描雷达与所述双目结构光深度相机的外参，将所述多线激光扫描雷达和所述双目结构光深度相机所获得的该测量站点处的全景三维点云进行融合拼接处理，最终得到该测量站点处的单站三维点云数据以及rgb高清图像数据。
作为本发明进一步的改进，步骤101中，在获取深度图像对应的三维点云坐标时：根据采集到的rgb图像的内参，该内参为所述双目结构光深度相机在x和y方向上的焦距，以及rgb图像的中心像素坐标，通过公式(1)-(3)，根据深度图像在(xd,yd)处的像素值depth(xd,yd)，可以得到该深度图像在(xd,yd)处所对应的三维点坐标；
x＝depth(xd,yd)*(xd-cx)/fx(1)
y＝depth(xd,yd)*(yd-cy)/fy(2)
z＝depth(xd,yd)(3)
上述式中，fx和fy分别为所述双目结构光深度相机在x和y方向上的焦距，cx和cy分别为rgb图像的中心像素坐标；
通过求解所有的深度图像坐标所对应的三维点坐标后，得到该帧深度图像所对应的三维点云，该三维点云坐标系原点为所述双目结构光深度相机光心的位置。
作为本发明进一步的改进，步骤103的具体方法为：事先标定好的所述多线激光扫描雷达与所述双目结构光深度相机的外参，该外参为所述多线激光扫描雷达相对于所述双目结构光深度相机的旋转矩阵r和平移矢量t，设所述多线激光扫描雷达的物理中心为所述多线激光扫描雷达系的坐标原点，则在所述多线激光扫描雷达坐标系下，所述多线激光扫描雷达所成的三维点云坐标为q＝(xlidar,ylidar,zlidar)，通过式(4)将所述多线激光扫描雷达坐标系下的所述多线激光扫描雷达所成三维点云坐标转换到所述双目结构光深度相机坐标系中，三维点云坐标变为p＝(xdepth,ydepth,zdepth)；
p＝r*q+t(4)
式中，r为所述多线激光扫描雷达相对于所述双目结构光深度相机的旋转矩阵，t为所述多线激光扫描雷达相对于所述双目结构光深度相机的平移矢量。
作为本发明进一步的改进，步骤2具体包括：
步骤201，通过单线激光扫描雷达获取360度二维扫描平面结构图；
步骤202，不断读取车轮里程计的里程信息，计算所述agv底盘实时的位移信息；
步骤203，从一个测量站点开始不断获取惯性测量单元imu的姿态信息；
步骤204，融合所述姿态信息与所述位移信息，基于紧耦合预积分的融合导航定位方法，得到所述建筑测量机器人实时的定位信息；
步骤205，将所述定位信息与所述二维扫描平面结构图进行融合校准，最终输出所述建筑测量机器人的位置信息。
作为本发明进一步的改进，步骤204的具体方法为：通过所述惯性测量单元imu的预积分可得到短时间内所述agv底盘的移动速度，再通过二次积分得到单位时间内所述agv底盘的移动距离，通过该移动距离与所述车轮里程计在单位时间内所得到的距离信息进行比较和综合，从而得到实时的定位信息。
作为本发明进一步的改进，步骤205的具体方法为：将当前时刻所得到的所述单线激光扫描雷达扫描的二维轮廓图像与前一时刻扫描的二维轮廓图像进行图像轮廓边缘的匹配，从而最终输出所述建筑测量机器人的实时的定位坐标(x,y,z)。
作为本发明进一步的改进，步骤3的具体方法为：利用采集到的三维点云数据，基于所述建筑测量机器人实时的定位信息，得到两个测量站点之间的所述建筑测量机器人的三维点坐标分别为(xn-1,yn-1,zn-1)和(xn,yn,zn)，根据公式(5)对两个测量站点的三维点云数据进行初始配准；
式中，t0为两个测量站点之间的初始变换矩阵。
作为本发明进一步的改进，步骤4具体包括：
步骤401，根据采集到的各个旋转角度方向上的rgb图像和深度图像，按照双轴旋转云台的旋转角度、结构几何尺寸以及手眼标定的结果，得到该测量站点经过校准后的全景rgb图像与全景深度图像；
步骤402，在相邻测量站点的两幅全景rgb图像中提取关键不变特征点，然后在深度图像中得到同样位置的关键不变特征点集合，即为关键点集合，并通过所述双目结构光深度相机的内参和外参，投影至三维点云中得到关键点的三维坐标，得到三维关键点集合；
步骤403，根据步骤402得到的三维关键点集合，提取成对点对的几何特征及其描述子，对两个测量站点的三维几何特征进行匹配。
作为本发明进一步的改进，步骤402中，关键不变特征点为sift特征点、加速鲁棒特征点和surf特征点中的一种。
本发明还提供了一种建筑测量机器人多传感器融合三维建模系统，包括：
单站数据融合采集模块，其用于分别采集单个测量站点在各个旋转角度方向上的rgb图像和深度图像，融合得到单个测量站点处的三维点云数据以及rgb高清图像数据；
agv多传感器融合自动导航定位模块，其用于获取建筑测量机器人实时的定位信息；
两站数据初始配准模块，其用于根据所述单站数据融合采集模块采集到的三维点云数据，并基于所述agv多传感器融合自动导航定位模块得到的建筑测量机器人的定位信息，对两个相邻测量站点的三维点云数据进行初始配准；
两站数据融合特征提取与匹配模块，其用于根据初始配准后的三维点云数据，对相邻测量站点的全景rgb图像和深度图像与三维点云中的融合特征进行提取和匹配；
两站数据精确拼接模块，其用于根据提取和匹配的结果，完成三维点云拼接，最终完成三维建模。
作为本发明进一步的改进，所述单站数据融合采集模块具体包括：
深度相机数据采集模块，其用于利用双轴旋转云台在各个旋转角度上的旋转，带动双目结构光深度相机的旋转，获取单个测量站点的全景式360度的图像数据采集，进而得到对应的全景三维点云；
扫描雷达数据采集模块，其用于所述双轴旋转云台在各个旋转角度旋转的同时，多线激光扫描雷达保持扫描，获取对应的全景三维点云；
全景数据融合模块，其用于利用事先标定好的所述多线激光扫描雷达与所述双目结构光深度相机的外参，将所述多线激光扫描雷达和所述双目结构光深度相机所获得的该测量站点处的全景三维点云进行融合拼接处理，最终得到该测量站点处的单站三维点云数据以及rgb高清图像数据。
作为本发明进一步的改进，所述深度相机数据采集模块中，在获取深度图像对应的三维点云坐标时：根据采集到的rgb图像的内参，该内参为所述双目结构光深度相机在x和y方向上的焦距，以及rgb图像的中心像素坐标，通过公式(6)-(8)，根据深度图像在(xd,yd)处的像素值depth(xd,yd)，可以得到该深度图像在(xd,yd)处所对应的三维点坐标；
x＝depth(xd,yd)*(xd-cx)/fx(6)
y＝depth(xd,yd)*(yd-cy)/fy(7)
z＝depth(xd,yd)(8)
上述式中，fx和fy分别为所述双目结构光深度相机在x和y方向上的焦距，cx和cy分别为rgb图像的中心像素坐标；
通过求解所有的深度图像坐标所对应的三维点坐标后，得到该帧深度图像所对应的三维点云，该三维点云坐标系原点为所述双目结构光深度相机光心的位置。
作为本发明进一步的改进，所述全景数据融合模块中：事先标定好所述多线激光扫描雷达与所述双目结构光深度相机的外参，该外参为所述多线激光扫描雷达相对于所述双目结构光深度相机的旋转矩阵r和平移矢量t，设所述多线激光扫描雷达的物理中心为所述多线激光扫描雷达系的坐标原点，则所述多线激光扫描雷达坐标系下，所述多线激光扫描雷达所成的三维点云坐标为q＝(xlidar,ylidar,zlidar)，通过式(9)将所述多线激光扫描雷达坐标系下的所述多线激光扫描雷达所成三维点云坐标转换到所述双目结构光深度相机坐标系中，三维点云坐标变为p＝(xdepth,ydepth,zdepth)；
p＝r*q+t(9)
式中，r为所述多线激光扫描雷达相对于所述双目结构光深度相机的旋转矩阵，t为所述多线激光扫描雷达相对于所述双目结构光深度相机的平移矢量。
作为本发明进一步的改进，所述agv多传感器融合自动导航定位模块具体包括：
扫描平面获取模块，其用于通过单线激光扫描雷达获取360度二维扫描平面结构图；
里程信息获取模块，其用于不断读取车轮里程计的里程信息，计算agv底盘的实时位移信息；
姿态信息获取模块，其用于从一个测量站点开始不断获取惯性测量单元imu的姿态信息；
传感数据融合模块，其用于融合所述姿态信息与所述位移信息，基于紧耦合预积分的融合导航定位方法，得到所述建筑测量机器人实时的定位信息；
定位信息校准输出模块，其用于将所述定位信息与所述二维扫描平面结构图进行融合校准，最终输出所述建筑测量机器人的位置信息。
作为本发明进一步的改进，所述传感数据融合模块在融合时：通过所述惯性测量单元imu的预积分得到短时间内所述agv底盘的移动速度，再通过二次积分得到单位时间内所述agv底盘的移动距离，通过该移动距离与所述车轮里程计在单位时间内所得到的距离信息进行比较和综合，消除明显的位移和方向的误差，从而得到实时的定位信息。
作为本发明进一步的改进，所述定位信息校准输出模块在校准时：将当前时刻所得到的所述单线激光扫描雷达扫描的二维轮廓图像与前一时刻扫描的二维轮廓图像进行图像轮廓边缘的匹配，从而最终输出所述建筑测量机器人实时的定位坐标(x,y,z)。
作为本发明进一步的改进，所述两站数据初始配准模块中：利用采集到的三维点云数据，基于所述建筑测量机器人实时的定位信息，得到两个测量站点之间的所述建筑测量机器人的三维点坐标分别为(xn-1,yn-1,zn-1)和(xn,yn,zn)，根据公式(10)对两个测量站点的三维点云数据进行初始配准；
式中，t0为两个测量站点之间的初始变换矩阵。
作为本发明进一步的改进，所述两站数据融合特征提取与匹配模块具体包括：
全景图像校准模块，其用于根据所述双目结构光深度相机采集到的各个旋转角度方向上的rgb图像和深度图像，按照所述双轴旋转云台的旋转角度、结构几何尺寸以及手眼标定的结果，得到该测量站点经过校准后的全景rgb图像与全景深度图像；
关键点提取模块，其用于在相邻测量站点的两幅全景rgb图像中提取关键不变特征点，然后在深度图像中得到同样位置的关键不变特征点集合，即为关键点集合，并通过所述双目结构光深度相机的内参和外参，将关键点均投影至三维点云中得到关键点的三维坐标，得到三维关键点集合；
特征匹配模块，其用于根据所述关键点提取模块得到的三维关键点集合，提取成对点对的几何特征及其描述子，对两个测量站点的三维几何特征进行匹配。
作为本发明进一步的改进，所述关键点提取模块中，关键不变特征点为sift特征点、加速鲁棒特征点或surf特征点中的一种。
本发明的有益效果：
(1)本发明针对复杂的建筑工地环境，为建筑测量机器人提供了一种高效、精确和鲁棒的三维建模方法；
(2)本发明将建筑测量机器人上安装的高精度双目结构光深度相机与多线激光扫描雷达相互融合，在水平和垂直方向上的旋转，完成了单个测量站点的360度全景扫描，克服了阳光直射等光线充足的室外场景情况下，深度相机无法成像以及多线激光扫描雷达精度分辨率较低的问题；
(3)本发明基于建筑测量机器人的agv底盘上的单线激光雷达、车轮里程计及惯性测量单元imu等多传感器融合，通过agv底盘输出的自动导航定位信息，可以快速实现多测量站点之间的三维点云的初始配准；
(4)本发明融合rgb图像与三维点云，完成两者的融合特征提取与匹配，也即关键特征点的成对点对几何特征的提取与匹配，实现了多站点之间三维点云的精确拼接和建模。
附图说明
通过参照附图详细描述其示例实施例，本公开的上述和其它目标、特征及优点将变得更加显而易见。下面描述的附图仅仅是本公开的一些实施例，对于本领域的普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1是本发明实施例一所述的一种建筑测量机器人多传感器融合三维建模方法的流程示意图；
图2为本发明所采用的建筑测量机器人的结构示意图；
图3为本发明实施例二所述的一种建筑测量机器人多传感器融合三维建模系统的系统框图。
具体实施方式
下面将参照附图更详细地描述本公开的示例性实施例。
虽然附图中显示了本公开的示例性实施例，然而应当理解，可以以各种形式实现本公开而不应被这里阐述的实施例所限制。相反，提供这些实施例是为了能够更透彻地理解本公开，并且能够将本公开的范围完整的传达给本领域的技术人员以使得本发明所属技术领域的技术人员能够容易实施。正如本发明所属技术领域的技术人员能够容易理解，将在后面描述的实施例在不脱离本发明的概念和范围的基础上可变形为多种形式。在附图中尽量将相同或相似的部分用相同的附图标记表示。
在此使用的专业术语只是用来说明特定实施例而提供的，并不是用来限制本发明。在此使用的单数形式在没有表示明确的相反含义的情况下也包含复数形式。在说明书中使用的“包含”的具体化了特定的特性、领域、常数、步骤、动作、要素及/或成分，并不排除其他特定的特性、领域、常数、步骤、动作、要素、成分及/或组的存在或附加。
将下面使用的技术用语及科学用语包括在内的所有用语具有与本发明所属技术领域的技术人员一般理解的含义相同的含义。在词典中所定义的用语被补充解释为与相关技术文献和当前公开的内容相符的含义，在没有定义的情况下，不能被解释为具有非常正式的含义。
实施例1，本发明提供了一种应用于建筑测量机器人的三维建模方法，主要解决在建筑工地等复杂环境下，基于建筑测量机器人的agv底盘上的多传感器融合的技术，针对纹理和几何结构简单的室内场景，进行精确的三维建模的问题。
本发明实施例所述的一种三维建模方法需要结合使用建筑测量机器人，采用如图2所示的建筑测量机器人，该建筑测量机器人中部安装双轴旋转云台，双轴旋转云台上搭载有多线激光扫描雷达和双目结构光深度相机，建筑测量机器人底部安装agv底盘，agv底盘上搭载有单线激光雷达、车轮里程计及惯性测量单元imu等多个传感器。
如图1所示，本发明的一种建筑测量机器人多传感器融合三维建模方法，包括：
步骤1，分别采集单个测量站点在各个旋转角度方向上的rgb图像和深度图像，融合得到单个测量站点处的三维点云数据以及rgb高清图像数据。
进一步的，步骤1具体包括：
步骤101，利用双轴旋转云台在各个旋转角度上的旋转，带动双目结构光深度相机的旋转，获取单个测量站点的全景式360度的图像数据采集，进而得到对应的全景三维点云。
在获取全景360度的图像数据的过程中，利用建筑机器人的移动特性，从一个测量站点移动至另外一个测量站点，分别采集各个测量站点的图像数据。当建筑测量机器人行驶到测量站点时，建筑测量机器上的双轴旋转云台在垂直方向旋转带动建筑测量机器上的双目结构光深度相机进行固定角度的多次俯仰旋转，采集垂直方向180度范围内的多帧rgb图像和深度图像。根据采集到的rgb图像的内参，以及采集到的深度图像的像素值，得到深度图像所对应的三维点云坐标。然后，双轴旋转云台在水平方向顺时针沿着固定角度旋转，再进行垂直方向上的俯仰旋转，依次类推，双目结构光深度相机最终完成单个测量站点的全景式360度的图像数据采集，进而可以根据前述方法计算得到对应的全景三维点云。
进一步的，获取深度图像三维点云坐标的具体方法为，根据采集到的rgb图像的内参，该内参为双目结构光深度相机在x和y方向上的焦距，以及rgb图像的中心像素坐标，通过公式(1)-(3)，根据深度图像在(xd,yd)处的像素值depth(xd,yd)，可以得到该深度图像在(xd,yd)处所对应的三维点坐标；
x＝depth(xd,yd)*(xd-cx)/fx(1)
y＝depth(xd,yd)*(yd-cy)/fy(2)
z＝depth(xd,yd)(3)
上述式中，fx和fy分别为所述双目结构光深度相机在x和y方向上的焦距，cx和cy分别为rgb图像的中心像素坐标；
通过求解所有的深度图像坐标所对应的三维点坐标后，得到该帧深度图像所对应的三维点云，该三维点云坐标系原点为所述双目结构光深度相机光心的位置。
步骤102，双轴旋转云台在垂直方向以及水平方向上各个旋转角度旋转的同时，建筑测量机器上的多线激光扫描雷达保持扫描，扫描获取对应的全景三维点云。
步骤103，利用事先标定好的多线激光扫描雷达与双目结构光深度相机的外参，将所述多线激光扫描雷达和所述双目结构光深度相机所获得的该测量站点处的全景三维点云进行融合拼接处理，最终得到该测量站点处的单站三维点云数据以及rgb高清图像数据。
进一步的，该步骤的具体方法为：事先标定好多线激光扫描雷达与双目结构光深度相机的外参，该外参为多线激光扫描雷达相对于双目结构光深度相机的旋转矩阵和平移矢量，设多线激光扫描雷达的物理中心为多线激光扫描雷达系的坐标原点，则多线激光扫描雷达坐标系下，多线激光扫描雷达所成的三维点云坐标为q＝(xlidar,ylidar,zlidar)，通过式(4)将多线激光扫描雷达坐标系下的多线激光扫描雷达所成三维点云坐标转换到双目结构光深度相机坐标系中，三维点云坐标变为p＝(xdepth,ydepth,zdepth)；
p＝r*q+t(4)
式中，r为多线激光扫描雷达相对于双目结构光深度相机的旋转矩阵，t为多线激光扫描雷达相对于双目结构光深度相机的平移矢量。
上述步骤将建筑测量机器人上安装的高精度双目结构光深度相机与多线激光扫描雷达相互融合，通过建筑测量机器上的双轴旋转云台在水平和垂直方向上的旋转，完成了单个测量站点的360度全景扫描，进而可以实现多个测量站点的360度全景扫描，克服了阳光直射等光线充足的室外场景下，深度相机无法成像以及多线激光扫描雷达扫描精度分辨率较低的问题。由于采用了多传感器融合方法，对不同传感器在相近时间段获取的时间进行结合，双目结构光深度相机与多线激光扫描雷达的数据融合后可以消除双目结构光深度相机与多线激光扫描雷达之间的误差，并融合互补，获取更为完整的数据，从而可以适应不同复杂程度的建筑工地环境，保证本发明的三维建模方法所获取的三维模型的准确性和稳定性。
步骤2，多传感器融合自动导航定位，获取到建筑测量机器人实时的定位信息。
本实施例中，在进行导航定位时，利用建筑测量机器人agv底盘上的单线激光雷达、车轮里程计及惯性测量单元imu等多个传感器来融合实现。
进一步的，步骤2具体包括：
步骤201，通过单线激光扫描雷达获取360度二维扫描平面结构图。
步骤202，不断读取车轮里程计的里程信息，计算所述agv底盘实时的位移信息。
步骤203，从一个测量站点开始不断获取惯性测量单元imu的姿态信息。
步骤204，融合惯性测量单元imu的姿态信息与车轮里程计的位移信息，基于紧耦合预积分的融合导航定位方法，得到建筑测量机器人的实时精确的定位信息。
进一步的，该步骤的具体方法为：通过惯性测量单元imu的预积分得到短时间内agv底盘的移动速度，再通过二次积分得到单位时间内agv底盘的移动距离，通过该移动距离与车轮里程计在单位时间内所得到的距离信息进行比较和综合，消除明显的位移和方向的误差，从而得到实时精确的定位信息。
步骤205，将步骤204获得的定位信息与单线激光扫描雷达的二维扫描平面结构图进行融合校准，最终输出建筑测量机器人的位置信息。该步骤的具体方法为：将当前时刻所得到的所述单线激光扫描雷达扫描的二维轮廓图像与前一时刻扫描的二维轮廓图像进行图像轮廓边缘的匹配，从而最终输出所述建筑测量机器人的实时精确的定位坐标(x,y,z)。
上述方法基于建筑测量机器人的agv底盘上的单线激光雷达、车轮里程计及惯性测量单元imu等多传感器，将gv底盘上的多个传感器数据融合校准后，可以通过agv底盘自动输出的导航定位信息，进而可以为后续快速实现多测量站点之间的三维点云的初始配准提供匹配基础。
步骤3，根据步骤1采集到的三维点云数据，并基于步骤2得到的建筑测量机器人的定位信息，对两个相邻测量站点的三维点云数据进行初始配准。
进一步的，步骤3的具体方法为：利用采集到的三维点云数据，基于建筑测量机器人的实时精确的定位信息，得到两个测量站点之间的建筑测量机器人的三维点坐标分别为(xn-1,yn-1,zn-1)和(xn,yn,zn)，根据公式(5)对两个测量站点的三维点云数据进行初始配准；
式中，t0为两个测量站点之间的初始变换矩阵。
上述步骤完成了多个测量站点之间的三维点云的初始配准，为后续三维点云中特征的提取与匹配提供了基准，提高了提取和匹配的准确度和精度。
步骤4，根据初始配准后的三维点云数据，对相邻测量站点的全景rgb图像和深度图像与三维点云中的融合特征进行提取和匹配。
进一步的，步骤4具体包括：
步骤401，根据双目结构光深度相机采集到的各个旋转角度方向上的rgb图像和深度图像，按照双轴旋转云台的旋转角度、结构几何尺寸以及手眼标定的结果，得到该测量站点经过校准后的全景rgb图像与全景深度图像。
步骤402，在相邻测量站点的两幅全景rgb图像中提取关键不变特征点，然后在深度图像中得到同样位置的关键不变特征点集合，即为关键点集合，并通过双目结构光深度相机的内参(即双目结构光深度相机在x和y方向上的焦距)和外参(即双目结构光深度相机的旋转矩阵r和平移矢量t)，将关键点均投影至三维点云中得到关键点的三维坐标，得到三维关键点集合。其中，关键不变特征点为sift(scale-invariantfeaturetransform尺度不变)特征点、加速鲁棒特征点或surf(speeded-uprobustfeatures)特征点中的一种。当然，关键点也不仅仅限于列举出来的几种，其他适用于区分及快速提取的特征都可以应用在本发明中。
步骤403，根据步骤402得到的三维关键点集合，提取成对点对的几何特征(kppf)及其描述子，对两个测量站点的三维几何特征进行匹配。
上述步骤将rgb图像与三维点云进行融合，完成两者的融合特征提取与匹配，提取和匹配出关键特征点的成对点对几何特征，为后续实现了多站点之间三维点云的精确拼接提供基础和保证，提高拼接的精度。
步骤5，根据提取和匹配的结果，将初始配准的三维点云进行进一步的精确拼接，然后利用结合rgb彩色信息的迭代最近邻点匹配算法(coloricp)完成精确的三维点云拼接，最终完成三维建模。
本发明实施例所述的一种建筑测量机器人多传感器融合三维建模方法，通过建筑测量机器人上安装的双目结构光深度相机与多线扫描激光雷达的水平和垂直方向上的旋转，完成单站点360度全景扫描。融合建筑测量机器人的agv底盘上的单线激光扫描雷达、车轮里程计以及惯性测量单元imu等多个传感器信息，实现agv底盘自动输出导航定位信息，完成多测量站点之间三维点云的初始配准。融合rgb图像与三维点云，完成关键特征点的成对点对几何特征的提取与匹配，实现多站点之间三维点云的精确拼接。本发明所提供的这种三维建模方法，适用于复杂的建筑工地环境，避免单一传感器在三维建模上的不足，将多个传感器数据进行融合，利用各个传感器各自的优势，也弥补各个传感器之间的劣势，融合互补，获取较为完整的数据，消除传感器见的误差，提高了三维建模的准确性和稳定性，使得建筑测量机器人在进行三维建模时更高效，更精确，更鲁棒。
实施例2，如图3所示，本发明还提供了一种建筑测量机器人多传感器融合三维建模系统，包括：单站数据融合采集模块、agv多传感器融合自动导航定位模块、两站数据初始配准模块、两站数据融合特征提取与匹配模块和两站数据精确拼接模块。
单站数据融合采集模块用于通过建筑测量机器人分别采集单个测量站点在各个旋转角度方向上的rgb图像和深度图像，融合得到单个测量站点处的三维点云数据以及rgb高清图像数据。
进一步的，单站数据融合采集模块具体包括：深度相机数据采集模块、扫描雷达数据采集模块和全景数据融合模块。
其中，深度相机数据采集模块用于利用双轴旋转云台在各个旋转角度上的旋转，带动双目结构光深度相机的旋转，获取单个测量站点的全景式360度的图像数据采集，进而得到对应的全景三维点云。
在获取全景360度的图像数据的过程中，利用建筑机器人的移动特性，从一个测量站点移动至另外一个测量站点，分别采集各个测量站点的图像数据。当建筑测量机器人行驶到测量站点时，建筑测量机器上的双轴旋转云台先在垂直方向旋转带动建筑测量机器上的双目结构光深度相机进行固定角度的多次俯仰旋转，此时双目结构光深度相机采集垂直方向180度范围内的多帧rgb图像和深度图像，根据采集到的rgb图像的内参以及采集到的深度图像的像素值，得到深度图像所对应的三维点云坐标。然后双轴旋转云台在水平方向顺时针沿着固定角度旋转，再进行垂直方向上的俯仰旋转，依次类推，双目结构光深度相机最终完成单个测量站点的全景式360度的图像数据采集，进而得到对应的全景三维点云。
进一步的，深度相机数据采集模块在获取深度图像对应的三维点云坐标时：根据采集到的rgb图像的内参，该内参为双目结构光深度相机在x和y方向上的焦距，以及rgb图像的中心像素坐标，通过公式(6)-(8)，根据深度图像在(xd,yd)处的像素值depth(xd,yd)，可以得到该深度图像在(xd,yd)处所对应的三维点坐标；
x＝depth(xd,yd)*(xd-cx)/fx(6)
y＝depth(xd,yd)*(yd-cy)/fy(7)
z＝depth(xd,yd)(8)
上述式中，fx和fy分别为双目结构光深度相机在x和y方向上的焦距，cx和cy分别为rgb图像的中心像素坐标；
通过求解所有的深度图像坐标所对应的三维点坐标后，得到该帧深度图像所对应的三维点云，该三维点云坐标系原点为双目结构光深度相机光心的位置。
扫描雷达数据采集模块用于在双轴旋转云台在垂直方向及水平方向上各个旋转角度旋转的同时，建筑测量机器上的多线激光扫描雷达保持扫描，获取对应的全景三维点云。
全景数据融合模块用于利用事先标定好的多线激光扫描雷达与双目结构光深度相机的外参，将多线激光扫描雷达和双目结构光深度相机所获得的该测量站点处的全景三维点云进行融合拼接处理，最终得到该测量站点处的单站三维点云数据以及rgb高清图像数据。
进一步的，全景数据融合模块中：事先标定好多线激光扫描雷达与双目结构光深度相机的外参，该外参为多线激光扫描雷达相对于双目结构光深度相机的旋转矩阵r和平移矢量t，设多线激光扫描雷达的物理中心为多线激光扫描雷达系的坐标原点，则多线激光扫描雷达坐标系下，多线激光扫描雷达所成的三维点云坐标为q＝(xlidar,ylidar,zlidar)，通过式(9)将多线激光扫描雷达坐标系下的多线激光扫描雷达所成三维点云坐标转换到双目结构光深度相机坐标系中，三维点云坐标变为p＝(xdepth,ydepth,zdepth)；
p＝r*q+t(9)
式中，r为多线激光扫描雷达相对于双目结构光深度相机的旋转矩阵，t为多线激光扫描雷达相对于双目结构光深度相机的平移矢量。
上述单站数据融合采集模块将建筑测量机器人上安装的高精度双目结构光深度相机与多线激光扫描雷达相互融合，通过建筑测量机器上的双轴旋转云台在水平和垂直方向上的旋转，完成了单个测量站点的360度全景扫描，进而可以实现多个测量站点的360度全景扫描，克服了阳光直射等光线充足的室外场景下，深度相机无法成像以及多线激光扫描雷达扫描精度分辨率较低的问题。该单站数据融合采集模块将双目结构光深度相机与多线激光扫描雷达进行融合，对不同传感器在相近时间段获取的时间进行结合，融合后可以消除双目结构光深度相机与多线激光扫描雷达之间的误差，并融合互补，获取更为完整的数据，从而可以适应不同复杂程度的建筑工地环境，保证本发明的三维建模系统所获取的三维模型的准确性和稳定性。
agv多传感器融合自动导航定位模块获取建筑测量机器人实时的定位信息。
本实施例中，在进行导航定位时，利用建筑测量机器人agv底盘上的单线激光雷达、车轮里程计及惯性测量单元imu等多个传感器来融合实现。其中，agv多传感器融合自动导航定位模块具体包括：扫描平面获取模块、里程信息获取模块和姿态信息获取模块。
扫描平面获取模块用于通过单线激光扫描雷达获取360度二维扫描平面结构图。
里程信息获取模块用于不断读取车轮里程计的里程信息，计算agv底盘实时的位移信息。
姿态信息获取模块用于从一个测量站点开始不断获取惯性测量单元imu的姿态信息。
传感数据融合模块用于融合惯性测量单元imu的姿态信息与车轮里程计的位移信息，基于紧耦合预积分的融合导航定位方法，得到建筑测量机器人的实时精确的定位信息。
进一步的，在融合时：通过惯性测量单元imu的预积分得到短时间内agv底盘的移动速度，再通过二次积分得到单位时间内agv底盘的移动距离，通过该移动距离与车轮里程计在单位时间内所得到的距离信息进行比较和综合，消除明显的位移和方向的误差，从而得到实时精确的定位信息。
定位信息校准输出模块用于将传感数据融合模块获得的定位信息与单线激光扫描雷达的二维扫描平面结构图进行融合校准，最终输出建筑测量机器人的位置信息。在校准时：将当前时刻所得到的单线激光扫描雷达扫描的二维轮廓图像与前一时刻扫描的二维轮廓图像进行图像轮廓边缘的匹配，从而最终输出建筑测量机器人的实时精确的定位坐标(x,y,z)。
上述agv多传感器融合自动导航定位模块基于建筑测量机器人的agv底盘上的单线激光雷达、车轮里程计及惯性测量单元imu等多传感器，将agv底盘上的多个传感器数据融合校准后，可以通过agv底盘自动输出的导航定位信息，进而可以为后续快速实现多测量站点之间的三维点云的初始配准提供匹配基础。
两站数据初始配准模块用于根据单站数据融合采集模块采集到的三维点云数据，并基于agv多传感器融合自动导航定位模块得到的建筑测量机器人的定位信息，对两个相邻测量站点的三维点云数据进行初始配准。
进一步的，两站数据初始配准模块中：利用采集到的三维点云数据，基于建筑测量机器人的实时精确的定位信息，得到两个测量站点之间的建筑测量机器人的三维点坐标分别为(xn-1,yn-1,zn-1)和(xn,yn,zn)，根据公式(10)对两个测量站点的三维点云数据进行初始配准；
式中，t0为两个测量站点之间的初始变换矩阵。
上述两站数据初始配准模块实现了多个测量站点之间的三维点云的初始配准，为后续三维点云中特征的提取与匹配提供了基准，提高了整个系统提取和匹配的准确度和精度。
两站数据融合特征提取与匹配模块用于根据初始配准后的三维点云数据，对相邻测量站点的全景rgb图像和深度图像与三维点云中的融合特征进行提取和匹配。具体的，两站数据融合特征提取与匹配模块具体包括：全景图像校准模块、关键点提取模块和特征匹配模块。
其中，全景图像校准模块用于根据双目结构光深度相机采集到的各个旋转角度方向上的rgb图像和深度图像，按照双轴旋转云台的旋转角度、结构几何尺寸以及手眼标定的结果，得到该测量站点经过校准后的全景rgb图像与全景深度图像。
关键点提取模块用于在相邻测量站点的两幅全景rgb图像中提取关键不变特征点，然后在深度图像中得到同样位置的关键不变特征点集合，即为关键点集合，并通过双目结构光深度相机的内参和外参，将关键点均投影至三维点云中得到关键点的三维坐标，得到三维关键点集合。其中，关键不变特征点为sift特征点、加速鲁棒特征点或surf特征点中的一种。当然，关键点也不仅仅限于列举出来的几种，其他适用于区分及快速提取的特征都可以应用在本发明中。
特征匹配模块用于根据关键点提取模块得到的三维关键点集合，提取成对点对的几何特征及其描述子，对两个测量站点的三维几何特征进行匹配。
上述两站数据融合特征提取与匹配模块将rgb图像与三维点云进行融合，完成两者的融合特征提取与匹配，提取和匹配出关键特征点的成对点对几何特征，为后续实现了多站点之间三维点云的精确拼接提供基础和保证，提高拼接的精度。
两站数据精确拼接模块用于根据提取和匹配的结果，将初始配准的三维点云进行进一步的精确拼接，然后利用结合rgb彩色信息的迭代最近邻点匹配算法完成精确的三维点云拼接，最终完成三维建模。
本发明实施例所述的一种建筑测量机器人多传感器融合三维建模系统，通过建筑测量机器人上安装的双目结构光深度相机与多线扫描激光雷达的水平和垂直方向上的旋转，完成单站点360度全景扫描。融合建筑测量机器人的agv底盘上的单线激光扫描雷达、车轮里程计以及惯性测量单元imu等信息，实现agv底盘自动输出导航定位信息，完成多测量站点之间三维点云的初始配准。融合rgb图像与三维点云，完成关键特征点的成对点对几何特征的提取与匹配，实现多站点之间三维点云的精确拼接。本发明所提供的这种三维建模系统，适用于复杂的建筑工地环境，避免单一传感器在三维建模上的不足，将多个传感器数据进行融合，利用各个传感器各自的优势，也弥补各个传感器之间的劣势，融合互补，获取较为完整的数据，消除传感器见的误差，提高了三维建模的准确性和稳定性，使得建筑测量机器人在进行三维建模时更高效，更精确，更鲁棒。
至此，本领域技术人员应认识到，虽然本文已详尽示出和描述了本发明的多个示例性实施例，但是，在不脱离本发明精神和范围的情况下，仍可根据本发明公开的内容直接确定或推导出符合本发明原理的许多其他变型或修改。因此，本发明的范围应被理解和认定为覆盖了所有这些其他变型或修改。
在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。
类似地，应当理解，为了精简本公开并帮助理解各个发明方面中的一个或多个，在上面对本发明的示例性实施例的描述中，本发明的各个特征有时被一起分组到单个实施例、图、或者对其的描述中。然而，并不应将该公开的方法解释成反映如下意图：即所要求保护的本发明要求比在每个权利要求中所明确记载的特征更多的特征。更确切地说，如下面的权利要求书所反映的那样，发明方面在于少于前面公开的单个实施例的所有特征。因此，遵循具体实施方式的权利要求书由此明确地并入该具体实施方式，其中每个权利要求本身都作为本发明的单独实施例。
此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包括的某些特征而不是其它特征，但是不同实施例的特征的组合意味着处于本发明的范围之内并且形成不同的实施例。例如，在权利要求书中，所要求保护的实施例的任意之一都可以以任意的组合方式来使用。
至此，本领域技术人员应认识到，虽然本文已详尽示出和描述了本发明的多个示例性实施例，但是，在不脱离本发明精神和范围的情况下，仍可根据本发明公开的内容直接确定或推导出符合本发明原理的许多其他变型或修改。因此，本发明的范围应被理解和认定为覆盖了所有这些其他变型或修改。
技术特征：
1.一种建筑测量机器人多传感器融合三维建模方法，其特征在于，包括：
步骤1，采集单个测量站点在各个旋转角度方向上的rgb图像和深度图像，融合得到单个测量站点处的三维点云数据以及rgb高清图像数据；
步骤2，多传感器融合自动导航定位，获取建筑测量机器人实时的定位信息；
步骤3，根据步骤1采集到的三维点云数据，并基于步骤2得到的建筑测量机器人的定位信息，对两个相邻测量站点的三维点云数据进行初始配准；
步骤4，根据初始配准后的三维点云数据，对相邻测量站点的全景rgb图像和深度图像与三维点云中的融合特征进行提取和匹配；
步骤5，根据提取和匹配的结果，完成三维点云拼接，最终完成三维建模。
2.如权利要求1所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤1具体包括：
步骤101，利用双轴旋转云台在各个旋转角度上的旋转，带动双目结构光深度相机的旋转，获取单个测量站点的全景式360度的图像数据采集，进而得到对应的全景三维点云；
步骤102，双轴旋转云台在各个旋转角度旋转的同时，多线激光扫描雷达保持扫描，获取对应的全景三维点云；
步骤103，利用事先标定好的所述多线激光扫描雷达与所述双目结构光深度相机的外参，将所述多线激光扫描雷达和所述双目结构光深度相机所获得的该测量站点处的全景三维点云进行融合拼接处理，最终得到该测量站点处的单站三维点云数据以及rgb高清图像数据。
3.如权利要求2所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤101中，在获取深度图像对应的三维点云坐标时：根据采集到的rgb图像的内参，该内参为所述双目结构光深度相机在x和y方向上的焦距，以及rgb图像的中心像素坐标，通过公式(1)-(3)，根据深度图像在(xd,yd)处的像素值depth(xd,yd)，可以得到该深度图像在(xd,yd)处所对应的三维点坐标；
x＝depth(xd,yd)*(xd-cx)/fx(1)
y＝depth(xd,yd)*(yd-cy)/fy(2)
z＝depth(xd,yd)(3)
上述式中，fx和fy分别为所述双目结构光深度相机在x和y方向上的焦距，cx和cy分别为rgb图像的中心像素坐标；
通过求解所有的深度图像坐标所对应的三维点坐标后，得到该帧深度图像所对应的三维点云，该三维点云坐标系原点为所述双目结构光深度相机光心的位置。
4.如权利要求2所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤103的具体方法为：事先标定好所述多线激光扫描雷达与所述双目结构光深度相机的外参，该外参为所述多线激光扫描雷达相对于所述双目结构光深度相机的旋转矩阵r和平移矢量t，设所述多线激光扫描雷达的物理中心为所述多线激光扫描雷达系的坐标原点，则所述多线激光扫描雷达坐标系下，所述多线激光扫描雷达所成的三维点云坐标为q＝(xlidar,ylidar,zlidar)，通过式(4)将所述多线激光扫描雷达坐标系下的所述多线激光扫描雷达所成三维点云坐标转换到所述双目结构光深度相机坐标系中，三维点云坐标变为p＝(xdepth,ydepth,zdepth)；
p＝r*q+t(4)
式中，r为所述多线激光扫描雷达相对于所述双目结构光深度相机的旋转矩阵，t为所述多线激光扫描雷达相对于所述双目结构光深度相机的平移矢量。
5.如权利要求1所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤2具体包括：
步骤201，通过单线激光扫描雷达获取360度二维扫描平面结构图；
步骤202，不断读取车轮里程计的里程信息，计算agv底盘实时的位移信息；
步骤203，从一个测量站点开始不断获取惯性测量单元imu的姿态信息；
步骤204，融合所述姿态信息与所述位移信息，基于紧耦合预积分的融合导航定位方法，得到所述建筑测量机器人实时的定位信息；
步骤205，将所述定位信息与所述二维扫描平面结构图进行融合校准，最终输出所述建筑测量机器人的位置信息。
6.如权利要求5所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤204的具体方法为：通过所述惯性测量单元imu的预积分得到短时间内所述agv底盘的移动速度，再通过二次积分得到单位时间内所述agv底盘的移动距离，通过该移动距离与所述车轮里程计在单位时间内所得到的距离信息进行比较和综合，得到实时的定位信息。
7.如权利要求6所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤205的具体方法为：将当前时刻所得到的所述单线激光扫描雷达扫描的二维轮廓图像与前一时刻扫描的二维轮廓图像进行图像轮廓边缘的匹配，从而最终输出所述建筑测量机器人实时的定位坐标(x,y,z)。
8.如权利要求1所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤3的具体方法为：利用采集到的三维点云数据，基于所述建筑测量机器人实时的定位信息，得到两个测量站点之间的所述建筑测量机器人的三维点坐标分别为(xn-1,yn-1,zn-1)和(xn,yn,zn)，根据公式(5)对两个测量站点的三维点云数据进行初始配准；
式中，t0为两个测量站点之间的初始变换矩阵。
9.如权利要求1所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤4具体包括：
步骤401，根据采集到的各个旋转角度方向上的rgb图像和深度图像，按照双轴旋转云台的旋转角度、结构几何尺寸以及手眼标定的结果，得到该测量站点经过校准后的全景rgb图像与全景深度图像；
步骤402，在相邻测量站点的两幅全景rgb图像中提取关键不变特征点，然后在深度图像中得到同样位置的关键不变特征点集合，即为关键点集合，并通过所述双目结构光深度相机的内参和外参，将关键点均投影至三维点云中得到关键点的三维坐标，得到三维关键点集合；
步骤403，根据步骤402得到的三维关键点集合，提取成对点对的几何特征及其描述子，对两个测量站点的三维几何特征进行匹配。
10.如权利要求9所述的一种建筑测量机器人多传感器融合三维建模方法，其特征在于，步骤402中，关键不变特征点为sift特征点、加速鲁棒特征点和surf特征点中的一种。
11.一种利用权利要求1-10中任意一项所述的建筑测量机器人多传感器融合三维建模方法所获取的建筑测量机器人多传感器融合三维建模系统，其特征在于，包括：
单站数据融合采集模块，其用于采集单个测量站点在各个旋转角度方向上的rgb图像和深度图像，融合得到单个测量站点处的三维点云数据以及rgb高清图像数据；
agv多传感器融合自动导航定位模块，其用于获取建筑测量机器人的实时定位信息；
两站数据初始配准模块，其用于根据所述单站数据融合采集模块采集到的三维点云数据，并基于所述agv多传感器融合自动导航定位模块得到的建筑测量机器人的定位信息，对两个相邻测量站点的三维点云数据进行初始配准；
两站数据融合特征提取与匹配模块，其用于根据初始配准后的三维点云数据，对相邻测量站点的全景rgb图像和深度图像与三维点云中的融合特征进行提取和匹配；
两站数据精确拼接模块，其用于根据提取和匹配的结果，完成三维点云拼接，最终完成三维建模。
技术总结
本发明提供一种建筑测量机器人多传感器融合三维建模方法，通过双目结构光深度相机与多线扫描激光雷达在水平和垂直方向上的旋转，完成单站点360度全景扫描；融合单线激光扫描雷达、车轮里程计以及惯性测量单元IMU等多传感器信息，自动输出导航定位信息，完成多测量站点间三维点云的初始配准；融合RGB图像与三维点云，完成关键特征点的成对点对几何特征的提取与匹配，实现多站点间三维点云的精确拼接。本发明还提供一种建筑测量机器人多传感器融合三维建模系统。本发明解决在建筑工地等复杂环境下，基于多传感器融合，针对纹理和几何结构简单的室内场景，进行精确的三维建模的问题。
技术研发人员：赵键;陈刚;刘波
受保护的技术使用者：广东博智林机器人有限公司
技术研发日：.11.19
技术公布日：.02.28
