机器人关节力矩是设计控制算法所需要的信息之一.由于安装传感器的成本较高, 通常选用测量电机电流后通过公式换算的方法获得关节力矩值.然而作为模拟量的电流易受测量噪声等因素影响变得不稳定, 进而使换算得到的关节力矩值波动较大, 不利于机器人的实时控制.

通过滤波减少电流噪声是提高力矩估计精度的直接方法.常用电流滤波方法可分为硬件滤波^[1-3]和软件滤波^[4-7]2大类.目前这2类方法的文献大多是对电机及其附属电路元件进行建模研究, 从电机本身的特性出发研究滤波方法, 并未综合考虑机器人的动力学特性.

除了电流滤波方法外, 也有研究者从其他方面入手, 提高关节力矩估计的精度.杨松等^[8]针对系统中摩擦力矩和电机波动力矩等扰动力矩补偿问题, 提出了综合PD前馈, 重复控制器和扰动观测器的控制策略.赵辉等^[9]提出了在电机系统控制指令中叠加补偿指令的波动力矩抑制方法, 构造了相应的多层前馈神经网络模型, 采用SPDS算法进行网络训练, 实现了波动力矩补偿指令的计算.Van Damme等^[10]提出了一种用滤波后的动力学模型对关节力矩进行实时估算的方法, 避免了对角加速度的计算, 减少了因二次差分计算角加速度所造成的噪声对关节力矩的影响.

为了提高关节力矩估计的准确性, 本文在建立平面关节型机器人(SCARA机器人)动力学模型, 获得关节力矩表达式的基础上, 借助卡尔曼滤波方法对由未经滤波的电机电流换算得到的带有噪声的关节力矩值进行滤波.尝试从机器人本身特性出发寻找关节力矩估计方法, 进而为后续的控制算法提供更准确的关节力矩.

1 SCARA机器人动力学建模

如图 1所示为SCARA机器人的DH模型示意图.由于第4轴旋转关节在运动时会对第3轴移动关节造成一定量的同方向耦合.同时, 考虑到第4轴的运动对SCARA机器人主要的水平面定位运动不产生影响.为合理简化动力学方程, 在建模时将第3关节和第4关节合并, 作为组合连杆3, 关节类型为移动关节, 并在实验时保持第4关节锁定.

动力学分析的研究方法有很多, 如牛顿欧拉方法、拉格朗日方法、旋量方法等^[11-12].本文用牛顿欧拉方法^[13]对SCARA机器人进行动力学建模.具体算法归纳如下.

根据关节类型, 按式(1)或(2)计算连杆坐标系{i+1}相对于自身坐标系的角速度ⁱ⁺¹ω_i+1, 角加速度ⁱ⁺¹${\dot {\mathit{\boldsymbol{\omega}} }}$_i+1, 线加速度ⁱ⁺¹${\dot {\mathit{\boldsymbol{v}} }}$_i+1, 以及连杆i+1质心处的加速度ⁱ⁺¹${\dot {\mathit{\boldsymbol{v}} }}$_Ci+1.

对于旋转关节有

$ \left. \begin{array}{l} {}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} = {}_i^{i + 1}{\mathit{\boldsymbol{R}}^i}{\mathit{\boldsymbol{\omega }}_i} + {{\mathit{\boldsymbol{\dot \theta }}}_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\hat Z}}}_{i + 1}},\\ {}^{i + 1}{{\mathit{\boldsymbol{\dot \omega }}}_{i + 1}} = {}_i^{i + 1}{\mathit{\boldsymbol{R}}^i}{{\mathit{\boldsymbol{\dot \omega }}}_i} + {}_i^{i + 1}{\mathit{\boldsymbol{R}}^i}{\mathit{\boldsymbol{\omega }}_i} \times {{\mathit{\boldsymbol{\dot \theta }}}_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\hat Z}}}_{i + 1}} + {{\mathit{\boldsymbol{\ddot \theta }}}_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\hat Z}}}_{i + 1}},\\ {}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{i + 1}} = {}_i^{i + 1}\mathit{\boldsymbol{R}}\left[ {{}^i{{\mathit{\boldsymbol{\dot \omega }}}_i} \times {}^i{\mathit{\boldsymbol{P}}_{i + 1}} + {}^i{\mathit{\boldsymbol{\omega }}_i} \times \left( {{}^i{\mathit{\boldsymbol{\omega }}_i} \times {}^i{\mathit{\boldsymbol{P}}_{i + 1}}} \right) + {}^i{{\mathit{\boldsymbol{\dot v}}}_i}} \right],\\ {}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{{C_{i + 1}}}} = {}^{i + 1}{{\mathit{\boldsymbol{\dot \omega }}}_{i + 1}} \times {}^{i + 1}{\mathit{\boldsymbol{P}}_{{C_{i + 1}}}} + {}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} \times \left( {{}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} \times } \right.\\ \;\;\;\;\;\;\;\;\;\;\;\;\left. {{}^{i + 1}{\mathit{\boldsymbol{P}}_{{C_{i + 1}}}}} \right) + {}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{i + 1}}. \end{array} \right\} $

(1)

对于移动关节有

