机器人控制系列教程之动力学建模(2)

接昨天的推文：https://editor.csdn.net/md/?articleId=139991958 ，动力学的求解通常是个相对比较复杂的过程，但现在基本上不用人工来推算求解各种公式和求解过程了，大家只需要知道其中的步骤即可，现代对于动力学问题的求解大致可以分为：数字化建模、仿真软件建模。数字化建模是指通过编程软件，如：Python、MATLAB等，可以快速搭建机器人的数字化模型，也称为动态模型，该方法对于新手不是很友好，前期需要多编程语言有一定基础，但是可以建立较为精确的模型；对于后期设计对应的控制系统也比较友好，模型接口便于调用。仿真软件建模，多体动力学的仿真软件比较多，如：ADAMS、SAMCEF、RecurDyn、SIMPACK（笔者在读研期间曾开发了一套基于Adams的机器人仿真教程可见：，最近在做活动都比较优惠哟！），这类软件得益于良好的GUI交互界面，可以快速的建立动力学模型，只要设置好各个部件的动力学参数也可以搭建出较为精确的动力学模型，但是对于基于该模型的控制系统设计不是很友好，这种软件中的接口较少，往往需要借助其他软件进行，而两个软件的相结合就会出现不可预知的BUG。除此之外，像ANSYS这类软件，可以将机器人的各个构件材料参数更加的精细化，但也带来了计算量巨幅增加，普通电脑计算缓慢，容易出现求解结果不收敛现象，笔者不是很推荐。下面我将会以MATLAB的机器人工具箱Robotics Toolbox为例来搭建机器人动力学模型，Robotics Toolbox安装教程见：

MATLAB工具箱Link类说明

Link类将保存与机器人关节和链接相关的所有信息，如运动学、参数、刚体惯性参数、电机和传动参数。

构造函数

Link # 通用构造函数。
Prismatic #标准型D-H参数构建的平移副连杆
PrismaticMDH #改进D-H参数构建的平移副连杆。
Revolute #标准D-H参数构建的旋转副连杆。
RevoluteMDH #改进D-H参数构建的旋转副连杆

信息/显示方法

display #以可读的形式打印Link参数
dyn #显示Link的动态参数
type #关节类型:'R'或'P

属性

运动学参数

theta     #关节角度。
d         #链接偏移。
a         #连杆长度。
alpha     #连杆扭转角。
Jointtype #运动副类型，转动为R；平移为P。
mdh       #D-H参数类型，标准D-H为0，否则为1。
offset    #关节变量偏移。
qlim      #关节变量限制,[min max].

动力学参数

m  #质量       
r  #连杆重心位置
I  #转动惯量
B  #粘性摩擦系数(马达相关)
Tc #库仑摩擦系数

执行机构参数

G  #传动比
Jm #电机惯性

串联机器人动力学模型

各连杆D-H参数及动力学参数定义如下：

