CN109454472B - 一种空间多自由度定位装置及其空间位置解算方法 - Google Patents

Abstract

本发明公开了一种空间多自由度定位装置及其空间位置解算方法。所述装置包括底板平台、第一三维转台、第二三维转台、第一转台底座、第二转台底座、伺服电机、丝杠和导轨，导轨铺设在底板平台上，第一三维转台安设在第一转台底座上，第一转台底座固定在底板平台的一端，第二三维转台安设在第二转台底座上，第二转台底座可以沿导轨做靠近或远离第一转台底座的运动，两个目标物体分别固定在第一三维转台和第二三维转台上。所述空间位置解算方法根据本发明装置的机械结构特点，通过固定在夹具上两物体间期望的空间位置姿态关系解算出装置上每个运动部件的运动量。本发明能够实现空间中两个物体之间的复杂相对位置姿态的定位。

Description

一种空间多自由度定位装置及其空间位置解算方法

技术领域

本发明属于部件空间定位领域，特别涉及了一种空间多自由度定位装置及其空间位置解算方法。

背景技术

现代工业、国防及航空航天等领域中某些场合需要模拟出两个物体间的空间位置姿态关系，以方便测量、定位、组装和设计等。

在宇宙空间中航天飞行器之间经常要做出特定的相对位置姿态以完成某项工作，例如两个卫星之间的对接、空间站中伸出的机械臂需要抓住远处的某个物体、两个飞船要以特定的姿态与转角做相对运动等，这些在前期都需要大量的模拟。由于科技水平的限制，目前人类不可能前往太空做实验，因此在地面实验室就需要有可以模拟飞行器与飞行器之间空间位置姿态关系的设备。

另外一些工业应用，如雷达、导弹跟踪做俯仰、偏航、翻滚运动的飞行器、一些发动机管路系统设计等，都需要能够做出空间多自由度运动的设备进行辅助设计。

现有的定位装置一般用于水管、燃气管道的安装，这类管道的管接头一般不需要在空间中有复杂的位置姿态关系，因此上述定位装置实现不了模拟三维空间内了两个物体之间的位置姿态，不适用于上述应用背景。

发明内容

为了解决上述背景技术提出的技术问题，本发明旨在提供一种空间多自由度定位装置及其空间位置解算方法，实现空间中两个物体之间的复杂相对位置姿态的定位。

为了实现上述技术目的，本发明的技术方案为：

一种空间多自由度定位装置，包括底板平台、第一三维转台、第二三维转台、第一转台底座、第二转台底座、伺服电机、丝杠和导轨，所述导轨铺设在底板平台上，第一三维转台安设在第一转台底座上，第一转台底座固定在底板平台的一端，第二三维转台安设在第二转台底座上，所述伺服电机设置在第一转台底座的内部，丝杠的顶端和底端通过支撑座固定在底板平台上，且丝杠与导轨平行，丝杠的顶端通过联轴器与伺服电机轴相连，丝杠上设有螺母座，当丝杠旋转时，螺母座沿丝杠移动，第二转台底座固定在螺母座上，导轨上设有能沿导轨滑动的滑块，且该滑块安装在第二转台底座底部，使得第二转台底座能够在导轨上滑动，在伺服电机带动下，第二转台底座带着第二三维转台沿着导轨滑动，两个目标物体分别固定在第一三维转台和第二三维转台上。

进一步地，该装置还包括第一夹具、第一夹具底座、第二夹具和第二夹具底座，所述第一夹具底座和第二夹具底座分别安设在第一三维转台和第二三维转台的中央，第一夹具和第二夹具分别安设在第一夹具底座和第二夹具底座上，两个目标物体分别固定在第一夹具和第二夹具上。

进一步地，第一三维转台和第二三维转台均能实现俯仰、偏航和翻滚三个自由度的运动，通过伺服电机和丝杠能够控制第一三维转台和第二三维转台之间的距离，从而实现七个自由度的运动。

