CN114683259A - 机械手臂校正系统及机械手臂校正方法 - Google Patents

机械手臂校正系统及机械手臂校正方法 Download PDF

Info

Publication number
CN114683259A
CN114683259A CN202011579295.4A CN202011579295A CN114683259A CN 114683259 A CN114683259 A CN 114683259A CN 202011579295 A CN202011579295 A CN 202011579295A CN 114683259 A CN114683259 A CN 114683259A
Authority
CN
China
Prior art keywords
particles
tracking device
end point
dimensional space
calculating
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202011579295.4A
Other languages
English (en)
Inventor
江俊亿
陈衍成
张仲尹
苏冠玮
杨棨证
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Industrial Technology Research Institute ITRI
Original Assignee
Industrial Technology Research Institute ITRI
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Industrial Technology Research Institute ITRI filed Critical Industrial Technology Research Institute ITRI
Priority to CN202011579295.4A priority Critical patent/CN114683259A/zh
Publication of CN114683259A publication Critical patent/CN114683259A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1602—Programme controls characterised by the control system, structure, architecture
    • B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor

Abstract

一种机械手臂校正系统与机械手臂校正方法。此方法包括下列步骤：定位机械手臂的末端点在三维空间中的位置，以计算器械手臂操作时末端点的实际运动轨迹；撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹；计算每一组粒子的适应运动轨迹与实际运动轨迹的位置误差，并用以计算粒子群算法的适应值以评估群体最佳位置；以及使用群体最佳位置对应的粒子的补偿项更新连杆参数。

Description

