在机器人零部件完成加工和装配过程中，不可避免地会引入结构参数误差，机器人运动学校是保证其可靠及高精度运行的有效手段^[1]. 对于校准问题的方法和理论，国内外学者对其作了大量研究. 最常见的方法是通过对机器人末端绝对定位二乘误差最小化来辨识结构参数误差^[2-4]. 工业机器人在激光切割、弧焊等领域应用中，比如切割或焊接三维空间长方形，需要进行相对路点运动（如沿着X轴方向前进100 mm）^[5-6]. 在这些应用中，前后2个位型的相对精度在一定程度上决定了机器人性能. 先前针对相对精度校准的研究主要是最小化前后2个位型的相对定位误差^[7-8]，忽视了相对定向精度对机器人性能的影响. 通过大量的机器人试验发现，如果机器人安装较长的工装，定向误差会经过工装放大到末端的定位误差^[9]. 考虑姿态信息的校准是很有必要的. Xu 等^[10]将机器人末端测量的多点位置建立坐标系，通过坐标系偏差计算绝对位姿残差，对残差进行优化. 位置与姿态误差的单位选择、坐标系建系策略都会直接影响校准效果及收敛性. 为了避免以上问题，本文通过最小化3个靶球对应的最大距离误差，可以同时提升相对定位及定向精度.

机器人校准问题可以视为一个非线性优化问题. 为了解决该问题，很多优化方法被提出，如高斯-牛顿法、莱文贝格-马夸特（LM）法、卡尔曼滤波法等. 由于能够快速收敛，高斯-牛顿法广泛地被应用于机器人校准，通常是最小化残差平方和^[10]. LM法解决了高斯-牛顿法最优解搜索过程中矩阵运算可能存在的奇异问题^[11]. 卡尔曼滤波法被用来减小测量及机器人重复性误差对辨识过程的影响^[12]. 以上优化方法只能解决单目标优化问题，无法解决最小化3个靶球对应的最大距离误差这样的多目标优化问题. 本文使用二次序列规划（SQP）方法对极小极大问题进行求解，增加了校准过程鲁棒性，利用主二元算法在满足不等式约束条件下快速搜索到局部最优解.

为了提升机器人的相对精度，首先需要通过高精度的测量设备测量和计算相应的指标. 如图1（a）所示，在机器人末端法兰盘上安装了3个可以原路反射激光的靶球，使用激光跟踪仪来测量这3个球心对应机器人末端的实际到达位置. 机器人每运动到一个位型，记录串联机器人各个关节轴的角度θ，同时测量能够反映机器人末端位姿信息的3个靶球位置.

以一个n自由度的串联机器人为例，机器人末端中心点的位置和姿态可以表示为以下非线性关系：

(1) ${{T}} = \left[ {\begin{array}{*{20}{c}} {{R}}&{{p}} \\ {{O}}&{\bf{1}} \end{array}} \right] = f({{\theta }},{{\mu }}).$

映射f描述了关节空间中θ、机器人结构参数μ和笛卡尔空间的末端位置p、旋转矩阵R的关系. 如图1（a）所示，激光跟踪仪测量了安装在末端传感模块上的3个靶球，它们在工具坐标系下的坐标是未知的，记为t_a、t_b和t_c. 测量点的名义坐标可以表示为

(2) $\left[ {{{{p}}_{\rm{a}}},{{{p}}_{\rm{b}}},{{{p}}_{\rm{c}}}} \right] = \left[ {{{R}}{{{t}}_{\rm{a}}} + {{p}},{{R}}{{{t}}_{\rm{b}}} + {{p}},{{R}}{{{t}}_{\rm{c}}} + {{p}}} \right].$

对式（2）两边求导，可得

(3) $\left.\begin{array}{l} {\rm{d}}{{{p}}_{\rm{a}}}({{\theta ,\mu ,}}{{{t}}_{\rm{a}}}) = {{{J}}_{{{x{\rm a}}}}}\left[ {\begin{array}{*{20}{c}} {{{{\rm d}{\mu} }}} \\ {{\rm{d}}{{{t}}_{\rm{a}}}} \end{array}} \right], \\ {{d}}{{{p}}_{\rm{b}}}({{\theta ,\mu ,}}{{{t}}_{\rm{b}}}) = {{{J}}_{{{x{\rm b}}}}}\left[ {\begin{array}{*{20}{c}} {{{{\rm d}{\mu} }}} \\ {{\rm{d}}{{{t}}_{\rm{b}}}} \end{array}} \right], \\ {\rm{d}}{{{p}}_{\rm{c}}}({{\theta ,\mu ,}}{{{t}}_{\rm{c}}}) = {{{J}}_{{{x{\rm c}}}}}\left[ {\begin{array}{*{20}{c}} {{{{\rm d}{\mu} }}} \\ {{\rm{d}}{{{t}}_{\rm{c}}}} \end{array}} \right]. \end{array} \right\}$

