大空间视觉跟踪6D测量系统及测量方法.pdf

本发明公开了一种大空间视觉跟踪6D测量系统及测量方法，该系统主要由自变焦视觉跟踪摄像机和彩色伪随机编码六自由度测头组成，利用靶标球上带有彩色伪随机编码表示的孔中心在摄像机成像面上的位置变化来确定测头的姿态和空间位置坐标，实现大尺寸空间六自由度的测量。该系统可以同关节臂坐标测量机组合起来使用，实现局部高精度的测量和定位；也可与电钻等装配设备结合使用，实现大型设备的精确装配。

1.大空间视觉跟踪6D测量系统，包括有一个摄像机和一个独立的六自由度
测头，其特征在于：所述摄像机安装在三维旋转云台上，绕云台的水平轴和垂直
轴转动，其转角用安装在三维旋转平台上的角度编码器读出；六自由度测头在空
间的三个坐标和三个姿态方位由摄像机跟踪测量获得。
2.根据权利要求1所述的大空间视觉跟踪6D测量系统，其特征在于：所述
摄像机为自动变焦摄像机，其光圈、焦距、焦点位置可以自动和手动调整。
3.根据权利要求1所述的大空间视觉跟踪6D测量系统，其特征在于：所述
的六自由度测头包括一个靶标球和一个球探头，两者之间用测杆相连接，在测杆
上还安装一个测头手柄。
4.根据权利要求1所述的大空间视觉跟踪6D测量系统，其特征在于：所述
靶标球为具有内部空腔的球体，其外表面上按照一定角度均匀地在经线和纬线的
相交位置布置圆柱孔，圆柱孔的轴线与球心相交。
5.根据权利要求1所述的大空间视觉跟踪6D测量系统，其特征在于：所述
靶标球圆柱孔里按照伪随机阵列编码的排列规则安置三种颜色标识，孔在表面上
的位置由伪随机阵列编码颜色标识唯一确定。
6.根据权利要求1所述的大空间视觉跟踪6D测量系统，其特征在于：所述
的三种颜色标识由红、黄、蓝三种发光二极管实现，也可以用不发光的红、黄、
蓝三种彩色物体实现。
7.根据权利要求1所述的大空间视觉跟踪6D测量系统，其特征在于：所述
测头手柄和摄像机云台内部安置无线收发器，实现六自由度测头与摄像机之间的
数据通讯。
8.基于权利要求1所述的大空间视觉跟踪6D测量系统的测量方法，其特征
在于：包括以下步骤：
(1)建立六自由度测头坐标系：以靶标球的球心为测头坐标系的原点，建
立测头坐标系O-Xt，Yt，Zt，靶标球表面上孔的中心位置在测头坐标系下的坐标设
为(xti，yti，zti)；测量时所使用的球探头中心在O-Xt，Yt，Zt坐标系下的坐标为
(Xtp，Ytp，Ztp)；所述球探头中心、靶标球表面上孔的中心位置在O-Xt，Yt，Zt坐标系
下的坐标值为已知量；
(2)建立云台坐标系：以摄像机坐标系O1-Xc，Yc，Zc的原点为原点建立云台
坐标系O1-Xr，Yr，Zr，所述云台坐标系O1-Xr，Yr，Zr的Yr坐标轴与云台水平回转面
中的零位重合，Xr轴与云台垂直转动轴中心线重合；
(3)建立摄像机坐标系和云台坐标系之间的关系：所述摄像机坐标系
O1-Xc，Yc，Zc随着云台做水平和垂直轴转动，在水平方向和垂直方向的旋转角度
分别为α，β，摄像机坐标系和云台坐标系之间的关系用如下方程表示：
X r Y r Z r = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 X c Y c Z c - - - ( a ) ; ]]>
(4)建立摄像机坐标系与测头坐标系之间的关系：当摄像机对靶标球成像
时，设靶标球表面上孔的中心位置在图像中的坐标为(ui，vi)，其对应的孔中心在
测头坐标系下的坐标为(xti，yti，zti)，建立如下方程：
λ u i v i 1 = f u s u 0 0 f v v 0 0 0 1 r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 x ti y ti z ti 1 - - - ( b ) , ]]>
其中：fu，fv为摄像机像平面u，v轴上的尺度因子，s为摄像机CCD像元
排列位置的倾斜修正项，(u0，v0)为摄像机CCD图像中心的像素坐标，fu、fv、
u0、v0、s为摄像机内部参数；r11，r12，……，r33为测头坐标系和摄像机坐标
系之间旋转矩阵参数，tx，ty，tz为测头坐标系和摄像机坐标系之间的平移矩阵
参数，r11，r12，……，r33，tx，ty，tz为摄像机相对于测头的外部参数；
(5)求解摄像机内外参数：测量时将球探头接触被测位置，自变焦视觉跟
踪的摄像机对靶标球进行跟踪拍摄，通过对拍摄得到的伪随机编码颜色图像进行
图像处理，求得颜色标识图像中心的图像坐标(ui，vi)，即靶标球表面上孔的中心
位置在图像中的坐标，结合对应的靶标球表面上孔中心在已知的测头坐标系下的
坐标(xti，yti，zti)，求解方程(b)确定摄像机的所有内外参数，包括有fu，fv，s，
r11，r12，……，r33，tx，ty，tz，u0，v0；
(6)求解球探头中心在云台坐标系下的坐标实现三维坐标测量：通过测头
坐标系和摄像机坐标系之间旋转矩阵参数r11，r12，……，r33和平移矩阵参数tx，
ty，tz，建立球探头中心(Xtp，Ytp，Ztp)在摄像机坐标系下的坐标方程：
X cp Y cp Z cp = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 X tp Y tp Z tp + t x t y t z - - - ( c ) , ]]>
联立方程(a)和(c)，得到球探头中心(Xtp，Ytp，Ztp)在云台坐标系下的坐标：
X rp Y rp Z rp = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 { r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 X tp Y tp Z tp + t x t y t z } - - - ( d ) ; ]]>
(7)求解测头姿态角：测头的姿态角用测头坐标系O-Xt，Yt，Zt相对于云台坐
标系O1-Xr，Yr，Zr的三个旋转角ψ，θ，φ表示，其中ψ为绕x轴旋转角度，θ为绕
y轴的旋转角度和φ为绕z轴的旋转角度，若令测头的坐标系O-Xt，Yt，Zt相对于
云台坐标系O1-Xr，Yr，Zr的旋转矩阵为R11，R12，……，R33，则由(d)式可以建
立下列关系：
R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 - - - ( e ) . ]]>
9.根据权利要求8所述的基于大空间视觉跟踪6D测量系统的测量方法，其
特征在于：所述步骤(7)中，测头的姿态角ψ，θ，φ按(f)式求得：

