JP7180695B2 - 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム - Google Patents
障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム Download PDFInfo
- 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
Links
Images
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
- B25J9/1666—Avoiding collision or forbidden zones
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/39—Robotics, robotics to robotics hand
- G05B2219/39091—Avoid collision with moving obstacles
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05B—CONTROL OR REGULATING SYSTEMS IN GENERAL; FUNCTIONAL ELEMENTS OF SUCH SYSTEMS; MONITORING OR TESTING ARRANGEMENTS FOR SUCH SYSTEMS OR ELEMENTS
- G05B2219/00—Program-control systems
- G05B2219/30—Nc systems
- G05B2219/40—Robotics, robotics mapping to robotics vision
- G05B2219/40476—Collision, planning for collision free path
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Description
図1は、第1実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図1に示す構成で、障害物回避制御システム1は、撮像装置100と、情報取得装置200と、ノミナル(Nominal)制御装置300と、障害物回避制御装置400とを備える。
ここでいう所望の動作は、制御対象機器900に設定されている目標を達成する動作である。ここでいう接触することは、単に触れることに限定されず、ぶつかることを含む。制御対象機器900が障害物に接触することは、制御対象機器900の少なくとも一部分が、障害物の少なくとも一部分に接触することである。
あるいは、制御対象機器900が、無人搬送車またはドローンなどの移動体であってもよい。また、制御対象機器900が、制御指令値にて制御可能であれば、自律的に動作する装置であってもよい。
また、制御対象機器900自らも障害物として扱われていてもよい。例えば、制御対象機器900が垂直多関節ロボットであり、その姿勢によってはロボットアームと台座部分とが接触する場合、障害物回避制御システム1が制御対象機器900を障害物として扱うことで、ロボットアームと台座部分との接触を回避し得る。
撮像装置100の撮像画像に基づいて、情報取得装置200が障害物の位置を特定する。撮像装置100として、例えばデプスカメラ(Depth Camera)、ステレオカメラ(Stereo Camera)など、3次元の情報を得られるカメラを用いるようにしてもよい。あるいは、情報取得装置200が、OpenPoseの3次元拡張など、2次元の画像から3次元の情報を得るための技術を用いるようにしてもよい。
また、撮像装置100が複数台のカメラで構成されていてもよい。撮像装置100が移動可能になっていてもよい。
撮像装置100が、観測対象の空間の状況をボクセルデータで示すようにしてもよい。
情報取得装置200が、制御対象機器900の位置情報をボクセルデータで送信するようにしてもよい。例えば、情報取得装置200が制御対象機器900の表面の位置情報をボクセルデータで送信することで、障害物回避制御装置400は、1点ではなく制御対象機器900の表面と、障害物との位置関係を把握でき、制御対象機器900と障害物との距離をより正確に把握できる。制御対象機器900と障害物との距離をより正確に把握できることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させる制御をより高精度に行うことができる。あるいは、情報取得装置200が、制御対象機器900の位置情報として、制御対象機器900に設定されている代表点の座標を送信するようにしてもよい。
あるいは、情報取得装置200が、制御対象機器の関節の角速度など、制御対象機器のアクチュエータの動きを示す情報を送信するようにしてもよい。
制御対象機器900の位置情報と制御対象機器900の動きを示す情報とを総称して制御対象機器900の状態情報と称する。
情報取得装置200は、制御対象機器の状態情報を、ノミナル制御装置300および障害物回避制御装置400へ送信する。
障害物の位置情報、あるいは、障害物が動く場合の、障害物の位置情報と障害物の動きを示す情報との組み合わせを、障害物の状態情報と称する。
情報取得装置200は、障害物の状態情報を障害物回避制御装置400へ送信する。
ノミナル制御装置300が算出するノミナル指令値は、障害物回避制御装置400が、制御対象機器900に対して指示する制御指令値(すなわち、実際に用いられる制御指令値)を取得するための基準となる制御指令値として用いられる。
障害物回避制御装置400は、制御対象機器900を制御するための制御指令値を取得する。障害物回避制御装置400は、取得した制御指令値を制御対象機器900へ送信することで、制御対象機器900を制御する。
第1通信部210は、他の装置と通信を行う。特に、第1通信部210は、制御対象機器900が備えるセンサからのセンシングデータを受信する。また、第1通信部210は、撮像装置100からのデータを受信する。第1通信部210は、撮像装置100の撮像画像の画像データを受信し、さらに、奥行き方向の距離情報など付加情報がある場合は、画像データに加えて付加情報を受信する。
第1制御部290は、情報取得装置200の各部を制御して各種処理を実行する。第1制御部290の機能は、情報取得装置200が備えるCPU(Central Processing Unit、中央処理装置)が、第1記憶部280からプログラムを読み出して実行することで実行される。
構造情報記憶部481は、制御対象機器900の構造情報を記憶する。制御対象機器900の構造情報は、制御対象機器900のアームの長さおよび太さ、関節が動く方向および可動範囲など、制御対象機器900の形状および動作に関する情報である。制御対象機器900のCAD(Computer Aided Design)データがある場合、そのCADデータを構造情報として用いるようにしてもよい。
動的モデルが、将来の時点における制御対象機器900の予測位置の位置情報を出力するようにしてもよい。あるいは、動的モデルが、制御対象機器900の動作量を出力するようにしてもよい。すなわち、動的モデルが、制御対象機器900の将来の予測位置から現在位置を減算した差分を出力するようにしてもよい。
動的モデルは、制御指令値uの入力に対して、制御対象機器900の状態情報xで示される状態の微分値または差分が求まるモデルであり、例えば状態空間モデルであってもよい。
特に、制御対象機器900の構造情報にアームの太さ等の情報が含まれていることで、障害物回避制御装置400は、制御対象機器900の骨格の位置のみでなく、アームの太さ等を考慮した位置情報を取得できる。すなわち、障害物回避制御装置400は、制御対象機器900の位置情報を、空間的な広がりを有する位置として把握できる。
干渉関数演算部491は、干渉関数値、および、干渉関数値の経時変化を示す値を算出する。干渉関数は、制御対象機器900と障害物との位置関係に応じた値を示す。干渉関数Bは、式(1)のような値をとる。
干渉関数値B(x)により、制御対象機器900が障害物に接触しているか否か、および、制御対象機器900と障害物との距離が示される。
干渉関数演算部491は、障害物の状態情報に基づいて干渉関数を生成し、第2記憶部480に記憶させておく。
干渉関数演算部491は、今回の制御ステップから次回の制御ステップまでの間の、干渉関数値B(x)の変化量を予測する。制御ステップ間における干渉関数値の変化量をΔB(x,u)と表記する。干渉関数値B(x)の変化量は制御対象機器900の位置の変化に依存し、制御対象機器900の位置の変化は制御指令値uに依存するため、制御指令値uを明示している。
一方、回避指令値演算部495が制御指令値の算出に失敗した場合、すなわち、障害物を回避できる制御指令値を得られなかった場合、機器制御部492は、制御対象機器900を減速させる制御指令値を、第2通信部410を介して制御対象機器900へ送信する。
回避要否判定部494は、制御対象機器900と障害物との位置関係に基づいて、回避指令値演算部495が制御指令値を取得するための処理を行う必要の有無を判定する。上記のように、回避要否判定部494は、制御対象機器900と障害物との距離と閾値とを比較して、距離が閾値より大きいか否かを判定する。具体的には、回避要否判定部494は、干渉関数値B(x)と所定の定数の閾値αとを比較し、B(x)>αか否かを判定する。
ベクトルまたは行列に上付の「T」は、ベクトルまたは行列の転置を示す。
「ur」は、ノミナル制御装置300からのノミナル指令値を示す。
制御指令値を示すu*、u、urのデータ形式は、同じ次元のベクトルであるとする。これらのベクトルの次元数は、障害物回避制御装置400が制御対象機器900へ送信する制御指令値の次元数と同じとする。
式(2)は、評価関数の例に該当する。式(2)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
γの値によって、制御対象機器900と障害物とが接触しないための、制御対象機器900と障害物との間隔の余裕分をどれだけ見込むか調整できる。
通常、制御対象機器900と障害物とは接触しておらず、B(x)が、制御対象機器900と障害物との距離を示す。制御対象機器900が障害物に接近してΔB(x,u)が負の値をとるとき、ΔB(x,u)の大きさがγB(x)以下である場合に式(3)が成り立つ。
複数の障害物が存在する場合、障害物毎に式(3)の制約条件を設けることができる。これにより、障害物回避制御装置400は、制御対象機器900が障害物全体に接触しないよう制御することができる。あるいは、複数の障害物をまとめたものに対して干渉関数を設計してもよい。
今回の制御ステップをtで示し、制御ステップtの次の制御ステップをt+1で示す。制御ステップtにおける干渉関数値をB(xt)と表記する。
制御ステップt+1における干渉関数値をB(xt+1)と表記する。B(xt+1)からB(xt)を減算した差分を、ΔB(xt,ut)と表記する。ΔB(xt,ut)は、式(4)のように示される。
回避指令値演算部495は、この距離を用いて式(3)の条件を判定することで、より正確に条件判定を行うことができる。この点で、回避指令値演算部495が、より高精度な制御指令値を得られ、機器制御部492が、より高精度に制御対象機器900を制御できると期待される。
式(3)の制約条件の判定に障害物の表面の位置情報が用いられる場合の例として、情報取得装置200が、障害物の表面の位置情報を例えばボクセルデータを障害物回避制御装置400へ送信し、干渉関数演算部491が、その位置情報を用いて干渉関数を生成する場合が挙げられる。
障害物が複数ある場合も、障害物毎に式(3)の条件式を設けることができる。障害物回避制御装置400では、この点で、障害物が複数の場合に比較的容易に対応できる。
識別番号iで識別される可動部分をi番目の可動部分と表記する。従って、uiは、i番目の可動部分に対する制御指令値である。
制御指令値uは、ui(i=1,2,・・・,N)を纏めてベクトルで表したものである。
上述したように、回避指令値演算部495は、回避指令値を取得するための処理が必要であると回避要否判定部494が判定した場合に、回避指令値を求める。
制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど大きい加速度で障害物から離れることであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900が障害物に近いほど、障害物から離れる距離が長いことであってもよい。あるいは、制御対象機器900が障害物に近いほど障害物から離れることは、制御対象機器900と障害物との距離が所定の閾値以下である場合のみ、制御対象機器900が障害物から離れることであってもよい。
障害物を質点(Point Mass)で代表し、障害物の座標を(x0,y0,z0)としてもよい。そして、仮想力場反映部496が、座標(x,y,z)に位置する制御対象機器900に対して、障害物から遠ざからせるための制御指令値upを、式(8)に基づいて算出するようにしてもよい。
以下では、仮想力場反映部496が、ノミナル指令値および回避指令値の両方に、算出した指令値を反映させる場合を例に説明する。
但し、仮想力場反映部496の処理は障害物回避制御装置400に必須ではない。従って、障害物回避制御装置400が仮想力場反映部496を備えていない構成となっていてもよい。
上述したように、制御対象機器900のセンサのセンシングデータは、情報取得装置200の状態観測部291へ入力される。
撮像装置100の画像データは、情報取得装置200の三次元計測部292へ入力される。撮像装置100の付加情報がある場合、画像データに加えて付加情報も三次元計測部292へ入力される。
図5は、障害物回避制御装置400が制御対象機器900に対する制御指令値を取得する処理手順の例を示すフローチャートである。障害物回避制御装置400は、制御対象機器900に対する制御の制御ステップ毎に図5の処理を行う。
図5の処理で、仮想力場反映部496は、干渉関数演算部491が算出するポテンシャル関数値P(x)に基づいて、力場を模擬するための制御指令値upを算出する(ステップS111)。制御指令値upは、ノミナル指令値または回避指令値に値を付加する補正項として用いられる。
干渉関数値B(x)が閾値αより大きいと判定した場合(ステップS112:YES)、機器制御部492は、制御対象機器900に対する制御指令値uを、ノミナル指令値urに制御指令値upを加えた値と算出する(ステップS121)。すなわち、機器制御部492は、u=ur+upと算出する。
そして、機器制御部492は、制御指令値uを、第2通信部410を介して制御対象機器900へ送信することで、制御対象機器900を制御する(ステップS161)。
ステップS161の後、障害物回避制御装置400は、図5の処理を終了する。
機器制御部492は、回避指令値演算部495がステップS131で最適化問題の解を得られたか否かを判定する(ステップS132)。
ステップS141の後、処理がステップS161へ進む。
ステップS151の後、処理がステップS161へ進む。
図6の処理で、干渉関数演算部491は、情報取得装置200が送信する制御対象機器900の情報、および、構造情報記憶部481が記憶している制御対象機器900の構造情報等を用いて、制御対象機器900の占有空間の位置情報を算出する(ステップS211)。干渉関数演算部491は、制御対象機器900の占有空間の位置情報を、制御対象機器900の座標系による座標値で算出する。ここでいう制御対象機器900の座標系は、例えば、障害物回避制御装置400が制御対象機器900に送信する制御指令値に用いられる座標値である。
また、干渉関数演算部491は、制御対象機器900の位置情報、制御対象機器900の動作情報、および、障害物の位置情報に基づいて、干渉関数値の変化量ΔB(x,u)を算出する(ステップS214)。
ステップS214の後、障害物回避制御装置400は、図6の処理を終了する。
図7の処理で、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出可能か判定する(ステップS221)。
算出可能と判定した場合(ステップS221;YES)、干渉関数演算部491は、動的モデルの演算でΔB(x,u)を算出する(ステップS231)。
ステップS231の後、障害物回避制御装置400は、図7の処理を終了する。
ステップS241の後、障害物回避制御装置400は図7の処理を終了する。
そこで、干渉関数演算部491は、ステップS231の処理でΔB(x,u)を算出可能か判定し、算出可能であればステップS231の処理を行う。一方、ステップS231の処理ではΔB(x,u)を算出できない場合、干渉関数演算部491は、ステップS241の処理を行う。
また、障害物回避制御装置400では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置400によれば、障害物が複数ある場合に比較的容易に対応できる。
ノミナル指令値は、制御対象機器900に設定されている目標を達成するように設定される指令値であり、障害物回避制御装置400が、障害物回避を考慮した回避指令値をノミナル指令値に近付けるように生成することで、目標を達成できると期待される。
障害物回避制御装置400によれば、このように、回避指令値を生成する際にノミナル指令値を基準として用いることができ、この点で、回避指令値を比較的容易に生成できる。
障害物回避制御装置400では、制御対象機器900と障害物との距離を干渉関数として用いることができ、制御対象機器900の位置と障害物の位置とから容易に関数値を算出できる。この点で、障害物回避制御装置400の負荷が軽くて済む。
回避指令値を取得するための処理が不要であると回避要否判定部494が判定した場合、ノミナル制御装置300が生成したノミナル指令値を制御対象機器900への指令値として用いることができ、障害物回避制御装置400が新たに指令値を生成する必要が無い。この点で、障害物回避制御装置400の負荷が比較的軽くて済む。
回避指令値演算部495が回避指令値の取得に失敗した場合、制御対象機器900と障害物との接触を回避できない可能性がある。そこで、機器制御部492が制御対象機器900を減速させる。これにより、制御対象機器900が障害物に接触するまでの時間を確保でき、例えば人手で障害物を動かすといった対応が可能である。また、制御対象機器900が障害物に接触した場合でも、制御対象機器900が減速していることで、不具合が生じないか、あるいは、不具合の程度が軽くて済むと期待される。
これにより、制御対象機器900は、障害物に近いほど、障害物から離れるように動作する。障害物回避制御装置400によれば、この点で、より確実に、制御対象機器900に障害物を回避させることができる。
このように、障害物回避制御装置400では、基準点毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、制御対象機器に基準点が複数設定されている場合に、比較的容易に対応できる。
このように、障害物回避制御装置400では、障害物毎に制約条件の式を設けることができる。障害物回避制御装置400によればこの点で、障害物が複数ある場合に、比較的容易に対応できる。
このように、回避指令値演算部495が、制御対象機器900と障害物とが最も近い位置における制御対象機器900と障害物との距離を用いて回避指令値を求めることで、制御対象機器900と障害物との接触の可能性をより正確に反映した回避指令値を求めることができる。障害物回避制御装置400によればこの点で、制御対象機器900の制御を比較的高精度に行うことができる。
図8は、第2実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図8に示す構成で、障害物回避制御システム2は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
物体情報記憶部581は、物体情報を記憶する。物体情報記憶部581は、物体の種類を識別する種類識別情報と、物体の形状および大きさなど物体の位置を特定するための情報とが紐付けられた情報を、物体情報として記憶する。
種類識別部591は、三次元計測部292が算出した障害物に関する情報に基づいて、障害物となっている物体の種類を特定する。
種類識別部591が、例えばYOLO(You Only Look Once)またはSSD(Single Shot Multibox Detector)などのように、ニューラルネットワークによる物体検出器として構成されていてもよい。
例えば、位置特定部592は、種類識別部591が特定した物体の種類に基づいて、物体情報記憶部581が記憶している物体情報から、物体の位置および大きさの情報を読み出す。そして、位置特定部592は、三次元計測部292が算出した障害物に関する情報が示す物体の位置に、物体情報から読み出した位置および大きさの物体があるものとして、その物体の位置情報を算出する。位置特定部592が、物体の位置を座標値の形式で算出するようにしてもよい。
図10を図4と比較すると、図10の障害物回避制御システム2は、障害物位置特定装置500を備える点で、図4の障害物回避制御システム1と異なる。この違いに伴い、図4では、情報取得装置200が、三次元計測部292が算出した障害物に関する情報を障害物回避制御装置400へ送信するのに対し、図10では、情報取得装置200は、三次元計測部292が算出した障害物に関する情報を障害物位置特定装置500へ送信する。
それ以外の点では、図10は図4の場合と同様である。
障害物の位置情報をより高精度に得られることで、障害物回避制御装置400は、制御対象機器900に障害物を回避させるための指令値を、より高精度に算出することができる。
図11は、第3実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図11に示す構成で、障害物回避制御システム3は、撮像装置100と、情報取得装置200と、ノミナル制御装置300と、障害物回避制御装置400と、障害物位置特定装置500と、障害物動作予測装置600とを備える。また、障害物回避制御システム3は制御対象機器900を制御する。
それ以外の点では、障害物回避制御システム3は障害物回避制御システム2と同様である。障害物回避制御システム3について、障害物回避制御システム2と同様の点の説明を省略し、必要に応じて図2に示す符号、図3に示す符号、および、図9に示す符号を引用する。
物体動作情報記憶部681は、物体動作情報を記憶する。物体動作情報記憶部681は、物体の種類を識別する種類識別情報と、物体の動作を予測するための情報とが紐付けられた情報を、物体動作情報として記憶する。
物体動作情報記憶部681が、物体の動作モデルを物体動作情報として記憶するようにしてもよい。あるいは、物体動作情報記憶部681が、障害物として検出された物体の位置の履歴情報を、物体動作情報として記憶するようにしてもよい。
位置予測部691は、障害物となっている物体の将来の位置を特定する。例えば、位置予測部691は、障害物位置特定装置500の種類識別部591が特定した物体の種類の識別情報を用いて、その物体の動作を予測するための情報を物体動作情報記憶部681から読み出す。そして、位置予測部691は、読み出した情報を用いて、制御ステップ間における物体の移動量を予測する。位置予測部691は、障害物位置特定装置500の位置特定部592が算出した物体の位置に、予測した移動量を加算することで、次の制御ステップにおける物体の位置を予測する。
図13を図10と比較すると、図13の障害物回避制御システム3は、障害物動作予測装置600を備える点で、図10の障害物回避制御システム2と異なる。この違いに伴い、図10では、障害物位置特定装置500が、障害物回避制御装置400へ情報を送信するのに対し、図13では、障害物位置特定装置500は、障害物動作予測装置600へ情報を送信し、障害物動作予測装置600が、障害物回避制御装置400へ情報を送信する。
それ以外の点では、図13は図10の場合と同様である。
図14は、第4実施形態に係る障害物回避制御システムの装置構成の例を示す概略構成図である。図14に示す構成で、障害物回避制御システム4は、撮像装置100と、情報取得装置200と、障害物回避制御装置401とを備える。また、障害物回避制御システム2は制御対象機器900を制御する。
それ以外の点では、障害物回避制御システム4は障害物回避制御システム1と同様である。障害物回避制御システム4について、障害物回避制御システム1と同様の点の説明を省略し、必要に応じて図2に示す符号を引用する。
それ以外の点では、障害物回避制御装置401は障害物回避制御装置400と同様である。
3種類の式のうち1種類目は、第1実施形態の場合と同様、制御対象機器900が障害物に接触しないための十分条件を示す式である。3種類の式のうち1種類目は、例えば上述した式(3)のように示される。
3種類の式のうち2種類目は、第1実施形態の場合と同様、制御指令値が、制御対象機器900の可動部の仕様により規定される上下限値の範囲内で設定されるという制約条件を示す式である。3種類の式のうち2種類目は、例えば上述した式(7)のように示される。
例えば、リアプノフ関数をVで示し、制御ステップ間におけるリアプノフ関数値の変化量をΔVで示して、制約条件を示す3種類目の式を式(11)のようにしてもよい。
d=0として解を得られた場合、その解は、制御対象機器900に設定される目標を厳密に達成するための制御指令値である。一方、d=0の場合、解をピンポイントで探索することになり、解を得られないことが懸念される。
そこで、d≧0とすることで、制御指令値に基づく制御対象機器900の動作結果と目標とのずれを許容して、解の探索範囲を広げることができる。以下では、制御対象機器900の動作結果と目標とのずれを誤差と称する。「d」の値が大きくなるほど、許容する誤差が大きくなり、解を得易くなる。
例えば、第4実施形態で回避指令値演算部495が解く最適化問題における制約条件を示す3種類目の式を、式(12)のようにしてもよい。
式13の「uTu」の項は、制御対象機器900が無駄な動作をしないよう、制御指令値の大きさをなるべく小さくするために設けられている。
この最適化問題の解の候補が2つ検出された場合、2つの解の候補の「uTu」の項の値が同じであれば、「p・d2」の項の値が小さい方が最適解として採用される。
式(13)は、評価関数の例に該当する。式(13)における最小解となる制御指令値uは、制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値の例に該当する。
図16を図4と比較すると、図16では障害物回避制御システム4がノミナル制御装置300を備えていない。これに伴い図16では、情報取得装置200からの制御対象に関する値が機器制御部492へ入力されている。
それ以外の点では、図16は図4の場合と同様である。
図17のステップS311における処理は、図5のステップS111における処理と同様である。
ステップS311の後、回避指令値演算部495は、上述した最適化問題の演算による制御指令値の取得を試みる(ステップS312)。
機器制御部492は、回避指令値演算部495がステップS312で最適化問題の解を得られたか否かを判定する(ステップS313)。
また、障害物回避制御装置401では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置401によれば、障害物が複数ある場合に比較的容易に対応できる。
図18は、第5実施形態に係る障害物回避制御装置の構成の例を示す図である。図18に示す障害物回避制御装置10は、回避指令値演算部11と、機器制御部12とを備える。
かかる構成で、回避指令値演算部11は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める。機器制御部12は、回避指令値演算部11による処理の結果に基づいて、制御対象機器を制御する。
また、障害物回避制御装置10では、障害物が複数ある場合、障害物毎に制約条件を設けることができる。この点で、障害物回避制御装置10によれば、障害物が複数ある場合に比較的容易に対応できる。
図19は、第6実施形態に係る障害物回避制御方法における処理の手順の例を示す図である。図19に示す障害物回避制御方法は、制御対象機器に対する制御指令値であって、制御対象機器が障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を評価関数に適用した評価値が所定の終了条件を満たす制御指令値である回避指令値を求める工程(ステップS11)と、ステップS11での処理の結果に基づいて、制御対象機器を制御する工程(ステップS12)とを含む。
図20に示す構成で、コンピュータ700は、CPU710と、主記憶装置720と、補助記憶装置730と、インタフェース740とを備える。
上記の情報取得装置200、ノミナル制御装置300、障害物回避制御装置400、障害物回避制御装置401、障害物位置特定装置500、および、障害物動作予測装置600のうち何れか1つ以上が、コンピュータ700に実装されてもよい。その場合、上述した各処理部の動作は、プログラムの形式で補助記憶装置730に記憶されている。CPU710は、プログラムを補助記憶装置730から読み出して主記憶装置720に展開し、当該プログラムに従って上記処理を実行する。また、CPU710は、プログラムに従って、上述した各記憶部に対応する記憶領域を主記憶装置720に確保する。各装置と他の装置との通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第1記憶部280に対応する記憶領域を主記憶装置720に確保する。第1通信部210が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第2記憶部480およびその各部に対応する記憶領域を主記憶装置720に確保する。第2通信部410が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第3記憶部580およびその各部に対応する記憶領域を主記憶装置720に確保する。第3通信部510が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
また、CPU710は、プログラムに従って、第4記憶部680およびその各部に対応する記憶領域を主記憶装置720に確保する。第4通信部610が行う通信は、インタフェース740が通信機能を有し、CPU710の制御に従って通信を行うことで実行される。
「コンピュータ読み取り可能な記録媒体」とは、フレキシブルディスク、光磁気ディスク、ROM(Read Only Memory)、CD-ROM(Compact Disc Read Only Memory)等の可搬媒体、コンピュータシステムに内蔵されるハードディスク等の記憶装置のことをいう。また上記プログラムは、前述した機能の一部を実現するためのものであっても良く、さらに前述した機能をコンピュータシステムにすでに記録されているプログラムとの組み合わせで実現できるものであっても良い。
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の何れか一項に記載の障害物回避制御装置。 - 前記回避指令値演算部は、前記制御対象機器に設定された基準点毎に、その基準点が前記障害物に接触しないための十分条件を満たす前記回避指令値を求める、請求項1から4の何れか一項に記載の障害物回避制御装置。
- 請求項1に記載の障害物回避制御装置と、
前記障害物となっている物体の種類を識別する種類識別部と、
前記物体の種類毎に前記物体の大きさおよび形状を示す物体情報から、識別された種類に応じた情報を読み出し、読み出した情報を用いて前記障害物の位置を特定する位置特定部と、
をさらに備え、
前記干渉関数演算部は、特定された前記障害物の位置に基づいて、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する、
障害物回避制御システム。 - 制御対象機器と障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する工程と、
前記制御対象機器に対する制御指令値であって、前記制御対象機器が前記障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を、制御対象機器に設定されている目的の実行についての評価関数に適用した評価値が、前記目的の実行可能性に関する所定の条件を満たす制御指令値である回避指令値を、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて求める工程と、
前記回避指令値の取得に成功したと判定した場合、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御し、前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる工程と、
を含む障害物回避制御方法。 - コンピュータに、
制御対象機器と障害物との位置関係に応じた値を示す干渉関数値、および、前記干渉関数値の経時変化を示す値を算出する工程と、
前記制御対象機器に対する制御指令値であって、前記制御対象機器が前記障害物に接触しないための十分条件を含む制約条件を満たし、かつ、その制御指令値を、制御対象機器に設定されている目的の実行についての評価関数に適用した評価値が、前記目的の実行可能性に関する所定の条件を満たす制御指令値である回避指令値を、前記干渉関数値、および、前記干渉関数値の経時変化を示す値を用いて求める工程と、
前記回避指令値の取得に成功したと判定した場合、前記回避指令値を求める工程での処理の結果に基づいて、前記制御対象機器を制御し、前記回避指令値の取得に失敗したと判定した場合、前記制御対象機器を減速させる工程と、
を実行させるためのプログラム。
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 (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
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)
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)
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 | ロボットアーム操作システムおよびその操作方法 |
JP6378783B2 (ja) * | 2014-12-25 | 2018-08-22 | 川崎重工業株式会社 | アーム型のロボットの障害物自動回避方法及び制御装置 |
JP7058126B2 (ja) * | 2018-01-12 | 2022-04-21 | 株式会社日立製作所 | ロボット制御装置および自動組立システム |
TWI664573B (zh) * | 2018-05-11 | 2019-07-01 | 國立交通大學 | 動作計算裝置、機器人系統及機器人控制方法 |
-
2019
- 2019-01-30 EP EP19914139.1A patent/EP3920001A4/en active Pending
- 2019-01-30 WO PCT/JP2019/003177 patent/WO2020157862A1/ja unknown
- 2019-01-30 JP JP2020569234A patent/JP7180695B2/ja active Active
- 2019-01-30 US US17/426,240 patent/US20220097231A1/en active Pending
Patent Citations (2)
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 |
---|---|
EP3920001A1 (en) | 2021-12-08 |
JPWO2020157862A1 (ja) | 2021-11-11 |
EP3920001A4 (en) | 2022-01-26 |
US20220097231A1 (en) | 2022-03-31 |
WO2020157862A1 (ja) | 2020-08-06 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
JP7180695B2 (ja) | 障害物回避制御装置、障害物回避制御システム、障害物回避制御方法およびプログラム | |
Janabi-Sharifi et al. | Automatic selection of image features for visual servoing | |
KR20210043632A (ko) | 물체 자세 추정 방법 및 장치 | |
JP5750657B2 (ja) | 強化学習装置、制御装置、および強化学習方法 | |
JP5803367B2 (ja) | 自己位置推定装置、自己位置推定方法およびプログラム | |
US11292132B2 (en) | Robot path planning method with static and dynamic collision avoidance in an uncertain environment | |
CN114072254A (zh) | 不确定环境中利用静态和动态碰撞规避的机器人路径规划方法 | |
JP7180696B2 (ja) | 制御装置、制御方法およびプログラム | |
JP2003271975A (ja) | 平面抽出方法、その装置、そのプログラム、その記録媒体及び平面抽出装置搭載型ロボット装置 | |
WO2020181257A1 (en) | Collaborative determination of a load footprint of a robotic vehicle | |
KR20200027087A (ko) | 로봇 및 그의 제어 방법 | |
Platt et al. | Simultaneous localization and grasping as a belief space control problem | |
Sadeghian et al. | Visual servoing with safe interaction using image moments | |
Kim et al. | Active object tracking using context estimation: handling occlusions and detecting missing targets | |
US20220297296A1 (en) | Control device, robot system, control method, and non-transitory computer-readable medium | |
US20220143836A1 (en) | Computer-readable recording medium storing operation control program, operation control method, and operation control apparatus | |
US20220148119A1 (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 | |
JP2022156954A (ja) | 制御装置、ロボットシステム、制御方法、および、プログラム | |
JP2008065562A (ja) | 情報処理装置、および同定処理方法、並びにコンピュータ・プログラム | |
US20230202026A1 (en) | Robot Training System | |
JP2021010994A (ja) | センサ位置姿勢キャリブレーション装置及びセンサ位置姿勢キャリブレーション方法 | |
CN111971149A (zh) | 记录介质、信息处理设备和信息处理方法 | |
KR20150050224A (ko) | 비정상 배회 탐지 장치 및 방법 |
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 |