﻿ 基于模型预测控制的磨削机器人末端力跟踪控制算法
 文章快速检索 高级检索
 山东大学学报(工学版)  2018, Vol. 48 Issue (1): 42-49  DOI: 10.6040/j.issn.1672-3961.0.2017.569 0

### 引用本文

LIU Zhe, SONG Rui, ZOU Tao. End force tracking control algorithm of grinding robot based on model predictive control[J]. Journal of Shandong University (Engineering Science), 2018, 48(1): 42-49. DOI: 10.6040/j.issn.1672-3961.0.2017.569.

### 文章历史

1. 中国科学院沈阳自动化研究所中国科学院网络化控制系统重点实验室, 辽宁 沈阳 110016;
2. 中国科学院大学, 北京 100049;
3. 山东大学控制科学与工程学院, 山东 济南 250061

End force tracking control algorithm of grinding robot based on model predictive control
LIU Zhe1,2, SONG Rui3, ZOU Tao1
1. Key Laboratory of Networked Control System of CAS, Shenyang Institute of Automation of Chinese Academy of Sciences, Shenyang 110016, Liaoning, China;
2. University of Chinese Academy of Sciences, Beijing 100049, China;
3. School of Control Science and Engineering, Shandong University, Jinan 250061, Shandong, China
Abstract: A precise control algorithm using the model predictive control and surface prediction was proposed. The dynamic model and the prediction model of the manipulator were given. The surface coordinates of the next moment were obtained according to the surface prediction algorithm, and the desired rotation angle of each joint was obtained by the fast algorithm and the inverse kinematic algorithm. And in order to realize the overshoot tracking of the desired joint angle, the dynamic matrix control algorithm was used to solve the input torque of each joint motor. The end-point of manipulator was controlled to track the continuous surface by experimental and simulation. The results showed that the end force control algorithm using surface prediction and model prediction algorithm could effectively track the continuous surface in real time, which satisfied the ideal force control requirement.
Key words: interaction environment    manipulator    force control    model predictive control
0 引言

