CN114872037B - 基于Nelder-Mead的并联机器人校准方法 - Google Patents

Abstract

本发明涉及一种基于Nelder‑Mead的四轴并联机器人校准方法，其首先求取机器人的正解，然后充分考虑各支链的长度差异、动静平台的误差、零点差异、工具坐标误差，从而抽象出了多个重要参数，接着使用激光跟踪仪器采样机器人工作空间位置及对应的关节角，使用任意两点间的距离构建评估函数，基于该评估函数，使用Nelder‑Mead方法求取最优解，获取嘴角校准参数，提高了机器人控制精度。

Description

基于Nelder-Mead的并联机器人校准方法

技术领域

本发明涉及机器人控制领域，具体涉及一种基于Nelder-Mead的四轴并联机器人校准方法。

背景技术

Delta并联机器人又常称为蜘蛛手，其具备了并联机构所具有的负载能力强、效率高、末端执行器精度高、运动惯性小，可以高速稳定运动等的优点。因此在机器人领域获得了越来越广泛的应用。在实际的加工装配过程中，存在误差，同时也很难保证主动臂的零点位置。

发明内容

针对现有技术存在的问题，本发明的目的在于提供一种基于Nelder-Mead的四轴并联机器人校准方法，通过对机器人结构参数的校准，提高机器人控制精度。

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

一种基于Nelder-Mead的四轴并联机器人校准方法，所述四轴并联机器人包括一静平台、三主动臂、三从动臂、一动平台、一末端机构、一可伸缩传动轴，其中，静平台与主动臂通过通过电机和减速机连接，主动臂和从动臂通过铰链连接，从动臂与动平台通过铰链连接；

所述校准方法包括以下步骤：

步骤1、构建四轴并联机器人的坐标系，并进行正解计算，得到机器人末端结构位姿；

步骤2、进行误差源分析，并确定需要校准的参数；

步骤3、获取测量位置，计算对应的名义位置；

使用激光跟踪仪采样机器人工作空间下N个点的位置p_T及相对应的关节角θ＝k·q+qero，q为机器人控制的角度，k为减速比修正因子，qero为零位修正因子，

在不考虑结构误差的前提下N组关节角θ，代入步骤1得到的机器人末端结构位姿公式中，获得N个名义位置p_idea；

步骤4、构造误差模型；

名义位置p_idea任意两点间的距离为：

DI＝norm(p_Ii-p_Ij)＝|F(q_i,X)-F(q_j,X)|,i≠j,

对应的测量点任意两点间的距离为：

DT＝norm(p_Ti-p_Tj),i≠j

对应距离的差的绝对值为误差评函数：

f(X)＝RMS(|DI-DT|)＝f(q_i,q_j,X)，X为需要校准的结构参数；

步骤5、利用Nelder-Mead算法校准结构参数X。

所述步骤5具体包括以下步骤：

步骤5.1、激光跟踪仪的采样次数为N，分别对X₀向量的每一个元素增0.01，X_i＝X₁+0.01·eye(N)构造出一个N+1行的初始单纯形，即分别对各校准参数单独增加了一个较小的值0.01，与初始值X₀一起组成了一个N+1行的矩阵，列数为校准参数的个数，初始单纯形即是N+1组校准参数；其中，X₀为初始值，X₁为设计值，X₁＝[qero,k,α,dB,L₁,L₂,Tool]；

步骤5.2、分别求N+1组校准参数的值对应的误差评估函数的值f(X_i)，按从小到大的顺序排列，构造出对应的序列：

Xs(1)...Xs(N),Xs(N+1)

f(Xs(1))...f(Xs(N)),f(Xs(N+1))

步骤5.3、计算前N个点Xs(1)...Xs(N)的中心：Xm＝(∑(X_i))/N。

步骤5.4、计算Xs(N+1)的反射点Xr＝2X_m-Xs(N+1)；

如果反射点的评估函数在最好和最差之间，即：f(Xs(1))≤f(Xr)≤f(Xs(N))，用Xr替代Xs(N+1),并回到步骤5.2，求评估函数的值并按从小到大排序；

如果反射点的评估函数比原序列的最好的值优f(Xr)≤f(Xs(1))，计算拓展点Xe＝Xm+2(Xm-Xs(N+1))；如果拓展点的评估函数比反射点好，即f(Xe)≤f(Xr)，则Xs(N+1)＝Xe；否则，Xs(N+1)＝Xr，回到步骤5.2,；

