(57)【要約】【課題】より安定性の高い制御を実現できるようにした移動体の制御装置及び制御システムを提供する。【解決手段】到達モジュール１４は、センサ１１により取得される目標Ｔの位置に応じて当該目標位置にエンドエフェクタ２を到達させるための到達制御値Ｃｒを出力する。回避モジュール１５は、センサ１１により取得される障害物Ｏの位置に応じてエンドエフェクタ２の障害物Ｏへの衝突を回避するための回避制御値Ｃａを出力する。混合モジュール１６は、到達モジュール１４の到達制御値、回避モジュール１５の回避制御値、到達制御値の重み付け値Ｒｅ、及び、回避制御値の重み付け値Ａｖに応じて混合制御値Ｃｍを算出する。このとき混合モジュール１６は、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが所定の閾値Ｄ０と等しいときには回避制御値の重み付け値Ａｖを０を含む第１許容範囲とし、到達制御値の重み付け値Ｒｅを０を含む第２許容範囲とし、且つ、回避制御値の微分値を０を含む第３許容範囲とし、到達制御値の重み付け値を０を含む第４許容範囲とする。【選択図】図４
2018202579.tif 000002
【特許請求の範囲】
【請求項１】
障害物（Ｏ）を回避しながら制御対象となる移動体（２）を目標（Ｔ）の位置に制御する移動体の制御装置（１０，１１０）であって、
目標位置センサ（１１）により取得される目標の位置に応じて当該目標位置に前記移動体を到達させるための到達制御値（Ｃｒ）を出力する到達制御値出力部（１４）と、
障害物位置センサ（１１）により取得される障害物の位置に応じて前記移動体の前記障害物への衝突を回避するための回避制御値（Ｃａ）を出力する回避制御値出力部（１５）と、
前記到達制御値出力部の到達制御値、前記回避制御値出力部の回避制御値、前記到達制御値の重み付け値、及び、前記回避制御値の重み付け値に応じて混合制御値（Ｃｍ）を算出する混合制御値算出部（１６）と、を備え、
前記混合制御値算出部は、前記移動体と前記障害物との間の距離が所定の閾値と等しいときには前記回避制御値の重み付け値を０を含む第１許容範囲とし、前記到達制御値の重み付け値を０を含む第２許容範囲とし、且つ、前記回避制御値の微分値を０を含む第３許容範囲とし、前記到達制御値の重み付け値を０を含む第４許容範囲とする、移動体の制御装置。
【請求項２】
障害物（Ｏ）を回避しながら制御対象となる移動体（２）を目標（Ｔ）の位置に制御する移動体の制御装置（１０，１１０）であって、
目標位置センサ（１１）により取得される目標の位置に応じて当該目標位置に前記移動体を到達させるための到達制御値（Ｃｒ）を出力する到達制御値出力部（１４）と、
障害物位置センサ（１１）により取得される障害物の位置に応じて前記移動体の前記障害物への衝突を回避するための回避制御値を出力する回避制御値出力部（１５）と、
前記到達制御値出力部の到達制御値、前記回避制御値出力部の回避制御値、前記到達制御値の重み付け値、及び、前記回避制御値の重み付け値に応じて混合制御値（Ｃｍ）を算出する混合制御値算出部（１６）と、を備え、
前記混合制御値算出部は、前記移動体と前記障害物との間の距離（Ｄ）が所定の到達閾値（Ｄｈｄ）より大きく当該移動体と前記障害物とが近接する状態のときには前記回避制御値の重み付け値（Ａｖ）を０とすると共に前記到達制御値の重み付け値（Ｒｅ）に切替え、前記移動体と前記障害物との間の距離が所定の回避閾値（Ｄｈｕ）より小さく当該移動体と前記障害物とが遠ざかる状態のときには前記到達制御値の重み付け値（Ｒｅ）を０とすると共に前記回避制御値の重み付け値（Ａｖ）に切替える、移動体の制御装置。
【請求項３】
前記回避閾値は、前記到達閾値よりも高く設定されている請求項２記載の移動体の制御装置。
【請求項４】
前記混合制御値算出部は、前記障害物及び前記移動体の相対距離及び相対速度を判定条件として用いて混合制御値を算出する請求項１から３の何れか一項に記載の移動体の制御装置。
【請求項５】
前記混合制御値算出部は、
前記移動体と前記障害物との相対距離ベクトルのスカラー値（｜→Ｄ｜）が所定の閾値（Ｄ０）を含みそれ以上のときには、前記混合制御値算出部は、前記到達制御値出力部の到達制御値（Ｃｒ）に０を超え１までの重み付け値を乗算した混合制御値とし、
前記移動体と前記障害物の相対距離ベクトルのスカラー値（｜→Ｄ｜）が前記所定の閾値（Ｄ０）より小さいとき、前記混合制御値算出部は、前記移動体と前記障害物の相対速度ベクトルと前記相対距離べクトルとの内積値（「→Ｖ」・「→Ｄ」）が０以上となるときに前記到達制御値出力部の到達制御値（Ｃｒ）に０を超え１までの重み付け値を乗算した混合制御値とし、前記内積値が０未満となるときには前記回避制御値出力部の回避制御値（Ｃａ）に０を超え１までの重み付け値を乗算した混合制御値とする、請求項４記載の移動体の制御装置。
【請求項６】
障害物（Ｏ）を回避しながら制御対象となる移動体（２）を目標（Ｔ）の位置に制御する移動体の制御装置（１１０）であって、
目標位置センサ（１１）により取得される目標（Ｔ）の位置に応じて当該目標の位置に前記移動体を到達させるための到達制御値を出力する到達制御値出力部（１４）と、
障害物位置センサ（１１）により取得される障害物（Ｏ）の位置に応じて前記移動体の前記障害物への衝突を回避するための回避制御値を出力する回避制御値出力部（１５）と、
前記到達制御値出力部の到達制御値、前記回避制御値出力部の回避制御値、前記到達制御値の重み付け値、及び、前記回避制御値の重み付け値に応じて混合制御値を算出する混合制御値算出部（１１６）と、
前記障害物が一定領域内に留まる時間を計時する計時部（１７）と、を備え、
前記計時部により一定領域内に留まる時間を計時することで障害物（Ｏ）が静止障害物と判定されたときには、前記障害物の位置と前記移動体の位置に基づいて当該移動体に動力を発生させる制御値を出力することで前記移動体と前記静止障害物との相対位置関係の硬直状態を解消する動力発生制御値出力部（１１６）と、
を備える移動体の制御装置。
【請求項７】
前記動力発生制御値出力部は、前記障害物が静止障害物と判定されたときには、前記障害物の位置と前記移動体の位置と前記目標位置とに基づいて当該移動体に動力を発生させる制御値を出力することで前記移動体と前記静止障害物との相対位置関係の硬直状態を解消する請求項６記載の移動体の制御装置。
【請求項８】
前記動力発生制御値出力部は、前記移動体に動力を発生させる制御値を出力するときには前記静止障害物と前記移動体とを結ぶ直線に対し交差する方向に動作させるための動力を発生させる制御値を出力する請求項６または７記載の移動体の制御装置。
【請求項９】
前記目標の位置、前記移動体の位置、前記障害物の位置のベクトルをそれぞれ「→Ｄ０」、「→Ｄ１」、「→Ｄ２」、とし、前記移動体に印加する力のベクトルを「→Ｆ」としたとき、
前記動力発生制御値出力部は、
（「→Ｄ２」−「→Ｄ１」）・「→Ｆ」＝０  …（１）
（「→Ｄ０」−「→Ｄ１」）・「→Ｆ」≧０  …（２）
の（１）（２）式を満たすように前記移動体に動力を発生させる制御値を出力する請求項６から８の何れか一項に記載の移動体の制御装置。
【請求項１０】
請求項１から９の何れか一項に記載の制御装置（１０，１１０）と、
目標（Ｔ）の位置を取得する目標位置センサ（１１）と、
障害物（Ｏ）の位置を取得する障害物位置センサ（１１）と、
前記制御装置による混合制御値を入力し前記移動体を動作させるコントローラ（１２）と、を備える移動体の制御システム。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の制御装置及び制御システムに関する。
【背景技術】
【０００２】
従来、例えばロボット制御技術では、人間がロボットの動作領域に侵入すると危険なため、安全防護柵を設けたり、人間の作業エリアにロボットが侵入できないように規制し、人間とロボットの動作領域を区分してきた。近年、人間とロボットが動作空間を共有して協調作業する取り組みが注目されており、特に、ロボットが各種障害物の存在位置を監視し、自律的に障害物との衝突を回避する手法が開発されつつある（例えば、特許文献１、２参照）。
【０００３】
例えば、特許文献１記載の技術によれば、全身制御モジュールと、衝突回避モジュールと、混合制御ユニットと、を備えており、算出された衝突危険がより低くなるほど、全身制御モジュールが出力するモーション制御出力信号の重み付け値が高くなり、算出された衝突危険がより低くなるほど、衝突回避モジュールが出力する衝突回避制御出力信号の重み付け値が高くなるようにしている。そして、衝突回避モジュールは、ロボットの異なるセグメント、またセグメントと他の対象物の最も近い２つのポイントを算出し、衝突回避作用を制御するようにしている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２００８−２３８３９５号公報
【特許文献２】特開２００８−２１１７８号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
特許文献１記載の技術を適用した場合においては、複数の制御モジュールを切替制御するときにロボット（エンドエフェクタ）が不安定な挙動を示す虞がある。すなわち、例えば特許文献１記載の技術において、全身制御モジュールが生成した出力信号のベクトル方向と衝突回避モジュールが生成した出力信号のベクトル方向とが互いに逆方向であった場合、前述の重み付け値に応じて混合制御信号が生成されるものの、この混合制御信号の安定性は保証されていない。このため、ロボットが不安定な挙動を示す虞があり、安定性の高い制御を実行できるようにすることが望まれている。また、ロボットを目的の位置まで制御するときに障害物との距離に応じて互いに静止し硬直状態となる虞もある。
【０００６】
この種の課題は、例えば、制御対象の多い多軸多関節型ロボットの他、車両などの移動体の運転アシスト技術や自動運転技術に適用する場合にも同様に適用できるものであり、例えば車両制御分野においては安定性及び安全性の高い制御を実行できるようにすることが望まれている。
【０００７】
本発明の第１目的は、より安定性の高い制御を実現できるようにした移動体の制御装置及び制御システムを提供することにあり、第２目的は障害物との距離に応じて硬直状態となっても当該硬直状態を解消できるようにした移動体の制御装置及び制御システムを提供することにある。
【課題を解決するための手段】
【０００８】
請求項１記載の発明によれば、混合制御値算出部は、移動体と障害物との間の距離が所定の閾値と等しいときには回避制御値の重み付け値を０を含む第１許容範囲とし、到達制御値の重み付け値を０を含む第２許容範囲とし、且つ、回避制御値の微分値を０を含む第３許容範囲とし、到達制御値の重み付け値を０を含む第４許容範囲とし、混合制御値を算出するようにしている。このため、距離が所定の閾値付近となるときに移動体が不安定な挙動を示すことがなくなり、より安定性の高い制御を実現できる。
【０００９】
請求項２記載の発明によれば、混合制御値算出部は、移動体と障害物との間の距離が所定の到達閾値より大きく当該移動体と障害物とが近接する状態のときには回避制御値の重み付け値を０とすると共に到達制御値の重み付け値に切替え、移動体と障害物との間の距離が所定の回避閾値より小さく当該移動体と障害物とが遠ざかる状態のときには到達制御値の重み付け値を０とすると共に回避制御値の重み付け値に切替えている。このため、より信頼性を向上した回避制御を実行できると共に、目標に到達させるために必要な時間の増大を極力抑制でき、より安定性の高い制御を実現できる。
【００１０】
請求項６記載の発明によれば、計時部は、障害物が一定領域内に留まる時間を計時し、動力発生制御値出力部は、計時部により一定領域内に留まる時間を計時することで障害物が静止障害物と判定されたときには、障害物の位置と移動体の位置に基づいて当該移動体に動力を発生させる制御値を出力することで移動体と静止障害物との相対位置関係の硬直状態を解消する。これにより、障害物との距離に応じて硬直状態となっても当該硬直状態を解消できる。
【図面の簡単な説明】
【００１１】
【図１】第１実施形態における移動体の制御システムの全体構成図
【図２】移動体の制御システムの電気的構成を機能的に示すブロック図
【図３】移動体と障害物と目標との位置関係を表す平面図
【図４】障害物からの距離に応じた各モジュールの重み付け値の変化を示す図
【図５】障害物からの距離に応じた混合制御値の出力値の一例を示す図
【図６】比較例における障害物からの距離に応じた混合制御値の出力値の一例を示す図
【図７】比較例における移動体の挙動イメージ図
【図８】第２実施形態において移動体が障害物の近接位置から遠ざかるときの回避制御値の重み付け値の変化を示す図
【図９】移動体が障害物に近接するときの到達制御値の重み付け値の変化を示す図
【図１０】第３実施形態における処理内容を説明するフローチャート
【図１１】第４実施形態における移動体の制御システムの電気的構成を機能的に示すブロック図
【図１２】移動体と障害物と目標との位置関係を表す平面図
【図１３】第５実施形態における移動体と障害物と目標との位置関係を表す平面図
【発明を実施するための形態】
【００１２】
以下、移動体の制御装置及び制御システムの幾つかの実施形態を説明する。以下の実施形態中では、各実施形態間で同一機能または類似機能を備えた部分に同一符号又は類似符号（例えば十の位及び一の位を同一値）を付して説明を行い、後述実施形態においては同一機能または類似機能を備えた部分の説明を必要に応じて省略する。
【００１３】
（第１実施形態）
図１から図７は第１実施形態の説明図を示す。図１は移動体の制御システムの全体構成を示し、一例としてロボット１のエンドエフェクタ２の制御システム３を示している。また図２は制御システムの電気的構成を機能的なブロック図により表している。
【００１４】
図１に示すロボット１は、例えば多関節型のロボットアームを備え、例えばロータリージョイント４などの少なくとも一つ以上のジョイント４〜７を介して互いに連結されたセグメントアーム４、８、９を用いて構成される。このロボット１のセグメントアーム４の先端にはロボットハンドなどのエンドエフェクタ２が移動体として取付けられている。なお図１には、ロボット１について２次元的な側面図により簡略的に図示しているが、３次元方向の何れの位置にもエンドエフェクタ２を移動制御可能になっている。ロボット１には制御装置１０及びセンサ１１が電気的に接続されている。
【００１５】
図２に示すように、この制御システム３は、制御装置１０、センサ１１、及びロボットジョイントコントローラ１２を備える。制御装置１０はプログラムが記憶される記憶部１３を備える。図３に示すように、制御システム３は記憶部１３に記憶されたプログラムに基づいて、障害物Ｏを避けながら目標Ｔの位置までエンドエフェクタ２を移動制御する。ここでは、エンドエフェクタ２と障害物Ｏとの間の距離をＤと定義している。
【００１６】
センサ１１は、図１には簡略的に示しているが例えばロボット１のエンドエフェクタ２と共にその周辺の障害物Ｏ及び目標Ｔを含む画像を撮像する撮像装置を備えている。このセンサ１１は、例えば畳み込みニューラルネットワークを用いた画像認識技術を適用し、撮像装置により撮像された画像データから所定の形状やパターンを認識し、当該画像データに含まれる目標Ｔや障害物Ｏを区別して画像認識処理を行い、これらの位置情報（すなわち目標Ｔの位置情報ＬT、障害物Ｏの位置情報Ｌｏ）を制御装置１０に出力する。
【００１７】
制御装置１０は、例えば、ＲＯＭ、ＲＡＭ、不揮発性メモリ等の記憶部１３を備えたマイクロコンピュータを主として構成され、センサ１１から取得される位置情報を用いて、視覚運動の連携に基づくニューラルネットワークを用いて目標Ｔの位置を学習し、各ジョイント４〜７の角度及び角速度を制御し、これによりエンドエフェクタ２を目標Ｔまで移動制御する。
【００１８】
この制御装置１０は、このような移動制御を実現するため、二段階のコントローラとして、到達モジュール１４、回避モジュール１５、及び、混合モジュール１６を備える。到達モジュール１４は、各ジョイント４〜７の角度及び角速度並びにエンドエフェクタ２の３次元位置の情報をロボットジョイントコントローラ１２から入力し、このエンドエフェクタ２の位置とセンサ１１により出力される目標Ｔの位置ＬTとの間の距離Ｄに基づく学習済みのニューラルネットワークに応じて到達制御値Ｃｒを算出し、この到達制御値Ｃｒを混合モジュール１６に出力する到達制御値出力部としての機能を備える。
【００１９】
回避モジュール１５は、各ジョイント４〜７の角度及び角速度情報並びにエンドエフェクタ２の３次元位置の情報をロボットジョイントコントローラ１２から入力し、このエンドエフェクタ２の位置とセンサ１１により出力される障害物Ｏの位置情報Ｌｏとの間の距離Ｄに基づく学習済みのニューラルネットワークに応じて回避制御値Ｃａを算出し、この回避制御値Ｃａを混合モジュール１６に出力する回避制御値出力部としての機能を備える。
【００２０】
混合モジュール１６は、センサ１１により出力される位置情報ＬT，Ｌｏ、到達モジュール１４により出力される到達制御値Ｃｒ、回避モジュール１５により出力される回避制御値Ｃａ、及び、エンドエフェクタ２の位置と障害物Ｏの位置Ｌｏとの間の距離Ｄに応じて定められる重み付け値、に応じて混合制御値Ｃｍを算出する混合制御値算出部としての機能を備える。
【００２１】
このとき例えば混合モジュール１６は、センサ１１により取得される障害物Ｏの位置情報Ｌｏとエンドエフェクタ２の位置との間の距離Ｄに応じて定められる重み付け値Ｒｅ，Ａｖを到達制御値Ｃｒと回避制御値Ｃａに所定の演算処理（例えば、何れか一方の制御値Ｃｒ又はＣａを０とし他方の制御値Ｃａ又はＣｒに重み付け演算処理）することで混合制御値Ｃｍを算出し、この混合制御値Ｃｍをロボットジョイントコントローラ１２に出力する。ロボットジョイントコントローラ１２は、この混合制御値Ｃｍに応じてジョイント４〜７の角度及び角速度を制御し、障害物Ｏを避けつつ目標Ｔまでエンドエフェクタ２を移動制御する。
【００２２】
上記基本的構成の特徴的な作用について図３〜図６をも参照しながら説明する。図３はエンドエフェクタ２の位置、その移動目標Ｔの位置ＬT、及び、障害物Ｏの位置Ｌｏを概略的に示す平面図である。
【００２３】
ここでは、センサ１１を構成する撮像装置が、エンドエフェクタ２、目標Ｔ、障害物Ｏの上方から撮像するように設置され、当該センサ１１が上方からこれらのエンドエフェクタ２、目標Ｔ、障害物Ｏの位置Ｌｏを画像認識技術により検出可能にする形態を示し、説明の簡略化のため、制御装置１０がエンドエフェクタ２を二次元的に目標Ｔの位置ＬTに移動制御する形態を示す。図３に示すように、エンドエフェクタ２と目標Ｔの位置ＬTとの間の直線上に障害物Ｏが存在する場合を考慮する。
【００２４】
図４はエンドエフェクタ２と障害物Ｏとの間の距離Ｄに応じた重み付け値Ｒｅ，Ａｖの関係性について示している。記憶部１３には、これらの重み付け値Ｒｅ，Ａｖと距離Ｄとの間の関係性が例えばテーブル又は数式等を用いて記憶されており、混合モジュール１６は、この記憶部１３に記憶された重み付け値Ｒｅ，Ａｖの関係性に応じて混合制御値Ｃｍを算出する。ここでは、エンドエフェクタ２の障害物Ｏの位置Ｌｏからの安全確保可能距離として閾値Ｄ０を定義している。
【００２５】
図４に示すように、回避制御値Ｃａの重み付け値Ａｖは、エンドエフェクタ２の障害物Ｏとの間の距離が低閾値Ｄｄ（但しＤｄ＜Ｄ０）より小さいときには１とされており、ある低閾値Ｄｄを境界として距離Ｄが大きくなると単純減少し、距離Ｄが閾値Ｄ０のときに０となる。そして距離Ｄが閾値Ｄ０を含み当該Ｄ０以上のときには距離Ｄが大きくなっても重み付け値Ａｖは０になるように設定されている。
【００２６】
この回避制御値Ｃａの重み付け値Ａｖは、少なくとも距離Ｄ０以下で且つ低閾値Ｄｄを超える距離Ｄの領域において当該距離Ｄにより微分可能な関数（例えば距離Ｄの反比例関数）により表現可能となっている。この回避制御値Ｃａの重み付け値Ａｖを関数により表現した場合、距離Ｄ０における回避制御値Ｃａの重み付け値Ａｖ（Ｄ０）の微分値が０となるようにパラメータ調整されている。すなわち回避制御値Ｃａの重み付け値Ａｖは距離Ｄ０付近において緩やかに変化することになる。
【００２７】
到達制御値Ｃｒの重み付け値Ｒｅは、エンドエフェクタ２の障害物Ｏとの距離Ｄが０に近く閾値Ｄ０より小さいときには０に設定されており、閾値Ｄ０を境界として距離Ｄが大きくなると単純増加し、ある高閾値Ｄｕ（但しＤｕ＞Ｄ０）のときに１となり、当該高閾値Ｄｕを境界として距離Ｄが大きくなると１に設定される。
【００２８】
また、到達制御値Ｃｒの重み付け値Ｒｅは、少なくとも距離Ｄ０以上で且つ高閾値Ｄｕを下回る距離Ｄの領域において距離Ｄにより微分可能な関数（例えば距離Ｄの二乗に応じて変化する関数）により表現可能となっており、この関数は、距離Ｄ０における到達制御値Ｃｒの重み付け値Ｒｅ（Ｄ０）の微分値が０となるように設定されている。すなわち到達制御値Ｃｒの重み付け値Ｒｅは距離Ｄ０付近において緩やかに変化することになる。
【００２９】
すなわち、エンドエフェクタ２と障害物Ｏの距離Ｄが閾値Ｄ０に一致するとき、回避制御値Ｃａの重み付け値Ａｖと到達制御値Ｃｒの重み付け値Ｒｅはそれぞれ０に設定され、回避制御値Ｃａの重み付け値Ａｖと到達制御値Ｃｒの重み付け値Ｒｅの距離Ｄによる微分値もそれぞれ０になっている。
【００３０】
なお、エンドエフェクタ２の挙動の不安定さを解消できる程度に抑えられるようであれば、この所定の閾値Ｄ０において、回避制御値Ｃａの重み付け値Ａｖ、到達制御値Ｃｒの重み付け値Ｒｅ、回避制御値Ｃａの重み付け値Ａｖの微分値、到達制御値Ｃｒの重み付け値Ｒｅの微分値を、それぞれ、０を含む第１許容範囲、第２許容範囲、第３許容範囲、第４許容範囲に設定できれば良い。
【００３１】
図５には、これらの重み付け値Ａｖ，Ｒｅを用いて混合モジュール１６が生成した混合制御値Ｃｍの一例を示す。この図５においては、縦軸正方向が回避制御方向を示しており、縦軸負方向が到達制御方向を示しており、制御装置１０が混合制御値Ｃｍを０にすることでエンドエフェクタ２を停止制御することを示す。図５に示すように、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが閾値Ｄ０よりも近いときには混合制御値Ｃｍは回避制御方向（すなわち正値）となり、距離Ｄが閾値Ｄ０よりも遠いときには混合制御値Ｃｍは到達制御方向（すなわち負値）となる。
【００３２】
前述したように、到達制御値Ｃｒ及び回避制御値Ｃａの重み付け値Ａｖ，Ｒｅは、閾値Ｄ０付近において前述した条件を満たすように設定されているため、この閾値Ｄ０付近の例えば下限値Ｄｄ１から上限値Ｄｕ１の範囲における混合制御値Ｃｍを緩やかに変化させることができる。このため、到達制御方向と回避制御方向の境界点、すなわち距離Ｄの閾値Ｄ０において緩やかに制御方向を切替えることができ、エンドエフェクタ２の挙動をより安定的に制御できる。
【００３３】
＜比較例の説明＞
図６及び図７は発明者らが考慮した比較例の説明図を示している。この比較例では、閾値Ｄ０付近で重み付け値Ａｖ，Ｒｅを前述のように設けることなく、距離Ｄ０を境界として回避モジュール１５の回避制御値Ｃａと到達モジュール１４の到達制御値Ｃｒとを単純に切り替える例を示している。図６に示すように、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが閾値Ｄ０よりも近いときに混合制御値Ｃｍは回避制御方向の値となり、距離Ｄが閾値Ｄ０よりも遠いときに混合制御値Ｃｍは到達制御方向の値となるが、この境界の閾値Ｄ０付近では混合制御値Ｃｍが急激に変化する。図７に、そのときのエンドエフェクタ２の挙動イメージ図を示しているが、エンドエフェクタ２による障害物Ｏとの距離Ｄに応じて障害物Ｏに近接したり回避したりする挙動を示す。このため、エンドエフェクタ２が不安定な挙動を示すことになる。
【００３４】
＜概念的なまとめ＞
要するに、本実施形態によれば、距離Ｄが所定の閾値Ｄ０に等しいときには回避制御値Ｃａの重み付け値Ａｖを０を含む第１許容範囲とし、到達制御値Ｃｒの重み付け値Ｒｅを０を含む第２許容範囲とし、且つ、回避制御値Ｃａの微分値を０を含む第３許容範囲とし、到達制御値Ｃｒの重み付け値を０を含む第４許容範囲としている。このような条件を用いることで、到達制御と回避制御とを繰り返す制御動作を極力なくすことができ、エンドエフェクタ２の挙動をより安定的に制御できる。これにより安定性の高い制御を実現できる。
【００３５】
（第２実施形態）
図８及び図９は第２実施形態の追加説明図を示している。図８はエンドエフェクタ２が障害物Ｏに近接した位置から遠ざかるときの回避制御値Ｃａの重み付け値Ａｖの変化を示しており、図９はエンドエフェクタ２が遠方から障害物Ｏに近接するときの到達制御値Ｃｒの重み付け値Ｒｅの変化を示している。
【００３６】
センサ１１は撮像された画像をリアルタイム処理することで、時間経過に伴い変化する障害物Ｏの位置Ｌｏを繰り返し検出でき、また目標Ｔの位置ＬTも繰り返し検出できる。このため混合モジュール１６は、エンドエフェクタ２と障害物Ｏとの間の距離Ｄを算出したときに、これらの距離Ｄが時間変化に伴い大きくなるときにはエンドエフェクタ２が障害物Ｏから遠ざかっていると判定でき、距離Ｄが時間変化に伴い小さくなるときにはエンドエフェクタ２が障害物Ｏに近接していると判定できる。図８に示すように、エンドエフェクタ２が障害物Ｏから遠ざかると判定されたときには、混合モジュール１６は、前述実施形態で示した閾値Ｄ０に代えて回避閾値Ｄｈｕを回避制御値Ｃａの重み付け値Ａｖの境界として用いる。
【００３７】
すなわち混合モジュール１６は、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが回避閾値Ｄｈｕとなるときに、回避制御値Ｃａの重み付け値Ａｖを０とし、且つ、この閾値Ｄｈｕにおける重み付け値Ａｖの微分値も０になる関数を用いて制御する。回避閾値Ｄｈｕを超えても回避制御値Ｃａの重み付け値Ａｖを０にする関数を用いると良い。また前述実施形態で説明したように許容範囲を設けることが望ましい。なお明確に図示はしていないが、混合モジュール１６は、距離Ｄが回避閾値Ｄｈｕを下回るときには到達制御値Ｃｒの重み付け値Ｒｅを０に設定し、少なくとも回避閾値Ｄｈｕ付近において到達制御値Ｃｒの重み付け値Ｒｅの微分値も０とし、回避閾値Ｄｈｕを超えると重み付け値Ｒｅを緩やかに上昇変化させる関数を適用する。
【００３８】
また図９に示すように、エンドエフェクタ２が遠方から障害物Ｏに近接する状態のときには、閾値Ｄ０に代えて到達閾値Ｄｈｄを到達制御値Ｃｒの重み付け値の境界として用いる。すなわち混合モジュール１６は、距離Ｄが到達閾値Ｄｈｄとなるときに到達制御値Ｃｒの重み付け値Ｒｅを０とし、且つ、この到達閾値Ｄｈｄにおける重み付け値Ｒｅの微分値もまた０になるような関数を用いて制御する。また前述実施形態で説明したように許容範囲を設けることが望ましい。なお明確に図示はしていないが、混合モジュール１６は、距離Ｄが到達閾値Ｄｈｄ以上であるときには回避制御値Ｃａの重み付け値Ａｖを０に設定し、少なくとも到達閾値Ｄｈｄ付近において回避制御値Ｃａの重み付け値Ａｖの微分値も０とし、到達閾値Ｄｈｄを下回ると重み付け値Ａｖを緩やかに上昇変化させる関数を適用する。
【００３９】
また回避閾値Ｄｈｕは到達閾値Ｄｈｄよりも高く設定されていることが望ましい。この場合、エンドエフェクタ２が障害物Ｏに必要以上に近接しているときには、距離Ｄが回避閾値Ｄｈｕに達するまで回避制御値Ｃａを用いて優先的に切替制御することになるため、障害物Ｏの極力遠方まで回避制御を優先させることができる。これにより、信頼性良く障害物Ｏへの衝突を回避できる。
【００４０】
逆に、エンドエフェクタ２が遠方から障害物Ｏ及び目標Ｔに近接するときには、障害物Ｏとの距離Ｄが到達閾値Ｄｈｄに達するまで到達制御値Ｃｒを用いて優先的に切替制御することになるため、極力障害物Ｏの近くまで回避制御を機能させることなく目標Ｔの近くまで到達制御できる。例えば、エンドエフェクタ２と障害物Ｏと目標Ｔとの位置関係が、図３に示す関係となるときには、目標Ｔの位置により近接するまで到達制御を継続することができ、エンドエフェクタ２を目標Ｔの位置に到達させるために必要な時間の増大を極力抑制できる。
【００４１】
また、回避閾値Ｄｈｕ＜到達閾値Ｄｈｄとしても良い。回避閾値Ｄｈｕ＜距離Ｄ＜到達閾値Ｄｈｄを満たすときには、到達制御値Ｃｒ及び回避制御値Ｃａの重み付け値Ｒｅ，Ａｖを共に０とすることが望ましく、この条件を満たす距離Ｄの場合には、エンドエフェクタ２は原則的に停止することになり、より安全性能を優先したい場合に有効である。
【００４２】
＜本実施形態の概念的なまとめ＞
要するに、本実施形態によれば、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが所定の到達閾値Ｄｈｄより大きくエンドエフェクタ２と障害物Ｏとが近接する状態のときには回避制御値Ｃａの重み付け値Ａｖを０とし到達制御値Ｃｒの重み付け値Ｒｅに切替え、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが所定の回避閾値Ｄｈｕより小さくエンドエフェクタ２と障害物Ｏとが遠ざかる状態のときには到達制御値Ｃｒの重み付け値Ｒｅを０とし回避制御値Ｃａの重み付け値Ａｖに切替えている。これにより、より信頼性を向上した回避制御を実行できると共に、目標Ｔに到達させるために必要な時間の増大を極力抑制でき、より安定性の高い制御を実現できる。
【００４３】
（第３実施形態）
図１０は第３実施形態の追加説明図を示す。第３実施形態においては、混合モジュール１６が、障害物Ｏの位置Ｌｏ及び速度、エンドエフェクタ２の位置及び速度、を判定条件として用いて到達制御値Ｃｒ及び回避制御値Ｃａに重み付けして混合制御値Ｃｍを算出する形態を示す。特に、エンドエフェクタ２が障害物Ｏから遠ざかる方向に移動しているときには、たとえ距離Ｄが閾値Ｄ０未満であっても到達モジュール１４の到達制御値Ｃｒを用いて制御可能にした形態を示す。
【００４４】
以下では、説明の便宜上、ある基準位置からのエンドエフェクタ２の位置ベクトルと、エンドエフェクタ２の速度ベクトルをそれぞれ、「→Ｄ０」、「→Ｖ０」、と表記し、基準位置からの障害物Ｏの位置ベクトルと、その障害物Ｏの速度ベクトルを、それぞれ、「→Ｄ１」、「→Ｖ１」、と表記する。
【００４５】
前述実施形態で示したエンドエフェクタ２と障害物Ｏとの間の距離Ｄを言い換えると、相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜として表すことができ、この相対距離ベクトル「→Ｄ」は「→Ｄ０」−「→Ｄ１」により定義できる。また、エンドエフェクタ２と障害物Ｏがそれぞれ速度べクトル「→Ｖ０」，「→Ｖ１」で移動しているため、これらの相対速度ベクトルを「→Ｖ」＝「→Ｖ０」−「→Ｖ１」と表すことができる。なお、前述実施形態の機能によれば、混合モジュール１６は、相対距離ベクトルのスカラー値｜→Ｄ｜が閾値Ｄ０を境界とした条件を用いて到達制御値Ｃｒ及び回避制御値Ｃａに重み付けして混合制御値Ｃｍを算出することになる。
【００４６】
本実施形態に係る機能説明を理解しやすくするため、前述実施形態で説明した回避制御値Ｃａ及び到達制御値Ｃｒの重み付け値Ａｖ，Ｒｅを単純化して０又は１とし、混合モジュール１６は、当該回避制御値Ｃａまたは到達制御値Ｃｒを選択切替して混合制御値Ｃｍを算出する形態を説明する。すなわちここでは、混合モジュール１６は、何れかのモジュール１４，１５の回避制御値Ｃｒ又は到達制御値Ｃｒの出力を混合制御値Ｃｍとする例を示す。
【００４７】
図１０は条件に応じた混合制御値Ｃｍの設定方法をフローチャートで示している。この図１０に示すように、混合モジュール１６は、相対距離ベクトルのスカラー値｜→Ｄ｜を算出し、Ｓ１において｜→Ｄ｜≧Ｄ０であると判定したときには、混合モジュール１６は、Ｓ１でＹＥＳと判定し、回避制御値Ｃａの重み付け値Ａｖを０とし到達制御値Ｃｒの重み付け値Ｒｅを１とし、これによりＳ２において到達モジュール１４の到達制御値Ｃｒに切替えて混合制御値Ｃｍとして出力する。
【００４８】
また本実施形態では、混合モジュール１６は相対距離ベクトルのスカラー値｜→Ｄ｜＜Ｄ０のときにＳ１でＮＯと判定し、Ｓ３において相対速度べクトル「→Ｖ」を判定条件として用いてＳ４又はＳ５において混合制御値Ｃｍを算出する。
【００４９】
ここで、Ｓ３において相対速度ベクトル「→Ｖ」と相対距離ベクトル「→Ｄ」の内積値「→Ｖ」・「→Ｄ」≧０の条件を満たすときには、Ｓ３でＹＥＳと判定し混合制御値Ｃｍを到達制御値Ｃｒに切替えて出力する。
【００５０】
これらのベクトルの内積値「→Ｖ」・「→Ｄ」≧０のときには、相対距離ベクトル「→Ｄ」と相対速度ベクトル「→Ｖ」の角度が鋭角又は垂直となるため、エンドエフェクタ２が障害物Ｏから遠ざかる方向に移動していることを示す。このため、相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜がたとえ閾値Ｄ０未満となっても、混合制御値Ｃｍとして到達制御値Ｃｒを用いて制御しても障害物Ｏに衝突することはない。
【００５１】
逆に、混合モジュール１６は、この内積値「→Ｖ」・「→Ｄ」が０未満のときには、Ｓ３でＮＯと判定しＳ５において混合制御値Ｃｍを回避制御値Ｃａに切替えて出力する。この内積値「→Ｖ」・「→Ｄ」が０未満のときには、相対距離ベクトル「→Ｄ」と相対速度ベクトル「→Ｖ」の角度が鈍角となるため、エンドエフェクタ２と障害物Ｏとが相対的に近接する方向に移動していることを示す。
【００５２】
このため、相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜が閾値Ｄ０未満となるときには、混合モジュール１６が回避制御値Ｃａを用いて混合制御値Ｃｍを生成することが望ましい。これにより、エンドエフェクタ２が障害物Ｏに衝突することを回避できる。
【００５３】
＜変形例＞
ここでは説明の簡単化のため、重み付け値Ａｖ、Ｒｅを０または１とした形態を示したが、これに限定されるものではなく、第１又は第２実施形態に示した条件を用いて重み付け値Ａｖ、Ｒｅを０を超え１までの値とし、これらの重み付け値Ａｖ、Ｒｅを用いて混合制御値Ｃｍを算出するようにしても良い。例えば混合モジュール１６は、回避制御値Ｃａに０を超え１までの重み付け値Ａｖを乗算して混合制御値Ｃｍとして出力したり、到達制御値Ｃｒに重み付け値Ｒｅを乗算して混合制御値Ｃｍとして出力したりするように構成できる。これによりエンドエフェクタ２の挙動を安定化できる。
【００５４】
＜本実施形態の概念的なまとめ＞
本実施形態によれば、混合モジュール１６は、エンドエフェクタ２と障害物Ｏの相対速度ベクトル「→Ｖ」を用いて制御しているので、たとえ障害物Ｏとエンドエフェクタ２の相対距離ベクトル「→Ｄ」のスカラー値｜→Ｄ｜が所定の閾値Ｄ０未満であったとしても、エンドエフェクタ２が障害物Ｏに接近していない限り到達制御値Ｃｒを用いて制御することになる。このため回避制御値Ｃａを用いて制御することはなくなり、エンドエフェクタ２をより目標Ｔの位置に近接するまで到達制御させることができ、目標Ｔの位置に到達させるために必要な時間の増大を極力抑制できる。これによりエンドエフェクタ２を目標Ｔの位置まで効率的に移動制御できる。
【００５５】
（第４実施形態）
図１１及び図１２は第４実施形態の追加説明図を示している。例えば、第１から第３の何れかの実施形態の制御方式を考慮した場合、エンドエフェクタ２又は障害物Ｏの何れか一方が移動すれば、これらの間の距離Ｄも変化する。このため、エンドエフェクタ２と障害物Ｏとの距離Ｄが変化することから、重み付け値Ａｖ，Ｒｅ又は各制御値Ｃｒ，Ｃａも変化し、障害物Ｏとの距離Ｄ及び目標Ｔとの距離に応じてエンドエフェクタ２を移動制御できるようになる。
【００５６】
しかし、エンドエフェクタ２と障害物Ｏとの間の距離Ｄが閾値Ｄ０に一致すると、障害物Ｏが一定の領域内に留まると共に、エンドエフェクタ２も一定の領域内に留まる。このとき、到達モジュール１４の到達制御値Ｃｒ、回避モジュール１５の回避制御値Ｃａは共に０となるため、混合制御値Ｃｍも０となり、目標到達制御は原理的に実行されなくなる。また、たとえ混合制御値Ｃｍが０にならなくても、混合制御値Ｃｍが０に近く小さくなる場合には、エンドエフェクタ２の動作も遅くなり、エンドエフェクタ２の挙動も不安定になりやすい。
【００５７】
このようなとき、効率的に障害物Ｏを回避するため、本実施形態の制御システム１０３においては、図１１に示すように計時モジュール１７を混合モジュール１１６に接続するように設けている。この計時モジュール１７は、障害物Ｏが一定領域内に留まる時間を計測する計時部としての機能を備える。混合モジュール１１６は、到達モジュール１４の到達制御値Ｃｒ、回避モジュール１５の回避制御値Ｃａが共に０又は０を含む所定範囲となったときに計時モジュール１７にトリガを出力し、計時モジュール１７がこのトリガを受付けることで経過時間を計時開始する。
【００５８】
混合モジュール１１６は、計時モジュール１７により経過時間を検出するが、到達制御値Ｃｒ及び回避制御値Ｃａが共に０を含む許容範囲内にある経過時間を検出することで障害物Ｏの運動状態を判定する。混合モジュール１１６は、この条件を満たす経過時間がある閾値を超えた場合に障害物Ｏを静止障害物と判定する。
【００５９】
混合モジュール１１６が障害物Ｏを静止障害物と判定したときには、エンドエフェクタ２の中心位置と障害物Ｏの中心位置とを通過する直線に対し、例えば水平面内で垂直な方向に動力を与えるような混合制御値Ｃｍを生成し、ロボットジョイントコントローラ１２に出力する。垂直方向に限らず直線に交差する方向に動力を与えるような混合制御値Ｃｍを生成しロボットジョイントコントローラ１２に出力するようにしても良い。
【００６０】
このとき混合モジュール１１６はエンドエフェクタ２に動力を発生させるための制御値を出力する動力発生制御値出力部として機能することになる。このとき生成される混合制御値Ｃｍは、目標到達制御が実行停止したときの混合制御値Ｃｍによる動力に比較して大幅に大きな動力となるように生成される。
【００６１】
するとエンドエフェクタ２は、この動力に応じた混合制御値Ｃｍに基づいて障害物Ｏとを結ぶ直線に対し交差方向に動作し始める。図１２にはエンドエフェクタ２の初動方向の一例を示している。これにより、エンドエフェクタ２と障害物Ｏとの間の相対位置関係の硬直状態を解消できる。しかも、エンドエフェクタ２が、当該エンドエフェクタ２と障害物Ｏとの間を結ぶ直線に対し垂直方向に動作することで障害物Ｏを効率的に回避できる。なお、エンドエフェクタ２の初動後の動作については、混合モジュール１６が到達制御値Ｃｒ及び回避制御値Ｃａに基づいて混合制御値Ｃｍを算出し、この混合制御値Ｃｍをロボットジョイントコントローラ１２に出力することになる。これにより前述実施形態と同様の動作を実行できる。
【００６２】
＜本実施形態の概念的なまとめ＞
本実施形態によれば、計時モジュール１７により障害物Ｏが一定領域内に留まる時間を計時し、混合モジュール１１６が障害物Ｏの運動状態を判定し静止障害物であると判定したときには、混合モジュール１１６は障害物Ｏの位置とエンドエフェクタ２の位置に基づいてエンドエフェクタ２に動力を発生させるように混合制御値Ｃｍを出力する。これによりエンドエフェクタ２と障害物Ｏとの相対位置関係の硬直状態を解消できる。
【００６３】
（第５実施形態）
図１３は第５実施形態の追加説明図を示している。第４実施形態ではエンドエフェクタ２の位置と障害物Ｏの位置との関係に基づいてエンドエフェクタ２に動力を発生させる形態を説明したが、これに限定されるものではなく、さらに目標Ｔの位置に応じてエンドエフェクタ２に印加する動力の方向を制御するようにしても良い。
【００６４】
目標Ｔの位置、エンドエフェクタ２の位置、障害物Ｏの位置、の各々のベクトルを「→Ｄ０」、「→Ｄ１」、「→Ｄ２」とし、エンドエフェクタ２に印加する力のベクトルを「→Ｆ」とする。このとき、印加する力のベクトル「→Ｆ」の方向は、下記の２つの式を満たすように定義することが望ましい。
【００６５】
（「→Ｄ２」−「→Ｄ１」）・「→Ｆ」＝０  …（１）
（「→Ｄ０」−「→Ｄ１」）・「→Ｆ」≧０  …（２）
すなわち（１）式は、エンドエフェクタ２から障害物Ｏに向かうベクトル（「→Ｄ２」−「→Ｄ１」）との間で内積値が０を満たす、すなわち、このベクトル（「→Ｄ２」−「→Ｄ１」）の方向と力ベクトル「→Ｆ」の方向との角度が垂直となる関係を満たす力ベクトル「→Ｆ」を定義している。
【００６６】
また（２）式は、エンドエフェクタ２の位置から目標Ｔの位置に向かうベクトル（「→Ｄ０」−「→Ｄ１」）との間で内積値が０以上となる、すなわち、このベクトル（「→Ｄ０」−「→Ｄ１」）の方向と力ベクトル「→Ｆ」の方向との角度が鋭角となるように力ベクトル「→Ｆ」を定義している。このとき、力ベクトル「→Ｆ」は、図１３に示すように目標Ｔの位置に少しでも近接する方向となり、エンドエフェクタ２は目標Ｔの位置に少しでも近接するように移動することになる。このため、障害物Ｏを回避しながら目標Ｔの位置に少しでも近づくことができるようになり効率的に移動制御できる。
【００６７】
本実施形態によれば、障害物Ｏが静止障害物と判定されたときには、障害物Ｏの位置とエンドエフェクタ２の位置と目標Ｔの位置とに基づいてエンドエフェクタ２に動力を発生させる制御値を出力するため、エンドエフェクタ２と障害物Ｏとの相対位置関係の硬直状態を解消できる。特に（１）（２）式を満たすように制御しているため、障害物Ｏを回避しながら目標Ｔの位置に効率的に移動制御できる。
【００６８】
（他の実施形態）
本発明は、前述実施形態に限定されるものではなく、例えば、以下に示す変形又は拡張が可能である。
【００６９】
前述実施形態では説明の簡略化のため、第１、第２実施形態では１次元、第３から第５の実施形態では２次元の制御を考慮した形態を示したが、全ての実施形態において２次元方向及び３次元方向の制御に適用できる。
【００７０】
「目標位置センサ」、「障害物位置センサ」、について、撮像装置を搭載したセンサ１１により障害物Ｏの位置や目標Ｔの位置を検出するように構成した形態を示したが、これに限定されるものではなく、例えば位置検出用の超音波センサ等を適用しても良いし、各種のセンサやその制御装置を一体に設けても別体に設けても良い。なお、エンドエフェクタ２を三次元的に目標Ｔの位置に移動制御する場合にも同様に適用できる。
【００７１】
制御対象としてロボット１のエンドエフェクタ２を制御する実施形態を示したが、自律型移動ロボットにも適用でき、さらに、自動車、自動二輪車などの車両に適用することでいわゆる運転支援技術、自動運転技術にも適用できる。すなわち、制御対象は、ロボット１のエンドエフェクタ２に限られず、車両などにも適用でき、各種の移動体に適用できる。
【００７２】
前述した実施形態の各要素を組み合わせて構成しても良い。前述実施形態に記述した各機能をハードウェアにより実現してもハードウェア及びソフトウェアの組合せにより実現しても良い。また、特許請求の範囲に記載した括弧内の符号は、本発明の一つの態様として前述する実施形態に記載の具体的手段との対応関係を示すものであって、本発明の技術的範囲を限定するものではない。前述実施形態の一部を、課題を解決できる限りにおいて省略した態様も実施形態と見做すことが可能である。また、特許請求の範囲に記載した文言によって特定される発明の本質を逸脱しない限度において、考え得るあらゆる態様も実施形態と見做すことが可能である。
【００７３】
本開示は、前述した実施形態に準拠して記述したが、当該実施形態や構造に限定されるものではないと理解される。本開示は、様々な変形例や均等範囲内の変形をも包含する。加えて、様々な組み合わせや形態、さらには、それらに一要素、それ以上、あるいはそれ以下、を含む他の組み合わせや形態をも、本開示の範畴や思想範囲に入るものである。
【符号の説明】
【００７４】
図面中、２はエンドエフェクタ（移動体）、３，１０３は制御システム、１０，１１０は制御装置、１１はセンサ（目標位置センサ、障害物位置センサ）、１２はロボットジョイントコントローラ（コントローラ）、１４は到達モジュール（到達制御値出力部）、１５は回避モジュール（回避制御値出力部）、１６は混合モジュール（混合制御値算出部）、１１６は混合モジュール（混合制御値算出部、動力発生制御値出力部）、１７は計時モジュール（計時部）、Ｏは障害物、Ｔは目標、Ｃａは回避制御値、Ｃｒは到達制御値、Ｃｍは混合制御値（制御値）、を示す。
【図１】
2018202579.tif 000003
【図２】
2018202579.tif 000004
【図３】
2018202579.tif 000005
【図４】
2018202579.tif 000006
【図５】
2018202579.tif 000007
【図６】
2018202579.tif 000008
【図７】
2018202579.tif 000009
【図８】
2018202579.tif 000010
【図９】
2018202579.tif 000011
【図１０】
2018202579.tif 000012
【図１１】
2018202579.tif 000013
【図１２】
2018202579.tif 000014
【図１３】
2018202579.tif 000015