机械手臂校正系统及机械手臂校正方法
技术领域
本公开是有关于一种校正装置及校正方法，且特别是有关于一种机械手臂校正系统及机械手臂校正方法。
背景技术
近年来机械手臂广泛使用于自动化产业，用以替代人力执行繁琐且高精度的生产及制造程序。为了让机械手臂能够持续精准地动作以避免出错并确保产品良率，业界对于机械手臂的精度及耐用性的要求较高。
一般而言，机械手臂在出厂前都会经过精密的调校以确保其精度，但在出厂后经过长时间的操作，机械性的磨耗常导致机械手臂的动作偏移或精度降低。此外，维修过程中的拆装动作也可能造成精度偏差。因此，如何能够在现场(即，产线上)就地校正机械手臂的偏差，避免耗费时间往返原厂维修，并能够确保校正后的精度在制程要求的范围内，实为业界急需解决的问题。
发明内容
本公开提供一种机械手臂校正系统与机械手臂校正方法，可有效提升机械手臂操作的稳定性及精度。
本公开的机械手臂校正系统包括轨迹追踪装置以及处理装置。轨迹追踪装置用以定位具有至少一个枢轴的机械手臂的末端点在三维空间中的位置。处理装置包括连接装置、存储装置及处理器。连接装置用以连接机械手臂及轨迹追踪装置。存储装置用以存储程序。处理器耦接连接装置及存储装置，且经配置以加载并执行程序以利用轨迹追踪装置所定位的位置，计算器械手臂操作时末端点的实际运动轨迹，撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹，计算每一组粒子的适应运动轨迹与实际运动轨迹的多个位置误差，并用以计算粒子群算法的适应值(Fitness value)以评估多组粒子中的群体最佳位置，以及使用群体最佳位置对应的粒子的补偿项更新连杆参数。
本公开的机械手臂校正方法，适于由连接轨迹追踪装置的处理装置。此方法包括下列步骤：利用轨迹追踪装置定位具有至少一个枢轴的机械手臂的末端点在三维空间中的位置，以计算器械手臂操作时末端点的实际运动轨迹；利用数据捕获设备撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹；计算每一组粒子的适应运动轨迹与实际运动轨迹的多个位置误差，并用以计算粒子群算法的适应值以评估多组粒子中的群体最佳位置；以及使用群体最佳位置对应的粒子的补偿项更新连杆参数。
基于上述，本公开的机械手臂校正系统与机械手臂校正方法利用在机械手臂上装设轨迹追踪装置，用以量测机械手臂移动的实际运动轨迹，并与通过枢轴马达的转角等连杆参数以顺向运动学所算出来的适应运动轨迹比对，从而通过粒子群算法计算补偿项以调整连杆参数，使得以顺向运动学推算出来的适应运动轨迹与实际运动轨迹符合。因此，可有效提升机械手臂操作的稳定性及精度。
为让本公开能更明显易懂，下文特举实施例，并配合附图作详细说明如下。
附图说明
图1是依照本公开一实施例所绘示的机械手臂校正系统的方块图。
图2是依照本公开一实施例所绘示的轨迹追踪装置的配置示意图。
图3是依照本公开一实施例所绘示的机械手臂校正方法的示意图。
图4是依照本发明一实施例所绘示的机械手臂校正方法的流程图。
图5是依照本公开一实施例所绘示的超宽带定位技术的示意图。
图6是依照本公开一实施例所绘示的机械手臂运动轨迹计算方法的流程图。
图7是本公开一实施例所绘示的产生连杆参数的补偿项的范例。
图8是依照本发明一实施例所绘示的机械手臂校正方法的流程图。
符号说明
1：机械手臂校正系统
10：处理装置
12：连接装置
14：存储装置
16：处理器
20：轨迹追踪装置
20a：方位指示棒
22：实际运动轨迹
30：机械手臂
32：适应运动轨迹
AN1～ANx：超宽带基站
X、Y、Z：方位
Figure BDA0002865491170000032
坐标轴向
Figure BDA0002865491170000031
补偿项
a2、a3：连杆长度
d3、d4：连杆距离
UWB：超宽带信号
S1～S4、S602～S608、S802～S812：步骤
具体实施方式
本专利提出一种可提高机械手臂操作精度的校正系统及校正方法。此方法是在机械手臂上装配一套轨迹追踪装置去撷取手臂实际运动轨迹，并将机械手臂本身的D-H(Denavit-Hartenberg)参数(即，后述的连杆参数)加上一个补偿项后导入粒子群优化(Particle swarm optimization，PSO)算法进行学习，而迭代出D-H参数的合适补偿值来调整D-H参数，因此使得经由顺向运动学所计算出来的适应运动轨迹能更贴近实际运动轨迹，从而提升机械手臂操作的稳定性及精度。
图1是依照本公开一实施例所绘示的机械手臂校正系统的方块图。请参照图1，本实施例的机械手臂校正系统1例如包括处理装置10及轨迹追踪装置20，其功能分述如下：
处理装置10例如是具备运算功能的个人计算机、服务器、工作站或其他装置，其例如包括连接装置12、存储装置14及处理器16。其中，连接装置12例如是通用串行总线(USB)、RS232、火线(Firewire)、雷雳(Thunderbolt)、蓝牙(Bluetooth，BT)、无线保真(Wirelessfidelity，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 SpecificIntegrated Circuit，ASIC)、可编程逻辑控制器(Programmable Logic Controller，PLC)或其他类似装置或这些装置的组合，而可加载并执行存储装置14中存储的程序，以执行本公开实施例的机械手臂校正方法。
轨迹追踪装置20例如是装设于机械手臂的末端点，而用以定位机械手臂的末端点在三维空间中的位置。轨迹追踪装置20例如是由惯性测量单元(Inertial measurementunit，IMU)与超宽带(Ultra-wideband，UWB)定位装置组合而成，其中包括利用惯性测量单元进行轨迹追踪装置20的姿态检测，以及利用超宽带定位装置定义轨迹追踪装置20在三维空间中的位置。
在一些实施例中，惯性测量单元可检测机械手臂的末端点在三维空间中的位移及方位，而由处理装置10的处理器16融合惯性测量单元所检测的位移及方位，计算出轨迹追踪装置20在三维空间中的姿态。
在一些实施例中，超宽带定位装置可检测周围多个超宽带基站所发出的超宽带信号，而由处理装置10的处理器16根据超宽带信号的强度，估测轨迹追踪装置20与各个超宽带基站的距离，据以计算出轨迹追踪装置20在三维空间中的位置。
图2是依照本公开一实施例所绘示的轨迹追踪装置的配置示意图。请参照图2，本实施例的轨迹追踪装置20架设于机械手臂30的末端点，其包括配置于末端的方位指示棒20a，以协助定义轴向(例如Z轴方向)。其中，轨迹追踪装置20需装设于机械手臂30的法兰面上，其组装过程中产生的误差及偏移需先经过校正，且其坐标系与机械手臂30的坐标系也需要校正。
在一些实施例中，例如可通过在轨迹追踪装置20的周围架设双轴红外线传感器(未绘示)，而通过校正算法计算并校正轨迹追踪装置20的倾斜量与位置。举例来说，可控制机械手臂30将轨迹追踪装置20移动至传感器中间，以通过量测一轴两点的位置找出斜率来补偿轨迹追踪装置20的倾斜量，且可控制机械手臂30绕圆以找出圆心来补偿位置偏移。此外，可计算轨迹追踪装置20的坐标与机械手臂30的坐标之间的转置矩阵，以初始量测点的归零动作，而开始追踪轨迹追踪装置20的移动轨迹。
图3是依照本公开一实施例所绘示的机械手臂校正方法的示意图。请同时参照图1、图2及图3，本实施例绘示本公开的机械手臂校正方法的概念，其中机械手臂30例如是经控制以预设的轨迹移动。而基于轨迹追踪装置20的定位结果，处理装置10例如会在步骤S1中计算出机械手臂30末端点的实际运动轨迹22，并根据从机械手臂30的各枢轴马达的编码器取得的关节转角以及机械手臂30中各个连杆的连杆扭角、连杆距离及连杆长度(即，连杆参数)，加上由粒子群算法随机产生的补偿项后，在步骤S2中利用顺向运动学计算出机械手臂30末端点的适应运动轨迹32。在步骤S3中，处理装置10例如会计算实际运动轨迹22与适应运动轨迹32的多个位置误差，从而在步骤S4中，将所计算的位置误差导入粒子群算法，以迭代出机械手臂30的连杆参数的补偿项(其中，各个粒子Pi可通过迭代方式移动至接近目标位置T的最佳位置)。最后，通过使用补偿项更新连杆参数，而完成机械手臂30的任务轨迹精度的提升。
图4是依照本发明一实施例所绘示的机械手臂校正方法的流程图。请同时参照图1、图3及图4，本实施例的方法适用于上述的机械手臂校正系统1，以下即搭配机械手臂校正系统10的各项装置或组件说明本实施例的机械手臂校正方法的详细步骤。
在步骤S402中，由处理装置10的处理器16利用轨迹追踪装置20定位机械手臂30的末端点在三维空间中的位置，以计算器械手臂30操作时末端点的实际运动轨迹22。
在一些实施例中，轨迹追踪装置20是结合惯性测量单元与超宽带定位装置来获取机械手臂30的末端点的实际运动轨迹22。惯性测量单元例如是由三轴加速度计、三轴陀螺仪与三轴磁力计组合，其可检测机械手臂30的末端点在三维空间中的位移及方位，而通过传感器融合(Sensor fusion)技术，则可将惯性测量单元所检测的位移及方位融合，从而计算出轨迹追踪装置在三维空间中的姿态。其中，加速度计可利用感测到的各个轴向的受力情况来得到该轴向的加速度大小和方向，因此可计算位移。陀螺仪是用以测量三维坐标系与传感器之间的夹角以计算角速度，并通过夹角和角速度来判别物体在三维空间的运动状态，因此可测量自身的旋转运动。磁力计是用以测量当前设备与东南西北四个方向上的夹角以获得实际方位。
超宽带定位装置可基于数个超宽带基站所发射的超宽带信号计算其与各个超宽带基站的相对位置，进而推算出轨迹追踪装置的位置。其中，超宽带无线通信是采用时间间隔极短(小于1奈秒)的脉冲进行通信，其具有穿透力强、功耗低、抗多径效果、人体危害低、安全性高、定位精确等优点。超宽带定位技术采用的是到达时间差(Time Difference ofArrival，TDOA)定位算法，通过在室内设置已知位置的超宽带基站，并在欲定位的设备上设置定位标签，此标签可按照一定的频率发射脉冲，而不断地和已知位置的超宽带基站进行测距。通过测量不同超宽带基站与移动终端的传输时延差来进行定位，从而可通过定位算法精确地计算出定位标签所在的位置。
举例来说，图5是依照本公开一实施例所绘示的超宽带定位技术的示意图。请参照图5，本实施例例如是在轨迹追踪装置20周围装设多个超宽带基站AN1～ANx，其中x为正整数。轨迹追踪装置20上的超宽带定位装置可检测超宽带基站AN1～ANx所发出的超宽带信号UWB，从而根据超宽带信号的强度，估测轨迹追踪装置20与各个超宽带基站AN1～ANx的距离，并据以计算出轨迹追踪装置20在三维空间中的位置。所述轨迹追踪装置20的位置可经由前述实施例中所计算的轨迹追踪装置20的坐标与机械手臂30的坐标之间的转置矩阵，换算为机械手臂30的末端点的位置。
图6是依照本公开一实施例所绘示的机械手臂运动轨迹计算方法的流程图。请同时参照图1及图6，本实施例说明上述的轨迹追踪装置20如何结合惯性测量单元与超宽带定位装置来获取机械手臂末端点的实际运动轨迹，以下即搭配机械手臂校正系统10的各项装置或组件说明本实施例的机械手臂校正方法的详细步骤。
在步骤S602中，由处理装置10的处理器16建立基于卡尔曼滤波器(Kalmanfilter)的系统模型。此系统模型例如表示机械手臂在三维空间中的运动状态及所观测到的量测状态。
举例来说，假设系统状态为x∈Rn，可观测到的量测状态为z∈Rm，其中m、n为空间维度，常以正整数表示，则机械手臂的系统模型可表示为：
Figure BDA0002865491170000071
zk＝Hxk+vk
其中，F为n×n的矩阵，用来表示不含过程噪声与控制输入的系统，连接前一时刻k-1与目前时刻k的系统状态。B为n×1的矩阵，用来表示控制输入u∈R1进入系统的方向。H为m×n的矩阵，用来将系统状态x映射到量测状态z。w与v分别是过程噪声与量测噪声。假设噪声来源之间互相独立且符合平均值为零的常态分布，其对应的协方差矩阵为Q(协方差矩阵)和R(观测噪声的协方差矩阵)。
在步骤S604中，处理器16将惯性测量单元及超宽带定位装置所量测到的多个量测值作为当前状态xk(机械手臂末端点在三维空间中的目前位置)使用系统模型估测下一状态xk+1(其中，xk+1＝Fxk，机械手臂末端点在三维空间中的下一时间位置)，并计算预测误差协方差Pk+1(下一时间噪声预测)及增益值Kk+1(下一时间的卡尔曼增益)，其公式如下：
Pk+1＝FPk+1FT+Q
Sk+1＝R+HPk+1HT
Figure BDA0002865491170000081
其中，Sk+1为量测余量的协方差。
在步骤S606中，处理器16计算所估测的下一状态的多个量测值zk+1与估测值
Figure BDA0002865491170000082
之间的预测误差εk+1，其公式如下：
Figure BDA0002865491170000083
在步骤S608中，处理器16计算利用所计算的预测误差εk+1及增益值Kk+1修正估测值xk+1，而获得修正后估测值x′k+1，用以作为机械手臂的末端点在三维空间中的位置，其公式如下：
x′k+1＝xk+1+Kk+1εk+1
在一些实施例中，上述的增益值Kk+1与协方差Sk+1可用以更新预测误差协方差Pk+1。处理器16可通过重复执行步骤S604～S608，持续预测机械手臂末端点在下一状态中的位置，并利用这些预测位置生成机械手臂末端点的实际运动轨迹，此实际运动轨迹可与后述通过顺向运动学所算出的适配运动轨迹比对以计算误差，最终通过将该误差导入粒子群算法而求得机械手臂30的连杆参数的补偿值。所述的更新预测误差协方差Pk+1的公式如下：
P′k+1＝Pk+1+Kk+1Sk+1Pk+1 T
回到图4的流程，在步骤S404中，由处理器16撷取机械手臂30的多个连杆参数，并利用粒子群算法随机产生包括这些连杆参数的多个补偿项的多组粒子，将每一组粒子的补偿项加上对应的连杆参数后导入顺向运动学，以计算末端点的适应运动轨迹。
举例来说，图7是本公开一实施例所绘示的产生连杆参数的补偿项的范例。请参照图7，本实施例的机械手臂70具备六个枢轴，而通过枢轴的转动以改变其运动姿态，其中
Figure BDA0002865491170000084
表示第一轴的坐标轴向，
Figure BDA0002865491170000085
Figure BDA0002865491170000086
表示第二轴的坐标轴向，以此类推。下表一列示机械手臂70的D-H参数表，其中包括连杆扭角(twist angle)αi-1、连杆长度ai-1、连杆距离di及关节转角(joint angle)θi。本实施例是利用粒子群算法随机产生包括下列10个补偿项
Figure BDA0002865491170000087
的粒子，从而将这些补偿项
Figure BDA0002865491170000088
加上对应的连杆参数后导入顺向运动学，以计算器械手臂70的末端点的适应运动轨迹。
Figure BDA0002865491170000091
表一
详细而言，在顺向运动学中，机械手臂70的各轴的空间转换矩阵Ai(其中i＝1～6)可以下式计算出：
Figure BDA0002865491170000092
据此，机械手臂70的末端点的位置T可以下式计算出：
T＝A1·A2·A3·A4·A5·A6
通过持续计算器械手臂70的末端点的位置T，即可生成机械手臂70的末端点的适应运动轨迹。
需说明的是，在本实施例中，对应于机械手臂70产生了10个补偿项
Figure BDA0002865491170000093
但在其他实施例中，对应于其他种类的机械手臂，则可依实际需要对其连杆参数设置不同数量的补偿项，本实施例并不限制补偿项的数量及设置方式。
在步骤S406中，由处理器16计算每一组粒子的适应运动轨迹与实际运动轨迹(例如由前述图6实施例所算出的实际运动轨迹)的多个位置误差，并用以计算粒子群算法的适应值，以评估多组粒子中的群体最佳位置。其中，处理器16例如是计算每一组粒子的多个位置误差的均方根值作为适应值，但不限于此。
详细而言，粒子群算法通过设计一种无质量的粒子来模拟鸟群中的鸟，这些粒子仅具有速度和位置两个属性，其中的速度代表移动的快慢，位置代表移动的方向。每个粒子都有一个由目标函数决定的适应值，并且会记录本身到目前为止经历过的最好位置(pbest)和现在位置，此记录可视为粒子本身的飞行经验。除此之外，每个粒子还记录有到目前为止整个群体中所有粒子经历过的最好位置(gbest，即所有pbest中的最佳值)，此记录可视为粒子同伴的经验。在粒子群算法中，每个粒子就是通过本身和同伴的经验来决定下一步的运动。
图8是依照本发明一实施例所绘示的机械手臂校正方法的流程图。请同时参照图1及图8，本实施例的方法适用于上述的机械手臂校正系统10，以下即搭配机械手臂校正系统10的各项装置或组件说明本实施例的机械手臂校正方法的详细步骤。
在步骤S802中，由处理装置10的处理器16计算每一组粒子当前的适应值。举例来说，假设q为机械手臂运动过程中所撷取的连杆参数，X为经由上述卡尔曼滤波器计算所得的实际位置，所述的连杆参数q经由顺向运动学的函数f计算后，可获得适应位置f(q)。
假设机械手臂的运动轨迹包括1000笔数据，处理器16例如产生50组粒子，其中每个粒子包含10个补偿项，则利用第一组粒子可算出1000笔适应位置f(q)，并进一步算出实际位置X与此适应位置f(q)的位置误差E，其中E＝X-f(q)。处理器16通过计算这1000笔位置误差的均方根值RMSE，以作为该组粒子的适应值。其中，第n组粒子的均方根值RMSE的计算公式如下：
Figure BDA0002865491170000101
其中，k为数据笔数，N为迭代次数。依此类推，50组粒子将产生50组适应值。
在步骤S804中，由处理器16根据每一组粒子的当前及过往的适应值，评估该组粒子的个体最佳位置(pbest)。在一些实施例中，处理器16例如是选择适应值最小的粒子作为个体最佳位置。
在步骤S806中，由处理器16选择多组粒子的个体最佳位置中的最佳者作为群体最佳位置。例如，从上述50组粒子的个体最佳位置(pbest)中选择最佳解(例如适应值最小者Min(RMSE))作为群体最佳位置(gbest)。
在步骤S808中，由处理器16判断是否满足目标条件。其中，所述目标条件例如是所计算的适应值小于预设阈值或迭代次数达到预设次数(例如100～1000中的任意值)。
若未满足目标条件，则在步骤S810中，由处理器16利用个体最佳位置及群体最佳位置更新各组粒子的位置与速度，并回到步骤S802，重新计算各组粒子的适应值。
例如，处理器16可将各组粒子的当前位置P(k)与当前速度V(k)更新为位置P(k+1)和速度V(k+1)，其计算公式如下：
V(k+1)＝wV(k)+c1rand()·(pbest-P(k))+c2Rand()·(gbest-P(k))
P(k+1)＝P(k)+V(k+1)
其中，w为惯性权重(inertiaweight)，c1和为个体经验权重，c2为群体经验权重，rand()和Rand()为两个在[0，1]范围中变化的随机值。
处理器16将会重复上述步骤S802～S810，直到满足目标条件时，进入步骤S812，以决定最终的群体最佳位置。此群体最佳位置即可用以作为上述步骤S406中由处理器16所评估的群体最佳位置。
最后，在步骤S408中，由处理器16使用群体最佳位置对应的粒子的补偿项更新连杆参数。即，处理器16可将机械手臂的D-H参数表中的每个D-H参数加上经由粒子群算法所算出的最佳补偿项，从而更新D-H参数表。机械手臂在接收任务指令时，即可参照此更新D-H参数表执行动作。因此，机械手臂的任务轨迹精度即可获得提升。
综上所述，本公开的机械手臂校正系统与机械手臂校正方法利用配置于机械手臂末端点的轨迹追踪装置通过融合惯性测量单元与超宽带定位装置的信号，而计算出末端点的实际运动轨迹，并将此实际运动轨迹与通过机械手臂各轴马达参数值经由顺向运动学公式所计算出来的适应运动轨迹比对以算出两者的误差量，并将此误差量经由强健的粒子群算法计算出机械手臂的D-H参数的补偿项以更新D-H参数。因此，可修正机械手臂长时间运动所累计的误差，并提高轨迹精度。
然本公开已以实施例揭示如上，然其并非用以限定本公开，任何本领域技术人员，在不脱离本公开的精神和范围内，当可作些许的更动与润饰，因此本公开的保护范围当视后附的权利要求及其均等范围所界定的为准。