10.根据权利要求8所述的基于大空间视觉跟踪6D测量系统的测量方法，
其特征在于：所述靶标球可以独立固定在待测物体上，随物体一起运动，用摄像
机跟踪待测物体的姿态和位置六个自由度，测量方法包括以下步骤：
(1)根据权利要求8的步骤(1)-(5)，求得测头坐标系和摄像机坐标系
之间旋转矩阵参数r11，r12，……，r33，测头坐标系原点和摄像机坐标系原点之
间的平移矩阵参数tx，ty，tz；
(2)求解待测物体的三个姿态角：待测物体的姿态角就是测头坐标系
O-Xt，Yt，Zt相对于云台坐标系O1-Xr，Yr，Zr的三个旋转角ψ，θ，φ表示，用公式(e)
和(f)求得；
(3)求解待测物体的位置：由于靶标球固定在待测物体上，待测物体的位
置可以用靶标球球心，即测头坐标系原点相对与云台坐标系的三个坐标
(Xr，Yr，Zr)表示：
X r Y r Z r = t x t y t z - - - ( g ) . ]]>

大空间视觉跟踪6D测量系统及测量方法

技术领域

本发明涉及工业用尺寸测量系统，特别是应用于工业现场的大尺寸空间精密
测量，具体为一种大空间视觉跟踪6D测量系统及测量方法。

背景技术

目前，随着我国大型装备制造业的发展，对空间大尺寸精密测量技术提出了
越来越高的要求。现有的空间大尺寸测量系统主要由以下几种构成：(1)经纬仪
测量系统；(2)激光跟踪测量系统；(3)数字近景工业摄影测量系统；(4)激光
扫描测量系统；(5)关节臂坐标测量系统；(6)室内GPS。以上测量系统大都是
单点测量，不能实现六个自由度的测量。瑞士Leica公司设计的激光跟踪仪虽然
实现了六个自由度的测量，但是需要激光的配合。