$ \left. \begin{array}{l} {}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} = {}_i^{i + 1}{\mathit{\boldsymbol{R}}^i}{\mathit{\boldsymbol{\omega }}_i},\\ {}^{i + 1}{{\mathit{\boldsymbol{\dot \omega }}}_{i + 1}} = {}_i^{i + 1}{\mathit{\boldsymbol{R}}^i}{{\mathit{\boldsymbol{\dot \omega }}}_i},\\ {}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{i + 1}} = {}_i^{i + 1}\mathit{\boldsymbol{R}}\left[ {{}^i{{\mathit{\boldsymbol{\dot \omega }}}_i} \times {}^i{\mathit{\boldsymbol{P}}_{i + 1}} + {}^i{\mathit{\boldsymbol{\omega }}_i} \times \left( {{}^i{\mathit{\boldsymbol{\omega }}_i} \times {}^i{\mathit{\boldsymbol{P}}_{i + 1}}} \right) + {}^i{{\mathit{\boldsymbol{\dot v}}}_i}} \right] + \\ \;\;\;\;\;\;\;\;\;\;\;2{}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} \times {{\mathit{\boldsymbol{\dot d}}}_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\hat Z}}}_{i + 1}} + {{\mathit{\boldsymbol{\ddot d}}}_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\hat Z}}}_{i + 1}},\\ {}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{{C_{i + 1}}}} = {}^{i + 1}{{\mathit{\boldsymbol{\dot \omega }}}_{i + 1}} \times {}^{i + 1}{\mathit{\boldsymbol{P}}_{{C_{i + 1}}}} + {}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} \times \left( {{}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} \times } \right.\\ \;\;\;\;\;\;\;\;\;\;\;\;\left. {{}^{i + 1}{\mathit{\boldsymbol{P}}_{{C_{i + 1}}}}} \right) + {}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{i + 1}}. \end{array} \right\} $

(2)

式中:_iⁱ⁺¹R为连杆坐标系之间的旋转变换矩阵, ⁱP_i+1为连杆坐标系{i+1}原点在连杆坐标系{i}下的位置矢量.${\dot {\mathit{\boldsymbol{\theta}} }}$_i+1ⁱ⁺¹${{\mathit{\boldsymbol{{\hat Z}}} }}$_i+1表示旋转关节i+1角速度矢量, ${\ddot {\mathit{\boldsymbol{\theta}} }}$_i+1ⁱ⁺¹${{\mathit{\boldsymbol{{\hat Z}}} }}$_i+1表示旋转关节i+1角加速度矢量, ⁱ⁺¹P_Ci+1表示连杆i+1质心相对于自身坐标系的位置矢量, ${\dot {\mathit{\boldsymbol{d}} }}$_i+1ⁱ⁺¹${{\mathit{\boldsymbol{{\hat Z}}} }}$_i+1表示移动关节i+1的速度矢量, ${\ddot {\mathit{\boldsymbol{d}} }}$_i+1ⁱ⁺¹${{\mathit{\boldsymbol{{\hat Z}}} }}$_i+1表示移动关节i+1的加速度矢量, 以上矢量均是相对于连杆坐标系{i+1}的值.

按式(3)计算作用在连杆i+1质心上的惯性力ⁱ⁺¹F_i+1和力矩ⁱ⁺¹M_i+1分别为

$ \left. \begin{array}{l} {}^{i + 1}{\mathit{\boldsymbol{F}}_{i + 1}} = {m_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\dot v}}}_{{C_{i + 1}}}},\\ {}^{i + 1}{\mathit{\boldsymbol{M}}_{i + 1}} = {}^{{C_{i + 1}}}{\mathit{\boldsymbol{I}}_{i + 1}}{}^{i + 1}{{\mathit{\boldsymbol{\dot \omega }}}_{i + 1}} + {}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}} \times {}^{{C_{i + 1}}}{\mathit{\boldsymbol{I}}_{i + 1}}{}^{i + 1}{\mathit{\boldsymbol{\omega }}_{i + 1}}. \end{array} \right\} $

(3)

式中:m_i+1为连杆i+1的质量, ^C_i+1I_i+1为连杆i+1在质心处的惯性张量.

式(1)~(3)是牛顿欧拉法的第一部分, 其作用是从连杆1到连杆n向外迭代计算连杆的速度和加速度.

按式(4)计算连杆j-1作用在连杆j上的力^jf_j和力矩^jM′_j.

