CN109291056B - 一种二自由度高速并联机器人零点标定方法 - Google Patents

Abstract

本发明公开了一种二自由度高速并联机器人零点标定方法，在基于传统卡尔曼滤波方法的零点辨识模型中引入了修正项，并建立了一种改进的L曲线法用于对模型递归计算过程中的修正参数进行优选，此外结合传统卡尔曼滤波方法递归过程中估计误差的协方差矩阵特性建立了修正参数的自适应优选方法，从而有效提高了机器人零点辨识过程棒性、准确性和辨识效率。

Description

一种二自由度高速并联机器人零点标定方法

技术领域

本发明涉及一种二自由度高速并联机器人零点标定方法。

背景技术

高速并联机器人的零点决定了系统的起始位姿，并且零点的准确性直接决定了其末端动平台在运动过程的精度。通常高速并联机器人在出厂之前会进行零点标定，但在实际使用过程中由于高速并联机器人经常执行快速分拣或装箱等操作，机器人容易发生运动碰撞、主动关节松动或其它控制故障，进而造成零点的丢失。故对高速并联机器人而言，需周期性的对机器人的零点误差进行标定。

高速并联机器人的零点误差标定通常需要经历四个步骤，即误差建模、测量、辨识和补偿。其中辨识方法的选择直接决定了零点误差辨识过程的鲁棒性和准确性。目前，最常用的辨识方法有最小二乘法和卡尔曼滤波法等。其中，最小二乘法的鲁棒性和准确性较差，卡尔曼滤波法在已准确获知系统过程噪声和测量噪声等先验参数的情况下具有相对较快的收敛速度，故被广泛使用。

然而，在实际高速并联机器人的零点误差辨识过程中，系统的过程噪声和测量噪声特性受到工作环境变化、测量仪器特性波动、伺服调节系统波动以及减速器误差等因素的影响而难以准确获得，通常需要不断地对相关参数进行手动试凑，直至卡尔曼滤波方法所辨识出的零点误差结果收敛到一定阈值位置为止。这极大地制约了零点标定过程的效率，并降低了标定过程的自动化程度。

发明内容

本发明的目的是为了克服现有技术中的不足，提出了一种高速并联机器人零点标定方法，在高速并联机器人的零点标定过程中，通过对基于卡尔曼滤波方法的零点标定模型进行改进，以降低其对于过程噪声和测量噪声等先验参数的依赖性，进而保证零点辨识过程的鲁棒性和准确性。与此同时，建立改进方法中修正参数的自适应优选方法，从而提升机器零点误差辨识结果的鲁棒性和准确性，并提高机器人零点标定过程的效率。

本发明为解决公知技术中存在的技术问题所采取的技术方案是：一种二自由度高速并联机器人零点标定方法，其步骤如下：

步骤1.建立机器人末端运动误差与零点误差之间的映射模型。

高速并联机器人由静平台、动平台、第一运动支链和第二运动支链组成。静平台和动平台之间安装一根伸缩尺，通过伸缩尺间接测量出动平台中心点P到基准坐标系O-xyz原点O之间的距离。

初始状态下，当该机器人的第一主动臂和第二主动臂与基准坐标系O-xyz的x轴平行时，即θ₁＝θ₂＝0°，此时机器人的零点误差Δθ₁＝Δθ₂＝0°。

利用空间矢量法和一阶摄动原理获得机器人末端运动的距离误差与机器人零点误差之间的映射模型，即

ΔC＝GΔp

其中，ΔC为末端运动的距离误差矩阵；Δp为机器人零点误差向量；G为误差传递矩阵。

步骤2.建立改进的卡尔曼滤波方法的机器人零点辨识模型。

基于机器人零点误差和末端运动误差和之间的映射模型，得到辨识系统的状态方程和观测方程

状态方程：Δp_m＝Δp_m-1+a_m-1

观测方程：ΔC_m＝GΔp_m+b_m

其中，Δp_m和Δp_m-1分别为第m次和第m-1次辨识过程中系统的状态向量(即零点误差向量)；ΔC_m为第m次辨识过程中通过测量所获得的机器人末端运动误差矩阵；a_m-1为第m-1次辨识过程中的过程噪声向量，该向量假设服从均值为0向量和协方差矩阵为Q的多元高斯分布；b_m为第m次测量过程中的测量噪声向量，该向量假设服从均值为0和协方差矩阵为R的多元高斯分布。

