CN109159112B - 一种基于无迹卡尔曼滤波的机器人运动参数估计方法 - Google Patents

一种基于无迹卡尔曼滤波的机器人运动参数估计方法 Download PDF

Info

Publication number
CN109159112B
CN109159112B CN201810743476.2A CN201810743476A CN109159112B CN 109159112 B CN109159112 B CN 109159112B CN 201810743476 A CN201810743476 A CN 201810743476A CN 109159112 B CN109159112 B CN 109159112B
Authority
CN
China
Prior art keywords
unscented kalman
sigma
target ball
industrial robot
motion
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.)
Active
Application number
CN201810743476.2A
Other languages
English (en)
Other versions
CN109159112A (zh
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.)
Tianjin University
Original Assignee
Tianjin University
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 Tianjin University filed Critical Tianjin University
Priority to CN201810743476.2A priority Critical patent/CN109159112B/zh
Publication of CN109159112A publication Critical patent/CN109159112A/zh
Application granted granted Critical
Publication of CN109159112B publication Critical patent/CN109159112B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

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/1607—Calculation of inertia, jacobian matrixes and inverses

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明涉及一种基于无迹卡尔曼滤波的机器人运动参数估计方法，在工业机器人末端安装靶球，利用跟踪装置跟踪靶球的运动信息，通过采样的运动信息确定无迹卡尔曼滤波器的各项参数，然后根据构建出的无迹卡尔曼滤波器进行计算，得到工业机器人运动参数的估计值。最后运算后得到的估计轨迹与跟踪装置的测量轨迹相比较，估计轨迹更接近工业机器人按照预设值运动时产生的实际轨迹，无迹卡尔曼滤波后的最大误差减少了28.3％，而平均误差减少了44.59％，由此可知，整体结构简单，可以降低测量仪器对工业机器人的测量误差，使运动信息的估计值更加贴近工业机器人的实际运动信息，为工业机器人完成加工、制造等工作精度的进一步提高提供了可能。

Description