$ \begin{array}{l} {}^j{\mathit{\boldsymbol{f}}_j} = {}_{j + 1}^j{\mathit{\boldsymbol{R}}^{j + 1}}{\mathit{\boldsymbol{f}}_{j + 1}} + {}^j{\mathit{\boldsymbol{F}}_j}\\ {}^j{{\mathit{\boldsymbol{M'}}}_j} = {}^j{\mathit{\boldsymbol{M}}_j} + {}_{j + 1}^j{\mathit{\boldsymbol{R}}^{j + 1}}{\mathit{\boldsymbol{n}}_{j + 1}} + {}^j{\mathit{\boldsymbol{P}}_{{C_j}}} \times {}^j{\mathit{\boldsymbol{F}}_j} + \\ \;\;\;\;\;\;\;{}^j{\mathit{\boldsymbol{P}}_{j + 1}} \times {}_{j + 1}^j{\mathit{\boldsymbol{R}}^{j + 1}}{\mathit{\boldsymbol{f}}_{j + 1}} \end{array} $

(4)

式中：

根据关节类型, 按式(5)或式(6)计算关节驱动力矩τ_j.

对于旋转关节有

$ {\mathit{\boldsymbol{\tau }}_j} = {}^j\mathit{\boldsymbol{M'}}_j^T{{\mathit{\boldsymbol{\hat Z}}}_j} $

(5)

对于移动关节有

$ {\mathit{\boldsymbol{\tau }}_j} = {}^j\mathit{\boldsymbol{f}}_j^T{{\mathit{\boldsymbol{\hat Z}}}_j} $

(6)

式中：${{\mathit{\boldsymbol{{\hat Z}}} }}$_j=[0, 0, 1]^T.

式(4)~(6)是牛顿欧拉法的第2部分, 其作用是从连杆n到连杆1向内迭代计算连杆间的相互作用力和力矩以及关节驱动力矩.

此外, 关节电机提供的力矩中, 除了包含驱动关节运动的部分, 还包含有克服摩擦力以及电机转子转动所需的附加力矩.对这部分的力矩按式(7)进行建模^[20]：

$ {\tau _{{\rm{subi}}}} = {{\ddot \theta }_i}J{m_i} + {{\dot \theta }_i} \cdot {f_{i1}} + {\rm{sign}}\left( {{{\dot \theta }_i}} \right) \cdot {f_{i2}} $

(7)

式中:${{\dot \theta }_i}$、${{\ddot \theta }_i}$分别为关节i的角速度和角加速度, Jm_i为电机的转动惯量, f_i1、f_i2分别为库伦摩擦力系数和黏滞摩擦力系数.

结合由牛顿欧拉方程推导的关节驱动力矩和附加力矩, 经过合并化简后, 获得关节力矩关于一组动力学可辨识参数集的线性表达式, 如式(8)所示.

$ \left. \begin{array}{l} {\tau _1} = {{\ddot q}_1}{P_1} + \left( {{{\ddot q}_1} + {{\ddot q}_2}} \right){P_2} + 0.2\left( {2{{\ddot q}_1}{c_2} + {{\ddot q}_2}{c_2} - } \right.\\ \;\;\;\;\;\;\left. {\dot q_2^2{s_2} - 2{{\dot q}_1}{{\dot q}_2}{s_2}} \right){P_3} - 0.2\left( {2{{\ddot q}_1}{s_2} + {{\ddot q}_2}{s_2} + } \right.\\ \;\;\;\;\;\;\left. {\dot q_2^2{s_2} + 2{{\dot q}_1}{{\dot q}_2}{s_2}} \right){P_4} + 0.04\left[ {2{{\ddot q}_1}\left( {1 + {c_2}} \right) + } \right.\\ \;\;\;\;\;\;\left. {{{\ddot q}_2}\left( {1 + {c_2}} \right) - \dot q_2^2{s_2} - 2{{\dot q}_1}{{\dot q}_2}{s_2}} \right]{P_5} + {{\dot q}_1}{f_{11}} + \\ \;\;\;\;\;\;{\rm{sign}}\left( {{{\dot q}_1}} \right){f_{12}},\\ {\tau _2} = \left( {{{\ddot q}_1} + {{\ddot q}_2}} \right){P_2} + 0.2\left( {{{\ddot q}_1}{c_2} + \dot q_1^2{s_2}} \right){P_3} - 0.2 \times \\ \;\;\;\;\;\;\;\left( {{{\ddot q}_1}{s_2} - \dot q_1^2{c_2}} \right){P_4} + 0.04\left[ {{{\ddot q}_1}\left( {1 + {c_2}} \right) + {{\ddot q}_2} + } \right.\\ \;\;\;\;\;\;\;\left. {\dot q_1^2{s_2}} \right]{P_5} + {{\dot q}_2}{f_{21}} + {\rm{sign}}\left( {{{\dot q}_2}} \right){f_{22}} + {{\ddot q}_2}J{m_2},\\ {\tau _3} = \left( {{{\ddot q}_3} + g} \right){P_5} + {{\dot q}_3}{f_{31}} + {\rm{sign}}\left( {{{\dot q}_3}} \right){f_{32}} + {{\ddot q}_3}J{m_3}. \end{array} \right\} $

(8)

式中:q_i、${{\dot q}_i}$、${{\ddot q}_i}$(i=1, 2, 3)分别为各关节角速度和角加速度；s₂=sin(q₂), c₂=cos (q₂), g=-9.81 m·s^-2；P₁, P₂, P₃, P₄, P₅为组合参数, 其符号表达式分别为P₁=Jm₁+IZZ₁+0.04m₂, P₂=IZZ₂+IZZ₃+IZZ₄+0.04m₃rx₃+0.4m₄rx₄, P₃=m₂rx₂+m₃rx₃+m₄rx₄, P₄=m₂ry₂-m₃ry₃-m₄ry₄, P₅=m₃+m₄, 组合参数中的各分量在原辨识方程中由于具有相同的系数而无法单独辨识出, 只能作为一个整体进行辨识；在此简单介绍符号表达式中变量的含义, Jm₁为关节1电机的转动惯量；m₂, m₃, m₄分别为连杆2、连杆3、末端载荷的质量；rx₂, ry₂, rx₃, ry₃, rx₄, ry₄分别为连杆2、连杆3、末端载荷质心相对于各自连杆坐标系的坐标；IZZ₁, IZZ₂, IZZ₃, IZZ₄分别为连杆1、连杆2、连杆3、末端载荷相对于各自连杆坐标系Z方向上的主惯性矩；记X=[P₁ P₂ P₃ P₄ P₅ f₁₁ f₁₂ f₂₁ f₂₂ f₃₁ f₃₂ Jm₂ Jm₃]^T, 为SCARA机器人的13×1可辨识参数集向量.

根据文献[14], 选取谐波个数为n=6, 基本角频率为ω_f=2π×0.08的傅里叶级数轨迹作为动力学参数辨识的激励轨迹.在经验知识和试验确定的范围内, 随机生成若干组满足关节位置、速度、加速度约束的傅里叶系数.将式(8)改写成方程组的向量形式, 即系数矩阵与可辨识参数集相乘的形式.当机器人运行时, 每一个时刻得到的一个相应系数矩阵；保证列数不变, 将所有时刻的系数矩阵合并成一个回归矩阵.可知, 每组傅里叶系数确定一条轨迹, 每条轨迹确定一个回归矩阵.选择使回归矩阵的二范数条件数最小的一组傅里叶系数所对应的轨迹作为激励轨迹.最后用最小二乘法进行动力学参数集辨识.辨识结果如表 1所示.

将表 1辨识结果代入式(8), 以关节位置、角速度、角加速度作为自变量, 重新整理可得

$ \left. \begin{array}{l} {\tau _1} = {a_1}{{\ddot q}_1} + {a_2}{{\ddot q}_2} - {a_3}\dot q_2^2 - 2{a_3}{{\dot q}_1}{{\dot q}_2} + {f_{11}}{{\dot q}_1} + \\ \;\;\;\;\;\;{\mathop{\rm sgn}} \left( {{{\dot q}_1}} \right){f_{12}},\\ {\tau _2} = {a_2}{{\ddot q}_1} + {a_4}{{\ddot q}_2} + {a_3}\dot q_1^2 + {f_{21}}{{\dot q}_2} + {\mathop{\rm sgn}} \left( {{{\dot q}_2}} \right){f_{22}},\\ {\tau _3} = \left( {{P_5} + J{m_3}} \right){{\ddot q}_3} + {f_{31}}{{\dot q}_3} + {\mathop{\rm sgn}} \left( {{{\dot q}_3}} \right){f_{32}} + {P_5}g. \end{array} \right\} $

(9)

式中,

$ {a_1} = {P_1} + {P_2} + 0.4{P_3}{c_2} - 0.4{P_4}{s_2} + 0.08{P_5}\left( {1 + {c_2}} \right); $

$ {a_2} = {P_2} + 0.2{P_3}{c_2} - 0.2{P_4}{s_2} + 0.04{P_5}\left( {1 + {c_2}} \right); $

$ {a_3} = 0.2{P_3}{s_2} + 0.2{P_4}{c_2} + 0.04{P_5}{s_2}; $

$ {a_4} = 0.2{P_3}{c_2} + 0.04{P_5} + J{m_2}. $

由式(9)可以推导出基于动力学模型的各关节角加速度估算公式：

$ \left. \begin{array}{l} {{\ddot q}_1} = \left( {{a_4}{\tau _1} - {a_2}{\tau _2} - {a_4}{f_{11}}{{\dot q}_1} + {a_2}{f_{21}}{{\dot q}_2} + {a_3}{a_4}\dot q_2^2 + } \right.\\ \;\;\;\;\;\;\;2{a_3}{a_4}{{\dot q}_1}{{\dot q}_2} + {a_2}{a_3}\dot q_1^2 - {a_4}{\rm{sign}}\left( {{{\dot q}_1}} \right){f_{12}} + \\ \;\;\;\;\;\;\;\left. {{a_2}{\rm{sign}}\left( {{{\dot q}_2}} \right){f_{22}}} \right)/\left( {{a_1}{a_4} - a_2^2} \right),\\ {{\ddot q}_2} = \left( {{a_1}{\tau _2} - {a_2}{\tau _1} - {a_1}{f_{21}}{{\dot q}_2} + {a_2}{f_{11}}{{\dot q}_1} + {a_1}{a_3}\dot q_1^2 - } \right.\\ \;\;\;\;\;\;\;2{a_2}{a_3}{{\dot q}_1}{{\dot q}_2} - {a_2}{a_3}\dot q_2^2 - {a_1}{\rm{sign}}\left( {{{\dot q}_2}} \right){f_{22}} + \\ \;\;\;\;\;\;\;\left. {{a_2}{\rm{sign}}\left( {{{\dot q}_1}} \right){f_{12}}} \right)/\left( {{a_1}{a_4} - a_2^2} \right),\\ {{\ddot q}_3} = \left( {{\tau _3} - {f_{31}}{{\dot q}_3} - {\rm{sign}}\left( {{{\dot q}_3}} \right){f_{32}} - {P_5}g} \right)/\left( {{P_5} + J{m_3}} \right). \end{array} \right\} $

(10)

2 卡尔曼估计基本算法

卡尔曼滤波方法^[15-19]是一种常用的基于时域状态空间模型的递推最优状态估计方法, 具有较好的实时性, 可以应用于受随机干扰的动态系统.Farsoni等^[12]通过基于四元数的卡尔曼滤波方法估算出末端载荷的线加速度、角加速度、角速度, 进一步计算出载荷的动力学特性, 并补偿到导纳算法中.Rigatos^[13]将无导数非线性卡尔曼滤波用于设计欠驱动多输入多输出机器人的鲁棒控制器.Ornelas等^[14]通过一个由扩展卡尔曼滤波器训练的递归神经网络, 来建立假设未知的非线性系统的模型.Asl等^[15]基于非奇异终端滑模控制, 提出了带有无迹卡尔曼滤波的, 能抗外部干扰和噪声的机器人控制率.

卡尔曼滤波的基本方法如下.假设线性系统的离散时间状态空间模型为^[20]

$ \left. \begin{array}{l} {\mathit{\boldsymbol{x}}_{k + 1}} = {\mathit{\boldsymbol{F}}_k}{\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{B}}_{wk}}{\mathit{\boldsymbol{w}}_k} + {\mathit{\boldsymbol{B}}_{uk}}{\mathit{\boldsymbol{w}}_k},\\ {\mathit{\boldsymbol{y}}_k} = {\mathit{\boldsymbol{C}}_k}{\mathit{\boldsymbol{x}}_k} + {\mathit{\boldsymbol{v}}_k}. \end{array} \right\} $

(11)

式中:F_k为状态转移矩阵，B_wk为噪声矩阵，B_uk为控制矩阵，C_k为量测矩阵，k为时间步, x_k为状态向量, u_k为控制输入, y_k为测量向量, w_k为过程噪声, v_k为测量噪声.考虑最简单的情况, 过程噪声w_k和测量噪声v_k是相互独立的零均值高斯白噪声, W_k和V_k分别是两者的协方差矩阵.

卡尔曼估计算法的一个步进循环包含2个步骤.假设已求得t时刻的验前均值${\hat {\mathit{\boldsymbol{x}} }_t}$和方差P_t.第1步计算t时刻的验后均值$\hat {\mathit{\boldsymbol{x}} }{'_t}$、验后方差P′_t和增益K_t.第2步计算t+1时刻的验前均值${\hat {\mathit{\boldsymbol{x}} }_{t+1}}$和方差P_t+1.具体算法为

t时刻验后估计：

$ \left. \begin{array}{l} {{\mathit{\boldsymbol{P'}}}_t} = {\left[ {\mathit{\boldsymbol{P}}_t^{ - 1} - \mathit{\boldsymbol{C}}_t^{\rm{T}}\mathit{\boldsymbol{V}}_t^{ - 1}{\mathit{\boldsymbol{C}}_t}} \right]^{ - 1}},\\ {\mathit{\boldsymbol{K}}_t} = {{\mathit{\boldsymbol{P'}}}_t}\mathit{\boldsymbol{C}}_k^{\rm{T}}\mathit{\boldsymbol{V}}_t^{ - 1},\\ {{\mathit{\boldsymbol{\hat x'}}}_t} = {{\mathit{\boldsymbol{\hat x}}}_t} + {\mathit{\boldsymbol{P}}_t}\left( {{\mathit{\boldsymbol{y}}_t} - {\mathit{\boldsymbol{C}}_t}{{\mathit{\boldsymbol{\hat x}}}_t}} \right). \end{array} \right\} $

(12)

t+1时刻验前估计：

$ \left. \begin{array}{l} {{\mathit{\boldsymbol{\hat x}}}_{t + 1}} = {\mathit{\boldsymbol{F}}_t}{{\mathit{\boldsymbol{\hat x'}}}_t} + {\mathit{\boldsymbol{B}}_{ut}}{\mathit{\boldsymbol{u}}_t},\\ {\mathit{\boldsymbol{P}}_{t + 1}} = {\mathit{\boldsymbol{F}}_t}{{\mathit{\boldsymbol{P'}}}_t}\mathit{\boldsymbol{F}}_t^{\rm{T}} + {\mathit{\boldsymbol{B}}_{wt}}{\mathit{\boldsymbol{W}}_t}\mathit{\boldsymbol{B}}_{wt}^{\rm{T}}. \end{array} \right\} $

(13)

要对机器人关节力矩运用卡尔曼估计, 关键在于获得以关节力矩为状态向量的离散时间状态空间模型.通过对机器人进行动力学建模可以获得非线性的关节力矩表达式.利用泰勒展开对这些表达式进行线性离散化, 即可得所需的状态空间模型.

3 关节力矩的卡尔曼估计

对第1小节中式(9)的各关节力矩公式在k时刻分别进行多元函数一阶泰勒展开, 可得到线性化的关节力矩递推公式, 如下：

$ \left. \begin{array}{l} {\tau _{1,k + 1}} = {\tau _{1,k}} + \left( {\frac{{\partial {\tau _1}}}{{\partial {{\dot q}_1}}}\Delta {{\dot q}_1} + \frac{{\partial {\tau _1}}}{{\partial {{\ddot q}_1}}}\Delta {{\ddot q}_1} + \frac{{\partial {\tau _1}}}{{\partial {q_2}}}\Delta {q_2} + } \right.\\ \;\;\;\;\;\;\;\;\;\;\left. {\frac{{\partial {\tau _1}}}{{\partial {{\dot q}_2}}}\Delta {{\dot q}_2} + \frac{{\partial {\tau _1}}}{{\partial {{\ddot q}_2}}}\Delta {{\ddot q}_2}} \right),\\ {\tau _{2,k + 1}} = {\tau _{2,k}} + \left( {\frac{{\partial {\tau _2}}}{{\partial {{\dot q}_1}}}\Delta {{\dot q}_1} + \frac{{\partial {\tau _2}}}{{\partial {{\ddot q}_1}}}\Delta {{\ddot q}_1} + \frac{{\partial {\tau _2}}}{{\partial {q_2}}}\Delta {q_2} + } \right.\\ \;\;\;\;\;\;\;\;\;\;\left. {\frac{{\partial {\tau _2}}}{{\partial {{\dot q}_2}}}\Delta {{\dot q}_2} + \frac{{\partial {\tau _2}}}{{\partial {{\ddot q}_2}}}\Delta {{\ddot q}_2}} \right),\\ {\tau _{3,k + 1}} = {\tau _{3,k}} + \left( {\frac{{\partial {\tau _3}}}{{\partial {{\dot q}_3}}}\Delta {{\dot q}_3} + \frac{{\partial {\tau _3}}}{{\partial {{\ddot q}_3}}}\Delta {{\ddot q}_3}} \right). \end{array} \right\} $

(14)

将式(14)进一步整理如式(11)所示状态空间的形式, 并假设过程噪声与测量噪声为相互独立的1维高斯白噪声.对于关节1有以下模型:

$ \left. \begin{array}{l} {\tau _{1,k + 1}} = {\tau _{1,k}} + {\mathit{\boldsymbol{B}}_{u1,k}}{\mathit{\boldsymbol{u}}_{1,k}} + {w_{1,k}},\\ {y_{1,k}} = {\tau _{1,k}} + {v_{1,k}}. \end{array} \right\} $

(15)

对于关节2有以下模型:

$ \left. \begin{array}{l} {\tau _{2,k + 1}} = {\tau _{2,k}} + {\mathit{\boldsymbol{B}}_{u2,k}}{\mathit{\boldsymbol{u}}_{2,k}} + {w_{2,k}},\\ {y_{2,k}} = {\tau _{2,k}} + {v_{2,k}}. \end{array} \right\} $

(16)

对于关节3有以下模型:

$ \left. \begin{array}{l} {\tau _{3,k + 1}} = {\tau _{3,k}} + {\mathit{\boldsymbol{B}}_{u3,k}}{\mathit{\boldsymbol{u}}_{3,k}} + {w_{3,k}},\\ {y_{3,k}} = {\tau _{3,k}} + {v_{3,k}}. \end{array} \right\} $

(17)

式中:${{\mathit{\boldsymbol{u}} }_{1, k}} = {{\mathit{\boldsymbol{u}} }_{2, k}} = {\left[{\Delta {{\dot q}_1}, \Delta {{\ddot q}_1}, \Delta {q_2}, \Delta {{\dot q}_2}, \Delta {{\ddot q}_2}} \right]^{\rm{T}}}; {{\mathit{\boldsymbol{u}} }_{3, k}} = {\left[{\Delta {{\dot q}_3}\; \; \Delta {{\ddot q}_3}} \right]^{\rm{T}}}; $

$ {\mathit{\boldsymbol{B}}_{u1,k}} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial {\tau _1}}}{{\partial {{\dot q}_1}}},}&{\frac{{\partial {\tau _1}}}{{\partial {{\ddot q}_1}}},}&{\frac{{\partial {\tau _1}}}{{\partial {q_2}}},}&{\frac{{\partial {\tau _1}}}{{\partial {{\dot q}_2}}},}&{\frac{{\partial {\tau _1}}}{{\partial {{\ddot q}_2}}},} \end{array}} \right]. $

