CN103454029B - 基于卡尔曼滤波与多次采集的多维力线性解耦方法 - Google Patents

Abstract

本发明针对多维力传感器的维间耦合问题，提出一种基于卡尔曼滤波与多次采集的分段线性解耦方法。其中包括：建立耦合误差模型，对多维力传感器每个力值的正负方向分别推导力值-电压的输入-输出公式，然后对多维力传感器做静态标定实验，采用卡尔曼滤波的方法对多维力传感器输出的电压进行滤波，并对每个加载力值点多次测量，得到加载力值与三维力传感器输出电压的输入输出关系图，然后采用求解超定线性方程组最小二乘解的方法，得到力值-电压拟合公式的待定系数，对系数矩阵求逆得到多维力传感器的解耦矩阵，完成解耦过程。本方法与传统解耦方法相比，在不增加计算量的情况下，能够显著提高多维力传感器的解耦精度。

Description

基于卡尔曼滤波与多次采集的多维力线性解耦方法

技术领域

本发明用于传感器领域，是一种多维力传感器解耦方法。适用于减小或消除多维力传感器的维间耦合干扰，提高多维力传感器的测量精度。

背景技术

多维力传感器是用于测量多维空间中力／力矩分量的传感器，主要用于机器人手腕部，利用它来测量机器人手部的受力情况并实现力反馈控制。随着研究的不断进步，六维力传感器已经广泛的应用于航空航天、制造与装配、体育竞技以及遥操作机器人等领域。多维力传感器的耦合误差的存在影响了多维力传感器精度的提高，同时也限制了多维力传感器在高精度测量与控制领域的应用。

单维力传感器能够通过改进结构，增加补偿测量应变电桥，达到很高的测量精度。多维力传感器由于需要兼顾不同方向力的灵敏度与精度的要求，单维力传感器的研究方法并不适用。多维力传感器在各个方向上都是对力敏感的，在传感器的某一维施加力会在其它方向上有输出，被称为耦合误差。

减小多维力传感器耦合误差的方法主要可分为优化传感器结构与改进解耦算法两类。通过优化多维力传感器结构减少耦合误差的方法，受机械加工误差，应变电桥的电阻误差及贴片位置等因数的影响较大。成本高昂，且良品率低，使得现阶段的实用性不高，而改进解耦算法具有可行性强、精确度高、价格低廉等优点，成为目前多维力传感器减少耦合误差的实用解决方案。

传统的解耦方法未能完整的标定耦合方向的数据，不能得到稳定可靠的解耦系数，因此影响了多维力传感器解耦精度的提高。

发明内容

本发明提出一种能够实现高精度解耦的基于卡尔曼滤波与多次采集的多维力传感器线性解耦方法。

本发明采用如下技术方案：

一种基于卡尔曼滤波与多次采集的多维力传感器线性解耦方法，包括以下几个步骤：

步骤1：采用分析方法建立多维力传感器耦合误差模型：

步骤1.1：对n维力／力矩传感器，n为正整数，n≤6，共有n维力／力矩输入f₁，f₂，...f_n和对应n维输出电压值u₁，u₂，...u_n，其中，f₁对u_i的影响值在u_i所占的部分为u_i，1，f₁对u_i的影响值在u_i所占的部分为u_i，2，…，f_n对u_i的影响值在u_i所占的部分为u_i，n，其中i＝1，2，3，...n，则

