机器人运动学正解逆解.rar

基于改进DH参数的机器人正解和逆解程序，逆解采用解析解形式，输出8组关节角度解

clc
clear all
close all
deg=pi/180;
warning off;
%% 读取数据_转换旋转矩阵
data = xlsread（‘handian.xlsx‘）;
%第一个点构建位姿矩阵
E1=deg2rad（data（15:7））;
p1=data（12:4）‘/1000;
R1=eul2rotm（E1 ‘ZYX‘）;
R11=R1;
R11（:4）=p1;
R11（4:）=[0 0 0 1];%旋转矩阵加位置，构建点1位姿矩阵
%第二个点构建位姿矩阵
E2=deg2rad（data（25:7））;
p2=data（22:4）‘/1000;
R2=eul2rotm（E2‘ZYX‘）;
R22=R2;
R22（:4）=p2;
R22（4:）=[0 0 0 1];%旋转矩阵加位置，构建点1位姿矩阵

%% 建立机器人_计算正逆运动学
L1= link（‘d‘ 0 ‘a‘ 0 ‘alpha‘ 0‘modified‘‘qlim‘ [-180 180].*deg）;
L2 = link（‘d‘ 0 ‘a‘ 0.325 ‘alpha‘ -pi/2‘modified‘‘qlim‘ [-150 -14].*deg）;
L3 = link（‘d‘ 0 ‘a‘ 1.150 ‘alpha‘ 0‘modified‘‘qlim‘ [-147 90].*deg）;
L4 = link（‘d‘ 1.225 ‘a‘ 0.300 ‘alpha‘ -pi/2 ‘modified‘‘qlim‘ [-210 210].*deg）;
L5 = link（‘d‘ 0 ‘a‘ 0 ‘alpha‘ pi/2‘‘modified‘ ‘qlim‘ [-130 130].*deg）;
L6 = link（‘d‘ 0.225 ‘a‘ 0 ‘alpha‘ -pi/2‘modified‘‘qlim‘ [-210 210].*deg）;
robot=Seriallink（[L1L2L3L4L5L6]‘name‘‘安川ms165‘）;  %Seriallink类函数
robot.display（）;%link类函数，显示建立机器人DH参数
robot.plot（[0 0 0 0 0 0]）;%初始位置
q1=mymodikine（R11）;
q2=mymodikine（R22）;
q11=q1（2:）;
q22=q2（2:）;
%q1=robot.ikine（R11）;%计算机器人逆运动学，关节坐标向量
%q2=robot.ikine（R22）;
t= linspace（0211）;%步长11，等分10段
g=jtraj（q11q22t）;%五次多项式规划轨迹
T=robot.fkine（g）;%正运动学计算轨迹每个点的位姿
robot.plot（g）%显示机器人轨迹
teach（robot）;

%% 计算轨迹长度
long=0;
i=1;
for i = 1:11
    x=T（14i）;
    y=T（24i）;
    z=T（34i）;
    long=long+sqrt（x^2 + y^2 + z^2）;
    i=i+1;
end

%% 旋转矩阵转欧拉角
for j= 1:11
    Eul=rotm2eul（T（1:31:3j） ‘ZYX‘）;
    Eo=Eul.*（180/pi）;
    a=T（1:34j）‘;
    save（j:）=[a Eo ];
    j=j+1;
end

 属性            大小     日期    时间   名称
----------- ---------  ---------- -----  ----

     文件       1775  2019-09-25 16:20  jtraj\guiji.m

     文件      11202  2019-09-24 18:15  jtraj\handian.xlsx

     文件       2769  2019-09-25 16:18  jtraj\myfkine.m

     文件       6534  2019-09-25 16:30  jtraj\mymodikine.m

    ..A..H.       165  2019-09-23 10:14  jtraj\~$handian.xlsx

     目录          0  2019-09-25 18:10  jtraj

----------- ---------  ---------- -----  ----

                22445                    6