多输入输出 LSTM pytorch 多输入输出滑膜控制

目录

C-0 概述与前期准备

C-1 Robotics system toolbox 获取机械臂动力学参数

P-1 官方文档

P-2 使用流程

C-2  Simscape 创建机械臂对象

P-1官方文档

P-2 使用流程

C-3 S-Function 实现控制器

P-1  S-Function基本使用方法

P-2  控制器编写

C-4 联合仿真

---

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 关节较多，只展示第一个

此外还生成了视频