[發明專利]一種空間多自由度定位裝置及其空間位置解算方法

有效

• 申請號: 201811294859.2
• 申請日: 2018-11-01
• 公開（公告）號: CN109454472B
• 公開（公告）日: 2019-11-05
• 發明（設計）人: 付興賀;陳澤華;劉凱
• 申請（專利權）人: 東南大學
• 主分類號: B23Q1/54
• 分類號: B23Q1/54
• 代理公司: 南京經緯專利商標代理有限公司 32200
• 代理人: 施昊

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個自由度變量全部解出。