一种考虑机械臂运动约束的三维重建中相机位姿估计方法技术

本发明专利技术公开了一种考虑机械臂运动约束的三维重建中相机位姿估计方法，包括：搭建机器人手眼视觉系统，同步获取RGBD相机的深度图像和彩色图像以及对应帧的机械臂位姿数据；对深度图像进行预处理，并将深度图像对齐到彩色图像；分别利用当前帧和上一帧的彩色图像、深度图像以及对应帧的机械臂位姿数据构建相应的光度误差函数、几何误差函数和三维坐标误差函数；对三种误差函数进行加权联合生成最终误差函数；对最终误差函数进行非线性迭代优化，求解相机的相对位姿。本发明专利技术在三维重建过程中，利用RGBD相机所输出的深度图像和彩色图像以及对应帧的机械臂位姿数据构建相应误差函数，通过联合优化三种误差函数提高了相机位姿估计的准确度。计的准确度。计的准确度。

A method of camera pose estimation in 3D reconstruction considering manipulator motion constraints

【技术实现步骤摘要】
一种考虑机械臂运动约束的三维重建中相机位姿估计方法


[0001]本专利技术涉及位姿估计
，具体为一种考虑机械臂运动约束的三维重建中相机位姿估计方法。

技术介绍

[0002]考虑将RGBD相机固定在机械臂末端，利用机械臂的运动带动相机运动，进而利用连续帧的图像信息完成对机器人周围场景进行三维重建，该应用背景在遥操作等任务执行过程中，所获得的三维场景视觉信息将为主端操作者提供临场沉浸感，从而提高任务执行的效率和安全性。而相机的位姿估计，即获得当前帧相机与上一帧相机之间的位姿变换矩阵将决定两帧间重建环境的拼接效果，进而影响整个三维重建的精度。
[0003]根据RGBD相机可同时传回彩色图像和深度图像，当前三维重建中的相机位姿估计方法可分为两大类：基于彩色图像信息和基于深度图像信息。利用彩色图像信息的方法有特征点法、直接法等，特征点法通过对相邻帧图像进行特征点提取和匹配以估计位姿，该方法不适用于纹理较弱或纹理重复性比较高的场景；直接法基于光度不变性，考虑对应像素点的灰度约束，使用图像的梯度估计两帧间的位姿转换关系，由此该方法对光度变化比较敏感，并且由于图像的非凸性，容易陷入局部最优。基于深度图像信息的方法主要是迭代最邻近点（ICP
‑
Iterative Closest Point）算法，将深度信息转化为点云，通过不断迭代寻找两幅点云中对应的匹配点，利用对应点对的几何约束求解两帧间的位姿转换关系，对于3D几何特征特征不明显的场景，相邻帧几何匹配极容易出现问题，导致算法出现较大偏差。当前应用广泛的三维重建算法采用联合优化的方法，即最小化光度误差和几何误差来求解相对相机位姿。但该方法在重建图像整体梯度变化较小、几何特点不明显的场景时，极容易陷入局部最优而导致位姿估计不准，进而使得整个三维重建失败。
[0004]当RGBD相机作为视觉传感器安装在机械臂末端，对环境进行三维重建过程时，RGBD相机位姿必然受到机械臂运动的约束。因此在该应用场景中，如何解决传统方法存在的对重建场景具有局限性的问题，能够充分利用各传感器信息，联合考虑各种约束获取更为精准的相机位姿，进而提高三维重建的精度，是一个值得关注的问题。

技术实现思路