基于上述空间多自由度定位装置的空间位置解算方法，为每个目标部件标定X、Y和Z轴，设两个目标部件处的坐标系分别为{A}和{B}，这两个坐标系间的坐标变换矩阵为^AT_B，这是一个给定的4×4矩阵，表示空间两个物体的相对位置和姿态；设第一三维转台和第二三维转台的回转中心处的坐标系分别为{A’}和{B’}，则这两个坐标系中的X、Y和Z三个坐标轴即对应三维转台的三个转轴，三维转台可分别绕这三个轴做俯仰、偏航和翻滚运动；设{A’}坐标系的Z轴、Y轴、X轴的转角分别为α1、β1、γ1，即第一三维转台的俯仰、偏航和翻滚的角度，{B’}坐标系的Z轴、Y轴、X轴的转角分别为α2、β2、γ2，即第二三维转台的俯仰、偏航和翻滚的角度，{A’}、{B’}两坐标系间距离为L，即两套三维转台中心处的距离，设两套三维转台处于零位时的基坐标系分别为{A”}和{B”}，la和lb分别为零位置时{A}相对{A”}的偏移距离和{B}相对于{B”}的偏移距离；

设{B”}的原点为P，即P点在{B”}内的坐标为^B”P＝[0 0 0 0]^T，则

^BP＝^BT_B' ^B'P＝^BT_B'[R_Z(α2)R_Y(β2)R_X(γ2)]^-1B”P＝[lb 0 0 0]^T

其中，^BP、^B'P分别为P点在{B}、{B’}内的坐标，^BT_B'为{B}和{B’}之间的坐标变换矩阵，R_Z(*)、R_Y(*)和R_X(*)分别为绕Z、Y和X轴旋转的变换矩阵；引入中间变量m、n、k：

根据得^A”P＝^A”T_B” ^B”P＝[L 0 0 1]^T，

而

其中，^AP、^A'P、^A”P分别为P点在{A}、{A’}、{A”}内的坐标，^A”T_B”表示{A”}与{B”}之间的坐标变换矩阵；

联立得方程组：

k(sinα1sinγ1+cosα1cosγ1sinβ1)-n(cosγ1sinα1-cosα1sinβ1sinγ1)+cosα1cosβ1la+cosα1cosβ1m＝L (1)

n(cosα1cosγ1+sinα1sinβ1sinγ1)-k(cosα1sinγ1-sinβ1cosγ1sinα1)+cosβ1sinα1la+cosβ1sinα1m＝0 (2)

kcosβ1cosγ1-msinβ1-lasinβ1+ncosβ1sinγ1＝0 (3)

预先给定α1、β1、γ1、L这4个变量中的其中一个变量的值，代入上述方程组，得到方程组的解，即其余3个变量的值；

然后将α1、β1、γ1、L代入以下方程，解出α2、β2、γ2：

(^A”T_B”)^-1R_Z(α1)R_Y(β1)R_X(γ1)^A'T_A ^AT_B(^B'T_B)^-1＝R_Z(α2)R_Y(β2)R_X(γ2) (4)

其中，^A'T_A表示{A}和{A’}之间的坐标变换矩阵；至此7个自由度变量全部解出。

进一步地，在α1、β1、γ1、L这4个变量中预先给定γ1的值，代入方程(1)-(4)，从而求解出另外6个变量的值；根据装置结构知L≥la+lb，当方程组解得|L|＜la+lb时，重新给定γ1的值求解方程组，当方程组解得L＜-(la+lb)时，令α1＝α1+180°，再重新计算L，使之最终满足L≥la+lb的条件。

采用上述技术方案带来的有益效果：

本发明提出的空间多自由度定位装置的结构，其中每套三维转台可以做出独立的俯仰、偏航和翻滚三个自由度的运动，而伺服电机可以牵引移动转台在底板平台上沿导轨做水平运动，这就可以改变两个三维转台间的距离，于是整套装置可以做出空间七个自由度的运动，因此由两套三维转台上夹具固定的两个物体之间可以在空间中做出任意的相对位置姿态。通过本发明提出的空间位置解算方法来控制装置，可以保证两个物体之间期望的相对位置姿态经过装置的一次性运动形成。

