(57)【特許請求の範囲】
【請求項１】
ロボットの手先部に存在する一方の物体を、他方の物体に対して移動させ、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるように、両者間に作用する力を制御するロボット制御装置において、
前記一方と前記他方との間に作用する力を計測する力計測部と、
前記一方を前記他方に対して、前記一方と前記他方との間に作用する力に基づいて並進移動させるときの方向である並進力制御方向を、１つ以上設定する並進力制御方向設定部と、
前記並進力制御方向における前記一方と前記他方との間に作用する力の目標値である並進力制御目標力を、夫々の前記並進力制御方向について、設定する並進力制御目標力設定部と、
前記一方を前記他方に対して、前記一方と前記他方との間に作用する力に基づいて回転移動させるときの回転軸である回転力制御軸を１つ以上設定し、前記回転力制御軸の内、少なくとも１つの軸は前記並進力制御方向と平行でない軸を設定する回転力制御軸設定部と、
前記一方と前記他方との間に作用する力に基づいて、前記一方を前記他方に対して前記並進力制御方向に並進移動させる移動量の目標値である、並進力制御方向目標移動量を算出する並進力制御方向目標移動量算出部と、
前記一方と前記他方との間に作用する力に基づいて、前記一方を前記他方に対して前記回転力制御軸の軸回りに回転移動させる移動量の目標値である、回転力制御軸回り目標移動量を算出する回転力制御軸回り目標移動量算出部と、
前記一方と前記他方との間に作用する力、および、
前記並進力制御方向設定部によって設定される前記並進力制御方向、および、
前記回転力制御軸設定部によって設定される前記回転力制御軸、および、
前記並進力制御方向目標移動量算出部によって算出される前記並進力制御方向目標移動量、および、
前記回転力制御軸回り目標移動量算出部によって算出される前記回転力制御軸回り目標移動量、
に基づいて、
前記一方の並進方向の移動量の目標値である並進方向目標移動量および前記一方の回転方向の移動量の目標値である回転方向目標移動量を算出する目標方向目標移動量算出部と、
前記目標方向目標移動量算出部によって算出される前記並進方向目標移動量および前記回転方向目標移動量に基づいて、
前記ロボットの動作指令を生成する動作指令生成部と、
を備え、
前記並進力制御方向目標移動量算出部は、
前記並進力制御方向における前記一方と前記他方との間に作用する力が、並進力制御方向の力に関する第１の所定閾値未満のとき、所定の移動量である第１の並進力制御方向移動量、または、前記所定の移動量である第１の並進力制御方向移動量および前記並進力制御方向の前記並進力制御目標力に基づいて、前記並進力制御方向目標移動量を算出し、
前記並進力制御方向における前記一方と前記他方との間に作用する力が、前記並進力制御方向の力に関する第１の所定閾値以上のとき、前記並進力制御方向における前記一方と前記他方との間に作用する力および前記並進力制御方向の前記並進力制御目標力、または、
前記並進力制御方向における前記一方と前記他方との間に作用する力および前記並進力制御方向の前記並進力制御目標力および所定の移動量である第２の並進力制御方向移動量、
に基づいて、前記並進力制御方向目標移動量を算出する、
ことを特徴とするロボット制御装置。
【請求項２】
前記並進力制御方向目標移動量算出部は、
前記一方の前記回転力制御軸の軸回りの回転移動の移動量と、
前記一方の前記回転力制御軸の軸回りの回転移動に伴う、前記一方と前記他方とが接触する部分の並進移動の移動量との関係に基づいて、
前記回転力制御軸回り目標移動量を基に、前記並進力制御方向目標移動量の補正移動量を算出し、前記補正移動量分を補正して前記並進力制御方向目標移動量を算出する、
請求項１に記載のロボット制御装置。
【請求項３】
前記ロボット制御装置は、さらに、
前記一方と前記他方とが接触するときの接触位置を設定する、または、
前記一方と前記他方との間に作用する力に基づいて前記一方と前記他方とが接触するときの接触位置を算出して設定する接触位置設定部を備え、
前記回転力制御軸設定部は、
所定の回転力制御軸を１つ以上設定すると共に、
前記一方における前記接触位置を通る軸または前記他方における前記接触位置を通る軸、かつ、前記所定の回転力制御軸に平行な軸を、接触位置基準の回転力制御軸として１つ以上設定し、
前記回転力制御軸回り目標移動量算出部は、
前記所定の回転力制御軸の軸回りの力に基づいて、または、前記所定の回転力制御軸に対して算出される前記回転力制御軸の軸回りに回転移動させる場合の目標移動量に基づいて、
前記接触位置基準の回転力制御軸の軸回りに回転移動させる目標移動量を算出して、前記算出された目標移動量を前記回転力制御軸回り目標移動量とする、
請求項１または請求項２に記載のロボット制御装置。
【請求項４】
前記ロボット制御装置は、さらに、
前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにする部分における、前記一方の前記接触させる部分の姿勢または前記他方の前記接触させる部分の姿勢または前記一方および前記他方の前記接触させる部分の姿勢を算出する接触部分姿勢算出部を備え、
前記接触部分姿勢算出部は、
前記一方と前記他方との間に作用する力に基づいて前記一方と前記他方とが接触するときの接触位置を算出し、複数の前記算出された接触位置に基づいて前記一方と前記他方とを接触させる部分の姿勢を算出し、
前記回転力制御軸回り目標移動量算出部は、前記一方と前記他方との間に作用する力、および、前記接触部分姿勢算出部が算出する前記一方と前記他方とを接触させる部分の姿勢に基づいて、前記回転力制御軸回り目標移動量を算出する、
請求項１から請求項３のいずれか一項に記載のロボット制御装置。
【請求項５】
前記ロボット制御装置は、さらに、
前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにするときの姿勢である合わせ状態姿勢を算出する合わせ状態姿勢算出部を備え、
前記ロボット制御装置は、
前記一方と前記他方とが接触したとき、前記一方の前記他方に対する前記並進力制御方向の移動を停止させると共に、前記一方の姿勢を取得し、
次に前記一方の前記他方に対する前記並進力制御方向の移動を停止させると共に、前記一方を前記他方に対して前記回転力制御軸の軸回りに回転移動させ、前記一方と前記他方とが接触したときの前記一方の姿勢を取得し、
前記合わせ状態姿勢算出部は、前記一方と前記他方とが接触したときに前記取得された前記一方の姿勢に基づいて前記合わせ状態姿勢を算出し、
前記回転力制御軸回り目標移動量算出部は、前記一方と前記他方との間に作用する力、および、前記合わせ状態姿勢算出部が算出した前記合わせ状態姿勢に基づいて、前記回転力制御軸回り目標移動量を算出する、
請求項１に記載のロボット制御装置。
【請求項６】
前記回転力制御軸回り目標移動量算出部は、
前記並進力制御方向設定部によって設定される前記並進力制御方向の内、いずれかの前記並進力制御方向と平行でない前記回転力制御軸について軸回りの目標移動量を算出する際、
前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、
前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、
前記第１の接触状態であって、前記第２の接触状態から前記第１の接触状態になるときではない場合、前記回転力制御軸回り目標移動量を０と算出し、
前記第２の接触状態から前記第１の接触状態となる場合、前記回転力制御軸回り目標移動量を０と算出する、または、前記第１の接触状態となるまでの前記第２の接触状態のときの前記回転力制御軸の軸回りの移動方向および移動量に基づいて前記回転力制御軸回り目標移動量を算出し、
前記第２の接触状態となる場合、
前記回転力制御軸の軸回りの力の符号と共に、
回転方向の所定の移動量である所定の回転方向移動量Ａ、または、前記回転力制御軸と平行でない前記並進力制御方向の並進力制御目標力、または、前記回転力制御軸と平行でない前記並進力制御方向の力、
の内、少なくとも１つに基づいて、
前記回転力制御軸回り目標移動量を算出する、
請求項１から請求項５のいずれか一項に記載のロボット制御装置。
【請求項７】
前記回転力制御軸回り目標移動量算出部は、
前記並進力制御方向設定部によって設定される前記並進力制御方向の内、いずれかの前記並進力制御方向と平行でない前記回転力制御軸について軸回りの目標移動量を算出する際、
前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、
前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、
前記第１の接触状態であって、前記第２の接触状態から前記第１の接触状態になるときではない場合、前記回転力制御軸回り目標移動量を０と算出し、
前記第２の接触状態から前記第１の接触状態となる場合、前記回転力制御軸回り目標移動量を０と算出する、または、前記第１の接触状態となるまでの前記第２の接触状態のときの前記回転力制御軸の軸回りの移動方向および移動量に基づいて前記回転力制御軸回り目標移動量を算出し、
前記第２の接触状態となる場合、
前記回転力制御軸の軸回りの力、または、
前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、または、
回転方向の所定の移動量である所定の回転方向移動量Ｂおよび前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、
に基づいて、
前記回転力制御軸回り目標移動量を算出する、
請求項１から請求項５のいずれか一項に記載のロボット制御装置。
【請求項８】
前記ロボット制御装置は、さらに、
前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態である、合わせ状態になっているか否か、または、
前記一方と前記他方とが前記合わせ状態に近い状態である、合わせ状態近傍になっているか否か、の前記一方と前記他方との前記合わせ状態の判定を実行する合わせ状態判定部を備え、
前記回転力制御軸回り目標移動量算出部は、
前記並進力制御方向設定部によって設定される前記並進力制御方向の内、いずれかの前記並進力制御方向と平行でない前記回転力制御軸について軸回りの目標移動量を算出する際、
前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、
前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、
前記第１の接触状態であって、前記第２の接触状態から前記第１の接触状態になるときではない場合、前記回転力制御軸回り目標移動量を０と算出し、
前記第２の接触状態から前記第１の接触状態となる場合、前記回転力制御軸回り目標移動量を０と算出する、または、前記第１の接触状態となるまでの前記第２の接触状態のときの前記回転力制御軸の軸回りの移動方向および移動量に基づいて前記回転力制御軸回り目標移動量を算出し、
前記第２の接触状態であって前記合わせ状態判定部によって前記合わせ状態近傍または前記合わせ状態になっていると判定される状態、または、前記第２の接触状態であって前記合わせ状態判定部によって前記合わせ状態近傍または前記合わせ状態になっていると判定されたとき以降の状態、を第２Ａの接触状態とし、
前記第２の接触状態であって前記第２Ａの接触状態でない状態を第２Ｂの接触状態とするとき、
前記第２Ｂの接触状態である場合、
前記回転力制御軸の軸回りの力の符号と共に、
回転方向の所定の移動量である所定の回転方向移動量Ａ、または、前記回転力制御軸と平行でない前記並進力制御方向の並進力制御目標力、または、前記回転力制御軸と平行でない前記並進力制御方向の力、
の内、少なくとも１つに基づいて、
前記回転力制御軸回り目標移動量を算出し、
前記第２Ａの接触状態である場合、
前記回転力制御軸の軸回りの力、または、
前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、または、
回転方向の所定の移動量である所定の回転方向移動量Ｂおよび前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、に基づいて前記回転力制御軸回り目標移動量を算出する、
または、前記第２Ｂの接触状態である場合に算出される前記回転力制御軸回り目標移動量より小さい値に調整して前記回転力制御軸回り目標移動量を算出する、
請求項１から請求項５のいずれか一項に記載のロボット制御装置。
【請求項９】
前記回転力制御軸と平行でない前記並進力制御方向における、前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記合わせ状態判定部によって前記合わせ状態になっていると判定される状態が、所定の時間以上連続するとき、
前記回転力制御軸回り目標移動量算出部は、
前記回転力制御軸回り目標移動量を０とする、または、
前記回転力制御軸回り目標移動量が、算出された前記回転力制御軸回り目標移動量より小さくなるように調整して算出し、
前記並進力制御方向目標移動量算出部は、
前記並進力制御方向目標移動量を０とする、または、
前記並進力制御方向目標移動量が、算出された前記並進力制御方向目標移動量より小さくなるように調整して算出する、
請求項８に記載のロボット制御装置。
【請求項１０】
前記ロボット制御装置は、さらに、
前記ロボットの台座の位置または姿勢または位置および姿勢が移動するとき、前記ロボットの台座の移動に基づいて、前記他方に対する前記一方の移動方向および移動量を補正して、または、
前記他方の位置または姿勢または位置および姿勢が移動するとき、前記他方の移動に基づいて、前記他方に対する前記一方の移動方向および移動量を補正して、または、
前記ロボットの台座の位置または姿勢または位置および姿勢と、前記他方の位置または姿勢または位置および姿勢とが移動するとき、前記ロボットの台座の移動、および、前記他方の移動に基づいて、前記他方に対する前記一方の移動方向および移動量を補正して、
前記一方を前記他方に対して移動させるように前記ロボットの動作指令を生成する、
請求項１から請求項９のいずれか一項に記載のロボット制御装置。
【請求項１１】
前記ロボット制御装置は、さらに、
前記一方と前記他方との間に所定時間中に所定の閾値を超える力がかかる状態、
または前記一方と前記他方との間に別の所定の閾値を超える力がかかる状態、
または前記一方と前記他方との間に作用する力の発振状態、
のうちの少なくとも１つの状態を検出することにより、前記一方と前記他方との間に作用する力の不安定状態を検出する力不安定状態検出部と、
前記ロボットの特異姿勢の近傍状態、または前記ロボットの発振状態、
のうちの少なくとも１つの状態を検出することにより、前記ロボットの移動動作の不安定状態を検出する移動動作不安定状態検出部と、
を備え、
前記力不安定状態検出部によって前記力の不安定状態が検出されたとき、または、
前記移動動作不安定状態検出部によって前記ロボットの移動動作の不安定状態が検出されたとき、
前記ロボット制御装置は、
前記回転力制御軸回り目標移動量を０にする、または、前記力の不安定状態または前記ロボットの移動動作の不安定状態が検出されない場合に算出される前記回転力制御軸回り目標移動量より小さい値となるように前記回転力制御軸回り目標移動量を調整して算出する、
請求項１から請求項１０のいずれか一項に記載のロボット制御装置。
【請求項１２】
前記ロボット制御装置は、さらに、
前記並進力制御方向の力の大きさが所定閾値を超える場合、
前記並進力制御方向の力の大きさを前記所定閾値として、
前記回転力制御軸の軸回りの力の大きさが別の所定閾値を超える場合、
前記回転力制御軸の軸回りの力の大きさを別の所定閾値とする、
請求項１から請求項１１のいずれか一項に記載のロボット制御装置。
【請求項１３】
前記並進力制御方向設定部は、
前記一方に対する所定の方向、または、
前記他方に対する所定の方向、または、
前記一方の移動動作によらない所定の方向、または、
前記回転力制御軸が設定されている場合、前記回転力制御軸、
の内、少なくとも１つに基づいて、
１つ以上の前記並進力制御方向を設定し、
前記回転力制御軸設定部は、
前記一方に対する所定の方向、または、
前記他方に対する所定の方向、または、
前記一方の移動動作によらない所定の方向、または、
前記並進力制御方向が設定されている場合、前記並進力制御方向、
の内、少なくとも１つに基づいて、
１つ以上の前記回転力制御軸を設定する、
請求項１から請求項１２のいずれか一項に記載のロボット制御装置。
【請求項１４】
前記並進力制御方向設定部は、
前記一方と前記他方とが接触するとき、
前記並進力制御方向の成分を含む方向、かつ、所定範囲を限度とする別の方向に、
前記一方を前記他方に対して押しつけたときの、前記回転力制御軸の軸回りの力に基づいて、前記並進力制御方向を設定する、
請求項１から請求項１３のいずれか一項に記載のロボット制御装置。
【請求項１５】
前記回転力制御軸回り目標移動量算出部は、
前記回転力制御軸が２つ以上ある場合、
所定の前記回転力制御軸について、
前記一方と前記他方との間に作用する力に基づいて、
前記回転力制御軸の軸回りの力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ｂより小さいとき、回転方向の所定の移動量である所定の回転方向移動量Ｃを前記回転力制御軸回り目標移動量とし、
前記回転力制御軸の軸回りの力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ｂ以上のとき、
前記回転力制御軸の軸回りの力と軸回りの所定の目標力、または、
回転方向の所定の移動量である所定の回転方向移動量Ｄおよび前記回転力制御軸の軸回りの力と軸回りの前記所定の目標力に基づいて、前記回転力制御軸回り目標移動量を算出する、
請求項１から請求項１４のいずれか一項に記載のロボット制御装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ロボットの手先部に存在する一方の物体を他方の物体に対して移動させ、前記一方と前記他方とが、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態である、合わせ状態となるように、両者間に作用する力を制御する前記ロボットのロボット制御装置に関する。
【背景技術】
【０００２】
嵌合作業では、一方の物体における或る部分の形状と、他方の物体における或る部分の形状とを嵌め合わせるように２つの物体を相対的に移動させる。ここで、嵌合とは、形状が合ったもの同士を嵌め合わせることであり、軸がくぼんだ所にかたくはまり合ったり、滑り動くようにゆるくはまり合ったりする関係となるように２つの物体を移動させることである。
【０００３】
このような嵌合のように一方の物体と他方の物体とを嵌め合わせるわけではなく、一方の物体と他方の物体とが、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるように、前記一方を前記他方に対して移動させるという作業が広くおこなわれている。
【０００４】
このとき、前記一方および前記他方は、ワーク、工具、加工装置、検査装置、計測装置、作業台、ジグなどの様々な組み合わせが有り得る。例えば、把持したワークを工作機械の加工台などに取りつける。また、把持したワークの面を設置された加工装置に面が合うように押しつけたり、把持したワークの直線部分を設置された加工装置に押しつけて合わせたりする。また、保持する工具の面を固定されて置かれているワークの面に合わせたり、保持する工具の面を固定されて置かれているワークの直線部分に合わせたり、保持工具の直線部分をワークの面にあわせたりする。また、ある部品を別の部品に対して、面や直線部分が合うように取りつける。また、ワークと、検査装置や計測装置とが面が合うように接触させる。
【０００５】
様々な状況における前述のような作業に対して、低コスト化、品質の安定化、危険な作業の自動化などのために、ロボットによって、より安定かつより素早く作業を実行することが望まれている。
【０００６】
このような作業をロボットによって自動化するように、ロボットの手先部に保持される一方の物体を他方の物体に移動させ、前記一方と前記他方とが、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにする方法として、コンプライアンス制御によって両者間に作用する力が目標とする力となるようにして両者間に作用する力を制御する方法が知られている。
【０００７】
コンプライアンス制御では、前記一方と前記他方とが、所定の並進方向について接触した状態、かつ、１つまたは２つの所定の回転軸回りの力が０となってつりあった状態となるようにすることによって、一方の物体の所定の部分と他方の物体の所定の部分とが接触して合わされた状態となるようにすることができる。このとき、コンプライアンス制御では、前記一方を回転させる回転中心点に制御点を設定し、所定の方向に所定の力で押しつけるように前記一方の制御点の位置を平行移動させると共に、前記制御点が存在する所定の回転軸の軸回りの力と目標とする力と力制御ゲインに応じて、前記一方を前記所定の回転軸回りに回転移動させる。
【０００８】
特許文献１には、一方と他方との間に作用する力や軸回りの力が目標値となるように、コンプライアンス機構の運動モデルに基づいてツールの位置を制御する方法が開示されている。この方法では、制御点を回転中心として軸回りの力に応じて姿勢を移動させる。
【０００９】
また特許文献２には、アプローチ点の姿勢誤差を修正するために、予め、穴部分と直交する平面に所定の力で押しつけ面接触させて、面を合わせることによって、姿勢誤差を算出する方法が記載されている。
【先行技術文献】
【特許文献】
【００１０】
【特許文献１】特公平０４−０４３７４４号公報
【特許文献２】特開平０９−０９１０２６号公報
【発明の概要】
【発明が解決しようとする課題】
【００１１】
先行技術によって、一方の物体と他方の物体とが、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態である、合わせ状態となるようにする場合、前記一方と前記他方との相対的な位置を制御して、１つ以上の所定の並進方向について接触した状態、かつ、１つ以上の所定の回転軸回りの力が０となってつりあった状態となるようにする。
【００１２】
前記所定の回転軸回りの力は一方の物体と他方の物体とが接触する位置によって変化する。また、回転軸回りの力がどの程度の大きさになるのかは一方の物体と他方の物体とがどのような物体であるかや押付力などの状況によって変わる。また、回転軸回りの力は押付方向の力と回転軸回りとの距離によって求まる値であるので変動しやすい。また、回転軸回りの力はノイズが載りやすかったり、振動しやすかったり、小さい値になったりすることがある。また、回転移動によって一方の物体と他方の物体とが離れて、回転軸回りの力が発生しなくなることがある。また、所定の回転軸回りの力が０となるように制御するので前記状態に近づくと所定の回転軸回りの力が小さくなり、回転軸回りの力の符号が頻繁に変わることがある。
【００１３】
このため、所定の回転軸回りの力に基づいて、素早く回転移動させて前記の状態にスムーズに収束させることは難しい。また、押付方向の目標力が小さい場合、所定の回転軸回りの力は小さいため、所定の回転軸回りの力に基づいて素早く回転移動させて前記の状態にスムーズに収束させることはさらに困難である。
【００１４】
また、嵌合作業の場合、或る部分から受ける力に基づいて姿勢を移動させると、別の部分との接触によって力を受けるようになり、その力に倣いながら、一方の物体を他方の物体に挿入させていくことによって、他方の物体に対する一方の物体の姿勢が収束していく。つまり、挿入が進むにつれて、目標とする姿勢を通過して行き過ぎる移動量は小さくなっていく。これに対して、一方の物体と他方の物体とが前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにする場合、一方の物体が他方の物体に対して挿入していくことによって姿勢が収束しやすくなることはなく、所定の回転軸回りの回転を速くすると、目標とする姿勢を通過して行き過ぎてしまうため、所定の回転軸回りの力に基づいて回転移動させて、短い時間で、前記の状態にスムーズに収束させることは難しい。
【００１５】
特許文献１に記載の方法や、一般的に実施されている方法では、一方と他方との間に作用する力の大きさと力制御ゲインに応じた移動量で移動させる。所定の回転軸回りの力の力制御ゲインを大きくするとロボットの動作が不安定かつ発振しやすくなるため、所定の回転軸回りの力制御ゲインの大きさには制限があり、所定の回転軸回りの回転動作の速度はあまり大きくすることはできない。
【００１６】
さらに、所定の回転軸回りの回転移動の速度が大きいと、所定の回転軸回りの回転移動によって前記一方と前記他方とが並進方向に離れる速度の方が、前記一方と前記他方との間に作用する力の大きさと力制御ゲインに基づく、前記一方と前記他方とを押付方向に近づける速度よりも大きくなるため、前記一方の回転移動によって前記一方と前記他方とは押付方向について離れやすくなり、または離れたりし、所定の回転軸回りの力が小さくなる。このため、所定の回転軸回りの回転移動の速度は結果的に小さくなる、または、前記一方と前記他方とが非接触状態となって所定の回転軸回りの力が０となるために回転移動が停止する。
【００１７】
この場合、次のようにして、回転移動が再開される。まず、前記一方と前記他方とが離れやすくなり、前記一方の回転移動の速度が小さくなると、押付方向の力と目標押付力と力制御ゲインによって算出される押付方向の移動によって、前記一方と前記他方とは再び接触するまたは押付方向の力が大きくなる。そして、前記一方と前記他方との押付方向の力が大きくなると、前記所定の回転軸回りの力も大きくなり、前記一方は再び回転移動するまたは前記一方の回転移動の速度は大きくなる。
【００１８】
押付方向の力についての力制御ゲインを大きくすると、ロボットの動作が不安定かつ発振しやすくなるため力制御ゲインはある程度までしか大きくすることができない。このため、力制御ゲインを大きくすることによって押付方向の移動動作の速度を大きくして、前記非接触状態の時間を短くすることや押付方向の力が小さくならないようにすることには限界がある。
【００１９】
前述のように、前記一方を前記他方に対して所定の回転軸回りに回転移動させるとき、力制御ゲインを大きくすることによって、回転移動の速度を大きくすることや、回転移動の速度を大きい状態で維持することは困難であり、前記一方と前記他方とを、安定的に素早く、前記一方と前記他方とが、１つ以上の所定の並進方向について接触した状態、かつ、１つ以上の所定の回転軸回りの力が０となってつりあった状態にすることは困難である。このため、回転動作を安定的かつ速くするためには、押付方向についての目標押付力をなるべく大きくせざるを得ない。また、回転移動の速度が速い場合、回転移動によって前述のように前記一方と前記他方とが離れやすくなり結果的に合わせ状態とするのに時間がかかったり、また、目標とする状態を超えて姿勢が移動し、目標とする状態に収束させるのに無駄に時間がかかったりする。
【００２０】
特許文献２における面を合わせる方法については、嵌合時にツール座標系における挿入方向以外の力モーメントが０になるようにする方法と同様と考えられるだけで、特に詳細な方法は記載されていない。前記一方と前記他方との間に作用する力と目標押付力と力制御ゲインを用いて、前記一方と前記他方とを、安定的に素早く、前記一方と前記他方とが、１つ以上の所定の並進方向について接触した状態、かつ、１つ以上の所定の回転軸回りの力が０となってつりあった状態にすることは、特許文献１における前述の説明と同様に困難である。
【課題を解決するための手段】
【００２１】
そこで本発明は、ロボットによって、一方の物体と他方の物体とを、一方の物体の所定の部分と他方の物体の所定の部分とが接触して合わされた状態である、合わせ状態となるように、より安定的により素早く、一方の物体と他方の物体とを相対的に移動させるロボット制御装置を提供することを目的とする。
【００２２】
上記目的を達成するために、本願第１の発明は、ロボットの手先部に存在する一方の物体を、他方の物体に対して移動させ、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるように、両者間に作用する力を制御するロボット制御装置において、前記一方と前記他方との間に作用する力を計測する力計測部と、前記一方を前記他方に対して、前記一方と前記他方との間に作用する力に基づいて並進移動させるときの方向である並進力制御方向を、１つ以上設定する並進力制御方向設定部と、前記並進力制御方向における前記一方と前記他方との間に作用する力の目標値である並進力制御目標力を、夫々の前記並進力制御方向について、設定する並進力制御目標力設定部と、前記一方を前記他方に対して、前記一方と前記他方との間に作用する力に基づいて回転移動させるときの回転軸である回転力制御軸を１つ以上設定し、前記回転力制御軸の内、少なくとも１つの軸は前記並進力制御方向と平行でない軸を設定する回転力制御軸設定部と、前記一方と前記他方との間に作用する力に基づいて、前記一方を前記他方に対して前記並進力制御方向に並進移動させる移動量の目標値である、並進力制御方向目標移動量を算出する並進力制御方向目標移動量算出部と、前記一方と前記他方との間に作用する力に基づいて、前記一方を前記他方に対して前記回転力制御軸の軸回りに回転移動させる移動量の目標値である、回転力制御軸回り目標移動量を算出する回転力制御軸回り目標移動量算出部と、前記一方と前記他方との間に作用する力、および、前記並進力制御方向設定部によって設定される前記並進力制御方向、および、前記回転力制御軸設定部によって設定される前記回転力制御軸、および、前記並進力制御方向目標移動量算出部によって算出される前記並進力制御方向目標移動量、および、前記回転力制御軸回り目標移動量算出部によって算出される前記回転力制御軸回り目標移動量、に基づいて、前記一方の並進方向の移動量の目標値である並進方向目標移動量および前記一方の回転方向の移動量の目標値である回転方向目標移動量を算出する目標方向目標移動量算出部と、前記目標方向目標移動量算出部によって算出される前記並進方向目標移動量および前記回転方向目標移動量に基づいて、前記ロボットの動作指令を生成する動作指令生成部と、を備え、前記並進力制御方向目標移動量算出部は、前記並進力制御方向における前記一方と前記他方との間に作用する力が、並進力制御方向の力に関する第１の所定閾値未満のとき、所定の移動量である第１の並進力制御方向移動量、または、前記所定の移動量である第１の並進力制御方向移動量および前記並進力制御方向の前記並進力制御目標力に基づいて、前記並進力制御方向目標移動量を算出し、前記並進力制御方向における前記一方と前記他方との間に作用する力が、前記並進力制御方向の力に関する第１の所定閾値以上のとき、前記並進力制御方向における前記一方と前記他方との間に作用する力および前記並進力制御方向の前記並進力制御目標力、または、前記並進力制御方向における前記一方と前記他方との間に作用する力および前記並進力制御方向の前記並進力制御目標力および所定の移動量である第２の並進力制御方向移動量、に基づいて、前記並進力制御方向目標移動量を算出する、ことを特徴とするロボット制御装置を提供する。
【００２３】
第２の発明は、第１の発明において、前記並進力制御方向目標移動量算出部は、前記一方の前記回転力制御軸の軸回りの回転移動の移動量と、前記一方の前記回転力制御軸の軸回りの回転移動に伴う、前記一方と前記他方とが接触する部分の並進移動の移動量との関係に基づいて、前記回転力制御軸回り目標移動量を基に、前記並進力制御方向目標移動量の補正移動量を算出し、前記補正移動量分を補正して前記並進力制御方向目標移動量を算出する、ロボット制御装置を提供する。
【００２４】
第３の発明は、第１または第２の発明において、前記ロボット制御装置は、さらに、前記一方と前記他方とが接触するときの接触位置を設定する、または、前記一方と前記他方との間に作用する力に基づいて前記一方と前記他方とが接触するときの接触位置を算出して設定する接触位置設定部を備え、前記回転力制御軸設定部は、所定の回転力制御軸を１つ以上設定すると共に、前記一方における前記接触位置を通る軸または前記他方における前記接触位置を通る軸、かつ、前記所定の回転力制御軸に平行な軸を、接触位置基準の回転力制御軸として１つ以上設定し、前記回転力制御軸回り目標移動量算出部は、前記所定の回転力制御軸の軸回りの力に基づいて、または、前記所定の回転力制御軸に対して算出される前記回転力制御軸の軸回りに回転移動させる場合の目標移動量に基づいて、前記接触位置基準の回転力制御軸の軸回りに回転移動させる目標移動量を算出して、前記算出された目標移動量を前記回転力制御軸回り目標移動量とする、ロボット制御装置を提供する。
【００２５】
第４の発明は、第１から第３のいずれか１つの発明において、前記ロボット制御装置は、さらに、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにする部分における、前記一方の前記接触させる部分の姿勢または前記他方の前記接触させる部分の姿勢または前記一方および前記他方の前記接触させる部分の姿勢を算出する接触部分姿勢算出部を備え、前記接触部分姿勢算出部は、前記一方と前記他方との間に作用する力に基づいて前記一方と前記他方とが接触するときの接触位置を算出し、複数の前記算出された接触位置に基づいて前記一方と前記他方とを接触させる部分の姿勢を算出し、前記回転力制御軸回り目標移動量算出部は、前記一方と前記他方との間に作用する力、および、前記接触部分姿勢算出部が算出する前記一方と前記他方とを接触させる部分の姿勢に基づいて、前記回転力制御軸回り目標移動量を算出する、ロボット制御装置を提供する。
【００２６】
第５の発明は、第１の発明において、前記ロボット制御装置は、さらに、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにするときの姿勢である合わせ状態姿勢を算出する合わせ状態姿勢算出部を備え、前記ロボット制御装置は、前記一方と前記他方とが接触したとき、前記一方の前記他方に対する前記並進力制御方向の移動を停止させると共に、前記一方の姿勢を取得し、次に前記一方の前記他方に対する前記並進力制御方向の移動を停止させると共に、前記一方を前記他方に対して前記回転力制御軸の軸回りに回転移動させ、前記一方と前記他方とが接触したときの前記一方の姿勢を取得し、前記合わせ状態姿勢算出部は、前記一方と前記他方とが接触したときに前記取得された前記一方の姿勢に基づいて前記合わせ状態姿勢を算出し、前記回転力制御軸回り目標移動量算出部は、前記一方と前記他方との間に作用する力、および、前記合わせ状態姿勢算出部が算出した前記合わせ状態姿勢に基づいて、前記回転力制御軸回り目標移動量を算出する、ロボット制御装置を提供する。
【００２７】
第６の発明は、第１から第５のいずれか１つの発明において、前記回転力制御軸回り目標移動量算出部は、前記並進力制御方向設定部によって設定される前記並進力制御方向の内、いずれかの前記並進力制御方向と平行でない前記回転力制御軸について軸回りの目標移動量を算出する際、前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、前記第１の接触状態であって、前記第２の接触状態から前記第１の接触状態になるときではない場合、前記回転力制御軸回り目標移動量を０と算出し、前記第２の接触状態から前記第１の接触状態となる場合、前記回転力制御軸回り目標移動量を０と算出する、または、前記第１の接触状態となるまでの前記第２の接触状態のときの前記回転力制御軸の軸回りの移動方向および移動量に基づいて前記回転力制御軸回り目標移動量を算出し、前記第２の接触状態となる場合、前記回転力制御軸の軸回りの力の符号と共に、回転方向の所定の移動量である所定の回転方向移動量Ａ、または、前記回転力制御軸と平行でない前記並進力制御方向の並進力制御目標力、または、前記回転力制御軸と平行でない前記並進力制御方向の力、の内、少なくとも１つに基づいて、前記回転力制御軸回り目標移動量を算出する、ロボット制御装置を提供する。
【００２８】
第７の発明は、第１から第５のいずれか１つの発明において、前記回転力制御軸回り目標移動量算出部は、前記並進力制御方向設定部によって設定される前記並進力制御方向の内、いずれかの前記並進力制御方向と平行でない前記回転力制御軸について軸回りの目標移動量を算出する際、前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、前記第１の接触状態であって、前記第２の接触状態から前記第１の接触状態になるときではない場合、前記回転力制御軸回り目標移動量を０と算出し、前記第２の接触状態から前記第１の接触状態となる場合、前記回転力制御軸回り目標移動量を０と算出する、または、前記第１の接触状態となるまでの前記第２の接触状態のときの前記回転力制御軸の軸回りの移動方向および移動量に基づいて前記回転力制御軸回り目標移動量を算出し、前記第２の接触状態となる場合、前記回転力制御軸の軸回りの力、または、前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、または、回転方向の所定の移動量である所定の回転方向移動量Ｂおよび前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、に基づいて、前記回転力制御軸回り目標移動量を算出する、ロボット制御装置を提供する。
【００２９】
第８の発明は、第１から第５のいずれか１つの発明において、前記ロボット制御装置は、さらに、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態である、合わせ状態になっているか否か、または、前記一方と前記他方とが前記合わせ状態に近い状態である、合わせ状態近傍になっているか否か、の前記一方と前記他方との前記合わせ状態の判定を実行する合わせ状態判定部を備え、前記回転力制御軸回り目標移動量算出部は、前記並進力制御方向設定部によって設定される前記並進力制御方向の内、いずれかの前記並進力制御方向と平行でない前記回転力制御軸について軸回りの目標移動量を算出する際、前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、前記回転力制御軸と平行でない前記並進力制御方向における前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記回転力制御軸の軸回りにおける前記一方と前記他方との間に作用する力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、前記第１の接触状態であって、前記第２の接触状態から前記第１の接触状態になるときではない場合、前記回転力制御軸回り目標移動量を０と算出し、前記第２の接触状態から前記第１の接触状態となる場合、前記回転力制御軸回り目標移動量を０と算出する、または、前記第１の接触状態となるまでの前記第２の接触状態のときの前記回転力制御軸の軸回りの移動方向および移動量に基づいて前記回転力制御軸回り目標移動量を算出し、前記第２の接触状態であって前記合わせ状態判定部によって前記合わせ状態近傍または前記合わせ状態になっていると判定される状態、または、前記第２の接触状態であって前記合わせ状態判定部によって前記合わせ状態近傍または前記合わせ状態になっていると判定されたとき以降の状態、を第２Ａの接触状態とし、前記第２の接触状態であって前記第２Ａの接触状態でない状態を第２Ｂの接触状態とするとき、前記第２Ｂの接触状態である場合、前記回転力制御軸の軸回りの力の符号と共に、回転方向の所定の移動量である所定の回転方向移動量Ａ、または、前記回転力制御軸と平行でない前記並進力制御方向の並進力制御目標力、または、前記回転力制御軸と平行でない前記並進力制御方向の力、の内、少なくとも１つに基づいて、前記回転力制御軸回り目標移動量を算出し、前記第２Ａの接触状態である場合、前記回転力制御軸の軸回りの力、または、前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、または、回転方向の所定の移動量である所定の回転方向移動量Ｂおよび前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、に基づいて前記回転力制御軸回り目標移動量を算出する、または、前記第２Ｂの接触状態である場合に算出される前記回転力制御軸回り目標移動量より小さい値に調整して前記回転力制御軸回り目標移動量を算出する、ロボット制御装置を提供する。
【００３０】
第９の発明は、第８の発明において、前記回転力制御軸と平行でない前記並進力制御方向における、前記一方と前記他方との間に作用する力が前記並進力制御方向の力に関する第２の所定閾値以上、かつ、前記合わせ状態判定部によって前記合わせ状態になっていると判定される状態が、所定の時間以上連続するとき、前記回転力制御軸回り目標移動量算出部は、前記回転力制御軸回り目標移動量を０とする、または、前記回転力制御軸回り目標移動量が、算出された前記回転力制御軸回り目標移動量より小さくなるように調整して算出し、前記並進力制御方向目標移動量算出部は、前記並進力制御方向目標移動量を０とする、または、前記並進力制御方向目標移動量が、算出された前記並進力制御方向目標移動量より小さくなるように調整して算出する、ロボット制御装置を提供する。
【００３１】
第１０の発明は、第１から第９のいずれか１つの発明において、前記ロボット制御装置は、さらに、前記ロボットの台座の位置または姿勢または位置および姿勢が移動するとき、前記ロボットの台座の移動に基づいて、前記他方に対する前記一方の移動方向および移動量を補正して、または、前記他方の位置または姿勢または位置および姿勢が移動するとき、前記他方の移動に基づいて、前記他方に対する前記一方の移動方向および移動量を補正して、または、前記ロボットの台座の位置または姿勢または位置および姿勢と、前記他方の位置または姿勢または位置および姿勢とが移動するとき、前記ロボットの台座の移動、および、前記他方の移動に基づいて、前記他方に対する前記一方の移動方向および移動量を補正して、前記一方を前記他方に対して移動させるように前記ロボットの動作指令を生成する、ロボット制御装置を提供する。
【００３２】
第１１の発明は、第１から第１０のいずれか１つの発明において、前記ロボット制御装置は、さらに、前記一方と前記他方との間に所定時間中に所定の閾値を超える力がかかる状態、または前記一方と前記他方との間に別の所定の閾値を超える力がかかる状態、または前記一方と前記他方との間に作用する力の発振状態、のうちの少なくとも１つの状態を検出することにより、前記一方と前記他方との間に作用する力の不安定状態を検出する力不安定状態検出部と、前記ロボットの特異姿勢の近傍状態、または前記ロボットの発振状態、のうちの少なくとも１つの状態を検出することにより、前記ロボットの移動動作の不安定状態を検出する移動動作不安定状態検出部と、を備え、前記力不安定状態検出部によって前記力の不安定状態が検出されたとき、または、前記移動動作不安定状態検出部によって前記ロボットの移動動作の不安定状態が検出されたとき、前記ロボット制御装置は、前記回転力制御軸回り目標移動量を０にする、または、前記力の不安定状態または前記ロボットの移動動作の不安定状態が検出されない場合に算出される前記回転力制御軸回り目標移動量より小さい値となるように前記回転力制御軸回り目標移動量を調整して算出する、ロボット制御装置を提供する。
【００３３】
第１２の発明は、第１から第１１のいずれか１つの発明において、前記ロボット制御装置は、さらに、前記並進力制御方向の力の大きさが所定閾値を超える場合、前記並進力制御方向の力の大きさを前記所定閾値として、前記回転力制御軸の軸回りの力の大きさが別の所定閾値を超える場合、前記回転力制御軸の軸回りの力の大きさを別の所定閾値とする、ロボット制御装置を提供する。
【００３４】
第１３の発明は、第１から第１２のいずれか１つの発明において、前記並進力制御方向設定部は、前記一方に対する所定の方向、または、前記他方に対する所定の方向、または、前記一方の移動動作によらない所定の方向、または、前記回転力制御軸が設定されている場合、前記回転力制御軸、の内、少なくとも１つに基づいて、１つ以上の前記並進力制御方向を設定し、前記回転力制御軸設定部は、前記一方に対する所定の方向、または、前記他方に対する所定の方向、または、前記一方の移動動作によらない所定の方向、または、前記並進力制御方向が設定されている場合、前記並進力制御方向、の内、少なくとも１つに基づいて、１つ以上の前記回転力制御軸を設定する、ロボット制御装置を提供する。
【００３５】
第１４の発明は、第１から第１３のいずれか１つの発明において、前記並進力制御方向設定部は、前記一方と前記他方とが接触するとき、前記並進力制御方向の成分を含む方向、かつ、所定範囲を限度とする別の方向に、前記一方を前記他方に対して押しつけたときの、前記回転力制御軸の軸回りの力に基づいて、前記並進力制御方向を設定する、ロボット制御装置を提供する。
【００３６】
第１５の発明は、第１から第１４のいずれか１つの発明において、前記回転力制御軸回り目標移動量算出部は、前記回転力制御軸が２つ以上ある場合、所定の前記回転力制御軸について、前記一方と前記他方との間に作用する力に基づいて、前記回転力制御軸の軸回りの力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ｂより小さいとき、回転方向の所定の移動量である所定の回転方向移動量Ｃを前記回転力制御軸回り目標移動量とし、前記回転力制御軸の軸回りの力が前記回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ｂ以上のとき、前記回転力制御軸の軸回りの力と軸回りの所定の目標力、または、回転方向の所定の移動量である所定の回転方向移動量Ｄおよび前記回転力制御軸の軸回りの力と軸回りの前記所定の目標力に基づいて、前記回転力制御軸回り目標移動量を算出する、ロボット制御装置を提供する。
【発明の効果】
【００３７】
本発明によれば、一方の物体を他方の物体に対して、所定の力で押しつけながら、回転移動させて姿勢を変えるとき、非接触状態となりにくくする、または一方の物体と他方の物体との間に作用する力に基づいて一方の物体を他方の物体に対して相対的に並進方向に移動させるときの方向である並進力制御方向の力が小さい状態となりにくくすることによって、より安定的かつより素早く回転移動させて、短時間で、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにすることができる。
【００３８】
また、前記一方を前記他方に対して相対的に回転移動させて姿勢を変えるとき、非接触状態となりにくくする、または一方の物体と他方の物体との間に作用する力に基づいて一方の物体を他方の物体に対して相対的に並進方向に移動させるときの方向である並進力制御方向の力が小さい状態となりにくくすると共に、回転移動の速度を安定的に大きくすることによって、より素早く回転移動させることができる。これによって、並進力制御方向の力制御の目標力が小さい場合であっても、安定的に素早く回転移動させて、短時間で、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにできる。
【００３９】
また、安定的に素早く回転移動させると共に、前記一方と前記他方との接触の状態に応じて、適宜、回転移動の速度を調整することによって、目的とする状態に素早く収束させることができる。
【００４０】
前述のようにして、本発明によれば、接触させるときの力が小さい場合であっても、安定的かつ短い時間で、前記一方の所定の部分と前記他方の所定の部分とが接触して合わされた状態となるようにすることができる。
【図面の簡単な説明】
【００４１】
【図１】本発明の一実施形態に係るロボット制御装置によって制御されるロボットを具備するロボットシステムの概略構成を示す図である。
【図２】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図３】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図４】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図５】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図６】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図７】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図８】発明の一実施形態に係るロボット制御装置の構成を機能的に示す図である。
【図９】合わせ状態の一例を示す図である。
【図１０】合わせ状態の一例を示す図である。
【図１１】合わせ状態の一例を示す図である。
【図１２】合わせ状態の一例を示す図である。
【図１３】合わせ状態にするときの一例を示す図である。
【図１４】合わせ状態にするときの問題点の例を示す図である。
【図１５】合わせ状態にするときの問題点の別の例を示す図である。
【図１６】合わせ状態にするときの動作を示す部分拡大図である。
【図１７】本発明の一実施形態に係るロボット制御装置による処理の一例を示すフローチャートである。
【図１８】合わせ状態にするときの一例を示す図である。
【図１９】合わせ状態にするときの一例を示す図である。
【図２０】並進力制御方向の設定方法を説明する部分拡大図である。
【図２１】並進力制御方向の別の設定方法を説明する部分拡大図である。
【発明を実施するための形態】
【００４２】
以下、添付図面を参照して本発明の実施形態を説明する。以下の図面において同様の部材には同様の参照符号が付けられている。理解を容易にするために、これら図面は縮尺を適宜変更している。
【００４３】
本発明において、「力」とは、特に断りがない限り、力の並進方向成分および力のモーメント成分を含むものとする。また、「位置および／または姿勢」とは、位置および姿勢の少なくとも一方を含むものとする。「力制御ゲイン」とは、作用する力に応じてロボットを移動させる力制御において、作用する力の大きさをもとに、ロボット制御装置の制御周期毎の、直交座標系上のロボットの先端部の位置および／または姿勢や、ロボットの各軸の位置などの移動量を求めるための係数とする。また、「合わせ状態」とは、一方の物体の所定の部分と他方の物体の所定の部分とが接触して合わされた状態とする。また、「合わせ状態近傍」とは、合わせ状態とする一方の物体の所定の部分と他方の物体の所定の部分とが近づき、合わせ状態に近い状態となっている状態とする。
【００４４】
本実施例では、空間に対して設定した座標系とは、空間に対して固定させた直交座標系上で、ロボットの手先部や、ロボットの手先部に保持された（取りつけられた状態も含む）一方の物体の位置および／または姿勢を表すことが可能な座標系とする。また、ツール座標系とは、ロボットの手先部またはロボットの手先部に保持された一方の物体に対して設定される座標系であり、ロボットの台座に対して設定される座標系上または空間に対して設定した基準座標系上の、ロボットの手先部またはロボットの手先部に保持された一方の物体の位置および／または姿勢を表すことが可能な座標系とする。また、制御点とは、ロボットの手先部や手先部に保持される一方の物体の位置を表す点とする。また、力制御座標系とは、一方の物体と他方の物体との間に作用する力に基づいて、ロボットの手先部に保持された一方の物体の位置および／または姿勢を制御するために、制御点に原点を持つように設定される座標系とする。
【００４５】
添付図面を参照して、本発明の一実施形態に係るロボット制御装置の例であるロボットシステムの構成及び作用について説明する。図１は、本発明の一実施形態に係るロボット制御装置１０と、ロボット制御装置１０によって制御されるロボット５０を具備するロボットシステム１１の構成例を示す概略図である。ロボット制御装置１０は、ロボット５０の手先部５１に保持される一方の物体２１の他方の物体２２に対する相対的な位置および／または姿勢を移動させ、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態である、合わせ状態となるように、両者間に作用する力を制御してロボット５０を移動させる。ロボットシステム１１では、ロボット５０の手先部５１に力センサ２５と一方の物体２１を保持するための保持部２３が取りつけられ、保持部２３は一方の物体２１を保持している。また、他方の物体２２は、他方の物体の設置装置２４に設置されている。また、ロボット５０が設置されたロボットの台座５２は、ロボットの設置装置５３に設置されている。
【００４６】
図９から図１２を参照して、ロボット５０の手先部５１に保持される一方の物体２１を他方の物体２２に対して相対的に移動させ、一方の物体２１と他方の物体２２とが、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた、合わせ状態となるようにするときの、一方の物体２１と他方の物体２２との合わせ状態について説明する。
【００４７】
図９は、一方の物体２１と他方の物体２２とを合わせ状態にするときの、合わせ状態の一例を説明する図である。一方の物体２１が図９（ｃ）に示す直方体形状の物体、または図９（ｄ）に示す円柱形状の物体であり、他方の物体２２が平面部分を含む物体とする。図９（ａ）、図９（ｂ）は、一方の物体２１と他方の物体２２とを合わせ状態にするときの断面図である。なお、説明を簡単にするため、回転移動の方向は一方向のみとしている。図９（ａ）に示されるように、一方の物体２１と他方の物体２２とを相対的に並進移動および回転移動させて接触させ、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが合うように移動させる。図９（ｂ）、図９（ｃ）に示されるように一方の物体２１と他方の物体２２とを相対的に移動させ、一方の物体２１の所定の面と他方の物体２２の所定の面とが接触し合わされた状態を合わせ状態とする。また、一方の物体２１の直線部分が他方の物体２２の平面部分に接触して合わされた状態を、合わせ状態としてもよい。また、一方の物体２１を円柱形状の物体とするとき、図９（ｂ）、図９（ｄ）に示されるように、一方の物体２１と他方の物体２２とを相対的に移動させ、一方の物体２１の円柱形状の所定の直線部分と、他方の物体２２の面部分とが接触して合わされた状態を合わせ状態とする。
【００４８】
図１０は、一方の物体２１と他方の物体２２とを合わせ状態にするときの、合わせ状態の別の一例を説明する図である。図１０に示される一方の物体２１は円筒形状の物体であり、他方の物体２２は平面部分を含む物体とする。図１０（ａ）、図１０（ｂ）は、一方の物体２１と他方の物体２２とを合わせ状態にするときの断面図である。なお、説明を簡単にするため、回転移動の方向は一方向のみとしている。図１０（ａ）に示されるように、一方の物体２１と他方の物体２２とを相対的に並進移動および回転移動させて接触させ、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが合うように移動させる。図１０（ｂ）、図１０（ｃ）に示されるように、前記一方の物体２１の所定の部分を一方の物体２１の底面とし、円筒形状の一方の物体２１の底面と、他方の物体２２の平面部分とが接触して面部分が合うように移動させるとき、一方の物体２１の所定の面と他方の物体２２の所定の面とが接触して合わされた状態を合わせ状態とする。このように、合わせ状態にする所定の部分は、円筒の底面のような平面部分に穴があるような面、平面上において連続的に物体が存在するような面の形状であってよく、または、平面上において不連続に物体が存在するような面の形状であってもよい。
【００４９】
図１１は、一方の物体２１と他方の物体２２とを合わせ状態にするときの、合わせ状態のさらに別の一例を説明する図である。図１１に示される一方の物体２１は、円錐形状の中に円錐の空洞がある物体であり、他方の物体２２は平面部分を含む物体とする。図１１に示すように他方の物体２２は、傾斜した状態で位置しているとする。図１１（ａ）、図１１（ｂ）は、一方の物体２１と他方の物体２２とを合わせ状態にするときの断面図である。なお、説明を簡単にするため、回転移動の方向は一方向のみとしている。図１１（ａ）に示されるように、一方の物体２１と他方の物体２２とを相対的に並進移動および回転移動させて接触させ、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが合うように移動させる。図１１（ｂ）、図１１（ｃ）に示されるように、前記一方の物体２１の所定の部分を一方の物体２１の底面とするとき、前述の一方の物体２１の底面と、他方の物体２２とを平面部分とが接触して面部分が合うように移動させるとき、一方の物体２１の所定の面と他方の物体２２の所定の面とが接触して合わされた状態を合わせ状態とする。このように、一方の物体２１は、面部分を含む複雑な形状を有する物体であってもよい。
【００５０】
図１２は、一方の物体２１と他方の物体２２とを合わせ状態にするときの、合わせ状態のさらに別の一例を説明する図である。図１２（ａ）、図１２（ｂ）、図１２（ｃ）に示される一方の物体２１は、図１２（ｃ）に示されるような、円錐形状の物体であり、他方の物体２２は円筒形状の物体とする。図１２（ａ）、図１２（ｂ）は、一方の物体２１と他方の物体２２とを合わせ状態にするときの断面図である。なお、説明を簡単にするため、回転移動の方向は一方向のみとしている。図１２（ａ）に示されるように、一方の物体２１と他方の物体２２とを相対的に並進移動および回転移動させて接触させ、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが合うように移動させる。ここでは、一方の物体２１の所定部分と他方の物体２２の所定部分とを合わせ状態にするときの所定部分は、円錐形状の一方の物体２１では円錐の側面部分とし、円筒形状の他方の物体２２では円筒の上面部分としている。この場合、図１２（ｂ）、図１２（ｃ）に示されるように、円錐形状の一方の物体２１の円錐の側面部分と、円筒形状の他方の物体２２の円筒の上面部分とが、接触して合わされた状態を合わせ状態とする。このように、一方の物体２１と他方の物体２２とが接触して合わされた部分が同一平面上に存在する状態を合わせ状態としてもよい。また、一方の物体２１と他方の物体２２とが接触して合わせられた部分が、一方の物体２１または他方の物体２２の形状による所定の曲面上に存在する状態を、合わせ状態としてもよい。
【００５１】
嵌合では、形状が合ったもの同士を嵌め合わせる、または、軸をくぼんだ所にかたくはまり合わせる、また、挿入が進むにつれて一方の物体と他方の物体とが嵌め合わされていく、さらに、通常、姿勢がずれた物体どうしを力制御で嵌め合わせるときには所定の大きさ以上のクリアランスを必要とするので、嵌合と、前述のような合わせ状態にすることとは異なる。また、このような合わせ状態にするとき、一方の物体２１と他方の物体２２との或る所定の部分においてのみ、所望の状態、かつ、接触された状態で合わせられるように一方の物体２１と他方の物体２２との相対的な位置および姿勢が変えられること、また、嵌合作業時に発生する、物体が詰まって動かなくなる現象などが発生しないことなどにおいても、嵌合と、前述のような合わせ状態にすることとは異なる。
【００５２】
一方の物体２１と他方の物体２２との間に作用する力に基づいて、このような合わせ状態に移動させるには、円錐形状の一方の物体２１と円筒形状の他方の物体２２とを接触させると共に、円錐形状の一方の物体２１の円錐部分の頂点に、または、円錐形状の一方の物体２１の物体の軸線上の点に、一方の物体２１の移動動作によらない所定の方向または他方の物体２２に対する所定の方向と平行な回転軸を設定して、その回転軸回りの力がつりあうように、両者間に作用する力を制御する。または、円錐形状の一方の物体２１と円筒形状の他方の物体２２とを接触させると共に、円筒形状の他方の物体２２の円筒の上面部分の中心点に、一方の物体２１の移動動作によらない所定の方向または他方の物体２２に対する所定の方向と平行な回転軸を設定して、その回転軸回りの力がつりあうように、両者間に作用する力を制御する。
【００５３】
また、図１２（ｄ）に示すように、一方の物体２１が円筒形状の物体であって、他方の物体２２が円錐形状の物体であってもよい。前述と同様の方法で、円筒形状の一方の物体２１と円錐形状の他方の物体２２とを相対的に移動させることによって、図１２（ｄ）に示されるような、円筒形状の一方の物体２１の底面と、円錐形状の他方の物体２２の側面とを合わせた、合わせ状態にすることができる。
【００５４】
また、前述の説明における円筒形状の物体を、内部に直方体の空洞があるような、断面が四角の形状でくりぬかれた形状の物体に置き換えて、一方の物体２１を円錐形状の物体として、他方の物体２２を断面が四角の形状でくりぬかれた直方体としてもよい。また、その逆として、一方の物体２１を断面が四角の形状でくりぬかれた直方体とし、他方の物体２２を円錐形状の物体としてもよい。そのような一方の物体２１と他方の物体２２とを相対的に移動させることによって、横からの断面図で見たとき、図１２（ａ）に示す状態から移動させ、一方の物体２１と他方の物体２２とが接触して所定の部分が合わせられ、横からの断面図が図１２（ｂ）に示される状態であり、一方の物体２１と他方の物体２２とが接触している部分が存在する平面上における断面図が図１２（ｅ）に示される断面図の状態のような状態を、合わせ状態としてもよい。
【００５５】
以上のように、一方の物体２１と他方の物体２２とを、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態にすることが可能であれば、一方の物体２１および他方の物体２２とは、どのような形状を有する物体の組み合わせで構成された物体であってもよい。また、一方の物体２１および他方の物体２２は、それぞれ複数形状の物体により構成された物体であってもよい。
【００５６】
ロボット制御装置１０は、所定の制御周期毎にロボット５０の各軸の位置を制御するように構成されている。また、ロボット制御装置１０は、演算処理装置、ＲＯＭ、ＲＡＭなどを含むハードウェア構成を有していて、後述する種々の機能を実行する。
【００５７】
本実施例のロボット５０は、６つの軸によって構成される垂直多関節型ロボットとする。しかしながら、ロボット５０は、ロボット制御装置１０によって制御される複数の軸を有し、作業に必要な移動動作が可能であれば、他の形態を有する公知のロボットでもよい。ロボットの設置装置５３は、位置および／または姿勢が移動可能な機構部を有する装置とする。例えば、回転移動や並進移動が可能なジグ、走行軸、床を移動可能なAGV（無人搬送車）などの移動ロボット、３次元空間内の位置および／または姿勢が移動可能な別の多関節型ロボットなどが考えられる。本実施例のロボットの台座５２はロボットの設置装置５３に設置されているとするが、平らな床や、傾斜のある架台や、位置および／または姿勢が移動可能な機構部を持つ装置や走行軸に設定されていてもよい。本実施例においては、説明の簡単化のために、特に述べない限り、ロボットの設置装置５３の移動動作を停止させているとする。なお、ロボット５０が移動可能な機構部を有する装置に設置されている場合、一方の物体２１を他方の物体２２に対して相対的に移動させるとき、他方の物体２２に対する一方の物体２１の位置および／または姿勢は、移動可能な機構部を有する装置によって移動させられる、一方の物体２１の移動方向および移動量をもとに補正して算出され、一方の物体２１は算出された移動方向および移動量をもとに他方の物体２２に対して移動されるようにしてもよい。
【００５８】
一方の物体２１は、ワーク、工具、装置、加工装置、検査装置、計測装置、ジグ、アダプタなどであり、他方の物体２２は、ワーク、工具、装置、加工装置、検査装置、計測装置、ジグ、アダプタ、作業台、コンベアの搬送台、コンベアの搬送台に置かれたワーク、位置および／または姿勢が移動可能な機構部を有する装置に存在するワーク、または、別のロボットの手先部にあるハンドやワークやアダプタなどである。一方の物体２１を作業台に置かれた他方の物体２２に接触させたり、一方の物体２１を作業台や装置などに接触させたりするようにしてもよい。一方の物体２１と他方の物体２２との組み合わせは、前述のような物体において、様々な組み合わせが考えられる。一方の物体２１はロボット５０の手先部５１の部分に存在すればよい。本実施例では、一方の物体２１は保持部２３で保持されるが、力センサ２５に取りつけられるようにして保持されるように、または、ロボット５０の手先部５１に取りつけられるようにして保持されるようにしてもよい。本実施例においては、他方の物体２２は、移動可能な機構部を有する装置である他方の物体の設置装置２４に設置されているとする。他方の物体の設置装置２４は、例えば、位置および／または姿勢が移動可能な機構部を有する、装置、コンベア、回転移動や並進移動が可能なジグ、床を移動可能なAGV（無人搬送車）などの移動ロボット、３次元空間内の位置および／または姿勢が移動可能な別の多関節型ロボットなどが考えられる。また、他方の物体の設置装置２４は、移動可能な機構部を有しない作業台などであってもよい。本実施例においては、説明の簡単化のために、特に述べない限り、他方の物体の設置装置２４の移動動作を停止させているとする。なお、他方の物体の設置装置２４が移動可能な機構部を有する装置である場合、一方の物体２１を他方の物体２２に対して相対的に移動させるとき、他方の物体２２に対する一方の物体２１の位置および／または姿勢は、他方の物体の設置装置２４によって移動させられる、他方の物体２２の移動方向および移動量をもとに補正して算出され、一方の物体２１は算出された移動方向および移動量をもとに他方の物体２２に対して移動されるようにしてもよい。
【００５９】
力センサ２５は、一方の物体２１と他方の物体２２との間に作用する力を検出する検出装置である。ロボット制御装置１０は、所定時間毎に検出された力センサ２５の出力をもとに、後述の力計測部３１によって、一方の物体２１と他方の物体２２との間に作用する力を計測する。力センサ２５は、本実施例では６軸の力センサとするが、作業に必要な自由度を有する力センサであればよい。また、力センサ２５の取付位置は、一方の物体２１と他方の物体２２との間に作用する力を検出できる限り、ロボット５０の手先部や関節部部分やロボット５０の設置部分、他方の物体２２の設置部分など、任意の箇所であってよい。また、力の検出方向（並進方向、回転方向、関節軸回りなど）、力の検出精度、力の検出分解能、力の検出目的などの異なる、力センサその他一方の物体２１と他方の物体２２との間に作用する力を検出、推定する際に必要な情報を検出するセンサを、前述のような異なる箇所に複数、設けて、組み合わせて使用するようにしてもよい。なお、本実施例においては、力計測部３１は力センサ２５の出力をもとに一方の物体２１と他方の物体２２との間に作用する力を計測するが、力センサ２５を使用せず、他の物理的情報を検出して、一方の物体２１と他方の物体２２との間に作用する力を計測するようにしてもよい。
【００６０】
図２は、本発明の１番目の実施形態に係るロボット制御装置１０ａの構成を機能的に示す図である。図示されるように、ロボット制御装置１０ａは、力計測部３１と、並進力制御方向設定部３２と、並進力制御目標力設定部３３と、回転力制御軸設定部３４と、並進力制御方向目標移動量算出部３５と、回転力制御軸回り目標移動量算出部３６と、目標方向目標移動量算出部３７と、動作指令生成部３８とを具備している。なお、ロボット制御装置１０は、図示しないが、他に、記憶部や各種信号を出力する信号出力部も具備している。
【００６１】
力計測部３１は、ロボット５０の手先部５１に保持される一方の物体２１と、他方の物体２２との間に作用する力を計測する。このために、力センサ２５が検出する力データをもとに、一方の物体２１、ロボット５０の手先部５１、ロボット５０に対して、設定される制御点を座標系原点とする力制御座標系上の力の並進方向の成分Ｆおよび力のモーメント成分Ｍを算出し、計測する。そして、力制御座標系上での力をもとに、後述の並進力制御方向設定部３２が設定する並進力制御方向について、一方の物体２１と他方の物体２２との間に作用する力を算出し、計測する。また、後述の回転力制御軸設定部３４が設定する回転力制御軸について、一方の物体２１と他方の物体２２との間に作用する回転力制御軸回りの力を算出し、計測する。ここで、本発明においては他の説明においても同様であるが、回転力制御軸回りと、回転力制御軸の軸回りとは、同じ意味とする。
【００６２】
なお、力計測部３１は、一方の物体２１と他方の物体２２との間に作用する力を計測するために、ロボット５０の手先部５１に取りつけられた力センサ２５を使用せず、ロボット５０を構成する軸を移動させるアクチュエータがモータである場合は電流値、または、ロボット５０を構成する軸の指令位置と実際の軸の位置との間の偏差、または、各軸に取付けられたトルクセンサの出力などをもとに、一方の物体２１と他方の物体２２との間に作用する力を推定して算出することによって、計測するようにしてもよい。
【００６３】
力計測部３１は、一方の物体２１と他方の物体２２との間に作用する力を計測するとき、力センサ２５が検出した力に対して、力センサ２５に取りつけられたツールやワークなどの物体が重力や慣性力（コリオリ力、ジャイロ効果を含む）などによって及ぼす影響を、公知の方法、例えば特許第４２６７０２７号に開示される手法などを用いて算出し、必要に応じて補償する。これにより、力計測部３１は、一方の物体２１と他方の物体２２との間に作用する正味の力を計測できる。または、力計測部３１は、力制御の開始時や力制御中などの、一方の物体２１と他方の物体２２とが非接触状態のときに取得される力をもとに、力を補償して、一方の物体２１と他方の物体２２との間に作用する力を算出して計測するようにしてもよい。
【００６４】
並進力制御方向設定部３２は、一方の物体２１を他方の物体２２に対して、一方の物体２１と他方の物体２２との間に作用する力に基づいて並進移動させるときの方向である、並進力制御方向を１つ以上設定する。並進力制御方向とは、一方の物体２１と他方の物体２２との間に作用する力に基づいて一方の物体２１を他方の物体２２に対して相対的に並進方向に移動させるときの方向であり、一方の物体２１を他方の物体２２に近づけ接触させる方向、または、並進方向の力が目標とする力となるように制御する方向とする。並進力制御方向設定部３２は、一方の物体２１と他方の物体２２との間に作用する力を、その方向について制御することによって一方の物体２１と他方の物体２２とが適切に接触するようになる方向に、並進力制御方向を設定することが好ましい。
【００６５】
また、一方の物体２１を他方の物体２２に対して或る方向に押しつけることによって回転力制御軸の軸回りの力を発生させて回転力制御軸の軸回りの回転移動をさせることを目的とせず、或る並進力制御方向について一方の物体２１と他方の物体２２との間に作用する力を制御することによって接触させながら受動的に移動させることを目的とする方向を並進力制御方向としてもよい。ただし、この場合には、一方の物体２１を他方の物体２２に対して別の或る並進力制御方向に押しつけることによって回転力制御軸の軸回りの力を発生させて回転力制御軸の軸回りの回転移動をさせるように、並進力制御方向を設定しておく。
【００６６】
並進力制御方向設定部３２は、次に述べるように並進力制御方向を設定する。
【００６７】
（１）並進力制御方向を、一方の物体２１に対する所定のツール座標系のいずれかの軸の方向と平行な方向とする。または、並進力制御方向を、ツール座標系上の所定の方向と平行な方向とする。このとき、並進力制御方向は、一方の物体２１の姿勢に応じて定まり、一方の物体２１の移動に応じた方向にすることができる。
【００６８】
（２）並進力制御方向を、他方の物体２２に対する所定の座標系のいずれかの軸の方向と平行な方向とする。または、他方の物体２２に対する所定の座標系上の所定の方向と平行な方向とする。または、他方の物体２２に対する所定の座標系と平行な座標系、かつ、座標系の原点が制御点にある座標系を力制御座標系に設定し、力制御座標系のいずれかの軸または力制御座標系上の所定の方向を、並進力制御方向とする。このとき、並進力制御方向は、他方の物体２２の姿勢に応じて定まり、他方の物体２２の姿勢が変化する場合、他方の物体２２の移動に応じた方向にすることができる。
【００６９】
（３）並進力制御方向を、一方の物体２１の移動動作によって位置および／または姿勢が変化しない、空間に対して設定した座標系のいずれかの軸または基準座標系のいずれかの軸と平行な方向とする。または、並進力制御方向を、一方の物体２１の移動動作によって位置および／または姿勢が変化しない、空間に対して設定した座標系上または基準座標系上の所定の方向と平行な方向とする。または、一方の物体２１の移動動作によって位置および／または姿勢が変化しない、空間に対して設定した座標系上または基準座標系と平行な座標系、かつ、座標系の原点が制御点にある座標系を力制御座標系に設定し、力制御座標系上のいずれかの軸の方向または力制御座標系上の所定の方向を、並進力制御方向とする。このとき、一方の物体２１の姿勢によらず、一方の物体２１を、所定の並進力制御方向に移動させることができる。
【００７０】
（４）後述の回転力制御軸設定部３４によって後述の回転力制御軸が設定されている場合、回転力制御軸の方向をもとに、並進力制御方向を、回転力制御軸と直交する方向と平行な方向、または、回転力制御軸と所定の角度を成す方向と平行な方向とする。回転力制御軸が２つ設定されている場合、２つの回転力制御軸と直交する方向と平行な方向を並進力制御方向としてもよい。このとき、回転力制御軸の方向に応じた並進力制御方向を設定することができる。
【００７１】
前記（１）から（４）の各設定方法について、２つ以上の方向を、並進力制御方向に設定してもよい。また、各設定方法によって定められる方向に基づいて、各設定方法の優先度に応じて並進力制御方向を選択的に設定したり、一方の物体２１や他方の物体２２の移動動作や接触の状況に応じて切り替えるように選択した方向を並進力制御方向に設定したり、各設定方法の方向の中から選択した方向のベクトルに重みをつけて足し合わせたベクトルの方向を並進力制御方向に設定したり、２つ以上の並進力制御方向を設定する場合は各並進力制御方向について前述の各設定方法によって定められた方向の中から選択した方向に基づいて並進力制御方向を設定したりしてもよい。
【００７２】
また、並進力制御方向設定部３２は、他方の物体２２が位置および／または姿勢を移動させるとき、他方の物体２２の移動方向や移動量を補正して、並進力制御方向を設定してもよい。また、ロボットの台座５２が、位置および／または姿勢の移動可能な機構部に設置されている場合、前記機構部の移動方向および移動量をもとに、他方の物体２２に対する一方の物体２１の並進力制御方向が所望の方向となるように並進力制御方向を補正してもよい。
【００７３】
並進力制御目標力設定部３３は、並進力制御方向設定部３２によって設定される並進力制御方向における、一方の物体２１と他方の物体２２との間に作用する力の目標値である並進力制御目標力を設定する。また、並進力制御方向設定部３２によって、２つ以上の並進力制御方向が設定されるとき、並進力制御目標力設定部３３は、各並進力制御方向について、並進力制御目標力を設定する。なお、並進力制御目標力は全ての並進力制御方向について同じ値であってもよい。
【００７４】
また、一方の物体２１を他方の物体２２に対して或る方向に押しつけることによって回転力制御軸の軸回りの力を発生させて回転力制御軸の軸回りの回転移動をさせることを目的とせず、或る並進力制御方向について一方の物体２１と他方の物体２２との間に作用する力を制御することによって接触させながら受動的に移動させることを目的とする場合、前記或る並進力制御方向の並進力制御目標力を０としてもよい。
【００７５】
並進力制御目標力設定部３３は、一方の物体２１や他方の物体２２の移動動作の状況に応じて、並進力制御目標力を変更するようにしてもよい。並進力制御目標力設定部３３は、一方の物体２１と他方の物体２２との間に作用する後述の回転力制御軸回りの力が所定時間以上、所定の閾値より小さい場合、または、一方の物体２１を移動させているときに一方の物体２１と他方の物体２２との間に作用する後述の回転力制御軸回りの力が所定時間当たりに別の所定時間以上、所定の閾値を下回る状態が所定回数以上である場合、または、一方の物体２１と他方の物体２２との間に作用する並進力制御方向の力が所定の閾値以上かつ一方の物体２１と他方の物体２２との間に作用する後述の回転力制御軸回りの力が所定の閾値より小さい場合など、後述の回転力制御軸回りの力が小さく、一方の物体２１を回転移動させることが困難であるような状況の場合には、並進力制御目標力を所定の値を上限として大きくするようにしてもよい。これによって、必要な場合のみ、並進力制御方向の力を大きくなるようにすることができる。
【００７６】
回転力制御軸設定部３４は、一方の物体２１を他方の物体２２に対して、一方の物体２１と他方の物体２２との間に作用する力に基づいて一方の物体２１を他方の物体２２に対して相対的に回転移動させるときの回転軸である回転力制御軸を１つ以上設定する。さらに、前記回転力制御軸の内、少なくとも１つの軸は並進力制御方向と平行ではない軸を設定する。回転力制御軸とは、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を他方の物体２２に対して相対的に回転移動させるときの回転軸とする。なお、回転力制御軸の内、少なくとも１つの軸は並進力制御方向と平行ではない軸を設定することによって、並進力制御方向に押しつけたときに、並進力制御方向と平行ではない回転力制御軸の軸回りについて一方の物体２１と他方の物体２２との間に作用する力を発生させ、その回転力制御軸の軸回りの力に基づいて、回転移動させることができる。回転力制御軸を適切に設定し、この回転力制御軸の軸回りの力に基づいて、移動させることによって、一方の物体２１の所定の部分と他方の物体２２の所定の部分とを接触して合わされた状態とすることができる。また、回転力制御軸は、一方の物体２１と他方の物体２２との直線部分、曲線部分、平面部分や曲面部分などを接触して合わせた、合わせ状態にしたり、一方の物体２１と他方の物体２２とを接触させるように移動させたりすることが可能となるような、回転軸とする。
【００７７】
回転力制御軸設定部３４は、次に述べるようにして回転力制御軸を設定する。なお、以下の（１）から（４）における、所定の点は、一方の物体２１に対する所定のツール座標系上に設定された点、または、他方の物体２２に対する所定の座標系上に設定された点、または、一方の物体２１の移動動作によって位置および／または姿勢が変化しない空間に対して設定した座標系または基準座標系上に設定された点とする。
【００７８】
回転力制御軸が、一方の物体２１に対して設定した制御点、または、一方の物体２１に対する所定のツール座標系上に設定された点を通るように設定されるとき、回転力制御軸は一方の物体２１の位置および姿勢に応じて定まるため、一方の物体２１の移動に応じて、一方の物体２１の回転移動の方向を変えることができる。
【００７９】
また、回転力制御軸が、他方の物体２２に対する所定の座標系上に設定された点を通過するように設定されるとき、他方の物体２２に対して、一方の物体２１を回転移動させることができる。このため、他方の物体２２が移動する場合、他方の物体２２の移動に応じて、一方の物体２１の回転移動の方向を変えることができる。
【００８０】
また、回転力制御軸が、一方の物体２１の移動動作によって位置および／または姿勢が変化しない空間に対して設定した座標系または基準座標系上に設定された点を通過するように設定されるとき、空間に対して設定した座標系または基準座標系上に設定された点を基準として、一方の物体２１を回転移動させることができる。
【００８１】
他方の物体２２が位置および／または姿勢を変えない、かつ、他方の物体２２に座標系を設定しない場合、空間に対して設定した座標系または基準座標系上に設定された点を基準に移動させてもよい。
【００８２】
回転力制御軸は、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態となるときの中心点を通る軸や、前記中心点から並進力制御方向上に存在する点を通る軸などとしてもよい。また、回転力制御軸が前記の点を通らず、このような点から離れていても、回転力制御軸の軸回りの力が所定の閾値以下であるか否かによって合わせ状態と判断するときの所定の閾値を調整することによって、合わせ状態となっていることを判定することができる。なお、並進力制御方向の並進力制御目標力が大きい場合、前記所定の閾値が大きくてもよいが、並進力制御方向の並進力制御目標力が小さい場合、前記所定の閾値が大きいと、合わせ状態であることを判定することが困難となる。
【００８３】
（１）回転力制御軸を、一方の物体２１に対する所定のツール座標系のいずれかの軸とする。または、回転力制御軸を、制御点を通る、かつ、ツール座標系上の所定の方向と平行な軸とする。または、回転力制御軸を、ツール座標系上のいずれかの軸と平行またはツール座標系上の所定の方向と平行、かつ、所定の点を通る軸とする。このとき、回転力制御軸は、一方の物体２１の移動に応じた回転軸とすることができる。
【００８４】
（２）回転力制御軸を、他方の物体２２に対する所定の座標系のいずれかの軸とする。または、回転力制御軸を、他方の物体２２に対する所定の座標系上における、所定の点を通る、かつ、所定の方向の軸とする。または、回転力制御軸を、他方の物体２２に対する所定の座標系上のいずれかの軸と平行、かつ、制御点または所定の点を通る軸とする。または、回転力制御軸を、他方の物体２２に対する所定の座標系上の所定の方向と平行、かつ、制御点または所定の点を通る軸とする。または、他方の物体２２に対する所定の座標系と平行な座標系かつ座標系の原点が制御点にある座標系を、力制御座標系に設定し、前記力制御座標系のいずれかの軸または力制御座標系上の所定の方向の軸を、回転力制御軸とする。このとき、他方の物体２２が位置および／または姿勢を移動させる場合、他方の物体２２の移動に応じた回転軸にすることができる。
【００８５】
（３）回転力制御軸を、一方の物体２１の移動動作によって位置および／または姿勢が変化しない、空間に対して設定した座標系のいずれかの軸または基準座標系のいずれかの軸と平行、かつ、制御点または所定の点を通る軸とする。または、回転力制御軸を、一方の物体２１の移動動作によって位置および／または姿勢が変化しない、空間に対して設定した座標系上または基準座標系上の所定の方向と平行、かつ、制御点または所定の点を通る軸とする。または、一方の物体２１の移動動作によって位置および／または姿勢が変化しない、空間に対して設定した座標系、または基準座標系と平行な座標系であって、かつ、座標系の原点が制御点にある座標系を力制御座標系に設定し、前記力制御座標系上のいずれかの軸または力制御座標系上の所定の方向の軸を、回転力制御軸とする。このとき、一方の物体２１の姿勢によらず、所定の回転軸回りに回転移動させることができる。
【００８６】
（４）並進力制御方向設定部３２によって並進力制御方向が設定されている場合、回転力制御軸を、並進力制御方向をもとに、並進力制御方向と直交する方向と平行、かつ、制御点または所定の点を通る軸とする。または、回転力制御軸を、並進力制御方向をもとに、並進力制御方向と所定の角度を成す方向と平行、かつ、制御点または所定の点を通る軸とする。または、並進力制御方向と直交かつ互いに直交、かつ、制御点または所定の点を通る、２つの軸を回転力制御軸としてもよい。並進力制御方向が２つ以上設定されている場合、所定のいずれかの並進力制御方向をもとに、回転力制御軸を設定するようにしてもよい。また、並進力制御方向に応じて、前記（１）、（２）、（３）による設定方法から選択して、回転力制御軸を設定するようにしてもよい。例えば、並進力制御方向が一方の物体２１に対する所定の方向である場合、他方の物体２２に対する所定の座標系または一方の物体２１の移動動作によらない座標系のいずれかの軸の内、２つの軸によって成される平面について、合わせ状態にする移動動作開始時の並進力制御方向と成す角度が直交に近い平面について、その平面を成す２つの軸を回転力制御軸と設定するようにしてもよい。このようにして、並進力制御方向に応じて、回転力制御軸を設定することができる。
【００８７】
また、前述の（１）から（４）の各設定方法を用いて、２つ以上の回転軸を、回転力制御軸に設定してもよい。
【００８８】
また、前述の（１）から（４）の各設定方法によって定められる軸に対して、状況に応じた各設定方法の優先度に応じて設定方法を選択し軸を回転力制御軸に設定したり、一方の物体２１や他方の物体２２の移動動作や接触の状況に応じて切り替えるように選択した方法の軸を回転力制御軸に設定したり、各設定方法によって定められる軸の内、選択した軸のベクトルの方向に重みをつけて前記ベクトル足し合わせたベクトルの方向の軸を回転力制御軸に設定したり、２つ以上の回転力制御軸を設定する場合は各回転力制御軸について各設定方法の回転軸の中から選択した軸に基づいて回転力制御軸を設定したりしてもよい。
【００８９】
また、回転力制御軸設定部３４は、他方の物体２２の位置および／または姿勢が変わるとき、他方の物体２２の移動方向および移動量に基づいて、回転軸の位置および／または姿勢を補正してもよい。また、ロボットの台座５２が、位置および／または姿勢の移動可能な機構部に設置されている場合、前記移動可能な機構部の移動方向および移動量をもとに、他方の物体２２に対する一方の物体２１の回転力制御軸回りの回転移動の方向が所望の方向となるように回転力制御軸の位置および／または姿勢を補正してもよい。
【００９０】
並進力制御方向目標移動量算出部３５は、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を他方の物体２２に対して並進力制御方向に並進移動させる目標移動量である並進力制御方向目標移動量を算出する。なお、並進力制御方向目標移動量は、ベクトルであり、移動方向および移動量の大きさを定めるとする。また、並進力制御方向目標移動量は、正負の符号を含み、並進力制御方向について、正方向または逆方向の、並進方向の移動の向きを定める。また、並進力制御方向設定部３２によって、２つ以上の並進力制御方向が設定されるとき、並進力制御方向目標移動量算出部３５は、各並進力制御方向について、並進力制御方向目標移動量を算出する。
【００９１】
並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が、並進力制御方向の力に関する第１の所定閾値未満のとき、所定の移動量である第１の並進力制御方向移動量、または、所定の移動量である第１の並進力制御方向移動量および並進力制御方向の並進力制御目標力に基づいて、並進力制御方向目標移動量を算出する。
【００９２】
並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満のとき、所定の並進移動速度や、並進移動速度を算出する関数などによって与えられる、ロボット制御装置１０の制御周期毎の所定の移動量である第１の並進力制御方向移動量に従って、並進力制御方向についての目標移動量を算出する。このときの並進力制御方向の力に関する第１の所定閾値は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が不十分であって接触していないと判定できる閾値であり、また、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が不十分であることによって後述の回転力制御軸の軸回りの力の大きさが小さくなっていて、その軸回りの力をもとに回転移動させることが困難な状態になっていると判定できる値とする。
【００９３】
第１の並進力制御方向移動量を定める所定の並進移動速度は、次のように設定してもよい。一方の物体２１と他方の物体２２とが接触するまでの並進移動速度と、一方の物体２１と他方の物体２２とが接触した後に、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満となるときの並進移動速度とは同じ大きさにしてもよい。また、最初に接触させるまでは速度を大きくして、一度接触した後は速度を小さくする場合、並進移動速度の大きさを変えてもよい。また、一方の物体２１と他方の物体２２とを最初に接触させた後は、非接触状態になってから接触状態になるまでの移動距離や時間が短くなる場合、一度接触した後に、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満になるとき、並進移動させるときの加速度を大きくしたり、目標とする並進移動速度を大きくしたりするようにして、移動動作中の並進移動速度が大きくなるようにしてもよい。所定の並進移動速度は、一方の物体２１と他方の物体２２とを接触させるときに発生する力について許容される大きさに基づいて設定したり、ロボット５０の移動動作における設定可能な時定数や、移動させているときの振動の状態などをもとに並進移動速度を自動的に調整して設定したりしてもよい。
【００９４】
また、並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満のとき、所定の移動量である第１の並進力制御方向移動量および並進力制御方向の並進力制御目標力に基づいて、並進力制御方向についての目標移動量を算出してもよい。つまり、前述のように、所定の並進移動速度に従って、並進力制御方向についての目標移動量を算出するときに、押付方方向の並進力制御目標力と力制御ゲインによって算出される速度をさらに足しこむようにして調整してもよい。これによって、並進力制御目標力の設定に応じて、並進力制御方向の並進移動速度を大きくしたり、小さくしたりして調整することができる。
【００９５】
並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が、並進力制御方向の力に関する第１の所定閾値以上のとき、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力および並進力制御方向の並進力制御目標力、または、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力および並進力制御方向の並進力制御目標力および所定の移動量である第２の並進力制御方向移動量、に基づいて、並進力制御方向目標移動量を算出する。このときの並進力制御方向の力に関する第１の所定閾値は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が不十分とはなっておらず接触していると判定できる値であり、また、後述の回転力制御軸の軸回りの力の大きさがある程度は大きくなっていて、その軸回りの力をもとに回転移動させることが可能な状態になっていると判定できる値とする。
【００９６】
並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が、並進力制御方向の力に関する第１の所定閾値以上のとき、一方の物体２１と他方の物体２２とが並進力制御方向について接触していると判定して、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が、並進力制御目標力となるように、並進力制御目標力と並進力制御方向の力との差分と力制御ゲインに基づいて、並進力制御方向目標移動量を算出する。
【００９７】
または、並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が、並進力制御方向の力に関する第１の所定閾値以上のとき、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満のときに用いる所定の第１の並進力制御方向移動量とは別に、所定の並進移動速度や、並進移動速度を算出する関数などによって与えられる、ロボット制御装置１０の制御周期毎の所定の移動量である第２の並進力制御方向移動量に基づいて、所定の移動量である第２の並進力制御方向移動量を並進力制御目標力と並進力制御方向の力によって調整し、並進力制御目標力と並進力制御方向の力との差分と力制御ゲインに基づいて算出される移動量に加算して、並進力制御方向目標移動量を算出する。
【００９８】
前述のように、所定の移動量である第１の並進力制御方向移動量は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満のときに使用する、所定の並進力制御方向の移動量であり、所定の移動量である第２の並進力制御方向移動量は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が、並進力制御方向の力に関する第１の所定閾値以上のときに使用する、所定の並進力制御方向の移動量である。
【００９９】
一方の物体２１と他方の物体２２とが最初に接触するより以前であって回転移動をしていないときに近づけさせる速度や、一方の物体２１と他方の物体２２とが接触した後であって一方の物体２１の回転移動によって一方の物体２１と他方の物体２２との間に作用する並進力制御方向の力が小さくなったりするときの移動速度や、一方の物体２１と他方の物体２２とが一度接触した後に非接触状態になって回転移動をしていないときに近づけさせる速度などは、一方の物体２１を移動させるとき移動動作の状況や、並進力制御目標力や力制御ゲインや回転移動の速度などに応じて、適切な値に調整することが好ましい。
【０１００】
また、設定や処理を簡単にするため、所定の移動量である第２の並進力制御方向移動量は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第１の所定閾値未満のときの、所定の移動量である第１の並進力制御方向移動量と同じ値であってもよい。
【０１０１】
また、非接触状態から接触状態に変わる時間が短い場合や、並進力制御方向目標移動量の調整が他の方法によっておこなわれている場合や、並進力制御方向の力が並進力制御目標力の大きさを超えて押しつけ気味になるような状態にさせたくない場合など、所定の移動量である第２の並進力制御方向移動量によって並進移動速度を大きくしなくてもよい場合は、所定の移動量である第２の並進力制御方向移動量は、所定の移動量である第１の並進力制御方向移動量より小さくしてもよい。
【０１０２】
また、一方の物体２１の後述の回転力制御軸の軸回りの回転移動の速度が大きいなどの状況によって、一方の物体２１と他方の物体２２とが離れやすくなっている場合に対して、一方の物体２１と他方の物体２２とが並進力制御方向について離れず接触した状態になるように、所定の移動量である第２の並進力制御方向移動量を、所定の移動量である第１の並進力制御方向移動量より大きくしてもよい。
【０１０３】
所定の移動量である第２の並進力制御方向移動量を並進力制御目標力と並進力制御方向の力によって調整するとき、並進力制御方向の力の大きさが並進力制御目標力と同じまたは前記或る所定閾値以下のとき、０となるようにし、並進力制御方向の力の大きさが或る所定閾値から並進力制御目標力の大きさの間であるとき、並進力制御方向の力の大きさが並進力制御目標力の大きさに近づくにつれて小さくなるように調整する。それ以外の条件では、所定の移動量である第２の並進力制御方向移動量を０に調整する。例えば、並進力制御方向の力をＦｐ、並進力制御目標力をＦｄ、所定の移動量である第２の並進力制御方向移動量をＶｃ、所定の移動量である第２の並進力制御方向移動量を調整して算出される並進力制御方向の移動量Ｖｔとするとき、並進力制御方向の力の大きさが０から並進力制御目標力の大きさの間の条件において、並進力制御目標力Ｆｄが０でない場合、第２の並進力制御方向移動量Ｖｔは次の式（１）のように調整して算出してもよい。
Ｖｔ＝（１−（Ｆｐ／Ｆｄ））×Ｖｃ      （１）
なお、並進力制御目標力Ｆｄが０である場合は、次の式（２）のように調整して算出する。
Ｖｔ＝（−Ｆｐ）×Ｖｃ      （２）
並進力制御目標力と並進力制御方向の力との差分と力制御ゲインに基づいて算出される移動量に対して、所定の移動量である第２の並進力制御方向移動量を並進力制御目標力と並進力制御方向の力によって調整した値を加算することによって、並進力制御方向の力をより早く並進力制御目標力に近づけることができる。
【０１０４】
コンプライアンス制御では、一方の物体２１と他方の物体２２とを合わせ状態にするとき、並進力制御方向については、一方の物体２１と他方の物体２２とが最初に接触した後は、並進力制御目標力と、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力との差に対して力制御ゲインを乗算して算出される移動量によって一方の物体２１の位置を移動させる。
【０１０５】
図１４は、一方の物体２１と他方の物体２２とを合わせ状態にするときの問題点の例を示す部分拡大図である。図１４では、一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１が設定され、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸が回転力制御軸６２として設定されている。並進力制御方向および回転力制御軸が図１４のように設定されることによって、回転力制御軸６２の軸回りの力と、並進力制御方向６１の力に従って、コンプライアンス制御を実行すれば、一方の物体２１と他方の物体２２とを合わせ状態にすることができる。
【０１０６】
図１４（ａ）では、一方の物体２１と他方の物体２２とは接触していない、かつ、一方の物体２１は並進力制御方向６１に並進移動している。一方の物体２１が並進力制御方向６１に並進移動していくと、図１４（ｂ）に示されるように、一方の物体２１と他方の物体２２とが最初に接触した状態となる。一方の物体２１と他方の物体２２とが並進力制御方向６１について接触すると、回転力制御軸６２の軸回りの力が発生して、力制御によって回転方向６３に回転移動する。すると、図１４（ｃ）に示されるように、一方の物体２１が回転力制御軸６２の軸回りの回転方向６３に回転移動するので、一方の物体２１と他方の物体２２とが接触している部分は、他方の物体２２から並進力制御方向６１の逆方向に移動する。一方の物体２１と他方の物体２２とが接触しない、または、並進力制御方向６１についての力が小さくなると、一方の物体２１は、回転方向６３への回転移動動作を停止させるか、回転移動の速度は小さくなる。このとき、一方の物体２１と他方の物体２２との並進力制御方向６１についての力と並進力制御目標力と力制御ゲインによって、一方の物体２１と他方の物体２２との並進力制御方向６１における力が大きくならないと、回転力制御軸６２の軸回りの力は発生しないか、大きくならない。
【０１０７】
また、図１５は、一方の物体２１と他方の物体２２とを合わせ状態にするときの問題点の別の例を示す部分拡大図である。図１５では、ツール座標系上の所定の方向のように一方の物体２１に対して予め設定された方向に並進力制御方向６１が設定され、一方の物体２１に対して予め設定された所定の方向であって一方の物体２１の制御点を通る軸に回転力制御軸６２が設定されている。並進力制御方向および回転力制御軸が図１５のように設定されることによって、回転力制御軸６２の軸回りの力と、並進力制御方向６１の力に従って、コンプライアンス制御を実行すれば、一方の物体２１と他方の物体２２とを合わせ状態にすることができる。
【０１０８】
図１５（ａ）では、一方の物体２１と他方の物体２２とは接触していない、かつ、一方の物体２１は、一方の物体２１の姿勢によって方向が変わる並進力制御方向６１に並進移動している。一方の物体２１が並進力制御方向６１に並進移動していくと、図１５（ｂ）に示されるように、一方の物体２１と他方の物体２２とが最初に接触した状態となる。一方の物体２１と他方の物体２２とが並進力制御方向６１について接触して力が発生すると、回転力制御軸６２の軸回りの力が発生して、回転方向６３に回転移動する。すると、図１５（ｃ）に示されるように、一方の物体２１が回転力制御軸６２の軸回りの回転方向６３に回転移動するので、一方の物体２１と他方の物体２２とが接触している部分は、他方の物体２２から並進力制御方向６１の逆方向に移動する。なお、このとき一方の物体２１の姿勢によって並進力制御方向６１は方向が変わる。一方の物体２１と他方の物体２２とが接触しない、または、並進力制御方向６１についての力が小さくなると、一方の物体２１は、回転方向６３への回転移動動作を停止させるか、回転移動の速度は小さくなる。このとき、一方の物体２１と他方の物体２２との並進力制御方向６１についての力と並進力制御目標力と力制御ゲインによって、一方の物体２１と他方の物体２２との並進力制御方向６１における力が大きくならないと、回転力制御軸６２の軸回りの力は発生しないか、大きくならない。このように、図１４を参照して説明した場合と同様に、並進力制御方向が一方の物体２１に移動動作によって変わる場合であっても、一方の物体２１が他方の物体２２に対して合わせ状態となるように回転移動をするときに、一方の物体２１と他方の物体２２とは非接触状態になったり、離れやすくなったりする。
【０１０９】
ところで、力制御ゲインや並進力制御目標力の調整には限度がある。力制御ゲインを大きくしていくと、ロボット５０の動作性能や、ロボット５０の動作に伴う振動や、力に載るノイズなどによってロボット５０の移動動作が発振しやすくなったり、力に応じて過大に移動したりするため、力制御ゲインはある程度の大きさにしかできない。並進力制御目標力については、一方の物体２１と他方の物体２２とを過大な力で接触させて、壊さないように、また変形させないように、また傷をつけないようにするために、並進力制御目標力を大きくできない場合も多い。
【０１１０】
前述のように、一方の物体２１を他方の物体２２に対して、並進力制御方向６１に並進力制御目標力で押しつけるように、並進力制御方向６１の力と並進力制御目標力との差分と力制御ゲインによって、並進力制御方向６１に移動させると共に、回転力制御軸６２の軸回りに発生する力に基づいて回転力制御軸６２の軸回りに回転移動させる場合であって、並進力制御目標力を大きくすることができない場合、力制御ゲインの大きさには限界があるため、並進力制御方向６１の力に基づく並進力制御方向６１への並進移動動作が素早くおこなわれず、回転力制御軸６２の軸回りの回転移動によって、一方の物体２１と他方の物体２２とが、離隔と接触を繰り返したり、並進力制御方向６１の力が不十分な状態に陥ったりして、回転移動の動作が安定せず、素早く回転移動させることが困難となる。このため、一方の物体２１と他方の物体２２とを合わせ状態するのに時間がかかる場合がある。
【０１１１】
これに対して、本発明による方法では、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が小さくなっているとき、前述のように並進力制御方向目標移動量を算出することによって、並進力制御目標力と、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力との差をもとに、力制御ゲインをかけて、並進移動速度を算出する方法に対して、より安定的に移動させると共に、一方の物体２１と他方の物体２２とをより短い時間で合わせ状態にすることができる。
【０１１２】
回転力制御軸回り目標移動量算出部３６は、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を他方の物体２２に対して回転力制御軸の軸回りに回転移動させる目標移動量である回転力制御軸回り目標移動量を算出する。なお、回転力制御軸回り目標移動量は、ベクトルであり、移動方向および移動量の大きさを定めるとする。また、回転力制御軸回り目標移動量は、正負の符号を含み、回転力制御軸回りの回転方向について、正方向または逆方向の、回転方向の移動の向きを定める。また、回転力制御軸設定部３４によって、２つ以上の回転力制御軸が設定されるとき、回転力制御軸回り目標移動量算出部３６は、それぞれの回転力制御軸について、回転力制御軸回り目標移動量を算出する。
【０１１３】
目標方向目標移動量算出部３７は、一方の物体２１と他方の物体２２との間に作用する力、および、並進力制御方向設定部３２によって設定される並進力制御方向、および、回転力制御軸設定部３４によって設定される回転力制御軸、および、並進力制御方向目標移動量算出部３５によって算出される並進力制御方向目標移動量、および、回転力制御軸回り目標移動量算出部３６によって算出される回転力制御軸回り目標移動量、に基づいて、一方の物体２１の並進方向の目標移動量である並進方向目標移動量と、一方の物体２１の回転方向の目標移動量である回転方向目標移動量を算出する。
【０１１４】
また、目標方向目標移動量算出部３７は、並進力制御方向目標移動量および回転力制御軸回り目標移動量と共に、一方の物体２１が他方の物体２２によって、並進力制御方向と直交する方向から力を作用される場合、前記並進力制御方向と直交する方向について作用される力に基づいて目標力を０とするように受動的に、換言すれば、作用される力の大きさが所定閾値より小さくなるように、並進方向に移動させる移動量を算出して、前記算出された移動量と、並進力制御方向目標移動量および回転力制御軸回り目標移動量とに基づいて、並進方向目標移動量および回転方向目標移動量を算出してもよい。
【０１１５】
また、目標方向目標移動量算出部３７は、並進力制御方向目標移動量および回転力制御軸回り目標移動量と共に、ロボットの台座５２が移動可能なロボットの設置装置５３によって移動されている場合であって、ロボットの台座５２の移動による一方の物体２１の移動が継続している場合、ロボット制御装置１０の次の制御周期での、ロボットの台座５２の移動による一方の物体２１の予想される移動量に基づいて、また、他方の物体２２が移動可能な他方の物体の設置装置２４によって移動されている場合であって、他方の物体２２の移動が継続している場合、ロボット制御装置１０の次の制御周期での、他方の物体２２の移動量に基づいて、並進方向目標移動量および回転方向目標移動量を算出してよい。
【０１１６】
また、目標方向目標移動量算出部３７は、一方の物体２１と他方の物体２２とを合わせ状態とするように一方の物体２１を他方の物体２２に対して相対的に移動させているときに、ロボット制御装置１０の外部から、教示装置などの移動操作の入力装置や、別の制御装置などからの移動操作の入力信号などによって、移動操作の入力があった場合、一時的に、並進力制御方向目標移動量および回転力制御軸回り目標移動量によらずに外部からの入力に基づいて移動させ、外部からの移動動作の入力が無効になった後は、並進力制御方向目標移動量および回転力制御軸回り目標移動量に基づいて移動させてもよい。
【０１１７】
または、目標方向目標移動量算出部３７は、一方の物体２１と他方の物体２２とが合わせ状態となるように一方の物体２１を他方の物体２２に対して相対的に移動させているときに、ロボット制御装置１０の外部から、教示装置などの移動操作の入力装置や、別の制御装置などからの移動操作の入力信号などによって、移動操作の入力があった場合、外部からの移動動作の入力による並進移動の移動量または回転移動の移動量または並進移動および回転移動の移動量と、並進力制御方向目標移動量および回転力制御軸回り目標移動量に基づいて、または、このとき各移動量に対して重み係数を乗算して合わせるなどして、並進方向目標移動量および回転方向目標移動量を算出してもよい。
【０１１８】
また、目標方向目標移動量算出部３７は、ロボット制御装置１０の外部から、教示装置や非常停止スイッチから入力される非常停止用の信号や、別の制御装置や別のセンサーなどから入力される非常停止用の信号などによって、減速停止や緊急停止の指示があった場合、その指示に応じて、並進方向目標移動量および回転方向目標移動量を算出する。
【０１１９】
また、並進力制御方向や並進力制御方向以外の方向からの力、また力制御座標系の軸の方向の力、また力制御座標系の軸回りの力、また回転力制御軸の軸回りの力や所定の回転軸の軸回りの力などの大きさが、所定の閾値を超える場合、また力計測部３１において過大な力が検出された場合や、また所定時間中に閾値を超える力がかかった場合、またロボット５０の力制御中の動作が発振的になった場合などの、力制御中の状況に応じて、減速停止または緊急停止させるように、並進方向目標移動量および回転方向目標移動量を算出する。
【０１２０】
また、一方の物体２１と他方の物体２２とを合わせ状態とするように一方の物体２１の移動させるときの開始時から、一方の物体２１の位置または姿勢が所定の閾値を越えて過大に変化する場合、減速停止または急停止させるように、並進方向目標移動量および回転方向目標移動量を算出する。
【０１２１】
また、力制御中の位置および姿勢やロボット５０の軸の位置が、移動可能な範囲や所定の範囲を超えて移動しようとするとき、所定の範囲を超える前に減速停止させるように、並進方向目標移動量および回転方向目標移動量を算出する。
【０１２２】
このように、目標方向目標移動量算出部３７は、力制御中の動作や、ロボット制御装置１０の外部からの入力などに応じて、並進方向目標移動量および回転方向目標移動量を調整して算出してもよい。なお、減速停止、緊急停止させる場合は、後述の動作指令生成部において、そのような指令を出すように、調整、算出して指令を生成してもよい。
【０１２３】
動作指令生成部３８は、目標方向目標移動量算出部３７によって算出される並進方向目標移動量および回転方向目標移動量に基づいて、ロボット５０の動作指令を生成する。
【０１２４】
このとき、動作指令生成部３８は、並進方向目標移動量と回転方向目標移動量とに基づいて、ロボット５０が滑らかに、かつ、所望の位置および姿勢に移動できるように、動作指令を生成し、ロボット５０のアクチュエータへの動作指令を出力する。また、動作指令生成部３８は、加加速度が急に大きくならないようにフィルタリング処理をしたり、また、ロボット５０の動作の振動が少なくなるように振動の低減の処理をしたり、また、目的とする位置に正確に移動できるようにロボット５０の撓みなどを考慮して指令の位置および／または姿勢を補正したり、などして動作指令を調整する。
【０１２５】
なお、ロボット制御装置１０の外部から、教示装置や非常停止スイッチから入力される非常停止用の信号や、別の制御装置や別のセンサーなどから入力される非常停止用の信号などによって、減速停止や緊急停止の指示があった場合、目標方向目標移動量算出部３７や動作指令生成部３８において、ロボット５０を減速停止または緊急停止させるようにする。
【０１２６】
次いで、図１７を参照して、前述した実施形態に係るロボット制御装置１０ａによってロボット５０の手先部５１に保持された一方の物体２１を他方の物体２２に対して相対的に移動させ一方の物体２１と他方の物体２２とが一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態、である合わせ状態となるように、両者間に作用する力を制御するときの、ロボット制御装置１０ａの処理の一例を説明する。図１７は、ロボット制御装置１０ａによる処理の過程の一例を示すフローチャートである。
【０１２７】
図１７に示される処理はロボット制御装置１０ａの制御周期毎に実行され、一方の物体２１と他方の物体２２との間に作用する力に基づいてロボット５０の動作指令を制御周期毎に生成する処理の一例である。このとき、一方の物体２１と他方の物体２２とが、１つ以上の所定の並進方向について接触した状態のときに、１つ以上の所定の回転軸回りの力が０か所定の閾値か否かによって、合わせ状態となっているか否かを判定するが、後述のように、回転軸回りの力が０かどうかで判定する方法以外の判定方法によって合わせ状態となっているかを判定してもよい。
【０１２８】
ロボット５０の手先部５１に保持された一方の物体２１を他方の物体２２に対して相対的に移動させ一方の物体２１と他方の物体２２とを合わせ状態となるようにする動作が開始されると、並進力制御方向設定部３２は一方の物体２１を他方の物体２２に対して相対的に並進移動させて押しつける方向である並進力制御方向を１つ以上設定し、回転力制御軸設定部３４は一方の物体２１を他方の物体２２に対して相対的に回転移動させるときの回転軸である回転力制御軸を１つ以上設定する（ステップＳ１）。このとき、回転力制御軸設定部３４は回転力制御軸の内、少なくとも１つの軸は並進力制御方向と平行ではないように設定する。
【０１２９】
並進力制御目標力設定部３３は、並進力制御方向設定部３２が設定した並進力制御方向について、また並進力制御方向が２つ以上存在する場合は各並進力制御方向について、一方の物体２１を他方の物体２２に対して相対的に並進移動させて押しつけるときの目標とする押付力である並進力制御目標力を設定する（ステップＳ２）。
【０１３０】
力計測部３１は、力センサ２５が検出する力データ、並進力制御方向および回転力制御軸の設定に基づいて、一方の物体２１と他方の物体２２との間に作用する力を計測し、一方の物体２１と他方の物体２２との間に作用する並進力制御方向の力、一方の物体２１と他方の物体２２との間に作用する回転力制御軸の軸回りの力を計測する（ステップＳ３）。
【０１３１】
並進力制御方向目標移動量算出部３５は、並進力制御方向の力が所定閾値未満か否かを判定する（ステップＳ４）。
【０１３２】
並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が第１の所定閾値未満のとき、所定の移動量である第１の並進力制御方向移動量、または、所定の移動量である第１の並進力制御方向移動量および並進力制御方向の並進力制御目標力に基づいて、並進力制御方向目標移動量を算出する（ステップＳ５）。
【０１３３】
並進力制御方向目標移動量算出部３５は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が第１の所定閾値以上のとき、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力および並進力制御方向の並進力制御目標力、または、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力および並進力制御方向の並進力制御目標力および所定の移動量である第２の並進力制御方向移動量、に基づいて、並進力制御方向目標移動量を算出する（ステップＳ６）。
【０１３４】
回転力制御軸回り目標移動量算出部３６は、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を他方の物体２２に対して回転力制御軸の軸回りに回転移動させる目標移動量である回転力制御軸回り目標移動量を算出する（ステップＳ７）。
【０１３５】
目標方向目標移動量算出部３７は、ステップＳ５、ステップＳ６で、並進力制御方向目標移動量算出部３５によって算出される並進力制御方向目標移動量、および、ステップＳ７で、回転力制御軸回り目標移動量算出部３６によって算出される回転力制御軸回り目標移動量、に基づいて、一方の物体２１の並進方向の目標移動量である並進方向目標移動量と回転方向の目標移動量である回転方向目標移動量を算出する（ステップＳ８）。
【０１３６】
動作指令生成部３８は、目標方向目標移動量算出部３７によって算出される並進方向目標移動量および回転方向目標移動量に基づいて、ロボット５０の動作指令を生成する（ステップＳ９）。
【０１３７】
以上のステップＳ１からＳ９までの処理によって、一方の物体２１と他方の物体２２との間に作用する力に基づいて、ロボット５０の手先部５１に保持された一方の物体２１を他方の物体２２に対して相対的に移動させ一方の物体２１と他方の物体２２とを合わせ状態となるようにするための移動動作の動作指令が、ロボット制御装置１０の制御周期毎に生成される。ステップＳ１からステップＳ９の処理は、ロボット制御装置１０の制御周期毎に実行され、一方の物体２１と他方の物体２２とを合わせ状態とするまで繰り返して実行される。
【０１３８】
本発明の２番目の実施形態においては、ロボット制御装置１０における、並進力制御方向目標移動量算出部３５は、本発明の１番目の実施形態のロボット制御装置１０に対して、さらに、一方の物体２１の回転力制御軸の軸回りの回転移動の移動量と、一方の物体２１の回転力制御軸の軸回りの回転移動に伴う、一方の物体２１と他方の物体２２とが接触する部分の並進移動の移動量との関係に基づいて、回転力制御軸回り目標移動量算出部３６が算出する回転力制御軸回り目標移動量を基に、並進力制御方向目標移動量の補正移動量を算出し、補正移動量分を補正して並進力制御方向目標移動量を算出することが好ましい。なお、一方の物体２１と他方の物体２２とが接触する部分の並進移動の移動量は、一方の物体２１と他方の物体２２とが接触する部分の並進力制御方向と平行な方向の移動量とすることが好ましい。
【０１３９】
本発明の２番目の実施形態の説明においては、主に本発明の１番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０１４０】
一方の物体２１と他方の物体２２とを合わせ状態にするとき、一方の物体２１の回転力制御軸の軸回りの回転移動に伴って、一方の物体２１と他方の物体２２とは接触部分が離れるので、一方の物体２１と他方の物体２２とを並進力制御方向について接触させるために必要な並進移動の移動量を、算出、または推定して、一方の物体２１を回転力制御軸の軸回りに回転移動させると同時に、一方の物体２１と他方の物体２２とを接触させるために必要な並進移動の移動量だけ、一方の物体２１を並進移動させる。このとき、一方の物体２１と他方の物体２２との間に作用する力が並進力制御目標力となるように力制御する移動量に、前記の並進移動の移動量を加算して補正することが好ましい。
【０１４１】
また、他方の物体２２が移動するとき、並進力制御方向と平行な方向に他方の物体２２が移動する移動量、および、一方の物体２１と他方の物体２２との間に作用する力が並進力制御目標力となるように力制御する移動量、および、一方の物体２１の物体の回転移動によって一方の物体２１と他方の物体２２とが離れる移動量を加算した移動量を、並進力制御方向目標移動量とすることが好ましい。
【０１４２】
図１４、図１５で示されるように、一方の物体２１と他方の物体２２とが接触し、回転力制御軸回り目標移動量算出部３６が算出する回転力制御軸回り目標移動量によって一方の物体２１が回転力制御軸６２の軸回りに回転移動するとき、一方の物体２１と他方の物体２２とが接触する部分は、並進力制御方向６１とは逆方向に移動することになる。このとき、一方の物体２１と他方の物体２２とが接触するように、一方の物体２１は他方の物体２２に対して、並進力制御方向６１に並進移動する必要がある。そして、並進力制御方向目標移動量算出部３５が算出する並進力制御方向６１の並進力制御方向目標移動量によって一方の物体２１が並進力制御方向６１に並進移動し、一方の物体２１と他方の物体２２とは接触する。このような回転移動と並進移動との繰り返しによって、一方の物体２１と他方の物体２２とは合わせ状態になる。つまり、図１４、図１５で示されるように、一方の物体２１と他方の物体２２とが最初に接触する位置に回転力制御軸が存在しない場合、一方の物体２１と他方の物体２２とが一度接触した後は、一方の物体２１と他方の物体２２とを接触させておくためには、回転移動によって一方の物体２１と他方の物体２２とが離れる距離だけ、並進移動によって移動させる必要がある。このとき、回転力制御軸回りの回転移動の移動量と、一方の物体２１と他方の物体２２とが接触する部分の並進力制御方向の並進移動の移動量との移動量の関係から、制御周期毎に算出される回転移動の移動量に対して一方の物体２１と他方の物体２２とが並進力制御方向について離れる距離を推定して補正量として算出し、算出された補正量分だけ、回転移動の移動動作と共に、並進移動させれば、一方の物体２１と他方の物体２２とは、なるべく接触させた状態やなるべく離れないようにして、回転移動をさせることができる。また、このように並進移動させることによって、一方の物体２１と他方の物体２２とをより安定かつ素早く、並進力制御目標力で押しつけるように制御することができる。このように移動させるため、並進力制御方向目標移動量算出部３５は、所定時間における、一方の物体２１の回転力制御軸の軸回りの回転移動の移動量と、一方の物体２１と他方の物体２２とが接触する部分の並進力制御方向の並進移動の移動量との関係を求め、一方の物体２１の回転移動に伴う、一方の物体２１と他方の物体２２とが接触する部分の並進力制御方向と平行な軸上の並進移動の移動量を算出する。このとき、回転力制御軸から一方の物体２１と他方の物体２２とが接触する部分までの距離ｄと、ロボット制御装置１０の制御周期毎の一方の物体２１の回転力制御軸の軸回りの回転移動の回転力制御軸回り目標移動量θとから、一方の物体２１と他方の物体２２とが接触する部分の並進力制御方向に並進移動させる並進力制御方向目標移動量の補正移動量Ｌを、近似的に、以下の式（３）のように算出してもよい。
Ｌ＝ｄ×θ      （３）
回転力制御軸から一方の物体２１と他方の物体２２とが接触する部分までの距離は、一方の物体２１の物体の形状情報を用いたり、一方の物体２１の回転力制御軸から前記接触点までの距離を予め計測したり、最初に接触した時の力を基に接触点までの距離を算出しておいたり、などして求めておくことが好ましい。
【０１４３】
また、並進力制御方向目標移動量の補正移動量を算出するとき、一方の物体２１の回転に伴って、一方の物体２１と他方の物体２２とが離れ、一方の物体２１と他方の物体２２とを接触させるために、一方の物体２１を並進力制御方向に移動させる移動量を実際の移動動作から取得して、取得したデータに基づいて求められた、一方の物体２１の回転力制御軸の軸回りの回転移動の移動量と、一方の物体２１の回転力制御軸の軸回りの回転移動に伴う、一方の物体２１と他方の物体２２とが接触する部分の並進移動の移動量との関係に基づいて、カルマンフィルタなどの状態推定法を使用して、回転力制御軸回り目標移動量を基に、並進力制御方向目標移動量の補正移動量を推定して算出してもよい。
【０１４４】
また、他方の物体２２が位置および／または姿勢を変えるとき、一方の物体２１の回転および他方の物体２２の移動に伴って一方の物体２１と他方の物体２２とが接触するために移動する距離を実際の移動動作から取得して、取得したデータをもとに、一方の物体２１と他方の物体２２との移動の状況から、次の制御周期で、一方の物体２１と他方の物体２２とが並進力制御方向に離れる距離を推定し、推定された値に所定の上限値を設け、その距離分だけ、並進力制御方向目標移動量を補正してもよい。つまり、一方の物体２１の回転移動および他方の物体２２の位置および／または姿勢の移動の状況から、一方の物体２１と他方の物体２２とが離れないようにするために必要な移動量を推定して、その移動量分だけ、一方の物体２１の並進力制御方向目標移動量を補正してもよい。
【０１４５】
前述のように算出した補正移動量分を補正して、並進力制御方向目標移動量を算出することによって、回転力制御軸回り目標移動量に従って一方の物体２１を回転移動させると共に、並進力制御方向について、なるべく離れないようにすることができる。なお、並進力制御方向が２つ以上ある場合、各方向について前述のように算出して補正する、または、或る方向についてのみ算出して補正する、または、制御周期毎に選択される方向について算出して補正するようにしてもよい。以上のようにして、一方の物体２１と他方の物体２２とをより安定的にかつ素早く、合わせ状態にすることが可能となる。
【０１４６】
図３は、本発明の３番目の実施形態に係るロボット制御装置１０ｂの構成を機能的に示す図である。本発明の３番目の実施形態においては、本発明の１番目または２番目の実施形態のロボット制御装置１０に対して、さらに、接触位置設定部３９を備えるロボット制御装置の構成としてもよい。本発明の３番目の実施形態においては、図３において図示されるように、ロボット制御装置１０ｂはロボット制御装置１０ａに対して、さらに、接触位置設定部３９を備える。また、後述の機能も含めて図５に図示されるロボット制御装置１０ｆの構成としてもよい。また、図１４、図１５、図１６を参照しつつ、本発明の３番目の実施形態に係るロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にする方法について説明する。
【０１４７】
本発明の３番目の実施形態においては、ロボット制御装置１０は、本発明の１番目または２番目の実施形態のロボット制御装置１０に対して、さらに、一方の物体２１と他方の物体２２とが接触するときの接触位置を設定する、または、一方の物体２１と他方の物体２２との間に作用する力に基づいて一方の物体２１と他方の物体２２とが接触するときの接触位置を算出して設定する接触位置設定部３９を備え、回転力制御軸設定部３４は、所定の回転力制御軸を１つ以上設定すると共に、一方の物体２１における接触位置を通る軸または他方の物体２２における接触位置を通る軸、かつ、所定の回転力制御軸に平行な軸を、接触位置基準の回転力制御軸として１つ以上設定し、回転力制御軸回り目標移動量算出部３６は、所定の回転力制御軸の軸回りの力に基づいて、または、所定の回転力制御軸に対して算出される回転力制御軸の軸回りに回転移動させる場合の目標移動量に基づいて、接触位置基準の回転力制御軸の軸回りに回転移動させる目標移動量を算出して、算出された目標移動量を回転力制御軸回り目標移動量とすることが好ましい。
【０１４８】
本発明の３番目の実施形態の説明においては、主に、本発明の１番目または２番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０１４９】
前述のように、図１４、図１５で示されるように、一方の物体２１と他方の物体２２とが接触し、回転力制御軸回り目標移動量算出部３６が算出する回転力制御軸回り目標移動量によって一方の物体２１が回転力制御軸６２の軸回りに回転移動するとき、並進力制御方向目標移動量算出部３５が算出する並進力制御方向６１の並進力制御方向目標移動量によって一方の物体２１は並進力制御方向６１に並進移動する。この回転移動と並進移動を繰り返すことによって、一方の物体２１と他方の物体２２とは合わせ状態となる。つまり、図１４、図１５で示されるように、最初に接触する位置に回転力制御軸が存在しない場合、一度接触した後は、回転移動によって一方の物体２１と他方の物体２２とが離れる距離だけ、並進移動によって移動させる必要がある。
【０１５０】
図１６は、本発明の３番目の実施形態のロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にするときの動作を示す部分拡大図である。図１６（ａ）には、一方の物体２１と他方の物体２２とを合わせ状態に移動させるときの開始時の状態が示されており、一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１が設定され、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸に回転力制御軸６２が設定されている。なお、他の実施例についても同様であるが、他方の物体２２に対する一方の物体２１の並進力制御方向を変化させるようにしてもよい。図１６（ａ）に示されるように、並進力制御方向６１に一方の物体２１を他方の物体２２に対して相対的に並進移動させていくと、図１６（ｂ）に示されるように、一方の物体２１は他方の物体２２と、接触点６５の部分において接触する。一方の物体２１と他方の物体２２とが接触したか否かは、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力と所定の閾値との比較によって判定する。または、回転力制御軸６２の軸回りの力と所定の閾値との比較や、回転力制御軸６２の軸回りの力の所定時間あたりの変化に基づいて、一方の物体２１と他方の物体２２とが接触したか否かを判定してもよい。なお、接触点６５は、一方の物体２１が他方の物体２２に対して直線部分で接触するとき、その直線部分の中心点や代表点としてもよく、連続しない複数部分で接触する場合にも、その部分の中心点や代表点としてもよい。また、接触点６５は一方の物体２１と他方の物体２２との接触箇所が変わった場合、そのときに接触している位置としてもよい。ここでは、接触点６５は、最初に接触する部分における代表的な点とする。図１４、図１５で示されるように、最初に接触する位置に回転力制御軸が存在しない場合、一度接触した後は、回転移動によって一方の物体２１と他方の物体２２との接触部分が離れる距離だけ、並進移動によって移動させる必要があるのに対して、図１６で示されるように、一方の物体２１を他方の物体２２に対して相対的に回転移動させるとき、接触点６５にある回転力制御軸の軸回りに移動させれば、回転移動によって一方の物体２１と他方の物体２２との接触部分が離れる距離を、なるべく小さくすることができる。本発明の３番目の実施形態においては、このように移動させるように、一方の物体２１と他方の物体２２とを接触位置を中心として相対的に回転移動させるようにする。
【０１５１】
接触位置設定部３９は、一方の物体２１と他方の物体２２とが最初に接触する位置が予め分かっているとき、または、一方の物体２１と他方の物体２２とを最初に所定の位置で接触させるように一方の物体２１と他方の物体２２とを相対的に移動させて接触させるとき、一方の物体２１または他方の物体２２に対する所定の位置を接触位置として設定する。
【０１５２】
または、接触位置設定部３９は、一方の物体２１と他方の物体２２とが接触したときに、一方の物体２１と他方の物体２２との間に作用する力に基づいて、接触したときの位置を算出し、一方の物体２１または他方の物体２２に対する前記算出された位置を、接触位置として設定する。ここで、一方の物体２１と他方の物体２２とが接触する位置は、一方の物体２１と他方の物体２２との間に作用する力に基づいて算出されるおおよその位置であってもよい。
【０１５３】
また、一方の物体２１と他方の物体２２との間に作用する力に基づいて、ロボット制御装置１０の制御周期毎に、カルマンフィルタなどの状態推定法を用いて、一方の物体２１と他方の物体２２との接触位置を、逐次、算出して推定し、更新するようにしてもよい。
【０１５４】
回転力制御軸設定部３４は、最初に、所定の回転力制御軸を１つ以上設定する。ここでは、説明を簡単にするために、回転力制御軸の数を１つとするが２つ以上であっても同様である。また、本発明の実施例の他の部分と同様に、回転力制御軸は、所定の並進方向について接触させると共に、その所定の回転力制御軸の軸回りの力がつりあった合わせ状態となるときに、一方の物体２１と他方の物体２２との直線部分や面部分を合わせたり、一方の物体２１と他方の物体２２とを所望の状態に移動させたりすることが可能となるような、回転軸とする。
【０１５５】
図１６（ａ）に示されるように、一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１を設定し、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸に回転力制御軸６２を設定する。
【０１５６】
次に、図１６（ｂ）に示されるように、一方の物体２１を他方の物体２２に対して並進力制御方向６１に移動させていき、一方の物体２１と他方の物体２２とが接触するとき、接触位置設定部３９は接触位置を設定する。
【０１５７】
回転力制御軸設定部３４は、接触位置設定部３９によって設定された接触位置をもとに、一方の物体２１に対して設定される接触位置における点を通る軸、または他方の物体２２に対して設定される接触位置における点を通る軸であって、かつ、所定の回転力制御軸６２に平行である軸を、接触位置基準の回転力制御軸６７として１つ以上設定する。なお、ここでは説明を簡単にするため、接触位置基準の回転力制御軸の数を１つとする。
【０１５８】
本発明の他の実施例の説明においても同様であるが、一方の物体２１と他方の物体２２とを合わせる部分が平面である場合、回転力制御軸の数を２つとし、各回転力制御軸について、或る回転力制御軸について移動させる方法と同様に移動させればよい。なお、各回転力制御軸の回転力制御軸回り目標移動量は、適宜、他の回転力制御軸における回転力制御軸回り目標移動量の大きさなどを考慮して調整してもよい。
【０１５９】
回転力制御軸を設定するとき、他方の物体２２が移動しないときは、一方の物体２１に対する接触点６５の位置でよいが、他方の物体２２が移動するとき、他方の物体２２に対して設定される接触点６５の位置に回転力制御軸を設定することが好ましい。また、並進力制御方向が一方の物体２１に対する所定の方向であり、一方の物体２１の移動動作に応じて、他方の物体２２に対する並進力制御方向が変わる場合、一方の物体２１に対して設定される接触点６５の位置に回転力制御軸６７を設定することが好ましい。
【０１６０】
並進力制御方向目標移動量算出部３５は、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を他方の物体２２に対して並進力制御方向６１と平行な方向に並進移動させる目標移動量である並進力制御方向目標移動量を算出する。このとき、並進力制御方向６１と平行な方向、かつ、接触位置基準の回転力制御軸６７が存在する位置を基準とした並進力制御方向６４について並進力制御方向目標移動量を算出する。
【０１６１】
回転力制御軸回り目標移動量算出部３６は、所定の回転力制御軸６２の軸回りの力に基づいて、または、所定の回転力制御軸６２に対して算出される所定の回転力制御軸６２の軸回りの回転方向６３に回転移動させる場合の目標移動量に基づいて、接触位置基準の回転力制御軸６７の軸回りの回転方向６６に回転移動させる目標移動量を算出し、算出された目標移動量を回転力制御軸回り目標移動量とする。ここで、一方の物体２１と他方の物体２２との間に作用する力に基づいて、接触位置基準の回転力制御軸６７の軸回りの力を算出した場合、設定した接触位置と実際の接触位置とが同じである場合は０であり、前記の位置が同じでなくとも近い場合は、所定の回転力制御軸６２に対して算出される所定の回転力制御軸６２の軸回りの力の大きさに比べてとても小さくなるため、接触位置基準の回転力制御軸６７の軸回りの力に基づいて移動させることは好ましくない。このため、所定の回転力制御軸６２の軸回りの力に基づいて、接触位置基準の回転力制御軸６７の軸回りに回転移動させる目標移動量を算出する。または、所定の回転力制御軸６２に対して算出される所定の回転力制御軸６２の軸回りに回転移動させる場合の目標移動量を算出して、算出された回転移動の目標移動量に基づいて、接触位置基準の回転力制御軸６７の軸回りに回転移動させるときの回転移動の目標移動量を算出する。一方の物体２１と他方の物体２２とが合わせ状態になるとき、所定の回転力制御軸６２に対して算出される所定の回転力制御軸６２の軸回りに回転移動させる場合の目標移動量は０になり、接触位置基準の回転力制御軸６７の軸回りに回転移動させる目標移動量も０になるため、一方の物体２１と他方の物体２２とをうまく合わせ状態にすることができる。このように、一方の物体２１と他方の物体２２とを相対的に回転移動させるときに、所定の回転力制御軸６２の軸回りの力に基づいて算出される所定の回転力制御軸６２の軸回りに回転移動させる移動量分だけ、接触位置基準の回転力制御軸６７の軸回りに回転移動させれば、一方の物体２１と他方の物体２２とを合わせ状態にすることができる。
【０１６２】
前述のように並進力制御方向目標移動量算出部３５によって算出された並進力制御方向目標移動量と、回転力制御軸回り目標移動量算出部３６によって算出された回転力制御軸回り目標移動量によって、一方の物体２１は他方の物体２２に対して、接触位置基準の回転力制御軸６７の軸回りに回転移動すると共に、並進力制御方向６４について並進力制御目標力で押しつけるように並進移動する。これによって、図１６（ｂ）、図１６（ｃ）に示されるように、一方の物体２１の回転移動によって一方の物体２１と他方の物体２２とが非接触状態になったり、離れやすくなったりする状況をなるべくなくし、より安定的かつ短い時間で、一方の物体２１と他方の物体２２とを合わせ状態にすることができる。このとき、実際に接触した位置と設定した接触位置とにずれがあったとしても、実際の接触位置に近い位置を回転中心とすることによって、回転力制御軸６２の軸回りに回転移動させるよりも、より安定的かつ素早く、一方の物体２１と他方の物体２２とを合わせ状態にすることができる。
【０１６３】
図４は、本発明の４番目の実施形態に係るロボット制御装置１０ｃの構成を機能的に示す図である。本発明の４番目の実施形態においては、本発明の１番目から３番目のいずれかの実施形態のロボット制御装置１０に対して、さらに、接触部分姿勢算出部４０を備えるロボット制御装置の構成としてもよい。本発明の４番目の実施形態においては、図４において図示されるように、ロボット制御装置１０ａに対して、さらに、接触部分姿勢算出部４０を備える。または、図示しないがロボット制御装置１０ｂに対して、さらに、接触部分姿勢算出部４０を備える。また、後述の機能も含むようにした、図８に図示されるロボット制御装置１０ｇの構成としてもよい。また、図１８を参照しつつ、本発明の４番目の実施形態に係る接触部分姿勢算出部４０を備えるロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にする方法について説明する。
【０１６４】
本発明の４番目の実施形態においては、ロボット制御装置１０は、本発明の１番目から３番目のいずれかの実施形態のロボット制御装置１０に対して、さらに、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態となるようにする部分における、一方の物体２１の前記接触させる部分の姿勢または他方の物体２２の前記接触させる部分の姿勢または一方の物体２１および他方の物体２２の前記接触させる部分の姿勢を算出する接触部分姿勢算出部４０を備え、接触部分姿勢算出部４０は、一方の物体２１と他方の物体２２との間に作用する力に基づいて一方の物体２１と他方の物体２２とが接触するときの接触位置を算出し、複数の算出された接触位置に基づいて一方の物体２１と他方の物体２２とを接触させる部分の姿勢を算出し、回転力制御軸回り目標移動量算出部３６は、一方の物体２１と他方の物体２２との間に作用する力、および、接触部分姿勢算出部４０が算出する一方の物体２１と他方の物体２２とを接触させる部分の姿勢に基づいて、回転力制御軸回り目標移動量を算出することが好ましい。
【０１６５】
本発明の４番目の実施形態の説明においては、主に、本発明の１番目から３番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０１６６】
図１８は、本発明の４番目の実施形態のロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にするときの動作を示す部分拡大図である。図１８（ａ）には、一方の物体２１と他方の物体２２とを合わせ状態に移動させるときの開始時の状態が示されており、一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１が設定され、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸に回転力制御軸６２が設定されている。なお、他の実施例についても同様であるが、他方の物体２２に対する、一方の物体２１の並進力制御方向が、所定の方向ではなく変化するようにしてもよい。図１８（ａ）に示されるように、一方の物体２１を他方の物体２２に対して並進力制御方向６１に相対的に並進移動させていくと、図１８（ｂ）に示されるように、一方の物体２１は他方の物体２２と、接触点６５の部分において接触する。一方の物体２１と他方の物体２２とが接触したか否かは、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力と所定の閾値との比較によって判定する。または、回転力制御軸６２の軸回りの力と所定の閾値との比較や、回転力制御軸６２の軸回りの力の所定時間あたりの変化に基づいて、一方の物体２１と他方の物体２２とが接触したか否かを判定してもよい。接触部分姿勢算出部４０は、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１と他方の物体２２とが接触するときの位置を算出する。なお、ロボット制御装置１０が接触位置設定部３９を備え、接触位置設定部３９が一方の物体２１と他方の物体２２とが接触するときの位置を算出するとき、接触部分姿勢算出部４０は、接触位置設定部３９と共通の算出部によって、接触位置を算出するようにしてもよい。
【０１６７】
続いて、図１８（ｃ）に示されるように、一方の物体２１を移動させて、他方の物体２２に対する接触位置が変わるようにする。このとき、一方の物体２１を回転力制御軸６２の軸回りに回転させながら並進力制御方向６１に移動させ、他方の物体２２に対する接触位置が変わるようにする。また、前記移動の際に、一方の物体２１の物体を他方の物体２２に対して、並進力制御方向６１と直交する方向などの並進方向に移動させて、他方の物体２２に対する接触位置が変わるようにしてもよい。また、本発明の３番目の実施形態に係るロボット制御装置１０で説明したように、一方の物体２１または他方の物体２２に対して定められる、一方の物体２１と他方の物体２２とが最初に接触する位置などの所定の位置を通るように回転力制御軸を設定して、設定された回転力制御軸の軸回りに移動させると共に、並進力制御方向６１と直交する方向または並進力制御方向６１と平行でない方向にさらに別の並進力制御方向を設定して移動させることによって、他方の物体２２に対する接触位置が変わるようにしてもよい。また、他方の物体２２に対する一方の物体２１の並進力制御方向を変化させて移動させるようにしてもよい。また、目標方向目標移動量算出部３７が並進力制御方向６１をもとに並進力制御方向６１と直交する方向や接触位置が変わるように並進移動させるための並進方向目標移動量を算出してもよい。なお、接触部分姿勢算出部４０によって一方の物体２１と他方の物体２２とを接触させる部分の姿勢が算出された後は、並進力制御方向６１以外の並進方向に一方の物体２１を移動させる動作を停止させ、合わせ状態にする動作のみをおこなうようにしてもよい。
【０１６８】
図１８（ｃ）では、或る時の一方の物体２１と他方の物体２２との接触位置が接触点６５ｂと示されており、一方の物体２１と他方の物体２２とが最初に接触した位置が接触点６５ａと示されている。前述のように一方の物体２１を、他方の物体２２に対する接触位置が変わるように移動させ、このときに算出した複数の接触位置に基づいて、一方の物体２１と他方の物体２２とを合わせ状態にする部分であり、他方の物体２２の接触させる部分の姿勢を算出する。
【０１６９】
また、図１８（ｄ）に示されるように、一方の物体２１を回転移動を伴って移動させて、他方の物体２２に対する接触位置を変える、かつ、一方の物体２１に対する接触位置を変えるようにしてもよい。図１８（ｄ）において、一方の物体２１と他方の物体２２とが接触する位置は接触点６５ｄと示されており、図１８（ｂ）で示された一方の物体２１と他方の物体２２とが最初に接触した位置は接触点６５ａと示されている。図１８（ｂ）に示されるように一方の物体２１と他方の物体２２とが最初に接触した後、回転力制御軸６２の軸回りの回転方向６３に速い速度で回転させたとき、合わせ状態の姿勢を通り過ぎて、さらに回転して図１８（ｄ）に示されるような回転力制御軸６２を挟んで逆側の位置で接触することがある。または、並進力制御方向６１への移動速度に対する、回転力制御軸６２の軸回りの回転速度を意図的に大きくして、図１８（ｄ）に示されるように接触させるようにしてもよい。
【０１７０】
また、図１８（ｃ）で示されるように一方の物体２１の或る部分を他方の物体２２に対して、接触点６５ａ、接触点６５ｂのように複数個所で接触させた後、図１８（ｅ）で示されるように一方の物体２１の前記或る部分とは別の部分を他方の物体２２に対して接触するように、接触点６５ｄ、接触点６５ｅで示される点の複数個所において接触させたときの接触位置を算出してもよい。
【０１７１】
以上のように他方の物体２２に対して一方の物体２１を移動させて、一方の物体２１と他方の物体２２とを接触させて、接触位置を算出する。これによって、一方の物体２１と他方の物体２２とを接触させる、一方の物体２１の接触させる部分の姿勢は、一方の物体２１に対する接触点である接触点６５ｃと接触点６５ｄから算出される。また、一方の物体２１と他方の物体２２とを接触させる、他方の物体２２の接触させる部分の姿勢は、他方の物体２２に対する接触点である接触点６５ａと接触点６５ｄ、または、接触点６５ａと接触点６５ｂと接触点６５ｄと接触点６５ｅから算出される。
【０１７２】
前述のように一方の物体２１と他方の物体２２との接触位置が変わるように移動させ、このときに算出した複数の接触位置に基づいて、一方の物体２１と他方の物体２２とを一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態である、合わせ状態にする部分であり、一方の物体２１と他方の物体２２とを接触させる、一方の物体２１の接触させる部分の姿勢、または、他方の物体２２の接触させる部分の姿勢、または、一方の物体２１の接触させる部分および他方の物体２２の接触させる部分の姿勢を算出することができる。
【０１７３】
回転力制御軸回り目標移動量算出部３６は、一方の物体２１と他方の物体２２との間に作用する力、および、前述のように接触部分姿勢算出部４０によって算出される一方の物体２１と他方の物体２２とを接触させる部分の姿勢に基づいて、回転力制御軸回り目標移動量を算出する。このとき、一方の物体２１における他方の物体２２に対して接触させる部分の姿勢が分かるようにロボット５０の手先部５１に対してツール座標系が設定されている場合、接触部分姿勢算出部４０によって他方の物体２２の接触させる部分の姿勢を算出して、回転力制御軸回り目標移動量算出部３６は、一方の物体２１における接触させる部分の姿勢が、他方の物体２２の接触させる部分の姿勢に近くなるまでは速く回転移動するように回転方向の目標移動量を大きくして、近づいたら回転方向の目標移動量を小さくする。また、他方の物体２２における接触させる部分の姿勢が分かるように座標系が設定されている場合、接触部分姿勢算出部４０によって一方の物体２１における接触させる部分の姿勢を算出して、回転力制御軸回り目標移動量算出部３６は、一方の物体２１の接触させる部分の姿勢が、他方の物体２２の接触させる部分の姿勢に近くなるまでは速く回転移動するように回転方向の目標移動量を大きくして、近づいたら回転方向の目標移動量を小さくする。また、接触部分姿勢算出部４０によって一方の物体２１における接触させる部分および他方の物体２２における接触させる部分の姿勢を算出して、回転力制御軸回り目標移動量算出部３６は、一方の物体２１の接触させる部分の姿勢と、他方の物体２２の接触させる部分の姿勢とが近くなるまでは速く回転移動するように回転方向の目標移動量を大きくして、近づいたら回転方向の目標移動量を小さくするようにしてもよい。また、一方の物体２１における接触させる部分の姿勢が分かるようにロボット５０の手先部５１に対してツール座標系が設定されている場合、接触部分姿勢算出部４０によって他方の物体２２における接触させる部分の姿勢を算出して、または、接触部分姿勢算出部４０によって一方の物体２１における接触させる部分および他方の物体２２における接触させる部分の姿勢を算出して、姿勢については算出された姿勢をもとに一方の物体２１と他方の物体２２とが合わせ状態となる姿勢となるように移動させて、並進力制御方向については、一方の物体２１と他方の物体２２との間に作用する力に基づいて、並進力制御方向における力が並進力制御目標力となるように並進力制御方向に移動させてもよい。また、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が小さい場合や、一方の物体２１と他方の物体２２とが接触していない場合に、一方の物体２１における接触させる部分の姿勢と、他方の物体２２における接触させる部分の姿勢とが近くなるまでは、所定の回転方向の移動量、またはこのときの所定の回転力制御軸と平行でない並進力制御方向の並進力制御目標力、の内、少なくとも１つに基づいて、回転力制御軸回り目標移動量を算出するようにしてもよい。
【０１７４】
また、前述のように算出される、一方の物体２１における接触させる部分の姿勢、または、他方の物体２２における接触させる部分の姿勢、または、一方の物体２１における接触させる部分および他方の物体２２における接触させる部分の姿勢と、実際の姿勢のずれにより、一方の物体２１と他方の物体２２との間に作用する力における回転力制御軸回りの力が大きくなるときには、算出された一方の物体２１と他方の物体２２とを接触させる部分の姿勢に基づいて移動させることを停止させ、本実施例の他に示される方法のみによって移動させることが好ましい。
【０１７５】
このように４番目の実施形態では一方の物体２１を移動させながら、一方の物体２１に対する、または、他方の物体２２に対する接触位置が変わるように移動させ、このときに算出した複数の接触位置に基づいて、一方の物体２１と他方の物体２２とを接触させる部分の姿勢を算出する。なお、他の実施形態と同様に、ロボット５０の移動動作以外によって一方の物体２１または他方の物体２２の位置および／または姿勢が変化する場合、その変化する移動量をもとに、一方の物体２１と他方の物体２２とを前述のように相対的に移動させることが好ましい。また、平面と平面を合わせ状態にする場合のように２方向について回転力制御軸を設定して、回転力制御軸の軸回りに移動させる場合、複数の回転力制御軸について、各方向について、前述のように移動させることが好ましい。
【０１７６】
前述のように、接触部分姿勢算出部４０によって、合わせ状態における、一方の物体２１と他方の物体２２とを接触させる部分の姿勢が算出して推定されると、素早くその姿勢に移動させると共に、並進力制御方向に素早く移動させればよいので、一方の物体２１と他方の物体２２とを、安定的に素早く、合わせ状態にすることができる。また、並進力制御方向の力の大きさや回転力制御軸の軸回りの力の大きさによらず、一方の物体２１と他方の物体２２とを小さい力で押し付ける場合であっても、一方の物体２１と他方の物体２２とを、安定的に素早く、合わせ状態にすることができる。
【０１７７】
図５は、本発明の５番目の実施形態に係るロボット制御装置１０ｄの構成を機能的に示す図である。本発明の５番目の実施形態においては、本発明の１番目の実施形態のロボット制御装置１０に対して、さらに、合わせ状態姿勢算出部４１を備えるロボット制御装置の構成としてもよい。本発明の５番目の実施形態においては、図５において図示されるように、ロボット制御装置１０ａに対して、さらに、合わせ状態姿勢算出部４１を備える。また、後述の機能も含めて図８に図示されるロボット制御装置１０ｇの構成としてもよい。また、図１９を参照しつつ、本発明の５番目の実施形態に係る合わせ状態姿勢算出部４１を備えるロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にする方法について説明する。
【０１７８】
本発明の５番目の実施形態においては、ロボット制御装置１０は、本発明の１番目の実施形態のロボット制御装置１０に対して、さらに、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態となるようにするときの姿勢である合わせ状態姿勢を算出する合わせ状態姿勢算出部４１を備え、ロボット制御装置１０は、一方の物体２１と他方の物体２２とが接触したとき、一方の物体２１の他方の物体２２に対する並進力制御方向の移動を停止させると共に、一方の物体２１の姿勢を取得し、次に一方の物体２１の他方の物体２２に対する並進力制御方向の移動を停止させると共に、一方の物体２１を他方の物体２２に対して回転力制御軸の軸回りに回転移動させ、一方の物体２１と他方の物体２２とが接触したときの一方の姿勢を取得し、合わせ状態姿勢算出部４１は、一方の物体２１と他方の物体２２とが接触したときに取得された一方の物体２１の姿勢に基づいて合わせ状態姿勢を算出し、回転力制御軸回り目標移動量算出部３６は、一方の物体２１と他方の物体２２との間に作用する力、および、合わせ状態姿勢算出部４１が算出した合わせ状態姿勢に基づいて、回転力制御軸回り目標移動量を算出することが好ましい。
【０１７９】
本発明の５番目の実施形態の説明においては、主に、本発明の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０１８０】
図１９は、本発明の５番目の実施形態のロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にするときの動作を示す部分拡大図である。図１９（ａ）には、一方の物体２１と他方の物体２２とを合わせ状態に移動させるときの開始時の状態が示されており、一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１が設定され、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸に回転力制御軸６２が設定されている。図１９（ａ）に示されるように、並進力制御方向６１に一方の物体２１を並進移動させていくと、図１９（ｂ）に示されるように、一方の物体２１と他方の物体２２とは接触点６５ａの部分において接触する。一方の物体２１と他方の物体２２とが接触したか否かは、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力と所定の閾値との比較によって判定する。または、回転力制御軸６２の軸回りの力と所定の閾値との比較や、回転力制御軸６２の軸回りの力の所定時間あたりの変化に基づいて、一方の物体２１と他方の物体２２とが接触したか否かを判定してもよい。一方の物体２１と他方の物体２２とが接触したとき、ロボット制御装置１０は、一方の物体２１の他方の物体２２に対する並進力制御方向の移動を停止させると共に、一方の物体２１の姿勢を取得する。このときの一方の物体２１の姿勢は、他方の物体２２に設定した座標系上、または、空間に対して設定した座標系上、または、ロボットの台座５２に対して設定した座標系上など、一方の物体２１の移動によって変化しない座標系上の姿勢を取得する。他方の物体２２が移動するときは、他方の物体２２に設置した座標系上の姿勢を取得してもよい。また、他方の物体２２が移動するとき、その移動量を考慮して、座標系上の姿勢を補正するようにしてもよい。
【０１８１】
一方の物体２１の他方の物体２２に対する並進力制御方向の移動を停止させるとき、一方の物体２１の他方の物体２２に対する並進力制御方向の目標移動量が０となるように、並進力制御方向目標移動量算出部３５によって算出される並進力制御方向目標移動量が０となるようにする。このとき、他の実施形態でも同様であるが、一方の物体２１の他方の物体２２に対する並進力制御方向の目標移動量が０であっても、ロボット５０の移動動作以外によって一方の物体２１または他方の物体２２の位置および／または姿勢が変化する場合、その変化する移動量をもとに、目標方向目標移動量算出部３７は、一方の物体２１を並進移動させるように移動量を算出することが好ましい。続いて、ロボット制御装置１０は、一方の物体２１の他方の物体２２に対する並進力制御方向の目標移動量が０となるように、並進力制御方向目標移動量算出部３５によって算出される並進力制御方向目標移動量が０となるようにすると共に、一方の物体２１を他方の物体２２に対して回転力制御軸の軸回りに回転移動させるように、回転力制御軸回り目標移動量算出部３６によって回転力制御軸回り目標移動量を算出する。図１９（ｂ）に示されるように、一方の物体２１と他方の物体２２とが接触したときの回転力制御軸の軸回りの力に基づいて回転移動させる方向を求め、回転方向６３に回転移動をさせる。このとき、回転力制御軸回り目標移動量算出部３６は、所定の回転方向の移動量、またはこのときの所定の回転力制御軸と平行でない並進力制御方向の並進力制御目標力、の内、少なくとも１つに基づいて、回転力制御軸回り目標移動量を算出する。並進力制御目標力を基にする場合、並進力制御目標力に対して所定の係数を乗算することによって得られる値を回転方向の移動量として算出する。または、所定の回転方向の移動量と、並進力制御目標力に所定の係数を乗算した値について、それらを選択的に使用したり、重みをつけて加算したりして算出される値を回転方向目標移動量としたり、または、前記算出される値についてロボット５０の移動動作の振動状況に応じて調整して求められる値を回転方向目標移動量としたりしてもよい。
【０１８２】
図１９（ｂ）に示される状態の一方の物体２１を、並進力制御方向６１には並進移動させず、回転力制御軸６２の軸回りの回転方向６３にのみ回転移動させると、図１９（ｃ）に示されるように、一方の物体２１と他方の物体２２とは接触点６５ｂの部分において接触する。一方の物体２１と他方の物体２２とが接触したか否かは、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力と所定の閾値との比較によって判定する。または、回転力制御軸６２の軸回りの力と所定の閾値との比較や、回転力制御軸６２の軸回りの力の所定時間あたりの変化に基づいて、一方の物体２１と他方の物体２２とが接触したか否かを判定してもよい。図１９（ｃ）に示されるように、一方の物体２１と他方の物体２２とが接触したとき、ロボット制御装置１０は、一方の物体２１の他方の物体２２に対する回転力制御軸６２の軸回りの回転方向６３への回転移動を停止させると共に、一方の物体２１の姿勢を取得する。このときの一方の物体２１の姿勢は、他方の物体２２に設定した座標系上、または、空間に対して設定した座標系上、または、ロボットの台座５２に対して設定した座標系上の姿勢を取得する。他方の物体２２が移動するときは、他方の物体２２に設置した座標系上の姿勢を取得してもよい。また、他方の物体２２が移動するとき、その移動量を考慮して、座標系上の姿勢を補正するようにしてもよい。ロボット制御装置１０が、一方の物体２１の他方の物体２２に対する回転力制御軸６２の軸回りの回転方向の回転移動を停止させるとき、一方の物体２１の他方の物体２２に対する回転力制御軸６２の軸回りの目標移動量が０となるように、回転力制御軸回り目標移動量算出部３６によって算出される回転力制御軸回り目標移動量が０となるようにする。このとき、他の実施形態でも同様であるが、一方の物体２１の他方の物体２２に対する回転力制御軸の軸回りの目標移動量が０であっても、ロボット５０の移動動作以外によって一方の物体２１または他方の物体２２の位置および／または姿勢が変化する場合、その変化する移動量をもとに、目標方向目標移動量算出部３７は、一方の物体２１を回転移動させるように移動量を算出することが好ましい。
【０１８３】
合わせ状態姿勢算出部４１は、前述のように、一方の物体２１と他方の物体２２とが接触したときに取得された一方の物体２１の姿勢に基づいて合わせ状態姿勢を算出する。図１９に示される場合では、回転力制御軸６２の位置が移動しない場合、図１９（ｃ）に示される、一方の物体２１と他方の物体２２とが接触点６５ａで接触したときの一方の物体２１ａの姿勢と、一方の物体２１と他方の物体２２とが接触点６５ｂで接触したときの姿勢に基づいて算出される、２つの姿勢の中間の姿勢が、一方の物体２１と他方の物体２２とを合わせ状態となるようにするときの姿勢である、合わせ状態姿勢となる。
【０１８４】
合わせ状態姿勢が算出された後、一方の物体２１の他方の物体２２に対する並進力制御方向６１の並進移動を再開するように、並進力制御方向目標移動量算出部３５は、並進力制御方向目標移動量を０とせずに、本発明の実施例に示されるように、一方の物体２１と他方の物体２２との間に作用する力に基づいて、並進力制御方向目標移動量を算出する。回転力制御軸回り目標移動量算出部３６は、一方の物体２１と他方の物体２２との間に作用する力、および、合わせ状態姿勢算出部４１が算出した合わせ状態姿勢に基づいて、回転力制御軸回り目標移動量を算出する。このとき、一方の物体２１の姿勢が合わせ状態姿勢に近くなるまでは、回転力制御軸回り目標移動量が大きくなるように算出し、一方の物体２１の姿勢が合わせ状態姿勢に近くなった場合、回転力制御軸回り目標移動量が小さくなるように算出する。また、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が小さい場合や、接触していない場合に、一方の物体２１の姿勢が合わせ状態姿勢に近くなるまでは、所定の回転方向の移動量、またはこのときの所定の回転力制御軸と平行でない並進力制御方向の並進力制御目標力、の内、少なくとも１つに基づいて、回転力制御軸回り目標移動量を算出するようにしてもよい。また、前述のように算出される合わせ状態姿勢と、実際の合わせ状態の姿勢のずれにより、合わせ状態姿勢に基づいて、回転力制御軸の軸回りに回転移動させているときに、一方の物体２１と他方の物体２２との間に作用する力における回転力制御軸回りの力が大きくなった場合、合わせ状態姿勢に基づいて移動させることをおこなわず、本発明の他の実施例に示される方法によって移動させることが好ましい。
【０１８５】
なお、前述の一方の物体２１と他方の物体２２とが接触したときの一方の姿勢は、ロボット５０の移動動作以外によって一方の物体２１または他方の物体２２の位置および／または姿勢が変化する場合、その変化する移動量をもとに、他方の物体２２に対する姿勢を算出することが好ましい。また、他の実施例と同様に、ロボット５０の移動動作以外によって一方の物体２１または他方の物体２２の位置および／または姿勢が変化する場合、その変化する移動量をもとに、一方の物体２１を他方の物体２２に対して移動させる相対的な移動量を算出することが好ましい。
【０１８６】
前述のように、合わせ状態姿勢算出部４１によって一方の物体２１と他方の物体２２とを合わせ状態となるようにするときの姿勢である、合わせ状態姿勢が算出して推定されると、おおよその目的とする姿勢が分かるため、素早くその姿勢に移動させると共に、並進力制御方向に素早く移動させることによって、一方の物体２１と他方の物体２２とを、安定的に素早く、合わせ状態にすることができる。また、並進力制御方向の力の大きさや回転力制御軸の軸回りの力の大きさによらず、一方の物体２１と他方の物体２２とを、安定的に素早く、合わせ状態にすることができる。
【０１８７】
本発明の６番目の実施形態においては、ロボット制御装置１０における、回転力制御軸回り目標移動量算出部３６は、本発明の１番目から５番目の実施形態のロボット制御装置１０に対して、さらに、並進力制御方向設定部３２によって設定される並進力制御方向の内、いずれかの並進力制御方向と平行でない回転力制御軸について軸回りの目標移動量を算出する際、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値以上、かつ、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、回転力制御軸回り目標移動量を０と算出し、第２の接触状態から第１の接触状態となる場合、回転力制御軸回り目標移動量を０と算出する、または、第１の接触状態となるまでの第２の接触状態のときの回転力制御軸の軸回りの移動方向および移動量に基づいて回転力制御軸回り目標移動量を算出し、第２の接触状態となる場合、回転力制御軸の軸回りの力の符号と共に、回転方向の所定の移動量である所定の回転方向移動量Ａ、または、回転力制御軸と平行でない並進力制御方向の並進力制御目標力、または、回転力制御軸と平行でない並進力制御方向の力、の内、少なくとも１つに基づいて、回転力制御軸回り目標移動量を算出することが好ましい。
【０１８８】
本発明の６番目の実施形態の説明においては、主に、本発明の１番目から５番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０１８９】
並進力制御方向設定部３２によって設定される並進力制御方向の内、いずれかの並進力制御方向と平行ではない所定の回転力制御軸については、回転力制御軸と平行ではない並進力制御方向に一方の物体２１と他方の物体２２とを押しつけることによって、回転力制御軸の軸回りの力を発生させ、その軸回りの力に基づいて、一方の物体２１と他方の物体２２とを相対的に回転移動させることが可能となる。
【０１９０】
並進力制御方向の力に関する第２の所定閾値は、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が不十分であって接触していないと判定できる値であり、また、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が不十分であることによって回転力制御軸の軸回りの力の大きさが小さくなっていて、その軸回りの力をもとに回転移動させることが困難な状態になっていると判定できる値とする。回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａは、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力の大きさが小さくなっていて、その軸回りの力をもとに回転移動させることが困難な状態になっていると判定できる値、または、一方の物体２１と他方の物体２２とが接触していて回転力制御軸の軸回りの力がつりあった状態またはつりあった状態に近い状態となっていると判定できる値とする。回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａは、一方の物体２１と他方の物体２２とが並進力制御方向について接触した後、所定時間の間の、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力に基づいて、設定するようにしてもよい。
【０１９１】
並進力制御方向の力に関する第２の所定閾値は、並進力制御方向の力に関する第１の所定閾値と同じ値であってもよい。また、並進力制御方向の力に関する第２の所定閾値は、並進力制御方向の力に関する第１の所定閾値とは異なる値に設定し、回転力制御軸回り目標移動量算出部３６が、回転力制御軸回り目標移動量を算出するのに適切な値に設定してもよい。また、並進力制御方向の力に関する第２の所定閾値を小さくすると共に、回転力制御軸回りの力が不十分でないことを確認するように、並進力制御方向の力に関する第２の所定閾値を、並進力制御方向の力に関する第１の所定閾値より大きい値に設定してもよい。また、回転力制御軸の軸回りの力を、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａによって確認するので、並進力制御方向の力に関する第２の所定閾値は、並進力制御方向の力に関する第１の所定閾値よりも小さい値にしてもよい。また、並進力制御方向の力に関する第２の所定閾値は、並進力制御方向の力に載るノイズや、ロボット５０の移動動作による振動や、力制御による振動などを考慮して設定することが好ましい。
【０１９２】
回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａを大きい値に設定する場合、移動すべき回転移動の方向をより正しく判断して、回転力制御軸の軸回りの適切な方向に移動させることができるが、回転力制御軸回りの力が小さくなって頻繁に前記所定閾値を下回る場合、回転力制御軸回りの力をもとに移動させることができず、適切に回転移動させられないことがある。また、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａを小さい値に設定する場合、回転力制御軸回りの力が小さい場合であっても回転力制御軸回りの力をもとに移動させることができるが、移動方向の向きなどを頻繁に不適切に判断し、適切に回転移動させられないことがある。
【０１９３】
このように、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａは、回転力制御軸の軸回りの力に載るノイズや、ロボット５０の移動動作による振動や、力制御による振動などを考慮して設定することが好ましい。回転力制御軸の軸回りの力をもとに移動させる場合、回転力制御軸の軸回りの力の正負の符号が頻繁に入れ替わることによって回転力制御軸の軸回りに回転移動させるときの方向を適切に判断できなくなったり、振動やノイズが載った力を不適切に使用してロボット５０を移動させると発振的な動作の原因になったりすることもある。このため、このような所定閾値を適切に設定して処理された回転力制御軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させることが好ましい。また、一方の物体２１と他方の物体２２との間に作用する力に基づいて一方の物体２１と他方の物体２２との接触状態を判断する場合、特に、回転軸回りの力が小さい場合にその値に基づいて一方の物体２１と他方の物体２２との接触状態を判断する場合、力データに対して適切なフィルタリング処理や推定処理や適切な所定閾値による値の調整などをおこなって、ノイズや急な値の変化に適切に処理して、そのような力データに基づいて、回転力制御軸回りの力が作用する状況を判断することが好ましい。
【０１９４】
第１の接触状態は、一方の物体２１と他方の物体２２とが並進力制御方向について非接触状態または適切に接触していない状態、または、一方の物体２１と他方の物体２２とが並進力制御方向について接触はしているが回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さい状態、または、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて回転力制御軸回りの力がつりあった状態である。
【０１９５】
第２の接触状態は、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さくはなく所定の値より大きい状態であり、回転力制御軸の軸回りの力に基づいて、一方の物体２１と他方の物体２２とを相対的に回転移動させることが可能な状態である。
【０１９６】
第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合とは、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さく、回転力制御軸の軸回りの力をもとに、一方の物体２１を適切に回転移動させることができない場合、かつ、それまでに一方の物体２１と他方の物体２２とが接触していない場合などである。第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、所定の回転力制御軸の軸回りについては回転移動をさせないように、回転力制御軸回り目標移動量算出部３６は、回転力制御軸回り目標移動量を０と算出する。
【０１９７】
第２の接触状態から第１の接触状態となる場合とは、一方の物体２１と他方の物体２２とが接触して回転力制御軸の軸回りの力が発生し、回転力制御軸の軸回りに回転移動させたことによって、一方の物体２１と他方の物体２２とが非接触状態または適切に接触していない状態、または、一方の物体２１と他方の物体２２とが並進力制御方向について接触はしているが回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さい状態、または、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて回転力制御軸回りの力がつりあった状態となる場合などである。
【０１９８】
第２の接触状態から第１の接触状態となる場合、回転力制御軸の軸回りについて回転移動をさせないように、回転力制御軸回り目標移動量算出部３６は、回転力制御軸回り目標移動量を０と算出する。第１の接触状態から、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて回転力制御軸の軸回りの力がつりあった状態になる場合、回転力制御軸の軸回りについて回転移動をさせないことが好ましい。また、それまでの移動動作からどのように移動させればよいかを推定して移動させることが困難であったり、適切な移動量以上に回転移動させて合わせ状態にするのに逆に時間がかかってしまったりする場合は、回転移動させないことが好ましい。
【０１９９】
または、第２の接触状態から第１の接触状態となる場合、一方の物体２１と他方の物体２２とが接触して回転力制御軸の軸回りの力が発生し、回転力制御軸の軸回りに回転移動させたことによって、第１の接触状態となるまでの、第２の接触状態における回転力制御軸の軸回りの移動方向や移動量に基づいて、それまでの移動動作からどのように移動させればよいかを推定して移動量を算出してもよい。このとき、それまでの回転移動についての移動方向および移動量から、次の制御周期での移動方向と移動量を推定して算出する。なお、このときの回転移動量が大きいと一方の物体２１と他方の物体２２とを合わせ状態に収束させるのに時間がかかるようになることもあるので、適切な移動量となるように小さく調整して算出することが好ましい。第１の接触状態が一方の物体２１と他方の物体２２とが非接触状態または接触が不十分となる場合であって、並進力制御方向への移動速度が大きく、第１の接触状態となっても短い時間で再び第１の接触状態から第２の接触状態にとなる場合、第１の接触状態の間に、前述のように移動させることによって、一方の物体２１と他方の物体２２とをより短い時間で合わせ状態にすることができる。
【０２００】
第２の接触状態では、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さくはなく所定の値より大きい状態であり、回転力制御軸の軸回りの力に基づいて、一方の物体２１と他方の物体２２とを相対的に回転移動させることができる。
【０２０１】
回転力制御軸の軸回りの力と、回転力制御軸の軸回りの力の目標力との差分に対して力制御ゲインを乗算して、一方の物体２１と他方の物体２２とを相対的に回転移動させる移動量を算出する場合、一方の物体２１と他方の物体２２とを合わせ状態とするには、回転力制御軸の軸回りの力の目標力を０または０に近い小さい値とする。回転力制御軸の軸回りの力の目標力を０とする場合では、回転力制御軸の軸回りの力に応じて一方の物体２１を受動的に回転移動させることになる。
【０２０２】
一方の物体２１と他方の物体２２との間に作用する回転力制御軸の軸回りの力に応じて、一方の物体２１と他方の物体２２とを合わせ状態にする場合、一方の物体２１と他方の物体２２の最初の相対的な位置および姿勢の関係や、一方の物体２１と他方の物体２２がどのような大きさ、形状および材質の物体であるかなどの状況によって、一方の物体２１と他方の物体２２とが接触するときの回転力制御軸の軸回りの力は変わるため、回転軸の軸回りの力の大きさによって、回転力制御軸の軸回りの回転移動は速くなったり遅くなったりする。
【０２０３】
また、並進力制御方向の力が小さい場合や、回転軸と接触する位置との距離が小さい場合は、回転軸の軸回りの力は小さくなる。さらに、回転軸の軸回りの力の大きさは、並進力制御方向の力と、力が作用する点と回転軸までの距離によって定まるため、一方の物体２１と他方の物体２２とが接触する位置の変動や、並進力制御方向の力の変動や、ロボット５０の移動による振動や、力制御による振動によって、ノイズが載りやすく、また変動も大きく、符号の反転が頻繁に発生する場合もある。一方の物体２１と他方の物体２２とを合わせ状態とするとき、並進力制御方向の力は所定の値の目標力となるように制御するのに対して、回転力制御軸の軸回りの力は、その回転力制御軸の軸回りの力がつりあった状態となるように、接触状態での回転力制御軸の軸回りの力が０となるように制御するため、前述のような回転力制御軸の軸回りの力の変化によって、力の符号が頻繁に変化したりすることがある。回転力制御軸の軸回りの力に大きなノイズが載ったり、力の変動が大きかったり、力の符号の反転が頻発したりする場合、回転力制御軸回りの力に力制御ゲインを乗算して、ロボット５０を移動させると発振しやすくなるため、力制御ゲインはあまり大きくすることができない。また、回転力制御軸の軸回りの力は小さいことが多く、回転力制御軸の軸回りの力と目標力との差分に対して力制御ゲインを乗算する方法では、回転力制御軸回りの移動速度を大きくすることができない場合がある。
【０２０４】
前述のように、回転力制御軸の軸回りの力に対して力制御ゲインを乗算して、一方の物体２１と他方の物体２２とを相対的に回転移動させる移動量を算出する方法では、安定的に、素早く、並進力制御目標力を小さくして、合わせ状態とすることは難しい場合がある。
【０２０５】
そこで、回転力制御軸の軸回りの力に対して、回転力制御軸の軸回りの力が所定閾値以上である場合の力の符号を用いるようにすると共に、より安定的な値や情報に基づいて回転力制御軸の軸回りの移動量の大きさを算出する。
【０２０６】
第２の接触状態のとき、回転力制御軸の軸回りの力の符号をもとに、回転力制御軸の軸回りの回転移動の方向を求め、移動量の符号を求める。回転力制御軸の軸回りの回転移動の移動量の大きさは、回転方向の所定の移動量である所定の回転方向移動量Ａをもとにして算出する。回転力制御軸の軸回りの力のように変動しやすい値を用いず、所定の値を基にすることによって安定的に移動させることができる。また、所定の移動量を用いることによって、回転移動の速度を所望の速度に設定することができる。
【０２０７】
または、回転力制御軸と平行でない並進力制御方向の並進力制御目標力をもとに、並進力制御目標力の大きさに所定の係数を乗算するなどして、回転力制御軸の軸回りの移動量の大きさを算出する。これによって、並進力制御目標力に応じて、回転力制御軸の軸回りの移動量の大きさを調整することができる。
【０２０８】
または、回転力制御軸と平行でない並進力制御方向の力をもとに、所定の係数を乗算するなどして、回転力制御軸の軸回りの移動量の大きさを算出する。これによって、並進力制御方向の力に応じて、回転力制御軸の軸回りの移動量の大きさを調整することができる。
【０２０９】
または、前述のように算出される値に重みをつけて加算したり、選択して加算させたりして、回転力制御軸の軸回りの移動量の大きさを算出するようにしてもよい。また、回転方向の所定の移動量である所定の回転方向移動量Ａ、回転力制御軸と平行でない並進力制御方向の並進力制御目標力、回転力制御軸と平行でない並進力制御方向の力を利用し、回転方向の所定の移動量である所定の回転方向移動量Ａに或る係数を乗算した値を、回転力制御軸の軸回りの移動量として算出するとき、前記或る係数を並進力制御方向の力が並進力制御目標力に近い場合、大きくなるように調整するようにして、回転力制御軸の軸回りの移動量の大きさを調整してもよい。これによって、一方の物体２１と他方の物体２２との間に作用する力と並進力制御目標力との差に基づいて、前記差が大きい場合は、安定した状態になっていないと判断して、回転力制御軸回りの移動速度を小さくし、前記差が小さい場合は、安定した状態になっていると判断して、回転力制御軸回りの移動速度を大きくすることができる。
【０２１０】
第２の接触状態の場合、以上のようにして算出した回転力制御軸の軸回りの回転移動の方向と、回転力制御軸の軸回りの回転移動の移動量の大きさとによって、回転力制御軸回り目標移動量を算出する。
【０２１１】
前述のように、回転力制御軸の軸回りの力の大きさを使用せず、より安定的に回転移動の移動量を算出することができるような値を基にすることによって、安定かつ素早く回転移動させることができる。
【０２１２】
本発明の７番目の実施形態においては、ロボット制御装置１０における、回転力制御軸回り目標移動量算出部３６は、本発明の１番目から５番目の実施形態のロボット制御装置１０に対して、さらに、並進力制御方向設定部３２によって設定される並進力制御方向の内、いずれかの並進力制御方向と平行でない回転力制御軸について軸回りの目標移動量を算出する際、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値以上、かつ、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、回転力制御軸回り目標移動量を０と算出し、第２の接触状態から第１の接触状態となる場合、回転力制御軸回り目標移動量を０と算出する、または、第１の接触状態となるまでの第２の接触状態のときの回転力制御軸の軸回りの移動方向および移動量に基づいて回転力制御軸回り目標移動量を算出し、第２の接触状態となる場合、回転力制御軸の軸回りの力、または、回転力制御軸の軸回りの力および一方の物体２１と他方の物体２２との間に作用する力を制御するときの所定条件での回転力制御軸の軸回りの力の最大値、または、回転方向の所定の移動量である所定の回転方向移動量Ｂおよび回転力制御軸の軸回りの力および一方の物体２１と他方の物体２２との間に作用する力を制御するときの所定条件での回転力制御軸の軸回りの力の最大値、に基づいて、回転力制御軸回り目標移動量を算出することが好ましい。
【０２１３】
本発明の７番目の実施形態の説明においては、主に、本発明の１番目から５番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０２１４】
第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、また、第２の接触状態から第１の接触状態となる場合、回転力制御軸回り目標移動量の算出は、本発明の６番目の実施形態のロボット制御装置１０における、第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、また、第２の接触状態から第１の接触状態となる場合と同様に実施される。
【０２１５】
第２の接触状態では、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さくはなく所定の値より大きい状態であり、回転力制御軸の軸回りの力に基づいて、一方の物体２１と他方の物体２２とを相対的に回転移動させるように、回転力制御軸回り目標移動量を算出してもよい。
【０２１６】
回転力制御軸の軸回りの力と、回転力制御軸の軸回りの力の目標力との差分に対して力制御ゲインを乗算して、一方の物体２１と他方の物体２２とを相対的に回転移動させる移動量を算出する場合、一方の物体２１と他方の物体２２とが合わせ状態となるようにするために、回転力制御軸の軸回りの力の目標力を０または０に近い小さい値とする。回転力制御軸の軸回りの力の目標力を０とする場合では、回転力制御軸の軸回りの力に応じて一方の物体２１を受動的に回転移動させることになる。
【０２１７】
回転力制御軸の軸回りの力が大きく不安定ではない場合、または、合わせ状態に近い状態であり回転力制御軸の軸回りの力の大きさに応じて移動させることが好ましい場合、または、回転力制御軸の軸回りの力データに対して適切な推定処理やフィルタリング処理が実行されて適切な値となるように調整されている場合、回転力制御軸の軸回りの力の大きさも考慮して、回転力制御軸の軸回りの力に基づいて、回転力制御軸回り目標移動量を算出することが好ましい。
【０２１８】
回転力制御軸の軸回りの力の大きさを考慮することによって、一方の物体２１と他方の物体２２とを合わせ状態を通過して移動させることなく、合わせ状態に近づくにつれて、回転力制御軸の軸回りの回転移動の移動量を適切に調整して、合わせ状態にスムーズに収束させることが可能となる。
【０２１９】
一方の物体２１と他方の物体２２との間に作用する回転力制御軸の軸回りの力に対して力制御ゲインを乗算する場合、回転力制御軸の軸回りの力が大きく、所定の上限値以上である場合は所定の上限値とし、回転力制御軸の軸回りの力が小さく、所定の下限値未満である場合は所定の下限値に調整して、力制御ゲインを乗算することが好ましい。
【０２２０】
回転力制御軸の軸回りの力を所定の値とする所定の上限値や所定の下限値の設定は、一方の物体２１と他方の物体２２とを接触させる状況がある程度分かっている場合や並進力制御目標力が大きい場合、並進力制御目標力、または想定される状況に基づいて、設定することが好ましい。なお、前記所定の下限値は、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａと同じ値であってもよい。
【０２２１】
また、第２の接触状態では、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力、および、一方の物体２１と他方の物体２２との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値に基づいて、回転力制御軸回り目標移動量を算出してもよい。
【０２２２】
なお、前記所定の上限値、前記所定の下限値を、一方の物体２１と他方の物体２２との間に作用する力を制御するときの所定条件での回転力制御軸の軸回りの力の最大値を基に、前記最大値に所定の係数を乗算することによって求めるようにしてもよい。これによって、一方の物体２１と他方の物体２２とが接触する状況に応じて、所定の上限値、所定の下限値を設定することができる。
【０２２３】
なお、回転力制御軸の軸回りの力や回転力制御軸の軸回りの力の最大値は、回転力制御軸の軸回りの力データに対して適切な、推定処理やフィルタリング処理を実行し、急激に変化した値や、他の値に対して異常に大きいまたは小さい値などの異常値を除去したり、適切な値となるように調整したりして算出される値に基づいて、求められることが好ましい。
【０２２４】
前記所定条件での回転力制御軸の軸回りの力の最大値は、次のような所定の条件で取得される回転力制御軸の軸回りの力の最大値とする。一方の物体２１と他方の物体２２との間に作用する力を制御するときの現時点までに取得される回転力制御軸の軸回りの力の最大値、または、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が最初に所定閾値または並進力制御方向の力に関する第２の所定閾値以上となったときの回転力制御軸の軸回りの力の最大値、または、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が最初に所定閾値または並進力制御方向の力に関する第２の所定閾値以上となってから所定時間だけ経過した後から別の所定時間の間において取得される回転力制御軸の軸回りの力の最大値、または、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が現時点までに所定閾値または並進力制御方向の力に関する第２の所定閾値以上となるときに取得される回転力制御軸の軸回りの力の最大値、または、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が最初に所定の閾値以上となるときに取得される回転力制御軸の軸回りの力の最大値、または、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が最初に所定の閾値以上となってから所定時間だけ経過した後から別の所定時間の間において取得される回転力制御軸の軸回りの力の最大値、または、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が現時点までに所定の閾値以上となるときに取得される回転力制御軸の軸回りの力の最大値などである。
【０２２５】
一方の物体２１と他方の物体２２との間に作用する回転力制御軸の軸回りの力に対して力制御ゲインを乗算して求められる値を基に回転移動の移動量を算出する場合、前述した所定条件において取得される回転力制御軸の軸回りの力の最大値に対して係数をかけて所定の上限値および所定の下限値を算出し、算出された所定の上限値、所定の下限値によって、回転力制御軸の軸回りの力の値を調整した値に対して、力制御ゲインを乗算するようにしてもよい。
【０２２６】
また、一方の物体２１と他方の物体２２との間に作用する回転力制御軸の軸回りの力に対して力制御ゲインを乗算する場合、所定の上限値、所定の下限値で調整すると共に、回転力制御軸回りの力を、前述した所定条件において取得される回転力制御軸の軸回りの力の最大値で除算して調整された値に対して、力制御ゲインを乗算するようにしてもよい。
【０２２７】
このように、所定条件において取得される回転力制御軸の軸回りの力の最大値を用いて、回転力制御軸の軸回りの力の値を調整した値に対して、力制御ゲインを乗算して、回転力制御軸回り目標移動量を算出することによって、一方の物体２１と他方の物体２２とを接触させるときの状況に応じて、回転力制御軸回り目標移動量が適切な値となるように調整することができ、移動量が過度に大きくなったり、意図しない方向に移動させることを防いだりすることができる。
【０２２８】
また、第２の接触状態では、回転方向の所定の移動量である所定の回転方向移動量Ｂ、および、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力、および、一方の物体２１と他方の物体２２との間に作用する力を制御するときの所定条件での回転力制御軸の軸回りの力の最大値に基づいて、回転力制御軸回り目標移動量を算出してもよい。
【０２２９】
本発明の７番目の実施形態のロボット制御装置１０における、所定の回転方向移動量Ｂは回転方向の所定の移動量とする。回転方向の所定の移動量である所定の回転方向移動量Ｂは、或る定数によって与えられる値、または、或る定数によって与えられる値と回転力制御軸と平行でない並進力制御方向の押付目標力に所定の係数を乗算して得られる値とに重み係数を乗算した値を加算して求められる値としてもよい。
【０２３０】
現在の回転力制御軸の軸回りの力を、前述のように所定条件で取得される回転力制御軸の軸回りの力の最大値によって除算して算出される値を、回転方向の所定の移動量である所定の回転方向移動量Ｂに対して乗算することによって、回転力制御軸回り目標移動量を算出してもよい。
【０２３１】
また、回転方向の所定の移動量である所定の回転方向移動量Ｂと、前述のように所定条件で取得される回転力制御軸の軸回りの力の最大値によって回転力制御軸の軸回りの力の値を調整して求められる値に力制御ゲインを乗算して算出される値とに対して、重み係数を乗算して加算した値を、回転力制御軸回り目標移動量として算出してもよい。
【０２３２】
また、前述のように所定条件で取得される回転力制御軸の軸回りの力の最大値によって回転力制御軸の軸回りの力の値を調整した値に力制御ゲインを乗算して算出される値と、前述のように、現在の回転力制御軸の軸回りの力を所定条件での回転力制御軸の軸回りの力の最大値によって除算して算出される値を回転方向の所定の移動量である所定の回転方向移動量Ｂに対して乗算して算出される値とに対して、重み係数を乗算して加算した値を、回転力制御軸回り目標移動量として算出してもよい。
【０２３３】
前述のように、所定の回転方向の移動量と、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力、および、一方の物体２１と他方の物体２２との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値に基づいて、回転力制御軸回り目標移動量を算出することによって、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力の大きさに応じて、回転力制御軸回り目標移動量を調整して算出することができる。一方の物体２１の移動動作によらない所定の方向に並進力制御方向が設定される場合などでは、一方の物体２１と他方の物体２２とが合わせ状態に近づくにつれて、回転力制御軸の軸回りの一方の物体２１と他方の物体２２との間に作用する力は小さくなる。このような場合、一方の物体２１と他方の物体２２とが合わせ状態でないとき、他方の物体２２に対する一方の物体２１の回転移動の速度をなるべく大きくして移動させ、一方の物体２１と他方の物体２２とが合わせ状態に近づくにつれて、また一方の物体２１と他方の物体２２とが合わせ状態に近づいた場合、他方の物体２２に対する一方の物体２１の回転力制御軸の軸回りの速度を小さくすることによって、合わせ状態を通過させることなく、合わせ状態にスムーズに収束させることができる。
【０２３４】
図６は、本発明の８番目の実施形態に係るロボット制御装置１０ｅの構成を機能的に示す図である。本発明の８番目の実施形態においては、本発明の１番目から５番目のいずれかの実施形態のロボット制御装置１０に対して、さらに、合わせ状態判定部４２を備えるロボット制御装置の構成としてもよい。本発明の８番目の実施形態においては、図６において図示されるように、ロボット制御装置１０ａに対して、さらに、合わせ状態判定部４２を備える。また、後述の機能も含めて図８に図示されるロボット制御装置１０ｇの構成としてもよい。
【０２３５】
本発明の８番目の実施形態においては、ロボット制御装置１０は、本発明の１番目から５番目の実施形態のロボット制御装置１０に対して、さらに、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態である、合わせ状態になっているか否か、または、一方の物体２１と他方の物体２２とが合わせ状態に近い状態である、合わせ状態近傍になっているか否か、の一方の物体２１と他方の物体２２との合わせ状態の判定を実行する合わせ状態判定部４２を備え、回転力制御軸回り目標移動量算出部３６は、並進力制御方向設定部３２によって設定される並進力制御方向の内、いずれかの並進力制御方向と平行でない回転力制御軸について軸回りの目標移動量を算出する際、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値未満、または、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ未満となる状態を第１の接触状態とし、回転力制御軸と平行でない並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値以上、かつ、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａ以上である状態を第２の接触状態とするとき、第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、回転力制御軸回り目標移動量を０と算出し、第２の接触状態から第１の接触状態となる場合、回転力制御軸回り目標移動量を０と算出する、または、第１の接触状態となるまでの第２の接触状態のときの回転力制御軸の軸回りの移動方向および移動量に基づいて回転力制御軸回り目標移動量を算出し、第２の接触状態であって合わせ状態判定部４２によって合わせ状態近傍または合わせ状態になっていると判定される状態、または、第２の接触状態であって合わせ状態判定部４２によって合わせ状態近傍または合わせ状態になっていると判定されたとき以降の状態、を第２Ａの接触状態とし、第２の接触状態であって第２Ａの接触状態でない状態を第２Ｂの接触状態とするとき、第２Ｂの接触状態である場合、回転力制御軸の軸回りの力の符号と共に、回転方向の所定の移動量である所定の回転方向移動量Ａ、または、回転力制御軸と平行でない並進力制御方向の並進力制御目標力、または、回転力制御軸と平行でない並進力制御方向の力、の内、少なくとも１つに基づいて、回転力制御軸回り目標移動量を算出し、第２Ａの接触状態である場合、回転力制御軸の軸回りの力、または、回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、または、回転方向の所定の移動量である所定の回転方向移動量Ｂおよび前記回転力制御軸の軸回りの力および前記一方と前記他方との間に作用する力を制御するときの所定条件での前記回転力制御軸の軸回りの力の最大値、に基づいて回転力制御軸回り目標移動量を算出する、または、第２Ｂの接触状態である場合に算出される回転力制御軸回り目標移動量より小さい値に調整して回転力制御軸回り目標移動量を算出することが好ましい。
【０２３６】
本発明の８番目の実施形態の説明においては、主に、本発明の１番目から５番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０２３７】
合わせ状態判定部４２は、一方の物体２１と他方の物体２２とが、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態である、合わせ状態になっているか否か、または、一方の物体２１と他方の物体２２とが合わせ状態に近い状態である、合わせ状態近傍になっているか否か、の一方の物体２１と他方の物体２２との合わせ状態の判定を実行する。
【０２３８】
合わせ状態判定部４２は、所定の回転力制御軸と直交する方向についての、一方の物体２１の制御点の所定時間における位置またはその変位、または、所定の回転力制御軸の軸回りの力と、所定の回転力制御軸の軸方向の力に基づいて算出される仮想作用点における所定の回転力制御軸に対する位置、または、所定の回転力制御軸の軸回りの力の符号の変化、または、所定の回転力制御軸の軸回りの力と所定閾値との比較、の内、少なくともいずれか１つに基づいて、所定の回転力制御軸の軸回りについて、合わせ状態になっているか否かの合わせ状態判定、または、合わせ状態近傍になっているか否かの合わせ状態の判定を実行する。
【０２３９】
回転力制御軸の軸回りの一方の物体２１と他方の物体２２との間に作用する力が、所定の閾値より小さいか否かによって、合わせ状態を判定する場合、所定の閾値の設定が不適切であると、一方の物体２１と他方の物体２２とが、合わせ状態または合わせ状態に近い状態になっているにも関わらず、合わせ状態または合わせ状態に近い状態になっていないと判定されることがある。これには、回転力制御軸の軸回りの回転移動の制御性能や、並進力制御方向の並進移動の制御性能や、回転力制御軸の軸回りの力に載るノイズや、ロボット５０の振動や、力制御時の振動や、一方の物体２１と他方の物体２２とが接触する部分の移動やずれ、一方の物体２１と他方の物体２２とが接触する部分の中心点の移動やずれなどの複数の要因がある。
【０２４０】
一方の物体２１と他方の物体２２とが、合わせ状態または合わせ状態に近い状態になっているにも関わらず、合わせ状態または合わせ状態に近い状態になっていないと判定されることがあるため、回転力制御軸の軸回りの一方の物体２１と他方の物体２２との間に作用する力が、所定の閾値より小さいか否かだけではなく、前述のような判定方法によって、合わせ状態になっているか否かの合わせ状態判定、または、合わせ状態近傍になっているか否かの合わせ状態の判定を実行することが好ましい。
【０２４１】
第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合、本発明の６番目、７番目の実施形態のロボット制御装置１０における、第１の接触状態であって、第２の接触状態から第１の接触状態になるときではない場合の処理と同様に実施される。
【０２４２】
また、第２の接触状態から第１の接触状態となる場合、回転力制御軸回り目標移動量の算出方法は、本発明の６番目、７番目の実施形態のロボット制御装置１０における、第２の接触状態から第１の接触状態となる場合と同様に実施されるが、本発明の８番目の実施形態におけるロボット制御装置１０の場合、合わせ状態判定部４２によって合わせ状態近傍または合わせ状態になっていると判定される場合、回転力制御軸回り目標移動量を０と算出してもよい。
【０２４３】
第２の接触状態では、一方の物体２１と他方の物体２２とが並進力制御方向について接触していて、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力が小さくはなく所定の値より大きい状態である。
【０２４４】
第１の接触状態となっているか否かを判定する閾値であり、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａを小さい値に設定することによって、回転力制御軸回りの力が小さく、その回転力制御軸回りの力に基づいて移動させることができない場合は、回転力制御軸回り目標移動量を０とすることができる。ここで、回転力制御軸回りの力が小さく、その回転力制御軸回りの力に基づいて移動させることができない場合には、一方の物体２１と他方の物体２２とが合わせ状態になっている場合も含まれる。
【０２４５】
なお、他の実施例と同様に、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａは、一方の物体２１と他方の物体２２とが並進力制御方向について所定閾値以上の力で接触した後の所定時間の間の、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力に基づいて設定するようにしてもよい。
第２の接触状態において、回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ａが小さい場合、一方の物体２１と他方の物体２２とは合わせ状態や合わせ状態に近い状態となっていることがある。また、第２の接触状態において、一方の物体２１と他方の物体２２とが、それ以前に合わせ状態または合わせ状態近傍となったが回転移動によって行き過ぎてしまった状態となっている場合や、合わせ状態または合わせ状態近傍の状態と、合わせ状態または合わせ状態近傍でない状態とを何度か行き来してスムーズに収束しない場合などがある。
【０２４６】
第２の接触状態であって一方の物体２１と他方の物体２２とが合わせ状態や合わせ状態に近い状態となっている場合である、第２Ａの接触状態の場合、合わせ状態にスムーズに収束させるように、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力の大きさに基づいて、回転力制御軸の軸回りに回転移動させることが好ましい。このとき、一方の物体２１と他方の物体２２との接触状況によって、一方の物体２１と他方の物体２２とが合わせ状態から離れた状態になることもあるので、合わせ状態判定部４２によって、合わせ状態または合わせ状態近傍ではないと判定される場合、回転移動の方法を変えて、なるべく速く移動させてもよい。
【０２４７】
第２の接触状態であって一方の物体２１と他方の物体２２とが合わせ状態や合わせ状態に近い状態となったとき以降の場合である、別の第２Ａの接触状態の場合、合わせ状態にスムーズに収束させるように、合わせ状態や合わせ状態に近い状態となったとき以降は、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力の大きさに基づいて、回転力制御軸の軸回りに回転移動させてもよい。これによって、回転移動の移動量が大きくなるように移動させている場合であっても、移動量の算出方法を切り替え、合わせ状態にスムーズに収束させることができるようになる。
【０２４８】
または、第２Ａの接触状態の場合には、第２Ｂの接触状態の場合に算出される回転力制御軸回り目標移動量より小さくなるように、回転力制御軸回り目標移動量を調整して算出してもよい。これによって、合わせ状態を通過して合わせ状態への収束に時間がかかるような状況を回避し、よりスムーズに合わせ状態に収束させることができる。また、第２Ｂの接触状態の場合、移動量が大きくなるようにし、第２Ａの接触状態の場合に、回転移動の移動量が小さくなるようにすることによって、算出方法が簡単になると共に、回転力制御軸回りの力の大きさを使用しなくても、合わせ状態にスムーズに収束させることができる。
【０２４９】
第２の接触状態であって第２Ａの接触状態でない、第２Ｂの接触状態の場合、安定的に回転移動の移動量が大きくなるように移動させる。これによって、一方の物体２１と他方の物体２２とが合わせ状態から離れている場合に、素早く安定的に、合わせ状態や合わせ状態近傍に移動させることができる。第２Ｂの接触状態である場合、このように移動させるため、前述の部分で説明した、本発明の第６番目の実施形態の第２の接触状態の場合に回転力制御軸回り目標移動量を算出する方法と同様に、回転力制御軸の軸回りの力の符号をもとに回転移動の方向を求め、回転方向の所定の移動量である所定の回転方向移動量Ａ、または、回転力制御軸と平行でない並進力制御方向の並進力制御目標力、または、回転力制御軸と平行でない並進力制御方向の力、の内、少なくとも１つに基づいて、回転移動の移動量の大きさを算出することによって、回転力制御軸回り目標移動量を算出する。
【０２５０】
第２Ａの接触状態である場合に、前述のように移動させるため、前述の部分で説明した、本発明の第７番目の実施形態の第２の接触状態の場合に回転力制御軸回り目標移動量を算出する方法と同様に、回転力制御軸の軸回りにおける一方の物体２１と他方の物体２２との間に作用する力の大きさも考慮するようにして、回転力制御軸回り目標移動量を算出する。または、第２Ｂの接触状態である場合に算出される回転力制御軸回り目標移動量より小さい値に調整して回転力制御軸回り目標移動量を算出する。このとき、第２Ｂの接触状態である場合に算出される回転力制御軸回り目標移動量に１より小さい係数を乗算して回転力制御軸回り目標移動量を算出する。前述の乗算する係数は、１より小さい所定の定数としてもよい。または、前述の乗算する係数は、第２Ｂの接触状態のときの回転移動の動作時の回転力制御軸回りの力の大きさに基づいて、回転力制御軸回りの力が小さい場合に小さい値となるような係数としてもよい。または、前述の乗算する係数は、回転力制御軸回りの力の変化または振動に基づいて、回転力制御軸回りの力の大きさの変化が大きいまたは振動が大きい場合は、小さい値となるような係数としてもよい。
【０２５１】
前述のように回転力制御軸回り目標移動量を算出して一方の物体２１を移動させることによって、一方の物体２１と他方の物体２２とが合わせ状態から離れた状態にある場合は、回転移動を速くして移動させ、合わせ状態に近い場合は、回転移動を状況に応じた速度にして合わせ状態に短い時間で収束させるようにすることによって、一方の物体２１と他方の物体２２とを素早く安定的に合わせ状態とすることができる。
【０２５２】
本発明の９番目の実施形態においては、ロボット制御装置１０における、回転力制御軸回り目標移動量算出部３６は、本発明の８番目の実施形態のロボット制御装置１０に対して、さらに、回転力制御軸と平行でない並進力制御方向における、一方の物体２１と他方の物体２２との間に作用する力が並進力制御方向の力に関する第２の所定閾値以上、かつ、合わせ状態判定部４２によって合わせ状態になっていると判定される状態が、所定の時間以上連続するとき、回転力制御軸回り目標移動量算出部３６は、回転力制御軸回り目標移動量を０とする、または、回転力制御軸回り目標移動量が、算出された回転力制御軸回り目標移動量より小さくなるように調整して算出し、並進力制御方向目標移動量算出部３５は、並進力制御方向目標移動量を０とする、または、並進力制御方向目標移動量が、算出された並進力制御方向目標移動量より小さくなるように調整して算出することが好ましい。
【０２５３】
本発明の９番目の実施形態の説明においては、主に、本発明の８番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０２５４】
回転力制御軸回り目標移動量と並進力制御方向目標移動量を算出するとき、並進力制御方向の力に関する第２の所定閾値以上、かつ、合わせ状態判定部４２によって合わせ状態になっていると判定される状態が、所定の時間以上連続する場合には、回転力制御軸回り目標移動量と並進力制御方向目標移動量を小さく調整することが好ましい。
【０２５５】
並進力制御方向の力に関する第２の所定閾値以上、かつ、合わせ状態判定部４２によって合わせ状態になっていると判定される状態が、所定の時間以上連続する場合、回転力制御軸回り目標移動量を０にする、または、それまでに算出された値に対して１より小さい係数をかけるなどして、それまでに算出された値より小さくなるよう調整して回転力制御軸回り目標移動量を算出する。さらに、並進力制御方向目標移動量を０にする、または、それまでに算出された値に対して１より小さい係数をかけるなどして、それまでに算出された値より小さくなるよう調整して並進力制御方向目標移動量を算出する。
【０２５６】
これによって、一方の物体２１と他方の物体２２とをより安定して合わせ状態にし、一方の物体２１と他方の物体２２との間に作用する力にノイズが載ったり、力制御による振動やロボット５０の移動動作の振動などによって一方の物体２１と他方の物体２２とが合わせ状態から離れたり、合わせ状態にスムーズに収束しなかったりする状況となることを防ぐことができる。
【０２５７】
本発明の１０番目の実施形態においては、ロボット制御装置１０は、本発明の１番目から９番目のいずれかの実施形態のロボット制御装置１０に対して、さらに、ロボットの台座５２の位置または姿勢または位置および姿勢が移動するとき、ロボットの台座５２の移動に基づいて、他方の物体２２に対する一方の物体２１の移動方向および移動量を補正して、または、他方の物体２２の位置または姿勢または位置および姿勢が移動するとき、他方の物体２２の移動に基づいて、他方の物体２２に対する一方の物体２１の移動方向および移動量を補正して、または、ロボットの台座５２の位置または姿勢または位置および姿勢と、他方の物体２２の位置または姿勢または位置および姿勢とが移動するとき、ロボットの台座５２の移動、および、他方の物体２２の移動に基づいて、他方の物体２２に対する一方の物体２１の移動方向および移動量を補正して、一方の物体２１を他方の物体２２に対して移動させるようにロボット５０の動作指令を生成することが好ましい。
【０２５８】
本発明の１０番目の実施形態の説明においては、主に、本発明の１番目から９番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０２５９】
本実施例では、一方の物体２１を保持しているロボットの台座５２が、位置および／または姿勢が移動可能な機構部を有する、装置や走行軸や別のロボットなどのロボットの設置装置５３に設置されたり、取りつけられたり、保持されたりなどして存在しているとする。また、他方の物体２２が位置および／または姿勢が移動可能な機構部を有する、装置やコンベアや別のロボットやジグなどの他方の物体の設置装置２４に設置されたり、取りつけられたり、保持されたりなどして存在しているとする。
【０２６０】
なお、ロボットの台座５２の位置および／または姿勢の移動量は、ロボットの設置装置５３を駆動させる別の制御装置からロボット制御装置１０に送信される。または、ロボットの設置装置５３はロボット制御装置１０によって駆動されるので、ロボットの台座５２の位置および／または姿勢の移動量は既知であるとする。または、ロボットの台座５２の位置および／または姿勢の移動量を取得するセンサによって、ロボットの台座５２の位置および／または姿勢の移動量は、ロボット制御装置１０に送信されるとする。
【０２６１】
また、他方の物体２２の位置および／または姿勢は、他方の物体の設置装置２４を駆動させる別の制御装置からロボット制御装置１０に送信される。または、他方の物体の設置装置２４はロボット制御装置１０によって駆動されるので、他方の物体２２の位置および／または姿勢は既知であるとする。または、他方の物体２２の位置および／または姿勢の移動量を取得するセンサによって、他方の物体２２の位置および／または姿勢は、ロボット制御装置１０に送信されるとする。
【０２６２】
ロボット５０が移動動作を停止し、かつ、他方の物体２２が空間に設定した座標系上で停止し、かつ、位置および／または姿勢が移動可能な機構部を有する、ロボットの設置装置５３によってロボットの台座５２が移動される場合、一方の物体２１は空間に設定した座標系上で移動し、他方の物体２２に対して移動することになる。また、一方の物体２１が空間に設定した座標系上で停止し、かつ、位置および／または姿勢が移動可能な機構部を有する、他方の物体の設置装置２４によって、他方の物体２２が移動する場合、他方の物体２２は空間に設定した座標系上で移動し、一方の物体２１に対して移動することになる。
【０２６３】
位置および／または姿勢が移動可能な機構部を有する、ロボットの設置装置５３が移動することによって、一方の物体２１を移動させる場合、または、位置および／または姿勢が移動可能な機構部を有する、他方の物体の設置装置２４が移動することによって、他方の物体２２を移動させる場合、他方の物体２２に対する一方の物体２１の移動の移動方向および移動量を補正して、一方の物体２１を他方の物体２２に移動させることによって、本発明の複数の実施例において、より安定的に素早く合わせ状態とすることができる。
【０２６４】
そのために、位置および／または姿勢が移動可能な機構部を有する、ロボットの設置装置５３が、ロボットの台座５２を移動させることによって一方の物体２１が移動する場合、ロボットの台座５２の移動によるロボット５０やロボット５０の手先部５１や一方の物体２１の移動方向および移動量に基づいて、または、位置および／または姿勢が移動可能な機構部を有する、他方の物体の設置装置２４の移動による他方の物体２２の移動方向および移動量に基づいて、ロボット５０、一方の物体２１、他方の物体２２の位置および／または姿勢を基準とした位置、姿勢、方向、移動量などを補正することが好ましい。
【０２６５】
ロボット５０、一方の物体２１、他方の物体２２の、位置および／または姿勢を基準とした情報は、他方の物体２２に対する所定の座標系、他方の物体２２に対する所定の方向、空間に対して設定した座標系上のロボット５０の移動によって移動する部分の位置および／または姿勢、一方の物体２１に対する所定の座標系、一方の物体２１に対する所定の方向などのように、一方の物体２１と他方の物体２２との相対的な移動に関する情報である。
【０２６６】
後述の本発明の実施形態も含めた、本発明の実施形態における、ロボット制御装置１０の力計測部３１、並進力制御方向設定部３２、並進力制御目標力設定部３３、回転力制御軸設定部３４、並進力制御方向目標移動量算出部３５、回転力制御軸回り目標移動量算出部３６、目標方向目標移動量算出部３７、動作指令生成部３８、接触位置設定部３９、接触部分姿勢算出部４０、合わせ状態姿勢算出部４１、合わせ状態判定部４２、後述の力不安定状態検出部４３、後述の移動動作不安定状態検出部４４は、ロボットの台座５２の移動によってロボット５０やロボット５０の手先部５１や一方の物体２１が移動する場合、または、他方の物体の設置装置２４によって他方の物体２２が移動する場合、ロボットの台座５２の移動によって移動する、一方の物体２１の位置および／または姿勢、移動方向、移動量、または、他方の物体の設置装置２４によって移動する、他方の物体２２の位置および／または姿勢、移動方向、移動量を考慮して、各機能の処理を実行することが好ましい。
【０２６７】
力計測部３１は、ロボットの台座５２の移動による一方の物体２１の移動を考慮して、重力や慣性力などの補償を実行する。また、一方の物体２１と他方の物体２２とが非接触状態のときに取得した力をもとに力を補償する場合、姿勢の変化を考慮させる。
【０２６８】
並進力制御方向設定部３２は、他方の物体２２に対する一方の物体２１の並進力制御方向が、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動がないとした場合と、相対的に同じ方向となるように、並進力制御方向を補正して設定する。または、並進力制御方向設定部３２は、並進力制御方向が一方の物体２１に対する所定の方向であるとき、ロボットの台座５２の移動による一方の物体２１の移動を考慮して並進力制御方向を補正する。または、並進力制御方向設定部３２は、並進力制御方向が他方の物体２２に対する所定の方向であるとき、他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して並進力制御方向を補正する。または、並進力制御方向設定部３２は、回転力制御軸が設定されていて、回転力制御軸に基づいて設定する場合、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して補正された回転力制御軸に基づいて並進力制御方向を補正する。
【０２６９】
回転力制御軸設定部３４は、他方の物体２２に対する回転力制御軸の位置および方向が、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動がないとした場合と、相対的に同じ位置および方向となるように、回転力制御軸を補正して設定する。または、回転力制御軸設定部３４は、回転力制御軸が一方の物体２１に対する所定の方向であるとき、ロボットの台座５２の移動による一方の物体２１の移動を考慮して回転力制御軸の位置および方向を補正する。または、回転力制御軸設定部３４は、回転力制御軸が他方の物体２２に対する所定の方向であるとき、他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して回転力制御軸の方向を補正する。または、回転力制御軸設定部３４は、並進力制御方向が設定されていて、並進力制御方向に基づいて設定する場合、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して補正された並進力制御方向に基づいて回転力制御軸を補正する。また、回転力制御軸設定部３４は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して回転力制御軸の位置を補正する。
【０２７０】
並進力制御方向目標移動量算出部３５は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、並進力制御方向目標移動量を算出する。
【０２７１】
回転力制御軸回り目標移動量算出部３６は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、回転力制御軸回り目標移動量を算出する。
【０２７２】
ロボット制御装置１０は、ロボットの設置装置５３によってロボットの台座５２が移動されて一方の物体２１の移動が継続する場合、ロボット制御装置１０の次の制御周期における、ロボットの設置装置５３の駆動装置がロボットの設置装置５３を移動させる目標移動量を取得して、ロボットの台座５２の移動量を算出し、算出されるロボットの台座５２の目標移動量をもとに、ロボットの台座５２の移動による一方の物体２１の移動量を算出する。または、ロボット制御装置１０は、ロボットの設置装置５３によってロボットの台座５２が移動され一方の物体２１の移動が継続する場合、ロボットの台座５２の移動量をもとに、ロボット制御装置１０の次の制御周期における、ロボットの台座５２の移動量を推定して算出し、推定したロボットの台座５２の移動量をもとに、ロボットの台座５２の移動による一方の物体２１の移動量を算出する。また、ロボット制御装置１０は、他方の物体の設置装置２４の移動によって他方の物体２２の移動が継続する場合、ロボット制御装置１０の次の制御周期における、他方の物体の設置装置２４の駆動装置が他方の物体の設置装置２４を移動させる目標移動量を取得して、他方の物体の設置装置２４の目標移動量をもとに、他方の物体２２の移動量を算出する。または、ロボット制御装置１０は、他方の物体の設置装置２４の移動によって他方の物体２２の移動が継続する場合、他方の物体２２の移動量をもとに、ロボット制御装置１０の次の制御周期における、他方の物体２２の移動量を推定して算出する。目標方向目標移動量算出部３７は、前述のように算出されたロボットの台座５２の移動による一方の物体２１の移動量および他方の物体の設置装置２４の移動による他方の物体２２の移動量と、補正された並進力制御方向目標移動量と、補正された回転力制御軸回り目標移動量とに基づいて、並進方向目標移動量と回転方向目標移動量を補正して算出する。
【０２７３】
動作指令生成部３８は、前述のように補正された並進方向目標移動量と、補正された回転方向目標移動量とに基づいて、ロボット５０の動作指令を生成する。
【０２７４】
接触位置設定部３９は、一方の物体２１と他方の物体２２とが接触するときの接触位置を設定するとき、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、一方の物体２１に対する接触位置が相対的に同じ位置となるように、または、他方の物体２２に対する接触位置が相対的に同じ位置となるように、補正して設定する。回転力制御軸設定部３４は、補正された接触位置と共に、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、接触位置基準の回転力制御軸を補正して設定する。回転力制御軸回り目標移動量算出部３６は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、接触位置基準の回転力制御軸などに基づいて、回転力制御軸回り目標移動量を算出する。
【０２７５】
接触部分姿勢算出部４０は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、一方の物体２１と他方の物体２２とを接触させる部分の姿勢を算出する。
【０２７６】
合わせ状態姿勢算出部４１は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、合わせ状態姿勢を算出する。
【０２７７】
合わせ状態判定部４２は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、合わせ状態の判定を実行する。
【０２７８】
後述の力不安定状態検出部４３は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、補正された力に基づいて、力の不安定状態を検出することが好ましい。
【０２７９】
後述の移動動作不安定状態検出部４４は、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を考慮して、ロボット５０が発振しているか否か、一方の物体２１と他方の物体２２との間に作用する力がロボット５０の移動動作の発振に起因するか否かなどを考慮して、ロボット５０の移動動作が発振した状態であるか否かを検出することが好ましい。
【０２８０】
なお、特に記述しない処理に関しても、ロボットの台座５２の移動による一方の物体２１の移動や他方の物体の設置装置２４の移動による他方の物体２２の移動を適宜、考慮した方がよい場合、適切な補正がなされることが好ましい。
【０２８１】
図７は、本発明の１１番目の実施形態に係るロボット制御装置１０ｆの構成を機能的に示す図である。本発明の１１番目の実施形態においては、本発明の１番目から１０番目のいずれかの実施形態のロボット制御装置１０に対して、さらに、力不安定状態検出部４３および移動動作不安定状態検出部４４を備えるロボット制御装置の構成としてもよい。本発明の１１番目の実施形態においては、図７において図示されるように、ロボット制御装置１０ａに対して、さらに、力不安定状態検出部４３および移動動作不安定状態検出部４４を備える。また、後述の機能も含めて図８に図示されるロボット制御装置１０ｇの構成としてもよい。
【０２８２】
本発明の１１番目の実施形態においては、ロボット制御装置１０は、本発明の１番目から１０番目の実施形態のロボット制御装置１０に対して、さらに、一方の物体２１と他方の物体２２との間に所定時間中に所定の閾値を超える力がかかる状態、または一方の物体２１と他方の物体２２との間に別の所定の閾値を超える力がかかる状態、または一方の物体２１と他方の物体２２との間に作用する力の発振状態、のうちの少なくとも１つの状態を検出することにより、一方の物体２１と他方の物体２２との間に作用する力の不安定状態を検出する力不安定状態検出部４３と、ロボット５０の特異姿勢の近傍状態、またはロボット５０の発振状態、のうちの少なくとも１つの状態を検出することにより、ロボット５０の移動動作の不安定状態を検出する移動動作不安定状態検出部４４と、を備え、力不安定状態検出部４３によって力の不安定状態が検出されたとき、または、移動動作不安定状態検出部４４によってロボット５０の移動動作の不安定状態が検出されたとき、ロボット制御装置１０は、回転力制御軸回り目標移動量を０にする、または、力の不安定状態またはロボットの移動動作の不安定状態が検出されない場合に算出される回転力制御軸回り目標移動量より小さい値となるように回転力制御軸回り目標移動量を調整して算出することが好ましい。
【０２８３】
本発明の１１番目の実施形態の説明においては、主に、本発明の１番目から１０番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０２８４】
力不安定状態検出部４３は、一方の物体２１と他方の物体２２との間に所定時間中に所定の閾値を超える力がかかる状態を検出する。これによって、力不安定状態検出部４３は、一方の物体２１と他方の物体２２との間に作用する力の変化が急に大きくなったことを検出し、ロボット５０が発振気味になっている状態、またロボット５０に対する指令の変化が急であり無理な動作指令が出ている状態、また一方の物体２１と他方の物体２２とを合わせ状態とするときに衝突または速度が大きい状態で接触した状態、また発振気味になって一方の物体２１と他方の物体２２とが衝突した状態、などの状態を検出することができる。前記所定時間や前記所定の閾値は、前記状態の検出が可能なように予め設定する、または一方の物体２１と他方の物体２２との接触状況やロボット５０の移動動作の状況によって自動的に設定されるようにしてもよい。
【０２８５】
また、力不安定状態検出部４３は、一方の物体２１と他方の物体２２との間に別の所定の閾値を超える力がかかる状態を検出する。これによって、力不安定状態検出部４３は、一方の物体２１と他方の物体２２との間に作用する力が過大になっている状態を検出し、他方の物体２２に対する一方の物体２１の移動、接触動作が適切ではない状態、また一方の物体２１と他方の物体２２との間に作用する力を適切に制御できていない状態を検出する。前記別の所定の閾値は、前記状態の検出が可能なように予め設定する、または一方の物体２１と他方の物体２２との接触状況やロボット５０の移動動作の状況によって自動的に設定されるようにしてもよい。
【０２８６】
また、力不安定状態検出部４３は、一方の物体２１と他方の物体２２との間に作用する力の発振状態を、力データの波形をもとに短時間フーリエ変換やウェーブレット変換によって周波数特性を分析することによって検出するなどの、既知の方法によって検出する。これによって、力不安定状態検出部４３は、一方の物体２１と他方の物体２２との接触状況が不安定になっている状態、またはロボット５０の移動動作が不安定になっている状態などを検出することができる。
【０２８７】
ここで、本実施例において、力不安定状態検出部４３が検出する前記のような状態を、力の不安定状態とする。
【０２８８】
移動動作不安定状態検出部４４は、ロボット５０の特異姿勢の近傍状態を検出する。ロボット５０の特異姿勢とは、ロボット５０の先端の直交座標系上の位置および／または姿勢を、各軸の位置に一意に逆変換することができない状態である。ロボット５０の特異姿勢の近傍状態とは、ロボット５０の姿勢が前記特異姿勢に近い状態とする。なお、この場合のロボットの姿勢とは、ロボットを構成するアクチュエータが或る状態であるときの、ロボットの機構部の姿勢とする。ロボット５０の姿勢が特異姿勢の近傍の状態であると、ロボット５０の移動動作は発振気味になったり、移動速度が大きくなったりして、不安定な状態になることがある。移動動作不安定状態検出部４４によって、ロボット５０の姿勢から、早めに、ロボット５０の移動動作が不安定となる状態を検出することができる。このとき、特異姿勢の近傍とする特異姿勢の周辺の領域は、ロボット５０の動作性能、または早期に特異姿勢の近くにあることを検出したい状況、または特異姿勢の近くと判定する領域を狭くしたい状況などに応じて適切に設定することが好ましい。
【０２８９】
また、移動動作不安定状態検出部４４は、ロボット５０の発振状態を検出する。このとき、移動動作不安定状態検出部４４は、ロボット５０への動作指令と実際の位置との乖離、ロボット５０の各軸の振動などの動作状況などに基づいて、ロボット５０の発振状態や初期的な発振状態を検出し、ロボット５０の移動動作が不安定になっている状態、不安定な動作の兆候がある状態を検出する。
【０２９０】
ここで、本実施例において、移動動作不安定状態検出部４４が検出する前記のような状況を、ロボットの移動動作の不安定状態とする。
【０２９１】
一方の物体２１と他方の物体２２とが合わせ状態となるように、一方の物体２１と他方の物体２２との間に作用する力に基づいて力制御を実行しているとき、回転力制御軸回りの力は、並進力制御方向の力と回転軸回りとの距離によって求まる値であるので、並進力制御方向の力の変動や接触位置の変動やロボット５０の移動動作の振動や力制御の振動などによって、ノイズが載ったり、大きく変動したり、振動したりする。また一方の物体２１の回転移動によって、一方の物体２１が他方の物体２２から離れたり、一方の物体２１と他方の物体２２とが接触と非接触を繰り返したりすることがある。このため、回転力制御軸回りの力に基づいて、他方の物体２２に対して一方の物体２１を回転移動させると、その移動動作に起因して、力の不安定状態やロボットの移動動作の不安定状態になることがある。
【０２９２】
力の不安定状態や、ロボットの移動動作の不安定状態となる場合、その時点における速度で回転移動を継続させると、回転力制御軸の軸回りの力はより振動的になり、回転力制御軸の軸回りの力に基づいて移動させることが困難となることがある。この場合、合わせ状態となるように移動させることが困難となり、合わせ状態にすることができなくなるか、または合わせ状態にするのに時間がかかってしまう。
【０２９３】
並進力制御方向については、並進力制御方向の力が所定の値の目標力となるように制御するので、回転力制御軸回りの力に基づいて移動させることと比較して、制御しやすい。そこで、力の不安定状態やロボットの移動動作の不安定状態を早めに検出して、回転移動を停止させるか、回転移動の速度を小さくして移動させて、並進力制御方向への制御を継続させる。力の不安定状態やロボットの移動動作不安定状態が力の不安定状態に起因している場合、一方の物体２１と他方の物体２２とが並進力制御方向に安定して接触するようになると、力の不安定状態やロボットの移動動作不安定状態が解消され、再び、回転移動の速度を大きくすることができるようになる。
【０２９４】
または、力の不安定状態やロボットの移動動作の不安定状態を早めに検出して、回転移動を停止させる、または、回転移動の速度を小さくして移動させるようにして回転移動の速度を調整すると共に、並進移動を停止させる、または、並進移動の速度を小さくして移動させるようにして並進移動の速度を調整してもよい。これによって、力の不安定状態やロボットの移動動作不安定状態が解消され、再び、回転移動、並進移動の速度を所定の値まで大きくすることができる、または、力の不安定状態やロボットの移動動作不安定状態にならない程度の、回転移動の速度、並進移動の速度に調整することができる。
【０２９５】
前述のように、他方の物体２２に対して、一方の物体２１を移動させるようにするため、ロボット制御装置１０は、力不安定状態検出部４３によって力の不安定状態が検出されたとき、または、移動動作不安定状態検出部４４によってロボット５０の移動動作の不安定状態が検出されたとき、回転力制御軸回り目標移動量を０にする。または、ロボット制御装置１０は、力不安定状態検出部４３によって力の不安定状態が検出されたとき、または、移動動作不安定状態検出部４４によってロボット５０の移動動作の不安定状態が検出されたとき、力の不安定状態またはロボット５０の移動動作の不安定状態が検出されない場合に算出される回転力制御軸回り目標移動量より小さい値となるように回転力制御軸回り目標移動量を調整して算出する。このとき、回転力制御軸回り目標移動量算出部３６または目標方向目標移動量算出部３７は、力不安定状態検出部４３および移動動作不安定状態検出部４４の出力に基づいて、力の不安定状態やロボットの移動動作の不安定状態が検出された場合、算出されている回転力制御軸回り目標移動量に対して１より小さい所定の係数をかけるようにして、小さくするように調整してもよい。
【０２９６】
前述のように、力の不安定状態やロボットの移動動作の不安定状態を検出して回転移動の速度を調整することによって、一方の物体２１と他方の物体２２とを、素早く、より短い時間で合わせ状態に収束させることができる。
【０２９７】
本発明の１２番目の実施形態においては、ロボット制御装置１０は、本発明の１番目から１１番目の実施形態のロボット制御装置１０に対して、さらに、並進力制御方向の力の大きさが所定閾値を超える場合、並進力制御方向の力の大きさを前記所定閾値として、回転力制御軸の軸回りの力の大きさが別の所定閾値を超える場合、回転力制御軸の軸回りの力の大きさを別の所定閾値とすることが好ましい。
【０２９８】
本発明の１２番目の実施形態の説明においては、主に、本発明の１番目から１１番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０２９９】
本発明の実施例において、一方の物体２１と他方の物体２２との間に作用する力が過度に大きい場合、その値をそのまま使用せず、適切な調整をした値に応じて、設定、算出、判定などの処理を適切に実行することが好ましい。一方の物体２１と他方の物体２２との間に作用する、並進力制御方向の力や回転力制御軸の軸回りの力が、所定の上限値より大きい場合、前記所定の上限値として算出などを実行することが好ましい。なお、所定の上限値は、方向に応じて、別の値を設定してもよい。
【０３００】
並進力制御方向目標移動量算出部３５は、並進力制御方向の力の大きさに応じて並進力制御方向目標移動量の大きさを変える場合、並進力制御方向の力の大きさが、その並進力制御方向に対して設定した所定の閾値より大きいとき、並進力制御方向の力の大きさを所定の閾値として、並進力制御方向目標移動量を算出することが好ましい。なお、並進力制御方向が２つ以上ある場合、所定の閾値を同じ値としたり、各並進力制御方向について所定の閾値を設定したりしてもよい。
【０３０１】
回転力制御軸回り目標移動量算出部３６は、回転力制御軸の軸回りの力の大きさに応じて回転力制御軸回り目標移動量の大きさを変える場合、回転力制御軸の軸回りの力の大きさが、その回転力制御軸に対して設定した所定の閾値より大きいとき、回転力制御軸の軸回りの力の大きさを所定の閾値として、回転力制御軸回り目標移動量を算出することが好ましい。なお、回転力制御軸が２つ以上ある場合、所定の閾値を同じ値としたり、各回転力制御軸について所定の閾値を設定したりしてもよい。
【０３０２】
所定の並進力制御方向における、一方の物体２１と他方の物体２２との間に作用する力が過度に大きいために、算出される並進力制御方向目標移動量が所定の閾値を超える場合、所定閾値の値を並進力制御方向目標移動量に設定してもよい。
【０３０３】
所定の回転力制御軸の軸回りにおける、一方の物体２１と他方の物体２２との間に作用する力が過度に大きいために、算出される回転力制御軸回り目標移動量が所定の閾値を超える場合、前記別の所定閾値の値を回転力制御軸回り目標移動量に設定してもよい。
【０３０４】
接触位置設定部３９、または接触部分姿勢算出部４０が、一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１と他方の物体２２とが接触するときの位置を算出する場合であって、一方の物体２１と他方の物体２２との間に作用する力が過度に大きいとき、そのときの一方の物体２１と他方の物体２２との間に作用する力に基づいて、接触位置を算出しないようにすることが好ましい。この場合、一方の物体２１と他方の物体２２との間に作用する力を、再度、取得する、または、接触状況が変わってから、取得する、または、異常状態を知らせるように教示装置や出力装置に表示するようにしてもよい。
【０３０５】
前述のようにすることによって、一方の物体２１と他方の物体２２との間に作用する力が過度に大きい場合に、ロボット５０の破損につながるような危険な移動をさせたり、ロボット５０を不適切に移動させて周囲の人や物に対して危険を及ぼしたり、一方の物体２１を不適切に移動させて破損させたり、一方の物体２１の不適切な移動によって一方の物体２１と他方の物体２２とを合わせ状態にするのに時間がかかるようになったりすることを防ぐことができる。
【０３０６】
本発明の１３番目の実施形態においては、ロボット制御装置１０における並進力制御方向設定部３２および回転力制御軸設定部３４は、本発明の１番目から１２番目の実施形態のロボット制御装置１０に対して、さらに、並進力制御方向設定部３２は、一方の物体２１に対する所定の方向、または、他方の物体２２に対する所定の方向、または、一方の物体２１の移動動作によらない所定の方向、または、回転力制御軸が設定されている場合、回転力制御軸、の内、少なくとも１つに基づいて、１つ以上の並進力制御方向を設定し、回転力制御軸設定部３４は、一方の物体２１に対する所定の方向、または、他方の物体２２に対する所定の方向、または、一方の物体２１の移動動作によらない所定の方向、または、並進力制御方向が設定されている場合、並進力制御方向、の内、少なくとも１つに基づいて、１つ以上の回転力制御軸を設定することが好ましい。
【０３０７】
本発明の１３番目の実施形態の説明においては、主に、本発明の１番目から１２番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０３０８】
並進力制御方向設定部３２の説明における、前述の説明のように、並進力制御方向は、一方の物体２１を他方の物体２２に対して相対的に並進移動させて押しつける方向であり、一方の物体２１と他方の物体２２との間に作用する並進方向の力が目標力となるように制御する方向である。並進力制御方向設定部３２は、一方の物体２１と他方の物体２２との間に作用する力を、その方向について制御することによって、一方の物体２１と他方の物体２２とが適切に接触するようになる方向に、並進力制御方向を設定することが好ましい。
【０３０９】
並進力制御方向設定部３２は、並進力制御方向を一方の物体２１に対する所定の方向に設定して、一方の物体２１の状況に応じて、並進力制御方向が変わるようにしてもよい。
【０３１０】
または、並進力制御方向設定部３２は、並進力制御方向を他方の物体２２に対する所定の方向に設定して、他方の物体２２に対する所定の方向に一方の物体２１が近づくようにしてもよい。この場合、他方の物体２２が移動する場合は、他方の物体２２の移動に応じて、一方の物体２１が他方の物体２２に対して近づくようにすることができる。
【０３１１】
または、並進力制御方向設定部３２は、並進力制御方向を一方の物体２１の移動動作によらない所定の方向に設定して、一方の物体２１の移動動作によらず、所定の方向に並進移動するようにして、一方の物体２１が他方の物体２２に近づくようにしてもよい。
【０３１２】
または、並進力制御方向設定部３２は、回転力制御軸が設定されている場合、並進力制御方向を回転力制御軸に基づいて、回転力制御軸と直交する方向と平行な方向や、回転力制御軸と所定の角度を成す方向とするなど、回転力制御軸の位置や方向に基づいて並進力制御方向を設定してもよい。これによって、回転力制御軸に応じて、並進力制御方向を設定することができる。
【０３１３】
または、並進力制御方向設定部３２は、前記の設定を組み合わせて、複数の並進力制御方向を設定したり、一方の物体２１と他方の物体２２の移動や接触の状況に応じて前記の設定から選択して並進力制御方向を設定したり、前記の方向を合わせた方向を並進力制御方向としたりしてもよい。
【０３１４】
前述のように並進力制御方向を設定することによって、一方の物体２１と他方の物体２２とを接触させたり、押しつけたりする方向を状況に応じて適切に設定し、一方の物体２１と他方の物体２２とを適切に接触させることが可能となる。
【０３１５】
回転力制御軸設定部３４の説明における、前述の説明のように、回転力制御軸は、一方の物体２１を他方の物体２２に対して相対的に回転移動させるときの回転軸である。回転力制御軸設定部３４は、一方の物体２１が他方の物体２２に対して相対的に、回転力制御軸の軸回りに回転移動し、所定の並進力制御方向に並進移動することによって、一方の物体２１と他方の物体２２とが適切に接触することができるように、回転力制御軸を設定することが好ましい。
【０３１６】
回転力制御軸設定部３４は、回転力制御軸の位置を設定するとき、一方の物体２１と他方の物体２２との移動状況や接触状況や移動方法によって、一方の物体２１に対して設定した制御点、または、一方の物体２１に対する所定のツール座標系上に設定された点、または、他方の物体２２に対する所定の座標系上に設定された点、または、一方の物体２１の移動動作によって位置および／または姿勢が変化しない空間に対して設定した座標系または基準座標系上に設定された点を通過するように設定する。
【０３１７】
回転力制御軸設定部３４は、回転力制御軸の方向を、一方の物体２１に対する所定の方向に基づいて設定する。これによって、一方の物体２１の移動状況に応じて、回転力制御軸の方向を変更させることができる。
【０３１８】
または、回転力制御軸設定部３４は、回転力制御軸の方向を、他方の物体２２に対する所定の方向に基づいて設定する。これによって、他方の物体２２の設置状況、または移動している場合は移動状況に応じて、回転力制御軸の方向を変更させることができる。
【０３１９】
または、回転力制御軸設定部３４は、回転力制御軸の方向を、一方の物体２１の移動動作によらない所定の方向に基づいて設定する。これによって、一方の物体２１の移動動作によらない方向に、回転力制御軸の方向を設定することができる。
【０３２０】
または、回転力制御軸設定部３４は、並進力制御方向が設定されている場合、回転力制御軸の方向を並進力制御方向に基づいて、並進力制御方向と直交とする方向と平行な方向や、並進力制御方向と所定の角度をなす方向と平行な方向とするなど、並進力制御方向に基づいて回転力制御軸の方向を設定してもよい。これによって、並進力制御方向に応じて、回転力制御軸の方向を設定することができる。
【０３２１】
または、回転力制御軸設定部３４は、前述のように説明した設定を組み合わせて、回転力制御軸の方向や、回転力制御軸の通過する点を設定して、複数の回転力制御軸を設定したり、一方の物体２１と他方の物体２２の移動や接触の状況に応じて前述のように説明した設定から選択して回転力制御軸の方向や、回転力制御軸の通過する点を設定したり、前記の方向を合わせた方向を回転力制御軸の方向としたりしてもよい。
【０３２２】
前述のように回転力制御軸を設定することによって、一方の物体２１と他方の物体２２とを相対的に回転移動させる方向を状況に応じて適切に設定し、一方の物体２１と他方の物体２２とを適切に接触させることが可能となる。
【０３２３】
本発明の１４番目の実施形態においては、ロボット制御装置１０における並進力制御方向設定部３２は、本発明の１番目から１３番目の実施形態のロボット制御装置１０に対して、さらに、一方の物体２１と他方の物体２２とが接触するとき、並進力制御方向の成分を含む方向、かつ、所定範囲を限度とする別の方向に、一方の物体２１を他方の物体２２に対して押しつけたときの、回転力制御軸の軸回りの力に基づいて、並進力制御方向を設定する。
【０３２４】
本発明の１４番目の実施形態の説明においては、主に、本発明の１番目から１３番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０３２５】
図２０は、本発明の１４番目の実施形態のロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にするときの、並進力制御方向の設定方法を説明する部分拡大図である。図２０（ａ）には、一方の物体２１と他方の物体２２とを合わせ状態に移動させるときの開始時の状態が示されており、一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１が設定され、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸に回転力制御軸６２が設定されている。または、一方の物体２１に対する所定の方向に設定された並進力制御方向６１としてもよい。
【０３２６】
図２０（ａ）に示されるように、並進力制御方向６１に一方の物体２１を並進移動させていくと、図２０（ｂ）に示されるように、一方の物体２１と他方の物体２２とは接触する。一方の物体２１と他方の物体２２とが接触したか否かは、並進力制御方向における一方の物体２１と他方の物体２２との間に作用する力と所定の閾値との比較によって判定する。または、回転力制御軸６２の軸回りの力と所定の閾値との比較や、回転力制御軸６２の軸回りの力の所定時間あたりの変化に基づいて、一方の物体２１と他方の物体２２とが接触したか否かを判定してもよい。
【０３２７】
一方の物体２１と他方の物体２２とが接触したとき、ロボット制御装置１０は、一方の物体２１の他方の物体２２に対する並進力制御方向の移動を停止させる。次に、一方の物体２１を回転力制御軸の軸回りに回転移動させずに、その時点の並進力制御方向の成分を含む方向であって、所定の範囲を限度とする、複数の別の方向に、一方の物体２１を押しつけるように移動させる。なお、別の方向とは、異なる方向を意味する。一方の物体２１を押しつける方向を別の方向に変えていくとき、回転力制御軸の軸回りの力の大きさに基づいて、回転力制御軸の軸回りの力が小さくなる方向にはしないようにする、または、回転力制御軸の軸回りの力が大きくなる方向にする。または、回転力制御軸の軸回りの力が最大となる方向を見つける、または回転力制御軸の軸回りの力が所定閾値を超える方向を見つけるなどの目的に応じて、押しつける方向を変える回数がなるべく少なくなるように方向を変えながら条件を満たす方向を探索させる。
【０３２８】
ここでは、前述のように、一方の物体２１を複数方向に、並進力制御方向の力が目標力となるように制御して押しつけるとき、複数の方向における回転力制御軸の軸回りの力を取得していき、回転力制御軸の軸回りの力が所定の閾値を超えたときの回転力制御軸の軸回りの力、および、方向を取得し、取得された方向を並進力制御方向として設定する。このとき、並進力制御方向は、取得された方向をもとに、一方の物体２１に対する所定の方向として設定して、一方の物体２１の移動に応じて変わるように設定する。以降、図２０（ｃ）に示されるように、このとき設定した方向を並進力制御方向６１として移動させる。なお、並進力制御方向を設定した次の制御周期では、並進力制御方向を設定したときに取得された回転力制御軸の軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させる。それ以降、回転力制御軸の軸回りの力が所定の閾値以下または０に近い値になるか、符号が変わるまで、前記の並進力制御方向を設定したときに取得された回転力制御軸の軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させてもよい。または、各時点での、回転力制御軸の軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させてもよい。
【０３２９】
または、次のように並進力制御方向を設定してもよい。一方の物体２１を複数方向に、並進力制御方向の力が目標力となるように制御して押しつけるとき、複数の方向における回転力制御軸の軸回りの力を取得していき、複数方向に変えたときの回転力制御軸の軸回りの力が最大であるときの方向を、並進力制御方向として設定する。なお、回転力制御軸の軸回りの力が最大となる方向は、所定の範囲内で方向を複数の方向に変えるときに最大となる方向とする。このとき、並進力制御方向は、取得された方向をもとに、一方の物体２１に対する所定の方向として設定して、一方の物体２１の移動に応じて変わるように設定する。なお、並進力制御方向を設定した次の制御周期では、回転力制御軸の軸回りの力が最大であったときの回転力制御軸の軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させる。それ以降、回転力制御軸の軸回りの力が所定の閾値以下または０に近い値になるか、符号が変わるまで、このときに取得された、回転力制御軸の軸回りの力が最大であったときの回転力制御軸の軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させてもよい。または、各時点での、回転力制御軸の軸回りの力に基づいて、回転力制御軸の軸回りに回転移動させてもよい。
【０３３０】
図２１は、本発明の１４番目の実施形態のロボット制御装置１０によって、一方の物体２１と他方の物体２２とを合わせ状態にするときの、並進力制御方向の別の設定方法を説明する部分拡大図である。図２０を参照しつつ説明した、並進力制御方向の設定方法では、一方の物体２１の回転移動を停止させて並進力制御方向を変えたが、並進力制御方向の別の設定方法では、一方の物体２１を回転移動させながら並進力制御方向を変え、回転力制御軸の軸回りの力が大きくなる方向を求めるようにして、並進力制御方向を設定する。
【０３３１】
図２１（ａ）には、一方の物体２１と他方の物体２２とが合わせ状態となるように移動させるときに、一方の物体２１と他方の物体２２とが最初に接触したときの状態が示されている。一方の物体２１の移動動作によらない所定の方向に並進力制御方向６１が設定され、一方の物体２１の移動動作によらない所定の方向かつ一方の物体２１の制御点を通る軸に回転力制御軸６２が設定されている。または、一方の物体２１に対する所定の方向を、並進力制御方向６１として設定してもよい。一方の物体２１と他方の物体２２との接触の判定方法は、前述の説明と同様である。
【０３３２】
このとき、図２０を参照して説明した方法とは異なり、回転力制御軸の軸回りの回転移動は停止させずに、並進力制御方向の押しつけと共に、回転力制御軸の軸回りの回転移動を実行する。
【０３３３】
図２１（ｂ）、図２１（ｃ）に示されるように、一方の物体２１と他方の物体２２とが合わせ状態となるように、一方の物体２１を他方の物体２２に対して移動させているとき、並進力制御方向の向きを変えながら、一方の物体２１を他方の物体２２に対して移動させる。なお、並進力制御方向の変更が完了するまでは、一方の物体２１の回転移動の速度を適切に小さく調整して移動させてもよい。このとき、一方の物体２１と他方の物体２２とが接触しているときの、回転力制御軸の軸回りの力を取得して、回転力制御軸の軸回りの力が所定の閾値を超える方向、または、複数の方向の中で最大となる方向を求める。並進力制御方向は、一方の物体２１に対する所定の方向として設定して、一方の物体２１の移動に応じて変わるように設定する。
【０３３４】
一方の物体２１を回転力制御軸の軸回りに回転移動させながら、並進力制御方向を変えるとき、その時点の並進力制御方向の成分を含む方向であって、所定の範囲を限度とする、複数の別の方向に、一方の物体２１を押しつけるように移動させる。一方の物体２１を押しつける方向を別の方向に変えていくとき、回転力制御軸の軸回りの力の大きさに基づいて、回転力制御軸の軸回りの力が小さくなる方向にはしないようにする、または、回転力制御軸の軸回りの力が大きくなる方向にする。または、回転力制御軸の軸回りの力が最大となる方向を見つける、または回転力制御軸の軸回りの力が所定閾値を超える方向を見つけるなどの目的に応じて、押しつける方向を変える回数がなるべく少なくなるように方向を変えながら条件を満たす方向を探索させる。
【０３３５】
一方の物体２１の並進力制御方向を変えながら移動させ、回転力制御軸の軸回りの力が、前回の方向のときよりも大きいとき、その並進力制御方向をもとに、一方の物体２１に対する所定の方向である、一方の物体２１との姿勢と並進力制御方向との関係を更新する。
【０３３６】
回転力制御軸の軸回りには、各時点での回転力制御軸の軸回りの力に基づいて、移動させる。一方の物体２１の並進力制御方向を変えながら移動させていき、回転力制御軸の軸回りの力が所定の閾値を超えたときの方向を最終的な並進力制御方向として、並進力制御方向の設定を完了する。
【０３３７】
または、次のように並進力制御方向を設定してもよい。一方の物体２１の並進力制御方向を変えながら移動させ、回転力制御軸の軸回りの力が、前回の方向のときよりも大きいとき、その並進力制御方向をもとに、一方の物体２１に対する所定の方向である、一方の物体２１との姿勢と並進力制御方向との関係を更新する。回転力制御軸の軸回りには、各時点での回転力制御軸の軸回りの力に基づいて、移動させる。一方の物体２１の並進力制御方向を変えながら移動させていき、所定の範囲の複数方向に変えたときの回転力制御軸の軸回りの力の大きさが最大であるときの方向を最終的な並進力制御方向として、並進力制御方向の設定を完了する。なお、方向は、回転力制御軸の軸回りの力が最大となる方向を短時間で見つけられるように変えていくことが好ましい。なお、回転力制御軸の軸回りの力が最大となる方向は、所定の範囲内で方向を複数の方向に変えるときに回転力制御軸の軸回りの力が最大となる方向とする。
【０３３８】
前述のように並進力制御方向設定部３２が並進力制御方向を設定することによって、予め設定した並進力制御方向が不適切であってもより適切な方向に変更されるので、一方の物体２１と他方の物体２２とをより素早く、確実に、合わせ状態とすることが可能となる。
【０３３９】
本発明の１５番目の実施形態においては、ロボット制御装置１０は、本発明の１番目から１４番目の実施形態のロボット制御装置１０に対して、さらに、回転力制御軸回り目標移動量算出部３６は、回転力制御軸が２つ以上ある場合、所定の回転力制御軸について、一方の物体２１と他方の物体２２との間に作用する力に基づいて、回転力制御軸の軸回りの力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ｂより小さいとき、回転方向の所定の移動量である所定の回転方向移動量Ｃを前記回転力制御軸回り目標移動量とし、回転力制御軸の軸回りの力が回転力制御軸回りの力に関する所定閾値である回転力制御軸回りの力の所定閾値Ｂ以上のとき、回転力制御軸の軸回りの力と軸回りの所定の目標力、または、回転方向の所定の移動量である所定の回転方向移動量Ｄおよび回転力制御軸の軸回りの力と軸回りの所定の目標力に基づいて、回転力制御軸回り目標移動量を算出することが好ましい。
【０３４０】
本発明の１５番目の実施形態の説明においては、主に、本発明の１番目から１４番目の実施形態におけるロボット制御装置１０と異なる点について説明する。特に述べない点に関しては、同様に実施されるものとする。
【０３４１】
本発明の１５番目の実施形態においては、本発明の１番目から１４番目の実施形態において、一方の物体２１の所定の部分と他方の物体２２の所定の部分とを接触して合わされた状態とするように、一方の物体２１を他方の物体２２に対して所定の回転力制御軸回りに回転移動させながら移動させる、かつ、前記合わせ状態とするための回転力制御軸とは異なる所定の回転力制御軸の軸回りに一方の物体２１を回転させるとき、一方の物体２１と他方の物体２２との間に作用する力に応じて前記合わせ状態とするための回転力制御軸とは異なる所定の回転力制御軸回りの目標移動量を算出する。
【０３４２】
一方の物体２１と他方の物体２２との間に作用する力に応じた前記目標移動量の算出は次のようにおこなう。所定の回転力制御軸の軸回りの力が所定閾値より小さいとき、回転方向の所定の移動量を回転力制御軸回り目標移動量とする。所定の回転力制御軸回りの力が所定閾値以上のとき、所定の回転力制御軸の軸回りの力と軸回りの所定の目標力とに基づいて、または、制御周期当たりの回転方向の所定の移動量と所定の回転力制御軸の軸回りの力と軸回りの所定の目標力とに基づいて、所定の回転力制御軸の軸回りの力が、所定の目標力となるように力制御をおこなう。このとき、例えば、（所定の目標力 − 所定の回転力制御軸の軸回りの力）／（所定の目標力）×（力制御ゲイン）、または、（所定の目標力 − 所定の回転力制御軸の軸回りの力）／（所定の目標力）×（回転方向の所定の移動量）、などのように算出した値を回転力制御軸回りの目標移動量とする。
【０３４３】
このように、一方の物体２１と他方の物体２２との間に作用する力に応じて、所定の回転力制御軸回りの目標移動量を算出することによって、一方の物体２１を所定の回転力制御軸の軸回りに回転させて一方の物体２１の前記所定の回転力制御軸回りの力が目標力となるように回転移動をさせながら、一方の物体２１の所定の部分と他方の物体２２の所定の部分とを接触して合わされた状態となるように一方の物体２１を他方の物体２２に対して移動させることが可能となる。
【０３４４】
これによって、例えば、一方の物体２１にキーが備えられ、他方の物体２２にキー溝が備えられている場合、一方の物体２１と他方の物体２２とを所定の部分が接触して合わされた状態にすると共に、一方の物体２１を回転力制御軸の軸回りの力に応じて前述のように制御し回転力制御軸の軸回りに回転させることによって、一方の物体２１のキーを他方の物体２２のキー溝に組みつけることができる。また、一方の物体２１と他方の物体２２とを組み付けるための凸凹形状がそれぞれに備えられている場合、一方の物体２１と他方の物体２２とを所定の部分が接触して合わされた状態にすると共に、一方の物体２１を回転力制御軸の軸回りの力に応じて前述のように制御し回転力制御軸の軸回りに回転させることによって、一方の物体２１と他方の物体２２の凸凹部分を組みつけることができる。また、一方の物体２１および他方の物体２２がギヤの場合などにおいて、一方の物体２１と他方の物体２２とを所定の部分が接触して合わされた状態にすると共に、一方の物体２１を回転力制御軸の軸回りの力に応じて前述のように制御し回転力制御軸の軸回りに回転させることによって、一方の物体２１と他方の物体２２とのギヤ部分を噛み合わせて回転させることができる。
【０３４５】
このように、本発明の１５番目の発明によって、一方の物体２１と他方の物体２２とを合わせ状態にすると共に、一方の物体２１と他方の物体２２の前記合わせ状態とする部分とは別の部分を適切な力で組みつけることが可能となる。
【０３４６】
以上のように、本発明の１番目から１５番目の実施形態のロボット制御装置１０について説明したが、複数の算出部や設定部によって、同一のパラメータを調整する場合、所定の値を限度として調整された値に対して調整したり、選択的に調整したり、重みをつけて調整して加算したり、最も調整量が大きい調整値を使用したりなどしてもよい。
【０３４７】
本発明の複数の実施例で説明したように並進力制御方向と回転力制御軸を設定し移動量を算出することによって、図９から図１２を参照して説明したような合わせ状態に、安定的に、また素早く、押付力が小さい場合であっても、移動させることが可能となる。
【０３４８】
図１３を参照して、本発明によって、一方の物体２１と他方の物体２２とを合わせ状態にするときの、別の例を説明する。図１３（ａ）、（ｂ）、（ｃ）、（ｄ）は、一方の物体２１と他方の物体２２とを合わせ状態にするときの、別の例を説明する図である。
【０３４９】
図１３（ａ）に示される一方の物体２１と他方の物体２２は、それぞれ２つの平面が所定の角度を成して組み合わされた形状を有する、同じ形状の物体とする。ここでは、一方の物体２１と他方の物体２２とを重ね合わせた状態を合わせ状態とする。また、空間または他方の物体２２に対して設定された３つの互いに直交する軸で構成される座標系と平行な、図１３（ａ）に示される軸６０ａ、軸６０ｂ、軸６０ｃで構成される座標系、かつ、座標系原点に一方の物体２１の制御点が存在する座標系（一方の物体２１に対する位置であり、前記座標系原点に一方の物体２１が存在しなくてもよい）を、前記一方の物体２１に対して設定しておく。つまり、この座標系は、一方の物体２１の移動により、原点位置は変わるが姿勢は変わらない。このとき、軸６０ｂ、軸６０ｃの方向の２つの方向を並進力制御方向６１とし、軸６０ａ、軸６０ｂ、軸６０ｃの３つの軸を回転力制御軸として、２つの並進力制御方向６１に押しつけると共に、軸６０ａ、軸６０ｂ、軸６０ｃの３つの回転力制御軸の軸回りの力がつりあった状態となるように、一方の物体２１を他方の物体２２に対して移動させることによって、一方の物体２１と他方の物体２２とを重ね合わせた、合わせ状態に移動させることができる。または、図１３（ａ）に示される状況において、一方の物体２１を他方の物体２２に重ね合わせる平面ごとに、２つの回転力制御軸を設定し、さらに、それらを一方の物体２１に対して設定して一方の物体２１の移動によらず一方の物体２１の姿勢と所定の位置関係が保たれるようにし、２つの並進力制御方向６１に移動させるときに、前記各平面の２つの回転力制御軸の軸回りの力がつりあうように、一方の物体２１を他方の物体２２に対して移動させることによっても、合わせ状態とすることができる。
【０３５０】
一方の物体２１と他方の物体２２とを合わせ状態にするときの、さらに別の例を説明する。図１３（ｂ）に示される一方の物体２１は直方体形状の物体であり、他方の物体２２は、３つの平面が直交するように組み合わされた形状を有する物体とする。ここでは、一方の物体２１の角部を他方の物体２２の隅に合わせた状態を合わせ状態とする。また、空間または他方の物体２２に対して設定された３つの互いに直交する軸で構成される座標系と平行な、図１３（ｂ）に示される軸６０ａ、軸６０ｂ、軸６０ｃで構成される座標系、かつ、座標系原点に一方の物体２１の制御点が存在する座標系を、前記一方の物体２１に対して設定しておく。つまり、この座標系は、一方の物体２１の移動により、原点位置は変わるが姿勢は変わらない。また、並進力制御方向および回転力制御軸は、３つの互いに直交する軸で構成される座標系における３つの軸をもとに設定する。このとき、軸６０ａ、軸６０ｂ、軸６０ｃの方向の３つの方向を並進力制御方向６１とし、軸６０ａ、軸６０ｂ、軸６０ｃの３つの軸を回転力制御軸として、３つの並進力制御方向６１に押しつけると共に、軸６０ａ、軸６０ｂ、軸６０ｃの３つの回転力制御軸の軸回りの力がつりあった状態となるように、一方の物体２１を他方の物体２２に対して移動させることによって、一方の物体２１の角部を他方の物体２２の隅に合わせた、合わせ状態にすることができる。
【０３５１】
一方の物体２１と他方の物体２２とを合わせ状態にするときの、さらに別の例を説明する。図１３（ｃ）に示される一方の物体２１は円筒形状の物体とする。図１３（ｃ）に示される他方の物体２２は、平面と、一方の物体２１の円筒形状の側面の曲面部分と重ね合わせることが可能な曲面部分を有する形状と、が組み合わされた物体とする。ここでは、一方の物体２１の円筒形状の底面を他方の物体２２の平面部分に合わせた状態、かつ、一方の物体２１の円筒形状の側面の曲面と他方の物体２２の前述の曲面とを重ね合わせた状態を合わせ状態とする。また、空間または他方の物体２２に対して設定された３つの互いに直交する軸で構成される座標系と平行な、図１３（ｃ）に示される軸６０ａ、軸６０ｂ、軸６０ｃで構成される座標系、かつ、座標系原点に一方の物体２１の制御点が存在する座標系を、前記一方の物体２１に対して設定しておく。つまり、この座標系は、一方の物体２１の移動により、原点位置は変わるが姿勢は変わらない。また、並進力制御方向および回転力制御軸は、３つの互いに直交する軸で構成される座標系における３つの軸をもとに設定する。このとき、軸６０ａ、軸６０ｂ、軸６０ｃの方向の３つの方向を並進力制御方向６１とする。この場合、軸６０ｂ、軸６０ｃの方向の並進力制御方向６１については０でない所定の大きさの並進力制御目標力で押しつける。軸６０ａの方向の並進力制御方向６１については、所定の大きさを０とした並進力制御目標力とし、受動的な力制御をおこなうようにさせる。また、軸６０ａ、軸６０ｂの２つの軸を回転力制御軸として、３つの並進力制御方向６１に押しつけると共に、軸６０ａ、軸６０ｂの軸回りの力がつりあった状態となるように、一方の物体２１を他方の物体２２に対して移動させることによって、一方の物体２１の円筒形状の底面を他方の物体２２の平面部分に合わせた状態にすると共に、一方の物体２１の円筒形状の側面の曲面と他方の物体２２の前述の曲面とを重ね合わせた状態である、合わせ状態にすることができる。
【０３５２】
一方の物体２１と他方の物体２２とを合わせ状態にするときの、さらに別の例を説明する。図１３（ｄ）に示される一方の物体２１は、図１３（ｃ）に示される円筒形状の物体の側面に、一方の物体２１の一部分２６のキーが取りつけられた形状の物体とする。図１３（ｄ）に示される他方の物体２２は、一方の物体２１の円筒形状の側面の曲面部分と重ね合わせることが可能な曲面部分を有し、かつ、一方の物体２１の側面の一部分２６のキーと合わせることが可能な、他方の物体２２の一部分２７のキー溝を有する形状と、平面とが組み合わされた形状の物体とする。ここでは、本発明の１５番目の実施形態におけるロボット制御装置１０を用いて、一方の物体２１と他方の物体２２とを相対的に移動させることによって、一方の物体２１の円筒形状の底面を、他方の物体２２の平面部分に合わせると共に、一方の物体２１の円筒形状の側面の曲面と、他方の物体２２の前述の曲面とを重ね合わせた合わせ状態にし、さらに、一方の物体２１の一部分２６のキーを、他方の物体２２の一部分２７のキー溝に合わせる。また、空間または他方の物体２２に対して設定された３つの互いに直交する軸で構成される座標系と平行な、図１３（ｄ）に示される軸６０ａ、軸６０ｂ、軸６０ｃで構成される座標系、かつ、座標系原点に一方の物体２１の制御点が存在する座標系を、前記一方の物体２１に対して設定しておく。つまり、この座標系は、一方の物体２１の移動により、原点位置は変わるが姿勢は変わらない。また、一方の物体２１を軸６０ｄの回りに回転させる図１３（ｄ）に示される軸６０ｄを、一方の物体２１に対して設定しておく。このとき、軸６０ａ、軸６０ｂ、軸６０ｃの方向の３つの方向を並進力制御方向６１とする。この場合、軸６０ｂ、軸６０ｃの方向の並進力制御方向６１については０でない所定の大きさの並進力制御目標力で押しつける。軸６０ａの方向の並進力制御方向６１については、所定の大きさを０とした並進力制御目標力とし、受動的な力制御をおこなうようにさせる。軸６０ａ、軸６０ｂの２つの軸を回転力制御軸として、３つの並進力制御方向６１に押しつけると共に、軸６０ａ、軸６０ｂの軸回りの力がつりあった状態となるようにする。さらに、軸６０ｄを回転力制御軸とする。このとき、軸６０ｄの回転軸については、軸６０ａ、軸６０ｂの軸回りについての回転移動のさせ方とは異なり、軸６０ｄの軸回りに回転移動させるときに、軸６０ｄの軸回りの力が所定の目標力となるように一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を回転移動させる。前述のように、一方の物体２１を他方の物体２２に対して移動させることによって、一方の物体２１の円筒形状の底面を、他方の物体２２の平面部分に合わせると共に、一方の物体２１の円筒形状の側面の曲面と、他方の物体２２の前述の曲面とを重ね合わせた合わせ状態にし、さらに、一方の物体２１の一部分２６のキーを、他方の物体２２の一部分２７のキー溝に合わせることができる。なお、前述の、空間または他方の物体２２に対して設定された３つの互いに直交する軸で構成される座標系と平行な、図１３（ｄ）に示される軸６０ａ、軸６０ｂ、軸６０ｃで構成される座標系、かつ、座標系原点に一方の物体２１の制御点が存在する座標系を、前記一方の物体２１に対して設定した、座標系の軸６０ｃ（一方の物体２１の移動によって姿勢が変わらない）を回転力制御軸として、前述の軸６０ｄに置き換え、その軸回りの力が所定の目標力となるように一方の物体２１と他方の物体２２との間に作用する力に基づいて、一方の物体２１を回転移動させるようにしてもよい。
【０３５３】
以上のように、本発明によって、一方の物体２１と他方の物体２２とは、一方の物体２１の所定の部分と他方の物体２２の所定の部分とが接触して合わされた状態にすることが可能であれば、どのような形状を有する物体の組み合わせであってもよく、そのような一方の物体２１と他方の物体２２とを、安定的に、また素早く、押付力が小さい場合であっても、合わせ状態に移動させることが可能となる。
【０３５４】
説明のために選定された特定の実施形態を参照して本発明が説明されたが、当業者には本発明の基本的概念及び範囲から逸脱することなく多数の変更が可能であることは明らかである。すなわち、本発明の技術的思想の範囲内で考えられる他の形態についても、本発明の範囲内に含まれる。
【符号の説明】
【０３５５】
１０、１０ａ−１０ｇ    ロボット制御装置
１１    ロボットシステム
２１、２１ａ    一方の物体
２２    他方の物体
２３    保持部
２４    他方の物体の設置装置
２５    力センサ
２６    キー
２７    キー溝
３１    力計測部
３２    並進力制御方向設定部
３３    並進力制御目標力設定部
３４    回転力制御軸設定部
３５    並進力制御方向目標移動量算出部
３６    回転力制御軸回り目標移動量算出部
３７    目標方向目標移動量算出部
３８    動作指令生成部
３９    接触位置設定部
４０    接触部分姿勢算出部
４１    合わせ状態姿勢算出部
４２    合わせ状態判定部
４３    力不安定状態検出部
４４    移動動作不安定状態検出部
５０    ロボット
５１    手先部
５２    ロボットの台座
５３    ロボットの設置装置
６０ａ−６０ｄ    軸
６１    並進力制御方向
６２    回転力制御軸
６３    回転方向
６４    並進力制御方向
６５、６５ａ−６５ｅ    接触点
６６    回転方向
６７    接触位置基準の回転力制御軸
【要約】
【課題】ロボットによって、一方の物体と他方の物体とを、一方の物体の所定の部分と他方の物体の所定の部分とが接触して合わされた状態である、合わせ状態となるように、より安定的により素早く、一方の物体と他方の物体とを相対的に移動させるロボット制御装置の提供。
【解決手段】ロボット制御装置１０ａは、物体２１と物体２２との間に作用する力を計測する力計測部３１と、並進力制御方向を設定する並進力制御方向設定部３２と、並進力制御目標力を設定する並進力制御目標力設定部３３と、回転力制御軸を設定する回転力制御軸設定部３４と、並進力制御方向目標移動量を算出する並進力制御方向目標移動量算出部３５と、回転力制御軸回り目標移動量を算出する回転力制御軸回り目標移動量算出部３６と、回転方向目標移動量を算出する目標方向目標移動量算出部３７と、ロボットの動作指令を生成する動作指令生成部３８とを備える。
【選択図】図１【図１】
0006046218.tif 000002
【図２】
0006046218.tif 000003
【図３】
0006046218.tif 000004
【図４】
0006046218.tif 000005
【図５】
0006046218.tif 000006
【図６】
0006046218.tif 000007
【図７】
0006046218.tif 000008
【図８】
0006046218.tif 000009
【図９】
0006046218.tif 000010
【図１０】
0006046218.tif 000011
【図１１】
0006046218.tif 000012
【図１２】
0006046218.tif 000013
【図１３】
0006046218.tif 000014
【図１４】
0006046218.tif 000015
【図１５】
0006046218.tif 000016
【図１６】
0006046218.tif 000017
【図１７】
0006046218.tif 000018
【図１８】
0006046218.tif 000019
【図１９】
0006046218.tif 000020
【図２０】
0006046218.tif 000021
【図２１】
0006046218.tif 000022