JPWO2020157862A1 - 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム - Google Patents

Abstract

障害物回避制御装置が、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める回避指令値演算部と、前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御する機器制御部と、を備える。

Description

本発明は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体に関する。

ロボット等の制御対象機器に障害物を回避させるように制御する技術が提案されている。例えば、特許文献１に記載のロボット制御装置は、ロボットのアームの先端部の目標位置と、先端部が目標位置に到達する前の位置である第１位置との間に障害物が存在する場合、障害物を避ける凸状の軌跡を設定する。アームの先端部は、その軌跡に従って移動する。

日本国特開２０１８−１６７３３３号公報

ロボットが障害物に接触する可能性がある箇所は、アームの先端部に限らない。ロボットアームの先端部に限らず、制御対象機器のいろいろな部分について、障害物を回避できることが好ましい。

本発明の目的の一例は、上記の問題を解決することができる障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体を提供することである。

本発明の第１の態様によれば、障害物回避制御装置は、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める回避指令値演算部と、前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御する機器制御部と、を備える。

本発明の第２の態様によれば、障害物回避制御方法は、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、を含む。

本発明の第３の態様によれば、記録媒体は、コンピュータに、制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、を実行させるためのプログラムを記録した記録媒体である。

上記した障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体によれば、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。

第１実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第１実施形態に係る情報取得装置の機能構成の例を示す概略ブロック図である。 第１実施形態に係る障害物回避制御装置の機能構成の例を示す概略ブロック図である。 第１実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第１実施形態に係る障害物回避制御装置が制御対象機器に対する制御指令値を取得する処理手順の例を示すフローチャートである。 第１実施形態に係る障害物回避制御装置が干渉関数に関する値を算出する処理手順の例を示すフローチャートである。 第１実施形態に係る障害物回避制御装置が干渉関数値の変化量ΔＢ（ｘ，ｕ）を算出する処理手順の例を示すフローチャートである。 第２実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第２実施形態に係る障害物位置特定装置の機能構成の例を示す概略ブロック図である。 第２実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第３実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第３実施形態に係る障害物動作予測装置の機能構成の例を示す概略ブロック図である。 第３実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第４実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第４実施形態に係る障害物回避制御装置の機能構成の例を示す概略ブロック図である。 第４実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第４実施形態に係る障害物回避制御装置が制御対象機器に対する制御指令値を取得する処理手順の例を示すフローチャートである。 第５実施形態に係る障害物回避制御装置の構成の例を示す図である。 第６実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。 少なくとも１つの実施形態に係るコンピュータの構成を示す概略ブロック図である。

以下、本発明の実施形態を説明するが、以下の実施形態は請求の範囲にかかる発明を限定するものではない。また、実施形態の中で説明されている特徴の組み合わせの全てが発明の解決手段に必須であるとは限らない。

＜第１実施形態＞
図１は、第１実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図１に示す構成で、障害物回避制御システム１は、撮像装置１００と、情報取得装置２００と、ノミナル（Nominal）制御装置３００と、障害物回避制御装置４００とを備える。

障害物回避制御システム１は、制御対象機器９００を制御する。障害物回避制御システム１は、制御対象機器９００に所望の動作をさせ、かつ、制御対象機器９００が障害物に接触しないように、制御対象機器９００を制御する。
ここでいう所望の動作は、制御対象機器９００に設定されている目標を達成する動作である。ここでいう接触することは、単に触れることに限定されず、ぶつかることを含む。制御対象機器９００が障害物に接触することは、制御対象機器９００の少なくとも一部分が、障害物の少なくとも一部分に接触することである。

以下では、制御対象機器９００が垂直多関節ロボットである場合を例に説明するが、障害物回避制御システム１の制御対象は、制御指令値に従って動作し、障害物に接触する可能性があるいろいろな機器とすることができる。例えば、制御対象機器９００が、垂直多関節ロボット以外の産業用ロボットであってもよい。

あるいは、制御対象機器９００が、建築用ロボットまたは家事ロボットなど、産業用ロボット以外のロボットであってもよい。特定の用途に限定されず、形状変化を伴ういろいろなロボットを、制御対象機器９００の例とすることができる。
あるいは、制御対象機器９００が、無人搬送車またはドローンなどの移動体であってもよい。また、制御対象機器９００が、制御指令値にて制御可能であれば、自律的に動作する装置であってもよい。

ここでいう障害物は、制御対象機器９００が接触する可能性がある物である。障害物は、特定の種類のものに限定されない。例えば、障害物が、人間、他のロボット、周囲の壁または機械、一時的に置かれた荷物の何れか、あるいはこれらの組み合わせであってもよい。
また、制御対象機器９００自らも障害物として扱われていてもよい。例えば、制御対象機器９００が垂直多関節ロボットであり、その姿勢によってはロボットアームと台座部分とが接触する場合、障害物回避制御システム１が制御対象機器９００を障害物として扱うことで、ロボットアームと台座部分との接触を回避し得る。

撮像装置１００は、障害物を撮像する。例えば、撮像装置１００は、制御対象機器９００が動作可能な空間全体を撮像範囲内に含むように撮像を行うことで、障害物がある場合にその障害物を撮像する。
撮像装置１００の撮像画像に基づいて、情報取得装置２００が障害物の位置を特定する。撮像装置１００として、例えばデプスカメラ（Depth Camera）、ステレオカメラ（Stereo Camera）など、３次元の情報を得られるカメラを用いるようにしてもよい。あるいは、情報取得装置２００が、ＯｐｅｎＰｏｓｅの３次元拡張など、２次元の画像から３次元の情報を得るための技術を用いるようにしてもよい。

あるいは、障害物回避制御システム１が、撮像装置１００に加えて、あるいは代えて、３Ｄ−ＬｉＤＡＲ（3-Dimension Light Detection and Ranging）など、３次元情報を得るための装置を備えるようにしてもよい。
また、撮像装置１００が複数台のカメラで構成されていてもよい。撮像装置１００が移動可能になっていてもよい。
撮像装置１００が、観測対象の空間の状況をボクセルデータで示すようにしてもよい。

情報取得装置２００は、制御対象機器９００が備えるセンサなど制御対象機器９００を観測するセンサからセンシングデータを取得して、制御対象機器９００の位置および動作を検出する。情報取得装置２００がセンシングデータを取得するセンサは、特定の種類のセンサに限定されない。例えば、情報取得装置２００が、センシングデータから、制御対象機器９００の各関節の、関節角度、関節角速度、関節速度、および、関節加速度のうち何れか、あるいはこれらの組み合わせの情報を取得するようにしてもよい。

情報取得装置２００は、得られた情報に基づいて、制御対象機器９００の位置情報、および、制御対象機器９００の動きを示す情報を生成し送信する。
情報取得装置２００が、制御対象機器９００の位置情報をボクセルデータで送信するようにしてもよい。例えば、情報取得装置２００が制御対象機器９００の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置４００は、１点ではなく制御対象機器９００の表面と、障害物との位置関係を把握でき、制御対象機器９００と障害物との距離をより正確に把握できる。制御対象機器９００と障害物との距離をより正確に把握できることで、障害物回避制御装置４００は、制御対象機器９００に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置２００が、制御対象機器９００の位置情報として、制御対象機器９００に設定されている代表点の座標を送信するようにしてもよい。

情報取得装置２００は、制御対象機器９００の動きを示す情報として、例えば、制御対象機器９００の速度、加速度、角速度、角加速度、またはこれらの組み合わせを送信する。情報取得装置２００が、制御対象機器９００の表面の動きを示す情報をボクセルデータで送信するようにしてもよい。あるいは、情報取得装置２００が、制御対象機器９００の代表点の動きを示すデータを送信するようにしてもよい。例えば、情報取得装置２００が、制御対象機器９００の一般化座標ｑと、一般化速度ｑ’とを並べたベクトルを送信するようにしてもよい。
あるいは、情報取得装置２００が、制御対象機器の関節の角速度など、制御対象機器のアクチュエータの動きを示す情報を送信するようにしてもよい。
制御対象機器９００の位置情報と制御対象機器９００の動きを示す情報とを総称して制御対象機器９００の状態情報と称する。
情報取得装置２００は、制御対象機器の状態情報を、ノミナル制御装置３００および障害物回避制御装置４００へ送信する。

情報取得装置２００は、障害物の位置情報を送信する。情報取得装置２００が、障害物の位置情報をボクセルデータのデータ形式で送信するようにしてもよい。例えば、情報取得装置２００が障害物の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置４００は、１点ではなく障害物の表面と、制御対象機器９００との位置関係を把握でき、制御対象機器９００と障害物との距離をより正確に把握できる。制御対象機器９００と障害物との距離をより正確に把握できることで、障害物回避制御装置４００は、制御対象機器９００に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置２００が、制御対象機器９００の位置情報として、制御対象機器９００に設定されている代表点の座標を送信するようにしてもよい。

障害物が動く場合、情報取得装置２００が、障害物の位置情報に加えて、障害物の動きを示す情報を送信するようにしてもよい。情報取得装置２００が、障害物の動きを示す情報として、例えば、障害物の速度、加速度、角速度、角加速度、またはこれらの組み合わせを送信するようにしてもよい。情報取得装置２００が、障害物の表面の動きを示す情報をボクセルデータで送信するようにしてもよい。あるいは、情報取得装置２００が、障害物の代表点の動きを示すデータを送信するようにしてもよい。例えば、情報取得装置２００が、障害物の一般化座標ｑと、一般化速度ｑ’とを並べたベクトルを送信するようにしてもよい。
障害物の位置情報、あるいは、障害物が動く場合の、障害物の位置情報と障害物の動きを示す情報との組み合わせを、障害物の状態情報と称する。
情報取得装置２００は、障害物の状態情報を障害物回避制御装置４００へ送信する。

ノミナル制御装置３００は、ノミナル指令値を算出する。ノミナル指令値は、制御対象機器９００による障害物回避を考慮しない場合の、制御対象機器９００に対する制御指令値である。すなわち、ノミナル指令値は、障害物が無いとの仮定の下で、制御対象機器９００に設定された目標を達成するための、制御対象機器９００に対する制御指令値である。

ノミナル制御装置３００がノミナル指令値を算出するための制御方法は特定のものに限定されず、公知のいろいろな制御方法を用いることができる。
ノミナル制御装置３００が算出するノミナル指令値は、障害物回避制御装置４００が、制御対象機器９００に対して指示する制御指令値（すなわち、実際に用いられる制御指令値）を取得するための基準となる制御指令値として用いられる。

