br>[0077] 這里，[R_b ' a( ' )] ' T表示R_a ' b的轉(zhuǎn)置矩陣。即，旋轉(zhuǎn)矩陣的尾標(biāo)能夠 通過(guò)對(duì)旋轉(zhuǎn)矩陣進(jìn)行轉(zhuǎn)置而更換，向量的尾標(biāo)能夠通過(guò)旋轉(zhuǎn)矩陣來(lái)更換。其中，以下除了必 要的情況以外，都將表示在坐標(biāo)系a中設(shè)定有坐標(biāo)系b的對(duì)象X的位置的位置向量p_b Λa(-)簡(jiǎn)稱(chēng)為坐標(biāo)系a中的對(duì)象X的位置。例如，將表示對(duì)在拍攝部坐標(biāo)系2_c中所示的 組裝對(duì)象O設(shè)定的組裝對(duì)象坐標(biāo)系Σ_〇的原點(diǎn)的位置的位置向量p_〇 a c (―)簡(jiǎn)稱(chēng)為拍 攝部坐標(biāo)系2_c中的組裝對(duì)象O的位置。
[0078] 與位置的情況相同，以下除了必要的情況以外，都將表示在坐標(biāo)系a中設(shè)定有坐 標(biāo)系b的對(duì)象X的姿勢(shì)的姿勢(shì)向量o_b Λ a(-)簡(jiǎn)稱(chēng)為坐標(biāo)系a中的對(duì)象X的姿勢(shì)。例 如，將對(duì)在拍攝部坐標(biāo)系2_c中所示的組裝對(duì)象O設(shè)定的組裝對(duì)象坐標(biāo)系Σ_〇的姿勢(shì)的 姿勢(shì)向量〇_〇 Λ c(-)簡(jiǎn)稱(chēng)為拍攝部坐標(biāo)系2_c中的組裝對(duì)象O的姿勢(shì)。
[0079] 接下來(lái)，參照?qǐng)D3對(duì)控制裝置30的硬件構(gòu)成進(jìn)行說(shuō)明。圖3是表示控制裝置30 的硬件構(gòu)成的一個(gè)例子的圖?？刂蒲b置30例如具備CPU(Central Processing Unit :中央 處理器）31、存儲(chǔ)部32、輸入受理部33、以及通信部34,經(jīng)由通信部34與拍攝部10、機(jī)械手 20等進(jìn)行通信。這些構(gòu)成要素經(jīng)由總線Bus彼此連接成能夠進(jìn)行通信。CPU31執(zhí)行儲(chǔ)存于 存儲(chǔ)部32的各種程序。
[0080] 存儲(chǔ)部 32 例如包括 HDD(Hard Disk Drive :硬盤(pán)驅(qū)動(dòng)器）、SSD(Solid State Drive:固態(tài)硬盤(pán)）、EEPROM(Electrically Erasable Programmable Read - Only Memory: 帶電可擦可編程只讀存儲(chǔ)器）、ROM (Read - Only Memory :只讀存儲(chǔ)器）、RAM (Random Access Memory:隨機(jī)存取存儲(chǔ)器）等，儲(chǔ)存控制裝置30處理的各種信息、圖像、程序。此 外，存儲(chǔ)部32也可以代替內(nèi)置于控制裝置30的存儲(chǔ)裝置，而是通過(guò)USB等數(shù)字輸入輸出端 口等連接的外置型存儲(chǔ)裝置。
[0081] 輸入受理部33例如是鍵盤(pán)、鼠標(biāo)、觸摸板、其他的輸入裝置。此外，輸入受理部33 也可以作為顯示部發(fā)揮作用，并且還可以構(gòu)成為觸摸面板。
[0082] 通信部34例如構(gòu)成為包括USB等數(shù)字輸入輸出端口、以太網(wǎng)端口等。
[0083] 接下來(lái)，通過(guò)參照?qǐng)D4,來(lái)對(duì)控制裝置30的功能構(gòu)成進(jìn)行說(shuō)明。圖4是表示控制裝 置30的功能構(gòu)成的一個(gè)例子的圖?？刂蒲b置30具備存儲(chǔ)部32、輸入受理部33、圖像取得 部35、以及控制部36。這些功能部中的一部分或者全部例如通過(guò)CPU31執(zhí)行存儲(chǔ)于存儲(chǔ)部 32的各種程序來(lái)實(shí)現(xiàn)。另外，這些功能部中的一部分或者全部也可以是LSI (Large Scale Integration:大規(guī)模集成電路）、ASIC(Application Specific Integrated Circuit:專(zhuān)用 集成電路）等硬件功能部。
[0084] 控制裝置30通過(guò)在視覺(jué)伺服（Visual Servo)的循環(huán)中組裝順應(yīng)運(yùn)動(dòng)控制，來(lái)以 不破壞操作對(duì)象M地成為組裝了操作對(duì)象M和組裝對(duì)象0的狀態(tài)的方式使操作對(duì)象M相對(duì) 于組裝對(duì)象0相對(duì)移動(dòng)。更具體而言，控制裝置30使用視覺(jué)伺服技術(shù)來(lái)控制機(jī)械手20,以 使操作對(duì)象M成為與預(yù)先存儲(chǔ)于存儲(chǔ)部32的模板圖像（例如，操作對(duì)象M和組裝對(duì)象0被 組裝了的狀態(tài)的CAD(Computer Aided Design:計(jì)算機(jī)輔助設(shè)計(jì)）圖像）相同的狀態(tài)。
[0085] 此時(shí)，控制裝置30檢測(cè)由拍攝部10拍攝到的拍攝圖像中的操作對(duì)象M與力傳感 器22的相對(duì)位置關(guān)系，在基于檢測(cè)到的相對(duì)位置關(guān)系，通過(guò)順應(yīng)運(yùn)動(dòng)控制控制機(jī)械手20 時(shí)，逐次決定規(guī)定該動(dòng)作的旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心姿勢(shì)。而且，控制裝置30控制機(jī)械 手20,以便在使機(jī)械手20進(jìn)行使操作對(duì)象M旋轉(zhuǎn)的動(dòng)作時(shí)，以旋轉(zhuǎn)中心位置為中心，使操作 對(duì)象M以旋轉(zhuǎn)中心姿勢(shì)為基準(zhǔn)來(lái)進(jìn)行旋轉(zhuǎn)。另外，控制裝置30控制機(jī)械手20,以便在機(jī)械 手20使操作對(duì)象M平移時(shí)，以旋轉(zhuǎn)中心姿勢(shì)為基準(zhǔn)來(lái)平移。
[0086] 另外，控制裝置30具備未圖示的計(jì)時(shí)部，在由計(jì)時(shí)部計(jì)時(shí)而達(dá)到的時(shí)刻，從圖像 取得部35取得由拍攝部10拍攝到的拍攝圖像，在與此相同的時(shí)刻，從機(jī)械手20的力傳感 器22取得表示由力傳感器22檢測(cè)到的力以及力矩的信息。
[0087] 圖像取得部35取得由拍攝部10拍攝到的拍攝圖像。圖像取得部35向控制部36 輸出取得到的拍攝圖像。
[0088] 控制部36具備目標(biāo)導(dǎo)出部39、旋轉(zhuǎn)中心位置計(jì)算部41、外力計(jì)算部43、負(fù)荷緩和 動(dòng)作量計(jì)算部45、移動(dòng)動(dòng)作量計(jì)算部46、動(dòng)作結(jié)束判定部47、以及機(jī)械手控制部49。
[0089] 目標(biāo)導(dǎo)出部39根據(jù)由圖像取得部35取得的拍攝圖像，檢測(cè)拍攝部坐標(biāo)系2_c中 的組裝對(duì)象〇的位置以及姿勢(shì)。
[0090] 另外，目標(biāo)導(dǎo)出部39基于拍攝部坐標(biāo)系Σ_(：中的組裝對(duì)象0的位置以及姿勢(shì)，來(lái) 導(dǎo)出組裝結(jié)束后的拍攝部坐標(biāo)系2_c中的操作對(duì)象M的位置以及姿勢(shì)。目標(biāo)導(dǎo)出部39在 進(jìn)行該導(dǎo)出時(shí)，基于上述的模板圖像來(lái)檢測(cè)組裝結(jié)束后的操作對(duì)象M的位置以及姿勢(shì)。以 下，將目標(biāo)導(dǎo)出部39導(dǎo)出的組裝結(jié)束后的拍攝部坐標(biāo)系2_c中的操作對(duì)象M的位置以及 姿勢(shì)稱(chēng)為目標(biāo)位置以及姿勢(shì)。
[0091] 旋轉(zhuǎn)中心位置計(jì)算部41根據(jù)由圖像取得部35取得到的拍攝圖像，來(lái)檢測(cè)拍攝部 坐標(biāo)系2_c中的操作對(duì)象M的當(dāng)前位置以及姿勢(shì)。另外，旋轉(zhuǎn)中心位置計(jì)算部41檢測(cè)工 具坐標(biāo)系Σ_?中的拍攝部10的位置以及姿勢(shì)。另外，旋轉(zhuǎn)中心位置計(jì)算部41基于檢測(cè)到 的拍攝部坐標(biāo)系2_c中的操作對(duì)象M的當(dāng)前位置以及姿勢(shì)、工具坐標(biāo)系Σ_?中的拍攝部 10的位置以及姿勢(shì)、以及由用戶(hù)預(yù)先設(shè)定的操作對(duì)象坐標(biāo)系Σ_πι中的旋轉(zhuǎn)中心位置以及 旋轉(zhuǎn)中心姿勢(shì)，來(lái)計(jì)算工具坐標(biāo)系Σ_?中的旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心姿勢(shì)。
[0092] 另外，旋轉(zhuǎn)中心位置計(jì)算部41根據(jù)正向運(yùn)動(dòng)學(xué)來(lái)計(jì)算作業(yè)坐標(biāo)系Σ_?中的力傳 感器22的位置以及姿勢(shì)。另外，旋轉(zhuǎn)中心位置計(jì)算部41基于計(jì)算出的工具坐標(biāo)系Σ_?中 的旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心姿勢(shì)、和作業(yè)坐標(biāo)系Σ_?中的力傳感器22的位置以及姿勢(shì)， 來(lái)計(jì)算外力坐標(biāo)系2_e中的機(jī)械手20的支承臺(tái)的位置以及姿勢(shì)。
[0093] 外力計(jì)算部43基于由旋轉(zhuǎn)中心位置計(jì)算部41計(jì)算出的外力坐標(biāo)系Σ_θ中的機(jī) 械手20的支承臺(tái)的位置以及姿勢(shì)，來(lái)計(jì)算將從力傳感器22取得的作用于把持部HND的力 以及力矩、即在工具坐標(biāo)系Σ_?中表示的力以及力矩在外力坐標(biāo)系Σ_θ中表示的力以及 力矩。
[0094] 負(fù)荷緩和動(dòng)作量計(jì)算部45基于由外力計(jì)算部43計(jì)算出的力、力矩、即在外力坐標(biāo) 系Σ_θ中表示的力以及力矩，來(lái)計(jì)算為了使作用于機(jī)械手20的把持部HND所把持的操作 對(duì)象M的力緩和而移動(dòng)操作對(duì)象M的緩和動(dòng)作量、即外力坐標(biāo)系Σ_θ中的緩和動(dòng)作量。這 里，緩和動(dòng)作量是指使操作對(duì)象M向檢測(cè)到的力所作用的方向平移的微小移動(dòng)量（以下，稱(chēng) 為微小緩和移動(dòng)量）、和使操作對(duì)象M向檢測(cè)出的力矩的方向旋轉(zhuǎn)的微小旋轉(zhuǎn)量（以下，稱(chēng) 為微小緩和旋轉(zhuǎn)量）。負(fù)荷緩和動(dòng)作量計(jì)算部45基于計(jì)算出的外力坐標(biāo)系Σ_θ中的緩和 動(dòng)作量，來(lái)計(jì)算作業(yè)坐標(biāo)系2_w中的緩和動(dòng)作量。
[0095] 移動(dòng)動(dòng)作量計(jì)算部46在拍攝部坐標(biāo)系2_c中計(jì)算出由把持部HND使操作對(duì)象M 移動(dòng)以使操作對(duì)象M的位置以及姿勢(shì)與由目標(biāo)導(dǎo)出部39計(jì)算出的目標(biāo)位置以及姿勢(shì)一致 的目標(biāo)動(dòng)作量。目標(biāo)動(dòng)作量是指用于使操作對(duì)象M向目標(biāo)位置平移的微小移動(dòng)量（以下， 稱(chēng)為微小目標(biāo)移動(dòng)量）、和用于使操作對(duì)象M向目標(biāo)姿勢(shì)旋轉(zhuǎn)的微小旋轉(zhuǎn)量（以下，稱(chēng)為微 小目標(biāo)旋轉(zhuǎn)量）。移動(dòng)動(dòng)作量計(jì)算部46基于計(jì)算出的拍攝部坐標(biāo)系2_c中的目標(biāo)動(dòng)作量， 來(lái)計(jì)算作業(yè)坐標(biāo)系2_w中的目標(biāo)動(dòng)作量。
[0096] 動(dòng)作結(jié)束判定部47基于由負(fù)荷緩和動(dòng)作量計(jì)算部45計(jì)算出的作業(yè)坐標(biāo)系2_w 中的緩和動(dòng)作量、以及由移動(dòng)動(dòng)作量計(jì)算部46計(jì)算出的作業(yè)坐標(biāo)系2_w中的目標(biāo)動(dòng)作量， 來(lái)判定機(jī)械手20將操作對(duì)象M組裝到組裝對(duì)象O的作業(yè)是否結(jié)束。此外，動(dòng)作結(jié)束判定部 47也可以?xún)H基于由移動(dòng)動(dòng)作量計(jì)算部46計(jì)算出的作業(yè)坐標(biāo)系2_w中的目標(biāo)動(dòng)作量，來(lái)判 定機(jī)械手20將操作對(duì)象M組裝到組裝對(duì)象O的作業(yè)是否結(jié)束。
[0097] 機(jī)械手控制部49基于由負(fù)荷緩和動(dòng)作量計(jì)算部45計(jì)算出的作業(yè)坐標(biāo)系Σ_￥中 的緩和動(dòng)作量、以及由移動(dòng)動(dòng)作量計(jì)算部46計(jì)算出的作業(yè)坐標(biāo)系2_w中的目標(biāo)動(dòng)作量，來(lái) 計(jì)算作業(yè)坐標(biāo)系Σ_?中的力傳感器22應(yīng)該移動(dòng)的位置以及姿勢(shì)。而且，機(jī)械手控制部49 控制機(jī)械手20,以使力傳感器22的位置以及姿勢(shì)與計(jì)算出的作業(yè)坐標(biāo)系2_w中的力傳感 器22應(yīng)該移動(dòng)的位置以及姿勢(shì)一致。
[0098] 以下，通過(guò)參照?qǐng)D5,來(lái)對(duì)控制裝置30用于使機(jī)械手20動(dòng)作以便組裝操作對(duì)象M 和組裝對(duì)象O的處理進(jìn)行說(shuō)明。圖5是表示控制裝置30用于使機(jī)械手20動(dòng)作以便組裝操 作對(duì)象M和組裝對(duì)象O的處理流程的一個(gè)例子的流程圖。
[0099] 首先，控制裝置30設(shè)定由用戶(hù)經(jīng)由輸入受理部33輸入的位置和姿勢(shì)，即操作對(duì)象 M和組裝對(duì)象O的組裝結(jié)束了的狀態(tài)的操作對(duì)象坐標(biāo)系Σ_πι中的組裝對(duì)象O的位置ρ_〇 Λm(-)和姿勢(shì) 〇_〇 " m(-)(步驟 S100)。
[0100] 接下來(lái)，控制裝置30設(shè)定由用戶(hù)經(jīng)由輸入受理部33輸入的旋轉(zhuǎn)中心位置和旋轉(zhuǎn) 中心姿勢(shì)，即操作對(duì)象坐標(biāo)系Σ_ηι中的旋轉(zhuǎn)中心位置p_e " m(-)和旋轉(zhuǎn)中心姿勢(shì)o_e ' m( - )(步驟 S110)〇
[0101] 步驟SlOO到步驟SllO是由用戶(hù)進(jìn)行的對(duì)控制裝置30的初始設(shè)定。從此開(kāi)始，對(duì) 在步驟SllO之后開(kāi)始拍攝部10的拍攝，控制部36從圖像取得部35取得拍攝圖像的情況 進(jìn)行說(shuō)明。
[0102] 在控制部36取得了拍攝圖像之后，控制裝置30的目標(biāo)導(dǎo)出部39基于由圖像取得 部35取得的拍攝圖像，來(lái)檢測(cè)拍攝部坐標(biāo)系2_c中的組裝對(duì)象O的位置p_〇 a c( - )和 姿勢(shì) 〇_〇 Λ c(-)(步驟 S120)。
[0103] 接下來(lái)，目標(biāo)導(dǎo)出部39基于在步驟SlOO中設(shè)定的操作對(duì)象M和組裝對(duì)象O的組 裝結(jié)束后的狀態(tài)的操作對(duì)象坐標(biāo)系Σ_πι中的組裝對(duì)象O的位置ρ_〇 Λ m(-)以及姿勢(shì)〇_ ο Λ m(-)、和在步驟S120中檢測(cè)到的拍攝部坐標(biāo)系2_c中的組裝對(duì)象