(18)

其中, $\frac{{\partial {\tau _1}}}{{\partial {{\dot q}_1}}} = - 2{a_3}{{\dot q}_2} + {f_{11}}, \frac{{\partial {\tau _1}}}{{\partial {{\ddot q}_1}}}{\rm{ = }}{a_{\rm{1}}}$,

$ \left. \begin{array}{l} \frac{{\partial {\tau _1}}}{{\partial {q_2}}} = - {a_3}\left( {2{{\ddot q}_1} + {{\ddot q}_2}} \right) - \left( {0.2{P_3}{c_2} - 0.2{P_4}{s_2} + } \right.\\ \;\;\;\;\;\;\;\;\;\;\left. {0.04{P_5}{c_2}} \right)\left( {\dot q_2^2 + 2{{\dot q}_1}{{\dot q}_2}} \right),\\ \frac{{\partial {\tau _1}}}{{\partial {{\dot q}_2}}} = - 2{a_3}\left( {{{\dot q}_2} + {{\dot q}_1}} \right),\frac{{\partial {\tau _1}}}{{\partial {{\ddot q}_2}}} = {a_2}; \end{array} \right\} $

(19)

$ {\mathit{\boldsymbol{B}}_{u2,k}} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial {\tau _2}}}{{\partial {{\dot q}_1}}},}&{\frac{{\partial {\tau _2}}}{{\partial {{\ddot q}_1}}},}&{\frac{{\partial {\tau _2}}}{{\partial {q_2}}},}&{\frac{{\partial {\tau _2}}}{{\partial {{\dot q}_2}}},}&{\frac{{\partial {\tau _2}}}{{\partial {{\ddot q}_2}}}} \end{array}} \right] $

