CN1262816C - 刚体空间位姿测量装置及其测量方法 - Google Patents

Abstract

刚体空间位姿测量装置及其测量方法，属于测控技术领域。为克服传统位姿测量装置的不足，本发明提出了一种接触式的低成本、高精度的刚体空间位姿测量装置，包括测量执行机构、数据采集装置以及存储计算程序的计算机；所述测量执行机构包括固定平台及固定在固定平台上的6个球铰，与运动刚体固连的运动平台及固定在运动平台上的6个球铰，两端分别与固定平台球铰和运动平台球铰相连的六个拉线式编码器。本发明还公开了一种刚体空间位姿测量方法，该方法根据上述测量装置获取的初始数据，利用迭代方程组得到被测刚体的空间位姿信息。本发明所述装置结构简单，系统实时性较高，能实现对被测刚体的全闭环实时控制。

Description

刚体空间位姿测量装置及其测量方法

技术领域

本发明属于测控技术领域，涉及一种用于测量刚体空间六自由度位姿的方法及实现装置。

背景技术

在笛卡尔坐标系中，刚体具有六个自由度。在机床、机构、机器人等的工业控制中，为了实现高精度的定位，通常控制会采用闭环反馈控制，而这就对空间位姿的动态测量提出了要求。目前采用的闭环反馈控制中，测量主要有两种方式：测量执行机构关节的位移(或角位移)进行反馈或直接测量终端的位姿进行反馈。

在测量执行机构关节位移(或角位移)进行反馈的方案中，又可分为半闭环反馈与全闭环反馈两种方法。半闭环反馈仅测量电机或丝杠的位移(或角位移)，忽略之后的传动系统误差，方案的优点是成本低，便于实现，缺点是忽略了机械传动系统的误差，精度低；全闭环反馈需测量实际机构关节的位移(或角位移)，常用的仪器是光栅尺等，这一方案的优点是精度高，缺点是造价高，对环境有一定的要求。上述两种方案存在的一个共同问题是，当被测刚体位姿不能全部可控时，这种测量方案无法完成测量要求。

直接测量终端位姿的方法，通常是采用光学测量仪器进行非接触式测量，如采用三台光学仪器测量刚体上三个点的位置信息即可得到刚体的六自由度位姿信息。但是普通的光学测量设备实时性差，而可以满足实时性要求时的光学测量设备，成本又成几十倍的增加。另外光学测量仪器对环境以及被测刚体的空间有一定的要求。

Stewart平台机构是最适宜用于实现空间六自由度运动的并联机构，它出现于二十世纪六十年代，通用Stewart平台由一个固定平台、一个运动平台和联接于固定平台和运动平台之间的六个执行器组成。通过驱动六个执行器，使运动平台实现空间六自由度运动。目前的Stewart平台机构都是用于主动控制中。

发明内容

本发明的目的在于克服传统位置及姿态测量装置的不足之处，提出一种接触式的低成本、高精度的刚体空间位置和姿态的测量装置及测量方法。本发明采用拉线式编码器取代通用Stewart平台机构中的六个执行器，使主动控制变为被动测量，从而获得被测运动刚体的位姿信息。

本发明公开的刚体空间位姿测量装置，其特征在于：该测量装置包括测量执行机构、数据采集装置以及存储计算程序的计算机；所述测量执行机构包括一个固定平台及固定在所述固定平台上的6个球铰，一个与运动刚体固连的运动平台及固定在所述运动平台上的6个球铰，两端分别与固定平台上的6个球铰和运动平台上的6个球铰相连的六个拉线式编码器；所述数据采集装置包括分别与拉线式编码器输出端相连的六个计数器，与所述各计数器输出端连接的多路复用器，以及与所述多路复用器相连的数据发送电路；所述数据发送电路与所述计算机相应输入端口连接；

所述各球铰在各自平台的位置坐标满足测量执行机构雅克比矩阵J_p的条件数小于等于10，