一种基于无迹卡尔曼滤波的机器人运动参数估计方法
技术领域
本发明属于工业机器人运动参数估算技术领域，尤其是涉及一种基于无迹卡尔曼滤波的机器人运动参数估计方法。
背景技术
以机器人智能柔性制造系统为主的现代制造技术近年来在全球范围发展极其迅速。中国在借鉴德国等发达国家提过的“工业4.0”计划的基础上，明确了以体现信息技术和制造技术深度融合的数字化、网络化、智能化制造的发展主线。但是由于其本身存在较大的机械加工误差，再加上工作过程中其它因素：如温度、振动等的影响，虽然机器人的重复定位精度较高，但其绝对定位精度很低。而且由于串联的开环结构，机械臂的刚度也较低，在承受负载的情况下，其误差更加明显。因此为了提高工业机器人完成加工、制造等工作的质量，需要对其真实运动参数进行估计，以便对其完成加工、制造等工作时的偏差进行补偿，而真实运动参数估计的关键就是进行无迹卡尔曼滤波。
目前最常用的提高工业机器人精度的方法是通过对机器人进行离线标定，提高工业机器人的绝对定位精度，通过其刚度模型的刚度辨识，减少由于负载引起的误差。然而这种方法虽然减少了工业机器人运动误差，但仍无法得到机器人真实运动参数，故无法进一步提高工业机器人运动的精度。
发明内容
本发明克服了现有技术的不足，提供了一种利用激光通过符合NIST标准的气体吸收池产生的气体吸收谱线来确定采样点数和吸收频率，并具有高重复性、高稳定性、低成本、占据空间小和更高系统适应性等特点的一种基于无迹卡尔曼滤波的机器人运动参数估计方法。
本发明解决上述技术问题所采用的技术方案是：
一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：包括以下步骤：
⑴在工业机器人运动端处安装靶球，设置工业机器人的运动轨迹和运动参数；
⑵设置工业机器人工作原点，并使其在世界坐标系内按预先设定的运动轨迹和运动参数移动，靶球跟踪装置自动跟踪采样靶球的移动；
⑶利用世界坐标系的采样点最小二乘拟合工作坐标系；
⑷根据激光测距仪采样的靶球在工作坐标系的不同坐标，得到靶球的运动信息，根据运动信息确定过程噪声均值Q、协方差初始值P0、观测噪声V(k)、噪声驱动矩阵G、状态转移矩阵F、维数L和无迹卡尔曼结构参数；
⑸构建无迹卡尔曼滤波器：
无迹卡尔曼滤波器的状态方程：
X(K+1)＝FX(k)+W(k)
W(k)＝G*Q*rand(3,1)
无迹卡尔曼滤波器的观测方程
Z(k)＝Dist(X(k))+V(k)
Figure GDA0003393717390000021
V(k)＝R2
其中，(xk,yk,zk)为k时刻靶球在工作坐标系下的坐标，X(k)为L维矢量系统，R为观测噪声方差。
⑹将靶球跟踪装置测得的数据作为系统观测量以及初始参数输入到无迹卡尔曼滤波器，得到工业机器人运动参数的估计值。
再有，步骤⑵所述工业机器人做匀速直线运动，所述采样靶球的移动的时间间隔为T。
再有，步骤⑷所述靶球的运动信息包括：
靶球的起点为M(Mx,My,Mz)，终点为N(Nx,Ny,Nz)；
靶球的平均速度为：
Figure GDA0003393717390000022
其中，
Figure GDA0003393717390000023
靶球的平均加速度为：
Figure GDA0003393717390000024
其中，ΔSi为相邻两测量点位移，将
Figure GDA0003393717390000025
沿
Figure GDA0003393717390000026
的方向向量分解为
Figure GDA0003393717390000027
再有，步骤⑷所述过程噪声均值Q为：
Figure GDA0003393717390000028
观测噪声V(k)为靶球跟踪装置的精度；
协方差初始值P0为：P0＝diag[1 0.1 1 0.1 1 0.1]。
再有，步骤⑷所述的噪声驱动矩阵G为：
Figure GDA0003393717390000031
状态转移矩阵F为：
Figure GDA0003393717390000032
维数L＝6；
无迹卡尔曼结构参数为：α＝0.001，κ＝0，β＝2，λ＝-5.999994。
再有，步骤⑸所述X(k)为：
Figure GDA0003393717390000033
观测噪声方差R为：R＝0.0001mm2。
再有，步骤⑸所述无迹卡尔曼滤波器的算法包括以下步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
Figure GDA0003393717390000034
第1～L号Sigma点：
Figure GDA0003393717390000035
第(L+1)～2L号Sigma点：
Figure GDA0003393717390000036
采样点对应权值：
第0号Sigma点：
Figure GDA0003393717390000037
Figure GDA0003393717390000041
其余2L个Sigma点：
Figure GDA0003393717390000042
其中，状态向量为L维，
Figure GDA0003393717390000043
为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，
Figure GDA0003393717390000044
为矩阵方根的第i列；
⑵对Sigma点集的一步预测：X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L；
⑶计算系统状态量的一步预测及协方差矩阵：
Figure GDA0003393717390000045
Figure GDA0003393717390000046
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)
第0号Sigma点：
Figure GDA0003393717390000047
第1～L号Sigma点：
Figure GDA0003393717390000048
第(L+1)～2L号Sigma点：
Figure GDA0003393717390000049
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量：
Z(i)(k+1|k)＝h[X(i)(k+1|k)]；
第⑹将新的预测的观测量通过加权求和得到系统预测的均值及协方差：
Figure GDA00033937173900000410
Figure GDA00033937173900000411
Figure GDA00033937173900000412
⑺计算卡尔曼增益矩阵：
Figure GDA00033937173900000413
⑻计算系统的状态方程的更新和协方差的更新：
Figure GDA00033937173900000414
Figure GDA00033937173900000415
本发明获得的技术效果是：
本发明中，在工业机器人末端安装靶球，利用跟踪装置跟踪靶球的运动信息，通过采样的运动信息确定无迹卡尔曼滤波器的各项参数，然后根据构建出的无迹卡尔曼滤波器进行计算，得到工业机器人运动参数的估计值。最后运算后得到的估计轨迹与跟踪装置的测量轨迹相比较，估计轨迹更接近工业机器人按照预设值运动时产生的实际轨迹，无迹卡尔曼滤波后的最大误差减少了28.3％，而平均误差减少了44.59％，由此可知，整体结构简单，可以降低测量仪器对工业机器人的测量误差，使运动信息的估计值更加贴近工业机器人的实际运动信息，为工业机器人完成加工、制造等工作精度的进一步提高提供了可能。
附图说明
图1为本发明的测量设备的结构示意图；
图2为无迹卡尔曼滤波器的工作流程图；
图3为工业机器人匀速直线运动时的估计轨迹、测量轨迹和实际轨迹的比较图；
图4为工业机器人匀速直线运动时的估计轨迹、测量轨迹和实际轨迹的局部放大图。
具体实施方式
下面通过实施案例及对比例对本发明作进一步阐述，但不限于本实施例。
一种基于无迹卡尔曼滤波的机器人运动参数估计方法，如图1～4所示，本发明的创新在于：测量设备包括靶球跟踪装置4、工业机器人1、机器人控制柜6和上位机5，在工业机器人运动端的末端设置靶座2，在靶座上安装靶球3，靶球跟踪装置可以选用激光跟踪仪、干涉仪等测距仪，下述过程中选用激光跟踪仪。
估计方法包括以下步骤：
⑴在工业机器人运动端处安装靶球，设置工业机器人的运动轨迹和运动参数；
在机器人控制面板通过LIN、CRC、等候等指令编写机器人运动轨迹(包括直线、曲线等)及运动参数(包括起点、终点、速度、加速度等)。设置工业机器人匀速直线运动的速度为v＝30mm/s。
⑵设置工业机器人工作原点，并使其在世界坐标系内按预先设定的运动轨迹和运动参数移动，激光跟踪仪自动跟踪采样靶球的移动；
⑶利用世界坐标系的采样点最小二乘拟合工作坐标系；
选取工业机器人工作范围内的某点O为原点，通过机器人控制面板分别使工业机器人沿世界坐标系的+X、+Y、+Z轴移动，同时用激光跟踪仪对靶球的坐标进行测量，以O为工作坐标系原点，分别以工业机器人沿其世界坐标系+X、+Y、+Z轴移动的点最小二乘拟合工作坐标系的+X、+Y、+Z轴。此时，工作坐标系的+X、+Y、+Z轴分别与工业机器人世界坐标系+X、+Y、+Z轴平行，在之后的测量处理过程中不再需要坐标系转换，提高了运动模型的精度。由于工业机器人与靶球之间是刚性连接，且工业机器人在运动过程中保持姿态不变，所以靶球和机器人末端的位移、速度、加速度相同，可以通过对靶球的测量完成对工业机器人末端运动参数的估计。
⑷根据激光测距仪采样的靶球在工作坐标系的不同坐标，得到靶球的运动信息，根据运动信息确定过程噪声均值Q、协方差初始值P0、观测噪声V(k)、噪声驱动矩阵G、状态转移矩阵F、维数L和无迹卡尔曼结构参数；
靶球的运动信息包括：
靶球的起点为M(Mx,My,Mz)，终点为N(Nx,Ny,Nz)；
靶球的平均速度为：
Figure GDA0003393717390000061
其中，
Figure GDA0003393717390000062
靶球的平均加速度为：
Figure GDA0003393717390000063
其中，ΔSi为相邻两测量点位移，将
Figure GDA0003393717390000064
沿
Figure GDA0003393717390000065
的方向向量分解为
Figure GDA0003393717390000066
过程噪声均值Q为：
Figure GDA0003393717390000067
观测噪声V(k)为激光跟踪仪的精度；
协方差初始值P0为：P0＝diag[1 0.1 1 0.1 1 0.1]。
噪声驱动矩阵G为：
Figure GDA0003393717390000071
状态转移矩阵F为：
Figure GDA0003393717390000072
维数L＝6；
无迹卡尔曼结构参数为：α＝0.001，κ＝0，β＝2，λ＝-5.999994。
⑸构建无迹卡尔曼滤波器：
无迹卡尔曼滤波器的状态方程：
X(K+1)＝FX(k)+W(k)
W(k)＝G*Q*rand(3,1)
无迹卡尔曼滤波器的观测方程
Z(k)＝Dist(X(k))+V(k)
Figure GDA0003393717390000073
V(k)＝R2
其中，(xk,yk,zk)为k时刻靶球在工作坐标系下的坐标，X(k)为L维矢量系统，R为观测噪声方差。
X(k)为：
Figure GDA0003393717390000074
观测噪声方差R为：R＝0.0001mm2。
⑹将激光跟踪仪测得的数据作为系统观测量以及初始参数输入到无迹卡尔曼滤波器，得到工业机器人运动参数的估计值。
无迹卡尔曼滤波器的算法包括以下步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
Figure GDA0003393717390000081
第1～L号Sigma点：
Figure GDA0003393717390000082
第(L+1)～2L号Sigma点：
Figure GDA0003393717390000083
采样点对应权值：
第0号Sigma点：
Figure GDA0003393717390000084
Figure GDA0003393717390000085
其余2L个Sigma点：
Figure GDA0003393717390000086
其中，状态向量为L维，
Figure GDA0003393717390000087
为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，
Figure GDA0003393717390000088
为矩阵方根的第i列；
⑵对Sigma点集的一步预测：X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L；
⑶计算系统状态量的一步预测及协方差矩阵：
Figure GDA0003393717390000089
Figure GDA00033937173900000810
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)
第0号Sigma点：
Figure GDA00033937173900000811
第1～L号Sigma点：
Figure GDA00033937173900000812
第(L+1)～2L号Sigma点：
Figure GDA00033937173900000813
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量：
Z(i)(k+1|k)＝h[X(i)(k+1|k)]；
第⑹将新的预测的观测量通过加权求和得到系统预测的均值及协方差：
Figure GDA0003393717390000091
Figure GDA0003393717390000092
Figure GDA0003393717390000093
⑺计算卡尔曼增益矩阵：
Figure GDA0003393717390000094
⑻计算系统的状态方程的更新和协方差的更新：
Figure GDA0003393717390000095
Figure GDA00033937173900000916
实施例
设置T＝1/3s，采样点为60个，由于工业机器人执行匀速直线运动指令时，在开始后会进行一段短时间的由0加速到30mm/s匀加速阶段，在结束前进行一段短时间的由30mm/s减速到0的匀减速阶段，因此，舍弃激光跟踪仪前面几个点和最后面几个点，保证处理的数据是匀速直线运动部分，只对剩下的2n+1＝57个点的数据进行处理，此时剩下的数据中的第一个点作为起点M(Mx,My,Mz)，即M(-146.3096,-245.1070,8.7997)，最后一个点作为终点N(Nx,Ny,Nz)，即N(54.2093,24.4073,456.5172)。则工业机器人的平均速度
Figure GDA0003393717390000096
通过以下公式求得
Figure GDA0003393717390000097
Figure GDA0003393717390000098
即：
Figure GDA0003393717390000099
工业机器人的平均加速度
Figure GDA00033937173900000910
通过以下公式求得
Figure GDA00033937173900000911
其中，ΔSi为相邻两测量点位移，将
Figure GDA00033937173900000912
沿
Figure GDA00033937173900000913
的方向向量分解为
Figure GDA00033937173900000914
即：
Figure GDA00033937173900000915
从而确定在工业机器人做匀速直线运动时，无迹卡尔曼滤波器的过程噪声均值Q和协方差初始值P0，其值通过以下公式得到。
Figure GDA0003393717390000101
P0＝diag[1 0.1 1 0.1 1 0.1]
即：
Figure GDA0003393717390000102
Figure GDA0003393717390000103
观测噪声V(k)即测量仪器的精度，即激光跟踪仪的精度，R＝0.0001mm2。
根据机器人控制器编写的机器人匀速直线运动运动参数确定过程噪声驱动矩阵G、状态转移矩阵F
Figure GDA0003393717390000104
Figure GDA0003393717390000105
将T＝1/3s代入上式得到
Figure GDA0003393717390000106
Figure GDA0003393717390000111
确定被估计状态的维数L＝6(即估计靶球X、Y、Z坐标及三个方向的速度)，确定无迹卡尔曼结构参数：α一般取0.001，描述Sigma点围绕均值分布的范围；κ通常取0，是中等尺度参数，β取2，因为噪声是高斯分布，λ通过以下公式求得：λ＝α2(L+κ)-L，即：λ＝-5.999994。
无迹卡尔曼滤波器的状态方程
X(K+1)＝FX(k)+W(k)
W(k)＝G*Q*rand(3,1)
即：
Figure GDA0003393717390000112
无迹卡尔曼滤波器的观测方程
Z(k)＝Dist(X(k))+V(k)
Figure GDA0003393717390000113
即：
Figure GDA0003393717390000114
其中，(xk,yk,zk)为k时刻工业机器人末端靶球在激光跟踪仪工作坐标系下的坐标，X(k)为L维矢量系统。
以第2点进行无迹卡尔曼滤波为例，即通过起点得到机器人在第2点运动的预测值，再与激光跟踪仪测量得到的机器人在第2点的测量值进行卡尔曼滤波，得到机器人第2点的估计值。
无迹卡尔曼滤波算法步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
Figure GDA0003393717390000115
第1～L号Sigma点：
Figure GDA0003393717390000121
第(L+1)～2L号Sigma点：
Figure GDA0003393717390000122
即：结果见表1、2：
X<sup>(i)</sup>(1︱1) i＝0 i＝1 i＝2 i＝3 i＝4 i＝5 i＝6
x/mm -146.310 -146.307 -146.310 -146.310 -146.310 -146.310 -146.310
v<sub>x</sub>/(mm/s) 10.746 10.746 10.747 10.746 10.746 10.746 10.746
y/mm -245.107 -245.107 -245.107 -245.105 -245.107 -245.107 -245.107
v<sub>y</sub>/(mm/s) 14.445 14.445 14.445 14.445 14.446 14.445 14.445
z/mm 8.800 8.800 8.800 8.800 8.800 8.802 8.800
v<sub>z</sub>/(mm/s) 23.997 23.997 23.997 23.997 23.997 23.997 23.998
表1：第0～6个采样点的数据
X<sup>(i)</sup>(1︱1) i＝7 i＝8 i＝9 i＝10 i＝11 i＝12
x/mm -146.312 -146.310 -146.310 -146.310 -146.310 -146.310
v<sub>x</sub>/(mm/s) 10.746 10.745 10.746 10.746 10.746 10.746
y/mm -245.107 -245.107 -245.109 -245.107 -245.107 -245.107
v<sub>y</sub>/(mm/s) 14.445 14.445 14.445 14.444 14.445 14.445
z/mm 8.800 8.800 8.800 8.800 8.797 8.800
v<sub>z</sub>/(mm/s) 23.997 23.997 23.997 23.997 23.997 23.996
表2：第7～12个采样点的数据
采样点对应权值：
第0号Sigma点：
Figure GDA0003393717390000123
Figure GDA0003393717390000124
其余2L个Sigma点：
Figure GDA0003393717390000125
结果见表3、4：
i 0 1 2 3 4 5 6
ω<sub>m</sub> -1000000 83000 83000 83000 83000 83000 83000
ω<sub>c</sub> -1000000 83000 83000 83000 83000 83000 83000
表3：第0～6个采样点的数据
i 7 8 9 10 11 12
ω<sub>m</sub> 83000 83000 83000 83000 83000 83000
ω<sub>c</sub> 83000 83000 83000 83000 83000 83000
表4：第7～12个采样点的数据
其中状态向量为L维，
Figure GDA0003393717390000126
为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，以下表示矩阵方根的第i列。
Figure GDA0003393717390000127
即：
Figure GDA0003393717390000131
⑵对Sigma点集的一步预测：
X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L
即：结果见表5、6：
X<sup>(i)</sup>(2︱1) i＝0 i＝1 i＝2 i＝3 i＝4 i＝5 i＝6
x/mm -142.728 -142.725 -142.727 -142.728 -142.728 -142.728 -142.728
v<sub>x</sub>/(mm/s) 10.746 10.746 10.747 10.746 10.746 10.746 10.746
y/mm -240.292 -240.292 -240.292 -240.290 -240.292 -240.292 -240.292
v<sub>y</sub>/(mm/s) 14.445 14.445 14.445 14.445 14.446 14.445 14.445
z/mm 16.799 16.799 16.799 16.799 16.799 16.801 16.799
v<sub>z</sub>/(mm/s) 23.997 23.997 23.997 23.997 23.997 23.997 23.998
表5：第0～6个采样点的数据
X<sup>(i)</sup>(2︱1) i＝7 i＝8 i＝9 i＝10 i＝11 i＝12
x/mm -142.730 -142.728 -142.728 -142.728 -142.728 -142.728
v<sub>x</sub>/(mm/s) 10.746 10.745 10.746 10.746 10.746 10.746
y/mm -240.292 -240.292 -240.294 -240.292 -240.292 -240.292
v<sub>y</sub>/(mm/s) 14.445 14.445 14.445 14.444 14.445 14.445
z/mm 16.799 16.799 16.799 16.799 16.796 16.798
v<sub>z</sub>/(mm/s) 23.997 23.997 23.997 23.997 23.997 23.996
表6：第7～12个采样点的数据
⑶计算系统状态量的一步预测及协方差矩阵
Figure GDA0003393717390000132
Figure GDA0003393717390000133
即：
Figure GDA0003393717390000134
Figure GDA0003393717390000141
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)：
第0号Sigma点：
Figure GDA0003393717390000142
第1～L号Sigma点：
Figure GDA0003393717390000143
第(L+1)～2L号Sigma点：
Figure GDA0003393717390000144
即：结果见表7、8：
X<sup>(i)</sup>(2︱1) i＝0 i＝1 i＝2 i＝3 i＝4 i＝5 i＝6
x -142.728 -142.725 -142.727 -142.728 -142.728 -142.728 -142.728
v<sub>x</sub> 10.746 10.746 10.747 10.746 10.746 10.746 10.746
y -240.292 -240.292 -240.292 -240.290 -240.292 -240.292 -240.292
v<sub>y</sub> 14.445 14.445 14.445 14.445 14.446 14.445 14.445
z 16.799 16.799 16.799 16.799 16.799 16.801 16.799
v<sub>z</sub> 23.997 23.997 23.997 23.997 23.997 23.997 23.998
表7：第0～6个采样点的数据
Figure GDA0003393717390000145
表8：第7～12个采样点的数据
Figure GDA0003393717390000146
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量
Z(i)(k+1|k)＝h[X(i)(k+1|k)]
即：结果见表9、10：
i＝0 i＝1 i＝2 i＝3 i＝4 i＝5 i＝6
Z<sup>(i)</sup>(2︱1) 279.989 279.987 279.989 279.986 279.989 279.989 279.989
表9：第0～6个采样点的数据
i＝7 i＝8 i＝9 i＝10 i＝11 i＝12
Z<sup>(i)</sup>(2︱1) 279.990 279.989 279.991 279.989 279.988 279.989
表10：第7～12个采样点的数据
⑹将新的预测的观测量通过加权求和得到系统预测的均值及协方差：
Figure GDA0003393717390000151
Figure GDA0003393717390000152
Figure GDA0003393717390000153
即：
Figure GDA0003393717390000154
Figure GDA0003393717390000155
Figure GDA0003393717390000156
⑺计算Kalman增益矩阵：
Figure GDA0003393717390000157
即：
K(2)＝[-0.5097 -0.0168 -0.8581 -0.0283 0.0600 0.0020]T
⑻计算系统的状态更新和协方差更新
Figure GDA0003393717390000158
Figure GDA0003393717390000159
即：
Figure GDA00033937173900001510
Figure GDA0003393717390000161
将得到的新的状态方程和新的协方差用于第3点的估计，不断的将新得到的变量代入下一个点的估计，从而得到全部点的估计值。
如图3所示，无迹卡尔曼滤波的估计轨迹，即UKF轨迹，激光跟踪仪的测量轨迹，即测量轨迹，都接近真实轨迹(工业机器人按照控制器中的设定值运动时产生的实际运动轨迹)。
但从图4的放大图可知，无迹卡尔曼滤波的估计轨迹(UKF轨迹)更接近工业机器人的真实轨迹。
表11是1～57次采样中，激光跟踪仪测量数据与真实轨迹之间的误差，无迹卡尔曼滤波后的数据与真实轨迹之间的误差。
Figure GDA0003393717390000162
Figure GDA0003393717390000171
表11：测量轨迹与真实轨迹之间的误差，估计轨迹与真实轨迹之间的误差
从表11可以看出，测量轨迹的最大误差是0.3574mm，经过无迹卡尔曼滤波后估计轨迹的最大误差是0.2560mm，减少了28.37％；测量轨迹的平均误差是0.1590mm，经过无迹卡尔曼滤波后的估计轨迹的平均误差是0.0881mm，减少了44.59％，精度提高了近2倍左右。
表12、13是工业机器人在匀速直线运动过程中，利用无迹卡尔曼滤波后对工业机器人的位移S和速度v进行估计的结果：
Figure GDA0003393717390000172
Figure GDA0003393717390000181
Figure GDA0003393717390000191
表12：位移S进行估计的结果
Figure GDA0003393717390000192
Figure GDA0003393717390000201
表13：速度v进行估计的结果
上述实验结果表明，本发明提出的基于无迹卡尔曼滤波的机器人运动参数估计方法能较好的减少测量误差，更加贴近机器人运动参数的真实值。
本发明中，在工业机器人末端安装靶球，利用跟踪装置跟踪靶球的运动信息，通过采样的运动信息确定无迹卡尔曼滤波器的各项参数，然后根据构建出的无迹卡尔曼滤波器进行计算，得到工业机器人运动参数的估计值。最后运算后得到的估计轨迹与跟踪装置的测量轨迹相比较，估计轨迹更接近工业机器人按照预设值运动时产生的实际轨迹，无迹卡尔曼滤波后的最大误差减少了28.3％，而平均误差减少了44.59％，由此可知，整体结构简单，可以降低测量仪器对工业机器人的测量误差，使运动信息的估计值更加贴近工业机器人的实际运动信息，为工业机器人完成加工、制造等工作精度的进一步提高提供了可能。