[0005]本专利技术旨在至少在一定程度上解决相关技术中的技术问题之一。为此，本专利技术的目的在于提供了一种考虑机械臂运动约束的三维重建中相机位姿估计方法，在三维重建过程中，利用RGBD相机所输出的深度图像和彩色图像以及对应帧的机械臂位姿数据构建相应误差函数，通过联合优化三种误差函数提高了相机位姿估计的准确度。
[0006]为实现上述目的，本专利技术第一方面实施例提出了一种考虑机械臂运动约束的三维重建中相机位姿估计方法，包括：搭建机器人手眼视觉系统，其中，RGBD相机安装在机械臂末端；同步获取RGBD相机的深度图像和彩色图像以及对应帧的机械臂位姿数据；
对深度图像进行预处理，并将深度图像对齐到彩色图像；分别利用当前帧和上一帧的彩色图像、深度图像以及对应帧的机械臂位姿数据构建相应的光度误差函数、几何误差函数和三维坐标误差函数；对三种误差函数进行加权联合生成最终误差函数；对最终误差函数进行非线性迭代优化，求解相机的相对位姿。
[0007]根据本专利技术的第一个实施例，所述搭建机器人手眼视觉系统包括进行相机标定和手眼标定，获得RGBD相机内参和手眼位姿矩阵。
[0008]根据本专利技术的第一个实施例，所述RGBD相机内参包括彩色相机的内参、深度相机的内参以及彩色相机与深度相机之间的位姿变换矩阵。
[0009]根据本专利技术的第一个实施例，所述同步获取RGBD相机的深度图像和彩色图像以及对应帧的机械臂位姿数据包括：根据周围场景设定预设轨迹，驱动机械臂按照预设轨迹运行扫描周围场景。
[0010]根据本专利技术的第一个实施例，所述对深度图像进行预处理包括去除有效距离之外的点。
[0011]根据本专利技术的第一个实施例，所述构建相应的光度误差函数包括：根据直接法，假设一个空间点在各个视角下成像的灰度是不变的，则根据空间点在当前时刻和上一帧时刻两幅彩色图像上的光度误差，构建光度误差函数，公式为：，其中，和分别表示空间点在当前时刻和上一帧时刻两幅彩色图像上的投影像素坐标，和分别表示空间点在当前时刻和上一帧时刻两幅彩色图像上的对应深度，代表RGBD相机中彩色相机的内参，代表估计的时刻与时刻之间相机的相对位姿，和分别表示空间点在当前时刻和上一帧刻两幅彩色图像上的对应光度。
[0012]根据本专利技术的第一个实施例，所述构建相应的几何误差函数包括：将相邻两帧深度图像分别转化为点云，根据空间点在当前帧和上一帧两幅点云上的几何误差，构建几何误差函数，公式为：，其中，和分别表示空间点在当前时刻和上一帧时刻相机坐标系下的三维坐标，代表估计的时刻与时刻之间相机的相对位姿，表示空间点在上一帧时刻的相机坐标系下的三维法向量。
[0013]根据本专利技术的第一个实施例，所述构建相应的三维坐标误差函数包括：
利用相邻帧的机械臂位姿，根据空间点经过当前时刻和上一帧时刻不同的位姿变换获得其在机械臂基坐标系下的三维坐标误差，构建三维坐标误差函数，公式为：；其中，表示空间点在当前时刻相机坐标系下的三维坐标，表示手眼位姿矩阵，代表估计的时刻与时刻之间相机的相对位姿，和分别表示当前时刻和上一帧时刻的机械臂位姿。
[0014]根据本专利技术的第一个实施例，所述对三种误差函数进行加权联合生成最终误差函数公式为：，其中，为最终误差函数，为光度误差函数，为几何误差函数，为三维坐标误差函数，为几何误差函数的权重值，为三维坐标误差函数的权重值。
[0015]根据本专利技术的第一个实施例，所述对最终误差函数进行非线性迭代优化，求解相机的相对位姿包括：利用高斯牛顿法构建线性最小二乘问题：，其中雅可比矩阵和残差根据误差函数利用CUDA中高度并行的多线程计算获得，为获得上述最优值，得到如下正规方程：；在CPU上通过Cholesky分解求解上述正规方程，获得时刻与时刻之间相机的相对位姿。
[0016]与现有技术相比，本专利技术的有益效果是：本专利技术的相机位姿估计方法在三维重建过程中，除了利用RGBD相机所输出的深度图像和彩色图像进行相机位姿的估计外，同时考虑了机械臂的运动约束，通过联合优化三种误差函数获得更为精准的相机位姿；该方法打破了重建场景的局限性，可以有效克服当重建场景非结构化、几何特征不明显、弱纹理导致图像整体梯度较小时，仅考虑深度图像和彩色图像时所造成得位姿估计出现较大偏差的问题。提高了相机位姿估计的准确度，进而可以提高整个三维重建的精度。
附图说明
[0017]图1为本专利技术一种考虑机械臂运动约束的三维重建中相机位姿估计方法的流程框图；图2为本专利技术一种考虑机械臂运动约束的三维重建中相机位姿估计方法中的详细流程示意图。
具体实施方式
[0018]为了使本
的人员更好地理解本申请方案，下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施本文档来自技高网...

【技术保护点】

【技术特征摘要】
1.一种考虑机械臂运动约束的三维重建中相机位姿估计方法，其特征在于，包括：搭建机器人手眼视觉系统，其中，RGBD相机安装在机械臂末端；同步获取RGBD相机的深度图像和彩色图像以及对应帧的机械臂位姿数据；对深度图像进行预处理，并将深度图像对齐到彩色图像；分别利用当前帧和上一帧的彩色图像、深度图像以及对应帧的机械臂位姿数据构建相应的光度误差函数、几何误差函数和三维坐标误差函数；对三种误差函数进行加权联合生成最终误差函数；对最终误差函数进行非线性迭代优化，求解相机的相对位姿。2.根据权利要求1所述的考虑机械臂运动约束的三维重建中相机位姿估计方法，其特征在于，所述搭建机器人手眼视觉系统包括进行相机标定和手眼标定，获得RGBD相机内参和手眼位姿矩阵。3.根据权利要求2所述的考虑机械臂运动约束的三维重建中相机位姿估计方法，其特征在于，所述RGBD相机内参包括彩色相机的内参、深度相机的内参以及彩色相机与深度相机之间的位姿变换矩阵。4.根据权利要求3所述的考虑机械臂运动约束的三维重建中相机位姿估计方法，其特征在于，所述同步获取RGBD相机的深度图像和彩色图像以及对应帧的机械臂位姿数据包括：根据周围场景设定预设轨迹，驱动机械臂按照预设轨迹运行扫描周围场景。5.根据权利要求4所述的考虑机械臂运动约束的三维重建中相机位姿估计方法，其特征在于，所述对深度图像进行预处理包括去除有效距离之外的点。6.根据权利要求1
‑
5中任一项权利要求所述的考虑机械臂运动约束的三维重建中相机位姿估计方法，其特征在于，所述构建相应的光度误差函数包括：根据直接法，假设一个空间点在各个视角下成像的灰度是不变的，则根据空间点在当前时刻和上一帧时刻两幅彩色图像上的光度误差，构建光度误差函数，公式为：，其中，和分别表示空间点在当前时刻和上一帧时刻两幅彩色图像上的投影像素坐标，和分别表示空间点在当前时刻和上一帧时刻两幅彩色...

【专利技术属性】
技术研发人员：张雪波，周燕玲，袁明星，赵铭慧，苑晶，
申请(专利权)人：南开大学，
类型：发明
国别省市：