式中：J_xi（i=a，b，c）是机器人末端定位误差-结构参数雅克比矩阵. 理想的状态是希望机器人末端实到位置与指令位姿一致. 由于机器人零部件在加工与制造过程中会引入结构参数与名义值的偏差，比如杆长、零位的偏差等，导致了机器人末端在笛卡尔空间中不均匀分布的误差.

相对定位精度（即距离精度）描述了机器人准确地从当前位型到达下一位型的能力，其定义描述在标准ISO 9283中：

(4) $\Delta d_j^i = \left| {{d_{\rm{m}}} - {d_{\rm{o}}}} \right|.$

式中： $\Delta d_j^i $为位型i和j之间的距离误差，末端距离测量值 ${d_{\rm{m}}} = \left\| {{{p}}_{{\rm{m}}i}} -\right. $ $\left. {{{p}}_{{\rm{m}}j}} \right\|$，距离指令值 ${d_{\rm{o}}} = \left\| {{{{p}}_{{\rm{o}}i}} - {{{p}}_{{\rm{o}}j}}} \right\|$，下标m和o分别表示测量值和指令值. 在标准ISO 9283中，没有对相对定向精度进行定义. 若参照绝对姿态精度的定义，将其描述为欧拉角的偏差，则在欧拉角奇异位型附近会产生较大的数值计算偏差. 为了避免奇异性问题，可以使用以下指标描述相对定向精度^[13]：

(5) $\Delta o = \left\| {\ln \;{{({{R}}_{\rm{m}}^{\rm{T}}{{{R}}_{\rm{o}}})}^ \vee }} \right\|.$

式中：正交矩阵R为末端工具坐标系从位型i变换到位型j的旋转矩阵. 指令相对姿态R_o到测量相对姿态R_m的微小姿态偏差可以视作三维刚体旋转变换. 根据Chasles定理可知，该刚体旋转变换可以通过绕单位向量轴的转动实现，∆o的物理意义是描述了刚体旋转变换绕空间单位轴旋转的角度，在一定程度上可以描述指令姿态与测量姿态的偏差^[14]. 与相对定位精度一致，相对姿态精度的表示与参考系无关.

如图1（a）所示，机器人末端传感模块包含3个测量点，描述了机器人末端的六维运动. 抽象地说，将末端传感模块视为笛卡尔空间中有6个自由度的三角形，每个位型对应一个这样的三角形. 将相对位姿不变性定义为任意2个不同位型对应的全等三角形相对位姿不变. 通过构建物理约束，可以使得由三角形与约束组成虚拟系统的内部自由度≤0，满足相对位姿不变性.

如图1（b）所示，2个位型三角形的对应角点通过3个测量距离进行约束，这些距离约束可以视为该距离长度的杆件通过球铰与三角形的角点连接. 通过该方式，构造虚拟的机械系统来分析内部自由度. 根据机构运动学理论可知，空间机构自由度M可以通过Kutzbach Grübler公式^[15]计算得到：

(6) $M = 6(n - g - 1) + \sum\limits_{i = 1}^g {{f_i}}. $

式中：n为系统机械元件数；g为运动副数；f_i为第i个运动副的自由度，对于球铰而言，f_i=3. 当位型数量达到3和4时，杆件变成如图1（c）、（d）所示的三角形和四面体桁架，并通过球铰与对应角点相连. 该虚拟机械系统的自由度分析如表1所示. 可以得出，当测量的位型总数≥4时，该虚拟机械系统的自由度<0，任意2个位型之间的相对位姿是不变的，充分性证明完毕.

当位型总数≥4时，相对位姿不变性的充分条件是同一靶球在任意2个不同位型下的距离误差为0. 模型参数名义值计算距离与测量距离值之间总是存在偏差，通过最小化3个距离对应的最大偏差，在优化相对定位精度的同时，尽可能保障相对定向精度，避免了直接计算姿态误差可能引入的单位同质化问题与参考坐标依赖问题.

通过1.4节的分析可得，当位型数≥4时，同一靶球在任意2个不同位型下的距离误差为0，任意2个不同位型的相对位姿不变. 在实际情况中，由于机器人结构非线性误差的存在，末端距离误差不可能为0. 需要寻找一种策略，尽可能减小末端的距离误差，末端距离误差的平方和可以表示为

(7) $F({{\mu }},{{t}}) = {({\rm{\Delta }}{{d}})^{\rm T}}{\rm{\Delta }}{{d}} = \sum\limits_{i = 1}^{n - 1} {\sum\limits_{j = i + 1}^n {{{[{\rm{\Delta }}d_j^i({{\mu }},{{t}})]}^2}} } .$

