基于阻抗控制的工业机器人轨迹跟踪系统 Simulink/Simscape 仿真

本文是对文献 [1] 的复现。

更新日志
关于阻抗控制
离散化阻抗控制控制器的实现
基于阻抗控制的工业机器人轨迹跟踪系统仿真
后记
项目开源
参考文献
Q&A

更新日志

Github 仓库
- 2021/02/03
  - 第一次上传
- 2021/04/18
  - 修改贴图路径为相对路径
  - 在 Model Properties 中手动添加模型装配数据脚本回调函数，解决中文路径问题
  - 添加 2018a - 2020b 版本仿真模型文件
  - 添加参考文献文件
文章本身
- 2021/04/28
  - 读者 Q&A 更新

关于阻抗控制

阻抗控制的提出可以追溯到 1984 年，Neville Hogan 在 American Control Conference 发表的 Impedance Control：An Approach to Manipulation ，次年，Hogan N. 又在 Journal of Dynamic Systems Measurement and Control-transactions of The Asme 发表了阻抗控制经典三部曲 —— Impedance Control：An Approach to Manipulation, Part I --Theory, Part II -- Implementation, Part III -- Applications。

_{图为 2021/01/21 时博主做的组会 Presentation}

博主理解的阻抗控制，实际其控制的目标是调节机器人的末端位置（笛卡尔坐标空间）和末端作用力之间的动态关系，以保证机器人在适当的柔顺运动过程中进行轨迹跟踪，至于是更加柔顺还是更注重轨迹跟踪精度则是由阻抗控制模型（算法）式（1）进行调节的。为了更好的理解阻抗模型各项的作用，可以对该阻抗模型进行简化，例如 \(f_e =k(x_d-x)\)，这个时候说明我们只考虑了控制机器人末端与环境之间的位置关系（由胡克定律，此时两者之间就像只有一个弹簧连接一样）；同样地，我们让 \(f_e = b(\dot{x}_d -\dot{x}) + k(x_d-x)\)，这个时候就同时考虑了控制机器人末端与环境之间的位置关系和速度关系（此时两者之间应该是一个弹簧-阻尼系统）；进一步同时考虑考虑位移-速度-加速度时(即弹簧-阻尼-质量系统)，则得到了 \(f_e = m(\ddot{x}_d -\ddot{x}) + b(\dot{x}_d -\dot{x}) + k(x_d-x)\) 这样完整的阻抗控制模型了。
当然，在这里所做的简单陈述只是为了更好地理解阻抗控制的核心思想，其内容远不止博主上面所说的这些（自己其实也学得稀里糊涂的），谨记。

离散化阻抗控制控制器的实现

这里使用了简化的阻抗控制模型，即要求机器人末端执行器在 X/Y/Z 三个方向的运动是解耦的，这样我们便可以对三个方向分别进行控制，也便于我们调节阻抗参数。
对于阻抗模型 \(f_e = m(\ddot{x}_d -\ddot{x}) + b(\dot{x}_d -\dot{x}) + k(x_d-x)\)，我们可以采用下面的方式进行离散化：

利用该离散化表达式，借助 Simulink 中的 MATLAB Function 模块，编辑 m 脚本便可以实现离散化的阻抗控制控制器。

_{离散化阻抗控制器}

function [Xr,Xr_dot,Xr_dotdot]  = fcnlmp(Xd,Xd_dot,Xd_dotdot,f)  
	m =1;b = 200;k=1200;T = 0.005; % 参数初始化  
	persistent Xr_pre Xr_dot_pre;% 持久变量 https://ww2.mathworks.cn/help/matlab/ref/persistent.html  
	if isempty(Xr_pre)   
	   Xr_pre = 0;  
	end  
	if isempty(Xr_dot_pre)   
	   Xr_dot_pre = 0;  
	end  
	Xr_dotdot_pre = Xd_dotdot + 1/m*(-f + b*(Xd_dot - Xr_dot_pre) + k*(Xd - Xr_pre)); % 上一采样时间 nT 机器人末端的实际运动加速度  
	Xr = Xr_pre + Xr_dot_pre*T;           % 当前采样时间 (n+1)T 机器人末端的实际运动位置  
	Xr_dot = Xr_dot_pre + Xr_dotdot_pre*T;% 当前采样时间 (n+1)T 机器人末端的实际运动速度  
	Xr_dotdot = Xr_dotdot_pre;            % 当前采样时间 (n+1)T 机器人末端的实际运动加速度  
	Xr_dot_pre = Xr_dot;                  % 上一采样时间 nT 机器人末端的实际运动速度  
	Xr_pre = Xr;                          % 上一采样时间 nT 机器人末端的实际运动位置  
end

_{图为 2021/01/21 时博主做的组会 Presentation}

基于阻抗控制的工业机器人轨迹跟踪系统仿真

本项目已开源，【仓库地址】： ER-16-Simulation 。

■ 环境说明

参考文献 [1]，对埃夫特公司的 ER-16 系列六轴机器人进行 Simulink/Simscape 仿真，环境说明如下：

类别	下载地址
ER16 系列机器人 3D-Model	埃夫特智能装备股份有限公司
Matlab 2020b （版本 9.9）	MathWorks 官网
Simulink （版本 10.2）	Matlab 菜单栏 -> 附加功能 -> 搜索安装即可
Simscape（版本 5.0）	Matlab 菜单栏 -> 附加功能 -> 搜索安装即可
Simscape Multibody（版本 7.2 ）	Matlab 菜单栏 -> 附加功能 -> 搜索安装即可
Simscape Multibody Link （版本 7.2）	参考文章《Matlab模型可视化...》安装即可

■ 系统说明

完整的轨迹跟踪系统模型模型如下，其中涉及了机器人本体模型的搭建，离散化阻抗控制器的实现，以及末端轨迹生成器的设计。

机器人本体模型的搭建 是从 Solidworks 导出 xml 文件，再在 Matlab 中用 SimMechanics Link 读取该 xml 文件从而获取装配信息得到的，相比于手动装配确实省事了不少。SimMechanics Link 的使用可以参考文章 Matlab模型可视化仿真:SimMechanics Link的安装与使用。机器人模型子系统呈现如下：

离散化阻抗控制器 是直接利用了之前的成果，将其拓展到 X/Y/Z 三个维度即可。
对于机器人的运动控制，一个很棘手的问题就是逆运动学求解，这里的处理方式是在机器人末端执行器的顶端额外添加一个关节（Bushing Joint 模块），将 X/Y/Z 三个方向的位置信息作为输入，由该关节 “牵” 着这个庞大的六轴机器人运动（有牵着鼻子走内味了嗷