$\left\{\begin{array}{c}{u}_1=u_{\mathrm{1,1}}+u_{\mathrm{1,2}}+\·\·\·+u_{1,n}\\ u_2=u_{\mathrm{2,1}}+u_{\mathrm{2,2}}+\·\·\·+u_{2,n}\\ \·\·\·\\ u_n=u_{n,1}+u_{n,2}+\·\·\·+u_{n,n}\end{array}\right.---\left(1\right)$

上式可简写为：

$\left\{\begin{array}{c}{u}_1=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{u}_{1,i}\\ u_2=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{u}_{2,i}\\ \·\·\·\\ u_n=\underset{i=1}{\overset{n}{\mathrm{\Σ}}}{u}_{n,i}\end{array}\right.---\left(2\right)$

设第s维输入力／力矩f_s在第i维输出电压为u_i，s，其中s=1，2，3，...n，i＝1，2，3，...n，u_i，s与f_s之间的斜率为k_i，s，即：

u_i，s=k_i，s·f_s           (3)

将式(3)带入式(1)得到多维力传感器耦合误差模型：

$\left[\begin{array}{c}{u}_1\\ u_2\\ \·\\ \·\\ \·\\ u_n\end{array}\right]=\left[\begin{array}{cccc}{k}_{\mathrm{1,1}}& k_{\mathrm{1,2}}& \·\·\·& k_{1,n}\\ k_{\mathrm{2,1}}& k_{\mathrm{2,2}}& \·\·\·& k_{2,n}\\ \·& \·& & \·\\ \·& \·& k_{i,s}& \·\\ \·& \·& & \·\\ k_{n,1}& k_{n,2}& ...& k_{n,n}\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ \·\\ \·\\ \·\\ f_n\end{array}\right]---\left(4\right)$

步骤1.2：将多维力／力矩空间按照n维空间坐标轴的正负方向分解成2ⁿ个空间，并按照编号规则对2ⁿ个空间进行编号，其编号规则为：编号由第n维的编号值、第n-1维的编号值、...、第3维的编号值、第2维的编号值及第1维的编号值顺序排列构成，当第s维输入力／力矩f_s>0时，第s维的编号值取值为0，当第s维输入力／力矩f_s<0时，第s维的编号值取值为1，s=n，n-1，...，3，2，1，通过上述规则即可将多维力传感器耦合误差模型公式(4)分解为2ⁿ个公式，并统称为公式(5)，公式前面小括号内为编号：

…     (5)

步骤1.3：将公式(5)中k_i，s且i＝1，2，3...n-1，n、s=1，2，3...n-1，n组成的系数矩阵分别求逆，得到对应的逆矩阵，并对公式(5)中各个等式的两边分别左乘对应的逆矩阵，并保持其编号不变，得到n维力/力矩传感器的解耦公式：

…       (6)

步骤2：建立卡尔曼滤波方程：

步骤2.1：在传感器标定过程中的加载力值稳定之后，对每个维度的输出电压分别建立卡尔曼滤波器方程，多维力传感器工作在线性状态，对传感器的每个维度建立线性离散系统的状态空间模型，其任一维度的表达式为：

$\left\{\begin{array}{c}{x}_t=x_{t-1}+w_t\\ u_t=x_t+v_t\end{array}\right.---\left(7\right)$

其中：

x_t为t时刻传感器的状态；

x_t-1为t-1时刻传感器的状态；

u_t为t时刻传感器的输出电压；

w_t、v_t分别为t时刻的传感器噪声和观测噪声，且相互独立，他们被设成高斯白噪声，且传感器噪声和观测噪声的协方差分别为Q，R，

步骤2.2：建立了如下卡尔曼滤波器方程：

(1)时间更新方程

其中：

为t时刻传感器状态的先验估计；

为t-1时刻传感器状态的后验估计；

为t时刻传感器的先验估计均方误差；

p_t-1为t-1时刻传感器的后验估计均方误差；

Q为传感器噪声的协方差；

(2)测量更新方程

其中：G_t为t时刻卡尔曼滤波器的增益；

为t时刻传感器的先验估计均方误差；

p_t为t时刻传感器的后验估计均方误差；

为t时刻传感器状态的后验估计；

为t时刻传感器状态的先验估计；

u_t为t时刻传感器的输出电压；

R为传感器观测噪声的协方差；

步骤3：进行多维力传感器静态标定试验，获取静态标定试验数据：

对n维力／力矩传感器进行标定加载试验，将每维的满量程加载力平均分成m个加载力值点，m≥10，将载荷从零值逐步加载至正向满量程，再逐步减少至零，并对每个加载力值点采集多维力传感器每个维度的输出电压，并用卡尔曼滤波器滤波后得到标定数据，每个加载／卸载力值点保存j组滤波后的数据，共进行了b次标定，每个力值点共有2×j×b组数据，

步骤4：确定多维力传感器耦合误差模型中的斜率：

步骤4.1：由标定数据绘制每个方向的加载力值与n个维度电压之间的区域图，(并以加载力为坐标的横轴，输出电压为纵轴)，找出每个加载力值点对应的多个采集电压的最大值与最小值，并对其取平均，得到输出电压的中间值，

步骤4.2：对每个方向上加载的力值与输出电压的中间值采用求超定线性方程组的最小二乘解的方法，得到斜率k_i，s，且i=1，2，3...n-1，n、s=1，2，3...n-1，n，

步骤4.3：将上一步骤中求解的斜率k_i，s代入公式(6)中，并对其系数矩阵求逆，得到标定传感器的解耦公式，

步骤5：标定后的多维力传感器测力解耦过程：

步骤5.1：对多维力传感器n个维度的零点电压进行卡尔曼滤波后，并采集j组零点电压数据，分别计算出n个维度零点电压的最大值与最小值之间的中间值，并作为传感器的零点，

步骤5.2：多维力传感器加载的力值f₁，f₂，f₃，...，f_n后，对多维力传感器输出的电压进行编号，其编号规则为：编号由第n维输出电压的编号值、第n-1维输出电压的编号值、...、第3维输出电压的编号值、第2维输出电压的编号值及第1维输出电压的编号值顺序排列构成，第s维输出的电压u为经过卡尔曼滤波后，减去初始零点的电压值，当u>0时，第s维输出电压的编号值取值为0，当u<0时，第s维输出电压的编号值取值为1，从步骤3.1中找出编号与电压编号相同的解耦方程组，将测量电压值带入求解，得到解耦后的多维力值的大小及方向。

与现有技术相比，本发明具有如下优点：

首先建立耦合误差模型，得到耦合误差公式。通过对每个维度的正负方向分别线性解耦并编号，得到带有编号的解耦公式。对多维力传感器标定实验，并采用卡尔曼滤波器对n个通道电压进行滤波，对每个加载力值点上传感器输出的各通道电压进行多次采集。对每个方向上加载的力值与输出电压的中间值采用求超定线性方程组的最小二乘解的方法，得到斜率。将斜率代入公式，求解得到解耦方程组。

由于多维力传感器耦合输出电压对电路噪声敏感，采用卡尔曼滤波的方法提高了耦合电压的采集精度，为实现高精度解耦提供了可靠的数据，对每个加载力值点进行多次测量，并保存所有的测量数据，得到加载力值与多维力传感器输出电压的区域图，采用求解超定线性方程组最小二乘解的方法，得到更加稳定、可靠且过零点的力值-电压的斜率，针对多维力传感器耦合输出非线性的特点，采用在正、负方向分别线性解耦，并采用电压编号与方程组编号匹配的方法，使解耦计算量不因方程组数目的增加而增加。

附图说明

图1是本发明解耦方法的流程图。

图2是本发明标定试验装置示意图。

图3为加载力f_x与输出电压u_x的区域图。

图4为加载力f_y与输出电压u_y的区域图。

图5为加载力f_z与输出电压u_z的区域图。

图6为加载力f_x+与耦合输出电压u_y的区域图。

图7为加载力f_x+与耦合输出电压u_z的区域图。

图8为加载力f_x-与耦合输出电压u_y的区域图。

图9为加载力f_x-与耦合输出电压u_z的区域图。

图10为加载力f_y+与耦合输出电压u_x的区域图。

图11为加载力f_y+与耦合输出电压u_z的区域图。

图12为加载力f_y-与耦合输出电压u_x的区域图。

图13为加载力f_y-与耦合输出电压u_z的区域图。

图14为加载力f_z+与耦合输出电压u_x的区域图。

图15为加载力f_z+与耦合输出电压u_y的区域图。

图16为加载力f_z-与耦合输出电压u_x的区域图。

图17为加载力f_z-与耦合输出电压u_y的区域图。

具体实施方式

本发明以东南大学江苏省远程测控技术重点实验室研制的十字梁型三维腕力传感器为例，来说明解耦方法的实施过程。该三维力传感器采用十字梁加浮动梁结构，并在传感器的十字梁上分别贴有电阻应变片，三维力传感器弹性体受力引起十字梁的微小变形，这种变形通过电阻应变片转换成电信号并输出。

图1为本发明解耦方法的流程图。分为标定与解耦两个部分，标定实验得到的数据为可靠的解耦做准备，当多维力传感器标定完成后，在使用过程中仅需要完成解耦部分的流程。

一种基于卡尔曼滤波与多次采集的多维力传感器线性解耦方法，包括以下几个步骤：

步骤1：采用分析方法建立三维力传感器耦合误差模型：

步骤1.1：对三维力传感器，共有三维力输入f₁，f₂，f₃和对应三维输出电压值u₁，u₂，u₃，其中，f₁对u_i的影响值在u_i所占的部分为u_i，1，f₂对u_i的影响值在u_i所占的部分为u_i，2，f₃对u_i的影响值在u_i所占的部分为u_i，3，其中i＝1，2，3，则

$\left\{\begin{array}{c}{u}_1=u_{\mathrm{1,1}}+u_{\mathrm{1,2}}+u_{\mathrm{1,3}}\\ u_2=u_{\mathrm{2,1}}+u_{\mathrm{2,2}}+u_{\mathrm{2,3}}\\ u_3=u_{\mathrm{3,1}}+u_{\mathrm{3,2}}+u_{3,3}\end{array}\right.---\left(1\right)$

上式可简写为：

$\left\{\begin{array}{c}{u}_1=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_{1,i}\\ u_2=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_{2,i}\\ u_3=\underset{i=1}{\overset{3}{\mathrm{\&Sigma;}}}{u}_{3,i}\end{array}\right.---\left(2\right)$

设第s维输入力f_s在第i维输出电压为u_i，s，其中s＝1，2，3，i＝1，2，3，u_i，s与f_s之间的斜率为k_i，s，即：

u_i，s＝k_i，s·f_s        (3)

将式(3)带入式(1)得到三维力传感器耦合误差模型：

$\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]=\left[\begin{array}{ccc}{k}_{\mathrm{1,1}}& k_{\mathrm{1,2}}& k_{\mathrm{1,3}}\\ k_{\mathrm{2,1}}& k_{\mathrm{2,2}}& k_{\mathrm{2,3}}\\ k_{\mathrm{3,1}}& k_{\mathrm{3,2}}& k_{\mathrm{3,3}}\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ f_3\end{array}\right]---\left(4\right)$

步骤1.2：将三维力空间按照三维空间坐标轴的正负方向分解成2³个空间，并按照编号规则对2³个空间进行编号，其编号规则为：编号由第3维的编号值、第2维的编号值及第1维的编号值顺序排列构成，当第s维输入力/力矩f_s＞0时，第s维的编号值取值为0，当第s维输入力/力矩f_s＜0时，第s维的编号值取值为1，其中s＝3，2，1，通过上述规则即可将多维力传感器耦合误差模型公式(4)分解为2³个公式，并统称为公式(5)，公式前面小括号内为编号：

$\left(\mathrm{0,0,0}\right):\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]=\left[\begin{array}{ccc}{k}_{\mathrm{1,1}}+& k_{\mathrm{1,2}}+& k_{\mathrm{1,3}}+\\ k_{\mathrm{2,1}}+& k_{\mathrm{2,2}}+& k_{\mathrm{2,3}}+\\ k_{\mathrm{3,1}}+& k_{\mathrm{3,2}}+& k_{\mathrm{3,3}}+\end{array}\right]\left[\begin{array}{c}{f}_1+\\ f_2+\\ f_3+\end{array}\right]$

$\left(\mathrm{0,0,1}\right):\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]=\left[\begin{array}{ccc}{k}_{\mathrm{1,1}}-& k_{\mathrm{1,2}}+& k_{\mathrm{1,3}}+\\ k_{\mathrm{2,1}}-& k_{\mathrm{2,2}}+& k_{\mathrm{2,3}}+\\ k_{\mathrm{3,1}}-& k_{\mathrm{3,2}}+& k_{\mathrm{3,3}}+\end{array}\right]\left[\begin{array}{c}{f}_1-\\ f_2+\\ f_3+\end{array}\right]$

…           (5)

$\left(\mathrm{1,1,0}\right):\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]=\left[\begin{array}{ccc}{k}_{\mathrm{1,1}}+& k_{\mathrm{1,2}}-& k_{\mathrm{1,3}}-\\ k_{\mathrm{2,1}}+& k_{\mathrm{2,2}}-& k_{\mathrm{2,3}}-\\ k_{\mathrm{3,1}}+& k_{\mathrm{3,2}}-& k_{\mathrm{3,3}}-\end{array}\right]\left[\begin{array}{c}{f}_1+\\ f_2-\\ f_3-\end{array}\right]$

$\left(\mathrm{1,1,1}\right):\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]\left[\begin{array}{ccc}{k}_{\mathrm{1,1}}-& k_{1,2}-& k_{\mathrm{1,3}}-\\ k_{\mathrm{2,1}}-& k_{\mathrm{2,2}}-& k_{\mathrm{2,3}}-\\ k_{\mathrm{3,1}}-& k_{\mathrm{3,2}}-& k_{\mathrm{3,3}}-\end{array}\right]\left[\begin{array}{c}{f}_1+\\ f_2-\\ f_3-\end{array}\right],$