式中：∆d为距离误差序列；t为描述在工具坐标系下的3个靶球对应的名义坐标， ${ t}=[t_{\rm a},\;t_{\rm b,\;}t_{\rm c}]^{\rm T} $. 机器人末端传感模块存在固有的物理约束，即3个靶球固定在机器人末端且两两之间的距离不随坐标系变化. 靶球之间的测量与名义距离偏差应该设置为小于由跟踪仪测量误差引入的不确定度. 为了使约束条件在微分空间连续，该优化问题采用以下等价的约束条件：

(8) $\left. {\begin{array}{*{20}{c}} {t_1^2 = {{\left[ {\left\| {{{{t}}_{\rm{a}}} - {{{t}}_{\rm{b}}}} \right\| - \dfrac{1}{n}\displaystyle\sum\limits_{j = 1}^n {\left\| {{{{p}}_{{\rm{am}}j}} - {{{p}}_{{\rm{bm}}j}}} \right\|} } \right]}^2} \leqslant \varepsilon ,}\\ {t_2^2 = {{\left[ {\left\| {{{{t}}_{\rm{b}}} - {{{t}}_{\rm{c}}}} \right\| - \dfrac{1}{n}\displaystyle\sum\limits_{j = 1}^n {\left\| {{{{p}}_{{\rm{bm}}j}} - {{{p}}_{{\rm{cm}}j}}} \right\|} } \right]}^2} \leqslant \varepsilon ,}\\ {t_3^2 = {{\left[ {\left\| {{{{t}}_{\rm{c}}} - {{{t}}_{\rm{a}}}} \right\| - \dfrac{1}{n}\displaystyle\sum\limits_{j = 1}^n {\left\| {{{{p}}_{{\rm{cm}}j}} - {{{p}}_{{\rm{am}}j}}} \right\|} } \right]}^2} \leqslant \varepsilon .} \end{array}} \right\}$

式中：t_l（l=1，2，3）是2个靶球间的距离偏差，测量距离通过计算n个位型下对应测量距离的平均值得到，ε为由测量误差引入的不确定度. 对于优化问题，该非线性不等式约束可以重新写成

(9) ${{c}}({{t}}) = \left[ {\begin{array}{*{20}{c}} {t_1^2 - \varepsilon } \\ {t_2^2 - \varepsilon } \\ {t_3^2 - \varepsilon } \end{array}} \right];\;{c_j}({{t}}) \leqslant 0,\;j = 1,2,3.$

为了尽可能减小同一靶球在2个不同位型下的距离误差，同时保证机器人末端在2个不同位型下有较高的相对定位及定向精度，将目标函数定义为

(10) $\left.\begin{array}{l} \mathop {\min }\limits_{\mu {{,{ t}}}} {\rm{ }}\mathop {{\rm{max}\;}}\limits_i {\rm{ }}{F_i}({{\mu }},{{t}}); \\ {\rm{s.t. }}\;{c_j}({{t}}) \leqslant 0,j = 1,2,3. \\ \end{array}\right\} $

式中：下标i表示安装在机器人末端的第i个靶球. 通过最小化3个靶球中最大的距离平方误差和，尽可能减少了机器人末端的距离误差，增加了校准过程的鲁棒性. 测量得到的机器人末端点位置坐标表示在测量坐标系下，若只通过距离误差的关系建立目标函数，则测量数据与参考系无关，在对机器人结构参数校准过程中不会引入从测量系到机器人基坐标系的坐标转换误差.

由于极小极大搜索问题不是一个典型的凸优化问题，需要将其转换为二次序列规划（SQP）子问题才能更好地对原问题进行求解^[16]. 极小极大问题可以等价为以下约束的极小化问题：

(11) $\left.\begin{array}{l} \mathop {{\rm{min}}}\limits_{{x}} \;\psi {\rm{(}}{{x}}{\rm{)}}; \\ {\rm{s.t. }}\;{{C}}{\rm{(}}{{x}}{\rm{) }}\leqslant{{0}}{\rm{.}} \\ \end{array}\right\} $

式中：x为在校准问题中待辨识的结构参数. 有以下关系 ${{x}} = ({{\mu }},{{t}})$， $\psi ({{x}}) = \mathop {{\rm{max}}\;}\limits_i \{ {F_i}({{x}})\} $，

(12) ${{C}}({{x}}) = \left[ {\begin{array}{*{20}{c}} {{F_1}({{x}}) - \psi ({{x}})} \\ {{F_2}({{x}}) - \psi ({{x}})} \\ {{F_3}({{x}}) - \psi ({{x}})} \\ {{{c}}({{x}})} \end{array}} \right].$

