CN106041926A - 一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法 - Google Patents

Abstract

本发明属于机械臂控制领域，涉及一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。本发明包括：在通用工业机械臂的腕部安装六维力/力矩传感器；根据传感器与机械臂末端工具参数，建立实际接触力的数学模型；根据力传感器的测量值，使用卡尔曼滤波器计算实际接触力；根据机械臂六个关节的位置信息，使用正运动学计算机械臂末端的当前位置等。本发明能在线实时估计环境刚度，并有效抑制力传感器中的干扰，增加了柔顺控制的稳定性，可使通用的工业机械臂具有柔顺能力，完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。

Description

一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法

技术领域

本发明属于机械臂控制领域，涉及一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。

背景技术

在工业机械臂的许多应用中，如轮廓跟踪、研磨、去毛刺和装配任务，机械臂末端需要与刚性环境接触。为了成功执行这些任务，必须精确控制机械臂的位置与接触力，使机械臂在接触时具有一定水平的柔顺能力。更具体的说，在需要高性能的任务中，对机械臂与环境之间产生的接触力进行最优化对于保证稳定性并完成期望操作任务而言，是至关重要的。

最近的研究表明，机械臂的柔顺控制已经成为机器人发展中的中心问题。高性能的力控制结构是直接在机械臂的各个轴上作用电机力矩，但通用的工业机械臂是通过运动控制板卡进行控制的，其只提供位置控制模式。而且，工业机械臂通常是进行离线轨迹规划，一旦机械臂开始执行轨迹后，便不能更改轨迹。所以，通用工业机械臂的灵活性差，很难完成柔顺控制这样的复杂任务。

所以，对通用的商业工业机械臂进行简单快捷地改造，在机械臂的腕部安装力/力矩传感器，设计力/位置混合控制算法，使其具备柔顺控制的能力，完成更多复杂的任务。但是，在物理交互控制中，一个主要的挑战是环境刚度未知或环境中有不可预测的干扰。而且，在工业环境中，力传感器直接采集的信号中通常包含热噪声、环境噪声等其他噪声，导致输出信号偏离真实值，甚至会影响力控制性能。本发明提供了一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，可使通用工业机械臂具有同时控制机械臂末端力与位置的能力，而且本发明可以有效抑制力传感器的噪声干扰，并能在线实时估计环境刚度，增加柔顺控制的稳定性。

发明内容

本发明的目的在于提供一种克服通用工业机械臂灵活性差、抗干扰能力差的缺陷，通过对其简单改造，使工业机械臂具有稳定的柔顺控制能力的基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。

本发明的目的是这样实现的：

一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，包括如下步骤：

(1)在通用工业机械臂的腕部安装六维力/力矩传感器；

(2)根据传感器与机械臂末端工具参数，建立实际接触力的数学模型；

(3)根据力传感器的测量值，使用卡尔曼滤波器计算实际接触力；

(4)根据机械臂六个关节的位置信息，使用正运动学计算机械臂末端的当前位置；

(5)根据真实的接触力信息与机械臂末端的当前位置，使用递推最小二乘法实时在线计算接触环境的刚度；

(6)并行计算位置控制器与力控制器，得到机械臂末端最终的期望位置，使用六自由度机械臂逆运动学计算机械臂各轴的期望位置；

(7)使用工业机械臂运动控制卡的位置跟踪模式，实现通用工业机械臂的力/位置混合控制。

所述的实际接触力的数学模型具体为：

(2.1)建立机械臂末端执行器及力传感器的坐标系；

(2.2)设任务过程中，忽略加速度引起的惯性力与力矩的影响，补偿重力与重力矩的影响，得到实际的环境接触力与力矩的数学模型：

$\left[\begin{array}{cc}{R}_E^C& 0\\ 0& R_E^C\end{array}\right]\left[\begin{array}{c}{F}_E^E\\ M_E^E\end{array}\right]=\left[\begin{array}{cc}{R}_S^C& 0\\ R_S^C\[S\left(r_{CS}^C\right)-S\left(r_{CE}^C\right)\]& R_S^C\end{array}\right]\left[\begin{array}{c}{F}_S^S\\ M_S^S\end{array}\right]+m\left[\begin{array}{c}-R_W^C\\ S\left(r_{CE}^C\right)R_W^C\end{array}\right]g^W$

其中，为环境接触力与力矩，为六维力传感器测量的力与力矩，g^W是相对世界坐标系的重力加速度，m为末端执行器的质量；是坐标系Σ_j相对于坐标系Σ_i的旋转矩阵，其中，设E为末端执行器与环境的接触点，坐标系为Σ_E，力/力矩传感器的质心为S，传感器坐标系位Σ_S，末端执行器的质心为C，坐标系Σ_C，世界坐标系定义为Σ_W；S(b)＝S_b为向量b的斜对称算子，为末端执行器质心C到力传感器质心S的向量在末端执行器坐标系中的表示，为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。

所述的计算实际接触力的卡尔曼滤波器具体为：

(3.1)在系统中加入力的变化速度的影响，取扩展状态向量和测量向量分别为：