障害物回避制御装置４００は、障害物回避制御システム１による制御対象機器９００の制御を実行する。したがって、障害物回避制御システム１について上述したように、障害物回避制御装置４００は、制御対象機器９００に所望の動作をさせ、かつ、制御対象機器９００が障害物に接触しないように、制御対象機器９００を制御する。
障害物回避制御装置４００は、制御対象機器９００を制御するための制御指令値を取得する。障害物回避制御装置４００は、取得した制御指令値を制御対象機器９００へ送信することで、制御対象機器９００を制御する。

制御対象機器９００と障害物との距離が所定の閾値よりも離れている場合、障害物回避制御装置４００は、ノミナル制御装置３００が算出した制御指令値を制御対象機器９００への制御指令値として用いる。ノミナル制御装置３００が算出した、障害物回避を考慮していない制御指令値を用いても、制御対象機器９００が障害物に接触しないと期待されるからである。ノミナル制御装置３００が算出した制御閾値を用いることで、障害物回避制御装置４００は、障害物回避を考慮した制御閾値を新たに算出する必要が無く、この点で障害物回避制御装置４００の負荷が軽くて済む。

一方、制御対象機器９００と障害物との距離が所定の閾値未満である場合。ノミナル制御装置３００が算出した制御指令値では、制御対象機器９００が障害物に接触する可能性が比較的高い。そこで、障害物回避制御装置４００は、制御対象機器９００が障害物を回避し、かつ、目標を達成するための制御指令値を算出し、算出した制御指令値を制御対象機器９００へ送信する。

図２は、情報取得装置２００の機能構成の例を示す概略ブロック図である。図２に示す構成で、情報取得装置２００は、第１通信部２１０と、第１記憶部２８０と、第１制御部２９０とを備える。第１制御部２９０は、状態観測部２９１と、三次元計測部２９２とを備える。
第１通信部２１０は、他の装置と通信を行う。特に、第１通信部２１０は、制御対象機器９００が備えるセンサからのセンシングデータを受信する。また、第１通信部２１０は、撮像装置１００からのデータを受信する。第１通信部２１０は、撮像装置１００の撮像画像の画像データを受信し、さらに、奥行き方向の距離情報など付加情報がある場合は、画像データに加えて付加情報を受信する。

また、第１通信部２１０は、情報取得装置２００が生成する制御対象機器９００の位置情報および動作情報をノミナル制御装置３００および障害物回避制御装置４００へ送信する。また、第１通信部２１０は、情報取得装置２００が生成する障害物の位置情報を障害物回避制御装置４００へ送信する。情報取得装置２００が障害物の動作も特定する場合、第１通信部２１０は、障害物の動作情報（例えば、速度または加速度の情報）を障害物回避制御装置４００へ送信する。

第１記憶部２８０は、各種データを記憶する。第１記憶部２８０の機能は、情報取得装置２００が備える記憶デバイスを用いて実行される。
第１制御部２９０は、情報取得装置２００の各部を制御して各種処理を実行する。第１制御部２９０の機能は、情報取得装置２００が備えるＣＰＵ（Central Processing Unit、中央処理装置）が、第１記憶部２８０からプログラムを読み出して実行することで実行される。

状態観測部２９１は、制御対象機器９００が備えるセンサからのセンシングデータに基づいて、制御対象機器９００の位置および動作を特定する。状態観測部２９１は、特定した制御対象機器９００の位置を示す座標値を算出する。状態観測部２９１は、制御対象機器９００の位置および動作を示すデータを、第１通信部２１０を介してノミナル制御装置３００および障害物回避制御装置４００へ送信する。すなわち、状態観測部２９１は、制御対象機器９００の状態情報を、第１通信部２１０を介してノミナル制御装置３００および障害物回避制御装置４００へ送信する。

三次元計測部２９２は、撮像装置１００からのデータに基づいて、障害物の位置を特定する。三次元計測部２９２は、特定した位置を示す座標値を算出し、算出したデータを、第１通信部２１０を介して障害物回避制御装置４００へ送信する。また、障害物が動く場合、三次元計測部２９２が、障害物の動作情報（例えば、速度または加速度の情報）を算出し、算出したデータを、第１通信部２１０を介して障害物回避制御装置４００へ送信するようにしてもよい。このように、三次元計測部２９２は、障害物の状態情報を、第１通信部２１０を介して障害物回避制御装置４００へ送信する

図３は、障害物回避制御装置４００の機能構成の例を示す概略ブロック図である。図３に示す構成で、障害物回避制御装置４００は、第２通信部４１０と、第２記憶部４８０と、第２制御部４９０とを備える。第２記憶部４８０は、構造情報記憶部４８１と、動的モデル記憶部４８２とを備える。第２制御部４９０は、干渉関数演算部４９１と、機器制御部４９２とを備える。機器制御部４９２は、ノミナル指令値取得部４９３と、回避要否判定部４９４と、回避指令値演算部４９５と、仮想力場反映部４９６とを備える。

第２通信部４１０は、他の装置と通信を行う。特に、第２通信部４１０は、情報取得装置２００が送信する、制御対象機器９００の状態情報および障害物の状態情報を受信する。また、第２通信部４１０は、ノミナル制御装置３００が算出して送信する制御指令値を受信する。また、第２通信部４１０は、制御対象機器９００へ制御指令値を送信する。障害物回避制御装置４００は、制御対象機器９００へ制御指令値を送信することで制御対象機器９００を制御する。

第２記憶部４８０は、各種データを記憶する。第２記憶部４８０の機能は、障害物回避制御装置４００が備える記憶デバイスを用いて実行される。
構造情報記憶部４８１は、制御対象機器９００の構造情報を記憶する。制御対象機器９００の構造情報は、制御対象機器９００のアームの長さおよび太さ、関節が動く方向および可動範囲など、制御対象機器９００の形状および動作に関する情報である。制御対象機器９００のＣＡＤ（Computer Aided Design）データがある場合、そのＣＡＤデータを構造情報として用いるようにしてもよい。

動的モデル記憶部４８２は、制御対象機器９００の動的モデルを記憶する。制御対象機器９００の動的モデルは、制御対象機器９００の状態情報、および、制御指令値の入力を受けて制御対象機器９００がその制御指令値に従う場合の動作を模擬する。
動的モデルが、将来の時点における制御対象機器９００の予測位置の位置情報を出力するようにしてもよい。あるいは、動的モデルが、制御対象機器９００の動作量を出力するようにしてもよい。すなわち、動的モデルが、制御対象機器９００の将来の予測位置から現在位置を減算した差分を出力するようにしてもよい。
動的モデルは、制御指令値ｕの入力に対して、制御対象機器９００の状態情報ｘで示される状態の微分値または差分が求まるモデルであり、例えば状態空間モデルであってもよい。

制御対象機器９００のセンサからの情報と、構造情報記憶部４８１が記憶する構造情報と、動的モデル記憶部４８２が記憶する動的モデルとを用いることで、制御対象機器９００が占有する空間を特定することができる。
特に、制御対象機器９００の構造情報にアームの太さ等の情報が含まれていることで、障害物回避制御装置４００は、制御対象機器９００の骨格の位置のみでなく、アームの太さ等を考慮した位置情報を取得できる。すなわち、障害物回避制御装置４００は、制御対象機器９００の位置情報を、空間的な広がりを有する位置として把握できる。

また、障害物回避制御装置４００は、制御対象機器９００の動的モデルを用いることで、制御対象機器９００の動作をより正確に把握できる。これにより、制御対象機器９００が障害物に接触するか否か、接触する場合は制御対象機器９００がどのように動いて接触するかを判定でき、制御対象機器９００による障害物回避をより高精度に行わせることができる。

第２制御部４９０は、障害物回避制御装置４００の各部を制御して各種処理を実行する。第２制御部４９０の機能は、障害物回避制御装置４００が備えるＣＰＵが第２記憶部４８０からプログラムを読み出して実行することで実行される。
干渉関数演算部４９１は、干渉関数値、および、干渉関数値の経時変化を示す値を算出する。干渉関数は、制御対象機器９００と障害物との位置関係に応じた値を示す。干渉関数Ｂは、式（１）のような値をとる。

式（１）では、ｘは、制御対象機器９００の状態情報を示す。例えば、情報取得装置２００が、制御対象機器９００の表面の位置情報をボクセルデータで送信し、干渉関数演算部４９１は、制御対象機器９００の状態情報を干渉関数Ｂに適用することで、制御対象機器９００と障害物とが最も近い位置における両者の距離を算出するようにしてもよい。

以下では、干渉関数値Ｂ（ｘ）が、制御対象機器９００の状態情報ｘが示す制御対象機器９００の位置と障害物との距離を示すものとする。障害物が複数ある場合は、制御対象機器９００の位置に最も近い障害物との距離を示すものとする。通常、制御対象機器９００は障害物の中には入らないので、制御対象機器９００が障害物の内部に位置する場合の干渉関数値Ｂ（ｘ）は定義されていなくてもよい。
干渉関数値Ｂ（ｘ）により、制御対象機器９００が障害物に接触しているか否か、および、制御対象機器９００と障害物との距離が示される。
干渉関数演算部４９１は、障害物の状態情報に基づいて干渉関数を生成し、第２記憶部４８０に記憶させておく。

制御対象機器９００が動作して制御対象機器９００の位置が経時変化することで、干渉関数値Ｂ（ｘ）も経時変化する。この場合、干渉関数演算部４９１は、干渉関数値Ｂ（ｘ）の経時変化を示す値として、制御ステップ間における干渉関数値Ｂ（ｘ）の変化量を算出する。

ここでの制御ステップは、障害物回避制御装置４００が制御対象機器９００に対して制御指令値を１回送信するための一連の処理のステップである。すなわち、障害物回避制御装置４００は、周期的な制御ステップ毎に制御対象機器９００に対して制御指令値を送信する。
干渉関数演算部４９１は、今回の制御ステップから次回の制御ステップまでの間の、干渉関数値Ｂ（ｘ）の変化量を予測する。制御ステップ間における干渉関数値の変化量をΔＢ（ｘ，ｕ）と表記する。干渉関数値Ｂ（ｘ）の変化量は制御対象機器９００の位置の変化に依存し、制御対象機器９００の位置の変化は制御指令値ｕに依存するため、制御指令値ｕを明示している。