发明内容

本发明的目的是提供一种大空间视觉跟踪6D测量系统及测量方法，以解决
现有大空间坐标测量系统无法同时测量测头或被测物体三个坐标和三个方位的
六自由度测量问题。

为了达到上述目的，本发明所采用的技术方案为：

大空间视觉跟踪6D测量系统，包括有一个摄像机和一个独立的六自由度测
头，其特征在于：所述摄像机安装在三维旋转云台上，绕云台的水平轴和垂直轴
转动，其转角用安装在三维旋转平台上的角度编码器读出；六自由度测头在空间
的三个坐标和三个姿态方位由摄像机跟踪测量获得。

所述的大空间视觉跟踪6D测量系统，其特征在于：所述摄像机为自动变焦
摄像机，其光圈、焦距、焦点位置可以自动和手动调整。

所述的大空间视觉跟踪6D测量系统，其特征在于：所述的六自由度测头包
括一个靶标球和一个球探头，两者之间用测杆相连接，在测杆上还安装一个测头
手柄。

所述的大空间视觉跟踪6D测量系统，其特征在于：所述靶标球为具有内部
空腔的球体，其外表面上按照一定角度均匀地在经线和纬线的相交位置布置圆柱
孔，圆柱孔的轴线与球心相交。

所述的大空间视觉跟踪6D测量系统，其特征在于：所述靶标球圆柱孔里按
照伪随机阵列编码的排列规则安置三种颜色标识，孔在表面上的位置由伪随机阵
列编码颜色标识唯一确定。

所述的大空间视觉跟踪6D测量系统，其特征在于：所述的三种颜色标识由
红、黄、蓝三种发光二极管实现，也可以用不发光的红、黄、蓝三种彩色物体实
现。

所述的大空间视觉跟踪6D测量系统，其特征在于：所述测头手柄和摄像机
云台内部安置无线收发器，实现六自由度测头与摄像机之间的数据通讯。

所述的大空间视觉跟踪6D测量系统的测量方法，其特征在于：包括以下步
骤：

(1)建立六自由度测头坐标系：以靶标球的球心为测头坐标系的原点，建
立测头坐标系O-Xt，Yt，Zt，靶标球表面上孔的中心位置在测头坐标系下的坐标设
为(xti，yti，zti)；测量时所使用的球探头中心在O-Xt，Yt，Zt坐标系下的坐标为
(Xtp，Ytp，Ztp)；所述球探头中心、靶标球表面上孔的中心位置在O-Xt，Yt，Zt坐标系
下的坐标值为已知量；

(2)建立云台坐标系：以摄像机坐标系O1-Xc，Yc，Zc的原点为原点建立云台
坐标系O1-Xr，Yr，Zr，所述云台坐标系O1-Xr，Yr，Zr的Yr坐标轴与云台水平回转面
中的零位重合，Xr轴与云台垂直转动轴中心线重合；

(3)建立摄像机坐标系和云台坐标系之间的关系：所述摄像机坐标系
O1-Xc，Yc，Zc随着云台做水平和垂直轴转动，在水平方向和垂直方向的旋转角度
分别为α，β，摄像机坐标系和云台坐标系之间的关系用如下方程表示：

X r Y r Z r = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 X c Y c Z c - - - ( a ) ; ]]>

(4)建立摄像机坐标系与测头坐标系之间的关系：当摄像机对靶标球成像
时，设靶标球表面上孔的中心位置在图像中的坐标为(ui，vi)，其对应的孔中心在
测头坐标系下的坐标为(xti，yti，zti)，建立如下方程：

λ u i v i 1 = f u s u 0 0 f v v 0 0 0 1 r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 x ti y ti z ti 1 - - - ( b ) , ]]>

其中：fu，fv为摄像机像平面u，v轴上的尺度因子，s为摄像机CCD像元
排列位置的倾斜修正项，(u0，v0)为摄像机CCD图像中心的像素坐标，fu、fv、
u0、v0、s为摄像机内部参数；r11，r12，……，r33为测头坐标系和摄像机坐标
系之间旋转矩阵参数，tx，ty，tz为测头坐标系和摄像机坐标系之间的平移矩阵
参数，r11，r12，……，r33，tx，ty，tz为摄像机相对于测头的外部参数；

