平面二连杆逆运动学仿真Vrep_matlab

平面二连杆逆运动学仿真 Vrep_matlab

1.创建平面二连杆模型

打开vrep，新建scene。
点击Add->Dummy,调整Dummy大小、颜色、位置。命名为Base.
点击Add->Joint->Revolute，命名为j1。双击j1，在scene object properties中将Mode设置为Inverse kinematics mode。length设置为0.15、Diameter设置为0.04。
点击Add->Primitive shape->Cylinder,设置参数X-size为0.05m,Z-size为0.5m。并调整颜色、位置。如下图。
按照上述创建关节与连杆的方式，再创建关节二，参数设置与上述相同。
再按下图调整各部件的关系图。
添加dummy对，点击Add添加dummy，命名为tip，并调整其大小、位置。再添加target，同样设置其参数。最后调整层次结构。配对dummy对，如图(其实可以不添加)

至此，模型创建完成。此处模型购买链接，提供咨询服务。

2.推导平面逆运动学方程

由余弦定理得

式-1

又由三角学知识可得

式2

代入上式求得

当时，上式有解。

式4

求解关节角 θ1，得

式5

再次应用余弦定理得

其中β∈(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  
",alaph*180/pi,beta*180/pi);

        fprintf("theta1:  %f°, theta2:  %f°
",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 
",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平面二连杆逆运动学仿真