六自由度关节式坐标测量机关节零位偏差的标定算法

HEATS

1．引言

　　六自由度关节式坐标测量机是一种新型的非笛卡尔式坐标测量机。它仿照人体关节结构，以角度基准取代长度基准，将六个杆件和一个测头通过六个旋转关节串联连接，一端固定在机座上，另一端（测头）可在空间自由运动，构成一个六自由度的封闭球形测量空间。与传统的笛卡尔式三坐标测量机相比，它具有机械结构简单，体积小，测量范围大，灵活方便等优点，主要应用于CAD／CAM中三维模型表面数字化和大型零部件几何尺寸的现场检测等领域^［1，2］。
　　六自由度关节式坐标测量机在装配过程中，由于角度光电编码器的零位与关节结构的理论零位不重合而产生的角度偏差，称为关节零位偏差。其特点是：各关节的零位偏差各不相同；由于装配工艺误差不可避免，关节零位偏差较大（约±3°）；对于每台装配好的关节式坐标测量机，各关节零位偏差值固定不变，属于系统误差。由于杆长的放大作用，关节零位偏差在末端测头处产生很大的位姿误差。因此，为了补偿关节零位偏差，提高测量精度，对关节式坐标测量机进行标定是非常重要的。

( `5 d6 {4 j- @

　　2．数学模型

T% A3 o: b1 C P4 }- r

图　六自由度关节式坐

　　标测量机结构模型六自由度关节式坐标测量机从机械结构上可以看成是串联的开式运动链。其结构模型如图所示（各坐标系的Y轴由“右手法则”确定）。
　　参见图示，将测头局部坐标系O₇-X₇Y₇X₇相对于基座参考坐标系O₀-X₀Y₀Z₀的位姿记为T₀₇，这是一个4×4的齐次矩阵，可描述为

T₀₇＝A₀₁A₁₂A₂₃A₃₄A₄₅A₅₆A₆₇　　　　　　　　（1）

* u0 w5 u, J* L) y" Z* p! B

式中，A_i-1i（i＝1，…，7）是杆件i相对于杆件i－1的齐次位姿变换矩阵。Denavit和Hartenberg在1995年提出了两个相互连接且相对运动的构件之间相互关系的分析方法，并给出了相应的齐次变换矩阵^［3］，即

2 y, Q( l' { f7 A) ]

　　　　　　　（2）

f5 G4 n& R5 f1 R- w

式中，θ_i是关节的转角，这里是变量，称为关节变量；φ_i是相邻关节旋转轴线的夹角，这里近似为直角；a_i是相邻关节轴线沿空间公垂线的距离，这里近似为0；d_i是相邻杆件坐标原点沿Z轴之间的距离，这里称为杆长。
　　对于多关节坐标测量机，测头在空间的姿态并不重要，而测头的空间位置坐标则是需要得到的。将式（1）、（2）合并后，测头位置坐标方程为

8 d) Q) u; t6 A- m3 y! I

　　P＝（R₁R₂R₃R₄R₅R₆）q₇＋（R₁R₂R₃R₄R₅）q₆＋R₁R₂R₃R₄）q₅
　　　　＋（R₁R₂R₃）q₄＋（R₁R₂）q₃＋R₁q₂＋q₁　　　　　　　　　　　　（3）

/ _2 Y2 v$ E7 V# ^

式中包括三个坐标分量方程，都是关节变量的函数，即

P＝F（θ₁，θ₂，θ₃，θ₄，θ₅，θ₆）　　　　　　　（4）

. g4 ~/ B9 @: Y* B

　　为了得到关节零位偏差与测头位置误差之间的关系，假设关节零位偏差足够小，对式（4）求全微分，近似得到测头位置误差方程为^［4］

　　　　　（5） 9 K, Q6 O x/ v0 g2 R, J0 o- M

　　将式（5）用矩阵方式简单描述，即

9 E2 E+ A* L9 F( A2 g, c. Y6 B+ V

ΔP＝J_δΔδ　　　　　（6）

式中　ΔP＝（ΔP_x　ΔP_y　ΔP_z）^T
　　
　　Δδ＝（Δθ₁　Δθ₂　Δθ₃　Δθ₄　Δθ₅　Δθ₆）^T

　　由式（4）和式（6）即可得到描述关节零位偏差与测头位置误差之间关系的线性方程。
　　3．标定算法
　　为测定各关节零位偏差值，需要一系列已知标准位置坐标，这些标准位置坐标可以通过高精度的三坐标测量机测得。设有m个标准位置坐标，关节式坐标测量机的测头分别触测这些标准位置，由光电编码器分别得到相应的关节转角，将这些关节转角分别代入方程式（4），计算出测头的理论位置坐标，然后与标准坐标比对，得到m个测头位置误差。把这些数据代入式（6），可得到3×m个位置误差方程，即

ΔQ＝GΔδ　　　　　（7）

其中

0 u8 d: K: E: M5 b

+ ]$ b. { `- W v- h7 L

式（7）中有6个未知量，只要3×m＞6，则可运用最小二乘法求解出关节零位偏差，即

7 c' O0 w" l* s2 D" J. k

Δδ＝（G^TG）^－1G^TΔQ　　　　　（8）

　　把计算出的关节零位偏差值作为零位偏差的修正量代入式（4），计算出新的测头位置坐标，然后将新的位置误差和新的系数矩阵代入式（7），再重复式（8）的计算。经过以上的反复迭代过程，直到测头位置误差小于设定值，最后获得最优解，即最接近实际的关节零位偏差值。

; R. v+ y) x9 V! ^4 J; c

