工程师的浪漫：用机械臂画一个爱心

Posted 2021-12-30 FrigidWinter

工程师的浪漫：用机械臂画一个爱心

• 0 写在前面
• 1 生成爱心轨迹
• 2 机械臂逆运动学实现
• 3 实现机械臂画指定轨迹
• • 3.1 读取数据
  • 3.2 绘制机械臂
  • 3.3 反解位姿
• 4 拓展

0 写在前面

🔥本文基于过去的博客📚平面2R机器人(二连杆)运动学与动力学建模+附仿真模型做一个运动学应用，即控制机械臂末端执行特定的轨迹，事实上，在机械臂工作空间内可以绘制任意图形，这里以爱心为例，效果动图如下所示，看完本文相信你也可以做到！

1 生成爱心轨迹

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

x_ = -1.15:0.01:1.15;
y1 = real(1/2*(x_.^2.^(1/3)+(x_.^4.^(1/3)-4*x_.^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;

效果如下：

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

save a2 saveddata

2 机械臂逆运动学实现

根据:平面2R机器人(二连杆)运动学与动力学建模+附仿真模型可知平面2R机器人运动学反解为:
$$\{\begin{array}{l}{\theta }_1=\frac{\pi }{2}-\left(\beta \pm \psi \right),{\theta }_2>0\text{时取}+\\ {\theta }_1=\frac{\pi }{2}+\left(\beta \pm \psi \right),{\theta }_2>0\text{时取}-\\ {\theta }_1=\frac{3\pi }{2}-\left(\beta \pm \psi \right),{\theta }_2>0\text{时取}+\\ {\theta }_1=\frac{3\pi }{2}+\left(\beta \pm \psi \right),{\theta }_2>0\text{时取}-\end{array}$$

运动学正解为

$${}_T^B\mathbf{T}\left(\mathbf{\theta }\right)=\left[\begin{array}{cccc}\cos \left({\theta }_1+{\theta }_2\right)& -\sin \left({\theta }_1+{\theta }_2\right)& 0& l\sin \left({\theta }_1+{\theta }_2\right)+l\sin {\theta }_1\\ \sin \left({\theta }_1+{\theta }_2\right)& \cos \left({\theta }_1+{\theta }_2\right)& 0& -l\cos \left({\theta }_1+{\theta }_2\right)-l\cos {\theta }_1\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

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

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% @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)/(2*L1)));
beta = abs(atan(y./x));
theta2 = acos((x.^2+y.^2)/(2*L1^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

实现了逆运动学后只需要根据轨迹点反解当前机械臂的位姿参数并记录即可。

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  &
以上是关于工程师的浪漫：用机械臂画一个爱心的主要内容，如果未能解决你的问题，请参考以下文章
 
 用代码写出浪漫__合集（pythonmatplotlibMatlabjava绘制爱心玫瑰花前端特效玫瑰爱心）
 女朋友生日如何浪漫送她一个3D音乐爱心相册
 花式浪漫爱心代码，爱心树来啦
 双十一特辑爱心代码（程序员的浪漫）-李峋
 爱心代码编程用啥软件
 HTML爱心代码 | 一起体验理工男的极致浪漫（电视剧男主同款）