机械臂画一个爱心

1 生成爱心轨迹
定义二连杆机械臂的臂长为1，则工作空间应在半径为2的圆内。

x_ = -1.15:0.01:1.15;
y1 = real(1/2*(x_.^2.(1/3)+(x_.^4.(1/3)-4x_.^2+4).(1/2)));
y2 = real(1/2(x_.^2.(1/3)-(x_.^4.(1/3)-4*x_.^2+4).(1/2)));
x = [x_,fliplr(x_)];
y = [y1,y2];
plot(x,y);
saveddata.x = x;
saveddata.y = y;
1
2
3
4
5
6
7
8
效果如下：

将其保存为.mat文件以便复用：

save a2 saveddata
1
2 机械臂逆运动学实现
根据:平面2R机器人(二连杆)运动学与动力学建模+附仿真模型可知平面2R机器人运动学反解为:
{ θ 1 = π 2 − ( β ± ψ ) , θ 2 > 0 时取 + θ 1 = π 2 + ( β ± ψ ) , θ 2 > 0 时取 − θ 1 = 3 π 2 − ( β ± ψ ) , θ 2 > 0 时取 + θ 1 = 3 π 2 + ( β ± ψ ) , θ 2 > 0 时取 −
⎧⎩⎨⎪⎪⎪⎪⎪⎪⎪⎪θ1=π2−(β±ψ),θ2>0时取+θ1=π2+(β±ψ),θ2>0时取−θ1=3π2−(β±ψ),θ2>0时取+θ1=3π2+(β±ψ),θ2>0时取−
{θ1=π2−(β±ψ),θ2>0时取+θ1=π2+(β±ψ),θ2>0时取−θ1=3π2−(β±ψ),θ2>0时取+θ1=3π2+(β±ψ),θ2>0时取−
⎩
⎪
⎪
⎪
⎨
⎪
⎪
⎪
⎧
​

θ
1
​

2
π
​
−(β±ψ),θ
2
​

0时取+
θ
1
​
=
2
π
​
+(β±ψ),θ
2
​
0时取−
θ
1
​
=
2
3π
​
−(β±ψ),θ
2
​
0时取+
θ
1
​
=
2
3π
​
+(β±ψ),θ
2
​
0时取−
​

运动学正解为

T B T ( θ ) = [ cos ⁡ ( θ 1 + θ 2 ) − sin ⁡ ( θ 1 + θ 2 ) 0 l sin ⁡ ( θ 1 + θ 2 ) + l sin ⁡ θ 1 sin ⁡ ( θ 1 + θ 2 ) cos ⁡ ( θ 1 + θ 2 ) 0 − l cos ⁡ ( θ 1 + θ 2 ) − l cos ⁡ θ 1 0 0 1 0 0 0 0 1 ] _{T}^{B}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =\left[
cos(θ1+θ2)sin(θ1+θ2)00−sin(θ1+θ2)cos(θ1+θ2)000010lsin(θ1+θ2)+lsinθ1−lcos(θ1+θ2)−lcosθ101
cos⁡(θ1+θ2)−sin⁡(θ1+θ2)0lsin⁡(θ1+θ2)+lsin⁡θ1sin⁡(θ1+θ2)cos⁡(θ1+θ2)0−lcos⁡(θ1+θ2)−lcos⁡θ100100001
\right]
T
B
​
T(θ)=
⎣
⎢
⎢
⎡
​

cos(θ
1
​
+θ
2
​
)
sin(θ
1
​
+θ
2
​
)
0
0
​

−sin(θ
1
​
+θ
2
​
)
cos(θ
1
​
+θ
2
​
)
0
0
​

0
0
1
0
​

lsin(θ
1
​
+θ
2
​
)+lsinθ
1
​

−lcos(θ
1
​
+θ
2
​
)−lcosθ
1
​

0
1
​

⎦
⎥
⎥
⎤
​

下面封装一个函数实现上面的模型：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% @file : IKrob.m
% @function : [theta] = IKrob(coord,l)
% brief : 二轴机械臂逆运动学求解函数
% version : 1.0
% input : coord ------------- 笛卡尔空间坐标
% l ------------- 连杆长度
% output: theta ------------- 机械臂关节角
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
function [theta] = IKrob(coord,l)

x = coord(:,1);
y = coord(:,2);
L1 = l(1);
pts = size(x);

fai = abs(acos(sqrt(x.^2+y.2)/(2L1)));
beta = abs(atan(y./x));
theta2 = acos((x.^2+y.2)/(2L1^2)-1);
theta1 = zeros(pts(1), 1);

for k=1:pts(1)
if(x(k,1) >= 0 && y(k,1) >= 0)
theta1(k,1) = pi/2 - (beta(k,1) + fai(k,1));
elseif(x(k,1) < 0 && y(k,1) >= 0)
theta1(k,1) = - pi/2 + (beta(k,1) - fai(k,1));
elseif(x(k,1) < 0 && y(k,1) < 0)
theta1(k,1) = -pi/2 - (beta(k,1) - fai(k,1));
elseif(x(k,1) >= 0 && y(k,1) < 0)
theta1(k,1) = pi/2 + (beta(k,1) - fai(k,1));
end
end
theta = [theta1 theta2];
end
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
实现了逆运动学后只需要根据轨迹点反解当前机械臂的位姿参数并记录即可。

3 实现机械臂画指定轨迹
3.1 读取数据
% 读取轨迹信息
load a2.mat % 轨迹数据对应名称为saveddata，包含坐标数据x,y等，轨迹为手写字母a的轨迹
trajactory_length = size(saveddata.x,2); % 读取轨迹长度
trajcoord = [saveddata.x’,saveddata.y’]; % 读取轨迹坐标
trajcoord(:,1) = trajcoord(:,1) - 1; % 改变一下轨迹的位置，方便机械臂运动
dt = 0.02;
%% 机械臂逆运动学求关节空间轨迹（求解thetaA(包含theta1，theta2)），并作出机械臂运动图（需要求关节1的位置）<---------
thetaA = zeros(trajactory_length,2); % 初始化theta的角度
midtrajA = zeros(trajactory_length,2); % 初始化关节1的位置
1
2
3
4
5
6
7
8
9
3.2 绘制机械臂
figure
Robotarm = VideoWriter(‘Robotarm.avi’); % 新建叫Robotarm.avi的文件
open(Robotarm); % 打开Robotarm.avi的文件

axis([-2 1.3 -1.8 1.5]) % 固定坐标轴
hold on
plot(trajcoord(:,1),trajcoord(:,2),‘r-’,‘linewidth’,2); % 画出轨迹
h1 = line([0 midtrajA(1,1)],[0 midtrajA(1,2)],‘LineWidth’,3); % 画杆1
h2 = line([midtrajA(1,1) trajcoord(1,1)],[midtrajA(1,2) trajcoord(1,2)],‘LineWidth’,3); % 画杆2
h3 = plot(midtrajA(1,1),midtrajA(1,2),‘bo’,‘LineWidth’,6); % 画关节1

M=moviein(trajactory_length); % 前面要有plot帮助moviein初始化
1
2
3
4
5
6
7
8
9
10
11
12

3.3 反解位姿
% 计算逆运动学
theta = IKrob(trajcoord, l); % 解出对应的关节角 <------------------
midtrajA = [l(1) * sin(theta(:,1)),l(1) * cos(theta(:,1))];

for k=1:trajactory_length
delete(h1);
delete(h2);
delete(h3);

axis([-2.5 1.3 -1.8 1.5])

h1 = line([0 midtrajA(k,1)],[0 midtrajA(k,2)],‘LineWidth’,3); % 画杆1
h2 = line([midtrajA(k,1) trajcoord(k,1)],[midtrajA(k,2) trajcoord(k,2)],‘LineWidth’,3); % 画杆2
h3 = plot(midtrajA(k,1),midtrajA(k,2),‘bo’,‘LineWidth’,6); % 画关节1

M(:,k)=getframe; % 抓取图形作为电影的画面
writeVideo(Robotarm,M(:,k));

end
% movie(M,1,30); % 以每秒30帧的速度播放1次
close(Robotarm); % 关闭
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21

运行的结果保存在’Robotarm.avi’，直接播放即可。

4 拓展
本项目旨在实践机械臂运动学模型，将图形换成字母也可以实现简单的写字应用，例如：

几