(5)求解摄像机内外参数：测量时将球探头接触被测位置，自变焦视觉跟
踪的摄像机对靶标球进行跟踪拍摄，通过对拍摄得到的伪随机编码颜色图像进行
图像处理，求得颜色标识图像中心的图像坐标(ui，vi)，即靶标球表面上孔的中心
位置在图像中的坐标，结合对应的靶标球表面上孔中心在已知的测头坐标系下的
坐标(xti，yti，zti)，求解方程(b)确定摄像机的所有内外参数，包括有fu，fv，s，
r11，r12，……，r33，tx，ty，tz，u0，v0；

(6)求解球探头中心在云台坐标系下的坐标实现三维坐标测量：通过测头
坐标系和摄像机坐标系之间旋转矩阵参数r11，r12，……，r33和平移矩阵参数tx，
ty，tz，建立球探头中心(Xtp，Ytp，Ztp)在摄像机坐标系下的坐标方程：

X cp Y cp Z cp = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 X tp Y tp Z tp + t x t y t z - - - ( c ) , ]]>

联立方程(a)和(c)，得到球探头中心(Xtp，Ytp，Ztp)在云台坐标系下的坐标：

X rp Y rp Z rp = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 { r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 X tp Y tp Z tp + t x t y t z } - - - ( d ) ; ]]>

(7)求解测头姿态角：测头的姿态角用测头坐标系O-Xt，Yt，Zt相对于云台坐
标系O1-Xr，Yr，Zr的三个旋转角ψ，θ，φ表示，其中ψ为绕x轴旋转角度，θ为绕
y轴的旋转角度和φ为绕z轴的旋转角度，若令测头的坐标系O-Xt，Yt，Zt相对于
云台坐标系O1-Xr，Yr，Zr的旋转矩阵为R11，R12，……，R33，则由(d)式可以建
立下列关系：

R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 - - - ( e ) . ]]>

所述的基于大空间视觉跟踪6D测量系统的测量方法，其特征在于：所述步
骤(7)中，测头的姿态角ψ，θ，φ按(f)式所述的方法求得：

所述的基于大空间视觉跟踪6D测量系统的测量方法，其特征在于：所述靶
标球可以独立固定在待测物体上，随物体一起运动，用摄像机跟踪待测物体的姿
态和位置六个自由度，测量方法包括以下步骤：

(1)根据权利要求8的步骤(1)-(5)，求得测头坐标系和摄像机坐标系
之间旋转矩阵参数r11，r12，……，r33，测头坐标系原点和摄像机坐标系原点之
间的平移矩阵参数tx，ty，tz；

(2)求解待测物体的三个姿态角：待测物体的姿态角就是测头坐标系
O-Xt，Yt，Zt相对于云台坐标系O1-Xr，Yr，Zr的三个旋转角ψ，θ，φ表示，用公式(e)
和(f)求得；

(3)求解待测物体的位置：由于靶标球固定在待测物体上，待测物体的位
置可以用靶标球球心，即测头坐标系原点相对与云台坐标系的三个坐标
(Xr，Yr，Zr)表示：

X r Y r Z r = t x t y t z - - - ( g ) . ]]>

本发明可应用于各种工业现场，实现大尺寸空间六自由度精密测量。具有便
携、灵活、适应性好、可以实现六个自由度的测量等特点。同时该系统可以同关
节臂坐标测量机组合起来使用，实现大空间和局部无盲区高精度的测量和定位；
也可以与电钻以及其它设备结合使用，实现大型设备的精确装配，如用在大飞机
总装中的机身、机翼、尾翼、发动机等大型部件的装配，通过提供准确定位，保
证装配的精确性。

本发明的特点是利用测头靶标球表面上圆孔中心在摄像机成像面上的位置
变化来确定测头的姿态，从而实现大尺寸空间六自由度的测量；通过颜色匹配与
颜色光视场流技术实现摄像机的自动变焦和主动视觉跟踪；利用无线工业局域网
技术实现自变焦视觉跟踪摄像机和测头之间无线数据传输与远程控制。

