我的订单|我的收藏|我的商城|帮助中心|返回首页
搜维尔[SouVR.com]>服务>VR研究院 孵化器>VR研究院>虚拟交互>haption

使用Phantom omni力反馈设备控制机器人

文章来源:Phantom 作者:frank 发布时间:2018年11月05日 点击数: 字号:

传统的工业机器人普遍采用电机 、齿轮减速器 、关节轴三者直接连接的传动机构,这种机构要求电机与减速器安装在机械臂关节附近,其缺点是对于多关节机械臂,下一级关节的电机与减速器等驱动装置成为上一级关节的额外负载 。这一额外负载带来的负面影响往往超过机械臂连杆等必要结构件,因此提高了对机械臂动力和驱动元件的要求,由此造成整体重量 、体积 、造价和内部消耗的增加,降低了机械臂对外做功的能力和效率。为了避免这一问题许多主动式医疗机器人采用绳驱动方式,使用柔性绳索远距离的传递运动和力矩。将驱动装置安装到远离相关关节的基座上,使得整个机械臂的结构紧凑 ,在减轻机械臂整体重量的同时提高了对外做功的能力。下图所示的就是达芬奇手术机器人的末端夹子,采用绳驱动方式可以在较小的机械结构内实现多自由度的灵活运动:

下面尝试在VREP中使用力反馈设备Phantom omni来控制医疗机器人的末端夹子。机构简图如下图所示,一共有5个自由度:其中移动自由度负责进给,一个整体旋转自由度,还有两个左右、上下弯曲的自由度,最后是控制夹子张合的自由度。

删除Solidworks CAD模型中的一些不必要特征,比如倒角、内部孔、螺纹孔等,导出成STL文件,再导入到VREP中。然后还需要进行继续化简,减少网格数量,当网格数减少到不影响几何外观就可以了。接下来在相应的位置添加关节,设置关节运动范围(软件限位),并将其设为Inverse kinematics模式。

设置Calculation Modules中的Inverse kinematics:

搭建好模型后可以先用键盘进行测试,按方向键移动ikTarget,VREP会根据构型自动计算运动学逆解,然后将ikTip移动到ikTarget处(这样就会带着整个机构运动)。

键盘控制的脚本代码如下:

  1. if (sim_call_type==sim_childscriptcall_initialization) then  
  2.  
  3.     -- Put some initialization code here  
  4.     targetHandle = simGetObjectHandle('ikTarget')  
  5.  
  6.  
  7. end  
  8.  
  9.  
  10. if (sim_call_type==sim_childscriptcall_actuation) then  
  11.  
  12.     -- Put your main ACTUATION code here  
  13.  
  14. end  
  15.  
  16.  
  17.  
  18. if (sim_call_type==sim_childscriptcall_sensing) then  
  19.  
  20.     -- Put your main SENSING code here  
  21.     -- Read the keyboard messages (make sure the focus is on the main window, scene view):  
  22.     message, auxiliaryData = simGetSimulatorMessage()  
  23.  
  24.     if (message == sim_message_keypress) then  
  25.  
  26.         if (auxiliaryData[1]==119) then  
  27.             -- W key  
  28.             local p = simGetObjectPosition(targetHandle, -1)  
  29.             p[1] = p[1] - 0.001  
  30.             simSetObjectPosition(targetHandle, -1, p)  
  31.         end  
  32.         if (auxiliaryData[1]==115) then  
  33.             -- S key  
  34.             local p = simGetObjectPosition(targetHandle, -1)  
  35.             p[1] = p[1] + 0.001  
  36.             simSetObjectPosition(targetHandle, -1, p)  
  37.         end  
  38.  
  39.  
  40.         if (auxiliaryData[1]==2007) then  
  41.             -- up key  
  42.             local p = simGetObjectPosition(targetHandle, -1)  
  43.             p[3] = p[3] + 0.001  
  44.             simSetObjectPosition(targetHandle, -1, p)  
  45.         end  
  46.         if (auxiliaryData[1]==2008) then  
  47.             -- down key  
  48.             local p = simGetObjectPosition(targetHandle, -1)  
  49.             p[3] = p[3] - 0.001  
  50.             simSetObjectPosition(targetHandle, -1, p)  
  51.         end  
  52.  
  53.  
  54.         if (auxiliaryData[1]==2009) then  
  55.             -- left key  
  56.             local p = simGetObjectPosition(targetHandle, -1)  
  57.             p[2] = p[2] - 0.001  
  58.             simSetObjectPosition(targetHandle, -1, p)  
  59.         end  
  60.         if (auxiliaryData[1]==2010) then  
  61.             -- right key  
  62.             local p = simGetObjectPosition(targetHandle, -1)  
  63.             p[2] = p[2] + 0.001  
  64.             simSetObjectPosition(targetHandle, -1, p)  
  65.         end  
  66.  
  67.     end  
  68. end  
  69.  
  70.  
  71. if (sim_call_type==sim_childscriptcall_cleanup) then  
  72.  
  73.     -- Put some restoration code here  
  74.  
  75. end 

 