また、干渉関数演算部４９１は、制御対象機器９００の位置に対するポテンシャル関数値Ｐ（ｘ）を算出する。ポテンシャル関数値Ｐ（ｘ）は、制御対象機器９００と障害物との距離に基づく仮想的な力場の値である。干渉関数演算部４９１は、制御対象機器９００の状態情報に示される制御対象機器９００の位置が障害物に近いほど（従って、干渉関数値Ｂ（ｘ）が小さいほど）ポテンシャル関数値Ｐ（ｘ）を大きく算出する。

機器制御部４９２は、ノミナル制御装置３００からのノミナル指令値、および、回避指令値演算部４９５による処理の結果のうち何れか一方に基づいて、制御対象機器９００を制御する。上述したように、ノミナル指令値は、障害物回避を考慮していない制御指令値である。一方、回避指令値演算部４９５は、制御対象機器９００が障害物を回避できるような制御指令値を求める。

制御対象機器９００と障害物との距離が所定の閾値より大きいと判定された場合、機器制御部４９２は、ノミナル制御装置３００からのノミナル指令値、または、ノミナル指令値に対して仮想力場反映部４９６による補正を加えた制御指令値を、第２通信部４１０を介して制御対象機器９００へ送信する。

一方、制御対象機器９００と障害物との距離が所定の閾値以下であると判定された場合、回避指令値演算部４９５が、障害物を回避できる制御指令値の算出を試みる。回避指令値演算部４９５が制御指令値の算出に成功した場合、機器制御部４９２は、回避指令値演算部４９５が算出した制御指令値、または、その制御指令値に対して仮想力場反映部４９６による補正を加えた制御指令値を、第２通信部４１０を介して制御対象機器９００へ送信する。
一方、回避指令値演算部４９５が制御指令値の算出に失敗した場合、すなわち、障害物を回避できる制御指令値を得られなかった場合、機器制御部４９２は、制御対象機器９００を減速させる制御指令値を、第２通信部４１０を介して制御対象機器９００へ送信する。

ノミナル指令値取得部４９３は、ノミナル指令値を取得する。具体的には、ノミナル指令値取得部４９３は、第２通信部４１０の受信信号の中から、ノミナル制御装置３００からのノミナル指令値を抽出する。
回避要否判定部４９４は、制御対象機器９００と障害物との位置関係に基づいて、回避指令値演算部４９５が制御指令値を取得するための処理を行う必要の有無を判定する。上記のように、回避要否判定部４９４は、制御対象機器９００と障害物との距離と閾値とを比較して、距離が閾値より大きいか否かを判定する。具体的には、回避要否判定部４９４は、干渉関数値Ｂ（ｘ）と所定の定数の閾値αとを比較し、Ｂ（ｘ）＞αか否かを判定する。

Ｂ（ｘ）＞αである場合、制御対象機器９００の位置が障害物から比較的離れている。この場合、回避要否判定部４９４は、回避指令値演算部４９５が制御指令値を取得するための処理を行う必要は無いと判定する。一方、Ｂ（ｘ）≦αである場合、制御対象機器９００の位置が障害物に比較的近い。この場合、回避要否判定部４９４は、回避指令値演算部４９５が制御指令値を取得するための処理を行う必要があると判定する。

回避指令値演算部４９５は、回避指令値を求める。回避指令値は、制御対象機器９００に対する制御指令値であって、制御対象機器９００が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である。特に、障害物回避制御装置４００が回避指令値を用いて制御対象機器９００を制御することで、制御対象機器９００が障害物に接触しないように制御できる。

回避指令値演算部４９５は、求める解としての制御指令値とノミナル指令値との相違の度合いが所定の条件を満たすという終了条件を用いて回避指令値を求める。例えば、回避指令値演算部４９５は、上記の制約条件の下で、求める解としての制御指令値とノミナル指令値との相違の度合いを最小化する最適化問題を解くことで、回避指令値を求める。

この場合、回避指令値演算部４９５が解く最適化問題における評価関数（目的関数とも称される）は、例えば、式（２）のように示される。

「ｕ^＊」は、この最適化問題の解となる制御指令値を示す。「ａｒｇｍｉｎ」は、引数の値を最小化する関数である。式（２）の場合、「ａｒｇｍｉｎ」は、引数「（ｕ−ｕ_ｒ）^Ｔ（ｕ−ｕ_ｒ）」を最小化するような、ｕの値を関数値とする。
ベクトルまたは行列に上付の「Ｔ」は、ベクトルまたは行列の転置を示す。
「ｕ_ｒ」は、ノミナル制御装置３００からのノミナル指令値を示す。

式（２）は、制約条件の下で、ノミナル指令値ｕ_ｒになるべく近い制御指令値を求めることを示している。ノミナル指令値ｕ_ｒは、制御対象機器９００に設定されている目的を制御対象機器９００に実行させるように算出された指令値なので、ノミナル指令値ｕ_ｒに近い指令値を求めることで、制御対象機器９００に設定されている目的を制御対象機器９００に実行させることができると期待される。
制御指令値を示すｕ^＊、ｕ、ｕ_ｒのデータ形式は、同じ次元のベクトルであるとする。これらのベクトルの次元数は、障害物回避制御装置４００が制御対象機器９００へ送信する制御指令値の次元数と同じとする。
式（２）は、評価関数の例に該当する。式（２）における最小解となる制御指令値ｕは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。

回避指令値演算部４９５が解く最小化問題における制約条件は、２種類の式で示される。２種類の式のうち１種類目は、式（３）のように示される。

γは、０≦γ＜１の定数である。
γの値によって、制御対象機器９００と障害物とが接触しないための、制御対象機器９００と障害物との間隔の余裕分をどれだけ見込むか調整できる。
通常、制御対象機器９００と障害物とは接触しておらず、Ｂ（ｘ）が、制御対象機器９００と障害物との距離を示す。制御対象機器９００が障害物に接近してΔＢ（ｘ，ｕ）が負の値をとるとき、ΔＢ（ｘ，ｕ）の大きさがγＢ（ｘ）以下である場合に式（３）が成り立つ。

このことから、Ｂ（ｘ）で示される制御対象機器９００と障害物との距離のうち、（１−γ）Ｂ（ｘ）の分は、制御対象機器９００と障害物とが接触しないための余裕分として、制御対象機器９００の動作可能範囲から除外されているといえる。γの値を大きく設定するほど、制御対象機器９００の動作可能範囲が広がる。一方、γの値を小さく設定するほど、制御対象機器９００と障害物とが接触しないための余裕分を大きくとることになり、例えば予期しない外力によって制御対象機器９００が障害物のほうへ押された場合でも、制御対象機器９００が障害物にぶつかりにくい。

式（３）に示されるように、回避指令値演算部４９５は、干渉関数値、および、干渉関数値の経時変化を示す値を用いて回避指令値を求める。
複数の障害物が存在する場合、障害物毎に式（３）の制約条件を設けることができる。これにより、障害物回避制御装置４００は、制御対象機器９００が障害物全体に接触しないよう制御することができる。あるいは、複数の障害物をまとめたものに対して干渉関数を設計してもよい。

式（３）は、制御対象機器９００が今回の制御ステップで障害物に接触していなければ、次の制御ステップでも障害物に接触しないことの十分条件を示す。この点について説明する。
今回の制御ステップをｔで示し、制御ステップｔの次の制御ステップをｔ＋１で示す。制御ステップｔにおける干渉関数値をＢ（ｘ_ｔ）と表記する。
制御ステップｔ＋１における干渉関数値をＢ（ｘ_ｔ＋１）と表記する。Ｂ（ｘ_ｔ＋１）からＢ（ｘ_ｔ）を減算した差分を、ΔＢ（ｘ_ｔ，ｕ_ｔ）と表記する。ΔＢ（ｘ_ｔ，ｕ_ｔ）は、式（４）のように示される。

また、式（３）より式（５）を得られる。

式（４）および式（４）より式（６）を得られる。

０≦γ＜１なので、Ｂ（ｘ_ｔ）＞０の場合、Ｂ（ｘ_ｔ）−γＢ（ｘ_ｔ）≧０であり、Ｂ（ｘ_ｔ＋１）＞０である。従って、制御ステップｔで制御対象機器９００の位置が障害物の外部にあれば、制御ステップｔ＋１でも制御対象機器９００の位置は障害物の外部にある。

式（３）の制約条件の判定に、制御対象機器９００の表面の位置情報が用いられる場合、回避指令値演算部４９５は、式（３）の条件の成否をより正確に判定できる。特に、制御対象機器９００の表面の位置情報と、障害物の表面の位置情報とが用いられる場合、回避指令値演算部４９５は、干渉関数Ｂを用いて制御対象機器９００と障害物とが最も近い位置での両者の距離を得られる。
回避指令値演算部４９５は、この距離を用いて式（３）の条件を判定することで、より正確に条件判定を行うことができる。この点で、回避指令値演算部４９５が、より高精度な制御指令値を得られ、機器制御部４９２が、より高精度に制御対象機器９００を制御できると期待される。

式（３）の制約条件の判定に制御対象機器９００の表面の位置情報が用いられる場合の例として、情報取得装置２００が、制御対象機器９００の表面の位置情報を例えばボクセルデータで障害物回避制御装置４００へ送信する場合が挙げられる。
式（３）の制約条件の判定に障害物の表面の位置情報が用いられる場合の例として、情報取得装置２００が、障害物の表面の位置情報を例えばボクセルデータを障害物回避制御装置４００へ送信し、干渉関数演算部４９１が、その位置情報を用いて干渉関数を生成する場合が挙げられる。

一方、式（３）の判定に制御対象機器９００に設定された基準点の位置情報が用いられる場合、基準点毎に式（３）の条件式を設けることができる。障害物回避制御装置４００では、この点で、基準点が複数の場合に比較的容易に対応できる。
障害物が複数ある場合も、障害物毎に式（３）の条件式を設けることができる。障害物回避制御装置４００では、この点で、障害物が複数の場合に比較的容易に対応できる。

回避指令値演算部４９５が解く最小化問題における制約条件を示す２種類の式のうち２種類目は、式（７）のように示される。

ｕ_ｉ（ｉは、１≦ｉ≦Ｎの整数）は、例えば、制御対象機器９００の関節毎など、制御対象機器９００の可動部分毎の制御指令値を示すスカラ値である。Ｎは、制御対象機器９００の可動部分の個数を示す。ｉは、可動部分を識別する識別番号である。
識別番号ｉで識別される可動部分をｉ番目の可動部分と表記する。従って、ｕ_ｉは、ｉ番目の可動部分に対する制御指令値である。