其中, $\frac{{\partial {\tau _2}}}{{\partial {{\dot q}_1}}} = - 2{a_3}{{\dot q}_1}, \frac{{\partial {\tau _2}}}{{\partial {{\ddot q}_1}}}{\rm{ = }}{a_2}$,

$ \left. \begin{array}{l} \frac{{\partial {\tau _2}}}{{\partial {q_2}}} = - {a_3}{{\ddot q}_1} - 0.2{P_3}{s_2}{{\ddot q}_2} + \left( {0.2{P_3}{c_2} - } \right.\\ \;\;\;\;\;\;\;\;\;\;0.2{P_4}{s_2} + \left. {0.04{P_5}{c_2}} \right)\dot q_1^2,\\ \frac{{\partial {\tau _2}}}{{\partial {{\dot q}_2}}} = {f_{21}},\frac{{\partial {\tau _2}}}{{\partial {{\ddot q}_2}}} = {a_4}; \end{array} \right\} $

(20)

$ {\mathit{\boldsymbol{B}}_{u3,k}} = \left[ {\begin{array}{*{20}{c}} {\frac{{\partial {\tau _3}}}{{\partial {{\dot q}_3}}},}&{\frac{{\partial {\tau _3}}}{{\partial {{\ddot q}_3}}}} \end{array}} \right] $

