CN114571465A - 一种基于模拟退火算法的四轴并联机器人校准方法 - Google Patents

Abstract

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

Description

一种基于模拟退火算法的四轴并联机器人校准方法

技术领域

本发明涉及机器人控制领域，具体涉及一种基于模拟退火算法的四轴Delta并联机器人校准方法。

背景技术

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

发明内容

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

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

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

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

步骤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、利用模拟退火算法获取最佳校准结构参数X。

所述步骤5具体如下：

在开始迭代前X为设计值，X＝[qero,k,α,dB,L₁,L₂,Tool]，在后续的迭代中不断的修正；

步骤5.1、需要校准的参数个数为n＝24，根据设计参数和激光跟踪仪采集到的机器人位姿及对应的关节角q，计算初始评估函数f(X)；根据实际要求设置马尔科夫链的长度Lk，马尔科夫链的迭代计数器k＝1；设置总迭代次数N，退火初始温度T0，退火终止温度Tend，降温系数dT，记录第一次马尔科夫迭代中结构参数及其误差评估函数：temp＝X和tempf＝f(X)；

步骤5.2、对初始值X添加扰动，得到新的结构参数X(k)＝X+0.001·(rand(1,n)-0.5)，计算初始结构参数和添加扰动后新的结构参数的误差评估函数；

步骤5.3、计算步骤步骤5.2中两误差评估函数其差值：Δf＝f(X(k))-f(X)；

如果Δf≤0，接受添加扰动后的结构参数：X＝X(k),f(X)＝f(X(k))；

如果Δf≥0，按照Metropolis准则，若：

接受添加扰动后的结构参数：X＝X(k),f(X)＝f(X(k))，否则，保留未添加扰动的结构参数；

步骤5.4、如果f(X)≤tempf，更新记录马尔科夫链内的更优的结构参数：temp＝X和tempf＝f(X)，马尔科夫链内迭代计数k加1；

步骤5.5、如果马尔科夫迭代次数小于链的长度：k≤Lk，回到步骤步骤5.2；否则，马儿科夫链迭代结束，总迭代计数ite加1，温度降低，即：T0＝dT*T0，ite＝ite+1；

步骤5.6、如果未降温到结束温度且迭代次数未到，即：T0≤Tend且ite≤N，设置k＝1，更新结构参数初始值X＝temp，重新开始步骤5.2进入迭代；否则，退出总迭代，最优结构参数即为：X＝temp。

所述步骤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代入方程组(8)公式中，合并化简得：

令：

g₁＝c₁-c₂

g₂＝c₂-c₃

g₃＝c₁-c₃

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

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

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

所述步骤1.6具体如下：

选取式(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)化简得到：

令：

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。

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

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

附图说明

图1为Delta机器人的结构示意图；

图2为机器人的支链向量图；

图3为构建误差函数流程图；

图4为模拟退火方法校准参数流程图。

具体实施方式

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

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

步骤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)化简得到：

令：

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；

(6)原点修正qzero_i；

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

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

(9)工具坐标误差。

支链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、利用模拟退火算法获取最佳校准结构参数X。

在开始迭代前X为设计值X＝[qero,k,α,dB,L₁,L₂,Tool]，在后续的迭代中不断的修正。

步骤5.1、需要校准的参数个数为n(n＝24)，根据设计参数和激光跟踪仪采集到的机器人位姿及对应的关节角q，计算初始评估函数f(X)；根据实际要求设置马尔科夫链的长度Lk(取求解规模乘以1000以上)，马尔科夫链的迭代计数器k＝1；设置总迭代次数N，退火初始温度T0(1e9)，退火终止温度Tend(1e-3)，降温系数dT(0.95)，记录第一次马尔科夫迭代中结构参数及其误差评估函数：temp＝X和tempf＝f(X)。

步骤5.2、对初始值X添加扰动，得到新的结构参数X(k)＝X+0.001·(rand(1,n)-0.5)，计算初始结构参数和添加扰动后新的结构参数的误差评估函数。

步骤5.3、计算步骤步骤5.2中两误差评估函数其差值：Δf＝f(X(k))-f(X)；

