机械手臂校正系统及机械手臂校正方法与流程

1.本公开是有关于一种校正装置及校正方法，且特别是有关于一种机械手臂校正系统及机械手臂校正方法。


背景技术：

2.近年来机械手臂广泛使用于自动化产业，用以替代人力执行繁琐且高精度的生产及制造程序。为了让机械手臂能够持续精准地动作以避免出错并确保产品良率，业界对于机械手臂的精度及耐用性的要求较高。
3.一般而言，机械手臂在出厂前都会经过精密的调校以确保其精度，但在出厂后经过长时间的操作，机械性的磨耗常导致机械手臂的动作偏移或精度降低。此外，维修过程中的拆装动作也可能造成精度偏差。因此，如何能够在现场(即，产线上)就地校正机械手臂的偏差，避免耗费时间往返原厂维修，并能够确保校正后的精度在制程要求的范围内，实为业界急需解决的问题。


技术实现要素：

4.本公开提供一种机械手臂校正系统与机械手臂校正方法，可有效提升机械手臂操作的稳定性及精度。
5.本公开的机械手臂校正系统包括轨迹追踪装置以及处理装置。轨迹追踪装置用以定位具有至少一个枢轴的机械手臂的末端点在三维空间中的位置。处理装置包括连接装置、存储装置及处理器。连接装置用以连接机械手臂及轨迹追踪装置。存储装置用以存储程序。处理器耦接连接装置及存储装置，且经配置以加载并执行程序以利用轨迹追踪装置所定位的位置，计算器械手臂操作时末端点的实际运动轨迹，撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹，计算每一组粒子的适应运动轨迹与实际运动轨迹的多个位置误差，并用以计算粒子群算法的适应值(fitness value)以评估多组粒子中的群体最佳位置，以及使用群体最佳位置对应的粒子的补偿项更新连杆参数。
6.本公开的机械手臂校正方法，适于由连接轨迹追踪装置的处理装置。此方法包括下列步骤：利用轨迹追踪装置定位具有至少一个枢轴的机械手臂的末端点在三维空间中的位置，以计算器械手臂操作时末端点的实际运动轨迹；利用数据捕获设备撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹；计算每一组粒子的适应运动轨迹与实际运动轨迹的多个位置误差，并用以计算粒子群算法的适应值以评估多组粒子中的群体最佳位置；以及使用群体最佳位置对应的粒子的补偿项更新连杆参数。
7.基于上述，本公开的机械手臂校正系统与机械手臂校正方法利用在机械手臂上装
设轨迹追踪装置，用以量测机械手臂移动的实际运动轨迹，并与通过枢轴马达的转角等连杆参数以顺向运动学所算出来的适应运动轨迹比对，从而通过粒子群算法计算补偿项以调整连杆参数，使得以顺向运动学推算出来的适应运动轨迹与实际运动轨迹符合。因此，可有效提升机械手臂操作的稳定性及精度。
8.为让本公开能更明显易懂，下文特举实施例，并配合附图作详细说明如下。
附图说明
9.图1是依照本公开一实施例所绘示的机械手臂校正系统的方块图。
10.图2是依照本公开一实施例所绘示的轨迹追踪装置的配置示意图。
11.图3是依照本公开一实施例所绘示的机械手臂校正方法的示意图。
12.图4是依照本发明一实施例所绘示的机械手臂校正方法的流程图。
13.图5是依照本公开一实施例所绘示的超宽带定位技术的示意图。
14.图6是依照本公开一实施例所绘示的机械手臂运动轨迹计算方法的流程图。
15.图7是本公开一实施例所绘示的产生连杆参数的补偿项的范例。
16.图8是依照本发明一实施例所绘示的机械手臂校正方法的流程图。
17.符号说明
18.1：机械手臂校正系统
19.10：处理装置
20.12：连接装置
21.14：存储装置
22.16：处理器
23.20：轨迹追踪装置
24.20a：方位指示棒
25.22：实际运动轨迹
26.30：机械手臂
27.32：适应运动轨迹
28.an1～anx：超宽带基站
29.x、y、z：方位
30.坐标轴向
31.补偿项
32.a2、a3：连杆长度
33.d3、d4：连杆距离
34.uwb：超宽带信号
35.s1～s4、s602～s608、s802～s812：步骤
具体实施方式
36.本专利提出一种可提高机械手臂操作精度的校正系统及校正方法。此方法是在机械手臂上装配一套轨迹追踪装置去撷取手臂实际运动轨迹，并将机械手臂本身的d-h(denavit-hartenberg)参数(即，后述的连杆参数)加上一个补偿项后导入粒子群优化
(particle swarm optimization，pso)算法进行学习，而迭代出d-h参数的合适补偿值来调整d-h参数，因此使得经由顺向运动学所计算出来的适应运动轨迹能更贴近实际运动轨迹，从而提升机械手臂操作的稳定性及精度。
37.图1是依照本公开一实施例所绘示的机械手臂校正系统的方块图。请参照图1，本实施例的机械手臂校正系统1例如包括处理装置10及轨迹追踪装置20，其功能分述如下：
38.处理装置10例如是具备运算功能的个人计算机、服务器、工作站或其他装置，其例如包括连接装置12、存储装置14及处理器16。其中，连接装置12例如是通用串行总线(usb)、rs232、火线(firewire)、雷雳(thunderbolt)、蓝牙(bluetooth，bt)、无线保真(wireless fidelity，wi-fi)等有线或无线的连接装置或接口，而用以与机械手臂(未绘示)、轨迹追踪装置20等外部装置连接，并传送数据。存储装置14例如是任何型态的固定式或可移动式随机存取存储器(random access memory，ram)、只读存储器(read-only memory，rom)、闪存(flash memory)、硬盘或类似组件或上述组件的组合，而用以存储可由处理器16执行的程序。在一些实施例中，存储装置14例如还存储连接装置12自外部装置接收的数据。处理器16耦接连接装置12与存储装置14，其例如是中央处理器(central processing unit，cpu)，或是其他可编程的一般用途或特殊用途的微处理器(microprocessor)、数字信号处理器(digital signal processor，dsp)、可编程控制器、专用集成电路(application specific integrated circuit，asic)、可编程逻辑控制器(programmable logic controller，plc)或其他类似装置或这些装置的组合，而可加载并执行存储装置14中存储的程序，以执行本公开实施例的机械手臂校正方法。
39.轨迹追踪装置20例如是装设于机械手臂的末端点，而用以定位机械手臂的末端点在三维空间中的位置。轨迹追踪装置20例如是由惯性测量单元(inertial measurement unit，imu)与超宽带(ultra-wideband，uwb)定位装置组合而成，其中包括利用惯性测量单元进行轨迹追踪装置20的姿态检测，以及利用超宽带定位装置定义轨迹追踪装置20在三维空间中的位置。
40.在一些实施例中，惯性测量单元可检测机械手臂的末端点在三维空间中的位移及方位，而由处理装置10的处理器16融合惯性测量单元所检测的位移及方位，计算出轨迹追踪装置20在三维空间中的姿态。
41.在一些实施例中，超宽带定位装置可检测周围多个超宽带基站所发出的超宽带信号，而由处理装置10的处理器16根据超宽带信号的强度，估测轨迹追踪装置20与各个超宽带基站的距离，据以计算出轨迹追踪装置20在三维空间中的位置。
42.图2是依照本公开一实施例所绘示的轨迹追踪装置的配置示意图。请参照图2，本实施例的轨迹追踪装置20架设于机械手臂30的末端点，其包括配置于末端的方位指示棒20a，以协助定义轴向(例如z轴方向)。其中，轨迹追踪装置20需装设于机械手臂30的法兰面上，其组装过程中产生的误差及偏移需先经过校正，且其坐标系与机械手臂30的坐标系也需要校正。
43.在一些实施例中，例如可通过在轨迹追踪装置20的周围架设双轴红外线传感器(未绘示)，而通过校正算法计算并校正轨迹追踪装置20的倾斜量与位置。举例来说，可控制机械手臂30将轨迹追踪装置20移动至传感器中间，以通过量测一轴两点的位置找出斜率来补偿轨迹追踪装置20的倾斜量，且可控制机械手臂30绕圆以找出圆心来补偿位置偏移。此
外，可计算轨迹追踪装置20的坐标与机械手臂30的坐标之间的转置矩阵，以初始量测点的归零动作，而开始追踪轨迹追踪装置20的移动轨迹。
44.图3是依照本公开一实施例所绘示的机械手臂校正方法的示意图。请同时参照图1、图2及图3，本实施例绘示本公开的机械手臂校正方法的概念，其中机械手臂30例如是经控制以预设的轨迹移动。而基于轨迹追踪装置20的定位结果，处理装置10例如会在步骤s1中计算出机械手臂30末端点的实际运动轨迹22，并根据从机械手臂30的各枢轴马达的编码器取得的关节转角以及机械手臂30中各个连杆的连杆扭角、连杆距离及连杆长度(即，连杆参数)，加上由粒子群算法随机产生的补偿项后，在步骤s2中利用顺向运动学计算出机械手臂30末端点的适应运动轨迹32。在步骤s3中，处理装置10例如会计算实际运动轨迹22与适应运动轨迹32的多个位置误差，从而在步骤s4中，将所计算的位置误差导入粒子群算法，以迭代出机械手臂30的连杆参数的补偿项(其中，各个粒子pi可通过迭代方式移动至接近目标位置t的最佳位置)。最后，通过使用补偿项更新连杆参数，而完成机械手臂30的任务轨迹精度的提升。
45.图4是依照本发明一实施例所绘示的机械手臂校正方法的流程图。请同时参照图1、图3及图4，本实施例的方法适用于上述的机械手臂校正系统1，以下即搭配机械手臂校正系统10的各项装置或组件说明本实施例的机械手臂校正方法的详细步骤。
46.在步骤s402中，由处理装置10的处理器16利用轨迹追踪装置20定位机械手臂30的末端点在三维空间中的位置，以计算器械手臂30操作时末端点的实际运动轨迹22。
47.在一些实施例中，轨迹追踪装置20是结合惯性测量单元与超宽带定位装置来获取机械手臂30的末端点的实际运动轨迹22。惯性测量单元例如是由三轴加速度计、三轴陀螺仪与三轴磁力计组合，其可检测机械手臂30的末端点在三维空间中的位移及方位，而通过传感器融合(sensor fusion)技术，则可将惯性测量单元所检测的位移及方位融合，从而计算出轨迹追踪装置在三维空间中的姿态。其中，加速度计可利用感测到的各个轴向的受力情况来得到该轴向的加速度大小和方向，因此可计算位移。陀螺仪是用以测量三维坐标系与传感器之间的夹角以计算角速度，并通过夹角和角速度来判别物体在三维空间的运动状态，因此可测量自身的旋转运动。磁力计是用以测量当前设备与东南西北四个方向上的夹角以获得实际方位。
48.超宽带定位装置可基于数个超宽带基站所发射的超宽带信号计算其与各个超宽带基站的相对位置，进而推算出轨迹追踪装置的位置。其中，超宽带无线通信是采用时间间隔极短(小于1奈秒)的脉冲进行通信，其具有穿透力强、功耗低、抗多径效果、人体危害低、安全性高、定位精确等优点。超宽带定位技术采用的是到达时间差(time difference of arrival，tdoa)定位算法，通过在室内设置已知位置的超宽带基站，并在欲定位的设备上设置定位标签，此标签可按照一定的频率发射脉冲，而不断地和已知位置的超宽带基站进行测距。通过测量不同超宽带基站与移动终端的传输时延差来进行定位，从而可通过定位算法精确地计算出定位标签所在的位置。
49.举例来说，图5是依照本公开一实施例所绘示的超宽带定位技术的示意图。请参照图5，本实施例例如是在轨迹追踪装置20周围装设多个超宽带基站an1～anx，其中x为正整数。轨迹追踪装置20上的超宽带定位装置可检测超宽带基站an1～anx所发出的超宽带信号uwb，从而根据超宽带信号的强度，估测轨迹追踪装置20与各个超宽带基站an1～anx的距
离，并据以计算出轨迹追踪装置20在三维空间中的位置。所述轨迹追踪装置20的位置可经由前述实施例中所计算的轨迹追踪装置20的坐标与机械手臂30的坐标之间的转置矩阵，换算为机械手臂30的末端点的位置。
50.图6是依照本公开一实施例所绘示的机械手臂运动轨迹计算方法的流程图。请同时参照图1及图6，本实施例说明上述的轨迹追踪装置20如何结合惯性测量单元与超宽带定位装置来获取机械手臂末端点的实际运动轨迹，以下即搭配机械手臂校正系统10的各项装置或组件说明本实施例的机械手臂校正方法的详细步骤。
51.在步骤s602中，由处理装置10的处理器16建立基于卡尔曼滤波器(kalman filter)的系统模型。此系统模型例如表示机械手臂在三维空间中的运动状态及所观测到的量测状态。
52.举例来说，假设系统状态为x∈rn，可观测到的量测状态为z∈rm，其中m、n为空间维度，常以正整数表示，则机械手臂的系统模型可表示为：
[0053][0054]
zk＝hxk+vk[0055]
其中，f为n
×
n的矩阵，用来表示不含过程噪声与控制输入的系统，连接前一时刻k-1与目前时刻k的系统状态。b为n
×
1的矩阵，用来表示控制输入u∈r1进入系统的方向。h为m
×
n的矩阵，用来将系统状态x映射到量测状态z。w与v分别是过程噪声与量测噪声。假设噪声来源之间互相独立且符合平均值为零的常态分布，其对应的协方差矩阵为q(协方差矩阵)和r(观测噪声的协方差矩阵)。
[0056]
在步骤s604中，处理器16将惯性测量单元及超宽带定位装置所量测到的多个量测值作为当前状态xk(机械手臂末端点在三维空间中的目前位置)使用系统模型估测下一状态x
k+1
(其中，x
k+1
＝fxk，机械手臂末端点在三维空间中的下一时间位置)，并计算预测误差协方差p
k+1
(下一时间噪声预测)及增益值k
k+1
(下一时间的卡尔曼增益)，其公式如下：
[0057]
p
k+1
＝fp
k+1ft
+q
[0058]sk+1
＝r+hp
k+1ht
[0059][0060]
其中，s
k+1
为量测余量的协方差。
[0061]
在步骤s606中，处理器16计算所估测的下一状态的多个量测值z
k+1
与估测值之间的预测误差ε
k+1
，其公式如下：
[0062][0063]
在步骤s608中，处理器16计算利用所计算的预测误差ε
k+1
及增益值k
k+1
修正估测值x
k+1
，而获得修正后估测值x
′
k+1
，用以作为机械手臂的末端点在三维空间中的位置，其公式如下：
[0064]
x
′
k+1
＝x
k+1
+k
k+1
ε
k+1
[0065]
在一些实施例中，上述的增益值k
k+1
与协方差s
k+1
可用以更新预测误差协方差p
k+1
。
处理器16可通过重复执行步骤s604～s608，持续预测机械手臂末端点在下一状态中的位置，并利用这些预测位置生成机械手臂末端点的实际运动轨迹，此实际运动轨迹可与后述通过顺向运动学所算出的适配运动轨迹比对以计算误差，最终通过将该误差导入粒子群算法而求得机械手臂30的连杆参数的补偿值。所述的更新预测误差协方差p
k+1
的公式如下：
[0066]
p
′
k+1
＝p
k+1
+k
k+1sk+1
p
k+1t
[0067]
回到图4的流程，在步骤s404中，由处理器16撷取机械手臂30的多个连杆参数，并利用粒子群算法随机产生包括这些连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹。
[0068]
举例来说，图7是本公开一实施例所绘示的产生连杆参数的补偿项的范例。请参照图7，本实施例的机械手臂70具备六个枢轴，而通过枢轴的转动以改变其运动姿态，其中表示第一轴的坐标轴向，表示第一轴的坐标轴向，表示第二轴的坐标轴向，以此类推。下表一列示机械手臂70的d-h参数表，其中包括连杆扭角(twist angle)α
i-1
、连杆长度a
i-1
、连杆距离di及关节转角(joint angle)θi。本实施例是利用粒子群算法随机产生包括下列10个补偿项的粒子，从而将这些补偿项加上对应的连杆参数后导入顺向运动学，以计算器械手臂70的末端点的适应运动轨迹。
[0069][0070]
表一
[0071]
详细而言，在顺向运动学中，机械手臂70的各轴的空间转换矩阵ai(其中i＝1～6)可以下式计算出：
[0072][0073]
据此，机械手臂70的末端点的位置t可以下式计算出：
[0074]
t＝a1·
a2·
a3·
a4·
a5·
a6[0075]
通过持续计算器械手臂70的末端点的位置t，即可生成机械手臂70的末端点的适应运动轨迹。
[0076]
需说明的是，在本实施例中，对应于机械手臂70产生了10个补偿项但在
其他实施例中，对应于其他种类的机械手臂，则可依实际需要对其连杆参数设置不同数量的补偿项，本实施例并不限制补偿项的数量及设置方式。
[0077]
在步骤s406中，由处理器16计算每一组粒子的适应运动轨迹与实际运动轨迹(例如由前述图6实施例所算出的实际运动轨迹)的多个位置误差，并用以计算粒子群算法的适应值，以评估多组粒子中的群体最佳位置。其中，处理器16例如是计算每一组粒子的多个位置误差的均方根值作为适应值，但不限于此。
[0078]
详细而言，粒子群算法通过设计一种无质量的粒子来模拟鸟群中的鸟，这些粒子仅具有速度和位置两个属性，其中的速度代表移动的快慢，位置代表移动的方向。每个粒子都有一个由目标函数决定的适应值，并且会记录本身到目前为止经历过的最好位置(pbest)和现在位置，此记录可视为粒子本身的飞行经验。除此之外，每个粒子还记录有到目前为止整个群体中所有粒子经历过的最好位置(gbest，即所有pbest中的最佳值)，此记录可视为粒子同伴的经验。在粒子群算法中，每个粒子就是通过本身和同伴的经验来决定下一步的运动。
[0079]
图8是依照本发明一实施例所绘示的机械手臂校正方法的流程图。请同时参照图1及图8，本实施例的方法适用于上述的机械手臂校正系统10，以下即搭配机械手臂校正系统10的各项装置或组件说明本实施例的机械手臂校正方法的详细步骤。
[0080]
在步骤s802中，由处理装置10的处理器16计算每一组粒子当前的适应值。举例来说，假设q为机械手臂运动过程中所撷取的连杆参数，x为经由上述卡尔曼滤波器计算所得的实际位置，所述的连杆参数q经由顺向运动学的函数f计算后，可获得适应位置f(q)。
[0081]
假设机械手臂的运动轨迹包括1000笔数据，处理器16例如产生50组粒子，其中每个粒子包含10个补偿项，则利用第一组粒子可算出1000笔适应位置f(q)，并进一步算出实际位置x与此适应位置f(q)的位置误差e，其中e＝x-f(q)。处理器16通过计算这1000笔位置误差的均方根值rmse，以作为该组粒子的适应值。其中，第n组粒子的均方根值rmse的计算公式如下：
[0082][0083]
其中，k为数据笔数，n为迭代次数。依此类推，50组粒子将产生50组适应值。
[0084]
在步骤s804中，由处理器16根据每一组粒子的当前及过往的适应值，评估该组粒子的个体最佳位置(pbest)。在一些实施例中，处理器16例如是选择适应值最小的粒子作为个体最佳位置。
[0085]
在步骤s806中，由处理器16选择多组粒子的个体最佳位置中的最佳者作为群体最佳位置。例如，从上述50组粒子的个体最佳位置(pbest)中选择最佳解(例如适应值最小者min(rmse))作为群体最佳位置(gbest)。
[0086]
在步骤s808中，由处理器16判断是否满足目标条件。其中，所述目标条件例如是所计算的适应值小于预设阈值或迭代次数达到预设次数(例如100～1000中的任意值)。
[0087]
若未满足目标条件，则在步骤s810中，由处理器16利用个体最佳位置及群体最佳位置更新各组粒子的位置与速度，并回到步骤s802，重新计算各组粒子的适应值。
[0088]
例如，处理器16可将各组粒子的当前位置p(k)与当前速度v(k)更新为位置p(k+1)和速度v(k+1)，其计算公式如下：
[0089]
v(k+1)＝wv(k)+c1rand()
·
(pbest-p(k))+c2rand()
·
(gbest-p(k))
[0090]
p(k+1)＝p(k)+v(k+1)
[0091]
其中，w为惯性权重(inertiaweight)，c1和为个体经验权重，c2为群体经验权重，rand()和rand()为两个在[0，1]范围中变化的随机值。
[0092]
处理器16将会重复上述步骤s802～s810，直到满足目标条件时，进入步骤s812，以决定最终的群体最佳位置。此群体最佳位置即可用以作为上述步骤s406中由处理器16所评估的群体最佳位置。
[0093]
最后，在步骤s408中，由处理器16使用群体最佳位置对应的粒子的补偿项更新连杆参数。即，处理器16可将机械手臂的d-h参数表中的每个d-h参数加上经由粒子群算法所算出的最佳补偿项，从而更新d-h参数表。机械手臂在接收任务指令时，即可参照此更新d-h参数表执行动作。因此，机械手臂的任务轨迹精度即可获得提升。
[0094]
综上所述，本公开的机械手臂校正系统与机械手臂校正方法利用配置于机械手臂末端点的轨迹追踪装置通过融合惯性测量单元与超宽带定位装置的信号，而计算出末端点的实际运动轨迹，并将此实际运动轨迹与通过机械手臂各轴马达参数值经由顺向运动学公式所计算出来的适应运动轨迹比对以算出两者的误差量，并将此误差量经由强健的粒子群算法计算出机械手臂的d-h参数的补偿项以更新d-h参数。因此，可修正机械手臂长时间运动所累计的误差，并提高轨迹精度。
[0095]
然本公开已以实施例揭示如上，然其并非用以限定本公开，任何本领域技术人员，在不脱离本公开的精神和范围内，当可作些许的更动与润饰，因此本公开的保护范围当视后附的权利要求及其均等范围所界定的为准。