﻿ 平面关节型机器人关节力矩的卡尔曼估计
 文章快速检索 高级检索
 浙江大学学报(工学版)  2018, Vol. 52 Issue (5): 951-959  DOI:10.3785/j.issn.1008-973X.2018.05.015 0

### 引用本文 [复制中英文]

dx.doi.org/10.3785/j.issn.1008-973X.2018.05.015
[复制中文]
ZHANG Tie, LIANG Xiao-hong. Kalman filter-based SCARA robot joint torque estimation[J]. Journal of Zhejiang University(Engineering Science), 2018, 52(5): 951-959.
dx.doi.org/10.3785/j.issn.1008-973X.2018.05.015
[复制英文]

### 文章历史

Kalman filter-based SCARA robot joint torque estimation
ZHANG Tie , LIANG Xiao-hong
School of Mechanical and Automotive Engineering, South China University of Technology, Guangzhou 510640, China
Abstract: A dynamic model based Kalman estimation method was designed for joint torque estimation in order to solve the problem that robot control information reliability was reduced because of the fluctuating joint torque value obtained by commonly used current measurement method while the current was affected by noise generated during robot operation. The Newton-Euler method was used for SCARA robot dynamic modeling and the nonlinear continuous joint torque equations were obtained. Then the nonlinear continuous joint torque equations were converted to linear discrete state space model of joint torque by first-order Taylor expansion of multivariate functions. The Kalman filter method was performed to estimate joint torque. Experimental results showed that the proposed method had better estimation accuracy for the first two axes of the robot and the mean square root error decreases by 2.9% and 14.7%, respectively, compared to mean filtering method. The real-time performance was good that the average time required to complete an estimation did not exceed 1 ms. The joint torque estimation accuracy was affected by the SCARA robot dynamic model accuracy.
Key words: SCARA robot    dynamic model    joint torque    Kalman estimation

1 SCARA机器人动力学建模

 1-底座; 2-连杆1;3-连杆2; 4-组合连杆3;5-负载, 虚线表示可装卸 图 1 DH模型示意图 Fig. 1 DH model of SCARA robot

 $\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)

 $\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)

 $\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)

 ${\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)

 ${\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)

 $\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}.$

 $\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 卡尔曼估计基本算法

 $\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)

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 关节力矩的卡尔曼估计

 $\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)

 $\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)

 $\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)

 $\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{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)

 $\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]$

 $\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]$

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

 1-本体；2-控制箱(含驱动器)；3-工作台 图 2 关节力矩信息采集实验装置 Fig. 2 Joint torque imformation acquisition Experimental device

 $\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)

4.2 数据处理

1) 第1步计算初始值.

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

 图 3 关节1角加速度估计算法比较 Fig. 3 Comparison between different joint 1 angular acceleration estimation algorithms

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)

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

 $\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)

 图 4 关节1力矩测量值与卡尔曼估计值比较 Fig. 4 Comparison between joint 1 torque measured value and Kalman estimation value
 图 5 关节2力矩测量值与卡尔曼估计值比较 Fig. 5 Comparison between joint 2 torque measured value and Kalman estimation value
 图 6 关节3力矩测量值与卡尔曼估计值比较 Fig. 6 Comparison between joint 3 torque measured value and Kalman estimation value

 图 7 不同数据处理方法误差比较 Fig. 7 Comparison between error of mean filter anderror of kalman estimation

