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

障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム Download PDF

Info

Publication number
JP7180695B2
JP7180695B2 JP2020569234A JP2020569234A JP7180695B2 JP 7180695 B2 JP7180695 B2 JP 7180695B2 JP 2020569234 A JP2020569234 A JP 2020569234A JP 2020569234 A JP2020569234 A JP 2020569234A JP 7180695 B2 JP7180695 B2 JP 7180695B2
Authority
JP
Japan
Prior art keywords
obstacle
command value
control
avoidance
value
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
JP2020569234A
Other languages
English (en)
Other versions
JPWO2020157862A1 (ja
Inventor
博之 大山
岳大 伊藤
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
NEC Corp
Original Assignee
NEC Corp
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by NEC Corp filed Critical NEC Corp
Publication of JPWO2020157862A1 publication Critical patent/JPWO2020157862A1/ja
Application granted granted Critical
Publication of JP7180695B2 publication Critical patent/JP7180695B2/ja
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • BPERFORMING OPERATIONS; TRANSPORTING
    • B25HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25JMANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00Programme-controlled manipulators
    • B25J9/16Programme controls
    • B25J9/1656Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666Avoiding collision or forbidden zones
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00Program-control systems
    • G05B2219/30Nc systems
    • G05B2219/39Robotics, robotics to robotics hand
    • G05B2219/39091Avoid collision with moving obstacles
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05BCONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
    • G05B2219/00Program-control systems
    • G05B2219/30Nc systems
    • G05B2219/40Robotics, robotics mapping to robotics vision
    • G05B2219/40476Collision, planning for collision free path

Landscapes

  • Engineering & Computer Science (AREA)
  • Robotics (AREA)
  • Mechanical Engineering (AREA)
  • Manipulator (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Description

本発明は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラムに関する。
ロボット等の制御対象機器に障害物を回避させるように制御する技術が提案されている。例えば、特許文献1に記載のロボット制御装置は、ロボットのアームの先端部の目標位置と、先端部が目標位置に到達する前の位置である第1位置との間に障害物が存在する場合、障害物を避ける凸状の軌跡を設定する。アームの先端部は、その軌跡に従って移動する。
日本国特開2018-167333号公報
ロボットが障害物に接触する可能性がある箇所は、アームの先端部に限らない。ロボットアームの先端部に限らず、制御対象機器のいろいろな部分について、障害物を回避できることが好ましい。
本発明の目的の一例は、上記の問題を解決することができる障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラムを提供することである。
本発明の第1の態様によれば、障害物回避制御装置は、制御対象機器と障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する干渉関数演算部と、前記制御対象機器に対する制御指令値であって、前記制御対象機器が前記障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を、制御対象機器に設定されている目的の実行についての評価関数に適用した評価値が、前記目的の実行可能性に関する所定の条件を満たす制御指令値である回避指令値を、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて求める回避指令値演算部と、前記回避指令値演算部が前記回避指令値の取得に成功したと判定した場合、前前記回避指令値演算部による処理の結果に基づいて、前記制御対象機器を制御し、前記回避指令値演算部が前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる機器制御部と、を備える。
本発明の第2の態様によれば、障害物回避制御方法は、制御対象機器と障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する工程と、前記制御対象機器に対する制御指令値であって、前記制御対象機器が前記障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を、制御対象機器に設定されている目的の実行についての評価関数に適用した評価値が、前記目的の実行可能性に関する所定の条件を満たす制御指令値である回避指令値を、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて求める工程と、前記回避指令値の取得に成功したと判定した場合、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御し、前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる工程と、を含む。
本発明の第3の態様によれば、プログラムは、コンピュータに、制御対象機器と障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する工程と、前記制御対象機器に対する制御指令値であって、前記制御対象機器が前記障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を、制御対象機器に設定されている目的の実行についての評価関数に適用した評価値が、前記目的の実行可能性に関する所定の条件を満たす制御指令値である回避指令値を、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて求める工程と、前記回避指令値の取得に成功したと判定した場合、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御し、前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる工程と、を実行させるためのプログラムである。
上記した障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体によれば、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
第1実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第1実施形態に係る情報取得装置の機能構成の例を示す概略ブロック図である。 第1実施形態に係る障害物回避制御装置の機能構成の例を示す概略ブロック図である。 第1実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第1実施形態に係る障害物回避制御装置が制御対象機器に対する制御指令値を取得する処理手順の例を示すフローチャートである。 第1実施形態に係る障害物回避制御装置が干渉関数に関する値を算出する処理手順の例を示すフローチャートである。 第1実施形態に係る障害物回避制御装置が干渉関数値の変化量ΔB(x,u)を算出する処理手順の例を示すフローチャートである。 第2実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第2実施形態に係る障害物位置特定装置の機能構成の例を示す概略ブロック図である。 第2実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第3実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第3実施形態に係る障害物動作予測装置の機能構成の例を示す概略ブロック図である。 第3実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第4実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。 第4実施形態に係る障害物回避制御装置の機能構成の例を示す概略ブロック図である。 第4実施形態に係る障害物回避制御システムにおけるデータの流れの例を示す図である。 第4実施形態に係る障害物回避制御装置が制御対象機器に対する制御指令値を取得する処理手順の例を示すフローチャートである。 第5実施形態に係る障害物回避制御装置の構成の例を示す図である。 第6実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。 少なくとも1つの実施形態に係るコンピュータの構成を示す概略ブロック図である。
以下、本発明の実施形態を説明するが、以下の実施形態は請求の範囲にかかる発明を限定するものではない。また、実施形態の中で説明されている特徴の組み合わせの全てが発明の解決手段に必須であるとは限らない。
<第1実施形態>
図1は、第1実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図1に示す構成で、障害物回避制御システム1は、撮像装置100と、情報取得装置200と、ノミナル(Nominal)制御装置300と、障害物回避制御装置400とを備える。
障害物回避制御システム1は、制御対象機器900を制御する。障害物回避制御システム1は、制御対象機器900に所望の動作をさせ、かつ、制御対象機器900が障害物に接触しないように、制御対象機器900を制御する。
ここでいう所望の動作は、制御対象機器900に設定されている目標を達成する動作である。ここでいう接触することは、単に触れることに限定されず、ぶつかることを含む。制御対象機器900が障害物に接触することは、制御対象機器900の少なくとも一部分が、障害物の少なくとも一部分に接触することである。
以下では、制御対象機器900が垂直多関節ロボットである場合を例に説明するが、障害物回避制御システム1の制御対象は、制御指令値に従って動作し、障害物に接触する可能性があるいろいろな機器とすることができる。例えば、制御対象機器900が、垂直多関節ロボット以外の産業用ロボットであってもよい。
あるいは、制御対象機器900が、建築用ロボットまたは家事ロボットなど、産業用ロボット以外のロボットであってもよい。特定の用途に限定されず、形状変化を伴ういろいろなロボットを、制御対象機器900の例とすることができる。
あるいは、制御対象機器900が、無人搬送車またはドローンなどの移動体であってもよい。また、制御対象機器900が、制御指令値にて制御可能であれば、自律的に動作する装置であってもよい。
ここでいう障害物は、制御対象機器900が接触する可能性がある物である。障害物は、特定の種類のものに限定されない。例えば、障害物が、人間、他のロボット、周囲の壁または機械、一時的に置かれた荷物の何れか、あるいはこれらの組み合わせであってもよい。
また、制御対象機器900自らも障害物として扱われていてもよい。例えば、制御対象機器900が垂直多関節ロボットであり、その姿勢によってはロボットアームと台座部分とが接触する場合、障害物回避制御システム1が制御対象機器900を障害物として扱うことで、ロボットアームと台座部分との接触を回避し得る。
撮像装置100は、障害物を撮像する。例えば、撮像装置100は、制御対象機器900が動作可能な空間全体を撮像範囲内に含むように撮像を行うことで、障害物がある場合にその障害物を撮像する。
撮像装置100の撮像画像に基づいて、情報取得装置200が障害物の位置を特定する。撮像装置100として、例えばデプスカメラ(Depth Camera)、ステレオカメラ(Stereo Camera)など、3次元の情報を得られるカメラを用いるようにしてもよい。あるいは、情報取得装置200が、OpenPoseの3次元拡張など、2次元の画像から3次元の情報を得るための技術を用いるようにしてもよい。
あるいは、障害物回避制御システム1が、撮像装置100に加えて、あるいは代えて、3D-LiDAR(3-Dimension Light Detection and Ranging)など、3次元情報を得るための装置を備えるようにしてもよい。
また、撮像装置100が複数台のカメラで構成されていてもよい。撮像装置100が移動可能になっていてもよい。
撮像装置100が、観測対象の空間の状況をボクセルデータで示すようにしてもよい。
情報取得装置200は、制御対象機器900が備えるセンサなど制御対象機器900を観測するセンサからセンシングデータを取得して、制御対象機器900の位置および動作を検出する。情報取得装置200がセンシングデータを取得するセンサは、特定の種類のセンサに限定されない。例えば、情報取得装置200が、センシングデータから、制御対象機器900の各関節の、関節角度、関節角速度、関節速度、および、関節加速度のうち何れか、あるいはこれらの組み合わせの情報を取得するようにしてもよい。
情報取得装置200は、得られた情報に基づいて、制御対象機器900の位置情報、および、制御対象機器900の動きを示す情報を生成し送信する。
情報取得装置200が、制御対象機器900の位置情報をボクセルデータで送信するようにしてもよい。例えば、情報取得装置200が制御対象機器900の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置400は、1点ではなく制御対象機器900の表面と、障害物との位置関係を把握でき、制御対象機器900と障害物との距離をより正確に把握できる。制御対象機器900と障害物との距離をより正確に把握できることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置200が、制御対象機器900の位置情報として、制御対象機器900に設定されている代表点の座標を送信するようにしてもよい。
情報取得装置200は、制御対象機器900の動きを示す情報として、例えば、制御対象機器900の速度、加速度、角速度、角加速度、またはこれらの組み合わせを送信する。情報取得装置200が、制御対象機器900の表面の動きを示す情報をボクセルデータで送信するようにしてもよい。あるいは、情報取得装置200が、制御対象機器900の代表点の動きを示すデータを送信するようにしてもよい。例えば、情報取得装置200が、制御対象機器900の一般化座標qと、一般化速度q’とを並べたベクトルを送信するようにしてもよい。
あるいは、情報取得装置200が、制御対象機器の関節の角速度など、制御対象機器のアクチュエータの動きを示す情報を送信するようにしてもよい。
制御対象機器900の位置情報と制御対象機器900の動きを示す情報とを総称して制御対象機器900の状態情報と称する。
情報取得装置200は、制御対象機器の状態情報を、ノミナル制御装置300および障害物回避制御装置400へ送信する。
情報取得装置200は、障害物の位置情報を送信する。情報取得装置200が、障害物の位置情報をボクセルデータのデータ形式で送信するようにしてもよい。例えば、情報取得装置200が障害物の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置400は、1点ではなく障害物の表面と、制御対象機器900との位置関係を把握でき、制御対象機器900と障害物との距離をより正確に把握できる。制御対象機器900と障害物との距離をより正確に把握できることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置200が、制御対象機器900の位置情報として、制御対象機器900に設定されている代表点の座標を送信するようにしてもよい。
障害物が動く場合、情報取得装置200が、障害物の位置情報に加えて、障害物の動きを示す情報を送信するようにしてもよい。情報取得装置200が、障害物の動きを示す情報として、例えば、障害物の速度、加速度、角速度、角加速度、またはこれらの組み合わせを送信するようにしてもよい。情報取得装置200が、障害物の表面の動きを示す情報をボクセルデータで送信するようにしてもよい。あるいは、情報取得装置200が、障害物の代表点の動きを示すデータを送信するようにしてもよい。例えば、情報取得装置200が、障害物の一般化座標qと、一般化速度q’とを並べたベクトルを送信するようにしてもよい。
障害物の位置情報、あるいは、障害物が動く場合の、障害物の位置情報と障害物の動きを示す情報との組み合わせを、障害物の状態情報と称する。
情報取得装置200は、障害物の状態情報を障害物回避制御装置400へ送信する。
ノミナル制御装置300は、ノミナル指令値を算出する。ノミナル指令値は、制御対象機器900による障害物回避を考慮しない場合の、制御対象機器900に対する制御指令値である。すなわち、ノミナル指令値は、障害物が無いとの仮定の下で、制御対象機器900に設定された目標を達成するための、制御対象機器900に対する制御指令値である。
ノミナル制御装置300がノミナル指令値を算出するための制御方法は特定のものに限定されず、公知のいろいろな制御方法を用いることができる。
ノミナル制御装置300が算出するノミナル指令値は、障害物回避制御装置400が、制御対象機器900に対して指示する制御指令値(すなわち、実際に用いられる制御指令値)を取得するための基準となる制御指令値として用いられる。
障害物回避制御装置400は、障害物回避制御システム1による制御対象機器900の制御を実行する。したがって、障害物回避制御システム1について上述したように、障害物回避制御装置400は、制御対象機器900に所望の動作をさせ、かつ、制御対象機器900が障害物に接触しないように、制御対象機器900を制御する。
障害物回避制御装置400は、制御対象機器900を制御するための制御指令値を取得する。障害物回避制御装置400は、取得した制御指令値を制御対象機器900へ送信することで、制御対象機器900を制御する。
制御対象機器900と障害物との距離が所定の閾値よりも離れている場合、障害物回避制御装置400は、ノミナル制御装置300が算出した制御指令値を制御対象機器900への制御指令値として用いる。ノミナル制御装置300が算出した、障害物回避を考慮していない制御指令値を用いても、制御対象機器900が障害物に接触しないと期待されるからである。ノミナル制御装置300が算出した制御閾値を用いることで、障害物回避制御装置400は、障害物回避を考慮した制御閾値を新たに算出する必要が無く、この点で障害物回避制御装置400の負荷が軽くて済む。
一方、制御対象機器900と障害物との距離が所定の閾値未満である場合。ノミナル制御装置300が算出した制御指令値では、制御対象機器900が障害物に接触する可能性が比較的高い。そこで、障害物回避制御装置400は、制御対象機器900が障害物を回避し、かつ、目標を達成するための制御指令値を算出し、算出した制御指令値を制御対象機器900へ送信する。
図2は、情報取得装置200の機能構成の例を示す概略ブロック図である。図2に示す構成で、情報取得装置200は、第1通信部210と、第1記憶部280と、第1制御部290とを備える。第1制御部290は、状態観測部291と、三次元計測部292とを備える。
第1通信部210は、他の装置と通信を行う。特に、第1通信部210は、制御対象機器900が備えるセンサからのセンシングデータを受信する。また、第1通信部210は、撮像装置100からのデータを受信する。第1通信部210は、撮像装置100の撮像画像の画像データを受信し、さらに、奥行き方向の距離情報など付加情報がある場合は、画像データに加えて付加情報を受信する。
また、第1通信部210は、情報取得装置200が生成する制御対象機器900の位置情報および動作情報をノミナル制御装置300および障害物回避制御装置400へ送信する。また、第1通信部210は、情報取得装置200が生成する障害物の位置情報を障害物回避制御装置400へ送信する。情報取得装置200が障害物の動作も特定する場合、第1通信部210は、障害物の動作情報(例えば、速度または加速度の情報)を障害物回避制御装置400へ送信する。
第1記憶部280は、各種データを記憶する。第1記憶部280の機能は、情報取得装置200が備える記憶デバイスを用いて実行される。
第1制御部290は、情報取得装置200の各部を制御して各種処理を実行する。第1制御部290の機能は、情報取得装置200が備えるCPU(Central Processing Unit、中央処理装置)が、第1記憶部280からプログラムを読み出して実行することで実行される。
状態観測部291は、制御対象機器900が備えるセンサからのセンシングデータに基づいて、制御対象機器900の位置および動作を特定する。状態観測部291は、特定した制御対象機器900の位置を示す座標値を算出する。状態観測部291は、制御対象機器900の位置および動作を示すデータを、第1通信部210を介してノミナル制御装置300および障害物回避制御装置400へ送信する。すなわち、状態観測部291は、制御対象機器900の状態情報を、第1通信部210を介してノミナル制御装置300および障害物回避制御装置400へ送信する。
三次元計測部292は、撮像装置100からのデータに基づいて、障害物の位置を特定する。三次元計測部292は、特定した位置を示す座標値を算出し、算出したデータを、第1通信部210を介して障害物回避制御装置400へ送信する。また、障害物が動く場合、三次元計測部292が、障害物の動作情報(例えば、速度または加速度の情報)を算出し、算出したデータを、第1通信部210を介して障害物回避制御装置400へ送信するようにしてもよい。このように、三次元計測部292は、障害物の状態情報を、第1通信部210を介して障害物回避制御装置400へ送信する
図3は、障害物回避制御装置400の機能構成の例を示す概略ブロック図である。図3に示す構成で、障害物回避制御装置400は、第2通信部410と、第2記憶部480と、第2制御部490とを備える。第2記憶部480は、構造情報記憶部481と、動的モデル記憶部482とを備える。第2制御部490は、干渉関数演算部491と、機器制御部492とを備える。機器制御部492は、ノミナル指令値取得部493と、回避要否判定部494と、回避指令値演算部495と、仮想力場反映部496とを備える。
第2通信部410は、他の装置と通信を行う。特に、第2通信部410は、情報取得装置200が送信する、制御対象機器900の状態情報および障害物の状態情報を受信する。また、第2通信部410は、ノミナル制御装置300が算出して送信する制御指令値を受信する。また、第2通信部410は、制御対象機器900へ制御指令値を送信する。障害物回避制御装置400は、制御対象機器900へ制御指令値を送信することで制御対象機器900を制御する。
第2記憶部480は、各種データを記憶する。第2記憶部480の機能は、障害物回避制御装置400が備える記憶デバイスを用いて実行される。
構造情報記憶部481は、制御対象機器900の構造情報を記憶する。制御対象機器900の構造情報は、制御対象機器900のアームの長さおよび太さ、関節が動く方向および可動範囲など、制御対象機器900の形状および動作に関する情報である。制御対象機器900のCAD(Computer Aided Design)データがある場合、そのCADデータを構造情報として用いるようにしてもよい。
動的モデル記憶部482は、制御対象機器900の動的モデルを記憶する。制御対象機器900の動的モデルは、制御対象機器900の状態情報、および、制御指令値の入力を受けて制御対象機器900がその制御指令値に従う場合の動作を模擬する。
動的モデルが、将来の時点における制御対象機器900の予測位置の位置情報を出力するようにしてもよい。あるいは、動的モデルが、制御対象機器900の動作量を出力するようにしてもよい。すなわち、動的モデルが、制御対象機器900の将来の予測位置から現在位置を減算した差分を出力するようにしてもよい。
動的モデルは、制御指令値uの入力に対して、制御対象機器900の状態情報xで示される状態の微分値または差分が求まるモデルであり、例えば状態空間モデルであってもよい。
制御対象機器900のセンサからの情報と、構造情報記憶部481が記憶する構造情報と、動的モデル記憶部482が記憶する動的モデルとを用いることで、制御対象機器900が占有する空間を特定することができる。
特に、制御対象機器900の構造情報にアームの太さ等の情報が含まれていることで、障害物回避制御装置400は、制御対象機器900の骨格の位置のみでなく、アームの太さ等を考慮した位置情報を取得できる。すなわち、障害物回避制御装置400は、制御対象機器900の位置情報を、空間的な広がりを有する位置として把握できる。
また、障害物回避制御装置400は、制御対象機器900の動的モデルを用いることで、制御対象機器900の動作をより正確に把握できる。これにより、制御対象機器900が障害物に接触するか否か、接触する場合は制御対象機器900がどのように動いて接触するかを判定でき、制御対象機器900による障害物回避をより高精度に行わせることができる。
第2制御部490は、障害物回避制御装置400の各部を制御して各種処理を実行する。第2制御部490の機能は、障害物回避制御装置400が備えるCPUが第2記憶部480からプログラムを読み出して実行することで実行される。
干渉関数演算部491は、干渉関数値、および、干渉関数値の経時変化を示す値を算出する。干渉関数は、制御対象機器900と障害物との位置関係に応じた値を示す。干渉関数Bは、式(1)のような値をとる。
Figure 0007180695000001
式(1)では、xは、制御対象機器900の状態情報を示す。例えば、情報取得装置200が、制御対象機器900の表面の位置情報をボクセルデータで送信し、干渉関数演算部491は、制御対象機器900の状態情報を干渉関数Bに適用することで、制御対象機器900と障害物とが最も近い位置における両者の距離を算出するようにしてもよい。
以下では、干渉関数値B(x)が、制御対象機器900の状態情報xが示す制御対象機器900の位置と障害物との距離を示すものとする。障害物が複数ある場合は、制御対象機器900の位置に最も近い障害物との距離を示すものとする。通常、制御対象機器900は障害物の中には入らないので、制御対象機器900が障害物の内部に位置する場合の干渉関数値B(x)は定義されていなくてもよい。
干渉関数値B(x)により、制御対象機器900が障害物に接触しているか否か、および、制御対象機器900と障害物との距離が示される。
干渉関数演算部491は、障害物の状態情報に基づいて干渉関数を生成し、第2記憶部480に記憶させておく。
制御対象機器900が動作して制御対象機器900の位置が経時変化することで、干渉関数値B(x)も経時変化する。この場合、干渉関数演算部491は、干渉関数値B(x)の経時変化を示す値として、制御ステップ間における干渉関数値B(x)の変化量を算出する。
ここでの制御ステップは、障害物回避制御装置400が制御対象機器900に対して制御指令値を1回送信するための一連の処理のステップである。すなわち、障害物回避制御装置400は、周期的な制御ステップ毎に制御対象機器900に対して制御指令値を送信する。
干渉関数演算部491は、今回の制御ステップから次回の制御ステップまでの間の、干渉関数値B(x)の変化量を予測する。制御ステップ間における干渉関数値の変化量をΔB(x,u)と表記する。干渉関数値B(x)の変化量は制御対象機器900の位置の変化に依存し、制御対象機器900の位置の変化は制御指令値uに依存するため、制御指令値uを明示している。
また、干渉関数演算部491は、制御対象機器900の位置に対するポテンシャル関数値P(x)を算出する。ポテンシャル関数値P(x)は、制御対象機器900と障害物との距離に基づく仮想的な力場の値である。干渉関数演算部491は、制御対象機器900の状態情報に示される制御対象機器900の位置が障害物に近いほど(従って、干渉関数値B(x)が小さいほど)ポテンシャル関数値P(x)を大きく算出する。
機器制御部492は、ノミナル制御装置300からのノミナル指令値、および、回避指令値演算部495による処理の結果のうち何れか一方に基づいて、制御対象機器900を制御する。上述したように、ノミナル指令値は、障害物回避を考慮していない制御指令値である。一方、回避指令値演算部495は、制御対象機器900が障害物を回避できるような制御指令値を求める。
制御対象機器900と障害物との距離が所定の閾値より大きいと判定された場合、機器制御部492は、ノミナル制御装置300からのノミナル指令値、または、ノミナル指令値に対して仮想力場反映部496による補正を加えた制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
一方、制御対象機器900と障害物との距離が所定の閾値以下であると判定された場合、回避指令値演算部495が、障害物を回避できる制御指令値の算出を試みる。回避指令値演算部495が制御指令値の算出に成功した場合、機器制御部492は、回避指令値演算部495が算出した制御指令値、または、その制御指令値に対して仮想力場反映部496による補正を加えた制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
一方、回避指令値演算部495が制御指令値の算出に失敗した場合、すなわち、障害物を回避できる制御指令値を得られなかった場合、機器制御部492は、制御対象機器900を減速させる制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
ノミナル指令値取得部493は、ノミナル指令値を取得する。具体的には、ノミナル指令値取得部493は、第2通信部410の受信信号の中から、ノミナル制御装置300からのノミナル指令値を抽出する。
回避要否判定部494は、制御対象機器900と障害物との位置関係に基づいて、回避指令値演算部495が制御指令値を取得するための処理を行う必要の有無を判定する。上記のように、回避要否判定部494は、制御対象機器900と障害物との距離と閾値とを比較して、距離が閾値より大きいか否かを判定する。具体的には、回避要否判定部494は、干渉関数値B(x)と所定の定数の閾値αとを比較し、B(x)>αか否かを判定する。
B(x)>αである場合、制御対象機器900の位置が障害物から比較的離れている。この場合、回避要否判定部494は、回避指令値演算部495が制御指令値を取得するための処理を行う必要は無いと判定する。一方、B(x)≦αである場合、制御対象機器900の位置が障害物に比較的近い。この場合、回避要否判定部494は、回避指令値演算部495が制御指令値を取得するための処理を行う必要があると判定する。
回避指令値演算部495は、回避指令値を求める。回避指令値は、制御対象機器900に対する制御指令値であって、制御対象機器900が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である。特に、障害物回避制御装置400が回避指令値を用いて制御対象機器900を制御することで、制御対象機器900が障害物に接触しないように制御できる。
回避指令値演算部495は、求める解としての制御指令値とノミナル指令値との相違の度合いが所定の条件を満たすという終了条件を用いて回避指令値を求める。例えば、回避指令値演算部495は、上記の制約条件の下で、求める解としての制御指令値とノミナル指令値との相違の度合いを最小化する最適化問題を解くことで、回避指令値を求める。
この場合、回避指令値演算部495が解く最適化問題における評価関数(目的関数とも称される)は、例えば、式(2)のように示される。
Figure 0007180695000002
「u」は、この最適化問題の解となる制御指令値を示す。「argmin」は、引数の値を最小化する関数である。式(2)の場合、「argmin」は、引数「(u-u(u-u)」を最小化するような、uの値を関数値とする。
ベクトルまたは行列に上付の「T」は、ベクトルまたは行列の転置を示す。
「u」は、ノミナル制御装置300からのノミナル指令値を示す。
式(2)は、制約条件の下で、ノミナル指令値uになるべく近い制御指令値を求めることを示している。ノミナル指令値uは、制御対象機器900に設定されている目的を制御対象機器900に実行させるように算出された指令値なので、ノミナル指令値uに近い指令値を求めることで、制御対象機器900に設定されている目的を制御対象機器900に実行させることができると期待される。
制御指令値を示すu、u、uのデータ形式は、同じ次元のベクトルであるとする。これらのベクトルの次元数は、障害物回避制御装置400が制御対象機器900へ送信する制御指令値の次元数と同じとする。
式(2)は、評価関数の例に該当する。式(2)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
回避指令値演算部495が解く最小化問題における制約条件は、2種類の式で示される。2種類の式のうち1種類目は、式(3)のように示される。
Figure 0007180695000003
γは、0≦γ<1の定数である。
γの値によって、制御対象機器900と障害物とが接触しないための、制御対象機器900と障害物との間隔の余裕分をどれだけ見込むか調整できる。
通常、制御対象機器900と障害物とは接触しておらず、B(x)が、制御対象機器900と障害物との距離を示す。制御対象機器900が障害物に接近してΔB(x,u)が負の値をとるとき、ΔB(x,u)の大きさがγB(x)以下である場合に式(3)が成り立つ。
このことから、B(x)で示される制御対象機器900と障害物との距離のうち、(1-γ)B(x)の分は、制御対象機器900と障害物とが接触しないための余裕分として、制御対象機器900の動作可能範囲から除外されているといえる。γの値を大きく設定するほど、制御対象機器900の動作可能範囲が広がる。一方、γの値を小さく設定するほど、制御対象機器900と障害物とが接触しないための余裕分を大きくとることになり、例えば予期しない外力によって制御対象機器900が障害物のほうへ押された場合でも、制御対象機器900が障害物にぶつかりにくい。
式(3)に示されるように、回避指令値演算部495は、干渉関数値、および、干渉関数値の経時変化を示す値を用いて回避指令値を求める。
複数の障害物が存在する場合、障害物毎に式(3)の制約条件を設けることができる。これにより、障害物回避制御装置400は、制御対象機器900が障害物全体に接触しないよう制御することができる。あるいは、複数の障害物をまとめたものに対して干渉関数を設計してもよい。
式(3)は、制御対象機器900が今回の制御ステップで障害物に接触していなければ、次の制御ステップでも障害物に接触しないことの十分条件を示す。この点について説明する。
今回の制御ステップをtで示し、制御ステップtの次の制御ステップをt+1で示す。制御ステップtにおける干渉関数値をB(x)と表記する。
制御ステップt+1における干渉関数値をB(xt+1)と表記する。B(xt+1)からB(x)を減算した差分を、ΔB(x,u)と表記する。ΔB(x,u)は、式(4)のように示される。
Figure 0007180695000004
また、式(3)より式(5)を得られる。
Figure 0007180695000005
式(4)および式(4)より式(6)を得られる。
Figure 0007180695000006
0≦γ<1なので、B(x)>0の場合、B(x)-γB(x)≧0であり、B(xt+1)>0である。従って、制御ステップtで制御対象機器900の位置が障害物の外部にあれば、制御ステップt+1でも制御対象機器900の位置は障害物の外部にある。
式(3)の制約条件の判定に、制御対象機器900の表面の位置情報が用いられる場合、回避指令値演算部495は、式(3)の条件の成否をより正確に判定できる。特に、制御対象機器900の表面の位置情報と、障害物の表面の位置情報とが用いられる場合、回避指令値演算部495は、干渉関数Bを用いて制御対象機器900と障害物とが最も近い位置での両者の距離を得られる。
回避指令値演算部495は、この距離を用いて式(3)の条件を判定することで、より正確に条件判定を行うことができる。この点で、回避指令値演算部495が、より高精度な制御指令値を得られ、機器制御部492が、より高精度に制御対象機器900を制御できると期待される。
式(3)の制約条件の判定に制御対象機器900の表面の位置情報が用いられる場合の例として、情報取得装置200が、制御対象機器900の表面の位置情報を例えばボクセルデータで障害物回避制御装置400へ送信する場合が挙げられる。
式(3)の制約条件の判定に障害物の表面の位置情報が用いられる場合の例として、情報取得装置200が、障害物の表面の位置情報を例えばボクセルデータを障害物回避制御装置400へ送信し、干渉関数演算部491が、その位置情報を用いて干渉関数を生成する場合が挙げられる。
一方、式(3)の判定に制御対象機器900に設定された基準点の位置情報が用いられる場合、基準点毎に式(3)の条件式を設けることができる。障害物回避制御装置400では、この点で、基準点が複数の場合に比較的容易に対応できる。
障害物が複数ある場合も、障害物毎に式(3)の条件式を設けることができる。障害物回避制御装置400では、この点で、障害物が複数の場合に比較的容易に対応できる。
回避指令値演算部495が解く最小化問題における制約条件を示す2種類の式のうち2種類目は、式(7)のように示される。
Figure 0007180695000007
(iは、1≦i≦Nの整数)は、例えば、制御対象機器900の関節毎など、制御対象機器900の可動部分毎の制御指令値を示すスカラ値である。Nは、制御対象機器900の可動部分の個数を示す。iは、可動部分を識別する識別番号である。
識別番号iで識別される可動部分をi番目の可動部分と表記する。従って、uは、i番目の可動部分に対する制御指令値である。
i_min、ui_maxは、それぞれ、制御対象機器900の仕様により予め定められている、uの下限値、上限値である。
式(7)は、いずれの制御指令値も、可動部分の仕様により規定される上下限値の範囲内で設定されるという制約条件を示している。可動部分の仕様は、例えば、その可動部分に用いられているアクチュエータの仕様により規定される。
制御指令値uは、u(i=1,2,・・・,N)を纏めてベクトルで表したものである。
上述したように、回避指令値演算部495は、回避指令値を取得するための処理が必要であると回避要否判定部494が判定した場合に、回避指令値を求める。
仮想力場反映部496は、制御対象機器900が障害物に近いほど障害物から離れる動作を、ノミナル指令値および回避指令値のうち少なくとも何れか一方に反映させる。
制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど大きい加速度で障害物から離れることであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど、障害物から離れる距離が長いことであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900と障害物との距離が所定の閾値以下である場合のみ、制御対象機器900が障害物から離れることであってもよい。
例えば、仮想力場反映部496が、制御対象機器900と障害物との位置関係に基づく仮想的な力場を模擬するようにしてもよい。この場合、仮想力場反映部496が、制御対象機器900と障害物との距離が近いほど反発力を大きく算出してもよい。制御対象機器900に生じさせる反発力が大きいほど、制御対象機器900の加速度が大きくなり、制御対象機器900が障害物から離れる移動量の大きさが大きくなる。
例えば、仮想力場反映部496が算出する制御指令値をuと表記する。制御指令値uが制御対象機器900に設定された基準点の移動量を示すとすると、u=(Δx,Δy,Δz)と示される。
障害物を質点(Point Mass)で代表し、障害物の座標を(x,y,z)としてもよい。そして、仮想力場反映部496が、座標(x,y,z)に位置する制御対象機器900に対して、障害物から遠ざからせるための制御指令値uを、式(8)に基づいて算出するようにしてもよい。
Figure 0007180695000008
P(x,y,z)はポテンシャル関数を示す。∇はナブラを示す。P(x,y,z)は、式(9)のように示される。
Figure 0007180695000009
なお、式(1)、式(3)から式(6)、および、式(10)から式(12)でxが制御対象機器900の3次元の位置を示すのと異なり、式(8)および式(9)では、(x,y,z)が座標を示す。したがって、式(1)、式(3)から式(6)、および、式(10)から式(12)でxがベクトル量または行列を示すのと異なり、式(8)および式(9)ではxはスカラ量を示す。
仮想力場反映部496が、例えばロボットアームの先端など代表的な1点について、指令値としての制御対象機器900の移動量を算出し、算出した移動量を各関節に分散させるようにしてもよい。あるいは、仮想力場反映部496が、ロボットの関節毎など制御対象機器900の複数部分の各々について、指示する移動量を算出するようにしてもよい。
仮想力場反映部496が、算出した指令値を制御指令値に反映させることで、制御対象機器900と障害物との接触を、より確実に防止できる。
以下では、仮想力場反映部496が、ノミナル指令値および回避指令値の両方に、算出した指令値を反映させる場合を例に説明する。
但し、仮想力場反映部496の処理は障害物回避制御装置400に必須ではない。従って、障害物回避制御装置400が仮想力場反映部496を備えていない構成となっていてもよい。
図4は、障害物回避制御システム1におけるデータの流れの例を示す図である。図4では、障害物に符号950を付している。障害物950は、上記で説明した障害物と同様である。
上述したように、制御対象機器900のセンサのセンシングデータは、情報取得装置200の状態観測部291へ入力される。
撮像装置100の画像データは、情報取得装置200の三次元計測部292へ入力される。撮像装置100の付加情報がある場合、画像データに加えて付加情報も三次元計測部292へ入力される。
情報取得装置200は、状態観測部291が算出する制御対象機器900の状態情報を、ノミナル制御装置300および障害物回避制御装置400へ送信する。例えば、情報取得装置200は、制御対象機器900の表面の位置情報をボクセルデータで送信する。加えて、情報取得装置200は、制御対象機器900の表面または基準点の速度、加速度、角速度または角加速度など、制御対象機器900の動作を示す情報を送信する。例えば、情報取得装置200が、一般化座標qと、一般化速度q’とを並べたベクトルを送信するようにしてもよい。
また、情報取得装置200は、障害物950の状態情報を障害物回避制御装置400へ送信する。特に情報取得装置200は、障害物950の3次元の位置情報を障害物回避制御装置400へ送信する。情報取得装置200が障害物950の位置情報を送信するデータ形式は、特定のものに限定されない。例えば、情報取得装置200が、障害物950の位置情報を、ボクセル(Voxel)データにて送信するようにしてもよい。あるいは、情報取得装置200が、障害物950の位置情報を、サーフェイス(Surface)データにて送信するようにしてもよい。あるいは、情報取得装置200が、障害物950の位置情報を、ポイントクラウド(Point Cloud)データにて送信するようにしてもよい。
ノミナル制御装置300は、情報取得装置200から受信する、制御対象機器900の状態情報を用いて、ノミナル指令値uを算出する。ノミナル制御装置300は、算出したノミナル指令値uを障害物回避制御装置400へ送信する。
障害物回避制御装置400の干渉関数演算部491は、情報取得装置200から得られた制御対象機器900の状態情報および障害物の状態情報と、機器制御部492が設定する制御指令値とに基づいて、干渉関数値B(x)、制御ステップ間におけるB(x)の変化量ΔB(x,u)、および、ポテンシャル関数値P(x)を算出する。上記のように、仮想力場反映部496の処理は必須ではない。従って、干渉関数演算部491によるポテンシャル関数値の算出は必須ではない。
障害物回避制御装置400の機器制御部492は、ノミナル制御装置300から得られるノミナル指令値uと、干渉関数演算部491から得られる干渉関数値B(x)、干渉関数値の変化量ΔB(x,u)、および、ポテンシャル関数値P(x)とを用いて、制御対象機器900に対する制御指令値uを取得する。機器制御部492は、取得した制御指令値uを、第2通信部410を介して制御対象機器900へ送信することで、制御対象機器900を制御する。
図5から図7を参照して、障害物回避制御装置400の動作について説明する。
図5は、障害物回避制御装置400が制御対象機器900に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置400は、制御対象機器900に対する制御の制御ステップ毎に図5の処理を行う。
図5の処理で、仮想力場反映部496は、干渉関数演算部491が算出するポテンシャル関数値P(x)に基づいて、力場を模擬するための制御指令値uを算出する(ステップS111)。制御指令値uは、ノミナル指令値または回避指令値に値を付加する補正項として用いられる。
また、回避要否判定部494は、干渉関数演算部491が算出する干渉関数値B(x)が、閾値αより大きいか否かを判定する(ステップS112)。
干渉関数値B(x)が閾値αより大きいと判定した場合(ステップS112:YES)、機器制御部492は、制御対象機器900に対する制御指令値uを、ノミナル指令値uに制御指令値uを加えた値と算出する(ステップS121)。すなわち、機器制御部492は、u=u+uと算出する。
そして、機器制御部492は、制御指令値uを、第2通信部410を介して制御対象機器900へ送信することで、制御対象機器900を制御する(ステップS161)。
ステップS161の後、障害物回避制御装置400は、図5の処理を終了する。
一方、ステップS112で干渉関数値B(x)が閾値α以下であると回避要否判定部494が判定した場合(ステップS112:NO)、回避指令値演算部495は、上述した最適化問題の演算による制御指令値の取得を試みる(ステップS131)。
機器制御部492は、回避指令値演算部495がステップS131で最適化問題の解を得られたか否かを判定する(ステップS132)。
解を得られたと判定した場合(ステップS132:YES)、機器制御部492は、制御対象機器900に対する制御指令値uを、最適化問題の解による制御指令値uに制御指令値uを加えた値と算出する(ステップS141)。すなわち、機器制御部492は、u=u+uと算出する。
ステップS141の後、処理がステップS161へ進む。
一方、ステップS132の判定で、最適化問題の解を得られていないと判定した場合(ステップS132:NO)、機器制御部492は、制御対象機器900を減速させるように制御指令値uを生成する(ステップS151)。
ステップS151の後、処理がステップS161へ進む。
図6は、障害物回避制御装置400が干渉関数に関する値を算出する処理手順の例を示すフローチャートである。障害物回避制御装置400は、制御対象機器900に対する制御の制御ステップ毎に、図5の処理の前処理として図6の処理を行う。
図6の処理で、干渉関数演算部491は、情報取得装置200が送信する制御対象機器900の情報、および、構造情報記憶部481が記憶している制御対象機器900の構造情報等を用いて、制御対象機器900の占有空間の位置情報を算出する(ステップS211)。干渉関数演算部491は、制御対象機器900の占有空間の位置情報を、制御対象機器900の座標系による座標値で算出する。ここでいう制御対象機器900の座標系は、例えば、障害物回避制御装置400が制御対象機器900に送信する制御指令値に用いられる座標値である。
次に干渉関数演算部491は、障害物の位置情報を取得する(ステップS212)。三次元計測部292が、障害物の位置を検出して制御対象機器900の座標系による位置情報を生成し、干渉関数演算部491が、その位置情報を用いるようにしてもよい。あるいは、三次元計測部292が、障害物の位置情報を、制御対象機器900の座標系とは異なる座標系の座標値で生成し、干渉関数演算部491が、制御対象機器900の座標系の座標値に座標変換するようにしてもよい。
次に、干渉関数演算部491は、制御対象機器900の位置情報および障害物の位置情報に基づいて、干渉関数値B(x)を算出する(ステップS213)。
また、干渉関数演算部491は、制御対象機器900の位置情報、制御対象機器900の動作情報、および、障害物の位置情報に基づいて、干渉関数値の変化量ΔB(x,u)を算出する(ステップS214)。
干渉関数演算部491が、制御対象機器900の動作情報として、状態観測部291が制御対象機器900のセンサのセンシングデータから算出する、制御対象機器900の動作情報を用いるようにしてもよい。あるいは、干渉関数演算部491が、制御対象機器900の動作情報として、前回の制御ステップでの制御対象機器900への制御指令値uを用いるようにしてもよい。あるいは、干渉関数演算部491が、制御対象機器900の動作情報として、これらの両方を用いるようにしてもよい。
ステップS214の後、障害物回避制御装置400は、図6の処理を終了する。
図7は、障害物回避制御装置400が干渉関数値の変化量ΔB(x,u)を算出する処理手順の例を示すフローチャートである。障害物回避制御装置400は、図6のステップS214で図7の処理を行う。
図7の処理で、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出可能か判定する(ステップS221)。
算出可能と判定した場合(ステップS221;YES)、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出する(ステップS231)。
ステップS231の後、障害物回避制御装置400は、図7の処理を終了する。
一方、ステップS221で、動的モデルの演算でΔB(x,u)を算出できないと判定した場合(ステップS221:NO)、干渉関数演算部491は、制御指令値uの微小変化を用いてΔB(x,u)を算出する。例えば、干渉関数演算部491は、式(10)を用いてΔB(x,u)を近似的に算出する。
Figure 0007180695000010
Δtは、制御ステップ間の時間間隔を示す。B(x,u)は、干渉関数値を示す。制御指令値uを変更すると制御対象機器900の動作が変わり干渉関数値が変わるので、干渉関数Bを制御指令値uの関数として表記している。
ステップS241の後、障害物回避制御装置400は図7の処理を終了する。
ステップS231の処理とステップS241の処理とを比較すると、ステップS231の処理では、uの微小な変化を用いた試行、および、式(10)のような近似を用いる必要が無い。この点で、ステップS231の処理のほうがステップS241の処理よりも、得られるΔB(x,u)の精度が高いと期待される。一方、ステップS231でΔB(x,u)を算出できない場合でも、ステップS241の処理によれば算出できると期待される。
そこで、干渉関数演算部491は、ステップS231の処理でΔB(x,u)を算出可能か判定し、算出可能であればステップS231の処理を行う。一方、ステップS231の処理ではΔB(x,u)を算出できない場合、干渉関数演算部491は、ステップS241の処理を行う。
以上のように、ノミナル指令値取得部493は、ノミナル指令値を取得する。ノミナル指令値は、制御対象機器900に対する指令値を求めるために用いられる、いわば基準となる制御指令値である。回避指令値演算部495は、回避指令値を求める。回避指令値は、式(3)のように、制御対象機器900が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、式(2)のように、ノミナル指令値との相違の度合いの小ささに関する所定の条件を満たす制御指令値である。機器制御部492は、ノミナル指令値、および、回避指令値演算部495による処理の結果のうち何れか一方に基づいて、制御対象機器900を制御する。
障害物回避制御装置400では、制御対象機器900と障害物との接触を回避するための制約条件を設けることができる。制御対象機器900と障害物との接触を回避するための基準点が制御対象機器900に複数設定される場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置400では、制御対象機器900について障害物を回避するように制御する基準点は1か所に限定されず、制御対象機器900のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置400では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置400によれば、障害物が複数ある場合に比較的容易に対応できる。
また、ノミナル指令値取得部493は、ノミナル指令値としてノミナル指令値を取得する。ノミナル指令値は、制御対象機器900による障害物回避を考慮しない場合の制御指令値である。
ノミナル指令値は、制御対象機器900に設定されている目標を達成するように設定される指令値であり、障害物回避制御装置400が、障害物回避を考慮した回避指令値をノミナル指令値に近付けるように生成することで、目標を達成できると期待される。
障害物回避制御装置400によれば、このように、回避指令値を生成する際にノミナル指令値を基準として用いることができ、この点で、回避指令値を比較的容易に生成できる。
また、干渉関数演算部491は、干渉関数値(B(x))、および、干渉関数値の経時変化を示す値(ΔB(x,u))を算出する。干渉関数値は、制御対象機器900と障害物との位置関係に応じた値を示す。回避指令値演算部495は、干渉関数値、および、干渉関数値の経時変化を示す値を用いて、回避指令値を求める。
障害物回避制御装置400によれば、これらの値を用いることで、制御対象機器900が障害物に接触する可能性の度合いを推定することができ、必要に応じて障害物回避等の措置をとることができる。
障害物回避制御装置400では、制御対象機器900と障害物との距離を干渉関数として用いることができ、制御対象機器900の位置と障害物の位置とから容易に関数値を算出できる。この点で、障害物回避制御装置400の負荷が軽くて済む。
また、回避要否判定部494は、制御対象機器900と障害物との位置関係に基づいて、回避指令値を取得するための処理の要否を判定する。回避指令値演算部495は、回避指令値を取得するための処理が必要であると回避要否判定部494が判定した場合に、回避指令値を求める。
回避指令値を取得するための処理が不要であると回避要否判定部494が判定した場合、ノミナル制御装置300が生成したノミナル指令値を制御対象機器900への指令値として用いることができ、障害物回避制御装置400が新たに指令値を生成する必要が無い。この点で、障害物回避制御装置400の負荷が比較的軽くて済む。
また、機器制御部492は、回避指令値演算部495が回避指令値の取得に失敗したと判定した場合、制御対象機器900を減速させる。
回避指令値演算部495が回避指令値の取得に失敗した場合、制御対象機器900と障害物との接触を回避できない可能性がある。そこで、機器制御部492が制御対象機器900を減速させる。これにより、制御対象機器900が障害物に接触するまでの時間を確保でき、例えば人手で障害物を動かすといった対応が可能である。また、制御対象機器900が障害物に接触した場合でも、制御対象機器900が減速していることで、不具合が生じないか、あるいは、不具合の程度が軽くて済むと期待される。
また、仮想力場反映部496は、制御対象機器900が障害物に近いほど大きい加速度で障害物から離れる動作を、ノミナル指令値および回避指令値のうち少なくとも何れか一方に反映させる。機器制御部492は、この動作が反映されたノミナル指令値、および、この動作が反映された回避指令値のうち何れか一方に基づいて、制御対象機器900を制御する。
これにより、制御対象機器900は、障害物に近いほど、障害物から離れるように動作する。障害物回避制御装置400によれば、この点で、より確実に、制御対象機器900に障害物を回避させることができる。
また、回避指令値演算部495は、制御対象機器900に設定された基準点毎に、その基準点が障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、障害物回避制御装置400では、基準点毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、制御対象機器に基準点が複数設定されている場合に、比較的容易に対応できる。
また、回避指令値演算部495は、障害物毎に、制御対象機器900がその障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、障害物回避制御装置400では、障害物毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、障害物が複数ある場合に、比較的容易に対応できる。
また、回避指令値演算部495は、制御対象機器900と障害物とが最も近い位置における制御対象機器900と障害物との距離に基づいて判定される、制御対象機器900が障害物に接触しないための十分条件を満たす回避指令値を求める。
このように、回避指令値演算部495が、制御対象機器900と障害物とが最も近い位置における制御対象機器900と障害物との距離を用いて回避指令値を求めることで、制御対象機器900と障害物との接触の可能性をより正確に反映した回避指令値を求めることができる。障害物回避制御装置400によればこの点で、制御対象機器900の制御を比較的高精度に行うことができる。
<第2実施形態>
図8は、第2実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図8に示す構成で、障害物回避制御システム2は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
障害物回避制御システム2は、障害物位置特定装置500を備える点で障害物回避制御システム1の場合と異なる。それ以外の点では、障害物回避制御システム2は障害物回避制御システム1と同様である。障害物回避制御システム2について、障害物回避制御システム1と同様の点の説明を省略し、必要に応じて図2に示す符号、および、図3に示す符号を引用する。
図9は、障害物位置特定装置500の機能構成の例を示す概略ブロック図である。図9に示す構成で、障害物位置特定装置500は、第3通信部510と、第3記憶部580と、第3制御部590とを備える。第3記憶部580は、物体情報記憶部581を備える。第3制御部590は、種類識別部591と、位置特定部592とを備える。
第3通信部510は、他の装置と通信を行う。特に、第3通信部510は、情報取得装置200からの障害物の位置情報を受信する。また、第3通信部510は、位置特定部592の処理が反映された障害物の位置情報を障害物回避制御装置400へ送信する。第3通信部510が、情報取得装置200から受信した情報に加えて、位置特定部592が算出した障害物の位置情報を障害物回避制御装置400へ送信するようにしてもよい。あるいは、第3通信部510が、情報取得装置200から受信した情報に対して、障害物の位置情報を位置特定部592が算出した位置情報に置き換えた情報を、障害物回避制御装置400へ送信するようにしてもよい。
第3記憶部580は、各種データを記憶する。第3記憶部580の機能は、障害物位置特定装置500が備える記憶デバイスを用いて実行される。
物体情報記憶部581は、物体情報を記憶する。物体情報記憶部581は、物体の種類を識別する種類識別情報と、物体の形状および大きさなど物体の位置を特定するための情報とが紐付けられた情報を、物体情報として記憶する。
ここでの物体の種類は識別可能なものであればよく、特定のものに限定されない。例えば、物体情報記憶部581が、ペットボトル、コップなど、物体の名称毎の物体情報を記憶していてもよい。あるいは、物体情報記憶部581が、円柱状の物、円形の物など、物体の形状毎の物体情報を記憶していてもよい。また、物体識別情報にペットボトルの情報が含まれる場合にペットボトルの容量がパラメータになっているなど、物体識別情報がパラメータを含んでいてもよい。
第3制御部590は、障害物位置特定装置500の各部を制御して各種処理を実行する。第3制御部590の機能は、障害物位置特定装置500が備えるCPUが、第3記憶部580からプログラムを読み出して実行することで実行される。
種類識別部591は、三次元計測部292が算出した障害物に関する情報に基づいて、障害物となっている物体の種類を特定する。
例えば、種類識別部591が、三次元計測部292が算出した情報で示される障害物を、物理的に一纏まりとなっている部分毎に分割するなど、障害物中の物体を抽出するようにしてもよい。そして、種類識別部591が、抽出した物体毎にパターンマッチングを行うことで、物体の種類を特定するようにしてもよい。
種類識別部591が、例えばYOLO(You Only Look Once)またはSSD(Single Shot Multibox Detector)などのように、ニューラルネットワークによる物体検出器として構成されていてもよい。
位置特定部592は、三次元計測部292が算出した障害物に関する情報、および、物体情報記憶部581が記憶している物体情報に基づいて、障害物となっている物体の位置を特定する。
例えば、位置特定部592は、種類識別部591が特定した物体の種類に基づいて、物体情報記憶部581が記憶している物体情報から、物体の位置および大きさの情報を読み出す。そして、位置特定部592は、三次元計測部292が算出した障害物に関する情報が示す物体の位置に、物体情報から読み出した位置および大きさの物体があるものとして、その物体の位置情報を算出する。位置特定部592が、物体の位置を座標値の形式で算出するようにしてもよい。
これにより、位置特定部592は、障害物を三次元的な広がりをもつ物体として捉え、その座標を算出する。例えば、撮像装置100が障害物を一方の側から撮像し、反対側の画像を得られていない場合に、位置特定部592は、画像に写っていない部分の障害物の位置情報を算出することができる。
位置特定部592は、算出した物体毎の位置情報を、第3通信部510を介して障害物回避制御装置400へ送信する。上述したように、第3通信部510が、情報取得装置200から受信した情報に加えて、位置特定部592が算出した物体毎の位置情報を障害物回避制御装置400へ送信するようにしてもよい。あるいは、第3通信部510が、情報取得装置200から受信した情報に対して、障害物の位置情報を位置特定部592が算出した位置情報に置き換えた情報を、障害物回避制御装置400へ送信するようにしてもよい。
図10は、障害物回避制御システム2におけるデータの流れの例を示す図である。
図10を図4と比較すると、図10の障害物回避制御システム2は、障害物位置特定装置500を備える点で、図4の障害物回避制御システム1と異なる。この違いに伴い、図4では、情報取得装置200が、三次元計測部292が算出した障害物に関する情報を障害物回避制御装置400へ送信するのに対し、図10では、情報取得装置200は、三次元計測部292が算出した障害物に関する情報を障害物位置特定装置500へ送信する。
図10では、障害物位置特定装置500は、情報取得装置200から受信した情報に対して、障害物の位置情報を位置特定部592が算出した位置情報に置き換えた情報を、障害物回避制御装置400へ送信する。障害物回避制御装置400では、干渉関数演算部491が、情報取得装置200の状態観測部291が算出した情報および障害物位置特定装置500からの情報を用いて、干渉関数値、干渉関数値の差分、および、ポテンシャル関数値を算出する。
それ以外の点では、図10は図4の場合と同様である。
以上のように、種類識別部591は、障害物となっている物体の種類を識別する。位置特定部592は、物体の種類毎に物体の大きさおよび形状を示す物体情報から、種類識別部591が識別した種類に応じた情報を読み出し、読み出した情報を用いて障害物となっている物体の位置を特定する。
これにより、障害物回避制御システム2では、障害物の位置情報をより高精度に得られる。例えば、障害物位置特定装置500は、撮像装置100の画像に写っていない部分について、障害物の位置情報を補完することができる。
障害物の位置情報をより高精度に得られることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させるための指令値を、より高精度に算出することができる。
<第3実施形態>
図11は、第3実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図11に示す構成で、障害物回避制御システム3は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500と、障害物動作予測装置600とを備える。また、障害物回避制御システム3は制御対象機器900を制御する。
障害物回避制御システム3は、障害物動作予測装置600を備える点で、障害物回避制御システム2と異なる。
それ以外の点では、障害物回避制御システム3は障害物回避制御システム2と同様である。障害物回避制御システム3について、障害物回避制御システム2と同様の点の説明を省略し、必要に応じて図2に示す符号、図3に示す符号、および、図9に示す符号を引用する。
図12は、障害物動作予測装置600の機能構成の例を示す概略ブロック図である。図12に示す構成で、障害物動作予測装置600は、第4通信部610と、第4記憶部680と、第4制御部690とを備える。第4記憶部680は、物体動作情報記憶部681を備える。第4制御部690は、位置予測部691を備える。
第4通信部610は、他の装置と通信を行う。障害物回避制御システム3では、障害物位置特定装置500が、種類識別部591が特定した障害物の種類を示す情報と、位置特定部592が算出した障害物の位置情報とを紐付けて送信する。第4通信部610は、障害物位置特定装置500が送信した情報を受信する。また、第4通信部610は、障害物位置特定装置500から受信した情報に、位置予測部691が予測する障害物の将来の位置情報を加えた情報を、障害物回避制御装置400へ送信する。
第4記憶部680は、各種データを記憶する。第4記憶部680の機能は、障害物動作予測装置600が備える記憶デバイスを用いて実行される。
物体動作情報記憶部681は、物体動作情報を記憶する。物体動作情報記憶部681は、物体の種類を識別する種類識別情報と、物体の動作を予測するための情報とが紐付けられた情報を、物体動作情報として記憶する。
物体動作情報記憶部681が、物体の動作モデルを物体動作情報として記憶するようにしてもよい。あるいは、物体動作情報記憶部681が、障害物として検出された物体の位置の履歴情報を、物体動作情報として記憶するようにしてもよい。
第4制御部690は、障害物動作予測装置600の各部を制御して各種処理を実行する。第4制御部690の機能は、障害物動作予測装置600が備えるCPUが、第4記憶部680からプログラムを読み出して実行することで実行される。
位置予測部691は、障害物となっている物体の将来の位置を特定する。例えば、位置予測部691は、障害物位置特定装置500の種類識別部591が特定した物体の種類の識別情報を用いて、その物体の動作を予測するための情報を物体動作情報記憶部681から読み出す。そして、位置予測部691は、読み出した情報を用いて、制御ステップ間における物体の移動量を予測する。位置予測部691は、障害物位置特定装置500の位置特定部592が算出した物体の位置に、予測した移動量を加算することで、次の制御ステップにおける物体の位置を予測する。
図13は、障害物回避制御システム3におけるデータの流れの例を示す図である。
図13を図10と比較すると、図13の障害物回避制御システム3は、障害物動作予測装置600を備える点で、図10の障害物回避制御システム2と異なる。この違いに伴い、図10では、障害物位置特定装置500が、障害物回避制御装置400へ情報を送信するのに対し、図13では、障害物位置特定装置500は、障害物動作予測装置600へ情報を送信し、障害物動作予測装置600が、障害物回避制御装置400へ情報を送信する。
図13では、障害物位置特定装置500は、物体毎に、物体の種類を示す情報と、物体の位置情報とを紐付けた情報を、障害物動作予測装置600へ送信する。障害物動作予測装置600は、物体毎に、物体の種類を示す情報と、物体の位置情報と、物体の位置の予測情報とが紐付けられた情報を、障害物回避制御装置400へ送信する。障害物回避制御装置400では、干渉関数演算部491が、情報取得装置200の状態観測部291が算出した情報および障害物動作予測装置600からの情報を用いて、干渉関数値、干渉関数値の差分、および、ポテンシャル関数値を算出する。
それ以外の点では、図13は図10の場合と同様である。
以上のように、障害物動作予測装置600は、物体の種類毎に前記物体の動きに関する情報を示す物体動作情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて障害物の将来の位置を予測する。障害物回避制御装置400の干渉関数演算部491は、予測された障害物の将来の位置に基づいて、干渉関数値の経時変化を示す値を算出する。
障害物回避制御システム3は、障害物の将来の位置の予測に、障害物となっている物体の特性として示されるその物体の動作に関する情報を用いることができ、この点で、障害物の将来の位置を比較的高精度に予測できる。この障害物の将来の位置の予測結果を制御対象機器900の障害物回避に用いる点で、障害物回避制御システム3は、制御対象機器900の障害物回避を比較的高精度に行うことができる。
<第4実施形態>
図14は、第4実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図14に示す構成で、障害物回避制御システム4は、撮像装置100と、情報取得装置200と、障害物回避制御装置401とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
障害物回避制御システム4は、ノミナル制御装置300を備えておらず、障害物回避制御装置400に代えて障害物回避制御装置401を備える点で障害物回避制御装置400と異なる。障害物回避制御装置401では、回避指令値を求める処理で用いる制約条件の1つに、制御対象機器900に設定される目標を制御指令値に反映させるための制約条件が含まれる。
それ以外の点では、障害物回避制御システム4は障害物回避制御システム1と同様である。障害物回避制御システム4について、障害物回避制御システム1と同様の点の説明を省略し、必要に応じて図2に示す符号を引用する。
以下では、第1実施形態に基づいて第4実施形態を実施する場合を例に説明するが、第2実施形態または第3実施形態に基づいて第4実施形態を実施するようにしてもよい。特に、障害物回避制御システム4が、第2実施形態にかかる障害物位置特定装置500を備えていてもよい。障害物回避制御システム4が、第3実施形態にかかる障害物位置特定装置500および障害物動作予測装置600を備えていてもよい。
図15は、障害物回避制御装置401の機能構成の例を示す概略ブロック図である。図15に示す構成で、障害物回避制御装置401は、第2通信部410と、第2記憶部480と、第2制御部490とを備える。第2記憶部480は、構造情報記憶部481と、動的モデル記憶部482と、制御関数記憶部483とを備える。第2制御部490は、干渉関数演算部491と、機器制御部492とを備える。機器制御部492は、回避指令値演算部495と、仮想力場反映部496とを備える。
障害物回避制御装置401は、第2記憶部480が制御関数記憶部483を備える点、および、機器制御部492がノミナル指令値取得部493と回避要否判定部494とを備えていない点で障害物回避制御装置400と異なる。この違いは、制御対象機器900に設定される目標を制御指令値に反映させる方法の違いに起因する。
障害物回避制御装置400では、回避指令値演算部495が、評価関数を用いて回避指令値をノミナル指令値に近付けることで、制御対象機器900に設定される目標を回避指令値に反映させる。一方、障害物回避制御装置401では、回避指令値演算部495が回避指令値を求める処理で用いる制約条件の1つに、制御対象機器900に設定される目標を制御指令値に反映させるための制約条件が含まれる。
それ以外の点では、障害物回避制御装置401は障害物回避制御装置400と同様である。
第4実施形態で回避指令値演算部495が解く最適化問題における制約条件は、3種類の式で示される。
3種類の式のうち1種類目は、第1実施形態の場合と同様、制御対象機器900が障害物に接触しないための十分条件を示す式である。3種類の式のうち1種類目は、例えば上述した式(3)のように示される。
3種類の式のうち2種類目は、第1実施形態の場合と同様、制御指令値が、制御対象機器900の可動部の仕様により規定される上下限値の範囲内で設定されるという制約条件を示す式である。3種類の式のうち2種類目は、例えば上述した式(7)のように示される。
一方、3種類の式のうち3種類目は、制御対象機器900に設定される目標が制御指令の実行によって安定に達成される度合いに関する条件を示す。
例えば、リアプノフ関数をVで示し、制御ステップ間におけるリアプノフ関数値の変化量をΔVで示して、制約条件を示す3種類目の式を式(11)のようにしてもよい。
Figure 0007180695000011
「d」は、制約条件を緩和して解を得易くするために設けられている。
d=0として解を得られた場合、その解は、制御対象機器900に設定される目標を厳密に達成するための制御指令値である。一方、d=0の場合、解をピンポイントで探索することになり、解を得られないことが懸念される。
そこで、d≧0とすることで、制御指令値に基づく制御対象機器900の動作結果と目標とのずれを許容して、解の探索範囲を広げることができる。以下では、制御対象機器900の動作結果と目標とのずれを誤差と称する。「d」の値が大きくなるほど、許容する誤差が大きくなり、解を得易くなる。
以下では、「d」がd≧0の変数である場合の例を説明する。この場合、後述するように最適化問題における評価関数に、「d」の値の大きさの評価のための項を設けるようにしてもよい。
第4実施形態で回避指令値演算部495が解く最適化問題における制約条件を示す3種類目の式は、式(11)に限定されず、制御対象機器900に設定される目標の達成度合いと、解の得易さとのトレードオフを調整可能ないろいろな式とすることができる。
例えば、第4実施形態で回避指令値演算部495が解く最適化問題における制約条件を示す3種類目の式を、式(12)のようにしてもよい。
Figure 0007180695000012
関数Dは、制御対象機器900に設定される目標値と実際値とのユークリッド距離を算出する関数である。制御対象機器900に設定される目標値がベクトルで示されているとして、回避指令値演算部495は、関数Dの演算で、そのベクトルの全ての要素について目標値と実際値との差の2乗を足し合わせて、その平方根を演算する。
関数ΔDは、制御対象機器900に設定される目標値と実際値とのユークリッド距離の、制御ステップ間における変化量を算出する関数である。例えば、制御ステップtにおける関数値D(x)をD(x)と表記し、制御ステップt+1における関数値D(x)をD(xt+1)と表記する。また、制御ステップtから制御ステップt+1までの時間における関数値D(x)の変化量ΔD(x,u)は、ΔD(x,u)=D(xt+1)-D(x)と示される。
式(12)は、制御対象機器900の安定性を保証する条件を緩和した制約式である。例えば、回避指令値演算部495で、ΔD(x,u)<0を満たす制御指令uが常に求まれば、制御対象機器900は安定に目標値を達成することができる。
第4実施形態で回避指令値演算部495が解く最適化問題における目的関数は、例えば式(13)のように示される。
Figure 0007180695000013
式13の「uu」の項は、制御指令値uのベクトルとしてのノルムの2乗を示す。ここでいうノルムは、ユークリッドノルムまたはユークリッド距離とも称される。
式13の「uu」の項は、制御対象機器900が無駄な動作をしないよう、制御指令値の大きさをなるべく小さくするために設けられている。
式13の「p・d」の項は、式(11)または式(12)における「d」の大きさを評価するための項である。「p・d」の「p」は、「uu」と「d」の重み付けを調整するための重みを示す。「p」は、例えばp>0の定数として設定される。
この最適化問題の解の候補が2つ検出された場合、2つの解の候補の「uu」の項の値が同じであれば、「p・d」の項の値が小さい方が最適解として採用される。
式(13)は、評価関数の例に該当する。式(13)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
図16は、障害物回避制御システム4におけるデータの流れの例を示す図である。
図16を図4と比較すると、図16では障害物回避制御システム4がノミナル制御装置300を備えていない。これに伴い図16では、情報取得装置200からの制御対象に関する値が機器制御部492へ入力されている。
それ以外の点では、図16は図4の場合と同様である。
図17は、障害物回避制御装置401が制御対象機器900に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置401は、制御対象機器900に対する制御の制御ステップ毎に図17の処理を行う。
図17のステップS311における処理は、図5のステップS111における処理と同様である。
ステップS311の後、回避指令値演算部495は、上述した最適化問題の演算による制御指令値の取得を試みる(ステップS312)。
機器制御部492は、回避指令値演算部495がステップS312で最適化問題の解を得られたか否かを判定する(ステップS313)。
解を得られたと機器制御部492が判定した場合(ステップS313:YES)、処理がステップS321へ進む。ステップS321における処理は、図5のステップS141における処理と同様である。ステップS321の後、処理がステップS341へ進む。ステップS341における処理は、図5のステップS161における処理と同様である。ステップS341の後、障害物回避制御装置401は図17の処理を終了する。
一方、ステップS313で解を得られなかったと機器制御部492が判定した場合(ステップS313:NO)、処理がステップS3331へ進む。ステップS331における処理は、図5のステップS151における処理と同様である。ステップS331の後、処理がステップS341に進む。
以上のように、障害物回避制御装置400の場合と同様、障害物回避制御装置401でも、制御対象機器900と障害物との接触を回避するための制約条件を設けることができる。制御対象機器900に、障害物との接触を回避するための基準点が複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置401によれば、制御対象機器900と障害物との接触を回避するための基準点を複数設定することができ、制御対象機器900のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置401では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置401によれば、障害物が複数ある場合に比較的容易に対応できる。
<第5実施形態>
図18は、第5実施形態に係る障害物回避制御装置の構成の例を示す図である。図18に示す障害物回避制御装置10は、回避指令値演算部11と、機器制御部12とを備える。
かかる構成で、回避指令値演算部11は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める。機器制御部12は、回避指令値演算部11による処理の結果に基づいて、制御対象機器を制御する。
障害物回避制御装置10では、制御対象機器と障害物との接触を回避するための制約条件を設けることができる。制御対象機器と障害物との接触を回避するための基準点が制御対象物に複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置10では、制御対象機器について障害物を回避するための基準点は1か所に限定されず、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
また、障害物回避制御装置10では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置10によれば、障害物が複数ある場合に比較的容易に対応できる。
<第6実施形態>
図19は、第6実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。図19に示す障害物回避制御方法は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程(ステップS11)と、ステップS11での処理の結果に基づいて、制御対象機器を制御する工程(ステップS12)とを含む。
この障害物回避制御方法では、制御対象機器と障害物との接触を回避するための制約条件を設けることができる。制御対象機器と障害物との接触を回避するための基準点が制御対象物に複数設定されている場合、基準点毎に制約条件を設けることができる。このように障害物回避制御装置10では、制御対象機器について障害物を回避するための基準点は1か所に限定されず、制御対象機器のいろいろな部分について、障害物を回避するように制御することができる。
図20は、少なくとも1つの実施形態に係るコンピュータの構成を示す概略ブロック図である。
図20に示す構成で、コンピュータ700は、CPU710と、主記憶装置720と、補助記憶装置730と、インタフェース740とを備える。
上記の情報取得装置200、ノミナル制御装置300、障害物回避制御装置400、障害物回避制御装置401、障害物位置特定装置500、および、障害物動作予測装置600のうち何れか1つ以上が、コンピュータ700に実装されてもよい。その場合、上述した各処理部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。また、CPU710は、プログラムに従って、上述した各記憶部に対応する記憶領域を主記憶装置720に確保する。各装置と他の装置との通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
情報取得装置200がコンピュータ700に実装される場合、第1制御部290およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第1記憶部280に対応する記憶領域を主記憶装置720に確保する。第1通信部210が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物回避制御装置400がコンピュータ700に実装される場合、第2制御部490およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物回避制御装置401がコンピュータ700に実装される場合、第2制御部490およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物位置特定装置500がコンピュータ700に実装される場合、第3制御部590およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第3記憶部580およびその各部に対応する記憶領域を主記憶装置720に確保する。第3通信部510が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
障害物動作予測装置600がコンピュータ700に実装される場合、第4制御部690およびその各部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。
また、CPU710は、プログラムに従って、第4記憶部680およびその各部に対応する記憶領域を主記憶装置720に確保する。第4通信部610が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
なお、情報取得装置200、ノミナル制御装置300、障害物回避制御装置400、障害物回避制御装置401、障害物位置特定装置500、および、障害物動作予測装置600の全部または一部の機能を実現するためのプログラムをコンピュータ読み取り可能な記録媒体に記録して、この記録媒体に記録されたプログラムをコンピュータシステムに読み込ませ、実行することにより各部の処理を行ってもよい。ここでいう「コンピュータシステム」とは、OS(オペレーティングシステム)や周辺機器等のハードウェアを含む。
「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ROM(Read Only Memory)、CD-ROM(Compact Disc Read Only Memory)等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
以上、本発明の実施形態について図面を参照して詳述してきたが、具体的な構成はこの実施形態に限られるものではなく、この発明の要旨を逸脱しない範囲の設計変更等も含まれる。
本発明の実施形態は、障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体に適用してもよい。
1、2、3、4 障害物回避制御システム
10、400、401 障害物回避制御装置
11、495 回避指令値演算部
12、492 機器制御部
100 撮像装置
200 情報取得装置
210 第1通信部
280 第1記憶部
290 第1制御部
291 状態観測部
292 三次元計測部
300 ノミナル制御装置
410 第2通信部
480 第2記憶部
481 構造情報記憶部
482 動的モデル記憶部
483 制御関数記憶部
490 第2制御部
491 干渉関数演算部
493 ノミナル指令値取得部
494 回避要否判定部
496 仮想力場反映部
500 障害物位置特定装置
510 第3通信部
580 第3記憶部
581 物体情報記憶部
590 第3制御部
591 種類識別部
592 位置特定部
600 障害物動作予測装置
610 第4通信部
680 第4記憶部
681 物体動作情報記憶部
690 第4制御部
691 位置予測部

Claims (8)

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

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/JP2019/003177 WO2020157862A1 (ja) 2019-01-30 2019-01-30 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法および記録媒体

Publications (2)

Publication Number Publication Date
JPWO2020157862A1 JPWO2020157862A1 (ja) 2021-11-11
JP7180695B2 true JP7180695B2 (ja) 2022-11-30

Family

ID=71841442

Family Applications (1)

Application Number Title Priority Date Filing Date
JP2020569234A Active JP7180695B2 (ja) 2019-01-30 2019-01-30 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム

Country Status (4)

Country Link
US (1) US20220097231A1 (ja)
EP (1) EP3920001A4 (ja)
JP (1) JP7180695B2 (ja)
WO (1) WO2020157862A1 (ja)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115003464A (zh) * 2020-02-12 2022-09-02 发那科株式会社 机器人系统
CN113625702B (zh) * 2020-10-28 2023-09-22 北京科技大学 基于二次规划的无人车同时路径跟踪与避障方法
CN113119119B (zh) * 2021-03-30 2022-07-05 广州大学 一种机器人动作的执行方法、装置和存储介质
WO2023047496A1 (ja) * 2021-09-22 2023-03-30 日本電気株式会社 制約条件取得装置、制御システム、制約条件取得方法および記録媒体
CN114782759B (zh) * 2022-06-22 2022-09-13 鲁东大学 一种基于YOLOv5网络对密集遮挡鱼类的检测方法
CN116755562B (zh) * 2023-07-04 2024-04-05 深圳市仙瞬科技有限公司 一种避障方法、装置、介质及ar/vr设备

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018167333A (ja) 2017-03-29 2018-11-01 セイコーエプソン株式会社 ロボット制御装置、ロボットおよびロボットシステム
JP2018190241A (ja) 2017-05-09 2018-11-29 オムロン株式会社 タスク実行システム、タスク実行方法、並びにその学習装置及び学習方法

Family Cites Families (9)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
SE456048B (sv) * 1982-02-24 1988-08-29 Philips Norden Ab Sett och anordning for att bestemma kollisionsrisken for tva inbordes rorliga kroppar
US5347459A (en) * 1993-03-17 1994-09-13 National Research Council Of Canada Real time collision detection
JP5112666B2 (ja) * 2006-09-11 2013-01-09 株式会社日立製作所 移動装置
JP4730440B2 (ja) * 2009-01-01 2011-07-20 ソニー株式会社 軌道計画装置及び軌道計画方法、並びにコンピューター・プログラム
JP2012011494A (ja) * 2010-06-30 2012-01-19 Yaskawa Electric Corp ロボットの角度データ変換方法及びこの方法を実施するための制御装置
JP2012011498A (ja) * 2010-06-30 2012-01-19 Toshiba Corp ロボットアーム操作システムおよびその操作方法
KR101941147B1 (ko) * 2014-12-25 2019-04-12 카와사키 주코교 카부시키 카이샤 암형 로봇의 장애물 자동 회피방법 및 제어장치
JP7058126B2 (ja) * 2018-01-12 2022-04-21 株式会社日立製作所 ロボット制御装置および自動組立システム
TWI664573B (zh) * 2018-05-11 2019-07-01 國立交通大學 動作計算裝置、機器人系統及機器人控制方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2018167333A (ja) 2017-03-29 2018-11-01 セイコーエプソン株式会社 ロボット制御装置、ロボットおよびロボットシステム
JP2018190241A (ja) 2017-05-09 2018-11-29 オムロン株式会社 タスク実行システム、タスク実行方法、並びにその学習装置及び学習方法

Also Published As

Publication number Publication date
US20220097231A1 (en) 2022-03-31
EP3920001A4 (en) 2022-01-26
WO2020157862A1 (ja) 2020-08-06
JPWO2020157862A1 (ja) 2021-11-11
EP3920001A1 (en) 2021-12-08

Similar Documents

Publication Publication Date Title
JP7180695B2 (ja) 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム
US11292132B2 (en) Robot path planning method with static and dynamic collision avoidance in an uncertain environment
Janabi-Sharifi et al. Automatic selection of image features for visual servoing
KR20210043632A (ko) 물체 자세 추정 방법 및 장치
JP5750657B2 (ja) 強化学習装置、制御装置、および強化学習方法
JP5803367B2 (ja) 自己位置推定装置、自己位置推定方法およびプログラム
CN114072254A (zh) 不确定环境中利用静态和动态碰撞规避的机器人路径规划方法
JP7180696B2 (ja) 制御装置、制御方法およびプログラム
JP2003271975A (ja) 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置
WO2020181257A1 (en) Collaborative determination of a load footprint of a robotic vehicle
KR20200027087A (ko) 로봇 및 그의 제어 방법
Sadeghian et al. Visual servoing with safe interaction using image moments
JP2022156954A (ja) 制御装置、ロボットシステム、制御方法、および、プログラム
Radmard et al. Resolving occlusion in active visual target search of high-dimensional robotic systems
JP2021010994A (ja) センサ位置姿勢キャリブレーション装置及びセンサ位置姿勢キャリブレーション方法
US20220143836A1 (en) Computer-readable recording medium storing operation control program, operation control method, and operation control apparatus
US11372475B2 (en) Information processing apparatus, information processing method, and floor modeling system
Du et al. Human‐Manipulator Interface Using Particle Filter
JP2008065562A (ja) 情報処理装置、および同定処理方法、並びにコンピュータ・プログラム
US20230202026A1 (en) Robot Training System
CN111971149A (zh) 记录介质、信息处理设备和信息处理方法
US20240316762A1 (en) Environmental feature-specific actions for robot navigation
KR20150050224A (ko) 비정상 배회 탐지 장치 및 방법
US12017372B2 (en) Robot control device
CN113269807B (zh) 对物体辨识模型的沿用可否进行判定的系统及方法

Legal Events

Date Code Title Description
A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20210709

A621 Written request for application examination

Free format text: JAPANESE INTERMEDIATE CODE: A621

Effective date: 20210709

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220222

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20220425

A131 Notification of reasons for refusal

Free format text: JAPANESE INTERMEDIATE CODE: A131

Effective date: 20220802

A521 Request for written amendment filed

Free format text: JAPANESE INTERMEDIATE CODE: A523

Effective date: 20221003

TRDD Decision of grant or rejection written
A01 Written decision to grant a patent or to grant a registration (utility model)

Free format text: JAPANESE INTERMEDIATE CODE: A01

Effective date: 20221018

A61 First payment of annual fees (during grant procedure)

Free format text: JAPANESE INTERMEDIATE CODE: A61

Effective date: 20221031

R151 Written notification of patent or utility model registration

Ref document number: 7180695

Country of ref document: JP

Free format text: JAPANESE INTERMEDIATE CODE: R151