中国
利用四元数表示法标定机器人基坐标系的
duck是什么意思方法
王伟，贠超，王刚
5 10 15 20 25 30 35 40
（北京航空航天大学机械工程及自动化学院，北京100191）
摘要：在多机器人协同、运动学参数辨识和机器人离线编程等作业任务下，需要标定出机器
人基坐标系在世界坐标系中的精确位姿。提出和实施了一种利用四元数表示法进行机器人基
坐标系精确标定的方法。利用指数积公式，建立机器人的运动学模型。利用外部测量设备，
获得五个不共面的工具中心点位置，并记录对应的机器人关节位形。以基坐标系旋转矩阵的
九个元素为变量，建立四个不相容方程，通过最小二乘法，求出基坐标系位姿的初步齐次矩阵。由于测
量误差和截断误差的存在，初步齐次矩阵的旋转矩阵不能满足单位正交阵的性质。利用四元数的几何约束条件，建立乘子罚函数形式的目标方程，将初步齐次矩阵进行正交化
修正。通过实际机器人的标定试验，验证了该标定方法的正确性，并提高了机器人末端的定
位精度。
关键词：机器人；标定；基坐标系；四元数；正交性
中图分类号：TP242
Calibration Method of Robot Ba Frame by Quaternion
Form
WANG Wei, YUN Chao, WANG Gang
(School of Mechanical Engineering and Automation, Beihang Universiy, Beijing 100191) Abstract: The accuracy po of robot ba frame with respect to the world frame is to be
calibrated under the task of multi-arm cooperation, identification of kinematic parameters and
off-line programming. A calibration method by the quaternion form is propod and implemented
to accurately calibration the robot ba frame. The kine matics model is established by the
product-of-exponential formula. Five different TCPs (Tool Center Point), which are not coplanar,
are measured with the external measuring device, and the corresponding five robot configurations
are also recorded. By considering the 9 elements of rotational matrix of the ba frame po as the variables, a group of 4 incompatible equations is t up, and the initial transformation of the ba frame is also obtained by least square method. However, due to the errors of measu ring procedure and truncation, the rotational matrix cannot satisfy the regulation of unit orthogonal matrix. By the geometrical constraints of a quaternion, an objective equation is established by the form of penalty function with a multiplier, and the in itial transformation is updated into a unit orthogonal matrix.
By a t of calibration experiments, the validity of calibration method is demonstrated, and it is
ud to update the accuracy of the robot.
Key words: Robot; Calibration; Ba frame; Quaternion; Orthogonality
0引言
工业机器人具有较大的灵活工作空间和较高的重复定位精度，能够完成焊接、喷涂、装
配和搬运等作业任务，已经被广泛应用在工业自动化生产中。为了满足更加精细的作业需求，如何进一步提高工业机器人的作业精度是当前的研究热点之一。利用特定几何约束或更高精
度的测量设备，对工业机器人的基坐标系、运动学参数和运动轨迹进行标定，是提高机器人
基金项目：高等学校博士学科点专项科研基金(20121102120020)
作者简介：王伟（1982-），男，讲师，主要研究方向：机器人标定和机器人变刚度关节驱动器等. E-mail: ************
定位精度的有效手段。一般情况下，机器人基坐标系设置在机器人底座上，当第一关节转角
45 50 55 60 65 70 75 80 为0 时，基坐标系与第一连杆坐标系重合。通常情况下，机器人基坐标系在世界坐标系中的精确位姿，是不能通过直接测量获得。机器人基坐标系的精确标定，在机器人离线编程[1]
和多机器人协调[2]等领域具有重要的意义。
工业机器人往往采用多关节串联/并联结构，其末端的操作运动通常相对于基坐标系描述。通过运动学正解，将关节运动合成为末端运动；通过运动学反解，可以将末端运动分解为各关节运动[3]。在运动学模型中，机器人基坐标系是设置在理想位置的，而实际机器人的基坐标系位置往往偏离理想位置。只有通过外部标定才能获得机器人基坐标系的精确位姿，从而提高末端的定位精度。
Haytti 模型[4]、CPC 模型[5]、S 模型[6]和指数积模型[7]等被广泛应用在机器人运动学参数的精确标定，而有关机器人基坐标系标定的研究则较少。利用这些标定模型和方法，研究者可以获得相对于测量系统坐标系的机器人运动学参数。如果不能获得机器人基坐标系相对于测量系统坐标系的位姿，那么标定后的机器人模型只能在测量系统坐标系中描述，而不是在基坐标系中描述。这种坐标系描述上的不统一，给机器人编程和实际应用带来许多不方便。目前国内外有关机器人基坐标系标定的研究主要从两个方面展开：快速标定和精细标定。
为了在工业现场快速、简便和低成本地获得机器人基坐标系的位姿，通常需要对机器人基坐标系进行快速标定。这种快速标定技术往往采用简单工具，归纳起来可分为三类。
第一类是采用球面、圆柱面和平面等特殊几何形状[8]，这种方法在工业现场应用最为普及。文献[9]采用AX=XB的标定方程，利用“peg-into-hole”的方式来标定两个机器人之间的
相对位置，最终的标定精度直接取决于孔和杆的几何精度。这一类标定方法利用机器人末端与特殊几
何形状接触，从而将开链机器人转化为闭链结构，根据不同接触位形建立非线性不相容方程组，最后求解该方程组的最小二乘解，实现机器人基坐标系的标定。
第二类是采用线性位移传感器等增量式测量设备。文献[10]整合了线性位移传感器(Linear Voltage Displacement Transducer, LVDT)，提出了一种“相对标定”的想法，首先利
用球面标定出LVDT相对于末端坐标系的位姿，然后再利用LVDT对机器人基坐标系和磨削
工具坐标系的相对位姿进行了标定。这种标定方法包括的标定步骤较多，包括的误差因素较多，限制了最终的标定精度。
第三类是采用单目或双目视觉标定技术，通常可以把摄像头安装在机器人末端[11]或者
安装在机器人环境中[12]。视觉标定技术往往也需要一个特殊几何形状的工件，其数学模型
与第一类标定方法相同，其最终目标也是建立一个待标定机器人与特殊几何形状的运动学闭链。视觉标定不需要实际的物理接触，标定步骤更加灵活，测量范围更大，有利于提高最终的标定精度，但是摄像头本身的精确度会影响最终的标定结果。
ABB等机器人公司集成了大部分已有的快速标定方法[13]，并提出了一种标准化的标定
软件包，将繁琐的机器人标定步骤转化为程序化工作，简化了操作，最终在实际工业现场提高整个机器人工作单元的定位精度。
精细标定往往需要采用外部的精确测量设备，并设计相应的离线标定算法，从而能够大大提高机器人基坐标系的最终标定精度。典型的测量设备包括激光跟踪仪[14]、三维坐标测
量机和被动式机器人测量臂[15]等。此类精细标定方法的主要实施步骤包括建模、测量、辨
识和校正等，标定周期较长，一般适用于离线标定场合。基坐标系的位姿一般用齐次矩阵的形式来表示，其中包括一个三维正交旋转子阵。文献[16]利用瞬时轴表示法对双臂协调机器
人的基坐标系相对位姿进行了标定和优化，提高了旋转矩阵标定结果的正交性。文献[17]对
ωi ] ，
0] ， v i ∈ ℜ 是指向移动方向的单位矢量。
⎛ ⎫ ⎛ ⎫
g st ( q ) = exp ξ1 q 1 ⎪ ...exp ξ n q n ⎪ g st (0 ) ⎛ ⎫
请说话= ∏ exp ξi i ⎪g st (0 )
⎝ ⎭
⎛ ⎫ = g ms ∏ exp ξi i ⎪g st (0 )
⎝ ⎭ 设末端{T }相对于测量坐标系{M }的位姿
g mt (q ) = ⎢
量坐标系{M }的位姿
g ms = ⎢
g st (q ) = ⎢ st
85
90
不精确的旋转矩阵进行奇异值分解，从而获得接近于单位齐次矩阵的真实旋转矩阵，并在多 种测量方
式下验证了该方法的正确性。旋转矩阵为单位正交矩阵，其几何意义就是基坐标系 的三个轴向方向必须正交，这是机器人基坐标系精确标定的难点。
本文的基坐标系标定方法属于精细标定领域。全文安排如下：在第一节中，利用指数积 公式建立了机器人运动学模型；第二节设计了基坐标系的初步标定方法；第三节提出了基坐
标系的精确标定方法；第四节介绍本文的试验系统和试验数据；第五节给出了全文的结论。
1 运动学建模
利用指数积公式建立串联机器人的运动学公式，需要选定参考坐标系{S } 和工具坐标系
{T }。本文选定{S } 为机器人基坐标系，{T }与末端被测点坐标系重合。当机器人的全部关
节转角都处于理论零位，即 q = {0}时，{S } 和{T }之间的刚体变换定义为参考位形 g st (0) 。
95
对于每一个关节，构造一个螺旋 ξ i 。对于转动关节，关节螺旋
ξi = [v i
T
v i = -ωi ⨯ O i ， ωi ∈ ℜ3 是关节轴线方向上的单位矢量， O i ∈ ℜ3 为轴线上的任一点。对于
移动关节，关节螺旋
ξi = [v i T 3
利用指数积公式将各关节的运动加以组合，可得 n 个关节的串联机器人正向运动学方
程：
100
i
=1
∧ ∧
⎝ ⎭ ⎝ ⎭
n ∧ q
(1)
在机器人基坐标系标定试验中，往往测量坐标系{M } 与机器人基坐标系{S } 不重合， 后者相对于前者存在一个刚体变换 g ms 。那么，在测量坐标系{M } 下描述的机器人正运动 学方程为：
g mt ( q ) = g ms g st ( q )
i =1
n ∧ q
(2)
105
2 基坐标系初步标定
⎡ R mt ⎣ 0
P mt ⎤ 1 ⎥⎦
，基坐标系{S }相对于测
⎡ R ms ⎣ 0
P ms ⎤ 1 ⎥⎦
，末端工具坐标系{T }相对于基坐标系{S }的位姿
⎡ R ⎣ 0
P st ⎤ 1 ⎥⎦ 。利用矩阵的分块乘法，由式（2）可得：
p m sx p s tx p m x = ⎢⎢ ⎥⎥ = ⎢⎢ ⎥⎥ 设 P m s m s y ， P st sty 。选取 5 个不共面的 TC P 位置，依次记为 P m t = ⎢ p m t y ⎥ ， i ⎢ i ⎥ p m sz p s tz p m z ⎣ ⎦ ⎡ p stx ⎤ P sti = ⎢ ⎥ ， i = 1,...,5。由式（3），依次做四次减法可得： ⎣ i ⎦
⎡ p m tx - p
1m tx ⎢
2 1
⎢ p mty mty ⎢ p mtz - p 1mtz ⎣
p mtx - p mtx
annapolis
- p p - p
p
p mtx - p mtx - p p - p
p
p mtx - p mt x ⎤ ⎡ p stx - p st x p mty mty ⎥ = R ms ⎢ p sty sty - p - p 1
p mtz - p mtz ⎥ ⎢ p st z - p
stz ⎣pbskids
p s tx - p stx 2 p - p p - p
p s tx 4 - p s tx
p - p p - p
p stx - p stx ⎤
5 4 ⎥（4） p sty sty ⎥ p stz - p stz ⎦
⎡ p mt x - p mtx ⎢ 3 2
⎢ p mt x mtx
⎢ p mt x - p mtx ⎢ 5 4
⎢⎣ p mtx mtx
- p
p mty mty
- p p mty mty - p p mty mty - p p mty mty
- p ⎤ ⎡ p - p
p mtz mtz stx stx - p p stx stx
- p = ⎢
4 3
p mtz - p mt z ⎥ ⎢ p stx - p
stx
- p - p
- p
p sty sty p st y - p sty
andrew blake- p p sty sty
- p p sty sty
p stz - p stz
⎤
- p p stz stz 2 ⎥ T
p stz stz 3 ⎥ ms - p R
- p p stz stz 4 ⎥⎦
⎦ ⎣ ⎡ p mtx - p mtx ⎢ 3 2
b 3 ] = ⎢ mtx mtx ⎢ p mtx - p mtx ⎢⎣ p mtx - p mtx - p p mty mty p mt
y - p mt y
- p p mty mty p mt y - p mt y p mtz - p mtz
⎤ p mtz - p mtz ⎥ p mtz - p mtz ⎥ p mtz - p mtz ⎥⎦
⎡ p stx - p stx ⎢ 3 2 令 A = ⎢ 4 3 ⎢ p stx - p stx ⎢ 5 4 ⎢⎣ p stx stx - p p sty sty
- p p sty sty - p p sty sty - p p sty sty p stz - p stz ⎤ 3 2 ⎥ p stz stz ⎥ p stz - p stz ⎥ - p p stz stz 4 ⎥⎦ 令 R m s = [ x 1 = Ab x j j
A + = ( A A ) A T
-1 中国
110
i
⎢ i ⎥ p sty ⎢ p stz ⎥ p p i
i 2
2 - p
3 2
3 2 mty mty 3 2 mtz mt z
4 3
4 3 mty mty 4 3 mtz mt z
5 4 2 1 5 4 ⎥ ⎢ 2
5 4 3
3 2 sty sty 3 2 stz stz
4 3 st y s ty 4 3 stz stz铁幕演说
3
5 4
- p 5 4 ⎥ R ms 包括 9 个未知数，式（4）可以分解为 12 个独立方程，那么式（4）为不相容方程
组。对式（4）两端做转置，可得：
115
2 1 - p 4
3 - p 2 1
3 2
4 3
5 4 2 1 2 1
3 2 ⎥ ⎢
3 2
p mtz mtz
⎥ 4 3 5 4 ⎥ ⎢ 5 4
p mtz mtz ⎥ ⎢ p stx stx 2 1
3
4 3
5 4 2 1 3
4 5
⎥ ⎥
（5）
令 B = [b 1 b 2 2 1 p - p 4 3 ⎢ 5 4 2 1 3 2 4 3
5 4 2 1 4 3
3 2
⎥
5 4
⎥
。
2 1 p stx - p stx - p 2 1
3 2
4 3
5 4 2 1
- p 4 5
⎥
。
T
x 2 x 3 ]，则由式（5）可得：
b j = Ax j
（6）
120
送餐服务其中 j = 1,...3 ， b j 和 A 为已知量， x j 为待求量。
建立不相容方程组，可得：
由于 A 为列满秩，可得：
T
+
（7）
（8）
125
利用式（7），即可求得 R m s 的三个行向量，从而确定基坐标系{S }相对于测量坐标系{M }
的姿态。再由式（3）可得：
P ms = P mt -R ms P st
（9）
利用式（9）即可求得 P ms ，从而确定基坐标系{S }相对于测量坐标系{M }的位置。由式 （7）和式（9）可以完成基坐标系{S }的初步标定。
利用上述初步标定方法，可以获得基坐标系{S }的初步位姿 g m s 。但是由于测量误差和
计算截断误差的存在，导致 g m s 中的旋转矩阵不能满足单位矩阵的正交性。在基坐标系精细
⎩
⎪
⎨ ⎢ ω -ω1 ⎥⎥ ， ω = [ω1 ω2 ω3 ] 。将式（10）、式（11）和式（13）
ˆ ⎢r r ⎣
r 31 = 2(q 1q 3 - q 0q 2 ) ， r 32 = 2(q 2q 3 + q 0q 1 ) ， r 33 = 1- 2( q 1 + q 2 )
2 2
中国
130
3 基坐标系精细标定
背诵为王
'
'
标定中，利用四元数的性质和乘子罚函数法来修正旋转矩阵的正交性，进一步提高标定精度。
135
3.1 单位四元数表示法
描述两坐标系之间的姿态可以用三个独立元素来描述。通常的描述方法包括：旋转矩阵、 欧拉角、RPY 角和瞬时轴等。单位四元数是一种描述两个正交坐标系之间的相对姿态的数 学表示法。单位四元数仅需要 4 个元素，需满足一个约束条件。而旋转矩阵包括 9 个元素， 则需满足 6 个约束条件。相对而言，四元数表示法比旋转矩阵更加简洁，且两者之间存在等 效变换关系。
140
养成良好的学习习惯
若存在四元数 q ，设为 q = q 0 + i q 1 + j q 2 + k q 3 ，i , j , k 分别表示三个正交坐标轴方向的 单位矢量。令 q = i q 1 + j q 2 + k q 3 ，可得：
θ = 2 cos -1 q 0
（10）
⎧ q ω = ⎪ sin (θ / 2 0
)
θ ≠ 0
其他
（11）
设与之对应的旋转矩阵为
R ，那么由关节螺旋定义可得：
145
R = exp (ωθ )
利用 Rodrigue 公式[18]，可得：
exp (ωˆθ ) = I 3⨯3 + ωˆ sin θ + ωˆ 2 (1- cos θ )
（12）
（13）
其中 ω = ⎡ 0
⎢ 3
⎢⎣-ω2
-ω3 0
ω1
ω2 ⎤ 0 ⎥⎦
T
带入式（12），可得：
150
⎡ r 11 r 12
R = ⎢ 21 22
⎢r 31 r 32 r 13 ⎤ r 23 ⎥⎥
r 33 ⎥⎦
（14）
其中，
r 11 = 1- 2( q 22 + q 32 ) ， r 12 = 2(q 1q 2 - q 0q 3 ) ， r 13 = 2(q 1q 3 + q 0q 2 )
r 21 = 2(q 1q 2 + q 0q 3 ) ， r 22 = 1- 2( q 1 + q 3 ) ， r 23 = 2(q 2q 3 - q 0q 1 )
waterboarding
2
155
3.2
旋转矩阵正交化
利用式（7）标定所得的初步旋转矩阵不能满足旋转矩阵的正交性，因此有必要对初步
标定结果进一步修正。旋转矩阵修正就是定义一个目标函数，当四元数变量取得最优值时，
