傳統(tǒng)的工業(yè)機(jī)器人普遍采用電機(jī) 、齒輪減速器 ,、關(guān)節(jié)軸三者直接連接的傳動(dòng)機(jī)構(gòu),，這種機(jī)構(gòu)要求電機(jī)與減速器安裝在機(jī)械臂關(guān)節(jié)附近，其缺點(diǎn)是對(duì)于多關(guān)節(jié)機(jī)械臂,，下一級(jí)關(guān)節(jié)的電機(jī)與減速器等驅(qū)動(dòng)裝置成為上一級(jí)關(guān)節(jié)的額外負(fù)載 ,。這一額外負(fù)載帶來(lái)的負(fù)面影響往往超過(guò)機(jī)械臂連桿等必要結(jié)構(gòu)件，因此提高了對(duì)機(jī)械臂動(dòng)力和驅(qū)動(dòng)元件的要求,，由此造成整體重量 、體積 ,、造價(jià)和內(nèi)部消耗的增加,，降低了機(jī)械臂對(duì)外做功的能力和效率。為了避免這一問(wèn)題許多主動(dòng)式醫(yī)療機(jī)器人采用繩驅(qū)動(dòng)方式,，使用柔性繩索遠(yuǎn)距離的傳遞運(yùn)動(dòng)和力矩,。將驅(qū)動(dòng)裝置安裝到遠(yuǎn)離相關(guān)關(guān)節(jié)的基座上，使得整個(gè)機(jī)械臂的結(jié)構(gòu)緊湊?,，在減輕機(jī)械臂整體重量的同時(shí)提高了對(duì)外做功的能力,。下圖所示的就是達(dá)芬奇手術(shù)機(jī)器人的末端夾子，采用繩驅(qū)動(dòng)方式可以在較小的機(jī)械結(jié)構(gòu)內(nèi)實(shí)現(xiàn)多自由度的靈活運(yùn)動(dòng)：

下面嘗試在VREP中使用力反饋設(shè)備Phantom omni來(lái)控制醫(yī)療機(jī)器人的末端夾子,。機(jī)構(gòu)簡(jiǎn)圖如下圖所示,，一共有5個(gè)自由度：其中移動(dòng)自由度負(fù)責(zé)進(jìn)給，一個(gè)整體旋轉(zhuǎn)自由度，還有兩個(gè)左右,、上下彎曲的自由度,，最后是控制夾子張合的自由度。

刪除Solidworks CAD模型中的一些不必要特征,，比如倒角,、內(nèi)部孔、螺紋孔等,，導(dǎo)出成STL文件,，再導(dǎo)入到VREP中。然后還需要進(jìn)行繼續(xù)化簡(jiǎn),，減少網(wǎng)格數(shù)量,，當(dāng)網(wǎng)格數(shù)減少到不影響幾何外觀就可以了。接下來(lái)在相應(yīng)的位置添加關(guān)節(jié),，設(shè)置關(guān)節(jié)運(yùn)動(dòng)范圍（軟件限位）,，并將其設(shè)為Inverse kinematics模式。

設(shè)置Calculation Modules中的Inverse kinematics：

搭建好模型后可以先用鍵盤進(jìn)行測(cè)試,，按方向鍵移動(dòng)ikTarget,，VREP會(huì)根據(jù)構(gòu)型自動(dòng)計(jì)算運(yùn)動(dòng)學(xué)逆解，然后將ikTip移動(dòng)到ikTarget處（這樣就會(huì)帶著整個(gè)機(jī)構(gòu)運(yùn)動(dòng)）,。

鍵盤控制的腳本代碼如下：

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?

?

測(cè)試沒(méi)問(wèn)題后可以使用CHAI3D插件來(lái)連接力反饋設(shè)備,。這里采用增量控制的模式，即計(jì)算當(dāng)前時(shí)刻與前一時(shí)刻手柄位置在X,、Y,、Z方向上的差，然后控制VREP中的ikTarget控制點(diǎn)按相應(yīng)的增量移動(dòng),。注意在VREP中機(jī)器人向前運(yùn)動(dòng)是沿X軸負(fù)方向,、向上運(yùn)動(dòng)是沿Z軸正方向、向右運(yùn)動(dòng)是沿Y軸正方向,，這與CHAI3D中坐標(biāo)系的定義一致（向前推手柄是沿著X軸負(fù)方向...）,，因此可以使用力反饋設(shè)備直觀的控制機(jī)器人的運(yùn)動(dòng)。當(dāng)然如果坐標(biāo)系定義不一致,，需要進(jìn)行簡(jiǎn)單的轉(zhuǎn)換才行,。

下面的代碼在sensing探測(cè)部分會(huì)使用simExtCHAI3D_readPosition來(lái)讀取當(dāng)前操作手柄的位置和按鈕狀態(tài)。按照VREP默認(rèn)設(shè)置,，這部分代碼會(huì)50ms執(zhí)行一次,，這里會(huì)出現(xiàn)一個(gè)問(wèn)題：如果采樣速率太快，會(huì)導(dǎo)致前后兩次采集到的位置數(shù)據(jù)偏差為零（人手的操作頻率沒(méi)那么快,，還來(lái)不及改變位置）,，那么輸出的控制量就一直是零,，這樣就沒(méi)辦法用增量控制的方式來(lái)操控機(jī)器人。解決辦法是延遲幾個(gè)周期再采樣,，等到有足夠大的偏差之后再生成控制量,。還有一個(gè)問(wèn)題是使用CHAI3D返回的數(shù)據(jù)以“米”為單位，而VREP世界中的單位有可能未知,，那么使用增量控制時(shí)需要對(duì)控制量乘一個(gè)比例系數(shù),，避免因操作端微小的移動(dòng)造成從動(dòng)端運(yùn)動(dòng)量過(guò)大，超出關(guān)節(jié)限制（無(wú)法到達(dá)的逆解）,?；蛘呖梢哉{(diào)節(jié)比例系數(shù)，用操作端的大位移來(lái)控制從動(dòng)端的小位移,，實(shí)現(xiàn)精細(xì)控制,。

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?

?

將力反饋設(shè)備手柄移動(dòng)到合適的位置之后就可以按住按鈕開(kāi)始操控機(jī)器人，松開(kāi)按鈕會(huì)停止控制,。如果在VREP虛擬場(chǎng)景中添加其它物體（比如障礙物）,，則還可以模擬環(huán)境中的力（接觸力、重力,、摩擦力,、彈簧力等）讓操控著“感覺(jué)”到。如果實(shí)際機(jī)器人上裝有力傳感器,，則在用Phantom omni控制機(jī)器人的同時(shí)也能讀取力的信息,，反饋給操作者。

下面是使用Phantom omni來(lái)控制機(jī)器人的動(dòng)態(tài)圖,，黃色的軌跡為使用Graph記錄的控制點(diǎn)的空間位置：

對(duì)于該機(jī)構(gòu)也可以自己實(shí)現(xiàn)運(yùn)動(dòng)學(xué)逆解的數(shù)值算法,，下面給出偽逆矩陣法和阻尼最小二乘法的參考：

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?

?