通常情况下，对协方差矩阵Q和R估计的准确性直接影响了卡尔曼滤波算法的效果。虽然测量噪声的协方差矩阵R在理想情况下可大致由测量仪器本身的测量误差参数给出，但实际中受工作环境变化或仪器本身不稳定性的影响，测量噪声的协方差矩阵R难以准确获得。此外，受伺服系统调节过程和减速器误差的影响，准确获得过程噪声协方差矩阵Q的先验信息将更加困难，故在此可将其简单设置为对角矩阵的形式，即

Q＝qI₁,R＝rI₂

其中，q和r为大于0的常数，在零点标定过程中q和r可取较小的数量级，如0～1×10²；I₁和I₂分别为与协方差矩阵Q和R同阶的单位矩阵。

故，建立改进的卡尔曼滤波方法的机器人零点辨识模型的过程如下：

步骤2.1预估辨识过程的状态向量

其中，

为第m次辨识过程中对于系统状态向量的预估值；

为第m-1次辨识过程后系统状态向量的更新值。在初始状态下，令

步骤2.2预估估计误差的协方差矩阵Q

P_m|m-1＝P_m-1|m-1+Q

其中，P_m|m-1为第m次辨识过程中所预估的估计误差的协方差矩阵；P_m-1|m-1为第m-1次辨识过程后所得到的估计误差的协方差矩阵。在初始状态下，令

P_1|0＝P_0|0＝pI₃

其中，I₃为单位矩阵；p为大于0的常数，如p＝1。

随着改进的卡尔曼滤波方法递归过程的进行，估计误差的协方差矩阵将逐渐下降并收敛。

步骤2.3建立改进的卡尔曼增益表达式

K_m(β_m)＝P_m|m-1G^T(GP_m|m-1G^T+(r+β_m)I₂)^-1

其中，K_m(β_m)为第m次辨识过程中改进的卡尔曼增益；β_m为第m次辨识过程中所采用的修正参数，且β_m>0。引入修正项β_mI₂是为了避免因为过程噪声和测量噪声的协方差矩阵Q和R估计不准确，造成逆矩阵(GP_m|m-1G^T+(r+β_m)I₂)^-1病态并产生接近于0的奇异值，进而造成辨识过程的发散。

步骤2.4利用改进的卡尔曼增益更新状态向量

其中，

为第m次辨识过程后所更新的系统状态向量。

步骤2.5更新估计误差的协方差矩阵

P_m|m＝P_m|m-1-K_m(β_m)GP_m|m-1

其中，P_m|m为第m次辨识过程后所更新的估计误差的协方差矩阵。

步骤2.6重复步骤2.1-步骤2.5的递归过程，直至相邻两次状态向量的估计结果满足

其中，ε为人为给定的辨识精度的阈值向量。

步骤3.采用改进的L曲线法对改进的卡尔曼滤波方法中的修正参数进行优选。

为提高零点辨识模型的鲁棒性和准确性，需对每一次递归过程中的修正参数β_m进行优选。本发明建立了一种改进的L曲线法，从而对修正参数βm进行优选。其来源分析如下：将更新后状态向量

的表达式改写为

W_X(β_m)＝I₁-K_m(β_m)G,W_Y(β_m)＝K_m(β_m)

可见，状态向量

是通过对其预估值

和实际观测信息ΔC_m之间按照一定的权重进行分配后计算获得的，对应权重分别为W_X(β_m)和W_Y(β_m)。若修正参数β_m→0,不合理的过程噪声和测量噪声协方差矩阵Q和R将致使W_Y(β_m)过高，从而状态向量

的更新过程更加信任于实际观测信息ΔC_m，而实际观测信息ΔC_m中的测量噪声将被改进的卡尔曼增益K_m(β_m)中病态的逆矩阵(GP_m|m-1GT+(r+β_m)I₂)-¹过度放大，进而造成递归过程的发散。