其中, $\frac{{\partial {\tau _3}}}{{\partial {{\dot q}_3}}} = {f_{31}}, \frac{{\partial {\tau _3}}}{{\partial {{\ddot q}_3}}}{\rm{ = }}{P_5} + J{m_3}$.

对获得的各关节力矩线性化模型, 即式(15)、式(16)、(17), 可用式(12)、(13)线性离散卡尔曼估计方法进行力矩估计.

4 关节力矩信息采集实验及卡尔曼估计数据处理 4.1 实验装置及实验步骤

如图 2(a)所示为国内某公司生产的SCARA机器人, 可运动轴数为4, 额定负载2 kg, 最大负载5 kg.第1轴和第2轴最高角速度可达600 °/s, 在笛卡尔坐标下第1轴和第2轴合成运动最高速度可达6.3 m/s.第3轴最高速度可达1.3 m/s.第4轴最高角速度可达1667 °/s.在笛卡尔坐标下, 第1轴和第2轴合成运动时, 机器人末端在水平面的重复定位精度可达±0.01 mm；第3轴运动时, 机器人末端在铅锤面的重复定位精度可达±0.01 mm.在关节坐标下, 第4轴旋转时机器人末端的重复定位精度可达±0.005 °.采用实时控制系统, 控制周期可达1 ms, 能够采回电机编码器信号以及电机电流信号.