Claims (20)

1.一种机械手臂校正系统，包括：
轨迹追踪装置，定位具有至少一枢轴的所述机械手臂的末端点在三维空间中的位置；以及
处理装置，包括：
连接装置，连接所述机械手臂及所述轨迹追踪装置；
存储装置，存储程序；以及
处理器，耦接所述连接装置及所述存储装置，且经配置以加载并执行所述程序以：
利用所述轨迹追踪装置所定位的所述位置，计算所述机械手臂操作时所述末端点的一实际运动轨迹；
撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括所述连杆参数的多个补偿项的多组粒子，将每一组所述粒子的所述补偿项加上对应的所述连杆参数后导入顺向运动学，以计算所述末端点的一适应运动轨迹；
计算每一组所述粒子的所述适应运动轨迹与所述实际运动轨迹的多个位置误差，并用以计算所述粒子群算法的适应值以评估所述多组粒子中的群体最佳位置；以及
使用所述群体最佳位置对应的所述粒子的所述补偿项更新所述连杆参数。
2.根据权利要求1所述的机械手臂校正系统，其中所述轨迹追踪装置包括：
惯性测量单元，检测所述机械手臂的所述末端点在所述三维空间中的位移及方位，其中所述处理器融合所述惯性测量单元所检测的所述位移及所述方位，以计算出所述轨迹追踪装置在所述三维空间中的姿态。
3.根据权利要求2所述的机械手臂校正系统，其中所述轨迹追踪装置还包括：
超宽带定位装置，检测周围多个超宽带基站所发出的超宽带信号，其中所述处理器根据所述超宽带信号的强度，估测所述轨迹追踪装置与各所述超宽带基站的距离，据以计算出所述轨迹追踪装置在所述三维空间中的位置。
4.根据权利要求3所述的机械手臂校正系统，其中所述处理器包括：
建立基于卡尔曼滤波器的一系统模型，所述系统模型表示所述机械手臂在所述三维空间中的运动状态及所观测到的量测状态；
将所述惯性测量单元及所述超宽带定位装置所量测到的多个量测值作为当前状态使用所述系统模型估测下一状态，并计算预测误差协方差及增益值；
计算所估测的所述下一状态的多个估测值与所述量测值之间的预测误差；以及
利用所计算的所述预测误差及所述增益值修正所述估测值，以作为所述机械手臂的所述末端点在三维空间中的所述位置。
5.根据权利要求2所述的机械手臂校正系统，其中所述惯性测量单元包括三轴的加速度计、陀螺仪及磁力计。
6.根据权利要求1所述的机械手臂校正系统，其中所述处理器包括计算每一组所述粒子的所述多个位置误差的均方根值作为所述适应值。
7.根据权利要求1所述的机械手臂校正系统，其中所述处理器包括：
随机产生各组粒子的位置与速度。
8.根据权利要求7所述的机械手臂校正系统，其中所述处理器包括：
计算每一组所述粒子当前的适应值并与过往的所述适应值比较，以评估该组粒子的个体最佳位置；
选择所述多组粒子的所述个体最佳位置中的最佳者作为所述群体最佳位置；以及
利用所述个体最佳位置及所述群体最佳位置更新各所述粒子的所述位置与所述速度，并重复上述步骤，直到满足一目标条件时，决定所述群体最佳位置。
9.根据权利要求8所述的机械手臂校正系统，其中所述目标条件包括所述适应值小于预设阈值或迭代次数达到预设次数。
10.根据权利要求1所述的机械手臂校正系统，其中所述连杆参数包括关节转角、连杆扭角、连杆距离及连杆长度。
11.一种机械手臂校正方法，适用于连接轨迹追踪装置的处理装置，所述方法包括下列步骤：
利用所述轨迹追踪装置定位具有至少一枢轴的机械手臂的末端点在三维空间中的位置，以计算所述机械手臂操作时所述末端点的一实际运动轨迹；
撷取机械手臂的多个连杆参数，并利用粒子群算法随机产生包括所述连杆参数的多个补偿项的多组粒子，将每一组所述粒子的所述补偿项加上对应的所述连杆参数后导入顺向运动学，以计算所述末端点的一适应运动轨迹；
计算每一组所述粒子的所述适应运动轨迹与所述实际运动轨迹的多个位置误差，并用以计算所述粒子群算法的适应值以评估所述多组粒子中的群体最佳位置；以及
使用所述群体最佳位置对应的所述粒子的所述补偿项更新所述连杆参数。
12.根据权利要求11所述的方法，其中所述轨迹追踪装置包括用以检测所述机械手臂的所述末端点在所述三维空间中的位移及方位的惯性测量单元，所述利用所述轨迹追踪装置定位具有至少一枢轴的机械手臂的末端点在三维空间中的位置的步骤包括：
融合所检测的所述位移及所述方位，以计算出所述轨迹追踪装置在所述三维空间中的姿态。
13.根据权利要求12所述的方法，其中所述轨迹追踪装置还包括用以检测周围多个超宽带基站所发出的超宽带信号的超宽带定位装置，所述利用所述轨迹追踪装置定位具有至少一枢轴的机械手臂的末端点在三维空间中的位置的步骤还包括：
根据所述超宽带信号的强度，估测所述轨迹追踪装置与各所述超宽带基站的距离，据以计算出所述轨迹追踪装置在所述三维空间中的位置。
14.根据权利要求13所述的方法，其中所述利用所述轨迹追踪装置定位具有至少一枢轴的机械手臂的末端点在三维空间中的位置的步骤包括：
建立基于卡尔曼滤波器的一系统模型，所述系统模型表示所述机械手臂在所述三维空间中的运动状态及所观测到的量测状态；
将所述惯性测量单元及所述超宽带定位装置所量测到的多个量测值作为当前状态使用所述系统模型估测下一状态，并计算预测误差协方差及增益值；
计算所估测的所述下一状态的多个估测值与所述量测值之间的预测误差；以及
利用所计算的所述预测误差及所述增益值修正所述估测值，以作为所述机械手臂的所述末端点在三维空间中的所述位置。
15.根据权利要求12所述的方法，其中所述惯性测量单元包括三轴的加速度计、陀螺仪及磁力计。
16.根据权利要求11所述的方法，其中所述利用每一组所述粒子的所述位置误差计算所述粒子群算法的适应值的步骤包括计算每一组所述粒子的所述多个位置误差的均方根值作为所述适应值。
17.根据权利要求11所述的方法，其中所述利用粒子群算法随机产生包括所述连杆参数的多个补偿项的多组粒子的步骤包括：
随机产生各组粒子的位置与速度。
18.根据权利要求17所述的方法，其中所述利用每一组所述粒子的所述位置误差计算所述粒子群算法的适应值以评估群体最佳位置的步骤包括：
计算每一组所述粒子当前的适应值并与过往的所述适应值比较，以评估该组粒子的个体最佳位置；
选择所述多组粒子的所述个体最佳位置中的最佳者作为所述群体最佳位置；以及
利用所述个体最佳位置及所述群体最佳位置更新各所述粒子的所述位置与所述速度，并重复上述步骤，直到满足一目标条件时，决定所述群体最佳位置。
19.根据权利要求18所述的方法，其中所述目标条件包括所述适应值小于预设阈值或迭代次数达到预设次数。
20.根据权利要求11所述的方法，其中所述连杆参数包括关节转角、连杆扭角、连杆距离及连杆长度。
CN202011579295.4A 2020-12-28 2020-12-28 机械手臂校正系统及机械手臂校正方法 Pending CN114683259A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011579295.4A CN114683259A (zh) 2020-12-28 2020-12-28 机械手臂校正系统及机械手臂校正方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011579295.4A CN114683259A (zh) 2020-12-28 2020-12-28 机械手臂校正系统及机械手臂校正方法