结合正定矩阵、对称矩阵的性质、奇异值分解和谱分解等理论，可以证明随着修正参数β_m的增大，||K_m(β_m)ΔC_m||将逐渐减小，而||GK_m(β_m)ΔC_m-ΔC_m||将逐渐增大。前者表示当前递归过程在更新状态向量的估计值

时，改进的卡尔曼滤波算法对当前观测信息ΔC_m的信任程度；后者为残差形式，表示没有被信任的观测信息量。曲线上的最大曲率点即为最佳平衡点，此时对应的修正参数β_m可以最小化算法的估计误差。

结合上述分析，采用改进的L曲线法对改进的卡尔曼滤波方法中的修正参数进行优选，过程如下：

步骤3.1按照间隔Δβ_m在区间[0,1×10²]上对β_m进行采样，从而获得一系列的离散点，离散点的横纵坐标分别为横坐标：γ(β_m)＝||GK_m(β_m)ΔC_m-ΔC_m||，纵坐标：η(β_m)＝||K_m(β_m)ΔC_m||。

步骤3.2利用三次样条插值法对离散点进行拟合,从而获得改进的L曲线

η(β_m)＝f_CSI(γ(β_m))

其中，f_CSI表示三次样条插值后所获得改进的L曲线方程。

步骤3.3求解改进的L曲线上的最大曲率

其中，最大曲率点所对应的修正参数β_mi即为最佳修正参数。

步骤4.修正参数的自适应优选。

结合卡尔曼滤波方法递归过程中估计误差的协方差矩阵特性，建立修正参数的自适应优选方法。由于协方差矩阵Q和R为固定矩阵，故改进的卡尔曼增益K_m(β_m)的数值在递归过程中的变化是由估计误差的协方差矩阵P_m-1|m-1所决定的，当P_m-1|m-1收敛时β_m也应当逐渐收敛。由于估计误差的协方差矩阵的迹可用来衡量算法的收敛程度，故当m≥2时，采用自适应的方式获得最佳的修正参数β_m，即

其中，τ(m)表示第m次辨识过程中的惩罚因子。

步骤5.将高速并联机器人的零点误差辨识结果补偿至机器人的运动学模型中。

本发明所提出的一种高速并联机器人零点标定方法通过在以往基于传统卡尔曼滤波方法的零点辨识模型中引入修正项，进而能够解决零点辨识模型因系统过程噪声和测量噪声协方差矩阵估计不准确而造成的辨识结果发散问题，从而有效的保证了零点辨识过程的鲁棒性和准确性。

本发明所提出的二自由度高速并联机器人零点标定方法，通过改进的L曲线法对改进的卡尔曼滤波方法的零点辨识模型递归过程中的修正参数进行优选，提高了辨识结果的鲁棒性和准确性。结合传统卡尔曼滤波方法递归过程中估计误差的协方差矩阵特性，建立修正参数的自适应优选方法，从而有效提高机器人零点辨识过程的效率。

附图说明

图1、是二自由度高速并联机器人的零点标定系统。

图2、是二自由度高速并联机器人的零点标定系统简化示意图。

图3、是改进的L曲线法示意图。

图4、是实验中的零点误差辨识效果对比图。

具体实施方式

下面结合附图和具体实施案例对本发明加以详细说明。

以中国发明专利《一种两自由度高速并联机器人的零点标定方法》(ZL201410364282.3)中的标定系统为实施案例，本发明的具体实施方式如下：

1.建立机器人零点误差与末端运动误差之间的映射模型

如图1所示，二自由度高速并联机器人由静平台1、动平台2、第一运动支链3和第二运动支链4组成。静平台1和动平台2之间安装有一根伸缩尺7，伸缩尺7的两端分别与静平台1和动平台2通过转动副进行连接。如图2所示，通过伸缩尺7可以间接测量出动平台中心点P到基准坐标系O-xyz原点O之间的距离。

如图1和图2所示初始状态下，当该机器人的第一主动臂5和第二主动臂6与基准坐标系O-xyz的x轴平行时(即θ₁＝θ₂＝0°)，此时机器人的零点误差Δθ₁＝Δθ₂＝0°；否则该机器人的零点误差Δθ₁和Δθ₂不为零。