如图 2(b)所示为实验装置工作时的逻辑框图.安装在工控机中的系统发出控制指令, 并通过EtherCAT协议传送给控制箱中的驱动器, 从而控制SCARA机器人运动.同时, 每个控制周期读取当前时刻的电机电流值和编码器值返回实时系统.

实验时运行实时控制系统, 令SCARA机器人按设计好的轨迹运动即可.系统采回的电机编码器信号及电机电流信号将用于后续的数据处理.机器人所运行的轨迹为傅里叶级数轨迹.对于关节j, 其理论轨迹及相应的角速度、角加速度表达式可写为^[14]

$ \left. \begin{array}{l} {q_j}\left( t \right) = {q_{0j}} + \sum\limits_{i = 1}^n {\frac{{{a_{ji}}}}{{{\omega _f}i}}\sin \left( {{\omega _f}it} \right)} - \frac{{{b_{ji}}}}{{{\omega _f}i}}{\cos} \left( {{\omega _f}it} \right),\\ {{\dot q}_j}\left( t \right) = \sum\limits_{i = 1}^n {{a_{ji}}{\cos}\left( {{\omega _f}it} \right) + {b_{ji}}\sin \left( {{\omega _f}it} \right)} ,\\ {{\ddot q}_j}\left( t \right) = \sum\limits_{i = 1}^n { - {a_{ji}}{\omega _f}i\sin \left( {{\omega _f}it} \right) + {b_{ji}}{\omega _f}i{\cos} \left( {{\omega _f}it} \right)} . \end{array} \right\} $

(21)

式中:q_0j为起始角, n为谐波的个数, ω_f为基本角频率, a_ji、b_ji (j=1, 2, 3;i=1, 2, …, n)为傅里叶系数.本文选取n=6, ω_f=2π×0.08.由基本角频率可以计算出激励轨迹的周期为12.5 s.在经验知识和试验确定的范围内, 随机生成一组满足关节位置、速度、加速度约束的傅里叶系数.各关节的傅里叶系数如表 2所示.

4.2 数据处理

通过查阅关节电机说明资料, 将读取到的电机编码器值及电机电流值按确定的公式转换为关节位置值以及关节力矩值.

1) 第1步计算初始值.

首先, 根据设计的轨迹计算出t=0时刻q₁, ${{{\dot q}_1}}$, ${{{\ddot q}_1}}$, q₂, ${{{\dot q}_2}}$, ${{{\ddot q}_2}}$, ${{{\dot q}_3}}$, ${{{\ddot q}_3}}$的理论值, 代入第1小节中的关节力矩公式, 即式(9), 将计算出的理论力矩值作为t=0时刻关节力矩的验前均值${{\hat \tau }_{1, 0}}$, ${{\hat \tau }_{2, 0}}$, ${{\hat \tau }_{3, 0}}$, 并设置方差P_{1, 0}=P_{2, 0}=P_{3, 0}=0.1.同时假设任意时刻过程噪声w_k协方差为W_k=0.01, 测量噪声v_k协方差为V_k=0.01.机器人控制周期为1 ms.实时系统每1 ms发送运动指令, 同时读取当前编码器值与电机电流值, 并转换为位置值和力矩值.

2) 第2步计算当前时刻各关节角速度与角加速度.

记机器人运动开始时刻为t=0时刻, 运动开始后每一个控制周期对应一个时刻.在t=4时刻之前, 取当前时刻i读取到的3个关节的位置值与力矩值q_{1, i}, q_{2, i}, q_{3, i}, τ_{1, i}, τ_{2, i}, τ_{3, i}, 再取i-1时刻3个关节的位置值q_{1, i-1}, q_{2, i-1}, q_{3, i-1}, 通过一次差分估算当前时刻关节角速度, 再通过第1小节中的式(10)估算各关节的角加速度.此处不用二次差分估计角加速度是因为二次差分会放大噪声.以关节1为例, 图 3(a)为二次差分计算的关节1的角加速度.图 3(b)中实线为式(10)估算的角加速度, 虚线为由运动轨迹计算的理论角加速度.从图中可以看出, 噪声使二次差分方法获得的关节角加速度值波动较大, 式(10)估算角加速度的精度要比二次差分好.

在t=4时刻及之后, 关节位置值的取法与t=4时刻之前相同, 关节角速度仍由一次差分算得.但当前时刻的关节力矩则变为取包括当前时刻的最近5个时刻均值滤波值, 即

$ \begin{array}{l} {\tau _i} = \left( {p\tau \left( i \right) + p\tau \left( {i - 1} \right) + p\tau \left( {i - 2} \right) + } \right.\\ \;\;\;\;\;\left. {p\tau \left( {i - 3} \right) + p\tau \left( {i - 4} \right)} \right)/5 \end{array} $

(22)

式中:pτ表示关节力矩测量序列；

计算好关节力矩值后, 代入第1小节中的式(10)计算关节角加速度.

3) 第3步对关节力矩进行卡尔曼估计.

将关节位置及第2步中计算得到的关节角速度和角加速度代入第3小节中的式(18)、(19)、(20)计算当前时刻的B_{u1, i}, B_{u2, i}, B_{u3, i}.在计算增量u_{1, i}, u_{2, i}, u_{3, i}时, 先由运动轨迹公式计算i+1时刻的理论角度q_{t, i+1}, 角速度${{\dot q}_{t, i + 1}}$, 角加速度${{\ddot q}_{t, i + 1}}$, 再减去第2步中算出的i时刻的相应值即可.