如果Δf≤0，接受添加扰动后的结构参数：X＝X(k),f(X)＝f(X(k))；

如果Δf≥0，按照Metropolis准则，若：

接受添加扰动后的结构参数：X＝X(k),f(X)＝f(X(k))，否则，保留未添加扰动的结构参数。

步骤5.4、如果f(X)≤tempf，更新记录马尔科夫链内的更优的结构参数：temp＝X和tempf＝f(X)，马尔科夫链内迭代计数k加1；

步骤5.5、如果马尔科夫迭代次数小于链的长度：k≤Lk，回到步骤步骤5.2；否则，马儿科夫链迭代结束，总迭代计数ite加1，温度降低，即：T0＝dT*T0，ite＝ite+1；

步骤5.6、如果未降温到结束温度且迭代次数未到，即：T0≤Tend且ite≤N，设置k＝1，更新结构参数初始值X＝temp，重新开始步骤5.2进入迭代；否则，退出总迭代，最优结构参数即为：X＝temp。

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

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

Claims (7)

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

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

步骤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、利用模拟退火算法获取最佳校准结构参数X。

2.根据权利要求1所述的一种基于模拟退火算法的四轴Delta并联机器人校准方法，其特征在于：所述步骤5具体如下：

在开始迭代前X为设计值，X＝[qero,k,α,dB,L₁,L₂,Tool]，在后续的迭代中不断的修正；

步骤5.1、需要校准的参数个数为n＝24，根据设计参数和激光跟踪仪采集到的机器人位姿及对应的关节角q，计算初始评估函数f(X)；根据实际要求设置马尔科夫链的长度Lk，马尔科夫链的迭代计数器k＝1；设置总迭代次数N，退火初始温度T0，退火终止温度Tend，降温系数dT，记录第一次马尔科夫迭代中结构参数及其误差评估函数：temp＝X和tempf＝f(X)；

步骤5.2、对初始值X添加扰动，得到新的结构参数X(k)＝X+0.001·(rand(1,n)-0.5)，计算初始结构参数和添加扰动后新的结构参数的误差评估函数；

步骤5.3、计算步骤步骤5.2中两误差评估函数其差值：Δf＝f(X(k))-f(X)；

如果Δf≤0，接受添加扰动后的结构参数：X＝X(k),f(X)＝f(X(k))；

如果Δf≥0，按照Metropolis准则，若：

接受添加扰动后的结构参数：X＝X(k),f(X)＝f(X(k))，否则，保留未添加扰动的结构参数；

步骤5.4、如果f(X)≤tempf，更新记录马尔科夫链内的更优的结构参数：temp＝X和tempf＝f(X)，马尔科夫链内迭代计数k加1；

步骤5.5、如果马尔科夫迭代次数小于链的长度：k≤Lk，回到步骤步骤5.2；否则，马儿科夫链迭代结束，总迭代计数ite加1，温度降低，即：T0＝dT*T0，ite＝ite+1；

步骤5.6、如果未降温到结束温度且迭代次数未到，即：T0≤Tend且ite≤N，设置k＝1，更新结构参数初始值X＝temp，重新开始步骤5.2进入迭代；否则，退出总迭代，最优结构参数即为：X＝temp。

3.根据权利要求1所述的一种基于模拟退火算法的四轴Delta并联机器人校准方法，其特征在于：所述步骤1中，利用空间解析几何求解机器人的正解。

4.根据权利要求3所述的一种基于模拟退火算法的四轴Delta并联机器人校准方法，其特征在于：所述步骤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代入方程组(8)公式中，合并化简得：

令：

g₁＝c₁-c₂

g₂＝c₂-c₃

g₃＝c₁-c₃

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

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

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

5.根据权利要求4所述的一种基于模拟退火算法的四轴Delta并联机器人校准方法，其特征在于：所述步骤1.6具体如下：

选取式(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

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

6.根据权利要求4所述的一种基于模拟退火算法的四轴Delta并联机器人校准方法，其特征在于：所述步骤1.2中，β_i＝0。

7.根据权利要求1所述的一种基于模拟退火算法的四轴Delta并联机器人校准方法，其特征在于：所述步骤3-5中，N≥50。

Images