特開 2022-113567 制御装置、位置推定方法及びプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】公開特許公報(A)

(11)【公開番号】P2022113567

(43)【公開日】2022-08-04

(54)【発明の名称】制御装置、位置推定方法及びプログラム

(51)【国際特許分類】

   B25J 19/06 20060101AFI20220728BHJP

【ＦＩ】

B25J19/06

【審査請求】未請求

【請求項の数】10

【出願形態】ＯＬ

(21)【出願番号】P 2021009887

(22)【出願日】2021-01-25

(71)【出願人】

【識別番号】000006208

【氏名又は名称】三菱重工業株式会社

(74)【代理人】

【識別番号】100149548

【弁理士】

【氏名又は名称】松沼 泰史

(74)【代理人】

【識別番号】100162868

【弁理士】

【氏名又は名称】伊藤 英輔

(74)【代理人】

【識別番号】100161702

【弁理士】

【氏名又は名称】橋本 宏之

(74)【代理人】

【識別番号】100189348

【弁理士】

【氏名又は名称】古都 智

(74)【代理人】

【識別番号】100196689

【弁理士】

【氏名又は名称】鎌田 康一郎

(72)【発明者】

【氏名】井川 友博

(72)【発明者】

【氏名】▲徳▼山 享大

(72)【発明者】

【氏名】村田 直史

(72)【発明者】

【氏名】光畑 幸史

(72)【発明者】

【氏名】久川 恭平

【テーマコード（参考）】

3C707

【Ｆターム（参考）】

3C707AS06

3C707AS12

3C707BS10

3C707HS27

3C707KS12

3C707KS20

3C707KS31

3C707KX05

3C707LS15

3C707LW05

3C707MS07

(57)【要約】

【課題】マニピュレータの動作中に障害物と接触した際に、障害物位置を推定する方法を提供する。
【解決手段】制御装置は、マニピュレータのアームが障害物へ接触したことを検知する接触検知部と、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する位置推定部と、を備える。
【選択図】図１

【特許請求の範囲】

【請求項1】

マニピュレータのアームが障害物へ接触したことを検知する接触検知部と、
前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する位置推定部と、
を備える制御装置。

【請求項2】

前記位置推定部は、前記アームが備える関節の動作状況に応じて、前記代表点を設定する、
請求項１に記載の制御装置。

【請求項3】

前記接触が検知されたリンクに接続された関節のうち、先端側の前記関節を動かしているときに前記障害物との接触が検知された場合、
前記位置推定部は、前記リンクの先端側の前記関節を前記代表点として設定する、
請求項２に記載の制御装置。

【請求項4】

前記接触が検知されたリンクに接続された関節のうち、基端側の前記関節だけを動かしているときに前記障害物との接触が検知された場合、
前記位置推定部は、前記リンクの基端側の前記関節を前記代表点として設定する、
請求項２または請求項３に記載の制御装置。

【請求項5】

前記接触検知部は、前記アームを構成するリンクの中の前記障害物への接触が検知された前記リンクのうち、最も先端側の前記リンクを前記障害物へ接触したリンクであると特定する、
請求項１から請求項４の何れか１項に記載の制御装置。

【請求項6】

前記代表点から前記障害物までの距離に前記アームを構成するリンクの半径の長さを設定する、
請求項１から請求項５の何れか１項に記載の制御装置。

【請求項7】

前記位置推定部は、推定した前記障害物の位置を記憶部に記録する、
請求項１から請求項６の何れか１項に記載の制御装置。

【請求項8】

前記記憶部に記録された前記障害物の位置に基づいて、前記障害物の形状を推定する形状推定部、
をさらに備える請求項７に記載の制御装置。

【請求項9】

マニピュレータのアームが障害物へ接触したことを検知し、
前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する、
位置推定方法。

【請求項10】

コンピュータに、
マニピュレータのアームが障害物へ接触したことを検知し、
前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する処理、
を実行させるプログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、制御装置、位置推定方法及びプログラムに関する。

【背景技術】

【0002】

