PatViewer专利搜索

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

发明公布  有效
申请(专利)号:CN201505080528.X国省代码:江苏 32
申请(专利权)人:南京埃斯顿自动化股份有限公司
温馨提示:Ctrl+D 请注意收藏,详细著录项请首页检索查看。 Please note the collection. For details, please search the home page.

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

主权项:
1.一种二自由度高速并联机器人零点标定方法,其步骤如下:步骤1.建立机器人末端运动误差与零点误差之间的映射模型二自由度高速并联机器人由静平台、动平台、第一运动支链和第二运动支链组成;静平台和动平台之间安装一根伸缩尺,通过伸缩尺间接测量出动平台中心点P到基准坐标系O‑xyz原点O之间的距离;初始状态下,当该机器人的第一主动臂和第二主动臂与基准坐标系O‑xyz的x轴平行时,即θ1=θ2=0°,此时机器人的零点误差Δθ1=Δθ2=0°;利用空间矢量法和一阶摄动原理获得机器人末端运动的距离误差与机器人零点误差之间的映射模型ΔC=GΔp其中,ΔC为末端运动的距离误差矩阵;Δp为机器人零点误差向量;G为误差传递矩阵;步骤2.建立改进的卡尔曼滤波方法的机器人零点辨识模型步骤2.1预估辨识过程的状态向量其中,为第m次辨识过程中对于系统状态向量的预估值;为第m‑1次辨识过程后系统状态向量的更新值;在初始状态下,令步骤2.2预估估计误差的协方差矩阵QPm|m‑1=Pm‑1|m‑1+Q其中,Pm|m‑1为第m次辨识过程中所预估的估计误差的协方差矩阵;Pm‑1|m‑1为第m‑1次辨识过程后所得到的估计误差的协方差矩阵;在初始状态下,令P1|0=P0|0=pI3其中,I3为单位矩阵;p为大于0的常数,如p=1;随着改进的卡尔曼滤波方法递归过程的进行,估计误差的协方差矩阵将逐渐下降并收敛;步骤2.3建立改进的卡尔曼增益表达式Km(βm)=Pm|m‑1GT(GPm|m‑1GT+(r+βm)I2)‑1其中,Km(βm)为第m次辨识过程中改进的卡尔曼增益;βm为第m次辨识过程中所采用的修正参数,且βm>0;步骤2.4利用改进的卡尔曼增益更新状态向量其中,为第m次辨识过程后所更新的系统状态向量;步骤2.5更新估计误差的协方差矩阵Pm|m=Pm|m‑1‑Km(βm)GPm|m‑1其中,Pm|m为第m次辨识过程后所更新的估计误差的协方差矩阵;步骤2.6重复步骤2.1‑步骤2.5的递归过程,直至相邻两次状态向量的估计结果满足其中,ε为人为给定的辨识精度的阈值向量;步骤3.采用改进的L曲线法对改进的卡尔曼滤波方法中的修正参数进行优选步骤3.1按照间隔Δβm在区间[0,1×102]上对βm进行采样,从而获得一系列的离散点,离散点的横纵坐标分别为横坐标:γ(βm)=||GKm(βm)ΔCm‑ΔCm||,纵坐标:η(βm)=||Km(βm)ΔCm||;步骤3.2利用三次样条插值法对离散点进行拟合,从而获得改进的L曲线η(βm)=fCSI(γ(βm))其中,fCSI表示三次样条插值后所获得改进的L曲线方程;步骤3.3求解改进的L曲线上的最大曲率其中,最大曲率点所对应的修正参数βmi即为最佳修正参数;步骤4.修正参数的自适应优选采用自适应的方式获得最佳的修正参数βm其中,τ(m)表示第m次辨识过程中的惩罚因子;步骤5.将高速并联机器人的零点误差辨识结果补偿至机器人的运动学模型中。


说明书

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

技术领域

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

背景技术

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

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

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

发明内容

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

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

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

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

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

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

ΔC=GΔp

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

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

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

状态方程:Δpm=Δpm-1+am-1

观测方程:ΔCm=GΔpm+bm

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

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

Q=qI1,R=rI2

其中,q和r为大于0的常数,在零点标定过程中q和r可取较小的数量级,如0~1×
102;I1和I2分别为与协方差矩阵Q和R同阶的单位矩阵。

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

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


其中,为第m次辨识过程中对于系统状态向量的预估值;为第m-1次
辨识过程后系统状态向量的更新值。在初始状态下,令


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

Pm|m-1=Pm-1|m-1+Q

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

P1|0=P0|0=pI3

其中,I3为单位矩阵;p为大于0的常数,如p=1。

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

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

Kmm)=Pm|m-1GT(GPm|m-1GT+(r+βm)I2)-1

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

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


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

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

