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

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

Info

Publication number
CN109159112A
CN109159112A CN201810743476.2A CN201810743476A CN109159112A CN 109159112 A CN109159112 A CN 109159112A CN 201810743476 A CN201810743476 A CN 201810743476A CN 109159112 A CN109159112 A CN 109159112A
Authority
CN
China
Prior art keywords
unscented kalman
kalman filtering
motion
target ball
industrial robot
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.)
Granted
Application number
CN201810743476.2A
Other languages
English (en)
Other versions
CN109159112B (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

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)
V(k)＝R2
其中，(xk,yk,zk)为k时刻靶球在工作坐标系下的坐标，X(k)为L维矢量***，R为观测噪声方差。
⑹将靶球跟踪装置测得的数据作为***观测量以及初始参数输入到无迹卡尔曼滤波器，得到工业机器人运动参数的估计值。
再有，步骤⑵所述工业机器人做匀速直线运动，所述采样靶球的移动的时间间隔为T。
再有，步骤⑷所述靶球的运动信息包括：
靶球的起点为M(Mx,My,Mz)，终点为N(Nx,Ny,Nz)；
靶球的平均速度为：
其中，
靶球的平均加速度为：
其中，ΔSi为相邻两测量点位移，将沿的方向向量分解为
再有，步骤⑷所述过程噪声均值Q为：
观测噪声V(k)为靶球跟踪装置的精度；
协方差初始值P0为：P0＝diag[1 0.1 1 0.1 1 0.1]。
再有，步骤⑷所述的噪声驱动矩阵G为：
状态转移矩阵F为：
维数L和＝6；
无迹卡尔曼结构参数为：α＝0.001，κ＝0，β＝2，λ＝-5.999994。
再有，步骤⑸所述X(k)为：
观测噪声方差R为：R＝0.0001mm2。
再有，步骤⑸所述无迹卡尔曼滤波器的算法包括以下步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
采样点对应权值：
第0号Sigma点：
其余2L个Sigma点：
其中，状态向量为L维，为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，为矩阵方根的第i列；
⑵对Sigma点集的一步预测：X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L；
⑶计算***状态量的一步预测及协方差矩阵：
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量：
Z(i)(k+1|k)＝h[X(i)(k+1|k)]；
第⑹将新的预测的观测量通过加权求和得到***预测的均值及协方差：
⑺计算卡尔曼增益矩阵：
⑻计算***的状态方程的更新和协方差的更新：
本发明获得的技术效果是：
本发明中，在工业机器人末端安装靶球，利用跟踪装置跟踪靶球的运动信息，通过采样的运动信息确定无迹卡尔曼滤波器的各项参数，然后根据构建出的无迹卡尔曼滤波器进行计算，得到工业机器人运动参数的估计值。最后运算后得到的估计轨迹与跟踪装置的测量轨迹相比较，估计轨迹更接近工业机器人按照预设值运动时产生的实际轨迹，无迹卡尔曼滤波后的最大误差减少了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)；
靶球的平均速度为：
其中，
靶球的平均加速度为：
其中，ΔSi为相邻两测量点位移，将沿的方向向量分解为
过程噪声均值Q为：
观测噪声V(k)为激光跟踪仪的精度；
协方差初始值P0为：P0＝diag[1 0.1 1 0.1 1 0.1]。
噪声驱动矩阵G为：
状态转移矩阵F为：
维数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)
V(k)＝R2
其中，(xk,yk,zk)为k时刻靶球在工作坐标系下的坐标，X(k)为L维矢量***，R为观测噪声方差。
X(k)为：
观测噪声方差R为：R＝0.0001mm2。
⑹将激光跟踪仪测得的数据作为***观测量以及初始参数输入到无迹卡尔曼滤波器，得到工业机器人运动参数的估计值。
无迹卡尔曼滤波器的算法包括以下步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
采样点对应权值：
第0号Sigma点：
其余2L个Sigma点：
其中，状态向量为L维，为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，为矩阵方根的第i列；
⑵对Sigma点集的一步预测：X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L；
⑶计算***状态量的一步预测及协方差矩阵：
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量：
Z(i)(k+1|k)＝h[X(i)(k+1|k)]；
第⑹将新的预测的观测量通过加权求和得到***预测的均值及协方差：
⑺计算卡尔曼增益矩阵：
⑻计算***的状态方程的更新和协方差的更新：
实施例
设置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)。则工业机器人的平均速度通过以下公式求得
即：
工业机器人的平均加速度通过以下公式求得
其中，ΔSi为相邻两测量点位移，将沿的方向向量分解为
即：
从而确定在工业机器人做匀速直线运动时，无迹卡尔曼滤波器的过程噪声均值Q和协方差初始值P0，其值通过以下公式得到。
P0＝diag[1 0.1 1 0.1 1 0.1]
即：
观测噪声V(k)即测量仪器的精度，即激光跟踪仪的精度，R＝0.0001mm2。
根据机器人控制器编写的机器人匀速直线运动运动参数确定过程噪声驱动矩阵G、状态转移矩阵F
将T＝1/3s代入上式得到
确定被估计状态的维数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)
即：
无迹卡尔曼滤波器的观测方程
Z(k)＝Dist(X(k))+V(k)
即：
其中，(xk,yk,zk)为k时刻工业机器人末端靶球在激光跟踪仪工作坐标系下的坐标，X(k)为L维矢量***。
以第2点进行无迹卡尔曼滤波为例，即通过起点得到机器人在第2点运动的预测值，再与激光跟踪仪测量得到的机器人在第2点的测量值进行卡尔曼滤波，得到机器人第2点的估计值。
无迹卡尔曼滤波算法步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
即：结果见表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点：
其余2L个Sigma点：
即(以下数据以科学计数法的形式进行表示，保留一位小数)：结果见表3、4：
i 0 1 2 3 4 5 6
ω<sub>m</sub> -1.0E+06 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04
ω<sub>c</sub> -1.0E+06 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04
表3：第0～6个采样点的数据
i 7 8 9 10 11 12
ω<sub>m</sub> 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04
ω<sub>c</sub> 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04 8.3E+04
表4：第7～12个采样点的数据
其中状态向量为L维，为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，以下表示矩阵方根的第i列。
即：
⑵对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个采样点的数据
⑶计算***状态量的一步预测及协方差矩阵
即：
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)：
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
即：结果见表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个采样点的数据
表8：第7～12个采样点的数据
⑸将新的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个采样点的数据
⑹将新的预测的观测量通过加权求和得到***预测的均值及协方差：
即：
⑺计算Kalman增益矩阵：
即：
K(2)＝[-0.5097 -0.0168 -0.8581 -0.0283 0.0600 0.0020]T
⑻计算***的状态更新和协方差更新
即：
将得到的新的状态方程和新的协方差用于第3点的估计，不断的将新得到的变量代入下一个点的估计，从而得到全部点的估计值。
如图3所示，无迹卡尔曼滤波的估计轨迹，即UKF轨迹，激光跟踪仪的测量轨迹，即测量轨迹，都接近真实轨迹(工业机器人按照控制器中的设定值运动时产生的实际运动轨迹)。
但从图4的放大图可知，无迹卡尔曼滤波的估计轨迹(UKF轨迹)更接近工业机器人的真实轨迹。
表11是1～57次采样中，激光跟踪仪测量数据与真实轨迹之间的误差，无迹卡尔曼滤波后的数据与真实轨迹之间的误差。
表11：测量轨迹与真实轨迹之间的误差，估计轨迹与真实轨迹之间的误差
从表11可以看出，测量轨迹的最大误差是0.3574mm，经过无迹卡尔曼滤波后估计轨迹的最大误差是0.2560mm，减少了28.37％；测量轨迹的平均误差是0.1590mm，经过无迹卡尔曼滤波后的估计轨迹的平均误差是0.0881mm，减少了44.59％，精度提高了近2倍左右。
表12、13是工业机器人在匀速直线运动过程中，利用无迹卡尔曼滤波后对工业机器人的位移S和速度v进行估计的结果：
表12：位移S进行估计的结果
表13：速度v进行估计的结果
上述实验结果表明，本发明提出的基于无迹卡尔曼滤波的机器人运动参数估计方法能较好的减少测量误差，更加贴近机器人运动参数的真实值。
本发明中，在工业机器人末端安装靶球，利用跟踪装置跟踪靶球的运动信息，通过采样的运动信息确定无迹卡尔曼滤波器的各项参数，然后根据构建出的无迹卡尔曼滤波器进行计算，得到工业机器人运动参数的估计值。最后运算后得到的估计轨迹与跟踪装置的测量轨迹相比较，估计轨迹更接近工业机器人按照预设值运动时产生的实际轨迹，无迹卡尔曼滤波后的最大误差减少了28.3％，而平均误差减少了44.59％，由此可知，整体结构简单，可以降低测量仪器对工业机器人的测量误差，使运动信息的估计值更加贴近工业机器人的实际运动信息，为工业机器人完成加工、制造等工作精度的进一步提高提供了可能。

