绘制scara机器人工作空间

Marc Pony

分类：机器人学

发布时间 2021.06.14阅读数 5484 评论数 1

文章目录

一、绘制scara机器人工作空间

二、MATLAB代码

一、绘制scara机器人工作空间

scaraæºå¨äººè¿å¨å¦æ£è§£

如上图，scara机器人大臂长 L 1 ，小臂长 L 2 ， θ 1 为关节1角度， θ 2 为关节2角度，且 θ 1 ∈ [ θ L 1 , θ U 1 ] , θ 2 ∈ [ θ L 2 , θ U 2 ]

å¨è¿éæå¥å¾çæè¿°

scara机器人工作空间由四段圆弧组成(如上图)，圆弧方程如下：

其中，左手系时， θ = θ L 2 ；右手系时， θ = θ U 2 。

二、MATLAB代码绘制2D圆弧：

%{
Function: draw_2d_arc
Description: 绘制平面圆弧
Input: 圆弧圆心(x0, y0),半径r，起始角度theta1(rad)，结束角度theta2(rad), 曲线样式选择options
Output: 无
Author: Marc Pony(marc_pony@163.com)
%}
function draw_2d_arc(x0, y0, r, theta1, theta2, options)
deltaTheta = 0.1 * pi / 180;
theta = theta1 : deltaTheta : theta2;
x = x0 + r * cos(theta);
y = y0 + r * sin(theta);
plot(x, y, 'LineStyle', options.LineStyle, 'Color', options.Color, 'LineWidth', options.LineWidth);
axis equal;
end

绘制scara机器人工作空间：

%{
Function: draw_scara_workspace
Description: 绘制scara机器人工作空间
Input: 大臂L1，小臂L2，关节1限位角度thetaLimit1(rad)，关节2限位角度thetaLimit2(rad)，手系handcoor
Output: 无
Author: Marc Pony(marc_pony@163.com)
%}
function draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
thetaL1 = thetaLimit1(1);
thetaU1 = thetaLimit1(2);
thetaL2 = thetaLimit2(1);
thetaU2 = thetaLimit2(2);

hold on;
if(handcoor == 1) %right handcoor
    options.LineStyle = '-';
    options.Color='g';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 + alpha;
    thetaEnd = thetaU1 + alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1;
    thetaEnd = thetaU1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1;
    thetaEnd = thetaL1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    title('Workspace in right handcoor', 'fontsize', 16);
else  %left handcoor
    options.LineStyle = '-';
    options.Color='b';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 - alpha;
    thetaEnd = thetaU1 - alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1 + thetaL2;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1 + thetaL2;
    thetaEnd = thetaL1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    title('Workspace in left handcoor', 'fontsize', 16);
end
set(gcf, 'color', 'w');
axis off;
end

绘制scara机器人工作空间草图：

%{
Function: draw_scara_workspace_sketch
Description: 绘制scara机器人工作空间草图
Input: 大臂L1，小臂L2，关节1限位角度thetaLimit1(rad)，关节2限位角度thetaLimit2(rad)，手系handcoor
Output: 无
Author: Marc Pony(marc_pony@163.com)
%}
function draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)

thetaL1 = thetaLimit1(1);
thetaU1 = thetaLimit1(2);
thetaL2 = thetaLimit2(1);
thetaU2 = thetaLimit2(2);

hold on;
if(handcoor == 1) %right handcoor
    options.LineStyle = '-';
    options.Color='g';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 + alpha;
    thetaEnd = thetaU1 + alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1;
    thetaEnd = thetaU1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1;
    thetaEnd = thetaL1 + thetaU2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    %-------------
    options.LineStyle = '--';
    options.Color='r';
    options.LineWidth = 0.5;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaU2));
    thetaStart = 0;
    thetaEnd = 2 * pi;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    r = L1 + L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    xA1 = L1 * cos(thetaL1);
    yA1 = L1 * sin(thetaL1);
    xB1 = xA1 + L2 * cos(thetaL1 + thetaU2);
    yB1 = yA1 + L2 * sin(thetaL1 + thetaU2);
    xA2 = L1 * cos(thetaU1);
    yA2 = L1 * sin(thetaU1);
    xB2 = xA2 + L2 * cos(thetaU1 + thetaU2);
    yB2 = yA2 + L2 * sin(thetaU1 + thetaU2);
    xC1 = (L1 + L2) * cos(thetaL1);
    yC1 = (L1 + L2) * sin(thetaL1);
    xC2 = (L1 + L2) * cos(thetaU1);
    yC2 = (L1 + L2) * sin(thetaU1);
    
    plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
    plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
    
    fontsize = 15;
    delta = 25;
    text(0, 0, 'O', 'Fontsize', fontsize);
    text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
    text(xB1, yB1 - delta, 'B_1', 'fontsize', fontsize);
    text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
    text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
    text(xC1, yC1, 'C_1', 'fontsize', fontsize);
    text(xC2, yC2, 'C_2', 'fontsize', fontsize);
    title('Workspace sketch in right handcoor', 'fontsize', 16);
    