$J_p=\left[\begin{array}{cc}{\stackrel{\→}{l}}_1^T& \frac{{({\mathrm{Rp}}_1\×{\stackrel{\→}{l}}_1)}^T}{\left|p_1\right|}\\ \·& \·\\ \·& \·\\ \·& \·\\ {\stackrel{\→}{l}}_6^T& \frac{{({\mathrm{Rp}}_6\×{\stackrel{\→}{l}}_6)}^T}{\left|p_6\right|}\end{array}\right]\∈R^{6\×6},$

式中 分别是沿每个拉线式编码器长度方向上的单位向量，P₁～P₆是运动平台上的6个球铰在运动平台坐标系p中的坐标，R为运动平台坐标系p关于固定平台坐标系P的方向余弦阵，按照依次绕固定平台坐标系P的x，y，z轴旋转角度α，β，γ的卡尔丹角表示方法，具体表达式为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\γ}& -\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\β}\\ -\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right].$

本发明的一种改进为所述的固定在运动平台上的6个球铰直接固定在运动刚体上，省去所述运动平台。

本发明所述拉线式编码器是数字式拉线式编码器。

本发明的一种改进为所述拉线式编码器是模拟式拉线式编码器，该刚体空间位姿测量装置还包括一个A/D采集卡，所述A/D采集卡的输入端与所述模拟式拉线式编码器的输出端相连，输出端分别与六个计数器相连。

本发明还公开了一种所述测量装置进行刚体空间位姿测量的方法，其特征在于：该测量方法包括如下步骤：

1)系统初始化，即确定各球铰在各自平台坐标系中的坐标，拉线式编码器的初始长度以及被测刚体在固定平台坐标系中的初始坐标，并输入所述计算机；

2)当被测刚体运动时，利用数据采集装置采集拉线式编码器的长度变化信号，并传送到计算机中；

3)以被测刚体在固定平台坐标系中的初始坐标为方程组初始值，计算机中的程序求解下述迭代方程组，得到被测刚体的空间姿态信息及位置信息：

L_i-|R·p_i+T-P_i|＝0，i＝1，…，6；

其中，L₁～L₆为6个拉线式编码器得到的测量长度，P_i是固定平台上的6个球铰在固定平台坐标系P中的坐标，p_i是运动平台上的6个球铰在运动平台坐标系p中的坐标，

R为运动平台坐标系p关于固定平台坐标系P的方向余弦阵，包含了待求的空间姿态信息，T＝[x y z]^T是运动平台坐标系p的原点在固定平台坐标系P中的位置，包含了待求的空间位置信息；

4)将当前运动刚体的位姿作为迭代方程组的初始值，实现连续测量。

本发明相比已有的技术具有如下的优点：

第一，本发明装置采用Stewart机构的连接方式，实现结构简单。

第二，本装置直接测量被测刚体的位姿，同时系统实时性较高，所以能够实现对被测刚体的全闭环实时控制。

第三，拉线式编码器成本低，使用可靠，精度可达1微米，能满足一般精度的测量要求。

第四，通过合理的球铰位置选择，使机构对姿态比较敏感，测量精度得到提高。

第五，测量时操作简单、快捷，对环境及被测刚体的空间要求不高，适用于多种场合。

附图说明

图1是本发明所述的刚体空间位姿测量装置的系统构成示意图。

图2是刚体空间位姿测量方法的流程图。

图3a是本发明的一个实施例的结构原理框图。

图3b是本发明的另一个实施例的结构原理框图。

具体实施方式

结合附图具体描述本发明所述的运动刚体位姿的测量方法与测量装置。

如图1所述，本发明所述刚体空间位姿测量装置包括测量执行机构、数据采集装置以及存储计算程序的计算机；所述测量执行机构包括固定平台1，安装在固定平台上的六个球铰101～106；运动平台2，安装在运动平台上的六个球铰201～206；相应连接球铰101～106与201～206的拉线式编码器301～306；数据采集装置4的输入端与拉线式编码器的输出端连接，数据采集装置4的输出端与计算机5的相应输入端口连接。

