CN105425582A - 一种基于卡尔曼滤波的Stewart机构在线标定方法 - Google Patents

Abstract

一种基于卡尔曼滤波的Stewart机构在线标定算法，它有五大步骤：一、将位姿测量设备固定在Stewart机构一侧，其工作空间覆盖Stewart机构的运动范围；二、调整Stewart机构位姿，使Stewart机构动平台沿不同方向运动，利用位姿测量设备测量Stewart机构动平台位姿，对比理论位姿数据与测量得到的位姿数据，得到位姿误差数据；三、将理论位姿数据与位姿误差数据代入卡尔曼滤波算法中，通过迭代运算，得到误差源数据；四、将误差源数据代入Stewart机构运动学正解中，对运动学正解进行修正；五、循环执行第二步至第四步，直到位姿误差数据收敛为止。本发明操作简单、实施成本低，标定效率高。

Description

一种基于卡尔曼滤波的Stewart机构在线标定方法

技术领域

本发明涉及并联机构技术领域，尤其涉及一种基于卡尔曼滤波的Stewart机构在线标定方法。

背景技术

Stewart机构具有较高的精度、刚度和负载能力等一系列优点，在工业领域得到了广泛的应用。精度是衡量Stewart机构性能的重要指标。采用先进的机构设计、加工和制造方法避免机构误差，可以提高Stewart机构的精度，但是实施成本较高，不能满足工业上的需求。运动学标定作为一种后验的误差补偿方法，是提高精度的有效方法。

机构标定的步骤为：首先建立机构的数学模型，用于描述机构支链误差与末端误差之间的函数关系；在此基础上通过实验的方式对机构进行测量，得到机构的实测数据；之后将得到的数据代入机构数学模型中进行辨识求解，计算误差量；最后将误差代入机构的运动学方程进行补偿。

Stewart机构的数据模型为多元线性方程组，理论上可以直接通过求方程组的解计算机构末端误差。但是，由于机构的实测数据存在一定的测量误差，使线性方程组的系数产生较大的摄动，从而得到错误解。这种现象被称为线性方程组的病态问题。解决方法是通过优化算法对线性方程组进行估计。Stewart机构标定常用的估计方法为最小二乘法。最小二乘法虽然能够解决标定问题，但是这种方法是一种离线估计方法，需要在标定前得到大量的测量数据，标定效率较低。卡尔曼滤波算法作为一种在线标定方法，利用实时数据进行标定，提高了最小二乘法的效率。目前卡尔曼滤波算法主要应用于导航或过程控制领域，尚未应用于Stewart机构的标定。如发明人李保国等人提出的专利“一种基于卡尔曼滤波的光纤捷联惯导系统现场标定方法”(专利申请号：201410116707.9)，该专利将卡尔曼滤波方法应用于光纤捷联惯导系统。

发明内容

本发明提供一种基于卡尔曼滤波的Stewart机构在线标定方法，用于解决现有技术中的实施成本高、无法在线标定、标定效率低等问题。

本发明提供的一种基于卡尔曼滤波的Stewart机构在线标定算法，其特征在于：它包括如下步骤：

步骤一：将位姿测量设备固定在Stewart机构一侧，其工作空间覆盖Stewart机构的运动范围；

步骤二：调整Stewart机构位姿，使Stewart机构动平台沿不同方向运动，利用位姿测量设备测量Stewart机构动平台位姿，对比理论位姿数据与测量得到的位姿数据，得到位姿误差数据；

步骤三：将理论位姿数据与位姿误差数据代入卡尔曼滤波算法中，通过迭代运算，得到误差源数据；

步骤四：将误差源数据代入Stewart机构运动学正解中，对运动学正解进行修正。

步骤五：循环执行第二步至第四步，直到位姿误差数据收敛为止。

其中，步骤一中所述的位姿测量设备包括但不限于三坐标测量仪、视觉测量仪；

其中，步骤二中所述的位姿误差数据δe_i(i＝1,2,L,n)可以表达为：

${\mathrm{\δe}}_i={\left[\begin{array}{cc}{\mathrm{\δp}}_i^T& {\mathrm{\δ\ω}}_i^T\end{array}\right]}^T=P_i^{\′}-P_i$

式中，P_i′为测量得到的位姿数据，P_i为理论位姿数据，n为测量次数，δp为位置误差向量，δω为姿态误差向量。

所述的误差源数据x＝[δL^Tδd^T]^T，定义a_i为Stewart机构动平台铰点A_i在{A}中的位置向量，b_i为静平台铰点B_i在{B}中的位置向量，L_i为第i个驱动杆从B_i到A_i的长度，i＝1,2,L6。式中，δL为L_i的误差，δd为a_i和b_i的误差。