1 机械臂模型以及工作环境模型 1.1 受限机械臂动力学分析

 图 1 三自由度机械臂结构图 Figure 1 The structure diagram of three degrees of freedom mechanical arm

 $\mathit{\boldsymbol{D}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{\ddot q}} + \mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right)\mathit{\boldsymbol{\dot q}} + \mathit{\boldsymbol{G}}\left( \mathit{\boldsymbol{q}} \right)\mathit{\boldsymbol{q}} - {\mathit{\boldsymbol{J}}^{\rm{T}}}\mathit{\boldsymbol{F}} = \mathit{\boldsymbol{\tau }},$ (1)

 $\left\{ \begin{array}{l} {{\mathit{\boldsymbol{\dot x}}}_1} = {\mathit{\boldsymbol{x}}_2}\\ {{\mathit{\boldsymbol{\dot x}}}_2} = - \mathit{\boldsymbol{D}}{\left( \mathit{\boldsymbol{q}} \right)^{ - 1}}\mathit{\boldsymbol{C}}\left( {\mathit{\boldsymbol{q}},\mathit{\boldsymbol{\dot q}}} \right) + \mathit{\boldsymbol{B\tau }}\left( t \right) - \mathit{\boldsymbol{D}}{\left( \mathit{\boldsymbol{q}} \right)^{ - 1}}\mathit{\boldsymbol{G}}。\\ \mathit{\boldsymbol{y}} = {\mathit{\boldsymbol{x}}_1} \end{array} \right.$ (2)

 $\mathit{\boldsymbol{x}} = \left[ {\begin{array}{*{20}{c}} {{\mathit{\boldsymbol{x}}_1}}\\ {{\mathit{\boldsymbol{x}}_2}} \end{array}} \right] = \left[ \begin{array}{l} {\mathit{\boldsymbol{x}}_1} + \mathit{\boldsymbol{T}}{{\mathit{\boldsymbol{\dot x}}}_1} + \frac{{{\mathit{\boldsymbol{T}}^2}}}{2}{{\mathit{\boldsymbol{\ddot x}}}_1}\\ {\mathit{\boldsymbol{x}}_2} + \mathit{\boldsymbol{T}}{{\mathit{\boldsymbol{\dot x}}}_2} \end{array} \right]。$ (3)

 $\left\{ \begin{array}{l} \mathit{\boldsymbol{x}}\left( {k + 1} \right) = \boldsymbol{x}\left( {t = \left( {k + 1} \right)T} \right) = \mathit{\boldsymbol{Ax}}\left( k \right) + \mathit{\boldsymbol{B\tau }}\left( k \right) + \mathit{\boldsymbol{G}}\\ \mathit{\boldsymbol{y}}\left( k \right) = \mathit{\boldsymbol{Hx}}\left( k \right),k = 0, \cdots ,N - 1 \end{array} \right.,$ (4)

1.2 未知作业曲面预测方程

 图 2 末端局部受力图 Figure 2 The figure of end force
 $\Delta {x_{\rm{E}}} = {x_{\rm{E}}} - {x_{{\rm{E0}}}} = {F_{\rm{E}}}/{K_{\rm{E}}},$

 图 3 工作曲面局部放大图 Figure 3 The enlarge figure of working surface

 $\left\{ \begin{array}{l} {X_{{\rm{E}},k + 1}} = {X_{{\rm{E}},k}} + {K_k}\Delta y + {e_{{X_{\rm{E}}}}}\\ {K_k} = {K_{k - 1}} + \Delta {K_{k - 1}}\\ \Delta {K_{k - 1}} = {K_{k - 1}} - {K_{k - 2}}\\ {e_{{X_{\rm{E}}}}} = {X_{\rm{R}}} - {F_{k - 1}}/{K_{\rm{E}}} - {X_{{\rm{E}},k}} \end{array} \right.,$ (5)

2 控制器设计 2.1 轨迹规划层控制器设计

 $\Delta {\mathit{\boldsymbol{F}}_{{\rm{ss\_TTSS}},k + 1}} = {\mathit{\boldsymbol{K}}_{{\rm{E}},k}}\Delta {\mathit{\boldsymbol{x}}_{{\rm{w}},k + 1}},k = 1. \cdots ,N,$

 ${\mathit{\boldsymbol{F}}_{{\rm{ss}},k}} = {\mathit{\boldsymbol{F}}_{{\rm{ss}},k - 1}} + {\mathit{\boldsymbol{K}}_{{\rm{E}},k}}\left( {{\mathit{\boldsymbol{x}}_{{\rm{ss\_measure}},k}} - {\mathit{\boldsymbol{x}}_{{\rm{ss}},k - 1}}} \right),$

 $\mathit{\boldsymbol{e}}\left( k \right) = \mathit{\boldsymbol{F}}\left( k \right) - {\mathit{\boldsymbol{F}}_{{\rm{ss\_TTSS}},k}},$

 ${\mathit{\boldsymbol{F}}_{{\rm{ss\_cor}},k + 1}} = {\mathit{\boldsymbol{F}}_{{\rm{ss\_TTSS}},k + 1}} + \mathit{\boldsymbol{e}}\left( k \right),$

 $\Delta {\mathit{\boldsymbol{x}}_{\rm{u}}} = \Delta {\mathit{\boldsymbol{x}}_{{\rm{ss}}\_{\rm{calc}}}}/N,$

 ${\mathit{\boldsymbol{x}}_{\rm{z}}}\left( {k + 1} \right) = {\mathit{\boldsymbol{x}}_{\rm{z}}}\left( k \right) + \Delta {\mathit{\boldsymbol{x}}_{\rm{u}}} + \Delta {\mathit{\boldsymbol{x}}_{\rm{E}}},$

 图 4 机械臂几何简图 Figure 4 The geometric diagram of robot
 $OC = \sqrt {{x^2} + {y^2}} ,$
 ${\theta ''_2} = \arctan \left( {\frac{z}{{OC}}} \right),$
 $OB = \sqrt {O{C^2} + {z^2}} 。$
 ${\theta '_2} = \arccos \left( {\frac{{OB}}{{2OA}}} \right),$
 ${\theta _2} = {\theta '_2} + {\theta ''_2},$
 ${\theta _3} = - 2{\theta _2},$
 ${\theta _1} = \arctan \left( {\frac{y}{x}} \right),$

2.2 轨迹跟踪层控制器设计

(1) 预测模型

 $\left\{ \begin{array}{l} \mathit{\boldsymbol{x}}\left( 0 \right) = {\mathit{\boldsymbol{x}}_{\rm{s}}}\\ \mathit{\boldsymbol{x}}\left( {k + 1} \right) = \mathit{\boldsymbol{Ax}}\left( k \right) + \mathit{\boldsymbol{B\tau }}\left( k \right) + \mathit{\boldsymbol{G}}\\ \mathit{\boldsymbol{y}}\left( {k + 1} \right) = \mathit{\boldsymbol{Hx}}\left( {k + 1} \right)\\ \mathit{\boldsymbol{\tau }}\left( k \right) = \mathit{\boldsymbol{\tau }}\left( {k - 1} \right) + \Delta \mathit{\boldsymbol{\tau }}\left( k \right)\\ \mathit{\boldsymbol{\tau }}\left( 0 \right) = {\mathit{\boldsymbol{\tau }}_{\rm{s}}} \end{array} \right.,$

${\boldsymbol{y}_{\rm PM}} = \left[ {\begin{array}{*{20}{c}} {\boldsymbol{x}\left( 1 \right)}\\ \vdots \\ {\boldsymbol{x}\left( N \right)} \end{array}} \right]$, ${\boldsymbol{u}_\tau } = \left[ {\begin{array}{*{20}{c}} {\boldsymbol{\tau} \left( 0 \right)}\\ \vdots \\ {\boldsymbol{\tau} \left( {N - 1} \right)} \end{array}} \right]$, 则系统的预测模型可表示为:

 ${\mathit{\boldsymbol{y}}_{{\rm{PM}}}}\left( k \right) = \mathit{\boldsymbol{L}}{\mathit{\boldsymbol{x}}_{\rm{s}}}\left( k \right) + \mathit{\boldsymbol{M}}{\mathit{\boldsymbol{u}}_\tau }\left( k \right) + \mathit{\boldsymbol{O}}\left( k \right),$

(2) 滚动优化

 $\begin{array}{*{20}{c}} {\mathit{\boldsymbol{J}}\left( {{\mathit{\boldsymbol{y}}_{{\rm{PM}}}}\left( t \right),{\mathit{\boldsymbol{u}}_\tau }\left( {t - 1} \right)} \right) = \sum\limits_{j = 1}^N {\left\| {{\mathit{\boldsymbol{y}}_{{\rm{PM}}}}\left( {t + i\left| t \right.} \right) - } \right.} }\\ {\left. {{\mathit{\boldsymbol{y}}_{{\rm{ref}}}}\left( {t + i\left| t \right.} \right)} \right\|_Q^2 + \sum\limits_{j = 1}^{{N_{\rm{c}}}} {\left\| {\Delta {\mathit{\boldsymbol{u}}_\tau }\left( {t + i\left| t \right.} \right)} \right\|_\mathit{\boldsymbol{R}}^2} ,} \end{array}$

 $\Delta {\mathit{\boldsymbol{u}}_\tau }\left( k \right) = {\left( {{\mathit{\boldsymbol{M}}^{\rm{T}}}\mathit{\boldsymbol{QM}} + \mathit{\boldsymbol{R}}} \right)^{ - 1}}{\mathit{\boldsymbol{A}}^{\rm{T}}}\mathit{\boldsymbol{Q}}\left( {{\mathit{\boldsymbol{y}}_{{\rm{ref}}}} - {\mathit{\boldsymbol{y}}_{{\rm{p0}}}}} \right),$

(3) 反馈校正

 ${\mathit{\boldsymbol{y}}_{{\rm{PM}}}}\left( k \right) = \mathit{\boldsymbol{L}}{\mathit{\boldsymbol{x}}_{\rm{s}}}\left( k \right) + \mathit{\boldsymbol{M}}{\mathit{\boldsymbol{u}}_\tau }\left( k \right) + \mathit{\boldsymbol{N}}\left( k \right)。$

 $\mathit{\boldsymbol{e}}\left( {k + 1} \right) = \mathit{\boldsymbol{y}}\left( {k + 1} \right) - {\mathit{\boldsymbol{y}}_{{\rm{PM}}}}\left( {k + 1\left| k \right.} \right)。$

 ${{\mathit{\boldsymbol{\tilde y}}}_{{\rm{cor}}}}\left( {k + 1} \right) = {\mathit{\boldsymbol{y}}_{{\rm{PM}}}}\left( k \right) + \mathit{\boldsymbol{he}}\left( {k + 1} \right),$

3 仿真试验

 图 5 控制框图 Figure 5 The diagram of control system

 图 6 预测曲面和实际曲面的曲线图和误差图 Figure 6 The graph and error figure of predictive surface andactual surface

 图 7 机械臂末端轨迹和实际曲面 Figure 7 The graph of end trajectory and actual surface

 图 8 三个关节角的实际曲面、期望轨迹和跟踪误差 Figure 8 The actual surface、desired trajectory and error figure of three joints

 图 9 机械臂末端力传感器返回值 Figure 9 The return value of end force sensor

4 结论

 [1] 谭福生, 葛景国. 力控制技术在机器人打磨中的应用及系统实现[J]. 上海电气技术, 2008, 1(2): 35-48 TAN Fusheng, GE Jingguo. Research on force-control based robotic machining and its package implementation[J]. Journal of Shanghai Electric Technology, 2008, 1(2): 35-48 [2] 范波涛, 张良. 蒙特卡洛方法在喷浆机器人工作空间分析中的应用[J]. 山东工业大学学报, 1999, 29(2): 146-151 FAN Botao, ZHANG Liang. Application of MONTE C-ARIO method in analysis of the working space of spray robot[J]. Journal of Shandong University of Technology, 1999, 29(2): 146-151 [3] 张秀丽, 谷小旭, 赵洪福. 一种基于串联弹性驱动器的柔顺机械臂设计[J]. 机器人, 2016, 38(4): 385-394 ZHANG Xiuli, GU Xiaoxu, ZHAO Hongfu. Design of a compliant robotic arm based on series elastic actuator[J]. Robot, 2016, 38(4): 385-394 [4] 洪汝渝. 六轴机器人柔顺控制的研究[J]. 机器人, 2000, 22(2): 143-147 HONG Ruyu. A study of the compliance control of six-DOF robots[J]. Robot, 2000, 22(2): 143-147 [5] 李汉初. 一种机器人的自适应控制[J]. 山东工业大学学报, 1990, 20(1): 35-40 LI Hanchu. One adaptive control scheme for robot[J]. Journal of Shandong University of Technology, 1990, 20(1): 35-40 [6] ALBU S A, HADDADIN S, OTT C, et al. The DLR lightweight robot: design and control concepts for robots in human environments[J]. Industrial Robot, 2007, 34(5): 376-385 DOI:10.1108/01439910710774386 [7] BORENSTEIN J, EVERETT H R, FENG L, et al. Mobile robot positioning: sensors and techniques[J]. Journal of Field Robotics, 2015, 14(4): 231-249 [8] YAMAD Y, HIRASAWA Y, HUANG S, et al. Human robot contact in the safe guarding space[J]. AS-ME Transaction on Mechatronics, 1997, 2(1): 230-236 [9] 黄剑斌. 基于阻抗控制的空间机械臂接触控制与轨迹规划技术[J]. 航天器工程, 2013, 4(22): 43-48 HUANG Jianbin. Space manipulator interaction control and trajector generation based on cartesion impedance control[J]. China Academy of Space Technology, 2013, 4(22): 43-48 [10] WILSON M, SPONG M W. Robot modeling and control[J]. Industrial Robot An International Journal, 2006, 17(5): 709-737 [11] 张铁, 胡广, 陈首彦. 基于模糊自整定PID的力控制磨削实验研究[J]. 现代制造工程, 2016, 1(9): 121-125 ZHANG Tie, HU Guang, CHEN Shouyan. Experiment research on force control grinding based on fuzzy-PID[J]. Modern Manufacturing Engineering, 2016, 1(9): 121-125 [12] 曾志文, 卢惠民. 基于模型预测控制的移动机器人轨迹跟踪[J]. 控制工程, 2011, 18(S1): 80-85 ZENG Zhiwen, LU Huimin. Trajectory tracking based on model predictive control for omni-directional mobile robot[J]. Control Engineering of China, 2011, 18(S1): 80-85 [13] 邹涛. 模型预测控制[M]. 北京: 化学工业出版社, 2010: 120-135. [14] 帅鑫, 李艳君, 吴铁军. 一种柔性机械臂末端轨迹跟踪的预测控制算法[J]. 浙江大学学报(工学版), 2010, 44(2): 259-264 SHUAI Xin, LI Yanjun, WU Tiejun. Real time predictive control algorithm for endpoint trajectory tracking of flexible manipulator[J]. Journal of Zhejiang University (Egineering Science), 2010, 44(2): 259-264 [15] 潘昊, 邹涛. 一种多变量快速预测控制算法的原理与仿真研究[J]. 控制与决策, 2014, 38(2): 617-643 PAN Hao, ZOU Tao. The principle and simulation of a fast algorithm of multivariable predictive control[J]. Control and Decision, 2014, 38(2): 617-643 [16] 付庆华, 曹玉强. 模型预测算法在温度控制中的应用[J]. 山东工业大学学报, 1996, 26(Suppl.1): 380-383 FU Qinghua, CAO Yuqiang. The application of model predication algorithm in temperature control[J]. Journal of Shandong University of Technology, 1996, 26(Suppl.1): 380-383 [17] 赵众, 池上康之, 中村政俊. 基于非线性分离模型的预测控制及其应用[J]. 山东大学学报(工学版), 2005, 35(3): 58-62 ZHAO Zhong, IKEGAMI Y, NAKAMURA M. Predictive control based on nonlinear separation model[J]. Journal of Shandong University(Engineering Science), 2005, 35(3): 58-62 [18] 邹涛, 孙浩杰. 一种多变量预测控制的分程控制策略实现方法[J]. 控制与决策, 2017, 32(4): 746-750 ZOU Tao, SUN Haojie. An implementation of split control strategy for multi_variable predictive control[J]. Control and Decision, 2017, 32(4): 746-750 [19] 刘卉, 张广明, 欧阳慧珉. 基于模型预测控制的轨迹跟踪算法研究[J]. 控制工程, 2016, 23(s1): 61-65 LIU Hui, ZHANG Guangming, OUYANG Huimin. Trajectory tracking algorithm based on model predictive control[J]. Control Engineering of China, 2016, 23(s1): 61-65 [20] 李波, 张瑾, 李国栋. 排爆机器人机械臂控制系统设[J]. 机电工程, 2015, 32(8): 1110-1114 LI Bo, ZHANG Jin, LI Guodong. Design of EOD robot manipulator control system[J]. Journal of Mechanical & Electrical Engineering, 2015, 32(8): 1110-1114