$ \left. \begin{array}{l} {\mathit{\boldsymbol{u}}_{1,i}} = {\mathit{\boldsymbol{u}}_{2,i}} = \left[ {\begin{array}{*{20}{c}} {{{\dot q}_{t1,i + 1}} - {{\dot q}_{1,i}}}\\ {{{\ddot q}_{t1,i + 1}} - {{\ddot q}_{1,i}}}\\ {{q_{t2,i + 1}} - {q_{2,i}}}\\ {{{\dot q}_{t2,i + 1}} - {{\dot q}_{2,i}}}\\ {{{\ddot q}_{t2,i + 1}} - {{\ddot q}_{2,i}}} \end{array}} \right],\\ {\mathit{\boldsymbol{u}}_{3,k}} = \left[ {\begin{array}{*{20}{c}} {{{\dot q}_{t3,i + 1}} - {{\dot q}_{3,i}}}\\ {{{\ddot q}_{t3,i + 1}} - {{\ddot q}_{3,i}}} \end{array}} \right]. \end{array} \right\} $

(23)

计算完B_{u1, i}, B_{u2, i}, B_{u3, i}, u_{1, i}, u_{2, i}, u_{3, i}后, 按第2小节中的式(12)、(13)对关节力矩进行卡尔曼估计.

估计结果如图 4~6所示.图中T为力矩，图 4(a)、5(a)、6(a)分别表示关节1、2、3力矩的原始测量值.图 4(b)、5(b)、6(b)中实线表示的是对应的关节力矩均值滤波值, 带实心圆点标记的虚线表示的是本文所提卡尔曼估计值, 点划线表示的是高斯滤波值.

从图中可见关节力矩的卡尔曼估计值与原始数据的变化趋势一致, 但具体数值有一定的偏差.关节1的估计值最理想.关节2的估计值有偏向曲线的波峰或波谷的趋势.关节3的估计值与实测值的相差较大, 不能很好地反映曲线的复杂波动, 只能反映大致的变化趋势.高斯滤波是一种常用的线性平滑滤波器, 是对整段信号的后期加权平均过程, 具有很好地去除噪声效果.由于实验所用机器人未安装高精度关节力矩传感器, 无法得到精确的关节力矩值.而动力学模型精度会影响计算得到的理论关节力矩值.因此选用高斯滤波值作为理论真值.

如图 7所示为不同方法力矩信息处理值与理论真值的误差比较图.实线表示均值滤波值与理论真值的误差.虚线表示卡尔曼估计值与理论真值的误差.

均值滤波、卡尔曼估计的均方根误差如表 3所示.可见, 对第1、2关节, 卡尔曼滤波的效果比均值估计好；对第3关节, 均值滤波的效果比卡尔曼估计好.此外, 在Matlab仿真中, 3个关节的均值滤波算法编写在同一个m文件, 程序运行需时0.010 s；第1、2关节的卡尔曼估计算法编写在同一个m文件, 第3关节的算法单独编写在另一个m文件, 程序运行总需时分别为0.953和0.413 s.由于是对12 500个时刻的数据处理, 则每个时刻第1、2关节卡尔曼估计算法需时约为0.076 ms, 第3关节需时约为0.033 ms.实验所用机器人控制周期为1 ms, 卡尔曼估计算法能满足控制的实时性要求.

经分析, 本文所提卡尔曼估计方法的精度主要与动力学模型的精确程度有关.加速度的估算公式是由第1小节中提出的动力学模型求得.卡尔曼滤波估计中用到的关节力矩的状态空间模型也是由第1小节提出的动力学模型经泰勒展开求得.另一方面, 从结构来看, SCARA机器人的前两轴为旋转轴, 其运动是由电机通过谐波减速器进行传递, 谐波减速器具有摩擦力小, 力矩传递均匀的特点.而第3轴为移动轴, 其运动是由电机经多级齿轮系减速器, 再经同步带传递到滚珠丝杠而实现.第3轴的传动机构比前两轴要复杂得多, 一些动力学特性很有可能并未包含到由牛顿欧拉方法建模的动力学方程中；且多级齿轮系减速器传递过程存在摩擦力不均匀等问题, 所选用的摩擦模型可能并没有完全对应实际的摩擦特性.回顾本文第1小节的关节力矩公式, 第3轴的关节力矩公式明显比前两轴要简单, 这意味着对力矩特性的描述不如前两轴精确, 而实验结果也表明第3轴的力矩估计值与理论值偏差较大.综上所述, 动力学模型的精度很大程度上影响了关节力矩估计的精度.此外, 对过程噪声和测量噪声的不准确估计也是影响估计精度的因素之一.

实验结果表明, 该估计算法具有可行性, 但动力学模型精度影响了估计精度.

5 结语

本文用牛顿欧拉法对SCARA机器人进行动力学建模, 并完成了动力学参数的辨识.结合机器人动力学模型, 通过多元函数的一阶泰勒展开获得了以关节力矩为状态变量的线性离散状态空间模型.根据获得的状态空间模型, 采用卡尔曼估计方法对SCARA机器人关节力矩进行估计.

实验结果表明, 该方法效果较好, 得到的关节力矩估计值较准确.第1、2、3关节的关节力矩卡尔曼估计值的RMSE分别为0.436 7、0.183 4、6.880 1, 均值滤波值的RMSE分别为0.450 0、0.215 1、2.829 8.可见, 卡尔曼估计方法对第1、2关节的关节力矩信息处理效果要比均值滤波方法好, 但对第3关节的力矩信息估计效果不如均值滤波方法.由于SCARA机器人的定位主要依赖于前2个关节, 因此在实际应用中若只对第1、2关节力矩运用卡尔曼估计, 仍有利于提高控制算法性能, 从而进一步提高机器人的定位精度.而对于第3关节则可考虑单独选用其他效果更好的力矩估计算法.同时, 卡尔曼估计方法是递推的, 能满足机器人应用中的实时性要求.Matlab仿真表明, 对第1、2关节的一个关节力矩原始测量值进行一次卡尔曼估计所需时间约为0.076 ms, 对第3关节需时约为0.033 ms.力矩信息估计过程所需时间均远小于SCARA机器人系统的控制周期1 ms.