Matlab Robotics Toolbox 正逆运动学及轨迹规划

https://www.cnblogs.com/zhongyuliang/p/11077103.html

1 理论知识

理论知识请参考：

• 机器人学导论++（原书第3版）_（美）HLHN+J.CRAIG著++贠超等译
• 机器人学课程讲义（丁烨）
• 机器人学课程讲义（赵言正）

2 机器人建模

本仿真程序仿照fanuc_M20ia机器人进行建模。

2.1 利用DH矩阵建立机器人模型（modified）

经测绘，用如下代码建立DH矩阵
使用robot.teach（）函数，进行机器人示教

clc;
%             theta   d         a        alpha     offset
ML1 =  Link([ 0,      0.4967,   0,       0,        0], 'modified');
ML2 =  Link([ -pi/2,  -0.18804, 0.2,     3*pi/2,   0], 'modified');
ML3 =  Link([ 0,      0.17248,  0.79876, 0 ,       0], 'modified');
ML4 =  Link([ 0,      0.98557,  0.25126, 3*pi/2,   0], 'modified');
ML5 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
ML6 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');

robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','Fanuc M20ia');
robot.teach(); %可以自由拖动的关节角度

2.2 正向运动学与机器人工作空间的求取

正向运动学

串联链式操作器的正向运动学问题，是在给定所有关节位置和所有连杆几何参数的情况下，求取末端相对于基座的位置和姿态。
末端执行器相对于基座的变换矩阵 (_60)T=(_10)T(_21)T(_32)T(_43)T(_54)T(_6^5)T

该变换矩阵是关于6个关节变量${\theta }_i$ 的函数，在给定一组θ下，机器人末端连杆在笛卡尔坐标系里的位置和姿态都可以计算得到。

采用蒙特卡洛法对机器人的工作空间进行仿真分析
机器人末端执行器能够到达的空间位置点的集合构成了其工作空间范围。现在采用蒙特卡洛法对机器人的工作空间进行分析。蒙特卡洛法是一种借助于随机抽样来解决数学问题的数值方法，具体求解步骤如下：
1）在机器人正运动学方程中，可以得到末端执行器在参考坐标系中相对基坐标系的位置向量。
2）根据机器人关节变量取值范围，在MATLAB中生成各关节变量随机值。
θi=θimin+（θimax-θimin）×RAND（N,1）
3）将所有关节变量的随机值代入运动学方程的位置向量中从而得到由随机点构成的云图，就构成了机器人的蒙特卡洛工作空间。

代码如下：

%ShowWorkspace.m

clear;
clc;
%             theta   d         a        alpha     offset
ML1 =  Link([ 0,      0.4967,   0,       0,        0], 'modified');
ML2 =  Link([ -pi/2,  -0.18804, 0.2,     3*pi/2,   0], 'modified');
ML3 =  Link([ 0,      0.17248,  0.79876, 0 ,       0], 'modified');
ML4 =  Link([ 0,      0.98557,  0.25126, 3*pi/2,   0], 'modified');
ML5 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');
ML6 =  Link([ 0,      0,        0,       pi/2 ,    0], 'modified');

robot = SerialLink([ML1 ML2 ML3 ML4 ML5 ML6],'name','Fanuc M20ia');
robot.plot([0,0,0,0,0,0]);
hold on;

N=3000;                                              %随机次数
theta1min = -180/180*pi; theta1max = 180/180*pi;
theta2min = -180/180*pi; theta2max = 0/180*pi;
theta3min = -90/180*pi;  theta3max = 90/180*pi;
theta4min = -180/180*pi; theta4max = 180/180*pi;
theta5min = -135/180*pi; theta5max = 135/180*pi;
theta6min = -180/180*pi; theta6max = 180/180*pi;

theta1=theta1min+(theta1max-theta1min)*rand(N,1); %关节1限制
theta2=theta2min+(theta2max-theta2min)*rand(N,1); %关节2限制
theta3=theta3min+(theta3max-theta3min)*rand(N,1); %关节3限制
theta4=theta4min+(theta4max-theta4min)*rand(N,1); %关节4限制
theta5=theta5min+(theta5max-theta5min)*rand(N,1); %关节5限制
theta6=theta6min+(theta6max-theta6min)*rand(N,1); %关节6限制

for n=1:N
q