Claims (1)

1.一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：包括以下步骤：
⑴在工业机器人运动端处安装靶球，设置工业机器人的运动轨迹和运动参数；
⑵设置工业机器人工作原点，并使其在世界坐标系内按预先设定的运动轨迹和运动参数移动，激光跟踪仪自动跟踪采样靶球的移动；
⑶利用世界坐标系的采样点最小二乘拟合工作坐标系；
⑷根据激光测距仪采样的靶球在工作坐标系的不同坐标，得到靶球的运动信息，根据运动信息确定过程噪声均值Q、协方差初始值P0、观测噪声V(k)、噪声驱动矩阵G、状态转移矩阵F、维数L和无迹卡尔曼结构参数；
⑸构建无迹卡尔曼滤波器：
无迹卡尔曼滤波器的状态方程：
X(K+1)＝FX(k)+W(k)
W(k)＝G*Q*rand(3,1)
无迹卡尔曼滤波器的观测方程
Z(k)＝Dist(X(k))+V(k)
Figure FDA0003393717380000011
V(k)＝R2
其中，(xk,yk,zk)为k时刻靶球在工作坐标系下的坐标，X(k)为L维矢量系统，R为观测噪声方差；
⑹将激光跟踪仪测得的数据作为系统观测量以及初始参数输入到无迹卡尔曼滤波器，得到工业机器人运动参数的估计值；
步骤⑵所述工业机器人做匀速直线运动，所述采样靶球的移动的时间间隔为T；
步骤⑷所述靶球的运动信息包括：
靶球的起点为M(Mx,My,Mz)，终点为N(Nx,Ny,Nz)；
靶球的平均速度为：
Figure FDA0003393717380000012
其中，
Figure FDA0003393717380000013
靶球的平均加速度为：
Figure FDA0003393717380000021
其中，ΔSi为相邻两测量点位移，将
Figure FDA0003393717380000022
沿
Figure FDA0003393717380000023
的方向向量分解为
Figure FDA0003393717380000024
步骤⑷所述过程噪声均值Q为：
Figure FDA0003393717380000025
观测噪声V(k)为激光跟踪仪的精度；
协方差初始值P0为：P0＝diag[1 0.1 1 0.1 1 0.1]；
步骤⑷所述的噪声驱动矩阵G为：
Figure FDA0003393717380000026
状态转移矩阵F为：
Figure FDA0003393717380000027
维数L＝6；
无迹卡尔曼结构参数为：α＝0.001，κ＝0，β＝2，λ＝-5.999994；
步骤⑸所述X(k)为：
Figure FDA0003393717380000028
观测噪声方差R为：R＝0.0001mm2；
步骤⑹所述无迹卡尔曼滤波器的算法包括以下步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
Figure FDA0003393717380000031
第1～L号Sigma点：
Figure FDA0003393717380000032
第(L+1)～2L号Sigma点：
Figure FDA0003393717380000033
采样点对应权值：
第0号Sigma点：
Figure FDA0003393717380000034
Figure FDA0003393717380000035
其余2L个Sigma点：
Figure FDA0003393717380000036
其中，状态向量为L维，
Figure FDA0003393717380000037
为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，
Figure FDA0003393717380000038
为矩阵方根的第i列；
⑵对Sigma点集的一步预测：X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L；
⑶计算系统状态量的一步预测及协方差矩阵：
Figure FDA0003393717380000039
Figure FDA00033937173800000310
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)
第0号Sigma点：
Figure FDA00033937173800000311
第1～L号Sigma点：
Figure FDA00033937173800000312
第(L+1)～2L号Sigma点：
Figure FDA00033937173800000313
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量：
Z(i)(k+1|k)＝h[X(i)(k+1|k)]；
第⑹将新的预测的观测量通过加权求和得到系统预测的均值及协方差：
Figure FDA00033937173800000314
Figure FDA00033937173800000315
Figure FDA0003393717380000041
⑺计算卡尔曼增益矩阵：
Figure FDA0003393717380000042
⑻计算系统的状态方程的更新和协方差的更新：
Figure FDA0003393717380000043
Figure FDA0003393717380000044
CN201810743476.2A 2018-07-09 2018-07-09 一种基于无迹卡尔曼滤波的机器人运动参数估计方法 Active CN109159112B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN201810743476.2A CN109159112B (zh) 2018-07-09 2018-07-09 一种基于无迹卡尔曼滤波的机器人运动参数估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201810743476.2A CN109159112B (zh) 2018-07-09 2018-07-09 一种基于无迹卡尔曼滤波的机器人运动参数估计方法

