UR机器人末端画圆轨迹规划与可控制算法及其程序实现
%UR5机器人D-H参数建模
L1 = Link( 'd', 0.089159, 'a', 0, 'alpha', pi/2 ,'standard' );
L2 = Link( 'd', 0, 'a', -0.42500, 'alpha', 0 ,'standard' );
L3 = Link( 'd', 0, 'a', -0.39225, 'alpha', 0 ,'standard' );
L4 = Link( 'd', 0.10915, 'a', 0, 'alpha', pi/2 ,'standard' );
L5 = Link( 'd', 0.09465, 'a', 0, 'alpha', -pi/2 ,'standard');
L6 = Link( 'd', 0.08230, 'a', 0, 'alpha', 0 ,'standard');
标准的和改进的DH如下图所示：
标准的和改进的DH MATLAB建模如下代码所示：
alpha = [0,pi/2,0,0,-pi/2,pi/2];
a = [0,0,.425,.39225,0,0];
d = [.08916,0,0,.10915,.09456,.0823];
offset = 0;
m = [3.7,8.393,2.275,1.219,1.219,.1879];
L1 = Link( 'd', d(1), 'a', a(1), 'alpha', alpha(1) ,'standard' );
L2 = Link( 'd', d(2), 'a', a(2), 'alpha', alpha(2) ,'standard' );
L3 = Link( 'd', d(3), 'a', a(3), 'alpha', alpha(3) ,'standard' );
L4 = Link( 'd', d(4), 'a', a(4), 'alpha', alpha(4) ,'standard' );
L5 = Link( 'd', d(5), 'a', a(5), 'alpha', alpha(5) ,'standard');
L6 = Link( 'd', d(6), 'a', a(6), 'alpha', alpha(6) ,'standard');
%%改进D-H模型
ML1=Link([0 d(1) a(1) alpha(1) 0 ],'modified');
ML2=Link([0 d(2) a(2) alpha(2) 0 ],'modified');
ML3=Link([0 d(3) a(3) alpha(3) 0 ],'modified');
ML4=Link([0 d(4) a(4) alpha(4) 0 ],'modified');
ML5=Link([0 d(5) a(5) alpha(5) 0 ],'modified');
ML6=Link([0 d(6) a(6) alpha(6) 0 ],'modified');
其中：
theta kinematic: joint angle
d kinematic: link offset
a kinematic: link length
alpha kinematic: link twist
除此之外，MATLAB机器人工具箱还提供了如下机器人参数信息：
jointtype kinematic: 'R' if revolute, 'P' if prismatic
mdh kinematic: 0 if standard D&H, else 1
offset kinematic: joint variable offset
qlim kinematic: joint variable limits [min max]
-
m dynamic: link mass
r dynamic: link COG wrt link coordinate frame 3x1
I dynamic: link inertia matrix, symmetric 3x3, about link COG.
B dynamic: link viscous friction (motor referred)
Tc dynamic: link Coulomb friction
-
G actuator: gear ratio
Jm actuator: motor inertia (motor referred)
UR机器人关节配有两个编码器，如图所示，其中一个是电机编码器，另一个是关节编码器。电机编码器位于更靠近关节输入端的位置，与电机转子相连，用于测量转子当前位置。关节编码器通过中空管与关节输出端相连，用于测量关节输出的角度。
机器人工具箱Matlab_Robotic_Toolbox-10.2 下载链接如下：
https://download.csdn.net/download/weixin_51367832/86095403?spm=1001.2014.3001.5503
UR5e机器人DH运动学及动力学参数如下：
https://blog.csdn.net/weixin_51367832/article/details/122609666
利用如下代码可实现UR机器人可视化操作：
robot_UR5=SerialLink([L1,L2,L3,L4,L5,L6],'name','UR5'); %SerialLink 类函数
结果如下：
robot_UR5.display(); %Link 类函数
robot_UR5.teach(); %可以自由拖动的关节角度
拖动示教，可以实现想要关节角信息和末端位置信息。
%% Set Up Robot Model
%调用importrobot从存储在统一机器人描述格式(URDF)文件中的描述生成一个rigidBodyTree模型。
%将DataFormat和Gravity属性设置为与Simscape一致。Simulink模型从仿真的工作空间访问该机器人模型。
addpath([pwd filesep 'Geometry']);
UR5 = importrobot('sm_universalUR5.urdf')
% 设定机器人的数据形式
UR5.DataFormat = 'column';
% 机器人初始位置的确定
initialConfig=[0 ; 0 ; 0; 0; 0; 0];
initialConfig = pi/180*(initialConfig);
figure;show(UR5,initialConfig)
初始姿态的展示：
各关节的旋转轴如下：
首先肩关节：
上肢大臂：
前臂小臂关节：
腕关节1：
腕关节2：
腕关节3：
换个姿态显示：
initialConfig=[0 ; 0 ; 0; 180; -90; 0];
initialConfig = pi/180*(initialConfig);
figure;show(UR5,initialConfig)
x=acost,y=bsint可以表示x^2/a^2+y^2/b^2=1的椭圆
圆的参数方程：x=a+rcosθ; y=b+rsinθ (θ为参数）
圆的参数方程为：
x=a+r cosθ
y=b+r sinθ
式中：（a，b）为圆心坐标，r为圆半径，θ是半径与x轴的夹角；
在Matlab function 中可编写笛卡尔空间末端圆轨迹：
function [xd, yd] = fcn(r, t, w, x, y)
alpha = t*w;
xd = x + r*cos(alpha);
yd = y + r*sin(alpha);
end
%% 其中，xd，yd为圆心位置，r为圆的半径，w为频率；
接下来可以通过Matlab 机器人工具箱中的Simulink模块进行其次变换矩阵的生成
可封装为：
圆轨迹也可用Simulink进行搭建：
其内部模块：
Matlab function为：
那么，最重要的是编写transl函数：
% Matlab 机器人工具箱中的函数为：
创建一个平移SE(3)矩阵::
％
% T = TRANSL(X, Y, Z)是一个SE(3)齐次变换(4x4)表示
% X Y Z的纯平动。
％
% T = TRANSL(P)是SE(3)齐次变换(4x4)，表示a
P=[X,Y,Z]的%平移。P (Mx3)表示一个序列，T
% (4x4xM)是一个齐次变换序列，使得T(:，:，i)
%对应于P的第i行。
％
提取SE(3)矩阵的平动部分:
％
% P = TRANSL(T)是齐次变换T作为a的平动部分
% 3元素列向量。T (4x4xM)是齐次变换
的平动分量，P (Mx3)的行为
%对应序列中的变换。
％
% [X,Y,Z] = TRANSL(T)是齐次变换的平动部分
% T是三个组成部分。如果T (4x4xM)是齐次变换序列
%则X,Y,Z (1xM)为对应的平动分量变换在序列中。
function [t1,t2,t3] = transl(x, y, z)
if nargin == 1
if ishomog(x)
if ndims(x) == 3
% transl(T) -> P, trajectory case
if nargout == 1
t1 = squeeze(x(1:3,4,:))';
elseif nargout == 3
t1 = squeeze(x(1,4,:))';
t2 = squeeze(x(2,4,:))';
t3 = squeeze(x(3,4,:))';
end
else
% transl(T) -> P
if nargout == 1 || nargout == 0
t1 = x(1:3,4);
elseif nargout == 3
t1 = x(1,4);
t2 = x(2,4);
t3 = x(3,4);
end
end
elseif numel(x) == 3
% transl(P) -> T
t = x(:);
t1 = [eye(3) t(:);
0 0 0 1];
else
% transl(P) -> T, trajectory case
n = size(x,1);
t1 = repmat(eye(4,4), [1 1 n]);
t1(1:3,4,:) = x';
end
elseif nargin == 3
% transl(x,y,z) -> T
t = [x; y; z];
t1 = [ eye(3) t; 0 0 0 1];
else
error('SMTB:transl:badargs', 'should be 1 or 3 arguments');
end
end
% one less function to upload for Cody/LTI assessments
function h = ishomog(tr, rtest)
d = size(tr);
if ndims(tr) >= 2
h = all(d(1:2) == [4 4]);
if h && nargin > 1
h = abs(det(tr(1:3,1:3)) - 1) < eps;
end
else
h = false;
end
end
也可用机器人系统工具箱进行解决齐次变换的问题
进而通过逆运动学将末端齐次变换矩阵得到所期望的关节位置：
利用如下函数可得到机器人实现末端画圆的期望轨迹各关节变化曲线：
function [q,qd,qdd] = fcn(tt, q0, q1)
tmax = 0.1;
q = zeros(6,1);
qd = zeros(6,1);
qdd = zeros(6,1);
coder.extrinsic('jtraj');
t = [0:100]'/100*tmax;
[dq,dqd,dqdd] = jtraj(q0,q1,t);
at = min(tt,tmax);
q = interp1(t,dq,at);
qd = interp1(t,dqd,at);
qdd = interp1(t,dqdd,at);
通过计算力矩控制方法实现动态系统的求解：
计算关节空间轨迹的函数为：
function [qt,qdt,qddt] = jtraj(q0, q1, tv, qd0, qd1)
if length(tv) > 1
tscal = max(tv);
t = tv(:)/tscal;
else
tscal = 1;
t = (0:(tv-1))'/(tv-1); % normalized time from 0 -> 1
end
q0 = q0(:);
q1 = q1(:);
if nargin == 3
qd0 = zeros(size(q0));
qd1 = qd0;
elseif nargin == 5
qd0 = qd0(:);
qd1 = qd1(:);
else
error('incorrect number of arguments')
end
% compute the polynomial coefficients
A = 6*(q1 - q0) - 3*(qd1+qd0)*tscal;
B = -15*(q1 - q0) + (8*qd0 + 7*qd1)*tscal;
C = 10*(q1 - q0) - (6*qd0 + 4*qd1)*tscal;
E = qd0*tscal; % as the t vector has been normalized
F = q0;
tt = [t.^5 t.^4 t.^3 t.^2 t ones(size(t))];
c = [A B C zeros(size(A)) E F]';
qt = tt*c;
% compute optional velocity
if nargout >= 2
c = [ zeros(size(A)) 5*A 4*B 3*C zeros(size(A)) E ]';
qdt = tt*c/tscal;
end
% compute optional acceleration
if nargout == 3
c = [ zeros(size(A)) zeros(size(A)) 20*A 12*B 6*C zeros(size(A))]';
qddt = tt*c/tscal^2;
end
以上重点为圆轨迹规划的一些内容，控制部分可采用简单计算力矩方法进行即可，即逆运动学方法。
参考文献：
【1】https://www.universal-robots.cn/products/ur5-robot/
【2】https://www.universal-robots.cn/technical-files/
【3】https://blog.csdn.net/ooorczgc/article/details/125110656
评论（0）
您还未登录，请登录后发表或查看评论