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機器人運動學反解為:
{ θ 1 = π 2 ? ( β ± ψ ) , θ 2 > 0 時取 + θ 1 = π 2 + ( β ± ψ ) , θ 2 > 0 時取 ? θ 1 = 3 π 2 ? ( β ± ψ ) , θ 2 > 0 時取 + θ 1 = 3 π 2 + ( β ± ψ ) , θ 2 > 0 時取 ? \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} ??????????θ1?=2π??(β±ψ),θ2?>0時取+θ1?=2π?+(β±ψ),θ2?>0時取?θ1?=23π??(β±ψ),θ2?>0時取+θ1?=23π?+(β±ψ),θ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[ \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] TB?T(θ)=?????cos(θ1?+θ2?)sin(θ1?+θ2?)00??sin(θ1?+θ2?)cos(θ1?+θ2?)00?0010?lsin(θ1?+θ2?)+lsinθ1??lcos(θ1?+θ2?)?lcosθ1?01??????

下面封裝一個函式實作上面的模型：

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% @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 拓展

本專案旨在實踐機械臂運動學模型，將圖形換成字母也可以實作簡單的寫字應用，例如：