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{cases} \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{cases}$$

运动学正解为

$$_{T}^{B}\boldsymbol{T}\left( \boldsymbol{\theta } \right) =\left[ \begin{matrix} \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{matrix} \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  = zeros(trajactory_length,2);                   % 初始化theta的角度
    midtrajA = zeros(trajactory_length,2);                  % 初始化关节1的位置

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初始化

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);                                            % 关闭

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

4 拓展

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

完整代码关注下方公众号回复 R001 获取

🔥 更多精彩专栏：

• 《机器人原理与技术》
• 《计算机视觉教程》
• 《机器学习》
• 《嵌入式系统》
• 《数值优化方法》
• …

🏠 欢迎加入社区和更多志同道合的朋友交流：AI 技术社