$x={\left[\begin{array}{cccccccc}{F}_S^T& M_S^T& F_E^T& M_E^T& {\stackrel{\·}{F}}_S^T& {\stackrel{\·}{M}}_S^T& {\stackrel{\·}{F}}_E^T& {\stackrel{\·}{M}}_E^T\end{array}\right]}^T$

$y={\left[\begin{array}{cccc}{F}_S^T& M_S^T& F_E^T& M_E^T\end{array}\right]}^T$

(3.2)连续线性系统模型与测量模型为：

$\stackrel{\·}{x}\left(t\right)=A_{0}x\left(t\right)+w_x$

y(t)＝H₀x(t)+D₀g+v_y

(3.3)取离散状态变量与离散量测向量为：

$x_k={\left[\begin{array}{cccccccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T& {\mathrm{\ΔF}}_{S,k}^T& {\mathrm{\ΔM}}_{S,k}^T& {\mathrm{\ΔF}}_{E,k}^T& {\mathrm{\ΔM}}_{E,k}^T\end{array}\right]}^T$

$y_k={\left[\begin{array}{cccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T\end{array}\right]}^T$

(3.4)线性系统方程与量测方程的随机差分方程：

x_k＝Ax_k-1+w_k-1，y_k＝Hx_k+Dg+v_k

$A=\left[\begin{array}{cc}{I}_{12\×12}& I_{12\×12}\\ 0_{12\×12}& I_{12\×12}\end{array}\right],D=m\left[\begin{array}{c}-I_{3\×3}\\ S\left(r_{CE}\right)\end{array}\right]$

H＝[I_6×6 C_6×6 0_6×12],

其中，下角标k,k-1表示离散时间步，w_k,v_k分别表示离散系统噪声与离散量测噪声，并假设为高斯白噪声，其协方差矩阵Q_k＝Q(t_k)；

(3.5)根据卡尔曼滤波算法，基于k-1时刻的结果计算k时刻的预测状态和预测协方差得时间更新方程：

${\hat{x}}_{k|k-1}^{-}=A{\hat{x}}_{k-1}$

$P_{k|k-1}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q.$

(3.6)计算卡尔曼增益K_k并更新协方差P_k：

$K_k=P_{k|k-1}^{-}{H}^T{({\mathrm{HP}}_{k|k-1}^{-}H+R)}^{-1}$

$P_k=(I-K_{k}H)P_{k|k-1}^{-}$

(3.7)根据当前k时刻实际的测量值y_k更新估计状态

${\hat{x}}_k={\hat{x}}_{k|k-1}^{-}+K_k\left(y_k-H{\hat{x}}_{k|k-1}^{-}\right)$

(3.8)根据新估计的状态计算系统输出y_k；

$y_k=H{\hat{x}}_k+Dg.$

所述的实时在线计算接触环境刚度的递推最小二乘法具体为：

(5.1)根据力传感器测量的力信息，使用卡尔曼滤波器计算得到实际接触力与力矩F_E；

(5.2)把接触环境建模为刚度为K_E的弹簧，并且环境刚度K_E远小于力传感器的刚度；把末端执行器与环境的接触力为F_E＝0时的接触点E的位置设为接触参考位置X_E0，则接触位置差ΔX_E为ΔX_E＝X_E-X_E0＝F_E/K_E；

(5.3)设机械臂末端执行器沿接触面的法线方向与接触面接触，通过各关节角与机械臂的正运动学方程计算得到末端沿法线方向的位移x，当机械臂末端与环境接触后，根据测得的一组值(x_t,F_E,t)可求得的初值：

${\hat{K}}_{E,t}={\left(x_t^T{x}_t\right)}^{-1}{x}_t^T{F}_{E,t},(t=1,2,...)$

(5.4)使用递推最小二乘法(RLS)迭代计算环境刚度 P₁＝I；C_t为遗忘因子，根据误差实时调整C_t的值；a₁,a₂及a₃可取经验值；取力传感器的多个测量值作为接触力F_E的测量值，当|F_E-F_d|≤0.5N时，认为已稳定，停止RLS计算；同理，可计算其他方向上的

K_t+1＝P_t+1x_t+1，P_t+1＝P_t/(C_t+x_t+1P_tx_t+1)，

$C_t=1-a_1\[\arctan \left(a_2\right(\left|{\hat{e}}_t\right|-a_3\left)\right)/\mathrm{\π}+0.5\].$

所述的并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体为：

(6.1)利用对角矩阵S将整个任务空间分为两个子空间，接触面的法线方向为力控制子空间，接触面切线方向为位置控制子空间；对角阵S定义了位置的控制方向，当其主对角线元素为1时，进行轨迹位置控制；同理，对角阵S`定义了力控制的方向；

(6.2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量；位置控制器根据期望位置x_d计算得期望轨迹对应的笛卡尔空间的末端位置P_d；使用PI控制器作为力控制器，根据期望力F_d、实际接触力F_E与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置P_f；位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总期望位置P：

P＝S·P_d+S`·P_f，

(6.3)根据六自由度机械臂的运动学逆解，把机械臂末端的总期望位置转换为机械臂各关节的期望位置P_j；将作业空间任意方向的力和位置，分配到各个关节，机械臂以独立的形式同时控制力和位置；

(6.4)使用工业机械臂运动控制卡的位置跟踪模式，其特点是中间任意时刻都可随时改变期望位置，然后以期望速度到达新的期望位置，以此实现通用工业机械臂的力/位置混合控制。

本发明的有益效果在于：

本发明能在线实时估计环境刚度，并有效抑制力传感器中的干扰，增加了柔顺控制的稳定性，可使通用的工业机械臂具有柔顺能力，完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。

附图说明

图1是机械臂末端工具与环境的接触示意图；

图2是基于卡尔曼滤波的力/位置混合控制结构图。

具体实施方式

下面结合附图对本发明做进一步描述。

本发明提供的是一类基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，属于机械臂控制领域。通用的工业机械臂灵活性差，很难完成需要同时控制力与位置的复杂任务，而且传感器中有大量噪声，环境刚度未知，严重影响机械臂的控制性能。针对这一现状，本发明提出了一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法。该方法首先根据真实接触力的数学模型，利用卡尔曼滤波器计算机械臂末端的真实接触力；然后根据真实的接触力信息与机械臂末端的当前位置，使用递推最小二乘法实时在线计算接触环境的刚度。最后，并行计算位置控制器与力控制器，得到机械臂末端最终的期望位置，通过逆运动学转换为机械臂各轴的期望位置，使用工业机械臂运动控制卡的位置跟踪模式，实现通用工业机械臂的力/位置混合控制。本发明的有益效果是，能在线实时估计环境刚度，并有效抑制力传感器中的干扰，增加了柔顺控制的稳定性，可使通用的工业机械臂具有柔顺能力，完成如轮廓跟踪、研磨、去毛刺及装配等多种复杂任务。

本发明采用的技术方案是：

基于卡尔曼滤波的工业机械臂力/位置混合控制算法，其中控制方法如下：

(1)在通用工业机械臂的腕部安装六维力/力矩传感器；

(2)根据传感器与机械臂末端工具参数，建立实际接触力的数学模型；

(3)根据力传感器的测量值，使用卡尔曼滤波器计算实际接触力；

(4)根据机械臂六个关节的位置信息，使用正运动学计算机械臂末端的当前位置；

(5)根据真实的接触力信息与机械臂末端的当前位置，使用递推最小二乘法实时在线计算接触环境的刚度；

(6)并行计算位置控制器与力控制器，得到机械臂末端最终的期望位置，使用六自由度机械臂逆运动学计算机械臂各轴的期望位置；

(7)使用工业机械臂运动控制卡的位置跟踪模式，实现通用工业机械臂的力/位置混合控制。

作为一种优化的技术方案，控制方法具体如下：

首先，在通用工业机械臂的腕部安装六维力/力矩传感器，根据真实接触力的数学模型，利用卡尔曼滤波器计算机械臂末端的真实接触力；然后根据真实的接触力信息与机械臂末端的当前位置，使用递推最小二乘法实时在线计算接触环境的刚度。最后，并行计算位置控制器与力控制器，得到机械臂末端最终的期望位置，通过逆运动学转换为机械臂各轴的期望位置，使用工业机械臂运动控制卡的位置跟踪模式，实现通用工业机械臂的力/位置混合控制。

作为一种优化的技术方案，实际接触力的数学模型具体为：

(1)建立机械臂末端执行器及力传感器的坐标系；

(2)设任务过程中，机械臂平稳运行且速度较低，加速度引起的惯性力与力矩的影响可以忽略，只需补偿重力与重力矩的影响，得到实际的环境接触力与力矩的数学模型：

$\left[\begin{array}{cc}{R}_E^C& 0\\ 0& R_E^C\end{array}\right]\left[\begin{array}{c}{F}_E^E\\ M_E^E\end{array}\right]=\left[\begin{array}{cc}{R}_S^C& 0\\ R_S^C\[S\left(r_{CS}^C\right)-S\left(r_{CE}^C\right)\]& R_S^C\end{array}\right]\left[\begin{array}{c}{F}_S^S\\ M_S^S\end{array}\right]+m\left[\begin{array}{c}-R_W^C\\ S\left(r_{CE}^C\right)R_W^C\end{array}\right]g^W$

其中，为环境接触力与力矩，为六维力传感器测量的力与力矩，g^W是相对世界坐标系的重力加速度，m为末端执行器的质量；是坐标系Σ_j相对于坐标系Σ_i的旋转矩阵，其中，设E为末端执行器与环境的接触点，坐标系为Σ_E，力/力矩传感器的质心为S，传感器坐标系位Σ_S，末端执行器的质心为C，坐标系Σ_C，世界坐标系定义为Σ_W；S(b)＝S_b为向量b的斜对称算子，为末端执行器质心C到力传感器质心S的向量在末端执行器坐标系中的表示，为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。

作为一种优化的技术方案，计算实际接触力的卡尔曼滤波器具体为：

(1)在系统中加入力的变化速度的影响，取扩展状态向量和测量向量分别为：

$x={\left[\begin{array}{cccccccc}{F}_S^T& M_S^T& F_E^T& M_E^T& {\stackrel{\·}{F}}_S^T& {\stackrel{\·}{M}}_S^T& {\stackrel{\·}{F}}_E^T& {\stackrel{\·}{M}}_E^T\end{array}\right]}^T$

$y={\left[\begin{array}{cccc}{F}_S^T& M_S^T& F_E^T& M_E^T\end{array}\right]}^T$

(2)连续线性系统模型与测量模型为：

$\stackrel{\·}{x}\left(t\right)=A_{0}x\left(t\right)+w_x$

y(t)＝H₀x(t)+D₀g+v_y

(3)取离散状态变量与离散量测向量为：

$x_k={\left[\begin{array}{cccccccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T& {\mathrm{\ΔF}}_{S,k}^T& {\mathrm{\ΔM}}_{S,k}^T& {\mathrm{\ΔF}}_{E,k}^T& {\mathrm{\ΔM}}_{E,k}^T\end{array}\right]}^T$

$y_k={\left[\begin{array}{cccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T\end{array}\right]}^T$

(4)线性系统方程与量测方程的随机差分方程：

x_k＝Ax_k-1+w_k-1，y_k＝Hx_k+Dg+v_k

$A=\left[\begin{array}{cc}{I}_{12\×12}& I_{12\×12}\\ 0_{12\×12}& I_{12\×12}\end{array}\right],D=m\left[\begin{array}{c}-I_{3\×3}\\ S\left(r_{CE}\right)\end{array}\right]$

$H=\left[\begin{array}{ccc}{I}_{6\×6}& C_{6\×6}& 0_{6\×12}\end{array}\right],C=\left[\begin{array}{cc}{I}_{3\×3}& 0_{3\×3}\\ S\left(r_{CS}\right)-S\left(r_{CE}\right)& I_{3\×3}\end{array}\right]$

其中，下角标k,k-1表示离散时间步，w_k,v_k分别表示离散系统噪声与离散量测噪声，并假设为高斯白噪声，其协方差矩阵Q_k＝Q(t_k)。

(5)根据卡尔曼滤波算法，基于k-1时刻的结果计算k时刻的预测状态和预测协方差得时间更新方程：

${\hat{x}}_{k|k-1}^{-}=A{\hat{x}}_{k-1}$

$P_{k|k-1}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q.$

(6)计算卡尔曼增益K_k并更新协方差P_k：

$K_k=P_{k|k-1}^{-}{H}^T{({\mathrm{HP}}_{k|k-1}^{-}H+R)}^{-1}$

$P_k=(I-K_{k}H)P_{k|k-1}^{-}$

(7)根据当前k时刻实际的测量值y_k更新估计状态

${\hat{x}}_k={\hat{x}}_{k|k-1}^{-}+K_k\left(y_k-H{\hat{x}}_{k|k-1}^{-}\right)$

(8)根据新估计的状态计算系统输出y_k。

$y_k=H{\hat{x}}_k+Dg$

作为一种优化的技术方案，实时在线计算接触环境刚度的递推最小二乘法具体为：

(1)根据力传感器测量的力信息，使用卡尔曼滤波器计算得到实际接触力与力矩F_E。

(2)把接触环境建模为刚度为K_E的弹簧，并且环境刚度K_E远小于力传感器的刚度。把末端执行器与环境的接触力为F_E＝0时的接触点E的位置设为接触参考位置X_E0，则接触位置差ΔX_E为ΔX_E＝X_E-X_E0＝F_E/K_E。

(3)设机械臂末端执行器沿接触面的法线方向与接触面接触，通过各关节角与机械臂的正运动学方程计算得到末端沿法线方向的位移x，当机械臂末端与环境接触后，根据测得的一组值(x_t,F_E,_t)可求得的初值：

${\hat{K}}_{E,t}={\left(x_t^T{x}_t\right)}^{-1}{x}_t^T{F}_{E,t},(t=1,2,...)$

(4)使用递推最小二乘法(RLS)迭代计算环境刚度 P₁＝I；C_t为遗忘因子，根据误差实时调整C_t的值。a₁,a₂及a₃可取经验值。取力传感器的多个测量值作为接触力F_E的测量值，当|F_E-F_d|≤0.5N时，认为已稳定，停止RLS计算。同理，可计算其他方向上的

K_t+1＝P_t+1x_t+1，P_t+1＝P_t/(C_t+x_t+1P_tx_t+1)，

$C_t=1-a_1\[\arctan \left(a_2\right(\left|{\hat{e}}_t\right|-a_3\left)\right)/\mathrm{\π}+0.5\].$

作为一种优化的技术方案，并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体为：

(1)利用对角矩阵S将整个任务空间分为两个子空间，接触面的法线方向为力控制子空间，接触面切线方向为位置控制子空间。对角阵S定义了位置的控制方向，当其主对角线元素为1时，进行轨迹位置控制。同理，对角阵S`定义了力控制的方向。

(2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量。位置控制器根据期望位置x_d计算得期望轨迹对应的笛卡尔空间的末端位置P_d；使用PI控制器作为力控制器，根据期望力F_d、实际接触力F_E与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置P_f。位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总期望位置P：

P＝S·P_d+S`·P_f，

(3)根据六自由度机械臂的运动学逆解，把机械臂末端的总期望位置转换为机械臂各关节的期望位置P_j。将作业空间任意方向的力和位置，分配到各个关节，机械臂以独立的形式同时控制力和位置。

(4)使用工业机械臂运动控制卡的位置跟踪模式，其特点是中间任意时刻都可随时改变期望位置，然后以期望速度到达新的期望位置，以此实现通用工业机械臂的力/位置混合控制。

(1)实际接触力的数学模型

在通用工业机械臂的腕部安装力/力矩传感器，传感器测量的力F_S与力矩M_S包括：F_E,M_E为环境接触力与力矩，F_I,M_I为惯性力与力矩，F_G,M_G为重力与重力矩。

$\left[\begin{array}{c}{F}_S\\ M_S\end{array}\right]=\left[\begin{array}{c}{F}_E\\ M_E\end{array}\right]+\left[\begin{array}{c}{F}_I\\ M_I\end{array}\right]+\left[\begin{array}{c}{F}_G\\ M_G\end{array}\right]---\left(1\right)$

如图1所示，设C为末端执行器的质心，坐标系Σ_C的原点位于C，同样，设E为末端执行器与环境的接触点，坐标系为Σ_E。力/力矩传感器的质心为S，传感器坐标系位Σ_S，世界坐标系定义为Σ_W。相对于末端执行器坐标系Σ_C的牛顿-欧拉方程为：

$F_S^C-F_E^C-{\mathrm{mg}}^C={\mathrm{ma}}_C^C---\left(2\right)$

$S\left(r_{CS}^C\right)F_S^C+M_S^C-S\left(r_{CE}^C\right)F_E^C-M_E^C=J_C^C{\mathrm{\α}}^C+S\left({\mathrm{\ω}}^C\right)J_C^C{\mathrm{\ω}}^C---\left(3\right)$

其中，末端执行器的质量为m，g^C为重力加速度向量，为工具的平移加速度，为相对于C的转动惯量，ω^C/α^C分别为末端执行器的角速度与角加速度。S(b)＝S_b为向量b的斜对称算子。

设机械臂在任务过程中平稳运行且速度较低，加速度的影响可以忽略。所以，可以忽略末端执行器的惯性力与力矩，只需补偿重力与重力矩的影响即可。所以，环境接触力与力矩可写为：

$F_E^C=F_S^C-{\mathrm{mg}}^C---\left(4\right)$

$M_E^C=S\left(r_{CS}^C\right)F_S^C+M_S^C-S\left(r_{CE}^C\right)F_E^C---\left(5\right)$

需要注意的是，式(5)中的量大部分都是在自身的坐标系内测量的，设矩阵是坐标系Σ_j相对于Σ_i的旋转矩阵。所以实际应用时需要对公式进行变换，写成矩阵形式为：

$\left[\begin{array}{cc}{R}_E^C& 0\\ 0& R_E^C\end{array}\right]\left[\begin{array}{c}{F}_E^E\\ M_E^E\end{array}\right]=\left[\begin{array}{cc}{R}_S^C& 0\\ R_S^C\[S\left(r_{CS}^C\right)-S\left(r_{CE}^C\right)\]& R_S^C\end{array}\right]\left[\begin{array}{c}{F}_S^S\\ M_S^S\end{array}\right]+m\left[\begin{array}{c}-R_W^C\\ S\left(r_{CE}^C\right)R_W^C\end{array}\right]g^W---\left(6\right)$

(2)计算实际接触力的卡尔曼滤波器

设所有的变量都是相对于坐标系Σ_C表示的，为了方便表示，省略了上角标C。因为力是实时变化的，所以需要考虑力的变化速度对系统的影响，取状态向量和测量向量分别为：

$x={\left[\begin{array}{cccccccc}{F}_S^T& M_S^T& F_E^T& M_E^T& {\stackrel{\·}{F}}_S^T& {\stackrel{\·}{M}}_S^T& {\stackrel{\·}{F}}_E^T& {\stackrel{\·}{M}}_E^T\end{array}\right]}^T---\left(7\right)$

$y={\left[\begin{array}{cccc}{F}_S^T& M_S^T& F_E^T& M_E^T\end{array}\right]}^T---\left(8\right)$

连续线性系统模型与测量模型为：

$\stackrel{\·}{x}\left(t\right)=A_{0}x\left(t\right)+w_x---\left(9\right)$

y(t)＝H₀x(t)+D₀g+v_y (10)

$A_0=\left[\begin{array}{cc}{0}_{12\×12}& I_{12\×12}\\ 0_{12\×12}& 0_{12\×12}\end{array}\right],D_0=m\left[\begin{array}{c}-I_{3\×3}\\ S\left(r_{CE}\right)\end{array}\right],$

H₀＝[I_6×6 C_6×6 0_6×12],

其中，w_x,v_y为高斯白噪声，其协方差分别为Q,R。

对连续线性系统进行离散化，得离散状态变量与离散量测向量

$x_k={\left[\begin{array}{cccccccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T& {\mathrm{\ΔF}}_{S,k}^T& {\mathrm{\ΔM}}_{S,k}^T& {\mathrm{\ΔF}}_{E,k}^T& {\mathrm{\ΔM}}_{E,k}^T\end{array}\right]}^T---\left(11\right)$

$y_k={\left[\begin{array}{cccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T\end{array}\right]}^T---\left(12\right)$

得线性系统方程与量测方程的随机差分方程：

x_k＝Ax_k-1+w_k-1 (13

y_k＝Hx_k+Dg+v_k (14)

$A=\left[\begin{array}{cc}{I}_{12\×12}& I_{12\×12}\\ 0_{12\×12}& I_{12\×12}\end{array}\right],H=H_0,D=D_0---\left(15\right)$

其中，下角标k,k-1表示离散时间步，w_k,v_k分别表示离散系统噪声与离散量测噪声，其协方差矩阵Q_k＝Q(t_k)。

根据卡尔曼滤波算法，基于k-1时刻的结果计算k时刻的预测状态x_k和预测协方差P_k，得时间更新方程：

$\begin{array}{c}{\hat{x}}_{k|k-1}^{-}=A{\hat{x}}_{k-1}\\ P_{k|k-1}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q.\end{array}---\left(16\right)$

然后，基于k时刻实际的测量值使用测量更新方程修正这些预测值，其中，第一个方程计算卡尔曼增益K_k，第二个方程根据当前的测量值y_k更新估计状态x_k，第三个方程更新协方差P_k：

$\begin{array}{c}{K}_k=P_{k|k-1}^{-}{H}^T{({\mathrm{HP}}_{k|k-1}^{-}H+R)}^{-1}\\ {\hat{x}}_k={\hat{x}}_{k|k-1}^{-}+K_k\left(y_k-H{\hat{x}}_{k|k-1}^{-}\right)\\ P_k=(I-K_{k}H)P_{k|k-1}^{-}\end{array}---\left(17\right)$

最后，根据新估计的状态计算系统输出y_k。

(3)实时在线计算接触环境刚度的递推最小二乘法

假设机械臂末端执行器接触的环境为刚性的，则可把接触环境建模为刚度为K_E的弹簧。其中环境刚度K_E远小于力传感器的刚度，即由接触力导致的传感器形变与导致的环境形变相比，可以忽略不计。设末端执行器与环境的接触力为F_E＝0时的接触点E(图1)的位置为接触参考位置X_E0。接触示意图如图1所示，则相对于X_E0的接触位置差ΔX_E定义为：

ΔX_E＝X_E-X_E0＝F_E/K_E (18)

设机械臂末端执行器沿接触面的法线方向与接触面接触，末端沿法线方向的位移x可通过测量的各关节角与机器人的运动学方程计算得到；力传感器测量的沿接触面法线方向的实际接触力F_E由卡尔曼滤波器得到。当机械臂末端与环境接触后，根据测得的一组值(x_t,F_E,t)可求得的初值：

${\hat{K}}_{E,t}={\left(x_t^T{x}_t\right)}^{-1}{x}_t^T{F}_{E,t},(t=1,2,...)---\left(19\right)$

然后由RLS得的迭代计算式：其中，P₁＝I；C_t为遗忘因子。根据误差实时调整C_t的值，当C_t较小时，收敛快但对干扰比较敏感，反之，收敛慢但有较强的抗干扰能力。a₁,a₂及a₃可取经验值。取F_E的多个测量值的均值作为实际接触力，当|F_E-F_d|≤0.5N时，认为已稳定，停止RLS计算。同理，可计算其他方向上的

${\hat{K}}_{E,t+1}={\hat{K}}_{E,t}+K_{t+1}\left(F_{E,t+1}-{\hat{K}}_{E,t}{x}_{t+1}\right)---\left(20\right)$

K_t+1＝P_t+1x_t+1 (21)

P_t+1＝P_t/(C_t+x_t+1P_tx_t+1) (22)

$C_t=1-a_1\[\arctan \left(a_2\right(\left|{\hat{e}}_t\right|-a_3\left)\right)/\mathrm{\π}+0.5\]---\left(23\right)$

(5)力/位置混合控制算法

如图2所示，工业机械臂的位置控制器与力控制器并行计算运行，x_d和F_d分别为期望位置向量与期望力向量；

利用对角矩阵S将整个任务空间分为两个子空间，接触面的法线方向为力控制子空间，接触面切线方向为位置控制子空间。对角阵S定义了位置的控制方向，当主对角线元素为1时，进行轨迹位置控制。同理，对角阵S`定义了力控制的方向。例如，在z轴方向控制接触力，在xy平面上控制轨迹位置，其选择矩阵为：

S＝diag(1,1,0,1,1,1),S`＝diag(0,0,1,0,0,0). (24)

位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量。位置控制器根据期望位置x_d与机械臂末端实际位置x计算得期望轨迹对应的笛卡尔空间的末端位置P_d；力控制器，如采用PI控制器，根据期望力F_d、实际接触力F_E与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置P_f，得机械臂末端在笛卡尔空间的总期望位置P。

P＝S·P_d+S`·P_f (32)

$P_f=\[K_{p}e\left(t\right)+K_i{\∫}_0^{t}e\left(t\right)dt\]K_E^{-1}---\left(33\right)$

通过六自由度机械臂的运动学逆解，把总期望位置转换为机械臂各关节的期望位置P_j。将作业空间任意方向的力和位置，分配到各个关节上，机器人以独立的形式同时控制力和位置。

(6)通用工业机械臂的力/位置混合控制的实现

由于使用的工业机械臂只提供位置控制模式，力控制可以理解为对由力引起的位移的控制，所以最终都是以关节位置的形式实现力/位置混合控制。

以实验工业机械臂为例，运动控制卡的位置跟踪模式为PA模式，指定期望位置后，各关节即以期望速度到达期望位置，中间任意时刻都可随时改变期望位置，然后运动控制卡重新规划关节运动轨迹，以期望速度到达新的期望位置。这是实现实时调整当前位置与力的基础。

Claims (5)

1.一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，其特征在于，包括如下步骤：

(1)在通用工业机械臂的腕部安装六维力/力矩传感器；

(2)根据传感器与机械臂末端工具参数，建立实际接触力的数学模型；

(3)根据力传感器的测量值，使用卡尔曼滤波器计算实际接触力；

(4)根据机械臂六个关节的位置信息，使用正运动学计算机械臂末端的当前位置；

(5)根据真实的接触力信息与机械臂末端的当前位置，使用递推最小二乘法实时在线计算接触环境的刚度；

(6)并行计算位置控制器与力控制器，得到机械臂末端最终的期望位置，使用六自由度机械臂逆运动学计算机械臂各轴的期望位置；

(7)使用工业机械臂运动控制卡的位置跟踪模式，实现通用工业机械臂的力/位置混合控制。

2.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，其特征在于：

所述的实际接触力的数学模型具体为：

(2.1)建立机械臂末端执行器及力传感器的坐标系；

(2.2)设任务过程中，忽略加速度引起的惯性力与力矩的影响，补偿重力与重力矩的影响，得到实际的环境接触力与力矩的数学模型：

$\left[\begin{array}{cc}{R}_E^C& 0\\ 0& R_E^C\end{array}\right]\left[\begin{array}{c}{F}_E^E\\ M_E^E\end{array}\right]=\left[\begin{array}{cc}{R}_S^C& 0\\ R_S^C\[S\left(r_{CS}^C\right)-S\left(r_{CE}^C\right)\]& R_S^C\end{array}\right]\left[\begin{array}{c}{F}_S^S\\ M_S^S\end{array}\right]+m\left[\begin{array}{c}-R_W^C\\ S\left(r_{CE}^C\right)R_W^C\end{array}\right]g^W$

其中，为环境接触力与力矩，为六维力传感器测量的力与力矩，g^W是相对世界坐标系的重力加速度，m为末端执行器的质量；是坐标系Σ_j相对于坐标系Σ_i的旋转矩阵，其中，设E为末端执行器与环境的接触点，坐标系为Σ_E，力/力矩传感器的质心为S，传感器坐标系位Σ_S，末端执行器的质心为C，坐标系Σ_C，世界坐标系定义为Σ_W；S(b)＝S_b为向量b的斜对称算子，为末端执行器质心C到力传感器质心S的向量在末端执行器坐标系中的表示，为末端执行器质心C到环境接触点E的向量在末端执行器坐标系中的表示。

3.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，其特征在于：

所述的计算实际接触力的卡尔曼滤波器具体为：

(3.1)在系统中加入力的变化速度的影响，取扩展状态向量和测量向量分别为：

$x={\left[\begin{array}{cccccccc}{F}_S^T& M_S^T& F_E^T& M_E^T& {\stackrel{\·}{F}}_S^T& {\stackrel{\·}{M}}_S^T& {\stackrel{\·}{F}}_E^T& {\stackrel{\·}{M}}_E^T\end{array}\right]}^T$

$y={\left[\begin{array}{cccc}{F}_S^T& M_S^T& F_E^T& M_E^T\end{array}\right]}^T$

(3.2)连续线性系统模型与测量模型为：

$\stackrel{\·}{x}\left(t\right)=A_{0}x\left(t\right)+w_x$

y(t)＝H₀x(t)+D₀g+v_y

(3.3)取离散状态变量与离散量测向量为：

$x_k={\left[\begin{array}{cccccccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T& {\mathrm{\ΔF}}_{S,k}^T& {\mathrm{\ΔM}}_{S,k}^T& {\mathrm{\ΔF}}_{E,k}^T& {\mathrm{\ΔM}}_{E,k}^T\end{array}\right]}^T$

$y_k={\left[\begin{array}{cccc}{F}_{S,k}^T& M_{S,k}^T& F_{E,k}^T& M_{E,k}^T\end{array}\right]}^T$

(3.4)线性系统方程与量测方程的随机差分方程：

x_k＝Ax_k-1+w_k-1，y_k＝Hx_k+Dg+v_k

$A=\left[\begin{array}{cc}{I}_{12\×12}& I_{12\×12}\\ 0_{12\×12}& I_{12\×12}\end{array}\right],D=m\left[\begin{array}{c}-I_{3\×3}\\ S\left(r_{CE}\right)\end{array}\right]$

H＝[I_6×6 C_6×6 0_6×12],

其中，下角标k,k-1表示离散时间步，w_k,v_k分别表示离散系统噪声与离散量测噪声，并假设为高斯白噪声，其协方差矩阵Q_k＝Q(t_k)；

(3.5)根据卡尔曼滤波算法，基于k-1时刻的结果计算k时刻的预测状态和预测协方差得时间更新方程：

${\hat{x}}_{k|k-1}^{-}=A{\hat{x}}_{k-1}$

$P_{k|k-1}^{-}={\mathrm{AP}}_{k-1}{A}^T+Q.$

(3.6)计算卡尔曼增益K_k并更新协方差P_k：

$K_k=P_{k|k-1}^{-}{H}^T{({\mathrm{HP}}_{k|k-1}^{-}H+R)}^{-1}$

$P_k=(I-K_{k}H)P_{k|k-1}^{-}$

(3.7)根据当前k时刻实际的测量值y_k更新估计状态

${\hat{x}}_k={\hat{x}}_{k|k-1}^{-}+K_k\left(y_k-H{\hat{x}}_{k|k-1}^{-}\right)$

(3.8)根据新估计的状态计算系统输出y_k；

$y_k=H{\hat{x}}_k+Dg.$

4.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，其特征在于：

所述的实时在线计算接触环境刚度的递推最小二乘法具体为：

(5.1)根据力传感器测量的力信息，使用卡尔曼滤波器计算得到实际接触力与力矩F_E；

(5.2)把接触环境建模为刚度为K_E的弹簧，并且环境刚度K_E远小于力传感器的刚度；把末端执行器与环境的接触力为F_E＝0时的接触点E的位置设为接触参考位置X_E0，则接触位置差ΔX_E为ΔX_E＝X_E-X_E0＝F_E/K_E；

(5.3)设机械臂末端执行器沿接触面的法线方向与接触面接触，通过各关节角与机械臂的正运动学方程计算得到末端沿法线方向的位移x，当机械臂末端与环境接触后，根据测得的一组值(x_t,F_E,t)可求得的初值：

${\hat{K}}_{E,t}={\left(x_t^T{x}_t\right)}^{-1}{x}_t^T{F}_{E,t},(t=1,2,...)$

(5.4)使用递推最小二乘法(RLS)迭代计算环境刚度 P₁＝I；C_t为遗忘因子，根据误差实时调整C_t的值；a₁,a₂及a₃可取经验值；取力传感器的多个测量值作为接触力F_E的测量值，当|F_E-F_d|≤0.5N时，认为已稳定，停止RLS计算；同理，可计算其他方向上的

K_t+1＝P_t+1x_t+1，P_t+1＝P_t/(C_t+x_t+1P_tx_t+1)，

$C_t=1-a_1\[\arctan \left(a_2\right(\left|{\hat{e}}_t\right|-a_3\left)\right)/\mathrm{\π}+0.5\].$

5.根据权利要求1中所述的一种基于卡尔曼滤波器的工业机械臂力/位置混合控制方法，其特征在于：

所述的并行计算位置控制器与力控制器实现通用工业机械臂的力/位置混合控制具体为：

(6.1)利用对角矩阵S将整个任务空间分为两个子空间，接触面的法线方向为力控制子空间，接触面切线方向为位置控制子空间；对角阵S定义了位置的控制方向，当其主对角线元素为1时，进行轨迹位置控制；同理，对角阵S`定义了力控制的方向；

(6.2)位置控制器与力控制器的输出最终要转换为机械臂关节空间的位置控制量；位置控制器根据期望位置x_d计算得期望轨迹对应的笛卡尔空间的末端位置P_d；使用PI控制器作为力控制器，根据期望力F_d、实际接触力F_E与当前的环境刚度计算期望力对应的笛卡尔空间的末端位置P_f；位置控制器与力控制器并行计算得到机械臂末端在笛卡尔空间的总期望位置P：

$P=S\·P_d+S`\·P_f,P_f=\[K_{p}e\left(t\right)+K_i{\∫}_0^{t}e\left(t\right)dt\]K_E^{-1}$

(6.3)根据六自由度机械臂的运动学逆解，把机械臂末端的总期望位置转换为机械臂各关节的期望位置P_j；将作业空间任意方向的力和位置，分配到各个关节，机械臂以独立的形式同时控制力和位置；

(6.4)使用工业机械臂运动控制卡的位置跟踪模式，其特点是中间任意时刻都可随时改变期望位置，然后以期望速度到达新的期望位置，以此实现通用工业机械臂的力/位置混合控制。