步骤1.3：将公式(5)中k_i，s，且i＝1，2，3、s＝1，2，3组成的系数矩阵分别求逆，得到对应的逆矩阵，并对公式(5)中各个等式的两边分别左乘对应的逆矩阵，并保持其编号不变，得到三维力传感器的解耦公式(6)：

$\left(\mathrm{0,0,0}\right):\left[\begin{array}{c}{f}_{1+}\\ f_{2+}\\ f_{3+}\end{array}\right]={\left[\begin{array}{ccc}{k}_{1,1}+& k_{\mathrm{1,2}}+& k_{\mathrm{1,3}}+\\ k_{\mathrm{2,1}}+& k_{\mathrm{2,2}}+& k_{\mathrm{2,3}}+\\ k_{\mathrm{3,1}}+& k_{\mathrm{3,2}}+& k_{\mathrm{3,3}}+\end{array}\right]}^{-1}\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right],$

$\left(\mathrm{0,0,1}\right):\left[\begin{array}{c}{f}_{1-}\\ f_2\\ f_3\end{array}\right]={\left[\begin{array}{ccc}{k}_{\mathrm{1,1}}-& k_{\mathrm{1,2}}+& k_{\mathrm{1,3}}+\\ k_{\mathrm{2,1}}-& k_{\mathrm{2,2}}+& k_{\mathrm{2,3}}+\\ k_{\mathrm{3,1}}-& k_{\mathrm{3,2}}+& k_{\mathrm{3,3}}+\end{array}\right]}^{-1}\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