clear,clc,close all;
deg = pi/180;
%% 建立机器人DH参数，初始状态为竖直状态
L1=Link('d',0.16250,'a',0,'alpha',0,'modified',...'m',5.6431, ...'r',[0.0002 0.0002 0.1264], ...'I',[0.1183 -0.0001 0.0001; ...-0.0001 0.1182 0.0001; ...0.0001 0.0001 0.0140], ...'B',1.48e-3,...'Tc',[0.395 -0.435], ...'G',81, ...'Jm',2.2e-4, ...'qlim',[-180 180]*deg);L2=Link('d',0,'a',0,'alpha',pi/2,'offset',pi/2,'modified',...'m',5.0478, ...'r',[0.0062 0.0001 0.1080], ...'I',[0.1183 -0.0001 0.0001; ...-0.0001 0.1182 0.0001; ...0.0001 0.0001 0.0140], ...'B',0.817e-3,...'Tc',[0.126 0.071], ...'G',121, ...'Jm',2.2e-4, ...'qlim',[-145,65]*deg);L3=Link('d',0,'a',0.425,'alpha',0,'modified', ...'m',5.7542, ...'r',[-0.0131 0.0001 0.2402], ...'I',[0.4263 0.0000 -0.0072; ...0.0000 0.4334 0.0001; ...-0.0072 0.0001 0.0191], ...'B',1.38e-3,...'Tc',[0.132 -0.105], ...'G',81, ...'Jm',2.2e-4, ...'qlim',[-65,220]*deg);L4=Link('d',0.12670+0.0066,'a',0.39225,'alpha',0,'offset',pi/2,'modified', ...'m',3.0870, ...'r',[-0.0850 0.0003 0.1540], ...'I',[0.0821 0.0000 -0.0314; ...0.0000 0.1257 0.0001; ...-0.0314 0.0001 0.1540], ...'B',71.2e-6,...'Tc',[11.2e-3 -16.9e-3], ...'G',81, ...'Jm',2.2e-4, ...'qlim',[-180,180]*deg);L5=Link('d',0.09970,'a',0,'alpha',pi/2,'modified', ...'m',2.0459, ...'r',[0.0001,0.0002,0.0982], ...'I',[0.0235,0.0000,-0.0002; ...0.0000,0.0253,0.0000; ...-0.0002,0.0000,0.0045], ...'B',82.6e-3,...'Tc',[9.26e-3, -14.5e-3], ...'G',81, ...'Jm',2.2e-4, ...'qlim',[-135,135]*deg);L6=Link('d',0.09960-0.0045,'a',0,'alpha',-pi/2,'modified', ...'m',2.6317, ...'r',[-0.0111,-0.0003,0.1366], ...'I',[0.0684,0.0000,0.0001; ...0.0000,0.0696,-0.0001; ...0.0001,-0.0001,0.0047], ...'B',36.7e-6,...'Tc',[3.96e-3, -10.5e-3], ...'G',51, ...'Jm',2.2e-4, ...'qlim',[-180,180]*deg);robot=SerialLink([L1 L2 L3 L4 L5 L6],'name','TechM');

模拟机器人关节空间从初始状态运动到[pi/2 -pi/2 pi/6 pi/12 0 0]，相关关节空间的轨迹规划请见：

qz = [0 0 0 0 0 0];    %初始位姿
qn = [pi/2 -pi/2 pi/6 pi/12 0 0];    %末端位姿
% 轨迹规划
t=[0:0.01:2];
g=jtraj(qz,qn,t);
[q,qd,qdd]=jtraj(qz,qn,t);
plot(robot,q,'trail','b-')    %图形演示figure
i=1:6;
subplot(2,2,1);
qplot(q(:,i));grid on;title('位置');ylabel('角度(rad)');%绘制每个关节位置
subplot(2,2,2);
qplot(qd(:,i));grid on;title('速度');ylabel('角速度(rad/s)');%绘制每个关节速度
subplot(2,2,3);
qplot(qdd(:,i));grid on;title('加速度');ylabel('角加速度(rad/s2)');%绘制每个关节加速度
Q = robot.rne(q,qd,qdd);%获得每个时间点所需要的关节力矩
subplot(2,2,4)
qplot(t,Q);grid on;title('力矩');ylabel('力矩(N*m)');

规划动画

各个关节的角度、速度、角加速度的变化情况如下：

Link其他常见使用方法

显示Link运动学参数

robot.links(1).dyn

显示Link动力学参数

robot.display()

Link的变换矩阵

robot.links(1).A(pi/6)

链路变换矩阵T = L.A(Q)是一个SE3对象，表示当链路变量Q为Denavit-Hartenberg参数THETA(转动)或D(移动)时链路帧之间的变换。

• 标准D-H参数，从前序坐标系转换到当前坐标系。
• 改进D-H参数，从当前坐标系到后序坐标系。

往期推荐