其中，步骤三所述的“将理论位姿数据与位姿误差数据代入卡尔曼滤波算法中，通过迭代运算，得到误差源数据；”其具体实现过程如下：

步骤1：建立Stewart机构的误差模型

定义u_i为L_i的方向矢量，i＝1,2,L,6，为Stewart机构动平台坐标系{A}到Stewart机构静平台坐标系{B}的方向余弦矩阵。

Stewart机构动平台中心点的误差δe可以表达为：

δe＝Jx

式中，J为误差雅克比矩阵。

$J=\[\begin{array}{cc}{J}_P^{-1}& -J_P^{-1}{J}_C\end{array}\]$

式中，J_P为雅克比矩阵：

$J_P=\left[\begin{array}{c}{u_1}^T{(R_A^B{a}_1\×u_1)}^T\\ \begin{array}{cc}M& M\end{array}\\ {u_6}^T{(R_A^B{a}_6\×u_6)}^T\end{array}\right]$

J_C为沿驱动杆方向矢量：

$J_C=\left[\begin{array}{ccccc}{u_1}^{T}R_A^B& -u_1^T& L& 0& 0\\ M& M& O& M& M\\ 0& 0& L& {u_6}^{T}R_A^B& -u_6^T\end{array}\right]$

步骤2：建立卡尔曼滤波器模型。

Stewart机构的误差模型可以进一步改写为：

δe_t＝Jx_t+ε_t,t＝1,L,N

式中，δe_t为观测误差量，x_t为状态量。N为机器人进行位姿变换的次数，ε_t为测量噪声，v_t:N(0,R_t)。

定义ω_t为过程噪声，w_t:N(0,Q_t)，y_t为观测量。则Stewart机构标定模型的状态转移方程和测量方程为：