固定平台1，用来安装六个球铰101～106，使球铰位置固定，在固定平台上建立一个固定不变的固定平台坐标系，精确测定球铰101～106在固定平台坐标系中的坐标。在具体实施例中，固定平台1的具体形式可为平板也可为框架结构，根据被测刚体的运动范围及角度，固定平台可安装在运动刚体的上方或者下方。

运动平台2，用来安装六个球铰201～206，同时另一端与所需测量的运动刚体固连。在运动平台上建立一个运动平台坐标系，精确测定球铰201～206在运动平台坐标系中的坐标。当所需测量的刚体上比较容易安装球铰时，可省略掉运动平台2，直接将六个球铰201～206安装在所需测量的刚体上，并在被测刚体上建立运动平台坐标系。本实施例中由于被测刚体面积较小不易安装球铰，故采用运动平台2。

各球铰在各自平台的位置应使被测刚体在运动范围内时，测量执行机构的雅克比矩阵条件数满足要求。雅克比矩阵表示运动平台的位姿误差量和支链长度误差量间的映射关系，其条件数反映了所设计的机构在空间中各个方向上精度的比例，是衡量测量精度的一个重要指标。根据研究发现，当雅克比矩阵的条件数大于10时，最终的测量精度将急剧下降，说明球铰坐标设计不合理，需重新设计球铰位置。本发明所述刚体空间位姿测量装置中的测量执行机构的雅克比矩阵的条件数小于等于10，优选在4～5以内。雅克比矩阵的具体表达式为：

$J_p=\left[\begin{array}{cc}{\stackrel{\→}{l}}_1^T& \frac{{({\mathrm{Rp}}_1\×{\stackrel{\→}{l}}_1)}^T}{\left|p_1\right|}\\ \·& \·\\ \·& \·\\ \·& \·\\ {\stackrel{\→}{l}}_6^T& \frac{{({\mathrm{Rp}}_6\×{\stackrel{\→}{l}}_6)}^T}{\left|p_6\right|}\end{array}\right]\∈R^{6\×6},$

式中 分别是沿每个拉线式编码器长度方向上的单位向量，p₁～p₆是运动平台上的6个球铰在运动平台坐标系p中的坐标，R为运动平台坐标系p关于固定平台坐标系P的方向余弦阵，按照依次绕固定平台坐标系P的x，y，z轴旋转角度α，β，γ的卡尔丹角表示方法，具体表达式为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\γ}& -\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\β}\\ -\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right].$

拉线式编码器301～306，两端分别与固定平台球铰和运动平台球铰相连，当被测刚体运动时通过编码器记录下对应球铰间长度变化的情况。数据采集装置4将拉线式编码器301～306的数据转换成计算机可以接收的形式，统一将数据传输至存储有位姿解算程序的计算机5中。

拉线式编码器在输出数据方式上可为数字式或模拟式。当采用数字式的拉线式编码器时，见图3a，数据采集装置4包括六个记录编码器变化量的计数器，将六路信号汇合成一路信号的多路复用器以及与多路复用器相连的将汇合后的信号发送给计算机的数据发送电路。数字式拉线式编码器输出端与六个计数器相连，各计数器输出端与多路复用器连接，数据发送电路与所述计算机相应输入端口连接。当采用模拟式拉线式编码器时，见图3b，该刚体空间位姿测量装置还包括一个将模拟信号转换为数字信号的A/D采集卡，所述A/D采集卡的输入端与所述模拟式拉线式编码器的输出端相连，输出端分别与数据采集装置4中的六个计数器相连。