利用空间矢量法和一阶摄动原理可以获得机器人末端运动的距离误差与机器人零点误差之间的映射模型，即

ΔC＝GΔp

其中，ΔC为末端运动的距离误差矩阵；Δp为机器人零点误差向量；G为误差传递矩阵。

2.建立改进的卡尔曼滤波方法的机器人零点辨识模型

基于机器人零点误差和末端运动误差和之间的映射模型，可以得到辨识系统的状态方程和观测方程

状态方程：Δp_m＝Δp_m-1+a_m-1

观测方程：ΔC_m＝GΔp_m+b_m

其中，Δp_m和Δp_m-1分别为第m次和第m-1次辨识过程中系统的状态向量(即零点误差向量)；ΔC_m为第m次辨识过程中通过测量所获得的机器人末端运动误差矩阵；a_m-1为第m-1次辨识过程中的过程噪声向量，该向量假设服从均值为0向量和协方差矩阵为Q的多元高斯分布；b_m为第m次测量过程中的测量噪声向量，该向量假设服从均值为0和协方差矩阵为R的多元高斯分布。

通常情况下，对协方差矩阵Q和R估计的准确性直接影响了卡尔曼滤波算法的效果。虽然测量噪声的协方差矩阵R在理想情况下可大致由测量仪器本身的测量误差参数给出，但实际中受工作环境变化或仪器本身不稳定性的影响，测量噪声的协方差矩阵R难以准确获得。此外，受伺服系统调节过程和减速器误差的影响，准确获得过程噪声协方差矩阵Q的先验信息将更加困难，故在此可将其简单设置为对角矩阵的形式，即

Q＝qI₁,R＝rI₂

其中，q和r为大于0的常数，在零点标定过程中q和r可取较小的数量级，如0～1×10²；I₁和I₂分别为与协方差矩阵Q和R同阶的单位矩阵。

故，建立改进的卡尔曼滤波方法的零点辨识模型，其具体步骤如下：

2.1预估辨识过程的状态向量

其中，

为第m次辨识过程中对于系统状态向量的预估值；

为第m-1次辨识过程后系统状态向量的更新值。在初始状态下，可令

2.2预估估计误差的协方差矩阵

P_m|m-1＝P_m-1|m-1+Q

其中，P_m|m-1为第m次辨识过程中所预估的估计误差的协方差矩阵；P_m-1|m-1为第m-1次辨识过程后所得到的估计误差的协方差矩阵。在初始状态下，可令

P_1|0＝P_0|0＝pI₃

其中，I₃为单位矩阵；p为大于0的常数，可给定一个较大数值，如p＝1。随着改进的卡尔曼滤波方法递归过程的进行，估计误差的协方差矩阵将逐渐下降并收敛。

2.3建立改进的卡尔曼增益表达式

K_m(β_m)＝P_m|m-1G^T(GP_m|m-1G^T+(r+β_m)I₂)^-1

其中，K_m(β_m)为第m次辨识过程中改进的卡尔曼增益；β_m为第m次辨识过程中所采用的修正参数，且β_m>0。引入修正项β_mI₂是为了避免因为过程噪声和测量噪声的协方差矩阵Q和R估计不准确，造成逆矩阵(GP_m|m-1G^T+(r+β_m)I₂)^-1病态并产生接近于0的奇异值，进而造成辨识过程的发散。

2.4利用改进的卡尔曼增益更新状态向量

其中，

为第m次辨识过程后所更新的系统状态向量。

2.5更新估计误差的协方差矩阵

P_m|m＝P_m|m-1-K_m(β_m)GP_m|m-1

其中，P_m|m为第m次辨识过程后所更新的估计误差的协方差矩阵。

2.6重复以上2.1～2.5的递归过程，直至相邻两次状态向量的估计结果满足

其中，ε为人为给定的辨识精度的阈值向量。

3.采用改进的L曲线法，对改进的卡尔曼滤波方法中的修正参数进行优选。

为提高零点辨识模型的鲁棒性和准确性，需对每一次递归过程中的修正参数β_m进行优选。本实施例建立了一种改进的L曲线，从而对修正参数βm进行优选：

将更新后状态向量

的表达式改写为