附图说明

图1为空间多自由度定位装置的结构示意图；

图2为空间多自由度定位装置的零位示意图；

图3为空间多自由度定位装置的坐标系标定示意图；

图4为空间位置解算方法流程图；

图5为空间多自由度定位装置执行某一工序后的示意图。

具体实施方式

以下将结合附图，对本发明的技术方案进行详细说明。

本发明设计的空间多自由度定位装置，如图1所示，包括底板平台1；第一三维转台201、第二三维转台202；第一转台底座301、第二转台底座302；丝杠4；伺服电机5；第一夹具601、第二夹具602；第一夹具底座701、第二夹具底座702；导轨8。

空间多自由度定位装置的初始状态如图2所示，本发明中将这种状态称作零位状态，此时两套三维转台的状态如110和120所示，两套三维转台各个轴的运动量为0。两个夹具底座处于水平状态，并且两个夹具水平贴靠在一起，如400所示。

在定位装置上建立如图3所示的坐标系，在上位机的人机交互界面中输入给定的代表两个目标物体空间位置姿态关系的矩阵^AT_B以及初始角度γ1，主控制器会依照根据图4所示的位置解算算法编写的程序计算出一组合适的α1，β1，α2，β2，γ2，L，电气控制系统会将包括初始角度γ1在内的7个自由度运动量转化为控制信号发送给本装置设备的各个运动部件，即三维转台和伺服电机。

如图4所示，本算法为每个目标部件标定X、Y和Z轴，设两个目标部件处的坐标系分别为{A}和{B}，两坐标系间的坐标变换矩阵为^AT_B，这是一个给定的4×4矩阵，表示空间两个物体的相对位置和姿态。设固定转台和移动转台的回转中心处的坐标系分别为{A’}和{B’}，假设空间中的一点J在坐标系{A}、{A’}、{B}和{B’}中的坐标分别为^AJ、^A'J、^BJ和^B'J。因为坐标系{A’}、{B’}标定在三维转台的回转中心处，所以坐标系中的X、Y和Z三个坐标轴即对应三维转台的三个转轴，三维转台可分别绕这三个轴做俯仰、偏航和翻滚运动。设{A’}坐标系的Z轴、Y轴、X轴的转角分别为α1、β1、γ1，即固定转台俯仰、偏航和翻滚的角度；{B’}坐标系的Z轴、Y轴、X轴的转角分别为α2、β2、γ2，即移动转台俯仰、偏航和翻滚的角度。{A’}、{B’}两坐标系间距离为L，即两个转台中心间的距离。设两套转台处于零位时的基坐标系分别为{A”}和{B”}，la和lb分别为零位置时{A}相对{A”}的偏移距离和{B}相对于{B”}的偏移距离。则可得

^A'J＝^A'T_A ^AJ

^B'J＝^B'T_B ^BJ

^A”J＝R_Z(α1)R_Y(β1)R_X(γ1)^A'J

^B”J＝R_Z(α2)R_Y(β2)R_X(γ2)^B'J

其中R_Z(*)、R_Y(*)和R_X(*)分别为绕Z、Y和X轴旋转的变换矩阵，联立上式可得

R_Z(α1)R_Y(β1)R_X(γ1)^A'T_A ^AT_B＝^A”T_B”R_Z(α2)R_Y(β2)R_X(γ2)^B'T_B

求取上式中的七个变量是运动学逆解问题。

运动学逆解是一个非线性问题，本发明提出的算法的权利要求在于根据本装置机构特点进行空间位置解算，特征如下：

设{B”}的原点为P，即P点坐标为^B”P＝[0 0 0 0]^T，则

^BP＝^BT_B' ^B'P＝^BT_B'[R_Z(α2)R_Y(β2)R_X(γ2)]^-1B”P＝[lb 0 0 0]^T

引入中间变量：