5 结语

 [1] DZHANKHOTOV V, PYRHÖNEN J., PASSIVE L C. Filter design considerations for motor applications[J]. IEEE Transactions on Industrial Electronics, 2013, 60(10): 4253-4259. DOI:10.1109/TIE.2012.2209612 [2] AKAGI H, ISOZAKI K. A hybrid active filter for a three-phase 12-pulse diode rectifier used as the front end of a medium-voltage motor drive[J]. IEEE Transactions on Power Electronics, 2012, 27(1): 69-77. DOI:10.1109/TPEL.2011.2157977 [3] KINSHEEL A, TAHA Z, DEBOUCHA A, et al. Robust least square estimation of the CRS A465 robot arms dynamic model parameters[J]. Journal of Mechanical Engineering Research, 2012, 4(3): 88-89. [4] 牛里, 杨明, 王庚, 等. 基于无差拍控制的永磁同步电机鲁棒电流控制算法研究[J]. 中国电机工程学报, 2013, 33(15): 78-85. NIU Li, YANG Ming, WANG Geng, et al. Research on the robust current control algorithm of permanent magnet synchronous motor based on deadbeat control principle[J]. Proceedings of the CSEE, 2013, 33(15): 78-85. [5] 王东文, 宋词, 周伟. 自适应滤波在永磁同步电机电流环优化中的应用[J]. 电工技术学报, 2014(增刊1): 90-94. WANG Dong-wen, SONG Ci, ZHOU Wei. Adaptive filter applied in optimizing current loop of permanent magnet synchronous Motor[J]. Transactions of China Electrotechnical Society, 2014(supp11): 90-94. [6] 张海洋, 许海平, 方程, 等. 基于谐振数字滤波器的直驱式永磁同步电机转矩脉动抑制方法[J]. 中国电机工程学报, 2018, 38(4): 1222-1231. ZHANG Hai-yang, XU Hai-ping, FANG Cheng, et al. Torque ripple suppression method of direct-drive permanent magnet synchronous motor based on resonant digital filter[J]. Proceedings of the CSEE, 2018, 38(4): 1222-1231+1299. [7] GROTJAHN M, DAEMI M, HEIMANN B. Friction and rigid body identification of robot dynamics[J]. International Journal of Solids and Structures, 2001, 38(10): 1889-1902. [8] 杨松, 王毅, 苏宝库. 高精确度伺服转台控制系统中的扰动力矩补偿[J]. 电机与控制学报, 2009, 13(4): 615-619. YANG Song, WANG Yi, SU Bao-ku. Study on disturbance torques compensation in high precise servo turntable control system[J]. Electric Machines and Control, 2009, 13(4): 615-619. [9] 赵辉, 冯英浚, 张志忠, 等. 神经网络用于永磁同步电机系统波动力矩补偿[J]. 哈尔滨工业大学学报, 2003, 35(1): 5-7. ZHAO Hui, FENG Ying-jun, ZHANG Zhi-zhong, et al. Application of neural network for ripple torque compensation of permanent magnet synchronous motor system[J]. Journal of Harbin Institute of Technology, 2003, 35(1): 5-7. [10] VAN DAMME M, BEYL P, Vanderborght B, et al. Estimating robot end-effector force from noisy actuator torque measurements[C]//Robotics and Automation (ICRA), 2011 IEEE International Conference on. Shanghai: IEEE, 2011: 1108-1113. http://dx.doi.org/10.1109/ICRA.2011.5980210 [11] ZOLLO L, LOPEZ E, SPEDALIERE L, et al. Identification of dynamic Parameters for robots with elastic joints[J]. Advances in Mechanical Engineering, 2015, 7(2): 843186-843186. DOI:10.1155/2014/843186 [12] ZHU S, CHEN Q, WANG X, et al. Dynamic modelling using screw theory and nonlinear sliding mode control of serial robot[J]. International Journal of Robotics and Automation, 2016, 31(1): 63-75. [13] CRAIG J J. Introduction to robotics:mechanics and control, third edition[M]. Upper Saddle River: Pearson Prentice Hall, 2005. [14] SOUSA C J S D. Dynamic model identification of robot manipulators: Solving the physical feasibility problem[D]. Coimbra: University of Coimbra, 2014. http://eg.sib.uc.pt/jspui/handle/10316/27082 [15] 陈金广, 李洁, 高新波. 双重迭代变分贝叶斯自适应卡尔曼滤波算法[J]. 电子科技大学学报, 2012, 41(3): 359-363. CHEN Jin-guang, LI Jie, GAO Xin-bo. Dual Recursive Variational Bayesian Adaptive Kalman Filtering Algorithm[J]. Journal of University of Electronic Science and Technology of China, 2012, 41(3): 359-363. [16] FARSONI S, LANDI C T, FERRAGUTI F, et al. Compensation of Load Dynamics for Admittance Controlled Interactive Industrial Robots Using a Quaternion-Based Kalman Filter[J]. IEEE Robotics and Automation Letters, 2017, 2(2): 672-679. DOI:10.1109/LRA.2017.2651393 [17] RIGATOS G G. Control and disturbances compensation in underactuated robotic systems using the derivative-free nonlinear Kalman filter[J]. Robotica, 2017, 35(3): 687-711. DOI:10.1017/S0263574715000776 [18] ORNELAS F, SANCHEZ E N, LOUKIANOV A G. Real-time inverse optimal control for a planar robot[C]//Intelligent Control (ISIC), 2010 IEEE International Symposium on. Yokohama: IEEE, 2010: 2332-2337. http://dx.doi.org/10.1109%2fISIC.2010.5612912 [19] ASL R M, HAGH Y S, PALM R. Robust control by adaptive non-singular terminal sliding mode[J]. Engineering Applications of Artificial Intelligence, 2017, 59: 205-217. DOI:10.1016/j.engappai.2017.01.005 [20] 钟万勰, 吴志刚, 谭述君. 状态空间控制理论与计算[M]. 北京: 科学出版社, 2007.