マニピュレータの制御では、手先の位置と姿勢が所望の目標軌道に追従するように、各関節の角度及び角速度を制御している。マニピュレータが物と接触した場合、障害物によりマニピュレータの駆動が妨げられるため、作業を継続しようとすると、モータやマニピュレータ本体、または障害物が損傷する可能性がある。特許文献１には、マニピュレータのリンク（関節と関節の間の部位）の外乱トルクを推定し、推定した外乱トルクからマニピュレータと障害物との接触を検知して、元の位置へ戻したり、停止させたりすることによって、衝突緩和を行う制御が開示されている。マニピュレータが障害物に接触した場合、単に衝突緩和を行うだけではなく、それ以外の動作（例えば、障害物を回避して目的位置へ到達する等）を柔軟に選択できるような制御方法が求められている。なお、非特許文献１には、マニピュレータと障害物との接触を検知する方法、接触があったリンクを特定する方法が開示されている。

【先行技術文献】

【特許文献】

【0003】

【特許文献1】特開２００６－１６７８２０号公報

【非特許文献】

【0004】

【非特許文献1】A. D. Luca et al, “Collision Detection and Safe Reaction with the DLR-III Lightweight Manipulator Arm”, IEEE/RSJ International Conference on Intelligent Robots and Systems, 2006.

【発明の概要】

【発明が解決しようとする課題】

【0005】

マニピュレータが障害物に接触した後に、その後の動作を様々に選択できるようにするためには、障害物の位置を把握する必要がある。

【0006】

本開示は、上記課題を解決することができる制御装置、位置推定方法及びプログラムを提供する。

【課題を解決するための手段】

【0007】

本開示の一実施形態によれば、制御装置は、マニピュレータのアームが障害物へ接触したことを検知する接触検知部と、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する位置推定部と、を備える。

【0008】

本開示の一実施形態によれば、位置推定方法は、マニピュレータのアームが障害物へ接触したことを検知するステップと、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定するステップと、を有する。

【0009】

本開示の一実施形態によれば、プログラムは、コンピュータにマニピュレータのアームが障害物へ接触したことを検知し、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する処理を実行させる。

【発明の効果】

【0010】

上述の制御装置、位置推定方法及びプログラムによれば、障害物の位置を推定することができる。

【図面の簡単な説明】

【0011】

【図1】実施形態に係るマニピュレータの一例を示す図である。

【図2】第一実施形態に係る障害物接触時の制御の流れを示すフローチャートである。

【図3】第一実施形態に係る障害物位置の推定処理のフローチャートである。

【図4】第一実施形態に係る障害物位置の推定処理の一例を説明する第１の図である。

【図5】第一実施形態に係る障害物位置の推定処理の一例を説明する第２の図である。

【図6】第一実施形態の係る障害物位置の推定結果の一例を説明する図である。

【図7】第二実施形態の係る障害物の形状を推定する処理について説明する図である。

【図8】実施形態の制御装置のハードウェア構成の一例を示す図である。

【発明を実施するための形態】

【0012】

＜実施形態＞
以下、本開示のマニピュレータについて、図１～図８を参照しながら説明する。

【0013】

（構成）
図１は、実施形態に係るマニピュレータの一例を示す図である。
図１に示すマニピュレータ１００は、制御装置１０とアーム２０とを含む。アーム２０は、リンク２０１Ａ～２０１Ｄと関節２０２Ａ～２０２Ｃを備える。リンク２０２Ｄの基端部は、移動体などの土台となる装置３０に固定されている。リンク２０１Ｄの先端部は関節２０２Ｃを介してリンク２０１Ｃの基端部に接続される。リンク２０１Ｃの先端部は関節２０２Ｂを介してリンク２０１Ｂの基端部と接続される。リンク２０１Ｂの先端部は関節２０２Ａを介してリンク２０１Ａの基端部と接続される。リンク２０１Ａの先端部には、図示しないエンドエフェクタ等の作業用ツールが取り付けられる。リンク２０１Ａ～２０１Ｄを総称してリンク２０１、関節２０２Ａ～２０２Ｃを総称して関節２０２と記載する場合がある。アーム２０は、異なる数のリンク、異なる数の関節を備えていてもよい。関節２０２には、モータ２０３と角度センサ２０４が内蔵されている。モータ２０３の回転によって関節１０２に接続されるリンク２０１の姿勢が制御される。角度センサ２０４によって、関節２０２の角度や角速度が計測される。制御装置１０は、角度センサ２０４が計測する関節２０２の角度や角速度を監視しながら、モータ２０３を制御し、アーム２０の姿勢を制御する。制御装置１０は、アーム２０（例えばリンク２０１Ｄ）の内部に設けられていてもよいし、アーム２０とは別に設けられていてもよい。なお、アーム２０の外部には、障害物などとの接触を検知するセンサ類が設けられていない。