　　4．仿真验算

　　为了进行计算机仿真验算，首先设定六自由度关节式坐标测量机的结构参数（φ_i，a_i，d_i），并假设关节零位偏差（Δθ_i），具体数值列于表1。

) H) K" l8 J, c% \( S7 _' l

表1　六自由度关节式坐标测量机结构参数

# E5 M5 s# H5 U& P" q; a 1 `9 H: o7 O ^% f) d4 b. J+ |. y! ]' u' s0 D6 K) K! {/ h7 D+ Z0 \7 {) ?. m+ b" J7 h0 a1 I n% I/ t. W( d) g9 Z) D$ T9 K7 ^: S8 I+ ]! @5 r' ~1 w9 T3 h3 r4 O P p* ?# s: M u. }8 J! k+ p( Y" ]& k$ a" I3 \: T) k% t; U1 D+ h. V" D: @7 @# M; I' S5 k T: K3 U4 A2 r2 G: _& e& a! t, z" V0 o$ R% e0 S7 ]+ V4 G5 ?. e1 t% B: B/ i" b) C9 W# Q* k' K7 A4 i+ @1 h* D+ [& K" |* s; [' ]) q: O. ~8 g J U8 o4 G6 d( y4 s: C1 _' E6 L' }- U$ N4 F& K |& `4 Q2 r/ ^* y5 V+ S, U; L C$ P1 ]7 x: Z; A7 e b; e( s/ d2 o8 A Q2 W& I+ q U, N5 t% L* G+ T# |9 ]6 O# J# R3 s4 Q! r8 N8 Z O3 E- N* |. f1 |: h! N6 P7 K$ r! {& g+ q0 e# m8 _# m2 ^& ?0 M4 Y" ~- B6 e- S7 Z+ f( k2 J, F2 U6 q$ ~' m3 c# O& ?. ^4 @- \. \8 C; \2 [1 g: u& Z$ s* p

杆件 序号	φi（°）	ai（mm）	di（mm）	零位偏差 Δθi（°）
1	－90．1	0．01	99．85	2．4
2	90．05	0．02	151．38	－2．05
3	90	－0．01	448．6	－1．5
4	－89．9	－0．03	101．1	2．2
5	－90．05	0．01	352．2	1．2
6	89．35	0．005	99．75	－1．8
7	0	0	150．25	0