本发明中所采用靶标球的表面上按照一定角度均匀地在经线和纬线的相交
位置布置圆柱孔。在孔里按照伪随机阵列编码的排列规则安置三种颜色标识，该
三种颜色标识可以由红、黄、蓝三种发光二极管实现，也可以用不发光的红、黄、
蓝三种物体实现。孔在表面上的位置由伪随机阵列编码颜色标识唯一确定。通过
变焦摄像机标定技术和摄像机光心及畸变的误差修正技术，可以实现大尺寸空间
六自由度的精密测量。

与已有技术相比，本发明的有益效果体现在：

1、本发明大空间视觉跟踪6D测量系统，其相对于其它的测量系统具有便携、
灵活、适应性好、可以实现六个自由度的测量等特点优势。

2、本发明大空间视觉跟踪6D测量系统，可以同关节臂坐标测量机组合使用，
实现局部高精度的测量和定位；也可以同电钻等装配设备组合使用，提供准确定
位，保证装配的精确性。

3、本发明采用的彩色伪随机编码六自由度跟踪测头，是一种新型的立体靶
六自由度测头，不仅可以实现单点坐标测量，还可以实现姿态测量，由于靶标球
上每一个彩色标识都具有唯一的编码，所以可以快速的实现图像匹配和视觉跟
踪，提高测量速率和效率。

附图说明

图1为本发明测量系统示意图。

图2为本发明六自由度测头示意图。

图3为本发明靶标球示意图。

具体实施方式

如图1、图2、图3所示。大空间视觉跟踪6D测量系统，包括有自变焦跟踪
摄像机1，自变焦跟踪摄像机1安装在三维旋转云台2上，云台上2安装了角度
码盘3和角度码盘4。还包括有独立的六自由度测头5，六自由度测头5的顶部
为靶标球6，靶标球6上按照一定角度均匀地在经线和纬线的相交位置布置圆柱
孔7。在孔里按照伪随机阵列编码的排列规则安置三种颜色标识8，该三种颜色
标识8可以由红、黄、蓝三种发光二极管实现，也可以用不发光的红、黄、蓝三
种物体实现。圆柱孔7的中心在靶标球6表面上的位置由伪随机阵列编码颜色标
识8唯一确定。六自由度测头5的底部为球探头9，通过测杆10与靶标球6相
连接，用于测量时接触被测物体实现对物体的测量。测杆上10还安置手柄11，
用于手持测头，并在内部放置无线收发器12，在云台2里还安置一个无线收发
器13.所述六自由度测头5和自变焦跟踪摄像机1之间通过无线收发器12和13
进行数据通信。

以靶标球的球心为测头坐标系的原点，建立测头坐标系O-Xt，Yt，Zt。靶球上
孔中心的位置相对于O-Xt，Yt，Zt坐标系的坐标可以通过伪随机阵列编码颜色标识
唯一确定下来，这个坐标可以设为(xti，yti，zti)；测量时所使用的球探头中心在
O-Xt，Yt，Zt坐标系下的坐标(Xtp，Ytp，Ztp)，这些坐标值都为已知量。

建立云台坐标系O1-Xr，Yr，Zr和摄像机坐标系O1-Xc，Yc，Zc，两个坐标系的原
点重合，且与摄像机的投影中心重合。云台坐标系O1-Xr，Yr，Zr的Yr坐标轴与云
台水平回转面中的零位重合，Xr轴与垂直转动轴中心线重合。测量时摄像机坐
标系O1-Xc，Yc，Zc随着云台做水平和垂直轴转动，其在水平方向和垂直方向的旋
转角度分别为α，β，通过角度码盘3和4读出。摄像机坐标系和云台坐标系之
间的关系可以用下面的公式来表示：

X c Y c Z c = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β X r Y r Z r ]]>公式1

可以得到：

X r Y r Z r = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 X c Y c Z c ]]>公式2

当需要进行测量时，移动六自由度测头，将球探头接触被测位置，自变焦视
觉跟踪摄像机，保持视场不变，自动变焦，对靶标球进行拍摄，通过对拍摄得到
的伪随机编码颜色图像进行图像处理，求得颜色标识图像中心的图像坐标
(ui，vi)，即靶标球表面上孔的中心位置在图像中的坐标。结合对应的孔中心在已
知的测头坐标系下的坐标(xti，yti，zti)，建立如下关系：