Publications (1)

Publication Number Publication Date
CN114683259A true CN114683259A (zh) 2022-07-01

Family

ID=82130499

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011579295.4A Pending CN114683259A (zh) 2020-12-28 2020-12-28 机械手臂校正系统及机械手臂校正方法

Country Status (1)

Country Link
CN (1) CN114683259A (zh)

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4763276A (en) * 1986-03-21 1988-08-09 Actel Partnership Methods for refining original robot command signals
DE19930087A1 (de) * 1999-06-30 2001-01-11 Charalambos Tassakos Verfahren und Vorrichtung zur Regelung der Vorhalteposition eines Manipulators eines Handhabungsgeräts
WO2010003289A1 (zh) * 2008-07-11 2010-01-14 中国科学院沈阳自动化研究所 一种使机器人高精度跟踪指定路径的设备和方法
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
US20150266183A1 (en) * 2012-10-19 2015-09-24 Inos Automationssoftware Gmbh Method for In-Line Calibration of an Industrial Robot, Calibration System for Performing Such a Method and Industrial Robot Comprising Such a Calibration System
CN107478183A (zh) * 2017-07-31 2017-12-15 华中科技大学 基于多点姿态采样的串联型机器人运动学参数标定方法
CN110405343A (zh) * 2019-08-15 2019-11-05 山东大学 一种基于Bagging集成的预测模型与粒子群优化算法的激光焊接工艺参数优化方法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法
CN107255795B (zh) * 2017-06-13 2020-01-07 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN110900608A (zh) * 2019-12-10 2020-03-24 哈尔滨工业大学 基于最优测量构型选择的机器人运动学标定方法

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US4763276A (en) * 1986-03-21 1988-08-09 Actel Partnership Methods for refining original robot command signals
DE19930087A1 (de) * 1999-06-30 2001-01-11 Charalambos Tassakos Verfahren und Vorrichtung zur Regelung der Vorhalteposition eines Manipulators eines Handhabungsgeräts
WO2010003289A1 (zh) * 2008-07-11 2010-01-14 中国科学院沈阳自动化研究所 一种使机器人高精度跟踪指定路径的设备和方法
US20150266183A1 (en) * 2012-10-19 2015-09-24 Inos Automationssoftware Gmbh Method for In-Line Calibration of an Industrial Robot, Calibration System for Performing Such a Method and Industrial Robot Comprising Such a Calibration System
CN103033186A (zh) * 2012-12-30 2013-04-10 东南大学 一种用于水下滑翔器的高精度组合导航定位方法
CN107255795B (zh) * 2017-06-13 2020-01-07 山东大学 基于ekf/efir混合滤波的室内移动机器人定位方法和装置
CN107478183A (zh) * 2017-07-31 2017-12-15 华中科技大学 基于多点姿态采样的串联型机器人运动学参数标定方法
CN110561424A (zh) * 2019-07-28 2019-12-13 华南理工大学 基于多传感器混合滤波器的在线机器人运动学校准方法
CN110405343A (zh) * 2019-08-15 2019-11-05 山东大学 一种基于Bagging集成的预测模型与粒子群优化算法的激光焊接工艺参数优化方法
CN110900608A (zh) * 2019-12-10 2020-03-24 哈尔滨工业大学 基于最优测量构型选择的机器人运动学标定方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
余跃庆;田浩;: "运动副间隙引起的并联机器人误差及其补偿", 光学精密工程, vol. 23, no. 05, pages 1331 - 1339 *
黄开启;魏文彬;陈荣华;丁问司;: "凿岩机器人钻臂定位误差补偿控制交叉精英反向粒子群优化算法", 机械科学与技术, vol. 37, no. 07, pages 1005 - 1012 *

