基于matlab rvc机器人工具箱的逆动力学rne函数源码分析和改进

paper__tiger

于 2023-10-09 22:07:43 发布

阅读量500

点赞数 1

文章标签： matlab 机器人开发语言

本文链接：https://blog.csdn.net/paper__tiger/article/details/133718034

版权

众所周知，在机械臂建模中，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;