λ u i v i 1 = f u s u 0 0 f v v 0 0 0 1 r 11 r 12 r 13 t x r 21 r 22 r 23 t y r 31 r 32 r 33 t z 0 0 0 1 x ti y ti z ti 1 ]]>公式3

其中：fu，fv为摄像机像平面u，v轴上的尺度因子，s为摄像机CCD像元
排列位置的倾斜修正项，(u0，v0)为摄像机CCD图像中心的像素坐标；fu、fv、
u0、v0、s也称为摄像机内部参数。r11，r12，……，r33为测头坐标系和摄像机
坐标系之间旋转矩阵参数，tx，ty，tz为测头坐标系和摄像机坐标系之间的平移
矩阵参数，这12个参数也称为摄像机相对于测头的外部参数。

通过多点建立如上关系，解方程组可以确定所有参数。通过测头坐标系和摄
像机坐标系之间旋转矩阵参数r11，r12，……，r33和平移矩阵参数tx，ty，tz可以
建立起球探头中心(Xtp，Ytp，Ztp)在摄像机坐标系下的坐标：

X cp Y cp Z cp = r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 X tp Y tp Z tp + t x t y t z ]]>公式4

结合摄像机坐标系和云台坐标系之间的关系，可以得到球探头中心
(Xtp，Ytp，Ztp)在云台坐标系下的坐标：

X rp Y rp Z rp = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 { r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 X tp Y tp Z tp + t x t y t z } ]]>公式5

测头的姿态角用测头坐标系O-Xt，Yt，Zt相对于云台坐标系O1-Xr，Yr，Zr的三
个旋转角ψ，θ，φ表示，其中ψ为绕x轴旋转角度，θ为绕y轴的旋转角度和φ为
绕z轴的旋转角度。若令测头坐标系O-Xt，Yt，Zt相对于云台坐标系O1-Xr，Yr，Zr的
旋转矩阵为R11，R12，……，R33，则由公式5可以建立下列关系：

R 11 R 12 R 13 R 21 R 22 R 23 R 31 R 32 R 33 = cos β sin α sin β cos α sin β 0 cos α sin α - sin β sin α cos β cos α cos β - 1 r 11 r 12 r 13 r 21 r 22 r 23 r 31 r 32 r 33 ]]>公式6

则测头的姿态角ψ，θ，φ可用公式7所示的方法求得：

公式7

在进行测量的时候，让球探头接触被测点，通过公式5可以得到被测点在云
台坐标系下的3个坐标值(Xrp，Yrp，Zrp)。通过公式6-7得到测头坐标系
O-Xt，Yt，Zt相对于云台坐标系O1-Xr，Yr，Zr的姿态角ψ，θ，φ，从而实现六个自由
度的测量。

靶标球可以独立固定在待测物体上，随物体一起运动，用摄像机跟踪测得物
体的姿态和位置六个自由度。测量方法为：

根据公式1至公式4，求得测头坐标系和摄像机坐标系之间旋转矩阵参数r11，
r12，……，r33，测头坐标系和摄像机坐标系之间的平移矩阵参数tx，ty，tz。待
测物体的姿态角就是测头坐标系O-Xt，Yt，Zt相对于云台坐标系O1-Xr，Yr，Zr的三
个旋转角ψ，θ，φ表示，用公式7求得。

待测物体的位置用靶标球球心(即测头坐标系原点)相对与云台坐标系的三
个坐标(Xr，Yr，Zr)表示，即：

X r Y r Z r = t x t y t z ]]>公式8

在实施中通过颜色匹配与颜色光视场流技术可以实现摄像机的自动变焦和
主动视觉跟踪。由于靶标球的直径是已知的，根据摄像机光学成像原理，可以求
出摄像机在一定视场下焦距的大小，通过外围电路设计实现自动变焦，使摄像机
的视场不随距离而改变，保证了测量的精度；同时利用摄像机拍摄图像中靶标球
编码图案可以确定靶标球表面上孔中心的坐标，利用前后靶标球的位移信息可以
实现主动的视觉跟踪。

为保证系统的正常工作，利用无线工业局域网技术实现摄像机和靶标之间的
通讯功能。