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

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

目录

更新日志

  • 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 三个方向的位置信息作为输入，由该关节 “牵” 着这个庞大的六轴机器人运动（有牵着鼻子走内味了嗷🤭），至于六个轴该怎么动就交给拥有强大的计算能力的 Matlab 自己去算吧！这样放养式的逆运动学求解最大的毛病就是多解的问题，再加上 Simscape 这个工具貌似没有碰撞检测，所以不加以限制机器人运动起来就会变成 “奇形种” hhhhh。博主是将第五关节模块）的运动设置成了必须精准计算（Accurate）情况才有所好转的！
  阻抗控制器 + 运动控制子系统 实现如下：

  末端轨迹生成器 需要生成 X/Y/Z 三个方向的位置提供给阻抗控制器，根据期望轨迹的不同，生成方式也有很多，这里只进行了圆周和直线轨迹的模拟，所以实现方式就比较简单，利用极坐标和插值就可以了。

function Traj = traj(Mode,Clock,Stoptime)  
	if Mode == 0 % 圆周轨迹  
	    theta = 2*pi*Clock/Stoptime;  
	    Xd = 0.2*cos(theta)-0.2;  
		Yd = 0;  
		Zd = 0.2*sin(theta);  
	    Traj = [Xd;Yd;Zd];  
	else        % 直线轨迹  
	    Xd = 0;  
	    Yd = 0;  
	    Zd = 0.2*Clock/Stoptime;  
	    Traj = [Xd;Yd;Zd];  
	end
end

■ 结果展示

  给定参考轨迹（包括圆周轨迹和直线轨迹两类），在机器人运动过程中施加外力干扰（借助信号发生器实现），机器人在进行轨迹跟踪的同时做出主动柔顺运动。

后记

  作为本科阶段最后一门专业课程，《机器人动力学与控制》这门课也没逃过感觉也没学到啥的命运，脑子里倒是清晰地记得老师反复念叨的那句 —— “别问我为什么要这么干，人家就是这样搞的，记公式就完事了！” 像极了期末抱佛脚的自己🤭
  经过四年的磨练，越发觉得一门课程学得好不好还是得看自己是不是真的能拿出来用了 —— 微机那些 8086 什么的没学会，实验学的汇编倒是用得挺溜；学完数电甚至能在 Minecraft 里面像模像样的玩起 “数电科技”😄 多亏了这些相关的实验和平时做的项目，自己才掌握了不少本领，也帮助我在和研究生导师面谈的时候有话可聊（有逼可吹）🌹 希望看到这篇文章的外校内校的学弟学妹们也能认真对待自己的实验~🐕

项目开源

【仓库地址】： https://github.com/Beta-y/ER-16-Simulation

参考文献

【1】陈明,甘亚辉,戴先中,等.基于阻抗控制的工业机器人轨迹跟踪系统设计[C].//中国人工智能学会.第十二届中国智能机器人大会论文集.2017:324-331.
【2】Hogan N. Impedance control - An approach to manipulation. I- Theory. II - Implementation. III - Applications[J]. AsmeTransactions Journal of Dynamic Systems & Measurement Control B, 1984, 107(1):304-313.
【3】李正义.机器人与环境间力/位置控制技术研究与应用[D].湖北:华中科技大学,2011. DOI:10.7666/d.d186146.
【4】杨振.基于阻抗控制的机器人柔顺性控制方法研究[D].江苏:东南大学,2005. DOI:10.7666/d.y943306.

Q&A

  1. 读者 “杜楠楠” 邮箱留言 2021年4月22日（星期四）上午10 : 19

" 同学，您好！我也开始在做阻抗控制方面的研究，最近在CSDN上看到了您的文章“基于阻抗控制的工业机器人轨迹跟踪系统 Simulink/Simscape 仿真”，并成功复现了您开源的代码。这里有几个问题想请教您。

1、在这篇文章中是不是没有针对阻抗环境修正后的位置xr的伺服控制（实际位置x跟踪xr，利用PID之类的），因为在plot文件中直接输出的就是xr的信号。
2、CAD软件导入Simulink的xml文件是不包含模型的物理信息（质量、质心、惯性矩等）吗？因为我看知乎中“Matlab模型可视化仿真:SimMechanics Link的安装与使用”这篇文章是这么说的。另外看您的datafile文件中通过代码的形式定义了各个部件的物理信息。
3、下一个问题就是，既然这个仿真模型中没有基于动力学模型，那为什么还要定义模型的物理信息呢？
4、另外，关于bushing joint环节，哪个输出是逆运动学解出来的关节位置变量呢？这部分看得比较晕。

在阻抗控制这方面，前期一直在看论文，代码能力不强，因此一直苦于仿真的实现。所以十分感谢您的共享，获益匪浅。期待您的回复。再次感谢。"

1.博主回复 2021年4月22日（星期四）上午10 : 55

“ 同学你好，很高兴收到你的来信，你提的几个问题可以说是问到了点子上，那我就我简单对它们解释一下吧。

问题1. 修正后的 Xr 的位置实际上就是机器人末端的实际位置 X，之所以没有加额外的伺服控制是因为这里直接借助了 Simscape 的输入期望关节的 Motion 自动解算其控制力矩 Force 的功能，这个你可以双击 BushingJoint 在 X/Y/Z Prismatic Primitive->Acutation 中找到；
问题2. xml 文件可以理解成整个机器人系统的装配信息，其中包含了各个零件之间的配合关系，当然也包含了它们的物理信息，你也可以用任意的文本编辑器 (比如记事本) 打开这个文件找到这些信息。至于 Datafile 文件的作用是将 xml 文件翻译成 Matlab 可读的 m 语言形式供 Simulink 仿真模型使用，其中也包含了装配信息和物理信息；
问题3. 这里是一个资源选取的问题，SolidWorks 导出 xml 文件时默认是将所有信息一并导出的，而我们确实只用到了装配信息但是实际确实没有使用到物理信息，你甚至可以将各个 Solid 模块的质量、密度设置成任意值，这都不会影响到我们的控制；
问题4.这个问题我在博客中用了一个很接地气的方式表述了一下 —— “牵着鼻子走”；之所以这样形容是因为确实我们没有做各个关节的逆运动学工作，而是依靠 Simscape 的自动求解功能完成的，这一点与问题1类似，你可以在机械臂各个关节的同一属性 Acutation 中找到 Torque = None, Motion = Automatically caculated 之类的设置。实际上 Bushing Joint 是我们设置的一个虚拟关节用于表征末端执行器的位置，实际上并不存在这一对象，我们所做的只是告知末端执行器应该往哪里走（Xr），然后借助 Torque -> Automatically caculated 让其到达指定位置，而其余几个臂的运动是由 Simscape 根据末端执行器的实际位置进行解算的。

以上是所有问题的回复，希望对你有帮助。

posted @ 2021-01-31 12:13  贝_塔  阅读(4061)  评论(2)  编辑  收藏  举报
👈 👉 👇 👆