基于双目视觉的3PSS/S并联机构位姿监测算法

邓云蛟，侯雨雷，侯荣伟，孔繁栋，周 挺，曾达幸

(燕山大学 机械工程学院，河北 秦皇岛 066004)

0 引言

并联机构因其比刚度大、结构紧凑、承载能力强等优点，广泛应用于航空航天、机床、医疗器械、人形机器人、六维力传感器[1]等诸多领域，而对其末端位姿的测量是进行运动学标定的基础，对于其高性能控制更起着至关重要的作用。

以往大多是通过安装传感器或利用激光跟踪仪等测量手段对并联机构末端位姿进行测量，梁顺攀等[2]利用球杆仪、高度游标卡尺和双轴倾角传感器配合测量了6-PUS/UPS并联机构动平台的位姿；Rauf等[3]通过双周倾角传感器、线性可变差动变压换能器和旋转传感器对六自由度的HexaSlide机构位姿进行了测量;张文昌等[4]以激光跟踪仪作为测量工具，为Delta并联机构运动学误差标定提供了位姿信息;樊锐等[5]则利用激光跟踪仪得到的6PUS并联机构位姿信息进行了整机标定实验。但由于这些测量手段或者检测设备昂贵，或者检测方法复杂，难以真正实现大范围实用化和商业化。

计算机视觉[6]因测量设备与被监测系统相互独立，对被监测系统运行无任何影响，引起了学者们的关注。计算机视觉被分为单目[7]、双目[8]和多目[9]，其中单目视觉无法准确获得深度信息;多目视觉可通过不同的摄像头来覆盖不同范围的场景，但随之空间占用大大增加且成本成倍增加，而受启发于“人眼”的双目视觉则相对灵活。双目视觉目前多应用于果实的采摘[10-12]中，而在并联机构位姿测量中的应用还处于起步阶段[13-14]。

为进行并联机构运动学标定，更好地控制并联机构实现所需运动，如何快速方便有效地测量并联机构末端位姿成为研究的方向和热点。本文以动平台为圆形的3PSS/S并联机构为研究对象，基于双目视觉，提出一套并联机构位姿监测方法，通过对比最小二乘法、遗传算法和粒子群算法所求动平台中心，选择残差最小的自适应权重粒子群算法(SAPSO)作为最优算法，并将整体流程进行可视化显示。

1 3PSS/S并联机构位姿监测系统

基于双目视觉，搭建3PSS/S并联机构位姿监测系统，如图1所示，主要由计算机、双目摄像机、3PSS/S并联机构三部分组成，其中双目摄像机对准并联机构，非接触地摄取其运动图像;计算机通过数据传输线与双目摄像机相连，负责处理由双目摄像机所摄回的图像，并返回并联机构动平台的位姿。

图1 3PSS/S并联机构位姿监测系统示意图
Fig.1 The schematic diagram of position monitoring system for 3PSS/S parallel mechanism

针对动平台为圆形的3PSS/S并联机构[15]，如图2所示，基于双目视觉，进行其动平台位姿监测的步骤如下：

1) 搭建双目立体监测平台，制作标定板，标定左右相机内外参数;

2) 利用双目立体监测平台，采集并联机构图像，经阈值分割和腐蚀处理提取动平台标识区域;

3) 将图像二值化处理，建立边缘搜索模型，搜索动平台标识区域边缘;

4) 采用最小二乘法、遗传算法和粒子群算法，分别进行边缘点椭圆拟合，提取动平台中心;

5) 三维重建出并联机构动平台中心点世界坐标。

图2 3PSS/S并联机构结构图
Fig.2 The structure diagram of 3PSS/S parallel mechanism

利用张氏标定法[16]，即可完成左右相机内外参数的标定，不予赘述。

2 阈值分割与边缘提取

从摄像机所拍摄的图像中提取机构动平台边缘点的关键在于，如何将动平台从机构图像背景中分离出来。此时，一种简便直接的方法便是利用图像本身的特征——颜色空间，将所得图像进行阈值分割，以将所需目标凸显，去除干扰背景。

2.1 阈值分割

阈值分割，即选取合适阈值把图像分成目标和背景，进而去除背景，保留目标[17]。