【0014】

制御装置１０は、センサ情報取得部１１と、アーム制御部１２と、接触検知部１３と、障害物位置推定部１４と、記憶部１５と、障害物形状推定部１６と、を備える。
センサ情報取得部１１は、角度センサ２０４が検出した角度や角速度を所定の時間間隔で継続的に取得する。
アーム制御部１２は、アーム２０先端の目標位置やアーム２０の目標姿勢などについての指示情報を受け付けて、アーム２０の動作を制御する。例えば、アーム制御部１２は、アーム２０の目標軌道、各関節２０２の目標角度などを算出し、センサ情報取得部１１が取得する角度や角速度が目標値となるようにモータ２０３を制御する。アーム制御部１２が関節２０２の制御を行う方法は公知の任意の制御方法であってよい。

【0015】

接触検知部１３は、リンク２０１の障害物への接触を検知する。接触検知部１３は、リンク２０１に生じる外乱トルクを計算し、その値が所定の閾値（例えば０、あるいは０とみなせる値）以上であれば、リンク２０１が障害物へ接触したと判定する。この判定によって、障害物への接触が検知される。リンク２０１に生じる外乱トルクの計算方法や障害物との接触検知の方法については、特許文献１や非特許文献１に記載がある。例えば、接触検知部１３は、これら文献に記載されたものと同様の方法によって、障害物への接触を検知する。非特許文献１に記載があるように、接触検知部１３は、リンク２０１ごとに障害物への接触を検知する。

【0016】

障害物位置推定部１４は、障害物の位置を推定し、推定した位置の情報を記憶部１５に記録する。障害物位置の推定方法については後述する。
記憶部１５は、アーム２０の制御に必要な情報を記憶する。また、記憶部１５は、障害物位置推定部１４が推定した障害物位置の情報を記憶する。
障害物形状推定部１６は、記憶部１５が記憶する障害物位置の情報に基づいて、障害物の形状や範囲を算出し、障害物の形状や障害物が存在する範囲を示す情報を出力する。

【0017】

＜第一実施形態＞
第一実施形態は、障害物位置の推定処理に関する。図２～図６を用いて、マニピュレータ１００の障害物の位置推定処理について説明する。

【0018】

（動作）
図２は、第一実施形態に係る障害物接触時の制御の流れを示すフローチャートである。
アーム制御部１２が指示情報に基づいてアーム２０の動作制御を行う間、並行して、制御装置１０は、センサ情報取得部１１が取得する角度センサ２０４の値を監視して、以下の処理を継続的に繰り返し行う。
接触検知部１３が、各リンク２０１Ａ～２０１Ｄに発生する外乱トルクを推定する（ステップＳ１１）。例えば、接触検知部１３は、角度センサ２０４が計測した値に基づいて、特許文献１や非特許文献１に記載の方法で、各リンク２０１に生じる外乱トルクを推定する。次に接触検知部１３は、外乱トルクの値が閾値以上か否かによって接触判定を行う（ステップＳ１２）。閾値とは、例えば、０とみなせる値である。接触検知部１３は、外乱トルクが閾値以上の場合は接触が生じたと判定し、外乱トルクが閾値未満の場合は接触が生じていないと判定する。接触検知部１３は、リンク２０１Ａ～２０１Ｄの各々について、この接触判定を行う。全てのリンク２０１にて接触が生じていないと判定した場合（ステップＳ１２；Ｎｏ）、本処理フローを終了する。

【0019】

何れかのリンクで接触が生じたと判定した場合（ステップＳ１２；Ｙｅｓ）、接触検知部１３は、接触リンクを特定する（ステップＳ１３）。接触リンクとは、障害物と接触したと特定するリンク２０１のことである。例えば、リンク２０１Ｂで障害物との接触が発生すると、多くの場合、リンク２０１Ｂ、２０１Ｃ、２０１Ｄで閾値以上の外乱トルクが検知される。接触検知部１３は、閾値以上の外乱トルクが検出されたリンク２０１のうち、先端側のリンクを接触トルクとして特定する。この例の場合、接触検知部１３は、リンク２０１Ｂを接触リンクとして特定する。つまり、複数のリンクで外乱トルクが閾値を超えた場合、接触検知部１３は、最も先端側のリンクを接触リンクと特定する。

