O的位置ρ_〇 Λc (―)以及姿勢〇_〇 Λ c (―)，并根據(jù)以下所示的公式（7)，來計算操作對象M和組裝對象 O的組裝結(jié)束后的狀態(tài)的操作對象M的目標位置和姿勢，即拍攝部坐標系2_c中的操作對 象M的目標位置p_m " c (d)(-)和姿勢o_m " c (d)(-)(步驟S130)。
[0104]
[0105] 這里，拍攝部坐標系2_c中的操作對象M的目標位置p_m Λ c(d)(―)以及姿勢〇_m ' c(d)( -)的"（d)"是為了與在步驟S140中由旋轉(zhuǎn)中心位置計算部41檢測到的拍 攝部坐標系2_c中的操作對象M的位置p_m Λ c(-)以及姿勢〇_m Λ c(-)區(qū)別而附 注的標號。
[0106] 步驟S120到步驟S130的處理是用于控制部36基于拍攝部10與組裝對象O的相 對位置以及姿勢、和操作對象M與組裝對象O的組裝結(jié)束后的狀態(tài)的操作對象M與組裝對 象O的相對位置關(guān)系（位置以及姿勢），間接地計算出操作對象M和組裝對象O的組裝結(jié)束 后的狀態(tài)的操作對象M與拍攝部10的相對位置以及姿勢的處理。
[0107] 接下來，旋轉(zhuǎn)中心位置計算部41根據(jù)取得到的拍攝圖像，來檢測拍攝部坐標系 2_c中的操作對象M的位置p_m " c (-)和姿勢o_m " c (-)(步驟S140)。
[0108] 接下來，旋轉(zhuǎn)中心位置計算部41根據(jù)取得到的拍攝圖像，來檢測工具坐標系Σ_? 中的拍攝部10的位置P_C A t (-)和姿勢o_c Λ t (-)(步驟S150)。
[0109] 接下來，旋轉(zhuǎn)中心位置計算部41基于在步驟S140中計算出的拍攝部坐標系2_c中的操作對象M的位置p_m " c(-)以及姿勢o_m " c(-)、和在步驟S150中計算出的 工具坐標系Σ_?中的拍攝部10的位置p_c ' t(-)以及姿勢〇_(： ' t(-)，并根據(jù)以 下所示的公式（8)，來計算工具坐標系Σ_?中的旋轉(zhuǎn)中心的位置p_e ' t(-)和姿勢o_eΛ t(-)(步驟 S160)。
[0110]
[0111] 這里，步驟S160的處理是檢測力傳感器22與上述的旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心 姿勢的相對位置關(guān)系的變化的處理。在步驟S160中計算出的工具坐標系Σ_?中的旋轉(zhuǎn)中 心的位置口_0 ' t(-)和姿勢o_e ' t(-)與在之前的程序中計算出的值不同的情況表 示力傳感器22與旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心姿勢的相對位置關(guān)系因外力而變化。通過控 制部36檢測該變化，即使相對于力傳感器22的旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心姿勢因外力而 變化，也能夠基于相對于變化后的力傳感器22的旋轉(zhuǎn)中心位置以及旋轉(zhuǎn)中心姿勢，控制成 機械手20總是以旋轉(zhuǎn)中心位置為中心，并以旋轉(zhuǎn)中心姿勢為基準使操作對象M進行旋轉(zhuǎn)。
[0112] 接下來，旋轉(zhuǎn)中心位置計算部41基于正向運動學(xué)，來計算作業(yè)坐標系Σ_?中的力 傳感器22的位置p_t " w (-)和姿勢o_t " w (-)(步驟S170)。
[0113] 接下來，旋轉(zhuǎn)中心位置計算部41基于在步驟S160中計算出的工具坐標系Σ_?中 的旋轉(zhuǎn)中心位置口_6 " t(-)和旋轉(zhuǎn)中心姿#o_e " t(-)、以及在步驟S170中計算出 的作業(yè)坐標系Σ_?中的力傳感器22的位置p_t Λ w(-)及姿勢o_t Λ w(-)，并根據(jù)以 下所示的公式（9)，來計算外力坐標系Σ_θ中的機械手20的支承臺的位置p_w Λ e( - ) 和姿勢〇_w " e (-)(步驟S180)。
[0114]
[0115] 這里，控制部36通過步驟S180的處理，來計算外力坐標系Σ_θ中的機械手20的 支承臺的位置P_w " e(-)和姿勢o_w " e(-)，由此能夠通過下一步驟S190的處理基 于作用于力傳感器22的力以及力矩來計算作用于旋轉(zhuǎn)中心位置的力以及力矩。
[0116] 接下來，外力計算部43基于由力傳感器22檢測出的力和力矩，即工具坐標系Σ_ t中的力f Λ t(-)和力矩m Λ t(-)，并根據(jù)以下所示的牛頓?歐拉方程式（10)，來計 算外力坐標系Σ_θ中的力（g卩，作用于旋轉(zhuǎn)中心位置的力）f a e( - )和力矩（g卩，在旋 轉(zhuǎn)中心位置產(chǎn)生的力矩）m Λ e(-)(步驟S190)。
[0117]
[0118] 這里，上述的公式（10)中的上段公式表示了作用于操作對象M的旋轉(zhuǎn)中心位置的 外力的分量f Λ e( - )由從力傳感器22所檢測出的力的分量f Λ t( - )減去了因重力 引起的分量mE( Λ )g( -)和因機械手20的臂部ARM的慣性運動引起的分量mE( Λ )p_cΛ w( - ) ( · ·)而得到的分量來表示。其中，附注了 "（ · ·）"的最近前的文字表示對于 時間進行了二次微分后的變量。另外，矩陣Ε( Λ )是單位矩陣，向量g( - )是表示重力加 速度的向量，標量m表示操作對象M的質(zhì)量。
[0119] 與上述的上段公式同樣，下段公式表示了通過作用于操作對象M的外力而在旋轉(zhuǎn) 中心產(chǎn)生的力矩m " e由以下的分量表不：從將扭轉(zhuǎn)力矩的分量I ( " )o_c " w( - ) ( ··)、 由科里奧利力引起的力矩的分量〇_c Λ w( - ) ( · ) X I ( Λ )o_c Λ w( - ) ( ·)、由力傳感 器22檢測出的力矩m Λ t( - )、因由力傳感器22檢測出的力的分量f Λ t( - )引起的 力矩的分量P_e ' t( -)Xf ' t(-)相加的合計值減去了因重力引起的力矩的分量p_ g Λ t( - ) XmR_w Λ t( Λ )g(-)而得到的分量。
[0120] 接下來，負荷緩和動作量計算部45基于在步驟S190中計算出的外力坐標系Σ_ e中的力f Λ e(-)以及力矩m Λ e(-)，并根據(jù)以下所示的公式（11)，來計算外力坐標 系Σ_θ中的操作對象M的緩和動作量的微小緩和移動量Λρ Λ e( - )和微小緩和旋轉(zhuǎn)量 Δ 〇 Λe (-)(步驟 S200)。
[0121]
[0122] 這里，步驟S200中使用的公式（11)的ΔΡ Λe(-)以及ΛΟ Λe(-)是分別對 Δρ Λ e( - )以及Λ〇 a e( - )進行了拉普拉斯變換后的量。另外，公式（11)根據(jù)在設(shè) 定于操作對象M的旋轉(zhuǎn)中心位置承受外力時，決定應(yīng)該向使該外力緩和的方向移動的動作 量（微小移動量以及微小旋轉(zhuǎn)量）的運動模型而不同。在本實施方式中，采用了如上述的 公式（11)所示，由與微小移動量相關(guān)的慣性質(zhì)量矩陣Μ_ρ( Λ )、阻尼系數(shù)矩陣D_p( Λ )、 彈簧乘數(shù)矩陣Κ_ρ( Λ )、以及與微小旋轉(zhuǎn)量相關(guān)的慣性質(zhì)量矩陣Μ_ο( Λ )、阻尼系數(shù)矩陣 D_o( Λ )、彈簧乘數(shù)矩陣Κ_ο( Λ )表示的運動模型。根據(jù)這樣的運動模型進行的控制被稱 為順應(yīng)運動控制中的阻抗控制。其中，標量s是拉普拉斯變換所使用的變量。作為順應(yīng)運 動控制，并不局限于阻抗控制，例如也可以應(yīng)用剛度控制或阻尼控制。
[0123] 接下來，負荷緩和動作量計算部45基于在步驟S200中計算出的外力坐標系Σ_θ中的操作對象M的緩和動作量，并根據(jù)以下所示的公式（12)，來計算微小緩和移動量Ap_t ' w(i)(-)和微小緩和旋轉(zhuǎn)量A〇_t ' w(i)(-)作為作業(yè)坐標系Σ_?中的力傳感器 22的緩和動作量（步驟S210)。
[0124]
[0125] 這里，作為緩和動作量的微小緩和移動量Ap_t ' w (i)(-)和微小緩和旋轉(zhuǎn)量 △ o_t ' w(i)( -)中的"（i)"是用于區(qū)別緩和動作量和目標動作量的標號?？刂撇?6的 機械手控制部49能夠基于通過步驟S200到步驟S210的處理由負荷緩和動作量計算部45 計算出的作業(yè)坐標系Σ_?中的緩和動作量控制機械手20,以使力傳感器22的位置以及姿 勢移動到作用于旋轉(zhuǎn)中心位置的力以及力矩被緩和的位置。
[0126] 接下來，移動動作量計算部46基于在步驟S130中計算出的操作對象M和組裝對 象〇的組裝結(jié)束后的狀態(tài)的拍攝部坐標系2_c中的操作對象M的目標位置p_m Λ c(d) (―)以及姿勢〇_m ' c(d)(―)、和在步驟S140中檢測出的拍攝部坐標系2_c中的操作對 象M的位置p_m ' c(-)與姿勢〇_m ' c(-)，并根據(jù)以下所示的公式（13)，來計算用于 使操作對象M向目標位置以及姿勢移動的目標動作量、即微小目標移動量Δ p_m Λ c (-) 和微小目標旋轉(zhuǎn)量△ 〇_m Λ c (-)作為拍攝部坐標系Σ _c中的目標動作量（步驟S220)。
[0127]
[0128] 接下來，移動動作量計算部46基于在步驟S220中計算出的拍攝部坐標系2_c中 的操作對象M的目標動作量，并根據(jù)以下所示的公式（14)，來計算微小目標移動量Λ p_t ' w(v)(-)和微小目標旋轉(zhuǎn)量Δ 〇_t " w(v)(-)作為作業(yè)坐標系2_w中的力傳感器22 的目標動作量（步驟S230)。
[0129]
[0130] 這里，作為目標動作量的微小目標移動量Δρ_? ' w(v)(―)和微小目標旋轉(zhuǎn)量 A〇_t ' w(v)( -)中的"（V)"是用于區(qū)別目標動作量和緩和動作量的標號。
[0131] 接下來，動作結(jié)束判定部47計算如以下所示的公式（15)那樣加上了在步驟S210 中計算出的緩和動作量和在步驟S230中計算出的目標動作量后的合計微小移動量Δρ_? "w (-)以及合計微小旋轉(zhuǎn)量Δ 〇_t " w (-)(步驟S240)。
[0132]
[0133] 接下來，動作結(jié)束判定部47判定在步驟S240中計算出的合計微小移動量Δρ_?Λ w(-)以及合計微小旋轉(zhuǎn)量Δ〇_? Λ w(-)雙方是否小于針對各自設(shè)定的規(guī)定閾值 (步驟S250)。其中，上述的針對各自設(shè)定的規(guī)定閾值被分別獨立設(shè)定。動作結(jié)束判定部47 在判定為合計微小移動量Ap_t ' w(-)以及合計微小旋轉(zhuǎn)量A〇_t ' w(-)雙方小于 針對各自設(shè)定的規(guī)定閾值的情況（步驟S250 -是）下，判斷為無需再移動操作對象M而結(jié) 束處理。
[0134] 另一方面，在動作結(jié)束判定部47判定為合計微小移動量Ap_t ' w(―)以及合 計微小旋轉(zhuǎn)量△〇_〖Λ w( - )中的任意一方或者雙方不小于針對各自設(shè)定的規(guī)定閾值的 情況（步驟S250 -否）下，機械手控制部49基于在步驟S210中計算出的緩和動作量和在 步驟S230中計算出的目標動作