ｕ_{ｉ＿ｍｉｎ}、ｕ_{ｉ＿ｍａｘ}は、それぞれ、制御対象機器９００の仕様により予め定められている、ｕ_ｉの下限値、上限値である。

式（７）は、いずれの制御指令値も、可動部分の仕様により規定される上下限値の範囲内で設定されるという制約条件を示している。可動部分の仕様は、例えば、その可動部分に用いられているアクチュエータの仕様により規定される。
制御指令値ｕは、ｕ_ｉ（ｉ＝１，２，・・・，Ｎ）を纏めてベクトルで表したものである。
上述したように、回避指令値演算部４９５は、回避指令値を取得するための処理が必要であると回避要否判定部４９４が判定した場合に、回避指令値を求める。

仮想力場反映部４９６は、制御対象機器９００が障害物に近いほど障害物から離れる動作を、ノミナル指令値および回避指令値のうち少なくとも何れか一方に反映させる。
制御対象機器９００が障害物に近いほど障害物から離れることは、制御対象機器９００が障害物に近いほど大きい加速度で障害物から離れることであってもよい。あるいは、制御対象機器９００が障害物に近いほど障害物から離れることは、制御対象機器９００が障害物に近いほど、障害物から離れる距離が長いことであってもよい。あるいは、制御対象機器９００が障害物に近いほど障害物から離れることは、制御対象機器９００と障害物との距離が所定の閾値以下である場合のみ、制御対象機器９００が障害物から離れることであってもよい。

例えば、仮想力場反映部４９６が、制御対象機器９００と障害物との位置関係に基づく仮想的な力場を模擬するようにしてもよい。この場合、仮想力場反映部４９６が、制御対象機器９００と障害物との距離が近いほど反発力を大きく算出してもよい。制御対象機器９００に生じさせる反発力が大きいほど、制御対象機器９００の加速度が大きくなり、制御対象機器９００が障害物から離れる移動量の大きさが大きくなる。

例えば、仮想力場反映部４９６が算出する制御指令値をｕ_ｐと表記する。制御指令値ｕ_Ｐが制御対象機器９００に設定された基準点の移動量を示すとすると、ｕ_ｐ＝（Δｘ，Δｙ，Δｚ）と示される。
障害物を質点（Point Mass）で代表し、障害物の座標を（ｘ_０，ｙ_０，ｚ_０）としてもよい。そして、仮想力場反映部４９６が、座標（ｘ，ｙ，ｚ）に位置する制御対象機器９００に対して、障害物から遠ざからせるための制御指令値ｕ_ｐを、式（８）に基づいて算出するようにしてもよい。

Ｐ（ｘ，ｙ，ｚ）はポテンシャル関数を示す。∇はナブラを示す。Ｐ_０（ｘ，ｙ，ｚ）は、式（９）のように示される。

なお、式（１）、式（３）から式（６）、および、式（１０）から式（１２）でｘが制御対象機器９００の３次元の位置を示すのと異なり、式（８）および式（９）では、（ｘ，ｙ，ｚ）が座標を示す。したがって、式（１）、式（３）から式（６）、および、式（１０）から式（１２）でｘがベクトル量または行列を示すのと異なり、式（８）および式（９）ではｘはスカラ量を示す。

仮想力場反映部４９６が、例えばロボットアームの先端など代表的な１点について、指令値としての制御対象機器９００の移動量を算出し、算出した移動量を各関節に分散させるようにしてもよい。あるいは、仮想力場反映部４９６が、ロボットの関節毎など制御対象機器９００の複数部分の各々について、指示する移動量を算出するようにしてもよい。

仮想力場反映部４９６が、算出した指令値を制御指令値に反映させることで、制御対象機器９００と障害物との接触を、より確実に防止できる。
以下では、仮想力場反映部４９６が、ノミナル指令値および回避指令値の両方に、算出した指令値を反映させる場合を例に説明する。
但し、仮想力場反映部４９６の処理は障害物回避制御装置４００に必須ではない。従って、障害物回避制御装置４００が仮想力場反映部４９６を備えていない構成となっていてもよい。

図４は、障害物回避制御システム１におけるデータの流れの例を示す図である。図４では、障害物に符号９５０を付している。障害物９５０は、上記で説明した障害物と同様である。
上述したように、制御対象機器９００のセンサのセンシングデータは、情報取得装置２００の状態観測部２９１へ入力される。
撮像装置１００の画像データは、情報取得装置２００の三次元計測部２９２へ入力される。撮像装置１００の付加情報がある場合、画像データに加えて付加情報も三次元計測部２９２へ入力される。

情報取得装置２００は、状態観測部２９１が算出する制御対象機器９００の状態情報を、ノミナル制御装置３００および障害物回避制御装置４００へ送信する。例えば、情報取得装置２００は、制御対象機器９００の表面の位置情報をボクセルデータで送信する。加えて、情報取得装置２００は、制御対象機器９００の表面または基準点の速度、加速度、角速度または角加速度など、制御対象機器９００の動作を示す情報を送信する。例えば、情報取得装置２００が、一般化座標ｑと、一般化速度ｑ’とを並べたベクトルを送信するようにしてもよい。

また、情報取得装置２００は、障害物９５０の状態情報を障害物回避制御装置４００へ送信する。特に情報取得装置２００は、障害物９５０の３次元の位置情報を障害物回避制御装置４００へ送信する。情報取得装置２００が障害物９５０の位置情報を送信するデータ形式は、特定のものに限定されない。例えば、情報取得装置２００が、障害物９５０の位置情報を、ボクセル（Voxel）データにて送信するようにしてもよい。あるいは、情報取得装置２００が、障害物９５０の位置情報を、サーフェイス（Surface）データにて送信するようにしてもよい。あるいは、情報取得装置２００が、障害物９５０の位置情報を、ポイントクラウド（Point Cloud）データにて送信するようにしてもよい。

ノミナル制御装置３００は、情報取得装置２００から受信する、制御対象機器９００の状態情報を用いて、ノミナル指令値ｕ_ｒを算出する。ノミナル制御装置３００は、算出したノミナル指令値ｕ_ｒを障害物回避制御装置４００へ送信する。

障害物回避制御装置４００の干渉関数演算部４９１は、情報取得装置２００から得られた制御対象機器９００の状態情報および障害物の状態情報と、機器制御部４９２が設定する制御指令値とに基づいて、干渉関数値Ｂ（ｘ）、制御ステップ間におけるＢ（ｘ）の変化量ΔＢ（ｘ，ｕ）、および、ポテンシャル関数値Ｐ（ｘ）を算出する。上記のように、仮想力場反映部４９６の処理は必須ではない。従って、干渉関数演算部４９１によるポテンシャル関数値の算出は必須ではない。

障害物回避制御装置４００の機器制御部４９２は、ノミナル制御装置３００から得られるノミナル指令値ｕ_ｒと、干渉関数演算部４９１から得られる干渉関数値Ｂ（ｘ）、干渉関数値の変化量ΔＢ（ｘ，ｕ）、および、ポテンシャル関数値Ｐ（ｘ）とを用いて、制御対象機器９００に対する制御指令値ｕを取得する。機器制御部４９２は、取得した制御指令値ｕを、第２通信部４１０を介して制御対象機器９００へ送信することで、制御対象機器９００を制御する。

図５から図７を参照して、障害物回避制御装置４００の動作について説明する。
図５は、障害物回避制御装置４００が制御対象機器９００に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置４００は、制御対象機器９００に対する制御の制御ステップ毎に図５の処理を行う。
図５の処理で、仮想力場反映部４９６は、干渉関数演算部４９１が算出するポテンシャル関数値Ｐ（ｘ）に基づいて、力場を模擬するための制御指令値ｕ_ｐを算出する（ステップＳ１１１）。制御指令値ｕ_ｐは、ノミナル指令値または回避指令値に値を付加する補正項として用いられる。

また、回避要否判定部４９４は、干渉関数演算部４９１が算出する干渉関数値Ｂ（ｘ）が、閾値αより大きいか否かを判定する（ステップＳ１１２）。
干渉関数値Ｂ（ｘ）が閾値αより大きいと判定した場合（ステップＳ１１２：ＹＥＳ）、機器制御部４９２は、制御対象機器９００に対する制御指令値ｕを、ノミナル指令値ｕ_ｒに制御指令値ｕ_ｐを加えた値と算出する（ステップＳ１２１）。すなわち、機器制御部４９２は、ｕ＝ｕ_ｒ＋ｕ_ｐと算出する。
そして、機器制御部４９２は、制御指令値ｕを、第２通信部４１０を介して制御対象機器９００へ送信することで、制御対象機器９００を制御する（ステップＳ１６１）。
ステップＳ１６１の後、障害物回避制御装置４００は、図５の処理を終了する。

一方、ステップＳ１１２で干渉関数値Ｂ（ｘ）が閾値α以下であると回避要否判定部４９４が判定した場合（ステップＳ１１２：ＮＯ）、回避指令値演算部４９５は、上述した最適化問題の演算による制御指令値の取得を試みる（ステップＳ１３１）。
機器制御部４９２は、回避指令値演算部４９５がステップＳ１３１で最適化問題の解を得られたか否かを判定する（ステップＳ１３２）。

解を得られたと判定した場合（ステップＳ１３２：ＹＥＳ）、機器制御部４９２は、制御対象機器９００に対する制御指令値ｕを、最適化問題の解による制御指令値ｕ^＊に制御指令値ｕ_ｐを加えた値と算出する（ステップＳ１４１）。すなわち、機器制御部４９２は、ｕ＝ｕ^＊＋ｕ_ｐと算出する。
ステップＳ１４１の後、処理がステップＳ１６１へ進む。

一方、ステップＳ１３２の判定で、最適化問題の解を得られていないと判定した場合（ステップＳ１３２：ＮＯ）、機器制御部４９２は、制御対象機器９００を減速させるように制御指令値ｕを生成する（ステップＳ１５１）。
ステップＳ１５１の後、処理がステップＳ１６１へ進む。

