![cover](https://img-blog.csdnimg.cn/3e68d48673e94fa3b284bf9a4e102ef8.png)
基于Simulink、Simscape、S-Function联合的机械臂仿真
多数论文中机械臂仿真一般只针对理想的二连杆模型。对于一个真实的机械臂的仿真流程中文互联网上教程还比较少。我尝试做一个。欢迎批评指正讨论。工业中一种常用的机械臂仿真流程可以概括为下图首先由solidworks建立机器人的外观,各连杆坐标系以及物理参数。然后使用插件sw_urdf_exporter自动导出urdf文件。在ros工作空间下功能包gluon的urdf文件夹中有作者生成的urdf文件(作者用
目录
C-1 Robotics system toolbox 获取机械臂动力学参数
C-0 概述与前期准备
多数论文中机械臂仿真一般只针对理想的二连杆模型。对于一个真实的机械臂的仿真流程中文互联网上教程还比较少。我尝试做一个。欢迎批评指正讨论。
工业中一种常用的机械臂仿真流程可以概括为下图
首先由solidworks建立机器人的外观,各连杆坐标系以及物理参数。然后使用插件sw_urdf_exporter自动导出urdf文件。将urdf文件导入matlab的robotics system toolbox可以调用工具箱算法计算机械臂的惯性矩阵,科氏力矩,重力矩阵等参数。将urdf文件导入simulink中simscape模块可以在simulink中生成机器人对象。最后编写S-Function,调用robotics system toolbox计算的参数矩阵和simscape机械臂对象返回的传感器参数实现控制算法。仿真完成后,simscape还将生成一段仿真视频。
需要提前做的准备
1. urdf
2. STL 文件
作者的机械臂是GLUON-2L6-4L3。使用的控制算法是滑模控制。滑模控制器需要对象的动力学参数,这也是使用robotics system toolbo的原因。如果控制器无需对象的信息就可以省略。
C-1 Robotics system toolbox 获取机械臂动力学参数
P-1 官方文档
Robotics System Toolbox Documentation - MathWorks 中国
P-2 使用流程
这里给出一些工具箱的使用案例。可以跳过。
clear,clc,close all
% gluon机器人在Robotics system toolbox中的例程
% 其中可见各种控制的基本量
% 值得注意的是。该工具箱只能计算科氏力矩C(q,qdot)*qdot。
% 导入机器人
robot = importrobot('record.urdf');
currentRobotJConfig = homeConfiguration(robot) % 获取当前关节配置
numJoints = numel(currentRobotJConfig) % 获取机器人自由度
% 关节角
q = [0 -pi/2 0 -pi/2 pi/2 0];
dq = zeros(1,6);
ddq = zeros(1,6);
% 生成随机位姿配置
configuration = randomConfiguration(robot);
for i = 1:6
configuration(i).JointPosition = q(i);
end
robot.show(configuration)
robot.DataFormat='row' %row为行输出,column为列输出
robot.Gravity = [0 0 -9.81]
%% 运动学
% 正运动学
T02 = getTransform(robot,q,'2_Link')
T23 = getTransform(robot,q,'3_Link','2_Link') % 参数分别为robot,targetframe,sourceframe
T32 = getTransform(robot,q,'3_Link','2_Link')
T07 = getTransform(robot,q,'6_Link')
% 逆运动学
ik = inverseKinematics('RigidBodyTree',robot);
weights = [0.25 0.25 0.25 1 1 1]; % 权重矩阵,前三项为姿态权重,后三项为位置权重
randConfig = robot.randomConfiguration;
tform = getTransform(robot,randConfig,'6_Link','base_link');
[configSoln,solnInfo] = ik('6_Link',tform,weights,[0.5 0.5 0.5 0 0 0]);
show(robot,configSoln)
% 定义外力
wrench = [1,0,0,0,0,0];
fext = externalForce(robot,'6_Link',wrench) % 外力被定义在base系内
fext = externalForce(robot,'6_Link',wrench,q) % 外力被定义在link6系内
% 计算雅克比
[com, comJac] = centerOfMass(robot, q) % 计算质心及质心雅克比
jacobian = geometricJacobian(robot,q,'6_Link')
%% 动力学
% 计算逆动力学
torque = inverseDynamics(robot,q,dq,ddq)
torque = inverseDynamics(robot,q,dq,ddq,fext)
% 计算正动力学
ddq = forwardDynamics(robot,q,dq,torque)
ddq = forwardDynamics(robot,q,dq,torque,fext)
% 获取动力学单项
M = massMatrix(robot,q)
% 这个Cdq官方文档的意思是抵消速度惯性对关节的影响所需的力矩,既C(q,qdot)*qdot。如果是求速度
% 惯性对关节的影响本身就要加负号。
Cdq = velocityProduct(robot,q,dq)
G = gravityTorque(robot,q)
%% 获取基本动力学信息
% 获取各刚体
L{1} = robot.getBody('1_Link');
L{2} = robot.getBody('2_Link');
L{3} = robot.getBody('3_Link');
L{4} = robot.getBody('4_Link');
L{5} = robot.getBody('5_Link');
L{6} = robot.getBody('6_Link');
% 读取质量
M1 = L{1}.Mass;
M2 = L{2}.Mass;
M3 = L{3}.Mass;
M4 = L{4}.Mass;
M5 = L{5}.Mass;
M6 = L{6}.Mass;
% 读取质心
c{1} = L{1}.CenterOfMass;
c{2} = L{2}.CenterOfMass;
c{3} = L{3}.CenterOfMass;
c{4} = L{4}.CenterOfMass;
c{5} = L{5}.CenterOfMass;
c{6} = L{6}.CenterOfMass;
% 读取惯量 [Ixx Iyy Izz Iyz Ixz Ixy]
Inertia = L{2}.Inertia;
I = [Inertia(1),Inertia(6),Inertia(5);
Inertia(6),Inertia(2),Inertia(4);
Inertia(5),Inertia(4),Inertia(3)]
C-2 Simscape 创建机械臂对象
P-1官方文档
Simscape Documentation - MathWorks 中国
P-2 使用流程
matlab命令行中输入如下
smimport('record.urdf'); %其中'record.urdf'是你的urdf
接着会自动在simulink中生成如下模型:
图1 机械臂对象
其中axis_joint_1一直到到axis_joint_6就是urdf中定义的连杆连接。双击其中一个打开可以看到如下:
图2 关节设置
可见其中已经忠诚的反映了urdf中定义的限制,摩擦等要素。
观察图1,其中base_link,x1_Link一直到到x6_Link就是urdf中定义的连杆。双击其中一个打开可以看到如下:
图3 连杆设置
这个子模块的输入输出F和F1都是坐标系。双击图3.3中的Visual模块,可以看到如下:
图44 Visual
你可能已经注意到有报错,是因为Geometry/File Name中填写的路径位置没有这个连杆的“皮肤”,即描述连杆3D外貌的STL文件。STL文件时solidworks生成的。你需要把所有的STL文件放到matlab文件夹中,并修改Geometry/File Name中填写的路径与之对应。这一步不可省略。
图5 添加STL文件
打开图3中Inertia模块
图6 Inertia模块
可见其中已经忠诚的反映了urdf中定义的连杆惯性矩阵,质心坐标和质量等信息。本实验采用滑模控制,控制量时力矩,因此我们需要向各个关节发送力矩指令,并且获取关节的角度以及角速度信息。为了达到这个目的,我们做如下操作:
图7 关节设置
如图7所示。将Z Revolute Primitive/Actuation/Torque改为Provided by Input。将Z Revolute Primitive/Actuation/Motion改为Automatically Computed。即力矩由用户输入提供,运动量自动计算。将Z Revolute Primitive/Sensing/Position和Z Revolute Primitive/Sensing/Velocity勾选。这意味着关节将会输出自己的角度和角速度信息。
图8 关节设置效果
保存退出后,将发现关节多出了力矩输入端‘t’,角度输出端‘q’,角速度输出端‘w’。下一步自然的想法是利用simulink接受或者向他发出数据。但是simscape的数据都是物理量,和simulink交流之前必须做模拟量和数字量之间的转换。
图9 上图物理量转数字量,下图数字量转物理量
因此对图8所示关节进一步做处理如下:
图10 关节设置完成
加装了信号输入端link_torque,信号输出端link1_q和link1_qdot,这样做是便于子系统封装时接口名规范。对六个关节都做如上的设置(STL文件设置,输入输出设置,信号端口加装),然后创建子系统,将整个机械臂对象封装如下:
图11 机械臂对象封装
显而易见,图11中机械臂对象左侧输入力矩,右侧输出各关节的角度和角速度信息。
C-3 S-Function 实现控制器
P-1 S-Function基本使用方法
见视频
S-Function/S函数,一个实例带你直接上手(非线性反步法搭建)_哔哩哔哩_bilibili
P-2 控制器编写
这一步就是你自己编写的控制器。该控制器接受机械臂对象传回的角度和角速度,输出控制力矩。作者使用的是自适应非奇异终端滑模控制器,控制律如下:
其中
function [sys,x0,str,ts,simStateCompliance] = sfuntmpl(t,x,u,flag,gluon)
switch flag,
case 0,
[sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes;
case 1,
sys=mdlDerivatives(t,x,u,gluon);
case 2,
sys=mdlUpdate(t,x,u);
case 3,
sys=mdlOutputs(t,x,u,gluon);
case 4,
sys=mdlGetTimeOfNextVarHit(t,x,u);
case 9,
sys=mdlTerminate(t,x,u);
otherwise
DAStudio.error('Simulink:blocks:unhandledFlag', num2str(flag));
end
function [sys,x0,str,ts,simStateCompliance]=mdlInitializeSizes
sizes = simsizes;
sizes.NumContStates = 3; % 三个自适应参数
sizes.NumDiscStates = 0;
sizes.NumOutputs = 15; % 六个输出力矩,三个自适应参数,六个滑膜量;
sizes.NumInputs = 30; % 六个期望角度,六个期望角速度,六个期望角加速度;
% 六个实际角度,六个实际角速度
%输入顺序如下
%{
期望角度,期望角速度,期望角加速度;重复6次。共18个输入
实际角度,实际角速度。重复6次。共12个输入。
%}
sizes.DirFeedthrough = 1;
sizes.NumSampleTimes = 1; % at least one sample time is needed
sys = simsizes(sizes);
x0 = [0, 0, 0];
str = [];
ts = [0 0];
simStateCompliance = 'UnknownSimState';
% end mdlInitializeSizes
function sys=mdlDerivatives(t,x,u,gluon)
qd_Link1 = u(1); qd_dot_Link1 = u(2); qd_ddot_Link1 = u(3);
qd_Link2 = u(4); qd_dot_Link2 = u(5); qd_ddot_Link2 = u(6);
qd_Link3 = u(7); qd_dot_Link3 = u(8); qd_ddot_Link3 = u(9);
qd_Link4 = u(10);qd_dot_Link4 = u(11);qd_ddot_Link4 = u(12);
qd_Link5 = u(13);qd_dot_Link5 = u(14);qd_ddot_Link5 = u(15);
qd_Link6 = u(16);qd_dot_Link6 = u(17);qd_ddot_Link6 = u(18);
qd = [qd_Link1;qd_Link2;qd_Link3;qd_Link4;qd_Link5;qd_Link6];
qd_dot = [qd_dot_Link1;qd_dot_Link2;qd_dot_Link3;qd_dot_Link4;qd_dot_Link5;qd_dot_Link6];
q_Link1 = u(19);q_dot_Link1 = u(20);
q_Link2 = u(21);q_dot_Link2 = u(22);
q_Link3 = u(23);q_dot_Link3 = u(24);
q_Link4 = u(25);q_dot_Link4 = u(26);
q_Link5 = u(27);q_dot_Link5 = u(28);
q_Link6 = u(29);q_dot_Link6 = u(30);
q = [q_Link1;q_Link2;q_Link3;q_Link4;q_Link5;q_Link6];
q_dot = [q_dot_Link1;q_dot_Link2;q_dot_Link3;q_dot_Link4;q_dot_Link5;q_dot_Link6];
EPS = gluon.param.EPS;
e = q-qd; % 位置误差。6x1向量。
e_dot = q_dot - qd_dot; % 角速度误差
s = e_dot + gluon.param.alpha * e + gluon.param.beta * norm(e) ^ (gluon.param.gamma_1/gluon.param.gamma_2)*sign(e);
% 自适应律
if norm(s) >= gluon.param.EPS
sys(1) = gluon.param.GAMMA_1 * norm(s);
else
sys(1) = 0;
end
if norm(s) >= gluon.param.EPS
sys(2) = gluon.param.GAMMA_2 * norm(s) * norm(q);
else
sys(2) = 0;
end
if norm(s) >= gluon.param.EPS
sys(3) = gluon.param.GAMMA_3 * norm(s) * norm(qd)^2;
else
sys(3) = 0;
end
% end mdlDerivatives
function sys=mdlUpdate(t,x,u)
sys = [];
% end mdlUpdate
function sys=mdlOutputs(t,x,u,gluon)
qd_Link1 = u(1); qd_dot_Link1 = u(2); qd_ddot_Link1 = u(3);
qd_Link2 = u(4); qd_dot_Link2 = u(5); qd_ddot_Link2 = u(6);
qd_Link3 = u(7); qd_dot_Link3 = u(8); qd_ddot_Link3 = u(9);
qd_Link4 = u(10);qd_dot_Link4 = u(11);qd_ddot_Link4 = u(12);
qd_Link5 = u(13);qd_dot_Link5 = u(14);qd_ddot_Link5 = u(15);
qd_Link6 = u(16);qd_dot_Link6 = u(17);qd_ddot_Link6 = u(18);
qd = [qd_Link1;qd_Link2;qd_Link3;qd_Link4;qd_Link5;qd_Link6];
qd_dot = [qd_dot_Link1;qd_dot_Link2;qd_dot_Link3;qd_dot_Link4;qd_dot_Link5;qd_dot_Link6];
qd_ddot = [qd_ddot_Link1;qd_ddot_Link2;qd_ddot_Link3;qd_ddot_Link4;qd_ddot_Link5;qd_ddot_Link6];
q_Link1 = u(19);q_dot_Link1 = u(20);
q_Link2 = u(21);q_dot_Link2 = u(22);
q_Link3 = u(23);q_dot_Link3 = u(24);
q_Link4 = u(25);q_dot_Link4 = u(26);
q_Link5 = u(27);q_dot_Link5 = u(28);
q_Link6 = u(29);q_dot_Link6 = u(30);
q = [q_Link1;q_Link2;q_Link3;q_Link4;q_Link5;q_Link6];
q_dot = [q_dot_Link1;q_dot_Link2;q_dot_Link3;q_dot_Link4;q_dot_Link5;q_dot_Link6];
e = q -qd;
e_dot = q_dot - qd_dot;
M0 = massMatrix(gluon.robot, q.');
C0q_dot = -velocityProduct(gluon.robot,q.',q_dot.').'; %列向量
G0 = gravityTorque(gluon.robot, q.').'; %列向量
s = e_dot + gluon.param.alpha * e + gluon.param.beta * norm(e) ^ (gluon.param.gamma_1/gluon.param.gamma_2)*sign(e);
if (norm(s/gluon.param.fai) >= 1)
sat = tanh(s/gluon.param.fai);
else
sat = s/gluon.param.fai;
end
ctrl = M0 * qd_ddot + C0q_dot + G0 ...
- M0 * (gluon.param.alpha*e_dot + gluon.param.beta*gluon.param.gamma_1/gluon.param.gamma_2 * norm(e) ^ (gluon.param.gamma_1/gluon.param.gamma_2 - 1)*e_dot)...
- M0 * (gluon.param.k_N * s + (x(1) + x(2)*norm(q) + x(3)*norm(q_dot)^2)*sign(s));
sys(1:6) = ctrl;
sys(7:9) = x;
sys(10:15) = s;
% end mdlOutputs
function sys=mdlGetTimeOfNextVarHit(t,x,u)
sampleTime = 1; % Example, set the next hit to be one second later.
sys = t + sampleTime;
function sys=mdlTerminate(t,x,u)
sys = [];
% end mdlTerminate
变量gluon是提前设置的控制器参数。
C-4 联合仿真
处理好前三步后,作者搭建了下图所示的控制系统。
其中gluon_ctrl是编写的控制器,gluon_robot是机械臂对象。右侧qd是期望角度、角速度和角加速度。disturb是扰动。设置仿真器为固定步长,开始仿真。仿真结果如下
图12 关节较多,只展示第一个
此外还生成了视频
![Logo](https://devpress.csdnimg.cn/79de2bf0b7994defa4242ef90d5513fa.jpg)
开放原子开发者工作坊旨在鼓励更多人参与开源活动,与志同道合的开发者们相互交流开发经验、分享开发心得、获取前沿技术趋势。工作坊有多种形式的开发者活动,如meetup、训练营等,主打技术交流,干货满满,真诚地邀请各位开发者共同参与!
更多推荐
所有评论(0)