将所摄取的3PSS/S并联机构图像表示在RGB颜色空间中，即得到各像素点(xi,yj) 处的R、G、B分量矩阵Ym×n×3，其中m和n分别为图像横向和纵向像素数，xi和yj分别为图像横向第i个像素和纵向第j个像素，且0≤i≤m，0≤j≤n。

提取一条穿过并联机构动平台直线x=l0上的R、G、B各分量分布，如图3所示。

图3 包含目标的直线R/G/B分布图
Fig.3 The R/G/B distribution of the line containing target

由于目标区域标有红色标识，由图可明显地看出目标线段与其他线段的R、G、B分量间关系的差别，基于此，可设置阈值：

(1)

其中，b1、b2分别为目标线段R分量相对G分量、B分量的阈值。

在式(1)的基础上，对图像上全部像素点进行遍历搜索，设置条件：

if “式(1)成立”

Y(xi,yj,:)=(0,0,0)(显示为黑色)

else

Y(xi,yj,:)=(255,255,255)(显示为白色)

得到初始的目标区域，如图4所示。

图4 阈值分割后的图像
Fig.4 The image after threshold segmentation

图4中，除目标区域外，非目标区域也可能会有少许黑点存在或目标区域内有空洞，此时可对分割后的整个图像进行腐蚀作用，去除非目标区域的干扰点并填补目标区域内的空洞。

2.2 边缘提取

将所得图像二值化处理，即设置转换条件：

ifY(xi,yj,:)=(0,0,0)

else

(2)

基于式(2)，对图4进行遍历搜索，并记录下满足条件的像素点坐标，进而得到并联机构动平台中心区域的边缘图像，如图5所示。

图5 边缘提取的图像
Fig.5 The image of edge extraction

3 动平台中心点提取

经阈值分割和边缘提取得到一系列的像素坐标值，而由于目标区域为圆形，考虑视角的影响，目标边缘点组成一个近于椭圆的轮廓，可借助椭圆拟合方法得到其中心点的像素坐标值。本文采用最小二乘法、遗传算法和粒子群算法分别进行椭圆拟合并加以对比。

3.1 基于最小二乘法的椭圆拟合

最小二乘法是一种数学优化技术，由最小误差平方和得到其最优函数，这里即求边缘检测出来的每个像素点到待检测曲线距离的平方和[18]。

对像素坐标(xu,yu)，由椭圆方程隐式形式：

Ax2+Bxy+Cy2+Dx+Ey+F=0,

(3)

令F=-1，则所提取的边缘点像素坐标值满足：

PX=Q,

(4)

对于式(4)，P为N×5矩阵，满足列满秩，且N>5，则该式没有精确解，此时为超定方程组。可借助MATLAB寻求超定方程组的最小二乘解，此时有

X=PQ。

(5)

根据式(3)与椭圆标准形式的转换公式[18]以及式(5)，可求得动平台的中心像素坐标值 (xc,yc)：

(6)

(7)

3.2 基于遗传算法的椭圆拟合

利用椭圆的中心位置(xc,yc)、长半轴a和短半轴b、长轴相对水平x轴的转角θ，基于椭圆的标准形式经简单推导得到椭圆的一般表示：

(8)

其中，x*、y*为椭圆方程标准型的坐标值,x*=x-xc，y*=y-yc，x、y为椭圆方程一般表示的坐标值。

选取优化参数ζ=(xcycabθ)T，并以所提取的边缘点与所拟合椭圆最小距离的平均值为目标函数，则有

(9)

采用遗传算法进行动平台中心提取的步骤具体如下：

1) 初始化ζ0=(xc0yc0a0b0θ0)T;

2) 计算群体上每一个体的适应度值f;

3) 判断收敛准则和终止代数Ng，满足收敛准则或超过终止代数则输出收敛结果，不满足收敛准则且未超过终止代数则进入下一步;

4) 进行选择、交叉、变异操作，返回到步骤2。

设置相应的参数见表1，收敛准则设定为：所提取的动平台边缘点与拟合椭圆最小距离的平均值不超过1像素，即

f(ζ)<1。

(10)

借助MATLAB，通过迭代可得最终优化参数，即可得到动平台的中心像素坐标值(xc,yc)。