【0020】

次に障害物位置推定部１４が、障害物位置を推定する（ステップＳ１４）。障害物位置推定部１４は、接触が検知された時間にアーム２０が移動していた方向に障害物があるとして、図３に示す手順で障害物を検知する。図３は、第一実施形態に係る障害物位置の推定処理のフローチャートである。まず、障害物位置推定部１４は、代表点を設定する（ステップＳ２１）。代表点とは、ステップＳ１３で特定した接触リンク２０１に接続された関節２０２である。関節２０２が両端にある場合、障害物位置推定部１４は、その何れかを代表点として特定する。両端の関節２０２のうちの何れを代表点とするかについては、後に図４、図５を用いて説明する。次に障害物位置推定部１４は、代表点の移動方向を算出する（ステップＳ２２）。障害物位置推定部１４は、接触検知時のアーム２０の関節角又は関節角速度から、障害物に接触しているリンク２０１(接触リンク)の代表点の移動方向を計算する。次に障害物位置推定部１４は、ステップＳ２２で算出した移動方向上に存在する１点を、障害物位置として推定する（ステップＳ２３）。

【0021】

ここで、図３のフローチャートで示した処理について具体例を挙げて説明する。
図４は、第一実施形態に係る障害物位置の推定処理の一例を説明する第１の図である。
図４に、接触リンクの先端側（手先側）の関節２０２を代表点とし、その代表点の移動方向に障害物位置を推定する方法を示す。図４は、アーム２０全体の関節２０２を動かしながら手先を矢印ＹＡ１が示す斜め左下向きに移動させているときに、リンク２０１Ｂに障害物４００が接触した状況を示している。このとき、ステップＳ１３により、リンク２０１Ｂが接触リンクとして特定される。全ての関節２０２を動かしてアーム２０を移動している間に、あるいは、接触リンクから見てアーム２０の先端側の関節２０２を動かしている間に、接触検知部１３が、障害物との接触を検知した場合、障害物位置推定部１４は、特定された接触リンクの先端側の関節２０２を代表点として設定する（ステップＳ２１）。図４において、接触リンクはリンク２０１Ｂであるから、障害物位置推定部１４は、関節２０２Ａを代表点として設定する。

【0022】

次に障害物位置推定部１４は、代表点の移動方向を次のようにして算出する（ステップＳ２２）。アーム２０の関節角度ｑ、関節角速度ｑ・（微分を表すドット）とする。ここで、関節角度ｑ、関節角速度ｑ・は、アーム２０の全ての関節２０２の角度、角速度の和のベクトルである。すると、代表点の位置は、以下の式（１）で表すことができる。
代表点の位置：ｘ_ｋ＝ｆ_ｋ（ｑ） ・・・（１）
ここで、ｆ_ｋ（ｑ）は代表点の位置の幾何式である。
代表点の速度は、以下の式（２）で表すことができる。

【0023】

【数1】

【0024】

ここでＪは、非特許文献１の「III.DETECTION AND REACTION WITH RIGID ROBOTS」に開示があるヤコビ行列である。
障害物位置推定部１４は、以下の式（３）から代表点の移動方向を算出する。

【0025】

【数2】

【0026】

次に障害物位置推定部１４は、次のようにして障害物位置を推定する（ステップＳ２３）。障害物位置推定部１４は、ステップＳ２２の式（３）で算出した移動方向に基づいて、以下の式（４）により、障害物位置μを推定する。なお、ｄは、接触リンクから障害物４００までの距離（最短距離）を示す（図４のｄ）。距離dには、例えば、アーム２０のリンク２０１Ｂの半径Ｒ（図４のＲ１÷２）を設定する。なお、リンク半径Ｒを距離dに設定することは一例であって、距離dには他の値を設定してもよい。

【0027】

【数3】

【0028】

