众所周知,在机械臂建模中,rvc工具箱是非常好用,其中有各种函数可以调用,rne函数是求解逆动力学的函数,但是有以下缺点
1、不能输出底座力
2、外力加载不在tool部分
不废话直接上代码分析部分
为了保证原本的还能用,新建两个函数
首先是[Q1,Q2]=rnene(robot,x1,g,w);%切换成新的rnene函数
rnene函数关键部分如下
% use the M-file implementation
% if robot.mdh == 0
% [varargout{1:nargout}] = rne_dh(robot, args{:});
% else
[varargout{1:nargout}] = rnene_mdh(robot, args{:});
因为我用的是改进的,所以我替换了rne_mdh变为rnene_mdh
进入rnene_mdh(robot, args{:});这个函数
function [tau,Base] = rnene_mdh(robot, a1, a2, a3, a4, a5)
z0 = [0;0;1];
grav = robot.gravity; % default gravity from the object
fext = zeros(6, 1); %初始化
% Set debug to:
% 0 no messages
% 1 display results of forward and backward recursions
% 2 display print R and p*
debug = 0;
n = robot.n;
%对两种输出形式进行了分类,统一填写进了Q Qd Qdd之中
if numcols(a1) == 3*n
Q = a1(:,1:n);
Qd = a1(:,n+1:2*n);
Qdd = a1(:,2*n+1:3*n);
np = numrows(Q);%提取行数
if nargin >= 3, %对输入函数进行分类
grav = a2(:);
end
if nargin == 4
fext = a3;
end
else
np = numrows(a1);
Q = a1;
Qd = a2;
Qdd = a3;
if numcols(a1) ~= n || numcols(Qd) ~= n || numcols(Qdd) ~= n || ...
numrows(Qd) ~= np || numrows(Qdd) ~= np
error('bad data');
end
if nargin >= 5,
grav = a4(:);
end
if nargin == 6
fext = a5;
end
end
%这一段改变了输出的数目
Base=zeros(np,6);
if robot.issym || any([isa(Q,'sym'), isa(Qd,'sym'), isa(Qdd,'sym')])%sym
tau = zeros(np,n, 'sym');%
else
tau = zeros(np,n);
end
for p=1:np %循环计算每一个step的输出
q = Q(p,:).';
qd = Qd(p,:).';
qdd = Qdd(p,:).';%转换列向量
Fm = [];
Nm = [];
pstarm = [];
Rm = [];
w = zeros(3,1);
wd = zeros(3,1);
vd = grav(:); %考虑到了重力的影响
%
% init some variables, compute the link rotation matrices
%
for j=1:n
link = robot.links(j);
Tj = link.A(q(j));%关节转换的矩阵提取
switch link.type
case 'R'
D = link.d;
case 'P'
D = q(j);
end
alpha = link.alpha;
pm = [link.a; -D*sin(alpha); D*cos(alpha)]; % (i-1) P i
if j == 1
pm = t2r(robot.base) * pm;
Tj = robot.base * Tj;
end
Pm(:,j) = pm;
Rm{j} = t2r(Tj);
if debug>1
Rm{j}
Pm(:,j).'
end
end
%Rm:每一步的转换矩阵 另一个的作用不知道
%
% the forward recursion
%
for j=1:n
link = robot.links(j);
R = Rm{j}.'; % transpose!!
P = Pm(:,j);
Pc = link.r;%重心位置
%
% trailing underscore means new value
%
switch link.type
case 'R'
% revolute axis
w_ = R*w+ z0*qd(j);
wd_ = R*wd + cross(R*w,z0*qd(j)) + z0*qdd(j);
%v = cross(w,P) + R*v;
vd_ = R * (cross(wd,P) + ...
cross(w, cross(w,P)) + vd);
case 'P'
% prismatic axis
w_ = R*w;
wd_ = R*wd;
%v = R*(z0*qd(j) + v) + cross(w,P);
vd_ = R*(cross(wd,P) + ...
cross(w, cross(w,P)) + vd ...
) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j);
end
% update variables
w = w_;
wd = wd_;
vd = vd_;
%正向推导质心加速度和角加速度
% 角加速度就是各轴角加速度的叠加
%但是加速度不一样,涉及到关节点加速度和w带来的向心加速度叠加
% vd_ = R*(cross(wd,P) + ...
% cross(w, cross(w,P)) + vd ...
% ) + 2*cross(R*w,z0*qd(j)) + z0*qdd(j);
vdC = cross(wd,Pc).' + ...
cross(w,cross(w,Pc)).' + vd;
%质心加速度推到
F = link.m*vdC;%非惯性力
N = link.I*wd + cross(w,link.I*w);
Fm = [Fm F];
Nm = [Nm N];
% if debug
% fprintf('w: '); fprintf('%.3f ', w)
% fprintf('\nwd: '); fprintf('%.3f ', wd)
% fprintf('\nvd: '); fprintf('%.3f ', vd)
% fprintf('\nvdbar: '); fprintf('%.3f ', vdC)
% fprintf('\n');
% end
end
%
% the backward recursion
%
fext = fext(:); %此处可以修改为变负载
f = fext(1:3); % force/moments on end of arm
nn = fext(4:6);
for j=n:-1:1 %倒推关节力矩
%
% order of these statements is important, since both
% nn and f are functions of previous f.
%
link = robot.links(j);
if j == n
R = eye(3,3);
P = robot.tool.t;
else
R = Rm{j+1};
P = Pm(:,j+1); % i/P/(i+1)
end
Pc = link.r;
f11 = R*f;
f12 = R*f;
f_ = R*f + Fm(:,j);
n11 = R*nn + cross(P,R*f);
n12 = Nm(:,j) + cross(Pc,Fm(:,j)).';
nn_ = Nm(:,j) + R*nn + cross(Pc,Fm(:,j)).' + ...
cross(P,R*f);
%Nm(:,j)构件n惯性惯性力矩 R*nn 构件n+1的继承力矩
%cross(Pc,Fm(:,j)) 构件n的惯性力力矩 cross(P,R*f);构件n+1的继承力矩
f = f_; %力
nn = nn_; %力矩
% 输出f和n的过程计算数据
fprintf('f继承: '); fprintf('%.3f ', f11)
fprintf('\nn继承: '); fprintf('%.3f ',n11)
fprintf('\n');
fprintf('f本体 '); fprintf('%.3f ', f12)
fprintf('\nn本体: '); fprintf('%.3f ',n12)
fprintf('\n');
fprintf('f: '); fprintf('%.3f ', f)
fprintf('\nn: '); fprintf('%.3f ', nn)
fprintf('\n');
switch link.type
case 'R'
% revolute
tau(p,j) = nn.'*z0 + ...
link.G^2 * link.Jm*qdd(j) - ...
friction(link, qd(j));
case 'P'
% prismatic
tau(p,j) = f.'*z0 + ...
link.G^2 * link.Jm*qdd(j) - ...
friction(link, qd(j));
end
end
f_base= f;
R_base = rotz(p(1,1));
P_base = (robot.base.t'+[0 0 robot.links(1, 1).d])';
nn_base = R_base*nn + cross(P_base,R_base*f);
% fprintf('f: '); fprintf('%.3f ',f_base)
% fprintf('\nn: '); fprintf('%.3f ', nn_base)
% fprintf('\n');
% fprintf('p: '); fprintf('%d ',p)
% fprintf('\n');
%输出底座三个方向的力和三个方向的扭矩
base=[f_base',nn_base'];%
Base(p,:)=base;
end
改动倒推第一步的位姿参数和新增Base输出,在[Q1,Q2]=rnene(robot,x1,g,w);将底座实时受力情况通过Q2导出。