表1 遗传算法相关参数设置
Tab.1 The related parameters in GA

参数数值种群规模Na50终止代数Ng100交叉概率pc0.7变异概率pm0.3

3.3 基于粒子群算法的椭圆拟合

与基于遗传算法的椭圆拟合类似，同样借助式(8)，选取优化参数ζ，以f(ζ)为目标函数，采用粒子群算法进行动平台中心提取的步骤具体如下：

1) 初始化各微粒的速度vs0和位置ζs0;

2) 计算每个微粒的适应度值fs;

5) 将每个微粒的最好位置ζgs与全体微粒所经历的最好位置ζg对比，以更新ζg的值。

6) 检查终止条件，是否满足式(10)或达到终止代数Np，若是则输出结果ζg，若否则返回步骤3。

为平衡粒子群算法的全局搜索能力和局部改良能力，这里采用自适应权重的粒子群算法(SAPSO)。设置相应的参数见表2，且采用非线性的动态惯性权重系数：

(11)

其中，ωmax、ωmin分别表示权重ω的最大值和最小值，f为微粒当前的目标函数值，favg、fmin分别表示当前所有微粒的平均目标值和最小目标值。

表2 SAPSO相关参数设置
Tab.2 The related parameters of SAPSO

参数数值种群规模Ns50终止代数Np100学习因子c12学习因子c22最大权重ωmax0.9最小权重ωmin0.4

借助MATLAB得到优化后的动平台中心像素坐标值(xc,yc)。

3.4 最优算法选取

根据图5中所得到的边缘点像素坐标，分别采用最小二乘法(LS)、遗传算法(GA)、自适应权重的粒子群算法(SAPSO)进行椭圆拟合并求取中心点像素坐标，结果见图6所示。

图6 不同椭圆拟合方法对比图
Fig.6 Comparison of different fitting methods for ellipse

基于式(3)和式(8)，将提取的边缘点坐标xu代入，组成一个超定方程组，令残差[19]为

(12)

(13)

将残差作为椭圆拟合精度的评判指标，根据图6，并结合所得残差(见表3)，可以得到自适应权重的粒子群算法(SAPSO)拟合精度最好，因此选取SAPSO作为动平台中心提取的最优算法。

表3 不同椭圆拟合方法对应的残差
Tab.3 The residual of different ellipse fitting methods

椭圆拟合方法残差值最小二乘法(LS)0.973 3遗传算法(GA)0.055 2自适应权重的粒子群算法(SAPSO)0.010 8

4 三维重建与可视化显示

经椭圆拟合算法得到的左右相机对应图像中动平台中心点像素坐标值，可通过双目视觉三维重建获得其世界坐标。

首先，借助张氏标定法得到左右相机投影矩阵M1和M2，根据图像坐标系、相机坐标系和世界坐标系转换关系，满足关系式：

(14)

(15)

其中，[u1,v1]、[u2,v2]为动平台中心点在双目中匹配点对像素坐标;[X,Y,Z]为动平台中心点在世界坐标系下的三维坐标Ps。Zc1、Zc2分别为动平台中心点在双目相机坐标系下的深度坐标值。

由式(14)和(15)可得到关系式

K1Ps=K2,

(16)

其中，Ps=[X,Y,Z]T，K2=[0 0 0 0]T，

由最小二乘法可求得动平台中心点世界坐标

(17)

借助MATLAB GUI语言，将上述流程进行程序化设计，可视化显示3PSS/S并联机构动平台中心点的位姿监测，如图7所示。

5 结论

本文以3PSS/S并联机构为研究对象，基于双目视觉，提出一套动平台中心点的位姿监测方法，并通过对比最小二乘法、遗传算法和粒子群算法，以残差为评判标准，选出自适应权重的粒子群算法(SAPSO)作为动平台中心提取的最优算法，借助坐标转换关系得到了动平台中心的世界坐标，并将整体流程进行了可视化显示。该方法简单方便、操作性强、成本低、精度高，且通用性强，可用于其他并联机构末端位姿的测量。

图7 并联机构动平台位姿监测可视化界面
Fig.7 The visual interface for pose monitoring of the moving platform in parallel mechanism