全ての関節２０２の角度センサ２０４により計測された角度や角速度を式（２）に代入し、また、リンク半径Ｒを式（４）のｄに代入することにより、障害物位置μを算出することができる。算出された障害物位置の位置を、図４の障害物位置μ０に示す。障害物位置推定部１４は、所定のリンク半径Ｒ、接触検知時にセンサ情報取得部１１が取得した各関節２０２の角度や角速度から代表点の位置ｘ_ｋや代表点の移動方向（式（３））を算出し、式（４）によって、障害物位置μ０を算出する。障害物位置μ０は、例えば、３次元の位置情報である。

【0029】

次に図５を参照する。図５は、第一実施形態に係る障害物位置の推定処理の一例を説明する第２の図である。
図５に、接触リンクの基端側（ベース側）の関節２０２の移動方向に障害物位置を推定する方法を示す。図５は、アーム２０の手首関節２０２Ａを動かすことなく、肘関節２０２Ｂを矢印ＹＡ２の方向へ移動させているときに障害物４０１に接触した状況を示している。このとき、ステップＳ１３により、リンク２０１Ｂが接触リンクとして特定される。手首関節２０２Ａが固定されているにもかかわらず、肘関節２０２Ｂを動かす動作を行う場合、手首関節２０２Ａの移動方向に障害物位置を推定するのが適切でない場合がある。そこで、アーム２０の動きに応じて、代表点を変更することで、より正確な障害物位置推定を行う。例えば、接触リンクから見て先端側の関節２０２を動かさずに、基端側の関節２０２を動かしている間に、接触検知部１３が、障害物４０１との接触を検知した場合、障害物位置推定部１４は、特定された接触リンクの基端側の関節２０２を代表点として設定する（ステップＳ２１）。図５において、接触リンクはリンク２０１Ｂであるから、障害物位置推定部１４は、関節２０２Ｂを代表点として設定する。

【0030】

次に障害物位置推定部１４は、上記の式（３）によって代表点の移動方向を算出する（ステップＳ２２）。次に障害物位置推定部１４は、上記の式（４）によって障害物位置μ１を推定する（ステップＳ２３）。

【0031】

図４、図５の説明では、障害物４００、４０１と接触が検知されたときに、接触リンクの先端側の関節２０２Ａが動いている場合は接触リンクの先端側の関節２０２Ａを代表点とし、接触リンクの先端側の関節２０２Ａが動いておらず基端側の関節２０２Ｂが動いている場合は接触リンクの基端側の関節２０２Ｂを代表点とすると説明したが、代表点の決定方法はこの方法に限らない。状況によっては、接触リンクの先端側の関節２０２Ａが動いている場合であっても、基端側を代表点として設定してもよいし、接触リンクの先端側の関節２０２Ａが動いていない場合でも、先端側の関節２０２Ａを代表点として設定してもよい。例えば、接触リンクの先端側の関節２０２Ａを代表点として障害物位置μを推定する処理と、接触リンクの基端側の関節２０２Ｂを代表点として障害物位置μを推定する処理の両方を行い、複数回の障害物位置推定処理の結果、より確からしい方向に推定された障害物位置μを障害物位置であるとして選択し、その場合の代表点を以降の同じ接触リンクに対する接触検知の際の代表点として定めてもよい。

【0032】

図２に戻る。図３の処理により、障害物位置μを推定すると、障害物位置推定部１４は、推定した障害物位置μを記憶部１５に記録する（ステップＳ１５）。推定した障害物位置μを図示した例を図６に示す。図６の線３００やその他の同様の線は、アーム２０を様々に動作させている間のアーム２０の姿勢の履歴を示している。また、障害物位置μ２～μ５は、アーム２０の姿勢を様々に変化させている間に障害物４０２との接触が検知された際に、ステップＳ１４の処理によって推定された障害物位置を示す。障害物位置推定部１４は、アーム２０の動作中に推定した複数の障害物位置μ２～μ５を記憶部１５に記録する。

【0033】