式（11）中描述的优化问题可以迭代生成二次序列规划子问题，该子问题是原校准问题的近似. SQP方法的核心思想是线性化原问题，对得到的线性近似问题设计具有快速局部收敛率的搜索方法. 由于F_i（x）是可微的，对于任何搜索方向∆x，方向导数为

(13) ${D_{{{\Delta x}}}}\psi ({{x}}) = \mathop {\max \;}\limits_{i \in I({{x}})} \{ \nabla F_i^{\rm{T}}({{x}}){\rm{\Delta }}{{x}}\} .$

式中：

(14) $I({{x}}) = \{ i:{F_{{i}}}({{x}}) = \psi ({{x}})\} .$

近似的二次序列规划问题可以表示为

(15) $\left.\begin{array}{l} \mathop {{\rm{min}}}\limits_{{{\Delta x}}} \dfrac{1}{2}{\rm{\Delta }}{{{x}}^{\rm{T}}}{{{{H}}}_{{k}}}{\rm{\Delta }}{{x}} + {D_{{{\Delta x}}}}\psi ({{{x}}^k}); \\ {\rm{s.t.}}\;{{C}}({{x}}) + {{G}}({{x}}){\rm{\Delta }}{{x}}{\rm{ }}\leqslant{{0}}. \\ \end{array}\right\} $

步长表示为∆x=x^k+1−x^k，其中k为二次序列规划问题的迭代次数. 海塞矩阵H_k可以通过BFGS方法^[17]不断更新迭代，且通过引入积累了二阶信息的海塞矩阵，可以实现二次序列规划问题的超线性收敛. 约束条件中G(x)是不等式约束C（x）的梯度. 为了保证生成的二次序列规划问题存在收敛解，该约束优化问题需要满足Karush-Kuhn-Tucker（KKT）条件^[18]，该条件的拉格朗日方程可以描述为

(16) $\begin{split} L({\rm{\Delta }}{{x}},{{\lambda }}) =& \frac{1}{2}{\rm{\Delta }}{{{x}}^{\rm{T}}}{{{H}}_{{k}}}{\rm{\Delta }}{{x}} + {D_{{\rm{\Delta }}{{x}}}}\psi ({{x}}) + \\ & {{{\lambda }}^{\rm{T}}}[{{C}}({{x}}) + {{G}}({{x}}){\rm{\Delta }}{{x}}]. \end{split} $

式中：λ为KKT条件的拉格朗日算子，λ≥0. 二次序列问题（15）的KKT条件可以推导为

(17) $\left. {\begin{array}{*{20}{l}} {\nabla {F_i}{\rm{(}}{{x}}{\rm{)}}{|_{i \in I({{x}})}} + {{{H}}_{{k}}}{\rm{\Delta }}{{x}} + {{({{{G}}^{\rm T}}({{x}}))}^ + }{{\lambda }} = {{0}},}\\ {{{({{C}}({{x}}) + {{G}}({{x}}){\rm{\Delta }}{{x}})}^ + } = {{0}},}\;{{{\lambda }}\geqslant{{0}},}\\ {{{{\lambda }}^{\rm T}}({{C}}({{x}}) + {{G}}({{x}}){\rm{\Delta }}{{x}}) = 0.} \end{array}} \right\}$

运算符（）⁺定义为

(18) ${(a)^ + } = \left\{ {\begin{array}{*{20}{l}} {a,\;a \geqslant 0;} \\ {0,\;a < 0.} \end{array}} \right.$

为了寻找迭代步长及算子（∆x，λ）的最优解，同时满足上述定义的KKT条件，使用主二元子梯度算法进行梯度方向的搜索：

(19) $\left. {\begin{array}{*{20}{l}} \begin{array}{l} 0 \in {\partial _{{\rm{\Delta }}{{x}}}}L({\rm{\Delta }}{{{x}}^p},{{{\lambda }}^p}) = {{{H}}_{{k}}}{\rm{\Delta }}{{{x}}^p} + \nabla {F_i}({{{x}}^k}){|_{i \in I({{x}})}}+\\ \qquad {({{{G}}^{\rm T}}({{{x}}^k}))^ + }{{{\lambda }}^p}, \end{array}\\ {0 = - {\nabla _{{\lambda }}}L({\rm{\Delta }}{{{x}}^p},{{{\lambda }}^p}) = - {{C}}({{{x}}^k}) - {{G}}({{{x}}^k}){\rm{\Delta }}{{{x}}^p}.} \end{array}} \right\}$

式中：p为二次规划子问题的迭代次数. 迭代过程中KKT算子可以定义为

