一种基于NURBS的无标记面状柔性体变形跟踪方法与流程

本发明属于一种柔性体变形跟踪方法技术领域，特别涉及一种基于nurbs的无标记面状肉形体变形跟踪方法。

背景技术：

现有的针对面状柔性体变形的跟踪算法，主要识别面状柔性体上的特征完成柔性体的变形跟踪。主要存在如下几个方面的局限性：针对特征较少的面状柔性体不能有效地跟踪变形体的变形；特征的遮挡影响面状柔性体的变形状态识别的准确率。

技术实现要素：

为克服已有技术的不足之处，本发明提出基于nurbs的无标记面状柔性体变形跟踪方法，该基于点云信息和nurbs数学模型对无标记面状柔性体的变形进行跟踪。

为达到此目的，本发明采用以下技术方案：

一种基于nurbs的无标记面状柔性体变形跟踪方法，该方法包括如下步骤：

步骤1：通过kinect深度相机得到含面状柔性物体点云的原始点云后，需要通过一定的流程分割出目标面状柔性体的点云，使用半径滤波或统计滤波去除空间中的噪音点云，从而得到纯净的目标点云；

步骤2：对获得的目标点云进行初始化获得初始的nurbs曲面；

步骤3：利用曲面拟合生成近似nurbs曲面，nurbs曲面是由两条nurbs曲面的张量积构成，分别针对两条nurbs曲线采用曲面拟合方法得到nurs曲面拟合结果；

步骤4：完成无标记面装柔性体的曲面拟合，通过nurbs德布尔算法得到变形前后面状柔性体上某一点的变化量。

相比于现有的技术，本发明的优点有：

(1)不需要利用面状柔性体上标记特征即可完成对面状柔性体的变形跟踪；

(2)建立了nurbs曲面数学模型和实际面状柔性体间的对应关系，通过该对应关系可以确定变形前后的面状柔性体上任意一点的变化量；

(3)提出一种描述无标记面状柔性变形体运动的方法，解决了无标记面状柔性体运动描述的问题；

(4)本发明提出一种针对无标记面状柔性体的点云处理方法，有效地从环境中分离出目标无标记面状柔性体。

附图说明

图1是本发明无标记面状柔性体点云的获取方案；

图2是本发明静止面状柔性体的变形跟踪方案；

图3是本发明面向无标记柔性体操作的视觉伺服控制系统；

图4是本发明无标记面状柔性体的运动和变形分离原理示意图

具体实施方式

本发明提出了一种基于nurbs的无标记面状柔性体变形跟踪算法。下面将结合附图，对本发明的具体实施例进行详细的描述。

图1是本发明所述的无标记面状柔性体点云的获取方案。本发明通过kinect深度相机得到含面状柔性物体点云的原始点云后，需要通过一定的流程分割出目标面状柔性体的点云。无标记的面状柔性体是纯色的且没有明显的特征。利用物体是纯色的这一特点可以使用颜色从原始点云中分割出包含目标柔性体的点云。kinectv2中的点云包含xyz信息和rgb信息，从rgb色彩空间不容易分割出纯色物体，所以本发明使用将rgb颜色空间转化为hsv颜色空间对目标点云进行分割。但同时背景环境中也可能包含这一颜色的物体，所以需要通过滤波去除该部分的噪声。目标点云较为集中，噪音点云较为分散。根据这一特点可以使用半径滤波或统计滤波去除空间中的噪音点云。经过上一步的滤波，我们可以得到纯净的目标点云。该状态下的点云较为稠密，对于描述目标柔性体的变形，许多点云信息是不必要的。考虑到需要提高程序的运行速度，我们使用降采样的方法在不过多损失变形信息的情况下降低点云的密度。

本发明将采用nurbs曲面对目标点云进行拟合。但是本发明并不是实时曲面拟合生成一个新曲面，而是通过点云的信息实时更新初始化生成的曲面的形状。使当前曲面的形状贴合点云形状的变化。这样做的原因是描述柔性体的变形需要知道当前状态下与前一状态下的差别，也就是柔性体的变形跟踪具有记忆性。假设有一个基准，需要将当前状态不停地与基准进行比较，从而得到当前的变形量。未变形的物体状态是一个基准，变形的情况是对比这个基准得到的。本发明在描述面状柔性体的变形时通过初始化一个曲面，将该曲面作为面状变形体未变形的基准。这样保证了面状柔性体变形的前后，曲面和面状柔性体之间的对应关系不变，变形的量可以通过计算曲面的特征得到。而实时曲面拟合生成新曲面的方式并不保证对应关系，所以也不适合变形体的变形描述。传统的实时三维重建虽然可以实时测量柔性体的状态变化，但是并没有前后帧状态的对比，也就是没有记忆性。所以本发明并不使用全局近似拟合或全局插值拟合等全局的曲面拟合方法重建柔性体的状态，而是通过局部改变nurbs曲面的形状。这一方法属于nurbs的形状修改方法。nurbs曲面通过定义控制点、权重和节点向量生成。改变这三者中的任意一项都可以局部改变nurbs曲面的形状。改变节点向量对应nurbs曲面的形状改变是不直观的，而改变控制点的位置和权重是直观的，所以本发明将通过改变后两种方式局部的改变nurbs曲面的形状。基于此，本发明制定的静止面状柔性体的变形跟踪方案如图2所示。

