平面二連桿逆運動學仿真  Vrep_matlab

1.創建平面二連桿模型

1.   打開vrep，新建scene。

2. 點擊Add->Dummy,調整Dummy大小、顏色、位置。命名為Base.

   

    

3. 點擊Add->Joint->Revolute，命名為j1。雙擊j1，在scene object properties中將Mode設置為Inverse kinematics mode。length設置為0.15、Diameter設置為0.04。

   

4. 點擊Add->Primitive shape->Cylinder,設置參數X-size為0.05m,Z-size為0.5m。並調整顏色、位置。如下圖。

   

   

    

    

5. 按照上述創建關節與連桿的方式，再創建關節二，參數設置與上述相同。

6. 再按下圖調整各部件的關系圖。

   

7. 添加dummy對，點擊Add添加dummy，命名為tip，並調整其大小、位置。再添加target，同樣設置其參數。最后調整層次結構。配對dummy對，如圖(其實可以不添加)

   

    

    

   至此，模型創建完成。 此處模型購買鏈接，提供咨詢服務。

2.推導平面逆運動學方程

由余弦定理得

                                              

式-1

 

又由三角學知識可得

式2

 

代入上式求得

式3

當時，上式有解。

式4

求解關節角 θ1，得

式5

再次應用余弦定理得

式6

 

其中β∈(0,π)

當

當

此處模型購買鏈接，提供咨詢服務。 

   3.Matlab代碼實現

在matlab中新建函數，函數名為InverseKinematics，添加如下代碼。

function [theta1,theta2] = InverseKinematics(Px,Py,a1,a2)
    %   此處顯示詳細說明
    
    if Px^2+Py^2<=(a1+a2)^2
        
       %關節2角度
       theta2=acos((Px^2+Py^2-a1^2-a2^2)/(2*a1*a2));
       
       %關節1角度
       if (Px<0 && Py>0)
            alaph=-pi+atan(Py/Px);
       elseif (Px<0 && Py<0)
           alaph=pi+atan(Py/Px);
       else
           alaph=atan(Py/Px);
       end
       beta=acos((Px^2+Py^2+a1^2-a2^2)/(2*a1*sqrt(Px^2+Py^2)));
       if  theta2>=0
           
            theta1=alaph-beta;
       else
            theta1=alaph+beta;
       end
       fprintf("alaph: %f°  , beta:  %f  \n",alaph*180/pi,beta*180/pi);

        fprintf("theta1:  %f°, theta2:  %f°\n",theta1*180/pi,theta2*180/pi);
    else
        
        disp('目標點不在工作范圍內！');
        
    end

    
end

添加新腳本

%兩連桿機械臂控制腳本

%建立matlab與vrep通信通道
    disp('Program started');
    % sim=remApi('remoteApi','extApi.h'); % using the header (requires a compiler)
    sim=remApi('remoteApi'); % using the prototype file (remoteApiProto.m)
    sim.simxFinish(-1); % just in case, close all opened connections
    clientID=sim.simxStart('127.0.0.1',19999,true,true,5000,5);
    
%獲取vrep模型參數
     if (clientID>-1)
                disp('Connected to remote API server!!!!');
     end
     a1=0.5;
     a2=0.5;
     %Position=[0,0];
     %取得目標點句柄                                           
     [returnCode,P_handle]=sim.simxGetObjectHandle(clientID,'target',sim.simx_opmode_oneshot_wait);
     %基坐標系句柄
     [returnCode,Base_handle]=sim.simxGetObjectHandle(clientID,'base',sim.simx_opmode_oneshot_wait);
     %取得目標點坐標
     [returnCode,Position]=sim.simxGetObjectPosition(clientID,P_handle,Base_handle,sim.simx_opmode_oneshot_wait);
     Px=Position(1);
     Py=Position(2);
     fprintf("Px: %f  ,Py:  %f \n",Px,Py);
%逆運動學解算
    [theta1,theta2]=InverseKinematics(Px,Py,a1,a2);
%關節角發送至vrep
%關節句柄
 [returnCode,J1_handle]=sim.simxGetObjectHandle(clientID,'j1',sim.simx_opmode_oneshot_wait);
 [returnCode,J2_handle]=sim.simxGetObjectHandle(clientID,'j2',sim.simx_opmode_oneshot_wait);
 %設置關節角度
 sim.simxSetJointPosition(clientID,J1_handle,theta1,sim.simx_opmode_oneshot);
 sim.simxSetJointPosition(clientID,J2_handle,theta2,sim.simx_opmode_oneshot);

 
 

 

視頻觀看地址 https://www.bilibili.com/video/bv1Xg4y1v7pR

vrep Q群 609286340

此處模型購買鏈接，提供咨詢服務。

vrep_matlab平面二連桿逆運動學仿真