(20) ${{T}}({\rm{\Delta }}{{{x}}^p},{{{\lambda }}^p}) = \left[ {\begin{array}{*{20}{c}} {{\partial _{{\rm{\Delta }}{{x}}}}L({\rm{\Delta }}{{{x}}^p},{{{\lambda }}^p})}\\ { - {{({\nabla _\lambda }L({\rm{\Delta }}{{{x}}^p},{{{\lambda }}^p}))}^ + }} \end{array}} \right].$

定义z=[∆x，λ]^T，则搜索最优解z^*的更新率为

(21) ${{{z}}^{p + 1}} = {{{z}}^p} - {\alpha _p}{{{T}}^p},$

式中：T^p∈T（z^p），α_p为迭代步长.

直接给出式（10）中目标函数及约束条件的梯度解析式，能够使优化问题更快地收敛到最优解. 近似的二次序列规划问题（15）的目标函数梯度可以表示为

(22) $P({{x}}) = {{{H}}_{{k}}}{{\Delta x}} + \nabla {F_i}({{x}}){|_{i \in I({{x}})}}.$

式中：海塞矩阵H_k可以通过BFGS方法得到；F_i（x）的梯度可以推导并表示为

(23) $\nabla {F_{{i}}} = 2{\left( {\frac{{\partial \left( {{\rm{\Delta }}{{{d}}_i}} \right)}}{{\partial {{{x}}^{\rm{T}}}}}} \right)^{\rm{T}}}{\rm{\Delta }}{{{d}}_i} = 2{{J}}_i^{\rm{T}}{\rm{\Delta }}{{{d}}_i},$

其中J_i为描述对应第i组距离误差的结构参数误差与F_i（x）梯度关系的雅克比矩阵. 根据空间几何理论，可以将机器人末端在2个位型下的距离误差近似转换为点位误差关系^[19]. 如图2所示，机器人末端的距离误差可以表示为

(24) $\Delta d = \left| {{P_i}{P_j}} \right| - \left| {{P_i}'{P_j}'} \right|.$

式中：P_i及P_j分别为第i和第j个位型机器人末端实际到达并用激光跟踪仪测量的点，P_i'及P_j'为机器人末端名义计算点. 有以下向量关系式成立：

(25) $\begin{split} \overrightarrow {{{{P}}_{{i}}}{{{P}}_{{j}}}} - \overrightarrow {{{{P}}_{{i}}}{{'}}{{{P}}_{{j}}}{{'}}} =& (\overrightarrow {{{O}}{{{P}}_{{j}}}} - \overrightarrow {{{O}}{{{P}}_{{i}}}} ) - (\overrightarrow {{{O}}{{{P}}_{{j}}}{{'}}} - \overrightarrow {{{O}}{{{P}}_{{i}}}{{'}}} ) = \\ & (\overrightarrow {{{O}}{{{P}}_{{j}}}} - \overrightarrow {{{O}}{{{P}}_{{j}}}{{'}}} ) - (\overrightarrow {{{O}}{{{P}}_{{i}}}} - \overrightarrow {{{O}}{{{P}}_{{i}}}{{'}}} ) = \\ & {\rm{\Delta }}{{{p}}_{{j}}} - {\rm{\Delta }}{{{p}}_{{i}}}. \\ \end{split} $

式中：∆p_j和∆p_i分别为机器人末端在点P_j和P_i处的绝对定位误差. 如图2所示，线段P_iP_j''与线段P_i'P_j'平行且等长，向量和距离之间有以下关系：

(26) $\begin{split} & \overrightarrow {{{{P}}_{{j}}}{{'}}{{{P}}_{{i}}}{{'}}} \cdot {\rm{(}}\overrightarrow {{{{P}}_{{i}}}{{{P}}_{{j}}}} - \overrightarrow {{{{P}}_{{i}}}{{'}}{{{P}}_{{j}}}{{'}}} {\rm{)}} = \overrightarrow {{{{P}}_{{j}}}{{''}}{{{P}}_i}} \cdot \overrightarrow {{{{P}}_{{j}}}{{''}}{{{P}}_{{j}}}}= \\&\quad \left| {{P_j}''{P_i}} \right| \cdot \left| {{P_j}''{P_j}} \right|\cos \theta {\rm{ }} \approx \left| {{P_i}{P_j}''} \right|(\left| {{P_i}{P_j}''} \right| - \left| {{P_i}{P_j}} \right|) = \\&\quad {\rm{ - }}\left| {{P_i}'{P_j}'} \right|{\rm{\Delta }}d. \\[-10pt] \end{split} $