由可知，^A”P＝^A”T_B” ^B”P＝[L 0 0 1]^T，而

可以联立得方程组

k(sinα1sinγ1+cosα1cosγ1sinβ1)-n(cosγ1sinα1-cosα1sinβ1sinγ1)+cosα1cosβ1la+cosα1cosβ1m＝L (1)

n(cosα1cosγ1+sinα1sinβ1sinγ1)-k(cosα1sinγ1-sinβ1cosγ1sinα1)+cosβ1sinα1la+cosβ1sinα1m＝0 (2)

kcosβ1cosγ1-msinβ1-lasinβ1+ncosβ1sinγ1＝0 (3)

理论上而言，上述方程组的解[α1 β1 γ1 α2 β2 γ2 L]^T存在无数种，利用它们都可使得机械装置模拟出正确的两物体间的位置姿态关系，但往往根据实际情况需要确定一个合理的定解。当7个变量中的某一个变量已知，其余6个变量随即确定，因此需要根据实际情况给定一个初始值，本发明中选择γ1作为已知初始值。

由方程(3)即可计算β1，将β1代入方程(2)可计算出α1，再将α1和β1代入方程(1)计算出L。由本装置的机械结构特点可知必有L≥la+lb，当方程组解得|L|＜la+lb时，重新给定γ1的值求解方程组，当方程组解得L＜-(la+lb)时，令α1＝α1+180°，再重新计算L，使之最终满足L≥la+lb的条件。

将α1、β1、γ1、L代入方程

(^A”T_B”)^-1R_Z(α1)R_Y(β1)R_X(γ1)^A'T_A ^AT_B(^B'T_B)^-1＝R_Z(α2)R_Y(β2)R_X(γ2)

可以解出α2、β2、γ2。

根据上位机传送的6个角度旋转量，本装置中的两个三维转台201、202可以独立做俯仰、偏航、翻滚的运动；安放在固定转台底座301的伺服电机5通过丝杠4牵引固连着移动转台202的移动转台底座302沿导轨8在底板平台1上做水平移动，此时根据图4的算法流程可知，伺服电机只能牵引移动转台向着远离固定转台的方向移动，因此算法起到了保护机械设备防止两个转台碰撞的作用。

此处的七个自由度的运动是同时启动的，当最后一个轴的运动结束后，整个装置停止运动，运动后的装置示意图如图5所示，此时安装在三维转台上夹具中的物体将摆出一定的空间位置姿态，这就是期望的两个物体间的空间位置姿态。可以看出，两个物体之间正确的相对位置姿态经过装置的一次性运动形成。

实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。

Claims (4)

1.基于空间多自由度定位装置的空间位置解算方法，其特征在于：所述空间多自由度定位装置包括底板平台、第一三维转台、第二三维转台、第一转台底座、第二转台底座、伺服电机、丝杠和导轨，所述导轨铺设在底板平台上，第一三维转台安设在第一转台底座上，第一转台底座固定在底板平台的一端，第二三维转台安设在第二转台底座上，所述伺服电机设置在第一转台底座的内部，丝杠的顶端和底端通过支撑座固定在底板平台上，且丝杠与导轨平行，丝杠的顶端通过联轴器与伺服电机轴相连，丝杠上设有螺母座，当丝杠旋转时，螺母座沿丝杠移动，第二转台底座固定在螺母座上，导轨上设有能沿导轨滑动的滑块，且该滑块安装在第二转台底座底部，使得第二转台底座能够在导轨上滑动，在伺服电机带动下，第二转台底座带着第二三维转台沿着导轨滑动，两个目标物体分别固定在第一三维转台和第二三维转台上；

所述空间位置解算方法如下：