Publications (2)

Publication Number Publication Date
CN109159112A CN109159112A (zh) 2019-01-08
CN109159112B true CN109159112B (zh) 2022-03-29

Family

ID=64897454

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201810743476.2A Active CN109159112B (zh) 2018-07-09 2018-07-09 一种基于无迹卡尔曼滤波的机器人运动参数估计方法

Country Status (1)

Country Link
CN (1) CN109159112B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111369629B (zh) * 2019-12-27 2024-05-24 浙江万里学院 一种基于挥拍击球动作双目视觉感知的回球轨迹预测方法
CN111409076B (zh) * 2020-04-28 2021-11-05 珠海格力智能装备有限公司 机械手运动状态的确定方法及装置
CN111761583B (zh) * 2020-07-08 2022-04-08 温州大学 一种智能机器人运动定位方法及系统
CN112959323B (zh) * 2021-03-02 2022-03-11 中国工程物理研究院激光聚变研究中心 一种机器人运动误差在位检测与补偿方法及设备
CN115723127B (zh) * 2022-11-14 2024-08-09 天津大学 一种基于光栅编码器的混联机器人轮廓误差预测方法
CN117032068B (zh) * 2023-07-24 2024-02-27 苏州福斯特万电子科技有限公司 一种点胶机控制方法、装置、设备及存储介质

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1903308A2 (en) * 2006-09-25 2008-03-26 Honeywell International Inc. Systems and methods for a hybrid state transition matrix
CN102795323A (zh) * 2011-05-25 2012-11-28 中国科学院沈阳自动化研究所 一种基于ukf的水下机器人状态和参数联合估计方法
CN105773622A (zh) * 2016-04-29 2016-07-20 江南大学 一种基于iekf的工业机器人绝对精度校准方法
CN106908762A (zh) * 2017-01-12 2017-06-30 浙江工业大学 一种针对uhf‑rfid系统的多假设ukf目标跟踪方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
EP1903308A2 (en) * 2006-09-25 2008-03-26 Honeywell International Inc. Systems and methods for a hybrid state transition matrix
CN102795323A (zh) * 2011-05-25 2012-11-28 中国科学院沈阳自动化研究所 一种基于ukf的水下机器人状态和参数联合估计方法
CN105773622A (zh) * 2016-04-29 2016-07-20 江南大学 一种基于iekf的工业机器人绝对精度校准方法
CN106908762A (zh) * 2017-01-12 2017-06-30 浙江工业大学 一种针对uhf‑rfid系统的多假设ukf目标跟踪方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
KUKA 工业机器人位姿测量与在线误差补偿;史晓佳 等;《机械工程学报》;20170430;第1-6页 *
基于无迹卡尔曼滤波的四旋翼无人飞行器姿态估计算法;朱岩 等;《测试技术学报》;20140331;第194-198页 *