また、障害物位置推定部１４は、障害物位置μを記憶部１５に記録するとともに、障害物位置μをアーム制御部１２へ出力する。アーム制御部１２は、障害物位置μに基づいて、接触後の処理を実行する（ステップＳ１６）。例えば、アーム制御部１２は、アーム２０を停止したり、アーム２０を障害物との接触が生じない元の位置へ戻したりといった衝突緩和の制御を実行してもよい。さらに障害物位置μが把握できると、アーム２０により積極的な動作を行わせることができる。例えば、アーム制御部１２は、障害物位置μに基づいて、障害物を回避しつつ、アーム２０の先端を目標位置へ近づけるような動作を行ってもよい。その際のアーム２０の制御には、目標位置へ到達させる方法、障害物を回避させる方法にそれぞれ公知の技術を用いても良い。例えば、障害物位置μが判明している場合に障害物を回避させる方法の一例として、ポテンシャル法が知られている。アーム制御部１２は、ポテンシャル法を適用して、障害物を回避するようアーム２０を制御してもよい。また、目標位置へ到達させる方法についても、アーム制御部１２は、マニピュレータを目標位置へ到達させる公知の技術を用いて、アーム２０の制御を行ってもよい。障害物位置μの推定結果を取得することにより、アーム制御部１２は、障害物接触後のアーム２０の動作を選択することができる。

【0034】

以上、図２～図６を用いて説明したように、第一実施形態によれば、接触リンク上に設定した代表点の位置、速度を基に、障害物の位置を推定することができる。障害物位置を認識することで、障害物に対する公知の回避技術を適用でき、接触後に巻き戻しや停止以外の動作をアーム２０に行わせることができる。これにより、従来技術であれば、障害物に接触した場合、停止などの衝突緩和の後にアーム２０の軌道を調整する等の手間が必要となるところ、本実施形態によれば、障害物を回避しつつ目標動作を行わせることを自動化することができるので、マニピュレータ１００を用いた作業効率が向上する。また、アーム２０の動作状況（手首関節２０２Ａを動かすか否か等）に応じて代表点の設定を切り換えることで、障害物位置をより正確に推定することができる。また、本実施形態によれば、マニピュレータ１００に、接触検知や障害物位置推定用の外界センサ（接触センサ、カメラ等）を設置しなくても障害物との接触を検知し、障害物位置を推定することができる。外界センサが不要なので、狭隘部にてアーム２０を動作させるような場合でもセンサ類によって動作空間を圧迫し、狭めることが無く、また、センサ類の故障や破損などを気にする必要が無い。例えば、本実施形態の制御方法は、カメラを設置できない環境でのマニピュレータの制御に適用することができる。また、外界センサを必要としないことで、コストや重量等を低減することができる。

【0035】

＜第二実施形態＞
第二実施形態は、第一実施形態の方法で推定した障害物位置に基づいて、障害物の形状や範囲を推定する処理に関する。図７を参照して、第二実施形態の説明を行う。

【0036】

図７は、第二実施形態の係る障害物の形状を推定する処理について説明する図である。
図２のステップＳ１５で説明したように、アーム２０の運転を継続すると、記憶部１５には、障害物位置μが複数（例えば、障害物位置μ２～μ５）記録される。障害物形状推定部１６は、複数の障害物位置μから障害物４０２の形状や範囲を推定してその推定結果を出力する。図７の左図７０１は、記憶部１５に記録された障害物位置μ２～μ５をその位置情報に基づいてプロットした図である。障害物形状推定部１６は、障害物位置μ２～μ５に基づいて、障害物の形状を推定する。例えば、障害物形状推定部１６は、障害物位置μ２～μ５をすべて内包し、その体積が最小となる直方体や円柱などを算出し、算出した直方体の形状を障害物の形状、算出した直方体が占める空間を障害物が存在する範囲であると推定する。また、障害物形状推定部１６は、推定した直方体等とその位置関係を表示装置や電子ファイル等に出力する。障害物形状推定部１６による障害物形状の推定結果およびその出力例を図７の右図７０２に示す。右図７０２の障害物ｕ６は、障害物形状推定部１６によって推定された障害物の形状や範囲を示している。推定結果を表示することで、障害物との実際の接触履歴に基づく障害物形状とマニピュレータ１００との位置関係を容易に把握することができる。また、障害物形状推定部１６が推定した障害物範囲の位置情報をアーム制御部１２へ渡すことにより、アーム制御部１２は、効率的に障害物を回避する制御が可能になる。

【0037】