为每个目标部件标定X、Y和Z轴，设两个目标部件处的坐标系分别为{A}和{B}，这两个坐标系间的坐标变换矩阵为^AT_B，这是一个给定的4×4矩阵，表示空间两个物体的相对位置和姿态；设第一三维转台和第二三维转台的回转中心处的坐标系分别为{A’}和{B’}，则这两个坐标系中的X、Y和Z三个坐标轴即对应三维转台的三个转轴，三维转台可分别绕这三个轴做俯仰、偏航和翻滚运动；设{A’}坐标系的Z轴、Y轴、X轴的转角分别为α1、β1、γ1，即第一三维转台的俯仰、偏航和翻滚的角度，{B’}坐标系的Z轴、Y轴、X轴的转角分别为α2、β2、γ2，即第二三维转台的俯仰、偏航和翻滚的角度，{A’}、{B’}两坐标系间距离为L，即两套三维转台中心处的距离，设两套三维转台处于零位时的基坐标系分别为{A”}和{B”}，la和lb分别为零位置时{A}相对{A”}的偏移距离和{B}相对于{B”}的偏移距离；

设{B”}的原点为P，即P点在{B”}内的坐标为^B”P＝[0 0 0 0]^T，则

^BP＝^BT_B' ^B'P＝^BT_B'[R_Z(α2)R_Y(β2)R_X(γ2)]^-1B”P＝[lb 0 0 0]^T

其中，^BP、^B'P分别为P点在{B}、{B’}内的坐标，^BT_B'为{B}和{B’}之间的坐标变换矩阵，R_Z(*)、R_Y(*)和R_X(*)分别为绕Z、Y和X轴旋转的变换矩阵；引入中间变量m、n、k：

根据得^A”P＝^A”T_B” ^B”P＝[L 0 0 1]^T，

而

其中，^AP、^A'P、^A”P分别为P点在{A}、{A’}、{A”}内的坐标，^A”T_B”表示{A”}与{B”}之间的坐标变换矩阵；

联立得方程组：

k(sinα1sinγ1+cosα1cosγ1sinβ1)-n(cosγ1sinα1-cosα1sinβ1sinγ1)+cosα1cosβ1la+cosα1cosβ1m＝L(1)

n(cosα1cosγ1+sinα1sinβ1sinγ1)-k(cosα1sinγ1-sinβ1cosγ1sinα1)+cosβ1sinα1la+cosβ1sinα1m＝0 (2)

kcosβ1cosγ1-msinβ1-lasinβ1+ncosβ1sinγ1＝0 (3)

预先给定α1、β1、γ1、L这4个变量中的其中一个变量的值，代入上述方程组，得到方程组的解，即其余3个变量的值；

然后将α1、β1、γ1、L代入以下方程，解出α2、β2、γ2：

(^A”T_B”)^-1R_Z(α1)R_Y(β1)R_X(γ1)^A'T_A ^AT_B(^B'T_B)^-1＝R_Z(α2)R_Y(β2)R_X(γ2) (4)

其中，^A'T_A表示{A}和{A’}之间的坐标变换矩阵；至此，空间多自由度定位装置的7个自由度变量全部解出。

2.根据权利要求1所述空间位置解算方法，其特征在于：在α1、β1、γ1、L这4个变量中预先给定γ1的值，代入方程(1)-(4)，从而求解出另外6个变量的值；根据装置结构知L≥la+lb，当方程组解得|L|＜la+lb时，重新给定γ1的值求解方程组，当方程组解得L＜-(la+lb)时，令α1＝α1+180°，再重新计算L，使之最终满足L≥la+lb的条件。

3.根据权利要求1所述空间位置解算方法，其特征在于：所述空间多自由度定位装置还包括第一夹具、第一夹具底座、第二夹具和第二夹具底座，所述第一夹具底座和第二夹具底座分别安设在第一三维转台和第二三维转台的中央，第一夹具和第二夹具分别安设在第一夹具底座和第二夹具底座上，两个目标部件分别固定在第一夹具和第二夹具上。

4.根据权利要求1所述空间位置解算方法，其特征在于：第一三维转台和第二三维转台均能实现俯仰、偏航和翻滚三个自由度的运动，通过伺服电机和丝杠能够控制第一三维转台和第二三维转台之间的距离，从而实现七个自由度的运动。