$\left\{\begin{array}{c}{x}_t=x_{t-1}+w_{t-1}\\ y_t={\mathrm{\δe}}_t=J_t{x}_t+{\mathrm{\ϵ}}_t\end{array}\right.$

定义和P_t是t时刻系统状态x_t的估计和协方差，则系统状态预测方程及y_t的估计值：

$\left\{\begin{array}{c}{\hat{x}}_{t|t-1}={\hat{x}}_{t-1}\\ {\hat{y}}_{t|t-1}=J_t{\hat{x}}_{t|t-1}=J_t{\mathrm{\φ}}_{t|t-1}{\hat{x}}_{t-1}\end{array}\right.$

增益方程：

$K_t=P_{t|t-1}{J}_t^T{(J_t{P}_{t|t-1}{J}_t^T+R_t)}^{-1}$

滤波方程：

${\hat{x}}_t={\hat{x}}_{t-1}+K_t(y_t-J_t{\hat{x}}_{t-1})$

预测误差：

P_t|t-1＝P_t-1+Q_k-1

估计误差：

P_t＝(I-K_tJ_t)P_t|t-1。

上述各式中的符号说明如下：K_t为卡尔曼增益，P_t为系统状态的方差。

与现有技术相比，本发明的有益效果是：

1.该方法为一种运动学标定方法，标定操作简单、实施成本低；

2.该方法能够实现在线标定，标定效率高；

附图说明

图1为本发明实施例提供的基于卡尔曼滤波的Stewart机构在线标定方法流程图。

图2为Stewart机构简图。

图中符号说明如下：

图1中，为系统状态的估计，P_t为系统状态的方差，t为系统时刻，K_t为卡尔曼增益，J_t为误差雅克比矩阵，R_t为测量噪声的方差，y_t为观测量。I为单位矩阵。x_t为状态量

图2中，L_i(i＝1,2,L,6)为Stewart机构的支杆，O_A-X_AY_AZ_A为Stewart机构动平台坐标系{A}，O_B-X_BY_BZ_B为Stewart机构静平台坐标系{B}，a₁(i＝1,2,L,6)为动平台铰链中心点在{A}中的位置向量，b₁(i＝1,2,L,6)为静平台铰链中心点在{B}中的位置向量。

具体实施方式

下面参照附图来说明本发明的实施例。在本发明的一个附图或一种实施方式中描述的元素和特征可以与一个或更多个其它附图或实施方式中示出的元素和特征相结合。应当注意，为了清楚的目的，附图和说明中省略了与本发明无关的、本领域普通技术人员已知的部件和处理的表示和描述。

下面结合附图，对本发明的技术方案做进一步的说明。见图1—图2，

本发明提供的一种基于卡尔曼滤波的Stewart机构在线标定算法，具体包括如下步骤：

步骤一：将位姿测量设备固定在Stewart机构一侧，其工作空间覆盖Stewart机构的运动范围；

所述的位姿测量设备包括但不限于三坐标测量仪、视觉测量仪；

步骤二：调整Stewart机构位姿，使Stewart机构动平台沿不同方向运动，利用位姿测量设备测量Stewart机构动平台位姿，对比理论位姿数据与测量得到的位姿数据，得到位姿误差数据；

所述的位姿误差数据δe_i(i＝1,2,L,n)可以表达为：

δe_i＝[δp_i ^Tδω_i ^T]^T＝P_i′-P_i

式中，P_i′为测量得到的位姿数据，P_i为理论位姿数据，n为测量次数，δp为位置误差向量，δω为姿态误差向量。

所述的误差源数据x＝[δL^Tδd^T]^T，定义a_i为Stewart机构动平台铰点A_i在{A}中的位置向量，b_i为静平台铰点B_i在{B}中的位置向量，L_i为第i个驱动杆从B_i到A_i的长度，i＝1,2,L6。式中，δL为L_i的误差，δd为a_i和b_i的误差。

步骤三：将理论位姿数据与位姿误差数据代入卡尔曼滤波算法中，通过迭代运算，得到误差源数据；

所述的卡尔曼滤波算法包括以下几个步骤：

步骤1：建立Stewart机构的误差模型

定义u_i为L_i的方向矢量，i＝1,2,L,6，为Stewart机构动平台坐标系{A}到Stewart机构静平台坐标系{B}的方向余弦矩阵，

Stewart机构动平台中心点的误差δe可以表达为：

δe＝Jx

式中，J为误差雅克比矩阵。

$J=\[\begin{array}{cc}{J}_P^{-1}& -J_P^{-1}{J}_C\end{array}\]$

式中，J_C为雅克比矩阵：

$J_P=\left[\begin{array}{c}{u_1}^T{(R_A^B{a}_1\×u_1)}^T\\ \begin{array}{cc}M& M\end{array}\\ {u_6}^T{(R_A^B{a}_6\×u_6)}^T\end{array}\right]$

J_C为沿驱动杆方向矢量：

$J_C=\left[\begin{array}{ccccc}{u_1}^{T}R_A^B& -u_1^T& L& 0& 0\\ M& M& O& M& M\\ 0& 0& L& {u_6}^{T}R_A^B& -u_6^T\end{array}\right]$

步骤2：建立卡尔曼滤波器模型。

Stewart机构的误差模型可以进一步改写为：

δe_t＝Jx_t+ε_t,t＝1,L,N

式中，δe_t为观测误差量，x为状态量。N为机器人进行位姿变换的次数，ε_t为测量噪声，v_t:N(0,R_t)。

定义ω_t为过程噪声，w_t:N(0,Q_t)。则Stewart机构标定模型的状态转移方程和测量方程为：

$\left\{\begin{array}{c}{x}_t=x_{t-1}+w_{t-1}\\ y_t={\mathrm{\δe}}_t=J_t{x}_t+{\mathrm{\ϵ}}_t\end{array}\right.$

定义和P_t是t时刻系统状态x_t的估计和协方差，则系统状态预测方程及y_t的估计值：

$\left\{\begin{array}{c}{\hat{x}}_{t|t-1}={\hat{x}}_{t-1}\\ {\hat{y}}_{t|t-1}=J_t{\hat{x}}_{t|t-1}=J_t{\mathrm{\φ}}_{t|t-1}{\hat{x}}_{t-1}\end{array}\right.$

增益方程：

$K_t=P_{t|t-1}{J}_t^T{(J_t{P}_{t|t-1}{J}_t^T+R_t)}^{-1}$

滤波方程：

${\hat{x}}_t={\hat{x}}_{t-1}+K_t(y_t-J_t{\hat{x}}_{t-1})$

预测误差：

P_t|t-1＝P_t-1+Q_k-1

估计误差：

P_t＝(I-K_tJ_t)P_t|t-1

上述各式中的符号说明如下：K_t为卡尔曼增益，P_t为系统状态的方差。

步骤四：将误差源数据代入Stewart机构运动学正解中，对运动学正解进行修正。

步骤五：循环执行第二步至第四步，直到位姿误差数据收敛为止。

以上参照附图对本发明的描述是示意性的，没有限制性，本领域的技术人员应该能够理解，在实际实施中，本发明中各构件的形状和布局方式均可能发生某些改变；而在本发明的启示下，其他人员也可以做出与本发明相似的设计或对本发明做出修改以及某个构件的等同替换。特别需要指出的是，只要不脱离本发明的设计宗旨，所有显而易见的改变以及具有等同替换的相似设计，均包含在本发明的保护范围之内。

Claims (4)

1.一种基于卡尔曼滤波的Stewart机构在线标定算法，其特征在于：它包括如下步骤：

步骤一：将位姿测量设备固定在Stewart机构一侧，其工作空间覆盖Stewart机构的运动范围；

步骤二：调整Stewart机构位姿，使Stewart机构动平台沿不同方向运动，利用位姿测量设备测量Stewart机构动平台位姿，对比理论位姿数据与测量得到的位姿数据，得到位姿误差数据；

步骤三：将理论位姿数据与位姿误差数据代入卡尔曼滤波算法中，通过迭代运算，得到误差源数据；

步骤四：将误差源数据代入Stewart机构运动学正解中，对运动学正解进行修正；

步骤五：循环执行第二步至第四步，直到位姿误差数据收敛为止。

2.根据权利要求1所述的一种基于卡尔曼滤波的Stewart机构在线标定算法，其特征在于：

步骤一中所述的位姿测量设备包括但不限于三坐标测量仪、视觉测量仪。

3.根据权利要求1所述的一种基于卡尔曼滤波的Stewart机构在线标定算法，其特征在于：

步骤二中所述的位姿误差数据δe_i(i＝1,2,L,n)表达为：

δe_i＝[δp_i ^Tδω_i ^T]^T＝P′_i-P_i

式中，P′_i为测量得到的位姿数据，P_i为理论位姿数据，n为测量次数，δp为位置误差向量，δω为姿态误差向量；

所述的误差源数据x＝[δL^Tδd^T]^T，定义a_i为Stewart机构动平台铰点A_i在{A}中的位置向量，b_i为静平台铰点B_i在{B}中的位置向量，L_i为第i个驱动杆从B_i到A_i的长度，i＝1,2,L6，式中，δL为L_i的误差，δd为a_i和b_i的误差。

4.根据权利要求1所述的一种基于卡尔曼滤波的Stewart机构在线标定算法，其特征在于：步骤三所述的“将理论位姿数据与位姿误差数据代入卡尔曼滤波算法中，通过迭代运算，得到误差源数据；”其具体实现过程如下：

步骤1：建立Stewart机构的误差模型

定义u_i为L_i的方向矢量，i＝1,2,L,6，为Stewart机构动平台坐标系{A}到Stewart机构静平台坐标系{B}的方向余弦矩阵；

Stewart机构动平台中心点的误差δe表达为：

δe＝Jx

式中，J为误差雅克比矩阵；

$J=\left[\begin{array}{cc}{J}_P^{-1}& -J_P^{-1}{J}_C\end{array}\right]$

式中，J_P为雅克比矩阵；

$J_P=\left[\begin{array}{c}{u_1}^T{\left(R_A^B{a}_1\×u_1\right)}^T\\ \begin{array}{cc}M& M\end{array}\\ {u_6}^T{\left(R_A^B{a}_6\×u_6\right)}^T\end{array}\right]$

J_C为沿驱动杆方向矢量；

$J_C=\left[\begin{array}{ccccc}{u_1}^{T}R_A^B& -u_1^T& L& 0& 0\\ M& M& O& M& M\\ 0& 0& L& {u_6}^{T}R_A^B& -u_6^T\end{array}\right]$

步骤2：建立卡尔曼滤波器模型；

Stewart机构的误差模型进一步改写为：

δe_t＝Jx_t+ε_t,t＝1,L,N

式中，δe_t为观测误差量，x_t为状态量，N为机器人进行位姿变换的次数，ε_t为测量噪声，v_t:N(0,R_t)；

定义ω_t为过程噪声，w_t:N(0,Q_t)，y_t为观测量，则Stewart机构标定模型的状态转移方程和测量方程为：

$\left\{\begin{array}{c}{x}_t=x_{t-1}+w_{t-1}\\ y_t={\mathrm{\δe}}_t=J_t{x}_t+{\mathrm{\ϵ}}_t\end{array}\right.$

定义和P_t是t时刻系统状态x_t的估计和协方差，则系统状态预测方程及y_t的估计值：

$\left\{\begin{array}{c}{\hat{x}}_{t|t-1}={\hat{x}}_{t-1}\\ {\hat{y}}_{t|t-1}=J_t{\hat{x}}_{t|t-1}=J_t{\mathrm{\φ}}_{t|t-1}{\hat{x}}_{t-1}\end{array}\right.$

增益方程：

$K_t=P_{t|t-1}{J}_t^T{(J_t{P}_{t|t-1}{J}_t^T+R_t)}^{-1}$

滤波方程：

${\hat{x}}_t={\hat{x}}_{t-1}+K_t(y_t-J_t{\hat{x}}_{t-1})$

预测误差：

P_t|t-1＝P_t-1+Q_k-1

估计误差：

P_t＝(I-K_tJ_t)P_t|t-1

上述各式中的符号说明如下：K_t为卡尔曼增益，P_t为系统状态的方差。