Similar Documents

Publication Publication Date Title
CN111207774B (zh) 一种用于激光-imu外参标定的方法及系统
CN108621161B (zh) 基于多传感器信息融合的足式机器人本体状态估计方法
CN106647791B (zh) 三维姿态测控装置、机械设备及三维姿态的测控方法
CN108748159B (zh) 一种机械臂工具坐标系自标定方法
CN111278613B (zh) 校准装置、校准方法和控制装置
CN110561424A (zh) 基于多传感器混合滤波器的在线机器人运动学校准方法
CN110253574B (zh) 一种多任务机械臂位姿检测和误差补偿方法
Stadelmann et al. End-effector pose correction for versatile large-scale multi-robotic systems
JP2012040634A (ja) 力制御ロボットのキャリブレーション装置と方法
TW201736069A (zh) 用於機器手臂系統之機構參數校正方法
CN108214476A (zh) 基于改进型径向基神经网络的机械臂绝对定位精度标定方法
US20200298400A1 (en) Control system and control method of manipulator
JP2018027580A (ja) ロボット制御装置
KR20080088165A (ko) 로봇 캘리브레이션 방법
CN113459094B (zh) 一种工业机器人工具坐标系及零点自标定方法
TW202224872A (zh) 機械手臂校正系統及機械手臂校正系統方法
CN111687845B (zh) 一种基于惯性测量单元的机械臂运动学参数标定方法
WO2018113966A1 (en) System and method for automatically adjusting a gravity vector of a robot
CN110986952A (zh) 一种针对行人的高精度抗干扰室内定位方法
KR102561103B1 (ko) 로봇 보정 시스템 및 그것의 보정 방법
Bazhenov et al. Software for the mobile robot spatial orientation system
EP3904015A1 (en) System and method for setting up a robotic assembly operation
CN114683259A (zh) 机械手臂校正系统及机械手臂校正方法
CN113324544A (zh) 一种基于图优化的uwb/imu的室内移动机器人协同定位方法
CN111438692A (zh) 机器人控制方法、装置、介质、设备以及机器人

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination