图像信息输入下的软线重构与抓取验证

邱介禄; 王明明

doi:10.11992/tis.202403021

图像信息输入下的软线重构与抓取验证

doi: 10.11992/tis.202403021

邱介禄^{1, 2,},
王明明^{1, 2, ,}

1.
西北工业大学航天学院, 陕西西安 710072;
2.
西北工业大学深圳研究院, 广东深圳 518057

基金项目: 国家自然科学基金项目(61973256)；广东省基础与应用基础研究基金项目(2024A1515011178).

详细信息

作者简介:
邱介禄，硕士研究生，主要研究方向为空间机器人在轨装配。E-mail：1145247832@qq.com;

王明明，副教授，博士生导师，主要研究方向为空间机器人在轨操控。主持国家自然科学基金项目等20余项。发表学术论文80余篇。E-mail：mwang@nwpu.edu.cn.

通讯作者:
王明明. E-mail：mwang@nwpu.edu.cn.

中图分类号: TP24
出版历程
- 收稿日期: 2024-03-22
- 网络出版日期: 2025-01-16

Flexible wire reconstructing and gripping verification under the image information input

QIU Jielu^{1, 2,},
WANG Mingming^{1, 2, ,}

1.
School of Aeronautics, Northwestern Polytechnical University, Xi’an 710072, China;
2.
Research & Shenzhen Research Institute of Northwestern Polytechnical University, Shenzhen 518057, China

摘要

摘要: 针对使用机器人对柔性物体进行操作的复杂任务，设计了一种简洁高效的软线抓取方法。首先利用双目相机和图像处理方法对软线进行测量，结合坐标系间的转换关系，从图像中还原了其特征点的空间坐标信息；采用Frenet标架对软线进行了建模重构，以获取软线上任意点的位置与弯曲方向信息；提出了六自由度机械臂的位姿表示及转换方法以及相机的标定流程，将曲线任意弧长处的坐标值和Frenet标架方向转换为夹爪末端的位姿参数，最后开展了目标点处沿软线弯曲方向抓取软线的地面实验，验证了方法的有效性。
- 软线 /
- 视觉感知 /
- 深度相机 /
- 图像处理 /
- 坐标计算 /
- 曲线重构 /
- 位姿转换 /
- 机械臂抓取
Abstract: To address the complex task of operating flexible objects with a robot, a concise and efficient method for grasping soft wires is designed. First, the soft wire is measured using a stereo camera and image processing techniques. By combining the transformation relationships between coordinate systems, three-dimensional coordinate information is reconstructed from the image data. The soft wire is then modeled and reconstructed using the Frenet frame, enabling the acquisition of positional and bending direction information at any arbitrary point along the wire. The representations and transformation methods for the six-DOF robotic arm pose, along with the camera calibration process, are provided. These enable the conversion of the Frenet frame’s coordinates and directional data for any point along the curve into the robotic gripper’s pose. Finally, a ground experiment was conducted to demonstrate the robot’s ability to grasp the soft wire along the bending direction of a target point, verifying the feasibility of this method.
- flexible wire /
- visual sensing /
- depth camera /
- image processing /
- coordinates calculation /
- curve reconstruction /
- pose transformation /
- robot arm grasping

HTML全文

工业机器人是我国重点研究领域之一，已经广泛应用于机械、汽车、医疗、服务等领域^[1]。然而，传统工业机器人的智能化水平较低，往往只能采用“示教”的方式来完成任务，即提前预设目标点位置，以固定的轨迹移动到目标点，如果目标的位置或者姿态发生变化，便需要重新示教，大大降低了工作效率，无法适应复杂多变的任务与环境^[2]。但随着机器人视觉技术的发展与应用，工业机器人可以自主识别目标的种类和位姿，提高了工作的灵活性和鲁棒性，大大提升了机器人的智能化水平^[3]。机器视觉是一门融合了视觉系统构建与标定、数字信号处理、图像处理算法、计算机应用技术等多方面知识的复杂技术，通过视觉传感器获取目标的特征信息^[4]。目前，视觉技术已被广泛地应用到工业生产、航天航空、城市交通、医学医疗、农业采摘等相关领域^[5-9]。目标识别与自主抓取是视觉工业机器人的关键技术，是机器人研究领域的一个关键分支，目标识别检测也是实现工业机器人抓取任务的前提，即通过对图像采集和处理，特征提取和分析，获得抓取物体的类别、空间位置、表面特征等信息，再由机器人夹爪完成对物体的抓取。研究工业机器人的视觉识别与抓取在现代智能化工业中有着广阔的应用前景^[10-12]。

1. 问题引入

目前，机器人的抓取任务大多面向的是刚性零件，对于柔性零件的研究相对较小，而可变形线性物体(deformable linear objects, DLO)，如电缆、电线、软管、导管等零件，在工业制造、航空航天领域有着广泛的应用，在面对DLO的精密装配要求时，由于DLO的高自由度和高灵活性，使得其与刚性物体的操作有很大区别^[13-14]。因此，研究DLO的自主装配是必要的。对于DLO的感知非常依赖于机器人视觉。Inoue等^[15]最早基于视觉研究DLO操作，运用手眼系统和视觉处理算法，借助立体视觉的三维测量，保证机械臂准确运行到抓取点。对于高自由度的细长线缆的抓取任务，如何对目标进行精确定位定姿成为了关键问题。随着传感器技术的发展，深度相机技术问世，深度相机可以通过深度计算获取目标的点云信息，包括点云中每个点的颜色和位置信息。Lui等^[16]利用RGB-D相机获取点云信息，通过基于粒子过滤器的算法，得到了柔性物体的信息。Kumra等^[17]提出了一个利用RGB-D图像预测最佳抓取姿态的机器人检测系统。Schulman等^[18]开发了三维点云数据接口，可以通过深度相机实时测量的点云信息对柔性物体进行追踪拟合。

点云信息的拟合过程可以视作将离散点拟合成曲线的过程。Yamakawa等^[19]正是通过把DLO抽象为没有粗细的线段，并计算机器人的运动从而得到DLO的动态。关于曲线拟合的研究是从二维曲线开始的，由最初的B样条以及其他样条插值这类简单的方法，到Levin^[20]提出的移动最小二乘法，再到Amenta等^[21]基于 $\mathrm{\beta }$ 骨架的多边形曲线逼近法等等。在二维曲线拟合研究的基础上，Yan等^[22]使用K-means聚类方法进行骨架提取，并在此基础上使用B样条法对三维曲线完成了拟合和重构。对于曲线的拓扑问题，朱东方^[23]研究了一种基于PCA(principle component analysis)的点云分类与曲线拟合方法，该算法能够同时进行点云分类和曲线拟合，适用于处理复杂拓扑结构的情形。林思宇^[24]提出了基于视觉来获取线缆几何形状和拓扑信息的方法，和基于CPD(coherent point drift)算法的缠绕线缆跟踪法，来解决装配过程中线缆受遮挡和拓扑形态变化的问题。

综上所述，机器人自主抓取柔性线缆是包含多项技术挑战的复杂工作。针对这一任务，本文提出了一种基于深度视觉感知使用六自由度机械臂自主抓取软线的方法。该方法直接利用二维图像和深度信息还原软线特征点的三维位置信息，避免了处理复杂的点云数据；采用Frenet标架的形式重构曲线，方便地描述曲线的形态和形变，不仅可以通过弧长计算抓取点的位置，还可以提供抓取点处软线“走势”，即切法向信息，以此调整夹爪的姿态，使抓取更加柔和。

本文的研究框架如图1所示，本文具体内容安排如下：

图 1 研究框架

Fig. 1 Research framework

下载: 全尺寸图片

第2节主要介绍基于图像提取特征点三维坐标的方法，首先通过图像处理方法确定软线特征点的二维像素坐标，再根据坐标系转换关系求解与之对应的三维相机坐标；第3节主要介绍基于Frenet标架的曲线拟合重构方法，根据离散的三维坐标点以弧长为自变量拟合软线任意点的坐标值、曲率和挠率，积分求解Frenet路径表征软线，再计算任意弧长处的单位切向量、主法向量和副法向量；第4节介绍了机械臂位姿的表示及转换方法，依据相机手眼标定方法求解相机坐标系与机械臂基坐标系的变换矩阵，从而将相机坐标系下的坐标值和切法向量值转换为机械臂夹爪的位姿参数；第5节为软线的抓取实验验证以及结果分析。

2. 基于图像提取特征点

为了获取可供曲线重构的特征点坐标，需要从相机拍摄的二维图像的像素信息中，还原出各个特征像素点对应的三维真实坐标，这需要进行一系列的图像处理和坐标变换操作。

2.1 特征像素点提取

本文使用ZED相机获取软线的RGB图像信息，主要使用OpenCV进行图像处理。ZED相机是一款双目立体相机，可以通过左右目图像自行解算各像素点对应的深度信息Z，这是后续进行坐标变换的重要信息。为了提取软线的坐标信息，需要对RGB图像进行预处理，将软线从背景中分离出来，只保留可以描述软线形状特征的像素点。

本文通过HSV滤波的原理从背景中提取软线。不同与RGB颜色三通道的高度耦合性，HSV颜色分别代表色相(H)、饱和度(S)和亮度(V)，可以更加准确地进行颜色滤波。通过公式便可以将图像RBG信息转换为HSV值：

$H=\left\{\begin{array}{l}{0}^{\circ },{M}_{1}={M}_{2}\\ {60}^{\circ }\times \dfrac{G-B}{{M}_{1}-{M}_{2}}+{0}^{\circ },\;R={M}_{1}\;\text{a}\text{n}\text{d}\;G > B\\ {60}^{\circ }\times \dfrac{G-B}{{M}_{1}-{M}_{2}}+{360}^{\circ },\;R={M}_{1}\;\text{a}\text{n}\text{d}\;G < B\\ {60}^{\circ }\times \dfrac{B-R}{{M}_{1}-{M}_{2}}+{120}^{\circ },R={M}_{1}\\ {60}^{\circ }\times \dfrac{B-R}{{M}_{1}-{M}_{2}}+24{0}^{\circ },B={M}_{1}\\ \end{array}\right.$

$S=\left\{\begin{array}{l}0,{M}_{1}=0\\ \dfrac{{M}_{1}-{M}_{2}}{{M}_{1}},其他\end{array}\right.$

$V={M}_{1}$

式中： ${M}_{1}=\mathrm{m}\mathrm{a}\mathrm{x}(R,G,B)$ ， ${M}_{2}=\mathrm{m}\mathrm{i}\mathrm{n}(R,G,B)$ 。

通过调整HSV的滤波阈值，即可将软线从背景中提取出来，图2 (a)为原始图像，图2 (b)为滤波后的图像。

图 2 图像处理

Fig. 2 Image processing

下载: 全尺寸图片

由于软线具有一定的宽度，图2(b)所示图像中包含软线的像素点数量依然比较庞大，不利于曲线的拟合重构，因此需要提取软线的中心骨架，即将软线形状缩小为1个像素的宽度。骨架提取可以通过形态学的细化和剪枝方法来完成。同时，考虑到滤波后的软线形状存在不规整的边缘，为了保证图形的细化效果，可以先通过形态学的膨胀和腐蚀方法对二值化图像进行预处理，再进行骨架提取。图2(c)为提取中心骨架后的图像。

经过图像处理后，只保留RGB值为(255, 255, 255)的像素点作为特征像素点，进行坐标变换。

2.2 求解特征点三维坐标

为了将特征像素点的真实三维坐标还原出来，首先引入4个与相机相关的坐标系，分别是像素坐标系、图像坐标系、相机坐标系和世界坐标系。可以在数学上用它们来描述坐标之间的相互变换关系。

1)像素坐标系：像素坐标系是一个二维坐标系，原点位于像平面的左上角，其中的坐标记为 $(u,v)$ ，最小增量为1个像素(pixel)。

2)图像坐标系：图像坐标系是一个对应真实世界尺寸的二维坐标系，原点为光轴与像平面的交点，其中的坐标记为 $(x,y)$ 。

3)相机坐标系：相机坐标系是一个三维坐标系，坐标系原点是相机等效镜头的光心， ${\textit{z}}$ 轴通常垂直于成像平面向外，其中的坐标记为 $(X,Y,Z)$ 。

4)世界坐标系：世界坐标系是一个三维坐标系，坐标系原点一般为环境中的某一点，其中的坐标记为 $({X}_{\mathrm{w}},{Y}_{\mathrm{w}},{Z}_{\mathrm{w}})$ 。

在相机坐标系中，像平面为Z=f (f为焦距，单位mm)。设空间中一点P在在像平面上的投影为点p。如图3(a)所示，根据三角形相似原理，可得p点图像坐标 $(x,y)$ 与P点相机坐标 $(X,Y)$ 的关系为

图 3 坐标系间的几何关系

Fig. 3 Geometrical relation between the coordinate systems　　　　

下载: 全尺寸图片

$x=\dfrac{f}{Z}X$

(1)

$y=\dfrac{f}{Z}Y$

(2)

设图像坐标系原点在像素坐标系中的坐标为 $({c}_{x}$ , ${c}_{y})$ ，如图3(b)所示，设每个像素在水平和竖直方向上的尺寸分别为 $k$ 、 $l$ (单位mm/pixel)，再令 ${f}_{x}=f/k$ ， ${f}_{y}=f/l$ ，结合式(1)和(2)，可得相机坐标与像素坐标的关系：

$X=(u-{c}_{x})\dfrac{Z}{{f}_{x}}$

(3)

$Y={(v-c}_{y})\dfrac{Z}{{f}_{y}}$

(4)

将式(3)和(4)表示为矩阵形式：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=\dfrac{1}{Z}\left[\begin{array}{ccc}{f}_{x}& 0& {c}_{x}\\ 0& {f}_{y}& {c}_{y}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{c}X\\ Y\\ Z\end{array}\right]$

式中三维方阵即为内参(intrinsics)矩阵。该矩阵描述了相机坐标系到像素坐标系的变换关系。ZED相机可以直接读取内参矩阵中的4个参数，也可以通过4.3.1节的相机标定方法求解内参矩阵。根据2.1节得到特征像素点的像素坐标 $(u,v)$ ，在获取深度信息Z和内参矩阵值后即可求解特征点在相机坐标系下的三维真实坐标 $(X,Y,Z)$ 。经过简单的过滤和排序处理，即可得到最终的特征坐标点集。

3. 曲线拟合重构

在得到曲线三维特征点坐标后，需要对散点坐标进行拟合，重构出软线的空间形态，以便获取软线上任意点的三维坐标值和该点处曲线的变化走势，用于求解机械臂的抓取位姿。本文介绍一种基于Frenet标架的曲线重构方法^[25]，通过该方法可以获取任意弧长处曲线的Frenet标架值，通过快速的变换即可得到机械臂的抓取姿态。

3.1 曲线形状求解

本文通过在 ${\mathbb{R}}^{3}$ 中定义的曲线集 $\{\boldsymbol{r}:[0,T]\to {\mathbb{R}}^{3}\}$ 来表示软线，为了简化阐述，假设曲线是正则的，即曲线关于时间的导数 $\dot{\boldsymbol{r}}\left(t\right)$ 在 $[0,T]$ 上处处存在。并记 $\mid \mid \cdot \mid \mid$ 为欧几里得范数， $\mid \mid \cdot \ { \mid \mid }_{2}$ 为 ${L}_{2}$ 范数。

对于曲线 $\boldsymbol{r}\left(t\right)$ ，其弧长定义为

$s\left(t\right)=\int_0^t\parallel\dot{\boldsymbol{r}}\left(u\right)\parallel_2\mathrm{d}u,\ t\in[0,T]$

(5)

则 $s\left(T\right) =L$ 是曲线 $\boldsymbol{R}=\left\{\boldsymbol{r}\right(t),t\in [0,T\left]\right\}$ 的总长度。曲线 $\boldsymbol{R}:[0,L]\to {\mathbb{R}}^{3}$ 的形状是函数 $\boldsymbol{r}\left(t\right)$ 的像，它满足 $\boldsymbol{r}\left(t\right)=\boldsymbol{R}\left(s\right(t\left)\right)$ 。关于弧长 $s$ 的导数用 $Y\mathrm{{'}}\left(s\right)=\mathrm{d}\left(Y\right(s\left)\right)/ \mathrm{d}s$ 的形式表示，而关于时间 $t$ 的导数用点表示，即 $\dot{\boldsymbol{r}}\left(t\right)$ 。在曲线的不同参数化下，形状函数保持不变，为了使用Frenet标架，将曲线进行弧长参数化。

曲率和挠率是曲线的几何不变量，与曲线 $\boldsymbol{r}\left(t\right)$ 的参数化方法无关。此外，它们完全描述了曲线的局部行为，从某种意义上说，具有相同曲率和挠率的2条曲线可以通过平移或旋转来达到重合。也就是说，它们在刚性(欧几里得)运动的作用下是不变的。曲率和挠率计算公式为

$\boldsymbol{\kappa }\left(s\left(t\right)\right)=\parallel \dot{\boldsymbol{r}}\left(t\right)\times \ddot{\boldsymbol{r}}\left(t\right)\parallel /\parallel \dot{\boldsymbol{r}}\left(t\right){\parallel }^{3}$

(6)

$\boldsymbol{\tau }\left(s\right(t\left)\right)=\left(\dot{\boldsymbol{r}}\left(t\right),\ddot{\boldsymbol{r}}\left(t\right),\dddot{\boldsymbol{r}}\left(t\right)\right)/\parallel \dot{\boldsymbol{r}}\left(t\right)\times \ddot{\boldsymbol{r}}\left(t\right){\parallel }^{2}$

(7)

由于曲线上的点位于 ${\mathbb{R}}^{3}$ 中，可以为每个点定义一个三维基。对曲线 $\boldsymbol{r}\left(t\right)=\boldsymbol{R}\left(s\right(t\left)\right)$ 进行弧长参数化后可得 $\dot{\boldsymbol{r}}\left(t\right)=\dot{s}\left(t\right)\boldsymbol{R}\mathbf{{'}}\left(s\right(t\left)\right)$ ，这意味着切向量 $\boldsymbol{T}\left(s\right)=\boldsymbol{R}\mathrm{{'}}\left(s\right)$ 对于 $[0,L]$ 中所有的 $s$ 均为单位长度。在 $\parallel \boldsymbol{T}\left(s\right)\parallel \ne 0$ 的点上，由于 $\parallel \boldsymbol{T}\left(s\right){\parallel }^{2}=1$ ，所以导数 $\boldsymbol{T}\mathrm{{'}}\left(s\right)$ 与 $\boldsymbol{T}\left(s\right)$ 正交，因此存在一个单位向量 $\boldsymbol{N}\left(s\right)\propto \boldsymbol{T}\mathrm{{'}}\left(s\right)$ 。曲率可以定义为 $s\mapsto \boldsymbol{\kappa }\left(s\right)=\parallel \boldsymbol{T}\mathrm{{'}}\left(s\right)\parallel$ ，它可以反映出曲线远离切线的速度。将法向量 $\boldsymbol{N}\left(s\right)=\boldsymbol{T}\mathrm{{'}}\left(s\right)/\boldsymbol{\kappa }\left(s\right)$ 与副法向量 $\boldsymbol{B}\left(s\right) =\boldsymbol{T}\left(s\right) \times \boldsymbol{N}\left(s\right)$ 与切向量 $\boldsymbol{T}\left(s\right)$ 结合，便可以定义一个 ${\mathbb{R}}^{3}$ 中的局部正交基系统。将局部基系统看作 $s$ 的函数，便定义了一个运动标架，即为Frenet标架。对于 $[0,L]$ 中的所有 $s$ ，挠率 $s\mapsto \boldsymbol{\tau }\left(s\right)$ 是满足 $\boldsymbol{B}\mathrm{{'}}\left(s\right)=-\boldsymbol{\tau }\left(s\right)\boldsymbol{N}\left(s\right)$ 的函数，它反映出曲线从切向量和法向量确定的平面上脱离的速度。由此可得Frenet-Serret微分方程组：

$\left\{\begin{array}{l} {\boldsymbol{T}}{'}\left(s\right)={\boldsymbol{\kappa}} \left(s\right){\boldsymbol{N}}\left(s\right)\\ {\boldsymbol{N}}{'}\left(s\right)=-{\boldsymbol{\kappa}} \left(s\right){\boldsymbol{T}}\left(s\right)+{\boldsymbol{\tau}} \left(s\right){\boldsymbol{B}}\left(s\right)\\ {\boldsymbol{B}}{'}\left(s\right)= -{\boldsymbol{\tau}} \left(s\right){\boldsymbol{N}}\left(s\right)\end{array}\right.$

其中初始状态为 $\left(\boldsymbol{T}\right(0),\boldsymbol{N}(0),\boldsymbol{B}(0\left)\right)$ 。这个移动的标架定义了一个曲线 $s\mapsto \boldsymbol{Q}\left(s\right)=\left[\boldsymbol{T}\right(s)\mid \boldsymbol{N}(s)\mid \boldsymbol{B}(s\left)\right]$ ，该曲线属于特殊正交矩阵群 $\mathbf{\boldsymbol{S}\boldsymbol{O}}\left(3\right)$ ，其中 $\mathbf{\boldsymbol{S}\boldsymbol{O}}\left(m\right)=\{\boldsymbol{Y}\mathrm{是}m\times m\mathrm{的}\mathrm{矩}\mathrm{阵}\mid {\boldsymbol{Y}}^{\mathrm{T }}\boldsymbol{Y}={\boldsymbol{I}}_{m},\mathrm{d}\mathrm{e}\mathrm{t}(\boldsymbol{Y})=1\}$ ，因为 $\mathbf{\boldsymbol{S}\boldsymbol{O}}\left(3\right)$ 是一个具有流形结构的李群，所以Frenet-Serret微分方程组可以视为定义在李群的微分方程组：

$\boldsymbol{Q}\mathrm{{'}}\left(s\right)=\boldsymbol{Q}\left(s\right)\boldsymbol{A}\left(s\right)$

其中

$\boldsymbol{A}\left(s\right)=\left[\begin{array}{ccc}0& -\boldsymbol{\kappa }\left(s\right)& 0\\ \boldsymbol{\kappa }\left(s\right)& 0& -\boldsymbol{\tau }\left(s\right)\\ 0& \boldsymbol{\tau }\left(s\right)& 0\end{array}\right]$

对于不同初始状态 $\left(\boldsymbol{T}\right(0),\boldsymbol{N}(0),\boldsymbol{B}(0\left)\right)$ ，可以积分求解出不同的曲线，称为Frenet路径，但正如上文所述，这些曲线可以通过平移或旋转来达到重合，而这些曲线的姿态仅与积分的初始状态 $\left(\boldsymbol{T}\right(0),\boldsymbol{N}(0),\boldsymbol{B}(0\left)\right)$ 有关，故只需确定曲线任意弧长处的曲率和挠率，便可以求解目标曲线的形状，再通过调整初始状态和轨迹起点完成曲线的对齐。

3.2 曲线对齐

3.1节提到，只需改变积分初始状态，即初始正交基 $\left(\boldsymbol{T}\right(0),\boldsymbol{N}(0),\boldsymbol{B}(0\left)\right)$ 的方向，即可改变曲线的姿态，假设最初的初始正交基为单位阵 ${\boldsymbol{I}}_{3}$ ，对其进行绕旋转轴 $\boldsymbol{n}$ =( ${n}_{x},{n}_{y},{n}_{{\textit{z}}})$ 旋转一个角度 $\theta$ 的变换，由绕空间任意轴旋转任意角度的旋转矩阵：

$\boldsymbol{R}\left(\boldsymbol{n},\theta \right)=$

$\left[\begin{array}{ccc}{Cn}_{x}^{2}+{\cos}\theta & C{n}_{x}{n}_{y}-{n}_{{\textit{z}}}{\sin}\theta & C{n}_{x}{{n}_{{\textit{z}}}+n}_{y}{\sin}\theta \\ {Cn}_{x}{n}_{y}{+n}_{{\textit{z}}}{\sin}\theta & {Cn}_{y}^{2}+{\cos}\theta & {Cn}_{y}{n}_{{\textit{z}}}{-n}_{x}{\sin}\theta \\ {Cn}_{x}{{n}_{{\textit{z}}}-n}_{y}{\sin}\theta & C{n}_{y}{{n}_{{\textit{z}}}+n}_{x}{\sin}\theta & {Cn}_{{\textit{z}}}^{2}+{\cos}\theta \end{array}\right]$

式中 $C=\left(1-\mathrm{c}\mathrm{o}\mathrm{s}\ \theta\right)$ ，可以得到，变换后的初始正交基为 $\boldsymbol{R}(\boldsymbol{n},\theta )$ ，将旋转轴 $\boldsymbol{n}$ =( ${n}_{x},{n}_{y},{n}_{{\textit{z}}})$ 和角度 $\theta$ 作为优化变量，以积分曲线 $\boldsymbol{Q}\left(s\right)$ 和目标曲线(即软线)上各个特征点间的距离的平方和为目标函数，求解优化问题，得到使目标函数达到最小值的 $\boldsymbol{n}$ 和 $\theta$ ，便可以确定初始正交基的方向 $\boldsymbol{R}(\boldsymbol{n},\theta )$ 。需要注意的是，在计算目标函数值前，必须先将轨迹的起点 $\boldsymbol{R}\left(0\right)=\boldsymbol{r}\left(0\right)$ 与目标曲线的起点 $(x\left(0\right),y\left(0\right),{\textit{z}}(0\left)\right)$ 平移对齐。

3.3 曲线重构

设置一个时间序列 $t=(1,2,\cdot\cdot\cdot,p)$ ( $p$ 为坐标点数量)，分别对坐标点的 $(x,y,{\textit{z}})$ 关于 $t$ 进行拟合，得到曲线 $\boldsymbol{r}\left(t\right)=(x\left(t\right),y\left(t\right),{\textit{z}}\left(t\right))$ 。根据式(5)，求解 $s\left(t\right)$ ，由式(6)和式(7)求解各个坐标点处的 $\boldsymbol{\kappa }\left(t\right)$ 和 $\boldsymbol{\tau }\left(t\right)$ 。由于 $s\left(t\right)$ 是单调函数，故可求其反函数 $t\left(s\right)$ ，代入 $\boldsymbol{\kappa }\left(t\right)$ 和 $\boldsymbol{\tau }\left(t\right)$ 即可得到曲率和挠率关于弧长 $s$ 的解析解 $\boldsymbol{\kappa }\left(s\right)$ 和 $\boldsymbol{\tau }\left(s\right)$ 。

在直接利用析解 $\boldsymbol{\kappa }\left(s\right)$ 和 $\boldsymbol{\tau }\left(s\right)$ 时，求解Frenet路径会耗费大量时间，此时对时间序列进行等距采样，根据 $s\left(t\right)$ ， $\boldsymbol{\kappa }\left(t\right)$ 和 $\boldsymbol{\tau }\left(t\right)$ 即可得到每个采样时间点对应的 $s$ ， $\boldsymbol{\kappa }$ 和 $\boldsymbol{\tau }$ ，再对 $\boldsymbol{\kappa }$ 和 $\boldsymbol{\tau }$ 关于 $s$ 进行拟合，便可得到曲率和挠率关于弧长 $s$ 的数值解 $\boldsymbol{\kappa }\left(s\right)$ 和 $\boldsymbol{\tau }\left(s\right)$ ，利用数值解求解Frenet路径可以大大减少求解时间。最终求解的Frenet路径如图4所示。

图 4 软线的Frenet建模

Fig. 4 Frenet modeling of the wire

下载: 全尺寸图片

同样地，对时间序列进行等距采样，根据 $s\left(t\right)$ ， $\boldsymbol{r}\left(t\right)=(x\left(t\right),y\left(t\right),{\textit{z}}\left(t\right))$ 即可得到每个采样时间点对应的弧长 $s$ 和坐标 $(x,y,{\textit{z}})$ ，再对 $(x,y,{\textit{z}})$ 关于弧长 $s$ 进行拟合得到 $\boldsymbol{r}\left(s\right)=(x\left(s\right),y\left(s\right),{\textit{z}}\left(s\right))$ ，便可以获得任意弧长处的坐标值。根据微分几何公式可知任意弧长处的单位切向量为

$\boldsymbol{T}\left(s\right)=\mathrm{d}\boldsymbol{r}\left(s\right)/\mathrm{d}s = (x{'}\left(s\right),y{'}\left(s\right),{\textit{z}}{'}\left(s\right))$

单位主法向量为

$\boldsymbol{N}\left(s\right)=\mathrm{d}\boldsymbol{T}/\left(\boldsymbol{\kappa }\right(s\left)\mathrm{d}s\right) = (x{'}{'}\left(s\right),y{'}{'}\left(s\right),{\textit{z}}{'}{'}\left(s\right))/\boldsymbol{\kappa }\left(s\right)$

单位副法向量为

$\boldsymbol{B}\left(s\right)=\boldsymbol{T}\left(s\right)\times \boldsymbol{N}\left(s\right)$

通过上述方法可以得到相机坐标系下曲线任意弧长处的三维坐标值和Frenet标架值。

4. 求解机械臂位姿参数

4.1 机器人坐标系及位姿表示

本文使用的机器人是六自由度UR(universal robots)机械臂装置，其定义了2个坐标系，分别是机座坐标系和工具坐标系。机座坐标系的原点位于机械臂的底座中心，工具坐标系的原点位于工具圆盘中心。TCP(tool central point)是机器人常用概念，即工具中心点，TCP的默认位置为工具坐标系原点。在使用夹具时，需要将原工具坐标系沿其 ${\textit{z}}$ 轴正方向平移至夹具的夹取中心，且三轴方向不变。本文将新的工具坐标系称为TCP坐标系，默认机座坐标系为基准(base)坐标系。

UR机械臂TCP的位姿由6个参数 $(x,y,{\textit{z}}, r_x,r_y,r_{\textit{z}})$ 表示，分别为TCP相对基准坐标系的位置和姿态，其中 $(r_x,r_y,r_{\textit{z}})$ 为旋转矢量，可以通过转换得到TCP坐标系到基准坐标系的旋转矩阵 $\boldsymbol{R}={\boldsymbol{R}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{B}}$ ，由表示位置的3个参数 $(x,y,{\textit{z}})$ 可得TCP坐标系到基准坐标系的平移矩阵 $\boldsymbol{t}={(x,y,{\textit{z}})}^{\mathrm{T}}$ ，进而得到TCP坐标系到基准坐标系的齐次变换矩阵 ${\boldsymbol{T}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{B}}$ 。已知变换矩阵，需要将旋转矩阵转换为旋转向量，才能作为输入参数对机械臂进行操控。旋转矩阵和旋转向量的相互转换关系可如下推导。

由罗德里格斯旋转公式知

$\boldsymbol{R}\left(\boldsymbol{n},\theta\right)=\boldsymbol{I}\mathrm{c}\mathrm{o}\mathrm{s}\ \theta+\left(1-\mathrm{c}\mathrm{o}\mathrm{s}\ \theta\right)\boldsymbol{n}\boldsymbol{n}^{\mathrm{T}}+$

$\mathrm{s}\mathrm{i}\mathrm{n}\ \theta \left[\begin{array}{ccc}0& -{n}_{{\textit{z}}}& {n}_{y}\\ {n}_{{\textit{z}}}& 0& -{n}_{x}\\ {-n}_{y}& {n}_{x}& 0\end{array}\right]$

(8)

式中： $\boldsymbol{n}$ 为旋转轴，是一个单位向量； $\theta$ 为旋转角度，单位是弧度； $\boldsymbol{I}$ 为3阶的单位矩阵。

已知一个旋转向量 $\boldsymbol{r}=({r}_{x},{r}_{y},{r}_{{\textit{z}}})$ ，则有旋转角度 $\theta =\mid \mid \boldsymbol{r} \ { \mid \mid }_{2}$ ，旋转轴 $\boldsymbol{n}=\boldsymbol{r}/\theta$ ，将角度和旋转轴代入式(8)即可得到旋转矩阵 $\boldsymbol{R}$ 。

根据式(8)可以推导得

$\boldsymbol{J}=\mathrm{s}\mathrm{i}\mathrm{n}\ \theta \left[\begin{array}{ccc}0& -{n}_{{\textit{z}}}& {n}_{y}\\ {n}_{{\textit{z}}}& 0& -{n}_{x}\\ {-n}_{y}& {n}_{x}& 0\end{array}\right]=\dfrac{\boldsymbol{R}-{\boldsymbol{R}}^{\mathrm{T}}}{2}$

设 $\boldsymbol{u}=\mathrm{s}\mathrm{i}\mathrm{n}\left(\theta \right)\boldsymbol{n}=\mathrm{s}\mathrm{i}\mathrm{n}\left(\theta \right){({n}_{x},{n}_{y},{n}_{{\textit{z}}})}^{\mathrm{T}}$ ，则有

$\boldsymbol{m}=\mathrm{s}\mathrm{i}\mathrm{n}\left(\theta \right)\boldsymbol{n}=(-{\boldsymbol{J}}_{23},{\boldsymbol{J}}_{13},-{\boldsymbol{J}}_{12}{)}^{\mathrm{T}}$

由于 $\boldsymbol{n}$ 为单位向量，可得 $\mathrm{s}\mathrm{i}\mathrm{n}\left(\theta \right)=\mid \mid \boldsymbol{u}\ { \mid \mid }_{2}$ ，再得到旋转角度 $\theta =\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{s}\mathrm{i}\mathrm{n}(\mid \mid \boldsymbol{u} \ { \mid \mid }_{2})$ ，旋转轴 $\boldsymbol{n}=\boldsymbol{u}/\mid \mid \boldsymbol{u}\ {\mid \mid }_{2}$ ，则旋转向量为

$\boldsymbol{r}=({r}_{x},{r}_{y},{r}_{{\textit{z}}})=\boldsymbol{n}\theta$

4.2 TCP位姿参数求解方法

通过曲线重构可以获取曲线上任意弧长处的单位切向量 $\boldsymbol{T}=({t}_{1},{t}_{2},{t}_{3})$ ，单位法向量 $\boldsymbol{N}=({n}_{1}, {n}_{2},{n}_{3})$ ，单位副法向量 $\boldsymbol{B}=({b}_{1},{b}_{2},{b}_{3})$ ，由于这3个向量为单位正交向量，故可以构成一个正交坐标系，称为TNB坐标系，即为Frenet标架。根据旋转矩阵的定义，可得TNB坐标系到相机坐标系的旋转矩阵为

${\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{C}}=\left[\begin{array}{ccc}{t}_{1}& {n}_{1}& {b}_{1}\\ {t}_{2}& {n}_{2}& {b}_{2}\\ {t}_{3}& {n}_{3}& {b}_{3}\end{array}\right]$

(9)

在进行实物抓取时，需要使机械臂TCP运动到目标抓取点处，并使TCP坐标系与目标点处的TNB坐标系重合。通过第2、3节介绍的方法可以获取抓取点在相机坐标系中的坐标值 $({x}_{\mathrm{c}},{y}_{\mathrm{c}},{{\textit{z}}}_{\mathrm{c}})$ ，以及TNB坐标系到相机坐标系的旋转矩阵 ${\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{C}}$ ，由坐标系重合可知，运动到抓取点时，TCP坐标系的原点在相机坐标系中的坐标值为 $({x}_{\mathrm{c}},{y}_{\mathrm{c}},{{\textit{z}}}_{\mathrm{c}})$ ， TCP坐标系到相机坐标系的旋转矩阵为 ${\boldsymbol{R}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{C}}={\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{C}}$ ，下文将 ${\boldsymbol{R}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{C}}$ 以 ${\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{C}}$ 代替。由4.1节可知，已知TCP原点在基准坐标系中的坐标值 $({x}_{\mathrm{b}},{y}_{\mathrm{b}},{{\textit{z}}}_{\mathrm{b}})$ 和TCP到基准坐标系的旋转矩阵 ${\boldsymbol{R}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{B}}$ 即可求取TCP的位姿参数 $(x,y,{\textit{z}},r_x,r_y,r_{\textit{z}})$ 。

由坐标变换关系可知

${[{x}_{\mathrm{b}},{y}_{\mathrm{b}},{{\textit{z}}}_{\mathrm{b}},1]}^{\mathrm{T}}={\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}{·} [{{x}_{\mathrm{c}},{y}_{\mathrm{c}},{{\textit{z}}}_{\mathrm{c}},1]}^{\mathrm{T}}$

(10)

${\boldsymbol{R}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{B}}={\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{B}}={\boldsymbol{R}}_{\mathrm{C}}^{\mathrm{B}}{\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{C}}$

(11)

式中： ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 为相机坐标系到基准坐标系的变换矩阵， ${\boldsymbol{R}}_{\mathrm{C}}^{\mathrm{B}}$ 为相机坐标系到基准坐标系的旋转矩阵。由于 ${\boldsymbol{R}}_{\mathrm{C}}^{\mathrm{B}}$ 包含于 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 中，故只需求得 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 矩阵，即可计算TCP位姿参数。

4.3 相机手眼标定方法

4.3.1 手眼标定原理

由4.2节可知，获取TCP位姿参数的前提是求取相机坐标系到基准坐标系的变换矩阵 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ ，求取 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 的过程即称为手眼标定。手眼标定的形式有2种，分别为eye-in-hand模式和eye-to-hand模式，2种模式的原理相同。本文采用eye-to-hand模式，如图5所示。eye-to-hand模式的相机固定在环境中某处，相对于基座的位姿关系不变，需要求解的量为相机坐标系到基准坐标系的变换矩阵 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 。

图 5 手眼标定eye-to-hand模式

Fig. 5 Eye-to-hand model of eye-hand calibration

下载: 全尺寸图片

在eye-to-hand模式下，对于机器人夹着标定板移动任意2个位姿，由于标定板与TCP的相对位姿以及相机与基座的相对位姿不变，有公式成立：

${\boldsymbol{T}}_{\mathrm{B}2}^{\mathrm{T}\mathrm{C}\mathrm{P}}{·} {\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}{·} {\boldsymbol{T}}_{\mathrm{O}2}^{\mathrm{C}}={T}_{\mathrm{B}1}^{\mathrm{T}\mathrm{C}\mathrm{P}}{·} {\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}{·} {T}_{\mathrm{O}1}^{\mathrm{C}}$

(12)

式中 ${\boldsymbol{T}}_{\mathrm{O}}^{\mathrm{C}}$ 为外参矩阵，需要通过外参标定求取。

式(12)经左右移项，可得

${{\boldsymbol{T}}_{\mathrm{B}1}^{\mathrm{T}\mathrm{C}\mathrm{P}}}^{-1}{·} {\boldsymbol{T}}_{\mathrm{B}2}^{\mathrm{T}\mathrm{C}\mathrm{P}}{·} {\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}={\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}{·} {T}_{\mathrm{O}1}^{\mathrm{C}}{·} {{\boldsymbol{T}}_{\mathrm{O}2}^{\mathrm{C}}}^{-1}$

(13)

式(13)可以表示为 $\boldsymbol{A}{·} \boldsymbol{X}=\boldsymbol{X}{·} \boldsymbol{B}$ 的形式，求解 $\boldsymbol{X}$ 即可得到变换矩阵 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 。

手眼标定过程中的标定板坐标系即可视作2.2节提到的世界坐标系，而外参标定的目标便是求取世界坐标系到相机坐标系的变换矩阵 ${\boldsymbol{T}}_{\mathrm{O}}^{\mathrm{C}}$ 。可采用张正友标定法^[26]进行标定，该方法可以同时标定相机的内参和外参，并且有较高的精度，张正友标定法通过推导定义了一个单应性矩阵：

$\boldsymbol{H}=s\left[\begin{array}{ccc}{f}_{x}& 0& {c}_{x}\\ 0& {f}_{y}& {c}_{y}\\ 0& 0& 1\end{array}\right]\left[\begin{array}{ccc}{\boldsymbol{r}}_{1}& {\boldsymbol{r}}_{2}& \boldsymbol{t}\end{array}\right]=s\boldsymbol{M}\left[\begin{array}{ccc}{{\boldsymbol{r}}}_{1}& {{\boldsymbol{r}}}_{2}& {\boldsymbol{t}}\end{array}\right]$

式中： s表示尺度因子， ${f}_{x}$ 、 ${f}_{y}$ 、 ${c}_{x}$ 、 ${c}_{y}$ 表示相机内参， ${\boldsymbol{r}}_{1}$ 、 ${\boldsymbol{r}}_{2}$ 、 $\boldsymbol{t}$ 表示相机外参， $\boldsymbol{M}$ 为内参矩阵。 $\boldsymbol{H}$ 用来描述物体在世界坐标系和像素坐标系之间的映射关系，它同时包含了内参和外参，求解单应性矩阵即可获取相机的内参和外参。具体求解方法本文不再赘述，可参考文献[26]。将已知的变换矩阵 ${\boldsymbol{T}}_{\mathrm{T}\mathrm{C}\mathrm{P}}^{\mathrm{B}}$ 和求解单应性矩阵得到的外参矩阵 ${\boldsymbol{T}}_{\mathrm{O}}^{\mathrm{C}}$ 代入式(13)，即可对 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 进行求解。

4.3.2 变换矩阵求解方法

4.3.1节提到，求解 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 的方程可以表示为 $\boldsymbol{A}{·} \boldsymbol{X}=\boldsymbol{X}{·} \boldsymbol{B}$ 的形式，考虑到 $\boldsymbol{X}$ 为齐次变换矩阵，具有特殊的形式，Tsai和Lenz由此提出一种简化的分步求解方法，先求解旋转矩阵 $\boldsymbol{R}$ ，再求解平移向量 $\boldsymbol{t}$ ，即为Tsai-Lenz分步法^[27]。其基本原理可总结为：

首先，将式(13)表示为

${\boldsymbol{T}}_{\mathrm{B}\mathrm{i}\mathrm{j}}{\boldsymbol{T}}_{\mathrm{C}\mathrm{B}}={\boldsymbol{T}}_{\mathrm{C}\mathrm{B}}{\boldsymbol{T}}_{\mathrm{C}\mathrm{i}\mathrm{j}}$

式中 ${\boldsymbol{T}}_{\mathrm{B}\mathrm{i}\mathrm{j}}={{\boldsymbol{T}}_{\mathrm{B}\mathrm{j}}^{\mathrm{T}\mathrm{C}\mathrm{P}}}^{-1}{·} {\boldsymbol{T}}_{\mathrm{B}\mathrm{i}}^{\mathrm{T}\mathrm{C}\mathrm{P}}$ ， ${\boldsymbol{T}}_{\mathrm{C}\mathrm{i}\mathrm{j}}={{\boldsymbol{T}}_{\mathrm{C}\mathrm{j}}^{\mathrm{O}}}^{-1}{·} {\boldsymbol{T}}_{\mathrm{C}\mathrm{i}}^{\mathrm{O}}$ 。

展开可得

$\begin{gathered} \left[\begin{array}{cc}{\boldsymbol{R}}_{\mathrm{B}\mathrm{i}\mathrm{j}}& {\boldsymbol{t}}_{\mathrm{B}\mathrm{i}\mathrm{j}}\\ {\bf{0}}& 1\end{array}\right]\left[\begin{array}{cc}{\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}& {\boldsymbol{t}}_{\mathrm{C}\mathrm{B}}\\ {\bf{0}}& 1\end{array}\right]=\\ \left[\begin{array}{cc}{\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}& {\boldsymbol{t}}_{\mathrm{C}\mathrm{B}}\\ {\bf{0}}& 1\end{array}\right]\left[\begin{array}{cc}{\boldsymbol{R}}_{\mathrm{C}\mathrm{i}\mathrm{j}}& {\boldsymbol{t}}_{\mathrm{C}\mathrm{i}\mathrm{j}}\\ {\bf{0}}& 1\end{array}\right] \end{gathered}$

依据矩阵乘法可得分步公式：

${\boldsymbol{R}}_{\mathrm{B}\mathrm{i}\mathrm{j}}{\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}={\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}{\boldsymbol{R}}_{\mathrm{C}\mathrm{i}\mathrm{j}}$

(14)

$({\boldsymbol{R}}_{\mathrm{B}\mathrm{i}\mathrm{j}}-\boldsymbol{I}){\boldsymbol{t}}_{\mathrm{C}\mathrm{B}}={\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}{\boldsymbol{t}}_{\mathrm{C}\mathrm{i}\mathrm{j}}-{\boldsymbol{t}}_{\mathrm{B}\mathrm{i}\mathrm{j}}$

(15)

令 ${\boldsymbol{P}}_{r}=2\mathrm{s}\mathrm{i}\mathrm{n}\dfrac{\theta }{2}{\boldsymbol{n}}^{\mathrm{T}}$ ，其中 $\theta$ 和 $\boldsymbol{n}$ 对应式(8)，将其代入式(8)可得

${\boldsymbol{R}}_{r}=\left(1-\dfrac{{\left|{\boldsymbol{P}}_{r}\right|}^{2}}{2}\right)\boldsymbol{I}+ \dfrac{1}{2}\left({\boldsymbol{P}}_{r}{\boldsymbol{P}}_{r}^{\mathrm{T}}+\sqrt{4-{\left|{\boldsymbol{P}}_{r}\right|}^{2}}\mathrm{s}\mathrm{k}\mathrm{e}\mathrm{w}\left({\boldsymbol{P}}_{r}\right)\right)$

(16)

式中 $\mathrm{s}\mathrm{k}\mathrm{e}\mathrm{w}$ (•)为反对称运算，假设一个三维向量 $\boldsymbol{n}=({n}_{x},{n}_{y},{n}_{{\textit{z}}})$ ，其反对称矩阵为

$\mathrm{s}\mathrm{k}\mathrm{e}\mathrm{w}\left(\boldsymbol{n}\right)=\left[\begin{array}{ccc}0& -{n}_{{\textit{z}}}& {n}_{y}\\ {n}_{{\textit{z}}}& 0& -{n}_{x}\\ {-n}_{y}& {n}_{x}& 0\end{array}\right]$

首先计算 ${\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}$ ，由式(14)可得线性方程组：

$\mathrm{s}\mathrm{k}\mathrm{e}\mathrm{w}({\boldsymbol{P}}_{\mathrm{B}\mathrm{i}\mathrm{j}}+{\boldsymbol{P}}_{\mathrm{C}\mathrm{i}\mathrm{j}}){\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}{'}={\boldsymbol{P}}_{\mathrm{C}\mathrm{i}\mathrm{j}}-{\boldsymbol{P}}_{\mathrm{B}\mathrm{i}\mathrm{j}}$

求解可得 ${\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}{'}$ ，进一步计算 ${\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}$ ：

$\theta =2\mathrm{a}\mathrm{r}\mathrm{c}\mathrm{t}\mathrm{a}\mathrm{n}({\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}{'})$

${\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}=\dfrac{2{\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}{'}}{\sqrt{1+{\left|{\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}{'}\right|}^{2}}}$

将 ${\boldsymbol{P}}_{\mathrm{C}\mathrm{B}}$ 代入式(16)即可求解 ${\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}$ 。将 ${\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}$ 代入式(15)，求解线性方程组得到 ${\boldsymbol{t}}_{\mathrm{C}\mathrm{B}}$ 。

该方法通过求解2个线性方程组，依次求解 ${\boldsymbol{R}}_{\mathrm{C}\mathrm{B}}$ 和 ${\boldsymbol{t}}_{\mathrm{C}\mathrm{B}}$ ，组合即可得到相机坐标系到基准坐标系的齐次变换矩阵 ${\boldsymbol{T}}_{\mathrm{C}}^{\mathrm{B}}$ 。由于线性方程组包含3个方程，故求解至少需要代入3组数据。结合式(10)、(11)即可求解TCP的位姿参数。

求解机械臂位姿参数的基本流程由图6所示。

图 6 机械臂位姿参数求解流程

Fig. 6 Solution procedure of the pose of the robotic arm

下载: 全尺寸图片

5. 实验结果分析

5.1 曲线拟合误差分析

在进行曲线重构时，考虑2种坐标拟合方法：贝塞尔多边形法和B样条插值法。

本文用Hausdorff距离来描述曲线拟合的效果。Hausdorff距离的定义式为

$h\left(A,B\right)=\underset{a\in A}{\mathrm{m}\mathrm{a}\mathrm{x}}(\underset{b\in B}{\mathrm{m}\mathrm{i}\mathrm{n}\left(d\right(a,b\left)\right))}$

式中：a和b分别为集合A和B中的点； $d(a,b)$ 为2点间的任何度量，此处采用欧几里得距离。

由于Hausdorff距离是定向的，即 $h\left(A,B\right)$ 不一定等于 $h\left(B,A\right)$ ，所以，更一般的Hausdorff距离定义为

$H\left(A,B\right)=\mathrm{m}\mathrm{a}\mathrm{x}(h\left(A,B\right),h\left(B,A\right))$

将通过图像处理和坐标变换后的曲线特征点坐标设为集合A，从中选取一定数量的点作为控制点进行拟合，将拟合曲线与控制点相对应的坐标设为集合B，测试在不同数量的控制点下，贝塞尔和B样条2种方法对应的Hausdorff距离。Hausdorff距离越小，则说明拟合效果越好。结果如图7所示。

图 7 2种拟合方法的Hausdorff距离

Fig. 7 Hausdorff distance of two fitting methods

下载: 全尺寸图片

由图7可知，控制点数量较多时，B样条的拟合效果远好于贝塞尔；在控制点数量较少时，贝塞尔和B样条的拟合效果相近，但此时的拟合效果远不及B样条在控制点较多时的拟合效果。因此在拟合曲线时，本文优先选择B样条插值法。

在进行弧长参数化时，需要对数据进行二次拟合，在同样采用B样条方法的情况下，最初的时间参数化的拟合效果和二次弧长参数化的拟合效果如图8所示。由图8可知，二次拟合的拟合效果与一次拟合相差无几，同时，控制点数量选择在20～40时，拟合效果较好。后续实验将分别验证2种参数化拟合对抓取效果的影响。

图 8 时间参数化和弧长参数化的Hausdorff距离

Fig. 8 Hausdorff distance of time parameterization and arc-length parameterization

下载: 全尺寸图片

5.2 软线抓取试验

对于任意弧长处的抓取点，求解得到的TCP位姿参数可能会对应机械夹爪朝上的姿态，由于桌面的限制，机械臂不可能自下而上运动来抓取软线，因此需要对TCP位姿进行一些变换。考虑到机械臂基座坐标的 ${\textit{z}}$ 轴指向向上，欲使机械夹爪朝下，需要保证TCP坐标系的 ${\textit{z}}$ 轴与基坐标系的 ${\textit{z}}$ 轴反向，结合式(9)可知，应保证 ${b}_{3}$ 为负值。

对于一根放置在桌面上的软线，会有紧贴桌面和翘曲离开桌面2种形态。当抓取点选在紧贴桌面的部分时，抓取时夹爪末端会与桌面发生接触挤压，影响抓取效果，如图9所示。同时，由于夹爪具有一定体积，当夹爪与桌面的夹角过小时，夹爪无法正常运动至抓取位姿，同样影响抓取效果。由于桌面和机械臂基座均保持水平位置，可以令 ${b}_{3}=-1$ ，此时 ${b}_{1}$ 、 ${b}_{2}$ 、 ${t}_{3}$ 和 ${n}_{3}$ 均为0，由原切向量在 $xy$ 平面的投影向量的方向，决定 ${t}_{1}$ 和 ${t}_{2}$ ，再计算 ${n}_{1}$ 和 ${n}_{2}$ ，得到新的TNB坐标系到相机坐标系的旋转矩阵 ${\boldsymbol{R}}_{\mathrm{T}\mathrm{N}\mathrm{B}}^{\mathrm{C}}$ ，再转换为机械臂的姿态参数。

图 9 夹爪与桌面发生接触挤压

Fig. 9 Gripper contacts and squeezes with the table

下载: 全尺寸图片

对软线进行时间参数化和弧长参数化，分别进行50次抓取实验，其中25次抓取紧贴桌面的位置，25次抓取远离桌面的位置。具体抓取情况如表1所示。

表 1 抓取成功率

Table 1 Success rate of gripping

参数化方法	抓取位置	成功率
时间参数化	紧贴桌面	0.88
时间参数化	离开桌面	0.96
时间参数化	任意	0.92
弧长参数化	紧贴桌面	0.92
弧长参数化	离开桌面	0.96
弧长参数化	任意	0.94

由表1可知，2种参数化方法总体上均有90%以上的成功率，这表明二次拟合产生的误差可以忽略，且旋转矩阵的调整成功削弱了抓取点紧贴桌面时夹爪和桌面发生接触挤压的影响。

抓取结果图如图10所示，展示了软线不同位姿下的抓取情况。其中图10(a)～(h)为抓取点紧贴桌面的情况，图10(i)～(o)为抓取点翘曲离开桌面的情况。图10(d)和(f)对应的软线横截面直径为1.5 mm，其他子图对应的软线横截面直径为3 mm。可以看出，机械臂不仅准确地抓取了软线，没有与桌面产生挤压，夹爪的姿态也顺应了软线的切向与法向。实验成功验证了本文提出的方法的可行性。

图 10 抓取结果示意

Fig. 10 Gripping results

下载: 全尺寸图片

6. 结束语

本文提出了一种利用深度相机的图像提取软线空间坐标信息并基于Frenet标架对软线建模重构的方法，依据六自由度机械臂位姿转换原理，完成了抓取验证，实验结果表明，图像处理和曲线拟合的方法可以实现对软线抓取点较高精度的定位和定姿。以往的软线抓取方法往往忽略了抓取点处软线的姿态特征，而本文的方法对抓取点的位置和姿态均做出了考量，使抓取效果更加完美。

本文使用的弧长参数化的Frenet标架的曲线重构方法，不仅可以获取软线任意弧长处的形态特征，还可以通过曲率挠率的变化方便地描述软线的形变，未来工作也将基于Frenet重构进一步实现如插拔、打结等更复杂的软线操作与控制。

图 1 研究框架

Fig. 1 Research framework

下载: 全尺寸图片

图 2 图像处理

Fig. 2 Image processing

下载: 全尺寸图片

图 3 坐标系间的几何关系

Fig. 3 Geometrical relation between the coordinate systems　　　　

下载: 全尺寸图片

图 4 软线的Frenet建模

Fig. 4 Frenet modeling of the wire

下载: 全尺寸图片

图 5 手眼标定eye-to-hand模式

Fig. 5 Eye-to-hand model of eye-hand calibration

下载: 全尺寸图片

图 6 机械臂位姿参数求解流程

Fig. 6 Solution procedure of the pose of the robotic arm

下载: 全尺寸图片

图 7 2种拟合方法的Hausdorff距离

Fig. 7 Hausdorff distance of two fitting methods

下载: 全尺寸图片

图 8 时间参数化和弧长参数化的Hausdorff距离

Fig. 8 Hausdorff distance of time parameterization and arc-length parameterization

下载: 全尺寸图片

图 9 夹爪与桌面发生接触挤压

Fig. 9 Gripper contacts and squeezes with the table

下载: 全尺寸图片

图 10 抓取结果示意

Fig. 10 Gripping results

下载: 全尺寸图片

表 1 抓取成功率

Table 1 Success rate of gripping

参数化方法	抓取位置	成功率
时间参数化	紧贴桌面	0.88
时间参数化	离开桌面	0.96
时间参数化	任意	0.92
弧长参数化	紧贴桌面	0.92
弧长参数化	离开桌面	0.96
弧长参数化	任意	0.94

参考文献(27)

[1]	计时鸣, 黄希欢. 工业机器人技术的发展与应用综述[J]. 机电工程, 2015, 32(1): 1−13. JI Shiming, HUANG Xihuan. Review of development and application of industrial robot technology[J]. Journal of mechanical & electrical engineering, 2015, 32(1): 1−13.
[2]	潘广耀. 基于机器视觉的工业机器人目标定位及检测系统研究[D]. 青岛: 青岛科技大学, 2022. PAN Guangyao. Research on target location and detection system of industrial robot based on machine vision[D]. Qingdao: Qingdao University of Science & Technology, 2022.
[3]	刘文龙. 面向智能制造的视觉识别技术及其应用研究[D]. 哈尔滨: 哈尔滨工业大学, 2021. LIU Wenlong. Research on visual identification technology for intelligent manufacturing and its application[D]. Harbin: Harbin Institute of Technology, 2021.
[4]	李泽辰. 基于双目视觉的工业机器人目标识别与定位系统研究[D]. 青岛: 青岛科技大学, 2021. LI Zechen. Research on industrial robot target recognition and positioning system based on binocular vision[D]. Qingdao: Qingdao University of Science & Technology, 2021.
[5]	陈英. 机器视觉技术的发展现状与应用动态研究[J]. 无线互联科技, 2018, 15(19): 147−148. doi: 10.3969/j.issn.1672-6944.2018.19.067 CHEN Ying. Study on development status and application trends of machine vision technology[J]. Wireless Internet technology, 2018, 15(19): 147−148. doi: 10.3969/j.issn.1672-6944.2018.19.067
[6]	王飞. 机器视觉技术发展及其工业应用[J]. 电子技术与软件工程, 2018(16): 246. WANG Fei. Development of machine vision technology and its industrial application[J]. Electronic technology & software engineering, 2018(16): 246.
[7]	滕婷婷. 面向交通复杂目标场景的机器视觉检测技术研究[D]. 南京: 南京邮电大学, 2022. TENG Tingting. Research on machine vision detection technologiesfor complex traffic target scenarios[D]. Nanjing: Nanjing University of Posts and Telecommunications, 2022.
[8]	王瑞阳, 徐洋. 机器视觉技术在农业机器人定位中的作用[J]. 南方农机, 2023, 54(13): 90−92. WANG Ruiyang, XU Yang. The role of machine vision technology in the positioning of agricultural robots[J]. China southern agricultural machinery, 2023, 54(13): 90−92.
[9]	唐浩然. 机器视觉的研究及应用进展[J]. 科技与创新, 2024(16): 52−55. TANG Haoran. Research and application progress of machine vision[J]. Science and technology & innovation, 2024(16): 52−55.
[10]	颉永鹏. 基于机器视觉的工业机器人目标识别和定位研究[D]. 沈阳: 沈阳工业大学, 2022. XIE Yongpeng. Research on target recognition and location of industrial robots based on machine vision[D]. Shenyang: Shenyang University of Technology, 2022.
[11]	刘远桃, 秦龙, 杨海能. 基于机器视觉的工业机器人分拣技术研究[J]. 机电工程技术, 2024, 53(9): 173−176. LIU Yuantao, QIN Long, YANG Haineng. Research on sorting technology of industrial robots based on machine vision[J]. Mechanical & electrical engineering technology, 2024, 53(9): 173−176.
[12]	程家浩.基于机器视觉的工业机器人定位与抓取方法研究[D]. 天津: 天津理工大学, 2024. CHENG Jiahao. Research on positioning and grasping methods for industrial robots based on machine vision[D]. Tianjin: Tianjin University of Technology, 2024.
[13]	LYU Naijing, LIU Jianhua, JIA Yunyi. Dynamic modeling and control of deformable linear objects for single-arm and dual-arm robot manipulations[J]. IEEE transactions on robotics, 2022, 38(4): 2341−2353. doi: 10.1109/TRO.2021.3139838
[14]	SANCHEZ J, CORRALES J A, BOUZGARROU B C, et al. Robotic manipulation and sensing of deformable objects in domestic and industrial applications: a survey[J]. The international journal of robotics research, 2018, 37(7): 688−716.
[15]	INOUE H. Hand eye coordination in rope handling[C]//Proceedings of International Symposium of Robotics Research. Cambridge: MIT Press, 1984: 163−174.
[16]	LUI W H, SAXENA A. Tangled: learning to untangle ropes with RGB-D perception[C]//2013 IEEE/RSJ International Conference on Intelligent Robots and Systems. Tokyo: IEEE, 2013: 837−844.
[17]	KUMRA S, KANAN C. Robotic grasp detection using deep convolutional neural networks[C]//2017 IEEE/RSJ International Conference on Intelligent Robots and Systems. Vancouver: IEEE, 2017: 769−776.
[18]	SCHULMAN J, LEE A, HO J, et al. Tracking deformable objects with point clouds[C]//2013 IEEE International Conference on Robotics and Automation. Karlsruhe: IEEE, 2013: 1130−1137.
[19]	YAMAKAWA Y, NAMIKI A, ISHIKAWA M. Dynamic high-speed knotting of a rope by a manipulator[J]. International journal of advanced robotic systems, 2013, 10(10): 361. doi: 10.5772/56783
[20]	LEVIN D. The approximation power of moving least-squares[J]. Mathematics of computation, 1998, 67(224): 1517−1531. doi: 10.1090/S0025-5718-98-00974-0
[21]	AMENTA N, BERN M, EPPSTEIN D. The crust and the β-skeleton: combinatorial curve reconstruction[J]. Graphical models and image processing, 1998, 60(2): 125−135. doi: 10.1006/gmip.1998.0465
[22]	YAN Dongming, WINTZ J, MOURRAIN B, et al. Efficient and robust tree model reconstruction from laser scanned data points[C]//Proceedings of the 11th IEEE International conference on Computer-Aided Design and Computer Graphics. Washington: IEEE, 2009: 572−576.
[23]	朱东方. 基于复杂拓扑结构点云的曲线拟合研究与应用[D]. 济南: 山东大学, 2015. ZHU Dongfang. Research and application of curve fitting based on complex topological structure point cloud[D]. Jinan: Shandong University, 2015.
[24]	林思宇. 受限空间下基于视觉的线缆装配方法研究[D]. 哈尔滨: 哈尔滨工业大学, 2021. LIN Siyu. Research on cable assembly method based on vision in confined space[D]. Harbin: Harbin Institute of Technology, 2021.
[25]	PARK J, BRUNEL N, CHASSAT P. Curvature and Torsion estimation of 3D functional data: a geometric approach to build the mean shape under the Frenet Serret framework[EB/OL]. (2022−03−04)[2024−3−12]. https://arxiv.org/abs/2203.02398v1.
[26]	ZHANG Zhengyou. Flexible camera calibration by viewing a plane from unknown orientations[C]//Proceedings of the Seventh IEEE International Conference on Computer Vision. Kerkyra: IEEE, 2002: 666−673.
[27]	TSAI R Y, LENZ R K. A new technique for fully autonomous and efficient 3D robotics hand/eye calibration[J]. IEEE transactions on robotics and automation, 1989, 5(3): 345−358. doi: 10.1109/70.34770

点击查看大图

图(10) / 表(1)

摘要

1. 问题引入
2. 基于图像提取特征点
2.1 特征像素点提取
2.2 求解特征点三维坐标
3. 曲线拟合重构
3.1 曲线形状求解
3.2 曲线对齐
3.3 曲线重构
4. 求解机械臂位姿参数
4.1 机器人坐标系及位姿表示
4.2 TCP位姿参数求解方法
4.3 相机手眼标定方法
5. 实验结果分析
5.1 曲线拟合误差分析
5.2 软线抓取试验
6. 结束语

1. 问题引入
2. 基于图像提取特征点
2.1 特征像素点提取
2.2 求解特征点三维坐标
3. 曲线拟合重构
3.1 曲线形状求解
3.2 曲线对齐
3.3 曲线重构
4. 求解机械臂位姿参数
4.1 机器人坐标系及位姿表示
4.2 TCP位姿参数求解方法
4.3 相机手眼标定方法
5. 实验结果分析
5.1 曲线拟合误差分析
5.2 软线抓取试验
6. 结束语

参考文献(27)

图像信息输入下的软线重构与抓取验证

doi: 10.11992/tis.202403021

通讯作者: 王明明. E-mail：mwang@nwpu.edu.cn.

出版历程

Flexible wire reconstructing and gripping verification under the image information input

1. 问题引入

2. 基于图像提取特征点

2.1 特征像素点提取

2.2 求解特征点三维坐标

3. 曲线拟合重构

3.1 曲线形状求解

3.2 曲线对齐

3.3 曲线重构

4. 求解机械臂位姿参数

4.1 机器人坐标系及位姿表示

4.2 TCP位姿参数求解方法

4.3 相机手眼标定方法

4.3.1 手眼标定原理

4.3.2 变换矩阵求解方法

5. 实验结果分析

5.1 曲线拟合误差分析

5.2 软线抓取试验

6. 结束语

出版历程

目录

1. 问题引入

2. 基于图像提取特征点

2.1 特征像素点提取

2.2 求解特征点三维坐标

3. 曲线拟合重构

3.1 曲线形状求解

3.2 曲线对齐

3.3 曲线重构

4. 求解机械臂位姿参数

4.1 机器人坐标系及位姿表示

4.2 TCP位姿参数求解方法

4.3 相机手眼标定方法

5. 实验结果分析

5.1 曲线拟合误差分析

5.2 软线抓取试验

6. 结束语

通讯作者:
王明明. E-mail：mwang@nwpu.edu.cn.