図６は、障害物回避制御装置４００が干渉関数に関する値を算出する処理手順の例を示すフローチャートである。障害物回避制御装置４００は、制御対象機器９００に対する制御の制御ステップ毎に、図５の処理の前処理として図６の処理を行う。
図６の処理で、干渉関数演算部４９１は、情報取得装置２００が送信する制御対象機器９００の情報、および、構造情報記憶部４８１が記憶している制御対象機器９００の構造情報等を用いて、制御対象機器９００の占有空間の位置情報を算出する（ステップＳ２１１）。干渉関数演算部４９１は、制御対象機器９００の占有空間の位置情報を、制御対象機器９００の座標系による座標値で算出する。ここでいう制御対象機器９００の座標系は、例えば、障害物回避制御装置４００が制御対象機器９００に送信する制御指令値に用いられる座標値である。

次に干渉関数演算部４９１は、障害物の位置情報を取得する（ステップＳ２１２）。三次元計測部２９２が、障害物の位置を検出して制御対象機器９００の座標系による位置情報を生成し、干渉関数演算部４９１が、その位置情報を用いるようにしてもよい。あるいは、三次元計測部２９２が、障害物の位置情報を、制御対象機器９００の座標系とは異なる座標系の座標値で生成し、干渉関数演算部４９１が、制御対象機器９００の座標系の座標値に座標変換するようにしてもよい。

次に、干渉関数演算部４９１は、制御対象機器９００の位置情報および障害物の位置情報に基づいて、干渉関数値Ｂ（ｘ）を算出する（ステップＳ２１３）。
また、干渉関数演算部４９１は、制御対象機器９００の位置情報、制御対象機器９００の動作情報、および、障害物の位置情報に基づいて、干渉関数値の変化量ΔＢ（ｘ，ｕ）を算出する（ステップＳ２１４）。

干渉関数演算部４９１が、制御対象機器９００の動作情報として、状態観測部２９１が制御対象機器９００のセンサのセンシングデータから算出する、制御対象機器９００の動作情報を用いるようにしてもよい。あるいは、干渉関数演算部４９１が、制御対象機器９００の動作情報として、前回の制御ステップでの制御対象機器９００への制御指令値ｕを用いるようにしてもよい。あるいは、干渉関数演算部４９１が、制御対象機器９００の動作情報として、これらの両方を用いるようにしてもよい。
ステップＳ２１４の後、障害物回避制御装置４００は、図６の処理を終了する。

図７は、障害物回避制御装置４００が干渉関数値の変化量ΔＢ（ｘ，ｕ）を算出する処理手順の例を示すフローチャートである。障害物回避制御装置４００は、図６のステップＳ２１４で図７の処理を行う。
図７の処理で、干渉関数演算部４９１は、動的モデルの演算でΔＢ（ｘ，ｕ）を算出可能か判定する（ステップＳ２２１）。
算出可能と判定した場合（ステップＳ２２１；ＹＥＳ）、干渉関数演算部４９１は、動的モデルの演算でΔＢ（ｘ，ｕ）を算出する（ステップＳ２３１）。
ステップＳ２３１の後、障害物回避制御装置４００は、図７の処理を終了する。

一方、ステップＳ２２１で、動的モデルの演算でΔＢ（ｘ，ｕ）を算出できないと判定した場合（ステップＳ２２１：ＮＯ）、干渉関数演算部４９１は、制御指令値ｕの微小変化を用いてΔＢ（ｘ，ｕ）を算出する。例えば、干渉関数演算部４９１は、式（１０）を用いてΔＢ（ｘ，ｕ）を近似的に算出する。

Δｔは、制御ステップ間の時間間隔を示す。Ｂ（ｘ，ｕ）は、干渉関数値を示す。制御指令値ｕを変更すると制御対象機器９００の動作が変わり干渉関数値が変わるので、干渉関数Ｂを制御指令値ｕの関数として表記している。
ステップＳ２４１の後、障害物回避制御装置４００は図７の処理を終了する。

ステップＳ２３１の処理とステップＳ２４１の処理とを比較すると、ステップＳ２３１の処理では、ｕの微小な変化を用いた試行、および、式（１０）のような近似を用いる必要が無い。この点で、ステップＳ２３１の処理のほうがステップＳ２４１の処理よりも、得られるΔＢ（ｘ，ｕ）の精度が高いと期待される。一方、ステップＳ２３１でΔＢ（ｘ，ｕ）を算出できない場合でも、ステップＳ２４１の処理によれば算出できると期待される。
そこで、干渉関数演算部４９１は、ステップＳ２３１の処理でΔＢ（ｘ，ｕ）を算出可能か判定し、算出可能であればステップＳ２３１の処理を行う。一方、ステップＳ２３１の処理ではΔＢ（ｘ，ｕ）を算出できない場合、干渉関数演算部４９１は、ステップＳ２４１の処理を行う。

以上のように、ノミナル指令値取得部４９３は、ノミナル指令値を取得する。ノミナル指令値は、制御対象機器９００に対する指令値を求めるために用いられる、いわば基準となる制御指令値である。回避指令値演算部４９５は、回避指令値を求める。回避指令値は、式（３）のように、制御対象機器９００が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、式（２）のように、ノミナル指令値との相違の度合いの小ささに関する所定の条件を満たす制御指令値である。機器制御部４９２は、ノミナル指令値、および、回避指令値演算部４９５による処理の結果のうち何れか一方に基づいて、制御対象機器９００を制御する。

障害物回避制御装置４００では、制御対象機器９００と障害物との接触を回避するための制約条件を設けることができる。制御対象機器９００と障害物との接触を回避するための基準点が制御対象機器９００に複数設定される場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置４００では、制御対象機器９００について障害物を回避するように制御する基準点は１か所に限定されず、制御対象機器９００のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置４００では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置４００によれば、障害物が複数ある場合に比較的容易に対応できる。

また、ノミナル指令値取得部４９３は、ノミナル指令値としてノミナル指令値を取得する。ノミナル指令値は、制御対象機器９００による障害物回避を考慮しない場合の制御指令値である。
ノミナル指令値は、制御対象機器９００に設定されている目標を達成するように設定される指令値であり、障害物回避制御装置４００が、障害物回避を考慮した回避指令値をノミナル指令値に近付けるように生成することで、目標を達成できると期待される。
障害物回避制御装置４００によれば、このように、回避指令値を生成する際にノミナル指令値を基準として用いることができ、この点で、回避指令値を比較的容易に生成できる。

また、干渉関数演算部４９１は、干渉関数値（Ｂ（ｘ））、および、干渉関数値の経時変化を示す値（ΔＢ（ｘ，ｕ））を算出する。干渉関数値は、制御対象機器９００と障害物との位置関係に応じた値を示す。回避指令値演算部４９５は、干渉関数値、および、干渉関数値の経時変化を示す値を用いて、回避指令値を求める。

障害物回避制御装置４００によれば、これらの値を用いることで、制御対象機器９００が障害物に接触する可能性の度合いを推定することができ、必要に応じて障害物回避等の措置をとることができる。
障害物回避制御装置４００では、制御対象機器９００と障害物との距離を干渉関数として用いることができ、制御対象機器９００の位置と障害物の位置とから容易に関数値を算出できる。この点で、障害物回避制御装置４００の負荷が軽くて済む。

また、回避要否判定部４９４は、制御対象機器９００と障害物との位置関係に基づいて、回避指令値を取得するための処理の要否を判定する。回避指令値演算部４９５は、回避指令値を取得するための処理が必要であると回避要否判定部４９４が判定した場合に、回避指令値を求める。
回避指令値を取得するための処理が不要であると回避要否判定部４９４が判定した場合、ノミナル制御装置３００が生成したノミナル指令値を制御対象機器９００への指令値として用いることができ、障害物回避制御装置４００が新たに指令値を生成する必要が無い。この点で、障害物回避制御装置４００の負荷が比較的軽くて済む。

また、機器制御部４９２は、回避指令値演算部４９５が回避指令値の取得に失敗したと判定した場合、制御対象機器９００を減速させる。
回避指令値演算部４９５が回避指令値の取得に失敗した場合、制御対象機器９００と障害物との接触を回避できない可能性がある。そこで、機器制御部４９２が制御対象機器９００を減速させる。これにより、制御対象機器９００が障害物に接触するまでの時間を確保でき、例えば人手で障害物を動かすといった対応が可能である。また、制御対象機器９００が障害物に接触した場合でも、制御対象機器９００が減速していることで、不具合が生じないか、あるいは、不具合の程度が軽くて済むと期待される。

また、仮想力場反映部４９６は、制御対象機器９００が障害物に近いほど大きい加速度で障害物から離れる動作を、ノミナル指令値および回避指令値のうち少なくとも何れか一方に反映させる。機器制御部４９２は、この動作が反映されたノミナル指令値、および、この動作が反映された回避指令値のうち何れか一方に基づいて、制御対象機器９００を制御する。
これにより、制御対象機器９００は、障害物に近いほど、障害物から離れるように動作する。障害物回避制御装置４００によれば、この点で、より確実に、制御対象機器９００に障害物を回避させることができる。

また、回避指令値演算部４９５は、制御対象機器９００に設定された基準点毎に、その基準点が障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、障害物回避制御装置４００では、基準点毎に制約条件の式を設けることができる。障害物回避制御装置４００によればこの点で、制御対象機器に基準点が複数設定されている場合に、比較的容易に対応できる。

また、回避指令値演算部４９５は、障害物毎に、制御対象機器９００がその障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、障害物回避制御装置４００では、障害物毎に制約条件の式を設けることができる。障害物回避制御装置４００によればこの点で、障害物が複数ある場合に、比較的容易に対応できる。

また、回避指令値演算部４９５は、制御対象機器９００と障害物とが最も近い位置における制御対象機器９００と障害物との距離に基づいて判定される、制御対象機器９００が障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、回避指令値演算部４９５が、制御対象機器９００と障害物とが最も近い位置における制御対象機器９００と障害物との距離を用いて回避指令値を求めることで、制御対象機器９００と障害物との接触の可能性をより正確に反映した回避指令値を求めることができる。障害物回避制御装置４００によればこの点で、制御対象機器９００の制御を比較的高精度に行うことができる。

＜第２実施形態＞
図８は、第２実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図８に示す構成で、障害物回避制御システム２は、撮像装置１００と、情報取得装置２００と、ノミナル制御装置３００と、障害物回避制御装置４００と、障害物位置特定装置５００とを備える。また、障害物回避制御システム２は制御対象機器９００を制御する。

障害物回避制御システム２は、障害物位置特定装置５００を備える点で障害物回避制御システム１の場合と異なる。それ以外の点では、障害物回避制御システム２は障害物回避制御システム１と同様である。障害物回避制御システム２について、障害物回避制御システム１と同様の点の説明を省略し、必要に応じて図２に示す符号、および、図３に示す符号を引用する。