测试没问题后可以使用CHAI3D插件来连接力反馈设备。这里采用增量控制的模式,即计算当前时刻与前一时刻手柄位置在X、Y、Z方向上的差,然后控制VREP中的ikTarget控制点按相应的增量移动。注意在VREP中机器人向前运动是沿X轴负方向、向上运动是沿Z轴正方向、向右运动是沿Y轴正方向,这与CHAI3D中坐标系的定义一致(向前推手柄是沿着X轴负方向...),因此可以使用力反馈设备直观的控制机器人的运动。当然如果坐标系定义不一致,需要进行简单的转换才行。

下面的代码在sensing探测部分会使用simExtCHAI3D_readPosition来读取当前操作手柄的位置和按钮状态。按照VREP默认设置,这部分代码会50ms执行一次,这里会出现一个问题:如果采样速率太快,会导致前后两次采集到的位置数据偏差为零(人手的操作频率没那么快,还来不及改变位置),那么输出的控制量就一直是零,这样就没办法用增量控制的方式来操控机器人。解决办法是延迟几个周期再采样,等到有足够大的偏差之后再生成控制量。还有一个问题是使用CHAI3D返回的数据以“米”为单位,而VREP世界中的单位有可能未知,那么使用增量控制时需要对控制量乘一个比例系数,避免因操作端微小的移动造成从动端运动量过大,超出关节限制(无法到达的逆解)。或者可以调节比例系数,用操作端的大位移来控制从动端的小位移,实现精细控制。

  1. if (sim_call_type==sim_childscriptcall_initialization) then  
  2.     -- Check if the plugin is loaded:  
  3.     moduleName=0  
  4.     moduleVersion=0  
  5.     index=0  
  6.     pluginNotFound=true 
  7.     while moduleName do 
  8.         moduleName,moduleVersion=simGetModuleName(index)  
  9.         if (moduleName=='CHAI3D') then  
  10.             pluginNotFound=false 
  11.         end  
  12.         index=index+1  
  13.     end  
  14.  
  15.     if (pluginNotFound) then  
  16.         simDisplayDialog('Error','CHAI3D plugin was not found, or was not correctly initialized (v_repExtCHAI3D.dll).',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})  
  17.     else 
  18.  
  19.         -- Start the device:  
  20.         local toolRadius = 0.001 -- the radius of the tool  
  21.         local workspaceRadius = 0.2 -- the workspace radius  
  22.         if simExtCHAI3D_start(0, toolRadius,workspaceRadius) ~= 1 then  
  23.             simDisplayDialog('Error','Device failed to initialize.',sim_dlgstyle_ok,false,nil,{0.8,0,0,0,0,0},{0.5,0,0,1,1,1})  
  24.         else 
  25.             CHAI3DPluginInitialized = true 
  26.  
  27.         end  
  28.     end  
  29.  
  30.     targetHandle = simGetObjectHandle('ikTarget')  
  31.  
  32.     deltaPos = {0, 0, 0}  
  33.     counter = 0  
  34.     ratio = 50  
  35.  
  36. end  
  37.  
  38.  
  39. if (sim_call_type==sim_childscriptcall_actuation) then  
  40.     if buttonState ==1 then  -- press the button  
  41.         local p = simGetObjectPosition(targetHandle, -1) -- get the target position  
  42.         -- add increment of the tool tip  
  43.         p[1] = p[1] + deltaPos[1] / ratio                    
  44.         p[2] = p[2] + deltaPos[2] / ratio  
  45.         p[3] = p[3] + deltaPos[3] / ratio  
  46.         simSetObjectPosition(targetHandle, -1, p)  -- move to the absolute position  
  47.     end  
  48. end  
  49.  
  50.  
  51.  
  52. if (sim_call_type==sim_childscriptcall_sensing) then  
  53.     if CHAI3DPluginInitialized then  
  54.         -- Read the current position of the cursor:  
  55.         local currentToolPosition = simExtCHAI3D_readPosition(0)  
  56.           
  57.         -- Read the buttons of the device:  
  58.         buttonState = simExtCHAI3D_readButtons(0)  
  59.  
  60.         counter = counter + 1      -- increase the counter  
  61.           
  62.         if counter % 30 == 1 then  -- keep the value  
  63.             prevToolPosition = currentToolPosition  
  64.         end  
  65.  
  66.         if counter % 30 == 0 then  -- calculate tool tip increment  
  67.             deltaPos[1] = currentToolPosition[1] - prevToolPosition[1]  -- X-axis increment  
  68.             deltaPos[2] = currentToolPosition[2] - prevToolPosition[2]  -- Y-axis increment  
  69.             deltaPos[3] = currentToolPosition[3] - prevToolPosition[3]  -- Z-axis increment  
  70.             counter = 0  -- reset counter  
  71.  
  72.  
  73.         local info = string.format("CurrentPosition:%.2f,%.2f,%.2f  DeltaPosition:%.2f,%.2f,%.2f",  
  74.                             currentToolPosition[1],currentToolPosition[2],currentToolPosition[3],  
  75.                             deltaPos[1],deltaPos[2],deltaPos[3])  
  76.         simAddStatusbarMessage(info)  
  77.  
  78.         end  
  79.  
  80.     end  
  81. end  
  82.  
  83.  
  84. if (sim_call_type==sim_childscriptcall_cleanup) then  
  85.     if CHAI3DPluginInitialized then  
  86.  
  87.         -- Disconnects all devices and removes all objects from the scene  
  88.         simExtCHAI3D_reset()  
  89.     end  
  90. end 

 

