的位置。 陽157] 運(yùn)里，將與關(guān)節(jié)角度θι~Θ 7對(duì)應(yīng)的旋轉(zhuǎn)矩陣分別設(shè)為Ri~R 7。另外，關(guān)節(jié)角度 計(jì)算部22使用W下的式12計(jì)算與使各關(guān)節(jié)角度為0時(shí)的從肩部132觀察的肘部134的位 置對(duì)應(yīng)的位置矢量3Iw、W及與從肘部134觀察的手腕部136的位置對(duì)應(yīng)的位置矢量41。"。 [0158](式 12)
陽161] 另外，手腕部136的位置的決定與手腕部136的各關(guān)節(jié)角度無關(guān)，因此，關(guān)節(jié)角度 計(jì)算部22通過W下的式13計(jì)算與手腕部136的位置對(duì)應(yīng)的位置矢量。 陽 162](式 13) 陽 163]
[0164] 另外，關(guān)于手腕部136的姿勢(shì)，分為肩部132、肘部134化及手腕部136,通過W下 的式14表不。 陽1化](式14)
[0166] °R^= "Rj'R/Ry 陽167] 運(yùn)里，關(guān)節(jié)角度計(jì)算部22使用上述的α、β、φ? W及θρ，根據(jù)式14計(jì)算W下的 式15。 陽 168](式 15) 陽 169] 陽170] 另外，可W使用僅由θιΚ及Θ 2構(gòu)成的化2來計(jì)算位置矢量03。因此，關(guān)節(jié)角度計(jì) 算部22通過W下的式16計(jì)算肘部134的位置矢量63。關(guān)節(jié)角度計(jì)算部22將α、β、Φ t W及θρ代入式16并求解，由此來計(jì)算θιΚ及θ 2。 陽 171](式 16) 陽 172]
陽17引另外，若考慮肩部132的基于3自由度的姿勢(shì)，則如W下的式17所示，第Ξ關(guān)節(jié)部 143的姿勢(shì)仰3)與基于α、β、Φ t W及Θ Ρ的姿勢(shì)一致。因此，關(guān)節(jié)角度計(jì)算部22使用 式17來計(jì)算Θ 3。
[0174](式 17) 陽1巧]
陽176] 并且，若考慮手腕部136的基于3自由度的姿勢(shì)，則如W下的式18那樣來定義4咕。 關(guān)節(jié)角度計(jì)算部22使用式18來計(jì)算0 5~Θ 7。 陽 177](式 18) 陽17引
陽179] 在步驟S134中，控制裝置2使機(jī)械臂120動(dòng)作。具體而言，控制裝置2的機(jī)械臂 控制部24控制機(jī)械臂120的各關(guān)節(jié)W使其成為在S132中計(jì)算出的各關(guān)節(jié)角度。此外，此 時(shí)，機(jī)械臂控制部24可W進(jìn)行機(jī)械臂120的各關(guān)節(jié)的所被允許的動(dòng)作區(qū)域W及速度區(qū)域的 檢查。而且，在不滿足動(dòng)作區(qū)域或者速度區(qū)域的情況下，控制裝置2可W再次重新決定目標(biāo) 臂角度Φ?。
[0180] 本實(shí)施方式中，用圓錐來模擬機(jī)械臂120的可動(dòng)區(qū)域，根據(jù)該圓錐（圓錐化W及 CU)與位于軀體110的表面上的干設(shè)判定線段Ld之間的交點(diǎn)來決定干設(shè)避免范圍。運(yùn)里， 圓錐與線段之間的交點(diǎn)可通過簡(jiǎn)單的幾何學(xué)方法而容易地計(jì)算。因此，本實(shí)施方式中，能夠 高速地決定機(jī)械臂120的干設(shè)避免范圍。 陽181] 作為避免機(jī)械臂的干設(shè)的其它方法，例如有如下的方法：關(guān)于一系列的手臂軌道， 事先設(shè)定位場(chǎng)，并使用該位場(chǎng)來探索可避免干設(shè)的路徑。然而，在運(yùn)樣的數(shù)值解析方法中， 需要進(jìn)行反復(fù)計(jì)算，存在直至解收斂至探索到最終的路徑為止的時(shí)間變得很長的顧慮。另 夕F，還有預(yù)先存儲(chǔ)表示能夠避免干設(shè)的各關(guān)節(jié)角度的表、并隨時(shí)參照該表的方法。然而，在 該方法中，需要事先設(shè)定表，而并非每次計(jì)算能夠避免干設(shè)的解。 陽182] 另一方面，本實(shí)施方式中，如上所述，能夠通過簡(jiǎn)單的幾何學(xué)方法決定機(jī)械臂120 的干設(shè)避免范圍。因此，本實(shí)施方式中，能夠高速地進(jìn)行用于避免機(jī)械臂的干設(shè)的計(jì)算。換 言之，能夠?qū)崟r(shí)地決定機(jī)械臂的干設(shè)避免解。并且，本實(shí)施方式中，能夠高速地（實(shí)時(shí)地） 決定干設(shè)避免解，因此能夠根據(jù)機(jī)器人的每一控制周期決定干設(shè)避免解。并且，由于能夠 高速地決定干設(shè)避免解，因此能夠縮短機(jī)器人的控制周期，由此，能夠提高力控制的響應(yīng)性 能。目P，本實(shí)施方式中，能夠考慮機(jī)械臂120與軀體110之間的干設(shè)避免而高速地求解各關(guān) 節(jié)角度，由此，能夠W高周期進(jìn)行力控制，因此能夠?qū)崿F(xiàn)可進(jìn)行高響應(yīng)的力相互作用的機(jī)器 人。 陽183] 另外，本實(shí)施方式中，在上臂部122的可動(dòng)區(qū)域的軌跡（圓錐CU)、和前臂部124的 可動(dòng)區(qū)域的軌跡（圓錐CL)分別進(jìn)行計(jì)算。通過像運(yùn)樣分別進(jìn)行計(jì)算，能夠簡(jiǎn)單地應(yīng)用計(jì) 算圓錐與線（線段）之間的交點(diǎn)運(yùn)樣的簡(jiǎn)單的計(jì)算方法。因此，能夠更高速地決定機(jī)械臂 120的干設(shè)避免范圍。
[0184] 另外，本實(shí)施方式中，在圓錐的底面X' y'平面上，W使得頂點(diǎn)位于Z'軸上的方式 進(jìn)行坐標(biāo)變換。由此，能夠使用于避免機(jī)械臂120的干設(shè)的計(jì)算更加簡(jiǎn)單。因此，能夠更高 速地決定機(jī)械臂120的干設(shè)避免范圍。
[0185] 另外，本實(shí)施方式中，當(dāng)在圓錐化中干設(shè)判定線段Ld與圓錐化的側(cè)面W及底面 具有交點(diǎn)、在圓錐CU中干設(shè)判定線段Ld與圓錐CU的側(cè)面W及底面具有交點(diǎn)的情況下，將 由干設(shè)避免范圍決定部20判定出的臂角度的存在范圍ΦΙ與Φυ重復(fù)的范圍決定為干設(shè) 避免范圍Φ3。通過形成為運(yùn)樣的結(jié)構(gòu)，即便在臂角度的干設(shè)避免范圍Φ8的一方的邊界角 度前臂部124與軀體110 (軀體110上的干設(shè)判定線段Ld)干設(shè)，在另一方的邊界角度上臂 部122與軀體110 (軀體110上的干設(shè)判定線段Ld)干設(shè)的情況下，也能夠通過簡(jiǎn)單的幾何 學(xué)方法決定干設(shè)避免范圍。因此，能夠更高速地決定機(jī)械臂120的干設(shè)避免范圍。
[0186] 另外，在運(yùn)樣的情況下，使用圓錐化的底面與干設(shè)判定線段Ld之間的交點(diǎn)來計(jì)算 與前臂部124相關(guān)的臂角度的存在范圍ΦΙ的邊界角度（1)1L。同樣，使用圓錐CU的底面與 干設(shè)判定線段Ld之間的交點(diǎn)來計(jì)算與上臂部122相關(guān)的臂角度的存在范圍Φυ的邊界角 度Φ?υ。通過W運(yùn)種方式構(gòu)成，能夠通過簡(jiǎn)單的幾何學(xué)方法，針對(duì)各圓錐化W及CU分別判 定臂角度的存在范圍ΦΙ W及Φυ。因此，能夠更高速地決定機(jī)械臂120的干設(shè)避免范圍。 陽187] 另外，本實(shí)施方式中，可W事先決定最佳臂角度Φ。，并使用該最佳臂角度Φ。決定 目標(biāo)臂角度Φ?。由此，能夠控制機(jī)械臂W避免機(jī)械臂的干設(shè)并且使其接近機(jī)械臂的基準(zhǔn)姿 勢(shì)。由此，能夠W避免機(jī)械臂的干設(shè)并且使機(jī)械臂的動(dòng)作更加自然的方式進(jìn)行控制。
[0188] (實(shí)施方式。
[0189] 接下來，對(duì)實(shí)施方式2進(jìn)行說明。在實(shí)施方式1中，將軀體110示意性地設(shè)為長方 體。另一方面，關(guān)于實(shí)施方式2,將軀體110的角部模擬為曲面形狀，運(yùn)點(diǎn)與實(shí)施方式1不 同。W下，使用附圖對(duì)實(shí)施方式1與實(shí)施方式2的比較進(jìn)行說明。
[0190] 圖15是示出實(shí)施方式1所設(shè)及的、目標(biāo)手部位置在軀體110的角部110a附近的 情況下的機(jī)械臂120的動(dòng)作的俯視圖。另外，圖16是示出實(shí)施方式2所設(shè)及的、目標(biāo)手部 位置在軀體110的角部11化附近的情況下的機(jī)械臂120的動(dòng)作的俯視圖。在圖15 W及圖 16中，示出機(jī)器人100的上側(cè)即從Z軸的正方向觀察的機(jī)器人100。 陽1川如上所述，在實(shí)施方式1中，將軀體110模擬為長方體，因此角部110a例如由大致 呈90度交叉的兩個(gè)平面形成。在運(yùn)樣的情況下，若在角部110a附近使手部126沿箭頭A 方向朝軀體110中央側(cè)稍稍移動(dòng)，則機(jī)械臂120為了避免與軀體110干設(shè)而如箭頭B所示 大幅移動(dòng)。換言之，在實(shí)施方式1中，存在手部126僅稍稍移動(dòng)而機(jī)械臂120卻急劇移動(dòng)的 可能性。在像運(yùn)樣使機(jī)械臂120急劇移動(dòng)情況下，機(jī)械臂120的各關(guān)節(jié)也急劇動(dòng)作。結(jié)果， 存在在各關(guān)節(jié)的馬達(dá)（未圖示）中產(chǎn)生超速W及超載，各關(guān)節(jié)惡化的可能性。 陽192] 另一方面，在實(shí)施方式2中，軀體110的角部11化由曲面形成。因此，如圖16所 示，在使手部126沿箭頭A方向W與圖15相同的變化量移動(dòng)的情況下，如箭頭C所示，機(jī)械 臂120的動(dòng)作變化得W緩和。因此，能夠抑制機(jī)械臂120的各關(guān)節(jié)的馬達(dá)中的超速W及超 載。 陽193] 圖17是示出實(shí)施方式2所設(shè)及的控制裝置2的結(jié)構(gòu)的功能框圖。實(shí)施方式2所 設(shè)及的控制裝置2的硬件構(gòu)成與實(shí)施方式1所設(shè)及的控制裝置2的硬件構(gòu)成實(shí)際相同。另 一方面，實(shí)施方式2所設(shè)及的控制裝置2除了具有實(shí)施方式1所設(shè)及的控制裝置2的各構(gòu) 成要素之外，還具有線段決定部30,運(yùn)點(diǎn)與實(shí)施方式1不同。 陽194] 線段決定部30決定干設(shè)判定線段Ld。即，線段決定部30具有作為決定干設(shè)判定 線段Ld的線段決定構(gòu)件的功能。在實(shí)施方式1中，干設(shè)判定線段Ld始終是線段AB。換言 之，在實(shí)施方式1中，無論手部126的位置如何，干設(shè)判定線段Ld是不變的。另一方面，在 實(shí)施方式2中，如后所述，干設(shè)判定線段Ld根據(jù)手部126的位置而變化。線段決定部30根 據(jù)手部126的位置（例如每當(dāng)手部126的位置被指定時(shí)），決定干設(shè)判定線段Ld。通過像 運(yùn)樣根據(jù)手部126的位置決定干設(shè)判定線段Ld，即便在利用曲面形成角部11化的情況下， 也能夠用與實(shí)施方式1相同的方法，通過簡(jiǎn)單的幾何學(xué)方法容易地決定干設(shè)避免范圍。 陽195] 圖18是示出實(shí)施方式2所設(shè)及的控制裝置2的處理的流程圖。在圖18中，除了 圖4的各處理之外，還追加了步驟S209。如圖18所示，在實(shí)施方式2中，控制裝置2進(jìn)行 與圖4所示的S102~S108分別實(shí)質(zhì)上相同的處理S202~S208。因此，省略S202~S208 的說明。 陽196] 在步驟S209中，控制裝置2決定干設(shè)判定線段Ld。具體而言，控制裝置2的線段 決定部30根據(jù)手部126的位置（目標(biāo)手部位置），針對(duì)圓錐CL (前臂部124的軌跡）W及 圓錐CU (上臂部122的軌跡），分別決定干設(shè)判定線段Ld。針對(duì)圓錐化，線段決定部30將 通過手部126的位置、且包含與角部11化的曲面相切的切線的切點(diǎn)的線，決定為干設(shè)判定 線段L屯。另外，針對(duì)圓錐CU，線段決定部30將通過肩部132的位置、且包含與角部11化的 曲面相切的切線的切點(diǎn)的線，決定為干設(shè)判定線段Ld。。W下具體地進(jìn)行說明。
[0197] 圖19是用于說明決定實(shí)施方式2所設(shè)及的圓錐化（前臂部124的軌跡）的干設(shè) 判定線段L屯的方法的圖。在圖19中，假設(shè)角部11化為由半徑R的圓弧形成的曲面。在手 部126位于位置A的情況下，線段決定部30將通過從位置A朝角部11化的曲面所引的切線 的切點(diǎn)a、且與Z軸平行的線，決定為干設(shè)判定線段L化。同樣，在手部126位于位置B(或 者位置C)的情況下，線段決定部30將通過從位置B (或者位置C)朝角部110b的曲面所引 的切線的切點(diǎn)b (或者切點(diǎn)c)、且與z軸平行的線，決定為干設(shè)判定線段L屯。此外，在手部 126位于位置D的情況下，無法從位置D向角部11化的曲面引切線。在運(yùn)樣的情況下，線段 決定部30將通過角部11化的曲面的端點(diǎn)t且與Z軸平行的線，決定為干設(shè)判定線段L屯。 [0198] 圖20是用于說明實(shí)施方式2所設(shè)及的決定切點(diǎn)的方法的圖。運(yùn)里，在決定切點(diǎn)時(shí)， 忽略Z軸分量。另外，W形成角部11化的曲面的圓弧的中屯、作為原點(diǎn)，W軀體110的前方 向（圖20的紙面上方向）作為X軸正方向，W軀體110的左方向（圖20的紙面左方向） 作為y軸正方向。在運(yùn)樣的坐標(biāo)系中，假設(shè)手部126的位置被決定為化y，hy)。此時(shí)，線段 決定部30按照如下方式?jīng)Q定切點(diǎn)（C,，Cy)。 陽199] 切點(diǎn)佔(zhàn)，Cy)位于半徑R的圓周上，因此，線段決定部30能夠得到W下的式19。 悅 00](式 19) 陽 2〇U Cx2+c/=r2 陽202] 另外，線段決定部30如W下的式20所示計(jì)算表示切線的方程式。 陽203](式 20) 陽 2〇4] CxX+CyY = R2 陽205] 運(yùn)里，切線通過手部位置化X，hy)，因此，線段決定部30將化X，hy)代入式20,從而 得到W下的式21。 陽206](式 21)
[0209] 并且，線段決定部30通過對(duì)將利用式21得到的Cy代入式19而得的C ,的2次方 程式進(jìn)行求解來計(jì)算Cy。并且，線段決定部30將所計(jì)算出的Cy代入式20來計(jì)算Cy。由此， 線段決定部30決定切點(diǎn)（Cy，Cy)。此外，雖然求出多個(gè)切點(diǎn)，但線段決定部30將位于圖20 的粗線部分（X > 0、y《0)的切點(diǎn)作為決定對(duì)象。此外，實(shí)際上xyz坐標(biāo)系的原點(diǎn)是肩部 132的位置，因此線段決定部30針對(duì)所計(jì)算出的切點(diǎn)（Cy，Cy)進(jìn)行用于變換原點(diǎn)位置的坐 標(biāo)變換。 陽210] 圖21是用于說明實(shí)施方式2所設(shè)及的決定圓錐CU(上臂部122的軌跡）上的干 設(shè)判定線段Ld。的方法的圖。線段決定部30將通過從肩部132朝角部11化所引的曲面的 切線的切點(diǎn)e、且與Z軸平行的線決定為干設(shè)判定線段Ld。。運(yùn)里，無論手部126的位置如 何，肩部132的位置是恒定的。因此，線段決定部30可W在手部126的位置被指定前預(yù)先 決定干設(shè)判定線段Ld。。 陽211] 在步驟S210中，控制裝置2的交點(diǎn)計(jì)算部18與圖4所示的S1