Matlab机器人仿真（六）：利用位置描述解算位姿矩阵

利用空间描述得出位姿矩阵p，
一、函数讲解
rpy2tr ：对SE(3)齐次变换的滚-俯仰-偏航角
T = rpy2tr(ROLL, PITCH, YAW, OPTIONS) （Roll:滚角；Pitch：俯仰角；Yaw：摆角）分别绕Z, Y, X轴旋转
以角度计算角度(默认弧度)
options:‘xyz’：绕X, Y, Z轴旋转(用于机器人抓手)；
‘ZYX’:绕Z, Y, X轴旋转(对于移动机器人，默认)；
‘yxz’:绕Y, X, Z轴旋转(用于相机)；
‘arm’:绕X, Y, Z轴旋转(用于机器人手臂)；
‘yxz’:绕Z, Y, X轴旋转(用于移动机器人)；
‘camera’:关于Y, X, Z轴的旋转(对于相机)；
例子：p = transl(30,0,-25)*rpy2tr(0,-pi,-pi);%得出位姿矩阵
（transl:‘30’ x轴数值；‘0’y轴数值；‘-25’z轴数值。rpy2tr:后三轴滚角、俯仰角、摆角。默认最后三轴在同一坐标原点）

二、matlab仿真

%%利用标准D-H法建立多轴机器人
clear;
close all;
clc;
L1 = Link('d', 5, 'a', 5, 'alpha', -pi/2,'offset',0);    %Link 类函数;offset建立初始的偏转角
L2 = Link('d', 0, 'a', 20, 'alpha', 0,'offset',0);
L3 = Link('d', 0, 'a', 5, 'alpha', -pi/2,'offset',0);
L4 = Link('d', 20, 'a', 0, 'alpha', pi/2,'offset',0);
L5 = Link('d', 0, 'a', 0, 'alpha', -pi/2,'offset',0);
L6 = Link('d', 10, 'a', 0, 'alpha', 0,   'offset',0);

L1.qlim = [-pi,pi];%利用qlim设置每个关节的旋转角度范围
robot=SerialLink([L1,L2,L3,L4,L5,L6],'name','S725');   %SerialLink 类函数
% 2、位置描述得出位姿矩阵p
p = transl(30,0,-25)*rpy2tr(0,-pi,-pi);%得出位姿矩阵
q = robot.ikine(p);%得出关节角theta
robot.plot(q);
disp(q);

仿真结果：

注意：机器人是有可达空间的，故坐标系的选取是必须到可达空间的。