…       (6)

$\left(\mathrm{1,1,0}\right):\left[\begin{array}{c}{f}_1\\ f_{2-}\\ f_{3-}\end{array}\right]={\left[\begin{array}{ccc}{k}_{1,1}+& k_{\mathrm{1,2}}-& k_{\mathrm{1,3}}-\\ k_{\mathrm{2,1}}+& k_{\mathrm{2,2}}-& k_{\mathrm{2,3}}-\\ k_{\mathrm{3,1}}+& k_{\mathrm{3,2}}-& k_{\mathrm{3,3}}-\end{array}\right]}^{-1}\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

$\left(\mathrm{1,1,1}\right):\left[\begin{array}{c}{f}_{1-}\\ f_{2-}\\ f_{3-}\end{array}\right]={\left[\begin{array}{ccc}{k}_{1,1}-& k_{\mathrm{1,2}}-& k_{\mathrm{1,3}}-\\ k_{2,1}-& k_{\mathrm{2,2}}-& k_{\mathrm{2,3}}-\\ k_{\mathrm{3,1}}-& k_{\mathrm{3,2}}-& k_{\mathrm{3,3}}-\end{array}\right]}^{-1}\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

步骤2：建立卡尔曼滤波方程：

步骤2.1：在传感器标定过程中的加载力值稳定之后，对每个维度的输出电压分别建立卡尔曼滤波器方程，三维力传感器工作在线性状态，对传感器的每个维度建立线性离散系统的状态空间模型，其任一维度的表达式为：

