【机器人学】1-3.六自由度机器人工作空间 【附MATLAB代码】

前言      

在六自由度机器人正解中，我们已得到六自由度机械臂的正运动学方程，将关节变量代入该方程即可求得机械臂末端的位姿。

实际生活中，机器人的每个关节是有限制的，也就意味着可以得到机械臂末端所能到达的所有位置，我们将这个可能到达的位置的集合称为可达工作空间，而这也是机械臂的一个重要参数指标。

 分析机械臂工作空间的方法有以下三种：几何构建法、解析法和数值计算法。

 蒙特卡洛法就是数值计算法的其中一种，该方法是随机选取大量的采样点，来尽可能构建出机器人完整的工作空间。

蒙特卡洛法

机械臂工作空间的求解方法主要有图解法、解析法和数值法。而图解法和解析法都受关节数目的限制，对于有些机械臂无法准确描述，数值法计算量太大，对于有些边界曲面可靠性得不到保证。因此采用从数值法衍生发展出来的基于随机概率的算法：蒙特卡洛法。

其计算思想如下：

• 机械臂的各关节是在其相应取值范围内工作；
• 所有关节在相应取值范围内随机遍历取值；
• 末端点的所有随机值的集合就构成了机械臂的工作空间

机器人工作空间

机器人的工作空间（workspace）是指当机器人执行所有可能动作时，其末端执行器扫过的总体空间体积。工作空间受限于机器人的几何结构以及各关节上的机械限位。例如六自由度机器人的每个关节都受不同的角度和关节长度的限制，如matlab机器人工具箱中：建立机器人模型的 qlim 函数，对机器人的不同的关节都进行了不同程度上的限位。

机械臂末端执行器以一种以上的方向到达的目标点的集合为可达工作空间。机械臂能以任意方向到达的目标点的集合为灵活工作空间，灵活工作空间是可达工作空间的子集。

废话少说，知道你们喜欢什么，直接上代码。

% 求齐次变换矩阵
function T = DHTrans(alpha, a, d, theta)
T= [cos(theta)             -sin(theta)            0            a;
    sin(theta)*cos(alpha)  cos(theta)*cos(alpha)  -sin(alpha)  -sin(alpha)*d;
    sin(theta)*sin(alpha)  cos(theta)*sin(alpha)  cos(alpha)   cos(alpha)*d;
    0                      0                      0            1];
end

function Rxyz=RotMat_AxisAngle(R)
    theta = acos((R(1,1)+R(2,2)+R(3,3)-1)/2);
    r = 1/2/sin(theta)*[R(3,2)-R(2,3);R(1,3)-R(3,1);R(2,1)-R(1,2)];
    Rxyz=theta*r;
end

% 输入机器人q1,q2,q3,q4,q5,q6的角度
% 输出末端位姿的轴线表示
function position = modelRobot(theta)
th(1) = theta(1); d(1) = 0.1607; a(1) = 0; alp(1) = 0;
th(2) = theta(2); d(2) = 0; a(2) = 0; alp(2) = pi/2;   
th(3) = theta(3); d(3) = 0; a(3) = 0.425; alp(3) = 0;
th(4) = theta(4); d(4) = 0.1133; a(4) = 0.393; alp(4) = 0;
th(5) = theta(5); d(5) = 0.099; a(5) = 0; alp(5) = -pi/2;
th(6) = theta(6); d(6) = 0.0936; a(6) = 0; alp(6) = pi/2;

T01 = DHTrans(alp(1), a(1), d(1), th(1));
T12 = DHTrans(alp(2), a(2), d(2), th(2)+pi/2);
T23 = DHTrans(alp(3), a(3), d(3), th(3));
T34 = DHTrans(alp(4), a(4), d(4), th(4)-pi/2);
T45 = DHTrans(alp(5), a(5), d(5), th(5));
T56 = DHTrans(alp(6), a(6), d(6), th(6));

T06=T01*T12*T23*T34*T45*T56;
axis = RotMat_AxisAngle(T06)/pi*180;
position = [T06(1:3,4:4),axis];
end

clc;clear;
theta1min = -360;theta1max = 360;
theta2min = -360 ;theta2max = 360 ;
theta3min = -360 ;theta3max = 360 ;
theta4min = -360;theta4max = 360;
theta5min = -360;theta5max = 360;
theta6min = -360;theta6max = 360;
% 
n = 300000;
x = zeros;y = zeros;z = zeros;
for i = 1:n

    theta1 = theta1min*(pi/180) + (theta1max-theta1min)*(pi/180)*rand;
    theta2 = theta2min*(pi/180) + (theta2max-theta2min)*(pi/180)*rand;
    theta3 = theta3min*(pi/180) + (theta3max-theta3min)*(pi/180)*rand;
    theta4 = theta4min*(pi/180) + (theta4max-theta4min)*(pi/180)*rand;
    theta5 = theta5min*(pi/180) + (theta5max-theta5min)*(pi/180)*rand;
    theta6 = theta6min*(pi/180) + (theta6max-theta6min)*(pi/180)*rand;

    Tws = modelRobot([theta1,theta2,theta3,theta4,theta5,theta6]);
    x(i) = Tws(1,1);
    y(i) = Tws(2,1);
    z(i) = Tws(3,1);
end
figure('color',[1 1 1]);
plot3(x,y,z,'b.','MarkerSize',0.5)
hold on
xlabel('x轴(millimeter)','color','r','fontsize',15);
ylabel('y轴(millimeter)','color','r','fontsize',15);
zlabel('z轴(millimeter)','color','r','fontsize',15);
grid on

以上4个文件依次运行就可以得到机器人的工作空间啦。

更新： 2024-07-29：感谢评论区小伙伴，x、y、z赋值有误，现已修改。