Also Published As

Publication number Publication date
CN109159112A (zh) 2019-01-08

Similar Documents

Publication Publication Date Title
CN109159112B (zh) 一种基于无迹卡尔曼滤波的机器人运动参数估计方法
CN111590581B (zh) 机器人的定位补偿方法及装置
CN111590594B (zh) 基于视觉引导的机器人轨迹跟踪控制方法
CN110202575B (zh) 一种用于工业测量的机器人目标轨迹精度补偿方法
CN110948504B (zh) 机器人加工作业法向恒力跟踪方法和装置
CN109186601A (zh) 一种基于自适应无迹卡尔曼滤波的激光slam算法
CN109129482B (zh) 一种动态补偿机器人直线导轨运动误差的方法
CN108645415A (zh) 一种船舶航迹预测方法
CN111178385A (zh) 一种鲁棒在线多传感器融合的目标跟踪方法
CN111983926B (zh) 一种最大协熵扩展椭球集员滤波方法
CN114131611B (zh) 机器人重力位姿分解的关节误差离线补偿方法、系统及终端
CN112433507B (zh) 基于lso-lssvm的五轴数控机床热误差综合建模方法
CN113910001B (zh) 一种数控机床空间误差辨识方法
CN111687845B (zh) 一种基于惯性测量单元的机械臂运动学参数标定方法
CN113459094A (zh) 一种工业机器人工具坐标系及零点自标定方法
CN112986977A (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN108021095B (zh) 一种基于置信域算法的多维空间轮廓误差估计方法
CN114952858B (zh) 一种基于摩擦补偿控制的工业机器人轨迹跟踪方法和系统
CN114839921A (zh) 一种基于数据驱动的五轴轮廓控制方法
CN111504327B (zh) 一种基于航迹平滑技术的广义标签多伯努利多目标跟踪方法
CN116587268B (zh) 一种空间大面域机器人铣削加工精度提升方法
CN109397293B (zh) 一种基于移动机器人的地面水平误差建模及补偿方法
CN110788859B (zh) 一种控制器参数全域自适应调节系统
CN113608442A (zh) 基于特征函数的非线性状态模型系统的状态估计方法
KR100248040B1 (ko) 목표물 예측방법

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
GR01 Patent grant
GR01 Patent grant