如果反射点最差f(Xr)≥f(Xs(N+1))，则计算收缩点Xc＝(Xs(N+1)+Xm)/2，比较收缩点与最差点的评估函数值，如果f(Xc)≤f(Xs(N+1))则用收缩点替代最差点Xs(N+1)＝Xc；否则，整体收缩Xs(i)＝(Xs(i)+Xs(1))/2,i≥2，并回到步骤5.2；

如果|f(Xs(1))-f(Xs(N+1))|≤1e-3，则所求的校准参数最优解为Xs(1)。

所述步骤1中，利用空间解析几何求解机器人的正解。

所述步骤1具体如下：

步骤1.1、以静平台中心为原点建立基坐标系O-xyz，主动臂杆长L1_i，从动臂杆长为L2_i，主动臂关节分布角为α_i，世界坐标系O-xyz，位于静平台的中心，u_i为主动臂的单位方向向量，w_i为从动臂的单位方向向量，r为动平台到中心相对于基坐标系原点O的坐标；则有：

r＝b_i+L1_iu_i+L2_iw_i (1)

其中：

步骤1.2、求取主动臂单位向量：令u_i与y重合，u_i与y轴重合，然后u_i绕z轴转动α_i-π/2，再绕y_i转动β_i，然后再绕x'_i轴转动θ_i，在机器人中向下为正，所以代入-θ_i；

步骤1.3、通过移项，将(1)改写为

r-b_i-L1_iu_i＝L2_iw_i (4)

式中：

求式(4)两边的向量的模，由于向量相等，则对应的模相等，同时乘以两向量的转置，得到

(r-b_i-L1_iu_i)^T(r-b_i-L1_iu_i)＝(L2_iw_i)^T(L2_iw_i)

展开化简得到：

将式(5)整理得：

步骤1.4、机器人的三条支链的向量表达方程组为：

公式中：r＝[x,y,z]，即动平台中心的坐标；

步骤1.5、将方程组(7)中的两两(1-2、2-3、1-3)相减，并代入b_i u_i r，得：

令：

g₁＝c₁-c₂

g₂＝c₂-c₃

g₃＝c₁-c₃

步骤1.6、取(9)中的任意两式，得到齐次线性方程组，并解方程组，得到x,y,z；

步骤1.7、机器人法兰盘末端(即末端机构)与动平台的距离为d，则法兰盘中心的位姿可表示为

机器人末端机构位姿，即法兰盘工具末端位姿为：

所述步骤1.6具体如下：

令：

a₁₁＝2(b_1x-b_2x+L1₁u_1x-L1₂u_2x)

a₁₂＝2(b_1y-b_2y+L1₁u_1y-L1₂u_2y)

a₁₃＝2(b_1z-b_2z+L1₁u_1z-L1₂u_2z)

a₂₁＝2(b_2x-b_3x+L1₂u_2x-L1₃u_3x)

a₂₂＝2(b_2y-b_3y+L1₂u_2y-L1₃u_3y)

a₂₃＝2(b_2z-b_3z+L1₂u_2z-L1₃u_3z)

得到齐次线性方程组，

a₁₁x+a₁₂y+a₁₃z＝g₁

a₂₁x+a₂₂y+a₂₃z＝g₂

g₁＝c₁-c₂

g₂＝c₂-c₃

令：

x＝m₂+n₂z

y＝m₁+n₁z

其中：

m₁＝-(a₁₁g₂-a₂₁g₁)/(a₁₂a₂₁-a₁₁a₂₂)

m₂＝-(a₂₂g₁-a₁₂g₂)/(a₁₂a₂₁-a₁₁a₂₂)

n₁＝(a₁₁a₂₃-a₁₃a₂₁)/(a₁₂a₂₁-a₁₁a₂₂)

n₁＝(a₂₂a₁₃-a₁₂a₂₃)/(a₁₂a₂₁-a₁₁a₂₂)

将r代入方程组(7)的第一式：

式中：

x＝m₂+n₂z

y＝m₁+n₁z

将x，y代入(11)化简得到：