図９は、障害物位置特定装置５００の機能構成の例を示す概略ブロック図である。図９に示す構成で、障害物位置特定装置５００は、第３通信部５１０と、第３記憶部５８０と、第３制御部５９０とを備える。第３記憶部５８０は、物体情報記憶部５８１を備える。第３制御部５９０は、種類識別部５９１と、位置特定部５９２とを備える。

第３通信部５１０は、他の装置と通信を行う。特に、第３通信部５１０は、情報取得装置２００からの障害物の位置情報を受信する。また、第３通信部５１０は、位置特定部５９２の処理が反映された障害物の位置情報を障害物回避制御装置４００へ送信する。第３通信部５１０が、情報取得装置２００から受信した情報に加えて、位置特定部５９２が算出した障害物の位置情報を障害物回避制御装置４００へ送信するようにしてもよい。あるいは、第３通信部５１０が、情報取得装置２００から受信した情報に対して、障害物の位置情報を位置特定部５９２が算出した位置情報に置き換えた情報を、障害物回避制御装置４００へ送信するようにしてもよい。

第３記憶部５８０は、各種データを記憶する。第３記憶部５８０の機能は、障害物位置特定装置５００が備える記憶デバイスを用いて実行される。
物体情報記憶部５８１は、物体情報を記憶する。物体情報記憶部５８１は、物体の種類を識別する種類識別情報と、物体の形状および大きさなど物体の位置を特定するための情報とが紐付けられた情報を、物体情報として記憶する。

ここでの物体の種類は識別可能なものであればよく、特定のものに限定されない。例えば、物体情報記憶部５８１が、ペットボトル、コップなど、物体の名称毎の物体情報を記憶していてもよい。あるいは、物体情報記憶部５８１が、円柱状の物、円形の物など、物体の形状毎の物体情報を記憶していてもよい。また、物体識別情報にペットボトルの情報が含まれる場合にペットボトルの容量がパラメータになっているなど、物体識別情報がパラメータを含んでいてもよい。

第３制御部５９０は、障害物位置特定装置５００の各部を制御して各種処理を実行する。第３制御部５９０の機能は、障害物位置特定装置５００が備えるＣＰＵが、第３記憶部５８０からプログラムを読み出して実行することで実行される。
種類識別部５９１は、三次元計測部２９２が算出した障害物に関する情報に基づいて、障害物となっている物体の種類を特定する。

例えば、種類識別部５９１が、三次元計測部２９２が算出した情報で示される障害物を、物理的に一纏まりとなっている部分毎に分割するなど、障害物中の物体を抽出するようにしてもよい。そして、種類識別部５９１が、抽出した物体毎にパターンマッチングを行うことで、物体の種類を特定するようにしてもよい。
種類識別部５９１が、例えばＹＯＬＯ（You Only Look Once）またはＳＳＤ（Single Shot Multibox Detector）などのように、ニューラルネットワークによる物体検出器として構成されていてもよい。

位置特定部５９２は、三次元計測部２９２が算出した障害物に関する情報、および、物体情報記憶部５８１が記憶している物体情報に基づいて、障害物となっている物体の位置を特定する。
例えば、位置特定部５９２は、種類識別部５９１が特定した物体の種類に基づいて、物体情報記憶部５８１が記憶している物体情報から、物体の位置および大きさの情報を読み出す。そして、位置特定部５９２は、三次元計測部２９２が算出した障害物に関する情報が示す物体の位置に、物体情報から読み出した位置および大きさの物体があるものとして、その物体の位置情報を算出する。位置特定部５９２が、物体の位置を座標値の形式で算出するようにしてもよい。

これにより、位置特定部５９２は、障害物を三次元的な広がりをもつ物体として捉え、その座標を算出する。例えば、撮像装置１００が障害物を一方の側から撮像し、反対側の画像を得られていない場合に、位置特定部５９２は、画像に写っていない部分の障害物の位置情報を算出することができる。

位置特定部５９２は、算出した物体毎の位置情報を、第３通信部５１０を介して障害物回避制御装置４００へ送信する。上述したように、第３通信部５１０が、情報取得装置２００から受信した情報に加えて、位置特定部５９２が算出した物体毎の位置情報を障害物回避制御装置４００へ送信するようにしてもよい。あるいは、第３通信部５１０が、情報取得装置２００から受信した情報に対して、障害物の位置情報を位置特定部５９２が算出した位置情報に置き換えた情報を、障害物回避制御装置４００へ送信するようにしてもよい。

図１０は、障害物回避制御システム２におけるデータの流れの例を示す図である。
図１０を図４と比較すると、図１０の障害物回避制御システム２は、障害物位置特定装置５００を備える点で、図４の障害物回避制御システム１と異なる。この違いに伴い、図４では、情報取得装置２００が、三次元計測部２９２が算出した障害物に関する情報を障害物回避制御装置４００へ送信するのに対し、図１０では、情報取得装置２００は、三次元計測部２９２が算出した障害物に関する情報を障害物位置特定装置５００へ送信する。

図１０では、障害物位置特定装置５００は、情報取得装置２００から受信した情報に対して、障害物の位置情報を位置特定部５９２が算出した位置情報に置き換えた情報を、障害物回避制御装置４００へ送信する。障害物回避制御装置４００では、干渉関数演算部４９１が、情報取得装置２００の状態観測部２９１が算出した情報および障害物位置特定装置５００からの情報を用いて、干渉関数値、干渉関数値の差分、および、ポテンシャル関数値を算出する。
それ以外の点では、図１０は図４の場合と同様である。

以上のように、種類識別部５９１は、障害物となっている物体の種類を識別する。位置特定部５９２は、物体の種類毎に物体の大きさおよび形状を示す物体情報から、種類識別部５９１が識別した種類に応じた情報を読み出し、読み出した情報を用いて障害物となっている物体の位置を特定する。

これにより、障害物回避制御システム２では、障害物の位置情報をより高精度に得られる。例えば、障害物位置特定装置５００は、撮像装置１００の画像に写っていない部分について、障害物の位置情報を補完することができる。
障害物の位置情報をより高精度に得られることで、障害物回避制御装置４００は、制御対象機器９００に障害物を回避させるための指令値を、より高精度に算出することができる。

＜第３実施形態＞
図１１は、第３実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図１１に示す構成で、障害物回避制御システム３は、撮像装置１００と、情報取得装置２００と、ノミナル制御装置３００と、障害物回避制御装置４００と、障害物位置特定装置５００と、障害物動作予測装置６００とを備える。また、障害物回避制御システム３は制御対象機器９００を制御する。

障害物回避制御システム３は、障害物動作予測装置６００を備える点で、障害物回避制御システム２と異なる。
それ以外の点では、障害物回避制御システム３は障害物回避制御システム２と同様である。障害物回避制御システム３について、障害物回避制御システム２と同様の点の説明を省略し、必要に応じて図２に示す符号、図３に示す符号、および、図９に示す符号を引用する。

図１２は、障害物動作予測装置６００の機能構成の例を示す概略ブロック図である。図１２に示す構成で、障害物動作予測装置６００は、第４通信部６１０と、第４記憶部６８０と、第４制御部６９０とを備える。第４記憶部６８０は、物体動作情報記憶部６８１を備える。第４制御部６９０は、位置予測部６９１を備える。

第４通信部６１０は、他の装置と通信を行う。障害物回避制御システム３では、障害物位置特定装置５００が、種類識別部５９１が特定した障害物の種類を示す情報と、位置特定部５９２が算出した障害物の位置情報とを紐付けて送信する。第４通信部６１０は、障害物位置特定装置５００が送信した情報を受信する。また、第４通信部６１０は、障害物位置特定装置５００から受信した情報に、位置予測部６９１が予測する障害物の将来の位置情報を加えた情報を、障害物回避制御装置４００へ送信する。

第４記憶部６８０は、各種データを記憶する。第４記憶部６８０の機能は、障害物動作予測装置６００が備える記憶デバイスを用いて実行される。
物体動作情報記憶部６８１は、物体動作情報を記憶する。物体動作情報記憶部６８１は、物体の種類を識別する種類識別情報と、物体の動作を予測するための情報とが紐付けられた情報を、物体動作情報として記憶する。
物体動作情報記憶部６８１が、物体の動作モデルを物体動作情報として記憶するようにしてもよい。あるいは、物体動作情報記憶部６８１が、障害物として検出された物体の位置の履歴情報を、物体動作情報として記憶するようにしてもよい。

第４制御部６９０は、障害物動作予測装置６００の各部を制御して各種処理を実行する。第４制御部６９０の機能は、障害物動作予測装置６００が備えるＣＰＵが、第４記憶部６８０からプログラムを読み出して実行することで実行される。
位置予測部６９１は、障害物となっている物体の将来の位置を特定する。例えば、位置予測部６９１は、障害物位置特定装置５００の種類識別部５９１が特定した物体の種類の識別情報を用いて、その物体の動作を予測するための情報を物体動作情報記憶部６８１から読み出す。そして、位置予測部６９１は、読み出した情報を用いて、制御ステップ間における物体の移動量を予測する。位置予測部６９１は、障害物位置特定装置５００の位置特定部５９２が算出した物体の位置に、予測した移動量を加算することで、次の制御ステップにおける物体の位置を予測する。

図１３は、障害物回避制御システム３におけるデータの流れの例を示す図である。
図１３を図１０と比較すると、図１３の障害物回避制御システム３は、障害物動作予測装置６００を備える点で、図１０の障害物回避制御システム２と異なる。この違いに伴い、図１０では、障害物位置特定装置５００が、障害物回避制御装置４００へ情報を送信するのに対し、図１３では、障害物位置特定装置５００は、障害物動作予測装置６００へ情報を送信し、障害物動作予測装置６００が、障害物回避制御装置４００へ情報を送信する。

図１３では、障害物位置特定装置５００は、物体毎に、物体の種類を示す情報と、物体の位置情報とを紐付けた情報を、障害物動作予測装置６００へ送信する。障害物動作予測装置６００は、物体毎に、物体の種類を示す情報と、物体の位置情報と、物体の位置の予測情報とが紐付けられた情報を、障害物回避制御装置４００へ送信する。障害物回避制御装置４００では、干渉関数演算部４９１が、情報取得装置２００の状態観測部２９１が算出した情報および障害物動作予測装置６００からの情報を用いて、干渉関数値、干渉関数値の差分、および、ポテンシャル関数値を算出する。
それ以外の点では、図１３は図１０の場合と同様である。