else  %left handcoor
    options.LineStyle = '-';
    options.Color='b';
    options.LineWidth = 3;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    alpha = acos((r^2 + L1^2 - L2^2) / (2 * r * L1));
    thetaStart = thetaL1 - alpha;
    thetaEnd = thetaU1 - alpha;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = 0;
    y0 = 0;
    r = L1 + L2;
    thetaStart = thetaL1;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    thetaStart = thetaU1 + thetaL2;
    thetaEnd = thetaU1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    r = L2;
    thetaStart = thetaL1 + thetaL2;
    thetaEnd = thetaL1;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    %-------------
    options.LineStyle = '--';
    options.Color='r';
    options.LineWidth = 0.5;
    
    x0 = 0;
    y0 = 0;
    r = sqrt(L1 * L1 + L2 * L2 + 2 * L1 * L2 * cos(thetaL2));
    thetaStart = 0;
    thetaEnd = 2 * pi;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    r = L1 + L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options)
    
    x0 = L1 * cos(thetaU1);
    y0 = L1 * sin(thetaU1);
    r = L2;
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    x0 = L1 * cos(thetaL1);
    y0 = L1 * sin(thetaL1);
    draw_2d_arc(x0, y0, r, thetaStart, thetaEnd, options);
    
    xA1 = L1 * cos(thetaL1);
    yA1 = L1 * sin(thetaL1);
    xB1 = xA1 + L2 * cos(thetaL1 + thetaL2);
    yB1 = yA1 + L2 * sin(thetaL1 + thetaL2);
    xA2 = L1 * cos(thetaU1);
    yA2 = L1 * sin(thetaU1);
    xB2 = xA2 + L2 * cos(thetaU1 + thetaL2);
    yB2 = yA2 + L2 * sin(thetaU1 + thetaL2);
    xC1 = (L1 + L2) * cos(thetaL1);
    yC1 = (L1 + L2) * sin(thetaL1);
    xC2 = (L1 + L2) * cos(thetaU1);
    yC2 = (L1 + L2) * sin(thetaU1);
    
    plot([0, xA1, xB1], [0, yA1, yB1], 'lineStyle', '-', 'color', 'k', 'lineWidth', 3);
    plot([0, xA2, xB2], [0, yA2, yB2], 'lineStyle', ':', 'color', 'k', 'lineWidth', 3);
    
    fontsize = 15;
    delta = 25;
    text(0, 0, 'O', 'fontsize', fontsize);
    text(xA1, yA1 - delta, 'A_1', 'fontsize', fontsize);
    text(xB1, yB1 + delta, 'B_1', 'fontsize', fontsize);
    text(xA2, yA2 + delta, 'A_2', 'fontsize', fontsize);
    text(xB2, yB2 - delta, 'B_2', 'fontsize', fontsize);
    text(xC1, yC1, 'C_1', 'fontsize', fontsize);
    text(xC2, yC2, 'C_2', 'fontsize', fontsize);
    title('Workspace sketch in left handcoor', 'fontsize', 16);
end
set(gcf, 'color', 'w');
axis off;
end

clc;
clear;
close all;
L1 = 200;
L2 = 200;
thetaLimit1 = [-135, 135] * pi / 180;
thetaLimit2 = [-145, 145] * pi / 180;

%% 画工作空间
figure(1);
handcoor = 0;
draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)
figure(2);
handcoor = 1;
draw_scara_workspace(L1, L2, thetaLimit1, thetaLimit2, handcoor)

%% 画工作空间草图
figure(3);
handcoor = 0;
draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)
figure(4);
handcoor = 1;
draw_scara_workspace_sketch(L1, L2, thetaLimit1, thetaLimit2, handcoor)

机械臂 MATLAB 工作空间 Scara

打赏 0

上一篇：scara机器人雅克比矩阵

下一篇：为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

绘制scara机器人工作空间

Marc Pony

为你推荐

力控机器人物理交互（直接力、阻抗、导纳）控制框架与性能分析

六自由度机器人（机械臂）运动学建模及运动规划系列（四）——轨迹规划

动力学参数辨识流程

机械臂机器人——使用Matlab Robotic ToolBox建立四轴机械臂模型并实现运动控制仿真

八、机器人控制系统设计（第一篇-总体框架设计）

机器人学——学习笔记7(Link Trans Examples & Summarization)

关于作者

Marc Pony

7

0

1

1

为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

scara机器人雅克比矩阵

空间圆弧路径参数化

相关推荐

足式机器人状态估计1-姿态解算

一起来开发一个磁条机器人（五）

点云聚类及匹配（KD-Tree & OCTree）

机器人学导论二

干货 | 机械臂控制第1篇：导论+单关节位置控制

Matlab机器人工具箱（Robotics Toolbox）学习笔记

热门泡泡

30积分失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

绘制scara机器人工作空间

Marc Pony

为你推荐

力控机器人物理交互（直接力、阻抗、导纳）控制框架与性能分析

六自由度机器人（机械臂）运动学建模及运动规划系列（四）——轨迹规划

动力学参数辨识流程

机械臂机器人——使用Matlab Robotic ToolBox建立四轴机械臂模型并实现运动控制仿真

八、机器人控制系统设计（第一篇-总体框架设计）

机器人学——学习笔记7(Link Trans Examples & Summarization)

评论（1）

关于作者

Marc Pony

7

0

1

1

为什么机器人运动学逆解最好采用双变量反正切函数atan2而不用反正/余弦函数？

scara机器人雅克比矩阵

空间圆弧路径参数化

相关推荐

足式机器人状态估计1-姿态解算

一起来开发一个磁条机器人（五）

点云聚类及匹配（KD-Tree & OCTree）

机器人学导论二

干货 | 机械臂控制第1篇：导论+单关节位置控制

Matlab机器人工具箱（Robotics Toolbox）学习笔记

热门泡泡

30积分 失眠，聊聊自己搞ROS的心得体会吧

ros学习路线

30积分 TF_REPEATED_DATA ignoring data错误

各位大佬，有什么ROS定位算法推荐吗

5积分 想买能用ROS2的开发套件。或者开发板

5积分 ros中启动gazebo时报错

给作者打赏

忘记密码

修改头像

添加你感兴趣的标签

举报类型（必选）

举报详情（选填）

30积分失眠，聊聊自己搞ROS的心得体会吧

5积分想买能用ROS2的开发套件。或者开发板