精华内容
下载资源
问答
  • 包含代码和文档 采用复化梯形公式和复化辛普森公式求积分,并与精确值进行比较得下表。 采用复化梯形公式和复化辛普森公式求积分,并与精确值进行比较得下表。
  • 机械设计提供计算支持,便于设计人员的计算校核,缩短设计计算时间,加快产品的开发速度。
  • 写出MATLAB函数为 function 而若用区间中点 的“高度” 近似取代平均高度 ,则可导出 中矩形公式(简称 矩形公式) 其MATLAB函数为 function 总结:更一般地,我们可以在区间 上适当选取某些节点 ,然后用 的加权...

    2f0ca1960975d55e47b79965e0312674.png

    数值积分的基本思想

    由积分中值定理可知,在积分区间

    内存在一点
    ,成立

    式的几何意义即为:底为
    而高为
    的矩形的面积恰等于所求曲边梯形的面积
    。因此,要想求出
    式左端积分,我们只需要知道三个值:
    即可,这里
    是显然的,问题在于
    的具体位置一般是不清楚的,从而
    未知。我们暂且将
    称为区间
    上的平均高度,我们的目标就是寻求一种求出平均高度
    的算法,这样我们的问题也就解决了。

    首先我们想到的是用区间两端点的“高度”

    的算术平均值作为平均高度
    的近似值,从而导出下述求积公式

    式即为我们我们熟知的
    梯形公式。写出MATLAB函数为
    function

    而若用区间中点

    的“高度”
    近似取代平均高度
    ,则可导出
    中矩形公式(简称 矩形公式

    其MATLAB函数为

    function

    总结:更一般地,我们可以在区间

    上适当选取某些节点
    ,然后用
    的加权平均得到平均高度
    的近似值,这样构造出来的求积公式具有下列通式

    其中

    称为
    求积节点
    称为
    求积系数,也称作伴随节点

    这种数值积分方法通常称为机械求积,其优势在于将积分求值问题归结为被积函数值的计算,很适合在计算机上使用。

    代数精度

    由于数值积分方法是近似方法,为保证精度,自然希望我们的求积公式能够对“尽可能多”的函数准确成立,这便提出了代数精度的概念。

    如果某个求积公式对于次数不超过
    的多项式均能够准确成立,但对于
    次多项式就不准确成立,则称该求积公式具有
    次代数精度
    (或 代数精确度)。

    前面提到的梯形公式

    和矩形公式
    均具有一次代数精度。

    求积公式的收敛性与稳定性

    在求积公式
    中,若
    其中
    ,则称求积公式
    收敛的。
    对任给
    ,若
    ,只要
    就有
    成立,其中
    为计算
    时产生误差
    后实际得到的值,即
    ,则称求积公式
    稳定的。

    定理

    若求积公式
    中系数
    ,则此求积公式是稳定的。

    牛顿-柯特斯(Newton-Cotes)公式

    将积分区间

    划分为
    等份,此时步长为
    ,选取等距节点
    构造出的插值型求积公式

    称为牛顿-柯特斯(Newton-Cotes)公式,其中

    称为
    柯特斯系数。可以通过下式确定

    • 时,
      这时的求积公式便是之前的梯形公式
    • 时,
      此时对应的求积公式便是
      辛普森(Simpson)公式:
      写成MATLAB函数为
    function
    • 时的牛顿-柯特斯公式则特别称为
      柯特斯公式,其形式为
      这里

    其MATLAB代码为

    function

    复合求积公式

    由于牛顿-柯特斯公式在

    时不具有稳定性,故不可能再通过提高阶的方法来提高求积精度。
    复合求积法便是通过把积分区间分成若干个子区间(通常是等分),再在每个子区间上使用低阶求积公式的方法来提高精度的。

    其实细心的同学可以发现,前面我所展示的MATLAB代码使用的便是对每个求积公式使用复合法完成的。常用的有复合梯形公式复合辛普森公式

    龙贝格(Romberg)求积公式

    前面介绍的复合求积方法可提高求积精度,如若精度仍不够,则可通过将步长逐次减半的方式来提高精度。如对复合梯形公式可导出其递推公式

    其中

    表示在
    基础上步长
    减半后的积分值。

    定理

    ,则有

    其中
    系数
    无关。

    式可以看出,
    阶,若用
    代替
    ,有

    再用

    式再减去
    式后再除以

    这里

    是与
    无关的系数。

    式可看出,用近似积分值
    ,其误差阶为
    ,显然误差阶是提高了。类似这种将计算
    的近似值的误差阶由
    提高到
    的方法称为
    外推算法,也称为 理查德森(Richardson)外推算法
    这是“数值分析”中一个重要的技巧,只要真值与近似值的误差能够表示成
    的幂级数,如
    式所示,都可以使用外推算法,提高精度。

    龙贝格(Romberg)算法

    次外推加速为

    余项为

    此方法常称为理查德森外推加速方法

    设用

    表示二分
    次后求得的梯形值,且以
    表示序列
    次加速值,则由递推公式
    可得

    上次则称为龙贝格求积算法,计算过程如下:

    1. ,求
      。令
      (
      记区间
      的二分次数)。
    2. 求梯形值
      ,即按递推公式
      计算
    3. 求加速值。
    4. (预先给定的精度),则终止计算,并且
      ;否则令
      继续计算。

    MATLAB代码为

    function

    自适应积分方法

    复合求积方法通常适用于被积函数变化不太大的积分,如果在求积区间中被积函数变化很大,有的部分函数值变化剧烈,另一部分却变化平缓。这时如果统一将区间等分再用复合求积公式计算积分将会导致计算量很大,我们想实现的是在满足误差要求的前提下,对变化剧烈部分将区间细分,而平缓部分则可使用大步长,也即针对被积函数在区间上不同情形采用不同的步长,使得再满足精度前提下积分计算工作量尽可能小。其算法技巧是在不同区间上预测被积函数变化的剧烈程度确定相应步长。这就是自适应积分方法的基本思想。

    下面是一个基于复合辛普森法的自适应积分算法的MATLAB代码:

    function

    下面我们计算积分

    ,可输入下列语句调用上述MATLAB函数进行计算
    clear
    

    结果为

    I 

    高斯求积公式

    下面研究带权积分

    为权函数)的求积公式

    下面看定义

    如果求积公式
    具有
    次代数精度,则称其节点
    高斯点,相应公式
    称为
    高斯型求积公式。

    这里主要包括四种高斯型求积公式,即

    • 高斯-勒让德求积公式
    • 高斯-切比雪夫求积公式
    • 高斯-拉盖尔求积公式
    • 高斯-埃尔米特求积公式

    多重积分

    这里主要说一下多重积分计算的思路,归结为一句话,就是:多重积分化累次积分,再由里到外使用数值积分公式进行求积计算。

    展开全文
  • matlab实现机械臂正逆运动学控制

    万次阅读 多人点赞 2019-03-28 21:19:46
    5.MATLAB 程序源代码; 一、 设计三轴机器人 设计出如上图的三轴机器人,第一个和第三个轴是旋转的,第二个是伸长的。第一个轴到第二个轴的距离是100cm,第二个轴的伸长量是0~100cm,第三个轴到手持器的距...

    设计要求:
    1.建立一个三自由度的机器人
    2.建立坐标系,给出 D-H 参数表;
    3.推导正运动学,并写出机器人的齐次变换矩阵;
    4.推导逆运动学,并让机器人完成按要求绘制给定图形。
    5.MATLAB 程序源代码;

    一、 设计三轴机器人
    tanunat
    设计出如上图的三轴机器人,第一个和第三个轴是旋转的,第二个是伸长的。第一个轴到第二个轴的距离是100cm,第二个轴的伸长量是0~100cm,第三个轴到手持器的距离是100cm。因此可以得出D-H参数表。

    二、 建立坐标系,给出 D-H 参数表
    建立坐标系如下所示,由下图得出下面的D-H参数表
    在这里插入图片描述
    在这里插入图片描述
    三、 推导正运动学,并写出三个齐次变换矩阵
    假设现在位于本地参考坐标系xn-zn,那么通过以下4步标准运动即可到达下一个本地参考坐标系xn+1-zn+1。

    1. 绕z_n轴旋转θ_(n+1),使得x_n 和x_(n+1)互相平行;
    2. 沿z_n 轴平移d_(n+1) 距离,使得x_n 和x_(n+1) 共线;
    3. 沿已经旋转过的x_n轴平移a_(n+1)的距离,使得x_n 和x_(n+1)的原点重合;
    4. 将z_n轴绕x_(n+1)轴旋转α_(n+1),使得z_n轴与z_(n+1)轴对准;
      根据矩阵右乘可得到以下结果
      在这里插入图片描述
      根据3dof_robot D-H表表以及上面公式,可得机器人的正运动学方程如下
      在这里插入图片描述
      在这里插入图片描述
      四、 推导逆运动学,并让机器人完成按要求绘制给定图形
      在这里插入图片描述
      在这里插入图片描述
      U1与A3对应颜色相比较得如下三个式子:
      100 – pz=100cos(zeta3)
      py
      cos(zeta1) - d2 - pxsin(zeta1)= 100sin(zeta3)
      pxcos(zeta1) + pysin(zeta1)=0

    三式联合求解得出
    zeta1=-arctan(px/py)
    zata3=arccos((100-pz)/100)
    D2=pycos(zeta1)-pxsin(zeta1)-100*sin(zata3)

    在这里插入图片描述
    在这里插入图片描述
    在这里插入图片描述
    主要matlab代码
    1.zq_robot_robot.m

    1.	建机器人
    ToDeg = 180/pi;
    ToRad = pi/180;
    UX = [1 0 0]';
    UY = [0 1 0]';
    UZ = [0 0 1]';
    
    Link= struct('name','Body' , 'th',  0, 'dz', 0,  'dy', 0, 'dx', 0, 'alf',90*ToRad,'az',UZ);     % az 
    Link(1)= struct('name','Base' , 'th',  0*ToRad, 'dz', 0, 'dy', 0,'dx', 0, 'alf',0*ToRad,'az',UZ);        %Base To 1
    Link(2) = struct('name','J1' , 'th',   0*ToRad, 'dz', 100, 'dy', 0, 'dx', 0, 'alf',-90*ToRad,'az',UZ);       %1 TO 2
    Link(3) = struct('name','J2' , 'th',  90*ToRad, 'dz', 200, 'dy', 0,  'dx', 0, 'alf',90*ToRad,'az',UZ);    %2 TO 3
    Link(4) = struct('name','J3' , 'th',  0*ToRad, 'dz', 0, 'dy', 0, 'dx', 100, 'alf',0*ToRad,'az',UZ);          %3 TO E
    
    

    2.draw_6DOF_Workplace.m

    2.	画工作空间
    close all;    %删除其句柄未隐藏的所有图窗。
    clear;        %清除工作空间
    
    ToDeg = 180/pi;   %转化为度数
    ToRad = pi/180;   %转化为弧度
    
    point1=[];    %设为矩阵
    point2=[];
    point3=[];
    th_interval = 40;    %弧间隔
    z_interval = 4;      %线间隔
    
    th1=0;   %为th1至th6设定初始值
    th2=0;
    th3=0;
    
    global Link
    
    num = 1;
    
    for theta1=-180:th_interval:180   %循环画工作空间
        for dt2=00:z_interval:100
            for theta3=-180:th_interval:180
    
                            zq_robot_dh(th1+theta1,th2+dt2,th3+theta3,1);  %,d4+dz4,th5+theta5,th6+theta6
                            point1(num) = Link(4).p(1);    %用这个矩阵来存数据,这里共存三行数据
                            point2(num) = Link(4).p(2);
                            point3(num) = Link(4).p(3);
                            num = num + 1;
                            plot3(point1,point2,point3,'r*');hold on;   %这里用了point1,point2,point3,而不用 point1(num),point2(num), point3(num)?  
    
            end
        end
    end
    
    cla;       %cla 从当前坐标区删除包含可见句柄的所有图形对象,把上面的图形清除。
    plot3(point1,point2,point3,'r*');      %这里再画一个图形
    axis([-400,400,-400,400,-400,400]);    %设置轴范围和纵横比
    grid on;                %显示 gca 命令返回的当前坐标区或图的主网格线。主网格线从每个刻度线延伸。grid off 删除当前坐标区或图上的所有网格线。
    
    

    3.zq_robot_qiunijie.m

    3.	根据逆运动学方程求关节角度
    %根据逆运动学方程求关节角度
    function [ th1,d2,th3] = zq_robot_qiunijie( px,py,pz )
    ToDeg = 180/pi;
    ToRad = pi/180;
    
    th1=-atan2(px,py);        %逆运动学方程
    th3=acos((100-pz)/100);
    d2=py*cos(th1)-px*sin(th1)-100*sin(th3);
    
    fprintf('th1=%4.2f \n',th1*ToDeg);   %观察输出结果
    fprintf('d2=%4.2f \n',d2);
    fprintf('th3=%4.2f \n',th3*ToDeg);
    
    end
    
    
    

    4.draw_cube.m

    4.	画正方体(此处用到正、逆运动学)
    %画正方体
    close all;
    clear;
    
    ToDeg = 180/pi;
    ToRad = pi/180;
    point1=[];    %设为矩阵
    point2=[];
    point3=[];
    
    num=1;
    global Link
    
    for z=0:5:50
        for y=-25:5:25
            for x=50:5:100
                
                [th1,d2,th3]= zq_robot_qiunijie(x,y,z); %逆运动学
    
                th1=th1*ToDeg;
                th3=th3*ToDeg;
    
                move=zq_robot_dh(th1,d2,th3,1);  %正运动学
                
                point1(num) = Link(4).p(1);    %用这个矩阵来存数据,这里共存三行数据
                point2(num) = Link(4).p(2);
                point3(num) = Link(4).p(3);
                
                plot3(point1,point2,point3,'r.');hold on;   %这里用了point1,point2,point3,而不用 point1(num),point2(num), point3(num)?  
                
                fprintf('point1=%4.2f \n',point1(num));     %观察输出点的情况
                fprintf('point2=%4.2f \n',point2(num));
                fprintf('point3=%4.2f \n',point3(num));
                
                num = num + 1;
            end
        end
    end
    grid on;   
    
    

    5.draw_writing

    5.	写“志”字(此处用到正、逆运动学)
    close all;
    clear;
    
    ToDeg = 180/pi;
    ToRad = pi/180;
    point1=zeros(100,1);    %设为矩阵
    point2=zeros(100,1);  
    point3=zeros(100,1);  
    
    num=1;
    global Link
    
    xx=50;
    yy=[20;20;12;14;16;18;20;22;24;26;28;20;18;20;22;14;12;16;18;20;22;24;26;26;18;20;22];
    zz=[50;48;46;46;46;46;46;46;46;46;46;44;42;42;42;38;36;36;34;34;34;34;35;34;38;37;38];
    
    for i=1:1:27
       
                
                [th1,d2,th3]= zq_robot_qiunijie(xx,yy(i),zz(i));    %求逆解
    
                th1=th1*ToDeg;
                th3=th3*ToDeg;
    
                move=zq_robot_dh(th1,d2,th3,1);
                
                point1(num) = Link(4).p(1);    %用这个矩阵来存数据,这里共存三行数据
                point2(num) = Link(4).p(2);
                point3(num) = Link(4).p(3);
                
                plot3(point1,point2,point3,'r*');hold on;   %这里用了point1,point2,point3,而不用 point1(num),point2(num), point3(num)?  
                
                fprintf('point1=%4.2f \n',point1(num));     %观察输出点的情况
                
                fprintf('point2=%4.2f \n',point2(num));
                fprintf('point3=%4.2f \n',point3(num));
                
                num = num + 1;
       
    end
    grid on;   
    
    

    6.Connect3D.m

    function Connect3D(p1,p2,option,pt)        %这是连接两个关节成一条杆的函数,Link(i).p表示第i个关节的空间位置。
    
    h = plot3([p1(1) p2(1)],[p1(2) p2(2)],[p1(3) p2(3)],option);    %画p1点到p2点的直线,p1,p2两点都是四行一列的矩阵,不过这里取前三行的值。option是线条颜色值。
    set(h,'LineWidth',pt)    %这里pt为线宽,即机器人杆的宽度。
    

    7.DHfk6Dof_Workplace

    function pic=DHfk6Dof_Workplace(th1,th2,th3,fcla,fplot)  %这是用来画工作空间的函数内核  d4,th5,th6,
    % close all
    global Link
    
    % zq_3dof_robot;
    Build_3DOFRobot_Lnya;
    radius    = 10;
    len       =   20;
    joint_col = 0;
    
    
    plot3(0,0,0,'ro'); 
    
    Link(2).th=th1*pi/180;           %变成弧度,th1取至draw_6DOF_Workplace。
    Link(3).th=th2*pi/180;
    Link(4).th=th3*pi/180;
    
    
    % p0=[0,0,0]';
    
    for i=1:4
    Matrix_DH_Ln(i);  %生成关节链接的D-H矩阵。
    end
    
    for i=2:4
          Link(i).A=Link(i-1).A*Link(i).A;     %第i+1个矩阵乘第i个矩阵,矩阵右乘,把所有矩阵相乘。 Link(i)函数取至Matrix_DH_Ln(i)。
          Link(i).p= Link(i).A(:,4);     %取Link(i).A中所有行的第4列放到Link(i).p,把第i个关节的位置存在Link(i).p中。
          Link(i).n= Link(i).A(:,1);     %同上
          Link(i).o= Link(i).A(:,2);     %同上
          Link(i).a= Link(i).A(:,3);     %同上
          Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];  %把第i个关节的姿态存在 Link(i).R中
          if fplot   %当fplot为1时执行下面两个函数
              Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;             %'b',是指线条为蓝色,Link(i)函数取至Matrix_DH_Ln(i)。画杆  ,hold on 保留当前坐标区中的绘图,从而使新添加到坐标区中的绘图不会删除现有绘图。
              DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az, radius,len, joint_col); hold on;   %画圆筒
          end
    end
    
    
    grid on;
    % view(134,12);
    axis([-500,500,-500,500,-500,500]);    %指定范围
    xlabel('x');
    ylabel('y');
    zlabel('z');
    drawnow;
    pic=getframe;
    
    if(fcla)
        cla;
    end
    
    
    
    

    8.Matrix_DH_Ln

    function Matrix_DH_Ln(i)     %这个是D-H矩阵的算法函数
    % Caculate the D-H Matrix
    global Link
    
    ToDeg = 180/pi;
    ToRad = pi/180;
    
    
    C=cos(Link(i).th);
    S=sin(Link(i).th);
    Ca=cos(Link(i).alf);
    Sa=sin(Link(i).alf);
    a=Link(i).dx;    %distance between zi and zi-1
    d=Link(i).dz;    %distance between xi and xi-1
    y=Link(i).dy;
    
    Link(i).n=[C,S,0,0]';
    Link(i).o=[-1*S*Ca,C*Ca,Sa,0]';
    Link(i).a=[S*Sa, -1*C*Sa,Ca,0]';
    Link(i).p=[a*C-y*S,a*S+y*C,d,1]';    %书本第57页的D-H矩阵
    
    Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];%把上面D-H矩阵前面的3*3矩阵存起来
    Link(i).A=[Link(i).n,Link(i).o,Link(i).a,Link(i).p];%把第i到i+1的D-H矩阵存进Link(i).A
    
    
    

    9.DrawCylinder.m

    function h = DrawCylinder(pos, az, radius,len, col)        %这是个画圆筒(关节)的函数
    % draw closed cylinder  
    %
    %******** rotation matrix
    az0 = [0;0;1];
    ax  = cross(az0,az);
    ax_n = norm(ax);
    if ax_n < eps 
    	rot = eye(3);
    else
        ax = ax/ax_n;
        ay = cross(az,ax);
        ay = ay/norm(ay);
        rot = [ax ay az];
    end
    
    %********** make cylinder
    % col = [0 0.5 0];  % cylinder color
    
    a = 20;    % number of side faces
    theta = (0:a)/a * 2*pi;
    
    x = [radius; radius]* cos(theta);
    y = [radius; radius] * sin(theta);
    z = [len/2; -len/2] * ones(1,a+1);
    cc = col*ones(size(x));
    
    for n=1:size(x,1)
       xyz = [x(n,:);y(n,:);z(n,:)];
       xyz2 = rot * xyz;
       x2(n,:) = xyz2(1,:);
       y2(n,:) = xyz2(2,:);
       z2(n,:) = xyz2(3,:);
    end
    
    %************* draw
    % side faces
    h = surf(x2+pos(1),y2+pos(2),z2+pos(3),cc);
    
    for n=1:2
    	patch(x2(n,:)+pos(1),y2(n,:)+pos(2),z2(n,:)+pos(3),cc(n,:));
    end	
    

    10.zq_robot_dh.m

    function pic = zq_robot_dh( th1,distance,th3,fcla )
    %UNTITLED4 此处显示有关此函数的摘要
    %   此处显示详细说明
    
    global Link
    
    zq_3dof_robot;
    radius = 10;
    len = 30;
    joint_col = 0;
    
    plot3(0,0,0,'ro');
    
    Link(2).th=th1*pi/180;
    Link(3).dz=distance;
    Link(4).th=th3*pi/180;
    
    p0=[0,0,0]';
    
    for i=1:4
    Matrix_DH_Ln(i);
    end
    
    for i=2:4
    Link(i).A=Link(i-1).A*Link(i).A;
    Link(i).p= Link(i).A(:,4);
    Link(i).n= Link(i).A(:,1);
    Link(i).o= Link(i).A(:,2);
    Link(i).a= Link(i).A(:,3);
    Link(i).R=[Link(i).n(1:3),Link(i).o(1:3),Link(i).a(1:3)];
    Connect3D(Link(i-1).p,Link(i).p,'b',2); hold on;
    DrawCylinder(Link(i-1).p, Link(i-1).R * Link(i).az,radius,len, joint_col); hold on;
    end
    
    grid on;
    % view(134,12);
    axis([-200,200,-200,200,-100,200]);
    xlabel('x');
    ylabel('y');
    zlabel('z');
    drawnow;
    pic=getframe;
    if(fcla)
    cla;
    end
    
    end
    
    
    

    11.zhengyundongxue.m

    %dh矩阵函数
    function [ result ] = zhengyundongxue( A )
    %ZHENGYUNDONGXUE 此处显示有关此函数的摘要
    %   此处显示详细说明
    zeta=A(1);
    d=A(2);
    a=A(3);
    alf=A(4);
    
    result=[cos(zeta)  -sin(zeta)*cos(alf)  sin(zeta)*sin(alf)  a*cos(zeta);
           sin(zeta)   cos(zeta)*cos(alf)    -cos(zeta)*sin(alf)  a*sin(zeta);
           0          sin(alf)             cos(alf)             d;
           0           0                    0                  1];
    
    end
    
    
    

    12.jisuanzhengyundong.m

    %这是用来求正运动学中各个dh矩阵的
    syms pi d2 zeta1 zeta2 zeta3 A4  a1 a2 a3   nx ny nz ox oy oz ax ay az px py pz  U1;
    %p1=[0 d2 0 0];
    p1=[zeta1 100 0 -pi/2];
    A1=zhengyundongxue(p1);
    A1=simplify(A1)
    
    %p2=[zeta2 0 100 pi/2];
    p2=[pi/2 d2 0 pi/2];
    A2=zhengyundongxue(p2);
    A2=simplify(A2)
    
    %p3=[zeta3 0 100 0];
    p3=[zeta3 0 100 0];
    A3=zhengyundongxue(p3);
    A3=simplify(A3)
    
    A4=A1*A2*A3;
    A4=simplify(A4)
    
    a1=inv(A1);     %求逆
    a2=inv(A2);
    a3=inv(A3);
    a1=simplify(a1)
    a2=simplify(a2)
    a3=simplify(a3)
    
    
    U=[nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1];%设出要求的矩阵U
    
    U1=a2*a1*U;
    U1=simplify(U1)
    

    13.Computer_T.m

    close all;
    clear;
    
    ToDeg = 180/pi;
    ToRad = pi/180;
    
    syms theta d a alpha y;
    T1 = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0; 0 0 0 1];
    T2 = [1 0 0 0; 0 1 0 0; 0 0 1 d; 0 0 0 1];
    T3 = [1 0 0 a; 0 1 0 0; 0 0 1 0; 0 0 0 1];
    T4 = [1 0 0 0; 0 cos(alpha) -sin(alpha) 0; 0 sin(alpha) cos(alpha) 0; 0 0 0 1];
    Ty = [1 0 0 0; 0 1 0 y; 0 0 1 0; 0 0 0 1];
    T = T1 * T2 * Ty * T3 * T4
    
    syms theta1 theta2 theta3 d4 theta5 theta6
    theta=theta1;d=100;y=0;a=0;alpha = -90*ToRad;
    A1 = subs(T)
    theta=-90*ToRad+theta2;d=0;y=0;a=100;alpha = 0;
    A2 = subs(T)
    theta=theta3;d=0;y=50;a=0;alpha = -90*ToRad;
    A3 = subs(T)
    % theta=0;d=50+d4;y=0;a=0;alpha = 0;
    % A4 = subs(T)
    % theta=theta5;d=50;y=0;a=0;alpha = 90*ToRad;
    % A5 = subs(T)
    % theta=90*ToRad+theta6;d=0;y=0;a=50;alpha = 0;
    % A6 = subs(T)
    A = A1 * A2 * A3   %* A4 * A5 * A6
    

    上面代码比较多,逐一放到matlab中运行一下,看看各个代码的效果如何。

    展开全文
  • 机械臂动力学方程线性化给大家分享一下,网上没有找到具体推导公式,不喜欢藏着掖着,这是自己推导的,仅供大家参考。有问题欢迎批评指正。1.问题来源进行机械臂惯性参数辨识,需将机械臂的动力学方程线性化,及将...

    机械臂动力学方程线性化

    给大家分享一下,网上没有找到具体推导公式,不喜欢藏着掖着,这是自己推导的,仅供大家参考。有问题欢迎批评指正。

    1.问题来源

    进行机械臂惯性参数辨识,需将机械臂的动力学方程线性化,及将动力学方程的形式写成力矩等于矩阵与惯性参数乘积的形式,且矩阵Y中不包含机械臂惯性参数,这样可以利用矩阵的广义逆,求解出惯性参数。本文以6R机械臂为例,假设机械臂运动过程中需要的关节力矩

    ,机械臂所有连杆的惯性参数组成的集合P,P对应的系数矩阵Y,则线性化动力学方程为式1.1,应用广义逆求解的公式为1.2。将动力学方程化简,求解系数矩阵Y和参数P,就是动力学方程线性化的问题。

    但是由于机械臂惯性参数对应的矩阵Y一般非满秩,一般寻求一组最小的惯性参数集Pr,使得最小惯性参数集的系数矩阵Yr满秩,进而使用广义逆求解。定义矩阵C,使得式1.3成立。

    则式1.1可化为:

    定义

    其中

    为满秩矩阵,可以直接用矩阵的广义逆求解惯性参数。根据机械臂的惯性参数集P,求解最小惯性参数集
    和转换矩阵C,这就是最小惯性参数集求解的问题。

    啰嗦一句,matlab求矩阵的逆,即使系数矩阵不满秩,也可以求,具体用机械臂60个参数去辨识还是用最小惯性参数集去辨识,这也是目前个人比较困惑的问题,我们这有人直接60个参数一起辨识,很多文献上都是最小惯性参数辨识,这两种区别,希望有大佬能够解答解答两者的区别。

    2.解决思路

    常见的机械臂动力学方程建模形式主要有牛顿欧拉动力学方程和拉格朗日动力学方程两种。由于拉格朗日动力学方程中需要动势对关节角求偏导,个人觉得不太好对动力学方程中进行化简,使用matlab符号运算推导,最后求解出的系数矩阵Y表达式很长,因此改用牛顿欧拉方式推导线性化动力学方程。拉格朗日线性化动力学方程推到过程参考霍伟《机器人动力学与控制》。

    线性化动力学方程存在两个主要的问题,一是如何将动力学方程中的惯性参数分离出来;二是方程组的化简。为了分离出惯性参数,获得较为简洁的线性化动力学方程,采用牛顿欧拉形式对动力学方程进行推导,推导过程中的根据自己的思路将部分跟惯性参数无关的变量化简,最后得到推导系数矩阵Y的递推公式。

    牛顿欧拉方式推导动力学方程,欧拉公式用于描述物体的转动,通用的形式如下:

    传统的欧拉公式相对连杆质心C坐标系建立,即欧拉公式中惯性矩阵I是相对于质心的惯性矩阵

    。而为了提取出惯性参数,这里欧拉公式是相对于连杆坐标系原点建立欧拉方程,即将惯性矩阵I表示相对于连杆坐标系原点处的惯性矩阵
    。方程线性化推导的具体过程,在后面详述。

    牛顿欧拉推导线性化动力学方程,求解过程主要分为以下几个步骤:

    1)求解相邻连杆坐标系的旋转变换矩阵

    和位置关系
    表示坐标系{i}相对坐标系{i-1}的旋转矩阵。
    表示坐标系{i}的原点在坐标系{i-1}中的位置。

    2)求连杆角速度

    。表示坐标系{i}中连杆i的角速度。

    3)求连杆角加速度

    ,表示坐标系{i}中连杆i的角加速度。

    4)求坐标系原点的加速度

    ,表示坐标系{i}中坐标系{i}原点的加速度。

    由于上述1)~4)中的运动学参数与机械臂惯性参数无关,且输入关节角

    、关节角速度
    、角加速度
    后可直接求出,因此认为可将其作为已知量,用于线性化动力学方程的求解。

    5)求解每个连杆的合力

    ,在坐标系{i}中表示,同时分离出惯性参数。

    6)从连杆末端到基座递推求连杆i受到连杆i-1的力

    和关节力矩
    ,同时分离出惯性参数。

    7)将每个关节力矩

    的系数矩阵提出,组成整个机械臂的系数矩阵Y。

    3.求解方案

    3.1求旋转矩阵

    和位置

    旋转矩阵

    的求法在机械臂运动学中大量运用,这里求法不推导,默认已知了。假设求得的6个旋转矩阵为
    ,6个位置向量

    3.2求连杆角速度

    由于基座不动,所以基座的角速度

    ,由基座向末端正向推导各连杆的角速度,基本关系式:

    其中

    。所以各连杆的角速度:

    由各连杆角速度表达式可知,连杆角速度

    只与关节角速度
    和旋转矩阵
    有关,与惯性参数P无关,且输入关节角
    、关节角速度
    后即可求出,因此视为7个已知量,带入后面的动力学参数的系数矩阵中。

    3.3求连杆角加速度

    由于基座不动,所以连杆0的角加速度

    ,同样正向推导,求连杆角加速度的基本公式:

    由公式3.3求各连杆的角加速度:

    同样由上式知角加速度表达式与惯性参数无关,所以将连杆角加速度的7个量可视为已知量,带入后面的动力学参数的系数矩阵中。

    3.4求坐标系原点的加速度

    已知基座不动,为将重力g的影响带入动力学方程,需在牛顿欧拉公式中,将基座的加速度设为(这里默认机械臂正装,侧装及倒装对应g的位置可能在x方向或者y方向上,具体跟设立的坐标系有关):

    由基座向机械臂末端正向递推求坐标系连杆的加速度,通用公式如下:

    根据式3.6,可以求得每个连杆坐标系的加速度:

    为坐标系{i}原点在坐标系{i-1}中的位置矢量,注意并不是质心位置,可视为为已知量。7个加速度矢量同样也可视为已知量,带入后面的动力学方程中。

    3.5逆推求连杆所受的合力

    设连杆质心处加速度

    表示,连杆i质心在本连杆坐标系中的位置
    ,连杆的质量
    已知各运动学参数,可根据连杆坐标系原点处的加速度求质心处的加速度

    可以求出连杆i受到的合力
    :

    由式3.9知,连杆受到的合力

    跟连杆质量
    和质量与质心位置乘积
    成线性关系,将惯性参数
    提取出来。已知叉乘可以用叉积矩阵S表示,定义:

    这个式子可以将叉乘写成叉积矩阵的形式,是分离动力学方程很重要的一个式子

    用式3.10改写式3.9:

    定义每个连杆的惯性参数:

    则整个机械臂所有连杆的惯性参数集P:

    将3.11式写成矩阵乘惯性参数的形式:

    定义一个新的矩阵
    ,使得:
    带入式3.14中,可得到:
    其中
    与是一个与惯性参数
    无关的
    的矩阵。

    3.6逆推求连杆合力矩

    和连杆
    给连杆i的力
    、力矩

    1)求关节处作用力

    对每个连杆列动力学方程,有:

    所以有:
    2)连杆合力矩

    研究连杆i的旋转,设连杆受到的合力矩在坐标系{i}中表示为

    ,坐标系原点处的惯性矩阵
    ,对连杆i坐标系的原点用欧拉方程:
    由于存在关系式:
    定义函数:

    (这个式子是第二个线性化动力学方程很重要的式子,将Iw转换成矩阵乘惯性参数的形式)

    用式3.20和式3.10将式3.19化简

    定义
    ,则式3.21可简化为:
    3)求关节处的力矩

    对连杆i坐标系原点列力矩平衡方程:

    可以得出关节处力矩
    4)由连杆末端向基座逆推关节处力矩
    ,定义

    即当i=6时,连杆受力

    将3.24式改写成系数矩阵与整机的惯性参数$P$相乘的形式,
    定义
    表示连杆力fi对应的系数矩阵,所以:
    由于
    :根据式3.23,有:

    当i=5时:

    根据式3.23可知:

    同理,对连杆4、3、2、1有:

    3.7求系数矩阵Y

    由于每个关节处力矩

    表示x,y,z三个方向力矩组成的向量,取z向的值即为每个关节处电机施加的力矩,所以整机的力矩
    有:
    所以求得系数矩阵Y:

    4.总结

    本文将牛顿欧拉公式中的部分跟惯性参数无关的运动学参数作为已知量,去简化动力学方程的推导,最终获得线性化的牛顿欧拉方程。本文简化过程均自我推导,仅供参考。

    展开全文
  • 上传文件为书中的matlab求解的源程序,非课后答案。 美国迈阿密大学Singiresu S.Rao教授的力作《机械振动》(MECHANICAL VIBRATIONS)一本书的第5版秉承了其一贯的内容详实、叙述简介、强调工程背景与计算技术的风格...
  • MATLAB 中的机械臂算法——动力学

    千次阅读 2019-12-11 08:57:47
    转自:https://mp.weixin.qq.com/s/opQcpv02sysIcz8seg3lkQ
    展开全文
  • ——节选自《人类最美的54个公式》在人类的学问里,最接近上帝的是数学。数学追求最高的精确、最合理的逻辑。但更奇妙的是,这个宇宙竟都是经得起每一个极简公式的一再推敲考证。不过,世界只有极少数人天生对数具有...
  • matlab 机械臂动力学

    2020-12-08 18:11:04
    机械臂动力学解决的问题: 1、 正动力学: 已知一个力矩τ,计算出操作臂的运动θ dθ ddθ ,用于模型仿真有用。 2、 逆动力学:已知轨迹点θ/dθ/ddθ ,求出期望的关节力矩矢量τ,用于机械臂的控制问题。必须...
  • 模糊PID算法及其MATLAB仿真(1)

    万次阅读 多人点赞 2019-04-15 20:34:35
    4、模糊自整定PID的理论内容(重点内容) 4.1 基本原理 典型PID控制器的传递函数(CSDN的公式编辑器不太好用,又截图了0.0): 其中 Kp , Ti , Td 分别是比例增益、积分和微分时间常数。PID控制器的设计和参数整定...
  • 通过MATLAB软件中的Robotics Toolbox,分别运算了机械臂的正、逆工作方程,进行了仿真实验。结果表明,函数测算结果与公式推导的数值基本一致,证实了模型结构和预算方法的一致性,对同类机械臂的研究具有很大的借鉴和...
  • 控制机械臂末端进行圆弧绘制,在进行机械臂控制时,使用逆解求解程序得到八组逆解,从中选择一组转角之和最小,且与上个步骤距离最小的关节组合进行控制,使之平滑运动。
  • 机械臂——六轴机械臂构型分析与MATLAB建模

    万次阅读 多人点赞 2018-08-26 14:15:56
    机械臂逆解计算 一、六轴机械臂构型分析 关节机器人,是应用于当前工业领域中最为广泛的工业机器人的构型之一,它也被称作关节型手臂机器人或者关节型机械手臂。它可应用于诸多工业领域,例如喷漆、自动装配、...
  • WORD 格式可编辑 第九章 最优化方法的 Matlab 实现 在生活和工作中人们对于同一个问题往往会提出多个解决方案并通过各方面的 论证从中提取最佳方案 最优化方法就是专门研究如何从多个方案中科学合理地提取出 最佳...
  • 第九章 最优化方法的Matlab实现 ? 在生活和工作中人们对于同一个问题往往会提出多个解决方案并通过各方面的论证从中提取最佳方案最优化方法就是专门研究如何从多个方案中科学合理地提取出最佳方案的科学由于优化问题...
  • PMSM的机械组成 本书目的 三相PMSM的数学建模 三相PMSM的基本数学模型 PMSM转子结构分类 PMSM模型假设 PMSM的基本数学模型4个方程自然坐标系下 三相PMSM的坐标变换 Clark变换 iClark变换 Park变换 iPark...
  • 机械原理大作业解析法分析 本次的作业使用的画图的工具为 MatlabMatlab 的代码可以帮助我们更好的去理解整个的运动过程和运动曲线的是如何形成的 分析运动状态 下面是我的原始数据 名称 数据 近休止状态 0˚ - 30...
  • Matlab验证Frobenius反演定理(数字信号处理课程) 随机产生4个多维矩阵,分别计算等式两边,然后对两结果作差后求2范数,判断范数是否为0(浮点运算,小于1e-6) 本程序代码清晰,注释明确,并在程序中提供了...
  • 我们在求解机械臂的正运动学的时候,对于在写论文的过程,我们需要对最终的位姿矩阵进行相应的化简,比如把sin(q1) ... 今天我利用Matlab 中的一些基本语句实现了对位姿矩阵的自动化简。 一般在DH参数中 theta 和...
  • 基于MATLAB的水果分级设计

    万次阅读 多人点赞 2018-06-14 14:19:35
    基于MATLAB的水果分级识别技术研究摘 要本次毕业设计介绍了基于MATLAB的水果分级自动识别,利用手机端获取苹果的样本图像,应用MATLAB软件编程实现了对样本图像的预处理,包括图像滤波、图像填充、图像灰度化、图像...
  • 这篇文章讲一点数值求解阻尼振子运动的微分方程相关的东西,总体来说是一个比较trivial的...但是我以前都是用mma做东西,现在也是学习matlab的一个机会(然后就发现天下软件都调库),然而matlab是真的真的丑,界面...
  • WORD 格式可编辑 第九章 最优化方法的 Matlab 实现 在生活和工作中人们对于同一个问题往往会提出多个解决方案并通过各方面的 论证从中提取最佳方案最优化方法就是专门研究如何从多个方案中科学合理地提取出 最佳方案...
  • 写在前面 本文使用改进D-H参数法建立模型,使用的是前面ABB机械臂模型,正运动学建模过程可见...Matlab仿真程序 % 适用于Modified DH参数法 function ik_T = myikine(fk_T) d(1) = 0; a(1) = 0; d(2) = 0.090; a(2) ...
  • 欧拉公式求长期率的matlab代码机械臂MPU6050 我是Salim Khazem,我的项目是机械手使用arduino和MPU6050通过手运动控制的机械手这是一个小项目,是使用MPU6050传感器通过手运动控制的机械手。我的程序尚未完成,但是...
  • 欧拉公式求长期率的matlab代码机械臂 该项目的目标是研究4-dof机械手臂的行为 为了模拟机械臂,我们将Matlab与Corke的机械工具箱一起使用。 为机械手臂建模 第一步是在模拟环境中对机器人进行建模,并定义必要的参数...
  • 仿真搭建2.1 运动轨迹函数2.2 机器人的机械仿真模型2.3 机器人的电气仿真模型2.3.1 直流电机部分2.3.2 减速器与传动装置 1. 概述 使用 simulink 搭建《用MATLAB玩转机器人》的第六章——单关节机器人的仿真,详细...
  • 我创建了一个函数,该函数使用拉格朗日公式计算对驱动关节的总扭矩分布有贡献的 3 个主要扭矩分量。 问题的更多详细信息位于 RR2_ID.m 文件中。 然后 GUI 使用此函数,并绘制每个分量(惯性扭矩、科里奥利扭矩、重力...
  • 在研究五杆机构的时候感觉网上的资料更多的在研究正运动学,包括机构顶端的运动轨迹、速度、加速度,缺少逆运动学的分析。当我们想用五杆机构做实际用途比如3D打印或轨迹描绘的时候逆...MATLAB正运动学代码:function
  • 高斯求积代码matlab Md Mirazul Islam的Matlab代码样本 。 第1章:方程组的求解 高斯消除: 雅可比方法: 高斯-塞德尔方法:。 第2章:求解非线性方程 分割方法: 牛顿法: 正割方法: 定点迭代:。 第三章:数值...
  • 如下参考链接1的作者大大实现了UR5机械臂的正运动学和逆运动学的Matlab代码。但逆解部分在不同版本的Matlab中运行有错误。 本篇文章是MatlabR2016a下完成的,并说明一下原代码错误的原因。 目录 一、参考链接...

空空如也

空空如也

1 2 3 4 5 ... 20
收藏数 1,359
精华内容 543
关键字:

matlab机械公式

matlab 订阅