Pm|m=Pm|m-1-Kmm)GPm|m-1

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

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


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

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

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


WXm)=I1-Kmm)G,WYm)=Kmm)

可见,状态向量是通过对其预估值和实际观测信息ΔCm之间按照一定
的权重进行分配后计算获得的,对应权重分别为WXm)和WYm)。若修正参数βm→0,不合理
的过程噪声和测量噪声协方差矩阵Q和R将致使WYm)过高,从而状态向量的更新过程
更加信任于实际观测信息ΔCm,而实际观测信息ΔCm中的测量噪声将被改进的卡尔曼增益
Kmm)中病态的逆矩阵(GPm|m-1GT+(r+βm)I2)-1过度放大,进而造成递归过程的发散。

结合正定矩阵、对称矩阵的性质、奇异值分解和谱分解等理论,可以证明随着修正
参数βm的增大,||Kmm)ΔCm||将逐渐减小,而||GKmm)ΔCm-ΔCm||将逐渐增大。前者表示
当前递归过程在更新状态向量的估计值时,改进的卡尔曼滤波算法对当前观测信息
ΔCm的信任程度;后者为残差形式,表示没有被信任的观测信息量。曲线上的最大曲率点即
为最佳平衡点,此时对应的修正参数βm可以最小化算法的估计误差。

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

步骤3.1按照间隔Δβm在区间[0,1×102]上对βm进行采样,从而获得一系列的离散
点,离散点的横纵坐标分别为横坐标:γ(βm)=||GKmm)ΔCm-ΔCm||,纵坐标:η(βm)=||Km
m)ΔCm||。

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

η(βm)=fCSI(γ(βm))

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

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


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

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

结合卡尔曼滤波方法递归过程中估计误差的协方差矩阵特性,建立修正参数的自
适应优选方法。由于协方差矩阵Q和R为固定矩阵,故改进的卡尔曼增益Kmm)的数值在递归
过程中的变化是由估计误差的协方差矩阵Pm-1|m-1所决定的,当Pm-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轴平行时(即θ1=θ2=0°),此时机器人的零点误差Δθ1=Δθ2=0°;否则该机
器人的零点误差Δθ1和Δθ2不为零。

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

ΔC=GΔp

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

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

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

状态方程:Δpm=Δpm-1+am-1

观测方程:ΔCm=GΔpm+bm

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

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

Q=qI1,R=rI2

其中,q和r为大于0的常数,在零点标定过程中q和r可取较小的数量级,如0~1×
102;I1和I2分别为与协方差矩阵Q和R同阶的单位矩阵。

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

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


其中,为第m次辨识过程中对于系统状态向量的预估值;为第m-1次
辨识过程后系统状态向量的更新值。在初始状态下,可令


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

Pm|m-1=Pm-1|m-1+Q

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

P1|0=P0|0=pI3

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

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

Kmm)=Pm|m-1GT(GPm|m-1GT+(r+βm)I2)-1

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

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


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

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

Pm|m=Pm|m-1-Kmm)GPm|m-1

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

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


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

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

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

将更新后状态向量的表达式改写为


WXm)=I1-Kmm)G,WYm)=Kmm)

可见,状态向量是通过对其预估值和实际观测信息ΔCm之间按照一定
的权重进行分配后计算获得的,对应权重分别为WXm)和WYm)。若修正参数βm→0,不合理
的过程噪声和测量噪声协方差矩阵Q和R将致使WYm)过高,从而状态向量的更新过程
更加信任于实际观测信息ΔCm,而实际观测信息ΔCm中的测量噪声将被改进的卡尔曼增益
Kmm)中病态的逆矩阵(GPm|m-1GT+(r+βm)I2)-1过度放大,进而造成递归过程的发散。

如图3所示,结合正定矩阵、对称矩阵的性质、奇异值分解和谱分解等理论,可以证
明随着修正参数βm的增大,||Kmm)ΔCm||将逐渐减小,而||GKmm)ΔCm-ΔCm||将逐渐增
大。前者表示当前递归过程在更新状态向量的估计值时,改进的卡尔曼滤波算法对当
前观测信息ΔCm的信任程度;后者为残差形式,表示没有被信任的观测信息量。曲线上的最
大曲率点即为最佳平衡点,此时对应的修正参数βm可以最小化算法的估计误差。

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

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

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

η(βm)=fCSI(γ(βm))

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

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


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

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

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


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

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

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

θ′1=θ1-Δθ1,θ′2=θ2-Δθ2

图1
©2018 IPPH.cn   PatViewer·专利搜索
主办单位:知识产权出版社有限责任公司  咨询热线:01082000860-8588
浏览器:IE9及以上、火狐等  京ICP备09007110号 京公网安备 11010802026659号