Claims (7)

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)
V(k)＝R2
其中，(xk,yk,zk)为k时刻靶球在工作坐标系下的坐标，X(k)为L维矢量***，R为观测噪声方差；
⑹将激光跟踪仪测得的数据作为***观测量以及初始参数输入到无迹卡尔曼滤波器，得到工业机器人运动参数的估计值。
2.根据权利要求1所述的一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：步骤⑵所述工业机器人做匀速直线运动，所述采样靶球的移动的时间间隔为T。
3.根据权利要求1所述的一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：步骤⑷所述靶球的运动信息包括：
靶球的起点为M(Mx,My,Mz)，终点为N(Nx,Ny,Nz)；
靶球的平均速度为：
其中，
靶球的平均加速度为：
其中，ΔSi为相邻两测量点位移，将沿的方向向量分解为
4.根据权利要求1所述的一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：步骤⑷所述过程噪声均值Q为：
观测噪声V(k)为激光跟踪仪的精度；
协方差初始值P0为：P0＝diag[1 0.1 1 0.1 1 0.1]。
5.根据权利要求1所述的一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：步骤⑷所述的噪声驱动矩阵G为：
状态转移矩阵F为：
维数L和＝6；
无迹卡尔曼结构参数为：α＝0.001，κ＝0，β＝2，λ＝-5.999994。
6.根据权利要求1所述的一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：步骤⑸所述X(k)为：
观测噪声方差R为：R＝0.0001mm2。
7.根据权利要求1所述的一种基于无迹卡尔曼滤波的机器人运动参数估计方法，其特征在于：步骤⑹所述无迹卡尔曼滤波器的算法包括以下步骤：
⑴获得一组Sigma点集X(i)(k︱k)及其对应权值：
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
采样点对应权值：
第0号Sigma点：
其余2L个Sigma点：
其中，状态向量为L维，为k时刻的状态向量估计值，P(k︱k)为k时刻状态向量的协方差矩阵，下标m代表均值，c代表协方差，为矩阵方根的第i列；
⑵对Sigma点集的一步预测：X(i)(k+1|k)＝f[k,X(i)(k|k)]，i＝0，1,2,…,2L；
⑶计算***状态量的一步预测及协方差矩阵：
⑷再次使用UT变换，产生新的Sigma点集X(i)(k+1︱k)
第0号Sigma点：
第1～L号Sigma点：
第(L+1)～2L号Sigma点：
⑸将新的Sigma点集代入观测方程，得到新的预测的观测量：
Z(i)(k+1|k)＝h[X(i)(k+1|k)]；
第⑹将新的预测的观测量通过加权求和得到***预测的均值及协方差：
⑺计算卡尔曼增益矩阵：
⑻计算***的状态方程的更新和协方差的更新：
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 true CN109159112A (zh) 2019-01-08
CN109159112B 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)