W_X(β_m)＝I₁-K_m(β_m)G,W_Y(β_m)＝K_m(β_m)

可见，状态向量

是通过对其预估值

和实际观测信息ΔC_m之间按照一定的权重进行分配后计算获得的，对应权重分别为W_X(β_m)和W_Y(β_m)。若修正参数β_m→0,不合理的过程噪声和测量噪声协方差矩阵Q和R将致使W_Y(β_m)过高，从而状态向量

的更新过程更加信任于实际观测信息ΔC_m，而实际观测信息ΔC_m中的测量噪声将被改进的卡尔曼增益K_m(β_m)中病态的逆矩阵(GP_m|m-1G^T+(r+β_m)I₂)^-1过度放大，进而造成递归过程的发散。

如图3所示，结合正定矩阵、对称矩阵的性质、奇异值分解和谱分解等理论，可以证明随着修正参数β_m的增大，||K_m(β_m)ΔC_m||将逐渐减小，而||GK_m(β_m)ΔC_m-ΔC_m||将逐渐增大。前者表示当前递归过程在更新状态向量的估计值

时，改进的卡尔曼滤波算法对当前观测信息ΔC_m的信任程度；后者为残差形式，表示没有被信任的观测信息量。曲线上的最大曲率点即为最佳平衡点，此时对应的修正参数β_m可以最小化算法的估计误差。

结合上述分析，改进的L曲线法的计算过程如下：

3.1按照一定的间隔Δβm在区间[0,1×10 2]上对βm进行采样，从而获得一系列的离散点，离散点的横纵坐标分别为横坐标：γ(β_m)＝||GK_m(β_m)ΔC_m-ΔC_m||，纵坐标：η(β_m)＝||K_m(β_m)ΔC_m||。

3.2利用三次样条插值法对离散点进行拟合,从而获得改进的L曲线

η(β_m)＝f_CSI(γ(β_m))

其中，f_CSI表示三次样条插值后所获得改进的L曲线方程。

3.3求解改进的L曲线上的最大曲率

其中，最大曲率点所对应的修正参数β_mi即为最佳的修正参数，需将其带入改进的卡尔曼滤波方法中进行计算。

4.结合卡尔曼滤波方法递归过程中估计误差的协方差矩阵特性，建立修正参数的自适应优选方法

由于协方差矩阵Q和R为固定矩阵，故改进的卡尔曼增益K_m(β_m)的数值在递归过程中的变化是由估计误差的协方差矩阵P_m-1|m-1所决定的，当P_m-1|m-1收敛时β_m也应当逐渐收敛。由于估计误差的协方差矩阵的迹可用来衡量算法的收敛程度，故当m≥2时，采用自适应的方式获得最佳的修正参数β_m，即

其中，τ(m)表示第m次辨识过程中的惩罚因子。

5.将机器人的零点误差辨识结果补偿至机器人的运动学模型中。

当通过辨识获得机器人的零点误差后，需将零点误差的辨识结果补偿至机器人的运动学模型中。如图1和图2所示，若二自由度并联机器人动平台2的中心点P位于工作空间中的任意处，假设机器人不存在零点误差(即Δθ₁＝Δθ₂＝0°)，则第一主动臂5和第二主动臂6所对应的理论转角输出分别为θ₁和θ₂。然而，实际机器人的零点误差Δθ₁和Δθ₂通过辨识得知并不为零，则此时机器人第一主动臂5和第二主动臂6的实际转角输出应当为

θ′₁＝θ₁-Δθ₁,θ′₂＝θ₂-Δθ₂

其中，θ′₂和θ′₂分别表示第一主动臂5和第二主动臂6的实际输出转角。

如图4所示，传统的基于卡尔曼滤波方法的零点辨识模型可能造成如图1所示的二自由度并联机器人零点辨识结果发散，而采用本发明则可以快速获得收敛的零点辨识结果。

Claims (1)

1.一种二自由度高速并联机器人零点标定方法，其步骤如下：

步骤1.建立机器人末端运动误差与零点误差之间的映射模型