拉线式编码器除输出方式外，在计数方面具体可采用增量式或绝对式，电路特性上，可选择电压型、电流型等，在具体实施例中，可选用封装完整的成品拉线式编码器以及多路复用器，如HONTKO公司的数字增量式电压型拉线式编码器HLS-M-1005，74151多路复用器等。数据采集装置的数据发送电路可选用通用的串口发送电路，并口发送电路等计算机可接收的方式。

存储了位姿解算算法的计算机5，将拉线式编码器的数据以及球铰101～106在固定平台坐标系中的坐标和201～206球铰在运动平台坐标系中的坐标结合起来，通过求解机构的运动学方程得到运动平台相对固定平台的位置和姿态。

如图2所示。本发明所述的刚体空间位姿测量方法的流程如下：

(1)系统初始化：这部分主要对后续测量和计算中应用到的参数进行确定，其中包括球铰101～106以及201～206分别在固定平台坐标系和运动平台坐标系中的坐标，拉线式编码器的初始长度以及用于迭代求解方程的被测刚体初始情况下在固定平台坐标系中的坐标，并将所述坐标输入计算机；

(2)将拉线式编码器测量的数据通过数据采集装置传输到计算机中。如果拉线式编码器，为增量式拉线式编码器，则需要将拉线式编码器中的长度变化量与拉线式编码器的初始长度相加得到各组球铰间的距离。

(3)求解如下迭代方程组得到运动刚体的空间位置和空间姿态。

L_i-|R·p_i+T-P_i|＝0，i＝1，…，6

式中T＝[x y z]^T是坐标系p的原点在坐标系P中的位置，可以得到运动刚体的空间位置信息；R为运动平台坐标系p关于固定平台坐标系P的方向余弦阵，按照依次绕固定平台坐标系P的x，y，z轴旋转有限角度α，β，γ的卡尔丹角表示方法，具体表达式为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\γ}& -\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\β}\\ -\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right]$

L₁～L₆为6个拉线式编码器得到的测量长度。P_i是固定平台上的六个球铰在固定平台坐标系P中的坐标。p_i是运动平台上的六个球铰在运动平台坐标系p中的坐标。

在具体的实施中，上述迭代方程组可采用牛顿-拉斐逊法求解。下面为了表述方便，将本迭代求解算法公式阐述如下：

采用牛顿-拉斐逊法求解Stewart机构运动学正解的方法。

机构的运动学正解方程可表示为：

f_i(x，y，z，α，β，γ)＝L_i-|R·p_i+T-P_i|＝0  i＝1，2，...6

该方程组为含有6个未知数的6个非线性的方程。令X＝(x，y，z，α，β，γ)，F(X)＝[f₁(X)，f₂(X)，f₃(X)，f₄(X)，f₅(X)，f₆(X)]^T。X^*为方程组的解。牛顿-拉斐逊法为一迭代算法。将方程Taylor展开，得到：

F(X+ΔX)＝F(X)+JδX+O(δX²)

式中J为方程的雅克比矩阵， $J_{\mathrm{ij}}=\frac{\∂f_i}{\∂X_j}.$ 忽略方程的二阶量，并令F(X+ΔX)＝0有：

JδX＝-F(X)

此为一线性方程组，解为δX＝J^-1F(X)。因此其迭代过程为：X_new＝X_old+δX。

(4)将当前运动刚体的位姿替代迭代方程的初始值，为下一次测量做准备，实现连续测量。

Claims (5)

1.刚体空间位姿测量装置，其特征在于：该测量装置包括测量执行机构、数据采集装置以及存储计算程序的计算机；所述测量执行机构包括一个固定平台及固定在所述固定平台上的6个球铰，一个与运动刚体固连的运动平台及固定在所述运动平台上的6个球铰，两端分别与固定平台上的6个球铰和运动平台上的6个球铰相连的六个拉线式编码器；所述数据采集装置包括分别与拉线式编码器输出端相连的六个计数器，与所述各计数器输出端连接的多路复用器，以及与所述多路复用器相连的数据发送电路；所述数据发送电路与所述计算机相应输入端口连接；