第二実施形態では、障害物位置推定部１４が推定した複数の障害物位置μのそれぞれが、障害物全体のある部分（点）であると考え、これらを統合して、障害物全体の形状を推定する。これにより、これまで障害物位置μとして推定されていない位置についても、その位置が障害物の一部であるかどうかを判断することができる。これにより、障害物の一部である可能性がある位置を予測して回避しつつ、アーム２０を動作させることができるので、目標位置に向けて、スムーズにアーム２０を動作させることができる。

【0038】

なお、アーム２０の制御を効率化するために、作業開始時にアーム２０を様々な方向（例えば、全方向に亘って網羅的に）に動作させて意図的に障害物と接触させ、障害物位置推定部１４によって障害物位置推定およびその位置情報の記録を行い、さらに障害物形状推定部１６によって障害物の形状を推定させる処理を行うようにしてもよい。作業の最初の段階で、障害物の形状や範囲を把握することにより、それ以降のマニピュレータ１００の制御を効率化することができる。

【0039】

図８は、実施形態の制御装置のハードウェア構成の一例を示す図である。
コンピュータ９００は、ＣＰＵ９０１、主記憶装置９０２、補助記憶装置９０３、入出力インタフェース９０４、通信インタフェース９０５を備える。
上述の制御装置１０は、コンピュータ９００に実装される。そして、上述した各機能（センサ情報取得部１１、アーム制御部１２、接触検知部１３、障害物位置推定部１４、μ１６）は、プログラムの形式で補助記憶装置９０３に記憶されている。ＣＰＵ９０１は、プログラムを補助記憶装置９０３から読み出して主記憶装置９０２に展開し、当該プログラムに従って上記処理を実行する。また、ＣＰＵ９０１は、プログラムに従って、記憶領域を主記憶装置９０２に確保する。また、ＣＰＵ９０１は、プログラムに従って、処理中のデータを記憶する記憶領域を補助記憶装置９０３に確保する。

【0040】

なお、制御装置１０の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各機能部による処理を行ってもよい。ここでいう「コンピュータシステム」とは、ＯＳや周辺機器等のハードウェアを含むものとする。また、「コンピュータシステム」は、ＷＷＷシステムを利用している場合であれば、ホームページ提供環境（あるいは表示環境）も含むものとする。また、「コンピュータ読み取り可能な記録媒体」とは、ＣＤ、ＤＶＤ、ＵＳＢ等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また、このプログラムが通信回線によってコンピュータ９００に配信される場合、配信を受けたコンピュータ９００が当該プログラムを主記憶装置９０２に展開し、上記処理を実行しても良い。また、上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであってもよい。

【0041】

以上のとおり、本開示に係るいくつかの実施形態を説明したが、これら全ての実施形態は、例として提示したものであり、発明の範囲を限定することを意図していない。これらの実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で種々の省略、置き換え、変更を行うことができる。これらの実施形態及びその変形は、発明の範囲や要旨に含まれると同様に、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

【0042】

＜付記＞
各実施形態に記載の制御装置１０、位置推定方法及びプログラムは、例えば以下のように把握される。

【0043】

（１）第１の態様に係る制御装置１０は、マニピュレータ１００のアーム２０が障害物４００、４０１等へ接触したことを検知する接触検知部１３と、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向（式（３））と、前記代表点から前記障害物までの距離ｄと、に基づいて前記障害物（μ０～μ５）の位置を推定する障害物位置推定部１４と、を備える。
これにより、マニピュレータ１００の外界を検出するセンサを設けなくても、障害物との接触を検知し、その位置を推定することができる。障害物の位置を推定することができるので、その位置情報を用いて、障害物との接触検知後のマニピュレータ１００の動作を任意に制御することができる。

【0044】

（２）第２の態様に係る制御装置１０は、（１）の制御装置１０であって、前記障害物位置推定部１４は、前記アーム２０が備える関節２０２の動作状況に応じて、前記代表点を設定する。
これにより、アーム２０の動かし方に応じて、代表点を設定することにより、正確に障害物位置を推定することができる。

【0045】