Cited By (6)

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

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 工业机器人位姿测量与在线误差补偿", 《机械工程学报》 *
朱岩 等: "基于无迹卡尔曼滤波的四旋翼无人飞行器姿态估计算法", 《测试技术学报》 *

Cited By (10)

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

Also Published As

Publication number Publication date
CN109159112B (zh) 2022-03-29

Similar Documents

Publication Publication Date Title
CN109159112A (zh) 一种基于无迹卡尔曼滤波的机器人运动参数估计方法
Westmore et al. Direct dynamic control of a robot using an end-point mounted camera and Kalman filter position estimation
CN106325073B (zh) 基于分数阶的伺服***位置环ip控制器无模型自校正方法
CN112558547B (zh) 一种五轴数控机床平动轴几何误差补偿数据快速优化方法
Ghaffari et al. Dynamic contour error estimation and feedback modification for high-precision contouring
CN110488749A (zh) 一种多轴运动***的轮廓误差控制器及其控制方法
CN105182906B (zh) 基于高阶s型运动轨迹的位置与速度控制方法
Kuang et al. Simplified newton-based CEE and discrete-time fractional-order sliding-mode CEC
CN106802143B (zh) 一种基于惯性仪器和迭代滤波算法的船体形变角测量方法
CN113910001B (zh) 一种数控机床空间误差辨识方法
CN114839921A (zh) 一种基于数据驱动的五轴轮廓控制方法
CN114952858A (zh) 一种基于摩擦补偿控制的工业机器人轨迹跟踪方法和***
Chen et al. Accelerated convergence interleaving iterative learning control and inverse dynamics identification
CN112986977A (zh) 一种克服雷达扩展卡尔曼航迹滤波发散的方法
CN115290295B (zh) 高速风洞级间分离与网格测力试验控制***及应用方法
CN116339233A (zh) 基于激光干涉跟踪仪测量场的机床误差控制方法及***
CN113689501B (zh) 一种基于收敛点的双机协同目标机定位跟踪控制方法
CN112003503B (zh) 一种基于蚁群龙贝格观测器的永磁同步直线电机控制方法
CN110788859B (zh) 一种控制器参数全域自适应调节***
CN113094642A (zh) 一种面向区域内机械臂定位误差估算的新增采样的方法
CN111274715A (zh) 一种基于多维时空数据的高精度动态轨迹约束方法
CN115407652B (zh) 一种用于原子自旋惯性测量装置的气室温度误差抑制方法
CN114714352B (zh) 机器人位姿信息确定方法、装置、设备及存储介质
Kang et al. Accurate Contour Error Estimation-based robust contour control for dual-linear-motor-driven gantry stages
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