所述各球铰在各自平台的位置坐标满足测量执行机构雅克比矩阵J_p的条件数小于等于10，

$J_p=\left[\begin{array}{cc}{\stackrel{\→}{l}}_1^T& \frac{{({\mathrm{Rp}}_1\×{\stackrel{\→}{l}}_1)}^T}{\left|p_1\right|}\\ \·& \·\\ \·& \·\\ \·& \·\\ {\stackrel{\→}{l}}_6^T& \frac{{({\mathrm{Rp}}_6\×{\stackrel{\→}{l}}_6)}^T}{\left|p_6\right|}\end{array}\right]\∈R^{6\×6},$

式中

分别是沿每个拉线式编码器长度方向上的单位向量，p₁～p₆是运动平台上的6个球铰在运动平台坐标系p中的坐标，R为运动平台坐标系p关于固定平台坐标系P的方向余弦阵，按照依次绕固定平台坐标系P的x，y，z轴旋转角度α，β，γ的卡尔丹角表示方法，具体表达式为：

$R=\left[\begin{array}{ccc}\cos \mathrm{\β}\cos \mathrm{\γ}& -\cos \mathrm{\β}\sin \mathrm{\γ}& \sin \mathrm{\β}\\ \sin \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\cos \mathrm{\α}\sin \mathrm{\γ}& -\sin \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\cos \mathrm{\α}\cos \mathrm{\γ}& -\sin \mathrm{\α}\cos \mathrm{\β}\\ -\cos \mathrm{\α}\sin \mathrm{\β}\cos \mathrm{\γ}+\sin \mathrm{\α}\sin \mathrm{\γ}& \cos \mathrm{\α}\sin \mathrm{\β}\sin \mathrm{\γ}+\sin \mathrm{\α}\cos \mathrm{\γ}& \cos \mathrm{\α}\cos \mathrm{\β}\end{array}\right].$

2.根据权利要求1所述的刚体空间位姿测量装置，其特征在于：所述的固定在运动平台上的6个球铰直接固定在运动刚体上，省去所述运动平台。

3.根据权利要求1或2所述的刚体空间位姿测量装置，其特征在于：所述拉线式编码器是数字式拉线式编码器。

4.根据权利要求1或2所述的刚体空间位姿测量装置，其特征在于：所述拉线式编码器是模拟式拉线式编码器，该刚体空间位姿测量装置还包括一个A/D采集卡，所述A/D采集卡的输入端与所述模拟式拉线式编码器的输出端相连，A/D采集卡的输出端分别与六个计数器相连。

5.利用权利要求1所述测量装置进行刚体空间位姿测量的方法，其特征在于：该测量方法包括如下步骤：

1)系统初始化，即确定各球铰在各自平台坐标系中的坐标，拉线式编码器的初始长度以及被测刚体在固定平台坐标系中的初始坐标，并输入所述计算机；

2)当被测刚体运动时，利用数据采集装置采集拉线式编码器的长度变化信号，并传送到计算机中；

3)以被测刚体在固定平台坐标系中的初始坐标为方程组初始值，计算机中的程序求解下述迭代方程组，得到被测刚体的空间姿态信息及位置信息：

L_i-｜R·p_i+T-P_i｜＝0，i＝1，…，6；

其中，L₁～L₆为6个拉线式编码器得到的测量长度，P_i是固定平台上的6个球铰在固定平台坐标系P中的坐标，P_i是运动平台上的6个球铰在运动平台坐标系P中的坐标，

R为运动平台坐标系p关于固定平台坐标系P的方向余弦阵，包含了待求的空间姿态信息，T＝[x y z]^T是运动平台坐标系p的原点在固定平台坐标系P中的位置，包含了待求的空间位置信息；

4)将当前运动刚体的位姿作为迭代方程组的初始值，实现连续测量。

Images