由于 $\left| {{P_j}''{P_i}} \right| \gg \left| {{P_j}''{P_j}} \right|$，有以下近似关系 $\left| {{P_j}''{P_j}} \right|\cos \theta \approx \left| {{P_i}{P_j}''} \right| - \left| {{P_i}{P_j}} \right|$. 将式（26）代入（25），有

(27) ${\rm{\Delta }}d \approx \frac{{\overrightarrow {{{{P}}_{{i}}}{{'}}{{{P}}_{{j}}}{{'}}} }}{{\left| {{P_i}'{P_j}} \right|}} \cdot {\rm{(\Delta }}{{{p}}_{{j}}} - {\rm{\Delta }}{{{p}}_{{i}}}{\rm{)}}.$

通过上述分析，结合机器人运动学微分学理论，对式（27）两边求微分，可得式（23）中距离误差-结构参数雅克比矩阵的解析式为

(28) ${{J}} = \frac{{\partial ({\rm{\Delta }}d)}}{{\partial {{{x}}^{\rm{T}}}}} \approx \frac{{\overrightarrow {{{{P}}_{{i}}}{{'}}{{{P}}_{{j}}}{{'}}} }}{{\left| {{P_i}'{P_j}} \right|}}({{{J}}_{{{x}}j}}{\rm{(}}{{{\theta }}_{{j}}},{{x}}{\rm{)}} - {{{J}}_{{{x}}i}}{\rm{(}}{{{\theta }}_{{i}}},{{x}}{\rm{)}}).$

二次序列规划问题不等式约束（15）的梯度可以表示为

(29) ${{G}}({{x}}) = \left[ {\begin{array}{*{20}{c}} {\nabla F_1^{\rm{T}}({{x}}) - \nabla F_i^{\rm{T}}({{x}}){|_{i \in I({{x}})}}}\\ {\nabla F_2^{\rm{T}}({{x}}) - \nabla F_i^{\rm{T}}({{x}}){|_{i \in I({{x}})}}}\\ {\nabla F_3^{\rm{T}}({{x}}) - \nabla F_i^{\rm{T}}({{x}}){|_{i \in I({{x}})}}}\\ {{{K}}({{x}})} \end{array}} \right].$

物理约束c（x）的梯度可以推导为

(30) ${{K}}({{x}}) = \left[ {\begin{array}{*{20}{c}} {2{t_1}{{\left( {\dfrac{{\partial {{{t}}_{{1}}}}}{{\partial {{x}}}}} \right)}^{\rm{T}}}} \\ {2{t_2}{{\left( {\dfrac{{\partial {{{t}}_{{2}}}}}{{\partial {{x}}}}} \right)}^{\rm{T}}}} \\ {2{t_3}{{\left( {\dfrac{{\partial {{{t}}_{{3}}}}}{{\partial {{x}}}}} \right)}^{\rm{T}}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{c}} {2{t_1}\dfrac{{{{{\rm{(}}{{{t}}_{\rm{a}}} - {{{t}}_{\rm{b}}}{\rm{)}}}^{\rm{T}}}}}{{\left\| {{{{t}}_{\rm{a}}} - {{{t}}_{\rm{b}}}} \right\|}}\dfrac{{\partial {\rm{(}}{{{t}}_{\rm{a}}} - {{{t}}_{\rm{b}}}{\rm{)}}}}{{\partial {{{x}}^{\rm{T}}}}}} \\ {2{t_2}\dfrac{{{{{\rm{(}}{{{t}}_{\rm{b}}} - {{{t}}_{\rm{c}}}{\rm{)}}}^{\rm{T}}}}}{{\left\| {{{{t}}_{\rm{b}}} - {{{t}}_{\rm{c}}}} \right\|}}\dfrac{{\partial {\rm{(}}{{{t}}_{\rm{b}}} - {{{t}}_{\rm{c}}}{\rm{)}}}}{{\partial {{{x}}^{\rm{T}}}}}} \\ {2{t_3}\dfrac{{{{{\rm{(}}{{{t}}_{\rm{c}}} - {{{t}}_{\rm{a}}}{\rm{)}}}^{\rm{T}}}}}{{\left\| {{{{t}}_{\rm{c}}} - {{{t}}_{\rm{a}}}} \right\|}}\dfrac{{\partial {\rm{(}}{{{t}}_{\rm{c}}} - {{{t}}_{\rm{a}}}{\rm{)}}}}{{\partial {{{x}}^{\rm{T}}}}}} \end{array}} \right].$

由于机器人正解过程中包含非线性三角函数的乘积，对于校准问题（10），其目标函数是非凸的. 通过将原优化问题转换为近似的二次序列规划问题，可以证明得到的线性近似问题是凸的. 根据凸优化的理论可知，目标函数及约束条件是凸的须满足以下条件：

(31) $\left.\begin{split} f\left( {\alpha {{x}} + \beta {{y}}} \right) \leqslant \alpha f({{x}}) + \beta f({{y}});\\ \alpha + \beta = 1,\alpha \geqslant 0,\beta \geqslant 0. \end{split}\right\}$

定义

(32) $Q({\rm{\Delta }}{{x}}) = \frac{1}{2}{\rm{\Delta }}{{{x}}^{\rm{T}}}{{{H}}_{{k}}}{\rm{\Delta }}{{x}} + {D_{{\rm{\Delta }}{{x}}}}\psi ({{x}}).$

由于海塞矩阵H_k是正定矩阵，有

(33) $\begin{split}& Q(\alpha {{x}} + \beta {{y}}) - \alpha Q({{x}}) - \beta Q({{y}}) = \\ &\quad - \alpha \beta {({{x}} - {{y}})^{\rm{T}}}{{{H}}_{{k}}}({{x}} - {{y}}) \leqslant 0, \\ \end{split} $

证明了二次序列规划问题的目标函数是凸的. 同样地，可以证明约束条件是凸的. 对于凸优化问题，当迭代步长满足以下条件时可以证明其全局收敛到最优解^[20]：

(34) ${\alpha _p} = \frac{{{\gamma _p}}}{{{{\left\| {{{{T}}^p}} \right\|}_2}}};\;\;{\gamma _p} > 0,\;\;\sum\limits_p {\gamma _p^2}< \infty .$

式中：ϒ_p为调节收敛率的比例因子. 对于校准原问题，由于SQP问题是对原问题的线性近似，原问题可以收敛到局部最小值.

对所有目标函数、约束条件及梯度进行初始化，然后对优化问题近似的二次规划子问题使用主二元子梯度算法寻找满足KKT条件的全局收敛解. 在子问题收敛后更新目标函数、约束条件及梯度，通过BFGS方法更新海塞矩阵. 在更新得到的解附近重新生成新的二次规划子问题，不断搜索直至原校准问题收敛.

算法：鲁棒的机器人运动学校准算法

输入：关节转角θ，名义结构参数x⁰，末端测量点p_am、p_bm和p_cm.

输出：辨识的结构参数 x^*.

初始化

∆d，F_i，▽F_i，C（x⁰），G（x⁰）<= =x⁰，H₀

迭代过程

while x未收敛 do

∆x⁰=0，λ⁰=0.

while ∆x未收敛 do

主二元子梯度算法

$\small{{{{T}}^p} = \left[ {\begin{array}{*{20}{c}}{{{{H}}_{{k}}}{\rm{\Delta }}{{{x}}^p} + \nabla {F_i}({{{x}}^k}){|_{i \in I}} + {{({{{G}}^{\rm{T}}}({{{x}}^k}))}^ + }{{{\lambda }}^p}}\\{ - {{({{C}}({{{x}}^k}) + {{G}}({{{x}}^k}){\rm{\Delta }}{{{x}}^p})}^ + }}\end{array}} \right]}$

${{{z}}^{p + 1}} = {{{z}}^p} - {\alpha _p}{{{T}}^p},{\alpha _p} = \dfrac{{{\gamma _p}}}{{{{\left\| {{{{T}}^p}} \right\|}_2}}}$

end while

更新状态、约束、梯度和海塞矩阵：

x^k+1=x^k+∆x^k，s_k=∆x^k，

$\nabla {F_i}{\rm{(}}{{{x}}^{k + 1}}{\rm{),\; }}{{C}}{\rm{(}}{{{x}}^{k + 1}}{\rm{),\; }}{{G}}{\rm{(}}{{{x}}^{k + 1}}{\rm{) < = = }}{{{x}}^{k + 1}}$

${{{q}}_k}{\rm{ = }}\nabla {F_i}{\rm{(}}{{{x}}^{k + 1}}{\rm{)}}{{\rm{|}}_{i \in {{I}}}}{\rm{ + }}{{{G}}^{\rm{T}}}{\rm{(}}{{{x}}^{k + 1}}{\rm{)}}{{{\lambda }}^{k + 1}}{\rm{ - [}}\nabla {F_i}{\rm{(}}{{{x}}^k}{\rm{)}}{{\rm{|}}_{i \in {{I}}}}{\rm{ + }}{{{G}}^{\rm{T}}}{\rm{(}}{{{x}}^k}{\rm{)}}{{{\lambda }}^k}{\rm{]}}$

${{{H}}_{k + 1}} = {{{H}}_k} + \dfrac{{{{{q}}_k}{{q}}_k^{\rm{T}}}}{{{{q}}_k^{\rm{T}}{{{s}}_k}}} - \dfrac{{{{{H}}_k}{{{s}}_k}{{s}}_k^{\rm{T}}{{H}}_k^{\rm{T}}}}{{{{s}}_k^{\rm{T}}{{{H}}_k}{{{s}}_k}}}$

end while

利用提出的校准算法在ABB机器人IRB2600上进行实验验证，名义的结构参数如表2所示，机器人及激光跟踪仪测量过程如图3所示. 测量设备使用的是Faro Vantage激光跟踪仪，测量精度为15 μm+0.8 μm/m. 测量时，机器人依次运动至工作空间内共100组任意位型处，使用激光跟踪仪测量末端3个靶球的位置，记录相应的关节角度. 如图4所示为机器人校准前的相对定位精度. 图中，M_f为频数. 结果表明，校准前机器人最大的相对定位误差为3.73 mm. 机器人末端安装有3个靶球，所以一共有3组对应的相对定位精度，其中表现最差的一组对应的相对定位误差均方根为1.20 mm.

为了验证提出算法对机器人相对精度提升的有效性，对该方法与目前常用的几种校准方法（见表3）进行对比试验. Wang等^[7]基于距离误差对机器人结构参数进行辨识，只测量并计算了安装在机器人末端一个靶球对应的距离误差. Xu等^[10-11]都是基于绝对精度指标提升末端精度，Xu等^[10]同时考虑末端的绝对定位与定向精度，Motta等^[11]只考虑定位精度.

对比试验的结果如图5所示. 图中，d_a、d_b、d_c分别为第1、2、3个靶球子在不同方法校准后的相对定位精度. 从图5可以看出，进行结构参数校准后所有方法均能明显提升相对精度. 基于距离误差的校准方法^[7]和只考虑定位误差的方法^[11]只对第1个靶球的对应指标进行优化，从图5（a）可以发现，这2个方法的第1个靶球对应相对定位精度指标优于考虑位姿的方法^[10]. 对于第2个和第3个靶球对应的相对精度指标，基于距离误差的方法和只考虑定位误差的方法的相对定位精度都略差于本文提出的方法. 对于相对定向指标，这2个方法明显差于本文提出的方法. 对比考虑位姿的方法，同时考虑绝对定位与定向精度的方法^[10]各项指标不及本文提出的方法. 为了更直观地反映各种方法的相对精度，不同方法的相对精度在不同特定区间内的概率分布如表4所示. 可以看出，本文提出的方法相对精度在接近0的区间内概率最高.

各种方法对比的相对精度结果均方根与最大值如表5所示. 表中，RMS_d、Δd_max分别为定位精度的均方根和最大值，RMS_o、Δo_max分别为定向精度的均方根和最大值. 使用提出的校准方法，相对定位及定向精度分别提升了67.98%和24.32%. 与其他方法相比，提出的方法相对于其他方法对相对精度的提升效果最好.

为了验证提出算法的通用性，在ABB七轴机器人上进行了实验验证. 不同方法的相对精度在不同特定区间内的概率分布如表6所示. 与六轴机器人的结果不同，文献[7]方法的相对定位及定向精度差于其他方法，说明如果只对1个靶球测量并计算的距离误差进行优化，相对精度不能大概率地稳定保持在一定区间范围内. 本文提出的校准算法在保留文献[7]方法的优点的同时，引入极小极大算法，增加了参数辨识过程的鲁棒性.

各种方法对比的相对精度结果均方根与最大值如表7所示. 使用提出的校准方法，相对定位及定向精度分别提升了90.61%和74.61%. 与其他方法相比，提出方法只在相对定位精度最大值方面略差于文献[10]、[11]的方法，其他指标均优于其他方法. 考虑到优化的指标函数是误差平方和，本文方法保证了末端误差的平方和最小，减小了由于测量不确定度引入的参数辨识偏差，增加了参数辨识过程的鲁棒性.

本文提出基于相对精度指标的机器人结构参数校准方法. 该方法通过最小化3个靶球对应的最大距离误差来保证相对定位及定向精度. 为了求解非凸的极小极大问题，使用SQP方法对原校准问题进行近似，在增加了校准过程鲁棒性的同时，通过主二元子梯度算法在满足不等式约束条件下快速搜索到局部最优解. 该方法有以下优势：1）以距离误差的平方和作为目标函数，距离误差的计算与坐标系的选择无关，在参数辨识过程中不用考虑测量系与基坐标系的对齐问题；2）利用极小极大算法可以提升校准算法的鲁棒性；3）利用该方法可以同时提升相对定位及定向精度. 在进行校准及精度验证后，实验结果表明，六轴机器人IRB2600相对定位及定向精度分别提升了67.98%和24.32%，七轴机器人IRB14000分别提升了90.61%和74.61%.