力反馈设备手柄移动到合适的位置之后就可以按住按钮开始操控机器人,松开按钮会停止控制。如果在VREP虚拟场景中添加其它物体(比如障碍物),则还可以模拟环境中的力(接触力、重力、摩擦力、弹簧力等)让操控着“感觉”到。如果实际机器人上装有力传感器,则在用Phantom omni控制机器人的同时也能读取力的信息,反馈给操作者。

下面是使用Phantom omni来控制机器人的动态图,黄色的轨迹为使用Graph记录的控制点的空间位置:

对于该机构也可以自己实现运动学逆解的数值算法,下面给出伪逆矩阵法和阻尼最小二乘法的参考:

  1. import math    
  2. import numpy as np
  3.  
  4. # link length  
  5. L1 = 1    
  6. L2 = 1  
  7.  
  8. gamma = 1       # step size  
  9. lamda = 0.2     # damping constant (DLS-method)  
  10. stol = 1e-3     # tolerance  
  11. nm = 10         # initial error  
  12. count = 0       # iteration count  
  13. ilimit = 20     # maximum iteration
  14.  
  15. # numerical method for inverse kinematics  
  16. method = 'Pseudo Inverse'  # 'Pseudo Inverse''DLS', ...
  17.  
  18. # initial joint value  
  19. q = np.array([0, 0, math.pi/2, 0]) # [theta1, d1, theta2, theta3]
  20.  
  21. # target position    
  22. target_pos = np.array([1, 0, 2])   # [x,y,z]  
  23.  
  24.  
  25. while True:  
  26.     if(nm > stol):
  27.         # forward kinematics:  
  28.         x = np.array([math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3]),\  
  29.         math.cos(q[2])*math.sin(q[0])*(L1+L2*math.cos(q[3]))-L2*math.cos(q[0])*math.sin(q[3]),\  
  30.         q[1]+(L1+L2*math.cos(q[3]))*math.sin(q[2])])
  31.  
  32.         # compute error  
  33.         error = target_pos - x
  34.  
  35.         # compute Jacobian  
  36.         J11 = -math.sin(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.cos(q[0])*math.sin(q[3])  
  37.         J12 = 0  
  38.         J13 = -math.sin(q[2])*math.cos(q[0])*(L1+L2*math.cos(q[3]))  
  39.         J14 = L2*(math.sin(q[0])*math.cos(q[3])-math.cos(q[0])*math.cos(q[2])*math.sin(q[3]))  
  40.         J21 = math.cos(q[0])*math.cos(q[2])*(L1+L2*math.cos(q[3]))+L2*math.sin(q[0])*math.sin(q[3])  
  41.         J22 = 0  
  42.         J23 = -math.sin(q[0])*math.sin(q[2])*(L1+L2*math.cos(q[3]))  
  43.         J24 = -L2*(math.cos(q[0])*math.cos(q[3])+math.sin(q[0])*math.cos(q[2])*math.sin(q[3]))  
  44.         J31 = 0  
  45.         J32 = 1  
  46.         J33 = math.cos(q[2])*(L1+L2*math.cos(q[3]))  
  47.         J34 = -L2*math.sin(q[2])*math.sin(q[3])  
  48.  
  49.         J = np.array([[J11,J12,J13,J14],[J21,J22,J23,J24],[J31,J32,J33,J34]])  
  50.           
  51.         if method == 'Pseudo Inverse'
  52.             # Pseudo Inverse Method  
  53.             J_pseudo = np.dot(J.transpose(), np.linalg.inv(J.dot(J.transpose()))) # compute pseudo inverse  
  54.             dq = gamma * J_pseudo.dot(error)  
  55.               
  56.         if method == 'DLS':
  57.             # Damped Least Squares Method  
  58.             f = np.linalg.solve(J.dot(J.transpose())+lamda**2*np.identity(3), error)  
  59.             dq = np.dot(J.transpose(), f)
  60.  
  61.         # update joint position     
  62.         q = q + dq  
  63.  
  64.         nm = np.linalg.norm(error)  
  65.  
  66.         count = count + 1  
  67.         if count > ilimit:  
  68.             print "Solution wouldn't converge!" 
  69.             break 
  70.     else:
  71.         # print result  
  72.         print 'theta1=' + str(q[0]*180/math.pi)+' d1='+str(q[1]) + ' theta2=' + str((q[2]-math.pi/2)*180/math.pi)+' theta3=' + str(q[3]*180/math.pi)  
  73.         print 'Current position: %.2f, %.2f, %.2f' %(x[0],x[1],x[2])      
  74.         print str(count)+' iterations'+'  err:'+str(nm)  
  75.         break 

 

电话:010-50951355 传真:010-50951352  邮箱:sales@souvr.com ;点击查看区域负责人电话
手机:13811546370 / 13720091697 / 13720096040 / 13811548270 / 13811981522 / 18600440988 /13810279720 /13581546145

  • 暂无资料
  • 暂无资料
  • 暂无资料
  • 暂无资料
  • 暂无资料