二自由度高速并联机器人由静平台、动平台、第一运动支链和第二运动支链以及第一主动臂和第二主动臂组成，第一运动支链和第二运动支链的一端均铰接在动平台上，另一端分别与第一运动支链和第二运动支链铰接，第一主动臂和第二主动臂的旋转轴位于静平台；静平台和动平台之间安装一根伸缩尺，通过伸缩尺间接测量出动平台中心点P到基准坐标系O-xyz原点O之间的距离；所述基准坐标系以第一主动臂和第二主动臂的旋转轴中心连线中点为原点O，第一主动臂和第二主动臂的旋转轴中心连线为x轴，第一主动臂指向第二主动臂方向为正，y轴以远离动平台方向为正，根据右手定则确定基准坐标系O-xyz；第一主动臂和第二主动臂与基准坐标系O-xyz的x轴的夹角分别为θ，θ₂；

初始状态下，当该机器人的第一主动臂和第二主动臂与基准坐标系O-xyz的x轴平行时，即θ₁＝θ₂＝0°，此时机器人的零点误差Δθ₁＝Δθ₂＝0°；

利用空间矢量法和一阶摄动原理获得机器人末端运动的距离误差与机器人零点误差之间的映射模型

ΔC＝GΔp

其中，ΔC为末端运动的距离误差矩阵；Δp为机器人零点误差向量；G为误差传递矩阵；

步骤2.建立改进的卡尔曼滤波方法的机器人零点辨识模型

步骤2.1预估辨识过程的状态向量

其中，

为第m次辨识过程中对于系统状态向量的预估值；

为第m-1次辨识过程后系统状态向量的更新值；在初始状态下，令

步骤2.2预估估计误差的协方差矩阵Q

P_m|m-1＝P_m-1|m-1+Q

其中，P_m|m-1为第m次辨识过程中所预估的估计误差的协方差矩阵；P_m-1|m-1为第m-1次辨识过程后所得到的估计误差的协方差矩阵；在初始状态下，令

P_1|0＝P_0|0＝pI₃

其中，I₃为单位矩阵；p为大于0的常数，如p＝1；

随着改进的卡尔曼滤波方法递归过程的进行，估计误差的协方差矩阵将逐渐下降并收敛；

步骤2.3建立改进的卡尔曼增益表达式

K_m(β_m)＝P_m|m-1G^T(GP_m|m-1G^T+(r+β_m)I₂)^-1

其中，K_m(β_m)为第m次辨识过程中改进的卡尔曼增益；β_m为第m次辨识过程中所采用的修正参数，且β_m>0；r为大于0的常数，I₂为单位矩阵；G为误差传递矩阵；

步骤2.4利用改进的卡尔曼增益更新状态向量

其中，

为第m次辨识过程后所更新的系统状态向量；

步骤2.5更新估计误差的协方差矩阵

P_m|m＝P_m|m-1-K_m(β_m)GP_m|m-1

其中，P_m|m为第m次辨识过程后所更新的估计误差的协方差矩阵；

步骤2.6重复步骤2.1-步骤2.5的递归过程，直至相邻两次状态向量的估计结果满足

其中，ε为人为给定的辨识精度的阈值向量；

步骤3.采用改进的L曲线法对改进的卡尔曼滤波方法中的修正参数进行优选

步骤3.1按照间隔Δβ_m在区间[0,1×10²]上对β_m进行采样，从而获得一系列的离散点，离散点的横纵坐标分别为

横坐标：γ(β_m)＝||GK_m(β_m)ΔC_m-ΔC_m||，纵坐标：η(β_m)＝||K_m(β_m)ΔC_m||；

步骤3.2利用三次样条插值法对离散点进行拟合,从而获得改进的L曲线

η(β_m)＝f_CSI(γ(β_m))

其中，f_CSI表示三次样条插值后所获得改进的L曲线方程；

步骤3.3求解改进的L曲线上的最大曲率

其中，最大曲率点所对应的修正参数β_mi即为最佳修正参数；

步骤4.修正参数的自适应优选

采用自适应的方式获得最佳的修正参数β_m

β_m＝β₁τ(m),

其中，τ(m)表示第m次辨识过程中的惩罚因子；

步骤5.将高速并联机器人的零点误差辨识结果补偿至机器人的运动学模型中。

Images