% L: ]/ k( Z- A! B1 j. a3 U& o# A

在验算中，对标定算法重复进行了三次仿真计算。每次随机选取3组关节转角组合，根据表1中的结构参数和关节零位偏差，按式（4）计算出3个标准（实际）的空间坐标矢量。同时，不考虑零位偏差，计算出3个理论坐标矢量，并得到相应的误差矢量。根据式（5）、（6）、（7），可得到9个误差方程。利用式（8）求解出6个关节零位偏差值。为了获得更准确的数据，采用迭代算法，将计算出的关节零位偏差值代入式（4）修正理论模型，重复以上过程，直到空间位置误差小于一设定值（这里设为0．3mm）。三次仿真计算的结果列于表2。 表2　仿真验算结果

5 W/ |$ Z3 T* {( n i2 s, y2 J9 X' n# J' z# z" R) U) r) g4 m$ a" }5 }, g) `% D# ~% q7 G- \& V+ B$ U( U" ]2 l' ~# @! {. f9 V! x9 E; |" ?! M9 X* m2 t+ N5 C" l7 h% ]9 P3 w" |, v, [0 L) Q; G* v5 e* m# ^" M+ E$ ]: S D8 F2 y. z9 E% {! }# @, b' \4 A" V3 h5 ^1 {4 m9 I( }2 j! }, E6 _1 ]4 Y6 n5 ]% s* V) f7 b& ], V) l8 I' h+ E# T$ x4 p7 M. U% q' [( ]( B4 M3 h! D' b5 R- O+ t) @$ @' h; R" x- o5 E, a) V% l6 B& I( V3 ^3 ]5 y$ q' t0 H9 Q s6 s+ u/ J9 b4 P& o5 }* Z0 o) Y; X( Z% n4 _! m+ K. T$ ^* [$ j/ u4 `1 d, z" b' F( s8 R# q( r; Q, ^% g0 \$ l& ^3 q$ D3 X' m% k# y6 e1 e$ r* E6 H" a% Q/ [, _# y; r2 F9 V$ Z' L9 w& b5 \& W! B: G4 a) l+ [8 [0 G" F) p. c9 z) o {! B$ P( ~! }5 P3 M% Y1 K: G: X7 q7 ^! Y" s4 i! m% J2 l2 f# i, z! h1 J( o( A5 m( O3 O, Y' d3 w5 } U! }" |# f+ I! s$ ?/ g" h+ T7 C- x6 ?! H; s* }, W. z6 ?( g& }/ ~+ T2 I+ O% n: I4 R/ m3 }( l" t: a k7 h$ e$ |* p( o- G. s6 G3 M, L; o$ R, s; c9 A: ^( S: S( n/ B+ H8 d' c1 B( \+ P" R- W9 N, W `. ~# N8 z9 p! J x! g- o" R' Q0 y# w7 \$ k( g5 h2 S y- F4 r! D+ f! q; T' l: p! |2 c1 w m+ A- w3 _, T2 E* Y( Y' b5 n! R a3 J, E& d# x& V& y: o5 M3 E6 A8 l0 |; |" ? | H4 l' g" L# e" \! ]6 {& I# N# u( P7 a/ P7 w7 W l7 H% H: v/ c( A( A, T$ W& d) G1 B! F$ `4 u; i+ d& Z3 m6 R Y+ [& H1 n- @4 i; e# d7 h9 n6 H( {# u& G' |+ }- M% U$ r9 k4 {4 e2 C, x( U- ^2 C: q) Y0 P# ]! z5 X. B5 x. I1 n$ B& W2 r7 |, [# T5 P8 l3 x, E) T' v6 k9 I+ i$ f* X6 i9 C4 D* }3 j+ A, Q6 |; ^3 P* I M* X- R" L: F' `% M* y4 c; T; F2 d! n" U( K3 @. E5 y4 i7 M8 j& Y5 P, }3 k" A6 c$ L$ y: ~; k4 L

仿真 次数

关节转角组合 （θ1，…，θ6） （°）

标准坐 标矢量 （mm）

位置 误差 （mm）

迭代1次

迭代2次

迭代3次

迭代4次

零位 误差 （°）

位置 误差 （mm）

零位 误差 （°）

位置 误差 （mm）

零位 误差 （°）

位置 误差 （mm）

零位 误差 （°）

位置 误差 （mm）

1

（-90，-90，0，90， 0，90）

138．558 575．782 -283．282

39．968

2．406 -2．063 -1．432 2．177 1．776 -1．375

1．717

2．406 -2．063 -1．490 2．177 1．719 -1．776

1．483

2．406 -2．063 -1．490 2．177 1．203 -1．776

0．148

2．406 -2．063 -1．492 2．182 1．203 -1．781

0．146

（120，90，120，-20， 70，50）

-652．092 645．842 202．687

30．043

1．201

0．955

0．206

4 z% p& @3 F! r' _9 B5 Q# ]

0．206

（-90，0，0，90， 0，0）

142．57 522．883 503，83

49．592

0．732

0．801

0．244

0．232

2

（-90，10，10，90， 10，10）

61．557 434．09 594．691

49．319

2．521 -1．948 -1．432 2．464 1．146 -1．261

1．479

2．406 -2．063 -1．490 2．181 1．203 -1．781

0．212

（0，90，0，-90， 0，90）

320．85 157．437 -251．305

23．654

1．155

; Y# b2 ]' j. j, q8 R, S' }5 M

0．230

< 0 m- ~; m" Y' C0 s m

<

（-120，140，20，60， 10，150）

-148．998 -518．998 -237．442

39．30

2．176

0．082

3

（50，-80，50，40， -60，40）

-463．903 -760．076 176．659

73．57

2．349 -2．120 -1．432 2．292 1．146 -1．432

0．603

2．406 -2．063 -1．490 2．180 1.203 -1．781

0．068

（10，80，30，150， 30，140）

186．165 160．292 290．97

26．475

0．894

0．069

< " m, B# L% e7 M- G$ Q* }

<

（40，30，60，150， 90，-90）

179．32 102．388 101．179

17．161

0．898

0．122

, U- I; c1 ~9 F3 E8 g! q

9 `7 g+ d# A3 p5 [$ X ~# P O( Z) |4 @# t y+ i5 W

由仿真验算结果可得出以下结论： 　　（1）较小的关节零位偏差会引起很大的测头位置误差，最大可达73．57mm； 　　（2）本文给出的标定算法是正确的，经标定得出的关节零位偏差值与设定的真值近似，最大误差为0．02°； 　　（3）迭代算法是收敛的，一般不超过4次迭代； 　　（4）三次仿真计算所得结果相同，说明只要在测量空间任取三点，即可准确且唯一地标定出6个关节零位偏差。 　　5．结语 2 y+ K+ w# @, m9 t 　　本文在Denavit-Hartenberg方法基础上，建立了六自由度关节式坐标测量机的数学模型。从模型可以看出，测头末端位置坐标与六个关节角度之间的关系是非线性的，这对于由已知空间坐标值来推算关节零位偏差是相当困难的。因此，我们运用全微分方法，求得了关节零位偏差与测头末端位置误差之间的线性关系，从而大大简化了标定过程。在已知空间点坐标情况下，应用最小二乘法和有限次数的迭代运算，求出最优的关节零位偏差值。最后，通过计算机仿真验算，证明了该算法的正确性。该标定算法也完全适用于其它结构参数误差的标定。该标定算法对提高六自由度关节式坐标测量机的测量精度具有重要意义。

$ Z R! @4 x) j6 L& N