（３）第３の態様に係る制御装置１０は、（２）の制御装置１０であって、前記接触が検知されたリンク（図４の２０１Ｂ）に接続された関節２０２のうち、先端側の関節２０２Ａを動かしているときに前記障害物４００との接触が検知された場合、前記障害物位置推定部１４は、前記リンクの先端側の関節２０２Ａを前記代表点として設定する。
本開示の第一実施形態では、接触リンクの移動方向に障害物があると推定するが、例えば、接触リンクの先端側の関節２０２を動かしているときに障害物と接触した場合（接触リンクの先端側の関節のさらに先のリンクでは外乱トルクが検知されていない）、接触リンクの先端側の関節の移動方向が基端側の移動方向よりも障害物位置に関係が深い（より適切に障害物位置を表す）と仮定し、接触リンクの先端側の関節を代表点として設定する。これにより、障害物位置を適切に推定する。

【0046】

（４）第４の態様に係る制御装置１０は、（２）～（３）の制御装置１０であって、前記接触が検知されたリンク（図５の２０１Ｂ）に接続された関節２０２のうち、基端側の関節２０２Ｂだけを動かしているときに前記障害物４０１との接触が検知された場合、前記障害物位置推定部１４は、前記リンクの基端側の関節２０２Ｂを前記代表点として設定する。
本開示の第一実施形態では、接触リンクの移動方向に障害物があると推定するが、例えば、接触リンクの基端側の関節２０２のみを動かしているときに障害物と接触した場合、接触リンクの基端側の関節の移動によって接触が生じたとすると、接触リンクの基端側の移動方向が先端側の移動方向よりも障害物位置に関係が深いと考え、接触リンクの基端側の関節を代表点として設定する。これにより、障害物位置を適切に推定する。

【0047】

（５）第５の態様に係る制御装置１０は、（１）～（４）の制御装置１０であって、前記接触検知部は、前記アームを構成するリンクの中の前記障害物への接触が検知された前記リンクのうち最も先端側の前記リンクを前記障害物へ接触したリンクであると特定する。
アームが障害物と接触すると、接触があったリンクと、それよりも基端側（ベース側）のリンクにて外乱トルクが観測される。従って、外乱トルクが観測されたリンクのうち、最も先端側のリンクを接触リンクと特定することで、実際に接触があったリンクを誤りなく特定できる確率が高まり、これにより、障害物位置の推定精度を確保することができる。

【0048】

（６）第６の態様に係る制御装置１０は、（１）～（５）の制御装置１０であって、前記代表点から前記障害物までの距離に前記アームを構成するリンクの半径の長さを設定する。
代表点から障害物までの距離は不明であるところ、大きく違わない値としてリンク半径を距離に設定することで障害物位置の推定が可能になる。

【0049】

（７）第７の態様に係る制御装置１０は、（１）～（６）の制御装置１０であって、前記位置推定部は、推定した前記障害物の位置を記憶部に記録する。
障害物位置を記録することで、障害物全体の形状や範囲の推定を行うための基礎データを蓄積することができる。

【0050】

（８）第８の態様に係る制御装置１０は、（７）の制御装置１０であって、前記記憶部に記録された前記障害物の位置に基づいて、前記障害物の形状を推定する形状推定部、をさらに備える。
障害物の形状を推定することで、障害物の回避が可能になる。これにより、マニピュレータ１００による作業効率を向上することができる。

【0051】

（９）第９の態様に係る位置推定方法は、マニピュレータのアームが障害物へ接触したことを検知するステップと、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定するステップと、を有する。

【0052】

（１０）第１０の態様に係るプログラムは、コンピュータ９００に、マニピュレータのアームが障害物へ接触したことを検知し、前記アームのうちの前記接触が検知されたリンク上に設定された代表点の移動方向と、前記代表点から前記障害物までの距離と、に基づいて前記障害物の位置を推定する処理を実行させる。

【符号の説明】

【0053】

１０・・・制御装置
１１・・・センサ情報取得部
１２・・・アーム制御部
１３・・・接触検知部
１４・・・障害物位置推定部
１５・・・記憶部
１６・・・障害物形状推定部
２０・・・アーム
１００・・・マニピュレータ
２０１、２０１Ａ、２０１Ｂ、２０１Ｃ、２０１Ｄ・・・リンク
２０２、２０２Ａ、２０２Ｂ、２０２Ｃ・・・関節
２０３・・・モータ
２０４・・・角度センサ
９００・・・コンピュータ
９０１・・・ＣＰＵ
９０２・・・主記憶装置
９０３・・・補助記憶装置
９０４・・・入出力インタフェース
９０５・・・通信インタフェース

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】