)。
[0060] 另外，將第一關(guān)節(jié)部141的關(guān)節(jié)角度設(shè)為Θ 1，第二關(guān)節(jié)部142的關(guān)節(jié)角度設(shè)為 0 2,第Ξ關(guān)節(jié)部143的關(guān)節(jié)角度設(shè)為03。同樣，將第四關(guān)節(jié)部144的關(guān)節(jié)角度設(shè)為04,第 五關(guān)節(jié)部145的關(guān)節(jié)角度設(shè)為Θ 5,第六關(guān)節(jié)部146的關(guān)節(jié)角度設(shè)為Θ e，第屯關(guān)節(jié)部147的 關(guān)節(jié)角度設(shè)為Θ 7。
[0061] 此外，本實施方式中，在后述的控制裝置2的計算中，用一個點模擬肩部132。良P， 假定第一關(guān)節(jié)部141、第二關(guān)節(jié)部142、第Ξ關(guān)節(jié)部143在一個點處交叉。同樣，在后述的控 制裝置2的計算中，用一個點模擬手腕部136。目P，假定第五關(guān)節(jié)部145、第六關(guān)節(jié)部146、第 屯關(guān)節(jié)部147在一個點處交叉。在該情況下，可W認為肩部132的Ξ個關(guān)節(jié)（第一關(guān)節(jié)部 141、第二關(guān)節(jié)部142、第Ξ關(guān)節(jié)部143) W及手腕部136的Ξ個關(guān)節(jié)（第五關(guān)節(jié)部145、第六 關(guān)節(jié)部146、第屯關(guān)節(jié)部147)分別構(gòu)成假設(shè)球面構(gòu)造。并且，本實施方式的機械臂120具有 7個關(guān)節(jié)，因此是具有7自由度的機械手。
[0062] 運里，定義W肩部132為原點0的坐標系。將通過原點0并W前側(cè)（軀體前表面 112側(cè)）方向（從圖1的紙面里側(cè)朝近前側(cè)的方向）為正方向的坐標軸設(shè)為X軸。另外，將 通過原點0并W從軀體的右側(cè)朝向左側(cè)的方向（圖1的從左向右的方向）為正方向的坐標 軸設(shè)為y軸。另外，將通過原點0并W朝向上側(cè)的方向（圖1的上方向）為正方向的坐標 軸設(shè)為Z軸。
[0063] 控制裝置2具有例如作為計算機的功能?？刂蒲b置2可W搭載于機器人100的內(nèi) 部，也可W與機器人100 W能夠經(jīng)由有線或者無線通信的方式連接?？刂蒲b置2控制機器 人100的動作、特別是機械臂120的動作。目P，在機器人系統(tǒng)1中，控制裝置2具有作為主 動裝置的功能，機器人100 (機械臂120)具有作為從動裝置的功能。運里，控制裝置2決定 機械臂120的各關(guān)節(jié)角度W避免機械臂120與軀體110等障礙物干設(shè)。詳細內(nèi)容后述。 W64] 運里，對本實施方式中使用的"臂角度"運一指標進行說明。"臂角度"在文獻"考 慮了關(guān)節(jié)的可動范圍的7自由度冗余機械手的解析逆運動學解法"（日本口術(shù)ッh學會誌 ￥〇1.25齡.4，口口.606~617,2007)中記載。^下，簡單地進行說明。 陽0化]一般地，空間內(nèi)的機械手的手部的位置W及姿勢可由6個參數(shù)唯一地表現(xiàn)。因此， 在7自由度機械手中，存在一個多余的自由度。由于該冗余自由度，在7自由度機械手中， 能夠進行不影響手部的位置W及姿勢的機構(gòu)內(nèi)部的1自由度的自運動。運里，如上所述，本 實施方式所設(shè)及的機械臂120具有7自由度。因此，本實施方式所設(shè)及的機械臂120具有 一個冗余自由度。在本實施方式中，使用"臂角度"運一指標作為表示該冗余自由度的參數(shù)。
[0066] 圖2是用于說明實施方式1所設(shè)及的臂角度的圖。如上所述，肩部132的Ξ個關(guān) 節(jié)W及手腕部136的Ξ個關(guān)節(jié)分別構(gòu)成假想的球面構(gòu)造。由此，即便指定手部126的位置 W及姿勢，并固定機械臂120的兩端即肩部132的位置（點0)和手部126的位置（點巧， 由于冗余自由度，肘部134的位置（點e)也會變化。此外，W下，手部126的位置假想地用 點表示。目P，假定手腕部136 W及手部126的位置彼此相同。
[0067] 如圖2所示，若指定手部126的位置（點P)，則能夠確定W肩部132 (點0)、肘部 134 (點e)、手部126 (點巧為頂點、W上臂部122、前臂部124、線段0P為Ξ邊的Ξ角形0 - e-P。此時，Ξ角形0-e-P能夠W線段0P為中屯、如箭頭A所示那樣旋轉(zhuǎn)。將像運樣 W線段0P為中屯、而使肘部134的位置（點e)旋轉(zhuǎn)的情況下的肘部134的位置（點e)的 旋轉(zhuǎn)角度設(shè)為臂角度Φ。另外，若在肘部134的位置位于預先決定的參照平面上的情況下 將該位置設(shè)為點e。，則Ξ角形0 - e。一 P的臂角度為Φ = 0。而且，將從點P觀察點0而 使點e順時針旋轉(zhuǎn)的方向（箭頭A的方向）設(shè)為臂角度的正方向。換言之，按照使大拇指 沿從點P朝向點0的方向豎起的右手法則來定義臂角度的正方向。若使臂角度Φ沿正方 向旋轉(zhuǎn)，則肘部134的位置e從Φ = 0的點e。起向e 1、62、移動。 W側(cè)另外，在像運樣W線段0Ρ為中屯、而使肘部134的位置（點e)旋轉(zhuǎn)的情況下，上臂 部122 W及前臂部124所描繪的軌跡分別是手部126的位置（點巧被指定的情況下的上 臂部122 W及前臂部124的可動區(qū)域。上臂部122 W及前臂部124的可動區(qū)域由W使肘部 134旋轉(zhuǎn)而得的軌跡所形成的圓作為底面、W上臂部122 W及前臂部124旋轉(zhuǎn)而得的軌跡所 形成的旋轉(zhuǎn)面分別作為側(cè)面的圓錐形成。
[0069] 圖3是示出實施方式1所設(shè)及的控制裝置2的結(jié)構(gòu)的功能框圖?？刂蒲b置2作 為主要的硬件結(jié)構(gòu)具有CPU(Central Processing Unit,中央處理單元）4、R〇]VKRead Only Memo巧，只讀存儲器）6、RAM(Random Access Memo巧，隨機訪問存儲器）8。CPU 4具有作為 進行控制處理W及運算處理等的運算裝置的功能。ROM 6具有用于存儲由CPU 4執(zhí)行的控 制程序W及運算程序等的功能。RAM 8具有用于暫時存儲處理數(shù)據(jù)等的功能。
[0070] 另外，控制裝置2具有信息存儲部12、手部位置決定部14、可動區(qū)域計算部16、交 點計算部18、干設(shè)避免范圍決定部20、關(guān)節(jié)角度計算部22 W及機械臂控制部24 ( W下稱為 "各構(gòu)成要素"）。信息存儲部12、手部位置決定部14、可動區(qū)域計算部16、交點計算部18、 干設(shè)避免范圍決定部20、關(guān)節(jié)角度計算部22 W及機械臂控制部24分別具有作為信息存儲 構(gòu)件、手部位置決定構(gòu)件、可動區(qū)域計算構(gòu)件、交點計算構(gòu)件、干設(shè)避免范圍決定構(gòu)件、關(guān)節(jié) 角度計算構(gòu)件W及機械臂控制構(gòu)件的功能。各構(gòu)成要素例如能夠通過CPU 4執(zhí)行存儲于 ROM 6的程序來實現(xiàn)。另外，可W將必要的程序預先記錄于任意的非易失性記錄介質(zhì)（非暫 存性計算機可讀介質(zhì)），并根據(jù)需要來進行安裝（install)。此外，各構(gòu)成要素并不限定于 如上所述通過軟件實現(xiàn)，也可W通過某些電路元件等硬件實現(xiàn)。使用圖4來說明各構(gòu)成要 素的功能。
[0071] 圖4是示出實施方式1所設(shè)及的控制裝置2的處理的流程圖。首先，在步驟S102 中，控制裝置2將機械臂120的控制所需要的信息存儲于信息存儲部12,由此來進行事先設(shè) 定。信息存儲部12存儲上臂部122的長度LiW及前臂部124的長度L 2。另外，信息存儲部 12存儲與軀體110(障礙物區(qū)域）有關(guān)的位置信息。具體而言，信息存儲部12存儲W肩部 132作為原點0的xyz坐標系中的、軀體110的位置信息（坐標數(shù)據(jù)）。更具體地說，信息 存儲部12存儲表示軀體前表面112的位置信息（坐標數(shù)據(jù)）W及表示軀體側(cè)面114的位 置信息（坐標數(shù)據(jù)）。并且，信息存儲部12存儲表示圖1所示的線段AB的位置信息（坐標 數(shù)據(jù)）。該線段AB如后所述作為用于判定干設(shè)的避免的干設(shè)判定線段Ld (第一線）使用。
[0072] 此外，存儲于信息存儲部12的軀體110的坐標數(shù)據(jù)示出考慮了實際的機械臂120 的粗細的、為了檢測干設(shè)而使用的與軀體110對應的障礙物的Ξ維形狀。另外，軀體110的 坐標數(shù)據(jù)示出包絡(luò)實際的軀體110的障礙物區(qū)域的邊界面的坐標。目P，與軀體110對應的 障礙物區(qū)域的坐標數(shù)據(jù)包含與機械臂120的粗細對應的余量（margin)。
[0073] 接下來，在步驟S104中，控制裝置2決定手部126的位置（目標手部位置）。具體 而言，控制裝置2的手部位置決定部14接收操作者使用鍵盤、鼠標或者觸感設(shè)備化aptic device)等的輸入構(gòu)件輸入的目標手部位置的坐標數(shù)據(jù)。由此，手部126的位置W及姿勢被 指定。
[0074] 接下來，在步驟S106中，控制裝置2進行手部126的干設(shè)的判定。運里，S106中 的干設(shè)的判定是手部126是否與軀體110干設(shè)的判定。具體而言，手部位置決定部14根據(jù) 所指定的目標手部位置判定手部126是否與軀體110干設(shè)。例如，手部位置決定部14可W 用多邊形表示手部126 W及軀體110,并使用0BB(化iented Bounding Box:方向包圍盒） 判走手部126是否與軀體110干設(shè)。
[0075] 在產(chǎn)生手部126的干設(shè)的情況下（S106為是），在步驟S140中，控制裝置2進行目 標手部位置的修正處理。具體而言，控制裝置2取消在S102的處理中被指定的目標手部位 置。而且，控制裝置2通知操作者由于在操作者所指定的目標手部位置產(chǎn)生干設(shè)因此要再 次指定目標手部位置。
[0076] 另一方面，當未產(chǎn)生手部126的干設(shè)的情況下（S106為否），在步驟S108中，控制 裝置2計算機械臂120的可動區(qū)域。具體而言，控制裝置2的可動區(qū)域計算部16使用上述 的臂角度Φ，計算上臂部122 W及前臂部124的可動區(qū)域。另外，此時計算的"可動區(qū)域" 并不考慮機械臂120與周圍障礙物之間的干設(shè)。W下，對可動區(qū)域計算部16中的計算方法 進行說明。
[0077] 圖5是用于說明實施方式1所設(shè)及的可動區(qū)域的計算方法的圖。在S104中，假設(shè) 手部126的位置P被指定為P。技，Υ，Ζ]τ?？蓜訁^(qū)域計算部16如圖5的（A)所示，在保持肩 部132的位置（原點0)與手部126的位置Ρ之間的距離
I不變的 情況下，使手部126的位置Ρ位于Ζ軸的鉛垂下方的位置（點Ρ')。此時，點Ρ'的坐標為
詐且，可動區(qū)域計算部16使肘部134的位置e位 于XZ平面上的X < 0的區(qū)域的位置（e')。由此，W肩部132 (點0)、肘部134(點e)、手部 126(點巧作為頂點的Ξ角形0 - e - P(0 - e' 一 P'）存在于XZ平面上。
[007引而且，可動區(qū)域計算部16如圖5的做所示，W使得手部126的位置P與在S104 的處理中被指定的手部位置P。一致的方式，使Ξ角形0-e-P繞X軸W旋轉(zhuǎn)角α進行 旋轉(zhuǎn)，繞y軸W旋轉(zhuǎn)角β進行旋轉(zhuǎn)。具體而言，可動區(qū)域計算部16使用W下的式1 W及式 2,將點Ρ' W及點e'分別變換為點Ρ。^及點e。。 W巧](式1)
[0080] P〇=RpR〇P， 陽0川（式。 陽 0間 e〇=RpRne， 陽08引運里，在式1 W及式2中，P'、e'、P?；癳。分別是與點P'、點e'、點P?；包ce。 有關(guān)的位置矢量。另外，R。是表示旋轉(zhuǎn)角α的旋轉(zhuǎn)矩陣。另外，Rp是表示旋轉(zhuǎn)角β的旋 轉(zhuǎn)矩陣。另外，機械臂120的連桿長度化iW及L2)已預先決定，因此能夠計算euW及e'， 能夠根據(jù)上述式1 W及式2,唯一地計算α W及0。
[0084] 可動區(qū)域計算部16將包含圖5的度）所示的旋轉(zhuǎn)后的Ξ角形0 - e。一 ?。（用斜線 表示）的平面設(shè)為作為臂角度的基準的參照平面。目P，可動區(qū)域計算部16假設(shè)當肘部134 的位置e為點e。的情況下臂角度Φ = 0。 陽0化]而且，如圖5的（C)所示，可動區(qū)域計算部16在將肩部132固定于原點0、將手部 126固定于點P。的狀態(tài)下，計算使上臂部122 W及前臂部124繞0P沿箭頭A的方向旋轉(zhuǎn)的 軌跡。該軌跡是手部126的位置（點巧被指定為點P。的情況下的上臂部122 W及前臂部 124的可動區(qū)域。而且，可動區(qū)域由W使肘部134旋轉(zhuǎn)而得的軌跡所形成的圓作為底面、W 由上臂部122 W及前臂部124旋轉(zhuǎn)而得的軌跡所形成的旋轉(zhuǎn)面分別作為側(cè)面的圓錐形成。 換言之，機械臂120的可動區(qū)域由底面相互對接的兩個圓錐形成。
[0086] 圖6是用于定義由實施方式1所設(shè)及的可動區(qū)域計算部16計算出的圓錐的各尺 寸的圖。可動區(qū)域計算部16計算由前臂部124的軌跡形成的圓錐CL(第一圓錐）W及由上 臂部122的軌跡形成的圓錐CU(第二圓錐）各自的尺寸。運里，將線段0P的長度設(shè)為L。。 另外，圓錐CU的底面與圓錐化的底面相同，因此各自的底面即圓的半徑r相同。運里，將 圓錐CU的高度設(shè)為ki，將圓錐化的高度設(shè)為kz。此時，ki+k2=L。。另外，此時，若將從肘 部134的位置e到線段0P所引的垂線的垂足設(shè)為H，則ki= 0H，k 2=閒。此外，圓錐CU是 使Ξ角形0 - e - Η繞線段0H旋轉(zhuǎn)而得的軌跡。同樣，圓錐化是使Ξ角形P - e - Η繞 線段PH旋轉(zhuǎn)而得的軌跡。
[0087] 運里，如上所述，上臂部122的長度LiW及前臂部1