基于Orion的正运动学matlab仿真及轨迹（附代码）

建立机器人模型

%定义机器人关节连杆参数，默认为标准DH
L1 = Link('d', 0, 'a', 75, 'alpha', pi/2);
L2 = Link('d', 0, 'a', 1101.48, 'alpha', 0);
L3 = Link('d', 0, 'a', 155.77, 'alpha', pi/2,'offset',pi/2);
L4 = Link('d', 483, 'a', 0, 'alpha',-pi/2);
L5 = Link('d', 0, 'a', 0, 'alpha', pi/2);
L6 = Link('d', 445.5, 'a', 0, 'alpha',0);
robot=SerialLink([L1,L2,L3,L4,L5,L6]); %用定义好的关节建立机器人
robot.display();  %显示建立的机器人的DH参数
robot.name = 'little M';
q0=[0 0 0 0 0 0]; 	%6个关节的角度变量值都设为0，可以更改
robot.plot(q0); 	%显示机器人的图像
title('原始位置');

画出机器人运动的动态图

q1=[pi/3,pi/2,pi/6,pi*3/4,pi/2,pi];       %终了位置6个关节点的参数
t=[0:0.1:2]';  %整个运动发生在10秒内、采样周期是50ms
disp('转动过程中的关节点参数：');
[q,qd,qdd]=jtraj(q0,q1,t);%返回末端位移、速度、加速度
T= fkine(robot,q);%前向运动学、返回末端变换矩阵

%画动态图
figure
robot.plot(q) %显示运动动态轨迹
title('机械臂运动动态图');

这里生成的矩阵T，是一个三维特殊格式矩阵，（不同时间的 机器人位姿）暂时不知道如何提取其内容
但是通过这个代码可以得到 末端执行器的三维坐标，P

about T

T [SE3] : 1x21 (2.8 kB)

p = transl(T);%轨迹的位移部分
about p

p [double] : 21x3 (504 bytes)

末端执行器轨迹

p = transl(T);%轨迹的位移部分
x=p(:,1);y=p(:,2);z=p(:,3);
figure('name','末端执行器轨迹图')
plot3(x,y,z);grid on