基于位置的视觉伺服系统，使用已经标定的相机，通过相机观察到目标物体的位姿，将这个相机下的信息经过坐标变换转移到机器人基坐标系下的笛卡尔空间下进行机器人控制。如图3所示，基于位置的视觉伺服系统包含了两个闭环的控制环，内环是各个关节的速度控制与反馈，外环是机械臂基坐标系下笛卡尔空间的速度控制与反馈。视觉部分包含了点云的获取，曲面的拟合以及曲面上的取点。通过视觉系统拿到目标点的坐标后，给定姿态和机械臂末端姿态的比较结果的位姿偏差来控制机械臂的位置和姿态。通过位姿偏差设计机械臂的运动控制策略，将机械臂末端的笛卡尔速度通过雅克比矩阵转化为ur5机械臂6个关节的各个关节的角速度。从而驱动机械臂运动到指定位置。

下面介绍基于nurbs曲面的点云拟合算法。

本发明采用nurbs曲面拟合目标点云用以描述面状柔性体的变形。在u方向是p阶，在v方向是q阶的nurbs曲面的有理函数形式可以通过式(1)表示。

式中，

pi，j——u和v这两个方向上控制点的集合；

ωi，j——每个点pi，j的权重；

ni，p(u)——u方向上的非有理b样条的基方程；

nj，a(v)——u方向上的非有理b样条的基方程。

为了简洁地表示式(1)，可以定义式(2)。

从而可以将式(1)可以重新写为式(3)。

首先对获得的点云进行初始化获得初始的nurbs曲面。算法流程如下：

在获得初始化的nurbs曲面之后，本发明利用曲面拟合生成近似曲面。首先介绍nurbs曲线的拟合。

曲线拟合的算法可以描述为假设有n+1个数据点d0；d1；...；dn，并且希望找到一个b样条曲线，该曲线遵循数据多边形的形状，而不必实际包含数据点。要做到这一点，我们需要两个更多的输入：控制点的数量(比如h+1个控制点)和曲线的阶数p，同时还必须满足n＞h＞＝p＞＝1。有了h和p就可以确定曲线的参数t0；t1；...；tn和一个节点向量。这里要保持参数的数量是和数据点的数量是一致的。式(4)给出了p阶b样条曲线的形式。

式中pi——点云中的h+1个尚未知道的控制点。

我们使用平方误差作为指标项，构建了式(5)。优化的目标就是找到一组控制点p1；p2；...ph-1来让式(5)的值最小化。

通过对式(5)进行每一项求偏导可以得到h个控制点的位置。通过更新控制点，从而改变曲线的形状。算法如下。

nurbs曲面是由两条nurbs曲线的张量积构成的，我们分别针对两条nurbs曲线采用曲线拟合方法可以得到nurbs曲面的拟合结果。算法如下。

最终完成无标记面状柔性体的曲面拟合，通过nurbs的德布尔算法可以得到变形前后面状柔性体上某一点的变化量。

在描述无标记面状柔性体的移动时，因目标物特征较少且易发生形变。所以本发明在使用机械臂操作目标物时，利用机械臂接触无标记面状柔性体的瞬间作为初始状态，通过构建机械臂和目标物之间的关系保证目标物的移动通过机械臂移动描述。如图4所示。

图4中符号所表示的含义如下。∑kinect相机坐标系，∑base机械臂基座坐标系，∑t_ini初始位置tcp坐标系，∑p_ini初始位置nurbs曲面坐标系，∑t_cur当前位置tcp坐标系，∑p_curr当前位置nurbs曲面坐标系。∑p_flat在机械臂末端在∑t_cur时，初始位置的∑p_ini在该状态下的表达；仅包含初始nurbs平面的位置移动信息，是一个虚拟的坐标系。a1是初始位置当纸片平放在桌面上时，∑t_ini相对于∑base的齐次变换矩阵。满足∑t_inia1＝∑base，可以在初始位置通过机械臂正运动学得到。a2是当前位置，∑t_cur相对于∑base的齐次变换矩阵。满足∑t_cura2＝∑base。a1和a2可以通过机械臂正运动学得到。

b1是初始位置当纸片平放在桌面上时，∑p_ini相对于∑kinect的齐次变换矩阵。满足∑p_inib1＝∑kinect，经过曲面拟合之后，可以直接在相机坐标系下通过曲面的得布尔算法得到。b2是当前位置，∑p_cur相对于∑kinect的齐次变换矩阵。既包含变形信息又包含位置信息。

c1是在初始状态，∑p_ini相对于∑t_ini的齐次坐标变换矩阵。c2当前状态，∑p_cur相对于∑t_cur的齐次坐标变换矩阵。d是当前工具坐标系到初始状态工具坐标系的齐次变换矩阵，满足∑t_curd＝∑t_ini。e是当前nurbs坐标系下的初始化平面到初始状态工具坐标系的齐次变换矩阵，满足∑p_flate＝∑p_ini。f是眼在手外的手眼标定齐次变换矩阵，满足∑kinectf＝∑base。

最终通过坐标变换，可以通过式(6)完成运动分离。

结合上文中的nurbs曲面拟合，可以完成无标记面状柔性体的变形跟踪。

综上，本发明中基于点云信息和nurbs数学模型对无标记面状柔性体的变形进行跟踪，使得不需要利用面状柔性体上标记特征即可完成对面状柔性体的变形跟踪，解决了无标记面状柔性体运动描述的问题，并有效地从环境中分离出目标无标记面状柔性体。

以上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定本发明的具体实施只局限于这些说明。对于本发明所属技术领域的普通技术人员来说，在不脱离本发明构思的前提下，还可以做出若干简单推演或替换，都应当视为属于本发明的保护范围。