(n₁ ²+n₂ ²+1)z²+2{m₂n₂+m₁n₁-(b_1x+L1₁u_1x)n₂-(b_1y+L1₁u_1y)n₁-(b_1z+L1₁u_1z)}z+(m₂ ²+m₁ ²-2(b_1x+L1₁u_1x)m₂-2(b_1y+L1₁u_1y)m₁)+c₁＝0 (12)

令：

k＝(n₁ ²+n₂ ²+1)

e＝2{m₂n₂+m₁n₁-(b_1x+L1₁u_1x)n₂-(b_1y+L1₁u_1y)n₁-(b_1z+L1₁u_1z)}

f＝(m₂ ²+m₁ ²-2(b_1x+L1₁u_1x)m₂-2(b_1y+L1₁u_1y)m₁)+c₁

得到2次方程：

kz²+ez+f＝0 (13)

求解得：

x＝m₂+n₂z

y＝m₁+n₁z

获得动平台中心的坐标r＝(x,y,z)^T。

所述步骤1.2中，β_i＝0。

所述步骤3-5中，N≥50。

采用上述方案后，本发明首先求取机器人的正解，然后充分考虑各支链的长度差异、动静平台的误差、零点差异、工具坐标误差，从而抽象出了多个重要参数，接着使用激光跟踪仪器采样机器人工作空间位置及对应的关节角，使用任意两点间的距离构建评估函数，基于该评估函数，使用Nelder-Mead方法求取最优解，获取嘴角校准参数，提高了机器人控制精度。

此外，本发明在进行机器人正解计算时，使用空间解析几何和代数的方法求解齐次方程组，避免了高次方程求解，在提高控制精度的基础上有效提高了效率。

附图说明

图1为四轴并联机器人的结构示意图；

图2为四轴并联机器人的单轴智联向量图；

图3为本发明流程图；

图4为本发明的校准流程图。

具体实施方式

如图1所示，本发明所应用的四轴并联机器人包括一静平台1、三主动臂4、三从动臂5、一动平台8、一末端机构9、一可伸缩传动轴10，其中，静平台与主动臂通过通过电机3和减速机2连接，主动臂4和从动臂5通过铰链连接，从动臂5与动平台8通过铰链连接。

结合图2-4所示，基于上述结构，本发明揭示了一种基于Nelder-Mead的四轴并联机器人校准方法，其包括以下步骤：

步骤1、构建四轴并联机器人的坐标系，并进行正解计算，得到机器人末端结构位姿。

具体如下：

步骤1.1、以静平台中心为原点建立基坐标系O-xyz，主动臂杆长L1_i，从动臂杆长为L2_i，主动臂关节分布角为α_i，世界坐标系O-xyz，位于静平台的中心，u_i为主动臂的单位方向向量，w_i为从动臂的单位方向向量，r为动平台到中心相对于基坐标系原点O的坐标。则有：

r＝b_i+L1_iu_i+L2_iw_i (1)

其中：

步骤1.2、求取主动臂单位向量：令u_i与y重合，u_i与y轴重合，然后u_i绕z轴转动α_i-π/2，再绕y_i转动β_i，然后再绕x'_i轴转动θ_i，在机器人中向下为正，所以代入-θ_i。在工程实际中β_i的值很小，在正逆解的验证中发现β_i引入后会出现明显误差，为了保证正逆解的可逆性质，β_i可忽略，即β_i＝0。

步骤1.3、通过移项，将(1)改写为

r-b_i-L1_iu_i＝L2_iw_i (4)

式中：

求式(4)两边的向量的模，由于向量相等，则对应的模相等，同时乘以两向量的转置，得到

(r-b_i-L1_iu_i)^T(r-b_i-L1_iu_i)＝(L2_iw_i)^T(L2_iw_i)

展开化简得到：

将式(5)整理得：

步骤1.4、由于机器人有三条支链，三条支链的向量表达方程组为：

公式中：r＝[x,y,z]，即动平台中心的坐标。

步骤1.5、将方程组(7)中的两两(1-2、2-3、1-3)相减可得如下三个公式构成的方程组：

将b_i u_i r代入方程组(8)公式中，合并化简得：

令：

g₁＝c₁-c₂

g₂＝c₂-c₃

g₃＝c₁-c₃

步骤1.6、取(9)中的任意两式，得到齐次线性方程组，并解方程组，得到x,y,z。注意方程组的奇异性问题，与坐标系的建立有关，构建坐标系或者选取9中的公式构建方程组时，避免系数为零，物理上即任意两电机连线的向量与坐标轴平行的情况

具体地，本实施例选取(1、2)构建齐次线性方程，令：

a₁₁＝2(b_1x-b_2x+L1₁u_1x-L1₂u_2x)

a₁₂＝2(b_1y-b_2y+L1₁u_1y-L1₂u_2y)

a₁₃＝2(b_1z-b_2z+L1₁u_1z-L1₂u_2z)

a₂₁＝2(b_2x-b_3x+L1₂u_2x-L1₃u_3x)

a₂₂＝2(b_2y-b_3y+L1₂u_2y-L1₃u_3y)

a₂₃＝2(b_2z-b_3z+L1₂u_2z-L1₃u_3z)

得到齐次线性方程组，

a₁₁x+a₁₂y+a₁₃z＝g₁

a₂₁x+a₂₂y+a₂₃z＝g₂

g₁＝c₁-c₂

g₂＝c₂-c₃

令：

x＝m₂+n₂z

y＝m₁+n₁z

其中：

m₁＝-(a₁₁g₂-a₂₁g₁)/(a₁₂a₂₁-a₁₁a₂₂)

m₂＝-(a₂₂g₁-a₁₂g₂)/(a₁₂a₂₁-a₁₁a₂₂)

n₁＝(a₁₁a₂₃-a₁₃a₂₁)/(a₁₂a₂₁-a₁₁a₂₂)

n₁＝(a₂₂a₁₃-a₁₂a₂₃)/(a₁₂a₂₁-a₁₁a₂₂)

将r代入方程组(7)的第一式：

式中：

x＝m₂+n₂z

y＝m₁+n₁z

将x，y代入(11)化简得到：

(n₁ ²+n₂ ²+1)z²+2{m₂n₂+m₁n₁-(b_1x+L1₁u_1x)n₂-(b_1y+L1₁u_1y)n₁-(b_1z+L1₁u_1z)}z+(m₂ ²+m₁ ²-2(b_1x+L1₁u_1x)m₂-2(b_1y+L1₁u_1y)m₁)+c₁＝0 (12)

令：

k＝(n₁ ²+n₂ ²+1)

e＝2{m₂n₂+m₁n₁-(b_1x+L1₁u_1x)n₂-(b_1y+L1₁u_1y)n₁-(b_1z+L1₁u_1z)}

f＝(m₂ ²+m₁ ²-2(b_1x+L1₁u_1x)m₂-2(b_1y+L1₁u_1y)m₁)+c₁

得到2次方程：

kz²+ez+f＝0 (13)

求解得：

x＝m₂+n₂z

y＝m₁+n₁z

上面的公式中,x,y是关于z的函数，z是关于k的函数，k是关于m,n的函数，m,n是关于u的函数，u是关于θ_i的函数向量，反过来，已知θ_i，可以求u，根据u可以求m,n，通过m,n求z，最后可求x,y。到此，可以通过各主动臂的关节角获得动平台中心的坐标r＝(x,y,z)^T。

步骤1.7、机器人法兰盘末端(即末端机构)与动平台的距离为d，则法兰盘中心的位姿可表示为

机器人末端机构位姿，即法兰盘工具末端位姿为：

步骤2、进行误差源分析，并确定需要校准的参数。

影响Delta并联机构位置精度的几何参数有：

(1)主动臂的长度误差；

(2)从动臂的长度误差；

(3)主动臂分布角α₁；

(4)主动臂铰链位置误差Δbi；

(5)减速机减速比修正k_i；

(5)原点修正qzero_i；

(6)主动臂铰链在静平台上的分布圆半径R；

(7)动平台铰链的分布圆；

(8)工具坐标误差。

支链1主动臂转轴位置在x轴上，则无误差，即Δα₁＝0，主动臂位置误差为0，此外支链2,3的位置分布在同一平面Δb_iz＝0。需要校准的参数如下：

步骤3、获取测量位置，计算对应的名义位置。

使用激光跟踪仪采样机器人工作空间下N个点的位置p_T及相对应的关节角θ＝k·q+qero，q为机器人控制的角度，k为减速比修正因子，qero为零位修正因子，为了获得更加精确的结果，一般取N≥50个点以上；

在不考虑结构误差的前提下N组关节角θ，代入公式(14)可获得N个名义位置p_idea；

步骤4、构造误差模型。

名义位置p_idea任意两点间的距离为：

DI＝norm(p_Ii-p_Ij)＝|F(q_i,X)-F(q_j,X)|,i≠j,

对应的测量点任意两点间的距离为：

DT＝norm(p_Ti-p_Tj),i≠j

对应距离的差的绝对值为误差评函数：

f(X)＝RMS(|DI-DT|)＝f(q_i,q_j,X)，X为需要校准的结构参数，具体如上述参数表中所示。

步骤5、利用Nelder-Mead算法校准结构参数X。

步骤5.1、激光跟踪仪的采样次数为N，分别对X₀向量的每一个元素增0.01，X_i＝X₁+0.01·eye(N)构造出一个N+1行的初始单纯形，即分别对各校准参数单独增加了一个较小的值0.01，与初始值X₀一起组成了一个N+1行的矩阵，列数为校准参数的个数，初始单纯形即是N+1组校准参数；其中，X₀为初始值，X₁为设计值，X₁＝[qero,k,α,dB,L₁,L₂,Tool]。

步骤5.2、分别求N+1组校准参数的值对应的误差评估函数的值f(X_i)，按从小到大的顺序排列，构造出对应的序列：

Xs(1)...Xs(N),Xs(N+1)

f(Xs(1))...f(Xs(N)),f(Xs(N+1))

步骤5.3、计算前N个点Xs(1)...Xs(N)的中心：Xm＝(∑(X_i))/N。

步骤5.4、计算Xs(N+1)的反射点Xr＝2X_m-Xs(N+1)；

如果反射点的评估函数在最好和最差之间，即：f(Xs(1))≤f(Xr)≤f(Xs(N))，用Xr替代Xs(N+1),并回到步骤5.2，求评估函数的值并按从小到大排序；

如果反射点的评估函数比原序列的最好的值优f(Xr)≤f(Xs(1))，计算拓展点Xe＝Xm+2(Xm-Xs(N+1))；如果拓展点的评估函数比反射点好，即f(Xe)≤f(Xr)，则Xs(N+1)＝Xe；否则，Xs(N+1)＝Xr，回到步骤5.2,；

如果反射点最差f(Xr)≥f(Xs(N+1))，则计算收缩点Xc＝(Xs(N+1)+Xm)/2，比较收缩点与最差点的评估函数值，如果f(Xc)≤f(Xs(N+1))则用收缩点替代最差点Xs(N+1)＝Xc；否则，整体收缩Xs(i)＝(Xs(i)+Xs(1))/2,i≥2，并回到步骤5.2；

如果|f(Xs(1))-f(Xs(N+1))|≤1e-3，则所求的校准参数最优解为Xs(1)。

综上，本发明使用空间解析几何和代数的方法获得机器人的正解；并充分考虑了各支链的长度差异、动静平台的误差、零点差异、工具坐标的误差，抽象出了影响机器人精度的多个重要参数；然后使用激光跟踪仪采样机器人工作空间中位置及对应的关节角，使用任意两点间的距离，构建评估函数；最后使用Nelder-Mead方法求最优解，获取最佳校准参数。本发明旨在基于空间解析几何求解机器人运动学正解的基础上，借助Nelder-Mead优化方法实现机器人的结构参数校准，提高机器人的运动控制精度，在工程实践中取得了较为理想的效果。

以上所述，仅是本发明实施例而已，并非对本发明的技术范围作任何限制，故凡是依据本发明的技术实质对以上实施例所作的任何细微修改、等同变化与修饰，均仍属于本发明技术方案的范围内。

Claims (7)

1.基于Nelder-Mead的并联机器人校准方法，其特征在于：所述并联机器人包括一静平台、三主动臂、三从动臂、一动平台、一末端机构、一可伸缩传动轴，其中，静平台与主动臂通过电机和减速机连接，主动臂和从动臂通过铰链连接，从动臂与动平台通过铰链连接；

所述校准方法包括以下步骤：

步骤1、构建四轴并联机器人的坐标系，并进行正解计算，得到机器人末端结构位姿；

步骤2、进行误差源分析，并确定需要校准的参数；

步骤3、获取测量位置，计算对应的名义位置；

使用激光跟踪仪采样机器人工作空间下N个点的位置p_T及相对应的关节角θ＝k·q+qero，q为机器人控制的角度，k为减速比修正因子，qero为零位修正因子，

在不考虑结构误差的前提下N组关节角θ，代入步骤1得到的机器人末端结构位姿公式中，获得N个名义位置p_idea；

步骤4、构造误差模型；

名义位置p_idea任意两点间的距离为：

DI＝norm(p_Ii-p_Ij)＝|F(q_i,X)-F(q_j,X)|,i≠j,

对应的测量点任意两点间的距离为：

DT＝norm(p_Ti-p_Tj),i≠j

对应距离的差的绝对值为误差评函数：

f(X)＝RMS(|DI-DT|)＝f(q_i,q_j,X)，X为需要校准的结构参数；

步骤5、利用Nelder-Mead算法校准结构参数X。

2.根据权利要求1所述的基于Nelder-Mead的并联机器人校准方法，其特征在于：所述步骤5具体包括以下步骤：

步骤5.1、激光跟踪仪的采样次数为N，分别对X₀向量的每一个元素增0.01，X_i＝X₁+0.01·eye(N)构造出一个N+1行的初始单纯形，即分别对各校准参数单独增加了一个较小的值0.01，与初始值X₀一起组成了一个N+1行的矩阵，列数为校准参数的个数，初始单纯形即是N+1组校准参数；其中，X₀为初始值，X₁为设计值，X₁＝[qero,k,α,dB,L₁,L₂,Tool]；

步骤5.2、分别求N+1组校准参数的值对应的误差评估函数的值f(X_i)，按从小到大的顺序排列，构造出对应的序列：

Xs(1)...Xs(N),Xs(N+1)

f(Xs(1))...f(Xs(N)),f(Xs(N+1))

步骤5.3、计算前N个点Xs(1)...Xs(N)的中心：Xm＝(∑(X_i))/N；

步骤5.4、计算Xs(N+1)的反射点Xr＝2X_m-Xs(N+1)；

如果反射点的评估函数在最好和最差之间，即：f(Xs(1))≤f(Xr)≤f(Xs(N))，用Xr替代Xs(N+1),并回到步骤5.2，求评估函数的值并按从小到大排序；

如果反射点的评估函数比原序列的最好的值优f(Xr)≤f(Xs(1))，计算拓展点Xe＝Xm+2(Xm-Xs(N+1))；如果拓展点的评估函数比反射点好，即f(Xe)≤f(Xr)，则Xs(N+1)＝Xe；否则，Xs(N+1)＝Xr，回到步骤5.2,；

如果反射点最差f(Xr)≥f(Xs(N+1))，则计算收缩点Xc＝(Xs(N+1)+Xm)/2，比较收缩点与最差点的评估函数值，如果f(Xc)≤f(Xs(N+1))则用收缩点替代最差点Xs(N+1)＝Xc；否则，整体收缩Xs(i)＝(Xs(i)+Xs(1))/2,i≥2，并回到步骤5.2；

如果|f(Xs(1))-f(Xs(N+1))|≤1e-3，则所求的校准参数最优解为Xs(1)。

3.根据权利要求1所述的基于Nelder-Mead的并联机器人校准方法，其特征在于：所述步骤1中，利用空间解析几何求解机器人的正解。

4.根据权利要求3所述的基于Nelder-Mead的并联机器人校准方法，其特征在于：所述步骤1具体如下：

步骤1.1、以静平台中心为原点建立基坐标系O-xyz，主动臂杆长L1_i，从动臂杆长为L2_i，主动臂关节分布角为α_i，世界坐标系O-xyz，位于静平台的中心，u_i为主动臂的单位方向向量，w_i为从动臂的单位方向向量，r为动平台到中心相对于基坐标系原点O的坐标；则有：

r＝b_i+L1_iu_i+L2_iw_i (1)

其中：

步骤1.2、求取主动臂单位向量：令u_i与y轴重合，然后u_i绕z轴转动α_i-π/2，再绕y_i转动β_i，然后再绕x_i'轴转动θ_i，在机器人中向下为正，所以代入-θ_i；