$\left\{\begin{array}{c}{x}_t=x_{t-1}+w_t\\ u_t=x_t+v_t\end{array}\right.---\left(7\right)$

其中：

x_t为t时刻传感器的状态；

u_t为t时刻传感器的输出电压；

w_t、v_t分别为t时刻的传感器噪声和观测噪声，且相互独立，他们被假设成高斯白噪声，且传感器噪声和观测噪声的协方差分别为Q，R，

步骤2.2：建立了如下卡尔曼滤波器方程：

(1)时间更新方程

其中：

为t时刻传感器状态的先验估计；

为t时刻传感器的先验估计均方误差；

p_t-1为t-1时刻传感器的后验估计均方误差；

(2)测量更新方程

其中：G_t为t时刻卡尔曼滤波器的增益；

步骤3：进行三维力传感器静态标定试验，获取静态标定试验数据：

参照图2，标定实验装置由标定台面(1)，L形支架(2)，三维力传感器(3)，标定轴(4)，钢丝绳(5)，砝码(6)等组成。将标定实验台调整水平，安装好L形支架，将三维力传感器固定在支架上，连接采集电路。给传感器供电至其零点稳定后，对三维力传感器的各个方向进行满量程预加载，消除传感器在加工与贴片过程中的应力。采集传感器每个方向的零点电压，并保存。正式标定的每个加载步为40N，满量程为400N。三维力传感器的每个方向标定次数b＝2，每次包括加载与卸载，每个标定力值点采集经卡尔曼滤波后的j＝10组数据点，因此整个标定过程中的每个加载力值点共有2×i×b＝40个数据点。

步骤4：确定三维力传感器耦合误差模型中的斜率：

步骤4.1：由标定数据绘制每个方向的加载力值与3个维度电压之间的区域图，(加载力为坐标的横轴，输出电压为纵轴)，找出每个加载力值点对应的多个采集电压的最大值与最小值，并对其取平均，得到输出电压的中间值，由两次标定得到的数据绘制三维力传感器加载力与主电压的区域图，如图3、图4、图5，以及加载力与耦合电压的区域图，如图6至图17。图3为加载力f_x与输出电压u_x的区域图。图4为加载力f_y与输出电压u_y的区域图。图5为加载力f_z与输出电压u_z的区域图。图6为加载力f_x+与耦合输出电压u_y的区域图。图7为加载力f_x+与耦合输出电压u_z的区域图。图8为加载力f_x-与耦合输出电压u_y的区域图。图9为加载力f_x-与耦合输出电压u_z的区域图。图10为加载力f_y+与耦合输出电压u_x的区域图。图11为加载力f_y+与耦合输出电压u_z的区域图。图12为加载力f_y-与耦合输出电压u_x的区域图。图13为加载力f_y-与耦合输出电压u_z的区域图。图14为加载力f_z+与耦合输出电压u_x的区域图。图15为加载力f_z+与耦合输出电压u_y的区域图。图16为加载力f_z-与耦合输出电压u_x的区域图。图17为加载力f_z-与耦合输出电压u_y的区域图。

步骤4.2：由图3至图17中的标定数据使用本发明提出的求解超定线性方程组的最小二乘解的方法得到力值-电压的斜率K_i，s，且i＝1，2，3、s＝1，2，3，

步骤4.3：将上一步骤中求解的斜率K_i，s带入公式(6)中，并对其系数矩阵求逆，得到标定传感器的8个解耦公式，分别如下所示：

$\left(000\right):\left[\begin{array}{c}{f}_{1+}\\ f_{2+}\\ f_{3+}\end{array}\right]=\left[\begin{array}{ccc}91.658& 2.0441& -0.8688\\ -0.0288& 99.1014& -3.0896\\ -3.2130& 1.6357& 98.2397\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right],\left(001\right):\left[\begin{array}{c}{f}_{1-}\\ f_{2+}\\ f_{3+}\end{array}\right]=\left[\begin{array}{ccc}91.4222& 2.0388& -0.8665\\ -0.2422& 99.0966& -3.0876\\ 1.4612& 1.7400& 98.1954\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

$\left(010\right):\left[\begin{array}{c}{f}_{1+}\\ f_{2-}\\ f_{3+}\end{array}\right]=\left[\begin{array}{ccc}91.6581& 1.7141& -0.8585\\ -0.0289& 99.4137& -3.0994\\ -3.2123& -0.6685& 98.3115\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right],\left(011\right):\left[\begin{array}{c}{f}_{1-}\\ f_{2-}\\ f_{3+}\end{array}\right]=\left[\begin{array}{ccc}91.4230& 1.7097& -0.8563\\ -0.2429& 99.4097& -3.0973\\ 1.4669& -0.5810& 98.2677\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

$\left(100\right):\left[\begin{array}{c}{f}_{1+}\\ f_{2+}\\ f_{3-}\end{array}\right]=\left[\begin{array}{ccc}91.6539& 2.0461& -0.7463\\ -0.0754& 99.1251& -1.6666\\ -3.0751& 1.5656& 94.0245\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right],\left(101\right):\left[\begin{array}{c}{f}_{1-}\\ f_{2+}\\ f_{3-}\end{array}\right]=\left[\begin{array}{ccc}91.4240& 2.0410& -0.7444\\ -0.2210& 99.1218& -1.6654\\ 1.3986& 1.6654& 93.9881\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

$\left(110\right):\left[\begin{array}{c}{f}_{1+}\\ f_{2-}\\ f_{3-}\end{array}\right]=\left[\begin{array}{ccc}91.6542& 1.7133& -0.7407\\ -0.0756& 99.4039& 1.6713\\ -3.0734& 0.6396& 94.0616\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right],\left(111\right):\left[\begin{array}{c}{f}_{1-}\\ f_{2-}\\ f_{3-}\end{array}\right]=\left[\begin{array}{ccc}91.4248& 1.7090& -0.7388\\ -0.2216& 99.4012& 1.6701\\ 1.4035& 0.5559& 94.0254\end{array}\right]\left[\begin{array}{c}{u}_1\\ u_2\\ u_3\end{array}\right]$

步骤5：标定后的三维力传感器测力解耦过程：

步骤5.1：对三维力传感器3个维度的零点电压进行卡尔曼滤波后，并采集10组零点数据，分别计算出3个维度零点电压的最大值与最小值之间的中间值，并作为传感器的零点，

步骤5.2：多维力传感器加载的力值f₁，f₂，f₃后，对其输出的电压进行编号，其编号规则为：编号由第3维输出电压的编号值、第2维输出电压的编号值及第1维输出电压的编号值顺序排列构成，第s维输出的电压u为经过卡尔曼滤波后，减去初始零点的电压值，当u＞0时，第s维输出电压的编号值取值为0，当u＜0时，第s维输出电压的编号值取值为1，其中s＝1，2，3。因三维空间中的力只有一组方程组与之对应，所以每次解耦只需计算一组方程组就能得到解耦力值的大小及方向，不因方程组数目的增加，而使计算量增加。

Claims (1)

1.一种基于卡尔曼滤波与多次采集的多维力传感器线性解耦方法，其特征在于包括以下几个步骤：

步骤1：采用分析方法建立n维力/力矩传感器耦合误差模型：

步骤1.1：对n维力/力矩传感器，n为正整数，n≤6，共有n维力/力矩输入f₁,f₂,...f_n和对应n维输出电压值u₁,u₂,...u_n，其中，f₁对u_i的影响值在u_i所占的部分为u_i,1，f₂对u_i的影响值在u_i所占的部分为u_i,2，…，f_n对u_i的影响值在u_i所占的部分为u_i,n，其中i＝1,2,3,...n，则

$\left\{\begin{array}{c}{u}_1=u_{\mathrm{1,1}}+u_{\mathrm{1,2}}+...+u_{1,n}\\ u_2=u_{\mathrm{2,1}}+u_{\mathrm{2,2}}+...+u_{2,n}\\ ...\\ u_n=u_{n,1}+u_{n,2}+...+u_{n,n}\end{array}\right.---\left(1\right)$

上式可简写为：

$\left\{\begin{array}{c}{u}_1=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{u}_{1,i}\\ u_2=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{u}_{2,i}\\ ...\\ u_n=\underset{i=1}{\overset{n}{\mathrm{\&Sigma;}}}{u}_{n,i}\end{array}\right.---\left(2\right)$

设第s维输入力/力矩f_s在第i维输出电压为u_i,s，其中s＝1,2,3,...n，i＝1,2,3,...n，u_i,s与f_s之间的斜率为k_i,s,即：

u_i,s＝k_i,s·f_s  (3)

将式(3)带入式(1)得到n维力/力矩传感器耦合误差模型：

$\left[\begin{array}{c}{u}_1\\ u_2\\ .\\ .\\ .\\ u_n\end{array}\right]=\left[\begin{array}{cccc}{k}_{\mathrm{1,1}}& k_{\mathrm{1,2}}& ...& k_{1,n}\\ k_{\mathrm{2,1}}& k_{\mathrm{2,2}}& ...& k_{2,n}\\ .& .& & .\\ .& .& k_{i,s}& .\\ .& .& & .\\ k_{n,1}& k_{n,2}& ...& k_{n,n}\end{array}\right]\left[\begin{array}{c}{f}_1\\ f_2\\ .\\ .\\ .\\ f_n\end{array}\right]---\left(4\right)$

步骤1.2：将多维力/力矩空间按照n维空间坐标轴的正负方向分解成2ⁿ个空间，并按照编号规则对2ⁿ个空间进行编号，其编号规则为：编号由第n维的编号值、第n-1维的编号值、…、第3维的编号值、第2维的编号值及第1维的编号值顺序排列构成，当第s维输入力/力矩f_s＞0时，第s维的编号值取值为0，当第s维输入力/力矩f_s＜0时，第s维的编号值取值为1，s＝n,n-1,...,3,2,1，通过上述规则即可将n维力/力矩传感器耦合误差模型公式(4)分解为2ⁿ个公式，并统称为公式(5)，公式前面小括号内为编号：

步骤1.3：将公式(5)中k_i,s且i＝1,2,3…n-1,n、s＝1,2,3…n-1,n组成的系数矩阵分别求逆，得到对应的逆矩阵，并对公式(5)中各个等式的两边分别左乘对应的逆矩阵，并保持其编号不变，得到n维力/力矩传感器的解耦公式：

步骤2：建立卡尔曼滤波方程：

步骤2.1：在传感器标定过程中的加载力值稳定之后，对每个维度的输出电压分别建立卡尔曼滤波器方程，n维力/力矩传感器工作在线性状态，对传感器的每个维度建立线性离散系统的状态空间模型，其任一维度的表达式为：

$\left\{\begin{array}{c}{x}_t=x_{t-1}+w_t\\ u_t=x_t+v_t\end{array}\right.---\left(7\right)$

其中:

x_t为t时刻传感器的状态；

x_t-1为t-1时刻传感器的状态；

u_t为t时刻传感器的输出电压；

w_t、v_t分别为t时刻的传感器噪声和观测噪声，且相互独立，他们被设成高斯白噪声，且传感器噪声和观测噪声的协方差分别为Q,R，

步骤2.2：建立了如下卡尔曼滤波器方程：

(1)时间更新方程

其中：

为t时刻传感器状态的先验估计；

为t-1时刻传感器状态的后验估计；

为t时刻传感器的先验估计均方误差；

p_t-1为t-1时刻传感器的后验估计均方误差；

Q为传感器噪声的协方差；

(2)测量更新方程

其中：G_t为t时刻卡尔曼滤波器的增益；

为t时刻传感器的先验估计均方误差；

p_t为t时刻传感器的后验估计均方误差；

为t时刻传感器状态的后验估计；

为t时刻传感器状态的先验估计；

u_t为t时刻传感器的输出电压；

R为传感器观测噪声的协方差；

步骤3：进行n维力/力矩传感器静态标定试验，获取静态标定试验数据：

对n维力/力矩传感器进行标定加载试验，将每维的满量程加载力平均分成m个加载力值点，m≥10，将载荷从零值逐步加载至正向满量程，再逐步减少至零，并对每个加载力值点采集n维力/力矩传感器每个维度的输出电压，并用卡尔曼滤波器滤波后得到标定数据，每个加载/卸载力值点保存j组滤波后的数据，共进行了b次标定，每个力值点共有2×j×b组数据，

步骤4：确定n维力/力矩传感器耦合误差模型中的斜率：

步骤4.1：由标定数据绘制每个方向的加载力值与n个维度电压之间的区域图，(并以加载力为坐标的横轴，输出电压为纵轴)，找出每个加载力值点对应的多个采集电压的最大值与最小值，并对其取平均，得到输出电压的中间值，

步骤4.2：对每个方向上加载的力值与输出电压的中间值采用求超定线性方程组的最小二乘解的方法，得到斜率k_i,s，且i＝1,2,3…n-1,n、s＝1,2,3…n-1,n，

步骤4.3：将上一步骤中求解的斜率k_i,s代入公式(6)中，并对其系数矩阵求逆，得到标定传感器的解耦公式，

步骤5：标定后的n维力/力矩传感器测力解耦过程：

步骤5.1：对n维力/力矩传感器n个维度的零点电压进行卡尔曼滤波后，并采集j组零点电压数据，分别计算出n个维度零点电压的最大值与最小值之间的中间值，并作为传感器的零点，

步骤5.2：n维力/力矩传感器加载的力值f₁,f₂,f₃,...,f_n后，对n维力/力矩传感器输出的电压进行编号，其编号规则为：编号由第n维输出电压的编号值、第n-1维输出电压的编号值、…、第3维输出电压的编号值、第2维输出电压的编号值及第1维输出电压的编号值顺序排列构成，第s维输出的电压u为经过卡尔曼滤波后，减去初始零点的电压值，当u＞0时，第s维输出电压的编号值取值为0，当u＜0时，第s维输出电压的编号值取值为1，从步骤3.1中找出编号与电压编号相同的解耦方程组，将测量电压值带入求解，得到解耦后的多维力值的大小及方向。