24的長度L 2已預(yù)先決定。另 夕F，通過將手部126的位置指定為點(diǎn)P。，線段0P的長度也被確定。因此，Ξ角形0 - e - P 與臂角度無關(guān)而被唯一地確定。因此，可動(dòng)區(qū)域計(jì)算部16能夠使用余弦定理計(jì)算；T、ki、k2。 [008引并且，對(duì)于前臂部124的軌跡即圓錐化，可動(dòng)區(qū)域計(jì)算部16 W使得頂點(diǎn)（點(diǎn)巧位 于Z'軸上、底面位于X' y'平面上的方式，進(jìn)行從坐標(biāo)系（X，y，Z)向坐標(biāo)系（X'，y'，Z') 的坐標(biāo)變換。具體而言，可動(dòng)區(qū)域計(jì)算部16使用W下的式3來進(jìn)行針對(duì)圓錐化的坐標(biāo)變 換。
[0089] (式如
[0090]
[0091] 在式3中，通過使用在上述式1 W及式2中使用的旋轉(zhuǎn)矩陣R。W及旋轉(zhuǎn)矩陣R P， W使得線段PH位于Z'軸上的方式變換坐標(biāo)系。運(yùn)相當(dāng)于將圖5的（A)所示的坐標(biāo)系從 (x，y，z)置換為（x'，y'，z')并使前臂部124繞0P旋轉(zhuǎn)而得的軌跡。另外，由于頂點(diǎn)（點(diǎn) 巧位于Z'軸的正向側(cè)，因此坐標(biāo)系的Z軸（Z'軸）的符號(hào)從正變?yōu)樨?fù)。并且，為了使底面 移動(dòng)至X' y'平面（即Z' = 0的平面）上，坐標(biāo)系朝Z'軸方向偏移一ki。由此，圖6所示 的點(diǎn)Η與坐標(biāo)系（X'，y'，Z')的原點(diǎn)0' -致。
[0092] 并且，可動(dòng)區(qū)域計(jì)算部16按照W下的式4計(jì)算變換后的圓錐底面的式，按照W下 的式5計(jì)算變換后的圓錐面（側(cè)面）的式。 陽09引（式4)
[0094] X'2+y'r 2, Z' = 0 陽0巧](式W
[0096] 1^2(χ' 2+y，2) = r2(z' -k2)2, {z' |〇《Z，《1?}
[0097] 同樣，針對(duì)所計(jì)算出的圓錐CU，可動(dòng)區(qū)域計(jì)算部16 W使得頂點(diǎn)（點(diǎn)0)位于z'軸 上、底面位于X' y'平面上的方式，進(jìn)行從坐標(biāo)系（x，y，z)向坐標(biāo)系（x'，y'，z')的坐標(biāo)變 換。具體而言，可動(dòng)區(qū)域計(jì)算部16使用W下的式6進(jìn)行針對(duì)圓錐CU的坐標(biāo)變換。 陽09引（式6)
[0099]
[0100] 式6中，與式3相同，使用在上述式1 W及式2中使用的旋轉(zhuǎn)矩陣R。^及旋轉(zhuǎn)矩 陣Rp，W使得線段0P位于Z'軸上的方式變換坐標(biāo)系。另外，為了使底面移動(dòng)至X' y'平面 (即z' = 0的平面）上，坐標(biāo)系朝Z'軸方向偏移ki。由此，圖6所示的點(diǎn)Η與坐標(biāo)系（X'， y'，Ζ')的原點(diǎn)0' -致。 陽101] 并且，可動(dòng)區(qū)域計(jì)算部16按照上述式4計(jì)算變換后的圓錐底面的式，按照W下的 式7計(jì)算變換后的圓錐面（側(cè)面）的式。 陽 102](式 7) 陽 10引 k/U，2+y，2)二 r2(z，-ki)2, {ζ，0《Z，《kil
[0104] 圖7Α W及圖7Β是示出由實(shí)施方式1所設(shè)及的可動(dòng)區(qū)域計(jì)算部16計(jì)算出的圓錐的 圖。如上，可動(dòng)區(qū)域計(jì)算部16計(jì)算圖7A所示的圓錐化，計(jì)算圖7B所示的圓錐CU。運(yùn)里， 若對(duì)肘部134的位置e位于參照平面的情況下（圖5的度)）的位置e。進(jìn)行坐標(biāo)變換，則變 換后的位置e。'的坐標(biāo)變?yōu)閇-r，0,0]。在該位置，臂角度為Φ =0。另外，臂角度的正方 向?qū)τ趫D7B所示的圓錐CU而言，根據(jù)圖2的箭頭A，從Z'軸的正方向觀察為逆時(shí)針方向。 另一方面，對(duì)于圓錐化而言，伴隨著在坐標(biāo)變換時(shí)Z軸（Z'軸）的符號(hào)從正變?yōu)樨?fù)，臂角度 的正方向如圖7A所示與圓錐CU相反，從Z'軸的正方向觀察為順時(shí)針。
[0105] 接下來，在步驟S110中，控制裝置2計(jì)算可動(dòng)區(qū)域與干設(shè)判定線段Ld之間的交 點(diǎn)。并且，在步驟S112~S120中，控制裝置2使用所計(jì)算出的交點(diǎn)，決定機(jī)械臂120在可 動(dòng)區(qū)域不與軀體110干設(shè)的范圍（干設(shè)避免范圍）。W下，對(duì)本實(shí)施方式中的、使用了臂角 度的決定干設(shè)避免范圍的機(jī)理進(jìn)行說明。
[0106] 如上所述，在指定了手部126的位置的情況下，機(jī)械臂120的可動(dòng)區(qū)域用使臂角度 Φ旋轉(zhuǎn)而得的軌跡即兩個(gè)圓錐CU W及化表示。運(yùn)里，為了避免軀體110與機(jī)械臂120之 間的干設(shè)，只要機(jī)械臂120存在于軀體110的外部即可。換言之，只要圓錐CU的側(cè)面上的 連結(jié)肩部132與肘部134的線段（圓錐CU的母線）、W及圓錐化的側(cè)面上的連結(jié)肘部134 與手部126的線段（圓錐化的母線）不進(jìn)入模擬軀體110的長方體Bd的內(nèi)部即可。即， 只要決定使得圓錐CU的母線W及圓錐化的母線不進(jìn)入長方體Bd的內(nèi)部的臂角度的范圍 即可。 陽107] 圖8A、圖8B W及圖8C是示出實(shí)施方式1所設(shè)及的機(jī)器人100的軀體110與機(jī)械 臂120之間的關(guān)系的圖。在圖8A、圖8B W及圖8C中，示出與手部126的位置對(duì)應(yīng)的、可動(dòng) 區(qū)域（圓錐CUW及CL)和軀體110 (長方體Bd)之間的位置關(guān)系。在手部126的位置被指 定為圖8A所示的位置的情況下，可動(dòng)區(qū)域（圓錐CU W及CL)的一部分進(jìn)入長方體Bd的內(nèi) 部。因此，在該情況下，只要決定可動(dòng)區(qū)域（圓錐CUW及化）中的、沒有進(jìn)入長方體Bd的 內(nèi)部的臂角度的范圍（干設(shè)避免范圍）即可。 陽108] 運(yùn)里，在圖8A所示那樣的情況下，可動(dòng)區(qū)域（圓錐CU W及CL)與軀體側(cè)面114 W 及軀體前表面112雙方相交。由此，可動(dòng)區(qū)域（圓錐CUW及CL)與線段AB(干設(shè)判定線段 Ld)相交。因此，本實(shí)施方式中，使用該可動(dòng)區(qū)域（圓錐CUW及CL)與線段AB(干設(shè)判定線 段Ld)之間的交點(diǎn)來決定干設(shè)避免范圍。
[0109] 另外，在手部126的位置被指定為圖8B所示的位置的情況下，除了肩部132附近 W及手部126的附近之外，可動(dòng)區(qū)域（圓錐CUW及CL)全部進(jìn)入長方體Bd的內(nèi)部。在該 情況下，無法避免軀體110與機(jī)械臂120之間的干設(shè)。因此，認(rèn)定為不存在干設(shè)避免范圍。 此外，在圖8B所示那樣的情況下，可動(dòng)區(qū)域（圓錐CU W及化）不與線段AB (干設(shè)判定線段 Ld)相交。
[0110] 另外，在手部126的位置被指定為圖8C所示那樣的位置的情況下，可動(dòng)區(qū)域（圓 錐CU W及化）也不與線段AB(干設(shè)判定線段Ld)相交。另一方面，在圖8C的情況下，可動(dòng) 區(qū)域（圓錐CU W及CL)全部存在于長方體Bd的外側(cè)、或者可動(dòng)區(qū)域（圓錐CU W及CL)僅 與軀體側(cè)面114相交。因此，在該情況下，根據(jù)可動(dòng)區(qū)域（圓錐CU W及CL)與軀體側(cè)面114 之間的關(guān)系決定干設(shè)避免范圍。 陽111] 對(duì)步驟S110進(jìn)行說明。控制裝置2的交點(diǎn)計(jì)算部18計(jì)算圖7A W及圖7B所示的 圓錐與干設(shè)判定線段Ld (圖1的線段AB)之間的交點(diǎn)。交點(diǎn)計(jì)算部18計(jì)算圓錐化與干設(shè) 判定線段Ld之間的交點(diǎn)，計(jì)算圓錐CU與干設(shè)判定線段Ld之間的交點(diǎn)。換言之，交點(diǎn)計(jì)算 部18針對(duì)圓錐化和圓錐CU分別計(jì)算交點(diǎn)。W下，對(duì)圓錐化和圓錐CU分開進(jìn)行說明。 [0112] 具體而言，交點(diǎn)計(jì)算部18在計(jì)算圓錐化與干設(shè)判定線段Ld之間的交點(diǎn)時(shí)，使用 上述式3,將點(diǎn)A的坐標(biāo)W及點(diǎn)B的坐標(biāo)從坐標(biāo)系（x，y，z)變換為坐標(biāo)系（x'，y'，z')。由 此，交點(diǎn)計(jì)算部18得到對(duì)點(diǎn)A進(jìn)行變換而得的點(diǎn)A'的坐標(biāo)W及對(duì)點(diǎn)B進(jìn)行變換而得的點(diǎn) B'的坐標(biāo)。并且，交點(diǎn)計(jì)算部18按照W下的式8計(jì)算表示干設(shè)判定線段Ld(線段A'B') 上的點(diǎn)I\d的式。 陽 113](式 8) 陽 114] PLd=度，-A，H+A，，{t|0《t《U 陽11引運(yùn)里，在式8中，t為變量。另外，在式8中，Pu為點(diǎn)Pu的位置矢量[X'，y'，Z'] T。同樣，在式8中，A' W及B'分別是點(diǎn)A' W及點(diǎn)B'的位置矢量。交點(diǎn)計(jì)算部18將式8 代入式4 W及式5,計(jì)算所得到的2次方程式中的t的解。當(dāng)存在滿足式4的t的實(shí)數(shù)的情 況下，交點(diǎn)計(jì)算部18判定為存在圓錐CL的底面與干設(shè)判定線段Ld之間的交點(diǎn)。另外，當(dāng) 存在滿足式5的t的實(shí)數(shù)的情況下，交點(diǎn)計(jì)算部18判定為存在圓錐化的側(cè)面（圓錐面） 與干設(shè)判定線段Ld之間的交點(diǎn)。然后，交點(diǎn)計(jì)算部18根據(jù)所得到的t的值來計(jì)算交點(diǎn)。
[0116] 同樣，交點(diǎn)計(jì)算部18在計(jì)算圓錐CU與干設(shè)判定線段Ld之間的交點(diǎn)時(shí)，使用上述 式6,將點(diǎn)A的坐標(biāo)W及點(diǎn)B的坐標(biāo)從坐標(biāo)系（X，y，Z)變換為坐標(biāo)系（X'，y'，Z')。由此， 交點(diǎn)計(jì)算部18得到對(duì)點(diǎn)A進(jìn)行變換而得的點(diǎn)A'的坐標(biāo)W及對(duì)點(diǎn)B進(jìn)行變換而得的點(diǎn)B' 的坐標(biāo)。并且，與圓錐化的情況同樣，交點(diǎn)計(jì)算部18按照上述式8,計(jì)算表示干設(shè)判定線段 Ld(線段A'B')上的點(diǎn)PLd的式。而且，與圓錐化的情況同樣，交點(diǎn)計(jì)算部18將式8代入 式4 W及式7,計(jì)算t的解。當(dāng)存在滿足式4的t的實(shí)數(shù)的情況下，交點(diǎn)計(jì)算部18判定為 存在圓錐CU的底面與干設(shè)判定線段Ld之間的交點(diǎn)。另外，當(dāng)存在滿足式7的t的實(shí)數(shù)的 情況下，交點(diǎn)計(jì)算部18判定為存在圓錐CU的側(cè)面（圓錐面）與干設(shè)判定線段Ld之間的交 點(diǎn)。而且，交點(diǎn)計(jì)算部18根據(jù)所得到的t的值來計(jì)算交點(diǎn)。
[0117] 在步驟S112中，控制裝置2的交點(diǎn)計(jì)算部18判定是否存在可動(dòng)區(qū)域（圓錐CU W 及CL)與線段A'B'(干設(shè)判定線段Ld)之間的交點(diǎn)。當(dāng)判定為不存在交點(diǎn)的情況下（S112 為否），處理進(jìn)入步驟S142。另一方面，當(dāng)判定為存在交點(diǎn)的情況下（S112為是），處理進(jìn)入 步驟S114。
[0118] 在步驟S142中，控制裝置2判定是否存在干設(shè)避免范圍。具體而言，控制裝置2 的干設(shè)避免范圍決定部20判定可動(dòng)區(qū)域（圓錐CU W及CL)與軀體110之間的位置關(guān)系是 圖8C所示的關(guān)系還是圖8B所示的關(guān)系。例如，當(dāng)連結(jié)肩部132和手部126的線段（0巧與 軀體前表面112相交的情況下，干設(shè)避免范圍決定部20可W判定為是圖8B所示的位置關(guān) 系。另一方面，當(dāng)線段0P不與軀體前表面112相交的情況下，干設(shè)避免范圍決定部20可W 判定為是圖8C所示的位置關(guān)系。
[0119] 在判定為不存在干設(shè)避免范圍的情況下（S142為否），即判定為是圖8B的狀態(tài)的 情況下，進(jìn)入處理S140。另一方面，當(dāng)判定為存在干設(shè)避免范圍的情況下（S142為是）、即 判定為是圖8C的狀態(tài)的情況下，在步驟S144中，干設(shè)避免范圍決定部20根據(jù)可動(dòng)區(qū)域（圓 錐CU W及CL)與軀體側(cè)面114之間的關(guān)系，決定能夠避免干設(shè)的臂角度的范圍（干設(shè)避免 范圍Φ3)。在該情況下，干設(shè)避免范圍決定部20根據(jù)可動(dòng)區(qū)域（圓錐CU W及CL)與軀體 側(cè)面114之間的交點(diǎn)，將位于軀體側(cè)面114的外側(cè)的區(qū)域的臂角度的范圍決定為干設(shè)避免 范圍。此外，當(dāng)不存在可動(dòng)區(qū)域（圓錐CU W及CL)與軀體側(cè)面114之間的交點(diǎn)的情況下， 干設(shè)避免范圍決定部20將臂角度的整周決定為干設(shè)避免范圍Φ3。
[0120] 在步驟S114中，干設(shè)避免范圍決定部20判定前臂部124的臂角度的存在范圍（機(jī) 械臂120不與軀體110干設(shè)的臂角度Φ的范圍）。具體而言，干設(shè)避免范圍決定部20判定 是否存在圓錐化的底面與干設(shè)判定線段Ld(線段Α'Β')的交點(diǎn)。并且，干設(shè)避免范圍決 定部20判定圓錐化的側(cè)面（圓錐面）與干設(shè)判定線段Ld之間的交點(diǎn)有幾個(gè)。 陽121] 圖9是例示圖7A所示的圓錐化的側(cè)面與干設(shè)判定線段Ld之間的交點(diǎn)存在兩個(gè) 的情況的圖。在將式8代入式5而得到的t的2次方程式具有兩個(gè)實(shí)數(shù)解的情況下，如圖 9所示，圓錐化的側(cè)面與干設(shè)判定線段Ld(線段A'B')之間的交點(diǎn)存在兩個(gè)。此外，在該 情況下，底面與干設(shè)判定線段Ld之間的交點(diǎn)不存在。 陽122] 干設(shè)避免范圍決定部20取得由交點(diǎn)計(jì)算部18計(jì)算出的交點(diǎn)a' W及b'的坐標(biāo)。 而且，干設(shè)避免范圍決定部20使用交點(diǎn)a'的坐標(biāo)，計(jì)算將交點(diǎn)a'向圓錐化的底面投影 (正射影）而得的映像即點(diǎn)a"。此外，圓錐化為正圓錐、且底面位于xV平面上，因此，當(dāng) 交點(diǎn)a'的坐標(biāo)為[X。'，y。'，Z。' ]的情況下，點(diǎn)a"的坐標(biāo)為[X。'，y/，0]。同樣，干設(shè)避免 范圍決定部20使用交點(diǎn)b'的坐標(biāo)，計(jì)算將交點(diǎn)b'向圓錐化的底面投影（正射影）而得 的映像即點(diǎn)b"。
[0123] 并且，干設(shè)避免范圍決定部20計(jì)算：在圓錐化的底面中，W X'軸的負(fù)方向（通過 點(diǎn)e。' [-r，0,0]的半徑）為基準(zhǔn)的、通過點(diǎn)a"的半徑Ra的角度（1)1L。另夕F，同樣，干設(shè) 避免范圍決定部20計(jì)算：在圓錐化的底面中，W X'軸的負(fù)方向?yàn)榛鶞?zhǔn)的、通過點(diǎn)b"的半 徑化的角度Φ化。運(yùn)里，如使用圖7A上述的那樣，臂角度Φ的基準(zhǔn)點(diǎn)即Φ = 0的點(diǎn)為點(diǎn) e。'。因此，所計(jì)算出的角度（ML W及Φ化成為臂角度Φ的極限角度（邊界角度）。此外， 如上所述，在圓錐化中，臂角度的正方向從Z'軸正方向觀察為順時(shí)針。因此，在點(diǎn)a' W及 點(diǎn)b'位于圖9那樣的位置的情況下，若一π《