步骤1.3、通过移项，将(1)改写为

r - b_i - L1_iu_i＝L2_iw_i (4)

式中：

r＝(x,y,z)^T,b_i＝(b_ix,b_iy,b_iz)^T,

求式(4)两边的向量的模，由于向量相等，则对应的模相等，同时乘以两向量的转置，得到

(r-b_i-L1_iu_i)^T(r-b_i-L1_iu_i)＝(L2_iw_i)^T(L2_iw_i)

展开化简得到：

将式(5)整理得：

步骤1.4、机器人的三条支链的向量表达方程组为：

公式中：r＝[x,y,z]，即动平台中心的坐标；

步骤1.5、将方程组(7)中的两两相减，并代入b_i u_i r，得：

令：

g₁＝c₁-c₂

g₂＝c₂-c₃

g₃＝c₁-c₃

步骤1.6、取(9)中的任意两式，得到齐次线性方程组，并解方程组，得到x,y,z；

步骤1.7、机器人法兰盘末端与动平台的距离为d，则法兰盘中心的位姿可表示为

机器人末端机构位姿，即法兰盘工具末端位姿为：

5.根据权利要求4所述的基于Nelder-Mead的并联机器人校准方法，其特征在于：所述步骤1.6具体如下：

令：

a₁₁＝2(b_1x-b_2x+L1₁u_1x-L1₂u_2x)

a₁₂＝2(b_1y-b_2y+L1₁u_1y-L1₂u_2y)

a₁₃＝2(b_1z-b_2z+L1₁u_1z-L1₂u_2z)

a₂₁＝2(b_2x-b_3x+L1₂u_2x-L1₃u_3x)

a₂₂＝2(b_2y-b_3y+L1₂u_2y-L1₃u_3y)

a₂₃＝2(b_2z-b_3z+L1₂u_2z-L1₃u_3z)

得到齐次线性方程组，

a₁₁x+a₁₂y+a₁₃z＝g₁

a₂₁x+a₂₂y+a₂₃z＝g₂

g₁＝c₁-c₂

g₂＝c₂-c₃

令：

x＝m₂+n₂z

y＝m₁+n₁z

其中：

m₁＝-(a₁₁g₂-a₂₁g₁)/(a₁₂a₂₁-a₁₁a₂₂)

m₂＝-(a₂₂g₁-a₁₂g₂)/(a₁₂a₂₁-a₁₁a₂₂)

n₁＝(a₁₁a₂₃-a₁₃a₂₁)/(a₁₂a₂₁-a₁₁a₂₂)

n₁＝(a₂₂a₁₃-a₁₂a₂₃)/(a₁₂a₂₁-a₁₁a₂₂)

将r代入方程组(7)的第一式：

x²+y²+z²-2{(b_1x+L1₁u_1x)x+(b_1y+L1₁u_1y)y+(b_1z+L1₁u_1z)z}+c₁＝0 (11)

式中：

x＝m₂+n₂z

y＝m₁+n₁z

将x，y代入(11)化简得到：

(n₁ ²+n₂ ²+1)z²+2{m₂n₂+m₁n₁-(b_1x+L1₁u_1x)n₂-(b_1y+L1₁u_1y)n₁-(b_1z+L1₁u_1z)}z+(m₂ ²+m₁ ²-2(b_1x+L1₁u_1x)m₂-2(b_1y+L1₁u_1y)m₁)+c₁＝0 (12)

令：

k＝(n₁ ²+n₂ ²+1)

e＝2{m₂n₂+m₁n₁-(b_1x+L1₁u_1x)n₂-(b_1y+L1₁u_1y)n₁-(b_1z+L1₁u_1z)}

f＝(m₂ ²+m₁ ²-2(b_1x+L1₁u_1x)m₂-2(b_1y+L1₁u_1y)m₁)+c₁

得到2次方程：

kz²+ez+f＝0 (13)

求解得：

x＝m₂+n₂z

y＝m₁+n₁z

获得动平台中心的坐标r＝(x,y,z)^T。

6.根据权利要求4所述的基于Nelder-Mead的并联机器人校准方法，其特征在于：所述步骤1.2中，β_i＝0。

7.根据权利要求1所述的基于Nelder-Mead的并联机器人校准方法，其特征在于：所述步骤3中，N≥50。