以上のように、障害物動作予測装置６００は、物体の種類毎に前記物体の動きに関する情報を示す物体動作情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて障害物の将来の位置を予測する。障害物回避制御装置４００の干渉関数演算部４９１は、予測された障害物の将来の位置に基づいて、干渉関数値の経時変化を示す値を算出する。

障害物回避制御システム３は、障害物の将来の位置の予測に、障害物となっている物体の特性として示されるその物体の動作に関する情報を用いることができ、この点で、障害物の将来の位置を比較的高精度に予測できる。この障害物の将来の位置の予測結果を制御対象機器９００の障害物回避に用いる点で、障害物回避制御システム３は、制御対象機器９００の障害物回避を比較的高精度に行うことができる。

＜第４実施形態＞
図１４は、第４実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図１４に示す構成で、障害物回避制御システム４は、撮像装置１００と、情報取得装置２００と、障害物回避制御装置４０１とを備える。また、障害物回避制御システム２は制御対象機器９００を制御する。

障害物回避制御システム４は、ノミナル制御装置３００を備えておらず、障害物回避制御装置４００に代えて障害物回避制御装置４０１を備える点で障害物回避制御装置４００と異なる。障害物回避制御装置４０１では、回避指令値を求める処理で用いる制約条件の１つに、制御対象機器９００に設定される目標を制御指令値に反映させるための制約条件が含まれる。
それ以外の点では、障害物回避制御システム４は障害物回避制御システム１と同様である。障害物回避制御システム４について、障害物回避制御システム１と同様の点の説明を省略し、必要に応じて図２に示す符号を引用する。

以下では、第１実施形態に基づいて第４実施形態を実施する場合を例に説明するが、第２実施形態または第３実施形態に基づいて第４実施形態を実施するようにしてもよい。特に、障害物回避制御システム４が、第２実施形態にかかる障害物位置特定装置５００を備えていてもよい。障害物回避制御システム４が、第３実施形態にかかる障害物位置特定装置５００および障害物動作予測装置６００を備えていてもよい。

図１５は、障害物回避制御装置４０１の機能構成の例を示す概略ブロック図である。図１５に示す構成で、障害物回避制御装置４０１は、第２通信部４１０と、第２記憶部４８０と、第２制御部４９０とを備える。第２記憶部４８０は、構造情報記憶部４８１と、動的モデル記憶部４８２と、制御関数記憶部４８３とを備える。第２制御部４９０は、干渉関数演算部４９１と、機器制御部４９２とを備える。機器制御部４９２は、回避指令値演算部４９５と、仮想力場反映部４９６とを備える。

障害物回避制御装置４０１は、第２記憶部４８０が制御関数記憶部４８３を備える点、および、機器制御部４９２がノミナル指令値取得部４９３と回避要否判定部４９４とを備えていない点で障害物回避制御装置４００と異なる。この違いは、制御対象機器９００に設定される目標を制御指令値に反映させる方法の違いに起因する。

障害物回避制御装置４００では、回避指令値演算部４９５が、評価関数を用いて回避指令値をノミナル指令値に近付けることで、制御対象機器９００に設定される目標を回避指令値に反映させる。一方、障害物回避制御装置４０１では、回避指令値演算部４９５が回避指令値を求める処理で用いる制約条件の１つに、制御対象機器９００に設定される目標を制御指令値に反映させるための制約条件が含まれる。
それ以外の点では、障害物回避制御装置４０１は障害物回避制御装置４００と同様である。

第４実施形態で回避指令値演算部４９５が解く最適化問題における制約条件は、３種類の式で示される。
３種類の式のうち１種類目は、第１実施形態の場合と同様、制御対象機器９００が障害物に接触しないための十分条件を示す式である。３種類の式のうち１種類目は、例えば上述した式（３）のように示される。
３種類の式のうち２種類目は、第１実施形態の場合と同様、制御指令値が、制御対象機器９００の可動部の仕様により規定される上下限値の範囲内で設定されるという制約条件を示す式である。３種類の式のうち２種類目は、例えば上述した式（７）のように示される。

一方、３種類の式のうち３種類目は、制御対象機器９００に設定される目標が制御指令の実行によって安定に達成される度合いに関する条件を示す。
例えば、リアプノフ関数をＶで示し、制御ステップ間におけるリアプノフ関数値の変化量をΔＶで示して、制約条件を示す３種類目の式を式（１１）のようにしてもよい。

「ｄ」は、制約条件を緩和して解を得易くするために設けられている。
ｄ＝０として解を得られた場合、その解は、制御対象機器９００に設定される目標を厳密に達成するための制御指令値である。一方、ｄ＝０の場合、解をピンポイントで探索することになり、解を得られないことが懸念される。
そこで、ｄ≧０とすることで、制御指令値に基づく制御対象機器９００の動作結果と目標とのずれを許容して、解の探索範囲を広げることができる。以下では、制御対象機器９００の動作結果と目標とのずれを誤差と称する。「ｄ」の値が大きくなるほど、許容する誤差が大きくなり、解を得易くなる。

以下では、「ｄ」がｄ≧０の変数である場合の例を説明する。この場合、後述するように最適化問題における評価関数に、「ｄ」の値の大きさの評価のための項を設けるようにしてもよい。

第４実施形態で回避指令値演算部４９５が解く最適化問題における制約条件を示す３種類目の式は、式（１１）に限定されず、制御対象機器９００に設定される目標の達成度合いと、解の得易さとのトレードオフを調整可能ないろいろな式とすることができる。
例えば、第４実施形態で回避指令値演算部４９５が解く最適化問題における制約条件を示す３種類目の式を、式（１２）のようにしてもよい。

関数Ｄは、制御対象機器９００に設定される目標値と実際値とのユークリッド距離を算出する関数である。制御対象機器９００に設定される目標値がベクトルで示されているとして、回避指令値演算部４９５は、関数Ｄの演算で、そのベクトルの全ての要素について目標値と実際値との差の２乗を足し合わせて、その平方根を演算する。

関数ΔＤは、制御対象機器９００に設定される目標値と実際値とのユークリッド距離の、制御ステップ間における変化量を算出する関数である。例えば、制御ステップｔにおける関数値Ｄ（ｘ）をＤ（ｘ_ｔ）と表記し、制御ステップｔ＋１における関数値Ｄ（ｘ）をＤ（ｘ_ｔ＋１）と表記する。また、制御ステップｔから制御ステップｔ＋１までの時間における関数値Ｄ（ｘ）の変化量ΔＤ（ｘ，ｕ）は、ΔＤ（ｘ_ｔ，ｕ_ｔ）＝Ｄ（ｘ_ｔ＋１）−Ｄ（ｘ_ｔ）と示される。

式（１２）は、制御対象機器９００の安定性を保証する条件を緩和した制約式である。例えば、回避指令値演算部４９５で、ΔＤ（ｘ，ｕ）＜０を満たす制御指令ｕが常に求まれば、制御対象機器９００は安定に目標値を達成することができる。

第４実施形態で回避指令値演算部４９５が解く最適化問題における目的関数は、例えば式（１３）のように示される。

式１３の「ｕ^Ｔｕ」の項は、制御指令値ｕのベクトルとしてのノルムの２乗を示す。ここでいうノルムは、ユークリッドノルムまたはユークリッド距離とも称される。
式１３の「ｕ^Ｔｕ」の項は、制御対象機器９００が無駄な動作をしないよう、制御指令値の大きさをなるべく小さくするために設けられている。

式１３の「ｐ・ｄ^２」の項は、式（１１）または式（１２）における「ｄ」の大きさを評価するための項である。「ｐ・ｄ^２」の「ｐ」は、「ｕ^Ｔｕ」と「ｄ^２」の重み付けを調整するための重みを示す。「ｐ」は、例えばｐ＞０の定数として設定される。
この最適化問題の解の候補が２つ検出された場合、２つの解の候補の「ｕ^Ｔｕ」の項の値が同じであれば、「ｐ・ｄ^２」の項の値が小さい方が最適解として採用される。
式（１３）は、評価関数の例に該当する。式（１３）における最小解となる制御指令値ｕは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。

図１６は、障害物回避制御システム４におけるデータの流れの例を示す図である。
図１６を図４と比較すると、図１６では障害物回避制御システム４がノミナル制御装置３００を備えていない。これに伴い図１６では、情報取得装置２００からの制御対象に関する値が機器制御部４９２へ入力されている。
それ以外の点では、図１６は図４の場合と同様である。

図１７は、障害物回避制御装置４０１が制御対象機器９００に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置４０１は、制御対象機器９００に対する制御の制御ステップ毎に図１７の処理を行う。
図１７のステップＳ３１１における処理は、図５のステップＳ１１１における処理と同様である。
ステップＳ３１１の後、回避指令値演算部４９５は、上述した最適化問題の演算による制御指令値の取得を試みる（ステップＳ３１２）。
機器制御部４９２は、回避指令値演算部４９５がステップＳ３１２で最適化問題の解を得られたか否かを判定する（ステップＳ３１３）。

解を得られたと機器制御部４９２が判定した場合（ステップＳ３１３：ＹＥＳ）、処理がステップＳ３２１へ進む。ステップＳ３２１における処理は、図５のステップＳ１４１における処理と同様である。ステップＳ３２１の後、処理がステップＳ３４１へ進む。ステップＳ３４１における処理は、図５のステップＳ１６１における処理と同様である。ステップＳ３４１の後、障害物回避制御装置４０１は図１７の処理を終了する。

一方、ステップＳ３１３で解を得られなかったと機器制御部４９２が判定した場合（ステップＳ３１３：ＮＯ）、処理がステップＳ３３３１へ進む。ステップＳ３３１における処理は、図５のステップＳ１５１における処理と同様である。ステップＳ３３１の後、処理がステップＳ３４１に進む。

以上のように、障害物回避制御装置４００の場合と同様、障害物回避制御装置４０１でも、制御対象機器９００と障害物との接触を回避するための制約条件を設けることができる。制御対象機器９００に、障害物との接触を回避するための基準点が複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置４０１によれば、制御対象機器９００と障害物との接触を回避するための基準点を複数設定することができ、制御対象機器９００のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置４０１では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置４０１によれば、障害物が複数ある場合に比較的容易に対応できる。

＜第５実施形態＞
図１８は、第５実施形態に係る障害物回避制御装置の構成の例を示す図である。図１８に示す障害物回避制御装置１０は、回避指令値演算部１１と、機器制御部１２とを備える。
かかる構成で、回避指令値演算部１１は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める。機器制御部１２は、回避指令値演算部１１による処理の結果に基づいて、制御対象機器を制御する。

障害物回避制御装置１０では、制御対象機器と障害物との接触を回避するための制約条件を設けることができる。制御対象機器と障害物との接触を回避するための基準点が制御対象物に複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置１０では、制御対象機器について障害物を回避するための基準点は１か所に限定されず、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置１０では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置１０によれば、障害物が複数ある場合に比較的容易に対応できる。

＜第６実施形態＞
図１９は、第６実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。図１９に示す障害物回避制御方法は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程（ステップＳ１１）と、ステップＳ１１での処理の結果に基づいて、制御対象機器を制御する工程（ステップＳ１２）とを含む。

この障害物回避制御方法では、制御対象機器と障害物との接触を回避するための制約条件を設けることができる。制御対象機器と障害物との接触を回避するための基準点が制御対象物に複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置１０では、制御対象機器について障害物を回避するための基準点は１か所に限定されず、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。

図２０は、少なくとも１つの実施形態に係るコンピュータの構成を示す概略ブロック図である。
図２０に示す構成で、コンピュータ７００は、ＣＰＵ７１０と、主記憶装置７２０と、補助記憶装置７３０と、インタフェース７４０とを備える。
上記の情報取得装置２００、ノミナル制御装置３００、障害物回避制御装置４００、障害物回避制御装置４０１、障害物位置特定装置５００、および、障害物動作予測装置６００のうち何れか１つ以上が、コンピュータ７００に実装されてもよい。その場合、上述した各処理部の動作は、プログラムの形式で補助記憶装置７３０に記憶されている。ＣＰＵ７１０は、プログラムを補助記憶装置７３０から読み出して主記憶装置７２０に展開し、当該プログラムに従って上記処理を実行する。また、ＣＰＵ７１０は、プログラムに従って、上述した各記憶部に対応する記憶領域を主記憶装置７２０に確保する。各装置と他の装置との通信は、インタフェース７４０が通信機能を有し、ＣＰＵ７１０の制御に従って通信を行うことで実行される。

情報取得装置２００がコンピュータ７００に実装される場合、第１制御部２９０およびその各部の動作は、プログラムの形式で補助記憶装置７３０に記憶されている。ＣＰＵ７１０は、プログラムを補助記憶装置７３０から読み出して主記憶装置７２０に展開し、当該プログラムに従って上記処理を実行する。
また、ＣＰＵ７１０は、プログラムに従って、第１記憶部２８０に対応する記憶領域を主記憶装置７２０に確保する。第１通信部２１０が行う通信は、インタフェース７４０が通信機能を有し、ＣＰＵ７１０の制御に従って通信を行うことで実行される。

障害物回避制御装置４００がコンピュータ７００に実装される場合、第２制御部４９０およびその各部の動作は、プログラムの形式で補助記憶装置７３０に記憶されている。ＣＰＵ７１０は、プログラムを補助記憶装置７３０から読み出して主記憶装置７２０に展開し、当該プログラムに従って上記処理を実行する。
また、ＣＰＵ７１０は、プログラムに従って、第２記憶部４８０およびその各部に対応する記憶領域を主記憶装置７２０に確保する。第２通信部４１０が行う通信は、インタフェース７４０が通信機能を有し、ＣＰＵ７１０の制御に従って通信を行うことで実行される。

障害物回避制御装置４０１がコンピュータ７００に実装される場合、第２制御部４９０およびその各部の動作は、プログラムの形式で補助記憶装置７３０に記憶されている。ＣＰＵ７１０は、プログラムを補助記憶装置７３０から読み出して主記憶装置７２０に展開し、当該プログラムに従って上記処理を実行する。
また、ＣＰＵ７１０は、プログラムに従って、第２記憶部４８０およびその各部に対応する記憶領域を主記憶装置７２０に確保する。第２通信部４１０が行う通信は、インタフェース７４０が通信機能を有し、ＣＰＵ７１０の制御に従って通信を行うことで実行される。

障害物位置特定装置５００がコンピュータ７００に実装される場合、第３制御部５９０およびその各部の動作は、プログラムの形式で補助記憶装置７３０に記憶されている。ＣＰＵ７１０は、プログラムを補助記憶装置７３０から読み出して主記憶装置７２０に展開し、当該プログラムに従って上記処理を実行する。
また、ＣＰＵ７１０は、プログラムに従って、第３記憶部５８０およびその各部に対応する記憶領域を主記憶装置７２０に確保する。第３通信部５１０が行う通信は、インタフェース７４０が通信機能を有し、ＣＰＵ７１０の制御に従って通信を行うことで実行される。

障害物動作予測装置６００がコンピュータ７００に実装される場合、第４制御部６９０およびその各部の動作は、プログラムの形式で補助記憶装置７３０に記憶されている。ＣＰＵ７１０は、プログラムを補助記憶装置７３０から読み出して主記憶装置７２０に展開し、当該プログラムに従って上記処理を実行する。
また、ＣＰＵ７１０は、プログラムに従って、第４記憶部６８０およびその各部に対応する記憶領域を主記憶装置７２０に確保する。第４通信部６１０が行う通信は、インタフェース７４０が通信機能を有し、ＣＰＵ７１０の制御に従って通信を行うことで実行される。

なお、情報取得装置２００、ノミナル制御装置３００、障害物回避制御装置４００、障害物回避制御装置４０１、障害物位置特定装置５００、および、障害物動作予測装置６００の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。ここでいう「コンピュータシステム」とは、ＯＳ（オペレーティングシステム）や周辺機器等のハードウェアを含む。
「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ＲＯＭ（Read Only Memory）、ＣＤ−ＲＯＭ（Compact Disc Read Only Memory）等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。

以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計変更等も含まれる。

本発明の実施形態は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体に適用してもよい。

１、２、３、４ 障害物回避制御システム
１０、４００、４０１ 障害物回避制御装置
１１、４９５ 回避指令値演算部
１２、４９２ 機器制御部
１００ 撮像装置
２００ 情報取得装置
２１０ 第１通信部
２８０ 第１記憶部
２９０ 第１制御部
２９１ 状態観測部
２９２ 三次元計測部
３００ ノミナル制御装置
４１０ 第２通信部
４８０ 第２記憶部
４８１ 構造情報記憶部
４８２ 動的モデル記憶部
４８３ 制御関数記憶部
４９０ 第２制御部
４９１ 干渉関数演算部
４９３ ノミナル指令値取得部
４９４ 回避要否判定部
４９６ 仮想力場反映部
５００ 障害物位置特定装置
５１０ 第３通信部
５８０ 第３記憶部
５８１ 物体情報記憶部
５９０ 第３制御部
５９１ 種類識別部
５９２ 位置特定部
６００ 障害物動作予測装置
６１０ 第４通信部
６８０ 第４記憶部
６８１ 物体動作情報記憶部
６９０ 第４制御部
６９１ 位置予測部

本発明は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラムに関する。

本発明の目的の一例は、上記の問題を解決することができる障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラムを提供することである。

Claims (13)

1. 制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める回避指令値演算部と、
   前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御する機器制御部と、
   を備える障害物回避制御装置。
2. 前記制御対象機器による障害物回避を考慮しない場合の制御指令値であるノミナル指令値を取得するノミナル指令値取得部をさらに備え、
   前記回避指令値演算部は、求める制御指令値と前記ノミナル指令値との相違の度合いが所定の条件を満たすという前記終了条件を用いて前記回避指令値を求め、
   前記機器制御部は、前記ノミナル指令値および前記回避指令値演算部による処理の結果のうち何れか一方に基づいて、前記制御対象機器を制御する、
   請求項１に記載の障害物回避制御装置。
3. 前記制御対象機器が、前記障害物に近いほど前記障害物から離れる動作を、前記ノミナル指令値および前記回避指令値のうち少なくとも何れか一方に反映させる仮想力場反映部をさらに備え、
   前記機器制御部は、前記動作が反映された前記ノミナル指令値、および、前記動作が反映された前記回避指令値のうち何れか一方に基づいて、前記制御対象機器を制御する、請求項２に記載の障害物回避制御装置。
4. 前記制御対象機器と前記障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する干渉関数演算部をさらに備え、
   前記回避指令値演算部は、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて前記回避指令値を求める、
   請求項１から３の何れか一項に記載の障害物回避制御装置。
5. 前記制御対象機器と前記障害物との位置関係に基づいて、前記回避指令値を取得するための処理を行う必要の有無を判定する回避要否判定部をさらに備え、
   前記回避指令値演算部は、前記回避指令値を取得するための処理を行う必要があると前記回避要否判定部が判定した場合に、前記回避指令値を求める
   請求項１から４の何れか一項に記載の障害物回避制御装置。
6. 前記機器制御部は、前記回避指令値演算部が前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる、請求項１から５の何れか一項に記載の障害物回避制御装置。
7. 前記回避指令値演算部は、前記制御対象機器に設定された基準点毎に、その基準点が前記障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項１から６の何れか一項に記載の障害物回避制御装置。
8. 前記回避指令値演算部は、障害物毎に、前記制御対象機器がその障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項１から７の何れか一項に記載の障害物回避制御装置。
9. 前記回避指令値演算部は、前記制御対象機器と前記障害物とが最も近い位置における前記制御対象機器と前記障害物との距離に基づいて判定される、前記制御対象機器が前記障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項１から６の何れか一項に記載の障害物回避制御装置。
10. 請求項４に記載の障害物回避制御装置と、
    前記障害物となっている物体の種類を識別する種類識別部と、
    前記物体の種類毎に前記物体の大きさおよび形状を示す物体情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて前記障害物の位置を特定する位置特定部と、
    をさらに備え、
    前記干渉関数演算部は、特定された前記障害物の位置に基づいて、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する、
    障害物回避制御システム。
11. 前記物体の種類毎に前記物体の動きに関する情報を示す物体動作情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて前記障害物の将来の位置を予測する位置予測部をさらに備え、
    前記干渉関数演算部は、予測された前記障害物の将来の位置に基づいて、前記干渉関数値の経時変化を示す値を算出する、
    請求項１０に記載の障害物回避制御システム。
12. 制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、
    前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、
    を含む障害物回避制御方法。
13. コンピュータに、
    制御対象機器に対する制御指令値であって、前記制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程と、
    前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御する工程と、
    を実行させるためのプログラムを記録した記録媒体。

Images